Predictive controller. More...
#include <predictive_controller.h>
Public Types | |
using | Ptr = std::shared_ptr< PredictiveController > |
![]() | |
using | ControlVector = Eigen::VectorXd |
using | Ptr = std::shared_ptr< ControllerInterface > |
using | StateVector = Eigen::VectorXd |
using | UPtr = std::unique_ptr< ControllerInterface > |
Public Member Functions | |
bool | getAutoUpdatePreviousControl () const |
void | getAvailableSignals (SignalTargetInterface &signal_target, const std::string &ns="") const override |
Retrieve available signals from the controller. More... | |
double | getControlDuration () const override |
Return the duration for which the control u obtained from step() is valid (useful for asynchronous control) More... | |
int | getControlInputDimension () const override |
Return the control input dimension. More... | |
ControllerInterface::Ptr | getInstance () const override |
Return a newly created shared instance of the implemented class. More... | |
const int & | getNumOcpIterations () const |
OptimalControlProblemInterface::Ptr | getOptimalControlProblem () |
int | getStateDimension () const override |
Return the dimension of the required plant state/output. More... | |
ControllerStatistics::Ptr | getStatistics () const override |
bool | hasPiecewiseConstantControls () const override |
Return true if the controller returns piecewise constant control pieces. More... | |
bool | initialize (const StateVector &x, ReferenceTrajectoryInterface &expected_xref, ReferenceTrajectoryInterface &expected_uref, const Duration &expected_dt, const Time &t, ReferenceTrajectoryInterface *sref=nullptr) override |
Initialize the controller. More... | |
const bool & | isPublishPrediction () const |
PredictiveController () | |
bool | providesFutureControls () const override |
bool | providesFutureStates () const override |
void | reset () override |
Reset internal controller state and caches. More... | |
void | sendSignals (double t, SignalTargetInterface &signal_target, const std::string &ns="") const override |
void | setAutoUpdatePreviousControl (bool enable) |
void | setNumOcpIterations (int ocp_iter) |
void | setOptimalControlProblem (OptimalControlProblemInterface::Ptr ocp) |
void | setOutputControlSequenceLenght (bool activate) |
void | setOutputStateSequenceLenght (bool activate) |
void | setPublishPrediction (bool publish) |
bool | step (const StateVector &x, ReferenceTrajectoryInterface &xref, ReferenceTrajectoryInterface &uref, const Duration &dt, const Time &t, TimeSeries::Ptr u_sequence, TimeSeries::Ptr x_sequence, SignalTargetInterface *signal_target=nullptr, ReferenceTrajectoryInterface *sref=nullptr, ReferenceTrajectoryInterface *xinit=nullptr, ReferenceTrajectoryInterface *uinit=nullptr, const std::string &ns="") override |
bool | supportsAsynchronousControl () const override |
Specify whether the controllers step function is independent of dt and getControlDuration() returns a valid value. More... | |
![]() | |
virtual bool | step (const StateVector &x, ReferenceTrajectoryInterface &xref, ReferenceTrajectoryInterface &uref, const Duration &dt, const Time &t, ControlVector &u, SignalTargetInterface *signal_target=nullptr, const std::string &ns="") |
Perform actual controller step / control law computation. More... | |
virtual | ~ControllerInterface () |
Virtual destructor. More... | |
Static Public Member Functions | |
static Ptr | getInstanceStatic () |
![]() | |
static Factory< ControllerInterface > & | getFactory () |
Get access to the associated factory. More... | |
Protected Attributes | |
bool | _auto_update_prev_control = true |
bool | _initialized = false |
int | _num_ocp_iterations = 1 |
OptimalControlProblemInterface::Ptr | _ocp |
bool | _output_control_sequence = false |
bool | _output_state_sequence = false |
bool | _publish_prediction = true |
ControllerStatistics | _statistics |
TimeSeries::Ptr | _u_ts |
TimeSeries::Ptr | _x_ts |
Predictive controller.
Implementation of a predictive controller that accepts a generic optimal control problem (OCP) that is solved within each step command.
The OCP is invoked repeatedly up to a user-specified number of iterations: refer to numOcpIterations().
Definition at line 50 of file predictive_controller.h.
using corbo::PredictiveController::Ptr = std::shared_ptr<PredictiveController> |
Definition at line 53 of file predictive_controller.h.
corbo::PredictiveController::PredictiveController | ( | ) |
Definition at line 32 of file predictive_controller.cpp.
|
inline |
Definition at line 107 of file predictive_controller.h.
|
overridevirtual |
Retrieve available signals from the controller.
Register a-priori known signals at the signal target. Registration is optional. Note, during step() execution further signals might occur without registration (in case the they are not known in advance or the implementation lacks a proper registration).
[in,out] | signal_target | Target for occuring signals [optional] |
Reimplemented from corbo::ControllerInterface.
Definition at line 81 of file predictive_controller.cpp.
|
inlineoverridevirtual |
Return the duration for which the control u obtained from step() is valid (useful for asynchronous control)
Reimplemented from corbo::ControllerInterface.
Definition at line 86 of file predictive_controller.h.
|
inlineoverridevirtual |
Return the control input dimension.
Implements corbo::ControllerInterface.
Definition at line 58 of file predictive_controller.h.
|
inlineoverridevirtual |
Return a newly created shared instance of the implemented class.
Implements corbo::ControllerInterface.
Definition at line 69 of file predictive_controller.h.
|
inlinestatic |
Definition at line 70 of file predictive_controller.h.
|
inline |
Definition at line 97 of file predictive_controller.h.
|
inline |
Definition at line 73 of file predictive_controller.h.
|
inlineoverridevirtual |
Return the dimension of the required plant state/output.
Depending on the controller type, the state dimension can also be just the plant output or the complete measured or observed state vector.
Implements corbo::ControllerInterface.
Definition at line 60 of file predictive_controller.h.
|
inlineoverridevirtual |
Reimplemented from corbo::ControllerInterface.
Definition at line 111 of file predictive_controller.h.
|
inlineoverridevirtual |
Return true if the controller returns piecewise constant control pieces.
Implements corbo::ControllerInterface.
Definition at line 62 of file predictive_controller.h.
|
overridevirtual |
Initialize the controller.
Initialization should be optional but it should facilitate memory allocation and trajectory initialization respectively warm starting.
[in] | x | Current plant state [getStateDimension() x 1] |
[in] | expected_xref | State reference (writable in order to allow to the reference object to precompute caches) |
[in] | expected_uref | Control reference (writable in order to allow to the reference object to precompute caches) |
[in] | expected_dt | Expected sampling interval length (controller rate) |
[in] | t | Current time stamp (can be sim-time or system-time, but compatible to state and control references) |
Reimplemented from corbo::ControllerInterface.
Definition at line 34 of file predictive_controller.cpp.
|
inline |
Definition at line 100 of file predictive_controller.h.
|
inlineoverridevirtual |
Implements corbo::ControllerInterface.
Definition at line 64 of file predictive_controller.h.
|
inlineoverridevirtual |
Implements corbo::ControllerInterface.
Definition at line 66 of file predictive_controller.h.
|
overridevirtual |
Reset internal controller state and caches.
Implements corbo::ControllerInterface.
Definition at line 94 of file predictive_controller.cpp.
|
overridevirtual |
Reimplemented from corbo::ControllerInterface.
Definition at line 99 of file predictive_controller.cpp.
|
inline |
Definition at line 106 of file predictive_controller.h.
|
inline |
Definition at line 98 of file predictive_controller.h.
|
inline |
Definition at line 72 of file predictive_controller.h.
|
inline |
Definition at line 103 of file predictive_controller.h.
|
inline |
Definition at line 104 of file predictive_controller.h.
|
inline |
Definition at line 101 of file predictive_controller.h.
|
overridevirtual |
Implements corbo::ControllerInterface.
Definition at line 46 of file predictive_controller.cpp.
|
inlineoverridevirtual |
Specify whether the controllers step function is independent of dt and getControlDuration() returns a valid value.
Reimplemented from corbo::ControllerInterface.
Definition at line 89 of file predictive_controller.h.
|
protected |
Definition at line 136 of file predictive_controller.h.
|
protected |
Definition at line 144 of file predictive_controller.h.
|
protected |
Definition at line 138 of file predictive_controller.h.
|
protected |
Definition at line 130 of file predictive_controller.h.
|
protected |
Definition at line 141 of file predictive_controller.h.
|
protected |
Definition at line 142 of file predictive_controller.h.
|
protected |
Definition at line 139 of file predictive_controller.h.
|
protected |
Definition at line 134 of file predictive_controller.h.
|
protected |
Definition at line 132 of file predictive_controller.h.
|
protected |
Definition at line 131 of file predictive_controller.h.