diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt index ec9391d0..678d931b 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} ) @@ -58,10 +65,16 @@ catkin_package( ## Specify additional locations of header files include_directories( include + include/rm_chassis_controllers ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ) +file(GLOB_RECURSE BIPEDAL_SOURCES + "src/bipedal_wheel_controller/*.cpp" + "src/bipedal_wheel_controller/*.c" +) + ## Declare cpp executables add_library(${PROJECT_NAME} src/chassis_base.cpp @@ -69,6 +82,7 @@ add_library(${PROJECT_NAME} src/sentry.cpp src/swerve.cpp src/balance.cpp + ${BIPEDAL_SOURCES} ) ## Specify libraries to link executable targets against @@ -76,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..ab970f46 --- /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")) diff --git a/rm_chassis_controllers/cfg/PowerLimit.cfg b/rm_chassis_controllers/cfg/PowerLimit.cfg new file mode 100644 index 00000000..52247c4d --- /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("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")) 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..2df41272 --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h @@ -0,0 +1,149 @@ +// +// Created by guanlin on 25-8-28. +// + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "rm_chassis_controllers/chassis_base.h" +#include +#include + +#include "bipedal_wheel_controller/helper_functions.h" +#include "bipedal_wheel_controller/definitions.h" +#include "bipedal_wheel_controller/controller_mode/mode_manager.h" +#include "bipedal_wheel_controller/vmc/VMC.h" + +namespace rm_chassis_controllers +{ +using Eigen::Matrix; + +struct LQRConfig +{ + double Q_theta{}, Q_d_theta{}, Q_x{}, Q_dx{}, Q_phi{}, Q_d_phi{}; + double R_T{}, R_Tp{}; +}; + +class BipedalController : public ChassisBase +{ +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() 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_; } + inline double getDefaultLegLength() const { return default_leg_length_;} + geometry_msgs::Vector3 getVelCmd(){ return vel_cmd_; } + bool getMoveFlag() const{ return move_flag_; } + void setMoveFlag(const bool& move_flag) { move_flag_ = move_flag; } + inline VMCPtr& getVMCPtr() { return vmc_; } + void setStateChange(bool state){ balance_state_changed_ = state; } + void setCompleteStand(bool state){ complete_stand_ = state; } + void setJumpCmd(bool cmd){ jumpCmd_ = cmd; } + void setMode(int mode){ balance_mode_ = mode; } + inline void clearRecoveryFlag() { overturn_ = false; } + void pubState(); + void pubLQRStatus(Eigen::Matrix left_error, Eigen::Matrix right_error, + 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; + void pubLegLenStatus(const bool& upstair_flag); + // clang-format on + void clearStatus(); + +private: + void updateEstimation(const ros::Time& time, const ros::Duration& period); + bool setupModelParams(ros::NodeHandle& controller_nh); + bool setupLQR(ros::NodeHandle& controller_nh); + bool setupControlParams(ros::NodeHandle& controller_nh); + bool setupBiasParams(ros::NodeHandle& controller_nh); + bool setupThresholdParams(ros::NodeHandle& controller_nh); + void polyfit(const std::vector>& 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_{}; + + std::shared_ptr 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; + std::unique_ptr mode_manager_; + VMCPtr vmc_; + + // Slippage_detection + double leftWheelVel{}, rightWheelVel{}, leftWheelVelAbsolute{}, rightWheelVelAbsolute{}, slip_alpha_{ 2.0 }, + slip_R_wheel_{}, R_wheel_{}; + int i = 0, sample_times_ = 3; + bool slip_flag_{ false }; + Eigen::Matrix 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 }; + bool move_flag_{ false }; + // stand up + bool complete_stand_ = false, overturn_ = false; + + // handles + hardware_interface::ImuSensorHandle imu_handle_; + hardware_interface::JointHandle left_wheel_joint_handle_, right_wheel_joint_handle_; + hardware_interface::JointHandle left_hip_joint_handle_, left_knee_joint_handle_, right_hip_joint_handle_, + right_knee_joint_handle_; + std::vector joint_handles_; + + // Leg Cmd + double legCmd_{ 0.2 }; + 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_; + std::shared_ptr> 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/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..aa6b392f --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_base.h @@ -0,0 +1,56 @@ +// +// Created by guanlin on 25-9-3. +// + +#pragma once + +#include +#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_; + 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 new file mode 100644 index 00000000..78b2eec3 --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_manager.h @@ -0,0 +1,42 @@ +// +// Created by guanlin on 25-9-4. +// + +#pragma once + +#include + +#include "bipedal_wheel_controller/controller_mode/mode_base.h" +#include "bipedal_wheel_controller/controller_mode/sit_down.h" +#include "bipedal_wheel_controller/controller_mode/stand_up.h" +#include "bipedal_wheel_controller/controller_mode/recover.h" +#include "bipedal_wheel_controller/controller_mode/normal.h" +#include "bipedal_wheel_controller/controller_mode/upstairs.h" + +namespace rm_chassis_controllers +{ +class ModeManager +{ +public: + ModeManager(ros::NodeHandle& controller_nh, const std::vector& 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_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_, 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 new file mode 100644 index 00000000..63fbbdfa --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h @@ -0,0 +1,48 @@ +// +// Created by guanlin on 25-9-3. +// + +#pragma once + +#include +#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, control_toolbox::Pid* pid_yaw_vel, + control_toolbox::Pid* pid_theta_diff, control_toolbox::Pid* pid_roll); + void execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) override; + const char* name() const override + { + return "NORMAL"; + } + +private: + double calculateSupportForce(double F, double Tp, double leg_length, const double& leg_len_spd, double acc_z, + Eigen::Matrix 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, + 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_; + + 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/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..adffde63 --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h @@ -0,0 +1,58 @@ +// +// 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 +{ + typedef enum + { + ForwardSlip, + BackwardSlip, + } RecoveryChassisState; + + typedef enum + { + INITIALIZED, + START, + MOVING, + MOVING_TOGETHER, + STOP, + } LegRecoveryCalibratedState; + + enum + { + WheelOnGround, + KneeOnGround + } LegState; + +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; + inline void detectChassisStateToRecover(); + const char* name() const override + { + return "RECOVER"; + } + +private: + std::vector joint_handles_; + std::vector pid_legs_, pid_thetas_; + control_toolbox::Pid* pid_theta_diff_; + 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 }; +}; +} // 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..446e4a92 --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h @@ -0,0 +1,64 @@ +// +// 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" +#include "bipedal_wheel_controller/vmc/VMC.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, bool& stop_flag); + /** + * Detect the leg state before stand up: UNDER, FRONT, BEHIND + * @param x + * @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_{}; + bool left_stop_{ false }, right_stop_{ false }; + 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/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..9ad26052 --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/upstairs.h @@ -0,0 +1,48 @@ +// +// 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" +#include "bipedal_wheel_controller/vmc/VMC.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: + /** + * Detect the leg state before stand up: UNDER, FRONT, BEHIND + * @param x + * @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/definitions.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h new file mode 100644 index 00000000..5cf127c8 --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h @@ -0,0 +1,110 @@ +// +// 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 + double f_spring; // Spring Force + double f_gravity; // Gravity Force +}; + +struct ControlParams +{ + double jumpOverTime_; + double p1_; + double p2_; + double p3_; + double p4_; +}; + +struct BiasParams +{ + double x; + double theta; + double pitch; + double roll; + double raw_pitch; + double raw_theta; +}; + +struct LegStateThresholdParams +{ + double under_lower; + double under_upper; + double front_lower; + double front_upper; + double behind_lower; + double behind_upper; + double upstair_des_theta; + double upstair_exit_threshold; +}; + +struct LegCommand +{ + double force; // Thrust + double torque; // Torque + double input[2]; // input +}; + +enum LegState +{ + UNDER, + FRONT, + BEHIND +}; + +enum JumpPhase +{ + LEG_RETRACTION, + JUMP_UP, + OFF_GROUND, + IDLE, +}; + +enum BalanceMode +{ + NORMAL, + STAND_UP, + SIT_DOWN, + RECOVER, + 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 } } +}; + +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..4879661d --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/dynamics/gen_A.h @@ -0,0 +1,33 @@ +/* + * 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]); +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 + +#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..f698871f --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/dynamics/gen_B.h @@ -0,0 +1,33 @@ +/* + * 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]); +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 + +#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..dc800130 --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h @@ -0,0 +1,174 @@ +// +// 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.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; + // 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., + 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 +} + +/** + * 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 + * @param overturn + * @return + */ +[[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; + if (!overturn) + { + 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); + } + } + leg_conv(cmd.force, cmd.torque, leg_angle[0], leg_angle[1], cmd.input); + return cmd; +} + +[[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; + 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; +} + +[[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; + 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; +} +[[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 + * @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/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..a2d06bfa --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/series_legged_vmc_controller.h @@ -0,0 +1,59 @@ +// +// Created by wiselook on 7/27/25. +// + +#pragma once + +#include +#include +#include +#include +#include +#include +#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" + +#include "bipedal_wheel_controller/vmc/VMC.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_, spring_force_; + double lengthCmd_, angleCmd_; + + std::unique_ptr vmcPtr_; + + ros::Publisher statePublisher_, jointCmdStatePublisher_; + ros::Subscriber cmdLegLengthSubscriber_, cmdLegAngleSubscriber_; +}; + +} // namespace rm_chassis_controllers 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..ee7deef5 --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h @@ -0,0 +1,72 @@ +// +// Created by wk on 2026/2/25. +// +#pragma once + +#include + +namespace rm_chassis_controllers +{ +class VMC +{ +public: + 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_; +}; +using VMCPtr = std::shared_ptr; +} // 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..5eda3d2c --- /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 phi4, 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_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/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..4583add1 --- /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 phi4, 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..794f6b96 --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_spd.h @@ -0,0 +1,32 @@ +/* + * 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 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 +} +#endif + +#endif +/* + * File trailer for leg_spd.h + * + * [EOF] + */ 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..93587dca 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -48,6 +48,7 @@ #include #include #include +#include namespace rm_chassis_controllers { @@ -100,7 +101,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. @@ -141,6 +142,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 +174,8 @@ 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_; 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/rm_chassis_controllers_plugins.xml b/rm_chassis_controllers/rm_chassis_controllers_plugins.xml index 6319ebc3..d1f951d5 100644 --- a/rm_chassis_controllers/rm_chassis_controllers_plugins.xml +++ b/rm_chassis_controllers/rm_chassis_controllers_plugins.xml @@ -32,6 +32,20 @@ EffortJointInterface type of hardware interface. - + + + The BalanceController is RoboMaster SeriesLegged standard robot SeriesLegged controller. It expects a + EffortJointInterface type of hardware interface. + + + + + The two link vmc controller. + + 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..38fb571e --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp @@ -0,0 +1,548 @@ +// +// Created by guanlin on 25-8-28. +// + +#include "bipedal_wheel_controller/controller.h" + +#include +#include +#include + +#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" + +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(); + 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) || !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; + }; + 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_.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(); + + // Slippage detection + 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(); + X_.setZero(); + U_.setZero(); + 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; +} + +void BipedalController::moveJoint(const ros::Time& time, const ros::Duration& period) +{ + if (!balance_state_changed_) + mode_manager_->switchMode(balance_mode_); + 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(); +} + +void BipedalController::clearStatus() +{ + x_left_(2) = x_right_(2) = -bias_params_->x; +} + +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_ = (abs(pitch) > 0.65 || abs(roll) > 0.4) && z_world.z() < 0.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; + 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 + 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]); + // 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_; + 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.; + 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_->update(X_, R_); + } + else + { + kalmanFilterPtr_->predict(U_); + i++; + } + auto x_hat_vel = kalmanFilterPtr_->getState(); + slip_flag_ = abs(x_hat_vel(0) - wheel_vel_aver) > 3.0; + + // update state + x_left_[3] = state_ != RAW ? x_hat_vel(0) : 0; + if (abs(x_left_[3]) <= 0.6f && 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; + 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; + + 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); + 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::SIT_DOWN; + 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 }, + { "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)) + { + ROS_ERROR("Param %s not given (namespace: %s)", e.first, controller_nh.getNamespace().c_str()); + 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()); + 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 = 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."); + return false; + } + Eigen::Matrix k = lqr.getK(); + 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; +} + +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 }, + { "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)) + { + ROS_ERROR("Param %s not given (namespace: %s)", e.first, controller_nh.getNamespace().c_str()); + return false; + } + return true; +} + +// [will unused] +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; +} + +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) || + !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"); + return false; + } + 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; +} + +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, + Eigen::Matrix F_leg_, const bool unstick[2]) const +{ + if (lqr_status_pub_->trylock()) + { + 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(); + } +} + +void BipedalController::pubLegLenStatus(const bool& upstair_flag) +{ + rm_msgs::LeggedUpstairStatus msg; + msg.upstair_flag = 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/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..ffc07e83 --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_base.cpp @@ -0,0 +1,46 @@ +// +// 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; + 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) +{ + 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..0c31ea7a --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp @@ -0,0 +1,51 @@ +// +// 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_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))) + 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_); + 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_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_))); +} +} // 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..66577b29 --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -0,0 +1,316 @@ +// +// 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, 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_vel_(pid_yaw_vel) + , pid_theta_diff_(pid_theta_diff) + , pid_roll_(pid_roll) +{ + 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(); + 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.15) + { + controller->setCompleteStand(true); + } + + auto vel_cmd_ = controller->getVelCmd(); + if (controller->getMoveFlag() && abs(x_left_[3]) < 0.1 && abs(vel_cmd_.x) < 0.1) + { + controller->setMoveFlag(false); + } + + 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); + double F_roll = pid_roll_->computeCommand(0. - roll_, period); + + // LQR + 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_; + Matrix x_left_ref, x_right_ref; + x_left_ref.setZero(); + x_right_ref.setZero(); + + if (controller->getCompleteStand()) + { + x_left_ref(2) = x_right_ref(2) = pos_des_; + x_left_ref(3) = x_right_ref(3) = friction_circle_alpha * vel_cmd_.x; + leg_length_des = controller->getLegCmd(); + } + 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; + + u_left = k_left * (-x_left); + u_right = k_right * (-x_right); + + // 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; + Eigen::Matrix F_leg; + + // check jump + if (jump_phase_ == JumpPhase::IDLE && + ros::Time::now() - lastJumpTime_ > ros::Duration(control_params_->jumpOverTime_) && controller->getJumpCmd()) + { + jump_phase_ = JumpPhase::LEG_RETRACTION; + ROS_INFO("[balance] Jump start"); + } + if (jump_phase_ == JumpPhase::IDLE) + { + double left_length_des = controller->getCompleteStand() ? leg_length_des : controller->getDefaultLegLength(); + 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) > 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; + } + 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: + { + 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; + F_leg(RIGHT) = pid_legs_[RIGHT]->computeCommand(leg_length_des - current_leg_length, period) + + gravity * cos(left_pos_[1]) - F_roll - spring_force; + if (current_leg_length < leg_length_des + 0.01) + { + 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; + 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_++; + } + if (jumpTime_ >= 2) + { + jumpTime_ = 0; + jump_phase_ = JumpPhase::OFF_GROUND; + } + 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) = -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) + { + jumpTime_++; + } + if (jumpTime_ >= 8) + { + jumpTime_ = 0; + jump_phase_ = JumpPhase::IDLE; + lastJumpTime_ = ros::Time::now(); + ROS_INFO("[balance] Jump end"); + } + break; + } + } + + // 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{ false }, right_unstick{ false }; + if (controller->getCompleteStand() && jump_phase_ != JumpPhase::LEG_RETRACTION) + { + 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(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); + + 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; + 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); + } + + // Control + double left_T[2], right_T[2]; + 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] } }, + 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() && + 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(); + controller->setMode(BalanceMode::UPSTAIRS); + controller->setStateChange(false); + controller->setJumpCmd(false); + left_wheel_cmd = right_wheel_cmd = 0; + ROS_INFO("[balance] Exit NORMAL"); + } + + // Protection + 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(); + 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, + Eigen::Matrix x, + const std::shared_ptr& model_params, const ros::Duration& period) +{ + 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; + 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_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; + + return Fn; +} + +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); + supportForceAveragePtr->input(Fn); + bool unstick_ = supportForceAveragePtr->output() < 10; + + if (unstick_ != last_unstick_) + { + if (!maybeChange) + { + judgeTime = ros::Time::now(); + maybeChange = true; + } + else + { + if (ros::Time::now() - judgeTime > ros::Duration(0.1)) + { + last_unstick_ = unstick_; + } + } + } + else + { + maybeChange = false; + } + return unstick_; +} +} // 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..8a9b9be0 --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp @@ -0,0 +1,84 @@ +// +// 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, control_toolbox::Pid* pid_theta_diff) + : joint_handles_(joint_handles), pid_legs_(pid_legs), pid_thetas_(pid_thetas), pid_theta_diff_(pid_theta_diff) +{ +} + +void Recover::execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) +{ + if (!controller->getStateChange()) + { + ROS_INFO("[balance] Enter RECOVER"); + detectd_flag = false; + controller->setStateChange(true); + } + + // until chassis + if (!detectd_flag && abs(x_left_[1]) < 0.1 && abs(x_left_[5]) < 0.1) + { + detectChassisStateToRecover(); + 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. } }; + // 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) + { + 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; + 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.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, 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, 5 * 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 (abs(pitch_) < 0.2 && linear_acc_base_.z > 5.0 && !controller->getOverturn()) + { + controller->setMode(BalanceMode::STAND_UP); + controller->setStateChange(false); + controller->clearRecoveryFlag(); + ROS_INFO("[balance] Exit RECOVER"); + } +} + +void Recover::detectChassisStateToRecover() +{ + // pitch_ is base_link pitch not model pitch + if (pitch_ > 0.45 && pitch_ < M_PI) + { + ROS_INFO("forward"); + recovery_chassis_state_ = RecoveryChassisState::ForwardSlip; + } + else if (pitch_ < -0.45 && pitch_ > -M_PI) + { + ROS_INFO("back"); + recovery_chassis_state_ = RecoveryChassisState::BackwardSlip; + } +} +} // 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..9d360211 --- /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 && controller->getBaseState() != 4) + { + 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..47775243 --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp @@ -0,0 +1,148 @@ +// +// 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); + leg_state_threshold_ = controller->getLegThresholdParams(); + vmcPtr_ = controller->getVMCPtr(); + StandUp::detectLegState(x_left_, left_leg_state); + StandUp::detectLegState(x_right_, right_leg_state); + } + + 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, + 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.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); + 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, bool& stop_flag) +{ + switch (leg_state) + { + case LegState::UNDER: + theta_des = -M_PI_2; + length_des = 0.36; + if (leg_length > 0.35) + { + leg_state = LegState::FRONT; + } + break; + case LegState::FRONT: + theta_des = M_PI_2 - 0.35; + length_des = 0.36; + 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: + 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.21) + theta_des = -0.1; + } + 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; + } +} + +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; + 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/controller_mode/upstairs.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp new file mode 100644 index 00000000..34d81f11 --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp @@ -0,0 +1,101 @@ +// +// 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); + 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, 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_[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(true); + controller->setMode(BalanceMode::STAND_UP); + controller->setStateChange(false); + 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; + } +} + +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 new file mode 100644 index 00000000..92bc6560 --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_A.c @@ -0,0 +1,345 @@ +/* + * 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 +#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_t22_tmp; + double b_t35_tmp; + double b_t42_tmp; + double c_t42_tmp; + double d; + double d1; + 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 t2; + double t22_tmp; + double t24_tmp; + double t3; + double t31; + double t31_tmp; + double t33_tmp; + double t34_tmp; + double t35_tmp; + double t36_tmp; + double t4; + double t42; + double t42_tmp; + double t5; + double t6; + double t7; + /* This function was generated by the Symbolic Math Toolbox version 23.2. + */ + /* 2025-12-05 01:47:45 */ + t2 = L * L; + t3 = Lm * Lm; + t4 = M * M; + t5 = R * R; + t6 = l * l; + t7 = mp * mp; + t17_tmp = Iw * M; + 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 / + (((((((((((((((((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_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; + 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) + + t34_tmp * mp * t4 * t5 * t6) + + d1 * mp * mw * t5 * t6); + A[2] = 0.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 = 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; + A[21] = 0.0; + A[22] = 0.0; + A[23] = 0.0; + A[24] = 0.0; + A[25] = t42 * t42_tmp; + A[26] = 0.0; + A[27] = e_t42_tmp * (t31 - Ip * R * g * t4 * t6); + A[28] = 0.0; + 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; + A[33] = 0.0; + A[34] = 1.0; + 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 + * + * [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..f091883d --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_B.c @@ -0,0 +1,276 @@ +/* + * 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 t10; + double t10_tmp; + double t11; + 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 t23_tmp; + double t28; + double t28_tmp; + double t29; + double t3; + double t30; + double t31; + double t32; + double t32_tmp; + double t33; + double t33_tmp; + double t4; + 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 23.2. + */ + /* 2025-12-05 01:47:45 */ + t2 = L * L; + t3 = Lm * Lm; + t4 = R * R; + t5 = l * l; + t6 = Im * Iw; + 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 * t15) + t28_tmp * t6) + Ip * t16) + Ip * t17) + t22_tmp * t6 * 2.0) + M * t2 * t6) + + M * t3 * t6) + + 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; + B[1] = -t44 * ((((((((((t6 + t8) + t9) + t13) + t14) + t15) + t16) + t17) + t28) + t32) + t33); + B[2] = 0.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; + t28_tmp = t10 + t11; + B[5] = -t44 * (((((t28_tmp + t22) + t23) + t29) + t30) + t31); + B[6] = 0.0; + B[7] = t44 * (((((((((((t6 + t10) + t11) + t14) + t15) + t16) + t17) + t29) + t30) + t31) + t32) + t33); + B[8] = 0.0; + B[9] = -R * t44 * (((t44_tmp + t22) + t23) + t28); + B[10] = 0.0; + B[11] = + 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) + + t32_tmp * t3 * t4) + + t33_tmp * t2 * t4) + + t33_tmp * t3 * t4) + + mp * mw * t2 * t4) + + t10_tmp * Lm * M * 2.0) + + 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 + * + * [EOF] + */ 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..93bbab58 --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp @@ -0,0 +1,125 @@ +// +// 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; + } + if (!controller_nh.getParam("spring_force", spring_force_)) + { + 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; +} + +void VMCController::starting(const ros::Time& /*time*/) +{ + angleCmd_ = 0.; + 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]; + + // 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(); + 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; + 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) - spring_force_; + effortCmd[1] = pidAngle_.computeCommand(angle_error, period); + + 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); + 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); + 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; + 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/src/bipedal_wheel_controller/vmc/VMC.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/VMC.cpp new file mode 100644 index 00000000..d6eeb4c2 --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/VMC.cpp @@ -0,0 +1,137 @@ +// +// 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); + + // 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) +{ + 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]) +{ + 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; + + // 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 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..0a843498 --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_conv.c @@ -0,0 +1,170 @@ +/* + * 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 "bipedal_wheel_controller/vmc/leg_spd.h" +#include "bipedal_wheel_controller/vmc/leg_params.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 phi4, double T[2]) +{ + 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 + phi4; + 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); +} + +// 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 + * + * [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..5953ae38 --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_pos.c @@ -0,0 +1,103 @@ +/* + * 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 "bipedal_wheel_controller/vmc/leg_params.h" +#include +#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 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 + 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 + * + * [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..229e9f54 --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_spd.c @@ -0,0 +1,191 @@ +/* + * 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 "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,DPHI4,PHI1,PHI4) + * + * Arguments : double dphi1 + * double dphi4 + * double phi1 + * double phi4 + * double spd[2] + * Return Type : void + */ +void leg_spd(double dphi1, double dphi4, double phi1, double phi4, 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 + phi4; + 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] = dphi4 / sqrt(t37_tmp) * (t9 * t30_tmp / 2.0 - t5 * t29_tmp / 2.0) / 2.0; + spd[1] = + (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 + * + * [EOF] + */ diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 627b4831..1e744ebd 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -117,6 +117,13 @@ 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_limit_srv_ = new dynamic_reconfigure::Server( + ros::NodeHandle(controller_nh, "power")); + dynamic_reconfigure::Server::CallbackType cb = + boost::bind(&ChassisBase::powerLimitReconfigCB, this, _1, _2); + power_limit_srv_->setCallback(cb); + return true; } @@ -385,6 +392,12 @@ template void ChassisBase::powerLimit() { double power_limit = cmd_rt_buffer_.readFromRT()->cmd_chassis_.power_limit; + 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; + // Three coefficients of a quadratic equation in one variable double a = 0., b = 0., c = 0.; for (const auto& joint : joint_handles_) @@ -398,8 +411,8 @@ void ChassisBase::powerLimit() c += square(real_vel); } } - a *= effort_coeff_; - c = c * velocity_coeff_ - power_offset_ - power_limit; + a *= effort_coeff; + 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_) @@ -458,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 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..d66cbc4b --- /dev/null +++ b/rm_chassis_controllers/test/vmc_controller.yaml @@ -0,0 +1,21 @@ +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 + 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 + # series_legged1 + l1: 0.218 + l2: 0.26 + # series_legged2 +# l1: 0.21 +# l2: 0.248