- 3.0.2 optimal control module.
|
Defines a Linear-Quadratic Optimal Control Problem, which is optionally constrained. More...
#include <LQOCProblem.hpp>
Public Member Functions | |
LQOCProblem (int N=0) | |
constructor More... | |
int | getNumberOfStages () |
returns the number of discrete time steps in the LQOCP, including terminal stage More... | |
void | changeNumStages (int N) |
change the number of discrete time steps in the LQOCP More... | |
void | setZero (const int &nGenConstr=0) |
set all member variables to zero More... | |
void | setInputBoxConstraint (const int index, const int nConstr, const constr_vec_t &u_lb, const constr_vec_t &u_ub, const VectorXi &sp, const ct::core::ControlVector< CONTROL_DIM, SCALAR > &u_nom_abs) |
set input box constraints at a specific index More... | |
void | setInputBoxConstraints (const int nConstr, const constr_vec_t &u_lb, const constr_vec_t &u_ub, const VectorXi &sp, const ct::core::ControlVectorArray< CONTROL_DIM, SCALAR > &u_nom_abs) |
set uniform input box constraints, with the same constraint being applied at each intermediate stage More... | |
void | setIntermediateStateBoxConstraint (const int index, const int nConstr, const constr_vec_t &x_lb, const constr_vec_t &x_ub, const VectorXi &sp, const ct::core::StateVector< STATE_DIM, SCALAR > &x_nom_abs) |
set state box constraints at a specific index More... | |
void | setIntermediateStateBoxConstraints (const int nConstr, const constr_vec_t &x_lb, const constr_vec_t &x_ub, const VectorXi &sp, const ct::core::StateVectorArray< STATE_DIM, SCALAR > &x_nom_abs) |
set uniform state box constraints, with the same constraint being applied at each intermediate stage More... | |
void | setTerminalBoxConstraints (const int nConstr, const constr_vec_t &x_lb, const constr_vec_t &x_ub, const VectorXi &sp, const ct::core::StateVector< STATE_DIM, SCALAR > &x_nom_abs) |
set box constraints for terminal stage More... | |
void | setGeneralConstraints (const constr_vec_t &d_lb, const constr_vec_t &d_ub, const constr_state_jac_t &C, const constr_control_jac_t &D) |
set general (in)equaltiy constraints, with the same constraint applied at each stage More... | |
void | setFromTimeInvariantLinearQuadraticProblem (ct::core::DiscreteLinearSystem< STATE_DIM, CONTROL_DIM, SCALAR > &linearSystem, ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR > &costFunction, const ct::core::StateVector< STATE_DIM, SCALAR > &stateOffset, const double dt) |
a convenience method which constructs an unconstrained LQOC Problem from an LTI system and continuous-time quadratic cost The discretization of the cost functions happens within this function. It employs an Euler-Discretization More... | |
bool | isConstrained () const |
return a flag indicating whether this LQOC Problem is constrained or not More... | |
bool | isInputBoxConstrained () const |
bool | isStateBoxConstrained () const |
bool | isGeneralConstrained () const |
Public Attributes | |
ct::core::StateMatrixArray< STATE_DIM, SCALAR > | A_ |
affine, time-varying system dynamics in discrete time More... | |
ct::core::StateControlMatrixArray< STATE_DIM, CONTROL_DIM, SCALAR > | B_ |
ct::core::StateVectorArray< STATE_DIM, SCALAR > | b_ |
ct::core::ScalarArray< SCALAR > | q_ |
constant term of in the LQ approximation of the cost function More... | |
ct::core::StateVectorArray< STATE_DIM, SCALAR > | qv_ |
LQ approximation of the pure state penalty, including terminal state penalty. More... | |
ct::core::StateMatrixArray< STATE_DIM, SCALAR > | Q_ |
ct::core::ControlVectorArray< CONTROL_DIM, SCALAR > | rv_ |
LQ approximation of the pure control penalty. More... | |
ct::core::ControlMatrixArray< CONTROL_DIM, SCALAR > | R_ |
ct::core::FeedbackArray< STATE_DIM, CONTROL_DIM, SCALAR > | P_ |
LQ approximation of the cross terms of the cost function. More... | |
input_box_constr_array_t | u_lb_ |
bounds of box constraints for input and state More... | |
input_box_constr_array_t | u_ub_ |
state_box_constr_array_t | x_lb_ |
state_box_constr_array_t | x_ub_ |
input_box_constr_sparsity_array_t | u_I_ |
container for the box constraint sparsity pattern An example for how an element of this array might look like: [0 1 4 7] This would mean that box constraints act on elements 0, 1, 4 and 7 of the state or input vector More... | |
state_box_constr_sparsity_array_t | x_I_ |
std::vector< int > | nbu_ |
the number of input box constraints at every stage. More... | |
std::vector< int > | nbx_ |
the number of state box constraints at every stage. More... | |
constr_vec_array_t | d_lb_ |
general constraint lower bound More... | |
constr_vec_array_t | d_ub_ |
general constraint upper bound More... | |
constr_state_jac_array_t | C_ |
linear general constraint matrices More... | |
constr_control_jac_array_t | D_ |
std::vector< int > | ng_ |
number of general inequality constraints More... | |
Defines a Linear-Quadratic Optimal Control Problem, which is optionally constrained.
This class defines a Linear Quadratic Optimal Control (LQOC) Problem in differential formulation, consisting of
The unconstrained LQ problem has the following form:
subject to
The constrained LQ problem additionally implements the box constraints and and the general inequality constraints
which are both always kept in relative coordinates.
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::constr_vec_t = Eigen::Matrix<SCALAR, -1, -1> |
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::constr_state_jac_t = Eigen::Matrix<SCALAR, -1, -1> |
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::constr_control_jac_t = Eigen::Matrix<SCALAR, -1, -1> |
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::constr_vec_array_t = ct::core::DiscreteArray<constr_vec_t> |
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::constr_state_jac_array_t = ct::core::DiscreteArray<constr_state_jac_t> |
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::constr_control_jac_array_t = ct::core::DiscreteArray<constr_control_jac_t> |
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::input_box_constr_vec_t = Eigen::Matrix<SCALAR, CONTROL_DIM, 1> |
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::state_box_constr_vec_t = Eigen::Matrix<SCALAR, STATE_DIM, 1> |
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::input_box_constr_array_t = ct::core::DiscreteArray<input_box_constr_vec_t> |
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::state_box_constr_array_t = ct::core::DiscreteArray<state_box_constr_vec_t> |
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::state_box_constr_sparsity_t = Eigen::Matrix<int, STATE_DIM, 1> |
a vector indicating which box constraints are active and which not
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::input_box_constr_sparsity_t = Eigen::Matrix<int, CONTROL_DIM, 1> |
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::input_box_constr_sparsity_array_t = ct::core::DiscreteArray<input_box_constr_sparsity_t> |
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::state_box_constr_sparsity_array_t = ct::core::DiscreteArray<state_box_constr_sparsity_t> |
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::VectorXi = Eigen::VectorXi |
ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::LQOCProblem | ( | int | N = 0 | ) |
constructor
References ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::changeNumStages().
int ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::getNumberOfStages | ( | ) |
returns the number of discrete time steps in the LQOCP, including terminal stage
void ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::changeNumStages | ( | int | N | ) |
change the number of discrete time steps in the LQOCP
References ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::A_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::B_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::b_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::C_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::D_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::d_lb_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::d_ub_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::nbu_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::nbx_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::ng_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::P_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::q_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::Q_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::qv_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::R_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::rv_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::u_I_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::u_lb_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::u_ub_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::x_I_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::x_lb_, and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::x_ub_.
Referenced by ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::LQOCProblem().
void ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setZero | ( | const int & | nGenConstr = 0 | ) |
set all member variables to zero
nGenConstr | by default, we resize the general constraint containers to zero |
References ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::A_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::B_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::b_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::C_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::D_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::d_lb_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::d_ub_, i, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::nbu_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::nbx_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::ng_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::P_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::q_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::Q_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::qv_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::R_, and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::rv_.
Referenced by ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setFromTimeInvariantLinearQuadraticProblem().
void ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setInputBoxConstraint | ( | const int | index, |
const int | nConstr, | ||
const constr_vec_t & | u_lb, | ||
const constr_vec_t & | u_ub, | ||
const VectorXi & | sp, | ||
const ct::core::ControlVector< CONTROL_DIM, SCALAR > & | u_nom_abs | ||
) |
set input box constraints at a specific index
index | the index |
nConstr | the number of constraints |
u_lb | control lower bound in absolute coordinates |
u_ub | control upper bound in absolute coordinates |
sp | the sparsity vector, with strictly increasing indices, e.g. [0 1 4 7] |
References i, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::nbu_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::u_I_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::u_lb_, and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::u_ub_.
Referenced by ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setInputBoxConstraints().
void ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setInputBoxConstraints | ( | const int | nConstr, |
const constr_vec_t & | u_lb, | ||
const constr_vec_t & | u_ub, | ||
const VectorXi & | sp, | ||
const ct::core::ControlVectorArray< CONTROL_DIM, SCALAR > & | u_nom_abs | ||
) |
set uniform input box constraints, with the same constraint being applied at each intermediate stage
nConstr | the number of constraints |
u_lb | control lower bound in absolute coordinates |
u_ub | control upper bound in absolute coordinates |
sp | the sparsity vector, with strictly increasing indices, e.g. [0 1 4 7] |
u_nom_abs | current nominal control trajectory in absolute coordinates, required for coordinate transform |
References i, and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setInputBoxConstraint().
void ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setIntermediateStateBoxConstraint | ( | const int | index, |
const int | nConstr, | ||
const constr_vec_t & | x_lb, | ||
const constr_vec_t & | x_ub, | ||
const VectorXi & | sp, | ||
const ct::core::StateVector< STATE_DIM, SCALAR > & | x_nom_abs | ||
) |
set state box constraints at a specific index
index | the index |
nConstr | the number of constraints |
x_lb | state lower bound in absolute coordinates |
x_ub | state upper bound in absolute coordinates |
sp | the sparsity vector, with strictly increasing indices, e.g. [0 1 4 7] |
x_nom_abs | current nominal state vector in absolute coordinates, required for coordinate transform |
References i, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::nbx_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::x_I_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::x_lb_, and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::x_ub_.
Referenced by ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setIntermediateStateBoxConstraints().
void ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setIntermediateStateBoxConstraints | ( | const int | nConstr, |
const constr_vec_t & | x_lb, | ||
const constr_vec_t & | x_ub, | ||
const VectorXi & | sp, | ||
const ct::core::StateVectorArray< STATE_DIM, SCALAR > & | x_nom_abs | ||
) |
set uniform state box constraints, with the same constraint being applied at each intermediate stage
nConstr | the number of constraints |
x_lb | control lower bound in absolute coordinates |
x_ub | control upper bound in absolute coordinates |
sp | the sparsity vector, with strictly increasing indices, e.g. [0 1 4 7] |
x_nom_abs | current nominal state trajectory in absolute coordinates, required for coordinate transform |
References i, and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setIntermediateStateBoxConstraint().
void ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setTerminalBoxConstraints | ( | const int | nConstr, |
const constr_vec_t & | x_lb, | ||
const constr_vec_t & | x_ub, | ||
const VectorXi & | sp, | ||
const ct::core::StateVector< STATE_DIM, SCALAR > & | x_nom_abs | ||
) |
set box constraints for terminal stage
nConstr | the number of constraints |
x_lb | state lower bound in absolute coordinates |
x_ub | state upper bound in absolute coordinates |
sp | the sparsity vector, with strictly increasing indices, e.g. [0 1 4 7] |
x_nom_abs | current nominal terminal state vector in absolute coordinates, required for coordinate transform |
References i, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::nbx_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::x_I_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::x_lb_, and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::x_ub_.
void ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setGeneralConstraints | ( | const constr_vec_t & | d_lb, |
const constr_vec_t & | d_ub, | ||
const constr_state_jac_t & | C, | ||
const constr_control_jac_t & | D | ||
) |
set general (in)equaltiy constraints, with the same constraint applied at each stage
d_lb | general constraint lower bound |
d_ub | general constraint upper bound |
C | general constraint state jacobian |
D | general constraint control jacobian |
References ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::C_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::D_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::d_lb_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::d_ub_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::ng_, and ct::core::DiscreteArray< T, Alloc >::setConstant().
void ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setFromTimeInvariantLinearQuadraticProblem | ( | ct::core::DiscreteLinearSystem< STATE_DIM, CONTROL_DIM, SCALAR > & | linearSystem, |
ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR > & | costFunction, | ||
const ct::core::StateVector< STATE_DIM, SCALAR > & | stateOffset, | ||
const double | dt | ||
) |
a convenience method which constructs an unconstrained LQOC Problem from an LTI system and continuous-time quadratic cost The discretization of the cost functions happens within this function. It employs an Euler-Discretization
linearSystem | the discrete-time LTI system |
costFunction | the continuous-time cost function |
stateOffset | the offset for the affine system dynamics demanded by the LQOC Solver |
dt | the sampling time, required for discretization |
References ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::A_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::B_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::b_, ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR >::controlDerivativeIntermediate(), ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR >::controlSecondDerivativeIntermediate(), dt, ct::core::DiscreteLinearSystem< STATE_DIM, CONTROL_DIM, SCALAR >::getAandB(), ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::P_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::Q_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::qv_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::R_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::rv_, ct::optcon::CostFunction< STATE_DIM, CONTROL_DIM, SCALAR >::setCurrentStateAndControl(), ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setZero(), ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR >::stateControlDerivativeIntermediate(), ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR >::stateDerivativeIntermediate(), ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR >::stateDerivativeTerminal(), ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR >::stateSecondDerivativeIntermediate(), ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR >::stateSecondDerivativeTerminal(), and x0.
bool ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::isConstrained | ( | ) | const |
return a flag indicating whether this LQOC Problem is constrained or not
References ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::isGeneralConstrained(), ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::isInputBoxConstrained(), and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::isStateBoxConstrained().
bool ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::isInputBoxConstrained | ( | ) | const |
bool ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::isStateBoxConstrained | ( | ) | const |
bool ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::isGeneralConstrained | ( | ) | const |
ct::core::StateMatrixArray<STATE_DIM, SCALAR> ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::A_ |
affine, time-varying system dynamics in discrete time
Referenced by ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::changeNumStages(), ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::computeCostToGo(), ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::computeStatesAndControls(), ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::designController(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::executeLQApproximation(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::logToMatlab(), ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setFromTimeInvariantLinearQuadraticProblem(), and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setZero().
ct::core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR> ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::B_ |
Referenced by ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::changeNumStages(), ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::computeStatesAndControls(), ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::designController(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::executeLQApproximation(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::logToMatlab(), ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setFromTimeInvariantLinearQuadraticProblem(), and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setZero().
ct::core::StateVectorArray<STATE_DIM, SCALAR> ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::b_ |
Referenced by ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::changeNumStages(), ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::computeCostToGo(), ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::computeStatesAndControls(), ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::designController(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::executeLQApproximation(), ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setFromTimeInvariantLinearQuadraticProblem(), and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setZero().
ct::core::ScalarArray<SCALAR> ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::q_ |
constant term of in the LQ approximation of the cost function
Referenced by ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::changeNumStages(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::logToMatlab(), and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setZero().
ct::core::StateVectorArray<STATE_DIM, SCALAR> ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::qv_ |
LQ approximation of the pure state penalty, including terminal state penalty.
Referenced by ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::changeNumStages(), ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::computeCostToGo(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::executeLQApproximation(), ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::initializeCostToGo(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::initializeCostToGo(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::logToMatlab(), ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setFromTimeInvariantLinearQuadraticProblem(), and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setZero().
ct::core::StateMatrixArray<STATE_DIM, SCALAR> ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::Q_ |
Referenced by ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::changeNumStages(), ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::computeCostToGo(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::executeLQApproximation(), ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::initializeCostToGo(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::initializeCostToGo(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::logToMatlab(), ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setFromTimeInvariantLinearQuadraticProblem(), and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setZero().
ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::rv_ |
LQ approximation of the pure control penalty.
Referenced by ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::changeNumStages(), ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::designController(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::executeLQApproximation(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::logToMatlab(), ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setFromTimeInvariantLinearQuadraticProblem(), and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setZero().
ct::core::ControlMatrixArray<CONTROL_DIM, SCALAR> ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::R_ |
Referenced by ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::changeNumStages(), ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::designController(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::executeLQApproximation(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::logToMatlab(), ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setFromTimeInvariantLinearQuadraticProblem(), and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setZero().
ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR> ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::P_ |
LQ approximation of the cross terms of the cost function.
Referenced by ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::changeNumStages(), ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::designController(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::executeLQApproximation(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::logToMatlab(), ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setFromTimeInvariantLinearQuadraticProblem(), and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setZero().
input_box_constr_array_t ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::u_lb_ |
bounds of box constraints for input and state
Referenced by ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::changeNumStages(), and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setInputBoxConstraint().
input_box_constr_array_t ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::u_ub_ |
state_box_constr_array_t ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::x_lb_ |
state_box_constr_array_t ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::x_ub_ |
input_box_constr_sparsity_array_t ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::u_I_ |
container for the box constraint sparsity pattern An example for how an element of this array might look like: [0 1 4 7] This would mean that box constraints act on elements 0, 1, 4 and 7 of the state or input vector
Referenced by ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::changeNumStages(), and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setInputBoxConstraint().
state_box_constr_sparsity_array_t ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::x_I_ |
std::vector<int> ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::nbu_ |
the number of input box constraints at every stage.
Referenced by ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::changeNumStages(), ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::isInputBoxConstrained(), ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setInputBoxConstraint(), and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setZero().
std::vector<int> ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::nbx_ |
the number of state box constraints at every stage.
Referenced by ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::changeNumStages(), ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::isStateBoxConstrained(), ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setIntermediateStateBoxConstraint(), ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setTerminalBoxConstraints(), and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setZero().
constr_vec_array_t ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::d_lb_ |
general constraint lower bound
Referenced by ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::changeNumStages(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::computeLinearizedConstraints(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::initializeCostToGo(), ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setGeneralConstraints(), and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setZero().
constr_vec_array_t ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::d_ub_ |
general constraint upper bound
Referenced by ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::changeNumStages(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::computeLinearizedConstraints(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::initializeCostToGo(), ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setGeneralConstraints(), and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setZero().
constr_state_jac_array_t ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::C_ |
linear general constraint matrices
Referenced by ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::changeNumStages(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::computeLinearizedConstraints(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::initializeCostToGo(), ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setGeneralConstraints(), and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setZero().
constr_control_jac_array_t ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::D_ |
Referenced by ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::changeNumStages(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::computeLinearizedConstraints(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::initializeCostToGo(), ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setGeneralConstraints(), and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setZero().
std::vector<int> ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::ng_ |
number of general inequality constraints
Referenced by ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::changeNumStages(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::computeLinearizedConstraints(), ct::optcon::NLOCBackendBase< STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS >::initializeCostToGo(), ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::isGeneralConstrained(), ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setGeneralConstraints(), and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setZero().