Collision Message Proposal

[DLu]

I have wanted a collision reporting scheme that could work across multiple simulators.

We have gazebo_ros_pkgs/ContactState.msg (and ContactsState) which contained

• the generic string info
• Two names
• geometry_msgs/Wrench[] for enumerating forces/torques
• One Wrench for the total force/torque
• Vector3[] contact positions
• Vector3[] contact normals (presumably the same size as positions)
• float64[] depths - Penetration depths

I boiled it down to a non-simulation specific pair of messages in collision_log_msgs/NamedCollision.msg which stripped it down to the two names.

There is also moveit_msgs/ContactInformation.msg which has

• Point position
• Vector3 normal
• float64 depth
• Two names, and associated body types (robot link/world object/robot attached)

Example

Consider this scenario:

We have four entities. At the time depicted, it might show

header:
    frame_id: /world
    stamp: 1234
collisions:
 - location: -3, 0, 0
   entity0: ground
   entity1: shaky
 - location: -2, 0, 0
   entity0: ground
   entity1: shaky
 - location: 1, 0, 0
   entity0: ground
   entity1: table
 - location: 1.5, 0, 1
   entity0: table
   entity1: arm
 - location: -2.5, 0, 1.5
   entity0: arm
   entity1: shaky

Considerations

• Even if the table is a box, there's likely many/infinite contact points between it and the ground. How should that be represented?
• The spec allows for entity0 == entity1, i.e. detect if the arm was in self collision
• Do we want additional fields for specifying more information about the subcomponent that is colliding? i.e. Do we want to know that /shaky/left_wheel is the in collision? Does that require additional contact positions (in the frame /left_wheel perhaps?)
• How much optional physics info do we need? i.e. normals/forces/depths.
• I imagine that you might want to control which collisions are reported in the simulator, i.e. ignore collisions with the floor and table.

[peci1]

I suggest renaming to Contact/Contacts. At least in Gazebo, Collision has a different meaning.

I'm personally quite sad that Gazebo doesn't allow reporting whole contacting (sub)surfaces but only points (for tracked vehicles, it would be great). But I don't know of any good representation. The area might get quite complicated. I see three ways:

1. Ignore area contacts and only support point contacts
2. Do not try to express the contacting area exactly, but report its centroid and add a field contact_area. That would e.g. allow computing pressure.
3. Add a polygon field that defines the contacting area. A polygon could be a good tradeoff between the possibly complex corner cases and the usual case.

Contact positions and normals are definitely needed for this message to be useful to a wider audience. Penetration depth wouldn't hurt, but I won't advocate for it. The forces could be nice for some applications. But they could also be left out for something like contact sensors.

Do we want to know that /shaky/left_wheel is the in collision?

I hope /shaky/left_wheel is a separate entity connected to /shaky by a revolute joint. Or does entity here denote just the model and not individual links? Maybe I'm too Gazebo-centric here :-D Anyways, it is definitely needed to know which links have the contacts.

• In more detailed cases, even the individual named collision elements could be of interest. We had, for example, a RL algorithm that was checking if some specific parts of the base link did not touch the ground, which would mean a broken sensor or another failure. But other parts were ok to touch (e.g. the bottom plate).
  • However, this would lead to first needing to define the collision structure of the robot - links, joints, collisions, visuals, inertials...

[jhanca-robotecai]

The simulation_interfaces package defines tools for managing the simulation, i.e. to add/remove elements, to find all available elements, to move them around, to pause/resume the simulation, etc. All tools are designed to be service-based, in which the response is given only when requested. Most importantly, the package defines a service to list all features of the given simulator: GetSimulatorFeatures.srv .

Although I see the need to standardize formats and to make them working across different simulators, I do not see the link of the proposed collision messages with the rest of the content of this repository. On the other hand, I cannot think of a better spot for such messages either. Maybe a new package, called simulation_messages, similar to gazebo_msgs should be drafted for all simulators?

If the community decides to keep the proposed messages within this repository, I believe the service definition to get the list of the contact points should be added to this PR and the tool should be defined within the list of the simulator features. Finally, I agree with @peci1 that normals should be included in the collision message and I vote for Contact name.

[DLu]

I suggest renaming to Contact/Contacts. At least in Gazebo, Collision has a different meaning.

Can you explain the difference in Gazebo?

All tools are designed to be service-based, in which the response is given only when requested.

I'll concede that broadly that is how the package is currently designed, but I don't think that's a requirement.

Maybe a new package, called simulation_messages, similar to gazebo_msgs should be drafted for all simulators?

I don't love it, but in ROS 2 there has been a transition from naming things _msgs to _interfaces. Because _interfaces includes messages AND services AND actions.

As a broader note, I'm coming at this from a navigation perspective. I just want to know if my stupid navigation algorithms have driven the robot into a box or not. But also, as someone who has worked to add ROS simulation interfaces to a simulator, I think there should be ways to report less. I don't even want to think about the logic that would be required for getting a full contact surface.

[peci1]

Gazebo has:

• collision = the collision geometries attached to links
• contact = the event when two collisions collide, and also the virtual contact joint

[jhanca-robotecai]

I don't love it, but in ROS 2 there has been a transition from naming things _msgs to _interfaces. Because _interfaces includes messages AND services AND actions.

I did not know that, thank you for this comment.

As mentioned earlier, I agree with the need to standardize interfaces between simulators, but I am not convinced we should push all possible interfaces to one package.

[adamdbrw]

My thoughts:

1. Reporting contacts is useful, @DLu gives a good example.
2. Some ground truth physics messages are already in the standard, such as EntityState. This one would not be a far fit.
3. I am not opinionated about whether this message should be separate.
4. I am not happy with publishing of such contacts / collisions being uncontrolled through interfaces.

How about making things more dynamic?

• SetCollisionReporting service, something like:

string entity_name                          # unique entity name. Note that if this entity and its descendants have no colliders, an error result ENTITY_HAS_NO_COLLIDERS will be returned. 
bool report true                               # Calling with true will turn on the reporting, calling with false will turn it off, in which case the remaining arguments are ignored. Turning off reporting removes the publisher. Turning when on, or turning off when already off does not change anything and results in ALREADY_IN_TARGET_STATE.
bool exclude_self_collisions           # if this is true, exclude collisions with any of entity's descendants.
string suggested_topic                   # report on this topic, if possible, with (reliable / history depth 100  / volatile QoS). If topic is empty, invalid, or exists with different QoS,  the service returns INVALID_TOPIC result, unless allow_topic_renaming is set to true. In such case, the caller should check the result for topic name . When calling the service repeatedly for multiple entities, it is up to the caller whether collisions for different entities are published on a shared topic or on distinct ones.
bool allow_topic_renaming         # whether the simulator can change the suggested topic 

---

uint8 INVALID_TOPIC = 101
uint8 ALREADY_IN_TARGET_STATE = 102
uint8 ENTITY_HAS_NO_COLLIDERS = 103

Result result
string topic                # A topic to listen to in order to get the collision data. 

If you like the general idea, I am open to suggestions about this service.

Then, you get a topic with the collision data. This can be implemented though a dynamic component / plugin, so that the simulation scene designer is not responsible for determining which collisions are interesting, but rather leaves that to test scenario developer. I think this would be better.

[pijaro]

I would keep things simple:

• Contact (I vote for that name) message with two names,
• location + normal vectors.

I don't think adding extra services for filtering is necessary. Don’t get me wrong - it’s not completely off the table, but I would be cautious about anything that might be seen as "forcing a design." Filtering can take place at multiple levels (engine, components, and end-user), and I wouldn't be happy if I were forced (or "strongly advised" 😄) to call multiple additional services and add several subscribers just to track the collisions I care about.

[adamdbrw]

" Filtering can take place at multiple levels (engine, components, and end-user), and I wouldn't be happy if I were forced (or "strongly advised" 😄) to call multiple additional services and add several subscribers just to track the collisions I care about.

You can still add components like collision/contact sensor the old way if you wish. This is a runtime option - because I believe it is increasingly useful to have dynamic API suitable for scenario testing (as in, not necessarily having to craft a new scene / rebuild for new scenario).

Also, the service can be called with the same topic for any number of entities and you will need just one subscription to handle it, if this is your preference.

[adamdbrw]

@DLu, @peci1 what do you think about the service I suggested?

[peci1]

I agree in general that publishing all contacts is not always a good idea. But I'd vouch for having an easy way to enable reporting of all contacts.

I can also imagine the request would profit from having an entity filter instead of just a single entity. I'm not sure how easy it is to write an all-matching filter, but maybe that would be a way to easily enabling the reporting of all contacts?

Also, some simulators could have different ways of enabling the reporting, e.g. config variables. That could solve @pijaro 's problem.

Just to be sure we all understand it the same way: for a contact to be reported, is it sufficient if reporting of contacts of one of the colliding bodies is enabled? Or do both need to be enabled?

[adamdbrw]

@peci1

Just to be sure we all understand it the same way: for a contact to be reported, is it sufficient if reporting of contacts of one of the colliding bodies is enabled? Or do both need to be enabled?

One body enabled is enough. I believe there is a reason to make this more powerful by enabling specifying pairs or sets / collision layers etc. in the long run.

I can also imagine the request would profit from having an entity filter instead of just a single entity. I'm not sure how easy it is to write an all-matching filter, but maybe that would be a way to easily enabling the reporting of all contacts?

I agree that registering multiple entities for watching could be useful, either through a list (could be obtained by filtering in a Get* call) or through a filter directly. I could introduce this as a proposed change if that's ok.

I will work my way to a PR tomorrow.