Global inverse kinematics

[JonathanKuelz]

Dear pink team,

first of all, thanks for the library! I was just experimenting with pink for a variety of 6-DoF robots and I was experiencing an unexpected high number of ik procedures not converging to a solution although it existed. It seems to me around 50% of the runs I was performing got stuck in local minima (position/orientation tradeoff).

In pseudocode, my application looks roughly like this:

q = sample_random_q()
r = create_random_6dof_robot()
goal = r.forward_kinematics(q)
r.reset_to_random_position()

ik = pink_ik(r, goal)  # Basically following the example provided in the readme

I compared the results against my own implementation of an ik solver based on the damped least squares inverse of the frame jacobian. While pink is significantly faster when finding a solution, I only find a solution in ~50% of cases compared to ~90% for my own solver. Playing around with the damping factor didn't really seem to improve the results significantly.

I would love to use pink, it seems to be a fast and nice library for solving the ik with pinocchio-based simulations. However, in order to do so, I would need to have a way to significantly improve the ratio of solved ik problems. Do you have any suggestions on possible adaptions that could help? I would be happy to contribute, but I don't know which approaches would be the most promising here as I have rarely worked with the quadratic programming solvers used here.

Cheers, Jonathan

[JonathanKuelz]

I repeated my trials with a small minimal piece of code based on pin and pink only and a 7DOF panda robot. Here's a summary of results for some runs (if I should try with other parameters, let me know and I run the experiment):

Damping: 1e-12 dt: 0.001 Success rate: 54.0% Average velocity when failed: 1.21 Average cartesian distance when failed: 0.2257 Average rotation distance when failed: 0.1301

Damping: 1e-12 dt: 0.006 Success rate: 54.0% Average velocity when failed: 1.19 Average cartesian distance when failed: 0.225 Average rotation distance when failed: 0.1306

Damping: 0.0001 dt: 0.006 Success rate: 54.0% Average velocity when failed: 1.15 Average cartesian distance when failed: 0.225 Average rotation distance when failed: 0.1306

As far as I know, the pin random sampler is deterministic, so the consistent success rate makes me believe the damping and dt parameter actually don't change which of the problems are solved and which ones are not.

Here's the code I am referring to:

``` import numpy as np import pinocchio as pin import pink from tqdm import tqdm package_dir = '...' # local path to Franka Emika Panda meshes urdf_file = '...' # local path to Franka Emika Panda URDF robot = pin.RobotWrapper.BuildFromURDF(urdf_file, package_dir) model = robot.model data = robot.data task = pink.tasks.FrameTask( body='panda_link8', position_cost=1.0, orientation_cost=.75, ) dt = 1e-3 damping = 1e-12 tolerance = 1e-3 num_trials = 200 cart_distance_when_failed = [] rot_distance_when_failed = [] velocities_when_failed = [] success = 0 for _ in tqdm(range(num_trials), total=num_trials): q_sol = pin.randomConfiguration(model) q_init = pin.randomConfiguration(model) pin.forwardKinematics(model, data, q_sol) pin.updateFramePlacements(model, data) goal = data.oMf[model.getFrameId('panda_link8')] task.set_target(goal) configuration = pink.configuration.Configuration(model, data, q_init) for t in np.arange(0, 30, dt): velocity = pink.solve_ik(configuration, (task,), dt, solver='quadprog', damping=damping) configuration.integrate_inplace(velocity, dt) tcp_pose = configuration.get_transform_frame_to_world('panda_link8') error = pin.log(tcp_pose.actInv(goal)).vector if np.linalg.norm(error[:3]) < tolerance and np.linalg.norm(error[3:]) < tolerance: success += 1 break else: # No success velocities_when_failed.append(velocity) cart_distance_when_failed.append(np.linalg.norm(error[:3])) rot_distance_when_failed.append(np.linalg.norm(error[3:])) velocities_when_failed = np.array(velocities_when_failed) print("Damping: ", damping, "dt: ", dt) print(f"Success rate: {100 * success / num_trials:.1f}%") print("Average velocity when failed: ", np.mean(np.linalg.norm(velocities_when_failed, axis=1)).round(2)) print("Average cartesian distance when failed: ", np.mean(cart_distance_when_failed).round(4)) print("Average rotation distance when failed: ", np.mean(rot_distance_when_failed).round(4)) ```

Finally, here's a screenshot for one of the examples that fail: The frame for which the ik should be optimized and the desired goal frame are shown.

[stephane-caron]

Kudos for sharing some reproducing code :+1: I've run your code, but for now it only prints success rate and a few metrics. Could you make it more minimal by narrowing it down to one use case that fails?

This way I would be able to test your issue directly. So far, from the screenshot I only suspect it is due to targets being in a different homotopy class than the one of the initial configuration (in which case differential IK will only follow the gradient until a joint limit is hit; it is a locally rather than globally optimal algorithm).

[JonathanKuelz]

Sure, I can do that - I assume you have a panda URDF locally available so I can use that one for the examples? If so, I can just collect a range of ~10 goals for which the ik will not converge and post them here with some initial guesses on q_init.

[stephane-caron]

Yes I'm using the Panda description from robot_descriptions.py (pip install robot_descriptions). Your code run as is, replacing the model loading part with:

from robot_descriptions.loaders.pinocchio import load_robot_description

robot = load_robot_description("panda_description")
model = robot.model
data = robot.data

If so, I can just collect a range of ~10 goals for which the ik will not converge and post them here with some initial guesses on q_init.

Just one will do :wink:

[JonathanKuelz]

Okay, so here is one example where the IK does not converge:

import numpy as np
import pinocchio as pin
import pink
from tqdm import tqdm

from robot_descriptions.loaders.pinocchio import load_robot_description

robot = load_robot_description("panda_description")
model = robot.model
data = robot.data

task = pink.tasks.FrameTask(
    body='panda_link8',
    position_cost=1.0,
    orientation_cost=.75,
)

dt = 1e-3
damping = 1e-12
tolerance = 1e-3
q_solution = np.array(
    [2.01874195, -0.3871077, 1.67996741, -0.56351698, 2.44279775, 0.68504683, -0.97782112, 0.03072918, 0.01111099])
q_init = np.array(
    [0.32026851, -0.08284433, 0.76474584, -1.96374742, 0.07952368, 3.63553733, 2.46978477, 0.02542847, 0.02869188])

pin.forwardKinematics(model, data, q_solution)
pin.updateFramePlacements(model, data)
goal = data.oMf[model.getFrameId('panda_link8')]
task.set_target(goal)

configuration = pink.configuration.Configuration(model, data, q_init)
for t in np.arange(0, 30, dt):
    velocity = pink.solve_ik(configuration, (task,), dt, solver='quadprog', damping=damping)
    configuration.integrate_inplace(velocity, dt)

    tcp_pose = configuration.get_transform_frame_to_world('panda_link8')
    error = pin.log(tcp_pose.actInv(goal)).vector
    if np.linalg.norm(error[:3]) < tolerance and np.linalg.norm(error[3:]) < tolerance:
        print("Solved ik successfully")
        break
else:
    print("Failed to solve ik")

I realized that, when choosing not one but ~2-5 different random q_init, the success rate reaches 100% at least for the first couple of trials I did, so I guess this would pose a viable solution - however, I can't always choose my initial configuration randomly. Would you have any other recommendations on how a solution could look like?

[stephane-caron]

Thank you for preparing this example :+1: This is interesting and not what I was expecting. I thought configuration limits would be hit and get the robot stuck, but it seems to be velocity limits that are limiting.

I've unwound the call to solve_ik to debug it as follows (full code below):

    # decompose call to solve_ik to check inequality slackness:
    problem = pink.build_ik(configuration, (task,), dt, damping=damping)
    Delta_q = qpsolvers.solve_problem(problem, solver="quadprog").x
    G, h = problem.G, problem.h
    s = G.dot(Delta_q) - h
    nq = model.nq
    print(f"{s.shape=}")
    for lim_index, lim_type in {0: "configuration", 1: "velocity"}.items():
        for side_index, side in {0: "upper", 1: "lower"}.items():
            for i in range(model.nq):
                j = lim_index * (2 * nq) + side_index * nq + i
                print(
                    f"Joint {i}'s {lim_type} {side} bound ({j=}) has slackness {s[j]}"
                )
    velocity = Delta_q / dt

The output looks like this:

Joint 0's configuration upper bound (j=0) has slackness -1.2917792817861966
Joint 1's configuration upper bound (j=1) has slackness -1.3253206793604673
Joint 2's configuration upper bound (j=2) has slackness -1.8547006342206926
Joint 3's configuration upper bound (j=3) has slackness -1.020930660931976
Joint 4's configuration upper bound (j=4) has slackness -2.967099999999999
Joint 5's configuration upper bound (j=5) has slackness -3.190449279254131e-12
Joint 6's configuration upper bound (j=6) has slackness -0.3905780167278878
Joint 7's configuration upper bound (j=7) has slackness -0.007285765
Joint 8's configuration upper bound (j=8) has slackness -0.005654060000000001
Joint 0's configuration lower bound (j=9) has slackness -1.6753207182138032
Joint 1's configuration lower bound (j=10) has slackness -0.5072793206395327
Joint 2's configuration lower bound (j=11) has slackness -1.1123993657793072
Joint 3's configuration lower bound (j=12) has slackness -0.593519339068024
Joint 4's configuration lower bound (j=13) has slackness -8.881784197001252e-16
Joint 5's configuration lower bound (j=14) has slackness -1.9547999999968093
Joint 6's configuration lower bound (j=15) has slackness -2.5765219832721122
Joint 7's configuration lower bound (j=16) has slackness -0.012714235
Joint 8's configuration lower bound (j=17) has slackness -0.01434594
Joint 0's velocity upper bound (j=18) has slackness -0.004360709287048004
Joint 1's velocity upper bound (j=19) has slackness -0.004046770594806363
Joint 2's velocity upper bound (j=20) has slackness -0.0011685538449054044
Joint 3's velocity upper bound (j=21) has slackness -0.004785
Joint 4's velocity upper bound (j=22) has slackness -0.002871
Joint 5's velocity upper bound (j=23) has slackness -0.002871000003748003
Joint 6's velocity upper bound (j=24) has slackness -0.003034090744755572
Joint 7's velocity upper bound (j=25) has slackness -0.0002
Joint 8's velocity upper bound (j=26) has slackness -0.0002
Joint 0's velocity lower bound (j=27) has slackness -0.0004242907129519964
Joint 1's velocity lower bound (j=28) has slackness -0.0007382294051936373
Joint 2's velocity lower bound (j=29) has slackness -0.0036164461550945957
Joint 3's velocity lower bound (j=30) has slackness 0.0
Joint 4's velocity lower bound (j=31) has slackness -0.002871
Joint 5's velocity lower bound (j=32) has slackness -0.0028709999962519966
Joint 6's velocity lower bound (j=33) has slackness -0.0027079092552444277
Joint 7's velocity lower bound (j=34) has slackness -0.0002
Joint 8's velocity lower bound (j=35) has slackness -0.0002

Where we see that the joint velocity lower bound is hit (here joint number 30). Next step would be to look at the conditioning of the Jacobian matrix in the configuration the robot gets stuck at.

Full code:

import time

import numpy as np
import pink
import pinocchio as pin
import qpsolvers
from pink.visualization import start_meshcat_visualizer
from robot_descriptions.loaders.pinocchio import load_robot_description
from tqdm import tqdm

import meshcat_shapes

robot = load_robot_description("panda_description")
model = robot.model
data = robot.data
viz = start_meshcat_visualizer(robot)

task = pink.tasks.FrameTask(
    body="panda_link8",
    position_cost=1.0,
    orientation_cost=0.75,
)

dt = 1e-3
damping = 1e-12
tolerance = 1e-3
q_solution = np.array(
    [
        2.01874195,
        -0.3871077,
        1.67996741,
        -0.56351698,
        2.44279775,
        0.68504683,
        -0.97782112,
        0.03072918,
        0.01111099,
    ]
)
q_init = np.array(
    [
        0.32026851,
        -0.08284433,
        0.76474584,
        -1.96374742,
        0.07952368,
        3.63553733,
        2.46978477,
        0.02542847,
        0.02869188,
    ]
)

pin.forwardKinematics(model, data, q_solution)
pin.updateFramePlacements(model, data)
goal = data.oMf[model.getFrameId("panda_link8")]
task.set_target(goal)

configuration = pink.configuration.Configuration(model, data, q_init)
viz.display(configuration.q)

viewer = viz.viewer
meshcat_shapes.frame(viewer["end_effector_target"], opacity=0.5)
meshcat_shapes.frame(viewer["end_effector"], opacity=1.0)
viewer["end_effector_target"].set_transform(goal.np)

for t in np.arange(0.0, 30.0, dt):
    # decompose call to solve_ik to check inequality slackness:
    problem = pink.build_ik(configuration, (task,), dt, damping=damping)
    Delta_q = qpsolvers.solve_problem(problem, solver="quadprog").x
    G, h = problem.G, problem.h
    s = G.dot(Delta_q) - h
    nq = model.nq
    print(f"{s.shape=}")
    for lim_index, lim_type in {0: "configuration", 1: "velocity"}.items():
        for side_index, side in {0: "upper", 1: "lower"}.items():
            for i in range(model.nq):
                j = lim_index * (2 * nq) + side_index * nq + i
                print(
                    f"Joint {i}'s {lim_type} {side} bound ({j=}) has slackness {s[j]}"
                )
    velocity = Delta_q / dt

    configuration.integrate_inplace(velocity, dt)

    tcp_pose = configuration.get_transform_frame_to_world("panda_link8")
    viewer["end_effector"].set_transform(tcp_pose.np)

    error = pin.log(tcp_pose.actInv(goal)).vector
    if (
        np.linalg.norm(error[:3]) < tolerance
        and np.linalg.norm(error[3:]) < tolerance
    ):
        print("Solved ik successfully")
    viz.display(configuration.q)
    time.sleep(dt)
else:
    print("Failed to solve ik")

[devbanu]

I had a similar inverse kinematics issue with a 6DOF robot: https://github.com/Mecademic/ROS/tree/master/mecademic_description.

I ran Jonathan's code performing 200 inverse kinematics trials, and got very similar results:

Damping: 1e-12 dt: 0.001 Success rate: 54.0% Average velocity when failed: 0.43 Average cartesian distance when failed: 0.119 Average rotation distance when failed: 0.022

It is a curious numerical "coincidence" that I also get exactly 54%.

Looking at the paper "TRAC-IK: An Open-Source Library for Improved Solving of Generic Inverse Kinematics" by Beeson et. al. quadratic programming algorithms (with a good constraint function) should be failing for less than 3% of inverse kinematics tasks, see Table I.

[stephane-caron]

Further analysis of the problem

I looked at this again, pruning inactive inequality constraints and looking at the (configuration-space and workspace) velocity vectors: this example ends up in a loop that looks like this:

== first step ==
Joint 5's configuration upper bound (j=5) is active (slackness: 3.2e-12)
Joint 4's configuration lower bound (j=13) is active (slackness: -8.9e-16)
Joint 3's velocity lower bound (j=30) is active (slackness: 0.0e+00)
np.round(np.dot(J, velocity), 2)=array([-0.43, -0.43, -0.36,  0.14, -0.13,  0.  ])
np.round(velocity, 2)=array([-1.97, -1.65,  1.22, -2.39, -0.  ,  0.  , -0.16,  0.  ,  0.  ])

== second step ==
Joint 5's configuration upper bound (j=5) is active (slackness: 0.0e+00)
Joint 4's configuration lower bound (j=13) is active (slackness: -8.9e-16)
Joint 3's velocity upper bound (j=21) is active (slackness: 0.0e+00)
np.round(np.dot(J, velocity), 2)=array([ 0.43,  0.44,  0.36, -0.13,  0.13,  0.  ])
np.round(velocity, 2)=array([ 1.97,  1.65, -1.22,  2.39,  0.  , -0.  ,  0.16,  0.  ,  0.  ])

== third step : same as first step ==
...

What happens is that joints 4 and 5 are hitting against their configuration limits, and the task gradient takes the system in the direction of these constraints. The closed-loop system has reached an optimum, but it is a constrained optimum where the task cost is not globally minimum.

Differential IK is a local algorithm

To reach the solution, the robot should turn some joints in the opposite direction, but it won't discover this behavior with a local algorithm like differential IK. Differential IK is greedy: it always follows a direction that improves the cost of the task, but sometimes this is short sighted.

The problem you are trying to solve here is global inverse kinematics, that is, always converging to a (minimum-cost) solution of the problem. Global inverse kinematics is a much harder problem: in terms of problem class, global IK can be cast as an MIQP while differential IK is only a QP.)

Example on a simple pendulum

I'm adding an example to the library to illustrate this. Let's say we set up a simple pendulum with joint limits (0, 2 * pi) radians. That is, joint angles have to be positive, but the joint can do a full turn. We then define the goal configuration with an angle of 5.5 rad, and initialize the robot at joint angle 0.5 rad. We task the pendulum with moving its tip to the tip-position of the goal configuration.

Here is how this plays out:

https://github.com/stephane-caron/pink/assets/1189580/77a8be06-6d93-4d3e-88a1-d4164d30cc64

(Left: initial tip configuration. Right: goal tip configuration.)

Because differential IK is a local (think "greedy") algorithm, it will move the tip on the shortest path to target by turning the pendulum clockwise. This will locally improve the task error, but eventually the pendulum will just get stuck at its configuration limit (joint angle = 0 rad). At this stage, differential IK will just keep pushing against the constraint, which is locally the least worst thing to do.

The global solution would be to accept a temporary increase in the "current tip to goal tip" distance, and make the pendulum turn anti-clockwise.

Hoping this helps! Feel free to let me know if you find some facts that contradict my analysis, or if you have further questions on the local behavior of differential IK.