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) x = np.array([[5], [4]]) end = SE2(R=R, x=x)
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) c = Bezier.constructBezierPath(start, end, 3, [2, 1]) fig = plt.figure() ax = fig.add_subplot(111, projection='3d')
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