Stabilizer.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 "Stabilizer.h"
15 #include "hrpsys/util/VectorConvert.h"
16 #include <math.h>
17 #include <boost/lambda/lambda.hpp>
18 
20 
21 #ifndef deg2rad
22 #define deg2rad(x) ((x) * M_PI / 180.0)
23 #endif
24 #ifndef rad2deg
25 #define rad2deg(rad) (rad * 180 / M_PI)
26 #endif
27 
28 // Module specification
29 // <rtc-template block="module_spec">
30 static const char* stabilizer_spec[] =
31  {
32  "implementation_id", "Stabilizer",
33  "type_name", "Stabilizer",
34  "description", "stabilizer",
35  "version", HRPSYS_PACKAGE_VERSION,
36  "vendor", "AIST",
37  "category", "example",
38  "activity_type", "DataFlowComponent",
39  "max_instance", "10",
40  "language", "C++",
41  "lang_type", "compile",
42  // Configuration variables
43  "conf.default.debugLevel", "0",
44  ""
45  };
46 // </rtc-template>
47 
48 static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm)
49 {
50  int pre = os.precision();
51  os.setf(std::ios::fixed);
52  os << std::setprecision(6)
53  << (tm.sec + tm.nsec/1e9)
54  << std::setprecision(pre);
55  os.unsetf(std::ios::fixed);
56  return os;
57 }
58 
59 static double switching_inpact_absorber(double force, double lower_th, double upper_th);
60 
62  : RTC::DataFlowComponentBase(manager),
63  // <rtc-template block="initializer">
64  m_qCurrentIn("qCurrent", m_qCurrent),
65  m_qRefIn("qRef", m_qRef),
66  m_rpyIn("rpy", m_rpy),
67  m_zmpRefIn("zmpRef", m_zmpRef),
68  m_StabilizerServicePort("StabilizerService"),
69  m_basePosIn("basePosIn", m_basePos),
70  m_baseRpyIn("baseRpyIn", m_baseRpy),
71  m_contactStatesIn("contactStates", m_contactStates),
72  m_toeheelRatioIn("toeheelRatio", m_toeheelRatio),
73  m_controlSwingSupportTimeIn("controlSwingSupportTime", m_controlSwingSupportTime),
74  m_qRefSeqIn("qRefSeq", m_qRefSeq),
75  m_walkingStatesIn("walkingStates", m_walkingStates),
76  m_sbpCogOffsetIn("sbpCogOffset", m_sbpCogOffset),
77  m_qRefOut("q", m_qRef),
78  m_tauOut("tau", m_tau),
79  m_zmpOut("zmp", m_zmp),
80  m_refCPOut("refCapturePoint", m_refCP),
81  m_actCPOut("actCapturePoint", m_actCP),
82  m_diffCPOut("diffCapturePoint", m_diffCP),
83  m_diffFootOriginExtMomentOut("diffFootOriginExtMoment", m_diffFootOriginExtMoment),
84  m_actContactStatesOut("actContactStates", m_actContactStates),
85  m_COPInfoOut("COPInfo", m_COPInfo),
86  m_emergencySignalOut("emergencySignal", m_emergencySignal),
87  // for debug output
88  m_originRefZmpOut("originRefZmp", m_originRefZmp),
89  m_originRefCogOut("originRefCog", m_originRefCog),
90  m_originRefCogVelOut("originRefCogVel", m_originRefCogVel),
91  m_originNewZmpOut("originNewZmp", m_originNewZmp),
92  m_originActZmpOut("originActZmp", m_originActZmp),
93  m_originActCogOut("originActCog", m_originActCog),
94  m_originActCogVelOut("originActCogVel", m_originActCogVel),
95  m_actBaseRpyOut("actBaseRpy", m_actBaseRpy),
96  m_currentBasePosOut("currentBasePos", m_currentBasePos),
97  m_currentBaseRpyOut("currentBaseRpy", m_currentBaseRpy),
98  m_allRefWrenchOut("allRefWrench", m_allRefWrench),
99  m_allEECompOut("allEEComp", m_allEEComp),
100  m_debugDataOut("debugData", m_debugData),
101  control_mode(MODE_IDLE),
102  st_algorithm(OpenHRP::StabilizerService::TPCC),
103  emergency_check_mode(OpenHRP::StabilizerService::NO_CHECK),
104  szd(NULL),
105  // </rtc-template>
106  m_debugLevel(0)
107 {
108  m_service0.stabilizer(this);
109 }
110 
112 {
113 }
114 
115 RTC::ReturnCode_t Stabilizer::onInitialize()
116 {
117  std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
118  // <rtc-template block="bind_config">
119  // Bind variables and configuration variable
120  bindParameter("debugLevel", m_debugLevel, "0");
121 
122  // </rtc-template>
123 
124  // Registration: InPort/OutPort/Service
125  // <rtc-template block="registration">
126  // Set InPort buffers
127  addInPort("qCurrent", m_qCurrentIn);
128  addInPort("qRef", m_qRefIn);
129  addInPort("rpy", m_rpyIn);
130  addInPort("zmpRef", m_zmpRefIn);
131  addInPort("basePosIn", m_basePosIn);
132  addInPort("baseRpyIn", m_baseRpyIn);
133  addInPort("contactStates", m_contactStatesIn);
134  addInPort("toeheelRatio", m_toeheelRatioIn);
135  addInPort("controlSwingSupportTime", m_controlSwingSupportTimeIn);
136  addInPort("qRefSeq", m_qRefSeqIn);
137  addInPort("walkingStates", m_walkingStatesIn);
138  addInPort("sbpCogOffset", m_sbpCogOffsetIn);
139 
140  // Set OutPort buffer
141  addOutPort("q", m_qRefOut);
142  addOutPort("tau", m_tauOut);
143  addOutPort("zmp", m_zmpOut);
144  addOutPort("refCapturePoint", m_refCPOut);
145  addOutPort("actCapturePoint", m_actCPOut);
146  addOutPort("diffCapturePoint", m_diffCPOut);
147  addOutPort("diffStaticBalancePointOffset", m_diffFootOriginExtMomentOut);
148  addOutPort("actContactStates", m_actContactStatesOut);
149  addOutPort("COPInfo", m_COPInfoOut);
150  addOutPort("emergencySignal", m_emergencySignalOut);
151  // for debug output
152  addOutPort("originRefZmp", m_originRefZmpOut);
153  addOutPort("originRefCog", m_originRefCogOut);
154  addOutPort("originRefCogVel", m_originRefCogVelOut);
155  addOutPort("originNewZmp", m_originNewZmpOut);
156  addOutPort("originActZmp", m_originActZmpOut);
157  addOutPort("originActCog", m_originActCogOut);
158  addOutPort("originActCogVel", m_originActCogVelOut);
159  addOutPort("actBaseRpy", m_actBaseRpyOut);
160  addOutPort("currentBasePos", m_currentBasePosOut);
161  addOutPort("currentBaseRpy", m_currentBaseRpyOut);
162  addOutPort("allRefWrench", m_allRefWrenchOut);
163  addOutPort("allEEComp", m_allEECompOut);
164  addOutPort("debugData", m_debugDataOut);
165 
166  // Set service provider to Ports
167  m_StabilizerServicePort.registerProvider("service0", "StabilizerService", m_service0);
168 
169  // Set service consumers to Ports
170 
171  // Set CORBA Service Ports
173 
174  // </rtc-template>
176  coil::stringTo(dt, prop["dt"].c_str());
177 
178  // parameters for corba
179  RTC::Manager& rtcManager = RTC::Manager::instance();
180  std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
181  int comPos = nameServer.find(",");
182  if (comPos < 0){
183  comPos = nameServer.length();
184  }
185  nameServer = nameServer.substr(0, comPos);
186  RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
187 
188  // parameters for internal robot model
189  m_robot = hrp::BodyPtr(new hrp::Body());
190  if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
191  CosNaming::NamingContext::_duplicate(naming.getRootContext())
192  )){
193  std::cerr << "[" << m_profile.instance_name << "]failed to load model[" << prop["model"] << "]" << std::endl;
194  return RTC::RTC_ERROR;
195  }
196 
197  // Setting for wrench data ports (real + virtual)
198  std::vector<std::string> force_sensor_names;
199 
200  // Find names for real force sensors
201  int npforce = m_robot->numSensors(hrp::Sensor::FORCE);
202  for (unsigned int i=0; i<npforce; ++i) {
203  force_sensor_names.push_back(m_robot->sensor(hrp::Sensor::FORCE, i)->name);
204  }
205 
206  // load virtual force sensors
207  readVirtualForceSensorParamFromProperties(m_vfs, m_robot, prop["virtual_force_sensor"], std::string(m_profile.instance_name));
208  int nvforce = m_vfs.size();
209  for (unsigned int i=0; i<nvforce; ++i) {
210  for ( std::map<std::string, hrp::VirtualForceSensorParam>::iterator it = m_vfs.begin(); it != m_vfs.end(); it++ ) {
211  if (it->second.id == i) {
212  force_sensor_names.push_back(it->first);
213  }
214  }
215  }
216 
217  int nforce = npforce + nvforce;
218  m_wrenches.resize(nforce);
219  m_wrenchesIn.resize(nforce);
220  m_ref_wrenches.resize(nforce);
221  m_ref_wrenchesIn.resize(nforce);
222  m_limbCOPOffset.resize(nforce);
223  m_limbCOPOffsetIn.resize(nforce);
224  std::cerr << "[" << m_profile.instance_name << "] force sensor ports (" << npforce << ")" << std::endl;
225  for (unsigned int i=0; i<nforce; ++i) {
226  std::string force_sensor_name = force_sensor_names[i];
227  // actual inport
228  m_wrenchesIn[i] = new RTC::InPort<RTC::TimedDoubleSeq>(force_sensor_name.c_str(), m_wrenches[i]);
229  m_wrenches[i].data.length(6);
230  registerInPort(force_sensor_name.c_str(), *m_wrenchesIn[i]);
231  // referecen inport
232  m_ref_wrenchesIn[i] = new RTC::InPort<RTC::TimedDoubleSeq>(std::string(force_sensor_name+"Ref").c_str(), m_ref_wrenches[i]);
233  m_ref_wrenches[i].data.length(6);
234  registerInPort(std::string(force_sensor_name+"Ref").c_str(), *m_ref_wrenchesIn[i]);
235  std::cerr << "[" << m_profile.instance_name << "] name = " << force_sensor_name << std::endl;
236  }
237  std::cerr << "[" << m_profile.instance_name << "] limbCOPOffset ports (" << npforce << ")" << std::endl;
238  for (unsigned int i=0; i<nforce; ++i) {
239  std::string force_sensor_name = force_sensor_names[i];
240  std::string nm("limbCOPOffset_"+force_sensor_name);
242  registerInPort(nm.c_str(), *m_limbCOPOffsetIn[i]);
243  std::cerr << "[" << m_profile.instance_name << "] name = " << nm << std::endl;
244  }
245 
246  // setting from conf file
247  // rleg,TARGET_LINK,BASE_LINK,x,y,z,rx,ry,rz,rth #<=pos + rot (axis+angle)
248  coil::vstring end_effectors_str = coil::split(prop["end_effectors"], ",");
249  if (end_effectors_str.size() > 0) {
250  size_t prop_num = 10;
251  size_t num = end_effectors_str.size()/prop_num;
252  for (size_t i = 0; i < num; i++) {
253  std::string ee_name, ee_target, ee_base;
254  coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str());
255  coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str());
256  coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str());
257  STIKParam ikp;
258  for (size_t j = 0; j < 3; j++) {
259  coil::stringTo(ikp.localp(j), end_effectors_str[i*prop_num+3+j].c_str());
260  }
261  double tmpv[4];
262  for (int j = 0; j < 4; j++ ) {
263  coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
264  }
265  ikp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
266  ikp.target_name = ee_target;
267  ikp.ee_name = ee_name;
268  {
269  bool is_ee_exists = false;
270  for (size_t j = 0; j < npforce; j++) {
271  hrp::Sensor* sensor = m_robot->sensor(hrp::Sensor::FORCE, j);
272  hrp::Link* alink = m_robot->link(ikp.target_name);
273  while (alink != NULL && alink->name != ee_base && !is_ee_exists) {
274  if ( alink->name == sensor->link->name ) {
275  is_ee_exists = true;
276  ikp.sensor_name = sensor->name;
277  }
278  alink = alink->parent;
279  }
280  }
281  }
282  ikp.avoid_gain = 0.001;
283  ikp.reference_gain = 0.01;
284  ikp.ik_loop_count = 3;
285  // For swing ee modification
286  ikp.target_ee_diff_p = hrp::Vector3::Zero();
287  ikp.target_ee_diff_r = hrp::Matrix33::Identity();
288  ikp.d_rpy_swing = hrp::Vector3::Zero();
289  ikp.d_pos_swing = hrp::Vector3::Zero();
292  ikp.prev_d_pos_swing = hrp::Vector3::Zero();
293  ikp.prev_d_rpy_swing = hrp::Vector3::Zero();
294  //
295  stikp.push_back(ikp);
296  jpe_v.push_back(hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), dt, false, std::string(m_profile.instance_name))));
297  // Fix for toe joint
298  if (ee_name.find("leg") != std::string::npos && jpe_v.back()->numJoints() == 7) { // leg and has 7dof joint (6dof leg +1dof toe)
299  std::vector<double> optw;
300  for (int j = 0; j < jpe_v.back()->numJoints(); j++ ) {
301  if ( j == jpe_v.back()->numJoints()-1 ) optw.push_back(0.0);
302  else optw.push_back(1.0);
303  }
304  jpe_v.back()->setOptionalWeightVector(optw);
305  }
306  target_ee_p.push_back(hrp::Vector3::Zero());
307  target_ee_R.push_back(hrp::Matrix33::Identity());
308  act_ee_p.push_back(hrp::Vector3::Zero());
309  act_ee_R.push_back(hrp::Matrix33::Identity());
310  projected_normal.push_back(hrp::Vector3::Zero());
311  act_force.push_back(hrp::Vector3::Zero());
312  ref_force.push_back(hrp::Vector3::Zero());
313  ref_moment.push_back(hrp::Vector3::Zero());
314  contact_states_index_map.insert(std::pair<std::string, size_t>(ee_name, i));
315  is_ik_enable.push_back( (ee_name.find("leg") != std::string::npos ? true : false) ); // Hands ik => disabled, feet ik => enabled, by default
316  is_feedback_control_enable.push_back( (ee_name.find("leg") != std::string::npos ? true : false) ); // Hands feedback control => disabled, feet feedback control => enabled, by default
317  is_zmp_calc_enable.push_back( (ee_name.find("leg") != std::string::npos ? true : false) ); // To zmp calculation, hands are disabled and feet are enabled, by default
318  std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl;
319  std::cerr << "[" << m_profile.instance_name << "] target = " << m_robot->link(ikp.target_name)->name << ", base = " << ee_base << ", sensor_name = " << ikp.sensor_name << std::endl;
320  std::cerr << "[" << m_profile.instance_name << "] offset_pos = " << ikp.localp.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl;
321  prev_act_force_z.push_back(0.0);
322  }
323  m_contactStates.data.length(num);
324  m_toeheelRatio.data.length(num);
325  m_will_fall_counter.resize(num);
326  }
327 
328  std::vector<std::pair<hrp::Link*, hrp::Link*> > interlocking_joints;
329  readInterlockingJointsParamFromProperties(interlocking_joints, m_robot, prop["interlocking_joints"], std::string(m_profile.instance_name));
330  if (interlocking_joints.size() > 0) {
331  for (size_t i = 0; i < jpe_v.size(); i++) {
332  std::cerr << "[" << m_profile.instance_name << "] Interlocking Joints for [" << stikp[i].ee_name << "]" << std::endl;
333  jpe_v[i]->setInterlockingJointPairIndices(interlocking_joints, std::string(m_profile.instance_name));
334  }
335  }
336 
337 
338  // parameters for TPCC
339  act_zmp = hrp::Vector3::Zero();
340  for (int i = 0; i < 2; i++) {
341  k_tpcc_p[i] = 0.2;
342  k_tpcc_x[i] = 4.0;
343  k_brot_p[i] = 0.1;
344  k_brot_tc[i] = 1.5;
345  }
346  // parameters for EEFM
347  double k_ratio = 0.9;
348  for (int i = 0; i < 2; i++) {
349  eefm_k1[i] = -1.41429*k_ratio;
350  eefm_k2[i] = -0.404082*k_ratio;
351  eefm_k3[i] = -0.18*k_ratio;
354  }
355  for (size_t i = 0; i < stikp.size(); i++) {
356  STIKParam& ikp = stikp[i];
357  hrp::Link* root = m_robot->link(ikp.target_name);
358  ikp.eefm_rot_damping_gain = hrp::Vector3(20*5, 20*5, 1e5);
359  ikp.eefm_rot_time_const = hrp::Vector3(1.5, 1.5, 1.5);
361  ikp.eefm_swing_rot_spring_gain = hrp::Vector3(0.0, 0.0, 0.0);
362  ikp.eefm_swing_rot_time_const = hrp::Vector3(1.5, 1.5, 1.5);
363  ikp.eefm_pos_damping_gain = hrp::Vector3(3500*10, 3500*10, 3500);
364  ikp.eefm_pos_time_const_support = hrp::Vector3(1.5, 1.5, 1.5);
365  ikp.eefm_pos_compensation_limit = 0.025;
366  ikp.eefm_swing_pos_spring_gain = hrp::Vector3(0.0, 0.0, 0.0);
367  ikp.eefm_swing_pos_time_const = hrp::Vector3(1.5, 1.5, 1.5);
368  ikp.eefm_ee_moment_limit = hrp::Vector3(1e4, 1e4, 1e4); // Default limit [Nm] is too large. Same as no limit.
369  if (ikp.ee_name.find("leg") == std::string::npos) { // Arm default
370  ikp.eefm_ee_forcemoment_distribution_weight = Eigen::Matrix<double, 6,1>::Zero();
371  } else { // Leg default
372  for (size_t j = 0; j < 3; j++) {
373  ikp.eefm_ee_forcemoment_distribution_weight[j] = 1; // Force
374  ikp.eefm_ee_forcemoment_distribution_weight[j+3] = 1e-2; // Moment
375  }
376  }
377  ikp.max_limb_length = 0.0;
378  while (!root->isRoot()) {
379  ikp.max_limb_length += root->b.norm();
380  ikp.parent_name = root->name;
381  root = root->parent;
382  }
383  ikp.limb_length_margin = 0.13;
384  ikp.support_time = 0.0;
385  }
386  eefm_swing_rot_damping_gain = hrp::Vector3(20*5, 20*5, 1e5);
387  eefm_swing_pos_damping_gain = hrp::Vector3(33600, 33600, 7000);
390  eefm_pos_margin_time = 0.02;
392  //eefm_leg_inside_margin = 0.065; // [m]
393  //eefm_leg_front_margin = 0.05;
394  //eefm_leg_rear_margin = 0.05;
395  //fm_wrench_alpha_blending = 1.0; // fz_alpha
396  eefm_gravitational_acceleration = 9.80665; // [m/s^2]
397  cop_check_margin = 20.0*1e-3; // [m]
398  cp_check_margin.resize(4, 30*1e-3); // [m]
399  cp_offset = hrp::Vector3(0.0, 0.0, 0.0); // [m]
400  tilt_margin.resize(2, 30 * M_PI / 180); // [rad]
401  contact_decision_threshold = 50; // [N]
403  eefm_use_swing_damping = false;
404  eefm_swing_damping_force_thre.resize(3, 300);
405  eefm_swing_damping_moment_thre.resize(3, 15);
407  is_walking = false;
408  is_estop_while_walking = false;
409  sbp_cog_offset = hrp::Vector3(0.0, 0.0, 0.0);
411  use_zmp_truncation = false;
413  limb_stretch_avoidance_vlimit[0] = -100 * 1e-3 * dt; // lower limit
414  limb_stretch_avoidance_vlimit[1] = 50 * 1e-3 * dt; // upper limit
416  detection_count_to_air = static_cast<int>(0.0 / dt);
417 
418  // parameters for RUNST
419  double ke = 0, tc = 0;
420  for (int i = 0; i < 2; i++) {
421  m_tau_x[i].setup(ke, tc, dt);
422  m_tau_x[i].setErrorPrefix(std::string(m_profile.instance_name));
423  m_tau_y[i].setup(ke, tc, dt);
424  m_tau_y[i].setErrorPrefix(std::string(m_profile.instance_name));
425  m_f_z.setup(ke, tc, dt);
426  m_f_z.setErrorPrefix(std::string(m_profile.instance_name));
427  }
428  pangx_ref = pangy_ref = pangx = pangy = 0;
429  rdx = rdy = rx = ry = 0;
430  pdr = hrp::Vector3::Zero();
431 
432  // Check is legged robot or not
433  is_legged_robot = false;
434  for (size_t i = 0; i < stikp.size(); i++) {
435  if (stikp[i].ee_name.find("leg") == std::string::npos) continue;
436  hrp::Sensor* sen= m_robot->sensor<hrp::ForceSensor>(stikp[i].sensor_name);
437  if ( sen != NULL ) is_legged_robot = true;
438  }
439  is_emergency = false;
440  reset_emergency_flag = false;
441 
442  m_qCurrent.data.length(m_robot->numJoints());
443  m_qRef.data.length(m_robot->numJoints());
444  m_tau.data.length(m_robot->numJoints());
445  transition_joint_q.resize(m_robot->numJoints());
446  qorg.resize(m_robot->numJoints());
447  qrefv.resize(m_robot->numJoints());
448  transition_count = 0;
449  loop = 0;
451  is_air_counter = 0;
452  total_mass = m_robot->totalMass();
453  ref_zmp_aux = hrp::Vector3::Zero();
454  m_actContactStates.data.length(m_contactStates.data.length());
455  for (size_t i = 0; i < m_contactStates.data.length(); i++) {
456  ref_contact_states.push_back(true);
457  prev_ref_contact_states.push_back(true);
458  m_actContactStates.data[i] = false;
459  act_contact_states.push_back(false);
460  toeheel_ratio.push_back(1.0);
461  }
462  m_COPInfo.data.length(m_contactStates.data.length()*3); // nx, ny, fz for each end-effectors
463  for (size_t i = 0; i < m_COPInfo.data.length(); i++) {
464  m_COPInfo.data[i] = 0.0;
465  }
466  transition_time = 2.0;
467  foot_origin_offset[0] = hrp::Vector3::Zero();
468  foot_origin_offset[1] = hrp::Vector3::Zero();
469 
470  //
472 
473  // for debug output
474  m_originRefZmp.data.x = m_originRefZmp.data.y = m_originRefZmp.data.z = 0.0;
475  m_originRefCog.data.x = m_originRefCog.data.y = m_originRefCog.data.z = 0.0;
476  m_originRefCogVel.data.x = m_originRefCogVel.data.y = m_originRefCogVel.data.z = 0.0;
477  m_originNewZmp.data.x = m_originNewZmp.data.y = m_originNewZmp.data.z = 0.0;
478  m_originActZmp.data.x = m_originActZmp.data.y = m_originActZmp.data.z = 0.0;
479  m_originActCog.data.x = m_originActCog.data.y = m_originActCog.data.z = 0.0;
480  m_originActCogVel.data.x = m_originActCogVel.data.y = m_originActCogVel.data.z = 0.0;
481  m_allRefWrench.data.length(stikp.size() * 6); // 6 is wrench dim
482  m_allEEComp.data.length(stikp.size() * 6); // 6 is pos+rot dim
483  m_debugData.data.length(1); m_debugData.data[0] = 0.0;
484 
485  //
486  szd = new SimpleZMPDistributor(dt);
487  std::vector<std::vector<Eigen::Vector2d> > support_polygon_vec;
488  for (size_t i = 0; i < stikp.size(); i++) {
489  support_polygon_vec.push_back(std::vector<Eigen::Vector2d>(1,Eigen::Vector2d::Zero()));
490  }
491  szd->set_vertices(support_polygon_vec);
492 
493  rel_ee_pos.reserve(stikp.size());
494  rel_ee_rot.reserve(stikp.size());
495  rel_ee_name.reserve(stikp.size());
496 
497  hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
498  if (sen == NULL) {
499  std::cerr << "[" << m_profile.instance_name << "] WARNING! This robot model has no GyroSensor named 'gyrometer'! " << std::endl;
500  }
501  return RTC::RTC_OK;
502 }
503 
504 
505 RTC::ReturnCode_t Stabilizer::onFinalize()
506 {
507  if (szd == NULL) {
508  delete szd;
509  szd = NULL;
510  }
511  return RTC::RTC_OK;
512 }
513 
514 /*
515 RTC::ReturnCode_t Stabilizer::onStartup(RTC::UniqueId ec_id)
516 {
517  return RTC::RTC_OK;
518 }
519 */
520 
521 /*
522 RTC::ReturnCode_t Stabilizer::onShutdown(RTC::UniqueId ec_id)
523 {
524  return RTC::RTC_OK;
525 }
526 */
527 
528 RTC::ReturnCode_t Stabilizer::onActivated(RTC::UniqueId ec_id)
529 {
530  std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl;
531  return RTC::RTC_OK;
532 }
533 
534 RTC::ReturnCode_t Stabilizer::onDeactivated(RTC::UniqueId ec_id)
535 {
536  Guard guard(m_mutex);
537  std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl;
538  if ( (control_mode == MODE_ST || control_mode == MODE_AIR) ) {
539  sync_2_idle ();
541  transition_count = 1; // sync in one controller loop
542  }
543  return RTC::RTC_OK;
544 }
545 
546 #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 )
547 #define DEBUGP2 (loop%10==0)
548 RTC::ReturnCode_t Stabilizer::onExecute(RTC::UniqueId ec_id)
549 {
550  loop++;
551  // std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl;
552 
553  if (m_qRefIn.isNew()) {
554  m_qRefIn.read();
555  }
556  if (m_qCurrentIn.isNew()) {
557  m_qCurrentIn.read();
558  }
559  if (m_rpyIn.isNew()) {
560  m_rpyIn.read();
561  }
562  if (m_zmpRefIn.isNew()) {
563  m_zmpRefIn.read();
564  }
565  if (m_basePosIn.isNew()){
566  m_basePosIn.read();
567  }
568  if (m_baseRpyIn.isNew()){
569  m_baseRpyIn.read();
570  }
571  if (m_contactStatesIn.isNew()){
573  for (size_t i = 0; i < m_contactStates.data.length(); i++) {
575  }
576  }
577  if (m_toeheelRatioIn.isNew()){
579  for (size_t i = 0; i < m_toeheelRatio.data.length(); i++) {
580  toeheel_ratio[i] = m_toeheelRatio.data[i];
581  }
582  }
585  }
586  for (size_t i = 0; i < m_wrenchesIn.size(); ++i) {
587  if ( m_wrenchesIn[i]->isNew() ) {
588  m_wrenchesIn[i]->read();
589  }
590  }
591  for (size_t i = 0; i < m_ref_wrenchesIn.size(); ++i) {
592  if ( m_ref_wrenchesIn[i]->isNew() ) {
593  m_ref_wrenchesIn[i]->read();
594  }
595  }
596  Guard guard(m_mutex);
597  for (size_t i = 0; i < m_limbCOPOffsetIn.size(); ++i) {
598  if ( m_limbCOPOffsetIn[i]->isNew() ) {
599  m_limbCOPOffsetIn[i]->read();
600  //stikp[i].localCOPPos = stikp[i].localp + stikp[i].localR * hrp::Vector3(m_limbCOPOffset[i].data.x, m_limbCOPOffset[i].data.y, m_limbCOPOffset[i].data.z);
601  stikp[i].localCOPPos = stikp[i].localp + stikp[i].localR * hrp::Vector3(m_limbCOPOffset[i].data.x, 0, m_limbCOPOffset[i].data.z);
602  }
603  }
604  if (m_qRefSeqIn.isNew()) {
605  m_qRefSeqIn.read();
606  is_seq_interpolating = true;
607  } else {
608  is_seq_interpolating = false;
609  }
610  if (m_walkingStatesIn.isNew()){
613  }
614  if (m_sbpCogOffsetIn.isNew()){
616  sbp_cog_offset(0) = m_sbpCogOffset.data.x;
617  sbp_cog_offset(1) = m_sbpCogOffset.data.y;
618  sbp_cog_offset(2) = m_sbpCogOffset.data.z;
619  }
620 
621  if (is_legged_robot) {
626  switch (control_mode) {
627  case MODE_IDLE:
628  break;
629  case MODE_AIR:
630  if ( transition_count == 0 && on_ground ) sync_2_st();
631  break;
632  case MODE_ST:
633  if (st_algorithm != OpenHRP::StabilizerService::TPCC) {
635  } else {
636  calcTPCC();
637  }
638  if ( transition_count == 0 && !on_ground ) {
641  } else is_air_counter = 0;
642  break;
643  case MODE_SYNC_TO_IDLE:
644  sync_2_idle();
646  break;
647  case MODE_SYNC_TO_AIR:
648  sync_2_idle();
650  break;
651  }
652  }
653  if ( m_robot->numJoints() == m_qRef.data.length() ) {
654  if (is_legged_robot) {
655  for ( int i = 0; i < m_robot->numJoints(); i++ ){
656  m_qRef.data[i] = m_robot->joint(i)->q;
657  //m_tau.data[i] = m_robot->joint(i)->u;
658  }
659  m_zmp.data.x = rel_act_zmp(0);
660  m_zmp.data.y = rel_act_zmp(1);
661  m_zmp.data.z = rel_act_zmp(2);
662  m_zmp.tm = m_qRef.tm;
663  m_zmpOut.write();
664  m_refCP.data.x = rel_ref_cp(0);
665  m_refCP.data.y = rel_ref_cp(1);
666  m_refCP.data.z = rel_ref_cp(2);
667  m_refCP.tm = m_qRef.tm;
668  m_refCPOut.write();
669  m_actCP.data.x = rel_act_cp(0);
670  m_actCP.data.y = rel_act_cp(1);
671  m_actCP.data.z = rel_act_cp(2);
672  m_actCP.tm = m_qRef.tm;
673  m_actCPOut.write();
674  {
676  m_diffCP.data.x = tmp_diff_cp(0);
677  m_diffCP.data.y = tmp_diff_cp(1);
678  m_diffCP.data.z = tmp_diff_cp(2);
679  m_diffCP.tm = m_qRef.tm;
680  m_diffCPOut.write();
681  }
687  m_actContactStates.tm = m_qRef.tm;
689  m_COPInfo.tm = m_qRef.tm;
691  //m_tauOut.write();
692  // for debug output
693  m_originRefZmp.data.x = ref_zmp(0); m_originRefZmp.data.y = ref_zmp(1); m_originRefZmp.data.z = ref_zmp(2);
694  m_originRefCog.data.x = ref_cog(0); m_originRefCog.data.y = ref_cog(1); m_originRefCog.data.z = ref_cog(2);
696  m_originNewZmp.data.x = new_refzmp(0); m_originNewZmp.data.y = new_refzmp(1); m_originNewZmp.data.z = new_refzmp(2);
697  m_originActZmp.data.x = act_zmp(0); m_originActZmp.data.y = act_zmp(1); m_originActZmp.data.z = act_zmp(2);
698  m_originActCog.data.x = act_cog(0); m_originActCog.data.y = act_cog(1); m_originActCog.data.z = act_cog(2);
700  m_originRefZmp.tm = m_qRef.tm;
702  m_originRefCog.tm = m_qRef.tm;
704  m_originRefCogVel.tm = m_qRef.tm;
706  m_originNewZmp.tm = m_qRef.tm;
708  m_originActZmp.tm = m_qRef.tm;
710  m_originActCog.tm = m_qRef.tm;
712  m_originActCogVel.tm = m_qRef.tm;
714  for (size_t i = 0; i < stikp.size(); i++) {
715  for (size_t j = 0; j < 3; j++) {
716  m_allRefWrench.data[6*i+j] = stikp[i].ref_force(j);
717  m_allRefWrench.data[6*i+j+3] = stikp[i].ref_moment(j);
718  m_allEEComp.data[6*i+j] = stikp[i].d_foot_pos(j);
719  m_allEEComp.data[6*i+j+3] = stikp[i].d_foot_rpy(j);
720  }
721  }
722  m_allRefWrench.tm = m_qRef.tm;
724  m_allEEComp.tm = m_qRef.tm;
726  m_actBaseRpy.data.r = act_base_rpy(0);
727  m_actBaseRpy.data.p = act_base_rpy(1);
728  m_actBaseRpy.data.y = act_base_rpy(2);
729  m_actBaseRpy.tm = m_qRef.tm;
733  m_currentBaseRpy.tm = m_qRef.tm;
737  m_currentBasePos.tm = m_qRef.tm;
741  m_debugData.tm = m_qRef.tm;
743  }
744  m_qRefOut.write();
745  // emergencySignal
746  if (reset_emergency_flag) {
747  m_emergencySignal.data = 0;
749  reset_emergency_flag = false;
750  } else if (is_emergency) {
751  m_emergencySignal.data = 1;
753  }
754  }
755 
756  return RTC::RTC_OK;
757 }
758 
760 {
761  current_root_p = m_robot->rootLink()->p;
762  current_root_R = m_robot->rootLink()->R;
763  for ( int i = 0; i < m_robot->numJoints(); i++ ){
764  qorg[i] = m_robot->joint(i)->q;
765  }
766 }
767 
768 void Stabilizer::calcFootOriginCoords (hrp::Vector3& foot_origin_pos, hrp::Matrix33& foot_origin_rot)
769 {
770  rats::coordinates leg_c[2], tmpc;
771  hrp::Vector3 ez = hrp::Vector3::UnitZ();
772  hrp::Vector3 ex = hrp::Vector3::UnitX();
773  for (size_t i = 0; i < stikp.size(); i++) {
774  if (stikp[i].ee_name.find("leg") == std::string::npos) continue;
775  hrp::Link* target = m_robot->sensor<hrp::ForceSensor>(stikp[i].sensor_name)->link;
776  leg_c[i].pos = target->p + target->R * foot_origin_offset[i];
777  hrp::Vector3 xv1(target->R * ex);
778  xv1(2)=0.0;
779  xv1.normalize();
780  hrp::Vector3 yv1(ez.cross(xv1));
781  leg_c[i].rot(0,0) = xv1(0); leg_c[i].rot(1,0) = xv1(1); leg_c[i].rot(2,0) = xv1(2);
782  leg_c[i].rot(0,1) = yv1(0); leg_c[i].rot(1,1) = yv1(1); leg_c[i].rot(2,1) = yv1(2);
783  leg_c[i].rot(0,2) = ez(0); leg_c[i].rot(1,2) = ez(1); leg_c[i].rot(2,2) = ez(2);
784  }
787  rats::mid_coords(tmpc, 0.5, leg_c[0], leg_c[1]);
788  foot_origin_pos = tmpc.pos;
789  foot_origin_rot = tmpc.rot;
790  } else if (ref_contact_states[contact_states_index_map["rleg"]]) {
791  foot_origin_pos = leg_c[contact_states_index_map["rleg"]].pos;
792  foot_origin_rot = leg_c[contact_states_index_map["rleg"]].rot;
793  } else {
794  foot_origin_pos = leg_c[contact_states_index_map["lleg"]].pos;
795  foot_origin_rot = leg_c[contact_states_index_map["lleg"]].rot;
796  }
797 }
798 
800 {
801  // Actual world frame =>
802  hrp::Vector3 foot_origin_pos;
803  hrp::Matrix33 foot_origin_rot;
804  if (st_algorithm != OpenHRP::StabilizerService::TPCC) {
805  // update by current joint angles
806  for ( int i = 0; i < m_robot->numJoints(); i++ ){
807  m_robot->joint(i)->q = m_qCurrent.data[i];
808  }
809  // tempolary
810  m_robot->rootLink()->p = hrp::Vector3::Zero();
811  m_robot->calcForwardKinematics();
812  hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
813  hrp::Matrix33 senR = sen->link->R * sen->localR;
814  hrp::Matrix33 act_Rs(hrp::rotFromRpy(m_rpy.data.r, m_rpy.data.p, m_rpy.data.y));
815  //hrp::Matrix33 act_Rs(hrp::rotFromRpy(m_rpy.data.r*0.5, m_rpy.data.p*0.5, m_rpy.data.y*0.5));
816  m_robot->rootLink()->R = act_Rs * (senR.transpose() * m_robot->rootLink()->R);
817  m_robot->calcForwardKinematics();
818  act_base_rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
819  calcFootOriginCoords (foot_origin_pos, foot_origin_rot);
820  } else {
821  for ( int i = 0; i < m_robot->numJoints(); i++ ) {
822  m_robot->joint(i)->q = qorg[i];
823  }
824  m_robot->rootLink()->p = current_root_p;
825  m_robot->rootLink()->R = current_root_R;
826  m_robot->calcForwardKinematics();
827  }
828  // cog
829  act_cog = m_robot->calcCM();
830  // zmp
831  on_ground = false;
832  if (st_algorithm != OpenHRP::StabilizerService::TPCC) {
833  on_ground = calcZMP(act_zmp, zmp_origin_off+foot_origin_pos(2));
834  } else {
836  }
837  // set actual contact states
838  for (size_t i = 0; i < stikp.size(); i++) {
839  std::string limb_name = stikp[i].ee_name;
840  size_t idx = contact_states_index_map[limb_name];
841  act_contact_states[idx] = isContact(idx);
842  m_actContactStates.data[idx] = act_contact_states[idx];
843  }
844  // <= Actual world frame
845 
846  // convert absolute (in st) -> root-link relative
847  rel_act_zmp = m_robot->rootLink()->R.transpose() * (act_zmp - m_robot->rootLink()->p);
848  if (st_algorithm != OpenHRP::StabilizerService::TPCC) {
849  // Actual foot_origin frame =>
850  act_zmp = foot_origin_rot.transpose() * (act_zmp - foot_origin_pos);
851  act_cog = foot_origin_rot.transpose() * (act_cog - foot_origin_pos);
852  //act_cogvel = foot_origin_rot.transpose() * act_cogvel;
854  act_cogvel = (foot_origin_rot.transpose() * prev_act_foot_origin_rot) * act_cogvel;
855  } else {
857  }
858  act_cogvel = act_cogvel_filter->passFilter(act_cogvel);
860  //act_root_rot = m_robot->rootLink()->R;
861  for (size_t i = 0; i < stikp.size(); i++) {
862  hrp::Link* target = m_robot->link(stikp[i].target_name);
863  //hrp::Vector3 act_ee_p = target->p + target->R * stikp[i].localCOPPos;
864  hrp::Vector3 _act_ee_p = target->p + target->R * stikp[i].localp;
865  act_ee_p[i] = foot_origin_rot.transpose() * (_act_ee_p - foot_origin_pos);
866  act_ee_R[i] = foot_origin_rot.transpose() * (target->R * stikp[i].localR);
867  }
868  // capture point
871  rel_act_cp = m_robot->rootLink()->R.transpose() * ((foot_origin_pos + foot_origin_rot * rel_act_cp) - m_robot->rootLink()->p);
872  // <= Actual foot_origin frame
873 
874  // Actual world frame =>
875  // new ZMP calculation
876  // Kajita's feedback law
877  // Basically Equation (26) in the paper [1].
878  hrp::Vector3 dcog=foot_origin_rot * (ref_cog - act_cog);
879  hrp::Vector3 dcogvel=foot_origin_rot * (ref_cogvel - act_cogvel);
880  hrp::Vector3 dzmp=foot_origin_rot * (ref_zmp - act_zmp);
881  new_refzmp = foot_origin_rot * new_refzmp + foot_origin_pos;
882  for (size_t i = 0; i < 2; i++) {
884  }
885  if (DEBUGP) {
886  // All state variables are foot_origin coords relative
887  std::cerr << "[" << m_profile.instance_name << "] state values" << std::endl;
888  std::cerr << "[" << m_profile.instance_name << "] "
889  << "ref_cog = " << hrp::Vector3(ref_cog*1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]"))
890  << ", act_cog = " << hrp::Vector3(act_cog*1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[mm]" << std::endl;
891  std::cerr << "[" << m_profile.instance_name << "] "
892  << "ref_cogvel = " << hrp::Vector3(ref_cogvel*1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]"))
893  << ", act_cogvel = " << hrp::Vector3(act_cogvel*1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[mm/s]" << std::endl;
894  std::cerr << "[" << m_profile.instance_name << "] "
895  << "ref_zmp = " << hrp::Vector3(ref_zmp*1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]"))
896  << ", act_zmp = " << hrp::Vector3(act_zmp*1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[mm]" << std::endl;
897  hrp::Vector3 tmpnew_refzmp;
898  tmpnew_refzmp = foot_origin_rot.transpose()*(new_refzmp-foot_origin_pos); // Actual world -> foot origin relative
899  std::cerr << "[" << m_profile.instance_name << "] "
900  << "new_zmp = " << hrp::Vector3(tmpnew_refzmp*1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]"))
901  << ", dif_zmp = " << hrp::Vector3((tmpnew_refzmp-ref_zmp)*1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[mm]" << std::endl;
902  }
903 
904  std::vector<std::string> ee_name;
905  // distribute new ZMP into foot force & moment
906  std::vector<hrp::Vector3> tmp_ref_force, tmp_ref_moment;
907  std::vector<double> limb_gains;
908  std::vector<hrp::dvector6> ee_forcemoment_distribution_weight;
909  std::vector<double> tmp_toeheel_ratio;
910  if (control_mode == MODE_ST) {
911  std::vector<hrp::Vector3> ee_pos, cop_pos;
912  std::vector<hrp::Matrix33> ee_rot;
913  std::vector<bool> is_contact_list;
914  is_contact_list.reserve(stikp.size());
915  for (size_t i = 0; i < stikp.size(); i++) {
916  STIKParam& ikp = stikp[i];
917  if (!is_feedback_control_enable[i]) continue;
918  hrp::Link* target = m_robot->link(ikp.target_name);
919  ee_pos.push_back(target->p + target->R * ikp.localp);
920  cop_pos.push_back(target->p + target->R * ikp.localCOPPos);
921  ee_rot.push_back(target->R * ikp.localR);
922  ee_name.push_back(ikp.ee_name);
923  limb_gains.push_back(ikp.swing_support_gain);
924  tmp_ref_force.push_back(hrp::Vector3(foot_origin_rot * ref_force[i]));
925  tmp_ref_moment.push_back(hrp::Vector3(foot_origin_rot * ref_moment[i]));
926  rel_ee_pos.push_back(foot_origin_rot.transpose() * (ee_pos.back() - foot_origin_pos));
927  rel_ee_rot.push_back(foot_origin_rot.transpose() * ee_rot.back());
928  rel_ee_name.push_back(ee_name.back());
929  is_contact_list.push_back(act_contact_states[i]);
930  // std::cerr << ee_forcemoment_distribution_weight[i] << std::endl;
931  ee_forcemoment_distribution_weight.push_back(hrp::dvector6::Zero(6,1));
932  for (size_t j = 0; j < 6; j++) {
933  ee_forcemoment_distribution_weight[i][j] = ikp.eefm_ee_forcemoment_distribution_weight[j];
934  }
935  tmp_toeheel_ratio.push_back(toeheel_ratio[i]);
936  }
937 
938  // All state variables are foot_origin coords relative
939  if (DEBUGP) {
940  std::cerr << "[" << m_profile.instance_name << "] ee values" << std::endl;
941  hrp::Vector3 tmpp;
942  for (size_t i = 0; i < ee_name.size(); i++) {
943  tmpp = foot_origin_rot.transpose()*(ee_pos[i]-foot_origin_pos);
944  std::cerr << "[" << m_profile.instance_name << "] "
945  << "ee_pos (" << ee_name[i] << ") = " << hrp::Vector3(tmpp*1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]"));
946  tmpp = foot_origin_rot.transpose()*(cop_pos[i]-foot_origin_pos);
947  std::cerr << ", cop_pos (" << ee_name[i] << ") = " << hrp::Vector3(tmpp*1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[mm]" << std::endl;
948  }
949  }
950 
951  // truncate ZMP
952  if (use_zmp_truncation) {
953  Eigen::Vector2d tmp_new_refzmp(new_refzmp.head(2));
956  if (!szd->is_inside_support_polygon(tmp_new_refzmp, hrp::Vector3::Zero(), true, std::string(m_profile.instance_name))) new_refzmp.head(2) = tmp_new_refzmp;
957  }
958 
959  // Distribute ZMP into each EE force/moment at each COP
960  if (st_algorithm == OpenHRP::StabilizerService::EEFM) {
961  // Modified version of distribution in Equation (4)-(6) and (10)-(13) in the paper [1].
962  szd->distributeZMPToForceMoments(tmp_ref_force, tmp_ref_moment,
963  ee_pos, cop_pos, ee_rot, ee_name, limb_gains, tmp_toeheel_ratio,
964  new_refzmp, hrp::Vector3(foot_origin_rot * ref_zmp + foot_origin_pos),
966  DEBUGP, std::string(m_profile.instance_name));
967  } else if (st_algorithm == OpenHRP::StabilizerService::EEFMQP) {
968  szd->distributeZMPToForceMomentsQP(tmp_ref_force, tmp_ref_moment,
969  ee_pos, cop_pos, ee_rot, ee_name, limb_gains, tmp_toeheel_ratio,
970  new_refzmp, hrp::Vector3(foot_origin_rot * ref_zmp + foot_origin_pos),
972  DEBUGP, std::string(m_profile.instance_name),
973  (st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP));
974  } else if (st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP) {
975  szd->distributeZMPToForceMomentsPseudoInverse(tmp_ref_force, tmp_ref_moment,
976  ee_pos, cop_pos, ee_rot, ee_name, limb_gains, tmp_toeheel_ratio,
977  new_refzmp, hrp::Vector3(foot_origin_rot * ref_zmp + foot_origin_pos),
979  DEBUGP, std::string(m_profile.instance_name),
980  (st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP), is_contact_list);
981  } else if (st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP2) {
982  szd->distributeZMPToForceMomentsPseudoInverse2(tmp_ref_force, tmp_ref_moment,
983  ee_pos, cop_pos, ee_rot, ee_name, limb_gains, tmp_toeheel_ratio,
984  new_refzmp, hrp::Vector3(foot_origin_rot * ref_zmp + foot_origin_pos),
985  foot_origin_rot * ref_total_force, foot_origin_rot * ref_total_moment,
986  ee_forcemoment_distribution_weight,
988  DEBUGP, std::string(m_profile.instance_name));
989  }
990  // for debug output
991  new_refzmp = foot_origin_rot.transpose() * (new_refzmp - foot_origin_pos);
992  }
993 
994  // foor modif
995  if (control_mode == MODE_ST) {
996  hrp::Vector3 f_diff(hrp::Vector3::Zero());
997  std::vector<bool> large_swing_f_diff(3, false);
998  // moment control
999  act_total_foot_origin_moment = hrp::Vector3::Zero();
1000  for (size_t i = 0; i < stikp.size(); i++) {
1001  STIKParam& ikp = stikp[i];
1002  std::vector<bool> large_swing_m_diff(3, false);
1003  if (!is_feedback_control_enable[i]) continue;
1004  hrp::Sensor* sensor = m_robot->sensor<hrp::ForceSensor>(ikp.sensor_name);
1005  hrp::Link* target = m_robot->link(ikp.target_name);
1006  // Convert moment at COP => moment at ee
1007  size_t idx = contact_states_index_map[ikp.ee_name];
1008  ikp.ref_moment = tmp_ref_moment[idx] + ((target->R * ikp.localCOPPos + target->p) - (target->R * ikp.localp + target->p)).cross(tmp_ref_force[idx]);
1009  ikp.ref_force = tmp_ref_force[idx];
1010  // Actual world frame =>
1011  hrp::Vector3 sensor_force = (sensor->link->R * sensor->localR) * hrp::Vector3(m_wrenches[i].data[0], m_wrenches[i].data[1], m_wrenches[i].data[2]);
1012  hrp::Vector3 sensor_moment = (sensor->link->R * sensor->localR) * hrp::Vector3(m_wrenches[i].data[3], m_wrenches[i].data[4], m_wrenches[i].data[5]);
1013  //hrp::Vector3 ee_moment = ((sensor->link->R * sensor->localPos + sensor->link->p) - (target->R * ikp.localCOPPos + target->p)).cross(sensor_force) + sensor_moment;
1014  hrp::Vector3 ee_moment = ((sensor->link->R * sensor->localPos + sensor->link->p) - (target->R * ikp.localp + target->p)).cross(sensor_force) + sensor_moment;
1015  // <= Actual world frame
1016  hrp::Matrix33 ee_R(target->R * ikp.localR);
1017  // Actual ee frame =>
1018  ikp.ref_moment = ee_R.transpose() * ikp.ref_moment;
1019  ikp.ref_force = ee_R.transpose() * ikp.ref_force;
1020  sensor_force = ee_R.transpose() * sensor_force;
1021  ee_moment = ee_R.transpose() * ee_moment;
1022  if ( i == 0 ) f_diff += -1*sensor_force;
1023  else f_diff += sensor_force;
1024  for (size_t j = 0; j < 3; ++j) {
1025  if ((!ref_contact_states[i] || !act_contact_states[i]) && fabs(ikp.ref_force(j) - sensor_force(j)) > eefm_swing_damping_force_thre[j]) large_swing_f_diff[j] = true;
1026  if ((!ref_contact_states[i] || !act_contact_states[i]) && (fabs(ikp.ref_moment(j) - ee_moment(j)) > eefm_swing_damping_moment_thre[j])) large_swing_m_diff[j] = true;
1027  }
1028  // Moment limitation
1029  ikp.ref_moment = ee_R * vlimit((ee_R.transpose() * ikp.ref_moment), ikp.eefm_ee_moment_limit);
1030  // calcDampingControl
1031  // ee_d_foot_rpy and ee_d_foot_pos is (actual) end effector coords relative value because these use end effector coords relative force & moment
1032  { // Rot
1033  // Basically Equation (16) and (17) in the paper [1]
1034  hrp::Vector3 tmp_damping_gain;
1035  for (size_t j = 0; j < 3; ++j) {
1036  if (!eefm_use_swing_damping || !large_swing_m_diff[j]) tmp_damping_gain(j) = (1-transition_smooth_gain) * ikp.eefm_rot_damping_gain(j) * 10 + transition_smooth_gain * ikp.eefm_rot_damping_gain(j);
1038  }
1039  ikp.ee_d_foot_rpy = calcDampingControl(ikp.ref_moment, ee_moment, ikp.ee_d_foot_rpy, tmp_damping_gain, ikp.eefm_rot_time_const);
1041  }
1042  if (!eefm_use_force_difference_control) { // Pos
1044  ikp.ee_d_foot_pos = calcDampingControl(ikp.ref_force, sensor_force, ikp.ee_d_foot_pos, tmp_damping_gain, ikp.eefm_pos_time_const_support);
1046  }
1047  // Convert force & moment as foot origin coords relative
1048  ikp.ref_moment = foot_origin_rot.transpose() * ee_R * ikp.ref_moment;
1049  ikp.ref_force = foot_origin_rot.transpose() * ee_R * ikp.ref_force;
1050  sensor_force = foot_origin_rot.transpose() * ee_R * sensor_force;
1051  ee_moment = foot_origin_rot.transpose() * ee_R * ee_moment;
1052  ikp.d_foot_rpy = foot_origin_rot.transpose() * ee_R * ikp.ee_d_foot_rpy;
1053  ikp.d_foot_pos = foot_origin_rot.transpose() * ee_R * ikp.ee_d_foot_pos;
1054  // tilt Check : only flat plane is supported
1055  {
1056  hrp::Vector3 plane_x = target_ee_R[i].col(0);
1057  hrp::Vector3 plane_y = target_ee_R[i].col(1);
1058  hrp::Matrix33 act_ee_R_world = target->R * stikp[i].localR;
1059  hrp::Vector3 normal_vector = act_ee_R_world.col(2);
1060  /* projected_normal = c1 * plane_x + c2 * plane_y : c1 = plane_x.dot(normal_vector), c2 = plane_y.dot(normal_vector) because (normal-vector - projected_normal) is orthogonal to plane */
1061  projected_normal.at(i) = plane_x.dot(normal_vector) * plane_x + plane_y.dot(normal_vector) * plane_y;
1062  act_force.at(i) = sensor_force;
1063  }
1064  //act_total_foot_origin_moment += (target->R * ikp.localCOPPos + target->p).cross(sensor_force) + ee_moment;
1065  act_total_foot_origin_moment += (target->R * ikp.localp + target->p - foot_origin_pos).cross(sensor_force) + ee_moment;
1066  }
1067  act_total_foot_origin_moment = foot_origin_rot.transpose() * act_total_foot_origin_moment;
1068 
1070  // fxyz control
1071  // foot force difference control version
1072  // Basically Equation (18) in the paper [1]
1073  hrp::Vector3 ref_f_diff = (stikp[1].ref_force-stikp[0].ref_force);
1074  if (ref_contact_states != prev_ref_contact_states) pos_ctrl = (foot_origin_rot.transpose() * prev_act_foot_origin_rot) * pos_ctrl;
1075  if (eefm_use_swing_damping) {
1076  hrp::Vector3 tmp_damping_gain;
1077  for (size_t i = 0; i < 3; ++i) {
1078  if (!large_swing_f_diff[i]) tmp_damping_gain(i) = (1-transition_smooth_gain) * stikp[0].eefm_pos_damping_gain(i) * 10 + transition_smooth_gain * stikp[0].eefm_pos_damping_gain(i);
1080  }
1081  pos_ctrl = calcDampingControl (ref_f_diff, f_diff, pos_ctrl,
1082  tmp_damping_gain, stikp[0].eefm_pos_time_const_support);
1083  } else {
1084  if ( (ref_contact_states[contact_states_index_map["rleg"]] && ref_contact_states[contact_states_index_map["lleg"]]) // Reference : double support phase
1085  || (act_contact_states[0] && act_contact_states[1]) ) { // Actual : double support phase
1086  // Temporarily use first pos damping gain (stikp[0])
1087  hrp::Vector3 tmp_damping_gain = (1-transition_smooth_gain) * stikp[0].eefm_pos_damping_gain * 10 + transition_smooth_gain * stikp[0].eefm_pos_damping_gain;
1088  pos_ctrl = calcDampingControl (ref_f_diff, f_diff, pos_ctrl,
1089  tmp_damping_gain, stikp[0].eefm_pos_time_const_support);
1090  } else {
1091  double remain_swing_time;
1092  if ( !ref_contact_states[contact_states_index_map["rleg"]] ) { // rleg swing
1093  remain_swing_time = m_controlSwingSupportTime.data[contact_states_index_map["rleg"]];
1094  } else { // lleg swing
1095  remain_swing_time = m_controlSwingSupportTime.data[contact_states_index_map["lleg"]];
1096  }
1097  // std::cerr << "st " << remain_swing_time << " rleg " << contact_states[contact_states_index_map["rleg"]] << " lleg " << contact_states[contact_states_index_map["lleg"]] << std::endl;
1098  double tmp_ratio = std::max(0.0, std::min(1.0, 1.0 - (remain_swing_time-eefm_pos_margin_time)/eefm_pos_transition_time)); // 0=>1
1099  // Temporarily use first pos damping gain (stikp[0])
1100  hrp::Vector3 tmp_damping_gain = (1-transition_smooth_gain) * stikp[0].eefm_pos_damping_gain * 10 + transition_smooth_gain * stikp[0].eefm_pos_damping_gain;
1101  hrp::Vector3 tmp_time_const = (1-tmp_ratio)*eefm_pos_time_const_swing*hrp::Vector3::Ones()+tmp_ratio*stikp[0].eefm_pos_time_const_support;
1102  pos_ctrl = calcDampingControl (tmp_ratio * ref_f_diff, tmp_ratio * f_diff, pos_ctrl, tmp_damping_gain, tmp_time_const);
1103  }
1104  }
1105  // zctrl = vlimit(zctrl, -0.02, 0.02);
1106  // Temporarily use first pos compensation limit (stikp[0])
1107  pos_ctrl = vlimit(pos_ctrl, -1 * stikp[0].eefm_pos_compensation_limit * 2, stikp[0].eefm_pos_compensation_limit * 2);
1108  // Divide pos_ctrl into rfoot and lfoot
1109  stikp[0].d_foot_pos = -0.5 * pos_ctrl;
1110  stikp[1].d_foot_pos = 0.5 * pos_ctrl;
1111  }
1112  if (DEBUGP) {
1113  std::cerr << "[" << m_profile.instance_name << "] Control values" << std::endl;
1115  std::cerr << "[" << m_profile.instance_name << "] "
1116  << "pos_ctrl = [" << pos_ctrl(0)*1e3 << " " << pos_ctrl(1)*1e3 << " "<< pos_ctrl(2)*1e3 << "] [mm]" << std::endl;
1117  }
1118  for (size_t i = 0; i < ee_name.size(); i++) {
1119  std::cerr << "[" << m_profile.instance_name << "] "
1120  << "d_foot_pos (" << ee_name[i] << ") = [" << stikp[i].d_foot_pos(0)*1e3 << " " << stikp[i].d_foot_pos(1)*1e3 << " " << stikp[i].d_foot_pos(2)*1e3 << "] [mm], "
1121  << "d_foot_rpy (" << ee_name[i] << ") = [" << stikp[i].d_foot_rpy(0)*180.0/M_PI << " " << stikp[i].d_foot_rpy(1)*180.0/M_PI << " " << stikp[i].d_foot_rpy(2)*180.0/M_PI << "] [deg]" << std::endl;
1122  }
1123  }
1124  // foot force independent damping control
1125  // for (size_t i = 0; i < 2; i++) {
1126  // f_zctrl[i] = calcDampingControl (ref_force[i](2),
1127  // fz[i], f_zctrl[i], eefm_pos_damping_gain, eefm_pos_time_const);
1128  // f_zctrl[i] = vlimit(f_zctrl[i], -0.05, 0.05);
1129  // }
1131  }
1132  } // st_algorithm == OpenHRP::StabilizerService::EEFM
1133 
1134  for ( int i = 0; i < m_robot->numJoints(); i++ ){
1135  m_robot->joint(i)->q = qrefv[i];
1136  }
1137  m_robot->rootLink()->p = target_root_p;
1138  m_robot->rootLink()->R = target_root_R;
1139  if ( !(control_mode == MODE_IDLE || control_mode == MODE_AIR) ) {
1140  for (size_t i = 0; i < jpe_v.size(); i++) {
1141  if (is_ik_enable[i]) {
1142  for ( int j = 0; j < jpe_v[i]->numJoints(); j++ ){
1143  int idx = jpe_v[i]->joint(j)->jointId;
1144  m_robot->joint(idx)->q = qorg[idx];
1145  }
1146  }
1147  }
1148  m_robot->rootLink()->p(0) = current_root_p(0);
1149  m_robot->rootLink()->p(1) = current_root_p(1);
1150  m_robot->rootLink()->R = current_root_R;
1151  m_robot->calcForwardKinematics();
1152  }
1153  copy (ref_contact_states.begin(), ref_contact_states.end(), prev_ref_contact_states.begin());
1154  if (control_mode != MODE_ST) d_pos_z_root = 0.0;
1155  prev_act_foot_origin_rot = foot_origin_rot;
1156 }
1157 
1159 {
1160  // Reference world frame =>
1161  // update internal robot model
1162  if ( transition_count == 0 ) {
1163  transition_smooth_gain = 1.0;
1164  } else {
1165  double max_transition_count = calcMaxTransitionCount();
1166  transition_smooth_gain = 1/(1+exp(-9.19*(((max_transition_count - std::fabs(transition_count)) / max_transition_count) - 0.5)));
1167  }
1168  if (transition_count > 0) {
1169  for ( int i = 0; i < m_robot->numJoints(); i++ ){
1171  }
1172  } else {
1173  for ( int i = 0; i < m_robot->numJoints(); i++ ){
1174  m_robot->joint(i)->q = m_qRef.data[i];
1175  }
1176  }
1177  if ( transition_count < 0 ) {
1178  transition_count++;
1179  } else if ( transition_count > 0 ) {
1180  if ( transition_count == 1 ) {
1181  std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm << "] Move to MODE_IDLE" << std::endl;
1182  reset_emergency_flag = true;
1183  }
1184  transition_count--;
1185  }
1186  for ( int i = 0; i < m_robot->numJoints(); i++ ){
1187  qrefv[i] = m_robot->joint(i)->q;
1188  }
1189  m_robot->rootLink()->p = hrp::Vector3(m_basePos.data.x, m_basePos.data.y, m_basePos.data.z);
1190  target_root_p = m_robot->rootLink()->p;
1191  target_root_R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
1192  m_robot->rootLink()->R = target_root_R;
1193  m_robot->calcForwardKinematics();
1194  ref_zmp = m_robot->rootLink()->R * hrp::Vector3(m_zmpRef.data.x, m_zmpRef.data.y, m_zmpRef.data.z) + m_robot->rootLink()->p; // base frame -> world frame
1195  hrp::Vector3 foot_origin_pos;
1196  hrp::Matrix33 foot_origin_rot;
1197  calcFootOriginCoords (foot_origin_pos, foot_origin_rot);
1198  if (st_algorithm != OpenHRP::StabilizerService::TPCC) {
1199  // apply inverse system
1202  ref_zmp = tmp_ref_zmp;
1203  }
1204  ref_cog = m_robot->calcCM();
1205  ref_total_force = hrp::Vector3::Zero();
1206  ref_total_moment = hrp::Vector3::Zero(); // Total moment around reference ZMP tmp
1207  ref_total_foot_origin_moment = hrp::Vector3::Zero();
1208  for (size_t i = 0; i < stikp.size(); i++) {
1209  hrp::Link* target = m_robot->link(stikp[i].target_name);
1210  //target_ee_p[i] = target->p + target->R * stikp[i].localCOPPos;
1211  target_ee_p[i] = target->p + target->R * stikp[i].localp;
1212  target_ee_R[i] = target->R * stikp[i].localR;
1214  ref_moment[i] = hrp::Vector3(m_ref_wrenches[i].data[3], m_ref_wrenches[i].data[4], m_ref_wrenches[i].data[5]);
1216 #ifdef FORCE_MOMENT_DIFF_CONTROL
1217  // Force/moment diff control
1219 #else
1220  // Force/moment control
1222  + hrp::Vector3(m_ref_wrenches[i].data[3], m_ref_wrenches[i].data[4], m_ref_wrenches[i].data[5]);
1223 #endif
1224  if (is_feedback_control_enable[i]) {
1225  ref_total_foot_origin_moment += (target_ee_p[i]-foot_origin_pos).cross(ref_force[i]) + ref_moment[i];
1226  }
1227  }
1228  // <= Reference world frame
1229 
1230  // Reset prev_ref_cog for transition (MODE_IDLE=>MODE_ST) because the coordinates for ref_cog differs among st algorithms.
1231  if (transition_count == (-1 * calcMaxTransitionCount() + 1)) { // max transition count. In MODE_IDLE => MODE_ST, transition_count is < 0 and upcounter. "+ 1" is upcount at the beginning of this function.
1233  std::cerr << "[" << m_profile.instance_name << "] Reset prev_ref_cog for transition (MODE_IDLE=>MODE_ST)." << std::endl;
1234  }
1235 
1236  if (st_algorithm != OpenHRP::StabilizerService::TPCC) {
1237  // Reference foot_origin frame =>
1238  // initialize for new_refzmp
1239  new_refzmp = ref_zmp;
1240  rel_cog = m_robot->rootLink()->R.transpose() * (ref_cog-m_robot->rootLink()->p);
1241  // convert world (current-tmp) => local (foot_origin)
1242  zmp_origin_off = ref_zmp(2) - foot_origin_pos(2);
1243  ref_zmp = foot_origin_rot.transpose() * (ref_zmp - foot_origin_pos);
1244  ref_cog = foot_origin_rot.transpose() * (ref_cog - foot_origin_pos);
1245  new_refzmp = foot_origin_rot.transpose() * (new_refzmp - foot_origin_pos);
1247  ref_cogvel = (foot_origin_rot.transpose() * prev_ref_foot_origin_rot) * ref_cogvel;
1248  } else {
1250  }
1251  prev_ref_foot_origin_rot = ref_foot_origin_rot = foot_origin_rot;
1252  for (size_t i = 0; i < stikp.size(); i++) {
1253  stikp[i].target_ee_diff_p = foot_origin_rot.transpose() * (target_ee_p[i] - foot_origin_pos);
1254  stikp[i].target_ee_diff_r = foot_origin_rot.transpose() * target_ee_R[i];
1255  ref_force[i] = foot_origin_rot.transpose() * ref_force[i];
1256  ref_moment[i] = foot_origin_rot.transpose() * ref_moment[i];
1257  }
1258  ref_total_foot_origin_moment = foot_origin_rot.transpose() * ref_total_foot_origin_moment;
1259  ref_total_force = foot_origin_rot.transpose() * ref_total_force;
1260  ref_total_moment = foot_origin_rot.transpose() * ref_total_moment;
1261  target_foot_origin_rot = foot_origin_rot;
1262  // capture point
1265  rel_ref_cp = m_robot->rootLink()->R.transpose() * ((foot_origin_pos + foot_origin_rot * rel_ref_cp) - m_robot->rootLink()->p);
1266  sbp_cog_offset = foot_origin_rot.transpose() * sbp_cog_offset;
1267  // <= Reference foot_origin frame
1268  } else {
1270  } // st_algorithm == OpenHRP::StabilizerService::EEFM
1272  // Calc swing support limb gain param
1274 }
1275 
1276 bool Stabilizer::calcZMP(hrp::Vector3& ret_zmp, const double zmp_z)
1277 {
1278  double tmpzmpx = 0;
1279  double tmpzmpy = 0;
1280  double tmpfz = 0, tmpfz2 = 0.0;
1281  for (size_t i = 0; i < stikp.size(); i++) {
1282  if (!is_zmp_calc_enable[i]) continue;
1283  hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(stikp[i].sensor_name);
1284  hrp::Vector3 fsp = sensor->link->p + sensor->link->R * sensor->localPos;
1285  hrp::Matrix33 tmpR;
1286  rats::rotm3times(tmpR, sensor->link->R, sensor->localR);
1287  hrp::Vector3 nf = tmpR * hrp::Vector3(m_wrenches[i].data[0], m_wrenches[i].data[1], m_wrenches[i].data[2]);
1288  hrp::Vector3 nm = tmpR * hrp::Vector3(m_wrenches[i].data[3], m_wrenches[i].data[4], m_wrenches[i].data[5]);
1289  tmpzmpx += nf(2) * fsp(0) - (fsp(2) - zmp_z) * nf(0) - nm(1);
1290  tmpzmpy += nf(2) * fsp(1) - (fsp(2) - zmp_z) * nf(1) + nm(0);
1291  tmpfz += nf(2);
1292  // calc ee-local COP
1293  hrp::Link* target = m_robot->link(stikp[i].target_name);
1294  hrp::Matrix33 eeR = target->R * stikp[i].localR;
1295  hrp::Vector3 ee_fsp = eeR.transpose() * (fsp - (target->p + target->R * stikp[i].localp)); // ee-local force sensor pos
1296  nf = eeR.transpose() * nf;
1297  nm = eeR.transpose() * nm;
1298  // ee-local total moment and total force at ee position
1299  double tmpcopmy = nf(2) * ee_fsp(0) - nf(0) * ee_fsp(2) - nm(1);
1300  double tmpcopmx = nf(2) * ee_fsp(1) - nf(1) * ee_fsp(2) + nm(0);
1301  double tmpcopfz = nf(2);
1302  m_COPInfo.data[i*3] = tmpcopmx;
1303  m_COPInfo.data[i*3+1] = tmpcopmy;
1304  m_COPInfo.data[i*3+2] = tmpcopfz;
1305  prev_act_force_z[i] = 0.85 * prev_act_force_z[i] + 0.15 * nf(2); // filter, cut off 5[Hz]
1306  tmpfz2 += prev_act_force_z[i];
1307  }
1308  if (tmpfz2 < contact_decision_threshold) {
1309  ret_zmp = act_zmp;
1310  return false; // in the air
1311  } else {
1312  ret_zmp = hrp::Vector3(tmpzmpx / tmpfz, tmpzmpy / tmpfz, zmp_z);
1313  return true; // on ground
1314  }
1315 };
1316 
1318 {
1319  // COP Check
1320  bool is_cop_outside = false;
1321  if (DEBUGP) {
1322  std::cerr << "[" << m_profile.instance_name << "] Check Emergency State (seq = " << (is_seq_interpolating?"interpolating":"empty") << ")" << std::endl;
1323  }
1324  if (on_ground && transition_count == 0 && control_mode == MODE_ST) {
1325  if (DEBUGP) {
1326  std::cerr << "[" << m_profile.instance_name << "] COP check" << std::endl;
1327  }
1328  for (size_t i = 0; i < stikp.size(); i++) {
1329  if (stikp[i].ee_name.find("leg") == std::string::npos) continue;
1330  // check COP inside
1331  if (m_COPInfo.data[i*3+2] > 20.0 ) {
1332  hrp::Vector3 tmpcop(m_COPInfo.data[i*3+1]/m_COPInfo.data[i*3+2], m_COPInfo.data[i*3]/m_COPInfo.data[i*3+2], 0);
1333  is_cop_outside = is_cop_outside ||
1334  (!szd->is_inside_foot(tmpcop, stikp[i].ee_name=="lleg", cop_check_margin) ||
1337  if (DEBUGP) {
1338  std::cerr << "[" << m_profile.instance_name << "] [" << stikp[i].ee_name << "] "
1339  << "outside(" << !szd->is_inside_foot(tmpcop, stikp[i].ee_name=="lleg", cop_check_margin) << ") "
1340  << "front(" << szd->is_front_of_foot(tmpcop, cop_check_margin) << ") "
1341  << "rear(" << szd->is_rear_of_foot(tmpcop, cop_check_margin) << ")" << std::endl;
1342  }
1343  } else {
1344  is_cop_outside = true;
1345  }
1346  }
1347  } else {
1348  is_cop_outside = false;
1349  }
1350  // CP Check
1351  bool is_cp_outside = false;
1352  if (on_ground && transition_count == 0 && control_mode == MODE_ST) {
1353  Eigen::Vector2d tmp_cp = act_cp.head(2);
1356  if (!is_walking || is_estop_while_walking) is_cp_outside = !szd->is_inside_support_polygon(tmp_cp, - sbp_cog_offset);
1357  if (DEBUGP) {
1358  std::cerr << "[" << m_profile.instance_name << "] CP value " << "[" << act_cp(0) << "," << act_cp(1) << "] [m], "
1359  << "sbp cog offset [" << sbp_cog_offset(0) << " " << sbp_cog_offset(1) << "], outside ? "
1360  << (is_cp_outside?"Outside":"Inside")
1361  << std::endl;
1362  }
1363  if (is_cp_outside) {
1364  if (initial_cp_too_large_error || loop % static_cast <int>(0.2/dt) == 0 ) { // once per 0.2[s]
1365  std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
1366  << "] CP too large error " << "[" << act_cp(0) << "," << act_cp(1) << "] [m]" << std::endl;
1367  }
1369  } else {
1371  }
1372  }
1373  // tilt Check
1374  hrp::Vector3 fall_direction = hrp::Vector3::Zero();
1375  bool is_falling = false, will_fall = false;
1376  {
1377  double total_force = 0.0;
1378  for (size_t i = 0; i < stikp.size(); i++) {
1379  if (is_zmp_calc_enable[i]) {
1380  if (is_walking) {
1381  if (projected_normal.at(i).norm() > sin(tilt_margin[0])) {
1382  will_fall = true;
1383  if (m_will_fall_counter[i] % static_cast <int>(1.0/dt) == 0 ) { // once per 1.0[s]
1384  std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
1385  << "] " << stikp[i].ee_name << " cannot support total weight, "
1386  << "swgsuptime : " << m_controlSwingSupportTime.data[i] << ", state : " << ref_contact_states[i]
1387  << ", otherwise robot will fall down toward " << "(" << projected_normal.at(i)(0) << "," << projected_normal.at(i)(1) << ") direction" << std::endl;
1388  }
1389  m_will_fall_counter[i]++;
1390  } else {
1391  m_will_fall_counter[i] = 0;
1392  }
1393  }
1394  fall_direction += projected_normal.at(i) * act_force.at(i).norm();
1395  total_force += act_force.at(i).norm();
1396  }
1397  }
1398  if (on_ground && transition_count == 0 && control_mode == MODE_ST) {
1399  fall_direction = fall_direction / total_force;
1400  } else {
1401  fall_direction = hrp::Vector3::Zero();
1402  }
1403  if (fall_direction.norm() > sin(tilt_margin[1])) {
1404  is_falling = true;
1405  if (m_is_falling_counter % static_cast <int>(0.2/dt) == 0) { // once per 0.2[s]
1406  std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
1407  << "] robot is falling down toward " << "(" << fall_direction(0) << "," << fall_direction(1) << ") direction" << std::endl;
1408  }
1410  } else {
1412  }
1413  }
1414  // Total check for emergency signal
1415  switch (emergency_check_mode) {
1416  case OpenHRP::StabilizerService::NO_CHECK:
1417  is_emergency = false;
1418  break;
1419  case OpenHRP::StabilizerService::COP:
1420  is_emergency = is_cop_outside && is_seq_interpolating;
1421  break;
1422  case OpenHRP::StabilizerService::CP:
1423  is_emergency = is_cp_outside;
1424  break;
1425  case OpenHRP::StabilizerService::TILT:
1426  is_emergency = will_fall || is_falling;
1427  break;
1428  default:
1429  break;
1430  }
1431  if (DEBUGP) {
1432  std::cerr << "[" << m_profile.instance_name << "] EmergencyCheck ("
1433  << (emergency_check_mode == OpenHRP::StabilizerService::NO_CHECK?"NO_CHECK": (emergency_check_mode == OpenHRP::StabilizerService::COP?"COP":"CP") )
1434  << ") " << (is_emergency?"emergency":"non-emergency") << std::endl;
1435  }
1436  rel_ee_pos.clear();
1437  rel_ee_rot.clear();
1438  rel_ee_name.clear();
1439 };
1440 
1442 {
1443  // Body rpy control
1444  // Basically Equation (1) and (2) in the paper [1]
1445  hrp::Vector3 ref_root_rpy = hrp::rpyFromRot(target_root_R);
1446  bool is_root_rot_limit = false;
1447  for (size_t i = 0; i < 2; i++) {
1449  d_rpy[i] = vlimit(d_rpy[i], -1 * root_rot_compensation_limit[i], root_rot_compensation_limit[i]);
1450  is_root_rot_limit = is_root_rot_limit || (std::fabs(std::fabs(d_rpy[i]) - root_rot_compensation_limit[i] ) < 1e-5); // near the limit
1451  }
1453  m_robot->rootLink()->R = current_root_R;
1455  m_robot->calcForwardKinematics();
1456  current_base_rpy = hrp::rpyFromRot(m_robot->rootLink()->R);
1457  current_base_pos = m_robot->rootLink()->p;
1458  if ( DEBUGP || (is_root_rot_limit && loop%200==0) ) {
1459  std::cerr << "[" << m_profile.instance_name << "] Root rot control" << std::endl;
1460  if (is_root_rot_limit) std::cerr << "[" << m_profile.instance_name << "] Root rot limit reached!!" << std::endl;
1461  std::cerr << "[" << m_profile.instance_name << "] ref = [" << rad2deg(ref_root_rpy(0)) << " " << rad2deg(ref_root_rpy(1)) << "], "
1462  << "act = [" << rad2deg(act_base_rpy(0)) << " " << rad2deg(act_base_rpy(1)) << "], "
1463  << "cur = [" << rad2deg(current_base_rpy(0)) << " " << rad2deg(current_base_rpy(1)) << "], "
1464  << "limit = [" << rad2deg(root_rot_compensation_limit[0]) << " " << rad2deg(root_rot_compensation_limit[1]) << "][deg]" << std::endl;
1465  }
1466 };
1467 
1469 {
1470  for (size_t i = 0; i < stikp.size(); i++) {
1471  STIKParam& ikp = stikp[i];
1472  if (ref_contact_states[i]) { // Support
1473  // Limit too large support time increment. Max time is 3600.0[s] = 1[h], this assumes that robot's one step time is smaller than 1[h].
1474  ikp.support_time = std::min(3600.0, ikp.support_time+dt);
1475  // In some PC, does not work because the first line is optimized out.
1476  // ikp.support_time += dt;
1477  // ikp.support_time = std::min(3600.0, ikp.support_time);
1480  } else {
1482  }
1484  } else { // Swing
1485  ikp.swing_support_gain = 0.0;
1486  ikp.support_time = 0.0;
1487  }
1488  }
1489  if (DEBUGP) {
1490  std::cerr << "[" << m_profile.instance_name << "] SwingSupportLimbGain = [";
1491  for (size_t i = 0; i < stikp.size(); i++) std::cerr << stikp[i].swing_support_gain << " ";
1492  std::cerr << "], ref_contact_states = [";
1493  for (size_t i = 0; i < stikp.size(); i++) std::cerr << ref_contact_states[i] << " ";
1494  std::cerr << "], sstime = [";
1495  for (size_t i = 0; i < stikp.size(); i++) std::cerr << m_controlSwingSupportTime.data[i] << " ";
1496  std::cerr << "], toeheel_ratio = [";
1497  for (size_t i = 0; i < stikp.size(); i++) std::cerr << toeheel_ratio[i] << " ";
1498  std::cerr << "], support_time = [";
1499  for (size_t i = 0; i < stikp.size(); i++) std::cerr << stikp[i].support_time << " ";
1500  std::cerr << "]" << std::endl;
1501  }
1502 }
1503 
1505  // stabilizer loop
1506  // Choi's feedback law
1507  hrp::Vector3 cog = m_robot->calcCM();
1508  hrp::Vector3 newcog = hrp::Vector3::Zero();
1509  hrp::Vector3 dcog(ref_cog - act_cog);
1510  hrp::Vector3 dzmp(ref_zmp - act_zmp);
1511  for (size_t i = 0; i < 2; i++) {
1512  double uu = ref_cogvel(i) - k_tpcc_p[i] * transition_smooth_gain * dzmp(i)
1513  + k_tpcc_x[i] * transition_smooth_gain * dcog(i);
1514  newcog(i) = uu * dt + cog(i);
1515  }
1516 
1518 
1519  // target at ee => target at link-origin
1520  hrp::Vector3 target_link_p[stikp.size()];
1521  hrp::Matrix33 target_link_R[stikp.size()];
1522  for (size_t i = 0; i < stikp.size(); i++) {
1523  rats::rotm3times(target_link_R[i], target_ee_R[i], stikp[i].localR.transpose());
1524  target_link_p[i] = target_ee_p[i] - target_ee_R[i] * stikp[i].localCOPPos;
1525  }
1526  // solveIK
1527  // IK target is link origin pos and rot, not ee pos and rot.
1528  //for (size_t jj = 0; jj < 5; jj++) {
1529  size_t max_ik_loop_count = 0;
1530  for (size_t i = 0; i < stikp.size(); i++) {
1531  if (max_ik_loop_count < stikp[i].ik_loop_count) max_ik_loop_count = stikp[i].ik_loop_count;
1532  }
1533  for (size_t jj = 0; jj < max_ik_loop_count; jj++) {
1534  hrp::Vector3 tmpcm = m_robot->calcCM();
1535  for (size_t i = 0; i < 2; i++) {
1536  m_robot->rootLink()->p(i) = m_robot->rootLink()->p(i) + 0.9 * (newcog(i) - tmpcm(i));
1537  }
1538  m_robot->calcForwardKinematics();
1539  for (size_t i = 0; i < stikp.size(); i++) {
1540  if (is_ik_enable[i]) {
1541  jpe_v[i]->calcInverseKinematics2Loop(target_link_p[i], target_link_R[i], 1.0, stikp[i].avoid_gain, stikp[i].reference_gain, &qrefv, transition_smooth_gain);
1542  }
1543  }
1544  }
1545 }
1546 
1547 
1549 
1550  // stabilizer loop
1551  // return to referencea
1552  m_robot->rootLink()->R = target_root_R;
1553  m_robot->rootLink()->p = target_root_p;
1554  for ( int i = 0; i < m_robot->numJoints(); i++ ) {
1555  m_robot->joint(i)->q = qrefv[i];
1556  }
1557  for (size_t i = 0; i < jpe_v.size(); i++) {
1558  if (is_ik_enable[i]) {
1559  for ( int j = 0; j < jpe_v[i]->numJoints(); j++ ){
1560  int idx = jpe_v[i]->joint(j)->jointId;
1561  m_robot->joint(idx)->q = qorg[idx];
1562  }
1563  }
1564  }
1565  // Fix for toe joint
1566  for (size_t i = 0; i < jpe_v.size(); i++) {
1567  if (is_ik_enable[i]) {
1568  if (jpe_v[i]->numJoints() == 7) {
1569  int idx = jpe_v[i]->joint(jpe_v[i]->numJoints() -1)->jointId;
1570  m_robot->joint(idx)->q = qrefv[idx];
1571  }
1572  }
1573  }
1574 
1575  // State calculation for swing ee compensation
1576  // joint angle : current control output
1577  // root pos : target root p
1578  // root rot : actual root rot
1579  {
1580  // Calc status
1581  m_robot->rootLink()->R = target_root_R;
1582  m_robot->rootLink()->p = target_root_p;
1583  m_robot->calcForwardKinematics();
1584  hrp::Sensor* sen = m_robot->sensor<hrp::RateGyroSensor>("gyrometer");
1585  hrp::Matrix33 senR = sen->link->R * sen->localR;
1586  hrp::Matrix33 act_Rs(hrp::rotFromRpy(m_rpy.data.r, m_rpy.data.p, m_rpy.data.y));
1587  m_robot->rootLink()->R = act_Rs * (senR.transpose() * m_robot->rootLink()->R);
1588  m_robot->calcForwardKinematics();
1589  hrp::Vector3 foot_origin_pos;
1590  hrp::Matrix33 foot_origin_rot;
1591  calcFootOriginCoords (foot_origin_pos, foot_origin_rot);
1592  // Calculate foot_origin_coords-relative ee pos and rot
1593  // Subtract them from target_ee_diff_xx
1594  for (size_t i = 0; i < stikp.size(); i++) {
1595  hrp::Link* target = m_robot->link(stikp[i].target_name);
1596  stikp[i].target_ee_diff_p -= foot_origin_rot.transpose() * (target->p + target->R * stikp[i].localp - foot_origin_pos);
1597  stikp[i].target_ee_diff_r = (foot_origin_rot.transpose() * target->R * stikp[i].localR).transpose() * stikp[i].target_ee_diff_r;
1598  }
1599  }
1600 
1601  // State calculation for control : calculate "current" state
1602  // joint angle : current control output
1603  // root pos : target + keep COG against rpy control
1604  // root rot : target + rpy control
1606 
1607  // Convert d_foot_pos in foot origin frame => "current" world frame
1608  hrp::Vector3 foot_origin_pos;
1609  hrp::Matrix33 foot_origin_rot;
1610  calcFootOriginCoords (foot_origin_pos, foot_origin_rot);
1611  std::vector<hrp::Vector3> current_d_foot_pos;
1612  for (size_t i = 0; i < stikp.size(); i++)
1613  current_d_foot_pos.push_back(foot_origin_rot * stikp[i].d_foot_pos);
1614 
1615  // Swing ee compensation.
1617 
1618  // solveIK
1619  // IK target is link origin pos and rot, not ee pos and rot.
1620  std::vector<hrp::Vector3> tmpp(stikp.size());
1621  std::vector<hrp::Matrix33> tmpR(stikp.size());
1622  double tmp_d_pos_z_root = 0.0;
1623  for (size_t i = 0; i < stikp.size(); i++) {
1624  if (is_ik_enable[i]) {
1625  // Add damping_control compensation to target value
1626  if (is_feedback_control_enable[i]) {
1627  rats::rotm3times(tmpR[i], target_ee_R[i], hrp::rotFromRpy(-1*stikp[i].ee_d_foot_rpy));
1628  // foot force difference control version
1629  // total_target_foot_p[i](2) = target_foot_p[i](2) + (i==0?0.5:-0.5)*zctrl;
1630  // foot force independent damping control
1631  tmpp[i] = target_ee_p[i] - current_d_foot_pos[i];
1632  } else {
1633  tmpp[i] = target_ee_p[i];
1634  tmpR[i] = target_ee_R[i];
1635  }
1636  // Add swing ee compensation
1637  rats::rotm3times(tmpR[i], tmpR[i], hrp::rotFromRpy(stikp[i].d_rpy_swing));
1638  tmpp[i] = tmpp[i] + foot_origin_rot * stikp[i].d_pos_swing;
1639  }
1640  }
1641 
1642  limbStretchAvoidanceControl(tmpp ,tmpR);
1643 
1644  // IK
1645  for (size_t i = 0; i < stikp.size(); i++) {
1646  if (is_ik_enable[i]) {
1647  for (size_t jj = 0; jj < stikp[i].ik_loop_count; jj++) {
1648  jpe_v[i]->calcInverseKinematics2Loop(tmpp[i], tmpR[i], 1.0, 0.001, 0.01, &qrefv, transition_smooth_gain,
1649  //stikp[i].localCOPPos;
1650  stikp[i].localp,
1651  stikp[i].localR);
1652  }
1653  }
1654  }
1655 }
1656 
1657 // Swing ee compensation.
1658 // Calculate compensation values to minimize the difference between "current" foot-origin-coords-relative pos and rot and "target" foot-origin-coords-relative pos and rot for swing ee.
1659 // Input : target_ee_diff_p, target_ee_diff_r
1660 // Output : d_pos_swing, d_rpy_swing
1662 {
1663  for (size_t i = 0; i < stikp.size(); i++) {
1664  // Calc compensation values
1665  double limit_pos = 30 * 1e-3; // 30[mm] limit
1666  double limit_rot = deg2rad(10); // 10[deg] limit
1668  // If actual contact or target contact is ON, do not use swing ee compensation. Exponential zero retrieving.
1669  stikp[i].d_rpy_swing = calcDampingControl(stikp[i].d_rpy_swing, stikp[i].eefm_swing_rot_time_const);
1670  stikp[i].d_pos_swing = calcDampingControl(stikp[i].d_pos_swing, stikp[i].eefm_swing_pos_time_const);
1671  stikp[i].target_ee_diff_p_filter->reset(stikp[i].d_pos_swing);
1672  stikp[i].target_ee_diff_r_filter->reset(stikp[i].d_rpy_swing);
1673  } else {
1674  /* position */
1675  {
1676  hrp::Vector3 tmpdiffp = stikp[i].eefm_swing_pos_spring_gain.cwiseProduct(stikp[i].target_ee_diff_p_filter->passFilter(stikp[i].target_ee_diff_p));
1677  double lvlimit = -50 * 1e-3 * dt, uvlimit = 50 * 1e-3 * dt; // 50 [mm/s]
1678  hrp::Vector3 limit_by_lvlimit = stikp[i].prev_d_pos_swing + lvlimit * hrp::Vector3::Ones();
1679  hrp::Vector3 limit_by_uvlimit = stikp[i].prev_d_pos_swing + uvlimit * hrp::Vector3::Ones();
1680  stikp[i].d_pos_swing = vlimit(vlimit(tmpdiffp, -1 * limit_pos, limit_pos), limit_by_lvlimit, limit_by_uvlimit);
1681  }
1682  /* rotation */
1683  {
1684  hrp::Vector3 tmpdiffr = stikp[i].eefm_swing_rot_spring_gain.cwiseProduct(stikp[i].target_ee_diff_r_filter->passFilter(hrp::rpyFromRot(stikp[i].target_ee_diff_r)));
1685  double lvlimit = deg2rad(-20.0*dt), uvlimit = deg2rad(20.0*dt); // 20 [deg/s]
1686  hrp::Vector3 limit_by_lvlimit = stikp[i].prev_d_rpy_swing + lvlimit * hrp::Vector3::Ones();
1687  hrp::Vector3 limit_by_uvlimit = stikp[i].prev_d_rpy_swing + uvlimit * hrp::Vector3::Ones();
1688  stikp[i].d_rpy_swing = vlimit(vlimit(tmpdiffr, -1 * limit_rot, limit_rot), limit_by_lvlimit, limit_by_uvlimit);
1689  }
1690  }
1691  stikp[i].prev_d_pos_swing = stikp[i].d_pos_swing;
1692  stikp[i].prev_d_rpy_swing = stikp[i].d_rpy_swing;
1693  }
1694  if (DEBUGP) {
1695  std::cerr << "[" << m_profile.instance_name << "] Swing foot control" << std::endl;
1696  for (size_t i = 0; i < stikp.size(); i++) {
1697  std::cerr << "[" << m_profile.instance_name << "] "
1698  << "d_rpy_swing (" << stikp[i].ee_name << ") = " << (stikp[i].d_rpy_swing / M_PI * 180.0).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[deg], "
1699  << "d_pos_swing (" << stikp[i].ee_name << ") = " << (stikp[i].d_pos_swing * 1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[mm]" << std::endl;
1700  }
1701  }
1702 };
1703 
1704 void Stabilizer::limbStretchAvoidanceControl (const std::vector<hrp::Vector3>& ee_p, const std::vector<hrp::Matrix33>& ee_R)
1705 {
1706  double tmp_d_pos_z_root = 0.0, prev_d_pos_z_root = d_pos_z_root;
1708  for (size_t i = 0; i < stikp.size(); i++) {
1709  if (is_ik_enable[i]) {
1710  // Check whether inside limb length limitation
1711  hrp::Link* parent_link = m_robot->link(stikp[i].parent_name);
1712  hrp::Vector3 targetp = (ee_p[i] - ee_R[i] * stikp[i].localR.transpose() * stikp[i].localp) - parent_link->p; // position from parent to target link (world frame)
1713  double limb_length_limitation = stikp[i].max_limb_length - stikp[i].limb_length_margin;
1714  double tmp = limb_length_limitation * limb_length_limitation - targetp(0) * targetp(0) - targetp(1) * targetp(1);
1715  if (targetp.norm() > limb_length_limitation && tmp >= 0) {
1716  tmp_d_pos_z_root = std::min(tmp_d_pos_z_root, targetp(2) + std::sqrt(tmp));
1717  }
1718  }
1719  }
1720  // Change root link height depending on limb length
1721  d_pos_z_root = tmp_d_pos_z_root == 0.0 ? calcDampingControl(d_pos_z_root, limb_stretch_avoidance_time_const) : tmp_d_pos_z_root;
1722  } else {
1724  }
1725  d_pos_z_root = vlimit(d_pos_z_root, prev_d_pos_z_root + limb_stretch_avoidance_vlimit[0], prev_d_pos_z_root + limb_stretch_avoidance_vlimit[1]);
1726  m_robot->rootLink()->p(2) += d_pos_z_root;
1727 }
1728 
1729 // Damping control functions
1730 // Basically Equation (14) in the paper [1]
1731 double Stabilizer::calcDampingControl (const double tau_d, const double tau, const double prev_d,
1732  const double DD, const double TT)
1733 {
1734  return (1/DD * (tau_d - tau) - 1/TT * prev_d) * dt + prev_d;
1735 };
1736 
1737 // Retrieving only
1739 {
1740  return (- prev_d.cwiseQuotient(TT)) * dt + prev_d;
1741 };
1742 
1743 // Retrieving only
1744 double Stabilizer::calcDampingControl (const double prev_d, const double TT)
1745 {
1746  return - 1/TT * prev_d * dt + prev_d;
1747 };
1748 
1750  const hrp::Vector3& DD, const hrp::Vector3& TT)
1751 {
1752  return ((tau_d - tau).cwiseQuotient(DD) - prev_d.cwiseQuotient(TT)) * dt + prev_d;
1753 };
1754 
1756 {
1757  // calc reference ext moment around foot origin pos
1758  // static const double grav = 9.80665; /* [m/s^2] */
1760  hrp::Vector3 ref_ext_moment = hrp::Vector3(mg * ref_cog(1) - ref_total_foot_origin_moment(0),
1761  -mg * ref_cog(0) - ref_total_foot_origin_moment(1),
1762  0);
1763  // calc act ext moment around foot origin pos
1764  hrp::Vector3 act_ext_moment = hrp::Vector3(mg * act_cog(1) - act_total_foot_origin_moment(0),
1765  -mg * act_cog(0) - act_total_foot_origin_moment(1),
1766  0);
1767  // Do not calculate actual value if in the air, because of invalid act_zmp.
1768  if ( !on_ground ) act_ext_moment = ref_ext_moment;
1769  // Calc diff
1770  diff_foot_origin_ext_moment = ref_ext_moment - act_ext_moment;
1771  if (DEBUGP) {
1772  std::cerr << "[" << m_profile.instance_name << "] DiffStaticBalancePointOffset" << std::endl;
1773  std::cerr << "[" << m_profile.instance_name << "] "
1774  << "ref_ext_moment = " << ref_ext_moment.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[mm], "
1775  << "act_ext_moment = " << act_ext_moment.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[mm], "
1776  << "diff ext_moment = " << diff_foot_origin_ext_moment.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[mm]" << std::endl;
1777  }
1778 };
1779 
1780 /*
1781 RTC::ReturnCode_t Stabilizer::onAborting(RTC::UniqueId ec_id)
1782 {
1783  return RTC::RTC_OK;
1784 }
1785 */
1786 
1787 /*
1788 RTC::ReturnCode_t Stabilizer::onError(RTC::UniqueId ec_id)
1789 {
1790  return RTC::RTC_OK;
1791 }
1792 */
1793 
1794 /*
1795 RTC::ReturnCode_t Stabilizer::onReset(RTC::UniqueId ec_id)
1796 {
1797  return RTC::RTC_OK;
1798 }
1799 */
1800 
1801 /*
1802 RTC::ReturnCode_t Stabilizer::onStateUpdate(RTC::UniqueId ec_id)
1803 {
1804  return RTC::RTC_OK;
1805 }
1806 */
1807 
1808 /*
1809 RTC::ReturnCode_t Stabilizer::onRateChanged(RTC::UniqueId ec_id)
1810 {
1811  return RTC::RTC_OK;
1812 }
1813 */
1814 
1816 {
1817  std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
1818  << "] Sync IDLE => ST" << std::endl;
1819  pangx_ref = pangy_ref = pangx = pangy = 0;
1820  rdx = rdy = rx = ry = 0;
1821  d_rpy[0] = d_rpy[1] = 0;
1822  pdr = hrp::Vector3::Zero();
1823  pos_ctrl = hrp::Vector3::Zero();
1824  for (size_t i = 0; i < stikp.size(); i++) {
1825  STIKParam& ikp = stikp[i];
1826  ikp.target_ee_diff_p = hrp::Vector3::Zero();
1827  ikp.target_ee_diff_r = hrp::Matrix33::Identity();
1828  ikp.d_pos_swing = ikp.prev_d_pos_swing = hrp::Vector3::Zero();
1829  ikp.d_rpy_swing = ikp.prev_d_rpy_swing = hrp::Vector3::Zero();
1830  ikp.target_ee_diff_p_filter->reset(hrp::Vector3::Zero());
1831  ikp.target_ee_diff_r_filter->reset(hrp::Vector3::Zero());
1832  ikp.d_foot_pos = ikp.ee_d_foot_pos = ikp.d_foot_rpy = ikp.ee_d_foot_rpy = hrp::Vector3::Zero();
1833  }
1834  if (on_ground) {
1837  } else {
1838  transition_count = 0;
1840  }
1841 }
1842 
1844 {
1845  std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm
1846  << "] Sync ST => IDLE" << std::endl;
1848  for (int i = 0; i < m_robot->numJoints(); i++ ) {
1849  transition_joint_q[i] = m_robot->joint(i)->q;
1850  }
1851 }
1852 
1854 {
1855  waitSTTransition(); // Wait until all transition has finished
1856  {
1857  Guard guard(m_mutex);
1858  if ( control_mode == MODE_IDLE ) {
1859  std::cerr << "[" << m_profile.instance_name << "] " << "Start ST" << std::endl;
1860  sync_2_st();
1861  }
1862  }
1863  waitSTTransition();
1864  std::cerr << "[" << m_profile.instance_name << "] " << "Start ST DONE" << std::endl;
1865 }
1866 
1868 {
1869  waitSTTransition(); // Wait until all transition has finished
1870  {
1871  Guard guard(m_mutex);
1872  if ( (control_mode == MODE_ST || control_mode == MODE_AIR) ) {
1873  std::cerr << "[" << m_profile.instance_name << "] " << "Stop ST" << std::endl;
1875  }
1876  }
1877  waitSTTransition();
1878  std::cerr << "[" << m_profile.instance_name << "] " << "Stop ST DONE" << std::endl;
1879 }
1880 
1881 void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp)
1882 {
1883  std::cerr << "[" << m_profile.instance_name << "] getParameter" << std::endl;
1884  for (size_t i = 0; i < 2; i++) {
1885  // i_stp.k_run_b[i] = k_run_b[i];
1886  // i_stp.d_run_b[i] = d_run_b[i];
1887  //m_tau_x[i].setup(i_stp.tdfke[0], i_stp.tdftc[0], dt);
1888  //m_tau_y[i].setup(i_stp.tdfke[0], i_stp.tdftc[0], dt);
1889  //m_f_z.setup(i_stp.tdfke[1], i_stp.tdftc[1], dt);
1890  i_stp.k_tpcc_p[i] = k_tpcc_p[i];
1891  i_stp.k_tpcc_x[i] = k_tpcc_x[i];
1892  i_stp.k_brot_p[i] = k_brot_p[i];
1893  i_stp.k_brot_tc[i] = k_brot_tc[i];
1894  }
1895  // i_stp.k_run_x = m_torque_k[0];
1896  // i_stp.k_run_y = m_torque_k[1];
1897  // i_stp.d_run_x = m_torque_d[0];
1898  // i_stp.d_run_y = m_torque_d[1];
1899  for (size_t i = 0; i < 2; i++) {
1900  i_stp.eefm_k1[i] = eefm_k1[i];
1901  i_stp.eefm_k2[i] = eefm_k2[i];
1902  i_stp.eefm_k3[i] = eefm_k3[i];
1903  i_stp.eefm_zmp_delay_time_const[i] = eefm_zmp_delay_time_const[i];
1904  i_stp.eefm_ref_zmp_aux[i] = ref_zmp_aux(i);
1905  i_stp.eefm_body_attitude_control_time_const[i] = eefm_body_attitude_control_time_const[i];
1906  i_stp.eefm_body_attitude_control_gain[i] = eefm_body_attitude_control_gain[i];
1907  i_stp.ref_capture_point[i] = ref_cp(i);
1908  i_stp.act_capture_point[i] = act_cp(i);
1909  i_stp.cp_offset[i] = cp_offset(i);
1910  }
1911  i_stp.eefm_pos_time_const_support.length(stikp.size());
1912  i_stp.eefm_pos_damping_gain.length(stikp.size());
1913  i_stp.eefm_pos_compensation_limit.length(stikp.size());
1914  i_stp.eefm_swing_pos_spring_gain.length(stikp.size());
1915  i_stp.eefm_swing_pos_time_const.length(stikp.size());
1916  i_stp.eefm_rot_time_const.length(stikp.size());
1917  i_stp.eefm_rot_damping_gain.length(stikp.size());
1918  i_stp.eefm_rot_compensation_limit.length(stikp.size());
1919  i_stp.eefm_swing_rot_spring_gain.length(stikp.size());
1920  i_stp.eefm_swing_rot_time_const.length(stikp.size());
1921  i_stp.eefm_ee_moment_limit.length(stikp.size());
1922  i_stp.eefm_ee_forcemoment_distribution_weight.length(stikp.size());
1923  for (size_t j = 0; j < stikp.size(); j++) {
1924  i_stp.eefm_pos_damping_gain[j].length(3);
1925  i_stp.eefm_pos_time_const_support[j].length(3);
1926  i_stp.eefm_swing_pos_spring_gain[j].length(3);
1927  i_stp.eefm_swing_pos_time_const[j].length(3);
1928  i_stp.eefm_rot_damping_gain[j].length(3);
1929  i_stp.eefm_rot_time_const[j].length(3);
1930  i_stp.eefm_swing_rot_spring_gain[j].length(3);
1931  i_stp.eefm_swing_rot_time_const[j].length(3);
1932  i_stp.eefm_ee_moment_limit[j].length(3);
1933  i_stp.eefm_ee_forcemoment_distribution_weight[j].length(6);
1934  for (size_t i = 0; i < 3; i++) {
1935  i_stp.eefm_pos_damping_gain[j][i] = stikp[j].eefm_pos_damping_gain(i);
1936  i_stp.eefm_pos_time_const_support[j][i] = stikp[j].eefm_pos_time_const_support(i);
1937  i_stp.eefm_swing_pos_spring_gain[j][i] = stikp[j].eefm_swing_pos_spring_gain(i);
1938  i_stp.eefm_swing_pos_time_const[j][i] = stikp[j].eefm_swing_pos_time_const(i);
1939  i_stp.eefm_rot_damping_gain[j][i] = stikp[j].eefm_rot_damping_gain(i);
1940  i_stp.eefm_rot_time_const[j][i] = stikp[j].eefm_rot_time_const(i);
1941  i_stp.eefm_swing_rot_spring_gain[j][i] = stikp[j].eefm_swing_rot_spring_gain(i);
1942  i_stp.eefm_swing_rot_time_const[j][i] = stikp[j].eefm_swing_rot_time_const(i);
1943  i_stp.eefm_ee_moment_limit[j][i] = stikp[j].eefm_ee_moment_limit(i);
1944  i_stp.eefm_ee_forcemoment_distribution_weight[j][i] = stikp[j].eefm_ee_forcemoment_distribution_weight(i);
1945  i_stp.eefm_ee_forcemoment_distribution_weight[j][i+3] = stikp[j].eefm_ee_forcemoment_distribution_weight(i+3);
1946  }
1947  i_stp.eefm_pos_compensation_limit[j] = stikp[j].eefm_pos_compensation_limit;
1948  i_stp.eefm_rot_compensation_limit[j] = stikp[j].eefm_rot_compensation_limit;
1949  }
1950  for (size_t i = 0; i < 3; i++) {
1951  i_stp.eefm_swing_pos_damping_gain[i] = eefm_swing_pos_damping_gain(i);
1952  i_stp.eefm_swing_rot_damping_gain[i] = eefm_swing_rot_damping_gain(i);
1953  }
1954  i_stp.eefm_pos_time_const_swing = eefm_pos_time_const_swing;
1955  i_stp.eefm_pos_transition_time = eefm_pos_transition_time;
1956  i_stp.eefm_pos_margin_time = eefm_pos_margin_time;
1957  i_stp.eefm_leg_inside_margin = szd->get_leg_inside_margin();
1958  i_stp.eefm_leg_outside_margin = szd->get_leg_outside_margin();
1959  i_stp.eefm_leg_front_margin = szd->get_leg_front_margin();
1960  i_stp.eefm_leg_rear_margin = szd->get_leg_rear_margin();
1961 
1962  std::vector<std::vector<Eigen::Vector2d> > support_polygon_vec;
1963  szd->get_vertices(support_polygon_vec);
1964  i_stp.eefm_support_polygon_vertices_sequence.length(support_polygon_vec.size());
1965  for (size_t ee_idx = 0; ee_idx < support_polygon_vec.size(); ee_idx++) {
1966  i_stp.eefm_support_polygon_vertices_sequence[ee_idx].vertices.length(support_polygon_vec[ee_idx].size());
1967  for (size_t v_idx = 0; v_idx < support_polygon_vec[ee_idx].size(); v_idx++) {
1968  i_stp.eefm_support_polygon_vertices_sequence[ee_idx].vertices[v_idx].pos[0] = support_polygon_vec[ee_idx][v_idx](0);
1969  i_stp.eefm_support_polygon_vertices_sequence[ee_idx].vertices[v_idx].pos[1] = support_polygon_vec[ee_idx][v_idx](1);
1970  }
1971  }
1972 
1973  i_stp.eefm_cogvel_cutoff_freq = act_cogvel_filter->getCutOffFreq();
1974  i_stp.eefm_wrench_alpha_blending = szd->get_wrench_alpha_blending();
1975  i_stp.eefm_alpha_cutoff_freq = szd->get_alpha_cutoff_freq();
1976  i_stp.eefm_gravitational_acceleration = eefm_gravitational_acceleration;
1977  i_stp.eefm_ee_error_cutoff_freq = stikp[0].target_ee_diff_p_filter->getCutOffFreq();
1978  i_stp.eefm_use_force_difference_control = eefm_use_force_difference_control;
1979  i_stp.eefm_use_swing_damping = eefm_use_swing_damping;
1980  for (size_t i = 0; i < 3; ++i) {
1981  i_stp.eefm_swing_damping_force_thre[i] = eefm_swing_damping_force_thre[i];
1982  i_stp.eefm_swing_damping_moment_thre[i] = eefm_swing_damping_moment_thre[i];
1983  }
1984  i_stp.is_ik_enable.length(is_ik_enable.size());
1985  for (size_t i = 0; i < is_ik_enable.size(); i++) {
1986  i_stp.is_ik_enable[i] = is_ik_enable[i];
1987  }
1988  i_stp.is_feedback_control_enable.length(is_feedback_control_enable.size());
1989  for (size_t i = 0; i < is_feedback_control_enable.size(); i++) {
1990  i_stp.is_feedback_control_enable[i] = is_feedback_control_enable[i];
1991  }
1992  i_stp.is_zmp_calc_enable.length(is_zmp_calc_enable.size());
1993  for (size_t i = 0; i < is_zmp_calc_enable.size(); i++) {
1994  i_stp.is_zmp_calc_enable[i] = is_zmp_calc_enable[i];
1995  }
1996 
1997  i_stp.foot_origin_offset.length(2);
1998  for (size_t i = 0; i < i_stp.foot_origin_offset.length(); i++) {
1999  i_stp.foot_origin_offset[i].length(3);
2000  i_stp.foot_origin_offset[i][0] = foot_origin_offset[i](0);
2001  i_stp.foot_origin_offset[i][1] = foot_origin_offset[i](1);
2002  i_stp.foot_origin_offset[i][2] = foot_origin_offset[i](2);
2003  }
2004  i_stp.st_algorithm = st_algorithm;
2005  i_stp.transition_time = transition_time;
2006  i_stp.cop_check_margin = cop_check_margin;
2007  for (size_t i = 0; i < cp_check_margin.size(); i++) {
2008  i_stp.cp_check_margin[i] = cp_check_margin[i];
2009  }
2010  for (size_t i = 0; i < tilt_margin.size(); i++) {
2011  i_stp.tilt_margin[i] = tilt_margin[i];
2012  }
2013  i_stp.contact_decision_threshold = contact_decision_threshold;
2014  i_stp.is_estop_while_walking = is_estop_while_walking;
2015  switch(control_mode) {
2016  case MODE_IDLE: i_stp.controller_mode = OpenHRP::StabilizerService::MODE_IDLE; break;
2017  case MODE_AIR: i_stp.controller_mode = OpenHRP::StabilizerService::MODE_AIR; break;
2018  case MODE_ST: i_stp.controller_mode = OpenHRP::StabilizerService::MODE_ST; break;
2019  case MODE_SYNC_TO_IDLE: i_stp.controller_mode = OpenHRP::StabilizerService::MODE_SYNC_TO_IDLE; break;
2020  case MODE_SYNC_TO_AIR: i_stp.controller_mode = OpenHRP::StabilizerService::MODE_SYNC_TO_AIR; break;
2021  default: break;
2022  }
2023  i_stp.emergency_check_mode = emergency_check_mode;
2024  i_stp.end_effector_list.length(stikp.size());
2025  i_stp.use_limb_stretch_avoidance = use_limb_stretch_avoidance;
2026  i_stp.use_zmp_truncation = use_zmp_truncation;
2027  i_stp.limb_stretch_avoidance_time_const = limb_stretch_avoidance_time_const;
2028  i_stp.limb_length_margin.length(stikp.size());
2029  i_stp.detection_time_to_air = detection_count_to_air * dt;
2030  for (size_t i = 0; i < 2; i++) {
2031  i_stp.limb_stretch_avoidance_vlimit[i] = limb_stretch_avoidance_vlimit[i];
2032  i_stp.root_rot_compensation_limit[i] = root_rot_compensation_limit[i];
2033  }
2034  for (size_t i = 0; i < stikp.size(); i++) {
2035  const rats::coordinates cur_ee = rats::coordinates(stikp.at(i).localp, stikp.at(i).localR);
2036  OpenHRP::AutoBalancerService::Footstep ret_ee;
2037  // position
2038  memcpy(ret_ee.pos, cur_ee.pos.data(), sizeof(double)*3);
2039  // rotation
2040  Eigen::Quaternion<double> qt(cur_ee.rot);
2041  ret_ee.rot[0] = qt.w();
2042  ret_ee.rot[1] = qt.x();
2043  ret_ee.rot[2] = qt.y();
2044  ret_ee.rot[3] = qt.z();
2045  // name
2046  ret_ee.leg = stikp.at(i).ee_name.c_str();
2047  // set
2048  i_stp.end_effector_list[i] = ret_ee;
2049  i_stp.limb_length_margin[i] = stikp[i].limb_length_margin;
2050  }
2051  i_stp.ik_limb_parameters.length(jpe_v.size());
2052  for (size_t i = 0; i < jpe_v.size(); i++) {
2053  OpenHRP::StabilizerService::IKLimbParameters& ilp = i_stp.ik_limb_parameters[i];
2054  ilp.ik_optional_weight_vector.length(jpe_v[i]->numJoints());
2055  std::vector<double> ov;
2056  ov.resize(jpe_v[i]->numJoints());
2057  jpe_v[i]->getOptionalWeightVector(ov);
2058  for (size_t j = 0; j < jpe_v[i]->numJoints(); j++) {
2059  ilp.ik_optional_weight_vector[j] = ov[j];
2060  }
2061  ilp.sr_gain = jpe_v[i]->getSRGain();
2062  ilp.avoid_gain = stikp[i].avoid_gain;
2063  ilp.reference_gain = stikp[i].reference_gain;
2064  ilp.manipulability_limit = jpe_v[i]->getManipulabilityLimit();
2065  ilp.ik_loop_count = stikp[i].ik_loop_count; // size_t -> unsigned short, value may change, but ik_loop_count is small value and value not change
2066  }
2067 };
2068 
2069 void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp)
2070 {
2071  Guard guard(m_mutex);
2072  std::cerr << "[" << m_profile.instance_name << "] setParameter" << std::endl;
2073  for (size_t i = 0; i < 2; i++) {
2074  k_tpcc_p[i] = i_stp.k_tpcc_p[i];
2075  k_tpcc_x[i] = i_stp.k_tpcc_x[i];
2076  k_brot_p[i] = i_stp.k_brot_p[i];
2077  k_brot_tc[i] = i_stp.k_brot_tc[i];
2078  }
2079  std::cerr << "[" << m_profile.instance_name << "] TPCC" << std::endl;
2080  std::cerr << "[" << m_profile.instance_name << "] k_tpcc_p = [" << k_tpcc_p[0] << ", " << k_tpcc_p[1] << "], k_tpcc_x = [" << k_tpcc_x[0] << ", " << k_tpcc_x[1] << "], k_brot_p = [" << k_brot_p[0] << ", " << k_brot_p[1] << "], k_brot_tc = [" << k_brot_tc[0] << ", " << k_brot_tc[1] << "]" << std::endl;
2081  // for (size_t i = 0; i < 2; i++) {
2082  // k_run_b[i] = i_stp.k_run_b[i];
2083  // d_run_b[i] = i_stp.d_run_b[i];
2084  // m_tau_x[i].setup(i_stp.tdfke[0], i_stp.tdftc[0], dt);
2085  // m_tau_y[i].setup(i_stp.tdfke[0], i_stp.tdftc[0], dt);
2086  // m_f_z.setup(i_stp.tdfke[1], i_stp.tdftc[1], dt);
2087  // }
2088  // m_torque_k[0] = i_stp.k_run_x;
2089  // m_torque_k[1] = i_stp.k_run_y;
2090  // m_torque_d[0] = i_stp.d_run_x;
2091  // m_torque_d[1] = i_stp.d_run_y;
2092  // std::cerr << "[" << m_profile.instance_name << "] RUNST" << std::endl;
2093  // std::cerr << "[" << m_profile.instance_name << "] m_torque_k = [" << m_torque_k[0] << ", " << m_torque_k[1] << "]" << std::endl;
2094  // std::cerr << "[" << m_profile.instance_name << "] m_torque_d = [" << m_torque_d[0] << ", " << m_torque_d[1] << "]" << std::endl;
2095  // std::cerr << "[" << m_profile.instance_name << "] k_run_b = [" << k_run_b[0] << ", " << k_run_b[1] << "]" << std::endl;
2096  // std::cerr << "[" << m_profile.instance_name << "] d_run_b = [" << d_run_b[0] << ", " << d_run_b[1] << "]" << std::endl;
2097  std::cerr << "[" << m_profile.instance_name << "] EEFM" << std::endl;
2098  for (size_t i = 0; i < 2; i++) {
2099  eefm_k1[i] = i_stp.eefm_k1[i];
2100  eefm_k2[i] = i_stp.eefm_k2[i];
2101  eefm_k3[i] = i_stp.eefm_k3[i];
2102  eefm_zmp_delay_time_const[i] = i_stp.eefm_zmp_delay_time_const[i];
2103  ref_zmp_aux(i) = i_stp.eefm_ref_zmp_aux[i];
2104  eefm_body_attitude_control_gain[i] = i_stp.eefm_body_attitude_control_gain[i];
2105  eefm_body_attitude_control_time_const[i] = i_stp.eefm_body_attitude_control_time_const[i];
2106  ref_cp(i) = i_stp.ref_capture_point[i];
2107  act_cp(i) = i_stp.act_capture_point[i];
2108  cp_offset(i) = i_stp.cp_offset[i];
2109  }
2110  bool is_damping_parameter_ok = true;
2111  if ( i_stp.eefm_pos_damping_gain.length () == stikp.size() &&
2112  i_stp.eefm_pos_time_const_support.length () == stikp.size() &&
2113  i_stp.eefm_pos_compensation_limit.length () == stikp.size() &&
2114  i_stp.eefm_swing_pos_spring_gain.length () == stikp.size() &&
2115  i_stp.eefm_swing_pos_time_const.length () == stikp.size() &&
2116  i_stp.eefm_rot_damping_gain.length () == stikp.size() &&
2117  i_stp.eefm_rot_time_const.length () == stikp.size() &&
2118  i_stp.eefm_rot_compensation_limit.length () == stikp.size() &&
2119  i_stp.eefm_swing_rot_spring_gain.length () == stikp.size() &&
2120  i_stp.eefm_swing_rot_time_const.length () == stikp.size() &&
2121  i_stp.eefm_ee_moment_limit.length () == stikp.size() &&
2122  i_stp.eefm_ee_forcemoment_distribution_weight.length () == stikp.size()) {
2123  is_damping_parameter_ok = true;
2124  for (size_t j = 0; j < stikp.size(); j++) {
2125  for (size_t i = 0; i < 3; i++) {
2126  stikp[j].eefm_pos_damping_gain(i) = i_stp.eefm_pos_damping_gain[j][i];
2127  stikp[j].eefm_pos_time_const_support(i) = i_stp.eefm_pos_time_const_support[j][i];
2128  stikp[j].eefm_swing_pos_spring_gain(i) = i_stp.eefm_swing_pos_spring_gain[j][i];
2129  stikp[j].eefm_swing_pos_time_const(i) = i_stp.eefm_swing_pos_time_const[j][i];
2130  stikp[j].eefm_rot_damping_gain(i) = i_stp.eefm_rot_damping_gain[j][i];
2131  stikp[j].eefm_rot_time_const(i) = i_stp.eefm_rot_time_const[j][i];
2132  stikp[j].eefm_swing_rot_spring_gain(i) = i_stp.eefm_swing_rot_spring_gain[j][i];
2133  stikp[j].eefm_swing_rot_time_const(i) = i_stp.eefm_swing_rot_time_const[j][i];
2134  stikp[j].eefm_ee_moment_limit(i) = i_stp.eefm_ee_moment_limit[j][i];
2135  stikp[j].eefm_ee_forcemoment_distribution_weight(i) = i_stp.eefm_ee_forcemoment_distribution_weight[j][i];
2136  stikp[j].eefm_ee_forcemoment_distribution_weight(i+3) = i_stp.eefm_ee_forcemoment_distribution_weight[j][i+3];
2137  }
2138  stikp[j].eefm_pos_compensation_limit = i_stp.eefm_pos_compensation_limit[j];
2139  stikp[j].eefm_rot_compensation_limit = i_stp.eefm_rot_compensation_limit[j];
2140  }
2141  } else {
2142  is_damping_parameter_ok = false;
2143  }
2144  for (size_t i = 0; i < 3; i++) {
2145  eefm_swing_pos_damping_gain(i) = i_stp.eefm_swing_pos_damping_gain[i];
2146  eefm_swing_rot_damping_gain(i) = i_stp.eefm_swing_rot_damping_gain[i];
2147  }
2148  eefm_pos_time_const_swing = i_stp.eefm_pos_time_const_swing;
2149  eefm_pos_transition_time = i_stp.eefm_pos_transition_time;
2150  eefm_pos_margin_time = i_stp.eefm_pos_margin_time;
2151  szd->set_leg_inside_margin(i_stp.eefm_leg_inside_margin);
2152  szd->set_leg_outside_margin(i_stp.eefm_leg_outside_margin);
2153  szd->set_leg_front_margin(i_stp.eefm_leg_front_margin);
2154  szd->set_leg_rear_margin(i_stp.eefm_leg_rear_margin);
2156 
2157  if (i_stp.eefm_support_polygon_vertices_sequence.length() != stikp.size()) {
2158  std::cerr << "[" << m_profile.instance_name << "] eefm_support_polygon_vertices_sequence cannot be set. Length " << i_stp.eefm_support_polygon_vertices_sequence.length() << " != " << stikp.size() << std::endl;
2159  } else {
2160  std::cerr << "[" << m_profile.instance_name << "] eefm_support_polygon_vertices_sequence set" << std::endl;
2161  std::vector<std::vector<Eigen::Vector2d> > support_polygon_vec;
2162  for (size_t ee_idx = 0; ee_idx < i_stp.eefm_support_polygon_vertices_sequence.length(); ee_idx++) {
2163  std::vector<Eigen::Vector2d> tvec;
2164  for (size_t v_idx = 0; v_idx < i_stp.eefm_support_polygon_vertices_sequence[ee_idx].vertices.length(); v_idx++) {
2165  tvec.push_back(Eigen::Vector2d(i_stp.eefm_support_polygon_vertices_sequence[ee_idx].vertices[v_idx].pos[0],
2166  i_stp.eefm_support_polygon_vertices_sequence[ee_idx].vertices[v_idx].pos[1]));
2167  }
2168  support_polygon_vec.push_back(tvec);
2169  }
2170  szd->set_vertices(support_polygon_vec);
2171  szd->print_vertices(std::string(m_profile.instance_name));
2172  }
2173  eefm_use_force_difference_control = i_stp.eefm_use_force_difference_control;
2174  eefm_use_swing_damping = i_stp.eefm_use_swing_damping;
2175  for (size_t i = 0; i < 3; ++i) {
2176  eefm_swing_damping_force_thre[i] = i_stp.eefm_swing_damping_force_thre[i];
2177  eefm_swing_damping_moment_thre[i] = i_stp.eefm_swing_damping_moment_thre[i];
2178  }
2179  act_cogvel_filter->setCutOffFreq(i_stp.eefm_cogvel_cutoff_freq);
2180  szd->set_wrench_alpha_blending(i_stp.eefm_wrench_alpha_blending);
2181  szd->set_alpha_cutoff_freq(i_stp.eefm_alpha_cutoff_freq);
2182  eefm_gravitational_acceleration = i_stp.eefm_gravitational_acceleration;
2183  for (size_t i = 0; i < stikp.size(); i++) {
2184  stikp[i].target_ee_diff_p_filter->setCutOffFreq(i_stp.eefm_ee_error_cutoff_freq);
2185  stikp[i].target_ee_diff_r_filter->setCutOffFreq(i_stp.eefm_ee_error_cutoff_freq);
2186  stikp[i].limb_length_margin = i_stp.limb_length_margin[i];
2187  }
2188  setBoolSequenceParam(is_ik_enable, i_stp.is_ik_enable, std::string("is_ik_enable"));
2189  setBoolSequenceParamWithCheckContact(is_feedback_control_enable, i_stp.is_feedback_control_enable, std::string("is_feedback_control_enable"));
2190  setBoolSequenceParam(is_zmp_calc_enable, i_stp.is_zmp_calc_enable, std::string("is_zmp_calc_enable"));
2191  emergency_check_mode = i_stp.emergency_check_mode;
2192 
2193  transition_time = i_stp.transition_time;
2194  cop_check_margin = i_stp.cop_check_margin;
2195  for (size_t i = 0; i < cp_check_margin.size(); i++) {
2196  cp_check_margin[i] = i_stp.cp_check_margin[i];
2197  }
2199  for (size_t i = 0; i < tilt_margin.size(); i++) {
2200  tilt_margin[i] = i_stp.tilt_margin[i];
2201  }
2202  contact_decision_threshold = i_stp.contact_decision_threshold;
2203  is_estop_while_walking = i_stp.is_estop_while_walking;
2204  use_limb_stretch_avoidance = i_stp.use_limb_stretch_avoidance;
2205  use_zmp_truncation = i_stp.use_zmp_truncation;
2206  limb_stretch_avoidance_time_const = i_stp.limb_stretch_avoidance_time_const;
2207  for (size_t i = 0; i < 2; i++) {
2208  limb_stretch_avoidance_vlimit[i] = i_stp.limb_stretch_avoidance_vlimit[i];
2209  root_rot_compensation_limit[i] = i_stp.root_rot_compensation_limit[i];
2210  }
2211  detection_count_to_air = static_cast<int>(i_stp.detection_time_to_air / dt);
2212  if (control_mode == MODE_IDLE) {
2213  for (size_t i = 0; i < i_stp.end_effector_list.length(); i++) {
2214  std::vector<STIKParam>::iterator it = std::find_if(stikp.begin(), stikp.end(), (&boost::lambda::_1->* &std::vector<STIKParam>::value_type::ee_name == std::string(i_stp.end_effector_list[i].leg)));
2215  memcpy(it->localp.data(), i_stp.end_effector_list[i].pos, sizeof(double)*3);
2216  it->localR = (Eigen::Quaternion<double>(i_stp.end_effector_list[i].rot[0], i_stp.end_effector_list[i].rot[1], i_stp.end_effector_list[i].rot[2], i_stp.end_effector_list[i].rot[3])).normalized().toRotationMatrix();
2217  }
2218  } else {
2219  std::cerr << "[" << m_profile.instance_name << "] cannot change end-effectors except during MODE_IDLE" << std::endl;
2220  }
2221  for (std::vector<STIKParam>::const_iterator it = stikp.begin(); it != stikp.end(); it++) {
2222  std::cerr << "[" << m_profile.instance_name << "] End Effector [" << it->ee_name << "]" << std::endl;
2223  std::cerr << "[" << m_profile.instance_name << "] localpos = " << it->localp.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl;
2224  std::cerr << "[" << m_profile.instance_name << "] localR = " << it->localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "", " [", "]")) << std::endl;
2225  }
2226  if (i_stp.foot_origin_offset.length () != 2) {
2227  std::cerr << "[" << m_profile.instance_name << "] foot_origin_offset cannot be set. Length " << i_stp.foot_origin_offset.length() << " != " << 2 << std::endl;
2228  } else if (control_mode != MODE_IDLE) {
2229  std::cerr << "[" << m_profile.instance_name << "] foot_origin_offset cannot be set. Current control_mode is " << control_mode << std::endl;
2230  } else {
2231  for (size_t i = 0; i < i_stp.foot_origin_offset.length(); i++) {
2232  foot_origin_offset[i](0) = i_stp.foot_origin_offset[i][0];
2233  foot_origin_offset[i](1) = i_stp.foot_origin_offset[i][1];
2234  foot_origin_offset[i](2) = i_stp.foot_origin_offset[i][2];
2235  }
2236  }
2237  std::cerr << "[" << m_profile.instance_name << "] foot_origin_offset is ";
2238  for (size_t i = 0; i < 2; i++) {
2239  std::cerr << foot_origin_offset[i].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]"));
2240  }
2241  std::cerr << "[m]" << std::endl;
2242  std::cerr << "[" << m_profile.instance_name << "] eefm_k1 = [" << eefm_k1[0] << ", " << eefm_k1[1] << "], eefm_k2 = [" << eefm_k2[0] << ", " << eefm_k2[1] << "], eefm_k3 = [" << eefm_k3[0] << ", " << eefm_k3[1] << "]" << std::endl;
2243  std::cerr << "[" << m_profile.instance_name << "] eefm_zmp_delay_time_const = [" << eefm_zmp_delay_time_const[0] << ", " << eefm_zmp_delay_time_const[1] << "][s], eefm_ref_zmp_aux = [" << ref_zmp_aux(0) << ", " << ref_zmp_aux(1) << "][m]" << std::endl;
2244  std::cerr << "[" << m_profile.instance_name << "] eefm_body_attitude_control_gain = [" << eefm_body_attitude_control_gain[0] << ", " << eefm_body_attitude_control_gain[1] << "], eefm_body_attitude_control_time_const = [" << eefm_body_attitude_control_time_const[0] << ", " << eefm_body_attitude_control_time_const[1] << "][s]" << std::endl;
2245  if (is_damping_parameter_ok) {
2246  for (size_t j = 0; j < stikp.size(); j++) {
2247  std::cerr << "[" << m_profile.instance_name << "] [" << stikp[j].ee_name << "] eefm_rot_damping_gain = "
2248  << stikp[j].eefm_rot_damping_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]"))
2249  << ", eefm_rot_time_const = "
2250  << stikp[j].eefm_rot_time_const.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]"))
2251  << "[s]" << std::endl;
2252  std::cerr << "[" << m_profile.instance_name << "] [" << stikp[j].ee_name << "] eefm_pos_damping_gain = "
2253  << stikp[j].eefm_pos_damping_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]"))
2254  << ", eefm_pos_time_const_support = "
2255  << stikp[j].eefm_pos_time_const_support.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]"))
2256  << "[s]" << std::endl;
2257  std::cerr << "[" << m_profile.instance_name << "] [" << stikp[j].ee_name << "] "
2258  << "eefm_pos_compensation_limit = " << stikp[j].eefm_pos_compensation_limit << "[m], "
2259  << "eefm_rot_compensation_limit = " << stikp[j].eefm_rot_compensation_limit << "[rad], "
2260  << "eefm_ee_moment_limit = " << stikp[j].eefm_ee_moment_limit.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[Nm]" << std::endl;
2261  std::cerr << "[" << m_profile.instance_name << "] [" << stikp[j].ee_name << "] "
2262  << "eefm_swing_pos_spring_gain = " << stikp[j].eefm_swing_pos_spring_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << ", "
2263  << "eefm_swing_pos_time_const = " << stikp[j].eefm_swing_pos_time_const.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << ", "
2264  << "eefm_swing_rot_spring_gain = " << stikp[j].eefm_swing_rot_spring_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << ", "
2265  << "eefm_swing_pos_time_const = " << stikp[j].eefm_swing_pos_time_const.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << ", "
2266  << std::endl;
2267  std::cerr << "[" << m_profile.instance_name << "] [" << stikp[j].ee_name << "] "
2268  << "eefm_ee_forcemoment_distribution_weight = " << stikp[j].eefm_ee_forcemoment_distribution_weight.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "" << std::endl;
2269  }
2270  } else {
2271  std::cerr << "[" << m_profile.instance_name << "] eefm damping parameters cannot be set because of invalid param." << std::endl;
2272  }
2273  std::cerr << "[" << m_profile.instance_name << "] eefm_pos_transition_time = " << eefm_pos_transition_time << "[s], eefm_pos_margin_time = " << eefm_pos_margin_time << "[s] eefm_pos_time_const_swing = " << eefm_pos_time_const_swing << "[s]" << std::endl;
2274  std::cerr << "[" << m_profile.instance_name << "] cogvel_cutoff_freq = " << act_cogvel_filter->getCutOffFreq() << "[Hz]" << std::endl;
2275  szd->print_params(std::string(m_profile.instance_name));
2276  std::cerr << "[" << m_profile.instance_name << "] eefm_gravitational_acceleration = " << eefm_gravitational_acceleration << "[m/s^2], eefm_use_force_difference_control = " << (eefm_use_force_difference_control? "true":"false") << ", eefm_use_swing_damping = " << (eefm_use_swing_damping? "true":"false") << std::endl;
2277  std::cerr << "[" << m_profile.instance_name << "] eefm_ee_error_cutoff_freq = " << stikp[0].target_ee_diff_p_filter->getCutOffFreq() << "[Hz]" << std::endl;
2278  std::cerr << "[" << m_profile.instance_name << "] COMMON" << std::endl;
2279  if (control_mode == MODE_IDLE) {
2280  st_algorithm = i_stp.st_algorithm;
2281  std::cerr << "[" << m_profile.instance_name << "] st_algorithm changed to [" << getStabilizerAlgorithmString(st_algorithm) << "]" << std::endl;
2282  } else {
2283  std::cerr << "[" << m_profile.instance_name << "] st_algorithm cannot be changed to [" << getStabilizerAlgorithmString(st_algorithm) << "] during MODE_AIR or MODE_ST." << std::endl;
2284  }
2285  std::cerr << "[" << m_profile.instance_name << "] emergency_check_mode changed to [" << (emergency_check_mode == OpenHRP::StabilizerService::NO_CHECK?"NO_CHECK": (emergency_check_mode == OpenHRP::StabilizerService::COP?"COP":"CP") ) << "]" << std::endl;
2286  std::cerr << "[" << m_profile.instance_name << "] transition_time = " << transition_time << "[s]" << std::endl;
2287  std::cerr << "[" << m_profile.instance_name << "] cop_check_margin = " << cop_check_margin << "[m], "
2288  << "cp_check_margin = [" << cp_check_margin[0] << ", " << cp_check_margin[1] << ", " << cp_check_margin[2] << ", " << cp_check_margin[3] << "] [m], "
2289  << "tilt_margin = [" << tilt_margin[0] << ", " << tilt_margin[1] << "] [rad]" << std::endl;
2290  std::cerr << "[" << m_profile.instance_name << "] contact_decision_threshold = " << contact_decision_threshold << "[N], detection_time_to_air = " << detection_count_to_air * dt << "[s]" << std::endl;
2291  std::cerr << "[" << m_profile.instance_name << "] root_rot_compensation_limit = [" << root_rot_compensation_limit[0] << " " << root_rot_compensation_limit[1] << "][rad]" << std::endl;
2292  // IK limb parameters
2293  std::cerr << "[" << m_profile.instance_name << "] IK limb parameters" << std::endl;
2294  bool is_ik_limb_parameter_valid_length = true;
2295  if (i_stp.ik_limb_parameters.length() != jpe_v.size()) {
2296  is_ik_limb_parameter_valid_length = false;
2297  std::cerr << "[" << m_profile.instance_name << "] ik_limb_parameters invalid length! Cannot be set. (input = " << i_stp.ik_limb_parameters.length() << ", desired = " << jpe_v.size() << ")" << std::endl;
2298  } else {
2299  for (size_t i = 0; i < jpe_v.size(); i++) {
2300  if (jpe_v[i]->numJoints() != i_stp.ik_limb_parameters[i].ik_optional_weight_vector.length())
2301  is_ik_limb_parameter_valid_length = false;
2302  }
2303  if (is_ik_limb_parameter_valid_length) {
2304  for (size_t i = 0; i < jpe_v.size(); i++) {
2305  const OpenHRP::StabilizerService::IKLimbParameters& ilp = i_stp.ik_limb_parameters[i];
2306  std::vector<double> ov;
2307  ov.resize(jpe_v[i]->numJoints());
2308  for (size_t j = 0; j < jpe_v[i]->numJoints(); j++) {
2309  ov[j] = ilp.ik_optional_weight_vector[j];
2310  }
2311  jpe_v[i]->setOptionalWeightVector(ov);
2312  jpe_v[i]->setSRGain(ilp.sr_gain);
2313  stikp[i].avoid_gain = ilp.avoid_gain;
2314  stikp[i].reference_gain = ilp.reference_gain;
2315  jpe_v[i]->setManipulabilityLimit(ilp.manipulability_limit);
2316  stikp[i].ik_loop_count = ilp.ik_loop_count; // unsigned short -> size_t, value not change
2317  }
2318  } else {
2319  std::cerr << "[" << m_profile.instance_name << "] ik_optional_weight_vector invalid length! Cannot be set. (input = [";
2320  for (size_t i = 0; i < jpe_v.size(); i++) {
2321  std::cerr << i_stp.ik_limb_parameters[i].ik_optional_weight_vector.length() << ", ";
2322  }
2323  std::cerr << "], desired = [";
2324  for (size_t i = 0; i < jpe_v.size(); i++) {
2325  std::cerr << jpe_v[i]->numJoints() << ", ";
2326  }
2327  std::cerr << "])" << std::endl;
2328  }
2329  }
2330  if (is_ik_limb_parameter_valid_length) {
2331  std::cerr << "[" << m_profile.instance_name << "] ik_optional_weight_vectors = ";
2332  for (size_t i = 0; i < jpe_v.size(); i++) {
2333  std::vector<double> ov;
2334  ov.resize(jpe_v[i]->numJoints());
2335  jpe_v[i]->getOptionalWeightVector(ov);
2336  std::cerr << "[";
2337  for (size_t j = 0; j < jpe_v[i]->numJoints(); j++) {
2338  std::cerr << ov[j] << " ";
2339  }
2340  std::cerr << "]";
2341  }
2342  std::cerr << std::endl;
2343  std::cerr << "[" << m_profile.instance_name << "] sr_gains = [";
2344  for (size_t i = 0; i < jpe_v.size(); i++) {
2345  std::cerr << jpe_v[i]->getSRGain() << ", ";
2346  }
2347  std::cerr << "]" << std::endl;
2348  std::cerr << "[" << m_profile.instance_name << "] avoid_gains = [";
2349  for (size_t i = 0; i < stikp.size(); i++) {
2350  std::cerr << stikp[i].avoid_gain << ", ";
2351  }
2352  std::cerr << "]" << std::endl;
2353  std::cerr << "[" << m_profile.instance_name << "] reference_gains = [";
2354  for (size_t i = 0; i < stikp.size(); i++) {
2355  std::cerr << stikp[i].reference_gain << ", ";
2356  }
2357  std::cerr << "]" << std::endl;
2358  std::cerr << "[" << m_profile.instance_name << "] manipulability_limits = [";
2359  for (size_t i = 0; i < jpe_v.size(); i++) {
2360  std::cerr << jpe_v[i]->getManipulabilityLimit() << ", ";
2361  }
2362  std::cerr << "]" << std::endl;
2363  std::cerr << "[" << m_profile.instance_name << "] ik_loop_count = [";
2364  for (size_t i = 0; i < stikp.size(); i++) {
2365  std::cerr << stikp[i].ik_loop_count << ", ";
2366  }
2367  std::cerr << "]" << std::endl;
2368  }
2369 }
2370 
2371 std::string Stabilizer::getStabilizerAlgorithmString (OpenHRP::StabilizerService::STAlgorithm _st_algorithm)
2372 {
2373  switch (_st_algorithm) {
2374  case OpenHRP::StabilizerService::TPCC:
2375  return "TPCC";
2376  case OpenHRP::StabilizerService::EEFM:
2377  return "EEFM";
2378  case OpenHRP::StabilizerService::EEFMQP:
2379  return "EEFMQP";
2380  case OpenHRP::StabilizerService::EEFMQPCOP:
2381  return "EEFMQPCOP";
2382  case OpenHRP::StabilizerService::EEFMQPCOP2:
2383  return "EEFMQPCOP2";
2384  default:
2385  return "";
2386  }
2387 };
2388 
2389 void Stabilizer::setBoolSequenceParam (std::vector<bool>& st_bool_values, const OpenHRP::StabilizerService::BoolSequence& output_bool_values, const std::string& prop_name)
2390 {
2391  std::vector<bool> prev_values;
2392  prev_values.resize(st_bool_values.size());
2393  copy (st_bool_values.begin(), st_bool_values.end(), prev_values.begin());
2394  if (st_bool_values.size() != output_bool_values.length()) {
2395  std::cerr << "[" << m_profile.instance_name << "] " << prop_name << " cannot be set. Length " << st_bool_values.size() << " != " << output_bool_values.length() << std::endl;
2396  } else if ( (control_mode != MODE_IDLE) ) {
2397  std::cerr << "[" << m_profile.instance_name << "] " << prop_name << " cannot be set. Current control_mode is " << control_mode << std::endl;
2398  } else {
2399  for (size_t i = 0; i < st_bool_values.size(); i++) {
2400  st_bool_values[i] = output_bool_values[i];
2401  }
2402  }
2403  std::cerr << "[" << m_profile.instance_name << "] " << prop_name << " is ";
2404  for (size_t i = 0; i < st_bool_values.size(); i++) {
2405  std::cerr <<"[" << st_bool_values[i] << "]";
2406  }
2407  std::cerr << "(set = ";
2408  for (size_t i = 0; i < output_bool_values.length(); i++) {
2409  std::cerr <<"[" << output_bool_values[i] << "]";
2410  }
2411  std::cerr << ", prev = ";
2412  for (size_t i = 0; i < prev_values.size(); i++) {
2413  std::cerr <<"[" << prev_values[i] << "]";
2414  }
2415  std::cerr << ")" << std::endl;
2416 };
2417 
2418 void Stabilizer::setBoolSequenceParamWithCheckContact (std::vector<bool>& st_bool_values, const OpenHRP::StabilizerService::BoolSequence& output_bool_values, const std::string& prop_name)
2419 {
2420  std::vector<bool> prev_values;
2421  prev_values.resize(st_bool_values.size());
2422  copy (st_bool_values.begin(), st_bool_values.end(), prev_values.begin());
2423  if (st_bool_values.size() != output_bool_values.length()) {
2424  std::cerr << "[" << m_profile.instance_name << "] " << prop_name << " cannot be set. Length " << st_bool_values.size() << " != " << output_bool_values.length() << std::endl;
2425  } else if ( control_mode == MODE_IDLE ) {
2426  for (size_t i = 0; i < st_bool_values.size(); i++) {
2427  st_bool_values[i] = output_bool_values[i];
2428  }
2429  } else {
2430  std::vector<size_t> failed_indices;
2431  for (size_t i = 0; i < st_bool_values.size(); i++) {
2432  if ( (st_bool_values[i] != output_bool_values[i]) ) { // If mode change
2433  if (!ref_contact_states[i] ) { // reference contact_states should be OFF
2434  st_bool_values[i] = output_bool_values[i];
2435  } else {
2436  failed_indices.push_back(i);
2437  }
2438  }
2439  }
2440  if (failed_indices.size() > 0) {
2441  std::cerr << "[" << m_profile.instance_name << "] " << prop_name << " cannot be set partially. failed_indices is [";
2442  for (size_t i = 0; i < failed_indices.size(); i++) {
2443  std::cerr << failed_indices[i] << " ";
2444  }
2445  std::cerr << "]" << std::endl;
2446  }
2447  }
2448  std::cerr << "[" << m_profile.instance_name << "] " << prop_name << " is ";
2449  for (size_t i = 0; i < st_bool_values.size(); i++) {
2450  std::cerr <<"[" << st_bool_values[i] << "]";
2451  }
2452  std::cerr << "(set = ";
2453  for (size_t i = 0; i < output_bool_values.length(); i++) {
2454  std::cerr <<"[" << output_bool_values[i] << "]";
2455  }
2456  std::cerr << ", prev = ";
2457  for (size_t i = 0; i < prev_values.size(); i++) {
2458  std::cerr <<"[" << prev_values[i] << "]";
2459  }
2460  std::cerr << ")" << std::endl;
2461 };
2462 
2464 {
2465  // Wait condition
2466  // 1. Check transition_count : Wait until transition is finished
2467  // 2. Check control_mode : Once control_mode is SYNC mode, wait until control_mode moves to the next mode (MODE_AIR or MODE_IDLE)
2469  while (transition_count != 0 ||
2470  (flag ? !(control_mode == MODE_IDLE || control_mode == MODE_AIR) : false) ) {
2471  usleep(10);
2473  }
2474  usleep(10);
2475 }
2476 
2477 double Stabilizer::vlimit(double value, double llimit_value, double ulimit_value)
2478 {
2479  if (value > ulimit_value) {
2480  return ulimit_value;
2481  } else if (value < llimit_value) {
2482  return llimit_value;
2483  }
2484  return value;
2485 }
2486 
2487 hrp::Vector3 Stabilizer::vlimit(const hrp::Vector3& value, double llimit_value, double ulimit_value)
2488 {
2489  hrp::Vector3 ret;
2490  for (size_t i = 0; i < 3; i++) {
2491  if (value(i) > ulimit_value) {
2492  ret(i) = ulimit_value;
2493  } else if (value(i) < llimit_value) {
2494  ret(i) = llimit_value;
2495  } else {
2496  ret(i) = value(i);
2497  }
2498  }
2499  return ret;
2500 }
2501 
2503 {
2504  hrp::Vector3 ret;
2505  for (size_t i = 0; i < 3; i++) {
2506  if (value(i) > limit_value(i)) {
2507  ret(i) = limit_value(i);
2508  } else if (value(i) < -1 * limit_value(i)) {
2509  ret(i) = -1 * limit_value(i);
2510  } else {
2511  ret(i) = value(i);
2512  }
2513  }
2514  return ret;
2515 }
2516 
2517 hrp::Vector3 Stabilizer::vlimit(const hrp::Vector3& value, const hrp::Vector3& llimit_value, const hrp::Vector3& ulimit_value)
2518 {
2519  hrp::Vector3 ret;
2520  for (size_t i = 0; i < 3; i++) {
2521  if (value(i) > ulimit_value(i)) {
2522  ret(i) = ulimit_value(i);
2523  } else if (value(i) < llimit_value(i)) {
2524  ret(i) = llimit_value(i);
2525  } else {
2526  ret(i) = value(i);
2527  }
2528  }
2529  return ret;
2530 }
2531 
2532 static double switching_inpact_absorber(double force, double lower_th, double upper_th)
2533 {
2534  double gradient, intercept;
2535  if (force < lower_th) {
2536  return 0;
2537  } else if (force > upper_th) {
2538  return 1;
2539  } else {
2540  gradient = 1.0 / (upper_th - lower_th);
2541  intercept = -lower_th * gradient;
2542  return gradient * force + intercept;
2543  }
2544 }
2545 
2547  if ( m_robot->numJoints() == m_qRef.data.length() ) {
2548  std::vector<std::string> target_name;
2549  target_name.push_back("L_ANKLE_R");
2550  target_name.push_back("R_ANKLE_R");
2551 
2552  double angvelx_ref;// = (m_rpyRef.data.r - pangx_ref)/dt;
2553  double angvely_ref;// = (m_rpyRef.data.p - pangy_ref)/dt;
2554  //pangx_ref = m_rpyRef.data.r;
2555  //pangy_ref = m_rpyRef.data.p;
2556  double angvelx = (m_rpy.data.r - pangx)/dt;
2557  double angvely = (m_rpy.data.r - pangy)/dt;
2558  pangx = m_rpy.data.r;
2559  pangy = m_rpy.data.p;
2560 
2561  // update internal robot model
2562  for ( int i = 0; i < m_robot->numJoints(); i++ ){
2563  qorg[i] = m_robot->joint(i)->q;
2564  m_robot->joint(i)->q = m_qRef.data[i];
2565  qrefv[i] = m_qRef.data[i];
2566  }
2567  //double orgjq = m_robot->link("L_FOOT")->joint->q;
2568  double orgjq = m_robot->joint(m_robot->link("L_ANKLE_P")->jointId)->q;
2569  //set root
2570  m_robot->rootLink()->p = hrp::Vector3(0,0,0);
2571  //m_robot->rootLink()->R = hrp::rotFromRpy(m_rpyRef.data.r,m_rpyRef.data.p,m_rpyRef.data.y);
2572  m_robot->calcForwardKinematics();
2573  hrp::Vector3 target_root_p = m_robot->rootLink()->p;
2574  hrp::Matrix33 target_root_R = m_robot->rootLink()->R;
2575  hrp::Vector3 target_foot_p[2];
2576  hrp::Matrix33 target_foot_R[2];
2577  for (size_t i = 0; i < 2; i++) {
2578  target_foot_p[i] = m_robot->link(target_name[i])->p;
2579  target_foot_R[i] = m_robot->link(target_name[i])->R;
2580  }
2581  hrp::Vector3 target_fm = (m_robot->link(target_name[0])->p + m_robot->link(target_name[1])->p)/2;
2582  //hrp::Vector3 org_cm = m_robot->rootLink()->R.transpose() * (m_robot->calcCM() - m_robot->rootLink()->p);
2583  hrp::Vector3 org_cm = m_robot->rootLink()->R.transpose() * (target_fm - m_robot->rootLink()->p);
2584 
2585  // stabilizer loop
2586  if ( ( m_wrenches[1].data.length() > 0 && m_wrenches[0].data.length() > 0 )
2587  //( m_wrenches[ST_LEFT].data[2] > m_robot->totalMass()/4 || m_wrenches[ST_RIGHT].data[2] > m_robot->totalMass()/4 )
2588  ) {
2589 
2590  for ( int i = 0; i < m_robot->numJoints(); i++ ) {
2591  m_robot->joint(i)->q = qorg[i];
2592  }
2593  // set root
2594  double rddx;// = k_run_b[0] * (m_rpyRef.data.r - m_rpy.data.r) + d_run_b[0] * (angvelx_ref - angvelx);
2595  double rddy;// = k_run_b[1] * (m_rpyRef.data.p - m_rpy.data.p) + d_run_b[1] * (angvely_ref - angvely);
2596  rdx += rddx * dt;
2597  rx += rdx * dt;
2598  rdy += rddy * dt;
2599  ry += rdy * dt;
2600  //rx += rddx * dt;
2601  //ry += rddy * dt;
2602  // if (DEBUGP2) {
2603  // std::cerr << "REFRPY " << m_rpyRef.data.r << " " << m_rpyRef.data.p << std::endl;
2604  // }
2605  // if (DEBUGP2) {
2606  // std::cerr << "RPY " << m_rpy.data.r << " " << m_rpy.data.p << std::endl;
2607  // std::cerr << " rx " << rx << " " << rdx << " " << rddx << std::endl;
2608  // std::cerr << " ry " << ry << " " << rdy << " " << rddy << std::endl;
2609  // }
2610  hrp::Vector3 root_p_s;
2611  hrp::Matrix33 root_R_s;
2612  rats::rotm3times(root_R_s, hrp::rotFromRpy(rx, ry, 0), target_root_R);
2613  if (DEBUGP2) {
2614  hrp::Vector3 tmp = hrp::rpyFromRot(root_R_s);
2615  std::cerr << "RPY2 " << tmp(0) << " " << tmp(1) << std::endl;
2616  }
2617  root_p_s = target_root_p + target_root_R * org_cm - root_R_s * org_cm;
2618  //m_robot->calcForwardKinematics();
2619  // FK
2620  m_robot->rootLink()->R = root_R_s;
2621  m_robot->rootLink()->p = root_p_s;
2622  if (DEBUGP2) {
2623  std::cerr << " rp " << root_p_s[0] << " " << root_p_s[1] << " " << root_p_s[2] << std::endl;
2624  }
2625  m_robot->calcForwardKinematics();
2626  //
2627  hrp::Vector3 current_fm = (m_robot->link(target_name[0])->p + m_robot->link(target_name[1])->p)/2;
2628 
2629  // 3D-LIP model contorller
2630  hrp::Vector3 dr = target_fm - current_fm;
2631  //hrp::Vector3 dr = current_fm - target_fm ;
2632  hrp::Vector3 dr_vel = (dr - pdr)/dt;
2633  pdr = dr;
2634  double tau_y = - m_torque_k[0] * dr(0) - m_torque_d[0] * dr_vel(0);
2635  double tau_x = m_torque_k[1] * dr(1) + m_torque_d[1] * dr_vel(1);
2636  if (DEBUGP2) {
2637  dr*=1e3;
2638  dr_vel*=1e3;
2639  std::cerr << "dr " << dr(0) << " " << dr(1) << " " << dr_vel(0) << " " << dr_vel(1) << std::endl;
2640  std::cerr << "tau_x " << tau_x << std::endl;
2641  std::cerr << "tau_y " << tau_y << std::endl;
2642  }
2643 
2644  double gamma = 0.5; // temp
2645  double tau_xl[2];
2646  double tau_yl[2];
2647  double xfront = 0.125;
2648  double xrear = 0.1;
2649  double yin = 0.02;
2650  double yout = 0.15;
2651  double mg = m_robot->totalMass() * 9.8 * 0.9;// margin
2652  double tq_y_ulimit = mg * xrear;
2653  double tq_y_llimit = -1 * mg * xfront;
2654  double tq_x_ulimit = mg * yout;
2655  double tq_x_llimit = mg * yin;
2656  // left
2657  tau_xl[0] = gamma * tau_x;
2658  tau_yl[0] = gamma * tau_y;
2659  tau_xl[0] = vlimit(tau_xl[0], tq_x_llimit, tq_x_ulimit);
2660  tau_yl[0] = vlimit(tau_yl[0], tq_y_llimit, tq_y_ulimit);
2661  // right
2662  tau_xl[1]= (1- gamma) * tau_x;
2663  tau_yl[1]= (1- gamma) * tau_y;
2664  tau_xl[1] = vlimit(tau_xl[1], -1*tq_x_ulimit, -1*tq_x_llimit);
2665  tau_yl[1] = vlimit(tau_yl[1], tq_y_llimit, tq_y_ulimit);
2666 
2667  double dleg_x[2];
2668  double dleg_y[2];
2669  double tau_y_total = (m_wrenches[1].data[4] + m_wrenches[0].data[4]) / 2;
2670  double dpz;
2671  if (DEBUGP2) {
2672  std::cerr << "tq limit " << tq_x_ulimit << " " << tq_x_llimit << " " << tq_y_ulimit << " " << tq_y_llimit << std::endl;
2673  }
2674  for (size_t i = 0; i < 2; i++) {
2675  // dleg_x[i] = m_tau_x[i].update(m_wrenches[i].data[3], tau_xl[i]);
2676  // dleg_y[i] = m_tau_y[i].update(m_wrenches[i].data[4], tau_yl[i]);
2677  //dleg_x[i] = m_tau_x[i].update(m_wrenches[i].data[3], tau_xl[i]);
2678  dleg_x[i] = m_tau_x[i].update(0,0);
2679  dleg_y[i] = m_tau_y[i].update(tau_y_total, tau_yl[i]);
2680  if (DEBUGP2) {
2681  std::cerr << i << " dleg_x " << dleg_x[i] << std::endl;
2682  std::cerr << i << " dleg_y " << dleg_y[i] << std::endl;
2683  std::cerr << i << " t_x " << m_wrenches[i].data[3] << " "<< tau_xl[i] << std::endl;
2684  std::cerr << i << " t_y " << m_wrenches[i].data[4] << " "<< tau_yl[i] << std::endl;
2685  }
2686  }
2687 
2688  // calc leg rot
2689  hrp::Matrix33 target_R[2];
2690  hrp::Vector3 target_p[2];
2691  for (size_t i = 0; i < 2; i++) {
2692  //rats::rotm3times(target_R[i], hrp::rotFromRpy(dleg_x[i], dleg_y[i], 0), target_foot_R[i]);
2693  rats::rotm3times(target_R[i], hrp::rotFromRpy(0, dleg_y[i], 0), target_foot_R[i]);
2694  //target_p[i] = target_foot_p[i] + target_foot_R[i] * org_cm - target_R[i] * org_cm;
2695  //target_p[i] = target_foot_p[i] + target_foot_R[i] * org_cm - target_R[i] * org_cm;
2696  target_p[i] = target_foot_p[i];
2697  }
2698  // 1=>left, 2=>right
2699  double refdfz = 0;
2700  dpz = m_f_z.update((m_wrenches[0].data[2] - m_wrenches[1].data[2]), refdfz);
2701  //target_p[0](2) = target_foot_p[0](2) + dpz/2;
2702  //target_p[1](2) = target_foot_p[1](2) - dpz/2;
2703  target_p[0](2) = target_foot_p[0](2);
2704  target_p[1](2) = target_foot_p[1](2);
2705 
2706  // IK
2707  for (size_t i = 0; i < 2; i++) {
2708  hrp::Link* target = m_robot->link(target_name[i]);
2709  hrp::Vector3 vel_p, vel_r;
2710  vel_p = target_p[i] - target->p;
2711  rats::difference_rotation(vel_r, target->R, target_R[i]);
2712  //jpe_v[i]->solveLimbIK(vel_p, vel_r, transition_count, 0.001, 0.01, MAX_TRANSITION_COUNT, qrefv, DEBUGP);
2713  //jpe_v[i]->solveLimbIK(vel_p, vel_r, transition_count, 0.001, 0.01, MAX_TRANSITION_COUNT, qrefv, false);
2714  //m_robot->joint(m_robot->link(target_name[i])->jointId)->q = dleg_y[i] + orgjq;
2715  }
2716  // m_robot->joint(m_robot->link("L_ANKLE_P")->jointId)->q = transition_smooth_gain * dleg_y[0] + orgjq + m_rpy.data.p;
2717  // m_robot->joint(m_robot->link("R_ANKLE_P")->jointId)->q = transition_smooth_gain * dleg_y[1] + orgjq + m_rpy.data.p;
2718  m_robot->joint(m_robot->link("L_ANKLE_P")->jointId)->q = transition_smooth_gain * dleg_y[0] + orgjq;
2719  m_robot->joint(m_robot->link("R_ANKLE_P")->jointId)->q = transition_smooth_gain * dleg_y[1] + orgjq;
2720  } else {
2721  // reinitialize
2722  for (int i = 0; i < 2; i++) {
2723  m_tau_x[i].reset();
2724  m_tau_y[i].reset();
2725  m_f_z.reset();
2726  }
2727  }
2728  }
2729 }
2730 
2731 void Stabilizer::calcContactMatrix (hrp::dmatrix& tm, const std::vector<hrp::Vector3>& contact_p)
2732 {
2733  // tm.resize(6,6*contact_p.size());
2734  // tm.setZero();
2735  // for (size_t c = 0; c < contact_p.size(); c++) {
2736  // for (size_t i = 0; i < 6; i++) tm(i,(c*6)+i) = 1.0;
2737  // hrp::Matrix33 cm;
2738  // rats::outer_product_matrix(cm, contact_p[c]);
2739  // for (size_t i = 0; i < 3; i++)
2740  // for (size_t j = 0; j < 3; j++) tm(i+3,(c*6)+j) = cm(i,j);
2741  // }
2742 }
2743 
2745 {
2746  m_robot->calcForwardKinematics();
2747  // buffers for the unit vector method
2748  hrp::Vector3 root_w_x_v;
2749  hrp::Vector3 g(0, 0, 9.80665);
2750  root_w_x_v = m_robot->rootLink()->w.cross(m_robot->rootLink()->vo + m_robot->rootLink()->w.cross(m_robot->rootLink()->p));
2751  m_robot->rootLink()->dvo = g - root_w_x_v; // dv = g, dw = 0
2752  m_robot->rootLink()->dw.setZero();
2753 
2754  hrp::Vector3 root_f;
2755  hrp::Vector3 root_t;
2756  m_robot->calcInverseDynamics(m_robot->rootLink(), root_f, root_t);
2757  // if (loop % 200 == 0) {
2758  // std::cerr << ":mass " << m_robot->totalMass() << std::endl;
2759  // std::cerr << ":cog "; rats::print_vector(std::cerr, m_robot->calcCM());
2760  // for(int i = 0; i < m_robot->numJoints(); ++i){
2761  // std::cerr << "(list :" << m_robot->link(i)->name << " "
2762  // << m_robot->joint(i)->jointId << " "
2763  // << m_robot->link(i)->m << " ";
2764  // hrp::Vector3 tmpc = m_robot->link(i)->p + m_robot->link(i)->R * m_robot->link(i)->c;
2765  // rats::print_vector(std::cerr, tmpc, false);
2766  // std::cerr << " ";
2767  // rats::print_vector(std::cerr, m_robot->link(i)->c, false);
2768  // std::cerr << ")" << std::endl;
2769  // }
2770  // }
2771  // if (loop % 200 == 0) {
2772  // std::cerr << ":IV1 (list ";
2773  // for(int i = 0; i < m_robot->numJoints(); ++i){
2774  // std::cerr << "(list :" << m_robot->joint(i)->name << " " << m_robot->joint(i)->u << ")";
2775  // }
2776  // std::cerr << ")" << std::endl;
2777  // }
2778  hrp::dmatrix contact_mat, contact_mat_inv;
2779  std::vector<hrp::Vector3> contact_p;
2780  for (size_t j = 0; j < 2; j++) contact_p.push_back(m_robot->sensor<hrp::ForceSensor>(stikp[j].sensor_name)->link->p);
2781  calcContactMatrix(contact_mat, contact_p);
2782  hrp::calcSRInverse(contact_mat, contact_mat_inv, 0.0);
2783  hrp::dvector root_ft(6);
2784  for (size_t j = 0; j < 3; j++) root_ft(j) = root_f(j);
2785  for (size_t j = 0; j < 3; j++) root_ft(j+3) = root_t(j);
2786  hrp::dvector contact_ft(2*6);
2787  contact_ft = contact_mat_inv * root_ft;
2788  // if (loop%200==0) {
2789  // std::cerr << ":mass " << m_robot->totalMass() << std::endl;
2790  // // std::cerr << ":ftv "; rats::print_vector(std::cerr, ftv);
2791  // // std::cerr << ":aa "; rats::print_matrix(std::cerr, aa);
2792  // // std::cerr << ":dv "; rats::print_vector(std::cerr, dv);
2793  // }
2794  for (size_t j = 0; j < 2; j++) {
2795  hrp::JointPathEx jm = hrp::JointPathEx(m_robot, m_robot->rootLink(), m_robot->sensor<hrp::ForceSensor>(stikp[j].sensor_name)->link, dt);
2796  hrp::dmatrix JJ;
2797  jm.calcJacobian(JJ);
2798  hrp::dvector ft(6);
2799  for (size_t i = 0; i < 6; i++) ft(i) = contact_ft(i+j*6);
2800  hrp::dvector tq_from_extft(jm.numJoints());
2801  tq_from_extft = JJ.transpose() * ft;
2802  // if (loop%200==0) {
2803  // std::cerr << ":ft "; rats::print_vector(std::cerr, ft);
2804  // std::cerr << ":JJ "; rats::print_matrix(std::cerr, JJ);
2805  // std::cerr << ":tq_from_extft "; rats::print_vector(std::cerr, tq_from_extft);
2806  // }
2807  for (size_t i = 0; i < jm.numJoints(); i++) jm.joint(i)->u -= tq_from_extft(i);
2808  }
2809  //hrp::dmatrix MM(6,m_robot->numJoints());
2810  //m_robot->calcMassMatrix(MM);
2811  // if (loop % 200 == 0) {
2812  // std::cerr << ":INVDYN2 (list "; rats::print_vector(std::cerr, root_f, false);
2813  // std::cerr << " "; rats::print_vector(std::cerr, root_t, false);
2814  // std::cerr << ")" << std::endl;
2815  // // hrp::dvector tqv(m_robot->numJoints());
2816  // // for(int i = 0; i < m_robot->numJoints(); ++i){p
2817  // // tqv[m_robot->joint(i)->jointId] = m_robot->joint(i)->u;
2818  // // }
2819  // // std::cerr << ":IV2 "; rats::print_vector(std::cerr, tqv);
2820  // std::cerr << ":IV2 (list ";
2821  // for(int i = 0; i < m_robot->numJoints(); ++i){
2822  // std::cerr << "(list :" << m_robot->joint(i)->name << " " << m_robot->joint(i)->u << ")";
2823  // }
2824  // std::cerr << ")" << std::endl;
2825  // }
2826 };
2827 
2828 extern "C"
2829 {
2830 
2832  {
2834  manager->registerFactory(profile,
2835  RTC::Create<Stabilizer>,
2836  RTC::Delete<Stabilizer>);
2837  }
2838 
2839 };
2840 
2841 
ComponentProfile m_profile
hrp::BodyPtr m_robot
Definition: Stabilizer.h:285
RTC::TimedPoint3D m_originActCog
Definition: Stabilizer.h:184
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
Definition: Stabilizer.cpp:528
double get_alpha_cutoff_freq()
png_infop png_charpp int png_charpp profile
hrp::Vector3 ref_zmp
Definition: Stabilizer.h:306
double k_brot_tc[2]
Definition: Stabilizer.h:316
RTC::TimedPoint3D m_originRefZmp
Definition: Stabilizer.h:183
hrp::Vector3 eefm_swing_rot_time_const
Definition: Stabilizer.h:268
hrp::Vector3 eefm_pos_damping_gain
Definition: Stabilizer.h:268
void getCurrentParameters()
Definition: Stabilizer.cpp:759
RTC::OutPort< RTC::TimedPoint3D > m_originActZmpOut
Definition: Stabilizer.h:229
TwoDofController m_f_z
Definition: Stabilizer.h:318
hrp::Vector3 rel_act_zmp
Definition: Stabilizer.h:307
RTC::OutPort< RTC::TimedPoint3D > m_actCPOut
Definition: Stabilizer.h:221
void calc_convex_hull(const std::vector< std::vector< Eigen::Vector2d > > &vs, const std::vector< bool > &cs, const std::vector< hrp::Vector3 > &ee_pos, const std::vector< hrp::Matrix33 > &ee_rot)
#define max(a, b)
std::string getStabilizerAlgorithmString(OpenHRP::StabilizerService::STAlgorithm _st_algorithm)
RTC::TimedDoubleSeq m_tau
Definition: Stabilizer.h:162
hrp::Vector3 act_cog
Definition: Stabilizer.h:307
RTC::TimedPoint3D m_originNewZmp
Definition: Stabilizer.h:183
hrp::Vector3 ref_cogvel
Definition: Stabilizer.h:306
RTC::InPort< RTC::TimedDoubleSeq > m_controlSwingSupportTimeIn
Definition: Stabilizer.h:202
static const char * stabilizer_spec[]
Definition: Stabilizer.cpp:30
std::vector< hrp::Vector3 > rel_ee_pos
Definition: Stabilizer.h:302
void calcSwingSupportLimbGain()
double eefm_body_attitude_control_gain[2]
Definition: Stabilizer.h:325
std::string sensor_name
Definition: Stabilizer.h:261
hrp::Vector3 d_foot_rpy
Definition: Stabilizer.h:267
std::vector< RTC::InPort< RTC::TimedPoint3D > * > m_limbCOPOffsetIn
Definition: Stabilizer.h:203
Eigen::MatrixXd dmatrix
RTC::OutPort< RTC::TimedDoubleSeq > m_allRefWrenchOut
Definition: Stabilizer.h:233
hrp::Vector3 eefm_rot_time_const
Definition: Stabilizer.h:268
std::string ee_name
Definition: Stabilizer.h:260
double eefm_gravitational_acceleration
Definition: Stabilizer.h:326
hrp::Matrix33 prev_act_foot_origin_rot
Definition: Stabilizer.h:301
OpenHRP::StabilizerService::STAlgorithm st_algorithm
Definition: Stabilizer.h:312
hrp::Vector3 ref_moment
Definition: Stabilizer.h:270
double pangy_ref
Definition: Stabilizer.h:321
void difference_rotation(hrp::Vector3 &ret_dif_rot, const hrp::Matrix33 &self_rot, const hrp::Matrix33 &target_rot)
Definition: RatsMatrix.cpp:40
hrp::Vector3 localp
Definition: Stabilizer.h:263
double eefm_k1[2]
Definition: Stabilizer.h:325
std::string name
bool stringTo(To &val, const char *str)
unsigned int m_debugLevel
Definition: Stabilizer.h:287
hrp::Matrix33 ref_foot_origin_rot
Definition: Stabilizer.h:301
hrp::Matrix33 rot
Definition: RatsMatrix.h:20
double eefm_body_attitude_control_time_const[2]
Definition: Stabilizer.h:325
RTC::OutPort< RTC::TimedOrientation3D > m_currentBaseRpyOut
Definition: Stabilizer.h:232
std::vector< std::vector< Eigen::Vector2d > > support_polygon_vetices
Definition: Stabilizer.h:314
RTC::OutPort< RTC::TimedLong > m_emergencySignalOut
Definition: Stabilizer.h:226
std::vector< std::vector< Eigen::Vector2d > > margined_support_polygon_vetices
Definition: Stabilizer.h:314
void calcTorque()
StabilizerService_impl m_service0
Definition: Stabilizer.h:252
int is_air_counter
Definition: Stabilizer.h:297
RTC::OutPort< RTC::TimedDoubleSeq > m_allEECompOut
Definition: Stabilizer.h:234
hrp::Vector3 eefm_swing_pos_spring_gain
Definition: Stabilizer.h:268
RTC::TimedPoint3D m_originActZmp
Definition: Stabilizer.h:184
RTC::TimedOrientation3D m_currentBaseRpy
Definition: Stabilizer.h:187
RTC::OutPort< RTC::TimedPoint3D > m_refCPOut
Definition: Stabilizer.h:220
double total_mass
Definition: Stabilizer.h:334
double limb_stretch_avoidance_vlimit[2]
Definition: Stabilizer.h:310
RTC::InPort< RTC::TimedDoubleSeq > m_qRefIn
Definition: Stabilizer.h:195
RTC::InPort< RTC::TimedPoint3D > m_sbpCogOffsetIn
Definition: Stabilizer.h:206
static double switching_inpact_absorber(double force, double lower_th, double upper_th)
bool is_inside_foot(const hrp::Vector3 &leg_pos, const bool is_lleg, const double margin=0.0)
std::vector< hrp::Vector3 > ref_moment
Definition: Stabilizer.h:302
double eefm_k3[2]
Definition: Stabilizer.h:325
static std::ostream & operator<<(std::ostream &os, const struct RTC::Time &tm)
Definition: Stabilizer.cpp:48
png_uint_32 size
void set_wrench_alpha_blending(const double a)
bool is_front_of_foot(const hrp::Vector3 &leg_pos, const double margin=0.0)
double pangx_ref
Definition: Stabilizer.h:321
std::vector< RTC::TimedDoubleSeq > m_ref_wrenches
Definition: Stabilizer.h:210
void calcJacobian(dmatrix &out_J, const Vector3 &local_p=Vector3::Zero()) const
RTC::CorbaPort m_StabilizerServicePort
Definition: Stabilizer.h:246
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
Definition: Stabilizer.cpp:548
std::vector< RTC::InPort< RTC::TimedDoubleSeq > * > m_wrenchesIn
Definition: Stabilizer.h:209
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
std::vector< hrp::Vector3 > act_ee_p
Definition: Stabilizer.h:302
hrp::Vector3 eefm_swing_rot_damping_gain
Definition: Stabilizer.h:333
hrp::Vector3 prev_d_rpy_swing
Definition: Stabilizer.h:275
double root_rot_compensation_limit[2]
Definition: Stabilizer.h:310
std::vector< double > eefm_swing_damping_force_thre
Definition: Stabilizer.h:327
hrp::Vector3 d_rpy_swing
Definition: Stabilizer.h:275
void calcDiffFootOriginExtMoment()
vstring split(const std::string &input, const std::string &delimiter, bool ignore_empty)
double ry
Definition: Stabilizer.h:323
std::vector< bool > is_ik_enable
Definition: Stabilizer.h:291
std::vector< RTC::TimedPoint3D > m_limbCOPOffset
Definition: Stabilizer.h:175
std::vector< hrp::Vector3 > target_ee_p
Definition: Stabilizer.h:302
std::vector< std::string > rel_ee_name
Definition: Stabilizer.h:304
bool eefm_use_force_difference_control
Definition: Stabilizer.h:298
void StabilizerInit(RTC::Manager *manager)
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
Definition: Stabilizer.cpp:534
bool is_legged_robot
Definition: Stabilizer.h:298
bool is_rear_of_foot(const hrp::Vector3 &leg_pos, const double margin=0.0)
CORBA::ORB_ptr getORB()
hrp::Vector3 rel_ref_cp
Definition: Stabilizer.h:306
Stabilizer(RTC::Manager *manager)
Constructor.
Definition: Stabilizer.cpp:61
TwoDofController m_tau_x[2]
Definition: Stabilizer.h:318
RTC::OutPort< RTC::TimedBooleanSeq > m_actContactStatesOut
Definition: Stabilizer.h:224
hrp::Vector3 eefm_swing_pos_damping_gain
Definition: Stabilizer.h:333
#define for
png_uint_32 i
bool is_seq_interpolating
Definition: Stabilizer.h:298
double m_torque_d[2]
Definition: Stabilizer.h:320
std::vector< STIKParam > stikp
Definition: Stabilizer.h:289
Eigen::VectorXd dvector
std::vector< hrp::Vector3 > act_force
Definition: Stabilizer.h:302
double transition_time
Definition: Stabilizer.h:334
coil::Properties & getProperties()
RTC::InPort< RTC::TimedPoint3D > m_zmpRefIn
Definition: Stabilizer.h:197
void calcContactMatrix(hrp::dmatrix &tm, const std::vector< hrp::Vector3 > &contact_p)
std::vector< bool > act_contact_states
Definition: Stabilizer.h:291
void calcEEForceMomentControl()
std::vector< double > eefm_swing_damping_moment_thre
Definition: Stabilizer.h:327
static Manager & instance()
Matrix33 localR
hrp::Vector3 new_refzmp
Definition: Stabilizer.h:328
hrp::Vector3 act_zmp
Definition: Stabilizer.h:307
std::vector< RTC::TimedDoubleSeq > m_wrenches
Definition: Stabilizer.h:208
RTC::TimedPoint3D m_sbpCogOffset
Definition: Stabilizer.h:181
hrp::Vector3 act_total_foot_origin_moment
Definition: Stabilizer.h:332
bool addOutPort(const char *name, OutPortBase &outport)
hrp::Vector3 act_cogvel
Definition: Stabilizer.h:307
double eefm_zmp_delay_time_const[2]
Definition: Stabilizer.h:325
std::vector< hrp::Matrix33 > rel_ee_rot
Definition: Stabilizer.h:303
hrp::Vector3 target_ee_diff_p
Definition: Stabilizer.h:275
std::vector< RTC::InPort< RTC::TimedDoubleSeq > * > m_ref_wrenchesIn
Definition: Stabilizer.h:211
bool eefm_use_swing_damping
Definition: Stabilizer.h:298
boost::shared_ptr< Body > BodyPtr
std::vector< bool > is_zmp_calc_enable
Definition: Stabilizer.h:291
std::vector< bool > is_feedback_control_enable
Definition: Stabilizer.h:291
RTC::TimedDoubleSeq m_controlSwingSupportTime
Definition: Stabilizer.h:174
RTC::TimedDoubleSeq m_debugData
Definition: Stabilizer.h:190
RTC::TimedPoint3D m_refCP
Definition: Stabilizer.h:166
std::vector< int > m_will_fall_counter
Definition: Stabilizer.h:296
void distributeZMPToForceMomentsQP(std::vector< hrp::Vector3 > &ref_foot_force, std::vector< hrp::Vector3 > &ref_foot_moment, const std::vector< hrp::Vector3 > &ee_pos, const std::vector< hrp::Vector3 > &cop_pos, const std::vector< hrp::Matrix33 > &ee_rot, const std::vector< std::string > &ee_name, const std::vector< double > &limb_gains, const std::vector< double > &toeheel_ratio, const hrp::Vector3 &new_refzmp, const hrp::Vector3 &ref_zmp, const double total_fz, const double dt, const bool printp=true, const std::string &print_str="", const bool use_cop_distribution=false)
double eefm_pos_time_const_swing
Definition: Stabilizer.h:326
Eigen::Vector3d Vector3
RTC::InPort< RTC::TimedDoubleSeq > m_qCurrentIn
Definition: Stabilizer.h:194
RTC::TimedDoubleSeq m_allEEComp
Definition: Stabilizer.h:189
RTC::OutPort< RTC::TimedDoubleSeq > m_debugDataOut
Definition: Stabilizer.h:235
hrp::dvector qrefv
Definition: Stabilizer.h:288
void getTargetParameters()
Matrix33 rotFromRpy(const Vector3 &rpy)
hrp::Matrix33 target_ee_diff_r
Definition: Stabilizer.h:276
double pangy
Definition: Stabilizer.h:321
bool isContact(const size_t idx)
Definition: Stabilizer.h:145
Eigen::Matrix3d Matrix33
#define min(a, b)
RTC::OutPort< RTC::TimedPoint3D > m_originActCogOut
Definition: Stabilizer.h:229
std::vector< std::string > vstring
double k_tpcc_p[2]
Definition: Stabilizer.h:316
hrp::Vector3 rel_cog
Definition: Stabilizer.h:328
coil::Mutex m_mutex
Definition: Stabilizer.h:286
void getActualParameters()
Definition: Stabilizer.cpp:799
coil::Properties & getConfig()
double m_torque_k[2]
Definition: Stabilizer.h:320
OpenHRP::StabilizerService::EmergencyCheckMode emergency_check_mode
Definition: Stabilizer.h:336
hrp::Matrix33 current_root_R
Definition: Stabilizer.h:301
double k_brot_p[2]
Definition: Stabilizer.h:316
RTC::TimedPoint3D m_originRefCog
Definition: Stabilizer.h:183
hrp::dvector transition_joint_q
Definition: Stabilizer.h:288
std::string target_name
Definition: Stabilizer.h:259
hrp::Vector3 ref_total_force
Definition: Stabilizer.h:330
void rotm3times(hrp::Matrix33 &m12, const hrp::Matrix33 &m1, const hrp::Matrix33 &m2)
Definition: RatsMatrix.cpp:31
RTC::TimedPoint3D m_zmpRef
Definition: Stabilizer.h:164
void set_leg_inside_margin(const double a)
ExecutionContextHandle_t UniqueId
void moveBasePosRotForBodyRPYControl()
std::string parent_name
Definition: Stabilizer.h:262
void startStabilizer(void)
void getParameter(OpenHRP::StabilizerService::stParam &i_stp)
bool use_zmp_truncation
Definition: Stabilizer.h:298
std::vector< bool > ref_contact_states
Definition: Stabilizer.h:291
void set_leg_front_margin(const double a)
void calcStateForEmergencySignal()
#define deg2rad(x)
Definition: Stabilizer.cpp:22
virtual ~Stabilizer()
Destructor.
Definition: Stabilizer.cpp:111
bool bindParameter(const char *param_name, VarType &var, const char *def_val, bool(*trans)(VarType &, const char *)=coil::stringTo)
coil::Guard< coil::Mutex > Guard
Definition: Stabilizer.cpp:19
RTC::OutPort< RTC::TimedDoubleSeq > m_qRefOut
Definition: Stabilizer.h:217
int calcMaxTransitionCount()
Definition: Stabilizer.h:149
RTC::OutPort< RTC::TimedDoubleSeq > m_COPInfoOut
Definition: Stabilizer.h:225
void setBoolSequenceParam(std::vector< bool > &st_bool_values, const OpenHRP::StabilizerService::BoolSequence &output_bool_values, const std::string &prop_name)
Vector3 localPos
hrp::Vector3 pos_ctrl
Definition: Stabilizer.h:329
int calcSRInverse(const dmatrix &_a, dmatrix &_a_sr, double _sr_ratio, dmatrix _w)
Definition: JointPathEx.cpp:42
double update(double _x, double _xd)
RTC::TimedPoint3D m_basePos
Definition: Stabilizer.h:170
std::vector< double > cp_check_margin
Definition: Stabilizer.h:335
hrp::Vector3 prev_act_cog
Definition: Stabilizer.h:307
std::vector< double > tilt_margin
Definition: Stabilizer.h:335
bool is_emergency
Definition: Stabilizer.h:298
#define rad2deg(rad)
Definition: Stabilizer.cpp:25
hrp::Vector3 d_foot_pos
Definition: Stabilizer.h:267
double eefm_pos_margin_time
Definition: Stabilizer.h:326
void calcFootOriginCoords(hrp::Vector3 &foot_origin_pos, hrp::Matrix33 &foot_origin_rot)
Definition: Stabilizer.cpp:768
double get_leg_outside_margin()
RTC::OutPort< RTC::TimedPoint3D > m_originNewZmpOut
Definition: Stabilizer.h:228
RTC::OutPort< RTC::TimedPoint3D > m_diffFootOriginExtMomentOut
Definition: Stabilizer.h:223
boost::shared_ptr< FirstOrderLowPassFilter< hrp::Vector3 > > target_ee_diff_p_filter
Definition: Stabilizer.h:274
int loadBodyFromModelLoader(::World *world, const char *name, const char *url, CosNaming::NamingContext_var cxt)
hrp::Vector3 ref_total_moment
Definition: Stabilizer.h:330
bool is_walking
Definition: Stabilizer.h:299
hrp::Vector3 eefm_rot_damping_gain
Definition: Stabilizer.h:268
int detection_count_to_air
Definition: Stabilizer.h:297
RTC::OutPort< RTC::TimedPoint3D > m_originRefCogOut
Definition: Stabilizer.h:228
hrp::Vector3 d_pos_swing
Definition: Stabilizer.h:275
int transition_count
Definition: Stabilizer.h:294
bool initial_cp_too_large_error
Definition: Stabilizer.h:298
boost::shared_ptr< FirstOrderLowPassFilter< hrp::Vector3 > > target_ee_diff_r_filter
Definition: Stabilizer.h:274
void set_vertices_from_margin_params()
RTC::InPort< RTC::TimedPoint3D > m_basePosIn
Definition: Stabilizer.h:198
double k_tpcc_x[2]
Definition: Stabilizer.h:316
void setErrorPrefix(const std::string &_error_prefix)
enum Stabilizer::cmode control_mode
hrp::Vector3 eefm_swing_rot_spring_gain
Definition: Stabilizer.h:268
RTC::TimedBooleanSeq m_contactStates
Definition: Stabilizer.h:172
hrp::Vector3 cp_offset
Definition: Stabilizer.h:307
std::vector< double > toeheel_ratio
Definition: Stabilizer.h:292
std::map< std::string, size_t > contact_states_index_map
Definition: Stabilizer.h:290
hrp::Vector3 prev_ref_cog
Definition: Stabilizer.h:306
hrp::Vector3 eefm_pos_time_const_support
Definition: Stabilizer.h:268
double get_wrench_alpha_blending()
boost::shared_ptr< FirstOrderLowPassFilter< hrp::Vector3 > > act_cogvel_filter
Definition: Stabilizer.h:311
hrp::Matrix33 target_root_R
Definition: Stabilizer.h:301
hrp::Matrix33 localR
Definition: Stabilizer.h:265
RTC::TimedOrientation3D m_actBaseRpy
Definition: Stabilizer.h:185
hrp::Vector3 target_root_p
Definition: Stabilizer.h:300
hrp::Vector3 foot_origin_offset[2]
Definition: Stabilizer.h:308
std::vector< double > prev_act_force_z
Definition: Stabilizer.h:309
double eefm_pos_transition_time
Definition: Stabilizer.h:326
RTC::TimedOrientation3D m_baseRpy
Definition: Stabilizer.h:171
prop
SimpleZMPDistributor * szd
Definition: Stabilizer.h:313
hrp::Vector3 current_base_pos
Definition: Stabilizer.h:307
void setParameter(const OpenHRP::StabilizerService::stParam &i_stp)
RTC::TimedDoubleSeq m_toeheelRatio
Definition: Stabilizer.h:173
void distributeZMPToForceMomentsPseudoInverse2(std::vector< hrp::Vector3 > &ref_foot_force, std::vector< hrp::Vector3 > &ref_foot_moment, const std::vector< hrp::Vector3 > &ee_pos, const std::vector< hrp::Vector3 > &cop_pos, const std::vector< hrp::Matrix33 > &ee_rot, const std::vector< std::string > &ee_name, const std::vector< double > &limb_gains, const std::vector< double > &toeheel_ratio, const hrp::Vector3 &new_refzmp, const hrp::Vector3 &ref_zmp, const hrp::Vector3 &total_force, const hrp::Vector3 &total_moment, const std::vector< hrp::dvector6 > &ee_forcemoment_distribution_weight, const double total_fz, const double dt, const bool printp=true, const std::string &print_str="")
std::vector< hrp::Vector3 > projected_normal
Definition: Stabilizer.h:302
RTC::InPort< RTC::TimedBoolean > m_walkingStatesIn
Definition: Stabilizer.h:205
RTC::OutPort< RTC::TimedPoint3D > m_currentBasePosOut
Definition: Stabilizer.h:231
double rdy
Definition: Stabilizer.h:323
hrp::Vector3 act_base_rpy
Definition: Stabilizer.h:307
double limb_stretch_avoidance_time_const
Definition: Stabilizer.h:310
double eefm_rot_compensation_limit
Definition: Stabilizer.h:269
HRP_UTIL_EXPORT Vector3 rpyFromRot(const Matrix33 &m)
hrp::Vector3 localCOPPos
Definition: Stabilizer.h:264
bool calcZMP(hrp::Vector3 &ret_zmp, const double zmp_z)
unsigned int numJoints() const
void stabilizer(Stabilizer *i_stabilizer)
hrp::Vector3 act_cp
Definition: Stabilizer.h:307
void readVirtualForceSensorParamFromProperties(std::map< std::string, hrp::VirtualForceSensorParam > &vfs, hrp::BodyPtr m_robot, const std::string &prop_string, const std::string &instance_name)
naming
void get_margined_vertices(std::vector< std::vector< Eigen::Vector2d > > &vs)
Vector3 cross(const Vector3 &v1, const Vector3 &v2)
#define M_PI
std::vector< bool > prev_ref_contact_states
Definition: Stabilizer.h:291
void distributeZMPToForceMoments(std::vector< hrp::Vector3 > &ref_foot_force, std::vector< hrp::Vector3 > &ref_foot_moment, const std::vector< hrp::Vector3 > &ee_pos, const std::vector< hrp::Vector3 > &cop_pos, const std::vector< hrp::Matrix33 > &ee_rot, const std::vector< std::string > &ee_name, const std::vector< double > &limb_gains, const std::vector< double > &toeheel_ratio, const hrp::Vector3 &new_refzmp, const hrp::Vector3 &ref_zmp, const double total_fz, const double dt, const bool printp=true, const std::string &print_str="")
virtual RTC::ReturnCode_t onFinalize()
Definition: Stabilizer.cpp:505
double vlimit(double value, double llimit_value, double ulimit_value)
void print_params(const std::string &str)
double rx
Definition: Stabilizer.h:323
void setBoolSequenceParamWithCheckContact(std::vector< bool > &st_bool_values, const OpenHRP::StabilizerService::BoolSequence &output_bool_values, const std::string &prop_name)
void calcRUNST()
double eefm_k2[2]
Definition: Stabilizer.h:325
bool is_inside_support_polygon(Eigen::Vector2d &p, const hrp::Vector3 &offset=hrp::Vector3::Zero(), const bool &truncate_p=false, const std::string &str="")
RTC::TimedPoint3D m_actCP
Definition: Stabilizer.h:167
void set_leg_outside_margin(const double a)
hrp::Vector3 ref_cog
Definition: Stabilizer.h:306
double calcDampingControl(const double tau_d, const double tau, const double prev_d, const double DD, const double TT)
RTC::TimedPoint3D m_originActCogVel
Definition: Stabilizer.h:184
double pangx
Definition: Stabilizer.h:321
void registerInPort(const char *name, InPortBase &inport)
RTC::InPort< RTC::TimedOrientation3D > m_baseRpyIn
Definition: Stabilizer.h:199
virtual bool isNew()
virtual RTC::ReturnCode_t onInitialize()
Definition: Stabilizer.cpp:115
void readInterlockingJointsParamFromProperties(std::vector< std::pair< Link *, Link *> > &pairs, hrp::BodyPtr m_robot, const std::string &prop_string, const std::string &instance_name)
hrp::Matrix33 prev_ref_foot_origin_rot
Definition: Stabilizer.h:301
RTC::TimedDoubleSeq m_qRef
Definition: Stabilizer.h:161
hrp::Vector3 pdr
Definition: Stabilizer.h:319
void get_vertices(std::vector< std::vector< Eigen::Vector2d > > &vs)
RTC::InPort< RTC::TimedDoubleSeq > m_toeheelRatioIn
Definition: Stabilizer.h:201
RTC::TimedDoubleSeq m_allRefWrench
Definition: Stabilizer.h:188
RTC::TimedDoubleSeq m_qCurrent
Definition: Stabilizer.h:160
void set_vertices(const std::vector< std::vector< Eigen::Vector2d > > &vs)
RTC::TimedOrientation3D m_rpy
Definition: Stabilizer.h:163
void calcSwingEEModification()
void set_alpha_cutoff_freq(const double a)
double get_leg_inside_margin()
bool addPort(PortBase &port)
RTC::OutPort< RTC::TimedPoint3D > m_originRefZmpOut
Definition: Stabilizer.h:228
virtual bool write(DataType &value)
std::vector< hrp::Matrix33 > target_ee_R
Definition: Stabilizer.h:303
double transition_smooth_gain
Definition: Stabilizer.h:310
bool use_limb_stretch_avoidance
Definition: Stabilizer.h:298
void stopStabilizer(void)
hrp::Vector3 rel_act_cp
Definition: Stabilizer.h:307
hrp::Vector3 current_root_p
Definition: Stabilizer.h:300
RTC::TimedDoubleSeq m_COPInfo
Definition: Stabilizer.h:177
double d_pos_z_root
Definition: Stabilizer.h:310
hrp::Vector3 pos
Definition: RatsMatrix.h:19
stabilizer component
RTC::TimedBooleanSeq m_actContactStates
Definition: Stabilizer.h:176
hrp::Vector3 diff_foot_origin_ext_moment
Definition: Stabilizer.h:328
hrp::Vector3 ref_cp
Definition: Stabilizer.h:306
void waitSTTransition()
double zmp_origin_off
Definition: Stabilizer.h:310
std::vector< hrp::JointPathExPtr > jpe_v
Definition: Stabilizer.h:284
RTC::InPort< RTC::TimedBooleanSeq > m_contactStatesIn
Definition: Stabilizer.h:200
JSAMPIMAGE data
hrp::Vector3 prev_ref_zmp
Definition: Stabilizer.h:306
int num
hrp::Vector3 ref_total_foot_origin_moment
Definition: Stabilizer.h:332
std::vector< hrp::Vector3 > ref_force
Definition: Stabilizer.h:302
RTC::InPort< RTC::TimedOrientation3D > m_rpyIn
Definition: Stabilizer.h:196
RTC::TimedPoint3D m_diffFootOriginExtMoment
Definition: Stabilizer.h:169
double dt
Definition: Stabilizer.h:293
hrp::Vector3 ref_force
Definition: Stabilizer.h:270
hrp::dvector6 eefm_ee_forcemoment_distribution_weight
Definition: Stabilizer.h:271
RTC::OutPort< RTC::TimedPoint3D > m_zmpOut
Definition: Stabilizer.h:219
void set_leg_rear_margin(const double a)
void print_vertices(const std::string &str)
bool on_ground
Definition: Stabilizer.h:298
void calcTPCC()
hrp::Vector3 ref_zmp_aux
Definition: Stabilizer.h:328
#define DEBUGP
Definition: Stabilizer.cpp:546
hrp::Vector3 eefm_swing_pos_time_const
Definition: Stabilizer.h:268
double contact_decision_threshold
Definition: Stabilizer.h:334
bool addInPort(const char *name, InPortBase &inport)
void distributeZMPToForceMomentsPseudoInverse(std::vector< hrp::Vector3 > &ref_foot_force, std::vector< hrp::Vector3 > &ref_foot_moment, const std::vector< hrp::Vector3 > &ee_pos, const std::vector< hrp::Vector3 > &cop_pos, const std::vector< hrp::Matrix33 > &ee_rot, const std::vector< std::string > &ee_name, const std::vector< double > &limb_gains, const std::vector< double > &toeheel_ratio, const hrp::Vector3 &new_refzmp, const hrp::Vector3 &ref_zmp, const double total_fz, const double dt, const bool printp=true, const std::string &print_str="", const bool use_cop_distribution=true, const std::vector< bool > is_contact_list=std::vector< bool >())
bool reset_emergency_flag
Definition: Stabilizer.h:298
RTC::TimedPoint3D m_zmp
Definition: Stabilizer.h:165
void limbStretchAvoidanceControl(const std::vector< hrp::Vector3 > &ee_p, const std::vector< hrp::Matrix33 > &ee_R)
hrp::Vector3 eefm_ee_moment_limit
Definition: Stabilizer.h:268
png_infop png_uint_32 flag
RTC::OutPort< RTC::TimedOrientation3D > m_actBaseRpyOut
Definition: Stabilizer.h:230
double cop_check_margin
Definition: Stabilizer.h:334
double d_rpy[2]
Definition: Stabilizer.h:316
Link * joint(int index) const
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
hrp::dvector qorg
Definition: Stabilizer.h:288
#define DEBUGP2
Definition: Stabilizer.cpp:547
void sync_2_st()
RTC::TimedLong m_emergencySignal
Definition: Stabilizer.h:178
hrp::Vector3 sbp_cog_offset
Definition: Stabilizer.h:307
TwoDofController m_tau_y[2]
Definition: Stabilizer.h:318
double rdx
Definition: Stabilizer.h:323
RTC::OutPort< RTC::TimedDoubleSeq > m_tauOut
Definition: Stabilizer.h:218
RTC::TimedPoint3D m_originRefCogVel
Definition: Stabilizer.h:183
hrp::Vector3 ee_d_foot_pos
Definition: Stabilizer.h:267
RTC::OutPort< RTC::TimedPoint3D > m_originActCogVelOut
Definition: Stabilizer.h:229
hrp::Matrix33 target_foot_origin_rot
Definition: Stabilizer.h:301
RTC::InPort< RTC::TimedDoubleSeq > m_qRefSeqIn
Definition: Stabilizer.h:204
RTC::OutPort< RTC::TimedPoint3D > m_diffCPOut
Definition: Stabilizer.h:222
int m_is_falling_counter
Definition: Stabilizer.h:295
hrp::Vector3 ee_d_foot_rpy
Definition: Stabilizer.h:267
void mid_coords(coordinates &mid_coords, const double p, const coordinates &c1, const coordinates &c2, const double eps)
Definition: RatsMatrix.cpp:58
std::map< std::string, hrp::VirtualForceSensorParam > m_vfs
Definition: Stabilizer.h:283
int usleep(useconds_t usec)
hrp::Vector3 prev_d_pos_swing
Definition: Stabilizer.h:275
bool registerProvider(const char *instance_name, const char *type_name, PortableServer::RefCountServantBase &provider)
hrp::Vector3 current_base_rpy
Definition: Stabilizer.h:307
RTC::TimedBoolean m_walkingStates
Definition: Stabilizer.h:180
void sync_2_idle()
RTC::OutPort< RTC::TimedPoint3D > m_originRefCogVelOut
Definition: Stabilizer.h:228
RTC::TimedPoint3D m_diffCP
Definition: Stabilizer.h:168
double eefm_pos_compensation_limit
Definition: Stabilizer.h:269
RTC::TimedPoint3D m_currentBasePos
Definition: Stabilizer.h:186
std::vector< hrp::Matrix33 > act_ee_R
Definition: Stabilizer.h:303
bool is_estop_while_walking
Definition: Stabilizer.h:299


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