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