2
$\begingroup$

i been trying to code IK solver for few days, and i finally move the robot to the desired position. However im having hard time to orient the end-effector as i want it.

I read the "Robotics Modeling, Planning control -Siciliano" and few simillar question&answers.

As i understand, there are two ways.

  1. Use analytic Jacobian to map the $ \dot{p} =[v_x, v_y, v_z, w_x, w_y, w_z]'$ to $ \dot{p}' =[v_x, v_y, v_z, \dot{\phi}, \dot{\theta}, \dot{\psi}]'$
  2. using $R_d=[n_d, s_d, a_d], R_e=[n_e, s_e, a_e]$ rotation matrix of the end-effector and desired rotation matrix, find orientation error. (angle-axis method maybe?)

I tried first method since I didn't really understand the second method. as far as i know analytic Jacobian can be obtained by

\begin{equation*} J_a(q) = \begin{bmatrix} I & 0 \\ 0 & B^{-1}(r,p,y) \\ \end{bmatrix}*J(q) \end{equation*}

where B matrix is \begin{equation*} B = \begin{bmatrix} -sin(r)cos(p)/sin(p) & cos(r)cos(p)/sin(p) & 1\\ cos(r) & sin(r) & 0 \\ sin(r)/sin(p) & -cos(r)/sin(p) & 0 \end{bmatrix} \end{equation*}

With this analytic jacobian i tried to find \begin{equation*} \dot{q}_{1-6} = Ja(q)^{-1}\dot{p}'\end{equation*} Considering there is no problem on getting jacobian. I will briefly explain my code in case there is something i am missing.

For the desired pos, orientation i gave x,y,z,r,p,yaw desire value with initial joint anlge. and I find roll,pitch,yaw value for each iteration to compare with desired value. I make geometric jacobian and analytic jacobian to get a thetadot by multiplying pseudoinvser analytic jacobian with [x,y,z,roll,pitch,yaw] and check if position and r,p,y values are converges. Also update theat value with thetaddot value.

error_X = Xdesire - Xcurrent;
error_Y = Ydesire - Ycurrent;
error_Z = Zdesire - Zcurrent;
error_Roll = Rolldesire - Rollcurrent;
error_Pitch = Pitchdesire - Pitchcurrent;
error_Yaw = Yawdesire - Yawcurrent;

For the error part, position error always convserge but not r,p,yaw error. Maybe problem here? Everything seems okay for me but orientation doesn't considered. Am i doing something wrong or is there something i am missing?

Mycode is based on the this work. https://kr.mathworks.com/matlabcentral/fileexchange/61380-3dof-inverse-kinematics-pseudoinverse-jacobian?s_tid=prof_contriblnk

I added my code if ther e is anyone who is kind enough to review it, i would really thankful. It doesn't requires any function so if you have matlab u can just run it.

thx for reading a long question. If there is any unclear things let me know!

clc
clear
close

goal_pos =[575 50 425 0 90 0]; % x, y, z, roll, pitch, yaw
init_ang =[10 20 5 10 10 10];

theta1=init_ang(1);  d1=0;   a1=50;  alpha1=-90; %parameter link1
theta2=init_ang(2);  d2=50;   a2=425;  alpha2=0; %parameter link2
theta3=init_ang(3);  d3=0;  a3=0;  alpha3=90; %parameter link3
theta4=init_ang(4);  d4=425;   a4=0;  alpha4=-90; %parameter link4
theta5=init_ang(5);  d5=0; a5=0;  alpha5=90; %parameter link5
theta6=init_ang(6);  d6=100; a6=0;  alpha6=0; %parameter link6
offset2 = -90;
offset3 = 90;
T01=[cosd(theta1) -cosd(alpha1)*sind(theta1) sind(alpha1)*sind(theta1) a1*cosd(theta1);
    sind(theta1) cosd(alpha1)*cosd(theta1)  -sind(alpha1)*cosd(theta1) a1*sind(theta1);
    0,sind(alpha1),cosd(alpha1),d1;
    0,0,0,1];
T12=[cosd(theta2+offset2) -cosd(alpha2)*sind(theta2+offset2) sind(alpha2)*sind(theta2+offset2) a2*cosd(theta2+offset2);
    sind(theta2+offset2) cosd(alpha2)*cosd(theta2+offset2)  -sind(alpha2)*cosd(theta2+offset2) a2*sind(theta2+offset2);
    0,sind(alpha2),cosd(alpha2),d2;
    0,0,0,1];
T23=[cosd(theta3+offset3) -cosd(alpha3)*sind(theta3+offset3) sind(alpha3)*sind(theta3+offset3) a3*cosd(theta3+offset3);
    sind(theta3+offset3) cosd(alpha3)*cosd(theta3+offset3)  -sind(alpha3)*cosd(theta3+offset3) a3*sind(theta3+offset3);
    0,sind(alpha3),cosd(alpha3),d3;
    0,0,0,1];
T34=[cosd(theta4) -cosd(alpha4)*sind(theta4) sind(alpha4)*sind(theta4) a4*cosd(theta4);
     sind(theta4) cosd(alpha4)*cosd(theta4)  -sind(alpha4)*cosd(theta4) a4*sind(theta4);
     0,sind(alpha4),cosd(alpha4),d4;
     0,0,0,1];
T45=[cosd(theta5) -cosd(alpha5)*sind(theta5) sind(alpha5)*sind(theta5) a5*cosd(theta5);
     sind(theta5) cosd(alpha5)*cosd(theta5)  -sind(alpha5)*cosd(theta5) a5*sind(theta5);
     0,sind(alpha5),cosd(alpha5),d5;
     0,0,0,1];
T56=[cosd(theta6) -cosd(alpha6)*sind(theta6) sind(alpha6)*sind(theta6) a6*cosd(theta6);
     sind(theta6) cosd(alpha6)*cosd(theta6)  -sind(alpha6)*cosd(theta6) a6*sind(theta6);
     0,sind(alpha6),cosd(alpha6),d6;
     0,0,0,1];
T01;
T02 = T01*T12;
T03 = T01*T12*T23;
T04 = T01*T12*T23*T34;
T05 = T01*T12*T23*T34*T45;
T06 = T01*T12*T23*T34*T45*T56;
%%%%%                      POSITION                %%%%%%%%%%%%%%%
ne = T06(1:3,1);
se = T06(1:3,2);
ae = T06(1:3,3);
Re = [ne se ae] % orientation of ee
roll = rad2deg(atan2(ne(2), ne(1)));
pitch = rad2deg(atan2(-ne(3),sqrt(se(3)^2+ae(3)^2)));
yaw = rad2deg(atan2(se(3),ae(3)));

%R = inv(R03')*R36; 
disp("init ee");
INIT_EE = T06;
% DH paramter 

Xc=goal_pos(1);Yc=goal_pos(2);Zc=goal_pos(3);
r_e=goal_pos(4);p_e=goal_pos(5);yaw_e=goal_pos(6);
% init pos and orientation(Euler)

iteration = 0;
b = 0 ;
deltatheta1=0;deltatheta2=0; deltatheta3=0; %theta1,2,3 velocity
deltatheta4=0;deltatheta5=0; deltatheta6=0; %theta4,5,6 velocity
while b == 0
%     result = [theta1 theta2 theta3 theta4 theta5 theta6];
%     disp("result"); disp(result);
    theta1=theta1+deltatheta1/2;
    theta2=theta2+deltatheta2/2;
    theta3=theta3+deltatheta3/2;
    theta4=theta4+deltatheta4/2;
    theta5=theta5+deltatheta5/2;
    theta6=theta6+deltatheta6/2;
    result = [deg2rad(theta1) deg2rad(theta2) deg2rad(theta3) deg2rad(theta4) deg2rad(theta5) deg2rad(theta6)];
    if(iteration > 1000)
        % avoid too many calculation
        b=1;
        iteration = 0;
    end
    T01=[cosd(theta1) -cosd(alpha1)*sind(theta1) sind(alpha1)*sind(theta1) a1*cosd(theta1);
        sind(theta1) cosd(alpha1)*cosd(theta1)  -sind(alpha1)*cosd(theta1) a1*sind(theta1);
        0,sind(alpha1),cosd(alpha1),d1;
        0,0,0,1];
    T12=[cosd(theta2+offset2) -cosd(alpha2)*sind(theta2+offset2) sind(alpha2)*sind(theta2+offset2) a2*cosd(theta2+offset2);
        sind(theta2+offset2) cosd(alpha2)*cosd(theta2+offset2)  -sind(alpha2)*cosd(theta2+offset2) a2*sind(theta2+offset2);
        0,sind(alpha2),cosd(alpha2),d2;
        0,0,0,1];
    T23=[cosd(theta3+offset3) -cosd(alpha3)*sind(theta3+offset3) sind(alpha3)*sind(theta3+offset3) a3*cosd(theta3+offset3);
        sind(theta3+offset3) cosd(alpha3)*cosd(theta3+offset3)  -sind(alpha3)*cosd(theta3+offset3) a3*sind(theta3+offset3);
        0,sind(alpha3),cosd(alpha3),d3;
        0,0,0,1];
    T34=[cosd(theta4) -cosd(alpha4)*sind(theta4) sind(alpha4)*sind(theta4) a4*cosd(theta4);
         sind(theta4) cosd(alpha4)*cosd(theta4)  -sind(alpha4)*cosd(theta4) a4*sind(theta4);
         0,sind(alpha4),cosd(alpha4),d4;
         0,0,0,1];
    T45=[cosd(theta5) -cosd(alpha5)*sind(theta5) sind(alpha5)*sind(theta5) a5*cosd(theta5);
         sind(theta5) cosd(alpha5)*cosd(theta5)  -sind(alpha5)*cosd(theta5) a5*sind(theta5);
         0,sind(alpha5),cosd(alpha5),d5;
         0,0,0,1];
    T56=[cosd(theta6) -cosd(alpha6)*sind(theta6) sind(alpha6)*sind(theta6) a6*cosd(theta6);
         sind(theta6) cosd(alpha6)*cosd(theta6)  -sind(alpha6)*cosd(theta6) a6*sind(theta6);
         0,sind(alpha6),cosd(alpha6),d6;
         0,0,0,1];
    T01;
    T02 = T01*T12;
    T03=T01*T12*T23;
    T04=T01*T12*T23*T34;
    T05=T01*T12*T23*T34*T45;
    T06=T01*T12*T23*T34*T45*T56;
    
    ne = T06(1:3,1);
    se = T06(1:3,2);
    ae = T06(1:3,3);
    Re = [ne se ae] % orientation of ee
    roll = rad2deg(atan2(ne(2), ne(1)));
    pitch = rad2deg(atan2(-ne(3),sqrt(se(3)^2+ae(3)^2)));
    yaw = rad2deg(atan2(se(3),ae(3)));

%     if theta6 < 90
%         theta6 = pi-theta6;
%     elseif theta6 > 90
%         theta6  = theta6;
%     end
    
    P0=[0 0 0];
    P1=transpose(T01(1:3,4));
    P2=transpose(T02(1:3,4));
    P3=transpose(T03(1:3,4));
    P4=transpose(T04(1:3,4));
    P5=transpose(T05(1:3,4));
    P6=transpose(T06(1:3,4));
    %%%%%%%%%%%%%%%%%%%%%%%
    Z0  = [0;0;1];Ori =[0;0;0]; O6=T06(1:3,4);
    Jv1 = cross(Z0,(O6-Ori));
    
    Z1  = T01(1:3,3);Ori_1=T01(1:3,4);
    Jv2 = cross(Z1,(O6-Ori_1));
    
    Z2  = T12(1:3,3);Ori_2=T12(1:3,4);
    Jv3 = cross(Z2,(O6-Ori_2));

    Z3  = T23(1:3,3);Ori_3=T23(1:3,4);
    Jv4 = cross(Z3,(O6-Ori_3));
    
    Z4  = T34(1:3,3);Ori_4=T34(1:3,4);
    Jv5 = cross(Z4,(O6-Ori_4));
    
    Z5  = T45(1:3,3);Ori_5=T45(1:3,4);
    Jv6 = cross(Z5,(O6-Ori_5));
    Jg=[Jv1 Jv2 Jv3 Jv4 Jv5 Jv6;
       Z0 Z1 Z2 Z3 Z4 Z5];
   
    B = [-sind(roll)*cosd(pitch)/sind(pitch) cosd(roll)*cosd(pitch)/sind(pitch) 1;
        cosd(roll) sind(roll) 0;
        sind(roll)/sind(pitch) -cosd(roll)/sind(pitch) 0];
    
    Ja =[eye(3) zeros(3);
        zeros(3) inv(B)] * Jg;
    Xinit=P6(1,1); 
    Yinit=P6(1,2);
    Zinit = P6(1,3);
    Rinit=roll; 
    Pinit=pitch;
    Yawinit = yaw;
    Xend=Xc;Yend=Yc; Zend = Zc;
    Rend=r_e; Pend=p_e; Yawend = yaw_e;
    Xspeed=(Xend-Xinit);
    Yspeed=(Yend-Yinit);
    Zspeed=(Zend-Zinit);
    Rspeed=(Rend-Rinit);
    Pspeed=(Pend-Pinit);
    Yawspeed=(Yawend-Yawinit);
    thetadot=pinv(Ja)*[Xspeed;Yspeed;Zspeed;Rspeed;Pspeed;Yawspeed];
    error_x = Xspeed^2;
    error_y = Yspeed^2;
    error_z = Zspeed^2;
    error_r = Rspeed^2;
    error_p = Pspeed^2;
    error_yaw = Yawspeed^2;
      
    if abs(error_x)<=0.2 && abs(error_y)<=0.2 && abs(error_z)<=0.2 && abs(error_r)<=0.5 && abs(error_p)<=0.5 && abs(error_yaw)<=0.5
        End_EE = T06;
        b=1;
    end
    theta1dot=thetadot(1,1);
    theta2dot=thetadot(2,1);
    theta3dot=thetadot(3,1);
    theta4dot=thetadot(4,1);
    theta5dot=thetadot(5,1);
    theta6dot=thetadot(6,1);
    
    deltatheta1=rad2deg(theta1dot);
    deltatheta2=rad2deg(theta2dot);
    deltatheta3=rad2deg(theta3dot);
    deltatheta4=rad2deg(theta4dot);
    deltatheta5=rad2deg(theta5dot);
    deltatheta6=rad2deg(theta6dot);
    Q1=[P0(1,1) P1(1,1) P2(1,1),P3(1,1),P4(1,1),P5(1,1)];
    Q2=[P0(1,2) P1(1,2) P2(1,2),P3(1,2),P4(1,2),P5(1,2)];
    Q3=[P0(1,3) P1(1,3) P2(1,3),P3(1,3),P4(1,3),P5(1,3)];
    plot3(Q1,Q2,Q3, '-o','LineWidth',4);
    axis([-700 700 -700 700 -700 700]);
    grid on
    pause(0.001);
    iteration = iteration +1;
end
Improve this question
$\endgroup$
1
  • $\begingroup$ Can you consider body/space Jacobean instead of analytical ? There seem to be some problem with your definition of roll,pitch and yaw angles. I can explain the answer to your problem in a slightly different way . Let me know if you are still looking to find the answer. $\endgroup$
    – McLovin
    Oct 14, 2022 at 21:39

0

Reset to default

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.