PDcontroller.h
Go to the documentation of this file.
1 // -*- mode: c++; -*-
10 #ifndef PDcontroller_H
11 #define PDcontroller_H
12 
13 #include <rtm/idl/BasicDataType.hh>
14 #include <rtm/Manager.h>
15 #include <rtm/DataFlowComponentBase.h>
16 #include <rtm/CorbaPort.h>
17 #include <rtm/CorbaNaming.h>
18 #include <rtm/DataInPort.h>
19 #include <rtm/DataOutPort.h>
20 #include <rtm/idl/BasicDataTypeSkel.h>
21 
22 #include <hrpUtil/EigenTypes.h>
23 #include <hrpModel/ModelLoaderUtil.h>
24 #include <hrpModel/Body.h>
25 #include <hrpModel/Link.h>
26 
27 // Service implementation headers
28 // <rtc-template block="service_impl_h">
29 
30 // </rtc-template>
31 
32 // Service Consumer stub headers
33 // <rtc-template block="consumer_stub_h">
34 
35 // </rtc-template>
36 
37 using namespace RTC;
38 
41 {
42  public:
44  ~PDcontroller();
45 
46  // The initialize action (on CREATED->ALIVE transition)
47  // formaer rtc_init_entry()
48  virtual RTC::ReturnCode_t onInitialize();
49 
50  // The finalize action (on ALIVE->END transition)
51  // formaer rtc_exiting_entry()
52  // virtual RTC::ReturnCode_t onFinalize();
53 
54  // The startup action when ExecutionContext startup
55  // former rtc_starting_entry()
56  // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id);
57 
58  // The shutdown action when ExecutionContext stop
59  // former rtc_stopping_entry()
60  // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);
61 
62  // The activated action (Active state entry action)
63  // former rtc_active_entry()
64  virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
65 
66  // The deactivated action (Active state exit action)
67  // former rtc_active_exit()
68  virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
69 
70  // The execution action that is invoked periodically
71  // former rtc_active_do()
72  virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
73 
74  // The aborting action when main logic error occurred.
75  // former rtc_aborting_entry()
76  // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id);
77 
78  // The error action in ERROR state
79  // former rtc_error_do()
80  // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id);
81 
82  // The reset action that is invoked resetting
83  // This is same but different the former rtc_init_entry()
84  // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id);
85 
86  // The state update action that is invoked after onExecute() action
87  // no corresponding operation exists in OpenRTm-aist-0.2.0
88  // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id);
89 
90  // The action that is invoked when execution context's rate is changed
91  // no corresponding operation exists in OpenRTm-aist-0.2.0
92  // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);
93 
94  protected:
95  // Configuration variable declaration
96  // <rtc-template block="config_declare">
97 
98  // </rtc-template>
99 
100  // DataInPort declaration
101  // <rtc-template block="inport_declare">
102  TimedDoubleSeq m_angle;
104  TimedDoubleSeq m_angleRef;
106 
107  // </rtc-template>
108 
109  // DataOutPort declaration
110  // <rtc-template block="outport_declare">
111  TimedDoubleSeq m_torque;
113 
114  // </rtc-template>
115 
116  // CORBA Port declaration
117  // <rtc-template block="corbaport_declare">
118 
119  // </rtc-template>
120 
121  // Service declaration
122  // <rtc-template block="service_declare">
123 
124  // </rtc-template>
125 
126  // Consumer declaration
127  // <rtc-template block="consumer_declare">
128 
129  // </rtc-template>
130 
131  private:
132  void readGainFile();
134  double dt; // sampling time of pd control
135  double ref_dt; // sampling time of renference angles
136  int step; // current interpolation step
137  int nstep; // the number of steps to interpolate references
138  std::ifstream gain;
139  std::string gain_fname;
140  hrp::dvector qold, qold_ref, Pgain, Dgain, tlimit_ratio;
141  size_t dof, loop;
142  unsigned int m_debugLevel;
143  int dummy;
144 };
145 
146 extern "C"
147 {
149 };
150 
151 #endif // PDcontroller_H
ec_id
std::string gain_fname
Definition: PDcontroller.h:139
TimedDoubleSeq m_torque
Definition: PDcontroller.h:111
OutPort< TimedDoubleSeq > m_torqueOut
Definition: PDcontroller.h:112
hrp::dvector tlimit_ratio
Definition: PDcontroller.h:140
InPort< TimedDoubleSeq > m_angleRefIn
Definition: PDcontroller.h:105
hrp::BodyPtr m_robot
Definition: PDcontroller.h:133
void PDcontrollerInit(RTC::Manager *manager)
manager
Eigen::VectorXd dvector
InPort< TimedDoubleSeq > m_angleIn
Definition: PDcontroller.h:103
ExecutionContextHandle_t UniqueId
std::ifstream gain
Definition: PDcontroller.h:138
unsigned int m_debugLevel
Definition: PDcontroller.h:142
TimedDoubleSeq m_angleRef
Definition: PDcontroller.h:104
TimedDoubleSeq m_angle
Definition: PDcontroller.h:102


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