From 30ef9a7ebff53f1b9f19cf06e89d4d22a42fe14b Mon Sep 17 00:00:00 2001
From: 321aurora <2508260166@qq.com>
Date: Sat, 5 Jul 2025 09:10:42 +0800
Subject: [PATCH 1/2] Change the gimbal speed-des to position difference.

---
 rm_gimbal_controllers/src/gimbal_base.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp
index e98d628b..01050f6f 100644
--- a/rm_gimbal_controllers/src/gimbal_base.cpp
+++ b/rm_gimbal_controllers/src/gimbal_base.cpp
@@ -420,7 +420,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period)
     if (ctrls_.find(2) != ctrls_.end())
       angular_vel.z = ctrls_.at(2)->joint_.getVelocity();
   }
-  double pos_real[3]{ 0. }, pos_des[3]{ 0. }, vel_des[3]{ 0. }, angle_error[3]{ 0. };
+  double pos_real[3]{ 0. }, pos_des[3]{ 0. }, last_pos_des[3]{ 0. }, vel_des[3]{ 0. }, angle_error[3]{ 0. };
   quatToRPY(odom2gimbal_des_.transform.rotation, pos_des[0], pos_des[1], pos_des[2]);
   quatToRPY(odom2gimbal_.transform.rotation, pos_real[0], pos_real[1], pos_real[2]);
   for (int i = 0; i < 3; i++)
@@ -452,7 +452,8 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period)
         tf2::doTransform(target_vel, target_vel, transform);
         tf2::fromMsg(target_pos, target_pos_tf);
         tf2::fromMsg(target_vel, target_vel_tf);
-        vel_des[2] = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2);
+        vel_des[2] = angles::shortest_angular_distance(last_pos_des[2], pos_des[2]) / period.toSec();
+        last_pos_des[2] = pos_des[2];
       }
       if (joint_urdfs_.find(1) != joint_urdfs_.end())
       {

From 398dea27619be7bd98b77d5dfc64a01075c8e715 Mon Sep 17 00:00:00 2001
From: 321aurora <2508260166@qq.com>
Date: Sat, 5 Jul 2025 16:44:29 +0800
Subject: [PATCH 2/2] Fix bug of last-pos-des update.

---
 .../include/rm_gimbal_controllers/gimbal_base.h             | 1 +
 rm_gimbal_controllers/src/gimbal_base.cpp                   | 6 +++---
 2 files changed, 4 insertions(+), 3 deletions(-)

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 71b5b7c5..bbb862e5 100644
--- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h
+++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h
@@ -181,6 +181,7 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
   double publish_rate_{};
   bool state_changed_{};
   int loop_count_{};
+  double last_pos_des_[3]{ 0. };
 
   // Transform
   geometry_msgs::TransformStamped odom2gimbal_des_, odom2gimbal_, odom2base_, last_odom2base_;
diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp
index 01050f6f..1eaed2e3 100644
--- a/rm_gimbal_controllers/src/gimbal_base.cpp
+++ b/rm_gimbal_controllers/src/gimbal_base.cpp
@@ -420,7 +420,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period)
     if (ctrls_.find(2) != ctrls_.end())
       angular_vel.z = ctrls_.at(2)->joint_.getVelocity();
   }
-  double pos_real[3]{ 0. }, pos_des[3]{ 0. }, last_pos_des[3]{ 0. }, vel_des[3]{ 0. }, angle_error[3]{ 0. };
+  double pos_real[3]{ 0. }, pos_des[3]{ 0. }, vel_des[3]{ 0. }, angle_error[3]{ 0. };
   quatToRPY(odom2gimbal_des_.transform.rotation, pos_des[0], pos_des[1], pos_des[2]);
   quatToRPY(odom2gimbal_.transform.rotation, pos_real[0], pos_real[1], pos_real[2]);
   for (int i = 0; i < 3; i++)
@@ -452,8 +452,8 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period)
         tf2::doTransform(target_vel, target_vel, transform);
         tf2::fromMsg(target_pos, target_pos_tf);
         tf2::fromMsg(target_vel, target_vel_tf);
-        vel_des[2] = angles::shortest_angular_distance(last_pos_des[2], pos_des[2]) / period.toSec();
-        last_pos_des[2] = pos_des[2];
+        vel_des[2] = angles::shortest_angular_distance(last_pos_des_[2], pos_des[2]) / period.toSec();
+        last_pos_des_[2] = pos_des[2];
       }
       if (joint_urdfs_.find(1) != joint_urdfs_.end())
       {