yq change env3d and plotting

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)

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

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())
+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