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 FullKnowledgeCollaborativePlanner(x1, x2, dt, traj_duration, weight, agentID, H, p1_goal, p2_goal, sm, v, ww, costmap, costmap0s, cthr, cres, rr, r_m2o, t_m2o): path_robot = [] path_fScore = [] path_intent = [] path_found = False initial_node = Nodes() initial_node.x_rob = x2 initial_node.x_ped = x1 initial_node.gScore_ = [0, 0] #initial_node.theta = theta_hat #initial_node.theta_new = theta_hat gScore = [] gScore.append(0) gScore_ = [] fScore = [] fScore.append(np.Inf) totalNodes = [] totalNodes.append(initial_node) closeSet = [] openSet = [] openSet.append(0) cameFrom = [] cameFrom.append(-1) rob_hScore = np.Inf min_dist = sm - 0.1 min_vel = 0.3 x_rob = initial_node.x_rob x_ped = initial_node.x_ped pathx = [] pathx_ = [] pathy = [] pathy_ = [] rob_s_angz = [] current = [] #plt.cla() while len(openSet) > 0: 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.intent ==-1: # print 'current velocity = {}'.format(current.x_ped) #print 'cn_node_id = {}'.format(current_node_id) #print 'cn node x_rob = {}'.format(current.x_rob) #print 'cn_node x_ped = {}'.format(current.x_ped) #print 'cn_node trajx = {}'.format(current.robot_trajx) #print 'cn_node trajy = {}'.format(current.robot_trajy) if (current.end_time > (traj_duration * H)) or (rob_hScore < 0.2): pathx, pathy, pathx_, pathy_, rob_s_angz, path_intent = ReconstructPathFromNode( cameFrom, current_node_id, totalNodes, agentID) #print 'path vel = {}'.format(np.sqrt((pathx[0]-pathx[1])**2+(pathy[0]-pathy[1])**2)/dt) path_found = True path_fScore = min_fScore #print 'path found' #print 'pathx_ = {}'.format(pathx_) #print 'pathy_ = {}'.format(pathy_) break #print 'openSet = {}'.format(openSet) #print 'fScore = {}'.format(fScore) print 'openSet = {}'.format(openSet) print 'current_node_id = {}'.format(current_node_id) openSet.pop(openSet.index(current_node_id)) closeSet.append(current_node_id) ang_num = 2 sp_num = 1 neighbors = SideBySideActionSampling(current, dt, traj_duration, agentID, v, sm, p1_goal, p2_goal, ang_num, sp_num) ped_intent_real = 2 ped_travel_cost = traj_duration rob_travel_cost = traj_duration #print 'len neighbors = {}'.format(len(neighbors)) for i in range(len(neighbors)): cn_node = neighbors[i] #if cn_node.intent==-1: # print 'velocity = {}'.format(cn_node.x_ped) #collision1 = CollisionCheck([cn_node.human_trajx,cn_node.human_trajy],[cn_node.robot_trajx,cn_node.robot_trajy],min_dist-0.02,min_vel) #collision2 = MapCollisionCheck(cn_node) # TODO collision1 = Check(cn_node.robot_trajx, cn_node.robot_trajy, costmap, costmap0s, cthr, cres, rr) #collision1 = Check(cn_node.robot_trajx,cn_node.robot_trajy,costmap,costmap0s,cthr,cres,rr,r_m2o,t_m2o) collision2 = Check(cn_node.human_trajx, cn_node.human_trajy, costmap, costmap0s, cthr, cres, rr) if collision1 or collision2: continue cn_node_id = len(totalNodes) #cn_node.ped_intent_history.append(current.ped_intent_history) #cn_node.ped_intent_history.append(ped_intent_real) #print 'current x_ped = {}'.format(current.x_ped) #print 'len cn node x ped, x_rob = {},{}'.format(cn_node.x_ped,cn_node.x_rob) #print 'x ped = {}'.format(x_ped) human_velx = cn_node.x_ped[2] human_vely = cn_node.x_ped[3] human_vel = np.sqrt(human_velx**2 + human_vely**2) gScore_new = np.add(current.gScore_, [rob_travel_cost, ped_travel_cost]) gScore_new = np.add(gScore_new, [0, ww * ((human_vel - v)**2)]) cn_node.gScore_ = gScore_new gScore.append( np.dot(weight, gScore_new) + np.random.normal() * 0.01) #add noise to avoid floating error cameFrom.append(current_node_id) x_ped_ = cn_node.x_ped pHx = x_ped_[0] pHy = x_ped_[1] x_rob_ = cn_node.x_rob pRx = x_rob_[0] pRy = x_rob_[1] ped_del = np.add([ pHx + x_ped_[2] * traj_duration, pHy + x_ped_[3] * traj_duration ], [-p1_goal[0], -p1_goal[1]]) rob_del = np.add([ pRx + x_rob_[2] * traj_duration, pRy + x_rob_[3] * traj_duration ], [-p2_goal[0], -p2_goal[1]]) ped_hScore = (np.sqrt(np.dot(ped_del, ped_del)) + human_vel * traj_duration) / v rob_hScore = ( np.sqrt(np.dot(rob_del, rob_del)) + np.sqrt(x_rob_[2]**2 + x_rob_[3]**2) * traj_duration) / v hScore = np.dot(weight, [rob_hScore, ped_hScore]) fScore.append(gScore[-1] + hScore) openSet.append(cn_node_id) totalNodes.append(cn_node) #print 'robot_trajx = {},{}'.format(cn_node_id,cn_node.robot_trajx) #print 'node end time = {}'.format(cn_node.end_time) #print 'robot_trajx = {}{}'.format(ccn_node.robot_trajx[0]mcn_node.robot_trajx[-1]) #plt.plot(cn_node.robot_trajx,cn_node.robot_trajx,'*r') #plt.plot(cn_node.human_trajy,cn_node.human_trajy,'*g') #plt.pause(0.1) if not path_found: print 'fk path not found after openSet cleared' print 'total node length = {}'.format(len(totalNodes)) #else : #print 'gScore = {}'.format(gScore_new) return path_found, pathx, pathy, pathx_, pathy_, rob_s_angz, path_fScore, current
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
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