def plot_path(self, V, resolution=np.pi / 24, **kwargs): pts = [] for i in range(len(V) - 1): pts.extend( path_sample(V[i], V[i + 1], self.turning_radius, self.turning_radius * resolution)[0]) plt.plot([x for x, y, th in pts], [y for x, y, th in pts], **kwargs)
def plan(self, turnRadius): q0 = (self.currentPos['x'], self.currentPos['y'], self.currentOrientation['yaw']) q1 = (self.currentTarget.pos['x'], self.currentTarget.pos['y'], self.currentTarget.orientation['yaw']) self.wayPoints, T = dubins.path_sample(q0, q1, turnRadius, self.sampleTime) self.wayOrientations = [] self.wayRadius = [] self.wayDotRadius = [] self.wayDotOrientation = [] for t in range(len(self.wayPoints) - 1): deltaT = T[t + 1] - T[t] deltaX = self.wayPoints[t + 1][0] - self.wayPoints[t][0] deltaY = self.wayPoints[t + 1][1] - self.wayPoints[t][1] orientation = np.arctan2(deltaY, deltaX) self.wayOrientations.append(orientation) self.wayOrientations.append(self.currentTarget.orientation['yaw']) for p in self.wayPoints: radius = p[0]**2 + p[1]**2 self.wayRadius.append(radius) ''' self.wayDotRadius.append(0.0) self.wayDotOrientation.append(0.0) for t in range(len(self.wayPoints)-1): dotRadius = (self.wayRadius[t+1]-self.wayRadius[t])/(T[t+1]-T[t]) dotOrientation = (self.wayOrientations[t+1]-self.wayOrientations[t])/(T[t+1]-T[t]) self.wayDotRadius.append(dotRadius) self.wayDotOrientation.append(dotOrientation) ''' self.tIdx = 1
def steer_towards_forward(self, x1, x2, eps): ########## Code starts here ########## """ from dubins import path_sample, path_length #div eps by 10 b/c 10 steps, unless premature ending configs = path_sample(x1,x2,1.001*self.turning_radius,eps/10) if len(configs[0]) < 10: x_new = np.array(configs[0][len(configs[0])-1]) else: x_new = np.array(configs[0][9]) #now cover the condition if we are within eps of goal so we can actually reach it if path_length(x_new,x1,self.turning_radius) < eps: return x2 else: return x_new """ from dubins import path_sample, path_length configs = path_sample(x1, x2, 1.001 * self.turning_radius, eps) if len(configs[0]) == 0: return x2 if len(configs[0]) < 2: x_new = np.array(configs[0][0]) else: x_new = np.array(configs[0][1]) #now cover the condition if we are within eps of goal so we can actually reach it if path_length(x_new, x1, self.turning_radius) < eps: return x2 else: return x_new
def interpolate(self, s1, s2): """Returns a sequence of states between two states given a motion that connects the two states. We use a shooting method type approach to interpolate between the two states @param: s1 - parent state @param: s2 - child state @return edge - sequence of states (including end points) """ step_size = self.path_resolution s1_n = (s1[0], s1[1], angles.normalize_angle_positive(s1[2])) s2_n = (s2[0], s2[1], angles.normalize_angle_positive(s2[2])) edge, _ = dubins.path_sample(s1, s2, self.radius - 0.01, step_size) #Dubins returns edge in [0, 2pi], we got to normalize it to (-pi, pi] for our code normalized_edge = [] for config in edge: new_config = (config[0], config[1], angles.normalize_angle(config[2])) normalized_edge.append(new_config) normalized_edge.append(tuple(s2)) return normalized_edge
def steer_towards_forward(self, x1, x2, eps): ########## Code starts here ########## dist = path_length(x1, x2, self.turning_radius) if dist < eps: return x2 else: return path_sample(x1, x2, 1.001 * self.turning_radius, eps)[0][1]
def car_control(db, rabbit_db, rabbit_db_lock, e_state=None): global car_movement global end_state for car in db: curr_car = db[car] curr_state = CarState((float(curr_car['x']), float(curr_car['y'])), float(curr_car['angle']), float(curr_car['time'])) if end_state != e_state: #if car not in car_movement: """end_state = CarState((float(curr_car['end_x']), float(curr_car['end_y'])), float(curr_car['end_angle']), float(curr_car['time']))""" end_state = e_state DELTA_SAMPLE = 0.1 path, time = dubins.path_sample(curr_state.get_xy_angle(), end_state.get_xy_angle(), TURNING_RADIUS, DELTA_SAMPLE) car_movement[car] = CarMovementFollowPath(path, DELTA_SAMPLE) car_movement[car].do_next_step(curr_state, car, rabbit_db, rabbit_db_lock)
def plot_path(self, resolution = np.pi/24, **kwargs): from dubins import path_sample pts = [] path = np.array(self.path) for i in range(path.shape[0] - 1): pts.extend(path_sample(path[i], path[i+1], self.turning_radius, self.turning_radius*resolution)[0]) plt.plot([x for x, y, th in pts], [y for x, y, th in pts], **kwargs)
def steer_towards_forward(self, x1, x2, eps): ########## Code starts here ########## from dubins import path_sample pts = path_sample(x1, x2, 1.001 * self.turning_radius, eps)[0] if len(pts) > 1: return np.array(pts[1]) else: return x2
def is_free_motion(self, obstacles, x1, x2, resolution = np.pi/6): pts = path_sample(x1, x2, self.turning_radius, self.turning_radius*resolution)[0] pts.append(x2) for i in range(len(pts) - 1): for line in obstacles: if line_line_intersection([pts[i][:2], pts[i+1][:2]], line): return False return True
def steer_towards_forward(self, x1, x2, eps): ########## Code starts here ########## d = path_length(x1, x2, self.turning_radius) if d > eps: pts = path_sample(x1, x2, 1.001 * self.turning_radius, eps)[0] return pts[1] else: return x2
def steer_towards_backward(self, x1, x2, eps): ########## Code starts here ########## from dubins import path_sample pts = path_sample(self.reverse_heading(x2), self.reverse_heading(x1), 1.001 * self.turning_radius, eps)[0] if len(pts) > 1: return self.reverse_heading(pts[1]) else: return x1
def steer_towards_backward(self, x1, x2, eps): ########## Code starts here ########## x1_rev = self.reverse_heading(x1) x2_rev = self.reverse_heading(x2) point_rev = x1_rev if self.distance( x2_rev, x1_rev) < eps else np.array( path_sample(x2_rev, x1_rev, 1.001 * self.turning_radius, eps)[0][1]) return self.reverse_heading(point_rev)
def plot_tree(self, V, P, resolution = np.pi/24, **kwargs): line_segments = [] for i in range(V.shape[0]): if P[i] >= 0: pts = path_sample(V[P[i],:], V[i,:], self.turning_radius, self.turning_radius*resolution)[0] pts.append(V[i,:]) for j in range(len(pts) - 1): line_segments.append((pts[j], pts[j+1])) plot_line_segments(line_segments, **kwargs)
def steer_towards_forward(self, x1, x2, eps): from dubins import path_length, path_sample # check if Dubins path length is smaller than eps if path_length(x1, x2, self.turning_radius) < eps: return x2 else: # otherwise, get the path_sample at epsilon distance away (second element of first column from path_sample) pts = path_sample(x1, x2, 1.001 * self.turning_radius, eps)[0] return np.asarray(pts[1])
def steer_towards_forward(self, x1, x2, eps): ########## Code starts here ########## from dubins import path_sample samples = path_sample(x1, x2, 1.001 * self.turning_radius, eps)[0] # Samples always include x1 if len(samples) > 1: return samples[1] else: return x2
def dubinsDist(self,a,b,cfg): turning_radius = cfg['flightPath']['min_turn'] step_size = 5 qs, _ = dubins.path_sample(a, b, turning_radius, step_size) qs = np.asarray(qs) dist = 0 for i in range(len(qs)-1): dist += np.sqrt((qs[i,0]-qs[i+1,0])**2+(qs[i,1]-qs[i+1,1])**2) return dist
def dubins_dist(a, b): turning_radius = 50 step_size = 5 qs, _ = dubins.path_sample(a, b, turning_radius, step_size) qs = np.asarray(qs) dist = 0 for i in range(len(qs) - 1): dist += np.sqrt((qs[i, 0] - qs[i + 1, 0])**2 + (qs[i, 1] - qs[i + 1, 1])**2) return dist
def steer_towards_forward(self, x1, x2, eps): ########## Code starts here ########## rho = 1.001 * self.turning_radius dx = path_length(x1, x2, rho) if dx < eps: x = x2 else: samples, _ = path_sample(x1, x2, rho, eps) x = samples[1] return x
def steer_towards(self, x, y, eps): # TODO: fill me in! # A subtle issue: if you use dubins.path_sample to return the point at distance # eps along the path from x to y, use a turning radius slightly larger than # self.turning_radius (i.e., 1.001*self.turning_radius). Without this hack, # dubins.path_sample might return a point that you can't quite get to in distance # eps (using self.turning_radius) due to numerical precision issues. if path_length(x, y, self.turning_radius) < eps: return y return path_sample(x, y, 1.001 * self.turning_radius, eps)[0][1]
def steer_towards_backward(self, x1, x2, eps): ########## Code starts here ########## dist = path_length(self.reverse_heading(x2), self.reverse_heading(x1), self.turning_radius) if dist < eps: return x1 else: return self.reverse_heading( path_sample(self.reverse_heading(x2), self.reverse_heading(x1), 1.001 * self.turning_radius, eps)[0][1])
def plot_tour_dubins(ax, tour, dict_map, r): """ Function will plot the GTSP tour. :param ax: :param tour: tour :param lines: :param dict_map: """ import math import dubins n = len(tour) for i in range(len(tour)): outgoing_node_idx = tour[i] incoming_node_idx = tour[(i + 1) % n] outgoing_segment, dir_o = dict_map[outgoing_node_idx] incoming_segment, dir_i = dict_map[incoming_node_idx] q0 = outgoing_segment.get_exit_info(dir_o) q1 = incoming_segment.get_entrance_info(dir_i) smpls, _ = dubins.path_sample(q0, q1, r, 0.05) x = [] y = [] xarrow = 0 yarrow = 0 for smpl in smpls: xt, yt, at = smpl x.append(xt) y.append(yt) if len(x) == int(math.floor(len(smpls) / 2)): # Insert arrow mid way through dubins curve xarrow = x[int(math.floor(len(smpls) / 2)) - 2] yarrow = y[int(math.floor(len(smpls) / 2)) - 2] dx = x[int(math.floor(len(smpls) / 2)) - 1] - x[int(math.floor(len(smpls) / 2)) - 2] dy = y[int(math.floor(len(smpls) / 2)) - 1] - y[int(math.floor(len(smpls) / 2)) - 2] ax.scatter(x, y, s=0.4, color='green', zorder=4) #dx = i_pt[0] - o_pt[0] #dy = i_pt[1] - o_pt[1] ax.arrow(xarrow, yarrow, dx, dy, head_width=0.15, ec='green', length_includes_head=True, zorder=4)
def steer_towards_forward(self, x1, x2, eps): ########## Code starts here ########## from dubins import path_length from dubins import path_sample distance = path_length(x1, x2, self.turning_radius) if distance < eps: return x2 else: return path_sample(x1, x2, 1.001 * self.turning_radius, eps)[0][1]
def steer_towards_forward(self, x1, x2, eps): ########## Code starts here ########## from dubins import path_sample from dubins import path_length samples = path_sample(x1, x2, 1.001 * self.turning_radius, eps)[0] if len(samples) > 1: if path_length(x1, x2, self.turning_radius) > path_length( x1, samples[1], self.turning_radius): x2 = samples[1] return x2
def steer_towards(self, x, y, eps): # A subtle issue: if you use dubins.path_sample to return the point at distance # eps along the path from x to y, use a turning radius slightly larger than # self.turning_radius (i.e., 1.001*self.turning_radius). Without this hack, # dubins.path_sample might return a point that you can't quite get to in distance # eps (using self.turning_radius) due to numerical precision issues. dubins_samples = path_sample(x, y, 1.001 * self.turning_radius, eps) if len(dubins_samples[0]) <= 2: return y else: return dubins_samples[0][1]
def steer_towards_backward(self, x1, x2, eps): ########## Code starts here ########## rho = 1.001 * self.turning_radius dx = path_length(x1, x2, rho) if dx < eps: x = x1 else: x1_rev = self.reverse_heading(x1) x2_rev = self.reverse_heading(x2) samples, _ = path_sample(x2_rev, x1_rev, rho, eps) x = self.reverse_heading(samples[1]) return x
def main(): CARS = 10 START_PTS = [] END_PTS = [] for i in xrange(CARS): START_PTS += [(100+i*30, 500, -math.pi/2)] START_PTS += [(100+i*30, 400, -math.pi/2)] END_PTS += [(370-i*30, 100, -math.pi/2)] END_PTS += [(370-i*30, 200, -math.pi/2)] dist_matrix = [] for start_pt in START_PTS: dist_vec = [] for end_pt in END_PTS: dist_vec.append(dubins.path_length(start_pt, end_pt, settings.TURNING_RADIUS)) print dist_vec dist_matrix.append(dist_vec) m = munkres.Munkres() indices = m.compute(dist_matrix) d_pt_match = {} for start_pt_ind, end_pt_ind in indices: d_pt_match[start_pt_ind] = end_pt_ind cars = [] for i in xrange(CARS): cars.append((START_PTS[i], END_PTS[d_pt_match[i]])) print cars[-1] COLLISION_DIST = 5 d_collisions = {} for i, (start_pt, end_pt) in enumerate(cars): d_collisions[i] = set() path, t = dubins.path_sample(start_pt, end_pt, settings.TURNING_RADIUS, 0.1) for j, (temp_start_pt, temp_end_pt) in enumerate(cars): if j == i: continue for pt in path: if norm(sub_vecs(pt, temp_end_pt)) < COLLISION_DIST: d_collisions[i].add(j) print d_collisions cars_order = list(toposort.toposort(d_collisions)) for car_set in cars_order[::-1]: print "Creating cars", car_set for car_ind in car_set: thread.start_new_thread(main_loop, ((cars[car_ind][0][0], cars[car_ind][0][1]), cars[car_ind][0][2], (cars[car_ind][1][0], cars[car_ind][1][1]), cars[car_ind][1][2])) time.sleep(10)
def plot_tour_dubins(ax, tour, dict_map, r, idx=0): """ Function will plot the GTSP tour. :param ax: :param tour: tour :param lines: :param dict_map: """ import math import dubins colors = ["#00FD91", "#1472FD", "#FFA100", "#FF4900"] n = len(tour) for i in range(len(tour)): outgoing_node_idx = tour[i] incoming_node_idx = tour[(i + 1) % n] outgoing_segment, dir_o = dict_map[outgoing_node_idx] incoming_segment, dir_i = dict_map[incoming_node_idx] q0 = outgoing_segment.get_exit_info(dir_o) q1 = incoming_segment.get_entrance_info(dir_i) smpls, _ = dubins.path_sample(q0, q1, r, 0.05) x = [] y = [] xarrow = 0 yarrow = 0 for smpl in smpls: xt, yt, at = smpl x.append(xt) y.append(yt) if len(x) == int(math.floor(len(smpls) / 2)): # Insert arrow mid way through dubins curve xarrow = x[int(math.floor(len(smpls) / 2)) - 2] yarrow = y[int(math.floor(len(smpls) / 2)) - 2] dx = x[int(math.floor(len(smpls) / 2)) - 1] - x[int(math.floor(len(smpls) / 2)) - 2] dy = y[int(math.floor(len(smpls) / 2)) - 1] - y[int(math.floor(len(smpls) / 2)) - 2] #ax.scatter(x, y, s=0.4, color='green', zorder=4) ax.scatter(x, y, alpha=1, s=0.4, color=colors[idx], linewidth=1, zorder=4)
def steer_towards(self, x1, x2, eps): ########## Code starts here ########## """ A subtle issue: if you use dubins.path_sample to return the point at distance eps along the path from x to y, use a turning radius slightly larger than self.turning_radius (i.e., 1.001*self.turning_radius). Without this hack, dubins.path_sample might return a point that can't quite get to in distance eps (using self.turning_radius) due to numerical precision issues. """ # if the distance is short enough, return x2, otherwise, return a point at a distance eps along the path between x1 and x2 return x2 if self.distance(x1, x2) < eps else np.array( path_sample(x1, x2, 1.001 * self.turning_radius, eps)[0][1])
def plot_dubins_path(q0, q1, r=1.0, step_size=0.5): qs, _ = dubins.path_sample(q0, q1, r, step_size) qs = numpy.array(qs) xs = qs[:, 0] ys = qs[:, 1] us = xs + numpy.cos(qs[:, 2]) vs = ys + numpy.sin(qs[:, 2]) plt.plot(xs, ys, 'b-') plt.plot(xs, ys, 'r.') for i in xrange(qs.shape[0]): plt.plot([xs[i], us[i]], [ys[i], vs[i]],'r-') ax = plt.gca() expand_plot(ax) ax.set_aspect('equal')
def plot_dubins_path(q0, q1, r=1.0, step_size=0.5): qs, _ = dubins.path_sample(q0, q1, r, step_size) qs = numpy.array(qs) xs = qs[:, 0] ys = qs[:, 1] us = xs + numpy.cos(qs[:, 2]) vs = ys + numpy.sin(qs[:, 2]) plt.plot(xs, ys, 'b-') plt.plot(xs, ys, 'r.') for i in range(qs.shape[0]): plt.plot([xs[i], us[i]], [ys[i], vs[i]],'r-') ax = plt.gca() expand_plot(ax) ax.set_aspect('equal')
def steer_towards(self, x, y, eps): (tups, trash) = path_sample(x, y, 1.001 * self.turning_radius, eps) dest = tups[0] closer = True for tup in tups: if path_length(x, tup, self.turning_radius) <= eps: dest = tup else: closer = False if closer: return tuple(y) else: return dest
def __init__(self, delta_state, start_angle = 0, isbackward=False): length = 3.0 width = 2.0 self.delta_state = delta_state self.start_angle = start_angle self.cost = dubins.path_length((0,0,self.start_angle), delta_state, motion_primitive.turning_radius) path_list, _ = dubins.path_sample((0,0,self.start_angle), self.delta_state, motion_primitive.turning_radius, 0.1) self.path = np.array(path_list) self.isbackward = isbackward if self.isbackward: self.delta_state[:2] = -self.delta_state[:2] #(-0.25*np.cos(start_angle),-0.25*np.sin(start_angle),start_angle) self.path[:,0:2] = -1*self.path[:,0:2] #self.path = [(-xx,-yy,tth) for (xx,yy,tth) in self.path] #self.cost *= 2 box_angle_tuples = [(box(x - length/2, y - width/2, x + length/2, y + width/2), theta) for (x,y,theta) in self.path] polygons = [affinity.rotate(a_box, theta, origin = 'centroid', use_radians = True) for (a_box, theta) in box_angle_tuples] if False: fig = plt.figure(10) fig.clear() plt.ion() ax = fig.add_subplot(111, aspect = 'equal') for poly in polygons: P = PolygonPatch(poly, fc = 'k', zorder = 2) ax.add_patch(P) ax.set_xlim(-5,5) ax.set_ylim(-5,5) fig.show() polygons = [poly.buffer(0.1) for poly in polygons] try: self.bounding_poly = cascaded_union(polygons).simplify(0.05) except: raise Exception('No bounding poly for primitive')
def car_control(db, rabbit_db, rabbit_db_lock, e_state = None): global car_movement global end_state for car in db: curr_car = db[car] curr_state = CarState((float(curr_car['x']), float(curr_car['y'])), float(curr_car['angle']), float(curr_car['time'])) if end_state != e_state: #if car not in car_movement: """end_state = CarState((float(curr_car['end_x']), float(curr_car['end_y'])), float(curr_car['end_angle']), float(curr_car['time']))""" end_state = e_state DELTA_SAMPLE = 0.1 path, time = dubins.path_sample(curr_state.get_xy_angle(), end_state.get_xy_angle(), TURNING_RADIUS, DELTA_SAMPLE) car_movement[car] = CarMovementFollowPath(path, DELTA_SAMPLE) car_movement[car].do_next_step(curr_state, car, rabbit_db, rabbit_db_lock)
def main(): parser = OptionParser() parser.add_option("-d", "--output", dest = "pathfile", help = "Path output file", metavar = "FILE") (options, args) = parser.parse_args() # 2D Dubins Path Part q0_2d = np.array([q0.x0, q0.y0, q0.yaw0]) q1_2d = np.array([q1.x0, q1.y0, q1.yaw0]) qs, _ = dubins.path_sample(q0_2d, q1_2d, AircraftParameters.MinTurnRadius, DubinsSolver.StepSize) sol_2d = np.asarray(qs); [Ns,Ndim] = sol_2d.shape; AirplaneSolution = VehicleSolution(np.zeros((Ns,3)),np.zeros((Ns,1))) AirplaneSolution.Path[:,0] = sol_2d[:,0]; AirplaneSolution.Path[:,1] = sol_2d[:,1]; AirplaneSolution.YAWvec = sol_2d[:,2]; PathLength = DubinsPathLength(sol_2d) TimeToDest = PathLength/AircraftParameters.V_travel # altitude path RateDes = abs(q0.z0-q1.z0)/TimeToDest if RateDes > AircraftParameters.MaxAscDescRate: # the system will not arrive to the desired altitude # it will rather ascend/descend with the maximum rate AirplaneSolution.Path[:,2] = MaxAscDesc(q0,q1,Ns,AircraftParameters.MaxAscDescRate,(PathLength/Ns)/AircraftParameters.V_travel) else: AirplaneSolution.Path[:,2] = LinearAscDesc(q0,q1,Ns) # save to file solution_mat = np.zeros((Ns,4)) solution_mat[:,0] = AirplaneSolution.Path[:,0]; solution_mat[:,1] = AirplaneSolution.Path[:,1]; solution_mat[:,2] = AirplaneSolution.Path[:,2]; solution_mat[:,3] = AirplaneSolution.YAWvec; np.savetxt(options.pathfile,solution_mat) # plot option plot3(AirplaneSolution.Path[:,0],AirplaneSolution.Path[:,1],AirplaneSolution.Path[:,2],'o','g') raw_input()
def search(self, vobj): """The Hybrid State A* search algorithm.""" tic = time.clock() get_grid_id = self.graph.get_grid_id frontier = PriorityQueue() frontier.put(list(self.start), 0) came_from = {} cost_so_far = {} came_from[tuple(self.start)] = None cost_so_far[get_grid_id(self.start)] = 0 dubins_path = False num_nodes = 0 while not frontier.empty(): current = frontier.get() if num_nodes % self.dubins_expansion_constant == 0: dpath,_ = dubins.path_sample(current, self.goal, self.turning_radius, self.step_size) if not self.map.is_occupied_discrete(dpath): # Success. Dubins expansion possible. self.path_found = True dubins_path = True break if np.linalg.norm(current[0:2] - self.goal[0:2]) < self.eps \ and np.abs(current[2]-self.goal[2]) < np.pi/8: self.path_found = True break for next in self.graph.neighbors(current, vobj.model.est_r_max): new_cost = cost_so_far[get_grid_id(current)] + \ self.graph.cost(current, next) if get_grid_id(next) not in cost_so_far or new_cost < cost_so_far[get_grid_id(next)]: cost_so_far[get_grid_id(next)] = new_cost priority = new_cost + heuristic(self.goal, next) frontier.put(list(next), priority) came_from[tuple(next)] = current num_nodes += 1 # Reconstruct path path = [current] while tuple(current) != tuple(self.start): current = came_from[tuple(current)] path.append(current) if dubins_path: path = np.array(path[::-1] + dpath) else: path = np.array(path[::-2]) print "Hybrid A-star CPU time: %.3f. Nodes expanded: %d" % ( time.clock() - tic, num_nodes) #print self.start return np.copy(path)
import dubins import math q0 = (0.0, 0.0, math.pi / 4) q1 = (-4.0, 4.0, -math.pi) turning_radius = 1.0 step_size = 0.5 qs, _ = dubins.path_sample(q0, q1, turning_radius, step_size) print qs
pathx.append(state[0]) pathy.append(state[1]) plt.plot(pathx,pathy) plt.show(block=False) def cost_function(state, motion_primitive): return motion_primitive.cost # + belief cost astar = AStar(motion_primitives, cost_function, heuristic, valid_edge_function, state_equality) plan = astar.plan(start_state, goal_state) if plan: pathx = []; pathy = []; for i in range(len(plan)-1,0,-1): seg, _ = dubins.path_sample(plan[i], plan[i-1], turning_radius, step_size) for state in seg: pathx.append(state[0]) pathy.append(state[1]) print state plt.figure(2) plt.subplot(111) plt.plot(pathx,pathy) plt.plot([0, 80],[30, 30]) plt.plot([70, 100],[60, 60]) plt.show()
def __init__(self, delta_state, cost): self.delta_state = delta_state self.cost = cost path,_ = dubins.path_sample((0,0,0), self.delta_state, turning_radius, step_size) self.path = path