From 8689d03e6cc7add707489b3303d5d9213c3d653e Mon Sep 17 00:00:00 2001 From: 391311qy <391311qy@gmail.com> Date: Wed, 24 Jun 2020 22:01:12 -0700 Subject: [PATCH] yq change env3d and plotting --- Sampling-based Planning/rrt_3D/env3D.py | 93 ++--- Sampling-based Planning/rrt_3D/plot_util3D.py | 152 ++++++++ Sampling-based Planning/rrt_3D/utils3D.py | 366 +++++++----------- 3 files changed, 347 insertions(+), 264 deletions(-) create mode 100644 Sampling-based Planning/rrt_3D/plot_util3D.py diff --git a/Sampling-based Planning/rrt_3D/env3D.py b/Sampling-based Planning/rrt_3D/env3D.py index 118452b..977aa8d 100644 --- a/Sampling-based Planning/rrt_3D/env3D.py +++ b/Sampling-based Planning/rrt_3D/env3D.py @@ -1,44 +1,49 @@ -# this is the three dimensional configuration space for rrt -# !/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -@author: yue qi -""" -import numpy as np - - -def getblocks(resolution): - # AABBs - block = [[3.10e+00, 0.00e+00, 2.10e+00, 3.90e+00, 5.00e+00, 6.00e+00], - [9.10e+00, 0.00e+00, 2.10e+00, 9.90e+00, 5.00e+00, 6.00e+00], - [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00], - [1.00e-01, 0.00e+00, 0.00e+00, 9.00e-01, 5.00e+00, 3.90e+00], - [6.10e+00, 0.00e+00, 0.00e+00, 6.90e+00, 5.00e+00, 3.90e+00], - [1.21e+01, 0.00e+00, 0.00e+00, 1.29e+01, 5.00e+00, 3.90e+00], - [1.81e+01, 0.00e+00, 0.00e+00, 1.89e+01, 5.00e+00, 3.90e+00]] - Obstacles = [] - for i in block: - i = np.array(i) - Obstacles.append((i[0] / resolution, i[1] / resolution, i[2] / resolution, i[3] / resolution, i[4] / resolution, - i[5] / resolution)) - return np.array(Obstacles) - - -class env(): - def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=5, zmax=6, resolution=1): - self.resolution = resolution - self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) / resolution - self.blocks = getblocks(resolution) - self.start = np.array([0.5, 2.5, 5.5]) - self.goal = np.array([19.0, 2.5, 5.5]) - - def visualize(self): - # fig = plt.figure() - # TODO: do visualizations - return - - -if __name__ == '__main__': - newenv = env() - X = StateSpace(newenv.boundary, newenv.resolution) - print(X) +# this is the three dimensional configuration space for rrt +# !/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +@author: yue qi +""" +import numpy as np + + +def getblocks(resolution): + # AABBs + block = [[3.10e+00, 0.00e+00, 2.10e+00, 3.90e+00, 5.00e+00, 6.00e+00], + [9.10e+00, 0.00e+00, 2.10e+00, 9.90e+00, 5.00e+00, 6.00e+00], + #[1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00], + #[1.00e-01, 0.00e+00, 0.00e+00, 9.00e-01, 5.00e+00, 3.90e+00], + #[6.10e+00, 0.00e+00, 0.00e+00, 6.90e+00, 5.00e+00, 3.90e+00], + [1.21e+01, 0.00e+00, 0.00e+00, 1.29e+01, 5.00e+00, 3.90e+00], + [1.81e+01, 0.00e+00, 0.00e+00, 1.89e+01, 5.00e+00, 3.90e+00]] + Obstacles = [] + for i in block: + i = np.array(i) + Obstacles.append([j/resolution for j in i]) + return np.array(Obstacles) + +def getballs(resolution): + spheres = [[16,2.5,3,2],[10,2.5,1,1]] + Obstacles = [] + for i in spheres: + Obstacles.append([j/resolution for j in i]) + return np.array(Obstacles) + +class env(): + def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=5, zmax=6, resolution=1): + self.resolution = resolution + self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) / resolution + self.blocks = getblocks(resolution) + self.balls = getballs(resolution) + self.start = np.array([0.5, 2.5, 5.5]) + self.goal = np.array([19.0, 2.5, 5.5]) + + def visualize(self): + # fig = plt.figure() + # TODO: do visualizations + return + + +if __name__ == '__main__': + newenv = env() + print(newenv.balls) diff --git a/Sampling-based Planning/rrt_3D/plot_util3D.py b/Sampling-based Planning/rrt_3D/plot_util3D.py new file mode 100644 index 0000000..6425bd5 --- /dev/null +++ b/Sampling-based Planning/rrt_3D/plot_util3D.py @@ -0,0 +1,152 @@ +# plotting +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +from mpl_toolkits.mplot3d.art3d import Poly3DCollection +import mpl_toolkits.mplot3d as plt3d +from mpl_toolkits.mplot3d import proj3d +import numpy as np + +def CreateSphere(center,r): + u = np.linspace(0,2* np.pi,30) + v = np.linspace(0,np.pi,30) + x=np.outer(np.cos(u),np.sin(v)) + y=np.outer(np.sin(u),np.sin(v)) + z=np.outer(np.ones(np.size(u)),np.cos(v)) + # shift and scale sphere + x = r*x + center[0] + y = r*y + center[1] + z = r*z + center[2] + return (x,y,z) + +def draw_Spheres(ax,balls): + for i in balls: + (xs,ys,zs) = CreateSphere(i[0:3],i[-1]) + ax.plot_wireframe(xs, ys, zs, alpha=0.15,color="b") + +def draw_block_list(ax, blocks): + ''' + Subroutine used by draw_map() to display the environment blocks + ''' + v = np.array([[0, 0, 0], [1, 0, 0], [1, 1, 0], [0, 1, 0], [0, 0, 1], [1, 0, 1], [1, 1, 1], [0, 1, 1]], + dtype='float') + f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]]) + # clr = blocks[:,6:]/255 + n = blocks.shape[0] + d = blocks[:, 3:6] - blocks[:, :3] + vl = np.zeros((8 * n, 3)) + fl = np.zeros((6 * n, 4), dtype='int64') + # fcl = np.zeros((6*n,3)) + for k in range(n): + vl[k * 8:(k + 1) * 8, :] = v * d[k] + blocks[k, :3] + fl[k * 6:(k + 1) * 6, :] = f + k * 8 + # fcl[k*6:(k+1)*6,:] = clr[k,:] + + if type(ax) is Poly3DCollection: + ax.set_verts(vl[fl]) + else: + pc = Poly3DCollection(vl[fl], alpha=0.15, linewidths=1, edgecolors='k') + # pc.set_facecolor(fcl) + h = ax.add_collection3d(pc) + return h + +def visualization(initparams): + V = np.array(initparams.V) + E = initparams.E + Path = np.array(initparams.Path) + start = initparams.env.start + goal = initparams.env.goal + ax = plt.subplot(111, projection='3d',adjustable='box') + ax.view_init(elev=0., azim=90) + ax.clear() + draw_Spheres(ax, initparams.env.balls) + draw_block_list(ax, initparams.env.blocks) + edges = E.get_edge() + if edges != []: + for i in edges: + 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) + ax.add_line(line) + + if Path != []: + for i in Path: + 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, color='r') + ax.add_line(line) + + 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') + + xmin, xmax = initparams.env.boundary[0], initparams.env.boundary[3] + ymin, ymax = initparams.env.boundary[1], initparams.env.boundary[4] + zmin, zmax = initparams.env.boundary[2], initparams.env.boundary[5] + dx, dy, dz = xmax-xmin, ymax-ymin, zmax-zmin + ax.set_xlim3d(xmin, xmax) + 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 + plt.xlabel('x') + plt.ylabel('y') + if not Path != []: + plt.pause(0.001) + else: + plt.show() + +def make_get_proj(self, rx, ry, rz): + ''' + Return a variation on :func:`~mpl_toolkit.mplot2d.axes3d.Axes3D.getproj` that + makes the box aspect ratio equal to *rx:ry:rz*, using an axes object *self*. + ''' + + rm = max(rx, ry, rz) + kx = rm / rx; ky = rm / ry; kz = rm / rz; + + # Copied directly from mpl_toolkit/mplot3d/axes3d.py. New or modified lines are + # marked by ## + def get_proj(): + relev, razim = np.pi * self.elev/180, np.pi * self.azim/180 + + xmin, xmax = self.get_xlim3d() + ymin, ymax = self.get_ylim3d() + zmin, zmax = self.get_zlim3d() + + # transform to uniform world coordinates 0-1.0,0-1.0,0-1.0 + worldM = proj3d.world_transformation(xmin, xmax, + ymin, ymax, + zmin, zmax) + + # adjust the aspect ratio ## + aspectM = proj3d.world_transformation(-kx + 1, kx, ## + -ky + 1, ky, ## + -kz + 1, kz) ## + + # look into the middle of the new coordinates + R = np.array([0.5, 0.5, 0.5]) + + xp = R[0] + np.cos(razim) * np.cos(relev) * self.dist + yp = R[1] + np.sin(razim) * np.cos(relev) * self.dist + zp = R[2] + np.sin(relev) * self.dist + E = np.array((xp, yp, zp)) + + self.eye = E + self.vvec = R - E + self.vvec = self.vvec / proj3d.mod(self.vvec) + + if abs(relev) > np.pi/2: + # upside down + V = np.array((0, 0, -1)) + else: + V = np.array((0, 0, 1)) + zfront, zback = -self.dist, self.dist + + viewM = proj3d.view_transformation(E, R, V) + perspM = proj3d.persp_transformation(zfront, zback) + M0 = np.dot(viewM, np.dot(aspectM, worldM)) ## + M = np.dot(perspM, M0) + return M + return get_proj \ No newline at end of file diff --git a/Sampling-based Planning/rrt_3D/utils3D.py b/Sampling-based Planning/rrt_3D/utils3D.py index 47ec705..56e5121 100644 --- a/Sampling-based Planning/rrt_3D/utils3D.py +++ b/Sampling-based Planning/rrt_3D/utils3D.py @@ -1,220 +1,146 @@ -import numpy as np -from numpy.matlib import repmat -import pyrr as pyrr -# plotting -import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D -from mpl_toolkits.mplot3d.art3d import Poly3DCollection -import mpl_toolkits.mplot3d as plt3d - - -def getRay(x, y): - direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]] - return np.array([x, direc]) - - -def getAABB(blocks): - AABB = [] - for i in blocks: - AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger - return AABB - - -def getDist(pos1, pos2): - return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2])) - - -def draw_block_list(ax, blocks): - ''' - Subroutine used by draw_map() to display the environment blocks - ''' - v = np.array([[0, 0, 0], [1, 0, 0], [1, 1, 0], [0, 1, 0], [0, 0, 1], [1, 0, 1], [1, 1, 1], [0, 1, 1]], - dtype='float') - f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]]) - # clr = blocks[:,6:]/255 - n = blocks.shape[0] - d = blocks[:, 3:6] - blocks[:, :3] - vl = np.zeros((8 * n, 3)) - fl = np.zeros((6 * n, 4), dtype='int64') - # fcl = np.zeros((6*n,3)) - for k in range(n): - vl[k * 8:(k + 1) * 8, :] = v * d[k] + blocks[k, :3] - fl[k * 6:(k + 1) * 6, :] = f + k * 8 - # fcl[k*6:(k+1)*6,:] = clr[k,:] - - if type(ax) is Poly3DCollection: - ax.set_verts(vl[fl]) - else: - pc = Poly3DCollection(vl[fl], alpha=0.15, linewidths=1, edgecolors='k') - # pc.set_facecolor(fcl) - h = ax.add_collection3d(pc) - return h - - -''' The following utils can be used for rrt or rrt*, - required param initparams should have - env, environement generated from env3D - V, node set - E, edge set - i, nodes added - maxiter, maximum iteration allowed - stepsize, leaf growth restriction - -''' - - -def sampleFree(initparams): - '''biased sampling''' - x = np.random.uniform(initparams.env.boundary[0:3], initparams.env.boundary[3:6]) - i = np.random.random() - if isinside(initparams, x): - return sampleFree(initparams) - else: - if i < 0.05: - return initparams.env.goal - else: return np.array(x) - - -def isinside(initparams, x): - '''see if inside obstacle''' - for i in initparams.env.blocks: - if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]: - return True - return False - - -def isCollide(initparams, x, y): - '''see if line intersects obstacle''' - ray = getRay(x, y) - dist = getDist(x, y) - for i in getAABB(initparams.env.blocks): - shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i) - if shot is not None: - dist_wall = getDist(x, shot) - if dist_wall <= dist: # collide - return True - return False - - -def nearest(initparams, x): - V = np.array(initparams.V) - if initparams.i == 0: - return initparams.V[0] - xr = repmat(x, len(V), 1) - dists = np.linalg.norm(xr - V, axis=1) - return initparams.V[np.argmin(dists)] - - -def steer(initparams, x, y): - direc = (y - x) / np.linalg.norm(y - x) - xnew = x + initparams.stepsize * direc - return xnew - - -def near(initparams, x): - # TODO: r = min{gamma*log(card(V)/card(V)1/d),eta} - r = initparams.stepsize - V = np.array(initparams.V) - if initparams.i == 0: - return initparams.V[0] - xr = repmat(x, len(V), 1) - inside = np.linalg.norm(xr - V, axis=1) < r - nearpoints = V[inside] - return np.array(nearpoints) - - -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])] - return cost(initparams, xparent) + getDist(x, xparent) - - -def visualization(initparams): - V = np.array(initparams.V) - E = initparams.E - Path = np.array(initparams.Path) - start = initparams.env.start - goal = initparams.env.goal - ax = plt.subplot(111, projection='3d') - ax.view_init(elev=0., azim=90) - ax.clear() - draw_block_list(ax, initparams.env.blocks) - edges = E.get_edge() - if edges != []: - for i in edges: - 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) - ax.add_line(line) - - if Path != []: - for i in Path: - 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, color='r') - ax.add_line(line) - - 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]) - plt.xlim(initparams.env.boundary[0], initparams.env.boundary[3]) - plt.ylim(initparams.env.boundary[1], initparams.env.boundary[4]) - ax.set_zlim(initparams.env.boundary[2], initparams.env.boundary[5]) - plt.xlabel('x') - plt.ylabel('y') - if not Path != []: - plt.pause(0.001) - else: - plt.show() - - -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])] - Path.append(np.array([x, x2])) - dist += getDist(x, x2) - x = x2 - return Path, dist - -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]) - if x in self.E: - self.E[x].append(y) - else: - self.E[x] = [y] - - def remove_edge(self,edge): - x, y = edge[0],edge[1] - self.E[self.hash(x)].remove(self.hash(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))) - return edges - -if __name__ == '__main__': - x = np.array([1.0,2.3,3.3]) - y = np.array([1.2,2.4,3.5]) - E = edgeset() - E.add_edge([x,y]) - print(E.get_edge()) - E.remove_edge([x,y]) - print(E.get_edge()) \ No newline at end of file +import numpy as np +from numpy.matlib import repmat +import pyrr as pyrr + +import os +import sys +sys.path.append(os.path.dirname(os.path.abspath(__file__))+"/../../Sampling-based Planning/") +from rrt_3D.plot_util3D import visualization + +def getRay(x, y): + direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]] + return np.array([x, direc]) + +def getAABB(blocks): + AABB = [] + for i in blocks: + AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger + return AABB + + +def getDist(pos1, pos2): + return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2])) + +''' The following utils can be used for rrt or rrt*, + required param initparams should have + env, environement generated from env3D + V, node set + E, edge set + i, nodes added + maxiter, maximum iteration allowed + stepsize, leaf growth restriction + +''' + + +def sampleFree(initparams): + '''biased sampling''' + x = np.random.uniform(initparams.env.boundary[0:3], initparams.env.boundary[3:6]) + i = np.random.random() + if isinside(initparams, x): + return sampleFree(initparams) + else: + if i < 0.05: + return initparams.env.goal + else: return np.array(x) + + +def isinside(initparams, x): + '''see if inside obstacle''' + for i in initparams.env.blocks: + if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]: + return True + return False + + +def isCollide(initparams, x, y): + '''see if line intersects obstacle''' + ray = getRay(x, y) + dist = getDist(x, y) + for i in getAABB(initparams.env.blocks): + shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i) + if shot is not None: + dist_wall = getDist(x, shot) + if dist_wall <= dist: # collide + return True + for i in initparams.env.balls: + shot = pyrr.geometric_tests.ray_intersect_sphere(ray, i) + if shot != []: + dists_wall = [getDist(x, j) for j in shot] + if all(dists_wall <= dist): # collide + return True + return False + + +def nearest(initparams, x): + V = np.array(initparams.V) + if initparams.i == 0: + return initparams.V[0] + xr = repmat(x, len(V), 1) + dists = np.linalg.norm(xr - V, axis=1) + return initparams.V[np.argmin(dists)] + + +def steer(initparams, x, y): + direc = (y - x) / np.linalg.norm(y - x) + xnew = x + initparams.stepsize * direc + return xnew + + +def near(initparams, x): + # TODO: r = min{gamma*log(card(V)/card(V)1/d),eta} + r = initparams.stepsize + V = np.array(initparams.V) + if initparams.i == 0: + return initparams.V[0] + xr = repmat(x, len(V), 1) + inside = np.linalg.norm(xr - V, axis=1) < r + nearpoints = V[inside] + return np.array(nearpoints) + + +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])] + 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])] + Path.append(np.array([x, x2])) + dist += getDist(x, x2) + x = x2 + return Path, dist + +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]) + if x in self.E: + self.E[x].append(y) + else: + self.E[x] = [y] + + def remove_edge(self,edge): + x, y = edge[0],edge[1] + self.E[self.hash(x)].remove(self.hash(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))) + return edges \ No newline at end of file