Enhance VMC and SeriesLeggedController with new features and optimizations

rm_chassis_controllers/CMakeLists.txt

@@ -21,10 +21,16 @@ find_package(catkin REQUIRED
         tf2_geometry_msgs
         nav_msgs
         angles
+        dynamic_reconfigure
 )
 
 find_package(Eigen3 REQUIRED)
 
+generate_dynamic_reconfigure_options(
+  cfg/LQRWeight.cfg
+  cfg/PowerLimit.cfg
+)
+
 ###################################
 ## catkin specific configuration ##
 ###################################
@@ -48,6 +54,7 @@ catkin_package(
         tf2_geometry_msgs
         nav_msgs
         angles
+        dynamic_reconfigure
         LIBRARIES ${PROJECT_NAME}
 )
 
@@ -58,24 +65,33 @@ catkin_package(
 ## Specify additional locations of header files
 include_directories(
         include
+        include/rm_chassis_controllers
         ${catkin_INCLUDE_DIRS}
         ${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
         src/omni.cpp
         src/sentry.cpp
         src/swerve.cpp
         src/balance.cpp
+        ${BIPEDAL_SOURCES}
 )
 
 ## Specify libraries to link executable targets against
 target_link_libraries(${PROJECT_NAME}
         ${catkin_LIBRARIES}
 )
 
+add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
+
 #############
 ## Install ##
 #############

rm_chassis_controllers/cfg/LQRWeight.cfg

@@ -0,0 +1,18 @@
+#!/usr/bin/env python3
+PACKAGE = "rm_chassis_controllers"
+
+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_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_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_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)
+gen.add("R_Tp", double_t, 0, "leg force weight",     1.0, 0.001, 10000.0)
+
+exit(gen.generate(PACKAGE, "chassis_controller", "LQRWeight"))

rm_chassis_controllers/cfg/PowerLimit.cfg

@@ -0,0 +1,12 @@
+#!/usr/bin/env python3
+PACKAGE = "rm_chassis_controllers"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("vel_coeff", double_t, 0, "velocity coefficient for power limit", 0.000, 0.000, 1.000)
+gen.add("effort_coeff", double_t, 0, "effort coefficient for power limit", 0.0, 0.0, 20.0)
+gen.add("power_offset", double_t, 0, "power offset for power limit", 0.0, -50.0, 50.0)
+
+exit(gen.generate(PACKAGE, "chassis_controller", "PowerLimit"))

rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h

@@ -0,0 +1,149 @@
+//
+// Created by guanlin on 25-8-28.
+//
+
+#pragma once
+
+#include <rm_common/lqr.h>
+#include <rm_msgs/LegCmd.h>
+#include <rm_msgs/LeggedChassisStatus.h>
+#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>
+#include <geometry_msgs/Vector3.h>
+#include <hardware_interface/imu_sensor_interface.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <std_msgs/Float64.h>
+#include <std_msgs/Bool.h>
+#include "rm_chassis_controllers/chassis_base.h"
+#include <dynamic_reconfigure/server.h>
+#include <rm_chassis_controllers/LQRWeightConfig.h>
+
+#include "bipedal_wheel_controller/helper_functions.h"
+#include "bipedal_wheel_controller/definitions.h"
+#include "bipedal_wheel_controller/controller_mode/mode_manager.h"
+#include "bipedal_wheel_controller/vmc/VMC.h"
+
+namespace rm_chassis_controllers
+{
+using Eigen::Matrix;
+
+struct LQRConfig
+{
+  double Q_theta{}, Q_d_theta{}, Q_x{}, Q_dx{}, Q_phi{}, Q_d_phi{};
+  double R_T{}, R_Tp{};
+};
+
+class BipedalController : public ChassisBase<rm_control::RobotStateInterface, hardware_interface::ImuSensorInterface,
+                                             hardware_interface::EffortJointInterface>
+{
+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();
+  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);
+  // 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 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;
+
+  void reconfigCB(rm_chassis_controllers::LQRWeightConfig& config, uint32_t level);
+
+  Eigen::Matrix<double, 4, CONTROL_DIM * STATE_DIM> coeffs_;
+  Eigen::Matrix<double, STATE_DIM, STATE_DIM> q_{};
+  Eigen::Matrix<double, CONTROL_DIM, CONTROL_DIM> r_{};
+
+  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;
+  std::unique_ptr<ModeManager> mode_manager_;
+  VMCPtr vmc_;
+
+  // Slippage_detection
+  double leftWheelVel{}, rightWheelVel{}, leftWheelVelAbsolute{}, rightWheelVelAbsolute{}, slip_alpha_{ 2.0 },
+      slip_R_wheel_{}, R_wheel_{};
+  int i = 0, sample_times_ = 3;
+  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 };
+  bool move_flag_{ false };
+  // stand up
+  bool complete_stand_ = false, overturn_ = false;
+
+  // handles
+  hardware_interface::ImuSensorHandle 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_;
+  std::vector<hardware_interface::JointHandle*> joint_handles_;
+
+  // Leg Cmd
+  double legCmd_{ 0.2 };
+  bool jumpCmd_{ false };
+
+  // ROS Interface
+  dynamic_reconfigure::Server<rm_chassis_controllers::LQRWeightConfig>* d_srv_;
+  realtime_tools::RealtimeBuffer<LQRConfig> config_rt_buffer_;
+  LQRConfig config_{};
+  bool dynamic_reconfig_initialized_{ false };
+  ros::Subscriber leg_cmd_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_;
+};
+}  // namespace rm_chassis_controllers

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

@@ -0,0 +1,56 @@
+//
+// Created by guanlin on 25-9-3.
+//
+
+#pragma once
+
+#include <ros/time.h>
+#include <Eigen/Dense>
+#include <geometry_msgs/Vector3.h>
+#include <angles/angles.h>
+
+#include "bipedal_wheel_controller/definitions.h"
+
+namespace rm_chassis_controllers
+{
+class BipedalController;
+
+class ModeBase
+{
+public:
+  virtual void execute(BipedalController* controller, 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_{};
+};
+
+}  // namespace rm_chassis_controllers

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

@@ -0,0 +1,42 @@
+//
+// Created by guanlin on 25-9-4.
+//
+
+#pragma once
+
+#include <control_toolbox/pid.h>
+
+#include "bipedal_wheel_controller/controller_mode/mode_base.h"
+#include "bipedal_wheel_controller/controller_mode/sit_down.h"
+#include "bipedal_wheel_controller/controller_mode/stand_up.h"
+#include "bipedal_wheel_controller/controller_mode/recover.h"
+#include "bipedal_wheel_controller/controller_mode/normal.h"
+#include "bipedal_wheel_controller/controller_mode/upstairs.h"
+
+namespace rm_chassis_controllers
+{
+class ModeManager
+{
+public:
+  ModeManager(ros::NodeHandle& controller_nh, const std::vector<hardware_interface::JointHandle*>& joint_handles);
+  virtual ~ModeManager() = default;
+  void switchMode(int mode)
+  {
+    mode_impl = mode_map_[mode];
+  }
+  const std::shared_ptr<ModeBase>& getModeImpl()
+  {
+    return mode_impl;
+  }
+
+private:
+  std::shared_ptr<ModeBase> mode_impl;
+  std::map<int, std::shared_ptr<ModeBase>> mode_map_;
+
+  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_;
+  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

@@ -0,0 +1,48 @@
+//
+// Created by guanlin on 25-9-3.
+//
+
+#pragma once
+
+#include <hardware_interface/joint_command_interface.h>
+#include <control_toolbox/pid.h>
+
+#include "rm_common/filters/filters.h"
+
+#include "bipedal_wheel_controller/controller_mode/mode_base.h"
+#include "bipedal_wheel_controller/definitions.h"
+
+namespace rm_chassis_controllers
+{
+class Normal : public ModeBase
+{
+public:
+  Normal(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;
+  const char* name() const override
+  {
+    return "NORMAL";
+  }
+
+private:
+  double calculateSupportForce(double F, double Tp, double leg_length, const double& leg_len_spd, double acc_z,
+                               Eigen::Matrix<double, STATE_DIM, 1> x, const std::shared_ptr<ModelParams>& model_params,
+                               const ros::Duration& period);
+  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 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_;
+
+  ros::Time lastJumpTime_{};
+
+  double pos_des_{ 0.0 }, leg_length_des{ 0.2 };
+  int jump_phase_ = JumpPhase::IDLE, jumpTime_{ 0 };
+  std::shared_ptr<MovingAverageFilter<double>> leftSupportForceAveragePtr_, rightSupportForceAveragePtr_;
+};
+}  // namespace rm_chassis_controllers