Inverse kinematics
Based on Section 2.12 of Robotics: Modelling, Planning and Control.
Link to the practical session on Inverse Kinematics
Problem definition and solutions perspective
The inverse kinematics (IK) problem copes with the determination of the joint variables corresponding to a given end-effector pose.
This is not an easy problem because:
The equations to solve are in general non-linear \(\implies\) not always a close form exists
Multiple solutions may exist
There might be no admissible solutions due to kinematic structure
About the solutions of the IK problem:
The existence of solutions is guaranteed iff the pose of the end effector belongs to the dexterous workspace.
In general, a 6 DOF manipulator without mechanical limits can have up to 16 admissible solutions (the greater the number of non null DH parameters, the greater the number of admissible solutions).
The IK problem can be solved following two approaches:
In closed form, computed with:
geometric solutions (used in Solution for the three-link planar arm)
algebraic solutions (used in Solution for the three-link planar arm)
Numerically (See section Inverse kinematics algorithms)
A six DOF manipulator has a closed form solution if:
three consecutive joints intersect at a common point (like in the case of the spherical wrist).
three consecutive revolute joint axes are parallel.
Solution for the three-link planar arm
The forward kinematics gave the pose of the End-Effector as:
In this example, we will use a geometric solution following these steps:
Considering the orientation we have:
\[\phi = \theta_1 + \theta_2 + \theta_3\]
Considering the origin of frame 2 (point W) we have:
\[P_{W_x} = P_x - a_3\,c_\phi = a_1\,c_1 + a_2\,c_{12}\]\[P_{W_y} = P_y - a_3\,s_\phi = a_1\,s_1 + a_2\,s_{12}\]
The cosine theorem gives:
\[P_{W_x}^2 + P_{W_y}^2 = a_1^2 + a_2^2 - 2\,a_1\,a_2\,cos(\pi - \theta_2)\]
Then, since \(cos(\pi - \theta_2) = -\,cos(\theta_2)\)
\[\implies \, c_2 = \frac{P_{W_x}^2 + P_{W_y}^2 - a_1^2 - a_2^2}{2\,a_1\,a_2}\]
The following must be satisfied \(\sqrt{P_{W_x}^2 + P_{W_y}^2} \;\le\; a_1 + a_2\) i.e., the pose has to belong to the reachable workspace. Then:
\[\begin{split}\theta_2 = \pm \text{acos}(c_2) \quad \left\{ \begin{array}{l} \theta_2 \in (-\pi,0) \;\Rightarrow\;\text{elbow up}\\ \theta_2 \in (0,\pi) \;\Rightarrow\;\text{elbow down} \end{array} \right.\end{split}\]
Now, find \(\theta_1\) by considering angles \(\alpha\) and \(\beta\), i.e.,
\[\alpha - \beta = \theta_1\]\[\alpha = \text{atan2}(P_{W_y},\,P_{W_x})\]
And using the cosine theorem again:
\[a_2^2 = a_1^2 + \left(P_{W_x}^2 + P_{W_y}^2\right) - 2\,a_1\,\sqrt{P_{W_x}^2 + P_{W_y}^2}\;\cos\beta\]\[\beta = \text{acos}\!\left(\frac{a_1^2 + P_{W_x}^2 + P_{W_y}^2 - a_2^2} {2\,a_1\,\sqrt{P_{W_x}^2 + P_{W_y}^2}}\right)\]\[\begin{split}\theta_1 = \alpha \pm \beta \quad \left\{ \begin{array}{l} \text{positive for }\theta_2 < 0\\ \text{negative for }\theta_2 > 0 \end{array} \right.\end{split}\]\[\theta_1 = \text{atan2}(P_{W_y},\,P_{W_x}) \pm \text{acos}\!\left(\frac{a_1^2 + P_{W_x}^2 + P_{W_y}^2 - a_2^2} {2\,a_1\,\sqrt{P_{W_x}^2 + P_{W_y}^2}}\right)\]
Finally,
\[\theta_3 = \phi - \theta_1 - \theta_2\]
Solution for a manipulator with a spherical wrist
The solution of the IK problem in this case defines, like in the previous example of the three-link planar manipulator, a point W whose position depends on:
\[\begin{split}\left\{ \begin{array}{l} \text{a reduced number of joints},\\ \text{the end-effector pose} \end{array} \right.\end{split}\]
The choice of point W makes it possible to decouple the IK problem into two independent problems:
\[\begin{split}\left\{ \begin{array}{l} \text{position}\\ \text{orientation} \end{array} \right.\end{split}\]
That is, this point W will be located at the end of the arm, where the 3 axes of the wrist intersect. This assumes that:
the first three joints locate that point in space
the last three joints orient the End-Effector
i.e. a decoupling between position and orientation is effective, and two simpler independent problems need now to be solved:
Solution for an anthropomorphic arm
The position subproblem considered here assumes an anthropomorphic arm (other arms like the spherical arm can be considered).
The position of W (coincident with the origin of frame 3) is:
In this example, we will use an algebraic solution following these three steps:
1. Compute :math:`theta_3`:
Let’s square and sum the coordinates of point W:
\[P_{W_x}^2 + P_{W_y}^2 + P_{W_z}^2 = a_2^2 + a_3^2 + 2\,a_2\,a_3\,c_3\]Then:
\[c_3 = \frac{P_{W_x}^2 + P_{W_y}^2 + P_{W_z}^2 \;-\; a_2^2 \;-\; a_3^2} {2\,a_2\,a_3}, \;\; \text{and the solution is admissible if } -1 \le c_3 \le 1\]i.e., if \(\lvert a_2 - a_3\rvert \le \sqrt{P_{W_x}^2 + P_{W_y}^2 + P_{W_z}^2} \le a_2 + a_3\), W is inside the reachable workspace.
Then:
\[\begin{split}s_3 = \pm \sqrt{1 - c_3^2} \quad \left\{ \begin{array}{l} s_3^+ = +\sqrt{1 - c_3^2}\\ s_3^- = -\sqrt{1 - c_3^2} \end{array} \right.\end{split}\]Finally:
\[\begin{split}\theta_3 = \text{atan2}(s_3,\,c_3) \quad \left\{ \begin{array}{l} \theta_{3_I} = \text{atan2}(s_3^+,\,c_3)\in[-\pi,\pi]\\ \theta_{3_{II}} = \text{atan2}(s_3^-,\,c_3) = -\theta_{3_I} \end{array} \right.\end{split}\]
2. Compute \(\theta_2\):
Start by squaring and summing \(P_{W_x}\) and \(P_{W_y}\):
\[P_{W_x}^2 + P_{W_y}^2 = \bigl(a_2\,c_2 + a_3\,c_{23}\bigr)^2\]Then, considering also \(P_{W_z}\) we have the following system of equations:
\[\begin{split}\left. \begin{array}{l} a_2\,c_2 + a_3\,c_{23} = \pm\sqrt{P_{W_x}^2 + P_{W_y}^2}\\ P_{W_z} = a_2\,s_2 + a_3\,s_{23} \end{array} \right\}\end{split}\]And, considering that \(c_{23} = c_2 c_3 - s_2 s_3\) and \(s_{23} = s_2 c_3 + c_2 s_3\), the solution is:
\[c_2 = \pm \frac{\sqrt{P_{W_x}^2 + P_{W_y}^2}\,(a_2 + a_3\,c_3) \;+\;P_{W_z}\,a_3\,s_3} {a_2^2 + a_3^2 + 2\,a_2\,a_3\,c_3}\]\[s_2 = \frac{P_{W_z}\,(a_2 + a_3\,c_3) \;\mp\;\sqrt{P_{W_x}^2 + P_{W_y}^2}\,a_3\,s_3} {a_2^2 + a_3^2 + 2\,a_2\,a_3\,c_3}\]and:
\[\begin{split}\theta_2 = \text{atan2}(s_2,\,c_2) \quad \left\{ \begin{array}{l} \theta_{2_I}\;\text{when using }s_3^+\text{ and }+\sqrt{\,\,}\\ \theta_{2_{II}}\;\text{when using }s_3^+\text{ and }-\sqrt{\,\,}\\ \theta_{2_{III}}\;\text{when using }s_3^-\text{ and }+\sqrt{\,\,}\\ \theta_{2_{IV}}\;\text{when using }s_3^-\text{ and }-\sqrt{\,\,} \end{array} \right.\end{split}\]
3. Compute \(\theta_1\)
From the forward kinematics we had:
\[\begin{split}\begin{aligned} P_{W_x} &= c_1\,(a_2\,c_2 + a_3\,c_{23})\\ P_{W_y} &= s_1\,(a_2\,c_2 + a_3\,c_{23}) \end{aligned} \quad\xrightarrow{\text{see above}}\quad \left\{ \begin{array}{l} P_{W_x} = c_1\left(\pm\sqrt{P_{W_x}^2 + P_{W_y}^2}\right),\\ P_{W_y} = s_1\left(\pm\sqrt{P_{W_x}^2 + P_{W_y}^2}\right) \end{array} \right.\end{split}\]Then:
\[\begin{split}\theta_1 = \text{atan2}(s_1,\,c_1) \quad\Rightarrow\quad \left\{ \begin{array}{l} \theta_{1_I} = \text{atan2}(P_{W_y},\,P_{W_x})\;\text{when using }+\sqrt{\,\,}\\ \theta_{1_{II}} = \text{atan2}(-P_{W_y},\,-P_{W_x})\;\text{when using }-\sqrt{\,\,} \end{array} \right.\end{split}\]As a result we have four possible triplets of solutions:
\[\begin{split}\begin{array}{|c|c|c|} \hline & s_3^+ & s_3^- \\ \hline +\sqrt{\,\,} & (\theta_{1_I},\,\theta_{2_I},\,\theta_{3_I}) & (\theta_{1_I},\,\theta_{2_{III}},\,\theta_{3_{II}}) \\ \hline -\sqrt{\,\,} & (\theta_{1_{II}},\,\theta_{2_{II}},\,\theta_{3_I}) & (\theta_{1_{II}},\,\theta_{2_{IV}},\,\theta_{3_{II}}) \\ \hline \end{array}\end{split}\]
Solution for the spherical wrist
Then: