- 3.0.2 optimal control module.
ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR > Class Template Reference

Defines a Linear-Quadratic Optimal Control Problem, which is optionally constrained. More...

#include <LQOCProblem.hpp>

Public Types

using constr_vec_t = Eigen::Matrix< SCALAR, -1, -1 >
 
using constr_state_jac_t = Eigen::Matrix< SCALAR, -1, -1 >
 
using constr_control_jac_t = Eigen::Matrix< SCALAR, -1, -1 >
 
using constr_vec_array_t = ct::core::DiscreteArray< constr_vec_t >
 
using constr_state_jac_array_t = ct::core::DiscreteArray< constr_state_jac_t >
 
using constr_control_jac_array_t = ct::core::DiscreteArray< constr_control_jac_t >
 
using input_box_constr_vec_t = Eigen::Matrix< SCALAR, CONTROL_DIM, 1 >
 
using state_box_constr_vec_t = Eigen::Matrix< SCALAR, STATE_DIM, 1 >
 
using input_box_constr_array_t = ct::core::DiscreteArray< input_box_constr_vec_t >
 
using state_box_constr_array_t = ct::core::DiscreteArray< state_box_constr_vec_t >
 
using state_box_constr_sparsity_t = Eigen::Matrix< int, STATE_DIM, 1 >
 a vector indicating which box constraints are active and which not More...
 
using input_box_constr_sparsity_t = Eigen::Matrix< int, CONTROL_DIM, 1 >
 
using input_box_constr_sparsity_array_t = ct::core::DiscreteArray< input_box_constr_sparsity_t >
 
using state_box_constr_sparsity_array_t = ct::core::DiscreteArray< state_box_constr_sparsity_t >
 
using VectorXi = Eigen::VectorXi
 

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, SCALARA_
 affine, time-varying system dynamics in discrete time More...
 
ct::core::StateControlMatrixArray< STATE_DIM, CONTROL_DIM, SCALARB_
 
ct::core::StateVectorArray< STATE_DIM, SCALARb_
 
ct::core::ScalarArray< SCALARq_
 constant term of in the LQ approximation of the cost function More...
 
ct::core::StateVectorArray< STATE_DIM, SCALARqv_
 LQ approximation of the pure state penalty, including terminal state penalty. More...
 
ct::core::StateMatrixArray< STATE_DIM, SCALARQ_
 
ct::core::ControlVectorArray< CONTROL_DIM, SCALARrv_
 LQ approximation of the pure control penalty. More...
 
ct::core::ControlMatrixArray< CONTROL_DIM, SCALARR_
 
ct::core::FeedbackArray< STATE_DIM, CONTROL_DIM, SCALARP_
 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...
 

Detailed Description

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR = double>
class ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >

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

  • affine system dynamics
  • LQ approximation of the cost function

The unconstrained LQ problem has the following form:

\[ \min_{\delta \mathbf{u}_n, \delta \mathbf{x}_n} \bigg \{ q_N + \delta \mathbf{x}_N^\top \mathbf{q}_N +\frac{1}{2} \delta \mathbf{x}_N^\top \mathbf{Q}_N \delta \mathbf{x}_N +\sum_{n=0}^{N-1} q_n + \delta \mathbf{x}_n^\top \mathbf{q}_n + \delta \mathbf{u}_n^\top \mathbf{r}_n + \frac{1}{2} \delta \mathbf{x}_n^\top\mathbf{Q}_n \delta \mathbf{x}_n +\frac{1}{2} \delta \mathbf{u}_n^\top \mathbf{R}_n \delta \mathbf{u}_n + \delta \mathbf{u}_n^\top \mathbf{P}_n \delta \mathbf{x}_n \bigg \} \]

subject to

\[ \mathbf \delta x_{n+1} = \mathbf A_n \delta \mathbf x_n + \mathbf B_n \delta \mathbf u_n +\mathbf b_n \]

The constrained LQ problem additionally implements the box constraints $ \mathbf x_{lb} \leq \mathbf \delta x_n \leq \mathbf x_{ub} \ \forall i=1,2,\ldots,N $ and $ \mathbf u_{lb} \leq \delta \mathbf u_n \leq \mathbf u_{ub} \ \forall i=0,1,\ldots,N-1$ and the general inequality constraints

\[ \mathbf d_{lb} \leq \mathbf C_n \delta \mathbf x_n + \mathbf D_n \delta \mathbf u_n \leq \mathbf d_{ub} \ \forall i=0,1,\ldots,N \]

which are both always kept in relative coordinates.

Note
The box constraint containers within this class are made fixed-size. Solvers can get the actual number of box constraints from a a dedicated container nbu_ and nbx_
In the differential notation we define $ \delta \mathbf x_n = \mathbf x_n - \hat \mathbf x_n $ and $ \delta \mathbf u_n = \mathbf u_n - \hat \mathbf u_n $ where x_n and u_n are current nominal/reference trajectories, around which the LQP is formed.
this problem does not contain any state or control trajectory! The fundamental assumption is that $ \delta \mathbf x_0 = 0 $
Todo:
Refactor the initializing methods such that const-references can be handed over.

Member Typedef Documentation

◆ constr_vec_t

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR = double>
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::constr_vec_t = Eigen::Matrix<SCALAR, -1, -1>

◆ constr_state_jac_t

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR = double>
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::constr_state_jac_t = Eigen::Matrix<SCALAR, -1, -1>

◆ constr_control_jac_t

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR = double>
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::constr_control_jac_t = Eigen::Matrix<SCALAR, -1, -1>

◆ constr_vec_array_t

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR = double>
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::constr_vec_array_t = ct::core::DiscreteArray<constr_vec_t>

◆ constr_state_jac_array_t

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR = double>
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::constr_state_jac_array_t = ct::core::DiscreteArray<constr_state_jac_t>

◆ constr_control_jac_array_t

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR = double>
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::constr_control_jac_array_t = ct::core::DiscreteArray<constr_control_jac_t>

◆ input_box_constr_vec_t

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR = double>
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::input_box_constr_vec_t = Eigen::Matrix<SCALAR, CONTROL_DIM, 1>

◆ state_box_constr_vec_t

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR = double>
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::state_box_constr_vec_t = Eigen::Matrix<SCALAR, STATE_DIM, 1>

◆ input_box_constr_array_t

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR = double>
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::input_box_constr_array_t = ct::core::DiscreteArray<input_box_constr_vec_t>

◆ state_box_constr_array_t

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR = double>
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::state_box_constr_array_t = ct::core::DiscreteArray<state_box_constr_vec_t>

◆ state_box_constr_sparsity_t

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR = double>
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

◆ input_box_constr_sparsity_t

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR = double>
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::input_box_constr_sparsity_t = Eigen::Matrix<int, CONTROL_DIM, 1>

◆ input_box_constr_sparsity_array_t

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR = double>
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::input_box_constr_sparsity_array_t = ct::core::DiscreteArray<input_box_constr_sparsity_t>

◆ state_box_constr_sparsity_array_t

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR = double>
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::state_box_constr_sparsity_array_t = ct::core::DiscreteArray<state_box_constr_sparsity_t>

◆ VectorXi

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR = double>
using ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::VectorXi = Eigen::VectorXi

Constructor & Destructor Documentation

◆ LQOCProblem()

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR >
ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::LQOCProblem ( int  N = 0)

Member Function Documentation

◆ getNumberOfStages()

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR >
int ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::getNumberOfStages ( )

returns the number of discrete time steps in the LQOCP, including terminal stage

◆ changeNumStages()

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR >
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().

◆ setZero()

◆ setInputBoxConstraint()

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR >
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

Parameters
indexthe index
nConstrthe number of constraints
u_lbcontrol lower bound in absolute coordinates
u_ubcontrol upper bound in absolute coordinates
spthe 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().

◆ setInputBoxConstraints()

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR >
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

Parameters
nConstrthe number of constraints
u_lbcontrol lower bound in absolute coordinates
u_ubcontrol upper bound in absolute coordinates
spthe sparsity vector, with strictly increasing indices, e.g. [0 1 4 7]
u_nom_abscurrent nominal control trajectory in absolute coordinates, required for coordinate transform

References i, and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setInputBoxConstraint().

◆ setIntermediateStateBoxConstraint()

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR >
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

Parameters
indexthe index
nConstrthe number of constraints
x_lbstate lower bound in absolute coordinates
x_ubstate upper bound in absolute coordinates
spthe sparsity vector, with strictly increasing indices, e.g. [0 1 4 7]
x_nom_abscurrent 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().

◆ setIntermediateStateBoxConstraints()

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR >
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

Parameters
nConstrthe number of constraints
x_lbcontrol lower bound in absolute coordinates
x_ubcontrol upper bound in absolute coordinates
spthe sparsity vector, with strictly increasing indices, e.g. [0 1 4 7]
x_nom_abscurrent nominal state trajectory in absolute coordinates, required for coordinate transform

References i, and ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::setIntermediateStateBoxConstraint().

◆ setTerminalBoxConstraints()

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR >
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

Parameters
nConstrthe number of constraints
x_lbstate lower bound in absolute coordinates
x_ubstate upper bound in absolute coordinates
spthe sparsity vector, with strictly increasing indices, e.g. [0 1 4 7]
x_nom_abscurrent 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_.

◆ setGeneralConstraints()

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR >
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

Parameters
d_lbgeneral constraint lower bound
d_ubgeneral constraint upper bound
Cgeneral constraint state jacobian
Dgeneral 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().

◆ setFromTimeInvariantLinearQuadraticProblem()

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR >
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

Parameters
linearSystemthe discrete-time LTI system
costFunctionthe continuous-time cost function
stateOffsetthe offset for the affine system dynamics demanded by the LQOC Solver
dtthe 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.

◆ isConstrained()

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR >
bool ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::isConstrained ( ) const

◆ isInputBoxConstrained()

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR >
bool ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::isInputBoxConstrained ( ) const

◆ isStateBoxConstrained()

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR >
bool ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::isStateBoxConstrained ( ) const

◆ isGeneralConstrained()

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR >
bool ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::isGeneralConstrained ( ) const

Member Data Documentation

◆ A_

◆ B_

◆ b_

◆ q_

◆ qv_

◆ Q_

◆ rv_

◆ R_

◆ P_

◆ u_lb_

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR = double>
input_box_constr_array_t ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::u_lb_

◆ u_ub_

◆ x_lb_

◆ x_ub_

◆ u_I_

template<int STATE_DIM, int CONTROL_DIM, typename SCALAR = double>
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().

◆ x_I_

◆ nbu_

◆ nbx_

◆ d_lb_

◆ d_ub_

◆ C_

◆ D_

◆ ng_


The documentation for this class was generated from the following files: