Encapsulate feedforward

rm_gimbal_controllers/include/rm_gimbal_controllers/feedforward.h

@@ -0,0 +1,54 @@
+//
+// Created by yezi on 24-1-11.
+//
+
+#pragma once
+
+#include <XmlRpc.h>
+#include <geometry_msgs/Vector3.h>
+#include <urdf/model.h>
+#include <rm_common/hardware_interface/robot_state_interface.h>
+
+class FrictionCompensation
+{
+public:
+  void init(XmlRpc::XmlRpcValue config);
+  double output(double vel_act, double effort_cmd) const;
+
+private:
+  double resistance_{ 0. };
+  double velocity_saturation_point_{ 0. }, effort_saturation_point_{ 0. };
+};
+
+class InputFeedforward
+{
+public:
+  void init(XmlRpc::XmlRpcValue config);
+  double output(double vel_desire, double accel_desire) const;
+
+private:
+  double k_v_{ 0. }, k_a_{ 0. };
+};
+
+class GravityCompensation
+{
+public:
+  void init(XmlRpc::XmlRpcValue config);
+  double output(rm_control::RobotStateHandle* robot_state_handle, const urdf::JointConstSharedPtr& joint_urdf,
+                ros::Time time) const;
+
+private:
+  geometry_msgs::Vector3 mass_origin_{};
+  double gravity_{};
+  bool enable_gravity_compensation_ = false;
+};
+
+class BaseVelCompensation
+{
+public:
+  void init(XmlRpc::XmlRpcValue config);
+  double output(double base_angular_vel_z) const;
+
+private:
+  double k_chassis_vel_{ 0. };
+};

rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h

@@ -52,6 +52,8 @@
 #include <Eigen/Eigen>
 #include <rm_common/filters/filters.h>
 
+#include "rm_gimbal_controllers/feedforward.h"
+
 namespace rm_gimbal_controllers
 {
 class ChassisVel
@@ -135,7 +137,6 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
   bool setDesIntoLimit(double& real_des, double current_des, double base2gimbal_current_des,
                        const urdf::JointConstSharedPtr& joint_urdf);
   void moveJoint(const ros::Time& time, const ros::Duration& period);
-  double feedForward(const ros::Time& time);
   void updateChassisVel();
   void commandCB(const rm_msgs::GimbalCmdConstPtr& msg);
   void trackCB(const rm_msgs::TrackDataConstPtr& msg);
@@ -165,20 +166,16 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
   geometry_msgs::TransformStamped odom2gimbal_des_, odom2pitch_, odom2base_, last_odom2base_;
 
   // Gravity Compensation
-  geometry_msgs::Vector3 mass_origin_;
-  double gravity_;
-  bool enable_gravity_compensation_;
+  GravityCompensation gravity_compensation_;
 
   // Input feedforward
-  double yaw_k_v_, yaw_k_a_;
-  double pitch_k_v_, pitch_k_a_;
+  InputFeedforward yaw_input_feedforward_, pitch_input_feedforward_;
 
   // Resistance compensation
-  double yaw_resistance_;
-  double velocity_saturation_point_, effort_saturation_point_;
+  FrictionCompensation yaw_friction_compensation_, pitch_friction_compensation_;
 
   // Chassis
-  double k_chassis_vel_;
+  BaseVelCompensation base_vel_compensation_;
   std::shared_ptr<ChassisVel> chassis_vel_;
 
   enum

rm_gimbal_controllers/src/feedforward.cpp

@@ -0,0 +1,86 @@
+//
+// Created by yezi on 24-1-11.
+//
+
+#include <ros/ros.h>
+#include <tf2_eigen/tf2_eigen.h>
+#include "rm_gimbal_controllers/feedforward.h"
+
+void FrictionCompensation::init(XmlRpc::XmlRpcValue config)
+{
+  ROS_ASSERT(config.getType() == XmlRpc::XmlRpcValue::TypeStruct);
+  resistance_ = config.hasMember("resistance") ? static_cast<double>(config["resistance"]) : 0.;
+  velocity_saturation_point_ =
+      config.hasMember("velocity_saturation_point") ? static_cast<double>(config["velocity_saturation_point"]) : 0.;
+  effort_saturation_point_ =
+      config.hasMember("effort_saturation_point") ? static_cast<double>(config["effort_saturation_point"]) : 0.;
+}
+
+double FrictionCompensation::output(double vel_act, double effort_cmd) const
+{
+  double resistance_compensation = 0.;
+  if (std::abs(vel_act) > velocity_saturation_point_)
+    resistance_compensation = (vel_act > 0 ? 1 : -1) * resistance_;
+  else if (std::abs(effort_cmd) > effort_saturation_point_)
+    resistance_compensation = (effort_cmd > 0 ? 1 : -1) * resistance_;
+  else
+    resistance_compensation = effort_cmd * resistance_ / effort_saturation_point_;
+  return resistance_compensation;
+}
+
+void InputFeedforward::init(XmlRpc::XmlRpcValue config)
+{
+  ROS_ASSERT(config.getType() == XmlRpc::XmlRpcValue::TypeStruct);
+  k_v_ = config.hasMember("k_v") ? static_cast<double>(config["k_v"]) : 0.;
+  k_a_ = config.hasMember("k_a") ? static_cast<double>(config["k_a"]) : 0.;
+}
+
+double InputFeedforward::output(double vel_desire, double accel_desire) const
+{
+  return k_v_ * vel_desire + k_a_ * accel_desire;
+}
+
+void GravityCompensation::init(XmlRpc::XmlRpcValue config)
+{
+  bool enable_feedforward;
+  enable_feedforward = config.hasMember("gravity_compensation");
+  if (enable_feedforward)
+  {
+    ROS_ASSERT(config["gravity_compensation"].hasMember("mass_origin"));
+    ROS_ASSERT(config["gravity_compensation"].hasMember("gravity"));
+    ROS_ASSERT(config["gravity_compensation"].hasMember("enable_gravity_compensation"));
+  }
+  mass_origin_.x = enable_feedforward ? static_cast<double>(config["gravity_compensation"]["mass_origin"][0]) : 0.;
+  mass_origin_.z = enable_feedforward ? static_cast<double>(config["gravity_compensation"]["mass_origin"][2]) : 0.;
+  gravity_ = enable_feedforward ? static_cast<double>(config["gravity_compensation"]["gravity"]) : 0.;
+  enable_gravity_compensation_ =
+      enable_feedforward && static_cast<bool>(config["gravity_compensation"]["enable_gravity_compensation"]);
+}
+
+double GravityCompensation::output(rm_control::RobotStateHandle* robot_state_handle,
+                                   const urdf::JointConstSharedPtr& joint_urdf, ros::Time time) const
+{
+  Eigen::Vector3d gravity(0, 0, -gravity_);
+  tf2::doTransform(gravity, gravity, robot_state_handle->lookupTransform(joint_urdf->child_link_name, "odom", time));
+  Eigen::Vector3d mass_origin(mass_origin_.x, 0, mass_origin_.z);
+  double feedforward = -mass_origin.cross(gravity).y();
+  if (enable_gravity_compensation_)
+  {
+    Eigen::Vector3d gravity_compensation(0, 0, gravity_);
+    tf2::doTransform(gravity_compensation, gravity_compensation,
+                     robot_state_handle->lookupTransform(joint_urdf->child_link_name, joint_urdf->parent_link_name,
+                                                         time));
+    feedforward -= mass_origin.cross(gravity_compensation).y();
+  }
+  return feedforward;
+}
+
+void BaseVelCompensation::init(XmlRpc::XmlRpcValue config)
+{
+  k_chassis_vel_ = config.hasMember("k_chassis_vel") ? static_cast<double>(config["k_chassis_vel"]) : 0.;
+}
+
+double BaseVelCompensation::output(double base_angular_vel_z) const
+{
+  return -k_chassis_vel_ * base_angular_vel_z;
+}

rm_gimbal_controllers/src/gimbal_base.cpp

@@ -49,36 +49,24 @@ namespace rm_gimbal_controllers
 bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh)
 {
   XmlRpc::XmlRpcValue xml_rpc_value;
-  bool enable_feedforward;
-  enable_feedforward = controller_nh.getParam("feedforward", xml_rpc_value);
-  if (enable_feedforward)
-  {
-    ROS_ASSERT(xml_rpc_value.hasMember("mass_origin"));
-    ROS_ASSERT(xml_rpc_value.hasMember("gravity"));
-    ROS_ASSERT(xml_rpc_value.hasMember("enable_gravity_compensation"));
-  }
-  mass_origin_.x = enable_feedforward ? (double)xml_rpc_value["mass_origin"][0] : 0.;
-  mass_origin_.z = enable_feedforward ? (double)xml_rpc_value["mass_origin"][2] : 0.;
-  gravity_ = enable_feedforward ? (double)xml_rpc_value["gravity"] : 0.;
-  enable_gravity_compensation_ = enable_feedforward && (bool)xml_rpc_value["enable_gravity_compensation"];
-
-  ros::NodeHandle resistance_compensation_nh(controller_nh, "yaw/resistance_compensation");
-  yaw_resistance_ = getParam(resistance_compensation_nh, "resistance", 0.);
-  velocity_saturation_point_ = getParam(resistance_compensation_nh, "velocity_saturation_point", 0.);
-  effort_saturation_point_ = getParam(resistance_compensation_nh, "effort_saturation_point", 0.);
+  ros::NodeHandle feedforward_nh(controller_nh, "feedforward");
+  feedforward_nh.getParam("yaw", xml_rpc_value);
+  yaw_friction_compensation_.init(xml_rpc_value);
+  yaw_input_feedforward_.init(xml_rpc_value);
+  base_vel_compensation_.init(xml_rpc_value);
+  feedforward_nh.getParam("pitch", xml_rpc_value);
+  pitch_friction_compensation_.init(xml_rpc_value);
+  pitch_input_feedforward_.init(xml_rpc_value);
+  gravity_compensation_.init(xml_rpc_value);
 
-  k_chassis_vel_ = getParam(controller_nh, "yaw/k_chassis_vel", 0.);
   ros::NodeHandle chassis_vel_nh(controller_nh, "chassis_vel");
   chassis_vel_ = std::make_shared<ChassisVel>(chassis_vel_nh);
+
   ros::NodeHandle nh_bullet_solver = ros::NodeHandle(controller_nh, "bullet_solver");
   bullet_solver_ = std::make_shared<BulletSolver>(nh_bullet_solver);
 
   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.);
-  yaw_k_a_ = getParam(nh_yaw, "k_a", 0.);
-  pitch_k_v_ = getParam(nh_pitch, "k_v", 0.);
-  pitch_k_a_ = getParam(nh_pitch, "k_a", 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))
@@ -379,35 +367,15 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period)
   ctrl_pitch_.setCommand(pitch_des, pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y);
   ctrl_yaw_.update(time, period);
   ctrl_pitch_.update(time, period);
-  double resistance_compensation = 0.;
-  if (std::abs(ctrl_yaw_.joint_.getVelocity()) > velocity_saturation_point_)
-    resistance_compensation = (ctrl_yaw_.joint_.getVelocity() > 0 ? 1 : -1) * yaw_resistance_;
-  else if (std::abs(ctrl_yaw_.joint_.getCommand()) > effort_saturation_point_)
-    resistance_compensation = (ctrl_yaw_.joint_.getCommand() > 0 ? 1 : -1) * yaw_resistance_;
-  else
-    resistance_compensation = ctrl_yaw_.joint_.getCommand() * yaw_resistance_ / effort_saturation_point_;
-  ctrl_yaw_.joint_.setCommand(ctrl_yaw_.joint_.getCommand() - k_chassis_vel_ * chassis_vel_->angular_->z() +
-                              yaw_k_v_ * yaw_vel_des + yaw_k_a_ * yaw_accel_des + resistance_compensation);
-  ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time) + pitch_k_v_ * pitch_vel_des +
-                                pitch_k_a_ * pitch_accel_des);
-}
-
-double Controller::feedForward(const ros::Time& time)
-{
-  Eigen::Vector3d gravity(0, 0, -gravity_);
-  tf2::doTransform(gravity, gravity,
-                   robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->child_link_name, "odom", time));
-  Eigen::Vector3d mass_origin(mass_origin_.x, 0, mass_origin_.z);
-  double feedforward = -mass_origin.cross(gravity).y();
-  if (enable_gravity_compensation_)
-  {
-    Eigen::Vector3d gravity_compensation(0, 0, gravity_);
-    tf2::doTransform(gravity_compensation, gravity_compensation,
-                     robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->child_link_name,
-                                                         ctrl_pitch_.joint_urdf_->parent_link_name, time));
-    feedforward -= mass_origin.cross(gravity_compensation).y();
-  }
-  return feedforward;
+  ctrl_yaw_.joint_.setCommand(
+      ctrl_yaw_.joint_.getCommand() + base_vel_compensation_.output(chassis_vel_->angular_->z()) +
+      yaw_input_feedforward_.output(yaw_vel_des, yaw_accel_des) +
+      yaw_friction_compensation_.output(ctrl_yaw_.joint_.getVelocity(), ctrl_yaw_.joint_.getCommand()));
+  ctrl_pitch_.joint_.setCommand(
+      ctrl_pitch_.joint_.getCommand() +
+      gravity_compensation_.output(&robot_state_handle_, ctrl_pitch_.joint_urdf_, time) +
+      pitch_input_feedforward_.output(pitch_vel_des, pitch_accel_des) +
+      pitch_friction_compensation_.output(ctrl_pitch_.joint_.getVelocity(), ctrl_pitch_.joint_.getCommand()));
 }
 
 void Controller::updateChassisVel()