pick_ik failing consistently on IK when using setPoseTarget, while KDL easily solves

[mpollayil]

Hi all!

Thank you for this nice piece of solver. I was quite excited to see and official bio_ik kinematics solver plugin for MoveIt2!

I was testing out this plugin, but have encountered some issues. Let me give some context first:
I'm using the ROS2 Humble Docker image and using a ros2 node with MoveGroupInterface to plan motions. My robot has 6 DoF and the first joint is continuous. I installed pick_ik through the ros2 Humble binaries. This is one of my attempts for a kinematics.yaml:

manipulator:
  kinematics_solver: pick_ik/PickIkPlugin
  kinematics_solver_timeout: 1.0
  kinematics_solver_attempts: 5
  mode: local
  position_scale: 1.0
  rotation_scale: 0.8
  position_threshold: 0.01
  orientation_threshold: 0.1
  cost_threshold: 0.01
  minimal_displacement_weight: 0.01
  gd_step_size: 0.01
  approximate_solution_cost_threshold: 0.5

I encountered the following:

• While planning to a joint value (using setJointValueTarget), everything works fine. This is reasonable since no IK should be performed to plan here.
• When planning to a reasonable (in the reachable workspace of the robot) Cartesian pose using setPoseTarget, the planning always fails with the following:

[move_group-3] [INFO] [1729066005.990528959] [moveit_move_group_default_capabilities.move_action_capability]: Received request
[move_group-3] [INFO] [1729066005.990929340] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[move_group-3] [INFO] [1729066005.994929103] [moveit_move_group_default_capabilities.move_action_capability]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[move_group-3] [INFO] [1729066005.996810194] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[move_group-3] [ERROR] [1729066011.002837195] [ompl]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:252 - manipulator/manipulator: Unable to sample any valid states for goal tree
[move_group-3] [WARN] [1729066011.002906882] [moveit.ompl_planning.model_based_planning_context]: Timed out
[move_group-3] [INFO] [1729066012.019552019] [moveit.ompl_planning.model_based_planning_context]: Unable to solve the planning problem
[move_group-3] [INFO] [1729066012.019594309] [moveit_move_group_default_capabilities.move_action_capability]: Timeout reached

• If I plan towards the same exact Cartesian pose using the default KDL solver with the same code (setPoseTarget), the plan is found in no time.
• If I use computeCartesianPath toward the same Cartesian pose using pick_ik, the planning is 100% successful.

I assume there are no issues with my robot model since KDL is successful all the time. Is there anything wrong with my kinematics.yaml? Can there be any issues with the joint limits or continuous joints? I assume the problem is here with the inverse kinematics.

I would appreciate any hints or help. Thanks in advance.

[sea-bass]

The local solve mode may not be good enough for far-away IK solves, and KDL gets around this by doing random restarts on failures.

For Cartesian planning, a string of close-by problems are solved, which may similarly be OK for the global solver.

If you switch the mode to global, how do your results change?

[mpollayil]

Hello,

Thank you for you reply.

The local solve mode may not be good enough for far-away IK solves, and KDL gets around this by doing random restarts on failures.

For Cartesian planning, a string of close-by problems are solved, which may similarly be OK for the global solver.

Ok, this makes sense to me.

If you switch the mode to global, how do your results change?

I tried also switching to mode: global. However, planning to a Cartesian pose using setPoseTarget, the planning still fails with the same:

[move_group-3] [INFO] [1730796132.861646777] [moveit_move_group_default_capabilities.move_action_capability]: Received request
[move_group-3] [INFO] [1730796132.861868670] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[move_group-3] [INFO] [1730796132.868218879] [moveit_move_group_default_capabilities.move_action_capability]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[move_group-3] [INFO] [1730796132.870564202] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[move_group-3] [ERROR] [1730796137.876447528] [ompl]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:252 - manipulator/manipulator: Unable to sample any valid states for goal tree
[move_group-3] [WARN] [1730796137.876536928] [moveit.ompl_planning.model_based_planning_context]: Timed out
[move_group-3] [INFO] [1730796139.393427605] [moveit.ompl_planning.model_based_planning_context]: Unable to solve the planning problem
[move_group-3] [INFO] [1730796139.393502574] [moveit_move_group_default_capabilities.move_action_capability]: Timeout reached

Is there anything wrong with my kinematics parameters? Are there any issues of pick_ik with the joint limits or continuous joints?

[sea-bass]

That seems to me like an IK solution was reached, but then the OMPL motion planner to get to that joint configuration failed due to some setup issue.

Never mind, misinterpreted that

[sea-bass]

Also, no issues with continuous joints in pick_ik since #59

[mpollayil]

That seems to me like an IK solution was reached, but then the OMPL motion planner to get to that joint configuration failed due to some setup issue.

I'm not quite sure I understood this. Why should this be an OMPL problem when it works by simply changing the kinematics plugin to KDL?

Maybe I'm getting it wrong, but it is my understanding that Unable to sample any valid states for goal tree means no valid IK was found for the specified Cartesian pose.

Thank you for you patience and support.

[sea-bass]

Hmm, yes, you're right. Sorry about that.

I wouldn't know what else to recommend besides debugging the solver to see why it fails for that far-away solve. I also don't know what your parameters are for the global solver, as that has its own variables not listed in the original post.

[sea-bass]

Looking at your parameters and the other defaults, I may suggest:

It seems your gd_step_size is very large. Could you set this back to 0.0001 or so?

What if you increase memetic_num_threads from 1 to like 4? Trying multiple IK solves ik parallel this way.

Also, what happens if you reduce the rotation_scale closer to its default of 0.5?