Public Member Functions | Protected Attributes | Private Attributes | List of all members
KalmanFilter Class Reference

sample RT component which has one data input port and one data output port More...

#include <KalmanFilter.h>

Inheritance diagram for KalmanFilter:
Inheritance graph
[legend]

Public Member Functions

bool getKalmanFilterParam (OpenHRP::KalmanFilterService::KalmanFilterParam &i_param)
 
 KalmanFilter (RTC::Manager *manager)
 Constructor. More...
 
virtual RTC::ReturnCode_t onActivated (RTC::UniqueId ec_id)
 
virtual RTC::ReturnCode_t onDeactivated (RTC::UniqueId ec_id)
 
virtual RTC::ReturnCode_t onExecute (RTC::UniqueId ec_id)
 
virtual RTC::ReturnCode_t onInitialize ()
 
bool resetKalmanFilterState ()
 
bool setKalmanFilterParam (const OpenHRP::KalmanFilterService::KalmanFilterParam &i_param)
 
virtual ~KalmanFilter ()
 Destructor. More...
 
- Public Member Functions inherited from RTC::DataFlowComponentBase
 DataFlowComponentBase (Manager *manager)
 
void init ()
 
virtual ~DataFlowComponentBase (void)
 
- Public Member Functions inherited from RTC::RTObject_impl
ReturnCode_t activate (RTC::UniqueId ec_id)
 
void addConfigurationParamListener (ConfigurationParamListenerType type, ConfigurationParamListener *listener, bool autoclean=true)
 
ConfigurationParamListeneraddConfigurationParamListener (ConfigurationParamListenerType listener_type, Listener &obj, void(Listener::*memfunc)(const char *, const char *))
 
void addConfigurationSetListener (ConfigurationSetListenerType type, ConfigurationSetListener *listener, bool autoclean=true)
 
ConfigurationSetListeneraddConfigurationSetListener (ConfigurationSetListenerType listener_type, Listener &obj, void(Listener::*memfunc)(const coil::Properties &config_set))
 
void addConfigurationSetNameListener (ConfigurationSetNameListenerType type, ConfigurationSetNameListener *listener, bool autoclean=true)
 
ConfigurationSetNameListeneraddConfigurationSetNameListener (ConfigurationSetNameListenerType type, Listener &obj, void(Listener::*memfunc)(const char *))
 
void addExecutionContextActionListener (ECActionListenerType listener_type, ECActionListener *listener, bool autoclean=true)
 
ECActionListeneraddExecutionContextActionListener (ECActionListenerType listener_type, Listener &obj, void(Listener::*memfunc)(UniqueId))
 
bool addInPort (const char *name, InPortBase &inport)
 
bool addOutPort (const char *name, OutPortBase &outport)
 
bool addPort (PortBase &port)
 
bool addPort (PortService_ptr port)
 
bool addPort (CorbaPort &port)
 
PortActionListeneraddPortActionListener (PortActionListenerType listener_type, Listener &obj, void(Listener::*memfunc)(const RTC::PortProfile &))
 
void addPortActionListener (PortActionListenerType listener_type, PortActionListener *listener, bool autoclean=true)
 
void addPortConnectListener (PortConnectListenerType listener_type, PortConnectListener *listener, bool autoclean=true)
 
PortConnectListeneraddPortConnectListener (PortConnectListenerType listener_type, Listener &obj, void(Listener::*memfunc)(const char *, ConnectorProfile &))
 
void addPortConnectRetListener (PortConnectRetListenerType listener_type, PortConnectRetListener *listener, bool autoclean=true)
 
PortConnectRetListeneraddPortConnectRetListener (PortConnectRetListenerType listener_type, Listener &obj, void(Listener::*memfunc)(const char *, ConnectorProfile &, ReturnCode_t))
 
void addPostComponentActionListener (PostComponentActionListenerType listener_type, PostComponentActionListener *listener, bool autoclean=true)
 
PostComponentActionListeneraddPostComponentActionListener (PostCompActionListenerType listener_type, Listener &obj, void(Listener::*memfunc)(UniqueId ec_id, ReturnCode_t ret))
 
void addPreComponentActionListener (PreComponentActionListenerType listener_type, PreComponentActionListener *listener, bool autoclean=true)
 
PreComponentActionListeneraddPreComponentActionListener (PreCompActionListenerType listener_type, Listener &obj, void(Listener::*memfunc)(UniqueId ec_id))
 
bool addSdoServiceConsumer (const SDOPackage::ServiceProfile &prof)
 
bool addSdoServiceProvider (const SDOPackage::ServiceProfile &prof, SdoServiceProviderBase *provider)
 
UniqueId attach_context (ExecutionContext_ptr exec_context)
 
UniqueId bindContext (ExecutionContext_ptr exec_context)
 
bool bindParameter (const char *param_name, VarType &var, const char *def_val, bool(*trans)(VarType &, const char *)=coil::stringTo)
 
ReturnCode_t deactivate (RTC::UniqueId ec_id)
 
void deletePort (PortService_ptr port)
 
void deletePort (CorbaPort &port)
 
void deletePort (PortBase &port)
 
void deletePortByName (const char *port_name)
 
ReturnCode_t detach_context (UniqueId exec_handle)
 
virtual ReturnCode_t exit ()
 
virtual ReturnCode_t finalize ()
 
void finalizeContexts ()
 
void finalizePorts ()
 
virtual ComponentProfile * get_component_profile ()
 
virtual SDOPackage::Configuration_ptr get_configuration ()
 
virtual ExecutionContext_ptr get_context (UniqueId exec_handle)
 
virtual ExecutionContextHandle_t get_context_handle (ExecutionContext_ptr cxt)
 
virtual SDOPackage::DeviceProfile * get_device_profile ()
 
virtual SDOPackage::Monitoring_ptr get_monitoring ()
 
virtual SDOPackage::OrganizationList * get_organizations ()
 
virtual ExecutionContextListget_owned_contexts ()
 
virtual SDOPackage::OrganizationList * get_owned_organizations ()
 
virtual ExecutionContextListget_participating_contexts ()
 
virtual PortServiceListget_ports ()
 
virtual char * get_sdo_id ()
 
virtual SDOPackage::SDOService_ptr get_sdo_service (const char *id)
 
virtual char * get_sdo_type ()
 
virtual SDOPackage::ServiceProfile * get_service_profile (const char *id)
 
virtual SDOPackage::ServiceProfileList * get_service_profiles ()
 
virtual CORBA::Any * get_status (const char *name)
 
virtual SDOPackage::NVListget_status_list ()
 
const char * getCategory ()
 
const char * getDescription ()
 
ExecutionContext_ptr getExecutionContext (RTC::UniqueId ec_id)
 
double getExecutionRate (RTC::UniqueId ec_id)
 
const char * getInstanceName ()
 
std::vector< std::string > getNamingNames ()
 
RTObject_ptr getObjRef () const
 
coil::PropertiesgetProperties ()
 
const char * getTypeName ()
 
const char * getVendor ()
 
const char * getVersion ()
 
virtual ReturnCode_t initialize ()
 
virtual CORBA::Boolean is_alive (ExecutionContext_ptr exec_context)
 
bool isOwnExecutionContext (RTC::UniqueId ec_id)
 
virtual ReturnCode_t on_aborting (UniqueId exec_handle)
 
virtual ReturnCode_t on_activated (UniqueId exec_handle)
 
virtual ReturnCode_t on_deactivated (UniqueId exec_handle)
 
virtual ReturnCode_t on_error (UniqueId exec_handle)
 
virtual ReturnCode_t on_execute (UniqueId exec_handle)
 
virtual ReturnCode_t on_finalize ()
 
virtual ReturnCode_t on_initialize ()
 
virtual ReturnCode_t on_rate_changed (UniqueId exec_handle)
 
virtual ReturnCode_t on_reset (UniqueId exec_handle)
 
virtual ReturnCode_t on_shutdown (UniqueId exec_handle)
 
virtual ReturnCode_t on_startup (UniqueId exec_handle)
 
virtual ReturnCode_t on_state_update (UniqueId exec_handle)
 
bool readAll ()
 
void registerInPort (const char *name, InPortBase &inport)
 
void registerOutPort (const char *name, OutPortBase &outport)
 
void registerPort (PortBase &port)
 
void registerPort (PortService_ptr port)
 
void registerPort (CorbaPort &port)
 
void removeConfigurationParamListener (ConfigurationParamListenerType type, ConfigurationParamListener *listener)
 
void removeConfigurationSetListener (ConfigurationSetListenerType type, ConfigurationSetListener *listener)
 
void removeConfigurationSetNameListener (ConfigurationSetNameListenerType type, ConfigurationSetNameListener *listener)
 
void removeExecutionContextActionListener (ECActionListenerType listener_type, ECActionListener *listener)
 
bool removeInPort (InPortBase &port)
 
bool removeOutPort (OutPortBase &port)
 
bool removePort (PortService_ptr port)
 
bool removePort (CorbaPort &port)
 
bool removePort (PortBase &port)
 
void removePortActionListener (PortActionListenerType listener_type, PortActionListener *listener)
 
void removePortConnectListener (PortConnectListenerType listener_type, PortConnectListener *listener)
 
void removePortConnectRetListener (PortConnectRetListenerType listener_type, PortConnectRetListener *listener)
 
void removePostComponentActionListener (PostComponentActionListenerType listener_type, PostComponentActionListener *listener)
 
void removePreComponentActionListener (PreComponentActionListenerType listener_type, PreComponentActionListener *listener)
 
bool removeSdoServiceConsumer (const char *id)
 
bool removeSdoServiceProvider (const char *id)
 
ReturnCode_t reset (RTC::UniqueId ec_id)
 
 RTObject_impl (CORBA::ORB_ptr orb, PortableServer::POA_ptr poa)
 
 RTObject_impl (Manager *manager)
 
ReturnCode_t setExecutionRate (RTC::UniqueId ec_id, double rate)
 
void setInstanceName (const char *instance_name)
 
void setObjRef (const RTObject_ptr rtobj)
 
void setProperties (const coil::Properties &prop)
 
void setReadAll (bool read=true, bool completion=false)
 
void setWriteAll (bool write=true, bool completion=false)
 
void updateParameters (const char *config_set)
 
bool writeAll ()
 
virtual ~RTObject_impl (void)
 

Protected Attributes

TimedAcceleration3D m_acc
 
InPort< TimedAcceleration3D > m_accIn
 
TimedAcceleration3D m_accRef
 
InPort< TimedAcceleration3D > m_accRefIn
 
RTC::TimedOrientation3D m_baseRpyCurrent
 
RTC::OutPort< RTC::TimedOrientation3D > m_baseRpyCurrentOut
 
RTC::CorbaPort m_KalmanFilterServicePort
 
RTC::TimedDoubleSeq m_qCurrent
 
RTC::InPort< RTC::TimedDoubleSeq > m_qCurrentIn
 
TimedAngularVelocity3D m_rate
 
InPort< TimedAngularVelocity3D > m_rateIn
 
TimedOrientation3D m_rpy
 
TimedOrientation3D m_rpy_prev
 
InPort< TimedAngularVelocity3D > m_rpyIn
 
OutPort< TimedOrientation3D > m_rpyOut
 
TimedOrientation3D m_rpyRaw
 
TimedOrientation3D m_rpyRaw_prev
 
OutPort< TimedOrientation3D > m_rpyRawOut
 
KalmanFilterService_impl m_service0
 
- Protected Attributes inherited from RTC::RTObject_impl
ComponentActionListeners m_actionListeners
 
ConfigAdmin m_configsets
 
bool m_created
 
std::vector< ExecutionContextBase *> m_eclist
 
ExecutionContextServiceList m_ecMine
 
ExecutionContextServiceList m_ecOther
 
bool m_exiting
 
std::vector< InPortBase *> m_inports
 
RTObject_var m_objref
 
std::vector< OutPortBase *> m_outports
 
Managerm_pManager
 
CORBA::ORB_var m_pORB
 
PortAdmin m_portAdmin
 
PortConnectListeners m_portconnListeners
 
PortableServer::POA_var m_pPOA
 
ComponentProfile m_profile
 
coil::Properties m_properties
 
SDOPackage::Configuration_var m_pSdoConfig
 
SDOPackage::Configuration_implm_pSdoConfigImpl
 
bool m_readAll
 
bool m_readAllCompletion
 
SDOPackage::OrganizationList m_sdoOrganizations
 
SDOPackage::OrganizationList m_sdoOwnedOrganizations
 
SdoServiceAdmin m_sdoservice
 
SDOPackage::NVList m_sdoStatus
 
bool m_writeAll
 
bool m_writeAllCompletion
 
Logger rtclog
 

Private Attributes

hrp::Vector3 acc_offset
 
int dummy
 
EKFilter ekf_filter
 
OpenHRP::KalmanFilterService::KFAlgorithm kf_algorithm
 
int loop
 
unsigned int m_debugLevel
 
double m_dt
 
hrp::BodyPtr m_robot
 
hrp::Matrix33 m_sensorR
 
RPYKalmanFilter rpy_kf
 
hrp::Matrix33 sensorR_offset
 

Additional Inherited Members

- Public Types inherited from RTC::RTObject_impl
typedef ExecutionContextActionListener ECActionListener
 
typedef ExecutionContextActionListenerType ECActionListenerType
 
typedef PostComponentActionListener PostCompActionListener
 
typedef PostComponentActionListenerType PostCompActionListenerType
 
typedef PreComponentActionListener PreCompActionListener
 
typedef PreComponentActionListenerType PreCompActionListenerType
 
- Protected Member Functions inherited from RTC::RTObject_impl
virtual ReturnCode_t onAborting (RTC::UniqueId exec_handle)
 
void onAddPort (const PortProfile &pprof)
 
void onAttachExecutionContext (UniqueId ec_id)
 
void onDetachExecutionContext (UniqueId ec_id)
 
virtual ReturnCode_t onError (RTC::UniqueId exec_handle)
 
virtual ReturnCode_t onFinalize ()
 
virtual ReturnCode_t onRateChanged (RTC::UniqueId exec_handle)
 
void onRemovePort (const PortProfile &pprof)
 
virtual ReturnCode_t onReset (RTC::UniqueId exec_handle)
 
virtual ReturnCode_t onShutdown (RTC::UniqueId exec_handle)
 
virtual ReturnCode_t onStartup (RTC::UniqueId exec_handle)
 
virtual ReturnCode_t onStateUpdate (RTC::UniqueId exec_handle)
 
void postOnAborting (UniqueId ec_id, ReturnCode_t ret)
 
void postOnActivated (UniqueId ec_id, ReturnCode_t ret)
 
void postOnDeactivated (UniqueId ec_id, ReturnCode_t ret)
 
void postOnError (UniqueId ec_id, ReturnCode_t ret)
 
void postOnExecute (UniqueId ec_id, ReturnCode_t ret)
 
void postOnFinalize (UniqueId ec_id, ReturnCode_t ret)
 
void postOnInitialize (UniqueId ec_id, ReturnCode_t ret)
 
void postOnRateChanged (UniqueId ec_id, ReturnCode_t ret)
 
void postOnReset (UniqueId ec_id, ReturnCode_t ret)
 
void postOnShutdown (UniqueId ec_id, ReturnCode_t ret)
 
void postOnStartup (UniqueId ec_id, ReturnCode_t ret)
 
void postOnStateUpdate (UniqueId ec_id, ReturnCode_t ret)
 
void preOnAborting (UniqueId ec_id)
 
void preOnActivated (UniqueId ec_id)
 
void preOnDeactivated (UniqueId ec_id)
 
void preOnError (UniqueId ec_id)
 
void preOnExecute (UniqueId ec_id)
 
void preOnFinalize (UniqueId ec_id)
 
void preOnInitialize (UniqueId ec_id)
 
void preOnRateChanged (UniqueId ec_id)
 
void preOnReset (UniqueId ec_id)
 
void preOnShutdown (UniqueId ec_id)
 
void preOnStartup (UniqueId ec_id)
 
void preOnStateUpdate (UniqueId ec_id)
 
void shutdown ()
 

Detailed Description

sample RT component which has one data input port and one data output port

Definition at line 43 of file KalmanFilter.h.

Constructor & Destructor Documentation

◆ KalmanFilter()

KalmanFilter::KalmanFilter ( RTC::Manager manager)

Constructor.

Parameters
managerpointer to the Manager

Definition at line 40 of file KalmanFilter.cpp.

◆ ~KalmanFilter()

KalmanFilter::~KalmanFilter ( )
virtual

Destructor.

Definition at line 61 of file KalmanFilter.cpp.

Member Function Documentation

◆ getKalmanFilterParam()

bool KalmanFilter::getKalmanFilterParam ( OpenHRP::KalmanFilterService::KalmanFilterParam &  i_param)

Definition at line 316 of file KalmanFilter.cpp.

◆ onActivated()

RTC::ReturnCode_t KalmanFilter::onActivated ( RTC::UniqueId  ec_id)
virtual

Reimplemented from RTC::RTObject_impl.

Definition at line 167 of file KalmanFilter.cpp.

◆ onDeactivated()

RTC::ReturnCode_t KalmanFilter::onDeactivated ( RTC::UniqueId  ec_id)
virtual

Reimplemented from RTC::RTObject_impl.

Definition at line 173 of file KalmanFilter.cpp.

◆ onExecute()

RTC::ReturnCode_t KalmanFilter::onExecute ( RTC::UniqueId  ec_id)
virtual

Reimplemented from RTC::RTObject_impl.

Definition at line 179 of file KalmanFilter.cpp.

◆ onInitialize()

RTC::ReturnCode_t KalmanFilter::onInitialize ( void  )
virtual

Reimplemented from RTC::RTObject_impl.

Definition at line 67 of file KalmanFilter.cpp.

◆ resetKalmanFilterState()

bool KalmanFilter::resetKalmanFilterState ( )

Definition at line 310 of file KalmanFilter.cpp.

◆ setKalmanFilterParam()

bool KalmanFilter::setKalmanFilterParam ( const OpenHRP::KalmanFilterService::KalmanFilterParam &  i_param)

Definition at line 291 of file KalmanFilter.cpp.

Member Data Documentation

◆ acc_offset

hrp::Vector3 KalmanFilter::acc_offset
private

Definition at line 165 of file KalmanFilter.h.

◆ dummy

int KalmanFilter::dummy
private

Definition at line 167 of file KalmanFilter.h.

◆ ekf_filter

EKFilter KalmanFilter::ekf_filter
private

Definition at line 162 of file KalmanFilter.h.

◆ kf_algorithm

OpenHRP::KalmanFilterService::KFAlgorithm KalmanFilter::kf_algorithm
private

Definition at line 168 of file KalmanFilter.h.

◆ loop

int KalmanFilter::loop
private

Definition at line 167 of file KalmanFilter.h.

◆ m_acc

TimedAcceleration3D KalmanFilter::m_acc
protected

Definition at line 115 of file KalmanFilter.h.

◆ m_accIn

InPort<TimedAcceleration3D> KalmanFilter::m_accIn
protected

Definition at line 125 of file KalmanFilter.h.

◆ m_accRef

TimedAcceleration3D KalmanFilter::m_accRef
protected

Definition at line 116 of file KalmanFilter.h.

◆ m_accRefIn

InPort<TimedAcceleration3D> KalmanFilter::m_accRefIn
protected

Definition at line 126 of file KalmanFilter.h.

◆ m_baseRpyCurrent

RTC::TimedOrientation3D KalmanFilter::m_baseRpyCurrent
protected

Definition at line 137 of file KalmanFilter.h.

◆ m_baseRpyCurrentOut

RTC::OutPort<RTC::TimedOrientation3D> KalmanFilter::m_baseRpyCurrentOut
protected

Definition at line 138 of file KalmanFilter.h.

◆ m_debugLevel

unsigned int KalmanFilter::m_debugLevel
private

Definition at line 166 of file KalmanFilter.h.

◆ m_dt

double KalmanFilter::m_dt
private

Definition at line 160 of file KalmanFilter.h.

◆ m_KalmanFilterServicePort

RTC::CorbaPort KalmanFilter::m_KalmanFilterServicePort
protected

Definition at line 149 of file KalmanFilter.h.

◆ m_qCurrent

RTC::TimedDoubleSeq KalmanFilter::m_qCurrent
protected

Definition at line 135 of file KalmanFilter.h.

◆ m_qCurrentIn

RTC::InPort<RTC::TimedDoubleSeq> KalmanFilter::m_qCurrentIn
protected

Definition at line 136 of file KalmanFilter.h.

◆ m_rate

TimedAngularVelocity3D KalmanFilter::m_rate
protected

Definition at line 114 of file KalmanFilter.h.

◆ m_rateIn

InPort<TimedAngularVelocity3D> KalmanFilter::m_rateIn
protected

Definition at line 124 of file KalmanFilter.h.

◆ m_robot

hrp::BodyPtr KalmanFilter::m_robot
private

Definition at line 163 of file KalmanFilter.h.

◆ m_rpy

TimedOrientation3D KalmanFilter::m_rpy
protected

Definition at line 117 of file KalmanFilter.h.

◆ m_rpy_prev

TimedOrientation3D KalmanFilter::m_rpy_prev
protected

Definition at line 119 of file KalmanFilter.h.

◆ m_rpyIn

InPort<TimedAngularVelocity3D> KalmanFilter::m_rpyIn
protected

Definition at line 127 of file KalmanFilter.h.

◆ m_rpyOut

OutPort<TimedOrientation3D> KalmanFilter::m_rpyOut
protected

Definition at line 133 of file KalmanFilter.h.

◆ m_rpyRaw

TimedOrientation3D KalmanFilter::m_rpyRaw
protected

Definition at line 118 of file KalmanFilter.h.

◆ m_rpyRaw_prev

TimedOrientation3D KalmanFilter::m_rpyRaw_prev
protected

Definition at line 120 of file KalmanFilter.h.

◆ m_rpyRawOut

OutPort<TimedOrientation3D> KalmanFilter::m_rpyRawOut
protected

Definition at line 134 of file KalmanFilter.h.

◆ m_sensorR

hrp::Matrix33 KalmanFilter::m_sensorR
private

Definition at line 164 of file KalmanFilter.h.

◆ m_service0

KalmanFilterService_impl KalmanFilter::m_service0
protected

Definition at line 155 of file KalmanFilter.h.

◆ rpy_kf

RPYKalmanFilter KalmanFilter::rpy_kf
private

Definition at line 161 of file KalmanFilter.h.

◆ sensorR_offset

hrp::Matrix33 KalmanFilter::sensorR_offset
private

Definition at line 164 of file KalmanFilter.h.


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


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:22