ThermoEstimator.h
Go to the documentation of this file.
1 // -*- C++ -*-
10 #ifndef THERMO_ESTIMATOR_H
11 #define THERMO_ESTIMATOR_H
12 
13 #include <rtm/idl/BasicDataType.hh>
14 #include "hrpsys/idl/HRPDataTypes.hh"
15 #include <rtm/Manager.h>
16 #include <rtm/DataFlowComponentBase.h>
17 #include <rtm/CorbaPort.h>
18 #include <rtm/DataInPort.h>
19 #include <rtm/DataOutPort.h>
20 #include <rtm/idl/BasicDataTypeSkel.h>
21 
22 #include <hrpModel/Body.h>
23 #include <hrpModel/Link.h>
24 #include <hrpModel/JointPath.h>
25 
26 #include "MotorHeatParam.h"
27 
28 // Service implementation headers
29 // <rtc-template block="service_impl_h">
30 // #include "ThermoEstimator_impl.h"
31 
32 // </rtc-template>
33 
34 // Service Consumer stub headers
35 // <rtc-template block="consumer_stub_h">
36 
37 // </rtc-template>
38 
39 using namespace RTC;
40 
46 {
47  public:
56  virtual ~ThermoEstimator();
57 
58  // The initialize action (on CREATED->ALIVE transition)
59  // formaer rtc_init_entry()
60  virtual RTC::ReturnCode_t onInitialize();
61 
62  // The finalize action (on ALIVE->END transition)
63  // formaer rtc_exiting_entry()
64  // virtual RTC::ReturnCode_t onFinalize();
65 
66  // The startup action when ExecutionContext startup
67  // former rtc_starting_entry()
68  // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id);
69 
70  // The shutdown action when ExecutionContext stop
71  // former rtc_stopping_entry()
72  // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);
73 
74  // The activated action (Active state entry action)
75  // former rtc_active_entry()
76  virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
77 
78  // The deactivated action (Active state exit action)
79  // former rtc_active_exit()
80  virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
81 
82  // The execution action that is invoked periodically
83  // former rtc_active_do()
84  virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
85 
86  // The aborting action when main logic error occurred.
87  // former rtc_aborting_entry()
88  // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id);
89 
90  // The error action in ERROR state
91  // former rtc_error_do()
92  // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id);
93 
94  // The reset action that is invoked resetting
95  // This is same but different the former rtc_init_entry()
96  // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id);
97 
98  // The state update action that is invoked after onExecute() action
99  // no corresponding operation exists in OpenRTm-aist-0.2.0
100  // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id);
101 
102  // The action that is invoked when execution context's rate is changed
103  // no corresponding operation exists in OpenRTm-aist-0.2.0
104  // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);
105 
106 
107  protected:
108  // Configuration variable declaration
109  // <rtc-template block="config_declare">
110 
111  // </rtc-template>
112  TimedDoubleSeq m_tauIn;
113  TimedDoubleSeq m_qRefIn;
114  TimedDoubleSeq m_qCurrentIn;
115  OpenHRP::TimedLongSeqSeq m_servoStateIn;
116 
117  TimedDoubleSeq m_tempOut;
118  OpenHRP::TimedLongSeqSeq m_servoStateOut;
119 
120  // DataInPort declaration
121  // <rtc-template block="inport_declare">
126 
127  // </rtc-template>
128 
129  // DataOutPort declaration
130  // <rtc-template block="outport_declare">
133 
134  // </rtc-template>
135 
136  // CORBA Port declaration
137  // <rtc-template block="corbaport_declare">
138 
139  // </rtc-template>
140 
141  // Service declaration
142  // <rtc-template block="service_declare">
143  //RTC::CorbaPort m_ThermoEstimatorServicePort;
144 
145  // </rtc-template>
146 
147  // Consumer declaration
148  // <rtc-template block="consumer_declare">
149  //ThermoEstimatorService_impl m_ThermoEstimatorService;
150 
151  // </rtc-template>
152 
153  private:
154 
155  double m_dt;
156  long long m_loop;
157  unsigned int m_debugLevel;
158  hrp::BodyPtr m_robot; // for numJoints
159  double m_ambientTemp; // Ta
160  std::vector<MotorHeatParam> m_motorHeatParams;
162  void estimateJointTorqueFromJointError(hrp::dvector &error, hrp::dvector &tau);
163  void calculateJointTemperature(double tau, MotorHeatParam& param);
164  bool isDebug(int cycle = 200);
165 };
166 
167 
168 extern "C"
169 {
170  void ThermoEstimatorInit(RTC::Manager* manager);
171 };
172 
173 #endif // TORQUE_FILTER_H
void ThermoEstimatorInit(RTC::Manager *manager)
ec_id
manager
Eigen::VectorXd dvector
unsigned int m_debugLevel
TimedDoubleSeq m_tauIn
InPort< OpenHRP::TimedLongSeqSeq > m_servoStateInIn
ExecutionContextHandle_t UniqueId
OpenHRP::TimedLongSeqSeq m_servoStateIn
std::vector< MotorHeatParam > m_motorHeatParams
InPort< TimedDoubleSeq > m_qCurrentInIn
TimedDoubleSeq m_qCurrentIn
TimedDoubleSeq m_qRefIn
TimedDoubleSeq m_tempOut
hrp::dvector m_error2tau
hrp::BodyPtr m_robot
InPort< TimedDoubleSeq > m_qRefInIn
OutPort< OpenHRP::TimedLongSeqSeq > m_servoStateOutOut
InPort< TimedDoubleSeq > m_tauInIn
sample RT component which has one data input port and one data output port
motor heat parmaeter structure
OpenHRP::TimedLongSeqSeq m_servoStateOut
OutPort< TimedDoubleSeq > m_tempOutOut


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Thu May 6 2021 02:41:51