Moving to named target with incompletly specified group joints fails to plan

2b-t commented 1 year ago

Description

When commanding a robot to a named target that does not specify all the joint values either with:

the robot seems to use 0.0 for the unspecified joints and as a result might fail to plan the motion. My main question is, is this a bug or expected behavior (and if yes, what is the reasoning behind it)? After all the doc-string states:

Joint variables in the group that are missing from variable_values remain unchanged (to reset all target variables to their current values in the robot use setJointValueTarget(getCurrentJointValues())).

Does unchanged in this context mean default initialized?

Your environment

ROS Distro: Galactic
OS Version: Ubuntu 20.04
Source build: Branch galactic, commit 1255936
RMW: CycloneDDS 0.8.0-focal.20221203.084342

Steps to reproduce

I have encountered an unexpected error when working with the mobile manipulator Hello-Robot Stretch. The particularity of this robot is that its linear axis joint_lift at position 0.0 is in collision with its base, meaning that moving the arm to 0.0 is in general not possible (see images below). When commanding the robot to a named target that does not specify all the joint values for all of its joints in the srdf, which is the case for most of the named targets defined in stretch_description.srdf (e.g. the stretch_arm group defines 7 joints

<group name="stretch_arm">
  <joint name="joint_lift"/>
  <joint name="joint_arm_l4"/>
  <joint name="joint_arm_l3"/>
  <joint name="joint_arm_l2"/>
  <joint name="joint_arm_l1"/>
  <joint name="joint_arm_l0"/>
  <joint name="joint_wrist_yaw"/>
</group>

while the group state wrist_in only defines a single joint, the joint_wrist_yaw:

<group_state name="wrist_in" group="stretch_arm">
  <joint name="joint_wrist_yaw" value="3.1415"/>
</group_state>

) the robot fails to plan the motion complaining about a collision between the arm and the mobile base while the two are clearly not in collision. When using RViz for planning on the other hand this works just fine!

A code snippet that is able to replicate this on the Hello-Robot stretch is the following:

std::string const planning_group_arm {"stretch_arm"};
std::string const named_target {"wrist_in};
auto move_group_interface_arm {std::make_shared<moveit::planning_interface::MoveGroupInterface>(node, planning_group_arm)};

if (!planning_group_arm->setNamedTarget(named_target)) {
  RCLCPP_ERROR_STREAM(this->get_logger(), "Could not find named target '" << named_target << "' for arm!");
  return false;
}
moveit::planning_interface::MoveGroupInterface::Plan plan {};
if (planning_group_arm->plan(plan) != moveit::planning_interface::MoveItErrorCode::SUCCESS) {
  RCLCPP_ERROR_STREAM(this->get_logger(), "Planning for arm for named target '" << named_target << "' failed!");
  return false;
}
if (planning_group_arm->execute(plan) != moveit::planning_interface::MoveItErrorCode::SUCCESS) {
  RCLCPP_ERROR_STREAM(this->get_logger(), "Moving arm to named target '" << named_target << "' failed!");
  return false;
}

replicating this might be hard as Hello-Robot is currently not offering a simulation package for the robot but one should be able to replicate this on any robot by specifying a named target only for some joints of a group inside the srdf, making sure that there is a collision if all joints are set to 0.0 (I assume could also be with an artificially inserted obstacle or one from an octomap from the depth camera) and then command the robot to move to that named target with setNamedTarget.

Expected behaviour

The robot should move to the given pose either:

keeping the other joints in the same way as in the current configuration or
move the other joints to avoid collisions (as they are unconstrained) in case reaching the goal is not possible without that

So when commanding the robot to wrist-in, corresponding to setting joint_wrist_yaw to 3.1415 rad or 180 deg I would expect the robot to do the following (shadow indicates the current pose while the solid robot the desired one):

ExpectedTarget_JointValues

Actual behaviour

Moveit detects a collision between base_link and link_gripper. From the debugging it seems as if it would assume 0.0 for all the unspecified joints and therefore plans for a configuration like below, which results in the collisions (again the shadow indicating its current pose and the solid robot depicting what seems to be happening with the end-effector colliding with the base).

ActualTarget_JointValues

The reason this works from RViz is that it does not use MoveGroupInterface::setNamedTarget to move to named targets and therefore has values for all the joints. Clearly if I read out the joints with getJointNames and the current joint values with getCurrentJointValues and combine them in an std::map<std::string, double> together with the values from getNamedTargetValues and call the corresponding overload of setJointValueTarget, everything works as expected. So something like:

auto const joint_names {move_group_interface->getJointNames()};
auto const current_joint_values {move_group_interface->getCurrentJointValues()};
if (joint_names.size() != current_joint_values.size()) {
  RCLCPP_ERROR_STREAM(this->get_logger(), "Size of joint names (" << joint_names.size() << ") and joint values (" << current_joint_values.size() << ") do not match!");
  return false;
}
std::map<std::string, double> joint_values {};
std::transform(joint_names.begin(), joint_names.end(), current_joint_values.begin(), std::inserter(joint_values, joint_values.end()),
  [](std::string const& a, double const b) {
  return std::make_pair(a, b);
});
std::map<std::string, double> const joint_target_values {move_group_interface->getNamedTargetValues(named_target)};
for (auto const& [name, value]: joint_target_values) {
  if (joint_values.find(name) != joint_values.end()) {
    joint_values.at(name) = value;
  } else {
    RCLCPP_ERROR_STREAM(this->get_logger(), "Joint name '" << name << "' not found!");
  }
}
if (!move_group_interface->setJointValueTarget(joint_values)) {
  RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to set joint values for named target '" << named_target << "'!");
  return false;
}

Similarly when de-activating collisions between the involved links inside the srdf the planning and execution succeeds.

Backtrace or Console output

When starting the stack MoveGroup already gives a warning:

[move_group-5] [WARN] [1694688829.872395681] [moveit_robot_model.robot_model]: Group state 'lift_down' doesn't specify all group joints in group 'stretch_arm'. joint_arm_l3, joint_arm_l2, joint_arm_l1, joint_arm_l0, joint_wrist_yaw are missing.
[move_group-5] [WARN] [1694688829.872455809] [moveit_robot_model.robot_model]: Group state 'lift_up' doesn't specify all group joints in group 'stretch_arm'. joint_arm_l3, joint_arm_l2, joint_arm_l1, joint_arm_l0, joint_wrist_yaw are missing.
[move_group-5] [WARN] [1694688829.872480587] [moveit_robot_model.robot_model]: Group state 'arm_in' doesn't specify all group joints in group 'stretch_arm'. joint_lift, joint_wrist_yaw are missing.
[move_group-5] [WARN] [1694688829.872501837] [moveit_robot_model.robot_model]: Group state 'arm_out' doesn't specify all group joints in group 'stretch_arm'. joint_lift, joint_wrist_yaw are missing.
[move_group-5] [WARN] [1694688829.872518396] [moveit_robot_model.robot_model]: Group state 'wrist_out' doesn't specify all group joints in group 'stretch_arm'. joint_lift, joint_arm_l3, joint_arm_l2, joint_arm_l1, joint_arm_l0 are missing.
[move_group-5] [WARN] [1694688829.872534124] [moveit_robot_model.robot_model]: Group state 'wrist_in' doesn't specify all group joints in group 'stretch_arm'. joint_lift, joint_arm_l3, joint_arm_l2, joint_arm_l1, joint_arm_l0 are missing.
[move_group-5] The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.

When commanding the motion the output from the two involved nodes stretch_moveit_server (the one containing the MoveGroupInterface) and move_group is the following:

[stretch_moveit_server-7] [INFO] [1694688854.578332367] [move_group_interface]: Ready to take commands for planning group stretch_arm.
[stretch_moveit_server-7] [INFO] [1694688854.610014100] [move_group_interface]: Ready to take commands for planning group position.
[rviz2-6] [INFO] [1694688854.644398631] [move_group_interface]: Ready to take commands for planning group mobile_base_arm.
[rviz2-6] [INFO] [1694688854.645484537] [move_group_interface]: Looking around: no
[rviz2-6] [INFO] [1694688854.648441388] [move_group_interface]: Replanning: no
[stretch_moveit_server-7] [INFO] [1694688858.765735270] [move_group_interface]: Ready to take commands for planning group stretch_gripper.
[stretch_moveit_server-7] [INFO] [1694688861.624239440] [move_group_interface]: Ready to take commands for planning group stretch_head.
[stretch_moveit_server-7] [INFO] [1694688864.253433426] [move_group_interface]: Ready to take commands for planning group mobile_base_arm.
[stretch_moveit_server-7] [INFO] [1694688872.271582910] [stretch_moveit_server]: Started Stretch Moveit server...
[stretch_moveit_server-7] [INFO] [1694688876.652217605] [move_group_interface]: MoveGroup action client/server ready
[move_group-5] [INFO] [1694688876.653200934] [moveit_move_group_default_capabilities.move_action_capability]: Received request
[move_group-5] [INFO] [1694688876.653634363] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[stretch_moveit_server-7] [INFO] [1694688876.653471534] [move_group_interface]: Planning request accepted
[move_group-5] [INFO] [1694688876.664533968] [moveit_move_group_default_capabilities.move_action_capability]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[move_group-5] [INFO] [1694688876.666819181] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'stretch_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[move_group-5] [INFO] [1694688876.790012063] [moveit_kinematic_constraints.kinematic_constraints]: Constraint satisfied:: Joint name: 'joint_lift', actual value: 0.000042, desired value: 0.000000, tolerance_above: 0.000100, tolerance_below: 0.000100
[move_group-5] [INFO] [1694688876.790053627] [moveit_kinematic_constraints.kinematic_constraints]: Constraint satisfied:: Joint name: 'joint_arm_l3', actual value: 0.000049, desired value: 0.000000, tolerance_above: 0.000100, tolerance_below: 0.000100
[move_group-5] [INFO] [1694688876.790065025] [moveit_kinematic_constraints.kinematic_constraints]: Constraint satisfied:: Joint name: 'joint_arm_l2', actual value: 0.000020, desired value: 0.000000, tolerance_above: 0.000100, tolerance_below: 0.000100
[move_group-5] [INFO] [1694688876.790074581] [moveit_kinematic_constraints.kinematic_constraints]: Constraint satisfied:: Joint name: 'joint_arm_l1', actual value: 0.000012, desired value: 0.000000, tolerance_above: 0.000100, tolerance_below: 0.000100
[move_group-5] [INFO] [1694688876.790092832] [moveit_kinematic_constraints.kinematic_constraints]: Constraint satisfied:: Joint name: 'joint_arm_l0', actual value: 0.000008, desired value: 0.000000, tolerance_above: 0.000100, tolerance_below: 0.000100
[move_group-5] [INFO] [1694688876.790135919] [moveit_kinematic_constraints.kinematic_constraints]: Constraint satisfied:: Joint name: 'joint_wrist_yaw', actual value: 3.141561, desired value: 3.141500, tolerance_above: 0.000100, tolerance_below: 0.000100
[move_group-5] [INFO] [1694688876.790346339] [moveit_collision_detection_fcl.collision_common]: Found a contact between 'base_link' (type 'Robot link') and 'link_gripper' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[move_group-5] [INFO] [1694688876.790362945] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[move_group-5] [ERROR] [1694688876.909511793] [ompl]: /tmp/binarydeb/ros-galactic-ompl-1.5.2/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:253 - stretch_arm/stretch_arm: Unable to sample any valid states for goal tree
[move_group-5] [INFO] [1694688876.909641684] [moveit.ompl_planning.model_based_planning_context]: Unable to solve the planning problem
[move_group-5] [INFO] [1694688876.909701561] [moveit_move_group_default_capabilities.move_action_capability]: No motion plan found. No execution attempted.
[stretch_moveit_server-7] [INFO] [1694688876.987217394] [move_group_interface]: Planning request aborted
[stretch_moveit_server-7] [ERROR] [1694688877.053870978] [move_group_interface]: MoveGroupInterface::plan() failed or timeout reached

github-actions[bot] commented 10 months ago

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

sea-bass commented 9 months ago

This should try use the current state instead of falling back to zero. Will take a look.

sea-bass commented 9 months ago

OK I just took a look and I think your initial question has it right -- so it appears to be expected behavior, but I agree that it is confusing when you haven't initialized all the joints to the current state. Namely:

Joint variables in the group that are missing from variable_values remain unchanged (to reset all target variables to their current values in the robot use setJointValueTarget(getCurrentJointValues())).

So I think your code can follow this recommendation:

move_group_interface->setJointValueTarget(move_group_interface->getCurrentJointValues());
move_group_interface->setNamedTarget("my_named_state");

Does this work?

Also, I have to say that Galactic is no longer supported, so if you'd like to try this on Humble/Rolling to see if the issue still persists, might be worth a shot. However, I don't know if this part of the code has really changed all that much in the last decade :)

2b-t commented 9 months ago

Hi @sea-bass Thanks for your response. That is quite similar to what I ended up doing. Wrote this issue some month ago when Galactic was still supported. We are using Humble now. Feel free to close this issue if you think this default behavior should/will not be changed anytime soon. Many thanks

sea-bass commented 9 months ago

Yeah, I think because users will probably want to control exactly what to do with those other joints, I'm inclined to close this issue.

Either way, this will serve as a good reference for others running into similar problems, so thank you for posting this detailed writeup!

moveit / moveit2