# Thread: Hexapod math problem with InverseKinematic

1. misk0 Relay Join Date
Nov 2017
Posts
5
Rep Power
9

## Hexapod math problem with InverseKinematic

Hi guys,

I'm trying to make my hexapod Phoenix 3DOF walk so I started to write code. I use Arduino Uno and Lynxmotion SSC-32U. I'm stuck on math calculations and I have difficulties to move on. So I found IK calculations online, there are different sites explaining the formulas and as I can understand they all are aligned.
I used one from Oscar Liang from Trigonometry Basic and I started from coxa joint. I choose left middle leg, and as starting point I decided coordinates (0,0,0) - (x,y,z). It's the point where coxa is attached to the case.
Then I measured where this leg touchs the ground and it's coordinate (0, 131, - 126) - z is negative because positive direction is up.
I want to make a calculations for coxa servo so when X moves forward and backward servo should change a possion accordingly.
When leg is going back, x is increasing and servo should move leg to the back, and when it goes forward then x is decreasing
So formula for this angle is This is a code I wrote:
Code:
```// coxa - 27mm
// femore - 82mm
// tibia - 122mm
// z-offset - 112mm

#include <SoftwareSerial.h>
float rootX = 0;
float rootY = 0;
float rootZ = 0;
float groundX = 131;
float groundY = 1;
float groundZ = 126;

SoftwareSerial usc(10,11);
void setup() {
Serial.begin(9600);
usc.begin(115200);

usc.println("#20P900");
delay(2000);
float zero = 1500;
float error = -250;
float servoMin = 900;
float servoMax = 1900;
float maxAngle = 90;

float gama = degrees(atan(groundX / groundY));
float servoPosition = zero + error + gama/maxAngle * (servoMax - servoMin);

for(int i = -131; i < 131; i +=5 ) {
delay(100);
groundY = i;
Serial.print("y = mm: ");
Serial.print(i);

Serial.print("  x/y = ");
Serial.print(groundX / groundY);

gama = degrees(atan(groundX / groundY));
Serial.print("   gama: ");
Serial.print(gama);

servoPosition = (zero + error) - (gama/maxAngle * (servoMax - servoMin));
Serial.print("   servo position: ");
Serial.println(servoPosition);
}
}

void loop() {
}```
Problem is that I'm trying to move leg from forward to back changing only X coordinate (from -131 to 131) but angles are changing from -45 to -90 and from 90 to 45.
This is the output I'm getting
Code:
```y = mm: -131  x/y = -1.00   gama: -45.00   servo position: 1750.00
y = mm: -126  x/y = -1.04   gama: -46.11   servo position: 1762.38
y = mm: -121  x/y = -1.08   gama: -47.27   servo position: 1775.25
y = mm: -116  x/y = -1.13   gama: -48.48   servo position: 1788.61
y = mm: -111  x/y = -1.18   gama: -49.72   servo position: 1802.49
y = mm: -106  x/y = -1.24   gama: -51.02   servo position: 1816.91
y = mm: -101  x/y = -1.30   gama: -52.37   servo position: 1831.87
y = mm: -96  x/y = -1.36   gama: -53.77   servo position: 1847.39
y = mm: -91  x/y = -1.44   gama: -55.21   servo position: 1863.49
y = mm: -86  x/y = -1.52   gama: -56.72   servo position: 1880.17
y = mm: -81  x/y = -1.62   gama: -58.27   servo position: 1897.45
y = mm: -76  x/y = -1.72   gama: -59.88   servo position: 1915.33
y = mm: -71  x/y = -1.85   gama: -61.54   servo position: 1933.81
y = mm: -66  x/y = -1.98   gama: -63.26   servo position: 1952.89
y = mm: -61  x/y = -2.15   gama: -65.03   servo position: 1972.57
y = mm: -56  x/y = -2.34   gama: -66.85   servo position: 1992.82
y = mm: -51  x/y = -2.57   gama: -68.73   servo position: 2013.65
y = mm: -46  x/y = -2.85   gama: -70.65   servo position: 2035.02
y = mm: -41  x/y = -3.20   gama: -72.62   servo position: 2056.90
y = mm: -36  x/y = -3.64   gama: -74.63   servo position: 2079.27
y = mm: -31  x/y = -4.23   gama: -76.69   servo position: 2102.07
y = mm: -26  x/y = -5.04   gama: -78.77   servo position: 2125.27
y = mm: -21  x/y = -6.24   gama: -80.89   servo position: 2148.81
y = mm: -16  x/y = -8.19   gama: -83.04   servo position: 2172.63
y = mm: -11  x/y = -11.91   gama: -85.20   servo position: 2196.67
y = mm: -6  x/y = -21.83   gama: -87.38   servo position: 2220.86
y = mm: -1  x/y = -131.00   gama: -89.56   servo position: 2245.14
y = mm: 4  x/y = 32.75   gama: 88.25   servo position: 269.43
y = mm: 9  x/y = 14.56   gama: 86.07   servo position: 293.67
y = mm: 14  x/y = 9.36   gama: 83.90   servo position: 317.78
y = mm: 19  x/y = 6.89   gama: 81.75   servo position: 341.69
y = mm: 24  x/y = 5.46   gama: 79.62   servo position: 365.35
y = mm: 29  x/y = 4.52   gama: 77.52   servo position: 388.69
y = mm: 34  x/y = 3.85   gama: 75.45   servo position: 411.66
y = mm: 39  x/y = 3.36   gama: 73.42   servo position: 434.21
y = mm: 44  x/y = 2.98   gama: 71.43   servo position: 456.29
y = mm: 49  x/y = 2.67   gama: 69.49   servo position: 477.87
y = mm: 54  x/y = 2.43   gama: 67.60   servo position: 498.91
y = mm: 59  x/y = 2.22   gama: 65.75   servo position: 519.40
y = mm: 64  x/y = 2.05   gama: 63.96   servo position: 539.31
y = mm: 69  x/y = 1.90   gama: 62.22   servo position: 558.63
y = mm: 74  x/y = 1.77   gama: 60.54   servo position: 577.35
y = mm: 79  x/y = 1.66   gama: 58.91   servo position: 595.47
y = mm: 84  x/y = 1.56   gama: 57.33   servo position: 612.99
y = mm: 89  x/y = 1.47   gama: 55.81   servo position: 629.91
y = mm: 94  x/y = 1.39   gama: 54.34   servo position: 646.24
y = mm: 99  x/y = 1.32   gama: 52.92   servo position: 661.99
y = mm: 104  x/y = 1.26   gama: 51.55   servo position: 677.18
y = mm: 109  x/y = 1.20   gama: 50.24   servo position: 691.81
y = mm: 114  x/y = 1.15   gama: 48.97   servo position: 705.90
y = mm: 119  x/y = 1.10   gama: 47.75   servo position: 719.47
y = mm: 124  x/y = 1.06   gama: 46.57   servo position: 732.53
y = mm: 129  x/y = 1.02   gama: 45.44   servo position: 745.10```
I know I'm calculating something wrong but I can not understand what.

p.s. If I posted in wrong section please move it in correct.  Reply With Quote

2. ## Re: Hexapod math problem with InverseKinematic

One problem is that your "ground Y" is very small, so all of the effective angle movement happens very near the 0.0 angle. You can't really build a leg that is that close to the rotation axis of the servo -- you'll typically want your Y to be at least 50 millimeters out, and probably more like 100 millimeters out.  Reply With Quote

3. misk0 Relay Join Date
Nov 2017
Posts
5
Rep Power
9

## Re: Hexapod math problem with InverseKinematic

@jwatte: Thanks for your answer. You are right about groundY. While code is correct, my explanation was not. So as you can see I'm changing groundY (not groundX) within for loop from -131 to 131 with step of 5. And angle calculation works well.
Angle calculations seems correct (gama angle).
I have a problem how to transform -45 to -90degrees and 90 to 45degrees as motor movement from 900-1500 and 1500-1900.
Motor 1500 (middle) position should be 90degrees.  Reply With Quote

4. TXBDan Multi-Core Processor  Join Date
Apr 2013
Location
Boston, MA
Posts
260
Images
11
Rep Power
30

## Re: Hexapod math problem with InverseKinematic

Here ya go:
Code:
`float gama = degrees(atan2(groundY, groundX)) + pi()/2;`
You want atan2 so that it operates correctly outside of a quadrant. the +90deg converts it to the format you want, 90deg at center.  Reply With Quote

5. misk0 Relay Join Date
Nov 2017
Posts
5
Rep Power
9

## Re: Hexapod math problem with InverseKinematic

Hey TXBDan that's great. You nailed it! This really solves the problem. Thank you.  Reply With Quote

6. misk0 Relay Join Date
Nov 2017
Posts
5
Rep Power
9

## Re: Hexapod math problem with InverseKinematic

Ok, I made middle left leg to move how I wanted. It is still not perfect, but I need to measure distances and leg parts. I want now to move Front Left leg.
I was woundering what position of the leg should be 0°? For middle left leg I decided it was position perpendicular on left body side. But what about Front Left leg? What would be better for the later calculations when I start to work on Body IK? Should I set green (option 1) or red (option 2) as zero angle?
Because if I set green then I have 0-90 as range. If I set red then I have -45° to 45°.  Reply With Quote

7. TXBDan Multi-Core Processor  Join Date
Apr 2013
Location
Boston, MA
Posts
260
Images
11
Rep Power
30

## Re: Hexapod math problem with InverseKinematic

Look up DH parameters. It's up to you to define the coordinate system for each ridgid part of the robot. If your leg IK does the math in the "leg" coordinate system, then you can relate the leg coord system to the body coord system with a homogeneous transformation matrix! It's not as complicated as it sounds, its simply translating and rotating the axes. In this case the translations are simply the distances in X and Y from body center to leg coxa joint and adding an angular offset for the angular difference between body zero and leg zero.  Reply With Quote

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

arduino uno, hexapod, inverse kinematics, phoenix 