ZMPDistributor.h
Go to the documentation of this file.
1 // -*- C++ -*-
10 #ifndef ZMP_DISTRIBUTOR_H
11 #define ZMP_DISTRIBUTOR_H
12 
13 #include <hrpModel/Body.h>
14 #include <iostream>
15 #include <iterator>
16 #include "../ImpedanceController/JointPathEx.h"
17 #include "../TorqueFilter/IIRFilter.h"
18 #include <hrpUtil/MatrixSolvers.h>
19 
20 #ifdef USE_QPOASES
21 #include <qpOASES.hpp>
22 using namespace qpOASES;
23 #endif
24 
26 {
27  std::vector<std::vector<Eigen::Vector2d> > foot_vertices; // RLEG, LLEG
28 public:
30  bool inside_foot (size_t idx)
31  {
32  return true;
33  };
34  Eigen::Vector2d get_foot_vertex (const size_t foot_idx, const size_t vtx_idx)
35  {
36  return foot_vertices[foot_idx][vtx_idx];
37  };
38  void set_vertices (const std::vector<std::vector<Eigen::Vector2d> >& vs) { foot_vertices = vs; };
39  void get_vertices (std::vector<std::vector<Eigen::Vector2d> >& vs) { vs = foot_vertices; };
40  void print_vertices (const std::string& str)
41  {
42  std::cerr << "[" << str << "] ";
43  for (size_t i = 0; i < foot_vertices.size(); i++) {
44  std::cerr << "vs = ";
45  for (size_t j = 0; j < foot_vertices[i].size(); j++) {
46  std::cerr << "[" << foot_vertices[i][j](0) << " " << foot_vertices[i][j](1) << "] ";
47  }
48  std::cerr << ((i==foot_vertices.size()-1)?"[m]":"[m], ");
49  }
50  std::cerr << std::endl;;
51  }
52 };
53 
54 //
55 
57 {
59  double leg_inside_margin, leg_outside_margin, leg_front_margin, leg_rear_margin, wrench_alpha_blending;
61  std::vector<Eigen::Vector2d> convex_hull;
63 public:
65  SimpleZMPDistributor (const double _dt) : wrench_alpha_blending (0.5)
66  {
67  alpha_filter = boost::shared_ptr<FirstOrderLowPassFilter<double> >(new FirstOrderLowPassFilter<double>(1e7, _dt, 0.5)); // [Hz], Almost no filter by default
68  };
69 
70  inline bool is_inside_foot (const hrp::Vector3& leg_pos, const bool is_lleg, const double margin = 0.0)
71  {
72  if (is_lleg) return (leg_pos(1) >= (-1 * leg_inside_margin + margin)) && (leg_pos(1) <= (leg_outside_margin - margin));
73  else return (leg_pos(1) <= (leg_inside_margin - margin)) && (leg_pos(1) >= (-1 * leg_outside_margin + margin));
74  };
75  inline bool is_front_of_foot (const hrp::Vector3& leg_pos, const double margin = 0.0)
76  {
77  return leg_pos(0) >= (leg_front_margin - margin);
78  };
79  inline bool is_rear_of_foot (const hrp::Vector3& leg_pos, const double margin = 0.0)
80  {
81  return leg_pos(0) <= (-1 * leg_rear_margin + margin);
82  };
83  inline bool is_inside_support_polygon (Eigen::Vector2d& p, const hrp::Vector3& offset = hrp::Vector3::Zero(), const bool& truncate_p = false, const std::string& str = "")
84  {
85  // set any inner point(ip) and binary search two vertices(convex_hull[v_a], convex_hull[v_b]) between which p is.
86  p -= offset.head(2);
87  size_t n_ch = convex_hull.size();
88  Eigen::Vector2d ip = (convex_hull[0] + convex_hull[n_ch/3] + convex_hull[2*n_ch/3]) / 3.0;
89  size_t v_a = 0, v_b = n_ch;
90  while (v_a + 1 < v_b) {
91  size_t v_c = (v_a + v_b) / 2;
92  if (calcCrossProduct(convex_hull[v_a], convex_hull[v_c], ip) > 0) {
93  if (calcCrossProduct(convex_hull[v_a], p, ip) > 0 && calcCrossProduct(convex_hull[v_c], p, ip) < 0) v_b = v_c;
94  else v_a = v_c;
95  } else {
96  if (calcCrossProduct(convex_hull[v_a], p, ip) < 0 && calcCrossProduct(convex_hull[v_c], p, ip) > 0) v_a = v_c;
97  else v_b = v_c;
98  }
99  }
100  v_b %= n_ch;
101  if (calcCrossProduct(convex_hull[v_a], convex_hull[v_b], p) >= 0) {
102  p += offset.head(2);
103  return true;
104  } else {
105  if (truncate_p) {
106  if (!calc_closest_boundary_point(p, v_a, v_b)) std::cerr << "[" << str << "] Cannot calculate closest boundary point on the convex hull" << std::endl;
107  }
108  p += offset.head(2);
109  return false;
110  }
111  };
112  void print_params (const std::string& str)
113  {
114  std::cerr << "[" << str << "] leg_inside_margin = " << leg_inside_margin << "[m], leg_outside_margin = " << leg_outside_margin << "[m], leg_front_margin = " << leg_front_margin << "[m], leg_rear_margin = " << leg_rear_margin << "[m]" << std::endl;
115  std::cerr << "[" << str << "] wrench_alpha_blending = " << wrench_alpha_blending << ", alpha_cutoff_freq = " << alpha_filter->getCutOffFreq() << "[Hz]" << std::endl;
116  }
117  void print_vertices (const std::string& str)
118  {
119  fs.print_vertices(str);
120  };
121  // Compare Vector2d for sorting lexicographically
122  static bool compare_eigen2d(const Eigen::Vector2d& lv, const Eigen::Vector2d& rv)
123  {
124  return lv(0) < rv(0) || (lv(0) == rv(0) && lv(1) < rv(1));
125  };
126  // Calculate 2D convex hull based on Andrew's algorithm
127  // Assume that the order of vs, ee, and cs is the same
128  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)
129  {
130  // transform coordinate
131  std::vector<Eigen::Vector2d> tvs;
132  hrp::Vector3 tpos;
133  tvs.reserve(cs.size()*vs[0].size());
134  for (size_t i = 0; i < cs.size(); i++) {
135  if (cs[i]) {
136  for (size_t j = 0; j < vs[i].size(); j++) {
137  tpos = ee_pos[i] + ee_rot[i] * hrp::Vector3(vs[i][j](0), vs[i][j](1), 0.0);
138  tvs.push_back(tpos.head(2));
139  }
140  }
141  }
142  // calculate convex hull
143  int n_tvs = tvs.size(), n_ch = 0;
144  convex_hull.resize(2*n_tvs);
145  std::sort(tvs.begin(), tvs.end(), compare_eigen2d);
146  for (int i = 0; i < n_tvs; convex_hull[n_ch++] = tvs[i++])
147  while (n_ch >= 2 && calcCrossProduct(convex_hull[n_ch-1], tvs[i], convex_hull[n_ch-2]) <= 0) n_ch--;
148  for (int i = n_tvs-2, j = n_ch+1; i >= 0; convex_hull[n_ch++] = tvs[i--])
149  while (n_ch >= j && calcCrossProduct(convex_hull[n_ch-1], tvs[i], convex_hull[n_ch-2]) <= 0) n_ch--;
150  convex_hull.resize(n_ch-1);
151  };
152  // Calculate closest boundary point on the convex hull
153  bool calc_closest_boundary_point (Eigen::Vector2d& p, size_t& right_idx, size_t& left_idx) {
154  size_t n_ch = convex_hull.size();
155  Eigen::Vector2d cur_closest_point;
156  for (size_t i = 0; i < n_ch; i++) {
157  switch(calcProjectedPoint(cur_closest_point, p, convex_hull[left_idx], convex_hull[right_idx])) {
158  case MIDDLE:
159  p = cur_closest_point;
160  return true;
161  case LEFT:
162  right_idx = left_idx;
163  left_idx = (left_idx + 1) % n_ch;
164  if ((p - convex_hull[right_idx]).dot(convex_hull[left_idx] - convex_hull[right_idx]) <= 0) {
165  p = cur_closest_point;
166  return true;
167  }
168  case RIGHT:
169  left_idx = right_idx;
170  right_idx = (right_idx - 1) % n_ch;
171  if ((p - convex_hull[left_idx]).dot(convex_hull[right_idx] - convex_hull[left_idx]) <= 0) {
172  p = cur_closest_point;
173  return true;
174  }
175  }
176  }
177  return false;
178  }
179  // setter
180  void set_wrench_alpha_blending (const double a) { wrench_alpha_blending = a; };
181  void set_leg_front_margin (const double a) { leg_front_margin = a; };
182  void set_leg_rear_margin (const double a) { leg_rear_margin = a; };
183  void set_leg_inside_margin (const double a) { leg_inside_margin = a; };
184  void set_leg_outside_margin (const double a) { leg_outside_margin = a; };
185  void set_alpha_cutoff_freq (const double a) { alpha_filter->setCutOffFreq(a); };
186  void set_vertices (const std::vector<std::vector<Eigen::Vector2d> >& vs)
187  {
188  fs.set_vertices(vs);
189  // leg_inside_margin = fs.get_foot_vertex(0, 0)(1);
190  // leg_front_margin = fs.get_foot_vertex(0, 0)(0);
191  // leg_rear_margin = std::fabs(fs.get_foot_vertex(0, 3)(0));
192  };
194  {
195  std::vector<std::vector<Eigen::Vector2d> > vec;
196  // RLEG
197  {
198  std::vector<Eigen::Vector2d> tvec;
199  tvec.push_back(Eigen::Vector2d(leg_front_margin, leg_inside_margin));
200  tvec.push_back(Eigen::Vector2d(leg_front_margin, -1*leg_outside_margin));
201  tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, -1*leg_outside_margin));
202  tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, leg_inside_margin));
203  vec.push_back(tvec);
204  }
205  // LLEG
206  {
207  std::vector<Eigen::Vector2d> tvec;
208  tvec.push_back(Eigen::Vector2d(leg_front_margin, leg_outside_margin));
209  tvec.push_back(Eigen::Vector2d(leg_front_margin, -1*leg_inside_margin));
210  tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, -1*leg_inside_margin));
211  tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, leg_outside_margin));
212  vec.push_back(tvec);
213  }
214  // {
215  // std::vector<Eigen::Vector2d> tvec;
216  // tvec.push_back(Eigen::Vector2d(leg_front_margin, leg_inside_margin));
217  // tvec.push_back(Eigen::Vector2d(leg_front_margin, -1*leg_outside_margin));
218  // tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, -1*leg_outside_margin));
219  // tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, leg_inside_margin));
220  // vec.push_back(tvec);
221  // }
222  // {
223  // std::vector<Eigen::Vector2d> tvec;
224  // tvec.push_back(Eigen::Vector2d(leg_front_margin, leg_inside_margin));
225  // tvec.push_back(Eigen::Vector2d(leg_front_margin, -1*leg_outside_margin));
226  // tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, -1*leg_outside_margin));
227  // tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, leg_inside_margin));
228  // vec.push_back(tvec);
229  // }
230  set_vertices(vec);
231  };
232  // Set vertices only for cp_check_margin for now
233  void set_vertices_from_margin_params (const std::vector<double>& margin)
234  {
235  std::vector<std::vector<Eigen::Vector2d> > vec;
236  // RLEG
237  {
238  std::vector<Eigen::Vector2d> tvec;
239  tvec.push_back(Eigen::Vector2d(leg_front_margin - margin[0], leg_inside_margin - margin[2]));
240  tvec.push_back(Eigen::Vector2d(leg_front_margin - margin[0], -1*(leg_outside_margin - margin[3])));
241  tvec.push_back(Eigen::Vector2d(-1*(leg_rear_margin - margin[1]), -1*(leg_outside_margin - margin[3])));
242  tvec.push_back(Eigen::Vector2d(-1*(leg_rear_margin - margin[1]), leg_inside_margin - margin[2]));
243  vec.push_back(tvec);
244  }
245  // LLEG
246  {
247  std::vector<Eigen::Vector2d> tvec;
248  tvec.push_back(Eigen::Vector2d(leg_front_margin - margin[0], leg_inside_margin - margin[3]));
249  tvec.push_back(Eigen::Vector2d(leg_front_margin - margin[0], -1*(leg_outside_margin - margin[2])));
250  tvec.push_back(Eigen::Vector2d(-1*(leg_rear_margin - margin[1]), -1*(leg_outside_margin - margin[2])));
251  tvec.push_back(Eigen::Vector2d(-1*(leg_rear_margin - margin[1]), leg_inside_margin - margin[3]));
252  vec.push_back(tvec);
253  }
254  fs_mgn.set_vertices(vec);
255  };
256  // getter
257  double get_wrench_alpha_blending () { return wrench_alpha_blending; };
258  double get_leg_front_margin () { return leg_front_margin; };
259  double get_leg_rear_margin () { return leg_rear_margin; };
260  double get_leg_inside_margin () { return leg_inside_margin; };
261  double get_leg_outside_margin () { return leg_outside_margin; };
262  double get_alpha_cutoff_freq () { return alpha_filter->getCutOffFreq(); };
263  void get_vertices (std::vector<std::vector<Eigen::Vector2d> >& vs) { fs.get_vertices(vs); };
264  void get_margined_vertices (std::vector<std::vector<Eigen::Vector2d> >& vs) { fs_mgn.get_vertices(vs); };
265  //
266  double calcAlpha (const hrp::Vector3& tmprefzmp,
267  const std::vector<hrp::Vector3>& ee_pos,
268  const std::vector<hrp::Matrix33>& ee_rot,
269  const std::vector<std::string>& ee_name)
270  {
271  double alpha;
272  size_t l_idx, r_idx;
273  for (size_t i = 0; i < ee_name.size(); i++) {
274  if (ee_name[i]=="rleg") r_idx = i;
275  else l_idx = i;
276  }
277  hrp::Vector3 l_local_zmp = ee_rot[l_idx].transpose() * (tmprefzmp-ee_pos[l_idx]);
278  hrp::Vector3 r_local_zmp = ee_rot[r_idx].transpose() * (tmprefzmp-ee_pos[r_idx]);
279 
280  // std::cerr << "a " << l_local_zmp(0) << " " << l_local_zmp(1) << std::endl;
281  // std::cerr << "b " << r_local_zmp(0) << " " << r_local_zmp(1) << std::endl;
282  if ( is_inside_foot(l_local_zmp, true) && !is_front_of_foot(l_local_zmp) && !is_rear_of_foot(l_local_zmp)) { // new_refzmp is inside lfoot
283  alpha = 0.0;
284  } else if ( is_inside_foot(r_local_zmp, false) && !is_front_of_foot(r_local_zmp) && !is_rear_of_foot(r_local_zmp)) { // new_refzmp is inside rfoot
285  alpha = 1.0;
286  } else {
287  hrp::Vector3 ledge_foot;
288  hrp::Vector3 redge_foot;
289  // lleg
290  if (is_inside_foot(l_local_zmp, true) && is_front_of_foot(l_local_zmp)) {
291  ledge_foot = hrp::Vector3(leg_front_margin, l_local_zmp(1), 0.0);
292  } else if (!is_inside_foot(l_local_zmp, true) && is_front_of_foot(l_local_zmp)) {
293  ledge_foot = hrp::Vector3(leg_front_margin, -1 * leg_inside_margin, 0.0);
294  } else if (!is_inside_foot(l_local_zmp, true) && !is_front_of_foot(l_local_zmp) && !is_rear_of_foot(l_local_zmp)) {
295  ledge_foot = hrp::Vector3(l_local_zmp(0), -1 * leg_inside_margin, 0.0);
296  } else if (!is_inside_foot(l_local_zmp, true) && is_rear_of_foot(l_local_zmp)) {
297  ledge_foot = hrp::Vector3(-1 * leg_rear_margin, -1 * leg_inside_margin, 0.0);
298  } else {
299  ledge_foot = hrp::Vector3(-1 * leg_rear_margin, l_local_zmp(1), 0.0);
300  }
301  ledge_foot = ee_rot[l_idx] * ledge_foot + ee_pos[l_idx];
302  // rleg
303  if (is_inside_foot(r_local_zmp, false) && is_front_of_foot(r_local_zmp)) {
304  redge_foot = hrp::Vector3(leg_front_margin, r_local_zmp(1), 0.0);
305  } else if (!is_inside_foot(r_local_zmp, false) && is_front_of_foot(r_local_zmp)) {
306  redge_foot = hrp::Vector3(leg_front_margin, leg_inside_margin, 0.0);
307  } else if (!is_inside_foot(r_local_zmp, false) && !is_front_of_foot(r_local_zmp) && !is_rear_of_foot(r_local_zmp)) {
308  redge_foot = hrp::Vector3(r_local_zmp(0), leg_inside_margin, 0.0);
309  } else if (!is_inside_foot(r_local_zmp, false) && is_rear_of_foot(r_local_zmp)) {
310  redge_foot = hrp::Vector3(-1 * leg_rear_margin, leg_inside_margin, 0.0);
311  } else {
312  redge_foot = hrp::Vector3(-1 * leg_rear_margin, r_local_zmp(1), 0.0);
313  }
314  redge_foot = ee_rot[r_idx] * redge_foot + ee_pos[r_idx];
315  // calc alpha
316  hrp::Vector3 difp = redge_foot - ledge_foot;
317  alpha = difp.dot(tmprefzmp-ledge_foot)/difp.squaredNorm();
318  }
319  // limit
320  if (alpha>1.0) alpha = 1.0;
321  if (alpha<0.0) alpha = 0.0;
322  return alpha;
323  };
324 
325  double calcAlphaFromCOP (const hrp::Vector3& tmprefzmp,
326  const std::vector<hrp::Vector3>& cop_pos,
327  const std::vector<std::string>& ee_name)
328  {
329  size_t l_idx, r_idx;
330  for (size_t i = 0; i < ee_name.size(); i++) {
331  if (ee_name[i]=="rleg") r_idx = i;
332  else l_idx = i;
333  }
334  hrp::Vector3 difp = (cop_pos[r_idx]-cop_pos[l_idx]);
335  double alpha = difp.dot(tmprefzmp - cop_pos[l_idx])/difp.dot(difp);
336 
337  // limit
338  if (alpha>1.0) alpha = 1.0;
339  if (alpha<0.0) alpha = 0.0;
340  return alpha;
341  };
342 
343  void calcAlphaVector (std::vector<double>& alpha_vector,
344  std::vector<double>& fz_alpha_vector,
345  const std::vector<hrp::Vector3>& ee_pos,
346  const std::vector<hrp::Matrix33>& ee_rot,
347  const std::vector<std::string>& ee_name,
348  const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp)
349  {
350  double fz_alpha = calcAlpha(ref_zmp, ee_pos, ee_rot, ee_name);
351  double tmpalpha = calcAlpha(new_refzmp, ee_pos, ee_rot, ee_name), alpha;
352  // LPF
353  alpha = alpha_filter->passFilter(tmpalpha);
354  // Blending
355  fz_alpha = wrench_alpha_blending * fz_alpha + (1-wrench_alpha_blending) * alpha;
356  for (size_t i = 0; i < ee_name.size(); i++) {
357  alpha_vector[i]= (ee_name[i]=="rleg") ? alpha : 1-alpha;
358  fz_alpha_vector[i]=(ee_name[i]=="rleg") ? fz_alpha : 1-fz_alpha;
359  }
360  };
361 
362  void calcAlphaVectorFromCOP (std::vector<double>& alpha_vector,
363  std::vector<double>& fz_alpha_vector,
364  const std::vector<hrp::Vector3>& cop_pos,
365  const std::vector<std::string>& ee_name,
366  const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp)
367  {
368  double fz_alpha = calcAlphaFromCOP(ref_zmp, cop_pos, ee_name);
369  double tmpalpha = calcAlphaFromCOP(new_refzmp, cop_pos, ee_name), alpha;
370  // LPF
371  alpha = alpha_filter->passFilter(tmpalpha);
372  // Blending
373  fz_alpha = wrench_alpha_blending * fz_alpha + (1-wrench_alpha_blending) * alpha;
374  for (size_t i = 0; i < ee_name.size(); i++) {
375  alpha_vector[i]= (ee_name[i]=="rleg") ? alpha : 1-alpha;
376  fz_alpha_vector[i]=(ee_name[i]=="rleg") ? fz_alpha : 1-fz_alpha;
377  }
378  };
379 
380  void calcAlphaVectorFromCOPDistanceCommon (std::vector<double>& alpha_vector,
381  const std::vector<hrp::Vector3>& cop_pos,
382  const std::vector<std::string>& ee_name,
383  const hrp::Vector3& ref_zmp)
384  {
385  std::vector<double> distances;
386  double tmpdistance = 0;
387  for (size_t i = 0; i < ee_name.size(); i++) {
388  distances.push_back((cop_pos[i]-ref_zmp).norm());
389  tmpdistance += (cop_pos[i]-ref_zmp).norm();
390  }
391  for (size_t i = 0; i < ee_name.size(); i++) {
392  alpha_vector[i] = tmpdistance/distances[i];
393  }
394  };
395 
396  void calcAlphaVectorFromCOPDistance (std::vector<double>& alpha_vector,
397  std::vector<double>& fz_alpha_vector,
398  const std::vector<hrp::Vector3>& cop_pos,
399  const std::vector<std::string>& ee_name,
400  const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp)
401  {
402  calcAlphaVectorFromCOPDistanceCommon(alpha_vector, cop_pos, ee_name, new_refzmp);
403  calcAlphaVectorFromCOPDistanceCommon(fz_alpha_vector, cop_pos, ee_name, ref_zmp);
404  for (size_t i = 0; i < ee_name.size(); i++) {
405  fz_alpha_vector[i] = wrench_alpha_blending * fz_alpha_vector[i] + (1-wrench_alpha_blending) * alpha_vector[i];
406  }
407  };
408 
409  void distributeZMPToForceMoments (std::vector<hrp::Vector3>& ref_foot_force, std::vector<hrp::Vector3>& ref_foot_moment,
410  const std::vector<hrp::Vector3>& ee_pos,
411  const std::vector<hrp::Vector3>& cop_pos,
412  const std::vector<hrp::Matrix33>& ee_rot,
413  const std::vector<std::string>& ee_name,
414  const std::vector<double>& limb_gains,
415  const std::vector<double>& toeheel_ratio,
416  const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp,
417  const double total_fz, const double dt, const bool printp = true, const std::string& print_str = "")
418  {
419  std::vector<double> alpha_vector(2), fz_alpha_vector(2);
420  calcAlphaVector(alpha_vector, fz_alpha_vector, ee_pos, ee_rot, ee_name, new_refzmp, ref_zmp);
421  ref_foot_force[0] = hrp::Vector3(0,0, fz_alpha_vector[0] * total_fz);
422  ref_foot_force[1] = hrp::Vector3(0,0, fz_alpha_vector[1] * total_fz);
423 
424  hrp::Vector3 tau_0 = hrp::Vector3::Zero();
425 #if 0
426  double gamma = fz_alpha;
427  double beta = m_wrenches[1].data[2] / ( m_wrenches[0].data[2] + m_wrenches[1].data[2] );
428  beta = isnan(beta) ? 0 : beta;
429  double steepness = 8; // change ration from alpha to beta (steepness >= 4)
430  double r = - 1/(1+exp(-6*steepness*(gamma-1+1/steepness))) + 1/(1+exp(-6*steepness*(gamma-1/steepness)));
431  fz_alpha = r * beta + ( 1 - r ) * gamma;
432  // alpha = fz_alpha;
433 
434  ref_foot_force[0] = hrp::Vector3(0,0, fz_alpha * total_fz);
435  ref_foot_force[1] = hrp::Vector3(0,0, (1-fz_alpha) * total_fz);
436  if (DEBUGP) {
437  std::cerr << "[" << m_profile.instance_name << "] slip st parameters" << std::endl;
438  std::cerr << "[" << m_profile.instance_name << "] " << ref_foot_force[1](2) << " " << ref_foot_force[0](2) << " a:"<< steepness << " beta:" << beta << " gamma:" << gamma << " r:" << r << " fz_alpha:" << fz_alpha << " alpha:" << alpha << std::endl;
439  }
440 #endif
441 
442  for (size_t i = 0; i < 2; i++) {
443  tau_0 -= (cop_pos[i] - new_refzmp).cross(ref_foot_force[i]);
444  }
445  {
446  // Foot-distribution-coords frame =>
447 // hrp::Vector3 foot_dist_coords_y = (cop_pos[1] - cop_pos[0]); // e_y'
448 // foot_dist_coords_y(2) = 0.0;
449 // foot_dist_coords_y.normalize();
450 // hrp::Vector3 foot_dist_coords_x = hrp::Vector3(foot_dist_coords_y.cross(hrp::Vector3::UnitZ())); // e_x'
451 // hrp::Matrix33 foot_dist_coords_rot;
452 // foot_dist_coords_rot(0,0) = foot_dist_coords_x(0);
453 // foot_dist_coords_rot(1,0) = foot_dist_coords_x(1);
454 // foot_dist_coords_rot(2,0) = foot_dist_coords_x(2);
455 // foot_dist_coords_rot(0,1) = foot_dist_coords_y(0);
456 // foot_dist_coords_rot(1,1) = foot_dist_coords_y(1);
457 // foot_dist_coords_rot(2,1) = foot_dist_coords_y(2);
458 // foot_dist_coords_rot(0,2) = 0;
459 // foot_dist_coords_rot(1,2) = 0;
460 // foot_dist_coords_rot(2,2) = 1;
461 // hrp::Vector3 tau_0_f = foot_dist_coords_rot.transpose() * tau_0; // tau_0'
462 // // x
463 // // // right
464 // // if (tau_0_f(0) > 0) ref_foot_moment[0](0) = tau_0_f(0);
465 // // else ref_foot_moment[0](0) = 0;
466 // // // left
467 // // if (tau_0_f(0) > 0) ref_foot_moment[1](0) = 0;
468 // // else ref_foot_moment[1](0) = tau_0_f(0);
469 // ref_foot_moment[0](0) = tau_0_f(0) * alpha;
470 // ref_foot_moment[1](0) = tau_0_f(0) * (1-alpha);
471 // // y
472 // ref_foot_moment[0](1) = tau_0_f(1) * alpha;
473 // ref_foot_moment[1](1) = tau_0_f(1) * (1-alpha);
474 // ref_foot_moment[0](2) = ref_foot_moment[1](2) = 0.0;
475 // // <= Foot-distribution-coords frame
476 // // Convert foot-distribution-coords frame => actual world frame
477 // ref_foot_moment[0] = foot_dist_coords_rot * ref_foot_moment[0];
478 // ref_foot_moment[1] = foot_dist_coords_rot * ref_foot_moment[1];
479  //
480  ref_foot_moment[0](0) = tau_0(0) * alpha_vector[0];
481  ref_foot_moment[1](0) = tau_0(0) * alpha_vector[1];
482  ref_foot_moment[0](1) = tau_0(1) * alpha_vector[0];
483  ref_foot_moment[1](1) = tau_0(1) * alpha_vector[1];
484  ref_foot_moment[0](2) = ref_foot_moment[1](2)= 0.0;
485  }
486 #if 0
487  {
488  // Foot-distribution-coords frame =>
489  hrp::Vector3 foot_dist_coords_y = (cop_pos[1] - cop_pos[0]); // e_y'
490  foot_dist_coords_y(2) = 0.0;
491  foot_dist_coords_y.normalize();
492  hrp::Vector3 foot_dist_coords_x = hrp::Vector3(foot_dist_coords_y.cross(hrp::Vector3::UnitZ())); // e_x'
493  hrp::Matrix33 foot_dist_coords_rot;
494  foot_dist_coords_rot(0,0) = foot_dist_coords_x(0);
495  foot_dist_coords_rot(1,0) = foot_dist_coords_x(1);
496  foot_dist_coords_rot(2,0) = foot_dist_coords_x(2);
497  foot_dist_coords_rot(0,1) = foot_dist_coords_y(0);
498  foot_dist_coords_rot(1,1) = foot_dist_coords_y(1);
499  foot_dist_coords_rot(2,1) = foot_dist_coords_y(2);
500  foot_dist_coords_rot(0,2) = 0;
501  foot_dist_coords_rot(1,2) = 0;
502  foot_dist_coords_rot(2,2) = 1;
503  tau_0 = hrp::Vector3::Zero();
504  //
505  hrp::dvector fvec(3);
506  fvec(0) = total_fz;
507  fvec(1) = tau_0(0);
508  fvec(2) = tau_0(1);
509  hrp::dmatrix Gmat(3,6);
510  Gmat(0,0) = 1.0; Gmat(0,1) = 0.0; Gmat(0,2) = 0.0;
511  Gmat(0,3) = 1.0; Gmat(0,4) = 0.0; Gmat(0,5) = 0.0;
512  Gmat(1,0) = (cop_pos[0](1)-new_refzmp(1));
513  Gmat(1,1) = 1.0;
514  Gmat(1,2) = 0.0;
515  Gmat(1,3) = (cop_pos[1](1)-new_refzmp(1));
516  Gmat(1,4) = 1.0;
517  Gmat(1,5) = 0.0;
518  Gmat(2,0) = -(cop_pos[0](0)-new_refzmp(0));
519  Gmat(2,1) = 0.0;
520  Gmat(2,2) = 1.0;
521  Gmat(2,3) = -(cop_pos[1](0)-new_refzmp(0));
522  Gmat(2,4) = 0.0;
523  Gmat(2,5) = 1.0;
524  hrp::dmatrix Wmat(6,6);
525  for (size_t i = 0; i < 6; i++) {
526  for (size_t j = 0; j < 6; j++) {
527  Wmat(i,j) = 0.0;
528  }
529  }
530  double beta_r =0 , beta_l =0;
531  double kk = 8.0;
532  double alpha_r = 0.9;
533  double alpha_l = 0.1;
534  if (alpha > alpha_r) beta_r = std::pow((alpha/alpha_r-1.0), kk)/std::pow((1.0/alpha_r-1.0), kk);
535  else beta_r = 0;
536  if (alpha < alpha_l) beta_l = std::pow((alpha/alpha_l-1.0), kk);
537  else beta_l = 0;
538  Wmat(0,0) = alpha;
539  Wmat(1,1) = beta_r;
540  Wmat(2,2) = alpha;
541  Wmat(3,3) = (1-alpha);
542  Wmat(5,5) = (1-alpha);
543  Wmat(4,4) = beta_l;
544  // if (printp) {
545  // std::cerr << Wmat << std::endl;
546  // }
547  hrp::dmatrix Gmat_ret(6,3);
548  hrp::calcSRInverse(Gmat, Gmat_ret, 0.0, Wmat);
549  hrp::dvector fmvec(6);
550  fmvec = Gmat_ret* fvec;
551  ref_foot_force[0] = hrp::Vector3(0,0,fmvec(0));
552  ref_foot_force[1] = hrp::Vector3(0,0,fmvec(3));
553  ref_foot_moment[0] = hrp::Vector3(fmvec(1),fmvec(2),0);
554  ref_foot_moment[1] = hrp::Vector3(fmvec(4),fmvec(5),0);
555  // <= Foot-distribution-coords frame
556  // Convert foot-distribution-coords frame => actual world frame
557  ref_foot_moment[0] = foot_dist_coords_rot * ref_foot_moment[0];
558  ref_foot_moment[1] = foot_dist_coords_rot * ref_foot_moment[1];
559  }
560 #endif
561 
562  if (printp) {
563  //std::cerr << "[" << print_str << "] alpha = " << alpha << ", fz_alpha = " << fz_alpha << std::endl;
564  std::cerr << "[" << print_str << "] "
565  << "total_tau = " << hrp::Vector3(tau_0).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
566  std::cerr << "[" << print_str << "] "
567  << "ref_force_R = " << hrp::Vector3(ref_foot_force[0]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N]" << std::endl;
568  std::cerr << "[" << print_str << "] "
569  << "ref_force_L = " << hrp::Vector3(ref_foot_force[1]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N]" << std::endl;
570  std::cerr << "[" << print_str << "] "
571  << "ref_moment_R = " << hrp::Vector3(ref_foot_moment[0]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
572  std::cerr << "[" << print_str << "] "
573  << "ref_moment_L = " << hrp::Vector3(ref_foot_moment[1]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
574  }
575  };
576 
577 #ifdef USE_QPOASES
578  void solveForceMomentQPOASES (std::vector<hrp::dvector>& fret,
579  const size_t state_dim,
580  const size_t ee_num,
581  const hrp::dmatrix& Hmat,
582  const hrp::dvector& gvec)
583  {
584  real_t* H = new real_t[state_dim*state_dim];
585  real_t* g = new real_t[state_dim];
586  real_t* lb = new real_t[state_dim];
587  real_t* ub = new real_t[state_dim];
588  for (size_t i = 0; i < state_dim; i++) {
589  for (size_t j = 0; j < state_dim; j++) {
590  H[i*state_dim+j] = Hmat(i,j);
591  }
592  g[i] = gvec(i);
593  lb[i] = 0.0;
594  ub[i] = 1e10;
595  }
596  QProblemB example( state_dim );
597  Options options;
598  //options.enableFlippingBounds = BT_FALSE;
599  options.initialStatusBounds = ST_INACTIVE;
600  options.numRefinementSteps = 1;
601  options.enableCholeskyRefactorisation = 1;
602  //options.printLevel = PL_LOW;
603  options.printLevel = PL_NONE;
604  example.setOptions( options );
605  /* Solve first QP. */
606  int nWSR = 10;
607  example.init( H,g,lb,ub, nWSR,0 );
608  real_t* xOpt = new real_t[state_dim];
609  example.getPrimalSolution( xOpt );
610  size_t state_dim_one = state_dim / ee_num;
611  for (size_t fidx = 0; fidx < ee_num; fidx++) {
612  for (size_t i = 0; i < state_dim_one; i++) {
613  fret[fidx](i) = xOpt[i+state_dim_one*fidx];
614  }
615  }
616  delete[] H;
617  delete[] g;
618  delete[] lb;
619  delete[] ub;
620  delete[] xOpt;
621  };
622 
623  void distributeZMPToForceMomentsQP (std::vector<hrp::Vector3>& ref_foot_force, std::vector<hrp::Vector3>& ref_foot_moment,
624  const std::vector<hrp::Vector3>& ee_pos,
625  const std::vector<hrp::Vector3>& cop_pos,
626  const std::vector<hrp::Matrix33>& ee_rot,
627  const std::vector<std::string>& ee_name,
628  const std::vector<double>& limb_gains,
629  const std::vector<double>& toeheel_ratio,
630  const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp,
631  const double total_fz, const double dt, const bool printp = true, const std::string& print_str = "",
632  const bool use_cop_distribution = false)
633  {
634  size_t ee_num = ee_name.size();
635  std::vector<double> alpha_vector(ee_num), fz_alpha_vector(ee_num);
636  if ( use_cop_distribution ) {
637  //calcAlphaVectorFromCOP(alpha_vector, fz_alpha_vector, cop_pos, ee_name, new_refzmp, ref_zmp);
638  calcAlphaVectorFromCOPDistance(alpha_vector, fz_alpha_vector, cop_pos, ee_name, new_refzmp, ref_zmp);
639  } else {
640  calcAlphaVector(alpha_vector, fz_alpha_vector, ee_pos, ee_rot, ee_name, new_refzmp, ref_zmp);
641  }
642 
643  // QP
644  double norm_weight = 1e-7;
645  double cop_weight = 1e-3;
646  double ref_force_weight = 0;// 1e-3;
647  hrp::dvector total_fm(3);
648  total_fm(0) = total_fz;
649  total_fm(1) = 0;
650  total_fm(2) = 0;
651  size_t state_dim = 4*ee_num, state_dim_one = 4; // TODO
652  //
653  std::vector<hrp::dvector> ff(ee_num, hrp::dvector(state_dim_one));
654  std::vector<hrp::dmatrix> mm(ee_num, hrp::dmatrix(3, state_dim_one));
655  //
656  hrp::dmatrix Hmat = hrp::dmatrix::Zero(state_dim,state_dim);
657  hrp::dvector gvec = hrp::dvector::Zero(state_dim);
658  double alpha_thre = 1e-20;
659  // fz_alpha inversion for weighing matrix
660  for (size_t i = 0; i < fz_alpha_vector.size(); i++) {
661  fz_alpha_vector[i] *= limb_gains[i];
662  fz_alpha_vector[i] = (fz_alpha_vector[i] < alpha_thre) ? 1/alpha_thre : 1/fz_alpha_vector[i];
663  }
664  for (size_t j = 0; j < fz_alpha_vector.size(); j++) {
665  for (size_t i = 0; i < state_dim_one; i++) {
666  Hmat(i+j*state_dim_one,i+j*state_dim_one) = norm_weight * fz_alpha_vector[j];
667  }
668  }
669  hrp::dmatrix Gmat(3,state_dim);
670  for (size_t i = 0; i < state_dim; i++) {
671  Gmat(0,i) = 1.0;
672  }
673  for (size_t fidx = 0; fidx < ee_num; fidx++) {
674  for (size_t i = 0; i < state_dim_one; i++) {
675  hrp::Vector3 fpos = ee_rot[fidx]*hrp::Vector3(fs.get_foot_vertex(fidx,i)(0), fs.get_foot_vertex(fidx,i)(1), 0) + ee_pos[fidx];
676  mm[fidx](0,i) = 1.0;
677  mm[fidx](1,i) = -(fpos(1)-cop_pos[fidx](1));
678  mm[fidx](2,i) = (fpos(0)-cop_pos[fidx](0));
679  Gmat(1,i+state_dim_one*fidx) = -(fpos(1)-new_refzmp(1));
680  Gmat(2,i+state_dim_one*fidx) = (fpos(0)-new_refzmp(0));
681  }
682  //std::cerr << "fpos " << fpos[0] << " " << fpos[1] << std::endl;
683  }
684  Hmat += Gmat.transpose() * Gmat;
685  gvec += -1 * Gmat.transpose() * total_fm;
686  // std::cerr << "Gmat " << std::endl;
687  // std::cerr << Gmat << std::endl;
688  // std::cerr << "total_fm " << std::endl;
689  // std::cerr << total_fm << std::endl;
690  //
691  {
692  hrp::dmatrix Kmat = hrp::dmatrix::Zero(ee_num,state_dim);
693  hrp::dmatrix KW = hrp::dmatrix::Zero(ee_num, ee_num);
694  hrp::dvector reff(ee_num);
695  for (size_t j = 0; j < ee_num; j++) {
696  for (size_t i = 0; i < state_dim_one; i++) {
697  Kmat(j,i+j*state_dim_one) = 1.0;
698  }
699  reff(j) = ref_foot_force[j](2);// total_fz/2.0;
700  KW(j,j) = ref_force_weight;
701  }
702  Hmat += Kmat.transpose() * KW * Kmat;
703  gvec += -1 * Kmat.transpose() * KW * reff;
704  }
705  {
706  hrp::dmatrix Cmat = hrp::dmatrix::Zero(ee_num*2,state_dim);
707  hrp::dmatrix CW = hrp::dmatrix::Zero(ee_num*2,ee_num*2);
708  hrp::Vector3 fpos;
709  for (size_t j = 0; j < ee_num; j++) {
710  for (size_t i = 0; i < state_dim_one; i++) {
711  fpos = ee_rot[j]*hrp::Vector3(fs.get_foot_vertex(j,i)(0), fs.get_foot_vertex(j,i)(1), 0) + ee_pos[j];
712  Cmat(j*2, i+j*state_dim_one) = fpos(0) - cop_pos[j](0);
713  Cmat(j*2+1,i+j*state_dim_one) = fpos(1) - cop_pos[j](1);
714  }
715  CW(j*2,j*2) = CW(j*2+1,j*2+1) = cop_weight;
716  }
717  Hmat += Cmat.transpose() * CW * Cmat;
718  }
719  // std::cerr << "H " << Hmat << std::endl;
720  // std::cerr << "g " << gvec << std::endl;
721  solveForceMomentQPOASES(ff, state_dim, ee_num, Hmat, gvec);
722  hrp::dvector tmpv(3);
723  for (size_t fidx = 0; fidx < ee_num; fidx++) {
724  tmpv = mm[fidx] * ff[fidx];
725  ref_foot_force[fidx] = hrp::Vector3(0,0,tmpv(0));
726  ref_foot_moment[fidx] = -1*hrp::Vector3(tmpv(1),tmpv(2),0);
727  }
728  if (printp) {
729  std::cerr << "[" << print_str << "] force moment distribution " << (use_cop_distribution ? "(QP COP)" : "(QP)") << std::endl;
730  //std::cerr << "[" << print_str << "] alpha = " << alpha << ", fz_alpha = " << fz_alpha << std::endl;
731  // std::cerr << "[" << print_str << "] "
732  // << "total_tau = " << hrp::Vector3(tau_0).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
733  for (size_t i = 0; i < ee_num; i++) {
734  std::cerr << "[" << print_str << "] "
735  << "ref_force [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N]" << std::endl;
736  std::cerr << "[" << print_str << "] "
737  << "ref_moment [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_moment[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
738  }
739  }
740  };
741 #else
742  void distributeZMPToForceMomentsQP (std::vector<hrp::Vector3>& ref_foot_force, std::vector<hrp::Vector3>& ref_foot_moment,
743  const std::vector<hrp::Vector3>& ee_pos,
744  const std::vector<hrp::Vector3>& cop_pos,
745  const std::vector<hrp::Matrix33>& ee_rot,
746  const std::vector<std::string>& ee_name,
747  const std::vector<double>& limb_gains,
748  const std::vector<double>& toeheel_ratio,
749  const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp,
750  const double total_fz, const double dt, const bool printp = true, const std::string& print_str = "",
751  const bool use_cop_distribution = false)
752  {
753  distributeZMPToForceMoments(ref_foot_force, ref_foot_moment,
754  ee_pos, cop_pos, ee_rot, ee_name, limb_gains, toeheel_ratio,
755  new_refzmp, ref_zmp,
756  total_fz, dt, printp, print_str);
757  };
758 #endif // USE_QPOASES
759 
760  // Solve A * x = b => x = W A^T (A W A^T)-1 b
761  // => x = W^{1/2} Pinv(A W^{1/2}) b
763  {
764  hrp::dmatrix W2 = hrp::dmatrix::Zero(W.rows(), W.cols());
765  for (size_t i = 0; i < W.rows(); i++) W2(i,i) = std::sqrt(W(i,i));
766  hrp::dmatrix Aw = A*W2;
767  hrp::dmatrix Aw_inv = hrp::dmatrix::Zero(A.cols(), A.rows());
768  hrp::calcPseudoInverse(Aw, Aw_inv);
769  ret = W2 * Aw_inv * b;
770  //ret = W2 * Aw.colPivHouseholderQr().solve(b);
771  };
772 
773  void distributeZMPToForceMomentsPseudoInverse (std::vector<hrp::Vector3>& ref_foot_force, std::vector<hrp::Vector3>& ref_foot_moment,
774  const std::vector<hrp::Vector3>& ee_pos,
775  const std::vector<hrp::Vector3>& cop_pos,
776  const std::vector<hrp::Matrix33>& ee_rot,
777  const std::vector<std::string>& ee_name,
778  const std::vector<double>& limb_gains,
779  const std::vector<double>& toeheel_ratio,
780  const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp,
781  const double total_fz, const double dt, const bool printp = true, const std::string& print_str = "",
782  const bool use_cop_distribution = true, const std::vector<bool> is_contact_list = std::vector<bool>())
783  {
784  size_t ee_num = ee_name.size();
785  std::vector<double> alpha_vector(ee_num), fz_alpha_vector(ee_num);
786  calcAlphaVectorFromCOPDistance(alpha_vector, fz_alpha_vector, cop_pos, ee_name, new_refzmp, ref_zmp);
787  if (printp) {
788  std::cerr << "[" << print_str << "] force moment distribution (Pinv)" << std::endl;
789  }
790 
791  // ref_foot_force and ref_foot_moment should be set
792  double norm_weight = 1e-7;
793  double norm_moment_weight = 1e2;
794  size_t total_wrench_dim = 5;
795  size_t state_dim = 6*ee_num;
796 
797  // Temporarily set ref_foot_force and ref_foot_moment based on reference values
798  {
799  size_t total_wrench_dim = 5;
800  //size_t total_wrench_dim = 3;
801  hrp::dmatrix Wmat = hrp::dmatrix::Identity(state_dim/2, state_dim/2);
802  hrp::dmatrix Gmat = hrp::dmatrix::Zero(total_wrench_dim, state_dim/2);
803  for (size_t j = 0; j < ee_num; j++) {
804  if (total_wrench_dim == 3) {
805  Gmat(0,3*j+2) = 1.0;
806  } else {
807  for (size_t k = 0; k < 3; k++) Gmat(k,3*j+k) = 1.0;
808  }
809  }
810  for (size_t i = 0; i < total_wrench_dim; i++) {
811  for (size_t j = 0; j < ee_num; j++) {
812  if ( i == total_wrench_dim-2 ) { // Nx
813  Gmat(i,3*j+1) = -(cop_pos[j](2) - ref_zmp(2));
814  Gmat(i,3*j+2) = (cop_pos[j](1) - ref_zmp(1));
815  } else if ( i == total_wrench_dim-1 ) { // Ny
816  Gmat(i,3*j) = (cop_pos[j](2) - ref_zmp(2));
817  Gmat(i,3*j+2) = -(cop_pos[j](0) - ref_zmp(0));
818  }
819  }
820  }
821  for (size_t j = 0; j < ee_num; j++) {
822  for (size_t i = 0; i < 3; i++) {
823  if (i != 2 && ee_num == 2)
824  Wmat(i+j*3, i+j*3) = 0;
825  else
826  Wmat(i+j*3, i+j*3) = Wmat(i+j*3, i+j*3) * limb_gains[j];
827  }
828  }
829  if (printp) {
830  std::cerr << "[" << print_str << "] Wmat(diag) = [";
831  for (size_t j = 0; j < ee_num; j++) {
832  for (size_t i = 0; i < 3; i++) {
833  std::cerr << Wmat(i+j*3, i+j*3) << " ";
834  }
835  }
836  std::cerr << "]" << std::endl;
837  }
838  hrp::dvector ret(state_dim/2);
839  hrp::dvector total_wrench = hrp::dvector::Zero(total_wrench_dim);
840  total_wrench(total_wrench_dim-3) = total_fz;
841  calcWeightedLinearEquation(ret, Gmat, Wmat, total_wrench);
842  for (size_t fidx = 0; fidx < ee_num; fidx++) {
843  ref_foot_force[fidx] = hrp::Vector3(ret(3*fidx), ret(3*fidx+1), ret(3*fidx+2));
844  ref_foot_moment[fidx] = hrp::Vector3::Zero();
845  }
846  // std::cerr << "GmatRef" << std::endl;
847  // std::cerr << Gmat << std::endl;
848  // std::cerr << "WmatRef" << std::endl;
849  // std::cerr << Wmat << std::endl;
850  // std::cerr << "ret" << std::endl;
851  // std::cerr << ret << std::endl;
852  }
853  if (printp) {
854  for (size_t i = 0; i < ee_num; i++) {
855  std::cerr << "[" << print_str << "] "
856  << "ref_force [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], "
857  << "ref_moment [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_moment[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
858  }
859  }
860 
861  // Calculate force/moment distribution matrix and vector
862  // We assume F = G f, calculate F and G. f is absolute here.
863  // 1. Calculate F (total_wrench)
864  hrp::dvector total_wrench = hrp::dvector::Zero(total_wrench_dim);
865  hrp::Vector3 ref_total_force = hrp::Vector3::Zero();
866  for (size_t fidx = 0; fidx < ee_num; fidx++) {
867  double tmp_tau_x = -(cop_pos[fidx](2)-ref_zmp(2)) * ref_foot_force[fidx](1)
868  + (cop_pos[fidx](1)-ref_zmp(1)) * ref_foot_force[fidx](2)
869  + ref_foot_moment[fidx](0);
870  total_wrench(3) -= tmp_tau_x;
871  double tmp_tau_y = (cop_pos[fidx](2)-ref_zmp(2)) * ref_foot_force[fidx](0)
872  - (cop_pos[fidx](0)-ref_zmp(0)) * ref_foot_force[fidx](2)
873  + ref_foot_moment[fidx](1);
874  total_wrench(4) -= tmp_tau_y;
875  ref_total_force += ref_foot_force[fidx];
876  }
877  total_wrench(3) -= -(ref_zmp(2) - new_refzmp(2)) * ref_total_force(1) + (ref_zmp(1) - new_refzmp(1)) * ref_total_force(2);
878  total_wrench(4) -= (ref_zmp(2) - new_refzmp(2)) * ref_total_force(0) - (ref_zmp(0) - new_refzmp(0)) * ref_total_force(2);
879  // 2. Calculate G (Gmat)
880  hrp::dmatrix Gmat = hrp::dmatrix::Zero(total_wrench_dim, state_dim);
881  for (size_t j = 0; j < ee_num; j++) {
882  for (size_t k = 0; k < total_wrench_dim; k++) Gmat(k,6*j+k) = 1.0;
883  }
884  for (size_t i = 0; i < total_wrench_dim; i++) {
885  for (size_t j = 0; j < ee_num; j++) {
886  if ( i == 3 ) { // Nx
887  Gmat(i,6*j+1) = -(cop_pos[j](2) - new_refzmp(2));
888  Gmat(i,6*j+2) = (cop_pos[j](1) - new_refzmp(1));
889  } else if ( i == 4) { // Ny
890  Gmat(i,6*j) = (cop_pos[j](2) - new_refzmp(2));
891  Gmat(i,6*j+2) = -(cop_pos[j](0) - new_refzmp(0));
892  }
893  }
894  }
895  // Calc rotation matrix to introduce local frame
896  // G = [tmpsubG_0, tmpsubG_1, ...]
897  // R = diag[tmpR_0, tmpR_1, ...]
898  // f = R f_l
899  // f : absolute, f_l : local
900  // G f = G R f_l
901  // G R = [tmpsubG_0 tmpR_0, tmpsubG_1 tmpR_1, ...] -> inserted to Gmat
902  hrp::dmatrix tmpsubG = hrp::dmatrix::Zero(total_wrench_dim, 6);
903  hrp::dmatrix tmpR = hrp::dmatrix::Zero(6,6);
904  for (size_t ei = 0; ei < ee_num; ei++) {
905  for (size_t i = 0; i < total_wrench_dim; i++) {
906  for (size_t j = 0; j < 6; j++) {
907  tmpsubG(i,j) = Gmat(i,6*ei+j);
908  }
909  }
910  for (size_t i = 0; i < 3; i++) {
911  for (size_t j = 0; j < 3; j++) {
912  tmpR(i,j) = tmpR(i+3,j+3) = ee_rot[ei](i,j);
913  }
914  }
915  tmpsubG = tmpsubG * tmpR;
916  for (size_t i = 0; i < total_wrench_dim; i++) {
917  for (size_t j = 0; j < 6; j++) {
918  Gmat(i,6*ei+j) = tmpsubG(i,j);
919  }
920  }
921  }
922 
923  // Calc weighting matrix
924  hrp::dmatrix Wmat = hrp::dmatrix::Zero(state_dim, state_dim);
925  for (size_t j = 0; j < ee_num; j++) {
926  for (size_t i = 0; i < 3; i++) {
927  Wmat(i+j*6, i+j*6) = fz_alpha_vector[j] * limb_gains[j];
928  Wmat(i+j*6+3, i+j*6+3) = (1.0/norm_moment_weight) * fz_alpha_vector[j] * limb_gains[j];
929  // Set local Y moment
930  // If toeheel_ratio is 0, toe and heel contact and local Y moment should be 0.
931  if (i == 1) {
932  Wmat(i+j*6+3, i+j*6+3) = toeheel_ratio[j] * Wmat(i+j*6+3, i+j*6+3);
933  }
934  // In actual swing phase, X/Y momoment should be 0.
935  if (!is_contact_list.empty()) {
936  if (!is_contact_list[j]) Wmat(i+j*6+3, i+j*6+3) = 0;
937  }
938  }
939  }
940  if (printp) {
941  std::cerr << "[" << print_str << "] newWmat(diag) = [";
942  for (size_t j = 0; j < ee_num; j++) {
943  for (size_t i = 0; i < 6; i++) {
944  std::cerr << Wmat(i+j*6, i+j*6) << " ";
945  }
946  }
947  std::cerr << "]" << std::endl;
948  }
949  // std::cerr << "total_wrench" << std::endl;
950  // std::cerr << total_wrench << std::endl;
951  // std::cerr << "Gmat" << std::endl;
952  // std::cerr << Gmat << std::endl;
953  // std::cerr << "Wmat" << std::endl;
954  // std::cerr << Wmat << std::endl;
955 
956  // Solve
957  hrp::dvector ret(state_dim);
958  calcWeightedLinearEquation(ret, Gmat, Wmat, total_wrench);
959 
960  // Extract force and moment with converting local frame -> absolute frame (ret is local frame)
961  for (size_t fidx = 0; fidx < ee_num; fidx++) {
962  ref_foot_force[fidx] += ee_rot[fidx] * hrp::Vector3(ret(6*fidx), ret(6*fidx+1), ret(6*fidx+2));
963  ref_foot_moment[fidx] += ee_rot[fidx] * hrp::Vector3(ret(6*fidx+3), ret(6*fidx+4), ret(6*fidx+5));
964  }
965  if (printp) {
966  for (size_t i = 0; i < ee_num; i++) {
967  std::cerr << "[" << print_str << "] "
968  << "new_ref_force [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], "
969  << "new_ref_moment [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_moment[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
970  }
971  }
972  };
973 
974  void distributeZMPToForceMomentsPseudoInverse2 (std::vector<hrp::Vector3>& ref_foot_force, std::vector<hrp::Vector3>& ref_foot_moment,
975  const std::vector<hrp::Vector3>& ee_pos,
976  const std::vector<hrp::Vector3>& cop_pos,
977  const std::vector<hrp::Matrix33>& ee_rot,
978  const std::vector<std::string>& ee_name,
979  const std::vector<double>& limb_gains,
980  const std::vector<double>& toeheel_ratio,
981  const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp,
982  const hrp::Vector3& total_force, const hrp::Vector3& total_moment,
983  const std::vector<hrp::dvector6>& ee_forcemoment_distribution_weight,
984  const double total_fz, const double dt, const bool printp = true, const std::string& print_str = "")
985  {
986 #define FORCE_MOMENT_DIFF_CONTROL
987 
988  size_t ee_num = ee_name.size();
989  std::vector<double> alpha_vector(ee_num), fz_alpha_vector(ee_num);
990  calcAlphaVectorFromCOPDistance(alpha_vector, fz_alpha_vector, cop_pos, ee_name, new_refzmp, ref_zmp);
991  if (printp) {
992  std::cerr << "[" << print_str << "] force moment distribution (Pinv2) ";
993 #ifdef FORCE_MOMENT_DIFF_CONTROL
994  std::cerr << "(FORCE_MOMENT_DIFF_CONTROL)" << std::endl;
995 #else
996  std::cerr << "(NOT FORCE_MOMENT_DIFF_CONTROL)" << std::endl;
997 #endif
998  }
999  // ref_foot_force and ref_foot_moment should be set
1000  size_t state_dim = 6*ee_num;
1001  if (printp) {
1002  std::cerr << "[" << print_str << "] fz_alpha_vector [";
1003  for (size_t j = 0; j < ee_num; j++) {
1004  std::cerr << fz_alpha_vector[j] << " ";
1005  }
1006  std::cerr << std::endl;
1007  }
1008 
1009  hrp::dvector total_wrench = hrp::dvector::Zero(6);
1010 #ifndef FORCE_MOMENT_DIFF_CONTROL
1011  total_wrench(0) = total_force(0);
1012  total_wrench(1) = total_force(1);
1013  total_wrench(2) = total_force(2);
1014 #endif
1015  total_wrench(3) = total_moment(0);
1016  total_wrench(4) = total_moment(1);
1017  total_wrench(5) = total_moment(2);
1018 #ifdef FORCE_MOMENT_DIFF_CONTROL
1019  for (size_t fidx = 0; fidx < ee_num; fidx++) {
1020  double tmp_tau_x = -(cop_pos[fidx](2)-new_refzmp(2)) * ref_foot_force[fidx](1) + (cop_pos[fidx](1)-new_refzmp(1)) * ref_foot_force[fidx](2);
1021  total_wrench(3) -= tmp_tau_x;
1022  double tmp_tau_y = (cop_pos[fidx](2)-new_refzmp(2)) * ref_foot_force[fidx](0) - (cop_pos[fidx](0)-new_refzmp(0)) * ref_foot_force[fidx](2);
1023  total_wrench(4) -= tmp_tau_y;
1024  }
1025 #endif
1026 
1027  hrp::dmatrix Wmat = hrp::dmatrix::Zero(state_dim, state_dim);
1028  hrp::dmatrix Gmat = hrp::dmatrix::Zero(6, state_dim);
1029  for (size_t j = 0; j < ee_num; j++) {
1030  for (size_t k = 0; k < 6; k++) Gmat(k,6*j+k) = 1.0;
1031  }
1032  for (size_t i = 0; i < 6; i++) {
1033  for (size_t j = 0; j < ee_num; j++) {
1034  if ( i == 3 ) { // Nx
1035  Gmat(i,6*j+1) = -(cop_pos[j](2) - new_refzmp(2));
1036  Gmat(i,6*j+2) = (cop_pos[j](1) - new_refzmp(1));
1037  } else if ( i == 4 ) { // Ny
1038  Gmat(i,6*j) = (cop_pos[j](2) - new_refzmp(2));
1039  Gmat(i,6*j+2) = -(cop_pos[j](0) - new_refzmp(0));
1040  }
1041  }
1042  }
1043  for (size_t j = 0; j < ee_num; j++) {
1044  for (size_t i = 0; i < 3; i++) {
1045  Wmat(i+j*6, i+j*6) = ee_forcemoment_distribution_weight[j][i] * fz_alpha_vector[j] * limb_gains[j];
1046  //double norm_moment_weight = 1e2;
1047  //Wmat(i+j*6+3, i+j*6+3) = ee_forcemoment_distribution_weight[j][i+3] * (1.0/norm_moment_weight) * fz_alpha_vector[j] * limb_gains[j];
1048  Wmat(i+j*6+3, i+j*6+3) = ee_forcemoment_distribution_weight[j][i+3] * fz_alpha_vector[j] * limb_gains[j];
1049  }
1050  }
1051  if (printp) {
1052  std::cerr << "[" << print_str << "] newWmat(diag) = [";
1053  for (size_t j = 0; j < ee_num; j++) {
1054  for (size_t i = 0; i < 6; i++) {
1055  std::cerr << Wmat(i+j*6, i+j*6) << " ";
1056  }
1057  }
1058  std::cerr << std::endl;
1059  // std::cerr << "[" << print_str << "] Gmat";
1060  // std::cerr << Gmat << std::endl;
1061  // std::cerr << "Wmat" << std::endl;
1062  // std::cerr << Wmat << std::endl;
1063  }
1064 
1065  hrp::dvector ret(state_dim);
1066  // Consider 6DOF total wrench (Fx, Fy, Fz, Mx, My, Mz)
1067  hrp::dmatrix selection_matrix = hrp::dmatrix::Identity(6,6);
1068  // Consdier 3DOF total wrench (Fz, Mx, My)
1069 // hrp::dmatrix selection_matrix = hrp::dmatrix::Zero(3,6);
1070 // selection_matrix(0,2) = 1.0;
1071 // selection_matrix(1,3) = 1.0;
1072 // selection_matrix(2,4) = 1.0;
1073  {
1074  hrp::dvector selected_total_wrench = hrp::dvector::Zero(selection_matrix.rows());
1075  hrp::dmatrix selected_Gmat = hrp::dmatrix::Zero(selection_matrix.rows(), Gmat.cols());
1076  selected_total_wrench = selection_matrix * total_wrench;
1077  selected_Gmat = selection_matrix * Gmat;
1078  calcWeightedLinearEquation(ret, selected_Gmat, Wmat, selected_total_wrench);
1079  }
1080 
1081  if (printp) {
1082  hrp::dvector tmpretv(total_wrench.size());
1083  tmpretv = Gmat * ret - total_wrench;
1084  std::cerr << "[" << print_str << "] "
1085  << "test_diff " << tmpretv.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N][Nm]" << std::endl;
1086  std::cerr << "[" << print_str << "] "
1087  << "total_wrench " << total_wrench.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N][Nm]" << std::endl;
1088  for (size_t i = 0; i < ee_num; i++) {
1089  std::cerr << "[" << print_str << "] "
1090  << "ref_force [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], "
1091  << "ref_moment [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_moment[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
1092  }
1093  for (size_t i = 0; i < ee_num; i++) {
1094 #ifdef FORCE_MOMENT_DIFF_CONTROL
1095  std::cerr << "[" << print_str << "] "
1096  << "dif_ref_force [" << ee_name[i] << "] " << hrp::Vector3(hrp::Vector3(ret(6*i), ret(6*i+1), ret(6*i+2))).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], "
1097  << "dif_ref_moment [" << ee_name[i] << "] " << hrp::Vector3(hrp::Vector3(ret(6*i+3), ret(6*i+4), ret(6*i+5))).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
1098 #else
1099  std::cerr << "[" << print_str << "] "
1100  << "dif_ref_force [" << ee_name[i] << "] " << hrp::Vector3(hrp::Vector3(ret(6*i), ret(6*i+1), ret(6*i+2))-ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], "
1101  << "dif_ref_moment [" << ee_name[i] << "] " << hrp::Vector3(hrp::Vector3(ret(6*i+3), ret(6*i+4), ret(6*i+5))-ref_foot_moment[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
1102 #endif
1103  }
1104  }
1105  for (size_t fidx = 0; fidx < ee_num; fidx++) {
1106 #ifdef FORCE_MOMENT_DIFF_CONTROL
1107  ref_foot_force[fidx] += hrp::Vector3(ret(6*fidx), ret(6*fidx+1), ret(6*fidx+2));
1108  ref_foot_moment[fidx] += hrp::Vector3(ret(6*fidx+3), ret(6*fidx+4), ret(6*fidx+5));
1109 #else
1110  ref_foot_force[fidx] = hrp::Vector3(ret(6*fidx), ret(6*fidx+1), ret(6*fidx+2));
1111  ref_foot_moment[fidx] = hrp::Vector3(ret(6*fidx+3), ret(6*fidx+4), ret(6*fidx+5));
1112 #endif
1113  }
1114  if (printp){
1115  for (size_t i = 0; i < ee_num; i++) {
1116  std::cerr << "[" << print_str << "] "
1117  << "new_ref_force [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], "
1118  << "new_ref_moment [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_moment[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
1119  }
1120  }
1121  };
1122 
1123  double calcCrossProduct(const Eigen::Vector2d& a, const Eigen::Vector2d& b, const Eigen::Vector2d& o)
1124  {
1125  return (a(0) - o(0)) * (b(1) - o(1)) - (a(1) - o(1)) * (b(0) - o(0));
1126  };
1127 
1128  projected_point_region calcProjectedPoint(Eigen::Vector2d& ret, const Eigen::Vector2d& target, const Eigen::Vector2d& a, const Eigen::Vector2d& b){
1129  Eigen::Vector2d v1 = target - b;
1130  Eigen::Vector2d v2 = a - b;
1131  double v2_norm = v2.norm();
1132  if ( v2_norm == 0 ) {
1133  ret = a;
1134  return LEFT;
1135  } else {
1136  double ratio = v1.dot(v2) / (v2_norm * v2_norm);
1137  if (ratio < 0){
1138  ret = b;
1139  return RIGHT;
1140  } else if (ratio > 1){
1141  ret = a;
1142  return LEFT;
1143  } else {
1144  ret = b + ratio * v2;
1145  return MIDDLE;
1146  }
1147  }
1148  };
1149 };
1150 
1151 #endif // ZMP_DISTRIBUTOR_H
double get_alpha_cutoff_freq()
double calcCrossProduct(const Eigen::Vector2d &a, const Eigen::Vector2d &b, const Eigen::Vector2d &o)
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)
Eigen::MatrixXd dmatrix
projected_point_region calcProjectedPoint(Eigen::Vector2d &ret, const Eigen::Vector2d &target, const Eigen::Vector2d &a, const Eigen::Vector2d &b)
LEFT
bool is_inside_foot(const hrp::Vector3 &leg_pos, const bool is_lleg, const double margin=0.0)
void set_wrench_alpha_blending(const double a)
bool is_front_of_foot(const hrp::Vector3 &leg_pos, const double margin=0.0)
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
bool calc_closest_boundary_point(Eigen::Vector2d &p, size_t &right_idx, size_t &left_idx)
void calcAlphaVectorFromCOPDistanceCommon(std::vector< double > &alpha_vector, const std::vector< hrp::Vector3 > &cop_pos, const std::vector< std::string > &ee_name, const hrp::Vector3 &ref_zmp)
bool is_rear_of_foot(const hrp::Vector3 &leg_pos, const double margin=0.0)
png_uint_32 i
void calcAlphaVectorFromCOP(std::vector< double > &alpha_vector, std::vector< double > &fz_alpha_vector, const std::vector< hrp::Vector3 > &cop_pos, const std::vector< std::string > &ee_name, const hrp::Vector3 &new_refzmp, const hrp::Vector3 &ref_zmp)
Eigen::Vector2d get_foot_vertex(const size_t foot_idx, const size_t vtx_idx)
Eigen::VectorXd dvector
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)
Eigen::Vector3d Vector3
double calcAlpha(const hrp::Vector3 &tmprefzmp, const std::vector< hrp::Vector3 > &ee_pos, const std::vector< hrp::Matrix33 > &ee_rot, const std::vector< std::string > &ee_name)
double calcAlphaFromCOP(const hrp::Vector3 &tmprefzmp, const std::vector< hrp::Vector3 > &cop_pos, const std::vector< std::string > &ee_name)
Eigen::Matrix3d Matrix33
static bool compare_eigen2d(const Eigen::Vector2d &lv, const Eigen::Vector2d &rv)
void calcWeightedLinearEquation(hrp::dvector &ret, const hrp::dmatrix &A, const hrp::dmatrix &W, const hrp::dvector &b)
boost::shared_ptr< FirstOrderLowPassFilter< double > > alpha_filter
void set_leg_inside_margin(const double a)
#define DEBUGP
void set_leg_front_margin(const double a)
int calcSRInverse(const dmatrix &_a, dmatrix &_a_sr, double _sr_ratio, dmatrix _w)
Definition: JointPathEx.cpp:42
std::vector< Eigen::Vector2d > convex_hull
double get_leg_outside_margin()
void set_vertices_from_margin_params()
int calcPseudoInverse(const dmatrix &_a, dmatrix &_a_pseu, double _sv_ratio)
double get_wrench_alpha_blending()
void get_vertices(std::vector< std::vector< Eigen::Vector2d > > &vs)
bool inside_foot(size_t idx)
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="")
def norm(a)
Definition: defs.h:6
void get_margined_vertices(std::vector< std::vector< Eigen::Vector2d > > &vs)
Vector3 cross(const Vector3 &v1, const Vector3 &v2)
void calcAlphaVectorFromCOPDistance(std::vector< double > &alpha_vector, std::vector< double > &fz_alpha_vector, const std::vector< hrp::Vector3 > &cop_pos, const std::vector< std::string > &ee_name, const hrp::Vector3 &new_refzmp, const hrp::Vector3 &ref_zmp)
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="")
void print_params(const std::string &str)
void print_vertices(const std::string &str)
bool is_inside_support_polygon(Eigen::Vector2d &p, const hrp::Vector3 &offset=hrp::Vector3::Zero(), const bool &truncate_p=false, const std::string &str="")
void calcAlphaVector(std::vector< double > &alpha_vector, std::vector< double > &fz_alpha_vector, const std::vector< hrp::Vector3 > &ee_pos, const std::vector< hrp::Matrix33 > &ee_rot, const std::vector< std::string > &ee_name, const hrp::Vector3 &new_refzmp, const hrp::Vector3 &ref_zmp)
void set_leg_outside_margin(const double a)
void get_vertices(std::vector< std::vector< Eigen::Vector2d > > &vs)
void set_vertices_from_margin_params(const std::vector< double > &margin)
void set_vertices(const std::vector< std::vector< Eigen::Vector2d > > &vs)
void set_alpha_cutoff_freq(const double a)
double get_leg_inside_margin()
RIGHT
void calcWeightedLinearEquation(hrp::dvector &ret, const hrp::dmatrix &A, const hrp::dmatrix &W, const hrp::dvector &b)
void set_leg_rear_margin(const double a)
void print_vertices(const std::string &str)
FootSupportPolygon fs_mgn
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 >())
SimpleZMPDistributor(const double _dt)
void set_vertices(const std::vector< std::vector< Eigen::Vector2d > > &vs)
std::vector< std::vector< Eigen::Vector2d > > foot_vertices


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