def __init__(self, world, start, goal):
"""
This is the constructor for the trajectory object. A fresh trajectory
object will be constructed before each mission. For a world trajectory,
the input arguments are start and end positions and a world object. You
are free to choose the path taken in any way you like.
You should initialize parameters and pre-compute values such as
polynomial coefficients here.
Parameters:
world, World object representing the environment obstacles
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
"""
# You must choose resolution and margin parameters to use for path
# planning. In the previous project these were provided to you; now you
# must chose them for yourself. Your may try these default values, but
# you should experiment with them!
self.resolution = np.array([0.25, 0.25, 0.25])
self.margin = 0.5
# You must store the dense path returned from your Dijkstra or AStar
# graph search algorithm as an object member. You will need it for
# debugging, it will be used when plotting results.
self.path = graph_search(world,
self.resolution,
self.margin,
start,
goal,
astar=True)
# You must generate a sparse set of waypoints to fly between. Your
# original Dijkstra or AStar path probably has too many points that are
# too close together. Store these waypoints as a class member; you will
# need it for debugging and it will be used when plotting results.
self.points = np.zeros((1, 3)) # shape=(n_pts,3)
# Finally, you must compute a trajectory through the waypoints similar
# to your task in the first project. One possibility is to use the
# WaypointTraj object you already wrote in the first project. However,
# you probably need to improve it using techniques we have learned this
# semester.
# STUDENT CODE HERE
self.world = world
self.max_sparse_dist = 1 # 1.8
self.max_speed = 2.3 # 2.5
self.points = self.get_sparse_points(self.path)
self.time_param = self.get_time_segment(self.points)
self.coefficients = self.get_spline_coeff(self.points,
self.time_param[0])
def __init__(self, world, start, goal):
"""
This is the constructor for the trajectory object. A fresh trajectory
object will be constructed before each mission. For a world trajectory,
the input arguments are start and end positions and a world object. You
are free to choose the path taken in any way you like.
You should initialize parameters and pre-compute values such as
polynomial coefficients here.
Parameters:
world, World object representing the environment obstacles
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
"""
# You must choose resolution and margin parameters to use for path
# planning. In the previous project these were provided to you; now you
# must chose them for yourself. Your may try these default values, but
# you should experiment with them!
self.resolution = np.array([0.25, 0.25, 0.25])
self.margin = 0.25
# You must store the dense path returned from your Dijkstra or AStar
# graph search algorithm as an object member. You will need it for
# debugging, it will be used when plotting results.
self.graphpath = graph_search(world,
self.resolution,
self.margin,
start,
goal,
astar=True)
# You must generate a sparse set of waypoints to fly between. Your
# original Dijkstra or AStar path probably has too many points that are
# too close together. Store these waypoints as a class member; you will
# need it for debugging and it will be used when plotting results.
# self.points = np.zeros((1,3)) # shape=(n_pts,3)
self.Start = start
self.Goal = goal
self.points = self.graphpath
self.path = self.graphpath
self.path_length = float(
round(np.sum(np.linalg.norm(np.diff(self.path, axis=0), axis=1)),
3))
self.v_av = 2.0
self.Time_list = self.get_time_list(self.v_av, self.path,
self.path_length)
self.K = self.get_coeff_5order(self.Time_list, self.path)
def __init__(self, world, start, goal):
"""
This is the constructor for the trajectory object. A fresh trajectory
object will be constructed before each mission. For a world trajectory,
the input arguments are start and end positions and a world object. You
are free to choose the path taken in any way you like.
You should initialize parameters and pre-compute values such as
polynomial coefficients here.
Parameters:
world, World object representing the environment obstacles
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
"""
# You must choose resolution and margin parameters to use for path
# planning. In the previous project these were provided to you; now you
# must chose them for yourself. Your may try these default values, but
# you should experiment with them!
self.resolution = np.array([0.2, 0.2, 0.2])
self.margin = 0.3
# You must store the dense path returned from your Dijkstra or AStar
# graph search algorithm as an object member. You will need it for
# debugging, it will be used when plotting results.
self.path = graph_search(world,
self.resolution,
self.margin,
start,
goal,
astar=True)
# You must generate a sparse set of waypoints to fly between. Your
# original Dijkstra or AStar path probably has too many points that are
# too close together. Store these waypoints as a class member; you will
# need it for debugging and it will be used when plotting results.
self.points = np.zeros((1, 3)) # shape=(n_pts,3)
# self.points = self.path
self.points = np.array(self.path[0])
for i in range(self.path.shape[0]):
if i != 0 and i != (self.path.shape[0] - 1):
segment_1 = (self.path[i] - self.path[i - 1])
segment_2 = (self.path[i + 1] - self.path[i - 1])
linear_segment = np.round(np.cross(segment_1, segment_2), 5)
if np.array_equal(linear_segment, [0, 0, 0]):
pass
else:
self.points = np.vstack((self.points, self.path[i]))
print('added point')
self.points = np.vstack((self.points, self.path[-1]))
print(self.points.shape)
# Finally, you must compute a trajectory through the waypoints similar
# to your task in the first project. One possibility is to use the
# WaypointTraj object you already wrote in the first project. However,
# you probably need to improve it using techniques we have learned this
# semester.
# STUDENT CODE HERE
self.v = 2.5 # m/s
self.t = np.zeros(len(self.points), )
for i in range(len(self.t) - 1):
self.t[(i + 1)] = np.linalg.norm(
(self.points[(i + 1)] - self.points[i])) / self.v
self.point_t = np.zeros(len(self.points), )
for i in range(int(len(self.t) - 1)):
self.point_t[(i + 1)] = self.point_t[i] + self.t[i + 1]
# self.f = CubicSpline(self.point_t, self.points, axis=0)
self.f = interp1d(self.point_t, self.points, axis=0)
def __init__(self, world, start, goal):
"""
This is the constructor for the trajectory object. A fresh trajectory
object will be constructed before each mission. For a world trajectory,
the input arguments are start and end positions and a world object. You
are free to choose the path taken in any way you like.
You should initialize parameters and pre-compute values such as
polynomial coefficients here.
Parameters:
world, World object representing the environment obstacles
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
"""
# You must choose resolution and margin parameters to use for path
# planning. In the previous project these were provided to you; now you
# must chose them for yourself. Your may try these default values, but
# you should experiment with them!
self.resolution = np.array([0.125, 0.125, 0.125])
self.margin = 0.3
# You must store the dense path returned from your Dijkstra or AStar
# graph search algorithm as an object member. You will need it for
# debugging, it will be used when plotting results.
self.path = graph_search(world,
self.resolution,
self.margin,
start,
goal,
astar=True)
#self.margin = 0.4
# You must generate a sparse set of waypoints to fly between. Your
# original Dijkstra or AStar path probably has too many points that are
# too close together. Store these waypoints as a class member; you will
# need it for debugging and it will be used when plotting results.
self.points = np.zeros((1, 3)) # shape=(n_pts,3)
self.points = [self.path[0]]
# loop through and check if points can be removed from path without issue
path_index = 2
point_index = 0
# have 2 indexes, one for the looping through the path, the other for the points being kept
while path_index < len(self.path):
# check if there is a collision when you remove a point
if world.path_collisions(
np.vstack(
(self.path[path_index], self.points[point_index])),
self.margin).size == 0:
# if there isnt, check the next point
path_index += 1
else:
# if there is add the point skipped to the kept points
point_index += 1
path_index += 1
self.points.append(self.path[path_index - 1])
# append the final point to the list
self.points.append(self.path[-1])
# Finally, you must compute a trajectory through the waypoints similar
# to your task in the first project. One possibility is to use the
# WaypointTraj object you already wrote in the first project. However,
# you probably need to improve it using techniques we have learned this
# semester.
# STUDENT CODE HERE
# Waypoint Traj is Bang Bang with non zero endpoint velocities based on a heuristic that is a function of the
# angle between the lines
self.traj = WaypointTraj(self.points)
def __init__(self, world, start, goal):
"""
This is the constructor for the trajectory object. A fresh trajectory
object will be constructed before each mission. For a world trajectory,
the input arguments are start and end positions and a world object. You
are free to choose the path taken in any way you like.
You should initialize parameters and pre-compute values such as
polynomial coefficients here.
Parameters:
world, World object representing the environment obstacles
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
"""
# You must choose resolution and margin parameters to use for path
# planning. In the previous project these were provided to you; now you
# must chose them for yourself. Your may try these default values, but
# you should experiment with them!
# self.resolution = np.array([0.25, 0.25, 0.25])
self.resolution = np.array([0.2, 0.2, 0.2])
self.margin = 0.3
# You must store the dense path returned from your Dijkstra or AStar
# graph search algorithm as an object member. You will need it for
# debugging, it will be used when plotting results.
self.path = graph_search(world,
self.resolution,
self.margin,
start,
goal,
astar=True)
# print("origin length: {}".format(self.path.shape[0]))
# remove unnecessary points:
self.path = np.delete(self.path, 1, axis=0)
# self.path = np.delete(self.path, 2, axis=0)
self.path = np.delete(self.path, -2, axis=0)
self.path = rm_inter_pt(self.path)
# print("new length: {}".format(self.path.shape[0]))
# You must generate a sparse set of waypoints to fly between. Your
# original Dijkstra or AStar path probably has too many points that are
# too close together. Store these waypoints as a class member; you will
# need it for debugging and it will be used when plotting results.
# self.points = np.zeros((1,3)) # shape=(n_pts,3)
self.points = np.delete(self.path, 1, axis=0)
# self.path = np.delete(self.path, 2, axis=0)
self.points = np.delete(self.path, -2, axis=0)
self.points = rm_inter_pt(self.path)
# Finally, you must compute a trajectory through the waypoints similar
# to your task in the first project. One possibility is to use the
# WaypointTraj object you already wrote in the first project. However,
# you probably need to improve it using techniques we have learned this
# semester.
self.start = start
self.goal = goal
# STUDENT CODE HERE
self.velo = 2.5
self.T_finish = 0.0
self.Length = 0.0
self.new_range = 0
self.traj_x, self.traj_y, self.traj_z, \
self.velo_x, self.velo_y, self.velo_z, \
self.acc_x, self.acc_y, self.acc_z, \
self.jerk_x, self.jerk_y, self.jerk_z = self.get_traj()
def __init__(self, world, start, goal):
"""
This is the constructor for the trajectory object. A fresh trajectory
object will be constructed before each mission. For a world trajectory,
the input arguments are start and end positions and a world object. You
are free to choose the path taken in any way you like.
You should initialize parameters and pre-compute values such as
polynomial coefficients here.
Parameters:
world, World object representing the environment obstacles
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
"""
# You must choose resolution and margin parameters to use for path
# planning. In the previous project these were provided to you; now you
# must chose them for yourself. Your may try these default values, but
# you should experiment with them!
# self.resolution = np.array([0.25, 0.25, 0.25])
# self.margin = 0.5
# for lab:
self.resolution = np.array([0.125, 0.125, 0.125])
self.margin = 0.3
# You must store the dense path returned from your Dijkstra or AStar
# graph search algorithm as an object member. You will need it for
# debugging, it will be used when plotting results.
# You must generate a sparse set of waypoints to fly between. Your
# original Dijkstra or AStar path probably has too many points that are
# too close together. Store these waypoints as a class member; you will
# need it for debugging and it will be used when plotting results.
# self.points = np.zeros((1,3)) # shape=(n_pts,3)
self.path = graph_search(world,
self.resolution,
self.margin,
start,
goal,
astar=True)
print(self.path)
exit()
self.points = self.path.copy()
# print('points:\n', self.points)
print(type(self.points))
print(self.points.shape)
if np.array_equal(self.points[0, :], self.points[1, :]):
self.points = np.delete(self.points, 1, 0)
# ------------------------------------------------------------------------
# p_curr1 = 0
# p_curr2 = 1
# p_check = 2
# v_curr = (self.points[p_curr2, :] - self.points[p_curr1, :]) / np.sqrt((np.square(self.points[p_curr2, :] - self.points[p_curr1, :])).sum())
# v_check = (self.points[p_check, :] - self.points[p_curr1, :]) / np.sqrt((np.square(self.points[p_check, :] - self.points[p_curr1, :])).sum())
#
# while True:
# if np.linalg.norm(v_check - v_curr) < 0.00001:
# self.points = np.delete(self.points, p_curr2, 0)
# if p_check == self.points.shape[0] - 1:
# break
# else:
# p_curr1 = p_check - 1
# p_curr2 = p_curr1 + 1
# p_check = p_curr2 + 1
# v_curr = (self.points[p_curr2, :] - self.points[p_curr1, :]) / np.sqrt(
# (np.square(self.points[p_curr2, :] - self.points[p_curr1, :])).sum())
# if p_check == self.points.shape[0] - 1:
# break
#
# v_check = (self.points[p_check, :] - self.points[p_curr1, :]) / np.sqrt(
# (np.square(self.points[p_check, :] - self.points[p_curr1, :])).sum())
#
# print('new points:\n', self.points)
# print(self.points.shape)
# self.Ps = self.points
# ------------------------------------------------------------------------
ph = 0
pch = 1
i = 0
while True:
i += 1
# print(i)
pathtmp = np.vstack((self.points[ph, :], self.points[pch, :]))
# flag = world.path_collisions(pathtmp, margin)
# print(flag.shape)
# print('flag:', flag)
if world.path_collisions(pathtmp,
self.margin + 0.05).shape[0] == 0:
self.points = np.delete(self.points, pch, 0)
# pch = pch + 1
else:
ph = pch
pch = pch + 1
if pch == self.points.shape[0] - 1:
break
print('after points:\n', self.points)
self.Ps = self.points
def __init__(self, world, start, goal):
"""
This is the constructor for the trajectory object. A fresh trajectory
object will be constructed before each mission. For a world trajectory,
the input arguments are start and end positions and a world object. You
are free to choose the path taken in any way you like.
You should initialize parameters and pre-compute values such as
polynomial coefficients here.
Parameters:
world, World object representing the environment obstacles
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
"""
# You must choose resolution and margin parameters to use for path
# planning. In the previous project these were provided to you; now you
# must chose them for yourself. Your may try these default values, but
# you should experiment with them!
self.resolution = np.array([0.25, 0.25, 0.25])
self.margin = 0.5
# You must store the dense path returned from your Dijkstra or AStar
# graph search algorithm as an object member. You will need it for
# debugging, it will be used when plotting results.
self.path = graph_search(world,
self.resolution,
self.margin,
start,
goal,
astar=True)
# You must generate a sparse set of waypoints to fly between. Your
# original Dijkstra or AStar path probably has too many points that are
# too close together. Store these waypoints as a class member; you will
# need it for debugging and it will be used when plotting results.
self.points = np.zeros((1, 3)) # shape=(n_pts,3)
# Finally, you must compute a trajectory through the waypoints similar
# to your task in the first project. One possibility is to use the
# WaypointTraj object you already wrote in the first project. However,
# you probably need to improve it using techniques we have learned this
# semester.
# STUDENT CODE HERE
# trim the path into sparse waypoint combination
n, d = self.path.shape
path = self.path
removeList = []
for i in range(n - 2):
if path[i + 2][0] - path[i + 1][0] == path[i + 1][0] - path[i][0] and path[i + 2][1] - path[i + 1][1] == \
path[i + 1][1] - path[i][1] and path[i + 2][2] - path[i + 1][2] == path[i + 1][2] - path[i][2]:
removeList.append(i + 1)
self.points = np.delete(path, removeList, axis=0)
self.num_pt, d = self.points.shape
self.maxVel = 2.57
self.x_int_pt = self.points[0:self.num_pt - 1, 0]
self.x_fin_pt = self.points[1:self.num_pt, 0]
self.y_int_pt = self.points[0:self.num_pt - 1, 1]
self.y_fin_pt = self.points[1:self.num_pt, 1]
self.z_int_pt = self.points[0:self.num_pt - 1, 2]
self.z_fin_pt = self.points[1:self.num_pt, 2]
self.t_spend = []
self.t_spend.append(0)
# estimate time spent on each path
for i in range(self.num_pt - 1):
x_dist = np.abs(self.x_fin_pt[i] - self.x_int_pt[i])
y_dist = np.abs(self.y_fin_pt[i] - self.y_int_pt[i])
z_dist = np.abs(self.z_fin_pt[i] - self.z_int_pt[i])
t_x = x_dist / self.maxVel
t_y = y_dist / self.maxVel
t_z = z_dist / self.maxVel
t_max = np.max([t_x, t_y, t_z])
self.t_spend.append(t_max + self.t_spend[-1])
def __init__(self, world, start, goal):
"""
This is the constructor for the trajectory object. A fresh trajectory
object will be constructed before each mission. For a world trajectory,
the input arguments are start and end positions and a world object. You
are free to choose the path taken in any way you like.
You should initialize parameters and pre-compute values such as
polynomial coefficients here.
Parameters:
world, World object representing the environment obstacles
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
"""
# You must choose resolution and margin parameters to use for path
# planning. In the previous project these were provided to you; now you
# must chose them for yourself. Your may try these default values, but
# you should experiment with them!
self.resolution = np.array([0.25, 0.25, 0.25])
self.margin = 0.5
yita = 2.2
#maze
if np.array_equal(np.array(world.world['goal']),
np.array([9.0, 7.0, 1.5])):
yita = 2.6
#under over
if np.array_equal(np.array(world.world['goal']),
np.array([19.0, 2.5, 5.5])):
yita = 2.3
#window
if np.array_equal(np.array(world.world['goal']),
np.array([8.0, 18.0, 3.0])):
yita = 2.55
#if np.linalg.norm(np.array(world.world['goal'])-np.array(world.world['start']))>35:
#yita = 2.5
# You must store the dense path returned from your Dijkstra or AStar
# graph search algorithm as an object member. You will need it for
# debugging, it will be used when plotting results.
# You must generate a sparse set of waypoints to fly between. Your
# original Dijkstra or AStar path probably has too many points that are
# too close together. Store these waypoints as a class member; you will
# need it for debugging and it will be used when plotting results.
self.points = graph_search(world,
self.resolution,
self.margin,
start,
goal,
astar=True)
self.points = list(self.points)
for point in range(len(self.points) - 1):
if math.sqrt(sum(
(self.points[point + 1] - self.points[point])**2)) > 3:
self.points.insert(
point + 1,
(self.points[point + 1] + self.points[point]) / 2)
for point in range(len(self.points) - 1):
if math.sqrt(sum(
(self.points[point + 1] - self.points[point])**2)) > 3:
self.points.insert(
point + 1,
(self.points[point + 1] + self.points[point]) / 2)
for point in range(len(self.points) - 1):
if math.sqrt(sum(
(self.points[point + 1] - self.points[point])**2)) > 3:
self.points.insert(
point + 1,
(self.points[point + 1] + self.points[point]) / 2)
for point in range(len(self.points) - 1):
if math.sqrt(sum(
(self.points[point + 1] - self.points[point])**2)) > 3:
self.points.insert(
point + 1,
(self.points[point + 1] + self.points[point]) / 2)
for point in range(len(self.points) - 1):
if math.sqrt(sum(
(self.points[point + 1] - self.points[point])**2)) > 3:
self.points.insert(
point + 1,
(self.points[point + 1] + self.points[point]) / 2)
for point in range(len(self.points) - 1):
if math.sqrt(sum(
(self.points[point + 1] - self.points[point])**2)) > 3:
self.points.insert(
point + 1,
(self.points[point + 1] + self.points[point]) / 2)
for point in range(len(self.points) - 1):
if math.sqrt(sum(
(self.points[point + 1] - self.points[point])**2)) > 3:
self.points.insert(
point + 1,
(self.points[point + 1] + self.points[point]) / 2)
for point in range(len(self.points) - 1):
if math.sqrt(sum(
(self.points[point + 1] - self.points[point])**2)) > 3:
self.points.insert(
point + 1,
(self.points[point + 1] + self.points[point]) / 2)
self.points = np.array(self.points)
print(self.points)
# Finally, you must compute a trajectory through the waypoints similar
# to your task in the first project. One possibility is to use the
# WaypointTraj object you already wrote in the first project. However,
# you probably need to improve it using techniques we have learned this
# semester.
# STUDENT CODE HERE
m = len(self.points) - 1
# the length of each segment
segments_len = np.array([
np.linalg.norm(self.points[i + 1] - self.points[i])
for i in range(m)
]) #shape=(m,)
# the time cost for each segment: [0:t0],[0,t1],..
t = segments_len / yita #shape= (m,)
C = np.zeros((6 * m, 3)) #coefficients for x, y and z
for j in range(3): #x,y,z direction
A = np.zeros((1, 6 * m))
# position constraints: start & end constraint for each segment
for i in range(m):
start = np.zeros((1, 6 * m))
start[0, 6 * i] = 1
A = np.vstack((A, start))
end = np.zeros((1, 6 * m))
end[0, 6 * i:6 * (i + 1)] = np.array(
[1, t[i], t[i]**2, t[i]**3, t[i]**4, t[i]**5])
A = np.vstack((A, end))
#currently, A.shape=(2m,6m)
#add derivative constraints: 2 for each inter-point: 2(m-1)
for i in range(m - 1):
#velocity continuity
v = np.zeros((1, 6 * m))
v[0, 6 * i:6 * (i + 1)] = np.array(
[0, 1, 2 * t[i], 3 * t[i]**2, 4 * t[i]**3, 5 * t[i]**4])
v[0, 6 * (i + 1) + 1] = -1
A = np.vstack((A, v))
#acceleration continuity
a = np.zeros((1, 6 * m))
a[0, 6 * i:6 * (i + 1)] = np.array(
[0, 0, 2, 6 * t[i], 12 * t[i]**2, 20 * t[i]**3])
a[0, 6 * (i + 1) + 2] = -2
A = np.vstack((A, a))
#add arbitrary constraints
jerk = np.zeros((1, 6 * m))
jerk[0, 6 * i:6 * (i + 1)] = np.array(
[0, 0, 0, 6, 24 * t[i], 60 * t[i]**2])
jerk[0, 6 * (i + 1) + 3] = -6
A = np.vstack((A, jerk))
snap = np.zeros((1, 6 * m))
snap[0, 6 * i:6 * (i + 1)] = np.array(
[0, 0, 0, 0, 24, 120 * t[i]])
jerk[0, 6 * (i + 1) + 4] = -24
A = np.vstack((A, snap))
#add 4 derivative boundary conditions:
start_v = np.zeros((1, 6 * m))
start_v[0, 1] = 1
A = np.vstack((A, start_v))
start_a = np.zeros((1, 6 * m))
start_a[0, 2] = 2
A = np.vstack((A, start_a))
end_v = np.zeros((1, 6 * m))
end_v[0, 6 * (m - 1):6 * m] = np.array(
[0, 1, 2 * t[-1], 3 * t[-1]**2, 4 * t[-1]**3, 5 * t[-1]**4])
A = np.vstack((A, end_v))
end_a = np.zeros((1, 6 * m))
end_a[0, 6 * (m - 1):6 * m] = np.array(
[0, 0, 2, 6 * t[-1], 12 * t[-1]**2, 20 * t[-1]**3])
A = np.vstack((A, end_a))
A = A[1:]
b = np.zeros(6 * m)
for i in range(m):
b[2 * i:2 * (i + 1)] = np.array(
[self.points[i, j], self.points[i + 1, j]])
#currently, A.shape = (4m+2,6m)
#ge the matrix form of the quatratic function
# H = np.zeros((6*m,6*m))
# for i in range(m-1):
# a = 36*t[m,j]
# b_ = 24**2/3*t[m,j]**3
# c = 3600/5*t[m,j]**5
# d = 2*24*6/2*t[m,j]**2
# e = 2*60*6/3*t[m,j]**3
# f = 120*24/3*t[m,j]**3
# H[6*m+3:6*(m+1),6*m+3:6*(m+1)] = np.array([[a,d/2,e/2],[d/2,b_,f/2],[e/2,f/2,c]])
# #CVXOPT
# sol = solvers.qp(H,0,G=None,h=None,A=A,b=b)
# C[:,j] = sol['x']
# print('b.shape=',b.shape)
C[:, j] = np.linalg.inv(A) @ b
self.C = C
self.m = m
self.t = t
def __init__(self, world, start, goal):
"""
This is the constructor for the trajectory object. A fresh trajectory
object will be constructed before each mission. For a world trajectory,
the input arguments are start and end positions and a world object. You
are free to choose the path taken in any way you like.
You should initialize parameters and pre-compute values such as
polynomial coefficients here.
Parameters:
world, World object representing the environment obstacles
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
"""
# You must choose resolution and margin parameters to use for path
# planning. In the previous project these were provided to you; now you
# must chose them for yourself. Your may try these default values, but
# you should experiment with them!
self.resolution = np.array([0.2, 0.2, 0.2])
self.margin = 0.4
# You must store the dense path returned from your Dijkstra or AStar
# graph search algorithm as an object member. You will need it for
# debugging, it will be used when plotting results.
self.path = graph_search(world,
self.resolution,
self.margin,
start,
goal,
astar=True)
# You must generate a sparse set of waypoints to fly between. Your
# original Dijkstra or AStar path probably has too many points that are
# too close together. Store these waypoints as a class member; you will
# need it for debugging and it will be used when plotting results.
self.points = np.zeros((1, 3)) # shape=(n_pts,3)
self.points = self.path
# Finally, you must compute a trajectory through the waypoints similar
# to your task in the first project. One possibility is to use the
# WaypointTraj object you already wrote in the first project. However,
# you probably need to improve it using techniques we have learned this
# semester.
# STUDENT CODE HERE
occ_map = OccupancyMap(world, self.resolution, self.margin)
def check_collision(p1, p2):
"""
This function check whether the connection of two waypoints collides with each other
Args:
p1: coordinates of point 1, a shape(3,) array
p2: coordinates of point 2, a shape(3,) array
Returns:
1: The connection of two waypoint collide
0: The connection of two waypoint does not collide
"""
seg_length = 0.1
distance = np.linalg.norm(p2 - p1)
unit_vec = (p2 - p1) / distance
segment = ceil(distance / seg_length)
seg_length = distance / segment # re-define the length of each segment
# store segment points
for i in range(1, segment):
seg_point = p1 + i * seg_length * unit_vec
seg_point_x = seg_point[0]
seg_point_y = seg_point[1]
seg_point_z = seg_point[2]
seg_point = (seg_point_x, seg_point_y, seg_point_z)
is_valid = occ_map.is_valid_metric(seg_point)
is_occupied = occ_map.is_occupied_metric(seg_point)
if is_valid and not (is_occupied):
bool_collide = 0
else:
bool_collide = 1
break
return bool_collide
# optimize path
check_point = start
check_point_idx = 0
check_goal = goal
idx = len(self.path) - 1
goal_idx = idx
new_path = np.array([start])
while not (check_point == goal).all():
while check_collision(check_point,
check_goal) and idx - check_point_idx > 1:
idx = idx - 1
check_goal = self.path[idx]
check_point = check_goal
check_point_idx = idx
check_goal = goal
idx = goal_idx
new_path = np.r_[new_path, [check_point]]
self.path = new_path
# quint trajectory starts here
self.pt = self.path # type: np.array
self.num_pts = len(self.pt)
self.acc_mean = 3.8 # the mean acceleartion
self.t_segment = [0]
self.t_between_pt = []
time_diff = 0
for i in range(0, self.num_pts - 1):
time_diff = 2 * np.sqrt(
np.linalg.norm(self.pt[i + 1] - self.pt[i])) / self.acc_mean
self.t_segment.append(
time_diff + self.t_segment[i]) # time for waypoint to reach
self.t_between_pt.append(
time_diff) # time different between two points
tg = self.t_between_pt[-1]
A = np.zeros((6 * (self.num_pts - 1), 6 * (self.num_pts - 1)))
# boundary constrain
A[0, 5] = 1
A[1, 4] = 1
A[2, 3] = 2
A[-3, -6:] = np.array([tg**5, tg**4, tg**3, tg**2, tg, 1])
A[-2, -6:] = np.array([5 * tg**4, 4 * tg**3, 3 * tg**2, 2 * tg, 1, 0])
A[-1, -6:] = np.array([20 * tg**3, 12 * tg**2, 6 * tg, 2, 0, 0])
# continuous constrain
for i in range(self.num_pts - 2):
tg = self.t_between_pt[i]
# position constrain
A[2 * i + 3, i * 6:(i + 1) * 6] = np.array(
[tg**5, tg**4, tg**3, tg**2, tg, 1])
A[2 * i + 4, (i + 2) * 6 - 1] = 1
# velosity constrain
A[3 + (self.num_pts - 2) * 2 + i, i * 6:(i + 1) * 6] = np.array(
[5 * tg**4, 4 * tg**3, 3 * tg**2, 2 * tg, 1, 0])
A[3 + (self.num_pts - 2) * 2 + i, (i + 2) * 6 - 2] = -1
# acceleration constrain
A[3 + (self.num_pts - 2) * 3 + i, i * 6:(i + 1) * 6] = np.array(
[20 * tg**3, 12 * tg**2, 6 * tg, 2, 0, 0])
A[3 + (self.num_pts - 2) * 3 + i, (i + 2) * 6 - 3] = -2
# jerk constrain
A[3 + (self.num_pts - 2) * 4 + i,
i * 6:(i + 1) * 6] = np.array([60 * tg**2, 24 * tg, 6, 0, 0, 0])
A[3 + (self.num_pts - 2) * 4 + i, (i + 2) * 6 - 4] = -6
# snap constrain
A[3 + (self.num_pts - 2) * 5 + i,
i * 6:(i + 1) * 6] = np.array([120 * tg, 24, 0, 0, 0, 0])
A[3 + (self.num_pts - 2) * 5 + i, (i + 2) * 6 - 5] = -24
# P vector
px = np.zeros((6 * (self.num_pts - 1), 1))
py = np.zeros((6 * (self.num_pts - 1), 1))
pz = np.zeros((6 * (self.num_pts - 1), 1))
px[0, 0] = start[0]
py[0, 0] = start[1]
pz[0, 0] = start[2]
px[-3, 0] = goal[0]
py[-3, 0] = goal[1]
pz[-3, 0] = goal[2]
for i in range(self.num_pts - 2):
px[2 * i + 3, 0] = new_path[i + 1, 0]
px[2 * i + 4, 0] = new_path[i + 1, 0]
py[2 * i + 3, 0] = new_path[i + 1, 1]
py[2 * i + 4, 0] = new_path[i + 1, 1]
pz[2 * i + 3, 0] = new_path[i + 1, 2]
pz[2 * i + 4, 0] = new_path[i + 1, 2]
self.Cx = np.linalg.inv(A) @ px
self.Cy = np.linalg.inv(A) @ py
self.Cz = np.linalg.inv(A) @ pz
