Analytic Inverse Kinematics v1.1

Analytic Inverse Kinematics

Inverse kinematics described in this technical paper uses an analytic method and is calculated based on standard DH parameters.

Standard D-H Parameter

• A rotation 𝜃_𝑖 about 𝑧_𝑖−1 axis
• A translation 𝑑_𝑖 along the 𝑧_𝑖−1 axis
• A translation 𝑎_𝑖 along the 𝑥_𝑖−1 axis
• A rotation 𝛼_𝑖 about 𝑥_𝑖−1 axis

	d1	d2	d3	d4	d5	d6	a1	a2
RB5-850	169.2	148.4	148.4	110.7	110.7	96.7	425.0	392.0
RB3-1200	169.2	148.4	148.4	110.7	110.7	96.7	566.9	522.4
RB10-1300	197.0	187.5	148.4	117.15	117.15	115.3	612.7	570.15

Standard(Spong)
link i	θi	di	ai	αi
L1	θ1	d1	0	-90
L2	θ2 - 90	-d2	0	0
L3	0	0	a1	0
L4	θ3	d3	0	0
L5	0	0	a2	0
L6	θ4 + 90	-d4	0	0
L7	0	0	0	90
L8	θ5	d5	0	-90
L9	θ6	-d6	0	90

Because coordinate system 5 and coordinate system 6 differ by 90 degrees,

% -------------------------------------------------
% Rainbow Robotics
% Analytic Inverse Kinematics Example Code (based on Octave)
% All Right Reserved.
% -------------------------------------------------

clc; clear all; close all;
% -------------------------------------------------
D2R = pi/180.;
R2D = 180./pi;
% -------------------------------------------------
disp('---------------------------------');
disp('Input Cartesian Value (mm & deg)');
disp('---------------------------------');
% Rainbow Robotics use ZYX euler notation
% Z -> Y' -> X''
input_x = -156.76
input_y = -155.15
input_z = 814.96
input_rx = -43.47
input_ry = 80.56
input_rz = -60.88
% -------------------------------------------------
x = input_x;
y = input_y;
z = input_z;
rx = input_rx*D2R;
ry = input_ry*D2R;
rz = input_rz*D2R;
% -------------------------------------------------
% Link Length parameter (RB5-850)
d1 = 169.2;
d2 = 148.4;
d3 = 148.4;
d4 = 110.7;
d5 = 110.7;
d6 = 96.7;

a1 = 425.0;
a2 = 392.0;
% -------------------------------------------------
Rz = [cos(rz) -sin(rz) 0;
      sin(rz) cos(rz) 0;
      0 0 1;];

Ry = [cos(ry) 0 sin(ry);
      0 1 0;
      -sin(ry) 0 cos(ry);];

Rx = [1 0 0;
      0 cos(rx) -sin(rx);
      0 sin(rx) cos(rx);];

R = Rz * Ry * Rx;
% -------------------------------------------------

Y06 = R(:, 2);
P06 = [x;y;z];


P05 = P06 + d6*Y06;

th1 = atan2(P05(2), P05(1)) - acos(d4/sqrt(P05(2)^2 + P05(1)^2))+0.5*pi;
th5 = +acos((sin(th1)*P06(1)-cos(th1)*P06(2)-d4)/d6);
th6 = atan2(-(-sin(th1)*R(1,1)+cos(th1)*R(2,1))/sin(th5), (-sin(th1)*R(1,3)+cos(th1)*R(2,3))/sin(th5))+0.5*pi;


A01 = [cos(th1) -cos(-pi*0.5)*sin(th1) sin(-pi*0.5)*sin(th1) 0;
       sin(th1) cos(-pi*0.5)*cos(th1) -sin(-pi*0.5)*cos(th1) 0;
       0 sin(-pi*0.5) cos(-pi*0.5) d1;
       0 0 0 1];

A67 = [cos(0) -cos(pi*0.5)*sin(0) sin(pi*0.5)*sin(0) 0;
       sin(0) cos(pi*0.5)*cos(0) -sin(pi*0.5)*cos(0) 0;
       0 sin(pi*0.5) cos(pi*0.5) 0;
       0 0 0 1];

A78 = [cos(th5) -cos(-pi*0.5)*sin(th5) sin(-pi*0.5)*sin(th5) 0;
       sin(th5) cos(-pi*0.5)*cos(th5) -sin(-pi*0.5)*cos(th5) 0;
       0 sin(-pi*0.5) cos(-pi*0.5) d5;
       0 0 0 1];

A89 = [cos(th6) -cos(pi*0.5)*sin(th6) sin(pi*0.5)*sin(th6) 0;
       sin(th6) cos(pi*0.5)*cos(th6) -sin(pi*0.5)*cos(th6) 0;
       0 sin(pi*0.5) cos(pi*0.5) -d6;
       0 0 0 1];

A17 = inv(A01)*[R P06;0 0 0 1]*inv(A89)*inv(A78)*inv(A67);

P14 = [A17(1,4);
       A17(2,4);
       A17(3,4)];

th3 = +acos((P14(1)^2+P14(2)^2-a1^2-a2^2)/(2*a1*a2));
th2 = atan2(P14(1), -P14(2))-asin(a2*sin(th3)/sqrt(P14(1)^2+P14(2)^2));

A12 = [cos(th2-pi*0.5) -cos(0)*sin(th2-pi*0.5) sin(0)*sin(th2-pi*0.5) 0;
       sin(th2-pi*0.5) cos(0)*cos(th2-pi*0.5) -sin(0)*cos(th2-pi*0.5) 0;
       0 sin(0) cos(0) -d2;
       0 0 0 1];

A23 = [cos(0) -cos(0)*sin(0) sin(0)*sin(0) a1*cos(0);
       sin(0) cos(0)*cos(0) -sin(0)*cos(0) a1*sin(0);
       0 sin(0) cos(0) 0;
       0 0 0 1];

A34 = [cos(th3) -cos(0)*sin(th3) sin(0)*sin(th3) 0;
       sin(th3) cos(0)*cos(th3) -sin(0)*cos(th3) 0;
       0 sin(0) cos(0) d3;
       0 0 0 1];

A45 = [cos(0) -cos(0)*sin(0) sin(0)*sin(0) a2*cos(0);
       sin(0) cos(0)*cos(0) -sin(0)*cos(0) a2*sin(0);
       0 sin(0) cos(0) 0;
       0 0 0 1];

A56_cal = inv(A45)*inv(A34)*inv(A23)*inv(A12)*inv(A01)*[R P06;0 0 0 1]*inv(A89)*inv(A78)*inv(A67);

th4 = atan2(A56_cal(2,1), A56_cal(1,1))-0.5*pi;
% -------------------------------------------------
disp('---------------------------------');
disp('Inverse Kinematics Result (deg)');
disp('---------------------------------');
th1 = th1 * R2D
th2 = th2 * R2D
th3 = th3 * R2D
th4 = th4 * R2D
th5 = th5 * R2D
th6 = th6 * R2D