testGaitGenerator.cpp
Go to the documentation of this file.
1 /* -*- coding:utf-8-unix; mode:c++; -*- */
2 
3 #include "GaitGenerator.h"
19 using namespace rats;
20 #include <cstdio>
21 #include <coil/stringutil.h>
22 
23 #define eps_eq(a,b,epsilon) (std::fabs((a)-(b)) < (epsilon))
24 #ifndef rad2deg
25 #define rad2deg(rad) (rad * 180 / M_PI)
26 #endif
27 #ifndef deg2rad
28 #define deg2rad(deg) (deg * M_PI / 180)
29 #endif
30 
31 // Difference checker to observe too large discontinuous values
32 // isSmallDiff is true, calculation is correct and continuous.
33 template<class T> class ValueDifferenceChecker
34 {
36  double diff_thre, max_value_diff;
38  double calcDiff (T& value) const { return 0; };
39 public:
40  ValueDifferenceChecker (double _diff_thre) : diff_thre (_diff_thre), max_value_diff(0), is_small_diff(true), is_initialized(false)
41  {
42  };
44  void checkValueDiff (T& value)
45  {
46  // Initialize prev value
47  if (!is_initialized) {
48  prev_value = value;
49  is_initialized = true;
50  }
51  // Calc diff and update
52  double diff = calcDiff(value);
53  if (diff > max_value_diff) max_value_diff = diff;
54  is_small_diff = (diff < diff_thre) && is_small_diff;
55  prev_value = value;
56  };
57  double getMaxValue () const { return max_value_diff; };
58  double getDiffThre () const { return diff_thre; };
59  bool isSmallDiff () const { return is_small_diff; };
60 };
61 
63 {
64  return (value - prev_value).norm();
65 };
66 
67 template<> double ValueDifferenceChecker< std::vector<hrp::Vector3> >::calcDiff (std::vector<hrp::Vector3>& value) const
68 {
69  double tmp = 0;
70  for (size_t i = 0; i < value.size(); i++) {
71  tmp += (value[i] - prev_value[i]).norm();
72  }
73  return tmp;
74 };
75 
76 // Error checker between two input values
77 // isSmallError is true, error is correct.
79 {
80  double error_thre, max_value_error;
82 public:
83  ValueErrorChecker (double _thre) : error_thre (_thre), max_value_error(0), is_small_error(true)
84  {
85  };
87  void checkValueError (const hrp::Vector3& p0, const hrp::Vector3& p1, std::vector<size_t> neglect_index = std::vector<size_t>())
88  {
89  hrp::Vector3 errorv(p0-p1);
90  for (size_t i = 0; i < neglect_index.size(); i++) errorv(neglect_index[i]) = 0.0;
91  double error = errorv.norm();
92  if (error > max_value_error) max_value_error = error;
93  is_small_error = (error < error_thre) && is_small_error;
94  };
95  double getMaxValue () const { return max_value_error; };
96  double getErrorThre () const { return error_thre; };
97  bool isSmallError () const { return is_small_error; };
98 };
99 
101 {
102 protected:
103  double dt; /* [s] */
104  std::vector<hrp::Vector3> leg_pos; /* default footstep transformations are necessary */
105  std::vector<std::string> all_limbs;
108  bool use_gnuplot, use_graph_append;
109  // previous values for walk pattern calculation
110  hrp::Vector3 prev_rfoot_pos, prev_lfoot_pos, prev_rfoot_rpy, prev_lfoot_rpy;
111  hrp::Vector3 min_rfoot_pos, min_lfoot_pos, max_rfoot_pos, max_lfoot_pos;
114  std::vector<bool> prev_contact_states;
115  std::vector<double> prev_swing_support_time;
116  double min_toe_heel_dif_angle, max_toe_heel_dif_angle, min_zmp_offset_x, max_zmp_offset_x;
117  // Value checker
119  // Check difference of value
120  ValueDifferenceChecker< hrp::Vector3 > refzmp_diff_checker, cartzmp_diff_checker, cog_diff_checker, ssmcpos_diff_checker, ssmcrot_diff_checker, ssmcposvel_diff_checker, ssmcrotvel_diff_checker;
121  ValueDifferenceChecker< std::vector<hrp::Vector3> > footpos_diff_checker, footrot_diff_checker, footposvel_diff_checker, footrotvel_diff_checker, zmpoffset_diff_checker;
122  // Check errors between two values
123  ValueErrorChecker zmp_error_checker, cogzmp_error_checker;
124  // Results of list of step time, toe/heel angle, and zmp offset
125  std::vector<double> step_time_list, min_toe_heel_dif_angle_list, max_toe_heel_dif_angle_list, min_zmp_offset_x_list, max_zmp_offset_x_list;
126  bool is_step_time_valid, is_toe_heel_dif_angle_valid, is_toe_heel_zmp_offset_x_valid;
127  // For plot
128  std::string test_doc_string;
129  std::string fname_cogzmp;
130  FILE* fp_cogzmp;
131  std::string fname_fpos;
132  FILE* fp_fpos;
133  std::string fname_frot;
134  FILE* fp_frot;
135  std::string fname_zoff;
136  FILE* fp_zoff;
137  std::string fname_fposvel;
138  FILE* fp_fposvel;
139  std::string fname_frotvel;
140  FILE* fp_frotvel;
141  std::string fname_thpos;
142  FILE* fp_thpos;
143  std::string fname_sstime;
144  FILE* fp_sstime;
145  std::string fname_ssmc;
146  FILE* fp_ssmc;
147  std::string fname_ssmcvel;
148  FILE* fp_ssmcvel;
149 private:
151  // plot and pattern generation
153 
154  // Plot gnuplot graph and and save graphs to eps files
155  void plot_and_save (FILE* gp, const std::string graph_fname, const std::string plot_str)
156  {
157  fprintf(gp, "%s\n unset multiplot\n", plot_str.c_str());
158  fprintf(gp, "set terminal postscript eps color\nset output '/tmp/%s.eps'\n", graph_fname.c_str());
159  fprintf(gp, "%s\n unset multiplot\n", plot_str.c_str());
160  fflush(gp);
161  };
162 
163  // Dump generated motion by proc_one_tick function.
164  // Calculate error checking and difference values
165  void proc_one_walking_motion (size_t i)
166  {
167  //std::cerr << gg->lcg.gp_count << std::endl;
168  // if ( gg->lcg.gp_index == 4 && gg->lcg.gp_count == 100) {
169  // //std::cerr << gg->lcg.gp_index << std::endl;
170  // gg->update_refzmp_queue(coordinates(hrp::Vector3(150, 105, 0)), coordinates(hrp::Vector3(150, -105, 0)));
171  // }
172 
173  // COG and ZMP
174  fprintf(fp_cogzmp, "%f ", i * dt);
175  for (size_t ii = 0; ii < 3; ii++) {
176  fprintf(fp_cogzmp, "%f ", gg->get_refzmp()(ii));
177  }
178  hrp::Vector3 czmp = gg->get_cart_zmp();
179  for (size_t ii = 0; ii < 3; ii++) {
180  fprintf(fp_cogzmp, "%f ", czmp(ii));
181  }
182  for (size_t ii = 0; ii < 3; ii++) {
183  double cogpos;
184  if (ii==2) {
185  coordinates tmpc;
187  cogpos = tmpc.pos(2)+gg->get_cog()(2);
188  } else {
189  cogpos = gg->get_cog()(ii);
190  }
191  fprintf(fp_cogzmp, "%f ", cogpos);
192  }
193  fprintf(fp_cogzmp, "\n");
194  fflush(fp_cogzmp);
195 
196 #define VEC1(s) std::vector<std::string> (1, s)
197 
198  // Foot pos
199  fprintf(fp_fpos, "%f ", i * dt);
200  hrp::Vector3 rfoot_pos = (gg->get_support_leg_names() == VEC1 ("rleg")) ? gg->get_support_leg_steps().front().worldcoords.pos : gg->get_swing_leg_steps().front().worldcoords.pos;
201  for (size_t ii = 0; ii < 3; ii++) {
202  fprintf(fp_fpos, "%f ", rfoot_pos(ii));
203  min_rfoot_pos(ii) = std::min(min_rfoot_pos(ii), rfoot_pos(ii));
204  max_rfoot_pos(ii) = std::max(max_rfoot_pos(ii), rfoot_pos(ii));
205  }
206  hrp::Vector3 lfoot_pos = (gg->get_support_leg_names() == VEC1("lleg")) ? gg->get_support_leg_steps().front().worldcoords.pos : gg->get_swing_leg_steps().front().worldcoords.pos;
207  for (size_t ii = 0; ii < 3; ii++) {
208  fprintf(fp_fpos, "%f ", lfoot_pos(ii));
209  min_lfoot_pos(ii) = std::min(min_lfoot_pos(ii), lfoot_pos(ii));
210  max_lfoot_pos(ii) = std::max(max_lfoot_pos(ii), lfoot_pos(ii));
211  }
212  fprintf(fp_fpos, "\n");
213  fflush(fp_fpos);
214 
215  // Foot rot
216  fprintf(fp_frot, "%f ", i * dt);
217  hrp::Matrix33 rfoot_rot = (gg->get_support_leg_names() == VEC1("rleg")) ? gg->get_support_leg_steps().front().worldcoords.rot : gg->get_swing_leg_steps().front().worldcoords.rot;
218  hrp::Vector3 rfoot_rpy = hrp::rpyFromRot(rfoot_rot);
219  for (size_t ii = 0; ii < 3; ii++) {
220  fprintf(fp_frot, "%f ", rad2deg(rfoot_rpy(ii)));
221  }
222  hrp::Matrix33 lfoot_rot = (gg->get_support_leg_names() == VEC1("lleg")) ? gg->get_support_leg_steps().front().worldcoords.rot : gg->get_swing_leg_steps().front().worldcoords.rot;
223  hrp::Vector3 lfoot_rpy = hrp::rpyFromRot(lfoot_rot);
224  for (size_t ii = 0; ii < 3; ii++) {
225  fprintf(fp_frot, "%f ", rad2deg(lfoot_rpy(ii)));
226  }
227  fprintf(fp_frot, "\n");
228  fflush(fp_frot);
229 
230  // ZMP offsets
231  fprintf(fp_zoff, "%f ", i * dt);
232  hrp::Vector3 rfoot_zmp_offset = (gg->get_support_leg_names() == VEC1("rleg")) ? gg->get_support_foot_zmp_offsets().front() : gg->get_swing_foot_zmp_offsets().front();
233  for (size_t ii = 0; ii < 3; ii++) {
234  fprintf(fp_zoff, "%f ", rfoot_zmp_offset(ii));
235  }
236  hrp::Vector3 lfoot_zmp_offset = (gg->get_support_leg_names() == VEC1("lleg")) ? gg->get_support_foot_zmp_offsets().front() : gg->get_swing_foot_zmp_offsets().front();
237  for (size_t ii = 0; ii < 3; ii++) {
238  fprintf(fp_zoff, "%f ", lfoot_zmp_offset(ii));
239  }
240  fprintf(fp_zoff, "\n");
241  fflush(fp_zoff);
242  double tmpzoff;
243  if (gg->get_support_leg_names()[0] == "lleg") tmpzoff = rfoot_zmp_offset(0);
244  else tmpzoff = lfoot_zmp_offset(0);
245  min_zmp_offset_x = std::min(min_zmp_offset_x, tmpzoff);
246  max_zmp_offset_x = std::max(max_zmp_offset_x, tmpzoff);
247 
248 #undef VEC1
249 
250  // Foot pos vel
251  fprintf(fp_fposvel, "%f ", i * dt);
252  if ( i == 0 ) prev_rfoot_pos = rfoot_pos;
253  hrp::Vector3 rfootpos_vel = (rfoot_pos - prev_rfoot_pos)/dt;
254  for (size_t ii = 0; ii < 3; ii++) {
255  fprintf(fp_fposvel, "%f ", rfootpos_vel(ii));
256  }
257  prev_rfoot_pos = rfoot_pos;
258  if ( i == 0 ) prev_lfoot_pos = lfoot_pos;
259  hrp::Vector3 lfootpos_vel = (lfoot_pos - prev_lfoot_pos)/dt;
260  for (size_t ii = 0; ii < 3; ii++) {
261  fprintf(fp_fposvel, "%f ", lfootpos_vel(ii));
262  }
263  prev_lfoot_pos = lfoot_pos;
264  fprintf(fp_fposvel, "\n");
265  fflush(fp_fposvel);
266 
267  // Foot rot vel
268  fprintf(fp_frotvel, "%f ", i * dt);
269  if ( i == 0 ) prev_rfoot_rpy = rfoot_rpy;
270  hrp::Vector3 rfootrot_vel = (rfoot_rpy - prev_rfoot_rpy)/dt;
271  for (size_t ii = 0; ii < 3; ii++) {
272  fprintf(fp_frotvel, "%f ", rfootrot_vel(ii));
273  }
274  prev_rfoot_rpy = rfoot_rpy;
275  if ( i == 0 ) prev_lfoot_rpy = lfoot_rpy;
276  hrp::Vector3 lfootrot_vel = (lfoot_rpy - prev_lfoot_rpy)/dt;
277  for (size_t ii = 0; ii < 3; ii++) {
278  fprintf(fp_frotvel, "%f ", lfootrot_vel(ii));
279  }
280  prev_lfoot_rpy = lfoot_rpy;
281  fprintf(fp_frotvel, "\n");
282  fflush(fp_frotvel);
283 
284  // Toe heel pos
285  fprintf(fp_thpos, "%f ", i * dt);
286  hrp::Vector3 tmppos;
287  tmppos = rfoot_pos+rfoot_rot*hrp::Vector3(gg->get_toe_pos_offset_x(), 0, 0);
288  for (size_t ii = 0; ii < 3; ii++) {
289  fprintf(fp_thpos, "%f ", tmppos(ii));
290  min_rfoot_pos(ii) = std::min(min_rfoot_pos(ii), tmppos(ii));
291  max_rfoot_pos(ii) = std::max(max_rfoot_pos(ii), tmppos(ii));
292  }
293  tmppos = lfoot_pos+lfoot_rot*hrp::Vector3(gg->get_toe_pos_offset_x(), 0, 0);
294  for (size_t ii = 0; ii < 3; ii++) {
295  fprintf(fp_thpos, "%f ", tmppos(ii));
296  min_lfoot_pos(ii) = std::min(min_lfoot_pos(ii), tmppos(ii));
297  max_lfoot_pos(ii) = std::max(max_lfoot_pos(ii), tmppos(ii));
298  }
299  tmppos = rfoot_pos+rfoot_rot*hrp::Vector3(gg->get_heel_pos_offset_x(), 0, 0);
300  for (size_t ii = 0; ii < 3; ii++) {
301  fprintf(fp_thpos, "%f ", tmppos(ii));
302  min_rfoot_pos(ii) = std::min(min_rfoot_pos(ii), tmppos(ii));
303  max_rfoot_pos(ii) = std::max(max_rfoot_pos(ii), tmppos(ii));
304  }
305  tmppos = lfoot_pos+lfoot_rot*hrp::Vector3(gg->get_heel_pos_offset_x(), 0, 0);
306  for (size_t ii = 0; ii < 3; ii++) {
307  fprintf(fp_thpos, "%f ", tmppos(ii));
308  min_lfoot_pos(ii) = std::min(min_lfoot_pos(ii), tmppos(ii));
309  max_lfoot_pos(ii) = std::max(max_lfoot_pos(ii), tmppos(ii));
310  }
311  fprintf(fp_thpos, "\n");
312  fflush(fp_thpos);
313 
314  // Swing time
315  fprintf(fp_sstime, "%f ", i * dt);
316  fprintf(fp_sstime, "%f %f ",
319  // Contact States
320  std::vector<leg_type> tmp_current_support_states = gg->get_current_support_states();
321  bool rleg_contact_states = std::find_if(tmp_current_support_states.begin(), tmp_current_support_states.end(), boost::lambda::_1 == RLEG) != tmp_current_support_states.end();
322  bool lleg_contact_states = std::find_if(tmp_current_support_states.begin(), tmp_current_support_states.end(), boost::lambda::_1 == LLEG) != tmp_current_support_states.end();
323  fprintf(fp_sstime, "%d %d %f",
324  (rleg_contact_states ? 1 : 0), (lleg_contact_states ? 1 : 0),
325  0.8*gg->get_current_toe_heel_ratio()+0.1); // scale+translation just for visualization
326  fprintf(fp_sstime, "\n");
327  fflush(fp_sstime);
328 
329  // swing support mid coords
330  fprintf(fp_ssmc, "%f ", i * dt);
331  coordinates tmp_ssmc;
332  gg->get_swing_support_mid_coords(tmp_ssmc);
333  for (size_t ii = 0; ii < 3; ii++) {
334  fprintf(fp_ssmc, "%f ", tmp_ssmc.pos(ii));
335  }
336  hrp::Vector3 tmp_ssmcr = hrp::rpyFromRot(tmp_ssmc.rot);
337  for (size_t ii = 0; ii < 3; ii++) {
338  fprintf(fp_ssmc, "%f ", rad2deg(tmp_ssmcr(ii)));
339  }
340  fprintf(fp_ssmc, "\n");
341  fflush(fp_ssmc);
342 
343  // swing support mid coords vel
344  fprintf(fp_ssmcvel, "%f ", i * dt);
345  if ( i == 0 ) prev_ssmc = tmp_ssmc;
346  hrp::Vector3 tmp_ssmcpos_vel = (tmp_ssmc.pos - prev_ssmc.pos)/dt;
347  for (size_t ii = 0; ii < 3; ii++) {
348  fprintf(fp_ssmcvel, "%f ", tmp_ssmcpos_vel(ii));
349  }
350  hrp::Vector3 prev_ssmcr = hrp::rpyFromRot(prev_ssmc.rot);
351  hrp::Vector3 tmp_ssmcrot_vel = (tmp_ssmcr - prev_ssmcr)/dt;
352  for (size_t ii = 0; ii < 3; ii++) {
353  fprintf(fp_ssmcvel, "%f ", tmp_ssmcrot_vel(ii));
354  }
355  fprintf(fp_ssmcvel, "\n");
356  fflush(fp_ssmcvel);
357  prev_ssmc = tmp_ssmc;
358 
359  // Toe heel angle
360  double tmp_toe_heel_dif_angle = gg->get_toe_heel_dif_angle();
361  min_toe_heel_dif_angle = std::min(min_toe_heel_dif_angle, tmp_toe_heel_dif_angle);
362  max_toe_heel_dif_angle = std::max(max_toe_heel_dif_angle, tmp_toe_heel_dif_angle);
363 
364  // footstep_node change
365  if (gg->get_lcg_count() == 0) {
366  step_time_list.push_back(gg->get_one_step_count()*dt);
367  min_toe_heel_dif_angle_list.push_back(min_toe_heel_dif_angle);
368  max_toe_heel_dif_angle_list.push_back(max_toe_heel_dif_angle);
369  min_toe_heel_dif_angle = 1e10;
370  max_toe_heel_dif_angle = -1e10;
371  if ( !eps_eq(min_zmp_offset_x, gg->get_default_zmp_offsets()[0](0), 1e-5) ) min_zmp_offset_x_list.push_back(min_zmp_offset_x);
372  else min_zmp_offset_x_list.push_back(0.0);
373  if ( !eps_eq(max_zmp_offset_x, gg->get_default_zmp_offsets()[0](0), 1e-5) ) max_zmp_offset_x_list.push_back(max_zmp_offset_x);
374  else max_zmp_offset_x_list.push_back(0.0);
375  min_zmp_offset_x = 1e10;
376  max_zmp_offset_x = -1e10;
377  }
378 
379  // Error checking
380  {
381  // Check error between RefZMP and CartZMP. If too large error, PreviewControl tracking is not enough.
382  zmp_error_checker.checkValueError(gg->get_cart_zmp(), gg->get_refzmp());
383  // Check too large differences (discontinuity)
384  // COG and ZMP
385  hrp::Vector3 tmp(gg->get_refzmp());
386  refzmp_diff_checker.checkValueDiff(tmp);
387  tmp = gg->get_cart_zmp();
388  cartzmp_diff_checker.checkValueDiff(tmp);
389  tmp = gg->get_cog();
390  cog_diff_checker.checkValueDiff(tmp);
391  // Foot pos and rot
392  std::vector<hrp::Vector3> tmpvec = boost::assign::list_of(rfoot_pos)(lfoot_pos);
393  footpos_diff_checker.checkValueDiff(tmpvec);
394  tmpvec = boost::assign::list_of(rfoot_rpy)(lfoot_rpy).convert_to_container < std::vector<hrp::Vector3> > ();
395  footrot_diff_checker.checkValueDiff(tmpvec);
396  tmpvec = boost::assign::list_of(rfootpos_vel)(lfootpos_vel).convert_to_container < std::vector<hrp::Vector3> > ();
397  footposvel_diff_checker.checkValueDiff(tmpvec);
398  tmpvec = boost::assign::list_of(rfootrot_vel)(lfootrot_vel).convert_to_container < std::vector<hrp::Vector3> > ();
399  footrotvel_diff_checker.checkValueDiff(tmpvec);
400  // Swing support mid coorsd
401  ssmcpos_diff_checker.checkValueDiff(tmp_ssmc.pos);
402  ssmcrot_diff_checker.checkValueDiff(tmp_ssmcr);
403  ssmcposvel_diff_checker.checkValueDiff(tmp_ssmcpos_vel);
404  ssmcrotvel_diff_checker.checkValueDiff(tmp_ssmcrot_vel);
405  // ZMP offset
406  tmpvec = boost::assign::list_of(rfoot_zmp_offset)(lfoot_zmp_offset).convert_to_container < std::vector<hrp::Vector3> > ();
407  zmpoffset_diff_checker.checkValueDiff(tmpvec);
408  }
409  // If contact states are not change, prev_swing_support_time is not dt, otherwise prev_swing_support_time is dt.
410  is_contact_states_swing_support_time_validity = is_contact_states_swing_support_time_validity &&
411  ((prev_contact_states[0] == rleg_contact_states) ? !eps_eq(prev_swing_support_time[0],dt,1e-5) : eps_eq(prev_swing_support_time[0],dt,1e-5)) &&
412  ((prev_contact_states[1] == lleg_contact_states) ? !eps_eq(prev_swing_support_time[1],dt,1e-5) : eps_eq(prev_swing_support_time[1],dt,1e-5));
413  prev_refzmp = gg->get_refzmp();
414  prev_contact_states[0] = rleg_contact_states;
415  prev_contact_states[1] = lleg_contact_states;
416  prev_swing_support_time[0] = gg->get_current_swing_time(RLEG);
417  prev_swing_support_time[1] = gg->get_current_swing_time(LLEG);
418  };
419 
420  // Plot state values and print error check
422  {
423  /* plot */
424  if (use_gnuplot) {
425  size_t gpsize = 12;
426  FILE* gps[gpsize];
427  for (size_t ii = 0; ii < gpsize;ii++) {
428  gps[ii] = popen("gnuplot", "w");
429  }
430  {
431  std::ostringstream oss("");
432  std::string gtitle("COG_and_ZMP");
433  size_t tmp_start = 2;
434  oss << "set multiplot layout 3, 1 title '" << gtitle << "'" << std::endl;
435  std::string titles[3] = {"X", "Y", "Z"};
436  for (size_t ii = 0; ii < 3; ii++) {
437  oss << "set xlabel 'Time [s]'" << std::endl;
438  oss << "set ylabel '" << titles[ii] << "[m]'" << std::endl;
439  oss << "plot "
440  << "'" << fname_cogzmp << "' using 1:" << (tmp_start+ii) << " with lines title 'REFZMP',"
441  << "'" << fname_cogzmp << "' using 1:" << (tmp_start+3+ii) << " with lines title 'CARTZMP',"
442  << "'" << fname_cogzmp << "' using 1:" << (tmp_start+6+ii) << " with lines title 'COG'"
443  << std::endl;
444  }
445  plot_and_save(gps[0], gtitle, oss.str());
446  }
447  {
448  std::ostringstream oss("");
449  std::string gtitle("Swing_support_pos");
450  size_t tmp_start = 2;
451  oss << "set multiplot layout 3, 1 title '" << gtitle << "'" << std::endl;
452  std::string titles[3] = {"X", "Y", "Z"};
453  for (size_t ii = 0; ii < 3; ii++) {
454  oss << "set xlabel 'Time [s]'" << std::endl;
455  oss << "set ylabel '" << titles[ii] << "[m]'" << std::endl;
456  oss << "plot "
457  << "'" << fname_fpos << "' using 1:" << (tmp_start+ii) << " with lines title 'rleg',"
458  << "'" << fname_fpos << "' using 1:" << (tmp_start+3+ii) << " with lines title 'lleg'"
459  << std::endl;
460  }
461  plot_and_save(gps[1], gtitle, oss.str());
462  }
463  {
464  std::ostringstream oss("");
465  std::string gtitle("Swing_support_rot");
466  size_t tmp_start = 2;
467  oss << "set multiplot layout 3, 1 title '" << gtitle << "'" << std::endl;
468  std::string titles[3] = {"Roll", "Pitch", "Yaw"};
469  for (size_t ii = 0; ii < 3; ii++) {
470  oss << "set xlabel 'Time [s]'" << std::endl;
471  oss << "set ylabel '" << titles[ii] << "[deg]'" << std::endl;
472  oss << "plot "
473  << "'" << fname_frot << "' using 1:" << (tmp_start+ii) << " with lines title 'rleg',"
474  << "'" << fname_frot << "' using 1:" << (tmp_start+3+ii) << " with lines title 'lleg'"
475  << std::endl;
476  }
477  plot_and_save(gps[2], gtitle, oss.str());
478  }
479  {
480  std::ostringstream oss("");
481  std::string gtitle("Swing_support_zmp_offset");
482  size_t tmp_start = 2;
483  oss << "set multiplot layout 3, 1 title '" << gtitle << "'" << std::endl;
484  std::string titles[3] = {"X", "Y", "Z"};
485  for (size_t ii = 0; ii < 3; ii++) {
486  oss << "set xlabel 'Time [s]'" << std::endl;
487  oss << "set ylabel '" << titles[ii] << "[m]'" << std::endl;
488  oss << "plot "
489  << "'" << fname_zoff << "' using 1:" << (tmp_start+ii) << " with lines title 'rleg',"
490  << "'" << fname_zoff << "' using 1:" << (tmp_start+3+ii) << " with lines title 'lleg'"
491  << std::endl;
492  }
493  plot_and_save(gps[3], gtitle, oss.str());
494  }
495  {
496  std::ostringstream oss("");
497  std::string gtitle("Swing_support_pos_vel");
498  size_t tmp_start = 2;
499  oss << "set multiplot layout 3, 1 title '" << gtitle << "'" << std::endl;
500  std::string titles[3] = {"X", "Y", "Z"};
501  for (size_t ii = 0; ii < 3; ii++) {
502  oss << "set xlabel 'Time [s]'" << std::endl;
503  oss << "set ylabel '" << titles[ii] << "[m/s]'" << std::endl;
504  oss << "plot "
505  << "'" << fname_fposvel << "' using 1:" << (tmp_start+ii) << " with lines title 'rleg',"
506  << "'" << fname_fposvel << "' using 1:" << (tmp_start+3+ii) << " with lines title 'lleg'"
507  << std::endl;
508  }
509  plot_and_save(gps[4], gtitle, oss.str());
510  }
511  {
512  std::ostringstream oss("");
513  std::string gtitle("Swing_support_rot_vel");
514  size_t tmp_start = 2;
515  oss << "set multiplot layout 3, 1 title '" << gtitle << "'" << std::endl;
516  std::string titles[3] = {"Roll", "Pitch", "Yaw"};
517  for (size_t ii = 0; ii < 3; ii++) {
518  oss << "set xlabel 'Time [s]'" << std::endl;
519  oss << "set ylabel '" << titles[ii] << "[deg/s]'" << std::endl;
520  oss << "plot "
521  << "'" << fname_frotvel << "' using 1:" << (tmp_start+ii) << " with lines title 'rleg',"
522  << "'" << fname_frotvel << "' using 1:" << (tmp_start+3+ii) << " with lines title 'lleg'"
523  << std::endl;
524  }
525  plot_and_save(gps[5], gtitle, oss.str());
526  }
527  {
528  std::ostringstream oss("");
529  std::string gtitle("Swing_support_pos_trajectory");
530  double min_v[3], max_v[3], range[3];
531  size_t tmp_thpos_start = 2;
532  for (size_t ii = 0; ii < 3; ii++) {
533  min_v[ii] = std::min(min_rfoot_pos(ii), min_lfoot_pos(ii));
534  max_v[ii] = std::max(max_rfoot_pos(ii), max_lfoot_pos(ii));
535  if (min_v[ii] == 0.0 && max_v[ii] == 0.0) {
536  min_v[ii] = -0.1;
537  max_v[ii] = 0.1;
538  }
539  range[ii] = max_v[ii] - min_v[ii];
540  double mid = (max_v[ii]+min_v[ii])/2.0;
541  min_v[ii] = mid + range[ii] * 1.05 * -0.5;
542  max_v[ii] = mid + range[ii] * 1.05 * 0.5;
543  }
544  oss << "set multiplot layout 2, 1 title '" << gtitle << "'" << std::endl;
545  //oss << "set title 'X-Z'" << std::endl;
546  oss << "set size ratio " << range[2]/range[0] << std::endl;
547  oss << "set xlabel 'X [m]'" << std::endl;
548  oss << "set ylabel 'Z [m]'" << std::endl;
549  oss << "plot "
550  << "[" << min_v[0]<< ":" << max_v[0] << "]"
551  << "[" << min_v[2] << ":" << max_v[2] << "]"
552  << "'" << fname_fpos << "' using " << (2) << ":" << (2+2) << " with lines title 'rleg ee',"
553  << "'" << fname_fpos << "' using " << (2+3) << ":" << (2+3+2) << " with lines title 'lleg ee',"
554  << "'" << fname_thpos << "' using " << (tmp_thpos_start) << ":" << (tmp_thpos_start+2) << " with lines title 'rleg toe',"
555  << "'" << fname_thpos << "' using " << (tmp_thpos_start+3) << ":" << (tmp_thpos_start+3+2) << " with lines title 'lleg toe',"
556  << "'" << fname_thpos << "' using " << (tmp_thpos_start+3+3) << ":" << (tmp_thpos_start+3+3+2) << " with lines title 'rleg heel',"
557  << "'" << fname_thpos << "' using " << (tmp_thpos_start+3+3+3) << ":" << (tmp_thpos_start+3+3+3+2) << " with lines title 'lleg heel'"
558  << std::endl;
559  //oss << "set title 'Y-Z'" << std::endl;
560  oss << "set size ratio " << range[2]/range[1] << std::endl;
561  oss << "set xlabel 'Y [m]'" << std::endl;
562  oss << "set ylabel 'Z [m]'" << std::endl;
563  oss << "plot "
564  << "[" << min_v[1]<< ":" << max_v[1] << "]"
565  << "[" << min_v[2] << ":" << max_v[2] << "]"
566  << "'" << fname_fpos << "' using " << (2+1) << ":" << (2+2) << " with lines title 'rleg ee',"
567  << "'" << fname_fpos << "' using " << (2+3+1) << ":" << (2+3+2) << " with lines title 'lleg ee',"
568  << "'" << fname_thpos << "' using " << (tmp_thpos_start+1) << ":" << (tmp_thpos_start+2) << " with lines title 'rleg toe',"
569  << "'" << fname_thpos << "' using " << (tmp_thpos_start+3+1) << ":" << (tmp_thpos_start+3+2) << " with lines title 'lleg toe',"
570  << "'" << fname_thpos << "' using " << (tmp_thpos_start+3+3+1) << ":" << (tmp_thpos_start+3+3+2) << " with lines title 'rleg heel',"
571  << "'" << fname_thpos << "' using " << (tmp_thpos_start+3+3+3+1) << ":" << (tmp_thpos_start+3+3+3+2) << " with lines title 'lleg heel'"
572  << std::endl;
573  plot_and_save(gps[6], gtitle, oss.str());
574  }
575  {
576  std::ostringstream oss("");
577  std::string gtitle("Swing_support_remain_time");
578  oss << "set multiplot layout 1, 1 title '" << gtitle << "'" << std::endl;
579  oss << "set title 'Remain Time'" << std::endl;
580  oss << "set xlabel 'Time [s]'" << std::endl;
581  oss << "set ylabel 'Time [s]'" << std::endl;
582  oss << "plot "
583  << "'" << fname_sstime << "' using 1:" << 2 << " with lines title 'rleg remain time',"
584  << "'" << fname_sstime << "' using 1:" << 3 << " with lines title 'lleg remain time',"
585  << "'" << fname_sstime << "' using 1:" << 4 << " with lines title 'rleg contact states',"
586  << "'" << fname_sstime << "' using 1:" << 5 << " with lines title 'lleg contact states',"
587  << "'" << fname_sstime << "' using 1:" << 6 << " with lines title 'toe_heel_ratio*0.8+0.1'"
588  << std::endl;
589  plot_and_save(gps[7], gtitle, oss.str());
590  }
591  {
592  std::ostringstream oss("");
593  std::string gtitle("Swing_support_mid_coords_pos");
594  size_t tmp_start = 2;
595  oss << "set multiplot layout 3, 1 title '" << gtitle << "'" << std::endl;
596  std::string titles[3] = {"X", "Y", "Z"};
597  for (size_t ii = 0; ii < 3; ii++) {
598  oss << "set xlabel 'Time [s]'" << std::endl;
599  oss << "set ylabel 'Pos " << titles[ii] << "[m]'" << std::endl;
600  oss << "plot "
601  << "'" << fname_ssmc << "' using 1:" << (tmp_start+ii) << " with lines title ''"
602  << std::endl;
603  }
604  plot_and_save(gps[8], gtitle, oss.str());
605  }
606  {
607  std::ostringstream oss("");
608  std::string gtitle("Swing_support_mid_coords_rot");
609  size_t tmp_start = 2;
610  oss << "set multiplot layout 3, 1 title '" << gtitle << "'" << std::endl;
611  std::string titles[3] = {"Roll", "Pitch", "Yaw"};
612  for (size_t ii = 0; ii < 3; ii++) {
613  oss << "set xlabel 'Time [s]'" << std::endl;
614  oss << "set ylabel 'Rot " << titles[ii] << "[deg]'" << std::endl;
615  oss << "plot "
616  << "'" << fname_ssmc << "' using 1:" << (tmp_start+ii+3) << " with lines title ''"
617  << std::endl;
618  }
619  plot_and_save(gps[9], gtitle, oss.str());
620  }
621  {
622  std::ostringstream oss("");
623  std::string gtitle("Swing_support_mid_coords_pos_vel");
624  size_t tmp_start = 2;
625  oss << "set multiplot layout 3, 1 title '" << gtitle << "'" << std::endl;
626  std::string titles[3] = {"X", "Y", "Z"};
627  for (size_t ii = 0; ii < 3; ii++) {
628  oss << "set xlabel 'Time [s]'" << std::endl;
629  oss << "set ylabel 'PosVel " << titles[ii] << "[m/s]'" << std::endl;
630  oss << "plot "
631  << "'" << fname_ssmcvel << "' using 1:" << (tmp_start+ii) << " with lines title ''"
632  << std::endl;
633  }
634  plot_and_save(gps[10], gtitle, oss.str());
635  }
636  {
637  std::ostringstream oss("");
638  std::string gtitle("Swing_support_mid_coords_rot_vel");
639  size_t tmp_start = 2;
640  oss << "set multiplot layout 3, 1 title '" << gtitle << "'" << std::endl;
641  std::string titles[3] = {"Roll", "Pitch", "Yaw"};
642  for (size_t ii = 0; ii < 3; ii++) {
643  oss << "set xlabel 'Time [s]'" << std::endl;
644  oss << "set ylabel 'RotVel " << titles[ii] << "[deg/s]'" << std::endl;
645  oss << "plot "
646  << "'" << fname_ssmcvel << "' using 1:" << (tmp_start+ii+3) << " with lines title ''"
647  << std::endl;
648  }
649  plot_and_save(gps[11], gtitle, oss.str());
650  }
651  //
652  if (use_graph_append) {
653  int ret = 0;
654  usleep(500000); // Wait for gnuplot plot finishing (0.5[s])
655  ret = system("bash -c 'for f in /tmp/*.eps; do convert -density 250x250 $f ${f//eps}jpg; done'");
656  ret = system("(cd /tmp; convert +append Swing_support_mid_coords_pos.jpg Swing_support_mid_coords_pos_vel.jpg Swing_support_mid_coords_rot.jpg Swing_support_mid_coords_rot_vel.jpg img1.jpg)");
657  ret = system("(cd /tmp/; convert +append Swing_support_pos.jpg Swing_support_pos_vel.jpg Swing_support_rot.jpg Swing_support_rot_vel.jpg img2.jpg)");
658  ret = system("(cd /tmp/; convert +append Swing_support_zmp_offset.jpg Swing_support_remain_time.jpg COG_and_ZMP.jpg Swing_support_pos_trajectory.jpg img3.jpg)");
659  std::string tmpstr = test_doc_string.substr(0, test_doc_string.find(":"));
660  for(size_t c = tmpstr.find_first_of(" "); c != std::string::npos; c = c = tmpstr.find_first_of(" ")){
661  tmpstr.erase(c,1);
662  }
663  ret = system(std::string("(cd /tmp/; convert -append img1.jpg img2.jpg img3.jpg testGaitGeneratorResults_"+tmpstr+".jpg; rm -f /tmp/img[123].jpg /tmp/COG_and_ZMP.jpg /tmp/Swing_support*.jpg)").c_str());
664  } else {
665  double tmp;
666  std::cin >> tmp;
667  }
668  for (size_t ii = 0; ii < gpsize; ii++) {
669  fprintf(gps[ii], "exit\n");
670  fflush(gps[ii]);
671  pclose(gps[ii]);
672  }
673  }
674  std::string tmpstr = test_doc_string.substr(0, test_doc_string.find(":"));
675  for(size_t c = tmpstr.find_first_of(" "); c != std::string::npos; c = c = tmpstr.find_first_of(" ")){
676  tmpstr.erase(c,1);
677  }
678  std::cerr << "Checking of " << tmpstr << " : all results = " << (check_all_results()?"true":"false") << std::endl;
679  std::cerr << " ZMP error : " << (zmp_error_checker.isSmallError()?"true":"false") << ", max_error : " << zmp_error_checker.getMaxValue()*1e3 << "[mm], thre : " << zmp_error_checker.getErrorThre()*1e3 << "[mm]" << std::endl;
680  std::cerr << " COGZMP error : " << (cogzmp_error_checker.isSmallError()?"true":"false") << ", max_error : " << cogzmp_error_checker.getMaxValue()*1e3 << "[mm], thre : " << cogzmp_error_checker.getErrorThre()*1e3 << "[mm]" << std::endl;
681  std::cerr << " RefZMP diff : " << (refzmp_diff_checker.isSmallDiff()?"true":"false") << ", max_diff : " << refzmp_diff_checker.getMaxValue()*1e3 << "[mm], thre : " << refzmp_diff_checker.getDiffThre()*1e3 << "[mm]" << std::endl;
682  std::cerr << " CartZMP diff : " << (cartzmp_diff_checker.isSmallDiff()?"true":"false") << ", max_diff : " << cartzmp_diff_checker.getMaxValue()*1e3 << "[mm], thre : " << cartzmp_diff_checker.getDiffThre()*1e3 << "[mm]" << std::endl;
683  std::cerr << " COG diff : " << (cog_diff_checker.isSmallDiff()?"true":"false") << ", max_diff : " << cog_diff_checker.getMaxValue()*1e3 << "[mm], thre : " << cog_diff_checker.getDiffThre()*1e3 << "[mm]" <<std::endl;
684  std::cerr << " FootPos diff : " << (footpos_diff_checker.isSmallDiff()?"true":"false") << ", max_diff : " << footpos_diff_checker.getMaxValue()*1e3 << "[mm], thre : " << footpos_diff_checker.getDiffThre()*1e3 << "[mm]" <<std::endl;
685  std::cerr << " FootRot diff : " << (footrot_diff_checker.isSmallDiff()?"true":"false") << ", max_diff : " << rad2deg(footrot_diff_checker.getMaxValue()) << "[deg], thre : " << rad2deg(footrot_diff_checker.getDiffThre()) << "[deg]" <<std::endl;
686  std::cerr << " FootPosVel diff : " << (footposvel_diff_checker.isSmallDiff()?"true":"false") << ", max_diff : " << footposvel_diff_checker.getMaxValue()*1e3 << "[mm/s], thre : " << footposvel_diff_checker.getDiffThre()*1e3 << "[mm/s]" <<std::endl;
687  std::cerr << " FootRotVel diff : " << (footrotvel_diff_checker.isSmallDiff()?"true":"false") << ", max_diff : " << rad2deg(footrotvel_diff_checker.getMaxValue()) << "[deg/s], thre : " << rad2deg(footrotvel_diff_checker.getDiffThre()) << "[deg/s]" <<std::endl;
688  std::cerr << " SwingSupportFootMidPos diff : " << (ssmcpos_diff_checker.isSmallDiff()?"true":"false") << ", max_diff : " << ssmcpos_diff_checker.getMaxValue()*1e3 << "[mm], thre : " << ssmcpos_diff_checker.getDiffThre()*1e3 << "[mm]" <<std::endl;
689  std::cerr << " SwingSupportFootMidRot diff : " << (ssmcrot_diff_checker.isSmallDiff()?"true":"false") << ", max_diff : " << rad2deg(ssmcrot_diff_checker.getMaxValue()) << "[deg], thre : " << rad2deg(ssmcrot_diff_checker.getDiffThre()) << "[deg]" <<std::endl;
690  std::cerr << " SwingSupportFootMidPosVel diff : " << (ssmcposvel_diff_checker.isSmallDiff()?"true":"false") << ", max_diff : " << ssmcposvel_diff_checker.getMaxValue()*1e3 << "[mm/s], thre : " << ssmcposvel_diff_checker.getDiffThre()*1e3 << "[mm/s]" <<std::endl;
691  std::cerr << " SwingSupportFootMidRotVel diff : " << (ssmcrotvel_diff_checker.isSmallDiff()?"true":"false") << ", max_diff : " << rad2deg(ssmcrotvel_diff_checker.getMaxValue()) << "[deg/s], thre : " << rad2deg(ssmcrotvel_diff_checker.getDiffThre()) << "[deg/s]" <<std::endl;
692  std::cerr << " ZMPOffset diff : " << (zmpoffset_diff_checker.isSmallDiff()?"true":"false") << ", max_diff : " << zmpoffset_diff_checker.getMaxValue()*1e3 << "[mm/s], thre : " << zmpoffset_diff_checker.getDiffThre()*1e3 << "[mm/s]" <<std::endl;
693  std::cerr << " Contact states & swing support time validity : " << (is_contact_states_swing_support_time_validity?"true":"false") << std::endl;
694  std::cerr << " Step time validity : " << (is_step_time_valid?"true":"false") << std::endl;
695  std::cerr << " ToeHeel angle validity : " << (is_toe_heel_dif_angle_valid?"true":"false") << std::endl;
696  std::cerr << " ToeHeel+ZMPoffset validity : " << (is_toe_heel_zmp_offset_x_valid?"true":"false") << std::endl;
697  };
698 
700  {
701  // Check if start/end COG(xy) is sufficiently close to REFZMP(xy).
702  std::vector<size_t> neglect_index = boost::assign::list_of(2);
703  cogzmp_error_checker.checkValueError(gg->get_cog(), gg->get_refzmp(), neglect_index);
704  };
705 
707  {
708  // Check if start/end COG(xy) is sufficiently close to REFZMP(xy).
709  std::vector<size_t> neglect_index = boost::assign::list_of(2);
710  cogzmp_error_checker.checkValueError(gg->get_cog(), gg->get_refzmp(), neglect_index);
711  // Check step times by comparing calculated step time vs input footsteps step times
712  std::vector< std::vector<step_node> > fsl;
713  gg->get_footstep_nodes_list(fsl);
714  is_step_time_valid = step_time_list.size() == fsl.size();
715  if (is_step_time_valid) {
716  for (size_t i = 0; i < step_time_list.size(); i++) {
717  is_step_time_valid = eps_eq(step_time_list[i], fsl[i][0].step_time, 1e-5) && is_step_time_valid;
718  }
719  }
720  // Check toe heel angle by comparing calculated toe heel angle (min/max = heel/toe) vs input footsteps toe heel angles
721  is_toe_heel_dif_angle_valid = (min_toe_heel_dif_angle_list.size() == fsl.size()) && (max_toe_heel_dif_angle_list.size() == fsl.size());
722  if (is_toe_heel_dif_angle_valid) {
723  if (gg->get_use_toe_heel_auto_set()) {
724  // TODO : not implemented yet
725  } else {
726  //for (size_t i = 0; i < min_toe_heel_dif_angle_list.size(); i++) {
727  for (size_t i = 1; i < min_toe_heel_dif_angle_list.size(); i++) {
728  // std::cerr << "[" << min_toe_heel_dif_angle_list[i] << " " << max_toe_heel_dif_angle_list[i] << " "
729  // << -fsl[i][0].heel_angle << " " << fsl[i][0].toe_angle << "]" << std::endl;
730  is_toe_heel_dif_angle_valid = eps_eq(min_toe_heel_dif_angle_list[i], (-fsl[i][0].heel_angle), 1e-5)
731  && eps_eq(max_toe_heel_dif_angle_list[i], fsl[i][0].toe_angle, 1e-5)
732  && is_toe_heel_dif_angle_valid;
733  }
734  }
735  }
736  // Check validity of toe heel + zmp offset
737  // If use_toe_heel_transition is true, use/not-use of toe/heel angle corresponds to use/not-use zmp transition of toe/heel.
738  is_toe_heel_zmp_offset_x_valid = (min_zmp_offset_x_list.size() == fsl.size()) && (max_zmp_offset_x_list.size() == fsl.size());
739  if (gg->get_use_toe_heel_transition()) {
740  for (size_t i = 0; i < min_zmp_offset_x_list.size(); i++) {
741  // std::cerr << "[" << min_zmp_offset_x_list[i] << " " << max_zmp_offset_x_list[i] << " " << gg->get_toe_zmp_offset_x() << " " << gg->get_heel_zmp_offset_x() << "]" << std::endl;
742  bool heel_valid = true;
743  if ( !eps_eq(min_toe_heel_dif_angle_list[i], 0.0, 1e-5) ) { // If use heel, zmp offset should be same as heel zmp offset
744  heel_valid = eps_eq(min_zmp_offset_x_list[i], gg->get_heel_zmp_offset_x(), 1e-5);
745  } else { // If not use heel, zmp offset should not be same as heel zmp offset
746  heel_valid = !eps_eq(min_zmp_offset_x_list[i], gg->get_heel_zmp_offset_x(), 1e-5);
747  }
748  bool toe_valid = true;
749  if ( !eps_eq(max_toe_heel_dif_angle_list[i], 0.0, 1e-5) ) { // If use toe, zmp offset should be same as toe zmp offset
750  toe_valid = eps_eq(max_zmp_offset_x_list[i], gg->get_toe_zmp_offset_x(), 1e-5);
751  } else { // If not use toe, zmp offset should not be same as toe zmp offset
752  toe_valid = !eps_eq(max_zmp_offset_x_list[i], gg->get_toe_zmp_offset_x(), 1e-5);
753  }
754  // std::cerr << heel_valid << " " << toe_valid << std::endl;
755  is_toe_heel_zmp_offset_x_valid = heel_valid && toe_valid && is_toe_heel_zmp_offset_x_valid;
756  }
757  } else {
758  // TODO : not implemented yet
759  }
760  };
761 
762  // Generate and plot walk pattern
763  void gen_and_plot_walk_pattern(const step_node& initial_support_leg_step, const step_node& initial_swing_leg_dst_step)
764  {
765  parse_params(false);
766  gg->print_param();
767  gg->initialize_gait_parameter(cog, boost::assign::list_of(initial_support_leg_step), boost::assign::list_of(initial_swing_leg_dst_step));
768  while ( !gg->proc_one_tick() );
769  //gg->print_footstep_list();
770  /* make step and dump */
771  check_start_values();
772  size_t i = 0;
773  while ( gg->proc_one_tick() ) {
774  proc_one_walking_motion(i);
775  i++;
776  }
777  check_end_values();
778  plot_and_print_errorcheck ();
779  };
780 
782  {
783  std::vector<std::string> tmp_string_vector = boost::assign::list_of("rleg");
784  if (gg->get_footstep_front_leg_names() == tmp_string_vector) {
785  step_node initial_support_leg_step = step_node(LLEG, coordinates(leg_pos[1]), 0, 0, 0, 0);
786  step_node initial_swing_leg_dst_step = step_node(RLEG, coordinates(leg_pos[0]), 0, 0, 0, 0);
787  gen_and_plot_walk_pattern(initial_support_leg_step, initial_swing_leg_dst_step);
788  } else {
789  step_node initial_support_leg_step = step_node(RLEG, coordinates(leg_pos[0]), 0, 0, 0, 0);
790  step_node initial_swing_leg_dst_step = step_node(LLEG, coordinates(leg_pos[1]), 0, 0, 0, 0);
791  gen_and_plot_walk_pattern(initial_support_leg_step, initial_swing_leg_dst_step);
792  }
793  }
794 
795 public:
796  std::vector<std::string> arg_strs;
797  testGaitGenerator(double _dt) : dt(_dt), use_gnuplot(true), use_graph_append(false), is_contact_states_swing_support_time_validity(true),
798  refzmp_diff_checker(20.0*1e-3), cartzmp_diff_checker(20.0*1e-3), cog_diff_checker(10.0*1e-3), // [mm]
799  ssmcpos_diff_checker(10.0*1e-3), ssmcrot_diff_checker(deg2rad(0.1)),
800  ssmcposvel_diff_checker(10.0*1e-3), ssmcrotvel_diff_checker(deg2rad(1)),
801  footpos_diff_checker(10.0*1e-3), footrot_diff_checker(deg2rad(1)),
802  footposvel_diff_checker(40*1e-3), footrotvel_diff_checker(deg2rad(15)),
803  zmpoffset_diff_checker(20.0*1e-3),
804  zmp_error_checker(50*1e-3), cogzmp_error_checker(1.5*1e-3),
805  min_rfoot_pos(1e10,1e10,1e10), min_lfoot_pos(1e10,1e10,1e10), max_rfoot_pos(-1e10,-1e10,-1e10), max_lfoot_pos(-1e10,-1e10,-1e10),
806  min_toe_heel_dif_angle(1e10), max_toe_heel_dif_angle(-1e10), min_zmp_offset_x(1e10), max_zmp_offset_x(-1e10),
807  prev_contact_states(2, true), // RLEG, LLEG
808  prev_swing_support_time(2, 1e2), // RLEG, LLEG
809  fname_cogzmp("/tmp/plot-cogzmp.dat"), fp_cogzmp(fopen(fname_cogzmp.c_str(), "w")),
810  fname_fpos("/tmp/plot-fpos.dat"), fp_fpos(fopen(fname_fpos.c_str(), "w")),
811  fname_frot("/tmp/plot-frot.dat"), fp_frot(fopen(fname_frot.c_str(), "w")),
812  fname_zoff("/tmp/plot-zoff.dat"), fp_zoff(fopen(fname_zoff.c_str(), "w")),
813  fname_fposvel("/tmp/plot-fposvel.dat"), fp_fposvel(fopen(fname_fposvel.c_str(), "w")),
814  fname_frotvel("/tmp/plot-frotvel.dat"), fp_frotvel(fopen(fname_frotvel.c_str(), "w")),
815  fname_thpos("/tmp/plot-thpos.dat"), fp_thpos(fopen(fname_thpos.c_str(), "w")),
816  fname_sstime("/tmp/plot-sstime.dat"), fp_sstime(fopen(fname_sstime.c_str(), "w")),
817  fname_ssmc("/tmp/plot-ssmc.dat"), fp_ssmc(fopen(fname_ssmc.c_str(), "w")),
818  fname_ssmcvel("/tmp/plot-ssmcvel.dat"), fp_ssmcvel(fopen(fname_ssmcvel.c_str(), "w"))
819  {};
820 
822  {
823  if (gg != NULL) {
824  delete gg;
825  gg = NULL;
826  }
827  fclose(fp_cogzmp);
828  fclose(fp_fpos);
829  fclose(fp_frot);
830  fclose(fp_zoff);
831  fclose(fp_fposvel);
832  fclose(fp_frotvel);
833  fclose(fp_thpos);
834  fclose(fp_sstime);
835  fclose(fp_ssmc);
836  fclose(fp_ssmcvel);
837  };
838 
839  void test0 ()
840  {
841  test_doc_string = "test0 : Set foot steps";
842  /* initialize sample footstep_list */
843  parse_params();
844  std::vector< std::vector<step_node> > fnsl;
845  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(0, 0, 0)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
846  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(0, 0, 0)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
847  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(100*1e-3, 0, 0)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
848  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(200*1e-3, 0, 0)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
849  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(200*1e-3, 0, 0)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
850  gg->set_foot_steps_list(fnsl);
851  gen_and_plot_walk_pattern();
852  };
853 
854  void test1 ()
855  {
856  test_doc_string = "test1 : Go pos x,y,th combination";
857  /* initialize sample footstep_list */
858  parse_params();
860  coordinates start_ref_coords;
861  mid_coords(start_ref_coords, 0.5, coordinates(leg_pos[0]), coordinates(leg_pos[1]));
862  gg->go_pos_param_2_footstep_nodes_list(200*1e-3, 100*1e-3, 20, boost::assign::list_of(coordinates(leg_pos[0])), start_ref_coords, boost::assign::list_of(RLEG));
863  gen_and_plot_walk_pattern();
864  };
865 
866  void test2 ()
867  {
868  test_doc_string = "test2 : Go pos x";
869  /* initialize sample footstep_list */
870  parse_params();
872  coordinates start_ref_coords;
873  mid_coords(start_ref_coords, 0.5, coordinates(leg_pos[1]), coordinates(leg_pos[0]));
874  gg->go_pos_param_2_footstep_nodes_list(300*1e-3, 0, 0, boost::assign::list_of(coordinates(leg_pos[1])), start_ref_coords, boost::assign::list_of(LLEG));
875  gen_and_plot_walk_pattern();
876  };
877 
878  void test3 ()
879  {
880  test_doc_string = "test3 : Go pos y";
881  /* initialize sample footstep_list */
882  parse_params();
884  coordinates start_ref_coords;
885  mid_coords(start_ref_coords, 0.5, coordinates(leg_pos[0]), coordinates(leg_pos[1]));
886  gg->go_pos_param_2_footstep_nodes_list(0, 150*1e-3, 0, boost::assign::list_of(coordinates(leg_pos[0])), start_ref_coords, boost::assign::list_of(RLEG));
887  gen_and_plot_walk_pattern();
888  };
889 
890  void test4 ()
891  {
892  test_doc_string = "test4 : Go pos th";
893  /* initialize sample footstep_list */
894  parse_params();
896  coordinates start_ref_coords;
897  mid_coords(start_ref_coords, 0.5, coordinates(leg_pos[1]), coordinates(leg_pos[0]));
898  gg->go_pos_param_2_footstep_nodes_list(0, 0, 30, boost::assign::list_of(coordinates(leg_pos[1])), start_ref_coords, boost::assign::list_of(LLEG));
899  gen_and_plot_walk_pattern();
900  };
901 
902  void test5 ()
903  {
904  test_doc_string = "test5 : Set foot steps with Z change";
905  /* initialize sample footstep_list */
906  parse_params();
908  std::vector< std::vector<step_node> > fnsl;
909  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(0, 0, 0)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
910  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(0, 0, 0)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
911  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(100*1e-3, 0, 100*1e-3)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
912  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(200*1e-3, 0, 200*1e-3)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
913  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(300*1e-3, 0, 300*1e-3)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
914  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(300*1e-3, 0, 300*1e-3)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
915  gg->set_foot_steps_list(fnsl);
916  gen_and_plot_walk_pattern();
917  };
918 
919  void test6 ()
920  {
921  test_doc_string = "test6 : Go single step";
922  parse_params();
924  std::vector< std::vector<step_node> > fnsl;
925  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(0, 0, 0)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
926  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(100*1e-3, 75*1e-3, 0)+leg_pos[1]), hrp::rotFromRpy(hrp::Vector3(deg2rad(5), deg2rad(-20), deg2rad(10)))),
928  gg->set_foot_steps_list(fnsl);
929  gen_and_plot_walk_pattern();
930  };
931 
932  void test7 ()
933  {
934  test_doc_string = "test7 : Toe heel walk";
935  /* initialize sample footstep_list */
936  parse_params();
937  std::vector<hrp::Vector3> dzo;
938  dzo.push_back(hrp::Vector3(20*1e-3,-30*1e-3,0));
939  dzo.push_back(hrp::Vector3(20*1e-3,30*1e-3,0));
940  gg->set_default_zmp_offsets(dzo);
941  gg->set_toe_zmp_offset_x(137*1e-3);
942  gg->set_heel_zmp_offset_x(-105*1e-3);
943  gg->set_toe_pos_offset_x(137*1e-3);
944  gg->set_heel_pos_offset_x(-105*1e-3);
945  gg->set_stride_parameters(0.2,0.1,20,0.2,0.1*0.5,20*0.5);
946  gg->set_use_toe_heel_auto_set(true);
947  gg->set_toe_angle(30);
948  gg->set_heel_angle(10);
949  // gg->set_use_toe_heel_transition(false);
950  gg->set_use_toe_heel_transition(true);
952  coordinates start_ref_coords;
953  mid_coords(start_ref_coords, 0.5, coordinates(leg_pos[1]), coordinates(leg_pos[0]));
954  gg->go_pos_param_2_footstep_nodes_list(400*1e-3, 0, 0, boost::assign::list_of(coordinates(leg_pos[1])), start_ref_coords, boost::assign::list_of(LLEG));
955  gen_and_plot_walk_pattern();
956  };
957 
958  void test8 ()
959  {
960  test_doc_string = "test8 : Toe heel walk on slope";
961  /* initialize sample footstep_list */
962  parse_params();
963  std::vector<hrp::Vector3> dzo;
964  dzo.push_back(hrp::Vector3(20*1e-3,-30*1e-3,0));
965  dzo.push_back(hrp::Vector3(20*1e-3,30*1e-3,0));
966  gg->set_default_zmp_offsets(dzo);
967  gg->set_toe_zmp_offset_x(137*1e-3);
968  gg->set_heel_zmp_offset_x(-105*1e-3);
969  gg->set_toe_pos_offset_x(137*1e-3);
970  gg->set_heel_pos_offset_x(-105*1e-3);
971  gg->set_toe_angle(30);
972  gg->set_heel_angle(10);
973  // gg->set_use_toe_heel_transition(false);
974  gg->set_use_toe_heel_transition(true);
976  hrp::Matrix33 initial_foot_mid_rot = Eigen::AngleAxis<double>(M_PI/2, hrp::Vector3::UnitZ()).toRotationMatrix();
977  //hrp::Matrix33 initial_foot_mid_rot = Eigen::AngleAxis<double>(M_PI, hrp::Vector3::UnitZ()).toRotationMatrix();
978  coordinates start_ref_coords;
979  mid_coords(start_ref_coords, 0.5, coordinates(initial_foot_mid_rot*leg_pos[1], initial_foot_mid_rot), coordinates(initial_foot_mid_rot*leg_pos[0], initial_foot_mid_rot));
980  gg->go_pos_param_2_footstep_nodes_list(100*1e-3, 0, 0, boost::assign::list_of(coordinates(initial_foot_mid_rot*leg_pos[1], initial_foot_mid_rot)), start_ref_coords, boost::assign::list_of(LLEG));
981 
982  std::vector<std::string> tmp_string_vector = boost::assign::list_of("rleg");
983  if (gg->get_footstep_front_leg_names() == tmp_string_vector) {
984  step_node initial_support_leg_step = step_node(LLEG, coordinates(hrp::Vector3(initial_foot_mid_rot * leg_pos[1]), initial_foot_mid_rot), 0, 0, 0, 0);
985  step_node initial_swing_leg_dst_step = step_node(RLEG, coordinates(hrp::Vector3(initial_foot_mid_rot * leg_pos[0]), initial_foot_mid_rot), 0, 0, 0, 0);
986  gen_and_plot_walk_pattern(initial_support_leg_step, initial_swing_leg_dst_step);
987  } else {
988  step_node initial_support_leg_step = step_node(RLEG, coordinates(hrp::Vector3(initial_foot_mid_rot * leg_pos[0]), initial_foot_mid_rot), 0, 0, 0, 0);
989  step_node initial_swing_leg_dst_step = step_node(LLEG, coordinates(hrp::Vector3(initial_foot_mid_rot * leg_pos[1]), initial_foot_mid_rot), 0, 0, 0, 0);
990  gen_and_plot_walk_pattern(initial_support_leg_step, initial_swing_leg_dst_step);
991  }
992  };
993 
994  void test9 ()
995  {
996  test_doc_string = "test9 : Stair walk";
997  /* initialize sample footstep_list */
998  parse_params();
1002  std::vector< std::vector<step_node> > fnsl;
1003  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(0, 0, 0)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1004  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(0, 0, 0)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1005  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(250*1e-3, 0, 200*1e-3)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1006  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(250*1e-3, 0, 200*1e-3)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1007  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(500*1e-3, 0, 400*1e-3)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1008  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(500*1e-3, 0, 400*1e-3)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1009  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(750*1e-3, 0, 600*1e-3)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1010  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(750*1e-3, 0, 600*1e-3)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1011  gg->set_foot_steps_list(fnsl);
1012  gen_and_plot_walk_pattern();
1013  };
1014 
1015  void test10 ()
1016  {
1017  test_doc_string = "test10 : Stair walk + toe heel contact";
1018  /* initialize sample footstep_list */
1019  parse_params();
1023  gg->set_toe_zmp_offset_x(137*1e-3);
1024  gg->set_heel_zmp_offset_x(-105*1e-3);
1025  gg->set_toe_pos_offset_x(137*1e-3);
1026  gg->set_heel_pos_offset_x(-105*1e-3);
1027  gg->set_toe_angle(20);
1028  gg->set_heel_angle(5);
1029  gg->set_default_step_time(1.5);
1032  double ratio[7] = {0.02, 0.28, 0.2, 0.0, 0.2, 0.25, 0.05};
1033  std::vector<double> ratio2(ratio, ratio+gg->get_NUM_TH_PHASES());
1034  gg->set_toe_heel_phase_ratio(ratio2);
1035  std::vector< std::vector<step_node> > fnsl;
1036  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(0, 0, 0)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1037  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(0, 0, 0)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1038  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(250*1e-3, 0, 200*1e-3)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1039  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(250*1e-3, 0, 200*1e-3)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1040  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(500*1e-3, 0, 400*1e-3)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1041  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(500*1e-3, 0, 400*1e-3)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1042  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(750*1e-3, 0, 600*1e-3)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1043  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(750*1e-3, 0, 600*1e-3)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1044  gg->set_foot_steps_list(fnsl);
1045  gen_and_plot_walk_pattern();
1046  };
1047 
1048  void test11 ()
1049  {
1050  test_doc_string = "test11 : Foot rot change";
1051  /* initialize sample footstep_list */
1052  parse_params();
1053  hrp::Matrix33 tmpr;
1054  std::vector< std::vector<step_node> > fnsl;
1055  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(0, 0, 0)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1056  tmpr = hrp::rotFromRpy(5*M_PI/180.0, 15*M_PI/180.0, 0);
1057  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(250*1e-3, 0, 0*1e-3)+leg_pos[0]), tmpr), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1058  tmpr = hrp::rotFromRpy(-5*M_PI/180.0, -15*M_PI/180.0, 0);
1059  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(250*1e-3, 0, 0*1e-3)+leg_pos[1]), tmpr), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1060  gg->set_foot_steps_list(fnsl);
1061  gen_and_plot_walk_pattern();
1062  };
1063 
1064  void test12 ()
1065  {
1066  test_doc_string = "test12 : Change step param in set foot steps";
1067  /* initialize sample footstep_list */
1068  parse_params();
1069  std::vector< std::vector<step_node> > fnsl;
1070  gg->set_default_step_time(4.0); // dummy
1071  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(0, 0, 0)+leg_pos[0])), gg->get_default_step_height(), 1.0, 0, 0)));
1072  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(0, 0, 0)+leg_pos[1])), gg->get_default_step_height(), 2.0, 0, 0)));
1073  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(100*1e-3, 0, 0)+leg_pos[0])), gg->get_default_step_height()*2, 1.5, 0, 0)));
1074  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(200*1e-3, 0, 0)+leg_pos[1])), gg->get_default_step_height(), 2.5, 0, 0)));
1075  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(300*1e-3, 0, 0)+leg_pos[0])), gg->get_default_step_height(), 1.0, 20, 5)));
1076  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(300*1e-3, 0, 0)+leg_pos[1])), gg->get_default_step_height(), 2.0, 0, 0)));
1077  gg->set_foot_steps_list(fnsl);
1078  gen_and_plot_walk_pattern();
1079  };
1080 
1081  void test13 ()
1082  {
1083  test_doc_string = "test13 : Arbitrary leg switching";
1084  /* initialize sample footstep_list */
1085  parse_params();
1086  std::vector< std::vector<step_node> > fnsl;
1087  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(0, 0, 0)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), 0, 0)));
1088  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(0, 0, 0)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), 0, 0)));
1089  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(100*1e-3, 0, 0)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), 0, 0)));
1090  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(100*1e-3, 0, 0)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), 0, 0)));
1091  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(200*1e-3, 0, 0)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), 0, 0)));
1092  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(200*1e-3, 0, 0)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), 0, 0)));
1093  gg->set_foot_steps_list(fnsl);
1094  gen_and_plot_walk_pattern();
1095  };
1096 
1097  void test14 ()
1098  {
1099  test_doc_string = "test14 : kick walk";
1100  /* initialize sample footstep_list */
1101  parse_params();
1104  coordinates start_ref_coords;
1105  mid_coords(start_ref_coords, 0.5, coordinates(leg_pos[1]), coordinates(leg_pos[0]));
1106  gg->go_pos_param_2_footstep_nodes_list(300*1e-3, 0, 0, boost::assign::list_of(coordinates(leg_pos[1])), start_ref_coords, boost::assign::list_of(LLEG));
1107  gen_and_plot_walk_pattern();
1108  };
1109 
1110  void test15 ()
1111  {
1112  test_doc_string = "test15 : Stair walk down";
1113  /* initialize sample footstep_list */
1114  parse_params();
1118  std::vector< std::vector<step_node> > fnsl;
1119  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(0, 0, 0)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1120  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(0, 0, 0)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1121  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(250*1e-3, 0, -200*1e-3)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1122  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(250*1e-3, 0, -200*1e-3)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1123  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(500*1e-3, 0, -400*1e-3)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1124  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(500*1e-3, 0, -400*1e-3)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1125  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(750*1e-3, 0, -600*1e-3)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1126  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(750*1e-3, 0, -600*1e-3)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle())));
1127  gg->set_foot_steps_list(fnsl);
1128  gen_and_plot_walk_pattern();
1129  };
1130 
1131  void test16 ()
1132  {
1133  test_doc_string = "test16 : Set foot steps with param (toe heel contact)";
1134  /* initialize sample footstep_list */
1135  parse_params();
1137  std::vector<hrp::Vector3> dzo;
1138  dzo.push_back(hrp::Vector3(20*1e-3,-30*1e-3,0));
1139  dzo.push_back(hrp::Vector3(20*1e-3,30*1e-3,0));
1140  gg->set_default_zmp_offsets(dzo);
1141  gg->set_toe_zmp_offset_x(137*1e-3);
1142  gg->set_heel_zmp_offset_x(-105*1e-3);
1143  gg->set_toe_pos_offset_x(137*1e-3);
1144  gg->set_heel_pos_offset_x(-105*1e-3);
1145  gg->set_toe_angle(20);
1146  gg->set_heel_angle(5);
1147  gg->set_default_step_time(2);
1150  gg->set_use_toe_heel_auto_set(true);
1151  gg->set_use_toe_heel_transition(true);
1152  //double ratio[7] = {0.02, 0.28, 0.2, 0.0, 0.2, 0.25, 0.05};
1153  double ratio[7] = {0.07, 0.20, 0.2, 0.0, 0.2, 0.25, 0.08};
1154  std::vector<double> ratio2(ratio, ratio+gg->get_NUM_TH_PHASES());
1155  gg->set_toe_heel_phase_ratio(ratio2);
1156  std::vector< std::vector<step_node> > fnsl;
1157  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(0, 0, 0)+leg_pos[0])), gg->get_default_step_height()*0.5, gg->get_default_step_time()*0.5, gg->get_toe_angle()*0.5, gg->get_heel_angle()*0.5)));
1158  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(100*1e-3, 0, 0)+leg_pos[1])), gg->get_default_step_height()*2.0, gg->get_default_step_time()*2.0, gg->get_toe_angle()*2.0, gg->get_heel_angle()*2.0)));
1159  fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(400*1e-3, 0, 0)+leg_pos[0])), gg->get_default_step_height()*0.5, gg->get_default_step_time()*0.5, gg->get_toe_angle()*0.5, gg->get_heel_angle()*0.5)));
1160  fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(400*1e-3, 0, 0)+leg_pos[1])), gg->get_default_step_height()*2.0, gg->get_default_step_time()*2.0, gg->get_toe_angle()*2.0, gg->get_heel_angle()*2.0)));
1161  gg->set_foot_steps_list(fnsl);
1162  gen_and_plot_walk_pattern();
1163  };
1164 
1165  void test17 ()
1166  {
1167  test_doc_string = "test17 : Test goVelocity (dx = 0.1, dy = 0.05, dth = 10.0)";
1168  /* initialize sample footstep_list */
1169  parse_params();
1171  gg->print_param();
1172  step_node initial_support_leg_step = step_node(LLEG, coordinates(leg_pos[1]), 0, 0, 0, 0);
1173  step_node initial_swing_leg_dst_step = step_node(RLEG, coordinates(leg_pos[0]), 0, 0, 0, 0);
1174  coordinates fm_coords;
1175  mid_coords(fm_coords, 0.5, initial_swing_leg_dst_step.worldcoords, initial_support_leg_step.worldcoords);
1176  gg->initialize_velocity_mode(fm_coords, 0.1, 0.05, 10.0, boost::assign::list_of(RLEG));
1177  gg->initialize_gait_parameter(cog, boost::assign::list_of(initial_support_leg_step), boost::assign::list_of(initial_swing_leg_dst_step));
1178  while ( !gg->proc_one_tick() );
1179  /* make step and dump */
1180  check_start_values();
1181  size_t i = 0;
1182  while ( gg->proc_one_tick() ) {
1183  proc_one_walking_motion(i);
1184  i++;
1185  if ( i > static_cast<size_t>(gg->get_default_step_time()/dt) && gg->get_overwrite_check_timing() ) {
1186  gg->finalize_velocity_mode();
1187  }
1188  }
1189  check_end_values();
1190  plot_and_print_errorcheck ();
1191  };
1192 
1193  void test18 ()
1194  {
1195  test_doc_string = "test18 : Test goVelocity with changing velocity (translation and rotation)";
1196  /* initialize sample footstep_list */
1197  parse_params();
1201  gg->print_param();
1202  step_node initial_support_leg_step = step_node(LLEG, coordinates(leg_pos[1]), 0, 0, 0, 0);
1203  step_node initial_swing_leg_dst_step = step_node(RLEG, coordinates(leg_pos[0]), 0, 0, 0, 0);
1204  coordinates fm_coords;
1205  mid_coords(fm_coords, 0.5, initial_swing_leg_dst_step.worldcoords, initial_support_leg_step.worldcoords);
1206  gg->initialize_velocity_mode(fm_coords, 0, 0, 0, boost::assign::list_of(RLEG));
1207  gg->initialize_gait_parameter(cog, boost::assign::list_of(initial_support_leg_step), boost::assign::list_of(initial_swing_leg_dst_step));
1208  while ( !gg->proc_one_tick() );
1209  /* make step and dump */
1210  check_start_values();
1211  size_t i = 0;
1212  while ( gg->proc_one_tick() ) {
1213  proc_one_walking_motion(i);
1214  i++;
1215  if ( i > static_cast<size_t>(gg->get_default_step_time()/dt)*5 && gg->get_overwrite_check_timing() ) {
1216  gg->finalize_velocity_mode();
1217  } else if ( i > static_cast<size_t>(gg->get_default_step_time()/dt)*4 && gg->get_overwrite_check_timing() ) {
1218  gg->set_velocity_param(0, 0, 0);
1219  } else if ( i > static_cast<size_t>(gg->get_default_step_time()/dt)*3 && gg->get_overwrite_check_timing() ) {
1220  gg->set_velocity_param(0, 0, 10);
1221  } else if ( i > static_cast<size_t>(gg->get_default_step_time()/dt)*2 && gg->get_overwrite_check_timing() ) {
1222  gg->set_velocity_param(0, 0, 0);
1223  } else if ( i > static_cast<size_t>(gg->get_default_step_time()/dt) && gg->get_overwrite_check_timing() ) {
1224  gg->set_velocity_param(0.1, 0.05, 0);
1225  }
1226  }
1227  check_end_values();
1228  plot_and_print_errorcheck ();
1229  };
1230 
1231  void test19 ()
1232  {
1233  test_doc_string = "test19 : Change stride parameter (translate)";
1234  /* initialize sample footstep_list */
1235  parse_params();
1236  std::vector<hrp::Vector3> dzo;
1237  dzo.push_back(hrp::Vector3(20*1e-3,-30*1e-3,0));
1238  dzo.push_back(hrp::Vector3(20*1e-3,30*1e-3,0));
1239  gg->set_default_zmp_offsets(dzo);
1240  gg->set_stride_parameters(0.25,0.15,25,0.25,0.1,10);
1242  coordinates start_ref_coords;
1243  mid_coords(start_ref_coords, 0.5, coordinates(leg_pos[1]), coordinates(leg_pos[0]));
1244  gg->go_pos_param_2_footstep_nodes_list(600*1e-3, -300*1e-3, 0, boost::assign::list_of(coordinates(leg_pos[1])), start_ref_coords, boost::assign::list_of(LLEG));
1245  gen_and_plot_walk_pattern();
1246  };
1247 
1248  void test20 ()
1249  {
1250  test_doc_string = "test20 : Change stride parameter (translate+rotate)";
1251  /* initialize sample footstep_list */
1252  parse_params();
1253  std::vector<hrp::Vector3> dzo;
1254  dzo.push_back(hrp::Vector3(20*1e-3,-30*1e-3,0));
1255  dzo.push_back(hrp::Vector3(20*1e-3,30*1e-3,0));
1256  gg->set_default_zmp_offsets(dzo);
1257  gg->set_stride_parameters(0.25,0.15,25,0.25,0.1,10);
1259  coordinates start_ref_coords;
1260  mid_coords(start_ref_coords, 0.5, coordinates(leg_pos[1]), coordinates(leg_pos[0]));
1261  gg->go_pos_param_2_footstep_nodes_list(400*1e-3, -200*1e-3, -55, boost::assign::list_of(coordinates(leg_pos[1])), start_ref_coords, boost::assign::list_of(LLEG));
1262  gen_and_plot_walk_pattern();
1263  };
1264 
1265  void parse_params (bool is_print_doc_setring = true)
1266  {
1267  if (is_print_doc_setring) std::cerr << test_doc_string << std::endl;
1268  for (unsigned int i = 0; i < arg_strs.size(); ++ i) {
1269  if ( arg_strs[i]== "--default-step-time" ) {
1270  if (++i < arg_strs.size()) gg->set_default_step_time(atof(arg_strs[i].c_str()));
1271  } else if ( arg_strs[i]== "--default-step-height" ) {
1272  if (++i < arg_strs.size()) gg->set_default_step_height(atof(arg_strs[i].c_str()));
1273  } else if ( arg_strs[i]== "--default-double-support-ratio-before" ) {
1274  if (++i < arg_strs.size()) gg->set_default_double_support_ratio_before(atof(arg_strs[i].c_str()));
1275  } else if ( arg_strs[i]== "--default-double-support-ratio-after" ) {
1276  if (++i < arg_strs.size()) gg->set_default_double_support_ratio_after(atof(arg_strs[i].c_str()));
1277  } else if ( arg_strs[i]== "--default-orbit-type" ) {
1278  if (++i < arg_strs.size()) {
1279  if (arg_strs[i] == "SHUFFLING") {
1281  } else if (arg_strs[i] == "CYCLOID") {
1283  } else if (arg_strs[i] == "RECTANGLE") {
1285  } else if (arg_strs[i] == "STAIR") {
1287  } else if (arg_strs[i] == "CYCLOIDDELAY") {
1289  } else if (arg_strs[i] == "CYCLOIDDELAYKICK") {
1291  } else if (arg_strs[i] == "CROSS") {
1293  } else {
1294  std::cerr << "No such default-orbit-type " << arg_strs[i] << std::endl;
1295  }
1296  }
1297  } else if ( arg_strs[i]== "--default-double-support-static-ratio-before" ) {
1298  if (++i < arg_strs.size()) gg->set_default_double_support_static_ratio_before(atof(arg_strs[i].c_str()));
1299  } else if ( arg_strs[i]== "--default-double-support-static-ratio-after" ) {
1300  if (++i < arg_strs.size()) gg->set_default_double_support_static_ratio_after(atof(arg_strs[i].c_str()));
1301  } else if ( arg_strs[i]== "--default-double-support-ratio-swing-before" ) {
1302  if (++i < arg_strs.size()) gg->set_default_double_support_ratio_swing_before(atof(arg_strs[i].c_str()));
1303  } else if ( arg_strs[i]== "--default-double-support-ratio-swing-after" ) {
1304  if (++i < arg_strs.size()) gg->set_default_double_support_ratio_swing_after(atof(arg_strs[i].c_str()));
1305  } else if ( arg_strs[i]== "--swing-trajectory-delay-time-offset" ) {
1306  if (++i < arg_strs.size()) gg->set_swing_trajectory_delay_time_offset(atof(arg_strs[i].c_str()));
1307  } else if ( arg_strs[i]== "--swing-trajectory-final-distance-weight" ) {
1308  if (++i < arg_strs.size()) gg->set_swing_trajectory_final_distance_weight(atof(arg_strs[i].c_str()));
1309  } else if ( arg_strs[i]== "--swing-trajectory-time-offset-xy2z" ) {
1310  if (++i < arg_strs.size()) gg->set_swing_trajectory_time_offset_xy2z(atof(arg_strs[i].c_str()));
1311  } else if ( arg_strs[i]== "--stair-trajectory-way-point-offset" ) {
1312  if (++i < arg_strs.size()) {
1313  coil::vstring strs = coil::split(std::string(arg_strs[i].c_str()), ",");
1314  gg->set_stair_trajectory_way_point_offset(hrp::Vector3(atof(strs[0].c_str()), atof(strs[1].c_str()), atof(strs[2].c_str())));
1315  }
1316  } else if ( arg_strs[i]== "--cycloid-delay-kick-point-offset" ) {
1317  if (++i < arg_strs.size()) {
1318  coil::vstring strs = coil::split(std::string(arg_strs[i].c_str()), ",");
1319  gg->set_cycloid_delay_kick_point_offset(hrp::Vector3(atof(strs[0].c_str()), atof(strs[1].c_str()), atof(strs[2].c_str())));
1320  }
1321  } else if ( arg_strs[i]== "--toe-angle" ) {
1322  if (++i < arg_strs.size()) gg->set_toe_angle(atof(arg_strs[i].c_str()));
1323  } else if ( arg_strs[i]== "--heel-angle" ) {
1324  if (++i < arg_strs.size()) gg->set_heel_angle(atof(arg_strs[i].c_str()));
1325  } else if ( arg_strs[i]== "--toe-heel-phase-ratio" ) {
1326  if (++i < arg_strs.size()) {
1327  coil::vstring strs = coil::split(std::string(arg_strs[i].c_str()), ",");
1328  std::vector<double> ratio;
1329  for (size_t i_th = 0; i_th < strs.size(); i_th++) {
1330  ratio.push_back(atof(strs[i_th].c_str()));
1331  }
1332  std::cerr << "[] "; // for set_toe_heel_phase_ratio
1333  gg->set_toe_heel_phase_ratio(ratio);
1334  }
1335  } else if ( arg_strs[i]== "--optional-go-pos-finalize-footstep-num" ) {
1336  if (++i < arg_strs.size()) gg->set_optional_go_pos_finalize_footstep_num(atoi(arg_strs[i].c_str()));
1337  } else if ( arg_strs[i]== "--use-gnuplot" ) {
1338  if (++i < arg_strs.size()) use_gnuplot = (arg_strs[i]=="true");
1339  } else if ( arg_strs[i]== "--use-graph-append" ) {
1340  if (++i < arg_strs.size()) use_graph_append = (arg_strs[i]=="true");
1341  }
1342  }
1343  };
1344 
1345  bool check_all_results () const
1346  {
1347  return refzmp_diff_checker.isSmallDiff() && cartzmp_diff_checker.isSmallDiff() && cog_diff_checker.isSmallDiff()
1348  && ssmcpos_diff_checker.isSmallDiff() && ssmcrot_diff_checker.isSmallDiff()
1349  && ssmcposvel_diff_checker.isSmallDiff() && ssmcrotvel_diff_checker.isSmallDiff()
1350  && footpos_diff_checker.isSmallDiff() && footrot_diff_checker.isSmallDiff()
1351  && zmpoffset_diff_checker.isSmallDiff()
1352  && footposvel_diff_checker.isSmallDiff() && footrotvel_diff_checker.isSmallDiff()
1353  && zmp_error_checker.isSmallError() && cogzmp_error_checker.isSmallError()
1354  && is_step_time_valid && is_toe_heel_dif_angle_valid && is_toe_heel_zmp_offset_x_valid
1355  && is_contact_states_swing_support_time_validity;
1356  };
1357 };
1358 
1360 {
1361  public:
1363  {
1364  cog = 1e-3*hrp::Vector3(6.785, 1.54359, 806.831);
1365  leg_pos.push_back(hrp::Vector3(0,1e-3*-105,0)); /* rleg */
1366  leg_pos.push_back(hrp::Vector3(0,1e-3* 105,0)); /* lleg */
1367  all_limbs.push_back("rleg");
1368  all_limbs.push_back("lleg");
1369  gg = new gait_generator(dt, leg_pos, all_limbs, 1e-3*150, 1e-3*50, 10, 1e-3*50, 1e-3*50*0.5, 10*0.5);
1370  };
1371 };
1372 
1374 {
1375  std::cerr << "Usage : testGaitGenerator [test-name] [option]" << std::endl;
1376  std::cerr << " [test-name] should be:" << std::endl;
1377  std::cerr << " --test0 : Set foot steps" << std::endl;
1378  std::cerr << " --test1 : Go pos x,y,th combination" << std::endl;
1379  std::cerr << " --test2 : Go pos x" << std::endl;
1380  std::cerr << " --test3 : Go pos y" << std::endl;
1381  std::cerr << " --test4 : Go pos th" << std::endl;
1382  std::cerr << " --test5 : Set foot steps with Z change" << std::endl;
1383  std::cerr << " --test6 : Go single step" << std::endl;
1384  std::cerr << " --test7 : Toe heel walk" << std::endl;
1385  std::cerr << " --test8 : Toe heel walk on slope" << std::endl;
1386  std::cerr << " --test9 : Stair walk" << std::endl;
1387  std::cerr << " --test10 : Stair walk + toe heel contact" << std::endl;
1388  std::cerr << " --test11 : Foot rot change" << std::endl;
1389  std::cerr << " --test12 : Change step param in set foot steps" << std::endl;
1390  std::cerr << " --test13 : Arbitrary leg switching" << std::endl;
1391  std::cerr << " --test14 : kick walk" << std::endl;
1392  std::cerr << " --test15 : Stair walk down" << std::endl;
1393  std::cerr << " --test16 : Set foot steps with param (toe heel contact)" << std::endl;
1394  std::cerr << " --test17 : Test goVelocity (dx = 0.1, dy = 0.05, dth = 10.0)" << std::endl;
1395  std::cerr << " --test18 : Test goVelocity with changing velocity (translation and rotation)" << std::endl;
1396  std::cerr << " --test19 : Change stride parameter (translate)" << std::endl;
1397  std::cerr << " --test20 : Change stride parameter (translate+rotate)" << std::endl;
1398  std::cerr << " [option] should be:" << std::endl;
1399  std::cerr << " --use-gnuplot : Use gnuplot and dump eps file to /tmp. (true/false, true by default)" << std::endl;
1400  std::cerr << " --use-graph-append : Append generated graph to /tmp/testGaitGenerator.jpg. (true/false, false by default)" << std::endl;
1401  std::cerr << " other options : for GaitGenerator" << std::endl;
1402 };
1403 
1404 int main(int argc, char* argv[])
1405 {
1406  int ret = 0;
1407  if (argc >= 2) {
1409  for (int i = 1; i < argc; ++ i) {
1410  tgg.arg_strs.push_back(std::string(argv[i]));
1411  }
1412  if (std::string(argv[1]) == "--test0") {
1413  tgg.test0();
1414  } else if (std::string(argv[1]) == "--test1") {
1415  tgg.test1();
1416  } else if (std::string(argv[1]) == "--test2") {
1417  tgg.test2();
1418  } else if (std::string(argv[1]) == "--test3") {
1419  tgg.test3();
1420  } else if (std::string(argv[1]) == "--test4") {
1421  tgg.test4();
1422  } else if (std::string(argv[1]) == "--test5") {
1423  tgg.test5();
1424  } else if (std::string(argv[1]) == "--test6") {
1425  tgg.test6();
1426  } else if (std::string(argv[1]) == "--test7") {
1427  tgg.test7();
1428  } else if (std::string(argv[1]) == "--test8") {
1429  tgg.test8();
1430  } else if (std::string(argv[1]) == "--test9") {
1431  tgg.test9();
1432  } else if (std::string(argv[1]) == "--test10") {
1433  tgg.test10();
1434  } else if (std::string(argv[1]) == "--test11") {
1435  tgg.test11();
1436  } else if (std::string(argv[1]) == "--test12") {
1437  tgg.test12();
1438  } else if (std::string(argv[1]) == "--test13") {
1439  tgg.test13();
1440  } else if (std::string(argv[1]) == "--test14") {
1441  tgg.test14();
1442  } else if (std::string(argv[1]) == "--test15") {
1443  tgg.test15();
1444  } else if (std::string(argv[1]) == "--test16") {
1445  tgg.test16();
1446  } else if (std::string(argv[1]) == "--test17") {
1447  tgg.test17();
1448  } else if (std::string(argv[1]) == "--test18") {
1449  tgg.test18();
1450  } else if (std::string(argv[1]) == "--test19") {
1451  tgg.test19();
1452  } else if (std::string(argv[1]) == "--test20") {
1453  tgg.test20();
1454  } else {
1455  print_usage();
1456  ret = 1;
1457  }
1458  ret = (tgg.check_all_results()?0:2);
1459  } else {
1460  print_usage();
1461  ret = 1;
1462  }
1463  return ret;
1464 }
1465 
const std::vector< step_node > & get_support_leg_steps() const
bool go_pos_param_2_footstep_nodes_list(const double goal_x, const double goal_y, const double goal_theta, const std::vector< coordinates > &initial_support_legs_coords, coordinates start_ref_coords, const std::vector< leg_type > &initial_support_legs, const bool is_initialize=true)
#define max(a, b)
void print_param(const std::string &print_str="") const
double get_toe_pos_offset_x() const
double get_heel_zmp_offset_x() const
void set_default_double_support_ratio_swing_before(const double _default_double_support_ratio_swing_before)
void set_default_double_support_static_ratio_after(const double _default_double_support_static_ratio_after)
double calcDiff(T &value) const
double get_toe_zmp_offset_x() const
void set_swing_trajectory_delay_time_offset(const double _time_offset)
void set_heel_angle(const double _angle)
const hrp::Vector3 & get_refzmp() const
void set_default_step_height(const double _tmp)
hrp::Matrix33 rot
Definition: RatsMatrix.h:20
size_t get_overwrite_check_timing() const
ValueErrorChecker(double _thre)
ValueDifferenceChecker< hrp::Vector3 > ssmcrotvel_diff_checker
void parse_params(bool is_print_doc_setring=true)
void set_swing_trajectory_final_distance_weight(const double _final_distance_weight)
size_t get_lcg_count() const
double getMaxValue() const
bool isSmallError() const
int get_NUM_TH_PHASES() const
void set_toe_pos_offset_x(const double _offx)
png_voidp int value
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
bool get_use_toe_heel_auto_set() const
void set_default_double_support_ratio_swing_after(const double _default_double_support_ratio_swing_after)
void get_swing_support_mid_coords(coordinates &ret) const
hrp::Vector3 prev_rfoot_rpy
void set_optional_go_pos_finalize_footstep_num(const size_t num)
testGaitGenerator(double _dt)
vstring split(const std::string &input, const std::string &delimiter, bool ignore_empty)
int main(int argc, char *argv[])
ValueDifferenceChecker< std::vector< hrp::Vector3 > > zmpoffset_diff_checker
void error(char *msg) const
void set_use_toe_heel_transition(const bool _u)
std::vector< bool > prev_contact_states
png_uint_32 i
void set_overwritable_footstep_index_offset(const size_t _of)
std::vector< std::string > all_limbs
bool set_toe_heel_phase_ratio(const std::vector< double > &ratio)
coordinates worldcoords
Definition: GaitGenerator.h:33
void checkValueError(const hrp::Vector3 &p0, const hrp::Vector3 &p1, std::vector< size_t > neglect_index=std::vector< size_t >())
double get_default_step_time() const
Eigen::Vector3d Vector3
void set_default_zmp_offsets(const std::vector< hrp::Vector3 > &tmp)
void initialize_gait_parameter(const hrp::Vector3 &cog, const std::vector< step_node > &initial_support_leg_steps, const std::vector< step_node > &initial_swing_leg_dst_steps, const double delay=1.6)
Matrix33 rotFromRpy(const Vector3 &rpy)
void set_default_double_support_ratio_before(const double _default_double_support_ratio_before)
Eigen::Matrix3d Matrix33
#define min(a, b)
std::vector< std::string > vstring
std::vector< hrp::Vector3 > leg_pos
void set_stair_trajectory_way_point_offset(const hrp::Vector3 _offset)
double get_heel_pos_offset_x() const
std::vector< double > prev_swing_support_time
const std::vector< std::string > get_support_leg_names() const
void set_cycloid_delay_kick_point_offset(const hrp::Vector3 _offset)
const std::vector< hrp::Vector3 > & get_swing_foot_zmp_offsets() const
void set_stride_parameters(const double _stride_fwd_x, const double _stride_outside_y, const double _stride_outside_theta, const double _stride_bwd_x, const double _stride_inside_y, const double _stride_inside_theta)
double get_heel_angle() const
std::vector< std::string > arg_strs
ValueDifferenceChecker(double _diff_thre)
void set_default_double_support_ratio_after(const double _default_double_support_ratio_after)
std::vector< double > step_time_list
void set_velocity_param(const double vel_x, const double vel_y, const double vel_theta)
void plot_and_save(FILE *gp, const std::string graph_fname, const std::string plot_str)
#define deg2rad(deg)
FILE * popen(const char *cmd, const char *mode)
void initialize_velocity_mode(const coordinates &_ref_coords, const double vel_x, const double vel_y, const double vel_theta, const std::vector< leg_type > &current_legs)
void set_default_orbit_type(const orbit_type type)
void set_default_step_time(const double _default_step_time)
#define VEC1(s)
void set_use_toe_heel_auto_set(const bool _u)
void gen_and_plot_walk_pattern(const step_node &initial_support_leg_step, const step_node &initial_swing_leg_dst_step)
double get_current_swing_time(const size_t idx) const
def norm(a)
std::vector< leg_type > get_current_support_states() const
HRP_UTIL_EXPORT Vector3 rpyFromRot(const Matrix33 &m)
bool get_use_toe_heel_transition() const
#define M_PI
double get_toe_angle() const
bool eps_eq(const double a, const double b, const double eps=0.001)
Definition: RatsMatrix.h:8
std::vector< hrp::Vector3 > get_support_foot_zmp_offsets() const
bool check_all_results() const
void set_swing_trajectory_time_offset_xy2z(const double _tmp)
const std::vector< step_node > & get_swing_leg_steps() const
bool is_initialized
Definition: Beeper.cpp:22
void set_toe_zmp_offset_x(const double _off)
hrp::Vector3 min_rfoot_pos
ValueErrorChecker zmp_error_checker
#define rad2deg(rad)
bool is_contact_states_swing_support_time_validity
hrp::Vector3 pos
Definition: RatsMatrix.h:19
std::vector< std::string > get_footstep_front_leg_names() const
void set_toe_angle(const double _angle)
void set_foot_steps_list(const std::vector< std::vector< step_node > > &fnsl)
double get_default_step_height() const
hrp::Vector3 get_cart_zmp() const
void set_heel_zmp_offset_x(const double _off)
void pclose(FILE *fd)
double get_current_toe_heel_ratio() const
void print_usage()
double getErrorThre() const
const hrp::Vector3 & get_cog() const
gait_generator * gg
void proc_one_walking_motion(size_t i)
void set_default_double_support_static_ratio_before(const double _default_double_support_static_ratio_before)
void mid_coords(coordinates &mid_coords, const double p, const coordinates &c1, const coordinates &c2, const double eps)
Definition: RatsMatrix.cpp:58
int usleep(useconds_t usec)
void set_heel_pos_offset_x(const double _offx)


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