# problem executing some basic pythagoras

I'm writing a basic IK algorithm for a hexapod leg, and I'm getting some wiggy behaviour when I execute the maths

out of the two snippets attached, the first one, with the maths as a composite execution returns weird negative values for the x^2 + y^2 and nan (not a number) for the final value

The second one returns the correct value (181.11)

The x and y values are just for demonstration

Is it necessary then to break up trig functions like this? I've looked for online tips as to which maths functions might work and which ones have to be dealt with in a non composite fashion, but have not found much useful

Also - if it is necessary to decompose the equation - would it be quicker, processing wise, to use int variables for the x^2 and y^2 values then only use float for the final value (being for an IK algorithm, I then have to put it through some trig operations)

I'm using an Arduino Nano

``````float L1;
int x = 132;
int y = 125;

L1 = sqrt(sq(x) + sq(y));
``````
``````float L1;
int x = 132;
int y = 125;

L1 = sq(x);
L1 += sq(y);

L1 = sqrt(L1);
``````

You overflow your int when you do x2 + y2

132 x 132 = 17 424
125 x 125 = 15 625
Total = 33 049

On the Arduino Uno (and other ATMega based boards) an int stores a 16-bit (2-byte) value. This yields a range of -32,768 to 32,767 (minimum value of -215 and a maximum value of (215 - 1).

So 33049 can’t be represented and the bits are the ones of a negative number you see

You can either use long int instead of int or cast everything to long int, or float when you do your math.

The reason it works when you break things Appart is that the value is cast or stored into a float and thus the overflow does not happen.

Aside from the overflow issues,

If you are doing pythagoras and you are just checking for limits, don't bother taking the square root.

if(x * x + y * y > 100)

does the same thing as

if(sqrt(x * x + y * y) > 10)

and it's faster. Also, there's no real reason not to use floats. Never use floats for things that need to be calculated exactly - time, currency, stepper motor steps - but for physical measurement it's quite reasonable to do everything floating point.

Thanks

I need the root, as its part of an Inverse Kinematics Algorithm to derive the angle of each of the three leg servos given the end effector coordinates - so will have to do some more subsequent manipulation with trig functions, then convert them back from radians to degrees.

J-M-L thanks for that, I was assuming once it started working on the maths operations in a float variable it would start treating the int's as floats straight away! So thanks for the useful info there

Do you know - what sort of execution speed difference is there between long int's and floats?

DillonMEK:
Do you know - what sort of execution speed difference is there between long int's and floats?

well - it's easy to go assess. here is some code

``````void setup() {

// using int small enough for no overflow
// declared as volatile otherwise pre-processor will pre compute our values
volatile int x_i = 25;
volatile int y_i = 35;

volatile long x_l = 25;
volatile long y_l = 35;

volatile float x_f = 25;
volatile float y_f = 35;

float result;

unsigned long startTime, endTime;

Serial.begin(115200);

startTime = micros();
result = sqrt(sq(x_i) + sq(y_i));
endTime = micros();
Serial.print("Duration with integer = ");
Serial.print(endTime - startTime);
Serial.print(" microseconds - result = ");
Serial.println(result, 8);

startTime = micros();
result = sqrt(sq(x_l) + sq(y_l));
endTime = micros();
Serial.print("Duration with long    = ");
Serial.print(endTime - startTime);
Serial.print(" microseconds - result = ");
Serial.println(result, 8);

startTime = micros();
result = sqrt(sq(x_f) + sq(y_f));
endTime = micros();
Serial.print("Duration with float   = ");
Serial.print(endTime - startTime);
Serial.print(" microseconds - result = ");
Serial.println(result, 8);
}

void loop() {}
``````

and it the console I get (don't forget to set it to 115200)

``````Duration with [color=blue]integer = 36[/color] microseconds - result = 43.01162719
Duration with long    = 56 microseconds - result = 43.01162719
Duration with [color=red]float   = 64[/color] microseconds - result = 43.01162719
``````

I used volatile to prevent the pre-processor to statically calculate the results of the Maths.
I used integers small enough to not overflow - so this might not be an option for you

as you can see there is a large difference between 2 byte and 4 byte computation - like 75% longer to compute but if you use long to not overflow, then the time difference with float is not as big, ~15% slower.

hope this helps

Your timings might be a little more accurate if you'd put a Serial.flush () after each set of prints.
Don't forget too that's there's a granularity of 4us.

And, of course, you are using floats anyway when you use sqrt(), you even add some extra int->float conversions that would not be there if you just used float to begin with!

DillonMEK:
Thanks

I need the root, as its part of an Inverse Kinematics Algorithm to derive the angle of each of the three leg servos given the end effector coordinates - so will have to do some more subsequent manipulation with trig functions, then convert them back from radians to degrees.

J-M-L thanks for that, I was assuming once it started working on the maths operations in a float variable it would start treating the int's as floats straight away! So thanks for the useful info there

Do you know - what sort of execution speed difference is there between long int's and floats?

This sounds like an expensive set of calculations to perform. How often are you going to be performing these calculations for how many different servo angles? What else is your controller required to do at the same time? Do you have 3 servos per leg, or 3 servos controlling all 6 legs?

I'm not familiar with inverse kinematics, but I do know my way around SOH CAH TOA, Sine Law and Cosine law (the general form of Pythagoras's Theorum). I'm wondering if some of these intermediate calculations could be simplified with a lookup table.

What inputs are being transformed into what outputs? What do your legs look like?

KeithRB:
And, of course, you are using floats anyway when you use sqrt(), you even add some extra int->float conversions that would not be there if you just used float to begin with!

Well you do enventually but late in the Process. The x2+y2 is still done first as int (hence the overflow issue of the OP in the first place)

I'm quite surprised and impressed that float only takes twice as long as int for those calculations. If you had asked me to guess I would have said 10 times longer!

PaulRB:
I'm quite surprised and impressed that float only takes twice as long as int for those calculations. If you had asked me to guess I would have said 10 times longer!

probably because it has to be converted to float for the sqrt() function.

I just noticed that the math.h header in AVRlibc has a hypot() function already. Try that one out and see if it's good enough for your needs.

Jiggy-Ninja:
This sounds like an expensive set of calculations to perform. How often are you going to be performing these calculations for how many different servo angles? What else is your controller required to do at the same time? Do you have 3 servos per leg, or 3 servos controlling all 6 legs?

I'm not familiar with inverse kinematics, but I do know my way around SOH CAH TOA, Sine Law and Cosine law (the general form of Pythagoras's Theorum). I'm wondering if some of these intermediate calculations could be simplified with a lookup table.

What inputs are being transformed into what outputs? What do your legs look like?

With the project I am working on, I'm looking at a reflex driven decentralised control system - each leg has its own controller (arduino nano), which runs the foot position calculations and IK calculations - from the x,y,z coordinates of the foot position, it then calculates and outputs the three servo positions on its leg

there is a central controller that deals with obstacle detection, and sends steering and velocity variables to the legs, and the gait is generated by signals sent between the legs to each other

The IK function looks like this:

``````void IKtrig(int x, int y, int z)
{

float x1 = x;
float y1 = y;
float z1 = z;
L1 = sqrt(sq(x1) + sq(y1));

gama = atan2(x1,y1) * dg;

L = sqrt(sq(L1 - CL) + sq(z1));

beta = acos((sq(TL) + sq(FL) - sq(L)) / ( 2 * TL * FL)) * dg;

alpha1 = acos(z1 / L) * dg;
alpha2 = acos((sq(FL) + sq(L) - sq(TL)) / (2 * FL * L)) * dg;
alpha = alpha1 + alpha2;

ta[ALPHA] = alpha;
ta[BETA] = beta;
ta[GAMA] = gama;
}
``````

and currently seems to take about 1ms to execute

(and is now working as desired!)

on another note - if I'm relying on lookup tables instead of math for many of the calculations, I would have thought I might run into memory issues?

I've calibrated all 18 of the servos in the project, and have the calibration outcomes determine how the angles are mapped on to the servo write command for extra accuracy
I considered just having a look up table for this information, but was worried about available memory, so have the data saved as a comment in my main program, and just the necessary data used on each control chip

Is this making more work for myself unnecessarily?