Merge branch 'main' into hssd_ao_utilities_JT

examples/tutorials/nb_python/ECCV_2020_Interactivity.py

@@ -320,7 +320,7 @@ def sample_object_state(
             )
 
         # test for penetration with the environment
-        if not sim.contact_test(obj.object_id):
+        if not obj.contact_test():
             valid_placement = True
 
     if not valid_placement:

examples/tutorials/notebooks/ECCV_2020_Interactivity.ipynb

@@ -319,7 +319,7 @@
     "            )\n",
     "\n",
     "        # test for penetration with the environment\n",
-    "        if not sim.contact_test(obj.object_id):\n",
+    "        if not obj.contact_test():\n",
     "            valid_placement = True\n",
     "\n",
     "    if not valid_placement:\n",

src/esp/bindings/PhysicsWrapperManagerBindings.cpp

@@ -233,6 +233,16 @@ void initPhysicsWrapperManagerBindings(pybind11::module& m) {
           R"(Instance an object into the scene via a template referenced by library id.
           Optionally attach the object to an existing SceneNode and assign its initial
           LightSetup key. Returns a reference to the created object.)")
+      .def(
+          "duplicate_object_by_id",
+#ifdef ESP_BUILD_WITH_BULLET
+          &RigidObjectManager::copyBulletObjectByID,
+#else
+          &RigidObjectManager::copyObjectByID,
+#endif
+          "object_id"_a,
+          R"(Duplicate an existing rigid object referenced by its ID and add it into the scene.
+          Returns a reference to the created object.)")
       .def(
           "add_object_by_template_handle",
 #ifdef ESP_BUILD_WITH_BULLET
@@ -300,7 +310,16 @@ void initPhysicsWrapperManagerBindings(pybind11::module& m) {
           R"(Instance an articulated object into the scene via a template referenced by its ID.
           Optionally force the articulated object's model to be reloaded from disk and assign its initial
           LightSetup key. Returns a reference to the created object.)")
-
+      .def(
+          "duplicate_articulated_object_by_id",
+#ifdef ESP_BUILD_WITH_BULLET
+          &ArticulatedObjectManager::copyBulletArticulatedObjectByID,
+#else
+          &ArticulatedObjectManager::copyArticulatedObjectByID,
+#endif
+          "ao_object_id"_a,
+          R"(Duplicate an existing articulated object referenced by its ID and add it into the scene.
+          Returns a reference to the created object.)")
       .def(
           "add_articulated_object_from_urdf",
 #ifdef ESP_BUILD_WITH_BULLET

src/esp/bindings/SimBindings.cpp

@@ -238,9 +238,6 @@ void initSimBindings(py::module& m) {
       .def("set_stage_is_collidable", &Simulator::setStageIsCollidable,
            "collidable"_a,
            R"(Set whether or not the static stage is collidable.)")
-      .def(
-          "contact_test", &Simulator::contactTest, "object_id"_a,
-          R"(DEPRECATED AND WILL BE REMOVED IN HABITAT-SIM 2.0. Run collision detection and return a binary indicator of penetration between the specified object and any other collision object. Physics must be enabled.)")
       .def(
           "get_physics_num_active_contact_points",
           &Simulator::getPhysicsNumActiveContactPoints,

src/esp/physics/ArticulatedObject.h

@@ -146,7 +146,9 @@ class ArticulatedLink : public RigidBase {
                   0,  // TODO: pass an actual object ID. This is currently
                       // assigned AFTER creation.
                   resMgr),
-        mbIndex_(index) {}
+        mbIndex_(index) {
+    setIsArticulated(true);
+  }
 
   ~ArticulatedLink() override = default;
 
@@ -263,9 +265,6 @@ class ArticulatedLink : public RigidBase {
   std::string linkName = "";
   std::string linkJointName = "";
 
-  /** @brief Return whether or not this object is articulated. */
-  bool isArticulated() const override { return true; }
-
  private:
   /**
    * @brief Finalize the initialization of this link.
@@ -301,7 +300,9 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
   ArticulatedObject(scene::SceneNode* rootNode,
                     assets::ResourceManager& resMgr,
                     int objectId)
-      : PhysicsObjectBase(rootNode, objectId, resMgr){};
+      : PhysicsObjectBase(rootNode, objectId, resMgr) {
+    setIsArticulated(true);
+  }
 
   ~ArticulatedObject() override {
     // clear links and delete their SceneNodes
@@ -996,9 +997,6 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
         metadata::attributes::ArticulatedObjectAttributes>();
   }
 
-  /** @brief Return whether or not this object is articulated. */
-  bool isArticulated() const override { return true; }
-
  protected:
   /**
    * @brief Used to synchronize simulator's notion of the object state

src/esp/physics/PhysicsManager.cpp

@@ -123,11 +123,27 @@ int PhysicsManager::addObject(int attributesID,
 int PhysicsManager::addObjectInstance(
     const esp::metadata::attributes::SceneObjectInstanceAttributes::cptr&
         objInstAttributes,
-    const std::string& attributesHandle,
     bool defaultCOMCorrection,
     DrawableGroup* drawables,
     scene::SceneNode* attachmentNode,
     const std::string& lightSetup) {
+  // Template attributes handle from instance
+  const std::string objAttrHandle = objInstAttributes->getHandle();
+  // Get full object template attributes handle
+  const std::string attributesHandle =
+      resourceManager_.getObjectAttributesManager()->getFullAttrNameFromStr(
+          objAttrHandle);
+
+  // make sure full handle is not empty, meaning template is not found in
+  // manager
+  ESP_CHECK(!attributesHandle.empty(),
+            Cr::Utility::formatString(
+                "PhysicsManager::addObjectInstance() : Attempt to "
+                "load object instance specified in current scene instance "
+                ":{} failed due to object instance configuration handle '{}' "
+                "being empty or unknown. Aborting",
+                simulator_->getActiveSceneDatasetName(), objAttrHandle));
+
   // Get ObjectAttributes
   auto objAttributes =
       resourceManager_.getObjectAttributesManager()->getObjectCopyByHandle(
@@ -136,8 +152,7 @@ int PhysicsManager::addObjectInstance(
   if (!objAttributes) {
     ESP_ERROR(Mn::Debug::Flag::NoSpace)
         << "Missing/improperly configured ObjectAttributes '"
-        << attributesHandle << "', whose handle contains '"
-        << objInstAttributes->getHandle()
+        << attributesHandle << "', whose handle contains '" << objAttrHandle
         << "' as specified in object instance attributes, so addObjectInstance "
            "aborted.";
     return ID_UNDEFINED;
@@ -184,6 +199,32 @@ int PhysicsManager::addObjectInstance(
 
 }  // PhysicsManager::addObjectInstance
 
+int PhysicsManager::cloneExistingObject(int objectID) {
+  // Retrieve object by ID
+  const auto existingObjIter = existingObjects_.find(objectID);
+  // Verify object id exists and is a rigid object
+  if (existingObjIter == existingObjects_.end()) {
+    ESP_ERROR(Mn::Debug::Flag::NoSpace)
+        << "Object cloning failed due to unknown existing object ID `"
+        << objectID << "`. Aborting";
+    return ID_UNDEFINED;
+  }
+  auto objPtr = existingObjIter->second;
+  // Get object instance attributes copy
+  esp::metadata::attributes::SceneObjectInstanceAttributes::cptr objInstAttrs =
+      objPtr->getInitObjectInstanceAttr();
+  // Create object instance
+  int newObjID = addObjectInstance(objInstAttrs, objPtr->isCOMCorrected(),
+                                   &simulator_->getDrawableGroup(), nullptr,
+                                   simulator_->getCurrentLightSetupKey());
+
+  // Update new object's values if necessary
+  // auto newObject = existingObjects_.find(newObjID);
+
+  return newObjID;
+
+}  // PhysicsManager::cloneExistingObject
+
 int PhysicsManager::addObjectAndSaveAttributes(
     const esp::metadata::attributes::ObjectAttributes::ptr& objAttributes,
     DrawableGroup* drawables,
@@ -434,9 +475,26 @@ int PhysicsManager::addArticulatedObjectInstance(
     const std::shared_ptr<
         const esp::metadata::attributes::SceneAOInstanceAttributes>&
         aObjInstAttributes,
-    const std::string& artObjAttrHandle,
     DrawableGroup* drawables,
     const std::string& lightSetup) {
+  // AO template attributes handle from articulated object instance
+  const std::string artObjHandle = aObjInstAttributes->getHandle();
+
+  // Get model file name from handle
+  const std::string artObjAttrHandle =
+      resourceManager_.getAOAttributesManager()->getFullAttrNameFromStr(
+          artObjHandle);
+
+  // make sure full handle is not empty
+  ESP_CHECK(
+      !artObjAttrHandle.empty(),
+      Cr::Utility::formatString(
+          "PhysicsManager::addArticulatedObjectInstance() : "
+          "Attempt to load articulated object instance specified in "
+          "current scene instance :{} failed due to AO instance "
+          "configuration file handle '{}' being empty or unknown. Aborting",
+          simulator_->getActiveSceneDatasetName(), artObjHandle));
+
   // Get ArticulatedObjectAttributes
   auto artObjAttributes =
       resourceManager_.getAOAttributesManager()->getObjectCopyByHandle(
@@ -510,6 +568,33 @@ int PhysicsManager::addArticulatedObjectInstance(
 
 }  // PhysicsManager::addArticulatedObjectInstance
 
+int PhysicsManager::cloneExistingArticulatedObject(int aObjectID) {
+  // Retrieve object by ID
+  const auto existingAOIter = existingArticulatedObjects_.find(aObjectID);
+  // Verify object id exists and is a rigid object
+  if (existingAOIter == existingArticulatedObjects_.end()) {
+    ESP_ERROR(Mn::Debug::Flag::NoSpace)
+        << "Articulated Object cloning failed due to unknown existing "
+           "articulated object ID `"
+        << aObjectID << "`. Aborting";
+    return ID_UNDEFINED;
+  }
+  auto aObjPtr = existingAOIter->second;
+  // Get object instance attributes copy
+  esp::metadata::attributes::SceneAOInstanceAttributes::cptr artObjInstAttrs =
+      aObjPtr->getInitObjectInstanceAttr();
+  // Create object instance
+  int newArtObjID = addArticulatedObjectInstance(
+      artObjInstAttrs, &simulator_->getDrawableGroup(),
+      simulator_->getCurrentLightSetupKey());
+
+  // Update new object's values if necessary
+  // auto newArtObj = existingArticulatedObjects_.find(newArtObjID);
+
+  return newArtObjID;
+
+}  // PhysicsManager::cloneExistingArticulatedObject
+
 int PhysicsManager::addArticulatedObjectAndSaveAttributes(
     const esp::metadata::attributes::ArticulatedObjectAttributes::ptr&
         artObjAttributes,

src/esp/physics/PhysicsManager.h

@@ -296,8 +296,6 @@ class PhysicsManager : public std::enable_shared_from_this<PhysicsManager> {
    * esp::metadata::attributes::SceneObjectInstanceAttributes file.
    * @param objInstAttributes The attributes that describe the desired state to
    * set this object.
-   * @param attributesHandle The handle of the object attributes used as the key
-   * to query @ref esp::metadata::managers::ObjectAttributesManager.
    * @param defaultCOMCorrection The default value of whether COM-based
    * translation correction needs to occur.
    * @param drawables Reference to the scene graph drawables group to enable
@@ -313,12 +311,22 @@ class PhysicsManager : public std::enable_shared_from_this<PhysicsManager> {
   int addObjectInstance(
       const esp::metadata::attributes::SceneObjectInstanceAttributes::cptr&
           objInstAttributes,
-      const std::string& attributesHandle,
       bool defaultCOMCorrection = false,
       DrawableGroup* drawables = nullptr,
       scene::SceneNode* attachmentNode = nullptr,
       const std::string& lightSetup = DEFAULT_LIGHTING_KEY);
 
+  /**
+   * @brief Duplicate an existing rigid using its @ref
+   * esp::metadata::attributes::SceneObjectInstanceAttributes template.
+   * @param objectID The ID of the existing @ref RigidObject we wish to duplicate.
+   * @return The instanced @ref RigidObject 's ID, mapping to the rigid
+   * object in @ref PhysicsManager::existingObjects_ if successful, or
+   * @ref esp::ID_UNDEFINED. These values come from the same pool used
+   * by articulated objects.
+   */
+  int cloneExistingObject(int objectID);
+
   /** @brief Instance a physical object from an object properties template in
    * the @ref esp::metadata::managers::ObjectAttributesManager.  This method
    * will query for a drawable group from simulator.
@@ -405,12 +413,9 @@ class PhysicsManager : public std::enable_shared_from_this<PhysicsManager> {
 
   /**
    * @brief Instance and place an @ref ArticulatedObject from a @ref
-   * esp::metadata::attributes::SceneAOInstanceAttributes file.
+   * esp::metadata::attributes::SceneAOInstanceAttributes template.
    * @param aObjInstAttributes The attributes that describe the desired initial
    * state to set for this articulated object.
-   * @param attributesHandle The handle of the @ref ArticulatedObject attributes
-   * used as the key to query @ref esp::metadata::managers::AOAttributesManager
-   * for the attributes.
    * @param drawables Reference to the scene graph drawables group to enable
    * rendering of the newly initialized object. If nullptr, will attempt to
    * query Simulator to retrieve a group.
@@ -424,10 +429,20 @@ class PhysicsManager : public std::enable_shared_from_this<PhysicsManager> {
       const std::shared_ptr<
           const esp::metadata::attributes::SceneAOInstanceAttributes>&
           aObjInstAttributes,
-      const std::string& attributesHandle,
       DrawableGroup* drawables = nullptr,
       const std::string& lightSetup = DEFAULT_LIGHTING_KEY);
 
+  /**
+   * @brief Duplicate an existing @ref ArticulatedObject using its @ref
+   * esp::metadata::attributes::SceneAOInstanceAttributes template.
+   * @param aObjectID The ID of the existing @ref ArticulatedObject we wish to duplicate.
+   * @return The instanced @ref ArticulatedObject 's ID, mapping to the articulated
+   * object in @ref PhysicsManager::existingObjects_ if successful, or
+   * @ref esp::ID_UNDEFINED. These values come from the same pool used
+   * by rigid objects.
+   */
+  int cloneExistingArticulatedObject(int aObjectID);
+
   /**
    * @brief Instance an @ref ArticulatedObject from an
    * @ref esp::metadata::attributes::ArticulatedObjectAttributes retrieved from the
@@ -678,31 +693,6 @@ class PhysicsManager : public std::enable_shared_from_this<PhysicsManager> {
   virtual void debugDraw(
       CORRADE_UNUSED const Magnum::Matrix4& projTrans) const {}
 
-  /**
-   * @brief Check whether an object is in contact with any other objects or the
-   * scene.
-   *
-   * @param physObjectID The object ID and key identifying the object in @ref
-   * PhysicsManager::existingObjects_.
-   * @return Whether or not the object is in contact with any other collision
-   * enabled objects.
-   */
-  virtual bool contactTest(const int physObjectID) {
-    const auto existingObjsIter = existingObjects_.find(physObjectID);
-    bool existingObjFound = (existingObjsIter != existingObjects_.end());
-    const auto existingArtObjsIter =
-        existingArticulatedObjects_.find(physObjectID);
-    CORRADE_INTERNAL_ASSERT(
-        existingObjFound ||
-        (existingArtObjsIter != existingArticulatedObjects_.end()));
-    if (existingObjFound) {
-      return existingObjsIter->second->contactTest();
-    } else {
-      return existingArtObjsIter->second->contactTest();
-    }
-    return false;
-  }
-
   /**
    * @brief Perform discrete collision detection for the scene with the derived
    * PhysicsManager implementation. Not implemented for default @ref

src/esp/physics/PhysicsObjectBase.h

@@ -600,9 +600,11 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D {
 
   /** @brief Return whether or not this object is articulated. Override in
    * ArticulatedObject */
-  virtual bool isArticulated() const { return false; }
+  bool isArticulated() const { return _isArticulated; }
 
  protected:
+  void setIsArticulated(bool isArticulated) { _isArticulated = isArticulated; }
+
   /** @brief Accessed internally. Get an appropriately cast copy of the @ref
    * metadata::attributes::SceneObjectInstanceAttributes used to place the
    * object within the scene.
@@ -709,10 +711,6 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D {
   metadata::attributes::AbstractAttributes::cptr initializationAttributes_ =
       nullptr;
 
-  /**
-   * @brief Saved reference to this object's instantiating template manager
-   */
-  // metadata::managers::AttributesManager::ptr templateManager_ = nullptr;
   /**
    * @brief Set the object's creation scale
    */
@@ -733,6 +731,12 @@ class PhysicsObjectBase : public Magnum::SceneGraph::AbstractFeature3D {
    */
   Mn::Vector3 _creationScale{1.0f, 1.0f, 1.0f};
 
+  /**
+   * @brief Whether or not this is an articulated object. Set to true in
+   * articulated object constructors.
+   */
+  bool _isArticulated = false;
+
  public:
   ESP_SMART_POINTERS(PhysicsObjectBase)
 };  // class PhysicsObjectBase

src/esp/physics/RigidObject.h

@@ -163,6 +163,7 @@ class RigidObject : public RigidBase {
   void setIsCOMCorrected(bool _isCOMCorrected) {
     isCOMCorrected_ = _isCOMCorrected;
   }
+  bool isCOMCorrected() const { return isCOMCorrected_; }
 
   /**
    * @brief Reverses the COM correction transformation for objects that require

src/esp/physics/objectManagers/ArticulatedObjectManager.cpp

@@ -74,5 +74,14 @@ ArticulatedObjectManager::addArticulatedObjectByID(
   return nullptr;
 }
 
+std::shared_ptr<ManagedArticulatedObject>
+ArticulatedObjectManager::copyArticulatedObjectByID(int aObjectID) {
+  if (auto physMgr = this->getPhysicsManager()) {
+    int newAObjID = physMgr->cloneExistingArticulatedObject(aObjectID);
+    return this->getObjectCopyByID(newAObjID);
+  }
+  return nullptr;
+}  // ArticulatedObjectManager::copyArticulatedObjectByID
+
 }  // namespace physics
 }  // namespace esp