search

PickNikRobotics / pick_ik

Inverse Kinematics solver for MoveIt
BSD 3-Clause "New" or "Revised" License
73 stars 20 forks source link

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

Open mpollayil opened 1 month ago

mpollayil commented 1 month ago

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:

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 commented 4 weeks ago

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 commented 3 weeks ago

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 commented 1 week ago

~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 commented 1 week ago

Also, no issues with continuous joints in pick_ik since https://github.com/PickNikRobotics/pick_ik/pull/59

mpollayil commented 1 week ago

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 commented 1 week ago

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 commented 1 week ago

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?