Skip to content

Add contact (bumper) sensor with Bool and WrenchStamped publishers - #213

Open
hijimasa wants to merge 2 commits into
Field-Robotics-Japan:masterfrom
hijimasa:feature/contact-sensor
Open

Add contact (bumper) sensor with Bool and WrenchStamped publishers#213
hijimasa wants to merge 2 commits into
Field-Robotics-Japan:masterfrom
hijimasa:feature/contact-sensor

Conversation

@hijimasa

Copy link
Copy Markdown
Contributor

Summary

This PR adds a contact (bumper) sensor to UnitySensors, following the existing sensor architecture (one small, self-contained sensor per PR). It detects collisions on the body it is attached to and publishes bumper-style contact state and the net contact wrench to ROS 2.

  • UnitySensors: ContactSensor + ContactEventRelay (core, ROS-independent)
  • UnitySensorsROS: BoolMsgPublisher (std_msgs/Bool) and WrenchStampedMsgPublisher (geometry_msgs/WrenchStamped)

Purely additive: no existing files are modified except the sensor list in README.md, and no asmdef or dependency changes are needed.

Design

Core (Packages/UnitySensors)

  • ContactSensor derives from UnitySensor and uses the standard _frequency / Init() / UpdateSensor() lifecycle. Collisions are accumulated in OnCollisionEnter/Stay/Exit and only aggregated at the sensor rate, so physics-rate events never leak through faster than frequency.
  • Exposed via IContactDataInterface (mirrors IImuDataInterface): isContact, per-contact list (ContactData: collider name, position, normal, force estimated from Collision.impulse / fixedDeltaTime), world-frame totalForce / totalTorque, and localTotalForce / localTotalTorque (sensor-frame accessors, same pattern as the IMU's local* properties).
  • Collider-only children: Unity delivers OnCollision* messages to the GameObject holding the touched collider. For articulated robots the colliders usually live on child GameObjects of the body link, so ContactSensor automatically attaches a lightweight ContactEventRelay to collider children (recursion stops at children that carry their own Rigidbody/ArticulationBody — those belong to another body). For setups outside this default scan, the public RegisterCollider(Collider) API lets the integrator wire any collider explicitly.
  • A collider destroyed mid-contact never sends OnCollisionExit; destroyed keys are pruned every sensor tick so isContact cannot get stuck.

ROS (Packages/UnitySensorsROS)

  • The serializers bind to generic source interfaces rather than the contact sensor itself, so they stay reusable: BoolMsgSerializer reads IBoolStateInterface (any boolean-state sensor) and WrenchStampedMsgSerializer reads IWrenchInterface (a local-frame force/torque pair — e.g. a future force-torque sensor can publish through the same serializer). ContactSensor implements both. Publishers are the usual thin RosMsgPublisher subclasses.
  • The wrench is expressed in the header frame (the sensor link), converted with .To().
  • Only message types shipped with ROS-TCP-Connector are used — no custom .msg files.

Validation

Tested end-to-end inside a ROS 2 (Humble) robot simulator built on this package (Unity 6000.3, Linux player): a 1 kg box with a contact sensor spawned 0.5 m above the ground —

  • while falling, /contact publishes false; on landing a false → true transition is observed and persists at rest (including after the body sleeps)
  • the resting /contact/wrench force points along the link-frame +z and matches the measured weight of the resting stack
  • sensor at 20 Hz with the publisher rate decoupled from the physics rate

Out of scope (possible follow-ups)

  • Sample scene/prefab (a meaningful demo needs a floor + falling body scene; happy to add one in a follow-up)
  • Publishing the per-contact array (no suitable standard message; would require a custom .msg)
  • Collider/layer filtering to observe only a subset of the link's colliders

hijimasa and others added 2 commits August 11, 2026 06:35
Add a ContactSensor to the UnitySensors package that tracks active
collisions via OnCollisionEnter/Stay/Exit. Unity delivers those
messages to the GameObject holding the touched collider, so the sensor
automatically relays events from collider-only children (via
ContactEventRelay) and exposes RegisterCollider() for colliders
outside the default scan. Per sampling tick it exposes whether any
contact is active, the list of active contacts (collider name,
position, normal, force estimated from Collision.impulse), and the
net contact force/torque, both in world and sensor-local frames.

The ROS serializers bind to new generic interfaces rather than the
contact sensor itself, so they stay reusable for future sensors:
IBoolStateInterface (std_msgs/Bool, bumper-style state) and
IWrenchInterface (geometry_msgs/WrenchStamped, a local-frame wrench -
e.g. a future force-torque sensor). Both publishers follow the
existing RosMsgPublisher/RosMsgSerializer pattern and use only
message types shipped with ROS-TCP-Connector.

Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
Unity delivers OnCollision* only to the GameObject carrying the Rigidbody or
ArticulationBody, never to a collider-only child. The sensor assumed the opposite -
"delivered to the GameObject that holds the touched collider" - and put relays on the
children, so a sensor placed on a part that has no body of its own heard nothing at all.

That is the normal case for URDF: a link attached by a fixed joint has no degree of
freedom, so a simulator may well give it no body and let its colliders belong to the
nearest ancestor. A bumper plate bolted to a chassis is exactly this, and it silently
reported no contact ever.

RegisterBodyRelay() subscribes the sensor to another GameObject's collisions, and
RegisterOwnCollider() says which colliders are the sensor's own. Once any is declared,
contacts elsewhere on the shared body are dropped - without that a sensor on one plate
would fire whenever any part of the whole assembly touched anything.

UpdateContact now walks every contact point rather than only the first, because one
Collision can carry points on several of the body's colliders and only some may be ours.
Where only part of them are, the reported force is the pair impulse scaled by their
share; the impulse is not available per point.

Verified in a scene with two collider-only children under one ArticulationBody: the body
GameObject receives the callbacks (the children receive none), and
ContactPoint.thisCollider names the child that was touched, so the filter has something
to key on.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
@hijimasa

Copy link
Copy Markdown
Contributor Author

Pushed a follow-up commit. The sensor reported nothing on a link whose colliders belong
to an ancestor body — the common case in URDF, since a fixed-joint link has no degree of
freedom and an importer may give it no body of its own.

The first version assumed OnCollision* reaches the GameObject holding the touched
collider. Unity only delivers it to the GameObject carrying the Rigidbody or
ArticulationBody, so relays on collider-only children never fired, and the sensor
reported "no contact" forever instead of failing loudly.

Added:

  • RegisterBodyRelay(GameObject) — subscribe to another GameObject's collisions when
    the body sits above the sensor's link.
  • RegisterOwnCollider(Collider) — declare the sensor's own colliders. Required once
    the relay is used, since the shared body reports contacts for the whole assembly.
    With none declared the behaviour is unchanged, so existing users are unaffected.
  • UpdateContact walks every contact point, not just the first: one Collision can
    carry points on several of the body's colliders.

Verified with two collider-only children under one ArticulationBody, ball dropped on
one of them:

body GameObject       : 11 calls, thisColliders=[left], otherColliders=[Sphere]
left plate GameObject :  0 calls
right plate GameObject:  0 calls

Callbacks go to the body, children get nothing, and ContactPoint.thisCollider names
the child that was touched. On a real robot, a projectile on one plate now raises that
plate's flag only, and pressing the chassis bumpers into a wall leaves all plates
silent.

Caveat: Collision.impulse is per collider pair, not per contact point. When a
collision mixes our points with others', the reported force is the pair impulse scaled
by our share. The boolean state is exact; the wrench in that case is approximate. Happy
to change this if there is a way to get a per-point impulse.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant