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 doOne(seed, flip90=False):
rg = RobotGrid(angStep, collisionBuffer, 2.2, seed)
xPos, yPos = utils.hexFromDia(nDia, pitch=22.4)
for ii, (xp, yp) in enumerate(zip(xPos, yPos)):
if flip90:
xrot = cos * xp + sin * yp
yrot = sin * xp - cos * yp
rg.addRobot(ii, xrot, yrot, hasApogee)
else:
rg.addRobot(ii, xp, yp, hasApogee)
rg.initGrid()
for ii in range(rg.nRobots):
r = rg.getRobot(ii)
r.setXYUniform() # r.setXYUniform can give nan values for alpha beta?
# set all positioners randomly (they are initialized at 0,0)
rg.decollide2()
rg.pathGen() # calling decollide twice breaks things?
robotsFolded = 0
for r in rg.allRobots:
if r.alpha == 0 and r.beta == 180:
robotsFolded += 1
# if rg.didFail:
# filename = "seed_%i"%seed
# if flip90:
# filename += "_flipped"
# else:
# filename += "_notflipped"
# utils.plotOne(1, rg, figname=filename+".png", isSequence=False, internalBuffer=collisionBuffer)
return robotsFolded
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 getGrid(angStep, cbuff, seed, nDia):
rg = RobotGrid(stepSize=angStep,
collisionBuffer=cbuff,
epsilon=angStep * 2,
seed=seed)
xPos, yPos = xPos, yPos = utils.hexFromDia(nDia, pitch=22.4, rotAngle=90)
for robotID, (x, y) in enumerate(zip(xPos, yPos)):
rg.addRobot(robotID, x, y, hasApogee)
rg.initGrid()
for robot in rg.robotDict.values():
robot.setXYUniform()
robot.setDestinationAlphaBeta(10, 170)
rg.decollideGrid()
# print("nCollisions in getGrid", rg.getNCollisions())
return rg
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_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_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")
matplotlib.use('Agg')
stepSize = 1 # degrees
nDia = 15
# r_phys = 2
b_step = 0.02
b_smooth = 0.04
epsilon = stepSize * 2.2
internalBuffer = 1.5
collisionBuffer = 0.25
rg = cKaiju.RobotGrid(stepSize, epsilon, 0)
rg.setCollisionBuffer(collisionBuffer)
xPos, yPos = utils.hexFromDia(nDia, pitch=22.4)
for ii, (xp, yp) in enumerate(zip(xPos, yPos)):
rg.addRobot(ii, xp, yp)
rg.initGrid()
rg.decollide()
rg.pathGen()
steps = range(rg.nSteps)
def plotOne(step):
plt.figure(figsize=(10, 10))
ax = plt.gca()
for robot in rg.allRobots:
alphaX = robot.roughAlphaX[step][1]
alphaY = robot.roughAlphaY[step][1]
def generatePath(seed=0, plot=False):
nDia = 3
pitch = 22.4
hasApogee = True
rg = RobotGrid(angStep, collisionBuffer, epsilon, seed)
xPos, yPos = utils.hexFromDia(nDia, pitch=pitch)
for ii, (xp, yp) in enumerate(zip(xPos, yPos)):
rg.addRobot(ii, xp, yp, hasApogee)
rg.initGrid()
for ii in range(rg.nRobots):
r = rg.getRobot(ii)
r.setXYUniform()
# set all positioners randomly (they are initialized at 0,0)
rg.decollide2()
rg.pathGen()
if rg.didFail:
print("path gen failed")
raise (RuntimeError, "path gen failed")
rg.smoothPaths(smoothPts)
rg.simplifyPaths()
rg.setCollisionBuffer(collisionBuffer - collisionShrink)
rg.verifySmoothed()
if rg.smoothCollisions:
print("smoothing failed with ", rg.smoothCollisions)
raise (RuntimeError, "smoothing failed")
# find the positioner with the most interpolated steps
useRobot = None
maxSteps = 0
for i, r in enumerate(rg.allRobots):
m = len(r.simplifiedBetaPath) # beta path is usually more complicated
if m > maxSteps:
maxSteps = m
useRobot = r
if plot:
plotTraj(useRobot, "seed_%i_" % seed, dpi=250)
# bp = numpy.array(useRobot.betaPath)
# sbp = numpy.array(useRobot.interpSmoothBetaPath)
ibp = numpy.array(useRobot.simplifiedBetaPath)
# ap = numpy.array(useRobot.alphaPath)
# sap = numpy.array(useRobot.interpSmoothAlphaPath)
iap = numpy.array(useRobot.simplifiedAlphaPath)
# generate kaiju trajectory (for robot 23)
# time = angStep * stepNum / speed
alphaTimesR = iap[:, 0] * angStep / RobotMaxSpeed
alphaDegR = iap[:, 1]
betaTimesR = ibp[:, 0] * angStep / RobotMaxSpeed
betaDegR = ibp[:, 1]
armPathR = {} # reverse path
armPathR["alpha"] = [(pos, time)
for pos, time in zip(alphaDegR, alphaTimesR)]
armPathR["beta"] = [(pos, time) for pos, time in zip(betaDegR, betaTimesR)]
reversePath = {robotID: armPathR}
# build forward path
alphaTimesF = numpy.abs(alphaTimesR - alphaTimesR[-1])[::-1]
alphaDegF = alphaDegR[::-1]
betaTimesF = numpy.abs(betaTimesR - betaTimesR[-1])[::-1]
betaDegF = betaDegR[::-1]
armPathF = {} # reverse path
armPathF["alpha"] = [(pos, time)
for pos, time in zip(alphaDegF, alphaTimesF)]
armPathF["beta"] = [(pos, time) for pos, time in zip(betaDegF, betaTimesF)]
forwardPath = {robotID: armPathF}
return forwardPath, reversePath, maxSteps
from multiprocessing import Pool, cpu_count
from functools import partial
import itertools
import numpy
from runSim import doOne, compileResults
import pandas as pd
import os
import numpy
from kaiju import utils
nDias = numpy.arange(7, 67, 4)
roboMap = {}
for nDia in nDias:
xPos, yPos = xPos, yPos = utils.hexFromDia(nDia, pitch=22.4, rotAngle=90)
roboMap[len(xPos)] = nDia
if __name__ == "__main__":
saveDir = "/home/csayres/kaijuRunVarNum"
nProcs = 18
seeds = range(0, 200)
cbuff = [2.5]
angStep = [0.1]
greedPhob = [(0.9, 0.3), (1, 0)]
# nDias = numpy.arange(7,67,4)
doOnePartial = partial(doOne, saveDir=saveDir)
gridIter = itertools.product(seeds, nDias, angStep, cbuff, greedPhob)
import time
from multiprocessing import Pool, cpu_count
import pickle
import numpy
import matplotlib.pyplot as plt
from keras.models import Sequential, load_model
from keras.layers import Dense
from kaiju import RobotGrid, utils
nHexDia = 7
xCoords, yCoords = utils.hexFromDia(nHexDia)
nPositioners = len(xCoords)
print("using %i positioners" % nPositioners)
nTargets = nPositioners
nProcs = 10 # processors to use for multiprocessing
batchSize = 100 # The number of samples to run through the network before the weights / gradient are updated
epochs = 5 # The number of times to iterate through the complete sample of training data
trainingRatio = 0.9
def getValidAssignments(seed):
"""seed is the random seed with which to initialze the RobotGrid
return dictionary keyed by positioner id with the coordinates of the
metrology fiber. These represent valid (non-collided) xy Fiber positions
for each robot
"""
rg = RobotGrid(seed=seed)
for ii, (xp, yp) in enumerate(zip(xCoords, yCoords)):