Public Member Functions | Private Attributes | List of all members
corbo::SimpleStateController Class Reference

State feedback controller wigh feedback gain matrix K. More...

#include <simple_state_controller.h>

Inheritance diagram for corbo::SimpleStateController:
Inheritance graph
[legend]

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
 
- Public Member Functions inherited from corbo::ControllerInterface
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

- Public Types inherited from corbo::ControllerInterface
using ControlVector = Eigen::VectorXd
 
using Ptr = std::shared_ptr< ControllerInterface >
 
using StateVector = Eigen::VectorXd
 
using UPtr = std::unique_ptr< ControllerInterface >
 
- Static Public Member Functions inherited from corbo::ControllerInterface
static Factory< ControllerInterface > & getFactory ()
 Get access to the associated factory. More...
 

Detailed Description

State feedback controller wigh feedback gain matrix K.

The controller implements the following control law: $ u = - K x $. Herby the number of rows in K corresponds to state dimension and the number of columns to the control input dimension.

See also
ControllerInterface LqrController PidController PredictiveController
Author
Christoph Rösmann (chris.nosp@m.toph.nosp@m..roes.nosp@m.mann.nosp@m.@tu-d.nosp@m.ortm.nosp@m.und.d.nosp@m.e)

Definition at line 46 of file simple_state_controller.h.

Constructor & Destructor Documentation

◆ SimpleStateController()

corbo::SimpleStateController::SimpleStateController ( )
default

Member Function Documentation

◆ getAvailableSignals()

void corbo::SimpleStateController::getAvailableSignals ( SignalTargetInterface signal_target,
const std::string &  ns = "" 
) const
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).

Parameters
[in,out]signal_targetTarget for occuring signals [optional]

Reimplemented from corbo::ControllerInterface.

Definition at line 88 of file simple_state_controller.cpp.

◆ getControlInputDimension()

int corbo::SimpleStateController::getControlInputDimension ( ) const
inlineoverridevirtual

Return the control input dimension.

Implements corbo::ControllerInterface.

Definition at line 52 of file simple_state_controller.h.

◆ getInstance()

Ptr corbo::SimpleStateController::getInstance ( ) const
inlineoverridevirtual

Return a newly created shared instance of the implemented class.

Implements corbo::ControllerInterface.

Definition at line 63 of file simple_state_controller.h.

◆ getStateDimension()

int corbo::SimpleStateController::getStateDimension ( ) const
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.

Returns
the controllers state dimension

Implements corbo::ControllerInterface.

Definition at line 54 of file simple_state_controller.h.

◆ hasPiecewiseConstantControls()

bool corbo::SimpleStateController::hasPiecewiseConstantControls ( ) const
inlineoverridevirtual

Return true if the controller returns piecewise constant control pieces.

Returns
true if the controls are piecewise constant

Implements corbo::ControllerInterface.

Definition at line 56 of file simple_state_controller.h.

◆ initialize()

bool corbo::SimpleStateController::initialize ( const StateVector x,
ReferenceTrajectoryInterface expected_xref,
ReferenceTrajectoryInterface expected_uref,
const Duration expected_dt,
const Time t,
ReferenceTrajectoryInterface expected_sref = nullptr 
)
overridevirtual

Initialize the controller.

Initialization should be optional but it should facilitate memory allocation and trajectory initialization respectively warm starting.

Parameters
[in]xCurrent plant state [getStateDimension() x 1]
[in]expected_xrefState reference (writable in order to allow to the reference object to precompute caches)
[in]expected_urefControl reference (writable in order to allow to the reference object to precompute caches)
[in]expected_dtExpected sampling interval length (controller rate)
[in]tCurrent time stamp (can be sim-time or system-time, but compatible to state and control references)
Returns
true if initialization was successful, false otherwise.

Reimplemented from corbo::ControllerInterface.

Definition at line 37 of file simple_state_controller.cpp.

◆ providesFutureControls()

bool corbo::SimpleStateController::providesFutureControls ( ) const
inlineoverridevirtual

Implements corbo::ControllerInterface.

Definition at line 58 of file simple_state_controller.h.

◆ providesFutureStates()

bool corbo::SimpleStateController::providesFutureStates ( ) const
inlineoverridevirtual

Implements corbo::ControllerInterface.

Definition at line 60 of file simple_state_controller.h.

◆ reset()

void corbo::SimpleStateController::reset ( )
overridevirtual

Reset internal controller state and caches.

Implements corbo::ControllerInterface.

Definition at line 96 of file simple_state_controller.cpp.

◆ setFilterMatrixV()

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.

◆ setGainMatrixK()

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.

◆ setPublishError()

void corbo::SimpleStateController::setPublishError ( bool  publish)
inline

Specify whether the state error should be published via signal.

Definition at line 95 of file simple_state_controller.h.

◆ step()

bool corbo::SimpleStateController::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 = "" 
)
overridevirtual

Implements corbo::ControllerInterface.

Definition at line 46 of file simple_state_controller.cpp.

Member Data Documentation

◆ _K

Eigen::MatrixXd corbo::SimpleStateController::_K
private

Definition at line 99 of file simple_state_controller.h.

◆ _publish_error

bool corbo::SimpleStateController::_publish_error = true
private

Definition at line 98 of file simple_state_controller.h.

◆ _V

Eigen::MatrixXd corbo::SimpleStateController::_V
private

Definition at line 100 of file simple_state_controller.h.


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


control_box_rst
Author(s): Christoph Rösmann
autogenerated on Mon Feb 28 2022 22:08:03