search

moveit / moveit

:robot: The MoveIt motion planning framework
http://moveit.ros.org
BSD 3-Clause "New" or "Revised" License
1.64k stars 943 forks source link

[MoveGroupInterface] Inconsistent usage of end-effector links between setJointValueTarget, setPoseTarget, and computeCartesianPath #577

Open hartmanndennis opened 7 years ago

hartmanndennis commented 7 years ago

Description

I use IKfast computed from base_link to tool0. The move group is defined in my SRDF like this <chain base_link="base_link" tip_link="tool0" />. If I use setEndEffectorLink of MoveGroupInterface to my end-effector link(different from tool0), setJointValueTarget, setPoseTarget, and computeCartesianPath(all called with a Pose) behave inconsistently.

Your environment

Steps to reproduce

Use IKFast with kinematic chain to tool0. Set end-effector by calling setEndEffectorLink to something different than tool0. Call setJointValueTarget, setPoseTarget, and computeCartesianPath with pose goals.

Expected behaviour

The end-effector set by setEndEffectorLink should end up at the specified pose.

Actual behaviour

setJointValueTarget works as expected. setPoseTarget, and computeCartesianPath ignore the provided end-effector and instead use tool0.

setJointValueTarget uses RobotState's setFromIK to get the IK and provides the end-effector link as "tip_in" and transforms the pose to solver_tip_frame. This is expected behaviour.

computeCartesianPath calls the MoveGroupCartesianPathService but does not set the field link_name of the request. The service then sets link_name link_name = jmg->getLinkModelNames().back(); at https://github.com/ros-planning/moveit/blob/kinetic-devel/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp#L86 to the last link of the kinematic chain, i.e. tool0. It then uses setFromIK with the wrong link as "tip_in". If you add req.link_name = getEndEffectorLink(); at https://github.com/ros-planning/moveit/blob/kinetic-devel/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp#L944 the end-effector link will be correctly used. A change to this could break user code, if they use a fix like suggested in https://groups.google.com/forum/#!topic/moveit-users/VB7VWsgV9-g and set their end-effector to something different than their IK tip frame.

setPoseTarget sets position and orientation goal constraints and correctly adds the end-effector link name to these constraits. I couldn't figure out where the IK is computed when using position and orientation constraints, so I have no idea why this doesn't work as expected.

v4hn commented 7 years ago

Ouch. Thanks for reporting and for debugging it this far!

If you add req.link_name = getEndEffectorLink(); at https://github.com/ros-planning/moveit/blob/kinetic-devel/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp#L944 the end-effector link will be correctly used. A change to this could break user code, if they use a fix like suggested in https://groups.google.com/forum/#!topic/moveit-users/VB7VWsgV9-g and set their end-effector to something different than their IK tip frame.

Could you please file a pull-request and give an example of where this changes current behavior? At the moment I'm inclined to consider this a plain bug and support a patch for kinetic.

I couldn't figure out where the IK is computed when using position and orientation constraints, so I have no idea why this doesn't work as expected.

Can't blame you. The code is horribly convoluted. The constraints are forwarded to the planner, the planner instantiates a constraint sampler manager and this one can decide for each individual plan to instantiate IKConstraintSampler (see here). The relevant code you are looking for is here and here.

hartmanndennis commented 7 years ago

I found the issue with setPoseTarget. https://github.com/ros-planning/moveit/commit/fba9babb8eb5d06c41bdf23d70f207da6f7198f8 added the ability to use links other than the ik tip but the implementation is conceptually wrong. It transforms the constraint before any sampling is done which is fine for pure orientation constraints, doesn't work at all for pure position constraints and could work for combined constraints but it isn't implemented correctly. I suggest getting the transform from end-effector to ik chain tip one time and then transforming after sampling just like the ik base frame transformation is done.

v4hn commented 7 years ago

On Wed, Aug 09, 2017 at 06:09:32AM -0700, Dennis Hartmann wrote:

I found the issue with setPoseTarget. [...] but the implementation is conceptually wrong.

Please elaborate on this. Is the problem with the IK solver that it computes IK for a different link than the one requested?

Shouldn't this work with generic IK plugins like TracIK and KDL?

Also I wonder why this code uses searchPositionIK and not RobotState::setFromIK. The latter should automatically take care of the transforms, but maybe it is a lot slower?

I suggest getting the transform from end-effector to ik chain tip one time and then transforming after sampling just like the ik base frame transformation is done.

Given the circumstances I believe this would be ok.

hartmanndennis commented 7 years ago

Please elaborate on this. Is the problem with the IK solver that it computes IK for a different link than the one requested?

Shouldn't this work with generic IK plugins like TracIK and KDL?

If you have a position constraint on the end-effector and want to call an IK with a different tip frame, you have to first sample the orientation of the end-effector and then transform because the translation part is dependent on the orientation of the end-effector (It is a local transform/right-side multiplication). If you transform before sampling you fix the position of the IK tip in space instead of the end-effector. This should be independet of the IK you are using.