# 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*}