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

#include <GNRiccatiSolver.hpp>

Inheritance diagram for ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >:
ct::optcon::LQOCSolver< STATE_DIM, CONTROL_DIM, SCALAR >

Public Types

typedef LQOCProblem< STATE_DIM, CONTROL_DIM, SCALARLQOCProblem_t
 
typedef ct::core::StateMatrix< STATE_DIM, SCALARStateMatrix
 
typedef ct::core::StateMatrixArray< STATE_DIM, SCALARStateMatrixArray
 
typedef ct::core::ControlVector< CONTROL_DIM, SCALARControlVector
 
typedef ct::core::ControlMatrix< CONTROL_DIM, SCALARControlMatrix
 
typedef ct::core::ControlMatrixArray< CONTROL_DIM, SCALARControlMatrixArray
 
typedef ct::core::StateControlMatrixArray< STATE_DIM, CONTROL_DIM, SCALARStateControlMatrixArray
 
typedef ct::core::FeedbackArray< STATE_DIM, CONTROL_DIM, SCALARFeedbackArray
 
typedef ct::core::StateVectorArray< STATE_DIM, SCALARStateVectorArray
 
typedef ct::core::ControlVectorArray< CONTROL_DIM, SCALARControlVectorArray
 

Public Member Functions

 GNRiccatiSolver (const std::shared_ptr< LQOCProblem_t > &lqocProblem=nullptr)
 
 GNRiccatiSolver (int N)
 
virtual void solve () override
 solve the LQOC problem More...
 
virtual void initializeAndAllocate () override
 a method reserved for memory allocation (e.g. required for HPIPM) More...
 
virtual void solveSingleStage (int N) override
 
virtual void configure (const NLOptConSettings &settings) override
 
virtual void computeStatesAndControls () override
 extract the solution (can be overriden if additional extraction steps required in specific solver) More...
 
virtual void computeFeedbackMatrices () override
 return TVLQR feedback matrices More...
 
virtual void compute_lv () override
 compute iLQR-style lv More...
 
virtual SCALAR getSmallestEigenvalue () override
 return the smallest eigenvalue More...
 
- Public Member Functions inherited from ct::optcon::LQOCSolver< STATE_DIM, CONTROL_DIM, SCALAR >
 LQOCSolver (const std::shared_ptr< LQOCProblem_t > &lqocProblem=nullptr)
 
virtual ~LQOCSolver ()=default
 
void setProblem (std::shared_ptr< LQOCProblem_t > lqocProblem)
 
virtual bool configureInputBoxConstraints (std::shared_ptr< LQOCProblem< STATE_DIM, CONTROL_DIM >> lqocProblem)
 setup and configure the box constraints More...
 
virtual bool configureStateBoxConstraints (std::shared_ptr< LQOCProblem< STATE_DIM, CONTROL_DIM >> lqocProblem)
 
virtual bool configureGeneralConstraints (std::shared_ptr< LQOCProblem< STATE_DIM, CONTROL_DIM >> lqocProblem)
 setup and configure the general (in)equality constraints More...
 
const ct::core::StateVectorArray< STATE_DIM, SCALAR > & getSolutionState ()
 return solution for state More...
 
const ct::core::ControlVectorArray< CONTROL_DIM, SCALAR > & getSolutionControl ()
 return solution for control More...
 
const ct::core::FeedbackArray< STATE_DIM, CONTROL_DIM, SCALAR > & getSolutionFeedback ()
 
virtual const ct::core::ControlVectorArray< CONTROL_DIM, SCALAR > & get_lv ()
 return iLQR-style feedforward lv More...
 

Static Public Attributes

static EIGEN_MAKE_ALIGNED_OPERATOR_NEW const int state_dim = STATE_DIM
 
static const int control_dim = CONTROL_DIM
 

Protected Member Functions

virtual void setProblemImpl (std::shared_ptr< LQOCProblem_t > lqocProblem) override
 
void changeNumberOfStages (int N)
 
void initializeCostToGo ()
 
void computeCostToGo (size_t k)
 
void designController (size_t k)
 
void logToMatlab ()
 
- Protected Member Functions inherited from ct::optcon::LQOCSolver< STATE_DIM, CONTROL_DIM, SCALAR >
virtual void setProblemImpl (std::shared_ptr< LQOCProblem_t > lqocProblem)=0
 

Protected Attributes

NLOptConSettings settings_
 
ControlVectorArray gv_
 
FeedbackArray G_
 
ControlMatrixArray H_
 
ControlMatrixArray Hi_
 
ControlMatrixArray Hi_inverse_
 
ControlMatrix H_corrFix_
 
StateVectorArray sv_
 
StateMatrixArray S_
 
int N_
 
SCALAR smallestEigenvalue_
 
Eigen::SelfAdjointEigenSolver< Eigen::Matrix< SCALAR, CONTROL_DIM, CONTROL_DIM > > eigenvalueSolver_
 Eigenvalue solver, used for inverting the Hessian and for regularization. More...
 
- Protected Attributes inherited from ct::optcon::LQOCSolver< STATE_DIM, CONTROL_DIM, SCALAR >
std::shared_ptr< LQOCProblem_tlqocProblem_
 
core::StateVectorArray< STATE_DIM, SCALARx_sol_
 
core::ControlVectorArray< CONTROL_DIM, SCALARu_sol_
 
ct::core::FeedbackArray< STATE_DIM, CONTROL_DIM, SCALARL_
 
ct::core::ControlVectorArray< CONTROL_DIM, SCALARlv_
 

Additional Inherited Members

- Public Attributes inherited from ct::optcon::LQOCSolver< STATE_DIM, CONTROL_DIM, SCALAR >
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef LQOCProblem< STATE_DIM, CONTROL_DIM, SCALARLQOCProblem_t
 

Detailed Description

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
class ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >

This class implements an general Riccati backward pass for solving an unconstrained linear-quadratic Optimal Control problem

Member Typedef Documentation

◆ LQOCProblem_t

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
typedef LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR> ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::LQOCProblem_t

◆ StateMatrix

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
typedef ct::core::StateMatrix<STATE_DIM, SCALAR> ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::StateMatrix

◆ StateMatrixArray

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
typedef ct::core::StateMatrixArray<STATE_DIM, SCALAR> ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::StateMatrixArray

◆ ControlVector

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
typedef ct::core::ControlVector<CONTROL_DIM, SCALAR> ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::ControlVector

◆ ControlMatrix

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
typedef ct::core::ControlMatrix<CONTROL_DIM, SCALAR> ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::ControlMatrix

◆ ControlMatrixArray

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
typedef ct::core::ControlMatrixArray<CONTROL_DIM, SCALAR> ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::ControlMatrixArray

◆ StateControlMatrixArray

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
typedef ct::core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR> ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::StateControlMatrixArray

◆ FeedbackArray

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
typedef ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR> ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::FeedbackArray

◆ StateVectorArray

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
typedef ct::core::StateVectorArray<STATE_DIM, SCALAR> ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::StateVectorArray

◆ ControlVectorArray

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
typedef ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::ControlVectorArray

Constructor & Destructor Documentation

◆ GNRiccatiSolver() [1/2]

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR >
ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::GNRiccatiSolver ( const std::shared_ptr< LQOCProblem_t > &  lqocProblem = nullptr)

◆ GNRiccatiSolver() [2/2]

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR >
ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::GNRiccatiSolver ( int  N)

Member Function Documentation

◆ solve()

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR >
void ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::solve ( )
overridevirtual

◆ initializeAndAllocate()

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR >
void ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::initializeAndAllocate ( )
overridevirtual

a method reserved for memory allocation (e.g. required for HPIPM)

Implements ct::optcon::LQOCSolver< STATE_DIM, CONTROL_DIM, SCALAR >.

◆ solveSingleStage()

◆ configure()

◆ computeStatesAndControls()

◆ computeFeedbackMatrices()

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR >
void ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::computeFeedbackMatrices ( )
overridevirtual

return TVLQR feedback matrices

Implements ct::optcon::LQOCSolver< STATE_DIM, CONTROL_DIM, SCALAR >.

Referenced by TEST().

◆ compute_lv()

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR >
void ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::compute_lv ( )
overridevirtual

compute iLQR-style lv

Implements ct::optcon::LQOCSolver< STATE_DIM, CONTROL_DIM, SCALAR >.

Referenced by TEST().

◆ getSmallestEigenvalue()

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR >
SCALAR ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::getSmallestEigenvalue ( )
overridevirtual

◆ setProblemImpl()

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR >
void ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::setProblemImpl ( std::shared_ptr< LQOCProblem_t lqocProblem)
overrideprotectedvirtual

◆ changeNumberOfStages()

◆ initializeCostToGo()

◆ computeCostToGo()

◆ designController()

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR >
void ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::designController ( size_t  k)
protected

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::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::eigenvalueSolver_, ct::optcon::NLOptConSettings::epsilon, ct::optcon::NLOptConSettings::fixedHessianCorrection, ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::G_, ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::gv_, ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::H_, ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::Hi_, ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::Hi_inverse_, ct::optcon::LQOCSolver< STATE_DIM, CONTROL_DIM, SCALAR >::L_, ct::optcon::LQOCSolver< STATE_DIM, CONTROL_DIM, SCALAR >::lqocProblem_, ct::optcon::LQOCSolver< STATE_DIM, CONTROL_DIM, SCALAR >::lv_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::P_, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::R_, ct::optcon::NLOptConSettings::recordSmallestEigenvalue, ct::optcon::LQOCProblem< STATE_DIM, CONTROL_DIM, SCALAR >::rv_, ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::S_, ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::settings_, ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::smallestEigenvalue_, and ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::sv_.

Referenced by ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::solveSingleStage().

◆ logToMatlab()

Member Data Documentation

◆ state_dim

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
EIGEN_MAKE_ALIGNED_OPERATOR_NEW const int ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::state_dim = STATE_DIM
static

◆ control_dim

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
const int ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::control_dim = CONTROL_DIM
static

◆ settings_

◆ gv_

◆ G_

◆ H_

◆ Hi_

◆ Hi_inverse_

◆ H_corrFix_

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
ControlMatrix ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::H_corrFix_
protected

◆ sv_

◆ S_

◆ N_

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
int ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::N_
protected

◆ smallestEigenvalue_

◆ eigenvalueSolver_

template<size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
Eigen::SelfAdjointEigenSolver<Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> > ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::eigenvalueSolver_
protected

Eigenvalue solver, used for inverting the Hessian and for regularization.

Referenced by ct::optcon::GNRiccatiSolver< STATE_DIM, CONTROL_DIM, SCALAR >::designController().


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