Revision history [back]

There may be an easier way to solve your underlying problem than implementing a custom IK solver.

The robot you're using moves its elbow joint using a parallel-bar linkage, which is driven by a servo attached to the same part of the robot as the servo for the shoulder lift joint. This means that the rotation of the elbow joint is equal to the difference between the rotation of the elbow servo and the shoulder lift servo. The same relationship also exists to calculate the rotational velocity and acceleration of the elbow joint.

You can perform this calculation in the part of your code that converts the robot joint trajectory from motion planning into servo commands for motion execution, which lets you design your robot model to represent the arm as a much simpler in-series manipulator, as if the elbow drive servo was actually located at the elbow, instead of a parallel manipulator. This strategy is usually how MoveIt motion planning is done for industrial robots that include these kinds of parallel links, such as the ABB IRB 8700.

Here's a picture of the robot you linked, for clarity: