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);