Comments (8)
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.1301Damping: 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.1306Damping: 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.
from pink.
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).
from pink.
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
.
from pink.
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 😉
from pink.
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?
from pink.
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 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")
from pink.
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.
from pink.
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:
simple_pendulum_with_configuration_limit.mp4
(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.
from pink.
Related Issues (20)
- Update to the Pinocchio 3 API HOT 3
- Given a trajectory for arm , the generated speed fluctuates greatly, how to increase the smoothness of the speed generated by pink? could you please give an example to produce smooth velocity
- Display a TF tree using pinocchio model HOT 1
- Build sparse matrices for sparse solvers
- Joint limits for planar joints
- Velocity limits without configuration limits
- Adding holonomic constraints HOT 7
- Fix sign of error and right-minus in body task
- Lie algebra formulation for linear holonomic task HOT 1
- Configuration is defined only for FrameType::Body, not for other enum values. HOT 1
- Numerical stability of Draco3 example HOT 1
- CoM task HOT 2
- Package name on PyPI
- Attribute Error : 'FrameTask' object has no attribute 'frame' HOT 2
- Proposal for Integration of Control Barrier Functions (CBFs) into pink HOT 11
- How to apply velocity smooth constraint and weight HOT 1
- Slowdown when the end effector approaches the target HOT 1
- how can we install pink by source code HOT 1
- Velocity vibrations in Go2 position barrier example HOT 1
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.
from pink.