op3_balance_control.h
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 #ifndef OP3_BALANCE_CONTROL_OP3_BALANCE_CONTROL_H_
20 #define OP3_BALANCE_CONTROL_OP3_BALANCE_CONTROL_H_
21 
22 #include <eigen3/Eigen/Eigen>
23 
25 
26 namespace robotis_op
27 {
28 
30 {
31 public:
32  static const int NoError = 0;
33  static const int BalanceLimit = 2;
34 };
35 
37 {
38 public:
40  DampingController(double time_unit_sec);
42 
43  double getDampingControllerOutput(double present_sensor_output);
44 
45  double desired_;
46 
47  double gain_;
49  double output_;
50 
52 
53 private:
55 };
56 
58 {
59 public:
62 
63  double desired_;
64 
65  double p_gain_;
66  double d_gain_;
67 
68  double getFeedBack(double present_sensor_output);
69 
70 private:
71  double curr_err_;
72  double prev_err_;
73 };
74 
76 {
77 public:
79  BalanceLowPassFilter(double control_cycle_sec, double cut_off_frequency);
81 
82  void initialize(double control_cycle_sec_, double cut_off_frequency);
83  void setCutOffFrequency(double cut_off_frequency);
84  double getCutOffFrequency(void);
85  double getFilteredOutput(double present_raw_value);
86 
87 private:
88  double cut_off_freq_;
90  double alpha_;
91 
92  double prev_output_;
93 };
94 
96 {
97 public:
100 
101  void initialize(const int control_cycle_msec);
102 
103  void setGyroBalanceEnable(bool enable);
104  void setOrientationBalanceEnable(bool enable);
105  void setForceTorqueBalanceEnable(bool enable);
106 
107  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);
108 
109  void setDesiredPose(const Eigen::MatrixXd &robot_to_cob, const Eigen::MatrixXd &robot_to_right_foot, const Eigen::MatrixXd &robot_to_left_foot);
110 
111  // all arguments are with respect to robot coordinate.
112  void setDesiredCOBGyro(double gyro_roll, double gyro_pitch);
113  void setDesiredCOBOrientation(double cob_orientation_roll, double cob_orientation_pitch);
114  void setDesiredFootForceTorque(double r_force_x_N, double r_force_y_N, double r_force_z_N,
115  double r_torque_roll_Nm, double r_torque_pitch_Nm, double r_torque_yaw_Nm,
116  double l_force_x_N, double l_force_y_N, double l_force_z_N,
117  double l_torque_roll_Nm, double l_torque_pitch_Nm, double l_torque_yaw_Nm);
118 
119  // with respect to robot coordinate.
120  void setCurrentGyroSensorOutput(double gyro_roll, double gyro_pitch);
121  void setCurrentOrientationSensorOutput(double cob_orientation_roll, double cob_orientation_pitch);
122 
123  // with respect to robot coordinate.
124  void setCurrentFootForceTorqueSensorOutput(double r_force_x_N, double r_force_y_N, double r_force_z_N,
125  double r_torque_roll_Nm, double r_torque_pitch_Nm, double r_torque_yaw_Nm,
126  double l_force_x_N, double l_force_y_N, double l_force_z_N,
127  double l_torque_roll_Nm, double l_torque_pitch_Nm, double l_torque_yaw_Nm);
128 
129 
130  // set maximum adjustment
131  void setMaximumAdjustment(double cob_x_max_adjustment_m, double cob_y_max_adjustment_m, double cob_z_max_adjustment_m,
132  double cob_roll_max_adjustment_rad, double cob_pitch_max_adjustment_rad, double cob_yaw_max_adjustment_rad,
133  double foot_x_max_adjustment_m, double foot_y_max_adjustment_m, double foot_z_max_adjustment_m,
134  double foot_roll_max_adjustment_rad, double foot_pitch_max_adjustment_rad, double foot_yaw_max_adjustment_rad);
135 
136  //Manual Adjustment
137  void setCOBManualAdjustment(double cob_x_adjustment_m, double cob_y_adjustment_m, double cob_z_adjustment_m);
138  double getCOBManualAdjustmentX();
139  double getCOBManualAdjustmentY();
140  double getCOBManualAdjustmentZ();
141 
142  void setGyroBalanceGainRatio(double gyro_balance_gain_ratio);
143  double getGyroBalanceGainRatio(void);
144 
145  // damping controllers
148 
152 
157 
162 
163 private:
166 
167  // balance enable
168  double gyro_enable_;
170  double ft_enable_;
171 
172 
173  // desired pose
174  Eigen::MatrixXd desired_robot_to_cob_;
177 
178  // for gyro balancing
184  double gyro_roll_filtered_, gyro_pitch_filtered_;
185  double desired_gyro_roll_, desired_gyro_pitch_;
186 
187  // sensed values
188  double current_gyro_roll_rad_per_sec_, current_gyro_pitch_rad_per_sec_;
189 
190  double current_orientation_roll_rad_, current_orientation_pitch_rad_;
191 
192  double current_right_fx_N_, current_right_fy_N_, current_right_fz_N_;
193  double current_right_tx_Nm_, current_right_ty_Nm_, current_right_tz_Nm_;
194  double current_left_fx_N_, current_left_fy_N_, current_left_fz_N_;
195  double current_left_tx_Nm_, current_left_ty_Nm_, current_left_tz_Nm_;
196 
197  // manual cob adjustment
201 
202  // result of balance control
205 
208 
212 
217 
222 
223  // sum of results of balance control
224  Eigen::VectorXd pose_cob_adjustment_;
226  Eigen::VectorXd pose_left_foot_adjustment_;
227 
228  Eigen::MatrixXd mat_robot_to_cob_modified_;
231 
232  // maximum adjustment
239 
246 
247 };
248 
250 {
251 public:
254 
255  void initialize(const int control_cycle_msec);
256 
257  void setGyroBalanceEnable(bool enable);
258  void setOrientationBalanceEnable(bool enable);
259  void setForceTorqueBalanceEnable(bool enable);
260 
261  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);
262 
263  void setDesiredPose(const Eigen::MatrixXd &robot_to_cob, const Eigen::MatrixXd &robot_to_right_foot, const Eigen::MatrixXd &robot_to_left_foot);
264 
265  // all arguments are with respect to robot coordinate.
266  void setDesiredCOBGyro(double gyro_roll, double gyro_pitch);
267  void setDesiredCOBOrientation(double cob_orientation_roll, double cob_orientation_pitch);
268  void setDesiredFootForceTorque(double r_force_x_N, double r_force_y_N, double r_force_z_N,
269  double r_torque_roll_Nm, double r_torque_pitch_Nm, double r_torque_yaw_Nm,
270  double l_force_x_N, double l_force_y_N, double l_force_z_N,
271  double l_torque_roll_Nm, double l_torque_pitch_Nm, double l_torque_yaw_Nm);
272 
273  // with respect to robot coordinate.
274  void setCurrentGyroSensorOutput(double gyro_roll, double gyro_pitch);
275  void setCurrentOrientationSensorOutput(double cob_orientation_roll, double cob_orientation_pitch);
276 
277  // with respect to robot coordinate.
278  void setCurrentFootForceTorqueSensorOutput(double r_force_x_N, double r_force_y_N, double r_force_z_N,
279  double r_torque_roll_Nm, double r_torque_pitch_Nm, double r_torque_yaw_Nm,
280  double l_force_x_N, double l_force_y_N, double l_force_z_N,
281  double l_torque_roll_Nm, double l_torque_pitch_Nm, double l_torque_yaw_Nm);
282 
283 
284  // set maximum adjustment
285  void setMaximumAdjustment(double cob_x_max_adjustment_m, double cob_y_max_adjustment_m, double cob_z_max_adjustment_m,
286  double cob_roll_max_adjustment_rad, double cob_pitch_max_adjustment_rad, double cob_yaw_max_adjustment_rad,
287  double foot_x_max_adjustment_m, double foot_y_max_adjustment_m, double foot_z_max_adjustment_m,
288  double foot_roll_max_adjustment_rad, double foot_pitch_max_adjustment_rad, double foot_yaw_max_adjustment_rad);
289 
290  //Manual Adjustment
291  void setCOBManualAdjustment(double cob_x_adjustment_m, double cob_y_adjustment_m, double cob_z_adjustment_m);
292  double getCOBManualAdjustmentX();
293  double getCOBManualAdjustmentY();
294  double getCOBManualAdjustmentZ();
295 
296  // damping controllers
301 
304 
309 
314 
317 
320 
326 
332 
333 private:
336 
337  // balance enable
338  double gyro_enable_;
340  double ft_enable_;
341 
342  // desired pose
343  Eigen::MatrixXd desired_robot_to_cob_;
346 
347  // sensed values
348  double current_gyro_roll_rad_per_sec_, current_gyro_pitch_rad_per_sec_;
349 
350  double current_orientation_roll_rad_, current_orientation_pitch_rad_;
351 
352  double current_right_fx_N_, current_right_fy_N_, current_right_fz_N_;
353  double current_right_tx_Nm_, current_right_ty_Nm_, current_right_tz_Nm_;
354  double current_left_fx_N_, current_left_fy_N_, current_left_fz_N_;
355  double current_left_tx_Nm_, current_left_ty_Nm_, current_left_tz_Nm_;
356 
357  // manual cob adjustment
361 
362  // result of balance control
365 
368 
371 
376 
381 
382  // sum of results of balance control
383  Eigen::VectorXd pose_cob_adjustment_;
385  Eigen::VectorXd pose_left_foot_adjustment_;
386 
387  Eigen::MatrixXd mat_robot_to_cob_modified_;
390 
391  // maximum adjustment
398 
405 };
406 
407 }
408 
409 #endif
ROSCONSOLE_DECL void initialize()


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