This is a short explanation of how to calculate the leg angles in our quadruped robot using basic trigonometry. If you don’t need the explanation but just want an example implementation scroll to the bottom to see the code sample.

As you can see in the graphic above, we want to find the angles A1 and A4:

• A1 is the rotation angle of our first segment around the hip joint
• A4 is the rotation angle of our second segment around the knee joint

What we know is L1, L2, P1 and P2 where:

• L1 is the length of our femur
• L2 is the length of our tibia
• P1 is the origin point, or position of our hip (0,0)
• P2 is the requested end position (or destination) for the feet

So, basically we want to how (much) does our hip and knee need to rotate in order for our feet to reach the requested end position.

We can calculate our knee angle using the law of cosines (https://en.wikipedia.org/wiki/Law_of_cosines). But to do so we need to know the length of the tree sides of the triangle L1, L2 & K.

So we start calculating our missing side K. We find the to segments H1 and H2 using the delta between our origin (P1) and our destination (P2):


int H1 = P2.x - P1.x; //delta on the x axis
int H2 = P2.y - P1.y; //delta on the y axis


Because H1 and H2 create a right-angled triangle, we can use the Pythagorean Theorem (http://www.mathplanet.com/education/pre-algebra/right-triangles-and-algebra/the-pythagorean-theorem) to find K (the hypothenuse):


float K = sqrt( pow(H1,2) + pow(H2,2));


Now we can get the knee angle:


//angle between L1 & L2 - the to segments, femur and tibia
float A1 = acos(( pow(L1,2) + pow(L2,2) - pow(K,2))/ (2 * L1 * L2));


Getting our hip angle is bit more convoluted. We need to find the angle A2 (no problem, having the tree sides is just more law of cosines), and A3 (another right-angled triangle – more Pythagoras).


//get the angle between the hypothenuse and the first segment (femur)
float A2 = acos( (pow(K,2) + pow(L1,2) - pow(L2,2)) / (2 * K * L1));

//get the angle between the hypothenuse and the x axis

float A3 = asin(H1/K);

Then we add this to angles and subtract the sum from 180 degrees and we are left with our hip angle.


float A4 = (PI / 2) - (A2 + A3);


Below you can see a function implemented for an Arduino sketch.


/**********************************************************
* Inverse Kinematic function for a two link planar system.
* Given the size of the two links an a desired position,
* it returns a pose struct with angles for both links
**********************************************************/
struct pose IK(int L1, int L2, struct point P2)
{
struct pose a;
struct point P1 = { HIP_ORIGIN_X, HIP_ORIGIN_Y }; //origin point (hip)

int H1 = P2.x – P1.x; //delta on the x axis
int H2 = P2.y – P1.y; //delta on the y axis

//this is the hypothenuse between the origin and the target
float K = sqrt( pow(H1,2) + pow(H2,2));

//the hypothenuse can not be larget the the sum of the segments
if( K > (L1 + L2 )) K = L1 + L2;

//knee rotational angle
float A1 = acos(( pow(L1,2) + pow(L2,2) – pow(K,2))/ (2 * L1 * L2));

//convert from radians to degrees and cast as an integer
float A1_ = A1 * RAD_TO_DEG;
int A1__ = (int) A1_;

//get the angle between the hypothenuse and the first segment (femur)
float A2 = acos( (pow(K,2) + pow(L1,2) – pow(L2,2)) / (2 * K * L1));

//get the angle between the hypothenuse and the x axis
float A3 = asin(H1/K);

//get the hip rotational angle
float A4 = (PI / 2) – (A2 + A3); //add the two angles, substract it from half a circle

float A4_ = A4 * RAD_TO_DEG;
int A4__ = (int) A4_;

a.hip_angle = A4__;
a.knee_angle = A1__;

return a;
}