What’s in this article?
Robot manipulators are fancy names for Robot arms.
How do you moving a robot arms to a position? Forward and Inverse Kinematics answer the question.
In this article, I will discuss
- What is the Forward Kinematics?
- What is the Inverse Kinematics?
- Implementation in python.
- Follow up discussion about manipulators.
1. The Robotic Manipulators
Robotic manipulators are everywhere nowadays. It is wildly used in the field of manufacture and medical surgery. Check these videos.
The mathematical tool to work with manipulators is the lie-group theory. There are excellent books about manipulators. The most famous one is the A Mathematical Introduction to Robotic Manipulation.
The book provides a comprehensive discussion of the manipulator kinematics mode in 3D. Comprehensive and details are good for experts but they raised the bar for normal people.
I’d like to give a simple example of the forward and the inverse kinematics in 2D in the lie-group framework.
I hope it can provide a big picture of the robotic manipulation problem.
2. The 2D Manipulator Problem
Consider a simple 2 links manipulator.
Given the definition, the end-effector position can be derived as a function of rotation of the joints.
\(e(R_1, R_2) = R_1 l_1 + R_2 R_1 l_2\)
Note:
1. We assume the length of a robot link doesn’t change. Although it’s straightforward to add the length as an input to the function.
2. We can use the angle of rotation \(\theta\) instead of a rotation matrix R. The benefit of using R is we can directly generalize the results to 3D.
3. The Forward Kinematics
The forward kinematics asks a question: where is the manipulator’s end-effector given a manipulator configuration (e.g. joint angles)?
\(e(R_1, R_2) = R_1 l_1 + R_2 R_1 l_2\) is a forward kinematics equation. It shows where the end-effector is given the joint angles of a manipulator.
Yes, forward kinematics are easy.
It defines a function which maps the robot configuration \(R_1 \in SO(2), R_2 \in SO(2)\) to the end-effector position \(e \in R^2\). Normally, the forward kinematics is a closed-form function.
4. The Inverse Kinematics
The inverse kinematics asks a question: I want to move the end-effector to a target position. What are the joints angles corresponding to the target position?
We can “inverse” the forward kinematics function to get the answer, \((R_1, R_2) = e^{-1}(p_{target})\). But most forward kinematics equation can’t be easily inverted.
How can we solve this problem?
We can model the inverse kinematics as an optimization problem.
\(\text{minimize}_{R_1, R_2} \; ||e(R_1, R_2) – p_{target}||^2\)
In English, we want to search (optimization) for a configuration \(R_1, R_2\) such that the end-effector position corresponding to the joint angles is close to \(p_{target}\).
We can formulate the problem as a least-squares. For a least-squares problem, we need to do the following,
- linearly approximate the residual function. \(r(x) \approx J(x_0) (x – x_0) + r(x_0)\)
- solve the Normal Equation of the approximated cost. \(J(x_0)^T W J(x_0) \Delta{x}^* = – J(x_0)^T W r(x_0)\)
- Update variables by \(x = x_0 + \Delta{x}^* \).
- Go back to 1. if not converge.
Here is a refresher for the least-squares problem. Make sure you understand it.
4.1 The Jacobian
The first step to solve the inverse kinematics least-squares problem is to derive the jacobian of the residual function w.r.t to optimization parameters.
We can do the definition to compute the Jacobian. But a easier way is to use the perturbation method.
4.1.1 The Perturbation Method
Derivative means how a small input maps to a small output. Following this definition, we can compute the derivative explicitly.
Consider we want to derive the derivative of,
\(f(x) = x^2\).
Apply a perturbation \(dx\) to the input \(x\),
\(f(x + dx) = (x + dx)^2\)
\(f(x + dx) = x^2 + 2 x dx + dx^2\)
Since \(dx\) is small, we can assume \(dx^2 \rightarrow 0\).
\(f(x + dx) = x^2 + 2 x dx\)
\(f(x + dx) = f(x) + 2 x dx\)
By the definition of derivative (or the Taylor series), we can see \(2x\) is the derivative of \(f(x)\) at x. Or you are can do some algebra to get the definition of the derivative.
\(\text{derivative} = \lim_{dx \rightarrow 0} \frac{f(x+dx) – f(x)}{dx}\).
Read this for details: http://ceres-solver.org/nnls_modeling.html#autodiffcostfunction
4.1.2 The Perturbation of the Rotation Group
For a 2D rotation matrix \(R(\theta)\),
\(R(\theta) = \begin{bmatrix} \cos \theta & – \sin \theta \\ \sin \theta & \cos \theta \end{bmatrix}\)
Consider we add a perturbation \(\xi\) to \(\theta\),
\(R(\theta + \xi) = \begin{bmatrix} \cos (\theta + \xi) & – \sin (\theta + \xi) \\ \sin (\theta + \xi) & \cos (\theta + \xi) \end{bmatrix}\)
Do some algebra, we get,
\(R(\theta + \xi) =
\begin{bmatrix} \cos \theta & – \sin \theta \\ \sin \theta & \cos \theta \end{bmatrix}
\begin{bmatrix} \cos \xi & – \sin \xi \\ \sin \xi & \cos \xi \end{bmatrix}\)
Assuming the perturbation \(\xi\) is small. i.e. \(\xi \rightarrow 0\). Note: strictly speaking, it’s not necessary. But it will make our lives much easier.
\(R(\theta + \xi) \approx
\begin{bmatrix} \cos \theta & – \sin \theta \\ \sin \theta & \cos \theta \end{bmatrix}
\begin{bmatrix} 1 & – \xi \\ \xi & 1 \end{bmatrix}
= \begin{bmatrix} \cos \theta & – \sin \theta \\ \sin \theta & \cos \theta \end{bmatrix}
(I + \begin{bmatrix} 0 & – \xi \\ \xi & 0 \end{bmatrix})
= R(\theta)(I + [\xi]_{\times})\)
Where we defined \([\xi]_{\times} = \begin{bmatrix} 0 & – \xi \\ \xi & 0 \end{bmatrix}\).
This is the perturbation for 2D rotation matrix.
In sum, we can define the perturbation(or small addition) for the rotation group as,
\(R \oplus \xi = R \begin{bmatrix} \cos \xi & – \sin \xi \\ \sin \xi & \cos \xi \end{bmatrix} \approx R \begin{bmatrix} 1 & – \xi \\ \xi & 1 \end{bmatrix}\)
4.1.3 Derive the Jacobian w.r.t Rotations
Having the perturbation method for rotation, we can derive the jacobian for the inverse kinematics residual function.
\(r(R_1, R_2) = e(R_1, R_2) – p_{target}\)
To compute its Jacobian w.r.t \(R_1\), we can perturb \(R_1\) and get its Jacobian by the perturbation method.
\( r(R_1 \oplus \xi_1 , R_2) = r(R_1 (I + [\xi_1]_{\times}), R_2) = R_1 (I + [\xi_1]_{\times}) l_1 + R_2 R_1 (I + [\xi_1]_{\times}) l_2 – p_{target}\)
\(= r(R_1, R_2) + R_1 [\xi_1]_{\times} l_1 + R_2 R_1 [\xi_1]_{\times} l_2 \; \; (4.1)\)
Let’s derive a useful identity for \([\xi]_{\times} l\). assuming \(l = [a, b]^T\)
\([\xi]_{\times} l = \begin{bmatrix} 0 & – \xi \\ \xi & 0 \end{bmatrix} \begin{bmatrix} a \\ b \end{bmatrix} = – \begin{bmatrix} b \\ -a \end{bmatrix} \xi\)
To make notation simpler, let me define \(g(l) = [b, -a]^T\). So we have,
\([\xi]_{\times} l = – g(l) \xi \; \; (4.2)\)
Go back to (4,1), apply (4.2),
\(r(R_1 \oplus \xi_1 , R_2) = r(R_1 (I + [\xi_1]_{\times}), R_2) = r(R_1, R_2) + R_1 [\xi_1]_{\times} l_1 + R_2 R_1 [\xi_1]_{\times} l_2
\\ = r(R_1, R_2) – R_1 g(l_1) \xi_1 – R_2 R_1 g(l_2) \xi_1
\\= r(R_1, R_2) + (- R_1 g(l_1) – R_2 R_1 g(l_2)) \xi_1\)
Then by definition of the Taylor series (although we didn’t define the Taylor series for function involving SO(3), but I hope you got the idea), the Jacobian of the residual w.r.t to \(R_1\) is,
\(\frac{\partial r}{\partial R_1} \rightarrow \frac{\partial r}{\partial \xi_1} = – R_1 g(l_1) – R_2 R_1 g(l_2)\)
Likewise, we can derive the Jacobian of the residual w.r.t to \(R_2\),
\(\frac{\partial r}{\partial R_2} \rightarrow \frac{\partial r}{\partial \xi_1} = – R_2 g(R_1 l_2)\)
4.1.4 The Update
In the previous section, we derived the Jacobian of the residual w.r.t rotation perturbation.
\(J = \frac{\partial r}{\partial \xi}\).
The next step of the least-squares is to solve the Normal equation \(J^TJ \xi = – Jr\) and get the optimal step \(\xi^*\).
Recall the inverse kinematics optimization problem,
\(\text{minimize}_{R_1, R_2} \; ||e(R_1, R_2) – p_{target}||^2 \; \; (4.3)\)
(4.1.3) can be interpreted as solving this optimization problem with the change of variables,
\(\text{minimize}_{\xi_1, \xi_2} \; ||e(R_1 \oplus \xi_1, R_2 \oplus \xi_2) – p_{target}||^2 \; \; (4.4)\)
Since \((\xi_1^*, \xi_2^*)\) is the optimal solution for (4,4), we can see the optimal solution for (4.3) is \((R_1^* = R_1 \oplus \xi_1, R_2^* = R_2 \oplus \xi_2)\) where we defined \(\oplus\) in 4.1.2 .
5. Implementation
Theory needs support from facts. Let’s implement the Inverse Kinematics.
5.1 Code
import numpy as np
# using logm & expm is another option
# from scipy.linalg import logm, expm
from math import cos, sin, pi
import matplotlib.pyplot as plt
def SO2_expm(theta):
return np.array([
[cos(theta), -sin(theta)],
[sin(theta), cos(theta)],
])
def SO2_log(R):
return np.arctan2(R[1, 0], R[0, 0])
def vee(l1):
assert(l1.size == 2)
a, b = l1
return np.array([b, -a])
class ManipulatorOptimization:
def __init__(self, target):
self.R1_init = SO2_expm(0.01)
self.R2_init = SO2_expm(0.01)
self.R1 = self.R1_init
self.R2 = self.R2_init
self.l1 = np.array([1., 0])
self.l2 = np.array([2., 0])
self.target = target
def get_theta(self):
assert(abs(np.linalg.det(self.R1) - 1.) < 1e-8)
assert(abs(np.linalg.det(self.R2) - 1.) < 1e-8)
return SO2_log(self.R1), SO2_log(self.R2)
def end_effector_position(self, R1, R2):
p1_position = R1 @ self.l1
p2_position = p1_position + R2 @ R1 @ self.l2
return p2_position
def end_effector_position_jacobi_wrt_theta(self):
jacobi = np.zeros([2, 2])
# dr / d w1
jacobi[:, 0] = - self.R1 @ vee(self.l1) - self.R2 @ self.R1 @ vee(self.l2)
# dr / d w2
jacobi[:, 1] = - self.R2 @ vee(self.R1 @ self.l2)
return jacobi
def gradient_checking(self):
jacobi_analytic = self.end_effector_position_jacobi_wrt_theta()
jacobi_nu = np.zeros([2, 2])
DELTA = 1e-8
jacobi_nu[:, 0] = (self.residual(self.R1 @ SO2_expm(DELTA), self.R2) \
- self.residual(self.R1 @ SO2_expm(- DELTA), self.R2)) / (2 * DELTA)
jacobi_nu[:, 1] = (self.residual(self.R1, self.R2 @ SO2_expm(DELTA)) \
- self.residual(self.R1, self.R2 @ SO2_expm(-DELTA))) / (2 * DELTA)
print('jacobi_analytic:', jacobi_analytic)
print('jacobi_nu :', jacobi_nu)
def residual(self, R1, R2):
return self.end_effector_position(R1, R2) - self.target
def SO2_generalized_plus(self, delta_local_params):
w1, w2 = delta_local_params
self.R1 = self.R1 @ SO2_expm(w1)
self.R2 = self.R2 @ SO2_expm(w2)
def optimize(self):
for iters in range(20):
# self.gradient_checking()
jacobi = self.end_effector_position_jacobi_wrt_theta()
b = self.residual(self.R1, self.R2)
delta_local_params = np.linalg.solve(jacobi.T @ jacobi, -jacobi.T @ b)
self.SO2_generalized_plus(delta_local_params)
cost = b.T @ b
print('cost: ', cost)
if cost < 1e-8:
print('converged at iteration: ', iters)
break
def interp_and_show_video(self):
...
def main():
end_effector_target = np.array([0, 1.2])
manipulator_on_the_manifold = ManipulatorOptimization(end_effector_target)
manipulator_on_the_manifold.optimize()
manipulator_on_the_manifold.interp_and_show_video()
if __name__ == "__main__":
main()The optimize(self)is the main loop for the least-squares iteration.
The end_effector_position(self) implements the forward kinematics.
The end_effector_position_jacobi_wrt_theta computes the jacobian of the end-effector w.r.t rotational local perturb. Note the residual is the end-effector position minus a constant. The constant doesn’t affect the Jacobian.
5.2 Interpolation & Video
def interp_and_show_video(self):
steps = 100
dt = 0.02
# basically so2
R1_speed = SO2_log(self.R1_init.T @ self.R1)
R2_speed = SO2_log(self.R2_init.T @ self.R2)
for k in np.linspace(0, 1, 100):
R1_k = self.R1_init @ SO2_expm(k * R1_speed)
R2_k = self.R2_init @ SO2_expm(k * R2_speed)
p1_position = R1_k @ self.l1
p2_position = p1_position + R2_k @ R1_k @ self.l2
p1x, p1y = p1_position
p2x, p2y = p2_position
plt.cla()
plt.plot([0, p1x, p2x], [0, p1y, p2y])
plt.xlim([-3, 3])
plt.ylim([-3, 3])
plt.pause(.001)
plt.show()After having the result, we want to show how the manipulator moves from the current configuration to the result configuration. We can simply interpolate the rotation.
Without go into the details, the general method to interpolate rotation is,
- Compute the angular speed \(so(n)\) from the start rotation to the end rotation. \(W = Log(R_{\text{start}}^T R_{\text{end}})\)
- Interpolate the using the angular speed. \(R_t = R_{\text{start}} Exp(t W)\)
5.3 Result
The target pose was \(p_{\text{target}} = [0, 1.2]\). Constants were \(l_1 = 1, l_2 = 2\), Initial variables value were \(R_1 = I, R_2 = I\).
The least-squares converged in 7 steps
yimu@yimu-mate:~/Desktop/blog$ /usr/bin/python3 yimu-blog/least_squares/manipulator/manipulator.py cost: 10.319806801536663 cost: 15.454460581610949 cost: 5.798587047552779 cost: 0.7106096368142792 cost: 0.01649962799304561 cost: 0.004500284032993574 cost: 1.64088365336658e-05 cost: 4.6453151515902636e-11 converged at iteration: 7
6. Further reading
6.1 Paper unlock
Introduction to Inverse Kinematics with
Jacobian Transpose, Pseudoinverse and Damped
Least Squares methods
Samuel R. Buss
This paper gave a review of the Least Squares based Inverse Kinematics problem.
It first introduce the forward kinematics and the inverse kinematics formally.
The author defined the end-effector as “certain points on the links”. So the inverse kinematics problem is defined as all end-effectors close to all target positions.
He also defined the manipulator Jacobian.
The manipulator Jacobian is defined as the Jacobian of the end-effectors w.r.t joint angles. It’s what we derived in (4.1).
Then the author stated his review of inverse kinematics method.
The fist method was the Jacobian transpose method, which is very strange in my mind. I will skip it.
The second method is the pseudoinverse method.
The pseudoinverse is a fancy name for the least-squares.
The third method is the damped least squares. It is least squares with regularization where we can add soft constraint to the problem.
The last method is SVD, which is a way to decomposition the Normal Equation. (Not sure why it was classified as a stand-along method).
6.2 A Mathematical Introduction to Robotic Manipulation
The previous paper did a good job summarizing the least-squares based inverse kinematics problem. But it didn’t go into the math of how to do the math(e.g. deriving the jacobian).
A Mathematical Introduction to Robotic Manipulation is a book way ahead its time (1994). It gave a simple yet very powerful mathematical framework to work with manipulators.
For example, the end-effector position can be expressed as the product of transformation matrices (which is the exponential map of twist \(\xi\)). It was basically the lie-group theory.
Using the lie-group theory, we can compute the Jacobian easier.
In the previous section, I basically derived the necessary lie-group operation in 2D. You can do the same math in 3D without changing any step.
For a comprehensive discussion of lie-group and its application for the 3D manipulator, please read this book.
6.3 Trajectory Planning
The inverse kinematics problem is the building block for the manipulator trajectory planning problem.
If we want the manipulator to follow a trajectory, we can do,
\(\text{minimize}_{R_1^t, R_2^t} \sum_t||e(R_1^t, R_2^t) – p_{target}^t || + \text{smoothness-cost}\)
Please read this article for a simple introduction to the model predicted control. In the last section, I discussed a classical manipulator planning paper.
7. Summery
In this article, we did a 2D inverse kinematics problem together. With the big picture in mind, I hope you can enjoy more advanced materials for manipulators.