0

I am trying to implement inverse kineamtics on an arduino controlled quadruped, but I got some inaccurate calculation results. To check what's wrong I ported the algorithm to C++ and ran it on PC. My problem is that I would assume the coordinate changes would give the same results, no matter which direction I'm altering the coordinates. But transitions on x and y axes give a few degrees of difference, depending on the direction. My math knowledge is a bit dusty, so maybe I missed something while checking the algorithm.The legs are configured like this http://te.unib.ac.id/lecturer/indraagustian/wp-content/uploads/2014/05/Inverse-Kinematics.jpg

What could cause the difference in the results?

#include <cmath>
#include <iostream>

#define CONNECTED_SERVOS 12
#define PI 3.1415

#define coxa 55
#define femur 40
#define tibia 47

int nogo_buffer[CONNECTED_SERVOS];
int end_position[CONNECTED_SERVOS];
int cur_pw[CONNECTED_SERVOS];

int calibrate[] = { 1500, 1520, 1500, 1560, 1400, 1410, 1520, 1550, 1500, 1560, 1510, 1620 }; //calibrate servos to simmetrical positions

int floatToInt(double num) {
    //Rounding a number avoiding truncation:

    return (int)(num < 0 ? (num - 0.5) : (num + 0.5));
}

int radToMicro(double rad, int ref) {

    //Make sure rad isn't negative:


    if (rad < 0) {
        while (rad < -PI) {
            rad += PI;
        }
    }
    else {
        while (rad > PI) {
            rad -= PI;
        }
    }

    //edit 2400
    return ref - floatToInt(572.958 * rad);
}



void IkLeg(int x, int y, int z, int nLeg)
{

    double L1 = sqrt(pow(x,2) + pow(z,2));
    double L = sqrt(pow(L1 - coxa,2) + pow(y,2));
    double t = acos((pow(tibia,2) + pow(femur,2) - pow(L,2)) / (2 * tibia * femur)) / PI * 180;
    double f1 = acos(y / L) / PI * 180;
    double f2 = acos((pow(femur,2) + pow(L,2) - pow(tibia,2)) / (2 * femur * L)) / PI * 180;
    double f = f1 + f2;
    double c = atan2(z, x) / PI * 180;
    std::cout << c << std::endl;
    std::cout << f-90 << std::endl;
    std::cout << t-90 << std::endl;

    //coxa angle
    nogo_buffer[3 * nLeg - 1] = radToMicro(c/180*PI, calibrate[3 * nLeg - 1]);

    std::cout << nogo_buffer[3 * nLeg - 1] << std::endl;

    //femur angle
    nogo_buffer[3 * nLeg - 2] = radToMicro(f / 180 * PI, calibrate[3 * nLeg - 2]+900);

    std::cout << nogo_buffer[3 * nLeg - 2] << std::endl;

    //tibia angle
    nogo_buffer[3 * nLeg - 3] = radToMicro(t / 180 * PI, calibrate[3 * nLeg - 3]+900);

    std::cout << nogo_buffer[3 * nLeg - 3] << std::endl;
}

int main() {
    IkLeg(95,47,0,1);
    std::cout << "x transition" << std::endl;
    IkLeg(105, 47, 0, 1);
    IkLeg(85, 47, 0, 1);
    std::cout << "y transition" << std::endl;
    IkLeg(95, 57, 0, 1);
    IkLeg(95, 37, 0, 1);
    std::cout << "z transition" << std::endl;
    IkLeg(95, 47, 30, 1);
    IkLeg(95, 47, -30, 1);

    return 0;
}
p0lt3rg3ist
  • 1
  • Can you share access to the original arduino code? I don't exactly know how the arduino is configured, but I'm wondering if it is a precision issue due to the size of the floating point (the double is usually 64 bits, while the float is usually 32 bits -- I suspect the arduino will only have support for the latter, while you are clearly using the former here). It typically doesn't matter, so it might not be your issue, but if you're seeing drift after a long time of integration, that may be the problem. If you can't share the arduino code, can you give an example of the outputs from both? – neocpp Oct 07 '15 at 13:50
  • The only difference in the code running on the arduino is, as you guessed it, the usage of float instead of double. That's one of the reasons I ported it to C++, the results are exactly the same, the servos I'm using support .1 degree (theoretical) precision. The drifting is present in small movements as well and is amplified by bigger changes in coordinates. Here is the output of the code I posted above http://pastebin.com/00PYWPeV – p0lt3rg3ist Oct 08 '15 at 16:07
  • Ah okay, I have misunderstood your question; I thought you were getting differences between your Arduino code and this C++ code. I guess I am confused about what the expected output is; do you think you can edit the question to include the expected output vs what you have in the pastebin? – neocpp Oct 08 '15 at 23:37

0 Answers0