PDcontroller.cpp
Go to the documentation of this file.
1 // -*- mode: c++; -*-
10 #include "PDcontroller.h"
11 #include <iostream>
12 #include <coil/stringutil.h>
13 
14 // Module specification
15 // <rtc-template block="module_spec">
16 static const char* PDcontroller_spec[] =
17  {
18  "implementation_id", "PDcontroller",
19  "type_name", "PDcontroller",
20  "description", "PDcontroller component",
21  "version", HRPSYS_PACKAGE_VERSION,
22  "vendor", "AIST",
23  "category", "example",
24  "activity_type", "DataFlowComponent",
25  "max_instance", "10",
26  "language", "C++",
27  "lang_type", "compile",
28  // Configuration variables
29  "conf.default.pdgains_sim_file_name", "",
30  "conf.default.debugLevel", "0",
31  ""
32  };
33 // </rtc-template>
34 
36  : RTC::DataFlowComponentBase(manager),
37  // <rtc-template block="initializer">
38  m_angleIn("angle", m_angle),
39  m_angleRefIn("angleRef", m_angleRef),
40  m_torqueOut("torque", m_torque),
41  dt(0.005),
42  // </rtc-template>
43  gain_fname(""),
44  dof(0), loop(0),
45  dummy(0)
46 {
47 }
48 
50 {
51 }
52 
53 
54 RTC::ReturnCode_t PDcontroller::onInitialize()
55 {
56  std::cout << m_profile.instance_name << ": onInitialize() " << std::endl;
57 
59  coil::stringTo(dt, prop["dt"].c_str());
60  ref_dt = dt;
61  coil::stringTo(ref_dt, prop["ref_dt"].c_str());
62  nstep = ref_dt/dt;
63  step = nstep;
64 
66 
67  RTC::Manager& rtcManager = RTC::Manager::instance();
68  std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
69  int comPos = nameServer.find(",");
70  if (comPos < 0){
71  comPos = nameServer.length();
72  }
73  nameServer = nameServer.substr(0, comPos);
74  RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
75  if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
76  CosNaming::NamingContext::_duplicate(naming.getRootContext())
77  )){
78  std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]"
79  << std::endl;
80  }
81 
82  // <rtc-template block="bind_config">
83  // Bind variables and configuration variable
84  bindParameter("pdgains_sim_file_name", gain_fname, "");
85  bindParameter("debugLevel", m_debugLevel, "0");
86 
87  // Set InPort buffers
88  addInPort("angle", m_angleIn);
89  addInPort("angleRef", m_angleRefIn);
90 
91  // Set OutPort buffer
92  addOutPort("torque", m_torqueOut);
93 
94  // </rtc-template>
95 
96  return RTC::RTC_OK;
97 }
98 
99 
100 
101 /*
102 RTC::ReturnCode_t PDcontroller::onFinalize()
103 {
104  return RTC::RTC_OK;
105 }
106 */
107 
108 /*
109 RTC::ReturnCode_t PDcontroller::onStartup(RTC::UniqueId ec_id)
110 {
111  return RTC::RTC_OK;
112 }
113 */
114 
115 /*
116 RTC::ReturnCode_t PDcontroller::onShutdown(RTC::UniqueId ec_id)
117 {
118  return RTC::RTC_OK;
119 }
120 */
121 
122 RTC::ReturnCode_t PDcontroller::onActivated(RTC::UniqueId ec_id)
123 {
124  std::cout << m_profile.instance_name << ": on Activated " << std::endl;
125  if(m_angleIn.isNew()){
126  m_angleIn.read();
127  if (dof == 0) {
128  dof = m_angle.data.length();
129  readGainFile();
130  }
131  }
132  return RTC::RTC_OK;
133 }
134 
136 {
137  std::cout << m_profile.instance_name << ": on Deactivated " << std::endl;
138  return RTC::RTC_OK;
139 }
140 
141 
142 RTC::ReturnCode_t PDcontroller::onExecute(RTC::UniqueId ec_id)
143 {
144  loop++;
145  if(m_angleIn.isNew()){
146  m_angleIn.read();
147  if (dof == 0) {
148  dof = m_angle.data.length();
149  readGainFile();
150  }
151  }
152  if(m_angleRefIn.isNew()){
153  m_angleRefIn.read();
154  step = nstep;
155  }
156 
157  for(size_t i=0; i<dof; i++){
158  double q = m_angle.data[i];
159  double q_ref = step > 0 ? qold_ref[i] + (m_angleRef.data[i] - qold_ref[i])/step : qold_ref[i];
160  double dq = (q - qold[i]) / dt;
161  double dq_ref = (q_ref - qold_ref[i]) / dt;
162  qold[i] = q;
163  qold_ref[i] = q_ref;
164  m_torque.data[i] = -(q - q_ref) * Pgain[i] - (dq - dq_ref) * Dgain[i];
165  double tlimit;
166  if (m_robot && m_robot->numJoints() == dof) {
167  tlimit = fabs(m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst * tlimit_ratio[i]);
168  } else {
170  if (i == 0 && loop % 500 == 0) {
171  std::cerr << "[" << m_profile.instance_name << "] m_robot is not set properly!! Maybe ModelLoader is missing?" << std::endl;
172  }
173  }
174  if (loop % 100 == 0 && m_debugLevel == 1) {
175  std::cerr << "[" << m_profile.instance_name << "] joint = "
176  << i << ", tq = " << m_torque.data[i] << ", q,qref = (" << q << ", " << q_ref << "), dq,dqref = (" << dq << ", " << dq_ref << "), pd = (" << Pgain[i] << ", " << Dgain[i] << "), tlimit = " << tlimit << std::endl;
177  }
178  m_torque.data[i] = std::max(std::min(m_torque.data[i], tlimit), -tlimit);
179  }
180  step--;
181 
182  m_torqueOut.write();
183 
184  return RTC::RTC_OK;
185 }
186 
188 {
189  if (gain_fname == "") {
191  coil::stringTo(gain_fname, prop["pdgains_sim_file_name"].c_str());
192  }
193  // initialize length of vectors
194  qold.resize(dof);
195  qold_ref.resize(dof);
196  m_torque.data.length(dof);
197  m_angleRef.data.length(dof);
198  Pgain.resize(dof);
199  Dgain.resize(dof);
200  gain.open(gain_fname.c_str());
201  tlimit_ratio.resize(dof);
202  if (gain.is_open()){
203  double tmp;
204  for (unsigned int i=0; i<dof; i++){
205  if (gain >> tmp) {
206  Pgain[i] = tmp;
207  } else {
208  std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl;
209  }
210  if (gain >> tmp) {
211  Dgain[i] = tmp;
212  } else {
213  std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl;
214  }
215  }
216  gain.close();
217  std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] opened" << std::endl;
218  }else{
219  std::cerr << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] not opened" << std::endl;
220  }
221  // tlimit_ratio initialize
222  {
224  if (prop["pdcontrol_tlimit_ratio"] != "") {
225  coil::vstring tlimit_ratio_str = coil::split(prop["pdcontrol_tlimit_ratio"], ",");
226  if (tlimit_ratio_str.size() == dof) {
227  for (size_t i = 0; i < dof; i++) {
228  coil::stringTo(tlimit_ratio[i], tlimit_ratio_str[i].c_str());
229  }
230  std::cerr << "[" << m_profile.instance_name << "] tlimit_ratio is set to " << prop["pdcontrol_tlimit_ratio"] << std::endl;
231  } else {
232  for (size_t i = 0; i < dof; i++) {
233  tlimit_ratio[i] = 1.0;
234  }
235  std::cerr << "[" << m_profile.instance_name << "] pdcontrol_tlimit_ratio found, but invalid length (" << tlimit_ratio_str.size() << " != " << dof << ")." << std::endl;
236  std::cerr << "[" << m_profile.instance_name << "] All tlimit_ratio are set to 1.0." << std::endl;
237  }
238  } else {
239  for (size_t i = 0; i < dof; i++) {
240  tlimit_ratio[i] = 1.0;
241  }
242  std::cerr << "[" << m_profile.instance_name << "] No pdcontrol_tlimit_ratio found." << std::endl;
243  std::cerr << "[" << m_profile.instance_name << "] All tlimit_ratio are set to 1.0." << std::endl;
244  }
245  }
246  // initialize angleRef, old_ref and old with angle
247  for(unsigned int i=0; i < dof; ++i){
248  m_angleRef.data[i] = qold_ref[i] = qold[i] = m_angle.data[i];
249  }
250  // Print loaded gain
251  std::cerr << "[" << m_profile.instance_name << "] loadGain" << std::endl;
252  for (unsigned int i=0; i<m_robot->numJoints(); i++) {
253  std::cerr << "[" << m_profile.instance_name << "] " << m_robot->joint(i)->name << ", pgain = " << Pgain[i] << ", dgain = " << Dgain[i] << std::endl;
254  }
255 }
256 
257 /*
258  RTC::ReturnCode_t PDcontroller::onAborting(RTC::UniqueId ec_id)
259  {
260  return RTC::RTC_OK;
261  }
262 */
263 
264 /*
265  RTC::ReturnCode_t PDcontroller::onError(RTC::UniqueId ec_id)
266  {
267  return RTC::RTC_OK;
268  }
269 */
270 
271 /*
272  RTC::ReturnCode_t PDcontroller::onReset(RTC::UniqueId ec_id)
273  {
274  return RTC::RTC_OK;
275  }
276 */
277 
278 /*
279  RTC::ReturnCode_t PDcontroller::onStateUpdate(RTC::UniqueId ec_id)
280  {
281  return RTC::RTC_OK;
282  }
283 */
284 
285 /*
286  RTC::ReturnCode_t PDcontroller::onRateChanged(RTC::UniqueId ec_id)
287  {
288  return RTC::RTC_OK;
289  }
290 */
291 
292 extern "C"
293 {
294 
296  {
298  manager->registerFactory(profile,
299  RTC::Create<PDcontroller>,
300  RTC::Delete<PDcontroller>);
301  }
302 
303 };
304 
305 
ComponentProfile m_profile
png_infop png_charpp int png_charpp profile
#define max(a, b)
std::string gain_fname
Definition: PDcontroller.h:139
TimedDoubleSeq m_torque
Definition: PDcontroller.h:111
hrp::dvector qold_ref
Definition: PDcontroller.h:140
OutPort< TimedDoubleSeq > m_torqueOut
Definition: PDcontroller.h:112
bool stringTo(To &val, const char *str)
void readGainFile()
hrp::dvector tlimit_ratio
Definition: PDcontroller.h:140
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
InPort< TimedDoubleSeq > m_angleRefIn
Definition: PDcontroller.h:105
hrp::BodyPtr m_robot
Definition: PDcontroller.h:133
hrp::dvector Dgain
Definition: PDcontroller.h:140
vstring split(const std::string &input, const std::string &delimiter, bool ignore_empty)
CORBA::ORB_ptr getORB()
png_uint_32 i
coil::Properties & getProperties()
static Manager & instance()
bool addOutPort(const char *name, OutPortBase &outport)
boost::shared_ptr< Body > BodyPtr
#define min(a, b)
std::vector< std::string > vstring
coil::Properties & getConfig()
InPort< TimedDoubleSeq > m_angleIn
Definition: PDcontroller.h:103
ExecutionContextHandle_t UniqueId
PDcontroller(RTC::Manager *manager)
bool bindParameter(const char *param_name, VarType &var, const char *def_val, bool(*trans)(VarType &, const char *)=coil::stringTo)
std::ifstream gain
Definition: PDcontroller.h:138
static const char * PDcontroller_spec[]
hrp::dvector Pgain
Definition: PDcontroller.h:140
void PDcontrollerInit(RTC::Manager *manager)
int loadBodyFromModelLoader(::World *world, const char *name, const char *url, CosNaming::NamingContext_var cxt)
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
Sample PD component.
prop
naming
virtual bool isNew()
virtual bool write(DataType &value)
unsigned int m_debugLevel
Definition: PDcontroller.h:142
virtual RTC::ReturnCode_t onInitialize()
bool addInPort(const char *name, InPortBase &inport)
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
TimedDoubleSeq m_angleRef
Definition: PDcontroller.h:104
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
TimedDoubleSeq m_angle
Definition: PDcontroller.h:102
hrp::dvector qold
Definition: PDcontroller.h:140


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