Update powerlimit for omni and swerve

rm_chassis_controllers/CMakeLists.txt

@@ -28,7 +28,6 @@ find_package(Eigen3 REQUIRED)
 
 generate_dynamic_reconfigure_options(
   cfg/LQRWeight.cfg
-  cfg/PowerLimit.cfg
 )
 
 ###################################

rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h

@@ -48,7 +48,6 @@
 #include <geometry_msgs/Vector3Stamped.h>
 #include <nav_msgs/Odometry.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <rm_chassis_controllers/PowerLimitConfig.h>
 #include <rm_common/ros_utilities.h>
 #include <rm_common/math_utilities.h>
 #include <rm_common/ori_tool.h>
@@ -178,7 +177,6 @@ class ChassisBase : public controller_interface::MultiInterfaceController<T...>
   void initialize_parameters(ros::NodeHandle& controller_nh);
   void slamCallback(const nav_msgs::Odometry::ConstPtr& msg);
   void localizationCallback(const geometry_msgs::TransformStamped::ConstPtr& msg);
-  void capacityCallback(const rm_msgs::PowerManagementSampleAndStatusData::ConstPtr& msg);
 
   rm_control::RobotStateHandle robot_state_handle_{};
   hardware_interface::EffortJointInterface* effort_joint_interface_{};
@@ -187,9 +185,8 @@ class ChassisBase : public controller_interface::MultiInterfaceController<T...>
   realtime_tools::RealtimeBuffer<nav_msgs::Odometry> slam_rt_buffer_{};
   realtime_tools::RealtimeBuffer<geometry_msgs::TransformStamped> localization_rt_buffer_{};
   std::unique_ptr<realtime_tools::RealtimePublisher<nav_msgs::Odometry>> odom_pub_;
-  std::unique_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64>> cpower_pub_;  // command power publisher
-  std::unique_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64>> epower_pub_;  // estimated power publisher
-  std::unique_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64>> chassis_power_pub_;  // chassis power publisher
+  std::unique_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64>> cmd_power_pub_;
+  std::unique_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64>> estimated_power_pub_;
 
   rm_common::TfRtBroadcaster brcst4global_map2robot_odom_{};
   rm_common::TfRtBroadcaster brcst4global_map2camera_init_{};
@@ -210,7 +207,6 @@ class ChassisBase : public controller_interface::MultiInterfaceController<T...>
   ros::Subscriber cmd_chassis_sub_;
   ros::Subscriber slam_sub_;
   ros::Subscriber localization_sub_;
-  ros::Subscriber capacity_sub_;
 
   std::unique_ptr<RampFilter<double>> ramp_x_{ nullptr };
   std::unique_ptr<RampFilter<double>> ramp_y_{ nullptr };
@@ -228,9 +224,6 @@ class ChassisBase : public controller_interface::MultiInterfaceController<T...>
   double twist_angular_{ M_PI / 6 };
   double max_odom_vel_{ 10.0 };
   double timeout_{ 0.1 };
-  double chassis_power_{ 0.0 };
-  bool capacity_update_flag_{ false };
-  bool use_rls_{ false };
   bool use_K_angle_{ false };
 
   bool odom_initialized_{ false };
@@ -248,8 +241,6 @@ class ChassisBase : public controller_interface::MultiInterfaceController<T...>
   std::string slam_topic_{ "/slam/odometry" };
   std::string localization_topic_{ "/shinji/result" };
 
-  std::string capacity_topic_{ "/rm_referee/power_management/sample_and_status" };
-
   ros::Time last_publish_time_{};
   geometry_msgs::Vector3 vel_cmd_{};  // x, y
   std::array<double, 36> twist_covariance_{};

rm_chassis_controllers/include/rm_chassis_controllers/omni.h

@@ -5,6 +5,7 @@
 #pragma once
 
 #include <Eigen/Dense>
+#include <rm_common/filters/lp_filter.h>
 
 #include "rm_chassis_controllers/chassis_base.h"
 
@@ -16,14 +17,19 @@ class OmniController : public ChassisBase<rm_control::RobotStateInterface, hardw
   OmniController() = default;
   bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override;
 
+protected:
+  void moveJoint(const ros::Time& time, const ros::Duration& period) override;
+
 private:
   geometry_msgs::Twist odometry() override;
 
   std::vector<std::shared_ptr<effort_controllers::JointVelocityController>> joints_;
   Eigen::MatrixXd chassis2joints_;
+  void powerLimit() override;
+  void updatePowerStatus() override;
+  virtual void stateJudge();
 
-protected:
-  void moveJoint(const ros::Time& time, const ros::Duration& period) override;
+  std::array<LowPassFilter*, 4> motor_lp_filters_{};
 };
 
 }  // namespace rm_chassis_controllers

rm_chassis_controllers/include/rm_chassis_controllers/swerve.h

@@ -41,6 +41,7 @@
 
 #include <rm_common/eigen_types.h>
 #include <effort_controllers/joint_position_controller.h>
+#include <rm_common/filters/lp_filter.h>
 
 namespace rm_chassis_controllers
 {
@@ -61,7 +62,17 @@ class SwerveController : public ChassisBase<rm_control::RobotStateInterface, har
 private:
   void moveJoint(const ros::Time& time, const ros::Duration& period) override;
   geometry_msgs::Twist odometry() override;
-  std::vector<Module> modules_;
+  void powerLimit() override;
+  void updatePowerStatus() override;
+  void getBaseGyro();
+  void stateJudge();
+
+  std::vector<Module> modules_{};
+  std::vector<hardware_interface::JointHandle> pivot_joint_handles_{};
+  std::unique_ptr<realtime_tools::RealtimePublisher<geometry_msgs::Vector3Stamped>> base_gyro_pub_;
+
+  PowerLimitor pivot_power_limitor_{};
+  std::array<std::array<LowPassFilter*, 4>, 2> motor_lp_filters_{};
 };
 
 }  // namespace rm_chassis_controllers

rm_chassis_controllers/src/chassis_base.cpp

@@ -105,13 +105,6 @@ bool ChassisBase<T...>::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan
   slam_sub_ = controller_nh.subscribe<nav_msgs::Odometry>(slam_topic_, 10, &ChassisBase::slamCallback, this);
   localization_sub_ = controller_nh.subscribe<geometry_msgs::TransformStamped>(
       localization_topic_, 10, &ChassisBase::localizationCallback, this);
-  capacity_sub_ = root_nh.subscribe<rm_msgs::PowerManagementSampleAndStatusData>(capacity_topic_, 10,
-                                                                                 &ChassisBase::capacityCallback, this);
-
-  // Setup real_power from capacity publishers.
-  auto chassis_power_publisher =
-      std::make_unique<realtime_tools::RealtimePublisher<std_msgs::Float64>>(controller_nh, "power/chassis_power", 100);
-  this->chassis_power_pub_ = std::move(chassis_power_publisher);
 
   ramp_x_ = std::make_unique<RampFilter<double>>(0, 0.001);
   ramp_y_ = std::make_unique<RampFilter<double>>(0, 0.001);
@@ -515,16 +508,4 @@ void ChassisBase<T...>::localizationCallback(const geometry_msgs::TransformStamp
   localization_updated_ = true;
 }
 
-template <typename... T>
-void ChassisBase<T...>::capacityCallback(const rm_msgs::PowerManagementSampleAndStatusData::ConstPtr& msg)
-{
-  chassis_power_ = msg->chassis_power + msg->capacity_discharge_power;
-  capacity_update_flag_ = true;
-  if (chassis_power_pub_->trylock())
-  {
-    chassis_power_pub_->msg_.data = chassis_power_;
-    chassis_power_pub_->unlockAndPublish();
-  }
-}
-
 }  // namespace rm_chassis_controllers

rm_chassis_controllers/src/omni.cpp

@@ -2,6 +2,8 @@
 // Created by qiayuan on 2022/7/29.
 //
 
+#include <algorithm>
+#include <cmath>
 #include <string>
 #include <Eigen/QR>
 
@@ -17,6 +19,34 @@ bool OmniController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle
 {
   ChassisBase::init(robot_hw, root_nh, controller_nh);
 
+  try
+  {
+    controller_nh.getParam("power/vel_coeff", wheel_power_limitor_.vel_coeff);
+    controller_nh.getParam("power/effort_coeff", wheel_power_limitor_.effort_coeff);
+    controller_nh.getParam("power/power_offset", wheel_power_limitor_.power_offset);
+    controller_nh.param<bool>("power/use_K_angle", use_K_angle_, false);
+  }
+  catch (const std::exception& e)
+  {
+    ROS_ERROR("Failed to get power limiter parameters: %s", e.what());
+    return false;
+  }
+
+  wheel_power_limitor_.err_upper = 500;
+  wheel_power_limitor_.err_lower = 0.01;
+
+  for (auto& filter : motor_lp_filters_)
+  {
+    filter = new LowPassFilter(20);
+  }
+
+  auto estimated_power_publisher =
+      std::make_unique<realtime_tools::RealtimePublisher<std_msgs::Float64>>(controller_nh, "power/estimated", 100);
+  this->estimated_power_pub_ = std::move(estimated_power_publisher);
+  auto cmd_power_publisher =
+      std::make_unique<realtime_tools::RealtimePublisher<std_msgs::Float64>>(controller_nh, "power/commanded", 100);
+  this->cmd_power_pub_ = std::move(cmd_power_publisher);
+
   XmlRpc::XmlRpcValue wheels;
   controller_nh.getParam("wheels", wheels);
   chassis2joints_.resize(wheels.size(), 3);
@@ -77,5 +107,167 @@ geometry_msgs::Twist OmniController::odometry()
   return twist;
 }
 
+void OmniController::stateJudge()
+{
+  if (!use_K_angle_)
+  {
+    for (size_t i = 0; i < joints_.size() && i < 4; ++i)
+    {
+      wheel_power_limitor_.K_angle[i] = 1.0;
+    }
+    return;
+  }
+  double sin_pitch{};
+  if (abs(pitch_) > 0.12)
+  {
+    sin_pitch = sin(pitch_);
+  }
+  else
+  {
+    sin_pitch = 0.0;
+  }
+
+  for (size_t i = 0; i < joints_.size() && i < 4; ++i)
+  {
+    auto& ctl = joints_[i];
+
+    if (ctl->joint_.getName().find("front") != std::string::npos)
+    {
+      if (ctl->getJointName().find("left") != std::string::npos)
+      {
+        wheel_power_limitor_.K_angle[i] = 1 + sin_pitch;
+      }
+      if (ctl->getJointName().find("right") != std::string::npos)
+      {
+        wheel_power_limitor_.K_angle[i] = 1 + sin_pitch;
+      }
+    }
+    if (ctl->joint_.getName().find("back") != std::string::npos)
+    {
+      if (ctl->getJointName().find("left") != std::string::npos)
+      {
+        wheel_power_limitor_.K_angle[i] = 1 - sin_pitch;
+      }
+      if (ctl->getJointName().find("right") != std::string::npos)
+      {
+        wheel_power_limitor_.K_angle[i] = 1 - sin_pitch;
+      }
+    }
+  }
+}
+
+void OmniController::powerLimit()
+{
+  updatePowerStatus();
+  stateJudge();
+
+  if (wheel_power_limitor_.err_sum > wheel_power_limitor_.err_upper)
+  {
+    wheel_power_limitor_.K = 1;
+  }
+  else if (wheel_power_limitor_.err_sum < wheel_power_limitor_.err_lower)
+  {
+    wheel_power_limitor_.K = 0;
+  }
+  else
+  {
+    wheel_power_limitor_.K = 1 - (wheel_power_limitor_.err_sum - wheel_power_limitor_.err_lower) /
+                                     (wheel_power_limitor_.err_upper - wheel_power_limitor_.err_lower);
+  }
+
+  for (int i = 0; i < 4; i++)
+  {
+    double wheel_zoom =
+        (wheel_power_limitor_.K * abs(wheel_power_limitor_.err[i]) / wheel_power_limitor_.err_sum) +
+        (1 - wheel_power_limitor_.K) * (abs(wheel_power_limitor_.power_in[i]) / wheel_power_limitor_.cmd_power);
+    wheel_zoom = limit(wheel_zoom, 0.0, 1.0);
+    wheel_power_limitor_.power_limit[i] = wheel_zoom * wheel_power_limitor_.max_power;
+  }
+  if (wheel_power_limitor_.cmd_power > wheel_power_limitor_.max_power)
+  {
+    for (size_t i = 0; i < joints_.size() && i < 4; ++i)
+    {
+      auto& ctl = joints_[i];
+      auto& joint = ctl->joint_;
+      double a = wheel_power_limitor_.effort_coeff;
+      double b = wheel_power_limitor_.omiga[i];
+      double c = abs(wheel_power_limitor_.omiga[i]) * wheel_power_limitor_.vel_coeff +
+                 wheel_power_limitor_.power_offset / 4 - wheel_power_limitor_.power_limit[i];
+      double delta = square(b) - 4 * a * c;
+      if (!std::isfinite(delta) || delta < 0.0)
+        delta = 0.0;
+      if (delta >= 0)
+      {
+        double sqrt = sqrtf(delta);
+        if (wheel_power_limitor_.torque[i] >= 0)
+          joint.setCommand(((-b + sqrt) / (2 * a)) * wheel_power_limitor_.K_angle[i]);
+        else
+          joint.setCommand(((-b - sqrt) / (2 * a)) * wheel_power_limitor_.K_angle[i]);
+      }
+      else
+      {
+        joint.setCommand(((-b) / (2 * a)) * wheel_power_limitor_.K_angle[i]);
+      }
+    }
+  }
+  else
+  {
+    for (size_t i = 0; i < joints_.size() && i < 4; ++i)
+    {
+      auto& ctl = joints_[i];
+      auto& joint = ctl->joint_;
+      joint.setCommand(wheel_power_limitor_.torque[i] * wheel_power_limitor_.K_angle[i]);
+    }
+  }
+}
+
+void OmniController::updatePowerStatus()
+{
+  double power_limit = cmd_rt_buffer_.readFromRT()->cmd_chassis_.power_limit;
+
+  wheel_power_limitor_.max_power = power_limit;
+  wheel_power_limitor_.err_sum = 0;
+
+  double estimated_wheel_power{}, cmd_wheel_power{};
+  for (size_t i = 0; i < joints_.size() && i < 4; ++i)
+  {
+    auto& ctl = joints_[i];
+
+    double cmd_torque = wheel_power_limitor_.torque[i] = ctl->joint_.getCommand();
+    double cmd_vel{};
+    ctl->getCommand(cmd_vel);
+    double real_vel = wheel_power_limitor_.omiga[i] = ctl->joint_.getVelocity();
+    motor_lp_filters_[i]->input(ctl->joint_.getEffort());
+    double real_torque = motor_lp_filters_[i]->output();
+
+    wheel_power_limitor_.err[i] = cmd_vel - real_vel;
+    wheel_power_limitor_.err_sum += abs(wheel_power_limitor_.err[i]);
+
+    estimated_wheel_power += real_torque * real_vel + wheel_power_limitor_.effort_coeff * square(real_torque) +
+                             wheel_power_limitor_.vel_coeff * abs(real_vel);
+    cmd_wheel_power += cmd_torque * real_vel + wheel_power_limitor_.effort_coeff * square(cmd_torque) +
+                       wheel_power_limitor_.vel_coeff * abs(real_vel);
+
+    wheel_power_limitor_.power_in[i] = cmd_torque * real_vel + wheel_power_limitor_.effort_coeff * square(cmd_torque) +
+                                       wheel_power_limitor_.vel_coeff * abs(real_vel);
+  }
+  wheel_power_limitor_.cmd_power = cmd_wheel_power + wheel_power_limitor_.power_offset;
+  wheel_power_limitor_.estimated_power = estimated_wheel_power + wheel_power_limitor_.power_offset;
+
+  double estimated_total_power = wheel_power_limitor_.estimated_power;
+  double cmd_total_power = wheel_power_limitor_.cmd_power;
+
+  auto publishPower = [](auto& pub, const double power) {
+    if (pub && pub->trylock())
+    {
+      pub->msg_.data = power;
+      pub->unlockAndPublish();
+    }
+  };
+
+  publishPower(estimated_power_pub_, estimated_total_power);
+  publishPower(cmd_power_pub_, cmd_total_power);
+}
+
 }  // namespace rm_chassis_controllers
 PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::OmniController, controller_interface::ControllerBase)