1
$\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
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.