The first task to working out the inverse kinematics is to work out the forward kinematics. I used Matlab to simulate the forward kinematics because its symbolic math will allow me to quickly and easily take derivative later on and then port the results to the Axon's C code. The following is a annotated picture of my 5DoF leg showing how I labeled each angle, distance, and joint in the code:

To get the rotation matrices around a variable axis of rotation I used the following snipet of code

Code:

function result = GetRotation(RotAngle,Axis)
%Normalize the Axis of rotation
Axis = Axis/(sum((Axis).^2))^(.5);
skewSymmMat = [ 0 -Axis(3) Axis(2);
Axis(3) 0 -Axis(1);
-Axis(2) Axis(1) 0;];
outerProduct = [Axis(1)*Axis(1) Axis(1)*Axis(2) Axis(1)*Axis(3);
Axis(2)*Axis(1) Axis(2)*Axis(2) Axis(2)*Axis(3);
Axis(3)*Axis(1) Axis(3)*Axis(2) Axis(3)*Axis(3);];
result = outerProduct + cos(RotAngle)*(eye(3,3)-outerProduct)+sin(RotAngle)*skewSymmMat;

Using this function I then wrote a function for calculating the forward kinematics of the entire leg:

Code:

function result = GetLegSeg(start,Displacement,RotAngle,Axis,drawColor)
Axis = Axis/(sum((Axis).^2))^(.5);
lengthDisp = (sum((Displacement).^2))^(.5);
RotMatrix = GetRotation(RotAngle,Axis);
finish = start + RotMatrix*Displacement;
result = finish-start;

Code:

function result = Get5DoFLeg(dist1,dist2,dist3,dist4,dist5,theta1,theta2,theta3,theta4,theta5)
d0 = [0; 1; 0];
H1= [0; 0; 0];
d1 = [0; 0; -dist1];
d1 = GetLegSeg(H1,d1,theta1,d0,'bo-');
H2 = H1+d1;
d2 = [0; dist2; 0];
d2 = GetLegSeg(H2,d2,theta2,d1,'ro-');
legBendAxis = cross(d1,d2);
H3 = H2+d2;
hipDirection = d1/(sum((d1).^2))^(.5);
d3 = dist3*hipDirection;
d3 = GetLegSeg(H3,d3,theta3,legBendAxis,'ko-');
H4 = H3+d3;
d4 = dist4*hipDirection;
d4 = GetLegSeg(H4,d4,theta3+theta4,legBendAxis,'ko-');
H5 = H4+d4;
d5 = dist5*hipDirection;
d5 = GetLegSeg(H5,d5,theta3+theta4+theta5,legBendAxis,'ko-');
H6 = H5+d5;
result = H6;

Using these functions I then plotted the leg segments at different theta angles:

These few sample pictures aren't much but they show the forward Kinematics in action. By inserting some symbolic math objects into the above matlab script I can calculate the set of three HUGE equations that express the end point of the leg (the effector) in terms of the joint angles. I won't bother reprinting them here since they are over half a page long. In my next post I will show how to use the forward kinematics to work out the Jacobian and the use the Jacobian to solve the inverse kinematic problem.

## Bookmarks