Software > Software

2DOF inverse kinematics


I have been working on building a 2DOF robot arm and I am having some trouble getting the inverse kinematics algorithms to work correctly. The problem i'm having is that the algorithm on

is yielding NaN from the

s2 = sqrt(1 - c2^2);

This is caused by 1 - c2^2 being a negative, which Processing will not allow. My c2 variable is around 832673, the x and y are between 0-800 and L1 and L2 are set at 300. My question is where is the 1 coming from and why 1?

It's not just Processing that doesn't allow square roots of negative numbers; it's the entire family of real numbers that don't allow it :-)
You are not deriving the c2 variable correctly It should be normalized to the 0..1 range, because it's divided by double the product of L1 and L2.
Have you properly implemented this line?

--- Code: --- (x^2 + y^2 - L1^2 - L2^2) / (2 * L1 * L2)
--- End code ---
Also, note that the "^" operator in C means "bit or" whereas in this code, it's intended to indicate squaring (so the actual C code would say x*x, not x^2).

If you look at what the values mean, "x*x + y*y" is just "square of distance to x,y coordinate" which must be less than the length of the two links, or the arm won't reach. Thus, it is possible that the numbers you're trying to put in are simply impossible to satisfy.
Another option for what could be going wrong is the frame of reference -- is the origin (x=0, y=0) at the center of the J1 joint, or somewhere else? (The tutorial doesn't mention this, but it looks like it assumes the x,y coordinates of the J1 joint are 0,0)

L1 and L2 are the lengths of each are segment.
x and y are the 2D position at the end of the arm.
The distance to x,y can not be greater than the total length of the arm.
Like this:
L1 + L2 >= sqrt( x^2 + y^2)
else the robot arm can not reach the given x.y position.

If that statement is false then the value of c2 will be greater than 1.0 and thus give a negative value under the radical.

Try your value in a spreadsheet (like Excel) to see all the calculation and to easily adjust values.

So your value for x and/or y are too large.
If L1 = L2 = 300, then
600 >= sqrt(x^2 + y^2)

if x and y = 800 then
L1 + L2 must be >= 1131.371

Thanks a lot for the help, I did exactly what you thought I did jwatte. I left out a parentheses, leading to incorrect value assigned  to the variable c2. This made the number to be sqrt() negative. Silly mistake :-)

~Thanks Idan


[0] Message Index

Go to full version