PDA

View Full Version : [Discussion] Inverse kinematics with Damped Least Squares Method

WGhost9
01-18-2010, 03:43 PM
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:
http://forums.trossenrobotics.com/gallery/files/2/9/7/4/flying_pig.jpg

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.

WGhost9
01-18-2010, 04:02 PM
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:

http://forums.trossenrobotics.com/gallery/files/2/9/7/4/annotated_leg.bmp

To get the rotation matrices around a variable axis of rotation I used the following snipet of 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:

function result = GetLegSeg(start,Displacement,RotAngle,Axis,drawCol or)

Axis = Axis/(sum((Axis).^2))^(.5);
lengthDisp = (sum((Displacement).^2))^(.5);

RotMatrix = GetRotation(RotAngle,Axis);

finish = start + RotMatrix*Displacement;

result = finish-start;

function result = Get5DoFLeg(dist1,dist2,dist3,dist4,dist5,theta1,th eta2,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:
http://forums.trossenrobotics.com/gallery/files/2/9/7/4/unperturbed_leg.jpg
http://forums.trossenrobotics.com/gallery/files/2/9/7/4/slightly_perturbed_leg.jpg
http://forums.trossenrobotics.com/gallery/files/2/9/7/4/globably_perturbed_leg.jpg

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.

01-18-2010, 06:57 PM
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?

WGhost9
01-18-2010, 09:43 PM
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

kamondelious
01-19-2010, 08:17 AM
I came across this in my never-ending quest for knowledge.

http://forums.trossenrobotics.com/tutorials/introduction-129/delta-robot-kinematics-3276/?catid=searchresults&searchid=4134

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

Cheers!

:)

WGhost9
01-19-2010, 10:21 AM
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.

WGhost9
01-19-2010, 12:57 PM
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:

>> 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,th eta2,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

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:

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 .

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),curre ntJoints(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 :happy:. 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.

WGhost9
01-19-2010, 10:14 PM
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.

WGhost9
01-20-2010, 11:39 AM
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.

01-20-2010, 09:02 PM
Absolutely you can hook the XMOS up to an SSC32. Or many many SSC32's.

Software serial is in the tutorials.

societyofrobots
01-21-2010, 09:59 PM
Wow, I believe you are the first to potentially use up all the Axon RAM!

Not knowing your programming skill level, I recommend you reading this C code optimization tutorial:
http://www.societyofrobots.com/member_tutorials/node/202
Especially section 09, Reducing SRAM requirements. Hopefully it'll help.

Anyway, thanks for posting the IK timing requirements - very useful to know!

lnxfergy
01-21-2010, 10:10 PM
Wow, I believe you are the first to potentially use up all the Axon RAM!

Actually, he's talking about the FLASH ROM, which I think is even crazier. You can eat up the RAM pretty fast storing occupancy grids (I've done it on other AVRs), but eating up all the ROM, in like a day of coding, is really impressive. The entire G-code interpreter for the RepRap project hasn't even quite managed that yet, and that's a 3+ year 20+ developer project....

-Fergs

societyofrobots
01-21-2010, 10:27 PM
Actually, he's talking about the FLASH ROM, which I think is even crazier. You can eat up the RAM pretty fast storing occupancy grids (I've done it on other AVRs), but eating up all the ROM, in like a day of coding, is really impressive. The entire G-code interpreter for the RepRap project hasn't even quite managed that yet, and that's a 3+ year 20+ developer project....

ROM? Hmmm I've never used more than 62% on my Axon, and thats with optimization turned off . . .

That said, the 640 has 64kb, but if you have l337 soldering skills, you can unsolder it and replace the chip with the 2560. It'll give you 256kb. You just need to change one line in the makefile to make it 100% compatible. WebbotLib also supports the 2560.

And yea, mapping eats RAM like a bunch of monkeys!

lnxfergy
01-21-2010, 10:33 PM
ROM? Hmmm I've never used more than 62% on my Axon

Yep, I run mega644p most of the time, so it's got the 64k as well, and I rarely top 25k of code...

-Fergs

WGhost9
01-22-2010, 02:15 AM
You can eat up the RAM pretty fast storing occupancy grids (I've done it on other AVRs), but eating up all the ROM, in like a day of coding, is really impressive.

Haha I can code like mad when needed but the real reason for the huge code space usage is that the forward kinematic equation for 5DoF is almost a page long, and the Jacobian for that is longer. There is a reason I used Matlab to generate those equations :)

That said, the 640 has 64kb, but if you have l337 soldering skills, you can unsolder it and replace the chip with the 2560. It'll give you 256kb. You just need to change one line in the makefile to make it 100% compatible. WebbotLib also supports the 2560.

I would love to do that but unfortunately I stink at soldering; I have very shaky hands. I am strictly a math and coding kind of guy.

WGhost9
01-22-2010, 01:11 PM
Well I have had 3 days to play around with the DLS algorithm and I would like to post my thoughts on the method.

First the straight up DLS algorithm has some drawbacks. One is that the joints can move freely around 360 degrees but in reality servos can only move 180 degrees around a specific offset. Two, the DLS algorithm allows for a single effector- let me explain that; the DLS algorithm only places the leg so that the tip in question reaches the goal point. However, if you want a second, third or more joints to be oriented or configured into certain ways relative to that goal point, you need to modify the DLS. For example, I not only want the pigs leg's to reach there goal point but I also want them to keep the foot segment of the leg as vertical as possible.

Luckily my math skills came in handy and I came up with solutions to these problems that work pretty well. First to account for joints that only bend so far, you can perform a change of variables on each joint.

theta(i) = offset(i) + range(i)*sin(gamma(i))

This change of variables fixes the the joint, theta(i) to only vary between (offset+range) and (offset-range). One then recalculates the Jacobian so that the differentials are in terms of gamma instead of theta:

J(i,j) = dE(i)/dGamma(j)

The DLS algorithm then proceeds to calculate the damped pseudo inverse Jacobian and uses that to calculate the the change in Gamma which can then be used to calculate the new Theta values for the joints.

Second to handle the single effector problem we can add addition effectors to the leg by growing the effector vector e to include more coordinates. For example, say I want the ankle (H5) to be placed vertically above the foot of the leg (H6) in my above pig robot. Then I create an effector:

E = [H6.x; H6.y; H6.z; H6.x; H6.y; (H6.z+d5);]
J(i,j) = dE(i)/dTheta(j) %where J is now a 6 by 5 matrix instead of a 3 by 5 matrix

By expanding the effector vector and the Jacobian with extra rows, the DLS algorithm attempts to minimize the distance between the current position of both effectors H5 and H6 and their goal positions.

Anyhow, I felt that since these useful modifications to the DLS algorithm aren't found in the academic literature, I ought to at least post them here for others to be aware of if they decide they want to use these techniques themselves. If you want more details on the DLS algorithm, feel free to ask me about it anytime.

innerbreed
01-29-2010, 06:10 PM
im also running a similar project, you may view it here.
http://www.lynxmotion.net/viewtopic.php?f=8&t=5788

the interesting thing is my leg design is remarkably similar.
I cannot think of anything weirder than that! :happy:
Great minds think alike!!

your project is fascinating. Why a Pig?

forgive me for not reading the whole thread but what servos are you using?

All the best,
Jonny - UK.

WGhost9
01-29-2010, 07:42 PM
Wow indeed very similar. The reason I was thinking pig was a am recycling two old robots to make this one and one of those robots had a pair of wings. With a quadruped wearing wings, I could go with a flying pig or a pegasus :). As for the servos the will mostly be 5645-MGs since I have 20 spare ones from the now scrapped robots. I would love to know what microcontroller/IK algorithms you are thinking of using for your project?

innerbreed
01-30-2010, 04:17 AM
just so i dont spam up your thread ill try and keep mine updated there. :wink:
but at the moment im only using simple servo command and no IK coding at the moment. just dont know were to start with that. im using SSC-32 Botboards2 with Atom28 Pro chip.

again great work, the Pegasus idea is cool. :happy:

j

dash
07-15-2011, 06:45 AM
Hi Wghost,

I am using the damped least squares method to determine the inverse kinematics of a system I am designing. I have a 3x4 jacobian. However I am struggling to figure out how to do joint constraints. do you know how to do it? is there a set formula for the damping ratio? Please can you assist me.

Thank you a lot your help will be extremely appreciated.