Template Class PolynomialOptimizationNonLinear

Class Documentation

template <int _N = 10>
class PolynomialOptimizationNonLinear

Implements a nonlinear optimization of the unconstrained optimization of paths consisting of polynomial segments as described in [1] [1]: Polynomial Trajectory Planning for Aggressive Quadrotor Flight in Dense Indoor Environments.

Charles Richter, Adam Bry, and Nicholas Roy. In ISRR 2013 _N specifies the number of coefficients for the underlying polynomials.

Public Types

enum [anonymous]

Values:

N = _N

Public Functions

PolynomialOptimizationNonLinear(size_t dimension, const NonlinearOptimizationParameters &parameters)

Sets up the nonlinear optimization problem.

Input: dimension = Spatial dimension of the problem. Usually 1 or 3. Input: parameters = Parameters for the optimization problem. Input: optimize_time_only = Specifies whether the optimization is run over the segment times only. If true, only the segment times are optimization parameters, and the remaining free parameters are found by solving the linear optimization problem with the given segment times in every iteration. If false, both segment times and free derivatives become optimization variables. The latter case is theoretically correct, but may result in more iterations.

bool setupFromVertices(const Vertex::Vector &vertices, const std::vector<double> &segment_times, int derivative_to_optimize = PolynomialOptimization<N>::kHighestDerivativeToOptimize)

Sets up the optimization problem from a vector of Vertex objects and a vector of times between the vertices.

Input: vertices = Vector containing the vertices defining the support points and constraints of the path. Input: segment_times = Vector containing an initial guess of the time between two vertices. Thus, its size is size(vertices) - 1. Input: derivative_to_optimize = Specifies the derivative of which the cost is optimized.

bool addMaximumMagnitudeConstraint(int derivative_order, double maximum_value)

Adds a constraint for the maximum of magnitude to the optimization problem.

Input: derivative_order = Order of the derivative, for which the constraint should be checked. Usually velocity (=1) or acceleration (=2). maximum_value = Maximum magnitude of the specified derivative.

bool solveLinear()

Solves the linear optimization problem according to [1].

The solver is re-used for every dimension, which means:

  • segment times are equal for each dimension.
  • each dimension has the same type/set of constraints. Their values can of course differ.

int optimize()

Runs the optimization until one of the stopping criteria in NonlinearOptimizationParameters and the constraints are met.

void getTrajectory(Trajectory *trajectory) const

Get the resulting trajectory out prefer this as the main method to get the results of the optimization, over getting the reference to the linear optimizer.

const PolynomialOptimization<N> &getPolynomialOptimizationRef() const

Returns a const reference to the underlying linear optimization object.

PolynomialOptimization<N> &getPolynomialOptimizationRef()

Returns a non-const reference to the underlying linear optimization object.

OptimizationInfo getOptimizationInfo() const
double getCost() const

Functions for optimization, but may be useful for diagnostics outside.

Gets the trajectory cost (same as the cost in the linear problem).

double getTotalCostWithSoftConstraints() const

Gets the cost including the soft constraints and time costs, should be the same cost function as used in the full optimization.

Returns the same metrics regardless of time estimation method set.

void scaleSegmentTimesWithViolation()