Global inverse kinematics #66
Replies: 10 comments 1 reply
-
|
I repeated my trials with a small minimal piece of code based on
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: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. |
Beta Was this translation helpful? Give feedback.
-
|
Kudos for sharing some reproducing code 👍 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). |
Beta Was this translation helpful? Give feedback.
-
|
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 |
Beta Was this translation helpful? Give feedback.
-
|
Yes I'm using the Panda description from robot_descriptions.py ( from robot_descriptions.loaders.pinocchio import load_robot_description
robot = load_robot_description("panda_description")
model = robot.model
data = robot.data
Just one will do 😉 |
Beta Was this translation helpful? Give feedback.
-
|
Okay, so here is one example where the IK does not converge: I realized that, when choosing not one but ~2-5 different random |
Beta Was this translation helpful? Give feedback.
-
|
Thank you for preparing this example 👍 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 # 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 / dtThe output looks like this: 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") |
Beta Was this translation helpful? Give feedback.
-
|
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:
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. |
Beta Was this translation helpful? Give feedback.
-
Further analysis of the problemI 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: 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 algorithmTo 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.) To my knowledge the best way to solve global inverse kinematics as of today is sampling-based motion planning, using algorithms like RRT or PRM. RRT is easy to implement and works remarkably well for arms like here, with only a few parameters to tune. Example on a simple pendulumI'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: simple_pendulum_with_configuration_limit.mp4(Left: initial tip configuration. Right: goal tip configuration. The pendulum hits its joint limit in the middle.) 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. |
Beta Was this translation helpful? Give feedback.
-
|
Thanks a lot for the insights @stephane-caron ! It would be interesting to see the "size and shape" of local areas around the local minima that converge towards them respectively. I'll try to find some literature or run experiments on my own for some 6 DOF manipulators. If I can come up with anything interesting, I'll add it to the discussion. (Repost, I initially commented with the wrong Github account) |
Beta Was this translation helpful? Give feedback.
-
|
@stephane-caron thanks for looking into this and for the clarification! I wonder if an approach is possible where a global IK problem can be solved with local IK by divide-and-conquer, i.e. solving for one or more intermediate positions where local IK would suffice. |
Beta Was this translation helpful? Give feedback.
-
|
To my knowledge the best approach to solve global inverse kinematics as of today is sampling-based motion planning, using algorithms like RRT or PRM for multiple queries. RRT is easy to implement and works remarkably well for arms like here, with only a few parameters to tune. Here is for instance a notebook illustrating RRT on a UR5. And RRT can be combined with differential IK by planning in the workspace rather than configuration space (in RRT terminology, differential IK is then used in the "steering method" of RRT.) The dynoRRT library implements RRT and works with Pinocchio (Python bindings on top of a faster C++ implementation). |
Beta Was this translation helpful? Give feedback.
-
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:
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
Beta Was this translation helpful? Give feedback.
All reactions