def TimeCostFunction(path: Bezier, timePolyCoeffs, minSpd, maxSpd, maxGs,
tspan):
# Cost function of 4 terms: total curvature, curvature variance, length, and speed variance
dt = 0.01
t = np.arange(0, tspan[1] - tspan[0],
dt) # time step for evaulating cost
tau = np.polyval(timePolyCoeffs, t)
tauPrime = np.polyval(np.polyder(timePolyCoeffs), t)
_, v = path.evalJet(tau)
speeds = np.linalg.norm(v, 2, 0) * tauPrime # Speed in real units
cost = 0
k = path.evalCurv(tau) # Curvature
ac = np.power(speeds, 2) * k # Centripetal Acceleration
Wvel = 100
Wk = 5
if (Wvel > 0):
cost += Wvel * np.sum(np.power(speeds - 0.5 *
(minSpd + maxSpd), 2))
if (Wk > 0):
cost += Wk * np.sum(np.power(ac, 2))
return cost
def BezierCostFunction(path: Bezier, optParams: FlightOptParams):
# Cost function of 4 terms: total curvature, curvature variance, length, and speed variance
t = np.arange(0, 1 + optParams.dt,
optParams.dt) # time step for evaulating cost
cost = 0
_, v = path.evalJet(t)
speeds = np.linalg.norm(v, 2, 0)
k = path.evalCurv(t)
if (optParams.Wlen > 0):
pathLength = optParams.dt * np.nansum(speeds)
cost += optParams.Wlen * pathLength
if (optParams.Wcurv > 0):
totalCurv = np.nansum(np.power(k, 2))
cost += optParams.Wcurv * totalCurv
if (optParams.Wkdev > 0):
curvDev = np.nanvar(k, ddof=1)
cost += optParams.Wkdev * curvDev
if (optParams.Wspdev > 0):
spddev = np.nanvar(speeds, ddof=1)
cost += optParams.Wspdev * spddev
if (optParams.Wagree > 0):
startVec = path.Q[:, 1] - path.Q[:, 0]
endVec = path.Q[:, 3] - path.Q[:, 2]
startAngle = np.arctan2(startVec[1], startVec[0])
endAngle = np.arctan2(endVec[1], endVec[0])
angles = np.linspace(startAngle, endAngle, np.shape(t)[0])
vecs = np.vstack((np.cos(angles), np.sin(angles)))
ramp = np.linspace(1, 0, int(len(t) / 10))
weight = np.concatenate(
(ramp, np.zeros(
(np.shape(t)[0] - 2 * np.shape(ramp)[0])), np.flip(ramp)))
agree = np.sum(angles * weight * v / (speeds + optParams.rho))
cost += optParams.Wkdev * agree
#print(cost)
#pdb.set_trace()
return cost
def __init__(self,
startPose,
endPose,
tspan=[0, 1],
bezierOrder=3,
optParams=FlightOptParams(),
spec=FlightSpec()):
super().__init__(tspan)
self.startPose = startPose
self.endPose = endPose
self.bezier = Bezier(bezierOrder)
self.tspan = tspan
self.duration = tspan[1] - tspan[0]
self.optParams = optParams
self.timePolyCoeffs = np.array(
[0, 0, 0, 0, 1 / (tspan[1] - tspan[0]),
0]) # By default, polynomial does not change s input
self.spec = spec
def error(self, variables):
my_params = variables.at(self.keys()[0])
params = np.empty((self.dimension * (self._order - 3) + 2, ))
if (self.optParams.init != None and self.optParams.final == None):
params[0] = self.duration * (self.optParams.init / self._order)
params[1:] = my_params
elif (self.optParams.init != None and self.optParams.final != None):
params[0] = self.duration * (self.optParams.init / self._order)
params[1] = self.duration * (self.optParams.final / self._order)
params[2:] = my_params
else:
params = my_params
b = Bezier.constructBezierPath(self._start, self._end, self._order,
params)
return np.array([Flight.BezierCostFunction(b, self.optParams)])
import numpy as np import Curves.Bezier as Bezier from Lie import SE3 from matplotlib import pyplot as plt b = Bezier(3) start = np.array([[0], [0]]) end = np.array([[5], [5]]) p1 = np.array([[1], [3]]) p2 = np.array([[4], [5]]) points2 = np.array([[0, 1, 9, 5], [0, 1, -1, 5], [0, 3, 2, 5]]) b.setControlPoints(points2) t = np.linspace(0, 1, 10) x = b.eval(t) v = b.evalJet(t) a = b.evalJet2(t) k = b.evalCurv(t) start = SE3() roll = np.pi / 3 pitch = np.pi / 4 yaw = np.pi / 6 R = np.matmul(np.matmul(SE3.RotZ(yaw), SE3.RotY(pitch)), SE3.RotX(roll)) x = np.array([[3], [6], [2]]) end = SE3(R=R, x=x)
B = np.zeros((4, 2))
B[2, 0] = 1
B[3, 1] = 1
print(B)
Q = np.eye(4)
linctrl = linear(np.zeros((4, 1)), np.zeros((2, 1)))
linctrl.setByCARE(A, B, Q)
linctrl.noControl()
leom = linear.systemDynamics(A, B)
#pathgen = linepath.linepath()
pathgen = Flight2D()
pathgen.bezier = Bezier(3)
pathgen.setDynConstraints(0, 10, 4)
ts = structure()
ts.Th = 0.5
ts.Td = 0.2
ts.vec2state = lambda x: x
cfS = structure(dt=0.05,
odeMethod=niODERK4,
controller=timepoints(pathgen, linctrl, ts))
tspan = [0, 20]
curveType = 'circ'
if (curveType == 'circ'):
from Curves import Flight import numpy as np import Curves import Curves.Bezier as Bezier import Lie.group.SE2.Homog as SE2 from matplotlib import pyplot as plt b = Bezier(3) start = np.array([[0], [0]]) end = np.array([[5], [5]]) p1 = np.array([[1], [3]]) p2 = np.array([[4], [5]]) points2 = np.array([[4, 1, 9, 5], [3, 1, 8, 5]]) b.setControlPoints(points2) t = np.linspace(0, 1, 10) x, v, a = b.evalJet2(t) k = b.evalCurv(t) print(v) print(a) print(k) optS = Flight.FlightOptParams(Wkdev=1) print(Flight.Flight.BezierCostFunction(b, optS)) start = SE2() theta = np.pi / 3 R = SE2.rotationMatrix(theta)
def optimizeBezierPath(self):
if (self.bezier.order == 3 and self.optParams.init != None
and self.optParams.final != None):
self.bezier = Bezier.constructBezierPath(
self.startPose, self.endPose, self.bezier.order,
self.duration * np.array([
self.optParams.init / self.bezier.order,
self.optParams.final / self.bezier.order
]))
else:
graph = minisam.FactorGraph()
#loss = minisam.CauchyLoss.Cauchy(0.) # TODO: Options Struct
loss = None
graph.add(
BezierCurveFactor(minisam.key('p', 0),
self.startPose,
self.endPose,
self.bezier.order,
self.duration,
loss,
optParams=self.optParams))
init_values = minisam.Variables()
opt_param = minisam.LevenbergMarquardtOptimizerParams()
#opt_param.verbosity_level = minisam.NonlinearOptimizerVerbosityLevel.ITERATION
opt = minisam.LevenbergMarquardtOptimizer(opt_param)
values = minisam.Variables()
linePts = self.startPose.getTranslation() + \
np.arange(0,1+1/(self.bezier.order),1/(self.bezier.order))*(self.endPose.getTranslation()- self.startPose.getTranslation())
#pdb.set_trace()
if (self.optParams.init != None and self.optParams.final == None):
# TODO: Initial Conditions
initialGuess = np.hstack((linePts[3:-2].reshape((1, -1)), 1))
#init_values.add(minisam.key('p', 0), np.ones((1+self.dimension*(self.bezier.order-3),)))
init_values.add(minisam.key('p', 0), initialGuess)
opt.optimize(graph, init_values, values)
d = np.array([self.optParams.init / self.bezier.order])
self.bezier = Bezier.constructBezierPath(
self.startPose, self.endPose, self.bezier.order,
np.hstack((d, values.at(minisam.key('p', 0)))))
elif (self.optParams.init != None
and self.optParams.final != None):
print("Both Constrained")
initialGuess = linePts[:, 2:-2].reshape((1, -1))
d = self.duration * np.array([
self.optParams.init / self.bezier.order,
self.optParams.final / self.bezier.order
])
unit = np.zeros((self.dimension))
unit[0] = 1
pos2 = self.startPose * (d[0] * unit)
pos3 = self.endPose * (-d[1] * unit)
v = (pos3 - pos2) / (self.bezier.order - 2)
initialGuess = pos2 + np.multiply(
np.arange(1, self.bezier.order - 3 + 1), v)
initialGuess = initialGuess.reshape((1, -1))
init_values.add(minisam.key('p', 0), np.squeeze(initialGuess))
#init_values.add(minisam.key('p', 0), np.ones((self.dimension*(self.bezier.order-3),)))
opt.optimize(graph, init_values, values)
#pdb.set_trace()
self.bezier = Bezier.constructBezierPath(
self.startPose, self.endPose, self.bezier.order,
np.hstack((d, values.at(minisam.key('p', 0)))))
else:
initialGuess = np.hstack((linePts[3:-2].reshape((1, -1))))
#init_values.add(minisam.key('p', 0), np.ones((2+self.dimension*(self.bezier.order-3),)))
init_values.add(minisam.key('p', 0), initialGuess)
opt.optimize(graph, init_values, values)
self.bezier = Bezier.constructBezierPath(
self.startPose, self.endPose, self.bezier.order,
values.at(minisam.key('p', 0)))
class Flight(CurveBase):
def __init__(self,
startPose,
endPose,
tspan=[0, 1],
bezierOrder=3,
optParams=FlightOptParams(),
spec=FlightSpec()):
super().__init__(tspan)
self.startPose = startPose
self.endPose = endPose
self.bezier = Bezier(bezierOrder)
self.tspan = tspan
self.duration = tspan[1] - tspan[0]
self.optParams = optParams
self.timePolyCoeffs = np.array(
[0, 0, 0, 0, 1 / (tspan[1] - tspan[0]),
0]) # By default, polynomial does not change s input
self.spec = spec
#self.dimension = len(startPose.getTranslation())
def constructBezierPath(self, param):
# Changes based on dimension
#constructBezierPath uses parameterization to define bezier curve
pts = np.empty((self.dimension, self.bezier.order + 1))
unit = np.zeros(self.dimension, )
unit[0] = 1
if (self.bezier.order == 3): # 3rd Order curve with 2 free points
pos2 = self.startPose * (param[0] * unit)
pos3 = self.endPose * (-param[1] * unit)
pts = np.hstack((self.startPose.getTranslation(), pos2, pos3,
self.endPose.getTranslation()))
elif (self.bezier.order > 3):
d1 = self.startPose * (param[0] * unit)
posmid = np.reshape(param[2:],
(self.dimension, self.bezier.order - 3))
d2 = self.endPose * (-param[1] * unit)
pts = np.hstack((self.startPose.getTranslation(), d1, posmid, d2,
self.endPose.getTranslation()))
self.bezier.setControlPoints(pts)
def optimizeBezierPath(self):
if (self.bezier.order == 3 and self.optParams.init != None
and self.optParams.final != None):
self.bezier = Bezier.constructBezierPath(
self.startPose, self.endPose, self.bezier.order,
self.duration * np.array([
self.optParams.init / self.bezier.order,
self.optParams.final / self.bezier.order
]))
else:
graph = minisam.FactorGraph()
#loss = minisam.CauchyLoss.Cauchy(0.) # TODO: Options Struct
loss = None
graph.add(
BezierCurveFactor(minisam.key('p', 0),
self.startPose,
self.endPose,
self.bezier.order,
self.duration,
loss,
optParams=self.optParams))
init_values = minisam.Variables()
opt_param = minisam.LevenbergMarquardtOptimizerParams()
#opt_param.verbosity_level = minisam.NonlinearOptimizerVerbosityLevel.ITERATION
opt = minisam.LevenbergMarquardtOptimizer(opt_param)
values = minisam.Variables()
linePts = self.startPose.getTranslation() + \
np.arange(0,1+1/(self.bezier.order),1/(self.bezier.order))*(self.endPose.getTranslation()- self.startPose.getTranslation())
#pdb.set_trace()
if (self.optParams.init != None and self.optParams.final == None):
# TODO: Initial Conditions
initialGuess = np.hstack((linePts[3:-2].reshape((1, -1)), 1))
#init_values.add(minisam.key('p', 0), np.ones((1+self.dimension*(self.bezier.order-3),)))
init_values.add(minisam.key('p', 0), initialGuess)
opt.optimize(graph, init_values, values)
d = np.array([self.optParams.init / self.bezier.order])
self.bezier = Bezier.constructBezierPath(
self.startPose, self.endPose, self.bezier.order,
np.hstack((d, values.at(minisam.key('p', 0)))))
elif (self.optParams.init != None
and self.optParams.final != None):
print("Both Constrained")
initialGuess = linePts[:, 2:-2].reshape((1, -1))
d = self.duration * np.array([
self.optParams.init / self.bezier.order,
self.optParams.final / self.bezier.order
])
unit = np.zeros((self.dimension))
unit[0] = 1
pos2 = self.startPose * (d[0] * unit)
pos3 = self.endPose * (-d[1] * unit)
v = (pos3 - pos2) / (self.bezier.order - 2)
initialGuess = pos2 + np.multiply(
np.arange(1, self.bezier.order - 3 + 1), v)
initialGuess = initialGuess.reshape((1, -1))
init_values.add(minisam.key('p', 0), np.squeeze(initialGuess))
#init_values.add(minisam.key('p', 0), np.ones((self.dimension*(self.bezier.order-3),)))
opt.optimize(graph, init_values, values)
#pdb.set_trace()
self.bezier = Bezier.constructBezierPath(
self.startPose, self.endPose, self.bezier.order,
np.hstack((d, values.at(minisam.key('p', 0)))))
else:
initialGuess = np.hstack((linePts[3:-2].reshape((1, -1))))
#init_values.add(minisam.key('p', 0), np.ones((2+self.dimension*(self.bezier.order-3),)))
init_values.add(minisam.key('p', 0), initialGuess)
opt.optimize(graph, init_values, values)
self.bezier = Bezier.constructBezierPath(
self.startPose, self.endPose, self.bezier.order,
values.at(minisam.key('p', 0)))
@staticmethod
def gen5thTimePoly(cVec, td):
if (td != 0):
b = np.array([
0, 1 - cVec[0] * td**2 - cVec[1] * td**3, 1 / td,
1 / td - 2 * cVec[0] * td - 3 * cVec[1] * td**2
])
b = np.reshape(b, (4, 1))
A = np.array([
[1, 0, 0, 0],
[1, td, td**4, td**5],
[0, 1, 0, 0],
[0, 1, 4 * td**3, 5 * td**4],
])
beta = np.matmul(np.linalg.inv(A), b)
beta = beta.T
beta = np.squeeze(beta)
coeffs = np.flip(np.hstack((beta[0:2], cVec, beta[2:4])))
return coeffs
#self.timePolyCoeffs = fliplr(obj.timePolyCoeffs);
else:
return np.zeros((6, ))
def setDynConstraints(self, minSpd, maxSpd, maxGs):
self.minSpd = minSpd
self.maxSpd = maxSpd
self.maxGs = maxGs
def optimizeTimePoly(self):
graph = minisam.FactorGraph()
#loss = minisam.CauchyLoss.Cauchy(0.) # TODO: Options Struct
loss = None
graph.add(
TimePolyFactor(minisam.key('p', 0),
self.bezier,
self.minSpd,
self.maxSpd,
self.maxGs,
loss,
ts=self.tspan[0],
tf=self.tspan[1]))
init_values = minisam.Variables()
opt_param = minisam.LevenbergMarquardtOptimizerParams()
#opt_param.verbosity_level = minisam.NonlinearOptimizerVerbosityLevel.ITERATION
opt = minisam.LevenbergMarquardtOptimizer(opt_param)
values = minisam.Variables()
init_values.add(minisam.key('p', 0), np.ones((2, )))
opt.optimize(graph, init_values, values)
self.timePolyCoeffs = Flight.gen5thTimePoly(
values.at(minisam.key('p', 0)), self.duration)
# In this function we use the time polynomial to "Stretch" s which is progress from 0-1
def evalTimePoly(self, t):
s = np.polyval(self.timePolyCoeffs, t)
#pdb.set_trace()
dsdt = np.polyval(np.polyder(self.timePolyCoeffs), t)
d2sdt2 = np.polyval(np.polyder(self.timePolyCoeffs, 2), t)
return s, dsdt, d2sdt2
# t is real time here. The time polynomial gives progress (s) to input into bezier eval
def evalPos(self, t):
(s, dsdt, _) = self.evalTimePoly(t - self.tspan[0])
return self.bezier.eval(s)
# same as evalPos but for velocity
def evalVel(self, t):
(s, dsdt, _) = self.evalTimePoly(t - self.tspan[0])
_, vs = self.bezier.evalJet(s)
v = vs * dsdt
return v
# same as evalPos but for velocity
def evalAcc(self, t):
(s, dsdt, d2sdt2) = self.evalTimePoly(t - self.tspan[0])
xs, vs, accs = self.bezier.evalJet2(s)
a = accs * (dsdt**2) + vs * d2sdt2
return a
def x(self, t):
return self.spec.vec2state(
np.vstack((self.evalPos(t), self.evalVel(t), self.evalAcc(t))))
def plotControlPoints(self, axes=None):
self.bezier.plot(axes)
@abc.abstractmethod
def plotCurve(self, axes=None):
return
@staticmethod
def BezierCostFunction(path: Bezier, optParams: FlightOptParams):
# Cost function of 4 terms: total curvature, curvature variance, length, and speed variance
t = np.arange(0, 1 + optParams.dt,
optParams.dt) # time step for evaulating cost
cost = 0
_, v = path.evalJet(t)
speeds = np.linalg.norm(v, 2, 0)
k = path.evalCurv(t)
if (optParams.Wlen > 0):
pathLength = optParams.dt * np.nansum(speeds)
cost += optParams.Wlen * pathLength
if (optParams.Wcurv > 0):
totalCurv = np.nansum(np.power(k, 2))
cost += optParams.Wcurv * totalCurv
if (optParams.Wkdev > 0):
curvDev = np.nanvar(k, ddof=1)
cost += optParams.Wkdev * curvDev
if (optParams.Wspdev > 0):
spddev = np.nanvar(speeds, ddof=1)
cost += optParams.Wspdev * spddev
if (optParams.Wagree > 0):
startVec = path.Q[:, 1] - path.Q[:, 0]
endVec = path.Q[:, 3] - path.Q[:, 2]
startAngle = np.arctan2(startVec[1], startVec[0])
endAngle = np.arctan2(endVec[1], endVec[0])
angles = np.linspace(startAngle, endAngle, np.shape(t)[0])
vecs = np.vstack((np.cos(angles), np.sin(angles)))
ramp = np.linspace(1, 0, int(len(t) / 10))
weight = np.concatenate(
(ramp, np.zeros(
(np.shape(t)[0] - 2 * np.shape(ramp)[0])), np.flip(ramp)))
agree = np.sum(angles * weight * v / (speeds + optParams.rho))
cost += optParams.Wkdev * agree
#print(cost)
#pdb.set_trace()
return cost
@staticmethod
def TimeCostFunction(path: Bezier, timePolyCoeffs, minSpd, maxSpd, maxGs,
tspan):
# Cost function of 4 terms: total curvature, curvature variance, length, and speed variance
dt = 0.01
t = np.arange(0, tspan[1] - tspan[0],
dt) # time step for evaulating cost
tau = np.polyval(timePolyCoeffs, t)
tauPrime = np.polyval(np.polyder(timePolyCoeffs), t)
_, v = path.evalJet(tau)
speeds = np.linalg.norm(v, 2, 0) * tauPrime # Speed in real units
cost = 0
k = path.evalCurv(tau) # Curvature
ac = np.power(speeds, 2) * k # Centripetal Acceleration
Wvel = 100
Wk = 5
if (Wvel > 0):
cost += Wvel * np.sum(np.power(speeds - 0.5 *
(minSpd + maxSpd), 2))
if (Wk > 0):
cost += Wk * np.sum(np.power(ac, 2))
return cost