def setup_observables(self):
"""
Sets up observables to be used for this robot
Returns:
OrderedDict: Dictionary mapping observable names to its corresponding Observable object
"""
# Get general robot observables first
observables = super().setup_observables()
# Get prefix from robot model to avoid naming clashes for multiple robots and define observables modality
pf = self.robot_model.naming_prefix
modality = f"{pf}proprio"
sensors = []
names = []
for arm in self.arms:
# Add in eef info
arm_sensors, arm_sensor_names = self._create_arm_sensors(
arm=arm, modality=modality)
sensors += arm_sensors
names += arm_sensor_names
# Create observables for this robot
for name, s in zip(names, sensors):
observables[name] = Observable(
name=name,
sensor=s,
sampling_rate=self.control_freq,
)
return observables
def _setup_observables(self):
"""
Sets up observables to be used for this environment. Creates object-based observables if enabled
Returns:
OrderedDict: Dictionary mapping observable names to its corresponding Observable object
"""
observables = super()._setup_observables()
# low-level object information
if self.use_object_obs:
# Get robot prefix and define observables modality
if self.env_configuration == "bimanual":
pf0 = self.robots[0].robot_model.naming_prefix + "right_"
pf1 = self.robots[0].robot_model.naming_prefix + "left_"
else:
pf0 = self.robots[0].robot_model.naming_prefix
pf1 = self.robots[1].robot_model.naming_prefix
modality = "object"
# position and rotation of object
@sensor(modality=modality)
def pot_pos(obs_cache):
return np.array(self.sim.data.body_xpos[self.pot_body_id])
@sensor(modality=modality)
def pot_quat(obs_cache):
return T.convert_quat(self.sim.data.body_xquat[self.pot_body_id], to="xyzw")
@sensor(modality=modality)
def handle0_xpos(obs_cache):
return np.array(self._handle0_xpos)
@sensor(modality=modality)
def handle1_xpos(obs_cache):
return np.array(self._handle1_xpos)
@sensor(modality=modality)
def gripper0_to_handle0(obs_cache):
return obs_cache["handle0_xpos"] - obs_cache[f"{pf0}eef_pos"] if \
"handle0_xpos" in obs_cache and f"{pf0}eef_pos" in obs_cache else np.zeros(3)
@sensor(modality=modality)
def gripper1_to_handle1(obs_cache):
return obs_cache["handle1_xpos"] - obs_cache[f"{pf1}eef_pos"] if \
"handle1_xpos" in obs_cache and f"{pf1}eef_pos" in obs_cache else np.zeros(3)
sensors = [pot_pos, pot_quat, handle0_xpos, handle1_xpos, gripper0_to_handle0, gripper1_to_handle1]
names = [s.__name__ for s in sensors]
# Create observables
for name, s in zip(names, sensors):
observables[name] = Observable(
name=name,
sensor=s,
sampling_rate=self.control_freq,
)
return observables
def _setup_observables(self):
"""
Sets up observables to be used for this environment. Creates object-based observables if enabled
Returns:
OrderedDict: Dictionary mapping observable names to its corresponding Observable object
"""
observables = super()._setup_observables()
# low-level object information
if self.use_object_obs:
# Get robot prefix and define observables modality
pf = self.robots[0].robot_model.naming_prefix
modality = "object"
# position and rotation of the first cube
@sensor(modality=modality)
def cubeA_pos(obs_cache):
return np.array(self.sim.data.body_xpos[self.cubeA_body_id])
@sensor(modality=modality)
def cubeA_quat(obs_cache):
return convert_quat(np.array(self.sim.data.body_xquat[self.cubeA_body_id]), to="xyzw")
@sensor(modality=modality)
def cubeB_pos(obs_cache):
return np.array(self.sim.data.body_xpos[self.cubeB_body_id])
@sensor(modality=modality)
def cubeB_quat(obs_cache):
return convert_quat(np.array(self.sim.data.body_xquat[self.cubeB_body_id]), to="xyzw")
@sensor(modality=modality)
def gripper_to_cubeA(obs_cache):
return obs_cache["cubeA_pos"] - obs_cache[f"{pf}eef_pos"] if \
"cubeA_pos" in obs_cache and f"{pf}eef_pos" in obs_cache else np.zeros(3)
@sensor(modality=modality)
def gripper_to_cubeB(obs_cache):
return obs_cache["cubeB_pos"] - obs_cache[f"{pf}eef_pos"] if \
"cubeB_pos" in obs_cache and f"{pf}eef_pos" in obs_cache else np.zeros(3)
@sensor(modality=modality)
def cubeA_to_cubeB(obs_cache):
return obs_cache["cubeB_pos"] - obs_cache["cubeA_pos"] if \
"cubeA_pos" in obs_cache and "cubeB_pos" in obs_cache else np.zeros(3)
sensors = [cubeA_pos, cubeA_quat, cubeB_pos, cubeB_quat, gripper_to_cubeA, gripper_to_cubeB, cubeA_to_cubeB]
names = [s.__name__ for s in sensors]
# Create observables
for name, s in zip(names, sensors):
observables[name] = Observable(
name=name,
sensor=s,
sampling_rate=self.control_freq,
)
return observables
def _setup_observables(self):
"""
Sets up observables to be used for this environment. Creates object-based observables if enabled
Returns:
OrderedDict: Dictionary mapping observable names to its corresponding Observable object
"""
observables = super()._setup_observables()
# low-level object information
if self.use_object_obs:
# Get robot prefix and define observables modality
pf = self.robots[0].robot_model.naming_prefix
modality = "object"
# Define sensor callbacks
@sensor(modality=modality)
def door_pos(obs_cache):
return np.array(self.sim.data.body_xpos[self.object_body_ids["door"]])
@sensor(modality=modality)
def handle_pos(obs_cache):
return self._handle_xpos
@sensor(modality=modality)
def door_to_eef_pos(obs_cache):
return obs_cache["door_pos"] - obs_cache[f"{pf}eef_pos"] if\
"door_pos" in obs_cache and f"{pf}eef_pos" in obs_cache else np.zeros(3)
@sensor(modality=modality)
def handle_to_eef_pos(obs_cache):
return obs_cache["handle_pos"] - obs_cache[f"{pf}eef_pos"] if\
"handle_pos" in obs_cache and f"{pf}eef_pos" in obs_cache else np.zeros(3)
@sensor(modality=modality)
def hinge_qpos(obs_cache):
return np.array([self.sim.data.qpos[self.hinge_qpos_addr]])
sensors = [door_pos, handle_pos, door_to_eef_pos, handle_to_eef_pos, hinge_qpos]
names = [s.__name__ for s in sensors]
# Also append handle qpos if we're using a locked door version with rotatable handle
if self.use_latch:
@sensor(modality=modality)
def handle_qpos(obs_cache):
return np.array([self.sim.data.qpos[self.handle_qpos_addr]])
sensors.append(handle_qpos)
names.append("handle_qpos")
# Create observables
for name, s in zip(names, sensors):
observables[name] = Observable(
name=name,
sensor=s,
sampling_rate=self.control_freq,
)
return observables
def setup_observables(self):
"""
Sets up observables to be used for this robot
Returns:
OrderedDict: Dictionary mapping observable names to its corresponding Observable object
"""
# Get general robot observables first
observables = super().setup_observables()
# Get prefix from robot model to avoid naming clashes for multiple robots and define observables modality
pf = self.robot_model.naming_prefix
modality = f"{pf}proprio"
# eef features
@sensor(modality=modality)
def eef_pos(obs_cache):
return np.array(self.sim.data.site_xpos[self.eef_site_id])
@sensor(modality=modality)
def eef_quat(obs_cache):
return T.convert_quat(self.sim.data.get_body_xquat(
self.robot_model.eef_name),
to="xyzw")
sensors = [eef_pos, eef_quat]
names = [f"{pf}eef_pos", f"{pf}eef_quat"]
# add in gripper sensors if this robot has a gripper
if self.has_gripper:
@sensor(modality=modality)
def gripper_qpos(obs_cache):
return np.array([
self.sim.data.qpos[x]
for x in self._ref_gripper_joint_pos_indexes
])
@sensor(modality=modality)
def gripper_qvel(obs_cache):
return np.array([
self.sim.data.qvel[x]
for x in self._ref_gripper_joint_vel_indexes
])
sensors += [gripper_qpos, gripper_qvel]
names += [f"{pf}gripper_qpos", f"{pf}gripper_qvel"]
# Create observables for this robot
for name, s in zip(names, sensors):
observables[name] = Observable(
name=name,
sensor=s,
sampling_rate=self.control_freq,
)
return observables
def setup_observables(self):
"""
Sets up observables to be used for this robot
Returns:
OrderedDict: Dictionary mapping observable names to its corresponding Observable object
"""
# Get prefix from robot model to avoid naming clashes for multiple robots and define observables modality
pf = self.robot_model.naming_prefix
pre_compute = f"{pf}joint_pos"
modality = f"{pf}proprio"
# proprioceptive features
@sensor(modality=modality)
def joint_pos(obs_cache):
return np.array(
[self.sim.data.qpos[x] for x in self._ref_joint_pos_indexes])
@sensor(modality=modality)
def joint_pos_cos(obs_cache):
return np.cos(obs_cache[pre_compute]
) if pre_compute in obs_cache else np.zeros(
self.robot_model.dof)
@sensor(modality=modality)
def joint_pos_sin(obs_cache):
return np.sin(obs_cache[pre_compute]
) if pre_compute in obs_cache else np.zeros(
self.robot_model.dof)
@sensor(modality=modality)
def joint_vel(obs_cache):
return np.array(
[self.sim.data.qvel[x] for x in self._ref_joint_vel_indexes])
sensors = [joint_pos, joint_pos_cos, joint_pos_sin, joint_vel]
names = ["joint_pos", "joint_pos_cos", "joint_pos_sin", "joint_vel"]
# We don't want to include the direct joint pos sensor outputs
actives = [False, True, True, True]
# Create observables for this robot
observables = OrderedDict()
for name, s, active in zip(names, sensors, actives):
obs_name = pf + name
observables[obs_name] = Observable(
name=obs_name,
sensor=s,
sampling_rate=self.control_freq,
active=active,
)
return observables
def _setup_observables(self):
"""
Sets up observables to be used for this environment. Loops through all robots and grabs their corresponding
observables to add to the procedurally generated dict of observables
Returns:
OrderedDict: Dictionary mapping observable names to its corresponding Observable object
"""
observables = super()._setup_observables()
# Loop through all robots and grab their observables, adding it to the proprioception modality
for robot in self.robots:
robot_obs = robot.setup_observables()
observables.update(robot_obs)
# Loop through cameras and update the observations if using camera obs
if self.use_camera_obs:
# Create sensor information
sensors = []
names = []
for (cam_name, cam_w, cam_h, cam_d, cam_segs) in zip(
self.camera_names,
self.camera_widths,
self.camera_heights,
self.camera_depths,
self.camera_segmentations,
):
# Add cameras associated to our arrays
cam_sensors, cam_sensor_names = self._create_camera_sensors(
cam_name,
cam_w=cam_w,
cam_h=cam_h,
cam_d=cam_d,
cam_segs=cam_segs,
modality="image")
sensors += cam_sensors
names += cam_sensor_names
# If any the camera segmentations are not None, then we shrink all the sites as a hacky way to
# prevent them from being rendered in the segmentation mask
if not all(seg is None for seg in self.camera_segmentations):
self.sim.model.site_size[:, :] = 1.0e-8
# Create observables for these cameras
for name, s in zip(names, sensors):
observables[name] = Observable(
name=name,
sensor=s,
sampling_rate=self.control_freq,
)
return observables
def _setup_observables(self):
"""
Sets up observables to be used for this environment. Loops through all robots and grabs their corresponding
observables to add to the procedurally generated dict of observables
Returns:
OrderedDict: Dictionary mapping observable names to its corresponding Observable object
"""
observables = super()._setup_observables()
# Loop through all robots and grab their observables, adding it to the proprioception modality
for robot in self.robots:
robot_obs = robot.setup_observables()
observables.update(robot_obs)
# Loop through cameras and update the observations if using camera obs
if self.use_camera_obs:
# Create sensor information
sensors = []
names = []
for (cam_name, cam_w, cam_h, cam_d) in \
zip(self.camera_names, self.camera_widths, self.camera_heights, self.camera_depths):
# Add cameras associated to our arrays
cam_sensors, cam_sensor_names = self._create_camera_sensors(
cam_name,
cam_w=cam_w,
cam_h=cam_h,
cam_d=cam_d,
modality="image")
sensors += cam_sensors
names += cam_sensor_names
# Create observables for these cameras
for name, s in zip(names, sensors):
observables[name] = Observable(
name=name,
sensor=s,
sampling_rate=self.control_freq,
)
return observables
def _setup_observables(self):
"""
Sets up observables to be used for this environment. Creates object-based observables if enabled
Returns:
OrderedDict: Dictionary mapping observable names to its corresponding Observable object
"""
observables = super()._setup_observables()
# Get prefix from robot model to avoid naming clashes for multiple robots
pf = self.robots[0].robot_model.naming_prefix
modality = "object"
sensors = []
names = []
# Add binary contact observation
if self.use_contact_obs:
@sensor(modality=f"{pf}proprio")
def gripper_contact(obs_cache):
return self._has_gripper_contact
sensors.append(gripper_contact)
names.append(f"{pf}contact")
# object information in the observation
if self.use_object_obs:
if self.use_condensed_obj_obs:
# use implicit representation of wiping objects
@sensor(modality=modality)
def wipe_radius(obs_cache):
wipe_rad, wipe_cent, _ = self._get_wipe_information()
obs_cache["wipe_centroid"] = wipe_cent
return wipe_rad
@sensor(modality=modality)
def wipe_centroid(obs_cache):
return obs_cache[
"wipe_centroid"] if "wipe_centroid" in obs_cache else np.zeros(
3)
@sensor(modality=modality)
def proportion_wiped(obs_cache):
return len(self.wiped_markers) / self.num_markers
sensors += [proportion_wiped, wipe_radius, wipe_centroid]
names += ["proportion_wiped", "wipe_radius", "wipe_centroid"]
if self.use_robot_obs:
# also use ego-centric obs
@sensor(modality=modality)
def gripper_to_wipe_centroid(obs_cache):
return (obs_cache["wipe_centroid"] -
obs_cache[f"{pf}eef_pos"]
if "wipe_centroid" in obs_cache
and f"{pf}eef_pos" in obs_cache else
np.zeros(3))
sensors.append(gripper_to_wipe_centroid)
names.append("gripper_to_wipe_centroid")
else:
# use explicit representation of wiping objects
for i, marker in enumerate(self.model.mujoco_arena.markers):
marker_sensors, marker_sensor_names = self._create_marker_sensors(
i, marker, modality)
sensors += marker_sensors
names += marker_sensor_names
# Create observables
for name, s in zip(names, sensors):
observables[name] = Observable(
name=name,
sensor=s,
sampling_rate=self.control_freq,
)
return observables
def _setup_observables(self):
"""
Sets up observables to be used for this environment. Loops through all robots and grabs their corresponding
observables to add to the procedurally generated dict of observables
Returns:
OrderedDict: Dictionary mapping observable names to its corresponding Observable object
"""
observables = super()._setup_observables()
# Loop through all robots and grab their observables, adding it to the proprioception modality
for robot in self.robots:
robot_obs = robot.setup_observables()
observables.update(robot_obs)
# Loop through cameras and update the observations if using camera obs
if self.use_camera_obs:
# Make sure we get correct convention
convention = IMAGE_CONVENTION_MAPPING[macros.IMAGE_CONVENTION]
# Create sensor information
sensors = []
names = []
for (cam_name, cam_w, cam_h, cam_d) in \
zip(self.camera_names, self.camera_widths, self.camera_heights, self.camera_depths):
# Add camera observables to the dict
rgb_sensor_name = f"{cam_name}_image"
depth_sensor_name = f"{cam_name}_depth"
@sensor(modality="image")
def camera_rgb(obs_cache):
img = self.sim.render(
camera_name=cam_name,
width=cam_w,
height=cam_h,
depth=cam_d,
)
if cam_d:
rgb, depth = img
obs_cache[depth_sensor_name] = np.expand_dims(depth[::convention], axis=-1)
return rgb[::convention]
else:
return img[::convention]
sensors.append(camera_rgb)
names.append(rgb_sensor_name)
if cam_d:
@sensor(modality="image")
def camera_depth(obs_cache):
return obs_cache[depth_sensor_name] if depth_sensor_name in obs_cache else \
np.zeros((cam_h, cam_w, 1))
sensors.append(camera_depth)
names.append(depth_sensor_name)
# Create observables for these cameras
for name, s in zip(names, sensors):
observables[name] = Observable(
name=name,
sensor=s,
sampling_rate=self.control_freq,
)
return observables
def _setup_observables(self):
"""
Sets up observables to be used for this environment. Creates object-based observables if enabled
Returns:
OrderedDict: Dictionary mapping observable names to its corresponding Observable object
"""
observables = super()._setup_observables()
# low-level object information
if self.use_object_obs:
# Get robot prefix and define observables modality
pf = self.robots[0].robot_model.naming_prefix
modality = "object"
# Reset nut sensor mappings
self.nut_id_to_sensors = {}
# for conversion to relative gripper frame
@sensor(modality=modality)
def world_pose_in_gripper(obs_cache):
return (T.pose_inv(
T.pose2mat((obs_cache[f"{pf}eef_pos"],
obs_cache[f"{pf}eef_quat"])))
if f"{pf}eef_pos" in obs_cache
and f"{pf}eef_quat" in obs_cache else np.eye(4))
sensors = [world_pose_in_gripper]
names = ["world_pose_in_gripper"]
enableds = [True]
actives = [False]
# Define nut related sensors
for i, nut in enumerate(self.nuts):
# Create sensors for this nut
using_nut = self.single_object_mode == 0 or self.nut_id == i
nut_sensors, nut_sensor_names = self._create_nut_sensors(
nut_name=nut.name, modality=modality)
sensors += nut_sensors
names += nut_sensor_names
enableds += [using_nut] * 4
actives += [using_nut] * 4
self.nut_id_to_sensors[i] = nut_sensor_names
if self.single_object_mode == 1:
# This is randomly sampled object, so we need to include object id as observation
@sensor(modality=modality)
def nut_id(obs_cache):
return self.nut_id
sensors.append(nut_id)
names.append("nut_id")
enableds.append(True)
actives.append(True)
# Create observables
for name, s, enabled, active in zip(names, sensors, enableds,
actives):
observables[name] = Observable(
name=name,
sensor=s,
sampling_rate=self.control_freq,
enabled=enabled,
active=active,
)
return observables
def _setup_observables(self):
"""
Sets up observables to be used for this environment. Creates object-based observables if enabled
Returns:
OrderedDict: Dictionary mapping observable names to its corresponding Observable object
"""
observables = super()._setup_observables()
# low-level object information
if self.use_object_obs:
# Get robot prefix and define observables modality
pf = self.robots[0].robot_model.naming_prefix
modality = "object"
sensors = []
names = []
# plate-related observables
if self.plate is not None:
@sensor(modality=modality)
def plate_pos(obs_cache):
return np.array(
self.sim.data.body_xpos[self.plate_body_id])
@sensor(modality=modality)
def plate_quat(obs_cache):
return convert_quat(np.array(
self.sim.data.body_xquat[self.plate_body_id]),
to="xyzw")
@sensor(modality=modality)
def gripper_to_plate_pos(obs_cache):
return obs_cache[f"{pf}eef_pos"] - obs_cache["plate_pos"] if \
f"{pf}eef_pos" in obs_cache and "plate_pos" in obs_cache else np.zeros(3)
sensors_plate = [plate_pos, plate_quat, gripper_to_plate_pos]
names_plate = [s.__name__ for s in sensors_plate]
sensors.extend(sensors_plate)
names.extend(names_plate)
# peg-related observables
if self.peg is not None:
@sensor(modality=modality)
def peg_pos(obs_cache):
return np.array(self.sim.data.body_xpos[self.peg_body_id])
@sensor(modality=modality)
def peg_quat(obs_cache):
return convert_quat(np.array(
self.sim.data.body_xquat[self.peg_body_id]),
to="xyzw")
@sensor(modality=modality)
def gripper_to_peg_pos(obs_cache):
return obs_cache[f"{pf}eef_pos"] - obs_cache["peg_pos"] if \
f"{pf}eef_pos" in obs_cache and "peg_pos" in obs_cache else np.zeros(3)
sensors_peg = [peg_pos, peg_quat, gripper_to_peg_pos]
names_peg = [s.__name__ for s in sensors_peg]
sensors.extend(sensors_peg)
names.extend(names_peg)
# Create observables
for name, s in zip(names, sensors):
observables[name] = Observable(
name=name,
sensor=s,
sampling_rate=self.control_freq,
)
return observables
def _setup_observables(self):
"""
Sets up observables to be used for this environment. Creates object-based observables if enabled
Returns:
OrderedDict: Dictionary mapping observable names to its corresponding Observable object
"""
observables = super()._setup_observables()
# low-level object information
if self.use_object_obs:
# Get robot prefix and define observables modality
if self.env_configuration == "bimanual":
pf0 = self.robots[0].robot_model.naming_prefix + "right_"
pf1 = self.robots[0].robot_model.naming_prefix + "left_"
else:
pf0 = self.robots[0].robot_model.naming_prefix
pf1 = self.robots[1].robot_model.naming_prefix
modality = "object"
# position and rotation of peg and hole
@sensor(modality=modality)
def hole_pos(obs_cache):
return np.array(self.sim.data.body_xpos[self.hole_body_id])
@sensor(modality=modality)
def hole_quat(obs_cache):
return T.convert_quat(
self.sim.data.body_xquat[self.hole_body_id], to="xyzw")
@sensor(modality=modality)
def peg_to_hole(obs_cache):
return obs_cache["hole_pos"] - np.array(self.sim.data.body_xpos[self.peg_body_id]) if \
"hole_pos" in obs_cache else np.zeros(3)
@sensor(modality=modality)
def peg_quat(obs_cache):
return T.convert_quat(
self.sim.data.body_xquat[self.peg_body_id], to="xyzw")
# Relative orientation parameters
@sensor(modality=modality)
def angle(obs_cache):
t, d, cos = self._compute_orientation()
obs_cache["t"] = t
obs_cache["d"] = d
return cos
@sensor(modality=modality)
def t(obs_cache):
return obs_cache["t"] if "t" in obs_cache else 0.0
@sensor(modality=modality)
def d(obs_cache):
return obs_cache["d"] if "d" in obs_cache else 0.0
sensors = [hole_pos, hole_quat, peg_to_hole, peg_quat, angle, t, d]
names = [s.__name__ for s in sensors]
# Create observables
for name, s in zip(names, sensors):
observables[name] = Observable(
name=name,
sensor=s,
sampling_rate=self.control_freq,
)
return observables
return curr_proprio_delay
# Define function to convert raw delay time to actual sampling delay (in discrete timesteps)
def calculate_proprio_delay():
base = env.model_timestep
return base * round(curr_proprio_delay / base) if corruption_mode else 0.0
proprio_modifiers = [proprio_corrupter, proprio_delayer, proprio_sampling_rate]
# We will create a separate "ground truth" delayed proprio observable to track exactly
# how much corruption we're getting in real time
proprio_sensor = env._observables[proprio_obs_name]._sensor
proprio_ground_truth_obs_name = f"{proprio_obs_name}_ground_truth"
observable = Observable(
name=proprio_ground_truth_obs_name,
sensor=proprio_sensor,
delayer=lambda: curr_proprio_delay,
sampling_rate=proprio_sampling_rate,
)
# Add this observable
env.add_observable(observable)
# We also need to set the normal joint pos observable to be active (not active by default)
env.modify_observable(observable_name=proprio_obs_name, attribute="active", modifier=True)
# Initialize settings
modify_obs(obs_name=proprio_obs_name, attrs=attributes, mods=proprio_modifiers)
# Add entry for the corruption / delay settings in dict
obs_settings[proprio_obs_name] = {
"attrs": attributes[:2],
def _setup_observables(self):
"""
Sets up observables to be used for this environment. Creates object-based observables if enabled
Returns:
OrderedDict: Dictionary mapping observable names to its corresponding Observable object
"""
observables = super()._setup_observables()
pf = self.robots[0].robot_model.naming_prefix
# Remove unnecessary observables
del observables[pf + "joint_pos"]
del observables[pf + "joint_pos_cos"]
del observables[pf + "joint_pos_sin"]
del observables[pf + "joint_vel"]
del observables[pf + "gripper_qvel"]
del observables[pf + "gripper_qpos"]
del observables[pf + "eef_pos"]
del observables[pf + "eef_quat"]
sensors = []
# probe information
modality = f"{pf}proprio" # Need to use this modality since proprio obs cannot be empty in GymWrapper
@sensor(modality=modality)
def eef_contact_force(obs_cache):
return self.sim.data.cfrc_ext[self.probe_id][-3:]
@sensor(modality=modality)
def eef_torque(obs_cache):
return self.robots[0].ee_torque
@sensor(modality=modality)
def eef_vel(obs_cache):
return self.robots[0]._hand_vel
@sensor(modality=modality)
def eef_contact_force_z_diff(obs_cache):
return self.z_contact_force_running_mean - self.goal_contact_z_force
@sensor(modality=modality)
def eef_contact_derivative_force_z_diff(obs_cache):
return self.der_z_contact_force - self.goal_der_contact_z_force
@sensor(modality=modality)
def eef_vel_diff(obs_cache):
return self.vel_running_mean - self.goal_velocity
@sensor(modality=modality)
def eef_pose_diff(obs_cache):
pos_error = self._eef_xpos - self.traj_pt
quat_error = difference_quat(self._eef_xquat, self.goal_quat)
pose_error = np.concatenate((pos_error, quat_error))
return pose_error
sensors += [
eef_contact_force, eef_torque, eef_vel, eef_contact_force_z_diff,
eef_contact_derivative_force_z_diff, eef_vel_diff, eef_pose_diff
]
names = [s.__name__ for s in sensors]
# Create observables
for name, s in zip(names, sensors):
observables[name] = Observable(
name=name,
sensor=s,
sampling_rate=self.control_freq,
)
return observables
