Inverse Kinematics (IK) is a really handy tool used when controlling moving linkages. It’s used whenever you want to move a series of linkages by locating the end of the linkages instead of having to worry about all the joint angles in between. For example, your brain does this when you want to grab a cup of coffee. You don’t think “shoulder angle to X, elbow angle to Y, wrist angle to Z”. You think “hand to coffee mug” and your brain takes care of all the angles necessary to achieve the hand goal position. IK is commonly used not only in robotics, but also in 2D and 3D animation, for example.

Like I mentioned in my intro post, I first worked on some leg IK a few  years ago. To get started, i dug up my old code. The method I used was a bit overkill and involved using rotation matrices rotating coordinate spaces defined for each joint in the leg. It basically involves setting up a bunch of matrices in a certain form and then crunching them up into closed form equations. It ended up being too processor intensive and took like 17ms to run. That’s out of my 33ms frame budget! There’s a lot more crunching ahead so that’s just too much. I decided to take a simpler approach which is possible with our simple 3DOF legs. You can figured it all out with some basic trig.

One great resource I found is Oscar Liang’s blog. His tutorial on solving 3DOF IK using trig is very clear and has better diagrams than i can draw so take a look at it here.

Going through his tutorial I was most of the way home. The final step was to add in angular offsets for my physical hardware. Because the Tibia servo is mounted on an offset bracket off the Femur, I added in that angle. And then because the tibia itself is mounted straight, i added in 90deg so that it pointing downward is centered. Finally the tibia itself has a bend to it so i added an offset for that as well.

My final function looks like:

/**************************************************************************************************************
legIK()
Calculates the coxa, femur, and tibia angles for input foot position in X,Y,Z.
***************************************************************************************************************/
void legIK(){

float CoxaFootDist, IKSW, IKA1, IKA2, tibAngle;

for( int legNum=0; legNum<6; legNum++ ){ CoxaFootDist = sqrt( sq(leg[legNum].footPosCalc.y) + sq(leg[legNum].footPosCalc.x) ); IKSW = sqrt( sq(CoxaFootDist-LENGTH_COXA) + sq(leg[legNum].footPosCalc.z) ); IKA1 = atan2( (CoxaFootDist - LENGTH_COXA) , leg[legNum].footPosCalc.z ); IKA2 = acos( (sq(LENGTH_TIBIA) - sq(LENGTH_FEMUR) - sq(IKSW) ) / (-2*IKSW*LENGTH_FEMUR) ); tibAngle = acos( (sq(IKSW) - sq(LENGTH_TIBIA) - sq(LENGTH_FEMUR)) / (-2*LENGTH_FEMUR*LENGTH_TIBIA) ); leg[legNum].jointAngles.coxa = 90 - degrees( atan2( leg[legNum].footPosCalc.y , leg[legNum].footPosCalc.x) ); leg[legNum].jointAngles.femur = 90 - degrees( IKA1 + IKA2 ); leg[legNum].jointAngles.tibia = 90 - degrees( tibAngle ); } // Applies necessary corrections to servo angles to account for hardware for( int legNum=0; legNum<3; legNum++ ){ leg[legNum].jointAngles.coxa = leg[legNum].jointAngles.coxa; leg[legNum].jointAngles.femur = leg[legNum].jointAngles.femur - 13.58; // accounts for offset servo bracket on femur leg[legNum].jointAngles.tibia = leg[legNum].jointAngles.tibia - 48.70 + 13.58 + 90; //counters offset servo bracket on femur, accounts for 90deg mounting, and bend of tibia } for( int legNum=3; legNum<6; legNum++ ){ leg[legNum].jointAngles.coxa = leg[legNum].jointAngles.coxa; leg[legNum].jointAngles.femur = -(leg[legNum].jointAngles.femur - 13.58); leg[legNum].jointAngles.tibia = -(leg[legNum].jointAngles.tibia - 48.70 + 13.58 + 90); } }[/cpp] On to Phase 3: Body Kinematics, i.e., Body Translation/Rotation with the Feet Planted!

Or head back to the beginning of Project B.E.T.H.