thormang3_balance_control.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 /*
18  * thormang3_wrc_balance_control.h
19  *
20  * Created on: 2016. 12. 14
21  * Author: Jay Song
22  */
23 
24 #include <iostream>
25 #include "thormang3_balance_control/thormang3_balance_control.h"
26 
27 using namespace thormang3;
28 
29 DampingController::DampingController()
30 {
31  desired_ = 0.0;
32 
33  gain_ = 0.0;
34  time_constant_sec_ = 1.0;
35  output_ = 0.0;
36  control_cycle_sec_ = 0.008;
37 
38  previous_result_ = 0.0;
39 }
40 
41 DampingController::DampingController(double time_unit_sec)
42 {
43  desired_ = 0.0;
44 
45  gain_ = 0.0;
46  time_constant_sec_ = 1.0;
47  output_ = 0.0;
48  control_cycle_sec_ = time_unit_sec;
49 
50  previous_result_ = 0.0;
51 }
52 
53 DampingController::~DampingController()
54 { }
55 
56 double DampingController::getDampingControllerOutput(double present_sensor_output)
57 {
58  double cut_off_freq = 1.0/time_constant_sec_;
59  double alpha = 1.0;
60  alpha = (2.0*M_PI*cut_off_freq*control_cycle_sec_)/(1.0+2.0*M_PI*cut_off_freq*control_cycle_sec_);
61 
62  previous_result_ = alpha*(desired_ - present_sensor_output) + (1.0 - alpha)*previous_result_;
63  output_ = gain_*previous_result_;
64 
65  return output_;
66 }
67 
68 
69 BalanceLowPassFilter::BalanceLowPassFilter()
70 {
71  cut_off_freq_ = 1.0;
72  control_cycle_sec_ = 0.008;
73  prev_output_ = 0;
74 
75  alpha_ = (2.0*M_PI*cut_off_freq_*control_cycle_sec_)/(1.0+2.0*M_PI*cut_off_freq_*control_cycle_sec_);
76 }
77 
78 BalanceLowPassFilter::BalanceLowPassFilter(double control_cycle_sec, double cut_off_frequency)
79 {
80  cut_off_freq_ = cut_off_frequency;
81  control_cycle_sec_ = control_cycle_sec;
82  prev_output_ = 0;
83 
84  if(cut_off_frequency > 0)
85  alpha_ = (2.0*M_PI*cut_off_freq_*control_cycle_sec_)/(1.0+2.0*M_PI*cut_off_freq_*control_cycle_sec_);
86  else
87  alpha_ = 1;
88 }
89 
90 BalanceLowPassFilter::~BalanceLowPassFilter()
91 { }
92 
93 void BalanceLowPassFilter::initialize(double control_cycle_sec, double cut_off_frequency)
94 {
95  cut_off_freq_ = cut_off_frequency;
96  control_cycle_sec_ = control_cycle_sec;
97  prev_output_ = 0;
98 
99  if(cut_off_frequency > 0)
100  alpha_ = (2.0*M_PI*cut_off_freq_*control_cycle_sec_)/(1.0+2.0*M_PI*cut_off_freq_*control_cycle_sec_);
101  else
102  alpha_ = 1;
103 }
104 
105 void BalanceLowPassFilter::setCutOffFrequency(double cut_off_frequency)
106 {
107  cut_off_freq_ = cut_off_frequency;
108 
109  if(cut_off_frequency > 0)
110  alpha_ = (2.0*M_PI*cut_off_freq_*control_cycle_sec_)/(1.0+2.0*M_PI*cut_off_freq_*control_cycle_sec_);
111  else
112  alpha_ = 1;
113 }
114 
115 double BalanceLowPassFilter::getCutOffFrequency(void)
116 {
117  return cut_off_freq_;
118 }
119 
120 double BalanceLowPassFilter::getFilteredOutput(double present_raw_value)
121 {
122  prev_output_ = alpha_*present_raw_value + (1.0 - alpha_)*prev_output_;
123  return prev_output_;
124 }
125 
126 
127 BalancePDController::BalancePDController()
128 {
129  desired_ = 0;
130  p_gain_ = 0;
131  d_gain_ = 0;
132  curr_err_ = 0;
133  prev_err_ = 0;
134 }
135 
136 BalancePDController::~BalancePDController()
137 { }
138 
139 double BalancePDController::getFeedBack(double present_sensor_output)
140 {
141  prev_err_ = curr_err_;
142  curr_err_ = desired_ - present_sensor_output;
143 
144  return (p_gain_*curr_err_ + d_gain_*(curr_err_ - prev_err_));
145 }
146 
147 
148 BalanceControlUsingDampingConroller::BalanceControlUsingDampingConroller()
149 {
150  balance_control_error_ = BalanceControlError::NoError;
151  control_cycle_sec_ = 0.008;
152 
153  // balance enable
154  gyro_enable_ = 1.0;
155  orientation_enable_ = 1.0;
156  ft_enable_ = 1.0;
157 
158  desired_robot_to_cob_ = Eigen::MatrixXd::Identity(4,4);
159  desired_robot_to_right_foot_ = Eigen::MatrixXd::Identity(4,4);
160  desired_robot_to_left_foot_ = Eigen::MatrixXd::Identity(4,4);
161 
162  gyro_cut_off_freq_ = 10.0;
163  gyro_lpf_alpha_ = 2.0*M_PI*gyro_cut_off_freq_*control_cycle_sec_/(1.0 + 2.0*M_PI*gyro_cut_off_freq_*control_cycle_sec_);
164  gyro_roll_filtered_ = gyro_pitch_filtered_ = 0;
165  desired_gyro_roll_ = desired_gyro_pitch_ = 0;
166 
167  //sensed values
168  current_gyro_roll_rad_per_sec_ = current_gyro_pitch_rad_per_sec_ = 0;
169 
170  current_orientation_roll_rad_ = current_orientation_pitch_rad_ = 0;
171 
172  current_right_fx_N_ = current_right_fy_N_ = current_right_fz_N_ = 0;
173  current_right_tx_Nm_ = current_right_ty_Nm_ = current_right_tz_Nm_ = 0;
174  current_left_fx_N_ = current_left_fy_N_ = current_left_fz_N_ = 0;
175  current_left_tx_Nm_ = current_left_ty_Nm_ = current_left_tz_Nm_ = 0;
176 
177  // balance algorithm result
178  foot_roll_adjustment_by_gyro_roll_ = 0;
179  foot_pitch_adjustment_by_gyro_pitch_ = 0;
180 
181  foot_roll_adjustment_by_orientation_roll_ = 0;
182  foot_pitch_adjustment_by_orientation_pitch_ = 0;
183 
184  foot_z_adjustment_by_force_z_difference_ = 0;
185  r_foot_z_adjustment_by_force_z_ = 0;
186  l_foot_z_adjustment_by_force_z_ = 0;
187 
188  r_foot_x_adjustment_by_force_x_ = 0;
189  r_foot_y_adjustment_by_force_y_ = 0;
190  r_foot_roll_adjustment_by_torque_roll_ = 0;
191  r_foot_pitch_adjustment_by_torque_pitch_ = 0;
192 
193  l_foot_x_adjustment_by_force_x_ = 0;
194  l_foot_y_adjustment_by_force_y_ = 0;
195  l_foot_roll_adjustment_by_torque_roll_ = 0;
196  l_foot_pitch_adjustment_by_torque_pitch_ = 0;
197 
198  // manual cob adjustment
199  cob_x_manual_adjustment_m_ = 0;
200  cob_y_manual_adjustment_m_ = 0;
201  cob_z_manual_adjustment_m_ = 0;
202 
203  // gyro gain
204  gyro_balance_gain_ratio_ = 0.0;
205  gyro_balance_roll_gain_ = -0.10*0.75*gyro_balance_gain_ratio_;
206  gyro_balance_pitch_gain_ = -0.10*0.5 *gyro_balance_gain_ratio_;
207 
208 
209  // maximum adjustment
210  cob_x_adjustment_abs_max_m_ = 0.05;
211  cob_y_adjustment_abs_max_m_ = 0.05;
212  cob_z_adjustment_abs_max_m_ = 0.05;
213  cob_roll_adjustment_abs_max_rad_ = 15.0*DEGREE2RADIAN;
214  cob_pitch_adjustment_abs_max_rad_ = 15.0*DEGREE2RADIAN;
215  cob_yaw_adjustment_abs_max_rad_ = 15.0*DEGREE2RADIAN;
216  foot_x_adjustment_abs_max_m_ = 0.05;
217  foot_y_adjustment_abs_max_m_ = 0.05;
218  foot_z_adjustment_abs_max_m_ = 0.05;
219  foot_roll_adjustment_abs_max_rad_ = 15.0*DEGREE2RADIAN;
220  foot_pitch_adjustment_abs_max_rad_ = 15.0*DEGREE2RADIAN;
221  foot_yaw_adjustment_abs_max_rad_ = 15.0*DEGREE2RADIAN;
222 
223  mat_robot_to_cob_modified_ = Eigen::MatrixXd::Identity(4,4);
224  mat_robot_to_right_foot_modified_ = Eigen::MatrixXd::Identity(4,4);
225  mat_robot_to_left_foot_modified_ = Eigen::MatrixXd::Identity(4,4);
226  pose_cob_adjustment_ = Eigen::VectorXd::Zero(6);
227  pose_right_foot_adjustment_ = Eigen::VectorXd::Zero(6);;
228  pose_left_foot_adjustment_ = Eigen::VectorXd::Zero(6);;
229 }
230 
231 BalanceControlUsingDampingConroller::~BalanceControlUsingDampingConroller()
232 { }
233 
234 void BalanceControlUsingDampingConroller::initialize(const int control_cycle_msec)
235 {
236  balance_control_error_ = BalanceControlError::NoError;
237 
238  control_cycle_sec_ = control_cycle_msec * 0.001;
239 
240  gyro_cut_off_freq_ = 10.0;
241  gyro_lpf_alpha_ = 2.0*M_PI*gyro_cut_off_freq_*control_cycle_sec_/(1.0 + 2.0*M_PI*gyro_cut_off_freq_*control_cycle_sec_);
242 
243  pose_cob_adjustment_.fill(0);
244  pose_right_foot_adjustment_.fill(0);
245  pose_left_foot_adjustment_.fill(0);
246 
247  foot_roll_angle_ctrl_.control_cycle_sec_ = control_cycle_sec_;
248  foot_pitch_angle_ctrl_.control_cycle_sec_ = control_cycle_sec_;
249 
250  foot_force_z_diff_ctrl_.control_cycle_sec_ = control_cycle_sec_;
251  right_foot_force_z_ctrl_.control_cycle_sec_ = control_cycle_sec_;
252  left_foot_force_z_ctrl_.control_cycle_sec_ = control_cycle_sec_;
253 
254  right_foot_force_x_ctrl_.control_cycle_sec_ = control_cycle_sec_;
255  right_foot_force_y_ctrl_.control_cycle_sec_ = control_cycle_sec_;
256  right_foot_torque_roll_ctrl_.control_cycle_sec_ = control_cycle_sec_;
257  right_foot_torque_pitch_ctrl_.control_cycle_sec_ = control_cycle_sec_;
258 
259  left_foot_force_x_ctrl_.control_cycle_sec_ = control_cycle_sec_;
260  left_foot_force_y_ctrl_.control_cycle_sec_ = control_cycle_sec_;
261  left_foot_torque_roll_ctrl_.control_cycle_sec_ = control_cycle_sec_;
262  left_foot_torque_pitch_ctrl_.control_cycle_sec_ = control_cycle_sec_;
263 }
264 
265 void BalanceControlUsingDampingConroller::setGyroBalanceEnable(bool enable)
266 {
267  if(enable)
268  gyro_enable_ = 1.0;
269  else
270  gyro_enable_ = 0.0;
271 }
272 
273 void BalanceControlUsingDampingConroller::setOrientationBalanceEnable(bool enable)
274 {
275  if(enable)
276  orientation_enable_ = 1.0;
277  else
278  orientation_enable_ = 0.0;
279 }
280 
281 void BalanceControlUsingDampingConroller::setForceTorqueBalanceEnable(bool enable)
282 {
283  if(enable)
284  ft_enable_ = 1.0;
285  else
286  ft_enable_ = 0.0;
287 }
288 
289 void BalanceControlUsingDampingConroller::process(int *balance_error, Eigen::MatrixXd *robot_to_cob_modified, Eigen::MatrixXd *robot_to_right_foot_modified, Eigen::MatrixXd *robot_to_left_foot_modified)
290 {
291  balance_control_error_ = BalanceControlError::NoError;
292 
293  pose_cob_adjustment_.fill(0);
294  pose_right_foot_adjustment_.fill(0);
295  pose_left_foot_adjustment_.fill(0);
296 
297  // gyro
298  gyro_roll_filtered_ = current_gyro_roll_rad_per_sec_*gyro_lpf_alpha_ + (1.0 - gyro_lpf_alpha_)*gyro_roll_filtered_;
299  gyro_pitch_filtered_ = current_gyro_pitch_rad_per_sec_*gyro_lpf_alpha_ + (1.0 - gyro_lpf_alpha_)*gyro_pitch_filtered_;
300 
301  foot_roll_adjustment_by_gyro_roll_ = gyro_enable_ * (desired_gyro_roll_ - gyro_roll_filtered_) * gyro_balance_roll_gain_;
302  foot_pitch_adjustment_by_gyro_pitch_ = gyro_enable_ * (desired_gyro_pitch_ - gyro_pitch_filtered_) * gyro_balance_pitch_gain_;
303 
304  // z by imu
305  foot_roll_adjustment_by_orientation_roll_ = orientation_enable_ * foot_roll_angle_ctrl_.getDampingControllerOutput(current_orientation_roll_rad_);
306  foot_pitch_adjustment_by_orientation_pitch_ = orientation_enable_ * foot_pitch_angle_ctrl_.getDampingControllerOutput(current_orientation_pitch_rad_);
307 
308  Eigen::MatrixXd mat_orientation_adjustment_by_imu = robotis_framework::getRotation4d(foot_roll_adjustment_by_gyro_roll_ + foot_roll_adjustment_by_orientation_roll_, foot_pitch_adjustment_by_gyro_pitch_ + foot_pitch_adjustment_by_orientation_pitch_, 0.0);
309  Eigen::MatrixXd mat_r_xy, mat_l_xy;
310  mat_r_xy.resize(4,1); mat_l_xy.resize(4,1);
311  mat_r_xy.coeffRef(0,0) = desired_robot_to_right_foot_.coeff(0,3) - 0.5*(desired_robot_to_right_foot_.coeff(0,3) + desired_robot_to_left_foot_.coeff(0,3));
312  mat_r_xy.coeffRef(1,0) = desired_robot_to_right_foot_.coeff(1,3) - 0.5*(desired_robot_to_right_foot_.coeff(1,3) + desired_robot_to_left_foot_.coeff(1,3));
313  mat_r_xy.coeffRef(2,0) = 0.0;
314  mat_r_xy.coeffRef(3,0) = 1;
315 
316  mat_l_xy.coeffRef(0,0) = desired_robot_to_left_foot_.coeff(0,3) - 0.5*(desired_robot_to_right_foot_.coeff(0,3) + desired_robot_to_left_foot_.coeff(0,3));
317  mat_l_xy.coeffRef(1,0) = desired_robot_to_left_foot_.coeff(1,3) - 0.5*(desired_robot_to_right_foot_.coeff(1,3) + desired_robot_to_left_foot_.coeff(1,3));
318  mat_l_xy.coeffRef(2,0) = 0.0;
319  mat_l_xy.coeffRef(3,0) = 1;
320 
321  mat_r_xy = mat_orientation_adjustment_by_imu * mat_r_xy;
322  mat_l_xy = mat_orientation_adjustment_by_imu * mat_l_xy;
323 
324  // ft sensor
325  foot_z_adjustment_by_force_z_difference_ = ft_enable_*0.001*foot_force_z_diff_ctrl_.getDampingControllerOutput(current_left_fz_N_ - current_right_fz_N_);
326  r_foot_z_adjustment_by_force_z_ = ft_enable_*0.001*right_foot_force_z_ctrl_.getDampingControllerOutput(current_right_fz_N_);
327  l_foot_z_adjustment_by_force_z_ = ft_enable_*0.001*left_foot_force_z_ctrl_.getDampingControllerOutput(current_left_fz_N_);
328 
329  r_foot_x_adjustment_by_force_x_ = ft_enable_*0.001*right_foot_force_x_ctrl_.getDampingControllerOutput(current_right_fx_N_);
330  r_foot_y_adjustment_by_force_y_ = ft_enable_*0.001*right_foot_force_y_ctrl_.getDampingControllerOutput(current_right_fy_N_);
331  r_foot_roll_adjustment_by_torque_roll_ = ft_enable_*right_foot_torque_roll_ctrl_.getDampingControllerOutput(current_right_tx_Nm_);
332  r_foot_pitch_adjustment_by_torque_pitch_ = ft_enable_*right_foot_torque_pitch_ctrl_.getDampingControllerOutput(current_right_ty_Nm_);
333 
334  l_foot_x_adjustment_by_force_x_ = ft_enable_*0.001*left_foot_force_x_ctrl_.getDampingControllerOutput(current_left_fx_N_);
335  l_foot_y_adjustment_by_force_y_ = ft_enable_*0.001*left_foot_force_y_ctrl_.getDampingControllerOutput(current_left_fy_N_);
336  l_foot_roll_adjustment_by_torque_roll_ = ft_enable_*left_foot_torque_roll_ctrl_.getDampingControllerOutput(current_left_tx_Nm_);
337  l_foot_pitch_adjustment_by_torque_pitch_ = ft_enable_*left_foot_torque_pitch_ctrl_.getDampingControllerOutput(current_left_ty_Nm_);
338 
339  r_foot_z_adjustment_by_force_z_ += ft_enable_*0.001*0.0*(right_foot_force_z_ctrl_.desired_ - current_right_fz_N_);
340  l_foot_z_adjustment_by_force_z_ += ft_enable_*0.001*0.0*(left_foot_force_z_ctrl_.desired_ - current_left_fz_N_);
341  // sum of sensory balance result
342  pose_cob_adjustment_.coeffRef(0) = cob_x_manual_adjustment_m_;
343  pose_cob_adjustment_.coeffRef(1) = cob_y_manual_adjustment_m_;
344  pose_cob_adjustment_.coeffRef(2) = cob_z_manual_adjustment_m_;
345 
346  pose_right_foot_adjustment_.coeffRef(0) = r_foot_x_adjustment_by_force_x_;
347  pose_right_foot_adjustment_.coeffRef(1) = r_foot_y_adjustment_by_force_y_;
348  pose_right_foot_adjustment_.coeffRef(2) = 0.5*0.0*foot_z_adjustment_by_force_z_difference_ + mat_r_xy.coeff(2, 0) + r_foot_z_adjustment_by_force_z_*1.0;
349  pose_right_foot_adjustment_.coeffRef(3) = (foot_roll_adjustment_by_gyro_roll_ + foot_roll_adjustment_by_orientation_roll_ + r_foot_roll_adjustment_by_torque_roll_);
350  pose_right_foot_adjustment_.coeffRef(4) = (foot_pitch_adjustment_by_gyro_pitch_ + foot_pitch_adjustment_by_orientation_pitch_ + r_foot_pitch_adjustment_by_torque_pitch_);
351 
352  pose_left_foot_adjustment_.coeffRef(0) = l_foot_x_adjustment_by_force_x_;
353  pose_left_foot_adjustment_.coeffRef(1) = l_foot_y_adjustment_by_force_y_;
354  pose_left_foot_adjustment_.coeffRef(2) = -0.5*0.0*foot_z_adjustment_by_force_z_difference_ + mat_l_xy.coeff(2, 0) + l_foot_z_adjustment_by_force_z_*1.0;
355  pose_left_foot_adjustment_.coeffRef(3) = (foot_roll_adjustment_by_gyro_roll_ + foot_roll_adjustment_by_orientation_roll_ + l_foot_roll_adjustment_by_torque_roll_);
356  pose_left_foot_adjustment_.coeffRef(4) = (foot_pitch_adjustment_by_gyro_pitch_ + foot_pitch_adjustment_by_orientation_pitch_ + l_foot_pitch_adjustment_by_torque_pitch_);
357 
358  // check limitation
359  if((fabs(pose_cob_adjustment_.coeff(0)) == cob_x_adjustment_abs_max_m_ ) ||
360  (fabs(pose_cob_adjustment_.coeff(1)) == cob_y_adjustment_abs_max_m_ ) ||
361  (fabs(pose_cob_adjustment_.coeff(2)) == cob_z_adjustment_abs_max_m_ ) ||
362  (fabs(pose_cob_adjustment_.coeff(3)) == cob_roll_adjustment_abs_max_rad_ ) ||
363  (fabs(pose_cob_adjustment_.coeff(4)) == cob_pitch_adjustment_abs_max_rad_) ||
364  (fabs(pose_right_foot_adjustment_.coeff(0)) == foot_x_adjustment_abs_max_m_ ) ||
365  (fabs(pose_right_foot_adjustment_.coeff(1)) == foot_y_adjustment_abs_max_m_ ) ||
366  (fabs(pose_right_foot_adjustment_.coeff(2)) == foot_z_adjustment_abs_max_m_ ) ||
367  (fabs(pose_right_foot_adjustment_.coeff(3)) == foot_roll_adjustment_abs_max_rad_ ) ||
368  (fabs(pose_right_foot_adjustment_.coeff(4)) == foot_pitch_adjustment_abs_max_rad_) ||
369  (fabs(pose_left_foot_adjustment_.coeff(0)) == foot_x_adjustment_abs_max_m_ ) ||
370  (fabs(pose_left_foot_adjustment_.coeff(1)) == foot_y_adjustment_abs_max_m_ ) ||
371  (fabs(pose_left_foot_adjustment_.coeff(2)) == foot_z_adjustment_abs_max_m_ ) ||
372  (fabs(pose_left_foot_adjustment_.coeff(3)) == foot_roll_adjustment_abs_max_rad_ ) ||
373  (fabs(pose_left_foot_adjustment_.coeff(4)) == foot_pitch_adjustment_abs_max_rad_))
374  balance_control_error_ &= BalanceControlError::BalanceLimit;
375 
376  pose_cob_adjustment_.coeffRef(0) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(0)), cob_x_adjustment_abs_max_m_ ), pose_cob_adjustment_.coeff(0));
377  pose_cob_adjustment_.coeffRef(1) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(1)), cob_x_adjustment_abs_max_m_ ), pose_cob_adjustment_.coeff(1));
378  pose_cob_adjustment_.coeffRef(2) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(2)), cob_x_adjustment_abs_max_m_ ), pose_cob_adjustment_.coeff(2));
379  pose_cob_adjustment_.coeffRef(3) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(3)), cob_roll_adjustment_abs_max_rad_ ), pose_cob_adjustment_.coeff(3));
380  pose_cob_adjustment_.coeffRef(4) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(4)), cob_pitch_adjustment_abs_max_rad_), pose_cob_adjustment_.coeff(4));
381  pose_cob_adjustment_.coeffRef(5) = 0;
382 
383  pose_right_foot_adjustment_.coeffRef(0) = copysign(fmin(fabs(pose_right_foot_adjustment_.coeff(0)), foot_x_adjustment_abs_max_m_ ), pose_right_foot_adjustment_.coeff(0));
384  pose_right_foot_adjustment_.coeffRef(1) = copysign(fmin(fabs(pose_right_foot_adjustment_.coeff(1)), foot_y_adjustment_abs_max_m_ ), pose_right_foot_adjustment_.coeff(1));
385  pose_right_foot_adjustment_.coeffRef(2) = copysign(fmin(fabs(pose_right_foot_adjustment_.coeff(2)), foot_z_adjustment_abs_max_m_ ), pose_right_foot_adjustment_.coeff(2));
386  pose_right_foot_adjustment_.coeffRef(3) = copysign(fmin(fabs(pose_right_foot_adjustment_.coeff(3)), foot_roll_adjustment_abs_max_rad_ ), pose_right_foot_adjustment_.coeff(3));
387  pose_right_foot_adjustment_.coeffRef(4) = copysign(fmin(fabs(pose_right_foot_adjustment_.coeff(4)), foot_pitch_adjustment_abs_max_rad_), pose_right_foot_adjustment_.coeff(4));
388  pose_right_foot_adjustment_.coeffRef(5) = 0;
389 
390  pose_left_foot_adjustment_.coeffRef(0) = copysign(fmin(fabs(pose_left_foot_adjustment_.coeff(0)), foot_x_adjustment_abs_max_m_ ), pose_left_foot_adjustment_.coeff(0));
391  pose_left_foot_adjustment_.coeffRef(1) = copysign(fmin(fabs(pose_left_foot_adjustment_.coeff(1)), foot_y_adjustment_abs_max_m_ ), pose_left_foot_adjustment_.coeff(1));
392  pose_left_foot_adjustment_.coeffRef(2) = copysign(fmin(fabs(pose_left_foot_adjustment_.coeff(2)), foot_z_adjustment_abs_max_m_ ), pose_left_foot_adjustment_.coeff(2));
393  pose_left_foot_adjustment_.coeffRef(3) = copysign(fmin(fabs(pose_left_foot_adjustment_.coeff(3)), foot_roll_adjustment_abs_max_rad_ ), pose_left_foot_adjustment_.coeff(3));
394  pose_left_foot_adjustment_.coeffRef(4) = copysign(fmin(fabs(pose_left_foot_adjustment_.coeff(4)), foot_pitch_adjustment_abs_max_rad_), pose_left_foot_adjustment_.coeff(4));
395  pose_left_foot_adjustment_.coeffRef(5) = 0;
396 
397  Eigen::MatrixXd cob_rotation_adj = robotis_framework::getRotationZ(pose_cob_adjustment_.coeff(5)) * robotis_framework::getRotationY(pose_cob_adjustment_.coeff(4)) * robotis_framework::getRotationX(pose_cob_adjustment_.coeff(3));
398  Eigen::MatrixXd rf_rotation_adj = robotis_framework::getRotationZ(pose_right_foot_adjustment_.coeff(5)) * robotis_framework::getRotationY(pose_right_foot_adjustment_.coeff(4)) * robotis_framework::getRotationX(pose_right_foot_adjustment_.coeff(3));
399  Eigen::MatrixXd lf_rotation_adj = robotis_framework::getRotationZ(pose_left_foot_adjustment_.coeff(5)) * robotis_framework::getRotationY(pose_left_foot_adjustment_.coeff(4)) * robotis_framework::getRotationX(pose_left_foot_adjustment_.coeff(3));
400  mat_robot_to_cob_modified_.block<3,3>(0,0) = cob_rotation_adj * desired_robot_to_cob_.block<3,3>(0,0);
401  mat_robot_to_right_foot_modified_.block<3,3>(0,0) = rf_rotation_adj * desired_robot_to_right_foot_.block<3,3>(0,0);;
402  mat_robot_to_left_foot_modified_.block<3,3>(0,0) = lf_rotation_adj * desired_robot_to_left_foot_.block<3,3>(0,0);;
403 
404  mat_robot_to_cob_modified_.coeffRef(0,3) = desired_robot_to_cob_.coeff(0,3) + pose_cob_adjustment_.coeff(0);
405  mat_robot_to_cob_modified_.coeffRef(1,3) = desired_robot_to_cob_.coeff(1,3) + pose_cob_adjustment_.coeff(1);
406  mat_robot_to_cob_modified_.coeffRef(2,3) = desired_robot_to_cob_.coeff(2,3) + pose_cob_adjustment_.coeff(2);
407 
408  mat_robot_to_right_foot_modified_.coeffRef(0,3) = desired_robot_to_right_foot_.coeff(0,3) + pose_right_foot_adjustment_.coeff(0);
409  mat_robot_to_right_foot_modified_.coeffRef(1,3) = desired_robot_to_right_foot_.coeff(1,3) + pose_right_foot_adjustment_.coeff(1);
410  mat_robot_to_right_foot_modified_.coeffRef(2,3) = desired_robot_to_right_foot_.coeff(2,3) + pose_right_foot_adjustment_.coeff(2);
411 
412  mat_robot_to_left_foot_modified_.coeffRef(0,3) = desired_robot_to_left_foot_.coeff(0,3) + pose_left_foot_adjustment_.coeff(0);
413  mat_robot_to_left_foot_modified_.coeffRef(1,3) = desired_robot_to_left_foot_.coeff(1,3) + pose_left_foot_adjustment_.coeff(1);
414  mat_robot_to_left_foot_modified_.coeffRef(2,3) = desired_robot_to_left_foot_.coeff(2,3) + pose_left_foot_adjustment_.coeff(2);
415 
416  if(balance_error != 0)
417  *balance_error = balance_control_error_;
418 
419  *robot_to_cob_modified = mat_robot_to_cob_modified_;
420  *robot_to_right_foot_modified = mat_robot_to_right_foot_modified_;
421  *robot_to_left_foot_modified = mat_robot_to_left_foot_modified_;
422 }
423 
424 void BalanceControlUsingDampingConroller::setDesiredPose(const Eigen::MatrixXd &robot_to_cob, const Eigen::MatrixXd &robot_to_right_foot, const Eigen::MatrixXd &robot_to_left_foot)
425 {
426  desired_robot_to_cob_ = robot_to_cob;
427  desired_robot_to_right_foot_ = robot_to_right_foot;
428  desired_robot_to_left_foot_ = robot_to_left_foot;
429 }
430 
431 void BalanceControlUsingDampingConroller::setDesiredCOBGyro(double gyro_roll, double gyro_pitch)\
432 {
433  desired_gyro_roll_ = gyro_roll;
434  desired_gyro_pitch_ = gyro_pitch;
435 }
436 
437 void BalanceControlUsingDampingConroller::setDesiredCOBOrientation(double cob_orientation_roll, double cob_orientation_pitch)
438 {
439  foot_roll_angle_ctrl_.desired_ = cob_orientation_roll;
440  foot_pitch_angle_ctrl_.desired_ = cob_orientation_pitch;
441 }
442 
443 void BalanceControlUsingDampingConroller::setDesiredFootForceTorque(double r_force_x_N, double r_force_y_N, double r_force_z_N,
444  double r_torque_roll_Nm, double r_torque_pitch_Nm, double r_torque_yaw_Nm,
445  double l_force_x_N, double l_force_y_N, double l_force_z_N,
446  double l_torque_roll_Nm, double l_torque_pitch_Nm, double l_torque_yaw_Nm)
447 {
448  foot_force_z_diff_ctrl_.desired_ = l_force_z_N - r_force_z_N;
449  right_foot_force_z_ctrl_.desired_ = r_force_z_N;
450  left_foot_force_z_ctrl_.desired_ = l_force_z_N;
451 
452  right_foot_force_x_ctrl_.desired_ = r_force_x_N;
453  right_foot_force_y_ctrl_.desired_ = r_force_y_N;
454  right_foot_torque_roll_ctrl_.desired_ = r_torque_roll_Nm;
455  right_foot_torque_pitch_ctrl_.desired_ = r_torque_pitch_Nm;
456 
457  left_foot_force_x_ctrl_.desired_ = l_force_x_N;
458  left_foot_force_y_ctrl_.desired_ = l_force_y_N;
459  left_foot_torque_roll_ctrl_.desired_ = l_torque_roll_Nm;
460  left_foot_torque_pitch_ctrl_.desired_ = l_torque_pitch_Nm;
461 }
462 
463 
464 void BalanceControlUsingDampingConroller::setCurrentGyroSensorOutput(double gyro_roll, double gyro_pitch)
465 {
466  current_gyro_roll_rad_per_sec_ = gyro_roll;
467  current_gyro_pitch_rad_per_sec_ = gyro_pitch;
468 }
469 
470 void BalanceControlUsingDampingConroller::setCurrentOrientationSensorOutput(double cob_orientation_roll, double cob_orientation_pitch)
471 {
472  current_orientation_roll_rad_ = cob_orientation_roll;
473  current_orientation_pitch_rad_ = cob_orientation_pitch;
474 }
475 
476 void BalanceControlUsingDampingConroller::setCurrentFootForceTorqueSensorOutput(double r_force_x_N, double r_force_y_N, double r_force_z_N,
477  double r_torque_roll_Nm, double r_torque_pitch_Nm, double r_torque_yaw_Nm,
478  double l_force_x_N, double l_force_y_N, double l_force_z_N,
479  double l_torque_roll_Nm, double l_torque_pitch_Nm, double l_torque_yaw_Nm)
480 {
481  current_right_fx_N_ = r_force_x_N;
482  current_right_fy_N_ = r_force_y_N;
483  current_right_fz_N_ = r_force_z_N;
484  current_right_tx_Nm_ = r_torque_roll_Nm;
485  current_right_ty_Nm_ = r_torque_pitch_Nm;
486  current_right_tz_Nm_ = r_torque_yaw_Nm;
487 
488  current_left_fx_N_ = l_force_x_N;
489  current_left_fy_N_ = l_force_y_N;
490  current_left_fz_N_ = l_force_z_N;
491  current_left_tx_Nm_ = l_torque_roll_Nm;
492  current_left_ty_Nm_ = l_torque_pitch_Nm;
493  current_left_tz_Nm_ = l_torque_yaw_Nm;
494 }
495 
496 // set maximum adjustment
497 void BalanceControlUsingDampingConroller::setMaximumAdjustment(double cob_x_max_adjustment_m, double cob_y_max_adjustment_m, double cob_z_max_adjustment_m,
498  double cob_roll_max_adjustment_rad, double cob_pitch_max_adjustment_rad, double cob_yaw_max_adjustment_rad,
499  double foot_x_max_adjustment_m, double foot_y_max_adjustment_m, double foot_z_max_adjustment_m,
500  double foot_roll_max_adjustment_rad, double foot_pitch_max_adjustment_rad, double foot_yaw_max_adjustment_rad)
501 {
502  cob_x_adjustment_abs_max_m_ = cob_x_max_adjustment_m;
503  cob_y_adjustment_abs_max_m_ = cob_y_max_adjustment_m;
504  cob_z_adjustment_abs_max_m_ = cob_z_max_adjustment_m;
505  cob_roll_adjustment_abs_max_rad_ = cob_roll_max_adjustment_rad;
506  cob_pitch_adjustment_abs_max_rad_ = cob_pitch_max_adjustment_rad;
507  cob_yaw_adjustment_abs_max_rad_ = cob_yaw_max_adjustment_rad;
508  foot_x_adjustment_abs_max_m_ = foot_x_max_adjustment_m;
509  foot_y_adjustment_abs_max_m_ = foot_y_max_adjustment_m;
510  foot_z_adjustment_abs_max_m_ = foot_z_max_adjustment_m;
511  foot_roll_adjustment_abs_max_rad_ = foot_roll_max_adjustment_rad;
512  foot_pitch_adjustment_abs_max_rad_ = foot_pitch_max_adjustment_rad;
513  foot_yaw_adjustment_abs_max_rad_ = foot_yaw_max_adjustment_rad;
514 }
515 
516 //Manual Adjustment
517 void BalanceControlUsingDampingConroller::setCOBManualAdjustment(double cob_x_adjustment_m, double cob_y_adjustment_m, double cob_z_adjustment_m)
518 {
519  cob_x_manual_adjustment_m_ = cob_x_adjustment_m;
520  cob_y_manual_adjustment_m_ = cob_y_adjustment_m;
521  cob_z_manual_adjustment_m_ = cob_z_adjustment_m;
522 }
523 
524 double BalanceControlUsingDampingConroller::getCOBManualAdjustmentX()
525 {
526  return cob_x_manual_adjustment_m_;
527 }
528 
529 double BalanceControlUsingDampingConroller::getCOBManualAdjustmentY()
530 {
531  return cob_y_manual_adjustment_m_;
532 }
533 
534 double BalanceControlUsingDampingConroller::getCOBManualAdjustmentZ()
535 {
536  return cob_z_manual_adjustment_m_;
537 }
538 
539 void BalanceControlUsingDampingConroller::setGyroBalanceGainRatio(double gyro_balance_gain_ratio)
540 {
541  gyro_balance_gain_ratio_ = gyro_balance_gain_ratio;
542  gyro_balance_roll_gain_ = -0.10*0.75*gyro_balance_gain_ratio_;
543  gyro_balance_pitch_gain_ = -0.10*0.5 *gyro_balance_gain_ratio_;
544 }
545 
546 double BalanceControlUsingDampingConroller::getGyroBalanceGainRatio(void)
547 {
548  return gyro_balance_gain_ratio_;
549 }
550 
551 
552 BalanceControlUsingPDController::BalanceControlUsingPDController()
553 {
554  balance_control_error_ = BalanceControlError::NoError;
555  control_cycle_sec_ = 0.008;
556 
557  // balance enable
558  gyro_enable_ = 1.0;
559  orientation_enable_ = 1.0;
560  ft_enable_ = 1.0;
561 
562  desired_robot_to_cob_ = Eigen::MatrixXd::Identity(4,4);
563  desired_robot_to_right_foot_ = Eigen::MatrixXd::Identity(4,4);
564  desired_robot_to_left_foot_ = Eigen::MatrixXd::Identity(4,4);
565 
566  //sensed values
567  current_gyro_roll_rad_per_sec_ = current_gyro_pitch_rad_per_sec_ = 0;
568 
569  current_orientation_roll_rad_ = current_orientation_pitch_rad_ = 0;
570 
571  current_right_fx_N_ = current_right_fy_N_ = current_right_fz_N_ = 0;
572  current_right_tx_Nm_ = current_right_ty_Nm_ = current_right_tz_Nm_ = 0;
573  current_left_fx_N_ = current_left_fy_N_ = current_left_fz_N_ = 0;
574  current_left_tx_Nm_ = current_left_ty_Nm_ = current_left_tz_Nm_ = 0;
575 
576  // balance algorithm result
577  foot_roll_adjustment_by_gyro_roll_ = 0;
578  foot_pitch_adjustment_by_gyro_pitch_ = 0;
579 
580  foot_roll_adjustment_by_orientation_roll_ = 0;
581  foot_pitch_adjustment_by_orientation_pitch_ = 0;
582 
583  r_foot_z_adjustment_by_force_z_ = 0;
584  l_foot_z_adjustment_by_force_z_ = 0;
585 
586  r_foot_x_adjustment_by_force_x_ = 0;
587  r_foot_y_adjustment_by_force_y_ = 0;
588  r_foot_roll_adjustment_by_torque_roll_ = 0;
589  r_foot_pitch_adjustment_by_torque_pitch_ = 0;
590 
591  l_foot_x_adjustment_by_force_x_ = 0;
592  l_foot_y_adjustment_by_force_y_ = 0;
593  l_foot_roll_adjustment_by_torque_roll_ = 0;
594  l_foot_pitch_adjustment_by_torque_pitch_ = 0;
595 
596  // manual cob adjustment
597  cob_x_manual_adjustment_m_ = 0;
598  cob_y_manual_adjustment_m_ = 0;
599  cob_z_manual_adjustment_m_ = 0;
600 
601  // maximum adjustment
602  cob_x_adjustment_abs_max_m_ = 0.05;
603  cob_y_adjustment_abs_max_m_ = 0.05;
604  cob_z_adjustment_abs_max_m_ = 0.05;
605  cob_roll_adjustment_abs_max_rad_ = 15.0*DEGREE2RADIAN;
606  cob_pitch_adjustment_abs_max_rad_ = 15.0*DEGREE2RADIAN;
607  cob_yaw_adjustment_abs_max_rad_ = 15.0*DEGREE2RADIAN;
608  foot_x_adjustment_abs_max_m_ = 0.05;
609  foot_y_adjustment_abs_max_m_ = 0.05;
610  foot_z_adjustment_abs_max_m_ = 0.05;
611  foot_roll_adjustment_abs_max_rad_ = 15.0*DEGREE2RADIAN;
612  foot_pitch_adjustment_abs_max_rad_ = 15.0*DEGREE2RADIAN;
613  foot_yaw_adjustment_abs_max_rad_ = 15.0*DEGREE2RADIAN;
614 
615  mat_robot_to_cob_modified_ = Eigen::MatrixXd::Identity(4,4);
616  mat_robot_to_right_foot_modified_ = Eigen::MatrixXd::Identity(4,4);
617  mat_robot_to_left_foot_modified_ = Eigen::MatrixXd::Identity(4,4);
618  pose_cob_adjustment_ = Eigen::VectorXd::Zero(6);
619  pose_right_foot_adjustment_ = Eigen::VectorXd::Zero(6);
620  pose_left_foot_adjustment_ = Eigen::VectorXd::Zero(6);
621 }
622 
623 BalanceControlUsingPDController::~BalanceControlUsingPDController()
624 { }
625 
626 void BalanceControlUsingPDController::initialize(const int control_cycle_msec)
627 {
628  balance_control_error_ = BalanceControlError::NoError;
629 
630  control_cycle_sec_ = control_cycle_msec * 0.001;
631 
632  pose_cob_adjustment_.fill(0);
633  pose_right_foot_adjustment_.fill(0);
634  pose_left_foot_adjustment_.fill(0);
635 
636  roll_gyro_lpf_.initialize(control_cycle_sec_, 1.0);
637  pitch_gyro_lpf_.initialize(control_cycle_sec_, 1.0);
638 
639  roll_angle_lpf_.initialize(control_cycle_sec_, 1.0);
640  pitch_angle_lpf_.initialize(control_cycle_sec_, 1.0);
641 
642  right_foot_force_x_lpf_.initialize(control_cycle_sec_, 1.0);
643  right_foot_force_y_lpf_.initialize(control_cycle_sec_, 1.0);
644  right_foot_force_z_lpf_.initialize(control_cycle_sec_, 1.0);
645  right_foot_torque_roll_lpf_.initialize(control_cycle_sec_, 1.0);
646  right_foot_torque_pitch_lpf_.initialize(control_cycle_sec_, 1.0);
647 
648  left_foot_force_x_lpf_.initialize(control_cycle_sec_, 1.0);
649  left_foot_force_y_lpf_.initialize(control_cycle_sec_, 1.0);
650  left_foot_force_z_lpf_.initialize(control_cycle_sec_, 1.0);
651  left_foot_torque_roll_lpf_.initialize(control_cycle_sec_, 1.0);
652  left_foot_torque_pitch_lpf_.initialize(control_cycle_sec_, 1.0);
653 }
654 
655 void BalanceControlUsingPDController::setGyroBalanceEnable(bool enable)
656 {
657  if(enable)
658  gyro_enable_ = 1.0;
659  else
660  gyro_enable_ = 0.0;
661 }
662 
663 void BalanceControlUsingPDController::setOrientationBalanceEnable(bool enable)
664 {
665  if(enable)
666  orientation_enable_ = 1.0;
667  else
668  orientation_enable_ = 0.0;
669 }
670 
671 void BalanceControlUsingPDController::setForceTorqueBalanceEnable(bool enable)
672 {
673  if(enable)
674  ft_enable_ = 1.0;
675  else
676  ft_enable_ = 0.0;
677 }
678 
679 void BalanceControlUsingPDController::process(int *balance_error, Eigen::MatrixXd *robot_to_cob_modified, Eigen::MatrixXd *robot_to_right_foot_modified, Eigen::MatrixXd *robot_to_left_foot_modified)
680 {
681  balance_control_error_ = BalanceControlError::NoError;
682 
683  pose_cob_adjustment_.fill(0);
684  pose_right_foot_adjustment_.fill(0);
685  pose_left_foot_adjustment_.fill(0);
686 
687  //get filtered value
688  double roll_gyro_filtered = roll_gyro_lpf_.getFilteredOutput(current_gyro_roll_rad_per_sec_);
689  double pitch_gyro_filtered = pitch_gyro_lpf_.getFilteredOutput(current_gyro_pitch_rad_per_sec_);
690 
691  double roll_angle_filtered = roll_angle_lpf_.getFilteredOutput(current_orientation_roll_rad_);
692  double pitch_angle_filtered = pitch_angle_lpf_.getFilteredOutput(current_orientation_pitch_rad_);
693 
694  double right_foot_force_x_filtered = right_foot_force_x_lpf_.getFilteredOutput(current_right_fx_N_);
695  double right_foot_force_y_filtered = right_foot_force_y_lpf_.getFilteredOutput(current_right_fy_N_);
696  double right_foot_force_z_filtered = right_foot_force_z_lpf_.getFilteredOutput(current_right_fz_N_);
697  double right_foot_torque_roll_filtered = right_foot_torque_roll_lpf_.getFilteredOutput(current_right_tx_Nm_);
698  double right_foot_torque_pitch_filtered = right_foot_torque_pitch_lpf_.getFilteredOutput(current_right_ty_Nm_);;
699 
700  double left_foot_force_x_filtered = left_foot_force_x_lpf_.getFilteredOutput(current_left_fx_N_);
701  double left_foot_force_y_filtered = left_foot_force_y_lpf_.getFilteredOutput(current_left_fy_N_);
702  double left_foot_force_z_filtered = left_foot_force_z_lpf_.getFilteredOutput(current_left_fz_N_);
703  double left_foot_torque_roll_filtered = left_foot_torque_roll_lpf_.getFilteredOutput(current_left_tx_Nm_);
704  double left_foot_torque_pitch_filtered = left_foot_torque_pitch_lpf_.getFilteredOutput(current_left_ty_Nm_);
705 
706 
707  // gyro
708  foot_roll_adjustment_by_gyro_roll_ = -0.1*gyro_enable_*foot_roll_gyro_ctrl_.getFeedBack(roll_gyro_filtered);
709  foot_pitch_adjustment_by_gyro_pitch_ = -0.1*gyro_enable_*foot_pitch_gyro_ctrl_.getFeedBack(pitch_gyro_filtered);
710 
711  // z by imu
712  foot_roll_adjustment_by_orientation_roll_ = -1.0*orientation_enable_ * foot_roll_angle_ctrl_.getFeedBack(roll_angle_filtered);
713  foot_pitch_adjustment_by_orientation_pitch_ = -1.0*orientation_enable_ * foot_pitch_angle_ctrl_.getFeedBack(pitch_angle_filtered);
714 
715  Eigen::MatrixXd mat_orientation_adjustment_by_imu = robotis_framework::getRotation4d(foot_roll_adjustment_by_gyro_roll_ + foot_roll_adjustment_by_orientation_roll_, foot_pitch_adjustment_by_gyro_pitch_ + foot_pitch_adjustment_by_orientation_pitch_, 0.0);
716  Eigen::MatrixXd mat_r_xy, mat_l_xy;
717  mat_r_xy.resize(4,1); mat_l_xy.resize(4,1);
718  mat_r_xy.coeffRef(0,0) = desired_robot_to_right_foot_.coeff(0,3) - 0.5*(desired_robot_to_right_foot_.coeff(0,3) + desired_robot_to_left_foot_.coeff(0,3));
719  mat_r_xy.coeffRef(1,0) = desired_robot_to_right_foot_.coeff(1,3) - 0.5*(desired_robot_to_right_foot_.coeff(1,3) + desired_robot_to_left_foot_.coeff(1,3));
720  mat_r_xy.coeffRef(2,0) = 0.0;
721  mat_r_xy.coeffRef(3,0) = 1;
722 
723  mat_l_xy.coeffRef(0,0) = desired_robot_to_left_foot_.coeff(0,3) - 0.5*(desired_robot_to_right_foot_.coeff(0,3) + desired_robot_to_left_foot_.coeff(0,3));
724  mat_l_xy.coeffRef(1,0) = desired_robot_to_left_foot_.coeff(1,3) - 0.5*(desired_robot_to_right_foot_.coeff(1,3) + desired_robot_to_left_foot_.coeff(1,3));
725  mat_l_xy.coeffRef(2,0) = 0.0;
726  mat_l_xy.coeffRef(3,0) = 1;
727 
728  mat_r_xy = mat_orientation_adjustment_by_imu * mat_r_xy;
729  mat_l_xy = mat_orientation_adjustment_by_imu * mat_l_xy;
730 
731  // ft sensor
732  r_foot_x_adjustment_by_force_x_ = ft_enable_*0.001*right_foot_force_x_ctrl_.getFeedBack(right_foot_force_x_filtered);
733  r_foot_y_adjustment_by_force_y_ = ft_enable_*0.001*right_foot_force_y_ctrl_.getFeedBack(right_foot_force_y_filtered);
734  r_foot_z_adjustment_by_force_z_ = ft_enable_*0.001*right_foot_force_z_ctrl_.getFeedBack(right_foot_force_z_filtered);
735  r_foot_roll_adjustment_by_torque_roll_ = ft_enable_*right_foot_torque_roll_ctrl_.getFeedBack(right_foot_torque_roll_filtered);
736  r_foot_pitch_adjustment_by_torque_pitch_ = ft_enable_*right_foot_torque_pitch_ctrl_.getFeedBack(right_foot_torque_pitch_filtered);
737 
738  l_foot_x_adjustment_by_force_x_ = ft_enable_*0.001*left_foot_force_x_ctrl_.getFeedBack(left_foot_force_x_filtered);
739  l_foot_y_adjustment_by_force_y_ = ft_enable_*0.001*left_foot_force_y_ctrl_.getFeedBack(left_foot_force_y_filtered);
740  l_foot_z_adjustment_by_force_z_ = ft_enable_*0.001*left_foot_force_z_ctrl_.getFeedBack(left_foot_force_z_filtered);
741  l_foot_roll_adjustment_by_torque_roll_ = ft_enable_*left_foot_torque_roll_ctrl_.getFeedBack(left_foot_torque_roll_filtered);
742  l_foot_pitch_adjustment_by_torque_pitch_ = ft_enable_*left_foot_torque_pitch_ctrl_.getFeedBack(left_foot_torque_pitch_filtered);
743 
744  // sum of sensory balance result
745  pose_cob_adjustment_.coeffRef(0) = cob_x_manual_adjustment_m_;
746  pose_cob_adjustment_.coeffRef(1) = cob_y_manual_adjustment_m_;
747  pose_cob_adjustment_.coeffRef(2) = cob_z_manual_adjustment_m_;
748 
749  pose_right_foot_adjustment_.coeffRef(0) = r_foot_x_adjustment_by_force_x_;
750  pose_right_foot_adjustment_.coeffRef(1) = r_foot_y_adjustment_by_force_y_;
751  pose_right_foot_adjustment_.coeffRef(2) = mat_r_xy.coeff(2, 0) + r_foot_z_adjustment_by_force_z_*1.0;
752  pose_right_foot_adjustment_.coeffRef(3) = (foot_roll_adjustment_by_gyro_roll_ + foot_roll_adjustment_by_orientation_roll_ + r_foot_roll_adjustment_by_torque_roll_);
753  pose_right_foot_adjustment_.coeffRef(4) = (foot_pitch_adjustment_by_gyro_pitch_ + foot_pitch_adjustment_by_orientation_pitch_ + r_foot_pitch_adjustment_by_torque_pitch_);
754 
755  pose_left_foot_adjustment_.coeffRef(0) = l_foot_x_adjustment_by_force_x_;
756  pose_left_foot_adjustment_.coeffRef(1) = l_foot_y_adjustment_by_force_y_;
757  pose_left_foot_adjustment_.coeffRef(2) = mat_l_xy.coeff(2, 0) + l_foot_z_adjustment_by_force_z_*1.0;
758  pose_left_foot_adjustment_.coeffRef(3) = (foot_roll_adjustment_by_gyro_roll_ + foot_roll_adjustment_by_orientation_roll_ + l_foot_roll_adjustment_by_torque_roll_);
759  pose_left_foot_adjustment_.coeffRef(4) = (foot_pitch_adjustment_by_gyro_pitch_ + foot_pitch_adjustment_by_orientation_pitch_ + l_foot_pitch_adjustment_by_torque_pitch_);
760 
761  // check limitation
762  if((fabs(pose_cob_adjustment_.coeff(0)) == cob_x_adjustment_abs_max_m_ ) ||
763  (fabs(pose_cob_adjustment_.coeff(1)) == cob_y_adjustment_abs_max_m_ ) ||
764  (fabs(pose_cob_adjustment_.coeff(2)) == cob_z_adjustment_abs_max_m_ ) ||
765  (fabs(pose_cob_adjustment_.coeff(3)) == cob_roll_adjustment_abs_max_rad_ ) ||
766  (fabs(pose_cob_adjustment_.coeff(4)) == cob_pitch_adjustment_abs_max_rad_) ||
767  (fabs(pose_right_foot_adjustment_.coeff(0)) == foot_x_adjustment_abs_max_m_ ) ||
768  (fabs(pose_right_foot_adjustment_.coeff(1)) == foot_y_adjustment_abs_max_m_ ) ||
769  (fabs(pose_right_foot_adjustment_.coeff(2)) == foot_z_adjustment_abs_max_m_ ) ||
770  (fabs(pose_right_foot_adjustment_.coeff(3)) == foot_roll_adjustment_abs_max_rad_ ) ||
771  (fabs(pose_right_foot_adjustment_.coeff(4)) == foot_pitch_adjustment_abs_max_rad_) ||
772  (fabs(pose_left_foot_adjustment_.coeff(0)) == foot_x_adjustment_abs_max_m_ ) ||
773  (fabs(pose_left_foot_adjustment_.coeff(1)) == foot_y_adjustment_abs_max_m_ ) ||
774  (fabs(pose_left_foot_adjustment_.coeff(2)) == foot_z_adjustment_abs_max_m_ ) ||
775  (fabs(pose_left_foot_adjustment_.coeff(3)) == foot_roll_adjustment_abs_max_rad_ ) ||
776  (fabs(pose_left_foot_adjustment_.coeff(4)) == foot_pitch_adjustment_abs_max_rad_))
777  balance_control_error_ &= BalanceControlError::BalanceLimit;
778 
779  pose_cob_adjustment_.coeffRef(0) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(0)), cob_x_adjustment_abs_max_m_ ), pose_cob_adjustment_.coeff(0));
780  pose_cob_adjustment_.coeffRef(1) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(1)), cob_x_adjustment_abs_max_m_ ), pose_cob_adjustment_.coeff(1));
781  pose_cob_adjustment_.coeffRef(2) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(2)), cob_x_adjustment_abs_max_m_ ), pose_cob_adjustment_.coeff(2));
782  pose_cob_adjustment_.coeffRef(3) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(3)), cob_roll_adjustment_abs_max_rad_ ), pose_cob_adjustment_.coeff(3));
783  pose_cob_adjustment_.coeffRef(4) = copysign(fmin(fabs(pose_cob_adjustment_.coeff(4)), cob_pitch_adjustment_abs_max_rad_), pose_cob_adjustment_.coeff(4));
784  pose_cob_adjustment_.coeffRef(5) = 0;
785 
786  pose_right_foot_adjustment_.coeffRef(0) = copysign(fmin(fabs(pose_right_foot_adjustment_.coeff(0)), foot_x_adjustment_abs_max_m_ ), pose_right_foot_adjustment_.coeff(0));
787  pose_right_foot_adjustment_.coeffRef(1) = copysign(fmin(fabs(pose_right_foot_adjustment_.coeff(1)), foot_y_adjustment_abs_max_m_ ), pose_right_foot_adjustment_.coeff(1));
788  pose_right_foot_adjustment_.coeffRef(2) = copysign(fmin(fabs(pose_right_foot_adjustment_.coeff(2)), foot_z_adjustment_abs_max_m_ ), pose_right_foot_adjustment_.coeff(2));
789  pose_right_foot_adjustment_.coeffRef(3) = copysign(fmin(fabs(pose_right_foot_adjustment_.coeff(3)), foot_roll_adjustment_abs_max_rad_ ), pose_right_foot_adjustment_.coeff(3));
790  pose_right_foot_adjustment_.coeffRef(4) = copysign(fmin(fabs(pose_right_foot_adjustment_.coeff(4)), foot_pitch_adjustment_abs_max_rad_), pose_right_foot_adjustment_.coeff(4));
791  pose_right_foot_adjustment_.coeffRef(5) = 0;
792 
793  pose_left_foot_adjustment_.coeffRef(0) = copysign(fmin(fabs(pose_left_foot_adjustment_.coeff(0)), foot_x_adjustment_abs_max_m_ ), pose_left_foot_adjustment_.coeff(0));
794  pose_left_foot_adjustment_.coeffRef(1) = copysign(fmin(fabs(pose_left_foot_adjustment_.coeff(1)), foot_y_adjustment_abs_max_m_ ), pose_left_foot_adjustment_.coeff(1));
795  pose_left_foot_adjustment_.coeffRef(2) = copysign(fmin(fabs(pose_left_foot_adjustment_.coeff(2)), foot_z_adjustment_abs_max_m_ ), pose_left_foot_adjustment_.coeff(2));
796  pose_left_foot_adjustment_.coeffRef(3) = copysign(fmin(fabs(pose_left_foot_adjustment_.coeff(3)), foot_roll_adjustment_abs_max_rad_ ), pose_left_foot_adjustment_.coeff(3));
797  pose_left_foot_adjustment_.coeffRef(4) = copysign(fmin(fabs(pose_left_foot_adjustment_.coeff(4)), foot_pitch_adjustment_abs_max_rad_), pose_left_foot_adjustment_.coeff(4));
798  pose_left_foot_adjustment_.coeffRef(5) = 0;
799 
800  Eigen::MatrixXd cob_rotation_adj = robotis_framework::getRotationZ(pose_cob_adjustment_.coeff(5)) * robotis_framework::getRotationY(pose_cob_adjustment_.coeff(4)) * robotis_framework::getRotationX(pose_cob_adjustment_.coeff(3));
801  Eigen::MatrixXd rf_rotation_adj = robotis_framework::getRotationZ(pose_right_foot_adjustment_.coeff(5)) * robotis_framework::getRotationY(pose_right_foot_adjustment_.coeff(4)) * robotis_framework::getRotationX(pose_right_foot_adjustment_.coeff(3));
802  Eigen::MatrixXd lf_rotation_adj = robotis_framework::getRotationZ(pose_left_foot_adjustment_.coeff(5)) * robotis_framework::getRotationY(pose_left_foot_adjustment_.coeff(4)) * robotis_framework::getRotationX(pose_left_foot_adjustment_.coeff(3));
803  mat_robot_to_cob_modified_.block<3,3>(0,0) = cob_rotation_adj * desired_robot_to_cob_.block<3,3>(0,0);
804  mat_robot_to_right_foot_modified_.block<3,3>(0,0) = rf_rotation_adj * desired_robot_to_right_foot_.block<3,3>(0,0);;
805  mat_robot_to_left_foot_modified_.block<3,3>(0,0) = lf_rotation_adj * desired_robot_to_left_foot_.block<3,3>(0,0);;
806 
807  mat_robot_to_cob_modified_.coeffRef(0,3) = desired_robot_to_cob_.coeff(0,3) + pose_cob_adjustment_.coeff(0);
808  mat_robot_to_cob_modified_.coeffRef(1,3) = desired_robot_to_cob_.coeff(1,3) + pose_cob_adjustment_.coeff(1);
809  mat_robot_to_cob_modified_.coeffRef(2,3) = desired_robot_to_cob_.coeff(2,3) + pose_cob_adjustment_.coeff(2);
810 
811  mat_robot_to_right_foot_modified_.coeffRef(0,3) = desired_robot_to_right_foot_.coeff(0,3) + pose_right_foot_adjustment_.coeff(0);
812  mat_robot_to_right_foot_modified_.coeffRef(1,3) = desired_robot_to_right_foot_.coeff(1,3) + pose_right_foot_adjustment_.coeff(1);
813  mat_robot_to_right_foot_modified_.coeffRef(2,3) = desired_robot_to_right_foot_.coeff(2,3) + pose_right_foot_adjustment_.coeff(2);
814 
815  mat_robot_to_left_foot_modified_.coeffRef(0,3) = desired_robot_to_left_foot_.coeff(0,3) + pose_left_foot_adjustment_.coeff(0);
816  mat_robot_to_left_foot_modified_.coeffRef(1,3) = desired_robot_to_left_foot_.coeff(1,3) + pose_left_foot_adjustment_.coeff(1);
817  mat_robot_to_left_foot_modified_.coeffRef(2,3) = desired_robot_to_left_foot_.coeff(2,3) + pose_left_foot_adjustment_.coeff(2);
818 
819  if(balance_error != 0)
820  *balance_error = balance_control_error_;
821 
822  *robot_to_cob_modified = mat_robot_to_cob_modified_;
823  *robot_to_right_foot_modified = mat_robot_to_right_foot_modified_;
824  *robot_to_left_foot_modified = mat_robot_to_left_foot_modified_;
825 }
826 
827 void BalanceControlUsingPDController::setDesiredPose(const Eigen::MatrixXd &robot_to_cob, const Eigen::MatrixXd &robot_to_right_foot, const Eigen::MatrixXd &robot_to_left_foot)
828 {
829  desired_robot_to_cob_ = robot_to_cob;
830  desired_robot_to_right_foot_ = robot_to_right_foot;
831  desired_robot_to_left_foot_ = robot_to_left_foot;
832 }
833 
834 void BalanceControlUsingPDController::setDesiredCOBGyro(double gyro_roll, double gyro_pitch)\
835 {
836  foot_roll_gyro_ctrl_.desired_ = gyro_roll;
837  foot_pitch_gyro_ctrl_.desired_ = gyro_pitch;
838 }
839 
840 void BalanceControlUsingPDController::setDesiredCOBOrientation(double cob_orientation_roll, double cob_orientation_pitch)
841 {
842  foot_roll_angle_ctrl_.desired_ = cob_orientation_roll;
843  foot_pitch_angle_ctrl_.desired_ = cob_orientation_pitch;
844 }
845 
846 void BalanceControlUsingPDController::setDesiredFootForceTorque(double r_force_x_N, double r_force_y_N, double r_force_z_N,
847  double r_torque_roll_Nm, double r_torque_pitch_Nm, double r_torque_yaw_Nm,
848  double l_force_x_N, double l_force_y_N, double l_force_z_N,
849  double l_torque_roll_Nm, double l_torque_pitch_Nm, double l_torque_yaw_Nm)
850 {
851  right_foot_force_x_ctrl_.desired_ = r_force_x_N;
852  right_foot_force_y_ctrl_.desired_ = r_force_y_N;
853  right_foot_force_z_ctrl_.desired_ = r_force_z_N;
854  right_foot_torque_roll_ctrl_.desired_ = r_torque_roll_Nm;
855  right_foot_torque_pitch_ctrl_.desired_ = r_torque_pitch_Nm;
856 
857  left_foot_force_x_ctrl_.desired_ = l_force_x_N;
858  left_foot_force_y_ctrl_.desired_ = l_force_y_N;
859  left_foot_force_z_ctrl_.desired_ = l_force_z_N;
860  left_foot_torque_roll_ctrl_.desired_ = l_torque_roll_Nm;
861  left_foot_torque_pitch_ctrl_.desired_ = l_torque_pitch_Nm;
862 }
863 
864 
865 void BalanceControlUsingPDController::setCurrentGyroSensorOutput(double gyro_roll, double gyro_pitch)
866 {
867  current_gyro_roll_rad_per_sec_ = gyro_roll;
868  current_gyro_pitch_rad_per_sec_ = gyro_pitch;
869 }
870 
871 void BalanceControlUsingPDController::setCurrentOrientationSensorOutput(double cob_orientation_roll, double cob_orientation_pitch)
872 {
873  current_orientation_roll_rad_ = cob_orientation_roll;
874  current_orientation_pitch_rad_ = cob_orientation_pitch;
875 }
876 
877 void BalanceControlUsingPDController::setCurrentFootForceTorqueSensorOutput(double r_force_x_N, double r_force_y_N, double r_force_z_N,
878  double r_torque_roll_Nm, double r_torque_pitch_Nm, double r_torque_yaw_Nm,
879  double l_force_x_N, double l_force_y_N, double l_force_z_N,
880  double l_torque_roll_Nm, double l_torque_pitch_Nm, double l_torque_yaw_Nm)
881 {
882  current_right_fx_N_ = r_force_x_N;
883  current_right_fy_N_ = r_force_y_N;
884  current_right_fz_N_ = r_force_z_N;
885  current_right_tx_Nm_ = r_torque_roll_Nm;
886  current_right_ty_Nm_ = r_torque_pitch_Nm;
887  current_right_tz_Nm_ = r_torque_yaw_Nm;
888 
889  current_left_fx_N_ = l_force_x_N;
890  current_left_fy_N_ = l_force_y_N;
891  current_left_fz_N_ = l_force_z_N;
892  current_left_tx_Nm_ = l_torque_roll_Nm;
893  current_left_ty_Nm_ = l_torque_pitch_Nm;
894  current_left_tz_Nm_ = l_torque_yaw_Nm;
895 }
896 
897 // set maximum adjustment
898 void BalanceControlUsingPDController::setMaximumAdjustment(double cob_x_max_adjustment_m, double cob_y_max_adjustment_m, double cob_z_max_adjustment_m,
899  double cob_roll_max_adjustment_rad, double cob_pitch_max_adjustment_rad, double cob_yaw_max_adjustment_rad,
900  double foot_x_max_adjustment_m, double foot_y_max_adjustment_m, double foot_z_max_adjustment_m,
901  double foot_roll_max_adjustment_rad, double foot_pitch_max_adjustment_rad, double foot_yaw_max_adjustment_rad)
902 {
903  cob_x_adjustment_abs_max_m_ = cob_x_max_adjustment_m;
904  cob_y_adjustment_abs_max_m_ = cob_y_max_adjustment_m;
905  cob_z_adjustment_abs_max_m_ = cob_z_max_adjustment_m;
906  cob_roll_adjustment_abs_max_rad_ = cob_roll_max_adjustment_rad;
907  cob_pitch_adjustment_abs_max_rad_ = cob_pitch_max_adjustment_rad;
908  cob_yaw_adjustment_abs_max_rad_ = cob_yaw_max_adjustment_rad;
909  foot_x_adjustment_abs_max_m_ = foot_x_max_adjustment_m;
910  foot_y_adjustment_abs_max_m_ = foot_y_max_adjustment_m;
911  foot_z_adjustment_abs_max_m_ = foot_z_max_adjustment_m;
912  foot_roll_adjustment_abs_max_rad_ = foot_roll_max_adjustment_rad;
913  foot_pitch_adjustment_abs_max_rad_ = foot_pitch_max_adjustment_rad;
914  foot_yaw_adjustment_abs_max_rad_ = foot_yaw_max_adjustment_rad;
915 }
916 
917 //Manual Adjustment
918 void BalanceControlUsingPDController::setCOBManualAdjustment(double cob_x_adjustment_m, double cob_y_adjustment_m, double cob_z_adjustment_m)
919 {
920  cob_x_manual_adjustment_m_ = cob_x_adjustment_m;
921  cob_y_manual_adjustment_m_ = cob_y_adjustment_m;
922  cob_z_manual_adjustment_m_ = cob_z_adjustment_m;
923 }
924 
925 double BalanceControlUsingPDController::getCOBManualAdjustmentX()
926 {
927  return cob_x_manual_adjustment_m_;
928 }
929 
930 double BalanceControlUsingPDController::getCOBManualAdjustmentY()
931 {
932  return cob_y_manual_adjustment_m_;
933 }
934 
935 double BalanceControlUsingPDController::getCOBManualAdjustmentZ()
936 {
937  return cob_z_manual_adjustment_m_;
938 }
thormang3::BalanceControlUsingDampingConroller::setCurrentOrientationSensorOutput
void setCurrentOrientationSensorOutput(double cob_orientation_roll, double cob_orientation_pitch)
Definition: thormang3_balance_control.cpp:470
robotis_framework::getRotationZ
Eigen::Matrix3d getRotationZ(double angle)
thormang3::BalanceLowPassFilter::getFilteredOutput
double getFilteredOutput(double present_raw_value)
Definition: thormang3_balance_control.cpp:120
thormang3::BalanceControlUsingPDController::setDesiredCOBOrientation
void setDesiredCOBOrientation(double cob_orientation_roll, double cob_orientation_pitch)
Definition: thormang3_balance_control.cpp:840
thormang3::BalanceControlUsingDampingConroller::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)
Definition: thormang3_balance_control.cpp:289
thormang3::BalanceControlUsingDampingConroller::setDesiredCOBOrientation
void setDesiredCOBOrientation(double cob_orientation_roll, double cob_orientation_pitch)
Definition: thormang3_balance_control.cpp:437
thormang3::BalanceControlUsingDampingConroller::setCurrentGyroSensorOutput
void setCurrentGyroSensorOutput(double gyro_roll, double gyro_pitch)
Definition: thormang3_balance_control.cpp:464
thormang3::DampingController::getDampingControllerOutput
double getDampingControllerOutput(double present_sensor_output)
Definition: thormang3_balance_control.cpp:56
robotis_framework::getRotationX
Eigen::Matrix3d getRotationX(double angle)
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)
Definition: thormang3_balance_control.cpp:827
robotis_framework::getRotationY
Eigen::Matrix3d getRotationY(double angle)
thormang3::BalanceControlUsingDampingConroller::setCOBManualAdjustment
void setCOBManualAdjustment(double cob_x_adjustment_m, double cob_y_adjustment_m, double cob_z_adjustment_m)
Definition: thormang3_balance_control.cpp:517
thormang3::BalanceControlUsingDampingConroller::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)
Definition: thormang3_balance_control.cpp:476
thormang3::BalanceControlUsingDampingConroller::setGyroBalanceGainRatio
void setGyroBalanceGainRatio(double gyro_balance_gain_ratio)
Definition: thormang3_balance_control.cpp:539
thormang3::BalanceControlUsingDampingConroller::setMaximumAdjustment
void setMaximumAdjustment(double cob_x_max_adjustment_m, double cob_y_max_adjustment_m, double cob_z_max_adjustment_m, double cob_roll_max_adjustment_rad, double cob_pitch_max_adjustment_rad, double cob_yaw_max_adjustment_rad, double foot_x_max_adjustment_m, double foot_y_max_adjustment_m, double foot_z_max_adjustment_m, double foot_roll_max_adjustment_rad, double foot_pitch_max_adjustment_rad, double foot_yaw_max_adjustment_rad)
Definition: thormang3_balance_control.cpp:497
thormang3::BalanceControlUsingPDController::setCurrentGyroSensorOutput
void setCurrentGyroSensorOutput(double gyro_roll, double gyro_pitch)
Definition: thormang3_balance_control.cpp:865
thormang3::BalancePDController::getFeedBack
double getFeedBack(double present_sensor_output)
Definition: thormang3_balance_control.cpp:139
DEGREE2RADIAN
#define DEGREE2RADIAN
thormang3::BalanceLowPassFilter::setCutOffFrequency
void setCutOffFrequency(double cut_off_frequency)
Definition: thormang3_balance_control.cpp:105
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)
Definition: thormang3_balance_control.cpp:679
thormang3::BalanceControlUsingPDController::setDesiredCOBGyro
void setDesiredCOBGyro(double gyro_roll, double gyro_pitch)
Definition: thormang3_balance_control.cpp:834
thormang3::BalanceControlUsingDampingConroller::setDesiredCOBGyro
void setDesiredCOBGyro(double gyro_roll, double gyro_pitch)
Definition: thormang3_balance_control.cpp:431
thormang3::BalanceControlUsingPDController::setMaximumAdjustment
void setMaximumAdjustment(double cob_x_max_adjustment_m, double cob_y_max_adjustment_m, double cob_z_max_adjustment_m, double cob_roll_max_adjustment_rad, double cob_pitch_max_adjustment_rad, double cob_yaw_max_adjustment_rad, double foot_x_max_adjustment_m, double foot_y_max_adjustment_m, double foot_z_max_adjustment_m, double foot_roll_max_adjustment_rad, double foot_pitch_max_adjustment_rad, double foot_yaw_max_adjustment_rad)
Definition: thormang3_balance_control.cpp:898
robotis_framework::getRotation4d
Eigen::Matrix4d getRotation4d(double roll, double pitch, double yaw)
thormang3::BalanceControlUsingDampingConroller::setDesiredPose
void setDesiredPose(const Eigen::MatrixXd &robot_to_cob, const Eigen::MatrixXd &robot_to_right_foot, const Eigen::MatrixXd &robot_to_left_foot)
Definition: thormang3_balance_control.cpp:424
thormang3::BalanceControlUsingDampingConroller::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)
Definition: thormang3_balance_control.cpp:443
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)
Definition: thormang3_balance_control.cpp:877
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)
Definition: thormang3_balance_control.cpp:846
thormang3::BalanceControlUsingPDController::setCurrentOrientationSensorOutput
void setCurrentOrientationSensorOutput(double cob_orientation_roll, double cob_orientation_pitch)
Definition: thormang3_balance_control.cpp:871
thormang3::BalanceLowPassFilter::initialize
void initialize(double control_cycle_sec_, double cut_off_frequency)
Definition: thormang3_balance_control.cpp:93
thormang3_balance_control.h
thormang3::BalanceControlUsingPDController::setCOBManualAdjustment
void setCOBManualAdjustment(double cob_x_adjustment_m, double cob_y_adjustment_m, double cob_z_adjustment_m)
Definition: thormang3_balance_control.cpp:918

thormang3_balance_control
Author(s): Jay Song, Kayman
autogenerated on Mon Jun 10 2019 15:37:45