19 #ifndef OP3_BALANCE_CONTROL_OP3_BALANCE_CONTROL_H_ 20 #define OP3_BALANCE_CONTROL_OP3_BALANCE_CONTROL_H_ 22 #include <eigen3/Eigen/Eigen> 43 double getDampingControllerOutput(
double present_sensor_output);
68 double getFeedBack(
double present_sensor_output);
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);
101 void initialize(
const int control_cycle_msec);
103 void setGyroBalanceEnable(
bool enable);
104 void setOrientationBalanceEnable(
bool enable);
105 void setForceTorqueBalanceEnable(
bool enable);
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);
109 void setDesiredPose(
const Eigen::MatrixXd &robot_to_cob,
const Eigen::MatrixXd &robot_to_right_foot,
const Eigen::MatrixXd &robot_to_left_foot);
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);
120 void setCurrentGyroSensorOutput(
double gyro_roll,
double gyro_pitch);
121 void setCurrentOrientationSensorOutput(
double cob_orientation_roll,
double cob_orientation_pitch);
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);
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);
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();
142 void setGyroBalanceGainRatio(
double gyro_balance_gain_ratio);
143 double getGyroBalanceGainRatio(
void);
255 void initialize(
const int control_cycle_msec);
257 void setGyroBalanceEnable(
bool enable);
258 void setOrientationBalanceEnable(
bool enable);
259 void setForceTorqueBalanceEnable(
bool enable);
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);
263 void setDesiredPose(
const Eigen::MatrixXd &robot_to_cob,
const Eigen::MatrixXd &robot_to_right_foot,
const Eigen::MatrixXd &robot_to_left_foot);
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);
274 void setCurrentGyroSensorOutput(
double gyro_roll,
double gyro_pitch);
275 void setCurrentOrientationSensorOutput(
double cob_orientation_roll,
double cob_orientation_pitch);
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);
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);
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();
int balance_control_error_
Eigen::VectorXd pose_left_foot_adjustment_
double current_right_fz_N_
double cob_z_adjustment_abs_max_m_
BalanceLowPassFilter roll_gyro_lpf_
double foot_roll_adjustment_by_orientation_roll_
Eigen::MatrixXd desired_robot_to_cob_
double foot_pitch_adjustment_abs_max_rad_
double r_foot_pitch_adjustment_by_torque_pitch_
double l_foot_z_adjustment_by_force_z_
double gyro_roll_filtered_
double control_cycle_sec_
double l_foot_pitch_adjustment_by_torque_pitch_
Eigen::MatrixXd mat_robot_to_left_foot_modified_
double r_foot_roll_adjustment_by_torque_roll_
double foot_pitch_adjustment_by_orientation_pitch_
double cob_z_manual_adjustment_m_
Eigen::VectorXd pose_right_foot_adjustment_
double foot_yaw_adjustment_abs_max_rad_
double control_cycle_sec_
Eigen::MatrixXd mat_robot_to_cob_modified_
BalanceLowPassFilter roll_angle_lpf_
ROSCONSOLE_DECL void initialize()
double orientation_enable_
double current_left_tz_Nm_
double cob_x_manual_adjustment_m_
BalancePDController right_foot_force_y_ctrl_
BalancePDController left_foot_force_y_ctrl_
double r_foot_z_adjustment_by_force_z_
Eigen::VectorXd pose_cob_adjustment_
double r_foot_z_adjustment_by_force_z_
double foot_x_adjustment_abs_max_m_
double foot_x_adjustment_abs_max_m_
double cob_y_adjustment_abs_max_m_
double cob_pitch_adjustment_abs_max_rad_
BalancePDController foot_roll_angle_ctrl_
double current_gyro_roll_rad_per_sec_
BalanceLowPassFilter right_foot_torque_pitch_lpf_
BalanceLowPassFilter right_foot_force_z_lpf_
BalanceLowPassFilter left_foot_force_z_lpf_
double l_foot_x_adjustment_by_force_x_
double cob_roll_adjustment_abs_max_rad_
double r_foot_y_adjustment_by_force_y_
double current_left_fz_N_
BalancePDController left_foot_torque_pitch_ctrl_
BalanceLowPassFilter left_foot_force_x_lpf_
double cob_y_manual_adjustment_m_
Eigen::MatrixXd desired_robot_to_right_foot_
double cob_y_adjustment_abs_max_m_
BalanceLowPassFilter right_foot_force_x_lpf_
double l_foot_y_adjustment_by_force_y_
double r_foot_x_adjustment_by_force_x_
double current_left_tz_Nm_
BalanceLowPassFilter pitch_angle_lpf_
double cob_y_manual_adjustment_m_
DampingController left_foot_force_y_ctrl_
Eigen::MatrixXd mat_robot_to_left_foot_modified_
double l_foot_pitch_adjustment_by_torque_pitch_
double foot_pitch_adjustment_by_orientation_pitch_
double gyro_balance_roll_gain_
double foot_y_adjustment_abs_max_m_
double cob_pitch_adjustment_abs_max_rad_
Eigen::MatrixXd desired_robot_to_left_foot_
double foot_roll_adjustment_by_gyro_roll_
double l_foot_y_adjustment_by_force_y_
double foot_roll_adjustment_by_orientation_roll_
double control_cycle_sec_
double cob_x_manual_adjustment_m_
BalancePDController left_foot_force_z_ctrl_
double foot_roll_adjustment_abs_max_rad_
double current_right_tz_Nm_
BalancePDController foot_pitch_gyro_ctrl_
double r_foot_x_adjustment_by_force_x_
double current_right_fz_N_
DampingController right_foot_force_y_ctrl_
double l_foot_x_adjustment_by_force_x_
double r_foot_y_adjustment_by_force_y_
double r_foot_pitch_adjustment_by_torque_pitch_
Eigen::MatrixXd mat_robot_to_right_foot_modified_
double gyro_cut_off_freq_
double time_constant_sec_
int balance_control_error_
double r_foot_roll_adjustment_by_torque_roll_
double foot_pitch_adjustment_abs_max_rad_
double l_foot_roll_adjustment_by_torque_roll_
double current_gyro_roll_rad_per_sec_
DampingController left_foot_torque_roll_ctrl_
BalancePDController right_foot_torque_roll_ctrl_
double foot_y_adjustment_abs_max_m_
double current_orientation_roll_rad_
BalancePDController foot_roll_gyro_ctrl_
double current_left_fz_N_
BalancePDController foot_pitch_angle_ctrl_
double foot_z_adjustment_abs_max_m_
DampingController right_foot_torque_roll_ctrl_
Eigen::MatrixXd mat_robot_to_right_foot_modified_
BalancePDController right_foot_torque_pitch_ctrl_
double foot_roll_adjustment_abs_max_rad_
static const int BalanceLimit
DampingController foot_pitch_angle_ctrl_
BalanceLowPassFilter left_foot_force_y_lpf_
double foot_pitch_adjustment_by_gyro_pitch_
double foot_roll_adjustment_by_gyro_roll_
Eigen::MatrixXd desired_robot_to_left_foot_
DampingController left_foot_force_x_ctrl_
double foot_yaw_adjustment_abs_max_rad_
double control_cycle_sec_
Eigen::VectorXd pose_left_foot_adjustment_
double cob_roll_adjustment_abs_max_rad_
Eigen::VectorXd pose_right_foot_adjustment_
DampingController foot_roll_angle_ctrl_
double foot_z_adjustment_by_force_z_difference_
BalanceLowPassFilter right_foot_force_y_lpf_
BalanceLowPassFilter right_foot_torque_roll_lpf_
double desired_gyro_roll_
BalancePDController right_foot_force_x_ctrl_
BalanceLowPassFilter pitch_gyro_lpf_
double cob_z_adjustment_abs_max_m_
DampingController left_foot_torque_pitch_ctrl_
DampingController right_foot_force_z_ctrl_
double cob_yaw_adjustment_abs_max_rad_
DampingController left_foot_force_z_ctrl_
Eigen::VectorXd pose_cob_adjustment_
double foot_z_adjustment_abs_max_m_
double l_foot_roll_adjustment_by_torque_roll_
double cob_x_adjustment_abs_max_m_
double cob_z_manual_adjustment_m_
double foot_pitch_adjustment_by_gyro_pitch_
double gyro_balance_gain_ratio_
BalancePDController left_foot_torque_roll_ctrl_
Eigen::MatrixXd desired_robot_to_right_foot_
double orientation_enable_
DampingController right_foot_force_x_ctrl_
DampingController right_foot_torque_pitch_ctrl_
double l_foot_z_adjustment_by_force_z_
double cob_x_adjustment_abs_max_m_
double gyro_balance_pitch_gain_
Eigen::MatrixXd desired_robot_to_cob_
double current_right_tz_Nm_
BalancePDController left_foot_force_x_ctrl_
BalanceLowPassFilter left_foot_torque_pitch_lpf_
Eigen::MatrixXd mat_robot_to_cob_modified_
BalanceLowPassFilter left_foot_torque_roll_lpf_
BalancePDController right_foot_force_z_ctrl_
double current_orientation_roll_rad_
DampingController foot_force_z_diff_ctrl_
double cob_yaw_adjustment_abs_max_rad_