def temporalSeparationConstraints(y, nveh, ndim, maxSep): """Calculate the separation between vehicles. The maximum separation is found by degree elevation. :param x: Optimization vector :type x: np.array :param nveh: Number of vehicles :type nveh: int :param dim: Dimension of the vehicles :type dim: int :param maxSep: Maximum separation between vehicles :type maxSep: float :return: Minimum temporal separation between the vehicles """ if nveh > 1: distVeh = np.empty(nveh - 1) vehTraj = bez.Bezier(y[0:ndim, :]) for i in range(1, nveh): tempTraj = bez.Bezier(y[i * ndim:(i + 1) * ndim, :]) dv = vehTraj - tempTraj distVeh[i - 1] = dv.normSquare().elev(10).cpts.min() return (distVeh - maxSep**2) else: return np.atleast_1d(0.0)
def _maxAngularRateConstraints(x, nVeh, dim, model, maxAngRate): """Creates the maximum angular rate constraint. This is useful for a dubins car model that has a constraint on the maximum angular rate. The dimension is assumed to be 2. :param x: Optimization vector :type x: numpy.ndarray :param nVeh: Number of vehicles :type nVeh: int :param dim: Dimension of the vehicles. Currently only works for 2D :type dim: int :param model: See model description in BezOptimization class description :type model: dict :param maxAngRate: Maximum angular rate of the vehicle (in radians). :type maxAngRate: float :return: Inequality constraint for the maximum angular rate :rtype: float """ y = reshapeVector(x, nVeh, dim, model) if model['type'].lower() == 'timeopt': tf = x[-1] else: tf = model['tf'] angularRates = [] for i in range(nVeh): pos = bez.Bezier(y[i*dim:i*dim+dim, :], tf=tf) angRate = angularRateSqr(pos.elev(10)) angularRates.append(angRate) angularRateCpts = np.concatenate( [i.cpts.squeeze() for i in angularRates]) return (maxAngRate**2 - angularRateCpts).squeeze()
def spatialSeparationConstraints(self, x): """ """ numVeh = self.model['numVeh'] dim = self.model['dim'] maxSep = self.model['maxSep'] numObs = numVeh + len(self.shapeObstacles) distances = [] vehList = [] y = self.reshapeVector(x) for i in range(numVeh): vehList.append(bez.Bezier(y[i*dim:(i+1)*dim, :])) for obstacle in self.shapeObstacles: vehList.append(obstacle) for i in range(numObs): for j in range(i, numObs): if j > i: distances.append(vehList[i].minDist(vehList[j])) return np.array(distances)-maxSep
def _objectiveFunction(x, nVeh, dim, model, minGoal): """Objective function to be optimized. :param x: Optimization vector :type x: numpy.ndarray :param nVeh: Number of vehicles :type nVeh: int :param dim: Dimension of the vehicles. Currently only works for 2D :type dim: int :param model: See model description in BezOptimization class description :type model: dict :param minGoal: Element to be minimized. This string can be one of a few different values: vel - Minimize the sum of velocities of the vehicle trajectories accel - Minimize the sum of accelerations of the vehicle trajectories jerk - Minimize the sum of jerks of the vehicle trajectories euclidean - Minimize the sum of the Euclidean distance between the control points of each trajectory. :type minGoal: str :return: Cost of the current iteration according to the minGoal :rtype: float """ y = reshapeVector(x, nVeh, dim, model) tf = model['tf'] curves = [] minGoal = minGoal.lower() if minGoal == 'euclidean': return euclideanObjective(y, nVeh, dim) elif minGoal == 'time': return x[-1] else: for i in range(nVeh): pos = bez.Bezier(y[i*dim:i*dim+dim, :], tf=tf) vel = pos.diff() if minGoal == 'accel': accel = vel.diff() curves.append(accel) elif minGoal == 'jerk': jerk = vel.diff().diff() curves.append(jerk) summation = 0.0 for curve in curves: temp = curve.normSquare() summation = summation + temp.cpts.sum() return summation
def _separationConstraints(x, nVeh, dim, model, maxSep): """Calculate the separation between vehicles. The maximum separation is found by degree elevation. NOTE: This only works for 2 dimensions. :param x: Optimization vector :type x: numpy.ndarray :param nVeh: Number of vehicles :type nVeh: int :param dim: Dimension of the vehicles. Currently only works for 2D :type dim: int :param model: See model description in BezOptimization class description :type model: dict :param maxSep: Maximum separation between vehicles. :type maxSep: float """ y = reshapeVector(x, nVeh, dim, model) if model['type'].lower() == 'obstacles' or \ model['type'].lower() == 'timeopt': nVeh += 2 obs = np.empty((4, y.shape[1])) obs[0, :] = 3 obs[1, :] = 2 obs[2, :] = 6 obs[3, :] = 7 y = np.vstack((y, obs)) if nVeh > 1: if model['type'].lower() == 'timeopt': tf = x[-1] else: tf = model['tf'] distVeh = [] vehList = [] for i in range(nVeh): vehList.append(bez.Bezier(y[i*dim:i*dim+dim, :], tf=tf)) for i in range(nVeh): for j in range(i, nVeh): if j > i: dv = vehList[i] - vehList[j] distVeh.append(dv.normSquare().elev(10)) distances = np.concatenate([i.cpts.squeeze() for i in distVeh]) return (distances - maxSep**2).squeeze() else: return None
def createCubicBezierSegment(self, startX, startY, endX, endY, ts, te): # print('STARTX', startX) # print('TS', ts) # print('TE', te) # print('ENDX', endX) x = [ startX, (1.0 / 3.0) * (endX - startX) + startX, endX - (1.0 / 3.0) * (endX - startX), endX ] y = [startY, (1.0 / 3.0) * te + startY, endY - (1.0 / 3.0) * ts, endY] # print('P0', x[0], 'P1', x[1], 'P2', x[2], 'P3', x[3]) # print('P0', y[0], 'P1', y[1], 'P2', y[2], 'P3', y[3]) bez = bz.Bezier(x, y) return bez
def _minJerkObjective(y, nVeh, dim, tf): """ """ curves = [] for i in range(nVeh): pos = bez.Bezier(y[i*dim:i*dim+dim, :], tf=tf) vel = pos.diff() jerk = vel.diff().diff() curves.append(jerk) summation = 0.0 for curve in curves: temp = curve.normSquare().elev(DEG_ELEV) summation = summation + temp.cpts.sum() return summation
def bezier_spacing(N, L, ptsx, ptsy, plot=0): """setup spacing based on bezier curve, higher the value = tighter the spacing""" pts = [] for i in range(len(ptsx)): pts.append(bz.pt(ptsx[i], ptsy[i])) bc = bz.Bezier(pts) dx = bz.xyBezier(bc, N) dx = 1.0 / (dx + 1.0) dx = (L / sum(dx)) * dx if plot > 0: bz.plotCP(pts) bz.plotBezier(bc, Nx) show() return np.array(dx)
def _minSpeedConstraints(y, nVeh, dim, tf, minSpeed): """Creates the minimum velocity constraints. Useful in systems such as aircraft who may not fall below a certain speed. :param x: Optimization vector :type x: numpy.ndarray :param nVeh: Number of vehicles :type nVeh: int :param dim: Dimension of the vehicles. Currently only works for 2D. :type dim: int :param model: See model description in BezOptimization class description. :type model: dict :param minSpeed: Minimum speed of the vehicle. :type minSpeed: float :return: Inequality constraint for the minimum speed. :rtype: float """ # y = reshapeVector(x, nVeh, dim, model) # if model['type'].lower() == 'timeopt': # tf = x[-1] # else: # tf = model['tf'] speeds = [] for i in range(nVeh): pos = bez.Bezier(y[i*dim:i*dim+dim, :], tf=tf) speed = pos.diff() speeds.append(speed) speedSqr = [curve.normSquare().elev(DEG_ELEV) for curve in speeds] speeds = np.concatenate([i.cpts.squeeze() for i in speedSqr]) return (speeds - minSpeed**2).squeeze()
def _maxSpeedConstraints(x, nVeh, dim, model, maxSpeed): """Creates the maximum velocity constraints. Useful for limiting the maximum speed of a vehicle. :param x: Optimization vector :type x: numpy.ndarray :param nVeh: Number of vehicles :type nVeh: int :param dim: Dimension of the vehicles. Currently only works for 2D :type dim: int :param model: See model description in BezOptimization class description :type model: dict :param maxSpeed: Maximum speed of the vehicle. :type maxSpeed: float :return: Inequality constraint for the maximum speed :rtype: float """ y = reshapeVector(x, nVeh, dim, model) if model['type'].lower() == 'timeopt': tf = x[-1] else: tf = model['tf'] speeds = [] for i in range(nVeh): pos = bez.Bezier(y[i*dim:i*dim+dim, :], tf=tf) speed = pos.diff() speeds.append(speed) speedSqr = [curve.normSquare().elev(10) for curve in speeds] speeds = np.concatenate([i.cpts.squeeze() for i in speedSqr]) return (maxSpeed**2 - speeds).squeeze()
def _temporalSeparationConstraints(y, nVeh, dim, maxSep): """Calculate the separation between vehicles. The maximum separation is found by degree elevation. NOTE: This only works for 2 dimensions. :param x: Optimization vector :type x: numpy.ndarray :param nVeh: Number of vehicles :type nVeh: int :param dim: Dimension of the vehicles. Currently only works for 2D :type dim: int :param maxSep: Maximum separation between vehicles. :type maxSep: float """ if nVeh > 1: distVeh = [] vehList = [] for i in range(nVeh): vehList.append(bez.Bezier(y[i*dim:(i+1)*dim, :])) for i in range(nVeh-1): for j in range(i+1, nVeh): dv = vehList[i] - vehList[j] distVeh.append(dv.normSquare().elev(DEG_ELEV)) # distVeh.append(dv.normSquare().min()) distances = np.concatenate([i.cpts.squeeze() for i in distVeh]) # distances = np.array(distVeh) return (distances - maxSep**2).squeeze() else: return None
xGuess = bezopt.generateGuess(std=0) ineqCons = [{ 'type': 'ineq', 'fun': bezopt.temporalSeparationConstraints }, { 'type': 'ineq', 'fun': bezopt.maxSpeedConstraints }, { 'type': 'ineq', 'fun': bezopt.maxAngularRateConstraints }, { 'type': 'ineq', 'fun': lambda x: x[-1] }] _ = bez.Bezier(bezopt.reshapeVector(xGuess)) _.elev(10) _ = _ * _ startTime = time.time() print('starting') results = sop.minimize(bezopt.objectiveFunction, x0=xGuess, method='SLSQP', constraints=ineqCons, options={ 'maxiter': 250, 'disp': True, 'iprint': 2 }) endTime = time.time()
cptsKNN = bezopt.reshapeVector(results.x) ########################################################################### # Plot Results ########################################################################### plt.close('all') numVeh = bezopt.model['numVeh'] dim = bezopt.model['dim'] maxSep = bezopt.model['maxSep'] ###### Straight Line Results fig, ax = plt.subplots() curves = [] for i in range(numVeh): curves.append(bez.Bezier(cptsSL[i*dim:(i+1)*dim])) for curve in curves: plt.plot(curve.curve[0], curve.curve[1], '-', curve.cpts[0], curve.cpts[1], '.--') obstacle1 = plt.Circle(bezopt.pointObstacles[0], radius=maxSep, edgecolor='Black', facecolor='red') obstacle2 = plt.Circle(bezopt.pointObstacles[1], radius=maxSep, edgecolor='Black', facecolor='green') ax.add_artist(obstacle1) ax.add_artist(obstacle2) plt.xlim([-10, 10])
fc="none") pp1.set_color("#95a7df") ax.add_patch(pp1) ax.scatter(*pt, s=32, facecolors='none', edgecolors='b') if use_text: ax.text(*((pt + closest) / 2), str(distance)) ax.text(*pt, str(pt.astype(np.int))) ax.text(*closest, str(closest.astype(np.int))) fig.canvas.draw() return None fig, ax = plt.subplots() cid = fig.canvas.mpl_connect('button_press_event', onclick) ax.grid() ax.axis('equal') ax.margins(0.4) plot_bezier(bez, ax) plt.title("Click next to the curve.") plt.show() if __name__ == "__main__": points = np.array([[0, 0], [0, 1], [1, .8], [1.5, 1]]).astype(np.float32) points *= 50 points += 10 bez = bezier.Bezier(points) matplotlib_example(bez, use_text=False)
@author: ckielasjensen """ import matplotlib.pyplot as plt import numpy as np import bezier as bez NPTS = 1001 plt.close('all') cpts1 = np.array([(0, 1, 2, 3, 4, 5), (3, 4, 2, 6, 7, 8)]) cpts2 = np.array([(0, 1, 2, 3, 4, 5), (7, 8, 5, 9, 2, 3)]) c1 = bez.Bezier(cpts1, tau=np.linspace(0, 1, NPTS)) c2 = bez.Bezier(cpts2, tau=np.linspace(0, 1, NPTS)) normsquare = (c1 - c2).normSquare() thing = ((c1 - c2) * (c1 - c2)).cpts[0, :] + ((c1 - c2) * (c1 - c2)).cpts[1, :] ax = normsquare.plot() x = np.linspace(0, 1, NPTS) y = (c1.curve - c2.curve)[0, :]**2 + (c1.curve - c2.curve)[1, :]**2 ax.plot(x, y) plt.show()
cpts4[1, :] -= 1 cpts5 = cpts1 - 3 cpts6 = np.array([(0, 1, 2, 3, 4, 5), (5, 0, 2, 5, 7, 5)]) cpts7 = np.array([(0, 1, 3, 5, 7, 7, 8, 9, 9), (0, 6, 9, 6, 8, 3, 7, 8, 3)]) poly1 = np.array([(1, 3, 3), (1, 3, 2), (1, 4, 1), (3, 3, 3), (1, 5, 1)]) poly2 = np.array([(1, 1, 3), (1, 1, 2), (1, 2, 1), (4, 0, 2), (1, 3, 1)]) poly3 = np.array([(1, 1, 0), (1, 3, 0), (2, 5, 0), (4, 4, 0)]) c1 = bez.Bezier(cpts1) c2 = bez.Bezier(cpts2) c3 = bez.Bezier(cpts3) c4 = bez.Bezier(cpts4) c5 = bez.Bezier(cpts5) c6 = bez.Bezier(cpts6) c7 = bez.Bezier(cpts7) # --- # Bezier to Bezier # --- d31, t31, t1 = c3.minDist(c1) d32, t32, t2 = c3.minDist(c2) d34, t34, t4 = c3.minDist(c4) d35, t35, t5 = c3.minDist(c5)
'finalAngs': np.array([0, 0]), 'pointObstacles': [[1, 2], [3, 4]] } bezopt = BezOptimization(**params) xGuess = bezopt.generateGuess() x = np.random.randint(0, 10, xLen) cpts1 = bezopt.reshapeVector(x)[:2, :] cpts2 = bezopt.reshapeVector(x)[2:, :] print('cpts1:\n{},\ncpts2:\n{}'.format(cpts1, cpts2)) print('X Guess:\n{}'.format(xGuess)) c1guess = bez.Bezier(bezopt.reshapeVector(xGuess)[:2, :]) c2guess = bez.Bezier(bezopt.reshapeVector(xGuess)[2:, :]) plt.close('all') ax1 = c1guess.plot() c2guess.plot(ax1) plt.title('X Guess') c1 = bez.Bezier(cpts1) c2 = bez.Bezier(cpts2) ax2 = c1.plot() c2.plot(ax2) plt.title('Reshape Vector') plt.show()
(0, 0, 0, 0, 0, 0,)]) poly1 = np.array([(1, 1, 3), (1, 1, 2), (1, 2, 1), (3, 1, 3), (1, 3, 1)]) poly2 = np.array([(1, 1, 3), (1, 1, 2), (1, 2, 1), (3, -1, 3), (1, 3, 1)]) # Creating curves from control points c1 = bez.Bezier(cpts1) c2 = bez.Bezier(cpts2) c3 = bez.Bezier(cpts3) c4 = bez.Bezier(cpts4) # --- # Example 1 - minimum distance between curves # --- dist12, t1, t2 = c1.minDist(c2) print(f'The minimum distance between C1 and C2 is {dist12}') # Plot the curves and the distance # Note that when plotting, the control points of the curve will also be # plotted. ax1 = c1.plot() c2.plot(ax1)
""" Created on Tue Mar 19 10:11:04 2019 @author: ckielasjensen """ import matplotlib.pyplot as plt import numpy as np import scipy.optimize as sop import time import bezier as bez from optimization import BezOptimization if __name__ == '__main__': track1 = bez.Bezier([[0, 0, 0, 3, 4, 5, 6, 7, 10, 10, 10], [0, 3, 4, 5, 6, 6, 6, 6, 7, 8, 10]]) track2 = bez.Bezier([[4, 4, 4, 7, 8, 9, 10, 11, 14, 14, 14], [0, 3, 4, 4, 4, 5, 5, 5, 7, 8, 10]]) tracks = [track1, track2] bezopt = BezOptimization(numVeh=1, dimension=2, degree=10, minimizeGoal='TimeOpt', maxSep=0.5, maxSpeed=5, maxAngRate=0.5, initPoints=(2, 1), finalPoints=(12, 9), initSpeeds=1,
}] res = sop.minimize(fun, x0, constraints=cons, method='SLSQP', options={ 'maxiter': 250, 'disp': True, 'iprint': 0 }) traj = reshape(res.x, traj, params.ndim, params.inipts[i, :], params.finalpts[i, :]) tend = time.time() print('===============================================================') print(f'Total computation time for {NVEH} vehicles: {tend-tstart}') print('===============================================================') temp = bez.Bezier(traj[0:NDIM, :]) vehList = [temp] ax = temp.plot(showCpts=False) plt.plot([temp.cpts[0, -1]], [temp.cpts[1, -1]], [temp.cpts[2, -1]], 'k.', markersize=15) for i in range(NVEH): temp = bez.Bezier(traj[i * NDIM:(i + 1) * NDIM, :]) vehList.append(temp) temp.plot(ax, showCpts=False) plt.plot([temp.cpts[0, -1]], [temp.cpts[1, -1]], [temp.cpts[2, -1]], 'k.', markersize=15)