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
