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 · 76 comments

Comments

Projects
None yet
8 participants
@j-petit
Copy link
Contributor

commented Apr 5, 2019

Implement continuous collision checking and integrate Bullet library

Google Summer of Code 2019
Mentors: @felixvd, @BryceStevenWilley.

This project should add many useful and necessary features to MoveIt’s collision detection, including:

  • Integration of a different collision checking library, such as Bullet
  • Continuous collision checking, to avoid collisions between discrete states
  • Optimizations to the collision checking process itself to improve performance (cache/scene reconstruction improvements, convex-convex collisions)
  • The ability to dynamically change large parts of the kinematics of the world (i.e. picking up a tray with many items should attach all of those items to the robot, not just the tray).
  • Next to API changes, this includes writing tests and tutorials.

Project Timeline

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

Code

Additional resources

Related issues / projects

  • #1467 PikNick summer intern implementing TrajOpt path planning
@welcome

This comment has been minimized.

Copy link

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

This comment has been minimized.

Copy link
Contributor Author

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

This comment has been minimized.

Copy link
Contributor

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

This comment has been minimized.

Copy link
Contributor

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

This comment has been minimized.

Copy link
Contributor

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

This comment has been minimized.

Copy link
Contributor

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

This comment has been minimized.

Copy link
Contributor Author

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

This comment has been minimized.

Copy link
Contributor

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

This comment has been minimized.

Copy link
Contributor Author

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

This comment has been minimized.

Copy link
Contributor

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

This comment has been minimized.

Copy link
Contributor

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.

@tsijs tsijs referenced this issue Apr 12, 2019

Open

MoveIt packages #76

@felixvd

This comment has been minimized.

Copy link
Contributor

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

This comment has been minimized.

Copy link
Contributor Author

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

This comment has been minimized.

Copy link
Contributor

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

This comment has been minimized.

Copy link
Contributor

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

This comment has been minimized.

Copy link
Contributor Author

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

This comment has been minimized.

Copy link
Contributor

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

This comment has been minimized.

Copy link
Contributor Author

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

This comment has been minimized.

Copy link
Contributor

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

This comment has been minimized.

Copy link
Contributor Author

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

This comment has been minimized.

Copy link
Contributor

commented May 8, 2019

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

@Levi-Armstrong

This comment has been minimized.

Copy link
Contributor

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

This comment has been minimized.

Copy link
Contributor

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

This comment has been minimized.

Copy link
Contributor Author

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

This comment has been minimized.

Copy link
Contributor

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

This comment has been minimized.

Copy link
Contributor

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

This comment has been minimized.

Copy link
Contributor Author

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

This comment has been minimized.

Copy link
Collaborator

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

This comment has been minimized.

Copy link
Contributor

commented May 9, 2019

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

@j-petit j-petit referenced this issue May 27, 2019

Closed

Integrating Bullet into MoveIt [WIP] (#1427) #1484

2 of 3 tasks complete
@j-petit

This comment has been minimized.

Copy link
Contributor Author

commented May 29, 2019

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

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

This comment has been minimized.

Copy link
Contributor

commented May 30, 2019

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

This comment has been minimized.

Copy link
Contributor Author

commented May 30, 2019

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

This comment has been minimized.

Copy link
Contributor

commented May 30, 2019

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

This comment has been minimized.

Copy link
Contributor Author

commented May 30, 2019

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

This comment has been minimized.

Copy link
Contributor Author

commented Jun 3, 2019

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:
image
Run this launchfile to reproduce.

@rhaschke

This comment has been minimized.

Copy link
Collaborator

commented Jun 3, 2019

How does this compare to FCL collision checking?

@BryceStevenWilley

This comment has been minimized.

Copy link
Contributor

commented Jun 4, 2019

@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

This comment has been minimized.

Copy link
Contributor

commented Jun 4, 2019

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

@BryceStevenWilley

This comment has been minimized.

Copy link
Contributor

commented Jun 4, 2019

Thanks for the correction @simonschmeisser, completely misread that.

@rhaschke

This comment has been minimized.

Copy link
Collaborator

commented Jun 4, 2019

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

j-petit added a commit to j-petit/moveit that referenced this issue Jun 4, 2019

@simonschmeisser

This comment has been minimized.

Copy link
Contributor

commented Jun 4, 2019

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.
grafik
(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

This comment has been minimized.

Copy link
Collaborator

commented Jun 4, 2019

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

This comment has been minimized.

Copy link
Member

commented Jun 4, 2019

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

@j-petit

This comment has been minimized.

Copy link
Contributor Author

commented Jun 4, 2019

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

This comment has been minimized.

Copy link
Contributor Author

commented Jun 11, 2019

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
image

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
image

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
image


The script to reproduce can be found here.

@j-petit j-petit closed this Jun 11, 2019

@j-petit j-petit reopened this Jun 11, 2019

@BryceStevenWilley

This comment has been minimized.

Copy link
Contributor

commented Jun 13, 2019

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

  • June 10th (3-4 weeks): a draft PR proof-of-concept making a Bullet collision checking plugin. (Success! #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

This comment has been minimized.

Copy link
Contributor

commented Jun 17, 2019

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

This comment has been minimized.

Copy link
Contributor Author

commented Jun 17, 2019

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

This comment has been minimized.

Copy link
Contributor

commented Jun 17, 2019

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

This comment has been minimized.

Copy link
Contributor

commented Jun 18, 2019

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

This comment has been minimized.

Copy link
Contributor

commented Jun 18, 2019

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.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.