# How can i consider orientation of 6dof manipulator for inverse kinematic?

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

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

%     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);
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