Collision of newly attached object not taken into account for first plan

[alaflaquiere]

Description

Collision of newly attached object not taken into account for first plan.

Your environment

• ROS Distro: Melodic
• OS Version: Ubuntu 18.04
• Binary
• 1.0.10-Alpha

Steps to reproduce

1. load a panda robot and launch RViz, similar to the tutorial
2. add a box obstacle to the planning scene using the python API [planning_scene_interface.add_box(...)]
3. add a sphere not too far from the robot's gripper, and attach it to the robot's gripper [planning_scene_interface.add_sphere(...), planning_scene_interface.attach_box(...)]
4. plan a path between two arm configurations such that the attached sphere should easily collide with the non-attached box

Expected behaviour

Planning should take into account the newly created and attached sphere to avoid collisions.

Actual behaviour

This results in a plan that doesn't take the sphere-box collision into account. However, after this initial plan is executed, the sphere-box collision is properly taken into account for following plans. The symmetric issue also exists: a just detached-and-removed sphere is still taken into account for the next planning/execution step.

Backtrace or Console output

No issue there.

[welcome[bot]]

Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.

[rhaschke]

The PSI provides an asynchronous (default, using a topic) and a synchronous (using a service) interface: https://github.com/ros-planning/moveit/blob/93fbc98cdb83ce16c6a46da0656b69b5b9566abb/moveit_commander/src/moveit_commander/planning_scene_interface.py#L67

If you use the asynchronous one (as you probably did), it depends on the ROS timing whether the message to modify the PS arrives before the message to plan. To be sure, you should use the synchronous service interface: psi = PlanningSceneInterface(synchronous=True)

[alaflaquiere]

Thanks for the recommendation. Unfortunately, making the PSI synchronous didn't change the behavior. There is quite a delay between attaching the sphere and planning again in my code anyway, so maybe that's not surprising.

Some additional information:

• Simply adding/removing obstacles works as expected; i.e. the first subsequent plan is correct
• Only adding+attaching and detaching+removing seem to lead to an incomplete update; i.e. the first subsequent plan is incorrect
• The problem can be visualized in RViz:
  • After adding+attaching a sphere to the gripper, the sphere is visible in the following visualizations:
    • MotionPlanning / Scene Robot / Show Robot Visual
    • MotionPlanning / Planning Request / Query Start State
    • MotionPlanning / Planning Request / Query Goal State.
  • However, it is not visible in the MotionPlanning / Planned Path / Show Robot Visual visualization of the path found by the first planning.
• (The symmetric is true with detaching+removing the sphere)
• Replanning doesn't solve the issue. The sphere is taken into account only after the first (incorrect) plan is executed.

[rhaschke]

It would be helpful if you could share a minimal python snippet to reproduce the problem with the tutorial's Panda robot. Thanks.

[alaflaquiere]

Well, I started everything from scratch to create a clean minimal setup... and everything is working properly! This is all on a different machine and with a less involved setup, so I will have to go chase what the issue can be with the original simulation.

[rhaschke]

Looking forward to you report...

[AndyZe]

I have a video. This video was taken from master branch, built from source, commit 811328b, ROS Noetic, running the basic "MoveIt Quickstart with RViz" tutorial.

roslaunch panda_moveit_config demo.launch rviz_tutorial:=true

Note the first plan succeeds, although it should fail due to collision. The second plan fails like it should.

https://user-images.githubusercontent.com/11284393/199997413-8ea3da02-a922-44ae-8162-669ef19e9f2f.mp4

[rhaschke]

~~I bisected the issue and found the offending commit: #2652~~ False alarm: that commit just re-enabled displaying of planned trajectories. This issue is at least as old as 1.1.2!

[rhaschke]

I identified the problem and it exists for years (already present in release 1.0.8). It occurs in two different situations:

1. when attaching an object from rviz (as in the video). In that case rviz sends an old, fully-qualified RobotState as the start state of the request, which doesn't yet have the attached object. Only after execution, rviz' current state is updated.
2. when attaching an object via the asynchronous PlanningSceneInterface, the message is actually lost due to the well-known problem that the actual connection between subscribers and the publishers is not yet established.

[rhaschke]

The planned trajectory must not be executed, because the RobotState differs from the current one. That's another bug!

[alaflaquiere]

Sorry for the lack of update. I struggled to find enough time to look into this.

The video shared by @AndyZe looks very similar to what I encountered, except the (missed) collision was between a sphere attached to the robot and a cube in the environment.

I'll try the fix as soon as I'm able to go back to the related project. Thanks!