# Thread: Hexapod math problem with InverseKinematic

1. Relay
Join Date
Nov 2017
Posts
5
Rep Power
6

## 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.
Thanks in advance.

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

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.

3. Relay
Join Date
Nov 2017
Posts
5
Rep Power
6

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

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

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

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

## Re: Hexapod math problem with InverseKinematic

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

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

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

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

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

#### Thread Information

##### Users Browsing this Thread

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

#### Posting Permissions

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