24 #ifndef THORMANG3_BALANCE_CONTROL_THORMANG3_BALANCE_CONTROL_H_ 25 #define THORMANG3_BALANCE_CONTROL_THORMANG3_BALANCE_CONTROL_H_ 27 #include <eigen3/Eigen/Eigen> 47 double getDampingControllerOutput(
double present_sensor_output);
72 double getFeedBack(
double present_sensor_output);
86 void initialize(
double control_cycle_sec_,
double cut_off_frequency);
87 void setCutOffFrequency(
double cut_off_frequency);
88 double getCutOffFrequency(
void);
89 double getFilteredOutput(
double present_raw_value);
105 void initialize(
const int control_cycle_msec);
107 void setGyroBalanceEnable(
bool enable);
108 void setOrientationBalanceEnable(
bool enable);
109 void setForceTorqueBalanceEnable(
bool enable);
111 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);
113 void setDesiredPose(
const Eigen::MatrixXd &robot_to_cob,
const Eigen::MatrixXd &robot_to_right_foot,
const Eigen::MatrixXd &robot_to_left_foot);
116 void setDesiredCOBGyro(
double gyro_roll,
double gyro_pitch);
117 void setDesiredCOBOrientation(
double cob_orientation_roll,
double cob_orientation_pitch);
118 void setDesiredFootForceTorque(
double r_force_x_N,
double r_force_y_N,
double r_force_z_N,
119 double r_torque_roll_Nm,
double r_torque_pitch_Nm,
double r_torque_yaw_Nm,
120 double l_force_x_N,
double l_force_y_N,
double l_force_z_N,
121 double l_torque_roll_Nm,
double l_torque_pitch_Nm,
double l_torque_yaw_Nm);
124 void setCurrentGyroSensorOutput(
double gyro_roll,
double gyro_pitch);
125 void setCurrentOrientationSensorOutput(
double cob_orientation_roll,
double cob_orientation_pitch);
128 void setCurrentFootForceTorqueSensorOutput(
double r_force_x_N,
double r_force_y_N,
double r_force_z_N,
129 double r_torque_roll_Nm,
double r_torque_pitch_Nm,
double r_torque_yaw_Nm,
130 double l_force_x_N,
double l_force_y_N,
double l_force_z_N,
131 double l_torque_roll_Nm,
double l_torque_pitch_Nm,
double l_torque_yaw_Nm);
135 void setMaximumAdjustment(
double cob_x_max_adjustment_m,
double cob_y_max_adjustment_m,
double cob_z_max_adjustment_m,
136 double cob_roll_max_adjustment_rad,
double cob_pitch_max_adjustment_rad,
double cob_yaw_max_adjustment_rad,
137 double foot_x_max_adjustment_m,
double foot_y_max_adjustment_m,
double foot_z_max_adjustment_m,
138 double foot_roll_max_adjustment_rad,
double foot_pitch_max_adjustment_rad,
double foot_yaw_max_adjustment_rad);
141 void setCOBManualAdjustment(
double cob_x_adjustment_m,
double cob_y_adjustment_m,
double cob_z_adjustment_m);
142 double getCOBManualAdjustmentX();
143 double getCOBManualAdjustmentY();
144 double getCOBManualAdjustmentZ();
146 void setGyroBalanceGainRatio(
double gyro_balance_gain_ratio);
147 double getGyroBalanceGainRatio(
void);
259 void initialize(
const int control_cycle_msec);
261 void setGyroBalanceEnable(
bool enable);
262 void setOrientationBalanceEnable(
bool enable);
263 void setForceTorqueBalanceEnable(
bool enable);
265 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);
267 void setDesiredPose(
const Eigen::MatrixXd &robot_to_cob,
const Eigen::MatrixXd &robot_to_right_foot,
const Eigen::MatrixXd &robot_to_left_foot);
270 void setDesiredCOBGyro(
double gyro_roll,
double gyro_pitch);
271 void setDesiredCOBOrientation(
double cob_orientation_roll,
double cob_orientation_pitch);
272 void setDesiredFootForceTorque(
double r_force_x_N,
double r_force_y_N,
double r_force_z_N,
273 double r_torque_roll_Nm,
double r_torque_pitch_Nm,
double r_torque_yaw_Nm,
274 double l_force_x_N,
double l_force_y_N,
double l_force_z_N,
275 double l_torque_roll_Nm,
double l_torque_pitch_Nm,
double l_torque_yaw_Nm);
278 void setCurrentGyroSensorOutput(
double gyro_roll,
double gyro_pitch);
279 void setCurrentOrientationSensorOutput(
double cob_orientation_roll,
double cob_orientation_pitch);
282 void setCurrentFootForceTorqueSensorOutput(
double r_force_x_N,
double r_force_y_N,
double r_force_z_N,
283 double r_torque_roll_Nm,
double r_torque_pitch_Nm,
double r_torque_yaw_Nm,
284 double l_force_x_N,
double l_force_y_N,
double l_force_z_N,
285 double l_torque_roll_Nm,
double l_torque_pitch_Nm,
double l_torque_yaw_Nm);
289 void setMaximumAdjustment(
double cob_x_max_adjustment_m,
double cob_y_max_adjustment_m,
double cob_z_max_adjustment_m,
290 double cob_roll_max_adjustment_rad,
double cob_pitch_max_adjustment_rad,
double cob_yaw_max_adjustment_rad,
291 double foot_x_max_adjustment_m,
double foot_y_max_adjustment_m,
double foot_z_max_adjustment_m,
292 double foot_roll_max_adjustment_rad,
double foot_pitch_max_adjustment_rad,
double foot_yaw_max_adjustment_rad);
295 void setCOBManualAdjustment(
double cob_x_adjustment_m,
double cob_y_adjustment_m,
double cob_z_adjustment_m);
296 double getCOBManualAdjustmentX();
297 double getCOBManualAdjustmentY();
298 double getCOBManualAdjustmentZ();
BalanceLowPassFilter left_foot_force_z_lpf_
double cob_roll_adjustment_abs_max_rad_
double foot_pitch_adjustment_by_gyro_pitch_
Eigen::MatrixXd desired_robot_to_left_foot_
BalanceLowPassFilter roll_gyro_lpf_
BalancePDController left_foot_force_y_ctrl_
double current_left_tz_Nm_
double foot_z_adjustment_abs_max_m_
double r_foot_y_adjustment_by_force_y_
BalanceLowPassFilter right_foot_torque_pitch_lpf_
double foot_roll_adjustment_abs_max_rad_
double cob_z_manual_adjustment_m_
BalanceLowPassFilter pitch_gyro_lpf_
BalanceLowPassFilter left_foot_torque_roll_lpf_
double r_foot_x_adjustment_by_force_x_
double foot_z_adjustment_by_force_z_difference_
double foot_y_adjustment_abs_max_m_
BalanceLowPassFilter left_foot_torque_pitch_lpf_
ROSCONSOLE_DECL void initialize()
double cob_yaw_adjustment_abs_max_rad_
DampingController right_foot_force_y_ctrl_
double foot_x_adjustment_abs_max_m_
Eigen::MatrixXd mat_robot_to_left_foot_modified_
Eigen::VectorXd pose_left_foot_adjustment_
double l_foot_pitch_adjustment_by_torque_pitch_
DampingController right_foot_force_x_ctrl_
double current_gyro_roll_rad_per_sec_
double cob_pitch_adjustment_abs_max_rad_
Eigen::MatrixXd mat_robot_to_right_foot_modified_
double cob_x_adjustment_abs_max_m_
double foot_roll_adjustment_by_orientation_roll_
double r_foot_roll_adjustment_by_torque_roll_
BalancePDController foot_roll_gyro_ctrl_
BalancePDController right_foot_force_y_ctrl_
double cob_z_manual_adjustment_m_
double cob_y_adjustment_abs_max_m_
BalancePDController right_foot_force_x_ctrl_
double l_foot_pitch_adjustment_by_torque_pitch_
double l_foot_x_adjustment_by_force_x_
double current_right_tz_Nm_
double l_foot_z_adjustment_by_force_z_
Eigen::VectorXd pose_left_foot_adjustment_
Eigen::MatrixXd mat_robot_to_left_foot_modified_
double cob_x_adjustment_abs_max_m_
BalancePDController left_foot_torque_roll_ctrl_
double cob_y_adjustment_abs_max_m_
double l_foot_roll_adjustment_by_torque_roll_
double cob_pitch_adjustment_abs_max_rad_
BalanceLowPassFilter right_foot_force_x_lpf_
Eigen::MatrixXd mat_robot_to_cob_modified_
double gyro_roll_filtered_
double r_foot_pitch_adjustment_by_torque_pitch_
double foot_z_adjustment_abs_max_m_
double current_right_fz_N_
double r_foot_y_adjustment_by_force_y_
DampingController right_foot_torque_roll_ctrl_
Eigen::MatrixXd desired_robot_to_left_foot_
double foot_yaw_adjustment_abs_max_rad_
double foot_yaw_adjustment_abs_max_rad_
BalanceLowPassFilter pitch_angle_lpf_
double foot_pitch_adjustment_by_orientation_pitch_
BalanceLowPassFilter left_foot_force_x_lpf_
DampingController left_foot_torque_pitch_ctrl_
DampingController left_foot_force_x_ctrl_
DampingController right_foot_torque_pitch_ctrl_
BalanceLowPassFilter right_foot_force_z_lpf_
double gyro_balance_roll_gain_
double current_left_fz_N_
double cob_yaw_adjustment_abs_max_rad_
double gyro_balance_gain_ratio_
double cob_roll_adjustment_abs_max_rad_
Eigen::MatrixXd mat_robot_to_right_foot_modified_
double current_left_fz_N_
double orientation_enable_
Eigen::MatrixXd desired_robot_to_right_foot_
double r_foot_roll_adjustment_by_torque_roll_
double r_foot_pitch_adjustment_by_torque_pitch_
Eigen::VectorXd pose_right_foot_adjustment_
BalanceLowPassFilter roll_angle_lpf_
double gyro_balance_pitch_gain_
double cob_x_manual_adjustment_m_
double l_foot_roll_adjustment_by_torque_roll_
Eigen::MatrixXd desired_robot_to_cob_
BalanceLowPassFilter right_foot_force_y_lpf_
double foot_pitch_adjustment_by_orientation_pitch_
DampingController left_foot_force_z_ctrl_
DampingController foot_force_z_diff_ctrl_
double control_cycle_sec_
Eigen::MatrixXd mat_robot_to_cob_modified_
Eigen::MatrixXd desired_robot_to_right_foot_
BalancePDController left_foot_force_x_ctrl_
DampingController foot_roll_angle_ctrl_
DampingController left_foot_force_y_ctrl_
double control_cycle_sec_
double current_orientation_roll_rad_
double foot_roll_adjustment_by_orientation_roll_
DampingController right_foot_force_z_ctrl_
double foot_pitch_adjustment_abs_max_rad_
double cob_z_adjustment_abs_max_m_
double foot_x_adjustment_abs_max_m_
double foot_pitch_adjustment_by_gyro_pitch_
DampingController foot_pitch_angle_ctrl_
Eigen::MatrixXd desired_robot_to_cob_
BalanceLowPassFilter left_foot_force_y_lpf_
Eigen::VectorXd pose_right_foot_adjustment_
BalancePDController left_foot_force_z_ctrl_
double l_foot_y_adjustment_by_force_y_
double gyro_cut_off_freq_
static const int BalanceLimit
BalancePDController foot_roll_angle_ctrl_
double current_right_fz_N_
BalancePDController right_foot_torque_pitch_ctrl_
BalanceLowPassFilter right_foot_torque_roll_lpf_
double cob_x_manual_adjustment_m_
int balance_control_error_
int balance_control_error_
double current_right_tz_Nm_
double l_foot_z_adjustment_by_force_z_
double foot_y_adjustment_abs_max_m_
double current_gyro_roll_rad_per_sec_
double time_constant_sec_
BalancePDController foot_pitch_angle_ctrl_
double r_foot_z_adjustment_by_force_z_
BalancePDController left_foot_torque_pitch_ctrl_
BalancePDController right_foot_torque_roll_ctrl_
double cob_y_manual_adjustment_m_
double current_orientation_roll_rad_
double foot_pitch_adjustment_abs_max_rad_
double r_foot_x_adjustment_by_force_x_
BalancePDController right_foot_force_z_ctrl_
Eigen::VectorXd pose_cob_adjustment_
BalancePDController foot_pitch_gyro_ctrl_
double cob_z_adjustment_abs_max_m_
double r_foot_z_adjustment_by_force_z_
double current_left_tz_Nm_
double orientation_enable_
double desired_gyro_roll_
DampingController left_foot_torque_roll_ctrl_
double control_cycle_sec_
Eigen::VectorXd pose_cob_adjustment_
double cob_y_manual_adjustment_m_
double foot_roll_adjustment_by_gyro_roll_
double control_cycle_sec_
double foot_roll_adjustment_abs_max_rad_
double l_foot_x_adjustment_by_force_x_
double foot_roll_adjustment_by_gyro_roll_
double l_foot_y_adjustment_by_force_y_