This test file is dependent on vrep.
@author: cz
"""
from scene import Scene
from sceneplot import ScenePlot
from robot import Robot
import numpy as np
try:
sc = Scene()
sp = ScenePlot(sc)
dynamics = 11
sc.dynamics = dynamics
sc.addRobot(np.float32([[-2, 0, 0], [0, 2 / 2, 0]]))
sc.addRobot(np.float32([[1, 3, 0], [1.732 / 2, -1 / 2, 0]]))
sc.addRobot(np.float32([[0, 0, 0], [-1.732 / 2, -1 / 2, 0]]))
sc.setADjMatrix(np.uint8([[0, 1, 1], [1, 0, 1], [1, 1, 0]]))
# vrep related
sc.initVrep()
# Choose sensor type
sc.SENSOR_TYPE = "VPL16" # None, 2d, VPL16, kinect
sc.objectNames = [
'Pioneer_p3dx', 'Pioneer_p3dx_leftMotor', 'Pioneer_p3dx_rightMotor'
]
if sc.SENSOR_TYPE == "None":
pass
elif sc.SENSOR_TYPE == "2d":
def generateData():
sc = Scene(recordData=True)
sp = ScenePlot(sc)
#sc.occupancyMapType = sc.OCCUPANCY_MAP_THREE_CHANNEL
sc.occupancyMapType = sc.OCCUPANCY_MAP_BINARY
sc.dynamics = sc.DYNAMICS_MODEL_BASED_LINEAR # robot dynamics
try:
sc.addRobot(np.float32([[-2, 0, 0], [0.0, 0.0, 0.0]]),
role=sc.ROLE_LEADER)
sc.addRobot(np.float32([[1, 3, 0], [-1.0, 0.0, 0.0]]),
role=sc.ROLE_FOLLOWER)
# sc.addRobot(np.float32([[1, 3, 0], [-1.0, 0.0, 0.0]]), role = sc.ROLE_FOLLOWER, learnedController = fcl.test)
#==============================================================================
# sc.addRobot(np.float32([[1, 3, 0], [0, -1, 0]]),
# dynamics = sc.DYNAMICS_LEARNED,
# learnedController = fcl.test)
#==============================================================================
# No leader
sc.setADjMatrix(np.uint8([[0, 0], [1, 0]]))
# Set robot 0 as the leader.
# vrep related
sc.initVrep()
# Choose sensor type
sc.SENSOR_TYPE = "VPL16" # None, 2d, VPL16, kinect
#sc.SENSOR_TYPE = "None" # None, 2d, VPL16, kinect
sc.objectNames = [
'Pioneer_p3dx', 'Pioneer_p3dx_leftMotor', 'Pioneer_p3dx_rightMotor'
]
if sc.SENSOR_TYPE == "None":
sc.setVrepHandles(0, '')
sc.setVrepHandles(1, '#0')
elif sc.SENSOR_TYPE == "2d":
sc.objectNames.append('LaserScanner_2D_front')
sc.objectNames.append('LaserScanner_2D_rear')
sc.setVrepHandles(0, '')
sc.setVrepHandles(1, '#0')
elif sc.SENSOR_TYPE == "VPL16":
sc.objectNames.append('velodyneVPL_16') # _ptCloud
sc.setVrepHandles(0, '')
sc.setVrepHandles(1, '#0')
elif sc.SENSOR_TYPE == "kinect":
sc.objectNames.append('kinect_depth')
sc.objectNames.append('kinect_rgb')
sc.setVrepHandles(0, '')
sc.setVrepHandles(1, '#0')
#sc.renderScene(waitTime = 3000)
tf = 40 # must be greater than 1
sc.resetPosition()
sp.plot(3, tf)
while sc.simulate():
#sc.renderScene(waitTime = int(sc.dt * 1000))
sc.showOccupancyMap(waitTime=int(sc.dt * 1000))
#print("---------------------")
#print("t = %.3f" % sc.t, "s")
if sc.t > 1:
maxAbsError = sc.getMaxFormationError()
if maxAbsError < 0.01:
pass #tf = sc.t - 0.01
#sp.plot(0, tf)
sp.plot(2, tf)
#sp.plot(1, tf)
sp.plot(3, tf)
sp.plot(4, tf)
sp.plot(6, tf)
if sc.t > tf:
print('maxAbsError = ', maxAbsError)
break
#print('robot 0: ', sc.robots[0].xi.x, ', ', sc.robots[0].xi.y, ', ', sc.robots[0].xi.theta)
#print('robot 1: ', sc.robots[1].xi.x, ', ', sc.robots[1].xi.y, ', ', sc.robots[1].xi.theta)
#print('robot 2: ', sc.robots[2].xi.x, ', ', sc.robots[2].xi.y, ', ', sc.robots[2].xi.theta)
#print('y01: ' + str(sc.robots[1].xi.y - sc.robots[0].xi.y))
#print('x02: ' + str(sc.robots[2].xi.x - sc.robots[0].xi.x))
sc.deallocate()
except KeyboardInterrupt:
x = input('Quit?(y/n)')
sc.deallocate()
if x == 'y' or x == 'Y':
tf = sc.t - 0.01
sp.plot(2, tf)
sp.plot(3, tf)
sp.plot(4, tf)
sp.plot(6, tf)
raise Exception('Aborted.')
except:
sc.deallocate()
raise
if True: #maxAbsError < 0.01:
return sc
else:
return None
def generateData():
sc = Scene(recordData=True)
sp = ScenePlot(sc)
try:
dynamics = 11
sc.dynamics = dynamics
sc.addRobot(np.float32([[-2, 0, 0], [0, 2 / 2, 0]]))
sc.addRobot(np.float32([[1, 3, 0], [1.732 / 2, -1 / 2, 0]]))
sc.addRobot(np.float32([[0, 0, 0], [-1.732 / 2, -1 / 2, 0]]))
# No leader
sc.setADjMatrix(np.uint8([[0, 1, 1], [1, 0, 1], [1, 1, 0]]))
# Set robot 0 as the leader.
# sc.setADjMatrix(np.uint8([[0, 0, 0], [1, 0, 1], [1, 1, 0]]))
# vrep related
sc.initVrep()
# Choose sensor type
sc.SENSOR_TYPE = "VPL16" # None, 2d, VPL16, kinect
sc.objectNames = [
'Pioneer_p3dx', 'Pioneer_p3dx_leftMotor', 'Pioneer_p3dx_rightMotor'
]
if sc.SENSOR_TYPE == "None":
pass
elif sc.SENSOR_TYPE == "2d":
sc.objectNames.append('LaserScanner_2D_front')
sc.objectNames.append('LaserScanner_2D_rear')
sc.setVrepHandles(0, '')
sc.setVrepHandles(1, '#0')
sc.setVrepHandles(2, '#1')
elif sc.SENSOR_TYPE == "VPL16":
sc.objectNames.append('velodyneVPL_16') # _ptCloud
sc.setVrepHandles(0, '')
sc.setVrepHandles(1, '#0')
sc.setVrepHandles(2, '#1')
elif sc.SENSOR_TYPE == "kinect":
sc.objectNames.append('kinect_depth')
sc.objectNames.append('kinect_rgb')
sc.setVrepHandles(0, '')
sc.setVrepHandles(1, '#0')
sc.setVrepHandles(2, '#1')
#sc.renderScene(waitTime = 3000)
tf = 10
sc.resetPosition()
sp.plot(3, tf)
while sc.simulate():
#sc.renderScene(waitTime = int(sc.dt * 1000))
sc.showOccupancyMap(waitTime=int(sc.dt * 1000))
#print("---------------------")
#print("t = %.3f" % sc.t, "s")
#sp.plot(0, tf)
sp.plot(2, tf)
#sp.plot(1, tf)
sp.plot(3, tf)
sp.plot(4, tf)
sp.plot(5, tf)
sp.plot(6, tf)
if sc.t > tf:
break
#print('robot 0: ', sc.robots[0].xi.x, ', ', sc.robots[0].xi.y, ', ', sc.robots[0].xi.theta)
#print('robot 1: ', sc.robots[1].xi.x, ', ', sc.robots[1].xi.y, ', ', sc.robots[1].xi.theta)
#print('robot 2: ', sc.robots[2].xi.x, ', ', sc.robots[2].xi.y, ', ', sc.robots[2].xi.theta)
#print('y01: ' + str(sc.robots[1].xi.y - sc.robots[0].xi.y))
#print('x02: ' + str(sc.robots[2].xi.x - sc.robots[0].xi.x))
sc.deallocate()
except KeyboardInterrupt:
x = input('Quit?(y/n)')
sc.deallocate()
if x == 'y' or x == 'Y':
raise Exception('Aborted.')
except:
sc.deallocate()
raise
# check max formation error
maxAbsError = 0
for key in sc.ydict[2]:
absError = abs(sc.ydict[2][key][-1])
if absError > maxAbsError:
maxAbsError = absError
print('maxAbsError = ', maxAbsError)
if maxAbsError < 0.5:
return sc
else:
return None
def generateData(i):
sc = Scene(fileName=__file__, recordData=True, runNum=i)
sp = ScenePlot(sc)
sp.saveEnabled = True # save plots?
#sc.occupancyMapType = sc.OCCUPANCY_MAP_THREE_CHANNEL
sc.occupancyMapType = sc.OCCUPANCY_MAP_BINARY
sc.dynamics = sc.DYNAMICS_MODEL_BASED_DISTANCE2_REFVEL # robot dynamics
sc.errorType = 0
try:
for i in range(robotNum):
sc.addRobot(np.float32([[-2, 0, 1], [0.0, 0.0, 0.0]]),
role=sc.ROLE_PEER)
#==============================================================================
# sc.addRobot(np.float32([[1, 3, 0], [0, -1, 0]]),
# dynamics = sc.DYNAMICS_LEARNED,
# learnedController = fcl.test)
#==============================================================================
# No leader
I = np.identity(robotNum, dtype=np.int8)
M = np.ones(robotNum, dtype=np.int8)
sc.setADjMatrix(M - I)
# Set robot 0 as the leader.
# vrep related
sc.initVrep()
# Choose sensor type
#sc.SENSOR_TYPE = "VPL16" # None, 2d, VPL16, kinect
sc.SENSOR_TYPE = "None" # None, 2d, VPL16, kinect
sc.objectNames = [
'Pioneer_p3dx', 'Pioneer_p3dx_leftMotor', 'Pioneer_p3dx_rightMotor'
]
if sc.SENSOR_TYPE == "None":
sc.setVrepHandles(0, '')
sc.setVrepHandles(1, '#0')
sc.setVrepHandles(2, '#1')
sc.setVrepHandles(3, '#2')
elif sc.SENSOR_TYPE == "VPL16":
sc.objectNames.append('velodyneVPL_16') # _ptCloud
sc.setVrepHandles(0, '')
sc.setVrepHandles(1, '#0')
sc.setVrepHandles(2, '#1')
sc.setVrepHandles(3, '#2')
#sc.renderScene(waitTime = 3000)
tf = 15 # must be greater than 1
errorCheckerEnabled = False
initRef(sc, i)
sc.resetPosition(4) # Random initial position
# Fixed initial position
#sc.robots[0].setPosition([0.0, 0.0, math.pi/2])
#sc.robots[1].setPosition([-2.2, -1.0, 0.3])
sp.plot(4, tf)
while sc.simulate():
#sc.renderScene(waitTime = int(sc.dt * 1000/2))
#sc.showOccupancyMap(waitTime = int(sc.dt * 1000))
#print("---------------------")
#print("t = %.3f" % sc.t, "s")
#print(sc.robots[2].xid0.y)
if sc.t > 1:
maxAbsError = sc.getMaxFormationError()
if maxAbsError < 0.01 and errorCheckerEnabled:
#tf = sc.t - 0.01
# set for how many seconds after convergence the simulator shall run
tExtra = 30
#tf = sc.t + tExtra
errorCheckerEnabled = False
print('Ending in ', str(tExtra), ' seconds...')
plot(sp, tf)
if sc.t > tf:
message = "maxAbsError = {0:.3f} m".format(maxAbsError)
sc.log(message)
print(message)
break
except KeyboardInterrupt:
x = input('Quit?(y/n)')
if x == 'y' or x == 'Y':
tf = sc.t - 0.01
plot(sp, tf)
raise Exception('Aborted.')
except VrepError as err:
sc.log(err.message)
print(err.message)
return None
except:
raise
finally:
sc.deallocate()
if True: #maxAbsError < 0.01:
return sc
else:
return None
def generateData(**kwargs):
sc = Scene(recordData=True)
sp = ScenePlot(sc)
try:
#dynamics = 20
arg2 = np.float32([.5, .5])
for name, value in kwargs.items():
if name == "dynamics":
dynamics = value
elif name == "arg2":
arg2 = value
sc.dynamics = dynamics
sc.addRobot(np.float32([[0, 0, 0], [0, 2 / 2, 0]]), arg2)
# No leader
sc.setADjMatrix(np.uint8([[0]]))
# Set robot 0 as the leader.
# sc.setADjMatrix(np.uint8([[0, 0, 0], [1, 0, 1], [1, 1, 0]]))
# vrep related
sc.initVrep()
# Choose sensor type
sc.SENSOR_TYPE = "None" # None, 2d, VPL16, kinect
sc.objectNames = [
'Pioneer_p3dx', 'Pioneer_p3dx_leftMotor', 'Pioneer_p3dx_rightMotor'
]
if sc.SENSOR_TYPE == "None":
sc.setVrepHandles(0, '')
elif sc.SENSOR_TYPE == "2d":
sc.objectNames.append('LaserScanner_2D_front')
sc.objectNames.append('LaserScanner_2D_rear')
sc.setVrepHandles(0, '')
sc.setVrepHandles(1, '#0')
sc.setVrepHandles(2, '#1')
elif sc.SENSOR_TYPE == "VPL16":
sc.objectNames.append('velodyneVPL_16') # _ptCloud
sc.setVrepHandles(0, '')
sc.setVrepHandles(1, '#0')
sc.setVrepHandles(2, '#1')
elif sc.SENSOR_TYPE == "kinect":
sc.objectNames.append('kinect_depth')
sc.objectNames.append('kinect_rgb')
sc.setVrepHandles(0, '')
sc.setVrepHandles(1, '#0')
sc.setVrepHandles(2, '#1')
#sc.renderScene(waitTime = 3000)
tf = 30
sp.plot(3, tf)
while sc.simulate():
#sc.renderScene(waitTime = int(sc.dt * 1000))
#sc.showOccupancyMap(waitTime = int(sc.dt * 1000))
#print("---------------------")
#print("t = %.3f" % sc.t, "s")
#sp.plot(0, tf)
#sp.plot(1, tf)
#sp.plot(2, tf)
sp.plot(3, tf)
sp.plot(4, tf)
sp.plot(5, tf)
sp.plot(6, tf)
if sc.t > tf:
break
sc.deallocate()
except KeyboardInterrupt:
x = input('Quit?(y/n)')
sc.deallocate()
if x == 'y' or x == 'Y':
raise Exception('Aborted.')
except:
sc.deallocate()
raise
return None
def generateData(i):
sc = Scene(fileName=__file__, recordData=False, runNum=i)
sp = ScenePlot(sc)
sp.saveEnabled = True # save plots?
#sc.occupancyMapType = sc.OCCUPANCY_MAP_THREE_CHANNEL
sc.occupancyMapType = sc.OCCUPANCY_MAP_BINARY
sc.dynamics = 5 # robot dynamics
sc.errorType = 0
try:
sc.addRobot(np.float32([[-.0, 0, 0], [0.0, 0.0, 0.0]]),
role=sc.ROLE_PEER)
sc.addRobot(np.float32([[-3, 4, 0], [1.0, 0.0, 0.0]]),
role=sc.ROLE_PEER)
sc.addRobot(np.float32([[2, 1, 0], [0.0, 1.732, 0.0]]),
role=sc.ROLE_PEER)
#==============================================================================
# sc.addRobot(np.float32([[1, 3, 0], [0, -1, 0]]),
# dynamics = sc.DYNAMICS_LEARNED,
# learnedController = fcl.test)
#==============================================================================
# No leader
sc.setADjMatrix(np.uint8([[0, 1, 1], [1, 0, 1], [1, 1, 0]]))
# Set robot 0 as the leader.
#sc.renderScene(waitTime = 3000)
tf = 15 # must be greater than 1
errorCheckerEnabled = False
initRef(sc, i)
#sc.resetPosition(2) # Random initial position
sc.robots[0].setPosition([.0, .0, .0])
sc.robots[1].setPosition([-2.0, 0.001, 0.0])
sc.robots[2].setPosition([2.0, 0.0, 0.0])
#sc.robots[0].setPosition([.0, .0, .0])
#sc.robots[1].setPosition([-3.0, 4.0, 0.0])
#sc.robots[2].setPosition([2.0, 1.0, 0.0])
# Fixed initial position
#sc.robots[0].setPosition([0.0, 0.0, math.pi/2])
#sc.robots[1].setPosition([-2.2, -1.0, 0.3])
sp.plot(4, tf)
while sc.simulate():
sc.renderScene(waitTime=int(sc.dt * 1000), mode=1)
#sc.showOccupancyMap(waitTime = int(sc.dt * 1000))
#print("---------------------")
#print("t = %.3f" % sc.t, "s")
#print(sc.robots[2].xid0.y)
if sc.t > 1:
maxAbsError = sc.getMaxFormationError()
if maxAbsError < 0.01 and errorCheckerEnabled:
#tf = sc.t - 0.01
# set for how many seconds after convergence the simulator shall run
tExtra = 30
#tf = sc.t + tExtra
errorCheckerEnabled = False
print('Ending in ', str(tExtra), ' seconds...')
plot(sp, tf)
if sc.t > tf:
message = "maxAbsError = {0:.3f} m".format(maxAbsError)
sc.log(message)
print(message)
break
except KeyboardInterrupt:
x = input('Quit?(y/n)')
if x == 'y' or x == 'Y':
tf = sc.t - 0.01
plot(sp, tf)
raise Exception('Aborted.')
except VrepError as err:
sc.log(err.message)
print(err.message)
return None
except:
raise
finally:
sc.deallocate()
if True: #maxAbsError < 0.01:
return sc
else:
return None