Skip to content
New issue

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

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

Already on GitHub? Sign in to your account

GSoC: Improved Collision Checking #1427

Open
j-petit opened this issue Apr 5, 2019 · 107 comments
Open

GSoC: Improved Collision Checking #1427

j-petit opened this issue Apr 5, 2019 · 107 comments

Comments

@j-petit
Copy link
Contributor

@j-petit j-petit commented Apr 5, 2019

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

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

Related issues / projects

  • #1467, #1593, #1626: PikNick summer intern implementing TrajOpt path planning
@welcome
Copy link

@welcome welcome bot commented Apr 5, 2019

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

@j-petit j-petit commented Apr 5, 2019

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

@simonschmeisser
Copy link
Contributor

@simonschmeisser simonschmeisser commented Apr 5, 2019

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

@simonschmeisser simonschmeisser commented Apr 5, 2019

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 ros/urdfdom#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
Copy link
Contributor

@simonschmeisser simonschmeisser commented Apr 5, 2019

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

@felixvd felixvd commented Apr 5, 2019

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

@j-petit j-petit commented Apr 5, 2019

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

@simonschmeisser
Copy link
Contributor

@simonschmeisser simonschmeisser commented Apr 5, 2019

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

@j-petit j-petit commented Apr 6, 2019

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

@BryceStevenWilley BryceStevenWilley commented Apr 9, 2019

@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 #29 (comment). 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
Copy link
Contributor

@tsijs tsijs commented Apr 10, 2019

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 (ros-industrial-consortium/trajopt_ros#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
Copy link
Member

@felixvd felixvd commented May 3, 2019

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

@j-petit j-petit commented May 6, 2019

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

@felixvd felixvd commented May 7, 2019

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

@BryceStevenWilley BryceStevenWilley commented May 7, 2019

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

@j-petit j-petit commented May 7, 2019

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

@simonschmeisser simonschmeisser commented May 7, 2019

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

@j-petit
Copy link
Contributor Author

@j-petit j-petit commented May 8, 2019

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

@BryceStevenWilley BryceStevenWilley commented May 8, 2019

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

@j-petit j-petit commented May 8, 2019

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

@BryceStevenWilley BryceStevenWilley commented May 8, 2019

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

@Levi-Armstrong
Copy link
Contributor

@Levi-Armstrong Levi-Armstrong commented May 9, 2019

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

@felixvd felixvd commented May 9, 2019

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

@j-petit j-petit commented May 9, 2019

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

@Levi-Armstrong Levi-Armstrong commented May 9, 2019

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

@BryceStevenWilley BryceStevenWilley commented May 9, 2019

@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 #29 (comment)

@j-petit
Copy link
Contributor Author

@j-petit j-petit commented May 9, 2019

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

@rhaschke rhaschke commented May 9, 2019

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

@simonschmeisser simonschmeisser commented May 9, 2019

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

@j-petit
Copy link
Contributor Author

@j-petit j-petit commented Jun 26, 2019

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

@j-petit j-petit commented Jun 28, 2019

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

@BryceStevenWilley BryceStevenWilley commented Jul 1, 2019

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

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

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

@felixvd felixvd commented Jul 3, 2019

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

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

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

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

Flowcharts for collision checking process using Bullet and FCL

Bullet_collision_moveit
collision_moveit_fcl

@BryceStevenWilley
Copy link
Contributor

@BryceStevenWilley BryceStevenWilley commented Jul 4, 2019

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

@BryceStevenWilley BryceStevenWilley commented Jul 4, 2019

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

@Levi-Armstrong Levi-Armstrong commented Jul 4, 2019

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

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

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 👍.

@j-petit
Copy link
Contributor Author

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

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.
BulletChecking_detailed

@j-petit
Copy link
Contributor Author

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

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

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

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

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

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

@BryceStevenWilley BryceStevenWilley commented Jul 23, 2019

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

@felixvd felixvd commented Jul 24, 2019

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

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

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

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

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

@j-petit
Copy link
Contributor Author

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

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

@j-petit j-petit commented Aug 9, 2019

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

@j-petit j-petit commented Aug 29, 2019

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:

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.

@mikramarc
Copy link

@mikramarc mikramarc commented May 14, 2021

Hi @j-petit
Are you planning on implementing the distanceSelf and distanceRobot methods for bullet collision detection? Any timeline on that if so? Happy to help contributing if that could accelerate things too, it's a quite useful tool and would be super useful for real-time collision avoidance.

@j-petit
Copy link
Contributor Author

@j-petit j-petit commented May 23, 2021

Hi @mikramarc, sorry for the late answer. I have moved professionally away from the ROS and robotics domain, therefore I will not be working on any new features any time soon.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Linked pull requests

Successfully merging a pull request may close this issue.

None yet
9 participants