def send_gear_commands(clientID, target_degree1, target_degree2, gearHandle1, gearHandle2): RAD = 180 / math.pi lastCmdTime = vrep.simxGetLastCmdTime(clientID) vrep.simxSynchronousTrigger(clientID) count = 0 while vrep.simxGetConnectionId(clientID) != -1: count += 1 if count > 20: break currCmdTime = vrep.simxGetLastCmdTime(clientID) dt = currCmdTime - lastCmdTime ret, gear_pos1 = vrep.simxGetJointPosition(clientID, gearHandle1, vrep.simx_opmode_buffer) ret, gear_pos2 = vrep.simxGetJointPosition(clientID, gearHandle2, vrep.simx_opmode_buffer) degree1 = round(gear_pos1 * RAD, 2) degree2 = round(gear_pos2 * RAD, 2) print(degree1, degree2) if abs(degree1 - target_degree1) < 0.5 and abs(degree2 - target_degree2) < 0.5: break vrep.simxPauseCommunication(clientID, True) vrep.simxSetJointTargetPosition(clientID, gearHandle1, target_degree1 / RAD, vrep.simx_opmode_oneshot) vrep.simxSetJointTargetPosition(clientID, gearHandle2, target_degree2 / RAD, vrep.simx_opmode_oneshot) vrep.simxPauseCommunication(clientID, False) lastCmdTime = currCmdTime vrep.simxSynchronousTrigger(clientID)
def getDroneImage(clientID, height): position = np.asarray([0, 0, height]) # Get Drone Handle errorCode, droneHandle = vrep.simxGetObjectHandle( clientID, 'drone', vrep.simx_opmode_oneshot_wait) # Set Drone Height (in meters) vrep.simxSetObjectPosition(clientID, droneHandle, -1, position, vrep.simx_opmode_oneshot_wait) # Get Drone Camera Image (First Call) errorDrone, resolution, droneImage = vrep.simxGetVisionSensorImage( clientID, droneHandle, 0, vrep.simx_opmode_streaming) count = 0 while (vrep.simxGetConnectionId(clientID) != -1): # Read in drone camera image (Note operating mode changed to buffer) errorDrone, resolution, droneImage = vrep.simxGetVisionSensorImage( clientID, droneHandle, 0, vrep.simx_opmode_buffer) count += 1 # quit after 5 images read (first images may be empty) if (errorDrone == vrep.simx_return_ok and count >= 1): droneImage = np.array(droneImage, dtype=np.uint8) droneImage.resize([resolution[1], resolution[0], 3]) break return resolution, droneImage
def streamVisionSensor(visionSensorName,clientID,pause=0.0001): #Get the handle of the vision sensor res1,visionSensorHandle=vrep.simxGetObjectHandle(clientID,visionSensorName,vrep.simx_opmode_oneshot_wait) #Get the image res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_streaming) #Allow the display to be refreshed plt.ion() #Initialiazation of the figure time.sleep(0.5) res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer) im = I.new("RGB", (resolution[0], resolution[1]), "white") #Give a title to the figure fig = plt.figure(1) fig.canvas.set_window_title(visionSensorName) #inverse the picture plotimg = plt.imshow(im,origin='lower') #Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash time.sleep(1) while (vrep.simxGetConnectionId(clientID)!=-1): #Get the image of the vision sensor res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer) #Transform the image so it can be displayed using pyplot image_byte_array = array.array('b',image) im = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "RGB", 0, 1) #Update the image plotimg.set_data(im) #Refresh the display plt.draw() #The mandatory pause ! (or it'll not work) plt.pause(pause) print 'End of Simulation'
def simulate(self): plt.xlabel('time') plt.ylabel('ditance, meters') while vrep.simxGetConnectionId(self.clientID) != -1: # получаем данные с сенсоров (errorCode, sensorState, sensorDetection, detectedObjectHandle, detectedSurfaceNormalVectorUp) = vrep.simxReadProximitySensor( self.clientID, self.sonic_sensor, vrep.simx_opmode_streaming) (errorCode, frontState, frontDetection, detectedObjectHandle, detectedSurfaceNormalVectorFr) = vrep.simxReadProximitySensor( self.clientID, self.front_sonic_sensor, vrep.simx_opmode_streaming) if (frontState and sensorState): # есть данные с двух сенсоров self.calulate(sensorState, min(sensorDetection[2], frontDetection[2])) self.dists.append(min(sensorDetection[2], frontDetection[2])) elif (frontState): # данные только с переднего сенсора self.calulate(frontState, frontDetection[2]) self.dists.append(frontDetection[2]) elif (sensorState): # данные только с бокового сенсора self.calulate(sensorState, sensorDetection[2]) self.dists.append(sensorDetection[2]) else: # нет данных self.calulate(sensorState, self.reqDist + 0.1) self.dists.append(self.reqDist + 0.1) self.timeLine.append(time.clock()) # добавляем точку на график в реальном времени plt.scatter(self.timeLine[-1], self.dists[-1]) plt.pause(0.05) time.sleep(0.12) plt.show()
def analysisVideo(vision_sensor_name, client_id, config, handle_video_coordlist): res1, visionSensorHandle = vrep.simxGetObjectHandle( client_id, vision_sensor_name, vrep.simx_opmode_oneshot_wait) # config["vision_sensor_handle"] = visionSensorHandle vrep.simxGetVisionSensorImage(client_id, visionSensorHandle, 0, vrep.simx_opmode_streaming) time.sleep(1) while (vrep.simxGetConnectionId(client_id) != -1): # if visionSensorHandle != config["vision_sensor_handle"]: # continue # print "config_handle:", config["vision_sensor_handle"] # print "handle:", visionSensorHandle res, resolution, image = vrep.simxGetVisionSensorImage( client_id, visionSensorHandle, 0, vrep.simx_opmode_buffer) while (not image): pass if len(resolution) == 0: continue image_byte_array = array.array('b', image) _img = I.frombuffer("RGB", (resolution[0], resolution[1]), image_byte_array, "raw", "RGB", 0, 1) im = np.array(_img)[::-1, :, ::-1].copy() coordlist = detect(im) # 检测目标物体坐标 handle_video_coordlist(coordlist, config) # if config["play_video"]: cv2.imshow(config["video_label"], im) if cv2.waitKey(15) & 0xFF == 27: # Press ESC to exit :) break print 'End of Analysis'
def compassRobot(clientID): # print('Getting Robot GPS Location') # Get Robot GPS Handle errorCode, gpsHandle = vrep.simxGetObjectHandle( clientID, 'GPS', vrep.simx_opmode_oneshot_wait) orientationError, orientation = vrep.simxGetObjectOrientation( clientID, gpsHandle, -1, vrep.simx_opmode_streaming) # loop while connected to simulation while (vrep.simxGetConnectionId(clientID) != -1): # Read in camera information (Note operating mode changed to buffer) orientationError, orientation = vrep.simxGetObjectOrientation( clientID, gpsHandle, -1, vrep.simx_opmode_buffer) orientation = math.degrees(orientation[2]) if orientation < 0: orientation += 360 errorCodes = [orientationError] # Check if have both image and xyz coordinate data available if (all(errors == vrep.simx_return_ok for errors in errorCodes)): # print ("Pose OK!!!") break elif (all(errors == vrep.simx_return_novalue_flag for errors in errorCodes)): # print ("Pose Not Available Yet") pass else: print(errorCodes) return orientation
def collsion_Read(): if (not clientID): print('connect first') elif (clientID[0] == -1): print('connect first') else: for i in collision_handle: check = [] while (vrep.simxGetConnectionId(clientID[0]) != -1): err, collision = vrep.simxReadCollision( clientID[0], i, vrep.simx_opmode_buffer) if err == vrep.simx_return_ok: print('collision information imported') del check[:] check.append(collision) break elif err == vrep.simx_return_novalue_flag: print("no collision information") else: print('Error') print(err) if (check[0] == 1): print('Collision detected') return 1 print('No Collision') return 0
def getimageinformation(sensor_index): if (not clientID): print('connect first') elif (clientID[0] == -1): print('connect first') else: if (sensor_index == 1 or sensor_index == 2): while (vrep.simxGetConnectionId(clientID[0]) != -1): err, resolution, image = vrep.simxGetVisionSensorImage( clientID[0], vision_sensor[sensor_index - 1], 0, vrep.simx_opmode_buffer) # Save image to txt file => But it have a problem if err == vrep.simx_return_ok: # save image information list print('vision information imported') image_byte_array = np.asarray(image).astype('uint8') result_image = Im.frombuffer( "RGB", (resolution[0], resolution[1]), image_byte_array, "raw", "RGB", 0, 1) return result_image elif err == vrep.simx_return_novalue_flag: print("no vision information") else: print('Error') print(err) else: print('Wrong index number')
def getEndEffector(): # only for objects if (not clientID): print('connect first') elif (clientID[0] == -1): print('connect first') else: Object_Handle = end_effector[0] while (vrep.simxGetConnectionId(clientID[0]) != -1): err, Output_Postion = vrep.simxGetObjectPosition( clientID[0], Object_Handle, UR3_handle[0], vrep.simx_opmode_buffer) if err == vrep.simx_return_ok: time.sleep(1) print('Position') print(Output_Postion) return Output_Postion elif err == vrep.simx_return_novalue_flag: print("no value") else: print("not yet")
def getVisionSensor(visionSensorName,clientID,sample_time): #Get the handle of the vision sensor res1,visionSensorHandle=vrep.simxGetObjectHandle(clientID,visionSensorName,vrep.simx_opmode_oneshot_wait) #Get the image res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_streaming) time.sleep(0.5) res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer) print resolution img = I.new("RGB", (resolution[0], resolution[1])) while (vrep.simxGetConnectionId(clientID)!=-1): start = time.time() #Get the image of the vision sensor res,resolution,image = vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer) image_byte_array = array.array('b',image) im = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "RGB", 0, 1) # #Set the image for naoqi # imagepixel = []#*(resolution[0]*resolution[1]) # for i in xrange(resolution[0] * resolution[1]): # r = i*3 # g = r+1 # b = r+2 # imagepixel.append((image[b], image[g], image[r])) # img.putdata(imagepixel) # print image # print imagepixel videoDeviceProxy.putImage(vision_definitions.kTopCamera, resolution[0], resolution[1], im.rotate(180).tostring()) end = time.time() dt = end-start # if dt < sample_time: # time.sleep(sample_time - dt) # else: # print "Can't keep a period (%gs>%gs)" % (dt,sample_time) print 'End of Simulation'
def gpsRobot(clientID): # print('Getting Robot GPS Location') # Get Robot GPS Handle errorCode, gpsHandle = vrep.simxGetObjectHandle( clientID, 'GPS', vrep.simx_opmode_oneshot_wait) # Get GPS location positionError, position = vrep.simxGetObjectPosition( clientID, gpsHandle, -1, vrep.simx_opmode_streaming) # loop while connected to simulation while (vrep.simxGetConnectionId(clientID) != -1): # Read in camera information (Note operating mode changed to buffer) positionError, position = vrep.simxGetObjectPosition( clientID, gpsHandle, -1, vrep.simx_opmode_buffer) errorCodes = [positionError] # Check if have both image and xyz coordinate data available if (all(errors == vrep.simx_return_ok for errors in errorCodes)): # print ("Pose OK!!!") break elif (all(errors == vrep.simx_return_novalue_flag for errors in errorCodes)): # print ("Pose Not Available Yet") pass else: print(errorCodes) position = np.asarray(position) # print("position") # print(position) position = position[:2] # print(position) return position
def streamVisionSensor(visionSensorName, clientID, pause=0.0001): #Get the handle of the vision sensor res1, visionSensorHandle = vrep.simxGetObjectHandle( clientID, visionSensorName, vrep.simx_opmode_oneshot_wait) #Get the image res2, resolution, image = vrep.simxGetVisionSensorImage( clientID, visionSensorHandle, 0, vrep.simx_opmode_streaming) #Initialiazation of the figure time.sleep(0.5) res, resolution, image = vrep.simxGetVisionSensorImage( clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer) im = I.new("RGB", (resolution[0], resolution[1]), "white") # Init figure plt.ion() fig = plt.figure(1) plotimg = plt.imshow(im, origin='lower') #Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash time.sleep(1) c = 0 while (vrep.simxGetConnectionId(clientID) != -1): #Get the image of the vision sensor res, resolution, image = vrep.simxGetVisionSensorImage( clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer) #Transform the image so it can be displayed img = np.array(image, dtype=np.uint8).reshape( [resolution[1], resolution[0], 3]) #Update the image fig.canvas.set_window_title(visionSensorName + '_' + str(c)) plotimg.set_data(img) plt.draw() plt.pause(pause) c += 1 print('End of Simulation')
def main(): # Initialize two Vision Sensors. If we don't do this, we can't retrieve images from them. vrep.simxGetVisionSensorImage(clientID, line_sensor, 0, vrep.simx_opmode_streaming) vrep.simxGetVisionSensorImage(clientID, obstacle_sensor, 0, vrep.simx_opmode_streaming) time.sleep(1) # PID Parameters # Motor Kp Ki Kd # 5 0.01 0 0 # 10 0.0001 0.0005 pid = pid_controller(1, 0.0001, 0.0005) # Set the Power! motor(10) # Start the main loop. while vrep.simxGetConnectionId(clientID) != -1: raw = get_line_image() # Crop the line image. raw = raw[128:192, :] # Find the desired moment. ret = get_moment(raw) if ret is None: continue cx, cy, img = ret # Calculate the angle we want to steer. angle = math.atan((128 - cx) / cy) steer(pid(angle)) cv2.imshow("result", img) if cv2.waitKey(1) & 0xFF == ord('q'): break
def streamVisionSensor(visionSensorName, clientID, pause, sample_time): #Get the handle of the vision sensor res1, visionSensorHandle = vrep.simxGetObjectHandle( clientID, visionSensorName, vrep.simx_opmode_oneshot_wait) #Get the image res2, resolution, image = vrep.simxGetVisionSensorImage( clientID, visionSensorHandle, 0, vrep.simx_opmode_streaming) #Allow the display to be refreshed plt.ion() #Initialiazation of the figure time.sleep(0.5) res, resolution, image = vrep.simxGetVisionSensorImage( clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer) print resolution im = I.new("RGB", (resolution[0], resolution[1]), "white") #Give a title to the figure fig = plt.figure(1) fig.canvas.set_window_title(visionSensorName) #inverse the picture plotimg = plt.imshow(im, origin='lower') #Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash time.sleep(1) img = I.new("RGB", (resolution[0], resolution[1])) while (vrep.simxGetConnectionId(clientID) != -1): start = time.time() #Get the image of the vision sensor res, resolution, image = vrep.simxGetVisionSensorImage( clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer) #Transform the image so it can be displayed using pyplot image_byte_array = array.array('b', image) im = I.frombuffer("RGB", (resolution[0], resolution[1]), image_byte_array, "raw", "RGB", 0, 1) #Update the image plotimg.set_data(im) #Refresh the display plt.draw() #The mandatory pause ! (or it'll not work) plt.pause(pause) #Set the image for naoqi imagepixel = [] #*(resolution[0]*resolution[1]) for i in xrange(resolution[0] * resolution[1]): r = i * 3 g = r + 1 b = r + 2 imagepixel.append((image[b], image[g], image[r])) img.putdata(imagepixel) # print image # print imagepixel data = img.tostring() im = im.rotate(180) videoDeviceProxy.putImage(vision_definitions.kTopCamera, resolution[0], resolution[1], im.tostring()) end = time.time() dt = end - start if dt < sample_time: time.sleep(sample_time - dt) else: print "Can't keep a period (%gs>%gs)" % (dt, sample_time) print 'End of Simulation'
def getPosition(Objects_num): # only for objects if (not clientID): print('connect first') elif (clientID[0] == -1): print('connect first') else: if (Objects_num < 10 and Objects_num >= 0): Object_Handle = Objects_handle[Objects_num] else: print('wrong object number') return 0 while (vrep.simxGetConnectionId(clientID[0]) != -1): err, Output_Postion = vrep.simxGetObjectPosition( clientID[0], Object_Handle, UR3_handle[0], vrep.simx_opmode_buffer) if err == vrep.simx_return_ok: time.sleep(1) print('Position') print(Output_Postion) return Output_Postion elif err == vrep.simx_return_novalue_flag: print("no value") else: print("not yet")
def run_sim_data(subject, clientID): vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait) # start #RhipX,RhipY,RhipZ,LhipX,LhipY,LhipZ,RKneeZ,LKneeZ,RAnkleX,RAnkleZ,LAnkleX,LAnkleZ legJoints = [0,0,0,0,0,0,0,0,0,0,0,0] #RhipX,RhipY,RhipZ,LhipX,LhipY,LhipZ,RKneeZ,LKneeZ,RAnkleX,RAnkleZ,LAnkleX,LAnkleZ legPositions = [0,0,0,0,0,0,0,0,0,0,0,0] legJointInv = [1,1,1,-1,-1,1,1,1,1,1,-1,1] hipJointInv = [1,1,1,1,1,1,1,1,1,1,1,1] returnCode01, legJoints[0] = vrep.simxGetObjectHandle(clientID, "rightLegJoint0", vrep.simx_opmode_blocking) returnCode02, legJoints[1] = vrep.simxGetObjectHandle(clientID, "rightLegJoint1", vrep.simx_opmode_blocking) returnCode03, legJoints[2] = vrep.simxGetObjectHandle(clientID, "rightLegJoint2", vrep.simx_opmode_blocking) returnCode04, legJoints[6] = vrep.simxGetObjectHandle(clientID, "rightLegJoint3", vrep.simx_opmode_blocking) returnCode05, legJoints[8] = vrep.simxGetObjectHandle(clientID, "rightLegJoint5", vrep.simx_opmode_blocking) returnCode06, legJoints[9] = vrep.simxGetObjectHandle(clientID, "rightLegJoint4", vrep.simx_opmode_blocking) returnCode07, legJoints[3] = vrep.simxGetObjectHandle(clientID, "leftLegJoint0", vrep.simx_opmode_blocking) returnCode08, legJoints[4] = vrep.simxGetObjectHandle(clientID, "leftLegJoint1", vrep.simx_opmode_blocking) returnCode09, legJoints[5] = vrep.simxGetObjectHandle(clientID, "leftLegJoint2", vrep.simx_opmode_blocking) returnCode010, legJoints[7] = vrep.simxGetObjectHandle(clientID, "leftLegJoint3", vrep.simx_opmode_blocking) returnCode011, legJoints[10] = vrep.simxGetObjectHandle(clientID, "leftLegJoint5", vrep.simx_opmode_blocking) returnCode012, legJoints[11] = vrep.simxGetObjectHandle(clientID, "leftLegJoint4", vrep.simx_opmode_blocking) returnCode013, head = vrep.simxGetObjectHandle(clientID, "Asti", vrep.simx_opmode_blocking) headLocation = [1,1,1]; returncode, headLocation = vrep.simxGetObjectPosition(clientID, head, -1, vrep.simx_opmode_streaming) for i in range(12): returnCode, legPositions[i] = vrep.simxGetJointPosition(clientID, legJoints[i], vrep.simx_opmode_streaming) #RhipX,RhipY,RhipZ,LhipX,LhipY,LhipZ,RKneeZ,LKneeZ,RAnkleX,RAnkleZ,LAnkleX,LAnkleZ start_time = int(round(time.time())) while vrep.simxGetConnectionId(clientID) != -1 and ((headLocation[2] > 0.3 or headLocation[2] == 0.0) and ((int(round(time.time())) - start_time) < 100)): for j in range(((subject * 101)),((subject * 101) + 202)): for i in range(12): returnCode, legPositions[i] = vrep.simxGetJointPosition(clientID, legJoints[i], vrep.simx_opmode_buffer) joint_data = model_iterator.getActual(j) newJointPositions = [0,0,0,0,0,0,0,0,0,0,0,0] for i in range(len(newJointPositions)): joint_data[i] = joint_data[i] * math.pi / 180 newJointPositions[i] = joint_data[i] * legJointInv[i] * hipJointInv[i] print(j) print(newJointPositions) print() vrep.simxPauseCommunication(clientID,1) for i in range(12): vrep.simxSetJointTargetPosition(clientID,legJoints[i],newJointPositions[i],vrep.simx_opmode_oneshot) vrep.simxPauseCommunication(clientID,0) time.sleep(0.01) vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait) time.sleep(0.25) return
def cameraOrientation(clientID): # Vision Sensor Object Handling errorCode, cameraHandle = vrep.simxGetObjectHandle( clientID, 'kinect_depth', vrep.simx_opmode_oneshot_wait) # Setting Up First Call error, orientation = vrep.simxGetObjectOrientation( clientID, cameraHandle, -1, vrep.simx_opmode_streaming) # Loop while connected processLimit = 5 count = 0 while (vrep.simxGetConnectionId(clientID) != -1): error, orientation = vrep.simxGetObjectOrientation( clientID, cameraHandle, -1, vrep.simx_opmode_buffer) if (error == vrep.simx_return_ok): count += 1 if count > processLimit: break # Assume only pitch possible in camera pitch = math.degrees(orientation[1]) - 90 return pitch
def get_connection_status(self): """ Function to inform if the connection with the server is active. Returns: connectionId: -1 if the client is not connected to the server. Different connection IDs indicate temporary disconections in-between. """ return vrep.simxGetConnectionId(self.clientID)
def __init__(self): print 'started' # Init vrep client vrep.simxFinish(-1) objectsList = ['Wall1','Wall2','Wall3','Wall4','Wall5','Wall6'] resolution = 0.01 self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) if self.clientID != -1: #The quad helper contains function implementations for various quadcopter functions quad_functions = quad_helper.quad_helper(self.clientID) print('Main Script Started') quad_functions.init_sensors() quad_functions.start_sim() #Setting initial time init_time = time.time() d1=0 #Initialize flags self.obstacleAvoidFlag = False positions = [[0,0,3],[0,1,3],[-1,1,3],[-2,1,3],[-2,2,3],[-2,3,3],[-1,3,3],[0,3,3],[1,3,3]] # Rate init self.rate = rospy.Rate(20.0) # MUST be more then 20Hz #Initializing publishers rospy.Subscriber("/quad_explore/target_position", Pose, self.posCb) #Code to shift the origin from the center to the bottom left obstOccMat = self.createObstacleOccupancyMat(objectsList,self.clientID,resolution) # self.cellDecomposition(obstOccMat) err,self.quadObjectHandle = vrep.simxGetObjectHandle(self.clientID,'Quadricopter',vrep.simx_opmode_blocking) while not rospy.is_shutdown() and vrep.simxGetConnectionId(self.clientID) != -1: #Getting object position with respect to first joint err,obj_pos = vrep.simxGetObjectPosition(self.clientID,self.quadObjectHandle,-1,vrep.simx_opmode_blocking) current_time = time.time() elapsed_time = current_time-init_time #Setting the position of the quadcopter ind = int(elapsed_time/3) #Looping the movement if ind > 2*len(positions)-1: print 'finished one' init_time = time.time() elif ind > len(positions)-1: ind_rev = len(positions)-1-ind self.quad_pos = positions[ind_rev] else: self.quad_pos = positions[ind] # print 'moving quad' quad_functions.move_quad(self.quad_pos) vrep.simxSynchronousTrigger(self.clientID); self.rate.sleep() quad_functions.stop_sim()
def streamVisionSensor(visionSensorName, clientID, pause=0.0001): # enable the synchronous mode on the client: vrep.simxSynchronous(clientID, 1) # start the simulation: vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking) # enable streaming of the iteration counter: res, iteration1 = vrep.simxGetIntegerSignal(clientID, "iteration", vrep.simx_opmode_streaming) print("iteration1: ", iteration1) #Get the handle of the vision sensor res1, visionSensorHandle = vrep.simxGetObjectHandle( clientID, visionSensorName, vrep.simx_opmode_oneshot_wait) #Get the image res2, resolution, image = vrep.simxGetVisionSensorImage( clientID, visionSensorHandle, 0, vrep.simx_opmode_streaming) #Initialiazation of the figure time.sleep(0.5) res, resolution, image = vrep.simxGetVisionSensorImage( clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer) im = I.new("RGB", (resolution[0], resolution[1]), "white") # Init figure plt.ion() fig = plt.figure(1) plotimg = plt.imshow(im, origin='lower') #Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash time.sleep(1) while (vrep.simxGetConnectionId(clientID) != -1): if iteration1 < 30: vrep.simxSynchronousTrigger(clientID) res, iteration1 = vrep.simxGetIntegerSignal( clientID, "iteration", vrep.simx_opmode_buffer) if res != vrep.simx_return_ok: iteration1 = -1 iteration2 = iteration1 while iteration2 == iteration1: # wait until the iteration counter has changed res, iteration2 = vrep.simxGetIntegerSignal( clientID, "iteration", vrep.simx_opmode_buffer) if res != vrep.simx_return_ok: iteration2 = -1 print(iteration2) #Get the image of the vision sensor res, resolution, image = vrep.simxGetVisionSensorImage( clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer) #Transform the image so it can be displayed img = np.array(image, dtype=np.uint8).reshape( [resolution[1], resolution[0], 3]) #Update the image fig.canvas.set_window_title(visionSensorName + '_' + str(iteration2)) plotimg.set_data(img) plt.draw() plt.pause(pause) # vrep.simxSynchronousTrigger(clientID) print('End of Simulation')
def loop(self, interval, learning, learnLoop): cnt=0 while vrep.simxGetConnectionId(self.getClientID())!=-1 and ((not learning) or cnt<learnLoop): for item in self.__items: item.loop() for rob in self.__robs: rob.observe() self.robPerception(rob) if cnt>1: rob.setRewards() # print rob.getLastAction() rob.act() # print rob.getName(), rob.getPosition() time.sleep(interval) cnt+=1 if vrep.simxGetConnectionId(self.getClientID())==-1: print >> sys.stderr, "Disconnected: Exiting from the main loop!" time.sleep(1) exit()
def get_sensor_image(vision_sensor): cnt = 0 while (vrep.simxGetConnectionId(clientID) != -1): err, resolution, image = vrep.simxGetVisionSensorImage(clientID, vision_sensor, 0, vrep.simx_opmode_buffer) cnt += 1 if err == vrep.simx_return_ok: image_buffer = change_color_and_resize(image, resolution) return image_buffer if cnt % 2048 == 0: logging.warning('get_sensor_image retry %d tiems' % cnt)
def streamVisionSensor(visionSensorName,clientID,pause=0.0001): nn = newNeuralNetwork() #print("Neural Network num_inputs:{n}".format(n=nn.num_inputs)) #Get the handle of the vision sensor res1,visionSensorHandle=vrep.simxGetObjectHandle(clientID,visionSensorName,vrep.simx_opmode_oneshot_wait) #Get the image res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_streaming) #Allow the display to be refreshed plt.ion() #Initialiazation of the figure time.sleep(0.5) res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer) im = I.new("RGB", (resolution[0], resolution[1]), "white") #Give a title to the figure im = im.crop((160, 0, 480, 480)) fig = plt.figure(1) fig.canvas.set_window_title(visionSensorName) #inverse the picture plotimg = plt.imshow(im,origin='lower') #Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash time.sleep(1) count = COUNTER while (vrep.simxGetConnectionId(clientID)!=-1): #Get the image of the vision sensor res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer) #Transform the image so it can be displayed using pyplot image_byte_array = array.array('b',image) im = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "RGB", 0, 1) im = im.crop((160, 0, 480, 480)) #Update the image plotimg.set_data(im) #Refresh the display plt.axis("off") plt.show() #The mandatory pause ! (or it'll not work) plt.pause(pause) if (nn): # feed the neural network with the image and get its prediction (if it has a plant) trainer = Trainer("live", imageIntoFeatures(im)) answer = nn.predict(trainer.inputs) print(answer) #assertAnswer(answer) #assertAnswerStrong(answer) if (SAVE_VISION and count % 60 == 0): name = str(count//60) + ".png" print("Saving " + name) im.save(name) count += 1 print('End of Simulation')
def main(): ur5 = UR5() ur5.connect() ur5.ankleinit() clientID = ur5.get_clientID() camera = Camera(clientID) lastCmdTime = vrep.simxGetLastCmdTime(clientID) vrep.simxSynchronousTrigger(clientID) count = 0 while vrep.simxGetConnectionId(clientID) != -1 and count < Simu_STEP: currCmdTime = vrep.simxGetLastCmdTime(clientID) dt = currCmdTime - lastCmdTime # Camera achieve data cur_depth, cur_color = camera.get_camera_data() cur_depth_path, cur_img_path = camera.save_image( cur_depth, cur_color, currCmdTime) # Call affordance map function affordance_cmd = 'th ' + Lua_PATH + ' -imgColorPath ' + cur_img_path + ' -imgDepthPath ' + cur_depth_path + ' -resultPath ' + Save_PATH_RES + str( currCmdTime) + '_results.h5' try: os.system(affordance_cmd) print('-- Successfully create affordance map!') except: print( '!!!!!!!!!!!!!!!!!!!!!!!! Error occurred during creating affordance map' ) hdf2affimg(Save_PATH_RES + str(currCmdTime) + '_results.h5', currCmdTime) # TODO: get the push u,v coordinate u = 256 v = 212 camera_coor = pixel2camera(u, v, cur_depth, camera.intri, camera.Dis_FAR) _, ur5_position = vrep.simxGetObjectPosition( clientID, ur5.get_handle(), -1, vrep.simx_opmode_oneshot_wait) location = camera2ur5(camera_coor, ur5_position, camera.cam_position) # Move ur5 # location = random.randint(100, 200, (1, 3)) # location = location[0] / 400 print("UR5 Move to %s" % (location)) ur5.ur5moveto(location[0], location[1], location[2] - 0.05) count += 1 lastCmdTime = currCmdTime vrep.simxSynchronousTrigger(clientID) vrep.simxGetPingTime(clientID) ur5.disconnect()
def getVisionSensor(visionSensorName,clientID): #Get the handle of the vision sensor res1,visionSensorHandle=vrep.simxGetObjectHandle(clientID,visionSensorName,vrep.simx_opmode_oneshot_wait) #Get the image res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_streaming) time.sleep(1) while (vrep.simxGetConnectionId(clientID)!=-1): #Get the image of the vision sensor res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer) print resolution print 'End of Simulation'
def loop(self, interval, learning, learnLoop): cnt = 0 while vrep.simxGetConnectionId(self.getClientID()) != -1 and ( (not learning) or cnt < learnLoop): for item in self.__items: item.loop() for rob in self.__robs: rob.observe() self.robPerception(rob) if cnt > 1: rob.setRewards() # print rob.getLastAction() rob.act() # print rob.getName(), rob.getPosition() time.sleep(interval) cnt += 1 if vrep.simxGetConnectionId(self.getClientID()) == -1: print >> sys.stderr, "Disconnected: Exiting from the main loop!" time.sleep(1) exit()
def streamVisionSensor(visionSensorName,clientID,pause,sample_time): #Get the handle of the vision sensor res1,visionSensorHandle=vrep.simxGetObjectHandle(clientID,visionSensorName,vrep.simx_opmode_oneshot_wait) #Get the image res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_streaming) #Allow the display to be refreshed plt.ion() #Initialiazation of the figure time.sleep(0.5) res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer) print resolution im = I.new("RGB", (resolution[0], resolution[1]), "white") #Give a title to the figure fig = plt.figure(1) fig.canvas.set_window_title(visionSensorName) #inverse the picture plotimg = plt.imshow(im,origin='lower') #Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash time.sleep(1) img = I.new("RGB", (resolution[0], resolution[1])) while (vrep.simxGetConnectionId(clientID)!=-1): start = time.time() #Get the image of the vision sensor res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer) #Transform the image so it can be displayed using pyplot image_byte_array = array.array('b',image) im = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "RGB", 0, 1) #Update the image plotimg.set_data(im) #Refresh the display plt.draw() #The mandatory pause ! (or it'll not work) plt.pause(pause) #Set the image for naoqi imagepixel = []#*(resolution[0]*resolution[1]) for i in xrange(resolution[0] * resolution[1]): r = i*3 g = r+1 b = r+2 imagepixel.append((image[b], image[g], image[r])) img.putdata(imagepixel) # print image # print imagepixel data = img.tostring() im = im.rotate(180) videoDeviceProxy.putImage(vision_definitions.kTopCamera, resolution[0], resolution[1], im.tostring()) end = time.time() dt = end-start if dt < sample_time: time.sleep(sample_time - dt) else: print "Can't keep a period (%gs>%gs)" % (dt,sample_time) print 'End of Simulation'
def collectImageData(clientID, action_arg): list_of_images = [] collector = [] if clientID!=-1: res,v0=vrep.simxGetObjectHandle(clientID,'Vision_sensor',vrep.simx_opmode_oneshot_wait) res,v1=vrep.simxGetObjectHandle(clientID,'PassiveVision_sensor',vrep.simx_opmode_oneshot_wait) ret_code, left_handle = vrep.simxGetObjectHandle(clientID,'DynamicLeftJoint', vrep.simx_opmode_oneshot_wait) ret_code, right_handle = vrep.simxGetObjectHandle(clientID,'DynamicRightJoint', vrep.simx_opmode_oneshot_wait) ret_code, base_handle = vrep.simxGetObjectHandle(clientID, 'LineTracerBase', vrep.simx_opmode_oneshot_wait) res,resolution,image=vrep.simxGetVisionSensorImage(clientID,v0,0,vrep.simx_opmode_streaming) ret_code, euler_angles = vrep.simxGetObjectOrientation(clientID, base_handle, -1, vrep.simx_opmode_streaming) count = 0 action = 2 inference_counter = 0 steps = 50 action_length = 0 while (vrep.simxGetConnectionId(clientID)!=-1 and count < steps): tim = time.time() for i in range(10): vrep.simxSynchronousTrigger(clientID) res,resolution,image=vrep.simxGetVisionSensorImage(clientID,v0,0,vrep.simx_opmode_buffer) if res==vrep.simx_return_ok: img = np.array(image,dtype=np.uint8) img.resize([resolution[1],resolution[0],3]) rotate_img = img.copy() rotate_img = np.flipud(img) list_of_images.append(rotate_img) ret_code, pos = vrep.simxGetObjectPosition(clientID, base_handle, -1, vrep.simx_opmode_oneshot) ret_code, velo, angle_velo = vrep.simxGetObjectVelocity(clientID, base_handle, vrep.simx_opmode_oneshot) ret_code, euler_angles = vrep.simxGetObjectOrientation(clientID, base_handle, -1, vrep.simx_opmode_buffer) collector.append([pos[0], pos[1], pos[2], velo[0], velo[1], velo[2], euler_angles[0], euler_angles[1], euler_angles[2], action]) if action_length < 1 and action_arg == -1: action_length = random.randint(2,10) action = np.random.randint(0, 5) elif action_arg > -1: action = action_arg action_length = action_length - 1 velo = (action -2) * 15 return_val = vrep.simxSetJointTargetVelocity(clientID, left_handle, velo, vrep.simx_opmode_oneshot) return_val2 = vrep.simxSetJointTargetVelocity(clientID, right_handle, velo, vrep.simx_opmode_oneshot_wait) count+=1 return list_of_images, collector else: sys.exit()
def reset_target(self): if vrep.simxGetConnectionId(self.client_id) == -1: self.connection() for i in range(3): random_value = np.random.randint(self.workspace[i][0], self.workspace[i][1]) self.target_input[i] = random_value / 1000.0 vrep.simxSetObjectPosition(self.client_id, self.target_handle, -1, self.target_input, vrep.simx_opmode_oneshot)
def JointControl(clientID,motionProxy,i,Body): #Head while(vrep.simxGetConnectionId(clientID)!=-1): #Getting joint's angles from Choregraphe (please check your robot's IP) commandAngles = motionProxy.getAngles('Body', False) #Allow the robot to move in VRep using choregraphe's angles vrep.simxSetJointTargetPosition(clientID,Body[0][i],commandAngles[0],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[1][i],commandAngles[1],vrep.simx_opmode_streaming) #Left Leg vrep.simxSetJointTargetPosition(clientID,Body[2][i],commandAngles[8],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[3][i],commandAngles[9],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[4][i],commandAngles[10],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[5][i],commandAngles[11],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[6][i],commandAngles[12],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[7][i],commandAngles[13],vrep.simx_opmode_streaming) #Right Leg vrep.simxSetJointTargetPosition(clientID,Body[8][i],commandAngles[14],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[9][i],commandAngles[15],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[10][i],commandAngles[16],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[11][i],commandAngles[17],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[12][i],commandAngles[18],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[13][i],commandAngles[19],vrep.simx_opmode_streaming) #Left Arm vrep.simxSetJointTargetPosition(clientID,Body[14][i],commandAngles[2],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[15][i],commandAngles[3],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[16][i],commandAngles[4],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[17][i],commandAngles[5],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[18][i],commandAngles[6],vrep.simx_opmode_streaming) #Right Arm vrep.simxSetJointTargetPosition(clientID,Body[19][i],commandAngles[20],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[20][i],commandAngles[21],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[21][i],commandAngles[22],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[22][i],commandAngles[23],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[23][i],commandAngles[24],vrep.simx_opmode_streaming) #Left Fingers vrep.simxSetJointTargetPosition(clientID,Body[25][i][0],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][1],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][2],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][3],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][4],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][5],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][6],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][7],1.0-commandAngles[7],vrep.simx_opmode_streaming) #Right Fingers vrep.simxSetJointTargetPosition(clientID,Body[27][i][0],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][1],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][2],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][3],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][4],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][5],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][6],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][7],1.0-commandAngles[25],vrep.simx_opmode_streaming) print 'End of simulation'
def run360(): print "connecting to vrep with address", IPADDRESS, "on port", PORT clientID = vrep.simxStart(IPADDRESS, PORT, True, True, 5000, 5) print "hello vrep! ", clientID if clientID == -1: print "failed to connect to vrep server" return # get the motor joint handle error, jointHandle = vrep.simxGetObjectHandle( clientID, "plantStandMotor", vrep.simx_opmode_oneshot_wait) print "starting the motor..." # set the velocity of the joint vrep.simxSetJointTargetVelocity(clientID, jointHandle, 1.0, vrep.simx_opmode_oneshot_wait) print "spinning 360 degrees..." # set up to stream joint positions vrep.simxGetJointPosition(clientID, jointHandle, vrep.simx_opmode_streaming) passed90Degrees = False # The control loop: while vrep.simxGetConnectionId( clientID) != -1: # while we are connected to the server.. (error, position) = vrep.simxGetJointPosition(clientID, jointHandle, vrep.simx_opmode_buffer) # Fetch the newest joint value from the inbox (func. returns immediately (non-blocking)): if error == vrep.simx_error_noerror: # here we have the newest joint position in variable jointPosition! # break when we've done a 360 if passed90Degrees and position >= 0 and position < math.pi / 2.0: break elif not passed90Degrees and position > math.pi / 2.0: passed90Degrees = True print "stoppping..." vrep.simxSetJointTargetVelocity(clientID, jointHandle, 0.0, vrep.simx_opmode_oneshot_wait) if clientID != -1: vrep.simxFinish(clientID) print "done!"
def getDepthImage(clientID): imageCount = 0 # to keep track of how many images have been processed processLimit = 5 # print ('Vision Sensor object handling') errorCode, rgbCamHandle = vrep.simxGetObjectHandle( clientID, 'kinect_rgb', vrep.simx_opmode_oneshot_wait) errorCode, xyzCamHandle = vrep.simxGetObjectHandle( clientID, 'kinect_pointcloud', vrep.simx_opmode_oneshot_wait) # print ('Setting up First Calls for Reading Images and Sensor Values') # Note that the image takes a while to display so cannot load first image but # have to set up a buffer errorRGB, resolution, rgbImage = vrep.simxGetVisionSensorImage( clientID, rgbCamHandle, 0, vrep.simx_opmode_streaming) # api return value for the coordinate extraction filter in camera is as follows: # (1) point count along x, (2) point count along Y, (3) point x1, (4) point y1, # (5) point z1, (6) distance point1, (7) point x2, etc. # and auxPacket[1] holds data in the format described above errorXYZ, detectionState, auxPackets = vrep.simxReadVisionSensor( clientID, xyzCamHandle, vrep.simx_opmode_streaming) # loop while connected to simulation while (vrep.simxGetConnectionId(clientID) != -1): # Read in camera information (Note operating mode changed to buffer) errorRGB, resolution, rgbImage = vrep.simxGetVisionSensorImage( clientID, rgbCamHandle, 0, vrep.simx_opmode_buffer) errorXYZ, detectionState, auxPackets = vrep.simxReadVisionSensor( clientID, xyzCamHandle, vrep.simx_opmode_buffer) errorCodes = [errorRGB, errorXYZ] # Check if have both image and xyz coordinate data available if (all(errors == vrep.simx_return_ok for errors in errorCodes)): # print ("image OK!!!") imageCount += 1 elif (all(errors == vrep.simx_return_novalue_flag for errors in errorCodes)): # print ("no image yet") pass else: # print (errorCodes) pass # exit if processed more than certain amount of images if imageCount > processLimit: break return auxPackets, resolution, rgbImage
def loop(self, interval): while vrep.simxGetConnectionId(self.getClientID()) != -1: self.__cnt = self.__cnt + 1 for rob in self.__robs: rob.loop() self.robPerception(rob) # print rob.getName(), rob.getPosition() for item in self.__items: item.loop() time.sleep(interval) print >> sys.stderr, "Disconnected: Exiting from the main loop!" time.sleep(1) exit()
def loop(self, interval): while vrep.simxGetConnectionId(self.getClientID())!=-1: self.__cnt=self.__cnt+1 for rob in self.__robs: rob.loop() self.robPerception(rob) # print rob.getName(), rob.getPosition() for item in self.__items: item.loop() time.sleep(interval) print >> sys.stderr, "Disconnected: Exiting from the main loop!" time.sleep(1) exit()
def step(self, delta_rotor_thrusts): if vrep.simxGetConnectionId(self.clientID) != -1: self.curr_rotor_thrusts = (np.array(self.curr_rotor_thrusts) + delta_rotor_thrusts).tolist() self.quad_functions.apply_rotor_thrust(self.curr_rotor_thrusts) for i in range(self.run_time): vrep.simxSynchronousTrigger(self.clientID) self.get_curr_state() reward = self.quad_functions.get_reward(self.curr_rotor_thrusts, self.curr_pos, self.curr_euler, self.target_pos, self.target_euler) goaldiffpos = np.linalg.norm(np.array(self.curr_pos) - np.array(self.target_pos)) goaldiffeuler = np.linalg.norm(np.array(self.curr_euler) - np.array(self.target_euler)) done = goaldiffpos < 0.5 and goaldiffeuler < 0.1 # TODO set appropriate threshold return np.array(self.curr_state), reward, done, None
def run360(): print "connecting to vrep with address", IPADDRESS, "on port", PORT clientID = vrep.simxStart(IPADDRESS, PORT, True, True, 5000, 5) print "hello vrep! ", clientID if clientID == -1: print "failed to connect to vrep server" return # get the motor joint handle error,jointHandle = vrep.simxGetObjectHandle(clientID, "plantStandMotor", vrep.simx_opmode_oneshot_wait) print "starting the motor..." # set the velocity of the joint vrep.simxSetJointTargetVelocity(clientID, jointHandle, 1.0, vrep.simx_opmode_oneshot_wait); print "spinning 360 degrees..." # set up to stream joint positions vrep.simxGetJointPosition(clientID, jointHandle, vrep.simx_opmode_streaming); passed90Degrees = False # The control loop: while vrep.simxGetConnectionId(clientID) != -1 : # while we are connected to the server.. (error,position) = vrep.simxGetJointPosition( clientID, jointHandle, vrep.simx_opmode_buffer ) # Fetch the newest joint value from the inbox (func. returns immediately (non-blocking)): if error==vrep.simx_error_noerror : # here we have the newest joint position in variable jointPosition! # break when we've done a 360 if passed90Degrees and position >= 0 and position < math.pi/2.0: break elif not passed90Degrees and position > math.pi/2.0: passed90Degrees = True print "stoppping..." vrep.simxSetJointTargetVelocity(clientID, jointHandle, 0.0, vrep.simx_opmode_oneshot_wait); if clientID != -1: vrep.simxFinish(clientID) print "done!"
def conduct_action(self, a): self.action = a / 1000.0 if vrep.simxGetConnectionId(self.client_id) == -1: self.connection() #print("action2", a) lasttime = time.time() while True: for i in range(3): vrep.simxSetFloatSignal(self.client_id, "myTestValue" + str(i), self.action[i], vrep.simx_opmode_oneshot) currtime = time.time() if currtime - lasttime > 0.1: break
def connectToVREP(self, VREP_World=None): "Connect to VREP and load the correct world if needed" "FIXME: SHOULD LAUNCH VREP IF NOT RUNNING" VREP_exec = 'vrep' if self.VREPSimulation == None: self.VREPSimulation = VREP_World '1. check that V-Rep is running and see whether we are already connected to it. Otherwise connect' if VREP_exec not in check_output(["ps","-f", "--no-headers", "ww", "-C", "vrep"]): raise Exception(("V-REP is not running! Please start V-REP with scene: %s" % self.VREPSimulation)) else: "Check if we are connected with the passed clientId already" if self._VREP_clientId is not None: print "ClientId = " ,self._VREP_clientId connId = vrep.simxGetConnectionId(self._VREP_clientId) print "My connId is " , connId if connId == -1: # we are not: set client Id to none and re-connect print "Disconnecting all existing connections to V-REP" vrep.simxFinish(-1) self._VREP_clientId = None while self._VREP_clientId is None: self._VREP_clientId = vrep.simxStart(self._host, self._kheperaPort,True,True, 5000,5) if not self._VREP_clientId == -1: eCode = vrep.simxSynchronous(self._VREP_clientId, True) if eCode != 0: raise Exception("Failed to connect to VREP simulation. Bailing out") # print " we are connected with clientId ", self._VREP_clientId "2. Check the correct world is running" if self.VREPSimulation is not None: VREP_Scene = split(self.VREPSimulation)[1] if VREP_Scene not in check_output(["ps","-f", "--no-headers", "ww", "-C", "vrep"]): eCode = vrep.simxLoadScene(self._VREP_clientId, self.VREPSimulation, 0, vrep.simx_opmode_oneshot_wait) if eCode != 0: raise Exception(("Could not load into V-REP the world", self.VREPSimulation)) "3. Make sure VREP has bees set to the correct directory for saving data" self.setDataDir(self.dataDir) '4. Start simulation' eCode = vrep.simxStartSimulation(self._VREP_clientId, vrep.simx_opmode_oneshot_wait) if eCode != 0: raise Exception("VREP simulation cannot get started") else: print "V-REP simulation is running with clientId: ", self._VREP_clientId return self._VREP_clientId
angleModel[0] = orientation[1] (velizq, velder, targetReached) = setSpeeds(x, y, angleModel, lockMode) if (joystick != oldJoystick) or targetReached: oldJoystick = joystick #print('x: '+str(x)+' y: '+str(y)+' vel: izq '+str(velizq)+' der '+str(velder)+' Lock Mode: '+str(lockMode)) vrep.simxSetJointTargetVelocity(clientID,lwmotor,velizq,vrep.simx_opmode_oneshot) vrep.simxSetJointTargetVelocity(clientID,rwmotor,velder,vrep.simx_opmode_oneshot) if (position != oldPosition): data.update({"item": "robotData"}, {"$set":{"position": {"x": round(position[0],4), "y": round(position[1],4), "z": round(position[2],4)}}}) oldPosition = position if (orientation!= oldOrientation): data.update({"item": "robotData"}, {"$set":{"orientation": {"alpha": round(math.degrees(orientation[0]),2), "beta": round(math.degrees(orientation[1]),2), "gamma": round(math.degrees(orientation[2]),2)}}}) oldOrientation = orientation if (speed!= oldSpeed): data.update({"item": "robotData"}, {"$set":{"speed": {"vx": round(speed[0],2), "vy": round(speed[1],2), "vz": round(speed[2],2)}}}) oldSpeed = speed if (angularSpeed!= oldAngularSpeed): data.update({"item": "robotData"}, {"$set":{"angularSpeed": {"dAlpha": round(angularSpeed[0],2), "dBeta": round(angularSpeed[1],2), "dGamma": round(angularSpeed[2],2)}}}) oldAngularSpeed = angularSpeed #Check connection if vrep.simxGetConnectionId(clientID)==-1: count += 1 if count>2: break close()
def JointControlWhileTakingPictures(clientID,motionProxy,i,Body, naoDirectory, visionSensorName): #Get the handle of the vision sensor res1,visionSensorHandle=vrep.simxGetObjectHandle(clientID,visionSensorName,vrep.simx_opmode_oneshot_wait) #Get the image res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer) time.sleep(1) #Head while(vrep.simxGetConnectionId(clientID)!=-1): #Getting joint's angles from Choregraphe (please check your robot's IP) commandAngles = motionProxy.getAngles('Body', False) #Allow the robot to move in VRep using choregraphe's angles vrep.simxSetJointTargetPosition(clientID,Body[0][i],commandAngles[0],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[1][i],commandAngles[1],vrep.simx_opmode_streaming) #Left Leg vrep.simxSetJointTargetPosition(clientID,Body[2][i],commandAngles[8],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[3][i],commandAngles[9],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[4][i],commandAngles[10],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[5][i],commandAngles[11],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[6][i],commandAngles[12],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[7][i],commandAngles[13],vrep.simx_opmode_streaming) #Right Leg vrep.simxSetJointTargetPosition(clientID,Body[8][i],commandAngles[14],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[9][i],commandAngles[15],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[10][i],commandAngles[16],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[11][i],commandAngles[17],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[12][i],commandAngles[18],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[13][i],commandAngles[19],vrep.simx_opmode_streaming) #Left Arm vrep.simxSetJointTargetPosition(clientID,Body[14][i],commandAngles[2],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[15][i],commandAngles[3],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[16][i],commandAngles[4],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[17][i],commandAngles[5],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[18][i],commandAngles[6],vrep.simx_opmode_streaming) #Right Arm vrep.simxSetJointTargetPosition(clientID,Body[19][i],commandAngles[20],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[20][i],commandAngles[21],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[21][i],commandAngles[22],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[22][i],commandAngles[23],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[23][i],commandAngles[24],vrep.simx_opmode_streaming) #Left Fingers vrep.simxSetJointTargetPosition(clientID,Body[25][i][0],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][1],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][2],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][3],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][4],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][5],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][6],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][7],1.0-commandAngles[7],vrep.simx_opmode_streaming) #Right Fingers vrep.simxSetJointTargetPosition(clientID,Body[27][i][0],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][1],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][2],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][3],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][4],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][5],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][6],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][7],1.0-commandAngles[25],vrep.simx_opmode_streaming) #Get the image of the vision sensor res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_oneshot_wait) #Transform the image so it can be displayed using pyplot image_byte_array = array.array('b',image) im = I.frombuffer("RGB", (640,480), image_byte_array, "raw", "RGB", 0, 1) #Save the image to disk date_string = datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S") naoDirectory = 'D:\_dev\mo416-final-project\Images\Semaforo_verde' file_path = naoDirectory + '\img_' + date_string + '.png' print file_path rotated_im = im.transpose(I.ROTATE_180) rotated_im.save(file_path,"PNG") print 'End of simulation'
def JointControl(clientID,motionProxy,i,Body,sample_time): #Head while(vrep.simxGetConnectionId(clientID)!=-1): start = time.time() #Getting joint's angles from Choregraphe (please check your robot's IP) commandAngles = motionProxy.getAngles('Body', False) #Allow the robot to move in VRep using choregraphe's angles vrep.simxSetJointTargetPosition(clientID,Body[0][i],commandAngles[0],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[1][i],commandAngles[1],vrep.simx_opmode_streaming) #Left Leg vrep.simxSetJointTargetPosition(clientID,Body[2][i],commandAngles[8],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[3][i],commandAngles[9],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[4][i],commandAngles[10],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[5][i],commandAngles[11],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[6][i],commandAngles[12],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[7][i],commandAngles[13],vrep.simx_opmode_streaming) #Right Leg vrep.simxSetJointTargetPosition(clientID,Body[8][i],commandAngles[14],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[9][i],commandAngles[15],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[10][i],commandAngles[16],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[11][i],commandAngles[17],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[12][i],commandAngles[18],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[13][i],commandAngles[19],vrep.simx_opmode_streaming) #Left Arm vrep.simxSetJointTargetPosition(clientID,Body[14][i],commandAngles[2],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[15][i],commandAngles[3],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[16][i],commandAngles[4],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[17][i],commandAngles[5],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[18][i],commandAngles[6],vrep.simx_opmode_streaming) #Right Arm vrep.simxSetJointTargetPosition(clientID,Body[19][i],commandAngles[20],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[20][i],commandAngles[21],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[21][i],commandAngles[22],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[22][i],commandAngles[23],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[23][i],commandAngles[24],vrep.simx_opmode_streaming) #Left Fingers vrep.simxSetJointTargetPosition(clientID,Body[25][i][0],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][1],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][2],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][3],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][4],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][5],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][6],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][7],1.0-commandAngles[7],vrep.simx_opmode_streaming) #Right Fingers vrep.simxSetJointTargetPosition(clientID,Body[27][i][0],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][1],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][2],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][3],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][4],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][5],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][6],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][7],1.0-commandAngles[25],vrep.simx_opmode_streaming) end = time.time() dt = end - start if dt < sample_time: time.sleep(sample_time - dt) else: print "Can't keep a period (%gs>%gs)" % (dt,sample_time) print 'End of simulation'
print 'Conectado ao motor direito!' #inicialização dos sensores (remoteApi) for i in range(16): erro, sensorHandle[i] = vrep.simxGetObjectHandle(clientID, "Pioneer_p3dx_ultrasonicSensor%d" % (i+1),vrep.simx_opmode_oneshot_wait) if erro <> 0: print "Handle do sensor Pioneer_p3dx_ultrasonicSensor%d nao encontrado!" % (i+1) else: print "Conectado ao sensor Pioneer_p3dx_ultrasonicSensor%d!" % (i+1) erro, state, coord, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor(clientID, sensorHandle[i],vrep.simx_opmode_streaming) #desvio e velocidade do robo while vrep.simxGetConnectionId(clientID) != -1: for i in range(16): erro, state, coord, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor(clientID, sensorHandle[i],vrep.simx_opmode_buffer) if erro == 0: dist = coord[2] if state > 0 and dist < noDetectionDist: if dist < maxDetectionDist: dist = maxDetectionDist detect[i] = 1-((dist-maxDetectionDist) / (noDetectionDist-maxDetectionDist)) else: detect[i] = 0 else:
def JointControl(clientID, motionProxy, postureProxy, i, Body): # Head # velocity_x = vrep.simxGetObjectFloatParameter(clientID,vrep.simxGetObjectHandle(clientID,'NAO#',vrep.simx_opmode_oneshot_wait)[1],11,vrep.simx_opmode_streaming) pos = vrep.simxGetObjectPosition(clientID,vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_streaming)[1] angularPos = vrep.simxGetObjectOrientation(clientID,vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_streaming)[1] alphaPosRegister = [] betaPosRegister = [] gamaPosRegister = [] j = 0 li = [] while vrep.simxGetConnectionId(clientID) != -1: for x in range(0, 150): commandAngles = motionProxy.getAngles('Body', False) j+=1 pos = vrep.simxGetObjectPosition(clientID, vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_buffer)[1] angularPos = vrep.simxGetObjectOrientation(clientID, vrep.simxGetObjectHandle(clientID, 'NAO#', vrep.simx_opmode_oneshot_wait)[1], -1, vrep.simx_opmode_buffer)[1] alphaPosRegister.append(angularPos[0]) betaPosRegister.append(angularPos[1]) gamaPosRegister.append(angularPos[2]) vrep.simxSetJointTargetPosition(clientID,Body[0][i],commandAngles[0],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[1][i],commandAngles[1],vrep.simx_opmode_streaming) # Left Leg vrep.simxSetJointTargetPosition(clientID,Body[2][i],commandAngles[8],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[3][i],commandAngles[9],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[4][i],commandAngles[10],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[5][i],commandAngles[11],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[6][i],commandAngles[12],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[7][i],commandAngles[13],vrep.simx_opmode_streaming) # Right Leg vrep.simxSetJointTargetPosition(clientID,Body[8][i],commandAngles[14],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[9][i],commandAngles[15],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[10][i],commandAngles[16],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[11][i],commandAngles[17],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[12][i],commandAngles[18],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[13][i],commandAngles[19],vrep.simx_opmode_streaming) # Left Arm vrep.simxSetJointTargetPosition(clientID,Body[14][i],commandAngles[2],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[15][i],commandAngles[3],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[16][i],commandAngles[4],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[17][i],commandAngles[5],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[18][i],commandAngles[6],vrep.simx_opmode_streaming) # Right Arm vrep.simxSetJointTargetPosition(clientID,Body[19][i],commandAngles[20],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[20][i],commandAngles[21],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[21][i],commandAngles[22],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[22][i],commandAngles[23],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[23][i],commandAngles[24],vrep.simx_opmode_streaming) # Left Fingers vrep.simxSetJointTargetPosition(clientID,Body[25][i][0],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][1],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][2],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][3],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][4],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][5],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][6],1.0-commandAngles[7],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[25][i][7],1.0-commandAngles[7],vrep.simx_opmode_streaming) # Right Fingers vrep.simxSetJointTargetPosition(clientID,Body[27][i][0],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][1],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][2],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][3],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][4],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][5],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][6],1.0-commandAngles[25],vrep.simx_opmode_streaming) vrep.simxSetJointTargetPosition(clientID,Body[27][i][7],1.0-commandAngles[25],vrep.simx_opmode_streaming) motionProxy.stopMove() postureProxy.stopMove() # postureProxy.goToPosture("Stand",0.5) vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait) break print 'End of simulation' print 'Final x pos' print pos calculos = {} calcular(alphaPosRegister,calculos) desvpadA = calculos['desvio_padrao'] calculos = {} calcular(betaPosRegister,calculos) desvpadB = calculos['desvio_padrao'] calculos = {} calcular(gamaPosRegister,calculos) desvpadG = calculos['desvio_padrao'] return [(10*pos[0] - (desvpadA + desvpadB + desvpadG)/3), pos[0]]