Thread: Inverse kinematics with Damped Least Squares Method

1. Inverse kinematics with Damped Least Squares Method

Hi Trossen Community,
I have been recycling old robots by taking them apart and thinking of new robots to build with them when I came across this unfinished idea of a 5DoF legged quadruped I had a while ago; the flying pig: bot: Now mind you I have all the spare servos, electronics and nearly all the brackets to make this robot and would only need to order the aluminum/abs plates, but before I move forward and even finish the solid works design, I want to know if I can handle 5DoF inverse kinematics on an Axon microcontroller.

So I started this thread to both document my exploration of advanced IK theory and to start a high level discussion regarding inverse kinematic techniques. What will follow is an in depth discussion of forward and inverse kinematics starting with my attempt to perform the Damped Least Squares method.  Reply With Quote

2. Re: Inverse kinematics with Damped Least Squares Method

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.  Reply With Quote Transistor Join Date
Dec 2009
Posts
26
Rep Power
37

Re: Inverse kinematics with Damped Least Squares Method

Like++.

Thanks for posting this, it's very interesting. When you port this into C, what matrix library do you plan to use? Does anyone have any good suggestions for basic matrix maths?  Reply With Quote

4. Re: Inverse kinematics with Damped Least Squares Method

Well I plan to write my own matrix library for C assuming I can't find one (a safe bet I think). We will see. Just as an update I have the Damped Least Squares working now in Matlab and will be posting more about it tomorrow when I am less tired :P  Reply With Quote

5. Re: Inverse kinematics with Damped Least Squares Method

I came across this in my never-ending quest for knowledge.

http://forums.trossenrobotics.com/tu...&searchid=4134

The code sample is not exactly what you're looking for, but could useful in writing your own library.

Cheers!   Reply With Quote

6. Re: Inverse kinematics with Damped Least Squares Method

Very Cool, but i believe the Delta Robot is one of those rare fully defined systems. You wouldn't need Damped Least Squares because there aren't redundant solutions to the IK problem. However I like the design very much, especially that it was pulled off with legos.  Reply With Quote

7. Re: Inverse kinematics with Damped Least Squares Method

So last night i finished the IK simulation on Matlab using the Damped Least Squares method. Using the symbolic math functions on Matlab I found the effector equation by typing in:

Code:
>> syms theta1
>> syms theta2
>> syms theta3
>> syms theta4
>> syms theta5
>> syms dist1
>> syms dist2
>> syms dist3
>> syms dist4
>> syms dist5
>> syms delta
>> effector = Get5DoFLeg(dist1,dist2,dist3,dist4,dist5,theta1,theta2,theta3,theta4,theta5)
>> effector = simplify(effector)
I then wrote the function GetJacobian and used it on the effector symbolic object to generate the jacobian equation for my leg

Code:
function result = GetJacobian(s,variables,numVar)

for i=1:3
for j=1:numVar
myJacobian(i,j)=diff(s(i),variables(j));
end
end

result = myJacobian;
I copy/pasted the resulting humongous equations into a new function which solved the jacobian for some given distances and theta angles.

The following video shows how I used the Jacobian to calculate the IK problem and shows a Matlab simulation of Damped Least Squares IK in action:

Using the Jacobian I calculated the damped pseudo inverse, Jdagger:

Code:
function result = GetJDagger(variables)

angle1 = variables(1);
angle2 = variables(2);
angle3 = variables(3);
angle4 = variables(4);
angle5 = variables(5);

d1 = 2;
d2 = 1;
d3 = 3;
d4 = 4;
d5 = 2;

delta = .5;

Jacobian = SolveJacobian(angle1,angle2,angle3,angle4,angle5);

Jdagger = transpose(Jacobian)*((Jacobian*transpose(Jacobian)+delta^2*eye(3,3))^-1);

result = Jdagger;
Finally I put together a simulation of the DLS process using the following function which .

Code:
function result = GetLegPath( Start, Target, iterations )

currentJoints = Start;
dist1 = 2;
dist2 = 1;
dist3 = 3;
dist4 = 4;
dist5 = 2;

for i=1:iterations
currentPos = Get5DoFLeg(currentJoints(1),currentJoints(2),currentJoints(3),currentJoints(4),currentJoints(5));
dE = Target - currentPos;
JDagger = GetJDagger(currentJoints);
dTheta = JDagger*dE;
currentJoints = currentJoints + dTheta;
end

result = currentJoints;
Each iterations of the GetLegPath brings the currentJoints closer to the values needed to place the leg at the Target position.

Well that's it. Within 24 hours I went from knowing nothing of advance IK theory to having a working Matlab model . My next task will be to port this code over to the Axon mcu and have it execute timed iterations of the DLS method to see if it is practical for such a processor to run a quadruped.
Last edited by WGhost9; 01-27-2010 at 12:12 AM.  Reply With Quote

8. Re: Inverse kinematics with Damped Least Squares Method

Update: Well I ported my some of my matlab code to C on the axon today and timed it. The forward kinematics can be calculated for a single leg in 4 msecs and the Jacobian can be calculated in 13-14msecs so I think the Axon CAN do four legged calculations on the fly. However, the ROM overhead is significant. With the Axon stripped of unneeded libraries I am still using up 75&#37; of the memory space to calculate Jdagger. I need to either find a better (read more program space) micro controller or a I need to double up two axon microcontrollers to get this to work.  Reply With Quote

9. Re: Inverse kinematics with Damped Least Squares Method

Okay so I have some results from the Axon (ATmega640) running the Damped Least Squares algorithm:

Time to calculate the forward kinematics of a 5DoF leg: 3-4msecs
Time to calculate the the Jacobian: 14 msecs
Time to calculate the damped pseudo inverse of the Jacobian (Jdagger): 4 msecs

Total time needed to place a leg: 22msecs
Total time needed to place 4 legs: 88msecs
Estimated refresh rate for leg placements: Ten times a second

Code Space Required For Fast DLS: 40-56 kBytes

As you can see, the Axon can probably perform a reasonably smooth DLS algorithm. However it won't have the code space left for any other application. What I need is a new microcontroller that is at least as fast as the Axon and has more code space. I am thinking of buying a XMOS XC-A1 board and hooking it up to an SSC-32 but I am not sure how to do this or if it is even feasible. Perhaps there is another microcontroller I should be looking at? Please let me know what you think.
Last edited by WGhost9; 01-20-2010 at 12:57 PM.  Reply With Quote

10. Re: Inverse kinematics with Damped Least Squares Method

Absolutely you can hook the XMOS up to an SSC32. Or many many SSC32's.

Software serial is in the tutorials.  Reply With Quote