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


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:56