00001 // -*- C++ -*- 00010 #ifndef TORQUE_FILTER_H 00011 #define TORQUE_FILTER_H 00012 00013 #include <rtm/idl/BasicDataType.hh> 00014 #include <rtm/Manager.h> 00015 #include <rtm/DataFlowComponentBase.h> 00016 #include <rtm/CorbaPort.h> 00017 #include <rtm/DataInPort.h> 00018 #include <rtm/DataOutPort.h> 00019 #include <rtm/idl/BasicDataTypeSkel.h> 00020 00021 #include <hrpModel/Body.h> 00022 #include <hrpModel/Link.h> 00023 #include <hrpModel/JointPath.h> 00024 00025 // Service implementation headers 00026 // <rtc-template block="service_impl_h"> 00027 // #include "TorqueFilter_impl.h" 00028 00029 #include "IIRFilter.h" 00030 00031 // </rtc-template> 00032 00033 // Service Consumer stub headers 00034 // <rtc-template block="consumer_stub_h"> 00035 00036 // </rtc-template> 00037 00038 using namespace RTC; 00039 00043 class TorqueFilter 00044 : public RTC::DataFlowComponentBase 00045 { 00046 public: 00051 TorqueFilter(RTC::Manager* manager); 00055 virtual ~TorqueFilter(); 00056 00057 // The initialize action (on CREATED->ALIVE transition) 00058 // formaer rtc_init_entry() 00059 virtual RTC::ReturnCode_t onInitialize(); 00060 00061 // The finalize action (on ALIVE->END transition) 00062 // formaer rtc_exiting_entry() 00063 // virtual RTC::ReturnCode_t onFinalize(); 00064 00065 // The startup action when ExecutionContext startup 00066 // former rtc_starting_entry() 00067 // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id); 00068 00069 // The shutdown action when ExecutionContext stop 00070 // former rtc_stopping_entry() 00071 // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id); 00072 00073 // The activated action (Active state entry action) 00074 // former rtc_active_entry() 00075 virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id); 00076 00077 // The deactivated action (Active state exit action) 00078 // former rtc_active_exit() 00079 virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); 00080 00081 // The execution action that is invoked periodically 00082 // former rtc_active_do() 00083 virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id); 00084 00085 // The aborting action when main logic error occurred. 00086 // former rtc_aborting_entry() 00087 // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id); 00088 00089 // The error action in ERROR state 00090 // former rtc_error_do() 00091 // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id); 00092 00093 // The reset action that is invoked resetting 00094 // This is same but different the former rtc_init_entry() 00095 // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id); 00096 00097 // The state update action that is invoked after onExecute() action 00098 // no corresponding operation exists in OpenRTm-aist-0.2.0 00099 // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id); 00100 00101 // The action that is invoked when execution context's rate is changed 00102 // no corresponding operation exists in OpenRTm-aist-0.2.0 00103 // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); 00104 00105 00106 protected: 00107 // Configuration variable declaration 00108 // <rtc-template block="config_declare"> 00109 00110 // </rtc-template> 00111 TimedDoubleSeq m_qCurrent; 00112 TimedDoubleSeq m_tauIn; 00113 TimedDoubleSeq m_tauOut; 00114 00115 // DataInPort declaration 00116 // <rtc-template block="inport_declare"> 00117 InPort<TimedDoubleSeq> m_qCurrentIn; 00118 InPort<TimedDoubleSeq> m_tauInIn; 00119 00120 // </rtc-template> 00121 00122 // DataOutPort declaration 00123 // <rtc-template block="outport_declare"> 00124 OutPort<TimedDoubleSeq> m_tauOutOut; 00125 00126 // </rtc-template> 00127 00128 // CORBA Port declaration 00129 // <rtc-template block="corbaport_declare"> 00130 00131 // </rtc-template> 00132 00133 // Service declaration 00134 // <rtc-template block="service_declare"> 00135 //RTC::CorbaPort m_TorqueFilterServicePort; 00136 00137 // </rtc-template> 00138 00139 // Consumer declaration 00140 // <rtc-template block="consumer_declare"> 00141 //TorqueFilterService_impl m_TorqueFilterService; 00142 00143 // </rtc-template> 00144 00145 private: 00146 00147 double m_dt; 00148 hrp::BodyPtr m_robot; 00149 unsigned int m_debugLevel; 00150 std::vector<double> m_torque_offset; 00151 std::vector<IIRFilter> m_filters; 00152 bool m_is_gravity_compensation; 00153 }; 00154 00155 00156 extern "C" 00157 { 00158 void TorqueFilterInit(RTC::Manager* manager); 00159 }; 00160 00161 #endif // TORQUE_FILTER_H