Inverse Kinematics: Part 4 - Full Robot Solution

Success! Final few sets of measurements later and I've got enough information to turn the full leg IK solution from my last kinematics post into a set of functions that allow placement of any leg to a co-ordinate defined in a Hexy body-centric co-ordinate system.

This should allow Hexy to use that pen he's learned to hold to write, also new walking gaits (with minimum programming effort), body rotations etc. I've included all the maths that I used to get to this position (referencing my previous posts where necessary), put in some new code snippets, plus there's a Moves attachment at the end that wraps it all up for an IK Hexy demo :-)

[WARNING: maths in post]

First job was to work out where each of Hexy's legs were relative to a central point on Hexy's body. Simple job with a ruler and a calibrated Mk 1 eyeball.

Plan view of Hexy's leg locations

Plan view of Hexy's leg locations

Lucky for us Hexy's a symmetric robot (does this make him beautiful?). So we only need to make three measurements: distance between adjacent legs on one side, distance between front and back legs on one side, and the distance between each side of Hexy.


Of course, we know that the angle LF, \widehat{LB} ,RB is a right angle, so some Pythagoras later and we have the following co-ordinates for Hexy's legs relative to the origin shown above, which is defined as the midpoint of opposing sets of legs.

LF = (-65.8, 76.3)
LM = (-103.3, 0)
LB = (-65.8, -76.3)
RB = (65.8, -76.3)
RM = (103.3, 0)
RF = (65.8, 76.3)

Last bit that we need to complete our Hexy model is the angle that the legs point relative to the body. Fortunately, we've already got this measurement done as we know that the mid-legs are pointing along the x-axis and we have symmetry plus x, y co-ordinates for the other legs. Some basic trig later and we know that the front and rear legs are about 40.8^\circ away from the y-axis.

Job done.

Well, not quite. This still needs to be implemented to actually control the robot.

This shouldn't be too bad, we've already got code that solves a full leg. All that is needed is to transform (offset by O and rotate by \theta)the Hexy-centric co-ordinates (H) into the individual leg co-ordinate system (L) and call our previous code.

\begin{pmatrix} L_x\\ L_y\\ L_z\end{pmatrix}=\begin{pmatrix} \cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{pmatrix} \begin{pmatrix} H_x-O_x \\ H_y-O_y \\ H_z \end{pmatrix}

The implementation of this maths (for the left-front leg) is shown below.

Of course, we didn't need to include the whole 3D rotation matrix as we know that the z co-ordinate would not change with this transform, plus the angle is measured in radians. Our full robot solution will have another 5 functions like this. I've also added some wrapper functions to control the legs in bunches (either the whole robot, or two independent tripods).

As promised, here's an example which wraps up all of this code. I've put some commands in to allow Hexy to walk in any direction, at a variable speed. Right at the end of the file is the walking code, with a couple of variables above it. "s" defines the step length (up to about 23mm at a maximum) and "theta" defines which direction Hexy should walk in (in radians from forwards).

Example Python IK Code

4 thoughts on “Inverse Kinematics: Part 4 - Full Robot Solution

  1. Hi

    Fantastic job. I have a question, though. While I understand the math, I"m having a hard time sorting out in my mind how to actually *use* your code. It's not a "move" like PoMoCo looks for. It seems to be standalone - yes? I'm slowly learning Python. If it's not a quick answer, then, no worries. I'll keep digging. If you could "point" me, I'd be grateful.



Comments are closed.