Inverse Kinematic equations for Lynxmotion 3DOF legs

Lynxmotion Legged Robots
1

I wanted to write my own code to control my new hexapod and I found it a little difficult to find a simplified set of IK equations, so thought I would post them here in case others are having similar issues.

I used this website as a guide in case anyone wanted a better explanation of the derivation of the equations. I am using VB6 right now, just because I am more comfortable with it, but the simplified equations are the obviously important part.

arduin0.blogspot.com/2012/01/inverse-kinematics-ik-implementation.html

These equations should be valid for almost all 3dof legs, but I am using the lynxmotion 3DOFB-BLK leg if anyone is interested.

lynxmotion.com/p-550-3dof-al … ervos.aspx

[code]Private Sub Command1_Click()

'inputs
pi = 3.14159265359
femurlength = 57
tibialength = 108
coxalength = 29

x = Val(Text3.Text)
y = Val(Text4.Text)
z = Val(Text5.Text)

'calculations
knee = ACos(((Sqr(((Sqr(x ^ 2 + z ^ 2)) - coxalength) ^ 2 + y ^ 2)) ^ 2 - tibialength ^ 2 - femurlength ^ 2) / (-2 * femurlength * tibialength)) * 180 / pi
swing = Atn(z / x) * 180 / pi
lift = (((Atn(((Sqr(x ^ 2 + z ^ 2)) - coxalength) / y)) + (ACos((tibialength ^ 2 - femurlength ^ 2 - (Sqr(((Sqr(x ^ 2 + z ^ 2)) - coxalength) ^ 2 + y ^ 2)) ^ 2) / (-2 * femurlength * (Sqr(((Sqr(x ^ 2 + z ^ 2)) - coxalength) ^ 2 + y ^ 2)))))) * 180 / pi) - 90

'display
Label10.Caption = FormatNumber(lift, 1)
Label11.Caption = FormatNumber(knee, 1)
Label12.Caption = FormatNumber(swing, 1)

Label13.Caption = 1436 + 9.7 * CInt(lift)
Label14.Caption = 402 + 9.7 * CInt(knee)
Label15.Caption = 1500 + 9.7 * CInt(swing)
End Sub

’ arc sine
’ error if value is outside the range -1,1]

Function ASin(value As Double) As Double
If Abs(CDbl(Val(value))) <> 1 Then
ASin = Atn(value / Sqr(1 - value * value))
Else
ASin = 1.5707963267949 * Sgn(value)
End If
End Function

’ arc cosine
’ error if NUMBER is outside the range -1,1]

Function ACos(ByVal number As Double) As Double
If Abs(CDbl(Val(number))) <> 1 Then
ACos = 1.5707963267949 - Atn(number / Sqr(1 - number * number))
ElseIf number = -1 Then
ACos = 3.14159265358979
End If
'elseif number=1 --> Acos=0 (implicit)
End Function[/code]

Variables in the attached screenshot equations are as follows:
L - Lift servo angle from horizontal
k - knee servo angle
s - horizontal servo swing angle
x - x foot position in mm
y - y foot position in mm
z - z foot position in mm
C - Coxa length
T - Tibia length
F - Femur Length

vb uses radians for all its calculations, so rad*180/pi will convert back to degrees, which is already in these equations.

Again, this is nothing new, but I figured if it took me as long as it did to get these simplified and working others might be struggling with the same issue either now or in the future.

screenshot.jpg
screenshot.jpg638×675 109 KB

1 Like
2

We get questions all the time about IK code / equations and refer people to this forum. Thanks for posting.

3

Here is my version for my raspberry pi hex. It’s based off of Kurt’s Arduino code but uses float point and normal math libraries. Excuse the poor comments. 

//====================================================================================== 
// Leg Inverse Kinematics 
//====================================================================================== 
// Calculates the angles of the coxa, femur and tibia for the given position of the feet 
// ikFeetPosX            - Input position of the Feet X 
// ikFeetPosY            - Input position of the Feet Y 
// ikFeetPosZ            - Input Position of the Feet Z
// legIKLedNr            - Input Leg Index for initCoxaAngle 
// femurAngle            - Output Angle of Femur in degrees 
// tibiaAngle            - Output Angle of Tibia in degrees 
// coxaAngle             - Output Angle of Coxa in degrees 
//====================================================================================== 
 
void Gait::LegIK ( int ikFeetPosX, int ikFeetPosY, int ikFeetPosZ, int legIKLegNr ) 
{ 
    float ikSW;                       // Length between Femur and Tibia 
    float ikRadiansFemurTibiaGround;  // Angle of the line Femur and Tibia with respect to the ground in radians 
    float ikRadiansFemurTibia;        // Angle of the line Femur and Tibia with respect to the femur in radians 
    float ikFeetPosXZ;                // Distance between the Coxa and Ground Contact 
 
    // Distance between the Coxa and Ground Contact 
    ikFeetPosXZ = sqrt ( pow (ikFeetPosX, 2 ) + pow (ikFeetPosZ, 2 ) ); 
 
    // ikSW - Length between Femur axis and Tibia 
    ikSW = sqrt ( pow ( ( ikFeetPosXZ - COXA_LENGTH ) , 2 ) + pow ( ikFeetPosY , 2 ) ); 
 
    // ikRadiansFemurTibiaGround - Angle between Femur and Tibia line and the ground in radians 
    ikRadiansFemurTibiaGround = atan2 ( ikFeetPosXZ - COXA_LENGTH, ikFeetPosY ); 
 
    // ikRadiansFemurTibia - Angle of the line Femur and Tibia with respect to the Femur in radians 
    ikRadiansFemurTibia = acos ( ( ( pow ( FEMUR_LENGTH, 2 ) - pow ( TIBIA_LENGTH, 2 ) ) + pow ( ikSW, 2 ) ) / ( 2 * FEMUR_LENGTH * ikSW ) ); 
 
    // ikCoxaAngle in degrees 
    coxaAngle[legIKLegNr] = atan2 ( ikFeetPosZ, ikFeetPosX ) * 180 / PI + initCoxaAngle[legIKLegNr]; 
 
    // ikFemurAngle in degrees 
    femurAngle[legIKLegNr] = -( ikRadiansFemurTibiaGround + ikRadiansFemurTibia ) * 180 / PI  + 90;
 
    // ikTibiaAngle in degrees 
    tibiaAngle[legIKLegNr] = -( 90 - ( ( ( acos ( ( pow (FEMUR_LENGTH, 2 ) + pow ( TIBIA_LENGTH, 2 ) - pow ( ikSW, 2 ) ) / ( 2 * FEMUR_LENGTH * TIBIA_LENGTH ) ) ) * 180 ) / PI ) ); 
}
4

I got a chance to test the IK equations today with a real leg… seems to work quite well…

youtube.com/watch?v=D7NpKWKqvNM

5

I wrote some more VB code to integrate a joystick control.

youtu.be/Wv3otTVdFpo

6

Maybe it’s time to grab the things again and resume my hexapod. This code makes my brain craves for work again at it… B)

7

Hi,
While specifying the X, Y and Z co ordinates should we specify the initial position or the final position in the code?

8

The code calculates the angles required for the final leg position

9

You are all making it too complicated by rigidly sticking to 1 coordinate system. Each femur & tibia pair can be in its own 2-D space which is then rotated by coxa. Finally all this is translated from the main body coordinate system. So simplify and use coordinate transformations instead. May not even need that as each leg only really has to move in it’s own space.
Get it? :grinning:

10

Very open to opinions, ideas and best practices. Could you perhaps start a new thread and share your ideas?

11

I agree with cbenson. I would really like to see more details, especially of a complete system.

12

To understand the mathematics / forces behind an 18 DoF hexapod and how to estimate the torque acting on each joint in a tripod gait: https://community.robotshop.com/tutorials/show/robot-leg-torque-tutorial
Note that this is NOT inverse kinematics. The Phoenix walking gait is an example of a hexapod gait.

13

Why is femur angle calculated like that. Shouldn’t the angle be the one between the y axis and the line between shoulder joint and the tip?