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_collide(plot=False):
# should make a grid
rg = RobotGridNominal()
collidedRobotIDs = []
for rid, r in rg.robotDict.items():
if r.holeID == "R-13C1":
r.setAlphaBeta(90, 0)
collidedRobotIDs.append(rid)
elif r.holeID == "R-13C2":
collidedRobotIDs.append(rid)
r.setAlphaBeta(270, 0)
else:
r.setAlphaBeta(90, 180)
for rid in collidedRobotIDs:
assert rg.isCollided(rid)
assert rg.getNCollisions() == 2
if plot:
utils.plotOne(0,
rg,
figname="test_collide.png",
isSequence=False,
xlim=[-30, 30],
ylim=[-30, 30])
def test_gfaCollision(plot=False):
rg = RobotGridCalib()
for alphaAng in numpy.linspace(0, 360, 100): #[0, 60, 120, 180, 240, 300, 360]:
for r in rg.robotDict.values():
r.setAlphaBeta(alphaAng, 0)
# assert rg.getNCollisions() == 6
if plot:
plotOne(0, rg, "gfa%i_collide.png"%alphaAng, False)
rg.decollideGrid()
assert rg.getNCollisions() == 0
if plot:
plotOne(0, rg, "gfa%i_decollide.png"%alphaAng, False)
def plot_state(
self, highlightRobot=None, plotRobotIDs=True,
returnax=True, figname="kaijuGrid.pdf"
):
"""Plot the current state of the robot grid
Unassigned robots are skyblue
Robots at their destination are gold
Robots that are kaiju-collided are red
Offline robots are black
Parameters
------------
highlightRobot : int, list, or None
Robot id to highlight in Orange
plotRobotIDs : bool
If True, plot the robotIDs
returnax : bool
If True, return the matplotlib axis (no figure is written to disk)
figname : str
If returnax is False, write the image to disk with this path.
Defaults to kaijuGrid.png.
Returns
---------
ax : matplotlib axes instance if returnax is True
"""
from kaiju.utils import plotOne
if returnax:
ax = plotOne(
step=0, robotGrid=self, isSequence=False,
plotRobotIDs=plotRobotIDs,
highlightRobot=highlightRobot, returnax=True
)
return ax
else:
plotOne(
step=0, robotGrid=self, figname=figname, isSequence=False,
plotRobotIDs=plotRobotIDs,
highlightRobot=highlightRobot
)
def test_targetAssign(plot=False):
# rg = utils.robotGridFromFilledHex()
print("test target assign")
rg = RobotGridNominal()
for robot in rg.robotDict.values():
robot.setXYUniform()
rg.decollideGrid()
if plot:
utils.plotOne(0, rg, figname="beforeAssign.png",
isSequence=False) #, highlightRobot=205)
# find boss target positions for all robots
rids = []
for rID, robot in rg.robotDict.items():
rids.append(rID)
xyzTarg = robot.bossWokXYZ
# give the target the same id as the robot
rg.addTarget(rID, xyzTarg, BossFiber)
targ = rg.targetDict[rID]
assert rID in targ.validRobotIDs
assert rID in robot.validTargetIDs
rg.assignRobot2Target(robotID=rID, targID=targ.id)
unreachableTargets = rg.unreachableTargets()
assert len(unreachableTargets) == 0
targetlessRobots = rg.targetlessRobots()
assert len(targetlessRobots) == 0
# unassign target 100
rg.unassignTarget(rids[0])
assert rg.robotDict[rids[0]].isAssigned() == False
assert rg.targetDict[rids[0]].isAssigned() == False
rg.unassignTarget(rids[1])
assert rg.robotDict[rids[1]].isAssigned() == False
assert rg.targetDict[rids[1]].isAssigned() == False
assignedTargets = rg.assignedTargets()
assert (rids[1] in assignedTargets) == False
assert (rids[0] in assignedTargets) == False
if plot:
utils.plotOne(0, rg, figname="afterAssign.png", isSequence=False)
def test_tofile(plot=False):
xPos, yPos = utils.hexFromDia(37, pitch=22.4, rotAngle=90)
print("n robots", len(xPos))
angStep = 1
greed = 0.8
phobia = 0.2
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()
assert rg.getNCollisions() > 10
rg.decollideGrid()
for robot in rg.robotDict.values():
robot.setDestinationAlphaBeta(10, 170)
rg.pathGenMDP(greed, phobia)
# rg.smoothPaths(3)
# rg.simplifyPaths()
# rg.verifySmoothed()
# t1 = time.time()
# rg.summaryJSON("json.txt")
# print("json took", (time.time()-t1))
# t1 = time.time()
# rg.summaryPickle("rg.pkl")
# print("pickle took", (time.time()-t1))
# t1 = time.time()
# g = json.load(open("json.txt", "r"))
# print("json load took", (time.time()-t1))
# t1 = time.time()
# g = pickle.load(open("rg.pkl", "rb"))
# print("pickle load took", (time.time()-t1))
if plot:
utils.plotOne(-1, rg, figname="tofile.png", isSequence=False)
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_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 doOne(inputList, saveDir, plot=False):
seed, nDia, angStep, cbuff, (greed, phobia) = inputList
outList = []
basename = "%i_%i_%.2f_%.2f_%.2f_%.2f" % (seed, nDia, angStep, cbuff,
greed, phobia)
filename = "summary_" + basename + ".json"
errname = "error_" + basename + ".err"
errpath = os.path.join(saveDir, errname)
filepath = os.path.join(saveDir, filename)
# if os.path.exists(filepath):
# print("skipping, already done")
# return
try:
rg = getGrid(angStep, cbuff, seed, nDia)
except:
with open(errpath, "a") as f:
f.write("decollide failed\n")
return
# totalReplaced = 0
replacementIDList = []
# record initial alpha beta positions for all robots
rbInit = {}
for rID, robot in rg.robotDict.items():
rbInit[rID] = [robot.alpha, robot.beta]
for i in range(maxReplacements):
# get a fresh grid each time
totalReplaced = len(set(replacementIDList))
if totalReplaced > rg.nRobots:
rg.totalReplaced = rg.nRobots
# don't start a new grid
# don't try further
# break here
outList.append(rg.robotGridSummaryDict())
break
rg = getGrid(angStep, cbuff, i, nDia)
rg.totalReplaced = totalReplaced
# initialize robots to starting place
for rID, robot in rg.robotDict.items():
initAlpha, initBeta = rbInit[rID]
robot.setAlphaBeta(initAlpha, initBeta)
# print("nCollisions at loop top", rg.getNCollisions())
if plot:
figname = os.path.join(saveDir, basename + "_%i_" % i)
utils.plotOne(1,
robotGrid=rg,
figname=figname + "_s.png",
isSequence=False)
tstart = time.time()
if greed == 1 and phobia == 0:
rg.pathGenGreedy()
# elif greed == -1 and phobia == -1:
# rg.pathGen()
else:
rg.pathGenMDP(greed, phobia)
tend = time.time()
if plot:
utils.plotOne(1,
robotGrid=rg,
figname=figname + "_e.png",
isSequence=False)
rg.runtime = tend - tstart
outList.append(rg.robotGridSummaryDict())
if not rg.didFail:
break # done!
# deadlockedRobots = numpy.array(rg.deadlockedRobots())
# numpy.random.shuffle(deadlockedRobots)
replacementIDs = findReplacementIDs(rg)
# print("deadlockedRobots", deadlockedRobots)
# if we're here pathGen failed. try to replace a deadlocked
# robot
for rID, robot in rg.robotDict.items():
initAlpha, initBeta = rbInit[rID]
robot.setAlphaBeta(initAlpha, initBeta)
# print("nCollisions after deadlock", rg.getNCollisions())
for rID in replacementIDs:
# throwAway will update the grid with new positions
# if successful. returns true/false to indicate success
robot = rg.robotDict[rID]
foundNewSpot = rg.throwAway(rID)
if foundNewSpot == False:
print("replacement failed")
with open(errpath, "a") as f:
f.write(
"failed to find replacement for robot %i replacement iter %i\n"
% (rID, i))
else:
# indicate this robots new spot in the init dict
replacementIDList.append(rID)
rbInit[rID] = [robot.alpha, robot.beta]
print("replacing robot", rID, "to", [robot.alpha, robot.beta])
# totalReplaced += 1
if plot:
# save last state
with open(os.path.join(saveDir, basename + ".pkl"), "wb") as f:
dout = {}
seed, nDia, angStep, cbuff, (greed, phobia)
dout["seed"] = seed
dout["nDia"] = nDia
dout["angStep"] = angStep
dout["cbuff"] = cbuff
dout["greed"] = greed
dout["phobia"] = phobia
dout["rbInit"] = rbInit
pickle.dump(dout, f)
with open(filepath, "w") as f:
json.dump(outList, f, separators=(',', ':'))
