Welcome to Robot Rebels, an online robot maker community.

Register today to post your projects, ask questions, share knowledge and meet like-minded people from around the world.

The Community

Author Topic: Arduino IK  (Read 646 times)


  • Member
  • ****
  • B
  • Posts: 20
Arduino IK
« on: February 25, 2019, 05:54:44 AM »
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
*Hind Angles.jpg (7.82 kB . 222x330 - viewed 56 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
*Fore Angles.jpg (6.25 kB . 222x255 - viewed 56 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

  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
*Wrist Angles.jpg (6.39 kB . 222x255 - viewed 55 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.


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.


* Search

* Recent Topics

Servo Calibrator by Bajdi
[May 24, 2019, 04:01:10 AM]

MKS Gen L 1.0 by Bappi
[May 21, 2019, 07:22:54 PM]

ESP32 "Life is Life" 64x32 flexible P4 RGB LED by jinx
[May 18, 2019, 11:19:44 AM]

Warco HV-4" Rotary Table by jinx
[May 08, 2019, 03:12:58 PM]

Warco WM14 milling machine by jinx
[April 28, 2019, 09:41:40 AM]

MKS GEN L Servo Issue by Reywas
[April 22, 2019, 09:10:08 PM]

Meowth Abandoned by Nemesis
[April 11, 2019, 11:08:00 AM]

Bench Power: Help a rebel out by jinx
[April 03, 2019, 04:20:10 AM]

Just a short intro... by MEgg
[March 18, 2019, 05:49:28 PM]

Yes I am alive! by jinx
[March 10, 2019, 04:38:53 AM]

CZ-1 2.0: problems, calibration and printing, part2 by MEgg
[February 26, 2019, 03:35:15 PM]

Hi, Mushroom glue here. by nschreiber0813
[February 25, 2019, 02:09:13 PM]

Arduino IK by BaldwinK
[February 25, 2019, 05:54:44 AM]

Raspberry Pi / Python eBook - free for the next 11 hours! by MEgg
[February 24, 2019, 09:49:43 AM]

Fun Fact by Impala
[February 13, 2019, 11:05:01 PM]

* Recent Posts

Re: Servo Calibrator by Bajdi
[May 24, 2019, 04:01:10 AM]

Re: Servo Calibrator by jinx
[May 22, 2019, 02:28:13 AM]

Re: MKS Gen L 1.0 by Bappi
[May 21, 2019, 07:22:54 PM]

Re: MKS Gen L 1.0 by jinx
[May 21, 2019, 03:27:08 PM]

Re: MKS Gen L 1.0 by Bappi
[May 21, 2019, 03:11:45 PM]

Servo Calibrator by BaldwinK
[May 21, 2019, 10:20:42 AM]

Re: MKS Gen L 1.0 by jinx
[May 21, 2019, 05:09:27 AM]

Re: MKS Gen L 1.0 by Bappi
[May 21, 2019, 03:34:54 AM]

Re: MKS Gen L 1.0 by jinx
[May 21, 2019, 02:28:17 AM]

Re: MKS Gen L 1.0 by Bappi
[May 20, 2019, 07:11:31 PM]

Re: MKS Gen L 1.0 by jinx
[May 20, 2019, 03:16:02 PM]

Re: MKS Gen L 1.0 by Bappi
[May 20, 2019, 02:53:00 PM]

Re: MKS Gen L 1.0 by jinx
[May 20, 2019, 03:31:49 AM]

Re: MKS Gen L 1.0 by Bappi
[May 19, 2019, 07:16:21 PM]

Re: MKS Gen L 1.0 by Bappi
[May 19, 2019, 04:04:47 PM]