thormang3_online_walking.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2 * Copyright 2018 ROBOTIS CO., LTD.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *******************************************************************************/
16 /*
17  * thormang3_online_walking.cpp
18  *
19  * Created on: 2016. 6. 10.
20  * Author: Jay Song
21  */
22 
23 #include <iostream>
24 #include <stdio.h>
25 #include "thormang3_walking_module/thormang3_online_walking.h"
26 
27 
28 using namespace thormang3;
29 
30 
31 static const double MMtoM = 0.001;
32 static const double MStoS = 0.001;
33 
34 static const int NO_STEP_IDX = -1;
35 
36 static const double TIME_UNIT = 0.008;
37 
38 static const int IN_WALKING_STARTING = 0;
39 static const int IN_WALKING = 1;
40 static const int IN_WALKING_ENDING = 2;
41 
42 static const int LEFT_FOOT_SWING = 1;
43 static const int RIGHT_FOOT_SWING = 2;
44 static const int STANDING = 3;
45 
46 static const int BalancingPhase0 = 0; // DSP : START
47 static const int BalancingPhase1 = 1; // DSP : R--O->L
48 static const int BalancingPhase2 = 2; // SSP : L_BALANCING1
49 static const int BalancingPhase3 = 3; // SSP : L_BALANCING2
50 static const int BalancingPhase4 = 4; // DSP : R--O<-L
51 static const int BalancingPhase5 = 5; // DSP : R<-O--L
52 static const int BalancingPhase6 = 6; // SSP : R_BALANCING1
53 static const int BalancingPhase7 = 7; // SSP : R_BALANCING2
54 static const int BalancingPhase8 = 8; // DSP : R->O--L
55 static const int BalancingPhase9 = 9; // DSP : END
56 
57 static const int StepDataStatus1 = 1; //
58 static const int StepDataStatus2 = 2; //
59 static const int StepDataStatus3 = 3; //
60 static const int StepDataStatus4 = 4; //
61 
62 
63 THORMANG3OnlineWalking::THORMANG3OnlineWalking()
64 {
65  thormang3_kd_ = new KinematicsDynamics(WholeBody);
66 
67  present_right_foot_pose_.x = 0.0; present_right_foot_pose_.y = -0.5*thormang3_kd_->leg_side_offset_m_;
68  present_right_foot_pose_.z = -0.630*MMtoM;
69  present_right_foot_pose_.roll = 0.0; present_right_foot_pose_.pitch = 0.0; present_right_foot_pose_.yaw = 0.0;
70 
71  present_left_foot_pose_.x = 0.0; present_left_foot_pose_.y = 0.5*thormang3_kd_->leg_side_offset_m_;
72  present_left_foot_pose_.z = -0.630*MMtoM;
73  present_left_foot_pose_.roll = 0.0; present_left_foot_pose_.pitch = 0.0; present_left_foot_pose_.yaw = 0.0;
74 
75  present_body_pose_.x = 0.0; present_body_pose_.y = 0.0; present_body_pose_.z = 0.0;
76  present_body_pose_.roll = 0.0; present_body_pose_.pitch = 0.0; present_body_pose_.yaw = 0;
77 
78  previous_step_right_foot_pose_ = present_right_foot_pose_;
79  previous_step_left_foot_pose_ = present_left_foot_pose_;
80  previous_step_body_pose_ = present_body_pose_;
81 
82  initial_right_foot_pose_ = previous_step_right_foot_pose_;
83  initial_left_foot_pose_ = previous_step_left_foot_pose_;
84  initial_body_pose_ = previous_step_body_pose_;
85 
86  mat_cob_to_rhip_ = robotis_framework::getTranslation4D(0.0, thormang3_kd_->thormang3_link_data_[ID_R_LEG_START]->relative_position_.coeff(1, 0), 0.0);
87  mat_rhip_to_cob_ = robotis_framework::getTranslation4D(0.0, -1.0*(thormang3_kd_->thormang3_link_data_[ID_R_LEG_START]->relative_position_.coeff(1, 0)), 0.0);
88  mat_cob_to_lhip_ = robotis_framework::getTranslation4D(0.0, thormang3_kd_->thormang3_link_data_[ID_L_LEG_START]->relative_position_.coeff(1, 0), 0.0);
89  mat_lhip_to_cob_ = robotis_framework::getTranslation4D(0.0, -1.0*(thormang3_kd_->thormang3_link_data_[ID_L_LEG_START]->relative_position_.coeff(1, 0)), 0.0);
90 
91  mat_rfoot_to_rft_ = robotis_framework::getRotation4d(M_PI,0,0);
92  mat_lfoot_to_lft_ = robotis_framework::getRotation4d(M_PI,0,0);
93  rot_x_pi_3d_ = robotis_framework::getRotationX(M_PI);
94  rot_z_pi_3d_ = robotis_framework::getRotationZ(M_PI);
95 
96 
97  mat_g_to_cob_ = robotis_framework::getTransformationXYZRPY(present_body_pose_.x, present_body_pose_.y, present_body_pose_.z,
98  present_body_pose_.roll, present_body_pose_.pitch, present_body_pose_.yaw);
99 
100  mat_cob_to_g_ = robotis_framework::getInverseTransformation(mat_g_to_cob_);
101 
102  mat_robot_to_cob_ = robotis_framework::getRotation4d(present_body_pose_.roll, present_body_pose_.pitch, 0);
103  mat_cob_to_robot_ = robotis_framework::getInverseTransformation(mat_robot_to_cob_);
104  mat_g_to_robot_ = mat_g_to_cob_ * mat_cob_to_robot_;
105  mat_robot_to_g_ = robotis_framework::getInverseTransformation(mat_g_to_robot_);
106 
107  mat_g_to_rfoot_ = robotis_framework::getTransformationXYZRPY(present_right_foot_pose_.x, present_right_foot_pose_.y, present_right_foot_pose_.z,
108  present_right_foot_pose_.roll, present_right_foot_pose_.pitch, present_right_foot_pose_.yaw);
109  mat_g_to_lfoot_ = robotis_framework::getTransformationXYZRPY(present_left_foot_pose_.x, present_left_foot_pose_.y, present_left_foot_pose_.z,
110  present_left_foot_pose_.roll, present_left_foot_pose_.pitch, present_left_foot_pose_.yaw);
111 
112  mat_robot_to_rfoot_ = mat_robot_to_g_ * mat_g_to_rfoot_;
113  mat_robot_to_lfoot_ = mat_robot_to_g_ * mat_g_to_lfoot_;
114 
115  rhip_to_rfoot_pose_ = robotis_framework::getPose3DfromTransformMatrix((mat_rhip_to_cob_ * mat_cob_to_robot_) * mat_robot_to_rfoot_);
116  lhip_to_lfoot_pose_ = robotis_framework::getPose3DfromTransformMatrix((mat_lhip_to_cob_ * mat_cob_to_robot_) * mat_robot_to_lfoot_);
117 
118  thormang3_kd_->calcInverseKinematicsForRightLeg(&r_leg_out_angle_rad_[0], rhip_to_rfoot_pose_.x, rhip_to_rfoot_pose_.y, rhip_to_rfoot_pose_.z, rhip_to_rfoot_pose_.roll, rhip_to_rfoot_pose_.pitch, rhip_to_rfoot_pose_.yaw);
119  thormang3_kd_->calcInverseKinematicsForLeftLeg(&l_leg_out_angle_rad_[0], lhip_to_lfoot_pose_.x, lhip_to_lfoot_pose_.y, lhip_to_lfoot_pose_.z, lhip_to_lfoot_pose_.roll, lhip_to_lfoot_pose_.pitch, lhip_to_lfoot_pose_.yaw);
120 
121 
122  r_shoulder_dir_ = thormang3_kd_->thormang3_link_data_[ID_R_ARM_START+2*0]->joint_axis_.coeff(1, 0);
123  r_elbow_dir_ = thormang3_kd_->thormang3_link_data_[ID_R_ARM_START+2*3]->joint_axis_.coeff(2, 0);
124  l_shoulder_dir_ = thormang3_kd_->thormang3_link_data_[ID_L_ARM_START+2*0]->joint_axis_.coeff(1, 0);
125  l_elbow_dir_ = thormang3_kd_->thormang3_link_data_[ID_L_ARM_START+2*3]->joint_axis_.coeff(2, 0);
126 
127 
128  r_init_shoulder_angle_rad_ = r_init_elbow_angle_rad_ = r_shoulder_out_angle_rad_ = r_elbow_out_angle_rad_ = 0;
129  l_init_shoulder_angle_rad_ = l_init_elbow_angle_rad_ = l_shoulder_out_angle_rad_ = l_elbow_out_angle_rad_ = 0;
130 
131  goal_waist_yaw_angle_rad_ = 0.0*M_PI;
132 
133  reference_step_data_for_addition_.position_data.moving_foot = STANDING;
134  reference_step_data_for_addition_.position_data.elbow_swing_gain = 0.1;
135  reference_step_data_for_addition_.position_data.shoulder_swing_gain = 0.05;
136  reference_step_data_for_addition_.position_data.foot_z_swap = 0.0;
137  reference_step_data_for_addition_.position_data.waist_pitch_angle = 0.0;
138  reference_step_data_for_addition_.position_data.waist_yaw_angle = goal_waist_yaw_angle_rad_;
139  reference_step_data_for_addition_.position_data.body_z_swap = 0.0;
140  reference_step_data_for_addition_.position_data.body_pose = previous_step_body_pose_;
141  reference_step_data_for_addition_.position_data.right_foot_pose = previous_step_right_foot_pose_;
142  reference_step_data_for_addition_.position_data.left_foot_pose = previous_step_left_foot_pose_;
143  reference_step_data_for_addition_.time_data.walking_state = IN_WALKING_ENDING;
144  reference_step_data_for_addition_.time_data.abs_step_time = 1.6;
145  reference_step_data_for_addition_.time_data.dsp_ratio = 0.2;
146  reference_step_data_for_addition_.time_data.start_time_delay_ratio_x = 0.0;
147  reference_step_data_for_addition_.time_data.start_time_delay_ratio_y = 0.0;
148  reference_step_data_for_addition_.time_data.start_time_delay_ratio_z = 0.0;
149  reference_step_data_for_addition_.time_data.start_time_delay_ratio_roll = 0.0;
150  reference_step_data_for_addition_.time_data.start_time_delay_ratio_pitch = 0.0;
151  reference_step_data_for_addition_.time_data.start_time_delay_ratio_yaw = 0.0;
152  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_x = 0.0;
153  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_y = 0.0;
154  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_z = 0.0;
155  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_roll = 0.0;
156  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_pitch = 0.0;
157  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_yaw = 0.0;
158 
159  present_waist_yaw_angle_rad_ = goal_waist_yaw_angle_rad_;
160  previous_step_waist_yaw_angle_rad_ = goal_waist_yaw_angle_rad_;
161 
162  shouler_swing_gain_ = 0.05;
163  elbow_swing_gain_ = 0.1;
164 
165  real_running = false; ctrl_running = false;
166  current_step_data_status_ = StepDataStatus4;
167 
168  walking_time_ = 0; reference_time_ = 0;
169  balancing_index_ = BalancingPhase0;
170 
171  preview_time_ = 1.6;
172  preview_size_ = round(preview_time_/TIME_UNIT);
173 
174  //These parameters are for preview control
175  k_s_ = 0;
176  current_start_idx_for_ref_zmp_ = 0;
177  ref_zmp_x_at_this_time_ = 0;
178  ref_zmp_y_at_this_time_ = 0;
179  sum_of_zmp_x_ = 0;
180  sum_of_zmp_y_ = 0;
181  sum_of_cx_ = 0;
182  sum_of_cy_ = 0;
183 
184  // variables for balance
185  hip_roll_feedforward_angle_rad_ = 0;
186 
187  current_right_fx_N_ = current_right_fy_N_ = current_right_fz_N_ = 0;
188  current_right_tx_Nm_ = current_right_ty_Nm_ = current_right_tz_Nm_ = 0;
189  current_left_fx_N_ = current_left_fy_N_ = current_left_fz_N_ = 0;
190  current_left_tx_Nm_ = current_left_ty_Nm_ = current_left_tz_Nm_ = 0;
191 
192  current_imu_roll_rad_ = current_imu_pitch_rad_ = 0;
193  current_gyro_roll_rad_per_sec_ = current_gyro_pitch_rad_per_sec_ = 0;
194 
195  total_mass_of_robot_ = thormang3_kd_->calcTotalMass(0);
196 
197  right_dsp_fz_N_ = -1.0*(total_mass_of_robot_)*9.8*0.5;
198  right_ssp_fz_N_ = -1.0*(total_mass_of_robot_)*9.8;
199  left_dsp_fz_N_ = -1.0*(total_mass_of_robot_)*9.8*0.5;
200  left_ssp_fz_N_ = -1.0*(total_mass_of_robot_)*9.8;
201 
202  left_fz_trajectory_start_time_ = 0;
203  left_fz_trajectory_end_time_ = 0;
204  left_fz_trajectory_target_ = left_dsp_fz_N_;
205  left_fz_trajectory_shift_ = left_dsp_fz_N_;
206 
207 
208  balance_error_ = thormang3::BalanceControlError::NoError;
209  quat_current_imu_.w() = cos(0.5*M_PI);
210  quat_current_imu_.x() = sin(0.5*M_PI);
211  quat_current_imu_.y() = 0;
212  quat_current_imu_.z() = 0;
213 }
214 
215 THORMANG3OnlineWalking::~THORMANG3OnlineWalking()
216 { }
217 
218 bool THORMANG3OnlineWalking::setInitialPose(double r_foot_x, double r_foot_y, double r_foot_z, double r_foot_roll, double r_foot_pitch, double r_foot_yaw,
219  double l_foot_x, double l_foot_y, double l_foot_z, double l_foot_roll, double l_foot_pitch, double l_foot_yaw,
220  double center_of_body_x, double center_of_body_y, double center_of_body_z,
221  double center_of_body_roll, double center_of_body_pitch, double center_of_body_yaw)
222 {
223 
224  if(real_running || ctrl_running)
225  return false;
226 
227  previous_step_right_foot_pose_.x = r_foot_x;
228  previous_step_right_foot_pose_.y = r_foot_y;
229  previous_step_right_foot_pose_.z = r_foot_z;
230  previous_step_right_foot_pose_.roll = r_foot_roll;
231  previous_step_right_foot_pose_.pitch = r_foot_pitch;
232  previous_step_right_foot_pose_.yaw = r_foot_yaw;
233 
234  previous_step_left_foot_pose_.x = l_foot_x;
235  previous_step_left_foot_pose_.y = l_foot_y;
236  previous_step_left_foot_pose_.z = l_foot_z;
237  previous_step_left_foot_pose_.roll = l_foot_roll;
238  previous_step_left_foot_pose_.pitch = l_foot_pitch;
239  previous_step_left_foot_pose_.yaw = l_foot_yaw;
240 
241  previous_step_body_pose_.x = center_of_body_x;
242  previous_step_body_pose_.y = center_of_body_y;
243  previous_step_body_pose_.z = center_of_body_z;
244  previous_step_body_pose_.roll = center_of_body_roll;
245  previous_step_body_pose_.pitch = center_of_body_pitch;
246  previous_step_body_pose_.yaw = center_of_body_yaw;
247 
248  initial_right_foot_pose_ = previous_step_right_foot_pose_;
249  initial_left_foot_pose_ = previous_step_left_foot_pose_;
250  initial_body_pose_ = previous_step_body_pose_;
251 
252  return true;
253 }
254 
255 void THORMANG3OnlineWalking::setInitalWaistYawAngle(double waist_yaw_angle_rad)
256 {
257  goal_waist_yaw_angle_rad_ = waist_yaw_angle_rad;
258 }
259 
260 void THORMANG3OnlineWalking::setInitialRightShoulderAngle(double shoulder_angle_rad)
261 {
262  r_init_shoulder_angle_rad_ = shoulder_angle_rad;
263 }
264 
265 void THORMANG3OnlineWalking::setInitialLeftShoulderAngle(double shoulder_angle_rad)
266 {
267  l_init_shoulder_angle_rad_ = shoulder_angle_rad;
268 }
269 
270 void THORMANG3OnlineWalking::setInitialRightElbowAngle(double elbow_angle_rad)
271 {
272  r_init_elbow_angle_rad_ = elbow_angle_rad;
273 }
274 
275 void THORMANG3OnlineWalking::setInitialLeftElbowAngle(double elbow_angle_rad)
276 {
277  l_init_elbow_angle_rad_ = elbow_angle_rad;
278 }
279 
280 void THORMANG3OnlineWalking::setCurrentIMUSensorOutput(double gyro_x, double gyro_y, double quat_x, double quat_y, double quat_z, double quat_w)
281 {
282  imu_data_mutex_lock_.lock();
283 
284  current_gyro_roll_rad_per_sec_ = gyro_x;
285  current_gyro_pitch_rad_per_sec_ = gyro_y;
286 
287  quat_current_imu_ = Eigen::Quaterniond(quat_w, quat_x, quat_y, quat_z);
288 
289  mat_current_imu_ = (rot_x_pi_3d_ * quat_current_imu_.toRotationMatrix()) * rot_z_pi_3d_;
290 
291  current_imu_roll_rad_ = atan2( mat_current_imu_.coeff(2,1), mat_current_imu_.coeff(2,2));
292  current_imu_pitch_rad_ = atan2(-mat_current_imu_.coeff(2,0), sqrt(robotis_framework::powDI(mat_current_imu_.coeff(2,1), 2) + robotis_framework::powDI(mat_current_imu_.coeff(2,2), 2)));
293 
294  imu_data_mutex_lock_.unlock();
295 }
296 
297 void THORMANG3OnlineWalking::initialize()
298 {
299  if(real_running)
300  return;
301 
302  step_data_mutex_lock_.lock();
303  added_step_data_.clear();
304 
305  // initialize balance
306  balance_ctrl_.initialize(TIME_UNIT*1000.0);
307  balance_ctrl_.setGyroBalanceEnable(true);
308  balance_ctrl_.setOrientationBalanceEnable(true);
309  balance_ctrl_.setForceTorqueBalanceEnable(true);
310 
311  // load balance offset gain
312  init_balance_offset_ = false;
313  std::string balance_offset_path = ros::package::getPath("thormang3_walking_module") + "/config/balance_offset.yaml";
314  parseBalanceOffsetData(balance_offset_path);
315 
316  //Initialize Time
317  walking_time_ = 0; reference_time_ = 0;
318 
319  present_right_foot_pose_ = previous_step_right_foot_pose_;
320  present_left_foot_pose_ = previous_step_left_foot_pose_;
321  present_body_pose_ = previous_step_body_pose_;
322 
323  mat_g_to_cob_ = robotis_framework::getTransformationXYZRPY(previous_step_body_pose_.x, previous_step_body_pose_.y, previous_step_body_pose_.z,
324  previous_step_body_pose_.roll, previous_step_body_pose_.pitch, previous_step_body_pose_.yaw);
325 
326  mat_cob_to_g_ = robotis_framework::getInverseTransformation(mat_g_to_cob_);
327 
328  mat_robot_to_cob_ = robotis_framework::getRotation4d(previous_step_body_pose_.roll, previous_step_body_pose_.pitch, 0);
329  mat_cob_to_robot_ = robotis_framework::getInverseTransformation(mat_robot_to_cob_);
330  mat_g_to_robot_ = mat_g_to_cob_ * mat_cob_to_robot_;
331  mat_robot_to_g_ = robotis_framework::getInverseTransformation(mat_g_to_robot_);
332 
333 
334  mat_g_to_rfoot_ = robotis_framework::getTransformationXYZRPY(previous_step_right_foot_pose_.x, previous_step_right_foot_pose_.y, previous_step_right_foot_pose_.z,
335  previous_step_right_foot_pose_.roll, previous_step_right_foot_pose_.pitch, previous_step_right_foot_pose_.yaw);
336  mat_g_to_lfoot_ = robotis_framework::getTransformationXYZRPY(previous_step_left_foot_pose_.x, previous_step_left_foot_pose_.y, previous_step_left_foot_pose_.z,
337  previous_step_left_foot_pose_.roll, previous_step_left_foot_pose_.pitch, previous_step_left_foot_pose_.yaw);
338 
339  mat_robot_to_rfoot_ = mat_robot_to_g_*mat_g_to_rfoot_;
340  mat_robot_to_lfoot_ = mat_robot_to_g_*mat_g_to_lfoot_;
341 
342  balance_ctrl_.process(&balance_error_, &mat_robot_to_cob_modified_, &mat_robot_to_rf_modified_, &mat_robot_to_lf_modified_);
343  mat_cob_to_robot_modified_ = robotis_framework::getInverseTransformation(mat_robot_to_cob_modified_);
344 
345  rhip_to_rfoot_pose_ = robotis_framework::getPose3DfromTransformMatrix((mat_rhip_to_cob_ * mat_cob_to_robot_modified_) * mat_robot_to_rf_modified_);
346  lhip_to_lfoot_pose_ = robotis_framework::getPose3DfromTransformMatrix((mat_lhip_to_cob_ * mat_cob_to_robot_modified_) * mat_robot_to_lf_modified_);
347 
348  if(thormang3_kd_->calcInverseKinematicsForRightLeg(&r_leg_out_angle_rad_[0], rhip_to_rfoot_pose_.x, rhip_to_rfoot_pose_.y, rhip_to_rfoot_pose_.z, rhip_to_rfoot_pose_.roll, rhip_to_rfoot_pose_.pitch, rhip_to_rfoot_pose_.yaw) == false)
349  {
350  printf("IK not Solved EPR : %f %f %f %f %f %f\n", rhip_to_rfoot_pose_.x, rhip_to_rfoot_pose_.y, rhip_to_rfoot_pose_.z, rhip_to_rfoot_pose_.roll, rhip_to_rfoot_pose_.pitch, rhip_to_rfoot_pose_.yaw);
351  return;
352  }
353 
354  if(thormang3_kd_->calcInverseKinematicsForLeftLeg(&l_leg_out_angle_rad_[0], lhip_to_lfoot_pose_.x, lhip_to_lfoot_pose_.y, lhip_to_lfoot_pose_.z, lhip_to_lfoot_pose_.roll, lhip_to_lfoot_pose_.pitch, lhip_to_lfoot_pose_.yaw) == false)
355  {
356  printf("IK not Solved EPL : %f %f %f %f %f %f\n", lhip_to_lfoot_pose_.x, lhip_to_lfoot_pose_.y, lhip_to_lfoot_pose_.z, lhip_to_lfoot_pose_.roll, lhip_to_lfoot_pose_.pitch, lhip_to_lfoot_pose_.yaw);
357  return;
358  }
359 
360  for(int angle_idx = 0; angle_idx < 6; angle_idx++)
361  {
362  out_angle_rad_[angle_idx+0] = r_leg_out_angle_rad_[angle_idx];
363  out_angle_rad_[angle_idx+6] = l_leg_out_angle_rad_[angle_idx];
364  curr_angle_rad_[angle_idx+0] = r_leg_out_angle_rad_[angle_idx];
365  curr_angle_rad_[angle_idx+6] = l_leg_out_angle_rad_[angle_idx];
366  }
367 
368  for(int feed_forward_idx = 0; feed_forward_idx < 12; feed_forward_idx++)
369  {
370  leg_angle_feed_back_[feed_forward_idx].p_gain_ = 0;
371  leg_angle_feed_back_[feed_forward_idx].d_gain_ = 0;
372  }
373 
374  reference_step_data_for_addition_.position_data.moving_foot = STANDING;
375  reference_step_data_for_addition_.position_data.elbow_swing_gain = 0.0;
376  reference_step_data_for_addition_.position_data.shoulder_swing_gain = 0.0;
377  reference_step_data_for_addition_.position_data.foot_z_swap = 0.0;
378  reference_step_data_for_addition_.position_data.waist_pitch_angle = 0.0;
379  reference_step_data_for_addition_.position_data.waist_yaw_angle = goal_waist_yaw_angle_rad_;
380  reference_step_data_for_addition_.position_data.body_z_swap = 0.0;
381  reference_step_data_for_addition_.position_data.body_pose = previous_step_body_pose_;
382  reference_step_data_for_addition_.position_data.right_foot_pose = previous_step_right_foot_pose_;
383  reference_step_data_for_addition_.position_data.left_foot_pose = previous_step_left_foot_pose_;
384  reference_step_data_for_addition_.time_data.walking_state = IN_WALKING_ENDING;
385  reference_step_data_for_addition_.time_data.abs_step_time = 0.0;
386  reference_step_data_for_addition_.time_data.dsp_ratio = 0.2;
387  reference_step_data_for_addition_.time_data.start_time_delay_ratio_x = 0.0;
388  reference_step_data_for_addition_.time_data.start_time_delay_ratio_y = 0.0;
389  reference_step_data_for_addition_.time_data.start_time_delay_ratio_z = 0.0;
390  reference_step_data_for_addition_.time_data.start_time_delay_ratio_roll = 0.0;
391  reference_step_data_for_addition_.time_data.start_time_delay_ratio_pitch = 0.0;
392  reference_step_data_for_addition_.time_data.start_time_delay_ratio_yaw = 0.0;
393  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_x = 0.0;
394  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_y = 0.0;
395  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_z = 0.0;
396  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_roll = 0.0;
397  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_pitch = 0.0;
398  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_yaw = 0.0;
399 
400  present_waist_yaw_angle_rad_ = goal_waist_yaw_angle_rad_;
401  previous_step_waist_yaw_angle_rad_ = goal_waist_yaw_angle_rad_;
402 
403  current_step_data_status_ = StepDataStatus4;
404 
405  // Initialize Matrix for Preview Control
406  double t = 0;
407  if(TIME_UNIT < 1.0)
408  t=TIME_UNIT;
409  else
410  t=TIME_UNIT/1000.0;
411 
412  preview_size_ = round(preview_time_/TIME_UNIT);
413 
414  step_idx_data_.resize(preview_size_);
415  step_idx_data_.fill(NO_STEP_IDX);
416  current_start_idx_for_ref_zmp_ = 0;
417  reference_zmp_x_.resize(preview_size_, 1);
418  reference_zmp_x_.fill(0.5*(present_right_foot_pose_.x + present_left_foot_pose_.x));
419  reference_zmp_y_.resize(preview_size_, 1);
420  reference_zmp_y_.fill(0.5*(present_right_foot_pose_.y + present_left_foot_pose_.y));
421 
422  A_.resize(3,3); b_.resize(3,1); c_.resize(1,3);
423  A_ << 1, t, t*t/2.0,
424  0, 1, t,
425  0, 0, 1;
426  b_(0,0) = t*t*t/6.0;
427  b_(1,0) = t*t/2.0;
428  b_(2,0) = t;
429 
430  c_(0,0) = 1; c_(0,1) = 0; c_(0,2) = -500.0*0.001/GRAVITY_ACCELERATION;
431 
432  k_s_ = 608.900142915471; //500.0
433  //k_s_ = 583.938789769793; //600.0
434 
435  k_x_.resize(1,3);
436  k_x_ << 35904.1790662895, 8609.63092261379, 112.710622775482; // 500
437  //k_x_ << 37452.749706802, 9756.07453643388, 121.001738374095; // 600
438 
439  f_.resize(1, preview_size_);
440  //500
441  f_ << 608.9001429, 760.1988656, 921.629618, 1034.769891, 1089.313783, 1096.765451, 1074.295061, 1036.842257, 994.5569472, 953.0295724,
442  914.5708045, 879.5752596, 847.5633512, 817.8237132, 789.7293917, 762.8419074, 736.9038288, 711.7889296, 687.4484508, 663.8698037,
443  641.050971, 618.987811, 597.6697941, 577.0801943, 557.1979969, 537.9999833, 519.4623366, 501.5616312, 484.2753137, 467.581849,
444  451.4606896, 435.8921765, 420.8574331, 406.3382777, 392.3171623, 378.7771307, 365.7017923, 353.0753019, 340.8823445, 329.1081203,
445  317.7383294, 306.759157, 296.1572572, 285.919738, 276.0341458, 266.4884502, 257.2710302, 248.3706597, 239.7764944, 231.4780587,
446  223.4652335, 215.7282439, 208.2576474, 201.0443231, 194.0794606, 187.3545495, 180.8613691, 174.5919788, 168.5387087, 162.6941501,
447  157.0511468, 151.6027868, 146.3423936, 141.2635185, 136.3599326, 131.62562, 127.0547695, 122.6417688, 118.3811969, 114.2678179,
448  110.2965751, 106.462584, 102.7611275, 99.18764938, 95.73774938, 92.40717757, 89.19182939, 86.08774068, 83.0910829, 80.19815849,
449  77.40539648, 74.70934812, 72.10668274, 69.59418373, 67.16874468, 64.82736558, 62.56714924, 60.38529775, 58.27910913, 56.24597404,
450  54.28337262, 52.38887145, 50.5601206, 48.79485075, 47.09087052, 45.44606371, 43.8583868, 42.32586646, 40.84659712, 39.4187387,
451  38.04051434, 36.71020825, 35.42616363, 34.18678064, 32.99051448, 31.83587345, 30.72141721, 29.64575495, 28.60754377, 27.60548695,
452  26.63833247, 25.70487138, 24.80393641, 23.93440049, 23.09517539, 22.28521038, 21.50349096, 20.74903761, 20.0209046, 19.3181788,
453  18.63997859, 17.98545279, 17.35377958, 16.74416551, 16.15584454, 15.58807707, 15.04014906, 14.51137112, 14.00107771, 13.50862627,
454  13.03339646, 12.57478939, 12.13222688, 11.70515076, 11.29302214, 10.89532081, 10.51154456, 10.14120854, 9.783844714, 9.439001248,
455  9.106241955, 8.78514576, 8.475306179, 8.176330819, 7.887840885, 7.609470721, 7.340867346, 7.081690027, 6.83160985, 6.590309314,
456  6.357481938, 6.132831881, 5.916073573, 5.706931359, 5.505139163, 5.310440149, 5.122586408, 4.941338646, 4.766465888, 4.597745188,
457  4.434961356, 4.277906685, 4.126380694, 3.980189879, 3.839147471, 3.703073201, 3.571793078, 3.445139169, 3.322949391, 3.205067309,
458  3.091341936, 2.981627549, 2.875783507, 2.773674068, 2.675168228, 2.58013955, 2.488466009, 2.400029838, 2.314717381, 2.232418947,
459  2.153028678, 2.076444411, 2.002567552, 1.931302952, 1.862558786, 1.796246438, 1.732280393, 1.670578121, 1.611059983, 1.553649123,
460  1.498271374, 1.444855165, 1.393331431, 1.343633522, 1.295697125, 1.249460178, 1.204862791, 1.161847176, 1.120357568, 1.080340156;
461 
462  //600
463 // f_ << 583.9387898, 750.9225137, 918.1585887, 1025.28224, 1068.572334, 1066.00235, 1038.071691, 1000.083662, 960.8853819, 924.332612,
464 // 891.3091699, 861.3465762, 833.615981, 807.3964237, 782.2132154, 757.8151538, 734.0999123, 711.0427972, 688.6479404, 666.9221777,
465 // 645.8649499, 625.4670304, 605.7128357, 586.5833441, 568.0583614, 550.1178678, 532.7426248, 515.914327, 499.6155377, 483.8295609,
466 // 468.5403224, 453.7322864, 439.3904039, 425.5000849, 412.0471825, 399.0179825, 386.3991934, 374.1779373, 362.3417377, 350.8785086,
467 // 339.7765423, 329.0244973, 318.6113874, 308.5265697, 298.7597346, 289.3008946, 280.140375, 271.2688034, 262.6771012, 254.3564733,
468 // 246.2984005, 238.4946298, 230.9371668, 223.6182676, 216.5304305, 209.6663889, 203.019104, 196.5817571, 190.3477436, 184.3106657,
469 // 178.464326, 172.8027215, 167.3200373, 162.010641, 156.8690766, 151.8900593, 147.0684701, 142.3993506, 137.8778978, 133.4994595,
470 // 129.2595297, 125.1537435, 121.1778731, 117.3278237, 113.5996286, 109.9894461, 106.4935549, 103.1083507, 99.83034228, 96.6561483,
471 // 93.58249356, 90.6062058, 87.72421248, 84.93353763, 82.23129884, 79.61470434, 77.08105014, 74.62771731, 72.25216927, 69.95194925,
472 // 67.72467778, 65.56805026, 63.47983461, 61.45786901, 59.50005969, 57.6043788, 55.76886233, 53.99160812, 52.27077392, 50.60457551,
473 // 48.99128487, 47.42922844, 45.91678537, 44.4523859, 43.03450975, 41.66168457, 40.33248441, 39.04552829, 37.79947877, 36.5930406,
474 // 35.42495939, 34.29402031, 33.19904685, 32.13889964, 31.11247526, 30.11870513, 29.15655437, 28.22502079, 27.32313386, 26.44995365,
475 // 25.60456997, 24.78610134, 23.99369414, 23.22652172, 22.48378355, 21.76470439, 21.06853351, 20.39454392, 19.74203158, 19.11031475,
476 // 18.49873322, 17.90664768, 17.33343901, 16.77850772, 16.24127324, 15.7211734, 15.21766381, 14.73021731, 14.25832344, 13.80148787,
477 // 13.35923194, 12.93109216, 12.51661969, 12.11537993, 11.72695202, 11.35092848, 10.98691468, 10.63452855, 10.29340009, 9.963171053,
478 // 9.643494539, 9.334034646, 9.034466125, 8.74447404, 8.463753446, 8.19200907, 7.928955006, 7.674314421, 7.427819265, 7.189209996,
479 // 6.958235311, 6.734651883, 6.518224112, 6.308723877, 6.105930303, 5.909629529, 5.719614489, 5.535684693, 5.357646024, 5.18531053,
480 // 5.018496236, 4.857026947, 4.700732073, 4.549446444, 4.403010145, 4.261268345, 4.124071137, 3.991273383, 3.862734561, 3.738318623,
481 // 3.617893846, 3.501332704, 3.388511725, 3.27931137, 3.173615908, 3.071313287, 2.97229503, 2.87645611, 2.783694848, 2.693912803,
482 // 2.607014671, 2.522908184, 2.441504017, 2.362715689, 2.286459477, 2.212654328, 2.141221772, 2.072085843, 2.005172996, 1.940412033;
483 
484  u_x.resize(1,1);
485  u_y.resize(1,1);
486 
487  x_lipm_.resize(3, 1); y_lipm_.resize(3, 1);
488  x_lipm_.fill(0.0); y_lipm_.fill(0.0);
489 
490  step_data_mutex_lock_.unlock();
491 
492  r_shoulder_out_angle_rad_ = r_shoulder_dir_*(mat_robot_to_rfoot_.coeff(0, 3) - mat_robot_to_lfoot_.coeff(0, 3))*shouler_swing_gain_ + r_init_shoulder_angle_rad_;
493  l_shoulder_out_angle_rad_ = l_shoulder_dir_*(mat_robot_to_lfoot_.coeff(0, 3) - mat_robot_to_rfoot_.coeff(0, 3))*shouler_swing_gain_ + l_init_shoulder_angle_rad_;
494  r_elbow_out_angle_rad_ = r_elbow_dir_*(mat_robot_to_rfoot_.coeff(0, 3) - mat_robot_to_lfoot_.coeff(0, 3))*elbow_swing_gain_ + r_init_elbow_angle_rad_;
495  l_elbow_out_angle_rad_ = l_elbow_dir_*(mat_robot_to_lfoot_.coeff(0, 3) - mat_robot_to_rfoot_.coeff(0, 3))*elbow_swing_gain_ + l_init_elbow_angle_rad_;
496 
497  left_fz_trajectory_start_time_ = 0;
498  left_fz_trajectory_end_time_ = 0;
499  left_fz_trajectory_target_ = left_dsp_fz_N_;
500  left_fz_trajectory_shift_ = left_dsp_fz_N_;
501 
502 }
503 
504 void THORMANG3OnlineWalking::reInitialize()
505 {
506  if(real_running)
507  return;
508 
509  step_data_mutex_lock_.lock();
510  added_step_data_.clear();
511 
512  //Initialize Time
513  walking_time_ = 0; reference_time_ = 0;
514 
515  previous_step_right_foot_pose_ = robotis_framework::getPose3DfromTransformMatrix(mat_robot_to_rfoot_);
516  previous_step_left_foot_pose_ = robotis_framework::getPose3DfromTransformMatrix(mat_robot_to_lfoot_);
517  previous_step_body_pose_ = robotis_framework::getPose3DfromTransformMatrix(mat_robot_to_cob_);
518 
519  present_right_foot_pose_ = previous_step_right_foot_pose_;
520  present_left_foot_pose_ = previous_step_left_foot_pose_;
521  present_body_pose_ = previous_step_body_pose_;
522 
523  mat_g_to_cob_ = robotis_framework::getTransformationXYZRPY(previous_step_body_pose_.x, previous_step_body_pose_.y, previous_step_body_pose_.z,
524  previous_step_body_pose_.roll, previous_step_body_pose_.pitch, previous_step_body_pose_.yaw);
525 
526  mat_cob_to_g_ = robotis_framework::getInverseTransformation(mat_g_to_cob_);
527  mat_g_to_robot_ = mat_g_to_cob_ * mat_cob_to_robot_;
528  mat_robot_to_g_ = robotis_framework::getInverseTransformation(mat_g_to_robot_);
529 
530  mat_g_to_rfoot_ = robotis_framework::getTransformationXYZRPY(previous_step_right_foot_pose_.x, previous_step_right_foot_pose_.y, previous_step_right_foot_pose_.z,
531  previous_step_right_foot_pose_.roll, previous_step_right_foot_pose_.pitch, previous_step_right_foot_pose_.yaw);
532  mat_g_to_lfoot_ = robotis_framework::getTransformationXYZRPY(previous_step_left_foot_pose_.x, previous_step_left_foot_pose_.y, previous_step_left_foot_pose_.z,
533  previous_step_left_foot_pose_.roll, previous_step_left_foot_pose_.pitch, previous_step_left_foot_pose_.yaw);
534 
535  mat_robot_to_rfoot_ = mat_rhip_to_cob_*mat_cob_to_g_*mat_g_to_rfoot_;
536  mat_robot_to_lfoot_ = mat_lhip_to_cob_*mat_cob_to_g_*mat_g_to_lfoot_;
537 
538  balance_ctrl_.process(&balance_error_, &mat_robot_to_cob_modified_, &mat_robot_to_rf_modified_, &mat_robot_to_lf_modified_);
539  mat_cob_to_robot_modified_ = robotis_framework::getInverseTransformation(mat_robot_to_cob_modified_);
540 
541  rhip_to_rfoot_pose_ = robotis_framework::getPose3DfromTransformMatrix((mat_rhip_to_cob_ * mat_cob_to_robot_modified_) * mat_robot_to_rf_modified_);
542  lhip_to_lfoot_pose_ = robotis_framework::getPose3DfromTransformMatrix((mat_lhip_to_cob_ * mat_cob_to_robot_modified_) * mat_robot_to_lf_modified_);
543 
544  if(thormang3_kd_->calcInverseKinematicsForRightLeg(&r_leg_out_angle_rad_[0], rhip_to_rfoot_pose_.x, rhip_to_rfoot_pose_.y, rhip_to_rfoot_pose_.z, rhip_to_rfoot_pose_.roll, rhip_to_rfoot_pose_.pitch, rhip_to_rfoot_pose_.yaw) == false)
545  {
546  printf("IK not Solved EPR : %f %f %f %f %f %f\n", rhip_to_rfoot_pose_.x, rhip_to_rfoot_pose_.y, rhip_to_rfoot_pose_.z, rhip_to_rfoot_pose_.roll, rhip_to_rfoot_pose_.pitch, rhip_to_rfoot_pose_.yaw);
547  return;
548  }
549 
550  if(thormang3_kd_->calcInverseKinematicsForLeftLeg(&l_leg_out_angle_rad_[0], lhip_to_lfoot_pose_.x, lhip_to_lfoot_pose_.y, lhip_to_lfoot_pose_.z, lhip_to_lfoot_pose_.roll, lhip_to_lfoot_pose_.pitch, lhip_to_lfoot_pose_.yaw) == false)
551  {
552  printf("IK not Solved EPL : %f %f %f %f %f %f\n", lhip_to_lfoot_pose_.x, lhip_to_lfoot_pose_.y, lhip_to_lfoot_pose_.z, lhip_to_lfoot_pose_.roll, lhip_to_lfoot_pose_.pitch, lhip_to_lfoot_pose_.yaw);
553  return;
554  }
555 
556  for(int angle_idx = 0; angle_idx < 6; angle_idx++)
557  {
558  out_angle_rad_[angle_idx+0] = r_leg_out_angle_rad_[angle_idx];
559  out_angle_rad_[angle_idx+6] = l_leg_out_angle_rad_[angle_idx];
560  }
561 
562  reference_step_data_for_addition_.position_data.moving_foot = STANDING;
563  reference_step_data_for_addition_.position_data.elbow_swing_gain = 0.0;
564  reference_step_data_for_addition_.position_data.shoulder_swing_gain = 0.0;
565  reference_step_data_for_addition_.position_data.foot_z_swap = 0.0;
566  reference_step_data_for_addition_.position_data.waist_pitch_angle = 0.0;
567  reference_step_data_for_addition_.position_data.waist_yaw_angle = goal_waist_yaw_angle_rad_;
568  reference_step_data_for_addition_.position_data.body_z_swap = 0.0;
569  reference_step_data_for_addition_.position_data.body_pose = previous_step_body_pose_;
570  reference_step_data_for_addition_.position_data.right_foot_pose = previous_step_right_foot_pose_;
571  reference_step_data_for_addition_.position_data.left_foot_pose = previous_step_left_foot_pose_;
572  reference_step_data_for_addition_.time_data.walking_state = IN_WALKING_ENDING;
573  reference_step_data_for_addition_.time_data.abs_step_time = 0.0;
574  reference_step_data_for_addition_.time_data.dsp_ratio = 0.2;
575  reference_step_data_for_addition_.time_data.start_time_delay_ratio_x = 0.0;
576  reference_step_data_for_addition_.time_data.start_time_delay_ratio_y = 0.0;
577  reference_step_data_for_addition_.time_data.start_time_delay_ratio_z = 0.0;
578  reference_step_data_for_addition_.time_data.start_time_delay_ratio_roll = 0.0;
579  reference_step_data_for_addition_.time_data.start_time_delay_ratio_pitch = 0.0;
580  reference_step_data_for_addition_.time_data.start_time_delay_ratio_yaw = 0.0;
581  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_x = 0.0;
582  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_y = 0.0;
583  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_z = 0.0;
584  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_roll = 0.0;
585  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_pitch = 0.0;
586  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_yaw = 0.0;
587 
588  present_waist_yaw_angle_rad_ = goal_waist_yaw_angle_rad_;
589  previous_step_waist_yaw_angle_rad_ = goal_waist_yaw_angle_rad_;
590 
591  current_step_data_status_ = StepDataStatus4;
592 
593  step_idx_data_.fill(NO_STEP_IDX);
594  current_start_idx_for_ref_zmp_ = 0;
595  reference_zmp_x_.fill(0.5*(present_right_foot_pose_.x + present_left_foot_pose_.x));
596  reference_zmp_y_.fill(0.5*(present_right_foot_pose_.y + present_left_foot_pose_.y));
597  sum_of_zmp_x_ = 0.0;
598  sum_of_zmp_y_ = 0.0;
599 
600  sum_of_cx_ = 0.0;
601  sum_of_cy_ = 0.0;
602  x_lipm_.fill(0.0); y_lipm_.fill(0.0);
603 
604  step_data_mutex_lock_.unlock();
605 
606  left_fz_trajectory_start_time_ = 0;
607  left_fz_trajectory_end_time_ = 0;
608  left_fz_trajectory_target_ = left_dsp_fz_N_;
609  left_fz_trajectory_shift_ = left_dsp_fz_N_;
610 }
611 
612 void THORMANG3OnlineWalking::start()
613 {
614  ctrl_running = true;
615  real_running = true;
616 }
617 
618 void THORMANG3OnlineWalking::stop()
619 {
620  ctrl_running = false;
621 }
622 
623 bool THORMANG3OnlineWalking::isRunning()
624 {
625  return real_running;
626 }
627 
628 bool THORMANG3OnlineWalking::addStepData(robotis_framework::StepData step_data)
629 {
630  step_data_mutex_lock_.lock();
631  added_step_data_.push_back(step_data);
632 
633  calcStepIdxData();
634  step_data_mutex_lock_.unlock();
635 
636  return true;
637 }
638 
639 int THORMANG3OnlineWalking::getNumofRemainingUnreservedStepData()
640 {
641  int step_idx = step_idx_data_(preview_size_ - 1);
642  int remain_step_num = 0;
643  if(step_idx != NO_STEP_IDX)
644  {
645  remain_step_num = (added_step_data_.size() - 1 - step_idx);
646  }
647  else
648  {
649  remain_step_num = 0;
650  }
651  return remain_step_num;
652 }
653 
654 void THORMANG3OnlineWalking::eraseLastStepData()
655 {
656  step_data_mutex_lock_.lock();
657  if(getNumofRemainingUnreservedStepData() != 0)
658  {
659  added_step_data_.pop_back();
660  }
661  step_data_mutex_lock_.unlock();
662 }
663 
664 void THORMANG3OnlineWalking::getReferenceStepDatafotAddition(robotis_framework::StepData *ref_step_data_for_addition)
665 {
666  reference_step_data_for_addition_.time_data.start_time_delay_ratio_x = 0.0;
667  reference_step_data_for_addition_.time_data.start_time_delay_ratio_y = 0.0;
668  reference_step_data_for_addition_.time_data.start_time_delay_ratio_z = 0.0;
669  reference_step_data_for_addition_.time_data.start_time_delay_ratio_roll = 0.0;
670  reference_step_data_for_addition_.time_data.start_time_delay_ratio_pitch = 0.0;
671  reference_step_data_for_addition_.time_data.start_time_delay_ratio_yaw = 0.0;
672  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_x = 0.0;
673  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_y = 0.0;
674  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_z = 0.0;
675  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_roll = 0.0;
676  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_pitch = 0.0;
677  reference_step_data_for_addition_.time_data.finish_time_advance_ratio_yaw = 0.0;
678  (*ref_step_data_for_addition) = reference_step_data_for_addition_;
679 }
680 
681 void THORMANG3OnlineWalking::calcStepIdxData()
682 {
683  unsigned int step_idx = 0, previous_step_idx = 0;
684  unsigned int step_data_size = added_step_data_.size();
685  if(added_step_data_.size() == 0)
686  {
687  step_idx_data_.fill(NO_STEP_IDX);
688  current_step_data_status_ = StepDataStatus4;
689  real_running = false;
690  }
691  else
692  {
693  if(walking_time_ >= added_step_data_[0].time_data.abs_step_time - 0.5*MStoS)
694  {
695  previous_step_waist_yaw_angle_rad_ = added_step_data_[0].position_data.waist_yaw_angle;
696  previous_step_left_foot_pose_ = added_step_data_[0].position_data.left_foot_pose;
697  previous_step_right_foot_pose_ = added_step_data_[0].position_data.right_foot_pose;
698  previous_step_body_pose_ = added_step_data_[0].position_data.body_pose;
699  previous_step_body_pose_.x = present_body_pose_.x;
700  previous_step_body_pose_.y = present_body_pose_.y;
701  reference_time_ = added_step_data_[0].time_data.abs_step_time;
702  added_step_data_.erase(added_step_data_.begin());
703  if(added_step_data_.size() == 0)
704  {
705  step_idx_data_.fill(NO_STEP_IDX);
706  current_step_data_status_ = StepDataStatus4;
707  real_running = false;
708  }
709  else
710  {
711  for(int idx = 0; idx < preview_size_; idx++)
712  {
713  //Get STepIDx
714  if(walking_time_ + (idx+1)*TIME_UNIT > added_step_data_[step_data_size -1].time_data.abs_step_time)
715  step_idx_data_(idx) = NO_STEP_IDX;
716  else
717  {
718  for(step_idx = previous_step_idx; step_idx < step_data_size; step_idx++)
719  {
720  if(walking_time_ + (idx+1)*TIME_UNIT <= added_step_data_[step_idx].time_data.abs_step_time)
721  break;
722  }
723  step_idx_data_(idx) = step_idx;
724  previous_step_idx = step_idx;
725  }
726  }
727  }
728  }
729  else
730  {
731  for(int idx = 0; idx < preview_size_; idx++)
732  {
733  //Get StepIdx
734  if(walking_time_ + (idx+1)*TIME_UNIT > added_step_data_[step_data_size -1].time_data.abs_step_time)
735  step_idx_data_(idx) = NO_STEP_IDX;
736  else
737  {
738  for(step_idx = previous_step_idx; step_idx < step_data_size; step_idx++)
739  {
740  if(walking_time_ + (idx+1)*TIME_UNIT <= added_step_data_[step_idx].time_data.abs_step_time)
741  break;
742  }
743  step_idx_data_(idx) = step_idx;
744  previous_step_idx = step_idx;
745  }
746  }
747  }
748  }
749 
750  if(step_idx_data_(preview_size_ - 1) != NO_STEP_IDX)
751  {
752  if(getNumofRemainingUnreservedStepData() != 0)
753  {
754  current_step_data_status_ = StepDataStatus1;
755  reference_step_data_for_addition_ = added_step_data_[step_idx_data_(preview_size_-1)];
756  }
757  else
758  {
759  current_step_data_status_ = StepDataStatus2;
760  reference_step_data_for_addition_ = added_step_data_[step_idx_data_(preview_size_-1)];
761  }
762  }
763  else
764  {
765  if(step_idx_data_(0) != NO_STEP_IDX)
766  {
767  reference_step_data_for_addition_ = added_step_data_[step_idx_data_(0)];
768  reference_step_data_for_addition_.time_data.walking_state = IN_WALKING_ENDING;
769  reference_step_data_for_addition_.time_data.abs_step_time += preview_time_;
770 
771  current_step_data_status_ = StepDataStatus3;
772  }
773  else
774  {
775  reference_step_data_for_addition_.time_data.walking_state = IN_WALKING_ENDING;
776  reference_step_data_for_addition_.time_data.abs_step_time = walking_time_ + preview_time_;
777  current_step_data_status_ = StepDataStatus4;
778  }
779  }
780 }
781 
782 void THORMANG3OnlineWalking::calcRefZMP()
783 {
784  int ref_zmp_idx = 0;
785  int step_idx = 0;
786  if(walking_time_ == 0)
787  {
788  if((step_idx_data_(ref_zmp_idx) == NO_STEP_IDX)/* && (m_StepData.size() == 0)*/)
789  {
790  reference_zmp_x_.fill((present_left_foot_pose_.x + present_right_foot_pose_.x)*0.5);
791  reference_zmp_y_.fill((present_left_foot_pose_.y + present_right_foot_pose_.y)*0.5);
792  return;
793  }
794 
795  for(ref_zmp_idx = 0; ref_zmp_idx < preview_size_; ref_zmp_idx++)
796  {
797  step_idx = step_idx_data_(ref_zmp_idx);
798  if(step_idx == NO_STEP_IDX)
799  {
800  reference_zmp_x_(ref_zmp_idx, 0) = reference_zmp_x_(ref_zmp_idx - 1, 0);
801  reference_zmp_y_(ref_zmp_idx, 0) = reference_zmp_y_(ref_zmp_idx - 1, 0);
802  }
803  else
804  {
805  if(added_step_data_[step_idx].time_data.walking_state == IN_WALKING)
806  {
807  if( added_step_data_[step_idx].position_data.moving_foot == RIGHT_FOOT_SWING )
808  {
809  reference_zmp_x_(ref_zmp_idx, 0) = added_step_data_[step_idx].position_data.left_foot_pose.x;
810  reference_zmp_y_(ref_zmp_idx, 0) = added_step_data_[step_idx].position_data.left_foot_pose.y;
811  }
812  else if( added_step_data_[step_idx].position_data.moving_foot == LEFT_FOOT_SWING )
813  {
814  reference_zmp_x_(ref_zmp_idx, 0) = added_step_data_[step_idx].position_data.right_foot_pose.x;
815  reference_zmp_y_(ref_zmp_idx, 0) = added_step_data_[step_idx].position_data.right_foot_pose.y;
816  }
817  else if( added_step_data_[step_idx].position_data.moving_foot == STANDING )
818  {
819  reference_zmp_x_(ref_zmp_idx, 0) = (added_step_data_[step_idx].position_data.left_foot_pose.x + added_step_data_[step_idx].position_data.right_foot_pose.x)*0.5;
820  reference_zmp_y_(ref_zmp_idx, 0) = (added_step_data_[step_idx].position_data.left_foot_pose.y + added_step_data_[step_idx].position_data.right_foot_pose.y)*0.5;
821  }
822  else
823  {
824  reference_zmp_x_(ref_zmp_idx, 0) = (added_step_data_[step_idx].position_data.left_foot_pose.x + added_step_data_[step_idx].position_data.right_foot_pose.x)*0.5;
825  reference_zmp_y_(ref_zmp_idx, 0) = (added_step_data_[step_idx].position_data.left_foot_pose.y + added_step_data_[step_idx].position_data.right_foot_pose.y)*0.5;
826  }
827  }
828  else if(added_step_data_[step_idx].time_data.walking_state == IN_WALKING_STARTING)
829  {
830  reference_zmp_x_(ref_zmp_idx, 0) = (added_step_data_[step_idx].position_data.left_foot_pose.x + added_step_data_[step_idx].position_data.right_foot_pose.x)*0.5;
831  reference_zmp_y_(ref_zmp_idx, 0) = (added_step_data_[step_idx].position_data.left_foot_pose.y + added_step_data_[step_idx].position_data.right_foot_pose.y)*0.5;
832  }
833  else if(added_step_data_[step_idx].time_data.walking_state == IN_WALKING_ENDING)
834  {
835  reference_zmp_x_(ref_zmp_idx, 0) = (added_step_data_[step_idx].position_data.left_foot_pose.x + added_step_data_[step_idx].position_data.right_foot_pose.x)*0.5;
836  reference_zmp_y_(ref_zmp_idx, 0) = (added_step_data_[step_idx].position_data.left_foot_pose.y + added_step_data_[step_idx].position_data.right_foot_pose.y)*0.5;
837  }
838  else
839  {
840  reference_zmp_x_(ref_zmp_idx, 0) = (added_step_data_[step_idx].position_data.left_foot_pose.x + added_step_data_[step_idx].position_data.right_foot_pose.x)*0.5;
841  reference_zmp_y_(ref_zmp_idx, 0) = (added_step_data_[step_idx].position_data.left_foot_pose.y + added_step_data_[step_idx].position_data.right_foot_pose.y)*0.5;
842  }
843  }
844  }
845  current_start_idx_for_ref_zmp_ = 0;
846  }
847  else
848  {
849  step_idx = step_idx_data_(preview_size_ - 1);
850 
851  if(current_start_idx_for_ref_zmp_ == 0)
852  ref_zmp_idx = preview_size_ - 1;
853  else
854  ref_zmp_idx = current_start_idx_for_ref_zmp_ - 1;
855 
856  if(step_idx == NO_STEP_IDX)
857  {
858  reference_zmp_x_(ref_zmp_idx, 0) = 0.5*(reference_step_data_for_addition_.position_data.right_foot_pose.x + reference_step_data_for_addition_.position_data.left_foot_pose.x);
859  reference_zmp_y_(ref_zmp_idx, 0) = 0.5*(reference_step_data_for_addition_.position_data.right_foot_pose.y + reference_step_data_for_addition_.position_data.left_foot_pose.y);
860  }
861  else
862  {
863  if(added_step_data_[step_idx].time_data.walking_state == IN_WALKING)
864  {
865  if( added_step_data_[step_idx].position_data.moving_foot == RIGHT_FOOT_SWING )
866  {
867  reference_zmp_x_(ref_zmp_idx, 0) = added_step_data_[step_idx].position_data.left_foot_pose.x;
868  reference_zmp_y_(ref_zmp_idx, 0) = added_step_data_[step_idx].position_data.left_foot_pose.y;
869  }
870  else if( added_step_data_[step_idx].position_data.moving_foot == LEFT_FOOT_SWING )
871  {
872  reference_zmp_x_(ref_zmp_idx, 0) = added_step_data_[step_idx].position_data.right_foot_pose.x;
873  reference_zmp_y_(ref_zmp_idx, 0) = added_step_data_[step_idx].position_data.right_foot_pose.y;
874  }
875  else if( added_step_data_[step_idx].position_data.moving_foot == STANDING )
876  {
877  reference_zmp_x_(ref_zmp_idx, 0) = (added_step_data_[step_idx].position_data.left_foot_pose.x + added_step_data_[step_idx].position_data.right_foot_pose.x)*0.5;
878  reference_zmp_y_(ref_zmp_idx, 0) = (added_step_data_[step_idx].position_data.left_foot_pose.y + added_step_data_[step_idx].position_data.right_foot_pose.y)*0.5;
879  }
880  else
881  {
882  reference_zmp_x_(ref_zmp_idx, 0) = (added_step_data_[step_idx].position_data.left_foot_pose.x + added_step_data_[step_idx].position_data.right_foot_pose.x)*0.5;
883  reference_zmp_y_(ref_zmp_idx, 0) = (added_step_data_[step_idx].position_data.left_foot_pose.y + added_step_data_[step_idx].position_data.right_foot_pose.y)*0.5;
884  }
885  }
886  else if(added_step_data_[step_idx].time_data.walking_state == IN_WALKING_STARTING)
887  {
888  reference_zmp_x_(ref_zmp_idx, 0) = (added_step_data_[step_idx].position_data.left_foot_pose.x + added_step_data_[step_idx].position_data.right_foot_pose.x)*0.5;
889  reference_zmp_y_(ref_zmp_idx, 0) = (added_step_data_[step_idx].position_data.left_foot_pose.y + added_step_data_[step_idx].position_data.right_foot_pose.y)*0.5;
890  }
891  else if(added_step_data_[step_idx].time_data.walking_state == IN_WALKING_ENDING)
892  {
893  reference_zmp_x_(ref_zmp_idx, 0) = (added_step_data_[step_idx].position_data.left_foot_pose.x + added_step_data_[step_idx].position_data.right_foot_pose.x)*0.5;
894  reference_zmp_y_(ref_zmp_idx, 0) = (added_step_data_[step_idx].position_data.left_foot_pose.y + added_step_data_[step_idx].position_data.right_foot_pose.y)*0.5;
895  }
896  else
897  {
898  reference_zmp_x_(ref_zmp_idx, 0) = (added_step_data_[step_idx].position_data.left_foot_pose.x + added_step_data_[step_idx].position_data.right_foot_pose.x)*0.5;
899  reference_zmp_y_(ref_zmp_idx, 0) = (added_step_data_[step_idx].position_data.left_foot_pose.y + added_step_data_[step_idx].position_data.right_foot_pose.y)*0.5;
900  }
901  }
902  }
903 }
904 
905 void THORMANG3OnlineWalking::calcDesiredPose()
906 {
908  //u_x = -K*x_LIPM + x_feed_forward_term;
909  //x_LIPM = A*x_LIPM + b*u_x;
910  //
911  //u_y = -K*y_LIPM + y_feed_forward_term;
912  //y_LIPM = A*y_LIPM + b*u_y;
913 
914  //Calc LIPM with Integral
915  Eigen::MatrixXd x_feed_forward_term2; //f_Preview*m_ZMP_Reference_X;
916  Eigen::MatrixXd y_feed_forward_term2; //f_Preview*m_ZMP_Reference_Y;
917 
918  x_feed_forward_term2.resize(1,1);
919  x_feed_forward_term2.fill(0.0);
920  y_feed_forward_term2.resize(1,1);
921  y_feed_forward_term2.fill(0.0);
922 
923  for(int i = 0; i < preview_size_; i++)
924  {
925  if(current_start_idx_for_ref_zmp_ + i < preview_size_) {
926  x_feed_forward_term2(0,0) += f_(i)*reference_zmp_x_(current_start_idx_for_ref_zmp_ + i, 0);
927  y_feed_forward_term2(0,0) += f_(i)*reference_zmp_y_(current_start_idx_for_ref_zmp_ + i, 0);
928  }
929  else {
930  x_feed_forward_term2(0,0) += f_(i)*reference_zmp_x_(current_start_idx_for_ref_zmp_ + i - preview_size_, 0);
931  y_feed_forward_term2(0,0) += f_(i)*reference_zmp_y_(current_start_idx_for_ref_zmp_ + i - preview_size_, 0);
932  }
933  }
934 
935  sum_of_cx_ += c_(0,0)*x_lipm_(0,0) + c_(0,1)*x_lipm_(1,0) + c_(0,2)*x_lipm_(2,0);
936  sum_of_cy_ += c_(0,0)*y_lipm_(0,0) + c_(0,1)*y_lipm_(1,0) + c_(0,2)*y_lipm_(2,0);
937 
938  u_x(0,0) = -k_s_*(sum_of_cx_ - sum_of_zmp_x_) - (k_x_(0,0)*x_lipm_(0,0) + k_x_(0,1)*x_lipm_(1,0) + k_x_(0,2)*x_lipm_(2,0)) + x_feed_forward_term2(0,0);
939  u_y(0,0) = -k_s_*(sum_of_cy_ - sum_of_zmp_y_) - (k_x_(0,0)*y_lipm_(0,0) + k_x_(0,1)*y_lipm_(1,0) + k_x_(0,2)*y_lipm_(2,0)) + y_feed_forward_term2(0,0);
940  x_lipm_ = A_*x_lipm_ + b_*u_x;
941  y_lipm_ = A_*y_lipm_ + b_*u_y;
942 
943 
944  ref_zmp_x_at_this_time_ = reference_zmp_x_.coeff(current_start_idx_for_ref_zmp_, 0);
945  ref_zmp_y_at_this_time_ = reference_zmp_y_.coeff(current_start_idx_for_ref_zmp_, 0);
946 
947  sum_of_zmp_x_ += reference_zmp_x_.coeff(current_start_idx_for_ref_zmp_, 0);
948  sum_of_zmp_y_ += reference_zmp_y_.coeff(current_start_idx_for_ref_zmp_, 0);
949 
950  present_body_pose_.x = x_lipm_.coeff(0,0);
951  present_body_pose_.y = y_lipm_.coeff(0,0);
952 
953  reference_step_data_for_addition_.position_data.body_pose.x = x_lipm_(0,0);
954  reference_step_data_for_addition_.position_data.body_pose.y = y_lipm_(0,0);
955 
956  current_start_idx_for_ref_zmp_++;
957  if(current_start_idx_for_ref_zmp_ == (preview_size_))
958  current_start_idx_for_ref_zmp_ = 0;
959 }
960 
961 void THORMANG3OnlineWalking::process()
962 {
963  if(!ctrl_running)
964  {
965  return;
966  }
967  else
968  {
969  step_data_mutex_lock_.lock();
970 
971  calcStepIdxData();
972  calcRefZMP();
973  calcDesiredPose();
974 
975  double hip_roll_swap = 0;
976 
977  if((added_step_data_.size() != 0) && real_running)
978  {
979  double period_time, dsp_ratio, ssp_ratio, foot_move_period_time, ssp_time_start, ssp_time_end;
980  period_time = added_step_data_[0].time_data.abs_step_time - reference_time_;
981  dsp_ratio = added_step_data_[0].time_data.dsp_ratio;
982  ssp_ratio = 1 - dsp_ratio;
983  foot_move_period_time = ssp_ratio*period_time;
984 
985  ssp_time_start = dsp_ratio*period_time/2.0 + reference_time_;
986  ssp_time_end = (1 + ssp_ratio)*period_time / 2.0 + reference_time_;
987 
988  double start_time_delay_ratio_x = added_step_data_[0].time_data.start_time_delay_ratio_x;
989  double start_time_delay_ratio_y = added_step_data_[0].time_data.start_time_delay_ratio_y;
990  double start_time_delay_ratio_z = added_step_data_[0].time_data.start_time_delay_ratio_z;
991  double start_time_delay_ratio_roll = added_step_data_[0].time_data.start_time_delay_ratio_roll;
992  double start_time_delay_ratio_pitch = added_step_data_[0].time_data.start_time_delay_ratio_pitch;
993  double start_time_delay_ratio_yaw = added_step_data_[0].time_data.start_time_delay_ratio_yaw;
994  double finish_time_advance_ratio_x = added_step_data_[0].time_data.finish_time_advance_ratio_x;
995  double finish_time_advance_ratio_y = added_step_data_[0].time_data.finish_time_advance_ratio_y;
996  double finish_time_advance_ratio_z = added_step_data_[0].time_data.finish_time_advance_ratio_z;
997  double finish_time_advance_ratio_roll = added_step_data_[0].time_data.finish_time_advance_ratio_roll;
998  double finish_time_advance_ratio_pitch = added_step_data_[0].time_data.finish_time_advance_ratio_pitch;
999  double finish_time_advance_ratio_yaw = added_step_data_[0].time_data.finish_time_advance_ratio_yaw;
1000 
1001  double hip_roll_swap_dir = 1.0;
1002 
1003  if( (walking_time_ - reference_time_) < TIME_UNIT)
1004  {
1005  waist_yaw_tra_.changeTrajectory(reference_time_, previous_step_waist_yaw_angle_rad_, 0, 0,
1006  added_step_data_[0].time_data.abs_step_time, added_step_data_[0].position_data.waist_yaw_angle, 0, 0);
1007  body_z_tra_.changeTrajectory(reference_time_, previous_step_body_pose_.z, 0, 0,
1008  added_step_data_[0].time_data.abs_step_time, added_step_data_[0].position_data.body_pose.z, 0, 0);
1009  body_roll_tra_.changeTrajectory(reference_time_, previous_step_body_pose_.roll, 0, 0,
1010  added_step_data_[0].time_data.abs_step_time, added_step_data_[0].position_data.body_pose.roll, 0, 0);
1011  body_pitch_tra_.changeTrajectory(reference_time_, previous_step_body_pose_.pitch, 0, 0,
1012  added_step_data_[0].time_data.abs_step_time, added_step_data_[0].position_data.body_pose.pitch, 0, 0);
1013 
1014  double bc_move_amp = added_step_data_[0].position_data.body_pose.yaw - previous_step_body_pose_.yaw;
1015 
1016  if(bc_move_amp >= M_PI)
1017  bc_move_amp -= 2.0*M_PI;
1018  else if(bc_move_amp <= -M_PI)
1019  bc_move_amp += 2.0*M_PI;
1020 
1021  body_yaw_tra_.changeTrajectory(reference_time_, previous_step_body_pose_.yaw, 0, 0,
1022  added_step_data_[0].time_data.abs_step_time, bc_move_amp + previous_step_body_pose_.yaw, 0, 0);
1023 
1024  body_z_swap_tra_.changeTrajectory(reference_time_,
1025  0, 0, 0,
1026  0.5*(added_step_data_[0].time_data.abs_step_time + reference_time_),
1027  added_step_data_[0].position_data.body_z_swap, 0, 0);
1028 
1029  if(added_step_data_[0].position_data.moving_foot == RIGHT_FOOT_SWING)
1030  {
1031  foot_x_tra_.changeTrajectory(ssp_time_start + start_time_delay_ratio_x*foot_move_period_time,
1032  previous_step_right_foot_pose_.x, 0, 0,
1033  ssp_time_end - finish_time_advance_ratio_x*foot_move_period_time,
1034  added_step_data_[0].position_data.right_foot_pose.x, 0, 0);
1035  foot_y_tra_.changeTrajectory(ssp_time_start + start_time_delay_ratio_y*foot_move_period_time,
1036  previous_step_right_foot_pose_.y, 0, 0,
1037  ssp_time_end - finish_time_advance_ratio_y*foot_move_period_time,
1038  added_step_data_[0].position_data.right_foot_pose.y, 0, 0);
1039  foot_z_tra_.changeTrajectory(ssp_time_start + start_time_delay_ratio_z*foot_move_period_time,
1040  previous_step_right_foot_pose_.z, 0, 0,
1041  ssp_time_end - finish_time_advance_ratio_z*foot_move_period_time,
1042  added_step_data_[0].position_data.right_foot_pose.z, 0, 0);
1043  foot_roll_tra_.changeTrajectory(ssp_time_start + start_time_delay_ratio_roll*foot_move_period_time,
1044  previous_step_right_foot_pose_.roll, 0, 0,
1045  ssp_time_end - finish_time_advance_ratio_roll*foot_move_period_time,
1046  added_step_data_[0].position_data.right_foot_pose.roll, 0, 0);
1047  foot_pitch_tra_.changeTrajectory(ssp_time_start + start_time_delay_ratio_pitch*foot_move_period_time,
1048  previous_step_right_foot_pose_.pitch, 0, 0,
1049  ssp_time_end - finish_time_advance_ratio_pitch*foot_move_period_time,
1050  added_step_data_[0].position_data.right_foot_pose.pitch, 0, 0);
1051 
1052  double c_move_amp = added_step_data_[0].position_data.right_foot_pose.yaw - previous_step_right_foot_pose_.yaw;
1053  if(c_move_amp >= M_PI)
1054  c_move_amp -= 2.0*M_PI;
1055  else if(c_move_amp <= -M_PI)
1056  c_move_amp += 2.0*M_PI;
1057 
1058  foot_yaw_tra_.changeTrajectory(ssp_time_start + start_time_delay_ratio_yaw*foot_move_period_time,
1059  previous_step_right_foot_pose_.yaw, 0, 0,
1060  ssp_time_end - finish_time_advance_ratio_yaw*foot_move_period_time,
1061  c_move_amp + previous_step_right_foot_pose_.yaw, 0, 0);
1062 
1063  foot_z_swap_tra_.changeTrajectory(ssp_time_start, 0, 0, 0,
1064  0.5*(ssp_time_start + ssp_time_end), added_step_data_[0].position_data.foot_z_swap, 0, 0);
1065 
1066  hip_roll_swap_tra_.changeTrajectory(ssp_time_start, 0, 0, 0,
1067  0.5*(ssp_time_start + ssp_time_end), hip_roll_feedforward_angle_rad_, 0, 0);
1068  }
1069  else if(added_step_data_[0].position_data.moving_foot == LEFT_FOOT_SWING)
1070  {
1071  foot_x_tra_.changeTrajectory(ssp_time_start + start_time_delay_ratio_x*foot_move_period_time,
1072  previous_step_left_foot_pose_.x, 0, 0,
1073  ssp_time_end - finish_time_advance_ratio_x*foot_move_period_time,
1074  added_step_data_[0].position_data.left_foot_pose.x, 0, 0);
1075  foot_y_tra_.changeTrajectory(ssp_time_start + start_time_delay_ratio_y*foot_move_period_time,
1076  previous_step_left_foot_pose_.y, 0, 0,
1077  ssp_time_end - finish_time_advance_ratio_y*foot_move_period_time,
1078  added_step_data_[0].position_data.left_foot_pose.y, 0, 0);
1079  foot_z_tra_.changeTrajectory(ssp_time_start + start_time_delay_ratio_z*foot_move_period_time,
1080  previous_step_left_foot_pose_.z, 0, 0,
1081  ssp_time_end - finish_time_advance_ratio_z*foot_move_period_time,
1082  added_step_data_[0].position_data.left_foot_pose.z, 0, 0);
1083  foot_roll_tra_.changeTrajectory(ssp_time_start + start_time_delay_ratio_roll*foot_move_period_time,
1084  previous_step_left_foot_pose_.roll, 0, 0,
1085  ssp_time_end - finish_time_advance_ratio_roll*foot_move_period_time,
1086  added_step_data_[0].position_data.left_foot_pose.roll, 0, 0);
1087  foot_pitch_tra_.changeTrajectory(ssp_time_start + start_time_delay_ratio_pitch*foot_move_period_time,
1088  previous_step_left_foot_pose_.pitch, 0, 0,
1089  ssp_time_end - finish_time_advance_ratio_pitch*foot_move_period_time,
1090  added_step_data_[0].position_data.left_foot_pose.pitch, 0, 0);
1091 
1092  double c_move_amp = added_step_data_[0].position_data.left_foot_pose.yaw - previous_step_left_foot_pose_.yaw;
1093  if(c_move_amp >= M_PI)
1094  c_move_amp -= 2.0*M_PI;
1095  else if(c_move_amp <= -M_PI)
1096  c_move_amp += 2.0*M_PI;
1097 
1098  foot_yaw_tra_.changeTrajectory(ssp_time_start + start_time_delay_ratio_yaw*foot_move_period_time,
1099  previous_step_left_foot_pose_.yaw, 0, 0,
1100  ssp_time_end - finish_time_advance_ratio_yaw*foot_move_period_time,
1101  c_move_amp + previous_step_left_foot_pose_.yaw, 0, 0);
1102 
1103  foot_z_swap_tra_.changeTrajectory(ssp_time_start, 0, 0, 0,
1104  0.5*(ssp_time_start + ssp_time_end), added_step_data_[0].position_data.foot_z_swap, 0, 0);
1105 
1106  hip_roll_swap_tra_.changeTrajectory(ssp_time_start, 0, 0, 0,
1107  0.5*(ssp_time_start + ssp_time_end), hip_roll_feedforward_angle_rad_, 0, 0);
1108 
1109  hip_roll_swap_dir = 1.0;
1110  }
1111  else
1112  {
1113  foot_z_swap_tra_.changeTrajectory(ssp_time_start, 0, 0, 0,
1114  ssp_time_end, 0, 0, 0);
1115 
1116  hip_roll_swap_tra_.changeTrajectory(ssp_time_start, 0, 0, 0,
1117  ssp_time_end, 0, 0, 0);
1118 
1119  hip_roll_swap_dir = 0.0;
1120  }
1121  }
1122 
1123  double z_swap = body_z_swap_tra_.getPosition(walking_time_);
1124  double wp_move = waist_yaw_tra_.getPosition(walking_time_);
1125  double bz_move = body_z_tra_.getPosition(walking_time_);
1126  double ba_move = body_roll_tra_.getPosition(walking_time_);
1127  double bb_move = body_pitch_tra_.getPosition(walking_time_);
1128  double bc_move = body_yaw_tra_.getPosition(walking_time_);
1129 
1130  present_waist_yaw_angle_rad_ = wp_move;
1131  present_body_pose_.z = bz_move + z_swap;
1132  present_body_pose_.roll = ba_move;
1133  present_body_pose_.pitch = bb_move;
1134  present_body_pose_.yaw = bc_move;
1135 
1136  //Feet
1137  double x_move, y_move, z_move, a_move, b_move, c_move, z_vibe;
1138  if( walking_time_ <= ssp_time_start)
1139  {
1140  x_move = foot_x_tra_.getPosition(ssp_time_start);
1141  y_move = foot_y_tra_.getPosition(ssp_time_start);
1142  z_move = foot_z_tra_.getPosition(ssp_time_start);
1143  a_move = foot_roll_tra_.getPosition(ssp_time_start);
1144  b_move = foot_pitch_tra_.getPosition(ssp_time_start);
1145  c_move = foot_yaw_tra_.getPosition(ssp_time_start);
1146 
1147  z_vibe = foot_z_swap_tra_.getPosition(ssp_time_start);
1148  hip_roll_swap = hip_roll_swap_tra_.getPosition(ssp_time_start);
1149 
1150  if(added_step_data_[0].position_data.moving_foot == RIGHT_FOOT_SWING)
1151  balancing_index_ = BalancingPhase1;
1152  else if(added_step_data_[0].position_data.moving_foot == LEFT_FOOT_SWING)
1153  balancing_index_ = BalancingPhase5;
1154  else
1155  balancing_index_ = BalancingPhase0;
1156  }
1157  else if( walking_time_ <= ssp_time_end)
1158  {
1159  x_move = foot_x_tra_.getPosition(walking_time_);
1160  y_move = foot_y_tra_.getPosition(walking_time_);
1161  z_move = foot_z_tra_.getPosition(walking_time_);
1162  a_move = foot_roll_tra_.getPosition(walking_time_);
1163  b_move = foot_pitch_tra_.getPosition(walking_time_);
1164  c_move = foot_yaw_tra_.getPosition(walking_time_);
1165 
1166  if(added_step_data_[0].position_data.moving_foot != STANDING)
1167  {
1168  if((walking_time_ >= 0.5*(ssp_time_start + ssp_time_end))
1169  && (walking_time_ < (0.5*(ssp_time_start + ssp_time_end) + TIME_UNIT)))
1170  {
1171  body_z_swap_tra_.changeTrajectory(0.5*(added_step_data_[0].time_data.abs_step_time + reference_time_),
1172  added_step_data_[0].position_data.body_z_swap, 0, 0,
1173  added_step_data_[0].time_data.abs_step_time,
1174  0, 0, 0);
1175 
1176  foot_z_swap_tra_.changeTrajectory(0.5*(ssp_time_start + ssp_time_end),
1177  added_step_data_[0].position_data.foot_z_swap, 0, 0,
1178  ssp_time_end, 0, 0, 0);
1179 
1180  hip_roll_swap_tra_.changeTrajectory(0.5*(ssp_time_start + ssp_time_end),
1181  hip_roll_feedforward_angle_rad_, 0, 0,
1182  ssp_time_end, 0, 0, 0);
1183  }
1184  }
1185 
1186  z_vibe = foot_z_swap_tra_.getPosition(walking_time_);
1187  hip_roll_swap = hip_roll_swap_tra_.getPosition(walking_time_);
1188 
1189  if(added_step_data_[0].position_data.moving_foot == RIGHT_FOOT_SWING)
1190  {
1191  if(walking_time_ <= (ssp_time_end + ssp_time_start)*0.5)
1192  balancing_index_ = BalancingPhase2;
1193  else
1194  balancing_index_ = BalancingPhase3;
1195  }
1196  else if(added_step_data_[0].position_data.moving_foot == LEFT_FOOT_SWING)
1197  {
1198  if(walking_time_ <= (ssp_time_end + ssp_time_start)*0.5)
1199  balancing_index_ = BalancingPhase6;
1200  else
1201  balancing_index_ = BalancingPhase7;
1202  }
1203  else
1204  balancing_index_ = BalancingPhase0;
1205  }
1206  else
1207  {
1208  x_move = foot_x_tra_.getPosition(ssp_time_end);
1209  y_move = foot_y_tra_.getPosition(ssp_time_end);
1210  z_move = foot_z_tra_.getPosition(ssp_time_end);
1211  a_move = foot_roll_tra_.getPosition(ssp_time_end);
1212  b_move = foot_pitch_tra_.getPosition(ssp_time_end);
1213  c_move = foot_yaw_tra_.getPosition(ssp_time_end);
1214 
1215  z_vibe = foot_z_swap_tra_.getPosition(ssp_time_end);
1216  hip_roll_swap = hip_roll_swap_tra_.getPosition(ssp_time_end);
1217 
1218  if(added_step_data_[0].position_data.moving_foot == RIGHT_FOOT_SWING)
1219  balancing_index_ = BalancingPhase4;
1220  else if(added_step_data_[0].position_data.moving_foot == LEFT_FOOT_SWING)
1221  balancing_index_ = BalancingPhase8;
1222  else
1223  balancing_index_ = BalancingPhase0;
1224  }
1225 
1226 
1227  if(added_step_data_[0].position_data.moving_foot == RIGHT_FOOT_SWING)
1228  {
1229  present_right_foot_pose_.x = x_move;
1230  present_right_foot_pose_.y = y_move;
1231  present_right_foot_pose_.z = z_move + z_vibe;
1232  present_right_foot_pose_.roll = a_move;
1233  present_right_foot_pose_.pitch = b_move;
1234  present_right_foot_pose_.yaw = c_move;
1235 
1236  present_left_foot_pose_ = added_step_data_[0].position_data.left_foot_pose;
1237 
1238  hip_roll_swap_dir = -1.0;
1239  }
1240  else if(added_step_data_[0].position_data.moving_foot == LEFT_FOOT_SWING)
1241  {
1242  present_right_foot_pose_ = added_step_data_[0].position_data.right_foot_pose;
1243 
1244  present_left_foot_pose_.x = x_move;
1245  present_left_foot_pose_.y = y_move;
1246  present_left_foot_pose_.z = z_move + z_vibe;
1247  present_left_foot_pose_.roll = a_move;
1248  present_left_foot_pose_.pitch = b_move;
1249  present_left_foot_pose_.yaw = c_move;
1250 
1251  hip_roll_swap_dir = 1.0;
1252  }
1253  else
1254  {
1255  present_right_foot_pose_ = added_step_data_[0].position_data.right_foot_pose;
1256  present_left_foot_pose_ = added_step_data_[0].position_data.left_foot_pose;
1257 
1258  hip_roll_swap_dir = 0.0;
1259  }
1260 
1261  hip_roll_swap = hip_roll_swap_dir * hip_roll_swap;
1262 
1263  shouler_swing_gain_ = added_step_data_[0].position_data.shoulder_swing_gain;
1264  elbow_swing_gain_ = added_step_data_[0].position_data.elbow_swing_gain;
1265 
1266  walking_time_ += TIME_UNIT;
1267 
1268  if(walking_time_ > added_step_data_[added_step_data_.size() - 1].time_data.abs_step_time - 0.5*0.001)
1269  {
1270  real_running = false;
1271  calcStepIdxData();
1272  step_data_mutex_lock_.unlock();
1273  reInitialize();
1274  }
1275 
1276  if(balancing_index_ == BalancingPhase0 || balancing_index_ == BalancingPhase9)
1277  {
1278  left_fz_trajectory_start_time_ = walking_time_;
1279  left_fz_trajectory_end_time_ = walking_time_;
1280  left_fz_trajectory_target_ = left_dsp_fz_N_;
1281  left_fz_trajectory_shift_ = left_dsp_fz_N_;
1282  }
1283  else if(balancing_index_ == BalancingPhase1 )
1284  {
1285  left_fz_trajectory_end_time_ = ssp_time_start;
1286  left_fz_trajectory_target_ = left_ssp_fz_N_;
1287  }
1288  else if(balancing_index_ == BalancingPhase4 )
1289  {
1290  left_fz_trajectory_start_time_ = ssp_time_end;
1291  left_fz_trajectory_shift_ = left_ssp_fz_N_;
1292  if(added_step_data_.size() > 1)
1293  {
1294  if(added_step_data_[1].position_data.moving_foot == STANDING)
1295  {
1296  left_fz_trajectory_target_ = left_dsp_fz_N_;
1297  left_fz_trajectory_end_time_ = added_step_data_[0].time_data.abs_step_time;
1298  }
1299  else if(added_step_data_[1].position_data.moving_foot == LEFT_FOOT_SWING)
1300  {
1301  left_fz_trajectory_target_ = 0.0;
1302  left_fz_trajectory_end_time_ = (added_step_data_[1].time_data.abs_step_time - added_step_data_[0].time_data.abs_step_time)*0.5*added_step_data_[1].time_data.dsp_ratio + added_step_data_[0].time_data.abs_step_time;
1303  }
1304  else
1305  {
1306  left_fz_trajectory_target_ = left_ssp_fz_N_;
1307  left_fz_trajectory_end_time_ = (added_step_data_[1].time_data.abs_step_time - added_step_data_[0].time_data.abs_step_time)*0.5*added_step_data_[1].time_data.dsp_ratio + added_step_data_[0].time_data.abs_step_time;
1308  }
1309  }
1310  else {
1311  left_fz_trajectory_target_ = left_dsp_fz_N_;
1312  left_fz_trajectory_end_time_ = added_step_data_[0].time_data.abs_step_time;
1313  }
1314  }
1315  else if(balancing_index_ == BalancingPhase5 )
1316  {
1317  left_fz_trajectory_end_time_ = ssp_time_start;
1318  left_fz_trajectory_target_ = 0.0;
1319  }
1320  else if(balancing_index_ == BalancingPhase8)
1321  {
1322  left_fz_trajectory_start_time_ = ssp_time_end;
1323  left_fz_trajectory_shift_ = 0.0;
1324  if(added_step_data_.size() > 1)
1325  {
1326  if(added_step_data_[1].position_data.moving_foot == STANDING)
1327  {
1328  left_fz_trajectory_target_ = left_dsp_fz_N_;
1329  left_fz_trajectory_end_time_ = added_step_data_[0].time_data.abs_step_time;
1330  }
1331  else if(added_step_data_[1].position_data.moving_foot == LEFT_FOOT_SWING)
1332  {
1333  left_fz_trajectory_target_ = 0.0;
1334  left_fz_trajectory_end_time_ = (added_step_data_[1].time_data.abs_step_time - added_step_data_[0].time_data.abs_step_time)*0.5*added_step_data_[1].time_data.dsp_ratio + added_step_data_[0].time_data.abs_step_time;
1335  }
1336  else
1337  {
1338  left_fz_trajectory_target_ = left_ssp_fz_N_;
1339  left_fz_trajectory_end_time_ = (added_step_data_[1].time_data.abs_step_time - added_step_data_[0].time_data.abs_step_time)*0.5*added_step_data_[1].time_data.dsp_ratio + added_step_data_[0].time_data.abs_step_time;
1340  }
1341  }
1342  else
1343  {
1344  left_fz_trajectory_target_ = left_dsp_fz_N_;
1345  left_fz_trajectory_end_time_ = added_step_data_[0].time_data.abs_step_time;
1346  }
1347  }
1348  else
1349  {
1350  left_fz_trajectory_start_time_ = walking_time_;
1351  left_fz_trajectory_end_time_ = walking_time_;
1352  }
1353  }
1354 
1355  step_data_mutex_lock_.unlock();
1356 
1357  mat_g_to_cob_ = robotis_framework::getTransformationXYZRPY(present_body_pose_.x, present_body_pose_.y, present_body_pose_.z,
1358  present_body_pose_.roll, present_body_pose_.pitch, present_body_pose_.yaw);
1359 
1360  mat_g_to_rfoot_ = robotis_framework::getTransformationXYZRPY(present_right_foot_pose_.x, present_right_foot_pose_.y, present_right_foot_pose_.z,
1361  present_right_foot_pose_.roll, present_right_foot_pose_.pitch, present_right_foot_pose_.yaw);
1362 
1363  mat_g_to_lfoot_ = robotis_framework::getTransformationXYZRPY(present_left_foot_pose_.x, present_left_foot_pose_.y, present_left_foot_pose_.z,
1364  present_left_foot_pose_.roll, present_left_foot_pose_.pitch, present_left_foot_pose_.yaw);
1365 
1366  mat_cob_to_g_ = robotis_framework::getInverseTransformation(mat_g_to_cob_);
1367 
1368  mat_robot_to_cob_ = robotis_framework::getRotation4d(present_body_pose_.roll, present_body_pose_.pitch, 0);
1369  mat_cob_to_robot_ = robotis_framework::getInverseTransformation(mat_robot_to_cob_);
1370  mat_g_to_robot_ = mat_g_to_cob_ * mat_cob_to_robot_;
1371  mat_robot_to_g_ = robotis_framework::getInverseTransformation(mat_g_to_robot_);
1372 
1373  mat_robot_to_rfoot_ = mat_robot_to_g_*mat_g_to_rfoot_;
1374  mat_robot_to_lfoot_ = mat_robot_to_g_*mat_g_to_lfoot_;
1375 
1376 
1377  //Stabilizer Start
1378  //Balancing Algorithm
1379  double target_fz_N = 0;
1380 
1381  double right_leg_fx_N = current_right_fx_N_;
1382  double right_leg_fy_N = current_right_fy_N_;
1383  double right_leg_fz_N = current_right_fz_N_;
1384  double right_leg_Tx_Nm = current_right_tx_Nm_;
1385  double right_leg_Ty_Nm = current_right_ty_Nm_;
1386  double right_leg_Tz_Nm = current_right_tz_Nm_;
1387 
1388  double left_leg_fx_N = current_left_fx_N_;
1389  double left_leg_fy_N = current_left_fy_N_;
1390  double left_leg_fz_N = current_left_fz_N_;
1391  double left_leg_Tx_Nm = current_left_tx_Nm_;
1392  double left_leg_Ty_Nm = current_left_ty_Nm_;
1393  double left_leg_Tz_Nm = current_left_tz_Nm_;
1394 
1395  Eigen::MatrixXd mat_right_force, mat_right_torque;
1396  mat_right_force.resize(4,1); mat_right_force.fill(0);
1397  mat_right_torque.resize(4,1); mat_right_torque.fill(0);
1398  mat_right_force(0,0) = right_leg_fx_N;
1399  mat_right_force(1,0) = right_leg_fy_N;
1400  mat_right_force(2,0) = right_leg_fz_N;
1401  mat_right_torque(0,0) = right_leg_Tx_Nm;
1402  mat_right_torque(1,0) = right_leg_Ty_Nm;
1403  mat_right_torque(2,0) = right_leg_Tz_Nm;
1404 
1405  Eigen::MatrixXd mat_left_force, mat_left_torque;
1406  mat_left_force.resize(4,1); mat_left_force.fill(0);
1407  mat_left_torque.resize(4,1); mat_left_torque.fill(0);
1408  mat_left_force(0,0) = left_leg_fx_N;
1409  mat_left_force(1,0) = left_leg_fy_N;
1410  mat_left_force(2,0) = left_leg_fz_N;
1411  mat_left_torque(0,0) = left_leg_Tx_Nm;
1412  mat_left_torque(1,0) = left_leg_Ty_Nm;
1413  mat_left_torque(2,0) = left_leg_Tz_Nm;
1414 
1415  mat_right_force = mat_robot_to_rfoot_*mat_rfoot_to_rft_*mat_right_force;
1416  mat_right_torque = mat_robot_to_rfoot_*mat_rfoot_to_rft_*mat_right_torque;
1417 
1418  mat_left_force = mat_robot_to_lfoot_*mat_lfoot_to_lft_*mat_left_force;
1419  mat_left_torque = mat_robot_to_lfoot_*mat_lfoot_to_lft_*mat_left_torque;
1420 
1421  imu_data_mutex_lock_.lock();
1422  double gyro_roll_rad_per_sec = current_gyro_roll_rad_per_sec_;
1423  double gyro_pitch_rad_per_sec = current_gyro_pitch_rad_per_sec_;
1424 
1425  double iu_roll_rad = current_imu_roll_rad_;
1426  double iu_pitch_rad = current_imu_pitch_rad_;
1427  imu_data_mutex_lock_.unlock();
1428 
1429  balance_ctrl_.setCurrentGyroSensorOutput(gyro_roll_rad_per_sec, gyro_pitch_rad_per_sec);
1430  balance_ctrl_.setCurrentOrientationSensorOutput(iu_roll_rad, iu_pitch_rad);
1431  balance_ctrl_.setCurrentFootForceTorqueSensorOutput(mat_right_force.coeff(0,0), mat_right_force.coeff(1,0), mat_right_force.coeff(2,0),
1432  mat_right_torque.coeff(0,0), mat_right_torque.coeff(1,0), mat_right_torque.coeff(2,0),
1433  mat_left_force.coeff(0,0), mat_left_force.coeff(1,0), mat_left_force.coeff(2,0),
1434  mat_left_torque.coeff(0,0), mat_left_torque.coeff(1,0), mat_left_torque.coeff(2,0));
1435 
1436 
1437  double r_target_fx_N = 0;
1438  double l_target_fx_N = 0;
1439  double r_target_fy_N = 0;
1440  double l_target_fy_N = 0;
1441  double r_target_fz_N = right_dsp_fz_N_;
1442  double l_target_fz_N = left_dsp_fz_N_;
1443 
1444  Eigen::MatrixXd mat_g_to_acc, mat_robot_to_acc;
1445  mat_g_to_acc.resize(4, 1);
1446  mat_g_to_acc.fill(0);
1447  mat_g_to_acc.coeffRef(0,0) = x_lipm_.coeff(2,0);
1448  mat_g_to_acc.coeffRef(1,0) = y_lipm_.coeff(2,0);
1449  mat_robot_to_acc = mat_robot_to_g_ * mat_g_to_acc;
1450 
1451 
1452  switch(balancing_index_)
1453  {
1454  case BalancingPhase0:
1455  //fprintf(stderr, "DSP : START\n");
1456  r_target_fx_N = l_target_fx_N = -0.5*total_mass_of_robot_*mat_robot_to_acc.coeff(0,0);
1457  r_target_fy_N = l_target_fy_N = -0.5*total_mass_of_robot_*mat_robot_to_acc.coeff(1,0);
1458  r_target_fz_N = right_dsp_fz_N_;
1459  l_target_fz_N = left_dsp_fz_N_;
1460  target_fz_N = left_dsp_fz_N_ - right_dsp_fz_N_;
1461  break;
1462  case BalancingPhase1:
1463  //fprintf(stderr, "DSP : R--O->L\n");
1464  r_target_fx_N = l_target_fx_N = -0.5*total_mass_of_robot_*mat_robot_to_acc.coeff(0,0);
1465  r_target_fy_N = l_target_fy_N = -0.5*total_mass_of_robot_*mat_robot_to_acc.coeff(1,0);
1466  r_target_fz_N = right_dsp_fz_N_;
1467  l_target_fz_N = left_dsp_fz_N_;
1468  target_fz_N = left_dsp_fz_N_ - right_dsp_fz_N_;
1469  break;
1470  case BalancingPhase2:
1471  //fprintf(stderr, "SSP : L_BALANCING1\n");
1472  r_target_fx_N = 0;
1473  r_target_fy_N = 0;
1474  r_target_fz_N = 0;
1475 
1476  l_target_fx_N = -1.0*total_mass_of_robot_*mat_robot_to_acc.coeff(0,0);
1477  l_target_fy_N = -1.0*total_mass_of_robot_*mat_robot_to_acc.coeff(1,0);
1478  l_target_fz_N = left_ssp_fz_N_;
1479  target_fz_N = left_ssp_fz_N_;
1480  break;
1481  case BalancingPhase3:
1482  //fprintf(stderr, "SSP : L_BALANCING2\n");
1483  r_target_fx_N = 0;
1484  r_target_fy_N = 0;
1485  r_target_fz_N = 0;
1486 
1487  l_target_fx_N = -1.0*total_mass_of_robot_*mat_robot_to_acc.coeff(0,0);
1488  l_target_fy_N = -1.0*total_mass_of_robot_*mat_robot_to_acc.coeff(1,0);
1489  l_target_fz_N = left_ssp_fz_N_;
1490  target_fz_N = left_ssp_fz_N_;
1491  break;
1492  case BalancingPhase4:
1493  //fprintf(stderr, "DSP : R--O<-L\n");
1494  r_target_fx_N = l_target_fx_N = -0.5*total_mass_of_robot_*mat_robot_to_acc.coeff(0,0);
1495  r_target_fy_N = l_target_fy_N = -0.5*total_mass_of_robot_*mat_robot_to_acc.coeff(1,0);
1496  r_target_fz_N = right_dsp_fz_N_;
1497  l_target_fz_N = left_dsp_fz_N_;
1498  target_fz_N = left_dsp_fz_N_ - right_dsp_fz_N_;
1499  break;
1500  case BalancingPhase5:
1501  //fprintf(stderr, "DSP : R<-O--L\n");
1502  r_target_fx_N = l_target_fx_N = -0.5*total_mass_of_robot_*mat_robot_to_acc.coeff(0,0);
1503  r_target_fy_N = l_target_fy_N = -0.5*total_mass_of_robot_*mat_robot_to_acc.coeff(1,0);
1504  r_target_fz_N = right_dsp_fz_N_;
1505  l_target_fz_N = left_dsp_fz_N_;
1506  target_fz_N = left_dsp_fz_N_ - right_dsp_fz_N_;
1507  break;
1508  case BalancingPhase6:
1509  //fprintf(stderr, "SSP : R_BALANCING1\n");
1510  r_target_fx_N = -1.0*total_mass_of_robot_*mat_robot_to_acc.coeff(0,0);
1511  r_target_fy_N = -1.0*total_mass_of_robot_*mat_robot_to_acc.coeff(1,0);
1512  r_target_fz_N = right_ssp_fz_N_;
1513 
1514  l_target_fx_N = 0;
1515  l_target_fy_N = 0;
1516  l_target_fz_N = 0;
1517  target_fz_N = -right_ssp_fz_N_;
1518  break;
1519  case BalancingPhase7:
1520  //fprintf(stderr, "SSP : R_BALANCING2\n");
1521  r_target_fx_N = -1.0*total_mass_of_robot_*mat_robot_to_acc.coeff(0,0);
1522  r_target_fy_N = -1.0*total_mass_of_robot_*mat_robot_to_acc.coeff(1,0);
1523  r_target_fz_N = right_ssp_fz_N_;
1524 
1525  l_target_fx_N = 0;
1526  l_target_fy_N = 0;
1527  l_target_fz_N = 0;
1528  target_fz_N = -right_ssp_fz_N_;
1529  break;
1530  case BalancingPhase8:
1531  //fprintf(stderr, "DSP : R->O--L");
1532  r_target_fx_N = l_target_fx_N = -0.5*total_mass_of_robot_*mat_robot_to_acc.coeff(0,0);
1533  r_target_fy_N = l_target_fy_N = -0.5*total_mass_of_robot_*mat_robot_to_acc.coeff(1,0);
1534  r_target_fz_N = right_dsp_fz_N_;
1535  l_target_fz_N = left_dsp_fz_N_;
1536  target_fz_N = left_dsp_fz_N_ - right_dsp_fz_N_;
1537  break;
1538  case BalancingPhase9:
1539  //fprintf(stderr, "DSP : END");
1540  r_target_fx_N = l_target_fx_N = -0.5*total_mass_of_robot_*mat_robot_to_acc.coeff(0,0);
1541  r_target_fy_N = l_target_fy_N = -0.5*total_mass_of_robot_*mat_robot_to_acc.coeff(1,0);
1542  r_target_fz_N = right_dsp_fz_N_;
1543  l_target_fz_N = left_dsp_fz_N_;
1544  target_fz_N = left_dsp_fz_N_ - right_dsp_fz_N_;
1545  break;
1546  default:
1547  break;
1548  }
1549 
1550 
1551  bool IsDSP = false;
1552  if( (balancing_index_ == BalancingPhase0) ||
1553  (balancing_index_ == BalancingPhase1) ||
1554  (balancing_index_ == BalancingPhase4) ||
1555  (balancing_index_ == BalancingPhase5) ||
1556  (balancing_index_ == BalancingPhase8) ||
1557  (balancing_index_ == BalancingPhase9) )
1558  {
1559  IsDSP = true;
1560  }
1561  else
1562  IsDSP = false;
1563 
1564  if(IsDSP)
1565  {
1566  if( (balancing_index_ == BalancingPhase0) || (balancing_index_ == BalancingPhase9) )
1567  {
1568  r_target_fz_N = right_dsp_fz_N_;
1569  l_target_fz_N = left_dsp_fz_N_;
1570  }
1571  else
1572  {
1573  l_target_fz_N = wsigmoid(walking_time_ - TIME_UNIT, left_fz_trajectory_end_time_ - left_fz_trajectory_start_time_, left_fz_trajectory_start_time_, left_fz_trajectory_target_ - left_fz_trajectory_shift_, left_fz_trajectory_shift_, 1.0, 1.0);
1574  r_target_fz_N = left_ssp_fz_N_ - l_target_fz_N;
1575  }
1576  }
1577  else
1578  {
1579  if( (balancing_index_ == BalancingPhase2) || (balancing_index_ == BalancingPhase3) )
1580  {
1581  r_target_fz_N = 0;
1582  l_target_fz_N = left_ssp_fz_N_;
1583  }
1584  else
1585  {
1586  r_target_fz_N = right_ssp_fz_N_;
1587  l_target_fz_N = 0;
1588  }
1589  }
1590 
1591  setBalanceOffset();
1592 
1593  balance_ctrl_.setDesiredCOBGyro(0,0);
1594 // balance_ctrl_.setDesiredCOBOrientation(present_body_pose_.roll, present_body_pose_.pitch);
1595  balance_ctrl_.setDesiredCOBOrientation(present_body_pose_.roll + des_balance_offset_.coeff(0,0),
1596  present_body_pose_.pitch + des_balance_offset_.coeff(1,0));
1597 
1598  balance_ctrl_.setDesiredFootForceTorque(r_target_fx_N*1.0, r_target_fy_N*1.0, r_target_fz_N, 0, 0, 0,
1599  l_target_fx_N*1.0, l_target_fy_N*1.0, l_target_fz_N, 0, 0, 0);
1600  balance_ctrl_.setDesiredPose(mat_robot_to_cob_, mat_robot_to_rfoot_, mat_robot_to_lfoot_);
1601 
1602  balance_ctrl_.process(&balance_error_, &mat_robot_to_cob_modified_, &mat_robot_to_rf_modified_, &mat_robot_to_lf_modified_);
1603  mat_cob_to_robot_modified_ = robotis_framework::getInverseTransformation(mat_robot_to_cob_modified_);
1604  //Stabilizer End
1605 
1606  rhip_to_rfoot_pose_ = robotis_framework::getPose3DfromTransformMatrix((mat_rhip_to_cob_ * mat_cob_to_robot_modified_) * mat_robot_to_rf_modified_);
1607  lhip_to_lfoot_pose_ = robotis_framework::getPose3DfromTransformMatrix((mat_lhip_to_cob_ * mat_cob_to_robot_modified_) * mat_robot_to_lf_modified_);
1608 
1609  if((rhip_to_rfoot_pose_.yaw > 30.0*M_PI/180.0) || (rhip_to_rfoot_pose_.yaw < -30.0*M_PI/180.0) )
1610  {
1611  return;
1612  }
1613 
1614  if((lhip_to_lfoot_pose_.yaw < -30.0*M_PI/180.0) || (lhip_to_lfoot_pose_.yaw > 30.0*M_PI/180.0) )
1615  {
1616  return;
1617  }
1618 
1619  if(thormang3_kd_->calcInverseKinematicsForRightLeg(&r_leg_out_angle_rad_[0], rhip_to_rfoot_pose_.x, rhip_to_rfoot_pose_.y, rhip_to_rfoot_pose_.z, rhip_to_rfoot_pose_.roll, rhip_to_rfoot_pose_.pitch, rhip_to_rfoot_pose_.yaw) == false)
1620  {
1621  printf("IK not Solved EPR : %f %f %f %f %f %f\n", rhip_to_rfoot_pose_.x, rhip_to_rfoot_pose_.y, rhip_to_rfoot_pose_.z, rhip_to_rfoot_pose_.roll, rhip_to_rfoot_pose_.pitch, rhip_to_rfoot_pose_.yaw);
1622  return;
1623  }
1624 
1625  if(thormang3_kd_->calcInverseKinematicsForLeftLeg(&l_leg_out_angle_rad_[0], lhip_to_lfoot_pose_.x, lhip_to_lfoot_pose_.y, lhip_to_lfoot_pose_.z, lhip_to_lfoot_pose_.roll, lhip_to_lfoot_pose_.pitch, lhip_to_lfoot_pose_.yaw) == false)
1626  {
1627  printf("IK not Solved EPL : %f %f %f %f %f %f\n", lhip_to_lfoot_pose_.x, lhip_to_lfoot_pose_.y, lhip_to_lfoot_pose_.z, lhip_to_lfoot_pose_.roll, lhip_to_lfoot_pose_.pitch, lhip_to_lfoot_pose_.yaw);
1628  return;
1629  }
1630 
1631 
1632  r_shoulder_out_angle_rad_ = r_shoulder_dir_*(mat_robot_to_rfoot_.coeff(0, 3) - mat_robot_to_lfoot_.coeff(0, 3))*shouler_swing_gain_ + r_init_shoulder_angle_rad_;
1633  l_shoulder_out_angle_rad_ = l_shoulder_dir_*(mat_robot_to_lfoot_.coeff(0, 3) - mat_robot_to_rfoot_.coeff(0, 3))*shouler_swing_gain_ + l_init_shoulder_angle_rad_;
1634  r_elbow_out_angle_rad_ = r_elbow_dir_*(mat_robot_to_rfoot_.coeff(0, 3) - mat_robot_to_lfoot_.coeff(0, 3))*elbow_swing_gain_ + r_init_elbow_angle_rad_;
1635  l_elbow_out_angle_rad_ = l_elbow_dir_*(mat_robot_to_lfoot_.coeff(0, 3) - mat_robot_to_rfoot_.coeff(0, 3))*elbow_swing_gain_ + l_init_elbow_angle_rad_;
1636 
1637 
1638  if(added_step_data_.size() != 0)
1639  {
1640  if(added_step_data_[0].position_data.moving_foot == LEFT_FOOT_SWING)
1641  r_leg_out_angle_rad_[1] = r_leg_out_angle_rad_[1] + hip_roll_swap;
1642  else if(added_step_data_[0].position_data.moving_foot == RIGHT_FOOT_SWING)
1643  r_leg_out_angle_rad_[1] = r_leg_out_angle_rad_[1] - 0.35*hip_roll_swap;
1644  }
1645 
1646  if(added_step_data_.size() != 0)
1647  {
1648  if(added_step_data_[0].position_data.moving_foot == RIGHT_FOOT_SWING)
1649  l_leg_out_angle_rad_[1] = l_leg_out_angle_rad_[1] + hip_roll_swap;
1650  else if(added_step_data_[0].position_data.moving_foot == LEFT_FOOT_SWING)
1651  l_leg_out_angle_rad_[1] = l_leg_out_angle_rad_[1] - 0.35*hip_roll_swap;
1652  }
1653 
1654  for(int angle_idx = 0; angle_idx < 6; angle_idx++)
1655  {
1656  leg_angle_feed_back_[angle_idx+0].desired_ = r_leg_out_angle_rad_[angle_idx];
1657  leg_angle_feed_back_[angle_idx+6].desired_ = l_leg_out_angle_rad_[angle_idx];
1658  out_angle_rad_[angle_idx+0] = r_leg_out_angle_rad_[angle_idx] + leg_angle_feed_back_[angle_idx+0].getFeedBack(curr_angle_rad_[angle_idx]);
1659  out_angle_rad_[angle_idx+6] = l_leg_out_angle_rad_[angle_idx] + leg_angle_feed_back_[angle_idx+6].getFeedBack(curr_angle_rad_[angle_idx+6]);
1660 // out_angle_rad_[angle_idx+0] = r_leg_out_angle_rad_[angle_idx];
1661 // out_angle_rad_[angle_idx+6] = l_leg_out_angle_rad_[angle_idx];
1662  }
1663  }
1664 }
1665 
1666 
1667 double THORMANG3OnlineWalking::wsin(double time, double period, double period_shift, double mag, double mag_shift)
1668 {
1669  return mag * sin(2 * M_PI / period * time - period_shift) + mag_shift;
1670 }
1671 
1672 double THORMANG3OnlineWalking::wsigmoid(double time, double period, double time_shift, double mag, double mag_shift, double sigmoid_ratio, double distortion_ratio)
1673 {
1674  double value = mag_shift, Amplitude = 0.0, sigmoid_distor_gain = 1.0, t = 0.0;
1675  if ((sigmoid_ratio >= 1.0) && (sigmoid_ratio < 2.0))
1676  {
1677  if( time >= time_shift+period*(2-sigmoid_ratio)) {
1678  value = mag_shift + mag;
1679  }
1680  else
1681  {
1682  t = 2.0*M_PI*(time - time_shift)/(period*(2-sigmoid_ratio));
1683  sigmoid_distor_gain = distortion_ratio + (1-distortion_ratio)*(time-(time_shift+period*(1-sigmoid_ratio)))/(period*(2-sigmoid_ratio));
1684  Amplitude = mag/(2.0*M_PI);
1685  value = mag_shift + Amplitude*(t - sigmoid_distor_gain*sin(t));
1686  }
1687  }
1688  else if( (sigmoid_ratio >= 0.0) && (sigmoid_ratio < 1.0))
1689  {
1690  if( time <= time_shift+period*(1-sigmoid_ratio))
1691  value = mag_shift;
1692  else {
1693  t = 2.0*M_PI*(time - time_shift-period*(1-sigmoid_ratio))/(period*sigmoid_ratio);
1694  sigmoid_distor_gain = distortion_ratio + (1-distortion_ratio)*(time-(time_shift+period*(1-sigmoid_ratio)))/(period*sigmoid_ratio);
1695  Amplitude = mag/(2.0*M_PI);
1696  value = mag_shift + Amplitude*(t - sigmoid_distor_gain*sin(t));
1697  }
1698  }
1699  else if(( sigmoid_ratio >= 2.0) && ( sigmoid_ratio < 3.0))
1700  {
1701  double nsigmoid_ratio = sigmoid_ratio - 2.0;
1702  if(time <= time_shift + period*(1.0-nsigmoid_ratio)*0.5)
1703  value = mag_shift;
1704  else if(time >= time_shift + period*(1.0+nsigmoid_ratio)*0.5)
1705  value = mag + mag_shift;
1706  else {
1707  t = 2.0*M_PI*(time - (time_shift+period*(1.0-nsigmoid_ratio)*0.5))/(period*nsigmoid_ratio);
1708  sigmoid_distor_gain = distortion_ratio + (1.0-distortion_ratio)*(time-(time_shift+period*(1.0-nsigmoid_ratio)*0.5))/(period*nsigmoid_ratio);
1709  Amplitude = mag/(2.0*M_PI);
1710  value = mag_shift + Amplitude*(t - sigmoid_distor_gain*sin(t));
1711  }
1712  }
1713  else
1714  value = mag_shift;
1715 
1716  return value;
1717 }
1718 
1719 void THORMANG3OnlineWalking::parseBalanceOffsetData(const std::string &path)
1720 {
1721  YAML::Node doc;
1722  try
1723  {
1724  // load yaml
1725  doc = YAML::LoadFile(path.c_str());
1726  }
1727  catch (const std::exception& e)
1728  {
1729  ROS_ERROR("Fail to load yaml file.");
1730  return;
1731  }
1732 
1733  r_leg_to_body_roll_gain_ = doc["r_leg_to_body_roll_gain"].as<double>();
1734  l_leg_to_body_roll_gain_ = doc["l_leg_to_body_roll_gain"].as<double>();
1735 
1736  r_leg_to_body_pitch_gain_ = doc["r_leg_to_body_pitch_gain"].as<double>();
1737  l_leg_to_body_pitch_gain_ = doc["l_leg_to_body_pitch_gain"].as<double>();
1738 
1739  ROS_INFO("r_leg_to_body_roll_gain_ : %f", r_leg_to_body_roll_gain_);
1740  ROS_INFO("l_leg_to_body_roll_gain_ : %f", l_leg_to_body_roll_gain_);
1741  ROS_INFO("r_leg_to_body_pitch_gain_ : %f", r_leg_to_body_pitch_gain_);
1742  ROS_INFO("l_leg_to_body_pitch_gain_ : %f", l_leg_to_body_pitch_gain_);
1743 }
1744 
1745 void THORMANG3OnlineWalking::initBalanceOffset()
1746 {
1747  if (init_balance_offset_ == false)
1748  {
1749  if((added_step_data_.size() != 0) && real_running)
1750  {
1751  double period_time = added_step_data_[0].time_data.abs_step_time - reference_time_;
1752  double dsp_ratio = added_step_data_[0].time_data.dsp_ratio;
1753 
1754 // ROS_INFO("period_time = %f", period_time);
1755 // ROS_INFO("dsp_ratio = %f", dsp_ratio);
1756 
1757  // feedforward trajectory
1758  std::vector<double_t> zero_vector;
1759  zero_vector.resize(1,0.0);
1760 
1761  std::vector<double_t> via_pos;
1762  via_pos.resize(3, 0.0);
1763  via_pos[0] = 1.0 * DEGREE2RADIAN;
1764 
1765  double init_time = 0.0;
1766  double fin_time = period_time;
1767  double via_time = 0.5 * (init_time + fin_time);
1768 
1769  feed_forward_tra_ =
1770  new robotis_framework::MinimumJerkViaPoint(init_time, fin_time, via_time, dsp_ratio,
1771  zero_vector, zero_vector, zero_vector,
1772  zero_vector, zero_vector, zero_vector,
1773  via_pos, zero_vector, zero_vector);
1774 
1775  mov_time_ = fin_time;
1776  mov_size_ = (int) (mov_time_ / TIME_UNIT) + 1;
1777 
1778  init_balance_offset_ = true;
1779  }
1780  }
1781 }
1782 
1783 void THORMANG3OnlineWalking::setBalanceOffset()
1784 {
1785  initBalanceOffset();
1786 
1787  bool is_DSP = false;
1788  bool is_l_balancing, is_r_balancing;
1789  if( (balancing_index_ == BalancingPhase0) ||
1790  (balancing_index_ == BalancingPhase1) ||
1791  (balancing_index_ == BalancingPhase4) ||
1792  (balancing_index_ == BalancingPhase5) ||
1793  (balancing_index_ == BalancingPhase8) ||
1794  (balancing_index_ == BalancingPhase9) )
1795  {
1796  is_DSP = true;
1797  is_l_balancing = false;
1798  is_r_balancing = false;
1799  }
1800  else
1801  is_DSP = false;
1802 
1803  if (balancing_index_ == 2 || balancing_index_ == 3)
1804  {
1805  is_l_balancing = true;
1806  is_r_balancing = false;
1807  }
1808 
1809  if (balancing_index_ == 6 || balancing_index_ == 7)
1810  {
1811  is_l_balancing = false;
1812  is_r_balancing = true;
1813  }
1814 
1815  des_balance_offset_ = Eigen::MatrixXd::Zero(2,1);
1816 
1817  if (init_balance_offset_)
1818  {
1819  double cur_time = (double) mov_step_ * TIME_UNIT;
1820 // ROS_INFO("cur_time : %f / %f", cur_time , mov_time_);
1821 
1822  if (is_DSP == false)
1823  {
1824 // ROS_INFO("SSP");
1825  std::vector<double_t> value = feed_forward_tra_->getPosition(cur_time);
1826 
1827  if (is_l_balancing)
1828  {
1829  des_balance_offset_.coeffRef(0,0) = r_leg_to_body_roll_gain_ * value[0];
1830  des_balance_offset_.coeffRef(1,0) = r_leg_to_body_pitch_gain_ * value[0];
1831 // ROS_INFO("L_BALANCING");
1832  }
1833 
1834  if (is_r_balancing)
1835  {
1836  des_balance_offset_.coeffRef(0,0) = l_leg_to_body_roll_gain_ * value[0];
1837  des_balance_offset_.coeffRef(1,0) = l_leg_to_body_pitch_gain_ * value[0];
1838 // ROS_INFO("R_BALANCING");
1839  }
1840  }
1841 
1842  if (mov_step_ == mov_size_-1)
1843  {
1844  mov_step_ = 0;
1845  init_balance_offset_ = false;
1846  }
1847  else
1848  mov_step_++;
1849 
1850 // ROS_INFO("des_balance_offset_ roll: %f, pitch: %f", des_balance_offset_.coeff(0,0), des_balance_offset_.coeff(1,0));
1851  }
1852 }
thormang3::THORMANG3OnlineWalking::present_left_foot_pose_
robotis_framework::Pose3D present_left_foot_pose_
Definition: thormang3_online_walking.h:160
robotis_framework::getRotationZ
Eigen::Matrix3d getRotationZ(double angle)
thormang3::THORMANG3OnlineWalking::rhip_to_rfoot_pose_
robotis_framework::Pose3D rhip_to_rfoot_pose_
Definition: thormang3_online_walking.h:162
BalancingPhase5
static const int BalancingPhase5
Definition: thormang3_online_walking.cpp:51
thormang3::THORMANG3OnlineWalking::setInitialLeftShoulderAngle
void setInitialLeftShoulderAngle(double shoulder_angle_rad)
Definition: thormang3_online_walking.cpp:265
STANDING
static const int STANDING
Definition: thormang3_online_walking.cpp:44
thormang3_online_walking.h
thormang3::THORMANG3OnlineWalking::initial_left_foot_pose_
robotis_framework::Pose3D initial_left_foot_pose_
Definition: thormang3_online_walking.h:159
ID_R_LEG_START
#define ID_R_LEG_START
thormang3::THORMANG3OnlineWalking::present_body_pose_
robotis_framework::Pose3D present_body_pose_
Definition: thormang3_online_walking.h:160
thormang3::BalanceControlUsingPDController::setDesiredCOBOrientation
void setDesiredCOBOrientation(double cob_orientation_roll, double cob_orientation_pitch)
ID_L_LEG_START
#define ID_L_LEG_START
thormang3::THORMANG3OnlineWalking::getReferenceStepDatafotAddition
void getReferenceStepDatafotAddition(robotis_framework::StepData *ref_step_data_for_addition)
Definition: thormang3_online_walking.cpp:664
robotis_framework::getTranslation4D
Eigen::Matrix4d getTranslation4D(double position_x, double position_y, double position_z)
IN_WALKING_STARTING
static const int IN_WALKING_STARTING
Definition: thormang3_online_walking.cpp:38
thormang3::THORMANG3OnlineWalking::foot_x_tra_
robotis_framework::FifthOrderPolynomialTrajectory foot_x_tra_
Definition: thormang3_online_walking.h:163
robotis_framework::StepTimeData::start_time_delay_ratio_y
double start_time_delay_ratio_y
robotis_framework::Pose3D::pitch
double pitch
robotis_framework::StepTimeData::abs_step_time
double abs_step_time
RIGHT_FOOT_SWING
static const int RIGHT_FOOT_SWING
Definition: thormang3_online_walking.cpp:43
thormang3::THORMANG3OnlineWalking::initial_body_pose_
robotis_framework::Pose3D initial_body_pose_
Definition: thormang3_online_walking.h:159
thormang3::THORMANG3OnlineWalking::foot_z_tra_
robotis_framework::FifthOrderPolynomialTrajectory foot_z_tra_
Definition: thormang3_online_walking.h:163
MStoS
static const double MStoS
Definition: thormang3_online_walking.cpp:32
robotis_framework::getRotationX
Eigen::Matrix3d getRotationX(double angle)
thormang3::THORMANG3OnlineWalking::setInitialRightElbowAngle
void setInitialRightElbowAngle(double elbow_angle_rad)
Definition: thormang3_online_walking.cpp:270
thormang3::THORMANG3OnlineWalking::body_z_tra_
robotis_framework::FifthOrderPolynomialTrajectory body_z_tra_
Definition: thormang3_online_walking.h:166
thormang3::BalanceControlUsingPDController::setDesiredPose
void setDesiredPose(const Eigen::MatrixXd &robot_to_cob, const Eigen::MatrixXd &robot_to_right_foot, const Eigen::MatrixXd &robot_to_left_foot)
StepDataStatus2
static const int StepDataStatus2
Definition: thormang3_online_walking.cpp:58
robotis_framework::StepPositionData::foot_z_swap
double foot_z_swap
BalancingPhase2
static const int BalancingPhase2
Definition: thormang3_online_walking.cpp:48
thormang3::BalanceControlUsingPDController::initialize
void initialize(const int control_cycle_msec)
robotis_framework::Pose3D::y
double y
BalancingPhase6
static const int BalancingPhase6
Definition: thormang3_online_walking.cpp:52
robotis_framework::Pose3D::x
double x
StepDataStatus3
static const int StepDataStatus3
Definition: thormang3_online_walking.cpp:59
robotis_framework::getInverseTransformation
Eigen::Matrix4d getInverseTransformation(const Eigen::MatrixXd &transform)
thormang3::THORMANG3OnlineWalking::parseBalanceOffsetData
void parseBalanceOffsetData(const std::string &path)
Definition: thormang3_online_walking.cpp:1719
IN_WALKING
static const int IN_WALKING
Definition: thormang3_online_walking.cpp:39
StepDataStatus1
static const int StepDataStatus1
Definition: thormang3_online_walking.cpp:57
thormang3::THORMANG3OnlineWalking::lhip_to_lfoot_pose_
robotis_framework::Pose3D lhip_to_lfoot_pose_
Definition: thormang3_online_walking.h:162
GRAVITY_ACCELERATION
#define GRAVITY_ACCELERATION
robotis_framework::Pose3D::roll
double roll
ID_R_ARM_START
#define ID_R_ARM_START
BalancingPhase4
static const int BalancingPhase4
Definition: thormang3_online_walking.cpp:50
thormang3::THORMANG3OnlineWalking::reference_step_data_for_addition_
robotis_framework::StepData reference_step_data_for_addition_
Definition: thormang3_online_walking.h:158
thormang3::THORMANG3OnlineWalking::balance_ctrl_
thormang3::BalanceControlUsingPDController balance_ctrl_
Definition: thormang3_online_walking.h:103
robotis_framework::StepData::position_data
StepPositionData position_data
robotis_framework::StepTimeData::finish_time_advance_ratio_pitch
double finish_time_advance_ratio_pitch
ID_L_ARM_START
#define ID_L_ARM_START
TIME_UNIT
static const double TIME_UNIT
Definition: thormang3_online_walking.cpp:36
robotis_framework::StepTimeData::start_time_delay_ratio_x
double start_time_delay_ratio_x
thormang3::THORMANG3OnlineWalking::feed_forward_tra_
robotis_framework::MinimumJerkViaPoint * feed_forward_tra_
Definition: thormang3_online_walking.h:128
robotis_framework::getTransformationXYZRPY
Eigen::Matrix4d getTransformationXYZRPY(double position_x, double position_y, double position_z, double roll, double pitch, double yaw)
thormang3::THORMANG3OnlineWalking::present_right_foot_pose_
robotis_framework::Pose3D present_right_foot_pose_
Definition: thormang3_online_walking.h:160
thormang3::THORMANG3OnlineWalking::setInitialPose
bool setInitialPose(double r_foot_x, double r_foot_y, double r_foot_z, double r_foot_roll, double r_foot_pitch, double r_foot_yaw, double l_foot_x, double l_foot_y, double l_foot_z, double l_foot_roll, double l_foot_pitch, double l_foot_yaw, double center_of_body_x, double center_of_body_y, double center_of_body_z, double center_of_body_roll, double center_of_body_pitch, double center_of_body_yaw)
Definition: thormang3_online_walking.cpp:218
thormang3::THORMANG3OnlineWalking::foot_z_swap_tra_
robotis_framework::FifthOrderPolynomialTrajectory foot_z_swap_tra_
Definition: thormang3_online_walking.h:165
thormang3::THORMANG3OnlineWalking::previous_step_body_pose_
robotis_framework::Pose3D previous_step_body_pose_
Definition: thormang3_online_walking.h:161
robotis_framework::StepTimeData::start_time_delay_ratio_z
double start_time_delay_ratio_z
thormang3::THORMANG3OnlineWalking::waist_yaw_tra_
robotis_framework::FifthOrderPolynomialTrajectory waist_yaw_tra_
Definition: thormang3_online_walking.h:167
thormang3::THORMANG3OnlineWalking::setInitialRightShoulderAngle
void setInitialRightShoulderAngle(double shoulder_angle_rad)
Definition: thormang3_online_walking.cpp:260
robotis_framework::StepPositionData::waist_pitch_angle
double waist_pitch_angle
thormang3::THORMANG3OnlineWalking::previous_step_left_foot_pose_
robotis_framework::Pose3D previous_step_left_foot_pose_
Definition: thormang3_online_walking.h:161
thormang3::THORMANG3OnlineWalking::added_step_data_
std::vector< robotis_framework::StepData > added_step_data_
Definition: thormang3_online_walking.h:154
thormang3::THORMANG3OnlineWalking::foot_roll_tra_
robotis_framework::FifthOrderPolynomialTrajectory foot_roll_tra_
Definition: thormang3_online_walking.h:164
MMtoM
static const double MMtoM
Definition: thormang3_online_walking.cpp:31
robotis_framework::FifthOrderPolynomialTrajectory::getPosition
double getPosition(double time)
thormang3::THORMANG3OnlineWalking::setCurrentIMUSensorOutput
void setCurrentIMUSensorOutput(double gyro_x, double gyro_y, double quat_x, double quat_y, double quat_z, double quat_w)
Definition: thormang3_online_walking.cpp:280
BalancingPhase8
static const int BalancingPhase8
Definition: thormang3_online_walking.cpp:54
thormang3::BalanceControlUsingPDController::setCurrentGyroSensorOutput
void setCurrentGyroSensorOutput(double gyro_roll, double gyro_pitch)
thormang3::THORMANG3OnlineWalking::body_pitch_tra_
robotis_framework::FifthOrderPolynomialTrajectory body_pitch_tra_
Definition: thormang3_online_walking.h:166
thormang3::THORMANG3OnlineWalking::setInitalWaistYawAngle
void setInitalWaistYawAngle(double waist_yaw_angle_rad)
Definition: thormang3_online_walking.cpp:255
thormang3::THORMANG3OnlineWalking::wsin
double wsin(double time, double period, double period_shift, double mag, double mag_shift)
Definition: thormang3_online_walking.cpp:1667
ROS_INFO
#define ROS_INFO(...)
thormang3::KinematicsDynamics::thormang3_link_data_
LinkData * thormang3_link_data_[ALL_JOINT_ID+1]
robotis_framework::StepPositionData::waist_yaw_angle
double waist_yaw_angle
thormang3::BalancePDController::getFeedBack
double getFeedBack(double present_sensor_output)
DEGREE2RADIAN
#define DEGREE2RADIAN
robotis_framework::Pose3D::z
double z
robotis_framework::StepPositionData::body_pose
Pose3D body_pose
robotis_framework::StepPositionData::moving_foot
int moving_foot
IN_WALKING_ENDING
static const int IN_WALKING_ENDING
Definition: thormang3_online_walking.cpp:40
BalancingPhase0
static const int BalancingPhase0
Definition: thormang3_online_walking.cpp:46
robotis_framework::StepTimeData::finish_time_advance_ratio_z
double finish_time_advance_ratio_z
robotis_framework::StepTimeData::finish_time_advance_ratio_x
double finish_time_advance_ratio_x
thormang3::THORMANG3OnlineWalking::body_yaw_tra_
robotis_framework::FifthOrderPolynomialTrajectory body_yaw_tra_
Definition: thormang3_online_walking.h:166
robotis_framework::StepTimeData::walking_state
int walking_state
robotis_framework::StepTimeData::finish_time_advance_ratio_yaw
double finish_time_advance_ratio_yaw
robotis_framework::StepTimeData::dsp_ratio
double dsp_ratio
thormang3::LinkData::joint_axis_
Eigen::MatrixXd joint_axis_
robotis_framework::Pose3D::yaw
double yaw
sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
robotis_framework::StepTimeData::finish_time_advance_ratio_y
double finish_time_advance_ratio_y
ros::package::getPath
ROSLIB_DECL std::string getPath(const std::string &package_name)
thormang3::THORMANG3OnlineWalking::addStepData
bool addStepData(robotis_framework::StepData step_data)
Definition: thormang3_online_walking.cpp:628
robotis_framework::getPose3DfromTransformMatrix
Pose3D getPose3DfromTransformMatrix(const Eigen::Matrix4d &transform)
robotis_framework::StepTimeData::finish_time_advance_ratio_roll
double finish_time_advance_ratio_roll
thormang3::BalanceControlUsingPDController::process
void process(int *balance_error, Eigen::MatrixXd *robot_to_cob_modified, Eigen::MatrixXd *robot_to_right_foot_modified, Eigen::MatrixXd *robot_to_left_foot_modified)
robotis_framework::MinimumJerkViaPoint::getPosition
std::vector< double_t > getPosition(double time)
robotis_framework::StepTimeData::start_time_delay_ratio_pitch
double start_time_delay_ratio_pitch
thormang3::THORMANG3OnlineWalking::hip_roll_swap_tra_
robotis_framework::FifthOrderPolynomialTrajectory hip_roll_swap_tra_
Definition: thormang3_online_walking.h:169
robotis_framework::StepPositionData::body_z_swap
double body_z_swap
thormang3::KinematicsDynamics::calcInverseKinematicsForRightLeg
bool calcInverseKinematicsForRightLeg(double *out, double x, double y, double z, double roll, double pitch, double yaw)
thormang3::BalanceControlUsingPDController::setDesiredCOBGyro
void setDesiredCOBGyro(double gyro_roll, double gyro_pitch)
robotis_framework::MinimumJerkViaPoint
thormang3::THORMANG3OnlineWalking::previous_step_right_foot_pose_
robotis_framework::Pose3D previous_step_right_foot_pose_
Definition: thormang3_online_walking.h:161
thormang3::THORMANG3OnlineWalking::body_roll_tra_
robotis_framework::FifthOrderPolynomialTrajectory body_roll_tra_
Definition: thormang3_online_walking.h:166
robotis_framework::StepTimeData::start_time_delay_ratio_roll
double start_time_delay_ratio_roll
robotis_framework::powDI
double powDI(double a, int b)
thormang3::THORMANG3OnlineWalking::foot_pitch_tra_
robotis_framework::FifthOrderPolynomialTrajectory foot_pitch_tra_
Definition: thormang3_online_walking.h:164
LEFT_FOOT_SWING
static const int LEFT_FOOT_SWING
Definition: thormang3_online_walking.cpp:42
robotis_framework::StepPositionData::left_foot_pose
Pose3D left_foot_pose
robotis_framework::StepPositionData::right_foot_pose
Pose3D right_foot_pose
thormang3::KinematicsDynamics::calcTotalMass
double calcTotalMass(int joint_id)
thormang3::THORMANG3OnlineWalking::initial_right_foot_pose_
robotis_framework::Pose3D initial_right_foot_pose_
Definition: thormang3_online_walking.h:159
thormang3::THORMANG3OnlineWalking::leg_angle_feed_back_
thormang3::BalancePDController leg_angle_feed_back_[12]
Definition: thormang3_online_walking.h:99
atan2
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
thormang3::KinematicsDynamics::calcInverseKinematicsForLeftLeg
bool calcInverseKinematicsForLeftLeg(double *out, double x, double y, double z, double roll, double pitch, double yaw)
robotis_framework::getRotation4d
Eigen::Matrix4d getRotation4d(double roll, double pitch, double yaw)
thormang3::LinkData::relative_position_
Eigen::MatrixXd relative_position_
robotis_framework::StepData
BalancingPhase3
static const int BalancingPhase3
Definition: thormang3_online_walking.cpp:49
thormang3::THORMANG3OnlineWalking::wsigmoid
double wsigmoid(double time, double period, double time_shift, double mag, double mag_shift, double sigmoid_ratio, double distortion_ratio)
Definition: thormang3_online_walking.cpp:1672
cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
thormang3::THORMANG3OnlineWalking::setInitialLeftElbowAngle
void setInitialLeftElbowAngle(double elbow_angle_rad)
Definition: thormang3_online_walking.cpp:275
thormang3::BalanceControlUsingPDController::setCurrentFootForceTorqueSensorOutput
void setCurrentFootForceTorqueSensorOutput(double r_force_x_N, double r_force_y_N, double r_force_z_N, double r_torque_roll_Nm, double r_torque_pitch_Nm, double r_torque_yaw_Nm, double l_force_x_N, double l_force_y_N, double l_force_z_N, double l_torque_roll_Nm, double l_torque_pitch_Nm, double l_torque_yaw_Nm)
robotis_framework::FifthOrderPolynomialTrajectory::changeTrajectory
bool changeTrajectory(double final_pos, double final_vel, double final_acc)
thormang3::BalanceControlUsingPDController::setDesiredFootForceTorque
void setDesiredFootForceTorque(double r_force_x_N, double r_force_y_N, double r_force_z_N, double r_torque_roll_Nm, double r_torque_pitch_Nm, double r_torque_yaw_Nm, double l_force_x_N, double l_force_y_N, double l_force_z_N, double l_torque_roll_Nm, double l_torque_pitch_Nm, double l_torque_yaw_Nm)
BalancingPhase1
static const int BalancingPhase1
Definition: thormang3_online_walking.cpp:47
thormang3::THORMANG3OnlineWalking::foot_yaw_tra_
robotis_framework::FifthOrderPolynomialTrajectory foot_yaw_tra_
Definition: thormang3_online_walking.h:164
thormang3::BalanceControlUsingPDController::setCurrentOrientationSensorOutput
void setCurrentOrientationSensorOutput(double cob_orientation_roll, double cob_orientation_pitch)
ROS_ERROR
#define ROS_ERROR(...)
robotis_framework::StepPositionData::shoulder_swing_gain
double shoulder_swing_gain
BalancingPhase7
static const int BalancingPhase7
Definition: thormang3_online_walking.cpp:53
sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
thormang3::THORMANG3OnlineWalking::body_z_swap_tra_
robotis_framework::FifthOrderPolynomialTrajectory body_z_swap_tra_
Definition: thormang3_online_walking.h:168
StepDataStatus4
static const int StepDataStatus4
Definition: thormang3_online_walking.cpp:60
NO_STEP_IDX
static const int NO_STEP_IDX
Definition: thormang3_online_walking.cpp:34
robotis_framework::StepTimeData::start_time_delay_ratio_yaw
double start_time_delay_ratio_yaw
robotis_framework::StepData::time_data
StepTimeData time_data
thormang3::THORMANG3OnlineWalking::foot_y_tra_
robotis_framework::FifthOrderPolynomialTrajectory foot_y_tra_
Definition: thormang3_online_walking.h:163
BalancingPhase9
static const int BalancingPhase9
Definition: thormang3_online_walking.cpp:55
robotis_framework::StepPositionData::elbow_swing_gain
double elbow_swing_gain

thormang3_walking_module
Author(s): Zerom , SCH , Kayman , Jay Song
autogenerated on Mon Jun 10 2019 15:37:56