- 3.0.2 optimal control module.
|
A simple quadratic cost function. More...
#include <CostFunctionQuadraticSimple.hpp>
Public Types | |
typedef Eigen::Matrix< SCALAR, CONTROL_DIM, CONTROL_DIM > | control_matrix_t |
typedef Eigen::Matrix< SCALAR, CONTROL_DIM, STATE_DIM > | control_state_matrix_t |
typedef core::StateVector< STATE_DIM, SCALAR > | state_vector_t |
typedef core::ControlVector< CONTROL_DIM, SCALAR > | control_vector_t |
Public Types inherited from ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR > | |
typedef Eigen::Matrix< SCALAR, CONTROL_DIM, CONTROL_DIM > | control_matrix_t |
typedef Eigen::Matrix< SCALAR, CONTROL_DIM, STATE_DIM > | control_state_matrix_t |
typedef core::StateVector< STATE_DIM, SCALAR > | state_vector_t |
typedef core::ControlVector< CONTROL_DIM, SCALAR > | control_vector_t |
typedef CostFunction< STATE_DIM, CONTROL_DIM, SCALAR > | BASE |
Public Types inherited from ct::optcon::CostFunction< STATE_DIM, CONTROL_DIM, SCALAR > | |
typedef core::ControlVector< CONTROL_DIM, SCALAR > | control_vector_t |
Public Member Functions | |
CostFunctionQuadraticSimple () | |
CostFunctionQuadraticSimple (const state_matrix_t &Q, const control_matrix_t &R, const state_vector_t &x_nominal, const control_vector_t &u_nominal, const state_vector_t &x_final, const state_matrix_t &Q_final) | |
virtual | ~CostFunctionQuadraticSimple () |
CostFunctionQuadraticSimple (const CostFunctionQuadraticSimple &arg) | |
CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR > * | clone () const override |
virtual void | setCurrentStateAndControl (const state_vector_t &x, const control_vector_t &u, const SCALAR &t) override |
virtual SCALAR | evaluateIntermediate () override |
evaluate intermediate costs More... | |
virtual state_vector_t | stateDerivativeIntermediate () override |
Computes intermediate-cost first-order derivative with respect to state. More... | |
virtual state_matrix_t | stateSecondDerivativeIntermediate () override |
Computes intermediate-cost second-order derivative with respect to state. More... | |
virtual control_vector_t | controlDerivativeIntermediate () override |
Computes intermediate-cost first-order derivative with respect to control. More... | |
virtual control_matrix_t | controlSecondDerivativeIntermediate () override |
Computes intermediate-cost second-order derivative with respect to input. More... | |
virtual control_state_matrix_t | stateControlDerivativeIntermediate () override |
Computes intermediate-cost derivative with respect to state and control. More... | |
virtual SCALAR | evaluateTerminal () override |
evaluate terminal costs More... | |
virtual state_vector_t | stateDerivativeTerminal () override |
virtual state_matrix_t | stateSecondDerivativeTerminal () override |
Computes final-cost second-order derivative with respect to state. More... | |
virtual void | updateReferenceState (const state_vector_t &x_ref) override |
update the reference state for intermediate cost terms More... | |
virtual void | updateFinalState (const state_vector_t &x_final) override |
update the reference state for final cost terms More... | |
Public Member Functions inherited from ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR > | |
CostFunctionQuadratic () | |
CostFunctionQuadratic (const CostFunctionQuadratic &arg) | |
virtual | ~CostFunctionQuadratic () |
virtual size_t | addIntermediateTerm (std::shared_ptr< TermBase< STATE_DIM, CONTROL_DIM, SCALAR >> term, bool verbose=false) |
Adds an intermediate term. More... | |
virtual size_t | addFinalTerm (std::shared_ptr< TermBase< STATE_DIM, CONTROL_DIM, SCALAR >> term, bool verbose=false) |
Adds a final term. More... | |
virtual void | loadFromConfigFile (const std::string &filename, bool verbose=false) |
Loads cost function from config file. More... | |
virtual control_vector_t | controlDerivativeTerminal () |
Computes terminal-cost first-order derivative with respect to control. More... | |
virtual control_matrix_t | controlSecondDerivativeTerminal () |
Computes final-cost second-order derivative with respect to input. More... | |
virtual control_state_matrix_t | stateControlDerivativeTerminal () |
Computes final-cost derivative with respect to state and control. More... | |
virtual void | updateReferenceControl (const control_vector_t &u_ref) |
update the reference control for intermediate cost terms More... | |
bool | stateDerivativeIntermediateTest (bool verbose=false) |
compare the state derivative against numerical differentiation More... | |
bool | controlDerivativeIntermediateTest (bool verbose=false) |
compare the control derivative against numerical differentiation More... | |
std::shared_ptr< TermBase< STATE_DIM, CONTROL_DIM, SCALAR > > | getIntermediateTermById (const size_t id) |
std::shared_ptr< TermBase< STATE_DIM, CONTROL_DIM, SCALAR > > | getFinalTermById (const size_t id) |
std::shared_ptr< TermBase< STATE_DIM, CONTROL_DIM, SCALAR > > | getIntermediateTermByName (const std::string &name) |
std::shared_ptr< TermBase< STATE_DIM, CONTROL_DIM, SCALAR > > | getFinalTermByName (const std::string &name) |
virtual void | initialize () |
initialize the cost function (e.g. to be used in CostFunctionAD) More... | |
Public Member Functions inherited from ct::optcon::CostFunction< STATE_DIM, CONTROL_DIM, SCALAR > | |
CostFunction () | |
Default constructor. More... | |
virtual | ~CostFunction () |
Destructor. More... | |
CostFunction (const CostFunction &arg) | |
Copy constructor. More... | |
virtual void | setCurrentStateAndControl (const state_vector_t &x, const control_vector_t &u, const SCALAR &t=SCALAR(0.0)) |
virtual void | getCurrentStateAndControl (Eigen::Matrix< SCALAR, STATE_DIM, 1 > &x, Eigen::Matrix< SCALAR, CONTROL_DIM, 1 > &u, SCALAR &t) const |
sets current state, control and time More... | |
virtual void | shiftTime (const SCALAR t) |
Public Attributes | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef Eigen::Matrix< SCALAR, STATE_DIM, STATE_DIM > | state_matrix_t |
Public Attributes inherited from ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR > | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef Eigen::Matrix< SCALAR, STATE_DIM, STATE_DIM > | state_matrix_t |
Public Attributes inherited from ct::optcon::CostFunction< STATE_DIM, CONTROL_DIM, SCALAR > | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef core::StateVector< STATE_DIM, SCALAR > | state_vector_t |
Protected Attributes | |
state_vector_t | x_deviation_ |
state_vector_t | x_nominal_ |
state_matrix_t | Q_ |
control_vector_t | u_deviation_ |
control_vector_t | u_nominal_ |
control_matrix_t | R_ |
state_vector_t | x_final_ |
state_matrix_t | Q_final_ |
Protected Attributes inherited from ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR > | |
SCALAR | eps_ |
stepsize for numerical differentiation More... | |
bool | doubleSidedDerivative_ = true |
use double sided derivatives in numerical differentiation More... | |
std::vector< std::shared_ptr< TermBase< STATE_DIM, CONTROL_DIM, SCALAR > > > | intermediateCostAnalytical_ |
std::vector< std::shared_ptr< TermBase< STATE_DIM, CONTROL_DIM, SCALAR > > > | finalCostAnalytical_ |
Protected Attributes inherited from ct::optcon::CostFunction< STATE_DIM, CONTROL_DIM, SCALAR > | |
state_vector_t | x_ |
control_vector_t | u_ |
SCALAR | t_ |
SCALAR | t_shift_ |
Additional Inherited Members | |
Protected Member Functions inherited from ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR > | |
SCALAR | evaluateIntermediateBase () |
evaluate intermediate analytical cost terms More... | |
SCALAR | evaluateTerminalBase () |
evaluate terminal analytical cost terms More... | |
state_vector_t | stateDerivativeIntermediateBase () |
evaluate intermediate analytical state derivatives More... | |
state_vector_t | stateDerivativeTerminalBase () |
evaluate terminal analytical state derivatives More... | |
state_matrix_t | stateSecondDerivativeIntermediateBase () |
evaluate intermediate analytical state second derivatives More... | |
state_matrix_t | stateSecondDerivativeTerminalBase () |
evaluate terminal analytical state second derivatives More... | |
control_vector_t | controlDerivativeIntermediateBase () |
evaluate intermediate analytical control derivatives More... | |
control_vector_t | controlDerivativeTerminalBase () |
evaluate terminal analytical control derivatives More... | |
control_matrix_t | controlSecondDerivativeIntermediateBase () |
evaluate intermediate analytical control second derivatives More... | |
control_matrix_t | controlSecondDerivativeTerminalBase () |
evaluate terminal analytical control second derivatives More... | |
control_state_matrix_t | stateControlDerivativeIntermediateBase () |
evaluate intermediate analytical control mixed state control derivatives More... | |
control_state_matrix_t | stateControlDerivativeTerminalBase () |
evaluate terminal analytical control mixed state control derivatives More... | |
state_vector_t | stateDerivativeIntermediateNumDiff () |
compute the state derivative by numerical differentiation (can be used for testing) More... | |
control_vector_t | controlDerivativeIntermediateNumDiff () |
compute the control derivative by numerical differentiation (can be used for testing) More... | |
A simple quadratic cost function.
A simple, purely-quadratic cost function of the form where indicate deviations from a nominal (desired) state and control
typedef Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM> ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::control_matrix_t |
typedef Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM> ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::control_state_matrix_t |
typedef core::StateVector<STATE_DIM, SCALAR> ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::state_vector_t |
typedef core::ControlVector<CONTROL_DIM, SCALAR> ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::control_vector_t |
ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::CostFunctionQuadraticSimple | ( | ) |
Constructs a simple, purely quadratic cost function with all zero elements.
ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::CostFunctionQuadraticSimple | ( | const state_matrix_t & | Q, |
const control_matrix_t & | R, | ||
const state_vector_t & | x_nominal, | ||
const control_vector_t & | u_nominal, | ||
const state_vector_t & | x_final, | ||
const state_matrix_t & | Q_final | ||
) |
Constructs a simple, purely quadratic cost function
Q | intermediate state cost weighting |
R | intermediate control cost weighting |
x_nominal | nominal (desired) state |
u_nominal | nominal (desired) control |
x_final | nominal (desired) final state |
Q_final | final state cost weighting |
References ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::u_deviation_, and ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::x_deviation_.
|
virtual |
ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::CostFunctionQuadraticSimple | ( | const CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR > & | arg | ) |
|
overridevirtual |
Clones the cost function.
Implements ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR >.
|
overridevirtual |
References t, ct::optcon::CostFunction< STATE_DIM, CONTROL_DIM, SCALAR >::t_, u, ct::optcon::CostFunction< STATE_DIM, CONTROL_DIM, SCALAR >::u_, ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::u_deviation_, ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::u_nominal_, x, ct::optcon::CostFunction< STATE_DIM, CONTROL_DIM, SCALAR >::x_, ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::x_deviation_, and ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::x_nominal_.
|
overridevirtual |
evaluate intermediate costs
Evaluates the running/intermediate cost function for the control, state and time set in setCurrentStateAndControl()
Implements ct::optcon::CostFunction< STATE_DIM, CONTROL_DIM, SCALAR >.
References ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::Q_, ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::R_, ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::u_deviation_, and ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::x_deviation_.
|
overridevirtual |
Computes intermediate-cost first-order derivative with respect to state.
Implements ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR >.
References ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::Q_, and ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::x_deviation_.
|
overridevirtual |
Computes intermediate-cost second-order derivative with respect to state.
Implements ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR >.
References ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::Q_.
|
overridevirtual |
Computes intermediate-cost first-order derivative with respect to control.
Implements ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR >.
References ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::R_, and ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::u_deviation_.
|
overridevirtual |
Computes intermediate-cost second-order derivative with respect to input.
Implements ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR >.
References ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::R_.
|
overridevirtual |
Computes intermediate-cost derivative with respect to state and control.
Implements ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR >.
|
overridevirtual |
evaluate terminal costs
Evaluates the terminal cost for a given state and control set in setCurrentStateAndControl(). This usually ignores time.
Implements ct::optcon::CostFunction< STATE_DIM, CONTROL_DIM, SCALAR >.
References ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::Q_final_, ct::optcon::CostFunction< STATE_DIM, CONTROL_DIM, SCALAR >::x_, and ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::x_final_.
|
overridevirtual |
Computes terminal-cost first-order derivative with respect to state
Implements ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR >.
References ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::Q_final_, ct::optcon::CostFunction< STATE_DIM, CONTROL_DIM, SCALAR >::x_, and ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::x_final_.
|
overridevirtual |
Computes final-cost second-order derivative with respect to state.
Implements ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR >.
References ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::Q_final_.
|
overridevirtual |
update the reference state for intermediate cost terms
Reimplemented from ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR >.
References ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::x_nominal_.
|
overridevirtual |
update the reference state for final cost terms
Reimplemented from ct::optcon::CostFunctionQuadratic< STATE_DIM, CONTROL_DIM, SCALAR >.
References ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::x_final_.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM> ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::state_matrix_t |
|
protected |
Referenced by ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::CostFunctionQuadraticSimple(), ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::evaluateIntermediate(), ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::setCurrentStateAndControl(), and ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::stateDerivativeIntermediate().
|
protected |
|
protected |
Referenced by ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::evaluateIntermediate(), ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::stateDerivativeIntermediate(), and ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::stateSecondDerivativeIntermediate().
|
protected |
Referenced by ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::controlDerivativeIntermediate(), ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::CostFunctionQuadraticSimple(), ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::evaluateIntermediate(), and ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::setCurrentStateAndControl().
|
protected |
|
protected |
Referenced by ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::controlDerivativeIntermediate(), ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::controlSecondDerivativeIntermediate(), and ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::evaluateIntermediate().
|
protected |
Referenced by ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::evaluateTerminal(), ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::stateDerivativeTerminal(), and ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::updateFinalState().
|
protected |
Referenced by ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::evaluateTerminal(), ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::stateDerivativeTerminal(), and ct::optcon::CostFunctionQuadraticSimple< STATE_DIM, CONTROL_DIM, SCALAR >::stateSecondDerivativeTerminal().