**Arduino IK**Previously I showed how Excel could produce a series of motor angles given the x,y coordinates of the toe of a moving quadruped. To be really useful this calculation needs to take place in the robot and finish in real time.

Balancing a quadruped requires a redistribution of weight before, during and after each leg is lifted. This effort involves mainly the diagonal pair which may need to be lengthened. If there are three motors in each leg then this gets tricky but adjusting only one value – IKy - is easy.

**Hind Leg **In the Excel approach the starting values were important as they influenced the final geometry. Many solutions can be found but they may appear ungainly. However, the geometry seems fine for all values if the shin segment is bisected by the purple line.

Hind Angles.jpg (7.82 kB . 222x330 - viewed 55 times) There is not enough processing time to calculate, always from the same starting point, using trig functions. So here we redefine the problem as drawn above. We know all the lengths but not the split point on the purple line.

Using the cosine rule, the two opposing angles at the split point are equal so we can lose the trig parts and derive a test value to find La.

First calculate these expressions just once:

Aexpr =(Lk/2)*(Lk/2) - Lp*Lp;

Bexpr =(Lk/2)*(Lk/2) - Lh*Lh;

Cexpr = acos((Hh - IKy)/Lt);

Lt = sqrt((Hh - IKy)*(Hh - IKy) + (IKx*IKx)); //purple target vector length

Then repeat the following calculation using a converging value of La until IKtest is between 0.98 and 1.02 say:

IKtest = (Lt-La)*(La*La + Aexpr)/La*((Lt-La)*(Lt-La) + Bexpr);

I use a loop counter in case it oscillates and stop at 13 anyway which will be good enough within a few percent. The iteration takes around 1mS.

Now we can solve the trig once for the three angles.

Aexpr = acos((La*La + Lp*Lp - (Lk/2)*(Lk/2))/(2*La*Lp));

if (IKx < 0)

angP = (Aexpr - Cexpr)*degRad;

if (IKx >= 0)

angP = (Aexpr + Cexpr)*degRad;

angK = 180 - degRad*acos((Lp*Lp + (Lk/2)*(Lk/2) - La*La)/(Lp*Lk));

angH = 180 - degRad*acos((Lh*Lh + (Lk/2)*(Lk/2) - (Lt - La)*

(Lt - La))/(Lh*Lk));

The whole process is finished in well under 2mS.

**Pattern generation**When the toe is on the ground and the bot is moving forward, then IKx decrements from ahead of the hip to reach the rearmost position. IKy is zero through this range. Then the toe is lifted and IKx increments to complete the gait.

Rather than use a lookup table for this recovery movement we could apply a second order curve. Or we can simply ramp up y with x for a while, run along for a bit, then ramp down y with x until the toe is on the ground again.

**Fore Leg**Fore Angles.jpg (6.25 kB . 222x255 - viewed 55 times) This can have a more complex pattern but iteration is unnecessary to find all the angles if the wrist is kept straight. This solution would also suit the leg style of Spot from Boston Dynamics.

Bexpr = acos((Lp*Lp + Lt*Lt - (Le + Lw)*(Le + Lw))/(2*Lp*Lt));

Cexpr = acos((Hh - IKy)/Lt); //target vector angle from vertical

Lt = sqrt((Hh - IKy)*(Hh - IKy) + (IKx*IKx)); //purple target vector length

then

angW = 0; //straight wrist always

if (IKx < 0)

angP = (Bexpr + Cexpr)*degRad;

if (IKx >= 0)

angP = (Bexpr - Cexpr)*degRad;

angE = 180 - degRad*acos((Lp*Lp + (Le+Lw)*(Le+Lw) - Lt*Lt)/(2*Lp*(Le+Lw)));

However, when a simple foreleg is at its rearmost travel it must be lifted by the shoulder motor to clear the ground before moving forward. This motor reversal may add a glitch to the otherwise smooth forward motion. Breaking the angle at the wrist would alleviate this problem.

So now we will need to inform the IK processor of the rule change during the lifting cycle. Also it will need to memorise the rearmost angP because that won’t be changing for a while.

Wrist Angles.jpg (6.39 kB . 222x255 - viewed 54 times)The geometry is reduced to a single triangle attached to the fixed elbow position. We know all the lengths and can solve for the angles at wrist and elbow.

Pmem = angP;

Aexpr = IKx + sin(Pmem/degRad)*Lp;

Bexpr = Hh - IKy - cos(Pmem/degRad)*Lp;

Lg = sqrt(Aexpr*Aexpr + Bexpr*Bexpr);

angH = 180 - degRad*(acos((Le*Le + Lw*Lw - Lg*Lg)/(2*Le*Lw)));

Cexpr = degRad*(acos((Le*Le + Lg*Lg - Lw*Lw)/(2*Le*Lg)));

Dexpr = degRad*(asin(Aexpr/Lg)); //gold angle

angK = Pmem + Dexpr + Cexpr;

At some point as IKx is incremented, the gold target vector will exceed the combined lengths Le + Lw. From then on the lifting rules are relaxed and the simple straight wrist solution will take over.

**Comment**I used to spend a lot of time patiently gathering angles and poses in complex lookup tables. But once a bot can ‘speak’ IK then these are no longer needed.

Now I get to micro-manage the stability issues at every moment of a sequence. The creation of a self balancing active chassis is that much closer.

It seems each hill we climb merely gives a view of the next.