From ba0936b25dbb4b34aa104d0185639eea1e5e89e4 Mon Sep 17 00:00:00 2001 From: 321aurora <2508260166@qq.com> Date: Sun, 17 Nov 2024 10:40:52 +0800 Subject: [PATCH 1/3] Update chassis compensation. --- rm_gimbal_controllers/cfg/GimbalBase.cfg | 4 +++- .../rm_gimbal_controllers/gimbal_base.h | 4 +++- rm_gimbal_controllers/src/gimbal_base.cpp | 22 ++++++++++++++----- 3 files changed, 23 insertions(+), 7 deletions(-) diff --git a/rm_gimbal_controllers/cfg/GimbalBase.cfg b/rm_gimbal_controllers/cfg/GimbalBase.cfg index debe59ca..9630d6d2 100755 --- 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("a_compensation",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0) +gen.add("b_compensation",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0) +gen.add("c_compensation",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 index c5a07105..bc8d29dd 100644 --- 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_, a_compensation, b_compensation, c_compensation; double accel_pitch_{}, accel_yaw_{}; }; @@ -148,6 +148,8 @@ class Controller : public controller_interface::MultiInterfaceControllerangular_->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_.a_compensation * pow(chassis_vel_angular_z,2) + config_.b_compensation * chassis_vel_angular_z + config_.c_compensation; + 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.a_compensation = init_config.a_compensation; + config.b_compensation = init_config.b_compensation; + config.c_compensation = init_config.c_compensation; 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_, + .a_compensation = config.a_compensation, + .b_compensation = config.b_compensation, + .c_compensation = config.c_compensation, .accel_pitch_ = config.accel_pitch_, .accel_yaw_ = config.accel_yaw_ }; config_rt_buffer_.writeFromNonRT(config_non_rt); From ed36c6f42d23848115ae7c8b5ad1b83af2d74fd7 Mon Sep 17 00:00:00 2001 From: 321aurora <2508260166@qq.com> Date: Sun, 17 Nov 2024 15:37:51 +0800 Subject: [PATCH 2/3] Fix some wrong. --- rm_gimbal_controllers/src/gimbal_base.cpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index e3695c97..9f399963 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -502,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); @@ -550,6 +550,12 @@ void Controller::updateChassisVel() last_odom2base_ = odom2base_; } +double Controller::updateCompensation(double chassis_vel_angular_z) +{ + chassis_compensation = config_.a_compensation * pow(chassis_vel_angular_z,2) + config_.b_compensation * chassis_vel_angular_z + config_.c_compensation; + return chassis_compensation; +} + void Controller::commandCB(const rm_msgs::GimbalCmdConstPtr& msg) { cmd_rt_buffer_.writeFromNonRT(*msg); @@ -570,14 +576,16 @@ 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.accel_pitch_ = init_config.accel_pitch_; - config.accel_yaw_ = init_config.accel_yaw_; + config.a_compensation = init_config.a_compensation; + config.b_compensation = init_config.b_compensation; + config.c_compensation = init_config.c_compensation; 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_, + .a_compensation = config.a_compensation, + .b_compensation = config.b_compensation, + .c_compensation = config.c_compensation, .accel_pitch_ = config.accel_pitch_, .accel_yaw_ = config.accel_yaw_ }; config_rt_buffer_.writeFromNonRT(config_non_rt); From 16a76c3b9bfa0d18f4b0576a96f5a3e5aff115a8 Mon Sep 17 00:00:00 2001 From: 321aurora <2508260166@qq.com> Date: Sun, 17 Nov 2024 15:38:47 +0800 Subject: [PATCH 3/3] Fix some wrong. --- rm_gimbal_controllers/src/gimbal_base.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 9f399963..8bbe3989 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -579,6 +579,8 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin config.a_compensation = init_config.a_compensation; config.b_compensation = init_config.b_compensation; config.c_compensation = init_config.c_compensation; + 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_,