AutoBalancer.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 "AutoBalancer.h"
15 #include <hrpModel/JointPath.h>
16 #include <hrpUtil/MatrixSolvers.h>
17 #include "hrpsys/util/Hrpsys.h"
18 
19 
21 using namespace rats;
22 
23 // Module specification
24 // <rtc-template block="module_spec">
25 static const char* autobalancer_spec[] =
26  {
27  "implementation_id", "AutoBalancer",
28  "type_name", "AutoBalancer",
29  "description", "autobalancer component",
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_zmpIn("zmpIn", m_zmp),
61  m_optionalDataIn("optionalData", m_optionalData),
62  m_emergencySignalIn("emergencySignal", m_emergencySignal),
63  m_diffCPIn("diffCapturePoint", m_diffCP),
64  m_refFootOriginExtMomentIn("refFootOriginExtMoment", m_refFootOriginExtMoment),
65  m_refFootOriginExtMomentIsHoldValueIn("refFootOriginExtMomentIsHoldValue", m_refFootOriginExtMomentIsHoldValue),
66  m_actContactStatesIn("actContactStates", m_actContactStates),
67  m_qOut("q", m_qRef),
68  m_zmpOut("zmpOut", m_zmp),
69  m_basePosOut("basePosOut", m_basePos),
70  m_baseRpyOut("baseRpyOut", m_baseRpy),
71  m_baseTformOut("baseTformOut", m_baseTform),
72  m_basePoseOut("basePoseOut", m_basePose),
73  m_accRefOut("accRef", m_accRef),
74  m_contactStatesOut("contactStates", m_contactStates),
75  m_toeheelRatioOut("toeheelRatio", m_toeheelRatio),
76  m_controlSwingSupportTimeOut("controlSwingSupportTime", m_controlSwingSupportTime),
77  m_walkingStatesOut("walkingStates", m_walkingStates),
78  m_sbpCogOffsetOut("sbpCogOffset", m_sbpCogOffset),
79  m_cogOut("cogOut", m_cog),
80  m_AutoBalancerServicePort("AutoBalancerService"),
81  // </rtc-template>
82  gait_type(BIPED),
83  m_robot(hrp::BodyPtr()),
84  m_debugLevel(0)
85 {
87 }
88 
90 {
91 }
92 
93 
94 RTC::ReturnCode_t AutoBalancer::onInitialize()
95 {
96  std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
97  bindParameter("debugLevel", m_debugLevel, "0");
98 
99  // Registration: InPort/OutPort/Service
100  // <rtc-template block="registration">
101  // Set InPort buffers
102  addInPort("qRef", m_qRefIn);
103  addInPort("basePosIn", m_basePosIn);
104  addInPort("baseRpyIn", m_baseRpyIn);
105  addInPort("zmpIn", m_zmpIn);
106  addInPort("optionalData", m_optionalDataIn);
107  addInPort("emergencySignal", m_emergencySignalIn);
108  addInPort("diffCapturePoint", m_diffCPIn);
109  addInPort("actContactStates", m_actContactStatesIn);
110  addInPort("refFootOriginExtMoment", m_refFootOriginExtMomentIn);
111  addInPort("refFootOriginExtMomentIsHoldValue", m_refFootOriginExtMomentIsHoldValueIn);
112 
113  // Set OutPort buffer
114  addOutPort("q", m_qOut);
115  addOutPort("zmpOut", m_zmpOut);
116  addOutPort("basePosOut", m_basePosOut);
117  addOutPort("baseRpyOut", m_baseRpyOut);
118  addOutPort("baseTformOut", m_baseTformOut);
119  addOutPort("basePoseOut", m_basePoseOut);
120  addOutPort("accRef", m_accRefOut);
121  addOutPort("contactStates", m_contactStatesOut);
122  addOutPort("toeheelRatio", m_toeheelRatioOut);
123  addOutPort("controlSwingSupportTime", m_controlSwingSupportTimeOut);
124  addOutPort("cogOut", m_cogOut);
125  addOutPort("walkingStates", m_walkingStatesOut);
126  addOutPort("sbpCogOffset", m_sbpCogOffsetOut);
127 
128  // Set service provider to Ports
129  m_AutoBalancerServicePort.registerProvider("service0", "AutoBalancerService", m_service0);
130 
131  // Set service consumers to Ports
132 
133  // Set CORBA Service Ports
135 
136  // </rtc-template>
137  // <rtc-template block="bind_config">
138  // Bind variables and configuration variable
139 
140  // </rtc-template>
141 
143  coil::stringTo(m_dt, prop["dt"].c_str());
144 
145  m_robot = hrp::BodyPtr(new hrp::Body());
146  RTC::Manager& rtcManager = RTC::Manager::instance();
147  std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
148  int comPos = nameServer.find(",");
149  if (comPos < 0){
150  comPos = nameServer.length();
151  }
152  nameServer = nameServer.substr(0, comPos);
153  RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
154  if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
155  CosNaming::NamingContext::_duplicate(naming.getRootContext())
156  )){
157  std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
158  return RTC::RTC_ERROR;
159  }
160 
161  // allocate memory for outPorts
162  m_qRef.data.length(m_robot->numJoints());
163  m_baseTform.data.length(12);
164 
166  loop = 0;
167 
168  // setting from conf file
169  // GaitGenerator requires abc_leg_offset and abc_stride_parameter in robot conf file
170  // setting leg_pos from conf file
171  coil::vstring leg_offset_str = coil::split(prop["abc_leg_offset"], ",");
172  leg_names.push_back("rleg");
173  leg_names.push_back("lleg");
174 
175  // Generate FIK
176  fik = fikPtr(new SimpleFullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt));
177 
178  // setting from conf file
179  // rleg,TARGET_LINK,BASE_LINK
180  coil::vstring end_effectors_str = coil::split(prop["end_effectors"], ",");
181  size_t prop_num = 10;
182  if (end_effectors_str.size() > 0) {
183  size_t num = end_effectors_str.size()/prop_num;
184  for (size_t i = 0; i < num; i++) {
185  std::string ee_name, ee_target, ee_base;
186  coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
187  coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
188  coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
189  ABCIKparam tp;
190  hrp::Link* root = m_robot->link(ee_target);
191  for (size_t j = 0; j < 3; j++) {
192  coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str());
193  }
194  double tmpv[4];
195  for (int j = 0; j < 4; j++ ) {
196  coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
197  }
198  tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
199  // FIK param
201  tmp_fikp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false, std::string(m_profile.instance_name)));
202  tmp_fikp.target_link = m_robot->link(ee_target);
203  tmp_fikp.localPos = tp.localPos;
204  tmp_fikp.localR = tp.localR;
205  tmp_fikp.max_limb_length = 0.0;
206  while (!root->isRoot()) {
207  tmp_fikp.max_limb_length += root->b.norm();
208  tmp_fikp.parent_name = root->name;
209  root = root->parent;
210  }
211  fik->ikp.insert(std::pair<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>(ee_name, tmp_fikp));
212  // Fix for toe joint
213  // Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link
214  // Without toe joints, "end-effector link == force-sensor link" is assumed.
215  // With toe joints, "end-effector link != force-sensor link" is assumed.
216  if (m_robot->link(ee_target)->sensors.size() == 0) { // If end-effector link has no force sensor
217  std::vector<double> optw(fik->ikp[ee_name].manip->numJoints(), 1.0);
218  optw.back() = 0.0; // Set weight = 0 for toe joint by default
219  fik->ikp[ee_name].manip->setOptionalWeightVector(optw);
220  tp.has_toe_joint = true;
221  } else {
222  tp.has_toe_joint = false;
223  }
224  tp.target_link = m_robot->link(ee_target);
225  ikp.insert(std::pair<std::string, ABCIKparam>(ee_name , tp));
226  ee_vec.push_back(ee_name);
227  std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
228  std::cerr << "[" << m_profile.instance_name << "] target = " << ikp[ee_name].target_link->name << ", base = " << ee_base << std::endl;
229  std::cerr << "[" << m_profile.instance_name << "] offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl;
230  std::cerr << "[" << m_profile.instance_name << "] has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl;
231  contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));
232  }
233  m_contactStates.data.length(num);
234  m_toeheelRatio.data.length(num);
235  if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
236  m_contactStates.data[contact_states_index_map["rleg"]] = true;
237  m_contactStates.data[contact_states_index_map["lleg"]] = true;
238  }
239  if (ikp.find("rarm") != ikp.end() && ikp.find("larm") != ikp.end()) {
240  m_contactStates.data[contact_states_index_map["rarm"]] = false;
241  m_contactStates.data[contact_states_index_map["larm"]] = false;
242  }
243  m_controlSwingSupportTime.data.length(num);
244  for (size_t i = 0; i < num; i++) m_controlSwingSupportTime.data[i] = 1.0;
245  for (size_t i = 0; i < num; i++) m_toeheelRatio.data[i] = rats::no_using_toe_heel_ratio;
246  }
247  std::vector<hrp::Vector3> leg_pos;
248  if (leg_offset_str.size() > 0) {
249  hrp::Vector3 leg_offset;
250  for (size_t i = 0; i < 3; i++) coil::stringTo(leg_offset(i), leg_offset_str[i].c_str());
251  std::cerr << "[" << m_profile.instance_name << "] abc_leg_offset = " << leg_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl;
252  leg_pos.push_back(hrp::Vector3(-1*leg_offset));
253  leg_pos.push_back(hrp::Vector3(leg_offset));
254  }
255  if (leg_pos.size() < ikp.size()) {
256  size_t tmp_leg_pos_size = leg_pos.size();
257  for (size_t i = 0; i < ikp.size() - tmp_leg_pos_size; i++) {
258  leg_pos.push_back(hrp::Vector3::Zero());
259  }
260  }
261 
262  std::vector<std::pair<hrp::Link*, hrp::Link*> > interlocking_joints;
263  readInterlockingJointsParamFromProperties(interlocking_joints, m_robot, prop["interlocking_joints"], std::string(m_profile.instance_name));
264  if (interlocking_joints.size() > 0) {
265  fik->initializeInterlockingJoints(interlocking_joints);
266  }
267 
268  zmp_offset_interpolator = new interpolator(ikp.size()*3, m_dt);
269  zmp_offset_interpolator->setName(std::string(m_profile.instance_name)+" zmp_offset_interpolator");
270  zmp_transition_time = 1.0;
272  transition_interpolator->setName(std::string(m_profile.instance_name)+" transition_interpolator");
275  adjust_footstep_interpolator->setName(std::string(m_profile.instance_name)+" adjust_footstep_interpolator");
276  transition_time = 2.0;
279  leg_names_interpolator->setName(std::string(m_profile.instance_name)+" leg_names_interpolator");
281 
282  // setting stride limitations from conf file
283  double stride_fwd_x_limit = 0.15;
284  double stride_outside_y_limit = 0.05;
285  double stride_outside_th_limit = 10;
286  double stride_bwd_x_limit = 0.05;
287  double stride_inside_y_limit = stride_outside_y_limit*0.5;
288  double stride_inside_th_limit = stride_outside_th_limit*0.5;
289  std::cerr << "[" << m_profile.instance_name << "] abc_stride_parameter : " << stride_fwd_x_limit << "[m], " << stride_outside_y_limit << "[m], " << stride_outside_th_limit << "[deg], " << stride_bwd_x_limit << "[m]" << std::endl;
290  if (default_zmp_offsets.size() == 0) {
291  for (size_t i = 0; i < ikp.size(); i++) default_zmp_offsets.push_back(hrp::Vector3::Zero());
292  }
293  if (leg_offset_str.size() > 0) {
294  gg = ggPtr(new rats::gait_generator(m_dt, leg_pos, leg_names, stride_fwd_x_limit/*[m]*/, stride_outside_y_limit/*[m]*/, stride_outside_th_limit/*[deg]*/,
295  stride_bwd_x_limit/*[m]*/, stride_inside_y_limit/*[m]*/, stride_inside_th_limit/*[m]*/));
296  gg->set_default_zmp_offsets(default_zmp_offsets);
297  }
298  gg_is_walking = gg_solved = false;
299  m_walkingStates.data = false;
301 
302  // load virtual force sensors
303  readVirtualForceSensorParamFromProperties(m_vfs, m_robot, prop["virtual_force_sensor"], std::string(m_profile.instance_name));
304  // ref force port
305  unsigned int npforce = m_robot->numSensors(hrp::Sensor::FORCE);
306  unsigned int nvforce = m_vfs.size();
307  unsigned int nforce = npforce + nvforce;
308  // check number of force sensors
309  if (nforce < m_contactStates.data.length()) {
310  std::cerr << "[" << m_profile.instance_name << "] WARNING! This robot model has less force sensors(" << nforce;
311  std::cerr << ") than end-effector settings(" << m_contactStates.data.length() << ") !" << std::endl;
312  }
313 
314  m_ref_force.resize(nforce);
315  m_ref_forceIn.resize(nforce);
316  m_force.resize(nforce);
317  m_ref_forceOut.resize(nforce);
318  m_limbCOPOffset.resize(nforce);
319  m_limbCOPOffsetOut.resize(nforce);
320  for (unsigned int i=0; i<npforce; i++){
321  sensor_names.push_back(m_robot->sensor(hrp::Sensor::FORCE, i)->name);
322  }
323  for (unsigned int i=0; i<nvforce; i++){
324  for ( std::map<std::string, hrp::VirtualForceSensorParam>::iterator it = m_vfs.begin(); it != m_vfs.end(); it++ ) {
325  if (it->second.id == (int)i) sensor_names.push_back(it->first);
326  }
327  }
328  // set ref force port
329  std::cerr << "[" << m_profile.instance_name << "] force sensor ports (" << nforce << ")" << std::endl;
330  for (unsigned int i=0; i<nforce; i++){
331  m_ref_forceIn[i] = new InPort<TimedDoubleSeq>(std::string("ref_"+sensor_names[i]).c_str(), m_ref_force[i]);
332  m_ref_force[i].data.length(6);
333  registerInPort(std::string("ref_"+sensor_names[i]).c_str(), *m_ref_forceIn[i]);
334  std::cerr << "[" << m_profile.instance_name << "] name = " << std::string("ref_"+sensor_names[i]) << std::endl;
335  ref_forces.push_back(hrp::Vector3(0,0,0));
336  ref_moments.push_back(hrp::Vector3(0,0,0));
337  }
338  // set force port
339  for (unsigned int i=0; i<nforce; i++){
340  m_ref_forceOut[i] = new OutPort<TimedDoubleSeq>(std::string(sensor_names[i]).c_str(), m_force[i]);
341  m_force[i].data.length(6);
342  m_force[i].data[0] = m_force[i].data[1] = m_force[i].data[2] = 0.0;
343  m_force[i].data[3] = m_force[i].data[4] = m_force[i].data[5] = 0.0;
344  registerOutPort(std::string(sensor_names[i]).c_str(), *m_ref_forceOut[i]);
345  std::cerr << "[" << m_profile.instance_name << "] name = " << std::string(sensor_names[i]) << std::endl;
346  }
347  // set limb cop offset port
348  std::cerr << "[" << m_profile.instance_name << "] limbCOPOffset ports (" << nforce << ")" << std::endl;
349  for (unsigned int i=0; i<nforce; i++){
350  std::string nm("limbCOPOffset_"+sensor_names[i]);
352  registerOutPort(nm.c_str(), *m_limbCOPOffsetOut[i]);
353  m_limbCOPOffset[i].data.x = m_limbCOPOffset[i].data.y = m_limbCOPOffset[i].data.z = 0.0;
354  std::cerr << "[" << m_profile.instance_name << "] name = " << nm << std::endl;
355  }
356  sbp_offset = hrp::Vector3(0,0,0);
357  sbp_cog_offset = hrp::Vector3(0,0,0);
358  //use_force = MODE_NO_FORCE;
360 
361  if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) {
362  is_legged_robot = true;
363  } else {
364  is_legged_robot = false;
365  }
366 
367  m_accRef.data.ax = m_accRef.data.ay = m_accRef.data.az = 0.0;
368  prev_imu_sensor_vel = hrp::Vector3::Zero();
369 
370  graspless_manip_mode = false;
371  graspless_manip_arm = "arms";
372  graspless_manip_p_gain = hrp::Vector3::Zero();
373 
374  is_stop_mode = false;
375  is_hand_fix_mode = false;
376 
377  hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
378  if (sen == NULL) {
379  std::cerr << "[" << m_profile.instance_name << "] WARNING! This robot model has no GyroSensor named 'gyrometer'! " << std::endl;
380  }
381 
383  additional_force_applied_point_offset = hrp::Vector3::Zero();
384  return RTC::RTC_OK;
385 }
386 
387 
388 
389 RTC::ReturnCode_t AutoBalancer::onFinalize()
390 {
394  delete leg_names_interpolator;
395  return RTC::RTC_OK;
396 }
397 
398 /*
399  RTC::ReturnCode_t AutoBalancer::onStartup(RTC::UniqueId ec_id)
400  {
401  return RTC::RTC_OK;
402  }
403 */
404 
405 /*
406  RTC::ReturnCode_t AutoBalancer::onShutdown(RTC::UniqueId ec_id)
407  {
408  return RTC::RTC_OK;
409  }
410 */
411 
412 RTC::ReturnCode_t AutoBalancer::onActivated(RTC::UniqueId ec_id)
413 {
414  std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
415 
416  return RTC::RTC_OK;
417 }
418 
420 {
421  std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
422  Guard guard(m_mutex);
423  if (control_mode == MODE_ABC) {
425  double tmp_ratio = 0.0;
426  transition_interpolator->setGoal(&tmp_ratio, m_dt, true); // sync in one controller loop
427  }
428  return RTC::RTC_OK;
429 }
430 
431 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
432 //#define DEBUGP2 ((loop%200==0))
433 #define DEBUGP2 (false)
434 RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id)
435 {
436  // std::cerr << "AutoBalancer::onExecute(" << ec_id << ")" << std::endl;
437  loop ++;
438 
439  // Read Inport
440  if (m_qRefIn.isNew()) {
441  m_qRefIn.read();
442  }
443  if (m_basePosIn.isNew()) {
444  m_basePosIn.read();
445  input_basePos(0) = m_basePos.data.x;
446  input_basePos(1) = m_basePos.data.y;
447  input_basePos(2) = m_basePos.data.z;
448  }
449  if (m_baseRpyIn.isNew()) {
450  m_baseRpyIn.read();
451  input_baseRot = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
452  }
453  if (m_zmpIn.isNew()) {
454  m_zmpIn.read();
455  input_zmp(0) = m_zmp.data.x;
456  input_zmp(1) = m_zmp.data.y;
457  input_zmp(2) = m_zmp.data.z;
458  }
459  for (unsigned int i=0; i<m_ref_forceIn.size(); i++){
460  if ( m_ref_forceIn[i]->isNew() ) {
461  m_ref_forceIn[i]->read();
462  }
463  }
464  if (m_optionalDataIn.isNew()) {
466  }
467  if (m_emergencySignalIn.isNew()){
469  // if (!is_stop_mode) {
470  // std::cerr << "[" << m_profile.instance_name << "] emergencySignal is set!" << std::endl;
471  // is_stop_mode = true;
472  // gg->emergency_stop();
473  // }
474  }
475  if (m_diffCPIn.isNew()) {
476  m_diffCPIn.read();
477  gg->set_diff_cp(hrp::Vector3(m_diffCP.data.x, m_diffCP.data.y, m_diffCP.data.z));
478  }
481  }
484  }
485  if (m_actContactStatesIn.isNew()) {
487  std::vector<bool> tmp_contacts(m_actContactStates.data.length());
488  for (size_t i = 0; i < m_actContactStates.data.length(); i++) {
489  tmp_contacts[i] = m_actContactStates.data[i];
490  }
491  gg->set_act_contact_states(tmp_contacts);
492  }
493 
494  // Calculation
495  Guard guard(m_mutex);
496  hrp::Vector3 ref_basePos;
497  hrp::Matrix33 ref_baseRot;
498  hrp::Vector3 rel_ref_zmp; // ref zmp in base frame
499  if ( is_legged_robot ) {
500  // For parameters
501  fik->storeCurrentParameters();
503  // Get transition ratio
504  bool is_transition_interpolator_empty = transition_interpolator->isEmpty();
505  if (!is_transition_interpolator_empty) {
507  } else {
509  }
510  if (control_mode != MODE_IDLE ) {
511  solveFullbodyIK();
512 // /////// Inverse Dynamics /////////
513 // if(!idsb.is_initialized){
514 // idsb.setInitState(m_robot, m_dt);
515 // invdyn_zmp_filters.resize(3);
516 // for(int i=0;i<3;i++){
517 // invdyn_zmp_filters[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt);
518 // invdyn_zmp_filters[i].reset(ref_zmp(i));
519 // }
520 // }
521 // calcAccelerationsForInverseDynamics(m_robot, idsb);
522 // if(gg_is_walking){
523 // calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp);
524 // for(int i=0;i<3;i++) ref_zmp(i) = invdyn_zmp_filters[i].passFilter(ref_zmp(i));
525 // }
526 // updateInvDynStateBuffer(idsb);
527 
528  rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p);
529  } else {
530  rel_ref_zmp = input_zmp;
531  fik->d_root_height = 0.0;
532  }
533  // Transition
534  if (!is_transition_interpolator_empty) {
535  // transition_interpolator_ratio 0=>1 : IDLE => ABC
536  // transition_interpolator_ratio 1=>0 : ABC => IDLE
538  rel_ref_zmp = (1-transition_interpolator_ratio) * input_zmp + transition_interpolator_ratio * rel_ref_zmp;
539  rats::mid_rot(ref_baseRot, transition_interpolator_ratio, input_baseRot, m_robot->rootLink()->R);
540  for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ) {
541  m_robot->joint(i)->q = (1-transition_interpolator_ratio) * m_qRef.data[i] + transition_interpolator_ratio * m_robot->joint(i)->q;
542  }
543  for (unsigned int i=0; i< m_force.size(); i++) {
544  for (unsigned int j=0; j<6; j++) {
546  }
547  }
548  for (unsigned int i=0; i< m_limbCOPOffset.size(); i++) {
549  // transition (TODO:set stopABCmode value instead of 0)
550  m_limbCOPOffset[i].data.x = transition_interpolator_ratio * m_limbCOPOffset[i].data.x;// + (1-transition_interpolator_ratio) * 0;
551  m_limbCOPOffset[i].data.y = transition_interpolator_ratio * m_limbCOPOffset[i].data.y;// + (1-transition_interpolator_ratio) * 0;
552  m_limbCOPOffset[i].data.z = transition_interpolator_ratio * m_limbCOPOffset[i].data.z;// + (1-transition_interpolator_ratio) * 0;
553  }
554  } else {
555  ref_basePos = m_robot->rootLink()->p;
556  ref_baseRot = m_robot->rootLink()->R;
557  }
558  // mode change for sync
562  std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
563  << "] Finished cleanup" << std::endl;
565  }
566  }
567 
568  // Write Outport
569  if ( m_qRef.data.length() != 0 ) { // initialized
570  if (is_legged_robot) {
571  for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
572  m_qRef.data[i] = m_robot->joint(i)->q;
573  }
574  }
575  m_qOut.write();
576  }
577  if (is_legged_robot) {
578  // basePos
579  m_basePos.data.x = ref_basePos(0);
580  m_basePos.data.y = ref_basePos(1);
581  m_basePos.data.z = ref_basePos(2);
582  m_basePos.tm = m_qRef.tm;
583  // baseRpy
584  hrp::Vector3 baseRpy = hrp::rpyFromRot(ref_baseRot);
585  m_baseRpy.data.r = baseRpy(0);
586  m_baseRpy.data.p = baseRpy(1);
587  m_baseRpy.data.y = baseRpy(2);
588  m_baseRpy.tm = m_qRef.tm;
589  // baseTform
590  double *tform_arr = m_baseTform.data.get_buffer();
591  tform_arr[0] = m_basePos.data.x;
592  tform_arr[1] = m_basePos.data.y;
593  tform_arr[2] = m_basePos.data.z;
594  hrp::setMatrix33ToRowMajorArray(ref_baseRot, tform_arr, 3);
595  m_baseTform.tm = m_qRef.tm;
596  // basePose
597  m_basePose.data.position.x = m_basePos.data.x;
598  m_basePose.data.position.y = m_basePos.data.y;
599  m_basePose.data.position.z = m_basePos.data.z;
600  m_basePose.data.orientation.r = m_baseRpy.data.r;
601  m_basePose.data.orientation.p = m_baseRpy.data.p;
602  m_basePose.data.orientation.y = m_baseRpy.data.y;
603  m_basePose.tm = m_qRef.tm;
604  // zmp
605  m_zmp.data.x = rel_ref_zmp(0);
606  m_zmp.data.y = rel_ref_zmp(1);
607  m_zmp.data.z = rel_ref_zmp(2);
608  m_zmp.tm = m_qRef.tm;
609  // cog
610  m_cog.data.x = ref_cog(0);
611  m_cog.data.y = ref_cog(1);
612  m_cog.data.z = ref_cog(2);
613  m_cog.tm = m_qRef.tm;
614  // sbpCogOffset
615  m_sbpCogOffset.data.x = sbp_cog_offset(0);
616  m_sbpCogOffset.data.y = sbp_cog_offset(1);
617  m_sbpCogOffset.data.z = sbp_cog_offset(2);
618  m_sbpCogOffset.tm = m_qRef.tm;
619  // write
624  m_zmpOut.write();
625  m_cogOut.write();
627 
628  // reference acceleration
629  hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
630  if (sen != NULL) {
631  hrp::Vector3 imu_sensor_pos = sen->link->p + sen->link->R * sen->localPos;
632  hrp::Vector3 imu_sensor_vel = (imu_sensor_pos - prev_imu_sensor_pos)/m_dt;
633  // convert to imu sensor local acceleration
634  hrp::Vector3 acc = (sen->link->R * sen->localR).transpose() * (imu_sensor_vel - prev_imu_sensor_vel)/m_dt;
635  m_accRef.data.ax = acc(0); m_accRef.data.ay = acc(1); m_accRef.data.az = acc(2);
636  m_accRefOut.write();
637  prev_imu_sensor_pos = imu_sensor_pos;
638  prev_imu_sensor_vel = imu_sensor_vel;
639  }
640 
641  // control parameters
642  m_contactStates.tm = m_qRef.tm;
646  m_toeheelRatio.tm = m_qRef.tm;
649  m_walkingStates.tm = m_qRef.tm;
651 
652  for (unsigned int i=0; i<m_ref_forceOut.size(); i++){
653  m_force[i].tm = m_qRef.tm;
654  m_ref_forceOut[i]->write();
655  }
656 
657  for (unsigned int i=0; i<m_limbCOPOffsetOut.size(); i++){
658  m_limbCOPOffset[i].tm = m_qRef.tm;
659  m_limbCOPOffsetOut[i]->write();
660  }
661  }
662 
663  return RTC::RTC_OK;
664 }
665 
667 {
668  // joint angles
669  for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
670  m_robot->joint(i)->q = m_qRef.data[i];
671  }
672  fik->setReferenceJointAngles();
673  // basepos, rot, zmp
674  m_robot->rootLink()->p = input_basePos;
675  m_robot->rootLink()->R = input_baseRot;
676  m_robot->calcForwardKinematics();
677  gg->proc_zmp_weight_map_interpolation();
678  if (control_mode != MODE_IDLE) {
680  // Calculate tmp_fix_coords and something
681  coordinates tmp_fix_coords;
682  if ( gg_is_walking ) {
683  gg->set_default_zmp_offsets(default_zmp_offsets);
684  gg_solved = gg->proc_one_tick();
685  gg->get_swing_support_mid_coords(tmp_fix_coords);
686  } else {
687  tmp_fix_coords = fix_leg_coords;
688  }
690  calcFixCoordsForAdjustFootstep(tmp_fix_coords);
691  fix_leg_coords = tmp_fix_coords;
692  }
693  // TODO : see explanation in this function
694  fixLegToCoords2(tmp_fix_coords);
695  fix_leg_coords2 = tmp_fix_coords;
696 
697  // Get output parameters and target EE coords
698  target_root_p = m_robot->rootLink()->p;
699  target_root_R = m_robot->rootLink()->R;
700  if ( gg_is_walking ) {
702  } else {
704  }
705  // Just for ik initial value
707  fik->current_root_p = target_root_p;
708  fik->current_root_R = target_root_R;
709  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
710  if ( std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) {
711  it->second.target_p0 = it->second.target_link->p + it->second.target_link->R * it->second.localPos;
712  it->second.target_r0 = it->second.target_link->R * it->second.localR;
713  }
714  }
715  }
716 
717  // Calculate other parameters
718  updateTargetCoordsForHandFixMode (tmp_fix_coords);
719  rotateRefForcesForFixCoords (tmp_fix_coords);
720  // TODO : see explanation in this function
722  // TODO : see explanation in this function
723  updateWalkingVelocityFromHandError(tmp_fix_coords);
725 
726  // Calculate ZMP, COG, and sbp targets
727  hrp::Vector3 tmp_ref_cog(m_robot->calcCM());
728  hrp::Vector3 tmp_foot_mid_pos = calcFootMidPosUsingZMPWeightMap ();
729  if (gg_is_walking) {
730  ref_cog = gg->get_cog();
731  } else {
732  ref_cog = tmp_foot_mid_pos;
733  }
734  ref_cog(2) = tmp_ref_cog(2);
735  if (gg_is_walking) {
736  ref_zmp = gg->get_refzmp();
737  } else {
738  ref_zmp(0) = ref_cog(0);
739  ref_zmp(1) = ref_cog(1);
740  ref_zmp(2) = tmp_foot_mid_pos(2);
741  }
742  } else { // MODE_IDLE
743  // Update fix_leg_coords based on input basePos and rot if MODE_IDLE
744  std::vector<coordinates> tmp_end_coords_list;
745  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
746  if ( std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) {
747  tmp_end_coords_list.push_back(coordinates(it->second.target_link->p+it->second.target_link->R*it->second.localPos, it->second.target_link->R*it->second.localR));
748  }
749  }
750  multi_mid_coords(fix_leg_coords, tmp_end_coords_list);
752  // Set force
753  for (unsigned int i=0; i< m_force.size(); i++) {
754  for (unsigned int j=0; j<6; j++) {
755  m_force[i].data[j] = m_ref_force[i].data[j];
756  }
757  }
758  }
759  // Just for stop walking
760  if (gg_is_walking && !gg_solved) stopWalking ();
761 };
762 
764 {
765  for (std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++) {
766  size_t idx = contact_states_index_map[it->first];
767  // Check whether "it->first" ee_name is included in leg_names. leg_names is equivalent to "swing" + "support" in gg.
768  if (std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end()) {
769  // Set EE coords
770  gg->get_swing_support_ee_coords_from_ee_name(it->second.target_p0, it->second.target_r0, it->first);
771  // Set contactStates
772  m_contactStates.data[idx] = gg->get_current_support_state_from_ee_name(it->first);
773  // Set controlSwingSupportTime
774  m_controlSwingSupportTime.data[idx] = gg->get_current_swing_time_from_ee_name(it->first);
775  // Set limbCOPOffset
776  hrp::Vector3 tmpzmpoff(m_limbCOPOffset[idx].data.x, m_limbCOPOffset[idx].data.y, m_limbCOPOffset[idx].data.z);
777  gg->get_swing_support_foot_zmp_offsets_from_ee_name(tmpzmpoff, it->first);
778  m_limbCOPOffset[idx].data.x = tmpzmpoff(0);
779  m_limbCOPOffset[idx].data.y = tmpzmpoff(1);
780  m_limbCOPOffset[idx].data.z = tmpzmpoff(2);
781  // Set toe heel ratio which can be used force moment distribution
782  double tmp = m_toeheelRatio.data[idx];
783  gg->get_current_toe_heel_ratio_from_ee_name(tmp, it->first);
784  m_toeheelRatio.data[idx] = tmp;
785  } else { // Not included in leg_names
786  // Set EE coords
787  it->second.target_p0 = it->second.target_link->p + it->second.target_link->R * it->second.localPos;
788  it->second.target_r0 = it->second.target_link->R * it->second.localR;
789  // contactStates is OFF other than leg_names
790  m_contactStates.data[idx] = false;
791  // controlSwingSupportTime is not used while double support period, 1.0 is neglected
792  m_controlSwingSupportTime.data[idx] = 1.0;
793  // Set limbCOPOffset
794  m_limbCOPOffset[idx].data.x = default_zmp_offsets[idx](0);
795  m_limbCOPOffset[idx].data.y = default_zmp_offsets[idx](1);
796  m_limbCOPOffset[idx].data.z = default_zmp_offsets[idx](2);
797  // Set toe heel ratio which can be used force moment distribution
799  }
800  }
801 };
802 
804 {
805  // double support by default
806  for (std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++) {
807  size_t idx = contact_states_index_map[it->first];
808  // Set EE coords.
809  // If not included in leg_names, calculate EE coords because of not being used in GaitGenerator.
810  // Otherwise, keep previous EE coords derived from GaitGenerator or initial value.
811  if ( std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end() ) {
812  it->second.target_p0 = it->second.target_link->p + it->second.target_link->R * it->second.localPos;
813  it->second.target_r0 = it->second.target_link->R * it->second.localR;
814  }
815  // Set contactStates
816  std::vector<std::string>::const_iterator dst = std::find_if(leg_names.begin(), leg_names.end(), (boost::lambda::_1 == it->first));
817  if (dst != leg_names.end()) {
818  m_contactStates.data[idx] = true;
819  } else {
820  m_contactStates.data[idx] = false;
821  }
822  // controlSwingSupportTime is not used while double support period, 1.0 is neglected
823  m_controlSwingSupportTime.data[idx] = 1.0;
824  // Set limbCOPOffset
825  m_limbCOPOffset[idx].data.x = default_zmp_offsets[idx](0);
826  m_limbCOPOffset[idx].data.y = default_zmp_offsets[idx](1);
827  m_limbCOPOffset[idx].data.z = default_zmp_offsets[idx](2);
828  // Set toe heel ratio is not used while double support
830  }
831 };
832 
834 {
835  // Set contactStates and controlSwingSupportTime
836  if (m_optionalData.data.length() >= contact_states_index_map.size()*2) {
837  // current optionalData is contactstates x limb and controlSwingSupportTime x limb
838  // If contactStates in optionalData is 1.0, m_contactStates is true. Otherwise, false.
839  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
840  m_contactStates.data[contact_states_index_map[it->first]] = isOptionalDataContact(it->first);
842  }
843  // If two feet have no contact, force set double support contact
844  if ( !m_contactStates.data[contact_states_index_map["rleg"]] && !m_contactStates.data[contact_states_index_map["lleg"]] ) {
845  m_contactStates.data[contact_states_index_map["rleg"]] = true;
846  m_contactStates.data[contact_states_index_map["lleg"]] = true;
847  }
848  }
849  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
850  size_t idx = contact_states_index_map[it->first];
851  // Set limbCOPOffset
852  m_limbCOPOffset[idx].data.x = 0;
853  m_limbCOPOffset[idx].data.y = 0;
854  m_limbCOPOffset[idx].data.z = 0;
855  // Set toe heel ratio is not used while double support
857  }
858 };
859 
861 {
863  double *default_zmp_offsets_output = new double[ikp.size()*3];
864  zmp_offset_interpolator->get(default_zmp_offsets_output, true);
865  for (size_t i = 0; i < ikp.size(); i++)
866  for (size_t j = 0; j < 3; j++)
867  default_zmp_offsets[i](j) = default_zmp_offsets_output[i*3+j];
868  delete[] default_zmp_offsets_output;
869  if (DEBUGP) {
870  std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets (interpolated)" << std::endl;
871  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
872  std::cerr << "[" << m_profile.instance_name << "] " << it->first << " = " << default_zmp_offsets[contact_states_index_map[it->first]].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl;
873  }
874  }
875  }
878  }else {
880  }
881 };
882 
884 {
885  double tmp = 0.0;
886  adjust_footstep_interpolator->get(&tmp, true);
887  //std::cerr << "[" << m_profile.instance_name << "] adjust ratio " << tmp << std::endl;
888  ikp["rleg"].target_p0 = (1-tmp) * ikp["rleg"].adjust_interpolation_org_p0 + tmp*ikp["rleg"].adjust_interpolation_target_p0;
889  ikp["lleg"].target_p0 = (1-tmp) * ikp["lleg"].adjust_interpolation_org_p0 + tmp*ikp["lleg"].adjust_interpolation_target_p0;
890  rats::mid_rot(ikp["rleg"].target_r0, tmp, ikp["rleg"].adjust_interpolation_org_r0, ikp["rleg"].adjust_interpolation_target_r0);
891  rats::mid_rot(ikp["lleg"].target_r0, tmp, ikp["lleg"].adjust_interpolation_org_r0, ikp["lleg"].adjust_interpolation_target_r0);
892  coordinates tmprc, tmplc;
893  tmprc.pos = ikp["rleg"].target_p0;
894  tmprc.rot = ikp["rleg"].target_r0;
895  tmplc.pos = ikp["lleg"].target_p0;
896  tmplc.rot = ikp["lleg"].target_r0;
897  rats::mid_coords(tmp_fix_coords, 0.5, tmprc, tmplc);
898  //fix_leg_coords = tmp_fix_coords;
899 };
900 
902 {
903  /* update ref_forces ;; StateHolder's absolute -> AutoBalancer's absolute */
904  for (size_t i = 0; i < m_ref_forceIn.size(); i++) {
905  // hrp::Matrix33 eeR;
906  // hrp::Link* parentlink;
907  // hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_names[i]);
908  // if (sensor) parentlink = sensor->link;
909  // else parentlink = m_vfs[sensor_names[i]].link;
910  // for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
911  // if (it->second.target_link->name == parentlink->name) eeR = parentlink->R * it->second.localR;
912  // }
913  // End effector frame
914  //ref_forces[i] = eeR * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
915  // world frame
916  ref_forces[i] = tmp_fix_coords.rot * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
917  ref_moments[i] = tmp_fix_coords.rot * hrp::Vector3(m_ref_force[i].data[3], m_ref_force[i].data[4], m_ref_force[i].data[5]);
918  }
919  sbp_offset = tmp_fix_coords.rot * hrp::Vector3(sbp_offset);
920 };
921 
923 {
924  // Move hand for hand fix mode
925  // If arms' ABCIKparam.is_active is true, move hands according to cog velocity.
926  // If is_hand_fix_mode is false, no hand fix mode and move hands according to cog velocity.
927  // If is_hand_fix_mode is true, hand fix mode and do not move hands in Y axis in tmp_fix_coords.rot.
928  if (gg_is_walking) {
929  // hand control while walking = solve hand ik with is_hand_fix_mode and solve hand ik without is_hand_fix_mode
930  bool is_hand_control_while_walking = false;
931  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
932  if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
933  && it->first.find("arm") != std::string::npos ) {
934  is_hand_control_while_walking = true;
935  }
936  }
937  if (is_hand_control_while_walking) {
938  //if (false) { // Disabled temporarily
939  // Store hand_fix_initial_offset in the initialization of walking
940  if (is_hand_fix_initial) hand_fix_initial_offset = tmp_fix_coords.rot.transpose() * (hrp::Vector3(gg->get_cog()(0), gg->get_cog()(1), tmp_fix_coords.pos(2)) - tmp_fix_coords.pos);
941  is_hand_fix_initial = false;
942  hrp::Vector3 dif_p = hrp::Vector3(gg->get_cog()(0), gg->get_cog()(1), tmp_fix_coords.pos(2)) - tmp_fix_coords.pos - tmp_fix_coords.rot * hand_fix_initial_offset;
943  if (is_hand_fix_mode) {
944  dif_p = tmp_fix_coords.rot.transpose() * dif_p;
945  dif_p(1) = 0;
946  dif_p = tmp_fix_coords.rot * dif_p;
947  }
948  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
949  if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()
950  && it->first.find("arm") != std::string::npos ) {
951  it->second.target_p0 = it->second.target_p0 + dif_p;
952  }
953  }
954  }
955  }
956 };
957 
959 {
960  // TODO : need to be updated for multicontact and other walking
961  if (leg_names.size() == 2) {
962  std::vector<hrp::Vector3> ee_pos;
963  for (size_t i = 0 ; i < leg_names.size(); i++) {
964  ABCIKparam& tmpikp = ikp[leg_names[i]];
965  ee_pos.push_back(tmpikp.target_p0 + tmpikp.target_r0 * default_zmp_offsets[i]);
966  }
967  double alpha = (ref_zmp - ee_pos[1]).norm() / ((ee_pos[0] - ref_zmp).norm() + (ee_pos[1] - ref_zmp).norm());
968  if (alpha>1.0) alpha = 1.0;
969  if (alpha<0.0) alpha = 0.0;
970  if (DEBUGP) {
971  std::cerr << "[" << m_profile.instance_name << "] alpha:" << alpha << std::endl;
972  }
973  double mg = m_robot->totalMass() * gg->get_gravitational_acceleration();
974  m_force[0].data[2] = alpha * mg;
975  m_force[1].data[2] = (1-alpha) * mg;
976  }
977  if ( use_force == MODE_REF_FORCE_WITH_FOOT || use_force == MODE_REF_FORCE_RFU_EXT_MOMENT ) { // TODO : use other use_force mode. This should be depends on Stabilizer distribution mode.
979  }
981 };
982 
984 {
985  hrp::Vector3 tmp_foot_mid_pos(hrp::Vector3::Zero());
986  std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
987  std::map<leg_type, double> zmp_weight_map = gg->get_zmp_weight_map();
988  double sum_of_weight = 0.0;
989  for (size_t i = 0; i < leg_names.size(); i++) {
990  ABCIKparam& tmpikp = ikp[leg_names[i]];
991  // for foot_mid_pos
992  std::map<leg_type, std::string>::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map<leg_type, std::string>::value_type::second == leg_names[i]));
993  tmp_foot_mid_pos += (tmpikp.target_p0 + tmpikp.target_r0 * default_zmp_offsets[i]) * zmp_weight_map[dst->first];
994  sum_of_weight += zmp_weight_map[dst->first];
995  }
996  tmp_foot_mid_pos *= (1.0 / sum_of_weight);
997  return tmp_foot_mid_pos;
998 };
999 
1001 {
1002  // TODO : check frame and robot state for this calculation
1003  if ( gg_is_walking && gg->get_lcg_count() == gg->get_overwrite_check_timing()+2 ) {
1004  hrp::Vector3 vel_htc(calc_vel_from_hand_error(tmp_fix_coords));
1005  gg->set_offset_velocity_param(vel_htc(0), vel_htc(1) ,vel_htc(2));
1006  }// else {
1007  // if ( gg_is_walking && gg->get_lcg_count() == static_cast<size_t>(gg->get_default_step_time()/(2*m_dt))-1) {
1008  // gg->set_offset_velocity_param(0,0,0);
1009  // }
1010  // }
1011 };
1012 
1014 {
1015  fik->overwrite_ref_ja_index_vec.clear();
1016  // Fix for toe joint
1017  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
1018  if (it->second.is_active && it->second.has_toe_joint && gg->get_use_toe_joint()) {
1019  int i = it->second.target_link->jointId;
1020  if (gg->get_swing_leg_names().front() == it->first) {
1021  fik->qrefv[i] = fik->qrefv[i] + -1 * gg->get_foot_dif_rot_angle();
1022  }
1023  fik->overwrite_ref_ja_index_vec.push_back(i);
1024  }
1025  }
1026 };
1027 
1029 {
1030  hrp::Vector3 vv = axis1.cross(axis2);
1031  if (fabs(vv.norm()-0.0) < 1e-5) {
1032  return rot;
1033  } else {
1034  Eigen::AngleAxis<double> tmpr(std::asin(vv.norm()), vv.normalized());
1035  return tmpr.toRotationMatrix() * rot;
1036  }
1037 }
1038 
1039 void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot)
1040 {
1041  // get current foot mid pos + rot
1042  std::vector<coordinates> foot_coords;
1043  for (size_t i = 0; i < leg_names.size(); i++) {
1044  ABCIKparam& tmpikp = ikp[leg_names[i]];
1045  if (leg_names[i].find("leg") != std::string::npos) foot_coords.push_back(coordinates((tmpikp.target_link->p + tmpikp.target_link->R * tmpikp.localPos),
1046  (tmpikp.target_link->R * tmpikp.localR)));
1047  }
1048  coordinates current_foot_mid_coords;
1049  multi_mid_coords(current_foot_mid_coords, foot_coords);
1050  hrp::Vector3 current_foot_mid_pos = current_foot_mid_coords.pos;
1051  hrp::Matrix33 current_foot_mid_rot = current_foot_mid_coords.rot;
1052  // fix root pos + rot to fix "coords" = "current_foot_mid_xx"
1053  hrp::Matrix33 tmpR (fix_rot * current_foot_mid_rot.transpose());
1054  m_robot->rootLink()->p = fix_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos);
1055  rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R);
1056  m_robot->calcForwardKinematics();
1057 }
1058 
1060 {
1061  // Tempolarily modify tmp_fix_coords
1062  // This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272
1063  // Snap input tmp_fix_coords to XY plan projection.
1064  {
1065  hrp::Vector3 ex = hrp::Vector3::UnitX();
1066  hrp::Vector3 ez = hrp::Vector3::UnitZ();
1067  hrp::Vector3 xv1 (tmp_fix_coords.rot * ex);
1068  xv1(2) = 0.0;
1069  xv1.normalize();
1070  hrp::Vector3 yv1(ez.cross(xv1));
1071  tmp_fix_coords.rot(0,0) = xv1(0); tmp_fix_coords.rot(1,0) = xv1(1); tmp_fix_coords.rot(2,0) = xv1(2);
1072  tmp_fix_coords.rot(0,1) = yv1(0); tmp_fix_coords.rot(1,1) = yv1(1); tmp_fix_coords.rot(2,1) = yv1(2);
1073  tmp_fix_coords.rot(0,2) = ez(0); tmp_fix_coords.rot(1,2) = ez(1); tmp_fix_coords.rot(2,2) = ez(2);
1074  }
1075  fixLegToCoords(tmp_fix_coords.pos, tmp_fix_coords.rot);
1076 }
1077 
1079 {
1080  // Set ik target params
1081  fik->target_root_p = target_root_p;
1082  fik->target_root_R = target_root_R;
1083  for ( std::map<std::string, SimpleFullbodyInverseKinematicsSolver::IKparam>::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) {
1084  it->second.target_p0 = ikp[it->first].target_p0;
1085  it->second.target_r0 = ikp[it->first].target_r0;
1086  }
1088 // fik->current_tm = m_qRef.tm;
1089  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
1090  fik->ikp[it->first].is_ik_enable = it->second.is_active;
1091  }
1092  // Revert
1093  fik->revertRobotStateToCurrent();
1094  // TODO : SBP calculation is outside of solve ik?
1095  hrp::Vector3 tmp_input_sbp = hrp::Vector3(0,0,0);
1096  static_balance_point_proc_one(tmp_input_sbp, ref_zmp(2));
1097  hrp::Vector3 dif_cog = tmp_input_sbp - ref_cog;
1098  // Solve IK
1099  fik->solveFullbodyIK (dif_cog, transition_interpolator->isEmpty());
1100 }
1101 
1102 
1103 /*
1104  RTC::ReturnCode_t AutoBalancer::onAborting(RTC::UniqueId ec_id)
1105  {
1106  return RTC::RTC_OK;
1107  }
1108 */
1109 
1110 /*
1111  RTC::ReturnCode_t AutoBalancer::onError(RTC::UniqueId ec_id)
1112  {
1113  return RTC::RTC_OK;
1114  }
1115 */
1116 
1117 /*
1118  RTC::ReturnCode_t AutoBalancer::onReset(RTC::UniqueId ec_id)
1119  {
1120  return RTC::RTC_OK;
1121  }
1122 */
1123 
1124 /*
1125  RTC::ReturnCode_t AutoBalancer::onStateUpdate(RTC::UniqueId ec_id)
1126  {
1127  return RTC::RTC_OK;
1128  }
1129 */
1130 
1131 /*
1132  RTC::ReturnCode_t AutoBalancer::onRateChanged(RTC::UniqueId ec_id)
1133  {
1134  return RTC::RTC_OK;
1135  }
1136 */
1137 
1138 void AutoBalancer::startABCparam(const OpenHRP::AutoBalancerService::StrSequence& limbs)
1139 {
1140  std::cerr << "[" << m_profile.instance_name << "] start auto balancer mode" << std::endl;
1141  Guard guard(m_mutex);
1142  double tmp_ratio = 0.0;
1144  transition_interpolator->set(&tmp_ratio);
1145  tmp_ratio = 1.0;
1146  transition_interpolator->setGoal(&tmp_ratio, transition_time, true);
1148  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
1149  it->second.is_active = false;
1150  }
1151 
1152  for (size_t i = 0; i < limbs.length(); i++) {
1153  ABCIKparam& tmp = ikp[std::string(limbs[i])];
1154  tmp.is_active = true;
1155  std::cerr << "[" << m_profile.instance_name << "] limb [" << std::string(limbs[i]) << "]" << std::endl;
1156  }
1157 
1159 }
1160 
1162 {
1163  std::cerr << "[" << m_profile.instance_name << "] stop auto balancer mode" << std::endl;
1164  //Guard guard(m_mutex);
1165  double tmp_ratio = 1.0;
1167  transition_interpolator->set(&tmp_ratio);
1168  tmp_ratio = 0.0;
1169  transition_interpolator->setGoal(&tmp_ratio, transition_time, true);
1171 }
1172 
1174 {
1175  if ( control_mode != MODE_ABC ) {
1176  std::cerr << "[" << m_profile.instance_name << "] Cannot start walking without MODE_ABC. Please startAutoBalancer." << std::endl;
1177  return false;
1178  }
1179  {
1180  Guard guard(m_mutex);
1181  fik->resetIKFailParam();
1182  std::vector<std::string> init_swing_leg_names(gg->get_footstep_front_leg_names());
1183  std::vector<std::string> tmp_all_limbs(leg_names);
1184  std::vector<std::string> init_support_leg_names;
1185  std::sort(tmp_all_limbs.begin(), tmp_all_limbs.end());
1186  std::sort(init_swing_leg_names.begin(), init_swing_leg_names.end());
1187  std::set_difference(tmp_all_limbs.begin(), tmp_all_limbs.end(),
1188  init_swing_leg_names.begin(), init_swing_leg_names.end(),
1189  std::back_inserter(init_support_leg_names));
1190  std::vector<step_node> init_support_leg_steps, init_swing_leg_dst_steps;
1191  for (std::vector<std::string>::iterator it = init_support_leg_names.begin(); it != init_support_leg_names.end(); it++)
1192  init_support_leg_steps.push_back(step_node(*it, coordinates(ikp[*it].target_p0, ikp[*it].target_r0), 0, 0, 0, 0));
1193  for (std::vector<std::string>::iterator it = init_swing_leg_names.begin(); it != init_swing_leg_names.end(); it++)
1194  init_swing_leg_dst_steps.push_back(step_node(*it, coordinates(ikp[*it].target_p0, ikp[*it].target_r0), 0, 0, 0, 0));
1195  gg->set_default_zmp_offsets(default_zmp_offsets);
1196  gg->initialize_gait_parameter(ref_cog, init_support_leg_steps, init_swing_leg_dst_steps);
1197  }
1198  is_hand_fix_initial = true;
1199  while ( !gg->proc_one_tick() );
1200  {
1201  Guard guard(m_mutex);
1202  gg_is_walking = gg_solved = true;
1203  }
1204  return true;
1205 }
1206 
1208 {
1209  std::vector<coordinates> tmp_end_coords_list;
1210  for (std::vector<string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) {
1211  if ((*it).find("leg") != std::string::npos) tmp_end_coords_list.push_back(coordinates(ikp[*it].target_p0, ikp[*it].target_r0));
1212  }
1213  multi_mid_coords(fix_leg_coords, tmp_end_coords_list);
1215  gg->clear_footstep_nodes_list();
1216  gg_is_walking = false;
1217 }
1218 
1219 bool AutoBalancer::startAutoBalancer (const OpenHRP::AutoBalancerService::StrSequence& limbs)
1220 {
1221  if (control_mode == MODE_IDLE) {
1222  fik->resetIKFailParam();
1223  startABCparam(limbs);
1225  return true;
1226  } else {
1227  return false;
1228  }
1229 }
1230 
1232 {
1233  if (control_mode == MODE_ABC) {
1234  stopABCparam();
1236  return true;
1237  } else {
1238  return false;
1239  }
1240 }
1241 
1243 {
1244  while (!transition_interpolator->isEmpty()) usleep(1000);
1245  usleep(1000);
1246 }
1247 bool AutoBalancer::goPos(const double& x, const double& y, const double& th)
1248 {
1249  // if ( !gg_is_walking && !is_stop_mode) {
1250  if ( !is_stop_mode) {
1251  gg->set_all_limbs(leg_names);
1252  coordinates start_ref_coords;
1253  std::vector<coordinates> initial_support_legs_coords;
1254  std::vector<leg_type> initial_support_legs;
1255  bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
1256  if (is_valid_gait_type == false) return false;
1257  bool ret = gg->go_pos_param_2_footstep_nodes_list(x, y, th,
1258  initial_support_legs_coords, // Dummy if gg_is_walking
1259  start_ref_coords, // Dummy if gg_is_walking
1260  initial_support_legs, // Dummy if gg_is_walking
1261  (!gg_is_walking)); // If gg_is_walking, initialize. Otherwise, not initialize and overwrite footsteps.
1262  if (!ret) {
1263  std::cerr << "[" << m_profile.instance_name << "] Cannot goPos because of invalid timing." << std::endl;
1264  }
1265  if ( !gg_is_walking ) { // Initializing
1266  ret = startWalking();
1267  }
1268  return ret;
1269  } else {
1270  std::cerr << "[" << m_profile.instance_name << "] Cannot goPos while stopping mode." << std::endl;
1271  return false;
1272  }
1273 }
1274 
1275 bool AutoBalancer::goVelocity(const double& vx, const double& vy, const double& vth)
1276 {
1277  gg->set_all_limbs(leg_names);
1278  bool ret = true;
1279  if (gg_is_walking && gg_solved) {
1280  gg->set_velocity_param(vx, vy, vth);
1281  } else {
1282  coordinates ref_coords;
1283  ref_coords.pos = (ikp["rleg"].target_p0+ikp["lleg"].target_p0)*0.5;
1284  mid_rot(ref_coords.rot, 0.5, ikp["rleg"].target_r0, ikp["lleg"].target_r0);
1285  std::vector<leg_type> current_legs;
1286  switch(gait_type) {
1287  case BIPED:
1288  current_legs.assign (1, vy > 0 ? RLEG : LLEG);
1289  break;
1290  case TROT:
1291  current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM)).convert_to_container < std::vector<leg_type> > ();
1292  break;
1293  case PACE:
1294  current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM)).convert_to_container < std::vector<leg_type> > ();
1295  break;
1296  case CRAWL:
1297  std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
1298  return false;
1299  case GALLOP:
1300  /* at least one leg shoud be in contact */
1301  std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
1302  return false;
1303  default: break;
1304  }
1305  gg->initialize_velocity_mode(ref_coords, vx, vy, vth, current_legs);
1306  ret = startWalking();
1307  }
1308  return ret;
1309 }
1310 
1312 {
1313  gg->finalize_velocity_mode();
1314  waitFootSteps();
1315  return true;
1316 }
1317 
1319 {
1320  std::cerr << "[" << m_profile.instance_name << "] emergencyStop" << std::endl;
1321  // is_stop_mode = true;
1322  gg->emergency_stop();
1323  waitFootSteps();
1324  return true;
1325 }
1326 
1328 {
1329  if (is_stop_mode) {
1330  std::cerr << "[" << m_profile.instance_name << "] releaseEmergencyStop" << std::endl;
1331  is_stop_mode = false;
1332  }
1333  return true;
1334 }
1335 
1336 bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx)
1337 {
1338  OpenHRP::AutoBalancerService::StepParamsSequence spss;
1339  spss.length(fss.length());
1340  // If gg_is_walking is false, initial footstep will be double support. So, set 0 for step_height and toe heel angles.
1341  // If gg_is_walking is true, do not set to 0.
1342  for (size_t i = 0; i < spss.length(); i++) {
1343  spss[i].sps.length(fss[i].fs.length());
1344  for (size_t j = 0; j < spss[i].sps.length(); j++) {
1345  spss[i].sps[j].step_height = ((!gg_is_walking && i==0) ? 0.0 : gg->get_default_step_height());
1346  spss[i].sps[j].step_time = gg->get_default_step_time();
1347  spss[i].sps[j].toe_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_toe_angle());
1348  spss[i].sps[j].heel_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_heel_angle());
1349  }
1350  }
1351  return setFootStepsWithParam(fss, spss, overwrite_fs_idx);
1352 }
1353 
1354 bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx)
1355 {
1356  if (!is_stop_mode) {
1357  std::cerr << "[" << m_profile.instance_name << "] setFootStepsList" << std::endl;
1358 
1359  // Initial footstep Snapping
1360  coordinates tmpfs, fstrans;
1361  step_node initial_support_step, initial_input_step;
1362  {
1363  std::vector<step_node> initial_support_steps;
1364  if (gg_is_walking) {
1365  if (overwrite_fs_idx <= 0) {
1366  std::cerr << "[" << m_profile.instance_name << "] Invalid overwrite index = " << overwrite_fs_idx << std::endl;
1367  return false;
1368  }
1369  if (!gg->get_footstep_nodes_by_index(initial_support_steps, overwrite_fs_idx-1)) {
1370  std::cerr << "[" << m_profile.instance_name << "] Invalid overwrite index = " << overwrite_fs_idx << std::endl;
1371  return false;
1372  }
1373  } else {
1374  // If walking, snap initial leg to current ABC foot coords.
1375  for (size_t i = 0; i < fss[0].fs.length(); i++) {
1376  ABCIKparam& tmpikp = ikp[std::string(fss[0].fs[i].leg)];
1377  initial_support_steps.push_back(step_node(std::string(fss[0].fs[i].leg), coordinates(tmpikp.target_p0, tmpikp.target_r0), 0, 0, 0, 0));
1378  }
1379  }
1380  initial_support_step = initial_support_steps.front(); /* use only one leg for representation */
1381  }
1382  {
1383  std::map<leg_type, std::string> leg_type_map = gg->get_leg_type_map();
1384  for (size_t i = 0; i < fss[0].fs.length(); i++) {
1385  if (std::string(fss[0].fs[i].leg) == leg_type_map[initial_support_step.l_r]) {
1386  coordinates tmp;
1387  memcpy(tmp.pos.data(), fss[0].fs[i].pos, sizeof(double)*3);
1388  tmp.rot = (Eigen::Quaternion<double>(fss[0].fs[i].rot[0], fss[0].fs[i].rot[1], fss[0].fs[i].rot[2], fss[0].fs[i].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
1389  initial_input_step = step_node(std::string(fss[0].fs[i].leg), tmp, 0, 0, 0, 0);
1390  }
1391  }
1392  }
1393 
1394  // Get footsteps
1395  std::vector< std::vector<coordinates> > fs_vec_list;
1396  std::vector< std::vector<std::string> > leg_name_vec_list;
1397  for (size_t i = 0; i < fss.length(); i++) {
1398  std::vector<coordinates> fs_vec;
1399  std::vector<std::string> leg_name_vec;
1400  for (size_t j = 0; j < fss[i].fs.length(); j++) {
1401  std::string leg(fss[i].fs[j].leg);
1402  if (std::find(leg_names.begin(), leg_names.end(), leg) != leg_names.end()) {
1403  memcpy(tmpfs.pos.data(), fss[i].fs[j].pos, sizeof(double)*3);
1404  tmpfs.rot = (Eigen::Quaternion<double>(fss[i].fs[j].rot[0], fss[i].fs[j].rot[1], fss[i].fs[j].rot[2], fss[i].fs[j].rot[3])).normalized().toRotationMatrix(); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
1405  initial_input_step.worldcoords.transformation(fstrans, tmpfs);
1406  tmpfs = initial_support_step.worldcoords;
1407  tmpfs.transform(fstrans);
1408  } else {
1409  std::cerr << "[" << m_profile.instance_name << "] No such target : " << leg << std::endl;
1410  return false;
1411  }
1412  leg_name_vec.push_back(leg);
1413  fs_vec.push_back(tmpfs);
1414  }
1415  leg_name_vec_list.push_back(leg_name_vec);
1416  fs_vec_list.push_back(fs_vec);
1417  }
1418  if (spss.length() != fs_vec_list.size()) {
1419  std::cerr << "[" << m_profile.instance_name << "] StepParam length " << spss.length () << " != Footstep length " << fs_vec_list.size() << std::endl;
1420  return false;
1421  }
1422  std::cerr << "[" << m_profile.instance_name << "] print footsteps " << std::endl;
1423  std::vector< std::vector<step_node> > fnsl;
1424  for (size_t i = 0; i < fs_vec_list.size(); i++) {
1425  if (!(gg_is_walking && i == 0)) { // If initial footstep, e.g., not walking, pass user-defined footstep list. If walking, pass cdr footsteps in order to neglect initial double support leg.
1426  std::vector<step_node> tmp_fns;
1427  for (size_t j = 0; j < fs_vec_list.at(i).size(); j++) {
1428  tmp_fns.push_back(step_node(leg_name_vec_list[i][j], fs_vec_list[i][j], spss[i].sps[j].step_height, spss[i].sps[j].step_time, spss[i].sps[j].toe_angle, spss[i].sps[j].heel_angle));
1429  }
1430  fnsl.push_back(tmp_fns);
1431  }
1432  }
1433  bool ret = true;
1434  if (gg_is_walking) {
1435  std::cerr << "[" << m_profile.instance_name << "] Set overwrite footsteps" << std::endl;
1436  gg->set_overwrite_foot_steps_list(fnsl);
1437  gg->set_overwrite_foot_step_index(overwrite_fs_idx);
1438  } else {
1439  std::cerr << "[" << m_profile.instance_name << "] Set normal footsteps" << std::endl;
1440  gg->set_foot_steps_list(fnsl);
1441  ret = startWalking();
1442  }
1443  return ret;
1444  } else {
1445  std::cerr << "[" << m_profile.instance_name << "] Cannot setFootSteps while walking." << std::endl;
1446  return false;
1447  }
1448 }
1449 
1451 {
1452  //while (gg_is_walking) usleep(10);
1454  usleep(1000);
1455  usleep(1000);
1456  gg->set_offset_velocity_param(0,0,0);
1457 }
1458 
1460 {
1461  if (!gg_is_walking) { return;}
1462  while ( !gg->is_finalizing(tm)|| !transition_interpolator->isEmpty() )
1463  usleep(1000);
1464  usleep(1000);
1465  gg->set_offset_velocity_param(0,0,0);
1466 }
1467 
1468 bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
1469 {
1470  std::cerr << "[" << m_profile.instance_name << "] setGaitGeneratorParam" << std::endl;
1471  if (i_param.stride_parameter.length() == 4) { // Support old stride_parameter definitions
1472  gg->set_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3],
1473  i_param.stride_parameter[1]*0.5, i_param.stride_parameter[2]*0.5);
1474  } else {
1475  gg->set_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3],
1476  i_param.stride_parameter[4], i_param.stride_parameter[5]);
1477  }
1478  std::vector<hrp::Vector3> off;
1479  for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
1480  off.push_back(hrp::Vector3(i_param.leg_default_translate_pos[i][0], i_param.leg_default_translate_pos[i][1], i_param.leg_default_translate_pos[i][2]));
1481  }
1482  gg->set_leg_default_translate_pos(off);
1483  gg->set_default_step_time(i_param.default_step_time);
1484  gg->set_default_step_height(i_param.default_step_height);
1485  gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio/2.0);
1486  gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio/2.0);
1487  gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio/2.0);
1488  gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio/2.0);
1489  gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio/2.0);
1490  gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio/2.0);
1491  // gg->set_default_double_support_ratio_before(i_param.default_double_support_ratio_before);
1492  // gg->set_default_double_support_ratio_after(i_param.default_double_support_ratio_after);
1493  // gg->set_default_double_support_static_ratio_before(i_param.default_double_support_static_ratio_before);
1494  // gg->set_default_double_support_static_ratio_after(i_param.default_double_support_static_ratio_after);
1495  // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_before);
1496  // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_after);
1497  // gg->set_default_double_support_ratio_swing_before(i_param.default_double_support_ratio_swing_before);
1498  // gg->set_default_double_support_ratio_swing_after(i_param.default_double_support_ratio_swing_after);
1499  if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::SHUFFLING) {
1500  gg->set_default_orbit_type(SHUFFLING);
1501  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOID) {
1502  gg->set_default_orbit_type(CYCLOID);
1503  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::RECTANGLE) {
1504  gg->set_default_orbit_type(RECTANGLE);
1505  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::STAIR) {
1506  gg->set_default_orbit_type(STAIR);
1507  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAY) {
1508  gg->set_default_orbit_type(CYCLOIDDELAY);
1509  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK) {
1510  gg->set_default_orbit_type(CYCLOIDDELAYKICK);
1511  } else if (i_param.default_orbit_type == OpenHRP::AutoBalancerService::CROSS) {
1512  gg->set_default_orbit_type(CROSS);
1513  }
1514  gg->set_swing_trajectory_delay_time_offset(i_param.swing_trajectory_delay_time_offset);
1515  gg->set_swing_trajectory_final_distance_weight(i_param.swing_trajectory_final_distance_weight);
1516  gg->set_swing_trajectory_time_offset_xy2z(i_param.swing_trajectory_time_offset_xy2z);
1517  gg->set_stair_trajectory_way_point_offset(hrp::Vector3(i_param.stair_trajectory_way_point_offset[0], i_param.stair_trajectory_way_point_offset[1], i_param.stair_trajectory_way_point_offset[2]));
1518  gg->set_cycloid_delay_kick_point_offset(hrp::Vector3(i_param.cycloid_delay_kick_point_offset[0], i_param.cycloid_delay_kick_point_offset[1], i_param.cycloid_delay_kick_point_offset[2]));
1519  gg->set_gravitational_acceleration(i_param.gravitational_acceleration);
1520  gg->set_toe_angle(i_param.toe_angle);
1521  gg->set_heel_angle(i_param.heel_angle);
1522  gg->set_toe_pos_offset_x(i_param.toe_pos_offset_x);
1523  gg->set_heel_pos_offset_x(i_param.heel_pos_offset_x);
1524  gg->set_toe_zmp_offset_x(i_param.toe_zmp_offset_x);
1525  gg->set_heel_zmp_offset_x(i_param.heel_zmp_offset_x);
1526  gg->set_toe_check_thre(i_param.toe_check_thre);
1527  gg->set_heel_check_thre(i_param.heel_check_thre);
1528  std::vector<double> tmp_ratio(i_param.toe_heel_phase_ratio.get_buffer(), i_param.toe_heel_phase_ratio.get_buffer()+i_param.toe_heel_phase_ratio.length());
1529  std::cerr << "[" << m_profile.instance_name << "] "; // for set_toe_heel_phase_ratio
1530  gg->set_toe_heel_phase_ratio(tmp_ratio);
1531  gg->set_use_toe_joint(i_param.use_toe_joint);
1532  gg->set_use_toe_heel_transition(i_param.use_toe_heel_transition);
1533  gg->set_use_toe_heel_auto_set(i_param.use_toe_heel_auto_set);
1534  gg->set_zmp_weight_map(boost::assign::map_list_of<leg_type, double>(RLEG, i_param.zmp_weight_map[0])(LLEG, i_param.zmp_weight_map[1])(RARM, i_param.zmp_weight_map[2])(LARM, i_param.zmp_weight_map[3]));
1535  gg->set_optional_go_pos_finalize_footstep_num(i_param.optional_go_pos_finalize_footstep_num);
1536  gg->set_overwritable_footstep_index_offset(i_param.overwritable_footstep_index_offset);
1537  gg->set_leg_margin(i_param.leg_margin);
1538  gg->set_stride_limitation_for_circle_type(i_param.stride_limitation_for_circle_type);
1539  gg->set_overwritable_stride_limitation(i_param.overwritable_stride_limitation);
1540  gg->set_use_stride_limitation(i_param.use_stride_limitation);
1541  gg->set_footstep_modification_gain(i_param.footstep_modification_gain);
1542  gg->set_modify_footsteps(i_param.modify_footsteps);
1543  gg->set_cp_check_margin(i_param.cp_check_margin);
1544  gg->set_margin_time_ratio(i_param.margin_time_ratio);
1545  if (i_param.stride_limitation_type == OpenHRP::AutoBalancerService::SQUARE) {
1546  gg->set_stride_limitation_type(SQUARE);
1547  } else if (i_param.stride_limitation_type == OpenHRP::AutoBalancerService::CIRCLE) {
1548  gg->set_stride_limitation_type(CIRCLE);
1549  }
1550 
1551  // print
1552  gg->print_param(std::string(m_profile.instance_name));
1553  return true;
1554 };
1555 
1556 bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param)
1557 {
1558  gg->get_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3], i_param.stride_parameter[4], i_param.stride_parameter[5]);
1559  std::vector<hrp::Vector3> off;
1560  gg->get_leg_default_translate_pos(off);
1561  i_param.leg_default_translate_pos.length(off.size());
1562  for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) {
1563  i_param.leg_default_translate_pos[i].length(3);
1564  i_param.leg_default_translate_pos[i][0] = off[i](0);
1565  i_param.leg_default_translate_pos[i][1] = off[i](1);
1566  i_param.leg_default_translate_pos[i][2] = off[i](2);
1567  }
1568  i_param.default_step_time = gg->get_default_step_time();
1569  i_param.default_step_height = gg->get_default_step_height();
1570  i_param.default_double_support_ratio_before = gg->get_default_double_support_ratio_before();
1571  i_param.default_double_support_ratio_after = gg->get_default_double_support_ratio_after();
1572  i_param.default_double_support_static_ratio_before = gg->get_default_double_support_static_ratio_before();
1573  i_param.default_double_support_static_ratio_after = gg->get_default_double_support_static_ratio_after();
1574  i_param.default_double_support_ratio_swing_before = gg->get_default_double_support_ratio_swing_before();
1575  i_param.default_double_support_ratio_swing_after = gg->get_default_double_support_ratio_swing_after();
1576  i_param.default_double_support_ratio = i_param.default_double_support_ratio_before + i_param.default_double_support_ratio_after;
1577  i_param.default_double_support_static_ratio = i_param.default_double_support_static_ratio_before + i_param.default_double_support_static_ratio_after;
1578  if (gg->get_default_orbit_type() == SHUFFLING) {
1579  i_param.default_orbit_type = OpenHRP::AutoBalancerService::SHUFFLING;
1580  } else if (gg->get_default_orbit_type() == CYCLOID) {
1581  i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOID;
1582  } else if (gg->get_default_orbit_type() == RECTANGLE) {
1583  i_param.default_orbit_type = OpenHRP::AutoBalancerService::RECTANGLE;
1584  } else if (gg->get_default_orbit_type() == STAIR) {
1585  i_param.default_orbit_type = OpenHRP::AutoBalancerService::STAIR;
1586  } else if (gg->get_default_orbit_type() == CYCLOIDDELAY) {
1587  i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAY;
1588  } else if (gg->get_default_orbit_type() == CYCLOIDDELAYKICK) {
1589  i_param.default_orbit_type = OpenHRP::AutoBalancerService::CYCLOIDDELAYKICK;
1590  } else if (gg->get_default_orbit_type() == CROSS) {
1591  i_param.default_orbit_type = OpenHRP::AutoBalancerService::CROSS;
1592  }
1593 
1594  hrp::Vector3 tmpv = gg->get_stair_trajectory_way_point_offset();
1595  for (size_t i = 0; i < 3; i++) i_param.stair_trajectory_way_point_offset[i] = tmpv(i);
1596  tmpv = gg->get_cycloid_delay_kick_point_offset();
1597  for (size_t i = 0; i < 3; i++) i_param.cycloid_delay_kick_point_offset[i] = tmpv(i);
1598  i_param.swing_trajectory_delay_time_offset = gg->get_swing_trajectory_delay_time_offset();
1599  i_param.swing_trajectory_final_distance_weight = gg->get_swing_trajectory_final_distance_weight();
1600  i_param.swing_trajectory_time_offset_xy2z = gg->get_swing_trajectory_time_offset_xy2z();
1601  i_param.gravitational_acceleration = gg->get_gravitational_acceleration();
1602  i_param.toe_angle = gg->get_toe_angle();
1603  i_param.heel_angle = gg->get_heel_angle();
1604  i_param.toe_pos_offset_x = gg->get_toe_pos_offset_x();
1605  i_param.heel_pos_offset_x = gg->get_heel_pos_offset_x();
1606  i_param.toe_zmp_offset_x = gg->get_toe_zmp_offset_x();
1607  i_param.heel_zmp_offset_x = gg->get_heel_zmp_offset_x();
1608  i_param.toe_check_thre = gg->get_toe_check_thre();
1609  i_param.heel_check_thre = gg->get_heel_check_thre();
1610  std::vector<double> ratio(gg->get_NUM_TH_PHASES(),0.0);
1611  gg->get_toe_heel_phase_ratio(ratio);
1612  for (int i = 0; i < gg->get_NUM_TH_PHASES(); i++) i_param.toe_heel_phase_ratio[i] = ratio[i];
1613  i_param.use_toe_joint = gg->get_use_toe_joint();
1614  i_param.use_toe_heel_transition = gg->get_use_toe_heel_transition();
1615  i_param.use_toe_heel_auto_set = gg->get_use_toe_heel_auto_set();
1616  std::map<leg_type, double> tmp_zmp_weight_map = gg->get_zmp_weight_map();
1617  i_param.zmp_weight_map[0] = tmp_zmp_weight_map[RLEG];
1618  i_param.zmp_weight_map[1] = tmp_zmp_weight_map[LLEG];
1619  i_param.zmp_weight_map[2] = tmp_zmp_weight_map[RARM];
1620  i_param.zmp_weight_map[3] = tmp_zmp_weight_map[LARM];
1621  i_param.optional_go_pos_finalize_footstep_num = gg->get_optional_go_pos_finalize_footstep_num();
1622  i_param.overwritable_footstep_index_offset = gg->get_overwritable_footstep_index_offset();
1623  for (size_t i=0; i<4; i++) {
1624  i_param.leg_margin[i] = gg->get_leg_margin(i);
1625  }
1626  for (size_t i=0; i<5; i++) {
1627  i_param.stride_limitation_for_circle_type[i] = gg->get_stride_limitation_for_circle_type(i);
1628  }
1629  for (size_t i=0; i<5; i++) {
1630  i_param.overwritable_stride_limitation[i] = gg->get_overwritable_stride_limitation(i);
1631  }
1632  i_param.use_stride_limitation = gg->get_use_stride_limitation();
1633  i_param.footstep_modification_gain = gg->get_footstep_modification_gain();
1634  i_param.modify_footsteps = gg->get_modify_footsteps();
1635  for (size_t i=0; i<2; i++) {
1636  i_param.cp_check_margin[i] = gg->get_cp_check_margin(i);
1637  }
1638  i_param.margin_time_ratio = gg->get_margin_time_ratio();
1639  if (gg->get_stride_limitation_type() == SQUARE) {
1640  i_param.stride_limitation_type = OpenHRP::AutoBalancerService::SQUARE;
1641  } else if (gg->get_stride_limitation_type() == CIRCLE) {
1642  i_param.stride_limitation_type = OpenHRP::AutoBalancerService::CIRCLE;
1643  }
1644  return true;
1645 };
1646 
1647 bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
1648 {
1649  Guard guard(m_mutex);
1650  std::cerr << "[" << m_profile.instance_name << "] setAutoBalancerParam" << std::endl;
1651  double *default_zmp_offsets_array = new double[ikp.size()*3];
1652  for (size_t i = 0; i < ikp.size(); i++)
1653  for (size_t j = 0; j < 3; j++)
1654  default_zmp_offsets_array[i*3+j] = i_param.default_zmp_offsets[i][j];
1655  zmp_transition_time = i_param.zmp_transition_time;
1656  adjust_footstep_transition_time = i_param.adjust_footstep_transition_time;
1659  zmp_offset_interpolator->setGoal(default_zmp_offsets_array, zmp_transition_time, true);
1660  } else {
1661  std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets cannot be set because interpolating." << std::endl;
1662  }
1663  if (control_mode == MODE_IDLE) {
1664  switch (i_param.use_force_mode) {
1665  case OpenHRP::AutoBalancerService::MODE_NO_FORCE:
1667  break;
1668  case OpenHRP::AutoBalancerService::MODE_REF_FORCE:
1670  break;
1671  case OpenHRP::AutoBalancerService::MODE_REF_FORCE_WITH_FOOT:
1673  break;
1674  case OpenHRP::AutoBalancerService::MODE_REF_FORCE_RFU_EXT_MOMENT:
1676  break;
1677  default:
1678  break;
1679  }
1680  } else {
1681  std::cerr << "[" << m_profile.instance_name << "] use_force_mode cannot be changed to [" << i_param.use_force_mode << "] during MODE_ABC, MODE_SYNC_TO_IDLE or MODE_SYNC_TO_ABC." << std::endl;
1682  }
1683  graspless_manip_mode = i_param.graspless_manip_mode;
1684  graspless_manip_arm = std::string(i_param.graspless_manip_arm);
1685  for (size_t j = 0; j < 3; j++)
1686  graspless_manip_p_gain[j] = i_param.graspless_manip_p_gain[j];
1687  for (size_t j = 0; j < 3; j++)
1688  graspless_manip_reference_trans_coords.pos[j] = i_param.graspless_manip_reference_trans_pos[j];
1689  graspless_manip_reference_trans_coords.rot = (Eigen::Quaternion<double>(i_param.graspless_manip_reference_trans_rot[0],
1690  i_param.graspless_manip_reference_trans_rot[1],
1691  i_param.graspless_manip_reference_trans_rot[2],
1692  i_param.graspless_manip_reference_trans_rot[3]).normalized().toRotationMatrix()); // rtc: (x, y, z, w) but eigen: (w, x, y, z)
1693  transition_time = i_param.transition_time;
1694  std::vector<std::string> cur_leg_names, dst_leg_names;
1695  cur_leg_names = leg_names;
1696  for (size_t i = 0; i < i_param.leg_names.length(); i++) {
1697  dst_leg_names.push_back(std::string(i_param.leg_names[i]));
1698  }
1699  std::sort(cur_leg_names.begin(), cur_leg_names.end());
1700  std::sort(dst_leg_names.begin(), dst_leg_names.end());
1701  if (cur_leg_names != dst_leg_names) {
1703  leg_names.clear();
1704  leg_names = dst_leg_names;
1705  if (control_mode == MODE_ABC) {
1706  double tmp_ratio = 0.0;
1707  leg_names_interpolator->set(&tmp_ratio);
1708  tmp_ratio = 1.0;
1709  leg_names_interpolator->setGoal(&tmp_ratio, 5.0, true);
1711  }
1712  }
1713  } else {
1714  std::cerr << "[" << m_profile.instance_name << "] leg_names cannot be set because interpolating." << std::endl;
1715  }
1716  if (!gg_is_walking) {
1717  is_hand_fix_mode = i_param.is_hand_fix_mode;
1718  std::cerr << "[" << m_profile.instance_name << "] is_hand_fix_mode = " << is_hand_fix_mode << std::endl;
1719  } else {
1720  std::cerr << "[" << m_profile.instance_name << "] is_hand_fix_mode cannot be set in (gg_is_walking = true). Current is_hand_fix_mode is " << (is_hand_fix_mode?"true":"false") << std::endl;
1721  }
1722  if (control_mode == MODE_IDLE) {
1723  for (size_t i = 0; i < i_param.end_effector_list.length(); i++) {
1724  std::map<std::string, ABCIKparam>::iterator it = ikp.find(std::string(i_param.end_effector_list[i].leg));
1725  memcpy(it->second.localPos.data(), i_param.end_effector_list[i].pos, sizeof(double)*3);
1726  it->second.localR = (Eigen::Quaternion<double>(i_param.end_effector_list[i].rot[0], i_param.end_effector_list[i].rot[1], i_param.end_effector_list[i].rot[2], i_param.end_effector_list[i].rot[3])).normalized().toRotationMatrix();
1727  }
1728  } else {
1729  std::cerr << "[" << m_profile.instance_name << "] cannot change end-effectors except during MODE_IDLE" << std::endl;
1730  }
1731  if (i_param.default_gait_type == OpenHRP::AutoBalancerService::BIPED) {
1732  gait_type = BIPED;
1733  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::TROT) {
1734  gait_type = TROT;
1735  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::PACE) {
1736  gait_type = PACE;
1737  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::CRAWL) {
1738  gait_type = CRAWL;
1739  } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::GALLOP) {
1740  gait_type = GALLOP;
1741  }
1742  // Ref force balancing
1743  std::cerr << "[" << m_profile.instance_name << "] Ref force balancing" << std::endl;
1745  std::cerr << "[" << m_profile.instance_name << "] additional_force_applied_point_offset and additional_force_applied_link_name cannot be updated during MODE_REF_FORCE_WITH_FOOT and non-MODE_IDLE"<< std::endl;
1746  } else if ( !m_robot->link(std::string(i_param.additional_force_applied_link_name)) ) {
1747  std::cerr << "[" << m_profile.instance_name << "] Invalid link name for additional_force_applied_link_name = " << i_param.additional_force_applied_link_name << std::endl;
1748  } else {
1749  additional_force_applied_link = m_robot->link(std::string(i_param.additional_force_applied_link_name));
1750  for (size_t i = 0; i < 3; i++) {
1751  additional_force_applied_point_offset(i) = i_param.additional_force_applied_point_offset[i];
1752  }
1753  std::cerr << "[" << m_profile.instance_name << "] Link name for additional_force_applied_link_name = " << additional_force_applied_link->name << ", additional_force_applied_point_offset = " << additional_force_applied_point_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl;
1754  }
1755 
1756  for (std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++) {
1757  std::cerr << "[" << m_profile.instance_name << "] End Effector [" << it->first << "]" << std::endl;
1758  std::cerr << "[" << m_profile.instance_name << "] localpos = " << it->second.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl;
1759  std::cerr << "[" << m_profile.instance_name << "] localR = " << it->second.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", " [", "]")) << std::endl;
1760  }
1761 
1762  std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets = ";
1763  for (size_t i = 0; i < ikp.size() * 3; i++) {
1764  std::cerr << default_zmp_offsets_array[i] << " ";
1765  }
1766  std::cerr << std::endl;
1767  delete[] default_zmp_offsets_array;
1768  std::cerr << "[" << m_profile.instance_name << "] use_force_mode = " << getUseForceModeString() << std::endl;
1769  std::cerr << "[" << m_profile.instance_name << "] graspless_manip_mode = " << graspless_manip_mode << std::endl;
1770  std::cerr << "[" << m_profile.instance_name << "] graspless_manip_arm = " << graspless_manip_arm << std::endl;
1771  std::cerr << "[" << m_profile.instance_name << "] graspless_manip_p_gain = " << graspless_manip_p_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl;
1772  std::cerr << "[" << m_profile.instance_name << "] graspless_manip_reference_trans_pos = " << graspless_manip_reference_trans_coords.pos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl;
1773  std::cerr << "[" << m_profile.instance_name << "] graspless_manip_reference_trans_rot = " << graspless_manip_reference_trans_coords.rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", " [", "]")) << std::endl;
1774  std::cerr << "[" << m_profile.instance_name << "] transition_time = " << transition_time << "[s], zmp_transition_time = " << zmp_transition_time << "[s], adjust_footstep_transition_time = " << adjust_footstep_transition_time << "[s]" << std::endl;
1775  for (std::vector<std::string>::iterator it = leg_names.begin(); it != leg_names.end(); it++) std::cerr << "[" << m_profile.instance_name << "] leg_names [" << *it << "]" << std::endl;
1776  std::cerr << "[" << m_profile.instance_name << "] default_gait_type = " << gait_type << std::endl;
1777  // FIK
1778  fik->move_base_gain = i_param.move_base_gain;
1779  fik->pos_ik_thre = i_param.pos_ik_thre;
1780  fik->rot_ik_thre = i_param.rot_ik_thre;
1781  fik->printParam();
1782  // IK limb parameters
1783  fik->setIKParam(ee_vec, i_param.ik_limb_parameters);
1784  // Limb stretch avoidance
1785  fik->use_limb_stretch_avoidance = i_param.use_limb_stretch_avoidance;
1786  fik->limb_stretch_avoidance_time_const = i_param.limb_stretch_avoidance_time_const;
1787  for (size_t i = 0; i < 2; i++) {
1788  fik->limb_stretch_avoidance_vlimit[i] = i_param.limb_stretch_avoidance_vlimit[i];
1789  }
1790  for (size_t i = 0; i < fik->ikp.size(); i++) {
1791  fik->ikp[ee_vec[i]].limb_length_margin = i_param.limb_length_margin[i];
1792  }
1793  return true;
1794 };
1795 
1796 bool AutoBalancer::getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalancerParam& i_param)
1797 {
1798  i_param.default_zmp_offsets.length(ikp.size());
1799  for (size_t i = 0; i < ikp.size(); i++) {
1800  i_param.default_zmp_offsets[i].length(3);
1801  for (size_t j = 0; j < 3; j++) {
1802  i_param.default_zmp_offsets[i][j] = default_zmp_offsets[i](j);
1803  }
1804  }
1805  switch(control_mode) {
1806  case MODE_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_IDLE; break;
1807  case MODE_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_ABC; break;
1808  case MODE_SYNC_TO_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_IDLE; break;
1809  case MODE_SYNC_TO_ABC: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_ABC; break;
1810  default: break;
1811  }
1812  switch(use_force) {
1813  case MODE_NO_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_NO_FORCE; break;
1814  case MODE_REF_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE; break;
1815  case MODE_REF_FORCE_WITH_FOOT: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE_WITH_FOOT; break;
1816  case MODE_REF_FORCE_RFU_EXT_MOMENT: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE_RFU_EXT_MOMENT; break;
1817  default: break;
1818  }
1819  i_param.graspless_manip_mode = graspless_manip_mode;
1820  i_param.graspless_manip_arm = graspless_manip_arm.c_str();
1821  for (size_t j = 0; j < 3; j++)
1822  i_param.graspless_manip_p_gain[j] = graspless_manip_p_gain[j];
1823  for (size_t j = 0; j < 3; j++)
1824  i_param.graspless_manip_reference_trans_pos[j] = graspless_manip_reference_trans_coords.pos[j];
1825  Eigen::Quaternion<double> qt(graspless_manip_reference_trans_coords.rot);
1826  i_param.graspless_manip_reference_trans_rot[0] = qt.w();
1827  i_param.graspless_manip_reference_trans_rot[1] = qt.x();
1828  i_param.graspless_manip_reference_trans_rot[2] = qt.y();
1829  i_param.graspless_manip_reference_trans_rot[3] = qt.z();
1830  i_param.transition_time = transition_time;
1831  i_param.zmp_transition_time = zmp_transition_time;
1832  i_param.adjust_footstep_transition_time = adjust_footstep_transition_time;
1833  i_param.leg_names.length(leg_names.size());
1834  for (size_t i = 0; i < leg_names.size(); i++) i_param.leg_names[i] = leg_names.at(i).c_str();
1835  i_param.is_hand_fix_mode = is_hand_fix_mode;
1836  i_param.end_effector_list.length(ikp.size());
1837  {
1838  size_t i = 0;
1839  for (std::map<std::string, ABCIKparam>::const_iterator it = ikp.begin(); it != ikp.end(); it++) {
1840  copyRatscoords2Footstep(i_param.end_effector_list[i],
1841  coordinates(it->second.localPos, it->second.localR));
1842  i_param.end_effector_list[i].leg = it->first.c_str();
1843  i++;
1844  }
1845  }
1846  switch(gait_type) {
1847  case BIPED: i_param.default_gait_type = OpenHRP::AutoBalancerService::BIPED; break;
1848  case TROT: i_param.default_gait_type = OpenHRP::AutoBalancerService::TROT; break;
1849  case PACE: i_param.default_gait_type = OpenHRP::AutoBalancerService::PACE; break;
1850  case CRAWL: i_param.default_gait_type = OpenHRP::AutoBalancerService::CRAWL; break;
1851  case GALLOP: i_param.default_gait_type = OpenHRP::AutoBalancerService::GALLOP; break;
1852  default: break;
1853  }
1854  // FIK
1855  i_param.move_base_gain = fik->move_base_gain;
1856  i_param.pos_ik_thre = fik->pos_ik_thre;
1857  i_param.rot_ik_thre = fik->rot_ik_thre;
1858  // IK limb parameters
1859  fik->getIKParam(ee_vec, i_param.ik_limb_parameters);
1860  // Limb stretch avoidance
1861  i_param.use_limb_stretch_avoidance = fik->use_limb_stretch_avoidance;
1862  i_param.limb_stretch_avoidance_time_const = fik->limb_stretch_avoidance_time_const;
1863  i_param.limb_length_margin.length(fik->ikp.size());
1864  for (size_t i = 0; i < 2; i++) {
1865  i_param.limb_stretch_avoidance_vlimit[i] = fik->limb_stretch_avoidance_vlimit[i];
1866  }
1867  for (size_t i = 0; i < ikp.size(); i++) {
1868  i_param.limb_length_margin[i] = fik->ikp[ee_vec[i]].limb_length_margin;
1869  }
1870  i_param.additional_force_applied_link_name = additional_force_applied_link->name.c_str();
1871  for (size_t i = 0; i < 3; i++) {
1872  i_param.additional_force_applied_point_offset[i] = additional_force_applied_point_offset(i);
1873  }
1874  return true;
1875 };
1876 
1877 void AutoBalancer::copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs)
1878 {
1879  memcpy(out_fs.pos, in_fs.pos.data(), sizeof(double)*3);
1880  Eigen::Quaternion<double> qt(in_fs.rot);
1881  out_fs.rot[0] = qt.w();
1882  out_fs.rot[1] = qt.x();
1883  out_fs.rot[2] = qt.y();
1884  out_fs.rot[3] = qt.z();
1885 };
1886 
1887 bool AutoBalancer::getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam& i_param)
1888 {
1889  copyRatscoords2Footstep(i_param.support_leg_coords, gg->get_support_leg_steps().front().worldcoords);
1890  copyRatscoords2Footstep(i_param.swing_leg_coords, gg->get_swing_leg_steps().front().worldcoords);
1891  copyRatscoords2Footstep(i_param.swing_leg_src_coords, gg->get_swing_leg_src_steps().front().worldcoords);
1892  copyRatscoords2Footstep(i_param.swing_leg_dst_coords, gg->get_swing_leg_dst_steps().front().worldcoords);
1893  copyRatscoords2Footstep(i_param.dst_foot_midcoords, gg->get_dst_foot_midcoords());
1894  if (gg->get_support_leg_names().front() == "rleg") {
1895  i_param.support_leg = OpenHRP::AutoBalancerService::RLEG;
1896  } else {
1897  i_param.support_leg = OpenHRP::AutoBalancerService::LLEG;
1898  }
1899  switch ( gg->get_current_support_states().front() ) {
1900  case BOTH: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::BOTH; break;
1901  case RLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::RLEG; break;
1902  case LLEG: i_param.support_leg_with_both = OpenHRP::AutoBalancerService::LLEG; break;
1903  default: break;
1904  }
1905  return true;
1906 };
1907 
1908 bool AutoBalancer::adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep& rfootstep, const OpenHRP::AutoBalancerService::Footstep& lfootstep)
1909 {
1910  std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
1911  << "] adjustFootSteps" << std::endl;
1913  Guard guard(m_mutex);
1914  //
1915  hrp::Vector3 eepos, org_mid_rpy, target_mid_rpy;
1916  hrp::Matrix33 eerot, tmprot;
1917  coordinates org_mid_coords, target_mid_coords;
1918  // Get org coords
1919  ikp["rleg"].adjust_interpolation_org_p0 = ikp["rleg"].target_p0;
1920  ikp["lleg"].adjust_interpolation_org_p0 = ikp["lleg"].target_p0;
1921  ikp["rleg"].adjust_interpolation_org_r0 = ikp["rleg"].target_r0;
1922  ikp["lleg"].adjust_interpolation_org_r0 = ikp["lleg"].target_r0;
1923  mid_coords(org_mid_coords, 0.5,
1924  coordinates(ikp["rleg"].adjust_interpolation_org_p0, ikp["rleg"].adjust_interpolation_org_r0),
1925  coordinates(ikp["lleg"].adjust_interpolation_org_p0, ikp["lleg"].adjust_interpolation_org_r0));
1926  org_mid_rpy = hrp::rpyFromRot(org_mid_coords.rot);
1927  // Get target coords
1928  // Input : ee coords
1929  // Output : link coords
1930  memcpy(eepos.data(), rfootstep.pos, sizeof(double)*3);
1931  eerot = (Eigen::Quaternion<double>(rfootstep.rot[0], rfootstep.rot[1], rfootstep.rot[2], rfootstep.rot[3])).normalized().toRotationMatrix(); // rtc:
1932  ikp["rleg"].adjust_interpolation_target_r0 = eerot;
1933  ikp["rleg"].adjust_interpolation_target_p0 = eepos;
1934  memcpy(eepos.data(), lfootstep.pos, sizeof(double)*3);
1935  eerot = (Eigen::Quaternion<double>(lfootstep.rot[0], lfootstep.rot[1], lfootstep.rot[2], lfootstep.rot[3])).normalized().toRotationMatrix(); // rtc:
1936  ikp["lleg"].adjust_interpolation_target_r0 = eerot;
1937  ikp["lleg"].adjust_interpolation_target_p0 = eepos;
1938  mid_coords(target_mid_coords, 0.5,
1939  coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0),
1940  coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
1941  coordinates rtrans, ltrans;
1942  target_mid_coords.transformation(rtrans, coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0));
1943  target_mid_coords.transformation(ltrans, coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0));
1944  target_mid_rpy = hrp::rpyFromRot(target_mid_coords.rot);
1945  // Fix target pos => org pos, target yaw => org yaw
1946  target_mid_rpy(2) = org_mid_rpy(2);
1947  target_mid_coords.rot = hrp::rotFromRpy(target_mid_rpy);
1948  target_mid_coords.pos = org_mid_coords.pos;
1949  // Calculate rleg and lleg coords
1950  coordinates tmpc;
1951  tmpc = target_mid_coords;
1952  tmpc.transform(rtrans);
1953  ikp["rleg"].adjust_interpolation_target_p0 = tmpc.pos;
1954  ikp["rleg"].adjust_interpolation_target_r0 = tmpc.rot;
1955  tmpc = target_mid_coords;
1956  tmpc.transform(ltrans);
1957  ikp["lleg"].adjust_interpolation_target_p0 = tmpc.pos;
1958  ikp["lleg"].adjust_interpolation_target_r0 = tmpc.rot;
1959  // Set interpolator
1961  double tmp = 0.0;
1963  tmp = 1.0;
1965  }
1967  usleep(1000);
1968  usleep(1000);
1969  return true;
1970 };
1971 
1972 bool AutoBalancer::getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long& o_current_fs_idx)
1973 {
1974  std::cerr << "[" << m_profile.instance_name << "] getRemainingFootstepSequence" << std::endl;
1975  o_footstep = new OpenHRP::AutoBalancerService::FootstepSequence;
1976  if (gg_is_walking) {
1977  std::vector< std::vector<step_node> > fsnl = gg->get_remaining_footstep_nodes_list();
1978  o_current_fs_idx = gg->get_footstep_index();
1979  o_footstep->length(fsnl.size());
1980  for (size_t i = 0; i < fsnl.size(); i++) {
1981  o_footstep[i].leg = (fsnl[i].front().l_r==RLEG?"rleg":"lleg");
1982  copyRatscoords2Footstep(o_footstep[i], fsnl[i].front().worldcoords);
1983  }
1984  }
1985  return true;
1986 };
1987 
1988 bool AutoBalancer::getGoPosFootstepsSequence(const double& x, const double& y, const double& th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep)
1989 {
1990  std::cerr << "[" << m_profile.instance_name << "] getGoPosFootstepsSequence" << std::endl;
1991  o_footstep = new OpenHRP::AutoBalancerService::FootstepsSequence;
1992  if (gg_is_walking) {
1993  std::cerr << "[" << m_profile.instance_name << "] Cannot call getGoPosFootstepsSequence in walking" << std::endl;
1994  return false;
1995  } else {
1996  gg->set_all_limbs(leg_names);
1997  std::vector< std::vector<step_node> > new_footstep_nodes_list;
1998  coordinates start_ref_coords;
1999  std::vector<coordinates> initial_support_legs_coords;
2000  std::vector<leg_type> initial_support_legs;
2001  bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords);
2002  if (is_valid_gait_type == false) return false;
2003  /* go_pos_param_2_footstep_nodes_list_core is const member function */
2004  gg->go_pos_param_2_footstep_nodes_list_core (x, y, th,
2005  initial_support_legs_coords, start_ref_coords, initial_support_legs,
2006  new_footstep_nodes_list, true, 0);
2007  o_footstep->length(new_footstep_nodes_list.size());
2008  for (size_t i = 0; i < new_footstep_nodes_list.size(); i++) {
2009  o_footstep[i].fs.length(new_footstep_nodes_list.at(i).size());
2010  for (size_t j = 0; j < new_footstep_nodes_list.at(i).size(); j++) {
2011  leg_type tmp_leg_type = new_footstep_nodes_list.at(i).at(j).l_r;
2012  o_footstep[i].fs[j].leg = ((tmp_leg_type == RLEG) ? "rleg":
2013  (tmp_leg_type == LLEG) ? "lleg":
2014  (tmp_leg_type == RARM) ? "rarm":
2015  "larm");
2016  copyRatscoords2Footstep(o_footstep[i].fs[j], new_footstep_nodes_list.at(i).at(j).worldcoords);
2017  }
2018  }
2019  return true;
2020  }
2021 };
2022 
2023 void AutoBalancer::static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height)
2024 {
2025  hrp::Vector3 target_sbp = hrp::Vector3(0, 0, 0);
2026  hrp::Vector3 tmpcog = m_robot->calcCM();
2027  if ( use_force == MODE_NO_FORCE ) {
2028  tmp_input_sbp = tmpcog + sbp_cog_offset;
2029  } else {
2030  calc_static_balance_point_from_forces(target_sbp, tmpcog, ref_com_height);
2031  tmp_input_sbp = target_sbp - sbp_offset;
2032  sbp_cog_offset = tmp_input_sbp - tmpcog;
2033  }
2034 };
2035 
2036 void AutoBalancer::calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height)
2037 {
2038  hrp::Vector3 denom, nume;
2039  /* sb_point[m] = nume[kg * m/s^2 * m] / denom[kg * m/s^2] */
2040  double mass = m_robot->totalMass();
2041  double mg = mass * gg->get_gravitational_acceleration();
2042  hrp::Vector3 total_sensor_ref_force = hrp::Vector3::Zero();
2043  for (size_t i = 0; i < ref_forces.size(); i++) {
2044  total_sensor_ref_force += ref_forces[i];
2045  }
2046  hrp::Vector3 total_nosensor_ref_force = mg * hrp::Vector3::UnitZ() - total_sensor_ref_force; // total ref force at the point without sensors, such as torso
2047  hrp::Vector3 tmp_ext_moment = fix_leg_coords2.pos.cross(total_nosensor_ref_force) + fix_leg_coords2.rot * hrp::Vector3(m_refFootOriginExtMoment.data.x, m_refFootOriginExtMoment.data.y, m_refFootOriginExtMoment.data.z);
2048  // For MODE_REF_FORCE_RFU_EXT_MOMENT, store previous root position to calculate influence from tmp_ext_moment while walking (basically, root link moves while walking).
2049  // Calculate values via fix_leg_coords2 relative/world values.
2050  static hrp::Vector3 prev_additional_force_applied_pos = fix_leg_coords2.rot.transpose() * (additional_force_applied_link->p-fix_leg_coords2.pos);
2051  // If not is_hold_value (not hold value), update prev_additional_force_applied_pos
2053  prev_additional_force_applied_pos = fix_leg_coords2.rot.transpose() * (additional_force_applied_link->p-fix_leg_coords2.pos);
2054  }
2055  hrp::Vector3 tmp_prev_additional_force_applied_pos = fix_leg_coords2.rot * prev_additional_force_applied_pos + fix_leg_coords2.pos;
2056  // Calculate SBP
2057  for (size_t j = 0; j < 2; j++) {
2058  nume(j) = mg * tmpcog(j);
2059  denom(j) = mg;
2061  //nume(j) += (j==0 ? tmp_ext_moment(1):-tmp_ext_moment(0));
2062  nume(j) += (tmp_prev_additional_force_applied_pos(j)-additional_force_applied_link->p(j))*total_nosensor_ref_force(2) + (j==0 ? tmp_ext_moment(1):-tmp_ext_moment(0));
2063  denom(j) -= total_nosensor_ref_force(2);
2064  } else {
2065  for ( std::map<std::string, ABCIKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
2066  // Check leg_names. leg_names is assumed to be support limb for locomotion, cannot be used for manipulation. If it->first is not included in leg_names, use it for manipulation and static balance point calculation.
2067  if (std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()) {
2068  size_t idx = contact_states_index_map[it->first];
2069  // Force applied point is assumed as end effector
2070  hrp::Vector3 fpos = it->second.target_link->p + it->second.target_link->R * it->second.localPos;
2071  nume(j) += ( (fpos(2) - ref_com_height) * ref_forces[idx](j) - fpos(j) * ref_forces[idx](2) );
2072  nume(j) += (j==0 ? ref_moments[idx](1):-ref_moments[idx](0));
2073  denom(j) -= ref_forces[idx](2);
2074  }
2075  }
2078  nume(j) += ( (fpos(2) - ref_com_height) * total_nosensor_ref_force(j) - fpos(j) * total_nosensor_ref_force(2) );
2079  denom(j) -= total_nosensor_ref_force(2);
2080  }
2081  }
2082  sb_point(j) = nume(j) / denom(j);
2083  }
2084  sb_point(2) = ref_com_height;
2085 };
2086 
2087 #ifndef rad2deg
2088 #define rad2deg(rad) (rad * 180 / M_PI)
2089 #endif
2090 #ifndef deg2rad
2091 #define deg2rad(deg) (deg * M_PI / 180)
2092 #endif
2093 
2095 {
2096  if (graspless_manip_mode) {
2097  hrp::Vector3 dp,dr;
2098  coordinates ref_hand_coords(gg->get_dst_foot_midcoords()), act_hand_coords;
2099  ref_hand_coords.transform(graspless_manip_reference_trans_coords); // desired arm coords
2100  hrp::Vector3 foot_pos(gg->get_dst_foot_midcoords().pos);
2101  if ( graspless_manip_arm == "arms" ) {
2102  hrp::Vector3 rarm_pos = ikp["rarm"].target_p0;
2103  hrp::Vector3 larm_pos = ikp["larm"].target_p0;
2104  act_hand_coords.pos = (rarm_pos+larm_pos)/2.0;
2105  hrp::Vector3 act_y = larm_pos-rarm_pos;
2106  act_y(2) = 0;
2107  act_y.normalize();
2108  hrp::Vector3 ref_y(ref_hand_coords.rot * hrp::Vector3::UnitY());
2109  ref_y(2) = 0;
2110  ref_y.normalize();
2111  dr = hrp::Vector3(0,0,(hrp::Vector3(ref_y.cross(act_y))(2) > 0 ? 1.0 : -1.0) * std::acos(ref_y.dot(act_y))); // fix for rotation
2112  } else {
2113  ABCIKparam& tmpikp = ikp[graspless_manip_arm];
2114  act_hand_coords = rats::coordinates(tmpikp.target_p0,
2115  tmpikp.target_r0);
2116  rats::difference_rotation(dr, ref_hand_coords.rot, act_hand_coords.rot);
2117  dr(0) = 0; dr(1) = 0;
2118  }
2119  dp = act_hand_coords.pos - ref_hand_coords.pos
2120  + dr.cross(hrp::Vector3(foot_pos - act_hand_coords.pos));
2121  dp(2) = 0;
2122  hrp::Matrix33 foot_mt(gg->get_dst_foot_midcoords().rot.transpose());
2123  //alias(dp) = foot_mt * dp;
2124  hrp::Vector3 dp2 = foot_mt * dp;
2125  //alias(dr) = foot_mt * dr;
2126  return hrp::Vector3(graspless_manip_p_gain[0] * dp2(0)/gg->get_default_step_time(),
2127  graspless_manip_p_gain[1] * dp2(1)/gg->get_default_step_time(),
2128  graspless_manip_p_gain[2] * rad2deg(dr(2))/gg->get_default_step_time());
2129  } else {
2130  return hrp::Vector3::Zero();
2131  }
2132 };
2133 
2134 bool AutoBalancer::calc_inital_support_legs(const double& y, std::vector<coordinates>& initial_support_legs_coords, std::vector<leg_type>& initial_support_legs, coordinates& start_ref_coords) {
2135  switch(gait_type) {
2136  case BIPED:
2137  initial_support_legs_coords.assign (1,
2138  y > 0 ?
2139  coordinates(ikp["rleg"].target_p0, ikp["rleg"].target_r0)
2140  : coordinates(ikp["lleg"].target_p0, ikp["lleg"].target_r0));
2141  initial_support_legs.assign (1, y > 0 ? RLEG : LLEG);
2142  break;
2143  case TROT:
2144  initial_support_legs_coords = (y > 0 ?
2145  boost::assign::list_of(coordinates(ikp["rleg"].target_p0, ikp["rleg"].target_r0))(coordinates(ikp["larm"].target_p0, ikp["larm"].target_r0))
2146  : boost::assign::list_of(coordinates(ikp["lleg"].target_p0, ikp["lleg"].target_r0))(coordinates(ikp["rarm"].target_p0, ikp["rarm"].target_r0))).convert_to_container < std::vector<coordinates> > ();
2147  initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM)).convert_to_container < std::vector<leg_type> > ();
2148  break;
2149  case PACE:
2150  initial_support_legs_coords = (y > 0 ?
2151  boost::assign::list_of(coordinates(ikp["rleg"].target_p0, ikp["rleg"].target_r0))(coordinates(ikp["rarm"].target_p0, ikp["rarm"].target_r0))
2152  : boost::assign::list_of(coordinates(ikp["lleg"].target_p0, ikp["lleg"].target_r0))(coordinates(ikp["larm"].target_p0, ikp["larm"].target_r0))).convert_to_container < std::vector<coordinates> > ();
2153  initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM)).convert_to_container < std::vector<leg_type> > ();
2154  break;
2155  case CRAWL:
2156  std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl;
2157  return false;
2158  case GALLOP:
2159  /* at least one leg shoud be in contact */
2160  std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl;
2161  return false;
2162  default: break;
2163  }
2164  start_ref_coords.pos = (ikp["rleg"].target_p0+ikp["lleg"].target_p0)*0.5;
2165  mid_rot(start_ref_coords.rot, 0.5, ikp["rleg"].target_r0, ikp["lleg"].target_r0);
2166  return true;
2167 };
2168 
2169 // TODO : Use same code as ZMPDistributor.h
2170 // Solve A * x = b => x = W A^T (A W A^T)-1 b
2171 // => x = W^{1/2} Pinv(A W^{1/2}) b
2172 // Copied from ZMPDistributor.h
2174 {
2175  hrp::dmatrix W2 = hrp::dmatrix::Zero(W.rows(), W.cols());
2176  for (size_t i = 0; i < W.rows(); i++) W2(i,i) = std::sqrt(W(i,i));
2177  hrp::dmatrix Aw = A*W2;
2178  hrp::dmatrix Aw_inv = hrp::dmatrix::Zero(A.cols(), A.rows());
2179  hrp::calcPseudoInverse(Aw, Aw_inv);
2180  ret = W2 * Aw_inv * b;
2181  //ret = W2 * Aw.colPivHouseholderQr().solve(b);
2182 };
2183 
2185 {
2186  // apply inverse system
2187  // TODO : fix 0.055 (zmp delay)
2188  hrp::Vector3 tmp_ref_zmp = _ref_zmp + 0.055 * (_ref_zmp - prev_ref_zmp) / m_dt;
2189 
2190  std::vector<hrp::Vector3> cop_pos;
2191  std::vector<double> limb_gains;
2192  for (size_t i = 0 ; i < leg_names.size(); i++) {
2193  ABCIKparam& tmpikp = ikp[leg_names[i]];
2194  cop_pos.push_back(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localR * default_zmp_offsets[i]);
2195  limb_gains.push_back(m_contactStates.data[contact_states_index_map[leg_names[i]]] ? 1.0 : 0.0);
2196  }
2197  size_t ee_num = leg_names.size();
2198  size_t state_dim = 6*ee_num;
2199  size_t total_wrench_dim = 5;
2200  // size_t total_fz = m_robot->totalMass() * gg->get_gravitational_acceleration();
2201  size_t total_fz = m_ref_force[0].data[2]+m_ref_force[1].data[2];
2202  //size_t total_wrench_dim = 3;
2203  hrp::dmatrix Wmat = hrp::dmatrix::Identity(state_dim/2, state_dim/2);
2204  hrp::dmatrix Gmat = hrp::dmatrix::Zero(total_wrench_dim, state_dim/2);
2205  // Set Gmat
2206  // Fill Fz
2207  for (size_t j = 0; j < ee_num; j++) {
2208  if (total_wrench_dim == 3) {
2209  Gmat(0,3*j+2) = 1.0;
2210  } else {
2211  for (size_t k = 0; k < 3; k++) Gmat(k,3*j+k) = 1.0;
2212  }
2213  }
2214  // Fill Nx and Ny
2215  for (size_t i = 0; i < total_wrench_dim; i++) {
2216  for (size_t j = 0; j < ee_num; j++) {
2217  if ( i == total_wrench_dim-2 ) { // Nx
2218  Gmat(i,3*j+1) = -(cop_pos[j](2) - tmp_ref_zmp(2));
2219  Gmat(i,3*j+2) = (cop_pos[j](1) - tmp_ref_zmp(1));
2220  } else if ( i == total_wrench_dim-1 ) { // Ny
2221  Gmat(i,3*j) = (cop_pos[j](2) - tmp_ref_zmp(2));
2222  Gmat(i,3*j+2) = -(cop_pos[j](0) - tmp_ref_zmp(0));
2223  }
2224  }
2225  }
2226  // Set Wmat
2227  for (size_t j = 0; j < ee_num; j++) {
2228  for (size_t i = 0; i < 3; i++) {
2229  if (ee_num == 2)
2230  Wmat(i+j*3, i+j*3) = Wmat(i+j*3, i+j*3) * limb_gains[j] * (i==2? 1.0 : 0.01);
2231  else
2232  Wmat(i+j*3, i+j*3) = Wmat(i+j*3, i+j*3) * limb_gains[j];
2233  }
2234  }
2235  // Ret is wrench around cop_pos
2236  // f_cop = f_ee
2237  // n_ee = (cop_pos - ee_pos) x f_cop + n_cop
2238  hrp::dvector ret(state_dim/2);
2239  hrp::dvector total_wrench = hrp::dvector::Zero(total_wrench_dim);
2240  total_wrench(total_wrench_dim-5) = m_ref_force[0].data[0]+m_ref_force[1].data[0];
2241  total_wrench(total_wrench_dim-4) = m_ref_force[0].data[1]+m_ref_force[1].data[1];
2242  total_wrench(total_wrench_dim-3) = total_fz;
2243  calcWeightedLinearEquation(ret, Gmat, Wmat, total_wrench);
2244  if (DEBUGP) {
2245  std::cerr << "[" << m_profile.instance_name << "] distributeReferenceZMPToWrenches" << std::endl;
2246  }
2247  for (size_t i = 0 ; i < leg_names.size(); i++) {
2248  size_t fidx = contact_states_index_map[leg_names[i]];
2249  ABCIKparam& tmpikp = ikp[leg_names[i]];
2250  hrp::Vector3 f_ee(ret(3*i), ret(3*i+1), ret(3*i+2));
2251  //hrp::Vector3 tmp_ee_pos = tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos;
2252  hrp::Vector3 tmp_ee_pos = tmpikp.target_p0;
2253  hrp::Vector3 n_ee = (cop_pos[i]-tmp_ee_pos).cross(f_ee); // n_cop = 0
2254  m_force[fidx].data[0] = f_ee(0);
2255  m_force[fidx].data[1] = f_ee(1);
2256  m_force[fidx].data[2] = f_ee(2);
2257  m_force[fidx].data[3] = n_ee(0);
2258  m_force[fidx].data[4] = n_ee(1);
2259  m_force[fidx].data[5] = n_ee(2);
2260  if (DEBUGP) {
2261  std::cerr << "[" << m_profile.instance_name << "] "
2262  << "ref_force [" << leg_names[i] << "] " << f_ee.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], "
2263  << "ref_moment [" << leg_names[i] << "] " << n_ee.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
2264  }
2265  }
2266  if (DEBUGP) {
2267  std::cerr << "[" << m_profile.instance_name << "] Gmat = " << Gmat.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N,Nm]" << std::endl;
2268  std::cerr << "[" << m_profile.instance_name << "] total_wrench = " << total_wrench.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N,Nm]" << std::endl;
2269  hrp::dvector tmp(total_wrench.size());
2270  tmp = Gmat*ret;
2271  std::cerr << "[" << m_profile.instance_name << "] Gmat*ret = " << tmp.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N,Nm]" << std::endl;
2272  std::cerr << "[" << m_profile.instance_name << "] (Gmat*ret-total_wrench) = " << (tmp-total_wrench).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N,Nm]" << std::endl;
2273  std::cerr << "[" << m_profile.instance_name << "] ret = " << ret.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N,Nm]" << std::endl;
2274  std::cerr << "[" << m_profile.instance_name << "] Wmat(diag) = [";
2275  for (size_t j = 0; j < ee_num; j++) {
2276  for (size_t i = 0; i < 3; i++) {
2277  std::cerr << Wmat(i+j*3, i+j*3) << " ";
2278  }
2279  }
2280  std::cerr << "]" << std::endl;
2281  }
2282 };
2283 
2285 {
2286  switch (use_force) {
2287  case OpenHRP::AutoBalancerService::MODE_NO_FORCE:
2288  return "MODE_NO_FORCE";
2289  case OpenHRP::AutoBalancerService::MODE_REF_FORCE:
2290  return "MODE_REF_FORCE";
2291  case OpenHRP::AutoBalancerService::MODE_REF_FORCE_WITH_FOOT:
2292  return "MODE_REF_FORCE_WITH_FOOT";
2293  case OpenHRP::AutoBalancerService::MODE_REF_FORCE_RFU_EXT_MOMENT:
2294  return "MODE_REF_FORCE_RFU_EXT_MOMENT";
2295  default:
2296  return "";
2297  }
2298 };
2299 
2300 //
2301 extern "C"
2302 {
2303 
2305  {
2307  manager->registerFactory(profile,
2308  RTC::Create<AutoBalancer>,
2309  RTC::Delete<AutoBalancer>);
2310  }
2311 
2312 };
2313 
2314 
ComponentProfile m_profile
TimedOrientation3D m_baseRpy
Definition: AutoBalancer.h:131
png_infop png_charpp int png_charpp profile
TimedBooleanSeq m_contactStates
Definition: AutoBalancer.h:166
TimedDoubleSeq m_toeheelRatio
Definition: AutoBalancer.h:168
InPort< TimedBoolean > m_refFootOriginExtMomentIsHoldValueIn
Definition: AutoBalancer.h:148
bool graspless_manip_mode
Definition: AutoBalancer.h:284
void getOutputParametersForIDLE()
SequenceElement & front(CorbaSequence &seq)
void autobalancer(AutoBalancer *i_autobalancer)
Eigen::MatrixXd dmatrix
TimedPoint3D m_diffCP
Definition: AutoBalancer.h:141
coil::Mutex m_mutex
Definition: AutoBalancer.h:264
void calculateOutputRefForces()
void readInterlockingJointsParamFromProperties(std::vector< std::pair< Link *, Link * > > &pairs, hrp::BodyPtr m_robot, const std::string &prop_string, const std::string &instance_name)
hrp::Vector3 sbp_offset
Definition: AutoBalancer.h:277
rats::coordinates fix_leg_coords2
Definition: AutoBalancer.h:260
void difference_rotation(hrp::Vector3 &ret_dif_rot, const hrp::Matrix33 &self_rot, const hrp::Matrix33 &target_rot)
Definition: RatsMatrix.cpp:40
void multi_mid_coords(coordinates &ret, const std::vector< coordinates > &cs, const double eps)
std::vector< InPort< TimedDoubleSeq > * > m_ref_forceIn
Definition: AutoBalancer.h:138
TimedPoint3D m_zmp
Definition: AutoBalancer.h:133
std::vector< std::string > leg_names
Definition: AutoBalancer.h:257
bool stringTo(To &val, const char *str)
void fixLegToCoords(const hrp::Vector3 &fix_pos, const hrp::Matrix33 &fix_rot)
hrp::Matrix33 rot
Definition: RatsMatrix.h:20
static std::ostream & operator<<(std::ostream &os, const struct RTC::Time &tm)
std::map< std::string, ABCIKparam > ikp
Definition: AutoBalancer.h:254
InPort< TimedPoint3D > m_basePosIn
Definition: AutoBalancer.h:130
void AutoBalancerInit(RTC::Manager *manager)
bool calc_inital_support_legs(const double &y, std::vector< rats::coordinates > &initial_support_legs_coords, std::vector< rats::leg_type > &initial_support_legs, rats::coordinates &start_ref_coords)
double transition_time
Definition: AutoBalancer.h:268
OutPort< TimedPoint3D > m_sbpCogOffsetOut
Definition: AutoBalancer.h:175
InPort< TimedDoubleSeq > m_optionalDataIn
Definition: AutoBalancer.h:136
hrp::Vector3 prev_imu_sensor_pos
Definition: AutoBalancer.h:251
OutPort< TimedDoubleSeq > m_controlSwingSupportTimeOut
Definition: AutoBalancer.h:171
void copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep &out_fs, const rats::coordinates &in_fs)
double transition_interpolator_ratio
Definition: AutoBalancer.h:268
void setName(const std::string &_name)
Definition: interpolator.h:63
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
std::vector< TimedPoint3D > m_limbCOPOffset
Definition: AutoBalancer.h:178
interpolator * adjust_footstep_interpolator
Definition: AutoBalancer.h:271
vstring split(const std::string &input, const std::string &delimiter, bool ignore_empty)
TimedDoubleSeq m_qRef
Definition: AutoBalancer.h:127
void transformation(coordinates &tc, coordinates c, const std::string &wrt=":local") const
Definition: RatsMatrix.h:63
enum AutoBalancer::@2 gait_type
void interpolateLegNamesAndZMPOffsets()
std::vector< hrp::Vector3 > default_zmp_offsets
Definition: AutoBalancer.h:261
CORBA::ORB_ptr getORB()
boost::shared_ptr< JointPathEx > JointPathExPtr
Definition: JointPathEx.h:67
void transform(const coordinates &c, const std::string &wrt=":local")
Definition: RatsMatrix.h:75
virtual RTC::ReturnCode_t onFinalize()
#define for
bool is_hand_fix_mode
Definition: AutoBalancer.h:282
png_uint_32 i
OutPort< TimedDoubleSeq > m_toeheelRatioOut
Definition: AutoBalancer.h:169
Eigen::VectorXd dvector
bool setAutoBalancerParam(const OpenHRP::AutoBalancerService::AutoBalancerParam &i_param)
boost::shared_ptr< SimpleFullbodyInverseKinematicsSolver > fikPtr
Definition: AutoBalancer.h:249
coil::Properties & getProperties()
bool getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam &i_param)
boost::shared_ptr< rats::gait_generator > ggPtr
Definition: AutoBalancer.h:245
static Manager & instance()
Matrix33 localR
hrp::Link * additional_force_applied_link
Definition: AutoBalancer.h:293
hrp::Vector3 calcFootMidPosUsingZMPWeightMap()
static double no_using_toe_heel_ratio
bool addOutPort(const char *name, OutPortBase &outport)
double zmp_transition_time
Definition: AutoBalancer.h:268
coordinates worldcoords
Definition: GaitGenerator.h:33
boost::shared_ptr< Body > BodyPtr
virtual ~AutoBalancer()
OutPort< TimedBooleanSeq > m_contactStatesOut
Definition: AutoBalancer.h:167
void updateWalkingVelocityFromHandError(rats::coordinates &tmp_fix_coords)
Eigen::Vector3d Vector3
bool is_hand_fix_initial
Definition: AutoBalancer.h:282
Matrix33 rotFromRpy(const Vector3 &rpy)
bool setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence &fs, const OpenHRP::AutoBalancerService::StepParamSequence &sps, CORBA::Long overwrite_fs_idx)
bool emergencyStop()
OutPort< TimedDoubleSeq > m_baseTformOut
Definition: AutoBalancer.h:161
Eigen::Matrix3d Matrix33
bool adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep &rfootstep, const OpenHRP::AutoBalancerService::Footstep &lfootstep)
void updateTargetCoordsForHandFixMode(rats::coordinates &tmp_fix_coords)
std::vector< std::string > vstring
void setMatrix33ToRowMajorArray(const Matrix33 &m33, Array &a, size_t top=0)
double leg_names_interpolator_ratio
Definition: AutoBalancer.h:268
std::string getUseForceModeString()
bool getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalancerParam &i_param)
hrp::Matrix33 OrientRotationMatrix(const hrp::Matrix33 &rot, const hrp::Vector3 &axis1, const hrp::Vector3 &axis2)
std::map< std::string, size_t > contact_states_index_map
Definition: AutoBalancer.h:255
TimedBooleanSeq m_actContactStates
Definition: AutoBalancer.h:143
coil::Properties & getConfig()
OutPort< TimedPoint3D > m_cogOut
Definition: AutoBalancer.h:181
std::vector< std::string > sensor_names
Definition: AutoBalancer.h:257
void getOutputParametersForWalking()
hrp::Vector3 hand_fix_initial_offset
Definition: AutoBalancer.h:251
bool getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGeneratorParam &i_param)
void rotm3times(hrp::Matrix33 &m12, const hrp::Matrix33 &m1, const hrp::Matrix33 &m2)
Definition: RatsMatrix.cpp:31
AutoBalancer(RTC::Manager *manager)
unsigned int m_debugLevel
Definition: AutoBalancer.h:281
virtual RTC::ReturnCode_t onInitialize()
ExecutionContextHandle_t UniqueId
std::string graspless_manip_arm
Definition: AutoBalancer.h:285
interpolator * transition_interpolator
Definition: AutoBalancer.h:270
std::vector< hrp::Vector3 > ref_moments
Definition: AutoBalancer.h:279
std::map< std::string, hrp::VirtualForceSensorParam > m_vfs
Definition: AutoBalancer.h:256
interpolator * leg_names_interpolator
Definition: AutoBalancer.h:272
InPort< TimedLong > m_emergencySignalIn
Definition: AutoBalancer.h:140
hrp::Vector3 input_zmp
Definition: AutoBalancer.h:273
TimedPoint3D m_refFootOriginExtMoment
Definition: AutoBalancer.h:145
#define DEBUGP
hrp::Vector3 ref_zmp
Definition: AutoBalancer.h:251
std::vector< std::string > ee_vec
Definition: AutoBalancer.h:257
bool startAutoBalancer(const ::OpenHRP::AutoBalancerService::StrSequence &limbs)
hrp::Vector3 sbp_cog_offset
Definition: AutoBalancer.h:277
bool bindParameter(const char *param_name, VarType &var, const char *def_val, bool(*trans)(VarType &, const char *)=coil::stringTo)
hrp::Vector3 prev_ref_zmp
Definition: AutoBalancer.h:251
def j(str, encoding="cp932")
Vector3 localPos
CORBA::Long find(const CorbaSequence &seq, Functor f)
RTC::OutPort< RTC::TimedPoint3D > m_zmpOut
Definition: AutoBalancer.h:157
static const char * autobalancer_spec[]
hrp::Vector3 prev_imu_sensor_vel
Definition: AutoBalancer.h:251
int loadBodyFromModelLoader(::World *world, const char *name, const char *url, CosNaming::NamingContext_var cxt)
void getTargetParameters()
std::vector< OutPort< TimedDoubleSeq > * > m_ref_forceOut
Definition: AutoBalancer.h:177
InPort< TimedPoint3D > m_zmpIn
Definition: AutoBalancer.h:134
enum AutoBalancer::@3 control_mode
std::vector< TimedDoubleSeq > m_ref_force
Definition: AutoBalancer.h:137
TimedPoint3D m_sbpCogOffset
Definition: AutoBalancer.h:174
OutPort< TimedPoint3D > m_basePosOut
Definition: AutoBalancer.h:158
autobalancer component
AutoBalancerService_impl m_service0
Definition: AutoBalancer.h:193
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
TimedAcceleration3D m_accRef
Definition: AutoBalancer.h:164
OutPort< TimedAcceleration3D > m_accRefOut
Definition: AutoBalancer.h:165
bool getGoPosFootstepsSequence(const double &x, const double &y, const double &th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep)
int calcPseudoInverse(const dmatrix &_a, dmatrix &_a_pseu, double _sv_ratio)
void calcFixCoordsForAdjustFootstep(rats::coordinates &tmp_fix_coords)
TimedDoubleSeq m_optionalData
Definition: AutoBalancer.h:135
void getOutputParametersForABC()
TimedBoolean m_walkingStates
Definition: AutoBalancer.h:172
prop
OutPort< TimedDoubleSeq > m_qOut
Definition: AutoBalancer.h:156
def norm(a)
void rotateRefForcesForFixCoords(rats::coordinates &tmp_fix_coords)
bool is_legged_robot
Definition: AutoBalancer.h:282
Definition: defs.h:6
HRP_UTIL_EXPORT Vector3 rpyFromRot(const Matrix33 &m)
hrp::Vector3 graspless_manip_p_gain
Definition: AutoBalancer.h:286
std::vector< hrp::Vector3 > ref_forces
Definition: AutoBalancer.h:279
void readVirtualForceSensorParamFromProperties(std::map< std::string, hrp::VirtualForceSensorParam > &vfs, hrp::BodyPtr m_robot, const std::string &prop_string, const std::string &instance_name)
naming
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
Vector3 cross(const Vector3 &v1, const Vector3 &v2)
void solveFullbodyIK()
TimedPoint3D m_cog
Definition: AutoBalancer.h:150
void calcReferenceJointAnglesForIK()
OutPort< TimedPose3D > m_basePoseOut
Definition: AutoBalancer.h:163
InPort< TimedBooleanSeq > m_actContactStatesIn
Definition: AutoBalancer.h:144
bool goVelocity(const double &vx, const double &vy, const double &vth)
std::vector< OutPort< TimedPoint3D > * > m_limbCOPOffsetOut
Definition: AutoBalancer.h:179
InPort< TimedDoubleSeq > m_qRefIn
Definition: AutoBalancer.h:128
TimedDoubleSeq m_baseTform
Definition: AutoBalancer.h:160
InPort< TimedPoint3D > m_refFootOriginExtMomentIn
Definition: AutoBalancer.h:146
void waitABCTransition()
void registerInPort(const char *name, InPortBase &inport)
InPort< TimedOrientation3D > m_baseRpyIn
Definition: AutoBalancer.h:132
virtual bool isNew()
hrp::BodyPtr m_robot
Definition: AutoBalancer.h:263
void get(double *x_, bool popp=true)
void fixLegToCoords2(rats::coordinates &tmp_fix_coords)
TimedDoubleSeq m_controlSwingSupportTime
Definition: AutoBalancer.h:170
bool addPort(PortBase &port)
virtual bool write(DataType &value)
RTC::CorbaPort m_AutoBalancerServicePort
Definition: AutoBalancer.h:187
hrp::Vector3 additional_force_applied_point_offset
Definition: AutoBalancer.h:294
rats::coordinates fix_leg_coords
Definition: AutoBalancer.h:260
hrp::BodyPtr m_robot
bool gg_is_walking
Definition: AutoBalancer.h:247
hrp::Vector3 pos
Definition: RatsMatrix.h:19
InPort< TimedPoint3D > m_diffCPIn
Definition: AutoBalancer.h:142
TimedPoint3D m_basePos
Definition: AutoBalancer.h:129
TimedPose3D m_basePose
Definition: AutoBalancer.h:162
void distributeReferenceZMPToWrenches(const hrp::Vector3 &_ref_zmp)
#define rad2deg(rad)
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
bool setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence &fs, CORBA::Long overwrite_fs_idx)
JSAMPIMAGE data
OutPort< TimedOrientation3D > m_baseRpyOut
Definition: AutoBalancer.h:159
enum AutoBalancer::@4 use_force
int num
void calcWeightedLinearEquation(hrp::dvector &ret, const hrp::dmatrix &A, const hrp::dmatrix &W, const hrp::dvector &b)
hrp::Vector3 ref_cog
Definition: AutoBalancer.h:251
hrp::Matrix33 target_root_R
Definition: AutoBalancer.h:259
bool releaseEmergencyStop()
bool getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long &o_current_fs_idx)
std::vector< TimedDoubleSeq > m_force
Definition: AutoBalancer.h:176
bool addInPort(const char *name, InPortBase &inport)
interpolator * zmp_offset_interpolator
Definition: AutoBalancer.h:269
bool stopAutoBalancer()
void static_balance_point_proc_one(hrp::Vector3 &tmp_input_sbp, const double ref_com_height)
void setGoal(const double *gx, const double *gv, double time, bool online=true)
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)
hrp::Vector3 calc_vel_from_hand_error(const rats::coordinates &tmp_fix_coords)
OutPort< TimedBoolean > m_walkingStatesOut
Definition: AutoBalancer.h:173
hrp::Matrix33 input_baseRot
Definition: AutoBalancer.h:274
double adjust_footstep_transition_time
Definition: AutoBalancer.h:268
rats::coordinates graspless_manip_reference_trans_coords
Definition: AutoBalancer.h:287
bool isOptionalDataContact(const std::string &ee_name)
Definition: AutoBalancer.h:237
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
void waitFootStepsEarly(const double tm)
void calc_static_balance_point_from_forces(hrp::Vector3 &sb_point, const hrp::Vector3 &tmpcog, const double ref_com_height)
Definition: defs.h:6
bool setGaitGeneratorParam(const OpenHRP::AutoBalancerService::GaitGeneratorParam &i_param)
void startABCparam(const ::OpenHRP::AutoBalancerService::StrSequence &limbs)
hrp::Vector3 input_basePos
Definition: AutoBalancer.h:273
void waitFootSteps()
coil::Guard< coil::Mutex > Guard
void set(const double *x, const double *v=NULL)
hrp::Vector3 target_root_p
Definition: AutoBalancer.h:258
TimedBoolean m_refFootOriginExtMomentIsHoldValue
Definition: AutoBalancer.h:147
void mid_coords(coordinates &mid_coords, const double p, const coordinates &c1, const coordinates &c2, const double eps)
Definition: RatsMatrix.cpp:58
bool goPos(const double &x, const double &y, const double &th)
int usleep(useconds_t usec)
bool registerProvider(const char *instance_name, const char *type_name, PortableServer::RefCountServantBase &provider)


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