Collision detected between 2 robot arms when planned trajectory does not collide

[chiatseen]

Description

The environment contains a ur3e and a ur5 arm. Both arms are programmed to move a short distance simultaneously, and collision is detected between the ur3e_upper_arm_link and ur5_forearm_link. It is possible for these two links to collide, but in the trajectory planned, the robots do not collide.

This only happened after commit OMPL constrained planning #347. The issue is solved after reverting to the previous commit.

Your environment

• ROS Distro: ROS2 Foxy
• OS Version: e.g. Ubuntu 20.04
• Source build
• Branch main at latest Servo: do not cause a joint jump when SuddenHalt() is called #410

Steps to reproduce

1. Create environment with ur3e and ur5 placed 0.7m apart
2. Plan and execute a small trajectory for both arms

Expected behaviour

Arms should plan and execute the trajectory without colliding.

Actual behaviour

Collision detected between ur3e_upper_arm_link and ur5_forearm_link. Planner is unable to solve the planning problem and planning for ur3e fails while planning and execution for ur5 completes.

Backtrace or Console output

[INFO] [1617895459.600051854] [moveit_collision_detection_fcl.collision_common]: Found a contact between 'ur3e_upper_arm_link' (type 'Robot link') and 'ur5_forearm_link' (type 'Robot link'), which constitutes a collision. Contact in

formation is not stored.
[INFO] [1617895459.600059826] [moveit_collision_detection_fcl.collision_common]: Collision checking is considered complete (collision was found and 0 contacts are stored)

[WARN] [1617895459.618092764] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/tools/multiplan/src/ParallelPlan.cpp:138 - ParallelPlan::solve(): Unable to find solution by any of the threads in 0.000129 seconds
[INFO] [1617895459.618181463] [moveit.ompl_planning.model_based_planning_context]: Unable to solve the planning problem
[ERROR] [1617895459.618220257] [moveit.ros_planning_interface.planning_component]: Could not compute plan successfully

[AndyZe]

This probably isn't your issue, but MoveIt collision checks are discrete at the moment. Collisions get checked at waypoint A and waypoint B, for example, but not in-between. So it's possible for MoveIt to say a trajectory is collision-free when it really isn't.

You could switch to the Bullet continuous collision checker to make sure that's not the issue. Tutorial here -- but it requires delving into source code a bit more than people usually do.

[Briancbn]

Hi @AndyZe , Thanks for the reply.

This only happened after commit OMPL constrained planning #347. The issue is solved after reverting to the previous commit.

This phantom detection only happens after #347 is merged. We don't know if this PR has changed the collision checking part for OMPL to cause this "phantom" collision detection problem. I am also happy to email you a shrink-down version of our code so that you can reproduce and help us debug if you have time.

Collisions get checked at waypoint A and waypoint B, for example, but not in-between.

Thanks for the explanation on the difference between discrete and continuous collision checker. But per my understanding is that continuous collision checker is able detect possible path collision that previously cannot be detected by a discrete collision checker. However, our issue currently is the FCL discrete collision checker with OMPL before #347 doesn't detect collision and plan successfully, but detects and fail after #347 with the same source code.

[JafarAbdi]

@Briancbn Do you mind sharing a reproducible example, so we can debug it and see where's the root issue .?

[henningkayser]

@chiatseen Could you test if the problem still exists with the latest changes in main? #455 fixed a bug directly related to the feature you referenced, and it would at least cause missed collisions at the start states.

[Briancbn]

Thanks @henningkayser for the prompt fix. We have verified that the previous consistent phantom collision doesn't occur any more. We will continue testing and can re-open this if necessary. @chiatseen we can close this issue now.