Results 1 to 7 of 7

Thread: Hexapod math problem with InverseKinematic

  1. 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. #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. 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. #4
    Join Date
    Apr 2013
    Location
    Boston, MA
    Posts
    254
    Images
    11
    Rep Power
    22

    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. Re: Hexapod math problem with InverseKinematic

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

  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?
    Click image for larger version. 

Name:	legs angles.jpg 
Views:	22 
Size:	25.6 KB 
ID:	7100

    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. #7
    Join Date
    Apr 2013
    Location
    Boston, MA
    Posts
    254
    Images
    11
    Rep Power
    22

    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)

Similar Threads

  1. Question(s) Gait implementation for hexapod + roll/pitch problem
    By teh_dot in forum Humanoids, Walkers & Crawlers
    Replies: 3
    Last Post: 08-19-2015, 11:23 AM
  2. Overloading MX-64: Am I doing the math wrong?
    By jwatte in forum Humanoids, Walkers & Crawlers
    Replies: 26
    Last Post: 04-11-2013, 10:20 AM
  3. Question(s) Math.h lib with Bioloid CM-530 and C
    By MattD in forum Software and Programming
    Replies: 7
    Last Post: 03-06-2013, 12:15 PM
  4. News/Announcement Math & Science Night at Spring Elementary at La Grange IL
    By sgarciav in forum Robotics General Discussion
    Replies: 1
    Last Post: 03-29-2011, 09:30 PM

Tags for this Thread

Posting Permissions

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