From 10e15f386e14a47e5997ef7ceebe2a30e7337142 Mon Sep 17 00:00:00 2001 From: dzran1210 Date: Thu, 10 Sep 2026 16:35:23 +0000 Subject: [PATCH] Update controllers from RM26 hole standard. --- rm_chassis_controllers/CMakeLists.txt | 1 - rm_chassis_controllers/README.md | 5 + rm_chassis_controllers/cfg/LQRWeight.cfg | 6 +- .../bipedal_wheel_controller/controller.h | 91 ++- .../controller_interface.h | 55 ++ .../controller_mode/mode_base.h | 28 +- .../controller_mode/mode_manager.h | 7 +- .../controller_mode/normal.h | 14 +- .../controller_mode/protect.h | 39 + .../controller_mode/recover.h | 18 +- .../controller_mode/sit_down.h | 5 +- .../controller_mode/stand_up.h | 38 +- .../controller_mode/upstairs.h | 18 +- .../bipedal_wheel_controller/definitions.h | 72 +- .../helper_functions.h | 118 +-- .../series_legged_vmc_controller.h | 25 +- .../bipedal_wheel_controller/vmc/VMC.h | 100 ++- .../rm_chassis_controllers/chassis_base.h | 156 +++- .../include/rm_chassis_controllers/omni.h | 12 +- .../include/rm_chassis_controllers/swerve.h | 23 +- rm_chassis_controllers/src/balance.cpp | 4 +- .../bipedal_wheel_controller/controller.cpp | 344 ++++++--- .../controller_mode/mode_base.cpp | 31 - .../controller_mode/mode_manager.cpp | 27 +- .../controller_mode/normal.cpp | 292 +++++--- .../controller_mode/protect.cpp | 100 +++ .../controller_mode/recover.cpp | 149 +++- .../controller_mode/sit_down.cpp | 30 +- .../controller_mode/stand_up.cpp | 205 ++++-- .../controller_mode/upstairs.cpp | 76 +- .../series_legged_vmc_controller.cpp | 87 ++- .../src/bipedal_wheel_controller/vmc/VMC.cpp | 196 ++++- rm_chassis_controllers/src/chassis_base.cpp | 491 +++++++------ rm_chassis_controllers/src/omni.cpp | 222 +++++- rm_chassis_controllers/src/sentry.cpp | 4 +- rm_chassis_controllers/src/swerve.cpp | 305 +++++++- rm_chassis_controllers/test/omni.yaml | 1 + rm_chassis_controllers/test/swerve.yaml | 1 + .../test/vmc_controller.yaml | 24 +- .../ARCHITECTURE_AND_CONTROL_REPORT.md | 141 ++++ rm_gimbal_controllers/CMakeLists.txt | 4 +- rm_gimbal_controllers/cfg/BulletSolver.cfg | 14 +- rm_gimbal_controllers/cfg/GimbalBase.cfg | 1 + .../rm_gimbal_controllers/bullet_solver.h | 113 +-- .../dual_yaw_controller.h | 38 + .../rm_gimbal_controllers/gimbal_base.h | 82 ++- rm_gimbal_controllers/package.xml | 1 + .../rm_gimbal_controllers_plugins.xml | 7 + .../src/ballistic_solver.cpp | 6 +- rm_gimbal_controllers/src/bullet_solver.cpp | 690 ++++++++++-------- .../src/dual_yaw_controller.cpp | 154 ++++ rm_gimbal_controllers/src/gimbal_base.cpp | 493 +++++++++---- .../orientation_controller.h | 5 + .../src/orientation_controller.cpp | 50 ++ rm_shooter_controllers/src/standard.cpp | 3 +- 55 files changed, 3774 insertions(+), 1448 deletions(-) create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_interface.h create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/protect.h create mode 100644 rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/protect.cpp create mode 100644 rm_gimbal_controllers/ARCHITECTURE_AND_CONTROL_REPORT.md mode change 100644 => 100755 rm_gimbal_controllers/cfg/GimbalBase.cfg create mode 100644 rm_gimbal_controllers/include/rm_gimbal_controllers/dual_yaw_controller.h create mode 100644 rm_gimbal_controllers/src/dual_yaw_controller.cpp diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt index 726a12e4..704e5deb 100644 --- a/rm_chassis_controllers/CMakeLists.txt +++ b/rm_chassis_controllers/CMakeLists.txt @@ -28,7 +28,6 @@ find_package(Eigen3 REQUIRED) generate_dynamic_reconfigure_options( cfg/LQRWeight.cfg - cfg/PowerLimit.cfg ) ################################### diff --git a/rm_chassis_controllers/README.md b/rm_chassis_controllers/README.md index 0b6a81ae..800750bf 100644 --- a/rm_chassis_controllers/README.md +++ b/rm_chassis_controllers/README.md @@ -119,6 +119,10 @@ sudo rosdep install --from-paths src Allowed period (in s) between two commands. If the time is exceed this period, the speed of chassis will be set 0. +* **`raw_yaw_feedforward_k`** (double, default: 0.0) + + Yaw feedforward time constant in seconds. Only used in `raw` mode, where the command-frame rotation uses `yaw + k * w` to compensate direction-change lag. + * **`power_offset`** (double) Fix the difference between theoretical power and actual power. @@ -245,6 +249,7 @@ sudo rosdep install --from-paths src power_offset: -8.41 twist_angular: 0.5233 timeout: 0.1 + raw_yaw_feedforward_k: 0.0 pid_follow: { p: 5.0, i: 0, d: 0.3, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] diff --git a/rm_chassis_controllers/cfg/LQRWeight.cfg b/rm_chassis_controllers/cfg/LQRWeight.cfg index ab970f46..c04fa9ab 100644 --- a/rm_chassis_controllers/cfg/LQRWeight.cfg +++ b/rm_chassis_controllers/cfg/LQRWeight.cfg @@ -5,11 +5,11 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("Q_theta", double_t, 0, "theta weight", 1.0, 0.0, 10000.0) +gen.add("Q_theta", double_t, 0, "theta weight", 1.0, 0.0, 200000.0) gen.add("Q_d_theta", double_t, 0, "d_theta weight", 1.0, 0.0, 10000.0) -gen.add("Q_x", double_t, 0, "x weight", 1.0, 0.0, 10000.0) +gen.add("Q_x", double_t, 0, "x weight", 1.0, 0.0, 100000.0) gen.add("Q_dx", double_t, 0, "dx weight", 1.0, 0.0, 10000.0) -gen.add("Q_phi", double_t, 0, "phi weight", 1.0, 0.0, 10000.0) +gen.add("Q_phi", double_t, 0, "phi weight", 1.0, 0.0, 200000.0) gen.add("Q_d_phi", double_t, 0, "d_phi weight", 1.0, 0.0, 10000.0) gen.add("R_T", double_t, 0, "wheel torque weight", 1.0, 0.001, 10000.0) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h index 2df41272..c0c3d4c8 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h @@ -13,6 +13,7 @@ #include #include #include +#include #include #include #include @@ -29,6 +30,7 @@ #include "bipedal_wheel_controller/definitions.h" #include "bipedal_wheel_controller/controller_mode/mode_manager.h" #include "bipedal_wheel_controller/vmc/VMC.h" +#include "bipedal_wheel_controller/controller_interface.h" namespace rm_chassis_controllers { @@ -41,7 +43,8 @@ struct LQRConfig }; class BipedalController : public ChassisBase + hardware_interface::EffortJointInterface>, + public BipedalControllerInterface { public: BipedalController() = default; @@ -49,43 +52,53 @@ class BipedalController : public ChassisBase 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(); + // BipedalControllerInterface implementations + bool getOverturn() const override { return overturn_; } + bool getStateChange() const override { return balance_state_changed_; } + bool getCompleteStand() const override { return complete_stand_; } + Eigen::Matrix getCoeffs() override { return coeffs_; } + const std::shared_ptr& getModelParams() const override { return model_params_; } + const std::shared_ptr& getControlParams() const override { return control_params_; } + const std::shared_ptr& getBiasParams() const override { return bias_params_; } + const std::shared_ptr& getLegThresholdParams() const override { return leg_threshold_params_; } + const std::shared_ptr& getChassisGeometryParams() const override { return chassis_geometry_params_; } + double getLegCmd() const override { return legCmd_; } + double getJumpCmd() const override { return jumpCmd_; } + int getBaseState() const override { return state_; } + inline double getDefaultLegLength() const override { return default_leg_length_;} + geometry_msgs::Vector3 getVelCmd() override { return vel_cmd_; } + inline bool getMoveFlag() const override { return move_flag_; } + inline void setMoveFlag(const bool& move_flag) override { move_flag_ = move_flag; } + inline const ChassisState& getChassisState() override { return chassis_state_; }; + inline LegState& getLegState(Side side) override { return leg_state_[side]; }; + inline void setStateChange(bool state) override { balance_state_changed_ = state; } + inline void setCompleteStand(bool state) override { complete_stand_ = state; } + void setJumpCmd(bool cmd) override { jumpCmd_ = cmd; } + void setMode(int mode) override { balance_mode_ = mode; } + inline void clearRecoveryFlag() override { overturn_ = false; } + double f_spring_force(double L0) override; + void pubState() override; void pubLQRStatus(Eigen::Matrix 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); + Eigen::Matrix F_leg_, const bool unstick[2]) const override; + void pubLegLenStatus(const bool& upstair_flag) override; + void clearStatus() override; + void pubDebugData(const std::string& name, double value) override{ debugPub_->add(name, value); }; + void setRecoveryLegSpdTurnback(bool recovery_leg_spd_turnback) override { recovery_leg_spd_turnback_ = recovery_leg_spd_turnback; } + bool getRecoveryLegSpdTurnback() const override { return recovery_leg_spd_turnback_;} // clang-format on - void clearStatus(); - private: void updateEstimation(const ros::Time& time, const ros::Duration& period); - bool setupModelParams(ros::NodeHandle& controller_nh); + bool setupLQR(ros::NodeHandle& controller_nh); + bool setupParams(ros::NodeHandle& controller_nh); + bool setupModelParams(ros::NodeHandle& controller_nh); bool setupControlParams(ros::NodeHandle& controller_nh); bool setupBiasParams(ros::NodeHandle& controller_nh); bool setupThresholdParams(ros::NodeHandle& controller_nh); + bool setupSpringParams(ros::NodeHandle& controller_nh); + bool setupChassisGeometryParams(ros::NodeHandle& controller_nh); void polyfit(const std::vector>& Ks, const std::vector& L0s, Eigen::Matrix& coeffs); geometry_msgs::Twist odometry() override; @@ -99,32 +112,35 @@ class BipedalController : public ChassisBase model_params_; std::shared_ptr control_params_; std::shared_ptr bias_params_; + std::shared_ptr spring_params_; + std::shared_ptr chassis_geometry_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_; + std::shared_ptr mode_manager_; // Slippage_detection double leftWheelVel{}, rightWheelVel{}, leftWheelVelAbsolute{}, rightWheelVelAbsolute{}, slip_alpha_{ 2.0 }, slip_R_wheel_{}, R_wheel_{}; - int i = 0, sample_times_ = 3; + int itor = 0, sample_times_ = 0; bool slip_flag_{ false }; Eigen::Matrix 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 }; + ChassisState chassis_state_; + LegState leg_state_[2]; + // Eigen::Matrix x_left_{}, x_right_{}; + double default_leg_length_{ 0.12 }; bool move_flag_{ false }; // stand up bool complete_stand_ = false, overturn_ = false; + // recovery + bool recovery_leg_spd_turnback_{ false }; // handles - hardware_interface::ImuSensorHandle imu_handle_; + hardware_interface::ImuSensorHandle imu_handle_, gimbal_imu_handle_; hardware_interface::JointHandle left_wheel_joint_handle_, right_wheel_joint_handle_; hardware_interface::JointHandle left_hip_joint_handle_, left_knee_joint_handle_, right_hip_joint_handle_, right_knee_joint_handle_; @@ -139,11 +155,12 @@ class BipedalController : public ChassisBase config_rt_buffer_; LQRConfig config_{}; bool dynamic_reconfig_initialized_{ false }; - ros::Subscriber leg_cmd_sub_; + ros::Subscriber leg_cmd_sub_, recovery_leg_spd_turnback_sub_; ros::Publisher unstick_pub_, upstair_status_pub_; std::shared_ptr> legged_chassis_status_pub_; std::shared_ptr> legged_chassis_mode_pub_; std::shared_ptr> lqr_status_pub_; ros::Time cmd_update_time_; + std::shared_ptr debugPub_; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_interface.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_interface.h new file mode 100644 index 00000000..1021f051 --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_interface.h @@ -0,0 +1,55 @@ +// +// Created by wk on 2026/4/11. +// +#pragma once +#include +#include +#include "bipedal_wheel_controller/definitions.h" +#include "bipedal_wheel_controller/vmc/VMC.h" + +namespace rm_chassis_controllers +{ +class BipedalControllerInterface +{ +public: + virtual ~BipedalControllerInterface() = default; + + virtual bool getStateChange() const = 0; + virtual void setStateChange(bool state) = 0; + virtual int getBaseState() const = 0; + virtual bool getOverturn() const = 0; + virtual void setMode(int mode) = 0; + virtual void clearRecoveryFlag() = 0; + virtual bool getCompleteStand() const = 0; + virtual Eigen::Matrix getCoeffs() = 0; + + virtual const std::shared_ptr& getModelParams() const = 0; + virtual const std::shared_ptr& getControlParams() const = 0; + virtual const std::shared_ptr& getBiasParams() const = 0; + virtual const std::shared_ptr& getLegThresholdParams() const = 0; + virtual const std::shared_ptr& getChassisGeometryParams() const = 0; + virtual double getLegCmd() const = 0; + virtual double getJumpCmd() const = 0; + virtual double getDefaultLegLength() const = 0; + virtual geometry_msgs::Vector3 getVelCmd() = 0; + virtual bool getMoveFlag() const = 0; + virtual void setMoveFlag(const bool& move_flag) = 0; + virtual const ChassisState& getChassisState() = 0; + virtual LegState& getLegState(Side side) = 0; + virtual void setCompleteStand(bool state) = 0; + virtual void setJumpCmd(bool cmd) = 0; + virtual double f_spring_force(double L0) = 0; + virtual void pubState() = 0; + virtual void pubLQRStatus(Eigen::Matrix 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 = 0; + virtual void pubLegLenStatus(const bool& upstair_flag) = 0; + virtual void clearStatus() = 0; + virtual void pubDebugData(const std::string& name, double value) = 0; + virtual bool getRecoveryLegSpdTurnback() const = 0; + virtual void setRecoveryLegSpdTurnback(bool recovery_leg_spd_turnback) = 0; +}; + +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_base.h index aa6b392f..81f92d7a 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_base.h @@ -10,6 +10,7 @@ #include #include "bipedal_wheel_controller/definitions.h" +#include "bipedal_wheel_controller/controller_interface.h" namespace rm_chassis_controllers { @@ -18,26 +19,13 @@ class BipedalController; class ModeBase { public: - virtual void execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) = 0; + explicit ModeBase(BipedalControllerInterface* controller_) : controller(controller_) + { + } + virtual void execute(const ros::Time& time, const ros::Duration& period) = 0; virtual const char* name() const = 0; virtual ~ModeBase() = default; - void updateEstimation(const Eigen::Matrix& 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() { @@ -45,12 +33,8 @@ class ModeBase } 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_{}; + BipedalControllerInterface* controller{ nullptr }; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_manager.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_manager.h index 78b2eec3..5ca2de4c 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_manager.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_manager.h @@ -12,13 +12,16 @@ #include "bipedal_wheel_controller/controller_mode/recover.h" #include "bipedal_wheel_controller/controller_mode/normal.h" #include "bipedal_wheel_controller/controller_mode/upstairs.h" +#include "bipedal_wheel_controller/controller_mode/protect.h" +#include "bipedal_wheel_controller/controller_interface.h" namespace rm_chassis_controllers { class ModeManager { public: - ModeManager(ros::NodeHandle& controller_nh, const std::vector& joint_handles); + ModeManager(BipedalControllerInterface* controller, ros::NodeHandle& controller_nh, + const std::vector& joint_handles); virtual ~ModeManager() = default; void switchMode(int mode) { @@ -36,7 +39,7 @@ class ModeManager control_toolbox::Pid pid_yaw_vel_, pid_left_leg_, pid_right_leg_, pid_theta_diff_, pid_roll_; control_toolbox::Pid pid_left_leg_stand_up_, pid_right_leg_stand_up_; control_toolbox::Pid pid_left_leg_theta_, pid_right_leg_theta_, pid_left_leg_theta_vel_, pid_right_leg_theta_vel_; - control_toolbox::Pid pid_left_wheel_vel_, pid_right_wheel_vel_; + control_toolbox::Pid pid_left_wheel_vel_, pid_right_wheel_vel_, pid_wheel_vel_diff_; std::vector pid_wheels_, pid_legs_, pid_thetas_, pid_legs_stand_up_; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h index 63fbbdfa..9a918c10 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h @@ -17,10 +17,11 @@ namespace rm_chassis_controllers class Normal : public ModeBase { public: - Normal(const std::vector& joint_handles, + Normal(BipedalControllerInterface* controller_, 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; + control_toolbox::Pid* pid_theta_diff, control_toolbox::Pid* pid_roll, + control_toolbox::Pid* pid_wheel_vel_diff); + void execute(const ros::Time& time, const ros::Duration& period) override; const char* name() const override { return "NORMAL"; @@ -33,16 +34,17 @@ class Normal : public ModeBase bool unstickDetection(const double& F_leg, const double& Tp, const double& leg_len_spd, const double& leg_length, const double& acc_z, const std::shared_ptr& model_params, Eigen::Matrix x, - std::shared_ptr> supportForceAveragePtr, + const 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_; - + control_toolbox::Pid *pid_yaw_vel_, *pid_theta_diff_, *pid_roll_, *pid_wheel_vel_diff_; ros::Time lastJumpTime_{}; double pos_des_{ 0.0 }, leg_length_des{ 0.2 }; + double unstick_threshold{ 15.0 }; int jump_phase_ = JumpPhase::IDLE, jumpTime_{ 0 }; + bool x_offset_flag_{ false }, protect_flag_{ false }; std::shared_ptr> leftSupportForceAveragePtr_, rightSupportForceAveragePtr_; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/protect.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/protect.h new file mode 100644 index 00000000..ac86520d --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/protect.h @@ -0,0 +1,39 @@ +// +// Created by wk on 2026/5/13. +// + +#pragma once + +#include +#include +#include + +#include "bipedal_wheel_controller/controller_mode/mode_base.h" +#include "bipedal_wheel_controller/definitions.h" + +namespace rm_chassis_controllers +{ +class Protect : public ModeBase +{ +public: + explicit Protect(BipedalControllerInterface* controller_, + const std::vector& joint_handles, + const std::vector& pid_legs, + const std::vector& pid_thetas, + const std::vector& pid_wheels, control_toolbox::Pid* pid_theta_diff, + control_toolbox::Pid* pid_yaw_vel); + void execute(const ros::Time& time, const ros::Duration& period) override; + const char* name() const override + { + return "PROTECT"; + } + +private: + double theta_des_l, theta_des_r, length_des_l, length_des_r; + std::vector joint_handles_; + std::vector pid_legs_, pid_thetas_; + std::vector pid_wheels_; + control_toolbox::Pid *pid_theta_diff_, *pid_yaw_vel_; + std::shared_ptr> ramp_length_des_l_, ramp_length_des_r_, ramp_angle_des_l_, ramp_angle_des_r_; +}; +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h index adffde63..7e533442 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h @@ -29,18 +29,19 @@ class Recover : public ModeBase STOP, } LegRecoveryCalibratedState; - enum + typedef enum { - WheelOnGround, - KneeOnGround - } LegState; + NotReady, + Ready + } LegRecoveryState; public: - Recover(const std::vector& joint_handles, + Recover(BipedalControllerInterface* controller_, const std::vector& joint_handles, const std::vector& pid_legs, const std::vector& pid_thetas, control_toolbox::Pid* pid_theta_diff); - void execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) override; + void execute(const ros::Time& time, const ros::Duration& period) override; inline void detectChassisStateToRecover(); + inline void detectLegRecoveryState(LegRecoveryState& recovery_state, const double& leg_pos); const char* name() const override { return "RECOVER"; @@ -50,9 +51,12 @@ class Recover : public ModeBase 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 }; + double leg_recovery_velocity_{ 5.0 }, threshold_{ 0.05 }, leg_theta_diff_{ 0.0 }, desired_leg_length_{ 0.36 }; + double left_leg_recovery_feed_forward{ 10.0 }, right_leg_recovery_feed_forward{ 10.0 }; const double leg_recovery_velocity_const_{ 5.0 }; + LegRecoveryState left_recovery_leg, right_recovery_leg; RecoveryChassisState recovery_chassis_state_{ ForwardSlip }; bool detectd_flag{ false }; + ChassisState chassis_state_; }; } // 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 index f6f140dd..e19f3b91 100644 --- 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 @@ -15,9 +15,10 @@ namespace rm_chassis_controllers class SitDown : public ModeBase { public: - explicit SitDown(const std::vector& joint_handles, + explicit SitDown(BipedalControllerInterface* controller_, + const std::vector& joint_handles, const std::vector& pid_wheels); - void execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) override; + void execute(const ros::Time& time, const ros::Duration& period) override; const char* name() const override { return "SIT_DOWN"; diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h index 446e4a92..e9c70cee 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h @@ -6,6 +6,7 @@ #include #include +#include #include "bipedal_wheel_controller/controller_mode/mode_base.h" #include "bipedal_wheel_controller/definitions.h" @@ -15,25 +16,32 @@ namespace rm_chassis_controllers { class StandUp : public ModeBase { + struct StandUpLegCommand + { + double desired_length; + double desired_angle; + double desired_angle_vel; + }; + public: - StandUp(const std::vector& joint_handles, + StandUp(BipedalControllerInterface* controller_, const std::vector& joint_handles, const std::vector& pid_legs, const std::vector& pid_thetas); - void execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) override; + void execute(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); + void setUpLegMotion(const Eigen::Matrix& x, const LegOrientation& other_leg_orientation, + const double& leg_length, const double& leg_theta, LegOrientation& leg_orientation, + StandUpLegCommand& legCommand, bool& stop_flag, bool& arrive_flag, ros::Time arrive_time); /** * Detect the leg state before stand up: UNDER, FRONT, BEHIND * @param x - * @param leg_state + * @param leg_orientation */ - void detectLegState(const Eigen::Matrix& x, int& leg_state); + void detectLegState(const Eigen::Matrix& x, LegOrientation& leg_orientation); /** * Compute the leg command using PID controllers * @param desired_length @@ -47,18 +55,20 @@ class StandUp : public ModeBase * @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); + inline LegCommand computePidLegCommand(const StandUpLegCommand& leg_command, const VMCPtr& vmc_, + control_toolbox::Pid& length_pid, control_toolbox::Pid& angle_pid, + control_toolbox::Pid& angle_vel_pid, const LegOrientation& leg_orientation, + 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; + LegOrientation left_leg_orientation, right_leg_orientation; double theta_des_l, theta_des_r, length_des_l, length_des_r; - double spring_force_{}; + StandUpLegCommand left_leg_command_, right_leg_command_; bool left_stop_{ false }, right_stop_{ false }; std::shared_ptr leg_state_threshold_; + ros::Time left_arrive_time_, right_arrive_time_; + bool left_arrive_flag_{ false }, right_arrive_flag_{ false }; VMCPtr vmcPtr_; + std::shared_ptr> ramp_length_des_l_, ramp_length_des_r_, ramp_angle_des_l_, ramp_angle_des_r_; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/upstairs.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/upstairs.h index 9ad26052..94682fd6 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/upstairs.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/upstairs.h @@ -17,9 +17,9 @@ namespace rm_chassis_controllers class Upstairs : public ModeBase { public: - Upstairs(const std::vector& joint_handles, + Upstairs(BipedalControllerInterface* controller_, const std::vector& joint_handles, const std::vector& pid_legs, const std::vector& pid_thetas); - void execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) override; + void execute(const ros::Time& time, const ros::Duration& period) override; const char* name() const override { return "Upstairs"; @@ -31,17 +31,15 @@ class Upstairs : public ModeBase * @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); + void detectLegState(const Eigen::Matrix& x, LegOrientation& leg_state); + inline LegCommand computePidLegCommand(double desired_length, double desired_angle, const VMCPtr& vmc_, + control_toolbox::Pid& length_pid, control_toolbox::Pid& angle_pid, + control_toolbox::Pid& angle_vel_pid, const LegOrientation& 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; + LegOrientation left_leg_orientation, right_leg_orientation; std::shared_ptr leg_state_threshold_; VMCPtr vmcPtr_; }; diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h index 5cf127c8..6e4b7dc4 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h @@ -4,11 +4,15 @@ #pragma once +#include "bipedal_wheel_controller/vmc/VMC.h" #include #include namespace rm_chassis_controllers { +constexpr static const int STATE_DIM = 6; +constexpr static const int CONTROL_DIM = 2; + struct ModelParams { double L_weight; // Length weight to wheel axis @@ -22,17 +26,26 @@ struct ModelParams double i_m; // Body inertia double r; // Wheel radius double g; // Gravity acceleration - double f_spring; // Spring Force double f_gravity; // Gravity Force }; +struct ChassisGeometryParams +{ + double chassis_height; // 底盘高度 (m) + double wheel_track; // 轮距(左右) +}; + +struct SpringParams +{ + double s2; + double s3; + double alpha_s; + double f_spring; // Spring Force +}; + struct ControlParams { double jumpOverTime_; - double p1_; - double p2_; - double p3_; - double p4_; }; struct BiasParams @@ -54,7 +67,11 @@ struct LegStateThresholdParams double behind_lower; double behind_upper; double upstair_des_theta; - double upstair_exit_threshold; + double upstair_des_length; + double upstair_exit_theta_threshold; + double upstair_exit_length_threshold; + double unstick_threshold; + double arrive_time_threshold; }; struct LegCommand @@ -64,7 +81,7 @@ struct LegCommand double input[2]; // input }; -enum LegState +enum LegOrientation { UNDER, FRONT, @@ -86,9 +103,10 @@ enum BalanceMode SIT_DOWN, RECOVER, UPSTAIRS, + PROTECT }; -enum +enum Side { LEFT = 0, RIGHT, @@ -96,15 +114,41 @@ enum enum { - LEG_T = 0, - LEG_Tp + WHEEL_T = 0, + LEG_Tp, }; -constexpr std::array, 3> jumpLengthDes = { - { { JumpPhase::LEG_RETRACTION, 0.12 }, { JumpPhase::JUMP_UP, 0.38 }, { JumpPhase::OFF_GROUND, 0.15 } } +enum +{ + THETA = 0, + D_THETA, + POS, + VEL, + PITCH, + D_PITCH, }; -constexpr static const int STATE_DIM = 6; -constexpr static const int CONTROL_DIM = 2; +struct LegState +{ + Eigen::Matrix x; // LQR状态量 + double angle[2]; // [0]: hip, [1]: knee + VMCPtr vmc{ nullptr }; + bool unstick = false; +}; +struct ChassisState +{ + geometry_msgs::Vector3 angular_vel; + geometry_msgs::Vector3 linear_acc; + double x_vel = 0.0; + double roll = 0.0; + double pitch = 0.0; + double yaw = 0.0; + double yaw_total = 0.0; + double yaw_total_last = 0.0; +}; + +constexpr std::array, 3> jumpLengthDes = { + { { JumpPhase::LEG_RETRACTION, 0.11 }, { JumpPhase::JUMP_UP, 0.34 }, { JumpPhase::OFF_GROUND, 0.11 } } +}; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h index dc800130..fe276e77 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h @@ -15,11 +15,25 @@ #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 { +static inline double get_LM(const double& l) +{ + return 0.218f * l + 0.075f; +}; + +static inline double get_i_p(const double& l) +{ + return 0.4f * l + 0.07f; +}; + +static inline double get_theta_leg_offset(const double& l) +{ + return M_PI_4 / 2; +} + /** * Generate continuous-time state space matrices A and B * @param model_params @@ -31,21 +45,31 @@ inline void generateAB(const std::shared_ptr& model_params, Eigen:: 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; + // double L = leg_length * model_params->L_weight; + // double Lm = leg_length * model_params->Lm_weight; + double Lm = get_LM(leg_length); + double L = leg_length - Lm; + double i_p = get_i_p(leg_length); + // 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); + // 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(model_params->i_m, 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, i_p, model_params->i_w, L, Lm, model_params->M, model_params->r, model_params->l, + model_params->m_p, model_params->m_w, B); + // clang-format off a<< 0. ,1.,0.,0.,0. ,0., A[1],0.,0.,0.,A[25],0., @@ -62,78 +86,6 @@ inline void generateAB(const std::shared_ptr& model_params, Eigen:: // 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 @@ -171,4 +123,12 @@ inline void quatToRPY(const geometry_msgs::Quaternion& q, double& roll, double& 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); } +inline void clamp(double& val, const double& minVal, const double& maxVal) +{ + if (val < minVal) + val = minVal; + if (val > maxVal) + val = maxVal; +} + } // 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 index a2d06bfa..39a48ae1 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/series_legged_vmc_controller.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/series_legged_vmc_controller.h @@ -12,11 +12,7 @@ #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 #include "bipedal_wheel_controller/vmc/VMC.h" @@ -44,6 +40,24 @@ class VMCController : public controller_interface::MultiInterfaceControllerdata; } + static inline double get_LM(const double& l) + { + return 0.218f * l + 0.075f; + }; + + static inline double get_theta_leg_offset(const double& l) + { + return M_PI_4 / 2; + } + + const double g_{ 9.81 }; + + double f_spring_force(double L0); + double s2_{}, s3_{}, alpha_s_{}; + + bool leg_gravity_compensation_debug_{ false }; + double leg_mass_{ 1.5 }; + hardware_interface::JointHandle jointThigh_, jointKnee_; control_toolbox::Pid pidLength_, pidAngle_; @@ -54,6 +68,7 @@ class VMCController : public controller_interface::MultiInterfaceController debugPub_; }; } // 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 index ee7deef5..84c547c9 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h @@ -7,6 +7,24 @@ namespace rm_chassis_controllers { +struct LegPos +{ + double L0; // Leg length + double theta; // Leg angle +}; + +struct LegSpd +{ + double dL0; // Leg length rate + double dTheta; // Leg angle rate +}; + +struct LegForce +{ + double F; // Force along leg length (radial) + double Tp; // Torque/Force corresponding to leg angle (tangential) +}; + class VMC { public: @@ -17,27 +35,23 @@ class VMC * @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). + * in polar coordinates (length L0 and angle theta) and updates the internal position state. * * @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; + void leg_pos(double phi1, double phi4); /** * @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. + * by mapping joint velocities through the Jacobian and updates the internal velocity state. * * @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]); + void leg_spd(double dphi1, double dphi4); /** * @brief Convert Cartesian forces/torques to joint torques. @@ -45,13 +59,68 @@ class VMC * 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 F Force along the leg length (radial force). + * @param Tp Torque/Force corresponding to the leg angle (tangential force/torque). + * @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 T[2]); + + /** + * @brief Convert joint torques back to Cartesian/Virtual forces. + * + * This function performs the inverse mapping of leg_conv, converting the torques + * applied at the joints (T1, T2) into the equivalent virtual forces acting on + * the leg end-effector in polar coordinates, and updates the internal force state. + * + * @param T1 Torque applied to the first joint (e.g., hip/thigh). + * @param T2 Torque applied to the second joint (e.g., knee/calf). + */ + void leg_conv_t(double T1, double T2); + + /** + * @brief Calculate and update the internal Jacobian matrix of the leg mechanism. + * + * The Jacobian relates joint velocities to end-effector velocities. + * This function computes the 2x2 Jacobian matrix for the current joint configuration + * and stores it internally for subsequent velocity or force conversions. + * * @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]); + void calc_jacobian(double phi1, double phi4); + + inline double getL1() const + { + return l1_; + } + inline double getL2() const + { + return l2_; + } + inline double getL3() const + { + return l3_; + } + inline double getL4() const + { + return l4_; + } + inline double getL5() const + { + return l5_; + } + inline const LegPos& getPos() const + { + return pos_; + } + inline const LegSpd& getSpd() const + { + return spd_; + } + inline const LegForce getForceReal() const + { + return force_real_; + } private: /** @@ -66,6 +135,13 @@ class VMC */ void calc_jacobian(double phi1, double phi4, double J[2][2]); + double J_[2][2]; + double phi1_, phi4_; + double dphi1_, dphi4_; + LegPos pos_; + LegSpd spd_; + LegForce force_real_; + double l1_, l2_, l3_, l4_, l5_; }; using VMCPtr = std::shared_ptr; 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 faca1374..69a8f01e 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -49,7 +49,16 @@ #include #include #include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include namespace rm_chassis_controllers { @@ -57,9 +66,30 @@ struct Command { geometry_msgs::Twist cmd_vel_; rm_msgs::ChassisCmd cmd_chassis_; - rm_msgs::ChassisActiveSusCmd cmd_active_sus_; ros::Time stamp_; }; + +struct PowerLimitor +{ + double vel_coeff{}; + double effort_coeff{}; + double power_offset{}; + double max_power{}; + double power_in[4]{}; + double cmd_power{}; + double estimated_power{}; + double power_limit[4]{}; + double ratio{ 1.0 }; + double err[4]{}; + double err_upper{}; + double err_lower{}; + double err_sum{}; + double omiga[4]{}; + double torque[4]{}; + double K{}; + double K_angle[4]{}; +}; + template class ChassisBase : public controller_interface::MultiInterfaceController { @@ -123,16 +153,18 @@ class ChassisBase : public controller_interface::MultiInterfaceController /** @brief Set chassis velocity to zero. */ void recovery(); + void fallen(); /** @brief Transform tf velocity to base link frame. * * @param from The father frame. */ - void tfVelToBase(const std::string& from); + void tfVelToBase(const std::string& from, double yaw_offset = 0.); /** @brief To limit the chassis power according to current power limit. * * Receive power limit from command. Set max_effort command to chassis to avoid exceed power limit. */ - void powerLimit(); + virtual void powerLimit(); + virtual void updatePowerStatus(); /** @brief Write current command from rm_msgs::ChassisCmd. * * @param msg This message contains various state parameter settings for basic chassis control @@ -143,49 +175,95 @@ class ChassisBase : public controller_interface::MultiInterfaceController * @param msg This expresses velocity in free space broken into its linear and angular parts. */ 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); + + void initialize_parameters(ros::NodeHandle& controller_nh); + void slamCallback(const nav_msgs::Odometry::ConstPtr& msg); + void localizationCallback(const geometry_msgs::TransformStamped::ConstPtr& msg); + void capacityCallback(const rm_msgs::PowerManagementSampleAndStatusData::ConstPtr& msg); rm_control::RobotStateHandle robot_state_handle_{}; hardware_interface::EffortJointInterface* effort_joint_interface_{}; - std::vector joint_handles_{}; - - double wheel_radius_{}, publish_rate_{}, twist_angular_{}, timeout_{}, effort_coeff_{}, velocity_coeff_{}, - power_offset_{}; - double roll_ = 0., pitch_ = 0., yaw_ = 0.; - double pitch_angle_threshold_ = 0., scale_ = 0.; - double max_odom_vel_; - bool enable_uphill_acceleration_ = false; - bool enable_odom_tf_ = false; - bool topic_update_ = false; - bool publish_odom_tf_ = false; - bool state_changed_ = true; + std::vector wheel_joint_handles_{}; + realtime_tools::RealtimeBuffer cmd_rt_buffer_{}; + realtime_tools::RealtimeBuffer slam_rt_buffer_{}; + realtime_tools::RealtimeBuffer localization_rt_buffer_{}; + std::unique_ptr> odometry_rt_pub_; + std::unique_ptr> cpower_pub_; // command power publisher + std::unique_ptr> epower_pub_; // estimated power publisher + std::unique_ptr> chassis_power_pub_; // chassis power publisher + + rm_common::TfRtBroadcaster brcst4global_map2robot_odom_{}; + rm_common::TfRtBroadcaster brcst4robot_odom2robot_base_{}; + rm_common::TfRtBroadcaster brcst4global_map2camera_init_{}; + + geometry_msgs::TransformStamped global_map2robot_odom_{}; + geometry_msgs::TransformStamped robot_odom2robot_base_{}; + geometry_msgs::TransformStamped robot_base2lidar_base_{}; + geometry_msgs::TransformStamped global_map2camera_init_{}; + + tf2::Transform T_global_map2robot_odom_{}; + tf2::Transform T_robot_odom_2robot_base_{}; + tf2::Transform T_lidar_odom2lidar_base_{}; + tf2::Transform T_robot_base2lidar_base_{}; + tf2::Transform T_global_map2lidar_odom_{}; + + ros::Subscriber cmd_vel_sub_; + ros::Subscriber cmd_chassis_sub_; + ros::Subscriber slam_sub_; + ros::Subscriber localization_sub_; + ros::Subscriber capacity_sub_; + + std::unique_ptr> ramp_x_{ nullptr }; + std::unique_ptr> ramp_y_{ nullptr }; + std::unique_ptr> ramp_w_{ nullptr }; + + double roll_{ 0. }, pitch_{ 0. }, yaw_{ 0. }; + + double publish_rate_{ 100.0 }; + bool publish_map_tf_{ false }; + bool publish_odom_tf_{ false }; + + double wheel_radius_{ 0.02 }; + double twist_angular_{ M_PI / 6 }; + double max_odom_vel_{ 10.0 }; + double timeout_{ 0.1 }; + double raw_yaw_feedforward_k_{ 0.0 }; + double chassis_power_{ 0.0 }; + bool capacity_update_flag_{ false }; + bool use_rls_{ false }; + bool use_K_angle_{ false }; + + bool gravity_estimation_offset_{ false }; + bool odom_initialized_{ false }; + bool slam_updated_{ false }; + bool localization_updated_{ false }; + bool state_changed_{ true }; + int state_{ RAW }; + + std::string follow_source_frame_{}; + std::string command_source_frame_{}; + std::string global_map_frame_id_{ "map" }; + std::string robot_odom_frame_id_{ "odom" }; + std::string robot_base_frame_id_{ "base_link" }; + std::string lidar_base_frame_id_{ "livox_frame" }; + std::string slam_topic_{ "/Odometry" }; + std::string localization_topic_{ "/hdl_global_localization/result" }; + std::string capacity_topic_{ "/rm_referee/power_management/sample_and_status" }; + + ros::Time last_publish_time_{}; + geometry_msgs::Vector3 vel_cmd_{}; // x, y + control_toolbox::Pid pid_follow_{}; + + Command cmd_struct_{}; + PowerLimitor wheel_power_limitor_{}; + enum { RAW, FOLLOW, - TWIST + TWIST, + FALLEN = 4 }; - int state_ = RAW; - RampFilter*ramp_x_{}, *ramp_y_{}, *ramp_w_{}; - std::string follow_source_frame_{}, command_source_frame_{}; - - ros::Time last_publish_time_; - geometry_msgs::TransformStamped odom2base_{}; - tf2::Transform world2odom_; - 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_; - ros::Subscriber cmd_chassis_sub_; - ros::Subscriber cmd_vel_sub_; - Command cmd_struct_; - realtime_tools::RealtimeBuffer cmd_rt_buffer_; - realtime_tools::RealtimeBuffer odom_buffer_; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h index 1bc67336..8c482f03 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h @@ -5,6 +5,8 @@ #pragma once #include +#include +#include #include "rm_chassis_controllers/chassis_base.h" @@ -16,14 +18,20 @@ class OmniController : public ChassisBase> joints_; Eigen::MatrixXd chassis2joints_; + void powerLimit() override; + void updatePowerStatus() override; + virtual void stateJudge(); -protected: - void moveJoint(const ros::Time& time, const ros::Duration& period) override; + std::array motor_lp_filters_{}; + std::unique_ptr> rls_{}; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h index 23ddfb94..bdf775b3 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h @@ -39,8 +39,15 @@ #include "rm_chassis_controllers/chassis_base.h" +#include +#include #include #include +#include +#include +#include +#include +#include namespace rm_chassis_controllers { @@ -52,7 +59,8 @@ struct Module effort_controllers::JointVelocityController* ctrl_wheel_; }; -class SwerveController : public ChassisBase +class SwerveController : public ChassisBase { public: SwerveController() = default; @@ -61,7 +69,18 @@ class SwerveController : public ChassisBase modules_; + void powerLimit() override; + void updatePowerStatus() override; + void getBaseGyro(); + void stateJudge(); + + std::vector modules_{}; + std::vector pivot_joint_handles_{}; + std::unique_ptr> base_gyro_pub_; + + PowerLimitor pivot_power_limitor_{}; + std::array, 2> motor_lp_filters_{}; + std::unique_ptr> rls_{}; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/balance.cpp b/rm_chassis_controllers/src/balance.cpp index 93b9bb73..dc5f0d7c 100644 --- a/rm_chassis_controllers/src/balance.cpp +++ b/rm_chassis_controllers/src/balance.cpp @@ -32,8 +32,8 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan } left_wheel_joint_handle_ = robot_hw->get()->getHandle(left_wheel_joint); right_wheel_joint_handle_ = robot_hw->get()->getHandle(right_wheel_joint); - joint_handles_.push_back(left_wheel_joint_handle_); - joint_handles_.push_back(right_wheel_joint_handle_); + wheel_joint_handles_.push_back(left_wheel_joint_handle_); + wheel_joint_handles_.push_back(right_wheel_joint_handle_); left_momentum_block_joint_handle_ = robot_hw->get()->getHandle(left_momentum_block_joint); right_momentum_block_joint_handle_ = diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp index 38fb571e..5c064620 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp @@ -11,11 +11,6 @@ #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, @@ -24,6 +19,7 @@ bool BipedalController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan ChassisBase::init(robot_hw, root_nh, controller_nh); imu_handle_ = robot_hw->get()->getHandle("base_imu"); + // gimbal_imu_handle_ = robot_hw->get()->getHandle("gimbal_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_ }, @@ -36,15 +32,13 @@ bool BipedalController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan 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)) + if (!setupParams(controller_nh)) + { + ROS_ERROR("[balance] Failed to setup parameters"); return false; + } + + mode_manager_ = std::make_shared(this, controller_nh, joint_handles_); d_srv_ = new dynamic_reconfigure::Server(ros::NodeHandle(controller_nh, "lqr")); @@ -56,24 +50,24 @@ bool BipedalController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan legCmd_ = msg->leg_length; jumpCmd_ = msg->jump; }; - leg_cmd_sub_ = controller_nh.subscribe("/leg_cmd", 10, legCmdCallback); - + auto recoveryLegSpdTurnbackCb = [this](const std_msgs::Bool::ConstPtr& msg) { setRecoveryLegSpdTurnback(msg->data); }; + leg_cmd_sub_ = controller_nh.subscribe("/leg_cmd", 5, legCmdCallback); + recovery_leg_spd_turnback_sub_ = + controller_nh.subscribe("/recovery_leg_spd_turnback", 1, recoveryLegSpdTurnbackCb); 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_status_pub_.reset( + (new realtime_tools::RealtimePublisher(controller_nh, "legged_chassis_status", 1))); legged_chassis_mode_pub_.reset( - (new realtime_tools::RealtimePublisher(controller_nh, "legged_chassis_mode", 10))); + (new realtime_tools::RealtimePublisher(controller_nh, "legged_chassis_mode", 1))); 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(); + (new realtime_tools::RealtimePublisher(controller_nh, "lqr_status", 1))); + leg_state_[LEFT].x.setZero(); + leg_state_[RIGHT].x.setZero(); // Slippage detection - A_ << 1, 0.0, 0, 1; + A_ << 1, 0.001f, 0, 1; H_ << 1, 0, 0, 1; Q_ << 1, 0, 0, 1; R_ << 200, 0, 0, 200; @@ -87,11 +81,7 @@ bool BipedalController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan kalmanFilterPtr_ = std::make_shared>(A_, B_, H_, Q_, R_); kalmanFilterPtr_->clear(X_); - // left_leg_angle_lpFilterPtr_ = std::make_shared(100); - // right_leg_angle_lpFilterPtr_ = std::make_shared(100); - - left_leg_angle_vel_lpFilterPtr_ = std::make_shared(60); - right_leg_angle_vel_lpFilterPtr_ = std::make_shared(60); + debugPub_ = std::make_shared(controller_nh, "debug_data"); return true; } @@ -111,13 +101,13 @@ void BipedalController::moveJoint(const ros::Time& time, const ros::Duration& pe mode_manager_->switchMode(RECOVER); } updateEstimation(time, period); - mode_manager_->getModeImpl()->execute(this, time, period); + mode_manager_->getModeImpl()->execute(time, period); pubState(); } void BipedalController::clearStatus() { - x_left_(2) = x_right_(2) = -bias_params_->x; + leg_state_[LEFT].x(2) = leg_state_[RIGHT].x(2) = 0; } void BipedalController::updateEstimation(const ros::Time& time, const ros::Duration& period) @@ -155,105 +145,122 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat tf2::Vector3 z_body(0, 0, 1); tf2::Vector3 z_world = tf2::quatRotate(odom2base.getRotation(), z_body); - overturn_ = (abs(pitch) > 0.65 || abs(roll) > 0.4) && z_world.z() < 0.0; + overturn_ = (abs(pitch) > 0.65 || abs(roll) > 0.8) && z_world.z() < 0.0; + + chassis_state_.angular_vel = angular_vel_base; + chassis_state_.linear_acc = linear_acc_base; + chassis_state_.roll = roll; + chassis_state_.pitch = pitch; + chassis_state_.yaw = yaw; } catch (tf2::TransformException& ex) { - ROS_WARN("%s", ex.what()); + ROS_WARN_ONCE("%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]{}; + double left_angle[2]{}, right_angle[2]{}; + + // double 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(); + // left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; + // left_angle[1] = left_knee_joint_handle_.getPosition(); + // right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; + // right_angle[1] = right_knee_joint_handle_.getPosition(); + + // gazebo + left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; + left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; + right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; + right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; + + // left vmc calc + leg_state_[LEFT].vmc->calc_jacobian(left_angle[0], left_angle[1]); + leg_state_[LEFT].vmc->leg_pos(left_angle[0], left_angle[1]); + leg_state_[LEFT].vmc->leg_spd(left_hip_joint_handle_.getVelocity(), left_knee_joint_handle_.getVelocity()); + leg_state_[LEFT].vmc->leg_conv_t(left_hip_joint_handle_.getEffort(), left_knee_joint_handle_.getEffort()); + + // right vmc calc + leg_state_[RIGHT].vmc->calc_jacobian(right_angle[0], right_angle[1]); + leg_state_[RIGHT].vmc->leg_pos(right_angle[0], right_angle[1]); + leg_state_[RIGHT].vmc->leg_spd(right_hip_joint_handle_.getVelocity(), right_knee_joint_handle_.getVelocity()); + leg_state_[RIGHT].vmc->leg_conv_t(right_hip_joint_handle_.getEffort(), right_knee_joint_handle_.getEffort()); + + const LegPos& left_pos = leg_state_[LEFT].vmc->getPos(); + const LegSpd& left_spd = leg_state_[LEFT].vmc->getSpd(); + const LegPos& right_pos = leg_state_[RIGHT].vmc->getPos(); + const LegSpd& right_spd = leg_state_[RIGHT].vmc->getSpd(); + const LegForce& left_F_real = leg_state_[LEFT].vmc->getForceReal(); + const LegForce& right_F_real = leg_state_[RIGHT].vmc->getForceReal(); // 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_); + leftWheelVel = (left_wheel_joint_handle_.getVelocity() + angular_vel_base.y + left_spd.dTheta) * wheel_radius_; + rightWheelVel = (right_wheel_joint_handle_.getVelocity() + angular_vel_base.y + right_spd.dTheta) * wheel_radius_; + leftWheelVelAbsolute = leftWheelVel + left_pos.L0 * left_spd.dTheta * cos(left_pos.theta + pitch) + + left_spd.dL0 * sin(left_pos.theta + pitch); + rightWheelVelAbsolute = rightWheelVel + right_pos.L0 * right_spd.dTheta * cos(right_pos.theta + pitch) + + right_spd.dL0 * sin(right_pos.theta + pitch); double wheel_vel_aver = (leftWheelVelAbsolute + rightWheelVelAbsolute) / 2.; R_(0, 0) = slip_flag_ ? slip_R_wheel_ : R_wheel_; - if (i >= sample_times_) + if (itor >= sample_times_) { // oversampling - i = 0; + static double last_linear_acc_base_x = linear_acc_base.x; + itor = 0; X_(0) = wheel_vel_aver; - X_(1) = linear_acc_base.x; + X_(1) = 0.2 * last_linear_acc_base_x + 0.8 * linear_acc_base.x; + last_linear_acc_base_x = X_(1); kalmanFilterPtr_->predict(U_); kalmanFilterPtr_->update(X_, R_); } else { kalmanFilterPtr_->predict(U_); - i++; + itor++; } 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) + leg_state_[LEFT].x[3] = state_ != RAW ? x_hat_vel(0) : 0; + // leg_state_[LEFT].x[3] = state_ != RAW ? wheel_vel_aver : 0; + if (state_ != RAW && abs(leg_state_[LEFT].x[3]) <= 0.5f && abs(vel_cmd_.x) <= 0.01f) { - x_left_[2] += state_ != RAW ? x_left_[3] * period.toSec() : 0; + leg_state_[LEFT].x[2] += state_ != RAW ? leg_state_[LEFT].x[3] * period.toSec() : 0; } else { - x_left_[2] = -bias_params_->x; + setMoveFlag(true); + leg_state_[LEFT].x[2] = 0.0; } - x_left_[0] = (left_pos[1] + pitch); - x_left_[1] = left_spd[1] + angular_vel_base.y; - x_left_[4] = -pitch; - x_left_[5] = -angular_vel_base.y; - x_right_ = x_left_; - x_right_[0] = (right_pos[1] + pitch); - x_right_[1] = right_spd[1] + angular_vel_base.y; + leg_state_[LEFT].x[0] = (left_pos.theta + pitch); + leg_state_[LEFT].x[1] = left_spd.dTheta + angular_vel_base.y; + leg_state_[LEFT].x[4] = -pitch; + leg_state_[LEFT].x[5] = -angular_vel_base.y; + leg_state_[RIGHT].x = leg_state_[LEFT].x; + leg_state_[RIGHT].x[0] = (right_pos.theta + pitch); + leg_state_[RIGHT].x[1] = right_spd.dTheta + angular_vel_base.y; + + chassis_state_.x_vel = x_hat_vel(0); if (legged_chassis_status_pub_->trylock()) { + legged_chassis_status_pub_->msg_.linear_acc_base.clear(); 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_.pitch = leg_state_[LEFT].x[4]; + legged_chassis_status_pub_->msg_.d_pitch = leg_state_[LEFT].x[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_.left_leg_length = left_pos.L0; + legged_chassis_status_pub_->msg_.right_leg_length = right_pos.L0; + legged_chassis_status_pub_->msg_.x = leg_state_[LEFT].x[2]; + legged_chassis_status_pub_->msg_.x_dot = leg_state_[LEFT].x[3]; + legged_chassis_status_pub_->msg_.left_leg_theta = leg_state_[LEFT].x[0]; + legged_chassis_status_pub_->msg_.left_leg_theta_dot = leg_state_[LEFT].x[1]; + legged_chassis_status_pub_->msg_.right_leg_theta = leg_state_[RIGHT].x[0]; + legged_chassis_status_pub_->msg_.right_leg_theta_dot = leg_state_[RIGHT].x[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); @@ -267,9 +274,12 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat 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); + debugPub_->add("left_spring_force", f_spring_force(left_pos.L0)); + debugPub_->add("right_spring_force", f_spring_force(right_pos.L0)); + debugPub_->add("left_F_real", left_F_real.F); + debugPub_->add("right_F_real", right_F_real.F); + debugPub_->add("wheel_vel_aver", wheel_vel_aver); + debugPub_->publish(); } void BipedalController::pubState() @@ -282,12 +292,28 @@ void BipedalController::pubState() void BipedalController::stopping(const ros::Time& time) { balance_mode_ = BalanceMode::SIT_DOWN; - balance_state_changed_ = false; + setStateChange(false); setJointCommands(joint_handles_, { 0, 0, { 0., 0. } }, { 0, 0, { 0., 0. } }); ROS_INFO("[balance] Controller Stop"); } +bool BipedalController::setupParams(ros::NodeHandle& controller_nh) +{ + model_params_ = std::make_shared(); + control_params_ = std::make_shared(); + bias_params_ = std::make_shared(); + leg_threshold_params_ = std::make_shared(); + spring_params_ = std::make_shared(); + chassis_geometry_params_ = std::make_shared(); + + if (!setupModelParams(controller_nh) || !setupLQR(controller_nh) || !setupBiasParams(controller_nh) || + !setupControlParams(controller_nh) || !setupThresholdParams(controller_nh) || !setupSpringParams(controller_nh) || + !setupChassisGeometryParams(controller_nh)) + return false; + return true; +} + bool BipedalController::setupModelParams(ros::NodeHandle& controller_nh) { const std::pair tbl[] = { { "m_w", &model_params_->m_w }, @@ -301,7 +327,6 @@ bool BipedalController::setupModelParams(ros::NodeHandle& controller_nh) { "Lm_weight", &model_params_->Lm_weight }, { "g", &model_params_->g }, { "wheel_radius", &model_params_->r }, - { "spring_force", &model_params_->f_spring }, { "gravity_force", &model_params_->f_gravity } }; for (const auto& e : tbl) @@ -317,7 +342,8 @@ bool BipedalController::setupModelParams(ros::NodeHandle& controller_nh) 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); + leg_state_[LEFT].vmc = std::make_shared(l1, l2); + leg_state_[RIGHT].vmc = std::make_shared(l1, l2); if (!controller_nh.getParam("default_leg_length", default_leg_length_)) { @@ -328,6 +354,21 @@ bool BipedalController::setupModelParams(ros::NodeHandle& controller_nh) return true; } +bool BipedalController::setupChassisGeometryParams(ros::NodeHandle& controller_nh) +{ + const std::pair tbl[] = { { "wheel_track", &chassis_geometry_params_->wheel_track }, + { "chassis_high", &chassis_geometry_params_->chassis_height } }; + + for (const auto& e : tbl) + if (!controller_nh.getParam(e.first, *e.second)) + { + ROS_ERROR("Param %s not given (namespace: %s)", e.first, controller_nh.getNamespace().c_str()); + return false; + } + + return true; +} + bool BipedalController::setupLQR(ros::NodeHandle& controller_nh) { // Set up weight matrices @@ -351,7 +392,7 @@ bool BipedalController::setupLQR(ros::NodeHandle& controller_nh) std::vector> ks; for (int i = 10; i < 40; i++) { - double length = i / 100.; + double length = i / 100.0f; lengths.push_back(length); Eigen::Matrix a{}; Eigen::Matrix b{}; @@ -363,6 +404,12 @@ bool BipedalController::setupLQR(ros::NodeHandle& controller_nh) return false; } Eigen::Matrix k = lqr.getK(); + if (length == 0.2f) + { + std::cout << "A: " << std::endl << a << std::endl; + std::cout << "B: " << std::endl << b << std::endl; + std::cout << "len: 0.2m LQR k: " << std::endl << k << std::endl; + } ks.push_back(k); } polyfit(ks, lengths, coeffs_); @@ -401,11 +448,9 @@ bool BipedalController::setupBiasParams(ros::NodeHandle& controller_nh) // [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_)) + if (!controller_nh.getParam("jumpOverTime", control_params_->jumpOverTime_)) { - ROS_ERROR("Load param fail, check the resist of jump_over_time, p1, p2, p3, p4"); + ROS_ERROR("Load param fail, check the resist of jump_over_time"); return false; } return true; @@ -413,19 +458,44 @@ bool BipedalController::setupControlParams(ros::NodeHandle& controller_nh) 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; - } + const std::pair tbl[] = { + { "under_lower_threshold", &leg_threshold_params_->under_lower }, + { "under_upper_threshold", &leg_threshold_params_->under_upper }, + { "front_lower_threshold", &leg_threshold_params_->front_lower }, + { "front_upper_threshold", &leg_threshold_params_->front_upper }, + { "behind_lower_threshold", &leg_threshold_params_->behind_lower }, + { "behind_upper_threshold", &leg_threshold_params_->behind_upper }, + { "upstair_exit_theta_threshold", &leg_threshold_params_->upstair_exit_theta_threshold }, + { "upstair_exit_length_threshold", &leg_threshold_params_->upstair_exit_length_threshold }, + { "upstair_des_theta", &leg_threshold_params_->upstair_des_theta }, + { "upstair_des_length", &leg_threshold_params_->upstair_des_length }, + { "unstick_threshold", &leg_threshold_params_->unstick_threshold }, + { "arrive_time_threshold", &leg_threshold_params_->arrive_time_threshold } + }; + for (const auto& e : tbl) + if (!controller_nh.getParam(e.first, *e.second)) + { + ROS_ERROR("Param %s not given (namespace: %s)", e.first, controller_nh.getNamespace().c_str()); + return false; + } + return true; +} + +bool BipedalController::setupSpringParams(ros::NodeHandle& controller_nh) +{ + const std::pair tbl[] = { + { "spring_s2", &spring_params_->s2 }, + { "spring_s3", &spring_params_->s3 }, + { "spring_alpha_s", &spring_params_->alpha_s }, + { "spring_force", &spring_params_->f_spring }, + }; + + 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; } @@ -448,8 +518,8 @@ 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(); + twist.linear.x = chassis_state_.x_vel; + twist.angular.z = chassis_state_.angular_vel.z; } else { @@ -470,6 +540,10 @@ void BipedalController::pubLQRStatus(Eigen::Matrix left_er if (lqr_status_pub_->trylock()) { int temp{}; + lqr_status_pub_->msg_.left_leg_error.clear(); + lqr_status_pub_->msg_.right_leg_error.clear(); + lqr_status_pub_->msg_.left_leg_ref.clear(); + lqr_status_pub_->msg_.right_leg_ref.clear(); for (temp = 0; temp < 6; ++temp) { lqr_status_pub_->msg_.left_leg_error.push_back(left_error(temp)); @@ -477,6 +551,10 @@ void BipedalController::pubLQRStatus(Eigen::Matrix left_er lqr_status_pub_->msg_.left_leg_ref.push_back(left_ref(temp)); lqr_status_pub_->msg_.right_leg_ref.push_back(right_ref(temp)); } + lqr_status_pub_->msg_.left_leg_u.clear(); + lqr_status_pub_->msg_.right_leg_u.clear(); + lqr_status_pub_->msg_.F_leg.clear(); + lqr_status_pub_->msg_.unstick.clear(); for (temp = 0; temp < 2; ++temp) { lqr_status_pub_->msg_.left_leg_u.push_back(u_left(temp)); @@ -542,6 +620,34 @@ void BipedalController::reconfigCB(rm_chassis_controllers::LQRWeightConfig& conf ks.push_back(k); } polyfit(ks, lengths, coeffs_); + + Matrix k; + k.setZero(); + + for (int i = 0; i < 2; ++i) + { + for (int j = 0; j < 6; ++j) + { + k(i, j) = coeffs_(0, i + 2 * j) * pow(0.2, 3) + coeffs_(1, i + 2 * j) * pow(0.2, 2) + + coeffs_(2, i + 2 * j) * 0.2 + coeffs_(3, i + 2 * j); + } + } + std::cout << "len: 0.2m LQR k: " << std::endl << k << std::endl; +} + +double BipedalController::f_spring_force(double L0) +{ + static double l1 = leg_state_[LEFT].vmc->getL1(), l2 = leg_state_[LEFT].vmc->getL2(); + static double Fs = spring_params_->f_spring, s2 = spring_params_->s2, s3 = spring_params_->s3, + alpha_s = spring_params_->alpha_s; + double cos_theta3, theta3, ls, Fv; + cos_theta3 = (l1 * l1 + l2 * l2 - L0 * L0) / (2 * l1 * l2); + theta3 = acos(cos_theta3); + ls = sqrt(s2 * s2 + s3 * s3 - 2 * s2 * s3 * cos(theta3 - alpha_s)); + Fv = Fs * (L0 * s2 * s3 * sin(theta3 - alpha_s)) / (ls * l1 * l2 * sin(theta3)); + return Fv; + + // return ((2094.45f * L0 - 3091.28f) * L0 + 1408.375f) * L0 - 80.91f; } } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_base.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_base.cpp index ffc07e83..00e8bb2d 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_base.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_base.cpp @@ -6,37 +6,6 @@ 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; diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp index 0c31ea7a..3edcc1c0 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp @@ -6,7 +6,7 @@ namespace rm_chassis_controllers { -ModeManager::ModeManager(ros::NodeHandle& controller_nh, +ModeManager::ModeManager(BipedalControllerInterface* controller, ros::NodeHandle& controller_nh, const std::vector& joint_handles) { const std::pair pids[] = { @@ -23,6 +23,7 @@ ModeManager::ModeManager(ros::NodeHandle& controller_nh, { "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_ }, + { "pid_wheel_vel_diff", &pid_wheel_vel_diff_ }, }; for (const auto& e : pids) if (controller_nh.hasParam(e.first) && !e.second->init(ros::NodeHandle(controller_nh, e.first))) @@ -38,14 +39,22 @@ ModeManager::ModeManager(ros::NodeHandle& controller_nh, 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::NORMAL, + std::make_unique(controller, joint_handles, pid_legs_, &pid_yaw_vel_, + &pid_theta_diff_, &pid_roll_, &pid_wheel_vel_diff_))); + mode_map_.insert(std::make_pair(BalanceMode::STAND_UP, std::make_unique(controller, joint_handles, + pid_legs_stand_up_, pid_thetas_))); 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_))); + std::make_pair(BalanceMode::RECOVER, std::make_unique(controller, joint_handles, pid_legs_stand_up_, + pid_thetas_, &pid_theta_diff_))); + mode_map_.insert( + std::make_pair(BalanceMode::SIT_DOWN, std::make_unique(controller, joint_handles, pid_wheels_))); + mode_map_.insert(std::make_pair(BalanceMode::UPSTAIRS, std::make_unique(controller, joint_handles, + pid_legs_stand_up_, pid_thetas_))); + mode_map_.insert(std::make_pair(BalanceMode::UPSTAIRS, std::make_unique(controller, joint_handles, + pid_legs_stand_up_, pid_thetas_))); + mode_map_.insert( + std::make_pair(BalanceMode::PROTECT, std::make_unique(controller, joint_handles, pid_legs_, pid_thetas_, + pid_wheels_, &pid_theta_diff_, &pid_yaw_vel_))); } } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp index 66577b29..07d59e70 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -7,49 +7,73 @@ namespace rm_chassis_controllers { -Normal::Normal(const std::vector& joint_handles, +Normal::Normal(BipedalControllerInterface* controller_, + 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) + control_toolbox::Pid* pid_theta_diff, control_toolbox::Pid* pid_roll, + control_toolbox::Pid* pid_wheel_vel_diff) + : ModeBase(controller_) + , joint_handles_(joint_handles) , pid_legs_(pid_legs) , pid_yaw_vel_(pid_yaw_vel) , pid_theta_diff_(pid_theta_diff) , pid_roll_(pid_roll) + , pid_wheel_vel_diff_(pid_wheel_vel_diff) { leftSupportForceAveragePtr_ = std::make_shared>(4); rightSupportForceAveragePtr_ = std::make_shared>(4); + if (controller_->getLegThresholdParams() != nullptr) + unstick_threshold = controller_->getLegThresholdParams()->unstick_threshold; } -void Normal::execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) +void Normal::execute(const ros::Time& time, const ros::Duration& period) { - auto bias_params_ = controller->getBiasParams(); + const auto& bias_params_ = controller->getBiasParams(); if (!controller->getStateChange()) { ROS_INFO("[balance] Enter NORMAL"); controller->clearStatus(); jump_phase_ = JumpPhase::IDLE; - pos_des_ = 0; + pos_des_ = 0.0f; controller->setStateChange(true); } + const auto& chassis_state = controller->getChassisState(); + auto& left_leg_state = controller->getLegState(LEFT); + auto& right_leg_state = controller->getLegState(RIGHT); + const auto& left_pos = left_leg_state.vmc->getPos(); + const auto& right_pos = right_leg_state.vmc->getPos(); + const auto& left_spd = left_leg_state.vmc->getSpd(); + const auto& right_spd = right_leg_state.vmc->getSpd(); - if (!controller->getCompleteStand() && abs(x_left_[4]) < 0.2 && (abs(x_left_[0] + x_right_[0]) / 2.0f) < 0.15) + if (abs(left_leg_state.x[PITCH]) < 0.2 && (abs(left_leg_state.x[THETA] + right_leg_state.x[THETA]) / 2.0f) < 0.2) { - controller->setCompleteStand(true); + protect_flag_ = false; + if (!controller->getCompleteStand()) + { + controller->setCompleteStand(true); + } } auto vel_cmd_ = controller->getVelCmd(); - if (controller->getMoveFlag() && abs(x_left_[3]) < 0.1 && abs(vel_cmd_.x) < 0.1) + double current_leg_length = (left_pos.L0 + right_pos.L0) / 2.0f; + if (abs(chassis_state.x_vel) < 0.1f && abs(vel_cmd_.x) < 0.01f) { controller->setMoveFlag(false); + if (x_offset_flag_) + { + x_offset_flag_ = false; + pos_des_ = + current_leg_length * sin(-(left_leg_state.x(THETA) + right_leg_state.x(THETA)) / 2.0f) + bias_params_->x; + } } - 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; + double friction_circle = chassis_state.x_vel * chassis_state.angular_vel.z; + double friction_circle_alpha = abs(friction_circle) > 10.0f ? (10.0f / 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); - + double T_yaw = pid_yaw_vel_->computeCommand(friction_circle_alpha * vel_cmd_.z - chassis_state.angular_vel.z, period); + double theta_diff = right_pos.theta - left_pos.theta; + double T_theta_diff = pid_theta_diff_->computeCommand(theta_diff, period); + double F_roll = pid_roll_->computeCommand(0. - chassis_state.roll, period); // LQR Matrix coeffs_ = controller->getCoeffs(); Matrix k_left, k_right; @@ -60,48 +84,96 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const { 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); + k_left(i, j) = coeffs_(0, i + 2 * j) * pow(left_pos.L0, 3) + coeffs_(1, i + 2 * j) * pow(left_pos.L0, 2) + + coeffs_(2, i + 2 * j) * left_pos.L0 + coeffs_(3, i + 2 * j); + k_right(i, j) = coeffs_(0, i + 2 * j) * pow(right_pos.L0, 3) + coeffs_(1, i + 2 * j) * pow(right_pos.L0, 2) + + coeffs_(2, i + 2 * j) * right_pos.L0 + 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_; + auto x_left = left_leg_state.x; + auto x_right = right_leg_state.x; 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_ref(POS) = x_right_ref(POS) = pos_des_; + if (controller->getBaseState() != rm_msgs::ChassisCmd::RAW) + { + x_left_ref(VEL) = x_right_ref(VEL) = friction_circle_alpha * vel_cmd_.x; + } + else + { + // raw move but bug + // x_left_ref(VEL) = x_right_ref(VEL) = vel_cmd_.x; + // x_left_ref(POS) = x_right_ref(POS) = 0.0f; + x_left_ref(VEL) = x_right_ref(VEL) = 0.0f; + } + if (protect_flag_) + { + x_left_ref(VEL) = x_right_ref(VEL) = 0.0f; + leg_length_des = controller->getDefaultLegLength(); + } + else + { + leg_length_des = controller->getLegCmd(); + } + } + else + { + leg_length_des = controller->getDefaultLegLength(); + } + if (controller->getBaseState() != rm_msgs::ChassisCmd::RAW) + { + if (!controller->getMoveFlag()) + { + x_offset_flag_ = true; + x_left(THETA) -= bias_params_->theta; + x_right(THETA) -= bias_params_->theta; + } + } + else + { + x_left(THETA) -= bias_params_->raw_theta; + x_right(THETA) -= bias_params_->raw_theta; + x_left(PITCH) -= bias_params_->raw_pitch; + x_right(PITCH) -= bias_params_->raw_pitch; } - 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; + clamp(x_left(VEL), -1.2f, 1.2f); + clamp(x_right(VEL), -1.2f, 1.2f); + + const double k_pitch = -0.1f, b = 0.35f; + double pitch_error_clamp = k_pitch * chassis_state.x_vel + b; + clamp(x_left(PITCH), -pitch_error_clamp, pitch_error_clamp); + clamp(x_right(PITCH), -pitch_error_clamp, pitch_error_clamp); + clamp(x_left(THETA), -0.6f, 0.6f); + clamp(x_right(THETA), -0.6f, 0.6f); + 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; + double wheel_vel_diff = abs(joint_handles_[4]->getVelocity()) - abs(joint_handles_[5]->getVelocity()); + double gravity = model_params_->f_gravity, left_spring_force = controller->f_spring_force(left_pos.L0), + right_spring_force = controller->f_spring_force(right_pos.L0); + double F_inertia_left = + model_params_->M * friction_circle * left_pos.L0 / controller->getChassisGeometryParams()->wheel_track; + double F_inertia_right = + model_params_->M * friction_circle * right_pos.L0 / controller->getChassisGeometryParams()->wheel_track; + double F_pid_left{}, F_pid_right{}, T_wheel_diff{}; Eigen::Matrix F_leg; - + F_leg.setZero(); // check jump if (jump_phase_ == JumpPhase::IDLE && ros::Time::now() - lastJumpTime_ > ros::Duration(control_params_->jumpOverTime_) && controller->getJumpCmd()) @@ -111,34 +183,42 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const } 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); + static double last_left_length_des = leg_length_des, last_right_length_des = leg_length_des; + double left_length_des = controller->getCompleteStand() ? (0.8 * leg_length_des + 0.2 * last_left_length_des) : + controller->getDefaultLegLength(); + double right_length_des = controller->getCompleteStand() ? (0.8 * leg_length_des + 0.2 * last_right_length_des) : + controller->getDefaultLegLength(); + last_left_length_des = left_length_des; + last_right_length_des = right_length_des; + F_pid_left = pid_legs_[LEFT]->computeCommand(left_length_des - current_leg_length, period); + 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; + F_leg[LEFT] = F_pid_left - F_inertia_left + gravity / cos(left_pos.theta) + F_roll - left_spring_force; + F_leg[RIGHT] = F_pid_right + F_inertia_right + gravity / cos(right_pos.theta) - F_roll - right_spring_force; + T_wheel_diff = controller->getBaseState() == rm_msgs::ChassisCmd::RAW ? + pid_wheel_vel_diff_->computeCommand(wheel_vel_diff, period) : + 0.0f; } 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); + double s_left = (left_pos.L0 - 0.12) / (0.35 - 0.11); + double s_right = (right_pos.L0 - 0.12) / (0.35 - 0.11); 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; + gravity / cos(left_pos.theta) + F_roll - left_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) + gravity / cos(right_pos.theta) - F_roll - right_spring_force; + if (current_leg_length < leg_length_des + 0.02f) { jumpTime_++; } - if (jumpTime_ >= 6) + if (jumpTime_ >= 10) { jumpTime_ = 0; jump_phase_ = JumpPhase::JUMP_UP; @@ -147,12 +227,8 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const } 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; + F_leg(0) = 300 * (1 - 3 * pow(s_left, 2) + 2 * pow(s_left, 3)) + gravity; + F_leg(1) = 300 * (1 - 3 * pow(s_right, 2) + 2 * pow(s_right, 3)) + gravity; if (current_leg_length > leg_length_des) { jumpTime_++; @@ -165,18 +241,16 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const break; case JumpPhase::OFF_GROUND: ROS_INFO("[balance] ENTER OFF_GROUND"); - // F_leg(0) = -(control_params_->p1_ * pow(left_pos_[0], 3) + control_params_->p2_ * pow(left_pos_[0], 2) + - // control_params_->p3_ * left_pos_[0] + control_params_->p4_); - // F_leg(1) = -(control_params_->p1_ * pow(right_pos_[0], 3) + control_params_->p2_ * pow(right_pos_[0], 2) + - // control_params_->p3_ * right_pos_[0] + control_params_->p4_); - F_leg(0) = -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)); + double s_left_flip = 1 - s_left; + double s_right_flip = 1 - s_left; + F_leg(0) = -175 * (1 - 3 * pow(s_left_flip, 2) + 2 * pow(s_left_flip, 3)) - left_spring_force; + F_leg(1) = -175 * (1 - 3 * pow(s_right_flip, 2) + 2 * pow(s_right_flip, 3)) - right_spring_force; - if (current_leg_length < leg_length_des) + if (current_leg_length < leg_length_des + 0.02f) { jumpTime_++; } - if (jumpTime_ >= 8) + if (jumpTime_ >= 100) { jumpTime_ = 0; jump_phase_ = JumpPhase::IDLE; @@ -194,65 +268,94 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const k_left_unstick.block<1, 2>(1, 0) = k_left.block<1, 2>(1, 0); k_right_unstick.block<1, 2>(1, 0) = k_right.block<1, 2>(1, 0); + static bool last_left_unstick{ false }, last_right_unstick{ false }; bool left_unstick{ false }, right_unstick{ false }; - if (controller->getCompleteStand() && jump_phase_ != JumpPhase::LEG_RETRACTION) + if (jump_phase_ == JumpPhase::OFF_GROUND) { - 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); + left_unstick = right_unstick = true; + } + else if (controller->getCompleteStand() && jump_phase_ != JumpPhase::LEG_RETRACTION && + controller->getBaseState() == rm_msgs::ChassisCmd::FOLLOW) + { + left_unstick = + unstickDetection(last_left_unstick ? F_pid_left : left_leg_state.vmc->getForceReal().F + left_spring_force, + u_left(LEG_Tp), left_spd.dL0, left_pos.L0, chassis_state.linear_acc.z, model_params_, + left_leg_state.x, leftSupportForceAveragePtr_, period); + right_unstick = + unstickDetection(last_right_unstick ? F_pid_right : right_leg_state.vmc->getForceReal().F + right_spring_force, + u_right(LEG_Tp), right_spd.dL0, right_pos.L0, chassis_state.linear_acc.z, model_params_, + right_leg_state.x, rightSupportForceAveragePtr_, period); } bool unstick[2]{}; unstick[0] = left_unstick; unstick[1] = right_unstick; + last_left_unstick = left_unstick; + last_right_unstick = 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) + // left_unstick = right_unstick = false; + bool unstick_flag = left_unstick && right_unstick; + if ((controller->getCompleteStand() && unstick_flag && jump_phase_ != JumpPhase::LEG_RETRACTION) || + jump_phase_ == JumpPhase::OFF_GROUND) { - 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] } }; + left_leg_state.vmc->leg_conv(F_leg[LEFT], u_left(LEG_Tp) + T_theta_diff, left_T); + right_leg_state.vmc->leg_conv(F_leg[RIGHT], u_right(LEG_Tp) - T_theta_diff, right_T); + double left_wheel_cmd = unstick_flag ? 0. : u_left(WHEEL_T) - T_yaw - T_wheel_diff; + double right_wheel_cmd = unstick_flag ? 0. : u_right(WHEEL_T) + T_yaw - T_wheel_diff; + LegCommand left_cmd = { F_leg[LEFT], u_left(LEG_Tp) + T_theta_diff, { left_T[0], left_T[1] } }, + right_cmd = { F_leg[RIGHT], u_right(LEG_Tp) - T_theta_diff, { 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 && + if (jump_phase_ == JumpPhase::IDLE && controller->getCompleteStand() && abs(x_left(0) + x_right(0)) / 2.0f > 0.50 && + abs(vel_cmd_.x) > 0.1 && abs(x_left(3)) > 0.1 && ((left_pos.L0 + right_pos.L0) / 2.0f) > 0.30 && leg_length_des > 0.30) { - leg_length_des = controller->getDefaultLegLength(); controller->setMode(BalanceMode::UPSTAIRS); controller->setStateChange(false); controller->setJumpCmd(false); + controller->setCompleteStand(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) + if (leg_length_des < 0.22) { - leg_length_des = controller->getDefaultLegLength(); - x_left_(2) = x_right_(2) = bias_params_->x; + // Protection + if ((abs(x_left(0)) > 0.6f || abs(x_right(0)) > 0.6f || abs(chassis_state.pitch) > 0.4 || + abs(chassis_state.roll) > 0.4) || + (abs(u_left(0)) + abs(u_right(0)) / 2.0f > 30.0f) || + (abs(chassis_state.x_vel - x_left_ref(VEL)) > 5.0f && abs(chassis_state.x_vel - vel_cmd_.x) > 5.0f)) + { + protect_flag_ = true; + leg_length_des = controller->getDefaultLegLength(); + left_leg_state.x(POS) = right_leg_state.x(POS) = 0; + controller->setMode(BalanceMode::PROTECT); + controller->setStateChange(false); + controller->setCompleteStand(false); + controller->setJumpCmd(false); + setJointCommands(joint_handles_, { 0, 0, { 0., 0. } }, { 0, 0, { 0., 0. } }); + ROS_INFO("[balance] Exit NORMAL"); + } + } + // Protection to sit_down + if (abs(x_left(THETA)) > 1.0 || abs(x_right(THETA)) > 1.0 || abs(chassis_state.pitch) > 0.6 || + abs(chassis_state.roll) > 0.8 || controller->getOverturn() || abs(theta_diff) > 1.0 || + controller->getBaseState() == rm_msgs::ChassisCmd::FALLEN) + { + left_leg_state.x(POS) = right_leg_state.x(POS) = 0; controller->setMode(BalanceMode::SIT_DOWN); controller->setStateChange(false); + controller->setCompleteStand(false); controller->setJumpCmd(false); setJointCommands(joint_handles_, { 0, 0, { 0., 0. } }, { 0, 0, { 0., 0. } }); ROS_INFO("[balance] Exit NORMAL"); @@ -265,16 +368,21 @@ double Normal::calculateSupportForce(double F, double Tp, double leg_length, con 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(); + last_ddot_theta = (x(1) - last_dot_theta) / period.toSec(), last_ddot_leg_len, d_leg_len[3]{}; + d_leg_len[0] = d_leg_len[1]; + d_leg_len[1] = d_leg_len[2]; + d_leg_len[2] = leg_len_spd; 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; + double ddot_zM = 0.3 * (acc_z - model_params->g) + 0.7 * last_ddot_zM; + double ddot_theta = 0.3 * ((x(1) - last_dot_theta) / period.toSec()) + 0.7 * last_ddot_theta; + double ddot_leg_len = 0.3 * (d_leg_len[2] - d_leg_len[0]) / 2 * period.toSec() + 0.7 * last_ddot_leg_len; 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))); + last_ddot_leg_len = ddot_leg_len; + double ddot_zw = ddot_zM - ddot_leg_len * cos(x(0)) + 2 * leg_len_spd * x(1) * sin(x(0)) + + +leg_length * (ddot_theta * sin(x(0)) + leg_length * 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; @@ -283,14 +391,14 @@ double Normal::calculateSupportForce(double F, double Tp, double leg_length, con bool Normal::unstickDetection(const double& F_leg, const double& Tp, const double& leg_len_spd, const double& leg_length, const double& acc_z, const std::shared_ptr& model_params, Eigen::Matrix x, - std::shared_ptr> supportForceAveragePtr, + const 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; + bool unstick_ = supportForceAveragePtr->output() < unstick_threshold; if (unstick_ != last_unstick_) { diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/protect.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/protect.cpp new file mode 100644 index 00000000..e75a7c59 --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/protect.cpp @@ -0,0 +1,100 @@ +// +// Created by wk on 2026/5/13. +// +#include "bipedal_wheel_controller/controller_mode/protect.h" +#include "bipedal_wheel_controller/controller.h" +#include "bipedal_wheel_controller/helper_functions.h" + +namespace rm_chassis_controllers +{ +Protect::Protect(BipedalControllerInterface* controller_, + const std::vector& joint_handles, + const std::vector& pid_legs, + const std::vector& pid_thetas, + const std::vector& pid_wheels, control_toolbox::Pid* pid_theta_diff, + control_toolbox::Pid* pid_yaw_vel) + : ModeBase(controller_) + , joint_handles_(joint_handles) + , pid_legs_(pid_legs) + , pid_thetas_(pid_thetas) + , pid_wheels_(pid_wheels) + , pid_theta_diff_(pid_theta_diff) + , pid_yaw_vel_(pid_yaw_vel) +{ + double leg_len_acc = 20, leg_theta_acc = 10; + ramp_length_des_l_ = std::make_shared>(leg_len_acc, 0.001); + ramp_length_des_r_ = std::make_shared>(leg_len_acc, 0.001); + ramp_angle_des_l_ = std::make_shared>(leg_theta_acc, 0.001); + ramp_angle_des_r_ = std::make_shared>(leg_theta_acc, 0.001); +} + +void Protect::execute(const ros::Time& time, const ros::Duration& period) +{ + if (!controller->getStateChange()) + { + ROS_INFO("[balance] Enter PROTECT"); + controller->setStateChange(true); + } + auto& left_leg_state = controller->getLegState(LEFT); + auto& right_leg_state = controller->getLegState(RIGHT); + const auto& left_pos = left_leg_state.vmc->getPos(); + const auto& right_pos = right_leg_state.vmc->getPos(); + const auto& chassis_geometry_params = controller->getChassisGeometryParams(); + const auto& chassis_state = controller->getChassisState(); + + double left_wheel_desired_vel{}, right_wheel_desired_vel{}; + + auto vel_cmd_ = controller->getVelCmd(); + left_wheel_desired_vel = vel_cmd_.x - vel_cmd_.z * chassis_geometry_params->wheel_track; + right_wheel_desired_vel = vel_cmd_.x + vel_cmd_.z * chassis_geometry_params->wheel_track; + + length_des_l = length_des_r = 0.11f; + theta_des_l = theta_des_r = 0.0f; + ramp_length_des_l_->input(length_des_l); + ramp_angle_des_l_->input(theta_des_l); + ramp_length_des_r_->input(length_des_r); + ramp_angle_des_r_->input(theta_des_r); + length_des_l = ramp_length_des_l_->output(); + theta_des_l = ramp_angle_des_l_->output(); + length_des_r = ramp_length_des_r_->output(); + theta_des_r = ramp_angle_des_r_->output(); + + LegCommand left_cmd{}, right_cmd{}; + double F_pid_left{}, F_pid_right{}; + F_pid_left = pid_legs_[LEFT]->computeCommand(length_des_l - left_pos.L0, period); + F_pid_right = pid_legs_[RIGHT]->computeCommand(length_des_r - right_pos.L0, period); + F_pid_left = abs(F_pid_left) > 200 ? std::copysign(1, F_pid_left) * 200 : F_pid_left; + F_pid_right = abs(F_pid_right) > 200 ? std::copysign(1, F_pid_right) * 200 : F_pid_right; + left_cmd.force = F_pid_left - controller->f_spring_force(left_pos.L0); + right_cmd.force = F_pid_right - controller->f_spring_force(right_pos.L0); + double T_theta_diff = pid_theta_diff_->computeCommand(right_pos.theta - left_pos.theta, period); + left_cmd.torque = pid_thetas_[0]->computeCommand(theta_des_l - left_pos.theta, period) + T_theta_diff; + right_cmd.torque = pid_thetas_[1]->computeCommand(theta_des_r - right_pos.theta, period) - T_theta_diff; + double T_yaw = pid_yaw_vel_->computeCommand(vel_cmd_.z - chassis_state.angular_vel.z, period); + double left_wheel_cmd = + pid_wheels_[0]->computeCommand(left_wheel_desired_vel - joint_handles_[0]->getVelocity(), period) - T_yaw; + double right_wheel_cmd = + pid_wheels_[1]->computeCommand(right_wheel_desired_vel - joint_handles_[1]->getVelocity(), period) + T_yaw; + + left_leg_state.vmc->leg_conv(left_cmd.force, left_cmd.torque, left_cmd.input); + right_leg_state.vmc->leg_conv(right_cmd.force, right_cmd.torque, right_cmd.input); + + setJointCommands(joint_handles_, left_cmd, right_cmd, left_wheel_cmd, right_wheel_cmd); + // Exit + if (abs(chassis_state.pitch) < 0.3f && abs(chassis_state.angular_vel.y) < 0.2f && + abs(left_pos.theta + right_pos.theta) / 2.0f < 0.2f) + { + controller->setMode(BalanceMode::NORMAL); + controller->setStateChange(false); + ROS_INFO("[balance] Exit PROTECT"); + } + else if (abs(chassis_state.angular_vel.y) < 0.1 && controller->getOverturn() && + controller->getBaseState() != rm_msgs::ChassisCmd::FALLEN) + { + controller->setStateChange(false); + controller->setMode(BalanceMode::RECOVER); + ROS_INFO("[balance] Exit PROTECT"); + } +} + +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp index 8a9b9be0..6b02b03f 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp @@ -7,14 +7,19 @@ namespace rm_chassis_controllers { -Recover::Recover(const std::vector& joint_handles, +Recover::Recover(BipedalControllerInterface* controller_, + 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) + : ModeBase(controller_) + , 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) +void Recover::execute(const ros::Time& time, const ros::Duration& period) { if (!controller->getStateChange()) { @@ -22,45 +27,112 @@ void Recover::execute(BipedalController* controller, const ros::Time& time, cons detectd_flag = false; controller->setStateChange(true); } + chassis_state_ = controller->getChassisState(); + auto& left_leg_state = controller->getLegState(LEFT); + auto& right_leg_state = controller->getLegState(RIGHT); + const auto& left_pos = left_leg_state.vmc->getPos(); + const auto& right_pos = right_leg_state.vmc->getPos(); + const auto& left_spd = left_leg_state.vmc->getSpd(); + const auto& right_spd = right_leg_state.vmc->getSpd(); // until chassis - if (!detectd_flag && abs(x_left_[1]) < 0.1 && abs(x_left_[5]) < 0.1) + if (!detectd_flag && abs(left_leg_state.x[1]) < 0.2 && abs(right_leg_state.x[5]) < 0.2 && + abs(chassis_state_.angular_vel.y) < 0.1) { detectChassisStateToRecover(); + detectLegRecoveryState(left_recovery_leg, left_pos.theta); + detectLegRecoveryState(right_recovery_leg, right_pos.theta); detectd_flag = true; + controller->setRecoveryLegSpdTurnback(false); 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]; + leg_theta_diff_ = angles::shortest_angular_distance(left_pos.theta, right_pos.theta); double T_theta_diff{ 0.0 }, feedforward_force{ 0.0 }; - if (controller->getBaseState() != 4) + if (controller->getBaseState() != 4 && detectd_flag) { - 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.force = pid_legs_[0]->computeCommand(desired_leg_length_ - left_pos.L0, period) + feedforward_force; + right_cmd.force = pid_legs_[1]->computeCommand(desired_leg_length_ - right_pos.L0, period) + feedforward_force; + + if (controller->getRecoveryLegSpdTurnback()) + { + controller->setRecoveryLegSpdTurnback(false); + leg_recovery_velocity_ = -leg_recovery_velocity_; + } + if (chassis_state_.roll < -0.5) { - 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); + left_cmd.torque = pid_thetas_[2]->computeCommand(leg_recovery_velocity_ - left_spd.dTheta, period); + right_cmd.torque = pid_thetas_[3]->computeCommand(0 - right_spd.dTheta, period); + left_leg_recovery_feed_forward = 2 * leg_recovery_velocity_; + right_leg_recovery_feed_forward = 0.0f; + left_leg_state.vmc->leg_conv(left_cmd.force, left_leg_recovery_feed_forward + left_cmd.torque, left_cmd.input); + right_leg_state.vmc->leg_conv(right_cmd.force, right_leg_recovery_feed_forward + right_cmd.torque, + right_cmd.input); + } + else if (chassis_state_.roll > 0.5) + { + left_cmd.torque = pid_thetas_[2]->computeCommand(0 - left_spd.dTheta, period); + right_cmd.torque = pid_thetas_[3]->computeCommand(leg_recovery_velocity_ - right_spd.dTheta, period); + left_leg_recovery_feed_forward = 0.0f; + right_leg_recovery_feed_forward = 2 * leg_recovery_velocity_; + left_leg_state.vmc->leg_conv(left_cmd.force, left_leg_recovery_feed_forward + left_cmd.torque, left_cmd.input); + right_leg_state.vmc->leg_conv(right_cmd.force, right_leg_recovery_feed_forward + right_cmd.torque, + right_cmd.input); + } + else + { + if (left_recovery_leg == NotReady && right_recovery_leg == Ready) + { + detectLegRecoveryState(left_recovery_leg, left_pos.theta); + detectLegRecoveryState(right_recovery_leg, right_pos.theta); + left_cmd.torque = pid_thetas_[2]->computeCommand(leg_recovery_velocity_ - left_spd.dTheta, period); + right_cmd.torque = pid_thetas_[3]->computeCommand(0 - right_spd.dTheta, period); + left_leg_recovery_feed_forward = 2 * leg_recovery_velocity_; + right_leg_recovery_feed_forward = 0.0f; + left_leg_state.vmc->leg_conv(left_cmd.force, left_leg_recovery_feed_forward + left_cmd.torque, left_cmd.input); + right_leg_state.vmc->leg_conv(right_cmd.force, right_leg_recovery_feed_forward + right_cmd.torque, + right_cmd.input); + } + if (left_recovery_leg == Ready && right_recovery_leg == NotReady) + { + detectLegRecoveryState(left_recovery_leg, left_pos.theta); + detectLegRecoveryState(right_recovery_leg, right_pos.theta); + left_cmd.torque = pid_thetas_[2]->computeCommand(0 - left_spd.dTheta, period); + right_cmd.torque = pid_thetas_[3]->computeCommand(leg_recovery_velocity_ - right_spd.dTheta, period); + left_leg_recovery_feed_forward = 0.0f; + right_leg_recovery_feed_forward = 2 * leg_recovery_velocity_; + left_leg_state.vmc->leg_conv(left_cmd.force, left_leg_recovery_feed_forward + left_cmd.torque, left_cmd.input); + right_leg_state.vmc->leg_conv(right_cmd.force, right_leg_recovery_feed_forward + right_cmd.torque, + right_cmd.input); + } + if (abs(leg_theta_diff_) < 0.4) + { + if ((left_recovery_leg == Ready && right_recovery_leg == Ready) || + (left_recovery_leg == NotReady && right_recovery_leg == NotReady)) + { + T_theta_diff = pid_theta_diff_->computeCommand(leg_theta_diff_, period); + detectLegRecoveryState(left_recovery_leg, left_pos.theta); + detectLegRecoveryState(right_recovery_leg, right_pos.theta); + left_cmd.torque = pid_thetas_[2]->computeCommand(leg_recovery_velocity_ - left_spd.dTheta, period); + right_cmd.torque = pid_thetas_[3]->computeCommand(leg_recovery_velocity_ - right_spd.dTheta, period); + left_leg_recovery_feed_forward = 2 * leg_recovery_velocity_; + right_leg_recovery_feed_forward = left_leg_recovery_feed_forward; + left_leg_state.vmc->leg_conv(left_cmd.force, left_leg_recovery_feed_forward + left_cmd.torque + T_theta_diff, + left_cmd.input); + right_leg_state.vmc->leg_conv( + right_cmd.force, right_leg_recovery_feed_forward + right_cmd.torque - T_theta_diff, right_cmd.input); + } + } } } setJointCommands(joint_handles_, left_cmd, right_cmd); // Exit - if (abs(pitch_) < 0.2 && linear_acc_base_.z > 5.0 && !controller->getOverturn()) + if (abs(chassis_state_.pitch) < 0.2 && chassis_state_.linear_acc.z > 5.0 && !controller->getOverturn()) { - controller->setMode(BalanceMode::STAND_UP); + controller->setMode(BalanceMode::SIT_DOWN); controller->setStateChange(false); controller->clearRecoveryFlag(); ROS_INFO("[balance] Exit RECOVER"); @@ -70,15 +142,42 @@ void Recover::execute(BipedalController* controller, const ros::Time& time, cons void Recover::detectChassisStateToRecover() { // pitch_ is base_link pitch not model pitch - if (pitch_ > 0.45 && pitch_ < M_PI) + if (chassis_state_.pitch > 0.45 && chassis_state_.pitch < M_PI) { ROS_INFO("forward"); recovery_chassis_state_ = RecoveryChassisState::ForwardSlip; } - else if (pitch_ < -0.45 && pitch_ > -M_PI) + else if (chassis_state_.pitch < -0.45 && chassis_state_.pitch > -M_PI) { ROS_INFO("back"); recovery_chassis_state_ = RecoveryChassisState::BackwardSlip; } } + +inline void Recover::detectLegRecoveryState(LegRecoveryState& leg_recovery_state, const double& leg_pos) +{ + if (recovery_chassis_state_ == RecoveryChassisState::ForwardSlip) + { + if ((leg_pos < M_PI && leg_pos > M_PI - 0.3) || (leg_pos < (-M_PI_2 + 0.4) && leg_pos > -M_PI)) + { + leg_recovery_state = Ready; + } + else + { + leg_recovery_state = NotReady; + } + } + else + { + if ((leg_pos > 0.5 && leg_pos < M_PI) || (leg_pos < (-M_PI_2 + 1.0) && leg_pos > -M_PI)) + { + leg_recovery_state = Ready; + } + else + { + leg_recovery_state = NotReady; + } + } + ROS_DEBUG("%d", leg_recovery_state); +} } // 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 index 9d360211..3b805c76 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp @@ -7,13 +7,14 @@ namespace rm_chassis_controllers { -SitDown::SitDown(const std::vector& joint_handles, +SitDown::SitDown(BipedalControllerInterface* controller_, + const std::vector& joint_handles, const std::vector& pid_wheels) - : joint_handles_(joint_handles), pid_wheels_(pid_wheels) + : ModeBase(controller_), joint_handles_(joint_handles), pid_wheels_(pid_wheels) { } -void SitDown::execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) +void SitDown::execute(const ros::Time& time, const ros::Duration& period) { if (!controller->getStateChange()) { @@ -21,19 +22,26 @@ void SitDown::execute(BipedalController* controller, const ros::Time& time, cons controller->setStateChange(true); } + auto& chassis_state = controller->getChassisState(); + // auto& left_leg_state = controller->getLegState(LEFT); + // auto& right_leg_state = controller->getLegState(RIGHT); 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); + // 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); // Exit - if (abs(x_left_(1)) < 0.1 && controller->getBaseState() != 4) + if (abs(chassis_state.angular_vel.y) < 0.1 && controller->getBaseState() != rm_msgs::ChassisCmd::FALLEN) { - if (!controller->getOverturn()) - controller->setMode(BalanceMode::STAND_UP); - else - controller->setMode(BalanceMode::RECOVER); controller->setStateChange(false); + if (controller->getOverturn()) + { + controller->setMode(BalanceMode::RECOVER); + } + else + { + controller->setMode(BalanceMode::STAND_UP); + } ROS_INFO("[balance] Exit SIT_DOWN"); } } diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp index 47775243..d78f1b35 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp @@ -8,96 +8,149 @@ namespace rm_chassis_controllers { -StandUp::StandUp(const std::vector& joint_handles, +StandUp::StandUp(BipedalControllerInterface* controller_, + 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) + : ModeBase(controller_), joint_handles_(joint_handles), pid_legs_(pid_legs), pid_thetas_(pid_thetas) { + double leg_len_acc = 50, leg_theta_acc = 7.5; + ramp_length_des_l_ = std::make_shared>(leg_len_acc, 0.001); + ramp_length_des_r_ = std::make_shared>(leg_len_acc, 0.001); + ramp_angle_des_l_ = std::make_shared>(leg_theta_acc, 0.001); + ramp_angle_des_r_ = std::make_shared>(leg_theta_acc, 0.001); } -void StandUp::execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) +void StandUp::execute(const ros::Time& time, const ros::Duration& period) { + auto& left_leg_state = controller->getLegState(LEFT); + auto& right_leg_state = controller->getLegState(RIGHT); 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); + // vmcPtr_ = controller->getVMCPtr(); + left_arrive_flag_ = right_arrive_flag_ = false; + StandUp::detectLegState(left_leg_state.x, left_leg_orientation); + StandUp::detectLegState(right_leg_state.x, right_leg_orientation); } + const auto& left_pos = left_leg_state.vmc->getPos(); + const auto& right_pos = right_leg_state.vmc->getPos(); + auto model_params_ = controller->getModelParams(); - spring_force_ = -model_params_->f_spring; + double left_spring_force = -controller->f_spring_force(left_pos.L0), + right_spring_force = -controller->f_spring_force(right_pos.L0); 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_); + setUpLegMotion(left_leg_state.x, right_leg_orientation, left_pos.L0, left_pos.theta, left_leg_orientation, + left_leg_command_, left_stop_, left_arrive_flag_, left_arrive_time_); + setUpLegMotion(right_leg_state.x, left_leg_orientation, right_pos.L0, right_pos.theta, right_leg_orientation, + right_leg_command_, right_stop_, right_arrive_flag_, right_arrive_time_); + + ramp_length_des_l_->input(left_leg_command_.desired_length); + ramp_angle_des_l_->input(left_leg_command_.desired_angle); + ramp_length_des_r_->input(right_leg_command_.desired_length); + ramp_angle_des_r_->input(right_leg_command_.desired_angle); + left_leg_command_.desired_length = ramp_length_des_l_->output(); + left_leg_command_.desired_angle = ramp_angle_des_l_->output(); + right_leg_command_.desired_length = ramp_length_des_r_->output(); + right_leg_command_.desired_angle = ramp_angle_des_r_->output(); + 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_); + left_cmd = computePidLegCommand(left_leg_command_, left_leg_state.vmc, *pid_legs_[0], *pid_thetas_[0], + *pid_thetas_[2], left_leg_orientation, period, left_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_); + right_cmd = computePidLegCommand(right_leg_command_, right_leg_state.vmc, *pid_legs_[1], *pid_thetas_[1], + *pid_thetas_[3], right_leg_orientation, period, right_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))) + if ((((abs(left_pos.theta) < 0.3f && left_leg_orientation == LegOrientation::BEHIND)) && + ((abs(right_pos.theta) < 0.3f && right_leg_orientation == LegOrientation::BEHIND))) || + ((abs(left_pos.theta) < 0.3f && left_leg_orientation == LegOrientation::UNDER) && + (abs(right_pos.theta) < 0.3f && right_leg_orientation == LegOrientation::UNDER))) { controller->setMode(BalanceMode::NORMAL); controller->setStateChange(false); ROS_INFO("[balance] Exit STAND_UP"); } + if (controller->getOverturn()) + { + controller->setMode(BalanceMode::RECOVER); + 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) +void StandUp::setUpLegMotion(const Eigen::Matrix& x, const LegOrientation& other_leg_orientation, + const double& leg_length, const double& leg_theta, LegOrientation& leg_orientation, + StandUpLegCommand& legCommand, bool& stop_flag, bool& arrive_flag, ros::Time arrive_time) { - switch (leg_state) + switch (leg_orientation) { - case LegState::UNDER: - theta_des = -M_PI_2; - length_des = 0.36; - if (leg_length > 0.35) + case LegOrientation::UNDER: + stop_flag = false; + legCommand.desired_angle = leg_theta; + legCommand.desired_length = 0.34; + if (leg_length > 0.33) { - leg_state = LegState::FRONT; + leg_orientation = LegOrientation::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; + case LegOrientation::FRONT: + stop_flag = false; + if (!arrive_flag) + arrive_flag = false; + legCommand.desired_angle = M_PI_2 - 0.35; + legCommand.desired_length = 0.34; + legCommand.desired_angle_vel = 0.0; + if (leg_length > 0.30) + legCommand.desired_angle_vel = -5.0; + if (abs(legCommand.desired_angle - leg_theta) < 0.5) + { + legCommand.desired_angle_vel = -1.5f; + } + if (abs(x[1]) < 0.1) + { + if (x[0] > 0 && x[0] < M_PI_2 + 0.4f) + { + legCommand.desired_angle_vel = -0.5f; + if (!arrive_flag) + { + arrive_flag = true; + arrive_time = ros::Time::now(); + } + if ((ros::Time::now() - arrive_time).toSec() > leg_state_threshold_->arrive_time_threshold) + leg_orientation = LegOrientation::BEHIND; + } + } break; - case LegState::BEHIND: + case LegOrientation::BEHIND: stop_flag = true; - theta_des = leg_theta; - length_des = leg_length; - if (other_leg_state != LegState::FRONT) + legCommand.desired_angle = leg_theta; + legCommand.desired_length = leg_length; + if (other_leg_orientation == LegOrientation::BEHIND) { stop_flag = false; - length_des = 0.18; - if (leg_length < 0.21) - theta_des = -0.1; + + legCommand.desired_length = 0.12f; + legCommand.desired_angle = 0.0f; + // legCommand.desired_angle = leg_theta; + // double h = controller->getChassisGeometryParams()->chassis_height; + // legCommand.desired_angle = acos(h / leg_length); } break; } } -inline void StandUp::detectLegState(const Eigen::Matrix& x, int& leg_state) +inline void StandUp::detectLegState(const Eigen::Matrix& x, LegOrientation& leg_orientation) { if (!leg_state_threshold_) { @@ -105,44 +158,74 @@ inline void StandUp::detectLegState(const Eigen::Matrix& x return; } if (x[0] > leg_state_threshold_->under_lower && x[0] < leg_state_threshold_->under_upper) - leg_state = LegState::UNDER; + leg_orientation = LegOrientation::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; + leg_orientation = LegOrientation::FRONT; else if (x[0] > leg_state_threshold_->behind_lower && x[0] < leg_state_threshold_->behind_upper) - leg_state = LegState::BEHIND; - switch (leg_state) + leg_orientation = LegOrientation::BEHIND; + switch (leg_orientation) { - case LegState::UNDER: + case LegOrientation::UNDER: ROS_INFO("[balance] x[0]: %.3f Leg state: UNDER", x[0]); break; - case LegState::FRONT: + case LegOrientation::FRONT: ROS_INFO("[balance] x[0]: %.3f Leg state: FRONT", x[0]); break; - case LegState::BEHIND: + case LegOrientation::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) +inline LegCommand StandUp::computePidLegCommand(const StandUpLegCommand& leg_command, const VMCPtr& vmc_, + control_toolbox::Pid& length_pid, control_toolbox::Pid& angle_pid, + control_toolbox::Pid& angle_vel_pid, + const LegOrientation& leg_orientation, 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) + + const auto& leg_pos = vmc_->getPos(); + const auto& leg_spd = vmc_->getSpd(); + + double Tp_leg_comp{}, F_leg_comp{}, beta{}; + double leg_mass = 2.0f; + double G_leg = leg_mass * 9.81f; + double l_leg = get_LM(leg_pos.L0); + double theta_leg_offset = get_theta_leg_offset(leg_pos.L0); + if (leg_pos.theta > -M_PI_2 && leg_pos.theta < M_PI_2) + { + beta = leg_pos.theta + theta_leg_offset; + F_leg_comp = -G_leg * l_leg * cos(beta); + } + else + { + if (leg_pos.theta > -M_PI && leg_pos.theta < -M_PI_2) + { + beta = -leg_pos.theta - M_PI - theta_leg_offset; + } + else if (leg_pos.theta > M_PI_2 && leg_pos.theta < M_PI) + { + beta = M_PI - leg_pos.theta - theta_leg_offset; + } + F_leg_comp = G_leg * l_leg * cos(beta); + } + Tp_leg_comp = G_leg * l_leg * sin(beta); + + double F_pid_force = length_pid.computeCommand(leg_command.desired_length - leg_pos.L0, period); + F_pid_force = abs(F_pid_force) > 200 ? std::copysign(1, F_pid_force) * 200 : F_pid_force; + cmd.force = F_pid_force + feedforward_force; + if (leg_orientation == LegOrientation::BEHIND || leg_orientation == LegOrientation::UNDER) { - cmd.torque = angle_pid.computeCommand(-angles::shortest_angular_distance(desired_angle, leg_pos[1]), period); + cmd.torque = + angle_pid.computeCommand(-angles::shortest_angular_distance(leg_command.desired_angle, leg_pos.theta), period); } else { - cmd.torque = angle_vel_pid.computeCommand(-5 - leg_spd[1], period); + cmd.torque = angle_vel_pid.computeCommand(leg_command.desired_angle_vel - leg_spd.dTheta, period); } - vmcPtr_->leg_conv(cmd.force, cmd.torque, leg_angle[0], leg_angle[1], cmd.input); + vmc_->leg_conv(cmd.force + F_leg_comp, cmd.torque + Tp_leg_comp, 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 index 34d81f11..ec0e2081 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp @@ -8,40 +8,51 @@ namespace rm_chassis_controllers { -Upstairs::Upstairs(const std::vector& joint_handles, +Upstairs::Upstairs(BipedalControllerInterface* controller_, + 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) + : ModeBase(controller_), 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) +void Upstairs::execute(const ros::Time& time, const ros::Duration& period) { + auto& left_leg_state = controller->getLegState(LEFT); + auto& right_leg_state = controller->getLegState(RIGHT); 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); + // vmcPtr_ = controller->getVMCPtr(); + detectLegState(left_leg_state.x, left_leg_orientation); + detectLegState(right_leg_state.x, right_leg_orientation); } - 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 }; + const auto& left_pos = left_leg_state.vmc->getPos(); + const auto& right_pos = right_leg_state.vmc->getPos(); + + double theta_des_l{ 1.57 }, theta_des_r{ 1.57 }, length_des_l{ 0.18 }, length_des_r{ 0.18 }; auto model_params_ = controller->getModelParams(); - spring_force_ = -model_params_->f_spring; + double left_spring_force = -controller->f_spring_force(left_pos.L0), + right_spring_force = -controller->f_spring_force(right_pos.L0); + + length_des_l = length_des_r = leg_state_threshold_->upstair_des_length; 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_); + left_cmd = computePidLegCommand(length_des_l, theta_des_l, left_leg_state.vmc, *pid_legs_[0], *pid_thetas_[0], + *pid_thetas_[2], left_leg_orientation, period, left_spring_force); + right_cmd = computePidLegCommand(length_des_r, theta_des_r, right_leg_state.vmc, *pid_legs_[1], *pid_thetas_[1], + *pid_thetas_[3], right_leg_orientation, period, right_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) + if (left_pos.theta > leg_state_threshold_->upstair_exit_theta_threshold && + right_pos.theta > leg_state_threshold_->upstair_exit_theta_threshold && + left_pos.L0 < leg_state_threshold_->upstair_exit_length_threshold && + right_pos.L0 < leg_state_threshold_->upstair_exit_length_threshold) { controller->pubLegLenStatus(true); controller->setMode(BalanceMode::STAND_UP); @@ -50,7 +61,7 @@ void Upstairs::execute(BipedalController* controller, const ros::Time& time, con } } -inline void Upstairs::detectLegState(const Eigen::Matrix& x, int& leg_state) +inline void Upstairs::detectLegState(const Eigen::Matrix& x, LegOrientation& leg_state) { if (!leg_state_threshold_) { @@ -58,44 +69,47 @@ inline void Upstairs::detectLegState(const Eigen::Matrix& return; } if (x[0] > leg_state_threshold_->under_lower && x[0] < leg_state_threshold_->under_upper) - leg_state = LegState::UNDER; + leg_state = LegOrientation::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; + leg_state = LegOrientation::FRONT; else if (x[0] > leg_state_threshold_->behind_lower && x[0] < leg_state_threshold_->behind_upper) - leg_state = LegState::BEHIND; + leg_state = LegOrientation::BEHIND; switch (leg_state) { - case LegState::UNDER: + case LegOrientation::UNDER: ROS_INFO("[balance] x[0]: %.3f Leg state: UNDER", x[0]); break; - case LegState::FRONT: + case LegOrientation::FRONT: ROS_INFO("[balance] x[0]: %.3f Leg state: FRONT", x[0]); break; - case LegState::BEHIND: + case LegOrientation::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) +inline LegCommand Upstairs::computePidLegCommand(double desired_length, double desired_angle, const VMCPtr& vmc_, + control_toolbox::Pid& length_pid, control_toolbox::Pid& angle_pid, + control_toolbox::Pid& angle_vel_pid, + const LegOrientation& leg_orientation, 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; + const auto& leg_pos = vmc_->getPos(); + const auto& leg_spd = vmc_->getSpd(); + + cmd.force = length_pid.computeCommand(desired_length - leg_pos.L0, 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) + if (leg_orientation == LegOrientation::BEHIND || leg_orientation == LegOrientation::UNDER) { - cmd.torque = angle_pid.computeCommand(-angles::shortest_angular_distance(desired_angle, leg_pos[1]), period); + cmd.torque = angle_pid.computeCommand(-angles::shortest_angular_distance(desired_angle, leg_pos.theta), period); } else { - cmd.torque = angle_vel_pid.computeCommand(-5 - leg_spd[1], period); + cmd.torque = angle_vel_pid.computeCommand(-5 - leg_spd.dTheta, period); } - vmcPtr_->leg_conv(cmd.force, cmd.torque, leg_angle[0], leg_angle[1], cmd.input); + vmc_->leg_conv(cmd.force, cmd.torque, cmd.input); return cmd; } } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp index 93bbab58..f2313973 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp @@ -55,10 +55,17 @@ bool VMCController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ROS_ERROR("Load param fail, check the resist of l1 or l2"); return false; } + leg_gravity_compensation_debug_ = controller_nh.param("leg_gravity_compensation_debug", false); + leg_mass_ = controller_nh.param("leg_mass", 1.55); + s2_ = controller_nh.param("s2", 0.0775); + s3_ = controller_nh.param("s3", 0.205); + alpha_s_ = controller_nh.param("alpha_s", 0.2); vmcPtr_ = std::make_unique(l1, l2, 0); jointThigh_ = robot_hw->get()->getHandle(thighJoint); jointKnee_ = robot_hw->get()->getHandle(kneeJoint); + + debugPub_ = std::make_shared(controller_nh, "vmc_debug_data"); return true; } @@ -70,7 +77,7 @@ void VMCController::starting(const ros::Time& /*time*/) void VMCController::update(const ros::Time& time, const ros::Duration& period) { - double knee_angle = 0, thigh_angle = 0, position[2], speed[2]; + double knee_angle = 0, thigh_angle = 0; // series leg vmc // gazebo @@ -80,30 +87,59 @@ void VMCController::update(const ros::Time& time, const ros::Duration& period) // 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); + vmcPtr_->calc_jacobian(thigh_angle, knee_angle); + vmcPtr_->leg_pos(thigh_angle, knee_angle); + vmcPtr_->leg_spd(jointThigh_.getVelocity(), jointKnee_.getVelocity()); + + const auto& leg_pos = vmcPtr_->getPos(); + const auto& leg_spd = vmcPtr_->getSpd(); double effortCmd[2], jointCmd[2]; - static double angleSinCmd_ = 0; - angleSinCmd_ -= 0.001; - if (angleSinCmd_ <= -M_PI) + double angle_error = angles::shortest_angular_distance(leg_pos.theta, angleCmd_); + double f_spring_force_comp = f_spring_force(leg_pos.L0); + if (leg_gravity_compensation_debug_) { - angleSinCmd_ = M_PI; + double Tp_leg_comp{}, F_leg_comp{}, beta{}; + double G_leg = leg_mass_ * g_; + double l_leg = get_LM(leg_pos.L0); + double theta_leg_offset = get_theta_leg_offset(leg_pos.L0); + if (leg_pos.theta > -M_PI_2 && leg_pos.theta < M_PI_2) + { + beta = leg_pos.theta + theta_leg_offset; + F_leg_comp = -G_leg * l_leg * cos(beta); + } + else + { + if (leg_pos.theta > -M_PI && leg_pos.theta < -M_PI_2) + { + beta = -leg_pos.theta - M_PI - theta_leg_offset; + } + else if (leg_pos.theta > M_PI_2 && leg_pos.theta < M_PI) + { + beta = M_PI - leg_pos.theta - theta_leg_offset; + } + F_leg_comp = G_leg * l_leg * cos(beta); + } + Tp_leg_comp = G_leg * l_leg * sin(beta); + effortCmd[0] = F_leg_comp - f_spring_force_comp; + effortCmd[1] = Tp_leg_comp; + debugPub_->add("F_leg_comp", F_leg_comp); + debugPub_->add("Tp_leg_comp", Tp_leg_comp); + } + else + { + effortCmd[0] = pidLength_.computeCommand(lengthCmd_ - leg_pos.L0, period) - f_spring_force(leg_pos.L0); + effortCmd[1] = pidAngle_.computeCommand(angle_error, period); } - // 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); + vmcPtr_->leg_conv(effortCmd[0], effortCmd[1], 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(leg_pos.L0); + state.data.push_back(leg_pos.theta); + state.data.push_back(leg_spd.dL0); + state.data.push_back(leg_spd.dTheta); state.data.push_back(angle_error); state.data.push_back(effortCmd[0]); state.data.push_back(effortCmd[1]); @@ -111,6 +147,10 @@ void VMCController::update(const ros::Time& time, const ros::Duration& period) state.data.push_back(jointCmd[1]); statePublisher_.publish(state); + debugPub_->add("f_spring_force", f_spring_force_comp); + debugPub_->add("F_effortCmd", effortCmd[0]); + debugPub_->publish(); + std_msgs::Float64MultiArray jointCmdState; jointCmdState.data.push_back(jointCmd[0]); jointCmdState.data.push_back(jointCmd[1]); @@ -120,6 +160,19 @@ void VMCController::update(const ros::Time& time, const ros::Duration& period) jointKnee_.setCommand(jointCmd[1]); } +double VMCController::f_spring_force(double L0) +{ + // double l1 = vmcPtr_->getL1(), l2 = vmcPtr_->getL2(), Fs = spring_force_, s2 = s2_, s3 = s3_, alpha_s = alpha_s_; + // double cos_theta3, theta3, ls, Fv; + // cos_theta3 = (l1 * l1 + l2 * l2 - L0 * L0) / (2 * l1 * l2); + // theta3 = acos(cos_theta3); + // ls = sqrt(s2 * s2 + s3 * s3 - 2 * s2 * s3 * cos(theta3 - alpha_s)); + // Fv = Fs * (L0 * s2 * s3 * sin(theta3 - alpha_s)) / (ls * l1 * l2 * sin(theta3)); + // return Fv; + + return ((2094.45f * L0 - 3091.28f) * L0 + 1408.375f) * L0 - 80.91f; +} + } // 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 index d6eeb4c2..c9e09127 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/VMC.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/VMC.cpp @@ -7,7 +7,7 @@ namespace rm_chassis_controllers { -void VMC::leg_pos(double phi1, double phi4, double pos[2]) const +void VMC::leg_pos(double phi1, double phi4) { double a_tmp; double t4; @@ -17,8 +17,8 @@ void VMC::leg_pos(double phi1, double phi4, double pos[2]) const 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); + pos_.L0 = sqrt(a_tmp * a_tmp + t4 * t4); + pos_.theta = atan2(t4, a_tmp); // five link vmc // double YD, YB, XD, XB, lBD, A0, B0, C0, phi2, XC, YC; @@ -36,34 +36,38 @@ void VMC::leg_pos(double phi1, double phi4, double pos[2]) const // 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; + // pos_.L0 = L0; + // pos_.theta = Phi0; } -void VMC::leg_spd(double dphi1, double dphi4, double phi1, double phi4, double* spd) +void VMC::leg_spd(double dphi1, double dphi4) { - double J[2][2]; - - calc_jacobian(phi1, phi4, J); + static double last_L0_spd = J_[0][0] * dphi1 + J_[0][1] * dphi4; + static double last_theta_spd = J_[1][0] * dphi1 + J_[1][1] * dphi4; // 速度映射 - spd[0] = J[0][0] * dphi1 + J[0][1] * dphi4; // dl0 - spd[1] = J[1][0] * dphi1 + J[1][1] * dphi4; // dphi0 + spd_.dL0 = J_[0][0] * dphi1 + J_[0][1] * dphi4; // dl0 + spd_.dTheta = J_[1][0] * dphi1 + J_[1][1] * dphi4; // dphi0 + + // leg_spd lp filter + spd_.dL0 = 0.4 * spd_.dL0 + 0.6 * last_L0_spd; + last_L0_spd = spd_.dL0; + spd_.dTheta = 0.4 * spd_.dTheta + 0.6 * last_theta_spd; + last_theta_spd = spd_.dTheta; } -void VMC::leg_conv(double F, double Tp, double phi1, double phi4, double* T) +void VMC::leg_conv(double F, double Tp, 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; + 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]) +void VMC::calc_jacobian(double phi1, double phi4) { + phi1_ = phi1; + phi4_ = phi4; + // gazebo double phi2 = phi1 + phi4; double c1 = cos(phi1); @@ -80,8 +84,8 @@ void VMC::calc_jacobian(double phi1, double phi4, double J[2][2]) // 防止奇异 if (L0 < 1e-8) { - J[0][0] = J[0][1] = 0.0; - J[1][0] = J[1][1] = 0.0; + J_[0][0] = J_[0][1] = 0.0; + J_[1][0] = J_[1][1] = 0.0; return; } @@ -96,12 +100,12 @@ void VMC::calc_jacobian(double phi1, double phi4, double J[2][2]) 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; + 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; + 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; @@ -129,9 +133,143 @@ void VMC::calc_jacobian(double phi1, double phi4, double J[2][2]) // 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; + // J_[0][0] = j11; + // J_[0][1] = j12; + // J_[1][0] = j21; + // J_[1][1] = j22; +} + +inline void VMC::calc_jacobian(double phi1, double phi4, double J[2][2]) +{ + phi1_ = phi1; + phi4_ = phi4; + // gazebo + // 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; +} + +void VMC::leg_conv_t(double T1, double T2) +{ + // (J^T)^-1 * [T1; T2] + double det = J_[0][0] * J_[1][1] - J_[0][1] * J_[1][0]; + if (fabs(det) < 1e-8) + { + force_real_.F = force_real_.Tp = 0.0; // 奇异情况 + return; + } + double inv_Jt[2][2]; + inv_Jt[0][0] = J_[1][1] / det; + inv_Jt[0][1] = -J_[0][1] / det; + inv_Jt[1][0] = -J_[1][0] / det; + inv_Jt[1][1] = J_[0][0] / det; + + force_real_.F = inv_Jt[0][0] * T1 + inv_Jt[0][1] * T2; // F + force_real_.Tp = inv_Jt[1][0] * T1 + inv_Jt[1][1] * T2; // Tp + + // five link vmc + // double YD, YB, XD, XB, lBD, A0, B0, C0, XC, YC; + // double phi2, phi3; + // 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); + // 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); + // + // double f_temp_11 = cos(phi0 - phi3); + // double f_temp_12 = cos(phi0 - phi2); + // double f_temp_21 = l4_ * sin(phi3 - phi4_); + // double f_temp_22 = l1_ * sin(phi1_ - phi2); + // + // double t_temp_11 = cos(phi0 - phi2); + // double t_temp_12 = sin(phi0 - phi3); + // double t_temp_21 = l1_ * sin(phi1_ - phi2); + // double t_temp_22 = l4_ * sin(phi3 - phi4_); + // + // double inv_Jt[2][2]; + // inv_Jt[0][0] = -f_temp_12 / f_temp_22; + // inv_Jt[0][1] = f_temp_11 / f_temp_21; + // inv_Jt[1][0] = L0 * t_temp_11 / t_temp_21; + // inv_Jt[1][1] = -L0 * t_temp_12 / t_temp_22; + // + // force_real_.F = inv_Jt[0][0] * T1 + inv_Jt[0][1] * T2; // F + // force_real_.Tp = inv_Jt[1][0] * T1 + inv_Jt[1][1] * T2; // Tp } } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 1e744ebd..800c9be2 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -35,12 +35,6 @@ // Created by huakang on 2021/3/21. // #include "rm_chassis_controllers/chassis_base.h" -#include -#include -#include -#include -#include -#include namespace rm_chassis_controllers { @@ -49,80 +43,93 @@ template class ChassisBase; template -bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, - ros::NodeHandle& controller_nh) +void ChassisBase::initialize_parameters(ros::NodeHandle& controller_nh) { - if (!controller_nh.getParam("publish_rate", publish_rate_) || !controller_nh.getParam("timeout", timeout_) || - !controller_nh.getParam("power/vel_coeff", velocity_coeff_) || - !controller_nh.getParam("power/effort_coeff", effort_coeff_) || - !controller_nh.getParam("power/power_offset", power_offset_)) + try { - ROS_ERROR("Some chassis params doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; + controller_nh.getParam("publish_rate", publish_rate_); + controller_nh.getParam("publish_map_tf", publish_map_tf_); + controller_nh.getParam("publish_odom_tf", publish_odom_tf_); + controller_nh.getParam("gravity_estimation_offset", gravity_estimation_offset_); + controller_nh.getParam("slam_topic", slam_topic_); + controller_nh.getParam("localization_topic", localization_topic_); + + controller_nh.getParam("wheel_radius", wheel_radius_); + controller_nh.getParam("twist_angular", twist_angular_); + controller_nh.getParam("max_odom_vel", max_odom_vel_); + controller_nh.getParam("timeout", timeout_); + raw_yaw_feedforward_k_ = getParam(controller_nh, "raw_yaw_feedforward_k", 0.0); + + if (controller_nh.hasParam("pid_follow")) + pid_follow_.init(ros::NodeHandle(controller_nh, "pid_follow")); } - pitch_angle_threshold_ = getParam(controller_nh, "pitch_angle_threshold", -0.25); - scale_ = getParam(controller_nh, "scale", 1.); - enable_uphill_acceleration_ = getParam(controller_nh, "enable_uphill_acceleration", false); - wheel_radius_ = getParam(controller_nh, "wheel_radius", 0.02); - twist_angular_ = getParam(controller_nh, "twist_angular", M_PI / 6); - max_odom_vel_ = getParam(controller_nh, "max_odom_vel", 0); - enable_odom_tf_ = getParam(controller_nh, "enable_odom_tf", true); - publish_odom_tf_ = getParam(controller_nh, "publish_odom_tf", false); - - // Get and check params for covariances - XmlRpc::XmlRpcValue twist_cov_list; - controller_nh.getParam("twist_covariance_diagonal", twist_cov_list); - ROS_ASSERT(twist_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray); - ROS_ASSERT(twist_cov_list.size() == 6); - for (int i = 0; i < twist_cov_list.size(); ++i) - ROS_ASSERT(twist_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble); - - robot_state_handle_ = robot_hw->get()->getHandle("robot_state"); - effort_joint_interface_ = robot_hw->get(); - - // Setup odometry realtime publisher + odom message constant fields - odom_pub_.reset(new realtime_tools::RealtimePublisher(root_nh, "odom", 100)); - odom_pub_->msg_.header.frame_id = "odom"; - odom_pub_->msg_.child_frame_id = "base_link"; - odom_pub_->msg_.twist.covariance = { static_cast(twist_cov_list[0]), 0., 0., 0., 0., 0., 0., - static_cast(twist_cov_list[1]), 0., 0., 0., 0., 0., 0., - static_cast(twist_cov_list[2]), 0., 0., 0., 0., 0., 0., - static_cast(twist_cov_list[3]), 0., 0., 0., 0., 0., 0., - static_cast(twist_cov_list[4]), 0., 0., 0., 0., 0., 0., - static_cast(twist_cov_list[5]) }; - - ramp_x_ = new RampFilter(0, 0.001); - ramp_y_ = new RampFilter(0, 0.001); - ramp_w_ = new RampFilter(0, 0.001); - - // init odom tf - if (enable_odom_tf_) + catch (std::exception& e) { - odom2base_.header.frame_id = "odom"; - odom2base_.header.stamp = ros::Time::now(); - odom2base_.child_frame_id = "base_link"; - odom2base_.transform.rotation.w = 1; - tf_broadcaster_.init(root_nh); - tf_broadcaster_.sendTransform(odom2base_); + ROS_ERROR("Chassis parameter initialization failed: %s", e.what()); } - world2odom_.setRotation(tf2::Quaternion::getIdentity()); +} +template +bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh) +{ + initialize_parameters(controller_nh); + robot_state_handle_ = robot_hw->get()->getHandle("robot_state"); + effort_joint_interface_ = robot_hw->get(); - outside_odom_sub_ = - controller_nh.subscribe("/odometry", 10, &ChassisBase::outsideOdomCallback, this); + cmd_vel_sub_ = root_nh.subscribe("cmd_vel", 1, &ChassisBase::cmdVelCallback, this); cmd_chassis_sub_ = controller_nh.subscribe("/cmd_chassis", 1, &ChassisBase::cmdChassisCallback, this); - cmd_vel_sub_ = root_nh.subscribe("cmd_vel", 1, &ChassisBase::cmdVelCallback, this); + slam_sub_ = controller_nh.subscribe(slam_topic_, 10, &ChassisBase::slamCallback, this); + localization_sub_ = controller_nh.subscribe( + localization_topic_, 10, &ChassisBase::localizationCallback, this); + capacity_sub_ = root_nh.subscribe(capacity_topic_, 10, + &ChassisBase::capacityCallback, this); + + // Setup real_power from capacity publishers. + auto chassis_power_publisher = + std::make_unique>(controller_nh, "power/chassis_power", 100); + this->chassis_power_pub_ = std::move(chassis_power_publisher); - if (controller_nh.hasParam("pid_follow")) - if (!pid_follow_.init(ros::NodeHandle(controller_nh, "pid_follow"))) - return false; + // Setup odometry realtime publisher + odom message constant fields + auto odometry_publisher = + std::make_unique>(root_nh, "odom", 100); + this->odometry_rt_pub_ = std::move(odometry_publisher); + odometry_rt_pub_->msg_.header.frame_id = robot_odom_frame_id_; + odometry_rt_pub_->msg_.child_frame_id = robot_base_frame_id_; + odometry_rt_pub_->msg_.twist.covariance = { 0.001, 0., 0., 0., 0., 0., 0., 0.001, 0., 0., 0., 0., + 0., 0., 0.001, 0., 0., 0., 0., 0., 0., 0.001, 0., 0., + 0., 0., 0., 0., 0.001, 0., 0., 0., 0., 0., 0., 0.001 }; + + ramp_x_ = std::make_unique>(0, 0.001); + ramp_y_ = std::make_unique>(0, 0.001); + ramp_w_ = std::make_unique>(0, 0.001); + + if (publish_map_tf_) + { + global_map2robot_odom_.header.stamp = ros::Time::now(); + global_map2robot_odom_.header.frame_id = global_map_frame_id_; + global_map2robot_odom_.child_frame_id = robot_odom_frame_id_; + global_map2robot_odom_.transform.rotation.w = 1; + brcst4global_map2robot_odom_.init(root_nh); + brcst4global_map2robot_odom_.sendTransform(global_map2robot_odom_); + + global_map2camera_init_.header.stamp = ros::Time::now(); + global_map2camera_init_.header.frame_id = global_map_frame_id_; + global_map2camera_init_.child_frame_id = "camera_init"; + global_map2camera_init_.transform.rotation.w = 1; + brcst4global_map2camera_init_.init(root_nh); + brcst4global_map2camera_init_.sendTransform(global_map2camera_init_); + } - // 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); + if (publish_odom_tf_) + { + robot_odom2robot_base_.header.stamp = ros::Time::now(); + robot_odom2robot_base_.header.frame_id = robot_odom_frame_id_; + robot_odom2robot_base_.child_frame_id = robot_base_frame_id_; + global_map2robot_odom_.transform.rotation.w = 1; + brcst4robot_odom2robot_base_.init(root_nh); + brcst4robot_odom2robot_base_.sendTransform(robot_odom2robot_base_); + } return true; } @@ -178,6 +185,9 @@ void ChassisBase::update(const ros::Time& time, const ros::Duration& perio case TWIST: twist(time, period); break; + case FALLEN: + fallen(); + break; } ramp_w_->setAcc(cmd_chassis.accel.angular.z); @@ -204,8 +214,9 @@ void ChassisBase::follow(const ros::Time& time, const ros::Duration& perio try { double roll{}, pitch{}, yaw{}; - quatToRPY(robot_state_handle_.lookupTransform("base_link", follow_source_frame_, ros::Time(0)).transform.rotation, - roll, pitch, yaw); + quatToRPY( + robot_state_handle_.lookupTransform(robot_base_frame_id_, follow_source_frame_, ros::Time(0)).transform.rotation, + roll, pitch, yaw); double follow_error = angles::shortest_angular_distance(yaw, 0); pid_follow_.computeCommand(-follow_error, period); vel_cmd_.z = pid_follow_.getCurrentCmd() + cmd_rt_buffer_.readFromRT()->cmd_chassis_.follow_vel_des; @@ -231,7 +242,8 @@ void ChassisBase::twist(const ros::Time& time, const ros::Duration& period try { double roll{}, pitch{}, yaw{}; - quatToRPY(robot_state_handle_.lookupTransform("base_link", command_source_frame_, ros::Time(0)).transform.rotation, + quatToRPY(robot_state_handle_.lookupTransform(robot_base_frame_id_, command_source_frame_, ros::Time(0)) + .transform.rotation, roll, pitch, yaw); double angle[4] = { -0.785, 0.785, 2.355, -2.355 }; @@ -266,116 +278,202 @@ void ChassisBase::raw() recovery(); } - tfVelToBase(command_source_frame_); + double yaw_offset; + if (command_source_frame_ == "yaw") + yaw_offset = raw_yaw_feedforward_k_ * vel_cmd_.z; + else + yaw_offset = 0.; + tfVelToBase(command_source_frame_, yaw_offset); +} + +template +void ChassisBase::fallen() +{ + if (state_changed_) + { + state_changed_ = false; + ROS_INFO("[Chassis] Enter FALLEN"); + } + + ramp_x_->clear(); + ramp_y_->clear(); + ramp_w_->clear(); + vel_cmd_.x = 0.; + vel_cmd_.y = 0.; + vel_cmd_.z = 0.; } template void ChassisBase::updateOdom(const ros::Time& time, const ros::Duration& period) { - geometry_msgs::Twist vel_base = odometry(); // on base_link frame - if (enable_odom_tf_) + if (publish_map_tf_) { - geometry_msgs::Vector3 linear_vel_odom, angular_vel_odom; - try + if (!odom_initialized_) { - odom2base_ = robot_state_handle_.lookupTransform("odom", "base_link", ros::Time(0)); - tf2::Quaternion q; - tf2::fromMsg(odom2base_.transform.rotation, q); - tf2::Matrix3x3(q).getEulerYPR(yaw_, pitch_, roll_); - } - catch (tf2::TransformException& ex) - { - tf_broadcaster_.sendTransform(odom2base_); // TODO: For some reason, the sendTransform in init sometime not work? - ROS_WARN("%s", ex.what()); - return; + try + { + geometry_msgs::TransformStamped global_map2lidar_odom = + robot_state_handle_.lookupTransform(robot_base_frame_id_, lidar_base_frame_id_, ros::Time(0)); + T_global_map2lidar_odom_.setOrigin(tf2::Vector3(global_map2lidar_odom.transform.translation.x, + global_map2lidar_odom.transform.translation.y, + global_map2lidar_odom.transform.translation.z)); + if (gravity_estimation_offset_) + { + T_global_map2lidar_odom_.setRotation(tf2::Quaternion(0, 0, 0, 1)); + global_map2camera_init_.transform.translation = global_map2lidar_odom.transform.translation; + } + else + { + T_global_map2lidar_odom_.setRotation( + tf2::Quaternion(global_map2lidar_odom.transform.rotation.x, global_map2lidar_odom.transform.rotation.y, + global_map2lidar_odom.transform.rotation.z, global_map2lidar_odom.transform.rotation.w)); + global_map2camera_init_.transform = global_map2lidar_odom.transform; + } + odom_initialized_ = true; + } + catch (...) + { + ROS_WARN("Failed to init robot_odom2lidar_odom."); + } } - odom2base_.header.stamp = time; - // integral vel to pos and angle - tf2::doTransform(vel_base.linear, linear_vel_odom, odom2base_); - tf2::doTransform(vel_base.angular, angular_vel_odom, odom2base_); - double length = - std::sqrt(std::pow(linear_vel_odom.x, 2) + std::pow(linear_vel_odom.y, 2) + std::pow(linear_vel_odom.z, 2)); - if (length < max_odom_vel_) + + if (localization_updated_) { - // avoid nan vel - odom2base_.transform.translation.x += linear_vel_odom.x * period.toSec(); - odom2base_.transform.translation.y += linear_vel_odom.y * period.toSec(); - odom2base_.transform.translation.z += linear_vel_odom.z * period.toSec(); - } - length = - std::sqrt(std::pow(angular_vel_odom.x, 2) + std::pow(angular_vel_odom.y, 2) + std::pow(angular_vel_odom.z, 2)); - if (length > 0.001) - { // avoid nan quat - tf2::Quaternion odom2base_quat, trans_quat; - tf2::fromMsg(odom2base_.transform.rotation, odom2base_quat); - trans_quat.setRotation(tf2::Vector3(angular_vel_odom.x / length, angular_vel_odom.y / length, - angular_vel_odom.z / length), - length * period.toSec()); - odom2base_quat = trans_quat * odom2base_quat; - odom2base_quat.normalize(); - odom2base_.transform.rotation = tf2::toMsg(odom2base_quat); + try + { + localization_updated_ = false; + const auto& localization = localization_rt_buffer_.readFromRT(); + T_global_map2lidar_odom_.setOrigin(tf2::Vector3(localization->transform.translation.x, + localization->transform.translation.y, + localization->transform.translation.z)); + T_global_map2lidar_odom_.setRotation( + tf2::Quaternion(localization->transform.rotation.x, localization->transform.rotation.y, + localization->transform.rotation.z, localization->transform.rotation.w)); + global_map2camera_init_.transform = tf2::toMsg(T_global_map2lidar_odom_); + } + catch (...) + { + ROS_WARN("Failed to update localization offset."); + } } - } + ros::Time tmp_time = ros::Time::now(); - if (topic_update_) - { - auto* odom_msg = odom_buffer_.readFromRT(); - - tf2::Transform world2sensor; - world2sensor.setOrigin( - tf2::Vector3(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z)); - world2sensor.setRotation(tf2::Quaternion(odom_msg->pose.pose.orientation.x, odom_msg->pose.pose.orientation.y, - odom_msg->pose.pose.orientation.z, odom_msg->pose.pose.orientation.w)); - - if (world2odom_.getRotation() == tf2::Quaternion::getIdentity()) // First received + if (slam_updated_) { - tf2::Transform odom2sensor; try { - geometry_msgs::TransformStamped tf_msg = - robot_state_handle_.lookupTransform("odom", "livox_frame", odom_msg->header.stamp); - tf2::fromMsg(tf_msg.transform, odom2sensor); + slam_updated_ = false; + const auto& slam = slam_rt_buffer_.readFromRT(); + tmp_time = slam->header.stamp; + T_lidar_odom2lidar_base_.setOrigin( + tf2::Vector3(slam->pose.pose.position.x, slam->pose.pose.position.y, slam->pose.pose.position.z)); + T_lidar_odom2lidar_base_.setRotation( + tf2::Quaternion(slam->pose.pose.orientation.x, slam->pose.pose.orientation.y, slam->pose.pose.orientation.z, + slam->pose.pose.orientation.w)); + + robot_base2lidar_base_ = + robot_state_handle_.lookupTransform(robot_base_frame_id_, lidar_base_frame_id_, slam->header.stamp); + T_robot_base2lidar_base_.setOrigin(tf2::Vector3(robot_base2lidar_base_.transform.translation.x, + robot_base2lidar_base_.transform.translation.y, + robot_base2lidar_base_.transform.translation.z)); + T_robot_base2lidar_base_.setRotation( + tf2::Quaternion(robot_base2lidar_base_.transform.rotation.x, robot_base2lidar_base_.transform.rotation.y, + robot_base2lidar_base_.transform.rotation.z, robot_base2lidar_base_.transform.rotation.w)); + + auto tmp_robot_odom2robot_base = + robot_state_handle_.lookupTransform(robot_odom_frame_id_, robot_base_frame_id_, slam->header.stamp); + + T_robot_odom_2robot_base_.setOrigin(tf2::Vector3(tmp_robot_odom2robot_base.transform.translation.x, + tmp_robot_odom2robot_base.transform.translation.y, + tmp_robot_odom2robot_base.transform.translation.z)); + T_robot_odom_2robot_base_.setRotation(tf2::Quaternion( + tmp_robot_odom2robot_base.transform.rotation.x, tmp_robot_odom2robot_base.transform.rotation.y, + tmp_robot_odom2robot_base.transform.rotation.z, tmp_robot_odom2robot_base.transform.rotation.w)); + + T_global_map2robot_odom_ = T_global_map2lidar_odom_ * T_lidar_odom2lidar_base_ * + T_robot_base2lidar_base_.inverse() * T_robot_odom_2robot_base_.inverse(); + + global_map2robot_odom_.transform = tf2::toMsg(T_global_map2robot_odom_); } - catch (tf2::TransformException& ex) + catch (...) { - ROS_WARN("%s", ex.what()); - return; + ROS_WARN("Failed to update global_map2robot_odom."); } - world2odom_ = world2sensor * odom2sensor.inverse(); } - tf2::Transform base2sensor; + global_map2robot_odom_.header.stamp = tmp_time; + global_map2camera_init_.header.stamp = tmp_time; + } + + if (publish_odom_tf_) + { try { - geometry_msgs::TransformStamped tf_msg = - robot_state_handle_.lookupTransform("base_link", "livox_frame", odom_msg->header.stamp); - tf2::fromMsg(tf_msg.transform, base2sensor); + robot_odom2robot_base_ = + robot_state_handle_.lookupTransform(robot_odom_frame_id_, robot_base_frame_id_, ros::Time(0)); + robot_odom2robot_base_.header.stamp = time; + geometry_msgs::Twist vel_base = odometry(); // on base_link frame + geometry_msgs::Vector3 linear_vel_odom, angular_vel_odom; + tf2::doTransform(vel_base.linear, linear_vel_odom, robot_odom2robot_base_); + tf2::doTransform(vel_base.angular, angular_vel_odom, robot_odom2robot_base_); + + double length = + std::sqrt(std::pow(linear_vel_odom.x, 2) + std::pow(linear_vel_odom.y, 2) + std::pow(linear_vel_odom.z, 2)); + if (length < max_odom_vel_) + { // avoid nan vel + robot_odom2robot_base_.transform.translation.x += linear_vel_odom.x * period.toSec(); + robot_odom2robot_base_.transform.translation.y += linear_vel_odom.y * period.toSec(); + robot_odom2robot_base_.transform.translation.z += linear_vel_odom.z * period.toSec(); + } + length = std::sqrt(std::pow(angular_vel_odom.x, 2) + std::pow(angular_vel_odom.y, 2) + + std::pow(angular_vel_odom.z, 2)); + if (length > 0.001) + { // avoid nan quat + tf2::Quaternion odom2base_quat, trans_quat; + tf2::fromMsg(robot_odom2robot_base_.transform.rotation, odom2base_quat); + trans_quat.setRotation(tf2::Vector3(angular_vel_odom.x / length, angular_vel_odom.y / length, + angular_vel_odom.z / length), + length * period.toSec()); + odom2base_quat = trans_quat * odom2base_quat; + odom2base_quat.normalize(); + robot_odom2robot_base_.transform.rotation = tf2::toMsg(odom2base_quat); + } + + quatToRPY(robot_odom2robot_base_.transform.rotation, roll_, pitch_, yaw_); + + robot_state_handle_.setTransform(robot_odom2robot_base_, "rm_chassis_controllers"); + + odometry_rt_pub_->msg_.header.stamp = time; + odometry_rt_pub_->msg_.pose.pose.position.x = robot_odom2robot_base_.transform.translation.x; + odometry_rt_pub_->msg_.pose.pose.position.y = robot_odom2robot_base_.transform.translation.y; + odometry_rt_pub_->msg_.pose.pose.orientation.x = robot_odom2robot_base_.transform.rotation.x; + odometry_rt_pub_->msg_.pose.pose.orientation.y = robot_odom2robot_base_.transform.rotation.y; + odometry_rt_pub_->msg_.pose.pose.orientation.z = robot_odom2robot_base_.transform.rotation.z; + odometry_rt_pub_->msg_.pose.pose.orientation.w = robot_odom2robot_base_.transform.rotation.w; + odometry_rt_pub_->msg_.twist.twist.linear.x = linear_vel_odom.x; + odometry_rt_pub_->msg_.twist.twist.linear.y = linear_vel_odom.y; + odometry_rt_pub_->msg_.twist.twist.angular.z = angular_vel_odom.z; } - catch (tf2::TransformException& ex) + catch (...) { - ROS_WARN("%s", ex.what()); - return; + ROS_WARN("Failed to update robot_odom2robot_base."); } - tf2::Transform odom2base = world2odom_.inverse() * world2sensor * base2sensor.inverse(); - odom2base_.transform.translation.x = odom2base.getOrigin().x(); - odom2base_.transform.translation.y = odom2base.getOrigin().y(); - odom2base_.transform.translation.z = odom2base.getOrigin().z(); - topic_update_ = false; } - robot_state_handle_.setTransform(odom2base_, "rm_chassis_controllers"); - if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) { - if (odom_pub_->trylock()) + if (publish_map_tf_) { - odom_pub_->msg_.header.stamp = time; - odom_pub_->msg_.twist.twist.linear.x = vel_base.linear.x; - odom_pub_->msg_.twist.twist.linear.y = vel_base.linear.y; - odom_pub_->msg_.twist.twist.angular.z = vel_base.angular.z; - odom_pub_->unlockAndPublish(); + brcst4global_map2robot_odom_.sendTransform(global_map2robot_odom_); + brcst4global_map2camera_init_.sendTransform(global_map2camera_init_); } - if (enable_odom_tf_ && publish_odom_tf_) - tf_broadcaster_.sendTransform(odom2base_); + + if (publish_odom_tf_) + { + brcst4robot_odom2robot_base_.sendTransform(robot_odom2robot_base_); + if (odometry_rt_pub_->trylock()) + odometry_rt_pub_->unlockAndPublish(); + } + last_publish_time_ = time; } } @@ -391,57 +489,30 @@ void ChassisBase::recovery() 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_) - { - double cmd_effort = joint.getCommand(); - double real_vel = joint.getVelocity(); - if (joint.getName().find("wheel") != std::string::npos) // The pivot joint of swerve drive doesn't need power limit - { - a += square(cmd_effort); - b += std::abs(cmd_effort * real_vel); - c += square(real_vel); - } - } - 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_) - if (joint.getName().find("wheel") != std::string::npos) - { - if (pitch_ < pitch_angle_threshold_ && enable_uphill_acceleration_) - { - if (joint.getName().find("back") != std::string::npos) - { - joint.setCommand(zoom_coeff > 1 ? joint.getCommand() : joint.getCommand() * zoom_coeff * scale_); - } - if (joint.getName().find("front") != std::string::npos) - { - joint.setCommand(zoom_coeff > 1 ? joint.getCommand() : joint.getCommand() * zoom_coeff); - } - } - else - { - joint.setCommand(zoom_coeff > 1 ? joint.getCommand() : joint.getCommand() * zoom_coeff); - } - } +template +void ChassisBase::updatePowerStatus() +{ } template -void ChassisBase::tfVelToBase(const std::string& from) +void ChassisBase::tfVelToBase(const std::string& from, double yaw_offset) { try { - tf2::doTransform(vel_cmd_, vel_cmd_, robot_state_handle_.lookupTransform("base_link", from, ros::Time(0))); + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform("base_link", from, ros::Time(0)); + if (std::abs(yaw_offset) > 1e-9) + { + tf2::Quaternion rotation; + tf2::fromMsg(transform.transform.rotation, rotation); + tf2::Quaternion yaw_feedforward; + yaw_feedforward.setRPY(0., 0., yaw_offset); + rotation *= yaw_feedforward; + rotation.normalize(); + transform.transform.rotation = tf2::toMsg(rotation); + } + tf2::doTransform(vel_cmd_, vel_cmd_, transform); } catch (tf2::TransformException& ex) { @@ -465,17 +536,29 @@ void ChassisBase::cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg } template -void ChassisBase::outsideOdomCallback(const nav_msgs::Odometry::ConstPtr& msg) +void ChassisBase::slamCallback(const nav_msgs::Odometry::ConstPtr& msg) { - odom_buffer_.writeFromNonRT(*msg); - topic_update_ = true; + slam_rt_buffer_.writeFromNonRT(*msg); + slam_updated_ = true; } template -void ChassisBase::powerLimitReconfigCB(rm_chassis_controllers::PowerLimitConfig& config, uint32_t /*level*/) +void ChassisBase::localizationCallback(const geometry_msgs::TransformStamped::ConstPtr& msg) { - ROS_INFO("[Power Limit] Dynamic params change"); - power_limit_rt_buffer_.writeFromNonRT(config); + localization_rt_buffer_.writeFromNonRT(*msg); + localization_updated_ = true; +} + +template +void ChassisBase::capacityCallback(const rm_msgs::PowerManagementSampleAndStatusData::ConstPtr& msg) +{ + chassis_power_ = msg->chassis_power + msg->capacity_discharge_power; + capacity_update_flag_ = true; + if (chassis_power_pub_->trylock()) + { + chassis_power_pub_->msg_.data = chassis_power_; + chassis_power_pub_->unlockAndPublish(); + } } } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/omni.cpp b/rm_chassis_controllers/src/omni.cpp index 903ad007..2582cc1f 100644 --- a/rm_chassis_controllers/src/omni.cpp +++ b/rm_chassis_controllers/src/omni.cpp @@ -2,6 +2,8 @@ // Created by qiayuan on 2022/7/29. // +#include +#include #include #include @@ -17,6 +19,41 @@ bool OmniController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle { ChassisBase::init(robot_hw, root_nh, controller_nh); + // Set init value for RLS and power limiters. + try + { + controller_nh.getParam("power/vel_coeff", wheel_power_limitor_.vel_coeff); + controller_nh.getParam("power/effort_coeff", wheel_power_limitor_.effort_coeff); + controller_nh.getParam("power/power_offset", wheel_power_limitor_.power_offset); + controller_nh.param("power/use_rls", use_rls_, false); + controller_nh.param("power/use_K_angle", use_K_angle_, false); + } + catch (const std::exception& e) + { + ROS_ERROR("Failed to get power limiter parameters: %s", e.what()); + return false; + } + + wheel_power_limitor_.err_upper = 500; + wheel_power_limitor_.err_lower = 0.01; + + rls_ = std::make_unique>(2, 1, 0.99999, 1e-5); + Eigen::Matrix w; + w << wheel_power_limitor_.effort_coeff, wheel_power_limitor_.vel_coeff; + rls_->setW(w); + + for (auto& filter : motor_lp_filters_) + { + filter = new LowPassFilter(20); + } + + auto epower_publisher = + std::make_unique>(controller_nh, "power/estimated", 100); + this->epower_pub_ = std::move(epower_publisher); + auto cpower_publisher = + std::make_unique>(controller_nh, "power/commanded", 100); + this->cpower_pub_ = std::move(cpower_publisher); + XmlRpc::XmlRpcValue wheels; controller_nh.getParam("wheels", wheels); chassis2joints_.resize(wheels.size(), 3); @@ -44,7 +81,7 @@ bool OmniController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle joints_.push_back(std::make_shared()); if (!joints_.back()->init(effort_joint_interface_, nh_wheel)) return false; - joint_handles_.push_back(joints_[i]->joint_); + wheel_joint_handles_.push_back(joints_[i]->joint_); i++; } @@ -77,5 +114,188 @@ geometry_msgs::Twist OmniController::odometry() return twist; } +void OmniController::stateJudge() +{ + if (!use_K_angle_) + { + for (size_t i = 0; i < joints_.size() && i < 4; ++i) + { + wheel_power_limitor_.K_angle[i] = 1.0; + } + return; + } + double sin_pitch{}; + if (abs(pitch_) > 0.12) + { + sin_pitch = sin(pitch_); + } + else + { + sin_pitch = 0.0; + } + + for (size_t i = 0; i < joints_.size() && i < 4; ++i) + { + auto& ctl = joints_[i]; + + if (ctl->joint_.getName().find("front") != std::string::npos) + { + if (ctl->getJointName().find("left") != std::string::npos) + { + wheel_power_limitor_.K_angle[i] = 1 + sin_pitch; + } + if (ctl->getJointName().find("right") != std::string::npos) + { + wheel_power_limitor_.K_angle[i] = 1 + sin_pitch; + } + } + if (ctl->joint_.getName().find("back") != std::string::npos) + { + if (ctl->getJointName().find("left") != std::string::npos) + { + wheel_power_limitor_.K_angle[i] = 1 - sin_pitch; + } + if (ctl->getJointName().find("right") != std::string::npos) + { + wheel_power_limitor_.K_angle[i] = 1 - sin_pitch; + } + } + } +} + +void OmniController::powerLimit() +{ + updatePowerStatus(); + stateJudge(); + // multiply K to limit power for wheel joints. + if (wheel_power_limitor_.err_sum > wheel_power_limitor_.err_upper) + { + wheel_power_limitor_.K = 1; + } + else if (wheel_power_limitor_.err_sum < wheel_power_limitor_.err_lower) + { + wheel_power_limitor_.K = 0; + } + else + { + wheel_power_limitor_.K = 1 - (wheel_power_limitor_.err_sum - wheel_power_limitor_.err_lower) / + (wheel_power_limitor_.err_upper - wheel_power_limitor_.err_lower); + } + // Set power limit to each joint according to K. + for (int i = 0; i < 4; i++) + { + double wheel_zoom = + (wheel_power_limitor_.K * abs(wheel_power_limitor_.err[i]) / wheel_power_limitor_.err_sum) + + (1 - wheel_power_limitor_.K) * (abs(wheel_power_limitor_.power_in[i]) / wheel_power_limitor_.cmd_power); + wheel_zoom = limit(wheel_zoom, 0.0, 1.0); + wheel_power_limitor_.power_limit[i] = wheel_zoom * wheel_power_limitor_.max_power; + } + if (wheel_power_limitor_.cmd_power > wheel_power_limitor_.max_power) + { + for (size_t i = 0; i < joints_.size() && i < 4; ++i) + { + auto& ctl = joints_[i]; + auto& joint = ctl->joint_; + double A = wheel_power_limitor_.effort_coeff; + double B = wheel_power_limitor_.omiga[i]; + double C = abs(wheel_power_limitor_.omiga[i]) * wheel_power_limitor_.vel_coeff + + wheel_power_limitor_.power_offset / 4 - wheel_power_limitor_.power_limit[i]; + double Delta = square(B) - 4 * A * C; + if (!std::isfinite(Delta) || Delta < 0.0) + Delta = 0.0; + if (Delta >= 0) + { + double Sqrt = sqrtf(Delta); + if (wheel_power_limitor_.torque[i] >= 0) + joint.setCommand(((-B + Sqrt) / (2 * A)) * wheel_power_limitor_.K_angle[i]); + else + joint.setCommand(((-B - Sqrt) / (2 * A)) * wheel_power_limitor_.K_angle[i]); + } + else + { + joint.setCommand(((-B) / (2 * A)) * wheel_power_limitor_.K_angle[i]); + } + } + } + else + { + for (size_t i = 0; i < joints_.size() && i < 4; ++i) + { + auto& ctl = joints_[i]; + auto& joint = ctl->joint_; + joint.setCommand(wheel_power_limitor_.torque[i] * wheel_power_limitor_.K_angle[i]); + } + } +} + +void OmniController::updatePowerStatus() +{ + double power_limit = cmd_rt_buffer_.readFromRT()->cmd_chassis_.power_limit; + + wheel_power_limitor_.max_power = power_limit; + wheel_power_limitor_.err_sum = 0; + + double ewheel_power{}, cwheel_power{}; + for (size_t i = 0; i < joints_.size() && i < 4; ++i) + { + auto& ctl = joints_[i]; + + double cmd_torque = wheel_power_limitor_.torque[i] = ctl->joint_.getCommand(); + double cmd_vel{}; + ctl->getCommand(cmd_vel); + double real_vel = wheel_power_limitor_.omiga[i] = ctl->joint_.getVelocity(); + motor_lp_filters_[i]->input(ctl->joint_.getEffort()); + double real_torque = motor_lp_filters_[i]->output(); + + wheel_power_limitor_.err[i] = cmd_vel - real_vel; + wheel_power_limitor_.err_sum += abs(wheel_power_limitor_.err[i]); + + ewheel_power += real_torque * real_vel + wheel_power_limitor_.effort_coeff * square(real_torque) + + wheel_power_limitor_.vel_coeff * abs(real_vel); + cwheel_power += cmd_torque * real_vel + wheel_power_limitor_.effort_coeff * square(cmd_torque) + + wheel_power_limitor_.vel_coeff * abs(real_vel); + + wheel_power_limitor_.power_in[i] = cmd_torque * real_vel + wheel_power_limitor_.effort_coeff * square(cmd_torque) + + wheel_power_limitor_.vel_coeff * abs(real_vel); + } + wheel_power_limitor_.cmd_power = cwheel_power + wheel_power_limitor_.power_offset; + wheel_power_limitor_.estimated_power = ewheel_power + wheel_power_limitor_.power_offset; + + double estimated_total_power = wheel_power_limitor_.estimated_power; + double cmd_total_power = wheel_power_limitor_.cmd_power; + + if (capacity_update_flag_ && use_rls_ && estimated_total_power > 0) + { + // Update Rls. + double all_in = limit(estimated_total_power, -power_limit, power_limit); + // Update Rls: compute regression vector x and pass actual measured power + Eigen::Matrix x; + x(0) = square(wheel_power_limitor_.torque[0]) + square(wheel_power_limitor_.torque[1]) + + square(wheel_power_limitor_.torque[2]) + square(wheel_power_limitor_.torque[3]); + x(1) = abs(wheel_power_limitor_.omiga[0]) + abs(wheel_power_limitor_.omiga[1]) + + abs(wheel_power_limitor_.omiga[2]) + abs(wheel_power_limitor_.omiga[3]); + rls_->setU(all_in); + rls_->setX(x); + rls_->setY(chassis_power_); + rls_->update(); + auto w = rls_->getW(); + wheel_power_limitor_.effort_coeff = std::max(w(0), 1e-3); + wheel_power_limitor_.vel_coeff = std::max(w(1), 1e-3); + capacity_update_flag_ = false; + } + + // Publish power status. + auto publishPower = [](auto& pub, const double power) { + if (pub && pub->trylock()) + { + pub->msg_.data = power; + pub->unlockAndPublish(); + } + }; + + publishPower(epower_pub_, estimated_total_power); + publishPower(cpower_pub_, cmd_total_power); +} + } // namespace rm_chassis_controllers PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::OmniController, controller_interface::ControllerBase) diff --git a/rm_chassis_controllers/src/sentry.cpp b/rm_chassis_controllers/src/sentry.cpp index e26ad8d3..ce5e0fb7 100644 --- a/rm_chassis_controllers/src/sentry.cpp +++ b/rm_chassis_controllers/src/sentry.cpp @@ -56,8 +56,8 @@ bool SentryController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand !ctrl_catapult_joint_.init(effort_joint_interface_, nh_brake)) return false; if_catapult_ = false; - joint_handles_.push_back(effort_joint_interface_->getHandle(ctrl_wheel_.getJointName())); - joint_handles_.push_back(effort_joint_interface_->getHandle(ctrl_catapult_joint_.getJointName())); + wheel_joint_handles_.push_back(effort_joint_interface_->getHandle(ctrl_wheel_.getJointName())); + wheel_joint_handles_.push_back(effort_joint_interface_->getHandle(ctrl_catapult_joint_.getJointName())); return true; } diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index 69c21077..9f54947e 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -36,7 +36,7 @@ // #include "rm_chassis_controllers/swerve.h" - +#include "rm_common/math_utilities.h" #include #include @@ -47,6 +47,55 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand { if (!ChassisBase::init(robot_hw, root_nh, controller_nh)) return false; + + // Set init value for RLS and power limiters. + try + { + controller_nh.getParam("power/vel_coeff", wheel_power_limitor_.vel_coeff); + controller_nh.getParam("power/effort_coeff", wheel_power_limitor_.effort_coeff); + controller_nh.getParam("power/power_offset", wheel_power_limitor_.power_offset); + controller_nh.getParam("power/pivot_vel_coeff", pivot_power_limitor_.vel_coeff); + controller_nh.getParam("power/pivot_effort_coeff", pivot_power_limitor_.effort_coeff); + controller_nh.getParam("power/pivot_power_offset", pivot_power_limitor_.power_offset); + controller_nh.getParam("power/pivot_power_ratio", pivot_power_limitor_.ratio); + controller_nh.param("power/use_rls", use_rls_, false); + controller_nh.param("power/use_K_angle", use_K_angle_, false); + } + catch (const std::exception& e) + { + ROS_ERROR("Failed to get power limiter parameters: %s", e.what()); + return false; + } + + // pivot don't need err to multiply power. + wheel_power_limitor_.err_upper = 500; + wheel_power_limitor_.err_lower = 0.01; + + rls_ = std::make_unique>(4, 1, 0.99999, 1e-5); + Eigen::Matrix w; + w << pivot_power_limitor_.effort_coeff, pivot_power_limitor_.vel_coeff, wheel_power_limitor_.effort_coeff, + wheel_power_limitor_.vel_coeff; + rls_->setW(w); + + // 0 for pivot, 1 for wheel. Each module has 4 joints at most. + for (auto& filter_group : motor_lp_filters_) + { + for (auto& filter : filter_group) + filter = new LowPassFilter(20); + } + + // Setup power publishers. + auto epower_publisher = + std::make_unique>(controller_nh, "power/estimated", 100); + this->epower_pub_ = std::move(epower_publisher); + auto cpower_publisher = + std::make_unique>(controller_nh, "power/commanded", 100); + this->cpower_pub_ = std::move(cpower_publisher); + // Todo :add base_imu and base_gyro publishers for better power estimation and detect state. + auto base_gyro_publisher = std::make_unique>( + controller_nh, "base_gyro", 100); + this->base_gyro_pub_ = std::move(base_gyro_publisher); + XmlRpc::XmlRpcValue modules; controller_nh.getParam("modules", modules); ROS_ASSERT(modules.getType() == XmlRpc::XmlRpcValue::TypeStruct); @@ -73,10 +122,11 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand return false; if (module.second["pivot"].hasMember("offset")) m.pivot_offset_ = module.second["pivot"]["offset"]; - joint_handles_.push_back(m.ctrl_pivot_->joint_); - joint_handles_.push_back(m.ctrl_wheel_->joint_); + pivot_joint_handles_.push_back(m.ctrl_pivot_->joint_); + wheel_joint_handles_.push_back(m.ctrl_wheel_->joint_); modules_.push_back(m); } + return true; } @@ -84,12 +134,12 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand void SwerveController::moveJoint(const ros::Time& time, const ros::Duration& period) { + stateJudge(); Vec2 vel_center(vel_cmd_.x, vel_cmd_.y); for (auto& module : modules_) { Vec2 vel = vel_center + vel_cmd_.z * Vec2(-module.position_.y(), module.position_.x()); double vel_angle = std::atan2(vel.y(), vel.x()) + module.pivot_offset_; - // Direction flipping and Stray module mitigation double a = angles::shortest_angular_distance(module.ctrl_pivot_->joint_.getPosition(), vel_angle); double b = angles::shortest_angular_distance(module.ctrl_pivot_->joint_.getPosition(), vel_angle + M_PI); module.ctrl_pivot_->setCommand(std::abs(a) < std::abs(b) ? vel_angle : vel_angle + M_PI); @@ -125,5 +175,252 @@ geometry_msgs::Twist SwerveController::odometry() return vel_data; } +void SwerveController::stateJudge() +{ + if (!use_K_angle_) + { + for (size_t i = 0; i < modules_.size() && i < 4; ++i) + { + wheel_power_limitor_.K_angle[i] = 1.0; + } + return; + } + double sin_pitch{}; + if (abs(pitch_) > 0.12) + { + sin_pitch = sin(pitch_); + } + else + { + sin_pitch = 0.0; + } + + for (size_t i = 0; i < modules_.size() && i < 4; ++i) + { + auto& module = modules_[i]; + + if (module.ctrl_wheel_->joint_.getName().find("front") != std::string::npos) + { + if (module.ctrl_wheel_->getJointName().find("left") != std::string::npos) + { + wheel_power_limitor_.K_angle[i] = 1 + sin_pitch; + } + if (module.ctrl_wheel_->getJointName().find("right") != std::string::npos) + { + wheel_power_limitor_.K_angle[i] = 1 + sin_pitch; + } + } + if (module.ctrl_wheel_->joint_.getName().find("back") != std::string::npos) + { + if (module.ctrl_wheel_->getJointName().find("left") != std::string::npos) + { + wheel_power_limitor_.K_angle[i] = 1 - sin_pitch; + } + if (module.ctrl_wheel_->getJointName().find("right") != std::string::npos) + { + wheel_power_limitor_.K_angle[i] = 1 - sin_pitch; + } + } + } +} + +// Ref: https://gitee.com/cod_-control/rmcod2026_-sentry/tree/dev +// https://github.com/hkustenterprize/RM2024-PowerModule + +void SwerveController::powerLimit() +{ + updatePowerStatus(); + + // multiply K to limit power for wheel joints. + if (wheel_power_limitor_.err_sum > wheel_power_limitor_.err_upper) + { + wheel_power_limitor_.K = 1; + } + else if (wheel_power_limitor_.err_sum < wheel_power_limitor_.err_lower) + { + wheel_power_limitor_.K = 0; + } + else + { + wheel_power_limitor_.K = 1 - (wheel_power_limitor_.err_sum - wheel_power_limitor_.err_lower) / + (wheel_power_limitor_.err_upper - wheel_power_limitor_.err_lower); + } + // Set power limit to each joint according to K. + for (int i = 0; i < 4; i++) + { + double pivot_zoom = abs(pivot_power_limitor_.power_in[i]) / pivot_power_limitor_.cmd_power; + pivot_zoom = limit(pivot_zoom, 0.0, 1.0); + double wheel_zoom = + (wheel_power_limitor_.K * abs(wheel_power_limitor_.err[i]) / wheel_power_limitor_.err_sum) + + (1 - wheel_power_limitor_.K) * (abs(wheel_power_limitor_.power_in[i]) / wheel_power_limitor_.cmd_power); + wheel_zoom = limit(wheel_zoom, 0.0, 1.0); + pivot_power_limitor_.power_limit[i] = pivot_zoom * pivot_power_limitor_.max_power; + wheel_power_limitor_.power_limit[i] = wheel_zoom * wheel_power_limitor_.max_power; + } + // Set command limit according to power limit. + if (pivot_power_limitor_.cmd_power > pivot_power_limitor_.max_power) + { + for (size_t i = 0; i < modules_.size() && i < 4; ++i) + { + auto& module = modules_[i]; + auto& joint = module.ctrl_pivot_->joint_; + double A = pivot_power_limitor_.effort_coeff; + double B = pivot_power_limitor_.omiga[i]; + double C = abs(pivot_power_limitor_.omiga[i]) * pivot_power_limitor_.vel_coeff + + pivot_power_limitor_.power_offset / 4 - pivot_power_limitor_.power_limit[i]; + double Delta = square(B) - 4 * A * C; + if (!std::isfinite(Delta) || Delta < 0.0) + Delta = 0.0; + if (Delta >= 0) + { + double Sqrt = sqrtf(Delta); + if (pivot_power_limitor_.torque[i] >= 0) + joint.setCommand((-B + Sqrt) / (2 * A)); + else + joint.setCommand((-B - Sqrt) / (2 * A)); + } + else + { + joint.setCommand((-B) / (2 * A)); + } + } + } + if (wheel_power_limitor_.cmd_power > wheel_power_limitor_.max_power) + { + for (size_t i = 0; i < modules_.size() && i < 4; ++i) + { + auto& module = modules_[i]; + auto& joint = module.ctrl_wheel_->joint_; + double A = wheel_power_limitor_.effort_coeff; + double B = wheel_power_limitor_.omiga[i]; + double C = abs(wheel_power_limitor_.omiga[i]) * wheel_power_limitor_.vel_coeff + + wheel_power_limitor_.power_offset / 4 - wheel_power_limitor_.power_limit[i]; + double Delta = square(B) - 4 * A * C; + if (!std::isfinite(Delta) || Delta < 0.0) + Delta = 0.0; + if (Delta >= 0) + { + double Sqrt = sqrtf(Delta); + if (wheel_power_limitor_.torque[i] >= 0) + joint.setCommand(((-B + Sqrt) / (2 * A)) * wheel_power_limitor_.K_angle[i]); + else + joint.setCommand(((-B - Sqrt) / (2 * A)) * wheel_power_limitor_.K_angle[i]); + } + else + { + joint.setCommand(((-B) / (2 * A)) * wheel_power_limitor_.K_angle[i]); + } + } + } + else + { + for (size_t i = 0; i < modules_.size() && i < 4; ++i) + { + auto& module = modules_[i]; + auto& joint = module.ctrl_wheel_->joint_; + joint.setCommand(wheel_power_limitor_.torque[i] * wheel_power_limitor_.K_angle[i]); + } + } +} + +void SwerveController::updatePowerStatus() +{ + double power_limit = cmd_rt_buffer_.readFromRT()->cmd_chassis_.power_limit; + + pivot_power_limitor_.max_power = pivot_power_limitor_.ratio * power_limit; + + double epivot_power{}, cpivot_power{}; + for (size_t i = 0; i < modules_.size() && i < 4; ++i) + { + auto& module = modules_[i]; + + double cmd_torque = pivot_power_limitor_.torque[i] = module.ctrl_pivot_->joint_.getCommand(); + double real_vel = pivot_power_limitor_.omiga[i] = module.ctrl_pivot_->joint_.getVelocity(); + motor_lp_filters_[0][i]->input(module.ctrl_pivot_->joint_.getEffort()); + double real_torque = motor_lp_filters_[0][i]->output(); + + epivot_power += real_torque * real_vel + pivot_power_limitor_.effort_coeff * square(real_torque) + + pivot_power_limitor_.vel_coeff * abs(real_vel); + cpivot_power += cmd_torque * real_vel + pivot_power_limitor_.effort_coeff * square(cmd_torque) + + pivot_power_limitor_.vel_coeff * abs(real_vel); + + pivot_power_limitor_.power_in[i] = cmd_torque * real_vel + pivot_power_limitor_.effort_coeff * square(cmd_torque) + + pivot_power_limitor_.vel_coeff * abs(real_vel); + } + + pivot_power_limitor_.cmd_power = cpivot_power + pivot_power_limitor_.power_offset; + pivot_power_limitor_.estimated_power = epivot_power + pivot_power_limitor_.power_offset; + + wheel_power_limitor_.max_power = power_limit - std::abs(pivot_power_limitor_.cmd_power); + wheel_power_limitor_.err_sum = 0; + + double ewheel_power{}, cwheel_power{}; + for (size_t i = 0; i < modules_.size() && i < 4; ++i) + { + auto& module = modules_[i]; + + double cmd_torque = wheel_power_limitor_.torque[i] = module.ctrl_wheel_->joint_.getCommand(); + double cmd_vel{}; + module.ctrl_wheel_->getCommand(cmd_vel); + double real_vel = wheel_power_limitor_.omiga[i] = module.ctrl_wheel_->joint_.getVelocity(); + motor_lp_filters_[1][i]->input(module.ctrl_wheel_->joint_.getEffort()); + double real_torque = motor_lp_filters_[1][i]->output(); + + wheel_power_limitor_.err[i] = cmd_vel - real_vel; + wheel_power_limitor_.err_sum += abs(wheel_power_limitor_.err[i]); + + ewheel_power += real_torque * real_vel + wheel_power_limitor_.effort_coeff * square(real_torque) + + wheel_power_limitor_.vel_coeff * abs(real_vel); + cwheel_power += cmd_torque * real_vel + wheel_power_limitor_.effort_coeff * square(cmd_torque) + + wheel_power_limitor_.vel_coeff * abs(real_vel); + + wheel_power_limitor_.power_in[i] = cmd_torque * real_vel + wheel_power_limitor_.effort_coeff * square(cmd_torque) + + wheel_power_limitor_.vel_coeff * abs(real_vel); + } + wheel_power_limitor_.cmd_power = cwheel_power + wheel_power_limitor_.power_offset; + wheel_power_limitor_.estimated_power = ewheel_power + wheel_power_limitor_.power_offset; + + double estimated_total_power = pivot_power_limitor_.estimated_power + wheel_power_limitor_.estimated_power; + double cmd_total_power = pivot_power_limitor_.cmd_power + wheel_power_limitor_.cmd_power; + + if (capacity_update_flag_ && use_rls_) + { + double all_in = limit(estimated_total_power, -power_limit, power_limit); + + // Update Rls: compute regression vector x and pass actual measured power + Eigen::Matrix x; + x(0) = square(pivot_power_limitor_.torque[0]) + square(pivot_power_limitor_.torque[1]) + + square(pivot_power_limitor_.torque[2]) + square(pivot_power_limitor_.torque[3]); + x(1) = abs(pivot_power_limitor_.omiga[0]) + abs(pivot_power_limitor_.omiga[1]) + + abs(pivot_power_limitor_.omiga[2]) + abs(pivot_power_limitor_.omiga[3]); + x(2) = square(wheel_power_limitor_.torque[0]) + square(wheel_power_limitor_.torque[1]) + + square(wheel_power_limitor_.torque[2]) + square(wheel_power_limitor_.torque[3]); + x(3) = abs(wheel_power_limitor_.omiga[0]) + abs(wheel_power_limitor_.omiga[1]) + + abs(wheel_power_limitor_.omiga[2]) + abs(wheel_power_limitor_.omiga[3]); + rls_->setU(all_in); + rls_->setX(x); + rls_->setY(chassis_power_); + rls_->update(); // Internally computes predicted output as x^T * w + auto w = rls_->getW(); + pivot_power_limitor_.effort_coeff = std::max(w(0), 1e-3); + pivot_power_limitor_.vel_coeff = std::max(w(1), 1e-3); + wheel_power_limitor_.effort_coeff = std::max(w(2), 1e-3); + wheel_power_limitor_.vel_coeff = std::max(w(3), 1e-3); + capacity_update_flag_ = false; + } + + // Publish power status. + auto publishPower = [](auto& pub, const double power) { + if (pub && pub->trylock()) + { + pub->msg_.data = power; + pub->unlockAndPublish(); + } + }; + + publishPower(epower_pub_, estimated_total_power); + publishPower(cpower_pub_, cmd_total_power); +} + PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::SwerveController, controller_interface::ControllerBase) } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/test/omni.yaml b/rm_chassis_controllers/test/omni.yaml index 564ecd64..ce352f09 100644 --- a/rm_chassis_controllers/test/omni.yaml +++ b/rm_chassis_controllers/test/omni.yaml @@ -10,6 +10,7 @@ controllers: power_offset: 0 twist_angular: 0.5233 timeout: 0.1 + raw_yaw_feedforward_k: 0.0 pid_follow: { p: 8, i: 0, d: 4.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] diff --git a/rm_chassis_controllers/test/swerve.yaml b/rm_chassis_controllers/test/swerve.yaml index 2c1bc975..9dc0a189 100644 --- a/rm_chassis_controllers/test/swerve.yaml +++ b/rm_chassis_controllers/test/swerve.yaml @@ -9,6 +9,7 @@ controllers: vel_coeff: 0.0048 power_offset: -3 timeout: 0.1 + raw_yaw_feedforward_k: 0.0 twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] pid_follow: { p: 5, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } # SwerveController diff --git a/rm_chassis_controllers/test/vmc_controller.yaml b/rm_chassis_controllers/test/vmc_controller.yaml index d66cbc4b..84e4bfeb 100644 --- a/rm_chassis_controllers/test/vmc_controller.yaml +++ b/rm_chassis_controllers/test/vmc_controller.yaml @@ -7,15 +7,27 @@ controllers: publish_rate: 100 vmc_controller: type: rm_chassis_controllers/VMCController - vmc_bias_angle: 1.57 - spring_force: 100.0 + vmc_bias_angle: 0.0 + leg_gravity_compensation_debug: true + leg_mass: 1.95 + LM_weight: 0.6 + spring_force: 450.0 + s2: 0.0486 + s3: 0.20 + alpha_s: 0.2448 + + # spring_force: 200.0 + # spring_force: 0.0 + # s2: 0.0775 + # s3: 0.205 + # alpha_s: 0.2 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 + # l1: 0.218 + # l2: 0.26 # series_legged2 -# l1: 0.21 -# l2: 0.248 + l1: 0.21 + l2: 0.248 diff --git a/rm_gimbal_controllers/ARCHITECTURE_AND_CONTROL_REPORT.md b/rm_gimbal_controllers/ARCHITECTURE_AND_CONTROL_REPORT.md new file mode 100644 index 00000000..96b0970f --- /dev/null +++ b/rm_gimbal_controllers/ARCHITECTURE_AND_CONTROL_REPORT.md @@ -0,0 +1,141 @@ +# rm_gimbal_controllers 架构与控制算法报告 + +## 摘要 + +`rm_gimbal_controllers` 采用“双模块协同”设计： + +- `Controller`：负责状态机、坐标变换、关节闭环控制与执行器命令输出。 +- `BulletSolver`：负责装甲板目标选择、弹道迭代求解、切换装甲过程中的过渡轨迹生成以及射击时序决策。 + +主要实现入口： + +- `src/gimbal_base.cpp`（`Controller::init`、`Controller::update`） +- `src/bullet_solver.cpp`（`BulletSolver` 构造函数、`BulletSolver::solve`） + +本报告仅基于当前实现进行说明，不改变运行时行为。 + +## 1. 架构设计 + +### 1.1 控制器插件层 + +`Controller` 以插件形式实现： + +- `controller_interface::MultiInterfaceController` + +它对应三条硬件/软件集成链路： + +- 机器人状态与 TF 查询（`RobotStateInterface`） +- IMU 反馈（`ImuSensorInterface`） +- 力矩命令输出（`EffortJointInterface`） + +关键声明位于 `include/rm_gimbal_controllers/gimbal_base.h`。 + +### 1.2 功能分层 + +该包可分为决策层、求解层与执行层： + +- 决策与模式处理： + - `Controller` 中的 `rate()`、`track()`、`direct()`、`traj()` +- 命令执行： + - `Controller` 中的 `moveJoint()` +- 弹道与目标求解子系统： + - `BulletSolver` 类（`solve`、`getGimbalError`、`judgeShootBeforehand`、`planningPoint`） + +### 1.3 运行时数据流 + +单次更新周期的控制流程如下： + +1. 从实时缓冲区读取指令与跟踪数据（`cmd_rt_buffer_`、`track_rt_buffer_`）。 +2. 更新变换关系（`odom -> gimbal`、`odom -> base`）并估计底盘速度。 +3. 按模式分发（`RATE / TRACK / DIRECT / TRAJ`）。 +4. 将目标姿态转换为各轴误差并输入控制回路。 +5. 通过关节速度控制器输出最终命令到力矩关节接口。 + +### 1.4 可观测接口 + +主要运行时输出包括： + +- 云台目标误差话题（`error`） +- 各轴状态话题（`pos_state`） +- 弹道模型可视化标记（`model_desire`、`model_real`） +- 射击时序命令（`shoot_beforehand_cmd`） +- 弹道调试数据（`bullet_solver_data`） + +## 2. 控制算法（主链路） + +### 2.1 模式状态机 + +模式切换在 `Controller::update` 中处理： + +- `RATE`：对 yaw/pitch 角速度指令积分，更新角度目标。 +- `TRACK`：调用弹道解算器，使用解算得到的 yaw/pitch 目标。 +- `DIRECT`：对目标点进行几何瞄准（`atan2` 计算 yaw/pitch）。 +- `TRAJ`：基于轨迹坐标系姿态叠加偏置命令。 + +### 2.2 目标生成与关节限位 + +目标生成流程： + +1. 生成云台目标姿态（`setDes`）。 +2. 将目标姿态转换为相对底座的 RPY。 +3. 按 URDF 关节上下限对各轴命令进行约束（`setDesIntoLimit`）。 +4. 发布/记录目标变换（`odom -> gimbal_des`）。 + +### 2.3 关节闭环控制 + +关节控制策略由以下部分组成： + +- yaw/pitch 位置误差外环（`control_toolbox::Pid`） +- 目标平滑（`NonlinearTrackingDifferentiator`） +- 速度前馈（`yaw_k_v`、`pitch_k_v`） +- IMU 角速度补偿 +- 底盘角速度补偿 +- pitch 轴可选重力前馈 + +在 `TRACK` 模式下，yaw 还可使用 `BulletSolver` 提供的轨迹型目标 yaw 与轨迹前馈量。 + +## 3. 弹道与装甲选择算法 + +### 3.1 弹道模型 + +`BulletSolver` 先按弹速区间选择阻力系数，再迭代求解满足命中条件的 yaw/pitch： + +- 用含阻力模型估计飞行时间 +- 在重力 + 阻力条件下计算弹丸竖直位移 +- 用飞行时间更新目标位置 +- 迭代直至误差收敛或达到迭代上限 + +### 3.2 装甲板选择 + +装甲索引切换依据包括： + +- 目标自旋角速度（`v_yaw`） +- 飞行时间后的预测 yaw 偏差 +- 切换角阈值与滞回 +- 用于切换确认的拟合计数 + +求解器会更新 `selected_armor_`，并在“跟踪单块装甲”与“中心跟踪”行为间切换。 + +### 3.3 切装甲过渡轨迹 + +当满足切换条件时，系统会生成 yaw 三次多项式过渡轨迹： + +- 边界条件：起止位置与起止速度 +- 求解多项式系数（`a0..a3`） +- 在切换时长内按时间评估轨迹目标 +- 在过渡窗口输出轨迹前馈力矩 + +### 3.4 射击时序决策 + +`judgeShootBeforehand` 输出以下三种之一： + +- `BAN_SHOOT` +- `ALLOW_SHOOT` +- `JUDGE_BY_ERROR` + +决策依据为切换时间窗、配置延迟和目标旋转速度。 + +## 备注 + +- 本报告是用于架构与算法理解的文档产物。 +- 它不会引入任何代码行为变更。 diff --git a/rm_gimbal_controllers/CMakeLists.txt b/rm_gimbal_controllers/CMakeLists.txt index 98e8736b..9a3a0e1c 100644 --- a/rm_gimbal_controllers/CMakeLists.txt +++ b/rm_gimbal_controllers/CMakeLists.txt @@ -14,6 +14,7 @@ add_definitions(-Wall -Werror) find_package(catkin REQUIRED COMPONENTS roscpp + std_msgs rm_common controller_interface effort_controllers @@ -45,6 +46,7 @@ catkin_package( LIBRARIES CATKIN_DEPENDS roscpp + std_msgs rm_common effort_controllers tf2_eigen @@ -65,7 +67,7 @@ include_directories( ) ## Declare a cpp library -add_library(${PROJECT_NAME} src/gimbal_base.cpp src/bullet_solver.cpp src/ballistic_solver.cpp) +add_library(${PROJECT_NAME} src/gimbal_base.cpp src/dual_yaw_controller.cpp src/bullet_solver.cpp src/ballistic_solver.cpp) ## Specify libraries to link executable targets against target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 3cf38627..0abda770 100644 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -10,22 +10,26 @@ gen.add("resistance_coff_qd_15", double_t, 0, "Air resistance divided by mass of gen.add("resistance_coff_qd_16", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) gen.add("resistance_coff_qd_18", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) gen.add("resistance_coff_qd_30", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) +gen.add("resistance_coff_qd_800", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) gen.add("g", double_t, 0, "Air resistance divided by mass", 9.8, 9.6, 10.0) gen.add("delay", double_t, 0, "Delay of bullet firing", 0.0, 0, 0.5) +gen.add("outpost_delay", double_t, 0, "Delay of bullet firing of outpost", 0.0, 0, 0.5) gen.add("center_delay", double_t, 0, "Delay of shooting this target when in center mode", 0.105, -0.5, 0.5) gen.add("max_switch_angle", double_t, 0, "Max switch angle", 40.0, 0.0, 90.0) gen.add("min_switch_angle", double_t, 0, "Min switch angle", 2.0, 0.0, 90.0) gen.add("switch_angle_offset", double_t, 0, "Switch angle offset", 0.0, -20.0, 20) -gen.add("switch_duration_scale",double_t, 0,"A param of gimbal switch duration", 0.0, -99.0, 99.0) -gen.add("switch_duration_rate",double_t, 0,"A param of gimbal switch duration", 0.0, -99.0, 99.0) -gen.add("switch_duration_offset",double_t, 0,"A param of gimbal switch duration", 0.0, -99.0, 99.0) -gen.add("min_fit_switch_count", int_t, 0, "Min count that current angle fit switch angle", 0, 3, 99) -gen.add("max_selected_armor_", int_t, 0, "Max selected armor number", 0, -4, 4) +gen.add("outpost_switch_angle_offset", double_t, 0,"Switch angle offset", 0.0, -20.0, 20) +gen.add("min_fit_switch_count", int_t, 0, "Min count that current angle fit switch angle", 0, 1, 99) +gen.add("traject_start_fit_", int_t, 0, "The time of traject start ahead of switch", 0, 1, 99) gen.add("min_shoot_beforehand_vel", double_t, 0, "Min velocity to shoot beforehand", 3.0, 0.0, 20) gen.add("track_rotate_target_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0) gen.add("track_move_target_delay", double_t, 0, "Used to estimate current armor's position when moving", 0.0, -1.0, 1.0) +gen.add("yaw_max_acc", double_t, 0, "the max acc of yaw joint" , 0.0, -400.0, 400.0) +gen.add("track_rotate_outpost_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0) gen.add("traject_ahead_",double_t, 0, "Used to switch in advance ", 0.0, -5.0, 5.0) gen.add("clean_shoot_num_", int_t, 0, "Used to clean shoot_num", 0, 0, 1) gen.add("end_pos_offset",double_t, 0, "The offset of the end pos", 0.0 , -1.0, 1.0) +gen.add("traject_k_effort",double_t, 0, "The offset of the traject_switch_time", 0.0 , -10.0, 10.0) +gen.add("traject_k_vel",double_t, 0, "The offset of the traject_switch_time", 0.0 , -10.0, 10.0) exit(gen.generate(PACKAGE, "bullet_solver", "BulletSolver")) diff --git a/rm_gimbal_controllers/cfg/GimbalBase.cfg b/rm_gimbal_controllers/cfg/GimbalBase.cfg old mode 100644 new mode 100755 index 5f614c33..6c6a3107 --- a/rm_gimbal_controllers/cfg/GimbalBase.cfg +++ b/rm_gimbal_controllers/cfg/GimbalBase.cfg @@ -13,5 +13,6 @@ gen.add("chassis_comp_a_",double_t, 0,"A param of chassis_compensation", 0.0, 0, gen.add("chassis_comp_b_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0) gen.add("chassis_comp_c_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0) gen.add("chassis_comp_d_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0) +gen.add("moment_of_inertia_", double_t, 0, "Moment of inertia of gimbal", 0.0, 0, 999.0) exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase")) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h index 163f8475..c8b1d396 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -57,15 +57,15 @@ namespace rm_gimbal_controllers { struct Config { - double resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18, - resistance_coff_qd_30, g, delay, center_delay, max_switch_angle, switch_angle_offset, switch_duration_scale, - switch_duration_rate, switch_duration_offset, min_shoot_beforehand_vel, track_rotate_target_delay, - track_move_target_delay; - int min_fit_switch_count; - int max_selected_armor_; + double resistance_coff_qd_1, resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, + resistance_coff_qd_18, resistance_coff_qd_30, resistance_coff_qd_800, g, delay, outpost_delay, center_delay, + max_switch_angle, switch_angle_offset, outpost_switch_angle_offset, min_shoot_beforehand_vel, + track_rotate_target_delay, track_move_target_delay, yaw_max_acc, track_rotate_outpost_delay; + int min_fit_switch_count, traject_start_fit_; double traject_ahead_; int clean_shoot_num_; double end_pos_offset; + double traject_k_effort, traject_k_vel_; }; struct TrajectoryFunctionCoefficients { @@ -82,22 +82,27 @@ class BulletSolver explicit BulletSolver(ros::NodeHandle& controller_nh); bool solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, double v_yaw, - double r1, double r2, double dz, int armors_num, double start_vel); + double r1, double r2, double dz, double armors_num, double start_vel, double track_id); double getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1, - double r2, double dz, int armors_num, double yaw_real, double pitch_real, double bullet_speed); - double getResistanceCoefficient(double bullet_speed) const; + double r2, double dz, double armors_num, double yaw_real, double pitch_real, + double bullet_speed); + double getResistanceCoefficient(double target_distance) const; double getYaw() const { - return output_yaw_; + return output_yaw_[0]; } double getPitch() const { - return -output_pitch_; + return -output_pitch_[0]; } double getTrajectYaw() const { return traject_output_yaw_; } + double getTrajectVel() const + { + return traject_vel_; + } bool getUsingtraject() const { return using_traject_; @@ -110,26 +115,32 @@ class BulletSolver { return traject_effort_ff_; } - bool getTrackTarget() + bool getTrackTarget() const { return track_target_; } - double getGimbalSwitchDuration(double v_yaw); + void CleanTrackCount() + { + track_count_ = 0; + } + double getFlyTime() + { + return fly_time_[0]; + } void getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel, geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, - double r1, double r2, double dz, int armors_num); - void judgeShootBeforehand(const ros::Time& time, double v_yaw); + double r1, double r2, double dz, double armors_num); + uint8_t judgeShootBeforehand(double v_yaw, int id); void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); void identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg); void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); - double planningPoint(ros::Time& time, ros::Time& start_trajectory_time_); + double planningPoint(ros::Time& time, ros::Time& start_trajectory_time_, double v_yaw); void heatCB(const rm_msgs::LocalHeatStateConstPtr& msg); ~BulletSolver() = default; private: std::shared_ptr> path_desire_pub_; std::shared_ptr> path_real_pub_; - std::shared_ptr> shoot_beforehand_cmd_pub_; std::shared_ptr> fly_time_pub_; std::shared_ptr> bullet_solver_pub; ros::Subscriber identified_target_change_sub_; @@ -138,46 +149,58 @@ class BulletSolver realtime_tools::RealtimeBuffer config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; Config config_{}; - double max_track_target_vel_; - double output_yaw_{}, output_pitch_{}, traject_output_yaw_{}; + double yaw_[150], pos_x[150], pos_y[150]; + double max_track_target_vel_{}; + double output_yaw_[150], output_pitch_[150], traject_output_yaw_{}; double bullet_speed_{}, resistance_coff_{}; - double fly_time_; - double switch_hysteresis_; + double fly_time_[150]; + double switch_hysteresis_{}; double last_yaw_{}, filtered_yaw_{}; double gimbal_switch_duration_{}; - double yaw_subtract_; - double switch_armor_angle; + double yaw_subtract_{}; + double switch_armor_angle{}; double filtered_v_yaw_{}; - double switchtime; - double traject_effort_ff_; - double traject_switch_time_; - double switche_time_yaw_; - double traject_max_acc_; - int shoot_num_ = 0; - int shoot_beforehand_cmd_{}; - int count_; - int traject_count_; - int ban_shoot_count_ = 0; - int selected_armor_ = 0; + double switchtime{}; + double traject_effort_ff_{}; + double traject_switch_time_{}; + double switche_time_yaw_{}; + double traject_max_acc_{}; + double last_output_yaw_{}; + double traject_vel_{}; + double r_traject_{}; + double intital_yaw_{}; + geometry_msgs::Vector3 pos_acc_{}; + geometry_msgs::Vector3 filter_vel_[3]{}; - bool track_target_ = true; + int shoot_num_{}; + int shoot_beforehand_cmd_{}; + int count_[150]{}; + int next_count_[150]{}; + int ban_shoot_count_{}; + int selected_armor_[150] = {}; + int last_selected_armor_ = {}; + bool track_target_ = false; + int track_count_{}; bool identified_target_change_ = true; - bool is_in_delay_before_switch_{}; bool dynamic_reconfig_initialized_{}; - bool change_armor = false; - bool using_traject_; - bool last_shoot_state_; + bool using_traject_{}; + bool last_shoot_state_{}; + bool is_aheading_two_[150]{}; + bool start_traject_{}; + double filtered_vel_des{}; geometry_msgs::Point after_traject_output_yaw_{}; - geometry_msgs::Point target_pos_{}; + geometry_msgs::Point target_pos_[150]{}; visualization_msgs::Marker marker_desire_; visualization_msgs::Marker marker_real_; - ros::Time start_using_traject_time; - ros::Time ban_shoot_time_; + ros::Time start_using_traject_time{}; + ros::Time ban_shoot_time_{}; + ros::Time last_output_time_{}; + ros::Time ban_shoot_start_time_{}; - TrajectoryFunctionCoefficients trajectory_function_coefficients; - TrajectoryLimitParams stauts_limit_; + TrajectoryFunctionCoefficients trajectory_function_coefficients{}; + TrajectoryLimitParams stauts_limit_{}; - mutable std::mutex heat_mutex_; + mutable std::mutex heat_mutex_{}; }; } // namespace rm_gimbal_controllers diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/dual_yaw_controller.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/dual_yaw_controller.h new file mode 100644 index 00000000..85dccb45 --- /dev/null +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/dual_yaw_controller.h @@ -0,0 +1,38 @@ +/******************************************************************************* + * BSD 3-Clause License + ******************************************************************************/ + +#pragma once + +#include + +namespace rm_gimbal_controllers +{ +class DualYawController : public Controller +{ +public: + DualYawController() = default; + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; + +protected: + bool shouldInitializeController(const std::string& name, const urdf::JointConstSharedPtr& joint_urdf, + int axis) const override; + bool shouldApplyJointLimit(int axis) const override; + std::string getBaseFrameID(const std::unordered_map& joint_urdfs) override; + void updateYawJoint(const ros::Time& time, const ros::Duration& period, const geometry_msgs::Vector3& angular_vel, + const double pos_real[3], const double pos_des[3], const double pos_des_temp[3], + const double vel_des[3], const double angle_error[3], const double traject_pos_des[3], + const double traject_angle_error[3]) override; + +private: + bool initBaseYaw(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& controller_nh); + bool getTrackArmorSetPoint(const ros::Time& time, double& armor_set_point); + void publishBaseYawState(const ros::Time& time, double base_yaw_pos_real, double base_yaw_set_point, + const double vel_des[3], double base_yaw_error); + + urdf::JointConstSharedPtr base_yaw_joint_; + std::unique_ptr base_yaw_ctrl_; + std::unique_ptr base_yaw_pid_pos_; + std::unique_ptr> base_yaw_pos_state_pub_; +}; +} // namespace rm_gimbal_controllers diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index 50c3eff2..42b5ea17 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -44,12 +44,12 @@ #include #include #include +#include #include #include #include #include #include -#include #include #include #include @@ -57,22 +57,24 @@ #include #include #include -#include -#include -#include +#include +#include +#include namespace rm_gimbal_controllers { struct GimbalConfig { double yaw_k_v_{}, pitch_k_v_{}, accel_pitch_{}, accel_yaw_{}; - double chassis_comp_a_{}, chassis_comp_b_{}, chassis_comp_c_{}, chassis_comp_d_{}; + double chassis_comp_a_{}, chassis_comp_b_{}, chassis_comp_c_{}, + chassis_comp_d_{}; // sine wave compensation, a * sin(b * chassis_angular_z + c) + d + double moment_of_inertia_{}; }; class ChassisVel { public: - explicit ChassisVel(const ros::NodeHandle& nh) + ChassisVel(const ros::NodeHandle& nh) { double num_data; nh.param("num_data", num_data, 20.0); @@ -141,26 +143,46 @@ class Controller : public controller_interface::MultiInterfaceController& joint_urdfs); + virtual std::string getBaseFrameID(const std::unordered_map& joint_urdfs); + virtual void updatePitchJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double vel_des[3], + const double angle_error[3]); + virtual void updateYawJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double pos_real[3], + const double pos_des[3], const double pos_des_temp[3], const double vel_des[3], + const double angle_error[3], const double traject_pos_des[3], + const double traject_angle_error[3]); + void updateGimbalYawController(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, + effort_controllers::JointVelocityController& ctrl, control_toolbox::Pid& pid_pos, + double k_v, const double vel_des[3], const double angle_error[3], + const double traject_angle_error[3]); void rate(const ros::Time& time, const ros::Duration& period); - void track(const ros::Time& time); + void track(const ros::Time& time, const ros::Duration& period); + void externalAimTrack(const ros::Time& time); void direct(const ros::Time& time); void traj(const ros::Time& time); + bool externalAimIsFresh(const ros::Time& time) const; bool setDesIntoLimit(double& angle, const urdf::JointConstSharedPtr& joint_urdf, bool update, double current_angle); void moveJoint(const ros::Time& time, const ros::Duration& period); - void updateChassisVel(); - void updateBallisticSolution(const ros::Time& time); double gravityFeedForward(const ros::Time& time); + void updateChassisVel(); double updateCompensation(double chassis_vel_angular_z); + void publishShootBeforehand(const ros::Time& time, uint8_t cmd); void commandCB(const rm_msgs::GimbalCmdConstPtr& msg); void trackCB(const rm_msgs::TrackDataConstPtr& msg); - void ballisticSolverRequestCB(const std_msgs::BoolConstPtr& msg); + void mpcGimbalAimCB(const rm_msgs::MPCGimbalAimCmdConstPtr& msg); void reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t); - std::string getGimbalFrameID(std::unordered_map joint_urdfs); - std::string getBaseFrameID(std::unordered_map joint_urdfs); + bool TrackSolver_typeCB(rm_msgs::StatusChangeRequest& req, rm_msgs::StatusChangeResponse& res); + void publishTargetIsArmor(); rm_control::RobotStateHandle robot_state_handle_; hardware_interface::ImuSensorHandle imu_sensor_handle_; @@ -169,34 +191,43 @@ class Controller : public controller_interface::MultiInterfaceController joint_urdfs_; std::unordered_map pos_des_in_limit_; bool has_imu_ = true; - double ballistic_yaw_{}, ballistic_pitch_{}; std::shared_ptr bullet_solver_; - std::shared_ptr ballistic_solver_; + double gimbal_real_z_vel_{}; + enum class TrackSolverType + { + BULLET_SOLVER, + EXTERNAL_MPC_AIM + }; + TrackSolverType track_solver_type_{ TrackSolverType::BULLET_SOLVER }; + double external_aim_timeout_{ 0.1 }; + bool external_aim_active_{ false }; // ROS Interface ros::Time last_publish_time_{}; - ros::Time last_ballistic_publish_time_{}; + ros::Time last_track_time_{}; std::unordered_map>> pos_state_pub_; std::shared_ptr> error_pub_; - std::shared_ptr> ballistic_solution_pub_; + std::shared_ptr> shoot_beforehand_cmd_pub_; ros::Subscriber cmd_gimbal_sub_; ros::Subscriber data_track_sub_; - ros::Subscriber ballistic_solver_request_sub_; + ros::Subscriber mpc_gimbal_aim_sub_; + ros::ServiceServer TrackSolver_type_srv_; + ros::Publisher target_is_armor_pub_; realtime_tools::RealtimeBuffer cmd_rt_buffer_; realtime_tools::RealtimeBuffer track_rt_buffer_; - realtime_tools::RealtimeBuffer ballistic_track_rt_buffer_; + realtime_tools::RealtimeBuffer gimbal_aim_rt_buffer_; rm_msgs::GimbalCmd cmd_gimbal_; rm_msgs::TrackData data_track_; + rm_msgs::MPCGimbalAimCmd gimbal_aim_; std::string gimbal_des_frame_id_{}, imu_name_{}, gimbal_traject_des_frame_id_; double publish_rate_{}; bool state_changed_{}; int loop_count_{}; // Transform - geometry_msgs::TransformStamped odom2gimbal_des_, odom2gimbal_, odom2base_, last_odom2base_, base2gimbal_, - odom2gimbal_traject_des_; + geometry_msgs::TransformStamped odom2gimbal_des_, odom2gimbal_, odom2base_, last_odom2base_, odom2gimbal_traject_des_; // Gravity Compensation geometry_msgs::Vector3 mass_origin_; @@ -221,9 +252,12 @@ class Controller : public controller_interface::MultiInterfaceControllercatkin roscpp + std_msgs rm_common diff --git a/rm_gimbal_controllers/rm_gimbal_controllers_plugins.xml b/rm_gimbal_controllers/rm_gimbal_controllers_plugins.xml index 8f436457..b12da6bd 100644 --- a/rm_gimbal_controllers/rm_gimbal_controllers_plugins.xml +++ b/rm_gimbal_controllers/rm_gimbal_controllers_plugins.xml @@ -8,4 +8,11 @@ + + + The DualYawController controls base_yaw, yaw and pitch from one GimbalCmd source. + + + diff --git a/rm_gimbal_controllers/src/ballistic_solver.cpp b/rm_gimbal_controllers/src/ballistic_solver.cpp index 97131f3b..f9c8284d 100644 --- a/rm_gimbal_controllers/src/ballistic_solver.cpp +++ b/rm_gimbal_controllers/src/ballistic_solver.cpp @@ -64,9 +64,9 @@ void BallisticSolver::solver(const geometry_msgs::TransformStamped& base2gimbal, double error_after_step = error_function(current_pitch + config.newton_pitch_epsilon); double error_derivative = (error_after_step - error) / config.newton_pitch_epsilon; double pitch_adjustment = error / error_derivative; - pitch_adjustment = (pitch_adjustment < -config.max_newton_step) ? -config.max_newton_step : - (pitch_adjustment > config.max_newton_step) ? config.max_newton_step : - pitch_adjustment; + pitch_adjustment = (pitch_adjustment < -config.max_newton_step) ? + -config.max_newton_step : + (pitch_adjustment > config.max_newton_step) ? config.max_newton_step : pitch_adjustment; double update_pitch = current_pitch - pitch_adjustment; double update_error = error_function(update_pitch); if (std::abs(update_error) < config.newton_convergence_tol) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 06f330f3..a604342d 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -40,6 +40,7 @@ #include #include #include +#include namespace rm_gimbal_controllers { @@ -51,22 +52,25 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .resistance_coff_qd_16 = getParam(controller_nh, "resistance_coff_qd_16", 0.), .resistance_coff_qd_18 = getParam(controller_nh, "resistance_coff_qd_18", 0.), .resistance_coff_qd_30 = getParam(controller_nh, "resistance_coff_qd_30", 0.), + .resistance_coff_qd_800 = getParam(controller_nh, "resistance_coff_qd_800", 0.001), .g = getParam(controller_nh, "g", 0.), .delay = getParam(controller_nh, "delay", 0.), + .outpost_delay = getParam(controller_nh, "outpost_delay", 0.), .center_delay = getParam(controller_nh, "center_delay", 0.0), - .max_switch_angle = getParam(controller_nh, "max_switch_angle", 40.0), .switch_angle_offset = getParam(controller_nh, "switch_angle_offset", 0.0), - .switch_duration_scale = getParam(controller_nh, "switch_duration_scale", 0.), - .switch_duration_rate = getParam(controller_nh, "switch_duration_rate", 0.), - .switch_duration_offset = getParam(controller_nh, "switch_duration_offset", 0.1), - .min_shoot_beforehand_vel = getParam(controller_nh, "min_shoot_beforehand_vel", 3.0), + .outpost_switch_angle_offset = getParam(controller_nh, "outpost_switch_angle_offset", 0.0), + .min_shoot_beforehand_vel = getParam(controller_nh, "min_shoot_beforehand_vel", 2.5), .track_rotate_target_delay = getParam(controller_nh, "track_rotate_target_delay", 0.), .track_move_target_delay = getParam(controller_nh, "track_move_target_delay", 0.), + .yaw_max_acc = getParam(controller_nh, "yaw_max_acc", 60.), + .track_rotate_outpost_delay = getParam(controller_nh, "track_rotate_outpost_delay", 0.), .min_fit_switch_count = getParam(controller_nh, "min_fit_switch_count", 3), - .max_selected_armor_ = getParam(controller_nh, "max_selecteed_armor", 3), + .traject_start_fit_ = getParam(controller_nh, "traject_start_fit_", 45), .traject_ahead_ = getParam(controller_nh, "traject_ahead_", 1.), .clean_shoot_num_ = getParam(controller_nh, "clean_shoot_num_", 1), .end_pos_offset = getParam(controller_nh, "end_pos_offset", 0.), + .traject_k_effort = getParam(controller_nh, "traject_k_effort", 0.0), + .traject_k_vel_ = getParam(controller_nh, "traject_k_vel", 0.65), }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); switch_hysteresis_ = getParam(controller_nh, "switch_hysteresis", 1.0); @@ -96,8 +100,6 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) new realtime_tools::RealtimePublisher(controller_nh, "model_desire", 10)); path_real_pub_.reset( new realtime_tools::RealtimePublisher(controller_nh, "model_real", 10)); - shoot_beforehand_cmd_pub_.reset( - new realtime_tools::RealtimePublisher(controller_nh, "shoot_beforehand_cmd", 10)); fly_time_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "fly_time", 10)); bullet_solver_pub.reset( new realtime_tools::RealtimePublisher(controller_nh, "bullet_solver_data", 10)); @@ -107,352 +109,385 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) &BulletSolver::heatCB, this); } -double BulletSolver::getResistanceCoefficient(double bullet_speed) const +double BulletSolver::getResistanceCoefficient(double target_distance) const { - // bullet_speed have 5 value:10,15,16,18,30 double resistance_coff; - if (bullet_speed < 12.5) - resistance_coff = config_.resistance_coff_qd_10; - else if (bullet_speed < 15.5) - resistance_coff = config_.resistance_coff_qd_15; - else if (bullet_speed < 17) - resistance_coff = config_.resistance_coff_qd_16; - else if (bullet_speed < 24) - resistance_coff = config_.resistance_coff_qd_18; - else - resistance_coff = config_.resistance_coff_qd_30; + // hero + resistance_coff = config_.resistance_coff_qd_800; return resistance_coff; } bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, - double v_yaw, double r1, double r2, double dz, int armors_num, double start_vel) + double v_yaw, double r1, double r2, double dz, double armors_num, double start_vel, + double track_id) { + while (yaw > M_PI / 2) + yaw -= M_PI; + while (yaw < -M_PI / 2) + yaw += M_PI; + if (armors_num == 3) + { + armors_num = 3.6; + } config_ = *config_rt_buffer_.readFromRT(); bullet_speed_ = bullet_speed; - resistance_coff_ = getResistanceCoefficient(bullet_speed_) != 0 ? getResistanceCoefficient(bullet_speed_) : 0.001; if (abs(yaw - last_yaw_) > 1.) filtered_yaw_ = yaw; else if (last_yaw_ != yaw) - { filtered_yaw_ = filtered_yaw_ + (yaw - filtered_yaw_) * (0.001 / (0.01 + 0.001)); - } last_yaw_ = yaw; filtered_v_yaw_ = filtered_v_yaw_ + (v_yaw - filtered_v_yaw_) * 0.05; - double temp_z = pos.z; - double target_rho = std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)); - double output_yaw_central = std::atan2(pos.y, pos.x); - output_pitch_ = std::atan2(temp_z, std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2))); - double rough_fly_time = - (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / resistance_coff_; - double r = r1; - double z = pos.z; - if (track_target_) - yaw += filtered_v_yaw_ * config_.track_rotate_target_delay; - pos.x += vel.x * config_.track_move_target_delay; - pos.y += vel.y * config_.track_move_target_delay; - if (track_target_) + if (track_count_ <= 300) { - if (std::abs(filtered_v_yaw_) >= max_track_target_vel_ + switch_hysteresis_) - track_target_ = false; + track_target_ = false; + track_count_++; } - else + if (track_count_ > 300) { - if (std::abs(filtered_v_yaw_) <= max_track_target_vel_ - switch_hysteresis_) + if (std::abs(filtered_v_yaw_) >= max_track_target_vel_ + switch_hysteresis_) + track_target_ = false; + else if (std::abs(filtered_v_yaw_) <= max_track_target_vel_ - switch_hysteresis_) track_target_ = true; - } - switch_armor_angle = acos(r1 / target_rho) - config_.switch_angle_offset; - traject_switch_time_ = 1.321 * exp(-0.289 * abs(filtered_v_yaw_ + 1.0)) * config_.traject_ahead_; - // traject_switch_time_ = (switch_armor_angle - config_.traject_ahead_ - switche_time_yaw_) / filtered_v_yaw_; - // yaw_subtract_ = filtered_yaw_ - output_yaw_central; - yaw_subtract_ = filtered_yaw_ - output_yaw_central; - while (yaw_subtract_ > M_PI) - yaw_subtract_ -= 2 * M_PI; - while (yaw_subtract_ < -M_PI) - yaw_subtract_ += 2 * M_PI; - double after_fly_yaw_subtract_ = yaw_subtract_ + v_yaw * rough_fly_time; - if (((after_fly_yaw_subtract_ < switch_armor_angle - 0.3 && filtered_v_yaw_ > 1.) || - (after_fly_yaw_subtract_ > switch_armor_angle + 0.3 && filtered_v_yaw_ < -1.)) && - track_target_) - { - selected_armor_ = 0; - } - else if (!track_target_) - { - selected_armor_ = 0; + if (sqrt(pow(pos.x, 2) + pow(pos.y, 2)) < 1.2) + track_target_ = false; } - if ((after_fly_yaw_subtract_ > switch_armor_angle && filtered_v_yaw_ > 1.) || - (after_fly_yaw_subtract_ < switch_armor_angle && filtered_v_yaw_ < -1.)) + if (identified_target_change_) { - count_++; - if (change_armor) + for (int i = 0; i < 150; i++) { - count_ = 0; - change_armor = false; - switche_time_yaw_ = after_fly_yaw_subtract_ + selected_armor_ * 2 * M_PI / armors_num; + count_[i] = 0; + next_count_[i] = 0; } - if (count_ >= config_.min_fit_switch_count) + identified_target_change_ = false; + } + for (int i = 0; i < 150; i++) + { + double temp_z = pos.z; + if (track_target_) { - selected_armor_ = v_yaw > 0. ? -1 : 1; - r = armors_num == 4 ? r2 : r1; - z = pos.z + dz; - if ((after_fly_yaw_subtract_ - 2 * M_PI / armors_num > switch_armor_angle) && v_yaw > 0.) - { - selected_armor_ = -2; - } - else if ((after_fly_yaw_subtract_ + 2 * M_PI / armors_num < switch_armor_angle) && v_yaw < 0.) + if (track_id == 6) { - selected_armor_ = 2; + yaw_[i] = yaw + filtered_v_yaw_ * (config_.track_rotate_outpost_delay + i * 0.001); } - if (count_ == config_.min_fit_switch_count) + else { - switch_armor_time_ = ros::Time::now(); - traject_count_ = 0; + yaw_[i] = yaw + filtered_v_yaw_ * (config_.track_rotate_target_delay + i * 0.001); } } - } - if (ros::Time::now().toSec() > (switch_armor_time_.toSec() + traject_switch_time_) && abs(filtered_v_yaw_) > 2. && - count_ > 2) - { - traject_count_++; - if (traject_count_ == 2) + else + yaw_[i] = yaw; + pos_x[i] = pos.x + vel.x * (config_.track_move_target_delay + i * 0.001); + pos_y[i] = pos.y + vel.y * (config_.track_move_target_delay + i * 0.001); + double target_rho = std::sqrt(std::pow(pos_x[i], 2) + std::pow(pos_y[i], 2)); + resistance_coff_ = getResistanceCoefficient(target_rho) != 0 ? getResistanceCoefficient(target_rho) : 0.001; + double output_yaw_central = std::atan2(pos_y[i], pos_x[i]); + output_pitch_[i] = std::atan2(temp_z, std::sqrt(std::pow(pos_x[i], 2) + std::pow(pos_y[i], 2))); + double rough_fly_time = + (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_[i])))) / + resistance_coff_; + if (bullet_speed == 0.0) { - using_traject_ = true; - start_using_traject_time = ros::Time::now(); + ROS_ERROR("solve bullet speed is zero"); + return false; } - } - double filtered_aim_armor_yaw = filtered_yaw_ + selected_armor_ * 1.57; - double aim_yaw_subtract = filtered_aim_armor_yaw - output_yaw_central; - while (aim_yaw_subtract > M_PI) - aim_yaw_subtract -= 2 * M_PI; - while (aim_yaw_subtract < -M_PI) - aim_yaw_subtract += 2 * M_PI; - is_in_delay_before_switch_ = - ((((aim_yaw_subtract + filtered_v_yaw_ * config_.delay) > switch_armor_angle) && filtered_v_yaw_ > 1.) || - (((aim_yaw_subtract + filtered_v_yaw_ * config_.delay) < -switch_armor_angle) && filtered_v_yaw_ < -1.)) && - track_target_; - int count{}; - double error = 999; - if (track_target_) - { - target_pos_.x = pos.x - r * cos(yaw + selected_armor_ * 1.57); - target_pos_.y = pos.y - r * sin(yaw + selected_armor_ * 1.57); - } - else - { - target_pos_.x = pos.x - r * cos(atan2(pos.y, pos.x)); - target_pos_.y = pos.y - r * sin(atan2(pos.y, pos.x)); - if ((filtered_v_yaw_ > 1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay)) > 0.5) || - (filtered_v_yaw_ < -1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay)) < -0.5)) + double r = r1; + double z = pos.z; + + switch_armor_angle = acos(r1 / target_rho) - config_.switch_angle_offset; + if (track_id == 6) { - selected_armor_ = filtered_v_yaw_ > 0. ? -1 : 1; + switch_armor_angle = acos(r1 / target_rho) - config_.outpost_switch_angle_offset; } - if (((filtered_v_yaw_ > 1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 1 * 2 * M_PI / armors_num) > 0.5) || - (filtered_v_yaw_ < -1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 1 * 2 * M_PI / armors_num) < -0.5))) + yaw_subtract_ = yaw_[i] - output_yaw_central; + // while (yaw_subtract_ > M_PI) + // yaw_subtract_ -= 2 * M_PI; + // while (yaw_subtract_ < -M_PI) + // yaw_subtract_ += 2 * M_PI; + double after_fly_yaw_subtract_ = yaw_subtract_ + v_yaw * rough_fly_time; + if (abs(v_yaw) < 2.) + selected_armor_[i] = 0; + else if ((after_fly_yaw_subtract_ < switch_armor_angle - 0.4 && filtered_v_yaw_ > 2.) || + (after_fly_yaw_subtract_ > -switch_armor_angle + 0.4 && filtered_v_yaw_ < -2.)) + selected_armor_[i] = 0; + + if ((after_fly_yaw_subtract_ > switch_armor_angle && filtered_v_yaw_ > 2.) || + (after_fly_yaw_subtract_ < -switch_armor_angle && filtered_v_yaw_ < -2.)) { - selected_armor_ = filtered_v_yaw_ > 0. ? -2 : 2; + count_[i]++; + if (count_[i] >= config_.min_fit_switch_count) + { + selected_armor_[i] = v_yaw > 0. ? -1 : 1; + r = armors_num == 4 ? r2 : r1; + z = pos.z + dz; + if ((after_fly_yaw_subtract_ - 2 * M_PI / armors_num > switch_armor_angle) && v_yaw > 0.) + { + selected_armor_[i] = -2; + r = armors_num == 4 ? r1 : r2; + z = pos.z; + next_count_[i]++; + if (next_count_[i] == config_.min_fit_switch_count) + { + switch_armor_time_ = ros::Time::now(); + is_aheading_two_[i] = true; + } + } + else if ((after_fly_yaw_subtract_ + 2 * M_PI / armors_num < -switch_armor_angle) && v_yaw < 0.) + { + selected_armor_[i] = 2; + r = armors_num == 4 ? r1 : r2; + z = pos.z; + next_count_[i]++; + if (next_count_[i] == config_.min_fit_switch_count) + { + switch_armor_time_ = ros::Time::now(); + is_aheading_two_[i] = true; + } + } + if (count_[i] == config_.min_fit_switch_count) + { + if (is_aheading_two_[i]) + { + is_aheading_two_[i] = false; + } + else + { + switch_armor_time_ = ros::Time::now(); + } + } + } } - if (((filtered_v_yaw_ > 1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 2 * 2 * M_PI / armors_num) > 0.5) || - (filtered_v_yaw_ < -1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 2 * 2 * M_PI / armors_num) < -0.5))) + int count{}; + double error = 999; + if (track_target_) { - selected_armor_ = filtered_v_yaw_ > 0. ? -3 : 3; + target_pos_[i].x = pos_x[i] - r * cos(yaw_[i] + selected_armor_[i] * 2 * M_PI / armors_num); + target_pos_[i].y = pos_y[i] - r * sin(yaw_[i] + selected_armor_[i] * 2 * M_PI / armors_num); } - if ((selected_armor_ > 0 && selected_armor_ > config_.max_selected_armor_) || - (selected_armor_ < 0 && selected_armor_ < -config_.max_selected_armor_)) + else { - selected_armor_ = selected_armor_ > 0 ? config_.max_selected_armor_ : -config_.max_selected_armor_; + target_pos_[i].x = pos_x[i] - r * cos(atan2(pos_y[i], pos_x[i])); + target_pos_[i].y = pos_y[i] - r * sin(atan2(pos_y[i], pos_x[i])); + selected_armor_[i] = 0; + if ((filtered_v_yaw_ > 2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay)) > 0.7) || + (filtered_v_yaw_ < -2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay)) < -0.7)) + { + selected_armor_[i] = filtered_v_yaw_ > 0. ? -1 : 1; + } + if (((filtered_v_yaw_ > 2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) - + 1 * 2 * M_PI / armors_num) > 0.7) || + (filtered_v_yaw_ < -2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) + + 1 * 2 * M_PI / armors_num) < -0.7))) + { + selected_armor_[i] = filtered_v_yaw_ > 0. ? -2 : 2; + } + if (((filtered_v_yaw_ > 2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) - + 2 * 2 * M_PI / armors_num) > 0.7) || + (filtered_v_yaw_ < -2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) + + 2 * 2 * M_PI / armors_num) < -0.7))) + { + selected_armor_[i] = filtered_v_yaw_ > 0. ? -3 : 3; + } + + if (selected_armor_[i] % 2 == 0) + { + r = r1; + z = pos.z; + } + else + { + r = r2; + z = pos.z + dz; + } } - if (selected_armor_ % 2 == 0) + target_pos_[i].z = z; + while (error >= 0.001) { - r = r1; - z = pos.z; + output_yaw_[i] = std::atan2(target_pos_[i].y, target_pos_[i].x); + output_pitch_[i] = std::atan2(temp_z, std::sqrt(std::pow(target_pos_[i].x, 2) + std::pow(target_pos_[i].y, 2))); + target_rho = std::sqrt(std::pow(target_pos_[i].x, 2) + std::pow(target_pos_[i].y, 2)); + fly_time_[i] = (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_[i])))) / + resistance_coff_; + double real_z = (bullet_speed_ * std::sin(output_pitch_[i]) + (config_.g / resistance_coff_)) * + (1 - std::exp(-resistance_coff_ * fly_time_[i])) / resistance_coff_ - + config_.g * fly_time_[i] / resistance_coff_; + if (track_target_) + { + target_pos_[i].x = pos_x[i] + vel.x * fly_time_[i] - + r * cos(yaw_[i] + v_yaw * fly_time_[i] + selected_armor_[i] * 2 * M_PI / armors_num); + target_pos_[i].y = pos_y[i] + vel.y * fly_time_[i] - + r * sin(yaw_[i] + v_yaw * fly_time_[i] + selected_armor_[i] * 2 * M_PI / armors_num); + } + else + { + double target_pos_after_fly_time[2]; + target_pos_after_fly_time[0] = pos_x[i] + vel.x * fly_time_[i]; + target_pos_after_fly_time[1] = pos_y[i] + vel.y * fly_time_[i]; + target_pos_[i].x = + target_pos_after_fly_time[0] - r * cos(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); + target_pos_[i].y = + target_pos_after_fly_time[1] - r * sin(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); + } + target_pos_[i].z = z + vel.z * fly_time_[i]; + + double target_yaw = std::atan2(target_pos_[i].y, target_pos_[i].x); + double error_theta = target_yaw - output_yaw_[i]; + double error_z = target_pos_[i].z - real_z; + temp_z += error_z; + error = std::sqrt(std::pow(error_theta * target_rho, 2) + std::pow(error_z, 2)); + count++; + if (count >= 20 || std::isnan(error)) + return false; } } - target_pos_.z = z; - double r_trajcet_; - if (selected_armor_ == 1 || selected_armor_ == -1) + if (fly_time_pub_->trylock()) { - r_trajcet_ = r1; + fly_time_pub_->msg_.data = fly_time_[0]; + fly_time_pub_->unlockAndPublish(); + } + if (selected_armor_[0] == 1 || selected_armor_[0] == -1 || selected_armor_[0] == 3 || selected_armor_[0] == -3) + { + r_traject_ = r1; } else { - r_trajcet_ = r2; + r_traject_ = r2; } - - if (!using_traject_) + for (int i = 1; i <= config_.traject_start_fit_; i++) { - switchtime = 0.08; - - if (filtered_v_yaw_ > 0) - { - after_traject_output_yaw_.x = - pos.x + vel.x * (fly_time_ + switchtime) - - r_trajcet_ * cos(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ - 1) * 2 * M_PI / armors_num); - after_traject_output_yaw_.y = - pos.y + vel.y * (fly_time_ + switchtime) - - r_trajcet_ * sin(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ - 1) * 2 * M_PI / armors_num); - } - else + if (selected_armor_[i] != selected_armor_[0] && using_traject_ == false && abs(v_yaw) > 4.5) { - after_traject_output_yaw_.x = - pos.x + vel.x * (fly_time_ + switchtime) - - r_trajcet_ * cos(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ + 1) * 2 * M_PI / armors_num); - after_traject_output_yaw_.y = - pos.y + vel.y * (fly_time_ + switchtime) - - r_trajcet_ * sin(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ + 1) * 2 * M_PI / armors_num); + start_traject_ = true; + using_traject_ = true; + start_using_traject_time = ros::Time::now(); + break; } - stauts_limit_.start_pos = output_yaw_; - stauts_limit_.start_vel = start_vel; - stauts_limit_.end_pos = - std::atan2(after_traject_output_yaw_.y, after_traject_output_yaw_.x) + config_.end_pos_offset; - stauts_limit_.end_vel = stauts_limit_.start_vel; + } - if (switchtime > 0.2) + if (start_traject_ && using_traject_) + { + traject_max_acc_ = 400.; + switchtime = 0.05; + while (abs(traject_max_acc_) > config_.yaw_max_acc) { - using_traject_ = false; - } - trajectory_function_coefficients.a0 = stauts_limit_.start_pos; - trajectory_function_coefficients.a1 = stauts_limit_.start_vel; - - Eigen::Matrix2d A; - A << std::pow(switchtime, 2), std::pow(switchtime, 3), 2 * switchtime, 3 * std::pow(switchtime, 2); - Eigen::Vector2d B; - B << stauts_limit_.end_pos - (stauts_limit_.start_pos + stauts_limit_.start_vel * switchtime), - stauts_limit_.end_vel - stauts_limit_.start_vel; + switchtime += 0.005; + if (filtered_v_yaw_ > 0) + { + after_traject_output_yaw_.x = pos_x[0] + vel.x * (fly_time_[0] + switchtime) - + r_traject_ * cos(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] - 1) * 2 * M_PI / armors_num); + after_traject_output_yaw_.y = pos_y[0] + vel.y * (fly_time_[0] + switchtime) - + r_traject_ * sin(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] - 1) * 2 * M_PI / armors_num); + } + else + { + after_traject_output_yaw_.x = pos_x[0] + vel.x * (fly_time_[0] + switchtime) - + r_traject_ * cos(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] + 1) * 2 * M_PI / armors_num); + after_traject_output_yaw_.y = pos_y[0] + vel.y * (fly_time_[0] + switchtime) - + r_traject_ * sin(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] + 1) * 2 * M_PI / armors_num); + } + stauts_limit_.start_pos = output_yaw_[0]; + stauts_limit_.start_vel = start_vel; + stauts_limit_.end_pos = + std::atan2(after_traject_output_yaw_.y, after_traject_output_yaw_.x) + config_.end_pos_offset; + stauts_limit_.end_pos = + stauts_limit_.start_pos + angles::shortest_angular_distance(stauts_limit_.start_pos, stauts_limit_.end_pos); + stauts_limit_.end_vel = stauts_limit_.start_vel; - Eigen::Vector2d X = A.colPivHouseholderQr().solve(B); + if (switchtime > 0.15) + { + break; + } + trajectory_function_coefficients.a0 = stauts_limit_.start_pos; + trajectory_function_coefficients.a1 = stauts_limit_.start_vel; - trajectory_function_coefficients.a2 = X(0); - trajectory_function_coefficients.a3 = X(1); - traject_max_acc_ = 2 * trajectory_function_coefficients.a2; - } + Eigen::Matrix2d A; + A << std::pow(switchtime, 2), std::pow(switchtime, 3), 2 * switchtime, 3 * std::pow(switchtime, 2); + Eigen::Vector2d B; + B << stauts_limit_.end_pos - (stauts_limit_.start_pos + stauts_limit_.start_vel * switchtime), + stauts_limit_.end_vel - stauts_limit_.start_vel; - while (error >= 0.001) - { - output_yaw_ = std::atan2(target_pos_.y, target_pos_.x); - output_pitch_ = std::atan2(temp_z, std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2))); - target_rho = std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2)); - fly_time_ = - (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / resistance_coff_; - double real_z = (bullet_speed_ * std::sin(output_pitch_) + (config_.g / resistance_coff_)) * - (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_ - - config_.g * fly_time_ / resistance_coff_; + Eigen::Vector2d X = A.colPivHouseholderQr().solve(B); - if (track_target_) - { - target_pos_.x = - pos.x + vel.x * fly_time_ - r * cos(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num); - target_pos_.y = - pos.y + vel.y * fly_time_ - r * sin(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num); + trajectory_function_coefficients.a2 = X(0); + trajectory_function_coefficients.a3 = X(1); + traject_max_acc_ = abs(2 * trajectory_function_coefficients.a2); } - else - { - double target_pos_after_fly_time[2]; - target_pos_after_fly_time[0] = pos.x + vel.x * fly_time_; - target_pos_after_fly_time[1] = pos.y + vel.y * fly_time_; - target_pos_.x = - target_pos_after_fly_time[0] - r * cos(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); - target_pos_.y = - target_pos_after_fly_time[1] - r * sin(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); - } - target_pos_.z = z + vel.z * fly_time_; - - double target_yaw = std::atan2(target_pos_.y, target_pos_.x); - double error_theta = target_yaw - output_yaw_; - double error_z = target_pos_.z - real_z; - temp_z += error_z; - error = std::sqrt(std::pow(error_theta * target_rho, 2) + std::pow(error_z, 2)); - count++; - - if (count >= 20 || std::isnan(error)) - return false; - } - if (fly_time_pub_->trylock()) - { - fly_time_pub_->msg_.data = fly_time_; - fly_time_pub_->unlockAndPublish(); + start_traject_ = false; } if (using_traject_) { ros::Time temp = ros::Time::now(); - traject_output_yaw_ = planningPoint(temp, start_using_traject_time); + traject_output_yaw_ = planningPoint(temp, start_using_traject_time, v_yaw); + while (traject_output_yaw_ > M_PI) + traject_output_yaw_ -= 2 * M_PI; + while (traject_output_yaw_ < -M_PI) + traject_output_yaw_ += 2 * M_PI; if (ros::Time::now() - start_using_traject_time > ros::Duration(switchtime)) { using_traject_ = false; } } - else + if (!using_traject_ || !track_target_) { - traject_output_yaw_ = output_yaw_; + traject_output_yaw_ = output_yaw_[0]; } + double vel_des = (traject_output_yaw_ - last_output_yaw_) / (ros::Time::now().toSec() - last_output_time_.toSec()); + filtered_vel_des = filtered_vel_des + (vel_des - filtered_vel_des) * (0.001 / (0.01 + 0.001)); + last_output_time_ = ros::Time::now(); + last_output_yaw_ = traject_output_yaw_; if (bullet_solver_pub->trylock()) { - bullet_solver_pub->msg_.selected_armor_ = selected_armor_; - bullet_solver_pub->msg_.yaw_subtract_ = yaw_subtract_; - bullet_solver_pub->msg_.output_yaw_central = output_yaw_central; - bullet_solver_pub->msg_.after_fly_yaw_subtract_ = after_fly_yaw_subtract_; - bullet_solver_pub->msg_.switch_armor_angle = switch_armor_angle; - bullet_solver_pub->msg_.test_number = traject_switch_time_; - bullet_solver_pub->msg_.switche_time_yaw_ = switche_time_yaw_; - bullet_solver_pub->msg_.traject_acc_ = traject_max_acc_; - bullet_solver_pub->msg_.test_number = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 2 * 2 * M_PI / armors_num); - if (v_yaw > 1.0) + for (int i = 0; i < 150; ++i) { - bullet_solver_pub->msg_.center_this_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 0 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 1 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_diagonal_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 2 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_triple_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 3 * 2 * M_PI / armors_num); - } - else if (v_yaw < -1.0) - { - bullet_solver_pub->msg_.center_this_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 0 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 1 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_diagonal_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 2 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_triple_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 3 * 2 * M_PI / armors_num); + bullet_solver_pub->msg_.selected_armor_[i] = static_cast(selected_armor_[i]); + bullet_solver_pub->msg_.count_number_[i] = static_cast(count_[i]); } + bullet_solver_pub->msg_.start_vel = stauts_limit_.start_vel; + bullet_solver_pub->msg_.switchtime = switchtime; + bullet_solver_pub->msg_.traject_output_yaw_ = traject_output_yaw_; + bullet_solver_pub->msg_.traject_acc_ = traject_effort_ff_; + bullet_solver_pub->msg_.traject_vel_ = filtered_vel_des; + bullet_solver_pub->msg_.using_traject_ = using_traject_; + bullet_solver_pub->msg_.test_number[0] = yaw_[0]; bullet_solver_pub->unlockAndPublish(); } return true; } + void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel, geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, - double v_yaw, double r1, double r2, double dz, int armors_num) + double v_yaw, double r1, double r2, double dz, double armors_num) { + if (armors_num == 3) + { + armors_num = 3.6; + } double r = r1, z = pos.z; - if (armors_num == 4 && selected_armor_ != 0 && armors_num == 3) + if (armors_num == 4 && selected_armor_[0] % 2 != 0) { r = r2; z = pos.z + dz; } - pos.x += vel.x * (config_.track_move_target_delay + fly_time_); - pos.y += vel.y * (config_.track_move_target_delay + fly_time_); + pos.x += vel.x * (config_.track_move_target_delay + fly_time_[0]); + pos.y += vel.y * (config_.track_move_target_delay + fly_time_[0]); if (track_target_) { - armor_pos.x = pos.x - r * cos(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + - selected_armor_ * 2 * M_PI / armors_num); - armor_pos.y = pos.y - r * sin(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + - selected_armor_ * 2 * M_PI / armors_num); + armor_pos.x = pos.x - r * cos(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 2 * M_PI / armors_num); + armor_pos.y = pos.y - r * sin(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 2 * M_PI / armors_num); armor_pos.z = z; armor_vel.x = vel.x + v_yaw * r * - sin(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + - selected_armor_ * 2 * M_PI / armors_num); + sin(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 2 * M_PI / armors_num); armor_vel.y = vel.y - v_yaw * r * - cos(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + - selected_armor_ * 2 * M_PI / armors_num); + cos(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 2 * M_PI / armors_num); armor_vel.z = vel.z; } else @@ -470,18 +505,18 @@ void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pi double roll{}, pitch{}, yaw{}; quatToRPY(odom2pitch.transform.rotation, roll, pitch, yaw); geometry_msgs::Point point_desire{}, point_real{}; - double target_rho = std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2)); + double target_rho = std::sqrt(std::pow(target_pos_[0].x, 2) + std::pow(target_pos_[0].y, 2)); int point_num = int(target_rho * 20); for (int i = 0; i <= point_num; i++) { double rt_bullet_rho = target_rho * i / point_num; - double fly_time = (-std::log(1 - rt_bullet_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / + double fly_time = (-std::log(1 - rt_bullet_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_[0])))) / resistance_coff_; - double rt_bullet_z = (bullet_speed_ * std::sin(output_pitch_) + (config_.g / resistance_coff_)) * + double rt_bullet_z = (bullet_speed_ * std::sin(output_pitch_[0]) + (config_.g / resistance_coff_)) * (1 - std::exp(-resistance_coff_ * fly_time)) / resistance_coff_ - config_.g * fly_time / resistance_coff_; - point_desire.x = rt_bullet_rho * std::cos(output_yaw_) + odom2pitch.transform.translation.x; - point_desire.y = rt_bullet_rho * std::sin(output_yaw_) + odom2pitch.transform.translation.y; + point_desire.x = rt_bullet_rho * std::cos(output_yaw_[0]) + odom2pitch.transform.translation.x; + point_desire.y = rt_bullet_rho * std::sin(output_yaw_[0]) + odom2pitch.transform.translation.y; point_desire.z = rt_bullet_z + odom2pitch.transform.translation.z; marker_desire_.points.push_back(point_desire); } @@ -513,14 +548,18 @@ void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pi } double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, - double r1, double r2, double dz, int armors_num, double yaw_real, double pitch_real, - double bullet_speed) + double r1, double r2, double dz, double armors_num, double yaw_real, + double pitch_real, double bullet_speed) { + if (armors_num == 3) + { + armors_num = 3.6; + } config_ = *config_rt_buffer_.readFromRT(); double delay; delay = track_target_ ? 0. : config_.center_delay; double r, z; - if (selected_armor_ % 2 == 0) + if (selected_armor_[0] % 2 == 0) { r = r1; z = pos.z; @@ -534,28 +573,28 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec if (track_target_) { double bullet_rho = - bullet_speed * std::cos(pitch_real) * (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_; + bullet_speed * std::cos(pitch_real) * (1 - std::exp(-resistance_coff_ * fly_time_[0])) / resistance_coff_; double bullet_x = bullet_rho * std::cos(yaw_real); double bullet_y = bullet_rho * std::sin(yaw_real); double bullet_z = (bullet_speed * std::sin(pitch_real) + (config_.g / resistance_coff_)) * - (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_ - - config_.g * fly_time_ / resistance_coff_; - error = std::sqrt(std::pow(target_pos_.x - bullet_x, 2) + std::pow(target_pos_.y - bullet_y, 2) + - std::pow(target_pos_.z - bullet_z, 2)); + (1 - std::exp(-resistance_coff_ * fly_time_[0])) / resistance_coff_ - + config_.g * fly_time_[0] / resistance_coff_; + error = std::sqrt(std::pow(target_pos_[0].x - bullet_x, 2) + std::pow(target_pos_[0].y - bullet_y, 2) + + std::pow(target_pos_[0].z - bullet_z, 2)); } else { geometry_msgs::Point target_pos_after_fly_time_and_delay{}; target_pos_after_fly_time_and_delay.x = - pos.x + vel.x * (fly_time_ + delay) - - r * cos(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num); + pos.x + vel.x * (fly_time_[0] + delay) - + r * cos(yaw + v_yaw * (fly_time_[0] + delay) + selected_armor_[0] * 2 * M_PI / armors_num); target_pos_after_fly_time_and_delay.y = - pos.y + vel.y * (fly_time_ + delay) - - r * sin(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num); - target_pos_after_fly_time_and_delay.z = z + vel.z * (fly_time_ + delay); - error = std::sqrt(std::pow(target_pos_.x - target_pos_after_fly_time_and_delay.x, 2) + - std::pow(target_pos_.y - target_pos_after_fly_time_and_delay.y, 2) + - std::pow(target_pos_.z - target_pos_after_fly_time_and_delay.z, 2)); + pos.y + vel.y * (fly_time_[0] + delay) - + r * sin(yaw + v_yaw * (fly_time_[0] + delay) + selected_armor_[0] * 2 * M_PI / armors_num); + target_pos_after_fly_time_and_delay.z = z + vel.z * (fly_time_[0] + delay); + error = std::sqrt(std::pow(target_pos_[0].x - target_pos_after_fly_time_and_delay.x, 2) + + std::pow(target_pos_[0].y - target_pos_after_fly_time_and_delay.y, 2) + + std::pow(target_pos_[0].z - target_pos_after_fly_time_and_delay.z, 2)); } return error; } @@ -566,47 +605,53 @@ void BulletSolver::identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg) identified_target_change_ = true; else if (!msg->data && identified_target_change_) { - change_armor = true; identified_target_change_ = false; } } -double BulletSolver::getGimbalSwitchDuration(double v_yaw) -{ - gimbal_switch_duration_ = - config_.switch_duration_scale * std::exp(config_.switch_duration_rate * (v_yaw * 60 / (2 * M_PI))) + - config_.switch_duration_offset; - return gimbal_switch_duration_; -} - -void BulletSolver::judgeShootBeforehand(const ros::Time& time, double v_yaw) +uint8_t BulletSolver::judgeShootBeforehand(double v_yaw, int id) { if (!track_target_) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; else if (std::abs(v_yaw) > config_.min_shoot_beforehand_vel) { - if ((ros::Time::now().toSec() > (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay)) && - (ros::Time::now().toSec() < (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay) + switchtime)) + int ban_shoot_delay_number = static_cast(55 + config_.delay * 1000); + if (id == 6) + { + ban_shoot_delay_number = static_cast(55 + config_.outpost_delay * 1000); + } + ban_shoot_delay_number = ban_shoot_delay_number > 149 ? 149 : ban_shoot_delay_number; + if (selected_armor_[ban_shoot_delay_number] != selected_armor_[0]) + { + ban_shoot_count_++; + shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; + if (ban_shoot_count_ == 1) + { + ban_shoot_start_time_ = ros::Time::now(); + } + } + else if (using_traject_ && ros::Time::now().toSec() < ban_shoot_start_time_.toSec() + switchtime) + { shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; - else if (ros::Time::now().toSec() > - (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay) + switchtime && - ros::Time::now().toSec() < - (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay) + switchtime + config_.delay) + } + else if (ros::Time::now().toSec() > ban_shoot_start_time_.toSec() + switchtime && + ros::Time::now().toSec() < ban_shoot_start_time_.toSec() + switchtime + config_.delay) + { shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT; + ban_shoot_count_ = 0; + } else + { shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; + ban_shoot_count_ = 0; + } } else shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; - if (shoot_beforehand_cmd_pub_->trylock()) - { - shoot_beforehand_cmd_pub_->msg_.stamp = time; - shoot_beforehand_cmd_pub_->msg_.cmd = shoot_beforehand_cmd_; - shoot_beforehand_cmd_pub_->unlockAndPublish(); - } + return static_cast(shoot_beforehand_cmd_); } -double BulletSolver::planningPoint(ros::Time& time, ros::Time& start_trajectory_time_) +double BulletSolver::planningPoint(ros::Time& time, ros::Time& start_trajectory_time_, double v_yaw) { double a0 = trajectory_function_coefficients.a0; double a1 = trajectory_function_coefficients.a1; @@ -614,8 +659,8 @@ double BulletSolver::planningPoint(ros::Time& time, ros::Time& start_trajectory_ double a3 = trajectory_function_coefficients.a3; double n_time_ = (time - start_trajectory_time_).toSec(); double plansetpoint = a0 + a1 * n_time_ + a2 * pow(n_time_, 2) + a3 * pow(n_time_, 3); - // traject_effort_ff_ = 1.4 - 1.4 * n_time_/switchtime; - traject_effort_ff_ = 0.0; + traject_effort_ff_ = (2 * a2 + 6 * a3 * n_time_) / (2 * a2) * 1.0 * config_.traject_k_effort; + traject_vel_ = (a1 + 2 * a2 * n_time_ + 3 * a3 * pow(n_time_, 2)) * config_.traject_k_vel_; return plansetpoint; } @@ -646,20 +691,22 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.resistance_coff_qd_30 = init_config.resistance_coff_qd_30; config.g = init_config.g; config.delay = init_config.delay; + config.outpost_delay = init_config.outpost_delay; config.center_delay = init_config.center_delay; - config.max_switch_angle = init_config.max_switch_angle; config.switch_angle_offset = init_config.switch_angle_offset; - config.switch_duration_scale = init_config.switch_duration_scale; - config.switch_duration_rate = init_config.switch_duration_rate; - config.switch_duration_offset = init_config.switch_duration_offset; + config.outpost_switch_angle_offset = init_config.outpost_switch_angle_offset; config.min_shoot_beforehand_vel = init_config.min_shoot_beforehand_vel; config.track_rotate_target_delay = init_config.track_rotate_target_delay; config.track_move_target_delay = init_config.track_move_target_delay; + config.yaw_max_acc = init_config.yaw_max_acc; + config.track_rotate_outpost_delay = init_config.track_rotate_outpost_delay; config.min_fit_switch_count = init_config.min_fit_switch_count; - config.max_selected_armor_ = init_config.max_selected_armor_; + config.traject_start_fit_ = init_config.traject_start_fit_; config.traject_ahead_ = init_config.traject_ahead_; config.clean_shoot_num_ = init_config.clean_shoot_num_; config.end_pos_offset = init_config.end_pos_offset; + config.traject_k_effort = init_config.traject_k_effort; + config.traject_k_vel = init_config.traject_k_vel_; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .resistance_coff_qd_10 = config.resistance_coff_qd_10, @@ -667,22 +714,25 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .resistance_coff_qd_16 = config.resistance_coff_qd_16, .resistance_coff_qd_18 = config.resistance_coff_qd_18, .resistance_coff_qd_30 = config.resistance_coff_qd_30, + .resistance_coff_qd_800 = config.resistance_coff_qd_800, .g = config.g, .delay = config.delay, + .outpost_delay = config.outpost_delay, .center_delay = config.center_delay, - .max_switch_angle = config.max_switch_angle, .switch_angle_offset = config.switch_angle_offset, - .switch_duration_scale = config.switch_duration_scale, - .switch_duration_rate = config.switch_duration_rate, - .switch_duration_offset = config.switch_duration_offset, + .outpost_switch_angle_offset = config.outpost_switch_angle_offset, .min_shoot_beforehand_vel = config.min_shoot_beforehand_vel, .track_rotate_target_delay = config.track_rotate_target_delay, .track_move_target_delay = config.track_move_target_delay, + .yaw_max_acc = config.yaw_max_acc, + .track_rotate_outpost_delay = config.track_rotate_outpost_delay, .min_fit_switch_count = config.min_fit_switch_count, - .max_selected_armor_ = config.max_selected_armor_, + .traject_start_fit_ = config.traject_start_fit_, .traject_ahead_ = config.traject_ahead_, .clean_shoot_num_ = config.clean_shoot_num_, - .end_pos_offset = config.end_pos_offset }; + .end_pos_offset = config.end_pos_offset, + .traject_k_effort = config.traject_k_effort, + .traject_k_vel_ = config.traject_k_vel }; config_rt_buffer_.writeFromNonRT(config_non_rt); } } // namespace rm_gimbal_controllers diff --git a/rm_gimbal_controllers/src/dual_yaw_controller.cpp b/rm_gimbal_controllers/src/dual_yaw_controller.cpp new file mode 100644 index 00000000..69ffa1b4 --- /dev/null +++ b/rm_gimbal_controllers/src/dual_yaw_controller.cpp @@ -0,0 +1,154 @@ +#include "rm_gimbal_controllers/dual_yaw_controller.h" + +#include +#include +#include +#include +#include +#include +#include + +namespace rm_gimbal_controllers +{ +bool DualYawController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh) +{ + if (!initBaseYaw(robot_hw, controller_nh)) + return false; + return Controller::init(robot_hw, root_nh, controller_nh); +} + +bool DualYawController::initBaseYaw(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& controller_nh) +{ + ros::NodeHandle base_yaw_nh(controller_nh, "controllers/base_yaw"); + ros::NodeHandle base_yaw_pid_nh(controller_nh, "controllers/base_yaw/pid_pos"); + + urdf::Model urdf; + if (!urdf.initParamWithNodeHandle("robot_description", controller_nh)) + { + ROS_ERROR("Failed to parse urdf file"); + return false; + } + base_yaw_joint_ = urdf.getJoint(getParam(base_yaw_nh, "joint", std::string("base_yaw_joint"))); + if (!base_yaw_joint_) + { + ROS_ERROR("Could not find base_yaw joint in urdf"); + return false; + } + + hardware_interface::EffortJointInterface* effort_joint_interface = + robot_hw->get(); + base_yaw_ctrl_ = std::make_unique(); + base_yaw_pid_pos_ = std::make_unique(); + base_yaw_pos_state_pub_ = + std::make_unique>(base_yaw_nh, "pos_state", 1); + return base_yaw_ctrl_->init(effort_joint_interface, base_yaw_nh) && base_yaw_pid_pos_->init(base_yaw_pid_nh); +} + +bool DualYawController::shouldInitializeController(const std::string& /*name*/, + const urdf::JointConstSharedPtr& joint_urdf, int axis) const +{ + return !(axis == 2 && base_yaw_joint_ && joint_urdf->name == base_yaw_joint_->name); +} + +bool DualYawController::shouldApplyJointLimit(int axis) const +{ + return axis != 2; +} + +std::string DualYawController::getBaseFrameID(const std::unordered_map& joint_urdfs) +{ + if (base_yaw_joint_) + return base_yaw_joint_->parent_link_name.c_str(); + return Controller::getBaseFrameID(joint_urdfs); +} + +void DualYawController::updateYawJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double pos_real[3], + const double pos_des[3], const double /*pos_des_temp*/[3], + const double vel_des[3], const double angle_error[3], + const double traject_pos_des[3], const double traject_angle_error[3]) +{ + if (pid_pos_.find(2) != pid_pos_.end() && ctrls_.find(2) != ctrls_.end()) + { + updateGimbalYawController(time, period, angular_vel, *ctrls_.at(2), *pid_pos_.at(2), config_.yaw_k_v_, vel_des, + angle_error, traject_angle_error); + } + + if (!base_yaw_ctrl_ || !base_yaw_pid_pos_) + return; + + double base_yaw_pos_real = 0.; + try + { + geometry_msgs::TransformStamped odom2base_yaw = + robot_state_handle_.lookupTransform("odom", base_yaw_joint_->child_link_name, time); + double roll{}, pitch{}; + quatToRPY(odom2base_yaw.transform.rotation, roll, pitch, base_yaw_pos_real); + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return; + } + + double armor_set_point{}; + double base_yaw_set_point = getTrackArmorSetPoint(time, armor_set_point) ? armor_set_point : pos_des[2]; + double base_yaw_error = angles::shortest_angular_distance(base_yaw_pos_real, base_yaw_set_point); + base_yaw_pid_pos_->computeCommand(base_yaw_error, period); + base_yaw_ctrl_->setCommand(base_yaw_pid_pos_->getCurrentCmd() - chassis_vel_->angular_->z()); + base_yaw_ctrl_->update(time, period); + publishBaseYawState(time, base_yaw_pos_real, base_yaw_set_point, vel_des, base_yaw_error); +} + +bool DualYawController::getTrackArmorSetPoint(const ros::Time& time, double& armor_set_point) +{ + if (state_ != TRACK || !base_yaw_joint_ || data_track_.header.stamp.isZero()) + return false; + + geometry_msgs::Point target_pos = data_track_.position; + if (!std::isfinite(target_pos.x) || !std::isfinite(target_pos.y) || !std::isfinite(target_pos.z)) + return false; + + try + { + geometry_msgs::TransformStamped odom2base_yaw = + robot_state_handle_.lookupTransform("odom", base_yaw_joint_->child_link_name, time); + geometry_msgs::Point base_yaw_pos_odom; + base_yaw_pos_odom.x = odom2base_yaw.transform.translation.x; + base_yaw_pos_odom.y = odom2base_yaw.transform.translation.y; + base_yaw_pos_odom.z = odom2base_yaw.transform.translation.z; + + armor_set_point = std::atan2(target_pos.y - base_yaw_pos_odom.y, target_pos.x - base_yaw_pos_odom.x); + if (!std::isfinite(armor_set_point)) + return false; + + return true; + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return false; + } +} + +void DualYawController::publishBaseYawState(const ros::Time& time, double base_yaw_pos_real, double base_yaw_set_point, + const double vel_des[3], double base_yaw_error) +{ + if (base_yaw_pos_state_pub_ && loop_count_ % 10 == 0 && base_yaw_pos_state_pub_->trylock()) + { + base_yaw_pos_state_pub_->msg_.header.stamp = time; + base_yaw_pos_state_pub_->msg_.set_point = base_yaw_set_point; + base_yaw_pos_state_pub_->msg_.traject_set_point = base_yaw_set_point; + base_yaw_pos_state_pub_->msg_.set_point_dot = vel_des[2]; + base_yaw_pos_state_pub_->msg_.process_value = base_yaw_pos_real; + base_yaw_pos_state_pub_->msg_.error = base_yaw_error; + base_yaw_pos_state_pub_->msg_.command = base_yaw_pid_pos_->getCurrentCmd(); + base_yaw_pos_state_pub_->msg_.shoot_number = bullet_solver_->getShootnum(); + base_yaw_pos_state_pub_->unlockAndPublish(); + } +} + +} // namespace rm_gimbal_controllers + +PLUGINLIB_EXPORT_CLASS(rm_gimbal_controllers::DualYawController, controller_interface::ControllerBase) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index e7c67730..0c5d5b75 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -36,6 +36,7 @@ // #include "rm_gimbal_controllers/gimbal_base.h" +#include #include #include #include @@ -48,27 +49,6 @@ namespace rm_gimbal_controllers { bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) { - ros::NodeHandle chassis_vel_nh(controller_nh, "chassis_vel"); - chassis_vel_ = std::make_shared(chassis_vel_nh); - ros::NodeHandle nh_bullet_solver = ros::NodeHandle(controller_nh, "bullet_solver"); - bullet_solver_ = std::make_shared(nh_bullet_solver); - ros::NodeHandle nh_ballistic_solver = ros::NodeHandle(controller_nh, "ballistic_solver"); - ballistic_solver_ = std::make_shared(nh_ballistic_solver); - - config_ = { .yaw_k_v_ = getParam(controller_nh, "controllers/yaw/k_v", 0.), - .pitch_k_v_ = getParam(controller_nh, "controllers/pitch/k_v", 0.), - .accel_pitch_ = getParam(controller_nh, "controllers/pitch/accel", 99.), - .accel_yaw_ = getParam(controller_nh, "controllers/yaw/accel", 99.), - .chassis_comp_a_ = getParam(controller_nh, "controllers/yaw/chassis_comp_a", 0.), - .chassis_comp_b_ = getParam(controller_nh, "controllers/yaw/chassis_comp_b", 0.), - .chassis_comp_c_ = getParam(controller_nh, "controllers/yaw/chassis_comp_c", 0.), - .chassis_comp_d_ = getParam(controller_nh, "controllers/yaw/chassis_comp_d", 0.) }; - config_rt_buffer_.initRT(config_); - d_srv_ = new dynamic_reconfigure::Server(controller_nh); - dynamic_reconfigure::Server::CallbackType cb = - [this](auto&& PH1, auto&& PH2) { reconfigCB(PH1, PH2); }; - d_srv_->setCallback(cb); - XmlRpc::XmlRpcValue xml_rpc_value; const bool enable_feedforward = controller_nh.getParam("feedforward", xml_rpc_value); if (enable_feedforward) @@ -82,7 +62,57 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro gravity_ = enable_feedforward ? (double)xml_rpc_value["gravity"] : 0.; enable_gravity_compensation_ = enable_feedforward && (bool)xml_rpc_value["enable_gravity_compensation"]; - auto* effort_joint_interface = robot_hw->get(); + ros::NodeHandle chassis_vel_nh(controller_nh, "chassis_vel"); + chassis_vel_ = std::make_shared(chassis_vel_nh); + ros::NodeHandle nh_bullet_solver = ros::NodeHandle(controller_nh, "bullet_solver"); + bullet_solver_ = std::make_shared(nh_bullet_solver); + std::string track_solver_type; + controller_nh.param("track_solver_type", track_solver_type, "bullet_solver"); + if (track_solver_type == "external_aim") + { + track_solver_type_ = TrackSolverType::EXTERNAL_MPC_AIM; + ROS_WARN("[Gimbal] track_solver_type=external_aim is deprecated, using external_mpc_aim instead"); + } + else if (track_solver_type == "external_mpc_aim") + { + track_solver_type_ = TrackSolverType::EXTERNAL_MPC_AIM; + ROS_INFO("[Gimbal] TRACK mode uses external MPC aim solver"); + } + else if (track_solver_type != "bullet_solver") + { + track_solver_type_ = TrackSolverType::BULLET_SOLVER; + ROS_WARN("[Gimbal] Unknown track_solver_type '%s', falling back to bullet_solver", track_solver_type.c_str()); + } + controller_nh.param("external_aim_timeout", external_aim_timeout_, 0.1); + std::string external_mpc_aim_topic; + controller_nh.param("external_mpc_aim_topic", external_mpc_aim_topic, "/sp_vision/mpc_gimbal_aim"); + + config_ = { + .yaw_k_v_ = getParam(controller_nh, "controllers/yaw/k_v", 0.), + .pitch_k_v_ = getParam(controller_nh, "controllers/pitch/k_v", 0.), + .accel_pitch_ = getParam(controller_nh, "controllers/pitch/accel", 99.), + .accel_yaw_ = getParam(controller_nh, "controllers/yaw/accel", 99.), + .chassis_comp_a_ = getParam(controller_nh, "controllers/yaw/chassis_comp_a", 0.), + .chassis_comp_b_ = getParam(controller_nh, "controllers/yaw/chassis_comp_b", 0.), + .chassis_comp_c_ = getParam(controller_nh, "controllers/yaw/chassis_comp_c", 0.), + .chassis_comp_d_ = getParam(controller_nh, "controllers/yaw/chassis_comp_d", 0.), + .moment_of_inertia_ = getParam(controller_nh, "moment_of_inertia", 0.), + }; + + config_rt_buffer_.initRT(config_); + d_srv_ = new dynamic_reconfigure::Server(controller_nh); + dynamic_reconfigure::Server::CallbackType cb = + [this](auto&& PH1, auto&& PH2) { reconfigCB(PH1, PH2); }; + d_srv_->setCallback(cb); + std::string target_is_armor_topic; + controller_nh.param("target_is_armor_topic", target_is_armor_topic, "/vision/target_is_armor"); + controller_nh.param("initial_target_is_armor", target_is_armor_, true); + target_is_armor_pub_ = root_nh.advertise(target_is_armor_topic, 1, true); + publishTargetIsArmor(); + TrackSolver_type_srv_ = root_nh.advertiseService("/Processor/status_change", &Controller::TrackSolver_typeCB, this); + + hardware_interface::EffortJointInterface* effort_joint_interface = + robot_hw->get(); if (controller_nh.getParam("controllers", xml_rpc_value)) { // Get URDF info about joint @@ -103,12 +133,15 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro return false; } const int axis = (joint_urdf->axis.x == 1) * 0 + (joint_urdf->axis.y == 1) * 1 + (joint_urdf->axis.z == 1) * 2; - joint_urdfs_.emplace(axis, std::move(joint_urdf)); - ctrls_.emplace(axis, std::make_unique()); - pid_pos_.emplace(axis, std::make_unique()); - pos_des_in_limit_.emplace(axis, true); - pos_state_pub_.emplace( - axis, std::make_unique>(nh, "pos_state", 1)); + if (!shouldInitializeController(it.first, joint_urdf, axis)) + continue; + joint_urdfs_.insert(std::make_pair(axis, joint_urdf)); + ctrls_.insert(std::make_pair(axis, std::make_unique())); + pid_pos_.insert(std::make_pair(axis, std::make_unique())); + pos_des_in_limit_.insert(std::make_pair(axis, true)); + pos_state_pub_.insert(std::make_pair( + axis, std::make_unique>(nh, "pos_state", 1))); + if (!ctrls_.at(axis)->init(effort_joint_interface, nh) || !pid_pos_.at(axis)->init(nh_pid_pos)) return false; } @@ -119,8 +152,10 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro if (has_imu_) { imu_name_ = getParam(controller_nh, "imu_name", static_cast("gimbal_imu")); - auto* imu_sensor_interface = robot_hw->get(); + hardware_interface::ImuSensorInterface* imu_sensor_interface = + robot_hw->get(); imu_sensor_handle_ = imu_sensor_interface->getHandle(imu_name_); + ROS_WARN_ONCE("has imu"); } else { @@ -137,18 +172,26 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro odom2base_.header.frame_id = "odom"; odom2base_.child_frame_id = getBaseFrameID(joint_urdfs_); odom2base_.transform.rotation.w = 1.; + odom2gimbal_traject_des_.header.frame_id = "odom"; odom2gimbal_traject_des_.child_frame_id = gimbal_traject_des_frame_id_; odom2gimbal_traject_des_.transform.rotation.w = 1.; + rm_msgs::GimbalCmd default_cmd; + rm_msgs::TrackData default_track; + rm_msgs::MPCGimbalAimCmd default_aim; + cmd_rt_buffer_.initRT(default_cmd); + track_rt_buffer_.initRT(default_track); + gimbal_aim_rt_buffer_.initRT(default_aim); + cmd_gimbal_sub_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); - data_track_sub_ = controller_nh.subscribe("/track", 1, &Controller::trackCB, this); - ballistic_solver_request_sub_ = controller_nh.subscribe("/ballistic_solver_request", 1, - &Controller::ballisticSolverRequestCB, this); + data_track_sub_ = controller_nh.subscribe("/sp_vision/track", 1, &Controller::trackCB, this); + mpc_gimbal_aim_sub_ = + controller_nh.subscribe(external_mpc_aim_topic, 1, &Controller::mpcGimbalAimCB, this); publish_rate_ = getParam(controller_nh, "publish_rate", 100.); error_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "error", 100)); - ballistic_solution_pub_.reset( - new realtime_tools::RealtimePublisher(controller_nh, "ballistic_solution", 100)); + shoot_beforehand_cmd_pub_.reset( + new realtime_tools::RealtimePublisher(controller_nh, "shoot_beforehand_cmd", 10)); return true; } @@ -164,19 +207,18 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) { cmd_gimbal_ = *cmd_rt_buffer_.readFromRT(); data_track_ = *track_rt_buffer_.readFromNonRT(); + gimbal_aim_ = *gimbal_aim_rt_buffer_.readFromRT(); config_ = *config_rt_buffer_.readFromRT(); try { odom2gimbal_ = robot_state_handle_.lookupTransform("odom", odom2gimbal_.child_frame_id, time); odom2base_ = robot_state_handle_.lookupTransform("odom", odom2base_.child_frame_id, time); - base2gimbal_ = robot_state_handle_.lookupTransform(odom2base_.child_frame_id, odom2gimbal_.child_frame_id, time); } catch (tf2::TransformException& ex) { ROS_WARN_THROTTLE(5, "%s\n", ex.what()); return; } - updateBallisticSolution(time); updateChassisVel(); if (state_ != cmd_gimbal_.mode) { @@ -189,7 +231,10 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) rate(time, period); break; case TRACK: - track(time); + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM && target_is_armor_) + externalAimTrack(time); + else + track(time, period); break; case DIRECT: direct(time); @@ -201,24 +246,40 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) moveJoint(time, period); } +bool Controller::shouldInitializeController(const std::string& /*name*/, + const urdf::JointConstSharedPtr& /*joint_urdf*/, int /*axis*/) const +{ + return true; +} + +bool Controller::shouldApplyJointLimit(int /*axis*/) const +{ + return true; +} + void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des, double traject_yaw_des, bool update_yaw, bool update_pitch) { - tf2::Quaternion odom2base, odom2gimbal_des, base2gimbal_des, base2gimbal_traject_des; + tf2::Quaternion odom2base, odom2gimbal_des_q, base2gimbal_des, base2gimbal_traject_des; tf2::fromMsg(odom2base_.transform.rotation, odom2base); - tf2::fromMsg(odom2gimbal_des_.transform.rotation, odom2gimbal_des); + tf2::fromMsg(odom2gimbal_des_.transform.rotation, odom2gimbal_des_q); double current_rpy[3], des_rpy[3], traject_rpy[3]; - quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des), current_rpy[0], current_rpy[1], current_rpy[2]); - odom2gimbal_des.setRPY(0, pitch_des, yaw_des); - quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des), des_rpy[0], des_rpy[1], des_rpy[2]); - odom2gimbal_des.setRPY(0, pitch_des, traject_yaw_des); - quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des), traject_rpy[0], traject_rpy[1], traject_rpy[2]); + quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des_q), current_rpy[0], current_rpy[1], current_rpy[2]); + odom2gimbal_des_q.setRPY(0, pitch_des, yaw_des); + quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des_q), des_rpy[0], des_rpy[1], des_rpy[2]); + odom2gimbal_des_q.setRPY(0, pitch_des, traject_yaw_des); + quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des_q), traject_rpy[0], traject_rpy[1], traject_rpy[2]); + for (const auto& it : joint_urdfs_) { - bool update = (it.first != 1 && it.first != 2) || (it.first == 1 && update_pitch) || (it.first == 2 && update_yaw); - pos_des_in_limit_[it.first] = setDesIntoLimit(des_rpy[it.first], it.second, update, current_rpy[it.first]); + bool update = (it.first == 1) ? update_pitch : ((it.first == 2) ? update_yaw : true); + if (shouldApplyJointLimit(it.first)) + pos_des_in_limit_[it.first] = setDesIntoLimit(des_rpy[it.first], it.second, update, current_rpy[it.first]); + else + pos_des_in_limit_[it.first] = true; } + base2gimbal_des.setRPY(des_rpy[0], des_rpy[1], des_rpy[2]); base2gimbal_traject_des.setRPY(traject_rpy[0], traject_rpy[1], traject_rpy[2]); odom2gimbal_des_.transform.rotation = tf2::toMsg(odom2base * base2gimbal_des); @@ -229,6 +290,7 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des, void Controller::rate(const ros::Time& time, const ros::Duration& period) { + bullet_solver_->CleanTrackCount(); if (state_changed_) { // on enter state_changed_ = false; @@ -254,8 +316,16 @@ void Controller::rate(const ros::Time& time, const ros::Duration& period) } } -void Controller::track(const ros::Time& time) +void Controller::track(const ros::Time& time, const ros::Duration& period) { + external_aim_active_ = false; + if (data_track_.header.stamp.toSec() - time.toSec() > 0.5) + { + state_ = RATE; + state_changed_ = true; + rate(time, period); + return; + } if (state_changed_) { // on enter state_changed_ = false; @@ -273,7 +343,7 @@ void Controller::track(const ros::Time& time) { if (!data_track_.header.frame_id.empty()) { - auto transform = + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform("odom", data_track_.header.frame_id, data_track_.header.stamp); tf2::doTransform(target_pos, target_pos, transform); tf2::doTransform(target_vel, target_vel, transform); @@ -296,8 +366,8 @@ void Controller::track(const ros::Time& time) target_vel.z -= chassis_vel_->linear_->z(); bool solve_success = bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, yaw, data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, data_track_.dz, - data_track_.armors_num, ctrls_.at(2)->joint_.getVelocity()); - bullet_solver_->judgeShootBeforehand(time, data_track_.v_yaw); + data_track_.armors_num, gimbal_real_z_vel_, data_track_.id); + publishShootBeforehand(time, bullet_solver_->judgeShootBeforehand(data_track_.v_yaw, data_track_.id)); if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) { @@ -324,6 +394,58 @@ void Controller::track(const ros::Time& time) } } +bool Controller::externalAimIsFresh(const ros::Time& time) const +{ + if (!gimbal_aim_.valid || gimbal_aim_.header.stamp.isZero()) + return false; + const double age = (time - gimbal_aim_.header.stamp).toSec(); + return age >= -external_aim_timeout_ && age <= external_aim_timeout_; +} + +void Controller::externalAimTrack(const ros::Time& time) +{ + if (state_changed_) + { + state_changed_ = false; + ROS_INFO("[Gimbal] Enter TRACK with external aim"); + } + + external_aim_active_ = externalAimIsFresh(time); + if (external_aim_active_) + { + setDes(time, gimbal_aim_.target_yaw, gimbal_aim_.pitch, gimbal_aim_.yaw); + publishShootBeforehand(time, gimbal_aim_.shoot_cmd == 1 ? rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT : + rm_msgs::ShootBeforehandCmd::BAN_SHOOT); + } + else + { + odom2gimbal_des_.header.stamp = time; + robot_state_handle_.setTransform(odom2gimbal_des_, "rm_gimbal_controllers"); + publishShootBeforehand(time, rm_msgs::ShootBeforehandCmd::BAN_SHOOT); + } + + if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) + { + if (error_pub_->trylock()) + { + error_pub_->msg_.stamp = time; + error_pub_->msg_.error = external_aim_active_ && std::isfinite(gimbal_aim_.error) ? gimbal_aim_.error : 1.0; + error_pub_->unlockAndPublish(); + } + last_publish_time_ = time; + } +} + +void Controller::publishShootBeforehand(const ros::Time& time, uint8_t cmd) +{ + if (shoot_beforehand_cmd_pub_ && shoot_beforehand_cmd_pub_->trylock()) + { + shoot_beforehand_cmd_pub_->msg_.stamp = time; + shoot_beforehand_cmd_pub_->msg_.cmd = cmd; + shoot_beforehand_cmd_pub_->unlockAndPublish(); + } +} + void Controller::direct(const ros::Time& time) { if (state_changed_) @@ -335,11 +457,9 @@ void Controller::direct(const ros::Time& time) try { if (!cmd_gimbal_.target_pos.header.frame_id.empty()) - { - auto transform = robot_state_handle_.lookupTransform("odom", cmd_gimbal_.target_pos.header.frame_id, - cmd_gimbal_.target_pos.header.stamp); - tf2::doTransform(aim_point_odom, aim_point_odom, transform); - } + tf2::doTransform(aim_point_odom, aim_point_odom, + robot_state_handle_.lookupTransform("odom", cmd_gimbal_.target_pos.header.frame_id, + cmd_gimbal_.target_pos.header.stamp)); } catch (tf2::TransformException& ex) { @@ -419,9 +539,9 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) gyro.z = imu_sensor_handle_.getAngularVelocity()[2]; try { - auto transform = - robot_state_handle_.lookupTransform(odom2gimbal_.child_frame_id, imu_sensor_handle_.getFrameId(), time); - tf2::doTransform(gyro, angular_vel, transform); + tf2::doTransform(gyro, angular_vel, + robot_state_handle_.lookupTransform(odom2gimbal_.child_frame_id, imu_sensor_handle_.getFrameId(), + time)); } catch (tf2::TransformException& ex) { @@ -438,6 +558,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) if (ctrls_.find(2) != ctrls_.end()) angular_vel.z = ctrls_.at(2)->joint_.getVelocity(); } + gimbal_real_z_vel_ = angular_vel.z; quatToRPY(odom2gimbal_des_.transform.rotation, pos_des[0], pos_des[1], pos_des[2]); quatToRPY(odom2gimbal_.transform.rotation, pos_real[0], pos_real[1], pos_real[2]); quatToRPY(odom2gimbal_traject_des_.transform.rotation, traject_pos_des[0], traject_pos_des[1], traject_pos_des[2]); @@ -453,79 +574,83 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } else if (state_ == TRACK) { - geometry_msgs::Point target_pos; - geometry_msgs::Vector3 target_vel; - if (data_track_.id != 12) + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM && target_is_armor_) { - geometry_msgs::Point pos = data_track_.position; - double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec()); - pos.x += data_track_.velocity.x * (time - data_track_.header.stamp).toSec(); - pos.y += data_track_.velocity.y * (time - data_track_.header.stamp).toSec(); - pos.z += data_track_.velocity.z * (time - data_track_.header.stamp).toSec(); - bullet_solver_->getSelectedArmorPosAndVel(target_pos, target_vel, pos, data_track_.velocity, yaw, - data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, - data_track_.dz, data_track_.armors_num); + if (external_aim_active_ && externalAimIsFresh(time)) + { + vel_des[2] = gimbal_aim_.yaw_rate; + vel_des[1] = gimbal_aim_.pitch_rate; + } + else + { + vel_des[2] = 0.; + vel_des[1] = 0.; + } } else { - target_pos = data_track_.position; - target_vel = data_track_.velocity; - } - target_vel.x -= chassis_vel_->linear_->x(); - target_vel.y -= chassis_vel_->linear_->y(); - target_vel.z -= chassis_vel_->linear_->z(); - tf2::Vector3 target_pos_tf, target_vel_tf; - try - { - if (joint_urdfs_.find(2) != joint_urdfs_.end()) + geometry_msgs::Point target_pos; + geometry_msgs::Vector3 target_vel; + if (data_track_.id != 12) { - auto transform = robot_state_handle_.lookupTransform(odom2base_.child_frame_id, data_track_.header.frame_id, - data_track_.header.stamp); - tf2::doTransform(target_pos, target_pos, transform); - tf2::doTransform(target_vel, target_vel, transform); - tf2::fromMsg(target_pos, target_pos_tf); - tf2::fromMsg(target_vel, target_vel_tf); - vel_des[2] = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); + geometry_msgs::Point pos = data_track_.position; + double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec()); + pos.x += data_track_.velocity.x * (time - data_track_.header.stamp).toSec(); + pos.y += data_track_.velocity.y * (time - data_track_.header.stamp).toSec(); + pos.z += data_track_.velocity.z * (time - data_track_.header.stamp).toSec(); + bullet_solver_->getSelectedArmorPosAndVel(target_pos, target_vel, pos, data_track_.velocity, yaw, + data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, + data_track_.dz, data_track_.armors_num); } - if (joint_urdfs_.find(1) != joint_urdfs_.end()) + else { - auto transform = robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->parent_link_name, - data_track_.header.frame_id, data_track_.header.stamp); - tf2::doTransform(target_pos, target_pos, transform); - tf2::doTransform(target_vel, target_vel, transform); - tf2::fromMsg(target_pos, target_pos_tf); - tf2::fromMsg(target_vel, target_vel_tf); - vel_des[1] = target_pos_tf.cross(target_vel_tf).y() / std::pow((target_pos_tf.length()), 2); + target_pos = data_track_.position; + target_vel = data_track_.velocity; + } + target_vel.x -= chassis_vel_->linear_->x(); + target_vel.y -= chassis_vel_->linear_->y(); + target_vel.z -= chassis_vel_->linear_->z(); + tf2::Vector3 target_pos_tf, target_vel_tf; + try + { + if (joint_urdfs_.find(2) != joint_urdfs_.end()) + { + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( + odom2base_.child_frame_id, data_track_.header.frame_id, data_track_.header.stamp); + tf2::doTransform(target_pos, target_pos, transform); + tf2::doTransform(target_vel, target_vel, transform); + tf2::fromMsg(target_pos, target_pos_tf); + tf2::fromMsg(target_vel, target_vel_tf); + vel_des[2] = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); + } + if (bullet_solver_->getUsingtraject() && bullet_solver_->getTrackTarget()) + { + vel_des[2] = bullet_solver_->getTrajectVel(); + } + if (joint_urdfs_.find(1) != joint_urdfs_.end()) + { + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( + joint_urdfs_.at(1)->parent_link_name, data_track_.header.frame_id, data_track_.header.stamp); + tf2::doTransform(target_pos, target_pos, transform); + tf2::doTransform(target_vel, target_vel, transform); + tf2::fromMsg(target_pos, target_pos_tf); + tf2::fromMsg(target_vel, target_vel_tf); + vel_des[1] = target_pos_tf.cross(target_vel_tf).y() / std::pow((target_pos_tf.length()), 2); + } + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); } - } - catch (tf2::TransformException& ex) - { - ROS_WARN("%s", ex.what()); } } + for (const auto& in_limit : pos_des_in_limit_) if (!in_limit.second) vel_des[in_limit.first] = 0.; - - if (pid_pos_.find(1) != pid_pos_.end() && ctrls_.find(1) != ctrls_.end()) - { - pid_pos_.at(1)->computeCommand(angle_error[1], period); - ctrls_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * vel_des[1] + - ctrls_.at(1)->joint_.getVelocity() - angular_vel.y); - ctrls_.at(1)->update(time, period); - ctrls_.at(1)->joint_.setCommand(ctrls_.at(1)->joint_.getCommand() + gravityFeedForward(time)); - } - if (pid_pos_.find(2) != pid_pos_.end() && ctrls_.find(2) != ctrls_.end()) - { - pid_pos_.at(2)->computeCommand(bullet_solver_->getUsingtraject() ? traject_angle_error[2] : angle_error[2], period); - double cmd = pid_pos_.at(2)->getCurrentCmd() - - updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + - config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z; - if (state_ == TRACK && bullet_solver_->getUsingtraject() && bullet_solver_->getTrackTarget()) - cmd += bullet_solver_->getTrajectEffortff(); - ctrls_.at(2)->setCommand(cmd); - ctrls_.at(2)->update(time, period); - } + updatePitchJoint(time, period, angular_vel, vel_des, angle_error); + updateYawJoint(time, period, angular_vel, pos_real, pos_des, pos_des_temp, vel_des, angle_error, traject_pos_des, + traject_angle_error); // publish state if (loop_count_ % 10 == 0) @@ -561,9 +686,9 @@ double Controller::gravityFeedForward(const ros::Time& time) if (enable_gravity_compensation_) { Eigen::Vector3d gravity_compensation(0, 0, gravity_); - auto transform = robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->child_link_name, - joint_urdfs_.at(1)->parent_link_name, time); - tf2::doTransform(gravity_compensation, gravity_compensation, transform); + tf2::doTransform(gravity_compensation, gravity_compensation, + robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->child_link_name, + joint_urdfs_.at(1)->parent_link_name, time)); feedforward -= mass_origin.cross(gravity_compensation).y(); } return feedforward; @@ -591,42 +716,97 @@ void Controller::updateChassisVel() last_odom2base_ = odom2base_; } -void Controller::updateBallisticSolution(const ros::Time& time) +void Controller::updatePitchJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double vel_des[3], + const double angle_error[3]) +{ + if (pid_pos_.find(1) != pid_pos_.end() && ctrls_.find(1) != ctrls_.end()) + { + pid_pos_.at(1)->computeCommand(angle_error[1], period); + ctrls_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * vel_des[1] + + ctrls_.at(1)->joint_.getVelocity() - angular_vel.y); + ctrls_.at(1)->update(time, period); + ctrls_.at(1)->joint_.setCommand(ctrls_.at(1)->joint_.getCommand() + gravityFeedForward(time)); + } +} + +void Controller::updateYawJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double /*pos_real*/[3], + const double /*pos_des*/[3], const double /*pos_des_temp*/[3], const double vel_des[3], + const double angle_error[3], const double /*traject_pos_des*/[3], + const double traject_angle_error[3]) { - if (publish_rate_ > 0.0 && last_ballistic_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) + if (pid_pos_.find(2) == pid_pos_.end() || ctrls_.find(2) == ctrls_.end()) + return; + + updateGimbalYawController(time, period, angular_vel, *ctrls_.at(2), *pid_pos_.at(2), config_.yaw_k_v_, vel_des, + angle_error, traject_angle_error); +} + +void Controller::updateGimbalYawController(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, + effort_controllers::JointVelocityController& ctrl, + control_toolbox::Pid& pid_pos, double k_v, const double vel_des[3], + const double angle_error[3], const double traject_angle_error[3]) +{ + if (state_ == TRACK) + { + pid_pos_.at(2)->computeCommand(traject_angle_error[2], period); + } + else + { + pid_pos_.at(2)->computeCommand(angle_error[2], period); + } + if (state_ == TRACK) { - if (ballistic_solution_pub_->trylock()) + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM && target_is_armor_) { - std_msgs::Float32MultiArray data; - data.data.emplace_back(ballistic_yaw_); - data.data.emplace_back(ballistic_pitch_); - ballistic_solution_pub_->msg_.data = data.data; - ballistic_solution_pub_->unlockAndPublish(); + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); + ctrls_.at(2)->update(time, period); + auto cmd = ctrls_.at(2)->joint_.getCommand(); + cmd = config_.moment_of_inertia_ * gimbal_aim_.yaw_acc + cmd; + ROS_WARN("cmd is %f, yaw_acc is %f", cmd, gimbal_aim_.yaw_acc); + ctrls_.at(2)->joint_.setCommand(cmd); + } + else + { + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); + ctrls_.at(2)->update(time, period); } - last_ballistic_publish_time_ = time; + } + else + { + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); + ctrls_.at(2)->update(time, period); } } -std::string Controller::getGimbalFrameID(std::unordered_map joint_urdfs) +std::string Controller::getGimbalFrameID(const std::unordered_map& joint_urdfs) { if (joint_urdfs.find(1) != joint_urdfs.end()) - return joint_urdfs.at(1)->child_link_name; + return joint_urdfs.at(1)->child_link_name.c_str(); if (joint_urdfs.find(0) != joint_urdfs.end()) - return joint_urdfs.at(0)->child_link_name; + return joint_urdfs.at(0)->child_link_name.c_str(); if (joint_urdfs.find(2) != joint_urdfs.end()) - return joint_urdfs.at(2)->child_link_name; - return {}; + return joint_urdfs.at(2)->child_link_name.c_str(); + return std::string(); } -std::string Controller::getBaseFrameID(std::unordered_map joint_urdfs) +std::string Controller::getBaseFrameID(const std::unordered_map& joint_urdfs) { if (joint_urdfs.find(2) != joint_urdfs.end()) - return joint_urdfs.at(2)->parent_link_name; + return joint_urdfs.at(2)->parent_link_name.c_str(); if (joint_urdfs.find(0) != joint_urdfs.end()) - return joint_urdfs.at(0)->parent_link_name; + return joint_urdfs.at(0)->parent_link_name.c_str(); if (joint_urdfs.find(1) != joint_urdfs.end()) - return joint_urdfs.at(1)->parent_link_name; - return {}; + return joint_urdfs.at(1)->parent_link_name.c_str(); + return std::string(); } double Controller::updateCompensation(double chassis_vel_angular_z) @@ -649,12 +829,27 @@ void Controller::trackCB(const rm_msgs::TrackDataConstPtr& msg) track_rt_buffer_.writeFromNonRT(*msg); } -void Controller::ballisticSolverRequestCB(const std_msgs::BoolConstPtr& msg) +void Controller::mpcGimbalAimCB(const rm_msgs::MPCGimbalAimCmdConstPtr& msg) +{ + gimbal_aim_rt_buffer_.writeFromNonRT(*msg); +} + +bool Controller::TrackSolver_typeCB(rm_msgs::StatusChangeRequest& req, rm_msgs::StatusChangeResponse& res) +{ + const bool previous = target_is_armor_; + target_is_armor_ = req.target == rm_msgs::StatusChangeRequest::ARMOR; + publishTargetIsArmor(); + if (previous != target_is_armor_) + ROS_INFO("[Gimbal] armor vision pipeline %s", target_is_armor_ ? "enabled" : "disabled"); + res.switch_is_success = true; + return true; +} + +void Controller::publishTargetIsArmor() { - ballistic_track_rt_buffer_.writeFromNonRT(*msg); - bool ballistic_solver_request = msg->data; - if (ballistic_solver_request) - ballistic_solver_->solver(base2gimbal_, ballistic_yaw_, ballistic_pitch_); + std_msgs::Bool msg; + msg.data = target_is_armor_; + target_is_armor_pub_.publish(msg); } void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t /*unused*/) @@ -665,12 +860,13 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin GimbalConfig init_config = *config_rt_buffer_.readFromNonRT(); // config init use yaml config.yaw_k_v_ = init_config.yaw_k_v_; config.pitch_k_v_ = init_config.pitch_k_v_; - config.accel_pitch_ = init_config.accel_pitch_; - config.accel_yaw_ = init_config.accel_yaw_; config.chassis_comp_a_ = init_config.chassis_comp_a_; config.chassis_comp_b_ = init_config.chassis_comp_b_; config.chassis_comp_c_ = init_config.chassis_comp_c_; config.chassis_comp_d_ = init_config.chassis_comp_d_; + config.accel_pitch_ = init_config.accel_pitch_; + config.accel_yaw_ = init_config.accel_yaw_; + config.moment_of_inertia_ = init_config.moment_of_inertia_; dynamic_reconfig_initialized_ = true; } GimbalConfig config_non_rt{ .yaw_k_v_ = config.yaw_k_v_, @@ -680,7 +876,8 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin .chassis_comp_a_ = config.chassis_comp_a_, .chassis_comp_b_ = config.chassis_comp_b_, .chassis_comp_c_ = config.chassis_comp_c_, - .chassis_comp_d_ = config.chassis_comp_d_ }; + .chassis_comp_d_ = config.chassis_comp_d_, + .moment_of_inertia_ = config.moment_of_inertia_ }; config_rt_buffer_.writeFromNonRT(config_non_rt); } diff --git a/rm_orientation_controller/include/rm_orientation_controller/orientation_controller.h b/rm_orientation_controller/include/rm_orientation_controller/orientation_controller.h index b03ac226..ef653261 100644 --- a/rm_orientation_controller/include/rm_orientation_controller/orientation_controller.h +++ b/rm_orientation_controller/include/rm_orientation_controller/orientation_controller.h @@ -10,6 +10,8 @@ #include #include #include +#include +#include namespace rm_orientation_controller { @@ -25,6 +27,7 @@ class Controller : public controller_interface::MultiInterfaceController> assembly_error_pub_; bool receive_imu_msg_ = false; + int loop_count_{}; }; } // namespace rm_orientation_controller diff --git a/rm_orientation_controller/src/orientation_controller.cpp b/rm_orientation_controller/src/orientation_controller.cpp index 35640f02..1db309ed 100644 --- a/rm_orientation_controller/src/orientation_controller.cpp +++ b/rm_orientation_controller/src/orientation_controller.cpp @@ -23,6 +23,8 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro tf_broadcaster_.init(root_nh); imu_data_sub_ = root_nh.subscribe("data", 1, &Controller::imuDataCallback, this); + assembly_error_pub_.reset( + new realtime_tools::RealtimePublisher(root_nh, "imu_assembly_error", 10)); source2target_msg_.header.frame_id = frame_source_; source2target_msg_.child_frame_id = frame_target_; source2target_msg_.transform.rotation.w = 1.0; @@ -48,6 +50,7 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) if (!receive_imu_msg_) tf_broadcaster_.sendTransform(source2target_msg_); } + AssemblyErrorPub(time); } bool Controller::getTransform(const ros::Time& time, geometry_msgs::TransformStamped& source2target, const double x, @@ -90,6 +93,53 @@ void Controller::imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg) tf_broadcaster_.sendTransform(source2target); } +void Controller::AssemblyErrorPub(const ros::Time& time) +{ + double roll_error, pitch_error, yaw_error, roll_imu, pitch_imu, yaw_imu; + geometry_msgs::TransformStamped tf_source2target, tf_target2imu; + tf2::Transform source2target, target2imu; + Eigen::Matrix3d roll_eigen, pitch_eigen, yaw_eigen, R_eigen; + Eigen::Vector3d error_eigen, assembly_error_eigen; + try + { + tf_source2target = robot_state_.lookupTransform(frame_source_, frame_target_, time); + tf_target2imu = robot_state_.lookupTransform(frame_target_, imu_sensor_.getFrameId(), time); + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + } + tf2::fromMsg(tf_source2target.transform, source2target); + tf2::Matrix3x3(source2target.getRotation()).getRPY(roll_error, pitch_error, yaw_error); + tf2::fromMsg(tf_target2imu.transform, target2imu); + tf2::Matrix3x3(target2imu.getRotation()).getRPY(roll_imu, pitch_imu, yaw_imu); + roll_imu = static_cast(roll_imu * 2 / M_PI); + pitch_imu = static_cast(pitch_imu * 2 / M_PI); + yaw_imu = static_cast(yaw_imu * 2 / M_PI); + roll_eigen << 1, 0, 0, 0, cos(roll_imu * M_PI / 2), -sin(roll_imu * M_PI / 2), 0, sin(roll_imu * M_PI / 2), + cos(roll_imu * M_PI / 2); + pitch_eigen << cos(pitch_imu * M_PI / 2), 0, sin(pitch_imu * M_PI / 2), 0, 1, 0, -sin(pitch_imu * M_PI / 2), 0, + cos(pitch_imu * M_PI / 2); + yaw_eigen << cos(yaw_imu * M_PI / 2), -sin(yaw_imu * M_PI / 2), 0, sin(yaw_imu * M_PI / 2), cos(yaw_imu * M_PI / 2), + 0, 0, 0, 1; + R_eigen = roll_eigen * pitch_eigen * yaw_eigen; + error_eigen << roll_error, pitch_error, 0.0; + assembly_error_eigen = R_eigen * error_eigen; + if (loop_count_ % 100 == 0) + { + if (assembly_error_pub_->trylock()) + { + assembly_error_pub_->msg_.header.stamp = time; + assembly_error_pub_->msg_.roll_error = assembly_error_eigen[0]; + assembly_error_pub_->msg_.pitch_error = assembly_error_eigen[1]; + assembly_error_pub_->msg_.yaw_error = assembly_error_eigen[2]; + assembly_error_pub_->unlockAndPublish(); + } + else + ROS_WARN_THROTTLE(1, "Can't publish assembly error data"); + } + loop_count_++; +} } // namespace rm_orientation_controller PLUGINLIB_EXPORT_CLASS(rm_orientation_controller::Controller, controller_interface::ControllerBase) diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index a68f830c..24bcf6cf 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -345,7 +345,8 @@ void Controller::judgeBulletShoot(const ros::Time& time, const ros::Duration& pe double friction_change_speed_derivative = friction_change_speed - last_friction_change_speed_; if (state_ != STOP) { - if (friction_change_speed_derivative > 0 && has_shoot_) + if (friction_change_speed_derivative > 0 && has_shoot_ && + friction_change_speed > config_.wheel_speed_raise_threshold) has_shoot_ = false; if (friction_change_speed < -config_.wheel_speed_drop_threshold && !has_shoot_ && friction_change_speed_derivative < 0)