-
Notifications
You must be signed in to change notification settings - Fork 938
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
Conversation
There was a problem hiding this 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.
moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h
Outdated
Show resolved
Hide resolved
moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h
Outdated
Show resolved
Hide resolved
moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h
Outdated
Show resolved
Hide resolved
|
||
// 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_) |
There was a problem hiding this comment.
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)
?
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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.
moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_env_fcl.h
Outdated
Show resolved
Hide resolved
How should we handle the |
423707c
to
33a718a
Compare
I was successful in merging |
Write more, many more tests and improve coverage? I can't think of much else, except to ask for manual beta testing. |
AFAIK MoveIt's PlanningSceneDisplay in rviz uses a replicated Note: this feature is not very accessible as of now ... I hope to do something about it at some point |
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. |
Yes, it is just three files.
Which feature do you mean? The |
> 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
|
772df38
to
165a1e2
Compare
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. |
There was a problem hiding this 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
moveit_core/collision_detection/include/moveit/collision_detection/collision_env.h
Outdated
Show resolved
Hide resolved
...core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h
Outdated
Show resolved
Hide resolved
moveit_core/collision_distance_field/src/collision_env_distance_field.cpp
Show resolved
Hide resolved
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. |
165a1e2
to
7021214
Compare
I added the PR for |
I added a PR in |
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 :-) |
We switched to |
Yes, rebased on master and changed new headers to also use |
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. |
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? |
@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? |
I am available from now (8 am UTC+2) until 8 pm UTC+2. |
Lets get this rolling: squash merging now. |
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
Everything (except moveit_tutorials on master) build locally right now, but unfortunately, I cannot push the changes with Travis failing, even with
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. |
@j-petit @gavanderhoorn The API change broke compatibility with stomp_ros. @j-petit Could you briefly look at it and maybe provide a patch? |
Yes, I'll look into it. |
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
Description
This PRs purpose is to show what changes are necessary for a unified collision environment instead of keeping separate
collision_robot
andcollision_world
. Main changes are combiningcollision_robot_fcl
andcollision_world_fcl
,collision_world_distance_field
andcollision_robot_distance_field
,collision_world_hybrid
andcollision_robot_hybrid
into a single environment each derived from the new base classcollision_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
andcollision_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 singlecollision_world
as in motion planning you need to check a large amount of robot poses against the same current world environment. Keeping a singlecollision_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
andcollision_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 incollision_robot_fcl
are used and their global pose updated with the currentrobot_state
.The unified
collision_env
It combines
collision_robot
andcollision_world
into a singlecollision_env
from which specific collision detection environment likecollision_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 thecollision_env_fcl
as it simply combinescollision_robot_fcl
andcollision_world_fcl
as it is (and there all areconst
), 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 thecollision_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 thecollision_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
andcollision_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
andcollision_robot_bullet
, the gain in performance is not so big.Con's
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).collision_world/robot_fcl
,collision_robot/world_distance_field
,collision_robot/world_hybrid
) have to be unifiedconst
goes all up to the planning scene and further