def compute_costs(P, mapping, radius, orig_poly): """ Compute dubins costs between path segments which could be either a line or a point. TODO: Collision checking!! """ MAX_COST = 1000000 num_nodes = len(mapping) r = radius cost = [[0 for i in range(num_nodes)] for i in range(num_nodes)] print("Size: %d nodes."%num_nodes) # Populate the cost matrix for i in range(num_nodes): # print(i) for j in range(num_nodes): q0 = mapping[i][0].get_exit_info(mapping[i][1]) q1 = mapping[j][0].get_entrance_info(mapping[j][1]) # Check for collisions x0 = q0[0]; y0 = q0[1] x1 = q1[0]; y1 = q1[1] #if has_collision(P, [(x0, y0), (x1, y1)]): if has_collision(orig_poly, [(x0, y0), (x1, y1)]): length = 100*dubins.path_length(q0, q1, r) length += 1000000 else: length = 100*dubins.path_length(q0, q1, r) cost[i][j] = length # Generate a cluster information list cluster_list = [] node_list = [] counter = 0 for i in range(num_nodes): segment, direction_id = mapping[i] if direction_id == 0: if node_list: cluster_list.append(node_list) counter += 1 node_list = [] node_list.append(i) cluster_list.append(node_list) return cost, cluster_list
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_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 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 steer_towards_backward(self, x1, x2, eps): ########## Code starts here ########## from dubins import path_sample from dubins import path_length rx1 = (x1[0], x1[1], x1[2] - np.pi) rx2 = (x2[0], x2[1], x2[2] - np.pi) samples = path_sample(rx2, rx1, 1.001 * self.turning_radius, eps)[0] #print samples if len(samples) > 1: if path_length(rx2, rx1, self.turning_radius) > path_length( rx2, samples[1], self.turning_radius): x1 = samples[1] x1 = (x1[0], x1[1], x1[2] - np.pi) return x1
def find_nearest(self, V, x): # TODO: fill me in! numStates = self.V_size lengths = [] for i in range(numStates): lengths.append(path_length(V[i, :], x, self.turning_radius)) return np.argmin(lengths)
def find_nearest_forward(self, V, x): ########## Code starts here ########## from dubins import path_length path_lengths = np.zeros(len(V)) for i in range(len(V)): path_lengths[i] = path_length(V[i, :], x, self.turning_radius) return np.argmin(path_lengths)
def find_nearest(self, V, x): from dubins import path_length ########## Code starts here ########## dist = np.zeros((np.shape(V)[0], )) for i in range(len(V)): dist[i] = path_length(V[i, :], x, self.turning_radius) return np.argmin(dist)
def find_nearest_backward(self, V, x): ########## Code starts here ########## from dubins import path_length return np.argmin([ path_length(x, V[i, :], self.turning_radius) for i in range(V.shape[0]) ])
def find_nearest_forward(self, V, x): ########## Code starts here ########## ds = [] for xs in V: ds.append(path_length(xs, x, self.turning_radius)) return np.argmin(ds)
def dubins_dist(node_from, node_to, radius=1.0): res = np.zeros(len(node_from)) for idx, node in enumerate(node_from): dist = dubins.path_length(node, node_to, radius) res[idx] = dist return res
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 find_nearest(self, V, x): from dubins import path_length ########## Code starts here ########## ds = [] for xs in V: ds.append(path_length(xs,x,self.turning_radius)) return np.argmin(ds)
def find_nearest(self, V, x): # TODO: fill me in! dist_arr = [] for i in range(self.tree_size): this_dist = path_length(V[i,:], x, self.turning_radius) dist_arr.append(this_dist) return np.argmin(dist_arr)
def find_nearest(self, V, x): """Returns the index of the nearest point in V to x.""" min_index, min_value = 0, float('inf') for index, value in enumerate(V): distance = dubins.path_length(value, x, self.turning_radius) if distance < min_value: min_index = index min_value = distance return min_index
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 get_cost(self, edge): assert len(edge[0]) == 3, "state must be of form (x,y,heading)" s1 = (edge[0][0], edge[0][1], angles.normalize_angle_positive(edge[0][2])) s2 = (edge[-1][0], edge[-1][1], angles.normalize_angle_positive(edge[-1][2])) return dubins.path_length(s1, s2, self.turning_radius)
def find_nearest(self, V, x): from dubins import path_length ########## Code starts here ########## # apply path_length to each element and find minimum path_length_vec = np.array([ path_length(V[i, :], x, self.turning_radius) for i in range(np.shape(V)[0]) ]) return np.argmin(path_length_vec)
def find_nearest(self, V, x): from dubins import path_length ########## Code starts here ########## n = np.shape(V)[0] # pull out size of V dist = [] for i in range(0, n): dist.append(path_length(V[i, :], x, self.turning_radius)) return np.argmin(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_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 find_nearest(self, V, x): minidx = 0 mindist = np.inf for i in range(np.shape(V)[0]): dist = path_length(V[i], x, self.turning_radius) if dist < mindist: minidx = i mindist = dist return minidx
def find_nearest_backward(self, V, x): ########## Code starts here ########## from dubins import path_length path_lengths = np.zeros(len(V)) for i in range(len(V)): #reverse the heading, since backwards path_lengths[i] = path_length(self.reverse_heading(V[i, :]), self.reverse_heading(x), self.turning_radius) return np.argmin(path_lengths)
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 find_nearest_forward(self, V, x): ########## Code starts here ########## min_path_length = np.inf min_path_idx = None for i in range(V.shape[0]): le = path_length(np.ndarray.squeeze(V[i]), x, self.turning_radius) if le < min_path_length: min_path_length = le min_path_idx = i return min_path_idx
def do_next_step(self, curr_state, car): #move_car(car, False, True, True, False) #return curr = curr_state.get_xy_angle() end = self._end_state.get_xy_angle() speed = self._get_speed(curr_state) right = False left = False next_move = dubins.next_move(curr, end, TURNING_RADIUS, 0.0001) d = norm(sub_vecs(curr, end)) if self.stop_dubins: self.remaining_path_length = TURNING_RADIUS * 2 * math.asin( d / (2 * TURNING_RADIUS)) else: self.remaining_path_length = dubins.path_length( curr, end, TURNING_RADIUS) vec_to_end_point = sub_vecs(end_state.point, curr_state.point) dist_to_end_point = norm(vec_to_end_point) if ((self.remaining_path_length - self._prev_path_length) > CarMovementDubins.PATH_LENGTH_TOLERANCE) or (self.stop_dubins): # Dubins missed #print "Dubins missed, dist to point:", dist_to_end_point angle_to_end_point = atan2(vec_to_end_point[1], vec_to_end_point[0]) #print "Angle to end point:", angle_to_end_point if (angle_to_end_point - curr_state.angle) % (2 * pi) < pi: right = True else: left = True #if left: # print "Turning left" #else: # print "Turning right" self.stop_dubins = True else: left = (next_move == dubins.RIGHT) right = (next_move == dubins.LEFT) self._prev_path_length = self.remaining_path_length forward = True if (self.remaining_path_length < (speed * speed / (2 * DECELERATION * 0.9)) or self.remaining_path_length < 50): forward = False move_car(car, left, right, forward, False)
def find_nearest_backward(self, V, x): ########## Code starts here ########## min_path_length = np.inf min_path_idx = None for i in range(V.shape[0]): # For backward tree we want to go from some point x towards the root V le = path_length(x, np.ndarray.squeeze(V[i]), self.turning_radius) if le < min_path_length: min_path_length = le min_path_idx = i return min_path_idx
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 nonholonomic(self, current): # theta = self.goal[2] # current = list(current) # current[0] -= self.goal[0] #x # current[1] -= self.goal[1] #y # current[2] -= theta #heading # current[0], current[1] = (np.cos(theta)*current[0]+np.sin(theta)*current[1], -np.sin(theta)*current[0]+np.cos(theta)*current[1]) q0 = current q1 = self.goal turning_radius = 5.74 cost = dubins.path_length(q0, q1, turning_radius) return cost
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 do_next_step(self, curr_state, car): #move_car(car, False, True, True, False) #return curr = curr_state.get_xy_angle() end = self._end_state.get_xy_angle() speed = self._get_speed(curr_state) right = False left = False next_move = dubins.next_move(curr, end, TURNING_RADIUS, 0.0001) d = norm(sub_vecs(curr,end)) if self.stop_dubins: self.remaining_path_length = TURNING_RADIUS*2*math.asin(d/(2*TURNING_RADIUS)) else: self.remaining_path_length = dubins.path_length(curr, end, TURNING_RADIUS) vec_to_end_point = sub_vecs(end_state.point, curr_state.point) dist_to_end_point = norm(vec_to_end_point) if ((self.remaining_path_length - self._prev_path_length) > CarMovementDubins.PATH_LENGTH_TOLERANCE) or (self.stop_dubins): # Dubins missed #print "Dubins missed, dist to point:", dist_to_end_point angle_to_end_point = atan2(vec_to_end_point[1], vec_to_end_point[0]) #print "Angle to end point:", angle_to_end_point if (angle_to_end_point-curr_state.angle)%(2*pi) < pi: right = True else: left = True #if left: # print "Turning left" #else: # print "Turning right" self.stop_dubins = True else: left = (next_move == dubins.RIGHT) right = (next_move == dubins.LEFT) self._prev_path_length = self.remaining_path_length forward = True if( self.remaining_path_length<(speed*speed/(2*DECELERATION*0.9)) or self.remaining_path_length<50): forward = False move_car(car, left, right, forward, False)
def computePrimitives(): [x,y,th] = np.mgrid[-3.25:3.25:7j, -3.25:3.25:7j, -np.pi:(0.75*np.pi):8j] x = x.flatten() y = y.flatten() th = th.flatten() print 'Generating motion primitives...' motion_primitives = {} start_angle = 0.0 print '\n' print np.around(start_angle/motion_primitive.theta_res) mps = [] for delta_state in zip(x,y,th): #print delta_state length = dubins.path_length((0,0,start_angle), delta_state, motion_primitive.turning_radius) if length < np.pi*motion_primitive.turning_radius and length > 0.005: mp = motion_primitive(np.array(delta_state),start_angle) print mp.cost, mp.start_angle, mp.delta_state if mp.bounding_poly is not None: mps.append(mp) else: print "failed check" # Add reverse to each layer back_lengths = np.arange(1.0,3.5,0.5) for back_len in back_lengths: delta_state = (back_len*np.cos(start_angle),back_len*np.sin(start_angle),start_angle) mp = motion_primitive(np.array(delta_state),start_angle, True) print mp.cost, mp.start_angle, mp.delta_state print mp.path mps.append(mp) print len(mps) motion_primitives[np.around(start_angle/motion_primitive.theta_res)] = mps ''' print '\n' i = 0 for mps in motion_primitives: print 'layer ' + str(i) print mps i += 1 for prim in motion_primitives[mps]: print prim.path[0], prim.path[-1] pdb.set_trace() ''' return motion_primitives
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')
delta_states = [np.array([ 5, 0, 0.0]), np.array([ 5, 5, math.pi/2.0]), np.array([ 5, -5, -math.pi/2.0]), np.array([ 5, 5, 0.0]), np.array([ 5, -5, 0.0]), np.array([ 5, 10, math.pi/2.0]), np.array([ 5, -10, -math.pi/2.0]), np.array([ 5, 10, 0.0]), np.array([ 5, -10, 0.0]), np.array([-1, 0, 0.0])] plt.figure(1) plt.subplot(111) motion_primitives = [] for i in range(0,len(delta_states)): cost = dubins.path_length((0,0,0), delta_states[i], turning_radius) motion_primitives.append(motion_primitive(delta_states[i],cost)) pathx = [] pathy = [] for state in motion_primitives[i].path: 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)
def test_turning_radius_scaling(self): a = dubins.path_length((0,0,0), (10,10,math.pi/4.0), 1.0) b = dubins.path_length((0,0,0), (10,10,math.pi/4.0), 2.0) self.assert_(b > a)
def test_non_unit_turning_radius(self): length = dubins.path_length((0,0,0), (10,0,0), 2.0) self.assertAlmostEqual(length, 10.0)
def test_half_loop(self): r = 1.0 dx = r * math.sqrt(2)/2 dy = r * math.sqrt(2)/2 length = dubins.path_length((0,0,0), (0, 2*r, -math.pi), r) self.assertAlmostEqual(length, math.pi*r)
def test_almost_full_loop(self): r = 2.0 dy = 0.1 length = dubins.path_length((0,0,0), (0,-dy,0), r) self.assertAlmostEqual(length, 2*math.pi*r + dy)
def test_simple(self): length = dubins.path_length((0,0,0), (10,0,0), 1.0) self.assertAlmostEqual(length, 10.0)
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 pathlength = dubins.path_length(q0, q1, turning_radius) print '\npath length: ' + repr(pathlength)