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 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): 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 main(): rospy.init_node('simple_test') # Attempt to connect to simulator sim_client = utils.connectToSimulation('127.0.0.1', 19997) vrep.simxStopSimulation(sim_client, VREP_BLOCKING) time.sleep(1) vrep.simxStartSimulation(sim_client, VREP_BLOCKING) time.sleep(1) rdd = RDD(sim_client) ur5 = UR5(sim_client, rdd) # Generate a cube position = [-0.2, 0.9, 0.05] orientation = [0, 0, 0] # orientation = [0, 0, 0] size = [0.1, 0.2, 0.05] mass = 1 color = [255, 0, 0] cube = utils.generateShape(sim_client, 'cube', 0, size, position, orientation, mass, color) time.sleep(1) pose = ur5.getEndEffectorPose() target_pose = pose.copy() # rdd.openFinger(RDD.NARROW) narrow_finger_joint = rdd.finger_joint_narrow.joint def print_joint_pos(joint): while True: # print utils.getJointPosition(sim_client, joint) res, cube_pose = utils.getObjectPosition(sim_client, cube) while any(np.isnan(cube_pose)): res, cube_pose = utils.getObjectPosition(sim_client, cube) print cube_pose thread.start_new_thread(print_joint_pos, (narrow_finger_joint, )) target_pose[1, 3] += 0.3 rdd.openFinger(RDD.NARROW) ur5.moveTo(target_pose) ur5.moveTo(pose)
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()
def restartSimulation(sim_client): vrep.simxStopSimulation(sim_client, VREP_BLOCKING) time.sleep(1) vrep.simxStartSimulation(sim_client, VREP_BLOCKING) time.sleep(1)
def stopSimulation(sim_client): vrep.simxStopSimulation(sim_client, VREP_BLOCKING)