41 lines
971 B
Mathematica
41 lines
971 B
Mathematica
|
T05 = myFk(0.001,-0.653+pi/2,-0.22,0.001,pi/2);
|
|||
|
|
|
|||
|
|
L1=0.151;L3=0.350; L4=0.139;
|
|||
|
|
Pe = T05(1:3,4);
|
|||
|
|
Ze = T05(1:3,3);
|
|||
|
|
|
|||
|
|
%求解W点（球形腕）
|
|||
|
|
Pw = Pe - L4*Ze;
|
|||
|
|
|
|||
|
|
%求解theta1
|
|||
|
|
theta_1 = atan2(Pw(2),Pw(1));
|
|||
|
|
|
|||
|
|
%求解theta2
|
|||
|
|
%L1^2 + L3^2 - 2*L1*L3*cos_th2 = norm(Pw)^2
|
|||
|
|
cos_th2 = (L1^2 + L3^2 - norm(Pw)^2)/(2*L1*L3);
|
|||
|
|
theta_2 = acos(cos_th2);
|
|||
|
|
|
|||
|
|
%计算T02
|
|||
|
|
T01 = dh2Tmat(theta_1,L1,0,pi/2); %连杆1
|
|||
|
|
T12 = dh2Tmat(theta_2,0,0,pi/2); %连杆2
|
|||
|
|
T02 = T01*T12;
|
|||
|
|
|
|||
|
|
%计算T25 T05 = T02 * T25
|
|||
|
|
T = inv(T02) * T05;
|
|||
|
|
%T25_f = dh2Tmat(0,0.5,0,-pi/2)*dh2Tmat(pi/4,0,0,pi/2)*dh2Tmat(0,0.3,0,0);
|
|||
|
|
|
|||
|
|
%腕关节(theta_5)朝上
|
|||
|
|
theta_3 = atan2(T(2,3),T(1,3));
|
|||
|
|
theta_4 = atan2(sqrt(T(1,3)^2+T(2,3)^2),T(3,3));
|
|||
|
|
theta_5 = atan2(T(3,2),-T(3,1));
|
|||
|
|
|
|||
|
|
%腕关节(theta_5)朝下
|
|||
|
|
theta_33 = atan2(-T(2,3),-T(1,3));
|
|||
|
|
theta_44 = atan2(-sqrt(T(1,3)^2+T(2,3)^2),T(3,3));
|
|||
|
|
theta_55 = atan2(-T(3,2),T(3,1));
|
|||
|
|
|
|||
|
|
theta = [theta_1,theta_2,theta_3,theta_4,theta_5;
|
|||
|
|
theta_1,theta_2,theta_33,theta_44,theta_55;]
|
|||
|
|
|
|||
|
|
|