def grow(plot=False):
angStep = 1
collisionBuffer = 2
fiducialCollisionBuffer = 1.5
epsilon = angStep * 2
hasApogee = True
robotID = 1
fiducialID = 10
seed = 0
for cb in [1.5, 2, 2.5, 3]:
rg = RobotGrid(angStep, epsilon, seed)
rg.setCollisionBuffer(cb)
rg.addRobot(robotID, str(robotID), [0, 0, 0], hasApogee)
rg.addFiducial(fiducialID,
[22.4, 0, coordio.defaults.POSITIONER_HEIGHT],
fiducialCollisionBuffer)
rg.initGrid()
robot = rg.getRobot(robotID)
robot.setAlphaBeta(90, 0)
if plot:
utils.plotOne(0,
rg,
figname="grow_%.2f.png" % cb,
isSequence=False,
xlim=[-30, 30],
ylim=[-30, 30])
def test_fiducial(plot=False):
angStep = 1
collisionBuffer = 2
fiducialCollisionBuffer = 1.5
epsilon = angStep * 2
hasApogee = True
robotID = 1
fiducialID = 10
seed = 0
rg = RobotGrid(angStep, epsilon, seed)
rg.setCollisionBuffer(collisionBuffer)
rg.addRobot(robotID, str(robotID), [0, 0, 0], hasApogee)
rg.addFiducial(fiducialID, [22.4, 0, coordio.defaults.POSITIONER_HEIGHT],
fiducialCollisionBuffer)
rg.initGrid()
robot = rg.getRobot(robotID)
for betaAng in range(20):
robot.setAlphaBeta(90, betaAng)
rColliders = rg.robotColliders(robotID)
fColliders = rg.fiducialColliders(robotID)
if plot:
utils.plotOne(0,
rg,
figname="fiducial_%i.png" % betaAng,
isSequence=False,
xlim=[-30, 30],
ylim=[-30, 30])
def test_uniqueFiducial():
angStep = 1
collisionBuffer = 2
epsilon = angStep * 2
hasApogee = True
fiducialID = 1
seed = 0
rg = RobotGrid(angStep, epsilon, seed)
rg.setCollisionBuffer(collisionBuffer)
rg.addFiducial(fiducialID, [0, 0, 0])
with pytest.raises(RuntimeError) as excinfo:
rg.addFiducial(fiducialID, [30, 0, 0])
assert "Fiducial ID already exists" in str(excinfo.value)
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_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")