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
