ODE contact points not displaying on bodies which are not moving

[osrf-migration]

Original report (archived issue) by Jennifer Buehler (Bitbucket: JenniferBuehler).

---

ODE has a nice feature to automatically disable bodies when they are not moving. This is great for performance. However there is one issue with this.
ODEPhysics skips disabled bodies in ODEPhysics::CollisionCallback which is called from ODEPhysics::UpdateCollision.

This affects computation of the contact points in the function ODEPhysics::Collide() which is then called later for all bodies not skipped in CollisionCallback. So in effect, no contact points are computed for links which are not moving and which are in contact with other non-moving links. This is certainly done for performance reasons.

The issue with this is: ODEPhysics::Collide() adds the contact information to the physics::ContactManager. But Collide() is only called for enabled bodies. Now if there is a subscriber to the contact points topic, they would not receive the contact points between the not moving links. But depending on the application, subscribers may also need the contact points between still objects. Example: the gzclient visualization doesn't show objects which are still. This can be tested by:

1. Starting gazebo in paused mode
2. Putting a sphere on the ground
3. Switching on Contacts visualization
4. Hit Play
5. Waiting until the object is still: The contact point will disappear.

This doesn't happen with Bullet.

I found this can be solved by some changes to ContactPointManager in my branch contact_manager_enforcable_additions (see also pull request #2629) and changing following lines in ODEPhysics::CollisionCallback:

    // Exit if both bodies are not enabled
    if (dGeomGetCategoryBits(_o1) != GZ_SENSOR_COLLIDE &&
        dGeomGetCategoryBits(_o2) != GZ_SENSOR_COLLIDE &&
        !self->contactManager->ContactsEnforced() &&
        !self->contactManager->SubscribersConnected(collision1, collision2) &&
        ((b1 && b2 && !dBodyIsEnabled(b1) && !dBodyIsEnabled(b2)) ||
        (!b2 && b1 && !dBodyIsEnabled(b1)) ||
        (!b1 && b2 && !dBodyIsEnabled(b2))))
    {
      return;
    }

Effectively this enforces the treatment of colliding objects if there are subscribers to the contact points (or if has been enforced for another reason).

However my only concern with this is that ODEPhysics::Collide() does not only fill in the contact point feedback, but also creates contact joints. I am not sure how they are used, and if this could badly impact performance while there are any contact point subscribers.

Any feedback on this would be appreciated so I can try to fix this up.

[osrf-migration]

Original comment by Louise Poubel (Bitbucket: chapulina, GitHub: chapulina).

---

• set component to "physics::ODEPhysics"

[osrf-migration]

Original comment by Jennifer Buehler (Bitbucket: JenniferBuehler).

---

See pull request #2709

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

• changed state from "new" to "resolved"

pull request #2709

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

Merged in JenniferBuehler/gazebo/fix_issue_2175 (pull request #2709)

Fix issue #2175

Approved-by: Addisu Z. Taddese addisu@openrobotics.org
Approved-by: Steven Peters scpeters@osrfoundation.org

→ <<cset 2ea5ebb>>

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

Fix issue #2175

→ <<cset 69b2841>>

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

Merged in JenniferBuehler/gazebo/fix_issue_2175 (pull request #2709)

Fix issue #2175

Approved-by: Addisu Z. Taddese addisu@openrobotics.org
Approved-by: Steven Peters scpeters@osrfoundation.org

→ <<cset 2ea5ebb>>