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])