From 95efabd0171d77f08568a608a371fc994c05766a Mon Sep 17 00:00:00 2001 From: 391311qy <391311qy@gmail.com> Date: Thu, 25 Jun 2020 01:53:19 -0700 Subject: [PATCH] changed ball radius --- Sampling-based Planning/rrt_3D/plot_util3D.py | 8 +-- Sampling-based Planning/rrt_3D/rrtstar3D.py | 62 +++++++++++-------- Sampling-based Planning/rrt_3D/utils3D.py | 36 ++++++----- 3 files changed, 61 insertions(+), 45 deletions(-) diff --git a/Sampling-based Planning/rrt_3D/plot_util3D.py b/Sampling-based Planning/rrt_3D/plot_util3D.py index 6425bd5..15381c0 100644 --- a/Sampling-based Planning/rrt_3D/plot_util3D.py +++ b/Sampling-based Planning/rrt_3D/plot_util3D.py @@ -66,7 +66,7 @@ def visualization(initparams): xs = i[0][0], i[1][0] ys = i[0][1], i[1][1] zs = i[0][2], i[1][2] - line = plt3d.art3d.Line3D(xs, ys, zs) + line = plt3d.art3d.Line3D(xs, ys, zs, alpha=0.25) ax.add_line(line) if Path != []: @@ -79,7 +79,7 @@ def visualization(initparams): ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k') ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k') - ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g') + ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g',) xmin, xmax = initparams.env.boundary[0], initparams.env.boundary[3] ymin, ymax = initparams.env.boundary[1], initparams.env.boundary[4] @@ -89,7 +89,7 @@ def visualization(initparams): ax.set_ylim3d(ymin, ymax) ax.set_zlim3d(zmin, zmax) ax.get_proj = make_get_proj(ax,1*dx, 1*dy, 2*dy) - ax.dist = 5 + #ax.dist = 5 plt.xlabel('x') plt.ylabel('y') if not Path != []: @@ -135,7 +135,7 @@ def make_get_proj(self, rx, ry, rz): self.eye = E self.vvec = R - E - self.vvec = self.vvec / proj3d.mod(self.vvec) + self.vvec = self.vvec / np.linalg.norm(self.vvec) if abs(relev) > np.pi/2: # upside down diff --git a/Sampling-based Planning/rrt_3D/rrtstar3D.py b/Sampling-based Planning/rrt_3D/rrtstar3D.py index bcb2ead..b7b93b9 100644 --- a/Sampling-based Planning/rrt_3D/rrtstar3D.py +++ b/Sampling-based Planning/rrt_3D/rrtstar3D.py @@ -12,41 +12,54 @@ import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/") from rrt_3D.env3D import env -from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset +from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset, hash3D, dehash class rrtstar(): def __init__(self): self.env = env() - self.Parent = defaultdict(lambda: defaultdict(dict)) + self.Parent = {} self.E = edgeset() self.V = [] self.i = 0 - self.maxiter = 10000 + self.maxiter = 4000 # at least 4000 in this env self.stepsize = 0.5 + self.gamma = 500 + self.eta = 1.1*self.stepsize self.Path = [] + self.done = False - def wireup(self, x, y): - self.E.add_edge([x, y]) # add edge - self.Parent[str(x[0])][str(x[1])][str(x[2])] = y + def wireup(self,x,y): + self.E.add_edge([x,y]) # add edge + self.Parent[hash3D(x)] = y - def removewire(self, xnear): - xparent = self.Parent[str(xnear[0])][str(xnear[1])][str(xnear[2])] - a = [xnear, xparent] - self.E.remove_edge(a) # remove and replace old the connection + def removewire(self,xnear): + xparent = self.Parent[hash3D(xnear)] + a = [xnear,xparent] + self.E.remove_edge(a) # remove and replace old the connection + #self.Parent.pop(hash3D(xnear), None) + + def reached(self): + self.done = True + xn = near(self,self.env.goal) + c = [cost(self,x) for x in xn] + xncmin = xn[np.argmin(c)] + self.wireup(self.env.goal,xncmin) + self.V.append(self.env.goal) + self.Path,self.D = path(self) def run(self): self.V.append(self.env.start) ind = 0 xnew = self.env.start - while ind < self.maxiter and getDist(xnew, self.env.goal) > 1: - # while ind < self.maxiter: - xrand = sampleFree(self) - xnearest = nearest(self, xrand) - xnew = steer(self, xnearest, xrand) - if not isCollide(self, xnearest, xnew): - Xnear = near(self, xnew) - self.V.append(xnew) # add point + print('start rrt*... ') + while ind < self.maxiter: + xrand = sampleFree(self) + xnearest = nearest(self,xrand) + xnew = steer(self,xnearest,xrand) + if not isCollide(self,xnearest,xnew): + Xnear = near(self,xnew) + self.V.append(xnew) # add point # visualization(self) # minimal path and minimal cost xmin, cmin = xnearest, cost(self, xnearest) + getDist(xnearest, xnew) @@ -70,16 +83,15 @@ class rrtstar(): self.wireup(xnear, xnew) self.i += 1 ind += 1 - # when the goal is reached - if getDist(xnew, self.env.goal) <= 1: - self.wireup(self.env.goal, xnew) - self.Path, self.D = path(self) + # max sample reached + self.reached() + print('time used = ' + str(time.time()-starttime)) + print('Total distance = '+str(self.D)) visualization(self) - print('Total distance = ' + str(self.D)) - + if __name__ == '__main__': p = rrtstar() starttime = time.time() p.run() - print('time used = ' + str(time.time() - starttime)) + diff --git a/Sampling-based Planning/rrt_3D/utils3D.py b/Sampling-based Planning/rrt_3D/utils3D.py index 56e5121..ca0b313 100644 --- a/Sampling-based Planning/rrt_3D/utils3D.py +++ b/Sampling-based Planning/rrt_3D/utils3D.py @@ -40,9 +40,10 @@ def sampleFree(initparams): if isinside(initparams, x): return sampleFree(initparams) else: - if i < 0.05: - return initparams.env.goal - else: return np.array(x) + #if i < 0.05: + # return initparams.env.goal+0.01 + #else: return np.array(x) + return np.array(x) def isinside(initparams, x): @@ -88,8 +89,11 @@ def steer(initparams, x, y): def near(initparams, x): - # TODO: r = min{gamma*log(card(V)/card(V)1/d),eta} - r = initparams.stepsize + cardV = len(initparams.V) + eta = initparams.eta + gamma = initparams.gamma + r = min(gamma*(np.log(cardV)/cardV),eta) + if initparams.done: r = 1 V = np.array(initparams.V) if initparams.i == 0: return initparams.V[0] @@ -103,31 +107,31 @@ def cost(initparams, x): '''here use the additive recursive cost function''' if all(x == initparams.env.start): return 0 - xparent = initparams.Parent[str(x[0])][str(x[1])][str(x[2])] + xparent = initparams.Parent[hash3D(x)] return cost(initparams, xparent) + getDist(x, xparent) def path(initparams, Path=[], dist=0): x = initparams.env.goal while not all(x == initparams.env.start): - x2 = initparams.Parent[str(x[0])][str(x[1])][str(x[2])] + x2 = initparams.Parent[hash3D(x)] Path.append(np.array([x, x2])) dist += getDist(x, x2) x = x2 return Path, dist +def hash3D(x): + return str(x[0])+' '+str(x[1])+' '+str(x[2]) + +def dehash(x): + return np.array([float(i) for i in x.split(' ')]) + class edgeset(object): def __init__(self): self.E = {} - def hash(self,x): - return str(x[0])+' '+str(x[1])+' '+str(x[2]) - - def dehash(self,x): - return np.array([float(i) for i in x.split(' ')]) - def add_edge(self,edge): - x, y = self.hash(edge[0]),self.hash(edge[1]) + x, y = hash3D(edge[0]),hash3D(edge[1]) if x in self.E: self.E[x].append(y) else: @@ -135,12 +139,12 @@ class edgeset(object): def remove_edge(self,edge): x, y = edge[0],edge[1] - self.E[self.hash(x)].remove(self.hash(y)) + self.E[hash3D(x)].remove(hash3D(y)) def get_edge(self): edges = [] for v in self.E: for n in self.E[v]: #if (n,v) not in edges: - edges.append((self.dehash(v),self.dehash(n))) + edges.append((dehash(v),dehash(n))) return edges \ No newline at end of file