Getting preferred (elbow-up/down) IK solution in PyKDL

Question

I am using PyKDL to compute IK solutions for the UR3e arm. However, as we know, multiple solutions are available for IK of the 6-DoF arm, and I need to identify the preferred one. Industrial arms have a variable (configuration) to specify these solutions. But here I am looking for ROS-way. The selected solution will be used to define the arm's trajectory in the future. Please see below to find some cases:

Case 1:

Case 2:

The code snippet is similar to the one employed by Baxter SDK here.

Answer 1

I had a quick look at the KDL CartToJnt() method. I looked at the c++ code as I am less familiar with python, but the implementation should be identical:

The ChainIkSolverPos is the KDL interface class for inverse position kinematics. There seem to be three implementations:

• ChainIkSolverPos_NR,
• ChainIkSolverPos_NR_JL,
• ChainIkSolverPos_LMA.

The Baxter SDK uses the ChainIkSolverPos_NR: see here.

This is an inverse position kinematics algorithm based on 'Newton-Raphson iterations':

• It starts from an initial guess for the joint positions,
• It calculates the forward position kinematics for that guess,
• It calculates the displacement twist (i.e. the diff between the effector frame corresponding for the 'guess' and the specified end effector frame),
• It then uses a forward velocity solver to calculate joint displacements that realize the displacement twist (for the solver used in the Baxter SDK, see here),
• Finally these joint displacements are added to the guess values, which should move the 'guess joint values' towards the 'desired joint values', and
• These steps are iterated untill the solution converges, see here.

I did not check the other two algorithms in detail, but the NR_JL one is 'Newton Raphson with Joint Limits', so should be more or less identical, and the LMA one also iterates based on an initial guess.

So it is clear that these algorithms will converge to one or another configuration based on the 'initial guess' values, i.e. the seed values for the algorithm.

There is no "one fits all" solution to choosing good seed values, this is highly application dependent. E.g.:

• If, for your application, you know that the robot will always start from a known configuration, than an obvious choice for the seed values would be the joint positions of that configuration,
• If you know that there are a few 'main configurations' in your task, then you could define seed values for each of these main configurations, and choose a set based on which main configuration your target is closest to (this needs a 'closest' metric though, which is not nessesarily straightforward if it comprises both position and orientation),
• If your goal is to move from one endeffector pose to another, you could calculate intermediate poses along that trajectory, and calculate for each intermediate pose the joint positions, using the previous joint positions as seed value (though this could equally well lead to undesired configurations, depending on which target poses you require in your application),
• You could calculate multiple times, with random seed values, and compare the results (but then you need a metric to descriminate a 'desired robot configuration' from an 'undesirable configuration'),
• Etc.

EDIT:

It is important to make a distinction between:

• A general kinematics framework, such as KDL, that is intended to work for any kind of robot manipulator,

  vs

• A custom, robot-specific implementation, that is optimized for, but only works for just one kind of robot.

E.g. a KDL::Chain can be defined for a 4 DOF palletizing robot, a 5 DOF scara robot and a 6 DOF manipulator, and for each of those chains KDL can provide forward and inverse kinematics.

But: if your task will always be executed by the same type of robot (e.g. a UR arm), than you can typically derive an analytical solution for the forward and inverse kinematics and implement that instead. It will be more efficient (e.g. not iterative), and can be tailored to your needs.

Industrial controllers typically use robot-specific implementations and not general-purpose, iterative solutions.

There also exist general solutions for analytical inverse kinematics, e.g. OpenRave, but I have no experience with those.

If you are looking for specific IK implementations for UR robots, then this google search yields some interesting results, e.g.:

• ur_kinematics
• COMPAS FAB - Analytical kinematics

And the best reference is probably the official Universal Robots repository which mentions MoveIt support.