41 lines
971 B
Matlab
41 lines
971 B
Matlab
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;]
|
||
|
||
|