def test_nonInit():
print("test nonInit")
rg = RobotGrid()
rxyz = [0, 0, 0]
rg.addRobot(robotID=1, holeID="none", basePos=rxyz)
fxyz = [22.4, 0, fpZ]
rg.addFiducial(fiducialID=1, xyzWok=fxyz)
with pytest.raises(RuntimeError) as excinfo:
txyz = [0, 10, fpZ]
rg.addTarget(targetID=1, xyzWok=txyz, fiberType=BossFiber, priority=1)
assert "Initialize RobotGrid before adding targets" in str(excinfo.value)
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_doubleFiducialID():
# should raise runtime error when two fiducials have same id
rg = RobotGrid(angStep, epsilon, seed)
fiducialID = 1
rg.addFiducial(fiducialID, [0, 0, coordio.defaults.POSITIONER_HEIGHT])
with pytest.raises(RuntimeError) as excinfo:
rg.addFiducial(fiducialID,
[100, 0, coordio.defaults.POSITIONER_HEIGHT])
assert "Fiducial ID already exists" in str(excinfo.value)
def test_doubleRobotID():
# should raise runtime error when two robots have same id
rg = RobotGrid(angStep, epsilon, seed)
robotID = 1
rg.addRobot(robotID,
str(robotID), [0, 0, 0],
hasApogee,
collisionBuffer=collisionBuffer)
with pytest.raises(RuntimeError) as excinfo:
rg.addRobot(robotID, str(robotID), [30, 0, 0], hasApogee)
assert "Robot ID already exists" in str(excinfo.value)
def test_hexGrid():
xPos, yPos = utils.hexFromDia(15, pitch=22.4)
rg = RobotGrid(angStep, epsilon, seed)
for robotID, (x, y) in enumerate(zip(xPos, yPos)):
basePos = [x, y, 0]
rg.addRobot(robotID,
str(robotID),
basePos,
hasApogee,
collisionBuffer=collisionBuffer)
rg.initGrid()
def test_addTarget():
print("test addTarget")
rg = RobotGrid()
rg.addRobot(robotID=1, holeID="none", basePos=[0, 0, 0])
rg.addFiducial(fiducialID=1, xyzWok=[22.4, 0, fpZ])
rg.initGrid()
rg.addTarget(targetID=1,
xyzWok=[0, 10, fpZ],
fiberType=BossFiber,
priority=1)
rg.addTarget(2, [10, 10, fpZ], ApogeeFiber)
rg.addTarget(3, [-20, -40, fpZ], MetrologyFiber)
assert True
def test_doubleTargetID():
print("test doubleTargetID")
rg = RobotGrid()
rg.addRobot(robotID=1, holeID="none", basePos=[0, 0, 0])
rg.addFiducial(fiducialID=1, xyzWok=[22.4, 0, fpZ])
rg.initGrid()
rg.addTarget(targetID=1,
xyzWok=[0, 10, fpZ],
fiberType=BossFiber,
priority=1)
with pytest.raises(RuntimeError) as excinfo:
rg.addTarget(targetID=1,
xyzWok=[0, 10, fpZ],
fiberType=BossFiber,
priority=1)
assert "Target ID already exists" in str(excinfo.value)
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_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")