def get_state(self) -> AgentState: habitat_sim.errors.assert_obj_valid(self.body) state = AgentState(np.array(self.body.object.absolute_translation), self.body.object.rotation) for k, v in self._sensors.items(): habitat_sim.errors.assert_obj_valid(v) state.sensor_states[k] = SixDOFPose( np.array(v.node.absolute_translation), quat_from_magnum(state.rotation * v.node.rotation), ) state.rotation = quat_from_magnum(state.rotation) return state
def test_kinematics(sim): cfg_settings = examples.settings.default_sim_settings.copy() cfg_settings[ "scene"] = "data/scene_datasets/habitat-test-scenes/skokloster-castle.glb" # enable the physics simulator: also clears available actions to no-op cfg_settings["enable_physics"] = True cfg_settings["depth_sensor"] = True # test loading the physical scene hab_cfg = examples.settings.make_cfg(cfg_settings) sim.reconfigure(hab_cfg) assert sim.get_physics_object_library_size() > 0 # test adding an object to the world object_id = sim.add_object(0) assert len(sim.get_existing_object_ids()) > 0 # test kinematics I = np.identity(4) # test get and set translation sim.set_translation(np.array([0, 1.0, 0]), object_id) assert np.allclose(sim.get_translation(object_id), np.array([0, 1.0, 0])) # test get and set transform sim.set_transformation(I, object_id) assert np.allclose(sim.get_transformation(object_id), I) # test get and set rotation Q = quat_from_angle_axis(np.pi, np.array([0, 1.0, 0])) expected = np.eye(4) expected[0:3, 0:3] = quaternion.as_rotation_matrix(Q) sim.set_rotation(quat_to_magnum(Q), object_id) assert np.allclose(sim.get_transformation(object_id), expected) assert np.allclose(quat_from_magnum(sim.get_rotation(object_id)), Q) # test object removal old_object_id = sim.remove_object(object_id) assert len(sim.get_existing_object_ids()) == 0 assert old_object_id == object_id object_id = sim.add_object(0) prev_time = 0.0 for _ in range(2): # do some kinematics here (todo: translating or rotating instead of absolute) sim.set_translation(np.random.rand(3), object_id) T = sim.get_transformation(object_id) # test getting observation obs = sim.step( random.choice(list(hab_cfg.agents[0].action_space.keys()))) # check that time is increasing in the world assert sim.get_world_time() > prev_time prev_time = sim.get_world_time()
def step_cas(self, vel_control, dt=1.0 / 30.0): agent_state = self._default_agent.state self._num_total_frames += 1 self._last_state = self._default_agent.get_state() previous_rigid_state = habitat_sim.RigidState( quat_to_magnum(agent_state.rotation), agent_state.position) # manually integrate the rigid state target_rigid_state = vel_control.integrate_transform( dt, previous_rigid_state) # snap rigid state to navmesh and set state to object/agent # calls pathfinder.try_step or self.pathfinder.try_step_no_sliding end_pos = self.step_filter(previous_rigid_state.translation, target_rigid_state.translation) # set the computed state agent_state.position = end_pos agent_state.rotation = quat_from_magnum(target_rigid_state.rotation) self._default_agent.set_state(agent_state) # Check if a collision occured dist_moved_before_filter = (target_rigid_state.translation - previous_rigid_state.translation).dot() dist_moved_after_filter = (end_pos - previous_rigid_state.translation).dot() # NB: There are some cases where ||filter_end - end_pos|| > 0 when a # collision _didn't_ happen. One such case is going up stairs. Instead, # we check to see if the the amount moved after the application of the filter # is _less_ than the amount moved before the application of the filter EPS = 0.2 collided = (dist_moved_after_filter + EPS) < dist_moved_before_filter # print("collidded", collided) # step physics by dt step_start_Time = time.time() super().step_world(dt) self._previous_step_time = time.time() - step_start_Time observations = self.get_sensor_observations() # Whether or not the action taken resulted in a collision observations["collided"] = collided return observations
display_path_agent_renders = True # @param{type:"boolean"} if display_path_agent_renders: print("Rendering observations at path points:") tangent = path_points[1] - path_points[0] agent_state = habitat_sim.AgentState() for ix, point in enumerate(path_points): if ix < len(path_points) - 1: tangent = path_points[ix + 1] - point agent_state.position = point tangent_orientation_matrix = mn.Matrix4.look_at( point, point + tangent, np.array([0, 1.0, 0]) ) tangent_orientation_q = mn.Quaternion.from_matrix( tangent_orientation_matrix.rotation() ) agent_state.rotation = utils.quat_from_magnum(tangent_orientation_q) agent.set_state(agent_state) observations = sim.get_sensor_observations() rgb = observations["color_sensor"] semantic = observations["semantic_sensor"] depth = observations["depth_sensor"] if display: display_sample(rgb, semantic, depth) # %% [markdown] # ##Loading a NavMesh for a scene: # %% [markdown]
def test_kinematics(): cfg_settings = examples.settings.default_sim_settings.copy() cfg_settings[ "scene"] = "data/scene_datasets/habitat-test-scenes/skokloster-castle.glb" # enable the physics simulator: also clears available actions to no-op cfg_settings["enable_physics"] = True cfg_settings["depth_sensor"] = True # test loading the physical scene hab_cfg = examples.settings.make_cfg(cfg_settings) with habitat_sim.Simulator(hab_cfg) as sim: obj_mgr = sim.get_object_template_manager() assert obj_mgr.get_num_templates() > 0 # test adding an object to the world # get handle for object 0, used to test obj_handle_list = obj_mgr.get_template_handles("cheezit") object_id = sim.add_object_by_handle(obj_handle_list[0]) assert len(sim.get_existing_object_ids()) > 0 # test setting the motion type assert sim.set_object_motion_type( habitat_sim.physics.MotionType.STATIC, object_id) assert (sim.get_object_motion_type(object_id) == habitat_sim.physics.MotionType.STATIC) assert sim.set_object_motion_type( habitat_sim.physics.MotionType.KINEMATIC, object_id) assert (sim.get_object_motion_type(object_id) == habitat_sim.physics.MotionType.KINEMATIC) # test kinematics I = np.identity(4) # test get and set translation sim.set_translation(np.array([0, 1.0, 0]), object_id) assert np.allclose(sim.get_translation(object_id), np.array([0, 1.0, 0])) # test object SceneNode object_node = sim.get_object_scene_node(object_id) assert np.allclose(sim.get_translation(object_id), object_node.translation) # test get and set transform sim.set_transformation(I, object_id) assert np.allclose(sim.get_transformation(object_id), I) # test get and set rotation Q = quat_from_angle_axis(np.pi, np.array([0, 1.0, 0])) expected = np.eye(4) expected[0:3, 0:3] = quaternion.as_rotation_matrix(Q) sim.set_rotation(quat_to_magnum(Q), object_id) assert np.allclose(sim.get_transformation(object_id), expected) assert np.allclose(quat_from_magnum(sim.get_rotation(object_id)), Q) # test object removal sim.remove_object(object_id) assert len(sim.get_existing_object_ids()) == 0 obj_handle_list = obj_mgr.get_template_handles("cheezit") object_id = sim.add_object_by_handle(obj_handle_list[0]) prev_time = 0.0 for _ in range(2): # do some kinematics here (todo: translating or rotating instead of absolute) sim.set_translation(np.random.rand(3), object_id) T = sim.get_transformation(object_id) # noqa : F841 # test getting observation sim.step(random.choice(list( hab_cfg.agents[0].action_space.keys()))) # check that time is increasing in the world assert sim.get_world_time() > prev_time prev_time = sim.get_world_time() sim.remove_object(object_id) # test attaching/dettaching an Agent to/from physics simulation agent_node = sim.agents[0].scene_node obj_handle_list = obj_mgr.get_template_handles("cheezit") object_id = sim.add_object_by_handle(obj_handle_list[0], agent_node) sim.set_translation(np.random.rand(3), object_id) assert np.allclose(agent_node.translation, sim.get_translation(object_id)) sim.remove_object(object_id, False) # don't delete the agent's node assert agent_node.translation # test get/set RigidState object_id = sim.add_object_by_handle(obj_handle_list[0]) targetRigidState = habitat_sim.bindings.RigidState( mn.Quaternion(), np.array([1.0, 2.0, 3.0])) sim.set_rigid_state(targetRigidState, object_id) objectRigidState = sim.get_rigid_state(object_id) assert np.allclose(objectRigidState.translation, targetRigidState.translation) assert objectRigidState.rotation == targetRigidState.rotation
def test_kinematics(): cfg_settings = examples.settings.default_sim_settings.copy() cfg_settings[ "scene"] = "data/scene_datasets/habitat-test-scenes/skokloster-castle.glb" # enable the physics simulator: also clears available actions to no-op cfg_settings["enable_physics"] = True cfg_settings["depth_sensor"] = True # test loading the physical scene hab_cfg = examples.settings.make_cfg(cfg_settings) with habitat_sim.Simulator(hab_cfg) as sim: # get the rigid object attributes manager, which manages # templates used to create objects obj_template_mgr = sim.get_object_template_manager() obj_template_mgr.load_configs("data/objects/example_objects/", True) assert obj_template_mgr.get_num_templates() > 0 # get the rigid object manager, which provides direct # access to objects rigid_obj_mgr = sim.get_rigid_object_manager() # test adding an object to the world # get handle for object 0, used to test obj_handle_list = obj_template_mgr.get_template_handles("cheezit") cheezit_box = rigid_obj_mgr.add_object_by_template_handle( obj_handle_list[0]) assert rigid_obj_mgr.get_num_objects() > 0 assert (len(rigid_obj_mgr.get_object_handles()) == rigid_obj_mgr.get_num_objects()) # test setting the motion type cheezit_box.motion_type = habitat_sim.physics.MotionType.STATIC assert cheezit_box.motion_type == habitat_sim.physics.MotionType.STATIC cheezit_box.motion_type = habitat_sim.physics.MotionType.KINEMATIC assert cheezit_box.motion_type == habitat_sim.physics.MotionType.KINEMATIC # test kinematics I = np.identity(4) # test get and set translation cheezit_box.translation = [0.0, 1.0, 0.0] assert np.allclose(cheezit_box.translation, np.array([0.0, 1.0, 0.0])) # test object SceneNode assert np.allclose(cheezit_box.translation, cheezit_box.root_scene_node.translation) # test get and set transform cheezit_box.transformation = I assert np.allclose(cheezit_box.transformation, I) # test get and set rotation Q = quat_from_angle_axis(np.pi, np.array([0.0, 1.0, 0.0])) expected = np.eye(4) expected[0:3, 0:3] = quaternion.as_rotation_matrix(Q) cheezit_box.rotation = quat_to_magnum(Q) assert np.allclose(cheezit_box.transformation, expected) assert np.allclose(quat_from_magnum(cheezit_box.rotation), Q) # test object removal rigid_obj_mgr.remove_object_by_id(cheezit_box.object_id) assert rigid_obj_mgr.get_num_objects() == 0 obj_handle_list = obj_template_mgr.get_template_handles("cheezit") cheezit_box = rigid_obj_mgr.add_object_by_template_handle( obj_handle_list[0]) prev_time = 0.0 for _ in range(2): # do some kinematics here (todo: translating or rotating instead of absolute) cheezit_box.translation = np.random.rand(3) T = cheezit_box.transformation # noqa : F841 # test getting observation sim.step(random.choice(list( hab_cfg.agents[0].action_space.keys()))) # check that time is increasing in the world assert sim.get_world_time() > prev_time prev_time = sim.get_world_time() rigid_obj_mgr.remove_object_by_id(cheezit_box.object_id) # test attaching/dettaching an Agent to/from physics simulation agent_node = sim.agents[0].scene_node obj_handle_list = obj_template_mgr.get_template_handles("cheezit") cheezit_agent = rigid_obj_mgr.add_object_by_template_handle( obj_handle_list[0], agent_node) cheezit_agent.translation = np.random.rand(3) assert np.allclose(agent_node.translation, cheezit_agent.translation) rigid_obj_mgr.remove_object_by_id( cheezit_agent.object_id, delete_object_node=False) # don't delete the agent's node assert agent_node.translation # test get/set RigidState cheezit_box = rigid_obj_mgr.add_object_by_template_handle( obj_handle_list[0]) targetRigidState = habitat_sim.bindings.RigidState( mn.Quaternion(), np.array([1.0, 2.0, 3.0])) cheezit_box.rigid_state = targetRigidState objectRigidState = cheezit_box.rigid_state assert np.allclose(objectRigidState.translation, targetRigidState.translation) assert objectRigidState.rotation == targetRigidState.rotation