def getDif(cid,copter,target):
copter_pos = vrep.simxGetObjectPosition(cid, copter, -1, mode())[1]
copter_vel = vrep.simxGetObjectVelocity(cid, copter, mode())[1]
copter_orientation = vrep.simxGetObjectOrientation(cid,copter,-1,mode())[1]
target_pos = vrep.simxGetObjectPosition(cid, target, -1, mode())[1]
target_vel = vrep.simxGetObjectVelocity(cid, target, mode())[1]
return np.asarray([(-np.asarray(copter_pos) + np.asarray(target_pos)),(-np.asarray(copter_vel) + np.asarray(target_vel)),np.asarray(copter_orientation)]).flatten()
def get_object_velocity(self, name):
"""Get an (x,y) velocity for object with name <name>"""
handle = self.get_handle(name)
if handle not in self._v:
vrep.simxGetObjectVelocity(
self.id, handle, vrep.simx_opmode_streaming)
self._v.append(handle)
time.sleep(0.5)
r, lin, ang = vrep.simxGetObjectVelocity(
self.id, handle, vrep.simx_opmode_buffer)
if r != vrep.simx_return_ok:
raise NameError('Velocity unknown')
return lin[:2]
def loop(self):
operationMode = vrep.simx_opmode_streaming
if self.__initLoop:
self.__initLoop = False
else:
operationMode = vrep.simx_opmode_buffer
returnCode, position = vrep.simxGetObjectPosition(self.__clientID, self.__bodyHandle, -1, operationMode)
if returnCode == vrep.simx_return_ok:
self.__position = [0.0, 0.0]
self.__position[0] = position[0] # X
self.__position[1] = position[1] # Y
else:
self.__position = None
# print >> sys.stderr, "Error in VRepBubbleRob.getPosition()"
returnCode, linearVelocity, angularVelocity = vrep.simxGetObjectVelocity(
self.__clientID, self.__bodyHandle, operationMode
)
if returnCode == vrep.simx_return_ok:
try:
self.__velocity = linearVelocity[0] * math.cos(self.__orientation) + linearVelocity[1] * math.sin(
self.__orientation
)
self.__mind.setInput("velocity", self.__velocity)
except TypeError:
pass
# if self.__name=="BubbleRob#1":
# print self.__velocity, linearVelocity[0], math.cos(self.__orientation), linearVelocity[1], math.sin(self.__orientation)
else:
self.__velocity = None
# print >> sys.stderr, "Error in VRepBubbleRob.getPosition()"
self.__cnt = self.__cnt + 1
def restart(self,earlyStop = False):
if (self.cid != None):
vrep.simxStopSimulation(self.cid, self.mode())
vrep.simxSynchronousTrigger(self.cid)
vrep.simxFinish(-1) # just in case, close all opened connections
time.sleep(1)
self.connect()
time.sleep(1)
vrep.simxLoadScene(self.cid, '/home/elias/[email protected]/_Winter2015/CSC494/Scenes/Base_Quad.ttt', 0, self.mode())
if earlyStop:
vrep.simxStopSimulation(self.cid, self.mode())
vrep.simxSynchronousTrigger(self.cid)
vrep.simxFinish(-1) # just in case, close all opened connections
return
vrep.simxStartSimulation(self.cid, self.mode())
self.runtime = 0
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.front_camera = vrep.simxGetObjectHandle(self.cid, 'Quadricopter_frontCamera', vrep.simx_opmode_oneshot)
err, lin, ang = vrep.simxGetObjectVelocity(self.cid, self.copter, vrep.simx_opmode_streaming)
self.getTargetStart()
for i in range(4):
self.propellerScripts[i] = vrep.simxGetObjectHandle(self.cid,
'Quadricopter_propeller_respondable' + str(i) + str(1),
self.mode())
def getState(self, initial=False):
if initial:
mode = vrep.simx_opmode_streaming
else:
mode = vrep.simx_opmode_buffer
# Retrieve IMU data
errorSignal, self.stepSeconds = vrep.simxGetFloatSignal(self.clientID,
'stepSeconds', mode)
errorOrien, baseEuler = vrep.simxGetObjectOrientation(self.clientID,
self.Quadbase, -1, mode)
errorPos, basePos = vrep.simxGetObjectPosition(self.clientID,
self.Quadbase,-1, mode)
errorVel, linVel, angVel = vrep.simxGetObjectVelocity(self.clientID,
self.Quadbase, mode)
if initial:
if (errorSignal or errorOrien or errorPos or errorVel != vrep.simx_return_ok):
time.sleep(0.05)
pass
else:
# Convert Euler angles to pitch, roll, yaw
rollRad, pitchRad = rotate((baseEuler[0], baseEuler[1]), baseEuler[2])
pitchRad = -pitchRad
yawRad = -baseEuler[2]
baseRad = np.array([yawRad,rollRad,pitchRad])+0.0
self.state = np.asarray(np.concatenate((basePos,linVel,angVel,baseRad)),dtype=np.float32)
#print("data_core: " + str(self.state))
return self.state
def loop(self):
operationMode=vrep.simx_opmode_streaming
if self.__initLoop:
self.__initLoop=False
else:
operationMode=vrep.simx_opmode_buffer
returnCode, orientation = vrep.simxGetObjectOrientation(self.__clientID, self.__bodyHandle, -1, operationMode)
if returnCode==vrep.simx_return_ok:
self.__orientation=orientation[2]
else:
self.__orientation = None
# print >> sys.stderr, "Error in VRepBubbleRob.getOrientation()"
self.__mind.setInput("orientation", self.__orientation)
returnCode, position = vrep.simxGetObjectPosition(self.__clientID, self.__bodyHandle, -1, operationMode)
if returnCode==vrep.simx_return_ok:
self.__position=[0.0,0.0]
self.__position[0]=position[0] #X
self.__position[1]=position[1] #Y
else:
self.__position=None
# print >> sys.stderr, "Error in VRepBubbleRob.getPosition()"
returnCode, linearVelocity, angularVelocity = vrep.simxGetObjectVelocity(self.__clientID, self.__bodyHandle, operationMode)
if returnCode==vrep.simx_return_ok:
try:
# self.__velocity=linearVelocity[0]*math.cos(self.__orientation)+linearVelocity[1]*math.sin(self.__orientation)
self.__velocity=math.sqrt(linearVelocity[0]**2+linearVelocity[1]**2)
self.__mind.setInput("velocity", self.__velocity)
except TypeError:
pass
# if self.__name=="BubbleRob#1":
# print self.__velocity, linearVelocity[0], math.cos(self.__orientation), linearVelocity[1], math.sin(self.__orientation)
else:
self.__velocity=None
# print >> sys.stderr, "Error in VRepBubbleRob.getPosition()"
returnCode, sensorTrigger, dp, doh, dsnv = vrep.simxReadProximitySensor(self.__clientID, self.__sensorHandle, operationMode)
if returnCode==vrep.simx_return_ok:
# We succeeded at reading the proximity sensor
self.__mind.setInput("lastProximitySensorTime", vrep.simxGetLastCmdTime(self.__clientID))
self.__mind.setInput("sensorTrigger", sensorTrigger)
self.__mind.setInput("mostSalientItem", self.__mind.selectMostSalient("I"))
# print self.__name, self.__mind.getAttendedItem(self.__mind.getOutput("attendedItemName"))
self.__mind.setInput("attendedItem", self.__mind.getAttendedItem(self.__mind.getOutput("attendedItemName")))
self.__mind.applyRules()
self.__mind.setStates()
if self.__mind.getOutput("steering")!=None:
self.setSteering(self.__mind.getOutput("steering"))
if self.__mind.getOutput("thrust")!=None:
self.setThrust(self.__mind.getOutput("thrust"))
if self.__mind.getOutput("reward")!=None:
if self.__mind.getOutput("reward")>0.5:
self.setEmotionalExpression(1)
elif self.__mind.getOutput("reward")<-0.5:
self.setEmotionalExpression(-1)
else:
self.setEmotionalExpression(0)
getSignalReturnCode, dMessage = vrep.simxGetStringSignal(self.__clientID, "Debug", vrep.simx_opmode_streaming)
if dMessage!="":
print("Debug:"+dMessage)
self.__cnt=self.__cnt+1
def calculate_error( self ):
# Return the state variables
err, self.ori = vrep.simxGetObjectOrientation(self.cid, self.copter, -1,
vrep_mode )
err, self.pos = vrep.simxGetObjectPosition(self.cid, self.copter, -1,
vrep_mode )
err, self.lin, self.ang = vrep.simxGetObjectVelocity(self.cid, self.copter,
vrep_mode )
self.ori = convert_angles(self.ori)
# Apply noise to each measurement if required
#FIXME this is a dumb way to do this, clean it up later
if self.noise:
n_pos = np.random.normal(0,self.noise_std[0],3)
n_lin = np.random.normal(0,self.noise_std[1],3)
n_ori = np.random.normal(0,self.noise_std[2],3)
n_ang = np.random.normal(0,self.noise_std[3],3)
for i in range(3):
self.pos[i] += n_pos[i]
self.lin[i] += n_lin[i]
self.ori[i] += n_ori[i]
self.ang[i] += n_ang[i]
#TODO: might have to wrap angles here
# Find the error
self.ori_err = [self.t_ori[0] - self.ori[0],
self.t_ori[1] - self.ori[1],
self.t_ori[2] - self.ori[2]]
cz = math.cos(self.ori[2])
sz = math.sin(self.ori[2])
x_err = self.t_pos[0] - self.pos[0]
y_err = self.t_pos[1] - self.pos[1]
self.pos_err = [ x_err * cz + y_err * sz,
-x_err * sz + y_err * cz,
self.t_pos[2] - self.pos[2]]
self.lin = [self.lin[0]*cz+self.lin[1]*sz, -self.lin[0]*sz+self.lin[1]*cz, self.lin[2]]
self.ang = [self.ang[0]*cz+self.ang[1]*sz, -self.ang[0]*sz+self.ang[1]*cz, self.ang[2]]
for i in range(3):
if self.ori_err[i] > math.pi:
self.ori_err[i] -= 2 * math.pi
elif self.ori_err[i] < -math.pi:
self.ori_err[i] += 2 * math.pi
def observe(self):
operationMode=vrep.simx_opmode_streaming
if self.__initLoop:
self.__initLoop=False
else:
operationMode=vrep.simx_opmode_buffer
returnCode, orientation = vrep.simxGetObjectOrientation(self.__clientID, self.__bodyHandle, -1, operationMode)
if returnCode==vrep.simx_return_ok:
self.__orientation=orientation[2]
else:
self.__orientation = None
# print >> sys.stderr, "Error in VRepBubbleRob.getOrientation()"
self.__mind.setInput("orientation", self.__orientation)
returnCode, position = vrep.simxGetObjectPosition(self.__clientID, self.__bodyHandle, -1, operationMode)
if returnCode==vrep.simx_return_ok:
self.__position=[0.0,0.0]
self.__position[0]=position[0] #X
self.__position[1]=position[1] #Y
else:
self.__position=None
print >> sys.stderr, "Error in VRepBubbleRob.getPosition()", self.__clientID, self.__bodyHandle
returnCode, linearVelocity, angularVelocity = vrep.simxGetObjectVelocity(self.__clientID, self.__bodyHandle, operationMode)
if returnCode==vrep.simx_return_ok:
try:
# self.__velocity=linearVelocity[0]*math.cos(self.__orientation)+linearVelocity[1]*math.sin(self.__orientation)
self.__velocity=math.sqrt(linearVelocity[0]**2+linearVelocity[1]**2)
self.__mind.setInput("velocity", self.__velocity)
self.__linearVelocity=linearVelocity
except TypeError:
pass
# if self.__name=="BubbleRob#1":
# print self.__velocity, linearVelocity[0], math.cos(self.__orientation), linearVelocity[1], math.sin(self.__orientation)
else:
self.__velocity=None
# print >> sys.stderr, "Error in VRepBubbleRob.getVelocity()"
returnCode, sensorTrigger, dp, doh, dsnv = vrep.simxReadProximitySensor(self.__clientID, self.__sensorHandle, operationMode)
if returnCode==vrep.simx_return_ok:
# We succeeded at reading the proximity sensor
self.__mind.setInput("lastProximitySensorTime", vrep.simxGetLastCmdTime(self.__clientID))
self.__mind.setInput("sensorTrigger", sensorTrigger)
self.blocked() # judge if blocked
def step(self, action):
done =False
VREP_com.vrep_moves_robot(clientID, Handle, action)
vrep.simxSynchronousTrigger(clientID)
vrep.simxGetPingTime(clientID)
## observe the state after taking movement
Observation_array = []
Observation_array = np.array(Observation_array)
### get joint position, angular_vel, head_vel, torque of each joints
joints_pos = VREP_com.vrep_get_joint_pos(clientID, Handle)
joints_angular_vel = VREP_com.vrep_get_angular_vel(clientID, Handle)
joints_torque = VREP_com.vrep_get_torque(clientID, Handle)
Observation_array = np.concatenate((joints_pos,joints_angular_vel, joints_torque))
err_code ,linearVelocity_cam_each_step, angularVelocity_cam = vrep.simxGetObjectVelocity(clientID,Handle[0, 9],vrep.simx_opmode_blocking)
if err_code !=0 : raise Exception()
robot_vel = linearVelocity_cam_each_step[1]
Observation_array = np.append(Observation_array, round(robot_vel, 4))
Observation_array = np.append(Observation_array, self.Target_vel)
## get reward
power = np.sum(np.abs(joints_torque*joints_angular_vel))
#nor_power = (np.sum((np.abs(joints_torque*joints_angular_vel)))/(self.torque_max*self.angular_vel_max))/8
#print('power: ', power)
#reward = ((1 - (np.abs(self.Target_vel - head_vel))/self.a1)**(1/self.a2))*((np.abs(1-nor_power))**(self.b1**-2))
#reward = np.round(reward, 5)
#print('head_velocity',head_vel )
err_code, MyRobotPos_Current = vrep.simxGetObjectPosition(clientID, Handle[0, 9], -1, vrep.simx_opmode_blocking)
MyRobotPos_Current = np.array(MyRobotPos_Current)
COT_inv = (5.000e-01 * 9.8 * MyRobotPos_Current[1])/power
self.state = np.array(Observation_array)
if robot_vel >= self.Target_vel:
done = True
return self.state, COT_inv, done, {}
def angular_velocity(cid, handle):
err, lin, ang = vrep.simxGetObjectVelocity(cid, handle, vrep_mode)
return ang
def angular_velocity(cid, handle):
err, lin, ang = vrep.simxGetObjectVelocity(cid, handle, vrep_mode)
return ang
def obterVelocidade(self, corpo):
angVel = vrep.simxGetObjectVelocity(self.cliente, corpo,
vrep.simx_opmode_streaming)
print(angVel)
return angVel[1]
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
import sys
vrep.simxFinish(-1)
clientID = vrep.simxStart('192.168.0.19',19999, True, True, 5000, 5)
if clientID != -1:
print('Conexion Establecida!')
else:
sys.exit('Error: No se puede conectar!')
_, left_motor = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
_, right_motor = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)
_, pioneer_handler = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
while True:
position = vrep.simxGetObjectPosition(clientID, pioneer_handler, -1, vrep.simx_opmode_streaming)
orientation = vrep.simxGetObjectOrientation(clientID, pioneer_handler, -1, vrep.simx_opmode_streaming)
velocity = vrep.simxGetObjectVelocity(clientID, pioneer_handler, vrep.simx_opmode_streaming)
quaternion = vrep.simxGetObjectQuaternion(clientID, pioneer_handler, -1, vrep.simx_opmode_streaming)
#print('p>:\t', position)
#print('o>:\t', orientation)
#print('q>:\t', quaternion)
#print(velocity[1])
RotMat = GetFlatRotationMatrix(orientation[1])
rowdata = np.append(RotMat, position[1])
rowdata = np.append(rowdata, velocity[1])
rowdata = np.append(rowdata, velocity[2])
#print(np.shape(rowdata))
print(rowdata)
vrep.simxFinish(-1)
#Sensor handlers
sensorList = ('Proximity_sensor1', 'Proximity_sensor2', 'Proximity_sensor3')
sensor_errorCode, sensor_handles = tv.ObjectHandle(clientID, sensorList)
#Read sensor raw data (first time initial)
readval_errorCode, sensor_val, sensor_state = tv.INI_ReadProximitySensor(clientID, sensor_handles)
#for x in range(1,3+1):
#errorCode,sensor_handle=vrep.simxGetObjectHandle(clientID,'Proximity_sensor'+str(x),vrep.simx_opmode_oneshot_wait)
#sensor_h.append(sensor_handle) #keep list of handles
#errorCode,detectionState,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=vrep.simxReadProximitySensor(clientID,sensor_handles[x-1],vrep.simx_opmode_streaming)
#sensor_val=np.append(sensor_val,np.linalg.norm(detectedPoint)) #get list of values
#sensor_state=np.append(sensor_state,detectionState) #get list of values
errorCode,linearVelocity,angularVelocity=vrep.simxGetObjectVelocity(clientID,car_handle,vrep.simx_opmode_streaming)
sensor_val=np.append(sensor_val,5*np.linalg.norm(linearVelocity))
train = np.floor(20*sensor_val).astype(int)
#Get New Rewards
#errorCode,linearVelocity,angularVelocity=vrep.simxGetObjectVelocity(clientID,cam_handle,vrep.simx_opmode_buffer)
#print(linearVelocity)
weightReadings = [1, 1, 1, 0]
reward = np.dot(train,weightReadings)
reward = reward.astype(int)
state = np.array([train])
state = state.astype(int)
proximitysensor = state[0][0], state[0][1], state[0][2]
loss_log = []
replay = []
def collectImageData(clientID):
list_of_images = []
collector = []
col_lst = []
if clientID != -1:
err, tracer_handle = vrep.simxGetObjectHandle(
clientID, 'LineTracer', vrep.simx_opmode_oneshot_wait)
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, tracer_handle, -1, vrep.simx_opmode_streaming)
count = 0
action = 2
inference_counter = 0
steps = 30
delay = 0
while (vrep.simxGetConnectionId(clientID) != -1 and count < steps):
tim = time.time()
'''The amount of time for inference for my model is .05 - .1 seconds'''
_x = np.random.randint(0, 10)
_x += 10
for i in range(_x):
vrep.simxSynchronousTrigger(clientID)
col_lst.append(detectCollisionSignal(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, tracer_handle, -1, vrep.simx_opmode_oneshot)
ret_code, velo, angle_velo = vrep.simxGetObjectVelocity(
clientID, tracer_handle, vrep.simx_opmode_oneshot)
ret_code, euler_angles = vrep.simxGetObjectOrientation(
clientID, tracer_handle, -1, vrep.simx_opmode_buffer)
collector.append([
pos[0], pos[1], pos[2], velo[0], velo[1], velo[2],
angle_velo[0], angle_velo[1], angle_velo[2],
euler_angles[0], euler_angles[1], euler_angles[2], action
])
if delay <= 0:
action = np.random.randint(0, 5)
delay = np.random.randint(2, 10)
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)
delay -= 1
count += 1
return list_of_images, collector, col_lst
else:
sys.exit()
def step(self, actions):
# Set propeller thrusts
print("Setting propeller thrusts...")
# Only PD control bc can't find api function for getting simulation time
self.propeller_vels = pid(self.pos_error, self.euler_new,
self.euler_error[2], self.vel_error,
self.angvel_error)
self.propeller_vels += actions
# Send propeller thrusts
print("Sending propeller thrusts...")
[
vrep.simxSetFloatSignal(self.clientID, prop, vels,
vrep.simx_opmode_oneshot)
for prop, vels in zip(self.propellers, self.propeller_vels)
]
# Trigger simulator step
print("Stepping simulator...")
vrep.simxSynchronousTrigger(self.clientID)
# Get quadrotor initial position and orientation
err, self.pos_new = vrep.simxGetObjectPosition(self.clientID,
self.quad_handle, -1,
vrep.simx_opmode_buffer)
err, self.euler_new = vrep.simxGetObjectOrientation(
self.clientID, self.quad_handle, -1, vrep.simx_opmode_buffer)
err, self.vel_new, self.angvel_new = vrep.simxGetObjectVelocity(
self.clientID, self.quad_handle, vrep.simx_opmode_buffer)
self.pos_new = np.asarray(self.pos_new)
self.euler_new = np.asarray(self.euler_new) * 10
self.vel_new = np.asarray(self.vel_new)
self.angvel_new = np.asarray(self.angvel_new)
self.pos_old = self.pos_new
self.euler_old = self.euler_new
self.vel_old = self.vel_new
self.angvel_old = self.angvel_new
self.pos_error = (self.pos_start + self.setpoint_delta[0:3]) \
- self.pos_new
self.euler_error = (self.euler_start + self.setpoint_delta[3:6]) \
- self.euler_new
self.euler_error %= 2 * np.pi
for i in range(len(self.euler_error)):
if self.euler_error[i] > np.pi:
self.euler_error[i] -= 2 * np.pi
self.vel_error = (self.vel_start + self.setpoint_delta[6:9]) \
- self.vel_new
self.angvel_error = (self.angvel_start + self.setpoint_delta[9:12]) \
- self.angvel_new
valid = self.is_valid_state()
rew = self.get_reward()
self.timestep += 1
done = False
if self.timestep > self.episode_len or not valid:
done = True
return self.get_state(), rew, done, {}
vrep.simx_opmode_streaming)
# wait for simulation
time.sleep(2)
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, vision_sensor, 0, vrep.simx_opmode_streaming)
ErrorCode, posL = vrep.simxGetJointPosition(clientID, motorLeft,
vrep.simx_opmode_streaming)
ErrorCode, posR = vrep.simxGetJointPosition(clientID, motorRight,
vrep.simx_opmode_streaming)
ErrorCode, pos = vrep.simxGetObjectPosition(clientID, car, -1,
vrep.simx_opmode_streaming)
ErrorCode, euler = vrep.simxGetObjectOrientation(clientID, car, -1,
vrep.simx_opmode_streaming)
speedx, speedy, speedz = vrep.simxGetObjectVelocity(clientID, car,
vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID, motorLeft, 0,
vrep.simx_opmode_buffer)
vrep.simxSetJointTargetPosition(clientID, motorRight, 0,
vrep.simx_opmode_buffer)
time.sleep(1)
euler[1] = -euler[1]
euler[2] = -euler[2]
if euler[2] <= 0:
euler[1] = math.pi - euler[1]
c = euler[1] + math.pi / 2
aux_gpsthetaant = c
desiredSteeringAngle = 0
j = 0
errorCode, left_motor_handle = vrep.simxGetObjectHandle(
clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
errorCode, right_motor_handle = vrep.simxGetObjectHandle(
clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)
# Get visionSensorHandle
visionSensorHandleReturnCode, visionSensorHandle = vrep.simxGetObjectHandle(
clientID, 'cam1', vrep.simx_opmode_oneshot_wait)
pioneerHandleReturnCode, pioneerHandle = vrep.simxGetObjectHandle(
clientID, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
# Setup visionSensorValues
visionSensorReturnCode, visionSensorDetectionState, visionSensorAuxPackets = vrep.simxReadVisionSensor(
clientID, visionSensorHandle, vrep.simx_opmode_streaming)
pioneerVelocityReturnCode, pioneerVelocityLinearVelocity, pioneerVelocityAngularVelocity = vrep.simxGetObjectVelocity(
clientID, pioneerHandle, vrep.simx_opmode_streaming)
sensor_h = [] #empty list for handles
sensor_val = np.array([]) #empty array for sensor measurements
#orientation of all the sensors:
sensor_loc = np.array([
-PI / 2, -50 / 180.0 * PI, -30 / 180.0 * PI, -10 / 180.0 * PI,
10 / 180.0 * PI, 30 / 180.0 * PI, 50 / 180.0 * PI, PI / 2, PI / 2,
130 / 180.0 * PI, 150 / 180.0 * PI, 170 / 180.0 * PI, -170 / 180.0 * PI,
-150 / 180.0 * PI, -130 / 180.0 * PI, -PI / 2
])
#for loop to retrieve sensor arrays and initiate sensors
for x in range(1, 16 + 1):
errorCode, sensor_handle = vrep.simxGetObjectHandle(
def collectImageData(clientID):
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)
t_end = time.time() + 2.8
collision_bool = False
count = 0
updated_counter = 0
while (vrep.simxGetConnectionId(clientID) != -1
and time.time() < t_end):
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, v0, 0, vrep.simx_opmode_buffer)
if count % 25 == 0:
act = np.random.randint(5)
action = (act - 2) * 15
updated_counter += 1
return_val = vrep.simxSetJointTargetVelocity(
clientID, left_handle, action, vrep.simx_opmode_oneshot)
return_val2 = vrep.simxSetJointTargetVelocity(
clientID, right_handle, action,
vrep.simx_opmode_oneshot_wait)
#convert the image add to numpy array we collect about 35 images per simulation
if res == vrep.simx_return_ok:
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
])
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)
count += 1
return list_of_images, collector
else:
sys.exit()
def getState():
#height
res, handle = vrep.simxGetObjectHandle(clientID, 'LHipYawPitch3',
vrep.simx_opmode_blocking)
print(handle)
res, pos = vrep.simxGetObjectPosition(clientID, handle, -1,
vrep.simx_opmode_blocking)
height = pos[2]
#torso
res, handle = vrep.simxGetObjectHandle(clientID, "imported_part_20_sub0",
vrep.simx_opmode_blocking)
print(handle)
res, Angle = vrep.simxGetObjectOrientation(clientID, handle, -1,
vrep.simx_opmode_blocking)
res, Torsolv, Torsoav = vrep.simxGetObjectVelocity(
clientID, handle, vrep.simx_opmode_blocking)
TorsoAngle = Angle
#Rthigh
res, handle = vrep.simxGetObjectHandle(clientID, "imported_part_13_sub",
vrep.simx_opmode_blocking)
res, Angle = vrep.simxGetObjectOrientation(clientID, handle, -1,
vrep.simx_opmode_blocking)
res, Rthighlv, Rthighav = vrep.simxGetObjectVelocity(
clientID, handle, vrep.simx_opmode_blocking)
RthighAngle = Angle
#Lthigh
res, handle = vrep.simxGetObjectHandle(clientID, "imported_part_10_sub",
vrep.simx_opmode_blocking)
res, Angle = vrep.simxGetObjectOrientation(clientID, handle, -1,
vrep.simx_opmode_blocking)
res, Lthighlv, Lthighav = vrep.simxGetObjectVelocity(
clientID, handle, vrep.simx_opmode_blocking)
LthighAngle = Angle
#Rcalf
res, handle = vrep.simxGetObjectHandle(clientID, "r_knee_pitch_link_pure2",
vrep.simx_opmode_blocking)
res, Angle = vrep.simxGetObjectOrientation(clientID, handle, -1,
vrep.simx_opmode_blocking)
res, Rcalflv, Rcalfav = vrep.simxGetObjectVelocity(
clientID, handle, vrep.simx_opmode_blocking)
RcalfAngle = Angle
#Lcalf
res, handle = vrep.simxGetObjectHandle(clientID, "l_knee_pitch_link_pure2",
vrep.simx_opmode_blocking)
res, Angle = vrep.simxGetObjectOrientation(clientID, handle, -1,
vrep.simx_opmode_blocking)
res, Lcalflv, Lcalfav = vrep.simxGetObjectVelocity(
clientID, handle, vrep.simx_opmode_blocking)
LcalfAngle = Angle
state = [height]
state.extend(TorsoAngle)
state.extend(Torsolv)
state.extend(Torsoav)
state.extend(RthighAngle)
state.extend(Rthighlv)
state.extend(Rthighav)
state.extend(LthighAngle)
state.extend(Lthighlv)
state.extend(Lthighav)
state.extend(RcalfAngle)
state.extend(Rcalflv)
state.extend(Rcalfav)
state.extend(LcalfAngle)
state.extend(Lcalflv)
state.extend(Lcalfav)
state = np.array(state)
return state
def readSensorData(self):
if self.scene.vrepConnected == False:
return
if "readSensorData_firstCall" not in self.__dict__:
self.readSensorData_firstCall = True
else:
self.readSensorData_firstCall = False
# Read robot states
res, pos = vrep.simxGetObjectPosition(self.scene.clientID,
self.robotHandle, -1,
vrep.simx_opmode_blocking)
if res != 0:
raise VrepError("Cannot get object position with error code " +
str(res))
res, ori = vrep.simxGetObjectOrientation(self.scene.clientID,
self.robotHandle, -1,
vrep.simx_opmode_blocking)
if res != 0:
raise VrepError("Cannot get object orientation with error code " +
str(res))
res, vel, omega = vrep.simxGetObjectVelocity(self.scene.clientID,
self.robotHandle,
vrep.simx_opmode_blocking)
if res != 0:
raise VrepError("Cannot get object velocity with error code " +
str(res))
#print("Linear speed: %.3f" % (vel[0]**2 + vel[1]**2)**0.5,
# "m/s. Angular speed: %.3f" % omega[2])
#print("pos: %.2f" % pos[0], ", %.2f" % pos[1])
#print("Robot #", self.index, " ori: %.3f" % ori[0], ", %.3f" % ori[1], ", %.3f" % ori[2])
self.xi.x = pos[0]
self.xi.y = pos[1]
self.xi.alpha = ori[0]
self.xi.beta = ori[1]
self.xi.theta = ori[2]
sgn = np.sign(
np.dot(
np.asarray(vel[0:2]),
np.asarray([math.cos(self.xi.theta),
math.sin(self.xi.theta)])))
self.vActual = sgn * (vel[0]**2 + vel[1]**2)**0.5
self.omegaActual = omega[2]
# Read laser/vision sensor data
if self.scene.SENSOR_TYPE == "2d_":
# self.laserFrontHandle
# self.laserRearHandle
if self.readSensorData_firstCall:
opmode = vrep.simx_opmode_streaming
else:
opmode = vrep.simx_opmode_buffer
laserFront_points = vrep.simxGetStringSignal(
self.scene.clientID, self.laserFrontName + '_signal', opmode)
print(self.laserFrontName + '_signal: ', len(laserFront_points[1]))
laserRear_points = vrep.simxGetStringSignal(
self.scene.clientID, self.laserRearName + '_signal', opmode)
print(self.laserRearName + '_signal: ', len(laserRear_points[1]))
elif self.scene.SENSOR_TYPE == "2d": # deprecated
raise Exception('2d sensor is not supported!!!!')
elif self.scene.SENSOR_TYPE == "VPL16":
# self.pointCloudHandle
velodyne_points = vrep.simxCallScriptFunction(
self.scene.clientID, self.pointCloudName, 1,
'getVelodyneData_function', [], [], [], 'abc',
vrep.simx_opmode_blocking)
#print(len(velodyne_points[2]))
#print(velodyne_points[2])
res = velodyne_points[0]
# Parse data
if 'VPL16_counter' not in self.__dict__:
self.VPL16_counter = 0
# reset the counter every fourth time
if self.VPL16_counter == 4:
self.VPL16_counter = 0
if self.VPL16_counter == 0:
# Reset point cloud
self.pointCloud.clearData()
#print('VPL16_counter = ', self.VPL16_counter)
self.pointCloud.addRawData(velodyne_points[2]) # will rotate here
if self.VPL16_counter == 3:
#print("Length of point cloud is " + str(len(self.pointCloud.data)))
if res != 0:
raise VrepError("Cannot get point cloud with error code " +
str(res))
#start = time.clock()
self.pointCloud.crop()
#end = time.clock()
#self.pointCloud.updateScanVector() # option 2
self.pointCloud.updateOccupancyMap() # option 1
#print('Time elapsed: ', end - start)
self.VPL16_counter += 1
elif self.scene.SENSOR_TYPE == "kinect":
pass
else:
return
setMotorPosition(clientID, steer_handle, 0)
vrep.simxSynchronousTrigger(clientID)
e = np.zeros((4, 1))
while not Reach:
print("-------------------------------------------")
vehLin = np.zeros(3)
_, vehiclePos = vrep.simxGetObjectPosition(clientID, vehicle_handle,
-1,
vrep.simx_opmode_blocking)
_, vehicleOri = vrep.simxGetObjectOrientation(
clientID, vehicle_handle, -1, vrep.simx_opmode_blocking)
_, vehicleLin, vehicleAng = vrep.simxGetObjectVelocity(
clientID, vehicle_handle, vrep.simx_opmode_streaming)
## global coordinate
vehLin[0] = (math.cos(vehicleOri[2]) * vehicleLin[0] +
math.sin(vehicleOri[2]) * vehicleLin[1]
) ## Rotate Vx from world frame to vehicle frame
vehLin[1] = (-math.sin(vehicleOri[2]) * vehicleLin[0] +
math.cos(vehicleOri[2]) * vehicleLin[1]
) ## Rotate Vy form wold frame to vehicle frame
vehLin[2] = vehicleLin[2]
goalDist = np.linalg.norm(np.subtract(vehiclePos, goalPos))
if goalDist < 0.5:
Reach = True
def show_speed(self):
_, spe, _ = vrep.simxGetObjectVelocity(self.client_id, self.body,
vrep.simx_opmode_oneshot)
# speed
print(sqrt(spe[0]**2 + spe[1]**2 + spe[2]**2), end='\r')
def __init__(self, episode_len=None):
self.clientID = None
# Start V-REP connection
try:
vrep.simxFinish(-1)
print("Connecting to simulator...")
self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True,
5000, 5)
if self.clientID == -1:
print("Failed to connect to remote API Server")
self.vrep_exit()
except KeyboardInterrupt:
self.vrep_exit()
return
self.episode_len = episode_len
self.timestep = 0
self.dt = .001
self.propellers = [
'rotor1thrust', 'rotor2thrust', 'rotor3thrust', 'rotor4thrust'
]
self.quad_name = 'Quadricopter'
self.scene_name = 'quad_innerloop.ttt'
self.setpoint_delta = np.array(
[0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0.])
self.quad_handle = None
self.pos_start = np.zeros(3)
self.euler_start = np.zeros(3)
self.vel_start = np.zeros(3)
self.angvel_start = np.zeros(3)
self.pos_old = self.pos_start
self.euler_old = self.euler_start
self.vel_old = self.vel_start
self.angvel_old = self.angvel_start
self.pos_new = self.pos_old
self.euler_new = self.euler_old
self.vel_new = self.vel_old
self.angvel_new = self.angvel_old
ob_high = np.array([
10., 10., 10., 20., 20., 20., 21., 21., 21., 21., 21., 21., 10.,
10., 10., 20., 20., 20., 21., 21., 21., 21., 21., 21.
])
ob_low = -ob_high
self.observation_space = Box(low=ob_low,
high=ob_high,
dtype=np.float32)
ac_high = np.array([20., 20., 20., 20.])
ac_low = np.zeros(4)
self.action_space = Box(low=ac_low, high=ac_high, dtype=np.float32)
print("Setting simulator to async mode...")
vrep.simxSynchronous(self.clientID, True)
vrep.simxSetFloatingParameter(
self.clientID,
vrep.sim_floatparam_simulation_time_step,
self.dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
# Load V-REP scene
print("Loading scene...")
vrep.simxLoadScene(self.clientID, self.scene_name, 0xFF,
vrep.simx_opmode_blocking)
# Get quadrotor handle
err, self.quad_handle = vrep.simxGetObjectHandle(
self.clientID, self.quad_name, vrep.simx_opmode_blocking)
# Initialize quadrotor position and orientation
vrep.simxGetObjectPosition(self.clientID, self.quad_handle, -1,
vrep.simx_opmode_streaming)
vrep.simxGetObjectOrientation(self.clientID, self.quad_handle, -1,
vrep.simx_opmode_streaming)
vrep.simxGetObjectVelocity(self.clientID, self.quad_handle,
vrep.simx_opmode_streaming)
def get_linear_velocity(self):
res, linear_vel, angular_vel = vrep.simxGetObjectVelocity(
self.client_id, self.pioneer, vrep.simx_opmode_oneshot_wait)
return linear_vel
def reset(self):
vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot_wait)
time.sleep(0.1)
# Setup V-REP simulation
print("Setting simulator to async mode...")
vrep.simxSynchronous(self.clientID, True)
vrep.simxSetFloatingParameter(
self.clientID,
vrep.sim_floatparam_simulation_time_step,
self.dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
# Load V-REP scene
print("Loading scene...")
vrep.simxLoadScene(self.clientID, self.scene_name, 0xFF,
vrep.simx_opmode_blocking)
# Get quadrotor handle
err, self.quad_handle = vrep.simxGetObjectHandle(
self.clientID, self.quad_name, vrep.simx_opmode_blocking)
# Initialize quadrotor position and orientation
vrep.simxGetObjectPosition(self.clientID, self.quad_handle, -1,
vrep.simx_opmode_streaming)
vrep.simxGetObjectOrientation(self.clientID, self.quad_handle, -1,
vrep.simx_opmode_streaming)
vrep.simxGetObjectVelocity(self.clientID, self.quad_handle,
vrep.simx_opmode_streaming)
# Start simulation
vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_blocking)
# Initialize rotors
print("Initializing propellers...")
for i in range(len(self.propellers)):
vrep.simxClearFloatSignal(self.clientID, self.propellers[i],
vrep.simx_opmode_oneshot)
# Get quadrotor initial position and orientation
err, self.pos_start = vrep.simxGetObjectPosition(
self.clientID, self.quad_handle, -1, vrep.simx_opmode_buffer)
err, self.euler_start = vrep.simxGetObjectOrientation(
self.clientID, self.quad_handle, -1, vrep.simx_opmode_buffer)
err, self.vel_start, self.angvel_start = vrep.simxGetObjectVelocity(
self.clientID, self.quad_handle, vrep.simx_opmode_buffer)
self.pos_start = np.asarray(self.pos_start)
self.euler_start = np.asarray(self.euler_start) * 10.
self.vel_start = np.asarray(self.vel_start)
self.angvel_start = np.asarray(self.angvel_start)
self.pos_old = self.pos_start
self.euler_old = self.euler_start
self.vel_old = self.vel_start
self.angvel_old = self.angvel_start
self.pos_new = self.pos_old
self.euler_new = self.euler_old
self.vel_new = self.vel_old
self.angvel_new = self.angvel_old
self.pos_error = (self.pos_start + self.setpoint_delta[0:3]) \
- self.pos_new
self.euler_error = (self.euler_start + self.setpoint_delta[3:6]) \
- self.euler_new
self.vel_error = (self.vel_start + self.setpoint_delta[6:9]) \
- self.vel_new
self.angvel_error = (self.angvel_start + self.setpoint_delta[9:12]) \
- self.angvel_new
self.pos_error_all = self.pos_error
self.euler_error_all = self.euler_error
self.init_f = 5.8
self.propeller_vels = [
self.init_f, self.init_f, self.init_f, self.init_f
]
self.timestep = 1
return self.get_state()
gpsZ = vrep.simxGetFloatSignal(clientID, 'gpsZ',
vrep.simx_opmode_oneshot)[1]
#print("GPS : {:.5f} {:.5f} {:.5f}".format(gpsX, gpsY, gpsZ))
# Absolute Position Get
errorCode, pos = vrep.simxGetObjectPosition(clientID, car_handle, -1,
vrep.simx_opmode_streaming)
#print("Position : {:.5f} {:.5f} {:.5f}\n".format(pos[0], pos[1], pos[2]))
# Orientation Get
errorCode, ori = vrep.simxGetObjectOrientation(
clientID, car_handle, -1, vrep.simx_opmode_streaming)
# Velocity
errorCode, LinearV, AngularV = vrep.simxGetObjectVelocity(
clientID, car_handle,
vrep.simx_opmode_streaming if i is 1 else vrep.simx_opmode_buffer)
# Distance
errorCode, d = vrep.simxReadDistance(clientID, dist_handle,
vrep.simx_opmode_streaming)
# Simulation Time
if t != vrep.simxGetLastCmdTime(clientID):
t = vrep.simxGetLastCmdTime(clientID)
if not i % 100:
print(i)
for col, data in enumerate([
pos[0], pos[1], pos[2], gyroX, gyroY, gyroZ, accelX,
def QC_controller(Quadricopter_target, Quadricopter_base, Quadricopter):
try:
import vrep
except:
print('--------------------------------------------------------------')
print('"vrep.py" could not be imported. This means very probably that')
print('either "vrep.py" or the remoteApi library could not be found.')
print('Make sure both are in the same folder as this file,')
print('or appropriately adjust the file "vrep.py"')
print('--------------------------------------------------------------')
print('')
import time
import numpy as np
from captains_log_v1 import log_data
import os
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
# output limits:
#min_output=0
#max_output=8.335
# program parameters:
global i
i = 0
xe = []
ye = []
ze = []
xs = []
ys = []
zs = []
x_qc = []
y_qc = []
z_qc = []
u = []
v1 = []
v2 = []
v3 = []
v4 = []
#global variables:
cumul = 0
last_e = 0
pAlphaE = 0
pBetaE = 0
psp2 = 0
psp1 = 0
prevEuler = 0
cumulAlpha = 0
cumulBeta = 0
cumulAlphaPos = 0
cumulBetaPos = 0
particlesTargetVelocities = [0, 0, 0, 0]
# weight how much the parameters are changed each iteration:
a = 0.1
# Compute derivative with respec to first parameter:
delta = 0.01
# control signals parameters:
last_thrust = 0
#speed weight:
vParam = -2
#parameters for vertical control
Kpv = 2
Kiv = 0
Kdv = 2
#parameters for horizontal control:
Kph = 0.4
Kih = 0.1
Kdh = 1.5
Kph_pos1 = 0.4
Kih_pos1 = 0.001
Kdh_pos1 = 0.05
Kph_pos0 = 0.4
Kih_pos0 = 0.001
Kdh_pos0 = 0.05
#parameters for rotational control:
Kpr = 0.05
Kir = 0
Kdr = 0.9
theta_v = [Kpv, Kiv, Kdv]
theta_h = [
Kph, Kih, Kdh, Kph_pos1, Kih_pos1, Kdh_pos1, Kph_pos0, Kih_pos0,
Kdh_pos0
]
theta_r = [Kpr, Kir, Kdr]
def sum_lists(l1, l2):
""" Arguments: list1, list2 """
ln = []
for i in xrange(len(l1)):
ln.append(l1[i] + l2[i])
return ln
def diff_lists(l1, l2):
""" Arguments: list1, list2 """
ln = []
for i in xrange(len(l1)):
ln.append(l1[i] - l2[i])
return ln
def dot_multiply(l1, k):
""" Arguments: list, scalar """
ln = []
for j in l1:
pom = j * k
ln.append(pom)
return ln
def cost(theta, sigma_err, delta_u):
""" theta = [Kp, Ki, Kd] """
# step (step of sum TBD?):
dt = 0.01
""" sigma_err = sum of errors -> integral/sum function
-> These sums are gained in the code. """
# Compute the integral:
t0 = 0
tf = 1
J = (1 / (tf - t0)) * (sigma_err**2 + delta_u**2) * dt
return J
def autotune(theta, var):
# weight how much the parameters are changed each iteration:
a = 0.1
# Compute derivative with respec to first parameter:
delta = 0.01
theta_v = [Kpv, Kiv, Kdv]
theta_h = [
Kph, Kih, Kdh, Kph_pos1, Kih_pos1, Kdh_pos1, Kph_pos0, Kih_pos0,
Kdh_pos0
]
theta_r = [Kpr, Kir, Kdr]
var_vi = [1, 0, 0]
var_vj = [0, 1, 0]
var_vk = [0, 0, 1]
theta_vKp1 = sum_lists(theta_v, dot_multiply(
var_vi, delta)) #theta_v + dot_multiply(var_vi,delta)
theta_vKp2 = diff_lists(theta_v, dot_multiply(
var_vi, delta)) #theta_v - dot_multiply(var_vi,delta)
theta_vKi1 = sum_lists(theta_v, dot_multiply(
var_vj, delta)) #theta_v + dot_multiply(var_vj,delta)
theta_vKi2 = diff_lists(theta_v, dot_multiply(
var_vj, delta)) #theta_v - dot_multiply(var_vj,delta)
theta_vKd1 = sum_lists(theta_v, dot_multiply(
var_vk, delta)) #theta_v + dot_multiply(var_vk,delta)
theta_vKd2 = diff_lists(theta_v, dot_multiply(
var_vk, delta)) #theta_v - dot_multiply(var_vk,delta)
derivative_vKp = (cost(theta_vKp1, cumul, delta_thrust) -
cost(theta_vKp2, cumul, delta_thrust)) / (2 * delta)
derivative_vKi = (cost(theta_vKi1, cumul, delta_thrust) -
cost(theta_vKi2, cumul, delta_thrust)) / (2 * delta)
derivative_vKd = (cost(theta_vKd1, cumul, delta_thrust) -
cost(theta_vKd2, cumul, delta_thrust)) / (2 * delta)
theta_v1 = sum_lists(theta_v,
dot_multiply(derivative_vKp,
a)) # OVA IDE NA KRAJ OD LOOP
theta_v2 = sum_lists(theta_v, dot_multiply(derivative_vKi, a))
theta_v3 = sum_lists(theta_v, dot_multiply(derivative_vKd, a))
theta_v = min(theta_v1, theta_v2, theta_v3)
derivative_h = (cost() - cost()) / (2 * delta)
derivative_r = (cost() - cost()) / (2 * delta)
""" SIMULATION: theta = [Kp Ki Kd] => sim(theta) """
theta_new = theta - dot_multiply(derivative, a) #a*derivative
print theta_new
return theta_new
print('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000,
5) # Connect to V-REP
if clientID != -1:
print('Connected to remote API server')
# enable the synchronous mode on the client:
vrep.simxSynchronous(clientID, True)
# start the simulation:
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
#functional/handle code:
emptyBuff = bytearray()
[returnCode,
targetObj] = vrep.simxGetObjectHandle(clientID, Quadricopter_target,
vrep.simx_opmode_blocking)
[returnCode,
qc_base_handle] = vrep.simxGetObjectHandle(clientID,
Quadricopter_base,
vrep.simx_opmode_blocking)
[returnCode,
qc_handle] = vrep.simxGetObjectHandle(clientID, Quadricopter,
vrep.simx_opmode_blocking)
# main loop:
while True:
#theta_v = [Kpv, Kiv, Kdv]
'''==========''' ''' vertical control: ''' '''=========='''
[returnCode,
targetPos] = vrep.simxGetObjectPosition(clientID, targetObj, -1,
vrep.simx_opmode_blocking)
[returnCode,
pos] = vrep.simxGetObjectPosition(clientID, qc_base_handle, -1,
vrep.simx_opmode_blocking)
[returnCode, l,
w] = vrep.simxGetObjectVelocity(clientID, qc_base_handle,
vrep.simx_opmode_blocking)
e = targetPos[2] - pos[2]
cumul = cumul + e
diff_e = e - last_e
Pvert = theta_v[0] * e
Ivert = theta_v[1] * cumul
Dvert = theta_v[2] * diff_e
thrust = 5.335 + Pvert + Ivert + Dvert + l[2] * vParam # get thrust
delta_thrust = thrust - last_thrust
last_thrust = thrust
last_e = e
# autotune:
# """ Code for self-tuning """
# var_vi = [1,0,0]
# var_vj = [0,1,0]
# var_vk = [0,0,1]
#
# theta_vKp1 = sum_lists(theta_v,dot_multiply(var_vi,delta))#theta_v + dot_multiply(var_vi,delta)
# theta_vKp2 = diff_lists(theta_v,dot_multiply(var_vi,delta)) #theta_v - dot_multiply(var_vi,delta)
# theta_vKi1 = sum_lists(theta_v,dot_multiply(var_vj,delta)) #theta_v + dot_multiply(var_vj,delta)
# theta_vKi2 = diff_lists(theta_v,dot_multiply(var_vj,delta)) #theta_v - dot_multiply(var_vj,delta)
# theta_vKd1 = sum_lists(theta_v,dot_multiply(var_vk,delta)) #theta_v + dot_multiply(var_vk,delta)
# theta_vKd2 = diff_lists(theta_v,dot_multiply(var_vk,delta)) #theta_v - dot_multiply(var_vk,delta)
#
# derivative_vKp = (cost(theta_vKp1,cumul,delta_thrust) - cost(theta_vKp2,cumul,delta_thrust))/(2*delta)
# derivative_vKi = (cost(theta_vKi1,cumul,delta_thrust) - cost(theta_vKi2,cumul,delta_thrust))/(2*delta)
# derivative_vKd = (cost(theta_vKd1,cumul,delta_thrust) - cost(theta_vKd2,cumul,delta_thrust))/(2*delta)
#
# print derivative_vKp
# theta_v1 = sum_lists(theta_v,dot_multiply(derivative_vKp,a))# OVA IDE NA KRAJ OD LOOP
# theta_v2 = sum_lists(theta_v,dot_multiply(derivative_vKi,a))
# theta_v3 = sum_lists(theta_v,dot_multiply(derivative_vKd,a))
# theta_v = min(theta_v1, theta_v2, theta_v3)
# print " THETA_V =", theta_v
# theta = [Kpv, Kiv, Kdv]
# sigma_err = cumul
# autotune(theta, sigma_err)
'''==========''' ''' horizontal control: ''' '''=========='''
[returnCode,
sp] = vrep.simxGetObjectPosition(clientID, targetObj,
qc_base_handle,
vrep.simx_opmode_blocking)
[rc, rc, vx, rc, rc] = vrep.simxCallScriptFunction(
clientID, Quadricopter, vrep.sim_scripttype_childscript,
'qc_Get_vx', [], [], [], emptyBuff, vrep.simx_opmode_blocking)
[rc, rc, vy, rc, rc] = vrep.simxCallScriptFunction(
clientID, Quadricopter, vrep.sim_scripttype_childscript,
'qc_Get_vy', [], [], [], emptyBuff, vrep.simx_opmode_blocking)
[rc, rc, rc, rc,
rc] = vrep.simxCallScriptFunction(clientID, Quadricopter,
vrep.sim_scripttype_childscript,
'qc_Get_Object_Matrix', [], [],
[], emptyBuff,
vrep.simx_opmode_blocking)
[errorCode,
M] = vrep.simxGetStringSignal(clientID, 'mtable',
vrep.simx_opmode_oneshot_wait)
if (errorCode == vrep.simx_return_ok):
m = vrep.simxUnpackFloats(M)
alphaE = vy[2] - m[11]
cumulAlpha = cumulAlpha + alphaE
diff_alphaE = alphaE - pAlphaE
alphaCorr = Kph * alphaE + Kih * cumulAlpha + Kdh * diff_alphaE #alpha correction
betaE = vx[2] - m[11]
cumulBeta = cumulBeta + betaE
diff_betaE = betaE - pBetaE
betaCorr = -Kph * betaE - Kih * cumulBeta - Kdh * diff_betaE #beta correction
pAlphaE = alphaE
pBetaE = betaE
cumulAlphaPos = cumulAlphaPos + sp[1]
cumulBetaPos = cumulBetaPos + sp[0]
alphaPos = Kph_pos1 * (
sp[1]) + Kih_pos1 * cumulAlphaPos + Kdh_pos1 * (
sp[1] - psp2) #alpha position correction
betaPos = Kph_pos0 * (
sp[0]) + Kih_pos0 * cumulBetaPos + Kdh_pos0 * (
sp[0] - psp1) #beta position correction
alphaCorr = alphaCorr + alphaPos
betaCorr = betaCorr - betaPos
psp2 = sp[1]
psp1 = sp[0]
'''==========''' ''' rotational control: ''' '''=========='''
[returnCode,
euler] = vrep.simxGetObjectOrientation(clientID, targetObj,
qc_base_handle,
vrep.simx_opmode_blocking)
[returnCode, orientation
] = vrep.simxGetObjectOrientation(clientID, qc_base_handle, -1,
vrep.simx_opmode_blocking)
Prot = Kpr * euler[2]
Drot = Kdr * (euler[2] - prevEuler)
rotCorr = Prot + Drot
prevEuler = euler[2]
# set propeller velocities:
propeller1_PTV = thrust * (1 - alphaCorr + betaCorr - rotCorr)
propeller2_PTV = thrust * (1 - alphaCorr - betaCorr + rotCorr)
propeller3_PTV = thrust * (1 + alphaCorr - betaCorr - rotCorr)
propeller4_PTV = thrust * (1 + alphaCorr + betaCorr + rotCorr)
particlesTargetVelocities = [
propeller1_PTV, propeller2_PTV, propeller3_PTV, propeller4_PTV
]
'''============================================================='''
C_parameters_vert = [0, 0, 0]
C_parameters_horr = [0, 0, 0, 0, 0, 0, 0]
C_parameters_rot = [0, 0, 0]
vert_comp = [0, 0, 0, 0]
horr_comp = [0, 0, 0, 0, [0, 0, 0], 0, 0, 0, 0, 0, 0]
rot_comp = [[0, 0, 0], 0]
C_parameters_vert[0] = Kpv
C_parameters_vert[1] = Kiv
C_parameters_vert[2] = Kdv
C_parameters_horr[0] = Kph
C_parameters_horr[1] = Kih
C_parameters_horr[2] = Kdh
C_parameters_horr[3] = Kph_pos0
C_parameters_horr[4] = Kdh_pos0
C_parameters_horr[5] = Kph_pos1
C_parameters_horr[6] = Kdh_pos1
C_parameters_rot[0] = Kpr
C_parameters_rot[1] = Kir
C_parameters_rot[2] = Kdr
vert_comp[0] = targetPos
vert_comp[1] = pos
vert_comp[2] = e
vert_comp[3] = thrust
horr_comp[0] = alphaE
horr_comp[1] = betaE
horr_comp[2] = cumulAlpha
horr_comp[3] = cumulBeta
horr_comp[4] = sp
horr_comp[5] = cumulAlphaPos
horr_comp[6] = cumulBetaPos
horr_comp[7] = alphaCorr
horr_comp[8] = betaCorr
horr_comp[9] = vx
horr_comp[10] = vy
rot_comp[0] = euler
rot_comp[1] = rotCorr
'''==========''' ''' PLOTTING ''' '''=========='''
ze.append(e) # otstapuvanje od z-oska
xe.append(sp[0]) # otstapuvanje od x-oska
ye.append(sp[1]) # otstapuvawe od y-oska
xs.append(targetPos[0])
ys.append(targetPos[1])
zs.append(targetPos[2])
x_qc.append(pos[0])
y_qc.append(pos[1])
z_qc.append(pos[2])
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
# ax.xaxis.label.set_color('blue')
# ax.yaxis.label.set_color('blue')
# ax.zaxis.label.set_color('blue')
# ax.tick_params(axis='xs',colors='blue')
# ax.tick_params(axis='ys',colors='blue')
# ax.tick_params(axis='zs',colors='blue')
ax.plot_wireframe(xs, ys, zs, color='blue')
# plt.hold(True)
# ax.xaxis.label.set_color('red')
# ax.yaxis.label.set_color('red')
# ax.zaxis.label.set_color('red')
# ax.tick_params(axis='x_qc',colors='red')
# ax.tick_params(axis='y_qc',colors='red')
# ax.tick_params(axis='z_qc',colors='red')
ax.plot_wireframe(x_qc, y_qc, z_qc, color='red')
plt.show()
# u.append(thrust)
# v1.append(propeller1_PTV)
# v2.append(propeller2_PTV)
# v3.append(propeller3_PTV)
# v4.append(propeller4_PTV)
#
# plt.plot(v1,color='blue')
# plt.hold(True)
# plt.plot(v2,color='red')
# plt.hold(True)
# plt.plot(v3,color='green')
# plt.hold(True)
# plt.plot(v4,color='pink')
# plt.hold(True)
# plt.axis([0,25,0,15])
# plt.show()
#
# plt.plot(xe,color='blue')
# plt.axis([0,100,-4,4])
# plt.show()
# plt.plot(ye,color='red')
# plt.axis([0,100,-4,4])
# plt.show()
# plt.plot(ze,color='green')
# plt.axis([0,100,-4,4])
# plt.show()
'''============================================================='''
# print '////////////'
# print '------------'
# print '-Controller parameters-'
# print 'Vertical control parameters=',[Kpv,Kiv,Kdv]
# print 'Horizontal control parameters=',[Kph,Kih,Kdh,Kph_pos0,Kdh_pos0,Kph_pos1,Kdh_pos1]
# print 'Rotational control parameters=',[Kpr,Kir,Kdr]
# print '------------'
# print '-Vertical component-'
# print 'targetPos=',targetPos
# print 'pos=',pos
# print 'e=',e
# print 'thrust=',thrust
# print '------------'
# print '-Horizontal component-'
# print 'cumulAlpha=',cumulAlpha
# print 'cumulBeta=',cumulBeta
# print 'cumulAlphaPos=',cumulAlphaPos
# print 'cumulBetaPos=',cumulBetaPos
# print 'alphaE=',alphaE
# print 'betaE=',betaE
# print 'alphaCorr=',alphaCorr
# print 'betaCorr=',betaCorr
# print 'orientation=',orientation
# print 'sp_X=',sp[0]
# print 'sp_Y=',sp[1]
# print '------------'
# print '-Rotational component-'
# print 'vx=',vx
# print 'vy=',vy
# print 'gamma=',euler[2]
# print 'rotCorr=',rotCorr
# print '------------'
# print 'Velocity_propeller_1 = ',particlesTargetVelocities[0]
# print 'Velocity_propeller_2 = ',particlesTargetVelocities[1]
# print 'Velocity_propeller_3 = ',particlesTargetVelocities[2]
# print 'Velocity_propeller_4 = ',particlesTargetVelocities[3]
# print '------------'
# print '////////////'
'''============================================================='''
## WRITE TO TEXT:
log_data(C_parameters_vert, C_parameters_horr, C_parameters_rot,
vert_comp, horr_comp, rot_comp, particlesTargetVelocities,
i)
i += 1
# send propeller velocities to output:
[res, retInts, retFloats, retStrings,
retBuffer] = vrep.simxCallScriptFunction(
clientID, Quadricopter, vrep.sim_scripttype_childscript,
'qc_propeller_v', [], particlesTargetVelocities, [],
emptyBuff, vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(clientID)
# stop the simulation:
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
# start the simulation:
vrep.simxStartSimulation(clientID,vrep.simx_opmode_blocking)
#functional/handle code:
emptyBuff=bytearray()
[returnCode,targetObj]=vrep.simxGetObjectHandle(clientID,'Quadricopter_target',vrep.simx_opmode_blocking)
[returnCode,qc_base_handle]=vrep.simxGetObjectHandle(clientID,'Quadricopter_base',vrep.simx_opmode_blocking)
[returnCode,qc_handle]=vrep.simxGetObjectHandle(clientID,'Quadricopter',vrep.simx_opmode_blocking)
# main loop:
while True:
# vertical control:
[returnCode,targetPos]=vrep.simxGetObjectPosition(clientID,targetObj,-1,vrep.simx_opmode_blocking)
[returnCode,pos]=vrep.simxGetObjectPosition(clientID,qc_base_handle,-1,vrep.simx_opmode_blocking)
[returnCode, l, w] = vrep.simxGetObjectVelocity(clientID, qc_base_handle, vrep.simx_opmode_blocking)
e=targetPos[2]-pos[2]
cumul=cumul+e
diff_e=e-last_e
Pvert=Kpv*e
Ivert=Kiv*cumul
Dvert=Kdv*diff_e
thrust=5.335+Pvert+Ivert+Dvert+l[2]*vParam# get thrust
last_e=e
# horizontal control:
[returnCode,sp]=vrep.simxGetObjectPosition(clientID,targetObj,qc_base_handle,vrep.simx_opmode_blocking)
[rc,rc,vx,rc,rc]=vrep.simxCallScriptFunction(clientID,'Quadricopter',vrep.sim_scripttype_childscript,'qc_Get_vx',[],[],[],emptyBuff,vrep.simx_opmode_blocking)
[rc,rc,vy,rc,rc]=vrep.simxCallScriptFunction(clientID,'Quadricopter',vrep.sim_scripttype_childscript,'qc_Get_vy',[],[],[],emptyBuff,vrep.simx_opmode_blocking)
[rc,rc,rc,rc,rc]=vrep.simxCallScriptFunction(clientID,'Quadricopter',vrep.sim_scripttype_childscript,'qc_Get_Object_Matrix',[],[],[],emptyBuff,vrep.simx_opmode_blocking)
[errorCode,M]=vrep.simxGetStringSignal(clientID,'mtable',vrep.simx_opmode_oneshot_wait);
ps = np.arange(0.004,0.02,0.002)
ds = np.arange(0.5,4,0.5)
with open("mesh.txt",'w') as f:
for p in ps:
for d in ds:
f.write("p: "+str(round(p,2))+"\td: "+str(round(d,2))+'\t')
print("p: "+str(round(p,2))+"\td: "+str(round(d,2)))
vrep.simxSetFloatSignal(clientID,"p",p,vrep.simx_opmode_blocking)
vrep.simxSetFloatSignal(clientID,"d",d,vrep.simx_opmode_blocking)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
start = time.time()
max_dis = 0
while True:
res, sp = vrep.simxGetObjectPosition(clientID,drone,target,vrep.simx_opmode_blocking)
max_dis =abs(sp[0]) if abs(sp[0]) > max_dis else max_dis
res, vel, _ = vrep.simxGetObjectVelocity(clientID,drone,vrep.simx_opmode_blocking)
if abs(sp[0])<0.1 and abs(vel[1])<0.1:
f.write("time: "+str(round(time.time() - start,2))+'\n')
f.write("dis: "+str(round(max_dis,2))+"\n\n")
print("time: "+str(round(time.time() - start,2)))
print("dis: "+str(round(max_dis,2)))
break
if max_dis > 2:
f.write("dis: "+str(round(max_dis,2))+"\n\n")
print("dis: "+str(round(max_dis,2))+"\n")
break
if time.time() - start > 7:
f.write("time: inf\n")
f.write("dis: "+str(round(max_dis,2))+"\n\n")
print("dis: "+str(round(max_dis,2)))
print("time: inf\n")
def velocity(cid, handle):
err, lin, ang = vrep.simxGetObjectVelocity(cid, handle, vrep_mode)
return [lin[0], lin[1], lin[2], ang[0], ang[1], ang[2]]
def collectImageData(ca_model, pn_model, clientID, states, input_type, use_ca):
if use_ca:
ca_model.eval()
pn_model.eval()
list_of_images = []
if (input_type == 0 or input_type == 2 or input_type == 3) and use_ca:
vid_states = states[0]
st_states = states[1]
elif input_type == 1:
if use_ca:
vid_states = states[2]
st_states = states[3]
pn_vidstates = states[0]
pn_ststates = states[1]
collector = []
if clientID != -1:
err, tracer_handle = vrep.simxGetObjectHandle(
clientID, 'LineTracer', vrep.simx_opmode_oneshot_wait)
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, tracer_handle, -1, vrep.simx_opmode_streaming)
t_end = time.time() + 5.2
count = 0
action = 0
inference_counter = 0
tim = 0
while (vrep.simxGetConnectionId(clientID) != -1
and time.time() < t_end):
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, tracer_handle, -1, vrep.simx_opmode_oneshot)
ret_code, velo, angle_velo = vrep.simxGetObjectVelocity(
clientID, tracer_handle, vrep.simx_opmode_oneshot)
ret_code, euler_angles = vrep.simxGetObjectOrientation(
clientID, tracer_handle, -1, vrep.simx_opmode_buffer)
collector.append([
pos[0], pos[1], pos[2], velo[0], velo[1], velo[2],
angle_velo[0], angle_velo[1], angle_velo[2],
euler_angles[0], euler_angles[1], euler_angles[2], action
])
if use_ca:
torch_vid = torch.from_numpy(
np.transpose(
np.expand_dims(
(list_of_images[-1]).astype('float'), axis=0),
(0, 3, 1, 2)))
torch_st = torch.from_numpy(
np.asarray(collector[-1]).astype('float'))
vid_to_ca = Variable(torch_vid.float().cuda(),
volatile=True)
st_to_ca = Variable(torch_st.float().cuda(), volatile=True)
output, vid_states, st_states = ca_model(
vid_to_ca, st_to_ca, vid_states, st_states)
output.detach_()
inference_counter += 1
if input_type == 0:
if count == 0:
a = torch.from_numpy(
list_of_images[-1].flatten()).float().cuda()
b = torch.from_numpy(
list_of_images[-1].flatten()).float().cuda()
c = torch.from_numpy(
np.asarray(
collector[-1]).astype('float')).float().cuda()
if use_ca:
d = torch.squeeze(output.data).float().cuda()
input_to_model = torch.cat([a, b, c, d])
else:
input_to_model = torch.cat([a, b, c])
else:
a = torch.from_numpy(
list_of_images[-1].flatten()).float().cuda()
b = torch.from_numpy(
list_of_images[-2].flatten()).float().cuda()
c = torch.from_numpy(
np.asarray(
collector[-1]).astype('float')).float().cuda()
if use_ca:
d = torch.squeeze(output.data).float().cuda()
input_to_model = torch.cat([a, b, c, d])
else:
input_to_model = torch.cat([a, b, c])
input_to_model = Variable(input_to_model, volatile=True)
out = pn_model(input_to_model)
elif input_type == 1:
pn_vid = torch.from_numpy(
np.transpose(
np.expand_dims(
(list_of_images[-1]).astype('float'), axis=0),
(0, 3, 1, 2)))
vid_input = Variable(pn_vid.float().cuda(), volatile=True)
c = torch.from_numpy(
np.asarray(
collector[-1]).astype('float')).float().cuda()
if use_ca:
d = torch.squeeze(output.data).float().cuda()
st_input = Variable(torch.cat([c, d]).unsqueeze(0),
volatile=True)
else:
st_input = Variable(c.unsqueeze(0), volatile=True)
out, pn_vidstates, pn_ststates = pn_model(
vid_input, st_input, pn_vidstates, pn_ststates)
elif input_type == 2 or input_type == 3:
#stack two images together and I need to verify the images being stacked are reasonable
if count == 0:
stacked_img = np.concatenate(
(np.transpose(
np.expand_dims(
(list_of_images[-1]).astype('float'),
axis=0), (0, 3, 1, 2)),
np.transpose(
np.expand_dims(
(list_of_images[-1]).astype('float'),
axis=0), (0, 3, 1, 2))),
axis=1)
vid_input = Variable(
torch.from_numpy(stacked_img).float().cuda(),
volatile=True)
c = torch.from_numpy(
np.asarray(
collector[-1]).astype('float')).float().cuda()
if use_ca:
d = torch.squeeze(output.data).float().cuda()
st_input = Variable(torch.cat([c, d]).unsqueeze(0),
volatile=True)
else:
st_input = Variable(c.unsqueeze(0), volatile=True)
else:
stacked_img = np.concatenate(
(np.transpose(
np.expand_dims(
(list_of_images[-1]).astype('float'),
axis=0), (0, 3, 1, 2)),
np.transpose(
np.expand_dims(
(list_of_images[-2]).astype('float'),
axis=0), (0, 3, 1, 2))),
axis=1)
vid_input = Variable(
torch.from_numpy(stacked_img).float().cuda(),
volatile=True)
c = torch.from_numpy(
np.asarray(
collector[-1]).astype('float')).float().cuda()
if use_ca:
d = torch.squeeze(output.data).float().cuda()
st_input = Variable(torch.cat([c, d]).unsqueeze(0),
volatile=True)
else:
st_input = Variable(c.unsqueeze(0), volatile=True)
out = pn_model(st_input, vid_input)
else:
print("Error 12")
sys.exit()
action = out.max(1)[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 velocity(cid, handle):
err, lin, ang = vrep.simxGetObjectVelocity(cid, handle, vrep_mode)
return [lin[0], lin[1], lin[2], ang[0], ang[1], ang[2]]
simDiscreteTime += dt * simStep
for i in range(0, jointNum - 1):
# 将控制器参数转化为关节角
vJointSet[i] = controlFunction.sin_controller(i, A[i], omega[i], B[i],
phi[i], simDiscreteTime)
vrep.c_SetJointTargetPosition(clientID, vJointHandle[i], vJointSet[i],
vrep.simx_opmode_oneshot)
# 读取各个模块的速度及位置
for i in range(0, moduleNum - 1):
_, jpos = vrep.simxGetObjectPosition(clientID, moduleHandle[i], -1,
vrep.simx_opmode_oneshot)
modulePositionSensor[i, :] = jpos
# print("modulePosition[",i,"] = ", modulePositionSensor[i])
_, vel, ang = vrep.simxGetObjectVelocity(clientID, moduleHandle[0],
vrep.simx_opmode_oneshot)
# print("velocity = ", vel)
data.append((vel[0], vel[1], vel[2], vJointSet[0], vJointSet[1],
vJointSet[2], vJointSet[3], vJointSet[4], vJointSet[5],
vJointSet[6], vJointSet[7], velAverageValue.x[0]))
# 当前速度
vc = vel[0] * vel[0] + vel[1] * vel[1]
vc = math.sqrt(vc)
# 求N次采样的平均速度
# velSample.x = velSample.x*(steps-1)/steps+(1/steps)*vc
# 求近10次采样平均
if steps < 10:
velSample.x[0, steps] = vc
else:
for i in range(1, 9):
time.sleep(1)
cid = connect()
time.sleep(1)
vrep.simxLoadScene(cid,'/home/elias/[email protected]/_Winter2015/CSC494/Scenes/Base_Quad.ttt',0,mode())
vrep.simxStartSimulation(cid,mode())
runtime = 0
err, copter = vrep.simxGetObjectHandle(cid, "Quadricopter_base",
vrep.simx_opmode_oneshot_wait)
err, target = vrep.simxGetObjectHandle(cid, "Quadricopter_target",
vrep.simx_opmode_oneshot_wait)
err, front_camera = vrep.simxGetObjectHandle(cid, 'Quadricopter_frontCamera', vrep.simx_opmode_oneshot)
err, lin, ang = vrep.simxGetObjectVelocity(cid, copter, vrep.simx_opmode_streaming)
err, orgpos = getTargetStart(cid,target)
real_args(args,orgpos )
args["addxToZ"] = [orgpos, 2.5]
args["fuckIt"] = [orgpos, 2.5]
#This prepared the data
print(err)
#while(1):
while(runtime <= 10):
(err, lwmotor) = vrep.simxGetObjectHandle(clientID,"K3_leftWheelMotor#",vrep.simx_opmode_oneshot_wait)
oldJoystick = data.find_one({"item": "joystick"})
while angleModel[1]==0:
(err, orientation) = vrep.simxGetObjectOrientation(clientID,robot,-1,vrep.simx_opmode_oneshot)
angleModel[1]= orientation[1]
angleModel[2] = abs(angleModel[1])
while clientID!=-1:
joystick = data.find_one({"_id": 0})
x = float(joystick['x'])
y = float(joystick['y'])
lockMode = str(joystick['mode'])
(err, position) = vrep.simxGetObjectPosition(clientID,robot,-1,vrep.simx_opmode_oneshot)
(err, orientation) = vrep.simxGetObjectOrientation(clientID,robot,-1,vrep.simx_opmode_oneshot)
(err, speed, angularSpeed) = vrep.simxGetObjectVelocity(clientID,robot,vrep.simx_opmode_oneshot)
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):
res, servo_front = vrep.simxGetObjectHandle(clientID, 'Front_rev',
vrep.simx_opmode_blocking)
res, servo_back = vrep.simxGetObjectHandle(clientID, 'Back_rev',
vrep.simx_opmode_blocking)
res, servo_middle = vrep.simxGetObjectHandle(clientID, 'Middle_rev',
vrep.simx_opmode_blocking)
res, body_back = vrep.simxGetObjectHandle(clientID, 'Back',
vrep.simx_opmode_blocking)
print('dude')
t = time.time()
z = 0
v = 0
while (time.time() - t) <= 100:
res, linearVelocity, angularVelocity = vrep.simxGetObjectVelocity(
clientID, body_back, vrep.simx_opmode_streaming)
print('vy', linearVelocity[1])
z = z + 1
v = v + (linearVelocity[1])
#plt.plot(time.time(),linearVelocity[1],'go--')
#plt.xlabel('time(s)')
#plt.ylabel('velocity(m/s)')
#plt.grid(True)
#plt.hold(True)
tf, tr, tm = phase(time.time())
vrep.simxSetJointTargetPosition(clientID, servo_front, tf,
def QC_controller(Quadricopter_target, Quadricopter_base, Quadricopter):
try:
import vrep
except:
print ('--------------------------------------------------------------')
print ('"vrep.py" could not be imported. This means very probably that')
print ('either "vrep.py" or the remoteApi library could not be found.')
print ('Make sure both are in the same folder as this file,')
print ('or appropriately adjust the file "vrep.py"')
print ('--------------------------------------------------------------')
print ('')
""" =========================================================== """
""" ============ Imported Libraries: ============ """
import time
import numpy as np
from captains_log_v1 import log_data
import os
import cost
import csv
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
""" =================================================================== """
# output limits:
#min_output=0
#max_output=8.335
# program parameters:
global i
i = 0
xe = []
ye = []
ze = []
xs = []
ys = []
zs = []
x_qc = []
y_qc = []
z_qc = []
u = []
v1 = []
v2 = []
v3 = []
v4 = []
#global variables:
cumul=0
last_e=0
pAlphaE=0
pBetaE=0
psp2=0
psp1=0
prevEuler=0
cumulAlpha = 0
cumulBeta = 0
cumulAlphaPos = 0
cumulBetaPos = 0
s_r = 0
particlesTargetVelocities=[0,0,0,0]
#speed weight:
vParam=-2
#parameters for vertical control
Kpv=2
Kiv=0
Kdv=2
#parameters for horizontal control:
Kph=0.4 # TBD
Kih=0.1 # TBD
Kdh=1.5
Kph_pos1=0.4
Kih_pos1=0.001
Kdh_pos1=0.05
Kph_pos0=0.4
Kih_pos0=0.001
Kdh_pos0=0.05
#parameters for rotational control:
Kpr=0.05
Kir=0
Kdr=0.9
""" =========================================================== """
# parameters needed for gradient descent:
t0 = 0
tf = 1
dt = 0.01
sum_h_alpha = []
sum_h_beta = []
sum_h_pos0 = []
sum_h_pos1 = []
last_alpha_angle = 0
last_alpha_pos = 0
last_beta_angle = 0
last_beta_pos = 0
delta_alpha_angle = []
delta_alpha_pos = []
delta_beta_angle = []
delta_beta_pos = []
J_h_alpha = []
J_h_beta = []
J_h_pos0 = []
J_h_pos1 = []
paramX = []
paramY = []
theta_h_angles = [Kph, Kih, Kdh]
theta_h_pos = [Kph_pos0, Kih_pos0, Kdh_pos0] # Kph_pos0 == Kph_pos1 ...
gd = 0
""" =========================================================== """
print ('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID=vrep.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to V-REP
if clientID!=-1:
print ('Connected to remote API server')
# enable the synchronous mode on the client:
vrep.simxSynchronous(clientID,True)
# start the simulation:
vrep.simxStartSimulation(clientID,vrep.simx_opmode_blocking)
#functional/handle code:
emptyBuff=bytearray()
[returnCode,targetObj]=vrep.simxGetObjectHandle(clientID,Quadricopter_target,vrep.simx_opmode_blocking)
[returnCode,qc_base_handle]=vrep.simxGetObjectHandle(clientID,Quadricopter_base,vrep.simx_opmode_blocking)
[returnCode,qc_handle]=vrep.simxGetObjectHandle(clientID,Quadricopter,vrep.simx_opmode_blocking)
# main loop:
while True:
""" ========== vertical control: ========== """
[returnCode,targetPos]=vrep.simxGetObjectPosition(clientID,targetObj,-1,vrep.simx_opmode_blocking)
[returnCode,pos]=vrep.simxGetObjectPosition(clientID,qc_base_handle,-1,vrep.simx_opmode_blocking)
[returnCode, l, w] = vrep.simxGetObjectVelocity(clientID, qc_base_handle, vrep.simx_opmode_blocking)
e=targetPos[2]-pos[2]
cumul=cumul+e
diff_e=e-last_e
Pvert=Kpv*e
Ivert=Kiv*cumul
Dvert=Kdv*diff_e
thrust=5.335+Pvert+Ivert+Dvert+l[2]*vParam# get thrust
last_e=e
""" =========================================================== """
""" ========== horizontal control: ========== """
[returnCode,sp]=vrep.simxGetObjectPosition(clientID,targetObj,qc_base_handle,vrep.simx_opmode_blocking)
[rc,rc,vx,rc,rc]=vrep.simxCallScriptFunction(clientID,Quadricopter,vrep.sim_scripttype_childscript,'qc_Get_vx',[],[],[],emptyBuff,vrep.simx_opmode_blocking)
[rc,rc,vy,rc,rc]=vrep.simxCallScriptFunction(clientID,Quadricopter,vrep.sim_scripttype_childscript,'qc_Get_vy',[],[],[],emptyBuff,vrep.simx_opmode_blocking)
[rc,rc,rc,rc,rc]=vrep.simxCallScriptFunction(clientID,Quadricopter,vrep.sim_scripttype_childscript,'qc_Get_Object_Matrix',[],[],[],emptyBuff,vrep.simx_opmode_blocking)
[errorCode,M]=vrep.simxGetStringSignal(clientID,'mtable',vrep.simx_opmode_oneshot_wait);
if (errorCode==vrep.simx_return_ok):
m=vrep.simxUnpackFloats(M)
alphaE=vy[2]-m[11]
cumulAlpha = cumulAlpha + alphaE
diff_alphaE=alphaE-pAlphaE
alphaCorr=Kph*alphaE + Kih*cumulAlpha + Kdh*diff_alphaE #alpha correction
betaE=vx[2]-m[11]
cumulBeta = cumulBeta + betaE
diff_betaE=betaE-pBetaE
betaCorr=-Kph*betaE - Kih*cumulBeta - Kdh*diff_betaE #beta correction
pAlphaE=alphaE
pBetaE=betaE
cumulAlphaPos = cumulAlphaPos + sp[1]
cumulBetaPos = cumulBetaPos + sp[0]
alphaPos=Kph_pos1*(sp[1])+ Kih_pos1*cumulAlphaPos +Kdh_pos1*(sp[1]-psp2) #alpha position correction
betaPos=Kph_pos0*(sp[0])+ Kih_pos0*cumulBetaPos + Kdh_pos0*(sp[0]-psp1) #beta position correction
alphaCorr=alphaCorr+alphaPos
betaCorr=betaCorr-betaPos
psp2=sp[1]
psp1=sp[0]
delta_alpha_angle.append(alphaCorr - last_alpha_angle)
delta_beta_angle.append(betaCorr - last_beta_angle)
delta_alpha_pos.append(alphaPos - last_alpha_pos)
delta_beta_pos.append(betaPos - last_beta_pos)
last_alpha_angle = alphaCorr
last_beta_angle = betaCorr
last_alpha_pos = alphaPos
last_beta_pos = betaPos
""" =========================================================== """
""" ========== rotational control: ========== """
[returnCode,euler]=vrep.simxGetObjectOrientation(clientID,targetObj,qc_base_handle,vrep.simx_opmode_blocking)
[returnCode,orientation]=vrep.simxGetObjectOrientation(clientID,qc_base_handle,-1,vrep.simx_opmode_blocking)
Prot=Kpr*euler[2]
Drot=Kdr*(euler[2]-prevEuler)
s_r = s_r + euler[2]
rotCorr=Prot+Drot
prevEuler=euler[2]
""" =========================================================== """
""" ========== set propeller velocities: ========== """
propeller1_PTV = thrust*(1-alphaCorr+betaCorr-rotCorr)
propeller2_PTV = thrust*(1-alphaCorr-betaCorr+rotCorr)
propeller3_PTV = thrust*(1+alphaCorr-betaCorr-rotCorr)
propeller4_PTV = thrust*(1+alphaCorr+betaCorr+rotCorr)
particlesTargetVelocities=[propeller1_PTV, propeller2_PTV, propeller3_PTV, propeller4_PTV]
""" =========================================================== """
""" ========== set parameters for logging data: ========== """
C_parameters_vert = [0,0,0]
C_parameters_horr = [0,0,0,0,0,0,0]
C_parameters_rot = [0,0,0]
vert_comp = [0,0,0,0]
horr_comp = [0,0,0,0,[0,0,0],0,0,0,0,0,0]
rot_comp = [[0,0,0],0]
C_parameters_vert[0] = Kpv
C_parameters_vert[1] = Kiv
C_parameters_vert[2] = Kdv
C_parameters_horr[0] = Kph
C_parameters_horr[1] = Kih
C_parameters_horr[2] = Kdh
C_parameters_horr[3] = Kph_pos0
C_parameters_horr[4] = Kdh_pos0
C_parameters_horr[5] = Kph_pos1
C_parameters_horr[6] = Kdh_pos1
C_parameters_rot[0] = Kpr
C_parameters_rot[1] = Kir
C_parameters_rot[2] = Kdr
vert_comp[0] = targetPos
vert_comp[1] = pos
vert_comp[2] = e
vert_comp[3] = thrust
horr_comp[0] = alphaE
horr_comp[1] = betaE
horr_comp[2] = cumulAlpha
horr_comp[3] = cumulBeta
horr_comp[4] = sp
horr_comp[5] = cumulAlphaPos
horr_comp[6] = cumulBetaPos
horr_comp[7] = alphaCorr
horr_comp[8] = betaCorr
horr_comp[9] = vx
horr_comp[10] = vy
rot_comp[0] = euler
rot_comp[1] = rotCorr
""" =========================================================== """
""" ========== PLOTTING: ========== """
## PLOTTING:
ze.append(e) # otstapuvanje od z-oska
xe.append(sp[0]) # otstapuvanje od x-oska
ye.append(sp[1]) # otstapuvawe od y-oska
# xs.append(targetPos[0])
# ys.append(targetPos[1])
# zs.append(targetPos[2])
# x_qc.append(pos[0])
# y_qc.append(pos[1])
# z_qc.append(pos[2])
# fig = plt.figure()
# ax = fig.add_subplot(111, projection='3d')
# ax.plot_wireframe(xs,ys,zs,color='blue')
# ax.plot_wireframe(x_qc,y_qc,z_qc,color='red')
# ax.set_xlim3d(-2.1,2.1)
# ax.set_ylim3d(-2.1,2.1)
# ax.set_zlim3d(0,1.9)
# plt.show()
# u.append(thrust)
# v1.append(propeller1_PTV)
# v2.append(propeller2_PTV)
# v3.append(propeller3_PTV)
# v4.append(propeller4_PTV)
#
plt.plot(xe,color='blue')
plt.hold(True)
plt.plot(ye,color='red')
plt.hold(True)
plt.plot(ze,color='green')
plt.hold(True)
# plt.plot(v4,color='pink')
# plt.hold(True)
# plt.axis([0,25,0,15])
# plt.show()
#
# plt.plot(xe,color='blue')
# plt.axis([0,100,-4,4])
# plt.show()
# plt.plot(ye,color='red')
# plt.axis([0,100,-4,4])
# plt.show()
# plt.plot(ze,color='green')
plt.axis([0,25,-2,2])
plt.show()
""" =========================================================== """
""" ========== Gradient descent: ========== """
sum_h_alpha.append(cumulAlpha)
sum_h_beta.append(cumulBeta)
sum_h_pos1.append(cumulAlphaPos)
sum_h_pos0.append(cumulBetaPos)
J_h_alpha.append((1/(tf - t0))*(5*cumulAlpha**2 + delta_alpha_angle[gd]**2)*dt)
J_h_beta.append((1/(tf - t0))*(5*cumulBeta**2 + delta_beta_angle[gd]**2)*dt)
J_h_pos1.append((1/(tf - t0))*(5*cumulAlphaPos**2 + delta_alpha_pos[gd]**2)*dt)
J_h_pos0.append((1/(tf - t0))*(5*cumulBetaPos**2 + delta_beta_pos[gd]**2)*dt)
paramX.append(targetPos[0])
paramY.append(targetPos[1])
print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
print "theta_h_angles : ", theta_h_angles
print "theta_h_pos : ", theta_h_pos
print "sum_h_alpha : ", sum_h_alpha[gd]
print "sum_h_beta : ", sum_h_beta[gd]
print "sum_h_pos1 : ", sum_h_pos1[gd]
print "sum_h_pos0 : ", sum_h_pos0[gd]
print "J_h_alpha : ", J_h_alpha[gd]
print "J_h_beta : ", J_h_beta[gd]
print "J_h_pos1 : ", J_h_pos1[gd]
print "J_h_pos0 : ", J_h_pos0[gd]
print "paramX : ", paramX[gd]
print "paramY : ", paramY[gd]
print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
gd = gd + 1
print "gd (iterations) : ", gd
print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
""" =========================================================== """
if gd == 400:
with open('GD_43.csv','wb') as f1:
writer=csv.writer(f1)
for i in range(0, gd):
writer.writerow([paramX[i],paramY[i],theta_h_angles[0],theta_h_angles[1],theta_h_angles[2],theta_h_pos[0],theta_h_pos[1],theta_h_pos[2],J_h_alpha[i],J_h_beta[i],J_h_pos1[i],J_h_pos0[i]])
break
""" =========================================================== """
""" ========== WRITE TO TEXT: ========== """
## WRITE TO TEXT:
log_data(C_parameters_vert, C_parameters_horr, C_parameters_rot, vert_comp, horr_comp, rot_comp, particlesTargetVelocities, i)
i +=1
""" =========================================================== """
# send propeller velocities to output:
[res,retInts,retFloats,retStrings,retBuffer]=vrep.simxCallScriptFunction(clientID,Quadricopter,vrep.sim_scripttype_childscript,'qc_propeller_v',[],particlesTargetVelocities,[],emptyBuff,vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(clientID)
""" =========================================================== """
# stop the simulation:
vrep.simxStopSimulation(clientID,vrep.simx_opmode_blocking)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print ('Failed connecting to remote API server')
'/home/elias/[email protected]/_Winter2015/CSC494/Scenes/Base_Quad.ttt', 0, mode()) vrep.simxStartSimulation(cid, mode()) runtime = 0 err, copter = vrep.simxGetObjectHandle(cid, "Quadricopter_base", vrep.simx_opmode_oneshot_wait) err, target = vrep.simxGetObjectHandle(cid, "Quadricopter_target", vrep.simx_opmode_oneshot_wait) err, front_camera = vrep.simxGetObjectHandle(cid, 'Quadricopter_frontCamera', vrep.simx_opmode_oneshot) err, lin, ang = vrep.simxGetObjectVelocity(cid, copter, vrep.simx_opmode_streaming) err, orgpos = getTargetStart(cid, target) real_args(args, orgpos) args["addxToZ"] = [orgpos, 2.5] args["fuckIt"] = [orgpos, 2.5] #This prepared the data print(err) #while(1): while (runtime <= 10): error, pos = vrep.simxGetObjectPosition(cid, copter, -1, mode()) args["AsinToZ"] = [pos, getTime]
def QC_controller(Quadricopter_target, Quadricopter_base, Quadricopter,
Quadricopter_target2, Quadricopter_base2, Quadricopter2):
try:
import vrep
except:
print('--------------------------------------------------------------')
print('"vrep.py" could not be imported. This means very probably that')
print('either "vrep.py" or the remoteApi library could not be found.')
print('Make sure both are in the same folder as this file,')
print('or appropriately adjust the file "vrep.py"')
print('--------------------------------------------------------------')
print('')
""" =========================================================== """
""" ============ Imported Libraries: ============ """
import time
import numpy as np
from captains_log_v1 import log_data
import os
import cost
import csv
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import main_PID_controller
""" =================================================================== """
# output limits:
#min_output=0
#max_output=8.335
# program parameters:
global i
i = 0
xf = []
xl = []
yl = []
yf = []
zl = []
zf = []
vxl = []
vxf = []
vyl = []
vyf = []
euler_l = []
euler_f = []
# xe = []
# ye = []
ze = []
# xs = []
# ys = []
# zs = []
x_qc = []
y_qc = []
z_qc = []
x_qc2 = []
y_qc2 = []
z_qc2 = []
# u = []
v1 = []
v2 = []
v3 = []
v4 = []
v12 = []
v22 = []
v32 = []
v42 = []
#global variables:
cumul = 0
last_e = 0
pAlphaE = 0
pBetaE = 0
psp2 = 0
psp1 = 0
prevEuler = 0
cumulAlpha = 0
cumulBeta = 0
cumulAlphaPos = 0
cumulBetaPos = 0
cumul2 = 0
last_e2 = 0
pAlphaE2 = 0
pBetaE2 = 0
psp22 = 0
psp12 = 0
prevEuler2 = 0
cumulAlpha2 = 0
cumulBeta2 = 0
cumulAlphaPos2 = 0
cumulBetaPos2 = 0
cumulPOS = 0
cumulVY = 0
cumulVX = 0
cumulSP0 = 0
cumulSP1 = 0
cumulEULER = 0
cumulPOS2 = 0
cumulVY2 = 0
cumulVX2 = 0
cumulSP02 = 0
cumulSP12 = 0
cumulEULER2 = 0
particlesTargetVelocities = [0, 0, 0, 0]
particlesTargetVelocities2 = [0, 0, 0, 0]
#speed weight:
vParam = -2
#parameters for vertical control
Kpv = 2
Kiv = 0
Kdv = 2
#parameters for horizontal control:
Kph = 0.4 # TBD
Kih = 0.1 # TBD
Kdh = 1.5
Kph_pos1 = 0.4
Kih_pos1 = 0.002
Kdh_pos1 = 0.05
Kph_pos0 = 0.4
Kih_pos0 = 0.002
Kdh_pos0 = 0.05
#parameters for rotational control:
Kpr = 0.05
# Kir=0
Kdr = 0.9
""" =========================================================== """
gd = 0
""" =========================================================== """
print('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000,
5) # Connect to V-REP
if clientID != -1:
print('Connected to remote API server')
# enable the synchronous mode on the client:
vrep.simxSynchronous(clientID, True)
# start the simulation:
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
#functional/handle code:
emptyBuff = bytearray()
[returnCode,
targetObj] = vrep.simxGetObjectHandle(clientID, Quadricopter_target,
vrep.simx_opmode_blocking)
[returnCode,
qc_base_handle] = vrep.simxGetObjectHandle(clientID,
Quadricopter_base,
vrep.simx_opmode_blocking)
[returnCode,
qc_handle] = vrep.simxGetObjectHandle(clientID, Quadricopter,
vrep.simx_opmode_blocking)
[returnCode,
targetObj2] = vrep.simxGetObjectHandle(clientID, Quadricopter_target2,
vrep.simx_opmode_blocking)
[returnCode,
qc_base_handle2] = vrep.simxGetObjectHandle(clientID,
Quadricopter_base2,
vrep.simx_opmode_blocking)
[returnCode,
qc_handle2] = vrep.simxGetObjectHandle(clientID, Quadricopter2,
vrep.simx_opmode_blocking)
# main loop:
while True:
""" ========== vertical control: ========== """
[returnCode,
targetPos] = vrep.simxGetObjectPosition(clientID, targetObj, -1,
vrep.simx_opmode_blocking)
[returnCode,
pos] = vrep.simxGetObjectPosition(clientID, qc_base_handle, -1,
vrep.simx_opmode_blocking)
[returnCode, l,
w] = vrep.simxGetObjectVelocity(clientID, qc_base_handle,
vrep.simx_opmode_blocking)
[returnCode, targetPos2
] = vrep.simxGetObjectPosition(clientID, targetObj2, -1,
vrep.simx_opmode_blocking)
[returnCode,
pos2] = vrep.simxGetObjectPosition(clientID, qc_base_handle2, -1,
vrep.simx_opmode_blocking)
[returnCode, l2,
w2] = vrep.simxGetObjectVelocity(clientID, qc_base_handle2,
vrep.simx_opmode_blocking)
""" =========================================================== """
""" ========== horizontal control: ========== """
[returnCode,
sp] = vrep.simxGetObjectPosition(clientID, targetObj,
qc_base_handle,
vrep.simx_opmode_blocking)
[rc, rc, vx, rc, rc] = vrep.simxCallScriptFunction(
clientID, Quadricopter, vrep.sim_scripttype_childscript,
'qc_Get_vx', [], [], [], emptyBuff, vrep.simx_opmode_blocking)
[rc, rc, vy, rc, rc] = vrep.simxCallScriptFunction(
clientID, Quadricopter, vrep.sim_scripttype_childscript,
'qc_Get_vy', [], [], [], emptyBuff, vrep.simx_opmode_blocking)
[rc, rc, rc, rc,
rc] = vrep.simxCallScriptFunction(clientID, Quadricopter,
vrep.sim_scripttype_childscript,
'qc_Get_Object_Matrix', [], [],
[], emptyBuff,
vrep.simx_opmode_blocking)
[errorCode,
M] = vrep.simxGetStringSignal(clientID, 'mtable',
vrep.simx_opmode_oneshot_wait)
if (errorCode == vrep.simx_return_ok):
m = vrep.simxUnpackFloats(M)
m1 = m[0]
m2 = m[1]
vx1 = [vx[0], vx[1], vx[2]]
vx2 = [vx[3], vx[4], vx[5]]
vy1 = [vy[0], vy[1], vy[2]]
vy2 = [vy[3], vy[4], vy[5]]
[returnCode,
sp2] = vrep.simxGetObjectPosition(clientID, targetObj2,
qc_base_handle2,
vrep.simx_opmode_blocking)
""" =========================================================== """
""" ========== rotational control: ========== """
[returnCode,
euler] = vrep.simxGetObjectOrientation(clientID, targetObj,
qc_base_handle,
vrep.simx_opmode_blocking)
[returnCode, orientation
] = vrep.simxGetObjectOrientation(clientID, qc_base_handle, -1,
vrep.simx_opmode_blocking)
[returnCode,
euler2] = vrep.simxGetObjectOrientation(clientID, targetObj2,
qc_base_handle2,
vrep.simx_opmode_blocking)
[returnCode, orientation2
] = vrep.simxGetObjectOrientation(clientID, qc_base_handle2, -1,
vrep.simx_opmode_blocking)
""" =========================================================== """
""" ========== set propeller velocities: ========== """
particlesTargetVelocities, cumul, last_e, cumulAlpha, pAlphaE, cumulBeta, pBetaE, cumulAlphaPos, cumulBetaPos, psp2, psp1, prevEuler, cumulPOS, cumulVY, cumulVX, cumulSP0, cumulSP1, cumulEULER = main_PID_controller.PID(
targetPos, pos, cumul, last_e, Kpv, Kiv, Kdv, vParam, l, vy1,
m1, cumulAlpha, pAlphaE, Kph, Kih, Kdh, vx1, cumulBeta, pBetaE,
cumulAlphaPos, cumulBetaPos, sp, Kph_pos1, Kih_pos1, Kdh_pos1,
Kph_pos0, Kih_pos0, Kdh_pos0, psp2, psp1, Kpr, Kdr, euler,
prevEuler, cumulPOS, cumulVY, cumulVX, cumulSP0, cumulSP1,
cumulEULER, vx, vy, pos, sp, euler)
particlesTargetVelocities2, cumul2, last_e2, cumulAlpha2, pAlphaE2, cumulBeta2, pBetaE2, cumulAlphaPos2, cumulBetaPos2, psp22, psp12, prevEuler2, cumulPOS2, cumulVY2, cumulVX2, cumulSP02, cumulSP12, cumulEULER2 = main_PID_controller.PID(
targetPos2, pos2, cumul2, last_e2, Kpv, Kiv, Kdv, vParam, l2,
vy2, m2, cumulAlpha2, pAlphaE2, Kph, Kih, Kdh, vx2, cumulBeta2,
pBetaE2, cumulAlphaPos2, cumulBetaPos2, sp2, Kph_pos1,
Kih_pos1, Kdh_pos1, Kph_pos0, Kih_pos0, Kdh_pos0, psp22, psp12,
Kpr, Kdr, euler2, prevEuler2, cumulPOS2, cumulVY2, cumulVX2,
cumulSP02, cumulSP12, cumulEULER2, vx, vy, pos, sp, euler)
particlesTargetVelocities_s = [
particlesTargetVelocities[0], particlesTargetVelocities[1],
particlesTargetVelocities[2], particlesTargetVelocities[3],
particlesTargetVelocities2[0], particlesTargetVelocities2[1],
particlesTargetVelocities2[2], particlesTargetVelocities2[3]
]
""" =========================================================== """
""" =========================================================== """
""" ========== PLOTTING: ========== """
## PLOTTING:
# ze.append(targetPos[2]-pos[2]) # otstapuvanje od z-oska
# xe.append(sp[0]) # otstapuvanje od x-oska
# ye.append(sp[1]) # otstapuvawe od y-oska
# xs.append(targetPos[0])
# ys.append(targetPos[1])
# zs.append(targetPos[2])
# x_qc.append(pos[0])
# y_qc.append(pos[1])
# z_qc.append(pos[2])
# x_qc2.append(pos2[0])
# y_qc2.append(pos2[1])
# z_qc2.append(pos2[2])
# fig = plt.figure()
# ax = fig.add_subplot(111, projection='3d')
# ax.plot_wireframe(x_qc,y_qc,z_qc,color='blue')
# ax.plot_wireframe(x_qc2,y_qc2,z_qc2,color='red')
# ax.set_xlim3d(-2.1,2.1)
# ax.set_ylim3d(-2.1,2.1)
# ax.set_zlim3d(0,1.9)
# plt.show()
# u.append(thrust)
''' !!! treba da se nacrta i pozicijata i da se sporedi so i bez consensus !!! '''
xl.append(pos[0])
xf.append(pos2[0])
yl.append(pos[1])
yf.append(pos2[1])
zl.append(pos[2])
zf.append(pos2[2])
vxl.append(vx1[2])
vxf.append(vx2[2])
vyl.append(vy1[2])
vyf.append(vy2[2])
euler_l.append(euler[2])
euler_f.append(euler2[2])
v1.append(particlesTargetVelocities[0])
v2.append(particlesTargetVelocities[1])
v3.append(particlesTargetVelocities[2])
v4.append(particlesTargetVelocities[3])
v12.append(particlesTargetVelocities2[0])
v22.append(particlesTargetVelocities2[1])
v32.append(particlesTargetVelocities2[2])
v42.append(particlesTargetVelocities2[3])
plt.plot(yl, color='blue')
plt.hold(True)
plt.plot(yf, color='red')
# plt.plot(euler_l,color='blue')
# plt.plot(euler_f,color='red')
# plt.plot(yl,color='green')
# plt.hold(True)
# plt.plot(yf,color='pink')
# plt.hold(True)
# plt.plot(v3,color='green')
# plt.hold(True)
# plt.plot(v4,color='pink')
plt.hold(True)
plt.axis([0, 149, -3, 3])
plt.show()
# print " Zaxis error between drones: ", pos[2]-pos2[2]
ze.append(pos[2] - pos2[2])
# ze.append(abs(euler[2])-abs(euler2[2]))
plt.plot(ze, color='green')
plt.axis([0, 149, -3, 3])
plt.show()
""" =========================================================== """
""" ========== Gradient descent: ========== """
# sum_h_alpha.append(cumulAlpha)
# sum_h_beta.append(cumulBeta)
# sum_h_pos1.append(cumulAlphaPos)
# sum_h_pos0.append(cumulBetaPos)
# J_h_alpha.append((1/(tf - t0))*(5*cumulAlpha**2 + delta_alpha_angle[gd]**2)*dt)
# J_h_beta.append((1/(tf - t0))*(5*cumulBeta**2 + delta_beta_angle[gd]**2)*dt)
# J_h_pos1.append((1/(tf - t0))*(5*cumulAlphaPos**2 + delta_alpha_pos[gd]**2)*dt)
# J_h_pos0.append((1/(tf - t0))*(5*cumulBetaPos**2 + delta_beta_pos[gd]**2)*dt)
# paramX.append(targetPos[0])
# paramY.append(targetPos[1])
# print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
# print "theta_h_angles : ", theta_h_angles
# print "theta_h_pos : ", theta_h_pos
#
# print "sum_h_alpha : ", sum_h_alpha[gd]
# print "sum_h_beta : ", sum_h_beta[gd]
# print "sum_h_pos1 : ", sum_h_pos1[gd]
# print "sum_h_pos0 : ", sum_h_pos0[gd]
# print "J_h_alpha : ", J_h_alpha[gd]
# print "J_h_beta : ", J_h_beta[gd]
# print "J_h_pos1 : ", J_h_pos1[gd]
# print "J_h_pos0 : ", J_h_pos0[gd]
# print "paramX : ", paramX[gd]
# print "paramY : ", paramY[gd]
# print "cumulPOS : ", cumulPOS2
# print "cumulVY : ", cumulVY2
# print "cumulVX : ", cumulVX2
# print "cumulSP0 : ", cumulSP02
# print "cumulSP1 : ", cumulSP12
# print "cumulEULER : ", cumulEULER2
# print "cumul : ", cumul
print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
gd = gd + 1
print "gd (iterations) : ", gd
print "=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*=*="
""" =========================================================== """
if gd == 150:
# with open('GD_43.csv','wb') as f1:
# writer=csv.writer(f1)
# for i in range(0, gd):
# writer.writerow([paramX[i],paramY[i],theta_h_angles[0],theta_h_angles[1],theta_h_angles[2],theta_h_pos[0],theta_h_pos[1],theta_h_pos[2],J_h_alpha[i],J_h_beta[i],J_h_pos1[i],J_h_pos0[i]])
break
# """ =========================================================== """
""" ========== WRITE TO TEXT: ========== """
## WRITE TO TEXT:
# log_data(C_parameters_vert, C_parameters_horr, C_parameters_rot, vert_comp, horr_comp, rot_comp, particlesTargetVelocities, i)
# i +=1
""" =========================================================== """
# send propeller velocities to output:
[res, retInts, retFloats, retStrings,
retBuffer] = vrep.simxCallScriptFunction(
clientID, Quadricopter, vrep.sim_scripttype_childscript,
'qc_propeller_v', [], particlesTargetVelocities_s, [],
emptyBuff, vrep.simx_opmode_blocking)
# [res,retInts,retFloats,retStrings,retBuffer]=vrep.simxCallScriptFunction(clientID,Quadricopter2,vrep.sim_scripttype_childscript,'qc_propeller_v',[],particlesTargetVelocities2,[],emptyBuff,vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(clientID)
""" =========================================================== """
print " Zaxis error between drones: ", ze
# stop the simulation:
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
print('Failed connecting to remote API server')
def start(clientID,
quads,
targets,
speed,
proxs,
path,
endpos,
leadfoll=False):
"""
Boids model main program
:param clientID: ID of the VRep connection
:param quads: quadrotor handles
:param targets: quadrotor target handles
:param speed: speed of quadrotors
:param proxs: proximity sensor handles
:param path: quadrotor path coordinates
:param endpos: ending position of quadrotors
:param leadfoll: True - leader/followers mode, False - all boids following path (default)
"""
# definition of constants
quadsNum = len(quads) # number of quadrotors
viewRange = 3 # view range of quadrotors
smp = 0.2 # sampling period
kS = [0.30,
2.0] # separation constants [multiplication const, power const]
kC = [0.30, 0.0] # cohesion constants [multiplication const, power const]
kA = [0.00, 0.0] # alignment constants [multiplication const, power const]
kD = [speed,
1.0] # path following constants [multiplication const, power const]
kO = [0.20, 2.0
] # obstacle avoidance constants [multiplication const, power const]
# data stream init
for i in range(quadsNum):
vrep.simxGetObjectPosition(clientID, quads[i], -1,
vrep.simx_opmode_streaming)
vrep.simxGetObjectVelocity(clientID, quads[i],
vrep.simx_opmode_streaming)
vrep.simxReadProximitySensor(clientID, proxs[i],
vrep.simx_opmode_streaming)
# variables init
position = [[0 for _ in range(3)]
for _ in range(quadsNum)] # position of quadrotors
velocity = [[0 for _ in range(3)]
for _ in range(quadsNum)] # velocity of quadrotors
closest = [[0 for _ in range(3)]
for _ in range(quadsNum)] # coords of closest obstacle to quads
visibleQuads = [[0 for _ in range(quadsNum)]
for _ in range(quadsNum)] # visible quadrotors
individualTarget = [
0
] * quadsNum # current waypoint index for each quadrotor
# get closest boid to starting point
leader = 0
_, tmp = vrep.simxGetObjectPosition(clientID, quads[0], -1,
vrep.simx_opmode_buffer)
dist = ut.norm(ut.sub(path[1], tmp))
for i in range(1, quadsNum):
_, tmp = vrep.simxGetObjectPosition(clientID, quads[i], -1,
vrep.simx_opmode_buffer)
nrm = ut.norm(ut.sub(path[1], tmp))
if nrm < dist:
dist = nrm
leader = i
# main boids program
t1 = 0
t2 = 0.0
finished = [False] * quadsNum
count = 0
counting = False
file = open('data.txt', 'wt', encoding='utf-8')
while vrep.simxGetConnectionId(clientID) != -1:
time.sleep(smp)
separation = [[0 for _ in range(3)]
for _ in range(quadsNum)] # separation force
cohesion = [[0 for _ in range(3)]
for _ in range(quadsNum)] # cohesion force
alignment = [[0 for _ in range(3)]
for _ in range(quadsNum)] # alignment force
destination = [[0 for _ in range(3)]
for _ in range(quadsNum)] # path following force
avoidance = [[0 for _ in range(3)]
for _ in range(quadsNum)] # obstacle avoidance force
output = [[0 for _ in range(3)]
for _ in range(quadsNum)] # output force
# check if all quads finished
if counting:
if count >= 0:
file.close()
return (t2 - t1) / 1000
count += 1
else:
for i in range(quadsNum):
nrm = ut.norm(ut.sub(position[i][0:2], path[-1][0:2]))
if nrm < 2 and not finished[i]:
finished[i] = True
print('Quad #' + str(i) + ' finished in ' +
str((vrep.simxGetLastCmdTime(clientID) - t1) /
1000) + 's')
if (leadfoll and finished[leader]) or (
not leadfoll and all(_ for _ in finished)):
counting = True
t2 = vrep.simxGetLastCmdTime(clientID)
if endpos is None:
file.close()
return (t2 - t1) / 1000
# read data from VRep
for i in range(quadsNum):
_, position[i] = vrep.simxGetObjectPosition(
clientID, quads[i], -1, vrep.simx_opmode_buffer)
_, velocity[i], _ = vrep.simxGetObjectVelocity(
clientID, quads[i], vrep.simx_opmode_buffer)
_, res, closest[i], _, _ = vrep.simxReadProximitySensor(
clientID, proxs[i], vrep.simx_opmode_buffer)
if not res:
closest[i] = [0, 0, 0]
closest[i][2] = 0
# write into data file
ct = vrep.simxGetLastCmdTime(clientID)
file.write(str(ct))
for i in range(quadsNum):
file.write(' ' + str(position[i][0]) + ' ' + str(position[i][1]) +
' ' + str(position[i][2]))
file.write(' ' + str(velocity[i][0]) + ' ' + str(velocity[i][1]) +
' ' + str(velocity[i][2]))
file.write(' ' + str(closest[i][0]) + ' ' + str(closest[i][1]))
file.write('\n')
# compute visible quadrotors
for i in range(quadsNum):
for j in range(quadsNum):
if i != j:
temp = ut.sub(position[i], position[j])
if ut.norm(temp) < viewRange:
visibleQuads[i][j] = 1
else:
visibleQuads[i][j] = 0
for i in range(quadsNum):
# compute separation force
for j in range(quadsNum):
if i != j and visibleQuads[i][j] == 1:
temp = ut.sub(position[i], position[j])
nrm = ut.norm(temp)
if nrm != 0:
temp = ut.mul(temp, kS[0] / (nrm**kS[1]))
separation[i] = ut.add(separation[i], temp)
# compute cohesion and alignment forces
center = [0, 0, 0] # center of the swarm
if sum(visibleQuads[i]) != 0:
for j in range(quadsNum):
if i != j and visibleQuads[i][j] == 1:
temp = ut.mul(position[j], 1 / sum(visibleQuads[i]))
center = ut.add(center, temp)
temp = ut.mul(velocity[j], 1 / sum(visibleQuads[i]))
alignment[i] = ut.add(alignment[i], temp)
cohesion[i] = ut.sub(center, position[i])
nrm = ut.norm(cohesion[i])
if nrm != 0:
cohesion[i] = ut.mul(cohesion[i], kC[0] / (nrm**kC[1]))
nrm = ut.norm(alignment[i])
if nrm != 0:
alignment[i] = ut.mul(alignment[i], kA[0] / (nrm**kA[1]))
# compute path following force
if not leadfoll or i == leader or counting:
if not counting:
nrm = ut.norm(
ut.sub(position[i][0:2],
path[individualTarget[i]][0:2]))
if individualTarget[i] != 0:
vec1 = ut.sub(path[individualTarget[i] - 1],
path[individualTarget[i]])
vec2 = ut.sub(position[i], path[individualTarget[i]])
if individualTarget[i] < len(path) - 1 and (
nrm <= 1
or ut.angle(vec1, vec2) >= math.pi / 2):
individualTarget[i] += 1
# if individualTarget[i] == 2 and min(individualTarget) == 2:
# print((vrep.simxGetLastCmdTime(clientID)-tt)/1000)
else:
vec1 = ut.sub(path[individualTarget[i] + 1],
path[individualTarget[i]])
vec2 = ut.sub(position[i], path[individualTarget[i]])
if nrm <= 1 or ut.angle(vec1, vec2) <= math.pi / 2:
individualTarget[i] += 1
if t1 == 0 and min(individualTarget) == 1:
t1 = vrep.simxGetLastCmdTime(clientID)
# tt = vrep.simxGetLastCmdTime(clientID)
destination[i] = ut.sub(path[individualTarget[i]],
position[i])
else:
destination[i] = ut.sub(endpos[i], position[i])
nrm = ut.norm(destination[i])
if nrm != 0:
if finished[i]:
destination[i] = ut.mul(destination[i], 0.1)
else:
destination[i] = ut.mul(destination[i],
kD[0] / (nrm**kD[1]))
# compute output force without obstacle avoidance
output[i] = separation[i]
output[i] = ut.add(output[i], cohesion[i])
output[i] = ut.add(output[i], alignment[i])
output[i] = ut.add(output[i], destination[i])
# compute obstacle avoidance force
# angle = ut.angle(closest[i], output[i])
# if angle > math.pi/2+0.3:
# avoidance[i] = [0, 0, 0]
# else:
avoidance[i] = ut.sub([0, 0, 0], closest[i])
nrm = ut.norm(avoidance[i])
if nrm != 0:
avoidance[i] = ut.mul(avoidance[i], kO[0] / (nrm**kO[1]))
# compute output force
output[i] = ut.add(output[i], avoidance[i])
if position[i][2] < 0.5:
output[i][2] = 0.05
# export output to VRep
for i in range(quadsNum):
vrep.simxSetObjectPosition(clientID, targets[i], quads[i],
output[i], vrep.simx_opmode_streaming)
