def PomdpFollower(x1, x2, dt, traj_duration, weight, agentID, H, theta_hat,
view_thr, thr, p1_goal, p2_goal, sm, v, ww, wc, wd, costmap,
costmap0s, cthr, cres, rr, r_m2o, t_m2o, wh):
path_robot = []
path_fScore = []
path_intent = []
pathx = []
pathy = []
pathx_ = []
pathy_ = []
robot_s_angz = []
path_fScore = np.Inf
current = []
path_found = False
initial_node = Nodes()
initial_node.x_rob = x2
initial_node.x_ped = x1
initial_node.theta = theta_hat
initial_node.theta_new = theta_hat
initial_node.gScore_ = [0, 0]
gScore = []
gScore.append(0)
gScore_ = []
gScore_.append([0, 0])
fScore = []
fScore.append(np.Inf)
fScore_ = []
fScore_.append([np.Inf, np.Inf])
totalNodes = []
totalNodes.append(initial_node)
closeSet = []
openSet = []
openSet.append(0)
cameFrom = []
cameFrom.append(-1)
rob_hScore = np.Inf
cn_node = []
cn_node.append(initial_node)
current = initial_node
min_dist = 0.3
min_vel = 0.3
x_rob = initial_node.x_rob
x_ped = initial_node.x_ped
theta = initial_node.theta
if agentID == 1:
agentID_ = 2
elif agentID == 2:
agentID_ = 1
belief_num = len(theta)
r_path_found = []
r_x = []
r_y = []
r_x_ = []
r_y_ = []
rob_s_angz = []
rollout_path_found = []
rollout_x = []
rollout_x_ = []
rollout_y = []
rollout_y_ = []
robot_s_angz = []
fk_path_found = 1
for i in range(belief_num):
if theta[i] > thr:
r_path_found, r_x, r_y, r_x_, r_y_, rob_s_angz, path_fScore, current_ = FullKnowledgeCollaborativePlanner(
x_ped, x_rob, dt, traj_duration, weight, agentID_, H,
p1_goal[i], p2_goal[i], sm, v, ww, costmap, costmap0s, cthr,
cres, rr, r_m2o, t_m2o)
if not r_path_found:
fk_path_found = 0
rollout_path_found.append(r_path_found)
rollout_x.append(r_x)
rollout_x_.append(r_x_)
rollout_y.append(r_y)
rollout_y_.append(r_y_)
robot_s_angz.append(rob_s_angz)
while len(openSet) > 0 and fk_path_found:
current_fScores = [fScore[j] for j in openSet]
min_fScore = min(current_fScores)
current_node_id = fScore.index(min_fScore)
if current_node_id > len(totalNodes):
print 'cureent node id greater than total node size, error'
break
current = totalNodes[current_node_id]
if (current.end_time > traj_duration * H) or (rob_hScore < 1.0):
pathx, pathy, pathx_, pathy_, rob_s_angz, path_intent = ReconstructPathFromNode(
cameFrom, current_node_id, totalNodes, agentID)
path_found = True
path_fScore = min_fScore
pathx_ = r_x
pathy_ = r_y
#print 'rob_s_angz = {}'.format(rob_s_angz)
#print 'rob_hScore = {}'.format(rob_hScore)
#print 'pomdp path vel = {}'.format(np.sqrt((pathx[0]-pathx[1])**2+(pathy[0]-pathy[1])**2)/dt)
break
if current_node_id in openSet:
openSet.pop(openSet.index(current_node_id))
else:
print 'current node id {} is not in list'.format(current_node_id)
print 'totalNodes length = {}'.format(len(totalNodes))
print 'closedSet = {}'.format(closeSet)
print 'openSet = {}'.format(openSet)
break
closeSet.append(current_node_id)
theta = current.theta_new
num_ti = [i for i in range(len(theta)) if theta[i] > thr]
ang_num = 1
sp_num = 1
H_ = max(1, H - 2)
if current.end_time < (traj_duration * H_):
sp_num = 1
#print 'agent id = {}'.format(agentID)
neighbors = NavigationActionSampling(current, dt, traj_duration,
agentID, v, sm, p1_goal, p2_goal,
ang_num, sp_num, len(num_ti))
#print 'neighbor size = {}x{}'.format(len(neighbors),len(neighbors[0]))
#print 'current node start time and end time = {},{}'.format(current.start_time,current.end_time)
#print 'child node start time and end time = {},{}'.format(neighbors[0][0].start_time,neighbors[0][0].end_time)
ped_travel_cost = traj_duration
rob_travel_cost = traj_duration
start_ind = int(np.floor(current.end_time / dt))
num_traj = int(np.floor(traj_duration / dt))
RolloutX = []
RolloutY = []
RolloutX_ = []
RolloutY_ = []
#print 'start ind, s+num = {},{}'.format(start_ind,start_ind+num_traj)
#print 'neighbor size = {},{}'.format(len(neighbors),len(neighbors[0]))
#print 'rollout length = {},{}'.format(len(rollout_x),len(rollout_x[0]))
for j in num_ti:
#print 'len rollout[j] = {}'.format(len(rollout_x[j]))
#print 'start index = {}, end index = {}'.format(start_ind,start_ind+num_traj)
RolloutX.append([
rollout_x[j][k]
for k in range(start_ind, start_ind + num_traj, 1)
])
RolloutX_.append([
rollout_x_[j][k]
for k in range(start_ind, start_ind + num_traj, 1)
])
RolloutY.append([
rollout_y[j][k]
for k in range(start_ind, start_ind + num_traj, 1)
])
RolloutY_.append([
rollout_y_[j][k]
for k in range(start_ind, start_ind + num_traj, 1)
])
Rollout = []
Rollout.append(RolloutX)
Rollout.append(RolloutY)
for i in range(len(neighbors[0])):
#nodes with the same sampled action
cn_node = [neighbors[j][i] for j in range(len(neighbors))]
traj = []
vel = [] #agent velocity
x = [] # agent position
#print 'robot velocity = {}'.format(cn_node[0].robot_velx[-1]**2+cn_node[0].robot_vely[-1]**2)
if agentID == 1:
traj.append(
[cn_node[0].human_trajx[j] for j in range(num_traj)])
traj.append(
[cn_node[0].human_trajy[j] for j in range(num_traj)])
traj.append(
[cn_node[0].human_velx[j] for j in range(num_traj)])
traj.append(
[cn_node[0].human_vely[j] for j in range(num_traj)])
vel.append(cn_node[0].x_ped[2])
vel.append(cn_node[0].x_ped[3])
x = [cn_node[0].x_ped[0], cn_node[0].x_ped[1]]
else:
traj.append(
[cn_node[0].robot_trajx[j] for j in range(num_traj)])
traj.append(
[cn_node[0].robot_trajy[j] for j in range(num_traj)])
traj.append(
[cn_node[0].robot_velx[j] for j in range(num_traj)])
traj.append(
[cn_node[0].robot_vely[j] for j in range(num_traj)])
#print 'cn_node vel = {}'.format(cn_node[0].x_rob)
vel.append(cn_node[0].x_rob[2])
vel.append(cn_node[0].x_rob[3])
x = [cn_node[0].x_rob[0], cn_node[0].x_rob[1]]
#collision1 = Check(cn_node[0].robot_trajx,cn_node[0].robot_trajy,costmap,costmap0s,cthr,cres,rr,r_m2o,t_m2o)
collision1 = Check(cn_node[0].robot_trajx, cn_node[0].robot_trajy,
costmap, costmap0s, cthr, cres, rr)
collision2 = PartnerCollisionCheck(Rollout, traj, min_dist - 0.02,
min_vel)
if collision1 or collision2:
#print 'collision 1,2 = {}{}'.format(collision1,collision2)
#print 'Rollout = {}'.format(Rollout)
#rint 'traj = {}'.format(traj)
continue
#print 'RolloutX_ = {}'.format(RolloutX_)
#print 'vel = {}'.format(vel)
wdist = 0.0
x_ = [] #partner state
x__ = [] #agent state appended
for j in range(len(RolloutX_)):
for k in range(len(RolloutX_[j])):
if np.isnan(RolloutX_[j][k]) or np.isnan(RolloutY_[j][k]):
print 'rollout is nan'
if np.isnan(traj[0][k]) or np.isnan(traj[1][k]):
print 'traj is nan'
wdist = wdist + theta[num_ti[j]] * np.sqrt(
(RolloutX_[j][k] - traj[0][j])**2 +
(RolloutY_[j][k] - traj[1][j])**2)
x_.append([RolloutX[j][-1], RolloutY[j][-1]])
x__.append(x)
if agentID == 1:
x_ped_ = x__
x_rob_ = x_
else:
x_rob_ = x__
x_ped_ = x_
cdist = [wdist, wdist]
cdist[agentID_] = 0.0
cdist = [0, 0] #TODO
#chvel = [0,ww*((v-np.sqrt(cn_node[0].x_ped[2]**2+cn_node[0].x_ped[3]**2))**2)]
chvel = [0, 0]
gScore_new = [0, 0]
#print 'gScorw = {}'.format(current.gScore_)
gScore_new[
0] = current.gScore_[0] + rob_travel_cost + chvel[0] + cdist[0]
gScore_new[
1] = current.gScore_[1] + ped_travel_cost + chvel[1] + cdist[1]
gScoreNew = np.dot(gScore_new, weight)
for j in num_ti:
#bayesian belief update based on obs:traj_
if len(num_ti) < belief_num:
# assuming only two states
theta = [0 for k in range(len(theta))]
theta[j] = 1.0
else:
obs = x_[j]
distx = [
np.sqrt((x_[k][0] - obs[0])**2 +
(x_[k][1] - obs[1])**2) for k in range(len(x_))
]
distx[j] = np.Inf
obs_x = [obs[0] - x[0], obs[1] - x[1]]
view = np.dot(vel, obs_x) / np.sqrt(np.dot(
obs_x, obs_x)) / np.sqrt(np.dot(vel, vel))
cn_node[j].view = view
if view > view_thr:
#belief updates
mindistx = min(distx)
minind = distx.index(mindistx)
#update: other
obs_s1 = ObsLikelihood(wd[j], mindistx)
obs_s = [1.0 - obs_s1 for k in range(belief_num)]
#update: self
obs_s[j] = obs_s1
s = theta #??
o = np.dot(obs_s, s)
s_new = np.multiply(obs_s, s)
theta = [k / o for k in s_new]
cn_ind = num_ti.index(j)
cn_node[cn_ind].theta_new = theta
cn_node[cn_ind].gScore_ = gScore_new
#cost_to_go using the updated theta
#print 'p1_goal and ped_pos = {},{}'.format(p1_goal,x_ped_)
#print 'p2_goal and rob_pos = {},{}'.format(p2_goal,x_rob_)
ped_dtgo = [
np.sqrt((p1_goal[num_ti[k]][0] - x_ped_[k][0])**2 +
(p1_goal[num_ti[k]][1] - x_ped_[k][1])**2) *
theta[num_ti[k]] / v for k in range(len(x_ped_))
]
ped_hScore = np.sum(ped_dtgo)
rob_dtgo = [
np.sqrt((p2_goal[num_ti[k]][0] - x_rob_[k][0])**2 +
(p2_goal[num_ti[k]][1] - x_rob_[k][1])**2) *
theta[num_ti[k]] / v for k in range(len(x_rob_))
]
rob_hScore = np.sum(rob_dtgo)
hScore = np.dot(weight, [rob_hScore, ped_hScore])
gScore.append(gScoreNew)
gScore_.append(gScore_new)
fScore_.append(
np.add(gScore_new, [
wh * weight[0] * rob_hScore,
wh * weight[1] * ped_hScore
]))
fScore.append(gScoreNew + wh * hScore +
np.random.normal() * 0.01)
#cost-to-go
cameFrom.append(current_node_id)
openSet.append(len(totalNodes))
totalNodes.append(cn_node[cn_ind])
if (len(totalNodes) - len(openSet)) > 400:
print 'gScore_ = {}'.format(gScore_[-1])
print 'gScoreNew = {}'.format(gScoreNew)
print 'fScore = {}'.format(fScore[-1])
#print 'too many nodes'
break
if not path_found:
print 'pomdp path not found after openSet cleared'
print 'total node length = {}'.format(len(totalNodes))
print 'cn_node end time = {}'.format(cn_node[0].end_time)
print 'current node time = {}'.format(current.end_time)
#else:
# print 'total node length = {}'.format(len(totalNodes))
return path_found, pathx, pathy, pathx_, pathy_, robot_s_angz, path_fScore, current