Physics benchmarking

data/default.phys_scene_config.json

@@ -1,6 +1,6 @@
 {
     "physics simulator": "bullet",
-    "timestep": 0.01,
+    "timestep": 0.0041666666,
     "gravity": [0,-9.8,0],
     "friction coefficient": 0.4,
     "restitution coefficient": 0.1,

examples/benchmark.py

@@ -39,6 +39,23 @@
     help="Whether to enable benchmarking of semantic sensor.",
 )
 parser.add_argument("--seed", type=int, default=1)
+parser.add_argument(
+    "--enable_physics",
+    action="store_true",
+    help="Whether to enable phyiscs (kinematic by default or dynamics if installed with bullet) during benchmark or not.",
+)
+parser.add_argument(
+    "--num_objects",
+    type=int,
+    default=10,
+    help="Number of objects to spawn if enable_physics is true.",
+)
+parser.add_argument(
+    "--test_object_index",
+    type=int,
+    default=0,
+    help="Index the objects to spawn if enable_physics is true. -1 indicates random.",
+)
 args = parser.parse_args()
 
 default_settings = dr.default_sim_settings.copy()
@@ -64,6 +81,14 @@
     benchmark_items["semantic_only"] = {"color_sensor": False, "semantic_sensor": True}
     benchmark_items["rgbd_semantic"] = {"depth_sensor": True, "semantic_sensor": True}
 
+if args.enable_physics:
+    # TODO: cannot benchmark physics with no sensors as this won't create a renderer or load the scene.
+    # benchmark_items["enable_physics_no_obs"] = {"color_sensor": False, "enable_physics": True}
+    benchmark_items["phys_rgb"] = {"enable_physics": True}
+    benchmark_items["phys_rgbd"] = {"depth_sensor": True, "enable_physics": True}
+    default_settings["num_objects"] = args.num_objects
+    default_settings["test_object_index"] = args.test_object_index
+
 resolutions = args.resolution
 nprocs_tests = args.num_procs
 
@@ -79,10 +104,10 @@
             print(" ---------------------- %s ------------------------ " % key)
             settings = default_settings.copy()
             settings.update(value)
-            perf[key] = demo_runner.benchmark(settings).get("fps")
+            perf[key] = demo_runner.benchmark(settings)
             print(
                 " ====== FPS (%d x %d, %s): %0.1f ======"
-                % (settings["width"], settings["height"], key, perf[key])
+                % (settings["width"], settings["height"], key, perf[key].get("fps"))
             )
         performance.append(perf)
 
@@ -101,8 +126,28 @@
     for idx in range(len(performance)):
         row = "%d x %d" % (resolutions[idx], resolutions[idx])
         for key, value in performance[idx].items():
-            row += "\t%-8.1f" % value
+            row += "\t%-8.1f" % value.get("fps")
         print(row)
     print(
         " =============================================================================="
     )
+
+    # also print the average time per simulation step (including object perturbations)
+    if args.enable_physics:
+        print(
+            " ================ Performance (step time: milliseconds) NPROC={} ===================================".format(
+                nproc
+            )
+        )
+        title = "Resolution "
+        for key, value in perf.items():
+            title += "\t%-10s" % key
+        print(title)
+        for idx in range(len(performance)):
+            row = "%d x %d" % (resolutions[idx], resolutions[idx])
+            for key, value in performance[idx].items():
+                row += "\t%-8.2f" % (value.get("avg_sim_step_time") * 1000)
+            print(row)
+        print(
+            " =============================================================================="
+        )

examples/demo_runner.py

@@ -17,6 +17,7 @@
 import habitat_sim
 import habitat_sim.agent
 import habitat_sim.bindings as hsim
+from habitat_sim.physics import MotionType
 from habitat_sim.utils.common import (
     d3_40_colors_rgb,
     download_and_unzip,
@@ -140,9 +141,17 @@ def init_physics_test_scene(self, num_objects):
         assert (
             object_lib_size > 0
         ), "!!!No objects loaded in library, aborting object instancing example!!!"
+
+        # clear the objects if we are re-running this initializer
+        for old_obj_id in self._sim.get_existing_object_ids():
+            self._sim.remove_object(old_obj_id)
+
         for obj_id in range(num_objects):
-            rand_obj_index = random.randint(0, object_lib_size - 1)
+            # rand_obj_index = random.randint(0, object_lib_size - 1)
             # rand_obj_index = 0  # overwrite for specific object only
+            rand_obj_index = self._sim_settings.get("test_object_index")
+            if rand_obj_index < 0:  # get random object on -1
+                rand_obj_index = random.randint(0, object_lib_size - 1)
             object_init_cell = (
                 random.randint(-object_init_grid_dim[0], object_init_grid_dim[0]),
                 random.randint(-object_init_grid_dim[1], object_init_grid_dim[1]),
@@ -177,6 +186,7 @@ def init_physics_test_scene(self, num_objects):
 
     def do_time_steps(self):
 
+        total_sim_step_time = 0.0
         total_frames = 0
         start_time = time.time()
         action_names = list(
@@ -185,7 +195,9 @@ def do_time_steps(self):
 
         # load an object and position the agent for physics testing
         if self._sim_settings["enable_physics"]:
-            self.init_physics_test_scene(num_objects=10)
+            self.init_physics_test_scene(
+                num_objects=self._sim_settings.get("num_objects")
+            )
             print("active object ids: " + str(self._sim.get_existing_object_ids()))
 
         time_per_step = []
@@ -198,16 +210,28 @@ def do_time_steps(self):
                 print("action", action)
 
             start_step_time = time.time()
-            # NOTE: uncomment this for random kinematic transform setting of all objects
-            # if self._sim_settings["enable_physics"]:
-            #    obj_ids = self._sim.get_existing_object_ids()
-            #    for obj_id in obj_ids:
-            #        rand_nudge = np.random.uniform(-0.05,0.05,3)
-            #        cur_pos = self._sim.get_translation(obj_id)
-            #        self._sim.set_translation(cur_pos + rand_nudge, obj_id)
+
+            # apply kinematic or dynamic control to all objects based on their MotionType
+            if self._sim_settings["enable_physics"]:
+                obj_ids = self._sim.get_existing_object_ids()
+                for obj_id in obj_ids:
+                    rand_nudge = np.random.uniform(-0.05, 0.05, 3)
+                    if self._sim.get_object_motion_type(obj_id) == MotionType.KINEMATIC:
+                        # TODO: just bind the trnslate function instead of emulating it here.
+                        cur_pos = self._sim.get_translation(obj_id)
+                        self._sim.set_translation(cur_pos + rand_nudge, obj_id)
+                    elif self._sim.get_object_motion_type(obj_id) == MotionType.DYNAMIC:
+                        self._sim.apply_force(rand_nudge, np.zeros(3), obj_id)
+
+            # get "interaction" time
+            total_sim_step_time += time.time() - start_step_time
+
             observations = self._sim.step(action)
             time_per_step.append(time.time() - start_step_time)
 
+            # get simulation step time without sensor observations
+            total_sim_step_time += self._sim._previous_step_time
+
             if self._sim_settings["save_png"]:
                 if self._sim_settings["color_sensor"]:
                     self.save_color_observation(observations, total_frames)
@@ -249,6 +273,7 @@ def do_time_steps(self):
         perf["frame_time"] = perf["total_time"] / total_frames
         perf["fps"] = 1.0 / perf["frame_time"]
         perf["time_per_step"] = time_per_step
+        perf["avg_sim_step_time"] = total_sim_step_time / total_frames
 
         return perf
 
@@ -348,6 +373,7 @@ def benchmark(self, settings):
             frame_time=sum(res["frame_time"]),
             fps=sum(res["fps"]),
             total_time=sum(res["total_time"]) / nprocs,
+            avg_sim_step_time=sum(res["avg_sim_step_time"]) / nprocs,
         )
 
     def example(self):

examples/settings.py

@@ -30,6 +30,8 @@
     "goal_headings": [[0, -0.980_785, 0, 0.195_090], [0.0, 1.0, 0.0, 0.0]],
     "enable_physics": False,
     "physics_config_file": "./data/default.phys_scene_config.json",
+    "num_objects": 10,
+    "test_object_index": 0,
 }
 
 # build SimulatorConfiguration

habitat_sim/physics.py

@@ -0,0 +1,7 @@
+# Copyright (c) Facebook, Inc. and its affiliates.
+# This source code is licensed under the MIT license found in the
+# LICENSE file in the root directory of this source tree.
+
+from habitat_sim._ext.habitat_sim_bindings import MotionType
+
+__all__ = ["MotionType"]

habitat_sim/simulator.py

@@ -5,6 +5,7 @@
 # LICENSE file in the root directory of this source tree.
 
 import os.path as osp
+import time
 from typing import Dict, List, Optional
 
 import attr
@@ -16,6 +17,7 @@
 from habitat_sim.agent import Agent, AgentConfiguration, AgentState
 from habitat_sim.logging import logger
 from habitat_sim.nav import GreedyGeodesicFollower
+from habitat_sim.physics import MotionType
 from habitat_sim.utils.common import quat_from_angle_axis
 
 torch = None
@@ -53,6 +55,7 @@ class Simulator:
     _num_total_frames: int = attr.ib(default=0, init=False)
     _default_agent: Agent = attr.ib(init=False, default=None)
     _sensors: Dict = attr.ib(factory=dict, init=False)
+    _previous_step_time = 0.0  # track the compute time of each step
 
     def __attrs_post_init__(self):
         config = self.config
@@ -212,8 +215,9 @@ def step(self, action, dt=1.0 / 60.0):
         self._last_state = self._default_agent.get_state()
 
         # step physics by dt
+        step_start_Time = time.time()
         self._sim.step_world(dt)
-        # print("World time is now: " + str(self._sim.get_world_time()))
+        _previous_step_time = time.time() - step_start_Time
 
         observations = self.get_sensor_observations()
         # Whether or not the action taken resulted in a collision
@@ -248,6 +252,9 @@ def remove_object(self, object_id):
     def get_existing_object_ids(self, scene_id=0):
         return self._sim.get_existing_object_ids(scene_id)
 
+    def get_object_motion_type(self, object_id, scene_id=0):
+        return self._sim.get_object_motion_type(object_id, scene_id)
+
     def set_transformation(self, transform, object_id, scene_id=0):
         self._sim.set_transformation(transform, object_id, scene_id)
 

src/esp/bindings/bindings.cpp

@@ -509,6 +509,13 @@ PYBIND11_MODULE(habitat_sim_bindings, m) {
 
   initShortestPathBindings(m);
 
+  // ==== enum object MotionType ====
+  py::enum_<MotionType>(m, "MotionType")
+      .value("ERROR_MOTIONTYPE", MotionType::ERROR_MOTIONTYPE)
+      .value("STATIC", MotionType::STATIC)
+      .value("KINEMATIC", MotionType::KINEMATIC)
+      .value("DYNAMIC", MotionType::DYNAMIC);
+
   // ==== Simulator ====
   simulator.def(py::init(&Simulator::create<const SimulatorConfiguration&>))
       .def("get_active_scene_graph", &Simulator::getActiveSceneGraph,
@@ -531,6 +538,8 @@ PYBIND11_MODULE(habitat_sim_bindings, m) {
            &Simulator::getPhysicsObjectLibrarySize)
       .def("remove_object", &Simulator::removeObject, "object_id"_a,
            "sceneID"_a = 0)
+      .def("get_object_motion_type", &Simulator::getObjectMotionType,
+           "object_id"_a, "sceneID"_a = 0)
       .def("get_existing_object_ids", &Simulator::getExistingObjectIDs,
            "sceneID"_a = 0)
       .def("step_world", &Simulator::stepWorld, "dt"_a = 1.0 / 60.0)

src/esp/gfx/Simulator.cpp

@@ -241,6 +241,14 @@ int Simulator::removeObject(const int objectID, const int sceneID) {
   return ID_UNDEFINED;
 }
 
+esp::physics::MotionType Simulator::getObjectMotionType(const int objectID,
+                                                        const int sceneID) {
+  if (physicsManager_ != nullptr && sceneID >= 0 && sceneID < sceneID_.size()) {
+    return physicsManager_->getObjectMotionType(objectID);
+  }
+  return esp::physics::MotionType::ERROR_MOTIONTYPE;
+}
+
 // apply forces and torques to objects
 void Simulator::applyTorque(const Magnum::Vector3& tau,
                             const int objectID,

src/esp/gfx/Simulator.h

@@ -123,6 +123,19 @@ class Simulator {
    */
   std::vector<int> getExistingObjectIDs(const int sceneID = 0);
 
+  /**
+   * @brief Get the @ref esp::physics::MotionType of an object.
+   * See @ref esp::physics::PhysicsManager::getExistingObjectIDs.
+   * @param objectID The ID of the object identifying it in @ref
+   * esp::physics::PhysicsManager::existingObjects_.
+   * @param sceneID !! Not used currently !! Specifies which physical scene to
+   * query.
+   * @return The @ref esp::physics::MotionType of the object or @ref
+   * esp::physics::MotionType::ERROR_MOTIONTYPE if query failed.
+   */
+  esp::physics::MotionType getObjectMotionType(const int objectID,
+                                               const int sceneID = 0);
+
   /**
    * @brief Apply torque to an object. See @ref
    * esp::physics::PhysicsManager::applyTorque.

src/esp/physics/PhysicsManager.cpp

@@ -9,9 +9,14 @@
 namespace esp {
 namespace physics {
 
-bool PhysicsManager::initPhysics(scene::SceneNode* node,
-                                 const assets::PhysicsManagerAttributes&) {
+bool PhysicsManager::initPhysics(
+    scene::SceneNode* node,
+    const assets::PhysicsManagerAttributes& physicsManagerAttributes) {
   physicsNode_ = node;
+
+  // Copy over relevant configuration
+  fixedTimeStep_ = physicsManagerAttributes.getDouble("timestep");
+
   //! Create new scene node
   sceneNode_ = new physics::RigidObject(physicsNode_);
   initialized_ = true;

src/esp/physics/bullet/BulletPhysicsManager.cpp

@@ -22,6 +22,9 @@ bool BulletPhysicsManager::initPhysics(
   // btGImpactCollisionAlgorithm::registerAlgorithm(&bDispatcher_);
   bWorld_ = std::make_shared<btDiscreteDynamicsWorld>(
       &bDispatcher_, &bBroadphase_, &bSolver_, &bCollisionConfig_);
+
+  // Copy over relevant configuration
+  fixedTimeStep_ = physicsManagerAttributes.getDouble("timestep");
   // currently GLB meshes are y-up
   bWorld_->setGravity(
       btVector3(physicsManagerAttributes.getMagnumVec3("gravity")));
@@ -141,7 +144,6 @@ void BulletPhysicsManager::stepPhysics(double dt) {
   }
 
   // ==== Physics stepforward ======
-
   // NOTE: worldTime_ will always be a multiple of sceneMetaData_.timestep
   int numSubStepsTaken =
       bWorld_->stepSimulation(dt, maxSubSteps_, fixedTimeStep_);