Hi All,
It has been a while since we last talked Inverse Kinematics! I have been working on a new quadruped and find myself making these calculations again. This time, I had ChatGPT to help and hinder my progress - but that is a different topic!! 
I have a version of code that is somewhat working. For every bug I found, I created another. I give an explanation (from my blog), pictures, and code. please take a look and give me some feedback. Much appreicated!
- did I miss some simplification?
- easier way to do things?
- something to be carefull with?
For Mojo5, I've chosen to use two MG995 servos, arranged in a stack. One servo is responsible for the 'hip' motion, and the other controls the 'knee.' The crux of IK in this setup is to map a target position within the coordinate plane to specific angles for these servos. To streamline the calculations, I've made a series of strategic design decisions. The lengths of the leg segments, L1 and L2, are set to be equal, each measuring 70mm. The hip servo is positioned as the origin point of our coordinate system. The mechanism for the knee is somewhat intricate, primarily because the servo controlling it is not mounted directly on the leg. Additionally, I've introduced a concept of 'yaw' movement along the z-axis, although, for the time being, our IK calculations will focus solely on movements within the x and y axes.
When it comes to calculating the necessary angles through IK, the approach is to visualize a triangle formed by the leg segments. Given that L1 and L2 are of equal length, this triangle is always isosceles. While this detail may seem minor at first, it becomes crucial when applying the Pythagorean theorem—a^2 = b^2 + c^2—to determine the distance (D) between the endpoints of the leg segments. This distance is key to assessing the feasibility of reaching a given target (x, y) position. To ensure reachability, D must not exceed the sum of the lengths of the two leg segments, or in other words, D <= 2*L.
Calculating the Hip Angle (Theta1)
To determine the hip angle, one must sum two distinct angles. The initial angle is formed between the horizontal axis and the target point (x,y) at the end of line D in our coordinate system. This can be calculated using the ArcTangent function, specifically arctan2(y/x) in standard practices. However, in my application, I employ arctan2(-y/x). The choice to use a negative y value due to my y values will consistently fall below zero. An alternative approach could involve taking the absolute value of the ArcTangent result to ensure a positive angle.
Following this, it's necessary to find the interior angle between line D and leg segment L1 within our conceptualized triangle. This angle, designated as alpha, can be determined through the law of cosines. In a simplified form, the calculation of alpha is expressed as acos(D / (2*L)). By adding alpha to the previously calculated angle, we derive the hip angle. However, there's a twist due to the servo's counterclockwise incrementation: the actual Theta1 is the supplement of the sum of alpha and our initial angle, mathematically expressed as Theta1 = 180 - (alpha + theta).
Calculating the Knee Angle (Theta2)
To calculate the knee angle, our first step involves identifying the interior angle between the two legs, L1 and L2, which we'll refer to as beta. Once again, the law of cosines proves invaluable for this calculation. While the deeper mathematical proofs are better left to academia, the simplified formula to compute beta is given by acos((2*L^2 - D^2) / (2*L^2)). This equation allows us to calculate beta, which represents the angle between the leg segments in our model.
However, to translate this angle into a form usable by the servo mechanism, additional adjustments are necessary due to the servo being linked to the leg segments via cams. We must take the supplementary angle to beta. This supplementary angle, once processed through the cam system, achieves the effect of pulling the leg segments into the correct position but in a reversed direction. Consequently, we must employ the complement of this supplementary angle to align with the actual geometry and movement direction required by the servo mechanism. This raises an interesting question: could the calculation have been simplified to just beta minus 90 degrees?
Code:
// Function to calculate and return the servo angles given a target (x, y)
void calcIK(float x, float y, float &theta1, float &theta2) {
float D = sqrt(x*x + y*y); // Distance from hip to target point (x hori, y vert always neg)
// Ensure the angle is within the robot's physical capability
if (D > 2 * L) {
// If the target is beyond the maximum reach, approximate values within range
D = 2 * L;
}
float theta = atan2(-y,x); // Angle to target from horizontal, need pos angle its upside down.
float alpha = acos(D / (2*L)); // Angle between leg segments for target
// For an inverted servo, use the complementary angle
theta1 = 180 - ((theta + alpha) * (180.0 / M_PI)); // Adjusting theta1 for the inverted servo
// Beta & Theta2 calculation, knee indepedent of hip
float beta = acos((2*L*L - D*D) / (2*L*L)); //interior angle between L1 and D
// Theta2 subtract from 180 to invert angle (complment); bisect beta for independent; adjust for servo rotation
theta2 = (180 - (beta * (180.0 / M_PI) / 2)) + ADJUST_B;
Serial.printf("D: %.2f, Theta: %.2f, Alpha: %.2f, Theta1: %.2f, Beta: %.2f, Theta2: %.2f\n",
D, theta * (180.0 / M_PI), alpha * (180.0 / M_PI), theta1, beta * (180.0 / M_PI), theta2);
}
)
-where is the rolling eyes emoji!
the crappy nature of these cheap servos, c) slight differences in connectors lengths, d) others...
