Update controllers from RM26 hole standard.

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/README.md

@@ -119,6 +119,10 @@ sudo rosdep install --from-paths src
 
   Allowed period (in s) between two commands. If the time is exceed this period, the speed of chassis will be set 0.
 
+* **`raw_yaw_feedforward_k`** (double, default: 0.0)
+
+  Yaw feedforward time constant in seconds. Only used in `raw` mode, where the command-frame rotation uses `yaw + k * w` to compensate direction-change lag.
+
 * **`power_offset`** (double)
 
   Fix the difference between theoretical power and actual power.
@@ -245,6 +249,7 @@ sudo rosdep install --from-paths src
       power_offset: -8.41
     twist_angular: 0.5233
     timeout: 0.1
+    raw_yaw_feedforward_k: 0.0
     pid_follow: { p: 5.0, i: 0, d: 0.3, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true }
     twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ]
 

rm_chassis_controllers/cfg/LQRWeight.cfg

@@ -5,11 +5,11 @@ from dynamic_reconfigure.parameter_generator_catkin import *
 
 gen = ParameterGenerator()
 
-gen.add("Q_theta", double_t, 0, "theta weight",     1.0,  0.0, 10000.0)
+gen.add("Q_theta", double_t, 0, "theta weight",     1.0,  0.0, 200000.0)
 gen.add("Q_d_theta", double_t, 0, "d_theta weight",   1.0,  0.0, 10000.0)
-gen.add("Q_x", double_t, 0, "x weight",         1.0,  0.0, 10000.0)
+gen.add("Q_x", double_t, 0, "x weight",         1.0,  0.0, 100000.0)
 gen.add("Q_dx", double_t, 0, "dx weight",        1.0,  0.0, 10000.0)
-gen.add("Q_phi", double_t, 0, "phi weight",       1.0,  0.0, 10000.0)
+gen.add("Q_phi", double_t, 0, "phi weight",       1.0,  0.0, 200000.0)
 gen.add("Q_d_phi", double_t, 0, "d_phi weight",     1.0,  0.0, 10000.0)
 
 gen.add("R_T", double_t, 0, "wheel torque weight",  1.0, 0.001, 10000.0)

rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h

@@ -13,6 +13,7 @@
 #include <rm_msgs/LeggedUpstairStatus.h>
 #include <rm_common/filters/kalman_filter.h>
 #include <rm_common/filters/lp_filter.h>
+#include <rm_common/DebugDataPublisher.h>
 #include <control_toolbox/pid.h>
 #include <controller_interface/multi_interface_controller.h>
 #include <geometry_msgs/TwistStamped.h>
@@ -29,6 +30,7 @@
 #include "bipedal_wheel_controller/definitions.h"
 #include "bipedal_wheel_controller/controller_mode/mode_manager.h"
 #include "bipedal_wheel_controller/vmc/VMC.h"
+#include "bipedal_wheel_controller/controller_interface.h"
 
 namespace rm_chassis_controllers
 {
@@ -41,51 +43,62 @@ struct LQRConfig
 };
 
 class BipedalController : public ChassisBase<rm_control::RobotStateInterface, hardware_interface::ImuSensorInterface,
-                                             hardware_interface::EffortJointInterface>
+                                             hardware_interface::EffortJointInterface>,
+                          public BipedalControllerInterface
 {
 public:
   BipedalController() = default;
   bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override;
   void moveJoint(const ros::Time& time, const ros::Duration& period) override;
   void stopping(const ros::Time& time) override;
   // clang-format off
-  bool getOverturn() const{ return overturn_; }
-  bool getStateChange() const{ return balance_state_changed_; }
-  bool getCompleteStand() const{ return complete_stand_; }
-  Eigen::Matrix<double, 4, CONTROL_DIM * STATE_DIM> getCoeffs() { return coeffs_; }
-  const std::shared_ptr<ModelParams>& getModelParams() const { return model_params_; }
-  const std::shared_ptr<ControlParams>& getControlParams() const { return control_params_; }
-  const std::shared_ptr<BiasParams>& getBiasParams() const { return bias_params_; }
-  const std::shared_ptr<LegStateThresholdParams>& getLegThresholdParams() const { return leg_threshold_params_; }
-  double getLegCmd() const{ return legCmd_; }
-  double getJumpCmd() const{ return jumpCmd_; }
-  int getBaseState() const{ return state_; }
-  inline double getDefaultLegLength() const { return default_leg_length_;}
-  geometry_msgs::Vector3 getVelCmd(){ return vel_cmd_; }
-  bool getMoveFlag() const{ return move_flag_; }
-  void setMoveFlag(const bool& move_flag) { move_flag_ = move_flag; }
-  inline VMCPtr& getVMCPtr() { return vmc_; }
-  void setStateChange(bool state){ balance_state_changed_ = state; }
-  void setCompleteStand(bool state){ complete_stand_ = state; }
-  void setJumpCmd(bool cmd){ jumpCmd_ = cmd; }
-  void setMode(int mode){ balance_mode_ = mode; }
-  inline void clearRecoveryFlag() { overturn_ = false; }
-  void pubState();
+  // BipedalControllerInterface implementations
+  bool getOverturn() const override { return overturn_; }
+  bool getStateChange() const override { return balance_state_changed_; }
+  bool getCompleteStand() const override { return complete_stand_; }
+  Eigen::Matrix<double, 4, CONTROL_DIM * STATE_DIM> getCoeffs() override { return coeffs_; }
+  const std::shared_ptr<ModelParams>& getModelParams() const override { return model_params_; }
+  const std::shared_ptr<ControlParams>& getControlParams() const override { return control_params_; }
+  const std::shared_ptr<BiasParams>& getBiasParams() const override { return bias_params_; }
+  const std::shared_ptr<LegStateThresholdParams>& getLegThresholdParams() const override { return leg_threshold_params_; }
+  const std::shared_ptr<ChassisGeometryParams>& getChassisGeometryParams() const override { return chassis_geometry_params_; }
+  double getLegCmd() const override { return legCmd_; }
+  double getJumpCmd() const override { return jumpCmd_; }
+  int getBaseState() const override { return state_; }
+  inline double getDefaultLegLength() const override { return default_leg_length_;}
+  geometry_msgs::Vector3 getVelCmd() override { return vel_cmd_; }
+  inline bool getMoveFlag() const override { return move_flag_; }
+  inline void setMoveFlag(const bool& move_flag) override { move_flag_ = move_flag; }
+  inline const ChassisState& getChassisState() override { return chassis_state_; };
+  inline LegState& getLegState(Side side) override { return leg_state_[side]; };
+  inline void setStateChange(bool state) override { balance_state_changed_ = state; }
+  inline void setCompleteStand(bool state) override { complete_stand_ = state; }
+  void setJumpCmd(bool cmd) override { jumpCmd_ = cmd; }
+  void setMode(int mode) override { balance_mode_ = mode; }
+  inline void clearRecoveryFlag() override { overturn_ = false; }
+  double f_spring_force(double L0) override;
+  void pubState() override;
   void pubLQRStatus(Eigen::Matrix<double, STATE_DIM, 1> left_error, Eigen::Matrix<double, STATE_DIM, 1> right_error,
                     Eigen::Matrix<double, STATE_DIM, 1> left_ref, Eigen::Matrix<double, STATE_DIM, 1> right_ref,
                     Eigen::Matrix<double, CONTROL_DIM, 1> u_left, Eigen::Matrix<double, CONTROL_DIM, 1> u_right,
-                    Eigen::Matrix<double, CONTROL_DIM, 1> F_leg_, const bool unstick[2]) const;
-  void pubLegLenStatus(const bool& upstair_flag);
+                    Eigen::Matrix<double, CONTROL_DIM, 1> F_leg_, const bool unstick[2]) const override;
+  void pubLegLenStatus(const bool& upstair_flag) override;
+  void clearStatus() override;
+  void pubDebugData(const std::string& name, double value) override{ debugPub_->add(name, value); };
+  void setRecoveryLegSpdTurnback(bool recovery_leg_spd_turnback) override { recovery_leg_spd_turnback_ = recovery_leg_spd_turnback; }
+  bool getRecoveryLegSpdTurnback() const override { return recovery_leg_spd_turnback_;}
   // clang-format on
-  void clearStatus();
-
 private:
   void updateEstimation(const ros::Time& time, const ros::Duration& period);
-  bool setupModelParams(ros::NodeHandle& controller_nh);
+
   bool setupLQR(ros::NodeHandle& controller_nh);
+  bool setupParams(ros::NodeHandle& controller_nh);
+  bool setupModelParams(ros::NodeHandle& controller_nh);
   bool setupControlParams(ros::NodeHandle& controller_nh);
   bool setupBiasParams(ros::NodeHandle& controller_nh);
   bool setupThresholdParams(ros::NodeHandle& controller_nh);
+  bool setupSpringParams(ros::NodeHandle& controller_nh);
+  bool setupChassisGeometryParams(ros::NodeHandle& controller_nh);
   void polyfit(const std::vector<Eigen::Matrix<double, 2, 6>>& Ks, const std::vector<double>& L0s,
                Eigen::Matrix<double, 4, 12>& coeffs);
   geometry_msgs::Twist odometry() override;
@@ -99,32 +112,35 @@ class BipedalController : public ChassisBase<rm_control::RobotStateInterface, ha
   std::shared_ptr<ModelParams> model_params_;
   std::shared_ptr<ControlParams> control_params_;
   std::shared_ptr<BiasParams> bias_params_;
+  std::shared_ptr<SpringParams> spring_params_;
+  std::shared_ptr<ChassisGeometryParams> chassis_geometry_params_;
   std::shared_ptr<LegStateThresholdParams> leg_threshold_params_;
 
   int balance_mode_ = BalanceMode::SIT_DOWN;
   bool balance_state_changed_ = false;
-  std::unique_ptr<ModeManager> mode_manager_;
-  VMCPtr vmc_;
+  std::shared_ptr<ModeManager> mode_manager_;
 
   // Slippage_detection
   double leftWheelVel{}, rightWheelVel{}, leftWheelVelAbsolute{}, rightWheelVelAbsolute{}, slip_alpha_{ 2.0 },
       slip_R_wheel_{}, R_wheel_{};
-  int i = 0, sample_times_ = 3;
+  int itor = 0, sample_times_ = 0;
   bool slip_flag_{ false };
   Eigen::Matrix<double, 2, 2> A_, B_, H_, Q_, R_;
   Eigen::Matrix<double, 2, 1> X_, U_;
   std::shared_ptr<KalmanFilter<double>> kalmanFilterPtr_;
-  std::shared_ptr<LowPassFilter> left_leg_angle_lpFilterPtr_, right_leg_angle_lpFilterPtr_,
-      left_leg_angle_vel_lpFilterPtr_, right_leg_angle_vel_lpFilterPtr_;
 
-  Eigen::Matrix<double, STATE_DIM, 1> x_left_{}, x_right_{};
-  double default_leg_length_{ 0.2 };
+  ChassisState chassis_state_;
+  LegState leg_state_[2];
+  //  Eigen::Matrix<double, STATE_DIM, 1> x_left_{}, x_right_{};
+  double default_leg_length_{ 0.12 };
   bool move_flag_{ false };
   // stand up
   bool complete_stand_ = false, overturn_ = false;
+  // recovery
+  bool recovery_leg_spd_turnback_{ false };
 
   // handles
-  hardware_interface::ImuSensorHandle imu_handle_;
+  hardware_interface::ImuSensorHandle imu_handle_, gimbal_imu_handle_;
   hardware_interface::JointHandle left_wheel_joint_handle_, right_wheel_joint_handle_;
   hardware_interface::JointHandle left_hip_joint_handle_, left_knee_joint_handle_, right_hip_joint_handle_,
       right_knee_joint_handle_;
@@ -139,11 +155,12 @@ class BipedalController : public ChassisBase<rm_control::RobotStateInterface, ha
   realtime_tools::RealtimeBuffer<LQRConfig> config_rt_buffer_;
   LQRConfig config_{};
   bool dynamic_reconfig_initialized_{ false };
-  ros::Subscriber leg_cmd_sub_;
+  ros::Subscriber leg_cmd_sub_, recovery_leg_spd_turnback_sub_;
   ros::Publisher unstick_pub_, upstair_status_pub_;
   std::shared_ptr<realtime_tools::RealtimePublisher<rm_msgs::LeggedChassisStatus>> legged_chassis_status_pub_;
   std::shared_ptr<realtime_tools::RealtimePublisher<rm_msgs::LeggedChassisMode>> legged_chassis_mode_pub_;
   std::shared_ptr<realtime_tools::RealtimePublisher<rm_msgs::LeggedLQRStatus>> lqr_status_pub_;
   ros::Time cmd_update_time_;
+  std::shared_ptr<DebugDataPublisher> debugPub_;
 };
 }  // namespace rm_chassis_controllers

rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_interface.h

@@ -0,0 +1,55 @@
+//
+// Created by wk on 2026/4/11.
+//
+#pragma once
+#include <Eigen/Dense>
+#include <geometry_msgs/Vector3.h>
+#include "bipedal_wheel_controller/definitions.h"
+#include "bipedal_wheel_controller/vmc/VMC.h"
+
+namespace rm_chassis_controllers
+{
+class BipedalControllerInterface
+{
+public:
+  virtual ~BipedalControllerInterface() = default;
+
+  virtual bool getStateChange() const = 0;
+  virtual void setStateChange(bool state) = 0;
+  virtual int getBaseState() const = 0;
+  virtual bool getOverturn() const = 0;
+  virtual void setMode(int mode) = 0;
+  virtual void clearRecoveryFlag() = 0;
+  virtual bool getCompleteStand() const = 0;
+  virtual Eigen::Matrix<double, 4, CONTROL_DIM * STATE_DIM> getCoeffs() = 0;
+
+  virtual const std::shared_ptr<ModelParams>& getModelParams() const = 0;
+  virtual const std::shared_ptr<ControlParams>& getControlParams() const = 0;
+  virtual const std::shared_ptr<BiasParams>& getBiasParams() const = 0;
+  virtual const std::shared_ptr<LegStateThresholdParams>& getLegThresholdParams() const = 0;
+  virtual const std::shared_ptr<ChassisGeometryParams>& getChassisGeometryParams() const = 0;
+  virtual double getLegCmd() const = 0;
+  virtual double getJumpCmd() const = 0;
+  virtual double getDefaultLegLength() const = 0;
+  virtual geometry_msgs::Vector3 getVelCmd() = 0;
+  virtual bool getMoveFlag() const = 0;
+  virtual void setMoveFlag(const bool& move_flag) = 0;
+  virtual const ChassisState& getChassisState() = 0;
+  virtual LegState& getLegState(Side side) = 0;
+  virtual void setCompleteStand(bool state) = 0;
+  virtual void setJumpCmd(bool cmd) = 0;
+  virtual double f_spring_force(double L0) = 0;
+  virtual void pubState() = 0;
+  virtual void pubLQRStatus(Eigen::Matrix<double, STATE_DIM, 1> left_error,
+                            Eigen::Matrix<double, STATE_DIM, 1> right_error,
+                            Eigen::Matrix<double, STATE_DIM, 1> left_ref, Eigen::Matrix<double, STATE_DIM, 1> right_ref,
+                            Eigen::Matrix<double, CONTROL_DIM, 1> u_left, Eigen::Matrix<double, CONTROL_DIM, 1> u_right,
+                            Eigen::Matrix<double, CONTROL_DIM, 1> F_leg_, const bool unstick[2]) const = 0;
+  virtual void pubLegLenStatus(const bool& upstair_flag) = 0;
+  virtual void clearStatus() = 0;
+  virtual void pubDebugData(const std::string& name, double value) = 0;
+  virtual bool getRecoveryLegSpdTurnback() const = 0;
+  virtual void setRecoveryLegSpdTurnback(bool recovery_leg_spd_turnback) = 0;
+};
+
+}  // namespace rm_chassis_controllers

rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_base.h

@@ -10,6 +10,7 @@
 #include <angles/angles.h>
 
 #include "bipedal_wheel_controller/definitions.h"
+#include "bipedal_wheel_controller/controller_interface.h"
 
 namespace rm_chassis_controllers
 {
@@ -18,39 +19,22 @@ class BipedalController;
 class ModeBase
 {
 public:
-  virtual void execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) = 0;
+  explicit ModeBase(BipedalControllerInterface* controller_) : controller(controller_)
+  {
+  }
+  virtual void execute(const ros::Time& time, const ros::Duration& period) = 0;
   virtual const char* name() const = 0;
   virtual ~ModeBase() = default;
-  void updateEstimation(const Eigen::Matrix<double, STATE_DIM, 1>& x_left,
-                        const Eigen::Matrix<double, STATE_DIM, 1>& x_right);
-  void updateLegKinematics(double* left_angle, double* right_angle, double* left_pos, double* left_spd,
-                           double* right_pos, double* right_spd);
-  void updateBaseState(const geometry_msgs::Vector3& angular_vel_base, const geometry_msgs::Vector3& linear_acc_base,
-                       const double& roll, const double& pitch, const double& yaw);
-
   void updateUnstick(const bool& left_unstick, const bool& right_unstick);
-  inline double getRealxVel()
-  {
-    return x_left_[3];
-  }
-
-  inline double getRealYawVel()
-  {
-    return angular_vel_base_.z;
-  }
 
   inline bool getUnstick()
   {
     return (left_unstick_ && right_unstick_);
   }
 
 protected:
-  Eigen::Matrix<double, STATE_DIM, 1> x_left_{}, x_right_{};
-  double left_angle_[2], right_angle_[2], left_pos_[2], left_spd_[2], right_pos_[2], right_spd_[2];
   bool left_unstick_{ false }, right_unstick_{ false };
-  geometry_msgs::Vector3 angular_vel_base_{}, linear_acc_base_{};
-  double roll_, pitch_, yaw_;
-  double yaw_total_{}, yaw_total_last_{};
+  BipedalControllerInterface* controller{ nullptr };
 };
 
 }  // namespace rm_chassis_controllers

rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_manager.h

@@ -12,13 +12,16 @@
 #include "bipedal_wheel_controller/controller_mode/recover.h"
 #include "bipedal_wheel_controller/controller_mode/normal.h"
 #include "bipedal_wheel_controller/controller_mode/upstairs.h"
+#include "bipedal_wheel_controller/controller_mode/protect.h"
+#include "bipedal_wheel_controller/controller_interface.h"
 
 namespace rm_chassis_controllers
 {
 class ModeManager
 {
 public:
-  ModeManager(ros::NodeHandle& controller_nh, const std::vector<hardware_interface::JointHandle*>& joint_handles);
+  ModeManager(BipedalControllerInterface* controller, ros::NodeHandle& controller_nh,
+              const std::vector<hardware_interface::JointHandle*>& joint_handles);
   virtual ~ModeManager() = default;
   void switchMode(int mode)
   {
@@ -36,7 +39,7 @@ class ModeManager
   control_toolbox::Pid pid_yaw_vel_, pid_left_leg_, pid_right_leg_, pid_theta_diff_, pid_roll_;
   control_toolbox::Pid pid_left_leg_stand_up_, pid_right_leg_stand_up_;
   control_toolbox::Pid pid_left_leg_theta_, pid_right_leg_theta_, pid_left_leg_theta_vel_, pid_right_leg_theta_vel_;
-  control_toolbox::Pid pid_left_wheel_vel_, pid_right_wheel_vel_;
+  control_toolbox::Pid pid_left_wheel_vel_, pid_right_wheel_vel_, pid_wheel_vel_diff_;
   std::vector<control_toolbox::Pid*> pid_wheels_, pid_legs_, pid_thetas_, pid_legs_stand_up_;
 };
 }  // namespace rm_chassis_controllers

rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h

@@ -17,10 +17,11 @@ namespace rm_chassis_controllers
 class Normal : public ModeBase
 {
 public:
-  Normal(const std::vector<hardware_interface::JointHandle*>& joint_handles,
+  Normal(BipedalControllerInterface* controller_, const std::vector<hardware_interface::JointHandle*>& joint_handles,
          const std::vector<control_toolbox::Pid*>& pid_legs, control_toolbox::Pid* pid_yaw_vel,
-         control_toolbox::Pid* pid_theta_diff, control_toolbox::Pid* pid_roll);
-  void execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) override;
+         control_toolbox::Pid* pid_theta_diff, control_toolbox::Pid* pid_roll,
+         control_toolbox::Pid* pid_wheel_vel_diff);
+  void execute(const ros::Time& time, const ros::Duration& period) override;
   const char* name() const override
   {
     return "NORMAL";
@@ -33,16 +34,17 @@ class Normal : public ModeBase
   bool unstickDetection(const double& F_leg, const double& Tp, const double& leg_len_spd, const double& leg_length,
                         const double& acc_z, const std::shared_ptr<ModelParams>& model_params,
                         Eigen::Matrix<double, STATE_DIM, 1> x,
-                        std::shared_ptr<MovingAverageFilter<double>> supportForceAveragePtr,
+                        const std::shared_ptr<MovingAverageFilter<double>>& supportForceAveragePtr,
                         const ros::Duration& period);
   std::vector<hardware_interface::JointHandle*> joint_handles_;
   std::vector<control_toolbox::Pid*> pid_legs_;
-  control_toolbox::Pid *pid_yaw_vel_, *pid_theta_diff_, *pid_roll_;
-
+  control_toolbox::Pid *pid_yaw_vel_, *pid_theta_diff_, *pid_roll_, *pid_wheel_vel_diff_;
   ros::Time lastJumpTime_{};
 
   double pos_des_{ 0.0 }, leg_length_des{ 0.2 };
+  double unstick_threshold{ 15.0 };
   int jump_phase_ = JumpPhase::IDLE, jumpTime_{ 0 };
+  bool x_offset_flag_{ false }, protect_flag_{ false };
   std::shared_ptr<MovingAverageFilter<double>> leftSupportForceAveragePtr_, rightSupportForceAveragePtr_;
 };
 }  // namespace rm_chassis_controllers