First of, hats off to Mr. Skyentific for sharing this code with us. I took a deep dive in it for the IK math implementation (seems too hard for me to implement from scratch), and I ran into several issues related to IK calculations:
(1) InverseK() function works good, but ForwardK had a math error:
Xfk[3]=Xfk[3]/PI*180.0; Xfk[4]=Xfk[4]/PI*180.0; Xfk[5]=Xfk[5]/PI*180.0;
The above lines are not interpreted correctly, as Arduino honors the multiplication first and division second in this case. The correct version for Arduino looks like:
Xfk[3] = Xfk[3] * 180.0 / PI; Xfk[4] = Xfk[4] * 180.0 / PI; Xfk[5] = Xfk[5] * 180.0 / PI;
(2) The dreaded singularity (and maybe division by zero issues):
I wrote some additions to the code that run IK calculations, take the result and run FK on them, then return the error between initial target and round-trip calculated values. So far I am getting a stable round-trip error of less than 0.001 when feeding IK otput directly to FK input - which proves the math is implemented correctly.
For the cartesian X, Y and Z axes everything is very stable.
However, or the ZYZ Euler angles I was getting weird numbers when input values are equal to 0, 90, -90, 180, -180 or anywhere beyond +- 180 degrees - this causes singularity which was not accounted for in original code, and it's strange it works so well in the videos given the heavy use of +-90 / 180 degree angles as example inputs.
Singularity looks quite detectable by calculating error. Normally at 89.9 degrees input max error is below 0.001 degree, however at 89.99 degrees the calculated error rises to 0.02, and at 90.0 the output goes haywire to random values like +-30 degrees.
They say, dealing with singularity is one of the more complex robotics problems. I see super simple ways to deal with it on a hobby scale:
1 (mandatory) - limit the possible input values strictly between -180 and +180 degrees and convert any values beyond those to within limits
2 (mandatory) - take into account axes travel limits and reject moves that would lead to out of bounds
3 (dirty) - add or substract a little value like 0.01 from a 0.00 / 90.00 / 180.00 degree input just to avoid singularity
4 (clean)- run an FK calculation, detect high calculated error and reject command before executing.
For a desktop robot - I agree skipping all this is totally fine. However, for upscaling (which I see the potential for in this project) to anything bigger needs a serious approach to math in such a complex subject as IK.
First of, hats off to Mr. Skyentific for sharing this code with us. I took a deep dive in it for the IK math implementation (seems too hard for me to implement from scratch), and I ran into several issues related to IK calculations:
(1) InverseK() function works good, but ForwardK had a math error:
Xfk[3]=Xfk[3]/PI*180.0; Xfk[4]=Xfk[4]/PI*180.0; Xfk[5]=Xfk[5]/PI*180.0;The above lines are not interpreted correctly, as Arduino honors the multiplication first and division second in this case. The correct version for Arduino looks like:Xfk[3] = Xfk[3] * 180.0 / PI; Xfk[4] = Xfk[4] * 180.0 / PI; Xfk[5] = Xfk[5] * 180.0 / PI;(2) The dreaded singularity (and maybe division by zero issues): I wrote some additions to the code that run IK calculations, take the result and run FK on them, then return the error between initial target and round-trip calculated values. So far I am getting a stable round-trip error of less than 0.001 when feeding IK otput directly to FK input - which proves the math is implemented correctly.
For the cartesian X, Y and Z axes everything is very stable. However, or the ZYZ Euler angles I was getting weird numbers when input values are equal to 0, 90, -90, 180, -180 or anywhere beyond +- 180 degrees - this causes singularity which was not accounted for in original code, and it's strange it works so well in the videos given the heavy use of +-90 / 180 degree angles as example inputs.
Singularity looks quite detectable by calculating error. Normally at 89.9 degrees input max error is below 0.001 degree, however at 89.99 degrees the calculated error rises to 0.02, and at 90.0 the output goes haywire to random values like +-30 degrees.
They say, dealing with singularity is one of the more complex robotics problems. I see super simple ways to deal with it on a hobby scale: 1 (mandatory) - limit the possible input values strictly between -180 and +180 degrees and convert any values beyond those to within limits 2 (mandatory) - take into account axes travel limits and reject moves that would lead to out of bounds 3 (dirty) - add or substract a little value like 0.01 from a 0.00 / 90.00 / 180.00 degree input just to avoid singularity 4 (clean)- run an FK calculation, detect high calculated error and reject command before executing.
For a desktop robot - I agree skipping all this is totally fine. However, for upscaling (which I see the potential for in this project) to anything bigger needs a serious approach to math in such a complex subject as IK.