def load_objs(name_obj_dat, sim, obj_ids, auto_sleep=True):
"""
- name_obj_dat: List[(str, List[
transformation as a 4x4 list of lists of floats,
int representing the motion type
])
"""
static_obj_ids = []
for i, (name, obj_dat) in enumerate(name_obj_dat):
if len(obj_ids) == 0:
obj_id = add_obj(name, sim)
else:
obj_id = obj_ids[i]
trans = obj_dat[0]
obj_type = obj_dat[1]
use_trans = mn.Matrix4(trans)
sim.set_transformation(use_trans, obj_id)
sim.set_linear_velocity(mn.Vector3(0, 0, 0), obj_id)
sim.set_angular_velocity(mn.Vector3(0, 0, 0), obj_id)
sim.set_object_motion_type(MotionType(obj_type), obj_id)
static_obj_ids.append(obj_id)
if len(obj_ids) != 0:
return obj_ids
return static_obj_ids
def test_raycast():
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"
# enable the physics simulator
cfg_settings["enable_physics"] = True
# 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()
if (sim.get_physics_simulation_library() !=
habitat_sim.physics.PhysicsSimulationLibrary.NONE):
# only test this if we have a physics simulator and therefore a collision world
test_ray_1 = habitat_sim.geo.Ray()
test_ray_1.direction = mn.Vector3(1.0, 0, 0)
raycast_results = sim.cast_ray(test_ray_1)
assert raycast_results.ray.direction == test_ray_1.direction
assert raycast_results.has_hits()
assert len(raycast_results.hits) == 1
assert np.allclose(raycast_results.hits[0].point,
np.array([6.83063, 0, 0]),
atol=0.07)
assert np.allclose(
raycast_results.hits[0].normal,
np.array([-0.999587, 0.0222882, -0.0181424]),
atol=0.07,
)
assert abs(raycast_results.hits[0].ray_distance - 6.831) < 0.001
assert raycast_results.hits[0].object_id == -1
# add a primitive object to the world
cube_prim_handle = obj_mgr.get_template_handles("cube")[0]
cube_obj_id = sim.add_object_by_handle(cube_prim_handle)
sim.set_translation(mn.Vector3(3.0, 0, 0), cube_obj_id)
raycast_results = sim.cast_ray(test_ray_1)
assert raycast_results.has_hits()
assert len(raycast_results.hits) == 2
assert np.allclose(raycast_results.hits[0].point,
np.array([2.89355, 0, 0]),
atol=0.07)
assert np.allclose(
raycast_results.hits[0].normal,
np.array([-0.998961, -0.0322245, -0.0322245]),
atol=0.07,
)
assert abs(raycast_results.hits[0].ray_distance - 2.8935) < 0.001
assert raycast_results.hits[0].object_id == 0
def test_scene_bounding_boxes(sim):
cfg_settings = examples.settings.default_sim_settings.copy()
cfg_settings["scene"] = "data/scene_datasets/habitat-test-scenes/van-gogh-room.glb"
hab_cfg = examples.settings.make_cfg(cfg_settings)
sim.reconfigure(hab_cfg)
scene_graph = sim._sim.get_active_scene_graph()
root_node = scene_graph.get_root_node()
root_node.compute_cumulative_bb()
scene_bb = root_node.cumulative_bb
ground_truth = mn.Range3D.from_size(
mn.Vector3(-0.775869, -0.0233012, -1.6706), mn.Vector3(6.76937, 3.86304, 3.5359)
)
assert ground_truth == scene_bb
def visualize_position(self, position, viz_id=None, r=0.05):
"""Adds the sphere object to the specified position for visualization purpose."""
if viz_id is None:
obj_mgr = self.get_object_template_manager()
template = obj_mgr.get_template_by_handle(
obj_mgr.get_template_handles("sphere")[0])
template.scale = mn.Vector3(r, r, r)
new_template_handle = obj_mgr.register_template(
template, "ball_new_viz")
viz_id = self.add_object(new_template_handle)
make_render_only(viz_id, self)
self.set_translation(mn.Vector3(*position), viz_id)
return viz_id
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 update(self) -> None:
"""Updates the camera transformations and performs necessary checks on
joint limits and sleep states.
"""
agent_node = self._sim._default_agent.scene_node
inv_T = agent_node.transformation.inverted()
for cam_prefix, sensor_names in self._cameras.items():
for sensor_name in sensor_names:
sens_obj = self._sim._sensors[sensor_name]._sensor_object
cam_info = self.params.cameras[cam_prefix]
if cam_info.attached_link_id == -1:
link_trans = self.sim_obj.transformation
else:
link_trans = self.sim_obj.get_link_scene_node(
self.params.ee_link
).transformation
cam_transform = mn.Matrix4.look_at(
cam_info.cam_offset_pos,
cam_info.cam_look_at_pos,
mn.Vector3(0, 1, 0),
)
cam_transform = link_trans @ cam_transform @ cam_info.relative_transform
cam_transform = inv_T @ cam_transform
sens_obj.node.transformation = cam_transform
# Guard against out of limit joints
# TODO: should auto clamping be enabled instead? How often should we clamp?
if self._limit_robo_joints:
self.sim_obj.clamp_joint_limits()
self.sim_obj.awake = True
def add_obj(name, sim):
if "/BOX_" in name:
size_parts = name.split("/BOX_")[-1]
size_parts = size_parts.split("_")
if len(size_parts) == 1:
size_x = size_y = size_z = float(size_parts[0])
else:
size_x = float(size_parts[0])
size_y = float(size_parts[1])
size_z = float(size_parts[2])
obj_mgr = sim.get_object_template_manager()
template_handle = obj_mgr.get_template_handles("cube")[0]
template = obj_mgr.get_template_by_handle(template_handle)
template.scale = mn.Vector3(size_x, size_y, size_z)
template.requires_lighting = True
new_template_handle = obj_mgr.register_template(template, "box_new")
obj_id = sim.add_object(new_template_handle)
sim.set_object_motion_type(MotionType.DYNAMIC, obj_id)
return obj_id
PROP_FILE_END = ".object_config.json"
use_name = name + PROP_FILE_END
obj_id = sim.add_object_by_handle(use_name)
return obj_id
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 draw_sphere(self, r, template_name="ball_new"):
obj_mgr = self.get_object_template_manager()
template_handle = obj_mgr.get_template_handles("sphere")[0]
template = obj_mgr.get_template_by_handle(template_handle)
template.scale = mn.Vector3(r, r, r)
new_template_handle = obj_mgr.register_template(template, "ball_new")
obj_id = self.add_object(new_template_handle)
self.set_object_motion_type(MotionType.KINEMATIC, obj_id)
return obj_id
def test_scene_bounding_boxes():
cfg_settings = examples.settings.default_sim_settings.copy()
cfg_settings["scene"] = "data/scene_datasets/habitat-test-scenes/van-gogh-room.glb"
hab_cfg = examples.settings.make_cfg(cfg_settings)
hab_cfg_mm = examples.settings.make_cfg(cfg_settings)
mm = habitat_sim.metadata.MetadataMediator(hab_cfg.sim_cfg)
hab_cfg_mm.metadata_mediator = mm
test_list = [hab_cfg, hab_cfg_mm]
for ctor_arg in test_list:
with habitat_sim.Simulator(ctor_arg) as sim:
scene_graph = sim.get_active_scene_graph()
root_node = scene_graph.get_root_node()
root_node.compute_cumulative_bb()
scene_bb = root_node.cumulative_bb
ground_truth = mn.Range3D.from_size(
mn.Vector3(-0.775869, -0.0233012, -1.6706),
mn.Vector3(6.76937, 3.86304, 3.5359),
)
assert ground_truth == scene_bb
def update(self) -> None:
"""Updates the camera transformations and performs necessary checks on
joint limits and sleep states.
"""
agent_node = self._sim._default_agent.scene_node
inv_T = agent_node.transformation.inverted()
# Update the cameras
for sensor_name in self._head_sensor_names:
sens_obj = self._sim._sensors[sensor_name]._sensor_object
look_at = self.sim_obj.transformation.transform_point(
self.params.head_cam_look_pos)
cam_pos = self.sim_obj.transformation.transform_point(
self.params.head_cam_offset_pos)
head_T = mn.Matrix4.look_at(cam_pos, look_at, mn.Vector3(0, 1, 0))
sens_obj.node.transformation = inv_T @ head_T
for sensor_name in self._third_sensor_names:
sens_obj = self._sim._sensors[sensor_name]._sensor_object
look_at = self.sim_obj.transformation.transform_point(
self.params.third_cam_look_pos)
cam_pos = self.sim_obj.transformation.transform_point(
self.params.third_cam_offset_pos)
third_T = mn.Matrix4.look_at(cam_pos, look_at, mn.Vector3(0, 1, 0))
sens_obj.node.transformation = inv_T @ third_T
for sensor_name in self._arm_sensor_names:
sens_obj = self._sim._sensors[sensor_name]._sensor_object
arm_T = self._get_arm_cam_transform()
sens_obj.node.transformation = inv_T @ arm_T
# Guard against out of limit joints
# TODO: should auto clamping be enabled instead? How often should we clamp?
if self._limit_robo_joints:
self.sim_obj.clamp_joint_limits()
self.sim_obj.awake = True
def _get_target_trans(self):
"""
This is how the target transforms should be accessed since
multiprocessing does not allow pickling.
"""
# Preprocess the ep_info making necessary datatype conversions.
target_trans = []
for i in range(len(self.ep_info["targets"])):
targ_idx, trans = self.ep_info["targets"][i]
if len(trans) == 3:
# Legacy position only format.
trans = mn.Matrix4.translation(mn.Vector3(*trans))
else:
trans = mn.Matrix4(trans)
target_trans.append((targ_idx, trans))
return target_trans
def place_robot_from_agent(
sim: habitat_sim.Simulator,
robot: habitat_sim._ext.habitat_sim_bindings.ManagedBulletArticulatedObject,
local_base_pos: list,
orientation_vector: list,
angle_correction: float,
) -> None:
"""Moves robot to reasonable transformation relative to agent."""
local_base_pos = np.array(local_base_pos)
# place the robot root state relative to the agent
agent_transform = sim.agents[0].scene_node.transformation_matrix()
base_transform = mn.Matrix4.rotation(
mn.Rad(angle_correction), mn.Vector3(*orientation_vector)
)
base_transform.translation = agent_transform.transform_point(local_base_pos)
robot.transformation = base_transform
def snap_to_obj(self, snap_obj_id: int, force: bool = True):
"""
:param snap_obj_id: Simulator object index.
:force: Will transform the object to be in the robot's grasp, even if
the object is already in the grasped state.
"""
if len(self._snap_constraints) != 0:
# We were already grabbing something else.
raise ValueError(
f"Tried snapping to {snap_obj_id} when already snapped to {self._snapped_obj_id}"
)
if force:
# Set the transformation to be in the robot's hand already.
self._sim.set_transformation(
self._sim.robot.ee_transform, snap_obj_id
)
if snap_obj_id == self._snapped_obj_id:
# Already grasping this object.
return
self._snapped_obj_id = snap_obj_id
# Set collision group to GraspedObject so that it doesn't collide
# with the links of the robot.
self.snap_rigid_obj.override_collision_group(
CollisionGroups.UserGroup7
)
def create_hold_constraint(pivot_in_link, pivot_in_obj):
c = RigidConstraintSettings()
c.object_id_a = self._sim.robot.get_robot_sim_id()
c.link_id_a = self._sim.robot.ee_link_id
c.object_id_b = self._snapped_obj_id
c.pivot_a = pivot_in_link
c.pivot_b = pivot_in_obj
c.max_impulse = self._config.GRASP_IMPULSE
return self._sim.create_rigid_constraint(c)
self._snap_constraints = [
create_hold_constraint(mn.Vector3(0.1, 0, 0), mn.Vector3(0, 0, 0)),
create_hold_constraint(
mn.Vector3(0.0, 0, 0), mn.Vector3(-0.1, 0, 0)
),
create_hold_constraint(
mn.Vector3(0.1, 0.0, 0.1), mn.Vector3(0.0, 0.0, 0.1)
),
]
if any((x == -1 for x in self._snap_constraints)):
raise ValueError("Created bad constraint")
def convert_fn(obj_dat):
fname = "/".join(obj_dat[0].split("/")[-2:])
if ".urdf" in fname:
obj_dat[0] = osp.join("data/replica_cad/urdf", fname)
else:
obj_dat[0] = obj_dat[0].replace("data/objects/",
"data/objects/ycb/")
if (len(obj_dat) == 2 and len(obj_dat[1]) == 4
and np.array(obj_dat[1]).shape == (4, 4)):
# Specifies the full transformation, no object type
return (obj_dat[0], (obj_dat[1], int(MotionType.DYNAMIC)))
elif len(obj_dat) == 2 and len(obj_dat[1]) == 3:
# Specifies XYZ, no object type
trans = mn.Matrix4.translation(mn.Vector3(obj_dat[1]))
return (obj_dat[0], (trans, int(MotionType.DYNAMIC)))
else:
# Specifies the full transformation and the object type
return (obj_dat[0], obj_dat[1])
def test_scene_attributes_managers(sim):
cfg_settings = examples.settings.default_sim_settings.copy()
cfg_settings["scene"] = "data/scene_datasets/habitat-test-scenes/van-gogh-room.glb"
cfg_settings["enable_physics"] = True
hab_cfg = examples.settings.make_cfg(cfg_settings)
sim.reconfigure(hab_cfg)
scene_name = cfg_settings["scene"]
# get attribute managers
scene_mgr = sim.get_scene_template_manager()
# perform general tests for this attributes manager
template0, _ = perform_general_tests(scene_mgr, scene_name)
# verify gravity in template is as expected
assert template0.gravity == mn.Vector3(0.0, -9.8, 0.0)
# verify creating new template
perform_add_blank_template_test(scene_mgr, template0.render_asset_handle)
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 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 set_object_in_front_of_agent(self, sim, obj_id, z_offset=-1.5):
r"""
Adds an object in front of the agent at some distance.
"""
#print("Agent : ")
#print(self.get_agent(0))
#agent_transform = sim.agents[0].scene_node.transformation_matrix()
agent_transform = self.get_agent(0).scene_node.transformation_matrix()
obj_translation = agent_transform.transform_point(
np.array([0, 0, z_offset]))
sim.set_translation(obj_translation, obj_id)
obj_node = sim.get_object_scene_node(obj_id)
xform_bb = habitat_sim.geo.get_transformed_bb(obj_node.cumulative_bb,
obj_node.transformation)
# also account for collision margin of the scene
scene_collision_margin = 0.04
y_translation = mn.Vector3(
0,
xform_bb.size_y() / 2.0 + scene_collision_margin, 0)
sim.set_translation(y_translation + sim.get_translation(obj_id),
obj_id)
def update(env, state, a, delta, vel_control, time_step):
dt = time_step
# angle = quaternion.as_euler_angles(env.habitat_env._sim.get_agent_state().rotation)
agent_forward = quat_to_magnum(
env.habitat_env._sim.get_agent_state().rotation).transform_vector(
mn.Vector3(0, 0, -1.0))
# print("linear velocity", state.v)
# print("angular velocity", state.yaw)
state.x = env.habitat_env._sim.get_agent_state().position[0]
state.y = -env.habitat_env._sim.get_agent_state().position[2]
# state.yaw = -angle[1]
state.yaw = math.atan2(agent_forward[0], agent_forward[2])
vel_control.linear_velocity = np.array([0, 0, -a])
# local up is y
vel_control.angular_velocity = np.array([0, delta, 0])
print("lin", a)
print("vel", delta)
obs, reward, done, info = env.step(vel_control)
return obs, info, state
def place_viz_objs(name_trans, sim, obj_ids):
viz_obj_ids = []
for i, (_, assoc_obj_idx, trans) in enumerate(name_trans):
if len(obj_ids) == 0:
obj_bb = get_aabb(assoc_obj_idx, sim, False)
obj_mgr = sim.get_object_template_manager()
template = obj_mgr.get_template_by_handle(
obj_mgr.get_template_handles("cubeWireframe")[0]
)
template.scale = (
mn.Vector3(obj_bb.size_x(), obj_bb.size_y(), obj_bb.size_z())
/ 2.0
)
new_template_handle = obj_mgr.register_template(
template, "new_obj_viz"
)
viz_obj_id = sim.add_object(new_template_handle)
else:
viz_obj_id = obj_ids[i]
make_render_only(viz_obj_id, sim)
sim.set_transformation(trans, viz_obj_id)
viz_obj_ids.append(viz_obj_id)
return viz_obj_ids
