def test_unproject(sim): cfg_settings = examples.settings.default_sim_settings.copy() # configure some settings in case defaults change cfg_settings["scene"] = "data/scene_datasets/habitat-test-scenes/apartment_1.glb" cfg_settings["width"] = 101 cfg_settings["height"] = 101 cfg_settings["sensor_height"] = 0 cfg_settings["color_sensor"] = True # loading the scene hab_cfg = examples.settings.make_cfg(cfg_settings) sim.reconfigure(hab_cfg) # position agent sim.agents[0].scene_node.rotation = mn.Quaternion() sim.agents[0].scene_node.translation = mn.Vector3(0.5, 0, 0) # setup camera visual_sensor = sim._sensors["color_sensor"] scene_graph = sim.get_active_scene_graph() scene_graph.set_default_render_camera_parameters(visual_sensor._sensor_object) render_camera = scene_graph.get_default_render_camera() # test unproject center_ray = render_camera.unproject(mn.Vector2i(50, 50)) # middle of the viewport assert np.allclose(center_ray.origin, np.array([0.5, 0, 0]), atol=0.07) assert np.allclose(center_ray.direction, np.array([0, 0, -1.0]), atol=0.02) test_ray_2 = render_camera.unproject( mn.Vector2i(100, 100) ) # bottom right of the viewport assert np.allclose( test_ray_2.direction, np.array([0.569653, -0.581161, -0.581161]), atol=0.07 )
def set_object_state_from_agent( sim, ob_id, offset=np.array([0, 2.0, -1.5]), orientation=mn.Quaternion(((0, 0, 0), 1)), ): agent_transform = sim.agents[0].scene_node.transformation_matrix() ob_translation = agent_transform.transform_point(offset) sim.set_translation(ob_translation, ob_id) sim.set_rotation(orientation, ob_id)
def random_quaternion(): r"""Convenience function to sample a random Magnum::Quaternion. See http://planning.cs.uiuc.edu/node198.html. """ u = np.random.rand(3) qAxis = np.array([ math.sqrt(1 - u[0]) * math.cos(2 * math.pi * u[1]), math.sqrt(u[0]) * math.sin(2 * math.pi * u[2]), math.sqrt(u[0]) * math.cos(2 * math.pi * u[2]), ]) return mn.Quaternion(qAxis, math.sqrt(1 - u[0]) * math.sin(2 * math.pi * u[1]))
def simulate(sim, dt=1.0, get_frames=True, data=None): """Simulate dt seconds at 60Hz to the nearest fixed timestep""" observations = [] start_time = sim.get_world_time() while sim.get_world_time() < start_time + dt: sim.step_physics(1.0 / 60.0) if get_frames: observations.append(sim.get_sensor_observations()) if "observations" in data: data["observations"] += observations def_offset = np.array([0, 1, -1.5]) def_orientation = mn.Quaternion(((0, 0, 0), 1)) def set_object_state_from_agent( sim, ob_id, offset=def_offset, orientation=def_orientation, ): agent_transform = sim.agents[0].scene_node.transformation_matrix() ob_translation = agent_transform.transform_point(offset) sim.set_translation(ob_translation, ob_id) sim.set_rotation(orientation, ob_id) # This tutorial walks through how to use VHACD and the time optimizations it can provide.
def quat_to_magnum(quat: qt.quaternion) -> mn.Quaternion: return mn.Quaternion(quat.imag, quat.real)
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_velocity_control(): cfg_settings = examples.settings.default_sim_settings.copy() cfg_settings["scene"] = "NONE" cfg_settings["enable_physics"] = True hab_cfg = examples.settings.make_cfg(cfg_settings) with habitat_sim.Simulator(hab_cfg) as sim: sim.set_gravity(np.array([0, 0, 0.0])) obj_mgr = sim.get_object_template_manager() template_path = osp.abspath("data/test_assets/objects/nested_box") template_ids = obj_mgr.load_object_configs(template_path) object_template = obj_mgr.get_template_by_ID(template_ids[0]) object_template.linear_damping = 0.0 object_template.angular_damping = 0.0 obj_mgr.register_template(object_template) obj_handle = obj_mgr.get_template_handle_by_ID(template_ids[0]) for iteration in range(2): sim.reset() object_id = sim.add_object_by_handle(obj_handle) vel_control = sim.get_object_velocity_control(object_id) if iteration == 0: if (sim.get_object_motion_type(object_id) != habitat_sim.physics.MotionType.DYNAMIC): # Non-dynamic simulator in use. Skip 1st pass. sim.remove_object(object_id) continue elif iteration == 1: # test KINEMATIC sim.set_object_motion_type( habitat_sim.physics.MotionType.KINEMATIC, object_id) # test global velocities vel_control.linear_velocity = np.array([1.0, 0, 0]) vel_control.angular_velocity = np.array([0, 1.0, 0]) vel_control.controlling_lin_vel = True vel_control.controlling_ang_vel = True while sim.get_world_time() < 1.0: # 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 assert np.allclose(sim.get_translation(object_id), ground_truth_pos, atol=0.01) ground_truth_q = mn.Quaternion([[0, 0.480551, 0], 0.876967]) angle_error = mn.math.angle(ground_truth_q, sim.get_rotation(object_id)) assert angle_error < mn.Rad(0.005) sim.reset() # test local velocities (turn in a half circle) vel_control.lin_vel_is_local = True vel_control.ang_vel_is_local = True vel_control.linear_velocity = np.array([0, 0, -math.pi]) vel_control.angular_velocity = np.array([math.pi * 2.0, 0, 0]) sim.set_translation(np.array([0, 0, 0.0]), object_id) sim.set_rotation(mn.Quaternion(), object_id) while sim.get_world_time() < 0.5: # 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]) print(sim.get_translation(object_id)) assert np.allclose(sim.get_translation(object_id), np.array([0, 1.0, 0.0]), atol=0.07) angle_error = mn.math.angle(ground_truth_q, sim.get_rotation(object_id)) assert angle_error < mn.Rad(0.05) sim.remove_object(object_id)
else: sim.reconfigure(playback_cfg) agent_state = habitat_sim.AgentState() sim.initialize_agent(0, agent_state) agent_node = sim.get_agent(0).body.object sensor_node = sim._sensors["rgba_camera"]._sensor_object.object # %% [markdown] # ## Place dummy agent with identity transform. # For replay playback, we place a dummy agent at the origin and then transform the sensor using the "sensor" user transform stored in the replay. In the future, Habitat will offer a cleaner way to play replays without an agent. # %% agent_node.translation = [0.0, 0.0, 0.0] agent_node.rotation = mn.Quaternion() # %% [markdown] # ## Load our earlier saved replay. # %% player = sim.gfx_replay_manager.read_keyframes_from_file(replay_filepath) assert player # %% [markdown] # ## Play the replay! # Note call to player.set_keyframe_index. Note also call to player.get_user_transform. For this playback, we restore our sensor to the original sensor transform from the episode. In this way, we reproduce the same observations. Note this doesn't happen automatically when using gfx replay; you must position your agent, sensor, or camera explicitly when playing a replay. # %% observations = [] print("play replay #0...")
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
def step( self, *args: Any, task: EmbodiedTask, linear_velocity: float, angular_velocity: float, time_step: Optional[float] = None, allow_sliding: Optional[bool] = None, **kwargs: Any, ): r"""Moves the agent with a provided linear and angular velocity for the provided amount of time Args: linear_velocity: between [-1,1], scaled according to config.LIN_VEL_RANGE angular_velocity: between [-1,1], scaled according to config.ANG_VEL_RANGE time_step: amount of time to move the agent for allow_sliding: whether the agent will slide on collision """ if allow_sliding is None: allow_sliding = self._sim.config.sim_cfg.allow_sliding # type: ignore if time_step is None: time_step = self.time_step # Convert from [-1, 1] to [0, 1] range linear_velocity = (linear_velocity + 1.0) / 2.0 angular_velocity = (angular_velocity + 1.0) / 2.0 # Scale actions linear_velocity = self.min_lin_vel + linear_velocity * ( self.max_lin_vel - self.min_lin_vel) angular_velocity = self.min_ang_vel + angular_velocity * ( self.max_ang_vel - self.min_ang_vel) # Stop is called if both linear/angular speed are below their threshold if (abs(linear_velocity) < self.min_abs_lin_speed and abs(angular_velocity) < self.min_abs_ang_speed): task.is_stop_called = True # type: ignore return self._sim.get_observations_at(position=None, rotation=None) angular_velocity = np.deg2rad(angular_velocity) self.vel_control.linear_velocity = np.array( [0.0, 0.0, -linear_velocity]) self.vel_control.angular_velocity = np.array( [0.0, angular_velocity, 0.0]) agent_state = self._sim.get_agent_state() # Convert from np.quaternion to mn.Quaternion normalized_quaternion = agent_state.rotation agent_mn_quat = mn.Quaternion(normalized_quaternion.imag, normalized_quaternion.real) current_rigid_state = RigidState( agent_mn_quat, agent_state.position, ) # manually integrate the rigid state goal_rigid_state = self.vel_control.integrate_transform( time_step, current_rigid_state) # snap rigid state to navmesh and set state to object/agent if allow_sliding: step_fn = self._sim.pathfinder.try_step # type: ignore else: step_fn = self._sim.pathfinder.try_step_no_sliding # type: ignore final_position = step_fn(agent_state.position, goal_rigid_state.translation) final_rotation = [ *goal_rigid_state.rotation.vector, goal_rigid_state.rotation.scalar, ] # Check if a collision occured dist_moved_before_filter = (goal_rigid_state.translation - agent_state.position).dot() dist_moved_after_filter = (final_position - agent_state.position).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 = 1e-5 collided = (dist_moved_after_filter + EPS) < dist_moved_before_filter agent_observations = self._sim.get_observations_at( position=final_position, rotation=final_rotation, keep_agent_at_new_pose=True, ) # TODO: Make a better way to flag collisions self._sim._prev_sim_obs["collided"] = collided # type: ignore return agent_observations
def euler_to_quat(rpy): rot = quaternion.from_euler_angles(rpy) rot = mn.Quaternion(mn.Vector3(rot.vec), rot.w) return rot