saberbud/PathPlanning
1
0
Fork 0
mirror of https://github.com/zhm-real/PathPlanning.git synced 2026-08-29 08:34:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

reformat

Browse Source
This commit is contained in:
zhm-real
2020-08-05 12:53:19 -07:00
parent b3f190ec36
commit 8e232410ed
31 changed files with 296 additions and 219 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Sampling_based_Planning/rrt_2D/dynamic_rrt.py
+1 -1
View File
@@ -93,7 +93,7 @@ class DynamicRrt:
print("Please choose right area!")
else:
x, y = int(x), int(y)
print("Add circle obstacle at: x =", x, ",", "y =", y)
print("Add circle obstacle at: s =", x, ",", "y =", y)
self.obs_add = [x, y, 2]
self.obs_circle.append([x, y, 2])
self.utils.update_obs(self.obs_circle, self.obs_boundary, self.obs_rectangle)
Sampling_based_Planning/rrt_2D/extended_rrt.py
+1 -1
View File
@@ -79,7 +79,7 @@ class ExtendedRrt:
print("Please choose right area!")
else:
x, y = int(x), int(y)
print("Add circle obstacle at: x =", x, ",", "y =", y)
print("Add circle obstacle at: s =", x, ",", "y =", y)
self.obs_circle.append([x, y, 2])
self.utils.update_obs(self.obs_circle, self.obs_boundary, self.obs_rectangle)
path, waypoint = self.replanning()
Sampling_based_Planning/rrt_2D/queue.py
+1 -1
View File
@@ -53,7 +53,7 @@ class QueuePrior:
return len(self.queue) == 0
def put(self, item, priority):
heapq.heappush(self.queue, (priority, item)) # reorder x using priority
heapq.heappush(self.queue, (priority, item)) # reorder s using priority
def get(self):
return heapq.heappop(self.queue)[1] # pop out the smallest item
Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py
+1 -1
View File
@@ -239,7 +239,7 @@ class dynamic_rrt_3D:
dx, dy, dz = xmax - xmin, ymax - ymin, zmax - zmin
ax.get_proj = make_get_proj(ax, 1 * dx, 1 * dy, 2 * dy)
make_transparent(ax)
# plt.xlabel('x')
# plt.xlabel('s')
# plt.ylabel('y')
ax.set_axis_off()
plt.pause(0.0001)
Sampling_based_Planning/rrt_3D/env3D.py
+4 -4
View File
@@ -8,7 +8,7 @@ import numpy as np
# from utils3D import OBB2AABB
def R_matrix(z_angle,y_angle,x_angle):
# x angle: row; y angle: pitch; z angle: yaw
# s angle: row; y angle: pitch; z angle: yaw
# generate rotation matrix in SO3
# RzRyRx = R, ZYX intrinsic rotation
# also (r1,r2,r3) in R3*3 in {W} frame
@@ -107,7 +107,7 @@ class env():
def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], theta = [0,0,0], block_to_move = 0, obb_to_move = 0, mode = 'uniform'):
# t is time , v is velocity in R3, a is acceleration in R3, s is increment ini time,
# R is an orthorgonal transform in R3*3, is the rotation matrix
# (x',t') = (x + tv, t) is uniform transformation
# (s',t') = (s + tv, t) is uniform transformation
if mode == 'uniform':
ori = np.array(self.blocks[block_to_move])
self.blocks[block_to_move] = \
@@ -129,7 +129,7 @@ class env():
# np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \
# ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution])
return a,ori
# (x',t') = (x + a, t + s) is a translation
# (s',t') = (s + a, t + s) is a translation
if mode == 'translation':
ori = np.array(self.blocks[block_to_move])
self.blocks[block_to_move] = \
@@ -152,7 +152,7 @@ class env():
np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \
ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution])
# return a,ori
# (x',t') = (Rx, t)
# (s',t') = (Rx, t)
if mode == 'rotation': # this makes an OBB rotate
ori = [self.OBB[obb_to_move]]
self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0],y_angle=theta[1],x_angle=theta[2])
Sampling_based_Planning/rrt_3D/plot_util3D.py
+1 -1
View File
@@ -128,7 +128,7 @@ def visualization(initparams):
dx, dy, dz = xmax - xmin, ymax - ymin, zmax - zmin
ax.get_proj = make_get_proj(ax, 1 * dx, 1 * dy, 2 * dy)
make_transparent(ax)
#plt.xlabel('x')
#plt.xlabel('s')
#plt.ylabel('y')
ax.set_axis_off()
plt.pause(0.0001)
Sampling_based_Planning/rrt_3D/rrt3D.py
+1 -1
View File
@@ -36,7 +36,7 @@ class rrt():
self.fig = plt.figure(figsize=(10, 8))
def wireup(self, x, y):
# self.E.add_edge([x, y]) # add edge
# self.E.add_edge([s, y]) # add edge
self.Parent[x] = y
def run(self):
Sampling_based_Planning/rrt_3D/rrtstar3D.py
+1 -1
View File
@@ -38,7 +38,7 @@ class rrtstar():
self.V.append(self.x0)
self.ind = 0
def wireup(self,x,y):
# self.E.add_edge([x,y]) # add edge
# self.E.add_edge([s,y]) # add edge
self.Parent[x] = y
def removewire(self,xnear):
Sampling_based_Planning/rrt_3D/utils3D.py
+8 -8
View File
@@ -121,7 +121,7 @@ def lineAABB(p0, p1, dist, aabb):
if abs(T[0]) > (aabb.E[0] + hl * abs(I[0])): return False
if abs(T[1]) > (aabb.E[1] + hl * abs(I[1])): return False
if abs(T[2]) > (aabb.E[2] + hl * abs(I[2])): return False
# I.cross(x axis) ?
# I.cross(s axis) ?
r = aabb.E[1] * abs(I[2]) + aabb.E[2] * abs(I[1])
if abs(T[1] * I[2] - T[2] * I[1]) > r: return False
# I.cross(y axis) ?
@@ -176,7 +176,7 @@ def nearest(initparams, x, isset=False):
return tuple(initparams.V[np.argmin(dists)])
def near(initparams, x):
# x = np.array(x)
# s = np.array(s)
V = np.array(initparams.V)
if initparams.i == 0:
return [initparams.V[0]]
@@ -192,15 +192,15 @@ def near(initparams, x):
return np.array(nearpoints)
def steer(initparams, x, y, DIST=False):
# steer from x to y
# steer from s to y
if np.equal(x, y).all():
return x, 0.0
dist, step = getDist(y, x), initparams.stepsize
step = min(dist, step)
increment = ((y[0] - x[0]) / dist * step, (y[1] - x[1]) / dist * step, (y[2] - x[2]) / dist * step)
xnew = (x[0] + increment[0], x[1] + increment[1], x[2] + increment[2])
# direc = (y - x) / np.linalg.norm(y - x)
# xnew = x + initparams.stepsize * direc
# direc = (y - s) / np.linalg.norm(y - s)
# xnew = s + initparams.stepsize * direc
if DIST:
return xnew, dist
return xnew, dist
@@ -274,7 +274,7 @@ def tree_add_edge(node_in_tree, x):
return node_to_add
def tree_bfs(head, x):
# searches x in order of bfs
# searches s in order of bfs
node = head
Q = []
Q.append(node)
@@ -286,7 +286,7 @@ def tree_bfs(head, x):
Q.append(child_node)
def tree_nearest(head, x):
# find the node nearest to x
# find the node nearest to s
D = np.inf
min_node = None
@@ -304,7 +304,7 @@ def tree_nearest(head, x):
return min_node
def tree_steer(initparams, node, x):
# steer from node to x
# steer from node to s
dist, step = getDist(node.pos, x), initparams.stepsize
increment = ((node.pos[0] - x[0]) / dist * step, (node.pos[1] - x[1]) / dist * step, (node.pos[2] - x[2]) / dist * step)
xnew = (x[0] + increment[0], x[1] + increment[1], x[2] + increment[2])