Update chassis compensation.

rm-controls · Nov 17, 2024 · 7be74d4 · 7be74d4
1 parent 1a0a23d
commit 7be74d4
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 6 deletions.
diff --git a/rm_gimbal_controllers/cfg/GimbalBase.cfg b/rm_gimbal_controllers/cfg/GimbalBase.cfg
@@ -9,6 +9,8 @@ gen.add("yaw_k_v_", double_t, 0, "Yaw input feedforward scale", 0.0, 0, 1.0)
 gen.add("pitch_k_v_", double_t, 0, "Pitch input feedforward scale", 0.0, 0, 1.0)
 gen.add("accel_yaw_", double_t, 0, "Acceleration of rate yaw", 0.0, 0, 999.0)
 gen.add("accel_pitch_", double_t, 0, "Acceleration of rate pitch", 0.0, 0, 999.0)
-gen.add("k_chassis_vel_", double_t, 0, "Yaw chassis velocity feedforward scale", 0.0, 0, 1.0)
+gen.add("chassis_comp_a_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0)
+gen.add("chassis_comp_b_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0)
+gen.add("chassis_comp_c_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0)
 
 exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase"))
diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h
@@ -60,7 +60,7 @@ namespace rm_gimbal_controllers
 {
 struct GimbalConfig
 {
-  double yaw_k_v_, pitch_k_v_, k_chassis_vel_;
+  double yaw_k_v_, pitch_k_v_, chassis_comp_a_, chassis_comp_b_, chassis_comp_c_;
   double accel_pitch_{}, accel_yaw_{};
 };
 
@@ -148,6 +148,7 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
   void moveJoint(const ros::Time& time, const ros::Duration& period);
   double feedForward(const ros::Time& time);
   void updateChassisVel();
+  double updateCompensation(double chassis_vel_angular_z);
   void commandCB(const rm_msgs::GimbalCmdConstPtr& msg);
   void trackCB(const rm_msgs::TrackDataConstPtr& msg);
   void reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t);
@@ -188,6 +189,7 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
 
   // Chassis
   std::shared_ptr<ChassisVel> chassis_vel_;
+  double chassis_compensation_;
 
   bool dynamic_reconfig_initialized_{};
   GimbalConfig config_{};

diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp
@@ -74,7 +74,9 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro
 
   config_ = { .yaw_k_v_ = getParam(nh_yaw, "k_v", 0.),
               .pitch_k_v_ = getParam(nh_pitch, "k_v", 0.),
-              .k_chassis_vel_ = getParam(controller_nh, "yaw/k_chassis_vel", 0.),
+              .chassis_comp_a_ = getParam(controller_nh,"yaw/chassis_comp_a",0.),
+              .chassis_comp_b_ = getParam(controller_nh,"yaw/chassis_comp_b",0.),
+              .chassis_comp_c_ = getParam(controller_nh,"yaw/chassis_comp_c",0.),
               .accel_pitch_ = getParam(controller_nh, "pitch/accel", 99.),
               .accel_yaw_ = getParam(controller_nh, "yaw/accel", 99.) };
   config_rt_buffer_.initRT(config_);
@@ -500,7 +502,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period)
   }
   loop_count_++;
 
-  ctrl_yaw_.setCommand(pid_yaw_pos_.getCurrentCmd() - config_.k_chassis_vel_ * chassis_vel_->angular_->z() +
+  ctrl_yaw_.setCommand(pid_yaw_pos_.getCurrentCmd() - updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() +
                        config_.yaw_k_v_ * yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z);
   ctrl_pitch_.setCommand(pid_pitch_pos_.getCurrentCmd() + config_.pitch_k_v_ * pitch_vel_des +
                          ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y);
@@ -548,6 +550,12 @@ void Controller::updateChassisVel()
   last_odom2base_ = odom2base_;
 }
 
+double Controller::updateCompensation(double chassis_vel_angular_z)
+{
+  chassis_compensation_ = config_.chassis_comp_a_ * pow(chassis_vel_angular_z,2) + config_.chassis_comp_b_ * chassis_vel_angular_z + config_.chassis_comp_c_;
+  return chassis_compensation_;
+}
+
 void Controller::commandCB(const rm_msgs::GimbalCmdConstPtr& msg)
 {
   cmd_rt_buffer_.writeFromNonRT(*msg);
@@ -568,14 +576,18 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin
     GimbalConfig init_config = *config_rt_buffer_.readFromNonRT();  // config init use yaml
     config.yaw_k_v_ = init_config.yaw_k_v_;
     config.pitch_k_v_ = init_config.pitch_k_v_;
-    config.k_chassis_vel_ = init_config.k_chassis_vel_;
+    config.chassis_comp_a_ = init_config.chassis_comp_a_;
+    config.chassis_comp_b_ = init_config.chassis_comp_b_;
+    config.chassis_comp_c_ = init_config.chassis_comp_c_;
     config.accel_pitch_ = init_config.accel_pitch_;
     config.accel_yaw_ = init_config.accel_yaw_;
     dynamic_reconfig_initialized_ = true;
   }
   GimbalConfig config_non_rt{ .yaw_k_v_ = config.yaw_k_v_,
                               .pitch_k_v_ = config.pitch_k_v_,
-                              .k_chassis_vel_ = config.k_chassis_vel_,
+                              .chassis_comp_a_ = config.chassis_comp_a_,
+                              .chassis_comp_b_ = config.chassis_comp_b_,
+                              .chassis_comp_c_ = config.chassis_comp_c_,
                               .accel_pitch_ = config.accel_pitch_,
                               .accel_yaw_ = config.accel_yaw_ };
   config_rt_buffer_.writeFromNonRT(config_non_rt);