bio_ik produces weird ik solutions

[fmessmer]

we were testing bio_ik as the kinematics plugin for moveit for a custom manipulator (total of 7 joints, including linear and mimic joints)

we observe the following:

• generating random goal poses
• call compute_ik service (capability)
• on success, call compute_fk with the ik_solution
• compare fk pose of ik_solution against random goal pose passed to ik request

❌ in some cases, fk pose of ik_solution is way off the random goal pose both wrt position (up to 0.5 m!) as well as orientation

to give numbers:
of 100 tests, we get like 65 poses where NO_IK_SOLUTION is reported (which is ok - because fully random poses might not be reachable by our manipulator), and of the 35 poses where SUCCESS is reported by compute_ik, about 7 have this weird ik solution, i.e. fk of ik solution differs significantly from the pose in the ik request

has anybody observed something similar?
is bio_ik able to handle linear joints?
is bio_ik able to handle mimich joints?

any idea how we could debug this any further?
would it make sense to add a "fk verification step" that does the fk of ik solution and compares it to the pose in the request and overwrites the outcome in case the pose diff is significant?

@v4hn @philipp1234 @rhaschke

[philipp1234]

Hi, thanks for reporting.

• Mimic joints and linear/prismatic joints are supported by bio_ik.
• No, it's not a known problem.
• For position and pose goals, bio_ik already has a kind of "fk verification step".
• We are typically using bio_ik via the C++ interface, but of course the ROS service should also produce correct results.
• Are you using the standard compute_ik service provided by the MoveIt move group, or are you using our bio_ik_service node?

Could you send us the necessary files to reproduce the issue (URDF, MoveIt configuration, and your test script)?

[fmessmer]

I added minimal files to produce the issue to our branch:
https://github.com/mojin-robotics/bio_ik/tree/debug_mojin

It contains:

• a "flattened" launch file based on our moveit_config's demo.launch
• a script doing fk/ik on random poses with some statistics and helpers

While you won't be able to visualize the robot as most of our robot_description is hosted in private repos, you should still be able to get an idea of what's going on.

Just run:

• roslaunch bio_ik mrl-a_demo.launch
• rosrun bio_ik verify_kinematics.py

It will run for a while - current ik timeout is quite high - and at the end you will see something like:

[INFO] [1586517315.731949]: total: 100
[INFO] [1586517315.732232]: no_ik_count: 71
[INFO] [1586517315.732580]: fk_diff_count: 5

The fk_diff_count is the number of iterations where the FK of the IK solution is off the goal pose from the IK request.
Those iterations will print something like this:

[INFO] [1586517134.656375]: goal_pose: {(x: -0.613812631947, y: -0.981980898748, z: 0.934112864439), (qx: -0.362425700129, qy: -0.26380325371, qz: -0.714005809883, qw: -0.537820749574) @ map}
[WARN] [1586517134.768498]: IK ErrorCode: SUCCESS
[WARN] [1586517134.774976]: FK ErrorCode: SUCCESS
position.x: 0.0222
position.y: -0.2624
position.z: 0.1889
rotation around x: 0.1908
rotation around y: 0.1782
rotation around z: 0.1978
[ERROR] [1586517134.776400]: pose_diff between goal_pose and fk of ik_solution

Let me know whether you can run/reproduce our files - and whether you need additional info.
Thanks!

[philipp1234]

Hi, I was able the reproduce the issue and am working on a fix. The problem can appear if there are mimic joints as well as revolute joints with a range of 2*pi or more. In your case, as a temporary workaround, you can simply remove or comment out && robot_model->getMimicJointModels().empty() in kinematics_plugin.cpp, line 561. Using that workaround, it should produce correct results for your robot model and your test script runs successfully.

[fmessmer]

Thanks for looking into it...looking forward to the fix...

* For position and pose goals, bio_ik already has a kind of "fk verification step".

But I wonder why this check did not kick in?
It should have rejected the proposed solution and return either FAILURE of NO_IK_SOLUTION rather than SUCCESS...

[fmessmer]

I can confirm that mojin-robotics@3873bc1 prevents bio_ik from returning ik solutions whos FK is off the initial pose in the ik request...

Some more observations:

• even with the "hack", I don't get any ik solutions at all if setting kinematics_solver_timeout to anything less than 0.05...(default for kinematics.yaml with KDL/LMA is 0.005)
• the success rate is not significantly higher than using LMA (~60-70% for 100 random poses)...however, LMA achieves the same rate at kinematics_solver_timeout of 0.005, i.e. much faster...
• further increasing kinematics_solver_timeout also does not increase the success rate significantly
• even with higher kinematics_solver_timeout only the tries that fail eventually use up the full timeout time...successful ik queries return much faster...
• kinematics_solver_search_resolution doesn't seem to have any effect
• kinematics_solver_attempts doesn't seem to have any effect either (is deprecated in melodic anyway)

[philipp1234]

But I wonder why this check did not kick in?
It should have rejected the proposed solution and return either FAILURE of NO_IK_SOLUTION rather than SUCCESS...

If it's a revolute joints with an angular range larger than 2pi and if it's neither a mimic joint nor being mimicked by a different one, the joint limits are first ignored while searching for an IK solution. Afterwards, the result is wrapped to a solution close to the initial pose in line 561 ff. When the wrapping did not happen, enforcePositionBounds in line 615 produced incorrect results.

Some more observations:

The kinematics configuration in the files you sent me (they were merged into a single .launch file, it's usually in kinematics.yaml) included some additional goals (avoid_joint_limits, etc.). Having multiple primary goals can sometimes lead to unintuitive results and our current implementation of secondary goals is admittedly not very fast (wasn't a priority during development, we'll probably release a new IK solver similar to bio_ik without these issues in the future). If you remove all additional goals (and restart ROS to clear the parameter server), the performance should be better. For full control over the goal configuration, you could also try the C++ interface or bio_ik_service (https://github.com/TAMS-Group/bio_ik_service). If you want to do incremental tracking, you could also try the gradient-based solvers that are included in bio_ik (see README.md).

kinematics_solver_search_resolution doesn't seem to have any effect

In bio_ik, the stop criterion can be configured using the parameters drot, dpos and dtwist (default: 1e-5). As far as I know, kinematics_solver_search_resolution was intended for a different purpose.

further increasing kinematics_solver_timeout also does not increase the success rate significantly

That indicates that it's an issue with the goal configuration (kinematics.yaml).

even with higher kinematics_solver_timeout only the tries that fail eventually use up the full timeout time...successful ik queries return much faster...

That's to be expected.

kinematics_solver_attempts doesn't seem to have any effect either (is deprecated in melodic anyway)

Most solvers in bio_ik do their own restarts. kinematics_solver_attempts should be set to 1, setting it to a different value only makes it slower.

[v4hn]

> `kinematics_solver_search_resolution` doesn't seem to have any effect In `bio_ik`, the stop criterion can be configured using the parameters `drot`, `dpos` and `dtwist` (default: `1e-5`). As far as I know, `kinematics_solver_search_resolution` was intended for a different purpose.

The `search_resolution` parameter is not relevant here. It's used to *discretize* one joint range and look for solutions assuming the fixed joint. A rather irrelevant use-case nowadays.

> `kinematics_solver_attempts` doesn't seem to have any effect either (is deprecated in melodic anyway) Most solvers in `bio_ik` do their own restarts. `kinematics_solver_attempts` should be set to 1, setting it to a different value only makes it slower.

This parameter was dropped in MoveIt melodic+, exactly because all solvers do their own internal restarts as they see fit.

[zichunxx]

Hi! How can I set drot and dpos with bio_ik_service?

It seems that there parameters are not available in ik_request.

Thanks!