def step(self, a): """ take a step :param a: action, int :return: observation, reward, done, info """ if a in [self.RIGHT, self.LEFT]: current_position = self.target_position target_pose = transformations.euler_matrix( self.target_orientation[0], self.target_orientation[1], self.target_orientation[2]) target_pose[:3, -1] = current_position if a == self.RIGHT: target_pose[1, 3] -= 0.01 elif a == self.LEFT: target_pose[1, 3] += 0.01 utils.setObjectPositionOneShot(self.sim_client, self.ur5.UR5_target, target_pose[:3, 3]) elif a == self.CLOSE: self.rdd.setFingerPos(-0.1) elif a == self.OPEN: self.rdd.setFingerPos() cube_orientation = self.cube_orientation cube_position = self.cube_position tip_position = self.tip_position narrow_position = self.narrow_position target_position = self.target_position # arm is in wrong pose # sim_ret, target_position = utils.getObjectPosition(self.sim_client, self.ur5.UR5_target) if target_position[1] < 0.42 or target_position[ 1] > 0.95 or target_position[2] < 0 or target_position[2] > 0.2: print 'Wrong arm position: ', target_position return None, -1, True, None # cube in wrong position while any(np.isnan(cube_position)): res, cube_position = utils.getObjectPosition( self.sim_client, self.cube) if cube_position[0] < self.cube_start_position[0] - self.cube_size[0] or \ cube_position[0] > self.cube_start_position[0] + self.cube_size[0] or \ cube_position[1] < self.cube_start_position[1] - self.cube_size[1] or \ cube_position[1] > self.cube_start_position[1] + self.cube_size[1]: print 'Wrong cube position: ', cube_position return None, 0, True, None # cube is lifted if np.all(tip_position > (np.array(cube_position) - np.array(self.cube_size))) and \ np.all(tip_position < (np.array(cube_position) + np.array(self.cube_size))) and \ cube_orientation[0] < -0.02 and \ narrow_position > -0.5: return None, 1, True, None # cube is not lifted return self.getObs(), 0, False, None
def step(self, a): """ take a step :param a: action, int :return: observation, reward, done, info """ self.sendClearSignal() sim_ret, target_position = utils.getObjectPosition( self.sim_client, self.ur5.UR5_target) sim_ret, target_orientation = utils.getObjectOrientation( self.sim_client, self.ur5.UR5_target) target_pose = transformations.euler_matrix(target_orientation[0], target_orientation[1], target_orientation[2]) target_pose[:3, -1] = target_position if a == self.RIGHT: target_pose[1, 3] -= 0.05 elif a == self.LEFT: target_pose[1, 3] += 0.05 elif a == self.UP: target_pose[2, 3] += 0.03 elif a == self.DOWN: target_pose[2, 3] -= 0.03 self.ur5.moveTo(target_pose) sim_ret, cube_orientation = utils.getObjectOrientation( self.sim_client, self.cube) sim_ret, cube_position = utils.getObjectPosition( self.sim_client, self.cube) sim_ret, target_position = utils.getObjectPosition( self.sim_client, self.ur5.UR5_target) # arm is in wrong pose # sim_ret, target_position = utils.getObjectPosition(self.sim_client, self.ur5.UR5_target) if target_position[1] < 0.42 or target_position[ 1] > 0.95 or target_position[2] < 0 or target_position[2] > 0.2: # print 'Wrong arm position: ', target_position return None, -1, True, None # cube in wrong position while any(np.isnan(cube_position)): res, cube_position = utils.getObjectPosition( self.sim_client, self.cube) if cube_position[0] < self.cube_start_position[0] - self.cube_size[0] or \ cube_position[0] > self.cube_start_position[0] + self.cube_size[0] or \ cube_position[1] < self.cube_start_position[1] - self.cube_size[1] or \ cube_position[1] > self.cube_start_position[1] + self.cube_size[1]: # print 'Wrong cube position: ', cube_position return None, 0, True, None # cube is lifted if cube_orientation[0] < -0.05: return None, 1, True, None # cube is not lifted return self.getObs(), 0, False, None
def getObjectPose(sim_client, obj_handle): sim_ret, obj_position = getObjectPosition(sim_client, obj_handle) sim_ret, obj_orientation = getObjectOrientation(sim_client, obj_handle) obj_pose = transformations.euler_matrix(obj_orientation[0], obj_orientation[1], obj_orientation[2]) obj_pose[:3, -1] = np.asarray(obj_position) return sim_ret, obj_pose
def __init__(self, sim_client, sensor_name, workspace, intrinsics, get_rgb=True, get_depth=True, z_near=0.01, z_far=10): ''' VRep vision sensor class. Args: - sim_client: VRep client object to communicate with simulator over - sensor_name: Sensor name in simulator - workspace: Workspace for the vision sensor in robot coordinates - intrinsics: sensor intrinsics - get_rgb: Should the sensor get the rgb image - get_depth: Should the sensor get the depth image - z_near: Minimum distance for depth sensing - z_far: Maximum distance for depth sensing ''' self.sim_client = sim_client self.workspace = workspace self.intrinsics = intrinsics self.get_rgb = get_rgb self.get_depth = get_depth # Setup sensor and default sensor values sim_ret, self.sensor = utils.getObjectHandle(self.sim_client, sensor_name) sim_ret, pos = utils.getObjectPosition(self.sim_client, self.sensor) sim_ret, rot = utils.getObjectOrientation(self.sim_client, self.sensor) cam_trans = np.eye(4, 4) cam_trans[:3, 3] = np.asarray(pos) rotm = np.linalg.inv( transformations.euler_matrix(-rot[0], -rot[1], -rot[2])) self.pose = np.dot(cam_trans, rotm) self.z_near = z_near self.z_far = z_far