# Kinematics of a symmetric leg In this post, we are taking a look at the kinematics of a simple leg with two links and one knee. Both links have the same length $$\ell$$, making the leg "symmetric" in the sense that the mirror of any configuration with respect to the vertical plane is also a valid configuration. The joint angles, depicted to the right, are further subject to the following limits:

\begin{align} | q_1 | & < \frac{\pi}{2} & | q_2 | & < \pi \end{align}

We will derive analytically the forward kinematics of this leg in the sagittal plane, then consider its inverse kinematics for crouching.

## Forward kinematics

Consider the leg depicted in the figure above. The hip angle $$q_1$$ and knee angle $$q_2$$ fully define the position of every point on every link of the leg, including the origin of the hip $$G$$ (this could be the center of gravity of a lumped mass sitting on top of the leg and representing the robot's chassis), the knee $$K$$ and the end effector $$C$$. Forward kinematics is the function that maps joint angles $$(q_1, q_2)$$ to the coordinates $$(x_C, z_C) = \mathrm{FK}(q_1, q_2)$$ of the end effector.

Our model is an articulated system (the most common case) where each link is connected to exactly one parent link by one (usually revolute) joint. For such systems, the forward kinematics function is naturally recursive: the coordinates of a point on a link can be decomposed into (1) a local offset from the joint between the link and its parent, and (2) the coordinates of that joint in the parent link. Applying this general idea to our case, we write:

\begin{align} x_C & = x_K + (x_C - x_K) \\ z_C & = z_K + (z_C - z_K) \end{align}

The coordinates of the knee point are fully determined by the hip angle and the length of the first link:

\begin{align} x_K & = x_G + \ell \sin(q_1) \\ z_K & = z_G - \ell \cos(q_1) \end{align}

We see how the recursive argument applies to the knee as well as with the appearance of the coordinates $$(x_G, z_G)$$ of the origin of the hip, which is also the root of our kinematic tree. Recursion stops there. Moving on to the second link, the figure to the right illustrates how we get the following sines and cosines:

\begin{align} x_C - x_K & = \ell \cos\left(\frac{\pi}{2} - q_1 - q_2\right) = \ell \sin(q_1 + q_2) \\ z_C - z_K & = -\ell \sin\left(\frac{\pi}{2} - q_1 - q_2\right) = -\ell \cos(q_1 + q_2) \end{align}

Combining this system of equations with the previous one for $$(x_K, z_K)$$, we get the overall formula for the forward kinematics of our leg:

\begin{align} x_C & = x_G + \ell \sin(q_1) + \ell \sin(q_1 + q_2) \\ z_C & = z_G - \ell cos(q_1) - \ell \cos(q_1 + q_2) \end{align}

This expression has the form $$(x_C, z_C) = \mathrm{FK}(q_1, q_2)$$, but the $$\mathrm{FK}$$ function implicitly depends on the coordinates $$(x_G, z_G)$$ of the origin of the hip frame. This frame is known as the floating base of the mobile robot. We can equivalently consider it as a free joint between the hip and an inertial frame (often called the "world" frame in physics simulators and robotics software), in which case the coordinates of this joint become an additional argument to forward kinematics: $$(x_C, z_C) = \mathrm{FK}(x_G, z_G, q_1, q_2)$$. If we had a robotic arm, $$G$$ would directly be a point of an inertial frame and we could without loss of generality fix it to, for instance, $$x_G = 0$$ and $$z_G = 0$$.

## Inverse kinematics for crouching Let us now compute an inverse kinematics function: given a desired "crouching height" $$h$$, we want to compute the joint angles $$(q_1, q_2) = \mathrm{IK}(h)$$ such that $$\mathrm{FK}(\mathrm{IK}(h)) = (x_G, z_G - h)$$. Injecting this property in the system of equations we obtained for forward kinematics, this amounts to solve:

\begin{align} 0 & = \sin(q_1) + \sin(q_1 + q_2) \\ h & = \ell cos(q_1) + \ell \cos(q_1 + q_2) \end{align}

One of the trick in the bag to manipulate systems of trigonometric equations is to make the identity $$\cos(x)^2 + \sin(x)^2 = 1$$ appear. Let us rewrite the system as:

\begin{align} \cos(q_1 + q_2) & = \frac{h}{\ell} - \cos(q_1) \\ \sin(q_1 + q_2) & = -\sin(q_1) \end{align}

Taking the sum of the squares of these two lines leads us to:

\begin{equation*} 1 = \frac{h^2}{\ell^2} - 2 \frac{h}{\ell} \cos(q_1) + 1 \end{equation*}

Which is great, as we now know that:

\begin{equation*} \cos(q_1) = \frac{h}{2 \ell} \end{equation*}

Injecting this equation back into the system gives us:

\begin{align} \cos(q_1 + q_2) & = \frac{h}{\ell} - \cos(q_1) = \frac{h}{2 \ell} = \cos(-q_1) \\ \sin(q_1 + q_2) & = \sin(-q_1) \end{align}

The two angles $$q_1 + q_2$$ and $$-q_1$$ have the same sine and cosine, therefore they are equal up to some $$2 k \pi, k \in \mathbb{Z}$$. But recall our joint limit assumption:

\begin{align} | q_1 | & < \frac{\pi}{2} & | q_2 | & < \pi \end{align}

This means $$q_1 + q_2 - (-q_1) = 2 q_1 + q_2 < 2 \pi$$, and the solution to our trigonometric system is unique: $$q_2 = -2 q_1$$. Solving for $$q_1$$ alone yields:

\begin{align} q_1 & = \arccos\left(\frac{h}{2 \ell}\right) \\ q_2 & = -2 \arccos\left(\frac{h}{2 \ell}\right) \end{align}

This is our inverse kinematics function $$(q_1, q_2) = \mathrm{IK}(h)$$. We can check that it matches what we expect at the two boundary cases: • Stretched legs: $$h = 2 \ell$$, then $$q_1 = q_2 \arccos(1) = 0$$ and we are indeed in the configuration where the leg is fully vertical.
• Full crouch: $$h = 0$$, then $$q_1 \to +\frac{\pi}{2}$$ with a positive sign since $$0 < q_1 < \frac{\pi}{2}$$. Similarly $$q_2 \to -\pi$$, so that the leg is crouching as depicted in the figure to the right.

We can feel in the latter case that there is a discontinuity where our IK function can jump and should be used with care, although we will be fine within the continuous domain $$| q_1 | < \frac{\pi}{2}$$ that we chose here. In practice the two links would collide before we reach the limit $$h \to 0$$, so the full crouch test here is rather an additional check for us to be convinced that our derivation is correct.

Note how we made another choice along the way: since $$h > 0$$, taking $$q_1 = \arccos(h / 2 \ell)$$ implies that $$q_1 > 0$$ and the knee will always bend forward (as in human legs). An equally valid solution would be $$q_1 = -\arccos(h / 2 \ell)$$, in which case the knee will bend backward (as in bird legs).

## To go further

For more complex models, it is rare that we derive the forward and inverse kinematics functions of a model analytically, although such a derivation can be done automatically for models as complex as 7-DOF robot arms. A more common approach is to write down a URDF description of the model, use fast rigid-body algorithms for forward kinematics, and numerical optimization for inverse kinematics.

## Discussion

You can use Markdown with $\LaTeX$ formulas in your comment.