Add GEOM_CUSTOM node type for user-defined shapes

[rjoomen]

In order to support continuous collision checking in Trajopt (currently done using Bullet), Coal needs support for custom shapes. For Bullet, this was trivial, as btConvexShape can be overridden. Now, using the new GEOM_CUSTOM, this is easy using Coal as well.

• Details about the new Coal collision manager for Tesseract are here: Tesseract collision Coal TODO list tesseract-robotics/tesseract_collision_coal#5. Benchmarks show a significant improvements using Coal!
• This also enables an easy way of implementing custom shapes, as discussed here: New primitive implementation #792. I added a test case demonstrating a DeformedCylinder.

As you might notice, a lot of this was created with the help of Claude, for completeness here is the Claude PR message:

Summary

• Add GEOM_CUSTOM node type and virtual computeShapeSupport() on ShapeBase, enabling user-defined shapes to participate in Coal's GJK/EPA collision, distance, and contact patch pipelines without modifying Coal internals
• Register GEOM_CUSTOM in all three dispatch matrices (collision, distance, contact patch) for every shape-pair combination, including BVH, HeightField, and OcTree
• Expose getNormalizeSupportDirection() as public API so wrapper shapes can delegate the Nesterov heuristic to their underlying shape via shape_traits

Motivation

Adding a new primitive shape to Coal currently requires modifying the NODE_TYPE enum, registering 63+ dispatch matrix entries, editing the Minkowski difference support dispatch, and adding serialization — all inside Coal's source tree. This was discussed in #792.

GEOM_CUSTOM reduces this to three steps for external users:

1. Subclass ShapeBase
2. Override getNodeType() to return GEOM_CUSTOM
3. Override computeShapeSupport() with the shape's support function

Everything else (GJK, EPA, contact patches, broadphase) works automatically via virtual dispatch.

Design

Entirely opt-in. ShapeBase does not default to GEOM_CUSTOM — it inherits BV_UNKNOWN from CollisionGeometry. Custom shapes must explicitly override getNodeType(). Built-in shapes are unaffected; all existing NODE_TYPE values and dispatch paths are unchanged.

Virtual dispatch only for GEOM_CUSTOM. The dispatch matrices route GEOM_CUSTOM pairs through ShapeShapeCollide<ShapeBase, T> (and equivalents for distance/contact patch), which call computeShapeSupport() via the virtual method. Built-in shapes continue to use their template-specialized fast paths.

No specialized fast-paths. GEOM_CUSTOM always goes through GJK/EPA. For shapes where closed-form solutions exist (e.g. sphere-sphere), implementing them as a built-in NODE_TYPE will still be faster.

getNormalizeSupportDirection exposed publicly so that wrapper shapes (e.g. swept-volume shapes for continuous collision detection) can query the Nesterov heuristic for their underlying shape through shape_traits, the single source of truth.

API additions

// collision_object.h
enum NODE_TYPE { ..., GEOM_CUSTOM, NODE_COUNT };

// geometric_shapes.h — on ShapeBase
virtual void computeShapeSupport(const Vec3s& dir, Vec3s& support,
int& hint, details::ShapeSupportData& data) const;
virtual bool needNesterovNormalizeHeuristic() const;

// minkowski_difference.h
COAL_DLLAPI bool getNormalizeSupportDirection(const ShapeBase* shape);

Tests

Two new test files with 21 test cases:

custom_shape.cpp (17 tests) — CustomSphere (leaf custom shape) and CastSphere (swept-volume wrapper, demonstrating continuous collision detection):

• Collision, distance, contact patches vs built-in shapes (Box, Sphere, Capsule, Cylinder, Ellipsoid)
• Custom-vs-custom pairs
• BVH mesh collision and distance
• HeightField collision (distance documents upstream limitation)
• OcTree collision and distance (both directions)
• computeBV<AABB/OBB/OBBRSS, ShapeBase> validation
• Built-in shapes retain their specific NODE_TYPE

custom_shape_deformed_cylinder.cpp (4 tests) — DeformedCylinder (convex hull of two oriented disks, from #792):

• Collision and distance vs Box
• Degenerates to built-in Cylinder when disks are parallel
• Contact patch computation

Test plan

• All existing Coal tests pass (no regressions)
• New custom_shape and custom_shape_deformed_cylinder tests pass
• Built-in shape NODE_TYPE values unchanged (verified by test)

[JonasBreuling]

Awesome, thank you @rjoomen !

[lmontaut]

Hey @rjoomen thanks a lot for this PR. There will probably be a bit of efficiency loss for custom shapes (because of the virtualization, which can become expensive when calling the support function for example) but I think this feature is very cool for experimentation. I asked @jorisv to have a look into it. Otherwise this looks good to me.

[rjoomen]

@lmontaut, @jorisv Friendly ping.

We (as in: the Tesseract Robotics project repo, docs) would really like to add Coal as a collision checker. Compared to the currently used Bullet and FCL it has increased performance, and it is actively maintained, whereas Bullet and FCL are not anymore.
Coal as it currently stands unfortunately does not allow anyone to implement custom shapes, and forking it just to add our cast hull shape is not desirable (and I don't think this custom shape is a good fit for inclusion in upstream Coal as a default shape). So to be able to move forward we'd really appreciate if this PR could be considered for merging.

(I do admit this GEOM_CUSTOM solution seems somewhat clunky, but it follows from the current design of the Coal dispatch internals. I've also iterated with Coal over a design brief for a different solution that would do away with the enormous CollisionFunctionMatrix, ContactPatchFunctionMatrix and DistanceFunctionMatrix altogether, but this might be too invasive a change.)

[lmontaut]

I'm not sure to understand this. details::getNormalizeSupportDirection calls needNesterovNormalizeHeuristic in the GEOM_CUSTOM case. Isn't this an infinitely self calling situation?

[lmontaut]

I am really not a fan of having the computeShapeSupport method in ShapeBase. And the fact that the default behavior is throwing, regardless of the shape.

All the shapes in coal have an implementation of the support function. This function lets the reader think that this is not the case. It should at least call getShapeSupport for shapes which are not GEOM_CUSTOM.

[lmontaut]

Hi @rjoomen, sorry for the delay. The team has been super focused on the release of Pinocchio 4 so @jorisv and I haven't had the time to properly look into this PR.

First thing, we need to make sure the CI is green for this to merge, as this library is used in various environments.

Second, apart from the comments I left, I think this is a good fix for shape customization in coal for now. We can see later how to improve on that. That being said, I would really appreciate if we could wait for @jorisv input on this. There are many ways we could go about shape customization in coal and I want to make sure we are taking a maintainable road.

[rjoomen]

Update: CI is fixed, including the upstream ROS issues.