English is currently available.
Robotics Engineering DISCUSSION
I have a planar arm with two servo joints, link lengths 100 mm and 80 mm, and I want to place the tip at a given (x, y). My inverse kinematics code uses the law of cosines. It works for most points, but it gives a pose that looks mirrored from what I expected, and for targets near the edge of the workspace it returns NaN.
What is the correct set of equations, how do I choose between the two solutions, and how should the code deal with points it cannot reach?
With L1 and L2 the link lengths and r² = x² + y², the elbow angle comes from the law of cosines: cos θ2 = (r² - L1² - L2²) / (2 × L1 × L2). Then θ1 = atan2(y, x) - atan2(L2 × sin θ2, L1 + L2 × cos θ2). Use atan2 everywhere, not atan, so the quadrant is right.
Example for (120, 60) mm: r² = 18000, cos θ2 = (18000 - 10000 - 6400) / 16000 = 0.1, so θ2 = 84.3°. Then θ1 = 26.6° - atan2(79.6, 108) = 26.6° - 36.4° = -9.8°.
The mirrored pose is the second valid solution. acos returns only the positive angle, but -θ2 satisfies the same equation: the elbow can sit on either side of the line from the base to the target. For the example above, θ2 = -84.3° gives θ1 = 26.6° + 36.4° = 63.0°, and the tip is at the same point.
Pick one by rule: whichever keeps both joints inside their limits, avoids obstacles such as the table, or is closest to the current joint angles so the arm does not flip from one configuration to the other in the middle of a path.
NaN comes from acos being given a value outside -1 to +1. That happens when the target is farther than L1 + L2 = 180 mm from the base or closer than L1 - L2 = 20 mm, and also right at the boundary, where rounding produces something like 1.0000001.
Test reach before solving and decide what unreachable should mean: reject the point, or clamp the cosine to the range -1 to +1, which drives the arm to the nearest point on the boundary in the direction of the target.
Verify with forward kinematics every time during development: x = L1 × cos θ1 + L2 × cos(θ1 + θ2), y = L1 × sin θ1 + L2 × sin(θ1 + θ2). Plugging in -9.8° and 84.3° gives 98.5 + 21.5 = 120 mm and -17.1 + 77.1 = 60 mm, so the solution is right.
Then check the mapping to the servos. The equations assume θ2 is measured relative to the first link and both angles are counter-clockwise positive. A servo mounted the other way round, or one whose zero is not along the link, gives exactly the kind of mirrored result you describe.
One more thing to expect near full stretch: even when the maths is valid, the arm behaves badly there. At full extension the two links are in line, which is a singularity. A small move of the tip along the radial direction needs a very large change in joint angle, so motion becomes jerky and servo resolution errors are magnified.
Plan the workspace so that normal targets stay inside roughly 90 to 95 percent of maximum reach.
Back to Robotics Engineering community
