Moveit: GSoC: Improved Collision Checking

[j-petit]

GSoC 19: Implement continuous collision checking and integrate Bullet

Mentors: @felixvd, @BryceStevenWilley.

This top post of the Github issue bundles all relevant information about the GSoC project. It is not necessary to go through all comments although many provide deeper insights into problems and discussions which occurred during the project.

The coding period started on May 26th and ended on August 28th

Initial Project Description

MoveIt currently only implements discrete collision checking for movements of the controlled robot. A major drawback of discrete collision checking methods is that they may miss collisions between the sampled time steps. While there exists techniques to alleviate this problem, resulting algorithms can be relatively slow. To provide stronger guarantees, continuous collision detection (CCD) techniques have been proposed by the research community. They compute the first time of contact between two moving objects along a path.

The planning framework Tesseract of ROS-Industrial has implemented CCD utilizing the Bullet library. The goal of the project is to port the feature from Tesseract to MoveIt which includes integrating Bullet as a new available collision checker. Optimizations to the collision checking process itself to improve performance (cache/scene reconstruction improvements, convex-convex collisions)

Besides API changes, the project includes writing tests and tutorials showing the new capabilities.

Original Project Timeline (drafted before GSoC start in May)

• June 10th (3-4 weeks): a draft PR proof-of-concept making a Bullet collision checking plugin.
• July 8th (another 4 weeks): ready to review PRs that (if necessary) change the CollisionPlugin/CollisionRobot/CollisionWorld interfaces, and also add Bullet as a CollisionPlugin.
• July 29th (3 more weeks): Start efforts trying to add CCD via Convex hulls to FCL, so it as well could support TrajOpt and the additional convex-convex collisions.
• August 12th - August 16th: I will be absent.
• August 19th (my last week with GSoC): Document collision plugins in depth, detailing how to write your own, and how to select which plugin is used at runtime.

Delivered Code Overview

• PR #1499 Empty template bullet checker
• PR #1504 Integrating bullet without CCD
• PR #1551 Adding CCD to bullet
• PR #1543 Generic templated collision test suite
• PR #1584 Unified collision environment including other affected satellite repos here and here
• PR #1572 Unified Bullet collision environment

Delivered Code Details

My work can be roughly grouped into two big parts which are partially interconnected.

1: Integrating Bullet and CCD (PR #1499, #1504, #1551)

This part mainly dealt with porting the available Tesseract code. I started out from a full fork of the Tesseract project which was built in the same catkin_ws before moving all relevant code into moveit_core/collision_detection_bullet and removing piece by piece unnecessary code and adding new glue code for the MoveIt integration. With #1551, the CCD was integrated and working. As I gained a deeper understanding of the collision checking process, we started the discussion about a unified collision environment. This finally led to the decision to restructure major parts of MoveIt's collision checking in the second work package.

2: Unified general collision environment (PR #1584)

Before GSoC, MoveIt's collision environment was split into two parts: CollisionRobot and CollisionWorld. However, a unified collision environment containing both has advantages: we can leverage the full potential of Bullet as a collision detector and reduce the complexity of the collision checking code in general. Consequently, we decided to create a new unified collision environment.
For more details on this decision please see the top post of the PR #1584. To make this change happen, I had to unify all existing collision detectors which included FCL, a signed distance field, a hybrid between signed distance / FCL and a dummy one returning always no collision. Furthermore, many changes in the planning scene were necessary which in turn then required changes throughout the codebase because the planning scene is such a central class.

1+2: Unified Bullet collision environment (PR #1572)

The last step, combining part one and two, was creating a unified Bullet collision environment. This leads to a reduction in complexity of the original Tesseract code (which has been changed in many places) and to considerable speed improvements when using Bullet. I benchmarked them in this post.

Documentation

• Bullet Tutorial
• Readme for speed benchmark script in PR #1648
• Documentation PRs #1488, #1505, #1645 for original FCL collision code
• Developers' concepts page explaining the collision detection with flowcharts
• Bullet Tutorial PR

Possible Future Work

I summarized some possible improvements to the collision detection process in this issue. Please see the issue for elaborations about the items. They mainly concern speed as well as maintainability:

• the allowed collision matrix (ACM) as a member of the collision environment
• continuous self-collision checks
• refactoring the other collision detectors (FCL, signed distance field, hybrid)
• removing the distinction between distance and collision requests
• better use Bullet specific capabilities
• more extensive benchmarking of Bullet and identification of bottlenecks for further speed tuning
• integration of CCD with TrajOpt motion planning (see related issues further down)

Additional resources

• my GSoC log
• initial GSoC discussion
• comparison TrajOpt+Tesseract to MoveIt
• collision checking MoveIt FCL flowchart (integrated into developers concepts page)
• collision checking MoveIt Bullet flowchart (integrated into developers concepts page)
• detailed bullet collision checking flowchart (integrated into developers concepts page)
• theoretical introduction to collision detection

Related issues / projects

• #1467, #1593, #1626: PikNick summer intern implementing TrajOpt path planning

[welcome[bot]]

Thanks for reporting an issue. We will have a look asap. If you can think of a fix, please consider providing it as a pull request.

[j-petit]

The cache/scene reconstruction improvements mentioned in 1) are not yet clear to me - @felixvd, could you give some more details?

[simonschmeisser]

trying to clarify your question: currently MoveIt (actually the fcl collision checking plugin for MoveIt) does separate collision requests for each NxN combinations of objects in the PlanningScene. Fcl (and other collision checkers) also offer broad phase collision checking that quickly determines which parts of the scene are actually somewhat close (based on tree structures etc). This would reduce the need for some overlap checking. The first part of the narrow phase collision checking that follows afterwards is AABB based overlap checking which is still quite cheap. So this will probably bring some speedup but not incredible amounts ;)

[simonschmeisser]

about convex/convex: this seems to have been integrated in fcl recently (@Levi-Armstrong knows more details) but is not yet used in any way by MoveIt. There are open (and stalled) discussions to add a "convex" tag to urdf https://github.com/ros/urdfdom/issues/108 and https://discourse.ros.org/t/add-attribute-to-indicate-if-a-mesh-is-a-convex-hull/4479/12

There are unfortunately no PRs for this so far

[simonschmeisser]

For Proposal 2) @BryceStevenWilley also has a branch on integrating trajopt in OMPL, what is your status/conclusion there? https://bitbucket.org/ompl/ompl/branch/TrajOpt

[felixvd]

I thought you're asking how the project affects the scene representation in MoveIt. As a quick overview, the scene is represented in MoveIt's PlanningScene as a RobotState and a world. The RobotState contains the robot links (including all fixed shapes defined in the URDF) and all attached items, while the world contains items spawned dynamically, like Octomaps from a depth camera or CollisionObjects that the robot may pick up/attach.

My understanding is that Tesseract combined the PlanningScene subunits into a BasicEnv class and also changed the CollisionObject msg into an AttachableObject msg that contains VisualGeometry and CollisionGeometry.

I imagined porting at least the CollisionObject/AttachableObject changes first, to smooth the next transitions, but I might be thinking that because of this open PR that used those messages a lot. Either way, I think changing the PlanningScene to the BasicEnv class would be too broad a change, but the extensions to the collision checking would fit there and in collision::world.

Would be keen to hear @Levi-Armstrong and @BryceStevenWilley opinions.

[j-petit]

If someone has time to spare, I wrote my application following the OSRF template. Feel free to comment in the document.

[simonschmeisser]

I think you need to focus on (a subset of) one of the two proposals in order to be able to finish within the given time frame. Giving some more details of what you want to achieve how also wouldn't hurt.

[j-petit]

I would be happy to especially focus on the "Continuous Collision Checking" part of the proposed project. For me it is difficult the judge the scope of the project, as my experience with MoveIt is only limited. I added the following text to my application and will extend it with more specific goals until the application deadline.

MoveIt currently only implements discrete collision checking for movements of the controlled robot arm. A major drawback of discrete collision checking methods is that they may miss collisions between the sampled time steps. While there exists techniques to alleviate this problem, resulting algorithms can be relatively slow. To provide stronger guarantees, continuous collision detection (CCD) techniques have been proposed by the research community. They compute the first time of contact between two moving objects along a path.
The new planning framework Tesseract of ROS-Industrial has implemented CCD utilizing the Bullet library. The aim of this project is porting the feature from Tesseract to MoveIt. As Tesseract draws its heritage back to MoveIt, both motion planning frameworks share similarities which makes porting feasible.

---

FCL also implements CCD (as far as I understood from this paper). Why not utilize then FCL instead of Bullet?

[BryceStevenWilley]

@simonschmeisser Yeah, I did a lot of work for that a while back. In short, most of TrajOpt is easily portable (could do with some quality updates, but nothing impossible), but the actual optimization software used is a bit confusing. The only openly available library is BPMPD, of which the source code is not available. The other alternative is Gurobi, which is only free for academics. BPMPD is definitely usable (it's what's tesseract is using now), but as a stretch goal, it should probably change to IPOPT or Ceres. The rest of TrajOpt isn't too different though; it should be similar to how CHOMP and STOMP are interfacing with MoveIt now.

@j-petit There's been some discussion on this at https://github.com/ros-planning/moveit/issues/29#issuecomment-239026362. However, there's a key difference between what FCL is doing for CCD and what tesseract/TrajOpt uses Bullet for. IIRC, FCL is calculating the smallest distance it can travel to guarantee it won't miss any collisions. However, with Bullet, we would be taking a link at two different poses and "casting" a convex hull around those poses, resulting in a single new mesh to collision check. This is described best in the TrajOpt paper. There's some approximation error there as well, but the key idea is mostly to speed up collision checking.

[tsijs]

If I could blend in here:
I know that the package @Levi-Armstrong maintains with their own "tesseract" environment representation also has qpoases and gurobi as optional solvers, of which qpoases is completely open source.
They were discussing to use Casadi here (https://github.com/ros-industrial-consortium/trajopt_ros/issues/67) as an interface instead of interfacing all solvers manually. Casadi supports a variety of solvers and the interfacing by using its callback API isn't too hard.
It might be something to consider if somebody is going to port this code to a moveit plugin anyway.

[felixvd]

@tsijs Thanks for the heads up. Sounds like it would also be interesting to know how your efforts to use TrajOpt with MoveIt went. What other packages did you find need to be modified and why? It sounds like there is a risk of duplicating some work. If you have time to share, please feel free.

[j-petit]

I have been selected for the GSoC 2019 :)! Thanks to the community for the valuable feedback during writing my project application. Looking forward to work together with my mentor @felixvd. Did MoveIt get a second student? In the next days, I will deepen my understanding of the project and hopefully come back with many questions / details to discuss.

[felixvd]

Congratulations! @BryceStevenWilley and I will mentor you together, just for the record :)

MoveIt sadly only got one GSoC student this year, but there are a few more interns at PickNik working on MoveIt as well. We are checking if it makes sense to share a Slack. I note that you are in time zone GMT+1, I am in GMT+8 (JST) and Bryce is somewhere in the US, so live calls might be tricky.

For preparation, I would suggest reading through and trying out Tesseract yourself and implementing a test scene there and in MoveIt. This PR might end up being related if you work on the PlanningScene, too.

Personally I think it would be perfectly appropriate and helpful to keep an open running log of what you understood of both MoveIt's and Tesseract's architecture here. It doesn't have to be perfect or even correct, but it should help you organize, share and confirm your ideas, and will be a good point of reference for later. So, feel free to drop your thoughts often.

[BryceStevenWilley]

Congrats j-petit! I second everything Felix just said, especially about dropping your thoughts here; hopefully a lot of people who have relevant knowledge will start watching this thread and can give advice when necessary.

I'd also suggest getting familiar with FCL and Bullet, mostly just going through the tutorials, maybe looking at some advanced user documentation, and noting any API differences.

(Edit: I'm in GMT - 4)

[j-petit]

Thanks for the advice, then I'll use this issue to log my activities. Just for the record: I am in GMT +2 because of Daylight Saving Time.

[simonschmeisser]

Nice! I'm interested in helping out with mentoring where needed and based in Freiburg, Germany (+2 as well)

[j-petit]

Some thoughts / questions. I decided to drop them more quickly instead of summarizing them into larger posts, if you think another style is better let me know:

• Dependencies between Project 1) and 2): TrajOpt is not only concerned with collision detection but a full path planning approach. Is it the aim of my project to integrate all of it? I don't see yet how we can only integrate the continuous collision detection without integrating TrajOpt as a full new motion planner. Any thoughts? Or is / will someone else work on this topic (PickNik intern etc. as no other GSoC student)?
• Understanding the details of the TrajOpt algorithm is not too important right now. Better focusing on the APIs of the involved frameworks and different libraries - the big picture in general.

- Which conversations should happen in public (this issue?) and which for example in the private slack channel?

I will also log my activity for GSoC in this Google Doc.

[BryceStevenWilley]

For your first week or so, I'd say it's better to do quick daily posts like this; quicker feedback means quicker iteration for you.

• Projects 1 and 2, in their entirety (with integration, cleaning code, optimization, and documentation) were intended to be full projects for 2 students, and would be a lot of work to do yourself. You should focus on Project 1 (collision detection), and depending on how far you get, we can reevaluate.
  
  
  
  • Sub point, continuous collision detection can definitely be integrated with existing motion planners! CollisionWorld already offers different functions for continuous functions, simply changing those calls to do actual continuous motion and not very fine discrete checks would speed-up/make motions safer. There are a lot of tradeoffs here, we can talk more about those soon.
  
  
• Agreed. Understanding the algorithms used in collision checking will be pretty important for you like GJK and various bounding volume hierarchies, but for the moment, I'd focus an the APIs and the libraries.
• Posts like the one you just made are great for this issue. But if you having trouble understanding a particular piece of code, running into a weird installation/implementation bug, or have a small questions that's blocking you, I'd through those in the Slack so Felix and I can respond a little quicker and get you un-blocked faster.

[j-petit]

For my project, how much do I need to understand of the actual implementation of TrajOpt+Tesseract in its current form? I am working on making a comparison [WIP] between TrajOpt+Tesseract and MoveIt, do you think this is a good thing to be working on right now?

[BryceStevenWilley]

That comparison would be very useful, and definitely a good focus for the moment.

[Levi-Armstrong]

@j-petit I have been working with TrajOpt and Tesseract for over a year and wanted to provide some insight into some things you will will need to address for TrajOpt to be full integrated into MoveIt. Currently TrajOpt collision cost/constraint requires the minimum translation vector between two objects in order to know how to move these objects out of collision. This is currently provided GJK which is used for checking ConvexHull to ConvexHull collision shapes. What this means is that when using TrajOpt you can not check a detailed mesh to detailed mesh because it will not produce sufficient information for TrajOpt to function correctly.

Currently convex hulls collision shapes are not support by urdfdom and urdfdom_header. I currently have an developer working on a PR for urdfdom and urdfdom_header to hopefully address this issue assuming the maintainers are supportive of adding new geometry types.

Another thing I found is that when loading mesh files, it is using assimp load the data and then it flattens all shapes in the file into a single triangle list. This is ok for visual objects and detailed mesh collision shapes but is not ok for convex shapes. In the case of the convex shapes you would want it to represent each shape in the file as its own collision object.

I believe these are the main items I have found outside of MoveIt that would need to be address for it to be fully supported. I hope this helps.

Also wanted to give you a head up that we will be refactoring TrajOpt in the very near future to improve the flexibility of the planner and integrate new optimization techniques provided by CASADI, PAGMO, etc..

[felixvd]

Thanks for the input!

I agree that continuous collision detection without TrajOpt makes sense to focus on first. If changes to the CollisionObject and PlanningScene interface make that easier, those may be good to think about, because they are probably worth doing anyway.

[j-petit]

To make this clear one more time for me: When we talk about integrating CCD into MoveIt, we assume that we want to do it by using the underlying algorithms of TrajOpt (e.g. similarly to casting the convex hull over two discrete poses and check this for collisions)?

[Levi-Armstrong]

In the case of motion planning I think casting the collision objects provides better information about how to move two moving objects out of collision.

[BryceStevenWilley]

@j-petit yes, in this context, we're talking about integrating CCD by casting convex hulls. It's a stepping stone towards integrating TrajOpt, but it has its own independent uses as well.

Thanks for the input Levi! I do agree that it's useful for moving objects out of collision. However, there is a rotation error that happens when using CCD via CH, so we do need to be clear that it's slightly different than normal CCD, such as that discussed in https://github.com/ros-planning/moveit/issues/29#issuecomment-238347670

[j-petit]

What is the difference between collision_detection_fcl and collision_detection? There seems to be some overlap in the header files I linked. I thought that all collision detection is processed through the structure given by FCL? Could someone clarify that for me?

[rhaschke]

The collision checking is designed as a plugin: collision_detection defines the API, while collision_detection_fcl is (one) concrete implementation of this API using FCL.
In principle, it should be possible to replace FCL with another collision checker, e.g. Bullet, but we are not aware of an alternative implementation of the API.

[simonschmeisser]

There is a dummy one that always returns no collision ...

[j-petit]

TrajOpt: For bringing CCD to MoveIt we don't need to solve the optimization problem described in the paper? Am I correct in this conclusion?

[BryceStevenWilley]

You are correct there. There is a lot of information in the paper about adapting CCD to be used by the optimization, that's only if you are using an optimization-based planner.

[j-petit]

I have some questions concerning the support mapping as defined in the TrajOpt paper:

• Does the coordinate system in which the object is defined matter?
• When it is stated that the support mapping implicitly represents the object, does this mean that if we know the mappings result for all directions, we could reconstruct the object?
• For the support mapping of the convex hull, the formula (see picture below) is presented in the paper without any further explanation. I have some difficulties getting my head around the intuition behind it.
  

General issues:

• Do we want to support the CCD only for convex polytopes?
• For CCD, we need to calculate the signed distance and therefore compute the distance between two shapes via GJK and penetration depth via EPA. In Tesseract, Bullet is used for this. FCL uses implementations via libccd. Why is it not possible to keep using libccd? We always talked about (partially) integrating Bullet into MoveIt.

[felixvd]

Caveat: I'm just sharing some intuition and corrections and additions are always welcome.

• Does the coordinate system in which the object is defined matter?

It should not.

• When it is stated that the support mapping implicitly represents the object, does this mean that if we know the mappings result for all directions, we could reconstruct the object?

If it is convex, yes. There would be no unique mapping to flat surfaces, but those would be implicitly defined by the edges. The support mapping of a cube would return only points on the edges (or for 6 some directions be ill defined?), for example.

• For the support mapping of the convex hull, the formula (see picture below) is presented in the paper without any further explanation. I have some difficulties getting my head around the intuition behind it.
  

You can read as a measure of distance, except that it is still conveniently signed so that the equation works out. It's a signed distance in the direction of from the origin. The term is positive when points are "behind" the origin and negative when the origin is "behind" the points when looking in the direction .

I scribbled a really simple example down here.

Another way to look at it: is a point in space, projects the point along the direction , and is the euclidean distance of that point to the origin. The term inside that norm can be used to compare the distance along the projected direction.

I feel like this is not perfectly intuitive either, but considering what the dot product means geometrically probably helps. Maybe someone has an easier way to put things.

edit: I think this page about vector projection should answer some questions.

General issues:

• Do we want to support the CCD only for convex polytopes?

It is my understanding that this method only works for convex shapes.

[BryceStevenWilley]

Does the coordinate system in which the object is defined matter?

Not sure how exactly to answer the question: for the support vectors, I don't think the coordinate system matters as long as both objects (both link placements for the convex hull, or the link and the object) are in the same system, i.e. the vector v points in the same direction for each object.

Felix gave some good answers for the support vector questions, don't think I could add much there.

Do we want to support the CCD only for convex polytopes?

For the moment, yes. We could also consider being able to specify multiple convex polytopes per link as to approximate a nonconvex link.

Why is it not possible to keep using libccd? We always talked about (partially) integrating Bullet into MoveIt

We should try to use both! Being able to use different collision checking plugins has never been well developed, as we really only have FCL working properly (spheres and distance fields don't work quite as well), and it's definitely interesting to get both work. From what I understand, Bullet might also have some performance benefits when compared to FCL.
In addition, I've personally run into some issues with FCL and signed distance in the past. These issues might be fixed, but if they're not, it might be more straight-forward to just use the Bullet library instead of finding and fixing those issues in FCL itself. (~Also small clarification, I believe Bullet uses libccd internally as well, at least according to libccd~ See Levi's comment below).

[Levi-Armstrong]

Does the coordinate system in which the object is defined matter?

As @BryceStevenWilley stated it does not matter as long as both are in the same coordinate system. That said, most of this is dictated by the library, like FCL and Bullet. In both these cases they are represented in a global coordinate system.

Do we want to support the CCD only for convex polytopes?

I agree, this provides useful information and is significantly faster than triangle mesh to triangle mesh in most cases. The way both Bullet and FCL represent detailed meshes, are as a bounding volume hierarchy (BVH) of triangles. Then all other shapes supported are convex shapes and since a triangle is a convex shape it leveraged CCD.

Why is it not possible to keep using libccd? We always talked about (partially) integrating Bullet into MoveIt

When discussing collision checking you have two parts, Broad Phase Collision checking and Narrow Phase Collision checking. The Narrow Phase Collision checking is what libccd performs which is checking the two objects for collision. The Broad Phase Collision checking is the BVH check. In the both Bullet and FCL when you add a collision shape it gets added to global BVH. Also some collision shapes themselves are represented by BVH of convex shape. For example, triangle meshes are represented by a BVH of triangle shapes. A octomap is represented by a BVH of box shapes. Once all shapes have been added and locations set. The libraries perform two operation. First they check each collision shape against the global BVH and if the bounding box of the shape intersects the bounding box of the another shape it then and only then enters into the narrow phase collision check (This is the most computation expense operation). By performing the Broad Phase check first you avoid doing unnecessary Narrow Phase collision checks.

Bullet has rewritten libccd or written its own within its API and does not depend on libccd but may have in the past. In the case of FCL it depend only certain components of libccd but most of it has been rewritten to handle degenerate case in CCD.

I personally, would not recommend using libccd directly as it provides little value without the Broad Phase component. I would recommend, working directly with FCL and Bullet to resolve any issues found because to write your own you would run into the same degenerate case to be resolved.

We always talked about (partially) integrating Bullet into MoveIt.

I not sure what is meant by (partially) integrated, but bullet can be fully integrated into MoveIt assuming the issues are address described in my previous comment. I have fully integrated both within tesseract_collision which could be directly used within MoveIt with minor contact data translation. We will be releasing a new version of the core packages which includes tesseract_collision as ROS agnostic libraries to support integration like this. If MoveIt does not want to depend on tesseract_collision then I would propose copy the code directly into MoveIt and go from there.

[felixvd]

Regarding @j-petit 's question about the support mapping of the convex hull:

I took the time to draw this out because another friend of mine was also confused. The fundamental idea to grasp is just that the dot product projects vectors onto each other. In this case, just read "a dot product projects a point onto a vector". Take any two points in cartesian space:

If you multiply (dot product) each point with the unit vector (1, 0), you project each point onto the x-axis. Trivially, that is the x-coordinate of each point.

However, you can do this with any direction, and imagine a new axis, like this:

The points are projected onto the new axis in the same way. The dot product's result is the position on the axis. The opposite direction just turns the axis around:

Knowing this, it should become clear that the formula you quoted simply says "take whichever point is furthest in the direction v" or "choose the point that has the highest value on the axis v":

PS: This also works for non-unit vectors, but they introduce distortion, so it does not look as pretty.

[j-petit]

Thanks for the explanations @BryceStevenWilley, @felixvd and @Levi-Armstrong!

[j-petit]

Integrating Bullet into MoveIt

I am getting an overview what needs to be done in order to support Bullet for collision checking. Feel free to add / comment. My findings so far:

Interface implementations

• collision_detection::CollisionDetectorAllocator
• collision_detection::CollisionPlugin
• collision_detection::CollisionRobot
• collision_detection::CollisionWorld

General classes

• collision object wrapper which wraps Bullet collision objects (analog to collision_detection::FCLObject; in Tesseract CollisionObjectWrapper)
• collision manager analog to collision_detection::FCLManager (in Tesseract BulletDiscreteBVHManager and BulletCastBVHManager
• collision object geometry representation analog to collision_detection::FCLGeometry

[BryceStevenWilley]

@j-petit, @felixvd, @davetcoleman @ommmid, and I just had a call discussing this effort, and we came up with some milestones we want to hit. These will be reevaluated along the way, and, depending on how feasible these targets are, the dates might change.

• June 10th (3-4 weeks): a draft PR proof-of-concept making a Bullet collision checking plugin. This PR might be a bit messy, and will most likely change some CollisionPlugin interfaces. The idea is to quickly prove that it's possible to have a Bullet collision checker in MoveIt, without having make everything perfect. In this PR, we'd include CCD via convex hulls.
• July 8th (another 4 weeks): a ready to review PRs that (if necessary) change the CollisionPlugin/CollisionRobot/CollisionWorld interfaces, and also add Bullet as a CollisionPlugin. Majority of this work will be properly changing the Collision Plugin interface for the existing collision plugins, and including the robot and world in the same collision scene, as discussed here and here.
• July 29th (3 more weeks): Start efforts trying to add CCD via Convex hulls to FCL, so it as well could support TrajOpt and the additional convex-convex collisions.
• August 14th (Jens' last week with GSOC): Document collision plugins in depth, detailing how to write your own, and how to select which plugin is used at runtime.

[j-petit]

High level understanding of collision detection in MoveIt:

Some passages are copied from the docstrings. See comparison document for more details.

Each PlanningScene has a map member PlanningScene::collision_ which contains possible multiple CollisionDetectors. CollisionDetectors bundle CollisionWorld and CollisionRobot of a PlanningScene.

A collision detector type is specified with an allocator which is a subclass of CollisionDetectorAllocator. This identifies a combination of CollisionWorld/CollisionRobot which can be used together. A new PlanningScene contains an FCL collision detector. The member PlanningScene::active_collision_ points to the active collision detector.

Two collision cases are distinguished in MoveIt. Collisions between robot-world and self-collisions/robot-robot. This is the motivation between having CollisionWorld and CollisionRobot.

When collision checks are expected from the planning scene (planning_scene->checkCollision()), then the active collision detector and with its CollisionWorld and CollisionRobot is returned and then the checks are performed.

Open questions:

I don't fully understand what the purpose of the CollisionDetectorAllocator is. Is this simply a wrapper which provides functionality to handle the CollisionWorld / CollisionRobot pair?

[BryceStevenWilley]

Solid summary.

A new PlanningScene contains an FCL collision detector.

This is something that I came across a while back: the call is right here. A minor nit, but IMO, this should be a part of the PlanningScene constructor, and that input should default to the FCL detector, i.e. I think dependency injection is a better idea here.

I don't fully understand what the purpose of the CollisionDetectorAllocator is. Is this simply a wrapper which provides functionality to handle the CollisionWorld / CollisionRobot pair?

To the best of my knowledge, yes. In some cases, you could have a collision world that is represented by a distance field, and a collision robot that is a standard mesh based robot. Trying to collision check those two would be difficult, and general, programming every possible CollisionRobot to work with every other CollisionWorld would be intractable. So by always allocating these from the CollisionDetectorAllocator, you avoid those problems.

[BryceStevenWilley]

During the call on Thursday, I mentioned that I'd give @j-petit some clear direction on where to start his first week of GSoC. Putting it here, so anyone can feel free to add comments.

A good deliverable that you could have for this would be to have (or at least have significant progress on) a simple bullet collision checker, something equivalent to Tesseract's Bullet Discrete Simple Manager. This will help you start simple, and you can also bootstrap the tests by reusing the FCL tests. Since this isn't adding new functionality to MoveIt, the majority of the development work will be porting tesseract's code here, and the biggest difference is that most of the object addition/removal/updates should go through the notifyObjectChange function, as compared to Tesseract's individual add collision object, set collision object transform, etc. (question for other maintainers: Should that belong to the CollisionWorld class? I can imagine a lot of MoveIt infra relying on the fact that planning scene updates are passed to the collision world like this). I think the best place for you to start is to take another close look at how the collision manager is handled in CollisionWorldFCL, I imagine that the collision manager in Bullet would be updated in a very similar way.

Some other thoughts for the upcoming weeks:

• After adding a discrete collision detector, you can focus on reimplementing checkRobotCollision between two different states to use the convex hull casting, dealing with the convex tags and assumptions then, so you don't have to worry about that right off the bat.
• Once we have a working CCD collision checker, we can get some performance metrics between all of the current checkers on some varying scenes (empty, normal collision objects, and very dense). There's a pretty good script for this here (which was very well hidden).
• Then, we can start experimenting with how to combine the Collision Robot and the Collision World, and compare and quantify any performance gains that we get out of that. Once we have the numbers, I think we'd have a good idea about how to proceed with major changes to the MoveIt API, if necessary.

[j-petit]

A new PlanningScene contains an FCL collision detector.

This is something that I came across a while back: the call is right here. A minor nit, but IMO, this should be a part of the PlanningScene constructor, and that input should default to the FCL detector, i.e. I think dependency injection is a better idea here.

Should I open a separate issue / PR for discussing this?

---

For integrating Bullet, should I use the version modified for Tesseract (purpose: adding threshold for returning AABB-broadphase collision checked objects) or the standard one provided by e.g.apt-get install libbullet2.87 (Ubuntu 18.04)?
Edit: Looking at this commit, this issue and the forked repo of @Levi-Armstrong, it looks like we can use the provided bullet package as the needed functionality was integrated in 2018. I haven't found how to use bullet for this purpose though.

[simonschmeisser]

Without having any further insight in changes between bullet2 and bullet3 I'd suggest using bullet3 as it has a CUDA backend which might be fun. :wink:

@BryceStevenWilley there was talk about modifying CollisionRobotFCL such that it exposes the whole collision hierarchy to FCL instead of doing n² individual queries. This would then give us broadphase collision checking. Do you think that would fit within the scope?

[j-petit]

Without having any further insight in changes between bullet2 and bullet3 I'd suggest using bullet3 as it has a CUDA backend which might be fun.

When I use libbullet2.87, this is bullet3, right?

[j-petit]

I looked into the environment representation / handling of MoveIt today. My understanding so far:

WorldPtr collision_detection::CollisionWorld::world_ contains the environment representation. When instantiating CollisionWorldFCL, an observer is created which tracks any handling of objects in the environment representation. The observer has as a member the function pointer to a callback fcn. Any action which manipulates an object, notifies the observer. The observer can be notified through different functions like collision_detection::World::notifyObserverAllObjects, collision_detection::World::notify(...), collision_detection::World::notifyAll(...) which all in the end invoke the callback which updates the specific to FCL CollisionWorld.fcl_objects_ representation of the environment as well as the FCLCollisionManager. This ensures that these representation are always up-to-date.

For the actual collision check, it suffices then to execute collide() of the FCL collision manager. The robot itself is passed as an argument (fcl_object) which is constructed from CollisionRobot and the RobotState.

[j-petit]

During the call on Thursday, I mentioned that I'd give @j-petit some clear direction on where to start his first week of GSoC. Putting it here, so anyone can feel free to add comments.

A good deliverable that you could have for this would be to have (or at least have significant progress on) a simple bullet collision checker, something equivalent to Tesseract's Bullet Discrete Simple Manager. This will help you start simple, and you can also bootstrap the tests by reusing the FCL tests. Since this isn't adding new functionality to MoveIt, the majority of the development work will be porting tesseract's code here, and the biggest difference is that most of the object addition/removal/updates should go through the notifyObjectChange function, as compared to Tesseract's individual add collision object, set collision object transform, etc. (question for other maintainers: Should that belong to the CollisionWorld class? I can imagine a lot of MoveIt infra relying on the fact that planning scene updates are passed to the collision world like this). I think the best place for you to start is to take another close look at how the collision manager is handled in CollisionWorldFCL, I imagine that the collision manager in Bullet would be updated in a very similar way.

I am a bit unsure what is the best way to tackle this problem. My approach would be to more or less keep the tesseract_bullet::BulletDiscreteSimpleManager and replace the manager member of CollisionWorldFCL with it to create the new CollisionWorldBullet.

Another idea would be to not use the tesseract_bullet::BulletDiscreteSimpleManager but to transfer its functionality directly into the new CollisionWorldBullet. I think this would be the cleaner / more efficient solution, but probably take longer. Any thoughts?

Some feedback would be nice here, @felixvd, @BryceStevenWilley :)

[j-petit]

In tesseract, there is the enum tesseract::tesseract_fcl::CollisionFilterGroups with values DefaultFilter |  StaticFilter |  KinematicFilter |  AllFilter which for example stops the collision checking process. I don't understand its purpose and can't find any further documentation. The enum is used as member tesseract::tesseract_fcl::CollisionObjectWrapper::m_collisionFilterGroup. Maybe @Levi-Armstrong can give me a quick hint?

[felixvd]

For the actual collision check, it suffices then to execute collide() of the FCL collision manager. The robot itself is passed as an argument (fcl_object) which is constructed from CollisionRobot and the RobotState.

Does this collide() function check if the object/robot in the argument collides with the world? How does this work in Bullet? Is there a difference between CollisionRobot and an fcl_object? Open question to anyone.

[BryceStevenWilley]

I am a bit unsure what is the best way to tackle this problem.

Discussed offline, but I think the first example (keeping tesseract_bullet::BulletDiscreteSimpleManager) is the best way to go about this at the moment. We can refactor to not be dependent on tesseract later.

In tesseract, there is the enum tesseract::tesseract_fcl::CollisionFilterGroups with values DefaultFilter | StaticFilter | KinematicFilter | AllFilter

My best guess here is that you can avoid collision-checking the static scene against the static scene, as mentioned in this comment.

Does this collide() function check if the object/robot in the argument collides with the world?

This loops over each robot link/geometry and checks it against the already constructed collision world, as seen here. As for bullet, the major difference seems to be just the difference we've discussed previously: that it does the robot self collision checks and world checks at the same time instead of separately.

[j-petit]

Does this collide() function check if the object/robot in the argument collides with the world? How does this work in Bullet?

Do you mean in Tesseract? If yes: when a new KDLenv is created with a robot, a collision manager is added and all the robot links are added to the manager (see here). The coordinate transformations of the links within the manager are updated when KDLEnv::setState() is used to change the robot state in the environment. When now a collision check should be performed through contactTest() of the manager, there are two nested for loops for each pair of collision objects in the cows_ vector of the manager. Bullet is used then to perform the actual check of each pair.

Is there a difference between CollisionRobot and an fcl_object? Open question to anyone.

Yes, fcl_object is a format used by FCL to directly interact with the FCL manager. It represents an atomic instance of an object which can be collision checked via FCL. CollisionRobot of MoveIt on the other hand contains all the information necessary for collision checks of a single robot. This includes checking self-collisions and against other robots.

[BryceStevenWilley]

@isucan generously joined us on our weekly sync and provided some reasoning for the separation of the CollisionRobot and the CollisionWorld classes.

• It's to your advantage if you can collision check several poses of the robot at the same time
• With a separate world and robot, to check multiple poses in parallel, you simply have to copy and change the robot, not the whole world
  
  
  
  • This (and much of the design in MoveIt) was made with the assumption that the collision world would be large and expensive to copy.
  
  
  • True in some cases (point clouds, maybe some complex octomaps), but not for others (simple geometry worlds would be easy)
  
  
  • Also made it easy to do some-to-all collision checks of the world (don't have to bother with checking the static world against itself)
  
  
  • Bullet should handle this too
  
  
• OMPL and parts of MoveIt use this parallel checking of the robot state against the world, naively combining the world and the robot classes might break that
• Certain functions weren't implemented in Bullet when Ioan started

• Overall sentiment: it should be possible to simply copy the CollisionWorld along with the CollisionRobot, but it might be more expensive to do so, especially for large point cloud based worlds. We should get some metrics on how big the world needs to be before the optimizations from combining the world and the robot are outweighed by the expense of copying the world for parallel state checks.

[j-petit]

Following roadmap for the next days:

• [x] add tesseract manger as member of CollisionWorld and CollisionRobot
• [x] when instantiating new CollisionRobot add robot links to the manager
• [x] can update the robot link state in the tesseract manager
• [x] successful self-collision checks
• [x] adding objects to CollisionWorld adds them to the manager
• [x] transfer of collision objects from one manager to the other
• [x] checking robot-world collisions
• [x] passing test scenario for FCL but now using bullet

[Levi-Armstrong]

With a separate world and robot, to check multiple poses in parallel, you simply have to copy and change the robot, not the whole world

This may have been the case at the time of implementation, but now in the case of FCL and Bullet the core collision shape should never need to be copied. In these libraries the collision shapes are stored as a shared pointer and they leverage wrappers around these to store the location and other data that needs to change. This allows you to use a single collision shape multiple times within an environment were the pointer is only copied.

This (and much of the design in MoveIt) was made with the assumption that the collision world would be large and expensive to copy.
True in some cases (point clouds, maybe some complex octomaps), but not for others (simple geometry worlds would be easy)

The coping of the collision environment should be negligible if done properly because it is mostly copying shared pointers. Also because of the way things get loaded currently the majority of static objects are within the urdf and considered part of the robot and the world is only used for PointCloud data in my experience. As a result there are a lot of unnecessary collision checks between static objects.

In the case of Bullet it leverages masks to identify static and kinematic objects and allows user defined to prevent unnecessary collision checks which I believe is a better way to approach this and provided more flexibility to the developer by having the ability to add custom masks. I have implemented this same concept native to Bullet for FCL within the tesseract fcl contact manager. Also as the scene changes it is much easier to manage updating masks than moving collision objects between say the collision robot and world to prevent unnecessary static object collision checks.

Also made it easy to do some-to-all collision checks of the world (don't have to bother with checking the static world against itself)

I believe a more elegant way of handling this would be to support masking instead of two separate classes/environment. This separation could then be handled directly within a single environment which is how Bullet handle this.

We should get some metrics on how big the world needs to be before the optimizations from combining the world and the robot are outweighed by the expense of copying the world for parallel state checks.

What would you consider a large world environment. I could easily test it and provide metrics on the what it takes to clone the environment.

[j-petit]

I have problems understanding the ContactTestType enum in Tesseract. As far as I know, the value of the enum is only concerned with the number of contacts between a _single_ pair of objects. However, what I then do not understand is why cdata.done is set here which breaks the N^2 collision checking nested loop as soon as the first contact is found (for all pairs of objects). This is somewhat contradictory.

Another issue is how a single call to algorithm->processCollision(...) (the function in tesseract which invokes Bullet) is even able to produce multiple contact points? I cannot find repeated calls to the callback which adds to the results vector in the bullet function (for example here). Or is this for the case if we use compound collision shapes which uses this algorithm?

Other related question I am still struggling with is what kind of objects we actually want to check for collision (only convex polytopes?) and what this implies for the number of contact points. What exactly constitutes a contact point? I see that if the objects only touch we have a real single contact point, but we cannot generalize this case. For example in the picture below, if we assume to triangles as our shapes, are then the blue or the green points the multiple contact points? Or something completely different?

Bullet returns two points for each contact (this makes sense to me as it also returns the distance between those two points, they represent either the closest points are the ones when calculating the minimum translation distance). FCL on the other hand only reports a single contact point back. Is this the midpoint on the line connecting the two points for the minimum translation distance?

[BryceStevenWilley]

As far as I know, the value of the enum is only concerned with the number of contacts between a single pair of objects.

That might be the good assumption, but the enum comments say that the FIRST enum is for any pair of objects. This is simply to determine a boolean if any collisions are present in the scene.

what kind of objects we actually want to check for collision (only convex polytopes?)

Yep. We could use non-convex polytopes, but they are much harder/slower to find the collision points, and it should be faster to decompose links into convex polytopes (discussion here) and check those. You'll have to design a way to handle this.

what this implies for the number of contact points

I'd take a look at this article about EPA, which is the algorithm Bullet and FCL use to find collision points. (Not 100% sure about this, but) I don't believe that these algorithms interpolate points along edges or faces (i.e. the green points in the image above, as they aren't points on the original triangle), so in the above example the only 2 collision points to be found are the blue points. Specifically for TrajOpt, we'll only care about the deepest collision point.

FCL on the other hand only reports a single contact point back.

This should be the deepest point of contact (one of the blue points in the image above). From that point, the normal vector, and the penetration distance, you can recreate the point on the surface of the object being penetrated.

[j-petit]

As far as I know, the value of the enum is only concerned with the number of contacts between a single pair of objects.

That might be the good assumption, but the enum comments say that the FIRST enum is for any pair of objects. This is simply to determine a boolean if any collisions are present in the scene.

That is also what I thought but then on the other hand the breaking of the loop only happens in the inner loop which leads to additional collision checks being performed (one per collision object which hasn't been selected in the outer loop). Maybe this was not intended and is a bug in tesseract?

FCL on the other hand only reports a single contact point back.

This should be the deepest point of contact (one of the blue points in the image above). From that point, the normal vector, and the penetration distance, you can recreate the point on the surface of the object being penetrated.

With single contact point I mean that for each contact (FCL can return multiple per object pair as Bullet) we only get one coordinate back (Bullet returns two).

[Levi-Armstrong]

That is also what I thought but then on the other hand the breaking of the loop only happens in the inner loop which leads to additional collision checks being performed (one per collision object which hasn't been selected in the outer loop). Maybe this was not intended and is a bug in tesseract?

If using enum FRIST, After the first contact is found it sets the done flag which gets checked at the outer loop. It should exit after the first contact is found.

With single contact point I mean that for each contact (FCL can return multiple per object pair as Bullet) we only get one coordinate back (Bullet returns two).

With Bullet, if you are checking two convex shapes you will get a single contact results between the objects which contains two contact points (One for each object). If you are checking a convex shape to triangle mesh you will get multiple contact results. Each one will be for a single triangle that is within your contact threshold. This is the same for triangle mesh to triangle mesh. This is more or less the same as FLC. In FCL you get a single contact point which if I remember correctly it is the point projected along the minimum translation vector to the surface of one of the shapes. Then provided the normal(translation vector) and distance you can determine the second contact point from FCL and is what is happening inside the FCL implementation.

[j-petit]

Thanks @Levi-Armstrong, then I think this is indeed a bug in the bullet_discrete_simple_manager.cpp where the check is only in the inner loop. I took a look at the bullet_cast_simple_manager, there we have two checks, one in the inner, another in the outer loop.

[j-petit]

I did a first quick benchmark of the simple bullet manager I have implemented into MoveIt (see last two commits in PR #1484). I haven't adapted running the bullet collision detection in multiple threads, but benchmarking a single thread resulted in:

Run this launchfile to reproduce.

[rhaschke]

How does this compare to FCL collision checking?

[BryceStevenWilley]

@rhaschke The FCL results are at the bottom of the image above: Bullet does 338 checks per ms and FCL does ~999~ 99 checks per ms. The results can be explained by the fact that the state used is always collision free (results can be cached pretty well) and that FCL is using a bounding-volume-hierarchy, which is faster on collision free queries. Bullet/Tesseract handles BVH's pretty well, I created a new card to track the fact that Bullet will eventually have that to be competitive to FCL.

It's a good start, but we definitely need more variation. To make these benchmarks more useful, we need a variant that does random states (probably on a more densely clutter scene than the default, which would just have self-collisions) that reports back time and percent of states in collision, and one that does a state always in collision. Shouldn't take long to add those variants.

[simonschmeisser]

the number for fcl has one digit less, so it's 99 not 999

[BryceStevenWilley]

Thanks for the correction @simonschmeisser, completely misread that.

[rhaschke]

Thanks, didn't notice the FCL numbers on the first skim...

[simonschmeisser]

Hmm, I didn't find any extremely low hanging fruits there that would explain the factor 3 difference. Hotspot (perf gui) also doesn't show anything super obvious.

(This is from a similar scenario where I looked at stomp performance)
You can see that almost all time is spend in the selfCollision part which is split into:

• ~12% looking up allowed collision matrix entries (the red "collision_det" at the left and the similarily shaped and colored entries in second & third column)
• ~18% are spent setting up and tearing down the broad-phase tree (last three columns)
• actual collision checking

So some performance could be gained by going from a std::string based allowed collision map to an actual matrix. By using a thread local cache of the fcl objects (they should still share the FCLGeometries) and the tree probably another 10% or so could be saved.

I'll test that at some point but still bullet is a lot faster here! (Note that there are actually no real collision checks to be seen here, it's all about bounding boxes and similar)

[rhaschke]

I remember, @Levi-Armstrong criticism of MoveIt's FCL collision checking was particularly about the repeated creation of FCL geometries. I think, they dramatically improved that in Tesseract.

[gavanderhoorn]

@rhaschke: #488 (and #29 sort of).

[j-petit]

One thing I don't (or maybe wrongly) understand is the way FCL does the broadphase collision checking. So CollisionRobot for example uses the fcl::DynamicAABBTreeCollisionManager<double> for the self-collision check. The collision checking process is started through manager.collide(..., callback). The manager will now traverse the tree and perform a broadphase check on all pairs of objects. The ones which survive the broadphase are then fed into the callback where a possible "real" collision check using GJK through FCL takes place.

However, the user defined checks of the ACM and other properties are taking place in the callback - which means only after the manager did a the first broadphase check. This does not make sense as IMHO the user defined allowed collision flags should be checked first before doing anything else to save on the more expensive AABB check. Or maybe AABB check is even cheaper than a lookup in the ACM?

Another argument for first performing the user defined checks is that in most cases when the objects are set in the ACM (for example connected links) they might be always in collision. What this means is that the broadphase check will never cull them and it will always result in the call of the callback. This means for such objects AABB check + user defined check = expensive. Compare this to only user defined check as the objects are allowed to be in collision if this check would be performed first.

---

I am right now working on a more comprehensive benchmark.

[j-petit]

Benchmarking FCL and Bullet

I had a bug in the implementation of the Bullet collision manager which resulted in premature ending of the collision checking process - this was the main reason it was so fast. The Bullet implementation is still WIP, uses no BVH and is not optimized for speed yet.

Case 1: 100 random placed meshes, 2 in collision with the robot
FCL: 1646 checks per second
Bullet: 632 checks per second

Case 2: 100 random placed meshes, none in collision with the robot
FCL: 61,525 checks per second
Bullet: 687 checks per second

Case 3: 100 random placed boxes, 1 in collision with the robot
FCL: 5,908 checks per second
Bullet: 1,512 checks per second

Case 4: 100 random placed boxes, none collision with the robot
FCL: 37,141 checks per second
Bullet: 1,780 checks per second

Case 5: Self collision check of the robot with no collision
FCL: 151,729 checks per second
Bullet: 20,388 checks per second

Case 5: Self collision check of the robot with collision (end checking after first reported collision)
FCL: 87,000 checks per second
Bullet: 18,000 checks per second

---

The script to reproduce can be found here.

[BryceStevenWilley]

On our sync call today, we re-targeted some of @j-petit's original goals. There's great progress being made, but the initial goals were probably just too ambitious:

Project Timeline

• [x] June 10th (3-4 weeks): a draft PR proof-of-concept making a Bullet collision checking plugin. (Success! https://github.com/ros-planning/moveit/pull/1500)
• [ ] July 15th (another 5 weeks): ready to review PRs that change the CollisionPlugin/CollisionRobot/CollisionWorld interfaces and loading convex shapes, and also add a discrete (or casting if a flag is passed) CollisionPlugin using Bullet
• [ ] July 29th (2 more weeks): Start research into trying to add CCD via Convex hulls to FCL, documenting what it would take to do so and leaving a detailed roadmap for others to use
• [ ] August 12th - August 16th: j-petit is absent.
• [ ] August 19th (last week with GSoC): Document collision plugins in depth, detailing how to write your own, and how to select which plugin is used at runtime.

[felixvd]

Can you rephrase the "Start research" goal? It's not measurable and I'm not sure if the date is the deadline or the start.

[j-petit]

Updated: Benchmarking FCL and Bullet with BVH

I ported the collision manager using BVH from Tesseract into MoveIt and have new benchmarking results:

Case 1: 100 random placed meshes, 2 in collision with the robot
FCL: 1700 checks per second
Bullet: 500 checks per second

Case 2: 100 random placed meshes, none in collision with the robot
FCL: 21,000 checks per second
Bullet: 600 checks per second

Case 3: Self collision check of the robot with no collision
FCL: 125,000 checks per second
Bullet: 90,000 checks per second

Case 4: Self collision check of the robot with collision (end checking after first reported collision)
FCL: 41,000 checks per second
Bullet: 38,000 checks per second

---

The script to reproduce can be found here.

Conclusion

From the robot self-collision checks using Bullet it seems that Bullet is at least in the same performance region as FCL (the code is not optimized yet, so there is definitely still room for improvement).

Due to the separate CollisionWorld and CollisionRobot representation, Bullet is much slower compared to FCL in the world collision checks as I am still transferring in each step the robot links collision objects to the world representation. The performance didn't really improve using the BVH. To have good performance, the code can either be optimized or we combine CollisionWorld and CollisionRobot (as done in Tesseract).

[felixvd]

Great job.

or we combine CollisionWorld and CollisionRobot (as done in Tesseract).

I'm definitely in favor if there is enough time to do it. I will be more useful as a mentor for that than right now, too.

[BryceStevenWilley]

I'm really loving the collision benchmark setup, it works great. Great jobs j-petit!

or we combine CollisionWorld and CollisionRobot (as done in Tesseract).

Agree with Felix here, this should be the next step after casting.

[Levi-Armstrong]

Updated: Benchmarking FCL and Bullet with BVH

I ported the collision manager using BVH from Tesseract into MoveIt and have new benchmarking results:

Case 1: 100 random placed meshes, 2 in collision with the robot
FCL: 1700 checks per second
Bullet: 500 checks per second

Case 2: 100 random placed meshes, none in collision with the robot
FCL: 21,000 checks per second
Bullet: 600 checks per second

Case 3: Self collision check of the robot with no collision
FCL: 125,000 checks per second
Bullet: 90,000 checks per second

Case 4: Self collision check of the robot with collision (end checking after first reported collision)
FCL: 41,000 checks per second
Bullet: 38,000 checks per second

I believe these results are correct but does not represent a true comparison of FLC to Bullet, but a MoveIt FCL to MoveIt Bullet implementation. If you profile the collision checking you find that in MoveIts for every collision request it must reconstruct the collision environment (BVH) then perform a collision check. This construction time can be significant and is most likely skewing the data. I believe that is why for simpler environment your data shows FCL and Bullet to be closer but for large environment there is a large separation. Most of these collision environments are not optimized for construction because the ideas is to only create them one time and them manipulate them.

[j-petit]

@Levi-Armstrong: In the KDLenv you create two collision managers, one continuous and one discrete and fill them through the same URDF model with the robot links. I did the same when transferring both managers into the MoveIt collision environment. However now I have the problem that they share their underlying btCollisionShapes (that's what I assume) and influence each other when I perform collision checks with both of them. How did you solve that? I use almost the exact same code as in the KDLenv to add the objects.

My code is here.

---

Update: Solved, I had an error in my implementation.

[felixvd]

Some minutes from our weekly call:

ToDos for Jens:

• Document the test for continuous collision checking with screenshots, like you did for the other tests (it's helpful and cool to look at)
• Isolate the problem you described above and when it occurs

Second, for easier review we talked about splitting your work into chunks:
1) The continuous collision checking with bullet, which you said depends on some modules you have imported from Tesseract, and which Omid's TrajOpt work will depend on
2) Combining CollisionWorld and CollisionRobot into the same environment, as done in Tesseract.

You worried that submitting a PR for 1) might involve work that will be made obsolete by a PR for 2). We shared your concern and encouraged you to figure out what has to be done for both of the PRs, so you can estimate the remaining work better.

Work packets for Omid:

• Detail the requirements for the collision checking API so Jens knows what he needs to supply you with
• Try to rebase on j-petit's branch when the requirements are met (which might already be true), so you have no dependency on tesseract in your environment anymore

Cheers!

[j-petit]

As pointed out in our weekly call (see @felixvd comment above), we have two possible large work packages / PRs. To clarify their scope and open them to discussions I summarize them below:

Work package 1: Integrating discrete collision detection and CCD using Bullet / Tesseract into the existing collision checking framework

A proof of concept PR was successful. CCD still has a bug which should be solved soon. However, to get this PR merged, the following points remain to be done:

• reduce transfer of collision objects between the collision managers in robot and world
• extend collision cases to match FCL functionality (world to world, robot to robot, distance requests...)

_All of the following ToDos are also required for workpackage 2:_

• handling of attached objects
• handling of robot padding
• resolve outside dependencies (bullet3_ros and tesseract_msgs)
• translating of collision flags (enable, active, filters, masks, attached...)
• refactoring tesseract code (these are around 7k LOC)
  
  
  
  • removing unused code
  
  
  • removing duplicate code to MoveIt
    
    
    
    
    
    • attached objects
    
    
    
  
  
  • creating an abstract parent class for both discrete and continuous collision managers
  
  
  • removing cast and discrete duplicate code for example in DiscreteCollisionCollector / CastCollisionCollector and more functions in bullet_utils.h
  
  
  • solving tesseract ToDos
  
  
  • adding more documentation
  
  
  • adding more tests
  
  

Open Questions:

• Is it sufficient to use the Tesseract BVH collision managers instead of the simple ones? Then we can get rid of the simple managers which would reduce the tesseract code which needs refactoring

Work package 2: Combining CollisionWorld and CollisionRobot to a single collision environment

As demonstrated by Tesseract, we can expect performance gains through a single collision environment. Another aspect is that the current split collision checking environment is difficult to understand and extend. The combination would improve the code maintainability.

ToDos:

• investigating all existing interactions with CollisionRobot and CollisionWorld - so far I think this includes the planning scene, planning scene monitor, kinematic constraints, all collision checkers (FCL, distance field, allvalid, Bullet) and the planners CHOMP and SBPL
• designing new collision environment
• porting FCL, distance field, Bullet (if already integrated) into the new single collision environment
• resolve all other dependencies to new collision environment
• thoroughly test and document as this is one of the core features of MoveIt

General questions and discussion points:

• if we can complete work package 2, then some of the optimization work for Bullet in package 1 might be obsolete
• I personally think that completing both work packages might not be feasible during the GSoC (until late August)

[j-petit]

Continuous collision test case between state 1 and state 2

Robot state 1:

Robot state 2:

Collision results:

ros.moveit_core: Checking panda_rightfinger vs box
ros.moveit_core: Contact btw box and panda_rightfinger dist: -0.0299702
ros.moveit_core: No entry in ACM found, collision check between panda_link7 and box
ros.moveit_core: Checking panda_link7 vs box
ros.moveit_core: Contact btw box and panda_link7 dist: -0.015847
ros.moveit_core: No entry in ACM found, collision check between panda_link6 and box
ros.moveit_core: Checking panda_link6 vs box
ros.moveit_core: Contact btw box and panda_link6 dist: 0.0396077
ros.moveit_core: No entry in ACM found, collision check between panda_link5 and box
ros.moveit_core: Checking panda_link5 vs box
ros.moveit_core: Contact btw box and panda_link5 dist: 0.0464478
ros.moveit_core: No entry in ACM found, collision check between panda_leftfinger and box
ros.moveit_core: Checking panda_leftfinger vs box
ros.moveit_core: Contact btw box and panda_leftfinger dist: -0.0299702
ros.moveit_core: No entry in ACM found, collision check between panda_hand and box
ros.moveit_core: Checking panda_hand vs box
ros.moveit_core: Contact btw box and panda_hand dist: -0.0809539

Results from this test case

[j-petit]

As I have been investigating errors in my port of Bullet/Tesseract into MoveIt, I ran Valgrind and noticed errors which also show up in Tesseract in general. For example when running this unit test of tesseract, I get the following conditional jump on not initialized values messages valgrind_output.txt. The code snippet in question deals with reading in the robot links from the URDF and constructing out of them the bullet collision objects. For the first constructed object, this results in an uninitialized read at this point. I did not find out yet what is the cause.

To reproduce, simply run the unit test with
valgrind devel/.private/tesseract_ros/lib/tesseract_ros/tesseract_ros_kdl_env_unit

Opened issue at ROS-Industrial

[gavanderhoorn]

As I have been investigating errors in my port of Bullet/Tesseract into MoveIt, I ran Valgrind and noticed errors which also show up in Tesseract in general. For example when running this unit test of tesseract, I get the following conditional jump on not initialized values messages valgrind_output.txt. The code snippet in question deals with reading in the robot links from the URDF and constructing out of them the bullet collision objects. For the first constructed object, this results in an uninitialized read at this point. I did not find out yet what is the cause.

To reproduce, simply run the unit test with
valgrind devel/.private/tesseract_ros/lib/tesseract_ros/tesseract_ros_kdl_env_unit

wouldn't this be something to report over at ros-industrial-consortium/tesseract/issues?

[BryceStevenWilley]

I agree with @gavanderhoorn that you should open an issue at the tesseract repo. If you don't think it's the exact thing that's causing your bugs, then I wouldn't focus on it too much.

What sort of errors have you been running into now? From what I saw, this commit addressed the issue with casting and discrete managers interfering with each other.

[j-petit]

I agree with @gavanderhoorn that you should open an issue at the tesseract repo. If you don't think it's the exact thing that's causing your bugs, then I wouldn't focus on it too much.

Agree, @gavanderhoorn and @BryceStevenWilley, I just opened an issue.
What sort of errors have you been running into now? From what I saw, this commit addressed the issue with casting and discrete managers interfering with each other.

Yes this was resolved, I had an implementation error. However, I get tons of uninitialized reads reported from Valgrind when I run any collision detection. I am not sure about their origin (my code, tesseract, or MoveIt?). As far as I can tell it does not influence the functionality of the code, but nevertheless seems something to resolve at some point.

[j-petit]

Design Question: How to remove mutable from bullet managers

As of now, I have declared the bullet collision managers as mutable members of their respective CollisionWorld and CollisionRobot. This is necessary as all collision checking functions are declared as const member functions and the collision managers need modification for multiple reasons:

• collision objects have to be transferred to have all of them in a single manager for a world-robot check
• collision objects within the managers have to be modified as their poses are updated with the newest robot states before a check
• the cast of two collision objects has to be recalculated for a continuous collision check

One option would be to change the collision checking API in CollisionRobot and CollisionWorld to not use const member function. The other option would be dismantle the bullet collision managers and make something similar to the FCL version with the collision objects being stored and modified directly as part of CollisionRobot and CollisionWorld.

Or does anyone see other options?

Comparison to FCL implementation
In FCL, the collision objects exists outside of the mangers through the geoms_ and fcl_objs_ members in both CollisionWorld and CollisionRobot.

For the CollisionRobot there is no collision manager member, only a temporary one is created each time self-collision check is performed. When this happens, it is filled with the collision objects and a check performed.

Contrary, in CollisionWorld a permanent manager exists. When performing a robot-world check, an FCL collision object is constructed out of the robot and this is checked against the world through the manager (see here).

Consequently, declaring all collision checking functions as const is no problem here as no modifications are happening.

[BryceStevenWilley]

Note that by keeping the objects in the manager instead of recreating them each collision check should allow for some additional performance gains.

And for some reason, I feel like we should still be able to calculate a list of objects and simply pass them into the collision manager at runtime, right? Essentially instead of stateful classes with a ton of members, just keep the precalculated broadphase hierarchies and COWs outside the class and pass them in through const refs. That would require a descent refactor of the tesseract utils though, and if we really want that I'd rather spend the effort on unifying CollisionRobot and CollisionWorld.

[j-petit]

Note that by keeping the objects in the manager instead of recreating them each collision check should allow for some additional performance gains.

And for some reason, I feel like we should still be able to calculate a list of objects and simply pass them into the collision manager at runtime, right? Essentially instead of stateful classes with a ton of members, just keep the precalculated broadphase hierarchies and COWs outside the class and pass them in through const refs. That would require a descent refactor of the tesseract utils though, and if we really want that I'd rather spend the effort on unifying CollisionRobot and CollisionWorld.

I changed some of the implementation to do specifically that in one of my latest commits. Now creating new objects and recalculating BVH etc. is kept to a minimum.

[felixvd]

I wrote this 3 days ago and forgot to submit. Whoops.

Note that by keeping the objects in the manager instead of recreating them each collision check should allow for some additional performance gains.

It was the main source of performance gains, if I understood Levi correctly.

Essentially instead of stateful classes with a ton of members, just keep the precalculated broadphase hierarchies and COWs outside the class and pass them in through const refs. That would require a descent refactor of the tesseract utils though

I feel like it might be worth making a diagram of how the collision-related classes (especially the managers) are organised in MoveIt and Tesseract currently. I don't think I have read the collision checking code deeply enough to comment much otherwise.

[j-petit]

I wrote this 3 days ago and forgot to submit. Whoops.

Note that by keeping the objects in the manager instead of recreating them each collision check should allow for some additional performance gains.

It was the main source of performance gains, if I understood Levi correctly.

FCL collision checking is not recreating collision objects at each collision check, what it does is bundling the collision objects and their geometries (which are permanently stored in the CollisionRobot and CollisionWorld in a larger FCLObject which is then passed into the collision managers. See here. I hope this becomes clearer when I make some progress on the diagrams - see below.

Essentially instead of stateful classes with a ton of members, just keep the precalculated broadphase hierarchies and COWs outside the class and pass them in through const refs. That would require a descent refactor of the tesseract utils though

I feel like it might be worth making a diagram of how the collision-related classes (especially the managers) are organised in MoveIt and Tesseract currently. I don't think I have read the collision checking code deeply enough to comment much otherwise.

I agree, I am on it here for FCL and here for Bullet.

[j-petit]

Flowcharts for collision checking process using Bullet and FCL

[BryceStevenWilley]

These flow charts are great! You should post the first one on the Bullet PR so it's right there when people review it. nm, you already did

[BryceStevenWilley]

Summary of our sync discussion today:

• The PR #1504 is very close to being ready for review, should be ready by 7/8: still need to:
  
  
  
  • add some tests to ensure attached objects and padding is implemented correctly
  
  
  • refactor the collision detector tests to reduce duplicate code, discussed here
  
  
  • read through one last time to ensure that the PR is a small as possible.
  
  
• There is not a clear way to keep the Collision methods const without performance penalties and a lot of additional refactoring, so we're going to make a case that we should remove const from those methods (if that breaks MoveIt beyound repair, then we can leave the bullet manager as mutable).
• Decided not to worry about handling collision costs like FCL does, as it seems to be a very forward thinking move by Ioan that was never used extensively.

[Levi-Armstrong]

There is not a clear way to keep the Collision methods const without performance penalties and a lot of additional refactoring, so we're going to make a case that we should remove const from those methods (if that breaks MoveIt beyound repair, then we can leave the bullet manager as mutable).

You could mimic the clone method of the tesseract contact managers which would solve the const issues. It would do exactly what FCL is doing where you copy the FCLObject and add it to the FCL manager. The clone method of the tesseract contact managers for Buillet copies the pointers to the collision objects and create new COW and new broadphase object. If you are using Tesseract Contact manager directly then in the same location you populate the FCLManager you would call the clone method and then perform the collision check.

[j-petit]

You could mimic the clone method of the tesseract contact managers which would solve the const issues. It would do exactly what FCL is doing where you copy the FCLObject and add it to the FCL manager. The clone method of the tesseract contact managers for Buillet copies the pointers to the collision objects and create new COW and new broadphase object. If you are using Tesseract Contact manager directly then in the same location you populate the FCLManager you would call the clone method and then perform the collision check.

Thanks for the suggestion @Levi-Armstrong, it does the job :+1:.

[j-petit]

PR #1504 is ready for review. I made another detailed flowchart of the collision checking process using Bullet as it is quite elaborate. For some more background information this article is very helpful.

[j-petit]

As discussed in our sync call yesterday, I will start prototyping to combine CollisionRobot and CollisionWorld into a single collision environment besides working on PR #1504.

What is the motivation for this?

• maintainability / complexity: The current setup is very complex which makes any changes difficult. For example, collision objects have to be transferred between the two environments and the interaction between the two is not always very apparent.
• speed: through reducing the complexity there should be a considerable speedup.
• better suited for bullet: The design with the collision managers in Tesseract is a much better fit for a unified collision environment.

Any thoughts and input welcome!

[j-petit]

As I finished a draft version of a unified collision environment #1572 using Bullet and ran some benchmarks again. Here are the results (all values in collision checks per second):

See updated results below

| Collision environment | Bullet World Robot | Bullet Unified | FCL World Robot |
| :--------------- |------------:| -----:|----:|
| Robot self check, no collision | 15,000 | 80,000 | 110,000 |
| World clutter 100 meshes, no collision | 2,000 | 14,000 | 35,000 |
| World clutter 100 meshes, 4 in collision | 1,600 | 5,000 | 800 |

Then I ran the collision checking with request.distance enabled which adds the closest distance in the collision environment to the results. This is required for TrajOpt. What this means for collision checking is that the broadphase culling is skipped and for all collision object pairs the distances are calculated and then the closest one selected as the global minimum. Here Bullet outperforms FCL:

| Collision environment | Bullet World Robot | FCL World Robot |
| :--------------- |------------:| -----:|
| Robot self check, no collision | 3000 | 100 |
| World clutter 100 meshes, no collision | 172 | 3 |
| World clutter 100 meshes, 4 in collision | 196 | 7 |

[j-petit]

I ran benchmarks again as I modified the broadphase culling (it was not working correctly) in Bullet. Here are the results in collision checks per second.

| Collision environment | Bullet Unified | FCL World Robot |
| :--------------- |------------:| -------:|
| Robot self check, no collision | 270,000 | 110,000 |
| World clutter 100 meshes, no collision | 50,000 | 35,000 |
| World clutter 100 meshes, 4 in collision | 8600 | 800 |

Bullet is able to outperform FCL in all setups as it provides a very sophisticated broadphase interface which has a cache for overlapping pairs and is based on AABB trees. It provides many options to optimize and tune the collision checking. Exemplary, pairs can be culled as early as possible (even before the broadphase check) through a custom callback. Two adjacent links can be handled in such a way that they are never classified as an overlapping pair.

What API changes does this require:

• combining robot and world into a single collision environment
• all collision checks are not const
• the allowed collision matrix as a member of the collision environment which should then be only modified through the collision environment

[BryceStevenWilley]

Very exciting!

Was the broadphase culling not working correctly in Bullet in the Bullet-World-Robot example as well? If so, does fixing it improve anything there?

[felixvd]

Considering that the unified environment is also preferable from a maintainability perspective, that seems like a "nice to know" question I wouldn't spend more than 20 minutes on. Do we agree?

[j-petit]

Very exciting!

Was the broadphase culling not working correctly in Bullet in the Bullet-World-Robot example as well? If so, does fixing it improve anything there?

Yes, it also was not working correctly but we don't get improvements there because the cloning of the managers (due to the const) slows the collision checking down a lot.

Update: As I was interested in how fast the separate Bullet Robot World environment could be if we allow the collision checking function to be not const, I made the bullet managers mutable members. This brings considerable speedup and results close to the unified environment. A main drawback is still that I think the structure with two separate collision environments is not very developer friendly and difficult to understand.

| Collision environment | Bullet Unified | Bullet World Robot mutable | Bullet World Robot | FCL World Robot |
| :--------------- |------------:| -------:|-----:|-----:|
| Robot self check, no collision | 270,000 | 156,000 | 15,000 | 110,000 |
| World clutter 100 meshes, no collision | 50,000 | 38,000 | 2,000 | 35,000 |
| World clutter 100 meshes, 4 in collision | 8,600 | 8,100 | 1,600 | 800 |

[j-petit]

See PR #1584 for discussions about a unified collision environment and a detailed description of the background.

[j-petit]

The proposed unified collision environment in #1584 compiles and all tests are passed. I would be happy to get feedback there as well as detailed PR review for #1551 where CCD is added to Bullet.

[j-petit]

Meeting notes of yesterdays sync call:

• next week (12-16) I will be off which makes a total of 8 days left for GSoC

• Besides further testing the unified collision environment, I will now focus on documenting my work and laying down the motivation / outlook for Bullet as a collision checker. This includes speed benchmarks which show why Bullet is so promising.

• I will write a tutorial showing the capabilities of Bullet (especially the continuous collision detection) and the differences to FCL.

• The top post of this Github issue will be used as "work product" as required by Google. It will bundle and summarize all relevant PRs, documents, discussions,

• The TrajOpt part of the feature-bullet-trajopt branch will be removed so the branch only contains Bullet collision checking. This allows us to keep the two projects independent and have the feature branch merged earlier. @ommmid will keep his work on TrajOpt separate.

• The goal is to have the feature branch merged before PR #1584. As soon as this is done, the unifying PR #1584 needs some more work to unify the bullet collision checking.

• The goal is to merge the unified environment first in PR #1584 and then the feature branch with all the bullet related work.

[j-petit]

That's it GSoC 19 is over for me! There are still a couple of open PRs where I am happy to get reviews on:

• #1572
• #1648
• #1645
• Tutorial on using Bullet

And then finally merging the feature branch into master :). I opened a separate issue for future work discussions in #1646.

Big THANK YOU to my mentors @BryceStevenWilley and @felixvd. It was great working with you and all the other members of the MoveIt community! I have learned a great deal of things and gained a lot of confidence in being a professional developer. I will hopefully be tying up some of the loose ends we still have in the Bullet integration over the next couple of months.