def getBall():
dt = 0.001
camera.setCameraPosition(0, 0)
while True:
ts = vrep.simxGetLastCmdTime(clientID)
camera.getCameraImage()
sign, x, y, area = camera.trackObject('blue')
cameraAreaControl.control(area)
cameraPosControl.control(y)
if (sign):
U1 = cameraPosControl.U
U2 = cameraAreaControl.U
#print(y)
else:
U1 = 0.02
U2 = 0
car.move(-U2 - U1, -U2 + U1, 0, dt)
#print(area)
if (area > 6700 and y < 126 and y > 122):
car.stop()
gripper.giveBall()
break
camera.showStream()
dt = (vrep.simxGetLastCmdTime(clientID) - ts) / 1000
def colibrate():
camera.setCameraPosition(30, 0.8)
dt = 0.001
while True:
ts = vrep.simxGetLastCmdTime(clientID)
camera.getCameraImage()
sign, x, y, area = camera.trackObject('red')
cameraAreaControl.control(area)
cameraPosControl.control(y)
if (sign):
U1 = cameraPosControl.U
U2 = cameraAreaControl.U
else:
U1 = 0.02
U2 = 0
car.move(-2 * U2 - U1, -2 * U2 + U1, 0, dt)
#print(area)
if (y < 128 and y > 122):
car.stop()
camera.destroyAllStream()
break
camera.showStream()
dt = (vrep.simxGetLastCmdTime(clientID) - ts) / 1000
def send_gear_commands(clientID, target_degree1, target_degree2, gearHandle1,
gearHandle2):
RAD = 180 / math.pi
lastCmdTime = vrep.simxGetLastCmdTime(clientID)
vrep.simxSynchronousTrigger(clientID)
count = 0
while vrep.simxGetConnectionId(clientID) != -1:
count += 1
if count > 20:
break
currCmdTime = vrep.simxGetLastCmdTime(clientID)
dt = currCmdTime - lastCmdTime
ret, gear_pos1 = vrep.simxGetJointPosition(clientID, gearHandle1,
vrep.simx_opmode_buffer)
ret, gear_pos2 = vrep.simxGetJointPosition(clientID, gearHandle2,
vrep.simx_opmode_buffer)
degree1 = round(gear_pos1 * RAD, 2)
degree2 = round(gear_pos2 * RAD, 2)
print(degree1, degree2)
if abs(degree1 - target_degree1) < 0.5 and abs(degree2 -
target_degree2) < 0.5:
break
vrep.simxPauseCommunication(clientID, True)
vrep.simxSetJointTargetPosition(clientID, gearHandle1,
target_degree1 / RAD,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(clientID, gearHandle2,
target_degree2 / RAD,
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
lastCmdTime = currCmdTime
vrep.simxSynchronousTrigger(clientID)
def searchDir(color):
camera.setCameraPosition(30, 0.8)
dt = 0.001
while True:
ts = vrep.simxGetLastCmdTime(clientID)
camera.getCameraImage()
sign, x, y, area = camera.trackObject(color)
cameraAreaControl.control(area)
cameraPosControl.control(y)
if (sign):
U1 = cameraPosControl.U
U2 = cameraAreaControl.U
else:
U1 = 0.02
U2 = 0
car.move(-U2 - U1, -U2 + U1, 0, dt)
#print(area)
if (area > 1000 and y < 128 and y > 122):
car.move(-velocity, -velocity, 0, dt)
break
camera.showStream()
dt = (vrep.simxGetLastCmdTime(clientID) - ts) / 1000
def readPositionSensors(self):
"""
get the position of robot TODO
"""
# get position
error, position_hexa_base = vrep.simxGetObjectPosition(
clientID, self.positionSensors["body"], -1,
vrep.simx_opmode_blocking)
time = vrep.simxGetLastCmdTime(clientID)
if error == 0:
self.XYZdata('bodyPosition', position_hexa_base, time)
else:
print("Position sensor read error: %d", error)
# get orientation
error, orientation_hexa_base = vrep.simxGetObjectOrientation(
clientID, self.positionSensors["body"], -1,
vrep.simx_opmode_blocking)
time = vrep.simxGetLastCmdTime(clientID)
if error == 0:
self.XYZdata('bodyOrientation', orientation_hexa_base, time)
else:
print("Orientation sensor read error: %d", error)
############################################################################
error, position_hexa = vrep.simxGetObjectPosition(
clientID, self.positionSensors["gyro"], -1,
vrep.simx_opmode_blocking)
time = vrep.simxGetLastCmdTime(clientID)
if error == 0:
self.XYZdata('gyro', position_hexa, time)
else:
print("Gyro sensor read error: %d", error)
def moveDir(vel, pos):
dt = 0
edgeS = False #машинка на склоне
edgeS2 = False #машинка проехала склон
position = 1
while (position < pos):
ts = vrep.simxGetLastCmdTime(clientID)
camera.getCameraImage()
sign, x, y, area = camera.trackObject('red')
cameraPosControl.control(y)
if area > 120 and area < 140:
position = 3
elif area > 180 and area < 200:
position = 4
elif area > 260 and area < 300:
position = 5
elif area > 450 and area < 550:
position = 6
elif area > 1300 and area < 1800:
position = 7
elif area > 2400 and area < 3000:
position = 8
if (sign):
U1 = cameraPosControl.U
else:
U1 = 0.02
car.move(-U1 - vel, +U1 - vel, 0, dt)
print(area, position)
#if(position>=pos and y<128 and y>122):
# break
#sens.updateAllSensors()
#regulator.pid(sens.lsVal-0.4,dt)
#imu.upgrateIMU()
#imuFilter.filter(imu.accelData,imu.gyroData,dt)
#if(imuFilter.roll>0.2 and not lineSensors.getGreenColorSignal()):
# U=0
# if not edgeS2 and not edgeS:
# edgeS=True
#else:
# #U=regulator.U
# U=0
#if(imuFilter.roll<0.2 and edgeS):
# edgeS2=True
# car.position=0
# edgeS=False
#if edgeS:
# car.position=0
#car.move(-vel-U,-vel+U,0,dt)
dt = (vrep.simxGetLastCmdTime(clientID) - ts) / 1000
car.stop()
car.position = 0
def main():
ur5 = UR5()
ur5.connect()
ur5.ankleinit()
clientID = ur5.get_clientID()
camera = Camera(clientID)
lastCmdTime = vrep.simxGetLastCmdTime(clientID)
vrep.simxSynchronousTrigger(clientID)
count = 0
while vrep.simxGetConnectionId(clientID) != -1 and count < Simu_STEP:
currCmdTime = vrep.simxGetLastCmdTime(clientID)
dt = currCmdTime - lastCmdTime
# Camera achieve data
cur_depth, cur_color = camera.get_camera_data()
cur_depth_path, cur_img_path = camera.save_image(
cur_depth, cur_color, currCmdTime)
# Call affordance map function
affordance_cmd = 'th ' + Lua_PATH + ' -imgColorPath ' + cur_img_path + ' -imgDepthPath ' + cur_depth_path + ' -resultPath ' + Save_PATH_RES + str(
currCmdTime) + '_results.h5'
try:
os.system(affordance_cmd)
print('-- Successfully create affordance map!')
except:
print(
'!!!!!!!!!!!!!!!!!!!!!!!! Error occurred during creating affordance map'
)
hdf2affimg(Save_PATH_RES + str(currCmdTime) + '_results.h5',
currCmdTime)
# TODO: get the push u,v coordinate
u = 256
v = 212
camera_coor = pixel2camera(u, v, cur_depth, camera.intri,
camera.Dis_FAR)
_, ur5_position = vrep.simxGetObjectPosition(
clientID, ur5.get_handle(), -1, vrep.simx_opmode_oneshot_wait)
location = camera2ur5(camera_coor, ur5_position, camera.cam_position)
# Move ur5
# location = random.randint(100, 200, (1, 3))
# location = location[0] / 400
print("UR5 Move to %s" % (location))
ur5.ur5moveto(location[0], location[1], location[2] - 0.05)
count += 1
lastCmdTime = currCmdTime
vrep.simxSynchronousTrigger(clientID)
vrep.simxGetPingTime(clientID)
ur5.disconnect()
def loop(self):
lastCmdTime = vrep.simxGetLastCmdTime(self.clientID) / 1000 # 记录当前时间
vrep.simxSynchronousTrigger(self.clientID) # 让仿真走一步
t = 0
while t < np.pi * 2:
currCmdTime = vrep.simxGetLastCmdTime(self.clientID) / 1000
dt = currCmdTime - lastCmdTime
t = t + dt
vrep.simxPauseCommunication(self.clientID, True)
vrep.simxSetJointTargetPosition(self.clientID,
self.robot1_jointHandle[0],
np.sin(t),
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(self.clientID,
self.robot1_jointHandle[1], 0,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(self.clientID,
self.robot1_jointHandle[2], 0,
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(self.clientID, False)
lastCmdTime = currCmdTime
vrep.simxSynchronousTrigger(self.clientID)
vrep.simxGetPingTime(self.clientID)
time.sleep(2)
t = 0
while t < np.pi * 2:
currCmdTime = vrep.simxGetLastCmdTime(self.clientID) / 1000
dt = currCmdTime - lastCmdTime
t = t + dt
vrep.simxPauseCommunication(self.clientID, True)
vrep.simxSetJointTargetPosition(self.clientID,
self.robot1_jointHandle[0], 0,
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(self.clientID,
self.robot1_jointHandle[1],
np.sin(t),
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetPosition(self.clientID,
self.robot1_jointHandle[2], 0,
vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(self.clientID, False)
lastCmdTime = currCmdTime
vrep.simxSynchronousTrigger(self.clientID)
vrep.simxGetPingTime(self.clientID)
def get_sim_time(self):
"""
Gets the simulation time. Returns zero if the simulation is stopped.
:return: simulation time in milliseconds.
"""
self.wait_for_ping()
return vrep.simxGetLastCmdTime(self._clientID)
def fetchKinect(clientID,depthSTR,rgbSTR):
errorCodeKinectRGB,kinectRGB=vrep.simxGetObjectHandle(clientID,rgbSTR,vrep.simx_opmode_blocking)
#errorCodeKinectDepth,kinectDepth=vrep.simxGetObjectHandle(clientID,depthSTR,vrep.simx_opmode_blocking)
#errorHere,resolution,image=vrep.simxGetVisionSensorImage(clientID,kinectRGB,0,vrep.simx_opmode_blocking)
# image_byte_array = array.array('b', image)
# image_buffer = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "BGR", 0, 1)
# image_buffer = image_buffer.transpose(Image.FLIP_LEFT_RIGHT)
# image_buffer = image_buffer.transpose(Image.ROTATE_180)
# img2 = np.asarray(image_buffer)
img_time=vrep.simxGetLastCmdTime(clientID)
# img,imgArr=Raftaar.ProcessImage(image,resolution)
# rgbArr=np.array(imgArr)
#print(len(image))
#print(image.shape)
#errorHere,resol,depth=vrep.simxGetVisionSensorDepthBuffer(clientID,kinectDepth,vrep.simx_opmode_oneshot_wait)
#depthArr=np.array(depth)
#print(image)
#return image,img_time
return img_time
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 main():
ur5 = UR5()
ur5.connect()
clientID = ur5.get_clientID()
camera = Camera(clientID)
lastCmdTime = vrep.simxGetLastCmdTime(clientID)
vrep.simxSynchronousTrigger(clientID)
count = 0
while vrep.simxGetConnectionId(clientID) != -1 and count < Simu_STEP:
currCmdTime = vrep.simxGetLastCmdTime(clientID)
dt = currCmdTime - lastCmdTime
# Camera achieve data
cur_depth, cur_color = camera.get_camera_data()
cur_depth_path, cur_img_path = camera.save_image(
cur_depth, cur_color, currCmdTime)
# Call affordance map function
affordance_cmd = 'th ' + Lua_PATH + ' -imgColorPath ' + cur_img_path + ' -imgDepthPath ' + cur_depth_path + ' -resultPath ' + Save_PATH_RES + str(
currCmdTime) + '_results.h5'
try:
os.system(affordance_cmd)
print('-- Successfully create affordance map!')
except:
print(
'!!!!!!!!!!!!!!!!!!!!!!!! Error occurred during creating affordance map'
)
hdf2affimg(Save_PATH_RES + str(currCmdTime) + '_results.h5',
currCmdTime)
# Move ur5
# TODO:
location = random.randint(100, 200, (1, 3))
location = location[0] / 400
print("UR5 Move to %s" % (location))
ur5.ur5moveto(location[0], location[1], location[2])
count += 1
lastCmdTime = currCmdTime
vrep.simxSynchronousTrigger(clientID)
vrep.simxGetPingTime(clientID)
ur5.disconnect()
def moveWhile(color):
dt = 0.001
lineSensors.updateLineSensors()
lineSensors.getBlackColorSignal()
lineSensors.getGreenColorSignal()
signal = False
while (not signal):
ts = vrep.simxGetLastCmdTime(clientID)
lineSensors.getBlackColorSignal()
lineSensors.getGreenColorSignal()
if color == 'green':
signal = lineSensors.greenSignal
elif color == 'black':
signal = lineSensors.blackSignal
#print(lineSensors.blackSignal )
car.move(-velocity, -velocity, 0, dt)
dt = (vrep.simxGetLastCmdTime(clientID) - ts) / 1000
time.sleep(2)
car.stop()
def readForceSensors(self):
for sensor in self.forceSensors.keys():
error, state, forceVector, torqueVector = vrep.simxReadForceSensor(
self.clientID, self.forceSensors[sensor],
vrep.simx_opmode_blocking)
time = vrep.simxGetLastCmdTime(clientID)
if error == 0:
self.XYZdata(sensor + 'force', forceVector, time)
self.XYZdata(sensor + 'torque', torqueVector, time)
else:
print("Force sensor read error: %d", error)
def reset(self):
self.get_point = False
self.grab_counter = 0
self.get_obstacles = False
self.obstacles_counter = 0
self.currCmdTime = vrep.simxGetLastCmdTime(self.clientID)
# dt = (self.currCmdTime - self.lastCmdTime) / 1000
self.moveto(self.config1, self.config2)
vrep.simxPauseCommunication(self.clientID, False)
self.lastCmdTime = self.currCmdTime
vrep.simxSynchronousTrigger(self.clientID)
vrep.simxGetPingTime(self.clientID)
s = self.getState_v1()
return s
def controller(self):
self.LCycleFreq = self.BaseFreq
self.RCycleFreq = self.BaseFreq
error = vrep.simxSetFloatSignal(self.clientID, self.LSignalName,
self.LCycleFreq,
vrep.simx_opmode_oneshot)
error = error or vrep.simxSetFloatSignal(
self.clientID, self.RSignalName, self.RCycleFreq,
vrep.simx_opmode_oneshot)
if error != 0:
print("Function error: ", error)
time = vrep.simxGetLastCmdTime(clientID)
self.telemetryData['cycleFrequencyTime'] = time
self.telemetryData['leftCycleFrequency'] = self.LCycleFreq
self.telemetryData['rightCycleFrequency'] = self.RCycleFreq
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] #Z
else:
self.__orientation = None
print >> sys.stderr, "Error in VRepBubbleRob.getOrientation()"
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:
print >> sys.stderr, "Error in VRepBubbleRob.getPosition()"
self.__position = None
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
simulationTime = vrep.simxGetLastCmdTime(self.__clientID)
thrust = 0.0
steering = 0.0
if simulationTime - self.__driveBackStartTime < 3000:
# driving backwards while slightly turning:
thrust = -1.0
steering = -1.0
else:
# going forward:
thrust = 1.0
steering = 0.0
if sensorTrigger:
self.__driveBackStartTime = simulationTime # We detected something, and start the backward mode
self.setSteering(steering)
self.setThrust(thrust)
getSignalReturnCode, dMessage = vrep.simxGetStringSignal(
self.__clientID, "Debug", vrep.simx_opmode_streaming)
if dMessage != "":
print("Debug:" + dMessage)
def init():
global robot
global blockHandleArray, connectionTime
print('Program started')
vrep.simxFinish(-1) # just in case, close all opened connections
int_portNb = 19999 # define port_nr
clientID = vrep.simxStart('127.0.0.1', int_portNb, True, True, 5000, 5) # connect to server
if clientID != -1:
print('Connected to remote API server')
res,objs = vrep.simxGetObjects(clientID,vrep.sim_handle_all,vrep.simx_opmode_oneshot_wait) # get all objects in the scene
if res == vrep.simx_return_ok: # Remote function call succeeded
print('Number of objects in the scene: ',len(objs))# print number of object in the scene
ret_lm, leftMotorHandle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
ret_rm, rightMotorHandle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)
ret_pr, pioneerRobotHandle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait)
ret_sl, ultraSonicSensorLeft = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_ultrasonicSensor4',vrep.simx_opmode_oneshot_wait)
ret_sr, ultraSonicSensorRight = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_ultrasonicSensor5',vrep.simx_opmode_oneshot_wait)
blockHandleArray = []
for i_block in range(12):
blockName = 'ConcretBlock#'+str(i_block)
retCode, handle = vrep.simxGetObjectHandle(clientID, blockName, vrep.simx_opmode_oneshot_wait)
assert retCode==0, retCode
if i_block>6:
rand_loc = [random.random()*6-1.5, random.random()*7-2.5, 0.0537] # x[-1.5,4.5] y[-2.5,-4.5]
vrep.simxSetObjectPosition(clientID, handle, -1, rand_loc, vrep.simx_opmode_oneshot_wait)
retCode,position = vrep.simxGetObjectPosition(clientID, handle, -1, vrep.simx_opmode_oneshot_wait)
assert retCode==0, retCode
blockHandleArray.append([handle,i_block,position])
robot = EasyDict(clientID=clientID,
leftMotorHandle=leftMotorHandle,
rightMotorHandle=rightMotorHandle,
pioneerRobotHandle=pioneerRobotHandle,
ultraSonicSensorLeft=ultraSonicSensorLeft,
ultraSonicSensorRight=ultraSonicSensorRight,
energySensor=None)
connectionTime = vrep.simxGetLastCmdTime(robot.clientID)
return robot
else:
print('Remote API function call returned with error code: ',res)
vrep.simxFinish(clientID) # close all opened connections
else:
print('Failed connecting to remote API server')
print('Program finished')
return {}
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 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] #Z
else:
self.__orientation = None
print >> sys.stderr, "Error in VRepBubbleRob.getOrientation()"
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:
print >> sys.stderr, "Error in VRepBubbleRob.getPosition()"
self.__position=None
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
simulationTime=vrep.simxGetLastCmdTime(self.__clientID)
thrust=0.0
steering=0.0
if simulationTime-self.__driveBackStartTime<3000:
# driving backwards while slightly turning:
thrust=-1.0
steering=-1.0
else:
# going forward:
thrust=1.0
steering=0.0
if sensorTrigger:
self.__driveBackStartTime=simulationTime # We detected something, and start the backward mode
self.setSteering(steering)
self.setThrust(thrust)
getSignalReturnCode, dMessage = vrep.simxGetStringSignal(self.__clientID, "Debug", vrep.simx_opmode_streaming)
if dMessage!="":
print("Debug:"+dMessage)
_, quadricopter_target_handle = vrep.simxGetObjectHandle(
clientID, "Quadricopter_target", vrep.simx_opmode_blocking)
_, quadricopter_handle = vrep.simxGetObjectHandle(clientID,
"Quadricopter_base",
vrep.simx_opmode_blocking)
_, v1 = vrep.simxGetObjectHandle(clientID, "zed_vision0",
vrep.simx_opmode_blocking)
print('Handles available!')
_, jpos = vrep.simxGetObjectPosition(clientID, quadricopter_target_handle, -1,
vrep.simx_opmode_streaming)
_, _, imageBuffer = vrep.simxGetVisionSensorImage(clientID, v1, -1,
vrep.simx_opmode_streaming)
lastCmdTime = vrep.simxGetLastCmdTime(clientID) # 記錄當前時間
vrep.simxSynchronousTrigger(clientID) # 讓模擬走一步
# 開始模擬
while vrep.simxGetConnectionId(clientID) != -1:
currCmdTime = vrep.simxGetLastCmdTime(clientID) # 記錄當前時間
dt = currCmdTime - lastCmdTime # 記錄時間間隔，用於控制
#-------------------------------------------
_, jpos = vrep.simxGetObjectPosition(clientID, quadricopter_target_handle,
-1, vrep.simx_opmode_buffer)
_, shape, imageBuffer = vrep.simxGetVisionSensorImage(
clientID, v1, -1, vrep.simx_opmode_streaming)
print(np.size(imageBuffer))
print(type(imageBuffer))
img = Image.fromarray(np.resize(np.array(imageBuffer), (720, 1280)),
mode='L')
print("Handles avaliable!")
# 然后首次读取关节的初始值，以streaming的形式
for i in range(0, jointNum - 1):
_, jpos = vrep.simxGetJointPosition(clientID, vJointHandle[i],
vrep.simx_opmode_streaming)
vJointPositionSensor[i] = jpos
for i in range(0, jointNum - 1):
_, jpos = vrep.simxGetJointPosition(clientID, hJointHandle[i],
vrep.simx_opmode_streaming)
hJointPositionSensor[i] = jpos
# Simulation
lastCmdTime = vrep.simxGetLastCmdTime(clientID) # 记录当前时间
vrep.simxSynchronousTrigger(clientID) # 让仿真走一步
# 开始仿真
steps = 0
while vrep.simxGetConnectionId(clientID) != -1 and steps <= 2000:
currCmdTime = vrep.simxGetLastCmdTime(clientID) # 记录当前时间
# strCmdTime = currCmdTime # 记录初始时间
dt = currCmdTime - lastCmdTime # 记录时间间隔，用于控制
# ---控制部分
# 读取当前的状态值，之后都用buffer形式读取
# 读取关节角（传感器返回值）
for i in range(0, jointNum - 1):
_, jpos = vrep.simxGetJointPosition(clientID, vJointHandle[i],
vrep.simx_opmode_buffer)
def get_sim_time(self) -> float:
"""Returns the current simulation time in seconds.
"""
return vrep.simxGetLastCmdTime(self.vr.client_id) / 1000.0
Q = 0.001
Rgps = np.array([[sqrt(0.5)], [sqrt(0.5)], [sqrt(0.0001)]])
R = np.zeros((3, 3))
np.fill_diagonal(R, Rgps)
car_EKF = dan_lib.EKF(vehicleWidth=width,
initialStates=initialStates,
probMatrix=probMatrix,
Q=Q)
#print(initialStates)
try:
global cur
aux = None
mean_time = []
vmax = 6
aux_m = 1
start = vrep.simxGetLastCmdTime(clientID)
start_pred = start
tempo = time.process_time()
nL = 0
nR = 0
still_neg_R = 0
still_neg_L = 0
auxR = 0
auxL = 0
iniR = 0
iniL = 0
index_pred = 0
while (True):
# Read sensor values for prior analysis
# 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,
accelY, accelZ, ori[0], ori[1], ori[2], LinearV[0],
LinearV[1], LinearV[2], t, d
]):
worksheet.write(i, col, data)
else:
i -= 1
#vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
sleep(0.01)
xx = []
zz = []
z_vel, x_vel = input("velocity in Z and X: ")
time1 = input("Time of flight: ")
vrep.simxSetObjectFloatParameter(clientID, obj1, vrep.sim_shapefloatparam_init_velocity_z, z_vel, vrep.simx_opmode_blocking)
vrep.simxSetObjectFloatParameter(clientID, obj1, vrep.sim_shapefloatparam_init_velocity_x, x_vel, vrep.simx_opmode_blocking)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
t_end = time.time() + time1
#while time.time() < t_end:
for i in range(0,10):
errorCodeKinectRGB,kinectRGB=vrep.simxGetObjectHandle(clientID,'kinect_rgb',vrep.simx_opmode_blocking)
#errorHere,resolution,image=vrep.simxGetVisionSensorImage(clientID,kinectRGB,0,vrep.simx_opmode_blocking)
img_time=vrep.simxGetLastCmdTime(clientID)
#errorCodeKinectDepth,kinectDepth=vrep.simxGetObjectHandle(clientID,depthSTR,vrep.simx_opmode_blocking)
errorHere,resolution,image=vrep.simxGetVisionSensorImage(clientID,kinectRGB,0,vrep.simx_opmode_oneshot_wait)
image_byte_array = array.array('b', image)
image_buffer = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "BGR", 0, 1)
image_buffer = image_buffer.transpose(Image.FLIP_LEFT_RIGHT)
image_buffer = image_buffer.transpose(Image.ROTATE_180)
img2 = np.asarray(image_buffer)
img_time=vrep.simxGetLastCmdTime(clientID)
#print(img_time)
#cv2.imwrite(str(img_time)+'.png',img2)
pose_list = []
xx = []
zz = []
#######################################################Setting Velocity###############################################################################
z_vel, x_vel, y_vel = 0.8, -5, -0.3
#z_vel, x_vel, y_vel = 0,0,0
vrep.simxSetObjectFloatParameter(clientID, obj1, vrep.sim_shapefloatparam_init_velocity_z, z_vel, vrep.simx_opmode_blocking)
vrep.simxSetObjectFloatParameter(clientID, obj1, vrep.sim_shapefloatparam_init_velocity_x, x_vel, vrep.simx_opmode_blocking)
# vrep.simxSetObjectFloatParameter(clientID, obj1, vrep.sim_shapefloatparam_init_velocity_y, y_vel, vrep.simx_opmode_blocking)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
for i in range(0,10):
time1=vrep.simxGetLastCmdTime(clientID)
pose = getPosition(clientID)
pose_list.append(pose)
if len(pose_list) > 0 & len(pose_list) <4:
xyz = pose[1]
xx.append(xyz[0])
zz.append(xyz[2])
if len(pose_list) >3:
a,b,c = calc_parabola_vertex(xx[0], zz[0], xx[1], zz[1], xx[2], zz[2])
z_req= (a*(x_req**2))+(b*x_req)+c # 2D parabolic equation : z = a*x^2 + b* x + c
if z_req < 0 : z_req = 0
print('Position from sim handle'+str(z_req))
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_buffer)
# Not Started
if not sum(pos):
continue
# Simulation Time
if time != vrep.simxGetLastCmdTime(clientID):
time_pre = time
time = vrep.simxGetLastCmdTime(clientID)
else:
continue
# First Step
if not ok:
ok = True
Previous_position = [pos[0], pos[1], pos[2]]
Predict_position = Previous_position[:]
time_pre = time
continue
t = (time - time_pre) / 1000
Predict_position = [
network = NN(ni,nj,nk)
vrep.simxStartSimulation(client_id, vrep.simx_opmode_oneshot_wait)
position = position_init # [x,y,theta]
network_input = [0, 0, 0]
for i in range(ni):
network_input[i] = position[i]-target[i]
prev_error = 100000
#while(distance(position, target)>0.001 or position[2]-target[2]>0.1):
while(True):
#current_time = time.time()
current_time = vrep.simxGetLastCmdTime(client_id)
command = network.runNN(network_input)
vrep.simxSetJointTargetVelocity(client_id, left_motor, 2*command[0], vrep.simx_opmode_oneshot_wait)
vrep.simxSetJointTargetVelocity(client_id, right_motor, 2*command[1], vrep.simx_opmode_oneshot_wait)
# wait for velocity stabilization (??ms)
time.sleep(0.050)
# calcul prochain input
res, tmp = vrep.simxGetObjectPosition(client_id, pioneer, -1, vrep.simx_opmode_oneshot_wait)
position[0] = tmp[0]
position[1] = tmp[1]
res, tmp = vrep.simxGetObjectOrientation(client_id, pioneer, -1, vrep.simx_opmode_oneshot_wait)
position[2] = tmp[2] # en radian
vrep.simx_opmode_blocking)
jointHandle[i] = returnHandle
_, baseHandle = vrep.simxGetObjectHandle(clientID, baseName,
vrep.simx_opmode_blocking)
print('Handles available!')
jointConfig = np.zeros((jointNum, ))
for i in range(jointNum):
_, jpos = vrep.simxGetJointPosition(clientID, jointHandle[i],
vrep.simx_opmode_streaming)
jointConfig[i] = jpos
# print('Handle avaliable')
# simulation
lastCmdTime = vrep.simxGetLastCmdTime(clientID)
vrep.simxSynchronousTrigger(clientID)
print('control')
target_pos = np.array([80, 160, 80, 30, 20, 80])
while vrep.simxGetConnectionId(clientID) != -1:
currCmdTime = vrep.simxGetLastCmdTime(clientID)
dt = currCmdTime - lastCmdTime
for i in range(jointNum):
_, jpos = vrep.simxGetJointPosition(clientID, jointHandle[i],
vrep.simx_opmode_buffer)
print('position', round(jpos * RAD2DEG, 2))
jointConfig[i] = jpos
vrep.simxPauseCommunication(clientID, True)
xx = []
zz = []
z_vel, x_vel = input("velocity in Z and X: ")
time1 = input("Time of flight: ")
vrep.simxSetObjectFloatParameter(clientID, obj1, vrep.sim_shapefloatparam_init_velocity_z, z_vel, vrep.simx_opmode_blocking)
vrep.simxSetObjectFloatParameter(clientID, obj1, vrep.sim_shapefloatparam_init_velocity_x, x_vel, vrep.simx_opmode_blocking)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
t_end = time.time() + time1
#while time.time() < t_end:
for i in range(0,10):
errorCodeKinectRGB,kinectRGB=vrep.simxGetObjectHandle(clientID,'kinect_rgb',vrep.simx_opmode_blocking)
#errorHere,resolution,image=vrep.simxGetVisionSensorImage(clientID,kinectRGB,0,vrep.simx_opmode_blocking)
img_time=vrep.simxGetLastCmdTime(clientID)
#errorCodeKinectDepth,kinectDepth=vrep.simxGetObjectHandle(clientID,depthSTR,vrep.simx_opmode_blocking)
errorHere,resolution,image=vrep.simxGetVisionSensorImage(clientID,kinectRGB,0,vrep.simx_opmode_oneshot_wait)
image_byte_array = array.array('b', image)
image_buffer = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "BGR", 0, 1)
image_buffer = image_buffer.transpose(Image.FLIP_LEFT_RIGHT)
image_buffer = image_buffer.transpose(Image.ROTATE_180)
img2 = np.asarray(image_buffer)
img_time=vrep.simxGetLastCmdTime(clientID)
#print(img_time)
#cv2.imwrite(str(img_time)+'.png',img2)
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)
time.sleep(5000.0 / 1000.0)
sys.exit(0)
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', portNb, True, True, 2000, 5)
if clientID != -1:
print('Connected to remote API server')
driveBackStartTime = -99000
motorSpeeds = [0, 0]
while vrep.simxGetConnectionId(clientID) != -1:
(errorCode, detectionState, detectedPoint, detectedObjectHandle,
detectedSurfaceNormalVector) = vrep.simxReadProximitySensor(
clientID, sensorHandle, vrep.simx_opmode_streaming)
if errorCode == vrep.simx_return_ok:
simulationTime = vrep.simxGetLastCmdTime(clientID)
if simulationTime - driveBackStartTime < 3000:
motorSpeeds[0] = -3.1415 * 0.5
motorSpeeds[1] = -3.1415 * 0.25
else:
motorSpeeds[0] = 3.1415
motorSpeeds[1] = 3.1415
if detectionState:
driveBackStartTime = simulationTime
errorCode = vrep.simxSetJointTargetVelocity(
clientID, leftMotorHandle, motorSpeeds[0],
vrep.simx_opmode_oneshot)
vrep.simxSetJointTargetVelocity(clientID, rightMotorHandle,
motorSpeeds[1],
vrep.simx_opmode_oneshot)
def getSimulationTime():
vrep.simxGetPingTime(robot.clientID)
return vrep.simxGetLastCmdTime(robot.clientID) - connectionTime
def labirintAlgorithm():
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
while clientID <= -1:
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID != -1:
print('Successful !!!')
else:
print('connection not successful')
#sys.exit('could not connect')
print('connected to remote api server')
#vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot)
imu = IMU(clientID)
regulator = pidControl(0.008, 0.01, 0.002)
car = Robot(clientID, 0.04)
sens = ProxSensor(clientID)
map = Map(clientID)
lineSensors = IRSensors(clientID)
map.getCameraImage()
map.getMap()
map.spreadRay()
#lineSensors.getBlackColorSignal()
i = 0
velocity = 0.1
dt = 0.0001
sens.updateAllSensors()
time.sleep(2)
U = 0
while True:
timeStart = vrep.simxGetLastCmdTime(clientID)
#lineSensors.getBlackColorSignal()
sens.updateAllSensors()
#if(sens.rsVal<0.6 and sens.rsVal>0.35) and (sens.lsVal<0.6 and sens.lsVal>0.35):
# regulator.pid(sens.rsVal-sens.lsVal,dt)
# U=regulator.U
#else:
# U=0
car.move(velocity + U, velocity - U, 0, dt)
#if(i==len(map.carWay)-1):
# time.sleep(4)
# break
#print(sens.drsVal)
if (sens.drsVal < 0.4 and sens.drsVal > 0.3) and (sens.dlsVal < 0.4 and
sens.dlsVal > 0.3):
if (map.carWay[i] == 1):
car.rotate(80)
else:
car.rotate(-80)
sens.updateAllSensors()
time.sleep(0.5)
i += 1
dt = (vrep.simxGetLastCmdTime(clientID) - timeStart) / 1000
car.stop()
time.sleep(1)
vrep.simxFinish(clientID)
