def _render_and_load_gt(sim, scene, sensor_type, gpu2gpu):
obs = _render_scene(sim, scene, sensor_type, gpu2gpu)
# now that sensors are constructed, test some getter/setters
if hasattr(sim.get_agent(0)._sensors[sensor_type], "fov"):
sim.get_agent(0)._sensors[sensor_type].fov = mn.Deg(80)
assert sim.get_agent(0)._sensors[sensor_type].fov == mn.Deg(
80
), "fov not set correctly"
assert sim.get_agent(0)._sensors[sensor_type].hfov == mn.Deg(
80
), "hfov not set correctly"
gt_obs_file = osp.abspath(
osp.join(
osp.dirname(__file__),
"gt_data",
"{}-{}.npy".format(osp.basename(osp.splitext(scene)[0]), sensor_type),
)
)
# if not osp.exists(gt_obs_file):
# np.save(gt_obs_file, obs[sensor_type])
gt = np.load(gt_obs_file)
return obs, gt
def _rotate_local(
scene_node: SceneNode, theta: float, axis: int, constraint: Optional[float] = None
):
if constraint is not None:
rotation = scene_node.rotation
if abs(float(rotation.angle())) > 0:
ref_vector = mn.Vector3()
ref_vector[axis] = 1
if mn.math.angle(ref_vector, rotation.axis().normalized()) > mn.Rad(1e-3):
raise RuntimeError(
"Constrained look only works for a singular look action type"
)
look_vector = rotation.transform_vector(FRONT)
if axis == 0:
look_angle = mn.Rad(np.arctan2(look_vector[1], -look_vector[2]))
elif axis == 1:
look_angle = -mn.Rad(np.arctan2(look_vector[0], -look_vector[2]))
new_angle = look_angle + mn.Deg(theta)
constraint = mn.Deg(constraint)
if new_angle > constraint:
theta = constraint - look_angle
elif new_angle < -constraint:
theta = -constraint - look_angle
_rotate_local_fns[axis](scene_node, mn.Deg(theta))
scene_node.rotation = scene_node.rotation.normalized()
def __init__(self, urdf_path, sim, limit_robo_joints=True, fixed_base=True):
fetch_params = MobileManipulatorParams(
arm_joints=list(range(15, 22)),
gripper_joints=[23, 24],
wheel_joints=[2, 4],
arm_init_params=[-0.45, -1.08, 0.1, 0.935, -0.001, 1.573, 0.005],
gripper_init_params=[0.00, 0.00],
ee_offset=mn.Vector3(0.08, 0, 0),
ee_link=22,
ee_constraint=np.array([[0.4, 1.2], [-0.7, 0.7], [0.25, 1.5]]),
cameras={
"robot_arm": RobotCameraParams(
cam_offset_pos=mn.Vector3(0, 0.0, 0.1),
cam_look_at_pos=mn.Vector3(0.1, 0.0, 0.0),
attached_link_id=22,
relative_transform=mn.Matrix4.rotation_y(mn.Deg(-90))
@ mn.Matrix4.rotation_z(mn.Deg(90)),
),
"robot_head": RobotCameraParams(
cam_offset_pos=mn.Vector3(0.17, 1.2, 0.0),
cam_look_at_pos=mn.Vector3(0.75, 1.0, 0.0),
attached_link_id=-1,
),
"robot_third": RobotCameraParams(
cam_offset_pos=mn.Vector3(-0.5, 1.7, -0.5),
cam_look_at_pos=mn.Vector3(1, 0.0, 0.75),
attached_link_id=-1,
),
},
gripper_closed_state=[0.0, 0.0],
gripper_open_state=[0.04, 0.04],
gripper_state_eps=0.001,
arm_mtr_pos_gain=0.3,
arm_mtr_vel_gain=0.3,
arm_mtr_max_impulse=10.0,
wheel_mtr_pos_gain=0.0,
wheel_mtr_vel_gain=1.3,
wheel_mtr_max_impulse=10.0,
base_offset=mn.Vector3(0, 0, 0),
base_link_names={
"base_link",
"r_wheel_link",
"l_wheel_link",
"r_wheel_link",
"bellows_link",
"bellows_link2",
"estop_link",
"laser_link",
"torso_fixed_link",
},
)
super().__init__(fetch_params, urdf_path, sim, limit_robo_joints, fixed_base)
self.back_joint_id = 6
self.head_rot_jid = 8
self.head_tilt_jid = 9
def _get_arm_cam_transform(self):
"""Helper function to get the transformation of where the arm camera
should be placed.
"""
ee_trans = self.sim_obj.get_link_scene_node(
self.params.ee_link).transformation
offset_trans = mn.Matrix4.translation(self.params.arm_cam_offset_pos)
rot_trans = mn.Matrix4.rotation_y(mn.Deg(-90))
spin_trans = mn.Matrix4.rotation_z(mn.Deg(90))
arm_T = ee_trans @ offset_trans @ rot_trans @ spin_trans
return arm_T
def test_initial_hfov(scene, sensor_type, make_cfg_settings):
if not osp.exists(scene):
pytest.skip("Skipping {}".format(scene))
make_cfg_settings["hfov"] = 70
with habitat_sim.Simulator(make_cfg(make_cfg_settings)) as sim:
assert sim.agents[0]._sensors[sensor_type].hfov == mn.Deg(
70), "HFOV was not properly set"
def _noisy_action_impl(
scene_node: hsim.SceneNode,
translate_amount: float,
rotate_amount: float,
multiplier: float,
model: MotionNoiseModel,
):
# Perform the action in the coordinate system of the node
transform = scene_node.transformation
move_ax = -transform[_Z_AXIS].xyz
perp_ax = transform[_X_AXIS].xyz
# + EPS to make sure 0 is positive. We multiply the mean by the sign of the translation
# as otherwise forward would overshoot on average and backward would undershoot, while
# both should overshoot
translation_noise = multiplier * np.random.multivariate_normal(
np.sign(translate_amount + 1e-8) * model.linear.mean, model.linear.cov)
scene_node.translate_local(move_ax *
(translate_amount + translation_noise[0]) +
perp_ax * translation_noise[1])
# Same deal with rotation about + EPS and why we multiply by the sign
rot_noise = multiplier * np.random.multivariate_normal(
np.sign(rotate_amount + 1e-8) * model.rotation.mean,
model.rotation.cov)
scene_node.rotate_y_local(mn.Deg(rotate_amount) + mn.Rad(rot_noise[0]))
scene_node.rotation = scene_node.rotation.normalized()
def __call__(
self, scene_node: habitat_sim.SceneNode, actuation_spec: SpinSpec
):
# Rotate about the +y (up) axis
rotation_ax = habitat_sim.geo.LEFT
scene_node.rotate_local(mn.Deg(actuation_spec.spin_amount), rotation_ax)
# Calling normalize is needed after rotating to deal with machine precision errors
scene_node.rotation = scene_node.rotation.normalized()
def _render_and_load_gt(sim, scene, sensor_type, gpu2gpu):
gt_data_pose_file = osp.abspath(
osp.join(
osp.dirname(__file__),
"gt_data",
"{}-state.json".format(osp.basename(osp.splitext(scene)[0])),
))
with open(gt_data_pose_file, "r") as f:
render_state = json.load(f)
state = habitat_sim.AgentState()
state.position = render_state["pos"]
state.rotation = quat_from_coeffs(render_state["rot"])
sim.initialize_agent(0, state)
obs = sim.step("move_forward")
assert sensor_type in obs, f"{sensor_type} not in obs"
# now that sensors are constructed, test some getter/setters
sim.get_agent(0)._sensors[sensor_type].fov = mn.Deg(80)
assert sim.get_agent(0)._sensors[sensor_type].fov == mn.Deg(
80), "fov not set correctly"
assert sim.get_agent(0)._sensors[sensor_type].hfov == mn.Deg(
80), "hfov not set correctly"
gt_obs_file = osp.abspath(
osp.join(
osp.dirname(__file__),
"gt_data",
"{}-{}.npy".format(osp.basename(osp.splitext(scene)[0]),
sensor_type),
))
gt = np.load(gt_obs_file)
if gpu2gpu:
torch = pytest.importorskip("torch")
for k, v in obs.items():
if torch.is_tensor(v):
obs[k] = v.cpu().numpy()
return obs, gt
def __call__(self, scene_node: habitat_sim.SceneNode,
actuation_spec: MoveAndSpinSpec):
forward_ax = (np.array(
scene_node.absolute_transformation().rotation_scaling())
@ 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(mn.Deg(actuation_spec.spin_amount),
rotation_ax)
# Calling normalize is needed after rotating to deal with machine precision errors
scene_node.rotation = scene_node.rotation.normalized()
def _noisy_action_impl(
scene_node: hsim.SceneNode,
translate_amount: float,
rotate_amount: float,
multiplier: float,
model: MotionNoiseModel,
motion_type: str,
):
# Perform the action in the coordinate system of the node
transform = scene_node.transformation
move_ax = -transform[_Z_AXIS].xyz
perp_ax = transform[_X_AXIS].xyz
if motion_type == "rotational":
translation_noise = multiplier * model.linear.sample()
else:
# The robot will always move a little bit. This has to be defined based on the intended actuation
# as otherwise small rotation amounts would be invalid. However, pretty quickly, we'll
# get to the truncation of 3 sigma
trunc = [(-0.95 * np.abs(translate_amount), None), None]
translation_noise = multiplier * model.linear.sample(trunc)
# + EPS to make sure 0 is positive. We multiply by the sign of the translation
# as otherwise forward would overshoot on average and backward would undershoot, while
# both should overshoot
translation_noise *= np.sign(translate_amount + 1e-8)
scene_node.translate_local(
move_ax * (translate_amount + translation_noise[0])
+ perp_ax * translation_noise[1]
)
if motion_type == "linear":
rot_noise = multiplier * model.rotation.sample()
else:
# The robot will always turn a little bit. This has to be defined based on the intended actuation
# as otherwise small rotation amounts would be invalid. However, pretty quickly, we'll
# get to the truncation of 3 sigma
trunc = [(-0.95 * np.abs(np.deg2rad(rotate_amount)), None)]
rot_noise = multiplier * model.rotation.sample(trunc)
# Same deal with rotation about + EPS and why we multiply by the sign
rot_noise *= np.sign(rotate_amount + 1e-8)
scene_node.rotate_y_local(mn.Deg(rotate_amount) + mn.Rad(rot_noise))
scene_node.rotation = scene_node.rotation.normalized()
def _update_attitude(self):
""" update agent orientation given angular velocity and delta time"""
state = self.env.sim.get_agent_state(0)
yaw = state.angular_velocity[2] / 3.1415926 * 180
dt = self._dt
_rotate_local_fns = [
hsim.SceneNode.rotate_x_local,
hsim.SceneNode.rotate_y_local,
hsim.SceneNode.rotate_z_local,
]
_rotate_local_fns[self._y_axis](
self.env._sim._sim.agents[0].scene_node, mn.Deg(yaw * dt))
self.env._sim._sim.agents[
0].scene_node.rotation = self.env._sim._sim.agents[
0].scene_node.rotation.normalized()
def _custom_action_impl(
scene_node: habitat_sim.SceneNode,
delta_dist: float, # in metres
delta_dist_angle: float, # in degrees
delta_angle: float # in degrees
):
forward_ax = (
np.array(scene_node.absolute_transformation().rotation_scaling())
@ habitat_sim.geo.FRONT)
move_angle = np.deg2rad(delta_dist_angle)
rotation = habitat_sim.utils.quat_from_angle_axis(move_angle,
habitat_sim.geo.UP)
move_ax = habitat_sim.utils.quat_rotate_vector(rotation, forward_ax)
scene_node.translate_local(move_ax * delta_dist)
scene_node.rotate_local(mn.Deg(delta_angle), habitat_sim.geo.UP)
def test_constrainted(
scene_graph, control_name, control_axis, actuation_amount, actuation_constraint
):
initial_look_angle = mn.Deg(
np.random.uniform(-actuation_constraint, actuation_constraint)
)
rotation_vector = mn.Vector3()
rotation_vector[control_axis] = 1
initial_rotation = mn.Quaternion.rotation(
mn.Rad(initial_look_angle), rotation_vector
)
node = scene_graph.get_root_node().create_child()
node.rotation = initial_rotation
spec = habitat_sim.agent.controls.ActuationSpec(
actuation_amount, actuation_constraint
)
habitat_sim.registry.get_move_fn(control_name)(node, spec)
expected_angle = initial_look_angle + mn.Deg(
-actuation_amount
if control_name in {"look_down", "look_right"}
else actuation_amount
)
if expected_angle > mn.Deg(actuation_constraint):
expected_angle = mn.Deg(actuation_constraint)
elif expected_angle < mn.Deg(-actuation_constraint):
expected_angle = mn.Deg(-actuation_constraint)
final_rotation = node.rotation
look_vector = final_rotation.transform_vector(habitat_sim.geo.FRONT)
if control_axis == 0:
look_angle = mn.Deg(mn.Rad(np.arctan2(look_vector[1], -look_vector[2])))
elif control_axis == 1:
look_angle = -mn.Deg(mn.Rad(np.arctan2(look_vector[0], -look_vector[2])))
assert np.abs(float(expected_angle - look_angle)) < 1e-1
def simulate_with_moving_agent(
sim,
duration=1.0,
agent_vel=np.array([0, 0, 0]),
look_rotation_vel=0.0,
get_frames=True,
):
sensor_node = sim._sensors["rgba_camera"]._sensor_object.object
agent_node = sim.get_agent(0).body.object
# simulate dt seconds at 60Hz to the nearest fixed timestep
time_step = 1.0 / 60.0
rotation_x = mn.Quaternion.rotation(
mn.Deg(look_rotation_vel) * time_step, mn.Vector3(1.0, 0, 0))
print("Simulating " + str(duration) + " world seconds.")
observations = []
start_time = sim.get_world_time()
while sim.get_world_time() < start_time + duration:
# move agent
agent_node.translation += agent_vel * time_step
# rotate sensor
sensor_node.rotation *= rotation_x
# Add user transforms for the agent and sensor. We'll use these later during
# replay playback.
gfx_replay_utils.add_node_user_transform(sim, agent_node, "agent")
gfx_replay_utils.add_node_user_transform(sim, sensor_node, "sensor")
sim.step_physics(time_step)
# save a replay keyframe after every physics step
sim.gfx_replay_manager.save_keyframe()
if get_frames:
observations.append(sim.get_sensor_observations())
return observations
def _noisy_move(scene_node: habitat_sim.SceneNode, forward_amount: float,
spin_amount: float, noise: bool):
forward_ax = (
np.array(scene_node.absolute_transformation().rotation_scaling())
@ habitat_sim.geo.FRONT)
forward_noise = np.random.normal(forward_amount,
FORWARD_NOISE) if noise else 0
forward = np.clip(forward_amount + forward_noise,
np.maximum(forward_amount - FN_CLIP_RATIO * X, 0),
forward_amount + FN_CLIP_RATIO * X)
scene_node.translate_local(forward_ax * forward)
spin_noise = np.random.normal(0,
SPIN_NOISE) * 180 / 3.141592 if noise else 0
spin = np.clip(spin_amount + spin_noise,
spin_amount - SN_CLIP_RATIO * THETA,
spin_amount + SN_CLIP_RATIO * THETA)
#print('forward : {}, spin : {}'.format(forward,spin))
# Rotate about the +y (up) axis
rotation_ax = habitat_sim.geo.UP
scene_node.rotate_local(mn.Deg(spin), rotation_ax)
# Calling normalize is needed after rotating to deal with machine precision errors
scene_node.rotation = scene_node.rotation.normalized()
def main(show_imgs=True, save_imgs=False):
if save_imgs and not os.path.exists(output_path):
os.mkdir(output_path)
# [default scene lighting]
# create the simulator and render flat shaded scene
cfg = make_configuration()
sim = habitat_sim.Simulator(cfg)
agent_transform = place_agent(sim)
get_obs(sim, show_imgs, save_imgs)
# [scene swap shader]
# close the simulator and re-initialize with DEFAULT_LIGHTING_KEY:
sim.close()
cfg = make_configuration()
cfg.sim_cfg.scene_light_setup = habitat_sim.gfx.DEFAULT_LIGHTING_KEY
sim = habitat_sim.Simulator(cfg)
agent_transform = place_agent(sim)
get_obs(sim, show_imgs, save_imgs)
# create and register new light setup:
my_scene_lighting_setup = [
LightInfo(vector=[0.0, 2.0, 0.6, 0.0], model=LightPositionModel.Global)
]
sim.set_light_setup(my_scene_lighting_setup, "my_scene_lighting")
# reconfigure with custom key:
new_cfg = make_configuration()
new_cfg.sim_cfg.scene_light_setup = "my_scene_lighting"
sim.reconfigure(new_cfg)
agent_transform = place_agent(sim)
get_obs(sim, show_imgs, save_imgs)
# [/scene]
# reset to default scene shading
sim.close()
cfg = make_configuration()
sim = habitat_sim.Simulator(cfg)
agent_transform = place_agent(sim) # noqa: F841
# [example 2]
# get the rigid object attributes manager, which manages
# templates used to create objects
obj_template_mgr = sim.get_object_template_manager()
# get the rigid object manager, which provides direct
# access to objects
rigid_obj_mgr = sim.get_rigid_object_manager()
# load some object templates from configuration files
sphere_template_id = obj_template_mgr.load_configs(
str(os.path.join(data_path, "test_assets/objects/sphere")))[0]
chair_template_id = obj_template_mgr.load_configs(
str(os.path.join(data_path, "test_assets/objects/chair")))[0]
# create a sphere and place it at a desired location
obj_1 = rigid_obj_mgr.add_object_by_template_id(sphere_template_id)
obj_1.translation = [3.2, 0.23, 0.03]
get_obs(sim, show_imgs, save_imgs)
# [/example 2]
# [example 3]
# create a custom light setup
my_default_lighting = [
LightInfo(vector=[2.0, 2.0, 1.0, 0.0], model=LightPositionModel.Camera)
]
# overwrite the default DEFAULT_LIGHTING_KEY light setup
sim.set_light_setup(my_default_lighting)
get_obs(sim, show_imgs, save_imgs)
# [/example 3]
# [example 4]
# create a chair and place it at a location with a specified orientation
obj_2 = rigid_obj_mgr.add_object_by_template_id(chair_template_id)
obj_2.rotation = mn.Quaternion.rotation(mn.Deg(-115), mn.Vector3.y_axis())
obj_2.translation = [3.06, 0.47, 1.15]
get_obs(sim, show_imgs, save_imgs)
# [/example 4]
# [example 5]
light_setup_2 = [
LightInfo(
vector=[2.0, 1.5, 5.0, 1.0],
color=[0.0, 100.0, 100.0],
model=LightPositionModel.Global,
)
]
sim.set_light_setup(light_setup_2, "my_custom_lighting")
# [/example 5]
rigid_obj_mgr.remove_all_objects()
# [example 6]
# create and place 2 chairs with custom light setups
chair_1 = rigid_obj_mgr.add_object_by_template_id(
chair_template_id, light_setup_key="my_custom_lighting")
chair_1.rotation = mn.Quaternion.rotation(mn.Deg(-115),
mn.Vector3.y_axis())
chair_1.translation = [3.06, 0.47, 1.15]
chair_2 = rigid_obj_mgr.add_object_by_template_id(
chair_template_id, light_setup_key="my_custom_lighting")
chair_2.rotation = mn.Quaternion.rotation(mn.Deg(50), mn.Vector3.y_axis())
chair_2.translation = [3.45927, 0.47, -0.624958]
get_obs(sim, show_imgs, save_imgs)
# [/example 6]
# [example 7]
existing_light_setup = sim.get_light_setup("my_custom_lighting")
# create a new setup with an additional light
new_light_setup = existing_light_setup + [
LightInfo(
vector=[0.0, 0.0, 1.0, 0.0],
color=[1.6, 1.6, 1.4],
model=LightPositionModel.Camera,
)
]
# register the old setup under a new name
sim.set_light_setup(existing_light_setup, "my_old_custom_lighting")
# [/example 7]
# [example 8]
# register the new setup overwriting the old one
sim.set_light_setup(new_light_setup, "my_custom_lighting")
get_obs(sim, show_imgs, save_imgs)
# [/example 8]
# [example 9]
chair_1.set_light_setup(habitat_sim.gfx.DEFAULT_LIGHTING_KEY)
get_obs(sim, show_imgs, save_imgs)
pass
# @markdown Configure the initial object location (local offset from the agent body node):
# default : offset=np.array([0,2.0,-1.5]), orientation=np.quaternion(1,0,0,0)
offset_x = 0.5 # @param {type:"slider", min:-2, max:2, step:0.1}
offset_y = 1.4 # @param {type:"slider", min:0, max:3.0, step:0.1}
offset_z = -1.5 # @param {type:"slider", min:-3, max:0, step:0.1}
offset = np.array([offset_x, offset_y, offset_z])
# @markdown Configure the initial object orientation via local Euler angle (degrees):
orientation_x = 0 # @param {type:"slider", min:-180, max:180, step:1}
orientation_y = 0 # @param {type:"slider", min:-180, max:180, step:1}
orientation_z = 0 # @param {type:"slider", min:-180, max:180, step:1}
# compose the rotations
rotation_x = mn.Quaternion.rotation(mn.Deg(orientation_x), mn.Vector3(1.0, 0, 0))
rotation_y = mn.Quaternion.rotation(mn.Deg(orientation_y), mn.Vector3(1.0, 0, 0))
rotation_z = mn.Quaternion.rotation(mn.Deg(orientation_z), mn.Vector3(1.0, 0, 0))
orientation = rotation_z * rotation_y * rotation_x
# Add object instantiated by desired template using template handle
obj_id_1 = sim.add_object_by_handle(obj_template_handle)
# @markdown Note: agent local coordinate system is Y up and -Z forward.
# Move object to be in front of the agent
set_object_state_from_agent(sim, obj_id_1, offset=offset, orientation=orientation)
# display a still frame of the scene after the object is added if RGB sensor is enabled
observations = sim.get_sensor_observations()
if display and sim_settings["color_sensor_1st_person"]:
display_sample(observations["color_sensor_1st_person"])
def perform_add_blank_template_test(attr_mgr, valid_render_handle=None):
# get size of template library
orig_num_templates = attr_mgr.get_num_templates()
# add new blank template
new_template_handle = "new template"
# create new default template, do not register it
new_template0 = attr_mgr.create_new_template(new_template_handle, False)
# change new template field
new_template0.set("test_key", "new_template_test")
# give new template valid render asset handle, otherwise registration might fail
if valid_render_handle is not None:
new_template0.render_asset_handle = valid_render_handle
# add new template
attr_mgr.register_template(new_template0, new_template_handle)
# get new size of library after remove and verify not same as original
curr_num_templates = attr_mgr.get_num_templates()
assert curr_num_templates > orig_num_templates
# verify new template added properly
new_template1 = attr_mgr.get_template_by_handle(new_template_handle)
# verify template 0 and template 1 are copies of the same template
assert new_template0.handle == new_template1.handle
assert new_template0.template_id == new_template1.template_id
assert new_template0.get_string("test_key") == new_template1.get_string(
"test_key")
# remove newly added default template
new_template2 = attr_mgr.remove_template_by_handle(new_template_handle)
# verify added template was one removed
assert new_template0.handle == new_template2.handle
assert new_template0.template_id == new_template2.template_id
assert new_template0.get_string("test_key") == new_template2.get_string(
"test_key")
# test addition of user-configurations and verify values
# create new template, do not register it
new_template_usr = attr_mgr.create_new_template(new_template_handle, False)
# give new template valid render asset handle, otherwise registration might fail
if valid_render_handle is not None:
new_template_usr.render_asset_handle = valid_render_handle
usr_template_handle = "new_usr_cfg_handle"
new_template_usr.handle = usr_template_handle
# get user configs and set key
new_template_usr.set_user_config_val("my_custom_key0", "my_custom_string")
assert (new_template_usr.get_user_config_string("my_custom_key0") ==
"my_custom_string")
new_template_usr.set_user_config_val("my_custom_key1", True)
assert new_template_usr.get_user_config_bool("my_custom_key1") == True
new_template_usr.set_user_config_val("my_custom_key2", 10)
assert new_template_usr.get_user_config_int("my_custom_key2") == 10
new_template_usr.set_user_config_val("my_custom_key3", 5.8)
assert new_template_usr.get_user_config_double("my_custom_key3") == 5.8
new_template_usr.set_user_config_val("my_custom_key4",
mn.Vector3(1.0, -2.8, 3.0))
assert new_template_usr.get_user_config_vec3(
"my_custom_key4") == mn.Vector3(1.0, -2.8, 3.0)
quat_val = mn.Quaternion.rotation(mn.Deg(-115), mn.Vector3.y_axis())
new_template_usr.set_user_config_val("my_custom_key5", quat_val)
assert new_template_usr.num_user_configs == 6
# add new template - should use template-specified name as handle
usr_tmplt_ID = attr_mgr.register_template(new_template_usr, "")
assert usr_tmplt_ID != -1
#
reg_template_usr = attr_mgr.get_template_by_handle(usr_template_handle)
assert reg_template_usr != None
assert reg_template_usr.num_user_configs == new_template_usr.num_user_configs
assert (reg_template_usr.get_user_config_string("my_custom_key0") ==
"my_custom_string")
assert reg_template_usr.get_user_config_bool("my_custom_key1") == True
assert reg_template_usr.get_user_config_int("my_custom_key2") == 10
assert reg_template_usr.get_user_config_double("my_custom_key3") == 5.8
assert reg_template_usr.get_user_config_vec3(
"my_custom_key4") == mn.Vector3(1.0, -2.8, 3.0)
assert reg_template_usr.get_user_config_quat("my_custom_key5") == quat_val
rmv_template_usr = attr_mgr.remove_template_by_handle(usr_template_handle)
assert rmv_template_usr != None
assert new_template_usr.handle == rmv_template_usr.handle
assert new_template_usr.template_id == rmv_template_usr.template_id
# get new size of library after remove and verify same as original
curr_num_templates = attr_mgr.get_num_templates()
assert curr_num_templates == orig_num_templates
def main(show_imgs=True, save_imgs=False):
if save_imgs and not os.path.exists(output_path):
os.mkdir(output_path)
# [default scene lighting]
# create the simulator and render flat shaded scene
cfg = make_configuration()
sim = habitat_sim.Simulator(cfg)
agent_transform = place_agent(sim)
get_obs(sim, show_imgs, save_imgs)
# [scene swap shader]
# close the simulator and re-initialize with DEFAULT_LIGHTING_KEY:
sim.close()
cfg = make_configuration()
cfg.sim_cfg.scene_light_setup = habitat_sim.gfx.DEFAULT_LIGHTING_KEY
sim = habitat_sim.Simulator(cfg)
agent_transform = place_agent(sim)
get_obs(sim, show_imgs, save_imgs)
# create and register new light setup:
my_scene_lighting_setup = [
LightInfo(vector=[0.0, 2.0, 0.6, 0.0], model=LightPositionModel.GLOBAL)
]
sim.set_light_setup(my_scene_lighting_setup, "my_scene_lighting")
# reconfigure with custom key:
new_cfg = make_configuration()
new_cfg.sim_cfg.scene_light_setup = "my_scene_lighting"
sim.reconfigure(new_cfg)
agent_transform = place_agent(sim)
get_obs(sim, show_imgs, save_imgs)
# [/scene]
# reset to default scene shading
sim.close()
cfg = make_configuration()
sim = habitat_sim.Simulator(cfg)
agent_transform = place_agent(sim) # noqa: F841
# [example 2]
# get the physics object attributes manager
obj_templates_mgr = sim.get_object_template_manager()
# load some object templates from configuration files
sphere_template_id = obj_templates_mgr.load_configs(
str(os.path.join(data_path, "test_assets/objects/sphere")))[0]
chair_template_id = obj_templates_mgr.load_configs(
str(os.path.join(data_path, "test_assets/objects/chair")))[0]
id_1 = sim.add_object(sphere_template_id)
sim.set_translation([3.2, 0.23, 0.03], id_1)
get_obs(sim, show_imgs, save_imgs)
# [/example 2]
# [example 3]
# create a custom light setup
my_default_lighting = [
LightInfo(vector=[2.0, 2.0, 1.0, 0.0], model=LightPositionModel.CAMERA)
]
# overwrite the default DEFAULT_LIGHTING_KEY light setup
sim.set_light_setup(my_default_lighting)
get_obs(sim, show_imgs, save_imgs)
# [/example 3]
# [example 4]
id_2 = sim.add_object(chair_template_id)
sim.set_rotation(mn.Quaternion.rotation(mn.Deg(-115), mn.Vector3.y_axis()),
id_2)
sim.set_translation([3.06, 0.47, 1.15], id_2)
get_obs(sim, show_imgs, save_imgs)
# [/example 4]
# [example 5]
light_setup_2 = [
LightInfo(
vector=[2.0, 1.5, 5.0, 1.0],
color=[0.0, 100.0, 100.0],
model=LightPositionModel.GLOBAL,
)
]
sim.set_light_setup(light_setup_2, "my_custom_lighting")
# [/example 5]
remove_all_objects(sim)
# [example 6]
id_1 = sim.add_object(chair_template_id,
light_setup_key="my_custom_lighting")
sim.set_rotation(mn.Quaternion.rotation(mn.Deg(-115), mn.Vector3.y_axis()),
id_1)
sim.set_translation([3.06, 0.47, 1.15], id_1)
id_2 = sim.add_object(chair_template_id,
light_setup_key="my_custom_lighting")
sim.set_rotation(mn.Quaternion.rotation(mn.Deg(50), mn.Vector3.y_axis()),
id_2)
sim.set_translation([3.45927, 0.47, -0.624958], id_2)
get_obs(sim, show_imgs, save_imgs)
# [/example 6]
# [example 7]
existing_light_setup = sim.get_light_setup("my_custom_lighting")
# create a new setup with an additional light
new_light_setup = existing_light_setup + [
LightInfo(
vector=[0.0, 0.0, 1.0, 0.0],
color=[1.6, 1.6, 1.4],
model=LightPositionModel.CAMERA,
)
]
# register the old setup under a new name
sim.set_light_setup(existing_light_setup, "my_old_custom_lighting")
# [/example 7]
# [example 8]
# register the new setup overwriting the old one
sim.set_light_setup(new_light_setup, "my_custom_lighting")
get_obs(sim, show_imgs, save_imgs)
# [/example 8]
# [example 9]
sim.set_object_light_setup(id_1, habitat_sim.gfx.DEFAULT_LIGHTING_KEY)
get_obs(sim, show_imgs, save_imgs)
def main(show_imgs=True, save_imgs=False):
if save_imgs:
if not os.path.exists(output_path):
os.mkdir(output_path)
# [semantic id]
# create the simulator and render flat shaded scene
cfg = make_configuration(scene_file="NONE")
sim = habitat_sim.Simulator(cfg)
test_scenes = [
"data/scene_datasets/habitat-test-scenes/apartment_1.glb",
"data/scene_datasets/habitat-test-scenes/van-gogh-room.glb",
]
for scene in test_scenes:
# reconfigure the simulator with a new scene asset
cfg = make_configuration(scene_file=scene)
sim.reconfigure(cfg)
agent_transform = place_agent(sim) # noqa: F841
get_obs(sim, show_imgs, save_imgs)
# get the physics object attributes manager
obj_templates_mgr = sim.get_object_template_manager()
# load some chair object template from configuration file
chair_template_id = obj_templates_mgr.load_object_configs(
str(os.path.join(data_path, "test_assets/objects/chair")))[0]
# add 2 chairs with default semanticId == 0 and arrange them
chair_ids = []
chair_ids.append(sim.add_object(chair_template_id))
chair_ids.append(sim.add_object(chair_template_id))
sim.set_rotation(
mn.Quaternion.rotation(mn.Deg(-115), mn.Vector3.y_axis()),
chair_ids[0])
sim.set_translation([2.0, 0.47, 0.9], chair_ids[0])
sim.set_translation([2.9, 0.47, 0.0], chair_ids[1])
get_obs(sim, show_imgs, save_imgs)
# set the semanticId for both chairs
sim.set_object_semantic_id(2, chair_ids[0])
sim.set_object_semantic_id(2, chair_ids[1])
get_obs(sim, show_imgs, save_imgs)
# set the semanticId for one chair
sim.set_object_semantic_id(1, chair_ids[1])
get_obs(sim, show_imgs, save_imgs)
# add a box with default semanticId configured in the template
box_template = habitat_sim.attributes.PhysicsObjectAttributes()
box_template.render_asset_handle = str(
os.path.join(data_path, "test_assets/objects/transform_box.glb"))
box_template.scale = np.array([0.2, 0.2, 0.2])
# set the default semantic id for this object template
box_template.semantic_id = 10
obj_templates_mgr = sim.get_object_template_manager()
box_template_id = obj_templates_mgr.register_template(
box_template, "box")
box_id = sim.add_object(box_template_id)
sim.set_translation([3.5, 0.47, 0.9], box_id)
sim.set_rotation(
mn.Quaternion.rotation(mn.Deg(-30), mn.Vector3.y_axis()), box_id)
get_obs(sim, show_imgs, save_imgs)
# set semantic id for specific SceneNode components of the box object
box_visual_nodes = sim.get_object_visual_scene_nodes(box_id)
box_visual_nodes[6].semantic_id = 3
box_visual_nodes[7].semantic_id = 4
get_obs(sim, show_imgs, save_imgs)
# load some object templates from configuration files
sphere_template_id = obj_templates_mgr.load_configs(
str(os.path.join(data_path, "test_assets/objects/sphere")))[0]
# add a sphere to the scene, returns the object
sphere_obj = rigid_obj_mgr.add_object_by_template_id(sphere_template_id)
# set user-defined configuration values
user_attributes_dict = {
"obj_val_0": "This is a sphere object named " + sphere_obj.handle,
"obj_val_1": 17,
"obj_val_2": False,
"obj_val_3": 19.7,
"obj_val_4": [2.50, 0.0, 0.2],
"obj_val_5": mn.Quaternion.rotation(mn.Deg(90.0), [-1.0, 0.0, 0.0]),
}
for k, v in user_attributes_dict.items():
sphere_obj.user_attributes.set(k, v)
for k, _ in user_attributes_dict.items():
print("Sphere Object user attribute : {} : {}".format(
k, sphere_obj.user_attributes.get(k)))
# [/object_user_configurations]
rigid_obj_mgr.remove_all_objects()
# %%
# [dynamic_control]
observations = []
def _rotate_local(scene_node: hsim.SceneNode, theta: float, axis: int):
_rotate_local_fns[axis](scene_node, mn.Deg(theta))
scene_node.rotation = scene_node.rotation.normalized()
# [dynamic_control]
observations = []
obj_templates_mgr.load_configs(str(os.path.join(data_path, "objects")))
# search for an object template by key sub-string
cheezit_template_handle = obj_templates_mgr.get_template_handles(
"data/objects/cheezit")[0]
# build multiple object initial positions
box_positions = [
[2.39, -0.37, 0.0],
[2.39, -0.64, 0.0],
[2.39, -0.91, 0.0],
[2.39, -0.64, -0.22],
[2.39, -0.64, 0.22],
]
box_orientation = mn.Quaternion.rotation(mn.Deg(90.0), [-1.0, 0.0, 0.0])
# instance and place the boxes
boxes = []
for b in range(len(box_positions)):
boxes.append(
rigid_obj_mgr.add_object_by_handle(cheezit_template_handle))
boxes[b].translation = box_positions[b]
boxes[b].rotation = box_orientation
# get the object's initialization attributes (all boxes initialized with same mass)
object_init_template = boxes[0].creation_attributes
# anti-gravity force f=m(-g)
anti_grav_force = -1.0 * sim.get_gravity() * object_init_template.mass
# throw a sphere at the boxes from the agent position
sphere_template = obj_templates_mgr.get_template_by_ID(sphere_template_id)
def reconfigure_scene(env, scene_path):
"""This function takes a habitat scene and adds relevant changes to the
scene that make it useful for locobot assistant.
For example, it sets initial positions to be deterministic, and it
adds some humans to the environment a reasonable places
"""
sim = env._robot.base.sim
# these are useful to know to understand co-ordinate normalization
# and where to place objects to be within bounds. They change wildly
# from scene to scene
print("scene bounds: {}".format(sim.pathfinder.get_bounds()))
agent = sim.get_agent(0)
# keep the initial rotation consistent across runs
old_agent_state = agent.get_state()
new_agent_state = habitat_sim.AgentState()
new_agent_state.position = old_agent_state.position
new_agent_state.rotation = np.quaternion(1.0, 0.0, 0.0, 0.0)
agent.set_state(new_agent_state)
env._robot.base.init_state = agent.get_state()
env.initial_rotation = new_agent_state.rotation
###########################
# scene-specific additions
###########################
scene_name = os.path.basename(scene_path).split(".")[0]
if scene_name == "mesh_semantic":
# this must be Replica Dataset
scene_name = os.path.basename(
os.path.dirname(os.path.dirname(scene_path)))
supported_scenes = ["skokloster-castle", "van-gogh-room", "apartment_0"]
if scene_name not in supported_scenes:
print("Scene {} not in supported scenes, so skipping adding objects".
format(scene_name))
return
assets_path = os.path.abspath(
os.path.join(os.path.dirname(__file__), "../test/test_assets"))
if hasattr(sim, 'get_object_template_manager'):
load_object_configs = sim.get_object_template_manager(
).load_object_configs
else:
load_object_configs = sim.load_object_configs
human_male_template_id = load_object_configs(
os.path.join(assets_path, "human_male"))[0]
human_female_template_id = load_object_configs(
os.path.join(assets_path, "human_female"))[0]
if scene_name == "apartment_0":
id_male = sim.add_object(human_male_template_id)
id_female = sim.add_object(human_female_template_id)
print("id_male, id_female: {} {}".format(id_male, id_female))
sim.set_translation([1.2, -0.81, 0.3],
id_female) # apartment_0, female
sim.set_translation([1.2, -0.75, -0.3], id_male) # apartment_0, male
rot = mn.Quaternion.rotation(mn.Deg(-75), mn.Vector3.y_axis())
sim.set_rotation(rot, id_male) # apartment_0
sim.set_rotation(rot, id_female) # apartment_0
elif scene_name == "skokloster-castle":
id_female = sim.add_object(human_female_template_id)
print("id_female: {}".format(id_female))
sim.set_translation([2.0, 3.0, 15.00], id_female) # skokloster castle
elif scene_name == "van-gogh-room":
id_female = sim.add_object(human_female_template_id)
print("id_female: {}".format(id_female))
sim.set_translation([1.0, 0.84, 0.00], id_female) # van-gogh-room
# make the objects STATIC so that collisions work
for obj in [id_male, id_female]:
sim.set_object_motion_type(habitat_sim.physics.MotionType.STATIC, obj)
navmesh_settings = habitat_sim.NavMeshSettings()
navmesh_settings.set_defaults()
sim.recompute_navmesh(sim.pathfinder,
navmesh_settings,
include_static_objects=True)
else:
sim.reconfigure(cfg)
agent_state = habitat_sim.AgentState()
agent = sim.initialize_agent(0, agent_state)
# %% [markdown]
# ## Initial placement for agent and sensor
# %%
agent_node = sim.get_agent(0).body.object
sensor_node = sim._sensors["rgba_camera"]._sensor_object.object
# initial agent transform
agent_node.translation = [-0.15, -1.5, 1.0]
agent_node.rotation = mn.Quaternion.rotation(mn.Deg(-75),
mn.Vector3(0.0, 1.0, 0))
# initial sensor local transform (relative to agent)
sensor_node.translation = [0.0, 0.6, 0.0]
sensor_node.rotation = mn.Quaternion.rotation(mn.Deg(-15),
mn.Vector3(1.0, 0.0, 0))
# %% [markdown]
# ## Start an episode by moving an agent in the scene and capturing observations.
# %%
observations = []
# simulate with empty scene
observations += simulate_with_moving_agent(
