def _clear_signals(self, mode=vrep.simx_opmode_oneshot): """Clears all signals this script can set in the simulation.""" vrep.simxClearStringSignal(self.clientID, 'run_drop_object', mode) vrep.simxClearIntegerSignal(self.clientID, 'object_resting', mode) vrep.simxClearIntegerSignal(self.clientID, 'run_grasp_attempt', mode) vrep.simxClearStringSignal(self.clientID, 'py_grasp_done', mode)
def show_path(path, clientID): #make a signal for each path-component datax = path[0] datay = path[1] dataz = path[2] packedDatax = vrep.simxPackFloats(datax) vrep.simxClearStringSignal(clientID, 'Path_Signalx', vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, 'Path_Signalx', packedDatax, vrep.simx_opmode_oneshot) packedDatay = vrep.simxPackFloats(datay) vrep.simxClearStringSignal(clientID, 'Path_Signaly', vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, 'Path_Signaly', packedDatay, vrep.simx_opmode_oneshot) packedDataz = vrep.simxPackFloats(dataz) vrep.simxClearStringSignal(clientID, 'Path_Signalz', vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, 'Path_Signalz', packedDataz, vrep.simx_opmode_oneshot) #signals that the data of the path is ready to be read data2 = [1] packedData2 = vrep.simxPackFloats(data2) vrep.simxClearStringSignal(clientID, 'Command_Path_Ready', vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, 'Command_Path_Ready', packedData2, vrep.simx_opmode_oneshot)
def _getCameraImage(self): """ :return: a PIL Image current (latest) camera image This always implies a request to the robot for sensing with vision sensor (camera). """ if not self._cameraEnabled: logging.error('cannot read camera image if not enabled') return self._image """ res, ret_ints, ret_floats, ret_strings, ret_buffer = vrep.simxCallScriptFunction(self.__clientID, self._name, vrep.sim_scripttype_childscript, 'getCameraSensorsForRemote', [], [], [], self.__empty_buff, vrep.simx_opmode_blocking) """ res, values = vrep.simxGetStringSignal(self.__clientID, self.__signalName + '_camera', vrep.simx_opmode_buffer) if res == vrep.simx_return_ok: imageValues = vrep.simxUnpackFloats(values) logging.debug(' camera values length: ', len(imageValues)) vrep.simxClearStringSignal(self.__clientID, self.__signalName + '_camera', vrep.simx_opmode_buffer) cameraVector = np.array(imageValues) # map pixel values (float in [0,1]) to integers [0-255] as RGB values imageVrep = list(map(lambda x: np.uint8(x * 255), cameraVector)) imageData = np.flipud( np.asarray(imageVrep).reshape((self._resolY, self._resolX, 3))) self._image = I.fromarray(imageData, 'RGB') # self._image.save('my.png') # self._image.show() return self._image else: logging.error( 'Remote function call getCameraSensorsForRemote failed')
def show_path(path, clientID): # make a signal for each path-component datax = path[0] datay = path[1] dataz = path[2] packedDatax = vrep.simxPackFloats(datax) vrep.simxClearStringSignal(clientID, "Path_Signalx", vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, "Path_Signalx", packedDatax, vrep.simx_opmode_oneshot) packedDatay = vrep.simxPackFloats(datay) vrep.simxClearStringSignal(clientID, "Path_Signaly", vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, "Path_Signaly", packedDatay, vrep.simx_opmode_oneshot) packedDataz = vrep.simxPackFloats(dataz) vrep.simxClearStringSignal(clientID, "Path_Signalz", vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, "Path_Signalz", packedDataz, vrep.simx_opmode_oneshot) # signals that the data of the path is ready to be read data2 = [1] packedData2 = vrep.simxPackFloats(data2) vrep.simxClearStringSignal(clientID, "Command_Path_Ready", vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, "Command_Path_Ready", packedData2, vrep.simx_opmode_oneshot)
def show_path2(path,clientID): errorCode,Graph=vrep.simxGetObjectHandle(clientID,'Graph',vrep.simx_opmode_oneshot_wait) print ("sending path to VREP") datax=path[0] datay=path[1] dataz=path[2] packedDatax=vrep.simxPackFloats(datax) vrep.simxClearStringSignal(clientID,'Path_Signalx',vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID,'Path_Signalx',packedDatax,vrep.simx_opmode_oneshot) packedDatay=vrep.simxPackFloats(datay) vrep.simxClearStringSignal(clientID,'Path_Signaly',vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID,'Path_Signaly',packedDatay,vrep.simx_opmode_oneshot) packedDataz=vrep.simxPackFloats(dataz) vrep.simxClearStringSignal(clientID,'Path_Signalz',vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID,'Path_Signalz',packedDataz,vrep.simx_opmode_oneshot)
#UAV_main.py import vrep import UAV_mapgen import UAV_pathfinding import UAV_VREP import numpy as np import time from scipy import ndimage #start Connection to V-REP 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 data=[0,0,1,0,0,0] packedData=vrep.simxPackFloats(data) vrep.simxClearStringSignal(clientID,'Command_Twist_Quad',vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID,'Command_Twist_Quad',packedData,vrep.simx_opmode_oneshot) #generate mapdata, load data if mapdata for scene exist mapdata=UAV_mapgen.mapgen_fast("columns_and_blocks",16,16,10,clientID) # Extending obstacles boundaries using dilation mapdata = ndimage.binary_dilation(mapdata).astype(np.int64) #mapdata[:,:,1] = np.ones_like(mapdata[:,:,1]) #mapdata[:,:,2] = np.ones_like(mapdata[:,:,2]) # Get start and goal data from v-REP start_position=UAV_VREP.getPosition(clientID,'UAV_target') goal_position=UAV_VREP.getPosition(clientID,'goal_new') print ("Start position:", start_position) print ("Goal position:", goal_position)
def followPath2(clientID,path,goal): pathx=path[0] pathy=path[1] pathz=path[2] errorCode,UAV=vrep.simxGetObjectHandle(clientID,'UAV',vrep.simx_opmode_oneshot_wait) errorCode,pos=vrep.simxGetObjectPosition(clientID,UAV,-1,vrep.simx_opmode_streaming) errorCode,orientation=vrep.simxGetObjectOrientation(clientID,UAV,-1,vrep.simx_opmode_streaming) time.sleep(0.1) errorCode,pos=vrep.simxGetObjectPosition(clientID,UAV,-1,vrep.simx_opmode_buffer) errorCode,orientation=vrep.simxGetObjectOrientation(clientID,UAV,-1,vrep.simx_opmode_buffer) xPosition=pos[0] yPosition=pos[1] zPosition=pos[2] vecp=[0,0,0] pdangle=0 pveloz=0 pangle=0 pref_angz=0 while (xPosition > pathx[199]+0.1) or (xPosition < pathx[199]-0.1) or (yPosition > pathy[199]+0.1) or (yPosition < pathy[199]-0.1) or (zPosition > pathz[199]+0.1) or (zPosition < pathz[199]-0.1): xvelomax=0.6 yvelomax=0.6 zmax=1 absolut_dis=math.sqrt((xPosition-pathx[199])**2+(yPosition-pathy[199])**2+(zPosition-pathz[199])**2) #print absolut_dis slowvelo_dis=4 if absolut_dis < slowvelo_dis: xvelomax=xvelomax*absolut_dis/slowvelo_dis yvelomax=yvelomax*absolut_dis/slowvelo_dis #zmax=zmax*absolut_dis/slowvelo_dis start_time = time.time() errorCode,pos=vrep.simxGetObjectPosition(clientID,UAV,-1,vrep.simx_opmode_buffer) errorCode,orientation=vrep.simxGetObjectOrientation(clientID,UAV,-1,vrep.simx_opmode_buffer) xPosition=pos[0] yPosition=pos[1] zPosition=pos[2] # Find nearest point pnear = pathfollowing.find_nearp(pos, path) vec=pathfollowing.findnearst(pos,path) absolut=math.sqrt(vec[0]**2+vec[1]**2+vec[2]**2) xvelo=0 yvelo=0 height=zPosition xvelo_w=xvelomax*vec[0]/absolut#ref_velx yvelo_w=yvelomax*vec[1]/absolut#ref_vely height = pnear[2] #ref_angz, angle between (1/0/0) and (xvelo/yvelo/0) a1=[1,0,0] b1=[xvelo_w,yvelo_w,0] ref_angz=angle_calculationx(a1,b1) if orientation[2]<0: angle=2*np.pi+orientation[2] else: angle=orientation[2] if ref_angz<0: ref_angz=2*np.pi+ref_angz dangle=angle-ref_angz if dangle>np.pi: veloz=6*(2*np.pi-dangle)/np.pi else: if dangle<-np.pi: veloz=-6*(2*np.pi+dangle)/np.pi else: veloz=-6*(dangle)/np.pi if dangle-pdangle>1: print (pdangle,dangle) print (pangle,angle) print (pref_angz,ref_angz) print (pveloz,veloz) pdangle=dangle pveloz=veloz pangle=angle pref_angz=ref_angz xvelo=xvelo_w*np.cos(-orientation[2])-yvelo_w*np.sin(-orientation[2]) yvelo=xvelo_w*np.sin(-orientation[2])+yvelo_w*np.cos(-orientation[2]) data=[xvelo,yvelo,height,0,0,veloz] packedData=vrep.simxPackFloats(data) vrep.simxClearStringSignal(clientID,'Command_Twist_Quad',vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID,'Command_Twist_Quad',packedData,vrep.simx_opmode_oneshot) data=[0,0,height,0,0,0] packedData=vrep.simxPackFloats(data) vrep.simxClearStringSignal(clientID,'Command_Twist_Quad',vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID,'Command_Twist_Quad',packedData,vrep.simx_opmode_oneshot)
import vrep import time vrep.simxFinish(-1) # This port number (19997) is not allowed to be used by teams clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot) vrep.simxClearStringSignal(clientID, 'startRoundSignal', vrep.simx_opmode_oneshot) time.sleep(1) while True: inp = raw_input( "Press Space + Enter to start the round q + Enter to quit ...") if inp is ' ': vrep.simxSetStringSignal(clientID, 'startRoundSignal', 'start', vrep.simx_opmode_oneshot) print 'round signal set' time.sleep(0.1) vrep.simxClearStringSignal(clientID, 'startRoundSignal', vrep.simx_opmode_oneshot) time.sleep(0.1) if inp is 'q': print 'finish' break
def followPath(clientID, path, goal): # arrange the data given to this function pathx = path[0] pathy = path[1] pathz = path[2] # get the start infos for the following errorCode, UAV = vrep.simxGetObjectHandle(clientID, "UAV", vrep.simx_opmode_oneshot_wait) errorCode, pos = vrep.simxGetObjectPosition(clientID, UAV, -1, vrep.simx_opmode_streaming) errorCode, orientation = vrep.simxGetObjectOrientation(clientID, UAV, -1, vrep.simx_opmode_streaming) time.sleep(0.1) errorCode, pos = vrep.simxGetObjectPosition(clientID, UAV, -1, vrep.simx_opmode_buffer) errorCode, orientation = vrep.simxGetObjectOrientation(clientID, UAV, -1, vrep.simx_opmode_buffer) # some initialization xPosition = pos[0] yPosition = pos[1] zPosition = pos[2] xp = [] yp = [] zp = [] xpnear = [] ypnear = [] zpnear = [] xerror = [] yerror = [] zerror = [] # define the radius around the goal, in which the UAV is slowed to lower the error between UAV and path slowvelo_dis = 4 absolut_dis = 1 # path-following loop # here is the goal defined, in this case reach the goal <5cm, <2cm also works in nearly all case, but needs some more time at the end while absolut_dis > 0.05: # define the max possible velocities xvelomax = 1.5 yvelomax = 1.5 # define the max possible turnrate turnmax = 8 # refresh the UAV information errorCode, pos = vrep.simxGetObjectPosition(clientID, UAV, -1, vrep.simx_opmode_buffer) errorCode, orientation = vrep.simxGetObjectOrientation(clientID, UAV, -1, vrep.simx_opmode_buffer) # arrange the information xPosition = pos[0] yPosition = pos[1] zPosition = pos[2] # calculate the distance to the goal absolut_dis = math.sqrt( (xPosition - pathx[(len(pathx) - 1)]) ** 2 + (yPosition - pathy[(len(pathx) - 1)]) ** 2 + (zPosition - pathz[(len(pathx) - 1)]) ** 2 ) # lower the max velocities in the defined radius, closer to the goal the UAV is more slowed if absolut_dis < slowvelo_dis: xvelomax = (xvelomax - 0.15) * absolut_dis / slowvelo_dis * absolut_dis / slowvelo_dis + 0.15 yvelomax = (yvelomax - 0.15) * absolut_dis / slowvelo_dis * absolut_dis / slowvelo_dis + 0.15 # save some information for the documentation after the following xp.append(xPosition) yp.append(yPosition) zp.append(zPosition) # get the direction the UAV should fly to follow the path vec, vec_path, pnear, dis = pathfollowing.findnearst(pos, path) # some more info for documentation xpnear.append(pnear[0]) ypnear.append(pnear[1]) zpnear.append(pnear[2]) xerror.append(abs(xPosition - pnear[0])) yerror.append(abs(yPosition - pnear[1])) zerror.append(abs(zPosition - pnear[2])) # calculate the velocities from the direction vector absolut = math.sqrt(vec[0, 0] ** 2 + vec[0, 1] ** 2) xvelo = 0 yvelo = 0 height = zPosition xvelo_w = xvelomax * vec[0, 0] / absolut # ref_velx yvelo_w = yvelomax * vec[0, 1] / absolut # ref_vely height = zPosition + vec[0, 2] # e # ref_angz, angle between (1/0/0) and (xvelo/yvelo/0) a1 = [1, 0, 0] b1 = [vec_path[0], vec_path[1], 0] ref_angz = angle_calculation(a1, b1) # arrange some info angle = orientation[2] dangle = ref_angz - angle # angle correction to prevent the UAV from turning not into the shorter direction if dangle > np.pi: dangle = dangle - 2 * np.pi if dangle < -np.pi: dangle = 2 * np.pi + dangle # calculate the turnrate, its lower, if the orientation error is lower, to prevent the UAV from doing a oszillation around the right orientation veloz = turnmax * (dangle) / np.pi # transfom the velocities into the UAV-coordinate-system from the world-coordinates xvelo = xvelo_w * np.cos(angle) + yvelo_w * np.sin(angle) yvelo = yvelo_w * np.cos(angle) - xvelo_w * np.sin(angle) # modify the velocities to improve the path following, by using the current distances and errors to the path and his orientation if abs(vec[0, 2]) > 0.2: xvelo = 0 yvelo = 0 else: if dis < 0.5: xvelo = ( xvelo * ((np.pi - abs(dangle)) / np.pi) ** 130 * (((0.2 - abs(vec[0, 2])) / 0.2) ** 6) * ((0.5 - dis) / 0.5) ) yvelo = ( yvelo * ((np.pi - abs(dangle)) / np.pi) ** 15 * (((0.2 - abs(vec[0, 2])) / 0.2) ** 6) * (dis + 0.2) ** 2 ) else: xvelo = ( xvelo * ((np.pi - abs(dangle)) / np.pi) ** 130 * (((0.2 - abs(vec[0, 2])) / 0.2) ** 6) * ((0.5 - dis) / 0.5) ) yvelo = yvelo * ((np.pi - abs(dangle)) / np.pi) ** 15 * (((0.2 - abs(vec[0, 2])) / 0.2) ** 6) if abs(vec_path[2]) > 0.003: print vec_path[2] xvelo = xvelo * (0.003 / abs(vec_path[2])) ** 3 yvelo = yvelo * (0.003 / abs(vec_path[2])) height = height + vec[0, 2] * 4 # create the data-signal, which is read to the LUA-script data = [xvelo, yvelo, height, 0, 0, veloz] packedData = vrep.simxPackFloats(data) vrep.simxClearStringSignal(clientID, "Command_Twist_Quad", vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, "Command_Twist_Quad", packedData, vrep.simx_opmode_oneshot) # trigger the next simulation step vrep.simxSynchronousTrigger(clientID) # clean up some signals data = [0, 0, pathz[(len(pathx) - 1)], 0, 0, 0] packedData = vrep.simxPackFloats(data) vrep.simxClearStringSignal(clientID, "Command_Twist_Quad", vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, "Command_Twist_Quad", packedData, vrep.simx_opmode_oneshot) data2 = [0] packedData2 = vrep.simxPackFloats(data2) vrep.simxClearStringSignal(clientID, "Command_Path_Ready", vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, "Command_Path_Ready", packedData2, vrep.simx_opmode_oneshot) # prepare some data for the plot of the path xyzparray = np.ndarray(shape=(3, len(xp)), dtype=float) for next in range(len(xp)): xyzparray[0, next] = xp[next] xyzparray[1, next] = yp[next] xyzparray[2, next] = zp[next] xyzneararray = np.ndarray(shape=(3, len(xpnear)), dtype=float) for next in range(len(xpnear)): xyzneararray[0, next] = xpnear[next] xyzneararray[1, next] = ypnear[next] xyzneararray[2, next] = zpnear[next] # return plot data return xyzparray, xyzneararray
def followPath(clientID, path, goal): #arrange the data given to this function pathx = path[0] pathy = path[1] pathz = path[2] #get the start infos for the following errorCode, UAV = vrep.simxGetObjectHandle(clientID, 'UAV', vrep.simx_opmode_oneshot_wait) errorCode, pos = vrep.simxGetObjectPosition(clientID, UAV, -1, vrep.simx_opmode_streaming) errorCode, orientation = vrep.simxGetObjectOrientation( clientID, UAV, -1, vrep.simx_opmode_streaming) time.sleep(0.1) errorCode, pos = vrep.simxGetObjectPosition(clientID, UAV, -1, vrep.simx_opmode_buffer) errorCode, orientation = vrep.simxGetObjectOrientation( clientID, UAV, -1, vrep.simx_opmode_buffer) #some initialization xPosition = pos[0] yPosition = pos[1] zPosition = pos[2] xp = [] yp = [] zp = [] xpnear = [] ypnear = [] zpnear = [] xerror = [] yerror = [] zerror = [] #define the radius around the goal, in which the UAV is slowed to lower the error between UAV and path slowvelo_dis = 4 absolut_dis = 1 #path-following loop #here is the goal defined, in this case reach the goal <5cm, <2cm also works in nearly all case, but needs some more time at the end while (absolut_dis > 0.05): #define the max possible velocities xvelomax = 1.5 yvelomax = 1.5 #define the max possible turnrate turnmax = 8 #refresh the UAV information errorCode, pos = vrep.simxGetObjectPosition(clientID, UAV, -1, vrep.simx_opmode_buffer) errorCode, orientation = vrep.simxGetObjectOrientation( clientID, UAV, -1, vrep.simx_opmode_buffer) #arrange the information xPosition = pos[0] yPosition = pos[1] zPosition = pos[2] #calculate the distance to the goal absolut_dis = math.sqrt((xPosition - pathx[(len(pathx) - 1)])**2 + (yPosition - pathy[(len(pathx) - 1)])**2 + (zPosition - pathz[(len(pathx) - 1)])**2) #lower the max velocities in the defined radius, closer to the goal the UAV is more slowed if absolut_dis < slowvelo_dis: xvelomax = ( xvelomax - 0.15 ) * absolut_dis / slowvelo_dis * absolut_dis / slowvelo_dis + 0.15 yvelomax = ( yvelomax - 0.15 ) * absolut_dis / slowvelo_dis * absolut_dis / slowvelo_dis + 0.15 #save some information for the documentation after the following xp.append(xPosition) yp.append(yPosition) zp.append(zPosition) #get the direction the UAV should fly to follow the path vec, vec_path, pnear, dis = pathfollowing.findnearst(pos, path) #some more info for documentation xpnear.append(pnear[0]) ypnear.append(pnear[1]) zpnear.append(pnear[2]) xerror.append(abs(xPosition - pnear[0])) yerror.append(abs(yPosition - pnear[1])) zerror.append(abs(zPosition - pnear[2])) #calculate the velocities from the direction vector absolut = math.sqrt(vec[0, 0]**2 + vec[0, 1]**2) xvelo = 0 yvelo = 0 height = zPosition xvelo_w = xvelomax * vec[0, 0] / absolut #ref_velx yvelo_w = yvelomax * vec[0, 1] / absolut #ref_vely height = zPosition + vec[0, 2] #e #ref_angz, angle between (1/0/0) and (xvelo/yvelo/0) a1 = [1, 0, 0] b1 = [vec_path[0], vec_path[1], 0] ref_angz = angle_calculation(a1, b1) #arrange some info angle = orientation[2] dangle = ref_angz - angle #angle correction to prevent the UAV from turning not into the shorter direction if dangle > np.pi: dangle = dangle - 2 * np.pi if dangle < -np.pi: dangle = 2 * np.pi + dangle #calculate the turnrate, its lower, if the orientation error is lower, to prevent the UAV from doing a oszillation around the right orientation veloz = turnmax * (dangle) / np.pi #transfom the velocities into the UAV-coordinate-system from the world-coordinates xvelo = xvelo_w * np.cos(angle) + yvelo_w * np.sin(angle) yvelo = yvelo_w * np.cos(angle) - xvelo_w * np.sin(angle) #modify the velocities to improve the path following, by using the current distances and errors to the path and his orientation if abs(vec[0, 2]) > 0.2: xvelo = 0 yvelo = 0 else: if dis < 0.5: xvelo = xvelo * ((np.pi - abs(dangle)) / np.pi)**130 * (( (0.2 - abs(vec[0, 2])) / 0.2)**6) * ((0.5 - dis) / 0.5) yvelo = yvelo * ((np.pi - abs(dangle)) / np.pi)**15 * (( (0.2 - abs(vec[0, 2])) / 0.2)**6) * (dis + 0.2)**2 else: xvelo = xvelo * ((np.pi - abs(dangle)) / np.pi)**130 * (( (0.2 - abs(vec[0, 2])) / 0.2)**6) * ((0.5 - dis) / 0.5) yvelo = yvelo * ((np.pi - abs(dangle)) / np.pi)**15 * (( (0.2 - abs(vec[0, 2])) / 0.2)**6) if abs(vec_path[2]) > 0.003: print vec_path[2] xvelo = xvelo * (0.003 / abs(vec_path[2]))**3 yvelo = yvelo * (0.003 / abs(vec_path[2])) height = height + vec[0, 2] * 4 #create the data-signal, which is read to the LUA-script data = [xvelo, yvelo, height, 0, 0, veloz] packedData = vrep.simxPackFloats(data) vrep.simxClearStringSignal(clientID, 'Command_Twist_Quad', vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, 'Command_Twist_Quad', packedData, vrep.simx_opmode_oneshot) #trigger the next simulation step vrep.simxSynchronousTrigger(clientID) #clean up some signals data = [0, 0, pathz[(len(pathx) - 1)], 0, 0, 0] packedData = vrep.simxPackFloats(data) vrep.simxClearStringSignal(clientID, 'Command_Twist_Quad', vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, 'Command_Twist_Quad', packedData, vrep.simx_opmode_oneshot) data2 = [0] packedData2 = vrep.simxPackFloats(data2) vrep.simxClearStringSignal(clientID, 'Command_Path_Ready', vrep.simx_opmode_oneshot) vrep.simxSetStringSignal(clientID, 'Command_Path_Ready', packedData2, vrep.simx_opmode_oneshot) #prepare some data for the plot of the path xyzparray = np.ndarray(shape=(3, len(xp)), dtype=float) for next in range(len(xp)): xyzparray[0, next] = xp[next] xyzparray[1, next] = yp[next] xyzparray[2, next] = zp[next] xyzneararray = np.ndarray(shape=(3, len(xpnear)), dtype=float) for next in range(len(xpnear)): xyzneararray[0, next] = xpnear[next] xyzneararray[1, next] = ypnear[next] xyzneararray[2, next] = zpnear[next] #return plot data return xyzparray, xyzneararray