ContactSensor does not contain information about Contact Depth, Normal and Wrench

[antbre]

Environment

• OS Version: Ubuntu 22.04
• Source or binary build? Binary Build gz-garden

Description

• Expected behavior: When including a contact sensor in a model the sensor publishes fully filled gz::msgs::Contacts messages containing information about the Contact's position, depth, normal, and wrench.
• Actual behavior: When including a contact sensor in a model the sensor publishes gz::msgs::Contacts messages that only contain information about the contact position with the functions depth_size(), normal_size(),wrench_size() each returning 0 as the arrays are empty.

Steps to reproduce

1. Start a simulation that contains a model with a contact sensor e.g. via gz sim visualize_contacts.sdf
2. Start the simulation with the play button
3. Echo the contact sensor topic via gz topic -e -t /world/visualize_contacts/model/cylinder/link/link/sensor/contact_sensor/contact and observe that the topic only contains information about the contact position.

Output

An example of a single gz::msgs::Contacts message:

header {
  stamp {
    sec: 4
    nsec: 566000000
  }
}
contact {
  header {
    stamp {
      sec: 4
      nsec: 566000000
    }
  }
  collision1 {
    id: 11
    name: "cylinder::link::collision"
  }
  collision2 {
    id: 16
    name: "vehicle::chassis::collision"
  }
  position {
    x: -0.52961003326708211
    y: -0.22961005942088414
    z: 0.7843629869615566
  }
  position {
    x: -0.62471763394302493
    y: -4.0153125908118034e-12
    z: 0.78436298671342319
  }
  position {
    x: -0.52961003327763323
    y: 0.22961005941722351
    z: 0.78436298696155571
  }
  position {
    x: -0.29999997385592914
    y: 0.32471766009116332
    z: 0.78436298756060141
  }
  position {
    x: -0.070389914429854733
    y: 0.22961005942777446
    z: 0.78436298815964733
  }
  position {
    x: 0.024717686246088
    y: 1.0905690819050139e-11
    z: 0.78436298840778063
  }
  position {
    x: -0.070389914419303978
    y: -0.22961005941033347
    z: 0.78436298815964822
  }
  position {
    x: -0.29999997384100807
    y: -0.32471766008427305
    z: 0.78436298756060252
  }
}

I suppose this is related to the (closed) issue #112 and when testing I can see the blue spheres but no green cylinders as shown in pull #234.

[antbre]

After investigating a bit it seems that the issue is that the physics engine does not fill the ContactSensorData correctly. When looking at the Contact system defined in include/systems/contact/Contact.hh and in particular looking at the UpdateSensors function:

//////////////////////////////////////////////////
void ContactPrivate::UpdateSensors(const UpdateInfo &_info,
                                   const EntityComponentManager &_ecm)
{
  GZ_PROFILE("ContactPrivate::UpdateSensors");
  for (const auto &item : this->entitySensorMap)
  {
    for (const Entity &entity : item.second->collisionEntities)
    {
      auto contacts = _ecm.Component<components::ContactSensorData>(entity);      

      // We will assume that the ContactData component will have been created if
      // this entity is in the collisionEntities list
      if (contacts->Data().contact_size() > 0)
      {
        item.second->AddContacts(_info.simTime, contacts->Data());
      }
    }
  }
}

I can see that the contacts in this line auto contacts = _ecm.Component<components::ContactSensorData>(entity); already does not contain any normal, depths, or wrench information. Not sure how to proceed from here, whether it's a matter of physics engine versioning or something else.

[azeey]

Yes, this is a missing feature. gz-physics exposes those fields in an extras data structure (see gazebosim/gz-physics#40) but the physics system is not reading them. The tests in that PR would be a good reference if you're interested in submitting a fix, which we would really appreciate.

[antbre]

I'd be happy to try my hand at adding this fix. I don't have too much experience with the gz libraries so maybe a few pointers on how to go about the implementation would be helpful.

Given my limited experience I currently see two ways to implement this:

1. Update the _ecm.components<>() function to fill the extra data when returning the contact
2. Add the additional data where gz-sim is interfacing (?) with gz-physics and add the additional data there.

Any hints would be appreciated.

[azeey]

I think just updating the Physics system (where gz-sim interfaces with gz-physics) would be sufficient for this. If you look at

gz-sim/src/systems/physics/Physics.cc

Line 3764 in 50a99e6

const auto &contact = contactComposite.Get<WorldShapeType::ContactPoint>();

we're only retrieving the WorldShapeType::ContactPoint type from the contactComposite, but contactComposite also contains ExtraContactData, so it's a matter of querying that type from contactComposite and populating contactMsg for each contact. See this gz-physics test to see how to query ExtraContactData.