Trossen Force Sensor  Top Banner
Page 1 of 2 12 LastLast
Results 1 to 10 of 20

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.

  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
    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,drawColor)
    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,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.

  3. Re: Inverse kinematics with Damped Least Squares Method


    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? - for robot builders and enthusiasts

  4. Trossen Distance Sensor  Thread Banner
  5. 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

  6. #5
    Join Date
    Sep 2008
    Toronto, Ontario
    Rep Power

    Re: Inverse kinematics with Damped Least Squares Method

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

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


  7. 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.

  8. 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:

    >> 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

    function result = GetJacobian(s,variables,numVar)
    for i=1:3
        for j=1:numVar
    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),currentJoints(3),currentJoints(4),currentJoints(5));
        dE = Target - currentPos;
        JDagger = GetJDagger(currentJoints);
        dTheta = JDagger*dE;
        currentJoints = currentJoints + dTheta;
    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-26-2010 at 11:12 PM.

  9. 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% 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.

  10. 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 11:57 AM.

  11. #10
    Join Date
    Apr 2008
    Sacramento, CA, USA Area
    Rep Power

    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.
    I Void Warranties´┐Ż

Thread Information

Users Browsing this Thread

There are currently 1 users browsing this thread. (0 members and 1 guests)

Similar Threads

  1. Phoenix Inverse Kinematics Port
    By zhanx in forum Software and Programming
    Replies: 21
    Last Post: 10-16-2009, 01:12 PM
  2. Generating closed form solutions for Inverse Kinematics
    By Obscene in forum Software and Programming
    Replies: 0
    Last Post: 12-08-2008, 12:40 AM
  3. Kondo web page translated.
    By Droid Works in forum Humanoids, Walkers & Crawlers
    Replies: 4
    Last Post: 10-06-2007, 05:49 PM

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts