invalidCollisionsEdges(prm)
# --- INIT PRM ---
# --- INIT PRM ---
# --- INIT PRM ---
#acado.debug()
if INIT_PRM:
print 'Init PRM Sample'
config(acado, 'connect')
prm(30, 10, 10, True)
print 'Connexify'
prm.connexifyPrm(NTRIAL=200, VERBOSE=True)
print 'Densify'
config(acado, 'traj')
prm.densifyPrm(500, VERBOSE=2)
#for i0,i1 in [ k for k,X in graph.states.items() if np.any(X>7) or np.any(X<-7) ]: graph.removeEdge(i0,i1)
prm.graph.save(dataRootPath + '/prm30-200-500')
else:
pass
#prm.graph.load(dataRootPath+'/nocoll')
#if env.sphericalObstacle: invalidCollisions(prm)
toto
prunePrm(prm)
config(acado, 'traj')
acado.iter = 80
acado.guessbak = acado.guess
acado.guess = nnguess
prm.nearestNeighborbak = prm.nearestNeighbor
cstar)
prm.graph.removeEdge(i0, i1)
# --- INIT PRM ---
# --- INIT PRM ---
# --- INIT PRM ---
#acado.debug()
if INIT_PRM:
print 'Init PRM Sample'
prm(30, 10, 10, True)
print 'Connexify'
prm.connexifyPrm(NTRIAL=100, VERBOSE=True)
print 'Densify'
config(acado, 'traj')
prm.densifyPrm(1000, VERBOSE=2)
prm.graph.save(dataRootPath + '/prm60-200-2000')
else:
#prm.graph.load(dataRootPath+'/prm30-100-1000')
prm.graph.load(dataRootPath + '/prm60-200-2000')
prunePrm(prm)
config(acado, 'traj')
acado.iter = 80
acado.guessbak = acado.guess
acado.guess = nnguess
prm.nearestNeighborbak = prm.nearestNeighbor
prm.nearestNeighbor = nnnear
# --- IREPA LOOP
# --- IREPA LOOP