def cost(x, vidx, params):
"""Cost function of the optimization problem."""
# y = reshape(x, np.atleast_2d([]), params.ndim, params.inipts[vidx, :], params.finalpts[vidx, :])
# return np.linalg.norm(np.diff(y))
# accel = Bernstein(y, t0=params.t0, tf=params.tf).diff().diff().normSquare()
# return accel.cpts.sum()
if vidx:
y = reshape(x, params.traj, 3, params.inipts[vidx, :],
params.finalpts[vidx, :])
nveh = y.shape[0] // 3
dist = 0.0
traj = Bernstein(y[0:3, :])
for i in range(1, nveh):
traj2 = Bernstein(y[i * 3:(i + 1) * 3, :])
dv = traj - traj2
# dist += dv.normSquare().cpts.sum()
# temp = dv.normSquare().min()
# if temp < params.dsafe:
# dist -= np.inf
# dist += temp
dist += sum(100 / dv.normSquare().elev(DEG_ELEV).cpts.squeeze())
return dist
else:
return 0.0
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
:rtype: np.ndarray
"""
if nveh > 1:
# distVeh = np.empty(nveh-1)
distVeh = []
# vehTraj = bez.Bezier(y[0:ndim, :])
vehTraj = Bernstein(y[0:ndim, :])
for i in range(1, nveh):
# tempTraj = bez.Bezier(y[i*ndim:(i+1)*ndim, :])
tempTraj = Bernstein(y[i * ndim:(i + 1) * ndim, :])
dv = vehTraj - tempTraj
# distVeh[i-1] = dv.normSquare().elev(DEG_ELEV).cpts.min()
distVeh.append(dv.normSquare().elev(DEG_ELEV).cpts.squeeze())
return (np.concatenate(distVeh) - maxSep**2)
else:
return np.atleast_1d(0.0)
def nonlcon(x, params):
"""
Nonlinear constraints for the optimization problem.
These constraints include maximum speed, maximum angular rate, minimum
safe temporal distance between vehicles, and minimum safe distance between
vehicles and obstacles.
Parameters
----------
x : numpy.array
1D optimization vector.
params : Parameters
Parameters for the problem being solved.
Returns
-------
numpy.array
Degree elevated approximation of the nonlinear constraints of the
problem where all constraints must be >= 0 to be feasible.
"""
y, tf = reshape(x, params.deg, params.inipt, params.finalpt,
params.inispeed, params.finalspeed, params.inipsi,
params.finalpsi)
traj = Bernstein(y, t0=0., tf=tf)
maxSpeed = params.vmax**2 - speed(traj)
angRate = angularRate(traj)
angRateMax = params.wmax - angRate
angRateMin = angRate + params.wmax
separation = obstacleAvoidance(
[traj], params.obstacles, elev=params.degElev) - params.dsafe**2
return np.concatenate([maxSpeed, angRateMax, angRateMin, separation])
def buildTrajList(y, nveh, times):
"""
Builds a list of Bernstein trajectory objects given the reshapped matrix y.
Parameters
----------
y : numpy.array
Reshapped optimization vector.
nveh : int
Number of vehicles.
times : numpy.array
Vector containing the final times of each vehicle. Note that initial
time is assumed to be 0.
Returns
-------
trajs : list
List of Bernstein trajectory objects.
"""
trajs = []
for i in range(nveh):
trajs.append(Bernstein(y[2 * i:2 * (i + 1), :], tf=times[i]))
return trajs
def build_traj_list(y, params):
"""
"""
trajs = []
for i in range(params.nveh):
trajs.append(Bernstein(y[i * params.ndim:(i + 1) * params.ndim, :]))
return trajs
def cost(x, vidx, params):
"""Cost function of the optimization problem
"""
# return 0
y = reshape(x, np.atleast_2d([]), params.ndim, params.inipts[vidx, :],
params.finalpts[vidx, :])
# return np.linalg.norm(np.diff(y))
accel = Bernstein(y, t0=params.t0, tf=params.tf).diff().diff().normSquare()
return accel.cpts.sum()
def distSqr(c1, c2, obs):
fig, ax = plt.subplots()
obsPoly = Bernstein(np.atleast_2d(obs.center).T*np.ones((2, c1.deg+1)), min(c1.t0, c2.t0), max(c1.tf, c2.tf))
c1c2 = c1 - c2
c1obs = c1 - obsPoly
c2obs = c2 - obsPoly
c1c2.normSquare().plot(ax, label=r'$||\mathbf{C}^{[1]}(t) - \mathbf{C}^{[2]}(t)||^2$')
c1obs.normSquare().plot(ax, label=r'$||\mathbf{C}^{[1]}(t) - \mathbf{Obs}(t)||^2$')
c2obs.normSquare().plot(ax, label=r'$||\mathbf{C}^{[2]}(t) - \mathbf{Obs}(t)||^2$')
ax.set_title('Squared Distance Between Trajectories and Obstacle', wrap=True)
ax.set_xlabel('Time (s)')
ax.set_ylabel(r'Squared Distance $(m^2)$')
ax.legend()
if __name__ == '__main__':
plt.close('all')
df = pd.read_csv('../Examples/HawksLogo_1000pts.csv')
finpts = np.concatenate([df.values, 100 * np.ones((1000, 1))], 1)
x = np.random.rand(1000, 1) * (finpts[:, 0].max() -
finpts[:, 0].min()) + finpts[:, 0].min()
y = np.random.rand(1000, 1) * (finpts[:, 1].max() -
finpts[:, 1].min()) + finpts[:, 1].min()
z = np.zeros((1000, 1))
inipts = np.concatenate([x, y, z], 1)
cpts = [np.linspace(pt[0], pt[1], 3).T for pt in zip(inipts, finpts)]
trajs = [Bernstein(cpt) for cpt in cpts]
ax = trajs[0].plot(showCpts=False)
for traj in trajs[1:]:
traj.plot(ax, showCpts=False)
# now = time.time()
# print('Full trajectories')
# test(trajs)
# print(f'Total time: {time.time()-now}')
# now = time.time()
# print('Partial Trajs')
# for i in np.linspace(0, 900, 10).astype(int):
# test(trajs[i:i+100])
distVeh.append(dv.normSquare().elev(elev).cpts)
return np.array(distVeh).flatten()
if __name__ == '__main__':
import matplotlib.pyplot as plt
from polynomial.bernstein import Bernstein
cpts1 = np.array([[0, 1, 2, 3, 4, 5], [4, 7, 3, 9, 4, 5]], dtype=float)
cpts2 = np.array([[9, 8, 9, 7, 9, 8], [0, 1, 2, 3, 4, 5]], dtype=float)
cpts3 = np.array([[2, 3, 4, 5, 6, 6], [10, 11, 12, 13, 14, 2]],
dtype=float)
cpts4 = np.array([[0, 1, 2, 3, 4, 5], [1, 1, 1, 1, 1, 1]], dtype=float)
c1 = Bernstein(cpts1)
c2 = Bernstein(cpts2)
c3 = Bernstein(cpts3)
c4 = Bernstein(cpts4)
bpList = [c1, c2, c3, c4]
# Testing whether temporal separation is working properly. Note that
# negative distances do make sense since it corresponds to the control
# points and not the actual curve. However, if a negative distance is found
# and the curves do not intersect, the degree elevation should be increased
distVeh = temporalSeparation(bpList)
print(distVeh)
if np.any(distVeh < 0):
print('[!] Warning, negative distance!')
results = minimize(cost,
x0,
constraints=cons,
bounds=bounds,
method='SLSQP',
options={
'maxiter': 250,
'disp': True,
'iprint': 1
})
# Plot everything
y, tf = reshape(results.x, params.deg, params.inipt, params.finalpt,
params.inispeed, params.finalspeed, params.inipsi,
params.finalpsi)
trajs.append(Bernstein(y, t0=0., tf=tf))
x0 = results.x
for traj in trajs:
traj.plot(ax, showCpts=False, label=next(legNamesI))
obs1 = plt.Circle(params.obstacles[0], radius=params.dsafe, ec='k', fc='r')
obs2 = plt.Circle(params.obstacles[1], radius=params.dsafe, ec='k', fc='g')
ax.add_artist(obs1)
ax.add_artist(obs2)
ax.set_title('Dubins Car Time Optimal')
ax.set_xlim([-0.5, 15.5])
ax.set_ylim([-0.5, 10.5])
ax.set_xlabel('X Position (m)')
ax.set_ylabel('Y Position (m)')
if __name__ == '__main__':
# Creates a Figures directory if it doesn't already exist
if SAVE_FIG:
import os
if not os.path.isdir('Figures'):
os.mkdir('Figures')
# Define control points as numpy arrays. Be sure to set the dtype to float.
cpts1 = np.array([[0, 1, 2, 3, 4, 5], [5, 0, 2, 5, 7, 5]], dtype=float)
cpts2 = np.array([[0, 2, 4, 6, 8, 10], [3, 7, 3, 5, 8, 9]], dtype=float)
t0 = 10 # Initial time
tf = 20 # Final time
c1 = Bernstein(cpts1, t0=t0, tf=tf)
c2 = Bernstein(cpts2, t0=t0, tf=tf)
# =========================================================================
# Examples of Bernstein polynomial properties
# =========================================================================
plt.close('all')
# Property 1 - Convex Hull
convexHull(c1)
# Property 2 - End Point Values
endPoints(c1)
# Property 3 - Derivatives
derivatives(c1)
def _obs2bp(obs, deg, t0, tf):
cpts = np.empty((2, deg + 1), dtype=float)
cpts[0, :] = obs[0]
cpts[1, :] = obs[1]
return Bernstein(cpts, t0=t0, tf=tf)
# Prune if the global min is less than the lower bound
if globMin < lb:
return globMin
# If we are within the desired tolerance, return
if ub - lb < tol:
return globMin
# Otherwise split and continue
else:
tdiv = (minIdx / bp.deg) * (bp.tf - bp.t0) + bp.t0
c1, c2 = bp.split(tdiv)
c1min = _min(c1, dim=dim, globMin=globMin, tol=tol)
c2min = _min(c2, dim=dim, globMin=globMin, tol=tol)
return min(c1min, c2min)
if __name__ == '__main__':
cpts1 = np.array([[0, 1, 2, 3, 4, 5], [33, 6, 7, 2, 4, 5]], dtype=float)
c1 = Bernstein(cpts1)
c1t = Bernstein(cpts1, t0=10, tf=20)
print(f'c1 min[0]: {c1.min(0)}')
print(f'c1 min[1]: {c1.min(1)}')
print(f'c1t min[0]: {c1.min(0)}')
print(f'c1t min[1]: {c1.min(1)}')
print(f'c1 norm sqr min: {c1.normSquare().min()}')
traj = reshape(res.x, traj, params.ndim, params.inipts[i, :],
params.finalpts[i, :])
params.traj = traj.copy()
trajs.append(traj)
tend = time.time()
trajs = np.concatenate(trajs)
print('===============================================================')
print(f'Total computation time for 1000 vehicles: {tend-tstart}')
print('===============================================================')
# Plot the trajectories
plt.close('all')
temp = Bernstein(trajs[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,
zorder=10)
for i in range(1000):
temp = Bernstein(trajs[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,
zorder=10)
Parameters
----------
bp : Bernstein
Bernstein polynomial whose speed will be determined using the L2 norm
and degree elevation. The resulting BP will be elevated by DEG_ELEV
defined in constants. Note that according to the product property, the
norm squared will be of order 2N before being elevated.
Returns
-------
speed : numpy.array
1D numpy array of control points representing the BP of the L2 speed
of the BP passed in.
"""
speed = bp.diff().normSquare().elev(elev).cpts.flatten()
return speed
if __name__ == '__main__':
from polynomial.bernstein import Bernstein
cpts = np.array([[0, 1, 2, 3, 4, 5], [4, 5, 3, 6, 8, 7]], dtype=float)
c = Bernstein(cpts)
vel = speed(c)
print(vel)
if __name__ == '__main__':
plt.close('all')
setRCParams()
obs = Sphere(4, 5, 5, 1, color='b')
cpts3 = np.array(
[[7, 3, 1, 1, 3, 7], [1, 2, 3, 8, 3, 5], [0, 2, 1, 9, 8, 10]],
dtype=float)
cpts4 = np.array(
[[1, 1, 4, 4, 8, 8], [5, 6, 9, 10, 8, 6], [1, 1, 3, 5, 11, 6]],
dtype=float)
c3 = Bernstein(cpts3, t0=10, tf=20)
c4 = Bernstein(cpts4, t0=10, tf=20)
initPlot(c3, c4, obs)
endPoints(c3, c4, obs)
convexHull(c3, c4, obs)
convexHullSplit(c3, c4, obs)
convexHullElev(c3, c4, obs)
speedSquared(c3, c4)
accelSquared(c3, c4)
distSqr(c3, c4, obs)
if SAVE_FIG:
saveFigs()
plt.show()
print('Done saving figures')
if __name__ == '__main__':
plt.close('all')
setRCParams()
# Rectangle obstacle
def obs(): return Circle((3, 4), 1, ec='k', lw=4)
# Control points
cpts1 = np.array([[0, 2, 4, 6, 8, 10],
[5, 0, 2, 3, 10, 3]], dtype=float)
cpts2 = np.array([[1, 3, 6, 8, 10, 12],
[6, 9, 10, 11, 8, 8]], dtype=float)
# Bernstein polynomials
c1 = Bernstein(cpts1, t0=10, tf=20)
c2 = Bernstein(cpts2, t0=10, tf=20)
initialPlot(c1, c2, obs())
endPoints(c1, c2, obs())
convexHull(c1, c2, obs())
convexHullSplit(c1, c2, obs())
convexHullElev(c1, c2, obs())
speedSquared(c1, c2)
headingAngle(c1, c2)
angularRate(c1, c2)
distSqr(c1, c2, obs())
plt.show()
if SAVE_FIG: