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

op3_balance_control
Author(s): SCH
autogenerated on Mon Jun 10 2019 14:41:08