Someone, please help me with some inverce kinematics

Using Arduino Programming Questions
1

im making a hexopod, and for now i decided to make only one leg, and controll it with coordinates, the measure for distance is milimeters, the measure for angles is degrees, however i have a hard time believing that this

turnToPos(81, 51, 0);

results in theese angles

beta = 61.82
-35
61
32

here is the entirety of the code:

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include<cmath>

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define SERVOMIN  60 
#define SERVOMAX  500
uint8_t servo = 0;
uint8_t servo1 = 1;
uint8_t servo2 = 2;
int legAngle1;
int legAngle;
int legAngle2;

int angle;
int legLength1 = 106;
int legLength2 = 104;
int distance;
double pi = M_PI; 

void setup() {
  Serial.begin(9600);
  pwm.begin();
  pwm.setPWMFreq(50);
}

void turnToAngle(int pin, int angle) {
  float pulse;
  pulse = map(angle, -90, 90, SERVOMIN, SERVOMAX);
  pwm.setPin(pin, pulse);
  Serial.println(angle);
}

void turnToPos(float x, float y, float z){
  float l = sqrt(pow(x, 2) + pow(y, 2)); // length between x and y
  float h = sqrt(pow(l, 2) + pow(z, 2)); // length between l and z
  float beta = (180/pi)*acos((pow(legLength1, 2) + pow(h, 2) - pow(legLength2, 2))/(2 * legLength1 * h));
  Serial.print("beta = ");
  Serial.println(beta);
  legAngle2 = (180/pi)*asin(y/l);
  if(z == 0){
    legAngle1 = beta;
  }else if(z > 0){
    legAngle1 = beta + (180/pi)*asin(z/h);
  }else if(z < 0){
    legAngle1 = (180/pi)*asin(z/h);
  }
  legAngle = 90 - (180 - ((180/pi)*acos((pow(legLength1, 2) + pow(legLength2, 2) - pow(h, 2))/(2 * legLength1 * legLength2))));
  turnToAngle(servo, legAngle);
  turnToAngle(servo1, legAngle1);
  turnToAngle(servo2, legAngle2);
}

void loop() {
  turnToPos(81, 51, 0);
  Serial.println("pos1^");
  delay(1000);
  turnToPos(40, -30, -5);
  Serial.println("pos2^");
  delay(1000);
}

and even tho the leg length isnt exact i doubt that a difference in 2 mm will make that huge of a difference.
Leg angle is for the leg furthest from the body, leg angle 1 is for the middle segment and leg angle 2 is for rotating the leg horisontaly

2

can you provide an illustration identifying what l, x, y, z, beta, legAngle1 and legAngle are?

3

IMG20240714141712
IMG202407141417121920×2560 215 KB

4

the angles are what the leg has during pos1 and the size of the leg on the drawing is almost the same as the actual leg

5

or i guess here is the actual leg

IMG20240714141938
IMG202407141419381920×2560 359 KB

6

but what do your l, x, y, and z represent?

l is a length, not a position. so what is above intended to do?

i help with the local robotics team. we had a workshop where students wrote code to determine the shoulder and elbow angles of a multi-segment (3) arm to position the end of the arm at the x,y position selected by a mouse on a screen. Seems similar to what you're doing

7

well x y and z are just overglorified lengths arent they, i mean if x = 50 that means the length to get to x is 50, l is distance to get to x and y

8

i don't know what you know.

lengths of what ?

i assume x is the horizontal distance between the target position and the vertical axis and similarly y iis the verrical distance between the target and horizontal axis.

but what is z?

what is beta?

yes, l is the distance between the origin and x,y.

but y and l are not perpendicular

9

z is the hight, and beta is just another angle that isnt a legAgngle

10

i can send a photo of my calculations on paper if thats gonna help u understand

11

if z is height, what is y?

it's difficult to understand your question without understanding the symbols?

12

IMG20240714152752
IMG202407141527521920×2560 309 KB

here is the image of my calculations on paper, the two triangles are connected, but i couldnt represent it on paper, just know that l is a shared line between the two triangles

13

well if were going with 3d axes then x is length y is width and z is height
(unless x and y are swapped i dunno)

14

isn't this 2-dimensional.

isn't x, the horizontal distance from the end of the 2nd leg segment (where is attaches to a foot) and the pivot point on the body (i.e. shoulder)?

isn't y the distance from the body to the foot?

15

why would it be 2 dimentional?
i mean for the MOST part yes, but the leg is still supposed to turn up down left and right so no its 3 dimentional

16

well no, the x y and z are the coordinates where the ENTIRE leg is supposed to go

17

or am i thinking of it all wrong?

18

leg
leg777×496 24.6 KB

here's code used to generate the coordinates for the image above tested on a laptop, using xgraph. It assumes a 2 segment arm reaching horizontally to the right and an angle of zero being on the x-axis (not like a compass)

it assumes both segment lengths are the same. it's simpler and a fair enough simplification considering their values of 106 and 104

#include <stdio.h>
#include <math.h>

float ang1;
float ang2;
float beta;

int len1 = 106;
int len2 = 106;

float deg (float x)  { return x * 180 / M_PI; }
float rad (float x)  { return x * M_PI / 180; }

// -----------------------------------------------------------------------------
void pos ()
{
    float x1 =  0 + len1 * cos(rad(ang1));
    float y1 =  0 + len1 * sin(rad(ang1));

    float x2 = x1 + len2 * cos(rad(ang1 + ang2));
    float y2 = y1 + len2 * sin(rad(ang1 + ang2));

    printf ("color=%s\nnext\n", "cyan");
    printf (" %8.4f %8.4f\n", 0,  0);
    printf (" %8.4f %8.4f\n", x1, y1);
    printf (" %8.4f %8.4f\n", x2, y2);
    printf ("anno %8.4f %8.4f (%.1f, %.1f)\n", x2, y2, x2, y2);
}

// -------------------------------------
void turnToPos (float x, float y, float z)
{
    float d = sqrt (pow (x, 2) + pow (y, 2)); // length between x and y

    beta = deg (acos ((d/2) / len1));

    ang1 = beta + deg (acos (x / d));
    ang2 = -2 * beta;

    printf (", d %6.2f",  d);
    printf (", beta %6.2f", beta);
    printf (", ang1 %6.2f", ang1);
    printf (", ang2 %6.2f", ang2);
    printf ("\n");

    pos ();
}

// -----------------------------------------------------------------------------
int main ()
{
    float x, y, z;

    for (x = 50; x < 120; x += 20)  {
        turnToPos (x, y, z);
        y += 15;
    }
    return 0;
}

full output

, d  50.00, beta  76.36, ang1  76.36, ang2 -152.72
color=cyan
next
   0.0000   0.0000
  25.0000 103.0097
  50.0000   0.0000
anno  50.0000   0.0000 (50.0, 0.0)
, d  71.59, beta  70.26, ang1  82.36, ang2 -140.53
color=cyan
next
   0.0000   0.0000
  14.0946 105.0588
  70.0000  15.0000
anno  70.0000  15.0000 (70.0, 15.0)
, d  94.87, beta  63.42, ang1  81.85, ang2 -126.83
color=cyan
next
   0.0000   0.0000
  15.0233 104.9300
  90.0000  30.0000
anno  90.0000  30.0000 (90.0, 30.0)
, d 118.85, beta  55.90, ang1  78.15, ang2 -111.80
color=cyan
next
   0.0000   0.0000
  21.7649 103.7415
 110.0000  45.0000
anno 110.0000  45.0000 (110.0, 45.0)
19

i think this fixes my problem, ty even tho it took me half an hour to desipher your text

20

it took me a lot longer to try to understand what you were trying to do and how you were trying to do.

there's really just 4 lines of geometry code