Bullet-featherstone: GetContactsFromLastStep may not return correct collision id

[iche033]

Environment

• OS Version: 24.04
• Source or binary build?
  source build, main branch

Description

• Expected behavior: GetContactsFromLastStep returns the correct collision id for a link containing collisions made of simple primitive shapes and convex decomposed shapes
• Actual behavior: GetContactsFromLastStep may not return the correct collision id

A contact point returned by bullet contains an index, e.g. m_index0, to indicate the index of the child in a compound shape with which the contact is associated. However when you have nested compound shapes, there is ambiguity in determining the correct child shape from one index value alone.

From the comments in the code for identifying collisions in bullet-featherstone/src/SimulationFeatures.cc:

      // todo(iche033) We do not have sufficient info to determine which
      // child shape is the collision if the link has convex decomposed mesh
      // collisions alongside of other collisions. See following example:
      // parentLink -> boxShape0
      //            -> boxShape1
      //            -> compoundShape -> convexShape0
      //                             -> convexShape1
      // A _childIndex of 1 is ambiguous as it could refer to either
      // boxShape1 or convexShape1
      // return nullptr in this case to indicate ambiguity

One option is to refactor the code in SDFFeatures.cc for creating collisions for convex decomposed meshes. Instead of creating a nested compound shape to hold all convex shapes (btConvexHullShape), add these shapes directly to the parent link shape (also a compound shape).