def NavigationActionSampling(parent, dt, traj_duration, agentID, v, sm,
p1_goal, p2_goal, ang_num, sp_num, num_obs):
t = np.dot(range(0, int(traj_duration / dt)), dt)
ang_del = 37.5 / 360 * np.pi
sp_del = 0.4
count = 0
act_num = (2 * ang_num + 1) * (2 * sp_num + 1)
Actions = []
dv_ = np.dot(range(-sp_num, sp_num + 1), sp_del)
ang_accel_ = np.dot(range(-ang_num, ang_num + 1), ang_del)
dv_mesh, ang_mesh = np.meshgrid(dv_, ang_accel_)
dv_sampled = dv_mesh.reshape(1, len(dv_) * len(ang_accel_))
ang_sampled = ang_mesh.reshape(1, len(dv_) * len(ang_accel_))
#plt.cla()
#plt.axis('equal')
#plt.grid(True)
#plt.autoscale(False)
#print 'len(dv_), len(ang_accel_) = {},{}'.format(len(dv_),len(ang_accel_))
for i in range(len(dv_) * len(ang_accel_)):
action = Nodes(parent, parent.end_time,
parent.end_time + traj_duration, [], [], [],
parent.theta_new)
dv = dv_sampled[0][i]
#print 'dv = {}'.format(dv)
ang_accel = ang_sampled[0][i]
count = count + 1
#initialization
if (agentID == 1):
init_x_ped = parent.x_ped[0]
init_y_ped = parent.x_ped[1]
pos = [init_x_ped, init_y_ped, 0, 0]
vx_ind = 2
vy_ind = 3
#vTravellerx = pos[2]
#vTravellery = pos[3]
vTx = p1_goal[0][0] - pos[0]
vTy = p1_goal[0][1] - pos[1]
vTx = parent.x_ped[2]
vTy = parent.x_ped[3]
else:
init_x_rob = parent.x_rob[0]
init_y_rob = parent.x_rob[1]
pos = [init_x_rob, init_y_rob, 0, 0]
vx_ind = 5
vy_ind = 6
#vTravellerx = pos[5]
#vTravellery = pos[6]
vTx = p2_goal[0][0] - pos[0]
vTy = p2_goal[0][1] - pos[1]
vTx = parent.x_rob[2]
vTy = parent.x_rob[3]
#print 'x_rob = {},{}'.format(parent.x_rob[2],parent.x_rob[3])
vTx_ = vTx / np.sqrt(vTx**2 + vTy**2) * (v + dv)
vTy_ = vTy / np.sqrt(vTx**2 + vTy**2) * (v + dv)
trajx = []
#trajx.append(pos[0])
trajy = []
#trajy.append(pos[1])
velx = np.dot(vTx_, np.cos(np.dot(t, ang_accel))) + np.dot(
vTy_, np.sin(np.dot(t, ang_accel)))
vely = -np.dot(vTx_, np.sin(np.dot(t, ang_accel))) + np.dot(
vTy_, np.cos(np.dot(t, ang_accel)))
for j in range(0, len(t)):
pos[0] = pos[0] + velx[j] * dt
pos[1] = pos[1] + vely[j] * dt
trajx.append(pos[0])
trajy.append(pos[1])
#plt.plot(pos[0],pos[1],'ok')
time = np.add(t, parent.end_time)
action.trajt = time
if dv < 0:
action.intent = 1
else:
action.intent = 0
if agentID == 1:
action.human_trajx = trajx
action.human_trajy = trajy
action.human_velx = velx
action.human_vely = vely
x_ped_ = []
x_ped_.append(pos[0])
x_ped_.append(pos[1])
x_ped_.append(velx[-1])
x_ped_.append(vely[-1])
action.x_ped = x_ped_
else:
action.robot_trajx = trajx
action.robot_trajy = trajy
action.robot_velx = velx
action.robot_vely = vely
action.robot_angz = [ang_accel for k in range(len(t))]
x_rob_ = []
x_rob_.append(pos[0])
x_rob_.append(pos[1])
x_rob_.append(velx[-1])
x_rob_.append(vely[-1])
action.x_rob = x_rob_
Actions.append(action)
Actions_ = []
for i in range(max(num_obs, 1)):
Actions_.append(Actions)
#print 'Actions_[0][0].rob_pos = {}'
plt.pause(0.01)
return Actions_
def SideBySideActionSampling(parent, dt, traj_duration, agentID, v, sm,
p1_goal, p2_goal, ang_num, sp_num):
t = np.dot(range(0, int(np.ceil((traj_duration / dt)))), dt)
ang_del = 37.5 / 360 * np.pi
sp_del = 0.4
count = 0
if agentID == 1: #human
sp_num = 0
else:
sp_num = 1
act_num = (2 * ang_num + 1) * (2 * sp_num + 1)
Actions = []
#for i in range(act_num):
# Actions.append(Nodes(parent,parent.end_time,parent.end_time+traj_duration,[],[],[],[],parent.theta_new))
dv_ = np.dot(range(-sp_num, sp_num + 1), sp_del)
ang_accel_ = np.dot(range(-ang_num, ang_num + 1), ang_del)
dv_mesh, ang_mesh = np.meshgrid(dv_, ang_accel_)
dv_sampled = dv_mesh.reshape(1, len(dv_) * len(ang_accel_))
ang_sampled = ang_mesh.reshape(1, len(dv_) * len(ang_accel_))
init_x_ped = parent.x_ped[0]
init_y_ped = parent.x_ped[1]
init_x_rob = parent.x_rob[0]
init_y_rob = parent.x_rob[1]
for i in range(act_num):
action = Nodes(parent, parent.end_time,
parent.end_time + traj_duration, [], [], [], [],
parent.theta_new)
dv = dv_sampled[0][i]
ang_accel = ang_sampled[0][i]
count = count + 1
#initialization
if (agentID == 1):
pos = [init_x_ped, init_y_ped]
pos_ = [init_x_rob, init_y_rob]
vx_ind = 2
vy_ind = 3
#vTravellerx = pos[2]
#vTravellery = pos[3]
#vTx = p1_goal[0]-pos[0]
#vTy = p1_goal[1]-pos[1]
vTx = parent.x_ped[2]
vTy = parent.x_ped[3]
else:
pos = [init_x_rob, init_y_rob]
pos_ = [init_x_ped, init_y_ped]
vx_ind = 5
vy_ind = 6
#vTravellerx = pos[2]
#vTravellery = pos[3]
#vTx = p2_goal[0]-pos[0]
#vTy = p2_goal[1]-pos[1]
vTx = parent.x_rob[2]
vTy = parent.x_rob[3]
#print 'x_rob full = {},{}'.format(parent.x_rob[2],parent.x_rob[3])
#v = np.sqrt(parent.x_ped[2]**2+parent.x_ped[3]**2)
vTx_ = vTx / np.sqrt(vTx**2 + vTy**2) * (v + dv)
vTy_ = vTy / np.sqrt(vTx**2 + vTy**2) * (v + dv)
trajx = []
#trajx.append(pos[0])
trajy = []
#trajy.append(pos[1])
trajx_ = []
#trajx_.append(pos_[0])
trajy_ = []
#trajy_.append(pos_[1])
#print 'vTx_ and t and ang_accel = {}{}{}'.format(vTx_,t,ang_accel)
velx = np.dot(vTx_, np.cos(np.dot(t, ang_accel))) + np.dot(
vTy_, np.sin(np.dot(t, ang_accel)))
vely = -np.dot(vTx_, np.sin(np.dot(t, ang_accel))) + np.dot(
vTy_, np.cos(np.dot(t, ang_accel)))
for j in range(0, len(t)):
pos[0] = pos[0] + velx[j] * dt
pos[1] = pos[1] + vely[j] * dt
s_ = np.cross([velx[j], vely[j]],
np.add([pos_[0], pos_[1]], [-pos[0], -pos[1]]))
the_ = np.sign(s_) * np.pi / 2
pos_[0] = pos[0] + np.dot([np.cos(the_), -np.sin(the_)],
[velx[j], vely[j]]) * sm
pos_[1] = pos[1] + np.dot(
[np.sin(the_), np.cos(the_)], [velx[j], vely[j]]) * sm
trajx.append(pos[0])
trajy.append(pos[1])
trajx_.append(pos_[0])
trajy_.append(pos_[1])
#plt.cla()
#plt.axis('equal')
x_min = -10
x_max = 10
y_min = 5
y_max = 20
#plt.xlim((x_min, x_max))
#plt.ylim((y_min, y_max))
#plt.grid(True)
#plt.autoscale(False)
#plt.plot(self.ped_pos[0],self.ped_pos[1],'ok')
#plt.plot(self.rob_pos[0],self.rob_pos[1],'om')
#if len(self.Xtraj)>0 :
# plt.plot(self.Xtraj,self.Ytraj,'r')
# plt.plot(xtraj,ytraj,'g')
# plt.pause(0.1)
time = np.add(t, parent.end_time + dt)
action.trajt = time
if dv < 0:
action.intent = 1
else:
action.intent = 0
if agentID == 1:
action.human_trajx = trajx
action.human_trajy = trajy
action.human_velx = velx
action.human_vely = vely
action.robot_trajx = trajx_
action.robot_trajy = trajy_
action.robot_velx = velx
action.robot_vely = vely
x_ped_ = []
x_ped_.append(pos[0])
x_ped_.append(pos[1])
x_ped_.append(velx[-1])
x_ped_.append(vely[-1])
x_rob_ = []
x_rob_.append(pos_[0])
x_rob_.append(pos_[1])
x_rob_.append(velx[-1])
x_rob_.append(vely[-1])
action.x_rob = x_rob_
action.x_ped = x_ped_
action.robot_angz = [ang_accel for k in range(len(t))]
else:
action.robot_trajx = trajx
action.robot_trajy = trajy
action.robot_velx = velx
action.robot_vely = vely
action.human_trajx = trajx_
action.human_trajy = trajy_
action.human_velx = velx
action.human_vely = vely
x_ped_ = []
x_ped_.append(pos_[0])
x_ped_.append(pos_[1])
x_ped_.append(velx[-1])
x_ped_.append(vely[-1])
x_rob_ = []
x_rob_.append(pos[0])
x_rob_.append(pos[1])
x_rob_.append(velx[-1])
x_rob_.append(vely[-1])
action.x_rob = x_rob_
action.x_ped = x_ped_
action.robot_angz = [ang_accel for k in range(len(t))]
Actions.append(action)
#rotating actions
ang_del = [np.pi / 1.3, -np.pi / 1.3]
for ang in ang_del:
action = Nodes(parent, parent.end_time,
parent.end_time + traj_duration, [], [], [], [],
parent.theta_new)
if (agentID == 1):
pos = [init_x_ped, init_y_ped]
pos_ = [init_x_rob, init_y_rob]
vTx = parent.x_ped[2]
vTy = parent.x_ped[3]
else:
pos = [init_x_rob, init_y_rob]
pos_ = [init_x_ped, init_y_ped]
vTx = parent.x_rob[2]
vTy = parent.x_rob[3]
vTx_ = (vTx * np.cos(ang) - vTy * np.sin(ang)) / np.sqrt(vTx**2 +
vTy**2) * 0.02
vTy_ = (vTx * np.sin(ang) + vTy * np.cos(ang)) / np.sqrt(vTx**2 +
vTy**2) * 0.02
velx = np.dot(t, vTx_)
vely = np.dot(t, vTy_)
trajx = [vTx_ * dt * i for i in range(0, len(t))]
trajy = [vTy_ * dt * i for i in range(0, len(t))]
s_ = np.cross([vTx_, vTy_],
np.add([pos_[0], pos_[1]], [-pos[0], -pos[1]]))
the_ = np.sign(s_) * np.pi / 2
trayx_ = np.add(
trajx,
np.dot([np.cos(the_), -np.sin(the_)], [vTx_, vTy_]) * sm)
trayy_ = np.add(
trajy,
np.dot([np.sin(the_), np.cos(the_)], [vTx_, vTy_]) * sm)
time = np.add(t, parent.end_time + dt)
action.trajt = time
action.intent = -1
if agentID == 1:
action.human_trajx = trajx
action.human_trajy = trajy
action.human_velx = velx
action.human_vely = vely
action.robot_trajx = trajx_
action.robot_trajy = trajy_
action.robot_velx = velx
action.robot_vely = vely
x_ped_ = []
x_ped_.append(trajx[-1])
x_ped_.append(trajy[-1])
x_ped_.append(velx[-1])
x_ped_.append(vely[-1])
x_rob_ = []
x_rob_.append(trajx_[0])
x_rob_.append(trajy_[1])
x_rob_.append(velx[-1])
x_rob_.append(vely[-1])
action.x_rob = x_rob_
action.x_ped = x_ped_
action.robot_angz = [ang for k in range(len(t))]
else:
action.robot_trajx = trajx
action.robot_trajy = trajy
action.robot_velx = velx
action.robot_vely = vely
action.human_trajx = trajx_
action.human_trajy = trajy_
action.human_velx = velx
action.human_vely = vely
x_ped_ = []
x_ped_.append(trajx_[0])
x_ped_.append(trajy_[1])
x_ped_.append(velx[-1])
x_ped_.append(vely[-1])
x_rob_ = []
x_rob_.append(trajx[0])
x_rob_.append(trajy[1])
x_rob_.append(velx[-1])
x_rob_.append(vely[-1])
action.x_rob = x_rob_
action.x_ped = x_ped_
action.robot_angz = [ang for k in range(len(t))]
#print 'x_ped = {}'.format(action.x_ped)
Actions.append(action)
return Actions