def make_cfg(self, settings): sim_cfg = habitat_sim.SimulatorConfiguration() sim_cfg.gpu_device_id = 0 sim_cfg.scene.id = settings["scene"] # Note: all sensors must have the same resolution sensors = { "color_sensor": { "sensor_type": habitat_sim.SensorType.COLOR, "resolution": [settings["height"], settings["width"]], "position": [0.0, settings["sensor_height"], 0.0], }, "depth_sensor": { "sensor_type": habitat_sim.SensorType.DEPTH, "resolution": [settings["height"], settings["width"]], "position": [0.0, settings["sensor_height"], 0.0], }, "semantic_sensor": { "sensor_type": habitat_sim.SensorType.SEMANTIC, "resolution": [settings["height"], settings["width"]], "position": [0.0, settings["sensor_height"], 0.0], }, } sensor_specs = [] for sensor_uuid, sensor_params in sensors.items(): if settings[sensor_uuid]: sensor_spec = habitat_sim.SensorSpec() sensor_spec.uuid = sensor_uuid sensor_spec.sensor_type = sensor_params["sensor_type"] sensor_spec.resolution = sensor_params["resolution"] sensor_spec.position = sensor_params["position"] sensor_specs.append(sensor_spec) # Here you can specify the amount of displacement in a forward action and the turn angle agent_cfg = habitat_sim.agent.AgentConfiguration() agent_cfg.sensor_specifications = sensor_specs agent_cfg.action_space = { "do_nothing": habitat_sim.agent.ActionSpec( "move_forward", habitat_sim.agent.ActuationSpec(amount=0.)), "move_forward": habitat_sim.agent.ActionSpec( "move_forward", habitat_sim.agent.ActuationSpec(amount=0.25)), "turn_left": habitat_sim.agent.ActionSpec( "turn_left", habitat_sim.agent.ActuationSpec(amount=self.turn_angle)), "turn_right": habitat_sim.agent.ActionSpec( "turn_right", habitat_sim.agent.ActuationSpec(amount=self.turn_angle)), "look_up": habitat_sim.ActionSpec( "look_up", habitat_sim.ActuationSpec(amount=self.rot_interval)), "look_down": habitat_sim.ActionSpec( "look_down", habitat_sim.ActuationSpec(amount=self.rot_interval)), "look_down_init": habitat_sim.ActionSpec("look_down", habitat_sim.ActuationSpec(amount=100.0)) } return habitat_sim.Configuration(sim_cfg, [agent_cfg]), sim_cfg
def test_pyrobot_noisy_actions(noise_multiplier, robot, controller): np.random.seed(0) scene_graph = hsim.SceneGraph() agent_config = habitat_sim.AgentConfiguration() agent_config.action_space = dict( noisy_move_backward=habitat_sim.ActionSpec( "pyrobot_noisy_move_backward", habitat_sim.PyRobotNoisyActuationSpec( amount=1.0, robot=robot, controller=controller, noise_multiplier=noise_multiplier, ), ), noisy_move_forward=habitat_sim.ActionSpec( "pyrobot_noisy_move_forward", habitat_sim.PyRobotNoisyActuationSpec( amount=1.0, robot=robot, controller=controller, noise_multiplier=noise_multiplier, ), ), noisy_turn_left=habitat_sim.ActionSpec( "pyrobot_noisy_turn_left", habitat_sim.PyRobotNoisyActuationSpec( amount=90.0, robot=robot, controller=controller, noise_multiplier=noise_multiplier, ), ), noisy_turn_right=habitat_sim.ActionSpec( "pyrobot_noisy_turn_right", habitat_sim.PyRobotNoisyActuationSpec( amount=90.0, robot=robot, controller=controller, noise_multiplier=noise_multiplier, ), ), move_backward=habitat_sim.ActionSpec( "move_backward", habitat_sim.ActuationSpec(amount=1.0)), move_forward=habitat_sim.ActionSpec( "move_forward", habitat_sim.ActuationSpec(amount=1.0)), turn_left=habitat_sim.ActionSpec( "turn_left", habitat_sim.ActuationSpec(amount=90.0)), turn_right=habitat_sim.ActionSpec( "turn_right", habitat_sim.ActuationSpec(amount=90.0)), ) agent = habitat_sim.Agent(scene_graph.get_root_node().create_child(), agent_config) for base_action in set( act.replace("noisy_", "") for act in agent_config.action_space): state = agent.state state.rotation = np.quaternion(1, 0, 0, 0) agent.state = state agent.act(base_action) base_state = agent.state delta_translations = [] delta_rotations = [] for _ in range(300): agent.state = state agent.act(f"noisy_{base_action}") noisy_state = agent.state delta_translations.append( _delta_translation(base_state, noisy_state)) delta_rotations.append(_delta_rotation(base_state, noisy_state)) delta_translations = np.stack(delta_translations) delta_rotations = np.stack(delta_rotations) if "move" in base_action: noise_model = pyrobot_noise_models[robot][controller].linear_motion else: noise_model = pyrobot_noise_models[robot][ controller].rotational_motion EPS = 5e-2 assert (np.linalg.norm(noise_model.linear.mean * noise_multiplier - np.abs(delta_translations.mean(0))) < EPS) assert (np.linalg.norm(noise_model.rotation.mean * noise_multiplier - np.abs(delta_rotations.mean(0))) < EPS) assert (np.linalg.norm(noise_model.linear.cov * noise_multiplier - np.diag(delta_translations.std(0)**2)) < EPS) assert (np.linalg.norm(noise_model.rotation.cov * noise_multiplier - (delta_rotations.std(0)**2)) < EPS)
def test_greedy_follower(test_navmesh, move_filter_fn, action_noise, pbar): global num_fails global num_tested global total_spl if not osp.exists(test_navmesh): pytest.skip(f"{test_navmesh} not found") pathfinder = habitat_sim.PathFinder() pathfinder.load_nav_mesh(test_navmesh) assert pathfinder.is_loaded pathfinder.seed(0) np.random.seed(seed=0) scene_graph = habitat_sim.SceneGraph() agent = habitat_sim.Agent(scene_graph.get_root_node().create_child()) agent.controls.move_filter_fn = getattr(pathfinder, move_filter_fn) agent.agent_config.action_space["turn_left"].actuation.amount = TURN_DEGREE agent.agent_config.action_space[ "turn_right"].actuation.amount = TURN_DEGREE if action_noise: # "_" prefix the perfect actions so that we can use noisy actions instead agent.agent_config.action_space = { "_" + k: v for k, v in agent.agent_config.action_space.items() } agent.agent_config.action_space.update(**dict( move_forward=habitat_sim.ActionSpec( "pyrobot_noisy_move_forward", habitat_sim.PyRobotNoisyActuationSpec(amount=0.25), ), turn_left=habitat_sim.ActionSpec( "pyrobot_noisy_turn_left", habitat_sim.PyRobotNoisyActuationSpec(amount=TURN_DEGREE), ), turn_right=habitat_sim.ActionSpec( "pyrobot_noisy_turn_right", habitat_sim.PyRobotNoisyActuationSpec(amount=TURN_DEGREE), ), )) follower = habitat_sim.GreedyGeodesicFollower( pathfinder, agent, forward_key="move_forward", left_key="turn_left", right_key="turn_right", ) test_spl = 0.0 for _ in range(NUM_TESTS): follower.reset() state = habitat_sim.AgentState() while True: state.position = pathfinder.get_random_navigable_point() goal_pos = pathfinder.get_random_navigable_point() path = habitat_sim.ShortestPath() path.requested_start = state.position path.requested_end = goal_pos if pathfinder.find_path(path) and path.geodesic_distance > 2.0: break agent.state = state failed = False gt_geo = path.geodesic_distance agent_distance = 0.0 last_xyz = state.position num_acts = 0 # If there is not action noise, then we can use find_path to get all the actions if not action_noise: try: action_list = follower.find_path(goal_pos) except habitat_sim.errors.GreedyFollowerError: action_list = [None] while True: # If there is action noise, we need to plan a single action, actually take it, and repeat if action_noise: try: next_action = follower.next_action_along(goal_pos) except habitat_sim.errors.GreedyFollowerError: break else: next_action = action_list[0] action_list = action_list[1:] if next_action is None: break agent.act(next_action) agent_distance += np.linalg.norm(last_xyz - agent.state.position) last_xyz = agent.state.position num_acts += 1 if num_acts > 1e4: break end_state = agent.state path.requested_start = end_state.position pathfinder.find_path(path) failed = path.geodesic_distance > follower.forward_spec.amount spl = float(not failed) * gt_geo / max(gt_geo, agent_distance) test_spl += spl if test_all: num_fails += float(failed) num_tested += 1 total_spl += spl pbar.set_postfix( num_fails=num_fails, failure_rate=num_fails / num_tested, spl=total_spl / num_tested, ) pbar.update() if not test_all: assert test_spl / NUM_TESTS >= ACCEPTABLE_SPLS[(move_filter_fn, action_noise)]
def get(self): return { HabitatSimActions.STOP: habitat_sim.ActionSpec("stop"), HabitatSimActions.MOVE_FORWARD: habitat_sim.ActionSpec( "pyrobot_noisy_move_forward", habitat_sim.PyRobotNoisyActuationSpec( amount=self.config.FORWARD_STEP_SIZE, robot=self.config.NOISE_MODEL.ROBOT, controller=self.config.NOISE_MODEL.CONTROLLER, noise_multiplier=self.config.NOISE_MODEL.NOISE_MULTIPLIER, ), ), HabitatSimActions.TURN_LEFT: habitat_sim.ActionSpec( "pyrobot_noisy_turn_left", habitat_sim.PyRobotNoisyActuationSpec( amount=self.config.TURN_ANGLE, robot=self.config.NOISE_MODEL.ROBOT, controller=self.config.NOISE_MODEL.CONTROLLER, noise_multiplier=self.config.NOISE_MODEL.NOISE_MULTIPLIER, ), ), HabitatSimActions.TURN_RIGHT: habitat_sim.ActionSpec( "pyrobot_noisy_turn_right", habitat_sim.PyRobotNoisyActuationSpec( amount=self.config.TURN_ANGLE, robot=self.config.NOISE_MODEL.ROBOT, controller=self.config.NOISE_MODEL.CONTROLLER, noise_multiplier=self.config.NOISE_MODEL.NOISE_MULTIPLIER, ), ), HabitatSimActions.LOOK_UP: habitat_sim.ActionSpec( "look_up", habitat_sim.ActuationSpec(amount=self.config.TILT_ANGLE), ), HabitatSimActions.LOOK_DOWN: habitat_sim.ActionSpec( "look_down", habitat_sim.ActuationSpec(amount=self.config.TILT_ANGLE), ), # The perfect actions are needed for the oracle planner "_forward": habitat_sim.ActionSpec( "move_forward", habitat_sim.ActuationSpec( amount=self.config.FORWARD_STEP_SIZE), ), "_left": habitat_sim.ActionSpec( "turn_left", habitat_sim.ActuationSpec(amount=self.config.TURN_ANGLE), ), "_right": habitat_sim.ActionSpec( "turn_right", habitat_sim.ActuationSpec(amount=self.config.TURN_ANGLE), ), }
def create_sim_config( self, _sensor_suite: SensorSuite) -> habitat_sim.Configuration: sim_config = habitat_sim.SimulatorConfiguration() sim_config.scene.id = self.config.SCENE sim_config.gpu_device_id = self.config.HABITAT_SIM_V0.GPU_DEVICE_ID agent_config = habitat_sim.AgentConfiguration() overwrite_config(config_from=self._get_agent_config(), config_to=agent_config) sensor_specifications = [] for sensor in _sensor_suite.sensors.values(): sim_sensor_cfg = habitat_sim.SensorSpec() sim_sensor_cfg.uuid = sensor.uuid sim_sensor_cfg.resolution = list( sensor.observation_space.shape[:2]) sim_sensor_cfg.parameters["hfov"] = str(sensor.config.HFOV) sim_sensor_cfg.position = sensor.config.POSITION # TODO(maksymets): Add configure method to Sensor API to avoid # accessing child attributes through parent interface sim_sensor_cfg.sensor_type = sensor.sim_sensor_type # type: ignore sensor_specifications.append(sim_sensor_cfg) # If there is no sensors specified create a dummy sensor so simulator # won't throw an error if not _sensor_suite.sensors.values(): sim_sensor_cfg = habitat_sim.SensorSpec() sim_sensor_cfg.resolution = [1, 1] sensor_specifications.append(sim_sensor_cfg) agent_config.sensor_specifications = sensor_specifications agent_config.action_space = { SimulatorActions.STOP.value: habitat_sim.ActionSpec("stop"), SimulatorActions.MOVE_FORWARD.value: habitat_sim.ActionSpec( "move_forward", habitat_sim.ActuationSpec( amount=self.config.FORWARD_STEP_SIZE), ), SimulatorActions.TURN_LEFT.value: habitat_sim.ActionSpec( "turn_left", habitat_sim.ActuationSpec(amount=self.config.TURN_ANGLE), ), SimulatorActions.TURN_RIGHT.value: habitat_sim.ActionSpec( "turn_right", habitat_sim.ActuationSpec(amount=self.config.TURN_ANGLE), ), SimulatorActions.LOOK_UP.value: habitat_sim.ActionSpec( "look_up", habitat_sim.ActuationSpec(amount=self.config.TILT_ANGLE), ), SimulatorActions.LOOK_DOWN.value: habitat_sim.ActionSpec( "look_down", habitat_sim.ActuationSpec(amount=self.config.TILT_ANGLE), ), } return habitat_sim.Configuration(sim_config, [agent_config])
def main(): # We will define an action that moves the agent and turns it by some amount # First, define a class to keep the parameters of the control # @attr.s is just syntatic sugar for creating these data-classes @attr.s(auto_attribs=True, slots=True) class MoveAndSpinSpec: forward_amount: float spin_amount: float print(MoveAndSpinSpec(1.0, 45.0)) # Register the control functor # This action will be an action that effects the body, so body_action=True @controls.register_move_fn(body_action=True) class MoveForwardAndSpin(habitat_sim.SceneNodeControl): def __call__(self, scene_node: habitat_sim.SceneNode, actuation_spec: MoveAndSpinSpec): forward_ax = (scene_node.absolute_transformation()[0:3, 0:3] @ habitat_sim.geo.FRONT) scene_node.translate_local(forward_ax * actuation_spec.forward_amount) # Rotate about the +y (up) axis rotation_ax = habitat_sim.geo.UP scene_node.rotate_local(np.deg2rad(actuation_spec.spin_amount), rotation_ax) # Calling normalize is needed after rotating to deal with machine precision errors scene_node.normalize() # We can also register the function with a custom name controls.register_move_fn(MoveForwardAndSpin, name="my_custom_name", body_action=True) # We can also re-register this function such that it effects just the sensors controls.register_move_fn(MoveForwardAndSpin, name="move_forward_and_spin_sensors", body_action=False) # Now we need to add this action to the agent's action space in the configuration! agent_config = habitat_sim.AgentConfiguration() _pprint(agent_config.action_space) # We can add the control function in a bunch of ways # Note that the name of the action does not need to match the name the control function # was registered under. # The habitat_sim.ActionSpec defines an action. The first arguement is the regsitered name # of the control spec, the second is the parameter spec agent_config.action_space["fwd_and_spin"] = habitat_sim.ActionSpec( "move_forward_and_spin", MoveAndSpinSpec(1.0, 45.0)) # Add the sensor version agent_config.action_space["fwd_and_spin_sensors"] = habitat_sim.ActionSpec( "move_forward_and_spin_sensors", MoveAndSpinSpec(1.0, 45.0)) # Add the same control with different parameters agent_config.action_space["fwd_and_spin_double"] = habitat_sim.ActionSpec( "move_forward_and_spin", MoveAndSpinSpec(2.0, 90.0)) # Use the custom name with an integer name for the action agent_config.action_space[100] = habitat_sim.ActionSpec( "my_custom_name", MoveAndSpinSpec(0.1, 1.0)) _pprint(agent_config.action_space) # Spin up the simulator backend_cfg = habitat_sim.SimulatorConfiguration() backend_cfg.scene.id = "data/scene_datasets/habitat-test-scenes/van-gogh-room.glb" sim = habitat_sim.Simulator( habitat_sim.Configuration(backend_cfg, [agent_config])) print(sim.get_agent(0).state) # Take the new actions! sim.step("fwd_and_spin") print(sim.get_agent(0).state) # Take the new actions! sim.step("fwd_and_spin_sensors") print(sim.get_agent(0).state) sim.step("fwd_and_spin_double") print(sim.get_agent(0).state) sim.step(100) print(sim.get_agent(0).state) sim.close() del sim # Let's define a strafe action! @attr.s(auto_attribs=True, slots=True) class StrafeActuationSpec: forward_amount: float # Classic strafing is to move perpendicular (90 deg) to the forward direction strafe_angle: float = 90.0 def _strafe_impl(scene_node: habitat_sim.SceneNode, forward_amount: float, strafe_angle: float): forward_ax = (scene_node.absolute_transformation()[0:3, 0:3] @ habitat_sim.geo.FRONT) rotation = habitat_sim.utils.quat_from_angle_axis( np.deg2rad(strafe_angle), habitat_sim.geo.UP) move_ax = habitat_sim.utils.quat_rotate_vector(rotation, forward_ax) scene_node.translate_local(move_ax * forward_amount) @controls.register_move_fn(body_action=True) class StrafeLeft(habitat_sim.SceneNodeControl): def __call__(self, scene_node: habitat_sim.SceneNode, actuation_spec: StrafeActuationSpec): _strafe_impl(scene_node, actuation_spec.forward_amount, actuation_spec.strafe_angle) @controls.register_move_fn(body_action=True) class StrafeRight(habitat_sim.SceneNodeControl): def __call__(self, scene_node: habitat_sim.SceneNode, actuation_spec: StrafeActuationSpec): _strafe_impl(scene_node, actuation_spec.forward_amount, -actuation_spec.strafe_angle) agent_config = habitat_sim.AgentConfiguration() agent_config.action_space["strafe_left"] = habitat_sim.ActionSpec( "strafe_left", StrafeActuationSpec(0.25)) agent_config.action_space["strafe_right"] = habitat_sim.ActionSpec( "strafe_right", StrafeActuationSpec(0.25)) sim = habitat_sim.Simulator( habitat_sim.Configuration(backend_cfg, [agent_config])) print(sim.get_agent(0).state) sim.step("strafe_left") print(sim.get_agent(0).state) sim.step("strafe_right") print(sim.get_agent(0).state)
def make_cfg(scene): default_sim_settings["scene"] = scene settings = default_sim_settings sim_cfg = hsim.SimulatorConfiguration() if "enable_physics" in settings.keys(): sim_cfg.enable_physics = settings["enable_physics"] else: sim_cfg.enable_physics = False if "physics_config_file" in settings.keys(): sim_cfg.physics_config_file = settings["physics_config_file"] print("sim_cfg.physics_config_file = " + sim_cfg.physics_config_file) sim_cfg.gpu_device_id = 0 #print(settings["scene"]) sim_cfg.scene.id = settings["scene"] # define default sensor parameters (see src/esp/Sensor/Sensor.h) sensors = { "color_sensor": { # active if sim_settings["color_sensor"] "sensor_type": hsim.SensorType.COLOR, "resolution": [settings["height"], settings["width"]], "position": [0.0, settings["sensor_height"], 0.0], }, "depth_sensor": { # active if sim_settings["depth_sensor"] "sensor_type": hsim.SensorType.DEPTH, "resolution": [settings["height"], settings["width"]], "position": [0.0, settings["sensor_height"], 0.0], }, "semantic_sensor": { # active if sim_settings["semantic_sensor"] "sensor_type": hsim.SensorType.SEMANTIC, "resolution": [settings["height"], settings["width"]], "position": [0.0, settings["sensor_height"], 0.0], }, } # create sensor specifications sensor_specs = [] for sensor_uuid, sensor_params in sensors.items(): if settings[sensor_uuid]: sensor_spec = hsim.SensorSpec() sensor_spec.uuid = sensor_uuid sensor_spec.sensor_type = sensor_params["sensor_type"] sensor_spec.resolution = sensor_params["resolution"] sensor_spec.position = sensor_params["position"] sensor_spec.gpu2gpu_transfer = False if not settings["silent"]: print("==== Initialized Sensor Spec: =====") print("Sensor uuid: ", sensor_spec.uuid) print("Sensor type: ", sensor_spec.sensor_type) print("Sensor position: ", sensor_spec.position) print("===================================") sensor_specs.append(sensor_spec) # create agent specifications agent_cfg = habitat_sim.agent.AgentConfiguration() agent_cfg.sensor_specifications = sensor_specs agent_cfg.action_space = { "move_forward": habitat_sim.agent.ActionSpec( "move_forward", habitat_sim.agent.ActuationSpec(amount=0.25) ), "move_backward": habitat_sim.agent.ActionSpec( "move_backward", habitat_sim.agent.ActuationSpec(amount=0.25) ), "move_up": habitat_sim.agent.ActionSpec( "move_up", habitat_sim.agent.ActuationSpec(amount=0.25) ), "move_down": habitat_sim.agent.ActionSpec( "move_down", habitat_sim.agent.ActuationSpec(amount=0.25) ), "move_right": habitat_sim.agent.ActionSpec( "move_right", habitat_sim.agent.ActuationSpec(amount=0.25) ), "move_left": habitat_sim.agent.ActionSpec( "move_left", habitat_sim.agent.ActuationSpec(amount=0.25) ), "turn_left": habitat_sim.agent.ActionSpec( "turn_left", habitat_sim.agent.ActuationSpec(amount=10.0) ), "turn_right": habitat_sim.agent.ActionSpec( "turn_right", habitat_sim.agent.ActuationSpec(amount=10.0) ), "rotate_right": habitat_sim.ActionSpec( "spin_y", SpinSpec(36.0) ), "rotate_left": habitat_sim.ActionSpec( "spin_y", SpinSpec(-36.0) ), "rotate_up": habitat_sim.ActionSpec( "spin_z", SpinSpec(36.0) ), "rotate_down": habitat_sim.ActionSpec( "spin_z", SpinSpec(-36.0) ), "rotate_forward": habitat_sim.ActionSpec( "spin_x", SpinSpec(36.0) ), "rotate_backward": habitat_sim.ActionSpec( "spin_x", SpinSpec(-36.0) ) } # override action space to no-op to test physics if sim_cfg.enable_physics: agent_cfg.action_space = { "move_forward": habitat_sim.agent.ActionSpec( "move_forward", habitat_sim.agent.ActuationSpec(amount=0.0) ) } return habitat_sim.Configuration(sim_cfg, [agent_cfg])