Inverse Kinematics implementation

[AngelLM]

It could be nice to have the IK implemented, in order to control the robot using XYZ coordinates and a rotation matrix.

[AngelLM]

Currently working on it.

[Danny-VdH]

The best open source so far (c# ), used in my software and working well. Made some little modification to it.

` //--------------------------------------------------------------------------- float[] _InverseK(float Xreal, float Yreal, float Zreal, float alfa1, float beta1, float gamma1) { float PI = 3.141592654f; float Rad = PI / 180; float Grad = 180 / PI;

    //initial values ​​of point P wrist, Px,Py,Pz

    //orientation matrix parameters
    float cosalfa1 = Mathf.Cos(alfa1 * Rad);
    float senalfa1 = Mathf.Sin(alfa1 * Rad);
    float cosbeta1 = Mathf.Cos(beta1 * Rad);
    float senbeta1 = Mathf.Sin(beta1 * Rad);
    float cosgamma1 = Mathf.Cos(gamma1 * Rad);
    float sengamma1 = Mathf.Sin(gamma1 * Rad);

    //X-Y-Z SET ANGLES
    //X'
    float r11 = (cosalfa1 * cosbeta1);   
    float r21 = (senalfa1 * cosbeta1);   
    float r31 = -senbeta1;               
    //Y'
    float r12 = (cosalfa1 * senbeta1 * sengamma1) - (senalfa1 * cosgamma1); 
    float r22 = (senalfa1 * senbeta1 * sengamma1) + (cosalfa1 * cosgamma1); 
    float r32 = (cosbeta1 * sengamma1);                                     
    //Z'
    float r13 = (cosalfa1 * senbeta1 * cosgamma1) + (senalfa1 * sengamma1);
    float r23 = (senalfa1 * senbeta1 * cosgamma1) - (cosalfa1 * sengamma1);
    float r33 = (cosbeta1 * cosgamma1);

    float a2 = LongBrazo;
    float d4 = LongAntBr + LongMunec / 2;
    float d6 = LongMunec / 2 + LongTool / 2;

// float d4 = LongAntBr; // float d6 = LongMunec + LongTool; float a22 = a2 * a2; float d42 = d4 * d4; //int d = 0;

    float xc = Xreal - d6 * r13;
    float yc = Yreal - d6 * r23;
    float zc = (Zreal - AlturaH) - d6 * r33;

    float x2 = xc * xc;
    float y2 = yc * yc;
    float z2 = zc * zc;
    //float d22 = d * d;

    //Calculate theta1
    theta1 = (Mathf.Atan2(yc, xc)) * Grad;

    //Calculate theta3
    float K = (x2 + y2 + z2 - a22 - d42) / (2 * a2);
    float Raiz3 = Mathf.Sqrt(Mathf.Abs(Mathf.Abs(d42) - (K * K)));

// float Raiz3 = Mathf.Sqrt(Mathf.Abs(d42) - (K * K)); theta3 = -(Mathf.Atan2(K, Raiz3)) * Grad;

    float costheta1 = Mathf.Cos(theta1 * Rad);
    float sentheta1 = Mathf.Sin(theta1 * Rad);
    float costheta3 = Mathf.Cos(theta3 * Rad);
    float sentheta3 = Mathf.Sin(theta3 * Rad);

    float Y = (-a2 * costheta3) * zc - (costheta1 * xc + sentheta1 * yc) * (d4 - a2 * sentheta3);
    float X = ((a2 * sentheta3 - d4) * zc + (a2 * costheta3) * (costheta1 * xc + sentheta1 * yc));

    //Calculate theta2
    float theta23 = (Mathf.Atan2(Y, X)) * Grad;
    theta2 = theta23 - theta3;
    float costheta2 = Mathf.Cos(theta2 * Rad);
    float sentheta2 = Mathf.Sin(theta2 * Rad);

    float costheta23 = (costheta2 * costheta3) - (sentheta2 * sentheta3);
    float sentheta23 = (sentheta2 * costheta3) + (costheta2 * sentheta3);

    //Calculate theta4
    Y = ((costheta1 * r23) - (sentheta1 * r13));
    X = (-(costheta1 * costheta23 * r13) - (sentheta1 * costheta23 * r23) + (sentheta23 * r33));

    theta4 = (Mathf.Atan2(Y, X)) * Grad;
    float costheta4 = Mathf.Cos(theta4 * Rad);
    float sentheta4 = Mathf.Sin(theta4 * Rad);

    //Calculate theta5
    Y = (-r13 * ((costheta1 * costheta23 * costheta4) + (sentheta1 * sentheta4)) - r23 * ((sentheta1 * costheta23 * costheta4) - (costheta1 * sentheta4)) + r33 * (sentheta23 * costheta4));
    X = (-r13 * (costheta1 * sentheta23) - r23 * (sentheta1 * sentheta23) - r33 * (costheta23));

    theta5 = (Mathf.Atan2(Y, X)) * Grad;
    float costheta5 = Mathf.Cos(theta5 * Rad);
    float sentheta5 = Mathf.Sin(theta5 * Rad);

    //Calculate theta6
    float Y2 = (-r11 * ((costheta1 * costheta23 * sentheta4) - (sentheta1 * costheta4)) - r21 * ((sentheta1 * costheta23 * sentheta4) + (costheta1 * costheta4)) + r31 * (sentheta23 * sentheta4));
    float X61 = (costheta1 * costheta23 * costheta4 + sentheta1 * sentheta4) * costheta5 - (costheta1 * sentheta23 * sentheta5);
    float X62 = (sentheta1 * costheta23 * costheta4 - costheta1 * sentheta4) * costheta5 - (sentheta1 * sentheta23 * sentheta5);
    float X63 = sentheta23 * costheta4 * costheta5 + costheta23 * sentheta5;
    float X2 = r11 * X61 + r21 * X62 - r31 * X63;

    theta6 = (Mathf.Atan2(Y2, X2)) * Grad;
    float costheta6 = Mathf.Cos(theta6 * Rad);
    float sentheta6 = Mathf.Sin(theta6 * Rad);

    // return all calculated angles
    return new float[] { theta1, theta2, theta3, theta4, theta5, theta6 };
}
//---------------------------------------------------------------------------  

` Hope it helps.

[Danny-VdH]

[Danny-VdH]

[AngelLM]

Wow it looks nice, thank you! I'll take a look. I was stuck on the Rotation Matrix and how to send a Gcode with those 9 parameters. I'm working on it :)

[b-adkins]

Why not use Robot Operating System? MoveIt makes IK and planning pretty easy. It's already used by a lot of robots. This has been my plan when I build one.

[AngelLM]

Hi! I never worked with ROS so... that's why I didn't use it. Maybe it could be a nice way to implement control... I'll look at it when I have time. Thanks!

[zisi]

Hey, you can also use kdl, http://www.orocos.org/kdl. It doesn't give analytical equations but it uses numerical methods to calculate the IK.

[b-adkins]

FYI, KDL is MoveIt's default IK solver. ROS and MoveIt make it really easy to control the robot in either XYZ space or joint space.

[b-adkins]

I set up Thor in ROS MoveIt!

I'm still getting my hands on all the parts, but in the meantime I did some coding... Check it out!

[VictorLamoine]

:+1: for Robot Operating System.

You'll never reach alone the complexity of what you can find in ROS, plus there is no point in coding that again apart for fun or educational purposes.

Moving this robot towards ROS will allow very complex path planning with occupancy grids and collision avoidance; torque control, adding vision (2D/3D) sensors... well pretty much anything you can imagine on a modern industrial robot.

Cheers for this project :beer: It's awesome

[AngelLM]

Hi all! I attempted to use KDL libraries when I was controlling the robot with 2 RAMPS boards. I had no enough time to make it work, so I moved to FK instead of IK. @b-adkins That looks amazing! I'll take a look :)

[AngelLM]

This task will be done in the Asgard repo, so I have taken note of it and closing this here. Thanks for your help!