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?
// 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); }