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
