Unified Collision Environment Integration by j-petit · Pull Request #1584 · moveit/moveit

j-petit · 2019-07-25T13:13:49Z

Description

This PRs purpose is to show what changes are necessary for a unified collision environment instead of keeping separate collision_robot and collision_world. Main changes are combining collision_robot_fcl and collision_world_fcl, collision_world_distance_field and collision_robot_distance_field, collision_world_hybrid and collision_robot_hybrid into a single environment each derived from the new base class collision_env. Furthermore, the planning scene and the collision detector allocator for all detectors had to be modified.

This is independent of integrating Bullet into MoveIt. The current state compiles and passes all tests in in Moveit.

Related PRs in other MoveIt repos

Background Information

The case for a separate `collision_world` and `collision_robot`

Currently, the collision detection is split into two separate parts. collision_robot is responsible for self-collision checks. collision_world for collision detection of the robot with the world environment.
For these, the collision function calls contain a collision_robot as an argument from which then the link collision geometries can be accessed.

The split was made for multi-threaded collision detection where you have one collision_robot per thread and only a single collision_world as in motion planning you need to check a large amount of robot poses against the same current world environment. Keeping a single collision_world has memory advantages especially in the case of complicated point cloud world environments.

About the multi-threading advantages I am not 100% sure and would appreciate any additional thoughts.

FCL implementation of `collision_robot` and `collision_world`

On collision_robot_fcl: The robot link geometries are saved as member variables. When self-collision checks are executed a new FCL manager is created and all links are added to it. As the link geometries are only calculated once and stored persistently (including their local AABB), adding them to the new collision manager is cheap. Then, the broadphase collision check with updated global AABB is executed before narrowphase calculations take place for overlapping pairs.

On collision_world_fcl: It contains a member collision manager where all world objects are added / removed. This manager has a persistent tree structure for fast overlap calculations. When a robot-world collision check is executed, all robot links are tested individually against the world as we do not need to do self-collision checks here. For this the pre-calculated collision geometries contained in collision_robot_fcl are used and their global pose updated with the current robot_state.

The unified `collision_env`

It combines collision_robot and collision_world into a single collision_env from which specific collision detection environment like collision_env_fcl can be derived.

Additional changes which are not yet reflected in this PR (to keep it simple first) are dropping const from all collision checking functions. This is not necessary for the collision_env_fcl as it simply combines collision_robot_fcl and collision_world_fcl as it is (and there all are const), but brings significant advantages for a Bullet unified environment in the future.

The following paragraph is not part of this PR, just general thoughts for improving the collision checking: I would advocate for including the allowed collision matrix (ACM) (which specifies which objects are allowed to collide) into the unified environment instead of providing it as a pointer to each single collision query. I think in most use cases the ACM stays the same between two checks and therefore it makes sense to keep this information persistent in the collision manager. After constructing the collison_env with an ACM, the ACM should only be modified through the collision_env so the internal collision manager is able to update its broadphase structure with disabled/enabled collisions and can keep those changes persistent. This gives performance improvements as disabled collision are culled even before broadphase checking and keeps the collision_env consistent as this is the place to modify any collision checking behaviour. This proposal is not yet reflected in this PR. I will create a separate issue for this.

Pro's
The biggest advantage is maintainability. The current split setup is difficult to understand. Changing anything is hard because the interaction and dependencies between collision_robot and collision_world are sometimes not clear.

Another point is that handling of attached objects which change from being part of the world to being part of the robot (and back) currently is inefficient and could be made very easy in a unified environment as they are naturally part of the same single environment and only some collision flags would need to change.

Collision detection's main application is in computer games and physics simulation where you naturally have a single collision environment. Through mirroring this in MoveIt we can more easily leverage specific capabilities of collision detection libraries which give better performance like very early culling based on groups and masks even before broadphase checks are performed. It reduces the complexity in MoveIt and instead relies more on the intelligence in the collision library which I think is advantageous. Examplary, if one is interested in both collision with itself and the world, a single pass through unified collision environment is possible instead of two separate checks.

The unified environment brings speed improvements for using Bullet (see this) and also possibly for using FCL. However, compared to the general improvements for using Bullet (in separate collision_world_bullet and collision_robot_bullet, the gain in performance is not so big.

Con's

For multi-threaded collision checks, it is not directly clear for me how we can avoid copying the whole collision_env. But as maybe memory got cheaper than it was some years ago, these advantages can be forsaken in turn for the above mentioned advantages. Or, other smart copying techniques could be applied (I am no expert here).
Major changes throughout the codebase are necessary
- all current collision environments (collision_world/robot_fcl, collision_robot/world_distance_field, collision_robot/world_hybrid) have to be unified
- dropping const goes all up to the planning scene and further

moveit_core/CMakeLists.txt

moveit_core/collision_detection_fcl/test/test_fcl_env.cpp

BryceStevenWilley

Looks good at the moment, I added a few small nits and will review again when the changes are a little further along.

moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h

BryceStevenWilley · 2019-07-29T03:23:11Z

moveit_core/planning_scene/src/planning_scene.cpp

  // Allocate CollisionRobot unless we can use the parent's crobot_.
  // If active_collision_->crobot_ is non-NULL there is local padding and we cannot use the parent's crobot_.
-  if (!detector->parent_ || active_collision_->crobot_)
+  if (!detector->parent_)


According to the comment here, this happens when there is local padding. Since the equilavent, active_collision_->cenv_ is always non-null, do we have to assume there is always local padding and this becomes if (true)?

I am really unsure about all of this parent planning scene. I think the behavior before was:

cworld_ is always allocated new. crobot_ is only allocated if we don't have a parent planning scene or it is the active detector. The unpadded version is allocated if we don't have a parent (although I thought it should only be allocated when the padding is changed...).

With cenv_ it should be: cenv_ is always allocated and the unpadded version only when the padding changes or the scene does not have a parent? I pushed those changes, however, I am not very confident here.

I want to defer to someone else on this, because I never felt like I really figured out the intricacies of these particular copies and pointers. But I note that reducing this complexity is another big reason why this PR has value, in my opinion.

moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h

moveit_core/collision_detection_fcl/test/test_fcl_env.cpp

moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp

moveit_core/collision_detection/src/collision_env.cpp

j-petit · 2019-07-29T09:52:46Z

How should we handle the all_valid collision detector? I don't really see how something which just always returns no collision is useful as it only inflates the codebase.

j-petit · 2019-07-29T12:50:02Z

I was successful in merging collision_distance_field and collision_hybrid into single collision environments. MoveIt as a whole compiles now (except MoveIt tutorials which needs to include the new correct header but I cannot change this here as it is not part of this repo) and all existing tests pass. However, I am not too sure if everything is really working as before. Ideas how to best ensure this?

felixvd · 2019-07-29T14:33:28Z

Ideas how to best ensure this?

Write more, many more tests and improve coverage? I can't think of much else, except to ask for manual beta testing.

simonschmeisser · 2019-07-31T08:46:52Z

How should we handle the all_valid collision detector? I don't really see how something which just always returns no collision is useful as it only inflates the codebase.

AFAIK MoveIt's PlanningSceneDisplay in rviz uses a replicated PlanningSceneMonitor which includes everything necessary for collision checking. If you don't need those collision checking features it saves quite some resources to use all_valid as it does not build up any collision data structures on every world change. Also it's about three files right?

Note: this feature is not very accessible as of now ... I hope to do something about it at some point

simonschmeisser · 2019-07-31T08:54:02Z

I'm currently on vacation so I cannot test your code for regression wrt multi-threaded collision checking. Running some OMPL planning should give you a simple test case however as it uses several threads in most algorithms.

j-petit · 2019-08-05T07:26:17Z

AFAIK MoveIt's PlanningSceneDisplay in rviz uses a replicated PlanningSceneMonitor which includes everything necessary for collision checking. If you don't need those collision checking features it saves quite some resources to use all_valid as it does not build up any collision data structures on every world change. Also it's about three files right?

Yes, it is just three files.

Note: this feature is not very accessible as of now ... I hope to do something about it at some point

Which feature do you mean? The all_valid collision checker or this PR?

simonschmeisser · 2019-08-05T08:01:23Z

> Note: this feature is not very accessible as of now ... I hope to do something about it at some point Which feature do you mean? The `all_valid` collision checker or this PR?

I was referring to my memory that switching the collision checking plugin of the display is complicated or maybe only possible from code and not by configuration. Not related to this PR at all :)

…

-- You are receiving this because you commented. Reply to this email directly or view it on GitHub: #1584 (comment)

-- Gesendet von meinem Jolla

davetcoleman · 2019-08-06T21:14:34Z

I would advocate for including the allowed collision matrix (ACM) (which specifies which objects are allowed to collide) into the unified environment instead of providing it as a pointer to each single collision query. I think in most use cases the ACM stays the same between two checks and therefore it makes sense to keep this information persistent in the collision manager

This concerns me, as I've seen use cases where you disable collision checking between fingers / grippers and objects / surfaces when doing fine grasping approaches.

davetcoleman

I've skimmed every file. Final approval to someone else.
Please add this change to MIGRATION.md

moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h

moveit_core/collision_detection/src/collision_env.cpp

...core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h

moveit_core/collision_distance_field/src/collision_env_distance_field.cpp

j-petit · 2019-08-07T07:15:36Z

I would advocate for including the allowed collision matrix (ACM) (which specifies which objects are allowed to collide) into the unified environment instead of providing it as a pointer to each single collision query. I think in most use cases the ACM stays the same between two checks and therefore it makes sense to keep this information persistent in the collision manager

This concerns me, as I've seen use cases where you disable collision checking between fingers / grippers and objects / surfaces when doing fine grasping approaches.

But these changes can be done through the collision environment instead of directly changing the ACM. I think in most collision cases the ACM stays the same, right? So it makes sense to store this information persistently in the broadphase collision checking and register any changes to the ACM direclty in the broadphase.

j-petit · 2019-08-07T10:20:58Z

I added the PR for moveit_tutorials which would allow Travis to build and run tests.

j-petit · 2019-08-09T10:08:22Z

I added a PR in moveit_visual_tools which adapts for the unified collision environment changes here.

davetcoleman · 2019-08-13T20:40:42Z

I would advocate for including the allowed collision matrix (ACM) (which specifies which objects are allowed to collide) into the unified environment instead of providing it as a pointer to each single collision query. I think in most use cases the ACM stays the same between two checks and therefore it makes sense to keep this information persistent in the collision manager

This concerns me, as I've seen use cases where you disable collision checking between fingers / grippers and objects / surfaces when doing fine grasping approaches.

But these changes can be done through the collision environment instead of directly changing the ACM. I think in most collision cases the ACM stays the same, right? So it makes sense to store this information persistently in the broadphase collision checking and register any changes to the ACM direclty in the broadphase.

If its possible to do the above scenario, then this sounds good to me. I don't exactly follow your approach, but that's just my ignorance... you're certainly more expert at this part of the code base now :-)

davetcoleman · 2019-08-13T20:41:49Z

We switched to pragma once... i bet that is what is causing these merge conflicts

j-petit · 2019-08-19T12:02:44Z

We switched to pragma once... i bet that is what is causing these merge conflicts

Yes, rebased on master and changed new headers to also use pragma once.

j-petit · 2019-08-19T12:20:15Z

If its possible to do the above scenario, then this sounds good to me. I don't exactly follow your approach, but that's just my ignorance... you're certainly more expert at this part of the code base now :-)

Anyway, this is not something for this PR. I will create issue(s) with thoughts about improvements to the collision checking process during the next days as part of wrapping up the GSoC.

davetcoleman · 2019-08-20T01:52:15Z

Let's merge this! Is tomorrow (Tue) a good time for you @j-petit to be on standby to fix any issues we missed across multiple repos?

fyi @rhaschke

j-petit · 2019-08-20T07:25:33Z

Let's merge this! Is tomorrow (Tue) a good time for you @j-petit to be on standby to fix any issues we missed across multiple repos?

fyi @rhaschke

Exciting! Today (Tuesday) I can't be available too long (only until 5 pm UTC + 2), but on Wednesday I could be available till late. So maybe postpone to Wednesday?

davetcoleman · 2019-08-20T23:49:49Z

@BryceStevenWilley can you do the honors of merging this and the related PRs in other repos?

@j-petit what time will you be available Wed?

j-petit · 2019-08-21T06:12:17Z

I am available from now (8 am UTC+2) until 8 pm UTC+2.

BryceStevenWilley · 2019-08-21T11:34:02Z

Lets get this rolling: squash merging now.

BryceStevenWilley · 2019-08-21T11:45:25Z

Squash completed locally, building locally to before pushing.

commit ac34c580ba6d24b6f62b178b0d68703fcbd3f929 Author: Jens Petit <petit.jens@gmail.com> Date: Mon Aug 19 14:00:37 2019 +0200 PR review: * change to pragma once include guards * enable test commit e3bad98375a016a0de28ed263da3cea38ac48f0e Author: Jens Petit <petit.jens@gmail.com> Date: Thu Aug 8 09:41:07 2019 +0200 Replaced getCollisionWorld/Robot with getCollisionEnv functions commit 9662efc2b776c221d3bbf340a07844a1480ffbed Author: Jens Petit <petit.jens@gmail.com> Date: Wed Aug 7 10:43:05 2019 +0200 Added change description to migration notes. commit 14e044e8925149b11869cb0e89cd7d9384ed8ea4 Author: Jens Petit <petit.jens@gmail.com> Date: Wed Aug 7 10:05:39 2019 +0200 PR review: * added as author * added documentation to collision environments commit a0a69bbf7b17cea367acd11a2633bee636331e53 Author: Jens Petit <petit.jens@gmail.com> Date: Mon Aug 5 17:54:02 2019 +0200 SBPL planner adapted for unified collision environment commit 8872e3907c62dc6ca31ba7c77a19c7a0035354ad Author: Jens Petit <petit.jens@gmail.com> Date: Mon Aug 5 12:08:12 2019 +0200 Replace references to CollisionWorld / CollisionRobot to new CollisionEnv commit 6f6b54c7e4a154deefb1f2bfc11c755982e432e8 Author: Jens Petit <petit.jens@gmail.com> Date: Mon Aug 5 12:07:42 2019 +0200 Unified all_valid collision detector commit 6acc3f16e662f4af25b0eb362bc0f9101cc4f795 Author: Jens Petit <petit.jens@gmail.com> Date: Mon Jul 29 11:38:25 2019 +0200 PR review: * collision environmnet test cases adapted * allocating of child planning scenes * valided padding and scaling added * reordering of member variables and functions * license adaptions commit 08bfaad4dfa30a06eb1881f1bc227d8dede03dd9 Author: Jens Petit <petit.jens@gmail.com> Date: Mon Jul 29 08:54:02 2019 +0200 Collision distance field and hybrid compiles commit cc479aa2fa1ca6762ae7b191f96e5d467dcb53a3 Author: Jens Petit <petit.jens@gmail.com> Date: Thu Jul 25 13:26:20 2019 +0200 Distance field collision environment commit 0078c26881bba3bdb78be2e2e28aa87b4efe39ae Author: Jens Petit <petit.jens@gmail.com> Date: Thu Jul 25 11:24:21 2019 +0200 Integrating FCL unified environment into the planning scene commit 7ae9ee4f93f05ffeb519b7f2452e3e6019a214c4 Author: Jens Petit <petit.jens@gmail.com> Date: Wed Jul 24 16:26:02 2019 +0200 Unified collision environment

BryceStevenWilley · 2019-08-21T12:16:38Z

Everything (except moveit_tutorials on master) build locally right now, but unfortunately, I cannot push the changes with Travis failing, even with --force.

$ git push --force upstream master
Counting objects: 97, done.
Delta compression using up to 4 threads.
Compressing objects: 100% (50/50), done.
Writing objects: 100% (97/97), 44.98 KiB | 14.99 MiB/s, done.
Total 97 (delta 53), reused 70 (delta 33)
remote: Resolving deltas: 100% (53/53), completed with 39 local objects.
remote: error: GH006: Protected branch update failed for refs/heads/master.
remote: error: Required status check "continuous-integration/travis-ci" is expected. At least 1 approving review is required by reviewers with write access.
To https://github.com/ros-planning/moveit
 ! [remote rejected]     master -> master (protected branch hook declined)
error: failed to push some refs to 'https://github.com/ros-planning/moveit'

According to this Github article, only administrators can ignore failing statuses (@davetcoleman @rhaschke).

In the meantime, I've pushed the squashed commit to master on my fork if you just want to grab and push that to master.

davetcoleman · 2019-08-21T15:17:16Z

v4hn · 2019-08-22T08:50:27Z

@j-petit @gavanderhoorn The API change broke compatibility with stomp_ros.

@j-petit Could you briefly look at it and maybe provide a patch?

j-petit · 2019-08-22T08:52:23Z

Yes, I'll look into it.

j-petit · 2019-08-22T09:16:11Z

Made the patch, see here

This reverts commit 8066a50.

* Unified collision environment * Integrating FCL unified environment into the planning scene * Distance field collision environment * Collision distance field and hybrid compiles * PR review: * collision environmnet test cases adapted * allocating of child planning scenes * valided padding and scaling added * reordering of member variables and functions * license adaptions * Unified all_valid collision detector * Replace references to CollisionWorld / CollisionRobot to new CollisionEnv * SBPL planner adapted for unified collision environment * PR review: * added as author * added documentation to collision environments * Added change description to migration notes. * Replaced getCollisionWorld/Robot with getCollisionEnv functions * PR review: * change to pragma once include guards * enable test

j-petit requested review from BryceStevenWilley, rhaschke and v4hn as code owners July 25, 2019 13:13

j-petit changed the title ~~Unified Collision Environment [WIP]~~ Unified Collision Environment Integration [WIP] Jul 25, 2019

davetcoleman reviewed Jul 25, 2019

View reviewed changes

j-petit mentioned this pull request Jul 26, 2019

GSoC: Improved Collision Checking #1427

Open

BryceStevenWilley reviewed Jul 29, 2019

View reviewed changes

j-petit requested a review from bmagyar as a code owner July 29, 2019 12:29

j-petit force-pushed the fcl_collision_env branch from 423707c to 33a718a Compare July 29, 2019 12:37

j-petit requested a review from jonbinney as a code owner August 5, 2019 15:55

j-petit force-pushed the fcl_collision_env branch from 772df38 to 165a1e2 Compare August 5, 2019 16:02

j-petit changed the title ~~Unified Collision Environment Integration [WIP]~~ Unified Collision Environment Integration Aug 6, 2019

davetcoleman approved these changes Aug 6, 2019

View reviewed changes

j-petit force-pushed the fcl_collision_env branch from 165a1e2 to 7021214 Compare August 7, 2019 08:18

j-petit mentioned this pull request Aug 7, 2019

Unified Collision Environment moveit/moveit_tutorials#374

Merged

j-petit mentioned this pull request Aug 9, 2019

Unified collision environment moveit/moveit_visual_tools#54

Merged

BryceStevenWilley self-requested a review August 13, 2019 12:06

j-petit mentioned this pull request Aug 20, 2019

Developers' concepts page added with collision detection info moveit/moveit.ros.org#326

Merged

1 task

davetcoleman merged commit 8066a50 into moveit:master Aug 21, 2019

j-petit mentioned this pull request Aug 21, 2019

Fix clang-tidy for unified collision environment #1638

Merged

1 task

j-petit deleted the fcl_collision_env branch August 22, 2019 08:09

BryceStevenWilley mentioned this pull request Aug 23, 2019

Unified Collision Environment Bullet #1572

Merged

j-petit mentioned this pull request Aug 26, 2019

Future work for improving MoveIt's collision detection #1646

Open

rhaschke added a commit to ubi-agni/moveit that referenced this pull request Sep 11, 2019

Revert "Unified Collision Environment Integration (moveit#1584)"

f53937b

This reverts commit 8066a50.

henningkayser mentioned this pull request Oct 18, 2019

collision_plugin_loader port to ROS 2 moveit/moveit2#69

Closed

rhaschke mentioned this pull request Nov 19, 2019

Setup assistant: stack smashing when generating ACM #1596

Closed

rhaschke mentioned this pull request Jan 16, 2020

Issues with PlanningScene updates #1835

Closed

j-petit mentioned this pull request Feb 18, 2020

"object" collides with "object": unconditionally allocate unpadded collision environments #1899

Merged

JeroenDM mentioned this pull request Mar 27, 2020

TrajOpt motion planner for MoveIt #1467

Open

5 tasks

This was referenced May 12, 2020

[melodic-devel] jog arm backport #2083

Merged

July 2020 Melodic Release #2066

Closed

captain-yoshi mentioned this pull request Feb 5, 2021

Bullet collision: memory leak #3622

Open

Conversation

j-petit commented Jul 25, 2019 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Related PRs in other MoveIt repos

Background Information

The case for a separate collision_world and collision_robot

FCL implementation of collision_robot and collision_world

The unified collision_env

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

BryceStevenWilley left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

BryceStevenWilley Jul 29, 2019

Choose a reason for hiding this comment

Uh oh!

j-petit Jul 29, 2019 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

felixvd Jul 29, 2019 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

j-petit commented Jul 29, 2019 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

j-petit commented Jul 29, 2019

Uh oh!

felixvd commented Jul 29, 2019

Uh oh!

simonschmeisser commented Jul 31, 2019 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

simonschmeisser commented Jul 31, 2019

Uh oh!

j-petit commented Aug 5, 2019

Uh oh!

simonschmeisser commented Aug 5, 2019 via email

Uh oh!

davetcoleman commented Aug 6, 2019

Uh oh!

davetcoleman left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

j-petit commented Aug 7, 2019

Uh oh!

j-petit commented Aug 7, 2019

Uh oh!

j-petit commented Aug 9, 2019 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

davetcoleman commented Aug 13, 2019

Uh oh!

davetcoleman commented Aug 13, 2019

Uh oh!

j-petit commented Aug 19, 2019

Uh oh!

j-petit commented Aug 19, 2019

j-petit commented Jul 25, 2019 •

edited

Loading

The case for a separate `collision_world` and `collision_robot`

FCL implementation of `collision_robot` and `collision_world`

The unified `collision_env`

j-petit Jul 29, 2019 •

edited

Loading

felixvd Jul 29, 2019 •

edited

Loading

j-petit commented Jul 29, 2019 •

edited

Loading

simonschmeisser commented Jul 31, 2019 •

edited

Loading

davetcoleman left a comment •

edited

Loading

j-petit commented Aug 9, 2019 •

edited

Loading

davetcoleman commented Aug 21, 2019 •

edited

Loading