def test_reverseMDP(plot=False):
greed = 0.9
phobia = 0.1
angStep = 1
downsample = int(numpy.floor(3 / angStep))
xPos, yPos = utils.hexFromDia(25, pitch=22.4)
seed = 1
rg = RobotGrid(stepSize=angStep, epsilon=epsilon, seed=seed)
for robotID, (x, y) in enumerate(zip(xPos, yPos)):
rg.addRobot(robotID, str(robotID), [x, y, 0], hasApogee)
rg.setCollisionBuffer(collisionBuffer)
rg.initGrid()
for rID in rg.robotDict:
robot = rg.getRobot(rID)
robot.setXYUniform()
assert rg.getNCollisions() > 10
rg.decollideGrid()
for robot in rg.robotDict.values():
robot.setDestinationAlphaBeta(0, 180)
assert rg.getNCollisions() == 0
rg.pathGenMDP(greed, phobia)
if plot:
utils.plotPaths(rg, downsample=downsample, filename="reverseMDP.mp4")
def test_fatty(plot=False):
xPos, yPos = utils.hexFromDia(11, pitch=22.4)
print("n robots", len(xPos))
angStep = 0.1
greed = 0.8
phobia = 0.2
collisionBuffer = 4
downsample = int(numpy.floor(3 / angStep))
rg = RobotGrid(stepSize=angStep, epsilon=epsilon, seed=1)
for robotID, (x, y) in enumerate(zip(xPos, yPos)):
rg.addRobot(robotID, str(robotID), [x, y, 0], hasApogee)
rg.setCollisionBuffer(collisionBuffer)
rg.initGrid()
for rID in rg.robotDict:
robot = rg.getRobot(rID)
robot.setXYUniform()
robot.setDestinationAlphaBeta(90, 180)
# assert rg.getNCollisions() > 10
rg.decollideGrid()
rg.pathGenMDP(greed, phobia)
# rg.smoothPaths(3)
# rg.simplifyPaths()
# rg.verifySmoothed()
if plot:
utils.plotPaths(rg, downsample=downsample, filename="fatty.mp4")
def test_forwardMDP(plot=False):
xPos, yPos = utils.hexFromDia(25, pitch=22.4)
seed = 1
rg = RobotGrid(stepSize=angStep, epsilon=epsilon, seed=seed)
for robotID, (x, y) in enumerate(zip(xPos, yPos)):
rg.addRobot(robotID, str(robotID), [x, y, 0], hasApogee)
rg.setCollisionBuffer(collisionBuffer)
rg.initGrid()
for rID in rg.robotDict:
robot = rg.getRobot(rID)
robot.setXYUniform()
assert rg.getNCollisions() > 10
rg.decollideGrid()
print("N collisions 1", rg.getNCollisions())
for robot in rg.robotDict.values():
robot.setDestinationAlphaBeta(robot.alpha, robot.beta)
robot.setAlphaBeta(0, 180)
print("N collisions 2", rg.getNCollisions())
# assert rg.getNCollisions() == 0
rg.pathGenMDP(0.9, 0.1)
if plot:
utils.plotPaths(rg, filename="forwardMDP.mp4")
def test_initialConfigs(plot=False):
xPos, yPos = utils.hexFromDia(21, pitch=22.4)
angStep = 0.1
greed = 0.8
phobia = 0.2
downsample = int(numpy.floor(10 / angStep))
rg = RobotGrid(stepSize=angStep, epsilon=epsilon, seed=1)
for robotID, (x, y) in enumerate(zip(xPos, yPos)):
rg.addRobot(robotID, str(robotID), [x, y, 0], hasApogee)
rg.setCollisionBuffer(collisionBuffer)
rg.initGrid()
for rID in rg.robotDict:
robot = rg.getRobot(rID)
robot.setXYUniform()
assert rg.getNCollisions() > 10
if plot:
utils.plotOne(-1, rg, figname="angStepO.png", isSequence=False)
rg.decollideGrid()
for robot in rg.robotDict.values():
robot.setDestinationAlphaBeta(0, 180)
if plot:
utils.plotOne(-1, rg, figname="angStepD.png", isSequence=False)
rg.pathGenMDP(greed, phobia)
if plot:
utils.plotOne(-1, rg, figname="angStepE.png", isSequence=False)
if plot:
utils.plotPaths(rg, downsample=downsample, filename="init.mp4")
def test_withDefulatArgs(plot=False):
rg = RobotGridNominal() # this is the test, that no args still works
for rID in rg.robotDict:
robot = rg.getRobot(rID)
robot.setXYUniform()
robot.setDestinationAlphaBeta(0, 180)
assert rg.getNCollisions() > 10
rg.decollideGrid()
assert rg.getNCollisions() == 0
rg.pathGenGreedy()
if plot:
utils.plotPaths(rg, filename="test_default.mp4")
def test_unevenCBs(plot=False):
hasApogee = True
greed = 0.8
phobia = 0.2
xPos, yPos = utils.hexFromDia(17, pitch=22.4)
seed = 1
# cb = 2.5
cs = 0.04
step = 0.1 # degrees per step in kaiju's rough path
smoothPts = 10 # width of velocity smoothing window
eps = step * 2.2
# downsample = int(numpy.floor(50 / step))
rg = RobotGrid(stepSize=step, epsilon=eps, seed=seed)
for robotID, (x, y) in enumerate(zip(xPos, yPos)):
rg.addRobot(robotID,
str(robotID), [x, y, 0],
hasApogee,
collisionBuffer=numpy.random.uniform(1.5, 3.5))
rg.initGrid()
for rID in rg.robotDict:
robot = rg.getRobot(rID)
robot.setXYUniform()
assert rg.getNCollisions() > 10
rg.decollideGrid()
for robot in rg.robotDict.values():
robot.setDestinationAlphaBeta(0, 180)
assert rg.getNCollisions() == 0
tstart = time.time()
rg.pathGenMDP(greed, phobia)
print("pathgen took", time.time() - tstart)
rg.smoothPaths(smoothPts)
rg.simplifyPaths()
rg.shrinkCollisionBuffer(cs)
rg.verifySmoothed()
assert rg.smoothCollisions == 0
print("n smooth collisions", rg.smoothCollisions)
if plot:
for r in rg.robotDict.values():
utils.plotTraj(r, "unevenCBs", dpi=250)
utils.plotPaths(rg, downsample=3, filename="unevenCBs.mp4")
def test_setMDP(plot=False):
greed = 0.8
phobia = 0.2
xPos, yPos = utils.hexFromDia(45, pitch=22.4)
print("using ", len(xPos), "robots")
# collisionBuffer = 3
angStep = 0.5
collisionBuffer = 3
downsample = int(numpy.floor(3 / angStep))
for seed in range(100):
rg = RobotGrid(stepSize=angStep, epsilon=epsilon, seed=seed)
for robotID, (x, y) in enumerate(zip(xPos, yPos)):
rg.addRobot(robotID, str(robotID), [x, y, 0], hasApogee)
rg.setCollisionBuffer(collisionBuffer)
rg.initGrid()
for rID in rg.robotDict:
robot = rg.getRobot(rID)
robot.setXYUniform()
assert rg.getNCollisions() > 10
rg.decollideGrid()
for robot in rg.robotDict.values():
robot.setDestinationAlphaBeta(90, 180)
assert rg.getNCollisions() == 0
rg.pathGenMDP(greed, phobia)
deadlockedRobots = []
for r in rg.robotDict.values():
# if not r.onTargetVec[-1]:
if r.score() > 0:
deadlockedRobots.append(r.id)
if len(deadlockedRobots):
print("seed", seed, "failed with these", deadlockedRobots, "in ",
rg.nSteps)
break
else:
print("seed", seed, "didn't fail", rg.nSteps, " taken to solve")
if plot:
utils.plotPaths(rg,
downsample=downsample,
filename="reverseSetMDP.mp4")
def test_pathGen(plot=False):
xPos, yPos = utils.hexFromDia(15, pitch=22.4)
print("using", len(xPos), "robots")
seed = 2
smoothPts = 5
collisionShrink = 0.03
angStep = 0.1
collisionBuffer = 2
# epsilon = angStep * 2
downsample = int(numpy.floor(3 / angStep))
rg = RobotGrid(angStep, seed=seed)
for robotID, (x, y) in enumerate(zip(xPos, yPos)):
rg.addRobot(robotID, str(robotID), [x, y, 0], hasApogee)
rg.robotDict[robotID].setDestinationAlphaBeta(0, 180)
rg.setCollisionBuffer(collisionBuffer)
rg.initGrid()
for rID in rg.robotDict:
robot = rg.getRobot(rID)
robot.setXYUniform()
assert rg.getNCollisions() > 10
if plot:
utils.plotOne(0, rg, figname="pathGenInitial.png", isSequence=False)
rg.decollideGrid()
assert rg.getNCollisions() == 0
if plot:
utils.plotOne(0, rg, figname="pathGenDecollided.png", isSequence=False)
rg.pathGenGreedy()
# print("rg alg type", rg.algType, type(rg.algType), str(rg.algType))
# sd = rg.robotGridSummaryDict()
# for d in sd["robotDict"].values():
# print(d["id"], d["alpha"], d["beta"])
# print("deadlocks", rg.deadlockedRobots())
assert not rg.didFail
rg.smoothPaths(smoothPts)
rg.simplifyPaths()
rg.verifySmoothed()
assert rg.smoothCollisions > 100
print(rg.smoothCollisions)
rg.shrinkCollisionBuffer(collisionShrink)
rg.verifySmoothed()
# assert rg.smoothCollisions == 0
print(rg.smoothCollisions)
if plot:
utils.plotOne(0, rg, figname="donepaht.png", isSequence=False)
utils.plotPaths(rg, downsample=downsample, filename="pathGen.mp4")
def test_me(plot=False):
angStep = 0.1
downsample = int(numpy.floor(10 / angStep))
seed = 4
rg = RobotGridNominal(stepSize=angStep, seed=seed)
rg.setCollisionBuffer(2)
for r in rg.robotDict.values():
r.setXYUniform()
r.setDestinationAlphaBeta(0, 180)
rg.decollideGrid()
rg.pathGenMDP(0.8, 0.2)
deadlockedRobots = []
for r in rg.robotDict.values():
# if not r.onTargetVec[-1]:
if r.score() > 0:
deadlockedRobots.append(r.id)
if plot:
utils.plotPaths(rg, downsample=downsample, filename="update.mp4")
xs = []
ys = []
types = []
for r in rg.robotDict.values():
xs.append(r.basePos[0])
ys.append(r.basePos[1])
if r.hasApogee:
types.append("BA")
else:
types.append("BOSS")
for f in rg.fiducialDict.values():
xs.append(f.xyzWok[0])
ys.append(f.xyzWok[1])
types.append("Fiducial")
sns.scatterplot(x=xs, y=ys, hue=types)
plt.savefig("scatterAPO.png")
if len(deadlockedRobots):
print("seed", seed, "failed with these", deadlockedRobots, "in ",
rg.nSteps)
assert False
def test_explode(plot=False):
rg = RobotGridCalib(seed=350)
rg.setCollisionBuffer(2.5)
for robot in rg.robotDict.values():
robot.setXYUniform()
robot.setDestinationAlphaBeta(0, 180)
rg.decollideGrid()
assert rg.getNCollisions() == 0
tstart = time.time()
rg.pathGenExplode(100)
print("pathgen took", time.time()-tstart)
if plot:
utils.plotPaths(rg, filename="explode.mp4")
def test_offlineRobots(plot=False):
# xPos, yPos = utils.hexFromDia(35, pitch=22.4)
seed = 6
rg = RobotGridCalib(
stepSize=angStep,
epsilon=epsilon, seed=seed
)
rg.setCollisionBuffer(collisionBuffer)
for rID in rg.robotDict:
robot = rg.getRobot(rID)
if rID == 1255:
robot.setAlphaBeta(305, 238.29)
robot.isOffline = True
elif rID == 717:
robot.setAlphaBeta(0,180)
robot.isOffline = True
elif rID == 1367:
robot.setAlphaBeta(0, 164.88)
robot.isOffline = True
elif rID == 398:
robot.setAlphaBeta(0, 152.45)
robot.isOffline = True
else:
robot.setXYUniform()
rg.decollideGrid()
# for robot in rg.robotDict.values():
# if numpy.random.uniform() > 0.998:
# robot.isOffline = True
for robot in rg.robotDict.values():
robot.setDestinationAlphaBeta(0, 180)
assert rg.getNCollisions() == 0
rg.pathGenMDP(0.8, 0.2)
print("didfai", rg.didFail)
if plot:
utils.plotPaths(rg, downsample=int(numpy.floor(10 / angStep)), filename="offline.mp4")
def test_hexDeadlockedPath(plot=False):
xPos, yPos = utils.hexFromDia(35, pitch=22.4)
seed = 3
cb = 2.5
angStep = 1
downsample = int(numpy.floor(3 / angStep))
rg = RobotGrid(angStep, seed=seed)
for robotID, (x, y) in enumerate(zip(xPos, yPos)):
rg.addRobot(robotID, str(robotID), [x, y, 0], hasApogee)
rg.robotDict[robotID].setDestinationAlphaBeta(0, 180)
rg.setCollisionBuffer(cb)
rg.initGrid()
for rID in rg.robotDict:
robot = rg.getRobot(rID)
robot.setXYUniform()
assert rg.getNCollisions() > 10
rg.decollideGrid()
assert rg.getNCollisions() == 0
rg.pathGenGreedy()
assert rg.didFail
if plot:
utils.plotPaths(rg,
downsample=downsample,
filename="test_hexDeadlockedPath.mp4")