Exemple #1
0
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")
Exemple #2
0
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")
Exemple #3
0
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")
Exemple #4
0
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")
Exemple #5
0
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
Exemple #6
0
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()
Exemple #7
0
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
Exemple #8
0
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")
Exemple #9
0
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")
Exemple #10
0
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")
Exemple #11
0
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)
Exemple #12
0
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")
Exemple #13
0
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]
Exemple #14
0
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
Exemple #15
0
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)
Exemple #16
0
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)):