State feedback controller wigh feedback gain matrix K. More...
#include <simple_state_controller.h>
Public Member Functions | |
void | getAvailableSignals (SignalTargetInterface &signal_target, const std::string &ns="") const override |
Retrieve available signals from the controller. More... | |
int | getControlInputDimension () const override |
Return the control input dimension. More... | |
Ptr | getInstance () const override |
Return a newly created shared instance of the implemented class. More... | |
int | getStateDimension () const override |
Return the dimension of the required plant state/output. More... | |
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... | |
bool | providesFutureControls () const override |
bool | providesFutureStates () const override |
void | reset () override |
Reset internal controller state and caches. More... | |
void | setFilterMatrixV (const Eigen::Ref< const Eigen::MatrixXd > &V) |
Set reference filter matrix V [control input dimension x output dimension]. More... | |
void | setGainMatrixK (const Eigen::Ref< const Eigen::MatrixXd > &K) |
Set feedback gain matrix K [control input dimension x state dimension]. More... | |
void | setPublishError (bool publish) |
Specify whether the state error should be published via signal. More... | |
SimpleStateController ()=default | |
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 |
![]() | |
virtual double | getControlDuration () const |
Return the duration for which the control u obtained from step() is valid (useful for asynchronous control) More... | |
virtual ControllerStatistics::Ptr | getStatistics () const |
virtual void | sendSignals (double t, SignalTargetInterface &signal_target, const std::string &ns="") const |
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 bool | supportsAsynchronousControl () const |
Specify whether the controllers step function is independent of dt and getControlDuration() returns a valid value. More... | |
virtual | ~ControllerInterface () |
Virtual destructor. More... | |
Private Attributes | |
Eigen::MatrixXd | _K |
bool | _publish_error = true |
Eigen::MatrixXd | _V |
Additional Inherited Members | |
![]() | |
using | ControlVector = Eigen::VectorXd |
using | Ptr = std::shared_ptr< ControllerInterface > |
using | StateVector = Eigen::VectorXd |
using | UPtr = std::unique_ptr< ControllerInterface > |
![]() | |
static Factory< ControllerInterface > & | getFactory () |
Get access to the associated factory. More... | |
State feedback controller wigh feedback gain matrix K.
The controller implements the following control law: . Herby the number of rows in K corresponds to state dimension and the number of columns to the control input dimension.
Definition at line 46 of file simple_state_controller.h.
|
default |
|
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 88 of file simple_state_controller.cpp.
|
inlineoverridevirtual |
Return the control input dimension.
Implements corbo::ControllerInterface.
Definition at line 52 of file simple_state_controller.h.
|
inlineoverridevirtual |
Return a newly created shared instance of the implemented class.
Implements corbo::ControllerInterface.
Definition at line 63 of file simple_state_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 54 of file simple_state_controller.h.
|
inlineoverridevirtual |
Return true if the controller returns piecewise constant control pieces.
Implements corbo::ControllerInterface.
Definition at line 56 of file simple_state_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 37 of file simple_state_controller.cpp.
|
inlineoverridevirtual |
Implements corbo::ControllerInterface.
Definition at line 58 of file simple_state_controller.h.
|
inlineoverridevirtual |
Implements corbo::ControllerInterface.
Definition at line 60 of file simple_state_controller.h.
|
overridevirtual |
Reset internal controller state and caches.
Implements corbo::ControllerInterface.
Definition at line 96 of file simple_state_controller.cpp.
void corbo::SimpleStateController::setFilterMatrixV | ( | const Eigen::Ref< const Eigen::MatrixXd > & | V | ) |
Set reference filter matrix V [control input dimension x output dimension].
Definition at line 35 of file simple_state_controller.cpp.
void corbo::SimpleStateController::setGainMatrixK | ( | const Eigen::Ref< const Eigen::MatrixXd > & | K | ) |
Set feedback gain matrix K [control input dimension x state dimension].
Definition at line 33 of file simple_state_controller.cpp.
|
inline |
Specify whether the state error should be published via signal.
Definition at line 95 of file simple_state_controller.h.
|
overridevirtual |
Implements corbo::ControllerInterface.
Definition at line 46 of file simple_state_controller.cpp.
|
private |
Definition at line 99 of file simple_state_controller.h.
|
private |
Definition at line 98 of file simple_state_controller.h.
|
private |
Definition at line 100 of file simple_state_controller.h.