From 491422eb72def35f50d15bf2bca4c710518cc552 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 16 Aug 2024 11:17:40 -0700 Subject: [PATCH 1/5] refactor to add expanded reset logic for Simulator including: reset object states, set light setup default key to scene's light key --- src/esp/physics/PhysicsManager.h | 10 ++++++++-- src/esp/physics/RigidObject.h | 3 +-- src/esp/physics/bullet/BulletArticulatedObject.cpp | 2 ++ src/esp/sim/Simulator.cpp | 12 ++++++++++-- src/esp/sim/Simulator.h | 7 +++++++ src_python/habitat_sim/simulator.py | 10 ++++++++++ 6 files changed, 38 insertions(+), 6 deletions(-) diff --git a/src/esp/physics/PhysicsManager.h b/src/esp/physics/PhysicsManager.h index 115108e12c..2c7d452b59 100644 --- a/src/esp/physics/PhysicsManager.h +++ b/src/esp/physics/PhysicsManager.h @@ -264,11 +264,17 @@ class PhysicsManager : public std::enable_shared_from_this { /** * @brief Reset the simulation and physical world. - * Sets the @ref worldTime_ to 0.0, does not change physical state. + * Sets the @ref worldTime_ to 0.0, changes the physical state of all objects back to their initial states. */ virtual void reset() { - /* TODO: reset object states or clear them? Other? */ + // reset object states from initial values (e.g. from scene instance) worldTime_ = 0.0; + for (const auto& bro : existingObjects_) { + bro.second->resetStateFromSceneInstanceAttr(); + } + for (const auto& bao : existingArticulatedObjects_) { + bao.second->resetStateFromSceneInstanceAttr(); + } } /** @brief Stores references to a set of drawable elements. */ diff --git a/src/esp/physics/RigidObject.h b/src/esp/physics/RigidObject.h index 8899d0c567..8a82df3171 100644 --- a/src/esp/physics/RigidObject.h +++ b/src/esp/physics/RigidObject.h @@ -207,8 +207,7 @@ class RigidObject : public RigidBase { VelocityControl::ptr getVelocityControl() { return velControl_; }; /** - * @brief Set the object's state from a @ref - * esp::metadata::attributes::SceneObjectInstanceAttributes + * @brief Set the object's state from a @ref esp::metadata::attributes::SceneObjectInstanceAttributes */ void resetStateFromSceneInstanceAttr() override; diff --git a/src/esp/physics/bullet/BulletArticulatedObject.cpp b/src/esp/physics/bullet/BulletArticulatedObject.cpp index c40c818088..532f9691c3 100644 --- a/src/esp/physics/bullet/BulletArticulatedObject.cpp +++ b/src/esp/physics/bullet/BulletArticulatedObject.cpp @@ -311,6 +311,8 @@ void BulletArticulatedObject::resetStateFromSceneInstanceAttr() { } aoJointVels[i] = initJointVels[i]; } + // clear any forces + setJointForces(std::vector(size_t(btMultiBody_->getNumDofs()))); } // BulletArticulatedObject::resetStateFromSceneInstanceAttr diff --git a/src/esp/sim/Simulator.cpp b/src/esp/sim/Simulator.cpp index e7e94f9b8b..5afbae9f19 100644 --- a/src/esp/sim/Simulator.cpp +++ b/src/esp/sim/Simulator.cpp @@ -672,7 +672,9 @@ bool Simulator::instanceArticulatedObjectsForSceneAttributes( void Simulator::reset() { if (physicsManager_ != nullptr) { - // Note: only resets time to 0 by default. + // Note: resets time to 0 and all existing objects set back to initial + // states. Does not add back deleted objects or delete added objects. Does + // not break ManagedObject pointers. physicsManager_->reset(); } @@ -680,7 +682,13 @@ void Simulator::reset() { agent->reset(); } getActiveSceneGraph().getRootNode().computeCumulativeBB(); - resourceManager_->setLightSetup(gfx::getDefaultLights()); + // set the default light key to reference the scene's light setup + auto initSceneInstanceAttr = + metadataMediator_->getSceneInstanceAttributesManager() + ->getObjectCopyByHandle(curSceneInstanceAttributes_->getHandle()); + auto sceneLightSetup = resourceManager_->getLightSetup( + initSceneInstanceAttr->getLightingHandle()); + resourceManager_->setLightSetup(*sceneLightSetup); } // Simulator::reset() metadata::attributes::SceneInstanceAttributes::ptr diff --git a/src/esp/sim/Simulator.h b/src/esp/sim/Simulator.h index a1873f8b7d..9d1218abc4 100644 --- a/src/esp/sim/Simulator.h +++ b/src/esp/sim/Simulator.h @@ -68,6 +68,13 @@ class Simulator { void reconfigure(const SimulatorConfiguration& cfg); + /** + * @brief Reset the simulation state including the state of all physics + * objects, agents, and the default light setup. + * Sets the @ref worldTime_ to 0.0, changes the physical state of all objects back to their initial states. + * Does not invalidate existing ManagedObject wrappers. + * Does not add or remove object instances. + */ void reset(); void seed(uint32_t newSeed); diff --git a/src_python/habitat_sim/simulator.py b/src_python/habitat_sim/simulator.py index 3db49904f1..05dc24a1e1 100755 --- a/src_python/habitat_sim/simulator.py +++ b/src_python/habitat_sim/simulator.py @@ -172,6 +172,16 @@ def reset(self, agent_ids: Optional[int] = None) -> ObservationDict: def reset( self, agent_ids: Union[Optional[int], List[int]] = None ) -> Union[ObservationDict, Dict[int, ObservationDict],]: + """ + Reset the simulation state including the state of all physics objects, agents, and the default light setup. + Sets the world time to 0.0, changes the physical state of all objects back to their initial states. + Does not invalidate existing ManagedObject wrappers. + Does not add or remove object instances. + + :param agent_ids: An optional list of agent ids for which to return the sensor observations. If none is provide, default agent is used. + + :return: Sensor observations in the reset state. + """ super().reset() for i in range(len(self.agents)): self.reset_agent(i) From 39f715297fea8e5032856c18711155d71a5399fe Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Fri, 16 Aug 2024 15:07:46 -0700 Subject: [PATCH 2/5] update the internal reset logic to add zero init for positions and velocities when no scene_instance init was provided. Also update unit test. --- src/esp/physics/PhysicsManager.h | 2 +- .../bullet/BulletArticulatedObject.cpp | 25 +++++- src/esp/sim/Simulator.h | 1 + src_python/habitat_sim/simulator.py | 1 + tests/test_simulator.py | 78 +++++++++++++++++-- 5 files changed, 97 insertions(+), 10 deletions(-) diff --git a/src/esp/physics/PhysicsManager.h b/src/esp/physics/PhysicsManager.h index 2c7d452b59..e1250c053b 100644 --- a/src/esp/physics/PhysicsManager.h +++ b/src/esp/physics/PhysicsManager.h @@ -264,7 +264,7 @@ class PhysicsManager : public std::enable_shared_from_this { /** * @brief Reset the simulation and physical world. - * Sets the @ref worldTime_ to 0.0, changes the physical state of all objects back to their initial states. + * Sets the @ref worldTime_ to 0.0, changes the physical state of all objects back to their initial states. Only changes motion_type when scene_instance specified a motion type. */ virtual void reset() { // reset object states from initial values (e.g. from scene instance) diff --git a/src/esp/physics/bullet/BulletArticulatedObject.cpp b/src/esp/physics/bullet/BulletArticulatedObject.cpp index 532f9691c3..07cd21d522 100644 --- a/src/esp/physics/bullet/BulletArticulatedObject.cpp +++ b/src/esp/physics/bullet/BulletArticulatedObject.cpp @@ -279,10 +279,24 @@ void BulletArticulatedObject::resetStateFromSceneInstanceAttr() { if (attrObjMotionType != physics::MotionType::UNDEFINED) { setMotionType(attrObjMotionType); } - // set initial joint positions - // get array of existing joint dofs + + // first clear all joint positions + setJointPositions(std::vector(size_t(btMultiBody_->getNumPosVars()))); + // then add back the ones for unit quaternions + int posCount = 0; + float quat_init[] = {0, 0, 0, 1}; + for (int i = 0; i < btMultiBody_->getNumLinks(); ++i) { + auto& link = btMultiBody_->getLink(i); + if (link.m_posVarCount == 4) { + // special handling for quaternions in spherical joints + btMultiBody_->setJointPosMultiDof(i, const_cast(quat_init)); + posCount += link.m_posVarCount; + } + } + + // set initial joint positions from instance config if applicable std::vector aoJointPose = getJointPositions(); - // get instance-specified initial joint positions + // get instance-specified initial joint velocities const auto& initJointPos = sceneObjInstanceAttr->getInitJointPose(); for (size_t i = 0; i < initJointPos.size(); ++i) { if (i >= aoJointPose.size()) { @@ -295,8 +309,9 @@ void BulletArticulatedObject::resetStateFromSceneInstanceAttr() { } setJointPositions(aoJointPose); + // first clear all joint vels + setJointVelocities(std::vector(size_t(btMultiBody_->getNumDofs()))); // set initial joint velocities - // get array of existing joint vel dofs std::vector aoJointVels = getJointVelocities(); // get instance-specified initial joint velocities std::vector initJointVels = @@ -311,6 +326,8 @@ void BulletArticulatedObject::resetStateFromSceneInstanceAttr() { } aoJointVels[i] = initJointVels[i]; } + setJointVelocities(aoJointVels); + // clear any forces setJointForces(std::vector(size_t(btMultiBody_->getNumDofs()))); diff --git a/src/esp/sim/Simulator.h b/src/esp/sim/Simulator.h index 9d1218abc4..3759500c56 100644 --- a/src/esp/sim/Simulator.h +++ b/src/esp/sim/Simulator.h @@ -74,6 +74,7 @@ class Simulator { * Sets the @ref worldTime_ to 0.0, changes the physical state of all objects back to their initial states. * Does not invalidate existing ManagedObject wrappers. * Does not add or remove object instances. + * Only changes motion_type when scene_instance specified a motion type. */ void reset(); diff --git a/src_python/habitat_sim/simulator.py b/src_python/habitat_sim/simulator.py index 05dc24a1e1..1f42253cf3 100755 --- a/src_python/habitat_sim/simulator.py +++ b/src_python/habitat_sim/simulator.py @@ -177,6 +177,7 @@ def reset( Sets the world time to 0.0, changes the physical state of all objects back to their initial states. Does not invalidate existing ManagedObject wrappers. Does not add or remove object instances. + Only changes motion_type when scene_instance specified a motion type. :param agent_ids: An optional list of agent ids for which to return the sensor observations. If none is provide, default agent is used. diff --git a/tests/test_simulator.py b/tests/test_simulator.py index e3d9937376..4777b27c12 100644 --- a/tests/test_simulator.py +++ b/tests/test_simulator.py @@ -74,24 +74,92 @@ def test_sim_reset(make_cfg_settings): mm = habitat_sim.metadata.MetadataMediator(hab_cfg.sim_cfg) hab_cfg_mm.metadata_mediator = mm + def check_isclose(val1, val2): + return np.isclose(val1, val2, rtol=1e-4).all() + test_list = [hab_cfg, hab_cfg_mm] for ctor_arg in test_list: with habitat_sim.Simulator(ctor_arg) as sim: agent_config = sim.config.agents[0] sim.initialize_agent(0) + # cache agent initial state initial_state = sim.agents[0].initial_state # Take random steps in the environment for _ in range(10): action = random.choice(list(agent_config.action_space.keys())) sim.step(action) + # add rigid and articulated objects + sim.metadata_mediator.ao_template_manager.load_configs( + "data/test_assets/urdf/" + ) + sim.metadata_mediator.object_template_manager.load_configs( + "data/test_assets/objects/" + ) + + chair_handle = ( + sim.metadata_mediator.object_template_manager.get_template_handles( + "chair" + )[0] + ) + ao_handle = sim.metadata_mediator.ao_template_manager.get_template_handles( + "prism" + )[0] + + ro = sim.get_rigid_object_manager().add_object_by_template_handle( + chair_handle + ) + ao = sim.get_articulated_object_manager().add_articulated_object_by_template_handle( + ao_handle + ) + + assert ro is not None + assert ao is not None + + # cache the initial state for verification + ao_initial_state = ( + ao.transformation, + ao.joint_positions, + ao.joint_velocities, + ) + ro_initial_state = ro.transformation + + assert check_isclose(ro.transformation, ro_initial_state) + assert check_isclose(ao.transformation, ao_initial_state[0]) + assert check_isclose(ao.joint_positions, ao_initial_state[1]) + assert check_isclose(ao.joint_velocities, ao_initial_state[2]) + + ro.translation = mn.Vector3(1, 2, 3) + ro.rotation = mn.Quaternion.rotation( + mn.Rad(0.123), mn.Vector3(0.1, 0.2, 0.3).normalized() + ) + ao.translation = mn.Vector3(3, 2, 1) + ao.rotation = mn.Quaternion.rotation( + mn.Rad(0.321), mn.Vector3(0.3, 0.2, 0.1).normalized() + ) + ao.joint_positions = np.array(ao_initial_state[1]) * 0.2 + ao.joint_velocities = np.ones(len(ao.joint_velocities)) + + assert not check_isclose(ro.transformation, ro_initial_state) + assert not check_isclose(ao.transformation, ao_initial_state[0]) + assert not check_isclose(ao.joint_positions, ao_initial_state[1]) + assert not check_isclose(ao.joint_velocities, ao_initial_state[2]) + + # do the reset sim.reset() + + # validate agent state reset new_state = sim.agents[0].get_state() - same_position = all(initial_state.position == new_state.position) - same_rotation = np.isclose( - initial_state.rotation, new_state.rotation, rtol=1e-4 - ) # Numerical error can cause slight deviations - assert same_position and same_rotation + # NOTE: Numerical error can cause slight deviations, use isclose + # assert same agent position and rotation + assert check_isclose(initial_state.position, new_state.position) + assert check_isclose(initial_state.rotation, new_state.rotation) + + # validate object state resets + assert check_isclose(ro.transformation, ro_initial_state) + assert check_isclose(ao.transformation, ao_initial_state[0]) + assert check_isclose(ao.joint_positions, ao_initial_state[1]) + assert check_isclose(ao.joint_velocities, ao_initial_state[2]) def test_sim_multiagent_move_and_reset(make_cfg_settings, num_agents=10): From 6d2f92573fb60200fd49ca561f19cd50672a07c6 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 19 Aug 2024 11:41:35 -0700 Subject: [PATCH 3/5] revert lighting reset change (moved to separate PR) --- src/esp/sim/Simulator.cpp | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/src/esp/sim/Simulator.cpp b/src/esp/sim/Simulator.cpp index 5afbae9f19..d1755998b3 100644 --- a/src/esp/sim/Simulator.cpp +++ b/src/esp/sim/Simulator.cpp @@ -682,13 +682,7 @@ void Simulator::reset() { agent->reset(); } getActiveSceneGraph().getRootNode().computeCumulativeBB(); - // set the default light key to reference the scene's light setup - auto initSceneInstanceAttr = - metadataMediator_->getSceneInstanceAttributesManager() - ->getObjectCopyByHandle(curSceneInstanceAttributes_->getHandle()); - auto sceneLightSetup = resourceManager_->getLightSetup( - initSceneInstanceAttr->getLightingHandle()); - resourceManager_->setLightSetup(*sceneLightSetup); + resourceManager_->setLightSetup(gfx::getDefaultLights()); } // Simulator::reset() metadata::attributes::SceneInstanceAttributes::ptr From 7525495cde169bdf579640528f30ce2da22df4c5 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 19 Aug 2024 15:02:35 -0700 Subject: [PATCH 4/5] refactor C++ tests leveraging reset for time resets only. --- src/tests/PhysicsTest.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/tests/PhysicsTest.cpp b/src/tests/PhysicsTest.cpp index ddc32d3d28..81af04e28b 100644 --- a/src/tests/PhysicsTest.cpp +++ b/src/tests/PhysicsTest.cpp @@ -648,13 +648,12 @@ void PhysicsTest::testRemoveSleepingSupport() { cubes[0]->setMotionType(esp::physics::MotionType::STATIC); for (int testCase = 0; testCase < 2; ++testCase) { - // reset time to 0, should not otherwise modify state - physicsManager_->reset(); + float currentTime = physicsManager_->getWorldTime(); CORRADE_COMPARE_AS(physicsManager_->getNumRigidObjects(), 0, Cr::TestSuite::Compare::Greater); // simulate to stabilize the stack and populate collision islands - while (physicsManager_->getWorldTime() < 4.0) { + while (physicsManager_->getWorldTime() < currentTime + 4.0) { physicsManager_->stepPhysics(0.1); } @@ -924,15 +923,15 @@ void PhysicsTest::testVelocityControl() { objectWrapper->resetTransformation(); objectWrapper->setTranslation(Magnum::Vector3{0, 2.0, 0}); physicsManager_->setGravity({}); // 0 gravity interference - physicsManager_->reset(); // reset time to 0 // should closely follow kinematic result while uninhibited in 0 gravity float targetTime = 0.5; + float currentTime = physicsManager_->getWorldTime(); esp::core::RigidState initialObjectState(objectWrapper->getRotation(), objectWrapper->getTranslation()); esp::core::RigidState kinematicResult = velControl->integrateTransform(targetTime, initialObjectState); - while (physicsManager_->getWorldTime() < targetTime) { + while (physicsManager_->getWorldTime() < currentTime + targetTime) { physicsManager_->stepPhysics(physicsManager_->getTimestep()); } CORRADE_COMPARE_AS( @@ -946,7 +945,7 @@ void PhysicsTest::testVelocityControl() { // should then get blocked by ground plane collision targetTime = 2.0; - while (physicsManager_->getWorldTime() < targetTime) { + while (physicsManager_->getWorldTime() < currentTime + targetTime) { physicsManager_->stepPhysics(physicsManager_->getTimestep()); } CORRADE_COMPARE_AS(objectWrapper->getTranslation()[1], 1.0 - errorEps, @@ -964,8 +963,8 @@ void PhysicsTest::testVelocityControl() { velControl->linVelIsLocal = true; targetTime = 10.0; - physicsManager_->reset(); // reset time to 0 - while (physicsManager_->getWorldTime() < targetTime) { + float currentTime = physicsManager_->getWorldTime(); + while (physicsManager_->getWorldTime() < currentTime + targetTime) { physicsManager_->stepPhysics(physicsManager_->getTimestep()); } From 58bacb0a991d3cc1c3eac66c5ae3a59fb15cf528 Mon Sep 17 00:00:00 2001 From: aclegg3 Date: Mon, 19 Aug 2024 16:56:18 -0700 Subject: [PATCH 5/5] fix spherical joint bug --- .../physics/bullet/BulletArticulatedObject.cpp | 13 +++++++------ tests/test_physics.py | 15 +++++++-------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/esp/physics/bullet/BulletArticulatedObject.cpp b/src/esp/physics/bullet/BulletArticulatedObject.cpp index 07cd21d522..9a6e15d0b2 100644 --- a/src/esp/physics/bullet/BulletArticulatedObject.cpp +++ b/src/esp/physics/bullet/BulletArticulatedObject.cpp @@ -280,15 +280,16 @@ void BulletArticulatedObject::resetStateFromSceneInstanceAttr() { setMotionType(attrObjMotionType); } - // first clear all joint positions - setJointPositions(std::vector(size_t(btMultiBody_->getNumPosVars()))); - // then add back the ones for unit quaternions + // initially set positions to zero (or identity quaternion for spherical + // joints) int posCount = 0; + // init unit quat float quat_init[] = {0, 0, 0, 1}; for (int i = 0; i < btMultiBody_->getNumLinks(); ++i) { auto& link = btMultiBody_->getLink(i); - if (link.m_posVarCount == 4) { - // special handling for quaternions in spherical joints + if (link.m_posVarCount > 0) { + // feed the unit quat to all setters, only spherical joints will hit the + // one at the end btMultiBody_->setJointPosMultiDof(i, const_cast(quat_init)); posCount += link.m_posVarCount; } @@ -296,7 +297,7 @@ void BulletArticulatedObject::resetStateFromSceneInstanceAttr() { // set initial joint positions from instance config if applicable std::vector aoJointPose = getJointPositions(); - // get instance-specified initial joint velocities + // get instance-specified initial joint positions const auto& initJointPos = sceneObjInstanceAttr->getInitJointPose(); for (size_t i = 0; i < initJointPos.size(); ++i) { if (i >= aoJointPose.size()) { diff --git a/tests/test_physics.py b/tests/test_physics.py index f4cfcb749b..4ccdac8ea8 100644 --- a/tests/test_physics.py +++ b/tests/test_physics.py @@ -447,7 +447,7 @@ def test_velocity_control(): obj_handle = obj_template_mgr.get_template_handle_by_id(template_ids[0]) for iteration in range(2): - sim.reset() + cur_time = sim.get_world_time() box_object = rigid_obj_mgr.add_object_by_template_handle(obj_handle) # Verify is not articulated @@ -471,17 +471,19 @@ def test_velocity_control(): vel_control.controlling_lin_vel = True vel_control.controlling_ang_vel = True - while sim.get_world_time() < 1.0: + while sim.get_world_time() < 1.0 + cur_time: # NOTE: stepping close to default timestep to get near-constant velocity control of DYNAMIC bodies. sim.step_physics(0.00416) - ground_truth_pos = sim.get_world_time() * vel_control.linear_velocity + ground_truth_pos = ( + sim.get_world_time() - cur_time + ) * vel_control.linear_velocity assert np.allclose(box_object.translation, ground_truth_pos, atol=0.01) ground_truth_q = mn.Quaternion([[0, 0.480551, 0], 0.876967]) angle_error = mn.math.half_angle(ground_truth_q, box_object.rotation) assert angle_error < mn.Rad(0.005) - sim.reset() + cur_time = sim.get_world_time() # test local velocities (turn in a half circle) vel_control.lin_vel_is_local = True @@ -492,15 +494,12 @@ def test_velocity_control(): box_object.translation = [0.0, 0.0, 0.0] box_object.rotation = mn.Quaternion() - while sim.get_world_time() < 0.5: + while sim.get_world_time() < 0.5 + cur_time: # NOTE: stepping close to default timestep to get near-constant velocity control of DYNAMIC bodies. sim.step_physics(0.008) - print(sim.get_world_time()) - # NOTE: explicit integration, so expect some error ground_truth_q = mn.Quaternion([[1.0, 0.0, 0.0], 0.0]) - print(box_object.translation) assert np.allclose( box_object.translation, np.array([0, 1.0, 0.0]), atol=0.07 )