Evaluating the similarity of two 7 Degree of Freedom Arms

Question

I am working on the Baxter robot where I have a first arm configuration and a bunch of other arm configurations, where I want to find the closest arm configuration to the first among the many other arm configurations. The trick here is that the end effector location/orientation is the exact same for all the arm configurations, they are just different ik solutions. Can anyone point me towards the right direction towards this? Thank you.

Answer 1

To compare two different arm configurations, Euclidean distance in joint space is usually sufficient.

$$ d = \sqrt{\sum_{i=0}^n(q_i - q_{i_{ref}})^2} $$

Where $q_i$ is joint $i$ of the test configuration, and $q_{i_{ref}}$ is the same joint in the reference configuration. This will work even if the end-effector has a different pose. But its usefulness goes down as the difference in end-effector poses grows. I believe you can combat this effect somewhat by using a weighted distance metric.

Note that in general, you might want to have some global notion of the quality of an IK solution when you don't have a reference configuration. The rest of this post deals with this scenario.

There are a few metrics for comparing IK solutions. The first, that I have found to be most useful is manipulability measure. Which is defined as:

$$ \mathit{w}(q) = \sqrt{det(\mathbf{J}(q)\mathbf{J}^T(q)} $$

This is a rough measure of how easy it is for the arm to move in any direction. It vanishes as singular configurations. In the image below, it is a measure of the roundness of the manipulability ellipsoid.

Interesting side note: an arm's (velocity) manipulability is orthogonal to it's force manipulability. Which kind of makes intuitive sense. Where your arm can move quickly, it can't lift very much, and vice versa.

Distance from mechanical joint stops is another common metric:

$$ \mathit{w}(q) = -\frac{1}{2n}\sum_{i=1}^{n}\Big(\frac{q_i - \bar{q}_i}{q_{iM} - q_{im}}\Big)^2 $$

Where $q_{iM}$ and $q_{im}$ are the maximum and minimum joint angles, and $\bar{q}_i$ is the middle value of the joint range. Note that because this is just a unitless metric, you can tweak to suit your needs. For example, using a higher exponent to have a flatter curve in the middle, and sharper penalties near the edges.

I have also seen something called the condition number used. Which is the ratio of the first and last eigen values of the SVD of the Jacobian. Which is kind of another measure of the roundness of the arm's Jacobian. But I have found this metric to be less useful.

If you are only interested in forces or velocities in a specific direction $u$, you can compute the manipulability ellipsoid like so:

$$ \alpha(q) = \Big(u^T\mathbf{J}(q)\mathbf{J}^T(q)u\Big)^{-1/2} $$ $$ \beta(q) = \Big(u^T\big(\mathbf{J}(q)\mathbf{J}^T(q)\big)^{-1}u\Big)^{-1/2} $$

Where $\alpha$ is the force component, and $\beta$ is the velocity component.

You could also calculate the theoretical current draw for each arm configuration to find the most power efficient one.

Or you can make your own metric and combine some of these.

Note that you can use these approaches while treating IKFast as a black box. IKFast gives you a number of potential solutions for the given end-effector pose. You can simply iterate through them, evaluating the desired metric on each, then pick the best. (Side note: IKFast works slightly differently in Python and C. In Python, OpenRave automatically iterates your fixed joints through their entire range with some discretization. So you can potentially get hundreds of solutions. But in C, it only gives you the few permutations of joint flips. So you only get a handful of solutions. You are forced to call it multiple times to iterate through your fixed joint range.)

Answer 2

When you have a 6 DOF manipulator, you must choose a "pose" such as elbow-up or elbow-down, etc. Combining the waist, elbow, and wrist options leads to eight different configurations to choose from - sixteen if the end effector can be flipped 180 degrees and still function. Inverse kinematics must have specific trigonometric solutions that will select a pose and stick with it so that the manipulator does not try to flip among the many possible solutions. These pose choices are often hard-coded into the inverse kinematics, in my experience.

With a redundant manipulator, the challenge is much greater. There is now an infinite number of possible solutions, each with the eight or sixteen distinct poses. NASA, Oak Ridge, and others who were controlling redundant telerobotic manipulators published a lot of work in this area in the 1990s. My favorite approach is to define an optimization criteria, and use the gradient of the null space Jacobian to optimize that criteria while maintaining the end effector inverse kinematic solution. Prof. Williams explains the procedure well, for an 8 DOF arm (but the approach applies to any redundant manipulator) here: http://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19940023270.pdf

EDIT (to provide specific example):

For instance, you could set your optimization criteria to be to keep the joint angles as close to the current pose as possible (minimize the norm of the least-squares vector $||\vec x_{i+1} - \vec x_{i}||$ ). The algorithm will try to minimize this norm, thus keeping the "average" joint change as small as possible when going to the next commanded position.