Update seires legged branch

rm_chassis_controllers/CMakeLists.txt

@@ -63,6 +63,11 @@ include_directories(
         ${EIGEN3_INCLUDE_DIR}
 )
 
+file(GLOB_RECURSE BIPEDAL_SOURCES
+        "src/bipedal_wheel_controller/*.cpp"
+        "src/bipedal_wheel_controller/*.c"
+)
+
 ## Declare cpp executables
 add_library(${PROJECT_NAME}
         src/chassis_base.cpp
@@ -71,20 +76,7 @@ add_library(${PROJECT_NAME}
         src/swerve.cpp
         src/balance.cpp
 
-        src/bipedal_wheel_controller/vmc/leg_conv.c
-        src/bipedal_wheel_controller/vmc/leg_pos.c
-        src/bipedal_wheel_controller/vmc/leg_spd.c
-        src/bipedal_wheel_controller/dynamics/gen_A.c
-        src/bipedal_wheel_controller/dynamics/gen_B.c
-        src/bipedal_wheel_controller/controller_mode/mode_base.cpp
-        src/bipedal_wheel_controller/controller_mode/mode_manager.cpp
-        src/bipedal_wheel_controller/controller_mode/stand_up.cpp
-        src/bipedal_wheel_controller/controller_mode/sit_down.cpp
-        src/bipedal_wheel_controller/controller_mode/recover.cpp
-        src/bipedal_wheel_controller/controller_mode/normal.cpp
-        src/bipedal_wheel_controller/controller_mode/upstairs.cpp
-        src/bipedal_wheel_controller/controller.cpp
-        src/bipedal_wheel_controller/series_legged_vmc_controller.cpp
+        ${BIPEDAL_SOURCES}
 )
 
 ## Specify libraries to link executable targets against

rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h

@@ -10,7 +10,9 @@
 #include <rm_msgs/LeggedLQRStatus.h>
 #include <rm_msgs/LQRkMatrix.h>
 #include <rm_msgs/LeggedChassisMode.h>
+#include <rm_msgs/LeggedUpstairStatus.h>
 #include <rm_common/filters/kalman_filter.h>
+#include <rm_common/filters/lp_filter.h>
 #include <control_toolbox/pid.h>
 #include <controller_interface/multi_interface_controller.h>
 #include <geometry_msgs/TwistStamped.h>
@@ -37,6 +39,7 @@ class BipedalController : public ChassisBase<rm_control::RobotStateInterface, ha
   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;
+  //  void follow(const ros::Time& time, const ros::Duration& period) override;
 
   // clang-format off
   bool getOverturn() const{ return overturn_; }
@@ -46,6 +49,7 @@ class BipedalController : public ChassisBase<rm_control::RobotStateInterface, ha
   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_; }
@@ -62,7 +66,7 @@ class BipedalController : public ChassisBase<rm_control::RobotStateInterface, ha
                     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& is_high_leg_flag);
+void pubLegLenStatus(const bool& upstair_flag);
   // clang-format on
   void clearStatus();
 
@@ -72,6 +76,7 @@ class BipedalController : public ChassisBase<rm_control::RobotStateInterface, ha
   bool setupLQR(ros::NodeHandle& controller_nh);
   bool setupControlParams(ros::NodeHandle& controller_nh);
   bool setupBiasParams(ros::NodeHandle& controller_nh);
+  bool setupThresholdParams(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;
@@ -82,6 +87,7 @@ 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<LegStateThresholdParams> leg_threshold_params_;
 
   int balance_mode_ = BalanceMode::SIT_DOWN;
   bool balance_state_changed_ = false;
@@ -95,6 +101,8 @@ class BipedalController : public ChassisBase<rm_control::RobotStateInterface, ha
   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 };
@@ -115,7 +123,7 @@ class BipedalController : public ChassisBase<rm_control::RobotStateInterface, ha
 
   // ROS Interface
   ros::Subscriber leg_cmd_sub_;
-  ros::Publisher unstick_pub_, leg_len_status_pub_;
+  ros::Publisher unstick_pub_, upstair_status_pub_;
   ros::Publisher legged_chassis_status_pub_, legged_chassis_mode_pub_;
   ros::Publisher lqr_status_pub_;
   ros::Time cmd_update_time_;

rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h

@@ -14,13 +14,33 @@ namespace rm_chassis_controllers
 {
 class Recover : public ModeBase
 {
+  typedef enum
+  {
+    ForwardSlip,
+    BackwardSlip,
+  } RecoveryChassisState;
+
+  typedef enum
+  {
+    INITIALIZED,
+    START,
+    MOVING,
+    MOVING_TOGETHER,
+    STOP,
+  } LegRecoveryCalibratedState;
+
+  enum
+  {
+    WheelOnGround,
+    KneeOnGround
+  } LegState;
+
 public:
   Recover(const std::vector<hardware_interface::JointHandle*>& joint_handles,
-          const std::vector<control_toolbox::Pid*>& pid_legs, const std::vector<control_toolbox::Pid*>& pid_thetas);
+          const std::vector<control_toolbox::Pid*>& pid_legs, const std::vector<control_toolbox::Pid*>& pid_thetas,
+          control_toolbox::Pid* pid_theta_diff);
   void execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) override;
-  void setUpLegMotion(const Eigen::Matrix<double, STATE_DIM, 1>& x, const int& other_leg_state,
-                      const double& leg_length, const double& leg_theta, int& leg_state, double& theta_des);
-  void detectLegRelState(const double& leg_theta, int& leg_state);
+  inline void detectChassisStateToRecover();
   const char* name() const override
   {
     return "RECOVER";
@@ -29,6 +49,10 @@ class Recover : public ModeBase
 private:
   std::vector<hardware_interface::JointHandle*> joint_handles_;
   std::vector<control_toolbox::Pid*> pid_legs_, pid_thetas_;
-  int left_leg_state, right_leg_state;
+  control_toolbox::Pid* pid_theta_diff_;
+  double leg_recovery_velocity_{ 2.0 }, threshold_{ 0.05 }, leg_theta_diff_{ 0.0 }, desired_leg_length_{ 0.36 };
+  const double leg_recovery_velocity_const_{ 2.0 };
+  RecoveryChassisState recovery_chassis_state_{ ForwardSlip };
+  bool detectd_flag{ false };
 };
 }  // namespace rm_chassis_controllers

rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h

@@ -27,8 +27,17 @@ class StandUp : public ModeBase
   void setUpLegMotion(const Eigen::Matrix<double, STATE_DIM, 1>& x, const int& other_leg_state,
                       const double& leg_length, const double& leg_theta, int& leg_state, double& theta_des,
                       double& length_des);
+  /**
+   * Detect the leg state before stand up: UNDER, FRONT, BEHIND
+   * @param x
+   * @param leg_state
+   */
+  void detectLegState(const Eigen::Matrix<double, STATE_DIM, 1>& x, int& leg_state);
   std::vector<hardware_interface::JointHandle*> joint_handles_;
   std::vector<control_toolbox::Pid*> pid_legs_, pid_thetas_;
   int left_leg_state, right_leg_state;
+  double theta_des_l, theta_des_r, length_des_l, length_des_r;
+  double spring_force_{};
+  std::shared_ptr<LegStateThresholdParams> leg_state_threshold_;
 };
 }  // namespace rm_chassis_controllers

rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/upstairs.h

@@ -25,8 +25,16 @@ class Upstairs : public ModeBase
   }
 
 private:
+  /**
+   * Detect the leg state before stand up: UNDER, FRONT, BEHIND
+   * @param x
+   * @param leg_state
+   */
+  void detectLegState(const Eigen::Matrix<double, STATE_DIM, 1>& x, int& leg_state);
+
   std::vector<hardware_interface::JointHandle*> joint_handles_;
   std::vector<control_toolbox::Pid*> pid_legs_, pid_thetas_;
   int left_leg_state, right_leg_state;
+  std::shared_ptr<LegStateThresholdParams> leg_state_threshold_;
 };
 }  // namespace rm_chassis_controllers

rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h

@@ -38,10 +38,23 @@ struct ControlParams
 struct BiasParams
 {
   double x;
+  double theta;
   double pitch;
   double roll;
 };
 
+struct LegStateThresholdParams
+{
+  double under_lower;
+  double under_upper;
+  double front_lower;
+  double front_upper;
+  double behind_lower;
+  double behind_upper;
+  double upstair_des_theta;
+  double upstair_exit_threshold;
+};
+
 struct LegCommand
 {
   double force;     // Thrust

rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h

@@ -54,33 +54,6 @@ inline void generateAB(const std::shared_ptr<ModelParams>& model_params, Eigen::
   // clang-format on
 }
 
-/**
- * Detect the leg state before stand up: UNDER, FRONT, BEHIND
- * @param x
- * @param leg_state
- */
-inline void detectLegState(const Eigen::Matrix<double, STATE_DIM, 1>& x, int& leg_state)
-{
-  if (x[0] > -M_PI / 2 + 0.7 && x[0] < (M_PI / 2 - 1.4))
-    leg_state = LegState::UNDER;
-  else if ((x[0] < -M_PI / 2 + 0.7 && x[0] > -M_PI) || (x[0] < M_PI && x[0] > M_PI - 0.5))
-    leg_state = LegState::FRONT;
-  else if (x[0] > (M_PI / 2 - 1.4) && x[0] < M_PI - 0.5)
-    leg_state = LegState::BEHIND;
-  switch (leg_state)
-  {
-    case LegState::UNDER:
-      ROS_INFO("[balance] x[0]: %.3f Leg state: UNDER", x[0]);
-      break;
-    case LegState::FRONT:
-      ROS_INFO("[balance] x[0]: %.3f Leg state: FRONT", x[0]);
-      break;
-    case LegState::BEHIND:
-      ROS_INFO("[balance] x[0]: %.3f Leg state: BEHIND", x[0]);
-      break;
-  }
-}
-
 /**
  * Compute the leg command using PID controllers
  * @param desired_length
@@ -103,31 +76,62 @@ inline LegCommand computePidLegCommand(double desired_length, double desired_ang
   cmd.force = length_pid.computeCommand(desired_length - leg_pos[0], period) + feedforward_force;
   if (!overturn)
   {
-    if (leg_state == LegState::BEHIND)
-      cmd.torque = angle_pid.computeCommand(-angles::shortest_angular_distance(desired_angle, leg_pos[1]), period);
-    else
-    {
-      if ((leg_pos[1] > M_PI_2 - 0.3 && leg_pos[1] < M_PI_2 + 0.3))
-        cmd.torque = angle_pid.computeCommand(-angles::shortest_angular_distance(desired_angle, leg_pos[1]), period);
-      else
-        cmd.torque = angle_vel_pid.computeCommand(-4 - leg_spd[1], period);
-    }
-  }
-  else
-  {
-    if (leg_state == LegState::FRONT)
+    if (leg_state == LegState::BEHIND || leg_state == LegState::UNDER)
     {
       cmd.torque = angle_pid.computeCommand(-angles::shortest_angular_distance(desired_angle, leg_pos[1]), period);
     }
     else
     {
-      cmd.torque = angle_vel_pid.computeCommand(-4 - leg_spd[1], period);
+      cmd.torque = angle_vel_pid.computeCommand(-5 - leg_spd[1], period);
     }
   }
+  //  else
+  //  {
+  //    if (leg_state == LegState::FRONT)
+  //    {
+  //      cmd.torque = angle_pid.computeCommand(-angles::shortest_angular_distance(desired_angle, leg_pos[1]), period);
+  //    }
+  //    else
+  //    {
+  //      cmd.torque = angle_vel_pid.computeCommand(-5 - leg_spd[1], period);
+  //    }
+  //  }
   leg_conv(cmd.force, cmd.torque, leg_angle[0], leg_angle[1], cmd.input);
   return cmd;
 }
 
+inline LegCommand computePidAngleVelLegCommand(double desired_length, double desired_leg_angle_vel, double leg_pos[2],
+                                               double leg_spd[2], control_toolbox::Pid& length_pid,
+                                               control_toolbox::Pid& angle_vel_pid, const double* leg_angle,
+                                               const ros::Duration& period, double feedforward_force = 0.0f)
+{
+  LegCommand cmd{ 0.0, 0.0, { 0.0, 0.0 } };
+  cmd.force = length_pid.computeCommand(desired_length - leg_pos[0], period) + feedforward_force;
+  cmd.torque = angle_vel_pid.computeCommand(desired_leg_angle_vel - leg_spd[1], period);
+  leg_conv(cmd.force, 10 * desired_leg_angle_vel + cmd.torque, leg_angle[0], leg_angle[1], cmd.input);
+  return cmd;
+}
+
+inline LegCommand computePidAngleLegCommand(double desired_length, double desired_leg_angle, double leg_pos[2],
+                                            control_toolbox::Pid& length_pid, control_toolbox::Pid& angle_pid,
+                                            const double* leg_angle, const ros::Duration& period,
+                                            double feedforward_force = 0.0f)
+{
+  LegCommand cmd{ 0.0, 0.0, { 0.0, 0.0 } };
+  cmd.force = length_pid.computeCommand(desired_length - leg_pos[0], period) + feedforward_force;
+  cmd.torque = angle_pid.computeCommand(-angles::shortest_angular_distance(desired_leg_angle, leg_pos[1]), period);
+  leg_conv(cmd.force, cmd.torque, leg_angle[0], leg_angle[1], cmd.input);
+  return cmd;
+}
+inline LegCommand computePidLenLegCommand(double desired_length, double leg_pos[2], control_toolbox::Pid& length_pid,
+                                          const double* leg_angle, const ros::Duration& period,
+                                          double feedforward_force = 0.0f)
+{
+  LegCommand cmd{ 0.0, 0.0, { 0.0, 0.0 } };
+  cmd.force = length_pid.computeCommand(desired_length - leg_pos[0], period) + feedforward_force;
+  leg_conv(cmd.force, 0.0, leg_angle[0], leg_angle[1], cmd.input);
+  return cmd;
+}
 /**
  * Set joint commands to the joint handles
  * @param joints

rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/series_legged_vmc_controller.h

@@ -18,6 +18,8 @@
 #include "bipedal_wheel_controller/vmc/leg_pos.h"
 #include "bipedal_wheel_controller/vmc/leg_spd.h"
 
+#include "bipedal_wheel_controller/vmc/VMC.h"
+
 #include <utility>
 
 namespace rm_chassis_controllers
@@ -48,6 +50,8 @@ class VMCController : public controller_interface::MultiInterfaceController<hard
   double vmcBiasAngle_, spring_force_;
   double lengthCmd_, angleCmd_;
 
+  std::unique_ptr<VMC> vmcPtr_;
+
   ros::Publisher statePublisher_, jointCmdStatePublisher_;
   ros::Subscriber cmdLegLengthSubscriber_, cmdLegAngleSubscriber_;
 };

rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h

@@ -0,0 +1,22 @@
+//
+// Created by wk on 2026/2/25.
+//
+
+namespace rm_chassis_controllers
+{
+class VMC
+{
+public:
+  explicit VMC(double l1, double l2, double l5) : l1_(l1), l2_(l2), l3_(l2), l4_(l1), l5_(l5){};
+  ~VMC() = default;
+
+  void leg_pos(double phi1, double phi4, double pos[2]) const;
+  void leg_spd(double dphi1, double dphi4, double phi1, double phi4, double spd[2]);
+  void leg_conv(double F, double Tp, double phi1, double phi4, double T[2]);
+
+private:
+  void calc_jacobian(double phi1, double phi4, double J[2][2]);
+
+  double l1_, l2_, l3_, l4_, l5_;
+};
+}  // namespace rm_chassis_controllers

rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h

@@ -100,7 +100,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController<T...>
    * @param time The current time.
    * @param period The time passed since the last call to update.
    */
-  void follow(const ros::Time& time, const ros::Duration& period);
+  virtual void follow(const ros::Time& time, const ros::Duration& period);
   /** @brief The mode TWIST: Just moving chassis.
    *
    * The mode TWIST: Chassis will move independent and will not effect by gimbal's move.