From 9caf528ea5933efd23c4f8d4d5dbcbff7a2eae97 Mon Sep 17 00:00:00 2001 From: Junichiro Sugihara Date: Wed, 13 Dec 2023 21:32:25 +0900 Subject: [PATCH] [Beetle] modified the method to calc ff wrench to pid control. --- .../aerial_robot_control/control/utils/pid.h | 26 ++++-- robots/beetle/config/BeetleControl_sim.yaml | 6 +- .../beetle/control/beetle_controller.h | 14 ++- .../beetle/src/control/beetle_controller.cpp | 93 +++++++++++-------- 4 files changed, 89 insertions(+), 50 deletions(-) diff --git a/aerial_robot_control/include/aerial_robot_control/control/utils/pid.h b/aerial_robot_control/include/aerial_robot_control/control/utils/pid.h index 31de87656..4468f5888 100644 --- a/aerial_robot_control/include/aerial_robot_control/control/utils/pid.h +++ b/aerial_robot_control/include/aerial_robot_control/control/utils/pid.h @@ -50,7 +50,7 @@ namespace aerial_robot_control const double p_gain = 0, const double i_gain = 0, const double d_gain = 0, const double limit_sum = 1e6, const double limit_p = 1e6, const double limit_i = 1e6, const double limit_d = 1e6, const double limit_err_p = 1e6, const double limit_err_i = 1e6, const double limit_err_d = 1e6): - name_(name), result_(0), err_p_(0), err_i_(0), err_i_prev_(0), err_d_(0), + name_(name), result_(0), err_p_(0), err_p_prev_(0), err_i_(0), err_i_prev_(0), err_d_(0), target_p_(0), target_d_(0), val_p_(0), val_d_(0), p_term_(0), i_term_(0), d_term_(0), i_comp_term_(0) @@ -66,13 +66,25 @@ namespace aerial_robot_control err_p_ = clamp(err_p, -limit_err_p_, limit_err_p_); err_i_prev_ = err_i_; err_d_ = clamp(err_d, -limit_err_d_, limit_err_d_); + err_i_ = clamp(err_i_ + err_p_ * du, -limit_err_i_, limit_err_i_) + i_comp_term_; - /* special process for I term */ - err_i_ = clamp(err_i_ + err_p_ * du, -limit_err_i_, limit_err_i_); - err_i_rec_ = err_i_ + i_comp_term_; + p_term_ = clamp(err_p_ * p_gain_, -limit_p_, limit_p_); + i_term_ = clamp(err_i_ * i_gain_, -limit_i_, limit_i_); + d_term_ = clamp(err_d_ * d_gain_, -limit_d_, limit_d_); + + result_ = clamp(p_term_ + i_term_ + d_term_ + feedforward_term, -limit_sum_, limit_sum_); + } + + virtual void updateWoVel(const double err_p, const double du, const double feedforward_term = 0) + { + err_p_prev_ = err_p_; + err_p_ = clamp(err_p, -limit_err_p_, limit_err_p_); + err_i_prev_ = err_i_; + err_d_ = clamp((err_p_ - err_p_prev_)/ du, -limit_err_d_, limit_err_d_); + err_i_ = clamp(err_i_ + err_p_ * du, -limit_err_i_, limit_err_i_) + i_comp_term_; p_term_ = clamp(err_p_ * p_gain_, -limit_p_, limit_p_); - i_term_ = clamp(err_i_rec_ * i_gain_, -limit_i_, limit_i_); + i_term_ = clamp(err_i_ * i_gain_, -limit_i_, limit_i_); d_term_ = clamp(err_d_ * d_gain_, -limit_d_, limit_d_); result_ = clamp(p_term_ + i_term_ + d_term_ + feedforward_term, -limit_sum_, limit_sum_); @@ -84,7 +96,9 @@ namespace aerial_robot_control { err_i_ = 0; err_i_prev_ = 0; + err_p_prev_ = 0; result_ = 0; + i_comp_term_ = 0; } const double& getPGain() const { return p_gain_; } @@ -146,7 +160,7 @@ namespace aerial_robot_control double result_; double p_gain_, i_gain_, d_gain_; double p_term_, i_term_, d_term_; - double err_p_, err_i_, err_i_prev_, err_d_; + double err_p_, err_p_prev_, err_i_, err_i_prev_, err_d_; double err_i_rec_; double limit_sum_, limit_p_, limit_i_, limit_d_; double limit_err_p_, limit_err_i_, limit_err_d_; diff --git a/robots/beetle/config/BeetleControl_sim.yaml b/robots/beetle/config/BeetleControl_sim.yaml index 251b291e8..73a379f31 100644 --- a/robots/beetle/config/BeetleControl_sim.yaml +++ b/robots/beetle/config/BeetleControl_sim.yaml @@ -3,14 +3,16 @@ aerial_robot_control_name: aerial_robot_control/beetle_controller controller: torque_allocation_matrix_inv_pub_interval: 0.05 wrench_allocation_matrix_pub_interval: 0.1 - gimbal_calc_in_fc : true + gimbal_calc_in_fc : false i_term_rp_calc_in_pc: true wrench_estimate_update_rate: 100 momentum_observer_force_weight: 3 momentum_observer_torque_weight: 2.5 wrench_estimate_flag: true comp_term_update_freq: 40 - wrench_comp_gain: 0.2 + wrench_comp_p_gain: 0.05 + wrench_comp_d_gain: 0.02 + wrench_comp_i_gain: 0.002 pd_wrench_comp_mode: true diff --git a/robots/beetle/include/beetle/control/beetle_controller.h b/robots/beetle/include/beetle/control/beetle_controller.h index 29be91e8c..b449b593a 100644 --- a/robots/beetle/include/beetle/control/beetle_controller.h +++ b/robots/beetle/include/beetle/control/beetle_controller.h @@ -8,6 +8,16 @@ namespace aerial_robot_control { + enum + { + FX = YAW +1, + FY, + FZ, + TX, + TY, + TZ, + }; + class BeetleController: public GimbalrotorController { public: @@ -42,7 +52,9 @@ namespace aerial_robot_control double comp_term_update_freq_; double prev_comp_update_time_; - double wrench_comp_gain_; + double wrench_comp_p_gain_; + double wrench_comp_i_gain_; + double wrench_comp_d_gain_; double I_comp_Fx_; double I_comp_Fy_; double I_comp_Fz_; diff --git a/robots/beetle/src/control/beetle_controller.cpp b/robots/beetle/src/control/beetle_controller.cpp index 6cc0a4c09..fcd310a37 100644 --- a/robots/beetle/src/control/beetle_controller.cpp +++ b/robots/beetle/src/control/beetle_controller.cpp @@ -38,14 +38,14 @@ namespace aerial_robot_control inter_wrench_list_.insert(make_pair(i+1, wrench)); wrench_comp_list_.insert(make_pair(i+1, wrench)); } - pid_controllers_.at(X).setLimitErrI(pid_controllers_.at(X).getLimitI() / pid_controllers_.at(X).getIGain()); - pid_controllers_.at(Y).setLimitErrI(pid_controllers_.at(Y).getLimitI() / pid_controllers_.at(Y).getIGain()); - pid_controllers_.at(Z).setLimitErrI(pid_controllers_.at(Z).getLimitI() / pid_controllers_.at(Z).getIGain()); - pid_controllers_.at(ROLL).setLimitErrI(pid_controllers_.at(ROLL).getLimitI() / pid_controllers_.at(ROLL).getIGain()); - pid_controllers_.at(PITCH).setLimitErrI(pid_controllers_.at(PITCH).getLimitI() / pid_controllers_.at(PITCH).getIGain()); - pid_controllers_.at(YAW).setLimitErrI(pid_controllers_.at(YAW).getLimitI() / pid_controllers_.at(YAW).getIGain()); - - prev_comp_update_time_ = 0; + pid_controllers_.push_back(PID("f_x", wrench_comp_p_gain_, wrench_comp_i_gain_, wrench_comp_d_gain_)); + pid_controllers_.push_back(PID("f_y", wrench_comp_p_gain_, wrench_comp_i_gain_, wrench_comp_d_gain_)); + pid_controllers_.push_back(PID("f_z", wrench_comp_p_gain_, wrench_comp_i_gain_, wrench_comp_d_gain_)); + pid_controllers_.push_back(PID("t_x", wrench_comp_p_gain_, wrench_comp_i_gain_, wrench_comp_d_gain_)); + pid_controllers_.push_back(PID("t_y", wrench_comp_p_gain_, wrench_comp_i_gain_, wrench_comp_d_gain_)); + pid_controllers_.push_back(PID("t_z", wrench_comp_p_gain_, wrench_comp_i_gain_, wrench_comp_d_gain_)); + + prev_comp_update_time_ = -1; } void BeetleController::controlCore() @@ -56,15 +56,10 @@ namespace aerial_robot_control bool comp_update_flag = false; double comp_update_interval = 1 / comp_term_update_freq_; if(beetle_robot_model_->getControlFlag() && - ros::Time::now().toSec() - prev_comp_update_time_ > comp_update_interval && module_state != SEPARATED){ calcInteractionWrench(); - prev_comp_update_time_ = ros::Time::now().toSec(); comp_update_flag = true; - } - - if(!beetle_robot_model_->getControlFlag() || - module_state == SEPARATED){ + }else{ for(int i = 0; i < max_modules_num; i++){ est_wrench_list_[i+1] = Eigen::VectorXd::Zero(6); inter_wrench_list_[i+1] = Eigen::VectorXd::Zero(6); @@ -93,12 +88,28 @@ namespace aerial_robot_control double IGain_Ty = pid_controllers_.at(PITCH).getIGain(); double IGain_Tz = pid_controllers_.at(YAW).getIGain(); - I_comp_Fx_ = I_reconfig_acc_cog_term(0) / IGain_Fx; - I_comp_Fy_ = I_reconfig_acc_cog_term(1) / IGain_Fy; - I_comp_Fz_ = I_reconfig_acc_cog_term(2) / IGain_Fz; - I_comp_Tx_ = I_reconfig_acc_cog_term(3) / IGain_Tx; - I_comp_Ty_ = I_reconfig_acc_cog_term(4) / IGain_Ty; - I_comp_Tz_ = I_reconfig_acc_cog_term(5) / IGain_Tz; + double du; + if(prev_comp_update_time_ < 0){ + prev_comp_update_time_ = ros::Time::now().toSec(); + return; + }else{ + du = ros::Time::now().toSec() - prev_comp_update_time_; + prev_comp_update_time_ = ros::Time::now().toSec(); + } + + pid_controllers_.at(FX).updateWoVel(I_reconfig_acc_cog_term(0) / IGain_Fx, du); + pid_controllers_.at(FY).updateWoVel(I_reconfig_acc_cog_term(1) / IGain_Fy, du); + pid_controllers_.at(FZ).updateWoVel(I_reconfig_acc_cog_term(2) / IGain_Fz, du); + pid_controllers_.at(TX).updateWoVel(I_reconfig_acc_cog_term(3) / IGain_Tx, du); + pid_controllers_.at(TY).updateWoVel(I_reconfig_acc_cog_term(4) / IGain_Ty, du); + pid_controllers_.at(TZ).updateWoVel(I_reconfig_acc_cog_term(5) / IGain_Tz, du); + + I_comp_Fx_ = pid_controllers_.at(FX).result(); + I_comp_Fy_ = pid_controllers_.at(FY).result(); + I_comp_Fz_ = pid_controllers_.at(FZ).result(); + I_comp_Tx_ = pid_controllers_.at(TX).result(); + I_comp_Ty_ = pid_controllers_.at(TY).result(); + I_comp_Tz_ = pid_controllers_.at(TZ).result(); pid_controllers_.at(X).setICompTerm(I_comp_Fx_); pid_controllers_.at(Y).setICompTerm(I_comp_Fy_); @@ -118,22 +129,18 @@ namespace aerial_robot_control external_wrench_compensation_pub_.publish(wrench_msg); }else{ - if(pre_module_state_ == FOLLOWER && module_state == SEPARATED) - { - pid_controllers_.at(X).setICompTerm(0.0); - pid_controllers_.at(Y).setICompTerm(0.0); - pid_controllers_.at(Z).setICompTerm(0.0); - pid_controllers_.at(ROLL).setICompTerm(0.0); - pid_controllers_.at(PITCH).setICompTerm(0.0); - pid_controllers_.at(YAW).setICompTerm(0.0); - - pid_controllers_.at(X).setErrI(pid_controllers_.at(X).getErrIRec()); - pid_controllers_.at(Y).setErrI(pid_controllers_.at(Y).getErrIRec()); - pid_controllers_.at(Z).setErrI(pid_controllers_.at(Z).getErrIRec()); - pid_controllers_.at(ROLL).setErrI(pid_controllers_.at(ROLL).getErrIRec()); - pid_controllers_.at(PITCH).setErrI(pid_controllers_.at(PITCH).getErrIRec()); - pid_controllers_.at(YAW).setErrI(pid_controllers_.at(YAW).getErrIRec()); - } + pid_controllers_.at(FX).reset(); + pid_controllers_.at(FY).reset(); + pid_controllers_.at(FZ).reset(); + pid_controllers_.at(TX).reset(); + pid_controllers_.at(TY).reset(); + pid_controllers_.at(TZ).reset(); + pid_controllers_.at(X).setICompTerm(0.0); + pid_controllers_.at(Y).setICompTerm(0.0); + pid_controllers_.at(Z).setICompTerm(0.0); + pid_controllers_.at(ROLL).setICompTerm(0.0); + pid_controllers_.at(PITCH).setICompTerm(0.0); + pid_controllers_.at(YAW).setICompTerm(0.0); } GimbalrotorController::controlCore(); @@ -192,7 +199,8 @@ namespace aerial_robot_control for(int i = leader_id-1; i > 0; i--){ if(assembly_flag[i]){ wrench_comp_sum_left += inter_wrench_list_[i]; - wrench_comp_list_[i] += wrench_comp_gain_ * wrench_comp_sum_left; + // wrench_comp_list_[i] += wrench_comp_gain_ * wrench_comp_sum_left; + wrench_comp_list_[i] = wrench_comp_sum_left; right_module_id = i; }else{ wrench_comp_list_[i] = Eigen::VectorXd::Zero(6); @@ -202,10 +210,11 @@ namespace aerial_robot_control int max_modules_num = beetle_robot_model_->getMaxModuleNum(); int left_module_id = leader_id; Eigen::VectorXd wrench_comp_sum_right = Eigen::VectorXd::Zero(6); - for(int i = leader_id+1; i < max_modules_num; i++){ + for(int i = leader_id+1; i <= max_modules_num; i++){ if(assembly_flag[i]){ wrench_comp_sum_right -= inter_wrench_list_[left_module_id]; - wrench_comp_list_[i] += wrench_comp_gain_ * wrench_comp_sum_right; + // wrench_comp_list_[i] += wrench_comp_gain_ * wrench_comp_sum_right; + wrench_comp_list_[i] = wrench_comp_sum_right; left_module_id = i; }else{ wrench_comp_list_[i] = Eigen::VectorXd::Zero(6); @@ -231,7 +240,9 @@ namespace aerial_robot_control ROS_INFO_STREAM("lower limit of external wrench : "<(control_nh, "comp_term_update_freq", comp_term_update_freq_, 10); - getParam(control_nh, "wrench_comp_gain", wrench_comp_gain_, 0.2); + getParam(control_nh, "wrench_comp_p_gain", wrench_comp_p_gain_, 0.1); + getParam(control_nh, "wrench_comp_i_gain", wrench_comp_i_gain_, 0.005); + getParam(control_nh, "wrench_comp_d_gain", wrench_comp_d_gain_, 0.05); } void BeetleController::externalWrenchEstimate() @@ -240,7 +251,7 @@ namespace aerial_robot_control if(navigator_->getNaviState() != aerial_robot_navigation::HOVER_STATE && navigator_->getNaviState() != aerial_robot_navigation::TAKEOFF_STATE && - navigator_->getNaviState() != aerial_robot_navigation::LAND_STATE) + navigator_->getNaviState() != aerial_robot_navigation:: LAND_STATE) { prev_est_wrench_timestamp_ = 0; integrate_term_ = Eigen::VectorXd::Zero(6);