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)
