From 33259009443e89a093fa2215f1cf6d9845c6c23e Mon Sep 17 00:00:00 2001
From: yezi <1536117624@qq.com>
Date: Sun, 18 Jun 2023 20:45:11 +0800
Subject: [PATCH] Add input feedforward and fix a bug in computing desire vel
 at TRACK mode.

---
 .../include/rm_gimbal_controllers/gimbal_base.h        |  4 ++++
 rm_gimbal_controllers/src/gimbal_base.cpp              | 10 ++++++----
 2 files changed, 10 insertions(+), 4 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 a0bbab9d..e09d968f 100644
--- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h
+++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h
@@ -169,6 +169,10 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
   double gravity_;
   bool enable_gravity_compensation_;
 
+  // Input feedforward
+  double yaw_k_v_;
+  double pitch_k_v_;
+
   // Resistance compensation
   double yaw_resistance_;
   double velocity_dead_zone_, effort_dead_zone_;
diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp
index 23ab77b6..d1f35c58 100644
--- a/rm_gimbal_controllers/src/gimbal_base.cpp
+++ b/rm_gimbal_controllers/src/gimbal_base.cpp
@@ -75,6 +75,8 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro
 
   ros::NodeHandle nh_yaw = ros::NodeHandle(controller_nh, "yaw");
   ros::NodeHandle nh_pitch = ros::NodeHandle(controller_nh, "pitch");
+  yaw_k_v_ = getParam(nh_yaw, "k_v", 0.);
+  pitch_k_v_ = getParam(nh_pitch, "k_v", 0.);
   hardware_interface::EffortJointInterface* effort_joint_interface =
       robot_hw->get<hardware_interface::EffortJointInterface>();
   if (!ctrl_yaw_.init(effort_joint_interface, nh_yaw) || !ctrl_pitch_.init(effort_joint_interface, nh_pitch))
@@ -375,14 +377,14 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period)
       tf2::fromMsg(target_pos, target_pos_tf);
       tf2::fromMsg(target_vel, target_vel_tf);
 
-      yaw_vel_des = target_vel_tf.cross(target_pos_tf).z() / std::pow((target_pos_tf.length()), 2);
+      yaw_vel_des = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2);
       transform = robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->parent_link_name,
                                                       data_track_.header.frame_id, data_track_.header.stamp);
       tf2::doTransform(target_pos, target_pos, transform);
       tf2::doTransform(target_vel, target_vel, transform);
       tf2::fromMsg(target_pos, target_pos_tf);
       tf2::fromMsg(target_vel, target_vel_tf);
-      pitch_vel_des = target_vel_tf.cross(target_pos_tf).y() / std::pow((target_pos_tf.length()), 2);
+      pitch_vel_des = target_pos_tf.cross(target_vel_tf).y() / std::pow((target_pos_tf.length()), 2);
     }
     catch (tf2::TransformException& ex)
     {
@@ -400,8 +402,8 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period)
   else if (std::abs(ctrl_yaw_.joint_.getCommand()) > effort_dead_zone_)
     resistance_compensation = (ctrl_yaw_.joint_.getCommand() > 0 ? 1 : -1) * yaw_resistance_;
   ctrl_yaw_.joint_.setCommand(ctrl_yaw_.joint_.getCommand() - k_chassis_vel_ * chassis_vel_->angular_->z() +
-                              resistance_compensation);
-  ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time));
+                              yaw_k_v_ * yaw_vel_des + resistance_compensation);
+  ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time) + pitch_k_v_ * pitch_vel_des);
 }
 
 double Controller::feedForward(const ros::Time& time)