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")
for ii, (xp, yp) in enumerate(zip(xPos, yPos)):
xrot = cos * xp + sin * yp
yrot = sin * xp - cos * yp
# print("%.8f, %.8f"%(xrot,yrot))
rg.addRobot(ii, xrot, yrot, 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()
rg.smoothPaths(smoothPts) # must be odd
rg.simplifyPaths()
rg.setCollisionBuffer(collisionBuffer - collisionShrink)
rg.verifySmoothed()
# find max beta path points
# only plot if we've found minimum
# amount of points
maxPts = 0
for r in rg.allRobots:
nPts = len(r.simplifiedBetaPath)
if nPts > maxPts:
maxPts = nPts
print("maxPts", maxPts)
if maxPts < minPoints:
continue
for r in rg.allRobots:
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
def generatePath(seed=0, plot=False, movie=False):
nDia = 3
pitch = 22.4
hasApogee = True
rg = RobotGrid(angStep, collisionBuffer, epsilon, seed)
# epfl grid is sideways
# xPos, yPos = utils.hexFromDia(nDia, pitch=pitch)
# for ii, (xp, yp), posID in enumerate(zip(xPos, yPos, posList)):
# rg.addRobot(posID, xp, yp, hasApogee)
# rg.initGrid()
for posID, (xp, yp) in posDict.items():
# print("loading pos %i [%.2f, %.2f]"%(posID, xp, yp))
rg.addRobot(posID, xp, yp, hasApogee)
rg.initGrid()
for ii in range(rg.nRobots):
r = rg.getRobot(ii)
# print("loading pos %i [%.2f, %.2f]"%(r.id, r.xPos, r.yPos))
# print("robotID", r.id)
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")
if movie:
plotMovie(rg, filename="movie_%i" % seed)
# find the positioner with the most interpolated steps
forwardPath = {}
reversePath = {}
for robotID, r in zip(posDict.keys(), rg.allRobots):
assert robotID == r.id
if plot:
plotTraj(r, "seed_%i_" % seed, dpi=250)
# bp = numpy.array(r.betaPath)
# sbp = numpy.array(r.interpSmoothBetaPath)
ibp = numpy.array(r.simplifiedBetaPath)
# ap = numpy.array(r.alphaPath)
# sap = numpy.array(r.interpSmoothAlphaPath)
iap = numpy.array(r.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
async def main():
# Set logging level to DEBUG
#log.set_level(20)
# Initialise the FPS instance.
fps = FPS(layout="grid7.txt")
await fps.initialise()
hasApogee = True
seed = 0 # ignored we're setting ppositions manually
rg = RobotGrid(angStep, collisionBuffer, epsilon, seed)
# epfl grid is sideways
# xPos, yPos = utils.hexFromDia(nDia, pitch=pitch)
# for ii, (xp, yp), posID in enumerate(zip(xPos, yPos, posList)):
# rg.addRobot(posID, xp, yp, hasApogee)
# rg.initGrid()
for posID, (xp, yp) in posDict.items():
# print("loading pos %i [%.2f, %.2f]"%(posID, xp, yp))
rg.addRobot(posID, xp, yp, hasApogee)
rg.initGrid()
#print("fps.positioners", fps.positioners)
for ii in range(rg.nRobots):
r = rg.getRobot(ii)
# print("loading pos %i [%.2f, %.2f]"%(r.id, r.xPos, r.yPos))
# print("robotID", r.id)
await fps.positioners[r.id].update_position()
alpha, beta = fps.positioners[r.id].position
print("IIII", r.id, alpha, beta)
r.setAlphaBeta(alpha, beta)
# 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
reversePath = {}
for robotID, r in zip(posDict.keys(), rg.allRobots):
assert robotID == r.id
# bp = numpy.array(r.betaPath)
# sbp = numpy.array(r.interpSmoothBetaPath)
ibp = numpy.array(r.simplifiedBetaPath)
# ap = numpy.array(r.alphaPath)
# sap = numpy.array(r.interpSmoothAlphaPath)
iap = numpy.array(r.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
await fps.send_trajectory(reversePath)
await fps.shutdown()