State observation API. More...
#include <mc_observers/Observer.h>
Public Member Functions | |
Observer (const std::string &type, double dt) | |
virtual | ~Observer ()=default |
virtual void | configure (const mc_control::MCController &, const mc_rtc::Configuration &) |
Configure observer. More... | |
virtual void | reset (const mc_control::MCController &ctl)=0 |
Reset estimator. More... | |
virtual bool | run (const mc_control::MCController &ctl)=0 |
Compute observer state. More... | |
virtual void | update (mc_control::MCController &ctl)=0 |
Update the real robot state from the observed state. More... | |
void | name (const std::string &name) |
Set the observer's name. More... | |
const std::string & | name () const noexcept |
Returns the observer's name. More... | |
void | addToLogger_ (const mc_control::MCController &ctl, mc_rtc::Logger &logger, const std::string &category="") |
Add observer entries to the logger under the categrory "category + name()". More... | |
void | removeFromLogger_ (mc_rtc::Logger &logger, const std::string &category="") |
Remove observer from logger. More... | |
void | addToGUI_ (const mc_control::MCController &ctl, mc_rtc::gui::StateBuilder &gui, std::vector< std::string > category={}) |
Add observer to the gui under category {category, name()}. More... | |
void | removeFromGUI_ (mc_rtc::gui::StateBuilder &gui, std::vector< std::string > category={}) |
Remove observer from gui. More... | |
const std::string & | desc () const noexcept |
Short description of the observer. More... | |
const std::string | type () const noexcept |
const std::string & | error () const noexcept |
double | dt () const noexcept |
Protected Member Functions | |
virtual void | addToLogger (const mc_control::MCController &, mc_rtc::Logger &, const std::string &) |
Add observer from logger. More... | |
virtual void | removeFromLogger (mc_rtc::Logger &, const std::string &) |
Remove observer from logger. More... | |
virtual void | addToGUI (const mc_control::MCController &, mc_rtc::gui::StateBuilder &, const std::vector< std::string > &) |
Add observer information the GUI. More... | |
virtual void | removeFromGUI (mc_rtc::gui::StateBuilder &, const std::vector< std::string > &category) |
Remove observer from GUI. More... | |
Protected Attributes | |
std::string | name_ |
Observer name. More... | |
std::string | type_ |
Observer type. More... | |
std::string | desc_ |
Short description of the observer and its configuration. More... | |
std::string | error_ |
Descriptive error message to show if the observer failed. More... | |
double | dt_ |
Timestep. More... | |
State observation API.
All new observers must inherit from this Observer class, and implement the required virtual functions (at least reset, run, updateRobots). The framework provides a mechanism to load state observation pipelines (see ObserverPipeline) consisting of multiple sequences of observers running sequentially, each estimating part of the robot state. In that case, observers are loaded by the framework from libraries, and must be exported using the EXPORT_OBSERVER_MODULE(NAME, TYPE) function defined in ObserverMacros.h
Each observer has access to the public members of its parent controller, and thus can access properties of the robots, sensors, datastore, logger, gui, and set its estimation results to the controller's realRobots instances.
|
inline |
|
virtualdefault |
|
inlineprotectedvirtual |
Add observer information the GUI.
Default implementation does nothing, each observer implementation is responsible for adding its own elements to the GUI.
category | Category in which to add this observer |
Reimplemented in mc_observers::KinematicInertialPoseObserver, mc_observers::BodySensorObserver, and mc_observers::KinematicInertialObserver.
|
inline |
Add observer to the gui under category {category, name()}.
|
inlineprotectedvirtual |
Add observer from logger.
Default implementation does nothing, each observer implementation is responsible for adding its own log entries
category | Category in which to log this observer |
Reimplemented in mc_observers::KinematicInertialPoseObserver, mc_observers::KinematicInertialObserver, mc_observers::BodySensorObserver, and mc_observers::EncoderObserver.
|
inline |
Add observer entries to the logger under the categrory "category + name()".
|
inlinevirtual |
Configure observer.
config | Configuration for this observer. Refer to the JSON Schema documentation fo each observer for details on supported entries: https://jrl-umi3218.github.io/mc_rtc/json.html#Observers-objects |
Reimplemented in mc_observers::KinematicInertialPoseObserver, mc_observers::EncoderObserver, mc_observers::KinematicInertialObserver, and mc_observers::BodySensorObserver.
|
inlinenoexcept |
Short description of the observer.
Used to display a short summary of the observers pipeline to the user, the description should be as short as possible while showing the important information about the observer.
The output might end up looking like: Encoders (Position+Velocity) -> KinematicInertial (cutoff=0.01) -> BodySensor
|
inlinenoexcept |
Controller timestep
|
inlinenoexcept |
Informative message about the last error
|
inlinenoexcept |
Returns the observer's name.
|
inline |
Set the observer's name.
name | Name of the observer |
|
protectedvirtual |
Remove observer from GUI.
Default implementation removes the category
gui | |
category | Categroy in which the observer was added |
|
inline |
Remove observer from gui.
Default implementation removes category {category, name()}
|
protectedvirtual |
Remove observer from logger.
The default implementation removes the entry that were registered by this
category | Category in which this observer entries are logged |
|
inline |
Remove observer from logger.
|
pure virtual |
Reset estimator.
The reset function should make sure that the observer is started in a state consistent with the current robot state.
This function is called after MCController::reset()
Implemented in mc_observers::KinematicInertialPoseObserver, mc_observers::EncoderObserver, mc_observers::BodySensorObserver, and mc_observers::KinematicInertialObserver.
|
pure virtual |
Compute observer state.
This function is called before MCController::run()
Implemented in mc_observers::KinematicInertialPoseObserver, mc_observers::BodySensorObserver, mc_observers::EncoderObserver, and mc_observers::KinematicInertialObserver.
|
inlinenoexcept |
|
pure virtual |
Update the real robot state from the observed state.
This function is expected to write the estimated parameters to the corresponding real robot instances ctl.realRobots(), and compute the necessary information to ensure a consistent state (e.g calling forward velocity after updating joint velocities, etc).
This function is called after Observer::run() if the observer is declared to update the real robot in the pipeline configuration.
ctl | Controller running this observer |
Implemented in mc_observers::KinematicInertialPoseObserver, mc_observers::BodySensorObserver, mc_observers::EncoderObserver, and mc_observers::KinematicInertialObserver.
|
protected |
Short description of the observer and its configuration.
|
protected |
Timestep.
|
protected |
Descriptive error message to show if the observer failed.
|
protected |
Observer name.
|
protected |
Observer type.