Merge branch '19-surface' into dev

requirements.txt

@@ -1,37 +1,6 @@
-attrs==21.4.0
-black==22.1.0
-click==8.0.3
-cvxpy==1.1.18
-cycler==0.11.0
-ecos==2.0.10
-fonttools==4.29.1
-iniconfig==1.1.1
-kiwisolver==1.3.2
-llvmlite==0.38.0
-matplotlib==3.5.1
-mypy-extensions==0.4.3
-networkx==2.6.3
-numba==0.55.1
-numpy==1.21.5
-osqp==0.6.2.post5
-packaging==21.3
-pathspec==0.9.0
-perlin-numpy==0.0.0
-Pillow==9.0.0
-platformdirs==2.4.1
-pluggy==1.0.0
-py==1.11.0
-pyfastnoisesimd==0.4.2
-pyparsing==3.0.7
-pytest==6.2.5
-python-dateutil==2.8.2
-PyYAML==6.0
-qdldl==0.1.5.post0
-rrtpp==0.0.0
-scipy==1.7.3
-scs==3.1.0
-six==1.16.0
-toml==0.10.2
-tomli==2.0.0
-tqdm==4.62.3
-typing_extensions==4.0.1
+cvxpy>=1.2
+numba>=0.55
+tqdm>=3
+networkx>=2.5
+matplotlib>=3.2
+pyfastnoisesimd>=0.4

rrtplanner/oggen.py

@@ -70,16 +70,49 @@ def perlin_terrain(w: int, h: int, scale: int = 1, frames: int = None) -> np.nda
  noise.frequency = 0.01 * scale
  noise.noiseType = pyfastnoisesimd.NoiseType(5)
  if frames is not None:
- xynoise = noise.genAsGrid(shape=[frames, w, h])
+ xynoise = noise.genAsGrid(shape=[frames, h, w])
  else:
- xynoise = noise.genAsGrid(shape=[1, w, h])
+ xynoise = noise.genAsGrid(shape=[1, h, w])
  xynoise = np.squeeze(xynoise)
  # normalize to [0,1]
  xynoise -= xynoise.min()
  xynoise = xynoise / (xynoise.max() - xynoise.min())
  return xynoise
 
 
+def apply_ridge(X, H, steep, width, fn="arctan"):
+ """
+ Apply a "ridge" function for terrain generation.
+ """
+ mid = np.ptp(X) / 2.0
+ if fn == "arctan":
+ for i in range(X.shape[1]):
+ t1 = np.arctan(steep * (X[:, i] - mid) - width)
+ t2 = np.arctan(steep * (X[:, i] - mid) + width)
+ H[:, i] *= -(t1 - t2) / (2.0 * np.arctan(width))
+ elif fn == "bell":
+ for i in range(X.shape[1]):
+ H[:, i] *= np.exp(-((X[:, i] - mid) ** 2) / (2.0 * steep**2))
+ return H
+
+
+def xy_grid(xrange, yrange, shape):
+ xr = np.linspace(xrange[0], xrange[1], shape[0])
+ yr = np.linspace(yrange[0], yrange[1], shape[1])
+ X, Y = np.meshgrid(xr, yr)
+ return X, Y
+
+
+def example_terrain(xmax, ymax, cols, rows, h=30.0):
+ X, Y = xy_grid([0, xmax], [0, ymax], [cols, rows])
+ H = perlin_terrain(cols, rows, scale=1.0)
+ H *= perlin_terrain(cols, rows, scale=2.0)
+ H *= perlin_terrain(cols, rows, scale=4.0)
+ H = apply_ridge(X, H, steep=5.0, width=1.0, fn="bell")
+ H *= 50.0 / np.ptp(H)
+ return X, Y, H
+
+
 if __name__ == "__main__":
  fig = plt.figure()
  ax = fig.add_subplot(111)

rrtplanner/plots.py

@@ -7,6 +7,34 @@
 from mpl_toolkits.mplot3d.axes3d import Axes3D
 
 
+def plot_surface(ax: Axes3D, X, Y, S, zsquash=0.2, wireframe=True, cmap="viridis"):
+ if not isinstance(ax, Axes3D):
+ raise TypeError(f"ax must be an Axes3D object. Got {type(ax)}")
+
+ ax.set_box_aspect((np.ptp(X), np.ptp(Y), np.ptp(S) * zsquash))
+ # ax.set_proj_type("ortho")
+ # draw sheet
+ if wireframe:
+ hmin, hmax = S.min(), S.max()
+ norm = Normalize((hmin - hmax) * 0.03, hmax)
+ colors = cm.get_cmap(cmap)(norm(S))
+ s = ax.plot_surface(
+ X,
+ Y,
+ S,
+ zorder=2,
+ linewidths=0.5,
+ shade=False,
+ facecolors=colors,
+ rcount=X.shape[0],
+ ccount=X.shape[1],
+ )
+ s.set_facecolor((0, 0, 0, 0))
+ else:
+ ax.plot_surface(X, Y, S, cmap=cm.get_cmap(cmap), zorder=2)
+ return ax
+
+
 def remove_axticks(ax):
  ax.set_xticks([])
  ax.set_yticks([])

rrtplanner/surface.py

@@ -1,5 +1,6 @@
 import cvxpy as cp
 import numpy as np
+<<<<<<< HEAD
 
 
 def getsurface(
@@ -161,3 +162,123 @@ def ridge_fn(x, mid, width):
  for i, f in enumerate([fig0, fig1, fig2, fig3]):
  print("Saving figure {}/4".format(i))
  f.savefig("./figure_output/{}.png".format(i), dpi=300)
+=======
+from mpl_toolkits.mplot3d.axes3d import Axes3D
+import matplotlib.pyplot as plt
+from matplotlib import cm
+from .oggen import perlin_terrain
+
+
+def xy_grid(xrange, yrange, shape):
+ xr = np.linspace(xrange[0], xrange[1], shape[0])
+ yr = np.linspace(yrange[0], yrange[1], shape[1])
+ X, Y = np.meshgrid(xr, yr)
+ return X, Y
+
+
+class Surface(object):
+ def __init__(self, X, Y, H, params):
+ # check that shapes match
+ if X.shape != Y.shape or X.shape != H.shape or Y.shape != H.shape:
+ raise ValueError(
+ f"X, Y, H must have the same shape. X: {X.shape}, Y: {Y.shape}, H: {H.shape}"
+ )
+ # check that problem parameters are present
+ for p in ["minh", "gaph", "mindx", "mind2x"]:
+ if p not in params:
+ raise ValueError(f"{p} must be in params")
+
+ self.X, self.Y, self.H = X, Y, None
+
+ # set parameter H
+ self.setH(H)
+ self.S = cp.Variable(shape=X.shape, name="Surface")
+ self.minh = params["minh"]
+ self.gaph = params["gaph"]
+ self.mindx = params["mindx"]
+ self.mind2x = params["mind2x"]
+
+ # problem
+ self.problem = self.setUp()
+
+ def setH(self, H):
+ if self.H is None:
+ self.H = cp.Parameter(shape=H.shape, name="Terrain Height", value=H)
+ else:
+ self.H.value = H
+
+ def _diff(self, x_1, x, x1, h, k=1):
+ if k == 1:
+ """first derivative"""
+ return (x1 - x_1) / (2 * h)
+ elif k == 2:
+ """second derivative"""
+ return (x1 - 2 * x + x_1) / (h**2)
+
+ def setUp(self, active_constr=["minh", "gaph", "mindx", "mind2x"]):
+ objective = 0.0
+ subjectto = []
+ dh = self.X[0, 1] - self.X[0, 0]
+
+ for constr in active_constr:
+ if constr == "minh":
+ c_minh = self.S >= self.minh
+ subjectto += [c_minh]
+ elif constr == "gaph":
+ c_gaph = self.S - self.H >= self.gaph
+ subjectto += [c_gaph]
+
+ # # forward and backward differences
+
+ fx_1 = self.S[:, 2:] # x_i-1
+ fx1 = self.S[:, :-2] # x_i+1
+ fx = self.S[:, 1:-1] # x_i
+
+ fy_1 = self.S[2:, :] # y_i-1
+ fy1 = self.S[:-2, :] # y_i+1
+ fy = self.S[1:-1, :] # y_i
+
+ # first diff
+ c_dx = cp.abs(self._diff(fx_1, fx, fx1, dh, k=1)) <= self.mindx
+ c_dy = cp.abs(self._diff(fy_1, fy, fy1, dh, k=1)) <= self.mindx
+
+ # second diff
+ c_d2x = cp.abs(self._diff(fx_1, fx, fx1, dh, k=2)) <= self.mind2x
+ c_d2y = cp.abs(self._diff(fy_1, fy, fy1, dh, k=2)) <= self.mind2x
+
+ subjectto += [c_dx, c_dy, c_d2x, c_d2y]
+ objective = cp.sum(self.S)
+
+ problem = cp.Problem(cp.Minimize(objective), subjectto)
+ return problem
+
+ def solve(self, verbose=False, solver=cp.ECOS, warm_start=False):
+ self.problem.solve(verbose=verbose, solver=solver, warm_start=warm_start)
+ return self.S.value
+
+
+def example_terrain(xmax, ymax, cols, rows, h=30.0):
+ X, Y = xy_grid([0, xmax], [0, ymax], [cols, rows])
+ H = perlin_terrain(cols, rows, scale=1.0)
+ H *= perlin_terrain(cols, rows, scale=2.0)
+ H *= perlin_terrain(cols, rows, scale=4.0)
+ H = apply_ridge(X, H, steep=5.0, width=1.0, fn="bell")
+ H *= 50.0 / np.ptp(H)
+ return X, Y, H
+
+
+def apply_ridge(X, H, steep, width, fn="arctan"):
+ """
+ Apply a "ridge" function for terrain generation.
+ """
+ mid = np.ptp(X) / 2.0
+ if fn == "arctan":
+ for i in range(X.shape[1]):
+ t1 = np.arctan(steep * (X[:, i] - mid) - width)
+ t2 = np.arctan(steep * (X[:, i] - mid) + width)
+ H[:, i] *= -(t1 - t2) / (2.0 * np.arctan(width))
+ elif fn == "bell":
+ for i in range(X.shape[1]):
+ H[:, i] *= np.exp(-((X[:, i] - mid) ** 2) / (2.0 * steep**2))
+ return H
+>>>>>>> 19-surface

tests/test_imports.py

@@ -0,0 +1 @@
+from rrtplanner import anim, oggen, plots, rrt, surface