def test_turning_radius_scaling(self): a = dubins.shortest_path((0, 0, 0), (10, 10, math.pi / 4.0), 1.0).path_length() b = dubins.shortest_path((0, 0, 0), (10, 10, math.pi / 4.0), 2.0).path_length() self.assert_(b > a)
def test_colocated_configurations(self): # Regression: previous version of the code did not work correctly # for goal and initial configurations were too close together qi = (0, 0, 0) qg = (0, 0, 0) turning_radius = 1.0 # not raising an exception is sufficient dubins.shortest_path(qi, qg, turning_radius)
def pursuitToWaypoint(waypoint, wp_cnt): q0 = (rear_axle_center.position.x, rear_axle_center.position.y, rear_axle_theta) q1 = (waypoint[0], waypoint[1], waypoint[2]) turning_radius = 2.0 step_size = 1 path = dubins.shortest_path(q0, q1, turning_radius) configurations, _ = path.sample_many(step_size) curve_pts = mark_straight(configurations) for i, c in enumerate(configurations): velocity = 2 MAX_VEL = 1.5 MIN_VEL = 0.2 if wp_cnt == 1: MAX_VEL = 1.5 elif wp_cnt == 2: MAX_VEL = 1 elif wp_cnt == 3: MAX_VEL = 1 elif wp_cnt == 4: MAX_VEL = 0.3 MIN_VEL = 0.05 velocity = max(MIN_VEL, min(MAX_VEL, velocity)) _pursuitToWaypoint([c[0], c[1]], velocity, wp_cnt)
def handle_dubins(course_data, exit_row_index, entry_row_index, angle_tolerance=0.523599): # exit_row_index = int(sys.argv[2]) # row the rover is exiting # entry_row_index = int(sys.argv[3]) # row it's about to go down exit_row, entry_row = [], [] print("incoming course data: {}".format(course_data)) print("exit_row_index: {}".format(exit_row_index)) print("exit_row_index type: {}".format(type(exit_row_index))) # gets start and end rows arrays: for row_obj in course_data['rows']: if int(row_obj['index']) == int(exit_row_index): exit_row = row_obj['row'] elif int(row_obj['index']) == int(entry_row_index): entry_row = row_obj['row'] print("exit and entry rows: {}; {}".format(exit_row, entry_row)) # pick end point for start row, first point end row: exit_row_angle = get_row_angle(exit_row) entry_row_angle = get_row_angle(entry_row) print("exit row angle: {}".format(math.degrees(exit_row_angle))) print("entry row angle: {}".format(math.degrees(entry_row_angle))) # calculates row spacing at exit/entry points: distance_apart = math.sqrt((exit_row[-1][0] - entry_row[0][0])**2 + (exit_row[-1][1] - entry_row[0][1])**2) print("Straight distance between exit and entry rows: {} meters".format( distance_apart)) # determines which end of row to use based on row angles: angle_diff = abs(exit_row_angle - entry_row_angle) # angle_diff_abs = math.degrees(abs(angle_diff)) print("angle diff b/w rows {}".format(math.degrees(angle_diff))) turning_radius = 1.5 step_size = 0.5 q0, q1 = determine_dubins_AB_points(exit_row, entry_row, exit_row_angle, entry_row_angle, angle_diff, angle_tolerance) path = dubins.shortest_path(q0, q1, turning_radius) configurations, _ = path.sample_many(step_size) plot_handler(configurations, exit_row, entry_row, q0, q1) dubins_course = np.array(configurations) return dubins_course
def get_heuristic(self, state, goal): assert len(state) == 3, "state must be of form (x,y,heading)" assert len(goal) == 3, "goal must be of form (x,y,heading)" s = (state[0], state[1], angles.normalize_angle_positive(state[2])) g = (goal[0], goal[1], angles.normalize_angle_positive(goal[2])) path = dubins.shortest_path(s, g, self.turning_radius - 0.01) return path.path_length()
def buffered_paths(self): sampling_path = {} true_path = {} for i, goal in enumerate(self.goals): path = dubins.shortest_path(self.cp, goal, self.tr) fconfig, _ = path.sample_many(self.ss / 10) ftemp = [] for c in fconfig: if c[0] > self.extent[0] and c[0] < self.extent[1] and c[ 1] > self.extent[2] and c[1] < self.extent[ 3] and not self.obstacle_world.in_obstacle( (c[0], c[1]), buff=0.0): ftemp.append(c) else: break try: ttemp = ftemp[0::10] for m, c in enumerate(ttemp): if c[0] <= self.extent[0] + 3 * self.tr or c[0] >= self.extent[1] - 3 * self.tr or c[1] <= \ self.extent[2] + 3 * self.tr or c[1] >= self.extent[ 3] - 3 * self.tr or self.obstacle_world.in_obstacle((c[0], c[1]), buff=3 * self.tr): ttemp = ttemp[0:m - 1] if len(ttemp) < 2: pass else: sampling_path[i] = ttemp true_path[i] = ftemp[0:ftemp.index(ttemp[-1]) + 1] except: pass return sampling_path, true_path
def nonholonomic_obstacle_free_(self, cur, target): """Since euclidean distance is not a good heuristic for non-holonomic vehicles, we use dubin's path to estimate path length. While these values can be pre-computed in a heuristic lookup table (HLUT), there are many positions to precompute so the time saved by this may not outweigh the high overhead of precomputing all state heuristics. Also, dubin's path is a fast computation and this library is a Cython wrapper and thus still performs quickly. Since dubin's path will estimate a large path length to perfectly reach a target, but we only care about approximately reaching, we return euclidean distance instead of the target is within some radius r. Args: cur ([type]): [description] target ([type]): [description] Returns: [type]: [description] """ start_time = time.time() decimal_places = 3 cur_pose = np.round(cur[:3], decimal_places) # x0, y0, theta0 target_pose = np.round(target[:3], decimal_places) # x1, y1, theta1 # due to floating precision errors, make angles discrete cur_pose[2] = round(cur_pose[2], 2) # NOTE: we perform search from q0=target_pos to q1=cur_pos # since we are doing backward search, this is actually what's # going on, and this avoids issues of transforming frames path = dubins.shortest_path(target_pose, cur_pose, self.min_turn_rad) dist = self.euc_dist(cur, target) end_time = time.time() self.nonholonomic_obstacle_free_time += (end_time - start_time) if dist < self.goal_thresh: return dist else: return path.path_length()
def make_sample_paths(self): '''Connect the current_pose to the goal places''' coords = {} true_coords = {} for i,goal in enumerate(self.goals): g = (goal[0],goal[1],self.cp[2]) path = dubins.shortest_path(self.cp, goal, self.tr) configurations, _ = path.sample_many(self.ss) true_coords[i], _ = path.sample_many(self.ss/5) coords[i] = [config for config in configurations if config[0] > self.extent[0] and config[0] < self.extent[1] and config[1] > self.extent[2] and config[1] < self.extent[3] and not self.obstacle_world.in_obstacle((config[0], config[1]), buff=self.tr)] # find the "shortest" path in sample space current_min = 1000 for key,path in coords.items(): if len(path) < current_min and len(path) > 1: current_min = len(path) # limit all paths to the shortest path in sample space # NOTE! for edge cases nar borders, this limits the paths significantly for key,path in coords.items(): if len(path) > current_min: path = path[0:current_min] coords[key]=path for key,path in true_coords.items(): ftemp = [] for c in path: if c[0] == coords[key][-1][0] and c[1] == coords[key][-1][1]: ftemp.append(c) break else: ftemp.append(c) true_path[key] = ftemp return coords, true_coords
def fitSpline(self,waypoints): # spline configurations turning_radius = 0.67 step_size = 0.5 waypoints = np.insert(waypoints, 0, [0,0,0], axis=0)# prepend zero state to waypoints # TODO: check yaw # find heading-fitting spline path_list = np.empty((0,3)) for i in range(waypoints.shape[0] - 1): q0 = (waypoints[i,0], waypoints[i,1], waypoints[i,2]) q1 = (waypoints[i+1,0], waypoints[i+1,1], waypoints[i+1,2]) path = dubins.shortest_path(q0, q1, turning_radius) configurations, _ = path.sample_many(step_size) configurations = np.array(configurations) path_list = np.vstack((path_list, configurations)) # Publish as nav_msgs/Path message path_msg = Path() path_msg.header.stamp = rospy.Time.now() path_msg.header.frame_id = '/map' for i in range(path_list.shape[0]): pose = PoseStamped() pose.pose.position.x = path_list[i,0] pose.pose.position.y = path_list[i,1] path_msg.poses.append(pose) self.spline_path_pub.publish(path_msg) self.spline_points = path_list self.spline_distance = np.sum(np.sqrt(np.sum(np.diff(path_list[:,:2], axis=0)**2, axis=1))) self.spline_cum_dist = np.cumsum(np.sqrt(np.sum(np.diff(path_list[:,:2], axis=0)**2, axis=1))) print("Published Spline Path. Distance (m): ", self.spline_distance)
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])) path = dubins.shortest_path(s1, s2, self.radius - 0.01) edge, _ = path.sample_many(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 goalCallback(msg): start_pose = (msg.start.x, msg.start.y, msg.start.theta) goal_pose = (msg.goal.x, msg.goal.y, msg.goal.theta) dubins_path = dubins.shortest_path(start_pose, goal_pose, 1.0) num_samples = msg.vel * msg.dt path_points = dubins_path.sample_many(num_samples)[0] dpath = Path() dpath.header.stamp = rospy.Time() dpath.header.frame_id = "map" dpath.path = [] for i in range(len(path_points)): point = Pose2D() point.x = path_points[i][0] point.y = path_points[i][1] point.theta = path_points[i][2] dpath.path.append(point) path_pub.publish(dpath)
def construct_dubins_traj(traj_point_0, traj_point_1): """ Construc a trajectory in the X-Y space and in the time-X,Y,Theta space. Arguments: traj_point_0 (list of floats): The trajectory's first trajectory point with time, X, Y, Theta (s, m, m, rad). traj_point_1 (list of floats): The trajectory's last trajectory point with time, X, Y, Theta (s, m, m, rad). Returns: traj (list of lists): A list of trajectory points with time, X, Y, Theta (s, m, m, rad). traj_distance (float): The length ofthe trajectory (m). """ q0 = (traj_point_0[1], traj_point_0[2], traj_point_0[3]) q1 = (traj_point_1[1], traj_point_1[2], traj_point_1[3]) turning_radius = 0.5 path = dubins.shortest_path(q0, q1, turning_radius) configurations, distances = path.sample_many(DISTANCE_STEP_SIZE) traj_distance = distances[-1] traj_time = traj_point_1[0] - traj_point_0[0] time_step_size = traj_time / len(distances) traj = [] traj_point_time = traj_point_0[0] for c in configurations: traj_point = [traj_point_time, c[0], c[1], c[2]] traj.append(traj_point) traj_point_time += time_step_size return traj, traj_distance
def construct_dubins_traj(traj_point_0, traj_point_1): """ Construc a trajectory in the X-Y space and in the time-X,Y,Theta space. Arguments: traj_point_0 (list of floats): The trajectory's first trajectory point with time, X, Y, Theta (s, m, m, rad). traj_point_1 (list of floats): The trajectory's last trajectory point with time, X, Y, Theta (s, m, m, rad). Returns: traj (list of lists): A list of trajectory points with time, X, Y, Theta (s, m, m, rad). traj_distance (float): The length ofthe trajectory (m). """ # traj = [] # traj_point = [0,0,0,0] # traj.append(traj_point) turning_rad = 0.25 step_size = DISTANCE_STEP_SIZE path = dubins.shortest_path(traj_point_0[1:], traj_point_1[1:], turning_rad) configs, _ = path.sample_many(step_size) dt = (traj_point_1[0] - traj_point_0[0]) / len(configs) timeStamp = traj_point_0[0] for i in range(len(configs)): configs[i] = list(configs[i]) configs[i].insert(0, timeStamp) timeStamp += dt configs.append(traj_point_1) traj = configs traj_distance = np.arange(len(configs)) * step_size #TODO: verify this return traj, traj_distance
def construct_dubins_traj(traj_point_0, traj_point_1): """ Construc a trajectory in the X-Y space and in the time-X,Y,Theta space. Arguments: traj_point_0 (list of floats): The trajectory's first trajectory point with time, X, Y, Theta (s, m, m, rad). traj_point_1 (list of floats): The trajectory's last trajectory point with time, X, Y, Theta (s, m, m, rad). Returns: traj (list of lists): A list of trajectory points with time, X, Y, Theta (s, m, m, rad). traj_distance (float): The length ofthe trajectory (m). """ traj = [] q0 = traj_point_0[1:] q1 = traj_point_1[1:] turning_radius = 1 path = dubins.shortest_path(q0, q1, turning_radius) configurations, _ = path.sample_many(DISTANCE_STEP_SIZE) t_0 = traj_point_0[0] time_step = (traj_point_1[0] - traj_point_0[0]) / len(configurations) traj_distance = len(configurations) * DISTANCE_STEP_SIZE for i in range(0, len(configurations)): step = [ t_0 + time_step * i, configurations[i][0], configurations[i][1], configurations[i][2] ] traj.append(step) return traj, traj_distance
def make_sample_paths(self): '''Connect the current_pose to the goal places''' coords = {} for i, goal in enumerate(self.goals): g = (goal[0], goal[1], self.cp[2]) path = dubins.shortest_path(self.cp, goal, self.tr) configurations, _ = path.sample_many(self.ss) coords[i] = [ config for config in configurations if config[0] > self.extent[0] and config[0] < self.extent[1] and config[1] > self.extent[2] and config[1] < self.extent[3] ] # find the "shortest" path in sample space current_min = 1000 for key, path in coords.items(): if len(path) < current_min and len(path) > 1: current_min = len(path) # limit all paths to the shortest path in sample space for key, path in coords.items(): if len(path) > current_min: path = path[0:current_min] coords[key] = path self.samples = coords return coords
def test_half_loop(self): r = 1.0 dx = r * math.sqrt(2) / 2 dy = r * math.sqrt(2) / 2 d = dubins.shortest_path((0, 0, 0), (0, 2 * r, -math.pi), r) length = d.path_length() self.assertAlmostEqual(length, math.pi * r)
def take_step(self, loc, goal): ''' Create an intermediary goal towards the point of interest such that the robot only translates the step size specified Input: Goal Output: Navigable points to the intermediary goal ''' coords = {} dist = np.sqrt((loc[0] - goal[0])**2 + (loc[1] - goal[1])**2) angle_to_goal = np.arctan2([goal[1] - loc[1]], [goal[0] - loc[0]])[0] if dist > self.step_size: new_goal = (loc[0] + self.step_size * np.sin(np.pi / 2 - angle_to_goal), loc[1] + self.step_size * np.sin(angle_to_goal), angle_to_goal) else: new_goal = (goal[0], goal[1], angle_to_goal) path = dubins.shortest_path(loc, new_goal, self.turning_radius) configurations, _ = path.sample_many(self.sample_step) configurations.append(new_goal) temp = [] for i, config in enumerate(configurations): if config[0] > self.ranges[0] and config[0] < self.ranges[ 1] and config[1] > self.ranges[2] and config[ 1] < self.ranges[3]: temp.append(config) else: pass return temp
def generatePath(self): dubins_start_list = list(self.coord_s) dubins_start_list.append(self.start_angle) dubins_end_list = list(self.coord_e) dubins_end_list.append(self.end_angle) start_params = tuple(dubins_start_list) end_params = tuple(dubins_end_list) self.path = dubins.shortest_path(start_params, end_params, self.turning_radius)
def test_readme_demo(self): q0 = (0, 0, math.pi / 2) q1 = (1, 1, 0.0) turning_radius = 1.0 step_size = 0.5 path = dubins.shortest_path(q0, q1, turning_radius) configurations, _ = path.sample_many(step_size)
def test_easy_lrl(self): # Regression for https://github.com/AndrewWalker/Dubins-Curves/issues/2 r = 1.0 q0 = (0, 0, math.pi / 2.) q1 = (1, 0, -math.pi / 2.) d = dubins.shortest_path(q0, q1, r) path_type = d.path_type() self.assertAlmostEqual(path_type, dubins.LRL)
def compute_heuristic(self, config, goal): """ Use the Dubins path length from config to goal as the heuristic distance. """ # Implement here path = dubins.shortest_path(config, goal, 1.0 / self.curvature) return path.path_length()
def generate_path(x0, y0, x1, y1, theta0, theta1): q0 = (x0, y0, theta0) q1 = (x1, y1, theta1) turning_radius = 2.0 step_size = 1.0 path = dubins.shortest_path(q0, q1, turning_radius) configurations, _ = path.sample_many(step_size) return configurations
def generate(self): ## Randomize starting and goal poses x0 = np.random.randint(-40, 40) y0 = np.random.randint(-40, 40) psi_0 = np.radians(np.random.randint(0, 360)) x1 = np.random.randint(-40, 40) y1 = np.random.randint(-40, 40) psi_1 = np.radians(np.random.randint(0, 360)) self.q0 = [x0, y0, psi_0] self.qg = [x1, y1, psi_1] ## Modify dubins to work for a straight offset from goal self.q1 = self.qg.copy() self.q1[0] -= 2 * self.turning_radius * np.cos(self.q1[2]) self.q1[1] -= 2 * self.turning_radius * np.sin(self.q1[2]) # Dubins path = dubins.shortest_path(self.q0, self.q1, self.turning_radius) qs, dist_dubins = path.sample_many(self.step_size) qs = np.array(qs) ## Concatenate with reverse straight s_s = self.distance((self.q1[0], self.q1[1]), (self.qg[0], self.qg[1])) n_steps = int(s_s // self.step_size) + 1 straight = np.array([ np.linspace(self.q1[0], self.qg[0], n_steps), np.linspace(self.q1[1], self.qg[1], n_steps), self.qg[2] * np.ones(n_steps) ]).T qs = np.vstack((qs, straight)) dist_straight = [dist_dubins[-1]] for j in range(len(straight)): dist_straight.append(dist_straight[j] + (s_s / n_steps)) self.dist = dist_dubins + dist_straight[1:] # ignore double counting ## x, y, curv, psi, dist self.curv = [] for n in range(len(qs)): if n == 0: self.curv.append( self.get_menger_curvature(qs[0], qs[n + 1], qs[n + 2])) elif n == len(qs) - 1: self.curv.append( self.get_menger_curvature(qs[n - 2], qs[n - 1], qs[n])) else: self.curv.append( self.get_menger_curvature(qs[n - 1], qs[n], qs[n + 1])) self.x = qs[:, 0] self.y = qs[:, 1] self.psi = qs[:, 2] return np.column_stack( [self.x, self.y, self.curv, self.psi, self.dist])
def generate_path(self, config1, config2): """ Generate a dubins path from config1 to config2 The generated path is not guaranteed to be collision free Use dubins_path_planning to get a path return: (numpy array of [x, y, yaw], curve length) """ path = dubins.shortest_path(config1, config2, 1.0 / self.curvature) configs, _ = path.sample_many(0.2) return np.array(configs), path.path_length()
def _evaluateChromosome(i, tr=0.5): # on distance travelled dist = 0 assns = i.get_assignments() for a1, a2 in zip(assns, assns[1:]): a1x, a1y = a1.task.get_loc() a2x, a2y = a2.task.get_loc() dist += dubins.shortest_path((a1x, a1y, a1.heading), (a2x, a2y, a2.heading), tr).path_length() return dist,
def rewire(neighborhood, center): for neighbor in neighborhood: if validPath(trajectory(center.configuration, neighbor.configuration)): if testDepth(neighbor, center) < neighbor.calcDepth(): newpath = dubins.shortest_path(center.configuration, neighbor.configuration, 0.2) neighborUpdate = Node(center, neighbor.configuration, newpath) nodes.remove(neighbor) nodes.append(neighborUpdate) assert nodes[nodes.index(neighborUpdate)].parent == center assert nodes[nodes.index(neighborUpdate)].path == newpath
def path(self, end, step_size): path = dubins.shortest_path((self.pos.x, self.pos.y, self.theta), (end.pos.x, end.pos.y, end.theta), turning_radius) configurations, lens = path.sample_many(step_size) dubinsCars = [] for c in configurations: dubinsCars.append(Car(Point(c[0], c[1]), c[2])) return dubinsCars
def rrt(): global locations while True: _sample = sample() _closestNode = nearest(_sample[0], _sample[1]) if validPath(trajectory(_closestNode.configuration, _sample)): path = dubins.shortest_path(_closestNode.configuration, _sample, 0.2) new = Node(_closestNode, _sample, path) nodes.append(new) locations = np.concatenate((locations, [new.location]), axis=0) if new.x == goal[0] and new.y == goal[1]: return new
def take_step(self, loc): ''' Given a current location and a goal, determine the dubins curve sampling path''' sampling_path = {} true_path = {} for i, goal in enumerate(self.goals): dist = np.sqrt((loc[0] - goal[0])**2 + (loc[1] - goal[1])**2) angle_to_goal = np.arctan2([goal[1] - loc[1]], [goal[0] - loc[0]])[0] if dist > self.step_size: new_goal = (loc[0] + self.step_size * np.sin(np.pi / 2 - angle_to_goal), loc[1] + self.step_size * np.sin(angle_to_goal), angle_to_goal) else: new_goal = (goal[0], goal[1], angle_to_goal) path = dubins.shortest_path(loc, new_goal, self.turning_radius) configurations, _ = path.sample_many(self.sample_step) configurations.append(new_goal) full_path = fconfig, _ = path.sample_many(self.sample_step / 5) temp = [] for config in configurations: if config[0] > self.ranges[0] and config[0] < self.ranges[ 1] and config[1] > self.ranges[2] and config[ 1] < self.ranges[ 3] and not self.obstacle_world.in_obstacle( (config[0], config[1]), buff=self.turning_radius): temp.append(config) else: break if len(temp) < 2: pass else: sampling_path[i] = temp ftemp = [] for c in fconfig: if len(temp) >= 2: if c[0] == temp[-1][0] and c[1] == temp[-1]: ftemp.append(c) break else: ftemp.append(c) else: pass true_path[i] = ftemp return sampling_path, true_path
def generate_swath(ship: Ship, prim: Primitives, eps=1e-10) -> Swath: """ Will have key of (edge, start heading) """ swath_dict = {} for origin, edge_set in prim.edge_set_dict.items(): start_pos = [prim.max_prim + ship.max_ship_length // 2] * 2 + [ origin[2] ] for e in edge_set: e = tuple(e) array = np.zeros( [(prim.max_prim + ship.max_ship_length // 2) * 2 + 1] * 2, dtype=bool) translated_e = np.asarray(e) + np.array( [start_pos[0], start_pos[1], 0]) theta_0 = heading_to_world_frame(start_pos[2], ship.initial_heading, prim.num_headings) theta_1 = heading_to_world_frame(translated_e[2], ship.initial_heading, prim.num_headings) dubins_path = dubins.shortest_path( (start_pos[0], start_pos[1], theta_0), (translated_e[0], translated_e[1], theta_1), ship.turning_radius - eps) configurations, _ = dubins_path.sample_many(0.1) for config in configurations: x_cell = int(round(config[0])) y_cell = int(round(config[1])) theta = config[2] - ship.initial_heading R = np.asarray([[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]]) rot_vi = np.round( np.array([[x_cell], [y_cell]]) + R @ ship.vertices.T).astype(int) rr, cc = draw.polygon(rot_vi[1, :], rot_vi[0, :]) array[rr, cc] = True # for each starting heading rotate the swath 4 times for i, h in enumerate( range(0, prim.num_headings, prim.num_headings // 4)): swath_dict[e, h + origin[2]] = np.rot90(array, k=i, axes=(1, 0)) return swath_dict