Monday, April 22, 2024

Mojo5 - My Inverse Kinematics Simplified (#8)

Inverse Kinematics pictured by Dalle3, it can be much simpler!

A much simpler approach

In my previous post on Inverse Kinematics (IK), the complexity might have left some readers puzzled about whether there could be a simpler method to achieve the same results. The good news is, yes, there is a simpler way! Thanks to insights from my roboticist friend Oracid over at the French Robot Forum on Robot Maker, I've streamlined the IK calculations for our Mojo5.

Understanding the Basics

The challenge remains the same: given a target point (x, y), we need to determine the angles of the hip servo (S1) and knee servo (S2). The geometric configuration of the robot's leg has not changed; we still deal with the leg segments L1 (upper) and L2 (lower), equal in length, connected at the knee and forming two sides of a parallelogram.

Geometric Simplification

This parallelogram setup allows us to simplify our calculations considerably. The angle from the horizontal to the leg segment L1 directly provides the angle for S1, and similarly, the angle from the horizontal to the side of L2 gives us S2.

Mojo5 - simplified Inverse Kinematics

Simplified Calculation Steps

Calculate Distance D:  First, we determine the distance from the hip servo to our target (x, y) using the Pythagorean theorem:  D = sqrt(x*x + y*y).  Note D plays a very important role in the following calculations for angles alpha and beta. It is also the line that divides our parallelogram in half.

Calculate alpha:  We will use the trigonometric law of cosines. Given that we know the length of our upper leg L1 and the length of D (aka the hypotenuse), we can calculate the angle between them.  We will call this alpha:  alpha = acos(D / (2*L)).  You may notice this is the same calculation that I used previously. More importantly, this angle alpha is the same on either side of the bisection of the parallelogram by D.

Calculate beta:  We will use a much simpler approach. Also using trigonometry, we see that there is a Right Triangle formed by the Y position, X length and the hypotenuse D. it is possible to calculate this angle opposite of y and D.  beta = asin ( abs(y) / D). It must be adjusted to its reflection in the event that x < 0, this is done (in radians) by subtracting PI from beta. Now one more interesting note. The angle we just calculated is the same angle at top horizontal, this makes it more clear how this beta angle will be used in our final calculations.

Final Servo Calculations:  For S1 we can take the value of beta and subtract apha from it.  Like wise to calculate the angle for S2 and can take the value of beta and add alpha to it to find its value.

  • S1 = beta - alpha
  • S2 = beta + alpha
Hopefully this explaination will help in visualizing the relationships and how to calcuate the correct angles for S1 and S2.  

Here you can view a simplified set of C++ code:

void calcIK(float x, float y, float &s1, float &s2,) {
  float L = 70; //length mm
  float d = sqrt(x*x + y*y);
  float alpha = acos(d / (2*L));
  float beta = asin(abs(y)/d);  if(x<0)beta=(M_PI-beta); 

  s1 = ((beta - alpha) * (180.0 / M_PI));
  s2 = ((beta + alpha) * (180.0 / M_PI));


No comments:

Post a Comment