# Inverse Kinematics: Part 1

Pen was put to paper today (and yesterday) to start to understand exactly how Hexy moved. Because I'm well aware just how little maths can fit into my brain at any one time, I decided to reduce the scope of the problem considerably from a whole robot to just part of one leg. This means that instead of having equations of motion/ position that have 18 degrees of freedom, I only have two.

[WARNING: maths in post]

Side elevation of Hexy's leg

For this post, I'm going to discuss the problem of the lower two Hexy leg joints. This conveniently gives us just a 2D problem, which is nice and easy to draw on a sheet of paper or publish on a blog. With just some basic trigonometry one can take the dimensions of a Hexy lower leg and joint positions (angles) and derive the position of the foot (relative to the origin, in this case the knee joint). This assumes that the Hexy body is level relative to the ground, and doesn't care which angle the hip joint is at.

Hexy's leg showing annotations for kinematic modelling

$\large x=a\cos (\theta) + b\cos (\theta + \phi)$

$\large y=a\sin (\theta) + b\sin (\theta + \phi)$

NB for Hexy I've measured the values of a and b to be 48 and 51mm respectively. The positive y-direction has been taken as down to make the maths "simpler".

Whilst doing the maths this way round (input = joint positions, output = foot positions), e.g. Kinematics, is reasonably simple, doing this backwards can be very tricky. This is Inverse-Kinematics (IK).

To be explicit, our IK problem is that for a given x, fixed values of a & b, what values of θ & φ will give the demanded y value?

I looked at these equations for some time and did not receive any flash of inspiration for inverting these non-linear, simultaneous equations. I did email them to a colleague of mine with a giant space brain, however, and then after lunch I got a reply with a closed form solution! The importance of a closed form solution really comes into force when you consider the alternative: iterative solution. These can be unstable, and slow (to name but two problems), so a closed form solution is ideal (given that we'd like to be able to control Hexy from some really low-power microcontrollers). NB IK problems don't typically have closed form solutions.

The solution to the IK problem stated above is as follows (some helper variables added to keep things "clear"):

$\large d=\sqrt {x^2+y^2}$

$\large k=\frac{d^2-b^2+a^2}{2d}$

$\large m=\pm \sqrt {a^2-k^2}$

Finally, the solutions to the joint angles:

$\large \theta =\tan^{-1}\left ( \frac{y}{x} \right )-\tan^{-1}\left ( \frac{m}{k} \right )$

$\large \phi =\tan^{-1}\left ( \frac{m}{k} \right )+\tan^{-1}\left ( \frac{m}{d-k} \right )$

NB to give a unique result, for the variable m, take the positive square-root.

With this maths in tow, I'll be able to start the journey into programming some very organic type moves for Hexy. But, baby-steps first: I shall attempt to improve on the stock "get up" movement, which appears to be quite jerky and I'm aware that it's stressful on the servos.

Perhaps, if one can suitable break down a robot positioning problem, the whole set of joint solutions can be calculated from closed form solutions. Don't know if this will be possible, but it feels like a decent target.

Looking to the future, this article suggests a simple method of iterating controller inputs to get a foot to a desired location. Something to think about for the next generation of code, perhaps?

## 2 thoughts on “Inverse Kinematics: Part 1”

1. You've made a mistake on the equation for "m" (note the dimensionality), the square root is over the whole fraction. Also, it can be simplified (couldn't be bothered to send you an update email) which will run faster:

m = (+/-) sqrt( a^2 - k^2 )

• Thanks for the keen eye. Post updated accordingly. I'm going to blame that on my first use of LaTeX.