diff --git a/data/default.phys_scene_config.json b/data/default.phys_scene_config.json index b42bcc3462..53b6a5d5bd 100644 --- a/data/default.phys_scene_config.json +++ b/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, diff --git a/examples/benchmark.py b/examples/benchmark.py index 06a7ac914e..a2670276dc 100755 --- a/examples/benchmark.py +++ b/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( + " ==============================================================================" + ) diff --git a/examples/demo_runner.py b/examples/demo_runner.py index aaf1d729eb..6b1ed1845a 100644 --- a/examples/demo_runner.py +++ b/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): diff --git a/examples/settings.py b/examples/settings.py index a20e554366..5dd172a9f1 100644 --- a/examples/settings.py +++ b/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 diff --git a/habitat_sim/physics.py b/habitat_sim/physics.py new file mode 100644 index 0000000000..407a83dff7 --- /dev/null +++ b/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"] diff --git a/habitat_sim/simulator.py b/habitat_sim/simulator.py index bc7d714cb3..39c929f5fb 100644 --- a/habitat_sim/simulator.py +++ b/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) diff --git a/src/esp/bindings/bindings.cpp b/src/esp/bindings/bindings.cpp index 9daf9df053..54cfe1860c 100644 --- a/src/esp/bindings/bindings.cpp +++ b/src/esp/bindings/bindings.cpp @@ -509,6 +509,13 @@ PYBIND11_MODULE(habitat_sim_bindings, m) { initShortestPathBindings(m); + // ==== enum object MotionType ==== + py::enum_(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)) .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) diff --git a/src/esp/gfx/Simulator.cpp b/src/esp/gfx/Simulator.cpp index dd65150e76..33bba6ea82 100644 --- a/src/esp/gfx/Simulator.cpp +++ b/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, diff --git a/src/esp/gfx/Simulator.h b/src/esp/gfx/Simulator.h index 8d3f7113a7..da270ec4e4 100644 --- a/src/esp/gfx/Simulator.h +++ b/src/esp/gfx/Simulator.h @@ -123,6 +123,19 @@ class Simulator { */ std::vector 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. diff --git a/src/esp/physics/PhysicsManager.cpp b/src/esp/physics/PhysicsManager.cpp index 459d146943..6057a03b86 100644 --- a/src/esp/physics/PhysicsManager.cpp +++ b/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; diff --git a/src/esp/physics/bullet/BulletPhysicsManager.cpp b/src/esp/physics/bullet/BulletPhysicsManager.cpp index fd99467efb..f69fd6fbcd 100644 --- a/src/esp/physics/bullet/BulletPhysicsManager.cpp +++ b/src/esp/physics/bullet/BulletPhysicsManager.cpp @@ -22,6 +22,9 @@ bool BulletPhysicsManager::initPhysics( // btGImpactCollisionAlgorithm::registerAlgorithm(&bDispatcher_); bWorld_ = std::make_shared( &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_);