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)])
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') c.plot(ax) c.plotCurve(np.linspace(0, 1, 100), ax) plt.show()
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)))