def walk_path(path, speed, times):
distances_from_start = [0.0] * len(path)
for i in xrange(1, len(path)):
distances_from_start[i] = distances_from_start[i - 1] + dist(
path[i - 1], path[i])
locations = [-1] * len(times)
locations[0] = path[0]
for time_i, t in enumerate(times):
desired_distance = t * speed
used_up_time = False
for i in xrange(1, len(path)):
prev = path[i - 1]
cur = path[i]
dist_to_prev = dist(prev, cur)
if distances_from_start[i] >= desired_distance:
#we overshot, the location is beween i-1 and i
overshoot = distances_from_start[i] - desired_distance
past_prev = dist_to_prev - overshoot
if dist_to_prev == 0:
frac = 1
else:
frac = past_prev / dist_to_prev
locations[time_i] = [
prev[0] * (1.0 - frac) + cur[0] * frac,
prev[1] * (1.0 - frac) + cur[1] * frac
]
used_up_time = True
break
if not used_up_time:
locations[time_i] = path[-1]
assert (len(locations) >= 29)
return locations
def walk_path(path, speed, times, x1, y1, x2, y2):
distances_from_start = [0.0] * len(path)
for i in xrange(1, len(path)):
distances_from_start[i] = distances_from_start[i - 1] + dist(
path[i - 1], path[i])
locations = [-1] * len(times)
locations[0] = np.round(path[0], 3)
for time_i, t in enumerate(times):
desired_distance = t * speed
used_up_time = False
for i in xrange(1, len(path)):
prev = path[i - 1]
cur = path[i]
dist_to_prev = dist(prev, cur)
if distances_from_start[i] >= desired_distance:
#we overshot, the location is beween i-1 and i
overshoot = distances_from_start[i] - desired_distance
past_prev = dist_to_prev - overshoot
if dist_to_prev == 0:
frac = 1
else:
frac = past_prev / dist_to_prev
locations[time_i] = np.round([
prev[0] * (1.0 - frac) + cur[0] * frac, prev[1] *
(1.0 - frac) + cur[1] * frac
], 3)
used_up_time = True
break
if not used_up_time:
locations[time_i] = np.round(path[-1], 3)
assert (len(locations) >= 29)
for i in xrange(len(locations) - 1):
if locations[i][0] == locations[i + 1][0]:
if locations[i][1] == locations[i + 1][1]:
continue
int_x, int_y, intersection_indicators = line_intersect(
locations[i][0], locations[i][1], locations[i + 1][0],
locations[i + 1][1], x1, y1, x2, y2)
if intersection_indicators.any():
#print "[[",locations[i][0],",", locations[i][1],"],[", locations[i+1][0],",",locations[i+1][1],"]],"
#print("throwing out RRT")
return []
#print ("keeping location")
#print locations
return locations
def get_distances(x1, y1, x2, y2, start_pt, goal_pt):
distances = []
for i in xrange(100):
path = run_rrt(start_pt, goal_pt, x1, y1, x2, y2)
for j in xrange(len(path) - 1):
distances.append(dist(path[j], path[j + 1]))
return distances
def optimize_path(x1, y1, x2, y2, orig_path, iters, std):
points = orig_path[:]
inner_pt_cnt = len(points) - 2
if inner_pt_cnt == 0:
return orig_path
for i in xrange(iters):
pt_i = 2 + (i % inner_pt_cnt)
if pt_i < 1:
continue
if pt_i >= len(orig_path) - 1:
continue
pre_pt = points[pt_i - 1]
pt = points[pt_i]
next_pt = points[pt_i + 1]
ad_pt = [
pt[0] + np.random.randn() * std, pt[1] + np.random.randn() * std
]
curr_dist = dist(pre_pt, pt) + dist(pt, next_pt)
back_int_x, back_int_y, back_intersection_indicators = line_intersect(
pre_pt[0], pre_pt[1], ad_pt[0], ad_pt[1], x1, y1, x2, y2)
forward_int_x, forward_int_y, forward_intersection_indicators = line_intersect(
ad_pt[0], ad_pt[1], next_pt[0], next_pt[1], x1, y1, x2, y2)
back_ok = not back_intersection_indicators.any()
front_ok = not forward_intersection_indicators.any()
new_dist = 0.0
if back_ok:
if front_ok:
new_dist = dist(pre_pt, ad_pt) + dist(ad_pt, next_pt)
if new_dist < curr_dist:
points[pt_i] = ad_pt
return points
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_simulation(sim_id, locs, seg_map, isovist, polys, epolys):
# unpack
rx1, ry1, rx2, ry2 = seg_map
intersection_cache = []
# create model of chaser
chaser_model = create_chaser_model(seg_map=seg_map,
locs=locs,
isovist=isovist)
# get evenly spead out starts and goal
int_start = 0
int_goal = 8
#chas_start = 1
# int_start = randint(0, len(locs)-1)
# int_goal = randint(0, len(locs)-1)
# while(dist(locs[int_start], locs[int_goal]) <= 0.5):
# int_goal = randint(0, len(locs)-1)
chas_start = randint(0, len(locs) - 1)
while (dist(locs[int_start], locs[chas_start]) <= 0.5):
chas_start = randint(0, len(locs) - 1)
runner_start_i = int_start
runner_start = locs[runner_start_i]
# get runner's random goal location (far enough from its start)
runner_goal_i = int_goal
runner_goal = locs[runner_goal_i]
# store history of runner's locations
runner_true_locs = [runner_start]
# get random start location
start_index = chas_start
chaser_start = locs[start_index]
# begin tracking history of movements
chaser_locs = [chaser_start]
#TEMP FOR TESTING (NAIVE RUNNER)
runner_path = planner.run_rrt_opt(np.atleast_2d(runner_start),
np.atleast_2d(runner_goal),
rx1,
ry1,
rx2,
ry2,
slow=True)
if not naive_runner:
# XXX smart runner
runner_path = [[0.10000000000000001, 0.099999999999999978],
[0.14370786380607686, 0.12428214655893158],
[0.18741572761215378, 0.14856429311786318],
[0.23112359141823066, 0.17284643967679481],
[0.27483145522430752, 0.19712858623572641],
[0.25854686086588807, 0.23830235506087813],
[0.25097278902994408, 0.28619750551848688],
[0.24540487293466112, 0.31648310191196649],
[0.19679616003865907, 0.30477015904546001],
[0.16866150321818352, 0.32844002322014876],
[0.15786536899041551, 0.37717509770481417],
[0.14948852463283413, 0.44046838810633692],
[0.14111168027525278, 0.47576167850785966],
[0.13273483591767141, 0.52505496890938252],
[0.12435799156009002, 0.56434825931090527],
[0.11598114720250866, 0.62364154971242802],
[0.13916939803284978, 0.66269402652378151],
[0.15039748869499225, 0.70969377654572163],
[0.15619668605039488, 0.75935633095075694],
[0.16199588340579754, 0.80901888535579236],
[0.16779508076120014, 0.85868143976082756],
[0.20837094819244148, 0.87335725908849149],
[0.25828353694770523, 0.87588922948695491],
[0.30825277792753181, 0.87764278470422896],
[0.35822201890735811, 0.87939633992150279],
[0.40819125988718469, 0.88114989513877684],
[0.45816050086701121, 0.88290345035605078],
[0.48130593126207499, 0.91889699806900549],
[0.53128113014992562, 0.92047163965355017],
[0.58125632903777602, 0.92204628123809473]]
for i in xrange(10):
runner_path.append(runner_path[len(runner_path) - 1])
runner_goal = runner_path[-1]
#--end of smart runner
assert len(runner_path) == 40
runner_locs = runner_path
Q_history = []
Q_history_ts = []
# begin timer
TIME_LIMIT = 35
for t in xrange(TIME_LIMIT):
# check if the runner has reached its goal
try:
if runner_locs[t + 1] == runner_goal:
print "Failed: Runner Reached Goal"
return False, Q_history_ts
break
except:
print "FAILED"
return False, Q_history_ts
# get true runner's location at time t + 1
runner_true_loc = runner_locs[t + 1]
runner_true_locs.append(runner_true_loc)
#---------------------------------
# create empty trace
Q = p.ProgramTrace(chaser_model)
# condition the trace
Q.condition("t", t)
Q.set_obs("enf_start", chas_start)
Q.condition("int_detected", True)
Q.condition("int_start", runner_start_i)
for pre_t in xrange(t + 1):
Q.set_obs("enf_x_" + str(pre_t), chaser_locs[pre_t][0])
Q.set_obs("enf_y_" + str(pre_t), chaser_locs[pre_t][1])
if pre_t < t:
Q.cache["enf_intersections_t_" +
str(pre_t)] = intersection_cache[pre_t]
# run inference
if naive_chaser:
post_sample_traces = run_inference(Q, post_samples=64, samples=32)
else:
post_sample_traces = run_inference(Q, post_samples=64,
samples=32) # nested inference
exp_next_step, exp_enf_path = rand_expected_future_step(
post_sample_traces, "enf_plan")
#XXX not working
#if point_in_obstacle(chaser_locs[t], epolys):
# print "chaser went inside building"
#return False
Q.keep("post_sample_traces", post_sample_traces)
# replan to its expected next step
#t_ = min(t, len())
# enf_plan = planner.run_rrt_opt( np.atleast_2d(chaser_locs[t]),
# np.atleast_2d(exp_next_step), rx1,ry1,rx2,ry2)
enf_plan = exp_enf_path
# ------------------------------------
if enf_plan is None:
print "trying again"
t -= 1
continue
enf_next_step = enf_plan[1]
# store step made
chaser_locs.append(enf_next_step)
inters = None
# check to see if the runner was detected
runner_detected = False
# only if he is close
if dist(enf_next_step, runner_true_loc) <= .4:
# add a bit of hearing by facing the chaser towards the runner
fv = direction(scale_up(runner_true_loc), scale_up(enf_next_step))
# check if runner can be detected
runner_true_next_loc = scale_up(runner_true_loc)
intersections = isovist.GetIsovistIntersections(
scale_up(enf_next_step), fv)
runner_detected = isovist.FindIntruderAtPoint(
scale_up(runner_true_loc), intersections)
inters = np.asarray(intersections)
# store intersections at time 't' facing in the direction it stepped
fv = direction(scale_up(chaser_locs[t + 1]), scale_up(chaser_locs[t]))
intersections = isovist.GetIsovistIntersections(
scale_up(chaser_locs[t + 1]), fv)
if not runner_detected:
runner_detected = isovist.FindIntruderAtPoint(
scale_up(runner_true_loc), intersections)
intersection_cache.append(intersections)
if inters is None:
inters = np.asarray(intersections)
inters /= 500.0
# plot behavior
plot_name = None
if runner_detected:
print "Success: Runner Detected Before Goal"
plot_name = str(sim_id) + "_t-" + str(t) + "_s-" + "T" + ".eps"
if naive_chaser:
save_chaser_post_traces_naive(
post_sample_traces,
plot_name=plot_name,
runner_true_locs=runner_true_locs,
chaser_true_locs=chaser_locs,
intersections=inters,
exp_enf_path=exp_enf_path)
else:
save_chaser_post_traces_nested(
post_sample_traces,
plot_name=plot_name,
runner_true_locs=runner_true_locs,
chaser_true_locs=chaser_locs,
intersections=inters,
exp_enf_path=exp_enf_path)
Q.keep("real_world_detection", True)
Q_history.append(Q)
Q_history_ts.append(Q.trace)
return True, Q_history_ts
else:
plot_name = str(sim_id) + "_t-" + str(t) + "_s-" + "F" + ".eps"
Q.keep("real_world_detection", False)
Q_history.append(Q)
Q_history_ts.append(Q.trace)
print "searching..."
if naive_chaser:
save_chaser_post_traces_naive(post_sample_traces,
plot_name=plot_name,
runner_true_locs=runner_true_locs,
chaser_true_locs=chaser_locs,
intersections=inters,
exp_enf_path=exp_enf_path)
else:
save_chaser_post_traces_nested(post_sample_traces,
plot_name=plot_name,
runner_true_locs=runner_true_locs,
chaser_true_locs=chaser_locs,
intersections=inters,
exp_enf_path=exp_enf_path)
print "Failed: Runner Reached Goal"
return False, Q_history_ts
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)
