def __init__(self, client_id, gripper_name): self.client_id = client_id self.name = gripper_name res, self.distance_handle = vrep.simxGetDistanceHandle( self.client_id, "touches", operationMode=vrep.simx_opmode_blocking) res, self.distance_handle = vrep.simxGetDistanceHandle( self.client_id, "touches", operationMode=vrep.simx_opmode_blocking)
def __init__(self, simulatorModel=None): self.robotOrientationFirstTime = True self.getDistanceToGoalFirstTime = True self.getUpDistanceFirstTime = True self.getObjectPositionFirstTime = True self.sysConf = LoadSystemConfiguration() #this data structure is like a cache for the joint handles self.jointHandleMapping = {} robotConf = LoadRobotConfiguration() self.model = simulatorModel self.LucyJoints = robotConf.getJointsName() for joint in self.LucyJoints: self.jointHandleMapping[joint]=0 self.clientId = self.connectVREP() RETRY_ATTEMPTS = 50 if simulatorModel: for i in range(RETRY_ATTEMPTS): #TODO try to reutilize the same scene for the sake of performance error = self.loadscn(self.clientId, simulatorModel) if not error: break print "retrying connection to vrep" if error: raise VrepException("error loading Vrep robot model", -1) if int(self.sysConf.getProperty("synchronous mode?"))==1: self.synchronous = True vrep.simxSynchronousTrigger(self.clientId) else: self.synchronous = False self.speedmodifier = int(self.sysConf.getProperty("speedmodifier")) #setting the simulation time step self.simulationTimeStepDT = float(self.sysConf.getProperty("simulation time step")) '''#testing printing in vrep error, consoleHandler = vrep.simxAuxiliaryConsoleOpen(self.clientId, "lucy debugging", 8, 22, None, None, None, None, vrep.simx_opmode_oneshot_wait) print "console handler: ", consoleHandler vrep.simxAuxiliaryConsoleShow(self.clientId, consoleHandler, 1, vrep.simx_opmode_oneshot_wait) error = vrep.simxAuxiliaryConsolePrint(self.clientId, consoleHandler, "Hello World", vrep.simx_opmode_oneshot_wait)''' error, self.upDistanceHandle = vrep.simxGetDistanceHandle(self.clientId, "upDistance#", vrep.simx_opmode_blocking) error, self.distLFootToGoalHandle = vrep.simxGetDistanceHandle(self.clientId, "distanceLFootGoal#", vrep.simx_opmode_blocking) error, self.distRFootToGoalHandle = vrep.simxGetDistanceHandle(self.clientId, "distanceRFootGoal#", vrep.simx_opmode_blocking) error, self.bioloidHandle = vrep.simxGetObjectHandle(self.clientId,"Bioloid", vrep.simx_opmode_oneshot_wait) error, self.cuboidHandle = vrep.simxGetObjectHandle(self.clientId,"Cuboid", vrep.simx_opmode_oneshot_wait) error, self.armHandler = vrep.simxGetObjectHandle(self.clientId, "Pivot", vrep.simx_opmode_oneshot_wait) self.populateJointHandleCache(self.clientId) self.isRobotUpFirstCall = True self.getDistanceToSceneGoal() #to fix the first invocation self.getUpDistance() self.isRobotUp(self.clientId) self.armPositionXAxis = -9.0000e-02
def __init__(self): # 建立通信 vrep.simxFinish(-1) # 每隔0.2s检测一次,直到连接上V-rep while True: self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) if self.clientID != -1: break else: time.sleep(0.1) print("Failed connecting to remote API server!") print("Connection success!") #设置机械臂仿真步长,为保持API端与VREP端相同步长 vrep.simxSetFloatingParameter(self.clientID, vrep.sim_floatparam_simulation_time_step, self.dt, vrep.simx_opmode_oneshot) # 打开同步模式 vrep.simxSynchronous(self.clientID, True) vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot) # 获取句柄joint self.robot1_jointHandle = np.zeros((self.jointNum, ), dtype=np.int) # joint 句柄 for i in range(self.jointNum): _, returnHandle = vrep.simxGetObjectHandle( self.clientID, self.jointName + str(i + 1), vrep.simx_opmode_blocking) self.robot1_jointHandle[i] = returnHandle # 获取末端frame _, self.end_handle = vrep.simxGetObjectHandle( self.clientID, 'end', vrep.simx_opmode_blocking) _, self.goal_handle = vrep.simxGetObjectHandle( self.clientID, 'goal_1', vrep.simx_opmode_blocking) # 障碍最短距离handle _, self.dist_handle = vrep.simxGetDistanceHandle( self.clientID, 'dis_robot1', vrep.simx_opmode_blocking) _, self.end_dis_handle = vrep.simxGetDistanceHandle( self.clientID, 'robot1_goal', vrep.simx_opmode_blocking) # 获取link句柄 self.link_handle = np.zeros((self.jointNum, ), dtype=np.int) #link 句柄 for i in range(self.jointNum): _, returnHandle = vrep.simxGetObjectHandle( self.clientID, self.linkName + str(i + 1), vrep.simx_opmode_blocking) self.link_handle[i] = returnHandle print('Handles available!')
def initialize_distance_calculation(self): rc, dist_handle_ = vrep.simxGetDistanceHandle(clientID, 'EE_to_target', blocking) if rc == 0: # print("Distance calculation started.") rc, dist_to_EE = vrep.simxReadDistance(clientID, dist_handle_, streaming) self.dist_handle = dist_handle_
def readDistance(self, name): if name not in self.distances: code, handle = vrep.simxGetDistanceHandle( self.id, name, vrep.simx_opmode_blocking) if code != 0: exit('Distance object "{}" not found'.format(name)) self.distances[name] = handle vrep.simxReadDistance(self.id, handle, vrep.simx_opmode_streaming) return None return vrep.simxReadDistance(self.id, self.distances[name], vrep.simx_opmode_buffer)[1]
def setup_devices(): """ Assign the devices from the simulator to specific IDs """ global robotID, left_motorID, right_motorID, ultraID, rewardRefID, goalID, wall0_collisionID, wall1_collisionID, wall2_collisionID, wall3_collisionID, target_collisionID # res: result (1(OK), -1(error), 0(not called)) # robot res, robotID = vrep.simxGetObjectHandle(clientID, 'robot#', WAIT) # motors res, left_motorID = vrep.simxGetObjectHandle(clientID, 'leftMotor#', WAIT) res, right_motorID = vrep.simxGetObjectHandle(clientID, 'rightMotor#', WAIT) # ultrasonic sensors for idx, item in enumerate(config.ultra_distribution): res, ultraID[idx] = vrep.simxGetObjectHandle(clientID, item, WAIT) # reward reference distance object res, rewardRefID = vrep.simxGetDistanceHandle(clientID, 'Distance#', WAIT) # if res == vrep.simx_return_ok: # [debug] # print("vrep.simxGetDistanceHandle executed fine") # goal reference object res, goalID = vrep.simxGetObjectHandle(clientID, 'Dummy#', WAIT) # collision object res, target_collisionID = vrep.simxGetCollisionHandle( clientID, "targetCollision#", BLOCKING) res, wall0_collisionID = vrep.simxGetCollisionHandle( clientID, "wall0#", BLOCKING) res, wall1_collisionID = vrep.simxGetCollisionHandle( clientID, "wall1#", BLOCKING) res, wall2_collisionID = vrep.simxGetCollisionHandle( clientID, "wall2#", BLOCKING) res, wall3_collisionID = vrep.simxGetCollisionHandle( clientID, "wall3#", BLOCKING) # start up devices # wheels vrep.simxSetJointTargetVelocity(clientID, left_motorID, 0, STREAMING) vrep.simxSetJointTargetVelocity(clientID, right_motorID, 0, STREAMING) # pose vrep.simxGetObjectPosition(clientID, robotID, -1, MODE_INI) vrep.simxGetObjectOrientation(clientID, robotID, -1, MODE_INI) # reading-related function initialization according to the recommended operationMode for i in ultraID: vrep.simxReadProximitySensor(clientID, i, STREAMING) vrep.simxReadDistance(clientID, rewardRefID, STREAMING) vrep.simxReadCollision(clientID, wall0_collisionID, STREAMING) vrep.simxReadCollision(clientID, wall1_collisionID, STREAMING) vrep.simxReadCollision(clientID, wall2_collisionID, STREAMING) vrep.simxReadCollision(clientID, wall3_collisionID, STREAMING) vrep.simxReadCollision(clientID, target_collisionID, STREAMING)
def __init__(self): # 建立通信 vrep.simxFinish(-1) # 每隔0.2s检测一次,直到连接上V-rep while True: self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) if self.clientID != -1: break else: time.sleep(0.1) print("Failed connecting to remote API server!") print("Connection success!") _, collisionHandle = vrep.simxGetCollisionHandle(self.clientID, 'Collision', vrep.simx_opmode_blocking) _, self.end1_Handle = vrep.simxGetObjectHandle(self.clientID, 'end', vrep.simx_opmode_blocking) _, self.end2_Handle = vrep.simxGetObjectHandle(self.clientID, 'end0', vrep.simx_opmode_blocking) _, self.goal1_Handle = vrep.simxGetObjectHandle(self.clientID, 'goal_1', vrep.simx_opmode_blocking) _, self.goal2_Handle = vrep.simxGetObjectHandle(self.clientID, 'goal_2', vrep.simx_opmode_blocking) _, self.ball1Handle = vrep.simxGetObjectHandle(self.clientID, 'ball1', vrep.simx_opmode_blocking) _, self.ball2Handle = vrep.simxGetObjectHandle(self.clientID, 'ball2', vrep.simx_opmode_blocking) _,self.base1Handle = vrep.simxGetObjectHandle(self.clientID, 'base', vrep.simx_opmode_blocking) _,self.base2Handle = vrep.simxGetObjectHandle(self.clientID, 'base0', vrep.simx_opmode_blocking) _,self.distanceHandle=vrep.simxGetDistanceHandle(self.clientID, 'robots_dist', vrep.simx_opmode_blocking) _, self.collisionHandle = vrep.simxGetCollisionHandle(self.clientID, 'Collision', vrep.simx_opmode_blocking) print('Handles available!') # _, self.pos1 = vrep.simxGetObjectPosition(self.clientID,self.ball1Handle, -1, vrep.simx_opmode_blocking) # _, self.pos2 = vrep.simxGetObjectPosition(self.clientID, self.ball2Handle, -1, vrep.simx_opmode_blocking) self.pos1 = [-0.5750,0.2959,0.0750] # robot1末端初始位置 self.pos2 = [0.5750,-0.2959,0.0750] # robot2末端初始位置 _, self.goal_1 = vrep.simxGetObjectPosition(self.clientID, self.goal1_Handle,self.base1Handle, vrep.simx_opmode_blocking) _, self.goal_2 = vrep.simxGetObjectPosition(self.clientID, self.goal2_Handle,self.base2Handle, vrep.simx_opmode_blocking) del self.goal_1[2] del self.goal_2[2] self.goal_1 = np.array(self.goal_1) self.goal_2 = np.array(self.goal_2) # 获取 joint 句柄 self.robot1_jointHandle = np.zeros((self.jointNum,), dtype=np.int) # joint 句柄 self.robot2_jointHandle = np.zeros((self.jointNum,), dtype=np.int) # joint 句柄 for i in range(self.jointNum): _, returnHandle = vrep.simxGetObjectHandle(self.clientID, self.jointName + str(i + 1), vrep.simx_opmode_blocking) self.robot1_jointHandle[i] = returnHandle for i in range(self.jointNum): _, returnHandle = vrep.simxGetObjectHandle(self.clientID, self.jointName + str(i + 4), vrep.simx_opmode_blocking) self.robot2_jointHandle[i] = returnHandle
A_H = [ampitude_h[0] * math.pi / 180.0, ampitude_h[1] * math.pi / 180.0] A_V = [ampitude_v[0] * math.pi / 180.0, ampitude_v[1] * math.pi / 180.0] P_V = [phase_v[0] * math.pi / 180.0, phase_v[1] * math.pi / 180.0] P_H = [phase_h[0] * math.pi / 180.0, phase_h[1] * math.pi / 180.0] P_C = [phase_cam[0] * math.pi / 180.0, phase_cam[1] * math.pi / 180.0] s = 0 # if s=0 we use the first set of parameters, if s=1 we use the second set of parameters #error_code_cam,joint_cam_handle=vrep.simxGetObjectHandle(clientID,'snake_joint_cam',vrep.simx_opmode_oneshot_wait) joints_h_handles = np.zeros([num_snakes, num_units, 1]) joints_v_handles = np.zeros([num_snakes, num_units, 1]) distance_measurement_handles = np.zeros([num_snakes, 1]) for snake in range(num_snakes): error_code_dist, distance_measurement_handles[ snake] = vrep.simxGetDistanceHandle(clientID, 'snake' + str(snake) + '_goal', vrep.simx_opmode_blocking) for i in range(1, num_units + 1): error_code_h, joints_h_handles[snake, i - 1] = vrep.simxGetObjectHandle( clientID, 'snake_joint_h' + str(i) + '#' + str(snake), vrep.simx_opmode_oneshot_wait) error_code_v, joints_v_handles[snake, i - 1] = vrep.simxGetObjectHandle( clientID, 'snake_joint_v' + str(i) + '#' + str(snake), vrep.simx_opmode_oneshot_wait) if error_code_h != 0 or error_code_v != 0: # or error_code_cam!=0: print("Problem!") IPython.embed()
def train(load_model_path, save_model_path, number_training_steps, data_path, image_shape, ratio=1, batch_size=2, save_online_batch=False, save_online_batch_path=None): # Initialize connection connection = VrepConnection() connection.synchronous_mode() connection.start() # Load data data_file = h5py.File(data_path,"r") x = h5py_to_array(data_file['images'][:1000], image_shape) y = data_file['joint_vel'][:1000] datapoints = x.shape[0] # Initialize ratios online_batchsize = int(np.floor(1.0 * batch_size/(ratio + 1))) data_images_batchsize = int(batch_size - online_batchsize) current_online_batch = 0 x_batch = np.empty((batch_size,) + image_shape) y_batch = np.empty((batch_size, 6)) jointpos_array = np.empty((batch_size, 6)) nextpos_array = np.empty((batch_size, 6)) print "Batch size: ", batch_size print "Online batch size: ", online_batchsize print "Dataset batch size: ", data_images_batchsize # Load keras model model = load_model(load_model_path) # Use client id from connection clientID = connection.clientID # Get joint handles jhList = [-1, -1, -1, -1, -1, -1] for i in range(6): err, jh = vrep.simxGetObjectHandle(clientID, "Mico_joint" + str(i + 1), vrep.simx_opmode_blocking) jhList[i] = jh # Initialize joint position jointpos = np.zeros(6) for i in range(6): err, jp = vrep.simxGetJointPosition(clientID, jhList[i], vrep.simx_opmode_streaming) jointpos[i] = jp # Initialize vision sensor res, v1 = vrep.simxGetObjectHandle(clientID, "vs1", vrep.simx_opmode_oneshot_wait) err, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_streaming) vrep.simxGetPingTime(clientID) err, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_buffer) # Initialize distance handle err, distanceHandle = vrep.simxGetDistanceHandle(clientID, "tipToTarget", vrep.simx_opmode_blocking) err, distance_to_target = vrep.simxReadDistance(clientID, distanceHandle, vrep.simx_opmode_streaming) # Initialize online batch hdf5 file if save_online_batch: f = h5py.File(save_online_batch_path, "w") dset1 = f.create_dataset("images", (batch_size,) + image_shape, dtype=np.float32) dset2 = f.create_dataset("joint_pos", (batch_size, 6), dtype=np.float32) dset3 = f.create_dataset("next_pos", (batch_size, 6), dtype=np.float32) dset4 = f.create_dataset("distance", (batch_size, 1), dtype=np.float32) dset5 = f.create_dataset("joint_vel", (batch_size, 6), dtype=np.float32) # Step while IK movement has not begun returnCode, signalValue = vrep.simxGetIntegerSignal(clientID, "ikstart", vrep.simx_opmode_streaming) while (signalValue == 0): vrep.simxSynchronousTrigger(clientID) vrep.simxGetPingTime(clientID) returnCode, signalValue = vrep.simxGetIntegerSignal(clientID, "ikstart", vrep.simx_opmode_streaming) # Iterate over number of steps to train model online path_empty_counter = 0 step_counter = 0 while step_counter < number_training_steps: # 1. Obtain image from vision sensor and next path position from Lua script err, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_buffer) img = np.resize(image, (1,) + image_shape) # resize into proper shape for input to neural network img = img.astype(np.uint8) img = img.astype(np.float32) img /= 255 inputInts = [] inputFloats = [] inputStrings = [] inputBuffer = bytearray() err, _, next_pos, _, _ = vrep.simxCallScriptFunction(clientID, 'Mico', vrep.sim_scripttype_childscript, 'getNextPathPos', inputInts, inputFloats, inputStrings, inputBuffer, vrep.simx_opmode_blocking) # 2. Pass into neural network to get joint velocities jointvel = model.predict(img, batch_size=1)[0] # output is a 2D array of 1X6, access the first variable to get vector stepsize = 1 jointvel *= stepsize # 3. Apply joint velocities to arm in V-REP for j in range(6): err, jp = vrep.simxGetJointPosition(clientID, jhList[j], vrep.simx_opmode_buffer) jointpos[j] = jp err = vrep.simxSetJointPosition(clientID, jhList[j], jointpos[j] + jointvel[j], vrep.simx_opmode_oneshot) err, distance_to_target = vrep.simxReadDistance(clientID, distanceHandle, vrep.simx_opmode_buffer) # Check if next pos is valid before fitting if len(next_pos) != 6: path_empty_counter += 1 continue ik_jointvel = joint_difference(jointpos, next_pos) ik_jointvel = ik_jointvel / np.sum(np.absolute(ik_jointvel)) * 0.5 * distance_to_target ik_jointvel = np.resize(ik_jointvel, (1, 6)) x_batch[current_online_batch] = img[0] y_batch[current_online_batch] = ik_jointvel[0] jointpos_array[current_online_batch] = jointpos nextpos_array[current_online_batch] = next_pos dset4[current_online_batch] = distance_to_target current_online_batch += 1 if current_online_batch == online_batchsize: # Step counter print "Training step: ", step_counter step_counter += 1 # Random sample from dataset if ratio > 0: indices = random.sample(range(int(datapoints)), int(data_images_batchsize)) indices = sorted(indices) x_batch[online_batchsize:] = x[indices] y_batch[online_batchsize:] = y[indices] dset4[online_batchsize:] = data_file["distance"][indices] dset2[online_batchsize:] = data_file["joint_pos"][indices] # 4. Fit model model.fit(x_batch, y_batch, batch_size=batch_size, epochs=1) # Reset counter current_online_batch = 0 # Save to online batch dataset dset1[:] = x_batch dset5[:] = y_batch dset2[:online_batchsize] = jointpos_array[:online_batchsize] dset3[:] = nextpos_array # Print statements # print "Predicted joint velocities: ", jointvel, "Abs sum: ", np.sum(np.absolute(jointvel)) # print "IK Joint velocity: ", ik_jointvel, "Abs sum: ", np.sum(np.absolute(ik_jointvel)) # print "Distance to cube: ", distanceToCube # trigger next step and wait for communication time vrep.simxSynchronousTrigger(clientID) vrep.simxGetPingTime(clientID) # stop the simulation: vrep.simxStopSimulation(clientID,vrep.simx_opmode_blocking) # Now close the connection to V-REP: vrep.simxFinish(clientID) # save updated model and delete model model.save(save_model_path) del model # Close file if save_online_batch: f.close() # Error check print "No of times path is empty:", path_empty_counter
vrep.simxFinish(-1) # just in case, close all opened connections clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # Connect to V-REP ErrorCode, steeringLeft = vrep.simxGetObjectHandle( clientID, 'nakedCar_steeringLeft', vrep.simx_opmode_oneshot_wait) ErrorCode, steeringRight = vrep.simxGetObjectHandle( clientID, 'nakedCar_steeringRight', vrep.simx_opmode_oneshot_wait) ErrorCode, motorLeft = vrep.simxGetObjectHandle(clientID, 'nakedCar_motorLeft', vrep.simx_opmode_oneshot_wait) ErrorCode, motorRight = vrep.simxGetObjectHandle(clientID, 'nakedCar_motorRight', vrep.simx_opmode_oneshot_wait) ErrorCode, vision_sensor = vrep.simxGetObjectHandle( clientID, 'Vision_sensor', vrep.simx_opmode_oneshot_wait) ErrorCode, leftdis = vrep.simxGetDistanceHandle(clientID, 'left_dist', vrep.simx_opmode_oneshot_wait) ErrorCode, rightdis = vrep.simxGetDistanceHandle(clientID, 'right_dist', vrep.simx_opmode_oneshot_wait) ErrorCode, totaldis = vrep.simxGetDistanceHandle(clientID, 'total_dist', vrep.simx_opmode_oneshot_wait) ErrorCode, car_size = vrep.simxGetDistanceHandle(clientID, 'car_size', vrep.simx_opmode_oneshot_wait) ErrorCode, disL = vrep.simxReadDistance(clientID, leftdis, vrep.simx_opmode_streaming) ErrorCode, disR = vrep.simxReadDistance(clientID, rightdis, vrep.simx_opmode_streaming) ErrorCode, dt = vrep.simxReadDistance(clientID, totaldis, vrep.simx_opmode_streaming) desiredWheelRotSpeed = 10 r = 0.15
err, init_cube_pos = vrep.simxGetObjectPosition(clientID, cube, -1, vrep.simx_opmode_blocking) # Get joint handles joint_handles = [-1, -1, -1] for i in range(3): err, jh = vrep.simxGetObjectHandle(clientID, "joint" + str(i + 1), vrep.simx_opmode_blocking) joint_handles[i] = jh # Get vision sensor handle err, vs_diag = vrep.simxGetObjectHandle(clientID, "vs_diag", vrep.simx_opmode_oneshot_wait) # Get distance handle err, distance_handle = vrep.simxGetDistanceHandle(clientID, "tip_to_target", vrep.simx_opmode_blocking) if mode == 0: # Open textfile textfile = open(textfile_path, "w") errorfile = open(error_text, "w") errorfile.write("cx cy cz j1 j2 j3\n") err_count = 0 success_count = 0 if pos_type == "random": while success_count < epochs: print("Epoch: {}/{}".format(success_count + 1, epochs)) # Reset cube pos cube_x_pos = np.random.choice([-1.0, 1.0]) * np.random.uniform(
def __init__(self, ip, port, num_robots, max_velocity, num_campos, tempo_por_partida): self.clientID = vrep.simxStart(ip, port, True, True, 2000, 5) if self.clientID == -1: exit() self.num_robots = num_robots self.robotObjectHandle = [0] * self.num_robots * num_campos * 2 self.robotOrientationtHandle = [0] * self.num_robots * num_campos * 2 self.leftMotorHandle = [0] * self.num_robots * num_campos * 2 self.rightMotorHandle = [0] * self.num_robots * num_campos * 2 self.leftRodaHandle = [0] * self.num_robots * num_campos * 2 self.rightRodaHandle = [0] * self.num_robots * num_campos * 2 self.golDistHandle = [0] * num_campos * 2 self.golObjectHandle = [0] * num_campos * 2 for i in range(0, num_campos * 2, 2): resGolT0, self.golObjectHandle[i] = vrep.simxGetObjectHandle( self.clientID, "gol" + str(i), vrep.simx_opmode_oneshot_wait) resGolT1, self.golObjectHandle[i + 1] = vrep.simxGetObjectHandle( self.clientID, "gol" + str(i + 1), vrep.simx_opmode_oneshot_wait) resGolTime0, self.golDistHandle[i] = vrep.simxGetDistanceHandle( self.clientID, "Dist_bola_gol" + str(i), vrep.simx_opmode_oneshot_wait) resGolTime1, self.golDistHandle[i + 1] = vrep.simxGetDistanceHandle( self.clientID, "Dist_bola_gol" + str(i + 1), vrep.simx_opmode_oneshot_wait) if resGolTime0 != vrep.simx_return_ok or resGolTime1 != vrep.simx_return_ok or resGolT0 != vrep.simx_return_ok or resGolT0 != vrep.simx_return_ok: exit() resBall, self.ballObjectHandle = vrep.simxGetObjectHandle( self.clientID, "bola", vrep.simx_opmode_oneshot_wait) if resBall: exit() for i in range(0, self.num_robots * 2 * num_campos * 2, 2): resBase, self.robotObjectHandle[int( i / 2)] = vrep.simxGetObjectHandle( self.clientID, "robo" + str(int(i / 2)), vrep.simx_opmode_oneshot_wait) resOrien, self.robotOrientationtHandle[int( i / 2)] = vrep.simxGetObjectHandle( self.clientID, "orientacao" + str(int(i / 2)), vrep.simx_opmode_oneshot_wait) resLeft, self.leftMotorHandle[int( i / 2)] = vrep.simxGetObjectHandle( self.clientID, "motor" + str(i + 1), vrep.simx_opmode_oneshot_wait) resRight, self.rightMotorHandle[int( i / 2)] = vrep.simxGetObjectHandle( self.clientID, "motor" + str(i), vrep.simx_opmode_oneshot_wait) resLeftR, self.leftRodaHandle[int( i / 2)] = vrep.simxGetObjectHandle( self.clientID, "Cylinder" + str(i + 1), vrep.simx_opmode_oneshot_wait) resRightR, self.rightRodaHandle[int( i / 2)] = vrep.simxGetObjectHandle( self.clientID, "Cylinder" + str(i), vrep.simx_opmode_oneshot_wait) if resLeft != vrep.simx_return_ok or resRight != vrep.simx_return_ok or resBase != vrep.simx_return_ok or resLeftR != vrep.simx_return_ok or resRightR != vrep.simx_return_ok: exit() self.state_size = self.num_robots * 2 * 2 self.action_size = self.num_robots * 9 self.max_velocity = max_velocity self.max_width = 0.9 self.max_height = 0.64 self.raio_robo = 0.1562 self.z = 0.0382 self.min_dist_gol = 0.13 self.num_campos = num_campos self.timers = [0.] * num_campos self.max_time = tempo_por_partida self.campo = [False] * self.num_campos self.semafaro = [False] * self.num_campos self.team = [0] * (self.num_campos * 2) self.time_step_action = 0.01 self.weight_rewards = [0.17, 0.17, 0.33, 0.33] self.actions = [(-1, -1), (-1, 0), (-1, 1), (0, -1), (0, 0), (0, 1), (1, -1), (1, 0), (1, 1)] * self.num_robots
def __enter__(self): print('[ROBOTENV] Starting environment...') sync_mode_str = 'TRUE' if self.sync_mode else 'FALSE' portNb_str = str(self.portNb) # launch v-rep vrep_cmd = [ self.vrepPath, '-gREMOTEAPISERVERSERVICE_' + portNb_str + '_FALSE_' + sync_mode_str ] if not self.showGUI: vrep_cmd.append('-h') # headless mode vrep_cmd.append(self.scenePath) # headless mode via ssh # vrep_cmd = "xvfb-run --auto-servernum --server-num=1 /homes/dam416/V-REP_PRO_EDU_V3_4_0_Linux/vrep.sh -h -s -q MicoRobot.ttt" # vrep_cmd = ['xvfb-run', '--auto-servernum', '--server-num=1', self.vrepPath, '-h', '-s', '-q', self.scenePath] # vrep_cmd = ['xvfb-run', '--auto-servernum', '--server-num=1', self.vrepPath, '-h', self.scenePath] print('[ROBOTENV] Launching V-REP...') # NOTE: do not use "stdout=subprocess.PIPE" below to buffer logs, causes deadlock at episode 464! (flushing the buffer may work... but buffering is not needed) self.vrepProcess = subprocess.Popen(vrep_cmd, shell=False, preexec_fn=os.setsid) # connect to V-Rep Remote Api Server vrep.simxFinish(-1) # close all opened connections # Connect to V-REP print('[ROBOTENV] Connecting to V-REP...') counter = 0 while True: self.clientID = vrep.simxStart('127.0.0.1', self.portNb, True, False, 5000, 0) if self.clientID != -1: break else: print("[ROBOTENV] Connection failed, retrying") counter += 1 if counter == 10: raise RuntimeError( '[ROBOTENV] Connection to V-REP failed.') if self.clientID == -1: print('[ROBOTENV] Failed connecting to remote API server') else: print('[ROBOTENV] Connected to remote API server') # close model browser and hierarchy window vrep.simxSetBooleanParameter(self.clientID, vrep.sim_boolparam_browser_visible, False, vrep.simx_opmode_blocking) vrep.simxSetBooleanParameter(self.clientID, vrep.sim_boolparam_hierarchy_visible, False, vrep.simx_opmode_blocking) vrep.simxSetBooleanParameter(self.clientID, vrep.sim_boolparam_console_visible, False, vrep.simx_opmode_blocking) # load scene # time.sleep(5) # to avoid errors # returnCode = vrep.simxLoadScene(self.clientID, self.scenePath, 1, vrep.simx_opmode_oneshot_wait) # vrep.simx_opmode_blocking is recommended # # Start simulation # # vrep.simxSetIntegerSignal(self.clientID, 'dummy', 1, vrep.simx_opmode_blocking) # # time.sleep(5) #to center window for recordings # if self.initial_joint_positions is not None: # self.initializeJointPositions(self.initial_joint_positions) # self.startSimulation() # # returnCode = vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking) # # printlog('simxStartSimulation', returnCode) # get handles and start streaming distance to goal for i in range(0, self.nSJoints): returnCode, self.jointHandles[i] = vrep.simxGetObjectHandle( self.clientID, 'Mico_joint' + str(i + 1), vrep.simx_opmode_blocking) printlog('simxGetObjectHandle', returnCode) returnCode, self.fingersH1 = vrep.simxGetObjectHandle( self.clientID, 'MicoHand_fingers12_motor1', vrep.simx_opmode_blocking) returnCode, self.fingersH2 = vrep.simxGetObjectHandle( self.clientID, 'MicoHand_fingers12_motor2', vrep.simx_opmode_blocking) returnCode, self.goalCubeH = vrep.simxGetObjectHandle( self.clientID, 'GoalCube', vrep.simx_opmode_blocking) returnCode, self.targetCubePositionH = vrep.simxGetObjectHandle( self.clientID, 'GoalPosition', vrep.simx_opmode_blocking) returnCode, self.robotBaseH = vrep.simxGetObjectHandle( self.clientID, 'Mico_link1_visible', vrep.simx_opmode_blocking) returnCode, self.jointsCollectionHandle = vrep.simxGetCollectionHandle( self.clientID, "sixJoints#", vrep.simx_opmode_blocking) returnCode, self.distToGoalHandle = vrep.simxGetDistanceHandle( self.clientID, "distanceToGoal#", vrep.simx_opmode_blocking) returnCode, self.endEffectorH = vrep.simxGetObjectHandle( self.clientID, "DummyFinger#", vrep.simx_opmode_blocking) # returnCode, self.distanceToGoal = vrep.simxReadDistance(self.clientID, self.distToGoalHandle, vrep.simx_opmode_streaming) #start streaming # returnCode, _, _, floatData, _ = vrep.simxGetObjectGroupData(self.clientID, self.jointsCollectionHandle, 15, vrep.simx_opmode_streaming) #start streaming if self.targetCubePosition is not None: # hide goal position plane visibility_layer_param_ID = 10 visible_layer = 1 returnCode = vrep.simxSetObjectIntParameter( self.clientID, self.targetCubePositionH, visibility_layer_param_ID, visible_layer, vrep.simx_opmode_blocking) # print('simxSetObjectIntParameter return Code: ', returnCode) self.initializeGoalPosition() # Start simulation if self.initial_joint_positions is not None: self.initializeJointPositions() self.reset() # self.startSimulation() # # returnCode = vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking) # # printlog('simxStartSimulation', returnCode) # self.updateState() # default initial state: 180 degrees (=pi radians) for all angles # check the state is valid # while True: # self.updateState() # print("\nstate: ", self.state) # print("\nreward: ", self.reward) # # wait for a state # if (self.state.shape == (1,self.observation_space_size) and abs(self.reward) < 1E+5): # print("sent reward3=", self.reward) # break print( '[ROBOTENV] Environment succesfully initialised, ready for simulations' ) # while vrep.simxGetConnectionId(self.clientID) != -1: # ##########################EXECUTE ACTIONS AND UPDATE STATE HERE ################################# # time.sleep(0.1) # #end of execution loop return self
# Ultrasonic Sensor #errorCode, sensor1 = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_ultrasonicSensor16", vrep.simx_opmode_blocking) #returnCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor(clientID, sensor1, vrep.simx_opmode_streaming) # Gyro sensor #errorCode, GyroSensor = vrep.simxGetObjectHandle(clientID, "GyroSensor_reference", vrep.simx_opmode_blocking) # Accelerometer #errorCode, AccelSensor = vrep.simxGetObjectHandle(clientID, "Accelerometer_mass", vrep.simx_opmode_blocking) # GPS #errorCode, GPS = vrep.simxGetObjectHandle(clientID, "GPS_reference", vrep.simx_opmode_blocking) #get Distance Object Handle errorCode, dist_handle = vrep.simxGetDistanceHandle(clientID, "Distance", vrep.simx_opmode_blocking) # Create a workbook and add a worksheet. workbook = xlsxwriter.Workbook('data.xlsx') worksheet = workbook.add_worksheet() # Some data we want to write to the worksheet. defaults = [ 'X-posi', 'Y-posi', 'Z-posi', 'gyroX', 'gyroY', 'gyroZ', 'accelX', 'accelY', 'accelZ', 'alpha', 'beta', 'gamma', 'vx', 'vy', 'vz', 'Time', 'dis' ] t = 0 # Iterate over the data and write it out row by row. for i, default in enumerate(defaults):
#Instancia objetos no Python para os handlers codErro, ref = vrep.simxGetObjectHandle(clientID, 'ref', operationMode=vrep.simx_opmode_oneshot) #Destino codErro, target = vrep.simxGetObjectHandle( clientID, 'target', operationMode=vrep.simx_opmode_oneshot) #Medidores de colisão codErro, pioneerColDetect = vrep.simxGetCollisionHandle( clientID, 'pioneerColDetect', operationMode=vrep.simx_opmode_blocking) #Medidores de distância codErro, roboObstDist = vrep.simxGetDistanceHandle( clientID, 'roboObstDistDetect', operationMode=vrep.simx_opmode_blocking) codErro, roboTargetDist = vrep.simxGetDistanceHandle( clientID, 'roboTargetDistDetect', operationMode=vrep.simx_opmode_blocking) codErro, obstaculos = vrep.simxGetCollectionHandle( clientID, 'obstaculos#', operationMode=vrep.simx_opmode_oneshot) codErro, robo = vrep.simxGetObjectHandle( clientID, 'Pionee_p3dx', operationMode=vrep.simx_opmode_oneshot) codErro, motorE = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_blocking) codErro, motorD = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_blocking) codErro, colisao = vrep.simxReadCollision( clientID, pioneerColDetect, operationMode=vrep.simx_opmode_streaming)
def control(load_model_path, total_episodes=10, im_function=False): # Initialize connection connection = VrepConnection() connection.synchronous_mode() connection.start() # Load keras model model = load_model(load_model_path, custom_objects={'empty': empty, 'mmd2_rbf_quad': empty}) # Initialize adam optimizer adam = keras.optimizers.adam(lr=0.001) model.compile(loss='mean_squared_error', optimizer=adam, metrics=['mae']) # Use client id from connection clientID = connection.clientID # Get joint handles jhList = [-1, -1, -1, -1, -1, -1] for i in range(6): err, jh = vrep.simxGetObjectHandle(clientID, "Mico_joint" + str(i + 1), vrep.simx_opmode_blocking) jhList[i] = jh # Initialize joint position jointpos = np.zeros(6) for i in range(6): err, jp = vrep.simxGetJointPosition(clientID, jhList[i], vrep.simx_opmode_streaming) jointpos[i] = jp # Initialize vision sensor res, v1 = vrep.simxGetObjectHandle(clientID, "vs1", vrep.simx_opmode_oneshot_wait) err, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_streaming) vrep.simxGetPingTime(clientID) err, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_buffer) # Initialize distance handle err, distanceToCubeHandle = vrep.simxGetDistanceHandle(clientID, "tipToCube", vrep.simx_opmode_blocking) err, distanceToCube = vrep.simxReadDistance(clientID, distanceToCubeHandle, vrep.simx_opmode_streaming) err, distanceToTargetHandle = vrep.simxGetDistanceHandle(clientID, "tipToTarget", vrep.simx_opmode_blocking) err, distanceToTarget = vrep.simxReadDistance(clientID, distanceToTargetHandle, vrep.simx_opmode_streaming) # numpy print options np.set_printoptions(precision=5) # Step while IK movement has not begun returnCode, signalValue = vrep.simxGetIntegerSignal(clientID, "ikstart", vrep.simx_opmode_streaming) while (signalValue == 0): vrep.simxSynchronousTrigger(clientID) vrep.simxGetPingTime(clientID) returnCode, signalValue = vrep.simxGetIntegerSignal(clientID, "ikstart", vrep.simx_opmode_streaming) # Iterate over total steps desired image_shape = (128, 128, 3) current_episode = 0 step_counter = 0 while current_episode < total_episodes + 1: # obtain current episode inputInts = [] inputFloats = [] inputStrings = [] inputBuffer = bytearray() episode_table = [] while len(episode_table) == 0: err, episode_table, _, _, _ = vrep.simxCallScriptFunction(clientID, 'Mico', vrep.sim_scripttype_childscript, 'episodeCount', inputInts, inputFloats, inputStrings, inputBuffer, vrep.simx_opmode_blocking) if episode_table[0] > current_episode: step_counter = 0 print "Episode: ", episode_table[0] current_episode = episode_table[0] step_counter += 1 # 1. Obtain image from vision sensor err, resolution, img = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_buffer) img = np.array(img, dtype=np.uint8) img = np.resize(img, (1,) + image_shape) # resize into proper shape for input to neural network img = img.astype('float32') img = img / 255 # normalize input image original_img = img if im_function: #img = adjust_gamma(img, gamma=2) img = tint_images(img, [1,1,0]) if (current_episode==1) and (step_counter==1): print "Displaying before and after image transformation" display_2images(original_img[0], img[0]) plt.show() # 2. Pass into neural network to get joint velocities jointvel = model.predict(img, batch_size=1)[0][0] # output is a 2D array of 1X6, access the first variable to get vector stepsize = 1 jointvel *= stepsize # 3. Apply joint velocities to arm in V-REP for j in range(6): err, jp = vrep.simxGetJointPosition(clientID, jhList[j], vrep.simx_opmode_buffer) jointpos[j] = jp err = vrep.simxSetJointPosition(clientID, jhList[j], jointpos[j] + jointvel[j], vrep.simx_opmode_oneshot) # Obtain distance err, distanceToCube = vrep.simxReadDistance(clientID, distanceToCubeHandle, vrep.simx_opmode_buffer) err, distanceToTarget = vrep.simxReadDistance(clientID, distanceToTargetHandle, vrep.simx_opmode_buffer) # Print statements # print "Step: ", step_counter print "Joint velocities: ", jointvel, " Abs sum: %.3f" % np.sum(np.absolute(jointvel)) # print "Distance to cube: %.3f" % distanceToCube, ", Distance to target: %.3f" % distanceToTarget # trigger next step and wait for communication time vrep.simxSynchronousTrigger(clientID) vrep.simxGetPingTime(clientID) # obtain performance metrics inputInts = [] inputFloats = [] inputStrings = [] inputBuffer = bytearray() err, minDistStep, minDist, _, _ = vrep.simxCallScriptFunction(clientID, 'Mico', vrep.sim_scripttype_childscript, 'performanceMetrics', inputInts, inputFloats, inputStrings, inputBuffer, vrep.simx_opmode_blocking) if res == vrep.simx_return_ok: print "Min distance: ", minDist minThreshold = 0.2 print "Success rate: ", 1.0*np.sum(np.array(minDist) <= minThreshold)/len(minDist) print "Total episodes: ", len(minDist) print "Average min distance: %.3f" % np.mean(minDist) print "Standard deviation: %.3f" % np.std(minDist) # other performance metrics such as success % can be defined (i.e. % reaching certain min threshold) # stop the simulation: vrep.simxStopSimulation(clientID,vrep.simx_opmode_blocking) # Now close the connection to V-REP: vrep.simxFinish(clientID) # delete model del model return
def generate(save_path, number_episodes, steps_per_episode, jointvel_factor=0.5): # Initialize connection connection = VrepConnection() connection.synchronous_mode() connection.start() # Use client id from connection clientID = connection.clientID # Get joint handles jhList = [-1, -1, -1, -1, -1, -1] for i in range(6): err, jh = vrep.simxGetObjectHandle(clientID, "Mico_joint" + str(i + 1), vrep.simx_opmode_blocking) jhList[i] = jh # Initialize joint position jointpos = np.zeros(6) for i in range(6): err, jp = vrep.simxGetJointPosition(clientID, jhList[i], vrep.simx_opmode_streaming) jointpos[i] = jp # Initialize vision sensor res, v1 = vrep.simxGetObjectHandle(clientID, "vs1", vrep.simx_opmode_oneshot_wait) err, resolution, image = vrep.simxGetVisionSensorImage( clientID, v1, 0, vrep.simx_opmode_streaming) vrep.simxGetPingTime(clientID) err, resolution, image = vrep.simxGetVisionSensorImage( clientID, v1, 0, vrep.simx_opmode_buffer) # Initialize distance handle err, distance_handle = vrep.simxGetDistanceHandle( clientID, "tipToTarget", vrep.simx_opmode_blocking) err, distance_to_target = vrep.simxReadDistance(clientID, distance_handle, vrep.simx_opmode_streaming) # Initialize data file f = h5py.File(save_path, "w") episode_count = 0 total_datapoints = number_episodes * steps_per_episode size_of_image = resolution[0] * resolution[1] * 3 dset1 = f.create_dataset("images", (total_datapoints, size_of_image), dtype="uint") dset2 = f.create_dataset("joint_pos", (total_datapoints, 6), dtype="float") dset3 = f.create_dataset("joint_vel", (total_datapoints, 6), dtype="float") dset4 = f.create_dataset("distance", (total_datapoints, 1), dtype="float") # Step while IK movement has not begun returnCode, signalValue = vrep.simxGetIntegerSignal( clientID, "ikstart", vrep.simx_opmode_streaming) while (signalValue == 0): vrep.simxSynchronousTrigger(clientID) vrep.simxGetPingTime(clientID) returnCode, signalValue = vrep.simxGetIntegerSignal( clientID, "ikstart", vrep.simx_opmode_streaming) for i in range(total_datapoints): # At start of each episode, check if path has been found in vrep if (i % steps_per_episode) == 0: returnCode, signalValue = vrep.simxGetIntegerSignal( clientID, "ikstart", vrep.simx_opmode_streaming) while (signalValue == 0): vrep.simxSynchronousTrigger(clientID) vrep.simxGetPingTime(clientID) returnCode, signalValue = vrep.simxGetIntegerSignal( clientID, "ikstart", vrep.simx_opmode_streaming) episode_count += 1 print "Episode: ", episode_count # obtain image and place into array err, resolution, image = vrep.simxGetVisionSensorImage( clientID, v1, 0, vrep.simx_opmode_buffer) img = np.array(image, dtype=np.uint8) dset1[i] = img # obtain joint pos and place into array jointpos = np.zeros(6) for j in range(6): err, jp = vrep.simxGetJointPosition(clientID, jhList[j], vrep.simx_opmode_buffer) jointpos[j] = jp dset2[i] = jointpos # obtain distance and place into array distance = np.zeros(1) err, distance_to_target = vrep.simxReadDistance( clientID, distance_handle, vrep.simx_opmode_buffer) distance[0] = distance_to_target dset4[i] = distance # trigger next step and wait for communication time vrep.simxSynchronousTrigger(clientID) vrep.simxGetPingTime(clientID) # stop the simulation: vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking) # Now close the connection to V-REP: vrep.simxFinish(clientID) # calculate joint velocities excluding final image for k in range(number_episodes): for i in range(steps_per_episode - 1): #jointvel = dset2[k*steps_per_episode + i+1]-dset2[k*steps_per_episode + i] jointvel = joint_difference(dset2[k * steps_per_episode + i], dset2[k * steps_per_episode + i + 1]) abs_sum = np.sum(np.absolute(jointvel)) if abs_sum == 0: dset3[k * steps_per_episode + i] = np.zeros(6) else: # abs sum norm dset3[k * steps_per_episode + i] = jointvel / abs_sum * jointvel_factor * dset4[ k * steps_per_episode + i] # close datafile f.close() return
else: print("Nao conectado ao VRep!!!") sys.exit("Xau!!") #Instancia objetos no Python para os handlers codErro, ref = vrep.simxGetObjectHandle(clientID, 'ref', vrep.simx_opmode_oneshot) codErro, dummy = vrep.simxGetObjectHandle(clientID, 'dummy', vrep.simx_opmode_oneshot) codErro, pioneerColDetect = vrep.simxGetCollisionHandle( clientID, 'pioneerColDetect', vrep.simx_opmode_oneshot) codErro, pioneerDistDetect = vrep.simxGetDistanceHandle( clientID, 'pioneerDistDetect', vrep.simx_opmode_oneshot) # codErro, obstaculos = vrep.simxGetCollectionHandle(clientID, 'obstaculos', vrep.simx_opmode_oneshot) codErro, robo = vrep.simxGetObjectHandle(clientID, 'Pionee_p3dx', vrep.simx_opmode_oneshot) codErro, chao = vrep.simxGetObjectHandle(clientID, 'ResizableFloor_5_25', vrep.simx_opmode_oneshot) posicao = vrep.simxGetObjectPosition(clientID, robo, -1, vrep.simx_opmode_buffer) angulos = vrep.simxGetObjectOrientation(clientID, robo, -1, vrep.simx_opmode_buffer) #distancia = vrep.simxReadDistance(clientID, pioneerDistDetect, #operationMode=vrep.simx_opmode_blocking)
def __init__(self, client_id, handle=None, sensor_array=None): Sensor.__init__(self, client_id, handle=handle) res, self.handle = vrep.simxGetDistanceHandle( self.client_id, 'beacon', vrep.simx_opmode_blocking) self.last_read = self.read(vrep.simx_opmode_streaming, self.handle)
# time.sleep(5.0) # sys.exit(0) # get Handles jointHandles = [0] * 6 for i in range(0, 6): returnCode, jointHandles[i] = vrep.simxGetObjectHandle( clientID, 'Mico_joint' + str(i + 1), vrep.simx_opmode_blocking) if i == 0: printlog('simxGetObjectHandle', returnCode) returnCode, fingersH1 = vrep.simxGetObjectHandle( clientID, 'MicoHand_fingers12_motor1', vrep.simx_opmode_blocking) returnCode, fingersH2 = vrep.simxGetObjectHandle( clientID, 'MicoHand_fingers12_motor2', vrep.simx_opmode_blocking) returnCode, jointsCollectionHandle = vrep.simxGetCollectionHandle( clientID, "sixJoints#", vrep.simx_opmode_blocking) returnCode, distToGoalHandle = vrep.simxGetDistanceHandle( clientID, "distanceToGoal#", vrep.simx_opmode_blocking) returnCode, distanceToGoal = vrep.simxReadDistance( clientID, distToGoalHandle, vrep.simx_opmode_streaming) #start streaming print('jointHandles', jointHandles) #Arm pi = np.pi vel = 1 # looks fast in simulation! #some test target positions for the 6 joints, in radians targetPos0 = np.zeros(6) targetPosPi = np.array( [pi] * 6) #default initial state: pi or 180 degrees for all joints targetPosPiO2 = np.array([pi / 2] * 6) targetPos1 = np.array([pi / 2] * 6)
def __init__(self): #建立通信 vrep.simxFinish(-1) # 每隔0.2s检测一次,直到连接上V-rep while True: self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) if self.clientID != -1: break else: time.sleep(0.1) print("Failed connecting to remote API server!") print("Connection success!") # # 设置机械臂仿真步长,为了保持API端与V-rep端相同步长 vrep.simxSetFloatingParameter(self.clientID, vrep.sim_floatparam_simulation_time_step, self.dt,\ vrep.simx_opmode_oneshot) # # 然后打开同步模式 vrep.simxSynchronous(self.clientID, False) vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot) # vrep.simxSynchronousTrigger(self.clientID) #获取 joint 句柄 self.robot1_jointHandle = np.zeros((self.jointNum, ), dtype=np.int) # joint 句柄 self.robot2_jointHandle = np.zeros((self.jointNum, ), dtype=np.int) # joint 句柄 for i in range(self.jointNum): _, returnHandle = vrep.simxGetObjectHandle( self.clientID, self.jointName + str(i + 1), vrep.simx_opmode_blocking) self.robot1_jointHandle[i] = returnHandle for i in range(self.jointNum): _, returnHandle = vrep.simxGetObjectHandle( self.clientID, self.jointName + str(i + 4), vrep.simx_opmode_blocking) self.robot2_jointHandle[i] = returnHandle # 获取 Link 句柄 self.robot1_linkHandle = np.zeros((self.jointNum, ), dtype=np.int) # link 句柄 self.robot2_linkHandle = np.zeros((self.jointNum, ), dtype=np.int) # link 句柄 for i in range(self.jointNum): _, returnHandle = vrep.simxGetObjectHandle( self.clientID, self.linkName + str(i + 1), vrep.simx_opmode_blocking) self.robot1_linkHandle[i] = returnHandle for i in range(self.jointNum): _, returnHandle = vrep.simxGetObjectHandle( self.clientID, self.linkName + str(i + 4), vrep.simx_opmode_blocking) self.robot2_linkHandle[i] = returnHandle # 获取碰撞句柄 _, self.robot1_collisionHandle = vrep.simxGetCollisionHandle( self.clientID, 'Collision_robot1', vrep.simx_opmode_blocking) _, self.robot2_collisionHandle = vrep.simxGetCollisionHandle( self.clientID, 'Collision_robot2', vrep.simx_opmode_blocking) # 获取距离句柄 # _,self.mindist_robot1_Handle = vrep.simxGetDistanceHandle(self.clientID,'dis_robot1',vrep.simx_opmode_blocking) # _,self.mindist_robot2_Handle = vrep.simxGetDistanceHandle(self.clientID,'dis_robot2', vrep.simx_opmode_blocking) # _,self.mindist_robots_Handle = vrep.simxGetDistanceHandle(self.clientID,'robots_dist', vrep.simx_opmode_blocking) _, self.robot1_goal_Handle = vrep.simxGetDistanceHandle( self.clientID, 'robot1_goal', vrep.simx_opmode_blocking) _, self.robot2_goal_Handle = vrep.simxGetDistanceHandle( self.clientID, 'robot2_goal', vrep.simx_opmode_blocking) # 获取末端句柄 _, self.end1_Handle = vrep.simxGetObjectHandle( self.clientID, 'end', vrep.simx_opmode_blocking) _, self.end2_Handle = vrep.simxGetObjectHandle( self.clientID, 'end0', vrep.simx_opmode_blocking) _, self.goal1_Handle = vrep.simxGetObjectHandle( self.clientID, 'goal_1', vrep.simx_opmode_blocking) _, self.goal2_Handle = vrep.simxGetObjectHandle( self.clientID, 'goal_2', vrep.simx_opmode_blocking) print('Handles available!') _, self.goal_1 = vrep.simxGetObjectPosition(self.clientID, self.goal1_Handle, -1, vrep.simx_opmode_blocking) _, self.goal_2 = vrep.simxGetObjectPosition(self.clientID, self.goal2_Handle, -1, vrep.simx_opmode_blocking) del self.goal_1[2] del self.goal_2[2] self.goal_1 = np.array(self.goal_1) self.goal_2 = np.array(self.goal_2) self.jointConfig1 = np.zeros((self.jointNum, )) self.jointConfig2 = np.zeros((self.jointNum, )) for i in range(self.jointNum): _, jpos = vrep.simxGetJointPosition(self.clientID, self.robot1_jointHandle[i], vrep.simx_opmode_blocking) self.jointConfig1[i] = jpos for i in range(self.jointNum): _, jpos = vrep.simxGetJointPosition(self.clientID, self.robot2_jointHandle[i], vrep.simx_opmode_blocking) self.jointConfig2[i] = jpos _, collision1 = vrep.simxReadCollision(self.clientID, self.robot1_collisionHandle, vrep.simx_opmode_blocking) _, collision2 = vrep.simxReadCollision(self.clientID, self.robot2_collisionHandle, vrep.simx_opmode_blocking) _, pos1 = vrep.simxGetObjectPosition(self.clientID, self.end1_Handle, -1, vrep.simx_opmode_blocking) _, pos2 = vrep.simxGetObjectPosition(self.clientID, self.end2_Handle, -1, vrep.simx_opmode_blocking) _, d1 = vrep.simxReadDistance(self.clientID, self.robot1_goal_Handle, vrep.simx_opmode_blocking) _, d2 = vrep.simxReadDistance(self.clientID, self.robot2_goal_Handle, vrep.simx_opmode_blocking) for i in range(self.jointNum): _, returnpos = vrep.simxGetObjectPosition( self.clientID, self.robot1_linkHandle[i], -1, vrep.simx_opmode_blocking) for i in range(self.jointNum): _, returnpos = vrep.simxGetObjectPosition( self.clientID, self.robot2_linkHandle[i], -1, vrep.simx_opmode_blocking) print('programing start!') vrep.simxSynchronousTrigger(self.clientID) # 让仿真走一步