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)
