Recompute NavMesh with STATIC RigidObjects

habitat_sim/simulator.py

@@ -329,8 +329,12 @@ def contact_test(self, object_id, scene_id=0):
     def get_world_time(self, scene_id=0):
         return self._sim.get_world_time()
 
-    def recompute_navmesh(self, pathfinder, navmesh_settings):
-        return self._sim.recompute_navmesh(pathfinder, navmesh_settings)
+    def recompute_navmesh(
+        self, pathfinder, navmesh_settings, include_static_objects=False
+    ):
+        return self._sim.recompute_navmesh(
+            pathfinder, navmesh_settings, include_static_objects
+        )
 
     # --- lighting functions ---
     def get_light_setup(self, key=DEFAULT_LIGHTING_KEY):

src/esp/assets/ResourceManager.cpp

@@ -758,14 +758,20 @@ int ResourceManager::getObjectID(const std::string& configFile) {
       std::find(physicsObjectConfigList_.begin(),
                 physicsObjectConfigList_.end(), configFile);
   if (itr == physicsObjectConfigList_.cend()) {
-    return -1;
+    return ID_UNDEFINED;
   } else {
     int objectID = std::distance(physicsObjectConfigList_.begin(), itr);
     return objectID;
   }
 }
 
 std::string ResourceManager::getObjectConfig(const int objectID) {
+  if (physicsObjectConfigList_.size() <= objectID) {
+    Corrade::Utility::Debug() << "ResourceManager::getObjectConfig - Aborting. "
+                                 "No template with index "
+                              << objectID;
+    return "";
+  }
   return physicsObjectConfigList_[objectID];
 }
 

src/esp/bindings/SimBindings.cpp

@@ -99,7 +99,7 @@ void initSimBindings(py::module& m) {
       .def("contact_test", &Simulator::contactTest, "object_id"_a,
            "sceneID"_a = 0)
       .def("recompute_navmesh", &Simulator::recomputeNavMesh, "pathfinder"_a,
-           "navmesh_settings"_a)
+           "navmesh_settings"_a, "include_static_objects"_a)
       .def("get_light_setup", &Simulator::getLightSetup,
            "key"_a = assets::ResourceManager::DEFAULT_LIGHTING_KEY)
       .def("set_light_setup", &Simulator::setLightSetup, "light_setup"_a,

src/esp/physics/PhysicsManager.cpp

@@ -517,5 +517,17 @@ scene::SceneNode& PhysicsManager::getObjectSceneNode(int physObjectID) {
           physObjectID));
 }
 
+const scene::SceneNode& PhysicsManager::getObjectVisualSceneNode(
+    int physObjectID) const {
+  assertIDValidity(physObjectID);
+  return *existingObjects_.at(physObjectID)->visualNode_;
+}
+
+const assets::PhysicsObjectAttributes&
+PhysicsManager::getInitializationAttributes(const int physObjectID) const {
+  assertIDValidity(physObjectID);
+  return existingObjects_.at(physObjectID)->getInitializationAttributes();
+}
+
 }  // namespace physics
 }  // namespace esp

src/esp/physics/PhysicsManager.h

@@ -763,6 +763,15 @@ class PhysicsManager {
   /** @overload */
   scene::SceneNode& getObjectSceneNode(int physObjectID);
 
+  /**
+   * @brief Get a const reference to the specified object's visual SceneNode for
+   * info query purposes.
+   * @param physObjectID The object ID and key identifying the object in @ref
+   * PhysicsManager::existingObjects_.
+   * @return Const reference to the object's visual scene node.
+   */
+  const scene::SceneNode& getObjectVisualSceneNode(int physObjectID) const;
+
   /** @brief Render any debugging visualizations provided by the underlying
    * physics simulator implementation. By default does nothing. See @ref
    * BulletPhysicsManager::debugDraw.
@@ -795,6 +804,16 @@ class PhysicsManager {
     return activePhysSimLib_;
   };
 
+  /**
+   * @brief Get the template used to initialize an object.
+   *
+   * PhysicsObjectAttributes templates are expected to be changed between
+   * instances of objects.
+   * @return The initialization settings of the specified object instance.
+   */
+  const assets::PhysicsObjectAttributes& getInitializationAttributes(
+      const int physObjectID) const;
+
  protected:
   /** @brief Check that a given object ID is valid (i.e. it refers to an
    * existing object). Terminate the program and report an error if not. This

src/esp/physics/RigidObject.cpp

@@ -28,7 +28,7 @@ bool RigidObject::initializeScene(
 }
 
 bool RigidObject::initializeObject(
-    const assets::PhysicsObjectAttributes&,
+    const assets::PhysicsObjectAttributes& physicsObjectAttributes,
     const std::vector<assets::CollisionMeshData>&) {
   // TODO (JH): Handling static/kinematic object type
   if (rigidObjectType_ != RigidObjectType::NONE) {
@@ -41,6 +41,8 @@ bool RigidObject::initializeObject(
   // default kineamtic unless a simulator is initialized...
   objectMotionType_ = MotionType::KINEMATIC;
 
+  initializationAttributes_ = physicsObjectAttributes;
+
   return true;
 }
 

src/esp/physics/RigidObject.h

@@ -21,6 +21,10 @@
 #include "esp/scene/SceneNode.h"
 
 namespace esp {
+
+namespace assets {
+class PhysicsObjectAttributes;
+}
 namespace physics {
 
 /**
@@ -545,6 +549,17 @@ class RigidObject : public Magnum::SceneGraph::AbstractFeature3D {
    */
   virtual void setAngularDamping(CORRADE_UNUSED const double angDamping){};
 
+  /**
+   * @brief Get the template used to initialize this object.
+   *
+   * PhysicsObjectAttributes templates are expected to be changed between
+   * instances of objects.
+   * @return The initialization settings of this object instance.
+   */
+  const assets::PhysicsObjectAttributes& getInitializationAttributes() const {
+    return initializationAttributes_;
+  };
+
   /** @brief public @ref esp::assets::Attributes object for user convenience.
    * Store whatever object attributes you want here! */
   assets::Attributes attributes_;
@@ -574,6 +589,11 @@ class RigidObject : public Magnum::SceneGraph::AbstractFeature3D {
    * identifies the object as uninitialized.*/
   RigidObjectType rigidObjectType_ = RigidObjectType::NONE;
 
+  /**
+   * @brief Saved attributes when the object was initialized.
+   */
+  assets::PhysicsObjectAttributes initializationAttributes_;
+
   /** @brief Used to synchronize other simulator's notion of the object state
    * after it was changed kinematically. Called automatically on kinematic
    * updates.*/

src/esp/physics/bullet/BulletRigidObject.cpp

@@ -174,6 +174,8 @@ bool BulletRigidObject::initializeObject(
     return false;
   }
 
+  initializationAttributes_ = physicsObjectAttributes;
+
   //! Turn on scene flag
   rigidObjectType_ = RigidObjectType::OBJECT;
   objectMotionType_ = MotionType::DYNAMIC;

src/esp/sim/Simulator.cpp

@@ -8,7 +8,9 @@
 
 #include <Corrade/Utility/Directory.h>
 #include <Corrade/Utility/String.h>
+#include <Magnum/EigenIntegration/GeometryIntegration.h>
 
+#include "esp/assets/Attributes.h"
 #include "esp/core/esp.h"
 #include "esp/gfx/Drawable.h"
 #include "esp/gfx/RenderCamera.h"
@@ -275,6 +277,17 @@ int Simulator::getPhysicsObjectLibrarySize() {
   return resourceManager_.getNumLibraryObjects();
 }
 
+assets::PhysicsObjectAttributes& Simulator::getPhysicsObjectAttributes(
+    int templateIndex) {
+  return resourceManager_.getPhysicsObjectAttributes(
+      resourceManager_.getObjectConfig(templateIndex));
+}
+
+assets::PhysicsObjectAttributes& Simulator::getPhysicsObjectAttributes(
+    const std::string& templateHandle) {
+  return resourceManager_.getPhysicsObjectAttributes(templateHandle);
+}
+
 // return a list of existing objected IDs in a physical scene
 std::vector<int> Simulator::getExistingObjectIDs(const int sceneID) {
   if (sceneHasPhysics(sceneID)) {
@@ -404,7 +417,8 @@ double Simulator::getWorldTime() {
 }
 
 bool Simulator::recomputeNavMesh(nav::PathFinder& pathfinder,
-                                 const nav::NavMeshSettings& navMeshSettings) {
+                                 const nav::NavMeshSettings& navMeshSettings,
+                                 bool includeStaticObjects) {
   CORRADE_ASSERT(
       config_.createRenderer,
       "Simulator::recomputeNavMesh: SimulatorConfiguration::createRenderer is "
@@ -415,6 +429,41 @@ bool Simulator::recomputeNavMesh(nav::PathFinder& pathfinder,
   assets::MeshData::uptr joinedMesh =
       resourceManager_.createJoinedCollisionMesh(config_.scene.id);
 
+  // add STATIC collision objects
+  if (includeStaticObjects) {
+    for (auto objectID : physicsManager_->getExistingObjectIDs()) {
+      if (physicsManager_->getObjectMotionType(objectID) ==
+          physics::MotionType::STATIC) {
+        auto objectTransform = Magnum::EigenIntegration::cast<
+            Eigen::Transform<float, 3, Eigen::Affine> >(
+            physicsManager_->getObjectVisualSceneNode(objectID)
+                .absoluteTransformationMatrix());
+        const assets::PhysicsObjectAttributes& initializationTemplate =
+            physicsManager_->getInitializationAttributes(objectID);
+        objectTransform.scale(Magnum::EigenIntegration::cast<vec3f>(
+            initializationTemplate.getMagnumVec3("scale")));
+        std::string meshHandle =
+            initializationTemplate.getString("collisionMeshHandle");
+        if (meshHandle.empty()) {
+          meshHandle = initializationTemplate.getString("renderMeshHandle");
+        }
+        assets::MeshData::uptr joinedObjectMesh =
+            resourceManager_.createJoinedCollisionMesh(meshHandle);
+        int prevNumIndices = joinedMesh->ibo.size();
+        int prevNumVerts = joinedMesh->vbo.size();
+        joinedMesh->ibo.resize(prevNumIndices + joinedObjectMesh->ibo.size());
+        for (size_t ix = 0; ix < joinedObjectMesh->ibo.size(); ++ix) {
+          joinedMesh->ibo[ix + prevNumIndices] =
+              joinedObjectMesh->ibo[ix] + prevNumVerts;
+        }
+        joinedMesh->vbo.reserve(joinedObjectMesh->vbo.size() + prevNumVerts);
+        for (auto& vert : joinedObjectMesh->vbo) {
+          joinedMesh->vbo.push_back(objectTransform * vert);
+        }
+      }
+    }
+  }
+
   if (!pathfinder.build(navMeshSettings, *joinedMesh)) {
     LOG(ERROR) << "Failed to build navmesh";
     return false;

src/esp/sim/Simulator.h

@@ -97,15 +97,16 @@ class Simulator {
    * esp::physics::PhysicsManager::addObject().
    * @param objectLibIndex The index of the object's template in @ref
    * esp::assets::ResourceManager::physicsObjectLibrary_.
+   * @param attachmentNode If provided, attach the RigidObject Feature to this
+   * node instead of creating a new one.
+   * @param lightSetupKey The string key for the LightSetup to be used by this
+   * object.
    * @param sceneID !! Not used currently !! Specifies which physical scene to
    * add an object to.
    * @return The ID assigned to new object which identifies it in @ref
    * esp::physics::PhysicsManager::existingObjects_ or @ref esp::ID_UNDEFINED if
    * instancing fails.
    */
-  // int addObject(int objectLibIndex, int sceneID = 0);
-
-  /** @overload */
   int addObject(int objectLibIndex,
                 scene::SceneNode* attachmentNode = nullptr,
                 const std::string& lightSetupKey =
@@ -120,6 +121,19 @@ class Simulator {
    */
   int getPhysicsObjectLibrarySize();
 
+  /**
+   * @brief Get an editable reference to a physics object template by index.
+   */
+  assets::PhysicsObjectAttributes& getPhysicsObjectAttributes(
+      int templateIndex);
+
+  /**
+   * @brief Get an editable reference to a physics object template by string
+   * key.
+   */
+  assets::PhysicsObjectAttributes& getPhysicsObjectAttributes(
+      const std::string& templateHandle);
+
   /**
    * @brief Remove an instanced object by ID. See @ref
    * esp::physics::PhysicsManager::removeObject().
@@ -315,7 +329,8 @@ class Simulator {
    * @return Whether or not the navmesh recomputation succeeded.
    */
   bool recomputeNavMesh(nav::PathFinder& pathfinder,
-                        const nav::NavMeshSettings& navMeshSettings);
+                        const nav::NavMeshSettings& navMeshSettings,
+                        bool includeStaticObjects = false);
 
   agent::Agent::ptr getAgent(int agentId);
 

src/tests/SimTest.cpp

@@ -6,12 +6,14 @@
 #include <Corrade/TestSuite/Tester.h>
 #include <Corrade/Utility/Directory.h>
 #include <Magnum/DebugTools/CompareImage.h>
+#include <Magnum/EigenIntegration/Integration.h>
 #include <Magnum/ImageView.h>
 #include <Magnum/Magnum.h>
 #include <Magnum/PixelFormat.h>
 #include <string>
 
 #include "esp/assets/ResourceManager.h"
+#include "esp/physics/RigidObject.h"
 #include "esp/sim/Simulator.h"
 
 #include "configure.h"
@@ -88,6 +90,7 @@ struct SimTest : Cr::TestSuite::Tester {
   void updateLightSetupRGBAObservation();
   void updateObjectLightSetupRGBAObservation();
   void multipleLightingSetupsRGBAObservation();
+  void recomputeNavmeshWithStaticObjects();
 
   // TODO: remove outlier pixels from image and lower maxThreshold
   const Magnum::Float maxThreshold = 255.f;
@@ -109,7 +112,8 @@ SimTest::SimTest() {
             &SimTest::getCustomLightingRGBAObservation,
             &SimTest::updateLightSetupRGBAObservation,
             &SimTest::updateObjectLightSetupRGBAObservation,
-            &SimTest::multipleLightingSetupsRGBAObservation});
+            &SimTest::multipleLightingSetupsRGBAObservation,
+            &SimTest::recomputeNavmeshWithStaticObjects});
   // clang-format on
 }
 
@@ -315,6 +319,64 @@ void SimTest::multipleLightingSetupsRGBAObservation() {
       *simulator, "SimTestExpectedDifferentLighting.png", maxThreshold, 0.01f);
 }
 
+void SimTest::recomputeNavmeshWithStaticObjects() {
+  auto simulator = getSimulator(skokloster);
+
+  // compute the initial navmesh
+  esp::nav::NavMeshSettings navMeshSettings;
+  navMeshSettings.setDefaults();
+  simulator->recomputeNavMesh(*simulator->getPathFinder().get(),
+                              navMeshSettings);
+
+  esp::vec3f randomNavPoint =
+      simulator->getPathFinder()->getRandomNavigablePoint();
+  while (simulator->getPathFinder()->distanceToClosestObstacle(randomNavPoint) <
+             1.0 ||
+         randomNavPoint[1] > 1.0) {
+    randomNavPoint = simulator->getPathFinder()->getRandomNavigablePoint();
+  }
+
+  // add static object at a known navigable point
+  int objectID = simulator->addObject(0);
+  simulator->setTranslation(Magnum::Vector3{randomNavPoint}, objectID);
+  simulator->setObjectMotionType(esp::physics::MotionType::STATIC, objectID);
+  CORRADE_VERIFY(
+      simulator->getPathFinder()->isNavigable({randomNavPoint}, 0.1));
+
+  // recompute with object
+  simulator->recomputeNavMesh(*simulator->getPathFinder().get(),
+                              navMeshSettings, true);
+  CORRADE_VERIFY(!simulator->getPathFinder()->isNavigable(randomNavPoint, 0.1));
+
+  // recompute without again
+  simulator->recomputeNavMesh(*simulator->getPathFinder().get(),
+                              navMeshSettings, false);
+  CORRADE_VERIFY(simulator->getPathFinder()->isNavigable(randomNavPoint, 0.1));
+
+  simulator->removeObject(objectID);
+
+  // test scaling
+  esp::assets::PhysicsObjectAttributes& objectTemplate =
+      simulator->getPhysicsObjectAttributes(0);
+  objectTemplate.setMagnumVec3("scale", {0.5, 0.5, 0.5});
+  objectID = simulator->addObject(0);
+  simulator->setTranslation(Magnum::Vector3{randomNavPoint}, objectID);
+  simulator->setTranslation(
+      simulator->getTranslation(objectID) + Magnum::Vector3{0, 0.5, 0},
+      objectID);
+  simulator->setObjectMotionType(esp::physics::MotionType::STATIC, objectID);
+  esp::vec3f offset(0.75, 0, 0);
+  CORRADE_VERIFY(simulator->getPathFinder()->isNavigable(randomNavPoint, 0.1));
+  CORRADE_VERIFY(
+      simulator->getPathFinder()->isNavigable(randomNavPoint + offset, 0.2));
+  // recompute with object
+  simulator->recomputeNavMesh(*simulator->getPathFinder().get(),
+                              navMeshSettings, true);
+  CORRADE_VERIFY(!simulator->getPathFinder()->isNavigable(randomNavPoint, 0.1));
+  CORRADE_VERIFY(
+      simulator->getPathFinder()->isNavigable(randomNavPoint + offset, 0.2));
+}
+
 }  // namespace
 
 CORRADE_TEST_MAIN(SimTest)