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")