def findMinimumPath(self, tree, end_node): # Find the leaves that connect to the end node connectedNodes = [] for i in range(0, np.size(tree, 0)): if tree[i, 5] == 1: connectedNodes.append(i) # Find the path with the shortest distance (could find a different heuristic for choosing which path to go with, # especially because we are going to shorten the path anyway??). Choose shortest after smoothing?? Or choose for # least turns. minIndex = np.argmin(tree[connectedNodes, 3]) minIndex = connectedNodes[minIndex] waypoints = msg_waypoints() waypoints.ned[:, waypoints.num_waypoints] = end_node[0:3] waypoints.num_waypoints += 1 waypoints.ned[:, waypoints.num_waypoints] = tree[minIndex, 0:3] waypoints.num_waypoints += 1 parentNode = int(tree[minIndex, 4]) while parentNode > 0: waypoints.ned[:, waypoints.num_waypoints] = tree[parentNode, 0:3] waypoints.num_waypoints += 1 parentNode = int(tree[parentNode, 4]) waypoints.ned[:, waypoints.num_waypoints] = tree[parentNode, 0:3] waypoints.num_waypoints += 1 # This adds the starting point waypoints2 = msg_waypoints() for i in range(0, waypoints.num_waypoints): waypoints2.ned[:, i] = waypoints.ned[:, waypoints.num_waypoints - i - 1] waypoints.ned = waypoints2.ned return waypoints
def smoothPath(self, path, map): waypoints = msg_waypoints() waypoints.ned[:, waypoints.num_waypoints] = path.ned[:, waypoints. num_waypoints] waypoints.num_waypoints += 1 index = 1 while index < path.num_waypoints - 1: if self.collision(waypoints.ned[:, waypoints.num_waypoints - 1], path.ned[:, index + 1], map): # self.flyablePath(smoothedPath[len(smoothedPath) - 1], path[index + 1], prev_chi, chi): waypoints.ned[:, waypoints.num_waypoints] = path.ned[:, index] waypoints.num_waypoints += 1 index += 1 waypoints.ned[:, waypoints.num_waypoints] = path.ned[:, path.num_waypoints - 1] waypoints.num_waypoints += 1 waypoints.airspeed = path.airspeed # smoothedPath.append(path[len(path) - 1]) # for i in range(0, len(smoothedPath) - 1): # Could add other things to this cost function if wanted # cost += np.sqrt( # (smoothedPath[i].n - smoothedPath[i + 1].n) ** 2 + (smoothedPath[i].e - smoothedPath[i + 1].e) ** 2 + \ # (smoothedPath[i].d - smoothedPath[i + 1].d) ** 2) return waypoints
def __init__(self): self.waypoints = msg_waypoints() self.segmentLength = 550 # standard length of path segments self.waypoints.type = 'dubins' self.dubins_path = dubins_parameters() self.dubins_path.radius = np.inf self.tree_courses = np.array([0.0])
def __init__(self, Ts, time_horizon): # message sent to path follower self.Ts = Ts self.path = msg_path() self.optimize = optimizer(Ts, time_horizon) self.waypoints = msg_waypoints() self.N = time_horizon
def planPath(self, wpp_start, wpp_end, R_min, map): self.segmentLength = 3.5 * R_min #2.5 ?? # desired down position is down position of end node pd = wpp_end.item(2) numSolvedPaths = 0 solvedPaths = [] solvedPathsWeights = [] while numSolvedPaths < 4: # specify start and end nodes from wpp_start and wpp_end # format: N, E, D, chi, cost, parentIndex, connectsToGoalFlag, start_node = np.array([ wpp_start.item(0), wpp_start.item(1), pd, self.mod(wpp_start.item(3)), 0, -1, 0 ]) end_node = np.array([ wpp_end.item(0), wpp_end.item(1), pd, self.mod(wpp_end.item(3)), 0, 0, 0 ]) # establish tree starting with the start node tree = np.array([start_node]) # check to see if start_node connects directly to end_node collision, weight = self.collision(start_node, end_node, map, R_min) if ((np.linalg.norm(start_node[0:3] - end_node[0:3]) < self.segmentLength) and np.linalg.norm(start_node[0:3] - end_node[0:3]) >= 3. * R_min and not collision): waypointsPath = msg_waypoints() waypointsPath.ned[:, waypointsPath.num_waypoints] = end_node[0:3] waypointsPath.airspeed[ waypointsPath.num_waypoints] = end_node[4] waypointsPath.course[waypointsPath.num_waypoints] = end_node[3] waypointsPath.num_waypoints = 1 return waypointsPath else: numPaths = 0 while numPaths < 1: #?? Change to do full trees multiple times. Then pick best path. Need to calculate dubin cost though to compare. tree, flag = self.extendTree(tree, end_node, R_min, map, pd) numPaths = numPaths + flag # find path with minimum cost to end_node if flag == 2: continue minPath = self.findMinimumPath(tree, end_node) smoothPath, newWeight = self.smoothPath(minPath, map, R_min) smoothPath2, newWeight2 = self.smoothPath(smoothPath, map, R_min) solvedPaths.append(smoothPath2) solvedPathsWeights.append(newWeight2) numSolvedPaths += 1 minIndex = solvedPathsWeights.index(min(solvedPathsWeights)) return solvedPaths[minIndex]
def __init__(self,map,world_view,initial_position): # waypoints definition self.waypoints = msg_waypoints() self.end = np.array([PLAN.city_width/2.0, PLAN.city_width/2.0, P.pd0])#, p.u0]) self.start = np.array([-PLAN.city_width, -PLAN.city_width, P.pd0])#, P.u0]) self.map = map self.rrt = planRRT(map,world_view) self.initial_position = initial_position
def __init__(self, map): self.waypoints = msg_waypoints() #segment length must be greater than 3*R_min (right now 3*175) for dubins path # I am just going to do two plus a little and hope that it works self.segmentLength = 400 # standard length of path segments #keep in mind the size of the buildings. Right now they are 72 wide self.min_distance = 100 # minimum distance from building center to path self.complete_path = [] self.reqPaths = 3
def createWaypoints(self, path, wp_type): waypoints = msg_waypoints() neds = np.zeros((path.size,3)) courses = np.zeros(path.size) for i in range(path.size): neds[i] = path[i].ned if wp_type == 'dubins': if i == path.size-1: vec = path[i].ned - path[i-1].ned else: vec = path[i+1].ned - path[i].ned courses[i] = np.arctan2(vec[0], vec[1]) waypoints.add_waypoints(wp_type, neds, courses) return waypoints
def smoothPath(self, path, map, R_min): waypoints = msg_waypoints() waypoints.ned[:, waypoints.num_waypoints] = path.ned[:, waypoints. num_waypoints] waypoints.course[:, waypoints.num_waypoints] = path.course[:, waypoints. num_waypoints] waypoints.num_waypoints += 1 index = 1 while index < path.num_waypoints - 1: # chi = np.arctan2((path.ned[1,index + 1] - waypoints.ned[1,waypoints.num_waypoints-1]), # (path.ned[0,index + 1] - waypoints.ned[0,waypoints.num_waypoints-1])) if self.collision(np.concatenate((waypoints.ned[:,waypoints.num_waypoints-1],waypoints.course[:,waypoints.num_waypoints-1]),axis=0), np.concatenate((path.ned[:,index + 1],path.course[:,index + 1]),axis=0), map, R_min) or \ np.linalg.norm(waypoints.ned[:,waypoints.num_waypoints-1] - path.ned[:,index + 1]) <= 2.*R_min: # self.flyablePath(smoothedPath[len(smoothedPath) - 1], path[index + 1], prev_chi, chi): waypoints.ned[:, waypoints.num_waypoints] = path.ned[:, index] waypoints.course[:, waypoints.num_waypoints] = path.course[:, index] waypoints.airspeed[:, waypoints. num_waypoints] = path.airspeed[:, index] waypoints.num_waypoints += 1 index += 1 waypoints.ned[:, waypoints.num_waypoints] = path.ned[:, path.num_waypoints - 1] waypoints.course[:, waypoints. num_waypoints] = path.course[:, path.num_waypoints - 1] waypoints.airspeed[:, waypoints. num_waypoints] = path.airspeed[:, path.num_waypoints - 1] waypoints.num_waypoints += 1 # smoothedPath.append(path[len(path) - 1]) # for i in range(0, len(smoothedPath) - 1): # Could add other things to this cost function if wanted # cost += np.sqrt( # (smoothedPath[i].n - smoothedPath[i + 1].n) ** 2 + (smoothedPath[i].e - smoothedPath[i + 1].e) ** 2 + \ # (smoothedPath[i].d - smoothedPath[i + 1].d) ** 2) return waypoints
def planPath(self, wpp_start, wpp_end, map): # desired down position is down position of end node pd = wpp_end.item(2) # specify start and end nodes from wpp_start and wpp_end # format: N, E, D, cost, parentIndex, connectsToGoalFlag, start_node = np.array( [wpp_start.item(0), wpp_start.item(1), pd, 0, -1, 0]) end_node = np.array([wpp_end.item(0), wpp_end.item(1), pd, 0, 0, 0]) # establish tree starting with the start node tree = np.array([start_node]) # check to see if start_node connects directly to end_node if ((np.linalg.norm(start_node[0:3] - end_node[0:3]) < self.segmentLength) and not self.collision(start_node, end_node, map)): waypointsPath = msg_waypoints() waypointsPath.ned[:, 0:2] = np.append([start_node[0:3]], [end_node[0:3]], axis=0).T # waypointsPath.airspeed[0:2] = np.append(start_node[3],end_node[3],axis=0) # np.append(tree, newNode, axis=0) waypointsPath.num_waypoints = 2 return waypointsPath else: numPaths = 0 while numPaths < 3: tree, flag = self.extendTree(tree, end_node, map, pd) numPaths = numPaths + flag # find path with minimum cost to end_node path = self.findMinimumPath(tree, end_node) return self.smoothPath(path, map)
from chap2.video_writer import video_writer video = video_writer(video_name="chap11_video.avi", bounding_box=(0, 0, 1000, 1000), output_rate=SIM.ts_video) # initialize elements of the architecture wind = wind_simulation(SIM.ts_simulation) mav = mav_dynamics(SIM.ts_simulation) ctrl = autopilot(SIM.ts_simulation) obsv = observer(SIM.ts_simulation) path_follow = path_follower() path_manage = path_manager() # waypoint definition from message_types.msg_waypoints import msg_waypoints waypoints = msg_waypoints() #waypoints.type = 'straight_line' #waypoints.type = 'fillet' waypoints.type = 'dubins' waypoints.num_waypoints = 4 Va = PLAN.Va0 waypoints.ned[:, 0:waypoints.num_waypoints] \ = np.array([[0, 0, -100], [1000, 0, -100], [0, 1000, -100], [1000, 1000, -100]]).T waypoints.airspeed[:, 0:waypoints.num_waypoints] \ = np.array([[Va, Va, Va, Va]]) waypoints.course[:, 0:waypoints.num_waypoints] \ = np.array([[np.radians(0), np.radians(45),
def __init__(self): self.waypoints = msg_waypoints() self.segmentLength = 200 # standard length of path segments
def __init__(self): # waypoints definition self.waypoints = msg_waypoints() self.rrt = planRRT()
def __init__(self): # waypoints definition self.waypoints = msg_waypoints()
def main(): if DATA: screen_pos = [2000, 0] # x, y position on screen data_view = data_viewer(*screen_pos) # initialize view of data plots # Holodeck Setup env = holodeck.make("Ocean", show_viewport=True) env.reset() # wave intensity: 1-13, wave size: 1-8, wave dir: 0-360 deg env.set_ocean_state(6, 3, 90) env.set_day_time(5) env.set_weather('Cloudy') env.set_aruco_code(False) alt = -PLAN.MAV.pd0*100 if DEBUG_HOLO: env.set_control_scheme("uav0", ControlSchemes.UAV_ROLL_PITCH_YAW_RATE_ALT) uav_cmd = np.array([0, -0.2, 0, alt]) uav_cmd = np.array([0,0,0,0]) boat_cmd = 0 env.act("uav0", uav_cmd) env.act("boat0", boat_cmd) # Just for getting UAV off the platform state = env.set_state("uav0", [-1500, 0, 6000], [0,0,0], [0,0,0], [0,0,0])["uav0"] state = env.set_state("uav0", [-1500, 0, 2000], [0,0,0], [0,0,0], [0,0,0])["uav0"] env.teleport("boat0", location=[3500, 0, 0], rotation=[0, 0, 0]) # Initialization pos = np.array([OFFSET_N, OFFSET_E, alt]) att = np.array([0, 0, 0]) vel = np.array([0, 0, 0]) * 100 angvel = np.array([0, 0, 0]) state = env.set_state("uav0", pos, att, vel, angvel)["uav0"] # Camera cam_alt = 300 env.teleport_camera([0, 0, cam_alt], [0, 0, -0.5]) if DEBUG_HOLO: pos = state["LocationSensor"] # env.teleport_camera([0, 0, cam_alt], [0, 0, -0.5]) for i in range(1): state = env.tick()["uav0"] pos = state["LocationSensor"] # env.teleport_camera([0, 0, cam_alt], [0, 0, -0.5]) if SHOW_PIXELS: pixels = state["RGBCamera"] cv2.imshow('Camera', pixels) cv2.waitKey(0) print("READY FOR SIM") # sys.exit(0) # initialize elements of the architecture wind = wind_simulation(SIM.ts_simulation) mav = mav_dynamics(SIM.ts_simulation) ctrl = autopilot(SIM.ts_simulation) obsv = observer(SIM.ts_simulation) path_follow = path_follower() path_manage = path_manager() # waypoint definition waypoints = msg_waypoints() waypoints.type = 'straight_line' waypoints.type = 'fillet' waypoints.type = 'dubins' waypoints.num_waypoints = 4 Va = PLAN.Va0 d = 1000.0 h = -PLAN.MAV.pd0 waypoints.ned[:waypoints.num_waypoints] = np.array([[0, 0, -h], [d, 0, -h], [0, d, -h], [d, d, -h]]) waypoints.airspeed[:waypoints.num_waypoints] = np.array([Va, Va, Va, Va]) waypoints.course[:waypoints.num_waypoints] = np.array([np.radians(0), np.radians(45), np.radians(45), np.radians(-135)]) # initialize the simulation time sim_time = SIM.start_time delta = np.zeros(4) mav.update(delta) # propagate the MAV dynamics mav.update_sensors() # main simulation loop # print("Waiting for keypress") # input() print("Press Q to exit...") while sim_time < SIM.end_time: #-------observer------------- measurements = mav.update_sensors() # get sensor measurements # estimate states from measurements estimated_state = obsv.update(measurements) #-------path manager------------- path = path_manage.update(waypoints, PLAN.R_min, estimated_state) #-------path follower------------- autopilot_commands = path_follow.update(path, estimated_state) # autopilot_commands = path_follow.update(path, mav.true_state) #-------controller------------- delta, commanded_state = ctrl.update(autopilot_commands, estimated_state) #-------physical system------j------- current_wind = wind.update() # get the new wind vector mav.update(delta, current_wind) # propagate the MAV dynamics #-------update holodeck------------- update_holodeck(env, mav.true_state) draw_holodeck(env, waypoints) #-------update viewer------------- if DATA: data_view.update(mav.true_state, # true states estimated_state, # estimated states commanded_state, # commanded states SIM.ts_simulation) #-------increment time------------- sim_time += SIM.ts_simulation
def smoothPath(self, path, map, R_min): waypoints = msg_waypoints() waypoints.ned[:, waypoints.num_waypoints] = path.ned[:, waypoints. num_waypoints] waypoints.course[:, waypoints.num_waypoints] = path.course[:, waypoints. num_waypoints] waypoints.num_waypoints += 1 weight = 0 index = 1 while index < path.num_waypoints - 1: collision, pathWeight = self.collision( np.concatenate( (waypoints.ned[:, waypoints.num_waypoints - 1], waypoints.course[:, waypoints.num_waypoints - 1]), axis=0), np.concatenate( (path.ned[:, index + 1], path.course[:, index + 1]), axis=0), map, R_min) if collision or \ np.linalg.norm(waypoints.ned[:,waypoints.num_waypoints-1] - path.ned[:,index + 1]) <= 2.*R_min: # self.flyablePath(smoothedPath[len(smoothedPath) - 1], path[index + 1], prev_chi, chi): waypoints.ned[:, waypoints.num_waypoints] = path.ned[:, index] waypoints.course[:, waypoints.num_waypoints] = path.course[:, index] waypoints.airspeed[:, waypoints. num_waypoints] = path.airspeed[:, index] weight += pathWeight waypoints.num_waypoints += 1 index += 1 collision, pathWeight = self.collision( np.concatenate((waypoints.ned[:, waypoints.num_waypoints - 1], waypoints.course[:, waypoints.num_waypoints - 1]), axis=0), np.concatenate((path.ned[:, index], path.course[:, index]), axis=0), map, R_min) waypoints.ned[:, waypoints.num_waypoints] = path.ned[:, path.num_waypoints - 1] waypoints.course[:, waypoints. num_waypoints] = path.course[:, path.num_waypoints - 1] waypoints.airspeed[:, waypoints. num_waypoints] = path.airspeed[:, path.num_waypoints - 1] weight += pathWeight waypoints.num_waypoints += 1 # #Loop through again, but jumping two # waypoints2 = msg_waypoints() # waypoints2.ned[:, waypoints.num_waypoints] = waypoints.ned[:, waypoints.num_waypoints] # waypoints2.course[:, waypoints.num_waypoints] = waypoints.course[:, waypoints.num_waypoints] # waypoints2.num_waypoints += 1 # weight = 0 # index = 2 # while index < waypoints.num_waypoints - 1: # collision, pathWeight = self.collision(np.concatenate( # (waypoints2.ned[:, waypoints2.num_waypoints - 1], waypoints2.course[:, waypoints2.num_waypoints - 1]), # axis=0), np.concatenate((waypoints.ned[:, index + 1], waypoints.course[:, index + 1]), axis=0), map, R_min) # if collision or \ # np.linalg.norm( # waypoints2.ned[:, waypoints2.num_waypoints - 1] - waypoints.ned[:, index + 1]) <= 2. * R_min: # # self.flyablePath(smoothedPath[len(smoothedPath) - 1], path[index + 1], prev_chi, chi): # waypoints2.ned[:, waypoints2.num_waypoints] = waypoints.ned[:, index] # waypoints2.course[:, waypoints2.num_waypoints] = waypoints.course[:, index] # waypoints2.airspeed[:, waypoints2.num_waypoints] = waypoints.airspeed[:, index] # weight += pathWeight # waypoints2.num_waypoints += 1 # index += 2 # collision, pathWeight = self.collision(np.concatenate( # (waypoints2.ned[:, waypoints2.num_waypoints - 1], waypoints2.course[:, waypoints2.num_waypoints - 1]), axis=0), # np.concatenate((waypoints.ned[:, index], waypoints.course[:, index]), # axis=0), map, R_min) # waypoints2.ned[:, waypoints.num_waypoints] = waypoints.ned[:, waypoints.num_waypoints - 1] # waypoints2.course[:, waypoints.num_waypoints] = waypoints.course[:, waypoints.num_waypoints - 1] # waypoints2.airspeed[:, waypoints.num_waypoints] = waypoints.airspeed[:, waypoints.num_waypoints - 1] # weight += pathWeight # waypoints2.num_waypoints += 1 return waypoints, weight
def __init__(self): # waypoints definition self.waypoints = msg_waypoints() self.rrt = planRRT(map) self.rrtDubins = planRRTDubins(map) self.rrtDubinsProj = planRRTDubinsProj(map)
def __init__(self, map): self.map = map self.num_closest_points = 3 self.end = np.array([[PLAN.city_width / 2., PLAN.city_width / 2.]]) start = np.array([[ P.pn0_corner, P.pe0_corner, ]]) start = np.array([np.transpose([start[0, 1], start[0, 0]])]) north = self.map.building_north.reshape((PLAN.num_blocks**2, 1)) east = self.map.building_east.reshape((PLAN.num_blocks**2, 1)) self.points = np.hstack((east, north)) self.E = [] # edges array vor = Voronoi(self.points) self.ridge = vor.ridge_vertices self.V = np.concatenate((vor.vertices, start, self.end), axis=0) num_vertices = len(vor.vertices) self.ridge_points = vor.ridge_points # debugging #voronoi_plot_2d(vor,line_colors='red',show_vertices=True) #plt.show() for vpair in self.ridge: if vpair[0] >= 0 and vpair[1] >= 0: v0 = vor.vertices[vpair[0]] v1 = vor.vertices[vpair[1]] self.E.append([[v0[0], v0[1]], [v1[0], v1[1]]]) if num_vertices < self.num_closest_points: self.num_closest_points = num_vertices closest = np.ones((self.num_closest_points, 2), dtype=float) closest *= PLAN.city_width * 1e90 for ii in range(num_vertices): distance = VT.calcDistance(vor.vertices[ii], start[0]) if distance < np.amax(closest[:, 0]): closest[np.argmax(closest[:, 0]), 1] = ii closest[np.argmax(closest[:, 0]), 0] = distance start_index = num_vertices for ii in range(self.num_closest_points): closest_index = int(closest[ii, 1]) self.ridge.append([closest_index, start_index]) self.E.append([[ vor.vertices[int(closest[ii, 1]), 0], vor.vertices[int(closest[ii, 1]), 1] ], [start[0][0], start[0][1]]]) closest = np.ones((self.num_closest_points, 2), dtype=float) closest *= PLAN.city_width * 1e9 for ii in range(num_vertices): distance = VT.calcDistance(vor.vertices[ii], self.end[0]) if distance < np.amax(closest[:, 0]): closest[np.argmax(closest[:, 0]), 1] = ii closest[np.argmax(closest[:, 0]), 0] = distance end_index = num_vertices + 1 for ii in range(self.num_closest_points): closest_index = int(closest[ii, 1]) self.ridge.append([closest_index, end_index]) self.E.append([[ vor.vertices[int(closest[ii, 1]), 0], vor.vertices[int(closest[ii, 1]), 1] ], [self.end[0][0], self.end[0][1]]]) #self.E.append([[start[0][0],start[0][1]],[self.end[0][0],self.end[0][1]]]) self.E.append([[start[0][0], start[0][1]], [start[0][0], start[0][1]]]) self.E = np.asarray(self.E) self.E_inf = [] center = self.points.mean(axis=0) for pointidx, simplex in zip(self.ridge_points, self.ridge): simplex = np.asarray(simplex) if np.any(simplex < 0): i = simplex[simplex >= 0][0] # finite end Voronoi vertex t = self.points[pointidx[1]] - self.points[ pointidx[0]] # tangent t = t / np.linalg.norm(t) n = np.array([-t[1], t[0]]) # normal midpoint = self.points[pointidx].mean(axis=0) far_point = vor.vertices[i] + np.sign( np.dot(midpoint - center, n)) * n * PLAN.city_width * 10.0 self.E_inf.append([[vor.vertices[i, 0], vor.vertices[i, 1]], [far_point[0], far_point[1]]]) #plt.plot([vor.vertices[i,0], far_point[0]],[vor.vertices[i,1], far_point[1]], 'g--') self.E_inf = np.asarray(self.E_inf) # assign weight weight = np.zeros((self.E.shape[0], 1)) self.ridge = list(filter(lambda x: x[0] >= 0, self.ridge)) for ii in range(len(self.ridge)): D_prime = np.zeros((self.points.shape[0], 1)) for jj in range(self.points.shape[0]): sigma_star = VT.calcSigmaStar(self.points[jj], self.E[ii, 0, :], self.E[ii, 1, :]) if sigma_star < 0.0: D_prime[jj] = np.linalg.norm(self.points[jj] - self.E[ii, 0, :]) elif sigma_star > 1.0: D_prime[jj] = np.linalg.norm(self.points[jj] - self.E[ii, 1, :]) else: D_prime[jj] = VT.calcWeight(self.points[jj], self.E[ii, 0, :], self.E[ii, 1, :]) weight[ii] = VT.calcCost(self.E[ii, 0, :], self.E[ii, 1, :], np.amin(D_prime)) if np.amin(D_prime) < 0: print(np.amin(D_prime)) path = VT.dijkstraSearch(self.V, self.ridge, weight) ''' path_weight = np.zeros((len(path),1)) for mm in range(len(path)): path_weight[mm] = weight[mm] print(path_weight) ''' self.path_pts = [] for ii in range(len(path) - 1): for jj in range(len(self.ridge)): if path[ii] < path[ii + 1]: a = path[ii] b = path[ii + 1] else: a = path[ii + 1] b = path[ii] if [a, b] == self.ridge[jj]: self.path_pts.append(self.E[jj, :, :]) break self.path_pts = np.asarray(self.path_pts) #return self.V[path[1]],start,np.amax(weight) # waypoints return message self.waypoints = msg_waypoints() self.waypoints.type = 'fillet' self.waypoints.num_waypoints = len(path) Va = PLAN.Va0 self.waypoints.airspeed[:, 0:self.waypoints.num_waypoints] \ = Va*np.ones((1,self.waypoints.num_waypoints)) path_plan = np.zeros((3, len(path))) for ii in range(len(path)): path_plan[:, ii] = [ self.V[path[ii]].item(1), self.V[path[ii]].item(0), -100. ] self.waypoints.ned[:, 0:self.waypoints.num_waypoints] \ = path_plan course_plan = np.zeros((1, len(path))) for ii in range(0, len(path) - 1): #print(self.waypoints.ned[1,ii+1]-self.waypoints.ned[1,ii]) #print(np.arctan2(self.waypoints.ned[1,ii+1]-self.waypoints.ned[1,ii],self.waypoints.ned[0,ii+1]-self.waypoints.ned[0,ii])) course_plan[0, ii] = np.arctan2( self.waypoints.ned[1, ii + 1] - self.waypoints.ned[1, ii], self.waypoints.ned[0, ii + 1] - self.waypoints.ned[0, ii]) self.waypoints.course[:, 0:self.waypoints.num_waypoints] \ = course_plan