def _get_obs(self): # positions grip_pos = self.sim.data.get_site_xpos('robot0:grip') dt = self.sim.nsubsteps * self.sim.model.opt.timestep grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) if self.has_object: object_pos = self.sim.data.get_site_xpos('object0') # rotations object_rot = rotations.mat2euler( self.sim.data.get_site_xmat('object0')) # velocities object_velp = self.sim.data.get_site_xvelp('object0') * dt object_velr = self.sim.data.get_site_xvelr('object0') * dt # gripper state object_rel_pos = object_pos - grip_pos object_velp -= grip_velp else: object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.zeros( 0) if not self.has_object: achieved_goal = grip_pos.copy() else: achieved_goal = self.sim.data.get_joint_qpos('object0:joint') obs = grip_pos return { 'observation': obs.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy(), }
def _get_obs(self): # positions grip_pos = self.sim.data.get_site_xpos('robot0:grip') dt = self.sim.nsubsteps * self.sim.model.opt.timestep grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) object0_pos = self.sim.data.get_site_xpos('object0') object1_pos = self.sim.data.get_site_xpos('object1') # gripper state object0_rel_pos = object0_pos - grip_pos object1_rel_pos = object1_pos - grip_pos gripper_state = robot_qpos[-2:] gripper_vel = robot_qvel[ -2:] * dt # change to a scalar if the gripper is made symmetric obs = np.concatenate([ self.goal - object0_pos, self.goal - object1_pos, object0_rel_pos, object1_rel_pos, self.object0_color, self.object1_color, gripper_state, gripper_vel ]) return { 'obs': obs.copy(), 'obs_task_params': self.goal_color_center.copy() }
def _get_obs(self): # positions grip_pos = self.sim.data.get_site_xpos('robot0:grip') dt = self.sim.nsubsteps * self.sim.model.opt.timestep grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) if self.has_object: object_pos = self.sim.data.get_site_xpos('object0') # rotations object_rot = rotations.mat2euler(self.sim.data.get_site_xmat('object0')) # velocities object_velp = self.sim.data.get_site_xvelp('object0') * dt object_velr = self.sim.data.get_site_xvelr('object0') * dt # gripper state object_rel_pos = object_pos - grip_pos object_velp -= grip_velp else: object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.zeros(0) gripper_state = robot_qpos[-2:] gripper_vel = robot_qvel[-2:] * dt # change to a scalar if the gripper is made symmetric if not self.has_object: achieved_goal = grip_pos.copy() else: achieved_goal = np.squeeze(object_pos.copy()) obs = np.concatenate([ grip_pos, object_pos.ravel(), object_rel_pos.ravel(), gripper_state, object_rot.ravel(), object_velp.ravel(), object_velr.ravel(), grip_velp, gripper_vel, ]) return { 'observation': obs.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy(), }
def _get_obs(self): # positions grip_pos = self.sim.data.get_site_xpos('robot0:grip') dt = self.sim.nsubsteps * self.sim.model.opt.timestep grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) # gripper state gripper_state = robot_qpos[-2:] gripper_vel = robot_qvel[-2:] * dt # change to a scalar if the gripper is made symmetric obs_stack = grip_pos, gripper_vel, gripper_state, grip_velp, for obj_key in self.obs_keys or self.obj_keys: obj_key, *attrs = obj_key.split('@') obs_dict = dict( pos=self.sim.data.get_site_xpos(obj_key), rot=rotations.mat2euler(self.sim.data.get_site_xmat(obj_key)), rel_vel=self.sim.data.get_site_xvelp(obj_key) * dt - grip_velp, vel_rot=self.sim.data.get_site_xvelr(obj_key) * dt, ) obs_dict['rel_pos'] = obs_dict['pos'] - grip_pos obs_stack += tuple([obs_dict[k] for k in attrs or obs_dict.keys()]) achieved_goal = self.sim.data.get_site_xpos(self.goal_key) return { 'observation': np.concatenate(obs_stack).copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy(), }
def _get_obs(self): # TODO: set position that has to reach target (object or on body), set to achieved goal # here: body_pos is the position that has to reach target # positions body_pos = self.sim.data.get_body_xpos(self.finder_name) dt = self.sim.nsubsteps * self.sim.model.opt.timestep body_velp = self.sim.data.get_body_xvelp(self.finder_name) * dt robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) # since no object object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.zeros(0) # gripper_state = robot_qpos[-2:] # gripper_vel = robot_qvel[-2:] * dt # change to a scalar if the gripper is made symmetric gripper_state = gripper_vel = np.zeros(0) # since no object achieved_goal = body_pos.copy() obs = np.concatenate([ body_pos, object_pos.ravel(), object_rel_pos.ravel(), gripper_state, object_rot.ravel(), object_velp.ravel(), object_velr.ravel(), body_velp, gripper_vel, ]) return { 'observation': obs.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy(), }
def _get_obs(self): # positions grip_pos = self.sim.data.get_site_xpos('robot0:grip') dt = self.sim.nsubsteps * self.sim.model.opt.timestep grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) if self.has_object: object_pos = self.sim.data.get_site_xpos('object0') # rotations object_rot = rotations.mat2euler(self.sim.data.get_site_xmat('object0')) # velocities object_velp = self.sim.data.get_site_xvelp('object0') * dt object_velr = self.sim.data.get_site_xvelr('object0') * dt # gripper state object_rel_pos = object_pos - grip_pos object_velp -= grip_velp else: object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.zeros(0) gripper_state = robot_qpos[-2:] gripper_vel = robot_qvel[-2:] * dt # change to a scalar if the gripper is made symmetric if not self.has_object: achieved_goal = grip_pos.copy() else: achieved_goal = np.squeeze(object_pos.copy()) obs = np.concatenate([ grip_pos, object_pos.ravel(), object_rel_pos.ravel(), gripper_state, object_rot.ravel(), object_velp.ravel(), object_velr.ravel(), grip_velp, gripper_vel, ]) obs_noobj = np.concatenate([ grip_pos, gripper_state, grip_velp, gripper_vel, self.goal.copy() ]) success = np.array(self._is_success(achieved_goal, self.goal)) state = np.concatenate([obs.copy(), self.goal.copy()]) real_world = 1.0 if self.real_world else 0.0 if self.use_vision: img_size = 64 img = self.sim.render(width=img_size, height=img_size, camera_name="external_camera_0")[::-1] state = { "state": obs_noobj, "pixels": img, 'observation': state, 'grip_pos': grip_pos.copy(), 'obj_pos': object_pos.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy(), 'success': success, 'real_world': np.array(real_world), } else: state = { "state" : state, 'observation': state, 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy(), 'success': success, 'real_world': np.array(real_world), } return state
def _get_obs(self): # positions grip_pos = self.sim.data.get_site_xpos('robot0:grip') dt = self.sim.nsubsteps * self.sim.model.opt.timestep grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) if self.has_object: object_pos = self.sim.data.get_site_xpos('object0') # rotations object_rot = rotations.mat2euler(self.sim.data.get_site_xmat('object0')) # velocities object_velp = self.sim.data.get_site_xvelp('object0') * dt object_velr = self.sim.data.get_site_xvelr('object0') * dt # gripper state object_rel_pos = object_pos - grip_pos object_velp -= grip_velp else: object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.zeros(0) gripper_state = robot_qpos[-2:] gripper_vel = robot_qvel[-2:] * dt # change to a scalar if the gripper is made symmetric if not self.has_object: achieved_goal = grip_pos.copy() else: achieved_goal = np.squeeze(object_pos.copy()) obs = np.concatenate([ grip_pos, object_pos.ravel(), object_rel_pos.ravel(), gripper_state, object_rot.ravel(), object_velp.ravel(), object_velr.ravel(), grip_velp, gripper_vel, ]) return { 'observation': obs.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy(), }
def _move_gripper_to(self, position): grip_pos = self.sim.data.get_site_xpos('robot0:grip') robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) '''print(grip_pos) print(robot_qpos) print(robot_qvel) print('____________________________________________')''' first_over = np.array([grip_pos[0], grip_pos[1], position[2] + 1.]) to_x_y = np.array([position[0], position[1], position[2] + 1.]) gripper_target = np.array( [position[0], position[1], position[2]]) #+ self.sim.data.get_site_xpos('robot0:grip') gripper_rotation = np.array([1., 0., 1., 0.]) #move self.sim.data.set_mocap_pos('robot0:mocap', first_over) self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation) for _ in range(10): self.sim.step() self.sim.forward() self.sim.data.set_mocap_pos('robot0:mocap', to_x_y) self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation) for _ in range(10): self.sim.step() self.sim.forward() self.sim.data.set_mocap_pos('robot0:mocap', gripper_target) self.sim.data.set_mocap_quat('robot0:mocap', gripper_rotation) for _ in range(10): self.sim.step() self.sim.forward() '''grip_pos = self.sim.data.get_site_xpos('robot0:grip')
def _get_obs(self): # positions grip_pos = self.sim.data.get_site_xpos("robot0:grip") dt = self.sim.nsubsteps * self.sim.model.opt.timestep grip_velp = self.sim.data.get_site_xvelp("robot0:grip") * dt robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) gripper_state = robot_qpos[-2:] gripper_vel = robot_qvel[ -2:] * dt # change to a scalar if the gripper is made symmetric achieved_goal = grip_pos.copy() state = np.concatenate([grip_pos, grip_velp]) # pixel = self.render(mode="rgb_array") return { "observation": state.copy(), "achieved_goal": np.zeros((0, )), "desired_goal": np.zeros((0, )), # "_pixel": pixel.copy(), "_state": state.copy(), "_achieved_goal": achieved_goal.copy(), "_desired_goal": self.goal.copy(), }
def _get_obs(self): robot_qpos, robot_qvel = robot_get_obs(self.sim) dt = self.sim.nsubsteps * self.sim.model.opt.timestep forearm_pose = self._get_body_pose('robot0:forearm', euler=True) forearm_velp = self.sim.data.get_body_xvelp('robot0:forearm') * dt palm_pos = self._get_palm_pose(no_rot=True)[:3] object_pose = np.zeros(0) object_vel = np.zeros(0) object_rel_pos = np.zeros(0) if self.has_object: object_vel = self.sim.data.get_joint_qvel('object:joint') object_pose = self._get_body_pose('object', euler=True) object_rel_pos = object_pose[:3] - palm_pos observation = np.concatenate([ forearm_pose, forearm_velp, palm_pos, object_rel_pos, robot_qpos, robot_qvel, object_pose, object_vel ]) return { 'observation': observation, 'achieved_goal': self._get_achieved_goal().ravel(), 'desired_goal': self.goal.ravel().copy(), }
def _get_obs(self): robot_qpos, robot_qvel = robot_get_obs(self.sim) object_qpos = self.sim.data.get_joint_qpos('object:joint') object_qvel = self.sim.data.get_joint_qvel('object:joint') #target_qpos, target_qvel # TODO: Fix this observation = np.concatenate( [robot_qpos, robot_qvel, object_qpos, object_qvel]) return {'observation': observation.copy()}
def _get_obs(self): # positions grip_pos = self.sim.data.get_site_xpos('robot0:grip') dt = self.sim.nsubsteps * self.sim.model.opt.timestep grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) if self.has_object: object_pos = self.sim.data.get_site_xpos('object0') # rotations object_rot = rotations.mat2euler( self.sim.data.get_site_xmat('object0')) # velocities object_velp = self.sim.data.get_site_xvelp('object0') * dt object_velr = self.sim.data.get_site_xvelr('object0') * dt # gripper state object_rel_pos = object_pos - grip_pos object_velp -= grip_velp else: object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.zeros( 0) gripper_state = robot_qpos[-2:] gripper_vel = robot_qvel[ -2:] * dt # change to a scalar if the gripper is made symmetric if not self.has_object: achieved_goal = grip_pos.copy() else: achieved_goal = np.squeeze(object_pos.copy()) # obs = np.concatenate([ # grip_pos, object_pos.ravel(), object_rel_pos.ravel(), gripper_state, object_rot.ravel(), # object_velp.ravel(), object_velr.ravel(), grip_velp, gripper_vel, # ]) if self.obs_with_time: obs = np.concatenate([ object_pos[:2], grip_pos[:2], object_pos[2:3], grip_pos[2:3], object_rel_pos.ravel(), gripper_state, object_rot.ravel(), object_velp.ravel(), object_velr.ravel(), grip_velp, gripper_vel, np.array([self.timestep / self.episode_len]) ]) else: obs = np.concatenate([ object_pos[:2], grip_pos[:2], object_pos[2:3], grip_pos[2:3], object_rel_pos.ravel(), gripper_state, object_rot.ravel(), object_velp.ravel(), object_velr.ravel(), grip_velp, gripper_vel, ]) return obs.copy()
def _get_obs(self): robot_qpos, robot_qvel = robot_get_obs(self.sim) achieved_goal = self._get_achieved_goal().ravel() observation = np.concatenate([robot_qpos, robot_qvel, achieved_goal]) return { 'observation': observation.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy(), }
def _get_obs(self): robot_qpos, robot_qvel = robot_get_obs(self.sim) achieved_goal = self._get_achieved_goal().ravel() observation = np.concatenate([robot_qpos, robot_qvel, achieved_goal]) return { 'observation': observation.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy(), }
def _get_obs(self): robot_qpos, robot_qvel = robot_get_obs(self.sim) object_qvel = self.sim.data.get_joint_qvel('object:joint') achieved_goal = self._get_achieved_goal().ravel() # this contains the object position + rotation observation = np.concatenate([robot_qpos, robot_qvel, object_qvel, achieved_goal]) return { 'observation': observation.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.ravel().copy(), }
def _get_obs(self): robot_qpos, robot_qvel = robot_get_obs(self.sim) object_qvel = self.sim.data.get_joint_qvel('object:joint') achieved_goal = self._get_achieved_goal().ravel() # this contains the object position + rotation observation = np.concatenate([robot_qpos, robot_qvel, object_qvel, achieved_goal]) return { 'observation': observation.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.ravel().copy(), }
def _get_obs(self): # positions grip_pos = self.sim.data.get_site_xpos('robot0:grip') dt = self.sim.nsubsteps * self.sim.model.opt.timestep grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) object_pos, object_rot, object_velp, object_velr, object_rel_pos = [], [], [], [], [] gripper_state = robot_qpos[-2:] gripper_vel = robot_qvel[-2:] * dt # change to a scalar if the gripper is made symmetric num_blocks = self.num_objs - 2 obs = np.concatenate([[num_blocks], grip_pos, gripper_state, grip_velp, gripper_vel]) block_features = None block_indices = list(range(num_blocks)) # np.random.shuffle(block_indices) for i in block_indices: obj_name = 'object{}'.format(i) temp_pos = self.sim.data.get_site_xpos(obj_name) # rotations temp_rot = rotations.mat2euler(self.sim.data.get_site_xmat(obj_name)) # velocities temp_velp = self.sim.data.get_site_xvelp(obj_name) * dt temp_velr = self.sim.data.get_site_xvelr(obj_name) * dt # gripper state temp_rel_pos = temp_pos - grip_pos temp_velp -= grip_velp block_obs = np.concatenate([temp_pos.ravel(), temp_rel_pos.ravel(), temp_rot.ravel(), temp_velp.ravel(), temp_velr.ravel(), one_hot_color(self.obj_colors[i+2]) ]) block_features = block_obs.shape[0] obs = np.concatenate([obs, block_obs]) padding = np.zeros(block_features * (self.max_num_blocks - num_blocks)) obs = np.concatenate([obs, padding]) assert(self._check_goal()) achieved_goal = self.achieved_goal.copy().ravel() max_goal_len = (self.max_num_blocks + 2) ** 2 achieved_goal = np.append(achieved_goal, np.zeros(max_goal_len - achieved_goal.shape[0])) desired_goal = np.append(self.goal.copy(), np.zeros(max_goal_len - self.goal.shape[0])) return { 'observation': obs.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': desired_goal.copy(), }
def _get_obs(self): # positions # grip_pos - Position of the gripper given in 3 positional elements and 4 rotational elements grip_pos = self.sim.data.get_site_xpos('robot0:grip') dt = self.sim.nsubsteps * self.sim.model.opt.timestep # grip_velp - The velocity of gripper moving grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) if self.has_object: # object_pos - Position of the object with respect to the world frame object_pos = self.sim.data.get_site_xpos('object0') # rotations object_rot - Yes. That is the orientation of the object with respect to world frame. object_rot = rotations.mat2euler( self.sim.data.get_site_xmat('object0')) # velocities object_velp - Positional velocity of the object with respect to the world frame object_velp = self.sim.data.get_site_xvelp('object0') * dt # object_velr - Rotational velocity of the object with respect to the world frame object_velr = self.sim.data.get_site_xvelr('object0') * dt # gripper state # object_rel_pos - Position of the object relative to the gripper object_rel_pos = object_pos - grip_pos object_velp -= grip_velp else: object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.zeros( 0) # gripper_state - No. It's not 0/1 signal, it is a float value and varies from 0 to 0.2 # for fetch robot. This varied gripper state helps in grasping different sized # object with different strengths. # gripper_state - The quantity to measure the opening of gripper gripper_state = robot_qpos[-2:] # gripper_vel - The velocity of gripper opening/closing gripper_vel = robot_qvel[ -2:] * dt # change to a scalar if the gripper is made symmetric if not self.has_object: achieved_goal = grip_pos.copy() else: achieved_goal = np.squeeze(object_pos.copy()) obs = np.concatenate([ grip_pos, object_pos.ravel(), object_rel_pos.ravel(), gripper_state, object_rot.ravel(), object_velp.ravel(), object_velr.ravel(), grip_velp, gripper_vel, ]) return { 'observation': obs.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy(), }
def _get_obs(self): # positions grip_pos = self.sim.data.get_site_xpos('robot0:grip') dt = self.sim.nsubsteps * self.sim.model.opt.timestep grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) if self.has_object: object_pos = self.sim.data.get_site_xpos('object0') # rotations object_rot = rotations.mat2euler( self.sim.data.get_site_xmat('object0')) # velocities object_velp = self.sim.data.get_site_xvelp('object0') * dt object_velr = self.sim.data.get_site_xvelr('object0') * dt # gripper state object_rel_pos = object_pos - grip_pos object_velp -= grip_velp else: object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.zeros( 0) gripper_state = robot_qpos[-2:] gripper_vel = robot_qvel[ -2:] * dt # change to a scalar if the gripper is made symmetric if not self.has_object: achieved_goal = grip_pos.copy() else: achieved_goal = np.squeeze(object_pos.copy()) if self.observation_type == "image": #can parameterize kwargs for the rendering options later # obs = np.transpose(self.render("rgb_array", 64, 64), (2,0,1)) # temporary send 1d, which will be reshaped by model. # obs = self.render("rgb_array", 64, 64).flatten() obs = np.transpose(self.render("rgb_array", 64, 64), (2, 0, 1)).flatten() else: obs = np.concatenate([ grip_pos, object_pos.ravel(), object_rel_pos.ravel(), gripper_state, object_rot.ravel(), object_velp.ravel(), object_velr.ravel(), grip_velp, gripper_vel, ]) return { 'observation': obs.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy(), }
def _get_obs(self): ''' Gets the observation of the previous action ____________ Returns: dictionary containing: observation achieved_goal desired_goal ''' grip_pos = self.sim.data.get_site_xpos('robot0:gripper') dt = self.sim.nsubsteps * self.sim.model.opt.timestep grip_velp = self.sim.data.get_site_xvelp('robot0:gripper') * dt robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) if self.has_object: object_pos = self.sim.data.get_site_xpos('object0') # Rotations object_rot = rotations.mat2euler( self.sim.data.get_site_xmat('object0')) # Velocities object_velp = self.sim.data.get_site_xvelp('object0') * dt object_velr = self.sim.data.get_site_xvelr('object0') * dt # Gripper state object_rel_pos = object_pos - grip_pos object_velp -= grip_velp else: object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.zeros( 3) gripper_state = robot_qpos[-2:] # Change to a scalar if the gripper is symmetric gripper_vel = robot_qvel[-2:] * dt if not self.has_object: achieved_goal = np.squeeze( self.sim.data.get_site_xpos('robot0:gripper').copy()) else: achieved_goal = np.squeeze(object_pos.copy()) obs = np.concatenate([ grip_pos, object_pos.ravel(), object_rel_pos.ravel(), gripper_state, object_rot.ravel(), object_velp.ravel(), object_velr.ravel(), grip_velp, gripper_vel ]) return { 'observation': obs.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy() }
def _get_obs_gripper_at(env, position): gripper_target = np.array([position[0], position[1], position[2]]) # positions grip_pos = env.env.env.sim.data.get_site_xpos('robot0:grip') dt = env.env.env.sim.nsubsteps * env.env.env.sim.model.opt.timestep grip_velp = env.env.env.sim.data.get_site_xvelp('robot0:grip') * dt robot_qpos, robot_qvel = utils.robot_get_obs(env.env.env.sim) if env.env.env.has_object: object_pos = env.env.env.sim.data.get_site_xpos('object0') # rotations object_rot = rotations.mat2euler( env.env.env.sim.data.get_site_xmat('object0')) # velocities object_velp = env.env.env.sim.data.get_site_xvelp('object0') * dt object_velr = env.env.env.sim.data.get_site_xvelr('object0') * dt # gripper state object_rel_pos = object_pos - grip_pos object_velp -= grip_velp else: object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.zeros( 0) gripper_state = robot_qpos[-2:] gripper_vel = robot_qvel[ -2:] * dt # change to a scalar if the gripper is made symmetric if not env.env.env.has_object: achieved_goal = grip_pos.copy() else: achieved_goal = np.squeeze(object_pos.copy()) obs = np.concatenate([ grip_pos, object_pos.ravel(), object_rel_pos.ravel(), gripper_state, object_rot.ravel(), object_velp.ravel(), object_velr.ravel(), grip_velp, gripper_vel, ]) # env.env.env._get_image('obs.png') return { 'observation': obs.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': env.env.env.goal.copy(), }
def _get_obs(self): # TODO: set position that has to reach target (object or on body), set to achieved goal # here: body_pos is the position that has to reach target # positions grip_pos = self.sim.data.get_body_xpos(self.finder_name) dt = self.sim.nsubsteps * self.sim.model.opt.timestep grip_velp = self.sim.data.get_body_xvelp(self.finder_name) * dt # for the following to work, all joints associated with the robot have to start with "robot" robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) # since object object_pos = self.sim.data.get_site_xpos('object0') # rotations object_rot = rotations.mat2euler( self.sim.data.get_site_xmat('object0')) # velocities object_velp = self.sim.data.get_site_xvelp('object0') * dt object_velr = self.sim.data.get_site_xvelr('object0') * dt # gripper state object_rel_pos = object_pos - grip_pos object_velp -= grip_velp gripper_state = robot_qpos[-1:] gripper_vel = robot_qvel[ -1:] * dt # changed to a scalar since the gripper is made symmetric [-3:] else achieved_goal = np.squeeze(object_pos.copy()) #if not object_pos[2] == 0.: # TODO: had changes in her/rollout.py (see todo) # achieved_goal = np.zeros_like(object_pos.copy()) # achieved_goal.fill(np.nan) obs = np.concatenate([ grip_pos, object_pos.ravel(), object_rel_pos.ravel(), gripper_state, object_rot.ravel(), object_velp.ravel(), object_velr.ravel(), grip_velp, gripper_vel, ]) return { 'observation': obs.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy(), }
def _get_obs(self): grip_pos = self.sim.data.get_site_xpos('robot0:grip') dt = self.sim.nsubsteps * self.sim.model.opt.timestep grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) #gripper_state = robot_qpos[-2:] gripper_vel = robot_qvel[-2:] * dt # change to a scalar if the gripper is made symmetric obs = np.concatenate([grip_pos, grip_velp, gripper_vel]) for g in self.goals: goal = [g.position[0], g.position[1], g.position[2], g.reached] obs = np.concatenate([obs, goal]) # print('Calculated observation at the end %s' % obs) #print('Shape %s' % obs.shape) return obs.copy()
def _get_obs(self): # proprioception robot_qpos, robot_qvel = robot_get_obs(self.sim) # touch sensor information touch = self.sim.data.sensordata[self._touch_sensor_id] achieved_goal = self._get_achieved_goal().ravel() observation = np.concatenate( [robot_qpos, robot_qvel, touch, achieved_goal, self.goal.copy()]) return { 'observation': observation.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy(), }
def _get_obs(self): # images img = self.sim.render(width=400, height=400, camera_name="external_camera_1") # positions grip_pos = self.sim.data.get_site_xpos('robot0:grip') holder_pos = grip_pos.copy() grip_pos += self.obs_noise_vector dt = self.sim.nsubsteps * self.sim.model.opt.timestep grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) if self.has_object: object_pos = self.sim.data.get_site_xpos('object0') # rotations object_rot = rotations.mat2euler(self.sim.data.get_site_xmat('object0')) # velocities object_velp = self.sim.data.get_site_xvelp('object0') * dt object_velr = self.sim.data.get_site_xvelr('object0') * dt # gripper state object_rel_pos = object_pos - grip_pos object_velp -= grip_velp else: object_pos = grip_pos object_rot = np.zeros(3) object_velp = grip_velp object_velr = np.zeros(3) object_rel_pos = np.zeros(3) gripper_state = robot_qpos[-2:] gripper_vel = robot_qvel[-2:] * dt # change to a scalar if the gripper is made symmetric if not self.has_object: achieved_goal = grip_pos.copy()# - self.sim.data.get_site_xpos("robot0:cam") else: achieved_goal = np.squeeze(object_pos.copy())# - self.sim.data.get_site_xpos("robot0:cam") obs = np.concatenate([ grip_pos, object_pos.ravel(), object_rel_pos.ravel(), gripper_state, object_rot.ravel(), object_velp.ravel(), object_velr.ravel(), grip_velp, gripper_vel, ]) return { 'observation': obs.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy(), 'image':(img/255).copy(), 'gripper_pose': holder_pos.copy() }
def _get_obs(self): palm = [self.sim.data.sensordata[-19], self.sim.data.sensordata[-18]] fingers = np.take(self.sim.data.sensordata, [-1, -4, -7, -10, -13]) robot_qpos, robot_qvel = robot_get_obs(self.sim) robot_qpos = np.delete(robot_qpos, [0, 1]) # ignore fixed joints: Wrist UDEV+PRO robot_qvel = np.delete(robot_qvel, [0, 1]) #object_qvel = self.sim.data.get_joint_qvel(self.target_body) #object_pos = self._get_achieved_qpos().ravel()[:3] # this contains the object position + rotation #mocap_pos = self.sim.data.mocap_pos.ravel() #delta = object_pos - mocap_pos observation = np.concatenate([ palm, fingers, robot_qpos, robot_qvel ]) #, np.zeros(delta.size), np.zeros(object_qvel.size)]) observation += self.np_random.normal(size=observation.size, scale=0.005) return {'observation': observation.copy()}
def _get_obs(self): # positions grip_pos = self.sim.data.get_site_xpos('robot0:grip') dt = self.sim.nsubsteps * self.sim.model.opt.timestep grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) object_pos, object_rot, object_velp, object_velr, object_rel_pos = [], [], [], [], [] gripper_state = robot_qpos[-2:] gripper_vel = robot_qvel[-2:] * dt # change to a scalar if the gripper is made symmetric obs = [] for i in range(self.num_objs-2): obj_name = 'object{}'.format(i) temp_pos = self.sim.data.get_site_xpos(obj_name) # rotations temp_rot = rotations.mat2euler(self.sim.data.get_site_xmat(obj_name)) # velocities temp_velp = self.sim.data.get_site_xvelp(obj_name) * dt temp_velr = self.sim.data.get_site_xvelr(obj_name) * dt # gripper state temp_rel_pos = temp_pos - grip_pos temp_velp -= grip_velp block_obs = np.concatenate([temp_pos.ravel(), temp_rel_pos.ravel(), temp_rot.ravel(), temp_velp.ravel(), temp_velr.ravel()]) obs = np.concatenate([obs, block_obs]) assert(self._check_goal()) achieved_goal = self.achieved_goal.copy().ravel() obs = np.concatenate([grip_pos, gripper_state, grip_velp, gripper_vel, obs]) return { 'observation': obs.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy(), }
def _get_obs(self): # positions grip_pos = self.sim.data.get_site_xpos('robot0:grip') dt = self.sim.nsubsteps * self.sim.model.opt.timestep grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt # 得到机器人的位置和速度非常简单,只需要调用位于utils中的robot_get_obs方法即可 robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) if self.has_object: object_list = [1]*self.n_objects for i in range(self.n_objects): object_pos = self.sim.data.get_site_xpos('object0') # rotations # Convert Rotation Matrix to Euler Angles object_rot = rotations.mat2euler(self.sim.data.get_site_xmat('object0')) # velocities object_velp = self.sim.data.get_site_xvelp('object0') * dt object_velr = self.sim.data.get_site_xvelr('object0') * dt # gripper state object_rel_pos = object_pos - grip_pos object_velp -= grip_velp object_list[i] = np.concatenate([object_pos, object_rot, object_velp, object_velr, object_rel_pos, object_velp]) else: object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.zeros(0) gripper_state = robot_qpos[-2:] gripper_vel = robot_qvel[-2:] * dt # change to a scalar if the gripper is made symmetric obs = np.concatenate([ grip_pos, gripper_state, grip_velp, gripper_vel, object_list[0],object_list[1],object_list[2] ]) if not self.has_object: achieved_goal = grip_pos.copy() else: achieved_goal = self._get_goal(obs,self.desired_shape) return { 'observation': obs.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy(), }
def _get_obs(self): # positions #print("get_obs") grip_pos = self.sim.data.get_site_xpos('robot0:grip') #end_effector position dt = self.sim.nsubsteps * self.sim.model.opt.timestep grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) #Returns all joint positions and velocities associated with a robot. if self.has_object: ## has_object=false object_pos = self.sim.data.get_site_xpos('object0') # rotations object_rot = rotations.mat2euler(self.sim.data.get_site_xmat('object0')) # velocities object_velp = self.sim.data.get_site_xvelp('object0') * dt object_velr = self.sim.data.get_site_xvelr('object0') * dt # gripper state object_rel_pos = object_pos - grip_pos object_velp -= grip_velp else: object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.zeros(0) gripper_state = robot_qpos[-2:] gripper_vel = robot_qvel[-2:] * dt # change to a scalar if the gripper is made symmetric if not self.has_object: ##has_object=false achieved_goal = grip_pos.copy() #end_effector postion else: achieved_goal = np.squeeze(object_pos.copy()) obs = np.concatenate([ grip_pos, object_pos.ravel(), object_rel_pos.ravel(), gripper_state, object_rot.ravel(), object_velp.ravel(), object_velr.ravel(), grip_velp, gripper_vel, ]) # print('grip_pos:'+str(grip_pos)) # print('gripper_state'+str(gripper_state)) # print('grip_velp'+str(grip_velp)) return { 'observation': obs.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy(), }
def _get_obs(self): # images # grip_pos = self.sim.data.get_site_xpos('robot0:grip') # # img = self.sim.render(width=512, height=512, camera_name="external_camera_1") # # # camera position and quaternion # cam_pos = self.sim.model.cam_pos[4].copy() # cam_quat = self.sim.model.cam_quat[4].copy() # # object_pos = self.sim.data.get_site_xpos('object0') # # # # rotations # # object_rot = rotations.mat2euler(self.sim.data.get_site_xmat('object0')) # # # velocities # # object_velp = self.sim.data.get_site_xvelp('object0') * dt # # object_velr = self.sim.data.get_site_xvelr('object0') * dt # # achieved_goal = np.squeeze(object_pos.copy())# - self.sim.data.get_site_xpos("robot0:cam") # obs = np.concatenate([ # cam_pos, cam_quat # ]) # obs += self.obs_noise_vector # # return { # 'observation': obs.copy(), # 'achieved_goal': achieved_goal.copy(), # 'desired_goal': self.goal.copy(), # 'image':(img/255).copy(), # 'gripper_pose': grip_pos.copy() # } # images if self.depth: img = self.sim.render(width=224, height=224, camera_name="external_camera_1", depth=True)[1] else: if self.two_cam: img = self.sim.render(width=224, height=224, camera_name="external_camera_2") / 255 # normalize by imagenet parameters img = (img - np.array([0.485, 0.456, 0.406])) / np.array( [0.229, 0.224, 0.225]) # second image img2 = self.sim.render(width=224, height=224, camera_name="external_camera_3") / 255 # normalize by imagenet parameters img2 = (img2 - np.array([0.485, 0.456, 0.406])) / np.array( [0.229, 0.224, 0.225]) img = np.concatenate([img, img2], axis=-1) else: img = self.sim.render(width=224, height=224, camera_name="external_camera_1") / 255 # normalize by imagenet parameters img = (img - np.array([0.485, 0.456, 0.406])) / np.array( [0.229, 0.224, 0.225]) # positions grip_pos = self.sim.data.get_site_xpos('robot0:grip') holder_pos = grip_pos.copy() # dt = self.sim.nsubsteps * self.sim.model.opt.timestep # grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) object_pos = self.sim.data.get_site_xpos('object0') # obj rotations rot_mat = np.reshape(self.sim.data.site_xmat[3], (3, 3)) v = np.zeros(3) v[np.argmax(self.sim.model.site_size[-1])] = 1 v = np.matmul(rot_mat, v) v[2] = 0 obj_radius = (math.atan2(v[0], v[1]) + math.pi) % (math.pi) - math.pi / 2 if obj_radius > math.pi / 2: obj_radius = (obj_radius - math.pi) # gripper rotations grip_mat = rotations.quat2mat(self.sim.data.mocap_quat) grip_v = np.squeeze(np.matmul(grip_mat, np.array([0, 1, 0]))) grip_radius = (math.atan2(grip_v[0], grip_v[1]) + 2 * math.pi) % (2 * math.pi) if grip_radius > math.pi: grip_radius = (grip_radius - 2 * math.pi) # rotations # object_rot = rotations.mat2euler(self.sim.data.get_site_xmat('object0')) # # velocities # object_velp = self.sim.data.get_site_xvelp('object0') * dt # object_velr = self.sim.data.get_site_xvelr('object0') * dt # # gripper state # object_rel_pos = object_pos - grip_pos # object_velp -= grip_velp # # gripper_state = robot_qpos[-2:] # gripper_vel = robot_qvel[-2:] * dt # change to a scalar if the gripper is made symmetric counter = np.array([self.counter]) achieved_goal = np.concatenate( [np.squeeze(object_pos.copy()), [obj_radius]]) holder_pos = np.concatenate([np.squeeze(holder_pos), [grip_radius]]) # obs = np.concatenate([ # grip_pos, object_pos.ravel(), object_rel_pos.ravel(), gripper_state, object_rot.ravel(), # object_velp.ravel(), object_velr.ravel(), grip_velp, gripper_vel, counter # ]) # obs = np.concatenate([ # grip_pos, gripper_state, grip_velp, gripper_vel, counter # ]) if self.use_task_index: obs = np.concatenate([counter, [0, 1, 0]]) else: # obs = np.concatenate([ # counter, self.cam_offset.copy() # ]) obs = counter # obs = np.empty(0) return { 'observation': obs.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy(), 'image': img.copy(), 'gripper_pose': holder_pos.copy() }
def _get_obs(self): # positions grip_pos = self.sim.data.get_site_xpos('robot0:grip') dt = self.sim.nsubsteps * self.sim.model.opt.timestep grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) if self.has_object: object_pos = self.sim.data.get_site_xpos( 'object0') # + 0.02 * (np.random.random(3) - 0.5) # rotations object_rot = rotations.mat2euler( self.sim.data.get_site_xmat('object0')) # velocities object_velp = self.sim.data.get_site_xvelp('object0') * dt object_velr = self.sim.data.get_site_xvelr('object0') * dt # gripper state object_rel_pos = object_pos - grip_pos # if np.linalg.norm(object_rel_pos) < 0.03: self.sim.data.qvel[self.sim.model.joint_name2id('object0:joint')+1] += np.random.random()*1.0-0.5 object_velp -= grip_velp else: object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.zeros( 0) gripper_state = robot_qpos[-2:] gripper_vel = robot_qvel[ -2:] * dt # change to a scalar if the gripper is made symmetric goal_rel_pos = self.goal.copy()[:3] - object_pos if not self.has_object: achieved_goal = grip_pos.copy() else: achieved_goal = np.squeeze(object_pos.copy()) if self.fragile_on: # if self.sim.data.sensordata[2] > 100 and self.broken_table == False: # self.sim.model.geom_matid[self.sim.model.body_geomadr[self.sim.model.body_name2id('table0')]] = 2 # self.broken_table = True l_finger_force = ( self.sim.data.ctrl[0] - self.sim.data.sensordata[self.sim.model.sensor_name2id( 'l_finger_jnt')]) * self.sim.model.actuator_gainprm[ self.sim.model. actuator_name2id('robot0:l_gripper_finger_joint'), 0] r_finger_force = ( self.sim.data.ctrl[1] - self.sim.data.sensordata[self.sim.model.sensor_name2id( 'r_finger_jnt')]) * self.sim.model.actuator_gainprm[ self.sim.model. actuator_name2id('robot0:r_gripper_finger_joint'), 0] l_finger_force = self.prev_lforce + (l_finger_force - self.prev_lforce) * dt / 0.05 r_finger_force = self.prev_rforce + (r_finger_force - self.prev_rforce) * dt / 0.05 object_force = self.prev_oforce + (self.sim.data.sensordata[ self.sim.model.sensor_name2id('object_frc')] - self.prev_oforce) * dt / 0.1 if (object_force > self.object_fragility): self.sim.model.geom_matid[self.sim.model.body_geomadr[ self.sim.model.body_name2id('object0')]] = 4 self.broken_object = 1.0 elif object_force > 0.0: self.sim.model.geom_matid[self.sim.model.body_geomadr[ self.sim.model.body_name2id('object0')]] = 3 self.broken_object = 0.0 # else: # self.sim.model.geom_matid[self.sim.model.body_geomadr[self.sim.model.body_name2id('object0')]] = 3 # self.broken_object = 2.0 conc_stiffness_data = np.concatenate([[ l_finger_force, r_finger_force, self.prev_stiffness ]]) / 1000.0 # print("Fragility:{}, minimum force:{}, grasping force:{}".format(self.object_fragility, self.min_grip*2, l_finger_force+r_finger_force)) # obs = np.concatenate([ # grip_pos, object_pos.ravel(), object_rel_pos.ravel(), gripper_state, object_rot.ravel(), # object_velp.ravel(), object_velr.ravel(), grip_velp, gripper_vel, goal_rel_pos.ravel(), conc_stiffness_data # ]) obs = np.concatenate([ grip_pos, object_rel_pos.ravel(), gripper_state, goal_rel_pos.ravel(), conc_stiffness_data ]) achieved_goal = np.concatenate( [achieved_goal, conc_stiffness_data[:2]]) self.prev_lforce = l_finger_force self.prev_rforce = r_finger_force self.prev_oforce = object_force else: # obs = np.concatenate([ # grip_pos, object_pos.ravel(), object_rel_pos.ravel(), gripper_state, object_rot.ravel(), # object_velp.ravel(), object_velr.ravel(), grip_velp, gripper_vel, goal_rel_pos.ravel() # ]) obs = np.concatenate([ grip_pos, object_rel_pos.ravel(), gripper_state, goal_rel_pos.ravel() ]) return { 'observation': obs.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy(), }
def _get_obs(self): # add noise to distractor objects positions object0_qpos = self.sim.data.get_joint_qpos('object0:joint') object1_qpos = self.sim.data.get_joint_qpos('object1:joint') object2_qpos = self.sim.data.get_joint_qpos('object2:joint') tmp = self.object3_xpos[:2].copy() + np.random.randn(2) * 0.005 i=1 while la.norm(tmp - object1_qpos[:2]) < 0.05 or la.norm(tmp - object0_qpos[:2]) < 0.05 or la.norm(tmp - object2_qpos[:2]) < 0.05 or la.norm(tmp - self.object4_xpos[:2]) < \ 0.05: tmp = self.object3_xpos[:2].copy() + np.random.randn(2) * 0.005 i += 1 if i == 100: tmp = self.object3_xpos[:2].copy() break self.object3_xpos[:2] = tmp.copy() tmp = self.object4_xpos[:2].copy() + np.random.randn(2) * 0.005 i = 1 while la.norm(tmp - object1_qpos[:2]) < 0.05 or la.norm(tmp - object0_qpos[:2]) < 0.05 or la.norm(tmp - object2_qpos[:2]) < 0.05 or la.norm(tmp - self.object3_xpos[:2]) < \ 0.05: tmp = self.object4_xpos[:2].copy() + np.random.randn(2) * 0.005 i += 1 if i == 100: tmp = self.object4_xpos[:2].copy() break self.object4_xpos[:2] = tmp.copy() # for video object3_qpos = self.sim.data.get_joint_qpos('object3:joint') object4_qpos = self.sim.data.get_joint_qpos('object4:joint') object3_qpos[:3] = self.object3_xpos object4_qpos[:3] = self.object4_xpos self.sim.data.set_joint_qpos('object3:joint', object3_qpos) self.sim.data.set_joint_qpos('object4:joint', object4_qpos) # positions grip_pos = self.sim.data.get_site_xpos('robot0:grip') dt = self.sim.nsubsteps * self.sim.model.opt.timestep grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) objects_pos = [] objects_rot = [] objects_velp = [] objects_velr = [] objects_rel_pos = [] if self.has_object: # object 0 object0_pos = self.sim.data.get_site_xpos('object0') # rotations object0_rot = rotations.mat2euler(self.sim.data.get_site_xmat('object0')) # velocities object0_velp = self.sim.data.get_site_xvelp('object0') * dt object0_velr = self.sim.data.get_site_xvelr('object0') * dt # gripper state object0_rel_pos = object0_pos - grip_pos object0_velp -= grip_velp # object 1 object1_pos = self.sim.data.get_site_xpos('object1') # rotations object1_rot = rotations.mat2euler(self.sim.data.get_site_xmat('object1')) # velocities object1_velp = self.sim.data.get_site_xvelp('object1') * dt object1_velr = self.sim.data.get_site_xvelr('object1') * dt # gripper state object1_rel_pos = object1_pos - grip_pos object1_velp -= grip_velp # object 2 object2_pos = self.sim.data.get_site_xpos('object2') if self.bias: object2_pos[0] += 0.05 # rotations object2_rot = rotations.mat2euler(self.sim.data.get_site_xmat('object2')) # velocities object2_velp = self.sim.data.get_site_xvelp('object2') * dt object2_velr = self.sim.data.get_site_xvelr('object2') * dt # gripper state object2_rel_pos = object2_pos - grip_pos object2_velp -= grip_velp else: object0_pos = object0_rot = object0_velp = object0_velr = object0_rel_pos = np.zeros(0) object1_pos = object1_rot = object1_velp = object1_velr = object1_rel_pos = np.zeros(0) object2_pos = object2_rot = object2_velp = object2_velr = object2_rel_pos = np.zeros(0) gripper_state = robot_qpos[-2:] gripper_vel = robot_qvel[-2:] * dt # change to a scalar if the gripper is made symmetric # if self.n_tasks == 7: # obs = np.concatenate([grip_pos, # object0_pos.ravel(), object1_pos.ravel(), object2_pos.ravel(), self.object3_xpos.squeeze(), self.object4_xpos.squeeze(), # object0_rel_pos.ravel(), object1_rel_pos.ravel(), object2_rel_pos.ravel(), # object0_rot.ravel(), object1_rot.ravel(), object2_rot.ravel(), # object0_velp.ravel(), object1_velp.ravel(), object2_velp.ravel(), # object0_velr.ravel(), object1_velr.ravel(), object2_velr.ravel(), # grip_velp, gripper_vel, gripper_state]) # else: obs = np.concatenate([grip_pos, object0_pos.ravel(), object1_pos.ravel(), object2_pos.ravel(), object0_rel_pos.ravel(), object1_rel_pos.ravel(), object2_rel_pos.ravel(), object0_rot.ravel(), object1_rot.ravel(), object2_rot.ravel(), object0_velp.ravel(), object1_velp.ravel(), object2_velp.ravel(), object0_velr.ravel(), object1_velr.ravel(), object2_velr.ravel(), grip_velp, gripper_vel, gripper_state]) self.last_obs = obs.copy() self._update_goals(obs) if not self.has_object: achieved_goal = grip_pos.copy() else: achieved_goal = self._compute_achieved_goal(obs.copy()) return { 'observation': obs.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy(), 'mask': self.mask }
def _get_obs(self): """ returns the observations dict """ # positions # grip_pos = self.sim.data.get_body_xpos('robot1:ee_link') # dt = self.sim.nsubsteps * self.sim.model.opt.timestep # grip_velp = self.sim.data.get_body_xvelp('robot1:ee_link') * dt grip_pos = self.sim.data.get_body_xpos('gripper_central') self.grip_pos = grip_pos dt = self.sim.nsubsteps * self.sim.model.opt.timestep grip_velp = self.sim.data.get_body_xvelp('gripper_central') * dt robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) if self.has_object: object_pos = self.sim.data.get_site_xpos('object0') # rotations object_rot = rotations.mat2euler(self.sim.data.get_site_xmat('object0')) # velocities object_velp = self.sim.data.get_site_xvelp('object0') * dt object_velr = self.sim.data.get_site_xvelr('object0') * dt # gripper state object_rel_pos = object_pos - grip_pos object_velp -= grip_velp elif self.has_cloth: #get the positions and velocities for 4 corners of the cloth vertices = ['CB0_0'] # Name vertices with respect to the cloth_length vertices.append('CB'+str(self.cloth_length-1)+'_'+'0') vertices.append('CB'+str(self.cloth_length-1)+'_'+str(self.cloth_length-1)) vertices.append('CB'+'0'+'_'+str(self.cloth_length-1)) vertice_pos, vertice_velp, vertice_velr, vertice_rel_pos = [], [], [], [] for vertice in vertices: vertice_pos.append(self.sim.data.get_body_xpos(vertice)) vertice_velp.append(self.sim.data.get_body_xvelp(vertice) * dt) #vertice_velr.append(self.sim.data.get_body_xvelr(vertice) * dt) #Do not need rotational velocities vertice_rel_pos = vertice_pos.copy() vertice_rel_pos -= grip_pos vertice_velp -= grip_velp else: object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.zeros(0) # if not using a fake gripper # gripper_state = robot_qpos[-2:] # gripper_vel = robot_qvel[-2:] * dt # change to a scalar if the gripper is made symmetric gripper_state = np.array([self.sim.model.eq_active[-1]]) # gripper_vel # Does not make sense for fake gripper if not self.has_object and not self.has_cloth: achieved_goal = grip_pos.copy() elif self.has_cloth and not self.has_object: if self.behavior=="diagonally": achieved_goal = np.squeeze(vertice_pos[0].copy()) elif self.behavior=="sideways": achieved_goal = np.concatenate([ vertice_pos[0].copy(), vertice_pos[1].copy(), ]) else: achieved_goal = np.squeeze(object_pos.copy()) # obs = np.concatenate([ # grip_pos, object_pos.ravel(), object_rel_pos.ravel(), gripper_state, object_rot.ravel(), # object_velp.ravel(), object_velr.ravel(), grip_velp, gripper_vel, # ]) # obs = np.concatenate([ # grip_pos, gripper_state, grip_velp, gripper_vel, vertice_pos[0], vertice_pos[1], vertice_pos[2], vertice_pos[3], # ]) # Creating dataset for Visual policy training self._render_callback() self.viewer = self._get_viewer('rgb_array') HEIGHT, WIDTH = 256, 256 self.viewer.render(HEIGHT, WIDTH) visual_data = self.viewer.read_pixels(HEIGHT, WIDTH, depth=False) # original image is upside-down, so flip it visual_data = cv2.UMat(visual_data[::-1, :, :]) # if self.visual_data_recording: # name = "/home/rjangir/Pictures/kinova_dataset/" + str(vertice_pos[0])+ str(vertice_pos[1]) + ".jpg" # cv2.imwrite(name, visual_data) #Save vertice labels vertice_pos_labels = [vertice_pos[0], vertice_pos[1], vertice_pos[2], vertice_pos[3]] label = [] #convert these points to 2D s = 1 # std for heatmap signal output_size = [HEIGHT, WIDTH] fov = 45 #field of view of camera cam_name = 'camera1' cam_pos = self.sim.data.get_camera_xpos(cam_name) cam_ori = gym.envs.robotics.rotations.mat2euler(self.sim.data.get_camera_xmat(cam_name)) for v in vertice_pos_labels: label.append(utils.global2label(v , cam_pos, cam_ori, output_size, fov=fov, s=s)) # for point in label: # cv2.circle(visual_data, (int(point[0]), int(point[1])), 3, (0, 0, 255), 5) if self.visual_data_recording: name = "/home/rjangir/workSpace/IRI-DL/datasets/sim2real/train/" + "image_" +str(self._index) + ".jpg" cv2.imwrite(name, visual_data) label_data = np.array([label[0], label[1], label[2], label[3]]) self._label_matrix.append(label_data) label_file = "/home/rjangir/workSpace/IRI-DL/datasets/sim2real/train/" + "train_ids" + ".npy" if np.asarray(self._label_matrix).shape[0] == 1000: print("saving the labels file") np.save(label_file, np.asarray(self._label_matrix), allow_pickle=True ) obs = np.concatenate([ grip_pos, gripper_state, grip_velp, vertice_pos[0], vertice_pos[1], vertice_pos[2], vertice_pos[3], vertice_velp[0], vertice_velp[1], vertice_velp[2], vertice_velp[3], ]) if self.explicit_policy: obs = np.concatenate([ obs, np.array(self.physical_params), ]) self._index += 1 return { 'observation': obs.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy(), }