add ManagedArticulatedObject attribute for directly querying the link…

…_id to object_id map (#2410)

src/esp/bindings/PhysicsObjectBindings.cpp

@@ -460,6 +460,12 @@ void declareArticulatedObjectWrapper(py::module& m,
  ("Get a dict mapping Habitat object ids to this " +
  objType + "'s link ids.")
  .c_str())
+ .def_property_readonly(
+ "link_ids_to_object_ids",
+ &ManagedArticulatedObject::getLinkIdsToObjectIds,
+ ("Get a dict mapping local link ids to Habitat object ids for this " +
+ objType + "'s link ids.")
+ .c_str())
  .def("get_link_id_from_name",
  &ManagedArticulatedObject::getLinkIdFromName,
  ("Get this " + objType +

src/esp/physics/ArticulatedObject.h

@@ -470,6 +470,15 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
  return objectIdToLinkId_;
  }
 
+ /**
+ * @brief Get a map of link ids to object ids.
+ *
+ * @return A a map of link ids to Habitat object ids for this AO's links.
+ */
+ std::unordered_map<int, int> getLinkIdsToObjectIds() const {
+ return linkIdToObjectId_;
+ }
+
  /**
  * @brief Given the list of passed points in this object's local space, return
  * those points transformed to world space.
@@ -934,6 +943,8 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
 
  //! map PhysicsManager objectId to local multibody linkId
  std::unordered_map<int, int> objectIdToLinkId_;
+ //! map local multibody linkId to PhysicsManager objectId
+ std::unordered_map<int, int> linkIdToObjectId_;
 
  /**
  * @brief Returns the @ref

src/esp/physics/bullet/BulletPhysicsManager.cpp

@@ -168,6 +168,7 @@ int BulletPhysicsManager::addArticulatedObjectInternal(
  ++linkIx) {
  int linkObjectId = allocateObjectID();
  articulatedObject->objectIdToLinkId_[linkObjectId] = linkIx;
+ articulatedObject->linkIdToObjectId_[linkIx] = linkObjectId;
  collisionObjToObjIds_->emplace(
  articulatedObject->btMultiBody_->getLinkCollider(linkIx), linkObjectId);
  ArticulatedLink& linkObject = articulatedObject->getLink(linkIx);

src/esp/physics/objectWrappers/ManagedArticulatedObject.h

@@ -104,6 +104,13 @@ class ManagedArticulatedObject
  return {};
  }
 
+ std::unordered_map<int, int> getLinkIdsToObjectIds() const {
+ if (auto sp = getObjectReference()) {
+ return sp->getLinkIdsToObjectIds();
+ }
+ return {};
+ }
+
  void setRootLinearVelocity(const Mn::Vector3& linVel) {
  if (auto sp = getObjectReference()) {
  sp->setRootLinearVelocity(linVel);

tests/test_physics.py

@@ -909,6 +909,19 @@ def test_articulated_object_add_remove():
  assert robot.is_alive
  assert robot.object_id == habitat_sim.stage_id + 1 # first robot added
 
+ # test object id to link mapping
+ link_ids = robot.get_link_ids()
+ obj_ids_to_link_ids = robot.link_object_ids
+ link_ids_to_object_ids = robot.link_ids_to_object_ids
+ assert len(obj_ids_to_link_ids) == len(link_ids_to_object_ids)
+ assert len(link_ids_to_object_ids) == robot.num_links
+ for link_id, obj_id in link_ids_to_object_ids.items():
+ assert obj_ids_to_link_ids[obj_id] == link_id
+ for obj_id, link_id in obj_ids_to_link_ids.items():
+ assert link_ids_to_object_ids[link_id] == obj_id
+ for link_id in link_ids:
+ assert link_id in link_ids_to_object_ids
+
  # add a second robot
  robot2 = art_obj_mgr.add_articulated_object_from_urdf(
  filepath=robot_file, global_scale=2.0