From 3ae8dfc6e9bc4cf326cf1fb33bae1d9500de52ed Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Tue, 23 Sep 2025 12:38:16 +0800 Subject: [PATCH 01/57] Add SeriesLeggedController. --- rm_chassis_controllers/CMakeLists.txt | 15 + .../bipedal_wheel_controller/controller.h | 88 ++++++ .../controller_mode/mode_base.h | 54 ++++ .../controller_mode/mode_manager.h | 40 +++ .../controller_mode/normal.h | 45 +++ .../controller_mode/recover.h | 30 ++ .../controller_mode/sit_down.h | 30 ++ .../controller_mode/stand_up.h | 34 +++ .../bipedal_wheel_controller/definitions.h | 64 +++++ .../bipedal_wheel_controller/dynamics/gen_A.h | 32 +++ .../bipedal_wheel_controller/dynamics/gen_B.h | 32 +++ .../helper_functions.h | 135 +++++++++ .../bipedal_wheel_controller/vmc/leg_conv.h | 31 +++ .../vmc/leg_conv_fwd.h | 31 +++ .../bipedal_wheel_controller/vmc/leg_pos.h | 31 +++ .../bipedal_wheel_controller/vmc/leg_spd.h | 31 +++ .../rm_chassis_controllers_plugins.xml | 8 + .../bipedal_wheel_controller/controller.cpp | 259 ++++++++++++++++++ .../controller_mode/mode_base.cpp | 44 +++ .../controller_mode/mode_manager.cpp | 41 +++ .../controller_mode/normal.cpp | 204 ++++++++++++++ .../controller_mode/recover.cpp | 46 ++++ .../controller_mode/sit_down.cpp | 40 +++ .../controller_mode/stand_up.cpp | 78 ++++++ .../bipedal_wheel_controller/dynamics/gen_A.c | 181 ++++++++++++ .../bipedal_wheel_controller/dynamics/gen_B.c | 139 ++++++++++ .../bipedal_wheel_controller/vmc/leg_conv.c | 62 +++++ .../vmc/leg_conv_fwd.c | 127 +++++++++ .../bipedal_wheel_controller/vmc/leg_pos.c | 41 +++ .../bipedal_wheel_controller/vmc/leg_spd.c | 58 ++++ 30 files changed, 2051 insertions(+) create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_base.h create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_manager.h create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/sit_down.h create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/dynamics/gen_A.h create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/dynamics/gen_B.h create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_conv.h create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_conv_fwd.h create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_pos.h create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_spd.h create mode 100644 rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp create mode 100644 rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_base.cpp create mode 100644 rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp create mode 100644 rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp create mode 100644 rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp create mode 100644 rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp create mode 100644 rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp create mode 100644 rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_A.c create mode 100644 rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_B.c create mode 100644 rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_conv.c create mode 100644 rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_conv_fwd.c create mode 100644 rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_pos.c create mode 100644 rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_spd.c diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt index ec9391d0..05f1804a 100644 --- a/rm_chassis_controllers/CMakeLists.txt +++ b/rm_chassis_controllers/CMakeLists.txt @@ -58,6 +58,7 @@ catkin_package( ## Specify additional locations of header files include_directories( include + include/rm_chassis_controllers ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ) @@ -69,6 +70,20 @@ add_library(${PROJECT_NAME} src/sentry.cpp src/swerve.cpp src/balance.cpp + + src/bipedal_wheel_controller/vmc/leg_conv_fwd.c + 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.cpp ) ## Specify libraries to link executable targets against diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h new file mode 100644 index 00000000..97dabec6 --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h @@ -0,0 +1,88 @@ +// +// Created by guanlin on 25-8-28. +// + +#pragma once + +#include +#include "rm_msgs/LegCmd.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include "rm_chassis_controllers/chassis_base.h" + +#include "bipedal_wheel_controller/helper_functions.h" +#include "bipedal_wheel_controller/definitions.h" +#include "bipedal_wheel_controller/controller_mode/mode_manager.h" + +namespace rm_chassis_controllers +{ +using Eigen::Matrix; + +class BipedalController : public ChassisBase +{ +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 getCoeffs() { return coeffs_; } + const std::shared_ptr& getModelParams(){ return model_params_; } + double getLegCmd() const{ return legCmd_; } + double getJumpCmd() const{ return jumpCmd_; } + geometry_msgs::Vector3 getVelCmd(){ return vel_cmd_; } + + 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; } + void pubState(); + // clang-format on + +private: + void updateEstimation(const ros::Time& time, const ros::Duration& period); + bool setupModelParams(ros::NodeHandle& controller_nh); + bool setupLQR(ros::NodeHandle& controller_nh); + void polyfit(const std::vector>& Ks, const std::vector& L0s, + Eigen::Matrix& coeffs); + geometry_msgs::Twist odometry() override; + Eigen::Matrix coeffs_; + Eigen::Matrix q_{}; + Eigen::Matrix r_{}; + + std::shared_ptr model_params_; + + int balance_mode_ = BalanceMode::SIT_DOWN; + bool balance_state_changed_ = false; + std::unique_ptr mode_manager_; + + // 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_, left_hip_joint_handle_, + left_knee_joint_handle_, right_hip_joint_handle_, right_knee_joint_handle_; + std::vector joint_handles_; + + // Leg Cmd + double legCmd_{}; + bool jumpCmd_{}; + + // ROS Interface + ros::Subscriber leg_cmd_sub_; + ros::Publisher unstick_pub_; + ros::Time cmd_update_time_; +}; +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_base.h new file mode 100644 index 00000000..ae678d9b --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_base.h @@ -0,0 +1,54 @@ +// +// Created by guanlin on 25-9-3. +// + +#pragma once + +#include +#include +#include + +#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& x_left, + const Eigen::Matrix& 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 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_; +}; + +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_manager.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_manager.h new file mode 100644 index 00000000..4a685e68 --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_manager.h @@ -0,0 +1,40 @@ +// +// Created by guanlin on 25-9-4. +// + +#pragma once + +#include + +#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" + +namespace rm_chassis_controllers +{ +class ModeManager +{ +public: + ModeManager(ros::NodeHandle& controller_nh, const std::vector& joint_handles); + virtual ~ModeManager() = default; + void switchMode(int mode) + { + mode_impl = mode_map_[mode]; + } + const std::shared_ptr& getModeImpl() + { + return mode_impl; + } + +private: + std::shared_ptr mode_impl; + std::map> 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_theta_, pid_right_leg_theta_; + control_toolbox::Pid pid_left_wheel_vel_, pid_right_wheel_vel_; + std::vector pid_wheels_, pid_legs_, pid_thetas_; +}; +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h new file mode 100644 index 00000000..f317ec30 --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h @@ -0,0 +1,45 @@ +// +// Created by guanlin on 25-9-3. +// + +#pragma once + +#include +#include + +#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& joint_handles, + const std::vector& pid_legs, const control_toolbox::Pid& pid_yaw_vel, + const control_toolbox::Pid& pid_theta_diff, const 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, double acc_z, + Eigen::Matrix x, Eigen::Matrix u, + const std::shared_ptr& model_params); + bool unstickDetection(const double& hip_effort, const double& knee_effort, const double& wheel_effort, + const double& hip_angle, const double& knee_angle, const double& leg_length, + const double& acc_z, const std::shared_ptr& model_params, + Eigen::Matrix x); + std::vector joint_handles_; + std::vector pid_legs_; + control_toolbox::Pid pid_yaw_vel_, pid_theta_diff_, pid_roll_; + + int jump_phase_ = JumpPhase::SQUAT; + bool start_jump_ = false; + std::unique_ptr> supportForceAveragePtr; +}; +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h new file mode 100644 index 00000000..062ed6a7 --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h @@ -0,0 +1,30 @@ +// +// Created by guanlin on 25-9-3. +// + +#pragma once + +#include +#include + +#include "bipedal_wheel_controller/controller_mode/mode_base.h" +#include "bipedal_wheel_controller/definitions.h" + +namespace rm_chassis_controllers +{ +class Recover : public ModeBase +{ +public: + Recover(const std::vector& joint_handles, + const std::vector& pid_legs, const std::vector& pid_thetas); + void execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) override; + const char* name() const override + { + return "RECOVER"; + } + +private: + std::vector joint_handles_; + std::vector pid_legs_, pid_thetas_; +}; +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/sit_down.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/sit_down.h new file mode 100644 index 00000000..f6f140dd --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/sit_down.h @@ -0,0 +1,30 @@ +// +// Created by guanlin on 25-9-3. +// + +#pragma once + +#include +#include + +#include "bipedal_wheel_controller/controller_mode/mode_base.h" +#include "bipedal_wheel_controller/definitions.h" + +namespace rm_chassis_controllers +{ +class SitDown : public ModeBase +{ +public: + explicit SitDown(const std::vector& joint_handles, + const std::vector& pid_wheels); + void execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) override; + const char* name() const override + { + return "SIT_DOWN"; + } + +private: + std::vector joint_handles_; + std::vector pid_wheels_; +}; +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h new file mode 100644 index 00000000..d599e8ea --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h @@ -0,0 +1,34 @@ +// +// Created by guanlin on 25-9-3. +// + +#pragma once + +#include +#include + +#include "bipedal_wheel_controller/controller_mode/mode_base.h" +#include "bipedal_wheel_controller/definitions.h" + +namespace rm_chassis_controllers +{ +class StandUp : public ModeBase +{ +public: + StandUp(const std::vector& joint_handles, + const std::vector& pid_legs, const std::vector& pid_thetas); + void execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) override; + const char* name() const override + { + return "STAND_UP"; + } + +private: + void setUpLegMotion(const Eigen::Matrix& x, const int& other_leg_state, + const double& leg_length, const double& leg_theta, int& leg_state, double& theta_des, + double& length_des); + std::vector joint_handles_; + std::vector pid_legs_, pid_thetas_; + int left_leg_state, right_leg_state; +}; +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h new file mode 100644 index 00000000..70266b5e --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h @@ -0,0 +1,64 @@ +// +// Created by guanlin on 25-8-30. +// + +#pragma once + +#include +#include + +namespace rm_chassis_controllers +{ +struct ModelParams +{ + double L_weight; // Length weight to wheel axis + double Lm_weight; // Length weight to mass center + double l; // Leg rest length + double m_w; // Wheel mass + double m_p; // Leg mass + double M; // Body mass + double i_w; // Wheel inertia + double i_p; // Leg inertia + double i_m; // Body inertia + double r; // Wheel radius + double g; // Gravity acceleration +}; + +struct LegCommand +{ + double force; // Thrust + double torque; // Torque + double input[2]; // input +}; + +enum LegState +{ + UNDER, + FRONT, + BEHIND +}; + +enum JumpPhase +{ + SQUAT, + JUMP, + SHRINK, + DONE +}; + +enum BalanceMode +{ + NORMAL, + STAND_UP, + SIT_DOWN, + RECOVER +}; + +constexpr std::array, 3> jumpLengthDes = { + { { JumpPhase::SQUAT, 0.15 }, { JumpPhase::JUMP, 0.32 }, { JumpPhase::SHRINK, 0.15 } } +}; + +constexpr static const int STATE_DIM = 6; +constexpr static const int CONTROL_DIM = 2; + +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/dynamics/gen_A.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/dynamics/gen_A.h new file mode 100644 index 00000000..6ee48d72 --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/dynamics/gen_A.h @@ -0,0 +1,32 @@ +/* + * File: gen_A.h + * + * MATLAB Coder version : 5.5 + * C/C++ source code generated on : 08-Aug-2025 17:34:32 + */ + +#ifndef GEN_A_H +#define GEN_A_H + +/* Include Files */ +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Function Declarations */ +extern void gen_A(double Im, double Ip, double Iw, double L, double Lm, double M, double R, double g, double l, + double mp, double mw, double A[36]); + +#ifdef __cplusplus +} +#endif + +#endif +/* + * File trailer for gen_A.h + * + * [EOF] + */ diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/dynamics/gen_B.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/dynamics/gen_B.h new file mode 100644 index 00000000..f3491aa6 --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/dynamics/gen_B.h @@ -0,0 +1,32 @@ +/* + * File: gen_B.h + * + * MATLAB Coder version : 5.5 + * C/C++ source code generated on : 08-Aug-2025 17:37:08 + */ + +#ifndef GEN_B_H +#define GEN_B_H + +/* Include Files */ +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Function Declarations */ +extern void gen_B(double Im, double Ip, double Iw, double L, double Lm, double M, double R, double l, double mp, + double mw, double B[12]); + +#ifdef __cplusplus +} +#endif + +#endif +/* + * File trailer for gen_B.h + * + * [EOF] + */ diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h new file mode 100644 index 00000000..9f01d877 --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h @@ -0,0 +1,135 @@ +// +// Created by guanlin on 25-8-27. +// + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "bipedal_wheel_controller/dynamics/gen_A.h" +#include "bipedal_wheel_controller/dynamics/gen_B.h" +#include "bipedal_wheel_controller/vmc/leg_conv_fwd.h" +#include "bipedal_wheel_controller/vmc/leg_conv.h" +#include "bipedal_wheel_controller/definitions.h" + +namespace rm_chassis_controllers +{ +/** + * Generate continuous-time state space matrices A and B + * @param model_params + * @param a + * @param b + * @param leg_length + */ +inline void generateAB(const std::shared_ptr& model_params, Eigen::Matrix& a, + Eigen::Matrix& b, double leg_length) +{ + double A[36] = { 0. }, B[12]{ 0. }; + double L = leg_length * model_params->L_weight; + double Lm = leg_length * model_params->Lm_weight; + gen_A(model_params->i_m, model_params->i_p, model_params->i_w, L, Lm, model_params->M, model_params->r, + model_params->g, model_params->l, model_params->m_p, model_params->m_w, A); + gen_B(model_params->i_m, model_params->i_p, model_params->i_w, L, Lm, model_params->M, model_params->r, + model_params->l, model_params->m_p, model_params->m_w, B); + + // clang-format off + a<< 0. ,1.,0.,0.,0. ,0., + A[1],0.,0.,0.,A[25],0., + 0. ,0.,0.,1.,0. ,0., + A[3],0.,0.,0.,A[27],0., + 0. ,0.,0.,0.,0. ,1., + A[5],0.,0.,0.,A[29],0.; + b<< 0. ,0. , + B[1],B[7], + 0. ,0. , + B[3],B[9], + 0. ,0. , + B[5],B[11]; + // clang-format on +} + +/** + * Detect the leg state before stand up: UNDER, FRONT, BEHIND + * @param x + * @param leg_state + */ +inline void detectLegState(const Eigen::Matrix& x, int& leg_state) +{ + if (x[0] > -M_PI / 2 + 0.1 && x[0] < M_PI / 2 - 0.3) + leg_state = LegState::UNDER; + else if (x[0] < -M_PI / 2 + 0.1 && x[0] > -M_PI) + leg_state = LegState::FRONT; + else if (x[0] > M_PI / 2 - 0.3 && x[0] < M_PI) + leg_state = LegState::BEHIND; + ROS_INFO("[balance] Leg state: %d", leg_state); +} + +/** + * Compute the leg command using PID controllers + * @param desired_length + * @param desired_angle + * @param current_length + * @param current_angle + * @param length_pid + * @param angle_pid + * @param leg_angle + * @param period + * @return + */ +inline LegCommand computePidLegCommand(double desired_length, double desired_angle, double current_length, + double current_angle, control_toolbox::Pid& length_pid, + control_toolbox::Pid& angle_pid, const double* leg_angle, + const ros::Duration& period) +{ + LegCommand cmd; + cmd.force = length_pid.computeCommand(desired_length - current_length, period); + cmd.torque = -angle_pid.computeCommand(-angles::shortest_angular_distance(desired_angle, current_angle), period); + leg_conv(cmd.force, cmd.torque, leg_angle[0], leg_angle[1], cmd.input); + return cmd; +} + +/** + * Set joint commands to the joint handles + * @param joints + * @param left_cmd + * @param right_cmd + * @param wheel_left + * @param wheel_right + */ +inline void setJointCommands(std::vector& joints, const LegCommand& left_cmd, + const LegCommand& right_cmd, double wheel_left = 0., double wheel_right = 0.) +{ + if (joints.size() != 6) + throw std::runtime_error("Joint handle vector size must be 6!"); + + joints[0]->setCommand(left_cmd.input[0]); + joints[1]->setCommand(left_cmd.input[1]); + joints[2]->setCommand(right_cmd.input[0]); + joints[3]->setCommand(right_cmd.input[1]); + joints[4]->setCommand(wheel_left); + joints[5]->setCommand(wheel_right); +} + +/** + * Convert quaternion to roll, pitch, yaw + * @param q + * @param roll + * @param pitch + * @param yaw + */ +inline void quatToRPY(const geometry_msgs::Quaternion& q, double& roll, double& pitch, double& yaw) +{ + double as = std::min(-2. * (q.x * q.z - q.w * q.y), .99999); + yaw = std::atan2(2 * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z); + pitch = std::asin(as); + roll = std::atan2(2 * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z); +} + +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_conv.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_conv.h new file mode 100644 index 00000000..ab13963d --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_conv.h @@ -0,0 +1,31 @@ +/* + * File: leg_conv.h + * + * MATLAB Coder version : 5.5 + * C/C++ source code generated on : 10-Aug-2025 14:29:16 + */ + +#ifndef LEG_CONV_H +#define LEG_CONV_H + +/* Include Files */ +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Function Declarations */ +extern void leg_conv(double F, double Tp, double phi1, double phi2, double T[2]); + +#ifdef __cplusplus +} +#endif + +#endif +/* + * File trailer for leg_conv.h + * + * [EOF] + */ diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_conv_fwd.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_conv_fwd.h new file mode 100644 index 00000000..d788c6b3 --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_conv_fwd.h @@ -0,0 +1,31 @@ +/* + * File: leg_conv_fwd.h + * + * MATLAB Coder version : 5.5 + * C/C++ source code generated on : 11-Aug-2025 13:45:27 + */ + +#ifndef LEG_CONV_FWD_H +#define LEG_CONV_FWD_H + +/* Include Files */ +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Function Declarations */ +extern void leg_conv_fwd(double T1, double T2, double phi1, double phi2, double T_real[2]); + +#ifdef __cplusplus +} +#endif + +#endif +/* + * File trailer for leg_conv_fwd.h + * + * [EOF] + */ diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_pos.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_pos.h new file mode 100644 index 00000000..4787a5a7 --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_pos.h @@ -0,0 +1,31 @@ +/* + * File: leg_pos.h + * + * MATLAB Coder version : 5.5 + * C/C++ source code generated on : 29-Aug-2025 12:36:02 + */ + +#ifndef LEG_POS_H +#define LEG_POS_H + +/* Include Files */ +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Function Declarations */ +extern void leg_pos(double phi1, double phi2, double pos[2]); + +#ifdef __cplusplus +} +#endif + +#endif +/* + * File trailer for leg_pos.h + * + * [EOF] + */ diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_spd.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_spd.h new file mode 100644 index 00000000..bc4eccee --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_spd.h @@ -0,0 +1,31 @@ +/* + * File: leg_spd.h + * + * MATLAB Coder version : 5.5 + * C/C++ source code generated on : 10-Aug-2025 14:30:13 + */ + +#ifndef LEG_SPD_H +#define LEG_SPD_H + +/* Include Files */ +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Function Declarations */ +extern void leg_spd(double dphi1, double dphi2, double phi1, double phi2, double spd[2]); + +#ifdef __cplusplus +} +#endif + +#endif +/* + * File trailer for leg_spd.h + * + * [EOF] + */ diff --git a/rm_chassis_controllers/rm_chassis_controllers_plugins.xml b/rm_chassis_controllers/rm_chassis_controllers_plugins.xml index 6319ebc3..020d5d80 100644 --- a/rm_chassis_controllers/rm_chassis_controllers_plugins.xml +++ b/rm_chassis_controllers/rm_chassis_controllers_plugins.xml @@ -32,6 +32,14 @@ EffortJointInterface type of hardware interface. + + + The BalanceController is RoboMaster SeriesLegged standard robot SeriesLegged controller. It expects a + EffortJointInterface type of hardware interface. + + diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp new file mode 100644 index 00000000..dc3a52bc --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp @@ -0,0 +1,259 @@ +// +// Created by guanlin on 25-8-28. +// + +#include "bipedal_wheel_controller/controller.h" + +#include +#include +#include + +#include +#include + +#include "bipedal_wheel_controller/vmc/leg_conv.h" +#include "bipedal_wheel_controller/vmc/leg_spd.h" +#include "bipedal_wheel_controller/vmc/leg_pos.h" + +namespace rm_chassis_controllers +{ +bool BipedalController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh) +{ + ChassisBase::init(robot_hw, root_nh, controller_nh); + + imu_handle_ = robot_hw->get()->getHandle("base_imu"); + const std::pair table[] = { + { "left_hip_joint", &left_hip_joint_handle_ }, { "left_knee_joint", &left_knee_joint_handle_ }, + { "right_hip_joint", &right_hip_joint_handle_ }, { "right_knee_joint", &right_knee_joint_handle_ }, + { "left_wheel_joint", &left_wheel_joint_handle_ }, { "right_wheel_joint", &right_wheel_joint_handle_ } + }; + auto* joint_interface = robot_hw->get(); + for (const auto& t : table) + { + *t.second = joint_interface->getHandle(t.first); + joint_handles_.push_back(t.second); + } + + mode_manager_ = std::make_unique(controller_nh, joint_handles_); + model_params_ = std::make_shared(); + + if (!setupModelParams(controller_nh) || !setupLQR(controller_nh)) + return false; + + auto legCmdCallback = [this](const rm_msgs::LegCmd::ConstPtr& msg) { + legCmd_ = msg->leg_length; + jumpCmd_ = msg->jump; + }; + leg_cmd_sub_ = controller_nh.subscribe("/leg_cmd", 1, legCmdCallback); + + unstick_pub_ = controller_nh.advertise("unstick", 1); + + ROS_INFO("chassis_controller init done!"); + return true; +} + +void BipedalController::moveJoint(const ros::Time& time, const ros::Duration& period) +{ + if (!balance_state_changed_) + mode_manager_->switchMode(balance_mode_); + updateEstimation(time, period); + mode_manager_->getModeImpl()->execute(this, time, period); + pubState(); +} + +void BipedalController::updateEstimation(const ros::Time& time, const ros::Duration& period) +{ + geometry_msgs::Vector3 gyro, acc; + gyro.x = imu_handle_.getAngularVelocity()[0]; + gyro.y = imu_handle_.getAngularVelocity()[1]; + gyro.z = imu_handle_.getAngularVelocity()[2]; + acc.x = imu_handle_.getLinearAcceleration()[0]; + acc.y = imu_handle_.getLinearAcceleration()[1]; + acc.z = imu_handle_.getLinearAcceleration()[2]; + tf2::Transform odom2imu, imu2base, odom2base; + geometry_msgs::Vector3 angular_vel_base{}, linear_acc_base{}; + double roll{}, pitch{}, yaw{}; + try + { + tf2::doTransform(gyro, angular_vel_base, + robot_state_handle_.lookupTransform("base_link", imu_handle_.getFrameId(), time)); + geometry_msgs::TransformStamped tf_msg; + tf_msg = robot_state_handle_.lookupTransform(imu_handle_.getFrameId(), "base_link", time); + tf2::fromMsg(tf_msg.transform, imu2base); + tf2::Quaternion odom2imu_quaternion; + tf2::Vector3 odom2imu_origin; + odom2imu_quaternion.setValue(imu_handle_.getOrientation()[0], imu_handle_.getOrientation()[1], + imu_handle_.getOrientation()[2], imu_handle_.getOrientation()[3]); + odom2imu_origin.setValue(0, 0, 0); + odom2imu.setOrigin(odom2imu_origin); + odom2imu.setRotation(odom2imu_quaternion); + odom2base = odom2imu * imu2base; + quatToRPY(toMsg(odom2base).rotation, roll, pitch, yaw); + + tf_msg.transform = tf2::toMsg(odom2imu.inverse()); + tf_msg.header.stamp = time; + tf2::doTransform(acc, linear_acc_base, tf_msg); + + tf2::Vector3 z_body(0, 0, 1); + tf2::Vector3 z_world = tf2::quatRotate(odom2base.getRotation(), z_body); + overturn_ = z_world.z() < 0; + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + setJointCommands(joint_handles_, { 0, 0, { 0., 0. } }, { 0, 0, { 0., 0. } }); + return; + } + + // vmc + double left_angle[2]{}, right_angle[2]{}, left_pos[2]{}, left_spd[2]{}, right_pos[2]{}, right_spd[2]{}; + // [0]:hip_vmc_joint [1]:knee_vmc_joint + left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI / 2.; + // left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI / 4.; + left_angle[1] = left_knee_joint_handle_.getPosition() - 1.6581; + right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI / 2.; + // right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI / 4.; + right_angle[1] = right_knee_joint_handle_.getPosition() - 1.6581; + // [0] is length, [1] is angle + leg_pos(left_angle[0], left_angle[1], left_pos); + leg_pos(right_angle[0], right_angle[1], right_pos); + leg_spd(left_hip_joint_handle_.getVelocity(), left_knee_joint_handle_.getVelocity(), left_angle[0], left_angle[1], + left_spd); + leg_spd(right_hip_joint_handle_.getVelocity(), right_knee_joint_handle_.getVelocity(), right_angle[0], right_angle[1], + right_spd); + + // update state + Eigen::Matrix x_left{}, x_right{}; + x_left[3] = + (left_wheel_joint_handle_.getVelocity() + right_wheel_joint_handle_.getVelocity()) / 2.0 * model_params_->r; + if (abs(x_left[3]) < 0.4 && vel_cmd_.x == 0.) + x_left[2] += x_left[3] * period.toSec(); + else + x_left[2] = 0.; + x_left[0] = left_pos[1] + pitch; + x_left[1] = -left_spd[1] + angular_vel_base.y; + x_left[4] = -pitch; + x_left[5] = -angular_vel_base.y; + x_right = x_left; + x_right[0] = right_pos[1] + pitch; + x_right[1] = -right_spd[1] + angular_vel_base.y; + + mode_manager_->getModeImpl()->updateEstimation(x_left, x_right); + mode_manager_->getModeImpl()->updateLegKinematics(left_angle, right_angle, left_pos, left_spd, right_pos, right_spd); + mode_manager_->getModeImpl()->updateBaseState(angular_vel_base, linear_acc_base, roll, pitch, yaw); +} + +void BipedalController::pubState() +{ + std_msgs::Bool msg; + msg.data = mode_manager_->getModeImpl()->getUnstick(); + unstick_pub_.publish(msg); +} + +void BipedalController::stopping(const ros::Time& time) +{ + balance_mode_ = BalanceMode::RECOVER; + balance_state_changed_ = false; + setJointCommands(joint_handles_, { 0, 0, { 0., 0. } }, { 0, 0, { 0., 0. } }); + + ROS_INFO("[balance] Controller Stop"); +} + +bool BipedalController::setupModelParams(ros::NodeHandle& controller_nh) +{ + const std::pair tbl[] = // + { { "m_w", &model_params_->m_w }, + { "m_p", &model_params_->m_p }, + { "M", &model_params_->M }, + { "i_w", &model_params_->i_w }, + { "i_m", &model_params_->i_m }, + { "i_p", &model_params_->i_p }, + { "l", &model_params_->l }, + { "L_weight", &model_params_->L_weight }, + { "Lm_weight", &model_params_->Lm_weight }, + { "g", &model_params_->g }, + { "wheel_radius", &model_params_->r } }; + + for (const auto& e : tbl) + if (!controller_nh.getParam(e.first, *e.second)) + { + ROS_ERROR("Param %s not given (namespace: %s)", e.first, controller_nh.getNamespace().c_str()); + return false; + } + return true; +} + +bool BipedalController::setupLQR(ros::NodeHandle& controller_nh) +{ + // Set up weight matrices + auto loadWeightMatrix = [](ros::NodeHandle& nh, const char* key, int dim) -> Eigen::VectorXd { + std::vector v; + if (!nh.getParam(key, v) || static_cast(v.size()) != dim) + return Eigen::VectorXd::Constant(dim, std::numeric_limits::quiet_NaN()); + return Eigen::VectorXd::Map(v.data(), dim); + }; + Eigen::VectorXd q_diag = loadWeightMatrix(controller_nh, "q", STATE_DIM); + Eigen::VectorXd r_diag = loadWeightMatrix(controller_nh, "r", CONTROL_DIM); + if (!q_diag.allFinite() || !r_diag.allFinite()) + return false; + q_.setZero(); + r_.setZero(); + q_.diagonal() = q_diag; + r_.diagonal() = r_diag; + + // Continuous model \dot{x} = A x + B u + std::vector lengths; + std::vector> ks; + for (int i = 5; i < 30; i++) + { + double length = i / 100.; + lengths.push_back(length); + Eigen::Matrix a{}; + Eigen::Matrix b{}; + generateAB(model_params_, a, b, length); + Lqr lqr(a, b, q_, r_); + if (!lqr.computeK()) + { + ROS_ERROR("Failed to compute K of LQR."); + return false; + } + Eigen::Matrix k = lqr.getK(); + ks.push_back(k); + } + polyfit(ks, lengths, coeffs_); + return true; +} + +void BipedalController::polyfit(const std::vector>& Ks, const std::vector& L0s, + Eigen::Matrix& coeffs) +{ + int N = L0s.size(); + Eigen::MatrixXd A(N, 4), B(N, 12); + for (int i = 0; i < N; ++i) + { + A.block(i, 0, 1, 4) << pow(L0s[i], 3), pow(L0s[i], 2), L0s[i], 1.0; + Eigen::Map> flat(Ks[i].data()); + B.row(i) = flat.transpose(); + } + coeffs = (A.transpose() * A).ldlt().solve(A.transpose() * B); +} + +geometry_msgs::Twist BipedalController::odometry() +{ + geometry_msgs::Twist twist; + if (mode_manager_->getModeImpl() != nullptr) + { + twist.linear.x = mode_manager_->getModeImpl()->getRealxVel(); + twist.angular.z = mode_manager_->getModeImpl()->getRealYawVel(); + } + else + { + twist.linear.x = 0; + twist.angular.z = 0; + } + return twist; +} + +} // namespace rm_chassis_controllers +PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::BipedalController, controller_interface::ControllerBase) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_base.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_base.cpp new file mode 100644 index 00000000..0fa8feff --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_base.cpp @@ -0,0 +1,44 @@ +// +// Created by guanlin on 25-9-4. +// + +#include "bipedal_wheel_controller/controller_mode/mode_base.h" + +namespace rm_chassis_controllers +{ +void ModeBase::updateEstimation(const Eigen::Matrix& x_left, + const Eigen::Matrix& x_right) +{ + x_left_ = x_left; + x_right_ = x_right; +} + +void ModeBase::updateLegKinematics(double* left_angle, double* right_angle, double* left_pos, double* left_spd, + double* right_pos, double* right_spd) +{ + std::memcpy(left_pos_, left_pos, 2 * sizeof(double)); + std::memcpy(left_spd_, left_spd, 2 * sizeof(double)); + std::memcpy(right_pos_, right_pos, 2 * sizeof(double)); + std::memcpy(right_spd_, right_spd, 2 * sizeof(double)); + std::memcpy(left_angle_, left_angle, 2 * sizeof(double)); + std::memcpy(right_angle_, right_angle, 2 * sizeof(double)); +} + +void ModeBase::updateBaseState(const geometry_msgs::Vector3& angular_vel_base, + const geometry_msgs::Vector3& linear_acc_base, const double& roll, const double& pitch, + const double& yaw) +{ + angular_vel_base_ = angular_vel_base; + linear_acc_base_ = linear_acc_base; + roll_ = roll; + pitch_ = pitch; + yaw_ = yaw; +} + +void ModeBase::updateUnstick(const bool& left_unstick, const bool& right_unstick) +{ + left_unstick_ = left_unstick; + right_unstick_ = right_unstick; +} + +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp new file mode 100644 index 00000000..7b004eea --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp @@ -0,0 +1,41 @@ +// +// Created by guanlin on 25-9-4. +// + +#include "bipedal_wheel_controller/controller_mode/mode_manager.h" + +namespace rm_chassis_controllers +{ +ModeManager::ModeManager(ros::NodeHandle& controller_nh, + const std::vector& joint_handles) +{ + const std::pair pids[] = { + { "pid_yaw_vel", &pid_yaw_vel_ }, + { "pid_left_leg", &pid_left_leg_ }, + { "pid_right_leg", &pid_right_leg_ }, + { "pid_theta_diff", &pid_theta_diff_ }, + { "pid_roll", &pid_roll_ }, + { "pid_left_leg_theta", &pid_left_leg_theta_ }, + { "pid_right_leg_theta", &pid_right_leg_theta_ }, + { "pid_left_wheel_vel", &pid_left_wheel_vel_ }, + { "pid_right_wheel_vel", &pid_right_wheel_vel_ }, + }; + for (const auto& e : pids) + if (controller_nh.hasParam(e.first) && !e.second->init(ros::NodeHandle(controller_nh, e.first))) + ROS_ERROR("Failed to load pid %s", e.first); + pid_wheels_.push_back(&pid_left_wheel_vel_); + pid_wheels_.push_back(&pid_right_wheel_vel_); + pid_legs_.push_back(&pid_left_leg_); + pid_legs_.push_back(&pid_right_leg_); + pid_thetas_.push_back(&pid_left_leg_theta_); + pid_thetas_.push_back(&pid_right_leg_theta_); + + mode_map_.insert(std::make_pair(BalanceMode::NORMAL, std::make_unique(joint_handles, pid_legs_, pid_yaw_vel_, + pid_theta_diff_, pid_roll_))); + mode_map_.insert( + std::make_pair(BalanceMode::STAND_UP, std::make_unique(joint_handles, pid_legs_, pid_thetas_))); + mode_map_.insert( + std::make_pair(BalanceMode::RECOVER, std::make_unique(joint_handles, pid_legs_, pid_thetas_))); + mode_map_.insert(std::make_pair(BalanceMode::SIT_DOWN, std::make_unique(joint_handles, pid_wheels_))); +} +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp new file mode 100644 index 00000000..bd888348 --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -0,0 +1,204 @@ +// +// Created by guanlin on 25-9-3. +// + +#include "bipedal_wheel_controller/controller_mode/normal.h" +#include "bipedal_wheel_controller/controller.h" + +namespace rm_chassis_controllers +{ +Normal::Normal(const std::vector& joint_handles, + const std::vector& pid_legs, const control_toolbox::Pid& pid_yaw_vel, + const control_toolbox::Pid& pid_theta_diff, const control_toolbox::Pid& pid_roll) + : joint_handles_(joint_handles) + , pid_legs_(pid_legs) + , pid_yaw_vel_(pid_yaw_vel) + , pid_theta_diff_(pid_theta_diff) + , pid_roll_(pid_roll) +{ + supportForceAveragePtr = std::make_unique>(4); +} + +void Normal::execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) +{ + if (!controller->getStateChange()) + { + ROS_INFO("[balance] Enter NORMAL"); + controller->setStateChange(true); + } + if (!controller->getCompleteStand() && abs(x_left_[4]) < 0.2) + controller->setCompleteStand(true); + + auto vel_cmd_ = controller->getVelCmd(); + + // PID + double T_yaw = pid_yaw_vel_.computeCommand(vel_cmd_.z - angular_vel_base_.z, period); + double T_theta_diff = pid_theta_diff_.computeCommand(left_pos_[1] - right_pos_[1], period); + double T_roll = pid_roll_.computeCommand(0. - roll_, period); + + // LQR + auto coeffs_ = controller->getCoeffs(); + Eigen::Matrix k_left{}, k_right{}; + for (int i = 0; i < 2; i++) + for (int j = 0; j < 6; j++) + { + k_left(i, j) = coeffs_(0, i + 2 * j) * pow(left_pos_[0], 3) + coeffs_(1, i + 2 * j) * pow(left_pos_[0], 2) + + coeffs_(2, i + 2 * j) * left_pos_[0] + coeffs_(3, i + 2 * j); + k_right(i, j) = coeffs_(0, i + 2 * j) * pow(right_pos_[0], 3) + coeffs_(1, i + 2 * j) * pow(right_pos_[0], 2) + + coeffs_(2, i + 2 * j) * right_pos_[0] + coeffs_(3, i + 2 * j); + } + Eigen::Matrix u_left, u_right; + auto x_left = x_left_; + auto x_right = x_right_; + x_left(2) = x_right(2) -= 0.; + if (controller->getCompleteStand()) + { + x_left(3) -= vel_cmd_.x; + x_right(3) -= vel_cmd_.x; + } + u_left = k_left * (-x_left); + u_right = k_right * (-x_right); + + // Compute leg thrust + auto model_params_ = controller->getModelParams(); + double gravity = 1. / 2. * model_params_->M * model_params_->g; + Eigen::Matrix F_leg; + double leg_length_des = controller->getLegCmd() == 0 ? 0.18 : controller->getLegCmd(); + if (!start_jump_ && controller->getJumpCmd() && abs(x_left[0]) < 0.1) + { + start_jump_ = true; + ROS_INFO("[balance] Jump start"); + } + if (start_jump_) + { + leg_length_des = jumpLengthDes[jump_phase_].second; + if (std::abs(leg_length_des - left_pos_[0]) < 0.01) + jump_phase_ += 1; + if (jump_phase_ == JumpPhase::DONE) + { + jump_phase_ = JumpPhase::SQUAT; + controller->setJumpCmd(false); + start_jump_ = false; + ROS_INFO("[balance] Jump finished"); + } + else if (jump_phase_ == JumpPhase::SHRINK) + { + gravity = 0; + } + F_leg[0] = + pid_legs_[0]->computeCommand(leg_length_des - left_pos_[0], period) + gravity * cos(left_pos_[1]) + T_roll; + F_leg[1] = + pid_legs_[1]->computeCommand(leg_length_des - right_pos_[0], period) + gravity * cos(right_pos_[1]) - T_roll; + } + else + { + double left_length_des = controller->getCompleteStand() ? leg_length_des / cos(x_left[0]) : 0.18; + double right_length_des = controller->getCompleteStand() ? leg_length_des / cos(x_right[0]) : 0.18; + F_leg[0] = + pid_legs_[0]->computeCommand(left_length_des - left_pos_[0], period) + gravity * cos(left_pos_[1]) + T_roll; + F_leg[1] = + pid_legs_[1]->computeCommand(right_length_des - right_pos_[0], period) + gravity * cos(right_pos_[1]) - T_roll; + } + + // Unstick detection + Eigen::Matrix k_left_unstick{}, k_right_unstick{}; + k_left_unstick.setZero(); + k_right_unstick.setZero(); + k_left_unstick.block<1, 2>(1, 0) = k_left.block<1, 2>(1, 0); + k_right_unstick.block<1, 2>(1, 0) = k_right.block<1, 2>(1, 0); + bool left_unstick = + unstickDetection(joint_handles_[0]->getEffort(), joint_handles_[1]->getEffort(), joint_handles_[4]->getEffort(), + left_angle_[0], left_angle_[1], left_pos_[0], linear_acc_base_.z, model_params_, x_left_); + bool right_unstick = + unstickDetection(joint_handles_[2]->getEffort(), joint_handles_[3]->getEffort(), joint_handles_[5]->getEffort(), + right_angle_[0], right_angle_[1], right_pos_[0], linear_acc_base_.z, model_params_, x_right_); + updateUnstick(left_unstick, right_unstick); + + if (left_unstick && jump_phase_ != JumpPhase::JUMP) + u_left = k_left_unstick * (-x_left); + if (right_unstick && jump_phase_ != JumpPhase::JUMP) + u_right = k_right_unstick * (-x_right); + + // Control + double left_T[2], right_T[2]; + leg_conv(F_leg[0], -u_left(1) + T_theta_diff, left_angle_[0], left_angle_[1], left_T); + leg_conv(F_leg[1], -u_right(1) - T_theta_diff, right_angle_[0], right_angle_[1], right_T); + double left_wheel_cmd = left_unstick ? 0. : u_left(0) - T_yaw; + double right_wheel_cmd = right_unstick ? 0. : u_right(0) + T_yaw; + LegCommand left_cmd = { F_leg[0], u_left[1], { left_T[0], left_T[1] } }, + right_cmd = { F_leg[1], u_right[1], { right_T[0], right_T[1] } }; + setJointCommands(joint_handles_, left_cmd, right_cmd, left_wheel_cmd, right_wheel_cmd); + + // Protection + if ((controller->getCompleteStand() && (abs(x_left(4)) > 0.4 || abs(x_left(0)) > 1.5)) || controller->getOverturn()) + { + controller->setMode(BalanceMode::SIT_DOWN); + controller->setStateChange(false); + controller->setJumpCmd(false); + setJointCommands(joint_handles_, { 0, 0, { 0., 0. } }, { 0, 0, { 0., 0. } }); + ROS_INFO("[balance] Exit NORMAL"); + } +} + +double Normal::calculateSupportForce(double F, double Tp, double leg_length, double acc_z, + Eigen::Matrix x, Eigen::Matrix u, + const std::shared_ptr& model_params) +{ + static double last_ddot_zM = acc_z - model_params->g; + Eigen::Matrix a; + Eigen::Matrix b; + generateAB(model_params, a, b, leg_length); + + double P = F * cos(x(0)) + Tp * sin(x(0)) / leg_length; + // lp filter + double ddot_zM = 0.7 * (acc_z - model_params->g) + 0.3 * last_ddot_zM; + auto ddot_x = a * x + b * u; + double ddot_theta = ddot_x(1); + double ddot_zw = ddot_zM - leg_length * cos(x(0)) + 2 * leg_length * x(1) * sin(x(0)) + + +leg_length * (ddot_theta * sin(x(0)) + x(1) * x(1) * cos(x(0))); + double Fn = model_params->m_w * ddot_zw + model_params->m_w * model_params->g + P; + + return Fn; +} + +bool Normal::unstickDetection(const double& hip_effort, const double& knee_effort, const double& wheel_effort, + const double& hip_angle, const double& knee_angle, const double& leg_length, + const double& acc_z, const std::shared_ptr& model_params, + Eigen::Matrix x) +{ + static bool maybeChange = false, last_unstick_ = false; + ros::Time judgeTime; + double leg_F[2]; + leg_conv_fwd(hip_effort, knee_effort, hip_angle, knee_angle, leg_F); + Eigen::Matrix u_left_real; + u_left_real << wheel_effort, leg_F[1]; + double Fn = calculateSupportForce(leg_F[0], leg_F[1], leg_length, acc_z, x, u_left_real, model_params); + + bool unstick_ = Fn < 20; + + if (unstick_ != last_unstick_) + { + if (!maybeChange) + { + judgeTime = ros::Time::now(); + maybeChange = true; + } + else + { + if (ros::Time::now() - judgeTime > ros::Duration(0.01)) + { + last_unstick_ = unstick_; + } + } + } + else + { + maybeChange = false; + } + return unstick_; + // if (Fn < 20.) + // return true; + // else + // return false; +} +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp new file mode 100644 index 00000000..5554ef5f --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp @@ -0,0 +1,46 @@ +// +// Created by guanlin on 25-9-3. +// + +#include "bipedal_wheel_controller/controller_mode/recover.h" +#include "bipedal_wheel_controller/controller.h" + +namespace rm_chassis_controllers +{ +Recover::Recover(const std::vector& joint_handles, + const std::vector& pid_legs, + const std::vector& pid_thetas) + : joint_handles_(joint_handles), pid_legs_(pid_legs), pid_thetas_(pid_thetas) +{ +} + +void Recover::execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) +{ + if (!controller->getStateChange()) + { + ROS_INFO("[balance] Enter RECOVER"); + controller->setStateChange(true); + } + + int left_leg_state, right_leg_state; + LegCommand left_cmd = { 0, 0, { 0., 0. } }, right_cmd = { 0, 0, { 0., 0. } }; + detectLegState(x_left_, left_leg_state); + detectLegState(x_right_, right_leg_state); + if (controller->getOverturn() && left_leg_state != LegState::FRONT) + left_cmd = computePidLegCommand(0.4, -M_PI / 2 - 0.8, left_pos_[0], left_pos_[1], *pid_legs_[0], *pid_thetas_[0], + left_angle_, period); + if (controller->getOverturn() && right_leg_state != LegState::FRONT) + right_cmd = computePidLegCommand(0.4, -M_PI / 2 - 0.8, right_pos_[0], right_pos_[1], *pid_legs_[1], *pid_thetas_[1], + right_angle_, period); + setJointCommands(joint_handles_, left_cmd, right_cmd); + + // Exit + if ((controller->getOverturn() && left_leg_state == LegState::FRONT && right_leg_state == LegState::FRONT) || + !controller->getOverturn()) + { + controller->setMode(BalanceMode::STAND_UP); + controller->setStateChange(false); + ROS_INFO("[balance] Exit RECOVER"); + } +} +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp new file mode 100644 index 00000000..7d06e95e --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp @@ -0,0 +1,40 @@ +// +// Created by guanlin on 25-9-3. +// + +#include "bipedal_wheel_controller/controller_mode/sit_down.h" +#include "bipedal_wheel_controller/controller.h" + +namespace rm_chassis_controllers +{ +SitDown::SitDown(const std::vector& joint_handles, + const std::vector& pid_wheels) + : joint_handles_(joint_handles), pid_wheels_(pid_wheels) +{ +} + +void SitDown::execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) +{ + if (!controller->getStateChange()) + { + ROS_INFO("[balance] Enter SIT_DOWN"); + controller->setStateChange(true); + } + + LegCommand left_cmd = { 0, 0, { 0., 0. } }, right_cmd = { 0, 0, { 0., 0. } }; + double left_wheel_cmd = pid_wheels_[0]->computeCommand(joint_handles_[0]->getVelocity(), period); + double right_wheel_cmd = pid_wheels_[1]->computeCommand(joint_handles_[1]->getVelocity(), period); + setJointCommands(joint_handles_, left_cmd, right_cmd, left_wheel_cmd, right_wheel_cmd); + + // Exit + if (abs(x_left_(1)) < 0.1 && abs(x_left_(5)) < 0.1 && abs(x_left_(3)) < 0.15) + { + if (!controller->getOverturn()) + controller->setMode(BalanceMode::STAND_UP); + else + controller->setMode(BalanceMode::RECOVER); + controller->setStateChange(false); + ROS_INFO("[balance] Exit SIT_DOWN"); + } +} +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp new file mode 100644 index 00000000..d1eabd22 --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp @@ -0,0 +1,78 @@ +// +// Created by guanlin on 25-9-3. +// + +#include "bipedal_wheel_controller/controller_mode/stand_up.h" +#include "bipedal_wheel_controller/controller.h" +#include "bipedal_wheel_controller/helper_functions.h" + +namespace rm_chassis_controllers +{ +StandUp::StandUp(const std::vector& joint_handles, + const std::vector& pid_legs, + const std::vector& pid_thetas) + : joint_handles_(joint_handles), pid_legs_(pid_legs), pid_thetas_(pid_thetas) +{ +} + +void StandUp::execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) +{ + if (!controller->getStateChange()) + { + ROS_INFO("[balance] Enter STAND_UP"); + controller->setStateChange(true); + controller->setCompleteStand(false); + detectLegState(x_left_, left_leg_state); + detectLegState(x_right_, right_leg_state); + } + + double theta_des_l, theta_des_r, length_des_l, length_des_r; + LegCommand left_cmd = { 0, 0, { 0., 0. } }, right_cmd = { 0, 0, { 0., 0. } }; + setUpLegMotion(x_left_, right_leg_state, left_pos_[0], left_pos_[1], left_leg_state, theta_des_l, length_des_l); + setUpLegMotion(x_right_, left_leg_state, right_pos_[0], right_pos_[1], right_leg_state, theta_des_r, length_des_r); + left_cmd = computePidLegCommand(length_des_l, theta_des_l, left_pos_[0], left_pos_[1], *pid_legs_[0], *pid_thetas_[0], + left_angle_, period); + right_cmd = computePidLegCommand(length_des_r, theta_des_r, right_pos_[0], right_pos_[1], *pid_legs_[1], + *pid_thetas_[1], right_angle_, period); + setJointCommands(joint_handles_, left_cmd, right_cmd); + + // Exit + if (((left_pos_[1] < 0. && left_leg_state == LegState::BEHIND) || + (left_pos_[1] > 0. && left_leg_state == LegState::UNDER)) && + ((right_pos_[1] < 0. && right_leg_state == LegState::BEHIND) || + (right_pos_[1] > 0. && right_leg_state == LegState::UNDER))) + { + controller->setMode(BalanceMode::NORMAL); + controller->setStateChange(false); + ROS_INFO("[balance] Exit STAND_UP"); + } +} + +void StandUp::setUpLegMotion(const Eigen::Matrix& x, const int& other_leg_state, + const double& leg_length, const double& leg_theta, int& leg_state, double& theta_des, + double& length_des) +{ + switch (leg_state) + { + case LegState::UNDER: + theta_des = 0.15; + length_des = 0.10; + break; + case LegState::FRONT: + theta_des = M_PI / 2 + 0.2; + length_des = 0.33; + if (abs(angles::shortest_angular_distance(x[0], M_PI / 2)) < 0.2 && abs(x[4]) < 0.1) + leg_state = LegState::BEHIND; + break; + case LegState::BEHIND: + theta_des = leg_theta; + length_des = leg_length; + if (other_leg_state != LegState::FRONT) + { + theta_des = 0.; + length_des = 0.10; + } + break; + } +} +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_A.c b/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_A.c new file mode 100644 index 00000000..c049b996 --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_A.c @@ -0,0 +1,181 @@ +/* + * File: gen_A.c + * + * MATLAB Coder version : 5.5 + * C/C++ source code generated on : 08-Aug-2025 17:34:32 + */ + +/* Include Files */ +#include "bipedal_wheel_controller/dynamics/gen_A.h" + +#include + +/* Function Definitions */ +/* + * gen_A + * A = gen_A(Im,Ip,Iw,L,Lm,M,R,G,l,MP,MW) + * + * Arguments : double Im + * double Ip + * double Iw + * double L + * double Lm + * double M + * double R + * double g + * double l + * double mp + * double mw + * double A[36] + * Return Type : void + */ +void gen_A(double Im, double Ip, double Iw, double L, double Lm, double M, double R, double g, double l, double mp, + double mw, double A[36]) +{ + double b_t18_tmp; + double b_t33_tmp; + double b_t41_tmp; + double c_t41_tmp; + double d; + double d1; + double d_t41_tmp; + double e_t41_tmp; + double f_t41_tmp; + double g_t41_tmp; + double h_t41_tmp; + double i_t41_tmp; + double j_t41_tmp; + double k_t41_tmp; + double t17_tmp; + double t18_tmp; + double t2; + double t23_tmp; + double t3; + double t31_tmp; + double t32_tmp; + double t33_tmp; + double t34; + double t34_tmp; + double t39_tmp; + double t39_tmp_tmp; + double t4; + double t41; + double t41_tmp; + double t5; + double t6; + double t7; + /* This function was generated by the Symbolic Math Toolbox version 9.2. + */ + /* 08-Aug-2025 17:33:02 */ + t2 = L * L; + t3 = Lm * Lm; + t4 = M * M; + t5 = R * R; + t6 = l * l; + t7 = mp * mp; + t17_tmp = Iw * M; + t18_tmp = Iw * L; + b_t18_tmp = t18_tmp * Lm; + t23_tmp = Iw * g * l; + t31_tmp = M * g; + t32_tmp = L * g; + t33_tmp = g * l; + b_t33_tmp = t33_tmp * mw; + t34_tmp = Lm * g; + t34 = t34_tmp * mp * t4 * t5 * t6; + t39_tmp_tmp = L * Lm; + t39_tmp = t39_tmp_tmp * g * l; + t41_tmp = Im * Ip; + b_t41_tmp = Im * Iw; + c_t41_tmp = b_t41_tmp * M; + d_t41_tmp = Im * M; + e_t41_tmp = d_t41_tmp * mw; + f_t41_tmp = Ip * M; + g_t41_tmp = b_t41_tmp * L; + h_t41_tmp = Im * L; + i_t41_tmp = h_t41_tmp * Lm; + j_t41_tmp = i_t41_tmp * M; + k_t41_tmp = Ip * Iw * M; + t41 = + 1.0 / + (((((((((((((((((t41_tmp * Iw + k_t41_tmp * t6) + b_t41_tmp * mp * t2) + t41_tmp * mp * t5) + t41_tmp * mw * t5) + + g_t41_tmp * Lm * M * 2.0) + + c_t41_tmp * t2) + + c_t41_tmp * t3) + + t41_tmp * M * t5) + + e_t41_tmp * t2 * t5) + + d_t41_tmp * mp * t3 * t5) + + e_t41_tmp * t3 * t5) + + t17_tmp * mp * t2 * t6) + + f_t41_tmp * mp * t5 * t6) + + f_t41_tmp * mw * t5 * t6) + + Im * mp * mw * t2 * t5) + + j_t41_tmp * mw * t5 * 2.0) + + M * mp * mw * t2 * t5 * t6); + A[0] = 0.0; + d = h_t41_tmp * g; + c_t41_tmp = h_t41_tmp * M * g; + e_t41_tmp = Im * Lm; + h_t41_tmp = e_t41_tmp * M * g; + d1 = L * M * g; + t41_tmp = (((t18_tmp * g * t4 * t6 + Iw * Lm * g * t4 * t6) + t32_tmp * mw * t4 * t5 * t6) + t34) + + t34_tmp * mw * t4 * t5 * t6; + A[1] = t41 * (((((((((((((((t41_tmp + g_t41_tmp * g * mp) + d * t4 * t5) + d * t5 * t7) + e_t41_tmp * g * t4 * t5) + + g_t41_tmp * M * g) + + b_t41_tmp * Lm * M * g) + + c_t41_tmp * mp * t5 * 2.0) + + c_t41_tmp * mw * t5) + + t18_tmp * M * g * mp * t6) + + h_t41_tmp * mp * t5) + + h_t41_tmp * mw * t5) + + d * mp * mw * t5) + + d1 * t5 * t6 * t7) + + t32_tmp * mp * t4 * t5 * t6) + + d1 * mp * mw * t5 * t6); + A[2] = 0.0; + d = Im * g; + c_t41_tmp = d * t2; + e_t41_tmp = L * t34; + A[3] = -(t41 * ((((((((e_t41_tmp + c_t41_tmp * t4 * t5) + d * t3 * t4 * t5) + c_t41_tmp * t5 * t7) + + g * mp * t2 * t4 * t5 * t6) + + i_t41_tmp * g * t4 * t5 * 2.0) + + d_t41_tmp * g * mp * t2 * t5 * 2.0) + + t31_tmp * t2 * t5 * t6 * t7) + + j_t41_tmp * g * mp * t5 * 2.0)); + A[4] = 0.0; + d = t39_tmp_tmp * M * g * l; + c_t41_tmp = + (((((((t17_tmp * g * l * mp * t2 + b_t18_tmp * g * l * t4 * 2.0) + t23_tmp * t2 * t4) + t23_tmp * t3 * t4) + + t31_tmp * l * mp * mw * t2 * t5) + + b_t33_tmp * t2 * t4 * t5) + + t33_tmp * mp * t3 * t4 * t5) + + b_t33_tmp * t3 * t4 * t5) + + t39_tmp * mw * t4 * t5 * 2.0; + A[5] = t41 * ((((c_t41_tmp + t39_tmp * mp * t4 * t5) + b_t18_tmp * M * g * l * mp) + d * t5 * t7) + d * mp * mw * t5); + A[6] = 1.0; + memset(&A[7], 0, 13U * sizeof(double)); + A[20] = 1.0; + A[21] = 0.0; + A[22] = 0.0; + A[23] = 0.0; + A[24] = 0.0; + A[25] = t41 * t41_tmp; + A[26] = 0.0; + d = Ip * g; + A[27] = -t41 * (e_t41_tmp - d * t4 * t5 * t6); + A[28] = 0.0; + e_t41_tmp = f_t41_tmp * g * l; + A[29] = t41 * ((((c_t41_tmp + k_t41_tmp * g * l) + d * l * t4 * t5) + e_t41_tmp * mp * t5) + e_t41_tmp * mw * t5); + A[30] = 0.0; + A[31] = 0.0; + A[32] = 0.0; + A[33] = 0.0; + A[34] = 1.0; + A[35] = 0.0; +} + +/* + * File trailer for gen_A.c + * + * [EOF] + */ diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_B.c b/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_B.c new file mode 100644 index 00000000..018d0671 --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_B.c @@ -0,0 +1,139 @@ +/* + * File: gen_B.c + * + * MATLAB Coder version : 5.5 + * C/C++ source code generated on : 08-Aug-2025 17:37:08 + */ + +/* Include Files */ +#include "bipedal_wheel_controller/dynamics/gen_B.h" + +/* Function Definitions */ +/* + * gen_B + * B = gen_B(Im,Ip,Iw,L,Lm,M,R,l,MP,MW) + * + * Arguments : double Im + * double Ip + * double Iw + * double L + * double Lm + * double M + * double R + * double l + * double mp + * double mw + * double B[12] + * Return Type : void + */ +void gen_B(double Im, double Ip, double Iw, double L, double Lm, double M, double R, double l, double mp, double mw, + double B[12]) +{ + double b_t41_tmp; + double d; + double t10; + double t10_tmp; + double t11; + double t11_tmp; + double t12; + double t13; + double t2; + double t23; + double t25; + double t25_tmp; + double t26; + double t26_tmp; + double t27; + double t28; + double t28_tmp; + double t29; + double t29_tmp; + double t3; + double t4; + double t41; + double t41_tmp; + double t5; + double t6; + double t8; + double t8_tmp; + double t9; + /* This function was generated by the Symbolic Math Toolbox version 9.2. + */ + /* 08-Aug-2025 17:33:03 */ + t2 = L * L; + t3 = Lm * Lm; + t4 = R * R; + t5 = l * l; + t6 = Im * Iw; + t8_tmp = Iw * L; + t8 = t8_tmp * M * l; + t9 = Iw * Lm * M * l; + t10_tmp = Im * M; + t10 = t10_tmp * t4; + t11_tmp = Iw * M; + t11 = t11_tmp * t5; + t12 = Im * mp * t4; + t13 = Im * mw * t4; + t25_tmp = L * M; + t25 = t25_tmp * l * mw * t4; + t26_tmp = Lm * M * l; + t26 = t26_tmp * mp * t4; + t27 = t26_tmp * mw * t4; + t28_tmp = M * mp; + t28 = t28_tmp * t4 * t5; + t29_tmp = M * mw; + t29 = t29_tmp * t4 * t5; + t23 = L * t10; + t26_tmp = mp * t2; + t41_tmp = mw * t2; + b_t41_tmp = L * Lm * M; + t41 = + 1.0 / + (((((((((((((((((Ip * t6 + Ip * t11) + t26_tmp * t6) + Ip * t12) + Ip * t13) + b_t41_tmp * t6 * 2.0) + M * t2 * t6) + + M * t3 * t6) + + Ip * t10) + + t41_tmp * t10) + + mp * t3 * t10) + + mw * t3 * t10) + + t26_tmp * t11) + + Ip * t28) + + Ip * t29) + + t41_tmp * t12) + + Lm * mw * t23 * 2.0) + + t41_tmp * t28); + B[0] = 0.0; + d = Im * L; + B[1] = -(t41 * ((((((((((t6 + t10) + t11) + t12) + t13) + t28) + t29) + d * M * R) + Im * Lm * M * R) + d * R * mp) + + t25_tmp * R * mp * t5)); + B[2] = 0.0; + t41_tmp = t10_tmp * R; + t10_tmp = Ip * M; + t25_tmp = t10_tmp * R; + t26_tmp = ((L * t12 + t23) + Lm * t10) + L * t28; + B[3] = t41 * (((((((t26_tmp + Im * Ip * R) + t41_tmp * t2) + t41_tmp * t3) + t25_tmp * t5) + Im * R * mp * t2) + + M * R * mp * t2 * t5) + + d * Lm * M * R * 2.0); + B[4] = 0.0; + d = (((t8 + t9) + t25) + t26) + t27; + B[5] = -(t41 * ((d - t25_tmp * l) + b_t41_tmp * R * l * mp)); + B[6] = 0.0; + B[7] = t41 * (((((((((((t6 + t8) + t9) + t10) + t11) + t12) + t13) + t25) + t26) + t27) + t28) + t29); + B[8] = 0.0; + B[9] = -(t41 * ((t26_tmp + L * t26) - t10_tmp * l * t4)); + B[10] = 0.0; + B[11] = + t41 * (((((((((((((d + Ip * Iw) + t10_tmp * t4) + t11_tmp * t2) + t11_tmp * t3) + Ip * mp * t4) + Iw * mp * t2) + + Ip * mw * t4) + + t28_tmp * t3 * t4) + + t29_tmp * t2 * t4) + + t29_tmp * t3 * t4) + + mp * mw * t2 * t4) + + t8_tmp * Lm * M * 2.0) + + b_t41_tmp * mw * t4 * 2.0); +} + +/* + * File trailer for gen_B.c + * + * [EOF] + */ diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_conv.c b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_conv.c new file mode 100644 index 00000000..454640c2 --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_conv.c @@ -0,0 +1,62 @@ +/* + * File: leg_conv.c + * + * MATLAB Coder version : 5.5 + * C/C++ source code generated on : 15-Sep-2025 13:09:38 + */ + +/* Include Files */ +#include "bipedal_wheel_controller/vmc/leg_conv.h" + +#include + +/* Function Definitions */ +/* + * LEG_CONV + * T = LEG_CONV(F,Tp,PHI1,PHI2) + * + * Arguments : double F + * double Tp + * double phi1 + * double phi2 + * double T[2] + * Return Type : void + */ +void leg_conv(double F, double Tp, double phi1, double phi2, double T[2]) +{ + Tp = -Tp; + double t11_tmp; + double t12_tmp; + double t21; + double t23; + double t30_tmp; + double t31; + double t32_tmp; + double t33; + double t34; + double t36_tmp; + /* This function was generated by the Symbolic Math Toolbox version 23.2. + */ + /* 2025-09-15 13:09:38 */ + t33 = phi1 + phi2; + t11_tmp = cos(t33); + t12_tmp = sin(t33); + t21 = t11_tmp * 0.26; + t23 = t12_tmp * 0.26; + t30_tmp = t23 + sin(phi1) * 0.218; + t31 = t30_tmp * t30_tmp; + t32_tmp = t21 + cos(phi1) * 0.218; + t33 = t32_tmp * t32_tmp; + t34 = 1.0 / t33; + t36_tmp = t31 + t33; + t33 = Tp * t33 * (1.0 / t36_tmp); + T[0] = t33 * (t31 * t34 + 1.0); + T[1] = F * (t12_tmp * t32_tmp * 0.52 - t11_tmp * t30_tmp * 0.52) * -0.5 / sqrt(t36_tmp) + + t33 * (t21 / t32_tmp + t30_tmp * t34 * t23); +} + +/* + * File trailer for leg_conv.c + * + * [EOF] + */ diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_conv_fwd.c b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_conv_fwd.c new file mode 100644 index 00000000..41ed5f64 --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_conv_fwd.c @@ -0,0 +1,127 @@ +/* + * File: leg_conv_fwd.c + * + * MATLAB Coder version : 5.5 + * C/C++ source code generated on : 15-Sep-2025 20:51:54 + */ + +/* Include Files */ +#include "bipedal_wheel_controller/vmc/leg_conv_fwd.h" + +#include + +/* Function Definitions */ +/* + * LEG_CONV_FWD + * T_real = LEG_CONV_FWD(T1,T2,PHI1,PHI2) + * + * Arguments : double T1 + * double T2 + * double phi1 + * double phi2 + * double T_real[2] + * Return Type : void + */ +void leg_conv_fwd(double T1, double T2, double phi1, double phi2, double T_real[2]) +{ + double T_real_tmp; + double b_T_real_tmp; + double c_T_real_tmp; + double d_T_real_tmp; + double e_T_real_tmp; + double f_T_real_tmp; + double g_T_real_tmp; + double h_T_real_tmp; + double i_T_real_tmp; + double j_T_real_tmp; + double k_T_real_tmp; + double l_T_real_tmp; + double m_T_real_tmp; + double n_T_real_tmp; + double t11_tmp; + double t19_tmp; + double t21_tmp; + double t23_tmp; + double t25_tmp; + double t4_tmp; + double t5_tmp; + double t7_tmp; + /* This function was generated by the Symbolic Math Toolbox version 23.2. + */ + /* 2025-09-15 20:51:54 */ + t4_tmp = cos(phi1); + t5_tmp = sin(phi1); + t7_tmp = phi1 + phi2; + t11_tmp = cos(t7_tmp); + t7_tmp = sin(t7_tmp); + t19_tmp = t4_tmp * t4_tmp; + t21_tmp = t5_tmp * t5_tmp; + t23_tmp = t11_tmp * t11_tmp; + t25_tmp = t7_tmp * t7_tmp; + T_real_tmp = T2 * 0.0 * 11881.0; + b_T_real_tmp = T1 * 0.0 * 16900.0; + c_T_real_tmp = T2 * 0.0 * 16900.0; + d_T_real_tmp = T1 * t4_tmp; + e_T_real_tmp = T2 * t4_tmp; + f_T_real_tmp = T1 * 0.0 * 0.0 * 14170.0; + g_T_real_tmp = e_T_real_tmp * 0.0; + h_T_real_tmp = T2 * 0.0 * 0.0 * 28340.0; + i_T_real_tmp = T1 * t5_tmp; + j_T_real_tmp = T2 * t5_tmp; + k_T_real_tmp = T1 * 0.0 * t7_tmp; + l_T_real_tmp = T2 * 0.0 * t7_tmp; + m_T_real_tmp = t4_tmp * t11_tmp; + n_T_real_tmp = t5_tmp * t7_tmp; + T_real[0] = + sqrt(((((m_T_real_tmp * 0.11336 + n_T_real_tmp * 0.11336) + t19_tmp * 0.047524) + t21_tmp * 0.047524) + + t23_tmp * 0.0676) + + t25_tmp * 0.0676) / + (t4_tmp * t7_tmp - t11_tmp * t5_tmp) * + (((((((((((((((((((((((T_real_tmp + T2 * t19_tmp * 11881.0) + T_real_tmp) - b_T_real_tmp) + T2 * t21_tmp * 11881.0) - + T1 * t23_tmp * 16900.0) + + c_T_real_tmp) - + b_T_real_tmp) + + T2 * t23_tmp * 16900.0) - + T1 * t25_tmp * 16900.0) + + c_T_real_tmp) + + T2 * t25_tmp * 16900.0) + + T2 * 0.0 * t5_tmp * 23762.0) - + g_T_real_tmp * 23762.0) - + f_T_real_tmp) + + h_T_real_tmp) - + k_T_real_tmp * 14170.0) - + d_T_real_tmp * t11_tmp * 14170.0) + + d_T_real_tmp * 0.0 * 14170.0) + + T1 * 0.0 * t11_tmp * 14170.0) - + i_T_real_tmp * 0.0 * 14170.0) + + l_T_real_tmp * 28340.0) + + (((((((((((e_T_real_tmp * t11_tmp * 28340.0 - f_T_real_tmp) - g_T_real_tmp * 28340.0) - + T2 * 0.0 * t11_tmp * 28340.0) + + j_T_real_tmp * 0.0 * 28340.0) + + h_T_real_tmp) - + i_T_real_tmp * t7_tmp * 14170.0) + + j_T_real_tmp * t7_tmp * 28340.0) - + k_T_real_tmp * 33800.0) + + T1 * t11_tmp * 0.0 * 33800.0) + + l_T_real_tmp * 33800.0) - + T2 * t11_tmp * 0.0 * 33800.0)) * + -17.642907551164431 / + (((((((((((((t19_tmp * 11881.0 + t21_tmp * 11881.0) + t23_tmp * 16900.0) + t25_tmp * 16900.0) + + 0.0 * t5_tmp * 23762.0) - + t4_tmp * 0.0 * 23762.0) + + 0.0 * t7_tmp * 28340.0) + + m_T_real_tmp * 28340.0) - + t4_tmp * 0.0 * 28340.0) - + 0.0 * t11_tmp * 28340.0) + + t5_tmp * 0.0 * 28340.0) + + n_T_real_tmp * 28340.0) + + 0.0 * t7_tmp * 33800.0) - + t11_tmp * 0.0 * 33800.0)); + T_real[1] = -T1; +} + +/* + * File trailer for leg_conv_fwd.c + * + * [EOF] + */ diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_pos.c b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_pos.c new file mode 100644 index 00000000..56ebf49b --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_pos.c @@ -0,0 +1,41 @@ +/* + * File: leg_pos.c + * + * MATLAB Coder version : 5.5 + * C/C++ source code generated on : 15-Sep-2025 20:28:02 + */ + +/* Include Files */ +#include "bipedal_wheel_controller/vmc/leg_pos.h" +#include + +/* Function Definitions */ +/* + * LEG_POS + * POS = LEG_POS(PHI1,PHI2) + * + * Arguments : double phi1 + * double phi2 + * double pos[2] + * Return Type : void + */ + +void leg_pos(double phi1, double phi2, double pos[2]) +{ + double a_tmp; + double t4; + /* This function was generated by the Symbolic Math Toolbox version 9.2. + */ + /* 15-Sep-2025 20:28:02 */ + t4 = phi1 + phi2; + a_tmp = cos(phi1) * 0.218 + cos(t4) * 0.26; + t4 = sin(phi1) * 0.218 + sin(t4) * 0.26; + pos[0] = sqrt(a_tmp * a_tmp + t4 * t4); + pos[1] = atan2(t4, a_tmp); +} + +/* + * File trailer for leg_pos.c + * + * [EOF] + */ diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_spd.c b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_spd.c new file mode 100644 index 00000000..5b484886 --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_spd.c @@ -0,0 +1,58 @@ +/* + * File: leg_spd.c + * + * MATLAB Coder version : 5.5 + * C/C++ source code generated on : 15-Sep-2025 20:28:02 + */ + +/* Include Files */ +#include "bipedal_wheel_controller/vmc/leg_spd.h" + +#include + +/* Function Definitions */ +/* + * LEG_SPD + * SPD = LEG_SPD(DPHI1,DPHI2,PHI1,PHI2) + * + * Arguments : double dphi1 + * double dphi2 + * double phi1 + * double phi2 + * double spd[2] + * Return Type : void + */ +void leg_spd(double dphi1, double dphi2, double phi1, double phi2, double spd[2]) +{ + double t29_tmp; + double t30_tmp; + double t32; + double t34; + double t35; + double t37; + double t37_tmp; + double t5; + double t9; + /* This function was generated by the Symbolic Math Toolbox version 23.2. + */ + /* 2025-09-15 20:28:02 */ + t5 = phi1 + phi2; + t9 = cos(t5); + t5 = sin(t5); + t29_tmp = cos(phi1) * 0.14 + t9 / 4.0; + t30_tmp = sin(phi1) * 0.14 + t5 / 4.0; + t32 = t30_tmp * t30_tmp; + t34 = t29_tmp * t29_tmp; + t35 = 1.0 / t34; + t37_tmp = t32 + t34; + t37 = 1.0 / t37_tmp; + spd[0] = dphi2 / sqrt(t37_tmp) * (t9 * t30_tmp / 2.0 - t5 * t29_tmp / 2.0) / 2.0; + spd[1] = + -(dphi2 * t34 * t37 * (t9 / 4.0 / t29_tmp + t30_tmp * t35 * (t5 / 4.0)) + dphi1 * t34 * t37 * (t32 * t35 + 1.0)); +} + +/* + * File trailer for leg_spd.c + * + * [EOF] + */ From 36002a1160c23eda0087c877f838ddbc6c081c7d Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Tue, 23 Sep 2025 12:54:28 +0800 Subject: [PATCH 02/57] Add VMCController. --- rm_chassis_controllers/CMakeLists.txt | 1 + .../series_legged_vmc_controller.h | 54 +++++++++ .../rm_chassis_controllers_plugins.xml | 8 +- .../series_legged_vmc_controller.cpp | 104 ++++++++++++++++++ .../test/load_vmc_controller.launch | 21 ++++ .../test/vmc_controller.yaml | 14 +++ 6 files changed, 201 insertions(+), 1 deletion(-) create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/series_legged_vmc_controller.h create mode 100644 rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp create mode 100644 rm_chassis_controllers/test/load_vmc_controller.launch create mode 100644 rm_chassis_controllers/test/vmc_controller.yaml diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt index 05f1804a..1ee41447 100644 --- a/rm_chassis_controllers/CMakeLists.txt +++ b/rm_chassis_controllers/CMakeLists.txt @@ -84,6 +84,7 @@ add_library(${PROJECT_NAME} src/bipedal_wheel_controller/controller_mode/recover.cpp src/bipedal_wheel_controller/controller_mode/normal.cpp src/bipedal_wheel_controller/controller.cpp + src/bipedal_wheel_controller/series_legged_vmc_controller.cpp ) ## Specify libraries to link executable targets against diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/series_legged_vmc_controller.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/series_legged_vmc_controller.h new file mode 100644 index 00000000..fdda179f --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/series_legged_vmc_controller.h @@ -0,0 +1,54 @@ +// +// Created by wiselook on 7/27/25. +// + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "bipedal_wheel_controller/vmc/leg_conv.h" +#include "bipedal_wheel_controller/vmc/leg_pos.h" +#include "bipedal_wheel_controller/vmc/leg_spd.h" + +#include + +namespace rm_chassis_controllers +{ +class VMCController : public controller_interface::MultiInterfaceController +{ +public: + VMCController() = default; + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; + void update(const ros::Time& time, const ros::Duration& period) override; + void starting(const ros::Time& /*time*/) override; + +private: + void commandLegLengthCB(const std_msgs::Float64ConstPtr& msg) + { + lengthCmd_ = msg->data; + } + + void commandLegAngleCB(const std_msgs::Float64ConstPtr& msg) + { + angleCmd_ = msg->data; + } + + hardware_interface::JointHandle jointThigh_, jointKnee_; + control_toolbox::Pid pidLength_, pidAngle_; + + double vmcBiasAngle_; + double lengthCmd_, angleCmd_; + + ros::Publisher statePublisher_, jointCmdStatePublisher_; + ros::Subscriber cmdLegLengthSubscriber_, cmdLegAngleSubscriber_; +}; + +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/rm_chassis_controllers_plugins.xml b/rm_chassis_controllers/rm_chassis_controllers_plugins.xml index 020d5d80..d1f951d5 100644 --- a/rm_chassis_controllers/rm_chassis_controllers_plugins.xml +++ b/rm_chassis_controllers/rm_chassis_controllers_plugins.xml @@ -40,6 +40,12 @@ EffortJointInterface type of hardware interface. - + + + The two link vmc controller. + + diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp new file mode 100644 index 00000000..3ba079c9 --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp @@ -0,0 +1,104 @@ +// +// Created by wiselook on 7/27/25. +// +#include "bipedal_wheel_controller/series_legged_vmc_controller.h" +#include +#include +#include +#include + +namespace rm_chassis_controllers +{ +bool VMCController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) +{ + cmdLegLengthSubscriber_ = + controller_nh.subscribe("command/leg_length", 1, &VMCController::commandLegLengthCB, this); + cmdLegAngleSubscriber_ = + controller_nh.subscribe("command/leg_angle", 1, &VMCController::commandLegAngleCB, this); + statePublisher_ = controller_nh.advertise("state", 1); + jointCmdStatePublisher_ = controller_nh.advertise("joint_cmd_state", 1); + if (controller_nh.hasParam("pid_length")) + { + if (!pidLength_.init(ros::NodeHandle(controller_nh, "pid_length"))) + { + ROS_ERROR("Load param fail, check the resist of pid_length"); + return false; + } + } + if (controller_nh.hasParam("pid_angle")) + { + if (!pidAngle_.init(ros::NodeHandle(controller_nh, "pid_angle"))) + { + ROS_ERROR("Load param fail, check the resist of pid_angle"); + return false; + } + } + if (!controller_nh.getParam("vmc_bias_angle", vmcBiasAngle_)) + { + ROS_ERROR("Load param fail, check the resist of vmc_bias_angle"); + return false; + } + std::string thighJoint, kneeJoint; + if (!(controller_nh.getParam("thigh_joint", thighJoint) && controller_nh.getParam("knee_joint", kneeJoint))) + { + ROS_ERROR("Load param fail, check the resist of thigh_joint or knee_joint"); + return false; + } + jointThigh_ = robot_hw->get()->getHandle(thighJoint); + jointKnee_ = robot_hw->get()->getHandle(kneeJoint); + return true; +} + +void VMCController::starting(const ros::Time& /*time*/) +{ + angleCmd_ = 0.; + lengthCmd_ = 0.18; +} + +void VMCController::update(const ros::Time& time, const ros::Duration& period) +{ + double knee_angle = 0, thigh_angle = 0, position[2], speed[2]; + + // [0]:thigh_vmc_joint [1]:knee_vmc_joint + thigh_angle = jointThigh_.getPosition() + M_PI / 2.; + knee_angle = jointKnee_.getPosition() - 1.6581; + leg_pos(thigh_angle, knee_angle, position); + leg_spd(jointThigh_.getVelocity(), jointKnee_.getVelocity(), thigh_angle, knee_angle, speed); + + double effortCmd[2], jointCmd[2]; + static double angleSinCmd_ = 0; + angleSinCmd_ -= 0.001; + if (angleSinCmd_ <= -M_PI) + { + angleSinCmd_ = M_PI; + } + // angleCmd_ = angleSinCmd_; + double angle_error = angles::shortest_angular_distance(position[1], angleCmd_); + + effortCmd[0] = pidLength_.computeCommand(lengthCmd_ - position[0], period); + effortCmd[1] = -pidAngle_.computeCommand(angle_error, period); + + leg_conv(effortCmd[0], effortCmd[1], thigh_angle, knee_angle, jointCmd); + + std_msgs::Float64MultiArray state; + state.data.push_back(thigh_angle); + state.data.push_back(knee_angle); + state.data.push_back(position[0]); + state.data.push_back(position[1]); + state.data.push_back(speed[0]); + state.data.push_back(speed[1]); + state.data.push_back(angle_error); + statePublisher_.publish(state); + + std_msgs::Float64MultiArray jointCmdState; + jointCmdState.data.push_back(jointCmd[0]); + jointCmdState.data.push_back(jointCmd[1]); + jointCmdStatePublisher_.publish(jointCmdState); + + jointThigh_.setCommand(jointCmd[0]); + jointKnee_.setCommand(jointCmd[1]); +} + +} // namespace rm_chassis_controllers + +PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::VMCController, controller_interface::ControllerBase) diff --git a/rm_chassis_controllers/test/load_vmc_controller.launch b/rm_chassis_controllers/test/load_vmc_controller.launch new file mode 100644 index 00000000..e103af3d --- /dev/null +++ b/rm_chassis_controllers/test/load_vmc_controller.launch @@ -0,0 +1,21 @@ + + + + + + + false + + + + + + + diff --git a/rm_chassis_controllers/test/vmc_controller.yaml b/rm_chassis_controllers/test/vmc_controller.yaml new file mode 100644 index 00000000..b247be8a --- /dev/null +++ b/rm_chassis_controllers/test/vmc_controller.yaml @@ -0,0 +1,14 @@ +controllers: + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 + robot_state_controller: + type: robot_state_controller/RobotStateController + publish_rate: 100 + vmc_controller: + type: rm_chassis_controllers/VMCController + vmc_bias_angle: 1.57 + pid_length: { p: 900.0, i: 0, d: 40, i_clamp_max: 15.0, i_clamp_min: -15.0, antiwindup: true, publish_state: true } + pid_angle: { p: 25.0, i: 0, d: 5, i_clamp_max: 15.0, i_clamp_min: -15.0, antiwindup: true, publish_state: true } + thigh_joint: right_hip_joint + knee_joint: right_knee_joint From 2036ecc7f8f071cb462bad48291a65d2e83a84ae Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sun, 12 Oct 2025 11:15:27 +0800 Subject: [PATCH 03/57] feat: add bias and pid_yaw_pos. --- .../bipedal_wheel_controller/controller.h | 2 ++ .../controller_mode/mode_base.h | 2 ++ .../controller_mode/mode_manager.h | 2 +- .../controller_mode/normal.h | 8 +++-- .../bipedal_wheel_controller/definitions.h | 7 +++++ .../bipedal_wheel_controller/controller.cpp | 31 ++++++++++++++++--- .../controller_mode/mode_base.cpp | 2 ++ .../controller_mode/mode_manager.cpp | 6 ++-- .../controller_mode/normal.cpp | 12 +++++-- 9 files changed, 58 insertions(+), 14 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h index 97dabec6..e46f8e78 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h @@ -54,6 +54,7 @@ class BipedalController : public ChassisBase>& Ks, const std::vector& L0s, Eigen::Matrix& coeffs); geometry_msgs::Twist odometry() override; @@ -62,6 +63,7 @@ class BipedalController : public ChassisBase r_{}; std::shared_ptr model_params_; + std::shared_ptr bias_params_; int balance_mode_ = BalanceMode::SIT_DOWN; bool balance_state_changed_ = false; diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_base.h index ae678d9b..aa6b392f 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_base.h @@ -7,6 +7,7 @@ #include #include #include +#include #include "bipedal_wheel_controller/definitions.h" @@ -49,6 +50,7 @@ class ModeBase 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 diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_manager.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_manager.h index 4a685e68..c66f18c4 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_manager.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_manager.h @@ -32,7 +32,7 @@ class ModeManager std::shared_ptr mode_impl; std::map> mode_map_; - control_toolbox::Pid pid_yaw_vel_, pid_left_leg_, pid_right_leg_, pid_theta_diff_, pid_roll_; + control_toolbox::Pid pid_yaw_pos_, pid_yaw_vel_, pid_left_leg_, pid_right_leg_, pid_theta_diff_, pid_roll_; control_toolbox::Pid pid_left_leg_theta_, pid_right_leg_theta_; control_toolbox::Pid pid_left_wheel_vel_, pid_right_wheel_vel_; std::vector pid_wheels_, pid_legs_, pid_thetas_; diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h index f317ec30..98fcf8a9 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h @@ -18,8 +18,9 @@ class Normal : public ModeBase { public: Normal(const std::vector& joint_handles, - const std::vector& pid_legs, const control_toolbox::Pid& pid_yaw_vel, - const control_toolbox::Pid& pid_theta_diff, const control_toolbox::Pid& pid_roll); + const std::vector& pid_legs, const control_toolbox::Pid& pid_yaw_pos, + const control_toolbox::Pid& pid_yaw_vel, const control_toolbox::Pid& pid_theta_diff, + const control_toolbox::Pid& pid_roll); void execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) override; const char* name() const override { @@ -36,10 +37,11 @@ class Normal : public ModeBase Eigen::Matrix x); std::vector joint_handles_; std::vector pid_legs_; - control_toolbox::Pid pid_yaw_vel_, pid_theta_diff_, pid_roll_; + control_toolbox::Pid pid_yaw_pos_, pid_yaw_vel_, pid_theta_diff_, pid_roll_; int jump_phase_ = JumpPhase::SQUAT; bool start_jump_ = false; std::unique_ptr> supportForceAveragePtr; + double yaw_des_; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h index 70266b5e..89f86c76 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h @@ -24,6 +24,13 @@ struct ModelParams double g; // Gravity acceleration }; +struct BiasParams +{ + double x; + double pitch; + double roll; +}; + struct LegCommand { double force; // Thrust diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp index dc3a52bc..e55192c9 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp @@ -37,8 +37,9 @@ bool BipedalController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan mode_manager_ = std::make_unique(controller_nh, joint_handles_); model_params_ = std::make_shared(); + bias_params_ = std::make_shared(); - if (!setupModelParams(controller_nh) || !setupLQR(controller_nh)) + if (!setupModelParams(controller_nh) || !setupLQR(controller_nh) || !setupBiasParams(controller_nh)) return false; auto legCmdCallback = [this](const rm_msgs::LegCmd::ConstPtr& msg) { @@ -109,12 +110,14 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat // vmc double left_angle[2]{}, right_angle[2]{}, left_pos[2]{}, left_spd[2]{}, right_pos[2]{}, right_spd[2]{}; // [0]:hip_vmc_joint [1]:knee_vmc_joint - left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI / 2.; + left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; // left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI / 4.; - left_angle[1] = left_knee_joint_handle_.getPosition() - 1.6581; - right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI / 2.; + // left_angle[1] = left_knee_joint_handle_.getPosition() - 1.6581; + left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; + right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; // right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI / 4.; - right_angle[1] = right_knee_joint_handle_.getPosition() - 1.6581; + // right_angle[1] = right_knee_joint_handle_.getPosition() - 1.6581; + right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; // [0] is length, [1] is angle leg_pos(left_angle[0], left_angle[1], left_pos); leg_pos(right_angle[0], right_angle[1], right_pos); @@ -131,6 +134,7 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat x_left[2] += x_left[3] * period.toSec(); else x_left[2] = 0.; + x_left[2] -= bias_params_->x; x_left[0] = left_pos[1] + pitch; x_left[1] = -left_spd[1] + angular_vel_base.y; x_left[4] = -pitch; @@ -225,6 +229,23 @@ bool BipedalController::setupLQR(ros::NodeHandle& controller_nh) return true; } +bool BipedalController::setupBiasParams(ros::NodeHandle& controller_nh) +{ + const std::pair tbl[] = { + { "x_bias", &bias_params_->x }, + { "pitch_bias", &bias_params_->pitch }, + { "roll_bias", &bias_params_->roll }, + }; + + for (const auto& e : tbl) + if (!controller_nh.getParam(e.first, *e.second)) + { + ROS_ERROR("Param %s not given (namespace: %s)", e.first, controller_nh.getNamespace().c_str()); + return false; + } + return true; +} + void BipedalController::polyfit(const std::vector>& Ks, const std::vector& L0s, Eigen::Matrix& coeffs) { diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_base.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_base.cpp index 0fa8feff..ffc07e83 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_base.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_base.cpp @@ -33,6 +33,8 @@ void ModeBase::updateBaseState(const geometry_msgs::Vector3& angular_vel_base, roll_ = roll; pitch_ = pitch; yaw_ = yaw; + yaw_total_last_ = yaw_total_; + yaw_total_ = yaw_total_last_ + angles::shortest_angular_distance(yaw_total_last_, yaw_); } void ModeBase::updateUnstick(const bool& left_unstick, const bool& right_unstick) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp index 7b004eea..85fba227 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp @@ -19,6 +19,7 @@ ModeManager::ModeManager(ros::NodeHandle& controller_nh, { "pid_right_leg_theta", &pid_right_leg_theta_ }, { "pid_left_wheel_vel", &pid_left_wheel_vel_ }, { "pid_right_wheel_vel", &pid_right_wheel_vel_ }, + { "pid_yaw_pos", &pid_yaw_pos_ }, }; for (const auto& e : pids) if (controller_nh.hasParam(e.first) && !e.second->init(ros::NodeHandle(controller_nh, e.first))) @@ -30,8 +31,9 @@ ModeManager::ModeManager(ros::NodeHandle& controller_nh, pid_thetas_.push_back(&pid_left_leg_theta_); pid_thetas_.push_back(&pid_right_leg_theta_); - mode_map_.insert(std::make_pair(BalanceMode::NORMAL, std::make_unique(joint_handles, pid_legs_, pid_yaw_vel_, - pid_theta_diff_, pid_roll_))); + mode_map_.insert( + std::make_pair(BalanceMode::NORMAL, std::make_unique(joint_handles, pid_legs_, pid_yaw_pos_, pid_yaw_vel_, + pid_theta_diff_, pid_roll_))); mode_map_.insert( std::make_pair(BalanceMode::STAND_UP, std::make_unique(joint_handles, pid_legs_, pid_thetas_))); mode_map_.insert( diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp index bd888348..adf0dec2 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -8,10 +8,12 @@ namespace rm_chassis_controllers { Normal::Normal(const std::vector& joint_handles, - const std::vector& pid_legs, const control_toolbox::Pid& pid_yaw_vel, - const control_toolbox::Pid& pid_theta_diff, const control_toolbox::Pid& pid_roll) + const std::vector& pid_legs, const control_toolbox::Pid& pid_yaw_pos, + const control_toolbox::Pid& pid_yaw_vel, const control_toolbox::Pid& pid_theta_diff, + const control_toolbox::Pid& pid_roll) : joint_handles_(joint_handles) , pid_legs_(pid_legs) + , pid_yaw_pos_(pid_yaw_pos) , pid_yaw_vel_(pid_yaw_vel) , pid_theta_diff_(pid_theta_diff) , pid_roll_(pid_roll) @@ -24,6 +26,8 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const if (!controller->getStateChange()) { ROS_INFO("[balance] Enter NORMAL"); + yaw_total_ = yaw_total_last_ = 0.; + yaw_des_ = yaw_total_; controller->setStateChange(true); } if (!controller->getCompleteStand() && abs(x_left_[4]) < 0.2) @@ -31,8 +35,10 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const auto vel_cmd_ = controller->getVelCmd(); + yaw_des_ += vel_cmd_.z * period.toSec(); // PID - double T_yaw = pid_yaw_vel_.computeCommand(vel_cmd_.z - angular_vel_base_.z, period); + double vel_yaw_ref = pid_yaw_pos_.computeCommand(yaw_des_ - yaw_total_, period); + double T_yaw = pid_yaw_vel_.computeCommand(vel_yaw_ref - angular_vel_base_.z, period); double T_theta_diff = pid_theta_diff_.computeCommand(left_pos_[1] - right_pos_[1], period); double T_roll = pid_roll_.computeCommand(0. - roll_, period); From 2be8e3c272410a3b8f3b6e49dc4894722a196e8c Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sun, 12 Oct 2025 11:16:01 +0800 Subject: [PATCH 04/57] feat: add vmc controller for test leg. --- .../series_legged_vmc_controller.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp index 3ba079c9..d40e5758 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp @@ -60,8 +60,9 @@ void VMCController::update(const ros::Time& time, const ros::Duration& period) double knee_angle = 0, thigh_angle = 0, position[2], speed[2]; // [0]:thigh_vmc_joint [1]:knee_vmc_joint - thigh_angle = jointThigh_.getPosition() + M_PI / 2.; - knee_angle = jointKnee_.getPosition() - 1.6581; + thigh_angle = jointThigh_.getPosition() + M_PI_2; + // knee_angle = jointKnee_.getPosition() - 1.6581; + knee_angle = jointKnee_.getPosition() - M_PI_2; leg_pos(thigh_angle, knee_angle, position); leg_spd(jointThigh_.getVelocity(), jointKnee_.getVelocity(), thigh_angle, knee_angle, speed); From 9bc676107e51a74b7a0aa578502e5b899d1667ba Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sat, 25 Oct 2025 12:53:50 +0800 Subject: [PATCH 05/57] Add slip detection and publish lqr_status. --- .../bipedal_wheel_controller/controller.h | 27 +++- .../bipedal_wheel_controller/controller.cpp | 119 ++++++++++++++---- 2 files changed, 117 insertions(+), 29 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h index e46f8e78..bbfd5314 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h @@ -5,7 +5,11 @@ #pragma once #include -#include "rm_msgs/LegCmd.h" +#include +#include +#include +#include +#include #include #include #include @@ -41,6 +45,7 @@ class BipedalController : public ChassisBase& getModelParams(){ return model_params_; } double getLegCmd() const{ return legCmd_; } double getJumpCmd() const{ return jumpCmd_; } + int getBaseState() const{ return state_; } geometry_msgs::Vector3 getVelCmd(){ return vel_cmd_; } void setStateChange(bool state){ balance_state_changed_ = state; } @@ -48,6 +53,8 @@ class BipedalController : public ChassisBase left_error, Eigen::Matrix right_error, + Eigen::Matrix u_left, Eigen::Matrix u_right) const; // clang-format on private: @@ -69,6 +76,15 @@ class BipedalController : public ChassisBase mode_manager_; + // Slippage_detection + double leftWheelVel{}, rightWheelVel{}, leftWheelVelAbsolute{}, rightWheelVelAbsolute{}; + int i = 0, sample_times_ = 3; + Eigen::Matrix A_, B_, H_, Q_, R_; + Eigen::Matrix X_, U_; + std::shared_ptr> kalmanFilterPtr_; + + Eigen::Matrix x_left_{}, x_right_{}; + // stand up bool complete_stand_ = false, overturn_ = false; @@ -79,12 +95,17 @@ class BipedalController : public ChassisBase joint_handles_; // Leg Cmd - double legCmd_{}; - bool jumpCmd_{}; + double legCmd_{ 0.2 }; + bool jumpCmd_{ false }; + + // ros msg + rm_msgs::LeggedChassisStatus legged_chassis_status_msg; // ROS Interface ros::Subscriber leg_cmd_sub_; ros::Publisher unstick_pub_; + ros::Publisher legged_chassis_status_pub_; + ros::Publisher lqr_status_pub_; ros::Time cmd_update_time_; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp index e55192c9..2ce6564c 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp @@ -49,8 +49,22 @@ bool BipedalController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan leg_cmd_sub_ = controller_nh.subscribe("/leg_cmd", 1, legCmdCallback); unstick_pub_ = controller_nh.advertise("unstick", 1); + legged_chassis_status_pub_ = controller_nh.advertise("legged_chassis_status", 1); + lqr_status_pub_ = controller_nh.advertise("lqr_status", 1); + x_left_.setZero(); + x_right_.setZero(); + + // Slippage detection + A_ << 1, 0, 0, 1; + H_ << 1, 0, 0, 1; + Q_ << 1, 0, 0, 1; + R_ << 150, 0, 0, 150; + B_.setZero(); + X_.setZero(); + U_.setZero(); + kalmanFilterPtr_ = std::make_shared>(A_, B_, H_, Q_, R_); + kalmanFilterPtr_->clear(X_); - ROS_INFO("chassis_controller init done!"); return true; } @@ -110,14 +124,11 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat // vmc double left_angle[2]{}, right_angle[2]{}, left_pos[2]{}, left_spd[2]{}, right_pos[2]{}, right_spd[2]{}; // [0]:hip_vmc_joint [1]:knee_vmc_joint - left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; - // left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI / 4.; - // left_angle[1] = left_knee_joint_handle_.getPosition() - 1.6581; - left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; - right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; - // right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI / 4.; - // right_angle[1] = right_knee_joint_handle_.getPosition() - 1.6581; - right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; + left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; + left_angle[1] = left_knee_joint_handle_.getPosition(); + right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; + right_angle[1] = right_knee_joint_handle_.getPosition(); + // [0] is length, [1] is angle leg_pos(left_angle[0], left_angle[1], left_pos); leg_pos(right_angle[0], right_angle[1], right_pos); @@ -126,24 +137,60 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat leg_spd(right_hip_joint_handle_.getVelocity(), right_knee_joint_handle_.getVelocity(), right_angle[0], right_angle[1], right_spd); + // Slippage_detection + leftWheelVel = (left_wheel_joint_handle_.getVelocity() - angular_vel_base.y + left_spd[1]) * wheel_radius_; + rightWheelVel = (right_wheel_joint_handle_.getVelocity() + angular_vel_base.y + right_spd[1]) * wheel_radius_; + leftWheelVelAbsolute = + leftWheelVel + left_pos[0] * left_spd[1] * cos(left_pos[1] + pitch_) + left_spd[0] * sin(left_pos[1] + pitch_); + rightWheelVelAbsolute = rightWheelVel + right_pos[0] * right_spd[1] * cos(right_pos[1] + pitch_) + + right_spd[0] * sin(right_pos[1] + pitch_); + + double wheel_vel_aver = (leftWheelVelAbsolute + rightWheelVelAbsolute) / 2.; + if (i >= sample_times_) + { // oversampling + i = 0; + X_(0) = wheel_vel_aver; + X_(1) = linear_acc_base.x; + kalmanFilterPtr_->predict(U_); + kalmanFilterPtr_->update(X_); + } + else + { + kalmanFilterPtr_->predict(U_); + i++; + } + auto x_hat_vel = kalmanFilterPtr_->getState(); + // update state - Eigen::Matrix x_left{}, x_right{}; - x_left[3] = - (left_wheel_joint_handle_.getVelocity() + right_wheel_joint_handle_.getVelocity()) / 2.0 * model_params_->r; - if (abs(x_left[3]) < 0.4 && vel_cmd_.x == 0.) - x_left[2] += x_left[3] * period.toSec(); + x_left_[3] = x_hat_vel(0); + if (abs(x_left_[3]) < 0.3 && vel_cmd_.x == 0.0) + x_left_[2] += x_left_[3] * period.toSec(); else - x_left[2] = 0.; - x_left[2] -= bias_params_->x; - x_left[0] = left_pos[1] + pitch; - x_left[1] = -left_spd[1] + angular_vel_base.y; - x_left[4] = -pitch; - x_left[5] = -angular_vel_base.y; - x_right = x_left; - x_right[0] = right_pos[1] + pitch; - x_right[1] = -right_spd[1] + angular_vel_base.y; - - mode_manager_->getModeImpl()->updateEstimation(x_left, x_right); + x_left_[2] = 0.; + x_left_[2] -= bias_params_->x; + x_left_[0] = (left_pos[1] + pitch); + x_left_[1] = left_spd[1] + angular_vel_base.y; + x_left_[4] = -pitch; + x_left_[5] = -angular_vel_base.y; + x_right_ = x_left_; + x_right_[0] = (right_pos[1] + pitch); + x_right_[1] = right_spd[1] + angular_vel_base.y; + + legged_chassis_status_msg.roll = roll; + legged_chassis_status_msg.pitch = x_left_[4]; + legged_chassis_status_msg.d_pitch = x_left_[5]; + legged_chassis_status_msg.yaw = yaw; + legged_chassis_status_msg.d_yaw = angular_vel_base.z; + legged_chassis_status_msg.left_leg_length = left_pos[0]; + legged_chassis_status_msg.right_leg_length = right_pos[0]; + legged_chassis_status_msg.x = x_left_[2]; + legged_chassis_status_msg.x_dot = x_left_[3]; + legged_chassis_status_msg.left_leg_theta = x_left_[0]; + legged_chassis_status_msg.left_leg_theta_dot = x_left_[1]; + legged_chassis_status_msg.right_leg_theta = x_right_[0]; + legged_chassis_status_msg.right_leg_theta_dot = x_right_[1]; + + mode_manager_->getModeImpl()->updateEstimation(x_left_, x_right_); mode_manager_->getModeImpl()->updateLegKinematics(left_angle, right_angle, left_pos, left_spd, right_pos, right_spd); mode_manager_->getModeImpl()->updateBaseState(angular_vel_base, linear_acc_base, roll, pitch, yaw); } @@ -153,6 +200,7 @@ void BipedalController::pubState() std_msgs::Bool msg; msg.data = mode_manager_->getModeImpl()->getUnstick(); unstick_pub_.publish(msg); + legged_chassis_status_pub_.publish(legged_chassis_status_msg); } void BipedalController::stopping(const ros::Time& time) @@ -209,7 +257,7 @@ bool BipedalController::setupLQR(ros::NodeHandle& controller_nh) // Continuous model \dot{x} = A x + B u std::vector lengths; std::vector> ks; - for (int i = 5; i < 30; i++) + for (int i = 10; i < 40; i++) { double length = i / 100.; lengths.push_back(length); @@ -276,5 +324,24 @@ geometry_msgs::Twist BipedalController::odometry() return twist; } +void BipedalController::pubLQRStatus(Eigen::Matrix left_error, + Eigen::Matrix right_error, + Eigen::Matrix u_left, + Eigen::Matrix u_right) const +{ + rm_msgs::LeggedLQRStatus msg; + for (int i = 0; i < 6; ++i) + { + msg.left_leg_error.push_back(left_error(i)); + msg.right_leg_error.push_back(right_error(i)); + } + for (int i = 0; i < 2; ++i) + { + msg.left_leg_u.push_back(u_left(i)); + msg.right_leg_u.push_back(u_right(i)); + } + lqr_status_pub_.publish(msg); +} + } // namespace rm_chassis_controllers PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::BipedalController, controller_interface::ControllerBase) From 4b91581dd3dd5d6960d92ea09f6fd6a29e523aac Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sat, 25 Oct 2025 12:56:05 +0800 Subject: [PATCH 06/57] Modify threshold and add leg_info. --- .../helper_functions.h | 21 ++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h index 9f01d877..a1fc5b75 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h @@ -62,13 +62,24 @@ inline void generateAB(const std::shared_ptr& model_params, Eigen:: */ inline void detectLegState(const Eigen::Matrix& x, int& leg_state) { - if (x[0] > -M_PI / 2 + 0.1 && x[0] < M_PI / 2 - 0.3) + if (x[0] > -M_PI / 2 + 0.4 && x[0] < M_PI / 2 - 0.4) leg_state = LegState::UNDER; - else if (x[0] < -M_PI / 2 + 0.1 && x[0] > -M_PI) + else if (x[0] < -M_PI / 2 + 0.4 && x[0] > -M_PI) leg_state = LegState::FRONT; - else if (x[0] > M_PI / 2 - 0.3 && x[0] < M_PI) + else if (x[0] > M_PI / 2 - 0.4 && x[0] < M_PI) leg_state = LegState::BEHIND; - ROS_INFO("[balance] Leg state: %d", leg_state); + switch (leg_state) + { + case LegState::UNDER: + ROS_INFO("[balance] Leg state: UNDER"); + break; + case LegState::FRONT: + ROS_INFO("[balance] Leg state: FRONT"); + break; + case LegState::BEHIND: + ROS_INFO("[balance] Leg state: BEHIND"); + break; + } } /** @@ -90,7 +101,7 @@ inline LegCommand computePidLegCommand(double desired_length, double desired_ang { LegCommand cmd; cmd.force = length_pid.computeCommand(desired_length - current_length, period); - cmd.torque = -angle_pid.computeCommand(-angles::shortest_angular_distance(desired_angle, current_angle), period); + cmd.torque = angle_pid.computeCommand(-angles::shortest_angular_distance(desired_angle, current_angle), period); leg_conv(cmd.force, cmd.torque, leg_angle[0], leg_angle[1], cmd.input); return cmd; } From ed497d7d7630eff6964c65024e3c81eae17f3e2c Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sat, 25 Oct 2025 12:59:11 +0800 Subject: [PATCH 07/57] Modify threshold. --- .../controller_mode/recover.cpp | 6 +++--- .../controller_mode/sit_down.cpp | 2 +- .../controller_mode/stand_up.cpp | 12 +++++++----- 3 files changed, 11 insertions(+), 9 deletions(-) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp index 5554ef5f..935c9963 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp @@ -27,11 +27,11 @@ void Recover::execute(BipedalController* controller, const ros::Time& time, cons detectLegState(x_left_, left_leg_state); detectLegState(x_right_, right_leg_state); if (controller->getOverturn() && left_leg_state != LegState::FRONT) - left_cmd = computePidLegCommand(0.4, -M_PI / 2 - 0.8, left_pos_[0], left_pos_[1], *pid_legs_[0], *pid_thetas_[0], + left_cmd = computePidLegCommand(0.36, -M_PI / 2 - 0.8, left_pos_[0], left_pos_[1], *pid_legs_[0], *pid_thetas_[0], left_angle_, period); if (controller->getOverturn() && right_leg_state != LegState::FRONT) - right_cmd = computePidLegCommand(0.4, -M_PI / 2 - 0.8, right_pos_[0], right_pos_[1], *pid_legs_[1], *pid_thetas_[1], - right_angle_, period); + right_cmd = computePidLegCommand(0.36, -M_PI / 2 - 0.8, right_pos_[0], right_pos_[1], *pid_legs_[1], + *pid_thetas_[1], right_angle_, period); setJointCommands(joint_handles_, left_cmd, right_cmd); // Exit diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp index 7d06e95e..71b04cfd 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp @@ -27,7 +27,7 @@ void SitDown::execute(BipedalController* controller, const ros::Time& time, cons setJointCommands(joint_handles_, left_cmd, right_cmd, left_wheel_cmd, right_wheel_cmd); // Exit - if (abs(x_left_(1)) < 0.1 && abs(x_left_(5)) < 0.1 && abs(x_left_(3)) < 0.15) + if (abs(x_left_(1)) < 0.1 && abs(x_left_(5)) < 0.1 && abs(x_left_(3)) < 0.15 && controller->getBaseState() != 4) { if (!controller->getOverturn()) controller->setMode(BalanceMode::STAND_UP); diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp index d1eabd22..3e6470e4 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp @@ -24,6 +24,8 @@ void StandUp::execute(BipedalController* controller, const ros::Time& time, cons controller->setCompleteStand(false); detectLegState(x_left_, left_leg_state); detectLegState(x_right_, right_leg_state); + ROS_INFO("x_left[0]: %f, x_left[1]: %f, x_left[2]: %f, x_left[3]: %f, x_left[4]: %f, x_left[5]: %f\n", x_left_[0], + x_left_[1], x_left_[2], x_left_[3], x_left_[4], x_left_[5]); } double theta_des_l, theta_des_r, length_des_l, length_des_r; @@ -37,9 +39,9 @@ void StandUp::execute(BipedalController* controller, const ros::Time& time, cons setJointCommands(joint_handles_, left_cmd, right_cmd); // Exit - if (((left_pos_[1] < 0. && left_leg_state == LegState::BEHIND) || + if (((left_pos_[1] < 0.39 && left_leg_state == LegState::BEHIND) || (left_pos_[1] > 0. && left_leg_state == LegState::UNDER)) && - ((right_pos_[1] < 0. && right_leg_state == LegState::BEHIND) || + ((right_pos_[1] < 0.39 && right_leg_state == LegState::BEHIND) || (right_pos_[1] > 0. && right_leg_state == LegState::UNDER))) { controller->setMode(BalanceMode::NORMAL); @@ -56,11 +58,11 @@ void StandUp::setUpLegMotion(const Eigen::Matrix& x, const { case LegState::UNDER: theta_des = 0.15; - length_des = 0.10; + length_des = 0.2; break; case LegState::FRONT: theta_des = M_PI / 2 + 0.2; - length_des = 0.33; + length_des = 0.36; if (abs(angles::shortest_angular_distance(x[0], M_PI / 2)) < 0.2 && abs(x[4]) < 0.1) leg_state = LegState::BEHIND; break; @@ -70,7 +72,7 @@ void StandUp::setUpLegMotion(const Eigen::Matrix& x, const if (other_leg_state != LegState::FRONT) { theta_des = 0.; - length_des = 0.10; + length_des = 0.2; } break; } From 83327956783283fa93309f9139a36fcfaf48ffff Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sat, 25 Oct 2025 13:07:47 +0800 Subject: [PATCH 08/57] Modify threshold and symbol. --- .../controller_mode/normal.cpp | 57 +++++++++++-------- 1 file changed, 32 insertions(+), 25 deletions(-) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp index adf0dec2..42dea1f4 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -28,6 +28,7 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const ROS_INFO("[balance] Enter NORMAL"); yaw_total_ = yaw_total_last_ = 0.; yaw_des_ = yaw_total_; + x_left_(2) = x_right_(2) = 0; controller->setStateChange(true); } if (!controller->getCompleteStand() && abs(x_left_[4]) < 0.2) @@ -35,28 +36,32 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const auto vel_cmd_ = controller->getVelCmd(); - yaw_des_ += vel_cmd_.z * period.toSec(); // PID - double vel_yaw_ref = pid_yaw_pos_.computeCommand(yaw_des_ - yaw_total_, period); - double T_yaw = pid_yaw_vel_.computeCommand(vel_yaw_ref - angular_vel_base_.z, period); - double T_theta_diff = pid_theta_diff_.computeCommand(left_pos_[1] - right_pos_[1], period); + double T_yaw = pid_yaw_vel_.computeCommand(vel_cmd_.z - angular_vel_base_.z, period); + double T_theta_diff = pid_theta_diff_.computeCommand(right_pos_[1] - left_pos_[1], period); double T_roll = pid_roll_.computeCommand(0. - roll_, period); // LQR - auto coeffs_ = controller->getCoeffs(); - Eigen::Matrix k_left{}, k_right{}; - for (int i = 0; i < 2; i++) - for (int j = 0; j < 6; j++) + Matrix coeffs_ = controller->getCoeffs(); + Matrix k_left, k_right; + k_left.setZero(); + k_right.setZero(); + + for (int i = 0; i < 2; ++i) + { + for (int j = 0; j < 6; ++j) { k_left(i, j) = coeffs_(0, i + 2 * j) * pow(left_pos_[0], 3) + coeffs_(1, i + 2 * j) * pow(left_pos_[0], 2) + coeffs_(2, i + 2 * j) * left_pos_[0] + coeffs_(3, i + 2 * j); k_right(i, j) = coeffs_(0, i + 2 * j) * pow(right_pos_[0], 3) + coeffs_(1, i + 2 * j) * pow(right_pos_[0], 2) + coeffs_(2, i + 2 * j) * right_pos_[0] + coeffs_(3, i + 2 * j); } + } Eigen::Matrix u_left, u_right; + u_left.setZero(); + u_right.setZero(); auto x_left = x_left_; auto x_right = x_right_; - x_left(2) = x_right(2) -= 0.; if (controller->getCompleteStand()) { x_left(3) -= vel_cmd_.x; @@ -65,11 +70,13 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const u_left = k_left * (-x_left); u_right = k_right * (-x_right); + controller->pubLQRStatus(-x_left, -x_right, u_left, u_right); + // Compute leg thrust auto model_params_ = controller->getModelParams(); double gravity = 1. / 2. * model_params_->M * model_params_->g; Eigen::Matrix F_leg; - double leg_length_des = controller->getLegCmd() == 0 ? 0.18 : controller->getLegCmd(); + double leg_length_des = controller->getLegCmd() == 0 ? 0.2 : controller->getLegCmd(); if (!start_jump_ && controller->getJumpCmd() && abs(x_left[0]) < 0.1) { start_jump_ = true; @@ -98,8 +105,8 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const } else { - double left_length_des = controller->getCompleteStand() ? leg_length_des / cos(x_left[0]) : 0.18; - double right_length_des = controller->getCompleteStand() ? leg_length_des / cos(x_right[0]) : 0.18; + double left_length_des = controller->getCompleteStand() ? leg_length_des / cos(x_left[0]) : 0.20; + double right_length_des = controller->getCompleteStand() ? leg_length_des / cos(x_right[0]) : 0.20; F_leg[0] = pid_legs_[0]->computeCommand(left_length_des - left_pos_[0], period) + gravity * cos(left_pos_[1]) + T_roll; F_leg[1] = @@ -112,12 +119,15 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const k_right_unstick.setZero(); k_left_unstick.block<1, 2>(1, 0) = k_left.block<1, 2>(1, 0); k_right_unstick.block<1, 2>(1, 0) = k_right.block<1, 2>(1, 0); - bool left_unstick = - unstickDetection(joint_handles_[0]->getEffort(), joint_handles_[1]->getEffort(), joint_handles_[4]->getEffort(), - left_angle_[0], left_angle_[1], left_pos_[0], linear_acc_base_.z, model_params_, x_left_); - bool right_unstick = - unstickDetection(joint_handles_[2]->getEffort(), joint_handles_[3]->getEffort(), joint_handles_[5]->getEffort(), - right_angle_[0], right_angle_[1], right_pos_[0], linear_acc_base_.z, model_params_, x_right_); + + // bool left_unstick = + // unstickDetection(joint_handles_[0]->getEffort(), joint_handles_[1]->getEffort(), joint_handles_[4]->getEffort(), + // left_angle_[0], left_angle_[1], left_pos_[0], linear_acc_base_.z, model_params_, x_left_); + // bool right_unstick = + // unstickDetection(joint_handles_[2]->getEffort(), joint_handles_[3]->getEffort(), joint_handles_[5]->getEffort(), + // right_angle_[0], right_angle_[1], right_pos_[0], linear_acc_base_.z, model_params_, x_right_); + bool left_unstick = false; + bool right_unstick = false; updateUnstick(left_unstick, right_unstick); if (left_unstick && jump_phase_ != JumpPhase::JUMP) @@ -127,8 +137,8 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const // Control double left_T[2], right_T[2]; - leg_conv(F_leg[0], -u_left(1) + T_theta_diff, left_angle_[0], left_angle_[1], left_T); - leg_conv(F_leg[1], -u_right(1) - T_theta_diff, right_angle_[0], right_angle_[1], right_T); + leg_conv(F_leg[0], u_left(1) + T_theta_diff, left_angle_[0], left_angle_[1], left_T); + leg_conv(F_leg[1], u_right(1) - T_theta_diff, right_angle_[0], right_angle_[1], right_T); double left_wheel_cmd = left_unstick ? 0. : u_left(0) - T_yaw; double right_wheel_cmd = right_unstick ? 0. : u_right(0) + T_yaw; LegCommand left_cmd = { F_leg[0], u_left[1], { left_T[0], left_T[1] } }, @@ -136,7 +146,8 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const setJointCommands(joint_handles_, left_cmd, right_cmd, left_wheel_cmd, right_wheel_cmd); // Protection - if ((controller->getCompleteStand() && (abs(x_left(4)) > 0.4 || abs(x_left(0)) > 1.5)) || controller->getOverturn()) + if ((controller->getCompleteStand() && (abs(x_left(4)) > 0.4 || abs(x_left(0)) > 1.5)) || controller->getOverturn() || + controller->getBaseState() == 4) { controller->setMode(BalanceMode::SIT_DOWN); controller->setStateChange(false); @@ -202,9 +213,5 @@ bool Normal::unstickDetection(const double& hip_effort, const double& knee_effor maybeChange = false; } return unstick_; - // if (Fn < 20.) - // return true; - // else - // return false; } } // namespace rm_chassis_controllers From 942669ac64cd8b85d52e0b9a80fe47810e720be1 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sat, 25 Oct 2025 13:31:58 +0800 Subject: [PATCH 09/57] Fix pid bug. --- .../controller_mode/mode_manager.h | 2 +- .../controller_mode/normal.h | 8 +++----- .../controller_mode/mode_manager.cpp | 6 ++---- .../controller_mode/normal.cpp | 14 +++++--------- 4 files changed, 11 insertions(+), 19 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_manager.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_manager.h index c66f18c4..4a685e68 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_manager.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_manager.h @@ -32,7 +32,7 @@ class ModeManager std::shared_ptr mode_impl; std::map> mode_map_; - control_toolbox::Pid pid_yaw_pos_, pid_yaw_vel_, pid_left_leg_, pid_right_leg_, pid_theta_diff_, pid_roll_; + control_toolbox::Pid pid_yaw_vel_, pid_left_leg_, pid_right_leg_, pid_theta_diff_, pid_roll_; control_toolbox::Pid pid_left_leg_theta_, pid_right_leg_theta_; control_toolbox::Pid pid_left_wheel_vel_, pid_right_wheel_vel_; std::vector pid_wheels_, pid_legs_, pid_thetas_; diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h index 98fcf8a9..74d0198a 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h @@ -18,9 +18,8 @@ class Normal : public ModeBase { public: Normal(const std::vector& joint_handles, - const std::vector& pid_legs, const control_toolbox::Pid& pid_yaw_pos, - const control_toolbox::Pid& pid_yaw_vel, const control_toolbox::Pid& pid_theta_diff, - const control_toolbox::Pid& pid_roll); + const std::vector& 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 { @@ -37,11 +36,10 @@ class Normal : public ModeBase Eigen::Matrix x); std::vector joint_handles_; std::vector pid_legs_; - control_toolbox::Pid pid_yaw_pos_, pid_yaw_vel_, pid_theta_diff_, pid_roll_; + control_toolbox::Pid *pid_yaw_vel_, *pid_theta_diff_, *pid_roll_; int jump_phase_ = JumpPhase::SQUAT; bool start_jump_ = false; std::unique_ptr> supportForceAveragePtr; - double yaw_des_; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp index 85fba227..80fbefa0 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp @@ -19,7 +19,6 @@ ModeManager::ModeManager(ros::NodeHandle& controller_nh, { "pid_right_leg_theta", &pid_right_leg_theta_ }, { "pid_left_wheel_vel", &pid_left_wheel_vel_ }, { "pid_right_wheel_vel", &pid_right_wheel_vel_ }, - { "pid_yaw_pos", &pid_yaw_pos_ }, }; for (const auto& e : pids) if (controller_nh.hasParam(e.first) && !e.second->init(ros::NodeHandle(controller_nh, e.first))) @@ -31,9 +30,8 @@ ModeManager::ModeManager(ros::NodeHandle& controller_nh, pid_thetas_.push_back(&pid_left_leg_theta_); pid_thetas_.push_back(&pid_right_leg_theta_); - mode_map_.insert( - std::make_pair(BalanceMode::NORMAL, std::make_unique(joint_handles, pid_legs_, pid_yaw_pos_, pid_yaw_vel_, - pid_theta_diff_, pid_roll_))); + mode_map_.insert(std::make_pair(BalanceMode::NORMAL, std::make_unique(joint_handles, pid_legs_, &pid_yaw_vel_, + &pid_theta_diff_, &pid_roll_))); mode_map_.insert( std::make_pair(BalanceMode::STAND_UP, std::make_unique(joint_handles, pid_legs_, pid_thetas_))); mode_map_.insert( diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp index 42dea1f4..9c5caac3 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -8,12 +8,10 @@ namespace rm_chassis_controllers { Normal::Normal(const std::vector& joint_handles, - const std::vector& pid_legs, const control_toolbox::Pid& pid_yaw_pos, - const control_toolbox::Pid& pid_yaw_vel, const control_toolbox::Pid& pid_theta_diff, - const control_toolbox::Pid& pid_roll) + const std::vector& pid_legs, control_toolbox::Pid* pid_yaw_vel, + control_toolbox::Pid* pid_theta_diff, control_toolbox::Pid* pid_roll) : joint_handles_(joint_handles) , pid_legs_(pid_legs) - , pid_yaw_pos_(pid_yaw_pos) , pid_yaw_vel_(pid_yaw_vel) , pid_theta_diff_(pid_theta_diff) , pid_roll_(pid_roll) @@ -26,8 +24,6 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const if (!controller->getStateChange()) { ROS_INFO("[balance] Enter NORMAL"); - yaw_total_ = yaw_total_last_ = 0.; - yaw_des_ = yaw_total_; x_left_(2) = x_right_(2) = 0; controller->setStateChange(true); } @@ -37,9 +33,9 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const auto vel_cmd_ = controller->getVelCmd(); // PID - double T_yaw = pid_yaw_vel_.computeCommand(vel_cmd_.z - angular_vel_base_.z, period); - double T_theta_diff = pid_theta_diff_.computeCommand(right_pos_[1] - left_pos_[1], period); - double T_roll = pid_roll_.computeCommand(0. - roll_, period); + double T_yaw = pid_yaw_vel_->computeCommand(vel_cmd_.z - angular_vel_base_.z, period); + double T_theta_diff = pid_theta_diff_->computeCommand(right_pos_[1] - left_pos_[1], period); + double T_roll = pid_roll_->computeCommand(0. - roll_, period); // LQR Matrix coeffs_ = controller->getCoeffs(); From 8ef9ef7cbfd3072e57ba82ee651e11fb3bd0ab66 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Thu, 20 Nov 2025 17:38:22 +0800 Subject: [PATCH 10/57] Add upstairs mode. --- .../controller_mode/upstairs.h | 32 +++++++++++++ .../bipedal_wheel_controller/vmc/leg_params.h | 17 +++++++ .../controller_mode/upstairs.cpp | 45 +++++++++++++++++++ 3 files changed, 94 insertions(+) create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/upstairs.h create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_params.h create mode 100644 rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/upstairs.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/upstairs.h new file mode 100644 index 00000000..bc594127 --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/upstairs.h @@ -0,0 +1,32 @@ +// +// Created by wk on 2025/11/1. +// +#pragma once + +#include +#include + +#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 Upstairs : public ModeBase +{ +public: + Upstairs(const std::vector& joint_handles, + const std::vector& pid_legs, const std::vector& pid_thetas); + void execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) override; + const char* name() const override + { + return "Upstairs"; + } + +private: + std::vector joint_handles_; + std::vector pid_legs_, pid_thetas_; + int left_leg_state, right_leg_state; +}; +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_params.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_params.h new file mode 100644 index 00000000..265e80db --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_params.h @@ -0,0 +1,17 @@ +// +// Created by wk on 2025/10/30. +// + +#ifndef LEG_PARAMS_H +#define LEG_PARAMS_H + +// physical parameters --------------------- +#define LEG_L1 (0.218f) // (m)腿1长度 +#define LEG_L2 (0.260f) // (m)腿2长度 +#define LEG_L3 (LEG_L2) // (m)腿3长度 +#define LEG_L4 (LEG_L1) // (m)腿4长度 +#define LEG_L5 (0.0f) // (m)关节间距 + +#include + +#endif // LEG_PARAMS_H diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp new file mode 100644 index 00000000..35dd9fc3 --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp @@ -0,0 +1,45 @@ +// +// Created by wk on 2025/11/1. +// + +#include "bipedal_wheel_controller/controller_mode/upstairs.h" +#include "bipedal_wheel_controller/controller.h" +#include "bipedal_wheel_controller/helper_functions.h" + +namespace rm_chassis_controllers +{ +Upstairs::Upstairs(const std::vector& joint_handles, + const std::vector& pid_legs, + const std::vector& pid_thetas) + : joint_handles_(joint_handles), pid_legs_(pid_legs), pid_thetas_(pid_thetas) +{ +} + +void Upstairs::execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) +{ + if (!controller->getStateChange()) + { + ROS_INFO("[balance] Enter Upstairs"); + controller->setStateChange(true); + controller->setCompleteStand(false); + detectLegState(x_left_, left_leg_state); + detectLegState(x_right_, right_leg_state); + } + + double theta_des_l{ M_PI_2 + 0.3 }, theta_des_r{ M_PI_2 + 0.3 }, length_des_l{ 0.17 }, length_des_r{ 0.17 }; + LegCommand left_cmd = { 0, 0, { 0., 0. } }, right_cmd = { 0, 0, { 0., 0. } }; + left_cmd = computePidLegCommand(length_des_l, theta_des_l, left_pos_, left_spd_, *pid_legs_[0], *pid_thetas_[0], + *pid_thetas_[2], left_angle_, left_leg_state, period); + right_cmd = computePidLegCommand(length_des_r, theta_des_r, right_pos_, right_spd_, *pid_legs_[1], *pid_thetas_[1], + *pid_thetas_[3], right_angle_, right_leg_state, period); + setJointCommands(joint_handles_, left_cmd, right_cmd); + + // Exit + if ((left_pos_[0] < 0.2 && left_pos_[1] > (M_PI_2 - 0.3)) && (right_pos_[0] < 0.2 && right_pos_[1] > (M_PI_2 - 0.3))) + { + controller->setMode(BalanceMode::NORMAL); + controller->setStateChange(false); + ROS_INFO("[balance] Exit Upstairs"); + } +} +} // namespace rm_chassis_controllers From de80227ec797fbecc1d47b9bd9305025d4cc0c39 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Thu, 20 Nov 2025 17:43:35 +0800 Subject: [PATCH 11/57] Update series_legged chassis controller. --- rm_chassis_controllers/CMakeLists.txt | 2 +- .../bipedal_wheel_controller/controller.h | 19 ++- .../controller_mode/mode_manager.h | 6 +- .../controller_mode/normal.h | 12 +- .../bipedal_wheel_controller/definitions.h | 6 +- .../helper_functions.h | 31 ++-- .../series_legged_vmc_controller.h | 3 +- .../bipedal_wheel_controller/vmc/leg_conv.h | 2 +- .../vmc/leg_conv_fwd.h | 31 ---- .../bipedal_wheel_controller/vmc/leg_pos.h | 2 +- .../bipedal_wheel_controller/vmc/leg_spd.h | 3 +- .../bipedal_wheel_controller/controller.cpp | 99 ++++++++---- .../controller_mode/mode_manager.cpp | 14 +- .../controller_mode/normal.cpp | 139 ++++++++++------ .../controller_mode/recover.cpp | 15 +- .../controller_mode/stand_up.cpp | 40 +++-- .../series_legged_vmc_controller.cpp | 26 ++- .../bipedal_wheel_controller/vmc/leg_conv.c | 116 +++++++++++++- .../vmc/leg_conv_fwd.c | 127 --------------- .../bipedal_wheel_controller/vmc/leg_pos.c | 66 +++++++- .../bipedal_wheel_controller/vmc/leg_spd.c | 149 +++++++++++++++++- .../test/vmc_controller.yaml | 9 +- 22 files changed, 591 insertions(+), 326 deletions(-) delete mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_conv_fwd.h delete mode 100644 rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_conv_fwd.c diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt index 1ee41447..ee8d78c7 100644 --- a/rm_chassis_controllers/CMakeLists.txt +++ b/rm_chassis_controllers/CMakeLists.txt @@ -71,7 +71,6 @@ add_library(${PROJECT_NAME} src/swerve.cpp src/balance.cpp - src/bipedal_wheel_controller/vmc/leg_conv_fwd.c src/bipedal_wheel_controller/vmc/leg_conv.c src/bipedal_wheel_controller/vmc/leg_pos.c src/bipedal_wheel_controller/vmc/leg_spd.c @@ -83,6 +82,7 @@ add_library(${PROJECT_NAME} 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 ) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h index bbfd5314..759ea236 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h @@ -46,16 +46,21 @@ class BipedalController : public ChassisBase left_error, Eigen::Matrix right_error, - Eigen::Matrix u_left, Eigen::Matrix u_right) const; + Eigen::Matrix left_ref, Eigen::Matrix right_ref, + Eigen::Matrix u_left, Eigen::Matrix u_right, + Eigen::Matrix F_leg_, const bool unstick[2]) const; // clang-format on + void clearStatus(); private: void updateEstimation(const ros::Time& time, const ros::Duration& period); @@ -77,14 +82,17 @@ class BipedalController : public ChassisBase mode_manager_; // Slippage_detection - double leftWheelVel{}, rightWheelVel{}, leftWheelVelAbsolute{}, rightWheelVelAbsolute{}; + 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 A_, B_, H_, Q_, R_; Eigen::Matrix X_, U_; std::shared_ptr> kalmanFilterPtr_; Eigen::Matrix x_left_{}, x_right_{}; - + double default_leg_length_{ 0.18 }; + bool move_flag_{ false }; // stand up bool complete_stand_ = false, overturn_ = false; @@ -98,9 +106,6 @@ class BipedalController : public ChassisBase> 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_theta_, pid_right_leg_theta_; + 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 pid_wheels_, pid_legs_, pid_thetas_; + std::vector pid_wheels_, pid_legs_, pid_thetas_, pid_legs_stand_up_; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h index 74d0198a..e8bef2c1 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h @@ -27,17 +27,17 @@ class Normal : public ModeBase } private: - double calculateSupportForce(double F, double Tp, double leg_length, double acc_z, - Eigen::Matrix x, Eigen::Matrix u, - const std::shared_ptr& model_params); - bool unstickDetection(const double& hip_effort, const double& knee_effort, const double& wheel_effort, - const double& hip_angle, const double& knee_angle, const double& leg_length, + double calculateSupportForce(double F, double Tp, double leg_length, const double& leg_len_spd, double acc_z, + Eigen::Matrix x, const std::shared_ptr& 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& model_params, - Eigen::Matrix x); + Eigen::Matrix x, const ros::Duration& period); std::vector joint_handles_; std::vector pid_legs_; control_toolbox::Pid *pid_yaw_vel_, *pid_theta_diff_, *pid_roll_; + double pos_des_{ 0.0 }, leg_length_des{ 0.18 }; int jump_phase_ = JumpPhase::SQUAT; bool start_jump_ = false; std::unique_ptr> supportForceAveragePtr; diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h index 89f86c76..7059e665 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h @@ -22,6 +22,7 @@ struct ModelParams double i_m; // Body inertia double r; // Wheel radius double g; // Gravity acceleration + double f_spring; // Spring Force }; struct BiasParams @@ -58,11 +59,12 @@ enum BalanceMode NORMAL, STAND_UP, SIT_DOWN, - RECOVER + RECOVER, + UPSTAIRS, }; constexpr std::array, 3> jumpLengthDes = { - { { JumpPhase::SQUAT, 0.15 }, { JumpPhase::JUMP, 0.32 }, { JumpPhase::SHRINK, 0.15 } } + { { JumpPhase::SQUAT, 0.15 }, { JumpPhase::JUMP, 0.38 }, { JumpPhase::SHRINK, 0.15 } } }; constexpr static const int STATE_DIM = 6; diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h index a1fc5b75..2a6a691c 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h @@ -15,7 +15,6 @@ #include "bipedal_wheel_controller/dynamics/gen_A.h" #include "bipedal_wheel_controller/dynamics/gen_B.h" -#include "bipedal_wheel_controller/vmc/leg_conv_fwd.h" #include "bipedal_wheel_controller/vmc/leg_conv.h" #include "bipedal_wheel_controller/definitions.h" @@ -62,22 +61,22 @@ inline void generateAB(const std::shared_ptr& model_params, Eigen:: */ inline void detectLegState(const Eigen::Matrix& x, int& leg_state) { - if (x[0] > -M_PI / 2 + 0.4 && x[0] < M_PI / 2 - 0.4) + if (x[0] > -M_PI / 2 + 0.7 && x[0] < M_PI / 2 - 0.7) leg_state = LegState::UNDER; - else if (x[0] < -M_PI / 2 + 0.4 && x[0] > -M_PI) + else if (x[0] < -M_PI / 2 + 0.7 && x[0] > -M_PI) leg_state = LegState::FRONT; - else if (x[0] > M_PI / 2 - 0.4 && x[0] < M_PI) + else if (x[0] > M_PI / 2 - 0.7 && x[0] < M_PI) leg_state = LegState::BEHIND; switch (leg_state) { case LegState::UNDER: - ROS_INFO("[balance] Leg state: UNDER"); + ROS_INFO("[balance] x[0]: %.3f Leg state: UNDER", x[0]); break; case LegState::FRONT: - ROS_INFO("[balance] Leg state: FRONT"); + ROS_INFO("[balance] x[0]: %.3f Leg state: FRONT", x[0]); break; case LegState::BEHIND: - ROS_INFO("[balance] Leg state: BEHIND"); + ROS_INFO("[balance] x[0]: %.3f Leg state: BEHIND", x[0]); break; } } @@ -94,14 +93,18 @@ inline void detectLegState(const Eigen::Matrix& x, int& le * @param period * @return */ -inline LegCommand computePidLegCommand(double desired_length, double desired_angle, double current_length, - double current_angle, control_toolbox::Pid& length_pid, - control_toolbox::Pid& angle_pid, const double* leg_angle, - const ros::Duration& period) +inline LegCommand computePidLegCommand(double desired_length, double desired_angle, double leg_pos[2], + double leg_spd[2], control_toolbox::Pid& length_pid, + control_toolbox::Pid& angle_pid, control_toolbox::Pid& angle_vel_pid, + const double* leg_angle, const int& leg_state, const ros::Duration& period, + double feedforward_force = 0.0f, const bool& overturn = false) { - LegCommand cmd; - cmd.force = length_pid.computeCommand(desired_length - current_length, period); - cmd.torque = angle_pid.computeCommand(-angles::shortest_angular_distance(desired_angle, current_angle), period); + LegCommand cmd{ 0.0, 0.0, { 0.0, 0.0 } }; + cmd.force = length_pid.computeCommand(desired_length - leg_pos[0], period) + feedforward_force; + if (leg_state == LegState::BEHIND && !overturn) + cmd.torque = angle_pid.computeCommand(-angles::shortest_angular_distance(desired_angle, leg_pos[1]), period); + else + cmd.torque = angle_vel_pid.computeCommand(-3.5 - leg_spd[1], period); leg_conv(cmd.force, cmd.torque, leg_angle[0], leg_angle[1], cmd.input); return cmd; } diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/series_legged_vmc_controller.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/series_legged_vmc_controller.h index fdda179f..01298526 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/series_legged_vmc_controller.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/series_legged_vmc_controller.h @@ -13,6 +13,7 @@ #include #include +#include "bipedal_wheel_controller/vmc/leg_params.h" #include "bipedal_wheel_controller/vmc/leg_conv.h" #include "bipedal_wheel_controller/vmc/leg_pos.h" #include "bipedal_wheel_controller/vmc/leg_spd.h" @@ -44,7 +45,7 @@ class VMCController : public controller_interface::MultiInterfaceController -#include - -#ifdef __cplusplus -extern "C" { -#endif - -/* Function Declarations */ -extern void leg_conv_fwd(double T1, double T2, double phi1, double phi2, double T_real[2]); - -#ifdef __cplusplus -} -#endif - -#endif -/* - * File trailer for leg_conv_fwd.h - * - * [EOF] - */ diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_pos.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_pos.h index 4787a5a7..4583add1 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_pos.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_pos.h @@ -17,7 +17,7 @@ extern "C" { #endif /* Function Declarations */ -extern void leg_pos(double phi1, double phi2, double pos[2]); +extern void leg_pos(double phi1, double phi4, double pos[2]); #ifdef __cplusplus } diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_spd.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_spd.h index bc4eccee..794f6b96 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_spd.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_spd.h @@ -17,7 +17,8 @@ extern "C" { #endif /* Function Declarations */ -extern void leg_spd(double dphi1, double dphi2, double phi1, double phi2, double spd[2]); +extern void CalcJacobian(double phi1, double phi4, double J_[2][2]); +extern void leg_spd(double dphi1, double dphi4, double phi1, double phi4, double spd[2]); #ifdef __cplusplus } diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp index 2ce6564c..8e1832b4 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp @@ -11,6 +11,7 @@ #include #include +#include "bipedal_wheel_controller/vmc/leg_params.h" #include "bipedal_wheel_controller/vmc/leg_conv.h" #include "bipedal_wheel_controller/vmc/leg_spd.h" #include "bipedal_wheel_controller/vmc/leg_pos.h" @@ -57,8 +58,10 @@ bool BipedalController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan // Slippage detection A_ << 1, 0, 0, 1; H_ << 1, 0, 0, 1; - Q_ << 1, 0, 0, 1; - R_ << 150, 0, 0, 150; + Q_ << 25, 0, 0, 2000; + R_ << 800, 0, 0, 0.01; + R_wheel_ = R_(0, 0); + slip_R_wheel_ = slip_alpha_ * R_wheel_; B_.setZero(); X_.setZero(); U_.setZero(); @@ -77,6 +80,11 @@ void BipedalController::moveJoint(const ros::Time& time, const ros::Duration& pe pubState(); } +void BipedalController::clearStatus() +{ + x_left_(2) = x_right_(2) = 0; +} + void BipedalController::updateEstimation(const ros::Time& time, const ros::Duration& period) { geometry_msgs::Vector3 gyro, acc; @@ -112,7 +120,8 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat tf2::Vector3 z_body(0, 0, 1); tf2::Vector3 z_world = tf2::quatRotate(odom2base.getRotation(), z_body); - overturn_ = z_world.z() < 0; + overturn_ = z_world.z() < -5.0; + overturn_ = false; } catch (tf2::TransformException& ex) { @@ -124,10 +133,16 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat // vmc double left_angle[2]{}, right_angle[2]{}, left_pos[2]{}, left_spd[2]{}, right_pos[2]{}, right_spd[2]{}; // [0]:hip_vmc_joint [1]:knee_vmc_joint - left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; - left_angle[1] = left_knee_joint_handle_.getPosition(); - right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; - right_angle[1] = right_knee_joint_handle_.getPosition(); + // left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; + // left_angle[1] = left_knee_joint_handle_.getPosition(); + // right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; + // right_angle[1] = right_knee_joint_handle_.getPosition(); + + // gazebo + left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; + left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; + right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; + right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; // [0] is length, [1] is angle leg_pos(left_angle[0], left_angle[1], left_pos); @@ -146,27 +161,35 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat right_spd[0] * sin(right_pos[1] + pitch_); double wheel_vel_aver = (leftWheelVelAbsolute + rightWheelVelAbsolute) / 2.; + R_(0, 0) = slip_flag_ ? slip_R_wheel_ : R_wheel_; if (i >= sample_times_) { // oversampling i = 0; X_(0) = wheel_vel_aver; X_(1) = linear_acc_base.x; - kalmanFilterPtr_->predict(U_); + kalmanFilterPtr_->predict(U_, R_); kalmanFilterPtr_->update(X_); } else { - kalmanFilterPtr_->predict(U_); + kalmanFilterPtr_->predict(U_, R_); i++; } auto x_hat_vel = kalmanFilterPtr_->getState(); + slip_flag_ = abs(x_hat_vel(0) - wheel_vel_aver) > 0.5; // update state + // x_left_[3] = state_ != RAW ? x_hat_vel(0) : 0; + // x_left_[2] += state_ != RAW ? x_left_[3] * period.toSec() : 0; x_left_[3] = x_hat_vel(0); - if (abs(x_left_[3]) < 0.3 && vel_cmd_.x == 0.0) - x_left_[2] += x_left_[3] * period.toSec(); - else - x_left_[2] = 0.; + x_left_[2] += x_left_[3] * period.toSec(); + // if (abs(x_left_[3]) <= 0.8f && abs(vel_cmd_.x) <= 0.1f) + // { + // } + // else + // { + // x_left_[2] = 0; + // } x_left_[2] -= bias_params_->x; x_left_[0] = (left_pos[1] + pitch); x_left_[1] = left_spd[1] + angular_vel_base.y; @@ -176,7 +199,10 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat x_right_[0] = (right_pos[1] + pitch); x_right_[1] = right_spd[1] + angular_vel_base.y; - legged_chassis_status_msg.roll = roll; + // ros msg + rm_msgs::LeggedChassisStatus legged_chassis_status_msg; + legged_chassis_status_msg.roll = + (left_wheel_joint_handle_.getVelocity() + right_wheel_joint_handle_.getVelocity()) * model_params_->r / 2.0; legged_chassis_status_msg.pitch = x_left_[4]; legged_chassis_status_msg.d_pitch = x_left_[5]; legged_chassis_status_msg.yaw = yaw; @@ -189,6 +215,10 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat legged_chassis_status_msg.left_leg_theta_dot = x_left_[1]; legged_chassis_status_msg.right_leg_theta = x_right_[0]; legged_chassis_status_msg.right_leg_theta_dot = x_right_[1]; + legged_chassis_status_msg.linear_acc_base.push_back(linear_acc_base.x); + legged_chassis_status_msg.linear_acc_base.push_back(linear_acc_base.y); + legged_chassis_status_msg.linear_acc_base.push_back(linear_acc_base.z); + legged_chassis_status_pub_.publish(legged_chassis_status_msg); mode_manager_->getModeImpl()->updateEstimation(x_left_, x_right_); mode_manager_->getModeImpl()->updateLegKinematics(left_angle, right_angle, left_pos, left_spd, right_pos, right_spd); @@ -200,7 +230,6 @@ void BipedalController::pubState() std_msgs::Bool msg; msg.data = mode_manager_->getModeImpl()->getUnstick(); unstick_pub_.publish(msg); - legged_chassis_status_pub_.publish(legged_chassis_status_msg); } void BipedalController::stopping(const ros::Time& time) @@ -214,18 +243,18 @@ void BipedalController::stopping(const ros::Time& time) bool BipedalController::setupModelParams(ros::NodeHandle& controller_nh) { - const std::pair tbl[] = // - { { "m_w", &model_params_->m_w }, - { "m_p", &model_params_->m_p }, - { "M", &model_params_->M }, - { "i_w", &model_params_->i_w }, - { "i_m", &model_params_->i_m }, - { "i_p", &model_params_->i_p }, - { "l", &model_params_->l }, - { "L_weight", &model_params_->L_weight }, - { "Lm_weight", &model_params_->Lm_weight }, - { "g", &model_params_->g }, - { "wheel_radius", &model_params_->r } }; + const std::pair tbl[] = { { "m_w", &model_params_->m_w }, + { "m_p", &model_params_->m_p }, + { "M", &model_params_->M }, + { "i_w", &model_params_->i_w }, + { "i_m", &model_params_->i_m }, + { "i_p", &model_params_->i_p }, + { "l", &model_params_->l }, + { "L_weight", &model_params_->L_weight }, + { "Lm_weight", &model_params_->Lm_weight }, + { "g", &model_params_->g }, + { "wheel_radius", &model_params_->r }, + { "spring_force", &model_params_->f_spring } }; for (const auto& e : tbl) if (!controller_nh.getParam(e.first, *e.second)) @@ -233,6 +262,13 @@ bool BipedalController::setupModelParams(ros::NodeHandle& controller_nh) ROS_ERROR("Param %s not given (namespace: %s)", e.first, controller_nh.getNamespace().c_str()); return false; } + + if (!controller_nh.getParam("default_leg_length", default_leg_length_)) + { + ROS_ERROR("Param %s not given (namespace: %s)", "default_leg_length", controller_nh.getNamespace().c_str()); + return false; + } + return true; } @@ -326,19 +362,26 @@ geometry_msgs::Twist BipedalController::odometry() void BipedalController::pubLQRStatus(Eigen::Matrix left_error, Eigen::Matrix right_error, + Eigen::Matrix left_ref, + Eigen::Matrix right_ref, Eigen::Matrix u_left, - Eigen::Matrix u_right) const + Eigen::Matrix u_right, + Eigen::Matrix F_leg_, const bool unstick[2]) const { rm_msgs::LeggedLQRStatus msg; for (int i = 0; i < 6; ++i) { msg.left_leg_error.push_back(left_error(i)); msg.right_leg_error.push_back(right_error(i)); + msg.left_leg_ref.push_back(left_ref(i)); + msg.right_leg_ref.push_back(right_ref(i)); } for (int i = 0; i < 2; ++i) { msg.left_leg_u.push_back(u_left(i)); msg.right_leg_u.push_back(u_right(i)); + msg.F_leg.push_back(F_leg_[i]); + msg.unstick.push_back(unstick[i]); } lqr_status_pub_.publish(msg); } diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp index 80fbefa0..7b835985 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp @@ -17,8 +17,12 @@ ModeManager::ModeManager(ros::NodeHandle& controller_nh, { "pid_roll", &pid_roll_ }, { "pid_left_leg_theta", &pid_left_leg_theta_ }, { "pid_right_leg_theta", &pid_right_leg_theta_ }, + { "pid_left_leg_theta_vel", &pid_left_leg_theta_vel_ }, + { "pid_right_leg_theta_vel", &pid_right_leg_theta_vel_ }, { "pid_left_wheel_vel", &pid_left_wheel_vel_ }, { "pid_right_wheel_vel", &pid_right_wheel_vel_ }, + { "pid_left_leg_stand_up", &pid_left_leg_stand_up_ }, + { "pid_right_leg_stand_up", &pid_right_leg_stand_up_ }, }; for (const auto& e : pids) if (controller_nh.hasParam(e.first) && !e.second->init(ros::NodeHandle(controller_nh, e.first))) @@ -29,13 +33,19 @@ ModeManager::ModeManager(ros::NodeHandle& controller_nh, pid_legs_.push_back(&pid_right_leg_); pid_thetas_.push_back(&pid_left_leg_theta_); pid_thetas_.push_back(&pid_right_leg_theta_); + pid_thetas_.push_back(&pid_left_leg_theta_vel_); + pid_thetas_.push_back(&pid_right_leg_theta_vel_); + pid_legs_stand_up_.push_back(&pid_left_leg_stand_up_); + pid_legs_stand_up_.push_back(&pid_right_leg_stand_up_); mode_map_.insert(std::make_pair(BalanceMode::NORMAL, std::make_unique(joint_handles, pid_legs_, &pid_yaw_vel_, &pid_theta_diff_, &pid_roll_))); mode_map_.insert( - std::make_pair(BalanceMode::STAND_UP, std::make_unique(joint_handles, pid_legs_, pid_thetas_))); + std::make_pair(BalanceMode::STAND_UP, std::make_unique(joint_handles, pid_legs_stand_up_, pid_thetas_))); mode_map_.insert( - std::make_pair(BalanceMode::RECOVER, std::make_unique(joint_handles, pid_legs_, pid_thetas_))); + std::make_pair(BalanceMode::RECOVER, std::make_unique(joint_handles, pid_legs_stand_up_, pid_thetas_))); mode_map_.insert(std::make_pair(BalanceMode::SIT_DOWN, std::make_unique(joint_handles, pid_wheels_))); + mode_map_.insert(std::make_pair(BalanceMode::UPSTAIRS, + std::make_unique(joint_handles, pid_legs_stand_up_, pid_thetas_))); } } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp index 9c5caac3..34e9b105 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -24,14 +24,27 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const if (!controller->getStateChange()) { ROS_INFO("[balance] Enter NORMAL"); - x_left_(2) = x_right_(2) = 0; + controller->clearStatus(); + if (abs(pos_des_ - x_left_(2)) > 5) + { + x_left_(2) = x_right_(2) = pos_des_ = 0; + } controller->setStateChange(true); } - if (!controller->getCompleteStand() && abs(x_left_[4]) < 0.2) + + if (!controller->getCompleteStand() && abs(x_left_[4]) < 0.2 && (abs(x_left_[0] + x_right_[0]) / 2.0f) < 0.1) + { + x_left_(2) = x_right_(2) = pos_des_ = 0; controller->setCompleteStand(true); + } auto vel_cmd_ = controller->getVelCmd(); - + pos_des_ += vel_cmd_.x * period.toSec(); + if (controller->getMoveFlag() && abs(x_left_[3]) < 0.1 && abs(vel_cmd_.x) < 0.1) + { + controller->setMoveFlag(false); + // pos_des_ = x_left_(2); + } // PID double T_yaw = pid_yaw_vel_->computeCommand(vel_cmd_.z - angular_vel_base_.z, period); double T_theta_diff = pid_theta_diff_->computeCommand(right_pos_[1] - left_pos_[1], period); @@ -58,21 +71,32 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const u_right.setZero(); auto x_left = x_left_; auto x_right = x_right_; + Matrix x_left_ref, x_right_ref; + x_left_ref.setZero(); + x_right_ref.setZero(); + if (controller->getCompleteStand()) { - x_left(3) -= vel_cmd_.x; - x_right(3) -= vel_cmd_.x; + x_left_ref(2) = x_right_ref(2) = pos_des_; + x_left_ref(3) = x_right_ref(3) = vel_cmd_.x; + leg_length_des = controller->getLegCmd(); } + x_left(0) -= 0.03; + x_right(0) -= 0.03; + x_left(4) -= 0.; + x_right(4) -= 0.; + + x_left -= x_left_ref; + x_right -= x_right_ref; + u_left = k_left * (-x_left); u_right = k_right * (-x_right); - controller->pubLQRStatus(-x_left, -x_right, u_left, u_right); - // Compute leg thrust auto model_params_ = controller->getModelParams(); - double gravity = 1. / 2. * model_params_->M * model_params_->g; + double gravity = model_params_->M * model_params_->g, current_leg_length = (left_pos_[0] + right_pos_[0]) / 2.0f, + spring_force = model_params_->f_spring; Eigen::Matrix F_leg; - double leg_length_des = controller->getLegCmd() == 0 ? 0.2 : controller->getLegCmd(); if (!start_jump_ && controller->getJumpCmd() && abs(x_left[0]) < 0.1) { start_jump_ = true; @@ -94,19 +118,19 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const { gravity = 0; } - F_leg[0] = - pid_legs_[0]->computeCommand(leg_length_des - left_pos_[0], period) + gravity * cos(left_pos_[1]) + T_roll; - F_leg[1] = - pid_legs_[1]->computeCommand(leg_length_des - right_pos_[0], period) + gravity * cos(right_pos_[1]) - T_roll; + F_leg[0] = pid_legs_[0]->computeCommand(leg_length_des - current_leg_length, period) + gravity * cos(left_pos_[1]) + + T_roll - spring_force; + F_leg[1] = pid_legs_[1]->computeCommand(leg_length_des - current_leg_length, period) + + gravity * cos(right_pos_[1]) - T_roll - spring_force; } else { - double left_length_des = controller->getCompleteStand() ? leg_length_des / cos(x_left[0]) : 0.20; - double right_length_des = controller->getCompleteStand() ? leg_length_des / cos(x_right[0]) : 0.20; - F_leg[0] = - pid_legs_[0]->computeCommand(left_length_des - left_pos_[0], period) + gravity * cos(left_pos_[1]) + T_roll; - F_leg[1] = - pid_legs_[1]->computeCommand(right_length_des - right_pos_[0], period) + gravity * cos(right_pos_[1]) - T_roll; + double left_length_des = controller->getCompleteStand() ? leg_length_des : controller->getDefaultLegLength(); + double right_length_des = controller->getCompleteStand() ? leg_length_des : controller->getDefaultLegLength(); + F_leg[0] = pid_legs_[0]->computeCommand(left_length_des - left_pos_[0], period) + gravity * cos(left_pos_[1]) + + T_roll - spring_force; + F_leg[1] = pid_legs_[1]->computeCommand(right_length_des - right_pos_[0], period) + gravity * cos(right_pos_[1]) - + T_roll - spring_force; } // Unstick detection @@ -116,19 +140,22 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const k_left_unstick.block<1, 2>(1, 0) = k_left.block<1, 2>(1, 0); k_right_unstick.block<1, 2>(1, 0) = k_right.block<1, 2>(1, 0); - // bool left_unstick = - // unstickDetection(joint_handles_[0]->getEffort(), joint_handles_[1]->getEffort(), joint_handles_[4]->getEffort(), - // left_angle_[0], left_angle_[1], left_pos_[0], linear_acc_base_.z, model_params_, x_left_); - // bool right_unstick = - // unstickDetection(joint_handles_[2]->getEffort(), joint_handles_[3]->getEffort(), joint_handles_[5]->getEffort(), - // right_angle_[0], right_angle_[1], right_pos_[0], linear_acc_base_.z, model_params_, x_right_); - bool left_unstick = false; - bool right_unstick = false; + bool left_unstick = unstickDetection(F_leg[0], u_left(1), left_spd_[0], left_pos_[0], linear_acc_base_.z, + model_params_, x_left_, period); + bool right_unstick = unstickDetection(F_leg[1], u_right(1), right_spd_[0], right_pos_[0], linear_acc_base_.z, + model_params_, x_right_, period); + bool unstick[2]{}; + unstick[0] = left_unstick; + unstick[1] = right_unstick; + controller->pubLQRStatus(-x_left, -x_right, x_left_ref, x_right_ref, u_left, u_right, F_leg, unstick); + + // left_unstick = false; + // right_unstick = false; updateUnstick(left_unstick, right_unstick); - if (left_unstick && jump_phase_ != JumpPhase::JUMP) + if (controller->getCompleteStand() && left_unstick && jump_phase_ != JumpPhase::JUMP) u_left = k_left_unstick * (-x_left); - if (right_unstick && jump_phase_ != JumpPhase::JUMP) + if (controller->getCompleteStand() && right_unstick && jump_phase_ != JumpPhase::JUMP) u_right = k_right_unstick * (-x_right); // Control @@ -141,10 +168,23 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const right_cmd = { F_leg[1], u_right[1], { right_T[0], right_T[1] } }; setJointCommands(joint_handles_, left_cmd, right_cmd, left_wheel_cmd, right_wheel_cmd); + // upstairs + if (x_left[4] < -0.2 && (controller->getCompleteStand()) && abs(vel_cmd_.x) > 0.1 && + ((left_pos_[0] + right_pos_[0]) / 2.0f) > 0.34) + { + // leg_length_des = controller->getDefaultLegLength(); + // controller->setMode(BalanceMode::UPSTAIRS); + // controller->setStateChange(false); + // controller->setJumpCmd(false); + // ROS_INFO("[balance] Exit NORMAL"); + } + // Protection - if ((controller->getCompleteStand() && (abs(x_left(4)) > 0.4 || abs(x_left(0)) > 1.5)) || controller->getOverturn() || - controller->getBaseState() == 4) + if ((controller->getCompleteStand() && (abs(x_left(4)) > 0.4 || abs(x_left(0)) > 1.35)) || + controller->getOverturn() || controller->getBaseState() == 4) { + leg_length_des = controller->getDefaultLegLength(); + x_left_(2) = x_right_(2) = pos_des_ = 0; controller->setMode(BalanceMode::SIT_DOWN); controller->setStateChange(false); controller->setJumpCmd(false); @@ -153,20 +193,19 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const } } -double Normal::calculateSupportForce(double F, double Tp, double leg_length, double acc_z, - Eigen::Matrix x, Eigen::Matrix u, - const std::shared_ptr& model_params) +double Normal::calculateSupportForce(double F, double Tp, double leg_length, const double& leg_len_spd, double acc_z, + Eigen::Matrix x, + const std::shared_ptr& model_params, const ros::Duration& period) { - static double last_ddot_zM = acc_z - model_params->g; - Eigen::Matrix a; - Eigen::Matrix b; - generateAB(model_params, a, b, leg_length); + static double last_ddot_zM = acc_z - model_params->g, last_dot_theta = x(1), + last_ddot_theta = (x(1) - last_dot_theta) / period.toSec(); double P = F * cos(x(0)) + Tp * sin(x(0)) / leg_length; // lp filter double ddot_zM = 0.7 * (acc_z - model_params->g) + 0.3 * last_ddot_zM; - auto ddot_x = a * x + b * u; - double ddot_theta = ddot_x(1); + double ddot_theta = 0.7 * ((x(1) - last_dot_theta) / period.toSec()) + 0.3 * last_ddot_theta; + last_dot_theta = x(1); + last_ddot_theta = ddot_theta; double ddot_zw = ddot_zM - leg_length * cos(x(0)) + 2 * leg_length * x(1) * sin(x(0)) + +leg_length * (ddot_theta * sin(x(0)) + x(1) * x(1) * cos(x(0))); double Fn = model_params->m_w * ddot_zw + model_params->m_w * model_params->g + P; @@ -174,20 +213,16 @@ double Normal::calculateSupportForce(double F, double Tp, double leg_length, dou return Fn; } -bool Normal::unstickDetection(const double& hip_effort, const double& knee_effort, const double& wheel_effort, - const double& hip_angle, const double& knee_angle, const double& leg_length, - const double& acc_z, const std::shared_ptr& model_params, - Eigen::Matrix x) +bool Normal::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& model_params, Eigen::Matrix x, + const ros::Duration& period) { static bool maybeChange = false, last_unstick_ = false; - ros::Time judgeTime; - double leg_F[2]; - leg_conv_fwd(hip_effort, knee_effort, hip_angle, knee_angle, leg_F); - Eigen::Matrix u_left_real; - u_left_real << wheel_effort, leg_F[1]; - double Fn = calculateSupportForce(leg_F[0], leg_F[1], leg_length, acc_z, x, u_left_real, model_params); + static ros::Time judgeTime; + double Fn = calculateSupportForce(F_leg, Tp, leg_length, leg_len_spd, acc_z, x, model_params, period); - bool unstick_ = Fn < 20; + bool unstick_ = Fn < 15; if (unstick_ != last_unstick_) { @@ -198,7 +233,7 @@ bool Normal::unstickDetection(const double& hip_effort, const double& knee_effor } else { - if (ros::Time::now() - judgeTime > ros::Duration(0.01)) + if (ros::Time::now() - judgeTime > ros::Duration(0.1)) { last_unstick_ = unstick_; } diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp index 935c9963..b720d487 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp @@ -23,20 +23,19 @@ void Recover::execute(BipedalController* controller, const ros::Time& time, cons } int left_leg_state, right_leg_state; + double left_desired_angle{ left_pos_[1] }, right_desired_angle{ right_pos_[1] }; LegCommand left_cmd = { 0, 0, { 0., 0. } }, right_cmd = { 0, 0, { 0., 0. } }; detectLegState(x_left_, left_leg_state); detectLegState(x_right_, right_leg_state); - if (controller->getOverturn() && left_leg_state != LegState::FRONT) - left_cmd = computePidLegCommand(0.36, -M_PI / 2 - 0.8, left_pos_[0], left_pos_[1], *pid_legs_[0], *pid_thetas_[0], - left_angle_, period); - if (controller->getOverturn() && right_leg_state != LegState::FRONT) - right_cmd = computePidLegCommand(0.36, -M_PI / 2 - 0.8, right_pos_[0], right_pos_[1], *pid_legs_[1], - *pid_thetas_[1], right_angle_, period); + left_cmd = computePidLegCommand(0.36, left_desired_angle, left_pos_, left_spd_, *pid_legs_[0], *pid_thetas_[0], + *pid_thetas_[2], left_angle_, left_leg_state, period, 0.0, controller->getOverturn()); + right_cmd = + computePidLegCommand(0.36, right_desired_angle, right_pos_, right_spd_, *pid_legs_[1], *pid_thetas_[1], + *pid_thetas_[3], right_angle_, right_leg_state, period, 0.0, controller->getOverturn()); setJointCommands(joint_handles_, left_cmd, right_cmd); // Exit - if ((controller->getOverturn() && left_leg_state == LegState::FRONT && right_leg_state == LegState::FRONT) || - !controller->getOverturn()) + if (!controller->getOverturn() && abs(x_left_[4]) < 0.2 && abs(roll_) < 0.1) { controller->setMode(BalanceMode::STAND_UP); controller->setStateChange(false); diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp index 3e6470e4..b7e334a1 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp @@ -24,25 +24,26 @@ void StandUp::execute(BipedalController* controller, const ros::Time& time, cons controller->setCompleteStand(false); detectLegState(x_left_, left_leg_state); detectLegState(x_right_, right_leg_state); - ROS_INFO("x_left[0]: %f, x_left[1]: %f, x_left[2]: %f, x_left[3]: %f, x_left[4]: %f, x_left[5]: %f\n", x_left_[0], - x_left_[1], x_left_[2], x_left_[3], x_left_[4], x_left_[5]); } + auto model_params_ = controller->getModelParams(); + double spring_force = -model_params_->f_spring; double theta_des_l, theta_des_r, length_des_l, length_des_r; LegCommand left_cmd = { 0, 0, { 0., 0. } }, right_cmd = { 0, 0, { 0., 0. } }; setUpLegMotion(x_left_, right_leg_state, left_pos_[0], left_pos_[1], left_leg_state, theta_des_l, length_des_l); setUpLegMotion(x_right_, left_leg_state, right_pos_[0], right_pos_[1], right_leg_state, theta_des_r, length_des_r); - left_cmd = computePidLegCommand(length_des_l, theta_des_l, left_pos_[0], left_pos_[1], *pid_legs_[0], *pid_thetas_[0], - left_angle_, period); - right_cmd = computePidLegCommand(length_des_r, theta_des_r, right_pos_[0], right_pos_[1], *pid_legs_[1], - *pid_thetas_[1], right_angle_, period); + left_cmd = computePidLegCommand(length_des_l, theta_des_l, left_pos_, left_spd_, *pid_legs_[0], *pid_thetas_[0], + *pid_thetas_[2], left_angle_, left_leg_state, period, spring_force); + right_cmd = computePidLegCommand(length_des_r, theta_des_r, right_pos_, right_spd_, *pid_legs_[1], *pid_thetas_[1], + *pid_thetas_[3], right_angle_, right_leg_state, period, spring_force); + setJointCommands(joint_handles_, left_cmd, right_cmd); // Exit - if (((left_pos_[1] < 0.39 && left_leg_state == LegState::BEHIND) || - (left_pos_[1] > 0. && left_leg_state == LegState::UNDER)) && - ((right_pos_[1] < 0.39 && right_leg_state == LegState::BEHIND) || - (right_pos_[1] > 0. && right_leg_state == LegState::UNDER))) + if (((left_pos_[1] < 0.3 && left_leg_state == LegState::BEHIND) || + (left_pos_[1] > -0.4 && left_leg_state == LegState::UNDER)) && + ((right_pos_[1] < 0.3 && right_leg_state == LegState::BEHIND) || + (right_pos_[1] > -0.4 && right_leg_state == LegState::UNDER))) { controller->setMode(BalanceMode::NORMAL); controller->setStateChange(false); @@ -57,13 +58,17 @@ void StandUp::setUpLegMotion(const Eigen::Matrix& x, const switch (leg_state) { case LegState::UNDER: - theta_des = 0.15; - length_des = 0.2; + theta_des = M_PI / 2 - 0.3; + length_des = 0.36; + if (leg_length > 0.35) + { + leg_state = LegState::FRONT; + } break; case LegState::FRONT: - theta_des = M_PI / 2 + 0.2; + theta_des = M_PI / 2 - 0.3; length_des = 0.36; - if (abs(angles::shortest_angular_distance(x[0], M_PI / 2)) < 0.2 && abs(x[4]) < 0.1) + if (abs(x[0] - theta_des) < 0.1 && abs(x[4]) < 0.2) leg_state = LegState::BEHIND; break; case LegState::BEHIND: @@ -71,8 +76,11 @@ void StandUp::setUpLegMotion(const Eigen::Matrix& x, const length_des = leg_length; if (other_leg_state != LegState::FRONT) { - theta_des = 0.; - length_des = 0.2; + length_des = 0.18; + } + if (leg_length < 0.20) + { + theta_des = 0; } break; } diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp index d40e5758..a0cb00ee 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp @@ -44,6 +44,11 @@ bool VMCController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ROS_ERROR("Load param fail, check the resist of thigh_joint or knee_joint"); return false; } + if (!controller_nh.getParam("spring_force", spring_force_)) + { + ROS_ERROR("Load param fail, check the resist of spring_force"); + return false; + } jointThigh_ = robot_hw->get()->getHandle(thighJoint); jointKnee_ = robot_hw->get()->getHandle(kneeJoint); return true; @@ -52,17 +57,21 @@ bool VMCController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& void VMCController::starting(const ros::Time& /*time*/) { angleCmd_ = 0.; - lengthCmd_ = 0.18; + lengthCmd_ = 0.20; } void VMCController::update(const ros::Time& time, const ros::Duration& period) { double knee_angle = 0, thigh_angle = 0, position[2], speed[2]; - // [0]:thigh_vmc_joint [1]:knee_vmc_joint - thigh_angle = jointThigh_.getPosition() + M_PI_2; - // knee_angle = jointKnee_.getPosition() - 1.6581; - knee_angle = jointKnee_.getPosition() - M_PI_2; + // series leg vmc + // gazebo + // thigh_angle = jointThigh_.getPosition() + M_PI_2; + // knee_angle = jointKnee_.getPosition() - M_PI_2; + + // five link vmc + thigh_angle = jointThigh_.getPosition() + M_PI; + knee_angle = jointKnee_.getPosition(); leg_pos(thigh_angle, knee_angle, position); leg_spd(jointThigh_.getVelocity(), jointKnee_.getVelocity(), thigh_angle, knee_angle, speed); @@ -76,11 +85,10 @@ void VMCController::update(const ros::Time& time, const ros::Duration& period) // angleCmd_ = angleSinCmd_; double angle_error = angles::shortest_angular_distance(position[1], angleCmd_); - effortCmd[0] = pidLength_.computeCommand(lengthCmd_ - position[0], period); - effortCmd[1] = -pidAngle_.computeCommand(angle_error, period); + effortCmd[0] = pidLength_.computeCommand(lengthCmd_ - position[0], period) - spring_force_; + effortCmd[1] = pidAngle_.computeCommand(angle_error, period); leg_conv(effortCmd[0], effortCmd[1], thigh_angle, knee_angle, jointCmd); - std_msgs::Float64MultiArray state; state.data.push_back(thigh_angle); state.data.push_back(knee_angle); @@ -89,6 +97,8 @@ void VMCController::update(const ros::Time& time, const ros::Duration& period) state.data.push_back(speed[0]); state.data.push_back(speed[1]); state.data.push_back(angle_error); + state.data.push_back(effortCmd[0]); + state.data.push_back(effortCmd[1]); statePublisher_.publish(state); std_msgs::Float64MultiArray jointCmdState; diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_conv.c b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_conv.c index 454640c2..0a843498 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_conv.c +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_conv.c @@ -7,7 +7,8 @@ /* Include Files */ #include "bipedal_wheel_controller/vmc/leg_conv.h" - +#include "bipedal_wheel_controller/vmc/leg_spd.h" +#include "bipedal_wheel_controller/vmc/leg_params.h" #include /* Function Definitions */ @@ -22,9 +23,8 @@ * double T[2] * Return Type : void */ -void leg_conv(double F, double Tp, double phi1, double phi2, double T[2]) +void leg_conv(double F, double Tp, double phi1, double phi4, double T[2]) { - Tp = -Tp; double t11_tmp; double t12_tmp; double t21; @@ -38,7 +38,7 @@ void leg_conv(double F, double Tp, double phi1, double phi2, double T[2]) /* This function was generated by the Symbolic Math Toolbox version 23.2. */ /* 2025-09-15 13:09:38 */ - t33 = phi1 + phi2; + t33 = phi1 + phi4; t11_tmp = cos(t33); t12_tmp = sin(t33); t21 = t11_tmp * 0.26; @@ -55,6 +55,114 @@ void leg_conv(double F, double Tp, double phi1, double phi2, double T[2]) t33 * (t21 / t32_tmp + t30_tmp * t34 * t23); } +// void leg_conv(double F, double Tp, double phi1, double phi4, double T[2]) +//{ +// double t112; +// double t114_tmp; +// double t118_tmp; +// double t130_tmp; +// double t133_tmp; +// double t141_tmp; +// double t16_tmp; +// double t184; +// double t18_tmp; +// double t33_tmp; +// double t34_tmp; +// double t35_tmp; +// double t36_tmp; +// double t49_tmp; +// double t4_tmp; +// double t50_tmp; +// double t57_tmp; +// double t58_tmp; +// double t5_tmp; +// double t64_tmp; +// double t65_tmp; +// double t66_tmp; +// double t6_tmp; +// double t7_tmp; +// double t91_tmp; +// /* This function was generated by the Symbolic Math Toolbox version 23.2. +// */ +// /* 2025-10-13 22:10:54 */ +// t4_tmp = cos(phi1); +// t5_tmp = cos(phi4); +// t6_tmp = sin(phi1); +// t7_tmp = sin(phi4); +// t16_tmp = t4_tmp * 0.218; +// t18_tmp = t6_tmp * 0.218; +// t33_tmp = t4_tmp * 0.11336; +// t34_tmp = t5_tmp * 0.11336; +// t35_tmp = t6_tmp * 0.11336; +// t36_tmp = t7_tmp * 0.11336; +// t49_tmp = t16_tmp - t5_tmp * 0.218; +// t50_tmp = t18_tmp - t7_tmp * 0.218; +// t57_tmp = t33_tmp - t34_tmp; +// t58_tmp = t35_tmp - t36_tmp; +// t184 = t6_tmp * t49_tmp * 0.436; +// t64_tmp = t7_tmp * t49_tmp * 0.436; +// t65_tmp = t4_tmp * t50_tmp * 0.436; +// t66_tmp = t5_tmp * t50_tmp * 0.436; +// t91_tmp = t49_tmp * t49_tmp + t50_tmp * t50_tmp; +// t49_tmp = 1.0 / ((t34_tmp - t33_tmp) + t91_tmp); +// t50_tmp = t49_tmp * t49_tmp; +// t112 = sqrt((t57_tmp * t57_tmp + t58_tmp * t58_tmp) - t91_tmp * t91_tmp); +// t114_tmp = 1.0 / t112; +// t118_tmp = (t36_tmp - t35_tmp) + t112; +// t130_tmp = atan(t49_tmp * t118_tmp) * 2.0; +// t133_tmp = cos(t130_tmp); +// t130_tmp = sin(t130_tmp); +// t141_tmp = 1.0 / (t50_tmp * (t118_tmp * t118_tmp) + 1.0); +// t112 = t50_tmp * ((t35_tmp + t65_tmp) - t184) * t118_tmp + +// t49_tmp * (t33_tmp - t114_tmp * +// ((t4_tmp * t58_tmp * 0.22672 - +// t6_tmp * t57_tmp * 0.22672) + +// t91_tmp * (t184 - t65_tmp) * 2.0) / +// 2.0); +// t6_tmp = t50_tmp * ((t36_tmp + t66_tmp) - t64_tmp) * t118_tmp + +// t49_tmp * (t34_tmp - t114_tmp * +// ((t5_tmp * t58_tmp * 0.22672 - +// t7_tmp * t57_tmp * 0.22672) + +// t91_tmp * (t64_tmp - t66_tmp) * 2.0) / +// 2.0); +// t33_tmp = t16_tmp + t133_tmp * 0.26; +// t35_tmp = t18_tmp + t130_tmp * 0.26; +// t114_tmp = t35_tmp * t35_tmp; +// t184 = 1.0 / t35_tmp; +// t118_tmp = t114_tmp + t33_tmp * t33_tmp; +// t4_tmp = t130_tmp * t141_tmp; +// t91_tmp = t18_tmp - t4_tmp * t112 * 0.52; +// t130_tmp = t133_tmp * t141_tmp; +// t50_tmp = t16_tmp - t130_tmp * t112 * 0.52; +// t112 = F * (1.0 / sqrt(t118_tmp)); +// t49_tmp = Tp * t114_tmp * (1.0 / t118_tmp); +// t118_tmp = t33_tmp * (1.0 / t114_tmp); +// T[0] = t112 * (t35_tmp * t50_tmp * 2.0 - t33_tmp * t91_tmp * 2.0) / 2.0 + +// t49_tmp * (t184 * t91_tmp + t118_tmp * t50_tmp); +// T[1] = t112 * +// (t130_tmp * t35_tmp * t6_tmp * 1.04 - +// t4_tmp * t33_tmp * t6_tmp * 1.04) / +// 2.0 + +// t49_tmp * (t184 * (t4_tmp * t6_tmp * 0.52) - +// t118_tmp * (0.0 - t130_tmp * t6_tmp * 0.52)); +// } + +// void leg_conv(double F, double Tp, double phi1, double phi4, double T[2]) +//{ +// double J[2][2]; +// CalcJacobian(phi1, phi4, J); +// // clang-format off +// double JT[2][2] = {{J[0][0],J[1][0]}, // 转置矩阵 +// {J[0][1],J[1][1]}}; +// double F_[2] = {F, Tp}; +// // clang-format on +// double T1 = JT[0][0] * F_[0] + JT[0][1] * F_[1]; +// double T2 = JT[1][0] * F_[0] + JT[1][1] * F_[1]; +// +// T[0] = T1; +// T[1] = T2; +// } + /* * File trailer for leg_conv.c * diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_conv_fwd.c b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_conv_fwd.c deleted file mode 100644 index 41ed5f64..00000000 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_conv_fwd.c +++ /dev/null @@ -1,127 +0,0 @@ -/* - * File: leg_conv_fwd.c - * - * MATLAB Coder version : 5.5 - * C/C++ source code generated on : 15-Sep-2025 20:51:54 - */ - -/* Include Files */ -#include "bipedal_wheel_controller/vmc/leg_conv_fwd.h" - -#include - -/* Function Definitions */ -/* - * LEG_CONV_FWD - * T_real = LEG_CONV_FWD(T1,T2,PHI1,PHI2) - * - * Arguments : double T1 - * double T2 - * double phi1 - * double phi2 - * double T_real[2] - * Return Type : void - */ -void leg_conv_fwd(double T1, double T2, double phi1, double phi2, double T_real[2]) -{ - double T_real_tmp; - double b_T_real_tmp; - double c_T_real_tmp; - double d_T_real_tmp; - double e_T_real_tmp; - double f_T_real_tmp; - double g_T_real_tmp; - double h_T_real_tmp; - double i_T_real_tmp; - double j_T_real_tmp; - double k_T_real_tmp; - double l_T_real_tmp; - double m_T_real_tmp; - double n_T_real_tmp; - double t11_tmp; - double t19_tmp; - double t21_tmp; - double t23_tmp; - double t25_tmp; - double t4_tmp; - double t5_tmp; - double t7_tmp; - /* This function was generated by the Symbolic Math Toolbox version 23.2. - */ - /* 2025-09-15 20:51:54 */ - t4_tmp = cos(phi1); - t5_tmp = sin(phi1); - t7_tmp = phi1 + phi2; - t11_tmp = cos(t7_tmp); - t7_tmp = sin(t7_tmp); - t19_tmp = t4_tmp * t4_tmp; - t21_tmp = t5_tmp * t5_tmp; - t23_tmp = t11_tmp * t11_tmp; - t25_tmp = t7_tmp * t7_tmp; - T_real_tmp = T2 * 0.0 * 11881.0; - b_T_real_tmp = T1 * 0.0 * 16900.0; - c_T_real_tmp = T2 * 0.0 * 16900.0; - d_T_real_tmp = T1 * t4_tmp; - e_T_real_tmp = T2 * t4_tmp; - f_T_real_tmp = T1 * 0.0 * 0.0 * 14170.0; - g_T_real_tmp = e_T_real_tmp * 0.0; - h_T_real_tmp = T2 * 0.0 * 0.0 * 28340.0; - i_T_real_tmp = T1 * t5_tmp; - j_T_real_tmp = T2 * t5_tmp; - k_T_real_tmp = T1 * 0.0 * t7_tmp; - l_T_real_tmp = T2 * 0.0 * t7_tmp; - m_T_real_tmp = t4_tmp * t11_tmp; - n_T_real_tmp = t5_tmp * t7_tmp; - T_real[0] = - sqrt(((((m_T_real_tmp * 0.11336 + n_T_real_tmp * 0.11336) + t19_tmp * 0.047524) + t21_tmp * 0.047524) + - t23_tmp * 0.0676) + - t25_tmp * 0.0676) / - (t4_tmp * t7_tmp - t11_tmp * t5_tmp) * - (((((((((((((((((((((((T_real_tmp + T2 * t19_tmp * 11881.0) + T_real_tmp) - b_T_real_tmp) + T2 * t21_tmp * 11881.0) - - T1 * t23_tmp * 16900.0) + - c_T_real_tmp) - - b_T_real_tmp) + - T2 * t23_tmp * 16900.0) - - T1 * t25_tmp * 16900.0) + - c_T_real_tmp) + - T2 * t25_tmp * 16900.0) + - T2 * 0.0 * t5_tmp * 23762.0) - - g_T_real_tmp * 23762.0) - - f_T_real_tmp) + - h_T_real_tmp) - - k_T_real_tmp * 14170.0) - - d_T_real_tmp * t11_tmp * 14170.0) + - d_T_real_tmp * 0.0 * 14170.0) + - T1 * 0.0 * t11_tmp * 14170.0) - - i_T_real_tmp * 0.0 * 14170.0) + - l_T_real_tmp * 28340.0) + - (((((((((((e_T_real_tmp * t11_tmp * 28340.0 - f_T_real_tmp) - g_T_real_tmp * 28340.0) - - T2 * 0.0 * t11_tmp * 28340.0) + - j_T_real_tmp * 0.0 * 28340.0) + - h_T_real_tmp) - - i_T_real_tmp * t7_tmp * 14170.0) + - j_T_real_tmp * t7_tmp * 28340.0) - - k_T_real_tmp * 33800.0) + - T1 * t11_tmp * 0.0 * 33800.0) + - l_T_real_tmp * 33800.0) - - T2 * t11_tmp * 0.0 * 33800.0)) * - -17.642907551164431 / - (((((((((((((t19_tmp * 11881.0 + t21_tmp * 11881.0) + t23_tmp * 16900.0) + t25_tmp * 16900.0) + - 0.0 * t5_tmp * 23762.0) - - t4_tmp * 0.0 * 23762.0) + - 0.0 * t7_tmp * 28340.0) + - m_T_real_tmp * 28340.0) - - t4_tmp * 0.0 * 28340.0) - - 0.0 * t11_tmp * 28340.0) + - t5_tmp * 0.0 * 28340.0) + - n_T_real_tmp * 28340.0) + - 0.0 * t7_tmp * 33800.0) - - t11_tmp * 0.0 * 33800.0)); - T_real[1] = -T1; -} - -/* - * File trailer for leg_conv_fwd.c - * - * [EOF] - */ diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_pos.c b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_pos.c index 56ebf49b..5953ae38 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_pos.c +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_pos.c @@ -7,6 +7,8 @@ /* Include Files */ #include "bipedal_wheel_controller/vmc/leg_pos.h" +#include "bipedal_wheel_controller/vmc/leg_params.h" +#include #include /* Function Definitions */ @@ -20,20 +22,80 @@ * Return Type : void */ -void leg_pos(double phi1, double phi2, double pos[2]) +void leg_pos(double phi1, double phi4, double pos[2]) { double a_tmp; double t4; /* This function was generated by the Symbolic Math Toolbox version 9.2. */ /* 15-Sep-2025 20:28:02 */ - t4 = phi1 + phi2; + t4 = phi1 + phi4; a_tmp = cos(phi1) * 0.218 + cos(t4) * 0.26; t4 = sin(phi1) * 0.218 + sin(t4) * 0.26; pos[0] = sqrt(a_tmp * a_tmp + t4 * t4); pos[1] = atan2(t4, a_tmp); } +// void leg_pos(double phi1, double phi4, double pos[2]) +//{ +// double a; +// double t12; +// double t13; +// double t14; +// double t15; +// double t2; +// double t3; +// double t4; +// double t5; +// double t6; +// double t8; +// /* This function was generated by the Symbolic Math Toolbox version 23.2. +// */ +// /* 2025-10-13 22:10:52 */ +// t2 = cos(phi1); +// t3 = cos(phi4); +// t4 = sin(phi1); +// t5 = sin(phi4); +// t6 = t2 * 0.218; +// t8 = t4 * 0.218; +// t12 = t2 * 0.11336; +// t13 = t3 * 0.11336; +// t14 = t4 * 0.11336; +// t15 = t5 * 0.11336; +// a = t6 - t3 * 0.218; +// t4 = t8 - t5 * 0.218; +// t3 = t12 - t13; +// t5 = t14 - t15; +// t4 = a * a + t4 * t4; +// t4 = atan(1.0 / ((t13 - t12) + t4) * ((t15 - t14) + sqrt((t3 * t3 + t5 * t5) - t4 * t4))) * 2.0; +// t3 = cos(t4); +// a = t6 + t3 * 0.26; +// t4 = t8 + sin(t4) * 0.26; +// pos[0] = sqrt(a * a + t4 * t4); +// pos[1] = atan2(t2 * -0.218 - t3 * 0.26, t4); +// } + +// void leg_pos(double phi1, double phi4, double pos[2]) +//{ +// double YD, YB, XD, XB, lBD, A0, B0, C0, phi2, XC, YC; +// double L0, Phi0; +// YD = LEG_L4 * sin(phi4); +// YB = LEG_L1 * sin(phi1); +// XD = LEG_L5 + LEG_L4 * cos(phi4); +// XB = LEG_L1 * cos(phi1); +// lBD = sqrt((XD - XB) * (XD - XB) + (YD - YB) * (YD - YB)); +// A0 = 2 * LEG_L2 * (XD - XB); +// B0 = 2 * LEG_L2 * (YD - YB); +// C0 = LEG_L2 * LEG_L2 + lBD * lBD - LEG_L3 * LEG_L3; +// phi2 = 2 * atan2((B0 + sqrt(A0 * A0 + B0 * B0 - C0 * C0)), (A0 + C0)); +// XC = LEG_L1 * cos(phi1) + LEG_L2 * cos(phi2); +// YC = LEG_L1 * sin(phi1) + LEG_L2 * sin(phi2); +// L0 = sqrt((XC - LEG_L5 / 2) * (XC - LEG_L5 / 2) + YC * YC); +// Phi0 = -atan2((XC - LEG_L5 / 2), YC); +// pos[0] = L0; +// pos[1] = Phi0; +// } + /* * File trailer for leg_pos.c * diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_spd.c b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_spd.c index 5b484886..229e9f54 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_spd.c +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_spd.c @@ -7,22 +7,55 @@ /* Include Files */ #include "bipedal_wheel_controller/vmc/leg_spd.h" - +#include "bipedal_wheel_controller/vmc/leg_params.h" #include +void CalcJacobian(double phi1, double phi4, double J_[2][2]) +{ + double YD, YB, XD, XB, lBD, A0, B0, C0, XC, YC; + double phi2, phi3; + double L0, phi0; + double j11, j12, j21, j22; + + YD = LEG_L4 * sin(phi4); + YB = LEG_L1 * sin(phi1); + XD = LEG_L5 + LEG_L4 * cos(phi4); + XB = LEG_L1 * cos(phi1); + lBD = sqrt((XD - XB) * (XD - XB) + (YD - YB) * (YD - YB)); + A0 = 2 * LEG_L2 * (XD - XB); + B0 = 2 * LEG_L2 * (YD - YB); + C0 = LEG_L2 * LEG_L2 + lBD * lBD - LEG_L3 * LEG_L3; + phi2 = 2 * atan2((B0 + sqrt(A0 * A0 + B0 * B0 - C0 * C0)), A0 + C0); + phi3 = atan2(YB - YD + LEG_L2 * sin(phi2), XB - XD + LEG_L2 * cos(phi2)); + XC = LEG_L1 * cos(phi1) + LEG_L2 * cos(phi2); + YC = LEG_L1 * sin(phi1) + LEG_L2 * sin(phi2); + L0 = sqrt((XC - LEG_L5 / 2) * (XC - LEG_L5 / 2) + YC * YC); + phi0 = atan2(YC, XC - LEG_L5 / 2); + + j11 = (LEG_L1 * sin(phi0 - phi3) * sin(phi1 - phi2)) / sin(phi3 - phi2); + j12 = (LEG_L4 * sin(phi0 - phi2) * sin(phi3 - phi4)) / sin(phi3 - phi2); + j21 = (LEG_L1 * cos(phi0 - phi3) * sin(phi1 - phi2)) / (L0 * sin(phi3 - phi2)); + j22 = (LEG_L4 * cos(phi0 - phi2) * sin(phi3 - phi4)) / (L0 * sin(phi3 - phi2)); + + J_[0][0] = j11; + J_[0][1] = j12; + J_[1][0] = j21; + J_[1][1] = j22; +} + /* Function Definitions */ /* * LEG_SPD - * SPD = LEG_SPD(DPHI1,DPHI2,PHI1,PHI2) + * SPD = LEG_SPD(DPHI1,DPHI4,PHI1,PHI4) * * Arguments : double dphi1 - * double dphi2 + * double dphi4 * double phi1 - * double phi2 + * double phi4 * double spd[2] * Return Type : void */ -void leg_spd(double dphi1, double dphi2, double phi1, double phi2, double spd[2]) +void leg_spd(double dphi1, double dphi4, double phi1, double phi4, double spd[2]) { double t29_tmp; double t30_tmp; @@ -36,7 +69,7 @@ void leg_spd(double dphi1, double dphi2, double phi1, double phi2, double spd[2] /* This function was generated by the Symbolic Math Toolbox version 23.2. */ /* 2025-09-15 20:28:02 */ - t5 = phi1 + phi2; + t5 = phi1 + phi4; t9 = cos(t5); t5 = sin(t5); t29_tmp = cos(phi1) * 0.14 + t9 / 4.0; @@ -46,11 +79,111 @@ void leg_spd(double dphi1, double dphi2, double phi1, double phi2, double spd[2] t35 = 1.0 / t34; t37_tmp = t32 + t34; t37 = 1.0 / t37_tmp; - spd[0] = dphi2 / sqrt(t37_tmp) * (t9 * t30_tmp / 2.0 - t5 * t29_tmp / 2.0) / 2.0; + spd[0] = dphi4 / sqrt(t37_tmp) * (t9 * t30_tmp / 2.0 - t5 * t29_tmp / 2.0) / 2.0; spd[1] = - -(dphi2 * t34 * t37 * (t9 / 4.0 / t29_tmp + t30_tmp * t35 * (t5 / 4.0)) + dphi1 * t34 * t37 * (t32 * t35 + 1.0)); + (dphi4 * t34 * t37 * (t9 / 4.0 / t29_tmp + t30_tmp * t35 * (t5 / 4.0)) + dphi1 * t34 * t37 * (t32 * t35 + 1.0)); } +// void leg_spd(double dphi1, double dphi4, double phi1, double phi4, +// double spd[2]) +//{ +// double t10_tmp; +// double t12_tmp; +// double t2; +// double t21; +// double t22; +// double t23; +// double t24; +// double t29; +// double t3; +// double t30; +// double t33; +// double t34; +// double t37; +// double t38; +// double t39; +// double t4; +// double t40; +// double t5; +// double t51; +// double t57; +// double t64; +// double t66; +// double t73; +// double t81; +// /* This function was generated by the Symbolic Math Toolbox version 23.2. +// */ +// /* 2025-10-13 22:10:53 */ +// t2 = cos(phi1); +// t3 = cos(phi4); +// t4 = sin(phi1); +// t5 = sin(phi4); +// t10_tmp = t2 * 0.218; +// t12_tmp = t4 * 0.218; +// t21 = t2 * 0.11336; +// t22 = t3 * 0.11336; +// t23 = t4 * 0.11336; +// t24 = t5 * 0.11336; +// t29 = t10_tmp - t3 * 0.218; +// t30 = t12_tmp - t5 * 0.218; +// t33 = t21 - t22; +// t34 = t23 - t24; +// t37 = t4 * t29 * 0.436; +// t38 = t5 * t29 * 0.436; +// t39 = t2 * t30 * 0.436; +// t40 = t3 * t30 * 0.436; +// t51 = t29 * t29 + t30 * t30; +// t57 = 1.0 / ((t22 - t21) + t51); +// t29 = t57 * t57; +// t64 = sqrt((t33 * t33 + t34 * t34) - t51 * t51); +// t30 = 1.0 / t64; +// t66 = (t24 - t23) + t64; +// t73 = atan(t57 * t66) * 2.0; +// t64 = cos(t73); +// t73 = sin(t73); +// t81 = 1.0 / (t29 * (t66 * t66) + 1.0); +// t37 = t29 * ((t23 + t39) - t37) * t66 + +// t57 * (t21 - t30 * +// ((t2 * t34 * 0.22672 - t4 * t33 * 0.22672) + +// t51 * (t37 - t39) * 2.0) / +// 2.0); +// t29 = t29 * ((t24 + t40) - t38) * t66 + +// t57 * (t22 - t30 * +// ((t3 * t34 * 0.22672 - t5 * t33 * 0.22672) + +// t51 * (t38 - t40) * 2.0) / +// 2.0); +// t4 = t10_tmp + t64 * 0.26; +// t21 = t12_tmp + t73 * 0.26; +// t2 = t64 * t81; +// t23 = t2 * t29; +// t64 = t73 * t81; +// t66 = t64 * t29; +// t51 = t21 * t21; +// t57 = 1.0 / t21; +// t30 = t51 + t4 * t4; +// t29 = 1.0 / sqrt(t30); +// t73 = 1.0 / t30; +// t64 = t12_tmp - t64 * t37 * 0.52; +// t30 = t10_tmp - t2 * t37 * 0.52; +// spd[0] = dphi4 * t29 * (t21 * t23 * 1.04 - t4 * t66 * 1.04) / 2.0 + +// dphi1 * t29 * (t21 * t30 * 2.0 - t4 * t64 * 2.0) / 2.0; +// t29 = t4 * (1.0 / t51); +// spd[1] = dphi1 * t51 * t73 * (t57 * t64 + t29 * t30) + +// dphi4 * t51 * t73 * (t57 * (t66 * 0.52) - t29 * (0.0 - t23 * 0.52)); +// } + +// void leg_spd(double dphi1, double dphi4, double phi1, double phi4, double spd[2]) +//{ +// double J[2][2]; +// CalcJacobian(phi1, phi4, J); +// // clang-format off +// double d_l0 = J[0][0] * dphi1 + J[0][1] * dphi4; +// double d_phi0 = J[1][0] * dphi1 + J[1][1] * dphi4; +// // clang-format on +// spd[0] = d_l0; +// spd[1] = d_phi0; +// } + /* * File trailer for leg_spd.c * diff --git a/rm_chassis_controllers/test/vmc_controller.yaml b/rm_chassis_controllers/test/vmc_controller.yaml index b247be8a..2a4f21e0 100644 --- a/rm_chassis_controllers/test/vmc_controller.yaml +++ b/rm_chassis_controllers/test/vmc_controller.yaml @@ -8,7 +8,8 @@ controllers: vmc_controller: type: rm_chassis_controllers/VMCController vmc_bias_angle: 1.57 - pid_length: { p: 900.0, i: 0, d: 40, i_clamp_max: 15.0, i_clamp_min: -15.0, antiwindup: true, publish_state: true } - pid_angle: { p: 25.0, i: 0, d: 5, i_clamp_max: 15.0, i_clamp_min: -15.0, antiwindup: true, publish_state: true } - thigh_joint: right_hip_joint - knee_joint: right_knee_joint + spring_force: 75.0 + pid_length: { p: 800.0, i: 0, d: 20, i_clamp_max: 20.0, i_clamp_min: -20.0, antiwindup: true, publish_state: true } + pid_angle: { p: 30.0, i: 0, d: 2, i_clamp_max: 15.0, i_clamp_min: -15.0, antiwindup: true, publish_state: true } + thigh_joint: left_hip_joint + knee_joint: left_knee_joint From 3dfc36ba4ad72f238ec9c5e6d3b6c4adada94825 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Fri, 19 Dec 2025 10:49:00 +0800 Subject: [PATCH 12/57] Updated series_legged controller. --- .../bipedal_wheel_controller/controller.h | 14 +- .../controller_mode/normal.h | 13 +- .../bipedal_wheel_controller/definitions.h | 20 +- .../helper_functions.h | 13 +- .../bipedal_wheel_controller/controller.cpp | 80 +++++--- .../controller_mode/normal.cpp | 176 ++++++++++------ .../controller_mode/recover.cpp | 16 +- .../controller_mode/sit_down.cpp | 4 +- .../controller_mode/stand_up.cpp | 10 +- .../controller_mode/upstairs.cpp | 8 +- .../bipedal_wheel_controller/dynamics/gen_A.c | 189 +++++++++--------- .../bipedal_wheel_controller/dynamics/gen_B.c | 149 +++++++------- .../series_legged_vmc_controller.cpp | 2 + 13 files changed, 413 insertions(+), 281 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h index 759ea236..f17ccb95 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -42,7 +43,9 @@ class BipedalController : public ChassisBase getCoeffs() { return coeffs_; } - const std::shared_ptr& getModelParams(){ return model_params_; } + const std::shared_ptr& getModelParams() const { return model_params_; } + const std::shared_ptr& getControlParams() const { return control_params_; } + const std::shared_ptr& getBiasParams() const { return bias_params_; } double getLegCmd() const{ return legCmd_; } double getJumpCmd() const{ return jumpCmd_; } int getBaseState() const{ return state_; } @@ -59,6 +62,7 @@ class BipedalController : public ChassisBase left_ref, Eigen::Matrix right_ref, Eigen::Matrix u_left, Eigen::Matrix u_right, Eigen::Matrix F_leg_, const bool unstick[2]) const; + void pubLegLenStatus(const bool& is_high_leg_flag); // clang-format on void clearStatus(); @@ -66,6 +70,7 @@ class BipedalController : public ChassisBase>& Ks, const std::vector& L0s, Eigen::Matrix& coeffs); @@ -75,6 +80,7 @@ class BipedalController : public ChassisBase r_{}; std::shared_ptr model_params_; + std::shared_ptr control_params_; std::shared_ptr bias_params_; int balance_mode_ = BalanceMode::SIT_DOWN; @@ -91,7 +97,7 @@ class BipedalController : public ChassisBase> kalmanFilterPtr_; Eigen::Matrix x_left_{}, x_right_{}; - double default_leg_length_{ 0.18 }; + double default_leg_length_{ 0.2 }; bool move_flag_{ false }; // stand up bool complete_stand_ = false, overturn_ = false; @@ -108,8 +114,8 @@ class BipedalController : public ChassisBase& model_params, - Eigen::Matrix x, const ros::Duration& period); + Eigen::Matrix x, + std::shared_ptr> supportForceAveragePtr, + const ros::Duration& period); std::vector joint_handles_; std::vector pid_legs_; control_toolbox::Pid *pid_yaw_vel_, *pid_theta_diff_, *pid_roll_; - double pos_des_{ 0.0 }, leg_length_des{ 0.18 }; - int jump_phase_ = JumpPhase::SQUAT; - bool start_jump_ = false; - std::unique_ptr> supportForceAveragePtr; + ros::Time lastJumpTime_{}; + + double pos_des_{ 0.0 }, leg_length_des{ 0.2 }; + int jump_phase_ = JumpPhase::IDLE, jumpTime_{ 0 }; + std::shared_ptr> leftSupportForceAveragePtr_, rightSupportForceAveragePtr_; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h index 7059e665..6b2ec1ae 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h @@ -23,6 +23,16 @@ struct ModelParams double r; // Wheel radius double g; // Gravity acceleration double f_spring; // Spring Force + double f_gravity; // Gravity Force +}; + +struct ControlParams +{ + double jumpOverTime_; + double p1_; + double p2_; + double p3_; + double p4_; }; struct BiasParams @@ -48,10 +58,10 @@ enum LegState enum JumpPhase { - SQUAT, - JUMP, - SHRINK, - DONE + LEG_RETRACTION, + JUMP_UP, + OFF_GROUND, + IDLE, }; enum BalanceMode @@ -64,7 +74,7 @@ enum BalanceMode }; constexpr std::array, 3> jumpLengthDes = { - { { JumpPhase::SQUAT, 0.15 }, { JumpPhase::JUMP, 0.38 }, { JumpPhase::SHRINK, 0.15 } } + { { JumpPhase::LEG_RETRACTION, 0.2 }, { JumpPhase::JUMP_UP, 0.36 }, { JumpPhase::OFF_GROUND, 0.22 } } }; constexpr static const int STATE_DIM = 6; diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h index 2a6a691c..bf5423a9 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h @@ -61,11 +61,11 @@ inline void generateAB(const std::shared_ptr& model_params, Eigen:: */ inline void detectLegState(const Eigen::Matrix& x, int& leg_state) { - if (x[0] > -M_PI / 2 + 0.7 && x[0] < M_PI / 2 - 0.7) + 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) + 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 - 0.7 && x[0] < M_PI) + else if (x[0] > (M_PI / 2 - 1.4) && x[0] < M_PI - 0.5) leg_state = LegState::BEHIND; switch (leg_state) { @@ -104,7 +104,12 @@ inline LegCommand computePidLegCommand(double desired_length, double desired_ang if (leg_state == LegState::BEHIND && !overturn) cmd.torque = angle_pid.computeCommand(-angles::shortest_angular_distance(desired_angle, leg_pos[1]), period); else - cmd.torque = angle_vel_pid.computeCommand(-3.5 - leg_spd[1], period); + { + 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); + } leg_conv(cmd.force, cmd.torque, leg_angle[0], leg_angle[1], cmd.input); return cmd; } diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp index 8e1832b4..cf71ab43 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp @@ -38,9 +38,11 @@ bool BipedalController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan mode_manager_ = std::make_unique(controller_nh, joint_handles_); model_params_ = std::make_shared(); + control_params_ = std::make_shared(); bias_params_ = std::make_shared(); - if (!setupModelParams(controller_nh) || !setupLQR(controller_nh) || !setupBiasParams(controller_nh)) + if (!setupModelParams(controller_nh) || !setupLQR(controller_nh) || !setupBiasParams(controller_nh) || + !setupControlParams(controller_nh)) return false; auto legCmdCallback = [this](const rm_msgs::LegCmd::ConstPtr& msg) { @@ -50,7 +52,9 @@ bool BipedalController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan leg_cmd_sub_ = controller_nh.subscribe("/leg_cmd", 1, legCmdCallback); unstick_pub_ = controller_nh.advertise("unstick", 1); + leg_len_status_pub_ = controller_nh.advertise("leg_len_status", 1); legged_chassis_status_pub_ = controller_nh.advertise("legged_chassis_status", 1); + legged_chassis_mode_pub_ = controller_nh.advertise("legged_chassis_mode", 1); lqr_status_pub_ = controller_nh.advertise("lqr_status", 1); x_left_.setZero(); x_right_.setZero(); @@ -58,8 +62,8 @@ bool BipedalController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan // Slippage detection A_ << 1, 0, 0, 1; H_ << 1, 0, 0, 1; - Q_ << 25, 0, 0, 2000; - R_ << 800, 0, 0, 0.01; + Q_ << 1, 0, 0, 1; + R_ << 200, 0, 0, 200; R_wheel_ = R_(0, 0); slip_R_wheel_ = slip_alpha_ * R_wheel_; B_.setZero(); @@ -75,6 +79,11 @@ void BipedalController::moveJoint(const ros::Time& time, const ros::Duration& pe { if (!balance_state_changed_) mode_manager_->switchMode(balance_mode_); + if (getBaseState() == 4) + { + balance_mode_ = SIT_DOWN; + mode_manager_->switchMode(SIT_DOWN); + } updateEstimation(time, period); mode_manager_->getModeImpl()->execute(this, time, period); pubState(); @@ -133,16 +142,16 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat // vmc double left_angle[2]{}, right_angle[2]{}, left_pos[2]{}, left_spd[2]{}, right_pos[2]{}, right_spd[2]{}; // [0]:hip_vmc_joint [1]:knee_vmc_joint - // left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; - // left_angle[1] = left_knee_joint_handle_.getPosition(); - // right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; - // right_angle[1] = right_knee_joint_handle_.getPosition(); + left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; + left_angle[1] = left_knee_joint_handle_.getPosition(); + right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; + right_angle[1] = right_knee_joint_handle_.getPosition(); // gazebo - left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; - left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; - right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; - right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; + // left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; + // left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; + // right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; + // right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; // [0] is length, [1] is angle leg_pos(left_angle[0], left_angle[1], left_pos); @@ -176,21 +185,18 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat i++; } auto x_hat_vel = kalmanFilterPtr_->getState(); - slip_flag_ = abs(x_hat_vel(0) - wheel_vel_aver) > 0.5; + slip_flag_ = abs(x_hat_vel(0) - wheel_vel_aver) > 1.5; // update state - // x_left_[3] = state_ != RAW ? x_hat_vel(0) : 0; - // x_left_[2] += state_ != RAW ? x_left_[3] * period.toSec() : 0; - x_left_[3] = x_hat_vel(0); - x_left_[2] += x_left_[3] * period.toSec(); - // if (abs(x_left_[3]) <= 0.8f && abs(vel_cmd_.x) <= 0.1f) - // { - // } - // else - // { - // x_left_[2] = 0; - // } - x_left_[2] -= bias_params_->x; + x_left_[3] = state_ != RAW ? x_hat_vel(0) : 0; + if (abs(x_left_[3]) <= 0.5f && abs(vel_cmd_.x) <= 0.1f) + { + x_left_[2] += state_ != RAW ? x_left_[3] * period.toSec() : 0; + } + else + { + x_left_[2] = -bias_params_->x; + } x_left_[0] = (left_pos[1] + pitch); x_left_[1] = left_spd[1] + angular_vel_base.y; x_left_[4] = -pitch; @@ -220,6 +226,10 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat legged_chassis_status_msg.linear_acc_base.push_back(linear_acc_base.z); legged_chassis_status_pub_.publish(legged_chassis_status_msg); + rm_msgs::LeggedChassisMode legged_chassis_mode_msg; + legged_chassis_mode_msg.mode = balance_mode_; + legged_chassis_mode_pub_.publish(legged_chassis_mode_msg); + mode_manager_->getModeImpl()->updateEstimation(x_left_, x_right_); mode_manager_->getModeImpl()->updateLegKinematics(left_angle, right_angle, left_pos, left_spd, right_pos, right_spd); mode_manager_->getModeImpl()->updateBaseState(angular_vel_base, linear_acc_base, roll, pitch, yaw); @@ -254,7 +264,8 @@ bool BipedalController::setupModelParams(ros::NodeHandle& controller_nh) { "Lm_weight", &model_params_->Lm_weight }, { "g", &model_params_->g }, { "wheel_radius", &model_params_->r }, - { "spring_force", &model_params_->f_spring } }; + { "spring_force", &model_params_->f_spring }, + { "gravity_force", &model_params_->f_gravity } }; for (const auto& e : tbl) if (!controller_nh.getParam(e.first, *e.second)) @@ -330,6 +341,18 @@ bool BipedalController::setupBiasParams(ros::NodeHandle& controller_nh) return true; } +bool BipedalController::setupControlParams(ros::NodeHandle& controller_nh) +{ + if (!controller_nh.getParam("jumpOverTime", control_params_->jumpOverTime_) || + !controller_nh.getParam("p1", control_params_->p1_) || !controller_nh.getParam("p2", control_params_->p2_) || + !controller_nh.getParam("p3", control_params_->p3_) || !controller_nh.getParam("p4", control_params_->p4_)) + { + ROS_ERROR("Load param fail, check the resist of jump_over_time, p1, p2, p3, p4"); + return false; + } + return true; +} + void BipedalController::polyfit(const std::vector>& Ks, const std::vector& L0s, Eigen::Matrix& coeffs) { @@ -386,5 +409,12 @@ void BipedalController::pubLQRStatus(Eigen::Matrix left_er lqr_status_pub_.publish(msg); } +void BipedalController::pubLegLenStatus(const bool& is_high_leg_flag) +{ + std_msgs::Bool msg; + msg.data = is_high_leg_flag; + leg_len_status_pub_.publish(msg); +} + } // namespace rm_chassis_controllers PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::BipedalController, controller_interface::ControllerBase) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp index 34e9b105..0702153c 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -16,37 +16,37 @@ Normal::Normal(const std::vector& joint_handle , pid_theta_diff_(pid_theta_diff) , pid_roll_(pid_roll) { - supportForceAveragePtr = std::make_unique>(4); + leftSupportForceAveragePtr_ = std::make_shared>(4); + rightSupportForceAveragePtr_ = std::make_shared>(4); } void Normal::execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) { + auto bias_params_ = controller->getBiasParams(); if (!controller->getStateChange()) { ROS_INFO("[balance] Enter NORMAL"); controller->clearStatus(); - if (abs(pos_des_ - x_left_(2)) > 5) - { - x_left_(2) = x_right_(2) = pos_des_ = 0; - } + jump_phase_ = JumpPhase::IDLE; + pos_des_ = 0; controller->setStateChange(true); } - if (!controller->getCompleteStand() && abs(x_left_[4]) < 0.2 && (abs(x_left_[0] + x_right_[0]) / 2.0f) < 0.1) + if (!controller->getCompleteStand() && abs(x_left_[4]) < 0.2 && (abs(x_left_[0] + x_right_[0]) / 2.0f) < 0.15) { - x_left_(2) = x_right_(2) = pos_des_ = 0; controller->setCompleteStand(true); } auto vel_cmd_ = controller->getVelCmd(); - pos_des_ += vel_cmd_.x * period.toSec(); if (controller->getMoveFlag() && abs(x_left_[3]) < 0.1 && abs(vel_cmd_.x) < 0.1) { controller->setMoveFlag(false); - // pos_des_ = x_left_(2); } + + double friction_circle = abs(x_left_(3) * angular_vel_base_.z); + double friction_circle_alpha = friction_circle > 5.0f ? (5.0f / friction_circle) : 1.0f; // PID - double T_yaw = pid_yaw_vel_->computeCommand(vel_cmd_.z - angular_vel_base_.z, period); + double T_yaw = pid_yaw_vel_->computeCommand(friction_circle_alpha * vel_cmd_.z - angular_vel_base_.z, period); double T_theta_diff = pid_theta_diff_->computeCommand(right_pos_[1] - left_pos_[1], period); double T_roll = pid_roll_->computeCommand(0. - roll_, period); @@ -66,6 +66,7 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const coeffs_(2, i + 2 * j) * right_pos_[0] + coeffs_(3, i + 2 * j); } } + Eigen::Matrix u_left, u_right; u_left.setZero(); u_right.setZero(); @@ -78,11 +79,11 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const if (controller->getCompleteStand()) { x_left_ref(2) = x_right_ref(2) = pos_des_; - x_left_ref(3) = x_right_ref(3) = vel_cmd_.x; + x_left_ref(3) = x_right_ref(3) = friction_circle_alpha * vel_cmd_.x; leg_length_des = controller->getLegCmd(); } - x_left(0) -= 0.03; - x_right(0) -= 0.03; + x_left(0) -= 0.07; + x_right(0) -= 0.07; x_left(4) -= 0.; x_right(4) -= 0.; @@ -92,45 +93,87 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const u_left = k_left * (-x_left); u_right = k_right * (-x_right); + // std::cout << "leg_len:" << left_pos_[0] << std::endl << k_left << std::endl; + // Compute leg thrust auto model_params_ = controller->getModelParams(); - double gravity = model_params_->M * model_params_->g, current_leg_length = (left_pos_[0] + right_pos_[0]) / 2.0f, + auto control_params_ = controller->getControlParams(); + double gravity = model_params_->f_gravity, current_leg_length = (left_pos_[0] + right_pos_[0]) / 2.0f, spring_force = model_params_->f_spring; + double F_inertia = model_params_->M * friction_circle; Eigen::Matrix F_leg; - if (!start_jump_ && controller->getJumpCmd() && abs(x_left[0]) < 0.1) + + // check jump + if (jump_phase_ == JumpPhase::IDLE && + ros::Time::now() - lastJumpTime_ > ros::Duration(control_params_->jumpOverTime_) && controller->getJumpCmd()) { - start_jump_ = true; + jump_phase_ = JumpPhase::LEG_RETRACTION; ROS_INFO("[balance] Jump start"); } - if (start_jump_) + if (jump_phase_ == JumpPhase::IDLE) { - leg_length_des = jumpLengthDes[jump_phase_].second; - if (std::abs(leg_length_des - left_pos_[0]) < 0.01) - jump_phase_ += 1; - if (jump_phase_ == JumpPhase::DONE) - { - jump_phase_ = JumpPhase::SQUAT; - controller->setJumpCmd(false); - start_jump_ = false; - ROS_INFO("[balance] Jump finished"); - } - else if (jump_phase_ == JumpPhase::SHRINK) - { - gravity = 0; - } - F_leg[0] = pid_legs_[0]->computeCommand(leg_length_des - current_leg_length, period) + gravity * cos(left_pos_[1]) + - T_roll - spring_force; - F_leg[1] = pid_legs_[1]->computeCommand(leg_length_des - current_leg_length, period) + + double left_length_des = controller->getCompleteStand() ? leg_length_des : controller->getDefaultLegLength(); + double right_length_des = controller->getCompleteStand() ? leg_length_des : controller->getDefaultLegLength(); + F_leg[0] = pid_legs_[0]->computeCommand(left_length_des - current_leg_length, period) - F_inertia + + gravity * cos(left_pos_[1]) + T_roll - spring_force; + F_leg[1] = pid_legs_[1]->computeCommand(right_length_des - current_leg_length, period) + F_inertia + gravity * cos(right_pos_[1]) - T_roll - spring_force; } else { - double left_length_des = controller->getCompleteStand() ? leg_length_des : controller->getDefaultLegLength(); - double right_length_des = controller->getCompleteStand() ? leg_length_des : controller->getDefaultLegLength(); - F_leg[0] = pid_legs_[0]->computeCommand(left_length_des - left_pos_[0], period) + gravity * cos(left_pos_[1]) + - T_roll - spring_force; - F_leg[1] = pid_legs_[1]->computeCommand(right_length_des - right_pos_[0], period) + gravity * cos(right_pos_[1]) - - T_roll - spring_force; + leg_length_des = jumpLengthDes[jump_phase_].second; + switch (jump_phase_) + { + case JumpPhase::LEG_RETRACTION: + ROS_INFO("[balance] ENTER LEG_RETRACTION"); + F_leg(0) = pid_legs_[0]->computeCommand(leg_length_des - current_leg_length, period) + + gravity * cos(left_pos_[1]) + T_roll - spring_force; + F_leg(1) = pid_legs_[1]->computeCommand(leg_length_des - current_leg_length, period) + + gravity * cos(left_pos_[1]) - T_roll - spring_force; + if (current_leg_length < leg_length_des + 0.1) + { + jumpTime_++; + } + if (jumpTime_ >= 6) + { + jumpTime_ = 0; + jump_phase_ = JumpPhase::JUMP_UP; + } + break; + case JumpPhase::JUMP_UP: + ROS_INFO("[balance] ENTER JUMP_UP"); + F_leg(0) = control_params_->p1_ * pow(left_pos_[0], 3) + control_params_->p2_ * pow(left_pos_[0], 2) + + control_params_->p3_ * left_pos_[0] + control_params_->p4_ + gravity; + F_leg(1) = control_params_->p1_ * pow(right_pos_[0], 3) + control_params_->p2_ * pow(right_pos_[0], 2) + + control_params_->p3_ * right_pos_[0] + control_params_->p4_ + gravity; + if (current_leg_length > leg_length_des) + { + jumpTime_++; + } + if (jumpTime_ >= 2) + { + jumpTime_ = 0; + jump_phase_ = JumpPhase::OFF_GROUND; + } + break; + case JumpPhase::OFF_GROUND: + ROS_INFO("[balance] ENTER OFF_GROUND"); + F_leg(0) = pid_legs_[0]->computeCommand(leg_length_des - current_leg_length, period); + F_leg(1) = pid_legs_[1]->computeCommand(leg_length_des - current_leg_length, period); + + if (current_leg_length < leg_length_des) + { + jumpTime_++; + } + if (jumpTime_ >= 4) + { + jumpTime_ = 0; + jump_phase_ = JumpPhase::IDLE; + lastJumpTime_ = ros::Time::now(); + ROS_INFO("[balance] Jump end"); + } + break; + } } // Unstick detection @@ -140,23 +183,34 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const k_left_unstick.block<1, 2>(1, 0) = k_left.block<1, 2>(1, 0); k_right_unstick.block<1, 2>(1, 0) = k_right.block<1, 2>(1, 0); - bool left_unstick = unstickDetection(F_leg[0], u_left(1), left_spd_[0], left_pos_[0], linear_acc_base_.z, - model_params_, x_left_, period); - bool right_unstick = unstickDetection(F_leg[1], u_right(1), right_spd_[0], right_pos_[0], linear_acc_base_.z, - model_params_, x_right_, period); + bool left_unstick{ false }, right_unstick{ false }; + if (controller->getCompleteStand() && jump_phase_ != JumpPhase::LEG_RETRACTION) + { + left_unstick = unstickDetection(F_leg[0], u_left(1), left_spd_[0], left_pos_[0], linear_acc_base_.z, model_params_, + x_left_, leftSupportForceAveragePtr_, period); + right_unstick = unstickDetection(F_leg[1], u_right(1), right_spd_[0], right_pos_[0], linear_acc_base_.z, + model_params_, x_right_, rightSupportForceAveragePtr_, period); + } bool unstick[2]{}; unstick[0] = left_unstick; unstick[1] = right_unstick; - controller->pubLQRStatus(-x_left, -x_right, x_left_ref, x_right_ref, u_left, u_right, F_leg, unstick); + Matrix F_N{}; + F_N(0) = leftSupportForceAveragePtr_->output(); + F_N(1) = rightSupportForceAveragePtr_->output(); + controller->pubLQRStatus(-x_left, -x_right, x_left_ref, x_right_ref, u_left, u_right, F_N, unstick); // left_unstick = false; // right_unstick = false; updateUnstick(left_unstick, right_unstick); - if (controller->getCompleteStand() && left_unstick && jump_phase_ != JumpPhase::JUMP) + if (controller->getCompleteStand() && left_unstick && jump_phase_ != JumpPhase::LEG_RETRACTION) + { u_left = k_left_unstick * (-x_left); - if (controller->getCompleteStand() && right_unstick && jump_phase_ != JumpPhase::JUMP) + } + if (controller->getCompleteStand() && right_unstick && jump_phase_ != JumpPhase::LEG_RETRACTION) + { u_right = k_right_unstick * (-x_right); + } // Control double left_T[2], right_T[2]; @@ -166,31 +220,34 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const double right_wheel_cmd = right_unstick ? 0. : u_right(0) + T_yaw; LegCommand left_cmd = { F_leg[0], u_left[1], { left_T[0], left_T[1] } }, right_cmd = { F_leg[1], u_right[1], { right_T[0], right_T[1] } }; - setJointCommands(joint_handles_, left_cmd, right_cmd, left_wheel_cmd, right_wheel_cmd); // upstairs - if (x_left[4] < -0.2 && (controller->getCompleteStand()) && abs(vel_cmd_.x) > 0.1 && + if (((x_left[4] < -0.25 && (x_left(0) + x_right(0) / 2.0f) > 0.25) || + (((left_pos_[1] + right_pos_[1]) / 2.0f) > 0.5)) && + controller->getCompleteStand() && abs(vel_cmd_.x) > 1.0 && abs(x_left(3)) > 0.2 && ((left_pos_[0] + right_pos_[0]) / 2.0f) > 0.34) { - // leg_length_des = controller->getDefaultLegLength(); - // controller->setMode(BalanceMode::UPSTAIRS); - // controller->setStateChange(false); - // controller->setJumpCmd(false); - // ROS_INFO("[balance] Exit NORMAL"); + leg_length_des = controller->getDefaultLegLength(); + controller->setMode(BalanceMode::UPSTAIRS); + controller->setStateChange(false); + controller->setJumpCmd(false); + left_wheel_cmd = right_wheel_cmd = 0; + ROS_INFO("[balance] Exit NORMAL"); } // Protection - if ((controller->getCompleteStand() && (abs(x_left(4)) > 0.4 || abs(x_left(0)) > 1.35)) || + if ((controller->getCompleteStand() && (abs(x_left(4)) > 0.95 || abs(x_left(0)) > 0.8)) || controller->getOverturn() || controller->getBaseState() == 4) { leg_length_des = controller->getDefaultLegLength(); - x_left_(2) = x_right_(2) = pos_des_ = 0; + x_left_(2) = x_right_(2) = bias_params_->x; controller->setMode(BalanceMode::SIT_DOWN); controller->setStateChange(false); controller->setJumpCmd(false); setJointCommands(joint_handles_, { 0, 0, { 0., 0. } }, { 0, 0, { 0., 0. } }); ROS_INFO("[balance] Exit NORMAL"); } + setJointCommands(joint_handles_, left_cmd, right_cmd, left_wheel_cmd, right_wheel_cmd); } double Normal::calculateSupportForce(double F, double Tp, double leg_length, const double& leg_len_spd, double acc_z, @@ -206,7 +263,7 @@ double Normal::calculateSupportForce(double F, double Tp, double leg_length, con double ddot_theta = 0.7 * ((x(1) - last_dot_theta) / period.toSec()) + 0.3 * last_ddot_theta; last_dot_theta = x(1); last_ddot_theta = ddot_theta; - double ddot_zw = ddot_zM - leg_length * cos(x(0)) + 2 * leg_length * x(1) * sin(x(0)) + + double ddot_zw = ddot_zM - leg_length * cos(x(0)) + 2 * leg_len_spd * x(1) * sin(x(0)) + +leg_length * (ddot_theta * sin(x(0)) + x(1) * x(1) * cos(x(0))); double Fn = model_params->m_w * ddot_zw + model_params->m_w * model_params->g + P; @@ -216,13 +273,14 @@ double Normal::calculateSupportForce(double F, double Tp, double leg_length, con bool Normal::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& model_params, Eigen::Matrix x, + std::shared_ptr> supportForceAveragePtr, const ros::Duration& period) { static bool maybeChange = false, last_unstick_ = false; static ros::Time judgeTime; double Fn = calculateSupportForce(F_leg, Tp, leg_length, leg_len_spd, acc_z, x, model_params, period); - - bool unstick_ = Fn < 15; + supportForceAveragePtr->input(Fn); + bool unstick_ = supportForceAveragePtr->output() < 15; if (unstick_ != last_unstick_) { diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp index b720d487..b2653472 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp @@ -24,14 +24,20 @@ void Recover::execute(BipedalController* controller, const ros::Time& time, cons int left_leg_state, right_leg_state; double left_desired_angle{ left_pos_[1] }, right_desired_angle{ right_pos_[1] }; + left_desired_angle = left_pos_[1]; + right_desired_angle = right_pos_[1]; LegCommand left_cmd = { 0, 0, { 0., 0. } }, right_cmd = { 0, 0, { 0., 0. } }; detectLegState(x_left_, left_leg_state); detectLegState(x_right_, right_leg_state); - left_cmd = computePidLegCommand(0.36, left_desired_angle, left_pos_, left_spd_, *pid_legs_[0], *pid_thetas_[0], - *pid_thetas_[2], left_angle_, left_leg_state, period, 0.0, controller->getOverturn()); - right_cmd = - computePidLegCommand(0.36, right_desired_angle, right_pos_, right_spd_, *pid_legs_[1], *pid_thetas_[1], - *pid_thetas_[3], right_angle_, right_leg_state, period, 0.0, controller->getOverturn()); + if (controller->getBaseState() != 4) + { + left_cmd = + computePidLegCommand(0.36, left_desired_angle, left_pos_, left_spd_, *pid_legs_[0], *pid_thetas_[0], + *pid_thetas_[2], left_angle_, left_leg_state, period, 0.0, controller->getOverturn()); + right_cmd = + computePidLegCommand(0.36, right_desired_angle, right_pos_, right_spd_, *pid_legs_[1], *pid_thetas_[1], + *pid_thetas_[3], right_angle_, right_leg_state, period, 0.0, controller->getOverturn()); + } setJointCommands(joint_handles_, left_cmd, right_cmd); // Exit diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp index 71b04cfd..5cab5c59 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp @@ -27,9 +27,9 @@ void SitDown::execute(BipedalController* controller, const ros::Time& time, cons setJointCommands(joint_handles_, left_cmd, right_cmd, left_wheel_cmd, right_wheel_cmd); // Exit - if (abs(x_left_(1)) < 0.1 && abs(x_left_(5)) < 0.1 && abs(x_left_(3)) < 0.15 && controller->getBaseState() != 4) + if (abs(x_left_(1)) < 0.1 && abs(x_left_(5)) < 0.2 && controller->getBaseState() != 4) { - if (!controller->getOverturn()) + if (!controller->getOverturn() && abs(x_left_(4)) < 0.1) controller->setMode(BalanceMode::STAND_UP); else controller->setMode(BalanceMode::RECOVER); diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp index b7e334a1..f5114cf8 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp @@ -58,7 +58,7 @@ void StandUp::setUpLegMotion(const Eigen::Matrix& x, const switch (leg_state) { case LegState::UNDER: - theta_des = M_PI / 2 - 0.3; + theta_des = M_PI / 2 - 0.35; length_des = 0.36; if (leg_length > 0.35) { @@ -66,19 +66,19 @@ void StandUp::setUpLegMotion(const Eigen::Matrix& x, const } break; case LegState::FRONT: - theta_des = M_PI / 2 - 0.3; + theta_des = M_PI / 2 - 0.35; length_des = 0.36; - if (abs(x[0] - theta_des) < 0.1 && abs(x[4]) < 0.2) + if (abs(x[0] - theta_des) < 0.3 && abs(x[4]) < 0.3) leg_state = LegState::BEHIND; break; case LegState::BEHIND: theta_des = leg_theta; length_des = leg_length; - if (other_leg_state != LegState::FRONT) + if (other_leg_state == LegState::BEHIND) { length_des = 0.18; } - if (leg_length < 0.20) + if (leg_length < 0.2) { theta_des = 0; } diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp index 35dd9fc3..537684d8 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp @@ -26,7 +26,7 @@ void Upstairs::execute(BipedalController* controller, const ros::Time& time, con detectLegState(x_right_, right_leg_state); } - double theta_des_l{ M_PI_2 + 0.3 }, theta_des_r{ M_PI_2 + 0.3 }, length_des_l{ 0.17 }, length_des_r{ 0.17 }; + double theta_des_l{ M_PI_2 - 0.4 }, theta_des_r{ M_PI_2 - 0.4 }, length_des_l{ 0.18 }, length_des_r{ 0.18 }; LegCommand left_cmd = { 0, 0, { 0., 0. } }, right_cmd = { 0, 0, { 0., 0. } }; left_cmd = computePidLegCommand(length_des_l, theta_des_l, left_pos_, left_spd_, *pid_legs_[0], *pid_thetas_[0], *pid_thetas_[2], left_angle_, left_leg_state, period); @@ -35,9 +35,11 @@ void Upstairs::execute(BipedalController* controller, const ros::Time& time, con setJointCommands(joint_handles_, left_cmd, right_cmd); // Exit - if ((left_pos_[0] < 0.2 && left_pos_[1] > (M_PI_2 - 0.3)) && (right_pos_[0] < 0.2 && right_pos_[1] > (M_PI_2 - 0.3))) + if ((left_pos_[0] < 0.2 && left_pos_[1] > (M_PI_2 - 0.5) && abs(left_spd_[1]) < 0.1) && + (right_pos_[0] < 0.2 && right_pos_[1] > (M_PI_2 - 0.5) && abs(right_spd_[1]) < 0.1)) { - controller->setMode(BalanceMode::NORMAL); + controller->pubLegLenStatus(false); + controller->setMode(BalanceMode::STAND_UP); controller->setStateChange(false); ROS_INFO("[balance] Exit Upstairs"); } diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_A.c b/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_A.c index c049b996..81c66e83 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_A.c +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_A.c @@ -32,41 +32,40 @@ void gen_A(double Im, double Ip, double Iw, double L, double Lm, double M, double R, double g, double l, double mp, double mw, double A[36]) { - double b_t18_tmp; - double b_t33_tmp; - double b_t41_tmp; - double c_t41_tmp; + double b_t22_tmp; + double b_t35_tmp; + double b_t42_tmp; + double c_t42_tmp; double d; double d1; - double d_t41_tmp; - double e_t41_tmp; - double f_t41_tmp; - double g_t41_tmp; - double h_t41_tmp; - double i_t41_tmp; - double j_t41_tmp; - double k_t41_tmp; + double d_t42_tmp; + double e_t42_tmp; + double f_t42_tmp; + double g_t42_tmp; + double h_t42_tmp; + double i_t42_tmp; + double j_t42_tmp; + double k_t42_tmp; double t17_tmp; - double t18_tmp; double t2; - double t23_tmp; + double t22_tmp; + double t24_tmp; double t3; + double t31; double t31_tmp; - double t32_tmp; double t33_tmp; - double t34; double t34_tmp; - double t39_tmp; - double t39_tmp_tmp; + double t35_tmp; + double t36_tmp; double t4; - double t41; - double t41_tmp; + double t42; + double t42_tmp; double t5; double t6; double t7; - /* This function was generated by the Symbolic Math Toolbox version 9.2. + /* This function was generated by the Symbolic Math Toolbox version 23.2. */ - /* 08-Aug-2025 17:33:02 */ + /* 2025-12-05 01:47:45 */ t2 = L * L; t3 = Lm * Lm; t4 = M * M; @@ -74,84 +73,83 @@ void gen_A(double Im, double Ip, double Iw, double L, double Lm, double M, doubl t6 = l * l; t7 = mp * mp; t17_tmp = Iw * M; - t18_tmp = Iw * L; - b_t18_tmp = t18_tmp * Lm; - t23_tmp = Iw * g * l; - t31_tmp = M * g; - t32_tmp = L * g; - t33_tmp = g * l; - b_t33_tmp = t33_tmp * mw; - t34_tmp = Lm * g; - t34 = t34_tmp * mp * t4 * t5 * t6; - t39_tmp_tmp = L * Lm; - t39_tmp = t39_tmp_tmp * g * l; - t41_tmp = Im * Ip; - b_t41_tmp = Im * Iw; - c_t41_tmp = b_t41_tmp * M; - d_t41_tmp = Im * M; - e_t41_tmp = d_t41_tmp * mw; - f_t41_tmp = Ip * M; - g_t41_tmp = b_t41_tmp * L; - h_t41_tmp = Im * L; - i_t41_tmp = h_t41_tmp * Lm; - j_t41_tmp = i_t41_tmp * M; - k_t41_tmp = Ip * Iw * M; - t41 = + t22_tmp = Iw * L; + b_t22_tmp = t22_tmp * Lm; + t24_tmp = Iw * g * l; + t31_tmp = L * Lm; + t31 = t31_tmp * R * g * mp * t4 * t6; + t33_tmp = t31_tmp * g * l; + t34_tmp = L * g; + t35_tmp = g * l; + b_t35_tmp = t35_tmp * mw; + t36_tmp = Lm * g; + t42_tmp = Im * Ip; + b_t42_tmp = Im * Iw; + c_t42_tmp = b_t42_tmp * M; + d_t42_tmp = Im * M; + e_t42_tmp = d_t42_tmp * mw; + f_t42_tmp = Ip * M; + g_t42_tmp = b_t42_tmp * L; + h_t42_tmp = Im * L; + i_t42_tmp = h_t42_tmp * Lm; + j_t42_tmp = i_t42_tmp * M; + k_t42_tmp = Ip * Iw * M; + t42 = 1.0 / - (((((((((((((((((t41_tmp * Iw + k_t41_tmp * t6) + b_t41_tmp * mp * t2) + t41_tmp * mp * t5) + t41_tmp * mw * t5) + - g_t41_tmp * Lm * M * 2.0) + - c_t41_tmp * t2) + - c_t41_tmp * t3) + - t41_tmp * M * t5) + - e_t41_tmp * t2 * t5) + - d_t41_tmp * mp * t3 * t5) + - e_t41_tmp * t3 * t5) + - t17_tmp * mp * t2 * t6) + - f_t41_tmp * mp * t5 * t6) + - f_t41_tmp * mw * t5 * t6) + - Im * mp * mw * t2 * t5) + - j_t41_tmp * mw * t5 * 2.0) + + (((((((((((((((((t42_tmp * Iw + k_t42_tmp * t6) + b_t42_tmp * mp * t2) + t42_tmp * mp * t5) + t42_tmp * mw * t5) + + g_t42_tmp * Lm * M * 2.0) + + c_t42_tmp * t2) + + c_t42_tmp * t3) + + t42_tmp * M * t5) + + j_t42_tmp * mw * t5 * 2.0) + + e_t42_tmp * t2 * t5) + + d_t42_tmp * mp * t3 * t5) + + e_t42_tmp * t3 * t5) + + t17_tmp * mp * t2 * t6) + + f_t42_tmp * mp * t5 * t6) + + f_t42_tmp * mw * t5 * t6) + + Im * mp * mw * t2 * t5) + M * mp * mw * t2 * t5 * t6); A[0] = 0.0; - d = h_t41_tmp * g; - c_t41_tmp = h_t41_tmp * M * g; - e_t41_tmp = Im * Lm; - h_t41_tmp = e_t41_tmp * M * g; + d = h_t42_tmp * g; + c_t42_tmp = h_t42_tmp * M * g; + e_t42_tmp = Im * Lm; + h_t42_tmp = e_t42_tmp * M * g; d1 = L * M * g; - t41_tmp = (((t18_tmp * g * t4 * t6 + Iw * Lm * g * t4 * t6) + t32_tmp * mw * t4 * t5 * t6) + t34) + - t34_tmp * mw * t4 * t5 * t6; - A[1] = t41 * (((((((((((((((t41_tmp + g_t41_tmp * g * mp) + d * t4 * t5) + d * t5 * t7) + e_t41_tmp * g * t4 * t5) + - g_t41_tmp * M * g) + - b_t41_tmp * Lm * M * g) + - c_t41_tmp * mp * t5 * 2.0) + - c_t41_tmp * mw * t5) + - t18_tmp * M * g * mp * t6) + - h_t41_tmp * mp * t5) + - h_t41_tmp * mw * t5) + + t42_tmp = + (((t22_tmp * g * t4 * t6 + Iw * Lm * g * t4 * t6) + t34_tmp * mw * t4 * t5 * t6) + t36_tmp * mp * t4 * t5 * t6) + + t36_tmp * mw * t4 * t5 * t6; + A[1] = t42 * (((((((((((((((t42_tmp + g_t42_tmp * g * mp) + d * t4 * t5) + d * t5 * t7) + e_t42_tmp * g * t4 * t5) + + g_t42_tmp * M * g) + + b_t42_tmp * Lm * M * g) + + c_t42_tmp * mp * t5 * 2.0) + + c_t42_tmp * mw * t5) + + t22_tmp * M * g * mp * t6) + + h_t42_tmp * mp * t5) + + h_t42_tmp * mw * t5) + d * mp * mw * t5) + d1 * t5 * t6 * t7) + - t32_tmp * mp * t4 * t5 * t6) + + t34_tmp * mp * t4 * t5 * t6) + d1 * mp * mw * t5 * t6); A[2] = 0.0; - d = Im * g; - c_t41_tmp = d * t2; - e_t41_tmp = L * t34; - A[3] = -(t41 * ((((((((e_t41_tmp + c_t41_tmp * t4 * t5) + d * t3 * t4 * t5) + c_t41_tmp * t5 * t7) + - g * mp * t2 * t4 * t5 * t6) + - i_t41_tmp * g * t4 * t5 * 2.0) + - d_t41_tmp * g * mp * t2 * t5 * 2.0) + - t31_tmp * t2 * t5 * t6 * t7) + - j_t41_tmp * g * mp * t5 * 2.0)); + d = Im * R * g; + c_t42_tmp = d * t2; + e_t42_tmp = -R * t42; + A[3] = e_t42_tmp * ((((((((t31 + c_t42_tmp * t4) + d * t3 * t4) + c_t42_tmp * t7) + i_t42_tmp * R * g * t4 * 2.0) + + d_t42_tmp * R * g * mp * t2 * 2.0) + + M * R * g * t2 * t6 * t7) + + R * g * mp * t2 * t4 * t6) + + j_t42_tmp * R * g * mp * 2.0); A[4] = 0.0; - d = t39_tmp_tmp * M * g * l; - c_t41_tmp = - (((((((t17_tmp * g * l * mp * t2 + b_t18_tmp * g * l * t4 * 2.0) + t23_tmp * t2 * t4) + t23_tmp * t3 * t4) + - t31_tmp * l * mp * mw * t2 * t5) + - b_t33_tmp * t2 * t4 * t5) + - t33_tmp * mp * t3 * t4 * t5) + - b_t33_tmp * t3 * t4 * t5) + - t39_tmp * mw * t4 * t5 * 2.0; - A[5] = t41 * ((((c_t41_tmp + t39_tmp * mp * t4 * t5) + b_t18_tmp * M * g * l * mp) + d * t5 * t7) + d * mp * mw * t5); + d = t31_tmp * M * g * l; + c_t42_tmp = + (((((((t17_tmp * g * l * mp * t2 + b_t22_tmp * g * l * t4 * 2.0) + t24_tmp * t2 * t4) + t24_tmp * t3 * t4) + + M * g * l * mp * mw * t2 * t5) + + t33_tmp * mw * t4 * t5 * 2.0) + + b_t35_tmp * t2 * t4 * t5) + + t35_tmp * mp * t3 * t4 * t5) + + b_t35_tmp * t3 * t4 * t5; + A[5] = t42 * ((((c_t42_tmp + t33_tmp * mp * t4 * t5) + b_t22_tmp * M * g * l * mp) + d * t5 * t7) + d * mp * mw * t5); A[6] = 1.0; memset(&A[7], 0, 13U * sizeof(double)); A[20] = 1.0; @@ -159,13 +157,12 @@ void gen_A(double Im, double Ip, double Iw, double L, double Lm, double M, doubl A[22] = 0.0; A[23] = 0.0; A[24] = 0.0; - A[25] = t41 * t41_tmp; + A[25] = t42 * t42_tmp; A[26] = 0.0; - d = Ip * g; - A[27] = -t41 * (e_t41_tmp - d * t4 * t5 * t6); + A[27] = e_t42_tmp * (t31 - Ip * R * g * t4 * t6); A[28] = 0.0; - e_t41_tmp = f_t41_tmp * g * l; - A[29] = t41 * ((((c_t41_tmp + k_t41_tmp * g * l) + d * l * t4 * t5) + e_t41_tmp * mp * t5) + e_t41_tmp * mw * t5); + d = f_t42_tmp * g * l; + A[29] = t42 * ((((c_t42_tmp + k_t42_tmp * g * l) + Ip * g * l * t4 * t5) + d * mp * t5) + d * mw * t5); A[30] = 0.0; A[31] = 0.0; A[32] = 0.0; diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_B.c b/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_B.c index 018d0671..2a0fb983 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_B.c +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_B.c @@ -29,107 +29,120 @@ void gen_B(double Im, double Ip, double Iw, double L, double Lm, double M, double R, double l, double mp, double mw, double B[12]) { - double b_t41_tmp; - double d; double t10; double t10_tmp; double t11; - double t11_tmp; - double t12; double t13; + double t14; + double t14_tmp; + double t15; + double t15_tmp; + double t16; + double t16_tmp; + double t17; double t2; + double t22; + double t22_tmp; double t23; - double t25; - double t25_tmp; - double t26; - double t26_tmp; - double t27; + double t23_tmp; double t28; double t28_tmp; double t29; - double t29_tmp; double t3; + double t30; + double t31; + double t32; + double t32_tmp; + double t33; + double t33_tmp; double t4; - double t41; - double t41_tmp; + double t44; + double t44_tmp; double t5; double t6; double t8; double t8_tmp; double t9; - /* This function was generated by the Symbolic Math Toolbox version 9.2. + /* This function was generated by the Symbolic Math Toolbox version 23.2. */ - /* 08-Aug-2025 17:33:03 */ + /* 2025-12-05 01:47:45 */ t2 = L * L; t3 = Lm * Lm; t4 = R * R; t5 = l * l; t6 = Im * Iw; - t8_tmp = Iw * L; - t8 = t8_tmp * M * l; - t9 = Iw * Lm * M * l; - t10_tmp = Im * M; - t10 = t10_tmp * t4; - t11_tmp = Iw * M; - t11 = t11_tmp * t5; - t12 = Im * mp * t4; - t13 = Im * mw * t4; - t25_tmp = L * M; - t25 = t25_tmp * l * mw * t4; - t26_tmp = Lm * M * l; - t26 = t26_tmp * mp * t4; - t27 = t26_tmp * mw * t4; - t28_tmp = M * mp; - t28 = t28_tmp * t4 * t5; - t29_tmp = M * mw; - t29 = t29_tmp * t4 * t5; - t23 = L * t10; - t26_tmp = mp * t2; - t41_tmp = mw * t2; - b_t41_tmp = L * Lm * M; - t41 = + t8_tmp = Im * L; + t8 = t8_tmp * M * R; + t9 = Im * Lm * M * R; + t10_tmp = Iw * L; + t10 = t10_tmp * M * l; + t11 = Iw * Lm * M * l; + t13 = t8_tmp * R * mp; + t44 = L * Lm; + t22_tmp = t44 * M; + t22 = t22_tmp * R * l * mp; + t14_tmp = Im * M; + t14 = t14_tmp * t4; + t15_tmp = Iw * M; + t15 = t15_tmp * t5; + t16_tmp = Im * mp; + t16 = t16_tmp * t4; + t17 = Im * mw * t4; + t23_tmp = Ip * M; + t23 = -(t23_tmp * R * l); + t28_tmp = L * M; + t28 = t28_tmp * R * mp * t5; + t29 = t28_tmp * l * mw * t4; + t28_tmp = Lm * M * l; + t30 = t28_tmp * mp * t4; + t31 = t28_tmp * mw * t4; + t32_tmp = M * mp; + t32 = t32_tmp * t4 * t5; + t33_tmp = M * mw; + t33 = t33_tmp * t4 * t5; + t28_tmp = mp * t2; + t44_tmp = mw * t2; + t44 = 1.0 / - (((((((((((((((((Ip * t6 + Ip * t11) + t26_tmp * t6) + Ip * t12) + Ip * t13) + b_t41_tmp * t6 * 2.0) + M * t2 * t6) + + (((((((((((((((((Ip * t6 + Ip * t15) + t28_tmp * t6) + Ip * t16) + Ip * t17) + t22_tmp * t6 * 2.0) + M * t2 * t6) + M * t3 * t6) + - Ip * t10) + - t41_tmp * t10) + - mp * t3 * t10) + - mw * t3 * t10) + - t26_tmp * t11) + - Ip * t28) + - Ip * t29) + - t41_tmp * t12) + - Lm * mw * t23 * 2.0) + - t41_tmp * t28); + Ip * t14) + + t44 * mw * t14 * 2.0) + + t44_tmp * t14) + + mp * t3 * t14) + + mw * t3 * t14) + + t28_tmp * t15) + + Ip * t32) + + Ip * t33) + + t44_tmp * t16) + + t44_tmp * t32); B[0] = 0.0; - d = Im * L; - B[1] = -(t41 * ((((((((((t6 + t10) + t11) + t12) + t13) + t28) + t29) + d * M * R) + Im * Lm * M * R) + d * R * mp) + - t25_tmp * R * mp * t5)); + B[1] = -t44 * ((((((((((t6 + t8) + t9) + t13) + t14) + t15) + t16) + t17) + t28) + t32) + t33); B[2] = 0.0; - t41_tmp = t10_tmp * R; - t10_tmp = Ip * M; - t25_tmp = t10_tmp * R; - t26_tmp = ((L * t12 + t23) + Lm * t10) + L * t28; - B[3] = t41 * (((((((t26_tmp + Im * Ip * R) + t41_tmp * t2) + t41_tmp * t3) + t25_tmp * t5) + Im * R * mp * t2) + - M * R * mp * t2 * t5) + - d * Lm * M * R * 2.0); + t44_tmp = (t8 + t9) + t13; + B[3] = R * t44 * + ((((((((t44_tmp + t28) + Im * Ip) + t14_tmp * t2) + t14_tmp * t3) + t23_tmp * t5) + t16_tmp * t2) + + t32_tmp * t2 * t5) + + t8_tmp * Lm * M * 2.0); B[4] = 0.0; - d = (((t8 + t9) + t25) + t26) + t27; - B[5] = -(t41 * ((d - t25_tmp * l) + b_t41_tmp * R * l * mp)); + t28_tmp = t10 + t11; + B[5] = -t44 * (((((t28_tmp + t22) + t23) + t29) + t30) + t31); B[6] = 0.0; - B[7] = t41 * (((((((((((t6 + t8) + t9) + t10) + t11) + t12) + t13) + t25) + t26) + t27) + t28) + t29); + B[7] = t44 * (((((((((((t6 + t10) + t11) + t14) + t15) + t16) + t17) + t29) + t30) + t31) + t32) + t33); B[8] = 0.0; - B[9] = -(t41 * ((t26_tmp + L * t26) - t10_tmp * l * t4)); + B[9] = -R * t44 * (((t44_tmp + t22) + t23) + t28); B[10] = 0.0; B[11] = - t41 * (((((((((((((d + Ip * Iw) + t10_tmp * t4) + t11_tmp * t2) + t11_tmp * t3) + Ip * mp * t4) + Iw * mp * t2) + + t44 * ((((((((((((((((t28_tmp + t29) + t30) + t31) + Ip * Iw) + t23_tmp * t4) + t15_tmp * t2) + t15_tmp * t3) + + Ip * mp * t4) + + Iw * mp * t2) + Ip * mw * t4) + - t28_tmp * t3 * t4) + - t29_tmp * t2 * t4) + - t29_tmp * t3 * t4) + + t32_tmp * t3 * t4) + + t33_tmp * t2 * t4) + + t33_tmp * t3 * t4) + mp * mw * t2 * t4) + - t8_tmp * Lm * M * 2.0) + - b_t41_tmp * mw * t4 * 2.0); + t10_tmp * Lm * M * 2.0) + + t22_tmp * mw * t4 * 2.0); } /* diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp index a0cb00ee..bc6e3da7 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp @@ -99,6 +99,8 @@ void VMCController::update(const ros::Time& time, const ros::Duration& period) state.data.push_back(angle_error); state.data.push_back(effortCmd[0]); state.data.push_back(effortCmd[1]); + state.data.push_back(jointCmd[0]); + state.data.push_back(jointCmd[1]); statePublisher_.publish(state); std_msgs::Float64MultiArray jointCmdState; From aaab1cb9a8a3adb52bf05e93d7508537d56c425c Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sun, 21 Dec 2025 20:14:27 +0800 Subject: [PATCH 13/57] Fix: fix turn right bug. --- .../src/bipedal_wheel_controller/controller_mode/normal.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp index 0702153c..5ab55e3f 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -43,8 +43,8 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const controller->setMoveFlag(false); } - double friction_circle = abs(x_left_(3) * angular_vel_base_.z); - double friction_circle_alpha = friction_circle > 5.0f ? (5.0f / friction_circle) : 1.0f; + double friction_circle = x_left_(3) * angular_vel_base_.z; + double friction_circle_alpha = abs(friction_circle) > 3.5f ? (3.5f / abs(friction_circle)) : 1.0f; // PID double T_yaw = pid_yaw_vel_->computeCommand(friction_circle_alpha * vel_cmd_.z - angular_vel_base_.z, period); double T_theta_diff = pid_theta_diff_->computeCommand(right_pos_[1] - left_pos_[1], period); @@ -280,7 +280,7 @@ bool Normal::unstickDetection(const double& F_leg, const double& Tp, const doubl static ros::Time judgeTime; double Fn = calculateSupportForce(F_leg, Tp, leg_length, leg_len_spd, acc_z, x, model_params, period); supportForceAveragePtr->input(Fn); - bool unstick_ = supportForceAveragePtr->output() < 15; + bool unstick_ = supportForceAveragePtr->output() < 10; if (unstick_ != last_unstick_) { From 86e89ccaf1f6f5a5b49fc16f57bc174d33c9a530 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Mon, 22 Dec 2025 16:36:55 +0800 Subject: [PATCH 14/57] Add overturn recovery function. --- .../controller_mode/recover.h | 4 ++ .../helper_functions.h | 20 ++++++- .../bipedal_wheel_controller/controller.cpp | 18 +++--- .../controller_mode/recover.cpp | 57 ++++++++++++++++++- 4 files changed, 85 insertions(+), 14 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h index 062ed6a7..1a2dbbad 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h @@ -18,6 +18,9 @@ class Recover : public ModeBase Recover(const std::vector& joint_handles, const std::vector& pid_legs, const std::vector& pid_thetas); void execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) override; + void setUpLegMotion(const Eigen::Matrix& 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); const char* name() const override { return "RECOVER"; @@ -26,5 +29,6 @@ class Recover : public ModeBase private: std::vector joint_handles_; std::vector pid_legs_, pid_thetas_; + int left_leg_state, right_leg_state; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h index bf5423a9..0dea5e85 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h @@ -101,14 +101,28 @@ inline LegCommand computePidLegCommand(double desired_length, double desired_ang { LegCommand cmd{ 0.0, 0.0, { 0.0, 0.0 } }; cmd.force = length_pid.computeCommand(desired_length - leg_pos[0], period) + feedforward_force; - if (leg_state == LegState::BEHIND && !overturn) - cmd.torque = angle_pid.computeCommand(-angles::shortest_angular_distance(desired_angle, leg_pos[1]), period); + 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_pos[1] > M_PI_2 - 0.3 && leg_pos[1] < M_PI_2 + 0.3)) + 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(-4 - leg_spd[1], period); + } } leg_conv(cmd.force, cmd.torque, leg_angle[0], leg_angle[1], cmd.input); return cmd; diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp index cf71ab43..ef542792 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp @@ -130,7 +130,7 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat tf2::Vector3 z_body(0, 0, 1); tf2::Vector3 z_world = tf2::quatRotate(odom2base.getRotation(), z_body); overturn_ = z_world.z() < -5.0; - overturn_ = false; + overturn_ = !(abs(pitch) < 0.9); } catch (tf2::TransformException& ex) { @@ -142,16 +142,16 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat // vmc double left_angle[2]{}, right_angle[2]{}, left_pos[2]{}, left_spd[2]{}, right_pos[2]{}, right_spd[2]{}; // [0]:hip_vmc_joint [1]:knee_vmc_joint - left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; - left_angle[1] = left_knee_joint_handle_.getPosition(); - right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; - right_angle[1] = right_knee_joint_handle_.getPosition(); + // left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; + // left_angle[1] = left_knee_joint_handle_.getPosition(); + // right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; + // right_angle[1] = right_knee_joint_handle_.getPosition(); // gazebo - // left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; - // left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; - // right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; - // right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; + left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; + left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; + right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; + right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; // [0] is length, [1] is angle leg_pos(left_angle[0], left_angle[1], left_pos); diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp index b2653472..5aefb6c2 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp @@ -19,6 +19,8 @@ void Recover::execute(BipedalController* controller, const ros::Time& time, cons if (!controller->getStateChange()) { ROS_INFO("[balance] Enter RECOVER"); + detectLegRelState(left_pos_[1], left_leg_state); + detectLegRelState(right_pos_[1], right_leg_state); controller->setStateChange(true); } @@ -27,8 +29,8 @@ void Recover::execute(BipedalController* controller, const ros::Time& time, cons left_desired_angle = left_pos_[1]; right_desired_angle = right_pos_[1]; LegCommand left_cmd = { 0, 0, { 0., 0. } }, right_cmd = { 0, 0, { 0., 0. } }; - detectLegState(x_left_, left_leg_state); - detectLegState(x_right_, right_leg_state); + setUpLegMotion(x_left_, right_leg_state, left_pos_[0], left_pos_[1], left_leg_state, left_desired_angle); + setUpLegMotion(x_right_, left_leg_state, right_pos_[0], right_pos_[1], right_leg_state, right_desired_angle); if (controller->getBaseState() != 4) { left_cmd = @@ -48,4 +50,55 @@ void Recover::execute(BipedalController* controller, const ros::Time& time, cons ROS_INFO("[balance] Exit RECOVER"); } } + +void Recover::setUpLegMotion(const Eigen::Matrix& x, const int& other_leg_state, + const double& leg_length, const double& leg_theta, int& leg_state, double& theta_des) +{ + switch (leg_state) + { + case LegState::UNDER: + theta_des = leg_theta; + if (leg_length > 0.35) + { + leg_state = LegState::BEHIND; + } + break; + case LegState::FRONT: + theta_des = leg_theta; + if (other_leg_state == LegState::FRONT) + { + theta_des = M_PI / 2 - 0.35; + } + break; + case LegState::BEHIND: + theta_des = leg_theta; + if ((leg_theta < -M_PI / 2 + 0.7 && leg_theta > -M_PI) || (leg_theta < M_PI && leg_theta > M_PI - 0.5)) + { + leg_state = LegState::FRONT; + } + break; + } +} + +void Recover::detectLegRelState(const double& leg_theta, int& leg_state) +{ + if (leg_theta > -M_PI / 2 + 0.7 && leg_theta < (M_PI / 2 - 1.4)) + leg_state = LegState::UNDER; + else if ((leg_theta < -M_PI / 2 + 0.7 && leg_theta > -M_PI) || (leg_theta < M_PI && leg_theta > M_PI - 0.5)) + leg_state = LegState::FRONT; + else if (leg_theta > (M_PI / 2 - 1.4) && leg_theta < M_PI - 0.5) + leg_state = LegState::BEHIND; + switch (leg_state) + { + case LegState::UNDER: + ROS_INFO("[balance] leg_theta: %.3f Leg state: UNDER", leg_theta); + break; + case LegState::FRONT: + ROS_INFO("[balance] leg_theta: %.3f Leg state: FRONT", leg_theta); + break; + case LegState::BEHIND: + ROS_INFO("[balance] leg_theta: %.3f Leg state: BEHIND", leg_theta); + break; + } +} } // namespace rm_chassis_controllers From 0094c2f1520b8eb28d90332d507cc205764a0575 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Thu, 25 Dec 2025 20:44:32 +0800 Subject: [PATCH 15/57] Optimize upstairs mode. --- .../bipedal_wheel_controller/controller_mode/upstairs.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp index 537684d8..a8976794 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp @@ -26,7 +26,7 @@ void Upstairs::execute(BipedalController* controller, const ros::Time& time, con detectLegState(x_right_, right_leg_state); } - double theta_des_l{ M_PI_2 - 0.4 }, theta_des_r{ M_PI_2 - 0.4 }, length_des_l{ 0.18 }, length_des_r{ 0.18 }; + double theta_des_l{ M_PI_2 - 0.3 }, theta_des_r{ M_PI_2 - 0.3 }, length_des_l{ 0.18 }, length_des_r{ 0.18 }; LegCommand left_cmd = { 0, 0, { 0., 0. } }, right_cmd = { 0, 0, { 0., 0. } }; left_cmd = computePidLegCommand(length_des_l, theta_des_l, left_pos_, left_spd_, *pid_legs_[0], *pid_thetas_[0], *pid_thetas_[2], left_angle_, left_leg_state, period); @@ -35,8 +35,7 @@ void Upstairs::execute(BipedalController* controller, const ros::Time& time, con setJointCommands(joint_handles_, left_cmd, right_cmd); // Exit - if ((left_pos_[0] < 0.2 && left_pos_[1] > (M_PI_2 - 0.5) && abs(left_spd_[1]) < 0.1) && - (right_pos_[0] < 0.2 && right_pos_[1] > (M_PI_2 - 0.5) && abs(right_spd_[1]) < 0.1)) + if (left_pos_[0] < 0.2 && left_pos_[1] > (M_PI_2 - 0.5) && right_pos_[0] < 0.2 && right_pos_[1] > (M_PI_2 - 0.5)) { controller->pubLegLenStatus(false); controller->setMode(BalanceMode::STAND_UP); From 2ec8f04f610cb8b8477c5fb30f8bad8379efa255 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Thu, 25 Dec 2025 20:45:01 +0800 Subject: [PATCH 16/57] Modify some recover params. --- .../src/bipedal_wheel_controller/controller_mode/recover.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp index 5aefb6c2..7eef77c4 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp @@ -43,7 +43,7 @@ void Recover::execute(BipedalController* controller, const ros::Time& time, cons setJointCommands(joint_handles_, left_cmd, right_cmd); // Exit - if (!controller->getOverturn() && abs(x_left_[4]) < 0.2 && abs(roll_) < 0.1) + if (!controller->getOverturn() && abs(x_left_[5]) < 0.2 && abs(x_left_[1]) < 0.2 && abs(x_right_[1]) < 0.2) { controller->setMode(BalanceMode::STAND_UP); controller->setStateChange(false); From 511082c3de10f511efb5a51f7aab07018dde00a7 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Thu, 25 Dec 2025 20:46:05 +0800 Subject: [PATCH 17/57] Try to use acc_z to check upstair status. --- .../bipedal_wheel_controller/controller_mode/normal.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp index 5ab55e3f..4c70140c 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -222,9 +222,11 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const right_cmd = { F_leg[1], u_right[1], { right_T[0], right_T[1] } }; // upstairs - if (((x_left[4] < -0.25 && (x_left(0) + x_right(0) / 2.0f) > 0.25) || - (((left_pos_[1] + right_pos_[1]) / 2.0f) > 0.5)) && - controller->getCompleteStand() && abs(vel_cmd_.x) > 1.0 && abs(x_left(3)) > 0.2 && + // if (((x_left[4] < -0.25 && (x_left(0) + x_right(0) / 2.0f) > 0.25) || + // (((left_pos_[1] + right_pos_[1]) / 2.0f) > 0.5)) && + // controller->getCompleteStand() && abs(vel_cmd_.x) > 1.0 && abs(x_left(3)) > 0.2 && + // ((left_pos_[0] + right_pos_[0]) / 2.0f) > 0.34) + if (linear_acc_base_.z < -3.0 && controller->getCompleteStand() && abs(vel_cmd_.x) > 1.0 && abs(x_left(3)) > 0.2 && ((left_pos_[0] + right_pos_[0]) / 2.0f) > 0.34) { leg_length_des = controller->getDefaultLegLength(); From 40eabadc9e648fa49b97fee1d4965278ecc00cc2 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Fri, 26 Dec 2025 18:27:46 +0800 Subject: [PATCH 18/57] Modify enter protection params. --- .../src/bipedal_wheel_controller/controller_mode/normal.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp index 4c70140c..a4ff30f2 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -238,7 +238,7 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const } // Protection - if ((controller->getCompleteStand() && (abs(x_left(4)) > 0.95 || abs(x_left(0)) > 0.8)) || + if ((controller->getCompleteStand() && (abs(x_left(4)) > 0.95 || abs(x_left(0)) > 0.8 || abs(roll_) > 1.3)) || controller->getOverturn() || controller->getBaseState() == 4) { leg_length_des = controller->getDefaultLegLength(); From 77372d79b16e83d7e784b7bc71dd03a9b60acb95 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sun, 28 Dec 2025 21:49:05 +0800 Subject: [PATCH 19/57] Add jump function in gazebo. --- .../bipedal_wheel_controller/controller.h | 5 +++-- .../bipedal_wheel_controller/definitions.h | 2 +- .../controller_mode/normal.cpp | 12 +++++++----- 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h index f17ccb95..be45f64d 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h @@ -104,8 +104,9 @@ class BipedalController : public ChassisBase joint_handles_; // Leg Cmd diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h index 6b2ec1ae..cb6dcba8 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h @@ -74,7 +74,7 @@ enum BalanceMode }; constexpr std::array, 3> jumpLengthDes = { - { { JumpPhase::LEG_RETRACTION, 0.2 }, { JumpPhase::JUMP_UP, 0.36 }, { JumpPhase::OFF_GROUND, 0.22 } } + { { JumpPhase::LEG_RETRACTION, 0.12 }, { JumpPhase::JUMP_UP, 0.38 }, { JumpPhase::OFF_GROUND, 0.15 } } }; constexpr static const int STATE_DIM = 6; diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp index a4ff30f2..d0ab33a4 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -130,7 +130,7 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const gravity * cos(left_pos_[1]) + T_roll - spring_force; F_leg(1) = pid_legs_[1]->computeCommand(leg_length_des - current_leg_length, period) + gravity * cos(left_pos_[1]) - T_roll - spring_force; - if (current_leg_length < leg_length_des + 0.1) + if (current_leg_length < leg_length_des + 0.01) { jumpTime_++; } @@ -158,8 +158,10 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const break; case JumpPhase::OFF_GROUND: ROS_INFO("[balance] ENTER OFF_GROUND"); - F_leg(0) = pid_legs_[0]->computeCommand(leg_length_des - current_leg_length, period); - F_leg(1) = pid_legs_[1]->computeCommand(leg_length_des - current_leg_length, period); + F_leg(0) = -(control_params_->p1_ * pow(left_pos_[0], 3) + control_params_->p2_ * pow(left_pos_[0], 2) + + control_params_->p3_ * left_pos_[0] + control_params_->p4_); + F_leg(1) = -(control_params_->p1_ * pow(right_pos_[0], 3) + control_params_->p2_ * pow(right_pos_[0], 2) + + control_params_->p3_ * right_pos_[0] + control_params_->p4_); if (current_leg_length < leg_length_des) { @@ -226,8 +228,8 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const // (((left_pos_[1] + right_pos_[1]) / 2.0f) > 0.5)) && // controller->getCompleteStand() && abs(vel_cmd_.x) > 1.0 && abs(x_left(3)) > 0.2 && // ((left_pos_[0] + right_pos_[0]) / 2.0f) > 0.34) - if (linear_acc_base_.z < -3.0 && controller->getCompleteStand() && abs(vel_cmd_.x) > 1.0 && abs(x_left(3)) > 0.2 && - ((left_pos_[0] + right_pos_[0]) / 2.0f) > 0.34) + if (jump_phase_ == JumpPhase::IDLE && linear_acc_base_.z < -3.0 && controller->getCompleteStand() && + abs(vel_cmd_.x) > 1.0 && abs(x_left(3)) > 0.2 && ((left_pos_[0] + right_pos_[0]) / 2.0f) > 0.34) { leg_length_des = controller->getDefaultLegLength(); controller->setMode(BalanceMode::UPSTAIRS); From 123c5709b85816de7b325c6f2a6ea6b1fc899835 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Tue, 30 Dec 2025 18:31:28 +0800 Subject: [PATCH 20/57] Feat: try to use hermite polynomials to plan jump force. --- .../controller_mode/normal.cpp | 22 ++++++++++++------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp index d0ab33a4..f5af01a2 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -122,6 +122,8 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const else { leg_length_des = jumpLengthDes[jump_phase_].second; + double s_left = (left_pos_[0] - 0.12) / (0.4 - 0.12); + double s_right = (right_pos_[0] - 0.12) / (0.4 - 0.12); switch (jump_phase_) { case JumpPhase::LEG_RETRACTION: @@ -142,10 +144,12 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const break; case JumpPhase::JUMP_UP: ROS_INFO("[balance] ENTER JUMP_UP"); - F_leg(0) = control_params_->p1_ * pow(left_pos_[0], 3) + control_params_->p2_ * pow(left_pos_[0], 2) + - control_params_->p3_ * left_pos_[0] + control_params_->p4_ + gravity; - F_leg(1) = control_params_->p1_ * pow(right_pos_[0], 3) + control_params_->p2_ * pow(right_pos_[0], 2) + - control_params_->p3_ * right_pos_[0] + control_params_->p4_ + gravity; + // F_leg(0) = control_params_->p1_ * pow(left_pos_[0], 3) + control_params_->p2_ * pow(left_pos_[0], 2) + + // control_params_->p3_ * left_pos_[0] + control_params_->p4_ + gravity; + // F_leg(1) = control_params_->p1_ * pow(right_pos_[0], 3) + control_params_->p2_ * pow(right_pos_[0], 2) + + // control_params_->p3_ * right_pos_[0] + control_params_->p4_ + gravity; + F_leg(0) = 400 * (1 - 3 * pow(s_left, 2) + 2 * pow(s_left, 3)) + gravity; + F_leg(1) = 400 * (1 - 3 * pow(s_right, 2) + 2 * pow(s_right, 3)) + gravity; if (current_leg_length > leg_length_des) { jumpTime_++; @@ -158,10 +162,12 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const break; case JumpPhase::OFF_GROUND: ROS_INFO("[balance] ENTER OFF_GROUND"); - F_leg(0) = -(control_params_->p1_ * pow(left_pos_[0], 3) + control_params_->p2_ * pow(left_pos_[0], 2) + - control_params_->p3_ * left_pos_[0] + control_params_->p4_); - F_leg(1) = -(control_params_->p1_ * pow(right_pos_[0], 3) + control_params_->p2_ * pow(right_pos_[0], 2) + - control_params_->p3_ * right_pos_[0] + control_params_->p4_); + // F_leg(0) = -(control_params_->p1_ * pow(left_pos_[0], 3) + control_params_->p2_ * pow(left_pos_[0], 2) + + // control_params_->p3_ * left_pos_[0] + control_params_->p4_); + // F_leg(1) = -(control_params_->p1_ * pow(right_pos_[0], 3) + control_params_->p2_ * pow(right_pos_[0], 2) + + // control_params_->p3_ * right_pos_[0] + control_params_->p4_); + F_leg(0) = -400 * (1 - 3 * pow(s_left, 2) + 2 * pow(s_left, 3)); + F_leg(1) = -400 * (1 - 3 * pow(s_right, 2) + 2 * pow(s_right, 3)); if (current_leg_length < leg_length_des) { From da1cee404da57807da68aac2fb510062d125c52e Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Fri, 2 Jan 2026 00:05:15 +0800 Subject: [PATCH 21/57] Optimize jump param. --- .../src/bipedal_wheel_controller/controller_mode/normal.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp index f5af01a2..e3604a9d 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -173,7 +173,7 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const { jumpTime_++; } - if (jumpTime_ >= 4) + if (jumpTime_ >= 8) { jumpTime_ = 0; jump_phase_ = JumpPhase::IDLE; From 74995065835bb0caa454529677a608a17db7aeb8 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Mon, 5 Jan 2026 14:06:34 +0800 Subject: [PATCH 22/57] Fix: append upstairs cond. --- .../src/bipedal_wheel_controller/controller_mode/normal.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp index e3604a9d..c4c0d35c 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -235,7 +235,8 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const // controller->getCompleteStand() && abs(vel_cmd_.x) > 1.0 && abs(x_left(3)) > 0.2 && // ((left_pos_[0] + right_pos_[0]) / 2.0f) > 0.34) if (jump_phase_ == JumpPhase::IDLE && linear_acc_base_.z < -3.0 && controller->getCompleteStand() && - abs(vel_cmd_.x) > 1.0 && abs(x_left(3)) > 0.2 && ((left_pos_[0] + right_pos_[0]) / 2.0f) > 0.34) + abs(vel_cmd_.x) > 1.0 && abs(x_left(3)) > 0.2 && ((left_pos_[0] + right_pos_[0]) / 2.0f) > 0.34 && + leg_length_des > 0.34) { leg_length_des = controller->getDefaultLegLength(); controller->setMode(BalanceMode::UPSTAIRS); From 7b529bc5163a59bb3d22e2757f8f8541f493001a Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sat, 24 Jan 2026 15:38:50 +0800 Subject: [PATCH 23/57] Optimize upstair. --- .../src/bipedal_wheel_controller/controller_mode/upstairs.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp index a8976794..bfc2522b 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp @@ -26,7 +26,7 @@ void Upstairs::execute(BipedalController* controller, const ros::Time& time, con detectLegState(x_right_, right_leg_state); } - double theta_des_l{ M_PI_2 - 0.3 }, theta_des_r{ M_PI_2 - 0.3 }, length_des_l{ 0.18 }, length_des_r{ 0.18 }; + double theta_des_l{ M_PI_2 - 0.6 }, theta_des_r{ M_PI_2 - 0.6 }, length_des_l{ 0.18 }, length_des_r{ 0.18 }; LegCommand left_cmd = { 0, 0, { 0., 0. } }, right_cmd = { 0, 0, { 0., 0. } }; left_cmd = computePidLegCommand(length_des_l, theta_des_l, left_pos_, left_spd_, *pid_legs_[0], *pid_thetas_[0], *pid_thetas_[2], left_angle_, left_leg_state, period); @@ -35,7 +35,7 @@ void Upstairs::execute(BipedalController* controller, const ros::Time& time, con setJointCommands(joint_handles_, left_cmd, right_cmd); // Exit - if (left_pos_[0] < 0.2 && left_pos_[1] > (M_PI_2 - 0.5) && right_pos_[0] < 0.2 && right_pos_[1] > (M_PI_2 - 0.5)) + if (left_pos_[0] < 0.2 && left_pos_[1] > (M_PI_2 - 0.65) && right_pos_[0] < 0.2 && right_pos_[1] > (M_PI_2 - 0.65)) { controller->pubLegLenStatus(false); controller->setMode(BalanceMode::STAND_UP); From 6a4c3760afeee165986cd8cb1d01eba78967a072 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sat, 24 Jan 2026 15:39:38 +0800 Subject: [PATCH 24/57] Add theta bias. --- .../controller_mode/normal.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp index c4c0d35c..db704096 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -82,10 +82,10 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const x_left_ref(3) = x_right_ref(3) = friction_circle_alpha * vel_cmd_.x; leg_length_des = controller->getLegCmd(); } - x_left(0) -= 0.07; - x_right(0) -= 0.07; - x_left(4) -= 0.; - x_right(4) -= 0.; + x_left(0) -= bias_params_->theta; + x_right(0) -= bias_params_->theta; + x_left(4) -= bias_params_->pitch; + x_right(4) -= bias_params_->pitch; x_left -= x_left_ref; x_right -= x_right_ref; @@ -234,8 +234,8 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const // (((left_pos_[1] + right_pos_[1]) / 2.0f) > 0.5)) && // controller->getCompleteStand() && abs(vel_cmd_.x) > 1.0 && abs(x_left(3)) > 0.2 && // ((left_pos_[0] + right_pos_[0]) / 2.0f) > 0.34) - if (jump_phase_ == JumpPhase::IDLE && linear_acc_base_.z < -3.0 && controller->getCompleteStand() && - abs(vel_cmd_.x) > 1.0 && abs(x_left(3)) > 0.2 && ((left_pos_[0] + right_pos_[0]) / 2.0f) > 0.34 && + if (jump_phase_ == JumpPhase::IDLE && linear_acc_base_.z < -7.0 && controller->getCompleteStand() && + abs(vel_cmd_.x) > 1.0 && abs(x_left(3)) > 0.2 && ((left_pos_[0] + right_pos_[0]) / 2.0f) > 0.32 && leg_length_des > 0.34) { leg_length_des = controller->getDefaultLegLength(); From d8945bdac74500551492fde8690411a141c96587 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sat, 24 Jan 2026 15:41:35 +0800 Subject: [PATCH 25/57] Optimize sit_down. --- .../src/bipedal_wheel_controller/controller_mode/sit_down.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp index 5cab5c59..ec0add9c 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp @@ -29,7 +29,7 @@ void SitDown::execute(BipedalController* controller, const ros::Time& time, cons // Exit if (abs(x_left_(1)) < 0.1 && abs(x_left_(5)) < 0.2 && controller->getBaseState() != 4) { - if (!controller->getOverturn() && abs(x_left_(4)) < 0.1) + if (!controller->getOverturn() && abs(x_left_(4)) < 0.3) controller->setMode(BalanceMode::STAND_UP); else controller->setMode(BalanceMode::RECOVER); From 56a40a2c1f55b5f28113e131c995bb47b949f696 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sat, 24 Jan 2026 15:42:13 +0800 Subject: [PATCH 26/57] Add leg_spd lp_filter and overturn check. --- .../bipedal_wheel_controller/controller.h | 4 ++ .../bipedal_wheel_controller/controller.cpp | 72 +++++++++++++++---- 2 files changed, 62 insertions(+), 14 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h index be45f64d..90c0071a 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -37,6 +38,7 @@ class BipedalController : public ChassisBase A_, B_, H_, Q_, R_; Eigen::Matrix X_, U_; std::shared_ptr> kalmanFilterPtr_; + std::shared_ptr left_leg_angle_lpFilterPtr_, right_leg_angle_lpFilterPtr_, + left_leg_angle_vel_lpFilterPtr_, right_leg_angle_vel_lpFilterPtr_; Eigen::Matrix x_left_{}, x_right_{}; double default_leg_length_{ 0.2 }; diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp index ef542792..9efe3a91 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp @@ -72,6 +72,11 @@ bool BipedalController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan kalmanFilterPtr_ = std::make_shared>(A_, B_, H_, Q_, R_); kalmanFilterPtr_->clear(X_); + // left_leg_angle_lpFilterPtr_ = std::make_shared(100); + // right_leg_angle_lpFilterPtr_ = std::make_shared(100); + + left_leg_angle_vel_lpFilterPtr_ = std::make_shared(60); + right_leg_angle_vel_lpFilterPtr_ = std::make_shared(60); return true; } @@ -91,7 +96,7 @@ void BipedalController::moveJoint(const ros::Time& time, const ros::Duration& pe void BipedalController::clearStatus() { - x_left_(2) = x_right_(2) = 0; + x_left_(2) = x_right_(2) = -bias_params_->x; } void BipedalController::updateEstimation(const ros::Time& time, const ros::Duration& period) @@ -129,8 +134,7 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat tf2::Vector3 z_body(0, 0, 1); tf2::Vector3 z_world = tf2::quatRotate(odom2base.getRotation(), z_body); - overturn_ = z_world.z() < -5.0; - overturn_ = !(abs(pitch) < 0.9); + overturn_ = !(abs(pitch) < 0.5) && z_world.z() < -3.0; } catch (tf2::TransformException& ex) { @@ -142,16 +146,16 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat // vmc double left_angle[2]{}, right_angle[2]{}, left_pos[2]{}, left_spd[2]{}, right_pos[2]{}, right_spd[2]{}; // [0]:hip_vmc_joint [1]:knee_vmc_joint - // left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; - // left_angle[1] = left_knee_joint_handle_.getPosition(); - // right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; - // right_angle[1] = right_knee_joint_handle_.getPosition(); + left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; + left_angle[1] = left_knee_joint_handle_.getPosition(); + right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; + right_angle[1] = right_knee_joint_handle_.getPosition(); // gazebo - left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; - left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; - right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; - right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; + // left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; + // left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; + // right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; + // right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; // [0] is length, [1] is angle leg_pos(left_angle[0], left_angle[1], left_pos); @@ -160,6 +164,15 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat left_spd); leg_spd(right_hip_joint_handle_.getVelocity(), right_knee_joint_handle_.getVelocity(), right_angle[0], right_angle[1], right_spd); + left_leg_angle_vel_lpFilterPtr_->input(left_spd[1]); + right_leg_angle_vel_lpFilterPtr_->input(right_spd[1]); + // left_leg_angle_lpFilterPtr_->input(left_pos[1]); + // right_leg_angle_lpFilterPtr_->input(right_pos[1]); + + // left_pos[1] = left_leg_angle_lpFilterPtr_->output(); + // right_pos[1] = right_leg_angle_lpFilterPtr_->output(); + left_spd[1] = left_leg_angle_vel_lpFilterPtr_->output(); + right_spd[1] = right_leg_angle_vel_lpFilterPtr_->output(); // Slippage_detection leftWheelVel = (left_wheel_joint_handle_.getVelocity() - angular_vel_base.y + left_spd[1]) * wheel_radius_; @@ -185,7 +198,7 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat i++; } auto x_hat_vel = kalmanFilterPtr_->getState(); - slip_flag_ = abs(x_hat_vel(0) - wheel_vel_aver) > 1.5; + slip_flag_ = abs(x_hat_vel(0) - wheel_vel_aver) > 3.0; // update state x_left_[3] = state_ != RAW ? x_hat_vel(0) : 0; @@ -207,8 +220,7 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat // ros msg rm_msgs::LeggedChassisStatus legged_chassis_status_msg; - legged_chassis_status_msg.roll = - (left_wheel_joint_handle_.getVelocity() + right_wheel_joint_handle_.getVelocity()) * model_params_->r / 2.0; + legged_chassis_status_msg.roll = wheel_vel_aver; legged_chassis_status_msg.pitch = x_left_[4]; legged_chassis_status_msg.d_pitch = x_left_[5]; legged_chassis_status_msg.yaw = yaw; @@ -328,6 +340,7 @@ bool BipedalController::setupBiasParams(ros::NodeHandle& controller_nh) { const std::pair tbl[] = { { "x_bias", &bias_params_->x }, + { "theta_bias", &bias_params_->theta }, { "pitch_bias", &bias_params_->pitch }, { "roll_bias", &bias_params_->roll }, }; @@ -416,5 +429,36 @@ void BipedalController::pubLegLenStatus(const bool& is_high_leg_flag) leg_len_status_pub_.publish(msg); } +void BipedalController::follow(const ros::Time& time, const ros::Duration& period) +{ + if (state_changed_) + { + state_changed_ = false; + ROS_INFO("[Chassis] Enter FOLLOW"); + + ChassisBase::recovery(); + pid_follow_.reset(); + } + + tfVelToBase(command_source_frame_); + try + { + double roll{}, pitch{}, yaw{}; + // double target_yaw_bias{ 0 }; + quatToRPY(robot_state_handle_.lookupTransform("base_link", follow_source_frame_, ros::Time(0)).transform.rotation, + roll, pitch, yaw); + double yawForwardError = angles::shortest_angular_distance(0, yaw); + double yawInverseError = angles::shortest_angular_distance(M_PI, yaw); + double yawError = abs(yawForwardError) < abs(yawInverseError) ? yawForwardError : yawInverseError; + pid_follow_.computeCommand(yawError, period); + vel_cmd_.z = pid_follow_.getCurrentCmd() + cmd_rt_buffer_.readFromRT()->cmd_chassis_.follow_vel_des; + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + } +} + } // namespace rm_chassis_controllers PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::BipedalController, controller_interface::ControllerBase) From a3d3cf1da683175a6ec0f10a9d7329e8fe8ab544 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Mon, 2 Feb 2026 20:18:09 +0800 Subject: [PATCH 27/57] Update CMakeLists.txt. --- rm_chassis_controllers/CMakeLists.txt | 20 ++++++-------------- 1 file changed, 6 insertions(+), 14 deletions(-) diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt index ee8d78c7..8c9d2e13 100644 --- a/rm_chassis_controllers/CMakeLists.txt +++ b/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 From a0e3dc2e0fb77435d9f69f72be55d29150bd56b3 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Mon, 2 Feb 2026 20:18:52 +0800 Subject: [PATCH 28/57] Refactor recover mode. --- .../controller_mode/recover.h | 45 +++++++- .../controller_mode/recover.cpp | 104 ++++++++---------- 2 files changed, 83 insertions(+), 66 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h index 1a2dbbad..719c61ef 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h @@ -14,13 +14,43 @@ 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; + + typedef struct + { + int recovery_state; + int countdown; + int leg_state; + } LegRecoveryState; + public: Recover(const std::vector& joint_handles, - const std::vector& pid_legs, const std::vector& pid_thetas); + const std::vector& pid_legs, const std::vector& 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& 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); + void LegRecovery(LegCommand& cmd, LegRecoveryState& leg_recovery, const LegRecoveryState& other_leg_recovery, + double* leg_pos, double* leg_spd, const double* leg_angle, control_toolbox::Pid& length_pid, + control_toolbox::Pid& angle_vel_pid, control_toolbox::Pid& angle_pid, const ros::Duration& period); + inline void detectChassisStateToRecover(); const char* name() const override { return "RECOVER"; @@ -29,6 +59,11 @@ class Recover : public ModeBase private: std::vector joint_handles_; std::vector pid_legs_, pid_thetas_; - int left_leg_state, right_leg_state; + control_toolbox::Pid* pid_theta_diff_; + LegRecoveryState left_recovery_{ STOP, 500, KneeOnGround }, right_recovery_{ STOP, 500, KneeOnGround }; + 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 diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp index 7eef77c4..31e7cb34 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp @@ -9,8 +9,8 @@ namespace rm_chassis_controllers { Recover::Recover(const std::vector& joint_handles, const std::vector& pid_legs, - const std::vector& pid_thetas) - : joint_handles_(joint_handles), pid_legs_(pid_legs), pid_thetas_(pid_thetas) + const std::vector& pid_thetas, control_toolbox::Pid* pid_theta_diff) + : joint_handles_(joint_handles), pid_legs_(pid_legs), pid_thetas_(pid_thetas), pid_theta_diff_(pid_theta_diff) { } @@ -19,31 +19,49 @@ void Recover::execute(BipedalController* controller, const ros::Time& time, cons if (!controller->getStateChange()) { ROS_INFO("[balance] Enter RECOVER"); - detectLegRelState(left_pos_[1], left_leg_state); - detectLegRelState(right_pos_[1], right_leg_state); + detectd_flag = false; + left_recovery_.recovery_state = STOP; + right_recovery_.recovery_state = STOP; controller->setStateChange(true); } - int left_leg_state, right_leg_state; - double left_desired_angle{ left_pos_[1] }, right_desired_angle{ right_pos_[1] }; - left_desired_angle = left_pos_[1]; - right_desired_angle = right_pos_[1]; + // until chassis + if (!detectd_flag && abs(x_left_[1]) < 0.1 && abs(x_left_[5]) < 0.1) + { + detectChassisStateToRecover(); + left_recovery_.recovery_state = INITIALIZED; + right_recovery_.recovery_state = INITIALIZED; + detectd_flag = true; + leg_recovery_velocity_ = + recovery_chassis_state_ == BackwardSlip ? -leg_recovery_velocity_const_ : leg_recovery_velocity_const_; + } + LegCommand left_cmd = { 0, 0, { 0., 0. } }, right_cmd = { 0, 0, { 0., 0. } }; - setUpLegMotion(x_left_, right_leg_state, left_pos_[0], left_pos_[1], left_leg_state, left_desired_angle); - setUpLegMotion(x_right_, left_leg_state, right_pos_[0], right_pos_[1], right_leg_state, right_desired_angle); + leg_theta_diff_ = angles::shortest_angular_distance(left_pos_[1], right_pos_[1]); + double T_theta_diff{ 0.0 }, feedforward_force{ 0.0 }; if (controller->getBaseState() != 4) { - left_cmd = - computePidLegCommand(0.36, left_desired_angle, left_pos_, left_spd_, *pid_legs_[0], *pid_thetas_[0], - *pid_thetas_[2], left_angle_, left_leg_state, period, 0.0, controller->getOverturn()); - right_cmd = - computePidLegCommand(0.36, right_desired_angle, right_pos_, right_spd_, *pid_legs_[1], *pid_thetas_[1], - *pid_thetas_[3], right_angle_, right_leg_state, period, 0.0, controller->getOverturn()); + const auto& model_params_ = controller->getModelParams(); + feedforward_force = model_params_->f_spring; + T_theta_diff = pid_theta_diff_->computeCommand(leg_theta_diff_, period); + left_cmd.force = pid_legs_[0]->computeCommand(desired_leg_length_ - left_pos_[0], period) + feedforward_force; + right_cmd.force = pid_legs_[1]->computeCommand(desired_leg_length_ - right_pos_[0], period) + feedforward_force; + leg_conv(left_cmd.force, T_theta_diff, left_angle_[0], left_angle_[1], left_cmd.input); + leg_conv(right_cmd.force, -T_theta_diff, right_angle_[0], right_angle_[1], right_cmd.input); + if (abs(leg_theta_diff_) < 0.1) + { + left_cmd.torque = pid_thetas_[2]->computeCommand(leg_recovery_velocity_ - left_spd_[1], period); + right_cmd.torque = pid_thetas_[3]->computeCommand(leg_recovery_velocity_ - right_spd_[1], period); + leg_conv(left_cmd.force, 10 * leg_recovery_velocity_ + left_cmd.torque + T_theta_diff, left_angle_[0], + left_angle_[1], left_cmd.input); + leg_conv(right_cmd.force, 10 * leg_recovery_velocity_ + right_cmd.torque - T_theta_diff, right_angle_[0], + right_angle_[1], right_cmd.input); + } } setJointCommands(joint_handles_, left_cmd, right_cmd); // Exit - if (!controller->getOverturn() && abs(x_left_[5]) < 0.2 && abs(x_left_[1]) < 0.2 && abs(x_right_[1]) < 0.2) + if (abs(pitch_) < 0.2 && linear_acc_base_.z > 5.0 && !controller->getOverturn()) { controller->setMode(BalanceMode::STAND_UP); controller->setStateChange(false); @@ -51,54 +69,18 @@ void Recover::execute(BipedalController* controller, const ros::Time& time, cons } } -void Recover::setUpLegMotion(const Eigen::Matrix& x, const int& other_leg_state, - const double& leg_length, const double& leg_theta, int& leg_state, double& theta_des) +void Recover::detectChassisStateToRecover() { - switch (leg_state) + // pitch_ is base_link pitch not model pitch + if (pitch_ > 0.45 && pitch_ < M_PI) { - case LegState::UNDER: - theta_des = leg_theta; - if (leg_length > 0.35) - { - leg_state = LegState::BEHIND; - } - break; - case LegState::FRONT: - theta_des = leg_theta; - if (other_leg_state == LegState::FRONT) - { - theta_des = M_PI / 2 - 0.35; - } - break; - case LegState::BEHIND: - theta_des = leg_theta; - if ((leg_theta < -M_PI / 2 + 0.7 && leg_theta > -M_PI) || (leg_theta < M_PI && leg_theta > M_PI - 0.5)) - { - leg_state = LegState::FRONT; - } - break; + ROS_INFO("forward"); + recovery_chassis_state_ = RecoveryChassisState::ForwardSlip; } -} - -void Recover::detectLegRelState(const double& leg_theta, int& leg_state) -{ - if (leg_theta > -M_PI / 2 + 0.7 && leg_theta < (M_PI / 2 - 1.4)) - leg_state = LegState::UNDER; - else if ((leg_theta < -M_PI / 2 + 0.7 && leg_theta > -M_PI) || (leg_theta < M_PI && leg_theta > M_PI - 0.5)) - leg_state = LegState::FRONT; - else if (leg_theta > (M_PI / 2 - 1.4) && leg_theta < M_PI - 0.5) - leg_state = LegState::BEHIND; - switch (leg_state) + else if (pitch_ < -0.45 && pitch_ > -M_PI) { - case LegState::UNDER: - ROS_INFO("[balance] leg_theta: %.3f Leg state: UNDER", leg_theta); - break; - case LegState::FRONT: - ROS_INFO("[balance] leg_theta: %.3f Leg state: FRONT", leg_theta); - break; - case LegState::BEHIND: - ROS_INFO("[balance] leg_theta: %.3f Leg state: BEHIND", leg_theta); - break; + ROS_INFO("back"); + recovery_chassis_state_ = RecoveryChassisState::BackwardSlip; } } } // namespace rm_chassis_controllers From 0b76263f91413c2de4f5033942a2d99aadd2782a Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Mon, 2 Feb 2026 20:19:02 +0800 Subject: [PATCH 29/57] Refactor recover mode. --- .../bipedal_wheel_controller/controller_mode/mode_manager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp index 7b835985..0c31ea7a 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp @@ -42,8 +42,8 @@ ModeManager::ModeManager(ros::NodeHandle& controller_nh, &pid_theta_diff_, &pid_roll_))); mode_map_.insert( std::make_pair(BalanceMode::STAND_UP, std::make_unique(joint_handles, pid_legs_stand_up_, pid_thetas_))); - mode_map_.insert( - std::make_pair(BalanceMode::RECOVER, std::make_unique(joint_handles, pid_legs_stand_up_, pid_thetas_))); + mode_map_.insert(std::make_pair(BalanceMode::RECOVER, std::make_unique(joint_handles, pid_legs_stand_up_, + pid_thetas_, &pid_theta_diff_))); mode_map_.insert(std::make_pair(BalanceMode::SIT_DOWN, std::make_unique(joint_handles, pid_wheels_))); mode_map_.insert(std::make_pair(BalanceMode::UPSTAIRS, std::make_unique(joint_handles, pid_legs_stand_up_, pid_thetas_))); From 80c89623f7b23967e5f67051f4d15af59ee6bc4a Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Mon, 2 Feb 2026 20:20:10 +0800 Subject: [PATCH 30/57] Modify condition entering upstair mode. --- .../src/bipedal_wheel_controller/controller_mode/normal.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp index db704096..8df9cd68 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -230,10 +230,6 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const right_cmd = { F_leg[1], u_right[1], { right_T[0], right_T[1] } }; // upstairs - // if (((x_left[4] < -0.25 && (x_left(0) + x_right(0) / 2.0f) > 0.25) || - // (((left_pos_[1] + right_pos_[1]) / 2.0f) > 0.5)) && - // controller->getCompleteStand() && abs(vel_cmd_.x) > 1.0 && abs(x_left(3)) > 0.2 && - // ((left_pos_[0] + right_pos_[0]) / 2.0f) > 0.34) if (jump_phase_ == JumpPhase::IDLE && linear_acc_base_.z < -7.0 && controller->getCompleteStand() && abs(vel_cmd_.x) > 1.0 && abs(x_left(3)) > 0.2 && ((left_pos_[0] + right_pos_[0]) / 2.0f) > 0.32 && leg_length_des > 0.34) From 313b6aa2423bfbcfaba1170d603b3dc883625a33 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Tue, 3 Feb 2026 10:26:07 +0800 Subject: [PATCH 31/57] Refactor function detectLegState of helper_function.h. --- .../bipedal_wheel_controller/controller.h | 3 + .../controller_mode/stand_up.h | 8 +++ .../controller_mode/upstairs.h | 8 +++ .../bipedal_wheel_controller/definitions.h | 12 ++++ .../helper_functions.h | 68 ++++++++++--------- .../rm_chassis_controllers/chassis_base.h | 2 +- .../bipedal_wheel_controller/controller.cpp | 53 ++++++++++----- .../controller_mode/stand_up.cpp | 44 +++++++++--- .../controller_mode/upstairs.cpp | 32 ++++++++- 9 files changed, 171 insertions(+), 59 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h index 90c0071a..391a182a 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h @@ -48,6 +48,7 @@ class BipedalController : public ChassisBase& getModelParams() const { return model_params_; } const std::shared_ptr& getControlParams() const { return control_params_; } const std::shared_ptr& getBiasParams() const { return bias_params_; } + const std::shared_ptr& getLegThresholdParams() const { return leg_threshold_params_; } double getLegCmd() const{ return legCmd_; } double getJumpCmd() const{ return jumpCmd_; } int getBaseState() const{ return state_; } @@ -74,6 +75,7 @@ class BipedalController : public ChassisBase>& Ks, const std::vector& L0s, Eigen::Matrix& coeffs); geometry_msgs::Twist odometry() override; @@ -84,6 +86,7 @@ class BipedalController : public ChassisBase model_params_; std::shared_ptr control_params_; std::shared_ptr bias_params_; + std::shared_ptr leg_threshold_params_; int balance_mode_ = BalanceMode::SIT_DOWN; bool balance_state_changed_ = false; diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h index d599e8ea..d9b70ec8 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h @@ -27,8 +27,16 @@ class StandUp : public ModeBase void setUpLegMotion(const Eigen::Matrix& 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& x, int& leg_state); std::vector joint_handles_; std::vector pid_legs_, pid_thetas_; int left_leg_state, right_leg_state; + double theta_des_l, theta_des_r, length_des_l, length_des_r; + std::shared_ptr leg_state_threshold_; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/upstairs.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/upstairs.h index bc594127..4ec6f475 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/upstairs.h +++ b/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& x, int& leg_state); + std::vector joint_handles_; std::vector pid_legs_, pid_thetas_; int left_leg_state, right_leg_state; + std::shared_ptr leg_state_threshold_; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h index cb6dcba8..0303e909 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h @@ -38,10 +38,22 @@ 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_exit_threshold; +}; + struct LegCommand { double force; // Thrust diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h index 0dea5e85..f9b56edd 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h @@ -54,33 +54,6 @@ inline void generateAB(const std::shared_ptr& 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& 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 @@ -104,13 +77,12 @@ inline LegCommand computePidLegCommand(double desired_length, double desired_ang 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); + cmd.torque = angle_vel_pid.computeCommand(-5 - leg_spd[1], period); } } else @@ -121,13 +93,45 @@ inline LegCommand computePidLegCommand(double desired_length, double desired_ang } else { - cmd.torque = angle_vel_pid.computeCommand(-4 - leg_spd[1], period); + 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 diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h index e979da0c..de527cda 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -100,7 +100,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController * @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. diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp index 9efe3a91..7c6d0a72 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp @@ -40,9 +40,10 @@ bool BipedalController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan model_params_ = std::make_shared(); control_params_ = std::make_shared(); bias_params_ = std::make_shared(); + leg_threshold_params_ = std::make_shared(); if (!setupModelParams(controller_nh) || !setupLQR(controller_nh) || !setupBiasParams(controller_nh) || - !setupControlParams(controller_nh)) + !setupControlParams(controller_nh) || !setupThresholdParams(controller_nh)) return false; auto legCmdCallback = [this](const rm_msgs::LegCmd::ConstPtr& msg) { @@ -60,10 +61,12 @@ bool BipedalController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan x_right_.setZero(); // Slippage detection - A_ << 1, 0, 0, 1; + A_ << 1, 0.0, 0, 1; H_ << 1, 0, 0, 1; Q_ << 1, 0, 0, 1; R_ << 200, 0, 0, 200; + // Q_ << 25, 0, 0, 2000; + // R_ << 800, 0, 0, 0.01; R_wheel_ = R_(0, 0); slip_R_wheel_ = slip_alpha_ * R_wheel_; B_.setZero(); @@ -134,7 +137,7 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat tf2::Vector3 z_body(0, 0, 1); tf2::Vector3 z_world = tf2::quatRotate(odom2base.getRotation(), z_body); - overturn_ = !(abs(pitch) < 0.5) && z_world.z() < -3.0; + overturn_ = abs(pitch) > 0.65 && z_world.z() < 0.0; } catch (tf2::TransformException& ex) { @@ -146,16 +149,16 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat // vmc double left_angle[2]{}, right_angle[2]{}, left_pos[2]{}, left_spd[2]{}, right_pos[2]{}, right_spd[2]{}; // [0]:hip_vmc_joint [1]:knee_vmc_joint - left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; - left_angle[1] = left_knee_joint_handle_.getPosition(); - right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; - right_angle[1] = right_knee_joint_handle_.getPosition(); + // left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; + // left_angle[1] = left_knee_joint_handle_.getPosition(); + // right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; + // right_angle[1] = right_knee_joint_handle_.getPosition(); // gazebo - // left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; - // left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; - // right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; - // right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; + left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; + left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; + right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; + right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; // [0] is length, [1] is angle leg_pos(left_angle[0], left_angle[1], left_pos); @@ -175,7 +178,7 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat right_spd[1] = right_leg_angle_vel_lpFilterPtr_->output(); // Slippage_detection - leftWheelVel = (left_wheel_joint_handle_.getVelocity() - angular_vel_base.y + left_spd[1]) * wheel_radius_; + leftWheelVel = (left_wheel_joint_handle_.getVelocity() + angular_vel_base.y + left_spd[1]) * wheel_radius_; rightWheelVel = (right_wheel_joint_handle_.getVelocity() + angular_vel_base.y + right_spd[1]) * wheel_radius_; leftWheelVelAbsolute = leftWheelVel + left_pos[0] * left_spd[1] * cos(left_pos[1] + pitch_) + left_spd[0] * sin(left_pos[1] + pitch_); @@ -189,12 +192,12 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat i = 0; X_(0) = wheel_vel_aver; X_(1) = linear_acc_base.x; - kalmanFilterPtr_->predict(U_, R_); - kalmanFilterPtr_->update(X_); + kalmanFilterPtr_->predict(U_); + kalmanFilterPtr_->update(X_, R_); } else { - kalmanFilterPtr_->predict(U_, R_); + kalmanFilterPtr_->predict(U_); i++; } auto x_hat_vel = kalmanFilterPtr_->getState(); @@ -202,7 +205,7 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat // update state x_left_[3] = state_ != RAW ? x_hat_vel(0) : 0; - if (abs(x_left_[3]) <= 0.5f && abs(vel_cmd_.x) <= 0.1f) + if (abs(x_left_[3]) <= 0.6f && abs(vel_cmd_.x) <= 0.1f) { x_left_[2] += state_ != RAW ? x_left_[3] * period.toSec() : 0; } @@ -354,6 +357,7 @@ bool BipedalController::setupBiasParams(ros::NodeHandle& controller_nh) return true; } +// [will unused] bool BipedalController::setupControlParams(ros::NodeHandle& controller_nh) { if (!controller_nh.getParam("jumpOverTime", control_params_->jumpOverTime_) || @@ -366,6 +370,23 @@ bool BipedalController::setupControlParams(ros::NodeHandle& controller_nh) return true; } +bool BipedalController::setupThresholdParams(ros::NodeHandle& controller_nh) +{ + if (!controller_nh.getParam("under_lower_threshold", leg_threshold_params_->under_lower) || + !controller_nh.getParam("under_upper_threshold", leg_threshold_params_->under_upper) || + !controller_nh.getParam("front_lower_threshold", leg_threshold_params_->front_lower) || + !controller_nh.getParam("front_upper_threshold", leg_threshold_params_->front_upper) || + !controller_nh.getParam("behind_lower_threshold", leg_threshold_params_->behind_lower) || + !controller_nh.getParam("behind_upper_threshold", leg_threshold_params_->behind_upper) || + !controller_nh.getParam("upstair_exit_threshold", leg_threshold_params_->upstair_exit_threshold)) + { + ROS_ERROR("Load threshold param fail, check the resist of " + "under_threshold, front_threshold, behind_threshold"); + return false; + } + return true; +} + void BipedalController::polyfit(const std::vector>& Ks, const std::vector& L0s, Eigen::Matrix& coeffs) { diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp index f5114cf8..8f51b9fe 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp @@ -22,13 +22,13 @@ void StandUp::execute(BipedalController* controller, const ros::Time& time, cons ROS_INFO("[balance] Enter STAND_UP"); controller->setStateChange(true); controller->setCompleteStand(false); - detectLegState(x_left_, left_leg_state); - detectLegState(x_right_, right_leg_state); + leg_state_threshold_ = controller->getLegThresholdParams(); + StandUp::detectLegState(x_left_, left_leg_state); + StandUp::detectLegState(x_right_, right_leg_state); } auto model_params_ = controller->getModelParams(); double spring_force = -model_params_->f_spring; - double theta_des_l, theta_des_r, length_des_l, length_des_r; LegCommand left_cmd = { 0, 0, { 0., 0. } }, right_cmd = { 0, 0, { 0., 0. } }; setUpLegMotion(x_left_, right_leg_state, left_pos_[0], left_pos_[1], left_leg_state, theta_des_l, length_des_l); setUpLegMotion(x_right_, left_leg_state, right_pos_[0], right_pos_[1], right_leg_state, theta_des_r, length_des_r); @@ -66,9 +66,9 @@ void StandUp::setUpLegMotion(const Eigen::Matrix& x, const } break; case LegState::FRONT: - theta_des = M_PI / 2 - 0.35; + theta_des = M_PI_2 - 0.35; length_des = 0.36; - if (abs(x[0] - theta_des) < 0.3 && abs(x[4]) < 0.3) + if ((abs(x[0] - theta_des) < 0.3 && abs(x[4]) < 0.3) || (abs(x[1]) < 0.1 && x[0] > M_PI_2)) leg_state = LegState::BEHIND; break; case LegState::BEHIND: @@ -77,11 +77,37 @@ void StandUp::setUpLegMotion(const Eigen::Matrix& x, const if (other_leg_state == LegState::BEHIND) { length_des = 0.18; + if (leg_length < 0.3) + theta_des = 0.0; } - if (leg_length < 0.2) - { - theta_des = 0; - } + break; + } +} + +inline void StandUp::detectLegState(const Eigen::Matrix& x, int& leg_state) +{ + if (!leg_state_threshold_) + { + ROS_ERROR_THROTTLE(1.0, "LegUtils threshold params not initialized!"); + return; + } + if (x[0] > leg_state_threshold_->under_lower && x[0] < leg_state_threshold_->under_upper) + leg_state = LegState::UNDER; + else if ((x[0] < leg_state_threshold_->front_lower && x[0] > -M_PI) || + (x[0] < M_PI && x[0] > leg_state_threshold_->front_upper)) + leg_state = LegState::FRONT; + else if (x[0] > leg_state_threshold_->behind_lower && x[0] < leg_state_threshold_->behind_upper) + 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; } } diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp index bfc2522b..a18cab25 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp @@ -22,6 +22,7 @@ void Upstairs::execute(BipedalController* controller, const ros::Time& time, con ROS_INFO("[balance] Enter Upstairs"); controller->setStateChange(true); controller->setCompleteStand(false); + leg_state_threshold_ = controller->getLegThresholdParams(); detectLegState(x_left_, left_leg_state); detectLegState(x_right_, right_leg_state); } @@ -35,7 +36,8 @@ void Upstairs::execute(BipedalController* controller, const ros::Time& time, con setJointCommands(joint_handles_, left_cmd, right_cmd); // Exit - if (left_pos_[0] < 0.2 && left_pos_[1] > (M_PI_2 - 0.65) && right_pos_[0] < 0.2 && right_pos_[1] > (M_PI_2 - 0.65)) + if (left_pos_[0] < 0.2 && left_pos_[1] > leg_state_threshold_->upstair_exit_threshold && right_pos_[0] < 0.2 && + right_pos_[1] > leg_state_threshold_->upstair_exit_threshold) { controller->pubLegLenStatus(false); controller->setMode(BalanceMode::STAND_UP); @@ -43,4 +45,32 @@ void Upstairs::execute(BipedalController* controller, const ros::Time& time, con ROS_INFO("[balance] Exit Upstairs"); } } + +inline void Upstairs::detectLegState(const Eigen::Matrix& x, int& leg_state) +{ + if (!leg_state_threshold_) + { + ROS_ERROR_THROTTLE(1.0, "LegUtils threshold params not initialized!"); + return; + } + if (x[0] > leg_state_threshold_->under_lower && x[0] < leg_state_threshold_->under_upper) + leg_state = LegState::UNDER; + else if ((x[0] < leg_state_threshold_->front_lower && x[0] > -M_PI) || + (x[0] < M_PI && x[0] > leg_state_threshold_->front_upper)) + leg_state = LegState::FRONT; + else if (x[0] > leg_state_threshold_->behind_lower && x[0] < leg_state_threshold_->behind_upper) + 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; + } +} } // namespace rm_chassis_controllers From 4ed80558e8bc90a7967db8e892ff676e1d4fcffd Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Fri, 6 Feb 2026 17:37:36 +0800 Subject: [PATCH 32/57] Delete some unused code. --- .../controller_mode/recover.h | 11 --------- .../helper_functions.h | 24 +++++++++---------- .../controller_mode/recover.cpp | 4 ---- 3 files changed, 12 insertions(+), 27 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h index 719c61ef..ca21df3a 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h @@ -35,21 +35,11 @@ class Recover : public ModeBase KneeOnGround } LegState; - typedef struct - { - int recovery_state; - int countdown; - int leg_state; - } LegRecoveryState; - public: Recover(const std::vector& joint_handles, const std::vector& pid_legs, const std::vector& pid_thetas, control_toolbox::Pid* pid_theta_diff); void execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) override; - void LegRecovery(LegCommand& cmd, LegRecoveryState& leg_recovery, const LegRecoveryState& other_leg_recovery, - double* leg_pos, double* leg_spd, const double* leg_angle, control_toolbox::Pid& length_pid, - control_toolbox::Pid& angle_vel_pid, control_toolbox::Pid& angle_pid, const ros::Duration& period); inline void detectChassisStateToRecover(); const char* name() const override { @@ -60,7 +50,6 @@ class Recover : public ModeBase std::vector joint_handles_; std::vector pid_legs_, pid_thetas_; control_toolbox::Pid* pid_theta_diff_; - LegRecoveryState left_recovery_{ STOP, 500, KneeOnGround }, right_recovery_{ STOP, 500, KneeOnGround }; 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 }; diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h index f9b56edd..6785810c 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h @@ -76,18 +76,7 @@ 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 - { - cmd.torque = angle_vel_pid.computeCommand(-5 - 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); } @@ -96,6 +85,17 @@ inline LegCommand computePidLegCommand(double desired_length, double desired_ang 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; } diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp index 31e7cb34..ca568c13 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp @@ -20,8 +20,6 @@ void Recover::execute(BipedalController* controller, const ros::Time& time, cons { ROS_INFO("[balance] Enter RECOVER"); detectd_flag = false; - left_recovery_.recovery_state = STOP; - right_recovery_.recovery_state = STOP; controller->setStateChange(true); } @@ -29,8 +27,6 @@ void Recover::execute(BipedalController* controller, const ros::Time& time, cons if (!detectd_flag && abs(x_left_[1]) < 0.1 && abs(x_left_[5]) < 0.1) { detectChassisStateToRecover(); - left_recovery_.recovery_state = INITIALIZED; - right_recovery_.recovery_state = INITIALIZED; detectd_flag = true; leg_recovery_velocity_ = recovery_chassis_state_ == BackwardSlip ? -leg_recovery_velocity_const_ : leg_recovery_velocity_const_; From d3b35fb59574605f8a16678bf24371243af9e73f Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Wed, 25 Feb 2026 10:39:22 +0800 Subject: [PATCH 33/57] Modify sit_down mode. --- .../src/bipedal_wheel_controller/controller_mode/sit_down.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp index ec0add9c..e2286e9a 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp @@ -29,7 +29,7 @@ void SitDown::execute(BipedalController* controller, const ros::Time& time, cons // Exit if (abs(x_left_(1)) < 0.1 && abs(x_left_(5)) < 0.2 && controller->getBaseState() != 4) { - if (!controller->getOverturn() && abs(x_left_(4)) < 0.3) + if (!controller->getOverturn()) controller->setMode(BalanceMode::STAND_UP); else controller->setMode(BalanceMode::RECOVER); From 396a14647749f94be9aeff1f70c82578510bca3f Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Wed, 25 Feb 2026 10:40:47 +0800 Subject: [PATCH 34/57] Modify stand_up mode and fix some stand_up bug. --- .../controller_mode/stand_up.h | 1 + .../controller_mode/stand_up.cpp | 24 ++++++++++++------- 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h index d9b70ec8..1fe4aa61 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h @@ -37,6 +37,7 @@ class StandUp : public ModeBase std::vector 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 leg_state_threshold_; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp index 8f51b9fe..f921e412 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp @@ -28,14 +28,14 @@ void StandUp::execute(BipedalController* controller, const ros::Time& time, cons } auto model_params_ = controller->getModelParams(); - double spring_force = -model_params_->f_spring; + spring_force_ = -model_params_->f_spring; LegCommand left_cmd = { 0, 0, { 0., 0. } }, right_cmd = { 0, 0, { 0., 0. } }; setUpLegMotion(x_left_, right_leg_state, left_pos_[0], left_pos_[1], left_leg_state, theta_des_l, length_des_l); setUpLegMotion(x_right_, left_leg_state, right_pos_[0], right_pos_[1], right_leg_state, theta_des_r, length_des_r); left_cmd = computePidLegCommand(length_des_l, theta_des_l, left_pos_, left_spd_, *pid_legs_[0], *pid_thetas_[0], - *pid_thetas_[2], left_angle_, left_leg_state, period, spring_force); + *pid_thetas_[2], left_angle_, left_leg_state, period, spring_force_); right_cmd = computePidLegCommand(length_des_r, theta_des_r, right_pos_, right_spd_, *pid_legs_[1], *pid_thetas_[1], - *pid_thetas_[3], right_angle_, right_leg_state, period, spring_force); + *pid_thetas_[3], right_angle_, right_leg_state, period, spring_force_); setJointCommands(joint_handles_, left_cmd, right_cmd); @@ -58,11 +58,19 @@ void StandUp::setUpLegMotion(const Eigen::Matrix& x, const switch (leg_state) { case LegState::UNDER: - theta_des = M_PI / 2 - 0.35; - length_des = 0.36; - if (leg_length > 0.35) + if (spring_force_ > 0) + { + theta_des = M_PI / 2 - 0.35; + length_des = 0.36; + if (leg_length > 0.35) + { + leg_state = LegState::FRONT; + } + } + else { - leg_state = LegState::FRONT; + theta_des = 0.0; + length_des = 0.18; } break; case LegState::FRONT: @@ -74,7 +82,7 @@ void StandUp::setUpLegMotion(const Eigen::Matrix& x, const case LegState::BEHIND: theta_des = leg_theta; length_des = leg_length; - if (other_leg_state == LegState::BEHIND) + if (other_leg_state != LegState::FRONT) { length_des = 0.18; if (leg_length < 0.3) From b4ebd888d3535f1a4fd1acc69fcfa0ec819d4ec0 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Wed, 25 Feb 2026 10:42:24 +0800 Subject: [PATCH 35/57] Optimize upstairs. --- .../bipedal_wheel_controller/controller.h | 7 +- .../bipedal_wheel_controller/definitions.h | 1 + .../bipedal_wheel_controller/controller.cpp | 108 ++++++++++-------- .../controller_mode/normal.cpp | 6 +- .../controller_mode/upstairs.cpp | 3 +- 5 files changed, 72 insertions(+), 53 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h index 391a182a..89790a01 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -38,7 +39,7 @@ class BipedalController : public ChassisBase left_ref, Eigen::Matrix right_ref, Eigen::Matrix u_left, Eigen::Matrix u_right, Eigen::Matrix 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(); @@ -122,7 +123,7 @@ class BipedalController : public ChassisBase("/leg_cmd", 1, legCmdCallback); unstick_pub_ = controller_nh.advertise("unstick", 1); - leg_len_status_pub_ = controller_nh.advertise("leg_len_status", 1); + upstair_status_pub_ = controller_nh.advertise("upstair_status", 1); legged_chassis_status_pub_ = controller_nh.advertise("legged_chassis_status", 1); legged_chassis_mode_pub_ = controller_nh.advertise("legged_chassis_mode", 1); lqr_status_pub_ = controller_nh.advertise("lqr_status", 1); @@ -137,7 +137,7 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat tf2::Vector3 z_body(0, 0, 1); tf2::Vector3 z_world = tf2::quatRotate(odom2base.getRotation(), z_body); - overturn_ = abs(pitch) > 0.65 && z_world.z() < 0.0; + overturn_ = (abs(pitch) > 0.65 || abs(roll) > 0.4) && z_world.z() < 0.0; } catch (tf2::TransformException& ex) { @@ -149,16 +149,16 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat // vmc double left_angle[2]{}, right_angle[2]{}, left_pos[2]{}, left_spd[2]{}, right_pos[2]{}, right_spd[2]{}; // [0]:hip_vmc_joint [1]:knee_vmc_joint - // left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; - // left_angle[1] = left_knee_joint_handle_.getPosition(); - // right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; - // right_angle[1] = right_knee_joint_handle_.getPosition(); + left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; + left_angle[1] = left_knee_joint_handle_.getPosition(); + right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; + right_angle[1] = right_knee_joint_handle_.getPosition(); // gazebo - left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; - left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; - right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; - right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; + // left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; + // left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; + // right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; + // right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; // [0] is length, [1] is angle leg_pos(left_angle[0], left_angle[1], left_pos); @@ -259,7 +259,7 @@ void BipedalController::pubState() void BipedalController::stopping(const ros::Time& time) { - balance_mode_ = BalanceMode::RECOVER; + balance_mode_ = BalanceMode::SIT_DOWN; balance_state_changed_ = false; setJointCommands(joint_handles_, { 0, 0, { 0., 0. } }, { 0, 0, { 0., 0. } }); @@ -378,7 +378,8 @@ bool BipedalController::setupThresholdParams(ros::NodeHandle& controller_nh) !controller_nh.getParam("front_upper_threshold", leg_threshold_params_->front_upper) || !controller_nh.getParam("behind_lower_threshold", leg_threshold_params_->behind_lower) || !controller_nh.getParam("behind_upper_threshold", leg_threshold_params_->behind_upper) || - !controller_nh.getParam("upstair_exit_threshold", leg_threshold_params_->upstair_exit_threshold)) + !controller_nh.getParam("upstair_exit_threshold", leg_threshold_params_->upstair_exit_threshold) || + !controller_nh.getParam("upstair_des_theta", leg_threshold_params_->upstair_des_theta)) { ROS_ERROR("Load threshold param fail, check the resist of " "under_threshold, front_threshold, behind_threshold"); @@ -443,43 +444,58 @@ void BipedalController::pubLQRStatus(Eigen::Matrix left_er lqr_status_pub_.publish(msg); } -void BipedalController::pubLegLenStatus(const bool& is_high_leg_flag) +void BipedalController::pubLegLenStatus(const bool& upstair_flag) { - std_msgs::Bool msg; - msg.data = is_high_leg_flag; - leg_len_status_pub_.publish(msg); + rm_msgs::LeggedUpstairStatus msg; + msg.upstair_flag = upstair_flag; + upstair_status_pub_.publish(msg); } -void BipedalController::follow(const ros::Time& time, const ros::Duration& period) -{ - if (state_changed_) - { - state_changed_ = false; - ROS_INFO("[Chassis] Enter FOLLOW"); - - ChassisBase::recovery(); - pid_follow_.reset(); - } - - tfVelToBase(command_source_frame_); - try - { - double roll{}, pitch{}, yaw{}; - // double target_yaw_bias{ 0 }; - quatToRPY(robot_state_handle_.lookupTransform("base_link", follow_source_frame_, ros::Time(0)).transform.rotation, - roll, pitch, yaw); - double yawForwardError = angles::shortest_angular_distance(0, yaw); - double yawInverseError = angles::shortest_angular_distance(M_PI, yaw); - double yawError = abs(yawForwardError) < abs(yawInverseError) ? yawForwardError : yawInverseError; - pid_follow_.computeCommand(yawError, period); - vel_cmd_.z = pid_follow_.getCurrentCmd() + cmd_rt_buffer_.readFromRT()->cmd_chassis_.follow_vel_des; - } - catch (tf2::TransformException& ex) - { - ROS_WARN("%s", ex.what()); - } -} +// void BipedalController::follow(const ros::Time& time, const ros::Duration& period) +//{ +// static bool follow_source_frame_changed_{ false }; +// static std::string last_follow_source_frame_{ follow_source_frame_ }; +// if (state_changed_) +// { +// state_changed_ = false; +// ROS_INFO("[Chassis] Enter FOLLOW"); +// +// ChassisBase::recovery(); +// pid_follow_.reset(); +// } +// +// tfVelToBase(command_source_frame_); +// try +// { +// double roll{}, pitch{}, yaw{}; +// // double target_yaw_bias{ 0 }; +// quatToRPY(robot_state_handle_.lookupTransform("base_link", follow_source_frame_, ros::Time(0)).transform.rotation, +// roll, pitch, yaw); +// double yawForwardError = angles::shortest_angular_distance(0, yaw); +// double yawInverseError = angles::shortest_angular_distance(M_PI, yaw); +// double yawError = abs(yawForwardError) < abs(yawInverseError) ? yawForwardError : yawInverseError; +// if (follow_source_frame_ != last_follow_source_frame_) +// { +// follow_source_frame_changed_ = true; +// } +// if (follow_source_frame_changed_) +// { +// yawError = yawForwardError; +// if (abs(yawError) < 0.1) +// { +// follow_source_frame_changed_ = false; +// } +// } +// pid_follow_.computeCommand(yawError, period); +// vel_cmd_.z = pid_follow_.getCurrentCmd() + cmd_rt_buffer_.readFromRT()->cmd_chassis_.follow_vel_des; +// } +// catch (tf2::TransformException& ex) +// { +// ROS_WARN("%s", ex.what()); +// } +// last_follow_source_frame_ = follow_source_frame_; +// } } // namespace rm_chassis_controllers PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::BipedalController, controller_interface::ControllerBase) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp index 8df9cd68..6345267d 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -231,8 +231,8 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const // upstairs if (jump_phase_ == JumpPhase::IDLE && linear_acc_base_.z < -7.0 && controller->getCompleteStand() && - abs(vel_cmd_.x) > 1.0 && abs(x_left(3)) > 0.2 && ((left_pos_[0] + right_pos_[0]) / 2.0f) > 0.32 && - leg_length_des > 0.34) + abs(vel_cmd_.x) > 0.1 && abs(x_left(3)) > 0.1 && ((left_pos_[0] + right_pos_[0]) / 2.0f) > 0.30 && + leg_length_des > 0.30) { leg_length_des = controller->getDefaultLegLength(); controller->setMode(BalanceMode::UPSTAIRS); @@ -243,7 +243,7 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const } // Protection - if ((controller->getCompleteStand() && (abs(x_left(4)) > 0.95 || abs(x_left(0)) > 0.8 || abs(roll_) > 1.3)) || + if (abs(x_left(4)) > 0.6 || abs(x_left(0)) > 1.0 || abs(x_right(0)) > 1.0 || abs(roll_) > 1.0 || controller->getOverturn() || controller->getBaseState() == 4) { leg_length_des = controller->getDefaultLegLength(); diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp index a18cab25..2f8c3026 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp @@ -28,6 +28,7 @@ void Upstairs::execute(BipedalController* controller, const ros::Time& time, con } double theta_des_l{ M_PI_2 - 0.6 }, theta_des_r{ M_PI_2 - 0.6 }, length_des_l{ 0.18 }, length_des_r{ 0.18 }; + theta_des_l = theta_des_r = leg_state_threshold_->upstair_des_theta; LegCommand left_cmd = { 0, 0, { 0., 0. } }, right_cmd = { 0, 0, { 0., 0. } }; left_cmd = computePidLegCommand(length_des_l, theta_des_l, left_pos_, left_spd_, *pid_legs_[0], *pid_thetas_[0], *pid_thetas_[2], left_angle_, left_leg_state, period); @@ -39,7 +40,7 @@ void Upstairs::execute(BipedalController* controller, const ros::Time& time, con if (left_pos_[0] < 0.2 && left_pos_[1] > leg_state_threshold_->upstair_exit_threshold && right_pos_[0] < 0.2 && right_pos_[1] > leg_state_threshold_->upstair_exit_threshold) { - controller->pubLegLenStatus(false); + controller->pubLegLenStatus(true); controller->setMode(BalanceMode::STAND_UP); controller->setStateChange(false); ROS_INFO("[balance] Exit Upstairs"); From ac651b4bac86e777792b09a556506da96f5748e7 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Wed, 25 Feb 2026 11:35:49 +0800 Subject: [PATCH 36/57] feat: Add VMC class and Gazebo test --- .../series_legged_vmc_controller.h | 4 + .../bipedal_wheel_controller/vmc/VMC.h | 22 +++++ .../series_legged_vmc_controller.cpp | 22 +++-- .../src/bipedal_wheel_controller/vmc/VMC.cpp | 90 +++++++++++++++++++ .../test/vmc_controller.yaml | 6 +- 5 files changed, 135 insertions(+), 9 deletions(-) create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h create mode 100644 rm_chassis_controllers/src/bipedal_wheel_controller/vmc/VMC.cpp diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/series_legged_vmc_controller.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/series_legged_vmc_controller.h index 01298526..a2d06bfa 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/series_legged_vmc_controller.h +++ b/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 namespace rm_chassis_controllers @@ -48,6 +50,8 @@ class VMCController : public controller_interface::MultiInterfaceController vmcPtr_; + ros::Publisher statePublisher_, jointCmdStatePublisher_; ros::Subscriber cmdLegLengthSubscriber_, cmdLegAngleSubscriber_; }; diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h new file mode 100644 index 00000000..9e2b7fcc --- /dev/null +++ b/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 diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp index bc6e3da7..18f3abea 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp @@ -49,6 +49,14 @@ bool VMCController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ROS_ERROR("Load param fail, check the resist of spring_force"); return false; } + double l1, l2; + if (!controller_nh.getParam("l1", l1) || !controller_nh.getParam("l2", l2)) + { + ROS_ERROR("Load param fail, check the resist of l1 or l2"); + return false; + } + vmcPtr_ = std::make_unique(l1, l2, 0); + jointThigh_ = robot_hw->get()->getHandle(thighJoint); jointKnee_ = robot_hw->get()->getHandle(kneeJoint); return true; @@ -66,14 +74,14 @@ void VMCController::update(const ros::Time& time, const ros::Duration& period) // series leg vmc // gazebo - // thigh_angle = jointThigh_.getPosition() + M_PI_2; - // knee_angle = jointKnee_.getPosition() - M_PI_2; + thigh_angle = jointThigh_.getPosition() + M_PI_2; + knee_angle = jointKnee_.getPosition() - M_PI_2; // five link vmc - thigh_angle = jointThigh_.getPosition() + M_PI; - knee_angle = jointKnee_.getPosition(); - leg_pos(thigh_angle, knee_angle, position); - leg_spd(jointThigh_.getVelocity(), jointKnee_.getVelocity(), thigh_angle, knee_angle, speed); + // thigh_angle = jointThigh_.getPosition() + M_PI; + // knee_angle = jointKnee_.getPosition(); + vmcPtr_->leg_pos(thigh_angle, knee_angle, position); + vmcPtr_->leg_spd(jointThigh_.getVelocity(), jointKnee_.getVelocity(), thigh_angle, knee_angle, speed); double effortCmd[2], jointCmd[2]; static double angleSinCmd_ = 0; @@ -88,7 +96,7 @@ void VMCController::update(const ros::Time& time, const ros::Duration& period) effortCmd[0] = pidLength_.computeCommand(lengthCmd_ - position[0], period) - spring_force_; effortCmd[1] = pidAngle_.computeCommand(angle_error, period); - leg_conv(effortCmd[0], effortCmd[1], thigh_angle, knee_angle, jointCmd); + vmcPtr_->leg_conv(effortCmd[0], effortCmd[1], thigh_angle, knee_angle, jointCmd); std_msgs::Float64MultiArray state; state.data.push_back(thigh_angle); state.data.push_back(knee_angle); diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/VMC.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/VMC.cpp new file mode 100644 index 00000000..a53bf332 --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/VMC.cpp @@ -0,0 +1,90 @@ +// +// Created by wk on 2026/2/25. +// + +#include "bipedal_wheel_controller/vmc/VMC.h" +#include + +namespace rm_chassis_controllers +{ +void VMC::leg_pos(double phi1, double phi4, double pos[2]) const +{ + double a_tmp; + double t4; + /* This function was generated by the Symbolic Math Toolbox version 9.2. + */ + /* 15-Sep-2025 20:28:02 */ + t4 = phi1 + phi4; + a_tmp = cos(phi1) * l1_ + cos(t4) * l2_; + t4 = sin(phi1) * l1_ + sin(t4) * l2_; + pos[0] = sqrt(a_tmp * a_tmp + t4 * t4); + pos[1] = atan2(t4, a_tmp); +} + +void VMC::leg_spd(double dphi1, double dphi4, double phi1, double phi4, double* spd) +{ + double J[2][2]; + + calc_jacobian(phi1, phi4, J); + + // 速度映射 + spd[0] = J[0][0] * dphi1 + J[0][1] * dphi4; // dl0 + spd[1] = J[1][0] * dphi1 + J[1][1] * dphi4; // dphi0 +} + +void VMC::leg_conv(double F, double Tp, double phi1, double phi4, double* T) +{ + double J[2][2]; + + calc_jacobian(phi1, phi4, J); + + // J^T * [F; Tp] + T[0] = J[0][0] * F + J[1][0] * Tp; + T[1] = J[0][1] * F + J[1][1] * Tp; +} + +void VMC::calc_jacobian(double phi1, double phi4, double J[2][2]) +{ + const double l1 = 0.218; + const double l2 = 0.26; + + double phi2 = phi1 + phi4; + + double c1 = cos(phi1); + double s1 = sin(phi1); + double c2 = cos(phi2); + double s2 = sin(phi2); + + // 末端笛卡尔坐标 + double Cx = l1 * c1 + l2 * c2; + double Cy = l1 * s1 + l2 * s2; + + double L0 = sqrt(Cx * Cx + Cy * Cy); + + // 防止奇异 + if (L0 < 1e-8) + { + J[0][0] = J[0][1] = 0.0; + J[1][0] = J[1][1] = 0.0; + return; + } + + // 偏导 + double dx_dphi1 = -l1 * s1 - l2 * s2; + double dy_dphi1 = l1 * c1 + l2 * c2; + + double dx_dphi4 = -l2 * s2; + double dy_dphi4 = l2 * c2; + + double inv_L0 = 1.0 / L0; + double inv_L0_sq = 1.0 / (L0 * L0); + + // 第一行：dl0/dphi + J[0][0] = (Cx * dx_dphi1 + Cy * dy_dphi1) * inv_L0; + J[0][1] = (Cx * dx_dphi4 + Cy * dy_dphi4) * inv_L0; + + // 第二行：dphi0/dphi + J[1][0] = (Cx * dy_dphi1 - Cy * dx_dphi1) * inv_L0_sq; + J[1][1] = (Cx * dy_dphi4 - Cy * dx_dphi4) * inv_L0_sq; +} +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/test/vmc_controller.yaml b/rm_chassis_controllers/test/vmc_controller.yaml index 2a4f21e0..ef0a4a37 100644 --- a/rm_chassis_controllers/test/vmc_controller.yaml +++ b/rm_chassis_controllers/test/vmc_controller.yaml @@ -8,8 +8,10 @@ controllers: vmc_controller: type: rm_chassis_controllers/VMCController vmc_bias_angle: 1.57 - spring_force: 75.0 - pid_length: { p: 800.0, i: 0, d: 20, i_clamp_max: 20.0, i_clamp_min: -20.0, antiwindup: true, publish_state: true } + spring_force: 0.0 + pid_length: { p: 800.0, i: 0, d: 25, i_clamp_max: 20.0, i_clamp_min: -20.0, antiwindup: true, publish_state: true } pid_angle: { p: 30.0, i: 0, d: 2, i_clamp_max: 15.0, i_clamp_min: -15.0, antiwindup: true, publish_state: true } thigh_joint: left_hip_joint knee_joint: left_knee_joint + l1: 0.218 + l2: 0.26 From fa69eb98dba8c52a450958396cfaf8b827d9a8cf Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sat, 28 Feb 2026 22:25:17 +0800 Subject: [PATCH 37/57] Delete override follow and add raw_pitch_bias and raw_theta_bias. --- .../bipedal_wheel_controller/controller.h | 1 - .../bipedal_wheel_controller/controller.cpp | 58 ++----------------- 2 files changed, 6 insertions(+), 53 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h index 89790a01..c077aeb4 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h @@ -39,7 +39,6 @@ class BipedalController : public ChassisBase tbl[] = { - { "x_bias", &bias_params_->x }, - { "theta_bias", &bias_params_->theta }, - { "pitch_bias", &bias_params_->pitch }, - { "roll_bias", &bias_params_->roll }, - }; + const std::pair tbl[] = { { "x_bias", &bias_params_->x }, + { "theta_bias", &bias_params_->theta }, + { "pitch_bias", &bias_params_->pitch }, + { "roll_bias", &bias_params_->roll }, + { "raw_pitch_bias", &bias_params_->raw_pitch }, + { "raw_theta_bias", &bias_params_->raw_theta } }; for (const auto& e : tbl) if (!controller_nh.getParam(e.first, *e.second)) @@ -451,51 +451,5 @@ void BipedalController::pubLegLenStatus(const bool& upstair_flag) upstair_status_pub_.publish(msg); } -// void BipedalController::follow(const ros::Time& time, const ros::Duration& period) -//{ -// static bool follow_source_frame_changed_{ false }; -// static std::string last_follow_source_frame_{ follow_source_frame_ }; -// if (state_changed_) -// { -// state_changed_ = false; -// ROS_INFO("[Chassis] Enter FOLLOW"); -// -// ChassisBase::recovery(); -// pid_follow_.reset(); -// } -// -// tfVelToBase(command_source_frame_); -// try -// { -// double roll{}, pitch{}, yaw{}; -// // double target_yaw_bias{ 0 }; -// quatToRPY(robot_state_handle_.lookupTransform("base_link", follow_source_frame_, ros::Time(0)).transform.rotation, -// roll, pitch, yaw); -// double yawForwardError = angles::shortest_angular_distance(0, yaw); -// double yawInverseError = angles::shortest_angular_distance(M_PI, yaw); -// double yawError = abs(yawForwardError) < abs(yawInverseError) ? yawForwardError : yawInverseError; -// if (follow_source_frame_ != last_follow_source_frame_) -// { -// follow_source_frame_changed_ = true; -// } -// if (follow_source_frame_changed_) -// { -// yawError = yawForwardError; -// if (abs(yawError) < 0.1) -// { -// follow_source_frame_changed_ = false; -// } -// } -// pid_follow_.computeCommand(yawError, period); -// vel_cmd_.z = pid_follow_.getCurrentCmd() + cmd_rt_buffer_.readFromRT()->cmd_chassis_.follow_vel_des; -// } -// catch (tf2::TransformException& ex) -// { -// ROS_WARN("%s", ex.what()); -// } -// last_follow_source_frame_ = follow_source_frame_; -// } - } // namespace rm_chassis_controllers PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::BipedalController, controller_interface::ControllerBase) From cc5bc863392c7ef03ce57cf27e213aa1a39a257e Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sat, 28 Feb 2026 22:25:36 +0800 Subject: [PATCH 38/57] Optimize code style. --- .../bipedal_wheel_controller/definitions.h | 14 ++++++++ .../controller_mode/normal.cpp | 34 +++++++++---------- 2 files changed, 31 insertions(+), 17 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h index 6a47baa1..5cf127c8 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h @@ -41,6 +41,8 @@ struct BiasParams double theta; double pitch; double roll; + double raw_pitch; + double raw_theta; }; struct LegStateThresholdParams @@ -86,6 +88,18 @@ enum BalanceMode UPSTAIRS, }; +enum +{ + LEFT = 0, + RIGHT, +}; + +enum +{ + LEG_T = 0, + LEG_Tp +}; + constexpr std::array, 3> jumpLengthDes = { { { JumpPhase::LEG_RETRACTION, 0.12 }, { JumpPhase::JUMP_UP, 0.38 }, { JumpPhase::OFF_GROUND, 0.15 } } }; diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp index 6345267d..ba5dc33e 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -114,10 +114,10 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const { double left_length_des = controller->getCompleteStand() ? leg_length_des : controller->getDefaultLegLength(); double right_length_des = controller->getCompleteStand() ? leg_length_des : controller->getDefaultLegLength(); - F_leg[0] = pid_legs_[0]->computeCommand(left_length_des - current_leg_length, period) - F_inertia + - gravity * cos(left_pos_[1]) + T_roll - spring_force; - F_leg[1] = pid_legs_[1]->computeCommand(right_length_des - current_leg_length, period) + F_inertia + - gravity * cos(right_pos_[1]) - T_roll - spring_force; + F_leg[LEFT] = pid_legs_[LEFT]->computeCommand(left_length_des - current_leg_length, period) - F_inertia + + gravity * cos(left_pos_[1]) + T_roll - spring_force; + F_leg[RIGHT] = pid_legs_[RIGHT]->computeCommand(right_length_des - current_leg_length, period) + F_inertia + + gravity * cos(right_pos_[1]) - T_roll - spring_force; } else { @@ -128,10 +128,10 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const { case JumpPhase::LEG_RETRACTION: ROS_INFO("[balance] ENTER LEG_RETRACTION"); - F_leg(0) = pid_legs_[0]->computeCommand(leg_length_des - current_leg_length, period) + - gravity * cos(left_pos_[1]) + T_roll - spring_force; - F_leg(1) = pid_legs_[1]->computeCommand(leg_length_des - current_leg_length, period) + - gravity * cos(left_pos_[1]) - T_roll - spring_force; + F_leg(LEFT) = pid_legs_[LEFT]->computeCommand(leg_length_des - current_leg_length, period) + + gravity * cos(left_pos_[1]) + T_roll - spring_force; + F_leg(RIGHT) = pid_legs_[RIGHT]->computeCommand(leg_length_des - current_leg_length, period) + + gravity * cos(left_pos_[1]) - T_roll - spring_force; if (current_leg_length < leg_length_des + 0.01) { jumpTime_++; @@ -194,17 +194,17 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const bool left_unstick{ false }, right_unstick{ false }; if (controller->getCompleteStand() && jump_phase_ != JumpPhase::LEG_RETRACTION) { - left_unstick = unstickDetection(F_leg[0], u_left(1), left_spd_[0], left_pos_[0], linear_acc_base_.z, model_params_, - x_left_, leftSupportForceAveragePtr_, period); - right_unstick = unstickDetection(F_leg[1], u_right(1), right_spd_[0], right_pos_[0], linear_acc_base_.z, + left_unstick = unstickDetection(F_leg[LEFT], u_left(1), left_spd_[0], left_pos_[0], linear_acc_base_.z, + model_params_, x_left_, leftSupportForceAveragePtr_, period); + right_unstick = unstickDetection(F_leg[RIGHT], u_right(1), right_spd_[0], right_pos_[0], linear_acc_base_.z, model_params_, x_right_, rightSupportForceAveragePtr_, period); } bool unstick[2]{}; unstick[0] = left_unstick; unstick[1] = right_unstick; Matrix F_N{}; - F_N(0) = leftSupportForceAveragePtr_->output(); - F_N(1) = rightSupportForceAveragePtr_->output(); + F_N(LEFT) = leftSupportForceAveragePtr_->output(); + F_N(RIGHT) = rightSupportForceAveragePtr_->output(); controller->pubLQRStatus(-x_left, -x_right, x_left_ref, x_right_ref, u_left, u_right, F_N, unstick); // left_unstick = false; @@ -222,12 +222,12 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const // Control double left_T[2], right_T[2]; - leg_conv(F_leg[0], u_left(1) + T_theta_diff, left_angle_[0], left_angle_[1], left_T); - leg_conv(F_leg[1], u_right(1) - T_theta_diff, right_angle_[0], right_angle_[1], right_T); + leg_conv(F_leg[LEFT], u_left(1) + T_theta_diff, left_angle_[0], left_angle_[1], left_T); + leg_conv(F_leg[RIGHT], u_right(1) - T_theta_diff, right_angle_[0], right_angle_[1], right_T); double left_wheel_cmd = left_unstick ? 0. : u_left(0) - T_yaw; double right_wheel_cmd = right_unstick ? 0. : u_right(0) + T_yaw; - LegCommand left_cmd = { F_leg[0], u_left[1], { left_T[0], left_T[1] } }, - right_cmd = { F_leg[1], u_right[1], { right_T[0], right_T[1] } }; + LegCommand left_cmd = { F_leg[LEFT], u_left[1], { left_T[0], left_T[1] } }, + right_cmd = { F_leg[RIGHT], u_right[1], { right_T[0], right_T[1] } }; // upstairs if (jump_phase_ == JumpPhase::IDLE && linear_acc_base_.z < -7.0 && controller->getCompleteStand() && From 06992b4b854c4e33dd9afbc41837cb285f87349f Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sun, 1 Mar 2026 16:12:40 +0800 Subject: [PATCH 39/57] Modify T_roll to F_roll. --- .../controller_mode/normal.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp index ba5dc33e..afb01ae7 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -48,7 +48,7 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const // PID double T_yaw = pid_yaw_vel_->computeCommand(friction_circle_alpha * vel_cmd_.z - angular_vel_base_.z, period); double T_theta_diff = pid_theta_diff_->computeCommand(right_pos_[1] - left_pos_[1], period); - double T_roll = pid_roll_->computeCommand(0. - roll_, period); + double F_roll = pid_roll_->computeCommand(0. - roll_, period); // LQR Matrix coeffs_ = controller->getCoeffs(); @@ -115,9 +115,9 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const double left_length_des = controller->getCompleteStand() ? leg_length_des : controller->getDefaultLegLength(); double right_length_des = controller->getCompleteStand() ? leg_length_des : controller->getDefaultLegLength(); F_leg[LEFT] = pid_legs_[LEFT]->computeCommand(left_length_des - current_leg_length, period) - F_inertia + - gravity * cos(left_pos_[1]) + T_roll - spring_force; + gravity * cos(left_pos_[1]) + F_roll - spring_force; F_leg[RIGHT] = pid_legs_[RIGHT]->computeCommand(right_length_des - current_leg_length, period) + F_inertia + - gravity * cos(right_pos_[1]) - T_roll - spring_force; + gravity * cos(right_pos_[1]) - F_roll - spring_force; } else { @@ -129,9 +129,9 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const case JumpPhase::LEG_RETRACTION: ROS_INFO("[balance] ENTER LEG_RETRACTION"); F_leg(LEFT) = pid_legs_[LEFT]->computeCommand(leg_length_des - current_leg_length, period) + - gravity * cos(left_pos_[1]) + T_roll - spring_force; + gravity * cos(left_pos_[1]) + F_roll - spring_force; F_leg(RIGHT) = pid_legs_[RIGHT]->computeCommand(leg_length_des - current_leg_length, period) + - gravity * cos(left_pos_[1]) - T_roll - spring_force; + gravity * cos(left_pos_[1]) - F_roll - spring_force; if (current_leg_length < leg_length_des + 0.01) { jumpTime_++; @@ -207,16 +207,16 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const F_N(RIGHT) = rightSupportForceAveragePtr_->output(); controller->pubLQRStatus(-x_left, -x_right, x_left_ref, x_right_ref, u_left, u_right, F_N, unstick); - // left_unstick = false; - // right_unstick = false; updateUnstick(left_unstick, right_unstick); if (controller->getCompleteStand() && left_unstick && jump_phase_ != JumpPhase::LEG_RETRACTION) { + F_leg[LEFT] -= F_roll; u_left = k_left_unstick * (-x_left); } if (controller->getCompleteStand() && right_unstick && jump_phase_ != JumpPhase::LEG_RETRACTION) { + F_leg[RIGHT] += F_roll; u_right = k_right_unstick * (-x_right); } From c4040edd0e045b9ccb21581818c1d468ff6d98cf Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sun, 1 Mar 2026 16:16:16 +0800 Subject: [PATCH 40/57] Add fivelink vmc for VMC class. --- .../src/bipedal_wheel_controller/vmc/VMC.cpp | 65 ++++++++++++++++--- 1 file changed, 56 insertions(+), 9 deletions(-) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/VMC.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/VMC.cpp index a53bf332..d6eeb4c2 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/VMC.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/VMC.cpp @@ -19,6 +19,25 @@ void VMC::leg_pos(double phi1, double phi4, double pos[2]) const t4 = sin(phi1) * l1_ + sin(t4) * l2_; pos[0] = sqrt(a_tmp * a_tmp + t4 * t4); pos[1] = atan2(t4, a_tmp); + + // five link vmc + // double YD, YB, XD, XB, lBD, A0, B0, C0, phi2, XC, YC; + // double L0, Phi0; + // YD = l4_ * sin(phi4); + // YB = l1_ * sin(phi1); + // XD = l5_ + l4_ * cos(phi4); + // XB = l1_ * cos(phi1); + // lBD = sqrt((XD - XB) * (XD - XB) + (YD - YB) * (YD - YB)); + // A0 = 2 * l2_ * (XD - XB); + // B0 = 2 * l2_ * (YD - YB); + // C0 = l2_ * l2_ + lBD * lBD - l3_ * l3_; + // phi2 = 2 * atan2((B0 + sqrt(A0 * A0 + B0 * B0 - C0 * C0)), (A0 + C0)); + // XC = l1_ * cos(phi1) + l2_ * cos(phi2); + // YC = l1_ * sin(phi1) + l2_ * sin(phi2); + // L0 = sqrt((XC - l5_ / 2) * (XC - l5_ / 2) + YC * YC); + // Phi0 = -atan2((XC - l5_ / 2), YC); + // pos[0] = L0; + // pos[1] = Phi0; } void VMC::leg_spd(double dphi1, double dphi4, double phi1, double phi4, double* spd) @@ -45,9 +64,6 @@ void VMC::leg_conv(double F, double Tp, double phi1, double phi4, double* T) void VMC::calc_jacobian(double phi1, double phi4, double J[2][2]) { - const double l1 = 0.218; - const double l2 = 0.26; - double phi2 = phi1 + phi4; double c1 = cos(phi1); @@ -56,8 +72,8 @@ void VMC::calc_jacobian(double phi1, double phi4, double J[2][2]) double s2 = sin(phi2); // 末端笛卡尔坐标 - double Cx = l1 * c1 + l2 * c2; - double Cy = l1 * s1 + l2 * s2; + double Cx = l1_ * c1 + l2_ * c2; + double Cy = l1_ * s1 + l2_ * s2; double L0 = sqrt(Cx * Cx + Cy * Cy); @@ -70,11 +86,11 @@ void VMC::calc_jacobian(double phi1, double phi4, double J[2][2]) } // 偏导 - double dx_dphi1 = -l1 * s1 - l2 * s2; - double dy_dphi1 = l1 * c1 + l2 * c2; + double dx_dphi1 = -l1_ * s1 - l2_ * s2; + double dy_dphi1 = l1_ * c1 + l2_ * c2; - double dx_dphi4 = -l2 * s2; - double dy_dphi4 = l2 * c2; + double dx_dphi4 = -l2_ * s2; + double dy_dphi4 = l2_ * c2; double inv_L0 = 1.0 / L0; double inv_L0_sq = 1.0 / (L0 * L0); @@ -86,5 +102,36 @@ void VMC::calc_jacobian(double phi1, double phi4, double J[2][2]) // 第二行：dphi0/dphi J[1][0] = (Cx * dy_dphi1 - Cy * dx_dphi1) * inv_L0_sq; J[1][1] = (Cx * dy_dphi4 - Cy * dx_dphi4) * inv_L0_sq; + + // five link vmc + // double YD, YB, XD, XB, lBD, A0, B0, C0, XC, YC; + // double phi2, phi3; + // double L0, phi0; + // double j11, j12, j21, j22; + // + // YD = l4_ * sin(phi4); + // YB = l1_ * sin(phi1); + // XD = l5_ + l4_ * cos(phi4); + // XB = l1_ * cos(phi1); + // lBD = sqrt((XD - XB) * (XD - XB) + (YD - YB) * (YD - YB)); + // A0 = 2 * l2_ * (XD - XB); + // B0 = 2 * l2_ * (YD - YB); + // C0 = l2_ * l2_ + lBD * lBD - l3_ * l3_; + // phi2 = 2 * atan2((B0 + sqrt(A0 * A0 + B0 * B0 - C0 * C0)), A0 + C0); + // phi3 = atan2(YB - YD + l2_ * sin(phi2), XB - XD + l2_ * cos(phi2)); + // XC = l1_ * cos(phi1) + l2_ * cos(phi2); + // YC = l1_ * sin(phi1) + l2_ * sin(phi2); + // L0 = sqrt((XC - l5_ / 2) * (XC - l5_ / 2) + YC * YC); + // phi0 = atan2(YC, XC - l5_ / 2); + // + // j11 = (l1_ * sin(phi0 - phi3) * sin(phi1 - phi2)) / sin(phi3 - phi2); + // j12 = (l4_ * sin(phi0 - phi2) * sin(phi3 - phi4)) / sin(phi3 - phi2); + // j21 = (l1_ * cos(phi0 - phi3) * sin(phi1 - phi2)) / (L0 * sin(phi3 - phi2)); + // j22 = (l4_ * cos(phi0 - phi2) * sin(phi3 - phi4)) / (L0 * sin(phi3 - phi2)); + // + // J[0][0] = j11; + // J[0][1] = j12; + // J[1][0] = j21; + // J[1][1] = j22; } } // namespace rm_chassis_controllers From 8834007dbda4a204e1dbb0a30bcd8cc2a59d8e38 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sun, 1 Mar 2026 16:17:06 +0800 Subject: [PATCH 41/57] Add VMC config for vmc_controller. --- rm_chassis_controllers/test/vmc_controller.yaml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rm_chassis_controllers/test/vmc_controller.yaml b/rm_chassis_controllers/test/vmc_controller.yaml index ef0a4a37..eccea9b1 100644 --- a/rm_chassis_controllers/test/vmc_controller.yaml +++ b/rm_chassis_controllers/test/vmc_controller.yaml @@ -13,5 +13,9 @@ controllers: pid_angle: { p: 30.0, i: 0, d: 2, i_clamp_max: 15.0, i_clamp_min: -15.0, antiwindup: true, publish_state: true } thigh_joint: left_hip_joint knee_joint: left_knee_joint + # series_legged1 l1: 0.218 l2: 0.26 + # series_legged2 +# l1: 0.21 +# l2: 0.248 From 7b47b18f8dbe6cf904b7a3a77865344cc667fe7c Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sun, 1 Mar 2026 12:51:07 +0000 Subject: [PATCH 42/57] Add powerlimit and LQR dynamicx params. --- rm_chassis_controllers/CMakeLists.txt | 10 ++- rm_chassis_controllers/cfg/LQRWeight.cfg | 18 ++++ rm_chassis_controllers/cfg/PowerLimit.cfg | 12 +++ .../bipedal_wheel_controller/controller.h | 15 ++++ .../rm_chassis_controllers/chassis_base.h | 12 +++ rm_chassis_controllers/package.xml | 1 + .../bipedal_wheel_controller/controller.cpp | 84 +++++++++++++++++-- rm_chassis_controllers/src/chassis_base.cpp | 41 ++++++++- 8 files changed, 181 insertions(+), 12 deletions(-) create mode 100644 rm_chassis_controllers/cfg/LQRWeight.cfg create mode 100644 rm_chassis_controllers/cfg/PowerLimit.cfg diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt index 8c9d2e13..9473d39b 100644 --- a/rm_chassis_controllers/CMakeLists.txt +++ b/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} ) @@ -75,7 +82,6 @@ add_library(${PROJECT_NAME} src/sentry.cpp src/swerve.cpp src/balance.cpp - ${BIPEDAL_SOURCES} ) @@ -84,6 +90,8 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ) +add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) + ############# ## Install ## ############# diff --git a/rm_chassis_controllers/cfg/LQRWeight.cfg b/rm_chassis_controllers/cfg/LQRWeight.cfg new file mode 100644 index 00000000..ddc3bec3 --- /dev/null +++ b/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")) \ No newline at end of file diff --git a/rm_chassis_controllers/cfg/PowerLimit.cfg b/rm_chassis_controllers/cfg/PowerLimit.cfg new file mode 100644 index 00000000..72aec5d4 --- /dev/null +++ b/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("velocity_coeff", double_t, 0, "velocity coefficient for power limit", 0.0, 0.0, 100.0) +gen.add("effort_coeff", double_t, 0, "effort coefficient for power limit", 0.0, 0.0, 100.0) +gen.add("power_offset", double_t, 0, "power offset for power limit", 0.0, 0.0, 100.0) + +exit(gen.generate(PACKAGE, "chassis_controller", "PowerLimit")) \ No newline at end of file diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h index c077aeb4..6e9f9847 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h @@ -22,6 +22,8 @@ #include #include #include "rm_chassis_controllers/chassis_base.h" +#include +#include #include "bipedal_wheel_controller/helper_functions.h" #include "bipedal_wheel_controller/definitions.h" @@ -31,6 +33,12 @@ 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 { @@ -79,6 +87,9 @@ void pubLegLenStatus(const bool& upstair_flag); void polyfit(const std::vector>& Ks, const std::vector& L0s, Eigen::Matrix& coeffs); geometry_msgs::Twist odometry() override; + + void reconfigCB(rm_chassis_controllers::LQRWeightConfig& config, uint32_t level); + Eigen::Matrix coeffs_; Eigen::Matrix q_{}; Eigen::Matrix r_{}; @@ -121,6 +132,10 @@ void pubLegLenStatus(const bool& upstair_flag); bool jumpCmd_{ false }; // ROS Interface + dynamic_reconfigure::Server* d_srv_; + realtime_tools::RealtimeBuffer config_rt_buffer_; + LQRConfig config_{}; + bool dynamic_reconfig_initialized_{ false }; ros::Subscriber leg_cmd_sub_; ros::Publisher unstick_pub_, upstair_status_pub_; ros::Publisher legged_chassis_status_pub_, legged_chassis_mode_pub_; diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h index de527cda..68e58d79 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -48,9 +48,16 @@ #include #include #include +#include namespace rm_chassis_controllers { +struct PowerLimitParams +{ + double velocity_coeff{}; + double effort_coeff{}; + double power_offset{}; +}; struct Command { geometry_msgs::Twist cmd_vel_; @@ -141,6 +148,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController */ void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg); void outsideOdomCallback(const nav_msgs::Odometry::ConstPtr& msg); + void powerLimitReconfigCB(rm_chassis_controllers::PowerLimitConfig& config, uint32_t level); rm_control::RobotStateHandle robot_state_handle_{}; hardware_interface::EffortJointInterface* effort_joint_interface_{}; @@ -172,6 +180,10 @@ class ChassisBase : public controller_interface::MultiInterfaceController geometry_msgs::Vector3 vel_cmd_{}; // x, y control_toolbox::Pid pid_follow_; + dynamic_reconfigure::Server* power_limit_srv_{}; + realtime_tools::RealtimeBuffer power_limit_rt_buffer_; + PowerLimitParams power_config_{}; + bool power_limit_reconfig_initialized_{ false }; std::shared_ptr > odom_pub_; rm_common::TfRtBroadcaster tf_broadcaster_{}; ros::Subscriber outside_odom_sub_; diff --git a/rm_chassis_controllers/package.xml b/rm_chassis_controllers/package.xml index bed77aad..ca9540ac 100644 --- a/rm_chassis_controllers/package.xml +++ b/rm_chassis_controllers/package.xml @@ -21,6 +21,7 @@ tf2_geometry_msgs nav_msgs angles + dynamic_reconfigure robot_localization diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp index 4ca24fa0..d8d5696d 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp @@ -46,6 +46,12 @@ bool BipedalController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan !setupControlParams(controller_nh) || !setupThresholdParams(controller_nh)) return false; + d_srv_ = + new dynamic_reconfigure::Server(ros::NodeHandle(controller_nh, "lqr")); + dynamic_reconfigure::Server::CallbackType cb = + boost::bind(&BipedalController::reconfigCB, this, _1, _2); + d_srv_->setCallback(cb); + auto legCmdCallback = [this](const rm_msgs::LegCmd::ConstPtr& msg) { legCmd_ = msg->leg_length; jumpCmd_ = msg->jump; @@ -149,16 +155,16 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat // vmc double left_angle[2]{}, right_angle[2]{}, left_pos[2]{}, left_spd[2]{}, right_pos[2]{}, right_spd[2]{}; // [0]:hip_vmc_joint [1]:knee_vmc_joint - left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; - left_angle[1] = left_knee_joint_handle_.getPosition(); - right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; - right_angle[1] = right_knee_joint_handle_.getPosition(); + // left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; + // left_angle[1] = left_knee_joint_handle_.getPosition(); + // right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; + // right_angle[1] = right_knee_joint_handle_.getPosition(); // gazebo - // left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; - // left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; - // right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; - // right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; + left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; + left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; + right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; + right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; // [0] is length, [1] is angle leg_pos(left_angle[0], left_angle[1], left_pos); @@ -223,7 +229,7 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat // ros msg rm_msgs::LeggedChassisStatus legged_chassis_status_msg; - legged_chassis_status_msg.roll = wheel_vel_aver; + legged_chassis_status_msg.roll = roll; legged_chassis_status_msg.pitch = x_left_[4]; legged_chassis_status_msg.d_pitch = x_left_[5]; legged_chassis_status_msg.yaw = yaw; @@ -336,6 +342,17 @@ bool BipedalController::setupLQR(ros::NodeHandle& controller_nh) ks.push_back(k); } polyfit(ks, lengths, coeffs_); + + config_.Q_theta = q_.diagonal()(0); + config_.Q_d_theta = q_.diagonal()(1); + config_.Q_x = q_.diagonal()(2); + config_.Q_dx = q_.diagonal()(3); + config_.Q_phi = q_.diagonal()(4); + config_.Q_d_phi = q_.diagonal()(5); + config_.R_T = r_.diagonal()(0); + config_.R_Tp = r_.diagonal()(1); + config_rt_buffer_.initRT(config_); + return true; } @@ -451,5 +468,54 @@ void BipedalController::pubLegLenStatus(const bool& upstair_flag) upstair_status_pub_.publish(msg); } +void BipedalController::reconfigCB(rm_chassis_controllers::LQRWeightConfig& config, uint32_t /*level*/) +{ + ROS_INFO("[LQR] Dynamic params change"); + if (!dynamic_reconfig_initialized_) + { + LQRConfig init_config = *config_rt_buffer_.readFromNonRT(); + config.Q_theta = init_config.Q_theta; + config.Q_d_theta = init_config.Q_d_theta; + config.Q_x = init_config.Q_x; + config.Q_dx = init_config.Q_dx; + config.Q_phi = init_config.Q_phi; + config.Q_d_phi = init_config.Q_d_phi; + config.R_T = init_config.R_T; + config.R_Tp = init_config.R_Tp; + dynamic_reconfig_initialized_ = true; + } + LQRConfig config_non_rt{ .Q_theta = config.Q_theta, + .Q_d_theta = config.Q_d_theta, + .Q_x = config.Q_x, + .Q_dx = config.Q_dx, + .Q_phi = config.Q_phi, + .Q_d_phi = config.Q_d_phi, + .R_T = config.R_T, + .R_Tp = config.R_Tp }; + config_rt_buffer_.writeFromNonRT(config_non_rt); + + q_.diagonal() << config.Q_theta, config.Q_d_theta, config.Q_x, config.Q_dx, config.Q_phi, config.Q_d_phi; + r_.diagonal() << config.R_T, config.R_Tp; + + std::vector lengths; + std::vector> ks; + for (int i = 10; i < 40; i++) + { + double length = i / 100.; + lengths.push_back(length); + Eigen::Matrix a{}; + Eigen::Matrix b{}; + generateAB(model_params_, a, b, length); + Lqr lqr(a, b, q_, r_); + if (!lqr.computeK()) + { + ROS_ERROR("Failed to compute K of LQR."); + } + Eigen::Matrix k = lqr.getK(); + ks.push_back(k); + } + polyfit(ks, lengths, coeffs_); +} + } // namespace rm_chassis_controllers PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::BipedalController, controller_interface::ControllerBase) diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 627b4831..48acde4c 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -117,6 +117,17 @@ bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan if (!pid_follow_.init(ros::NodeHandle(controller_nh, "pid_follow"))) return false; + // dynamic reconfigure + power_config_.power_offset = power_offset_; + power_config_.effort_coeff = effort_coeff_; + power_config_.velocity_coeff = velocity_coeff_; + power_limit_rt_buffer_.initRT(power_config_); + power_limit_srv_ = new dynamic_reconfigure::Server( + ros::NodeHandle(controller_nh, "power_limit")); + dynamic_reconfigure::Server::CallbackType cb = + boost::bind(&ChassisBase::powerLimitReconfigCB, this, _1, _2); + power_limit_srv_->setCallback(cb); + return true; } @@ -381,10 +392,36 @@ void ChassisBase::recovery() ramp_w_->clear(vel_cmd_.z); } +template +void ChassisBase::powerLimitReconfigCB(rm_chassis_controllers::PowerLimitConfig& config, uint32_t /*level*/) +{ + ROS_INFO("[Power Limit] Dynamic params change"); + if (!power_limit_reconfig_initialized_) + { + PowerLimitParams init_config = *power_limit_rt_buffer_.readFromNonRT(); + config.velocity_coeff = init_config.velocity_coeff; + config.effort_coeff = init_config.effort_coeff; + config.power_offset = init_config.power_offset; + power_limit_reconfig_initialized_ = true; + return; + } + + PowerLimitParams config_non_rt; + config_non_rt.velocity_coeff = config.velocity_coeff; + config_non_rt.effort_coeff = config.effort_coeff; + config_non_rt.power_offset = config.power_offset; + power_limit_rt_buffer_.writeFromNonRT(config_non_rt); +} + template void ChassisBase::powerLimit() { double power_limit = cmd_rt_buffer_.readFromRT()->cmd_chassis_.power_limit; + power_config_ = *power_limit_rt_buffer_.readFromRT(); + + double velocity_coeff = power_config_.velocity_coeff; + double effort_coeff = power_config_.effort_coeff; + double power_offset = power_config_.power_offset; // Three coefficients of a quadratic equation in one variable double a = 0., b = 0., c = 0.; for (const auto& joint : joint_handles_) @@ -398,8 +435,8 @@ void ChassisBase::powerLimit() c += square(real_vel); } } - a *= effort_coeff_; - c = c * velocity_coeff_ - power_offset_ - power_limit; + a *= effort_coeff; + c = c * velocity_coeff - power_offset - power_limit; // Root formula for quadratic equation in one variable double zoom_coeff = (square(b) - 4 * a * c) > 0 ? ((-b + sqrt(square(b) - 4 * a * c)) / (2 * a)) : 0.; for (auto joint : joint_handles_) From 7df1c6cfe5753dde62201b5aa23e2a26ad2495b8 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sun, 1 Mar 2026 21:43:30 +0800 Subject: [PATCH 43/57] Add VMC for series_legged controller. --- .../bipedal_wheel_controller/controller.h | 4 ++- .../bipedal_wheel_controller/vmc/VMC.h | 4 ++- .../bipedal_wheel_controller/controller.cpp | 36 +++++++++++-------- .../controller_mode/normal.cpp | 6 ++-- 4 files changed, 30 insertions(+), 20 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h index c077aeb4..62014818 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h @@ -26,6 +26,7 @@ #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 { @@ -39,7 +40,6 @@ class BipedalController : public ChassisBase mode_manager_; + VMCPtr vmc_; // Slippage_detection double leftWheelVel{}, rightWheelVel{}, leftWheelVelAbsolute{}, rightWheelVelAbsolute{}, slip_alpha_{ 2.0 }, diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h index 9e2b7fcc..52a06523 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h @@ -1,13 +1,14 @@ // // Created by wk on 2026/2/25. // +#include 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){}; + explicit VMC(double l1, double l2, double l5 = 0) : l1_(l1), l2_(l2), l3_(l2), l4_(l1), l5_(l5){}; ~VMC() = default; void leg_pos(double phi1, double phi4, double pos[2]) const; @@ -19,4 +20,5 @@ class VMC double l1_, l2_, l3_, l4_, l5_; }; +using VMCPtr = std::shared_ptr; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp index 4ca24fa0..7bdf28b0 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp @@ -149,24 +149,24 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat // vmc double left_angle[2]{}, right_angle[2]{}, left_pos[2]{}, left_spd[2]{}, right_pos[2]{}, right_spd[2]{}; // [0]:hip_vmc_joint [1]:knee_vmc_joint - left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; - left_angle[1] = left_knee_joint_handle_.getPosition(); - right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; - right_angle[1] = right_knee_joint_handle_.getPosition(); + // left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; + // left_angle[1] = left_knee_joint_handle_.getPosition(); + // right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; + // right_angle[1] = right_knee_joint_handle_.getPosition(); // gazebo - // left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; - // left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; - // right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; - // right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; + left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; + left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; + right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; + right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; // [0] is length, [1] is angle - leg_pos(left_angle[0], left_angle[1], left_pos); - leg_pos(right_angle[0], right_angle[1], right_pos); - leg_spd(left_hip_joint_handle_.getVelocity(), left_knee_joint_handle_.getVelocity(), left_angle[0], left_angle[1], - left_spd); - leg_spd(right_hip_joint_handle_.getVelocity(), right_knee_joint_handle_.getVelocity(), right_angle[0], right_angle[1], - right_spd); + vmc_->leg_pos(left_angle[0], left_angle[1], left_pos); + vmc_->leg_pos(right_angle[0], right_angle[1], right_pos); + vmc_->leg_spd(left_hip_joint_handle_.getVelocity(), left_knee_joint_handle_.getVelocity(), left_angle[0], + left_angle[1], left_spd); + vmc_->leg_spd(right_hip_joint_handle_.getVelocity(), right_knee_joint_handle_.getVelocity(), right_angle[0], + right_angle[1], right_spd); left_leg_angle_vel_lpFilterPtr_->input(left_spd[1]); right_leg_angle_vel_lpFilterPtr_->input(right_spd[1]); // left_leg_angle_lpFilterPtr_->input(left_pos[1]); @@ -289,6 +289,14 @@ bool BipedalController::setupModelParams(ros::NodeHandle& controller_nh) return false; } + double l1, l2; + if (!controller_nh.getParam("l1", l1) || !controller_nh.getParam("l2", l2)) + { + ROS_ERROR("Param %s or %s not given (namespace: %s)", "l1", "l2", controller_nh.getNamespace().c_str()); + return false; + } + vmc_ = std::make_shared(l1, l2); + if (!controller_nh.getParam("default_leg_length", default_leg_length_)) { ROS_ERROR("Param %s not given (namespace: %s)", "default_leg_length", controller_nh.getNamespace().c_str()); diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp index afb01ae7..eab19b09 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -93,8 +93,6 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const u_left = k_left * (-x_left); u_right = k_right * (-x_right); - // std::cout << "leg_len:" << left_pos_[0] << std::endl << k_left << std::endl; - // Compute leg thrust auto model_params_ = controller->getModelParams(); auto control_params_ = controller->getControlParams(); @@ -222,8 +220,8 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const // Control double left_T[2], right_T[2]; - leg_conv(F_leg[LEFT], u_left(1) + T_theta_diff, left_angle_[0], left_angle_[1], left_T); - leg_conv(F_leg[RIGHT], u_right(1) - T_theta_diff, right_angle_[0], right_angle_[1], right_T); + controller->getVMCPtr()->leg_conv(F_leg[LEFT], u_left(1) + T_theta_diff, left_angle_[0], left_angle_[1], left_T); + controller->getVMCPtr()->leg_conv(F_leg[RIGHT], u_right(1) - T_theta_diff, right_angle_[0], right_angle_[1], right_T); double left_wheel_cmd = left_unstick ? 0. : u_left(0) - T_yaw; double right_wheel_cmd = right_unstick ? 0. : u_right(0) + T_yaw; LegCommand left_cmd = { F_leg[LEFT], u_left[1], { left_T[0], left_T[1] } }, From d189f472add834f650631b513b9dc22837917cfb Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sun, 1 Mar 2026 13:48:54 +0000 Subject: [PATCH 44/57] Change gazebo to vmc. --- .../src/bipedal_wheel_controller/controller.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp index d8d5696d..afd12a49 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp @@ -155,16 +155,16 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat // vmc double left_angle[2]{}, right_angle[2]{}, left_pos[2]{}, left_spd[2]{}, right_pos[2]{}, right_spd[2]{}; // [0]:hip_vmc_joint [1]:knee_vmc_joint - // left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; - // left_angle[1] = left_knee_joint_handle_.getPosition(); - // right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; - // right_angle[1] = right_knee_joint_handle_.getPosition(); + left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; + left_angle[1] = left_knee_joint_handle_.getPosition(); + right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; + right_angle[1] = right_knee_joint_handle_.getPosition(); // gazebo - left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; - left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; - right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; - right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; + // left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; + // left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; + // right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; + // right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; // [0] is length, [1] is angle leg_pos(left_angle[0], left_angle[1], left_pos); From 5df9d9f6a699a8b27beaed7b07770b293d210041 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sun, 1 Mar 2026 21:54:41 +0800 Subject: [PATCH 45/57] Delete unused code. --- .../bipedal_wheel_controller/helper_functions.h | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h index 6785810c..8c5406ff 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h @@ -85,17 +85,6 @@ inline LegCommand computePidLegCommand(double desired_length, double desired_ang 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; } From e7e0e1fe6fb4a7cc677084d3db8da9f20c4209f0 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sun, 1 Mar 2026 21:54:59 +0800 Subject: [PATCH 46/57] Use VMC class for recover mode. --- .../controller_mode/recover.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp index ca568c13..fe3fe441 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp @@ -42,16 +42,16 @@ void Recover::execute(BipedalController* controller, const ros::Time& time, cons T_theta_diff = pid_theta_diff_->computeCommand(leg_theta_diff_, period); left_cmd.force = pid_legs_[0]->computeCommand(desired_leg_length_ - left_pos_[0], period) + feedforward_force; right_cmd.force = pid_legs_[1]->computeCommand(desired_leg_length_ - right_pos_[0], period) + feedforward_force; - leg_conv(left_cmd.force, T_theta_diff, left_angle_[0], left_angle_[1], left_cmd.input); - leg_conv(right_cmd.force, -T_theta_diff, right_angle_[0], right_angle_[1], right_cmd.input); + controller->getVMCPtr()->leg_conv(left_cmd.force, T_theta_diff, left_angle_[0], left_angle_[1], left_cmd.input); + controller->getVMCPtr()->leg_conv(right_cmd.force, -T_theta_diff, right_angle_[0], right_angle_[1], right_cmd.input); if (abs(leg_theta_diff_) < 0.1) { left_cmd.torque = pid_thetas_[2]->computeCommand(leg_recovery_velocity_ - left_spd_[1], period); right_cmd.torque = pid_thetas_[3]->computeCommand(leg_recovery_velocity_ - right_spd_[1], period); - leg_conv(left_cmd.force, 10 * leg_recovery_velocity_ + left_cmd.torque + T_theta_diff, left_angle_[0], - left_angle_[1], left_cmd.input); - leg_conv(right_cmd.force, 10 * leg_recovery_velocity_ + right_cmd.torque - T_theta_diff, right_angle_[0], - right_angle_[1], right_cmd.input); + controller->getVMCPtr()->leg_conv(left_cmd.force, 10 * leg_recovery_velocity_ + left_cmd.torque + T_theta_diff, + left_angle_[0], left_angle_[1], left_cmd.input); + controller->getVMCPtr()->leg_conv(right_cmd.force, 10 * leg_recovery_velocity_ + right_cmd.torque - T_theta_diff, + right_angle_[0], right_angle_[1], right_cmd.input); } } setJointCommands(joint_handles_, left_cmd, right_cmd); From 661e1bb14abde24f89503d43e690dc502461aaf3 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Mon, 2 Mar 2026 10:58:58 +0800 Subject: [PATCH 47/57] docs: Add doxygen docs for VMC class function. --- .../bipedal_wheel_controller/vmc/VMC.h | 46 +++++++++++++++++++ 1 file changed, 46 insertions(+) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h index 52a06523..4b0ad58b 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h @@ -11,11 +11,57 @@ class VMC explicit VMC(double l1, double l2, double l5 = 0) : l1_(l1), l2_(l2), l3_(l2), l4_(l1), l5_(l5){}; ~VMC() = default; + /** + * @brief Calculate the leg position (length and angle) based on joint angles. + * + * This function computes the forward kinematics to find the end-effector position + * in polar coordinates (length L0 and angle Phi0). + * + * @param phi1 The first joint angle (e.g., hip/thigh joint). + * @param phi4 The second joint angle (e.g., knee/calf joint), possibly relative or absolute depending on mechanism. + * @param pos Output array where pos[0] is the length (L0) and pos[1] is the angle (Phi0). + */ void leg_pos(double phi1, double phi4, double pos[2]) const; + + /** + * @brief Calculate the leg velocity (length rate and angle rate) based on joint velocities. + * + * This function computes the end-effector velocity in polar coordinates + * by mapping joint velocities through the Jacobian. + * + * @param dphi1 Velocity of the first joint. + * @param dphi4 Velocity of the second joint. + * @param phi1 Current position of the first joint. + * @param phi4 Current position of the second joint. + * @param spd Output array where spd[0] is linear velocity (dL0) and spd[1] is angular velocity (dPhi0). + */ void leg_spd(double dphi1, double dphi4, double phi1, double phi4, double spd[2]); + + /** + * @brief Convert Cartesian forces/torques to joint torques. + * + * This function maps forces acting on the leg end-effector (in polar space) + * to the required joint torques using the transpose of the Jacobian. + * + * @param F Force along the leg length (radial force). + * @param Tp Torque/Force corresponding to the leg angle (tangential force/torque). + * @param phi1 Current position of the first joint. + * @param phi4 Current position of the second joint. + * @param T Output array where T[0] is the torque for joint 1 and T[1] is the torque for joint 2. + */ void leg_conv(double F, double Tp, double phi1, double phi4, double T[2]); private: + /** + * @brief Calculate the Jacobian matrix of the leg mechanism. + * + * The Jacobian J relates joint velocities to end-effector velocities: + * [v_L; v_phi] = J * [dphi1; dphi4] + * + * @param phi1 Current position of the first joint. + * @param phi4 Current position of the second joint. + * @param J Output 2x2 Jacobian matrix. + */ void calc_jacobian(double phi1, double phi4, double J[2][2]); double l1_, l2_, l3_, l4_, l5_; From f18b15cd69adf7b1deb3233d93bed1a5c5d46d93 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Mon, 2 Mar 2026 11:02:31 +0800 Subject: [PATCH 48/57] refactor: mark unused function to silence warnings. --- .../helper_functions.h | 37 +++++++++++-------- 1 file changed, 21 insertions(+), 16 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h index 8c5406ff..1303446d 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h @@ -64,13 +64,16 @@ inline void generateAB(const std::shared_ptr& model_params, Eigen:: * @param angle_pid * @param leg_angle * @param period + * @param feedforward_force + * @param overturn * @return */ -inline LegCommand computePidLegCommand(double desired_length, double desired_angle, double leg_pos[2], - double leg_spd[2], control_toolbox::Pid& length_pid, - control_toolbox::Pid& angle_pid, control_toolbox::Pid& angle_vel_pid, - const double* leg_angle, const int& leg_state, const ros::Duration& period, - double feedforward_force = 0.0f, const bool& overturn = false) +[[maybe_unused]] inline LegCommand computePidLegCommand(double desired_length, double desired_angle, double leg_pos[2], + double leg_spd[2], control_toolbox::Pid& length_pid, + control_toolbox::Pid& angle_pid, + control_toolbox::Pid& angle_vel_pid, const double* leg_angle, + const int& leg_state, const ros::Duration& period, + double feedforward_force = 0.0f, const bool& overturn = false) { LegCommand cmd{ 0.0, 0.0, { 0.0, 0.0 } }; cmd.force = length_pid.computeCommand(desired_length - leg_pos[0], period) + feedforward_force; @@ -89,10 +92,10 @@ inline LegCommand computePidLegCommand(double desired_length, double desired_ang 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) +[[maybe_unused]] 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; @@ -101,10 +104,11 @@ inline LegCommand computePidAngleVelLegCommand(double desired_length, double des 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) +[[maybe_unused]] 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; @@ -112,15 +116,16 @@ inline LegCommand computePidAngleLegCommand(double desired_length, double desire 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) +[[maybe_unused]] 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 From 867182586dae6a2a59eac8d22362606a3fb43009 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Mon, 2 Mar 2026 11:12:08 +0800 Subject: [PATCH 49/57] fix: fix include error bug. --- .../rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h index 4b0ad58b..ee7deef5 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h @@ -1,6 +1,8 @@ // // Created by wk on 2026/2/25. // +#pragma once + #include namespace rm_chassis_controllers From 851f55b0a48b558a6024701362770dae7293ec4a Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Mon, 2 Mar 2026 15:50:42 +0800 Subject: [PATCH 50/57] Add VMC class for stand_up mode. --- .../controller_mode/stand_up.h | 20 ++++++++++++++++++ .../controller_mode/stand_up.cpp | 21 +++++++++++++++++++ 2 files changed, 41 insertions(+) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h index 1fe4aa61..837fadb8 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h @@ -9,6 +9,7 @@ #include "bipedal_wheel_controller/controller_mode/mode_base.h" #include "bipedal_wheel_controller/definitions.h" +#include "bipedal_wheel_controller/vmc/VMC.h" namespace rm_chassis_controllers { @@ -33,11 +34,30 @@ class StandUp : public ModeBase * @param leg_state */ void detectLegState(const Eigen::Matrix& x, int& leg_state); + /** + * Compute the leg command using PID controllers + * @param desired_length + * @param desired_angle + * @param current_length + * @param current_angle + * @param length_pid + * @param angle_pid + * @param leg_angle + * @param period + * @param feedforward_force + * @return + */ + inline LegCommand computePidLegCommand(double desired_length, double desired_angle, double leg_pos[2], + double leg_spd[2], control_toolbox::Pid& length_pid, + control_toolbox::Pid& angle_pid, control_toolbox::Pid& angle_vel_pid, + const double* leg_angle, const int& leg_state, const ros::Duration& period, + double feedforward_force = 0.0f); std::vector joint_handles_; std::vector 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 leg_state_threshold_; + VMCPtr vmcPtr_; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp index f921e412..fed3c8a7 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp @@ -23,6 +23,7 @@ void StandUp::execute(BipedalController* controller, const ros::Time& time, cons controller->setStateChange(true); controller->setCompleteStand(false); leg_state_threshold_ = controller->getLegThresholdParams(); + vmcPtr_ = controller->getVMCPtr(); StandUp::detectLegState(x_left_, left_leg_state); StandUp::detectLegState(x_right_, right_leg_state); } @@ -119,4 +120,24 @@ inline void StandUp::detectLegState(const Eigen::Matrix& x break; } } + +inline LegCommand StandUp::computePidLegCommand(double desired_length, double desired_angle, double leg_pos[2], + double leg_spd[2], control_toolbox::Pid& length_pid, + control_toolbox::Pid& angle_pid, control_toolbox::Pid& angle_vel_pid, + const double* leg_angle, const int& leg_state, + const ros::Duration& period, double feedforward_force) +{ + LegCommand cmd{ 0.0, 0.0, { 0.0, 0.0 } }; + cmd.force = length_pid.computeCommand(desired_length - leg_pos[0], period) + feedforward_force; + 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(-5 - leg_spd[1], period); + } + vmcPtr_->leg_conv(cmd.force, cmd.torque, leg_angle[0], leg_angle[1], cmd.input); + return cmd; +} } // namespace rm_chassis_controllers From 983230f9f33f490d88259bdc9a22ef5e955db80d Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Mon, 2 Mar 2026 15:51:00 +0800 Subject: [PATCH 51/57] Optimize code. --- .../bipedal_wheel_controller/controller.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h index 8d8305e8..ef134836 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h @@ -64,7 +64,7 @@ class BipedalController : public ChassisBase left_ref, Eigen::Matrix right_ref, Eigen::Matrix u_left, Eigen::Matrix u_right, Eigen::Matrix F_leg_, const bool unstick[2]) const; -void pubLegLenStatus(const bool& upstair_flag); + void pubLegLenStatus(const bool& upstair_flag); // clang-format on void clearStatus(); From 1b51fa2274b8e90537eacb6bc3a411e52d613fcf Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Mon, 2 Mar 2026 15:51:11 +0800 Subject: [PATCH 52/57] Optimize legged_chassis_mode_msg pub. --- .../src/bipedal_wheel_controller/controller.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp index 28e6cf03..65962e67 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp @@ -249,6 +249,7 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat rm_msgs::LeggedChassisMode legged_chassis_mode_msg; legged_chassis_mode_msg.mode = balance_mode_; + legged_chassis_mode_msg.mode_name = mode_manager_->getModeImpl()->name(); legged_chassis_mode_pub_.publish(legged_chassis_mode_msg); mode_manager_->getModeImpl()->updateEstimation(x_left_, x_right_); From 7d8c49e79f8decd08d48e371dd5f75ce9b34866b Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Mon, 9 Mar 2026 16:11:33 +0000 Subject: [PATCH 53/57] FIx powerlimit bugs. --- rm_chassis_controllers/cfg/PowerLimit.cfg | 6 +-- .../rm_chassis_controllers/chassis_base.h | 10 +---- rm_chassis_controllers/src/chassis_base.cpp | 45 ++++++------------- 3 files changed, 18 insertions(+), 43 deletions(-) diff --git a/rm_chassis_controllers/cfg/PowerLimit.cfg b/rm_chassis_controllers/cfg/PowerLimit.cfg index 72aec5d4..a9611774 100644 --- a/rm_chassis_controllers/cfg/PowerLimit.cfg +++ b/rm_chassis_controllers/cfg/PowerLimit.cfg @@ -5,8 +5,8 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("velocity_coeff", double_t, 0, "velocity coefficient for power limit", 0.0, 0.0, 100.0) -gen.add("effort_coeff", double_t, 0, "effort coefficient for power limit", 0.0, 0.0, 100.0) -gen.add("power_offset", double_t, 0, "power offset for power limit", 0.0, 0.0, 100.0) +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")) \ No newline at end of file diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h index 68e58d79..93587dca 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -52,12 +52,6 @@ namespace rm_chassis_controllers { -struct PowerLimitParams -{ - double velocity_coeff{}; - double effort_coeff{}; - double power_offset{}; -}; struct Command { geometry_msgs::Twist cmd_vel_; @@ -181,9 +175,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController control_toolbox::Pid pid_follow_; dynamic_reconfigure::Server* power_limit_srv_{}; - realtime_tools::RealtimeBuffer power_limit_rt_buffer_; - PowerLimitParams power_config_{}; - bool power_limit_reconfig_initialized_{ false }; + realtime_tools::RealtimeBuffer power_limit_rt_buffer_; std::shared_ptr > odom_pub_; rm_common::TfRtBroadcaster tf_broadcaster_{}; ros::Subscriber outside_odom_sub_; diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 48acde4c..1e744ebd 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -118,12 +118,8 @@ bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan return false; // dynamic reconfigure - power_config_.power_offset = power_offset_; - power_config_.effort_coeff = effort_coeff_; - power_config_.velocity_coeff = velocity_coeff_; - power_limit_rt_buffer_.initRT(power_config_); power_limit_srv_ = new dynamic_reconfigure::Server( - ros::NodeHandle(controller_nh, "power_limit")); + ros::NodeHandle(controller_nh, "power")); dynamic_reconfigure::Server::CallbackType cb = boost::bind(&ChassisBase::powerLimitReconfigCB, this, _1, _2); power_limit_srv_->setCallback(cb); @@ -392,36 +388,16 @@ void ChassisBase::recovery() ramp_w_->clear(vel_cmd_.z); } -template -void ChassisBase::powerLimitReconfigCB(rm_chassis_controllers::PowerLimitConfig& config, uint32_t /*level*/) -{ - ROS_INFO("[Power Limit] Dynamic params change"); - if (!power_limit_reconfig_initialized_) - { - PowerLimitParams init_config = *power_limit_rt_buffer_.readFromNonRT(); - config.velocity_coeff = init_config.velocity_coeff; - config.effort_coeff = init_config.effort_coeff; - config.power_offset = init_config.power_offset; - power_limit_reconfig_initialized_ = true; - return; - } - - PowerLimitParams config_non_rt; - config_non_rt.velocity_coeff = config.velocity_coeff; - config_non_rt.effort_coeff = config.effort_coeff; - config_non_rt.power_offset = config.power_offset; - power_limit_rt_buffer_.writeFromNonRT(config_non_rt); -} - template void ChassisBase::powerLimit() { double power_limit = cmd_rt_buffer_.readFromRT()->cmd_chassis_.power_limit; - power_config_ = *power_limit_rt_buffer_.readFromRT(); + const auto& power_config = *power_limit_rt_buffer_.readFromRT(); + + double vel_coeff = power_config.vel_coeff; + double effort_coeff = power_config.effort_coeff; + double power_offset = power_config.power_offset; - double velocity_coeff = power_config_.velocity_coeff; - double effort_coeff = power_config_.effort_coeff; - double power_offset = power_config_.power_offset; // Three coefficients of a quadratic equation in one variable double a = 0., b = 0., c = 0.; for (const auto& joint : joint_handles_) @@ -436,7 +412,7 @@ void ChassisBase::powerLimit() } } a *= effort_coeff; - c = c * velocity_coeff - power_offset - power_limit; + c = c * vel_coeff - power_offset - power_limit; // Root formula for quadratic equation in one variable double zoom_coeff = (square(b) - 4 * a * c) > 0 ? ((-b + sqrt(square(b) - 4 * a * c)) / (2 * a)) : 0.; for (auto joint : joint_handles_) @@ -495,4 +471,11 @@ void ChassisBase::outsideOdomCallback(const nav_msgs::Odometry::ConstPtr& topic_update_ = true; } +template +void ChassisBase::powerLimitReconfigCB(rm_chassis_controllers::PowerLimitConfig& config, uint32_t /*level*/) +{ + ROS_INFO("[Power Limit] Dynamic params change"); + power_limit_rt_buffer_.writeFromNonRT(config); +} + } // namespace rm_chassis_controllers From 74a4e0c7d32700bc951607da50b593d0889f29d1 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Mon, 9 Mar 2026 16:16:21 +0000 Subject: [PATCH 54/57] FIx bugs. --- rm_chassis_controllers/CMakeLists.txt | 2 +- rm_chassis_controllers/cfg/LQRWeight.cfg | 2 +- rm_chassis_controllers/cfg/PowerLimit.cfg | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt index 9473d39b..678d931b 100644 --- a/rm_chassis_controllers/CMakeLists.txt +++ b/rm_chassis_controllers/CMakeLists.txt @@ -28,7 +28,7 @@ find_package(Eigen3 REQUIRED) generate_dynamic_reconfigure_options( cfg/LQRWeight.cfg - cfg/PowerLimit.cfg + cfg/PowerLimit.cfg ) ################################### diff --git a/rm_chassis_controllers/cfg/LQRWeight.cfg b/rm_chassis_controllers/cfg/LQRWeight.cfg index ddc3bec3..ab970f46 100644 --- a/rm_chassis_controllers/cfg/LQRWeight.cfg +++ b/rm_chassis_controllers/cfg/LQRWeight.cfg @@ -15,4 +15,4 @@ 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")) \ No newline at end of file +exit(gen.generate(PACKAGE, "chassis_controller", "LQRWeight")) diff --git a/rm_chassis_controllers/cfg/PowerLimit.cfg b/rm_chassis_controllers/cfg/PowerLimit.cfg index a9611774..52247c4d 100644 --- a/rm_chassis_controllers/cfg/PowerLimit.cfg +++ b/rm_chassis_controllers/cfg/PowerLimit.cfg @@ -9,4 +9,4 @@ gen.add("vel_coeff", double_t, 0, "velocity coefficient for power limit", 0.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")) \ No newline at end of file +exit(gen.generate(PACKAGE, "chassis_controller", "PowerLimit")) From d4c240498bef505239f7967e6fe5263510c47f20 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Wed, 11 Mar 2026 21:33:17 +0800 Subject: [PATCH 55/57] Optimize stand_up. --- .../controller_mode/stand_up.h | 3 +- .../controller_mode/stand_up.cpp | 50 ++++++++++--------- 2 files changed, 29 insertions(+), 24 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h index 837fadb8..446e4a92 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h @@ -27,7 +27,7 @@ class StandUp : public ModeBase private: void setUpLegMotion(const Eigen::Matrix& x, const int& other_leg_state, const double& leg_length, const double& leg_theta, int& leg_state, double& theta_des, - double& length_des); + double& length_des, bool& stop_flag); /** * Detect the leg state before stand up: UNDER, FRONT, BEHIND * @param x @@ -57,6 +57,7 @@ class StandUp : public ModeBase int left_leg_state, right_leg_state; double theta_des_l, theta_des_r, length_des_l, length_des_r; double spring_force_{}; + bool left_stop_{ false }, right_stop_{ false }; std::shared_ptr leg_state_threshold_; VMCPtr vmcPtr_; }; diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp index fed3c8a7..fc361d1b 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp @@ -31,20 +31,30 @@ void StandUp::execute(BipedalController* controller, const ros::Time& time, cons auto model_params_ = controller->getModelParams(); spring_force_ = -model_params_->f_spring; LegCommand left_cmd = { 0, 0, { 0., 0. } }, right_cmd = { 0, 0, { 0., 0. } }; - setUpLegMotion(x_left_, right_leg_state, left_pos_[0], left_pos_[1], left_leg_state, theta_des_l, length_des_l); - setUpLegMotion(x_right_, left_leg_state, right_pos_[0], right_pos_[1], right_leg_state, theta_des_r, length_des_r); - left_cmd = computePidLegCommand(length_des_l, theta_des_l, left_pos_, left_spd_, *pid_legs_[0], *pid_thetas_[0], - *pid_thetas_[2], left_angle_, left_leg_state, period, spring_force_); - right_cmd = computePidLegCommand(length_des_r, theta_des_r, right_pos_, right_spd_, *pid_legs_[1], *pid_thetas_[1], - *pid_thetas_[3], right_angle_, right_leg_state, period, spring_force_); + setUpLegMotion(x_left_, right_leg_state, left_pos_[0], left_pos_[1], left_leg_state, theta_des_l, length_des_l, + left_stop_); + setUpLegMotion(x_right_, left_leg_state, right_pos_[0], right_pos_[1], right_leg_state, theta_des_r, length_des_r, + right_stop_); + if (!left_stop_) + { + left_cmd = computePidLegCommand(length_des_l, theta_des_l, left_pos_, left_spd_, *pid_legs_[0], *pid_thetas_[0], + *pid_thetas_[2], left_angle_, left_leg_state, period, spring_force_); + } + if (!right_stop_) + { + right_cmd = computePidLegCommand(length_des_r, theta_des_r, right_pos_, right_spd_, *pid_legs_[1], *pid_thetas_[1], + *pid_thetas_[3], right_angle_, right_leg_state, period, spring_force_); + } setJointCommands(joint_handles_, left_cmd, right_cmd); // Exit - if (((left_pos_[1] < 0.3 && left_leg_state == LegState::BEHIND) || - (left_pos_[1] > -0.4 && left_leg_state == LegState::UNDER)) && - ((right_pos_[1] < 0.3 && right_leg_state == LegState::BEHIND) || - (right_pos_[1] > -0.4 && right_leg_state == LegState::UNDER))) + // if (((left_pos_[1] < 0.3 && left_leg_state == LegState::BEHIND) || + // (left_pos_[1] > -0.3 && left_leg_state == LegState::UNDER)) && + // ((right_pos_[1] < 0.3 && right_leg_state == LegState::BEHIND) || + // (right_pos_[1] > -0.3 && right_leg_state == LegState::UNDER))) + if (((left_pos_[1] < 0.3 && left_leg_state == LegState::BEHIND)) && + ((right_pos_[1] < 0.3 && right_leg_state == LegState::BEHIND))) { controller->setMode(BalanceMode::NORMAL); controller->setStateChange(false); @@ -54,24 +64,16 @@ void StandUp::execute(BipedalController* controller, const ros::Time& time, cons void StandUp::setUpLegMotion(const Eigen::Matrix& x, const int& other_leg_state, const double& leg_length, const double& leg_theta, int& leg_state, double& theta_des, - double& length_des) + double& length_des, bool& stop_flag) { switch (leg_state) { case LegState::UNDER: - if (spring_force_ > 0) - { - theta_des = M_PI / 2 - 0.35; - length_des = 0.36; - if (leg_length > 0.35) - { - leg_state = LegState::FRONT; - } - } - else + theta_des = -M_PI_2; + length_des = 0.36; + if (leg_length > 0.35) { - theta_des = 0.0; - length_des = 0.18; + leg_state = LegState::FRONT; } break; case LegState::FRONT: @@ -81,10 +83,12 @@ void StandUp::setUpLegMotion(const Eigen::Matrix& x, const leg_state = LegState::BEHIND; break; case LegState::BEHIND: + stop_flag = true; theta_des = leg_theta; length_des = leg_length; if (other_leg_state != LegState::FRONT) { + stop_flag = false; length_des = 0.18; if (leg_length < 0.3) theta_des = 0.0; From 37c9f22db883c78777cc6cc1dc4416e02805ed19 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Wed, 11 Mar 2026 21:33:49 +0800 Subject: [PATCH 56/57] Add F_leg output limit in normal mode. --- .../controller_mode/normal.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp index eab19b09..234954fa 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -112,10 +112,12 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const { double left_length_des = controller->getCompleteStand() ? leg_length_des : controller->getDefaultLegLength(); double right_length_des = controller->getCompleteStand() ? leg_length_des : controller->getDefaultLegLength(); - F_leg[LEFT] = pid_legs_[LEFT]->computeCommand(left_length_des - current_leg_length, period) - F_inertia + - gravity * cos(left_pos_[1]) + F_roll - spring_force; - F_leg[RIGHT] = pid_legs_[RIGHT]->computeCommand(right_length_des - current_leg_length, period) + F_inertia + - gravity * cos(right_pos_[1]) - F_roll - spring_force; + double F_pid_left = pid_legs_[LEFT]->computeCommand(left_length_des - current_leg_length, period); + double F_pid_right = pid_legs_[RIGHT]->computeCommand(right_length_des - current_leg_length, period); + F_pid_left = abs(F_pid_left) > 90 ? std::copysign(1, F_pid_left) * 90 : F_pid_left; + F_pid_right = abs(F_pid_right) > 90 ? std::copysign(1, F_pid_right) * 90 : F_pid_right; + F_leg[LEFT] = F_pid_left - F_inertia + gravity * cos(left_pos_[1]) + F_roll - spring_force; + F_leg[RIGHT] = F_pid_right + F_inertia + gravity * cos(right_pos_[1]) - F_roll - spring_force; } else { @@ -125,6 +127,7 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const switch (jump_phase_) { case JumpPhase::LEG_RETRACTION: + { ROS_INFO("[balance] ENTER LEG_RETRACTION"); F_leg(LEFT) = pid_legs_[LEFT]->computeCommand(leg_length_des - current_leg_length, period) + gravity * cos(left_pos_[1]) + F_roll - spring_force; @@ -140,6 +143,7 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const jump_phase_ = JumpPhase::JUMP_UP; } break; + } case JumpPhase::JUMP_UP: ROS_INFO("[balance] ENTER JUMP_UP"); // F_leg(0) = control_params_->p1_ * pow(left_pos_[0], 3) + control_params_->p2_ * pow(left_pos_[0], 2) + @@ -229,7 +233,7 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const // upstairs if (jump_phase_ == JumpPhase::IDLE && linear_acc_base_.z < -7.0 && controller->getCompleteStand() && - abs(vel_cmd_.x) > 0.1 && abs(x_left(3)) > 0.1 && ((left_pos_[0] + right_pos_[0]) / 2.0f) > 0.30 && + abs(vel_cmd_.x) > 0.1 && abs(x_left(3)) > 0.1 && ((left_pos_[0] + right_pos_[0]) / 2.0f) > 0.3 && leg_length_des > 0.30) { leg_length_des = controller->getDefaultLegLength(); @@ -241,7 +245,7 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const } // Protection - if (abs(x_left(4)) > 0.6 || abs(x_left(0)) > 1.0 || abs(x_right(0)) > 1.0 || abs(roll_) > 1.0 || + if (abs(x_left(4)) > 0.6 || abs(x_left(0)) > 0.7 || abs(x_right(0)) > 0.7 || abs(roll_) > 1.0 || controller->getOverturn() || controller->getBaseState() == 4) { leg_length_des = controller->getDefaultLegLength(); From a9cadcfe687cd6b5d0784aa46c5173d5ce0af3d8 Mon Sep 17 00:00:00 2001 From: wiselook <1656438881@qq.com> Date: Sun, 15 Mar 2026 21:36:19 +0800 Subject: [PATCH 57/57] Update series_legged controller. --- .../bipedal_wheel_controller/controller.h | 6 +- .../controller_mode/recover.h | 4 +- .../controller_mode/upstairs.h | 8 + .../bipedal_wheel_controller/dynamics/gen_A.h | 3 +- .../bipedal_wheel_controller/dynamics/gen_B.h | 3 +- .../helper_functions.h | 8 + .../bipedal_wheel_controller/controller.cpp | 120 +++++++------ .../controller_mode/normal.cpp | 9 +- .../controller_mode/recover.cpp | 10 +- .../controller_mode/sit_down.cpp | 2 +- .../controller_mode/stand_up.cpp | 5 +- .../controller_mode/upstairs.cpp | 28 ++- .../bipedal_wheel_controller/dynamics/gen_A.c | 169 +++++++++++++++++- .../bipedal_wheel_controller/dynamics/gen_B.c | 124 +++++++++++++ .../series_legged_vmc_controller.cpp | 8 +- .../test/vmc_controller.yaml | 4 +- 16 files changed, 434 insertions(+), 77 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h index ef134836..2df41272 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h @@ -69,6 +69,7 @@ class BipedalController : public ChassisBase left_error, Eigen::Matrix right_error, Eigen::Matrix left_ref, Eigen::Matrix right_ref, @@ -140,8 +141,9 @@ class BipedalController : public ChassisBase> legged_chassis_status_pub_; + std::shared_ptr> legged_chassis_mode_pub_; + std::shared_ptr> lqr_status_pub_; ros::Time cmd_update_time_; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h index ca21df3a..adffde63 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h @@ -50,8 +50,8 @@ class Recover : public ModeBase std::vector joint_handles_; std::vector pid_legs_, pid_thetas_; 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 }; + double leg_recovery_velocity_{ 5.0 }, threshold_{ 0.05 }, leg_theta_diff_{ 0.0 }, desired_leg_length_{ 0.38 }; + const double leg_recovery_velocity_const_{ 5.0 }; RecoveryChassisState recovery_chassis_state_{ ForwardSlip }; bool detectd_flag{ false }; }; diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/upstairs.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/upstairs.h index 4ec6f475..9ad26052 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/upstairs.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/upstairs.h @@ -10,6 +10,7 @@ #include "bipedal_wheel_controller/controller_mode/mode_base.h" #include "bipedal_wheel_controller/definitions.h" +#include "bipedal_wheel_controller/vmc/VMC.h" namespace rm_chassis_controllers { @@ -31,10 +32,17 @@ class Upstairs : public ModeBase * @param leg_state */ void detectLegState(const Eigen::Matrix& x, int& leg_state); + inline LegCommand computePidLegCommand(double desired_length, double desired_angle, double leg_pos[2], + double leg_spd[2], control_toolbox::Pid& length_pid, + control_toolbox::Pid& angle_pid, control_toolbox::Pid& angle_vel_pid, + const double* leg_angle, const int& leg_state, const ros::Duration& period, + double feedforward_force); std::vector joint_handles_; std::vector pid_legs_, pid_thetas_; + double spring_force_{}; int left_leg_state, right_leg_state; std::shared_ptr leg_state_threshold_; + VMCPtr vmcPtr_; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/dynamics/gen_A.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/dynamics/gen_A.h index 6ee48d72..4879661d 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/dynamics/gen_A.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/dynamics/gen_A.h @@ -19,7 +19,8 @@ extern "C" { /* Function Declarations */ extern void gen_A(double Im, double Ip, double Iw, double L, double Lm, double M, double R, double g, double l, double mp, double mw, double A[36]); - +extern void gen_A_leg_offset(double Im, double Ip, double Iw, double L, double Lm, double M, double R, double g, + double l, double mp, double mw, double theta_l0, double A[36]); #ifdef __cplusplus } #endif diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/dynamics/gen_B.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/dynamics/gen_B.h index f3491aa6..f698871f 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/dynamics/gen_B.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/dynamics/gen_B.h @@ -19,7 +19,8 @@ extern "C" { /* Function Declarations */ extern void gen_B(double Im, double Ip, double Iw, double L, double Lm, double M, double R, double l, double mp, double mw, double B[12]); - +extern void gen_B_leg_offset(double Im, double Ip, double Iw, double L, double Lm, double M, double R, double g, + double l, double mp, double mw, double theta_l0, double B[12]); #ifdef __cplusplus } #endif diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h index 1303446d..dc800130 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h @@ -33,10 +33,18 @@ inline void generateAB(const std::shared_ptr& model_params, Eigen:: double A[36] = { 0. }, B[12]{ 0. }; double L = leg_length * model_params->L_weight; double Lm = leg_length * model_params->Lm_weight; + // auto theta_from_length = [](double L) -> double { + // return -23.36693691 * L * L * L + 24.76241959 * L * L - 11.65313741 * L + 2.49258628; + // }; + // double theta_L = theta_from_length(leg_length); gen_A(model_params->i_m, model_params->i_p, model_params->i_w, L, Lm, model_params->M, model_params->r, model_params->g, model_params->l, model_params->m_p, model_params->m_w, A); gen_B(model_params->i_m, model_params->i_p, model_params->i_w, L, Lm, model_params->M, model_params->r, model_params->l, model_params->m_p, model_params->m_w, B); + // gen_A_leg_offset(model_params->i_m, model_params->i_p, model_params->i_w, L, Lm, model_params->M, model_params->r, + // model_params->g, model_params->l, model_params->m_p, model_params->m_w, theta_L, A); + // gen_B_leg_offset(model_params->i_m, model_params->i_p, model_params->i_w, L, Lm, model_params->M, model_params->r, + // model_params->g, model_params->l, model_params->m_p, model_params->m_w, theta_L, B); // clang-format off a<< 0. ,1.,0.,0.,0. ,0., diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp index 65962e67..38fb571e 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp @@ -56,13 +56,19 @@ bool BipedalController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan legCmd_ = msg->leg_length; jumpCmd_ = msg->jump; }; - leg_cmd_sub_ = controller_nh.subscribe("/leg_cmd", 1, legCmdCallback); + leg_cmd_sub_ = controller_nh.subscribe("/leg_cmd", 10, legCmdCallback); unstick_pub_ = controller_nh.advertise("unstick", 1); upstair_status_pub_ = controller_nh.advertise("upstair_status", 1); - legged_chassis_status_pub_ = controller_nh.advertise("legged_chassis_status", 1); - legged_chassis_mode_pub_ = controller_nh.advertise("legged_chassis_mode", 1); - lqr_status_pub_ = controller_nh.advertise("lqr_status", 1); + legged_chassis_status_pub_.reset((new realtime_tools::RealtimePublisher( + controller_nh, "legged_chassis_status", 100))); + legged_chassis_mode_pub_.reset( + (new realtime_tools::RealtimePublisher(controller_nh, "legged_chassis_mode", 10))); + lqr_status_pub_.reset( + (new realtime_tools::RealtimePublisher(controller_nh, "lqr_status", 100))); + // legged_chassis_status_pub_ = controller_nh.advertise("legged_chassis_status", 1); + // legged_chassis_mode_pub_ = controller_nh.advertise("legged_chassis_mode", 1); + // lqr_status_pub_ = controller_nh.advertise("lqr_status", 1); x_left_.setZero(); x_right_.setZero(); @@ -93,11 +99,17 @@ void BipedalController::moveJoint(const ros::Time& time, const ros::Duration& pe { if (!balance_state_changed_) mode_manager_->switchMode(balance_mode_); - if (getBaseState() == 4) + if (getBaseState() == rm_msgs::ChassisCmd::FALLEN) { balance_mode_ = SIT_DOWN; mode_manager_->switchMode(SIT_DOWN); } + else if (!overturn_ && getBaseState() == rm_msgs::ChassisCmd::RECOVERY) + { + overturn_ = true; + balance_mode_ = RECOVER; + mode_manager_->switchMode(RECOVER); + } updateEstimation(time, period); mode_manager_->getModeImpl()->execute(this, time, period); pubState(); @@ -155,16 +167,16 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat // vmc double left_angle[2]{}, right_angle[2]{}, left_pos[2]{}, left_spd[2]{}, right_pos[2]{}, right_spd[2]{}; // [0]:hip_vmc_joint [1]:knee_vmc_joint - // left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; - // left_angle[1] = left_knee_joint_handle_.getPosition(); - // right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; - // right_angle[1] = right_knee_joint_handle_.getPosition(); + left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; + left_angle[1] = left_knee_joint_handle_.getPosition(); + right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; + right_angle[1] = right_knee_joint_handle_.getPosition(); // gazebo - left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; - left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; - right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; - right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; + // left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; + // left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; + // right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; + // right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; // [0] is length, [1] is angle vmc_->leg_pos(left_angle[0], left_angle[1], left_pos); @@ -227,30 +239,33 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat x_right_[0] = (right_pos[1] + pitch); x_right_[1] = right_spd[1] + angular_vel_base.y; - // ros msg - rm_msgs::LeggedChassisStatus legged_chassis_status_msg; - legged_chassis_status_msg.roll = roll; - legged_chassis_status_msg.pitch = x_left_[4]; - legged_chassis_status_msg.d_pitch = x_left_[5]; - legged_chassis_status_msg.yaw = yaw; - legged_chassis_status_msg.d_yaw = angular_vel_base.z; - legged_chassis_status_msg.left_leg_length = left_pos[0]; - legged_chassis_status_msg.right_leg_length = right_pos[0]; - legged_chassis_status_msg.x = x_left_[2]; - legged_chassis_status_msg.x_dot = x_left_[3]; - legged_chassis_status_msg.left_leg_theta = x_left_[0]; - legged_chassis_status_msg.left_leg_theta_dot = x_left_[1]; - legged_chassis_status_msg.right_leg_theta = x_right_[0]; - legged_chassis_status_msg.right_leg_theta_dot = x_right_[1]; - legged_chassis_status_msg.linear_acc_base.push_back(linear_acc_base.x); - legged_chassis_status_msg.linear_acc_base.push_back(linear_acc_base.y); - legged_chassis_status_msg.linear_acc_base.push_back(linear_acc_base.z); - legged_chassis_status_pub_.publish(legged_chassis_status_msg); - - rm_msgs::LeggedChassisMode legged_chassis_mode_msg; - legged_chassis_mode_msg.mode = balance_mode_; - legged_chassis_mode_msg.mode_name = mode_manager_->getModeImpl()->name(); - legged_chassis_mode_pub_.publish(legged_chassis_mode_msg); + if (legged_chassis_status_pub_->trylock()) + { + legged_chassis_status_pub_->msg_.roll = roll; + legged_chassis_status_pub_->msg_.pitch = x_left_[4]; + legged_chassis_status_pub_->msg_.d_pitch = x_left_[5]; + legged_chassis_status_pub_->msg_.yaw = yaw; + legged_chassis_status_pub_->msg_.d_yaw = angular_vel_base.z; + legged_chassis_status_pub_->msg_.left_leg_length = left_pos[0]; + legged_chassis_status_pub_->msg_.right_leg_length = right_pos[0]; + legged_chassis_status_pub_->msg_.x = x_left_[2]; + legged_chassis_status_pub_->msg_.x_dot = x_left_[3]; + legged_chassis_status_pub_->msg_.left_leg_theta = x_left_[0]; + legged_chassis_status_pub_->msg_.left_leg_theta_dot = x_left_[1]; + legged_chassis_status_pub_->msg_.right_leg_theta = x_right_[0]; + legged_chassis_status_pub_->msg_.right_leg_theta_dot = x_right_[1]; + legged_chassis_status_pub_->msg_.linear_acc_base.push_back(linear_acc_base.x); + legged_chassis_status_pub_->msg_.linear_acc_base.push_back(linear_acc_base.y); + legged_chassis_status_pub_->msg_.linear_acc_base.push_back(linear_acc_base.z); + legged_chassis_status_pub_->unlockAndPublish(); + } + + if (legged_chassis_mode_pub_->trylock()) + { + legged_chassis_mode_pub_->msg_.mode = balance_mode_; + legged_chassis_mode_pub_->msg_.mode_name = mode_manager_->getModeImpl()->name(); + legged_chassis_mode_pub_->unlockAndPublish(); + } mode_manager_->getModeImpl()->updateEstimation(x_left_, x_right_); mode_manager_->getModeImpl()->updateLegKinematics(left_angle, right_angle, left_pos, left_spd, right_pos, right_spd); @@ -452,22 +467,25 @@ void BipedalController::pubLQRStatus(Eigen::Matrix left_er Eigen::Matrix u_right, Eigen::Matrix F_leg_, const bool unstick[2]) const { - rm_msgs::LeggedLQRStatus msg; - for (int i = 0; i < 6; ++i) + if (lqr_status_pub_->trylock()) { - msg.left_leg_error.push_back(left_error(i)); - msg.right_leg_error.push_back(right_error(i)); - msg.left_leg_ref.push_back(left_ref(i)); - msg.right_leg_ref.push_back(right_ref(i)); - } - for (int i = 0; i < 2; ++i) - { - msg.left_leg_u.push_back(u_left(i)); - msg.right_leg_u.push_back(u_right(i)); - msg.F_leg.push_back(F_leg_[i]); - msg.unstick.push_back(unstick[i]); + int temp{}; + for (temp = 0; temp < 6; ++temp) + { + lqr_status_pub_->msg_.left_leg_error.push_back(left_error(temp)); + lqr_status_pub_->msg_.right_leg_error.push_back(right_error(temp)); + lqr_status_pub_->msg_.left_leg_ref.push_back(left_ref(temp)); + lqr_status_pub_->msg_.right_leg_ref.push_back(right_ref(temp)); + } + for (temp = 0; temp < 2; ++temp) + { + lqr_status_pub_->msg_.left_leg_u.push_back(u_left(temp)); + lqr_status_pub_->msg_.right_leg_u.push_back(u_right(temp)); + lqr_status_pub_->msg_.F_leg.push_back(F_leg_[temp]); + lqr_status_pub_->msg_.unstick.push_back(unstick[temp]); + } + lqr_status_pub_->unlockAndPublish(); } - lqr_status_pub_.publish(msg); } void BipedalController::pubLegLenStatus(const bool& upstair_flag) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp index 234954fa..66577b29 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -96,6 +96,7 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const // Compute leg thrust auto model_params_ = controller->getModelParams(); auto control_params_ = controller->getControlParams(); + // auto f_spring_force = [](double l) { return ((2094.45f * l - 3091.28f) * l + 1408.375f) * l - 80.91f; }; double gravity = model_params_->f_gravity, current_leg_length = (left_pos_[0] + right_pos_[0]) / 2.0f, spring_force = model_params_->f_spring; double F_inertia = model_params_->M * friction_circle; @@ -114,8 +115,8 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const double right_length_des = controller->getCompleteStand() ? leg_length_des : controller->getDefaultLegLength(); double F_pid_left = pid_legs_[LEFT]->computeCommand(left_length_des - current_leg_length, period); double F_pid_right = pid_legs_[RIGHT]->computeCommand(right_length_des - current_leg_length, period); - F_pid_left = abs(F_pid_left) > 90 ? std::copysign(1, F_pid_left) * 90 : F_pid_left; - F_pid_right = abs(F_pid_right) > 90 ? std::copysign(1, F_pid_right) * 90 : F_pid_right; + F_pid_left = abs(F_pid_left) > 150 ? std::copysign(1, F_pid_left) * 150 : F_pid_left; + F_pid_right = abs(F_pid_right) > 150 ? std::copysign(1, F_pid_right) * 150 : F_pid_right; F_leg[LEFT] = F_pid_left - F_inertia + gravity * cos(left_pos_[1]) + F_roll - spring_force; F_leg[RIGHT] = F_pid_right + F_inertia + gravity * cos(right_pos_[1]) - F_roll - spring_force; } @@ -210,7 +211,7 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const controller->pubLQRStatus(-x_left, -x_right, x_left_ref, x_right_ref, u_left, u_right, F_N, unstick); updateUnstick(left_unstick, right_unstick); - + left_unstick = right_unstick = false; if (controller->getCompleteStand() && left_unstick && jump_phase_ != JumpPhase::LEG_RETRACTION) { F_leg[LEFT] -= F_roll; @@ -245,7 +246,7 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const } // Protection - if (abs(x_left(4)) > 0.6 || abs(x_left(0)) > 0.7 || abs(x_right(0)) > 0.7 || abs(roll_) > 1.0 || + if (abs(x_left(4)) > 0.6 || abs(x_left(0)) > 0.9 || abs(x_right(0)) > 0.9 || abs(roll_) > 1.0 || controller->getOverturn() || controller->getBaseState() == 4) { leg_length_des = controller->getDefaultLegLength(); diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp index fe3fe441..8a9b9be0 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp @@ -33,7 +33,8 @@ void Recover::execute(BipedalController* controller, const ros::Time& time, cons } LegCommand left_cmd = { 0, 0, { 0., 0. } }, right_cmd = { 0, 0, { 0., 0. } }; - leg_theta_diff_ = angles::shortest_angular_distance(left_pos_[1], right_pos_[1]); + // leg_theta_diff_ = angles::shortest_angular_distance(left_pos_[1], right_pos_[1]); + leg_theta_diff_ = right_pos_[1] - left_pos_[1]; double T_theta_diff{ 0.0 }, feedforward_force{ 0.0 }; if (controller->getBaseState() != 4) { @@ -44,13 +45,13 @@ void Recover::execute(BipedalController* controller, const ros::Time& time, cons right_cmd.force = pid_legs_[1]->computeCommand(desired_leg_length_ - right_pos_[0], period) + feedforward_force; controller->getVMCPtr()->leg_conv(left_cmd.force, T_theta_diff, left_angle_[0], left_angle_[1], left_cmd.input); controller->getVMCPtr()->leg_conv(right_cmd.force, -T_theta_diff, right_angle_[0], right_angle_[1], right_cmd.input); - if (abs(leg_theta_diff_) < 0.1) + if (abs(leg_theta_diff_) < 0.3) { left_cmd.torque = pid_thetas_[2]->computeCommand(leg_recovery_velocity_ - left_spd_[1], period); right_cmd.torque = pid_thetas_[3]->computeCommand(leg_recovery_velocity_ - right_spd_[1], period); - controller->getVMCPtr()->leg_conv(left_cmd.force, 10 * leg_recovery_velocity_ + left_cmd.torque + T_theta_diff, + controller->getVMCPtr()->leg_conv(left_cmd.force, 5 * leg_recovery_velocity_ + left_cmd.torque + T_theta_diff, left_angle_[0], left_angle_[1], left_cmd.input); - controller->getVMCPtr()->leg_conv(right_cmd.force, 10 * leg_recovery_velocity_ + right_cmd.torque - T_theta_diff, + controller->getVMCPtr()->leg_conv(right_cmd.force, 5 * leg_recovery_velocity_ + right_cmd.torque - T_theta_diff, right_angle_[0], right_angle_[1], right_cmd.input); } } @@ -61,6 +62,7 @@ void Recover::execute(BipedalController* controller, const ros::Time& time, cons { controller->setMode(BalanceMode::STAND_UP); controller->setStateChange(false); + controller->clearRecoveryFlag(); ROS_INFO("[balance] Exit RECOVER"); } } diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp index e2286e9a..9d360211 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp @@ -27,7 +27,7 @@ void SitDown::execute(BipedalController* controller, const ros::Time& time, cons setJointCommands(joint_handles_, left_cmd, right_cmd, left_wheel_cmd, right_wheel_cmd); // Exit - if (abs(x_left_(1)) < 0.1 && abs(x_left_(5)) < 0.2 && controller->getBaseState() != 4) + if (abs(x_left_(1)) < 0.1 && controller->getBaseState() != 4) { if (!controller->getOverturn()) controller->setMode(BalanceMode::STAND_UP); diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp index fc361d1b..47775243 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp @@ -90,8 +90,8 @@ void StandUp::setUpLegMotion(const Eigen::Matrix& x, const { stop_flag = false; length_des = 0.18; - if (leg_length < 0.3) - theta_des = 0.0; + if (leg_length < 0.21) + theta_des = -0.1; } break; } @@ -133,6 +133,7 @@ inline LegCommand StandUp::computePidLegCommand(double desired_length, double de { LegCommand cmd{ 0.0, 0.0, { 0.0, 0.0 } }; cmd.force = length_pid.computeCommand(desired_length - leg_pos[0], period) + feedforward_force; + cmd.force = abs(cmd.force) > 250 ? std::copysign(1, cmd.force) * 250 : cmd.force; if (leg_state == LegState::BEHIND || leg_state == LegState::UNDER) { cmd.torque = angle_pid.computeCommand(-angles::shortest_angular_distance(desired_angle, leg_pos[1]), period); diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp index 2f8c3026..34d81f11 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp @@ -23,17 +23,20 @@ void Upstairs::execute(BipedalController* controller, const ros::Time& time, con controller->setStateChange(true); controller->setCompleteStand(false); leg_state_threshold_ = controller->getLegThresholdParams(); + vmcPtr_ = controller->getVMCPtr(); detectLegState(x_left_, left_leg_state); detectLegState(x_right_, right_leg_state); } double theta_des_l{ M_PI_2 - 0.6 }, theta_des_r{ M_PI_2 - 0.6 }, length_des_l{ 0.18 }, length_des_r{ 0.18 }; + auto model_params_ = controller->getModelParams(); + spring_force_ = -model_params_->f_spring; theta_des_l = theta_des_r = leg_state_threshold_->upstair_des_theta; LegCommand left_cmd = { 0, 0, { 0., 0. } }, right_cmd = { 0, 0, { 0., 0. } }; left_cmd = computePidLegCommand(length_des_l, theta_des_l, left_pos_, left_spd_, *pid_legs_[0], *pid_thetas_[0], - *pid_thetas_[2], left_angle_, left_leg_state, period); + *pid_thetas_[2], left_angle_, left_leg_state, period, spring_force_); right_cmd = computePidLegCommand(length_des_r, theta_des_r, right_pos_, right_spd_, *pid_legs_[1], *pid_thetas_[1], - *pid_thetas_[3], right_angle_, right_leg_state, period); + *pid_thetas_[3], right_angle_, right_leg_state, period, spring_force_); setJointCommands(joint_handles_, left_cmd, right_cmd); // Exit @@ -74,4 +77,25 @@ inline void Upstairs::detectLegState(const Eigen::Matrix& break; } } + +inline LegCommand Upstairs::computePidLegCommand(double desired_length, double desired_angle, double leg_pos[2], + double leg_spd[2], control_toolbox::Pid& length_pid, + control_toolbox::Pid& angle_pid, control_toolbox::Pid& angle_vel_pid, + const double* leg_angle, const int& leg_state, + const ros::Duration& period, double feedforward_force) +{ + LegCommand cmd{ 0.0, 0.0, { 0.0, 0.0 } }; + cmd.force = length_pid.computeCommand(desired_length - leg_pos[0], period) + feedforward_force; + cmd.force = abs(cmd.force) > 250 ? std::copysign(1, cmd.force) * 250 : cmd.force; + 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(-5 - leg_spd[1], period); + } + vmcPtr_->leg_conv(cmd.force, cmd.torque, leg_angle[0], leg_angle[1], cmd.input); + return cmd; +} } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_A.c b/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_A.c index 81c66e83..92bc6560 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_A.c +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_A.c @@ -7,7 +7,7 @@ /* Include Files */ #include "bipedal_wheel_controller/dynamics/gen_A.h" - +#include #include /* Function Definitions */ @@ -171,6 +171,173 @@ void gen_A(double Im, double Ip, double Iw, double L, double Lm, double M, doubl A[35] = 0.0; } +/* Function Definitions */ +/* + * gen_A_leg_offset + * A = gen_A_leg_offset(Im,Ip,Iw,L,Lm,M,R,G,l,MP,MW,THETA_L0) + * + * Arguments : double Im + * double Ip + * double Iw + * double L + * double Lm + * double M + * double R + * double g + * double l + * double mp + * double mw + * double theta_l0 + * double A[36] + * Return Type : void + */ +void gen_A_leg_offset(double Im, double Ip, double Iw, double L, double Lm, double M, double R, double g, double l, + double mp, double mw, double theta_l0, double A[36]) +{ + double b_t18_tmp; + double b_t19_tmp; + double b_t27_tmp; + double b_t28_tmp; + double b_t33_tmp; + double c_t33_tmp; + double d; + double d1; + double d2; + double d3; + double d_t33_tmp; + double e_t33_tmp; + double f_t33_tmp; + double g_t33_tmp; + double h_t33_tmp; + double i_t33_tmp; + double t18_tmp; + double t19_tmp; + double t2; + double t27_tmp; + double t28_tmp; + double t29_tmp; + double t3; + double t33; + double t33_tmp; + double t4; + double t5; + double t6; + double t7; + double t8; + /* This function was generated by the Symbolic Math Toolbox version 23.2. + */ + /* 2026-03-02 20:29:44 */ + t2 = cos(theta_l0); + t3 = L * L; + t4 = Lm * Lm; + t5 = M * M; + t6 = R * R; + t7 = l * l; + t8 = mp * mp; + t18_tmp = Iw * L; + b_t18_tmp = t18_tmp * Lm; + t19_tmp = Iw * M; + b_t19_tmp = t19_tmp * g * l * mp; + t27_tmp = L * Lm; + b_t27_tmp = t27_tmp * g * l; + t28_tmp = M * g * l; + b_t28_tmp = t28_tmp * mp * mw; + t29_tmp = L * g; + t33_tmp = Im * Iw; + b_t33_tmp = Im * Ip; + c_t33_tmp = Ip * M; + d_t33_tmp = t33_tmp * L; + e_t33_tmp = Im * L; + f_t33_tmp = e_t33_tmp * Lm; + g_t33_tmp = Im * M; + h_t33_tmp = f_t33_tmp * M; + i_t33_tmp = Ip * Iw * M; + t33 = + 1.0 / + ((((((((((((((b_t33_tmp * Iw + d_t33_tmp * Lm * M) + i_t33_tmp * t7) + t33_tmp * mp * t3) + b_t33_tmp * mp * t6) + + b_t33_tmp * mw * t6) + + t33_tmp * M * t3) + + b_t33_tmp * M * t6) + + h_t33_tmp * mw * t6) + + g_t33_tmp * mw * t3 * t6) + + t19_tmp * mp * t3 * t7) + + c_t33_tmp * mp * t6 * t7) + + c_t33_tmp * mw * t6 * t7) + + Im * mp * mw * t3 * t6) + + M * mp * mw * t3 * t6 * t7); + A[0] = 0.0; + d = e_t33_tmp * g; + t19_tmp = e_t33_tmp * M * g; + b_t33_tmp = Im * Lm; + e_t33_tmp = b_t33_tmp * g; + d1 = L * M * g; + d2 = Lm * M * g; + d3 = t18_tmp * g * t5 * t7 + t29_tmp * mw * t5 * t6 * t7; + A[1] = t33 * ((((((((((((((((((d3 + d_t33_tmp * g * mp) + d * t5 * t6) + d * t6 * t8) + d_t33_tmp * M * g) - + t33_tmp * Lm * g * mp * t2) + + t19_tmp * mp * t6 * 2.0) + + t19_tmp * mw * t6) + + t18_tmp * M * g * mp * t7) + + d * mp * mw * t6) - + e_t33_tmp * t2 * t6 * t8) + + d1 * t6 * t7 * t8) + + t29_tmp * mp * t5 * t6 * t7) - + e_t33_tmp * mp * mw * t2 * t6) + + d1 * mp * mw * t6 * t7) - + d2 * t2 * t6 * t7 * t8) - + b_t33_tmp * M * g * mp * t2 * t6) - + Iw * Lm * M * g * mp * t2 * t7) - + d2 * mp * mw * t2 * t6 * t7); + A[2] = 0.0; + d = Im * R * g * t3; + t19_tmp = g_t33_tmp * R * g * mp; + b_t33_tmp = f_t33_tmp * R * g; + e_t33_tmp = h_t33_tmp * R * g * mp; + d1 = t27_tmp * M; + A[3] = -R * t33 * + ((((((((((d * t5 + d * t8) + b_t33_tmp * t5) + t19_tmp * t3 * 2.0) + M * R * g * t3 * t7 * t8) + + R * g * mp * t3 * t5 * t7) - + t19_tmp * t2 * t4) + + e_t33_tmp) - + b_t33_tmp * t2 * t8) - + e_t33_tmp * t2) - + d1 * R * g * t2 * t7 * t8); + A[4] = 0.0; + d = b_t18_tmp * M * g * l * mp; + t19_tmp = d1 * g * l; + b_t33_tmp = t19_tmp * mp * mw; + e_t33_tmp = ((((b_t18_tmp * g * l * t5 + b_t19_tmp * t3) + Iw * g * l * t3 * t5) + b_t27_tmp * mw * t5 * t6) + + b_t28_tmp * t3 * t6) + + g * l * mw * t3 * t5 * t6; + A[5] = + t33 * (((((((((e_t33_tmp - b_t19_tmp * t2 * t4) + b_t27_tmp * mp * t5 * t6) - t28_tmp * t2 * t4 * t6 * t8) + d) + + t19_tmp * t6 * t8) - + d * t2) + + b_t33_tmp * t6) - + b_t28_tmp * t2 * t4 * t6) - + b_t33_tmp * t2 * t6); + A[6] = 1.0; + memset(&A[7], 0, 13U * sizeof(double)); + A[20] = 1.0; + A[21] = 0.0; + A[22] = 0.0; + A[23] = 0.0; + A[24] = 0.0; + A[25] = t33 * d3; + A[26] = 0.0; + d = Ip * g; + A[27] = d * t5 * t6 * t7 * t33; + A[28] = 0.0; + t19_tmp = c_t33_tmp * g * l; + A[29] = t33 * ((((e_t33_tmp + i_t33_tmp * g * l) + d * l * t5 * t6) + t19_tmp * mp * t6) + t19_tmp * mw * t6); + A[30] = 0.0; + A[31] = 0.0; + A[32] = 0.0; + A[33] = 0.0; + A[34] = 1.0; + A[35] = 0.0; +} + /* * File trailer for gen_A.c * diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_B.c b/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_B.c index 2a0fb983..f091883d 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_B.c +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_B.c @@ -145,6 +145,130 @@ void gen_B(double Im, double Ip, double Iw, double L, double Lm, double M, doubl t22_tmp * mw * t4 * 2.0); } +/* Function Definitions */ +/* + * gen_B_leg_offset + * B = gen_B_leg_offset(Im,Ip,Iw,L,Lm,M,R,G,l,MP,MW,THETA_L0) + * + * Arguments : double Im + * double Ip + * double Iw + * double L + * double Lm + * double M + * double R + * double g + * double l + * double mp + * double mw + * double theta_l0 + * double B[12] + * Return Type : void + */ +void gen_B_leg_offset(double Im, double Ip, double Iw, double L, double Lm, double M, double R, double g, double l, + double mp, double mw, double theta_l0, double B[12]) +{ + double b_t39_tmp; + double t12; + double t13; + double t13_tmp; + double t14; + double t14_tmp; + double t15; + double t15_tmp; + double t16; + double t2; + double t22; + double t22_tmp; + double t25; + double t25_tmp; + double t26; + double t27; + double t28; + double t29; + double t29_tmp; + double t3; + double t30; + double t30_tmp; + double t39; + double t39_tmp; + double t4; + double t5; + double t7; + double t7_tmp; + double t9; + double t9_tmp; + (void)g; + (void)theta_l0; + /* This function was generated by the Symbolic Math Toolbox version 23.2. + */ + /* 2026-03-02 20:29:44 */ + t2 = L * L; + t3 = R * R; + t4 = l * l; + t5 = Im * Iw; + t7_tmp = Im * L; + t7 = t7_tmp * M * R; + t9_tmp = Iw * L; + t9 = t9_tmp * M * l; + t12 = t7_tmp * R * mp; + t13_tmp = Im * M; + t13 = t13_tmp * t3; + t14_tmp = Iw * M; + t14 = t14_tmp * t4; + t15_tmp = Im * mp; + t15 = t15_tmp * t3; + t16 = Im * mw * t3; + t22_tmp = Ip * M; + t22 = -(t22_tmp * R * l); + t25_tmp = L * M; + t25 = t25_tmp * R * mp * t4; + t26 = t25_tmp * l * mw * t3; + t25_tmp = Lm * M * l; + t27 = t25_tmp * mp * t3; + t28 = t25_tmp * mw * t3; + t29_tmp = M * mp; + t29 = t29_tmp * t3 * t4; + t30_tmp = M * mw; + t30 = t30_tmp * t3 * t4; + t25_tmp = L * Lm; + t39_tmp = mp * t2; + t39 = mw * t2; + b_t39_tmp = t25_tmp * M; + t39 = + 1.0 / ((((((((((((((Ip * t5 + b_t39_tmp * t5) + Ip * t14) + t39_tmp * t5) + Ip * t15) + Ip * t16) + M * t2 * t5) + + Ip * t13) + + t25_tmp * mw * t13) + + t39 * t13) + + t39_tmp * t14) + + Ip * t29) + + Ip * t30) + + t39 * t15) + + t39 * t29); + B[0] = 0.0; + B[1] = -t39 * (((((((((t5 + t7) + t12) + t13) + t14) + t15) + t16) + t25) + t29) + t30); + B[2] = 0.0; + t39_tmp = (t7 + Im * Lm * M * R) + t12; + B[3] = R * t39 * + (((((((t39_tmp + t25) + Im * Ip) + t13_tmp * t2) + t22_tmp * t4) + t15_tmp * t2) + t29_tmp * t2 * t4) + + t7_tmp * Lm * M); + B[4] = 0.0; + t25_tmp = t9 + Iw * Lm * M * l; + B[5] = -t39 * (((((t25_tmp + t22) + t26) + t27) + t28) + b_t39_tmp * R * l * mp); + B[6] = 0.0; + B[7] = t39 * ((((((((t5 + t9) + t13) + t14) + t15) + t16) + t26) + t29) + t30); + B[8] = 0.0; + B[9] = -R * t39 * ((t39_tmp + t22) + t25); + B[10] = 0.0; + B[11] = t39 * (((((((((((((t25_tmp + t26) + t27) + t28) + Ip * Iw) + t22_tmp * t3) + t14_tmp * t2) + Ip * mp * t3) + + Iw * mp * t2) + + Ip * mw * t3) + + t30_tmp * t2 * t3) + + mp * mw * t2 * t3) + + t9_tmp * Lm * M) + + b_t39_tmp * mw * t3); +} + /* * File trailer for gen_B.c * diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp index 18f3abea..93bbab58 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp @@ -74,12 +74,12 @@ void VMCController::update(const ros::Time& time, const ros::Duration& period) // series leg vmc // gazebo - thigh_angle = jointThigh_.getPosition() + M_PI_2; - knee_angle = jointKnee_.getPosition() - M_PI_2; + // thigh_angle = jointThigh_.getPosition() + M_PI_2; + // knee_angle = jointKnee_.getPosition() - M_PI_2; // five link vmc - // thigh_angle = jointThigh_.getPosition() + M_PI; - // knee_angle = jointKnee_.getPosition(); + thigh_angle = jointThigh_.getPosition() + M_PI; + knee_angle = jointKnee_.getPosition(); vmcPtr_->leg_pos(thigh_angle, knee_angle, position); vmcPtr_->leg_spd(jointThigh_.getVelocity(), jointKnee_.getVelocity(), thigh_angle, knee_angle, speed); diff --git a/rm_chassis_controllers/test/vmc_controller.yaml b/rm_chassis_controllers/test/vmc_controller.yaml index eccea9b1..d66cbc4b 100644 --- a/rm_chassis_controllers/test/vmc_controller.yaml +++ b/rm_chassis_controllers/test/vmc_controller.yaml @@ -8,8 +8,8 @@ controllers: vmc_controller: type: rm_chassis_controllers/VMCController vmc_bias_angle: 1.57 - spring_force: 0.0 - pid_length: { p: 800.0, i: 0, d: 25, i_clamp_max: 20.0, i_clamp_min: -20.0, antiwindup: true, publish_state: true } + spring_force: 100.0 + pid_length: { p: 800.0, i: 0, d: 35, i_clamp_max: 20.0, i_clamp_min: -20.0, antiwindup: true, publish_state: true } pid_angle: { p: 30.0, i: 0, d: 2, i_clamp_max: 15.0, i_clamp_min: -15.0, antiwindup: true, publish_state: true } thigh_joint: left_hip_joint knee_joint: left_knee_joint