robotis_foot_step_generator.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2  * Copyright (c) 2016, ROBOTIS CO., LTD.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright notice, this
9  * list of conditions and the following disclaimer.
10  *
11  * * Redistributions in binary form must reproduce the above copyright notice,
12  * this list of conditions and the following disclaimer in the documentation
13  * and/or other materials provided with the distribution.
14  *
15  * * Neither the name of ROBOTIS nor the names of its
16  * contributors may be used to endorse or promote products derived from
17  * this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
23  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
24  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
25  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
26  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
27  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
28  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29  *******************************************************************************/
30 
31 /*
32  * robotis_foot_step_generator.cpp
33  *
34  * Created on: 2016. 6. 10.
35  * Author: Jay Song
36  */
37 
38 #include <cmath>
39 #include "thormang3_foot_step_generator/robotis_foot_step_generator.h"
40 
41 
42 using namespace thormang3;
43 
44 #define RAD2DEG (M_PI/180.0)
45 
46 double sign(double n)
47 {
48  if(n < 0)
49  return -1;
50  else if(n > 0)
51  return 1;
52  else
53  return 0;
54 }
55 
56 FootStepGenerator::FootStepGenerator()
57 {
58  num_of_step_ = 2*2 + 2;
59  fb_step_length_m_ = 0.1;
60  rl_step_length_m_ = 0.07;
61  rotate_step_angle_rad_ = 10.0*RAD2DEG;
62 
63  step_time_sec_ = 1.0;
64  start_end_time_sec_ = 1.6;
65  dsp_ratio_ = 0.2;
66 
67  foot_z_swap_m_ = 0.1;
68  body_z_swap_m_ = 0.01;
69 
70  default_y_feet_offset_m_ = 0.186;
71 
72  previous_step_type_ = STOP_WALKING;
73 
74  step_data_array_.clear();
75 }
76 
77 
78 FootStepGenerator::~FootStepGenerator()
79 { }
80 
81 void FootStepGenerator::initialize()
82 {
83  previous_step_type_ = STOP_WALKING;
84  step_data_array_.clear();
85 }
86 
87 Eigen::MatrixXd FootStepGenerator::getTransformationXYZRPY(double position_x, double position_y, double position_z, double roll, double pitch, double yaw)
88 {
89  double sr = sin(roll), cr = cos(roll);
90  double sp = sin(pitch), cp = cos(pitch);
91  double sy = sin(yaw), cy = cos(yaw);
92 
93  Eigen::MatrixXd mat_roll(4,4);
94  Eigen::MatrixXd mat_pitch(4,4);
95  Eigen::MatrixXd mat_yaw(4,4);
96 
97  mat_roll <<
98  1, 0, 0, 0,
99  0, cr, -sr, 0,
100  0, sr, cr, 0,
101  0, 0, 0, 1;
102 
103  mat_pitch <<
104  cp, 0, sp, 0,
105  0, 1, 0, 0,
106  -sp, 0, cp, 0,
107  0, 0, 0, 1;
108 
109  mat_yaw <<
110  cy, -sy, 0, 0,
111  sy, cy, 0, 0,
112  0, 0, 1, 0,
113  0, 0, 0, 1;
114 
115  Eigen::MatrixXd mat_xyzrpy = (mat_yaw*mat_pitch)*mat_roll;
116 
117  mat_xyzrpy.coeffRef(0, 3) = position_x;
118  mat_xyzrpy.coeffRef(1, 3) = position_y;
119  mat_xyzrpy.coeffRef(2, 3) = position_z;
120 
121 
122  return mat_xyzrpy;
123 }
124 
125 void FootStepGenerator::getPosefromTransformMatrix(const Eigen::MatrixXd &matTransform, double *position_x, double *position_y, double *position_z, double *roll, double *pitch, double *yaw)
126 {
127  *position_x = matTransform.coeff(0, 3);
128  *position_y = matTransform.coeff(1, 3);
129  *position_z = matTransform.coeff(2, 3);
130  *roll = atan2( matTransform.coeff(2,1), matTransform.coeff(2,2));
131  *pitch = atan2(-matTransform.coeff(2,0), sqrt(matTransform.coeff(2,1)*matTransform.coeff(2,1) + matTransform.coeff(2,2)*matTransform.coeff(2,2)) );
132  *yaw = atan2( matTransform.coeff(1,0), matTransform.coeff(0,0));
133 }
134 
135 thormang3_walking_module_msgs::PoseXYZRPY FootStepGenerator::getPosefromTransformMatrix(const Eigen::MatrixXd &matTransform)
136 {
137  thormang3_walking_module_msgs::PoseXYZRPY pose;
138 
139  double pose_x = 0;
140  double pose_y = 0;
141  double pose_z = 0;
142  double pose_roll = 0;
143  double pose_pitch = 0;
144  double pose_yaw = 0;
145 
146  getPosefromTransformMatrix(matTransform, &pose_x, &pose_y, &pose_z, &pose_roll, &pose_pitch, &pose_yaw);
147 
148  pose.x = pose_x;
149  pose.y = pose_y;
150  pose.z = pose_z;
151  pose.roll = pose_roll;
152  pose.pitch = pose_pitch;
153  pose.yaw = pose_yaw;
154 
155  return pose;
156 }
157 
158 Eigen::MatrixXd FootStepGenerator::getInverseTransformation(Eigen::MatrixXd transform)
159 {
160  // If T is Transform Matrix A from B, the BOA is translation component coordi. B to coordi. A
161 
162  Eigen::Vector3d vec_boa;
163  Eigen::Vector3d vec_x, vec_y, vec_z;
164  Eigen::MatrixXd inv_t(4,4);
165 
166  vec_boa(0) = -transform(0,3);
167  vec_boa(1) = -transform(1,3);
168  vec_boa(2) = -transform(2,3);
169 
170  vec_x(0) = transform(0,0); vec_x(1) = transform(1,0); vec_x(2) = transform(2,0);
171  vec_y(0) = transform(0,1); vec_y(1) = transform(1,1); vec_y(2) = transform(2,1);
172  vec_z(0) = transform(0,2); vec_z(1) = transform(1,2); vec_z(2) = transform(2,2);
173 
174  inv_t <<
175  vec_x(0), vec_x(1), vec_x(2), vec_boa.dot(vec_x),
176  vec_y(0), vec_y(1), vec_y(2), vec_boa.dot(vec_y),
177  vec_z(0), vec_z(1), vec_z(2), vec_boa.dot(vec_z),
178  0, 0, 0, 1;
179 
180  return inv_t;
181 }
182 
183 void FootStepGenerator::getStepData(thormang3_walking_module_msgs::AddStepDataArray::Request::_step_data_array_type* step_data_array, const thormang3_walking_module_msgs::StepData& ref_step_data, int desired_step_type)
184 {
185  step_data_array->clear();
186  step_data_array_.clear();
187 
188  if(calcStep(ref_step_data, previous_step_type_, desired_step_type))
189  {
190  previous_step_type_ = desired_step_type;
191  for(unsigned int stp_idx = 0; stp_idx < step_data_array_.size(); stp_idx++)
192  {
193  step_data_array->push_back(step_data_array_[stp_idx]);
194  }
195  }
196  else
197  {
198  return;
199  }
200 }
201 
202 
203 
204 void FootStepGenerator::getStepDataFromStepData2DArray(thormang3_walking_module_msgs::AddStepDataArray::Request::_step_data_array_type* step_data_array,
205  const thormang3_walking_module_msgs::StepData& ref_step_data,
206  const thormang3_foot_step_generator::Step2DArray::ConstPtr& request_step_2d)
207 {
208  step_data_array->clear();
209 
210  thormang3_walking_module_msgs::StepData stp_data;
211 
212  stp_data = ref_step_data;
213  stp_data.time_data.abs_step_time += start_end_time_sec_;
214  stp_data.time_data.dsp_ratio = dsp_ratio_;
215  stp_data.time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING_STARTING;
216  stp_data.time_data.start_time_delay_ratio_x = 0.0;
217  stp_data.time_data.start_time_delay_ratio_y = 0.0;
218  stp_data.time_data.start_time_delay_ratio_z = 0.0;
219  stp_data.time_data.start_time_delay_ratio_roll = 0.0;
220  stp_data.time_data.start_time_delay_ratio_pitch = 0.0;
221  stp_data.time_data.start_time_delay_ratio_yaw = 0.0;
222  stp_data.time_data.finish_time_advance_ratio_x = 0.0;
223  stp_data.time_data.finish_time_advance_ratio_y = 0.0;
224  stp_data.time_data.finish_time_advance_ratio_z = 0.0;
225  stp_data.time_data.finish_time_advance_ratio_roll = 0.0;
226  stp_data.time_data.finish_time_advance_ratio_pitch = 0.0;
227  stp_data.time_data.finish_time_advance_ratio_yaw = 0.0;
228 
229  stp_data.position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::STANDING;
230  stp_data.position_data.foot_z_swap = 0;
231  stp_data.position_data.body_z_swap = 0;
232 
233  step_data_array->push_back(stp_data);
234 
235  for(unsigned int stp_idx = 0; stp_idx < request_step_2d->footsteps_2d.size(); stp_idx++)
236  {
237  stp_data.time_data.abs_step_time += step_time_sec_;
238  stp_data.time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING;
239 
240  if(request_step_2d->footsteps_2d[stp_idx].moving_foot == thormang3_foot_step_generator::Step2D::LEFT_FOOT_SWING)
241  {
242  stp_data.position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
243  stp_data.position_data.body_z_swap = body_z_swap_m_;
244  stp_data.position_data.foot_z_swap = foot_z_swap_m_;
245  stp_data.position_data.left_foot_pose.x = request_step_2d->footsteps_2d[stp_idx].step2d.x;
246  stp_data.position_data.left_foot_pose.y = request_step_2d->footsteps_2d[stp_idx].step2d.y;
247  stp_data.position_data.left_foot_pose.yaw = request_step_2d->footsteps_2d[stp_idx].step2d.theta;
248 
249  }
250  else if(request_step_2d->footsteps_2d[stp_idx].moving_foot == thormang3_foot_step_generator::Step2D::RIGHT_FOOT_SWING)
251  {
252  stp_data.position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
253  stp_data.position_data.body_z_swap = body_z_swap_m_;
254  stp_data.position_data.foot_z_swap = foot_z_swap_m_;
255  stp_data.position_data.right_foot_pose.x = request_step_2d->footsteps_2d[stp_idx].step2d.x;
256  stp_data.position_data.right_foot_pose.y = request_step_2d->footsteps_2d[stp_idx].step2d.y;
257  stp_data.position_data.right_foot_pose.yaw = request_step_2d->footsteps_2d[stp_idx].step2d.theta;
258  }
259  else
260  {
261  ROS_ERROR("Invalid Step2D");
262  step_data_array->clear();
263  return;
264  }
265 
266  if(fabs(stp_data.position_data.right_foot_pose.yaw - stp_data.position_data.left_foot_pose.yaw) > M_PI)
267  {
268  stp_data.position_data.body_pose.yaw = 0.5*(stp_data.position_data.right_foot_pose.yaw + stp_data.position_data.left_foot_pose.yaw)
269  - sign(0.5*(stp_data.position_data.right_foot_pose.yaw - stp_data.position_data.left_foot_pose.yaw))*M_PI;
270  }
271  else
272  {
273  stp_data.position_data.body_pose.yaw = 0.5*(stp_data.position_data.right_foot_pose.yaw
274  + stp_data.position_data.left_foot_pose.yaw);
275  }
276 
277  step_data_array->push_back(stp_data);
278  }
279 
280  stp_data.time_data.abs_step_time += start_end_time_sec_;
281  stp_data.time_data.dsp_ratio = dsp_ratio_;
282  stp_data.time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING_ENDING;
283  stp_data.time_data.start_time_delay_ratio_x = 0.0;
284  stp_data.time_data.start_time_delay_ratio_y = 0.0;
285  stp_data.time_data.start_time_delay_ratio_z = 0.0;
286  stp_data.time_data.start_time_delay_ratio_roll = 0.0;
287  stp_data.time_data.start_time_delay_ratio_pitch = 0.0;
288  stp_data.time_data.start_time_delay_ratio_yaw = 0.0;
289  stp_data.time_data.finish_time_advance_ratio_x = 0.0;
290  stp_data.time_data.finish_time_advance_ratio_y = 0.0;
291  stp_data.time_data.finish_time_advance_ratio_z = 0.0;
292  stp_data.time_data.finish_time_advance_ratio_roll = 0.0;
293  stp_data.time_data.finish_time_advance_ratio_pitch = 0.0;
294  stp_data.time_data.finish_time_advance_ratio_yaw = 0.0;
295 
296  stp_data.position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::STANDING;
297  stp_data.position_data.foot_z_swap = 0;
298  stp_data.position_data.body_z_swap = 0;
299 
300  step_data_array->push_back(stp_data);
301 }
302 
303 //
304 bool FootStepGenerator::calcStep(const thormang3_walking_module_msgs::StepData& ref_step_data, int previous_step_type, int desired_step_type)
305 {
306  int direction = 0;
307  thormang3_walking_module_msgs::StepData stp_data[2];
308 
309  thormang3_walking_module_msgs::PoseXYZRPY poseGtoRF, poseGtoLF;
310  thormang3_walking_module_msgs::PoseXYZRPY poseLtoRF, poseLtoLF;
311 
312  poseGtoRF = ref_step_data.position_data.right_foot_pose;
313  poseGtoLF = ref_step_data.position_data.left_foot_pose;
314 
315  Eigen::MatrixXd mat_g_to_rf = getTransformationXYZRPY(poseGtoRF.x, poseGtoRF.y, poseGtoRF.z, 0, 0, poseGtoRF.yaw);
316  Eigen::MatrixXd mat_g_to_lf = getTransformationXYZRPY(poseGtoLF.x, poseGtoLF.y, poseGtoLF.z, 0, 0, poseGtoLF.yaw);
317 
318  //the local coordinate is set as below.
319  //the below local does not means real local coordinate.
320  //it is just for calculating step data.
321  //the local coordinate will be decide by the moving foot of ref step data
322  Eigen::MatrixXd mat_lf_to_local = getTransformationXYZRPY(0, -0.5*default_y_feet_offset_m_, 0, 0, 0, 0);
323  Eigen::MatrixXd mat_rf_to_local = getTransformationXYZRPY(0, 0.5*default_y_feet_offset_m_, 0, 0, 0, 0);
324  Eigen::MatrixXd mat_global_to_local, mat_local_to_global;
325  if(ref_step_data.position_data.moving_foot == thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING)
326  {
327  mat_global_to_local = mat_g_to_rf*mat_rf_to_local;
328  mat_local_to_global = getInverseTransformation(mat_global_to_local);
329  mat_lf_to_local = getInverseTransformation(mat_g_to_lf) * mat_global_to_local;
330  }
331  else
332  {
333  mat_global_to_local = mat_g_to_lf * mat_lf_to_local;;
334  mat_local_to_global = getInverseTransformation(mat_global_to_local);
335  mat_rf_to_local = getInverseTransformation(mat_g_to_rf) * mat_global_to_local;
336  }
337 
338  Eigen::MatrixXd mat_local_to_rf = mat_local_to_global * mat_g_to_rf;
339  Eigen::MatrixXd mat_local_to_lf = mat_local_to_global * mat_g_to_lf;
340 
341  poseLtoRF = getPosefromTransformMatrix(mat_local_to_rf);
342  poseLtoLF = getPosefromTransformMatrix(mat_local_to_lf);
343 
344  if((desired_step_type == FORWARD_WALKING) || (desired_step_type == LEFTWARD_WALKING) || (desired_step_type == LEFT_ROTATING_WALKING))
345  direction = 1;
346  else if((desired_step_type == BACKWARD_WALKING ) || (desired_step_type == RIGHTWARD_WALKING) || (desired_step_type == RIGHT_ROTATING_WALKING))
347  direction = -1;
348  else if(desired_step_type == STOP_WALKING)
349  direction = 0;
350  else
351  return false;
352 
353 
354  stp_data[0] = ref_step_data;
355  stp_data[0].position_data.torso_yaw_angle_rad = 0.0*M_PI;
356 
357  stp_data[0].position_data.right_foot_pose = poseLtoRF;
358  stp_data[0].position_data.left_foot_pose = poseLtoLF;
359  stp_data[0].time_data.start_time_delay_ratio_x = 0.0;
360  stp_data[0].time_data.start_time_delay_ratio_y = 0.0;
361  stp_data[0].time_data.start_time_delay_ratio_z = 0.0;
362  stp_data[0].time_data.start_time_delay_ratio_roll = 0.0;
363  stp_data[0].time_data.start_time_delay_ratio_pitch = 0.0;
364  stp_data[0].time_data.start_time_delay_ratio_yaw = 0.0;
365  stp_data[0].time_data.finish_time_advance_ratio_x = 0.0;
366  stp_data[0].time_data.finish_time_advance_ratio_y = 0.0;
367  stp_data[0].time_data.finish_time_advance_ratio_z = 0.0;
368  stp_data[0].time_data.finish_time_advance_ratio_roll = 0.0;
369  stp_data[0].time_data.finish_time_advance_ratio_pitch = 0.0;
370  stp_data[0].time_data.finish_time_advance_ratio_yaw = 0.0;
371 
372 
373  if(stp_data[0].time_data.walking_state != thormang3_walking_module_msgs::StepTimeData::IN_WALKING)
374  {
375  if(desired_step_type == FORWARD_WALKING || desired_step_type == BACKWARD_WALKING )
376  calcFBStep(stp_data[0], direction);
377  else if(desired_step_type == RIGHTWARD_WALKING || desired_step_type == LEFTWARD_WALKING )
378  calcRLStep(stp_data[0], direction);
379  else if(desired_step_type == LEFT_ROTATING_WALKING || desired_step_type == RIGHT_ROTATING_WALKING )
380  calcRoStep(stp_data[0], direction);
381  else if(desired_step_type == STOP_WALKING)
382  calcStopStep(stp_data[0], direction);
383  else
384  return false;
385  }
386  else
387  {
388  if(desired_step_type != previous_step_type)
389  {
390  stp_data[0].time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING;
391  if((fabs(poseLtoRF.yaw - poseLtoLF.yaw) > 0)
392  || (fabs(poseLtoRF.y - poseLtoLF.y) > default_y_feet_offset_m_)
393  || (fabs(poseLtoRF.x - poseLtoLF.x) > 0))
394  {
395  stp_data[0].time_data.abs_step_time += step_time_sec_;
396  if(ref_step_data.position_data.moving_foot == thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING)
397  {
398  stp_data[0].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
399  stp_data[0].position_data.right_foot_pose.x = stp_data[0].position_data.left_foot_pose.x;
400  stp_data[0].position_data.right_foot_pose.y = stp_data[0].position_data.left_foot_pose.y - default_y_feet_offset_m_;
401  stp_data[0].position_data.right_foot_pose.yaw = stp_data[0].position_data.left_foot_pose.yaw;
402  }
403  else
404  {
405  stp_data[0].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
406  stp_data[0].position_data.left_foot_pose.x = stp_data[0].position_data.right_foot_pose.x;
407  stp_data[0].position_data.left_foot_pose.y = stp_data[0].position_data.right_foot_pose.y + default_y_feet_offset_m_;
408  stp_data[0].position_data.left_foot_pose.yaw = stp_data[0].position_data.right_foot_pose.yaw;
409  }
410  step_data_array_.push_back(stp_data[0]);
411  }
412 
413 // if(previous_step_type == FORWARD_WALKING || previous_step_type == BACKWARD_WALKING)
414 // {
415 // if(fabs(poseLtoRF.x - poseLtoLF.x) >= 0.001)
416 // {
417 // stp_data[0].time_data.abs_step_time += step_time_sec_;
418 //
419 // if(ref_step_data.position_data.moving_foot == thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING)
420 // {
421 // stp_data[0].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
422 // stp_data[0].position_data.right_foot_pose.x = stp_data[0].position_data.left_foot_pose.x;
423 // }
424 // else {
425 // stp_data[0].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
426 // stp_data[0].position_data.left_foot_pose.x = stp_data[0].position_data.right_foot_pose.x;
427 // }
428 //
429 // step_data_array_.push_back(stp_data[0]);
430 // }
431 // }
432 // else if(previous_step_type == LEFTWARD_WALKING || previous_step_type == RIGHTWARD_WALKING)
433 // {
434 // if(fabs(poseLtoRF.y - poseLtoLF.y) >= default_y_feet_offset_m_ - 0.001)
435 // {
436 // stp_data[0].time_data.abs_step_time += step_time_sec_;
437 //
438 // if(ref_step_data.position_data.moving_foot == thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING)
439 // {
440 // stp_data[0].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
441 // stp_data[0].position_data.right_foot_pose.y = stp_data[0].position_data.left_foot_pose.y - default_y_feet_offset_m_;
442 // }
443 // else
444 // {
445 // stp_data[0].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
446 // stp_data[0].position_data.left_foot_pose.y = stp_data[0].position_data.right_foot_pose.y + default_y_feet_offset_m_;
447 // }
448 //
449 // step_data_array_.push_back(stp_data[0]);
450 // }
451 // }
452 // else if(previous_step_type == LEFT_ROTATING_WALKING || previous_step_type == RIGHT_ROTATING_WALKING)
453 // {
454 // if(fabs(poseLtoRF.yaw - poseLtoLF.yaw) > 0.0000001)
455 // {
456 // stp_data[0].time_data.abs_step_time += step_time_sec_;
457 // if(ref_step_data.position_data.moving_foot == thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING)
458 // {
459 // stp_data[0].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
460 // stp_data[0].position_data.right_foot_pose.x = stp_data[0].position_data.left_foot_pose.x;
461 // stp_data[0].position_data.right_foot_pose.y = stp_data[0].position_data.left_foot_pose.y - default_y_feet_offset_m_;
462 // stp_data[0].position_data.right_foot_pose.yaw = stp_data[0].position_data.left_foot_pose.yaw;
463 // }
464 // else
465 // {
466 // stp_data[0].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
467 // stp_data[0].position_data.left_foot_pose.x = stp_data[0].position_data.right_foot_pose.x;
468 // stp_data[0].position_data.left_foot_pose.y = stp_data[0].position_data.right_foot_pose.y + default_y_feet_offset_m_;
469 // stp_data[0].position_data.left_foot_pose.yaw = stp_data[0].position_data.right_foot_pose.yaw;
470 // }
471 // step_data_array_.push_back(stp_data[0]);
472 // }
473 // }
474 // else if(previous_step_type == STOP_WALKING)
475 // {
476 // if((fabs(poseLtoRF.yaw - poseLtoLF.yaw) > 0.0000001)
477 // || (fabs(poseLtoRF.y - poseLtoLF.y) >= default_y_feet_offset_m_ - 0.001)
478 // || (fabs(poseLtoRF.x - poseLtoLF.x) >= 0.001))
479 // {
480 // stp_data[0].time_data.abs_step_time += step_time_sec_;
481 // if(ref_step_data.position_data.moving_foot == thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING)
482 // {
483 // stp_data[0].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
484 // stp_data[0].position_data.right_foot_pose.x = stp_data[0].position_data.left_foot_pose.x;
485 // stp_data[0].position_data.right_foot_pose.y = stp_data[0].position_data.left_foot_pose.y - default_y_feet_offset_m_;
486 // stp_data[0].position_data.right_foot_pose.yaw = stp_data[0].position_data.left_foot_pose.yaw;
487 // }
488 // else
489 // {
490 // stp_data[0].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
491 // stp_data[0].position_data.left_foot_pose.x = stp_data[0].position_data.right_foot_pose.x;
492 // stp_data[0].position_data.left_foot_pose.y = stp_data[0].position_data.right_foot_pose.y + default_y_feet_offset_m_;
493 // stp_data[0].position_data.left_foot_pose.yaw = stp_data[0].position_data.right_foot_pose.yaw;
494 // }
495 // step_data_array_.push_back(stp_data[0]);
496 // }
497 // }
498 // else
499 // {
500 // return false;
501 // }
502 
503 
504  stp_data[1] = stp_data[0];
505  if(desired_step_type == FORWARD_WALKING || desired_step_type == BACKWARD_WALKING || desired_step_type == STOP_WALKING)
506  {
507 
508  }
509  else if(desired_step_type == LEFTWARD_WALKING || desired_step_type == LEFT_ROTATING_WALKING)
510  {
511  if(stp_data[0].position_data.moving_foot == thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING)
512  {
513  stp_data[1].time_data.abs_step_time += step_time_sec_;
514  stp_data[1].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
515  step_data_array_.push_back(stp_data[1]);
516  }
517  }
518  else if(desired_step_type == RIGHTWARD_WALKING || desired_step_type == RIGHT_ROTATING_WALKING)
519  {
520  if(stp_data[0].position_data.moving_foot == thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING)
521  {
522  stp_data[1].time_data.abs_step_time += step_time_sec_;
523  stp_data[1].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
524  step_data_array_.push_back(stp_data[1]);
525  }
526  }
527  else
528  {
529  return false;
530  }
531 
532  if(desired_step_type == FORWARD_WALKING || desired_step_type == BACKWARD_WALKING )
533  {
534  calcFBStep(stp_data[1], direction);
535  }
536  else if(desired_step_type == RIGHTWARD_WALKING || desired_step_type == LEFTWARD_WALKING )
537  {
538  calcRLStep(stp_data[1], direction);
539  }
540  else if(desired_step_type == LEFT_ROTATING_WALKING || desired_step_type == RIGHT_ROTATING_WALKING )
541  {
542  calcRoStep(stp_data[1], direction);
543  }
544  else if(desired_step_type == STOP_WALKING)
545  {
546  calcStopStep(stp_data[1], direction);
547  }
548  else
549  {
550  return false;
551  }
552  }
553  else
554  {
555  if(desired_step_type == FORWARD_WALKING || desired_step_type == BACKWARD_WALKING )
556  {
557  calcFBStep(stp_data[0], direction);
558  }
559  else if(desired_step_type == RIGHTWARD_WALKING || desired_step_type == LEFTWARD_WALKING )
560  {
561  calcRLStep(stp_data[0], direction);
562  }
563  else if(desired_step_type == LEFT_ROTATING_WALKING || desired_step_type == RIGHT_ROTATING_WALKING )
564  {
565  calcRoStep(stp_data[0], direction);
566  }
567  else if(desired_step_type == STOP_WALKING)
568  {
569  calcStopStep(stp_data[0], direction);
570  }
571  else
572  {
573  return false;
574  }
575  }
576  }
577 
578 
579  for(unsigned int stp_idx = 0; stp_idx < step_data_array_.size(); stp_idx++)
580  {
581  Eigen::MatrixXd mat_r_foot = getTransformationXYZRPY(step_data_array_[stp_idx].position_data.right_foot_pose.x,
582  step_data_array_[stp_idx].position_data.right_foot_pose.y,
583  step_data_array_[stp_idx].position_data.right_foot_pose.z,
584  step_data_array_[stp_idx].position_data.right_foot_pose.roll,
585  step_data_array_[stp_idx].position_data.right_foot_pose.pitch,
586  step_data_array_[stp_idx].position_data.right_foot_pose.yaw);
587 
588  Eigen::MatrixXd mat_l_foot = getTransformationXYZRPY(step_data_array_[stp_idx].position_data.left_foot_pose.x,
589  step_data_array_[stp_idx].position_data.left_foot_pose.y,
590  step_data_array_[stp_idx].position_data.left_foot_pose.z,
591  step_data_array_[stp_idx].position_data.left_foot_pose.roll,
592  step_data_array_[stp_idx].position_data.left_foot_pose.pitch,
593  step_data_array_[stp_idx].position_data.left_foot_pose.yaw);
594 
595  step_data_array_[stp_idx].position_data.right_foot_pose = getPosefromTransformMatrix(mat_global_to_local * mat_r_foot);
596  step_data_array_[stp_idx].position_data.left_foot_pose = getPosefromTransformMatrix(mat_global_to_local * mat_l_foot);
597 
598 
599  if(fabs(step_data_array_[stp_idx].position_data.right_foot_pose.yaw - step_data_array_[stp_idx].position_data.left_foot_pose.yaw) > M_PI)
600  {
601  step_data_array_[stp_idx].position_data.body_pose.yaw = 0.5*(step_data_array_[stp_idx].position_data.right_foot_pose.yaw + step_data_array_[stp_idx].position_data.left_foot_pose.yaw)
602  - sign(0.5*(step_data_array_[stp_idx].position_data.right_foot_pose.yaw - step_data_array_[stp_idx].position_data.left_foot_pose.yaw))*M_PI;
603  }
604  else
605  {
606  step_data_array_[stp_idx].position_data.body_pose.yaw = 0.5*(step_data_array_[stp_idx].position_data.right_foot_pose.yaw
607  + step_data_array_[stp_idx].position_data.left_foot_pose.yaw);
608  }
609  }
610 
611  return true;
612 }
613 
614 
615 void FootStepGenerator::calcFBStep(const thormang3_walking_module_msgs::StepData& ref_step_data, int direction)
616 {
617  thormang3_walking_module_msgs::StepData stp_data[num_of_step_];
618  stp_data[0] = ref_step_data;
619 
620  if(ref_step_data.time_data.walking_state == thormang3_walking_module_msgs::StepTimeData::IN_WALKING)
621  {
622  stp_data[0].time_data.abs_step_time += step_time_sec_;
623  stp_data[0].time_data.dsp_ratio = dsp_ratio_;
624  stp_data[0].position_data.body_z_swap = body_z_swap_m_;
625  stp_data[0].position_data.foot_z_swap = foot_z_swap_m_;
626  if(stp_data[0].position_data.moving_foot == thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING)
627  {
628  stp_data[0].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
629  stp_data[0].position_data.right_foot_pose.x = stp_data[0].position_data.left_foot_pose.x + (double)direction*fb_step_length_m_;
630  }
631  else
632  {
633  stp_data[0].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
634  stp_data[0].position_data.left_foot_pose.x = stp_data[0].position_data.right_foot_pose.x + (double)direction*fb_step_length_m_;
635  }
636 
637  for(int stp_idx = 1; stp_idx < num_of_step_-2; stp_idx++)
638  {
639  stp_data[stp_idx] = stp_data[stp_idx-1];
640  stp_data[stp_idx].time_data.abs_step_time += step_time_sec_;
641  if(stp_data[stp_idx].position_data.moving_foot == thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING)
642  {
643  stp_data[stp_idx].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
644  stp_data[stp_idx].position_data.right_foot_pose.x = stp_data[stp_idx].position_data.left_foot_pose.x + (double)direction*fb_step_length_m_;
645  }
646  else
647  {
648  stp_data[stp_idx].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
649  stp_data[stp_idx].position_data.left_foot_pose.x = stp_data[stp_idx].position_data.right_foot_pose.x + (double)direction*fb_step_length_m_;
650  }
651  }
652 
653  stp_data[num_of_step_-2] = stp_data[num_of_step_-3];
654  stp_data[num_of_step_-2].time_data.abs_step_time += step_time_sec_;
655  if(stp_data[num_of_step_-2].position_data.moving_foot == thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING)
656  {
657  stp_data[num_of_step_-2].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
658  stp_data[num_of_step_-2].position_data.right_foot_pose.x = stp_data[num_of_step_-2].position_data.left_foot_pose.x;
659  }
660  else
661  {
662  stp_data[num_of_step_-2].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
663  stp_data[num_of_step_-2].position_data.left_foot_pose.x = stp_data[num_of_step_-2].position_data.right_foot_pose.x;
664  }
665 
666  stp_data[num_of_step_-1] = stp_data[num_of_step_-2];
667  stp_data[num_of_step_-1].time_data.abs_step_time += start_end_time_sec_;
668  stp_data[num_of_step_-1].time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING_ENDING;
669  stp_data[num_of_step_-1].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::STANDING;
670  stp_data[num_of_step_-1].position_data.body_z_swap = 0;
671  }
672  else
673  {
674  stp_data[0].time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING_STARTING;
675  stp_data[0].time_data.abs_step_time += start_end_time_sec_;
676  stp_data[0].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::STANDING;
677  stp_data[0].position_data.body_z_swap = 0;
678 
679  for(int stp_idx = 1; stp_idx < num_of_step_-2; stp_idx++)
680  {
681  stp_data[stp_idx] = stp_data[stp_idx-1];
682  stp_data[stp_idx].time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING;
683  stp_data[stp_idx].time_data.abs_step_time += step_time_sec_;
684  stp_data[stp_idx].time_data.dsp_ratio = dsp_ratio_;
685  stp_data[stp_idx].position_data.body_z_swap = body_z_swap_m_;
686  stp_data[stp_idx].position_data.foot_z_swap = foot_z_swap_m_;
687 
688  if(stp_data[stp_idx].position_data.moving_foot == thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING)
689  {
690  stp_data[stp_idx].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
691  stp_data[stp_idx].position_data.right_foot_pose.x = stp_data[stp_idx].position_data.left_foot_pose.x + (double)direction*fb_step_length_m_;
692  }
693  else
694  {
695  stp_data[stp_idx].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
696  stp_data[stp_idx].position_data.left_foot_pose.x = stp_data[stp_idx].position_data.right_foot_pose.x + (double)direction*fb_step_length_m_;
697  }
698  }
699 
700  stp_data[num_of_step_-2] = stp_data[num_of_step_-3];
701  stp_data[num_of_step_-2].time_data.abs_step_time += step_time_sec_;
702  if(stp_data[num_of_step_-2].position_data.moving_foot == thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING)
703  {
704  stp_data[num_of_step_-2].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
705  stp_data[num_of_step_-2].position_data.right_foot_pose.x = stp_data[num_of_step_-2].position_data.left_foot_pose.x;
706  }
707  else
708  {
709  stp_data[num_of_step_-2].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
710  stp_data[num_of_step_-2].position_data.left_foot_pose.x = stp_data[num_of_step_-2].position_data.right_foot_pose.x;
711  }
712 
713  stp_data[num_of_step_-1] = stp_data[num_of_step_-2];
714  stp_data[num_of_step_-1].time_data.abs_step_time += start_end_time_sec_;
715  stp_data[num_of_step_-1].time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING_ENDING;
716  stp_data[num_of_step_-1].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::STANDING;
717  stp_data[num_of_step_-1].position_data.body_z_swap = 0;
718  }
719 
720  for(int stp_idx = 0; stp_idx < num_of_step_; stp_idx++)
721  {
722  step_data_array_.push_back(stp_data[stp_idx]);
723  }
724 }
725 
726 void FootStepGenerator::calcRLStep(const thormang3_walking_module_msgs::StepData& ref_step_data, int direction)
727 {
728  thormang3_walking_module_msgs::StepData stp_data[num_of_step_];
729  stp_data[0] = ref_step_data;
730 
731  if(ref_step_data.time_data.walking_state == thormang3_walking_module_msgs::StepTimeData::IN_WALKING)
732  {
733  stp_data[0].time_data.abs_step_time += step_time_sec_;
734  stp_data[0].time_data.dsp_ratio = dsp_ratio_;
735  stp_data[0].position_data.body_z_swap = body_z_swap_m_;
736  stp_data[0].position_data.foot_z_swap = foot_z_swap_m_;
737  if(stp_data[0].position_data.moving_foot == thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING)
738  {
739  stp_data[0].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
740  stp_data[0].position_data.right_foot_pose.y = stp_data[0].position_data.right_foot_pose.y + (double)direction*rl_step_length_m_;
741  }
742  else
743  {
744  stp_data[0].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
745  stp_data[0].position_data.left_foot_pose.y = stp_data[0].position_data.left_foot_pose.y + (double)direction*rl_step_length_m_;
746  }
747 
748  for(int stp_idx = 1; stp_idx < num_of_step_-2; stp_idx++)
749  {
750  stp_data[stp_idx] = stp_data[stp_idx-1];
751  stp_data[stp_idx].time_data.abs_step_time += step_time_sec_;
752  if(stp_data[stp_idx].position_data.moving_foot == thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING)
753  {
754  stp_data[stp_idx].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
755  stp_data[stp_idx].position_data.right_foot_pose.y = stp_data[stp_idx].position_data.right_foot_pose.y + (double)direction*rl_step_length_m_;
756  }
757  else
758  {
759  stp_data[stp_idx].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
760  stp_data[stp_idx].position_data.left_foot_pose.y = stp_data[stp_idx].position_data.left_foot_pose.y + (double)direction*rl_step_length_m_;
761  }
762  }
763 
764  stp_data[num_of_step_-2] = stp_data[num_of_step_-3];
765  stp_data[num_of_step_-2].time_data.abs_step_time += step_time_sec_;
766  if(stp_data[num_of_step_-2].position_data.moving_foot == thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING)
767  {
768  stp_data[num_of_step_-2].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
769  stp_data[num_of_step_-2].position_data.right_foot_pose.y = stp_data[num_of_step_-2].position_data.left_foot_pose.y - default_y_feet_offset_m_;
770  }
771  else
772  {
773  stp_data[num_of_step_-2].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
774  stp_data[num_of_step_-2].position_data.left_foot_pose.y = stp_data[num_of_step_-2].position_data.right_foot_pose.y + default_y_feet_offset_m_;
775  }
776 
777  stp_data[num_of_step_-1] = stp_data[num_of_step_-2];
778  stp_data[num_of_step_-1].time_data.abs_step_time += start_end_time_sec_;
779  stp_data[num_of_step_-1].time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING_ENDING;
780  stp_data[num_of_step_-1].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::STANDING;
781  stp_data[num_of_step_-1].position_data.body_z_swap = 0;
782  }
783  else
784  {
785  stp_data[0].time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING_STARTING;
786  stp_data[0].time_data.abs_step_time += start_end_time_sec_;
787  stp_data[0].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::STANDING;
788  stp_data[0].position_data.body_z_swap = 0;
789 
790 
791  stp_data[1] = stp_data[0];
792  stp_data[1].time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING;
793  stp_data[1].time_data.abs_step_time += step_time_sec_;
794  stp_data[1].time_data.dsp_ratio = dsp_ratio_;
795  stp_data[1].position_data.body_z_swap = body_z_swap_m_;
796  stp_data[1].position_data.foot_z_swap = foot_z_swap_m_;
797 
798  if(direction < 0)
799  {
800  stp_data[1].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
801  stp_data[1].position_data.right_foot_pose.y = stp_data[1].position_data.right_foot_pose.y + (double)direction*rl_step_length_m_;
802  }
803  else
804  {
805  stp_data[1].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
806  stp_data[1].position_data.left_foot_pose.y = stp_data[1].position_data.left_foot_pose.y + (double)direction*rl_step_length_m_;
807  }
808 
809  for(int stp_idx = 2; stp_idx < num_of_step_-2; stp_idx++)
810  {
811  stp_data[stp_idx] = stp_data[stp_idx-1];
812  stp_data[stp_idx].time_data.abs_step_time += step_time_sec_;
813  if(stp_data[stp_idx].position_data.moving_foot == thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING)
814  {
815  stp_data[stp_idx].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
816  stp_data[stp_idx].position_data.right_foot_pose.y = stp_data[stp_idx].position_data.right_foot_pose.y + (double)direction*rl_step_length_m_;
817  }
818  else
819  {
820  stp_data[stp_idx].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
821  stp_data[stp_idx].position_data.left_foot_pose.y = stp_data[stp_idx].position_data.left_foot_pose.y + (double)direction*rl_step_length_m_;
822  }
823  }
824 
825  stp_data[num_of_step_-2] = stp_data[num_of_step_-3];
826  stp_data[num_of_step_-2].time_data.abs_step_time += step_time_sec_;
827  if(stp_data[num_of_step_-2].position_data.moving_foot == thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING)
828  {
829  stp_data[num_of_step_-2].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
830  stp_data[num_of_step_-2].position_data.right_foot_pose.y = stp_data[num_of_step_-2].position_data.left_foot_pose.y - default_y_feet_offset_m_;
831  }
832  else
833  {
834  stp_data[num_of_step_-2].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
835  stp_data[num_of_step_-2].position_data.left_foot_pose.y = stp_data[num_of_step_-2].position_data.right_foot_pose.y + default_y_feet_offset_m_;
836  }
837 
838  stp_data[num_of_step_-1] = stp_data[num_of_step_-2];
839  stp_data[num_of_step_-1].time_data.abs_step_time += start_end_time_sec_;
840  stp_data[num_of_step_-1].time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING_ENDING;
841  stp_data[num_of_step_-1].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::STANDING;
842  stp_data[num_of_step_-1].position_data.body_z_swap = 0;
843 
844  }
845 
846  for(int stp_idx = 0; stp_idx < num_of_step_; stp_idx++)
847  {
848  step_data_array_.push_back(stp_data[stp_idx]);
849  }
850 }
851 
852 void FootStepGenerator::calcRoStep(const thormang3_walking_module_msgs::StepData& ref_step_data, int direction)
853 {
854  thormang3_walking_module_msgs::StepData stp_data[num_of_step_];
855  stp_data[0] = ref_step_data;
856  if(ref_step_data.time_data.walking_state == thormang3_walking_module_msgs::StepTimeData::IN_WALKING)
857  {
858  stp_data[0].time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING;
859  stp_data[0].time_data.abs_step_time += step_time_sec_;
860  stp_data[0].time_data.dsp_ratio = dsp_ratio_;
861  stp_data[0].position_data.body_z_swap = body_z_swap_m_;
862  stp_data[0].position_data.foot_z_swap = foot_z_swap_m_;
863 
864  if(stp_data[0].position_data.moving_foot == thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING)
865  {
866  stp_data[0].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
867  stp_data[0].position_data.right_foot_pose.yaw = stp_data[0].position_data.right_foot_pose.yaw + (double)direction*rotate_step_angle_rad_;
868 
869  if(fabs(stp_data[0].position_data.right_foot_pose.yaw) > 2.0*M_PI)
870  stp_data[0].position_data.right_foot_pose.yaw += -2.0*M_PI*sign(stp_data[0].position_data.right_foot_pose.yaw);
871 
872  stp_data[0].position_data.right_foot_pose.x = 0.5*default_y_feet_offset_m_*sin(stp_data[0].position_data.right_foot_pose.yaw);
873  stp_data[0].position_data.right_foot_pose.y = -0.5*default_y_feet_offset_m_*cos(stp_data[0].position_data.right_foot_pose.yaw);
874  }
875  else
876  {
877  stp_data[0].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
878  stp_data[0].position_data.left_foot_pose.yaw = stp_data[0].position_data.left_foot_pose.yaw + (double)direction*rotate_step_angle_rad_;
879 
880  if(fabs(stp_data[0].position_data.left_foot_pose.yaw) > 2.0*M_PI)
881  stp_data[0].position_data.left_foot_pose.yaw += -2.0*M_PI*sign(stp_data[0].position_data.left_foot_pose.yaw);
882 
883  stp_data[0].position_data.left_foot_pose.x = -0.5*default_y_feet_offset_m_*sin(stp_data[0].position_data.left_foot_pose.yaw);
884  stp_data[0].position_data.left_foot_pose.y = 0.5*default_y_feet_offset_m_*cos(stp_data[0].position_data.left_foot_pose.yaw);
885  }
886 
887 
888  for(int stp_idx = 1; stp_idx < num_of_step_-2; stp_idx++)
889  {
890  stp_data[stp_idx] = stp_data[stp_idx-1];
891  stp_data[stp_idx].time_data.abs_step_time += step_time_sec_;
892  if(stp_data[stp_idx].position_data.moving_foot == thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING)
893  {
894  stp_data[stp_idx].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
895  stp_data[stp_idx].position_data.right_foot_pose.yaw = stp_data[stp_idx].position_data.right_foot_pose.yaw + (double)direction*rotate_step_angle_rad_;
896 
897  if(fabs(stp_data[stp_idx].position_data.right_foot_pose.yaw) > 2.0*M_PI)
898  stp_data[stp_idx].position_data.right_foot_pose.yaw += -2.0*M_PI*sign(stp_data[stp_idx].position_data.right_foot_pose.yaw);
899 
900  stp_data[stp_idx].position_data.right_foot_pose.x = 0.5*default_y_feet_offset_m_*sin(stp_data[stp_idx].position_data.right_foot_pose.yaw);
901  stp_data[stp_idx].position_data.right_foot_pose.y = -0.5*default_y_feet_offset_m_*cos(stp_data[stp_idx].position_data.right_foot_pose.yaw);
902  }
903  else
904  {
905  stp_data[stp_idx].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
906  stp_data[stp_idx].position_data.left_foot_pose.yaw = stp_data[stp_idx].position_data.left_foot_pose.yaw + (double)direction*rotate_step_angle_rad_;
907 
908  if(fabs(stp_data[stp_idx].position_data.left_foot_pose.yaw) > 2.0*M_PI)
909  stp_data[stp_idx].position_data.left_foot_pose.yaw += -2.0*M_PI*sign(stp_data[stp_idx].position_data.left_foot_pose.yaw);
910 
911  stp_data[stp_idx].position_data.left_foot_pose.x = -0.5*default_y_feet_offset_m_*sin(stp_data[stp_idx].position_data.left_foot_pose.yaw);
912  stp_data[stp_idx].position_data.left_foot_pose.y = 0.5*default_y_feet_offset_m_*cos(stp_data[stp_idx].position_data.left_foot_pose.yaw);
913 
914  }
915 
916  if(fabs(stp_data[stp_idx].position_data.right_foot_pose.yaw) > M_PI)
917  stp_data[stp_idx].position_data.right_foot_pose.yaw -= 2.0*M_PI*sign(stp_data[stp_idx].position_data.right_foot_pose.yaw);
918  if(fabs(stp_data[stp_idx].position_data.left_foot_pose.yaw) > M_PI)
919  stp_data[stp_idx].position_data.left_foot_pose.yaw -= 2.0*M_PI*sign(stp_data[stp_idx].position_data.left_foot_pose.yaw);
920  }
921 
922  stp_data[num_of_step_-2] = stp_data[num_of_step_-3];
923  stp_data[num_of_step_-2].time_data.abs_step_time += step_time_sec_;
924  if(stp_data[num_of_step_-2].position_data.moving_foot == thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING)
925  {
926  stp_data[num_of_step_-2].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
927  stp_data[num_of_step_-2].position_data.right_foot_pose.yaw = stp_data[num_of_step_-2].position_data.left_foot_pose.yaw;
928  stp_data[num_of_step_-2].position_data.right_foot_pose.x = 0.5*default_y_feet_offset_m_*sin(stp_data[num_of_step_-2].position_data.left_foot_pose.yaw);
929  stp_data[num_of_step_-2].position_data.right_foot_pose.y = -0.5*default_y_feet_offset_m_*cos(stp_data[num_of_step_-2].position_data.left_foot_pose.yaw);
930 
931  }
932  else
933  {
934  stp_data[num_of_step_-2].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
935  stp_data[num_of_step_-2].position_data.left_foot_pose.yaw = stp_data[num_of_step_-2].position_data.right_foot_pose.yaw;
936  stp_data[num_of_step_-2].position_data.left_foot_pose.x = -0.5*default_y_feet_offset_m_*sin(stp_data[num_of_step_-2].position_data.right_foot_pose.yaw);
937  stp_data[num_of_step_-2].position_data.left_foot_pose.y = 0.5*default_y_feet_offset_m_*cos(stp_data[num_of_step_-2].position_data.right_foot_pose.yaw);
938  }
939 
940  stp_data[num_of_step_-1] = stp_data[num_of_step_-2];
941  stp_data[num_of_step_-1].time_data.abs_step_time += start_end_time_sec_;
942  stp_data[num_of_step_-1].time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING_ENDING;
943  stp_data[num_of_step_-1].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::STANDING;
944  stp_data[num_of_step_-1].position_data.body_z_swap = 0;
945  }
946  else
947  {
948  stp_data[0].time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING_STARTING;
949  stp_data[0].time_data.abs_step_time += start_end_time_sec_;
950  stp_data[0].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::STANDING;
951  stp_data[0].position_data.body_z_swap = 0;
952  stp_data[0].position_data.foot_z_swap = 0;
953 
954 
955  stp_data[1] = stp_data[0];
956  stp_data[1].time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING;
957  stp_data[1].time_data.abs_step_time += step_time_sec_;
958  stp_data[1].time_data.dsp_ratio = dsp_ratio_;
959  stp_data[1].position_data.body_z_swap = body_z_swap_m_;
960  stp_data[1].position_data.foot_z_swap = foot_z_swap_m_;
961 
962  if(direction < 0)
963  {
964  stp_data[1].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
965  stp_data[1].position_data.right_foot_pose.yaw = stp_data[1].position_data.right_foot_pose.yaw + (double)direction*rotate_step_angle_rad_;
966 
967  if(fabs(stp_data[1].position_data.right_foot_pose.yaw) > 2.0*M_PI)
968  stp_data[1].position_data.right_foot_pose.yaw += -2.0*M_PI*sign(stp_data[1].position_data.right_foot_pose.yaw);
969 
970  stp_data[1].position_data.right_foot_pose.x = 0.5*default_y_feet_offset_m_*sin(stp_data[1].position_data.left_foot_pose.yaw);
971  stp_data[1].position_data.right_foot_pose.y = -0.5*default_y_feet_offset_m_*cos(stp_data[1].position_data.left_foot_pose.yaw);
972  }
973  else
974  {
975  stp_data[1].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
976  stp_data[1].position_data.left_foot_pose.yaw = stp_data[1].position_data.left_foot_pose.yaw + (double)direction*rotate_step_angle_rad_;
977 
978  if(fabs(stp_data[1].position_data.left_foot_pose.yaw) > 2.0*M_PI)
979  stp_data[1].position_data.left_foot_pose.yaw += -2.0*M_PI*sign(stp_data[1].position_data.left_foot_pose.yaw);
980 
981 
982  stp_data[1].position_data.left_foot_pose.x = -0.5*default_y_feet_offset_m_*sin(stp_data[1].position_data.left_foot_pose.yaw);
983  stp_data[1].position_data.left_foot_pose.y = 0.5*default_y_feet_offset_m_*cos(stp_data[1].position_data.left_foot_pose.yaw);
984  }
985 
986  for(int stp_idx = 2; stp_idx < num_of_step_-2; stp_idx++)
987  {
988  stp_data[stp_idx] = stp_data[stp_idx-1];
989  stp_data[stp_idx].time_data.abs_step_time += step_time_sec_;
990  if(stp_data[stp_idx].position_data.moving_foot == thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING)
991  {
992  stp_data[stp_idx].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
993  stp_data[stp_idx].position_data.right_foot_pose.yaw = stp_data[stp_idx].position_data.right_foot_pose.yaw + (double)direction*rotate_step_angle_rad_;
994 
995  if(fabs(stp_data[stp_idx].position_data.right_foot_pose.yaw) > 2.0*M_PI)
996  stp_data[stp_idx].position_data.right_foot_pose.yaw += -2.0*M_PI*sign(stp_data[stp_idx].position_data.right_foot_pose.yaw);
997 
998  stp_data[stp_idx].position_data.right_foot_pose.x = 0.5*default_y_feet_offset_m_*sin(stp_data[stp_idx].position_data.right_foot_pose.yaw);
999  stp_data[stp_idx].position_data.right_foot_pose.y = -0.5*default_y_feet_offset_m_*cos(stp_data[stp_idx].position_data.right_foot_pose.yaw);
1000  }
1001  else
1002  {
1003  stp_data[stp_idx].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
1004  stp_data[stp_idx].position_data.left_foot_pose.yaw = stp_data[stp_idx].position_data.left_foot_pose.yaw + (double)direction*rotate_step_angle_rad_;
1005 
1006  if(fabs(stp_data[stp_idx].position_data.left_foot_pose.yaw) > 2.0*M_PI)
1007  stp_data[stp_idx].position_data.left_foot_pose.yaw += -2.0*M_PI*sign(stp_data[stp_idx].position_data.left_foot_pose.yaw);
1008 
1009  stp_data[stp_idx].position_data.left_foot_pose.x = -0.5*default_y_feet_offset_m_*sin(stp_data[stp_idx].position_data.left_foot_pose.yaw);
1010  stp_data[stp_idx].position_data.left_foot_pose.y = 0.5*default_y_feet_offset_m_*cos(stp_data[stp_idx].position_data.left_foot_pose.yaw);
1011 
1012  }
1013 
1014  if(fabs(stp_data[stp_idx].position_data.right_foot_pose.yaw) > M_PI)
1015  stp_data[stp_idx].position_data.right_foot_pose.yaw -= 2.0*M_PI*sign(stp_data[stp_idx].position_data.right_foot_pose.yaw);
1016  if(fabs(stp_data[stp_idx].position_data.left_foot_pose.yaw) > M_PI)
1017  stp_data[stp_idx].position_data.left_foot_pose.yaw -= 2.0*M_PI*sign(stp_data[stp_idx].position_data.left_foot_pose.yaw);
1018  }
1019 
1020  stp_data[num_of_step_-2] = stp_data[num_of_step_-3];
1021  stp_data[num_of_step_-2].time_data.abs_step_time += step_time_sec_;
1022  if(stp_data[num_of_step_-2].position_data.moving_foot == thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING)
1023  {
1024  stp_data[num_of_step_-2].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
1025  stp_data[num_of_step_-2].position_data.right_foot_pose.yaw = stp_data[num_of_step_-2].position_data.left_foot_pose.yaw;
1026  stp_data[num_of_step_-2].position_data.right_foot_pose.x = 0.5*default_y_feet_offset_m_*sin(stp_data[num_of_step_-2].position_data.left_foot_pose.yaw);
1027  stp_data[num_of_step_-2].position_data.right_foot_pose.y = -0.5*default_y_feet_offset_m_*cos(stp_data[num_of_step_-2].position_data.left_foot_pose.yaw);
1028 
1029  }
1030  else
1031  {
1032  stp_data[num_of_step_-2].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
1033  stp_data[num_of_step_-2].position_data.left_foot_pose.yaw = stp_data[num_of_step_-2].position_data.right_foot_pose.yaw;
1034  stp_data[num_of_step_-2].position_data.left_foot_pose.x = -0.5*default_y_feet_offset_m_*sin(stp_data[num_of_step_-2].position_data.right_foot_pose.yaw);
1035  stp_data[num_of_step_-2].position_data.left_foot_pose.y = 0.5*default_y_feet_offset_m_*cos(stp_data[num_of_step_-2].position_data.right_foot_pose.yaw);
1036  }
1037 
1038  stp_data[num_of_step_-1] = stp_data[num_of_step_-2];
1039  stp_data[num_of_step_-1].time_data.abs_step_time += start_end_time_sec_;
1040  stp_data[num_of_step_-1].time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING_ENDING;
1041  stp_data[num_of_step_-1].position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::STANDING;
1042  stp_data[num_of_step_-1].position_data.body_z_swap = 0;
1043 
1044  }
1045 
1046  for(int stp_idx = 0; stp_idx < num_of_step_; stp_idx++)
1047  {
1048  step_data_array_.push_back(stp_data[stp_idx]);
1049  }
1050 }
1051 
1052 
1053 void FootStepGenerator::calcStopStep(const thormang3_walking_module_msgs::StepData& ref_step_data, int direction)
1054 {
1055  thormang3_walking_module_msgs::StepData stp_data;
1056  stp_data = ref_step_data;
1057  stp_data.time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING_ENDING;
1058  stp_data.time_data.abs_step_time += start_end_time_sec_;
1059  stp_data.position_data.body_z_swap = 0;
1060  stp_data.position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::STANDING;
1061 
1062  step_data_array_.push_back(stp_data);
1063 }
1064 
1065 
1066 void FootStepGenerator::calcRightKickStep(thormang3_walking_module_msgs::AddStepDataArray::Request::_step_data_array_type* step_data_array,
1067  const thormang3_walking_module_msgs::StepData& ref_step_data)
1068 {
1069  thormang3_walking_module_msgs::StepData step_data_msg;
1070  //meter
1071  double kick_height = 0.08;
1072  double kick_far = 0.18;
1073  double kick_pitch = 15.0*M_PI/180.0;
1074 
1075  //sec
1076  double kick_time = 0.8;
1077 
1078  step_data_msg = ref_step_data;
1079 
1080  step_data_array->clear();
1081  step_data_array_.clear();
1082 
1083  //Start 1 Step Data
1084  step_data_msg.time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING_STARTING;
1085  step_data_msg.time_data.abs_step_time += kick_time*1.8;
1086  step_data_msg.time_data.dsp_ratio = 1.0;
1087 
1088  step_data_msg.position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::STANDING;
1089  step_data_msg.position_data.foot_z_swap = 0;
1090  step_data_msg.position_data.body_z_swap = 0;
1091  step_data_array_.push_back(step_data_msg);
1092 
1093 
1094  //StepData 2 move back Left Foot
1095  step_data_msg.time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING;
1096  step_data_msg.time_data.abs_step_time += kick_time*1.0;
1097  step_data_msg.time_data.dsp_ratio = 0.0;
1098 
1099  step_data_msg.position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
1100  step_data_msg.position_data.right_foot_pose.x = -0.8*kick_far;
1101  step_data_msg.position_data.right_foot_pose.z += kick_height;
1102  step_data_msg.position_data.right_foot_pose.pitch = kick_pitch;
1103  step_data_msg.position_data.foot_z_swap = 0.05;
1104  step_data_array_.push_back(step_data_msg);
1105 
1106 
1107  //StepData 3 kick
1108  step_data_msg.time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING;
1109  step_data_msg.time_data.abs_step_time += kick_time*1.2;
1110  step_data_msg.time_data.dsp_ratio = 0.0;
1111 
1112  step_data_msg.position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
1113  step_data_msg.position_data.right_foot_pose.x = 1.5*kick_far;
1114  step_data_msg.position_data.right_foot_pose.pitch = -kick_pitch;
1115  step_data_msg.position_data.foot_z_swap = 0.0;
1116  step_data_array_.push_back(step_data_msg);
1117 
1118 
1119  //StepData 4 move back
1120  step_data_msg.time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING;
1121  step_data_msg.time_data.abs_step_time += kick_time*1.2;
1122  step_data_msg.time_data.dsp_ratio = 0.0;
1123 
1124  step_data_msg.position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::RIGHT_FOOT_SWING;
1125  step_data_msg.position_data.right_foot_pose.x = 0;
1126  step_data_msg.position_data.right_foot_pose.z -= kick_height;
1127  step_data_msg.position_data.right_foot_pose.pitch = 0;
1128  step_data_msg.position_data.foot_z_swap = 0.05;
1129  step_data_array_.push_back(step_data_msg);
1130 
1131 
1132  //StepData 5 End
1133  step_data_msg.time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING_ENDING;
1134  step_data_msg.time_data.abs_step_time += kick_time*1.8;
1135  step_data_msg.time_data.dsp_ratio = 0.0;
1136 
1137  step_data_msg.position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::STANDING;
1138  step_data_array_.push_back(step_data_msg);
1139 
1140  for(unsigned int stp_idx = 0; stp_idx < step_data_array_.size(); stp_idx++)
1141  {
1142  step_data_array->push_back(step_data_array_[stp_idx]);
1143  }
1144 
1145 }
1146 
1147 void FootStepGenerator::calcLeftKickStep(thormang3_walking_module_msgs::AddStepDataArray::Request::_step_data_array_type* step_data_array,
1148  const thormang3_walking_module_msgs::StepData& ref_step_data)
1149 {
1150  thormang3_walking_module_msgs::StepData step_data_msg;
1151  //meter
1152  double kick_height = 0.08;
1153  double kick_far = 0.18;
1154  double kick_pitch = 15.0*M_PI/180.0;
1155 
1156  //sec
1157  double kick_time = 0.8;
1158 
1159  step_data_msg = ref_step_data;
1160 
1161  step_data_array->clear();
1162  step_data_array_.clear();
1163 
1164  //Start 1 Step Data
1165  step_data_msg.time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING_STARTING;
1166  step_data_msg.time_data.abs_step_time += kick_time*1.8;
1167  step_data_msg.time_data.dsp_ratio = 1.0;
1168 
1169  step_data_msg.position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::STANDING;
1170  step_data_msg.position_data.foot_z_swap = 0;
1171  step_data_msg.position_data.body_z_swap = 0;
1172  step_data_array_.push_back(step_data_msg);
1173 
1174 
1175  //StepData 2 move back Left Foot
1176  step_data_msg.time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING;
1177  step_data_msg.time_data.abs_step_time += kick_time*1.0;
1178  step_data_msg.time_data.dsp_ratio = 0.0;
1179 
1180  step_data_msg.position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
1181  step_data_msg.position_data.left_foot_pose.x = -0.8*kick_far;
1182  step_data_msg.position_data.left_foot_pose.z += kick_height;
1183  step_data_msg.position_data.left_foot_pose.pitch = kick_pitch;
1184  step_data_msg.position_data.foot_z_swap = 0.05;
1185  step_data_array_.push_back(step_data_msg);
1186 
1187 
1188  //StepData 3 kick
1189  step_data_msg.time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING;
1190  step_data_msg.time_data.abs_step_time += kick_time*1.2;
1191  step_data_msg.time_data.dsp_ratio = 0.0;
1192 
1193  step_data_msg.position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
1194  step_data_msg.position_data.left_foot_pose.x = 1.5*kick_far;
1195  step_data_msg.position_data.left_foot_pose.pitch = -kick_pitch;
1196  step_data_msg.position_data.foot_z_swap = 0.0;
1197  step_data_array_.push_back(step_data_msg);
1198 
1199 
1200  //StepData 4 move back
1201  step_data_msg.time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING;
1202  step_data_msg.time_data.abs_step_time += kick_time*1.2;
1203  step_data_msg.time_data.dsp_ratio = 0.0;
1204 
1205  step_data_msg.position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::LEFT_FOOT_SWING;
1206  step_data_msg.position_data.left_foot_pose.x = 0;
1207  step_data_msg.position_data.left_foot_pose.z -= kick_height;
1208  step_data_msg.position_data.left_foot_pose.pitch = 0;
1209  step_data_msg.position_data.foot_z_swap = 0.05;
1210  step_data_array_.push_back(step_data_msg);
1211 
1212 
1213  //StepData 5 End
1214  step_data_msg.time_data.walking_state = thormang3_walking_module_msgs::StepTimeData::IN_WALKING_ENDING;
1215  step_data_msg.time_data.abs_step_time += kick_time*1.8;
1216  step_data_msg.time_data.dsp_ratio = 0.0;
1217 
1218  step_data_msg.position_data.moving_foot = thormang3_walking_module_msgs::StepPositionData::STANDING;
1219  step_data_array_.push_back(step_data_msg);
1220 
1221  for(unsigned int stp_idx = 0; stp_idx < step_data_array_.size(); stp_idx++)
1222  {
1223  step_data_array->push_back(step_data_array_[stp_idx]);
1224  }
1225 }
1226 
LEFTWARD_WALKING
#define LEFTWARD_WALKING
Definition: robotis_foot_step_generator.h:36
STOP_WALKING
#define STOP_WALKING
Definition: robotis_foot_step_generator.h:32
thormang3::FootStepGenerator::calcRoStep
void calcRoStep(const thormang3_walking_module_msgs::StepData &ref_step_data, int direction)
Definition: robotis_foot_step_generator.cpp:852
thormang3::FootStepGenerator::getTransformationXYZRPY
Eigen::MatrixXd getTransformationXYZRPY(double position_x, double position_y, double position_z, double roll, double pitch, double yaw)
Definition: robotis_foot_step_generator.cpp:87
thormang3::FootStepGenerator::getInverseTransformation
Eigen::MatrixXd getInverseTransformation(Eigen::MatrixXd transform)
Definition: robotis_foot_step_generator.cpp:158
sign
double sign(double n)
Definition: robotis_foot_step_generator.cpp:46
BACKWARD_WALKING
#define BACKWARD_WALKING
Definition: robotis_foot_step_generator.h:34
robotis_foot_step_generator.h
thormang3::FootStepGenerator::calcFBStep
void calcFBStep(const thormang3_walking_module_msgs::StepData &ref_step_data, int direction)
Definition: robotis_foot_step_generator.cpp:615
thormang3::FootStepGenerator::calcStopStep
void calcStopStep(const thormang3_walking_module_msgs::StepData &ref_step_data, int direction)
Definition: robotis_foot_step_generator.cpp:1053
RAD2DEG
#define RAD2DEG
Definition: robotis_foot_step_generator.cpp:44
RIGHT_ROTATING_WALKING
#define RIGHT_ROTATING_WALKING
Definition: robotis_foot_step_generator.h:38
thormang3::FootStepGenerator::getStepData
void getStepData(thormang3_walking_module_msgs::AddStepDataArray::Request::_step_data_array_type *step_data_array, const thormang3_walking_module_msgs::StepData &ref_step_data, int desired_step_type)
Definition: robotis_foot_step_generator.cpp:183
thormang3::FootStepGenerator::calcRightKickStep
void calcRightKickStep(thormang3_walking_module_msgs::AddStepDataArray::Request::_step_data_array_type *step_data_array, const thormang3_walking_module_msgs::StepData &ref_step_data)
Definition: robotis_foot_step_generator.cpp:1066
RIGHTWARD_WALKING
#define RIGHTWARD_WALKING
Definition: robotis_foot_step_generator.h:35
thormang3::FootStepGenerator::calcLeftKickStep
void calcLeftKickStep(thormang3_walking_module_msgs::AddStepDataArray::Request::_step_data_array_type *step_data_array, const thormang3_walking_module_msgs::StepData &ref_step_data)
Definition: robotis_foot_step_generator.cpp:1147
thormang3::FootStepGenerator::calcStep
bool calcStep(const thormang3_walking_module_msgs::StepData &ref_step_data, int previous_step_type, int desired_step_type)
Definition: robotis_foot_step_generator.cpp:304
FORWARD_WALKING
#define FORWARD_WALKING
Definition: robotis_foot_step_generator.h:33
LEFT_ROTATING_WALKING
#define LEFT_ROTATING_WALKING
Definition: robotis_foot_step_generator.h:37
thormang3::FootStepGenerator::step_data_array_
thormang3_walking_module_msgs::AddStepDataArray::Request::_step_data_array_type step_data_array_
Definition: robotis_foot_step_generator.h:93
thormang3::FootStepGenerator::getStepDataFromStepData2DArray
void getStepDataFromStepData2DArray(thormang3_walking_module_msgs::AddStepDataArray::Request::_step_data_array_type *step_data_array, const thormang3_walking_module_msgs::StepData &ref_step_data, const thormang3_foot_step_generator::Step2DArray::ConstPtr &request_step_2d)
Definition: robotis_foot_step_generator.cpp:204
thormang3::FootStepGenerator::calcRLStep
void calcRLStep(const thormang3_walking_module_msgs::StepData &ref_step_data, int direction)
Definition: robotis_foot_step_generator.cpp:726
thormang3::FootStepGenerator::getPosefromTransformMatrix
void getPosefromTransformMatrix(const Eigen::MatrixXd &matTransform, double *position_x, double *position_y, double *position_z, double *roll, double *pitch, double *yaw)
Definition: robotis_foot_step_generator.cpp:125
ROS_ERROR
#define ROS_ERROR(...)

thormang3_foot_step_generator
Author(s): Kayman , Jay Song
autogenerated on Mon Jun 10 2019 15:38:28