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