Robot: 6R-PUMA Type

PUMA 6R Robot Schematic

  • Joint 1 is the base/trunk joint
  • Joints 2-3 for Shoulder & elbow
  • Joints 4-6 forming the spherical wrist

Existence of Analytical Solution

According to the Pieper criterion, an analytical solution exists for a 6-DOF manipulator if the last three revolute joint axes intersect at a single point (forming a spherical wrist).

This criterion justifies splitting the inverse kinematics problem into:

  • Position inverse kinematics of the wrist center
  • Orientation inverse kinematics of the end effector

Position Inverse Kinematics

The wrist center is given by:

\[\vec{p_w} = [w_x, w_y, w_z]^T\]

Assigning joint angle symbols:

Joint Symbol
Joint 1 \(\theta_1\)
Joint 2 \(\theta_2\)
Joint 3 \(\theta_3\)

Breaking into subproblems

  • Base rotation in XY plane
  • 2R planar robot

Base rotation in XY plane

\(\theta_1 = atan2(w_y, w_x)\)

2R Planar Robot

Considering joint 2 (\(J_2\)), joint 3 (\(J_3\)) and the wrist center \(p_w\). Finding the joint angles \((\theta_2, \theta_3)\) using Geometry.

Joint centers:

Joint Center
\(\vec{J_{\text{2c}}}\) \([q_{\text{2x}}, q_{\text{2y}}, q_{\text{2z}}]^T\)
\(\vec{J_{\text{3c}}}\) \([q_{\text{3x}}, q_{\text{3y}}, q_{\text{3z}}]^T\)

Offset from fixed frame:

  • \(J_2\): \(\vec{d_2}\)
  • \(J_3\): \(\vec{d_3}\)

Ensure a triangle solution exists

Mapping links to sides:

Finding the Euclidean distance between points, which will serve as the side length.

Side Link
\(a\) \(|\vec{J3} - \vec{J2}|\)
\(b\) \(|\vec{J3} - \vec{p_w}|\)
\(c\) \(|\vec{J2} - \vec{p_w}|\)

Mapping angles:

Angle between sides Subproblem angle symbol
\(ab\) \(\beta\)
\(bc\) \(\gamma\)
\(ac\) \(\delta\)

Applying triangle inequality to the 2R planar robot configuration:

\[|a - b| < c < a + b\]

Applying law of cosines

  • For \(\beta\):

    \[c^2 = a^2 + b^2 - 2ab \cos(\beta)\] \[\beta = \cos^{-1}\!\left(\frac{a^2 + b^2 - c^2}{2ab}\right)\]

  • For \(\delta\):

    \[b^2 = a^2 + c^2 - 2ac \cos(\delta)\] \[\delta = \cos^{-1}\!\left(\frac{a^2 + c^2 - b^2}{2ac}\right)\]

Note: Two solutions exist (elbow-up and elbow-down).

Concluding joint angles:

Joint angle Expression
\(\theta_1\) \(\arctan2(w_y, w_x)\)
\(\theta_2\) \(\pm \delta\)
\(\theta_3\) \(\pm \beta\)

Orientation Inverse Kinematics of the End-Effector

The forward kinematics of the end-effector is captured by the Product of Exponentials (PoE) formula:

\[T_{\text{target}} = e^{[S_1] \theta_1} e^{[S_2] \theta_2} e^{[S_3] \theta_3} e^{[S_4] \theta_4} e^{[S_5] \theta_5} e^{[S_6] \theta_6} M\]

where:

  • \(T_{\text{target}}\) represents the pose of the target object in the global frame.
  • Each \(e^{[S_i] \theta_i}\) represents a homogeneous transformation, where \(S_i\) is the screw axis and \([S_i] \theta_i\) is its skew-symmetric matrix scaled by the joint angle.
  • \(M\) is the pose of the end-effector in its home configuration.

Note: \(M\) represents the home pose of the chosen end-effector frame — it can be at joint 6 or include a tool offset. The derivation of wrist angles remains unchanged regardless of this choice.

Grouping by sub-problem:

\[T_{\text{target}} = T_{\text{1\_3}} \cdot T_{\text{4\_6}} \cdot M\]

Here:

  • \(T_{1\_3}\) is the transformation from the base to the wrist (composed of the first three joint transformations).
  • \(T_{4\_6}\) is the transformation of the wrist itself.

Since we have already solved for the first three joint angles (\(\theta_1, \theta_2, \theta_3\)), the transformation \(T_{1\_3}\) is known. We can rearrange the equation to isolate the unknown wrist transformation, \(T_{4\_6}\):

Rearranging:

\[(T_{1\_3})^{-1} \cdot T_{\text{target}} = T_{4\_6} \cdot M\]

Inverse of a transform

Since the determinant of a Special Euclidean transformation is the same as that of its rotation subgroup:

\[SE(3) = \det(SO(3)), \quad \det(T) = \det(R) = 1\]

Hence, the inverse of the rotation matrix is simply its transpose, For a homogeneous transform:

\[T = \begin{bmatrix} R & \vec{p} \\ 0 & 1 \end{bmatrix}\]

We require that:

\[T \cdot T^{-1} = I\]

Assume:

\[T^{-1} = \begin{bmatrix} X & \vec{q} \\ 0 & 1 \end{bmatrix}\]

Multiplying out:

\[\begin{bmatrix} R & \vec{p} \\ 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} X & \vec{q} \\ 0 & 1 \end{bmatrix}\] \[\begin{bmatrix} RX & R \vec{q}+\vec{p}\\ 0 & 1 \end{bmatrix} = \begin{bmatrix} I & 0 \\ 0 & 1 \end{bmatrix}\]

From which:

\[R X = I \quad \Rightarrow \quad X = R^{-1}\] \[R \vec{q}+\vec{p} = 0 \quad \Rightarrow \vec{q} = - R^{-1}\vec{p}\]

Therefore:

\[T^{T} = \begin{bmatrix} R^{T} & -R^{T} \vec{p} \\ 0 & 1 \end{bmatrix}\]

Returning to the equation

\[T_{\text{1\_3}}^T \cdot T_{\text{target}} = T_{\text{4\_6}} \cdot M\]

Isolating \(T_{\text{4\_6}}\):

\[T_{\text{4\_6}} = (T_{\text{1\_3}}^T \cdot T_{\text{target}}) M^{-1}\]

Simplify terms:

\[T_{\text{4\_6}} = T_{\text{partial}} M^{-1}\]

Comparing components

\[\begin{bmatrix} R_{\text{4\_6}} & p_{\text{4\_6}} \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} R_{\text{partial}} & \vec{p_\text{partial}} \\ 0 & 1 \end{bmatrix}\]

Equating the unknown part (orientation):

\[R_{\text{4\_6}} = R_{\text{partial}}\]

Expansion of Wrist Orientation Matrix

The wrist transformation can be expressed as the product of three joint rotations:

\[R_{4\_6} = R_4(\theta_4) \, R_5(\theta_5) \, R_6(\theta_6)\]

where:

  • \(R_4(\theta_4)\) is the rotation about the axis of joint 4,
  • \(R_5(\theta_5)\) is the rotation about the axis of joint 5,
  • \(R_6(\theta_6)\) is the rotation about the axis of joint 6.

Define placeholders, For clarity, let:

\[\{c_i, s_i\} = \{\cos(\theta_i), \sin(\theta_i)\}, \quad \forall i \in \{4,5,6\}\]

Typical PUMA-style wrist configuration, In a PUMA-type 6R arm, the axes are often arranged as:

  • Joint 4: rotation about x-axis
  • Joint 5: rotation about y-axis
  • Joint 6: rotation about x-axis

Thus:

\[R_4(\theta_4) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & c_4 & -s_4 \\ 0 & s_4 & c_4 \end{bmatrix}\] \[R_5(\theta_5) = \begin{bmatrix} c_5 & 0 & s_5 \\ 0 & 1 & 0 \\ - s_5 & 0 & c_5 \end{bmatrix}\] \[R_6(\theta_6) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & c_6 & -s_6 \\ 0 & s_6 & c_6 \end{bmatrix}\]

Multiply them, first:

\[R_{45} = R_4 R_5 = \begin{bmatrix} c_5 & 0 & s_5 \\ s_4 s_5 & c_4 & -s_4 c_5 \\ - c_4 s_5 & s_4 & c_4 c_5 \end{bmatrix}\]

Then:

\[R_{4\_6} = R_{45} R_6\] \[R_{4\_6} = \begin{bmatrix} c_5 & s_5 s_6 & s_5 c_6 \\ s_4 s_5 & c_4 c_6 - s_4 c_5 s_6 & -c_4 s_6 - s_4 c_5 c_6 \\ - c_4 s_5 & s_4 c_6 + c_4 c_5 s_6 & -s_4 s_6 + c_4 c_5 c_6 \end{bmatrix}\]

Equating with partial

Recall:

\[R_{4\_6} = R_{\text{partial}}\]

So each element of \(R_{4\_6}\) gives an equation in terms of \(\theta_4, \theta_5, \theta_6\) that must match the corresponding entry of \(R_{\text{partial}}\).

This forms the system of equations we solve for the wrist joint angles.

Image credit: Chegg, link