ReferenceForceUpdater.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
10 #include <rtm/CorbaNaming.h>
11 #include <hrpModel/Link.h>
12 #include <hrpModel/Sensor.h>
13 #include <hrpModel/ModelLoaderUtil.h>
14 #include <hrpModel/JointPath.h>
15 #include <hrpUtil/MatrixSolvers.h>
16 #include "hrpsys/util/Hrpsys.h"
17 #include <boost/assign.hpp>
18 #include "ReferenceForceUpdater.h"
19 #include "hrpsys/util/VectorConvert.h"
20 
22 
23 // Module specification
24 // <rtc-template block="module_spec">
25 static const char* ReferenceForceUpdater_spec[] =
26  {
27  "implementation_id", "ReferenceForceUpdater",
28  "type_name", "ReferenceForceUpdater",
29  "description", "update reference force",
30  "version", HRPSYS_PACKAGE_VERSION,
31  "vendor", "AIST",
32  "category", "example",
33  "activity_type", "DataFlowComponent",
34  "max_instance", "10",
35  "language", "C++",
36  "lang_type", "compile",
37  // Configuration variables
38  "conf.default.debugLevel", "0",
39  ""
40  };
41 // </rtc-template>
42 
43 static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm)
44 {
45  int pre = os.precision();
46  os.setf(std::ios::fixed);
47  os << std::setprecision(6)
48  << (tm.sec + tm.nsec/1e9)
49  << std::setprecision(pre);
50  os.unsetf(std::ios::fixed);
51  return os;
52 }
53 
55  : RTC::DataFlowComponentBase(manager),
56  // <rtc-template block="initializer">
57  m_qRefIn("qRef", m_qRef),
58  m_basePosIn("basePosIn", m_basePos),
59  m_baseRpyIn("baseRpyIn", m_baseRpy),
60  m_rpyIn("rpy", m_rpy),
61  m_diffFootOriginExtMomentIn("diffFootOriginExtMoment", m_diffFootOriginExtMoment),
62  m_refFootOriginExtMomentOut("refFootOriginExtMoment", m_refFootOriginExtMoment),
63  m_refFootOriginExtMomentIsHoldValueOut("refFootOriginExtMomentIsHoldValue", m_refFootOriginExtMomentIsHoldValue),
64  m_ReferenceForceUpdaterServicePort("ReferenceForceUpdaterService"),
65  // </rtc-template>
66  m_robot(hrp::BodyPtr()),
67  m_debugLevel(0),
68  use_sh_base_pos_rpy(false),
69  footoriginextmoment_name("footoriginextmoment"),
70  objextmoment0_name("objextmoment0")
71 {
73 }
74 
76 {
77 }
78 
79 
80 
82 {
83  std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
84  // <rtc-template block="bind_config">
85  // Bind variables and configuration variable
86  bindParameter("debugLevel", m_debugLevel, "0");
87 
88  // </rtc-template>
89 
90  // Registration: InPort/OutPort/Service
91  // <rtc-template block="registration">
92  // Set InPort buffers
93  addInPort("qRef", m_qRefIn);
94  addInPort("basePosIn", m_basePosIn);
95  addInPort("baseRpyIn",m_baseRpyIn);
96  addInPort("rpy",m_rpyIn);
97  addInPort("diffFootOriginExtMoment", m_diffFootOriginExtMomentIn);
98 
99  addOutPort("refFootOriginExtMoment", m_refFootOriginExtMomentOut);
100  addOutPort("refFootOriginExtMomentIsHoldValue", m_refFootOriginExtMomentIsHoldValueOut);
101 
102  // Set service provider to Ports
103  m_ReferenceForceUpdaterServicePort.registerProvider("service0", "ReferenceForceUpdaterService", m_ReferenceForceUpdaterService);
104 
105  // Set service consumers to Ports
106  // Set CORBA Service Ports
108 
109  // Get dt
110  RTC::Properties& prop = getProperties(); // get properties information from .wrl file
111  coil::stringTo(m_dt, prop["dt"].c_str());
112 
113  // Make m_robot instance
114  m_robot = hrp::BodyPtr(new hrp::Body());
115  RTC::Manager& rtcManager = RTC::Manager::instance();
116  std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
117  int comPos = nameServer.find(",");
118  if (comPos < 0){
119  comPos = nameServer.length();
120  }
121  nameServer = nameServer.substr(0, comPos);
122  RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
123  if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), // load robot model for m_robot
124  CosNaming::NamingContext::_duplicate(naming.getRootContext())
125  )){
126  std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
127  return RTC::RTC_ERROR;
128  }
129 
130  // Setting for wrench data ports (real + virtual)
131  std::vector<std::string> fsensor_names;
132  // find names for real force sensors
133  unsigned int npforce = m_robot->numSensors(hrp::Sensor::FORCE);
134  for (unsigned int i=0; i<npforce; i++){
135  fsensor_names.push_back(m_robot->sensor(hrp::Sensor::FORCE, i)->name);
136  }
137  // load virtual force sensors
138  readVirtualForceSensorParamFromProperties(m_vfs, m_robot, prop["virtual_force_sensor"], std::string(m_profile.instance_name));
139  unsigned int nvforce = m_vfs.size();
140  for (unsigned int i=0; i<nvforce; i++){
141  for ( std::map<std::string, hrp::VirtualForceSensorParam>::iterator it = m_vfs.begin(); it != m_vfs.end(); it++ ) {
142  if (it->second.id == (int)i) fsensor_names.push_back(it->first);
143  }
144  }
145 
146  // add ports for all force sensors (real force, input and output of ref_force)
147  unsigned int nforce = npforce + nvforce;
148  m_force.resize(nforce);
149  m_forceIn.resize(nforce);
150  m_ref_force_in.resize(nforce);
151  m_ref_force_out.resize(nforce);
152  m_ref_forceIn.resize(nforce);
153  m_ref_forceOut.resize(nforce);
154  std::cerr << "[" << m_profile.instance_name << "] create force sensor ports" << std::endl;
155  for (unsigned int i=0; i<nforce; i++){
156  // actual inport
157  m_forceIn[i] = new InPort<TimedDoubleSeq>(fsensor_names[i].c_str(), m_force[i]);
158  m_force[i].data.length(6);
159  registerInPort(fsensor_names[i].c_str(), *m_forceIn[i]);
160  // ref inport
161  m_ref_force_in[i].data.length(6);
162  for (unsigned int j=0; j<6; j++) m_ref_force_in[i].data[j] = 0.0;
163  m_ref_forceIn[i] = new InPort<TimedDoubleSeq>(std::string("ref_"+fsensor_names[i]+"In").c_str(), m_ref_force_in[i]);
164  registerInPort(std::string("ref_"+fsensor_names[i]+"In").c_str(), *m_ref_forceIn[i]);
165  std::cerr << "[" << m_profile.instance_name << "] name = " << fsensor_names[i] << std::endl;
166  // ref Outport
167  m_ref_force_out[i].data.length(6);
168  for (unsigned int j=0; j<6; j++) m_ref_force_out[i].data[j] = 0.0;
169  m_ref_forceOut[i] = new OutPort<TimedDoubleSeq>(std::string("ref_"+fsensor_names[i]+"Out").c_str(), m_ref_force_out[i]);
170  registerOutPort(std::string("ref_"+fsensor_names[i]+"Out").c_str(), *m_ref_forceOut[i]);
171  std::cerr << "[" << m_profile.instance_name << "] name = " << fsensor_names[i] << std::endl;
172  }
173 
174  // setting from conf file
175  // rleg,TARGET_LINK,BASE_LINK,x,y,z,rx,ry,rz,rth #<=pos + rot (axis+angle)
176  coil::vstring end_effectors_str = coil::split(prop["end_effectors"], ",");
177  if (end_effectors_str.size() > 0) {
178  size_t prop_num = 10;
179  size_t num = end_effectors_str.size()/prop_num;
180  for (size_t i = 0; i < num; i++) {
181  std::string ee_name, ee_target, ee_base;
182  coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
183  coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
184  coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
185  ee_trans eet;
186  for (size_t j = 0; j < 3; j++) {
187  coil::stringTo(eet.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
188  }
189  double tmpv[4];
190  for (int j = 0; j < 4; j++ ) {
191  coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
192  }
193  eet.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
194  eet.target_name = ee_target;
195  {
196  bool is_ee_exists = false;
197  for (size_t j = 0; j < nforce; j++) {
198  hrp::Sensor* sensor = m_robot->sensor(hrp::Sensor::FORCE, j);
199  hrp::Link* alink = m_robot->link(ee_target);
200  while (alink != NULL && alink->name != ee_base && !is_ee_exists) {
201  if ( alink->name == sensor->link->name ) {
202  is_ee_exists = true;
203  eet.sensor_name = sensor->name;
204  }
205  alink = alink->parent;
206  }
207  }
208  }
209  ee_map.insert(std::pair<std::string, ee_trans>(ee_name , eet));
210 
211  if (( ee_name != "rleg" ) && ( ee_name != "lleg" ))
212  m_RFUParam.insert(std::pair<std::string, ReferenceForceUpdaterParam>(ee_name , ReferenceForceUpdaterParam(m_dt)));
213 
214  ee_index_map.insert(std::pair<std::string, size_t>(ee_name, i));
215  ref_force.push_back(hrp::Vector3::Zero());
216  //ref_force_interpolator.insert(std::pair<std::string, interpolator*>(ee_name, new interpolator(3, m_dt)));
217  ref_force_interpolator.insert(std::pair<std::string, interpolator*>(ee_name, new interpolator(3, m_dt, interpolator::LINEAR)));
218  if (( ee_name != "lleg" ) && ( ee_name != "rleg" )) transition_interpolator.insert(std::pair<std::string, interpolator*>(ee_name, new interpolator(1, m_dt)));
219  std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << ee_target << " " << ee_base << std::endl;
220  std::cerr << "[" << m_profile.instance_name << "] target = " << ee_target << ", base = " << ee_base << ", sensor_name = " << eet.sensor_name << std::endl;
221  std::cerr << "[" << m_profile.instance_name << "] localPos = " << eet.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl;
222  std::cerr << "[" << m_profile.instance_name << "] localR = " << eet.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", " [", "]")) << std::endl;
223  }
224  // For FootOriginExtMoment
225  {
226  std::string ee_name = footoriginextmoment_name;
227  m_RFUParam.insert(std::pair<std::string, ReferenceForceUpdaterParam>(ee_name, ReferenceForceUpdaterParam(m_dt)));
228  // Initial param
229  m_RFUParam[ee_name].update_freq = 1/m_dt; // [Hz], update in every control loop
230  m_RFUParam[ee_name].update_count = 1; // update in every control loop, round((1/rfu_param.update_freq)/m_dt)
231  m_RFUParam[ee_name].update_time_ratio = 1.0;
232  m_RFUParam[ee_name].p_gain = 0.003;
233  m_RFUParam[ee_name].act_force_filter->setCutOffFreq(25.0); // [Hz]
234  ee_trans eet;
235  eet.localPos = hrp::Vector3::Zero();
236  eet.localR = hrp::Matrix33::Identity();
237  eet.target_name = "";
238  ee_map.insert(std::pair<std::string, ee_trans>(ee_name , eet));
239  ee_index_map.insert(std::pair<std::string, size_t>(ee_name, ref_force.size()));
240  ref_force.push_back(hrp::Vector3::Zero());
241  ref_force_interpolator.insert(std::pair<std::string, interpolator*>(ee_name, new interpolator(3, m_dt, interpolator::LINEAR)));
242  transition_interpolator.insert(std::pair<std::string, interpolator*>(ee_name, new interpolator(1, m_dt)));
243  }
244  // For ObjExtMoment0
245  {
246  std::string ee_name = objextmoment0_name;
247  m_RFUParam.insert(std::pair<std::string, ReferenceForceUpdaterParam>(ee_name, ReferenceForceUpdaterParam(m_dt)));
248  // Initial param
249  m_RFUParam[ee_name].update_freq = 1/m_dt; // [Hz], update in every control loop
250  m_RFUParam[ee_name].update_count = 1; // update in every control loop, round((1/rfu_param.update_freq)/m_dt)
251  m_RFUParam[ee_name].update_time_ratio = 1.0;
252  m_RFUParam[ee_name].act_force_filter->setCutOffFreq(20.0); // [Hz]
253  // other param
254  ee_trans eet;
255  eet.localPos = hrp::Vector3::Zero();
256  eet.localR = hrp::Matrix33::Identity();
257  eet.target_name = "";
258  ee_map.insert(std::pair<std::string, ee_trans>(ee_name , eet));
259  ee_index_map.insert(std::pair<std::string, size_t>(ee_name, ref_force.size()));
260  ref_force.push_back(hrp::Vector3::Zero());
261  ref_force_interpolator.insert(std::pair<std::string, interpolator*>(ee_name, new interpolator(3, m_dt, interpolator::LINEAR)));
262  transition_interpolator.insert(std::pair<std::string, interpolator*>(ee_name, new interpolator(1, m_dt)));
263  }
264  }
265 
266  // check if the dof of m_robot match the number of joint in m_robot
267  unsigned int dof = m_robot->numJoints();
268  for ( unsigned int i = 0 ; i < dof; i++ ){
269  if ( (int)i != m_robot->joint(i)->jointId ) {
270  std::cerr << "[" << m_profile.instance_name << "] jointId is not equal to the index number" << std::endl;
271  return RTC::RTC_ERROR;
272  }
273  }
274 
275  loop = 0;
277  for (unsigned int i=0; i<transition_interpolator.size(); i++ ) transition_interpolator_ratio[i] = 0;
278 
279  return RTC::RTC_OK;
280 }
281 
282 
283 
285 {
286  std::cerr << "[" << m_profile.instance_name << "] onFinalize()" << std::endl;
287  for ( std::map<std::string, interpolator*>::iterator it = ref_force_interpolator.begin(); it != ref_force_interpolator.end(); it++ ) {
288  delete it->second;
289  }
290  for ( std::map<std::string, interpolator*>::iterator it = transition_interpolator.begin(); it != transition_interpolator.end(); it++ ) {
291  delete it->second;
292  }
293  ref_force_interpolator.clear();
294  transition_interpolator.clear();
295  return RTC::RTC_OK;
296 }
297 
298 /*
299 RTC::ReturnCode_t ReferenceForceUpdater::onStartup(RTC::UniqueId ec_id)
300 {
301  return RTC::RTC_OK;
302 }
303 */
304 
305 /*
306 RTC::ReturnCode_t ReferenceForceUpdater::onShutdown(RTC::UniqueId ec_id)
307 {
308  return RTC::RTC_OK;
309 }
310 */
311 
313 {
314  std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
315  return RTC::RTC_OK;
316 }
317 
319 {
320  std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
321  return RTC::RTC_OK;
322 }
323 
324 
325 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
327 {
328  loop ++;
329 
330  // check dataport input
331  for (unsigned int i=0; i<m_forceIn.size(); i++){
332  if ( m_forceIn[i]->isNew() ) {
333  m_forceIn[i]->read();
334  }
335  if ( m_ref_forceIn[i]->isNew() ) {
336  m_ref_forceIn[i]->read();
337  }
338  }
339  if (m_basePosIn.isNew()) {
340  m_basePosIn.read();
341  }
342  if (m_baseRpyIn.isNew()) {
343  m_baseRpyIn.read();
344  }
345  if (m_rpyIn.isNew()) {
346  m_rpyIn.read();
347  }
350  }
351  if (m_qRefIn.isNew()) {
352  m_qRefIn.read();
353  }
354 
355  //syncronize m_robot to the real robot
356  if ( m_qRef.data.length() == m_robot->numJoints() ) {
357  Guard guard(m_mutex);
358 
359  // Interpolator
360  for (std::map<std::string, ReferenceForceUpdaterParam>::iterator itr = m_RFUParam.begin(); itr != m_RFUParam.end(); itr++ ) {
361  std::string arm = itr->first;
362  size_t arm_idx = ee_index_map[arm];
363  bool transition_interpolator_isEmpty = transition_interpolator[arm]->isEmpty();
364  if ( ! transition_interpolator_isEmpty ) {
365  transition_interpolator[arm]->get(&transition_interpolator_ratio[arm_idx], true);
366  if ( transition_interpolator[arm]->isEmpty() && m_RFUParam[arm].is_active && m_RFUParam[arm].is_stopping ) {
367  std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm << "] ReferenceForceUpdater [" << arm << "] active => inactive." << std::endl;
368  m_RFUParam[arm].is_active = false;
369  m_RFUParam[arm].is_stopping = false;
370  }
371  }
372  }
373 
374  // Get and set reference (target) parameters
376 
377  // Get force sensor values
378  // Force sensor's force value is absolute in reference frame
379  for (unsigned int i=0; i<m_force.size(); i++ ){
380  hrp::Sensor* sensor = m_robot->sensor(hrp::Sensor::FORCE, i);
381  hrp::Vector3 act_force = (sensor->link->R * sensor->localR) * hrp::Vector3(m_force[i].data[0], m_force[i].data[1], m_force[i].data[2]);
382  for (std::map<std::string, ReferenceForceUpdaterParam>::iterator itr = m_RFUParam.begin(); itr != m_RFUParam.end(); itr++ ) {
383  if (ee_index_map[itr->first] == i) itr->second.act_force_filter->passFilter(act_force);
384  }
385  }
386  // DiffFootOriginExtMoment value is absolute in reference frame
387  {
389  m_RFUParam[footoriginextmoment_name].act_force_filter->passFilter(df);
390  }
391 
392  // If RFU is not active
393  {
394  bool all_arm_is_not_active = true;
395  const hrp::Vector3 default_ref_foot_origin_ext_moment = hrp::Vector3::Zero();
396  for (std::map<std::string, ReferenceForceUpdaterParam>::iterator itr = m_RFUParam.begin(); itr != m_RFUParam.end(); itr++ ) {
397  std::string arm = itr->first;
398  size_t arm_idx = ee_index_map[arm];
399  if ( m_RFUParam[arm].is_active ) all_arm_is_not_active = false;
400  else {
401  if ( arm == footoriginextmoment_name ) {
402  for (unsigned int j=0; j<3; j++ ) ref_force[arm_idx](j) = default_ref_foot_origin_ext_moment(j);
403  } else if ( arm == objextmoment0_name ) {
404  for (unsigned int j=0; j<3; j++ ) ref_force[arm_idx](j) = 0;
405  } else {
406  for (unsigned int j=0; j<3; j++ ) ref_force[arm_idx](j) = m_ref_force_in[arm_idx].data[j];
407  }
408  }
409  }
410  //determin ref_force_out from ref_force_in
411  if ( all_arm_is_not_active ) {
412  for (unsigned int i=0; i<m_ref_force_in.size(); i++ ){
413  for (unsigned int j=0; j<6; j++ ) {
414  m_ref_force_out[i].data[j] = m_ref_force_in[i].data[j];
415  }
416  m_ref_force_out[i].tm = m_ref_force_in[i].tm;
417  m_ref_forceOut[i]->write();
418  }
419  m_refFootOriginExtMoment.data.x = default_ref_foot_origin_ext_moment(0);
420  m_refFootOriginExtMoment.data.y = default_ref_foot_origin_ext_moment(1);
421  m_refFootOriginExtMoment.data.z = default_ref_foot_origin_ext_moment(2);
427  return RTC::RTC_OK;
428  }
429  }
430 
431  // If RFU is active
432 
433  // Update reference force
434  for (std::map<std::string, ReferenceForceUpdaterParam>::iterator itr = m_RFUParam.begin(); itr != m_RFUParam.end(); itr++ ) {
435  std::string arm = itr->first;
436  if ( m_RFUParam[arm].is_active && loop % m_RFUParam[arm].update_count == 0 ) {
438  else if ( arm == objextmoment0_name ) updateRefObjExtMoment0(arm);
439  else updateRefForces(arm);
440  }
441  if (!ref_force_interpolator[arm]->isEmpty()) {
442  ref_force_interpolator[arm]->get(ref_force[ee_index_map[arm]].data(), true);
443  }
444  }
445  }
446 
447  //determin ref_force_out from ref_force_in
448  for (unsigned int i=0; i<m_ref_force_in.size(); i++ ){
449  for (unsigned int j=0; j<6; j++ ) {
450  m_ref_force_out[i].data[j] = m_ref_force_in[i].data[j];
451  }
452  if (m_RFUParam[objextmoment0_name].is_active) { // TODO:tempolary
453  size_t idx = ee_index_map[objextmoment0_name];
454  for (unsigned int j=0; j<3; j++ ) {
455  m_ref_force_out[i].data[j] = m_ref_force_in[i].data[j];
456  if (ee_index_map["rarm"] == i) {
457  m_ref_force_out[i].data[j] += ref_force[idx](j) * transition_interpolator_ratio[idx];
458  } else if (ee_index_map["larm"] == i) {
459  m_ref_force_out[i].data[j] -= ref_force[idx](j) * transition_interpolator_ratio[idx];
460  }
461  }
462  } else {
463  for (unsigned int j=0; j<3; j++ ) {
465  }
466  }
467  m_ref_force_out[i].tm = m_ref_force_in[i].tm;
468  m_ref_forceOut[i]->write();
469  }
470 
471  // FootOriginExtMoment
473  hrp::Vector3 tmp_moment = (foot_origin_rot.transpose() * ref_force[idx]) * transition_interpolator_ratio[idx];
474  m_refFootOriginExtMoment.data.x = tmp_moment(0);
475  m_refFootOriginExtMoment.data.y = tmp_moment(1);
476  m_refFootOriginExtMoment.data.z = tmp_moment(2);
482 
483  return RTC::RTC_OK;
484 }
485 
487 {
488  // reference model
489  for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
490  m_robot->joint(i)->q = m_qRef.data[i];
491  }
492  m_robot->rootLink()->p = hrp::Vector3(m_basePos.data.x, m_basePos.data.y, m_basePos.data.z);
493  m_robot->rootLink()->R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
494  m_robot->calcForwardKinematics();
495  if ( (ee_map.find("rleg") != ee_map.end() && ee_map.find("lleg") != ee_map.end()) // if legged robot
496  && !use_sh_base_pos_rpy ) {
497  // TODO
498  // Tempolarily modify root coords to fix foot pos rot
499  // This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272
500 
501  // get current foot mid pos + rot
502  std::vector<hrp::Vector3> foot_pos;
503  std::vector<hrp::Matrix33> foot_rot;
504  std::vector<std::string> leg_names;
505  leg_names.push_back("rleg");
506  leg_names.push_back("lleg");
507  for (size_t i = 0; i < leg_names.size(); i++) {
508  hrp::Link* target_link = m_robot->link(ee_map[leg_names[i]].target_name);
509  foot_pos.push_back(target_link->p + target_link->R * ee_map[leg_names[i]].localPos);
510  foot_rot.push_back(target_link->R * ee_map[leg_names[i]].localR);
511  }
512  hrp::Vector3 current_foot_mid_pos ((foot_pos[0]+foot_pos[1])/2.0);
513  hrp::Matrix33 current_foot_mid_rot;
514  rats::mid_rot(current_foot_mid_rot, 0.5, foot_rot[0], foot_rot[1]);
515  // calculate fix pos + rot
516  hrp::Vector3 new_foot_mid_pos(current_foot_mid_pos);
517  hrp::Matrix33 new_foot_mid_rot;
518  {
519  hrp::Vector3 ex = hrp::Vector3::UnitX();
520  hrp::Vector3 ez = hrp::Vector3::UnitZ();
521  hrp::Vector3 xv1 (current_foot_mid_rot * ex);
522  xv1(2) = 0.0;
523  xv1.normalize();
524  hrp::Vector3 yv1(ez.cross(xv1));
525  new_foot_mid_rot(0,0) = xv1(0); new_foot_mid_rot(1,0) = xv1(1); new_foot_mid_rot(2,0) = xv1(2);
526  new_foot_mid_rot(0,1) = yv1(0); new_foot_mid_rot(1,1) = yv1(1); new_foot_mid_rot(2,1) = yv1(2);
527  new_foot_mid_rot(0,2) = ez(0); new_foot_mid_rot(1,2) = ez(1); new_foot_mid_rot(2,2) = ez(2);
528  }
529  // fix root pos + rot to fix "coords" = "current_foot_mid_xx"
530  hrp::Matrix33 tmpR (new_foot_mid_rot * current_foot_mid_rot.transpose());
531  m_robot->rootLink()->p = new_foot_mid_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos);
532  rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R);
533  m_robot->calcForwardKinematics();
534  }
535 
536  hrp::Vector3 foot_origin_pos;
537  calcFootOriginCoords(foot_origin_pos, foot_origin_rot);
538 };
539 
541 {
542  rats::coordinates leg_c[2], tmpc;
543  hrp::Vector3 ez = hrp::Vector3::UnitZ();
544  hrp::Vector3 ex = hrp::Vector3::UnitX();
545  size_t i = 0;
546  for (std::map<std::string, ee_trans>::iterator itr = ee_map.begin(); itr != ee_map.end(); itr++ ) {
547  if (itr->first.find("leg") == std::string::npos) continue;
548  hrp::Link* target = m_robot->sensor<hrp::ForceSensor>(itr->second.sensor_name)->link;
549  leg_c[i].pos = target->p;
550  hrp::Vector3 xv1(target->R * ex);
551  xv1(2)=0.0;
552  xv1.normalize();
553  hrp::Vector3 yv1(ez.cross(xv1));
554  leg_c[i].rot(0,0) = xv1(0); leg_c[i].rot(1,0) = xv1(1); leg_c[i].rot(2,0) = xv1(2);
555  leg_c[i].rot(0,1) = yv1(0); leg_c[i].rot(1,1) = yv1(1); leg_c[i].rot(2,1) = yv1(2);
556  leg_c[i].rot(0,2) = ez(0); leg_c[i].rot(1,2) = ez(1); leg_c[i].rot(2,2) = ez(2);
557  i++;
558  }
559  // Only double support is assumed
560  rats::mid_coords(tmpc, 0.5, leg_c[0], leg_c[1]);
561  foot_origin_pos = tmpc.pos;
562  foot_origin_rot = tmpc.rot;
563 }
564 
566 {
567  double interpolation_time = 0;
568  size_t arm_idx = ee_index_map[arm];
569  hrp::Vector3 df = m_RFUParam[arm].act_force_filter->getCurrentValue();
570  if (!m_RFUParam[arm].is_hold_value)
571  ref_force[arm_idx] = ref_force[arm_idx] + (m_RFUParam[arm].p_gain * transition_interpolator_ratio[arm_idx]) * df;
572  interpolation_time = (1/m_RFUParam[arm].update_freq) * m_RFUParam[arm].update_time_ratio;
573  if ( ref_force_interpolator[arm]->isEmpty() ) {
574  ref_force_interpolator[arm]->setGoal(ref_force[arm_idx].data(), interpolation_time, true);
575  }
576  if ( DEBUGP ) {
577  std::cerr << "[" << m_profile.instance_name << "] Updating reference moment [" << arm << "]" << std::endl;
578  std::cerr << "[" << m_profile.instance_name << "] diff foot origin ext moment = " << df.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[Nm], interpolation_time = " << interpolation_time << "[s]" << std::endl;
579  std::cerr << "[" << m_profile.instance_name << "] new foot origin ext moment = " << ref_force[arm_idx].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[Nm]" << std::endl;
580  }
581 };
582 
584 {
585  size_t arm_idx = ee_index_map[arm];
586  size_t rarm_idx = ee_index_map["rarm"];
587  size_t larm_idx = ee_index_map["larm"];
588  hrp::Vector3 input_rarm_ref_force = hrp::Vector3(m_ref_force_in[rarm_idx].data[0], m_ref_force_in[rarm_idx].data[1], m_ref_force_in[rarm_idx].data[2]);
589  hrp::Vector3 input_larm_ref_force = hrp::Vector3(m_ref_force_in[larm_idx].data[0], m_ref_force_in[larm_idx].data[1], m_ref_force_in[larm_idx].data[2]);
590  hrp::Vector3 current_rarm_ref_force = input_rarm_ref_force + ref_force[arm_idx];
591  hrp::Vector3 current_larm_ref_force = input_larm_ref_force - ref_force[arm_idx];
592  //
593  hrp::Vector3 df = hrp::Vector3::Zero();
594  hrp::Vector3 diff_rarm_force = (m_RFUParam["rarm"].act_force_filter->getCurrentValue() - current_rarm_ref_force);
595  if (diff_rarm_force(2) > 0) { // r > a, tarinai
596  df(2) += diff_rarm_force(2);
597  }
598  hrp::Vector3 diff_larm_force = (m_RFUParam["larm"].act_force_filter->getCurrentValue() - current_larm_ref_force);
599  if (diff_larm_force(2) > 0) { // r > a, tarinai
600  df(2) -= diff_larm_force(2);
601  }
602  if (!m_RFUParam[arm].is_hold_value) {
603  ref_force[arm_idx] += hrp::Vector3(0,0,((m_RFUParam[arm].p_gain * transition_interpolator_ratio[arm_idx]) * df)(2));
604  }
605  double interpolation_time = (1/m_RFUParam[arm].update_freq) * m_RFUParam[arm].update_time_ratio;
606  if ( ref_force_interpolator[arm]->isEmpty() ) {
607  ref_force_interpolator[arm]->setGoal(ref_force[arm_idx].data(), interpolation_time, true);
608  }
609  if ( DEBUGP ) {
610  std::cerr << "[" << m_profile.instance_name << "] Updating reference res moment [" << arm << "]" << std::endl;
611  std::cerr << "[" << m_profile.instance_name << "] df " << df.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[Nm], interpolation_time = " << interpolation_time << "[s]" << std::endl;
612  std::cerr << "[" << m_profile.instance_name << "] buffer = " << ref_force[arm_idx].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[Nm]" << std::endl;
613  std::cerr << "[" << m_profile.instance_name << "] diff_rarm_force = " << diff_rarm_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]"))
614  << ", act_rarm_force = " << m_RFUParam["rarm"].act_force_filter->getCurrentValue().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]"))
615  << ", ref_rarm_force = " << current_rarm_ref_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]"))
616  << ", input_rarm_ref_force = " << input_rarm_ref_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]"))
617  << "[N]" << std::endl;
618  std::cerr << "[" << m_profile.instance_name << "] diff_larm_force = " << diff_larm_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]"))
619  << ", act_larm_force = " << m_RFUParam["larm"].act_force_filter->getCurrentValue().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]"))
620  << ", ref_larm_force = " << current_larm_ref_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]"))
621  << ", input_larm_ref_force = " << input_larm_ref_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]"))
622  << "[N]" << std::endl;
623  }
624 };
625 
626 void ReferenceForceUpdater::updateRefForces (const std::string& arm)
627 {
628  hrp::Vector3 internal_force = hrp::Vector3::Zero();
629  double interpolation_time = 0;
630  size_t arm_idx = ee_index_map[arm];
631  hrp::Link* target_link = m_robot->link(ee_map[arm].target_name);
632  hrp::Vector3 abs_motion_dir, df;
633  hrp::Matrix33 ee_rot;
634  ee_rot = target_link->R * ee_map[arm].localR;
635  if ( m_RFUParam[arm].frame=="local" )
636  abs_motion_dir = ee_rot * m_RFUParam[arm].motion_dir;
637  else {
638  hrp::Matrix33 current_foot_mid_rot;
639  std::vector<hrp::Matrix33> foot_rot;
640  std::vector<std::string> leg_names;
641  leg_names.push_back("rleg");
642  leg_names.push_back("lleg");
643  for (size_t i = 0; i < leg_names.size(); i++) {
644  hrp::Link* target_link = m_robot->link(ee_map[leg_names[i]].target_name);
645  foot_rot.push_back(target_link->R * ee_map[leg_names[i]].localR);
646  }
647  rats::mid_rot(current_foot_mid_rot, 0.5, foot_rot[0], foot_rot[1]);
648  abs_motion_dir = current_foot_mid_rot * m_RFUParam[arm].motion_dir;
649  }
650  // Calc abs force diff
651  df = m_RFUParam[arm].act_force_filter->getCurrentValue() - ref_force[arm_idx];
652  double inner_product = 0;
653  if ( ! std::fabs((abs_motion_dir.norm() - 0.0)) < 1e-5 ) {
654  abs_motion_dir.normalize();
655  inner_product = df.dot(abs_motion_dir);
656  if ( ! (inner_product < 0 && ref_force[arm_idx].dot(abs_motion_dir) < 0.0) ) {
657  hrp::Vector3 in_f = ee_rot * internal_force;
658  if (!m_RFUParam[arm].is_hold_value)
659  ref_force[arm_idx] = ref_force[arm_idx].dot(abs_motion_dir) * abs_motion_dir + in_f + (m_RFUParam[arm].p_gain * inner_product * transition_interpolator_ratio[arm_idx]) * abs_motion_dir;
660  interpolation_time = (1/m_RFUParam[arm].update_freq) * m_RFUParam[arm].update_time_ratio;
661  if ( ref_force_interpolator[arm]->isEmpty() ) {
662  ref_force_interpolator[arm]->setGoal(ref_force[arm_idx].data(), interpolation_time, true);
663  }
664  }
665  }
666  if ( DEBUGP ) {
667  std::cerr << "[" << m_profile.instance_name << "] Updating reference force [" << arm << "]" << std::endl;
668  std::cerr << "[" << m_profile.instance_name << "] inner_product = " << inner_product << "[N], ref_force = " << ref_force[arm_idx].dot(abs_motion_dir) << "[N], interpolation_time = " << interpolation_time << "[s]" << std::endl;
669  std::cerr << "[" << m_profile.instance_name << "] new ref_force = " << ref_force[arm_idx].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[N]" << std::endl;
670  std::cerr << "[" << m_profile.instance_name << "] filtered act_force = " << m_RFUParam[arm].act_force_filter->getCurrentValue().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[N]" << std::endl;
671  std::cerr << "[" << m_profile.instance_name << "] df = " << df.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[N]" << std::endl;
672  }
673 };
674 
675 /*
676 RTC::ReturnCode_t ReferenceForceUpdater::onAborting(RTC::UniqueId ec_id)
677 {
678 return RTC::RTC_OK;
679 }
680 */
681 
682 /*
683 RTC::ReturnCode_t ReferenceForceUpdater::onError(RTC::UniqueId ec_id)
684 {
685  return RTC::RTC_OK;
686 }
687 */
688 
689 /*
690 RTC::ReturnCode_t ReferenceForceUpdater::onReset(RTC::UniqueId ec_id)
691 {
692  return RTC::RTC_OK;
693 }
694 */
695 
696 /*
697 RTC::ReturnCode_t ReferenceForceUpdater::onStateUpdate(RTC::UniqueId ec_id)
698 {
699  return RTC::RTC_OK;
700 }
701 */
702 
703 /*
704 RTC::ReturnCode_t ReferenceForceUpdater::onRateChanged(RTC::UniqueId ec_id)
705 {
706  return RTC::RTC_OK;
707 }
708 */
709 
710 bool ReferenceForceUpdater::setReferenceForceUpdaterParam(const std::string& i_name_, const OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam& i_param)
711 {
712  std::string arm = std::string(i_name_);
713  std::cerr << "[" << m_profile.instance_name << "] setReferenceForceUpdaterParam [" << i_name_ << "]" << std::endl;
714  Guard guard(m_mutex);
715  if ( m_RFUParam.find(i_name_) == m_RFUParam.end() ) {
716  std::cerr << "[" << m_profile.instance_name << "] Could not found reference force updater param [" << i_name_ << "]" << std::endl;
717  return false;
718  }
719  if ( std::string(i_param.frame) != "local" && std::string(i_param.frame) != "world" ) {
720  std::cerr << "[" << m_profile.instance_name << "] \"frame\" parameter must be local/world. could not set \"" << std::string(i_param.frame) << "\"" <<std::endl;
721  return false;
722  }
723  // Parameters which cannot be changed when active
724  if ( m_RFUParam[arm].is_active ) { // When active, check parameter changing, and if changed, do not change the value.
725  if ( !eps_eq(m_RFUParam[arm].update_freq, i_param.update_freq) ) {
726  std::cerr << "[" << m_profile.instance_name << "] Could not set update_freq because rfu [" << i_name_ << "] is active (current = " << m_RFUParam[arm].update_freq << ", new = " << i_param.update_freq << ")" << std::endl;
727  return false;
728  } else {
729  m_RFUParam[arm].update_freq = i_param.update_freq;
730  }
731  if ( !eps_eq(m_RFUParam[arm].update_time_ratio, i_param.update_time_ratio) ) {
732  std::cerr << "[" << m_profile.instance_name << "] Could not set update_time_ratio because rfu [" << i_name_ << "] is active (current = " << m_RFUParam[arm].update_time_ratio << ", new = " << i_param.update_time_ratio << ")" << std::endl;
733  return false;
734  } else {
735  m_RFUParam[arm].update_time_ratio = i_param.update_time_ratio;
736  }
737  if ( m_RFUParam[arm].frame != std::string(i_param.frame) ) {
738  std::cerr << "[" << m_profile.instance_name << "] Could not set frame because rfu [" << i_name_ << "] is active (current = " << m_RFUParam[arm].frame << ", new = " << i_param.frame << ")" << std::endl;
739  return false;
740  } else {
741  m_RFUParam[arm].frame = i_param.frame;
742  }
743  } else { // When not active, update parameters
744  m_RFUParam[arm].update_freq = i_param.update_freq;
745  m_RFUParam[arm].update_time_ratio = i_param.update_time_ratio;
746  m_RFUParam[arm].update_count=round((1/m_RFUParam[arm].update_freq)/m_dt);
747  m_RFUParam[arm].frame=std::string(i_param.frame);
748  }
749  // Parameters which can be changed regardless of active/inactive
750  m_RFUParam[arm].p_gain = i_param.p_gain;
751  m_RFUParam[arm].d_gain = i_param.d_gain;
752  m_RFUParam[arm].i_gain = i_param.i_gain;
753  m_RFUParam[arm].is_hold_value = i_param.is_hold_value;
754  m_RFUParam[arm].transition_time = i_param.transition_time;
755  m_RFUParam[arm].act_force_filter->setCutOffFreq(i_param.cutoff_freq);
756  for (size_t i = 0; i < 3; i++ ) m_RFUParam[arm].motion_dir(i) = i_param.motion_dir[i];
757 
758  // Print values
759  m_RFUParam[arm].printParam(std::string(m_profile.instance_name));
760  return true;
761 };
762 
763 bool ReferenceForceUpdater::getReferenceForceUpdaterParam(const std::string& i_name_, OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam_out i_param)
764 {
765  std::string arm = std::string(i_name_);
766  std::cerr << "[" << m_profile.instance_name << "] getReferenceForceUpdaterParam [" << i_name_ << "]" << std::endl;
767  if ( m_RFUParam.find(i_name_) == m_RFUParam.end() ) {
768  std::cerr << "[" << m_profile.instance_name << "] Could not found reference force updater param [" << i_name_ << "]" << std::endl;
769  return false;
770  }
771  Guard guard(m_mutex);
772  i_param->p_gain = m_RFUParam[arm].p_gain;
773  i_param->d_gain = m_RFUParam[arm].d_gain;
774  i_param->i_gain = m_RFUParam[arm].i_gain;
775  i_param->update_freq = m_RFUParam[arm].update_freq;
776  i_param->update_time_ratio = m_RFUParam[arm].update_time_ratio;
777  i_param->frame = m_RFUParam[arm].frame.c_str();
778  i_param->is_hold_value = m_RFUParam[arm].is_hold_value;
779  i_param->transition_time = m_RFUParam[arm].transition_time;
780  i_param->cutoff_freq = m_RFUParam[arm].act_force_filter->getCutOffFreq();
781  for (size_t i = 0; i < 3; i++ ) i_param->motion_dir[i] = m_RFUParam[arm].motion_dir(i);
782  return true;
783 };
784 
786 {
787  std::cerr << "[" << m_profile.instance_name << "] startReferenceForceUpdater [" << i_name_ << "]" << std::endl;
788  {
789  Guard guard(m_mutex);
790  if ( m_RFUParam.find(i_name_) == m_RFUParam.end() ) {
791  std::cerr << "[" << m_profile.instance_name << "] Could not found reference force updater param [" << i_name_ << "]" << std::endl;
792  return false;
793  }
794  if ( m_RFUParam[i_name_].is_active == true )
795  return true;
796  if (transition_interpolator[i_name_]->isEmpty()) {
797  m_RFUParam[i_name_].is_active = true;
798  double tmpstart = 0.0, tmpgoal = 1.0;
799  size_t arm_idx = ee_index_map[i_name_];
800  hrp::Vector3 currentRefForce;
801  if ( std::string(i_name_) == footoriginextmoment_name ) {
803  } else if ( std::string(i_name_) == objextmoment0_name ) {
804  currentRefForce = hrp::Vector3::Zero();
805  } else {
806  currentRefForce = hrp::Vector3( m_ref_force_in[arm_idx].data[0], m_ref_force_in[arm_idx].data[1], m_ref_force_in[arm_idx].data[2] );
807  }
808  ref_force_interpolator[i_name_]->set(currentRefForce.data());
809  transition_interpolator[i_name_]->set(&tmpstart);
810  transition_interpolator[i_name_]->setGoal(&tmpgoal, m_RFUParam[i_name_].transition_time, true);
811  } else {
812  return false;
813  }
814  }
815  return true;
816 };
817 
819 {
820  std::cerr << "[" << m_profile.instance_name << "] stopReferenceForceUpdater [" << i_name_ << "]" << std::endl;
821  {
822  Guard guard(m_mutex);
823  if ( m_RFUParam.find(i_name_) == m_RFUParam.end() ) {
824  std::cerr << "[" << m_profile.instance_name << "] Could not found reference force updater param [" << i_name_ << "]" << std::endl;
825  return false;
826  }
827  if ( m_RFUParam[i_name_].is_active == false ){//already not active
828  return true;
829  }
830  double tmpstart = 1.0, tmpgoal = 0.0;
831  transition_interpolator[i_name_]->set(&tmpstart);
832  transition_interpolator[i_name_]->setGoal(&tmpgoal, m_RFUParam[i_name_].transition_time, true);
833  m_RFUParam[i_name_].is_stopping = true;
834  }
835  return true;
836 };
837 
839 {
840  bool ret = startReferenceForceUpdaterNoWait(i_name_);
841  if (ret) waitReferenceForceUpdaterTransition(i_name_);
842  return ret;
843 };
844 
845 bool ReferenceForceUpdater::stopReferenceForceUpdater(const std::string& i_name_)
846 {
847  bool ret = stopReferenceForceUpdaterNoWait(i_name_);
848  if (ret) waitReferenceForceUpdaterTransition(i_name_);
849  return ret;
850 };
851 
853 {
854  while (!transition_interpolator[i_name_]->isEmpty()) usleep(1000);
855  usleep(1000);
856 };
857 
858 bool ReferenceForceUpdater::getSupportedReferenceForceUpdaterNameSequence(OpenHRP::ReferenceForceUpdaterService::StrSequence_out o_names)
859 {
860  std::cerr << "[" << m_profile.instance_name << "] getSupportedReferenceForceUpdaterNameSequence" << std::endl;
861  Guard guard(m_mutex);
862  o_names->length(m_RFUParam.size());
863  size_t i = 0;
864  for (std::map<std::string, ReferenceForceUpdaterParam>::iterator itr = m_RFUParam.begin(); itr != m_RFUParam.end(); itr++ ) {
865  o_names[i] = itr->first.c_str();
866  i++;
867  }
868  return true;
869 };
870 
871 extern "C"
872 {
873 
875  {
877  manager->registerFactory(profile,
878  RTC::Create<ReferenceForceUpdater>,
879  RTC::Delete<ReferenceForceUpdater>);
880  }
881 
882 };
ComponentProfile m_profile
png_infop png_charpp int png_charpp profile
std::map< std::string, hrp::VirtualForceSensorParam > m_vfs
bool stopReferenceForceUpdaterNoWait(const std::string &i_name_)
InPort< TimedOrientation3D > m_baseRpyIn
OutPort< TimedPoint3D > m_refFootOriginExtMomentOut
std::string name
const std::string objextmoment0_name
#define DEBUGP
bool stringTo(To &val, const char *str)
void calcFootOriginCoords(hrp::Vector3 &foot_origin_pos, hrp::Matrix33 &foot_origin_rot)
hrp::Matrix33 rot
Definition: RatsMatrix.h:20
InPort< TimedOrientation3D > m_rpyIn
coil::Guard< coil::Mutex > Guard
InPort< TimedPoint3D > m_diffFootOriginExtMomentIn
TimedOrientation3D m_baseRpy
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
std::map< std::string, ee_trans > ee_map
ReferenceForceUpdater(RTC::Manager *manager)
Constructor.
vstring split(const std::string &input, const std::string &delimiter, bool ignore_empty)
InPort< TimedPoint3D > m_basePosIn
void waitReferenceForceUpdaterTransition(const std::string &i_name_)
std::vector< TimedDoubleSeq > m_ref_force_out
ReferenceForceUpdaterService_impl m_ReferenceForceUpdaterService
CORBA::ORB_ptr getORB()
png_uint_32 i
coil::Properties & getProperties()
static Manager & instance()
Matrix33 localR
TimedBoolean m_refFootOriginExtMomentIsHoldValue
bool addOutPort(const char *name, OutPortBase &outport)
bool stopReferenceForceUpdater(const std::string &i_name_)
boost::shared_ptr< Body > BodyPtr
std::vector< InPort< TimedDoubleSeq > * > m_ref_forceIn
virtual ~ReferenceForceUpdater()
Destructor.
Eigen::Vector3d Vector3
void ReferenceForceUpdaterInit(RTC::Manager *manager)
Matrix33 rotFromRpy(const Vector3 &rpy)
Eigen::Matrix3d Matrix33
static int frame
Definition: iob.cpp:19
std::vector< std::string > vstring
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
coil::Properties & getConfig()
void rotm3times(hrp::Matrix33 &m12, const hrp::Matrix33 &m1, const hrp::Matrix33 &m2)
Definition: RatsMatrix.cpp:31
ExecutionContextHandle_t UniqueId
bool bindParameter(const char *param_name, VarType &var, const char *def_val, bool(*trans)(VarType &, const char *)=coil::stringTo)
bool startReferenceForceUpdaterNoWait(const std::string &i_name_)
std::map< std::string, size_t > ee_index_map
std::map< std::string, interpolator * > ref_force_interpolator
bool getReferenceForceUpdaterParam(const std::string &i_name_, OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam_out i_param)
int loadBodyFromModelLoader(::World *world, const char *name, const char *url, CosNaming::NamingContext_var cxt)
std::vector< TimedDoubleSeq > m_force
std::vector< TimedDoubleSeq > m_ref_force_in
bool getSupportedReferenceForceUpdaterNameSequence(OpenHRP::ReferenceForceUpdaterService::StrSequence_out o_names)
std::vector< InPort< TimedDoubleSeq > * > m_forceIn
RTC::CorbaPort m_ReferenceForceUpdaterServicePort
bool startReferenceForceUpdater(const std::string &i_name_)
prop
static std::ostream & operator<<(std::ostream &os, const struct RTC::Time &tm)
void readVirtualForceSensorParamFromProperties(std::map< std::string, hrp::VirtualForceSensorParam > &vfs, hrp::BodyPtr m_robot, const std::string &prop_string, const std::string &instance_name)
naming
double dot(const Vector3 &v1, const Vector3 &v2)
InPort< TimedDoubleSeq > m_qRefIn
bool eps_eq(const double a, const double b, const double eps=1e-3)
bool setReferenceForceUpdaterParam(const std::string &i_name_, const OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam &i_param)
TimedPoint3D m_diffFootOriginExtMoment
void registerInPort(const char *name, InPortBase &inport)
virtual bool isNew()
bool addPort(PortBase &port)
virtual bool write(DataType &value)
OutPort< TimedBoolean > m_refFootOriginExtMomentIsHoldValueOut
hrp::BodyPtr m_robot
hrp::Vector3 pos
Definition: RatsMatrix.h:19
JSAMPIMAGE data
int num
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
std::vector< OutPort< TimedDoubleSeq > * > m_ref_forceOut
std::vector< double > transition_interpolator_ratio
void updateRefForces(const std::string &arm)
bool addInPort(const char *name, InPortBase &inport)
virtual RTC::ReturnCode_t onInitialize()
void mid_rot(hrp::Matrix33 &mid_rot, const double p, const hrp::Matrix33 &rot1, const hrp::Matrix33 &rot2, const double eps)
Definition: RatsMatrix.cpp:46
void registerOutPort(const char *name, OutPortBase &outport)
void updateRefObjExtMoment0(const std::string &arm)
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
std::vector< hrp::Vector3 > ref_force
const std::string footoriginextmoment_name
std::map< std::string, ReferenceForceUpdaterParam > m_RFUParam
std::map< std::string, interpolator * > transition_interpolator
virtual RTC::ReturnCode_t onFinalize()
void updateRefFootOriginExtMoment(const std::string &arm)
static const char * ReferenceForceUpdater_spec[]
void mid_coords(coordinates &mid_coords, const double p, const coordinates &c1, const coordinates &c2, const double eps)
Definition: RatsMatrix.cpp:58
int usleep(useconds_t usec)
bool registerProvider(const char *instance_name, const char *type_name, PortableServer::RefCountServantBase &provider)
ReferenceForceUpdater.
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)


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