Skip to content

Commit

Permalink
[Beetle] modified the method to calc ff wrench to pid control.
Browse files Browse the repository at this point in the history
  • Loading branch information
@sugihara-16
sugihara-16 committed Dec 13, 2023
1 parent a72f1ce commit 9caf528
Show file tree
Hide file tree
Showing 4 changed files with 89 additions and 50 deletions.
26 changes: 20 additions & 6 deletions aerial_robot_control/include/aerial_robot_control/control/utils/pid.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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_);
Expand All @@ -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_; }
Expand Down Expand Up @@ -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_;
Expand Down
6 changes: 4 additions & 2 deletions robots/beetle/config/BeetleControl_sim.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
14 changes: 13 additions & 1 deletion robots/beetle/include/beetle/control/beetle_controller.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,16 @@

namespace aerial_robot_control
{
enum
{
FX = YAW +1,
FY,
FZ,
TX,
TY,
TZ,
};

class BeetleController: public GimbalrotorController
{
public:
Expand Down Expand Up @@ -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_;
Expand Down
93 changes: 52 additions & 41 deletions robots/beetle/src/control/beetle_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand All @@ -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);
Expand Down Expand Up @@ -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_);
Expand All @@ -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();
Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand All @@ -231,7 +240,9 @@ namespace aerial_robot_control
ROS_INFO_STREAM("lower limit of external wrench : "<<external_wrench_lower_limit_.transpose());

getParam<double>(control_nh, "comp_term_update_freq", comp_term_update_freq_, 10);
getParam<double>(control_nh, "wrench_comp_gain", wrench_comp_gain_, 0.2);
getParam<double>(control_nh, "wrench_comp_p_gain", wrench_comp_p_gain_, 0.1);
getParam<double>(control_nh, "wrench_comp_i_gain", wrench_comp_i_gain_, 0.005);
getParam<double>(control_nh, "wrench_comp_d_gain", wrench_comp_d_gain_, 0.05);
}

void BeetleController::externalWrenchEstimate()
Expand All @@ -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);
Expand Down

0 comments on commit 9caf528

Please sign in to comment.