def uav_plan(t, results, UAV_loc, X1, Y1, X2, Y2):
# infer s,g,rrt | data
# print " inferring..."
global model
Q = q.Q(model)
# we haven't seen the intruder up to this point. if we had,
# the episode would have ended.
Q.condition(name="data", value=a2d([False] * (t + 1)))
model.cur_t = t
model.intersections = results['intersections']
Q.analyze()
results, scores = Q.opt_adam(alpha=0.01, itercnt=20, rolloutcnt=10)
# sample a bunch of RRTs from the variational distribution
# print " sampling..."
set_of_rrts = []
q_samples = []
for i in range(20):
# print " sample %d" % i
rval = Q.run_model()
set_of_rrts.append(rval[2]) # start_loc, goal_loc, rrt_path
q_samples.append(rval)
# construct a heat cube
# print " planning..."
pts = []
for k in set_of_rrts:
if t < len(k):
pts.append(k[t])
heatmap = make_heatmap(pts, ss=100)
# plan to go to the region of highest probability
goal_loc = np.unravel_index(np.argmax(heatmap),
heatmap.shape) # XXX row/column madness?
uav_plan_t = run_rrt(a2d(UAV_loc), a2d(rrt_loc(goal_loc)), X1, Y1, X2, Y2)
return uav_plan_t, q_samples, Q
from shaders import *
from my_rrt import *
from local import *
import os,sys,inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(currentdir)
sys.path.insert(0,parentdir)
import q
RRT_LEN = 100
RRT_CNT = 100
# our approximation object
Q = q.Q()
Q.always_sample = True
# ========================================================
# ========================================================
# ========================================================
def S_mog_grid( Q, sz, name=None ):
w = Q.rand( sz[0], sz[1], name=name+'_w' )
w = w/np.sum( w )
inds = np.reshape( range(0,np.prod(sz)), sz )
mc = Q.choice( inds.ravel(), p=w.ravel(), name=name+'_mc' )
X,Y = np.meshgrid( range(0,sz[0]), range(0,sz[1]) )
Xc = float( X.ravel()[ mc ] ) / float( sz[0] )
import os,sys,inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(currentdir)
sys.path.insert(0,parentdir)
import q
import matplotlib.pyplot as plt
import full_world
from my_rrt import *
bdata = load_polygons( "./paths.txt" )
model = full_world.Fullworld()
Q = q.Q( model )
Q.condition( name="data", value=True ) # condition on the fact that we DID see the intruder
Q.analyze()
#results, scores = Q.opt_adam( itercnt=1000, rolloutcnt=10 )
#results, scores = Q.opt_sgd( alpha=0.001, itercnt=1000, rolloutcnt=10 )
#results, scores = Q.opt_adam( alpha=0.001, itercnt=300, rolloutcnt=10 )
#results, scores = Q.opt_adam( alpha=0.001, itercnt=300, rolloutcnt=3 )
results, scores = Q.opt_adam( alpha=0.0025, itercnt=100, rolloutcnt=10 )
start_loc,goal_loc,isfnd,rrt_path,ints = Q.run_model()
if isfnd:
color = 'r'
else: