Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Unified Collision Environment Integration #1584

Merged
merged 12 commits into from
Aug 21, 2019

Conversation

j-petit
Copy link
Contributor

@j-petit j-petit commented Jul 25, 2019

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

@j-petit j-petit changed the title Unified Collision Environment [WIP] Unified Collision Environment Integration [WIP] Jul 25, 2019
Copy link
Contributor

@BryceStevenWilley BryceStevenWilley left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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


// 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_)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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)?

Copy link
Contributor Author

@j-petit j-petit Jul 29, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Contributor

@felixvd felixvd Jul 29, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

@j-petit
Copy link
Contributor Author

j-petit commented Jul 29, 2019

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
Copy link
Contributor Author

j-petit commented Jul 29, 2019

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
Copy link
Contributor

felixvd commented Jul 29, 2019

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
Copy link
Contributor

simonschmeisser commented Jul 31, 2019

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
Copy link
Contributor

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
Copy link
Contributor Author

j-petit commented Aug 5, 2019

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
Copy link
Contributor

simonschmeisser commented Aug 5, 2019 via email

@j-petit j-petit requested a review from jonbinney as a code owner August 5, 2019 15:55
@j-petit j-petit changed the title Unified Collision Environment Integration [WIP] Unified Collision Environment Integration Aug 6, 2019
@davetcoleman
Copy link
Member

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.

Copy link
Member

@davetcoleman davetcoleman left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

@j-petit
Copy link
Contributor Author

j-petit commented Aug 7, 2019

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
Copy link
Contributor Author

j-petit commented Aug 7, 2019

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

@j-petit
Copy link
Contributor Author

j-petit commented Aug 9, 2019

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

@davetcoleman
Copy link
Member

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
Copy link
Member

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

@j-petit
Copy link
Contributor Author

j-petit commented Aug 19, 2019

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
Copy link
Contributor Author

j-petit commented Aug 19, 2019

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
Copy link
Member

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
Copy link
Contributor Author

j-petit commented Aug 20, 2019

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
Copy link
Member

@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
Copy link
Contributor Author

j-petit commented Aug 21, 2019

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

@BryceStevenWilley
Copy link
Contributor

Lets get this rolling: squash merging now.

@BryceStevenWilley
Copy link
Contributor

Squash completed locally, building locally to before pushing.

BryceStevenWilley pushed a commit to BryceStevenWilley/moveit that referenced this pull request Aug 21, 2019
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
Copy link
Contributor

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 davetcoleman merged commit 8066a50 into moveit:master Aug 21, 2019
@davetcoleman
Copy link
Member

davetcoleman commented Aug 21, 2019

giphy

@j-petit j-petit deleted the fcl_collision_env branch August 22, 2019 08:09
@v4hn
Copy link
Contributor

v4hn commented Aug 22, 2019

@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
Copy link
Contributor Author

j-petit commented Aug 22, 2019

Yes, I'll look into it.

@j-petit
Copy link
Contributor Author

j-petit commented Aug 22, 2019

Made the patch, see here

rhaschke added a commit to ubi-agni/moveit that referenced this pull request Sep 11, 2019
henningkayser pushed a commit to PickNikRobotics/moveit that referenced this pull request Nov 21, 2019
* 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
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

6 participants