# James Gregson's Website

## Inverse Kinematics

I've been looking into inverse kinematics a bit lately. Inverse kinematics is the problem of solving for joint parameters given a linkage and end-effector position. There are a number of algorithms for doing this but I'm most interested in cases where there may be cycles and (slightly) incompatible joint configurations.

Given two links A and B with initial reference frames $$F_A$$ and $$F_B$$ connected by a joint J with initial reference frame $$F_J$$, the transformations of each link reference frame to the joint reference frame are:

\begin{align*} T_{AJ} = F_J^{-1} F_A \\ T_{BJ} = F_J^{-1} F_B \end{align*}

These transforms are fixed for all time using the reference frames for the links and joint during setup. Using these transforms allows the links to be brought into a common (but unknown) joint frame:

\begin{align*} \hat{T}_A = T_{AJ} T_A \\ \hat{T}_B = T_{BJ} T_B \end{align*}

Once in the joint frame, the joint constraint functions can be defined. In the case of a ball and socket joint this is:

\begin{equation*} \hat{T}_A = R \hat{T}_B \end{equation*}

This allows the joint transformation to be found:

\begin{equation*} R = \hat{T}_A \hat{T}_B^{-1} \end{equation*}

Once the transformation is found, its parameters can be extracted and/or constrained. In the case of the ball and socket joint, this requires setting the translational components to zero, producing a constrained configuration $$R_*$$. With $$R_*$$ defined, it's then possible to express the constraint in the global frame for points $$P_A$$ & $$P_B$$ defined in the coordinates of A and B:

\begin{equation*} T_{AJ} T_A p_A = R_* T_{BJ} T_B P_b \end{equation*}

This equation forms the basis of constraints for the joint. The constraints can be enforced by ensuring this equation holds for three points that are not collinear. However the points are expressed in the frames of A and B) which is inconvenient. Instead each point can be expressed in the frame of $$F_J$$ initially and transformed by $$T_{AJ}^{-1}$$ and $$T_{BJ}^{-1}$$ respectively:

\begin{align*} p_A = T_{AJ}^{-1} p_j \\ P_B = T_{BJ}^{-1} p_j \\ T_{AJ} T_A p_A = R_* T_{BJ} T_B p_B \end{align*}

This allows basis vectors in the frame of the joint to be used to specify the correspondences for the constraints.

To actually solve this in practice, the constraint equation can be linearized. If $$\alpha$$ and $$\delta \alpha$$ are the transformation parameters and incremental change of A, then $$T_A$$ can be approximated with a first order Taylor series:

\begin{equation*} T_{\alpha+\delta\alpha} p_A \approx T_\alpha p_A + J_{\alpha, p_A} \delta \alpha \end{equation*}

where $$J_{\alpha,p_A}$$ is the Jacobian of the transform of $$p_A$$ with respect to link parameters $$\alpha$$. This lets the constraint be written as:

\begin{equation*} T_{AJ} \left( T_\alpha p_A + J_{\alpha,p_A} \delta \alpha \right) = R_* T_{BJ} \left( T_\beta p_B + J_{\beta,p_B} \delta \beta \right) \end{equation*}

which can be simplified to:

\begin{equation*} T_{AJ} J_{\alpha,p_A} \delta \alpha - R_* T_{BJ} J_{\beta,p_B} \delta \beta = R_* T_{BJ} T_\beta p_B - T_{AJ} T_\alpha p_A \end{equation*}

## Rotation Matrix Euler Angles

This page describes a basic approach to extracting Euler angles from rotation matrices. This follows what OpenCV does, (which is where I got the process) but I also wanted to understand why this approach was used.

Here I'm considering rotation performed first around x-axis ($$\alpha$$), then y-axis ($$\beta$$) and finally z-axis ($$\gamma$$). For this, the rotation matrix is shown below. Other rotation orders are analogous.

\begin{equation*} R(\alpha,\beta,\gamma) = \left[\begin{matrix}\cos{\left (\beta \right )} \cos{\left (\gamma \right )} & \sin{\left (\alpha \right )} \sin{\left (\beta \right )} \cos{\left (\gamma \right )} - \sin{\left (\gamma \right )} \cos{\left (\alpha \right )} & \sin{\left (\alpha \right )} \sin{\left (\gamma \right )} + \sin{\left (\beta \right )} \cos{\left (\alpha \right )} \cos{\left (\gamma \right )} & 0\\\sin{\left (\gamma \right )} \cos{\left (\beta \right )} & \sin{\left (\alpha\right )} \sin{\left (\beta \right )} \sin{\left (\gamma \right )} + \cos{\left (\alpha \right )} \cos{\left (\gamma \right )} & - \sin{\left (\alpha \right )} \cos{\left (\gamma \right )} + \sin{\left (\beta \right )} \sin{\left (\gamma \right )} \cos{\left (\alpha \right )} & 0\\- \sin{\left (\beta \right )} & \sin{\left (\alpha \right )} \cos{\left (\beta \right )} & \cos{\left (\alpha \right )} \cos{\left (\beta \right )} & 0\\0 & 0 & 0 & 1\end{matrix}\right] \end{equation*}

To solve for the Euler angles, we can use a little bit of trigonometry. The best way, in my opinion, is to find pairs of entries that are each products of two factors. Using the identity $$\sin^2 + \cos^2 = 1$$ can be used to isolate specific angles, e.g. $$R_{0,0}$$ and $$R_{0,1}$$ can be used to find $$\cos(\beta)$$ which unlocks the rest of the angles:

\begin{align*} (-1)^k \cos(\beta + k \pi) = \sqrt{ \cos^2(\beta) ( \sin^2(\gamma) + \cos^2(\gamma)) } = \sqrt{ R_{0,0}^2 + R_{1,0}^2 } \\ \cos(\beta) \approx \sqrt{ R_{0,0}^2 + R_{1,0}^2 } \end{align*}

Pay attention to the first line: For any true value of $$\beta$$, this formula will return the same value for $$\beta + k\pi$$ where $$k$$ is an arbitrary integer. This will in turn determine the angles $$\alpha$$ and $$\gamma$$ in order to be compatible. The approximate equality will return $$\beta \in [-\pi/2, \pi/2]$$.

Once $$\cos(\beta)$$ is found we can find the angles directly using atan2:

\begin{align*} \beta = \mbox{atan2}( -R_{2,0}, \cos(\beta) ) \\ \alpha = \mbox{atan2}\left( \frac{R_{2,1}}{\cos(\beta)}, \frac{R_{2,2}}{\cos(\beta)} \right) \\ \gamma = \mbox{atan2}\left( \frac{R_{1,0}}{\cos(\beta)}, \frac{R_{0,0}}{\cos(\beta)} \right) \end{align*}

The other issue occurs when $$|\cos(\beta)| = 0$$, which causes division by zero in the equations for $$\alpha,\gamma$$ ($$\beta$$ is still well defined). In this case there are a number of options. What OpenCV does is to arbitrarily decide that $$\gamma = 0$$, which means that $$\sin(\gamma) = 0$$ and $$\cos(\gamma) = 1$$. The formulas in this case are:

\begin{align*} \beta = \mbox{atan2}( -R_{2,0}, \cos(\beta) ) \\ \alpha = \mbox{atan2}( -R_{1,2}, R_{1,1} ) \\ \gamma = 0 \end{align*}

There are, of course, other options. Python code implementing these operations is below, along with unit testing for the degenerate and non-degerate cases.

import unittest
import numpy

def x_rotation( theta ):
"""Generate a 4x4 homogeneous rotation matrix about x-axis"""
c = numpy.cos(theta)
s = numpy.sin(theta)
return numpy.array([
[ 1, 0, 0, 0],
[ 0, c,-s, 0],
[ 0, s, c, 0],
[ 0, 0, 0, 1]
])

def y_rotation( theta ):
"""Generate a 4x4 homogeneous rotation matrix about y-axis"""
c = numpy.cos(theta)
s = numpy.sin(theta)
return numpy.array([
[ c, 0, s, 0],
[ 0, 1, 0, 0],
[-s, 0, c, 0],
[ 0, 0, 0, 1]
])

def z_rotation( theta ):
"""Generate a 4x4 homogeneous rotation matrix about z-axis"""
c = numpy.cos(theta)
s = numpy.sin(theta)
return numpy.array([
[ c,-s, 0, 0],
[ s, c, 0, 0],
[ 0, 0, 1, 0],
[ 0, 0, 0, 1]
])

def xyz_rotation( angles ):
"""Generate 4x4 homogeneous rotation in x then y then z order"""
return numpy.dot( z_rotation(angles[2]), numpy.dot( y_rotation(angles[1]), x_rotation(angles[0]) ) )

def xyz_rotation_angles( R, eps=1e-8, check=False ):
"""Back out the Euler angles that would lead to the given matrix"""

if check and numpy.linalg.norm( numpy.dot( R.transpose(), R ) - numpy.eye(4) ) > eps:
raise ValueError('Input matrix is not a pure rotation, R\'R != I')

cos_beta = numpy.sqrt( R[0,0]**2 + R[1,0]**2 )
if numpy.abs(cos_beta) > eps:
alpha = numpy.arctan2( R[2,1]/cos_beta, R[2,2]/cos_beta )
beta  = numpy.arctan2(-R[2,0], cos_beta )
gamma = numpy.arctan2( R[1,0]/cos_beta, R[0,0]/cos_beta )
return numpy.array((alpha,beta,gamma))
else:
alpha = numpy.arctan2(-R[1,2], R[1,1] )
beta  = numpy.arctan2(-R[2,0], cos_beta )
gamma = 0
return numpy.array((alpha,beta,gamma))

class test_euler_angles(unittest.TestCase):
def test_angles(self):
"""Do fuzz testing on arbitrary rotations, limiting beta to valid range"""
for i in range(1000):
alpha = (numpy.random.rand()-0.5)*numpy.pi*2
beta  = (numpy.random.rand()-0.5)*numpy.pi*0.9999
gamma = (numpy.random.rand()-0.5)*numpy.pi*2

ang = xyz_rotation_angles( xyz_rotation([alpha,beta,gamma]))
self.assertAlmostEqual( alpha, ang[0] )
self.assertAlmostEqual( beta,  ang[1] )
self.assertAlmostEqual( gamma, ang[2] )

def test_degeneracies(self):
"""Do fuzz testing on the degenerate condition of beta = +- pi/2"""
for i in range(1000):
alpha = (numpy.random.rand()-0.5)*numpy.pi*2
beta  = numpy.sign(numpy.random.randn())*numpy.pi/2
gamma = (numpy.random.rand()-0.5)*numpy.pi*2
R     = xyz_rotation((alpha,beta,gamma))
ang   = xyz_rotation_angles( R )
R2    = xyz_rotation( ang )
# check that beta is recovered, gamma is set to zero and
# the rotation matrices match to high precision
self.assertAlmostEqual( beta, ang[1] )
self.assertAlmostEqual( ang[2], 0 )
for j in range(16):
self.assertAlmostEqual( R.ravel()[j], R2.ravel()[j] )

if __name__ == '__main__':
unittest.main()

## Transformation Matrix Jacobians

Starting with the $$4\times4$$ homogeneous transform matrix with parameters $$\beta = [ \theta_x, \theta_y, \theta_z, t_x, t_y, t_z ]^T$$ where rotations are performed in XYZ order and using the following substitutions:

\begin{align*} c_x = \cos(\theta_x) \\ s_x = \sin(\theta_x) \\ c_y = \cos(\theta_y) \\ s_y = \sin(\theta_y) \\ c_z = \cos(\theta_z) \\ s_z = \sin(\theta_z) \\ \end{align*}

the vector function mapping a point $$p = [p_x, p_y, p_z, 1]^T$$ in the body coordinate system to a point in the world coordinate system $$w = [w_x, w_y, w_z, 1]^T$$ is:

\begin{equation*} \begin{bmatrix} w_x \\ w_y \\ w_z \\ 1 \end{bmatrix} = F( p, \beta ) = \left[\begin{matrix}c_{y} c_{z} & - c_{x} s_{z} + c_{z} s_{x} s_{y} & c_{x} c_{z} s_{y} + s_{x} s_{z} & t_{x}\\c_{y} s_{z} & c_{x} c_{z} + s_{x} s_{y} s_{z} & c_{x} s_{y} s_{z} - c_{z} s_{x} & t_{y}\\- s_{y} & c_{y} s_{x} & c_{x} c_{y} & t_{z}\\0 & 0 & 0 & 1\end{matrix}\right]\begin{bmatrix} p_x \\ p_y \\ p_z \\ 1 \end{bmatrix} \end{equation*}

and the Jacobian with respect to the parameters $$\beta$$ is:

\begin{equation*} J_F = \left[\begin{matrix}p_{y} \left(c_{x} c_{z} s_{y} + s_{x} s_{z}\right) + p_{z} \left(c_{x} s_{z} - c_{z} s_{x} s_{y}\right) & c_{x} c_{y} c_{z} p_{z} + c_{y} c_{z} p_{y} s_{x} - c_{z}p_{x} s_{y} & - c_{y} p_{x} s_{z} + p_{y} \left(- c_{x} c_{z} - s_{x} s_{y} s_{z}\right) + p_{z} \left(- c_{x} s_{y} s_{z} + c_{z} s_{x}\right) & 1 & 0 & 0\\p_{y} \left(c_{x} s_{y} s_{z} - c_{z} s_{x}\right) + p_{z} \left(- c_{x} c_{z} - s_{x} s_{y} s_{z}\right) & c_{x} c_{y} p_{z} s_{z} + c_{y} p_{y} s_{x} s_{z} - p_{x} s_{y} s_{z} & c_{y} c_{z} p_{x} + p_{y} \left(- c_{x} s_{z} + c_{z} s_{x} s_{y}\right) + p_{z} \left(c_{x} c_{z} s_{y} + s_{x} s_{z}\right) & 0 & 1 & 0\\c_{x} c_{y} p_{y} - c_{y} p_{z} s_{x} & - c_{x} p_{z} s_{y} - c_{y} p_{x} -p_{y} s_{x} s_{y} & 0 & 0 & 0 & 1\\0 & 0 & 0 & 0 & 0 & 0\end{matrix}\right] \end{equation*}

Python code for these respective operations is below:

def make_transform( beta ):
c_x = numpy.cos(beta[0])
s_x = numpy.sin(beta[0])
c_y = numpy.cos(beta[1])
s_y = numpy.sin(beta[1])
c_z = numpy.cos(beta[2])
s_z = numpy.sin(beta[2])
t_x = beta[3]
t_y = beta[4]
t_z = beta[5]
return numpy.array([
[c_y*c_z, -c_x*s_z + c_z*s_x*s_y, c_x*c_z*s_y + s_x*s_z, t_x],
[c_y*s_z, c_x*c_z + s_x*s_y*s_z, c_x*s_y*s_z - c_z*s_x, t_y],
[-s_y, c_y*s_x, c_x*c_y, t_z],
[0, 0, 0, 1]
])

def make_transform_jacobian( beta, p ):
c_x = numpy.cos(beta[0])
s_x = numpy.sin(beta[0])
c_y = numpy.cos(beta[1])
s_y = numpy.sin(beta[1])
c_z = numpy.cos(beta[2])
s_z = numpy.sin(beta[2])
t_x = beta[3]
t_y = beta[4]
t_z = beta[5]
p_x = p[0]
p_y = p[1]
p_z = p[2]
return numpy.array([
[p_y*(c_x*c_z*s_y + s_x*s_z) + p_z*(c_x*s_z - c_z*s_x*s_y), c_x*c_y*c_z*p_z + c_y*c_z*p_y*s_x - c_z*p_x*s_y, -c_y*p_x*s_z + p_y*(-c_x*c_z - s_x*s_y*s_z) + p_z*(-c_x*s_y*s_z + c_z*s_x), 1, 0, 0],
[p_y*(c_x*s_y*s_z - c_z*s_x) + p_z*(-c_x*c_z - s_x*s_y*s_z), c_x*c_y*p_z*s_z + c_y*p_y*s_x*s_z - p_x*s_y*s_z, c_y*c_z*p_x + p_y*(-c_x*s_z + c_z*s_x*s_y) + p_z*(c_x*c_z*s_y + s_x*s_z), 0, 1, 0],
[c_x*c_y*p_y - c_y*p_z*s_x, -c_x*p_z*s_y - c_y*p_x - p_y*s_x*s_y, 0, 0, 0, 1],
[0, 0, 0, 0, 0, 0]
])

I generated these using sympy to build the transformations and used a finite-difference Jacobian function to check the output.