collapse

Welcome!



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 RobotRebel.org Community

Author Topic: Arduino IK  (Read 162 times)

BaldwinK

  • Member
  • ****
  • B
  • Posts: 18
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 24 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 24 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
*Wrist Angles.jpg (6.39 kB . 222x255 - viewed 23 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.


 

* Search


* Recent Topics

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


MKS GEN L Servo Issue by cmptrguy1
[March 14, 2019, 01:18:28 AM]


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]


MKS Gen L 1.0 by MasterDomino
[February 24, 2019, 06:34:24 AM]


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


Animabot Rev3 - Advanced Hexapod Robot by Smashley
[February 05, 2019, 03:41:50 PM]


Finally tried out a Chinese PCB service by Bajdi
[February 02, 2019, 08:40:49 AM]


PLX-DAQ by BaldwinK
[January 16, 2019, 06:44:18 AM]


Four Motor Leg Module by BaldwinK
[January 09, 2019, 04:55:14 PM]


Android Adware by Diegotorry
[January 08, 2019, 05:34:48 AM]


Solid Works maker license 1 year Free!! by maelh
[January 04, 2019, 01:50:08 PM]

* Recent Posts

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


Re: Just a short intro... by Killer Angel
[March 18, 2019, 06:55:35 AM]


Re: Just a short intro... by jinx
[March 16, 2019, 04:36:13 AM]


Re: Just a short intro... by Bajdi
[March 15, 2019, 04:22:09 PM]


Just a short intro... by cybersamurai
[March 15, 2019, 04:19:22 PM]


Re: MKS GEN L Servo Issue by cmptrguy1
[March 14, 2019, 01:18:28 AM]


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


Re: Yes I am alive! by nschreiber0813
[March 09, 2019, 03:59:04 PM]


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


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


Yes I am alive! by nschreiber0813
[February 25, 2019, 02:08:12 PM]


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


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


Re: MKS Gen L 1.0 by MasterDomino
[February 24, 2019, 06:34:24 AM]


Re: MKS Gen L 1.0 by ronin
[February 23, 2019, 07:33:41 PM]