www.gusucode.com > matlab编程求解机器人的正逆解 采用遗传算法对机器人进行路径规划源码程序 > code/基本代码/Transform1.m

    
T = cell(1,7);

syms t1;syms d1;
syms t2;syms d2;
syms t3;syms d3;syms a3;
syms t4;syms d4;syms a4;
syms t5;syms d5;
syms t6;syms d6;
syms t7;syms d7;

syms nx;syms ny;syms nz;
syms sx;syms sy;syms sz;
syms ax;syms ay;syms az;
syms px;syms py;syms pz;

T{1} = [cos(t1),0,sin(t1),0;
        sin(t1),0,-cos(t1),0;
        0,1,0,d1;
        0,0,0,1];

T{2} = [cos(t2),0,sin(t2),0;
        sin(t2),0,-cos(t2),0;
        0,1,0,d2;
        0,0,0,1];

T{3} = [cos(t3),-sin(t3),0,a3*cos(t3);
        sin(t3),cos(t3),0,a3*sin(t3);
        0,0,1,d3;
        0,0,0,1];

T{4} = [cos(t4),-sin(t4),0,a4*cos(t4);
    sin(t4),cos(t4),0,a4*sin(t4);
    0,0,1,d4;
    0,0,0,1];

T{5} = [cos(t5),0,sin(t5),0;
        sin(t5),0,-cos(t5),0;
        0,1,0,d5;
        0,0,0,1];
    
T{6} = [cos(t6),0,sin(t6),0;
    sin(t6),0,-cos(t6),0;
    0,1,0,d6;
    0,0,0,1];

T{7} = [cos(t7),0,sin(t7),0;
    sin(t7),0,-cos(t7),0;
    0,1,0,d7;
    0,0,0,1];

T_sum = eye(4);

for i=1:7
    T_sum = T_sum*T{i};
end



T_target = [nx sx ax px;
    ny sy ay py;
    nz sz az pz;
    0 0 0 1];              %目标位姿

T_temp1_1 = T{2}*T{3}*T{4}*T{5}*T{6}*T{7};

T_temp1_2 = inv(T{1})*T_target;                         %第一阶段等式

T_temp2_1 = inv(T{2})*inv(T{1})*T_target*inv(T{7})*inv(T{6});
T_temp2_2 = T{3}*T{4}*T{5};

A = T_temp2_1(2,4);
B = T_temp2_1(1,4);