Moveit: move_group does not fill in goal tolerances

[methylDragon]

Description

Goal tolerances are not passed through to a FollowJointTrajectory action goal when using either the move_group interface (on C++, Python untested), or by using the MoveIt! RViz plugin.

Your environment

• ROS Distro: Melodic
• OS Version: Ubuntu 18.04
• Source: https://github.com/ros-planning/moveit/commit/9f3773d849f7fb98c8d7519d29d508ab9f5d4ca7

Steps to reproduce

• Start any simulated arm with MoveIt! (for example, the Panda arm)
• Attempt to assign a goal with a non-zero goal tolerance
• Attempt to assign a goal with a move_group interface configured to have a non-zero goal_time_tolerance by using the moveit::planning_interface::MoveGroup::setGoalTolerance or related class methods, then use moveit::planning_interface::MoveGroup::move to send the goal.

Expected behaviour

• When subscribing to the goal endpoint on the follow_joint_trajectory action server, the incoming message should include a goal_tolerance field as a non-empty array reflecting the tolerances of each individual joint.
• I'm not sure on this, but the same should be said for the path_tolerance field.

Actual behaviour

• The field remains as an empty array.

Is this intended behaviour?

[welcome[bot]]

Thanks for reporting an issue. We will have a look asap. If you can think of a fix, please consider providing it as a pull request.

[felixvd]

It doesn't sound like that's intentional, but it would be amazingly convenient if you could reproduce this in an example package that someone can run and test with.

[methylDragon]

Hmm, the issue is reproducible but it'll take a tiny bit of time to set up.

Setup (ensure ROS is sourced)

Run these commands

mkdir ~/moveit_arm_test_ws
cd ~/moveit_arm_test_ws

mkdir src
cd src
git clone https://github.com/berkeleyopenarms/blue_simulator.git
git clone https://github.com/berkeleyopenarms/blue_core.git
git clone https://github.com/berkeleyopenarms/blue_moveit.git
git clone https://github.com/methylDragon/moveit_goal_tolerance_tester.git
cd blue_simulator
git submodule update --init --recursive

cd ../..

catkin build
source devel/setup.bash

RViz Testing

# Terminal 1
roslaunch blue_moveit_bringup gazebo_demo.launch

# Terminal 2
rostopic echo /left_arm/blue_controllers/joint_trajectory_controller/follow_joint_trajectory/goal

Then add a MotionPlanning scene in RViz, by clicking Add

And selecting the plugin

Then go to Planning and input a non-zero goal tolerance

And click plan and execute.

The terminal with the rostopic echo will start reflecting the goal, and there you should find that the message section reflects empty tolerances.

  path_tolerance: []
  goal_tolerance: []
  goal_time_tolerance: 
    secs: 0
    nsecs:         0

Terminal 1 will also reflect the following messages:

[ WARN] [1561107389.272837020, 22.052000000]: Controller left_arm/blue_controllers/joint_trajectory_controllerfailed with error GOAL_TOLERANCE_VIOLATED: 
[ERROR] [1561107389.917968642, 22.696000000]: Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 4.593225 seconds). Stopping trajectory.

The WARN is due to the missing tolerance arrays OR the goal_time_tolerance
Whereas the ERROR is just because RViz has no way to configure a proper goal_time_tolerance.

move_base C++ Testing

# Terminal 1
roslaunch blue_moveit_bringup gazebo_demo.launch

# Terminal 2
rostopic echo /left_arm/blue_controllers/joint_trajectory_controller/follow_joint_trajectory/goal

# Terminal 3
rosrun moveit_goal_tolerance_tester moveit_goal_tolerance_tester

Terminal 3 runs a C++ MoveIt! move_group script with tolerances set to 0.025. But Terminal 2 ends up showing only an empty array.

  path_tolerance: []
  goal_tolerance: []
  goal_time_tolerance: 
    secs: 15
    nsecs:         0

This does not affect the performance of the arm, but is an issue if one requires the tolerances to be set.

goal_time_tolerance works because I have a library manually adding it into the message since the move_group interface has no provision for setting goal time tolerances.

Do note that the pick and place demo will cause Gazebo to crash when the arm touches the payload object. This is an unrelated issue that is unrelated to this MoveIt! issue.

[methylDragon]

I edited the above comment, anyone who's interested can test for both RViz and C++

[felixvd]

Thanks for the quick reply and detailed instructions. If you make a package, you can use the panda_moveit_config's demo.launch, so that the package can run when cloned into this docker container's workspace.

[v4hn]

By current design, it fully makes sense that the tolerances in planning_interface::MoveGroupInterface are not passed on with the resulting trajectory.
I don't believe we want to change this either.

The tolerances specified in the planning interface refer to the region of valid goals that the planner might consider (this means different things for joint targets and Cartesian targets).
The planner will generate exact targets from within these goal regions and will try to generate a trajectory to reach this exact target.
Importantly, some (or even most) of the poses in the specified region could result in collisions and would fail collision checking.
In this case you clearly do not forward these tolerances to the execution if the planner also finds some concrete valid target in the region.
So these two tolerances are logically independent.

That being said, moveit could use a mechanism to specify tolerances for execution.
@rhaschke implemented a simple version of this a while ago in https://github.com/ros-planning/moveit/pull/1352/commits/749846f3f46b3b5a2436e47a3c10d2e74d6df535 .
The patch allows to specify static tolerances in the controllers.yaml for the moveit config.
Further discussions and patches that generalize this to runtime configuration are very welcome.

Hope this clears up some confusion.

Of course, as an ugly workaround you could also generate the trajectories, add the execution tolerances and forward them to your controller.

[felixvd]

Actually that's right, I didn't read attentively enough while sorting through issues. The execution tolerances are separate from the planning tolerances. My bad.

[methylDragon]

Ah alright, thanks for the clarification (: :fire:

[v4hn]

Thank you for your detailed report though @methylDragon !
I know this takes quite some time to prepare and it does help maintainers a lot with actual bugs.

In your case, please consider to add a sentence somewhere in the tutorials to clear up the confusion for the next developer wondering about it.

[v4hn]

Looking at your profile, if you happen to make music involving a MoveIt-based setup, please notify us!
Incidentally, my lab currently does research in that direction. :smile:

[methylDragon]

Thanks for the acknowledgement!
Relevant pull request sent.

I'm mostly just supporting the Berkeley Blue arm development, haha. No intentions at the moment to jump into any super complex manipulator projects yet ;)