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
