def create_RRT(poly_map, isovist, locs, name="runner_plan_naive"):
rx1, ry1, rx2, ry2 = poly_map
path = planner.run_rrt_opt(np.atleast_2d(locs[4]), np.atleast_2d(locs[8]),
rx1, ry1, rx2, ry2)
print path
fig, ax = setup_plot(poly_map, locs)
for i in xrange(28):
ax.plot([path[i][0], path[i + 1][0]], [path[i][1], path[i + 1][1]],
'orange',
linestyle=":",
linewidth=1,
label="Agent's Plan")
t = 5
ax.scatter(path[t][0],
path[t][1],
s=95,
facecolors='none',
edgecolors='orange')
rand_id = str(int(time.time()))
close_plot(fig, ax, plot_name="runner_paths/" + name + rand_id + ".eps")
pickle.dump(path, open("runner_paths/" + name + rand_id + ".p", "wb"))
def run_basic(self, Q, path_noise=0.003):
rx1, ry1, rx2, ry2 = self.seg_map
t = Q.choice(p=1.0 / 40 * np.ones((1, 40)), name="t")
start_i = Q.choice(p=1.0 / self.cnt * np.ones((1, self.cnt)),
name="run_start")
goal_i = Q.choice(p=1.0 / self.cnt * np.ones((1, self.cnt)),
name="run_goal")
start = np.atleast_2d(self.locs[start_i])
goal = np.atleast_2d(self.locs[goal_i])
# plan using the latent variables of start and goal
my_plan = planner.run_rrt_opt(np.atleast_2d(start),
np.atleast_2d(goal), rx1, ry1, rx2, ry2)
my_loc = np.atleast_2d(my_plan[t])
# add noise to the plan
my_noisy_plan = [my_plan[0]]
for i in xrange(
1,
len(my_plan) - 1
): #loc_t = np.random.multivariate_normal(my_plan[i], [[path_noise, 0], [0, path_noise]]) # name 't_i' i.e. t_1, t_2,...t_n
loc_x = Q.randn(mu=my_plan[i][0],
sigma=path_noise,
name="run_x_" + str(i))
loc_y = Q.randn(mu=my_plan[i][1],
sigma=path_noise,
name="run_y_" + str(i))
loc_t = [loc_x, loc_y]
my_noisy_plan.append(loc_t)
my_noisy_plan.append(my_plan[-1])
Q.keep("runner_plan", my_noisy_plan)
def run_smart(self, Q):
t = Q.choice( p=1.0/40*np.ones((1,40)), name="t" )
# current location of chaser (at time 't')
curr_loc = [Q.get_obs("enf_x_"+str(t)), Q.get_obs("enf_y_"+str(t))]
# randomly choose a start and goal for runner
cnt = len(self.locs)
int_start_i = Q.choice( p=1.0/cnt*np.ones((1,cnt)), name="int_start" )
int_goal_i = Q.choice( p=1.0/cnt*np.ones((1,cnt)), name="int_goal" )
# create empty trace using model
q = p.ProgramTrace(self.runner_model)
# condition q on Q
q = self.condition_q(Q, q)
# run inference to get intruder's expected next step
post_sample_traces = run_inference(q, post_samples=6, samples=5) # 10, 5
runner_exp_next_step, runner_exp_path = rand_expected_future_step(post_sample_traces, "int_plan")
#if point_in_obstacle(runner_exp_next_step, self.epolys):
# runner_exp_next_step = get_clear_goal(curr_loc, runner_exp_next_step, self.polys)
Q.keep("q", q.trace)
Q.keep("runner_post_sample_traces", post_sample_traces)
Q.keep("runner_exp_next_step", runner_exp_next_step)
# plan path toward anticipated location (t+1) for runner
#enf_plan = self.plan_path(np.atleast_2d( curr_loc), np.atleast_2d( runner_exp_next_step))
rx1,ry1,rx2,ry2 = self.seg_map
enf_plan = planner.run_rrt_opt( np.atleast_2d( curr_loc), np.atleast_2d( runner_exp_next_step), rx1,ry1,rx2,ry2)
Q.keep("enf_plan", enf_plan)
# set up the enforcer view (forward vector, fv) for the next step
cur_enf_loc = scale_up(curr_loc)
next_enf_loc = scale_up(enf_plan[1])
next_int_loc = scale_up(runner_exp_path[t+1])
fv = direction(next_int_loc, cur_enf_loc)
intersections = self.isovist.GetIsovistIntersections(next_enf_loc, fv)
# does the enforcer see me at time 't'
#runner_next_loc = scale_up(runner_exp_next_step)
will_runner_be_seen = self.isovist.FindIntruderAtPoint( next_int_loc, intersections )
detected_prob = 0.999*will_runner_be_seen + 0.001*(1-will_runner_be_seen) # ~ flip(seen*.999 + (1-seen*.001)
# XXX should consider seeing if the enforcer will see the intruder
# before reaching its simulated goal location, not just in the next step
# down side: it would need more isovist calculations at each step
runner_detected = Q.flip( p=detected_prob, name="int_detected" )
def run_naive(self, Q):
t = Q.choice( p=1.0/40*np.ones((1,40)), name="t" )
# current location of chaser (at time 't')
curr_loc = [Q.get_obs("enf_x_"+str(t)), Q.get_obs("enf_y_"+str(t))]
# randomly choose a start and goal for runner
cnt = len(self.locs)
int_start_i = Q.choice( p=1.0/cnt*np.ones((1,cnt)), name="int_start" )
int_goal_i = Q.choice( p=1.0/cnt*np.ones((1,cnt)), name="int_goal" )
rx1,ry1,rx2,ry2 = self.seg_map
int_plan = planner.run_rrt_opt( np.atleast_2d(self.locs[int_start_i]),
np.atleast_2d( self.locs[int_goal_i]), rx1,ry1,rx2,ry2, slow=True)
# plan path toward anticipated location (t+1) for runner
#enf_plan = self.plan_path(np.atleast_2d( curr_loc), np.atleast_2d( runner_exp_next_step))
t_ = min(t+12, len(int_plan)-1)
enf_plan = planner.run_rrt_opt( np.atleast_2d( curr_loc), np.atleast_2d( int_plan[t_]), rx1,ry1,rx2,ry2)
Q.keep("enf_plan", enf_plan)
Q.keep("int_plan", int_plan)
# set up the enforcer view (forward vector, fv) for the next step
cur_enf_loc = scale_up(curr_loc)
next_enf_loc = scale_up(enf_plan[1])
fv = direction(next_enf_loc, cur_enf_loc)
intersections = self.isovist.GetIsovistIntersections(next_enf_loc, fv)
# does the enforcer see me at time 't'
runner_next_loc = scale_up(int_plan[t+1])
will_runner_be_seen = self.isovist.FindIntruderAtPoint( int_plan[t+1], intersections )
detected_prob = 0.999*will_runner_be_seen + 0.001*(1-will_runner_be_seen) # ~ flip(seen*.999 + (1-seen*.001)
# XXX should consider seeing if the enforcer will see the intruder
# before reaching its simulated goal location, not just in the next step
# down side: it would need more isovist calculations at each step
runner_detected = Q.flip( p=detected_prob, name="int_detected" )
def tom_run_nested_inference(self, Q, path_noise=0.003):
# represents the other agent
rx1, ry1, rx2, ry2 = self.seg_map
t = Q.choice(p=1.0 / 40 * np.ones((1, 40)), name="t")
start_i = Q.choice(p=1.0 / self.cnt * np.ones((1, self.cnt)),
name="co_run_start")
goal_i = Q.choice(p=1.0 / self.cnt * np.ones((1, self.cnt)),
name="co_run_goal")
start = np.atleast_2d(self.locs[start_i])
goal = np.atleast_2d(self.locs[goal_i])
# plan using the latent variables of start and goal
my_plan = planner.run_rrt_opt(np.atleast_2d(start),
np.atleast_2d(goal), rx1, ry1, rx2, ry2)
my_loc = np.atleast_2d(my_plan[t])
# add noise to the plan
my_noisy_plan = [my_plan[0]]
for i in xrange(
1,
len(my_plan) - 1
): #loc_t = np.random.multivariate_normal(my_plan[i], [[path_noise, 0], [0, path_noise]]) # name 't_i' i.e. t_1, t_2,...t_n
loc_x = Q.randn(mu=my_plan[i][0],
sigma=path_noise,
name="co_run_x_" + str(i))
loc_y = Q.randn(mu=my_plan[i][1],
sigma=path_noise,
name="co_run_y_" + str(i))
loc_t = [loc_x, loc_y]
my_noisy_plan.append(loc_t)
my_noisy_plan.append(my_plan[-1])
post_sample_traces = self.collaborative_nested_inference(Q)
# get most probable goal
partners_goal = self.get_most_probable_goal(post_sample_traces,
"run_goal")
same_goal = 0
if goal_i == partners_goal:
same_goal = 1
same_goal_prob = 0.999 * same_goal + 0.001 * (1 - same_goal)
runners_same_goal = Q.flip(p=same_goal_prob, name="same_goal")
Q.keep("co_runner_plan", my_noisy_plan)
Q.keep("partner_goal", partners_goal)
Q.keep("nested_post_sample_traces", post_sample_traces)
def run(self, Q):
#----------------------------------------------------------
# simplified enforcer model
#----------------------------------------------------------
t = Q.choice(p=1.0 / 40 * np.ones((1, 40)), name="t")
cnt = len(self.locs)
enf_start_i = Q.choice(p=1.0 / cnt * np.ones((1, cnt)),
name="enf_start")
enf_goal_i = Q.choice(p=1.0 / cnt * np.ones((1, cnt)), name="enf_goal")
enf_start = np.atleast_2d(self.locs[enf_start_i])
enf_goal = np.atleast_2d(self.locs[enf_goal_i])
path_noise = .003
#enf_plan = self.plan_path(enf_start, enf_goal)
rx1, ry1, rx2, ry2 = self.seg_map
enf_plan = planner.run_rrt_opt(np.atleast_2d(enf_start),
np.atleast_2d(enf_goal), rx1, ry1, rx2,
ry2)
enf_noisy_plan = [enf_plan[0]]
for i in xrange(
1,
len(enf_plan) - 1
): #loc_t = np.random.multivariate_normal(enf_plan[i], [[path_noise, 0], [0, path_noise]]) # name 't_i' i.e. t_1, t_2,...t_n
loc_x = Q.randn(mu=enf_plan[i][0],
sigma=path_noise,
name="enf_x_" + str(i))
loc_y = Q.randn(mu=enf_plan[i][1],
sigma=path_noise,
name="enf_y_" + str(i))
loc_t = [loc_x, loc_y]
enf_noisy_plan.append(loc_t)
enf_noisy_plan.append(enf_plan[-1])
enf_loc = np.atleast_2d(enf_noisy_plan[t])
#----------------- end of enforcer model ------------------
# XXX the runner wants to do goal inference to figure out the next step of the enf
# TWO desires:
# 1) high likelihood for a enf path conditioned on past locations
# 2) high likelihood for his own path conditioned of not being detected for the next step and conditioned
# on past enf locations !
#----------------------------------------------------------
# runner (intruder) model
#----------------------------------------------------------
start_i = Q.choice(p=1.0 / cnt * np.ones((1, cnt)), name="int_start")
goal_i = Q.choice(p=1.0 / cnt * np.ones((1, cnt)), name="int_goal")
start = np.atleast_2d(self.locs[start_i])
goal = np.atleast_2d(self.locs[goal_i])
#my_plan = self.plan_path(start, goal)
my_plan = planner.run_rrt_opt(np.atleast_2d(start),
np.atleast_2d(goal), rx1, ry1, rx2, ry2)
my_loc = np.atleast_2d(my_plan[t])
my_noisy_plan = [my_plan[0]]
for i in xrange(
1,
len(my_plan) - 1
): #loc_t = np.random.multivariate_normal(my_plan[i], [[path_noise, 0], [0, path_noise]]) # name 't_i' i.e. t_1, t_2,...t_n
loc_x = Q.randn(mu=my_plan[i][0],
sigma=path_noise,
name="int_x_" + str(i))
loc_y = Q.randn(mu=my_plan[i][1],
sigma=path_noise,
name="int_y_" + str(i))
loc_t = [loc_x, loc_y]
my_noisy_plan.append(loc_t)
my_noisy_plan.append(my_plan[-1])
# XXX Need to make sure the runner wasn't seen in any of the previous time steps
i_already_seen = 0
for i in xrange(t):
intersections = Q.cache["enf_intersections_t_" +
str(i)] # get enforcer's fv for time t
i_already_seen = self.isovist.FindIntruderAtPoint(
my_noisy_plan[i], intersections)
if (i_already_seen):
detected_prob = 0.999 * i_already_seen + 0.001 * (
1 - i_already_seen)
if not i_already_seen:
# set up the enforcer view (forward vector, fv) for the next step
cur_enf_loc = scale_up(enf_noisy_plan[t])
next_enf_loc = scale_up(enf_noisy_plan[t + 1])
fv = direction(next_enf_loc, cur_enf_loc)
intersections = self.isovist.GetIsovistIntersections(
next_enf_loc, fv)
# does the enforcer see me at time 't'
my_next_loc = scale_up(my_noisy_plan[t + 1])
will_i_be_seen = self.isovist.FindIntruderAtPoint(
my_next_loc, intersections)
detected_prob = 0.999 * will_i_be_seen + 0.001 * (
1 - will_i_be_seen) # ~ flip(seen*.999 + (1-seen*.001)
future_detection = Q.flip(p=detected_prob, name="int_detected")
# for rendering purposes
Q.keep("int_plan", my_noisy_plan)
Q.keep("enf_plan", enf_noisy_plan)
def run_basic_partial(self, Q, path_noise=0.003):
rx1, ry1, rx2, ry2 = self.seg_map
t = Q.choice(p=1.0 / 40 * np.ones((1, 40)), name="t")
# #------------- model other agent movements (past and future) -------------- XXX need to change RV names
o_start_i = Q.choice(p=1.0 / self.cnt * np.ones((1, self.cnt)),
name="other_run_start")
o_goal_i = Q.choice(p=1.0 / self.cnt * np.ones((1, self.cnt)),
name="other_run_goal")
o_start = np.atleast_2d(self.locs[o_start_i])
o_goal = np.atleast_2d(self.locs[o_goal_i])
# plan using the latent variables of start and goal
other_plan = planner.run_rrt_opt(np.atleast_2d(o_start),
np.atleast_2d(o_goal), rx1, ry1, rx2,
ry2)
# add noise to the plan
other_noisy_plan = [other_plan[0]]
for i in xrange(
1,
len(other_plan) - 1
): #loc_t = np.random.multivariate_normal(my_plan[i], [[path_noise, 0], [0, path_noise]]) # name 't_i' i.e. t_1, t_2,...t_n
loc_x = Q.randn(mu=other_plan[i][0],
sigma=path_noise,
name="other_run_x_" + str(i))
loc_y = Q.randn(mu=other_plan[i][1],
sigma=path_noise,
name="other_run_y_" + str(i))
loc_t = [loc_x, loc_y]
other_noisy_plan.append(loc_t)
other_noisy_plan.append(other_plan[-1])
#------------- model agent's own movements (past and future) --------------
start_i = Q.choice(p=1.0 / self.cnt * np.ones((1, self.cnt)),
name="run_start")
goal_i = Q.choice(p=1.0 / self.cnt * np.ones((1, self.cnt)),
name="run_goal")
start = np.atleast_2d(self.locs[start_i])
goal = np.atleast_2d(self.locs[goal_i])
# plan using the latent variables of start and goal
my_plan = planner.run_rrt_opt(np.atleast_2d(start),
np.atleast_2d(goal), rx1, ry1, rx2, ry2)
# add noise to the plan
my_noisy_plan = [my_plan[0]]
for i in xrange(
1,
len(my_plan) - 1
): #loc_t = np.random.multivariate_normal(my_plan[i], [[path_noise, 0], [0, path_noise]]) # name 't_i' i.e. t_1, t_2,...t_n
loc_x = Q.randn(mu=my_plan[i][0],
sigma=path_noise,
name="run_x_" + str(i))
loc_y = Q.randn(mu=my_plan[i][1],
sigma=path_noise,
name="run_y_" + str(i))
loc_t = [loc_x, loc_y]
my_noisy_plan.append(loc_t)
my_noisy_plan.append(my_plan[-1])
my_loc = my_noisy_plan[t]
#---------------- need to add RV of detection for each time step ----------
t_detected = []
PATH_LIMIT = 40
for i in xrange(0, PATH_LIMIT):
cur_loc = scale_up(my_noisy_plan[i])
intersections = None
detection_prob = 0.001
# face the runner if within certain radius
if dist(my_noisy_plan[i], other_noisy_plan[i]) <= .4: #.35:
fv = direction(scale_up(other_noisy_plan[i]), cur_loc)
intersections = self.isovist.GetIsovistIntersections(
cur_loc, fv)
# does the agent see other at time 't'
other_loc = scale_up(other_noisy_plan[i])
will_other_be_seen = self.isovist.FindIntruderAtPoint(
other_loc, intersections)
if will_other_be_seen:
detection_prob = 0.999
t_detected.append(i)
future_detection = Q.flip(p=detection_prob,
name="detected_t_" + str(i))
Q.keep("intersections-t-" + str(i), intersections)
same_goal = 0
if goal_i == o_goal_i:
same_goal = 1
same_goal_prob = 0.999 * same_goal + 0.001 * (1 - same_goal)
runners_same_goal = Q.flip(p=same_goal_prob, name="same_goal")
Q.keep("t_detected", t_detected)
Q.keep("my_plan", my_noisy_plan)
Q.keep("other_plan", other_noisy_plan)
def run_tom_partial(self, Q, path_noise=0.003):
rx1,ry1,rx2,ry2 = self.seg_map
t = Q.choice( p=1.0/40*np.ones((1,40)), name="t" )
#------------- model agent's movements (past and future) --------------
start_i = Q.choice( p=1.0/self.cnt*np.ones((1,self.cnt)), name="init_run_start" )
goal_i = Q.choice( p=1.0/self.cnt*np.ones((1,self.cnt)), name="init_run_goal" )
start = np.atleast_2d( self.locs[start_i] )
goal = np.atleast_2d( self.locs[goal_i] )
# plan using the latent variables of start and goal
my_plan = planner.run_rrt_opt( np.atleast_2d(start),
np.atleast_2d(goal), rx1,ry1,rx2,ry2 )
# add noise to the plan
my_noisy_plan = [my_plan[0]]
for i in xrange(1, len(my_plan)-1):#loc_t = np.random.multivariate_normal(my_plan[i], [[path_noise, 0], [0, path_noise]]) # name 't_i' i.e. t_1, t_2,...t_n
loc_x = Q.randn( mu=my_plan[i][0], sigma=path_noise, name="init_run_x_"+str(i) )
loc_y = Q.randn( mu=my_plan[i][1], sigma=path_noise, name="init_run_y_"+str(i) )
loc_t = [loc_x, loc_y]
my_noisy_plan.append(loc_t)
my_noisy_plan.append(my_plan[-1])
my_loc = my_noisy_plan[t]
#---------------- do inference --------------------------------------------
other_noisy_plan = None
other_inferred_trace = None
if self.mode == "collab":
post_sample_traces, other_inferred_goal = self.collaborative_nested_inference(Q)
for trace in post_sample_traces:
if trace["run_goal"] == other_inferred_goal:
other_inferred_trace = trace
other_noisy_plan = trace["my_plan"]
break
if self.mode == "advers":
# return the trace with least detections
nested_post_sample_traces, other_inferred_trace = self.adversarial_nested_inference(Q)
other_noisy_plan = other_inferred_trace["my_plan"]
#---------------- need to add RV of detection for each time step ----------
t_detected = []
PATH_LIMIT = 40
for i in xrange(0, PATH_LIMIT):
cur_loc = scale_up(my_noisy_plan[i])
intersections = None
detection_prob = -10000.0
# face the runner if within certain radius
if dist(my_noisy_plan[i], other_noisy_plan[i]) <= .4: #.35:
fv = direction(scale_up(other_noisy_plan[i]), cur_loc)
intersections = self.isovist.GetIsovistIntersections(cur_loc, fv)
# does the agent see other at time 't'
other_loc = scale_up(other_noisy_plan[i])
will_other_be_seen = self.isovist.FindIntruderAtPoint( other_loc, intersections )
if will_other_be_seen:
detection_prob = -.01
t_detected.append(i)
future_detection = Q.lflip( lp=detection_prob, name="detected_t_"+str(i) )
Q.keep("intersections-t-"+str(i), intersections)
if self.mode == "collab":
same_goal = 0
if goal_i == other_inferred_goal:
same_goal = 1
same_goal_prob = 0.999*same_goal + 0.001*(1-same_goal)
runners_same_goal = Q.flip( p=same_goal_prob, name="same_goal" ) #TODO: update this lflip 1.19.2018
#print t_detected
Q.keep("t_detected", t_detected)
Q.keep("my_plan", my_noisy_plan)
Q.keep("other_plan", other_noisy_plan)
Q.keep("other_run_start", other_inferred_trace["run_start"])
Q.keep("other_run_goal", other_inferred_trace["run_goal"])
Q.keep("nested_post_samples", nested_post_sample_traces)
def run_basic_partial(self, Q, path_noise=0.000):
rx1,ry1,rx2,ry2 = self.seg_map
t = Q.choice( p=1.0/40*np.ones((1,40)), name="t" )
# #------------- model other agent movements (past and future) --------------
o_start_i = Q.choice( p=1.0/self.cnt*np.ones((1,self.cnt)), name="other_run_start" )
o_goal_i = Q.choice( p=1.0/self.cnt*np.ones((1,self.cnt)), name="other_run_goal" )
o_start = np.atleast_2d( self.locs[o_start_i] )
o_goal = np.atleast_2d( self.locs[o_goal_i] )
# plan using the latent variables of start and goal
# other_plan = planner.run_rrt_opt( np.atleast_2d(o_start),
# np.atleast_2d(o_goal), rx1,ry1,rx2,ry2 )
#print Q.fetch_obs()
prev_true_loc = np.atleast_2d([Q.get_obs("other_run_x_"+str(t-1)), Q.get_obs("other_run_y_"+str(t-1))])
#print "prev_true_loc", prev_true_loc
other_plan = planner.run_rrt_opt( prev_true_loc, np.atleast_2d(o_goal), rx1,ry1,rx2,ry2 )
u = 1
# add noise to the plan
other_noisy_plan = [self.locs[o_start_i]]
for i in xrange(1, len(other_plan)-1):#loc_t = np.random.multivariate_normal(my_plan[i], [[path_noise, 0], [0, path_noise]]) # name 't_i' i.e. t_1, t_2,...t_n
if i < t:
# do not sample if already known to be true:
loc_t = [Q.get_obs(name="other_run_x_"+str(i)), Q.get_obs(name="other_run_y_"+str(i))]
else:
loc_x = Q.randn( mu=other_plan[u][0], sigma=path_noise, name="other_run_x_"+str(i) )
loc_y = Q.randn( mu=other_plan[u][1], sigma=path_noise, name="other_run_y_"+str(i) )
loc_t = [loc_x, loc_y]
u+=1
other_noisy_plan.append(loc_t)
other_noisy_plan.append(other_plan[-1])
#------------- model agent's own movements (past and future) --------------
start_i = Q.choice( p=1.0/self.cnt*np.ones((1,self.cnt)), name="run_start" )
goal_i = Q.choice( p=1.0/self.cnt*np.ones((1,self.cnt)), name="run_goal" )
#insert as a way to get goals not too close
goal_i, Q = get_goal(start_i, Q)
start = np.atleast_2d( self.locs[start_i] )
goal = np.atleast_2d( self.locs[goal_i] )
# plan using the latent variables of start and goal
my_plan = planner.run_rrt_opt( np.atleast_2d(start),
np.atleast_2d(goal), rx1,ry1,rx2,ry2 )
# add noise to the plan
my_noisy_plan = [my_plan[0]]
for i in xrange(1, len(my_plan)-1):#loc_t = np.random.multivariate_normal(my_plan[i], [[path_noise, 0], [0, path_noise]]) # name 't_i' i.e. t_1, t_2,...t_n
loc_x = Q.randn( mu=my_plan[i][0], sigma=path_noise, name="run_x_"+str(i) )
loc_y = Q.randn( mu=my_plan[i][1], sigma=path_noise, name="run_y_"+str(i) )
loc_t = [loc_x, loc_y]
my_noisy_plan.append(loc_t)
my_noisy_plan.append(my_plan[-1])
my_loc = my_noisy_plan[t]
#-------------------------- detections ---------------------
if self.mode == "collab":
t_detected = []
PATH_LIMIT = 30
for i in xrange(0, PATH_LIMIT):
cur_loc = scale_up(my_noisy_plan[i])
intersections = None
detection_prob = 0.1
# face the runner if within certain radius
if dist(my_noisy_plan[i], other_noisy_plan[i]) <= .4: #.35:
fv = direction(scale_up(other_noisy_plan[i]), cur_loc)
intersections = self.isovist.GetIsovistIntersections(cur_loc, fv)
# does the agent see other at time 't'
other_loc = scale_up(other_noisy_plan[i])
will_other_be_seen = self.isovist.FindIntruderAtPoint( other_loc, intersections )
if will_other_be_seen:
detection_prob = 0.9
t_detected.append(i)
future_detection = Q.flip( p=detection_prob, name="detected_t_"+str(i) )
# ^ might want to change this to lflip when you do experiments next
# you wrote this on 1.19.2018 - iris
Q.keep("intersections-t-"+str(i), intersections)
same_goal = 0
if goal_i == o_goal_i:
same_goal = 1
same_goal_prob = .999*same_goal + .001*(1-same_goal)
runners_same_goal = Q.lflip( lp=same_goal_prob, name="same_goal" )
Q.keep("t_detected", t_detected)
Q.keep("my_plan", my_noisy_plan)
Q.keep("other_plan", other_noisy_plan)
if self.mode == "advers":
t_detected = []
PATH_LIMIT = 30
for i in xrange(0, PATH_LIMIT):
other_loc = scale_up(other_noisy_plan[i])
intersections = None
detection_prob = -0.0001
# face the runner if within certain radius
if dist(my_noisy_plan[i], other_noisy_plan[i]) <= .4: #.35:
# -----if other is looking for me
#print ("fv:", scale_up(my_noisy_plan[i]), other_loc)
fv = direction(scale_up(my_noisy_plan[i]), other_loc)
intersections = self.isovist.GetIsovistIntersections(other_loc, fv)
#print intersections
# does the chaser see other at time 'i'
curr_loc = scale_up(my_noisy_plan[i])
#if other is looking for me
will_I_be_seen = self.isovist.FindIntruderAtPoint( curr_loc, intersections )
#print will_I_be_seen
if will_I_be_seen:
detection_prob = -1.0
t_detected.append(i)
future_detection = Q.clflip( lp=detection_prob, name="detected_t_"+str(i) )
#future_detection = Q.flip( p=detection_prob, name="detected_t_"+str(i) )
Q.keep("intersections-t-"+str(i), intersections)
Q.keep("t_detected", t_detected)
Q.keep("my_plan", my_noisy_plan)
Q.keep("other_plan", other_noisy_plan)
def run_tom_partial(self, Q, path_noise=0.000):
rx1,ry1,rx2,ry2 = self.seg_map
t = Q.choice( p=1.0/40*np.ones((1,40)), name="t" )
#------------- model agent's movements (past and future) --------------
start_i = Q.choice( p=1.0/self.cnt*np.ones((1,self.cnt)), name="init_run_start" )
goal_i = Q.choice( p=1.0/self.cnt*np.ones((1,self.cnt)), name="init_run_goal" )
start = np.atleast_2d( self.locs[start_i] )
goal = np.atleast_2d( self.locs[goal_i] )
#print "HERE:", goal_i, goal
# plan using the latent variables of start and goal
prev_true_loc = np.atleast_2d([Q.get_obs("init_run_x_"+str(t-1)), Q.get_obs("init_run_y_"+str(t-1))])
my_plan = planner.run_rrt_opt( prev_true_loc, goal, rx1,ry1,rx2,ry2 )
u = 1
my_noisy_plan = [self.locs[start_i]]
for i in xrange(1, len(my_plan)-1):#loc_t = np.random.multivariate_normal(my_plan[i], [[path_noise, 0], [0, path_noise]]) # name 't_i' i.e. t_1, t_2,...t_n
if i < t:
# do not sample if already known to be true
loc_t = [Q.get_obs("init_run_x_"+str(i)), Q.get_obs("init_run_y_"+str(i))]
else:
loc_x = Q.randn( mu=my_plan[u][0], sigma=path_noise, name="init_run_x_"+str(i) )
loc_y = Q.randn( mu=my_plan[u][1], sigma=path_noise, name="init_run_y_"+str(i) )
loc_t = [loc_x, loc_y]
u += 1
my_noisy_plan.append(loc_t)
my_noisy_plan.append(my_plan[-1])
my_loc = my_noisy_plan[t]
#assert (len(my_noisy_plan) == 30)
#---------------- do inference --------------------------------------------
other_noisy_plan = None
other_inferred_trace = None
if self.mode == "collab":
post_sample_traces, other_inferred_goal = self.collaborative_nested_inference(Q)
for trace in post_sample_traces:
if trace["run_goal"] == other_inferred_goal:
other_inferred_trace = trace
other_noisy_plan = trace["my_plan"]
break
if self.mode == "advers":
all_t_detected = []
other_plans = []
L = self.L
all_Qls_scores = []
all_Qls_traces = []
all_Qls_obs = []
all_ql_scores = []
#print "init Q score:", Q.get_score()
for l in xrange(L):
q = self.condition_middle_model(Q)
# print ("Q obs:", Q.fetch_obs())
# print ("q obs:", q.fetch_obs())
# raw_input()
Q_l = Q.get_copy()
#print "---------running runner model--------------"
q_score_l, q_l = q.run_model()
#print '----------running rest of chaser model-----------'
#plot_middlemost_sample(q_l, q_score_l, self.directory)
other_noisy_plan = q_l["my_plan"]
#---------------- need to add RV of detection for each time step ----------
t_detected = []
PATH_LIMIT = 30
will_other_be_seen = False
for i in xrange(0, PATH_LIMIT):
cur_loc = scale_up(my_noisy_plan[i])
intersections = None
detection_prob = -.0001
# face the runner if within certain radius
if dist(my_noisy_plan[i], other_noisy_plan[i]) <= .4: #.35:
fv = direction(scale_up(other_noisy_plan[i]), cur_loc)
intersections = self.isovist.GetIsovistIntersections(cur_loc, fv)
# does the agent see other at time 't'
other_loc = scale_up(other_noisy_plan[i])
will_other_be_seen = self.isovist.FindIntruderAtPoint( other_loc, intersections )
if will_other_be_seen:
detection_prob = -1
t_detected.append(i)
#future_detection = Q_l.flip( p=detection_prob, name="detected_t_"+str(i) )
future_detection = Q_l.lflip( lp=detection_prob, name="detected_t_"+str(i))
Q_l.add_trace(name="q_trace", trace=q_l, score=q_score_l)
all_ql_scores.append(q_score_l)
#print "list:", all_ql_scores
# Q_l.keep("t_detected", t_detected)
all_t_detected.append(t_detected)
# Q_l.keep("my_plan", my_noisy_plan)
# Q_l.keep("other_plan", other_noisy_plan)
other_plans.append(other_noisy_plan)
# Q_l.keep("other_run_start", q_l["run_start"])
# Q_l.keep("other_run_goal", q_l["run_goal"])
all_Qls_scores.append(Q_l.get_score())
all_Qls_traces.append(Q_l.get_trace())
all_Qls_obs.append(Q_l.get_obs())
#print ("all ql scores:", all_ql_scores)
#print ("all_Qls_scores:", all_Qls_scores)
Q.keep("all_ql_scores", np.array(all_ql_scores))
Q.keep("all_Qls_obs", all_Qls_obs)
Q.keep("all_Qls_traces", all_Qls_traces)
Q.keep("all_Qls_scores", np.array(all_Qls_scores))
Q.keep("mean", np.mean(all_Qls_scores))
Q.keep("my_plan", my_noisy_plan)
Q.keep("t_detected", all_t_detected)
Q.keep("other_plan", other_plans)
#update actual score of Q.
Q.cur_trace_score = np.mean(all_Qls_scores)
