def _get_observation(self):
"""
Returns an OrderedDict containing observations [(name_string, np.array), ...].
Important keys:
robot-state: contains robot-centric information.
object-state: requires @self.use_object_obs to be True.
contains object-centric information.
image: requires @self.use_camera_obs to be True.
contains a rendered frame from the simulation.
depth: requires @self.use_camera_obs and @self.camera_depth to be True.
contains a rendered depth map from the simulation
"""
di = super()._get_observation()
# camera observations
if self.use_camera_obs:
camera_obs = self.sim.render(
camera_name=self.camera_name,
width=self.camera_width,
height=self.camera_height,
depth=self.camera_depth,
)
if self.camera_depth:
di["image"], di["depth"] = camera_obs
else:
di["image"] = camera_obs
# low-level object information
if self.use_object_obs:
# position and rotation of cylinder and hole
hole_pos = np.array(self.sim.data.body_xpos[self.hole_body_id])
hole_quat = T.convert_quat(
self.sim.data.body_xquat[self.hole_body_id], to="xyzw")
di["hole_pos"] = hole_pos
di["hole_quat"] = hole_quat
cyl_pos = np.array(self.sim.data.body_xpos[self.cyl_body_id])
cyl_quat = T.convert_quat(
self.sim.data.body_xquat[self.cyl_body_id], to="xyzw")
di["cyl_to_hole"] = cyl_pos - hole_pos
di["cyl_quat"] = cyl_quat
# Relative orientation parameters
t, d, cos = self._compute_orientation()
di["angle"] = cos
di["t"] = t
di["d"] = d
di["object-state"] = np.concatenate([
di["hole_pos"],
di["hole_quat"],
di["cyl_to_hole"],
di["cyl_quat"],
[di["angle"]],
[di["t"]],
[di["d"]],
])
return di
def _get_observation(self):
"""
Returns an OrderedDict containing observations [(name_string, np.array), ...].
Important keys:
robot-state: contains robot-centric information.
object-state: requires @self.use_object_obs to be True.
contains object-centric information.
image: requires @self.use_camera_obs to be True.
contains a rendered frame from the simulation.
depth: requires @self.use_camera_obs and @self.camera_depth to be True.
contains a rendered depth map from the simulation
"""
di = super()._get_observation()
# camera observations
if self.use_camera_obs:
camera_obs = self.sim.render(
camera_name=self.camera_name,
width=self.camera_width,
height=self.camera_height,
depth=self.camera_depth,
)
if self.camera_depth:
di["image"], di["depth"] = camera_obs
else:
di["image"] = camera_obs
# low-level object information
if self.use_object_obs:
# position and rotation of object
cube_pos = np.array(self.sim.data.body_xpos[self.cube_body_id])
cube_quat = T.convert_quat(
self.sim.data.body_xquat[self.cube_body_id], to="xyzw")
di["cube_pos"] = cube_pos
di["cube_quat"] = cube_quat
di["l_eef_xpos"] = np.array(self._l_eef_xpos)
di["r_eef_xpos"] = np.array(self._r_eef_xpos)
di["handle_1_xpos"] = np.array(self._handle_1_xpos)
di["handle_2_xpos"] = np.array(self._handle_2_xpos)
di["l_gripper_to_handle"] = np.array(self._l_gripper_to_handle)
di["r_gripper_to_handle"] = np.array(self._r_gripper_to_handle)
di["object-state"] = np.concatenate([
di["cube_pos"],
di["cube_quat"],
di["l_eef_xpos"],
di["r_eef_xpos"],
di["handle_1_xpos"],
di["handle_2_xpos"],
di["l_gripper_to_handle"],
di["r_gripper_to_handle"],
])
return di
def _get_observation(self):
"""
Returns an OrderedDict containing observations [(name_string, np.array), ...].
Important keys:
robot-state: contains robot-centric information.
"""
di = super()._get_observation()
# proprioceptive features
di["joint_pos"] = np.array(
[self.sim.data.qpos[x] for x in self._ref_joint_pos_indexes])
di["joint_vel"] = np.array(
[self.sim.data.qvel[x] for x in self._ref_joint_vel_indexes])
robot_states = [
np.sin(di["joint_pos"]),
np.cos(di["joint_pos"]),
di["joint_vel"],
]
if self.has_gripper:
di["gripper_qpos"] = np.array([
self.sim.data.qpos[x]
for x in self._ref_gripper_joint_pos_indexes
])
di["gripper_qvel"] = np.array([
self.sim.data.qvel[x]
for x in self._ref_gripper_joint_vel_indexes
])
di["eef_pos"] = np.array(self.sim.data.site_xpos[self.eef_site_id])
di["eef_quat"] = T.convert_quat(
self.sim.data.get_body_xquat("right_hand"), to="xyzw")
# add in gripper information
robot_states.extend(
[di["gripper_qpos"], di["eef_pos"], di["eef_quat"]])
di["robot-state"] = np.concatenate(robot_states)
return di
def _world_quat(self):
"""World quaternion."""
return T.convert_quat(np.array([1, 0, 0, 0]), to="xyzw")
def _pot_quat(self):
"""Returns the orientation of the pot."""
return T.convert_quat(self.sim.data.body_xquat[self.cube_body_id],
to="xyzw")
def _get_observation(self):
"""
Returns an OrderedDict containing observations [(name_string, np.array), ...].
Important keys:
robot-state: contains robot-centric information.
object-state: requires @self.use_object_obs to be True.
contains object-centric information.
image: requires @self.use_camera_obs to be True.
contains a rendered frame from the simulation.
depth: requires @self.use_camera_obs and @self.camera_depth to be True.
contains a rendered depth map from the simulation
"""
di = super()._get_observation()
if self.use_camera_obs:
camera_obs = self.sim.render(
camera_name=self.camera_name,
width=self.camera_width,
height=self.camera_height,
depth=self.camera_depth,
)
if self.camera_depth:
di["image"], di["depth"] = camera_obs
else:
di["image"] = camera_obs
# low-level object information
if self.use_object_obs:
# remember the keys to collect into object info
object_state_keys = []
# for conversion to relative gripper frame
gripper_pose = T.pose2mat((di["eef_pos"], di["eef_quat"]))
world_pose_in_gripper = T.pose_inv(gripper_pose)
for i in range(len(self.item_names_org)):
if self.single_object_mode == 2 and self.object_id != i:
# Skip adding to observations
continue
obj_str = str(self.item_names_org[i]) + "0"
obj_pos = np.array(
self.sim.data.body_xpos[self.obj_body_id[obj_str]])
obj_quat = T.convert_quat(
self.sim.data.body_xquat[self.obj_body_id[obj_str]],
to="xyzw")
di["{}_pos".format(obj_str)] = obj_pos
di["{}_quat".format(obj_str)] = obj_quat
# get relative pose of object in gripper frame
object_pose = T.pose2mat((obj_pos, obj_quat))
rel_pose = T.pose_in_A_to_pose_in_B(object_pose,
world_pose_in_gripper)
rel_pos, rel_quat = T.mat2pose(rel_pose)
di["{}_to_eef_pos".format(obj_str)] = rel_pos
di["{}_to_eef_quat".format(obj_str)] = rel_quat
object_state_keys.append("{}_pos".format(obj_str))
object_state_keys.append("{}_quat".format(obj_str))
object_state_keys.append("{}_to_eef_pos".format(obj_str))
object_state_keys.append("{}_to_eef_quat".format(obj_str))
if self.single_object_mode == 1:
# Zero out other objects observations
for obj_str, obj_mjcf in self.mujoco_objects.items():
if obj_str == self.obj_to_use:
continue
else:
di["{}_pos".format(obj_str)] *= 0.0
di["{}_quat".format(obj_str)] *= 0.0
di["{}_to_eef_pos".format(obj_str)] *= 0.0
di["{}_to_eef_quat".format(obj_str)] *= 0.0
di["object-state"] = np.concatenate(
[di[k] for k in object_state_keys])
return di
