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

Merge branch 'master' of https://github.com/zhm-real/path-planning-algorithms

Browse Source
This commit is contained in:
yue qi
2020-08-06 00:40:37 -06:00
parent 84a030d4b3 9c1f680dbf
commit c251a2f0c2
31 changed files with 296 additions and 222 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Search_based_Planning/Search_3D/Dstar3D.py
+2 -2
View File
@@ -64,7 +64,7 @@ class D_star(object):
return None, -1
def insert(self, x, h_new):
# inserting a key and value into OPEN list (x, kx)
# inserting a key and value into OPEN list (s, kx)
# depending on following situations
if self.tag[x] == 'New':
kx = h_new
@@ -83,7 +83,7 @@ class D_star(object):
self.V.add(x)
if x is None:
return -1
# check if 1st timer x
# check if 1st timer s
self.checkState(x)
if kold < self.h[x]: # raised states
for y in children(self, x):
Search_based_Planning/Search_3D/env3D.py
+4 -4
View File
@@ -10,7 +10,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
@@ -120,7 +120,7 @@ class env():
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] = \
@@ -142,7 +142,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] = \
@@ -165,7 +165,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])
Search_based_Planning/Search_3D/plot_util3D.py
+1 -1
View File
@@ -112,7 +112,7 @@ def visualization(initparams):
zmin, zmax = initparams.env.boundary[2], initparams.env.boundary[5]
dx, dy, dz = xmax-xmin, ymax-ymin, zmax-zmin
ax.get_proj = make_get_proj(ax,1*dx, 1*dy, 2*dy)
plt.xlabel('x')
plt.xlabel('s')
plt.ylabel('y')
plt.pause(0.0001)
Search_based_Planning/Search_3D/queue.py
+7 -7
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
@@ -131,13 +131,13 @@ class MinheapPQ:
# def put(self, item, priority):
# count = 0
# for (p, x) in self.queue:
# if x == item:
# for (p, s) in self.queue:
# if s == item:
# self.queue[count] = (priority, item)
# break
# count += 1
# if count == len(self.queue):
# 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
@@ -146,9 +146,9 @@ class MinheapPQ:
# return self.queue
# def check_remove(self, item):
# for (p, x) in self.queue:
# if item == x:
# self.queue.remove((p, x))
# for (p, s) in self.queue:
# if item == s:
# self.queue.remove((p, s))
# def top_key(self):
# return self.queue[0][0]
Search_based_Planning/Search_3D/utils3D.py
+12 -12
View File
@@ -72,7 +72,7 @@ def OBB2AABB(obb):
P = obb.P
a = obb.E
A = obb.O
# a1(A1 dot x) + a2(A2 dot x) + a3(A3 dot x)
# a1(A1 dot s) + a2(A2 dot s) + a3(A3 dot s)
Ex = a[0]*abs(A[0][0]) + a[1]*abs(A[1][0]) + a[2]*abs(A[2][0])
Ey = a[0]*abs(A[0][1]) + a[1]*abs(A[1][1]) + a[2]*abs(A[2][1])
Ez = a[0]*abs(A[0][2]) + a[1]*abs(A[1][2]) + a[2]*abs(A[2][2])
@@ -111,7 +111,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) ?
@@ -170,63 +170,63 @@ def OBBOBB(obb1, obb2):
return False
#9 cross products
#L = A0 x B0
#L = A0 s B0
ra = a[1]*abs(R[2][0]) + a[2]*abs(R[1][0])
rb = b[1]*abs(R[0][2]) + b[2]*abs(R[0][1])
t = abs(T[2]*R[1][0] - T[1]*R[2][0])
if t > ra + rb:
return False
#L = A0 x B1
#L = A0 s B1
ra = a[1]*abs(R[2][1]) + a[2]*abs(R[1][1])
rb = b[0]*abs(R[0][2]) + b[2]*abs(R[0][0])
t = abs(T[2]*R[1][1] - T[1]*R[2][1])
if t > ra + rb:
return False
#L = A0 x B2
#L = A0 s B2
ra = a[1]*abs(R[2][2]) + a[2]*abs(R[1][2])
rb = b[0]*abs(R[0][1]) + b[1]*abs(R[0][0])
t = abs(T[2]*R[1][2] - T[1]*R[2][2])
if t > ra + rb:
return False
#L = A1 x B0
#L = A1 s B0
ra = a[0]*abs(R[2][0]) + a[2]*abs(R[0][0])
rb = b[1]*abs(R[1][2]) + b[2]*abs(R[1][1])
t = abs( T[0]*R[2][0] - T[2]*R[0][0] )
if t > ra + rb:
return False
# L = A1 x B1
# L = A1 s B1
ra = a[0]*abs(R[2][1]) + a[2]*abs(R[0][1])
rb = b[0]*abs(R[1][2]) + b[2]*abs(R[1][0])
t = abs( T[0]*R[2][1] - T[2]*R[0][1] )
if t > ra + rb:
return False
#L = A1 x B2
#L = A1 s B2
ra = a[0]*abs(R[2][2]) + a[2]*abs(R[0][2])
rb = b[0]*abs(R[1][1]) + b[1]*abs(R[1][0])
t = abs( T[0]*R[2][2] - T[2]*R[0][2] )
if t > ra + rb:
return False
#L = A2 x B0
#L = A2 s B0
ra = a[0]*abs(R[1][0]) + a[1]*abs(R[0][0])
rb = b[1]*abs(R[2][2]) + b[2]*abs(R[2][1])
t = abs( T[1]*R[0][0] - T[0]*R[1][0] )
if t > ra + rb:
return False
# L = A2 x B1
# L = A2 s B1
ra = a[0]*abs(R[1][1]) + a[1]*abs(R[0][1])
rb = b[0] *abs(R[2][2]) + b[2]*abs(R[2][0])
t = abs( T[1]*R[0][1] - T[0]*R[1][1] )
if t > ra + rb:
return False
#L = A2 x B2
#L = A2 s B2
ra = a[0]*abs(R[1][2]) + a[1]*abs(R[0][2])
rb = b[0]*abs(R[2][1]) + b[1]*abs(R[2][0])
t = abs( T[1]*R[0][2] - T[0]*R[1][2] )
@@ -255,7 +255,7 @@ def StateSpace(env, factor=0):
def g_Space(initparams):
'''This function is used to get nodes and discretize the space.
State space is by x*y*z,3 where each 3 is a point in 3D.'''
State space is by s*y*z,3 where each 3 is a point in 3D.'''
g = {}
Space = StateSpace(initparams.env)
for v in Space: