def get_sparse_point(self, path_points):
'''
This function takes in the points from graph search and removes some unnecessary ones
Input: path_points a list[] of points in metrics
Output: sparse_points a new list[] of points
'''
start = path_points[0] # first point is the start
goal = path_points[-1] # last goal
# While not required, we have provided an occupancy map you may use or modify.
occ_map = OccupancyMap(self.world, self.resolution, self.margin)
# Retrieve the index in the occupancy grid matrix corresponding to a position in space.
start_index = tuple(occ_map.metric_to_index(start))
goal_index = tuple(occ_map.metric_to_index(goal))
map = occ_map.map # initialize the map
map_shape = occ_map.map.shape # get the shape of the map [x, y, z]
print(map_shape) ############################### for test
mean_edge = np.array(map_shape).mean() * np.mean(
self.resolution) # mean is used
print(mean_edge, type(mean_edge))
dis_threshold = 0.2 * mean_edge # 0.5 * mean_edge
print(dis_threshold)
#=========== Connect the points if not collide ========
sparse_point = [start] # return the sparse point list
available_point = start # set initial search point to be start point[0]
check_point = path_points[1] # checking start from the second point
points_queue = path_points.copy().tolist(
) # pop out the checked point / path_point is np.array, convert to list
points_queue.pop(
0
) # remove the first start / point_queue must be a list for "pop" attribute
# while available_point is not goal: # is goal is updated then the whole path is checked
while points_queue: # point_queue is not empty
check_point = points_queue[0] # the queue is being popped
if self.collision_detect(np.array(available_point),
np.array(check_point),
occ_map) is True: #and \
#np.linalg.norm(np.array(available_point)-np.array(check_point)) < dis_threshold: # no collision & distance within range, connect!
points_queue.pop(0) # pop out the checked 1st point
last_point = check_point # current check_point is checked, store into the last point
if (np.array(check_point) == np.array(goal)
).all(): # 1. connection & 2. reached the goal point
sparse_point.append(
check_point
) # At goal: 1. point_queue[0]== goal is popped; 2. store the check_point=goal into path
else: # collide, no pop; update: last_point --> available_point & sparse[], check_point start from this one in the next iter
sparse_point.append(
last_point) # store the last point into sparse points
available_point = last_point # set teh last_point as the available_point
return sparse_point # return the sparse point list
def getPath(start_index, goal_index, parent, map):
path_index = [goal_index]
while path_index[-1] != start_index:
path_index.append(parent[path_index[-1]])
path_index = path_index[::-1]
path = []
for i in path_index:
if i == start_index:
#and i == goal_index:
# path.append(OccupancyMap.index_to_metric_negative_corner(map, i))
path.append(start)
elif i == goal_index:
path.append(goal)
else:
path.append(OccupancyMap.index_to_metric_center(map, i))
path = np.array(path)
return path
def get_sparse_points(self, path):
"""
:param path: a path returned by astar algorithms
:return: a trimmed path consists of sparse waypoints
"""
sparse_points = []
segment = len(self.path) - 1 # get segment number
start_pt = self.path[0, :]
goal_pt = self.path[-1, :]
sparse_points.append(start_pt.tolist())
occ_map = OccupancyMap(self.world, self.resolution, self.margin)
for cur_seg_num in range(segment):
dist = np.sqrt(sum((self.path[cur_seg_num + 1, :] - start_pt)**2))
if self.has_line_collided(start_pt, self.path[cur_seg_num + 1, :],
occ_map):
start_pt = self.path[cur_seg_num, :]
sparse_points.append(start_pt.tolist())
goal_pt_exist = False # since we only append the previous pt
else:
if (dist > self.max_sparse_dist) or (
(cur_seg_num + 1) is
segment): # end pt reached or max dist exceeded
start_pt = self.path[cur_seg_num + 1, :]
sparse_points.append(start_pt.tolist())
goal_pt_exist = True # if not collided, goal pt must be appended
if goal_pt_exist is False:
sparse_points.append(goal_pt.tolist())
# remove the points near the goal
# del sparse_points[-2]
# del sparse_points[-3]
# del sparse_points[-4]
sparse_points = np.asarray(sparse_points)
return sparse_points
def graph_search(world, resolution, margin, start, goal, astar):
"""
Parameters:
world, World object representing the environment obstacles
resolution, xyz resolution in meters for an occupancy map, shape=(3,)
margin, minimum allowed distance in meters from path to obstacles.
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
astar, if True use A*, else use Dijkstra
Output:
path, xyz position coordinates along the path in meters with
shape=(N,3). These are typically the centers of visited
voxels of an occupancy map. The first point must be the
start and the last point must be the goal. If no path
exists, return None.
"""
# While not required, we have provided an occupancy map you may use or modify.
occ_map = OccupancyMap(world, resolution, margin)
# Retrieve the index in the occupancy grid matrix corresponding to a position in space.
start_index = tuple(occ_map.metric_to_index(start))
# print("start index: {}".format(start_index))
goal_index = tuple(occ_map.metric_to_index(goal))
# print(occ_map.map)
x_L, y_L, z_L = occ_map.map.shape[0], occ_map.map.shape[
1], occ_map.map.shape[2]
# print(type(start_index))
# print(goal_index)
if astar == True:
flg = 1
else:
flg = 0
frontier = PriorityQueue()
frontier.push(start_index, 0)
came_from = {}
cost_so_far = {}
came_from[start_index] = start_index
cost_so_far[start_index] = 0
graph = create_graph(occ_map, x_L, y_L, z_L)
# keep track of path using parent dictionary:
parent_ND = {}
parent_ND[start_index] = start_index
# closed = set()
ALL_INDEX = []
for i in range(x_L):
for j in range(y_L):
for k in range(z_L):
ALL_INDEX.append((i, j, k))
closed = set(ALL_INDEX)
# test graph:
# print("test neighbors: {}".format(graph[(1,0,0)]))
# path_idx
path_idx = []
while not frontier.empty():
current_index = frontier.pop()
# print("fron: {}".format(frontier._queue))
if current_index == goal_index:
temp = current_index
# print("temp: {}".format(temp))
# print(came_from)
while temp != came_from[temp]:
path_idx.append(temp)
temp = came_from[temp]
# print("path: {}".format(path_idx[::-1]))
# print("path_length: {}".format(len(path_idx[::-1])))
path_idx.append(start_index)
out_path_idx = path_idx[::-1]
# print("out path index: {}".format(out_path_idx))
out_path = []
for idx in out_path_idx:
temp_pos = occ_map.index_to_metric_center(idx)
temp_pos = temp_pos.tolist()
out_path.append(temp_pos)
final_path_temp = np.array(out_path)
# print(start)
path_a = np.concatenate((start.reshape(1, 3), final_path_temp),
axis=0)
path_b = np.concatenate((path_a, goal.reshape(1, 3)), axis=0)
# final_path_ = path_b
# print("out path: {}".format(final_path))
return path_b
# print(current_index)
for next_index in graph[tuple(current_index)]:
# Use position to compute cost & use index to store values:
# Heuristics represents eu distance:
current_pos = occ_map.index_to_metric_center(current_index)
next_pos = occ_map.index_to_metric_center(next_index)
new_cost = cost_so_far[current_index] + Heuristics(
current_pos, next_pos)
if next_index not in cost_so_far or new_cost < cost_so_far[
next_index]:
# if next_index not in cost_so_far:
# save path and judge if reach the goal:
# parent_ND[next_index] = current_index
# if next_index == goal_index:
# temp = next_index
# while (temp != parent_ND[temp]):
# path_idx.append(temp)
# temp = parent_ND[temp]
# return list(path_idx[::-1])
cost_so_far[next_index] = new_cost
next_pos = occ_map.index_to_metric_center(next_index)
priority = new_cost + flg * Heuristics(
goal, next_pos) # goal is already the position
frontier.push(next_index, priority)
came_from[next_index] = current_index
return None
def graph_search(world, resolution, margin, start, goal, astar):
"""
Parameters:
world, World object representing the environment obstacles
resolution, xyz resolution in meters for an occupancy map, shape=(3,)
margin, minimum allowed distance in meters from path to obstacles.
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
astar, if True use A*, else use Dijkstra
Output:
path, xyz position coordinates along the path in meters with
shape=(N,3). These are typically the centers of visited
voxels of an occupancy map. The first point must be the
start and the last point must be the goal. If no path
exists, return None.
"""
# While not required, we have provided an occupancy map you may use or modify.
occ_map = OccupancyMap(world, resolution, margin)
# Retrieve the index in the occupancy grid matrix corresponding to a position in space.
start_index = tuple(occ_map.metric_to_index(start))
goal_index = tuple(occ_map.metric_to_index(goal))
g_index = np.array(goal_index)
def getNeighbors(curr_index, map):
neighbors = []
diff = [-1, 0, 1]
for i in diff:
for j in diff:
for k in diff:
neighbor_index = [
curr_index[0] + i, curr_index[1] + j, curr_index[2] + k
]
if (map.is_valid_index(neighbor_index)
and not map.is_occupied_index(neighbor_index)):
neighbors.append(tuple(neighbor_index))
neighbors.remove(curr_index)
return neighbors
def getPath(start_index, goal_index, parent, map):
path_index = [goal_index]
while path_index[-1] != start_index:
path_index.append(parent[path_index[-1]])
path_index = path_index[::-1]
path = []
for i in path_index:
if i == start_index:
#and i == goal_index:
# path.append(OccupancyMap.index_to_metric_negative_corner(map, i))
path.append(start)
elif i == goal_index:
path.append(goal)
else:
path.append(OccupancyMap.index_to_metric_center(map, i))
path = np.array(path)
return path
# Initialize variables
open = [] #list of nodes we can still step to
isVisited = {}
g = {} #cost
parent = {}
graph = ((i, j, k) for i in range((occ_map.map.shape[0]))
for j in range((occ_map.map.shape[1]))
for k in range((occ_map.map.shape[2])))
for node in graph:
isVisited[node] = False
g[node] = np.inf
parent[node] = 0
g[start_index] = 0
heappush(open, ([g[start_index], start_index]))
iterations = 0
while (isVisited[goal_index] == False):
curr_index = heappop(open)[1]
if (isVisited[curr_index] == False):
iterations += 1
isVisited[curr_index] = True
curr_neighbors = getNeighbors(curr_index, occ_map)
for v in curr_neighbors:
neighbor_cost = g[curr_index] + np.linalg.norm(
np.array(curr_index) - np.array(v))
if (astar):
heuristic = np.linalg.norm(g_index - np.array(v))
else:
heuristic = 0
if (neighbor_cost < g[v]):
heappush(open, ([neighbor_cost + heuristic, v]))
g[v] = neighbor_cost
parent[v] = curr_index
# build path
if (isVisited[goal_index]):
path = getPath(start_index, goal_index, parent, occ_map)
print("Path found")
# print(f"{iterations} nodes visited")
print(f"{len(path)} nodes visited")
else:
print("Path not found")
return path
def graph_search(world, resolution, margin, start, goal, astar):
"""
Parameters:
world, World object representing the environment obstacles
resolution, xyz resolution in meters for an occupancy map, shape=(3,)
margin, minimum allowed distance in meters from path to obstacles.
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
astar, if True use A*, else use Dijkstra
Output:
path, xyz position coordinates along the path in meters with
shape=(N,3). These are typically the centers of visited
voxels of an occupancy map. The first point must be the
start and the last point must be the goal. If no path
exists, return None.
"""
occ_map = OccupancyMap(world, resolution, margin)
# Retrieve the index in the occupancy grid matrix corresponding to a position in space.
start_index = tuple(occ_map.metric_to_index(start))
goal_index = tuple(occ_map.metric_to_index(goal))
# print('start',start)
# print('end',goal)
# Initialization----------------------------------------------------------------------------------------------------
# PQ = [(0, start_index)] # Priority queue of open/ alive nodes
g_v_cost = np.full(occ_map.map.shape,
np.inf) # Initially set all distances as inf
p_v_parent = np.zeros((occ_map.map.shape[0], occ_map.map.shape[1],
occ_map.map.shape[2], 3)) # parent node
neighbour = []
for i in range(-1, 2):
for j in range(-1, 2):
for k in range(-1, 2):
neighbour.append([i, j, k])
neighbour.remove([0, 0, 0])
# ------------------------------------------------------------------------------------------------------------------
# astar = True
if not astar: # Dijkstra's Algorithm
g_v_cost[start_index] = 0 # distance from start to start is 0
PQ = [(g_v_cost[start_index], start_index)
] # Priority queue initializes
# , (g_v_cost[goal_index], goal_index)
# heapq.heapify(PQ)
# heapq.heapify(PQ)
# print(PQ)
#
# print(g_v_cost[start_index])
# heappop(PQ)
#
# print('new',PQ)
# if (g_v_cost[goal_index],goal_index) in PQ:
# print('Yes')
# print(np.min(g_v_cost))
cnt = 0
while len(PQ) > 0:
# while (g_v_cost[goal_index], goal_index) in PQ and np.min(g_v_cost) < np.inf:
min_element = heappop(PQ) # eg: (0.0,(2,2,2))
u = min_element[
1] # note: here u is tuple. cannot do addition like array
for i in range(26):
v = np.asarray(u) + neighbour[i] # one neighbour
if not occ_map.is_valid_index(v) or occ_map.is_occupied_index(
v):
pass
elif g_v_cost[(v[0], v[1], v[2])] != np.inf:
pass
else:
d = g_v_cost[u] + np.linalg.norm(v - u)
if d < g_v_cost[(
v[0], v[1], v[2]
)]: # need tuple to access g_v_cost # A shorter path to v has been found
g_v_cost[(v[0], v[1], v[2])] = d
heappush(PQ, (d, (v[0], v[1], v[2])))
p_v_parent[v[0], v[1], v[2], :] = u
# print(p_v_parent[v[0], v[1], v[2],:])
cnt += 1
# print('Dijk Node', cnt)
else: # A*
F = np.full(occ_map.map.shape, np.inf) # F(v) = g(v) + h(v)
g_v_cost[start_index] = 0 # distance from start to start is 0
F[start_index] = g_v_cost[start_index] + np.linalg.norm(
np.asarray(goal_index) - np.asarray(start_index))
PQ = [(F[start_index], start_index),
(F[goal_index], goal_index)] # Priority queue initializes
# while len(PQ) > 0:
count = 0
while (F[goal_index], goal_index) in PQ and np.min(F) < np.inf:
min_element = heappop(PQ) # eg: (0.0,(2,2,2))
u = min_element[
1] # note: here u is tuple. cannot do addition like array
for i in range(26):
v = np.asarray(u) + neighbour[i] # one neighbour
if not occ_map.is_valid_index(v) or occ_map.is_occupied_index(
v):
pass
elif g_v_cost[(
v[0], v[1], v[2]
)] != np.inf: # I dont have to find 26 neighbours all the time
pass
else:
d = g_v_cost[u] + np.linalg.norm(v - u)
if d < g_v_cost[(
v[0], v[1], v[2]
)]: # need tuple to access g_v_cost # A shorter path to v has been found
g_v_cost[(v[0], v[1], v[2])] = d
heappush(PQ, (d, (v[0], v[1], v[2])))
p_v_parent[v[0], v[1], v[2], :] = u
F[(v[0], v[1], v[2]
)] = g_v_cost[(v[0], v[1], v[2])] + np.linalg.norm(
np.asarray(goal_index) - np.asarray(v))
count += 1
# print('A Star Node:', count)
# Find Path---------------------------------------------------------------------------------------------------------
Path = []
temp = goal_index
while (temp[0], temp[1], temp[2]) != (start_index[0], start_index[1],
start_index[2]):
Path.append(occ_map.index_to_metric_center(temp))
temp = p_v_parent[int(temp[0]), int(temp[1]), int(temp[2])]
Path.append(occ_map.index_to_metric_center(start_index))
Path.append(start)
Path.reverse()
Path.append(goal)
return np.asarray(Path)
def graph_search(world, resolution, margin, start, goal, astar):
"""
Parameters:
world, World object representing the environment obstacles
resolution, xyz resolution in meters for an occupancy map, shape=(3,)
margin, minimum allowed distance in meters from path to obstacles.
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
astar, if True use A*, else use Dijkstra
Output:
path, xyz position coordinates along the path in meters with
shape=(N,3). These are typically the centers of visited
voxels of an occupancy map. The first point must be the
start and the last point must be the goal. If no path
exists, return None.
"""
# While not required, we have provided an occupancy map you may use or modify.
occ_map = OccupancyMap(world, resolution, margin)
# Retrieve the index in the occupancy grid matrix corresponding to a position in space.
start_index = tuple(occ_map.metric_to_index(start))
goal_index = tuple(occ_map.metric_to_index(goal))
occ_map._init_map_from_world()
all_cost = np.zeros(occ_map.map.shape) + 10000 # add a dummy large number for representing inf.
all_cost[start_index] = 0 # add 0 cost for the start
# heapq stores the tuple with cost and index
Q = [] # heapq storing all the neighbor points
if astar is True:
h0 = np.sqrt((resolution[0] * (start_index[0] - goal_index[0])) ** 2
+ (resolution[1] * (start_index[1] - goal_index[1])) ** 2
+ (resolution[2] * (start_index[2] - goal_index[2])) ** 2)
else:
h0 = 0
heappush(Q, (0 + h0, start_index)) # push the start point
parent = dict() # initialize a empty dict for storing parent nodes
# finding the path
while Q:
cur_node = heappop(Q)
if cur_node[1] is goal_index:
break
for neighbors in find_neighbors(cur_node[1]):
if occ_map.is_valid_index(neighbors) and not occ_map.is_occupied_index(neighbors):
if astar is True: # add heuristic if astar is activated
h = np.sqrt((resolution[0] * (neighbors[0] - goal_index[0])) ** 2
+ (resolution[1] * (neighbors[1] - goal_index[1])) ** 2
+ (resolution[2] * (neighbors[2] - goal_index[2])) ** 2)
else:
h = 0
cost = all_cost[cur_node[1]] + np.sqrt((resolution[0] * (neighbors[0] - cur_node[1][0])) ** 2
+ (resolution[1] * (neighbors[1] - cur_node[1][1])) ** 2
+ (resolution[2] * (neighbors[2] - cur_node[1][2])) ** 2)
if cost < all_cost[neighbors]:
heappush(Q, (cost + h, neighbors)) # child found, push into heap
all_cost[neighbors] = cost # update the cost matrix
parent[neighbors] = cur_node[1]
# print('Is goal still in dict? ', goal_index in parent.keys())
# check if the path exists
if goal_index in parent.keys():
pass
else:
return None
# traverse the parent nodes
parent_key = goal_index # initialize the parent key as goal_index
path = list() # initialize the path list
while True: # goal_index in parent.keys():
parent_key = parent[parent_key] # update the next parent key
if parent_key is start_index:
break
parent_in_metric = occ_map.index_to_metric_center(parent_key)
path.append(tuple(parent_in_metric))
path.reverse() # reverse the order back
path.insert(0, start)
path.append(goal)
path = np.asarray(path)
return path
def graph_search(world, resolution, margin, start, goal, astar):
"""
Parameters:
world, World object representing the environment obstacles
resolution, xyz resolution in meters for an occupancy map, shape=(3,)
margin, minimum allowed distance in meters from path to obstacles.
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
astar, if True use A*, else use Dijkstra
Output:
path, xyz position coordinates along the path in meters with
shape=(N,3). These are typically the centers of visited
voxels of an occupancy map. The first point must be the
start and the last point must be the goal. If no path
exists, return None.
"""
# While not required, we have provided an occupancy map you may use or modify.
occ_map = OccupancyMap(world, resolution, margin)
# Retrieve the index in the occupancy grid matrix corresponding to a position in space.
start_index = tuple(occ_map.metric_to_index(start))
start_cost = 0
start_x,start_y,start_z = start_index
goal_index = tuple(occ_map.metric_to_index(goal))
goal_x,goal_y,goal_z = goal_index
goal_cost = np.inf
heuristic = sqrt((start_x-goal_x)**2+(start_y-goal_y)**2+(start_z-goal_z)**2)
start_f = heuristic
goal_f = np.inf
# Initialization
Q = [] # priority queue of open nodes
G = [] # all the graph
path = np.array([list(goal)]) # initialize path
# world representing in voxel
x = occ_map.map.shape[0]
y = occ_map.map.shape[1]
z = occ_map.map.shape[2]
index = 0
pos = np.zeros((x,y,z)) # position in a list
class grid(object):
"""
This class used to store features of a voxel
"""
def __init__(self,idx,cost_to_come=np.inf,parent=np.NaN,heu=heuristic,cost=np.inf):
self.idx = idx
self.cost_to_come = cost_to_come
self.parent = parent
self.heu = heu
self.cost = cost
# store the features of all node in an object list
for i in range(x):
for j in range(y):
for k in range(z):
pos[i][j][k]=index # store index
if start_index == (i,j,k):
vox = grid(idx=(i,j,k),cost_to_come=0)
start_pos = index
elif goal_index == (i,j,k):
vox = grid(idx=(i,j,k))
goal_pos = index
else:
vox = grid(idx=(i,j,k))
index+=1
G.append(vox) # store all the voxel object in a list *** Do Not Change the idx***
if astar:
heappush(Q,(start_f,G[start_pos].idx))
heappush(Q,(goal_f,G[goal_pos].idx))
# A* algorithm
while (goal_cost,goal_index) in Q and Q[0][0]<np.inf:
u = Q[0] # node that has smllest cost
ux,uy,uz = u[1] # node's coordination
heappop(Q) # alwasy pop out node that has smallest cost
if (ux,uy,uz) == goal_index:
break
# push neigbor(u) into Q
for dx in range(-1,2):
for dy in range(-1,2):
for dz in range(-1,2):
if dx==0 and dy == 0 and dz==0:
continue
# update new node
new_x = dx+ux
new_y = dy+uy
new_z = dz+uz
is_valid_neigbor = occ_map.is_valid_index((new_x,new_y,new_z))
if is_valid_neigbor:
is_occupy = occ_map.is_occupied_index((new_x,new_y,new_z))
if not is_occupy:
idx_in_G = int(pos[new_x][new_y][new_z])
v = G[idx_in_G]
c_u_v = sqrt(dx**2+dy**2+dz**2)
idx_of_u = int(pos[ux][uy][uz])
d = G[idx_of_u].cost_to_come+c_u_v
if d<v.cost_to_come:
heuristic = sqrt((new_x-goal_x)**2+(new_y-goal_y)**2+(new_z-goal_z)**2)
f = heuristic+d # calculate the cost of a node
G[idx_in_G] = grid((new_x,new_y,new_z),d,(ux,uy,uz),heuristic,f) # update new node's feature
heappush(Q,(G[idx_in_G].cost,G[idx_in_G].idx)) # update the open list
else:
heappush(Q,(start_cost,G[start_pos].idx))
heappush(Q,(goal_cost,G[goal_pos].idx))
# Dijstra Algorithms starts here#
while (goal_cost,goal_index) in Q and Q[0][0]<np.inf:
u = Q[0] # node that has smllest cost
ux,uy,uz = u[1] # node's coordination
heappop(Q) # alwasy pop out node that has smallest cost
# push neigbor(u) into h
for dx in range(-1,2):
for dy in range(-1,2):
for dz in range(-1,2):
if dx==0 and dy == 0 and dz==0:
continue
# update new node
new_x = dx+ux
new_y = dy+uy
new_z = dz+uz
is_valid_neigbor = occ_map.is_valid_index((new_x,new_y,new_z))
if is_valid_neigbor:
is_occupy = occ_map.is_occupied_index((new_x,new_y,new_z))
if not is_occupy:
idx_in_G = int(pos[new_x][new_y][new_z])
v = G[idx_in_G]
c_u_v = sqrt(dx**2+dy**2+dz**2)
idx_of_u = int(pos[ux][uy][uz])
d = G[idx_of_u].cost_to_come+c_u_v
if d<v.cost_to_come:
G[idx_in_G] = grid((new_x,new_y,new_z),d,(ux,uy,uz)) # update new node's feature
heappush(Q,(G[idx_in_G].cost_to_come,G[idx_in_G].idx)) # update open list
# trace parent
if np.any(np.isnan(G[goal_pos].parent)):
return None
else:
parent_x,parent_y,parent_z = G[goal_pos].parent
parent = occ_map.index_to_metric_center((parent_x,parent_y,parent_z))
path = np.r_[path,np.array([list(parent)])]
idx_in_G = int(pos[parent_x][parent_y][parent_z])
while 1:
parent_x,parent_y,parent_z = G[idx_in_G].parent
parent = occ_map.index_to_metric_center((parent_x,parent_y,parent_z))
path = np.r_[path,np.array([list(parent)])]
idx_in_G = int(pos[parent_x][parent_y][parent_z])
if idx_in_G == start_pos:
break
path = np.r_[path,np.array([list(start)])]
path = np.flipud(path)
return path
def __init__(self, world):
self.resolution = np.array([0.25, 0.25, 0.25])
self.margin = 0.25
self.occ_map = OccupancyMap(world, self.resolution, self.margin)
self.D = np.zeros((4, 3))
class successors(object):
def __init__(self, world):
self.resolution = np.array([0.25, 0.25, 0.25])
self.margin = 0.25
self.occ_map = OccupancyMap(world, self.resolution, self.margin)
self.D = np.zeros((4, 3))
def reachable(self, Um, x_init, tau):
# TODO
'''
Given an initial state and set of motion primitives return the
set of coeffecients which give all the reachable states
* Also check for collsions
Input
Um = N X 1 X 3 np array of motion primitives
tau = duration of motion primitive
x_init = 1 X 17 array of initial state [x0, v0, a0, q0, w0]
p0 = 1 X 3 -- postion in m
v0 = 1 X 3 -- velocity in m/s
a0 = 1 X 3 -- acceleration in m/s^2
q0 = 1 X 4 -- quaternion
w0 = 1 X 3 -- angular velocity in 1/s^2
Output
Rs = N X 3 X 3 np array of the reachable set from the initial point given the Um
Cs = N X 1 np array of the cost of all the reachable points
'''
N = Um.shape[0]
J0 = Um
x0 = x_init
process_arr = []
Rs = np.zeros((N, 1, 16))
Cs = np.zeros((N, 1))
manager = multiprocessing.Manager()
return_Rs = manager.dict()
return_cc = manager.dict()
D = np.zeros((N, 4, 3))
#pdb.set_trace()
for i in range(N):
collision_flag = False
D[i, :, :] = np.append(x0[0:9], J0[i, :, :]).reshape((4, 3))
xf = D[0, :] + D[1, :] * tau + 0.5 * D[2, :] * tau**2 + (
1 / 6) * D[3, :] * tau**3
collision_flag = self.collision_check(xf[0:2])
Cs[i] = np.sum(np.abs(J0[i, :, :]))
# Forward Simulation and Collison Check
rr, cc = self.forward_simulate(x0, J0, tau, D, Rs, Cs)
return rr, cc
def forward_simulate(self, x0, J0, tau, D, return_Rs, return_cc):
# TODO
'''
Given the desired trajectory run a forward simulation to identify
if the desired trajectory can be followed and identify the dynamic
feasibilty based on the ability of the controller to follow the trajectory
Input:
x0 = 1 X 17 array of initial state [x0, v0, a0, q0, w0]
p0 = 1 X 3 -- postion in m
v0 = 1 X 3 -- velocity in m/s
a0 = 1 X 3 -- acceleration in m/s^2
q0 = 1 X 4 -- quaternion
w0 = 1 X 3 -- angular velocity in 1/s^2
tau = Duration of planner
Output:
Rs = 3 X 3 np array of state at the end of the period after the simulation
err_cs = M X 1 np array of the cost of the trajectory based on the controller's
ability to follow the trajectory
'''
t_final = tau
initial_state = {
'x': tuple(x0[0:3]),
'v': tuple(x0[3:6]),
'q': tuple(x0[9:13]),
'w': tuple(x0[13:16])
}
quadrotor = Quadrotor(quad_params)
my_se3_controller = SE3Control(quad_params)
# Traj object has been created to main tain consistency with the simulator which
# needs the trajectory to be an ibject with an update function
traj = trajectory(D)
(sim_time, state, control, flat, exit,
col_c) = simulate(initial_state, quadrotor, my_se3_controller, traj,
t_final, self.occ_map)
# TODO: Exit state check. Taking flat outputs
if True:
err = np.array(
flat['x'][-1].shape
) # TODO check if order is stored in the same order in both dictionary
err_cs = np.sum(np.absolute(err))
Rsx = flat['x'][-1]
Rsv = flat['x_dot'][-1]
# Rsx=state['x'][-1]
# Rsv=state['v'][-1]
Rsa = flat['x_ddot'][-1].reshape(729, 3)
Rsq = np.zeros((729, 4))
Rsw = np.zeros((729, 3))
# pdb.set_trace()
Rs = np.hstack((Rsx, Rsv, Rsa, Rsq, Rsw))
# pdb.set_trace()
else:
err_cs = np.inf
Rs = None
return_Rs = Rs
return_cc = return_cc + err_cs + col_c.reshape(729, 1)
return return_Rs, return_cc
def collision_check(self, x):
'''
Given a point in metric coordinates, identify if it is collision free
Input:
x = np array 3 X 1 of the (x,y,z) positions
Ouput:
Flag: True or False based on collision
'''
idx = self.occ_map.metric_to_index(x)
idx = idx[0]
try:
if self.occ_map.map[[0], idx[1], idx[2]]:
return True
else:
return False
except:
return True
def graph_search(world, resolution, margin, start, goal, astar):
"""
Parameters:
world, World object representing the environment obstacles
resolution, xyz resolution in meters for an occupancy map, shape=(3,)
margin, minimum allowed distance in meters from path to obstacles.
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
astar, if True use A*, else use Dijkstra
Output:
path, xyz position coordinates along the path in meters with
shape=(N,3). These are typically the centers of visited
voxels of an occupancy map. The first point must be the
start and the last point must be the goal. If no path
exists, return None.
"""
# While not required, we have provided an occupancy map you may use or modify.
occ_map = OccupancyMap(world, resolution, margin)
# Retrieve the index in the occupancy grid matrix corresponding to a position in space.
start_index = tuple(occ_map.metric_to_index(start))
goal_index = tuple(occ_map.metric_to_index(goal))
visited_nodes = []
cost2come = np.inf * np.ones_like(occ_map.map)
parent_nodes = 0 * np.empty_like(occ_map.map)
parent_nodes = parent_nodes.tolist()
if astar:
pHeap = [(0, 0, start_index)]
else:
pHeap = [(0, start_index)]
# List all neighbors (26-connectivity)
neighbors = [[-1, 0, 1], [0, 0, 1], [1, 0, 1], [-1, 0, 0], [1, 0, 0], [-1, 0, -1], [0, 0, -1], [1, 0, -1],
[-1, 1, 1], [0, 1, 1], [1, 1, 1], [-1, 1, 0], [1, 1, 0], [-1, 1, -1], [0, 1, -1], [1, 1, -1],
[-1, -1, 1], [0, -1, 1], [1, -1, 1], [-1, -1, 0], [1, -1, 0], [-1, -1, -1], [0, -1, -1],
[1, -1, -1],
[0, -1, 0], [0, 1, 0]]
while len(pHeap) > 0:
if astar:
f, cur_c2c, cur_index = heappop(pHeap)
else:
cur_c2c, cur_index = heappop(pHeap)
if goal_index in visited_nodes: # exit if goal is visited
break
if not occ_map.is_valid_index(cur_index): # skip node if outside map
continue
if cur_index in visited_nodes: # skip node if already visited
continue
if occ_map.is_occupied_index(cur_index): # skip node if there's an obstacle there
continue
for i in neighbors:
i = np.array(i)
neighbor_ind = cur_index + i
if not occ_map.is_valid_index(neighbor_ind): # skip neighbor if outside map
continue
if occ_map.is_occupied_index(neighbor_ind): # skip neighbor if occupied by obstacle
continue
if astar:
dist = np.linalg.norm(i)
c2c = cur_c2c + dist
h = np.linalg.norm(np.array((cur_index))-np.array((goal_index)))**1.2 # heuristic (L2 Norm)
f = c2c + h # f = g + h
else:
dist = np.linalg.norm(i)
c2c = cur_c2c + dist
if c2c < cost2come[(neighbor_ind)[0], (neighbor_ind)[1], (neighbor_ind)[2]]:
cost2come[(neighbor_ind)[0], (neighbor_ind)[1], (neighbor_ind)[2]] = c2c
if astar:
heappush(pHeap, [f, c2c, tuple(neighbor_ind)])
else:
heappush(pHeap, [c2c, tuple(neighbor_ind)])
parent_nodes[(neighbor_ind)[0]][(neighbor_ind)[1]][(neighbor_ind)[2]] = cur_index
visited_nodes.append(tuple(cur_index))
if parent_nodes[goal_index[0]][goal_index[1]][goal_index[2]] == 0:
return None
# build path from start to goal using parent_nodes
backTostart = np.array([])
path = np.array(goal)
counter = 0
while not np.array_equal(backTostart,np.array(start_index)):
if counter == 0:
backTostart = parent_nodes[goal_index[0]][goal_index[1]][goal_index[2]]
else:
backTostart = parent_nodes[backTostart[0]][backTostart[1]][backTostart[2]]
path = np.vstack((path,occ_map.index_to_metric_center(backTostart)))
counter += 1
path = np.vstack((path, start))
path = np.flip(path,axis=0)
return path
def graph_search(world, resolution, margin, start, goal, astar):
"""
Parameters:
world, World object representing the environment obstacles
resolution, xyz resolution in meters for an occupancy map, shape=(3,)
margin, minimum allowed distance in meters from path to obstacles.
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
astar, if True use A*, else use Dijkstra
Output:
path, xyz position coordinates along the path in meters with
shape=(N,3). These are typically the centers of visited
voxels of an occupancy map. The first point must be the
start and the last point must be the goal. If no path
exists, return None.
"""
# While not required, we have provided an occupancy map you may use or modify.
occ_map = OccupancyMap(world, resolution, margin)
# Retrieve the index in the occupancy grid matrix corresponding to a position in space.
start_index = tuple(occ_map.metric_to_index(start))
goal_index = tuple(occ_map.metric_to_index(goal))
print('Start:', start_index)
print('Goal:', goal_index)
Cost_dic = {}
Parent_dic = dict()
# start_time = time.time()
for i, _ in enumerate(occ_map.map[:]):
for j, _ in enumerate(occ_map.map[i][:]):
for k, _ in enumerate(occ_map.map[i][j][:]):
if occ_map.map[i][j][
k] == False: # Only consider points where there are no obstacles
parent = {
tuple(np.array([i, j, k])): tuple(np.array([0, 0, 0]))
}
Parent_dic.update(parent) # Dictionary of Parents
if tuple(np.array([i, j, k])) != start_index:
cost = {tuple(np.array([i, j, k])): np.inf}
else:
cost = {tuple(np.array([i, j, k])): 0}
Cost_dic.update(cost) # Dictionary of costs
heap = []
node = 0
heappush(heap, [Cost_dic[start_index], start_index])
min_cost = heappop(heap) #<---------------<<<<< Reuse this
# ---------- Done with intialization ------------------
goal_flag = 0
impossible = 0
flag = True
# while heap and min_cost[0] < np.inf:
if goal_index in Cost_dic.keys():
while goal_flag == 0:
# while heap:
U = min_cost[1]
neigh = get_neighbour(U)
neigh_idx = 0
no_neigbour = 0
g = [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0
] # <-------<< List to store cost of all neigbours
h = [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0
] # <-------<< Heuristics
f = [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0
] # <-------<< Varibale
while neigh and neigh_idx < len(neigh):
no_neigbour += 1
if neigh[neigh_idx] in Cost_dic.keys():
# A* in action
if astar == True: #<----------------------<< Checking for A* flag
if 0 <= neigh_idx < 18:
g[neigh_idx] = Cost_dic[U] + 1.414
h[neigh_idx] = np.linalg.norm(
np.asarray(goal_index) -
np.asarray(neigh[neigh_idx]))
f[neigh_idx] = g[neigh_idx] + h[neigh_idx]
elif neigh_idx in [19, 20, 22, 23]:
g[neigh_idx] = Cost_dic[U] + 1.414
h[neigh_idx] = np.linalg.norm(
np.asarray(goal_index) -
np.asarray(neigh[neigh_idx]))
f[neigh_idx] = g[neigh_idx] + h[neigh_idx]
else:
g[neigh_idx] = Cost_dic[U] + 1
h[neigh_idx] = np.linalg.norm(
np.asarray(goal_index) -
np.asarray(neigh[neigh_idx]))
f[neigh_idx] = g[neigh_idx] + h[neigh_idx]
if neigh[neigh_idx] == goal_index:
print('Reached Goal')
goal_flag = 1
if g[neigh_idx] < Cost_dic[neigh[neigh_idx]]:
Cost_dic[neigh[neigh_idx]] = g[neigh_idx]
heappush(heap, [f[neigh_idx], neigh[neigh_idx]])
Parent_dic[neigh[neigh_idx]] = U
# Dijkstra in action
else:
if 0 <= neigh_idx < 18:
g[neigh_idx] = Cost_dic[U] + 5
elif neigh_idx in [19, 20, 22, 23]:
g[neigh_idx] = Cost_dic[U] + 1.5
else:
g[neigh_idx] = Cost_dic[U] + 1
if neigh[neigh_idx] == goal_index:
print('Reached Goal')
goal_flag = 1
if g[neigh_idx] < Cost_dic[neigh[neigh_idx]]:
Cost_dic[neigh[neigh_idx]] = g[neigh_idx]
heappush(
heap,
[Cost_dic[neigh[neigh_idx]], neigh[neigh_idx]])
Parent_dic[neigh[neigh_idx]] = U
neigh_idx += 1 # <-------------<< Goto next Neighbour
if impossible != 1 and heap == []:
impossible = 1
flag = False
break
min_cost = heappop(heap) #<---------------<<<<< Reuse this
node += 1
# print('Cost to go: ', min_cost[0])
print('Nodes: ', node)
path = []
point = goal_index
while flag is True: # <-------------------<< Create the path list
mtpoint = occ_map.index_to_metric_center(Parent_dic[point])
# mtpoint = occ_map.index_to_metric_negative_corner(Parent_dic[point])
path.append(mtpoint)
point = Parent_dic[point]
if point == start_index:
flag = False
if impossible == 0:
path.append(start)
path.reverse()
path.append(goal)
# k = path.sort()
path = np.asarray(path)
length = float(
round(np.sum(np.linalg.norm(np.diff(path, axis=0), axis=1)),
3))
print('Path Length: ', length)
elif impossible == 1:
path = None
sparse_path = get_path(path, goal, length)
# sparse_path = path
return sparse_path
else:
goal_flag == 1
path = []
path = [start_index]
path = np.asarray(path)
print('Goal Not Reachable')
return None
def graph_search(world, resolution, margin, start, goal, astar):
"""
Parameters:
world, World object representing the environment obstacles
resolution, xyz resolution in meters for an occupancy map, shape=(3,)
margin, minimum allowed distance in meters from path to obstacles.
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
astar, if True use A*, else use Dijkstra
Output:
path, xyz position coordinates along the path in meters with
shape=(N,3). These are typically the centers of visited
voxels of an occupancy map. The first point must be the
start and the last point must be the goal. If no path
exists, return None.
"""
# While not required, we have provided an occupancy map you may use or modify.
occ_map = OccupancyMap(world, resolution, margin)
# Retrieve the index in the occupancy grid matrix corresponding to a position in space.
start_index = tuple(occ_map.metric_to_index(start))
goal_index = tuple(occ_map.metric_to_index(goal))
path = [goal]
i, j, k = occ_map.map.shape
g = np.full((i, j, k), np.inf)
p = np.zeros((i, j, k, 3))
G = []
co = 10
if astar:
h1 = max(
abs(start_index[0] - goal_index[0]) * co,
abs(start_index[1] - goal_index[1]) * co,
abs(start_index[2] - goal_index[2]) * co)
# # euclidean
# h1 = math.sqrt(
# (abs(start_index[0] - goal_index[0]) * co) ** 2 + (abs(start_index[1] - goal_index[1]) * co) ** 2 + \
# (abs(start_index[2] - goal_index[2]) * co) ** 2)
else:
h1 = 0
g[start_index] = h1
count3 = 0
count4 = 0
heappush(G, (g[start_index], start_index))
heappush(G, (g[goal_index], goal_index))
state = np.zeros((i, j, k)) # 0 is not open, 1 is open, 2 is close
state[start_index] = 1 # add start_index to open
print("enter loop")
while np.min(g) < np.inf and not state[
goal_index] == 2: # while there is node not open and the goal is not closed
u = heappop(G)[1]
state[u] = 2
count3 = count3 + 1
for i in range(u[0] - 1, u[0] + 2):
for j in range(u[1] - 1, u[1] + 2):
for k in range(u[2] - 1, u[2] + 2):
v = (i, j, k)
# determine the distance between voxel
if occ_map.is_valid_index(v):
if not occ_map.is_occupied_index(v):
if not state[v] == 2: # if neighbot is not closed
# calculate maximum distance
count4 = count4 + 1
if astar:
h = max(
abs(goal_index[0] - v[0]) * co,
abs(goal_index[1] - v[1]) * co,
abs(goal_index[2] - v[2]) * co)
# euclidean
# h = math.sqrt(
# (abs(goal_index[0] - v[0]) * co) ** 2 + (abs(goal_index[1] - v[1]) * co) ** 2 \
# + (abs(goal_index[2] - v[2]) * co) ** 2)
else:
h = 0
if abs(v[0] - u[0]) + abs(v[1] - u[1]) + abs(
v[2] - u[2]) == 1:
c = 10
elif abs(v[0] - u[0]) + abs(v[1] - u[1]) + abs(
v[2] - u[2]) == 2:
c = 14
else:
c = 17
d = g[u] + c + h
# print("in loop")
if state[
v] == 1: # this neighbor is already opened
if g[v] > d:
G.remove((g[v], v))
g[v] = d
p[v] = u
heappush(G, (g[v], v))
elif state[
v] == 0: # this neighbor haven't been touched
g[v] = d
p[v] = u
heappush(G, (g[v], v))
state[v] = 1
# print("count1")
# print(count3)
# print("count2")
# print(count4)
# print("out loop")
temp = state[goal_index]
f = p[goal_index]
if (p[goal_index] == (0, 0, 0)).all(): # goal not found
path = None
else:
pointer = goal_index
while pointer != start_index:
par = p[pointer]
path.append(occ_map.index_to_metric_center(par))
ind1 = int(par[0])
ind2 = int(par[1])
ind3 = int(par[2])
pointer = (ind1, ind2, ind3)
path.append(start)
path.reverse()
# print(path)
path = np.asarray(path)
return 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.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