Adding Bullet and Tesseract into MoveIt #1504

j-petit · 2019-06-14T16:16:02Z

Description

This PR is a follow up to #1499 and #1500 which all deal with adding Bullet as collision checker (see #1427) and fills the empty template using the modified tesseract/trajopt code. I used the given CollisionWorld and CollisionRobot structure.

This is an intermediate state where the continuous collision detection has not been added yet. See PR #1551 for the full bullet integration.

Building requires libbullet-dev and libbullet-extras-dev which can both be installed through apt.
Bullet passes all tests which were originally designed for FCL and I proposed in a separate PR #1543 a typed test suite so we can share the test scenarios across all collision checkers. As soon as this is merged, tests in this PR will be replaced with an instantiation of the typed test suite.

See this high level flowchart for the collision checking process using Bullet:

To further understand the collision checking process using Bullet see this detailed flowchart:

Thanks for reviewing!

BryceStevenWilley

I've done a very quick first pass on this PR. There are a lot of other style things that we should tackle after a first functionality pass is through. In general, look for places where we can use MoveIt structures instead of bringing in new tesseract structs internal to the collision managers.

moveit_core/CMakeLists.txt

moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h

...core/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_common.h

...re/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bt.h

...collision_detection_bullet/include/moveit/collision_detection_bullet/tesseract/basic_types.h

...tion_bullet/include/moveit/collision_detection_bullet/tesseract/bullet_cast_simple_manager.h

...core/collision_detection_bullet/include/moveit/collision_detection_bullet/tesseract/macros.h

moveit_planners/trajopt/src/trajopt_planner_manager.cpp

BryceStevenWilley

(Sorry, I though I submitted this review sometime yesterday)

moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h

...re/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_world_bt.h

...ollision_detection_bullet/include/moveit/collision_detection_bullet/tesseract/bullet_utils.h

..._detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h

moveit_core/collision_detection_bullet/src/tesseract/bullet_cast_bvh_manager.cpp

moveit_core/collision_detection_bullet/src/tesseract/bullet_discrete_bvh_manager.cpp

moveit_core/collision_detection_bullet/test/test_bt_collision_detection.cpp

BryceStevenWilley · 2019-07-04T13:42:00Z

This is close to being ready, the last big thing that I'd suggest is to git rebase -i and cleanup the commits so they're easier to read through, i.e. making format commits fixups of previous commits, combining related commits, etc.

BryceStevenWilley

Few more changes/some open questions for review

...t_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp

moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h

...re/collision_detection_bullet/include/moveit/collision_detection_bullet/collision_robot_bt.h

...collision_detection_bullet/include/moveit/collision_detection_bullet/tesseract/basic_types.h

...tection_bullet/include/moveit/collision_detection_bullet/tesseract/bullet_cast_bvh_manager.h

rhaschke

In general I approve this as well. I have the same remark as Dave: Try to replace tesseract with moveit wherever possible, particularly in header guards.
There are (still) several instances, where you write bt instead of bullet. As you seem to have decided for the latter, go for it consequently ;-)

moveit_core/CMakeLists.txt

moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h

moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common.h

moveit_core/collision_detection_bullet/CMakeLists.txt

...collision_detection_bullet/include/moveit/collision_detection_bullet/tesseract/basic_types.h

rhaschke · 2019-07-12T12:33:05Z

...collision_detection_bullet/include/moveit/collision_detection_bullet/tesseract/basic_types.h

+  {
+  }
+
+  const std::vector<std::string>& active;


clang-tidy --fix does this automatically. And yes, you should adapt all MoveIt code, otherwise Travis will complain anyway.

rhaschke · 2019-07-12T12:34:01Z

...collision_detection_bullet/include/moveit/collision_detection_bullet/tesseract/basic_types.h

+  const std::vector<std::string>& active;
+
+  /** \brief If after a positive broadphase check the distance is below this threshold, a contact is added. */
+  const double& contact_distance;


Shouldn't this be called broadphase_threshold then (according to the comment)?
A contact should only be added if there is a real contact...

No, this is not the broadphase threshold (which is another parameter specific to each collision object). For a broadphase candidate pair, their distance is calculated and if its below the contact_distance the distance and all other information is added as a result. In Bullet this is regarded as a contact. Note that contact does not necessarily refer to a real contact (distance < 0 is possible).

Contrary to FCL, Bullet does not make a difference between distance and collision requests which in my opinion is the cleaner solution with less duplicate code (the FCL MoveIt implementation has plenty of duplicate code because of this distinction). In Bullet, distance requests are simply treated as contacts with the only difference that the contact depth/distance is positive. So a contact is not necessarily a real collision. This wording is used throughout the Bullet Library.

If I get you correct, Bullet performs the detailed collision/distance check only if the broadphase check passed. So, if two objects fail the broadphase check (e.g. because they are nicely aligned and their axes-aligned bounding boxes don't overlap), their distance will not be reported?

No, they won't. But you can easily expand the AABB of each object by a distance (this is a parameter) and then they will be reported. So I could expand each AABB by 5 cm and then I will get for sure all objects which are closer than 5 cm and their respective distance.

moveit_core/collision_detection_bullet/test/test_bt_collision_detection_panda.cpp

* adapting variable names * license match MoveIt style * added ToDos

rhaschke

Thanks for the additional improvements! Some small remarks remain.

...collision_detection_bullet/include/moveit/collision_detection_bullet/tesseract/basic_types.h

moveit_core/package.xml

davetcoleman · 2019-07-16T19:58:10Z

Excited to seeing this close to ready! Travis is still unhappy...

* renaming COW to CollisionObjectWrapper * adding documentation

j-petit · 2019-07-17T10:29:25Z

Excited to seeing this close to ready! Travis is still unhappy...

Only kinetic is now failing (it does not need to pass, right?), the three other builds pass 🎉

rhaschke · 2019-07-17T14:03:45Z

Only kinetic is now failing.

Looks like Kinetic/Xenial ships with an older Bullet version. I suggest dropping Kinetic support for now.

davetcoleman · 2019-07-18T02:16:43Z

Moving this convo here: #1570
;-)

j-petit · 2019-07-23T09:49:32Z

Ping
Can we get this merged / further discussed / reviewed ?

j-petit · 2019-07-24T12:36:43Z

Thanks for reviewing, next up is #1551 for the continuous collision checking.

* Added discrete BVH Bullet manager * Added continuous collision detection * Cleanup of tesseract code * removed simple collision managers * changed enum to enum classes * fixed typos * removing debugging statements * removing tesseract allowed collision matrix * removed tesseract macros * replaced typedefs of stl containers * removed tesseract attached object code * ACM members of collision robot and world removed and ACM check into callback out of class * remove ContactTestType and replace through MoveIt CollisionRequest * BodyType int changed to enum * removed tesseract_msgs dependency and unnecessary tesseract code * changed dependency from bullet3_ros to debian Bullet package * Adding bullet and tesseract simple collision manager to the template: * added benchmark case for checking collision speed * tests for bullet collision checking * Adding missing features: * attached objects * contact reporting max number of contacts fixed * Bullet plugin xml name fixed * padding and scaling for robot added * updated tests

* Empty collision checker template for usage with tesseract and bullet (#1499) * Adding documentation to collision detection (#1488) * CMake adaptions for Tesseract integration * Empty collision detector template for tesseract and bullet * Making bullet as a collision plugin available * Adding missing error messages * Adding new folders and libraries to cmake * Fixing the BSD license * clang-format * Generic collision detection test suite (#1543) Generalize collision detection tests by using a templated test fixture. * Bullet Collision Detection (#1504) * Added discrete BVH Bullet manager * Added continuous collision detection * Cleanup of tesseract code * removed simple collision managers * changed enum to enum classes * fixed typos * removing debugging statements * removing tesseract allowed collision matrix * removed tesseract macros * replaced typedefs of stl containers * removed tesseract attached object code * ACM members of collision robot and world removed and ACM check into callback out of class * remove ContactTestType and replace through MoveIt CollisionRequest * BodyType int changed to enum * removed tesseract_msgs dependency and unnecessary tesseract code * changed dependency from bullet3_ros to debian Bullet package * Adding bullet and tesseract simple collision manager to the template: * added benchmark case for checking collision speed * tests for bullet collision checking * Adding missing features: * attached objects * contact reporting max number of contacts fixed * Bullet plugin xml name fixed * padding and scaling for robot added * updated tests * Adding continuous collision detection to Bullet (#1551) * Adding continous collision detection: * check only active links added * benchmark FCL vs Bullet * renaming files from bt to bullet * renaming variables from bt to bullet * clang-tidy and clang-format * renaming of variables to be in moveit format * continuous tests added * CCD active links changed * distance testing added to panda test suite * New collision features: * broadphase early culling * minimal distance reporting * renaming collision filter and group * removed extra margin on AABB for bullet * ACM in test from SRDF * PR review * Unified Collision Environment Bullet (#1572) * Templated tests adapted for unified collision env * Unified Bullet collision environment: * broadphase filtering adapted for early culling * ACM from SRDF in test * cleanup for bullet single collision env * removed link2castcow for CCD and only use constructor of COW directly * parent class for collision managers * removed function pointer for ACM check * removed extra self-collision manager and use only single manager * speed benchmark for unified environment * PR review: * more descriptive variable names * added user to TODO * PR review: * shortening namespace * documentation improvements * virtual destructor of BVH manager * remove extra speed benchmark * bugfix for not initialized managers * Licenses revised of old tesseract files * PR review: * replaced include guards through pragma * used default instead of empty {} for ctor/dtor * Comments in Bullet readme about thread safety and speed * Readme for speed benchmark (#1648) * FCL Bullet benchmark readme * Benchmark script and launch file updated * PR review fixups * Adapt cmake for Bullet (#1744) * cmake for using Bullet only if correct version available * Fixup * PR review fixup * Rewrite with own FindBullet.cmake * Use pkg-config * Fix travis failure for melodic * Fix moveit_ros_planning dependency on Bullet * Rename PR2-related collision test files (#1856)

j-petit requested review from davetcoleman, henningkayser, rhaschke and v4hn as code owners June 14, 2019 16:16

j-petit mentioned this pull request Jun 14, 2019

Adding Bullet and Tesseract into MoveIt [WIP] #1500

Closed

j-petit requested a review from BryceStevenWilley as a code owner June 20, 2019 13:49

j-petit mentioned this pull request Jun 21, 2019

GSoC: Improved Collision Checking #1427

Open

j-petit force-pushed the integrating_bullet branch from 9dd093c to c155118 Compare June 24, 2019 15:51

BryceStevenWilley suggested changes Jun 26, 2019

View reviewed changes

j-petit changed the base branch from master to feature-bullet-collision June 26, 2019 07:24

j-petit force-pushed the integrating_bullet branch from 0577b4b to 4797bc4 Compare June 27, 2019 08:02

BryceStevenWilley approved these changes Jul 2, 2019

View reviewed changes

j-petit force-pushed the integrating_bullet branch from 6fd7094 to 7062244 Compare July 2, 2019 12:46

BryceStevenWilley closed this Jul 3, 2019

BryceStevenWilley reopened this Jul 3, 2019

j-petit force-pushed the integrating_bullet branch 2 times, most recently from 43b0d5c to 3ab813e Compare July 4, 2019 09:09

BryceStevenWilley reviewed Jul 8, 2019

View reviewed changes

j-petit force-pushed the integrating_bullet branch 2 times, most recently from 2d46eb5 to fd1c53f Compare July 8, 2019 15:29

BryceStevenWilley closed this Jul 8, 2019

BryceStevenWilley reopened this Jul 8, 2019

j-petit force-pushed the integrating_bullet branch 2 times, most recently from 607fc77 to 8cca933 Compare July 8, 2019 15:57

j-petit closed this Jul 8, 2019

j-petit reopened this Jul 8, 2019

j-petit force-pushed the integrating_bullet branch 2 times, most recently from 9c4f2a5 to 01fdb79 Compare July 9, 2019 16:00

BryceStevenWilley changed the title ~~Adding Bullet and Tesseract into MoveIt [WIP]~~ Adding Bullet and Tesseract into MoveIt Jul 9, 2019

rhaschke reviewed Jul 12, 2019

View reviewed changes

j-petit added 7 commits July 12, 2019 16:00

Renaming all files from bt to bullet

0506401

Renaming bt to bullet

099685f

clang-tidy and clang-format

1d55f88

PR review:

5d8b9ab

* adapting variable names * license match MoveIt style * added ToDos

Added panda typed test for FCL and Bullet

30c1d5e

Added documentation to tesseract code

fc6c1f1

removed eigen_conversions

444a7bf

rhaschke reviewed Jul 15, 2019

View reviewed changes

...collision_detection_bullet/include/moveit/collision_detection_bullet/tesseract/basic_types.h Outdated Show resolved Hide resolved

moveit_core/package.xml Outdated Show resolved Hide resolved

j-petit added 5 commits July 17, 2019 09:49

Check only active links added

88ff0cc

eigen_conversions removed

cfd0d92

World collision test disabled

6c4d080

PR review:

dc7a29f

* renaming COW to CollisionObjectWrapper * adding documentation

Renamed tesseract folder

38851bb

davetcoleman mentioned this pull request Jul 18, 2019

Dropping MoveIt 'master' branch support of Ubuntu 16.04 #1570

Closed

j-petit mentioned this pull request Jul 18, 2019

Unified Collision Environment Bullet #1572

Merged

drop kinetic support

bd9f660

rhaschke merged commit c143e20 into moveit:feature-bullet-collision Jul 23, 2019

j-petit deleted the integrating_bullet branch August 22, 2019 08:21

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Adding Bullet and Tesseract into MoveIt #1504

Adding Bullet and Tesseract into MoveIt #1504

j-petit commented Jun 14, 2019 •

edited

BryceStevenWilley left a comment

BryceStevenWilley left a comment

BryceStevenWilley commented Jul 4, 2019

BryceStevenWilley left a comment

rhaschke left a comment

rhaschke Jul 12, 2019

rhaschke Jul 12, 2019

j-petit Jul 15, 2019 •

edited

rhaschke Jul 15, 2019

j-petit Jul 15, 2019 •

edited

rhaschke left a comment

davetcoleman commented Jul 16, 2019

j-petit commented Jul 17, 2019 •

edited

rhaschke commented Jul 17, 2019

davetcoleman commented Jul 18, 2019

j-petit commented Jul 23, 2019

j-petit commented Jul 24, 2019

Adding Bullet and Tesseract into MoveIt #1504

Adding Bullet and Tesseract into MoveIt #1504

Conversation

j-petit commented Jun 14, 2019 • edited

Description

BryceStevenWilley left a comment

Choose a reason for hiding this comment

BryceStevenWilley left a comment

Choose a reason for hiding this comment

BryceStevenWilley commented Jul 4, 2019

BryceStevenWilley left a comment

Choose a reason for hiding this comment

rhaschke left a comment

Choose a reason for hiding this comment

rhaschke Jul 12, 2019

Choose a reason for hiding this comment

rhaschke Jul 12, 2019

Choose a reason for hiding this comment

j-petit Jul 15, 2019 • edited

Choose a reason for hiding this comment

rhaschke Jul 15, 2019

Choose a reason for hiding this comment

j-petit Jul 15, 2019 • edited

Choose a reason for hiding this comment

rhaschke left a comment

Choose a reason for hiding this comment

davetcoleman commented Jul 16, 2019

j-petit commented Jul 17, 2019 • edited

rhaschke commented Jul 17, 2019

davetcoleman commented Jul 18, 2019

j-petit commented Jul 23, 2019

j-petit commented Jul 24, 2019

j-petit commented Jun 14, 2019 •

edited

j-petit Jul 15, 2019 •

edited

j-petit Jul 15, 2019 •

edited

j-petit commented Jul 17, 2019 •

edited