Robot multigeometry

[ddengster]

This PR provides support for adding circular bodies with an offset from the main robot. The current limit is set to 1 additional body for performance reasons. The main collision algorithm code lies in rmf_traffic/SplineCCD.cpp' collide_seperable_circles. It computes a distance vector to cover along the spline, then uses bilateral advancement to find roots.

see rmf_traffic/Profile.hpp for public api changes

Notable points of contention, let me know your thoughts on any of these:

1. WRT check_collision, if there are no extra geometry for the profile, we revert to the old FCL method.
   Edit*: We could have the new algorithm completely supercede the old method but would entail that we drop our Box tests (since this algorithm uses only circles atm). Also, the old FCL method is faster, check the rmf_planner_viz scenario Add conflict tests #8

2. Tolerance: Set to 0.1 in terms of distance. We might want to allow adjustment of these tolerance values elsewhere in future. Also, the FCL method seems to use a time of contact error tolerance as seen here

3. Safety checks: There is another limit based on the number of distance checks used in the algorithm, currently set to 120. This is to prevent infinite loops in runtime caused by a small tolerance values.

4. Profile api: Should we allow the extra geometry to be named? Y/N?

5. Vicinity shapes: We still use only one shape for the vicinity. Given the ability to add extra shapes to the footprint, we can automatically calculate the vicinity to never be smaller than the combined geometry. Problem is how do we deal with custom vicinity geometry? Is it time to enforce a circular vicinity geometry instead? Another option is to let users specify the vicinity shape along with the extra footprint geometry, incurring a performance penalty and extra maintainence cost. Yet another option is to leave the current implementation be, and let users maintain the single vicinity shape themshelves.

Other:

• There's a TODO in the new tests in test_Conflict detailing a problem with differential splines generated from splines with no motions. It's not scenario that's likely to occur IRL so we can put it as low priority for now
• you can do additional testing by running test_sidecar in https://github.com/osrf/rmf_planner_viz/
• Performance tests of minimum 8ms per DetectConflict check have been added

[mxgrey]

This looks like a really great proof of concept. When we start to contend with more precise and demanding use cases, this feature will be very valuable.

Before we can merge this PR, I think we'll need to refine some details of the public API, implementation, and documentation. The time it will take to make these changes means we'll have to manually migrate this PR over to the new repo.

Here are some notes on the public API:

• It seems that only the footprint supports offset geometries. This is problematic because the offset geometry is more relevant for defining the vicinity. Robots tend to be more bothered by seeing obstacles in front of themselves rather than behind or to the sides, so being able to add a forward-offset shape to the vicinity would very helpful.
• I think we can do a cleaner design than using the Profile class as an aggregator of offset shapes. I would recommend that we instead define the following classes:
• OffsetShape inherits Shape and ties together an offset and a ShapePtr
• ShapeGroup inherits Shape and ties together a set of ShapePtr objects
• ConvexOffsetShape inherits OffsetShape but is specialized for ConvexShapePtr
• ConvexShapeGroup inherits ShapeGroup but is specialized for ConvexShapePtr
• FinalOffsetShape, FinalShapeGroup, FinalConvexOffsetShape, and FinalConvexShapeGroup would be the "finalized" versions of the above classes. When generating these objects, we would flatten any nested group hierarchies that may have formed, applying offsets as necessary to make sure that the net offset of each leaf shape has the correct value.
• With the above approach, we can leave the Profile class the way it is, and allow users to simply pass a FinalConvexShapeGroup in for the footprint or the vicinity.

For the implementation:

• I'm not completely sure, but it looks like the implementation only works for circle-circle collisions right now. I'd be very reluctant to merge this feature if the implementation can't be extended to other types of convex shapes.
• Let's explore the feasibility of adding at least one more convex shape to this, like a capsule. It may be a good idea to first introduce capsules separately as a different PR, and just use the built-in FCL methods for that.

For the documentation:

• It would be good to have some kind of write-up of the performance characteristics of the sidecar. E.g. when you add a sidecar to your profile, how does it impact the speed of conflict detection?
• It would also be good to have a write-up on reliability. Does this implementation give any false positives or false negatives for conflict detection? If so, what are the conditions for those false results? Are there any strategies that users can leverage to mitigate those conditions?

[ddengster]

Thanks for the review @mxgrey. Off the top of my head:

I'm not completely sure, but it looks like the implementation only works for circle-circle collisions right now. I'd be very reluctant to merge this feature if the implementation can't be extended to other types of convex shapes.

That's correct. It's easy to extend it to other convex by replacing the distance function. It's on my todo list.

Let's explore the feasibility of adding at least one more convex shape to this, like a capsule. It may be a good idea to first introduce capsules separately as a different PR, and just use the built-in FCL methods for that.

There's a problem in that the FCL implementation of capsules assumes the same radius for both endpoints. This essentially means if we have 2 bodies of different sizes and we want to merge them into a capsule we'll have to take the larger sized radius, using up a good amount of space. Let me know if you think that's fine and I'll put it in.

[mxgrey]

There's a problem in that the FCL implementation of capsules assumes the same radius for both endpoints.

This actually agrees with the definition of "capsule" that I"m familiar with: the volume of a ball swept along a line segment, with uniform radius from end to end. The more complex convex hull you're thinking of would be a very cool thing to support as well, but I think the ordinary uniform capsule would be a great thing to start with.

[ddengster]

It would be good to have some kind of write-up of the performance characteristics of the sidecar. E.g. when you add a sidecar to your profile, how does it impact the speed of conflict detection?

The current implementation is a conditional, it uses the original fcl code path if you have single collision shapes on respective trajectories, otherwise it'll use the bilateral advancement algorithm for more seperable shapes.

Here are some performance results for collision between single objects on respective trajectories using the original fcl vs the bilateral advancement algorithm. Naturally, the latter's performance highly depends on the number of iterations for the bilateral advancement algorithm to reach the goal tolerance levels.

Bilateral advancement algorithm

You can increase the tolerance to 0.1 and the performance becomes 0.126ms with 4 distance checks.

FCL

It would also be good to have a write-up on reliability. Does this implementation give any false positives or false negatives for conflict detection? If so, what are the conditions for those false results? Are there any strategies that users can leverage to mitigate those conditions?

We can sometimes miss collisions and the chance of this happening increases if you specify trajectories with both a change in position and a high change in rotation. The solution is to increase the tolerance used in the algorithm. The following picture shows an example (the bright green double circle is the starting position, the dark green double circle is where the predicted collision is). The trajectory is an arc and there's a rotation too.

I'll put in a PR for the capsule implementation after the repo re-org. But let me know if there are any deal breakers for this method.

[ddengster]

Let's explore the feasibility of adding at least one more convex shape to this, like a capsule. It may be a good idea to first introduce capsules separately as a different PR, and just use the built-in FCL methods for that.

After some more digging, some blockers for using the built-in FCL methods:

1. The capsule that FCL uses is catered to 3d, and is constructed by a radius and a length representing the z-extension (we could call it 'height'), see the computation of the AABB.

2. FCL capsules have the origin at the center of the capsule, see this issue.

If we continue on this path, we'd have to creatively configure the axis of the trajectories and offset the trajectories to align the circles. I'll continue on with adapting the current algorithm for boxes, but let me know if there's still ways around that can be done that you want explored.

[ddengster]

• I think we can do a cleaner design than using the Profile class as an aggregator of offset shapes. I would recommend that we instead define the following classes: ...

With the purpose of minimizing the amount of code/objects, I'm doing this instead:

• ShapeGroup would be better served as a std::vector<ShapePtr> of objects, vice versa FinalConvexShapeGroup as std::vector<FinalConvexShapePtr>
• Use std::optional<Eigen::Vec2> offset; as a member of Shape, eliminating a whole bunch of Offset objects.
• FinalConvexShapeGroup as input to to Profile's footprint and vicinity

Let me know if there's anything of concern.