[Beetle] modified the method to update wrench compensation

sugihara-16 · Dec 6, 2023 · b4c872d · b4c872d
1 parent 6b37d87
commit b4c872d
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 11 deletions.
diff --git a/robots/beetle/config/BeetleControl.yaml b/robots/beetle/config/BeetleControl.yaml
@@ -1,15 +1,15 @@
 aerial_robot_control_name: aerial_robot_control/beetle_controller
 
 controller:
-  exter
   torque_allocation_matrix_inv_pub_interval: 0.05
   wrench_allocation_matrix_pub_interval: 0.1
   gimbal_calc_in_fc : 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: 5
+  comp_term_update_freq: 20
+  wrench_comp_gain: 0.2
 
   pd_wrench_comp_mode: true
 

diff --git a/robots/beetle/config/BeetleControl_sim.yaml b/robots/beetle/config/BeetleControl_sim.yaml
@@ -8,7 +8,8 @@ controller:
   momentum_observer_force_weight: 3
   momentum_observer_torque_weight: 2.5
   wrench_estimate_flag: true
-  comp_term_update_freq: 5
+  comp_term_update_freq: 20
+  wrench_comp_gain: 0.2
 
   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
@@ -42,6 +42,7 @@ namespace aerial_robot_control
 
     double comp_term_update_freq_;
     double prev_comp_update_time_;
+    double wrench_comp_gain_;
 
     void controlCore() override;
     void calcInteractionWrench();

diff --git a/robots/beetle/src/control/beetle_controller.cpp b/robots/beetle/src/control/beetle_controller.cpp
@@ -87,12 +87,12 @@ namespace aerial_robot_control
 
       geometry_msgs::WrenchStamped wrench_msg;
       wrench_msg.header.stamp.fromSec(estimator_->getImuLatestTimeStamp());
-      wrench_msg.wrench.force.x = I_reconfig_acc_cog_term(0);
-      wrench_msg.wrench.force.y = I_reconfig_acc_cog_term(1);
-      wrench_msg.wrench.force.z = I_reconfig_acc_cog_term(2);
-      wrench_msg.wrench.torque.x = I_reconfig_acc_cog_term(3);
-      wrench_msg.wrench.torque.y = I_reconfig_acc_cog_term(4);
-      wrench_msg.wrench.torque.z = I_reconfig_acc_cog_term(5);
+      wrench_msg.wrench.force.x = wrench_comp_term(0);
+      wrench_msg.wrench.force.y = wrench_comp_term(1);
+      wrench_msg.wrench.force.z = wrench_comp_term(2);
+      wrench_msg.wrench.torque.x = wrench_comp_term(3);
+      wrench_msg.wrench.torque.y = wrench_comp_term(4);
+      wrench_msg.wrench.torque.z = wrench_comp_term(5);
       external_wrench_compensation_pub_.publish(wrench_msg);
       GimbalrotorController::controlCore();
     }else{
@@ -154,7 +154,7 @@ 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_sum_left;
+        wrench_comp_list_[i] += wrench_comp_gain_ *  wrench_comp_sum_left;
         right_module_id = i;
       }
     }
@@ -165,7 +165,7 @@ namespace aerial_robot_control
     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_sum_right;
+        wrench_comp_list_[i] += wrench_comp_gain_ * wrench_comp_sum_right;
         left_module_id = i;
       }
     }
@@ -189,6 +189,7 @@ 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);
   }
 
   void BeetleController::externalWrenchEstimate()