def reset(self): """ reset the environment :return: the observation, List[List[float], List[float]] """ vrep.simxStopSimulation(self.sim_client, utils.VREP_BLOCKING) time.sleep(1) vrep.simxStartSimulation(self.sim_client, utils.VREP_BLOCKING) time.sleep(1) sim_ret, self.cube = utils.getObjectHandle(self.sim_client, 'cube') self.rdd.setFingerPos(-0.1) utils.setObjectPosition(self.sim_client, self.ur5.UR5_target, [-0.2, 0.6, 0.08]) # utils.setObjectPosition(self.sim_client, self.ur5.UR5_target, [-0.2, 0.6, 0.15]) dy = 0.3 * np.random.random() # dz = 0.1 * np.random.random() - 0.05 current_pose = self.ur5.getEndEffectorPose() target_pose = current_pose.copy() target_pose[1, 3] += dy # target_pose[2, 3] += dz self.sendClearSignal() self.ur5.moveTo(target_pose) self.rdd.setFingerPos() return self.getObs()
def reset(self): """ reset the environment :return: the observation, List[List[float], List[float]] """ vrep.simxStopSimulation(self.sim_client, VREP_BLOCKING) time.sleep(1) vrep.simxStartSimulation(self.sim_client, VREP_BLOCKING) time.sleep(1) sim_ret, self.cube = utils.getObjectHandle(self.sim_client, 'cube') utils.setObjectPosition(self.sim_client, self.ur5.UR5_target, [-0.2, 0.6, 0.08]) dy = 0.3 * np.random.random() # dy = 0 # dz = 0.1 * np.random.random() - 0.05 current_pose = self.ur5.getEndEffectorPose() target_pose = current_pose.copy() target_pose[1, 3] += dy # target_pose[2, 3] += dz self.rdd.setFingerPos(-0.1) self.ur5.moveTo(target_pose) self.narrow_p = [] self.target_position = None while self.target_position is None: time.sleep(0.1) return [0. for _ in range(20)]
def reset(self): vrep.simxStopSimulation(self.sim_client, VREP_BLOCKING) time.sleep(1) vrep.simxStartSimulation(self.sim_client, VREP_BLOCKING) time.sleep(1) # Generate a cube # position = self.cube_start_position # orientation = [np.radians(90), 0, np.radians(90)] # # orientation = [0, 0, 0] # size = self.cube_size # mass = 0.1 # color = [255, 0, 0] # self.cube = utils.importShape(self.sim_client, 'cube', CUBE_MESH, position, orientation, color) # self.cube = utils.generateShape(self.sim_client, 'cube', 0, size, position, orientation, mass, color) # time.sleep(1) sim_ret, self.cube = utils.getObjectHandle(self.sim_client, 'cube') utils.setObjectPosition(self.sim_client, self.ur5.UR5_target, [-0.2, 0.6, 0.07]) dy = 0.3 * np.random.random() # dy = 0.3 current_pose = self.ur5.getEndEffectorPose() target_pose = current_pose.copy() target_pose[1, 3] += dy self.ur5.moveTo(target_pose) # time.sleep(0.5) self.rdd.openFinger(RDD.NARROW) return self.rdd.getFingerPosition(RDD.NARROW)
def step(self, a): if a in [self.N, self.S, self.W, self.E]: current_pose = self.ur5.getEndEffectorPose() target_pose = current_pose.copy() if a == self.N: target_pose[0, 3] += 0.01 elif a == self.S: target_pose[0, 3] -= 0.01 elif a == self.W: target_pose[1, 3] += 0.01 elif a == self.E: target_pose[1, 3] -= 0.01 utils.setObjectPosition(self.sim_client, self.ur5.UR5_target, target_pose[:3, 3]) elif a is self.CLOSE: closed = self.rdd.close() if not closed: sim_ret, cube_position = utils.getObjectPosition( self.sim_client, self.cube) sim_ret, tip_position = utils.getObjectPosition( self.sim_client, self.ur5.gripper_tip) 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))): return None, 1, True, None self.rdd.open(self.open_position) return self.getState(), 0, False, None
def reset(self): vrep.simxStopSimulation(self.sim_client, VREP_BLOCKING) time.sleep(1) vrep.simxStartSimulation(self.sim_client, VREP_BLOCKING) time.sleep(1) sim_ret, self.cube = utils.getObjectHandle(self.sim_client, 'cube') utils.setObjectPosition(self.sim_client, self.ur5.UR5_target, [-0.2, 0.6, 0.08]) dy = 0.3 * np.random.random() # dy = 0 # dz = 0.1 * np.random.random() - 0.05 current_pose = self.ur5.getEndEffectorPose() target_pose = current_pose.copy() target_pose[1, 3] += dy # target_pose[2, 3] += dz self.ur5.moveTo(target_pose) # self.rdd.open(self.open_position) self.target_position = None while self.target_position is None: time.sleep(0.1) return self.state
def step(self, a): if a in [self.RIGHT, self.LEFT]: current_pose = self.ur5.getEndEffectorPose() target_pose = current_pose.copy() if a == self.RIGHT: target_pose[1, 3] -= 0.01 elif a == self.LEFT: target_pose[1, 3] += 0.01 utils.setObjectPosition(self.sim_client, self.ur5.UR5_target, target_pose[:3, 3]) elif a == self.CLOSE: self.rdd.close() sim_ret, cube_orientation = utils.getObjectOrientation( self.sim_client, self.cube) sim_ret, cube_position = utils.getObjectPosition( self.sim_client, self.cube) sim_ret, tip_position = utils.getObjectPosition( self.sim_client, self.ur5.gripper_tip) 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 or cube_position[2] > self.cube_start_position[2] + 0.005): return None, 1, True, None self.rdd.open(self.open_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 else: # cube in wrong position sim_ret, cube_position = utils.getObjectPosition( self.sim_client, self.cube) 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 not lifted return self.state, 0, False, None
def step(self, a): current_pose = self.ur5.getEndEffectorPose() target_pose = current_pose.copy() if a == 0: target_pose[1, 3] -= 0.02 else: target_pose[1, 3] += 0.02 utils.setObjectPosition(self.sim_client, self.ur5.UR5_target, target_pose[:3, 3]) # utils.setObjectOrientation(self.sim_client, self.ur5.UR5_target, target_pose.flatten()[:-4]) # self.ur5.moveTo(target_pose) # time.sleep(0.5) # arm is in wrong pose sim_ret, tip_position = utils.getObjectPosition( self.sim_client, self.ur5.gripper_tip) # if np.linalg.norm(tip_position - target_pose[:3, -1]) > 0.05: # print 'Wrong position, dist: ', np.linalg.norm(tip_position - target_pose[:3, -1]) # return None, -1, True, None if tip_position[1] < 0.42 or tip_position[1] > 0.95: print 'Wrong arm position: ', tip_position return None, -1, True, None else: # cube is lifted sim_ret, cube_orientation = utils.getObjectOrientation( self.sim_client, self.cube) if cube_orientation[0] < -0.02: return None, 1, True, None # cube in wrong position sim_ret, cube_position = utils.getObjectPosition( self.sim_client, self.cube) while any(np.isnan(cube_position)): res, cube_position = utils.getObjectPosition( self.sim_client, self.cube) # if np.any(np.array(cube_position) < np.array(self.cube_start_position) - 0.5 * np.array(self.cube_size))\ # or np.any(np.array(cube_position) > np.array(self.cube_start_position) + 0.5 * np.array(self.cube_size)): 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 not lifted return self.rdd.getFingerPosition(RDD.NARROW), 0, False, None
def moveTo(self, pose, move_step_size=0.01, single_step=False): ''' Moves the tip of the gripper to the target pose Args: - pose: 4D target pose ''' # Get current position and orientation of UR5 target sim_ret, UR5_target_position = utils.getObjectPosition( self.sim_client, self.UR5_target) sim_ret, UR5_target_orientation = utils.getObjectOrientation( self.sim_client, self.UR5_target) # Calculate the movement increments move_direction = pose[:3, -1] - UR5_target_position move_magnitude = np.linalg.norm(move_direction) move_step = move_step_size * move_direction / move_magnitude num_move_steps = int(np.ceil(move_magnitude / move_step_size)) # Calculate the rotation increments rotation = np.asarray(transformations.euler_from_matrix(pose)) rotation_step = rotation - UR5_target_orientation rotation_step[rotation >= 0] = 0.1 rotation_step[rotation < 0] = -0.1 num_rotation_steps = np.ceil( (rotation - UR5_target_orientation) / rotation_step).astype(np.int) # Move and rotate to the target pose if not single_step: for i in range(max(num_move_steps, np.max(num_rotation_steps))): pos = UR5_target_position + move_step * min(i, num_move_steps) rot = [ UR5_target_orientation[0] + rotation_step[0] * min(i, num_rotation_steps[0]), UR5_target_orientation[1] + rotation_step[1] * min(i, num_rotation_steps[1]), UR5_target_orientation[2] + rotation_step[2] * min(i, num_rotation_steps[2]) ] utils.setObjectPosition(self.sim_client, self.UR5_target, pos) utils.setObjectOrientation(self.sim_client, self.UR5_target, rot) utils.setObjectPosition(self.sim_client, self.UR5_target, pose[:3, -1]) utils.setObjectOrientation(self.sim_client, self.UR5_target, rotation)
def reset(self): vrep.simxStopSimulation(self.sim_client, VREP_BLOCKING) time.sleep(1) vrep.simxStartSimulation(self.sim_client, VREP_BLOCKING) time.sleep(1) # Generate a cube position = self.cube_start_position # orientation = [np.radians(90), 0, np.radians(90)] orientation = [0, 0, 0] size = self.cube_size mass = 10 color = [255, 0, 0] self.cube = utils.generateShape(self.sim_client, 'cube', 0, size, position, orientation, mass, color) self.rdd.open(self.open_position) utils.setObjectPosition(self.sim_client, self.ur5.UR5_target, [-0.2, 0.5, 0.05]) # time.sleep(1) return self.getState()