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 __call__(self, t, values):
if self.size is None:
packedData = vrep.simxPackFloats(values.flatten())
else:
packedData = vrep.simxPackFloats(values[:self.size].flatten())
raw_bytes = (ctypes.c_ubyte *
len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, self.signal_name, raw_bytes,
self.mode)
def __call__( self, t, values ):
if self.size is None:
packedData=vrep.simxPackFloats(values.flatten())
else:
packedData=vrep.simxPackFloats(values[:self.size].flatten())
raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, self.signal_name,
raw_bytes,
self.mode)
def sendAction(self, s):
msg = String()
changeAction = self.checkConditions(s, self.prev['A'])
angles = self.getAngles(s)
states = self.splitState(s)
ret = []
for i, name in enumerate(self.pubs.keys()):
loc = "robot" + str(i)
angle = angles[loc]
state = states[loc]
if changeAction[i]:
self.counter[i] = self.period
action = self.getNextAction(state, angle, i)
else:
action = self.prev['A'][i]
ret.append(action)
action = self.getPrimitive(angle, self.actionMap[action], i)
msg.data = vrep.simxPackFloats(action)
self.pubs[name].publish(msg)
self.counter[i] -= 1
self.time += 1
if self.time % np.inf == 0:
self.turn = -1 # turn left for now # np.random.randint(2) + 1 if self.turn == 0 else 0
self.phase = (self.phase + 1) % 4
print("PHASE ", self.phase)
print(ret, self.time)
return ret
def _setMotorSpeeds(self, leftSpeed, rightSpeed):
"""
set the desired speeds of the left and right wheel of the robot
:param
leftSpeed: float
speed in rad/sec of left motor
:param
rightSpeed: float
speed in rad/sec of right motor
"""
"""
res, ret_ints, ret_floats, ret_strings, ret_buffer = vrep.simxCallScriptFunction(self.__clientID, self._name,
vrep.sim_scripttype_childscript,
'setVelocitiesForRemote', [],
[leftSpeed, rightSpeed], [],
self.__empty_buff, vrep.simx_opmode_blocking)
"""
velocities = vrep.simxPackFloats([leftSpeed, rightSpeed])
res = vrep.simxSetStringSignal(self.__clientID, self.__signalName+'_velocities', velocities, vrep.simx_opmode_oneshot)
# res = vrep.simx_return_ok
# res = vrep.simxSetFloatSignal(self.__clientID, self.__signalName+'_velLeft', 0.0, vrep.simx_opmode_oneshot)
# res = vrep.simxSetFloatSignal(self.__clientID, self.__signalName+'_velRight', 0.0, vrep.simx_opmode_oneshot)
if res == vrep.simx_return_ok:
logging.debug(' motor values', leftSpeed, rightSpeed)
else:
logging.error('Remote function call simxSetStringSignal for signal EPUCK_velLeft/-Right failed')
def run_threaded_drop(self, frame_work2obj):
"""Gets an initial object pose by 'dropping' it WRT frame_work2obj.
This function launches a threaded script that sets an object position,
enables the object to be dynamically simulated, and allows it to fall
to the ground and come to a resting pose. This is usually the first
step when collecting grasps.
Note that we also set the gripper to be non-collidable / respondable,
so it doesn't interfere with the drop process
"""
self._clear_signals()
self.set_gripper_properties(visible=True,
dynamic=True,
collidable=True)
frame = self._format_matrix(frame_work2obj)
# Launch the threaded script
vrep.simxSetStringSignal(self.clientID, 'run_drop_object',
vrep.simxPackFloats(frame),
vrep.simx_opmode_oneshot)
r = -1
while r != vrep.simx_return_ok:
r, success = vrep.simxGetIntegerSignal(
self.clientID, 'object_resting', vrep.simx_opmode_oneshot_wait)
if success == 0:
raise Exception('Error dropping object! Return code: ', r)
def __init__( self, sim_dt=0.01, max_target_distance=3, noise=False,
noise_std=[0,0,0,0,0,0],
):
# Call the superclass of Quadcopter, which is Robot
super(Quadcopter, self).__init__(sim_dt)
err, self.copter = vrep.simxGetObjectHandle(self.cid, "Quadricopter_base",
vrep.simx_opmode_oneshot_wait )
# Reset the motor commands to zero
packedData=vrep.simxPackFloats([0,0,0,0])
raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities",
raw_bytes,
vrep_mode)
self.pos = [0,0,0]
self.pos_err = [0,0,0]
self.t_pos = [0,0,0]
self.lin = [0,0,0]
self.ori = [0,0,0]
self.ori_err = [0,0,0]
self.t_ori = [0,0,0]
self.ang = [0,0,0]
# Maximum target distance error that can be returned
self.max_target_distance = max_target_distance
# If noise is being modelled
self.noise = noise
# Standard Deviation of the noise for the 4 state variables
self.noise_std = noise_std
def __init__( self, sim_dt=0.01, max_target_distance=3, noise=False,
noise_std=[0,0,0,0,0,0],
target_func=None,
):
super(Quadcopter, self).__init__(sim_dt)
err, self.copter = vrep.simxGetObjectHandle(self.cid, "Quadricopter_base",
vrep.simx_opmode_oneshot_wait )
err, self.target = vrep.simxGetObjectHandle(self.cid, "Quadricopter_target",
vrep.simx_opmode_oneshot_wait )
# Reset the motor commands to zero
packedData=vrep.simxPackFloats([0,0,0,0])
raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities",
raw_bytes,
vrep_mode)
self.pos = [0,0,0]
self.pos_err = [0,0,0]
self.t_pos = [0,0,0]
self.lin = [0,0,0]
self.ori = [0,0,0]
self.ori_err = [0,0,0]
self.t_ori = [0,0,0]
self.ang = [0,0,0]
self.vert_prox_dist = 0
self.left_prox_dist = 0
self.right_prox_dist = 0
# Distance reading recorded when nothing is in range
self.max_vert_dist = 1.5
self.max_left_dist = 1.0
self.max_right_dist = 1.0
# Maximum target distance error that can be returned
self.max_target_distance = max_target_distance
# If noise is being modelled
self.noise = noise
# Standard Deviation of the noise for the 4 state variables
self.noise_std = noise_std
# Overwrite the get_target method if the target is to be controlled by a
# function instead of by V-REP
if target_func is not None:
self.step = 0
self.target_func = target_func
def get_target():
self.t_pos, self.t_ori = self.target_func( self.step )
self.step += 1
self.get_target = get_target
def calcTheForce(self):
# расчет сил
dx = self.XYZ_d[0] - self.XYZ[0]
dy = self.XYZ_d[1] - self.XYZ[1]
self.ABG_d[2] = np.arctan2(dy, dx)
Fx_d = 0
if -0.05 <= self.ABG_d[2] - self.ABG[2] <= 0.05:
if dx < 0 and dy < 0:
if dx > dy:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0],
self.XYZ[0]) * -1
else:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1],
self.XYZ[1]) * -1
if dx > 0 and dy > 0:
if dx > dy:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0])
else:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1])
if dx < 0 and dy > 0:
if dx > dy:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0],
self.XYZ[0]) * -1
else:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1])
if dx > 0 and dy < 0:
if dx > dy:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0])
else:
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1],
self.XYZ[1]) * -1
# перемещения вдоль игрек
if -0.5 <= dx <= 0.5 and (self.XYZ[1] <= self.XYZ_d[1]):
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1])
if (-0.5 <= dx <= 0.5) and (self.XYZ[1] > self.XYZ_d[1]):
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[1], self.XYZ[1]) * -1
# перемещения вдоль икс
if -0.5 <= dy <= 0.5 and (self.XYZ[0] <= self.XYZ_d[0]):
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0])
if (-0.5 <= dy <= 0.5) and (self.XYZ[0] > self.XYZ_d[0]):
Fx_d = self.fwd_ctrl.pidUpdate(self.XYZ_d[0], self.XYZ[0]) * -1
else:
Fx_d = 0
Fy_d = 0
Fz_d = PIDc.pidUpdate(self.stab_ctrl, self.XYZ_d[2], self.XYZ[2])
# повороты: рыскание, тангаж и крен
Mx_d = PIDc.pidUpdate(self.rotation_ctrl, self.ABG_d[2],
self.ABG[2]) # вокруг Z, рыскание
My_d = PIDc.pidUpdate(self.pitchroll_ctrl, self.ABG_d[1], self.ABG[1])
Mz_d = PIDc.pidUpdate(self.pitchroll_ctrl, self.ABG_d[0], self.ABG[0])
forceD = [Fx_d, Fy_d, Fz_d, Mx_d, My_d, Mz_d]
s_force = vrep.simxPackFloats(forceD)
vrep.simxSetStringSignal(self.clientID, "ForceD", s_force,
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)
def quadcopter_rotors(cid, handle, val):
# This function does not use handle, it just exists in the signature
# to make it consistent
motor_values = np.zeros(4)
for i in range(4):
motor_values[i] = val[i]
packedData = vrep.simxPackFloats(motor_values.flatten())
raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(cid, "rotorTargetVelocities", raw_bytes,
vrep_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 send_motor_commands( self, values ):
motor_values = np.zeros(4)
for i in range(4):
motor_values[i] = values[i]
packedData=vrep.simxPackFloats(motor_values.flatten())
raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities",
raw_bytes,
vrep_mode)
def forward(self, action=None):
controller.target_move(self.cid,self.target,self.function,self.args)
if not(action is None):
packedData = vrep.simxPackFloats([x for x in action])
raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities",
packedData,
vrep.simx_opmode_oneshot)
firstPass = False
else:
controller.controller_motor(self.cid,self.copter,self.target)
def quadcopter_rotors(cid, handle, val):
# This function does not use handle, it just exists in the signature
# to make it consistent
motor_values = np.zeros(4)
for i in range(4):
motor_values[i] = val[i]
packedData=vrep.simxPackFloats(motor_values.flatten())
raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(cid, "rotorTargetVelocities",
raw_bytes,
vrep_mode)
def send_path_4_drawing(path, sleep_time=0.07):
#the bigger the sleep time the more accurate the points are placed but you have to be very patient :D
for i in path:
point2send = transform_points_from_image2real(i)
packedData = vrep.simxPackFloats(point2send.flatten())
raw_bytes = (ctypes.c_ubyte *
len(packedData)).from_buffer_copy(packedData)
returnCode = vrep.simxWriteStringStream(clientID, "path_coord",
raw_bytes,
vrep.simx_opmode_oneshot)
time.sleep(sleep_time)
def forward(self, action=None):
controller.target_move(self.cid, self.target, self.function, self.args)
if not (action is None):
packedData = vrep.simxPackFloats([x for x in action])
raw_bytes = (ctypes.c_ubyte *
len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities",
packedData,
vrep.simx_opmode_oneshot)
firstPass = False
else:
controller.controller_motor(self.cid, self.copter, self.target)
def send_motor_commands(self, values):
# Limit motors by max and min values
motor_values = np.zeros(4)
for i in range(4):
# if values[i] > 30:
# motor_values[i] = 30
# elif values[i] < 0:
# motor_values[i] = 0
# else:
# motor_values[i] = values[i]
motor_values[i] = values[i]
# print('motor_value', motor_values)
packedData = vrep.simxPackFloats(motor_values.flatten())
raw_bytes = (ctypes.c_ubyte *
len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities",
raw_bytes, vrep_mode)
def send_motor_commands( self, values ):
# Limit motors by max and min values
motor_values = np.zeros(4)
for i in range(4):
"""
if values[i] > 30:
motor_values[i] = 30
elif values[i] < 0:
motor_values[i] = 0
else:
motor_values[i] = values[i]
"""
motor_values[i] = values[i]
packedData=vrep.simxPackFloats(motor_values.flatten())
raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities",
raw_bytes,
vrep_mode)
def _connect(self):
self.__clientID = vrep.simxStart(self.__host_url, self.__port, True,
True, 5000, 5) # Connect to V-REP
if self.__clientID != -1:
print('Connected to remote API server of Vrep with clientID: ',
self.__clientID)
self._connectionStatus = True
vrep.simxGetStringSignal(
self.__clientID, self.__signalName + '_allSens',
vrep.simx_opmode_streaming) #first time call
vrep.simxGetStringSignal(
self.__clientID, self.__signalName + '_camera',
vrep.simx_opmode_streaming) #first time call
vrep.simxSetStringSignal(
self.__clientID, self.__signalName + '_velocities',
vrep.simxPackFloats([0.0, 0.0]),
vrep.simx_opmode_streaming) #first time call
# if self.__synchronous:
# self.startsim()
else:
logging.error('Failed connecting to remote API server of Vrep')
initialCall=True
print ('VREP Simulation Program')
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
if clientID!=-1:
print ('Connected to remote API server')
joystick = quadCTRL()
# Object handle
# _,Quadbase=vrep.simxGetObjectHandle(clientID,'Quadricopter_base',vrep.simx_opmode_oneshot_wait)
else:
print('Connection Failure')
sys.exit('Abort Connection')
while True:
# Code for testing...
if initialCall:
mode = vrep.simx_opmode_streaming
initialCall = False
else:
mode = vrep.simx_opmode_buffer
# Poll controller
demands = joystick.DetectAction()
print("R: " + str(demands[0]) + " " +str(demands[1]) + " " +
"L: " + str(demands[2]) + " " +str(demands[3]))
demandString=vrep.simxPackFloats(demands)
vrep.simxSetStringSignal(clientID,'demandstring',demandString,vrep.simx_opmode_oneshot)
print('EOS')
def __init__( self, max_target_distance=4, noise=False,
noise_std=None, dodging=True,
target_func=None, cid=None
):
# If a cid is specified, assume the connection has already been
# established and should remain open
if cid is None:
vrep.simxFinish(-1) # just in case, close all opened connections
self.cid = vrep.simxStart('127.0.0.1',19997,True,True,5000,5)
else:
self.cid = cid
if self.cid != -1:
print ('Connected to V-REP remote API server, client id: %s' % self.cid)
vrep.simxStartSimulation( self.cid, vrep.simx_opmode_oneshot )
if SYNC:
vrep.simxSynchronous( self.cid, True )
else:
print ('Failed connecting to V-REP remote API server')
self.exit()
err, self.copter = vrep.simxGetObjectHandle(self.cid, "Quadricopter_base",
vrep.simx_opmode_oneshot_wait )
err, self.target = vrep.simxGetObjectHandle(self.cid, "Quadricopter_target",
vrep.simx_opmode_oneshot_wait )
# Reset the motor commands to zero
packedData=vrep.simxPackFloats([0,0,0,0])
raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities",
raw_bytes,
vrep_mode)
self.pos = [0,0,0]
self.pos_err = [0,0,0]
self.t_pos = [0,0,0]
self.lin = [0,0,0]
self.ori = [0,0,0]
self.ori_err = [0,0,0]
self.t_ori = [0,0,0]
self.ang = [0,0,0]
self.count = 0
# Maximum target distance error that can be returned
self.max_target_distance = max_target_distance
# If noise is being modelled
if noise_std is not None:
self.noise = True
else:
self.noise = False
# Standard Deviation of the noise for the 4 state variables
self.noise_std = noise_std
# Overwrite the get_target method if the target is to be controlled by a
# function instead of by V-REP
if target_func is not None:
self.step = 0
self.target_func = target_func
def get_target():
self.t_pos, self.t_ori = self.target_func( self.step )
self.step += 1
self.get_target = get_target
def __set(self, action):
self.action = np.clip(action, self.action_space.low,
self.action_space.high)
vrep.simxSetStringSignal(self.__ID, "actions",
vrep.simxPackFloats(self.action),
vrep.simx_opmode_oneshot)
def useTheForce(self, Fx, Fy, Fz, Rx, Ry, Rz):
forceD = [Fx, Fy, Fz, Rx, Ry, Rz]
s_force = vrep.simxPackFloats(forceD)
res = vrep.simxSetStringSignal(self.clientID, "ForceD", s_force,
vrep.simx_opmode_oneshot)
def controller_motor(cid, copter, targHandle):
global firstPass
global pParam
global iParam
global dParam
global vParam
global cumul
global lastE
global pAlphaE
global pBetaE
global psp2
global psp1
global prevEuler
global firstPass
global particlesTargetVelocities
global propellerScripts
if (firstPass):
propellerScripts=[-1,-1,-1,-1]
for i in range(4):
propellerScripts[i]= vrep.simxGetObjectHandle(cid,'Quadricopter_propeller_respondable'+str(i)+str(1),mode())
particlesTargetVelocities = [0]*4
pParam=.8
iParam=0
dParam=0
vParam= .6
cumul=0
lastE=0
pAlphaE=0
pBetaE=0
psp2=0
psp1=0
prevEuler=0
print("OMG FIRSTPASS")
print("PREV EULER")
print(prevEuler)
#Vertical control:
#d = vrep.simxGetObjectHandle(cid,'Quadricopter_base',mode())[1]
#targetPos=simGetObjectPosition(targetObj,-1)
targetPos = vrep.simxGetObjectPosition(cid,targHandle,-1,mode())[1]
print("Target_Pos IS")
print(targetPos)
#pos = simGetObjectPosition(d,-1)
pos = vrep.simxGetObjectPosition(cid, copter, -1, mode())[1]
l=vrep.simxGetObjectVelocity(cid, copter, mode())[1]
print("l is:{}".format(l))
e=(targetPos[2]-pos[2])
cumul=cumul+e
pv=pParam*e
#thrust=5.335+pv+iParam*cumul+dParam*(e-lastE)+l[2]*vParam
lastE=e
thrust =5.335+ pParam * e +vParam *(0- l[2])
print("THIS IS THRUST" + str(thrust))
#Horizontal control:
#sp=simGetObjectPosition(targetObj,d)
sp = vrep.simxGetObjectPosition(cid,targHandle,copter,mode())[1]
m=simGetObjectMatrix(cid,copter,-1)
#m = vrep.simxGetJointMatrix(cid,d,mode())
print(m)
vx=np.array([1,0,0,1], dtype = np.float64)
vx = np.reshape(vx,(4,1))
#vx=simMultiplyVector(m,vx)
print(m.shape)
print(m)
print(vx.shape)
print(vx)
vx = np.dot(m,vx)
print("CHECK KEK")
print(m.shape)
print(m)
print(vx.shape)
print(vx)
vy = np.array([0,1,0,1],dtype = np.float64)
vy = np.reshape(vy,(4,1))
vy=np.dot(m,vy)
m = m.flatten()
alphaE=(vy[2]-m[11])
alphaCorr=0.25*alphaE+2.1*(alphaE-pAlphaE)
betaE=(vx[2]-m[11])
betaCorr=-0.25*betaE-2.1*(betaE-pBetaE)
pAlphaE=alphaE
pBetaE=betaE
alphaCorr=alphaCorr+sp[1]*0.005+1*(sp[1]-psp2)
betaCorr=betaCorr-sp[0]*0.005-1*(sp[0]-psp1)
psp2=sp[1]
psp1=sp[0]
#-- Rotational control:
#euler=simGetObjectOrientation(d,targetObj)
euler = vrep.simxGetObjectOrientation(cid,copter,targHandle,mode())[1]
rotCorr=euler[2]*0.1+2*(euler[2]-prevEuler)
prevEuler=euler[2]
#-- Decide of the motor velocities:
particlesTargetVelocities[0]=thrust*(1-alphaCorr+betaCorr+rotCorr)
particlesTargetVelocities[1]=thrust*(1-alphaCorr-betaCorr-rotCorr)
particlesTargetVelocities[2]=thrust*(1+alphaCorr-betaCorr+rotCorr)
particlesTargetVelocities[3]=thrust*(1+alphaCorr+betaCorr-rotCorr)
#-- Send the desired motor velocities to the 4 rotors:
packedData = vrep.simxPackFloats([x for x in particlesTargetVelocities])
#packedData = vrep.simxPackFloats([100,100,100,100])
raw_bytes = (ctypes.c_ubyte * len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(cid, "rotorTargetVelocities",
packedData,
vrep.simx_opmode_oneshot)
firstPass = False
return particlesTargetVelocities
def __init__(self,
max_target_distance=4,
noise=False,
noise_std=None,
dodging=True,
target_func=None,
cid=None):
# If a cid is specified, assume the connection has already been
# established and should remain open
if cid is None:
vrep.simxFinish(-1) # just in case, close all opened connections
self.cid = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
else:
self.cid = cid
if self.cid != -1:
print('Connected to V-REP remote API server, client id: %s' %
self.cid)
vrep.simxStartSimulation(self.cid, vrep.simx_opmode_oneshot)
if SYNC:
vrep.simxSynchronous(self.cid, True)
else:
print('Failed connecting to V-REP remote API server')
self.exit()
err, self.copter = vrep.simxGetObjectHandle(
self.cid, "Quadricopter_base", vrep.simx_opmode_oneshot_wait)
err, self.target = vrep.simxGetObjectHandle(
self.cid, "Quadricopter_target", vrep.simx_opmode_oneshot_wait)
err, self.laser_signal = vrep.simxReadStringStream(
self.cid, 'laser_data', vrep.simx_opmode_streaming)
err, self.dis_signal = vrep.simxReadStringStream(
self.cid, 'dis_data', vrep.simx_opmode_streaming)
# Reset the motor commands to zero
packedData = vrep.simxPackFloats([0, 0, 0, 0])
raw_bytes = (ctypes.c_ubyte *
len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities",
raw_bytes, vrep_mode)
self.pos = [0, 0, 0]
self.pos_err = [0, 0, 0]
self.t_pos = [0, 0, 0]
self.lin = [0, 0, 0]
self.ori = [0, 0, 0]
self.ori_err = [0, 0, 0]
self.t_ori = [0, 0, 0]
self.ang = [0, 0, 0]
self.count = 0
self.first = True
self.figure = plt.figure()
self.x_min = 999
self.y_min = 999 #min distance to wall
# Maximum target distance error that can be returned
self.max_target_distance = max_target_distance
# If noise is being modelled
if noise_std is not None:
self.noise = True
else:
self.noise = False
# Standard Deviation of the noise for the 4 state variables
self.noise_std = noise_std
def __init__(self,
max_target_distance=4,
noise=False,
noise_std=None,
dodging=True,
target_func=None,
cid=None,
ori_mode=False):
self.ori_mode = ori_mode
# If a cid is specified, assume the connection has already been
# established and should remain open
if cid is None:
vrep.simxFinish(-1) # just in case, close all opened connections
self.cid = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
else:
self.cid = cid
if self.cid != -1:
print('Connected to V-REP remote API server, client id: %s' %
self.cid)
vrep.simxStartSimulation(self.cid, vrep.simx_opmode_oneshot)
if SYNC:
vrep.simxSynchronous(self.cid, True)
else:
print('Failed connecting to V-REP remote API server')
self.exit()
err, self.copter = vrep.simxGetObjectHandle(
self.cid, "Quadricopter_base", vrep.simx_opmode_oneshot_wait)
err, self.target = vrep.simxGetObjectHandle(
self.cid, "Quadricopter_target", vrep.simx_opmode_oneshot_wait)
# Reset the motor commands to zero
packedData = vrep.simxPackFloats([0, 0, 0, 0])
raw_bytes = (ctypes.c_ubyte *
len(packedData)).from_buffer_copy(packedData)
err = vrep.simxSetStringSignal(self.cid, "rotorTargetVelocities",
raw_bytes, vrep_mode)
self.pos = [0, 0, 0]
self.pos_err = [0, 0, 0]
self.t_pos = [0, 0, 0]
self.lin = [0, 0, 0]
self.ori = [0, 0, 0]
self.ori_err = [0, 0, 0]
self.t_ori = [0, 0, 0]
self.ang = [0, 0, 0]
self.count = 0
# Maximum target distance error that can be returned
self.max_target_distance = max_target_distance
# If noise is being modelled
if noise_std is not None:
self.noise = True
else:
self.noise = False
# Standard Deviation of the noise for the 4 state variables
self.noise_std = noise_std
# Overwrite the get_target method if the target is to be controlled by a
# function instead of by V-REP
if target_func is not None:
self.step = 0
self.target_func = target_func
def get_target():
self.t_pos, self.t_ori = self.target_func(self.step)
self.step += 1
self.get_target = get_target
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
#
# Replies are coded in a same way. An executed command should reply with the same command counter.
#
# Above simple protocol is just an example: you could use your own protocol! (but it has to be the same on the V-REP side)
# 1. First send a command to display a specific message in a dialog box:
cmdID=1 # this command id means: 'display a text in a message box'
cmdCnt=0
cmdData=b'Hello world!'
dataToSend=getCmdString(cmdID,cmdCnt,cmdData)
vrep.simxWriteStringStream(clientID,'commandsFromRemoteApiClient',dataToSend,vrep.simx_opmode_oneshot)
replyData=waitForCmdReply(cmdCnt)
if sys.version_info[0] == 3:
replyData=str(replyData,'utf-8')
print ('Return string: ',replyData) # display the reply from V-REP (in this case, just a string)
# 2. Now create a dummy object at coordinate 0.1,0.2,0.3:
cmdID=2 # this command id means: 'create a dummy at a specific coordinate with a specific name'
cmdCnt=cmdCnt+1
cmdData=b'MyDummyName'+vrep.simxPackFloats([0.1,0.2,0.3])
dataToSend=getCmdString(cmdID,cmdCnt,cmdData)
vrep.simxWriteStringStream(clientID,'commandsFromRemoteApiClient',dataToSend,vrep.simx_opmode_oneshot)
replyData=waitForCmdReply(cmdCnt)
print ('Dummy handle: ',vrep.simxUnpackInts(replyData)[0]) # display the reply from V-REP (in this case, the handle of the created dummy)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print ('Failed connecting to remote API server')
print ('Program ended')
def run_simulation(self, trajectory):
"""
Trajectory is a list 6 pairs of vectors, each of the same length.
For each pair:
1. The first vector is the position of the motor in rad.
2. The second vector is the max velocity of the motor in rad/s.
"""
self._com(remote_api.simxStopSimulation, get=False)
if self.verbose:
print("Setting parameters...")
traj = self._prepare_traj(trajectory)
packed_data = remote_api.simxPackFloats(traj)
raw_bytes = (ctypes.c_ubyte * len(packed_data)).from_buffer_copy(packed_data)
assert remote_api.simxSetStringSignal(self.api_id, 'trajectory', raw_bytes,
remote_api.simx_opmode_oneshot_wait) == 0
self._com(remote_api.simxSetIntegerSignal, 'state', 1, get=False)
self._com(remote_api.simxGetIntegerSignal, 'state', get=True,
mode=remote_api.simx_opmode_streaming, ret_ok=(0, 1))
time.sleep(0.1)
self._com(remote_api.simxStartSimulation, get=False)
if self.verbose:
print("Simulation started.")
time.sleep(0.01)
start_time = time.time()
while not self.simulation_paused():
if time.time() - start_time > 60.0:
print("Simulation seems hung. Exiting...")
sys.exit(1)
time.sleep(0.005)
time.sleep(1.0)
if self.verbose:
print("Getting resulting parameters.")
object_sensors = self._get_signal('object_sensors')
collide_data = self._get_signal('collide_data')
contact_data = self._get_signal('contact_data')
contact_type = self._get_signal('contact_type', int_type=True)
contacts = []
for i in range(0, len(contact_type), 3):
step = contact_type[i]
obj_names = [self.handles[contact_type[i+1]],self.handles[contact_type[i+2]]]
data = contact_data[2*i:2*i+6]
contacts.append(Contact(step, obj_names, data))
first_c, last_c = None, None
max_f, max_c = 0.0, None
for c in contacts:
if c.obj_names[0] in self.tracked_objects or c.obj_names[1] in self.tracked_objects:
if first_c is None:
first_c = c
last_c = c
if max_f < c.force_norm:
max_f, max_c = c.force_norm, c
salient_contacts = {'first': first_c, 'last': last_c, 'max': max_c}
if first_c is not None:
print('first contact: {} {:.2f} N'.format('|'.join(first_c.obj_names), first_c.force_norm))
if max_c is not None:
print(' max contact: {} {:.2f} N'.format('|'.join(max_c.obj_names), max_c.force_norm))
marker_sensors = None
if self.cfg.sprims.tip:
marker_sensors = np.array(remote_api.simxUnpackFloats(
remote_api.simxGetStringSignal(self.api_id, 'marker_sensors',
remote_api.simx_opmode_oneshot_wait)))
# assert len(positions) == len(quaternions) == len(velocities)
if self.verbose:
print("End of simulation.")
joint_sensors = None
return {'object_sensors' : object_sensors,
'joint_sensors' : joint_sensors,
'marker_sensors' : marker_sensors,
'collide_data' : collide_data,
'contacts' : contacts,
'salient_contacts': salient_contacts}
h, Xside, Yside, vel = cui.setStuff()
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 15000, 5)
if clientID != -1:
print('| Connected to AUV |')
res, auv = vrep.simxGetObjectHandle(clientID, "Sphere",
vrep.simx_opmode_oneshot_wait)
# создание объектов классов
path = PP(clientID, auv, vel)
camera = cam(clientID, "Vision_sensor")
# отправка нулевых сил
forceD = [0, 0, 0, 0]
s_force = vrep.simxPackFloats(forceD)
vrep.simxSetStringSignal(clientID, "ForceD", s_force, vrep.simx_opmode_oneshot)
# планировщик маршрута
path.setPolygon(h, Xside, Yside, vel)
path.makePath()
# ========== начало работы =============================================================================================
time.sleep(1)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
print("| start |")
print("| |")
while True:
path.stateOfMove()
camera.stateOfSearch(path.returnCoord(), auv)
"""
#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)
def run_simulation(self, trajectory):
"""
Trajectory is a list 6 pairs of vectors, each of the same length.
For each pair:
1. The first vector is the position of the motor in rad.
2. The second vector is the max velocity of the motor in rad/s.
"""
self._com(remote_api.simxStopSimulation, get=False)
if self.verbose:
print("Setting parameters...")
traj = self._prepare_traj(trajectory)
packed_data = remote_api.simxPackFloats(traj)
raw_bytes = (ctypes.c_ubyte *
len(packed_data)).from_buffer_copy(packed_data)
assert remote_api.simxSetStringSignal(
self.api_id, 'trajectory', raw_bytes,
remote_api.simx_opmode_oneshot_wait) == 0
self._com(remote_api.simxSetIntegerSignal, 'state', 1, get=False)
self._com(remote_api.simxGetIntegerSignal,
'state',
get=True,
mode=remote_api.simx_opmode_streaming,
ret_ok=(0, 1))
time.sleep(0.1)
self._com(remote_api.simxStartSimulation, get=False)
if self.verbose:
print("Simulation started.")
time.sleep(0.01)
start_time = time.time()
while not self.simulation_paused():
if time.time() - start_time > 60.0:
print("Simulation seems hung. Exiting...")
sys.exit(1)
time.sleep(0.005)
time.sleep(1.0)
if self.verbose:
print("Getting resulting parameters.")
object_sensors = self._get_signal('object_sensors')
collide_data = self._get_signal('collide_data')
contact_data = self._get_signal('contact_data')
contact_type = self._get_signal('contact_type', int_type=True)
contacts = []
for i in range(0, len(contact_type), 3):
step = contact_type[i]
obj_names = [
self.handles[contact_type[i + 1]],
self.handles[contact_type[i + 2]]
]
data = contact_data[2 * i:2 * i + 6]
contacts.append(Contact(step, obj_names, data))
first_c, last_c = None, None
max_f, max_c = 0.0, None
for c in contacts:
if c.obj_names[0] in self.tracked_objects or c.obj_names[
1] in self.tracked_objects:
if first_c is None:
first_c = c
last_c = c
if max_f < c.force_norm:
max_f, max_c = c.force_norm, c
salient_contacts = {'first': first_c, 'last': last_c, 'max': max_c}
if first_c is not None:
print('first contact: {} {:.2f} N'.format(
'|'.join(first_c.obj_names), first_c.force_norm))
if max_c is not None:
print(' max contact: {} {:.2f} N'.format(
'|'.join(max_c.obj_names), max_c.force_norm))
marker_sensors = None
if self.cfg.sprims.tip:
marker_sensors = np.array(
remote_api.simxUnpackFloats(
remote_api.simxGetStringSignal(
self.api_id, 'marker_sensors',
remote_api.simx_opmode_oneshot_wait)))
# assert len(positions) == len(quaternions) == len(velocities)
if self.verbose:
print("End of simulation.")
joint_sensors = None
return {
'object_sensors': object_sensors,
'joint_sensors': joint_sensors,
'marker_sensors': marker_sensors,
'collide_data': collide_data,
'contacts': contacts,
'salient_contacts': salient_contacts
}
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)
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
cmdID = 1 # this command id means: 'display a text in a message box'
cmdCnt = 0
cmdData = b'Hello world!'
dataToSend = getCmdString(cmdID, cmdCnt, cmdData)
vrep.simxWriteStringStream(clientID, 'commandsFromRemoteApiClient',
dataToSend, vrep.simx_opmode_oneshot)
replyData = waitForCmdReply(cmdCnt)
if sys.version_info[0] == 3:
replyData = str(replyData, 'utf-8')
print('Return string: ', replyData
) # display the reply from V-REP (in this case, just a string)
# 2. Now create a dummy object at coordinate 0.1,0.2,0.3:
cmdID = 2 # this command id means: 'create a dummy at a specific coordinate with a specific name'
cmdCnt = cmdCnt + 1
cmdData = b'MyDummyName' + vrep.simxPackFloats([0.1, 0.2, 0.3])
dataToSend = getCmdString(cmdID, cmdCnt, cmdData)
vrep.simxWriteStringStream(clientID, 'commandsFromRemoteApiClient',
dataToSend, vrep.simx_opmode_oneshot)
replyData = waitForCmdReply(cmdCnt)
print(
'Dummy handle: ',
vrep.simxUnpackInts(replyData)[0]
) # display the reply from V-REP (in this case, the handle of the created dummy)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
print('Program ended')
def _set_Action(self, action, prefix=""):
vrep.simxSetStringSignal(self.id_, prefix + "actions",
vrep.simxPackFloats(action),
vrep.simx_opmode_oneshot)
