Inverse Kinematics of PUMA 560 robot

The mathematical model of the inverse kinematics of PUMA 560 robot manipulator defines the joint angles at each joint for a given transformation matrix T⁰₆. The general homogeneous matrix Tⁿ⁻¹_n consist the rotation and displacement vectors for a given joint angles. In section 2, the DH parameters in Table 1 is the basis for the link lengths and offsets in deriving the joint angles. The DH parameters is defined from the joint representation in Figure 1.

Frame	θ	α	r	d
1	θ 1	-90o	0	671.83 mm
2	θ 2	0o	431.80 mm	139.70 mm
3	θ 3	90o	-20.32 mm	0
4	θ 4	-90o	0	431.80 mm
5	θ 5	90o	0	0
6	θ 6	0o	0	56.50 mm

Table 1: DH Parameters of PUMA 560 Robot

In this paper, we derived the inverse kinematic of a PUMA 560 robot manipulator by decoupling method. We initially solve the joint angles from frame 0 to frame 3 and apply forward kinematics to build the homogeneous transformation matrix T⁰₃. This is discussed in section 2. For the joint angles in frame 4 to frame 5, we derive the equation defining the transformation matrix T³₆ and peg the resulting values from the dot product between the inverse of T⁰₃ and the transformation matrix T⁰₆ of the end-effector.

Furthermore, the inverse kinematic equation in section 2 is implemented in MATLAB. We evaluate solution of the inverse kinematic for PUMA 560 robot manipulator by comparing the result of fPUMA and iPUMA functions. The joint angles from the iPUMA function must be close to initiallize angle or to the end-effector in the fPUMA function.

2. Methods

Figure 1: Joint representation of PUMA 560 robot

2.1 Modelling of the Inverse Kinematics Equations

In inverse kinematic problem, we find a solution θ given a set of position and orientation of the end-effector. For PUMA 560 robot manipulator, we are given the DH model T⁰₆ which contains the orientation (rotation matrix) and the position (displacement matrix) of the end-effector. The angles at each joint is determined by decoupling the PUMA 560 robot manipulator. We initial solve the position (P_5x, P_5y, P_5z) of the frame 5 and used these coordinate values to determine the angles (θ₁, θ₂, θ₃) at the first three joint.

We start by determining the position of the frame 5 as shown in Figure 1. That is

Figure 2: x₀ − y₀ plane (Top View)

Next, we solve the angles at the first three joints by viewing PUMA 560 robot manipulator movement across x₀ − y₀ and x₀ − z₀ planes. In Figure 2, the joint angle θ1 is defined by the difference between angles φ₁ and α₁. The angle α₁ is defined as

where

Equation (2) can be expressed in terms of atan2 function. We define

and yield

The angle φ₁ is defined as

The joint angle θ₁ is derived as

Figure 3: x₀ − z₀ plane (Side View)

In Figure 3, we derived the equations to solve for the joint angles θ₁ and θ₂. We define the distances

and apply cosine law to determine angles, α2 and β. We get the equations

and

that gives the cosine value of α and β. We set

and

to define

and

The angle φ₂ is determined by

The joint angle θ₂ is the difference between angles φ₂ and α₂ as shown in Figure 3. That is

For the joint angle θ₃, we have

After we obtain the angles at the first three joints, we used the angles to solve for the forward kinematics for frame 0 to frame 3. We solve the homogeneous transformation matrix T³₆ for frame 3 to frame 6 by taking the dot product between T⁰₆ and the inverse of T⁰₃. In general, DH model for 6 degrees of freedom manipulator is defined as

We can simplify the model to T⁰₆ = T⁰₃T³₆ and define

The angles at joint 4 to 6 is determine by deriving a set of equations from the homogeneous transformation matrix T³₆. The rotation matrix R³₆ holds the orientation of the end-effector in reference to frame 3. That is

where the notation, sθ = sin θ and cθ = cos θ, is adapted. The R³₆ is a component in the homogeneous transformation matrix T³₆. We derived the joint angles θ₄, θ₅ and θ₆ by equating the numerical values of T³₆ in equation (26).

We obtain the joint angle θ₄ by

The joint angle θ₄ is defined as

For joint angle θ₅, we have cos θ₅ = T³₆(3, 3) and used this equation to find θ₅ in term of atan2 function. The joint θ₅ is defined as

Lastly, the joint angle θ₆ is derived by

We have the joint angle θ₆ as

2.2 MATLAB Implementation

In this section, we implement the derived inverse kinematic equations for PUMA 560 robot manipulator. We start by initializing the homogeneous transformation matrix T⁰₆ and the position of frame 5.

Listing 1: Initialize T⁰₆ and P for frame 5

%DH parameter
A =  [-90 0 90 -90 90 0] ; %twist angle
r = [0 431.80 -20.32 0 0 0]; %offset as to xn
d = [671.83 139.70 0 431.80 0 56.50]; %offset as to z(n-1)
%DH model
T0_6 = [nx ox ax Px; ny oy ay Py; nz oz az Pz; 0 0 0 1];
% Joint 5 position
P = [Px-56.50*ax; Py-56.50*ay; Pz-56.50*az];

Next, we implement the equations to solve the joint angles θ₁, θ₂ and θ₃ from the previous section. The MATLAB script is shown in listing 2.

Listing 2: Initialize solve the joint angles θ₁, θ₂ and θ₃

%Determining Joint angles for frames 1,2,and 3
C1 = sqrt(P(1)^2+P(2)^2);
C2 = P(3)-d(1);
C3 = sqrt(C1^2+C2^2);
C4 = sqrt(r(3)^2+d(4)^2);
D1 = d(2)/C1;
D2 = (C3^2+r(2)^2-C4^2)/(2*r(2)*C3);
D3 = (r(2)^2+C4^2-C3^2)/(2*r(2)*C4);

a1 = atan2d(D1,sqrt(abs(1-D1^2)));
a2 = atan2d(sqrt(abs(1-D2^2)),D2);
b = atan2d(sqrt(abs(1-D3^2)),D3);
p1 = atan2d(P(2),P(1));
p2 = atan2d(C2,C1);
%Joint angles: theta_1, theta_2, and theta_3
J = [p1-a1 round(a2-p2) round(b-90)];

Then, we write a script to solve the forward kinematics for frame 0 to frame 6 and the homogeneous transformation matrix T³₆. The script is in listing 3.

Listing 3: Forward kinematics for first three joint

%Apply forward kinematics at first three joints
T = [];
for n = 1:3
    matT = [cosd(J(n)) -sind(J(n))*cosd(A(n)) ...
            sind(J(n))*sind(A(n)) r(n)*cosd(J(n));
            sind(J(n)) cosd(J(n))*cosd(A(n)) ...
            -cosd(J(n))*sind(A(n)) r(n)*sind(J(n));
            0 sind(A(n)) cosd(A(n)) d(n);
            0 0 0 1];
        T = [T; {matT}];
end
T0_3 = T{1}*T{2}*T{3};
T3_6 = inv(T0_3)*T0_6;

Hence we already have the transformation matrix T³₆, we implement the equations to solve for the joint angles θ₄, θ₅ and θ₆, as shown in listing 4.

Listing 4: Solving joint angles θ₄, θ₅ and θ₆

%Joint angle: theta_4, theta_5, and theta_6
J4 = round(atan2d(T3_6(2,3),T3_6(1,3)));
J5 = round(atan2d(sqrt(abs(1-T3_6(3,3)^2)),T3_6(3,3)));
J6 = atan2d(T3_6(3,2),-T3_6(3,1));

Lastly, we solve the forward kinematics given the generated angles in the previous step and display a three-dimensional plot of the robot links and rotation.

All script discussed earlier was compiled to iPUMA function. The function accounts all derived equations for the inverse kinematics of PUMA 560 robot manipulator. The orientation as to the x-axis, y-axis and z-axis, and position of the end-effector are the input of the iPUMA function. Afterwhich, we compare the resulting angles and plot to the forward kinematics of PUMA 560 robot manipulator. This is to check if the derived equation arrived to the same end-effector point as generated by the forward kinematic model. The
full MATLAB script is shown in listing 5.

Listing 5: iPUMA function

function iPUMA(nx, ny, nz, ox, oy, oz, ax, ay, az, Px, Py, Pz)
%input: end-effector position
%ouput: joint angles at each frames

%initial condition: iPUMA(1,0,0,0,1,0,0,0,1,411.50,139.70,1160.10)
format compact
format short
%DH parameter
A =  [-90 0 90 -90 90 0] ; %twist angle
r = [0 431.80 -20.32 0 0 0]; %offset as to xn
d = [671.83 139.70 0 431.80 0 56.50]; %offset as to z(n-1)
%DH model
T0_6 = [nx ox ax Px; ny oy ay Py; nz oz az Pz; 0 0 0 1];
% Joint 5 position
P = [Px-56.50*ax; Py-56.50*ay; Pz-56.50*az];

%Determining Joint angles for frames 1,2,and 3
C1 = sqrt(P(1)^2+P(2)^2);
C2 = P(3)-d(1);
C3 = sqrt(C1^2+C2^2);
C4 = sqrt(r(3)^2+d(4)^2);
D1 = d(2)/C1;
D2 = (C3^2+r(2)^2-C4^2)/(2*r(2)*C3);
D3 = (r(2)^2+C4^2-C3^2)/(2*r(2)*C4);

a1 = atan2d(D1,sqrt(abs(1-D1^2)));
a2 = atan2d(sqrt(abs(1-D2^2)),D2);
b = atan2d(sqrt(abs(1-D3^2)),D3);
p1 = atan2d(P(2),P(1));
p2 = atan2d(C2,C1);
%Joint angles: theta_1, theta_2, and theta_3
J = [p1-a1 round(a2-p2) round(b-90)];

%Apply forward kinematics at first three joints
T = [];
for n = 1:3
    matT = [cosd(J(n)) -sind(J(n))*cosd(A(n)) ...
            sind(J(n))*sind(A(n)) r(n)*cosd(J(n));
            sind(J(n)) cosd(J(n))*cosd(A(n)) ...
            -cosd(J(n))*sind(A(n)) r(n)*sind(J(n));
            0 sind(A(n)) cosd(A(n)) d(n);
            0 0 0 1];
        T = [T; {matT}];
end
T0_3 = T{1}*T{2}*T{3};
T3_6 = inv(T0_3)*T0_6;

%Joint angle: theta_4, theta_5, and theta_6
J4 = round(atan2d(T3_6(2,3),T3_6(1,3)));
J5 = round(atan2d(sqrt(abs(1-T3_6(3,3)^2)),T3_6(3,3)));
J6 = atan2d(T3_6(3,2),-T3_6(3,1));

J = [J J4 J5 J6];
%Plotting the result
if J(1,1) >= -160 && J(1,1) <= 160 && J(1,2)>= -225 ...
        && J(1,2) <= 45 && J(1,3) >= -45 && J(1,3) <= 225 ...
        && J(1,4) >= -110 && J(1,4) <= 170 && J(1,5) >= -100 ...
        && J(1,5) <= 100 && J(1,6) >= -266 && J(1,6) <= 266
    T = [];
    for n = 1:6
        matT = [cosd(J(n)) -sind(J(n))*cosd(A(n)) ...
            sind(J(n))*sind(A(n)) r(n)*cosd(J(n));
            sind(J(n)) cosd(J(n))*cosd(A(n)) ...
            -cosd(J(n))*sind(A(n)) r(n)*sind(J(n));
            0 sind(A(n)) cosd(A(n)) d(n);
            0 0 0 1];
        T = [T; {matT}];
    end
    P = [];
    for i = 1:6
        if i == 1
            P = [P,{T{i}}];
        else 
            matP = P{i-1}*T{i};
            P = [P, {matP}];
        end
    end
    x = [0 P{1}(1,4) P{1}(1,4) P{2}(1,4) ...
        P{3}(1,4) P{4}(1,4) P{5}(1,4) P{6}(1,4)];
    y = [0 P{1}(2,4) P{2}(2,4) P{2}(2,4) ...
        P{3}(2,4) P{4}(2,4) P{5}(2,4) P{6}(2,4)];
    z = [0 P{1}(3,4) P{1}(3,4) P{2}(3,4) ...
        P{3}(3,4) P{4}(3,4) P{5}(3,4) P{6}(3,4)];
    hold on
    grid on
    rotate3d on
    plot3([x(1) x(2)],[y(1) y(2)],[z(1) z(2)]...
        ,'Color','b','LineWidth',5)
    plot3([x(2) x(3)],[y(2) y(3)],[z(2) z(3)]...
        ,'Color','r','LineWidth',5)
    plot3([x(3) x(4)],[y(3) y(4)],[z(3) z(4)]...
        ,'Color','g','LineWidth',5)
    plot3([x(4) x(5)],[y(4) y(5)],[z(4) z(5)]...
        ,'Color','c','LineWidth',5)
    plot3([x(5) x(6)],[y(5) y(6)],[z(5) z(6)]...
        ,'Color','m','LineWidth',5)
    plot3([x(6) x(7)],[y(6) y(7)],[z(6) z(7)]...
        ,'Color','y','LineWidth',5)
    plot3([x(7) x(8)],[y(7) y(8)],[z(7) z(8)]...
        ,'Color','k','LineWidth',5)
    plot3(x,y,z,'o','Color','k')
    xlabel('x-axis')
    ylabel('y-axis')
    zlabel('z-axis');
    title('Inverse Kinematics of PUMA 560 Manipulator (6DoF)')
    view(0,0)
    disp('The joint angles  are:   ');
    fprintf('theta1 = %f, theta2 = %f, theta3 = %f '...
        ,J(1),J(2),J(3));
    fprintf('theta4 = %f, theta5 = %f, and theta6 = %f  '...
        ,J(4),J(5),J(6));   
else
    disp('Resulting joint angles is out of range');
end

3. Result

In this section, we present the result of the MATLAB implementation and validate it by comparing the it to the forward kinematic model. Initially, we set the input as

at initial condition where all joint angles are zero. The iPUMA function result to a joint angles: θ₁ = 0, θ₂= 0, θ₃= 0, θ₄ = 0, θ₅ = 0, and θ₆ = 0. Figure 4 shows the plot indicating the orientation and position of each frames of the robot manipulator. At initial condition, we observed that both the forward and inverse kinematics solution yield same values of joint angles, and orientation and position of end-effector.

(a) fPUMA function

(b) iPUMA function

Figure 4: Results at initial condition

Next, we set the

and have the joint angles as θ₁ = 44.999712, θ₂ = 43.000000, θ₃ = 46.000000, θ₄ = 44.000000, θ₅ = 46.000000 and θ₆ = 45.987531. We note that the input matrix is yield from all joint angles set to 45^o. The resulting angles are close to 45^o except for 1^o to 2^ooffset in joint angles. This is due to truncation and rounding off error in the inverse trigonometric functions in the code. Figure 5 shows the resulting plot.

(a) fPUMA function

(b) iPUMA function

Figure 5: Results at joints angles set to 45^o

Furthermore, we set are transformation matrix as

for all joint angles set to zero except θ₂, = −90^o.

(a) fPUMA function

(b) iPUMA function

(c) Combination view at x-z plane

(d) Combination 3D view

Figure 6: Results at zero except θ₂, = −90^o

The iPUMA function result to θ₁ = 144.144160, θ₂ = 3.000000, θ₃ = 0.000000, θ₄ = 36.000000, θ₅ = 88.000000, and θ₆ = 178.241701. For this simulation, it is evident that the angles are have large offset as to the actual angles for the given end-effector however the set of joint angles still reaches the same end-effector point as shown in Figure 6. This is unique to the solution of inverse kinematics. It generate multiple combination of angles that adhere to the given orientation and position of an end-effector.

4. Conclusion

In this paper, we modelled the inverse kinematics of PUMA 560 robot manipulator. We derived the equations of the joint angles by decoupling the PUMA 560 frames. We initial solve for frame 0 to frame 3 and then the last remaining frames. A detailed derivation of equations for the joint angles are in section 2. The inverse kinematic equations was implemented in MATLAB and verify the result by comparing it with the forward kinematics. In section 3, the iPUMA function generates the joint angle with minimal error. We also observed that in some cases, the angles generated are not the same with the basis but still reaches the same end-effector. Thus, inverse kinematic of a 6 degrees of freedom robot manipulator has multiple possible solution for a given orientation and position of the end-effector.

5. References

[1] Lynch, K. and F. Park. “Modern Robotics: Mechanics, Planning, and Control.” (2017).

[2] John J. Craig. 1989. Introduction to Robotics: Mechanics and Control (2nd. ed.). Addison-Wesley Longman Publishing Co., Inc., USA.

[3] Merat, Frank. (1987). Introduction to robotics: Mechanics and control. Robotics and Automation, IEEE Journal of. 3. 166 - 166. 10.1109/JRA.1987.1087086

[4] M. Aghajarian and K. Kiani, "Inverse Kinematics solution of PUMA 560 robot arm using ANFIS," 2011 8th International Conference on Ubiquitous Robots and Ambient Intelligence (URAI), Incheon, 2011, pp. 574-578, doi: 10.1109/URAI.2011.6145885.

[5] Singh, T. P., Suresh, P., & Chandan, S. (2017). Forward and inverse kinematic analysis of robotic manipulators. International Research Journal of Engineering and Technology (IRJET), 4(2), 1459-1468.

[6] Singh, G., Banga, V. K., & Yingthawornsuk, T. (2019, November). Inverse Kinematics Solution of Programmable Universal Machine for Assembly (PUMA) Robot. In 2019 15th International Conference on Signal-Image Technology & Internet-Based Systems (SITIS) (pp. 518-524). IEEE.

[7] Hayat, Abdullah & Chittawadigi, Rajeevlochan & Udai, Arun & Saha, Subir. (2013). Identification of Denavit-Hartenberg Parameters of an Industrial Robot. ACM International Conference Proceeding Series. 1-6. 10.1145/2506095.2506121.

[8] Aristidou, Andreas & Lasenby, Joan. (2009). Inverse Kinematics: a review of existing techniques and introduction of a new fast iterative solver.

[9] LIU, Hua-shan & Zhou, Wuneng & Lai, Xiaobo & Zhu, Shiqiang. (2013). An Efficient Inverse Kinematic Algorithm for a PUMA560-Structured Robot Manipulator. International Journal of Advanced Robotic Systems. 10. 1. 10.5772/56403.

(Note: All images in the text are drawn by the author except those with attached citations. The cover 3D model of PUMA 560 is created by Gonzalo Loredo Neri and is free to download at grabcad.com))

---

If your interested with the forward and inverse kinematics of robots, check these post:
1. Forward Kinematics of PUMA 560 Robot using DH Method
2. Forward and inverse kinematics of 3R Planar Manipulator