diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt index ee8d78c7..8c9d2e13 100644 --- a/rm_chassis_controllers/CMakeLists.txt +++ b/rm_chassis_controllers/CMakeLists.txt @@ -63,6 +63,11 @@ include_directories( ${EIGEN3_INCLUDE_DIR} ) +file(GLOB_RECURSE BIPEDAL_SOURCES + "src/bipedal_wheel_controller/*.cpp" + "src/bipedal_wheel_controller/*.c" +) + ## Declare cpp executables add_library(${PROJECT_NAME} src/chassis_base.cpp @@ -71,20 +76,7 @@ add_library(${PROJECT_NAME} src/swerve.cpp src/balance.cpp - src/bipedal_wheel_controller/vmc/leg_conv.c - src/bipedal_wheel_controller/vmc/leg_pos.c - src/bipedal_wheel_controller/vmc/leg_spd.c - src/bipedal_wheel_controller/dynamics/gen_A.c - src/bipedal_wheel_controller/dynamics/gen_B.c - src/bipedal_wheel_controller/controller_mode/mode_base.cpp - src/bipedal_wheel_controller/controller_mode/mode_manager.cpp - src/bipedal_wheel_controller/controller_mode/stand_up.cpp - src/bipedal_wheel_controller/controller_mode/sit_down.cpp - src/bipedal_wheel_controller/controller_mode/recover.cpp - src/bipedal_wheel_controller/controller_mode/normal.cpp - src/bipedal_wheel_controller/controller_mode/upstairs.cpp - src/bipedal_wheel_controller/controller.cpp - src/bipedal_wheel_controller/series_legged_vmc_controller.cpp + ${BIPEDAL_SOURCES} ) ## Specify libraries to link executable targets against diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h index be45f64d..89790a01 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h @@ -10,7 +10,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -37,6 +39,7 @@ class BipedalController : public ChassisBase& getModelParams() const { return model_params_; } const std::shared_ptr& getControlParams() const { return control_params_; } const std::shared_ptr& getBiasParams() const { return bias_params_; } + const std::shared_ptr& getLegThresholdParams() const { return leg_threshold_params_; } double getLegCmd() const{ return legCmd_; } double getJumpCmd() const{ return jumpCmd_; } int getBaseState() const{ return state_; } @@ -62,7 +66,7 @@ class BipedalController : public ChassisBase left_ref, Eigen::Matrix right_ref, Eigen::Matrix u_left, Eigen::Matrix u_right, Eigen::Matrix F_leg_, const bool unstick[2]) const; - void pubLegLenStatus(const bool& is_high_leg_flag); +void pubLegLenStatus(const bool& upstair_flag); // clang-format on void clearStatus(); @@ -72,6 +76,7 @@ class BipedalController : public ChassisBase>& Ks, const std::vector& L0s, Eigen::Matrix& coeffs); geometry_msgs::Twist odometry() override; @@ -82,6 +87,7 @@ class BipedalController : public ChassisBase model_params_; std::shared_ptr control_params_; std::shared_ptr bias_params_; + std::shared_ptr leg_threshold_params_; int balance_mode_ = BalanceMode::SIT_DOWN; bool balance_state_changed_ = false; @@ -95,6 +101,8 @@ class BipedalController : public ChassisBase A_, B_, H_, Q_, R_; Eigen::Matrix X_, U_; std::shared_ptr> kalmanFilterPtr_; + std::shared_ptr left_leg_angle_lpFilterPtr_, right_leg_angle_lpFilterPtr_, + left_leg_angle_vel_lpFilterPtr_, right_leg_angle_vel_lpFilterPtr_; Eigen::Matrix x_left_{}, x_right_{}; double default_leg_length_{ 0.2 }; @@ -115,7 +123,7 @@ class BipedalController : public ChassisBase& joint_handles, - const std::vector& pid_legs, const std::vector& pid_thetas); + const std::vector& pid_legs, const std::vector& pid_thetas, + control_toolbox::Pid* pid_theta_diff); void execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) override; - void setUpLegMotion(const Eigen::Matrix& x, const int& other_leg_state, - const double& leg_length, const double& leg_theta, int& leg_state, double& theta_des); - void detectLegRelState(const double& leg_theta, int& leg_state); + inline void detectChassisStateToRecover(); const char* name() const override { return "RECOVER"; @@ -29,6 +49,10 @@ class Recover : public ModeBase private: std::vector joint_handles_; std::vector pid_legs_, pid_thetas_; - int left_leg_state, right_leg_state; + control_toolbox::Pid* pid_theta_diff_; + double leg_recovery_velocity_{ 2.0 }, threshold_{ 0.05 }, leg_theta_diff_{ 0.0 }, desired_leg_length_{ 0.36 }; + const double leg_recovery_velocity_const_{ 2.0 }; + RecoveryChassisState recovery_chassis_state_{ ForwardSlip }; + bool detectd_flag{ false }; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h index d599e8ea..1fe4aa61 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h @@ -27,8 +27,17 @@ class StandUp : public ModeBase void setUpLegMotion(const Eigen::Matrix& x, const int& other_leg_state, const double& leg_length, const double& leg_theta, int& leg_state, double& theta_des, double& length_des); + /** + * Detect the leg state before stand up: UNDER, FRONT, BEHIND + * @param x + * @param leg_state + */ + void detectLegState(const Eigen::Matrix& x, int& leg_state); std::vector joint_handles_; std::vector pid_legs_, pid_thetas_; int left_leg_state, right_leg_state; + double theta_des_l, theta_des_r, length_des_l, length_des_r; + double spring_force_{}; + std::shared_ptr leg_state_threshold_; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/upstairs.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/upstairs.h index bc594127..4ec6f475 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/upstairs.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/upstairs.h @@ -25,8 +25,16 @@ class Upstairs : public ModeBase } private: + /** + * Detect the leg state before stand up: UNDER, FRONT, BEHIND + * @param x + * @param leg_state + */ + void detectLegState(const Eigen::Matrix& x, int& leg_state); + std::vector joint_handles_; std::vector pid_legs_, pid_thetas_; int left_leg_state, right_leg_state; + std::shared_ptr leg_state_threshold_; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h index cb6dcba8..6a47baa1 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h @@ -38,10 +38,23 @@ struct ControlParams struct BiasParams { double x; + double theta; double pitch; double roll; }; +struct LegStateThresholdParams +{ + double under_lower; + double under_upper; + double front_lower; + double front_upper; + double behind_lower; + double behind_upper; + double upstair_des_theta; + double upstair_exit_threshold; +}; + struct LegCommand { double force; // Thrust diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h index 0dea5e85..6785810c 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h @@ -54,33 +54,6 @@ inline void generateAB(const std::shared_ptr& model_params, Eigen:: // clang-format on } -/** - * Detect the leg state before stand up: UNDER, FRONT, BEHIND - * @param x - * @param leg_state - */ -inline void detectLegState(const Eigen::Matrix& x, int& leg_state) -{ - if (x[0] > -M_PI / 2 + 0.7 && x[0] < (M_PI / 2 - 1.4)) - leg_state = LegState::UNDER; - else if ((x[0] < -M_PI / 2 + 0.7 && x[0] > -M_PI) || (x[0] < M_PI && x[0] > M_PI - 0.5)) - leg_state = LegState::FRONT; - else if (x[0] > (M_PI / 2 - 1.4) && x[0] < M_PI - 0.5) - leg_state = LegState::BEHIND; - switch (leg_state) - { - case LegState::UNDER: - ROS_INFO("[balance] x[0]: %.3f Leg state: UNDER", x[0]); - break; - case LegState::FRONT: - ROS_INFO("[balance] x[0]: %.3f Leg state: FRONT", x[0]); - break; - case LegState::BEHIND: - ROS_INFO("[balance] x[0]: %.3f Leg state: BEHIND", x[0]); - break; - } -} - /** * Compute the leg command using PID controllers * @param desired_length @@ -103,31 +76,62 @@ inline LegCommand computePidLegCommand(double desired_length, double desired_ang cmd.force = length_pid.computeCommand(desired_length - leg_pos[0], period) + feedforward_force; if (!overturn) { - if (leg_state == LegState::BEHIND) - cmd.torque = angle_pid.computeCommand(-angles::shortest_angular_distance(desired_angle, leg_pos[1]), period); - else - { - if ((leg_pos[1] > M_PI_2 - 0.3 && leg_pos[1] < M_PI_2 + 0.3)) - cmd.torque = angle_pid.computeCommand(-angles::shortest_angular_distance(desired_angle, leg_pos[1]), period); - else - cmd.torque = angle_vel_pid.computeCommand(-4 - leg_spd[1], period); - } - } - else - { - if (leg_state == LegState::FRONT) + if (leg_state == LegState::BEHIND || leg_state == LegState::UNDER) { cmd.torque = angle_pid.computeCommand(-angles::shortest_angular_distance(desired_angle, leg_pos[1]), period); } else { - cmd.torque = angle_vel_pid.computeCommand(-4 - leg_spd[1], period); + cmd.torque = angle_vel_pid.computeCommand(-5 - leg_spd[1], period); } } + // else + // { + // if (leg_state == LegState::FRONT) + // { + // cmd.torque = angle_pid.computeCommand(-angles::shortest_angular_distance(desired_angle, leg_pos[1]), period); + // } + // else + // { + // cmd.torque = angle_vel_pid.computeCommand(-5 - leg_spd[1], period); + // } + // } leg_conv(cmd.force, cmd.torque, leg_angle[0], leg_angle[1], cmd.input); return cmd; } +inline LegCommand computePidAngleVelLegCommand(double desired_length, double desired_leg_angle_vel, double leg_pos[2], + double leg_spd[2], control_toolbox::Pid& length_pid, + control_toolbox::Pid& angle_vel_pid, const double* leg_angle, + const ros::Duration& period, double feedforward_force = 0.0f) +{ + LegCommand cmd{ 0.0, 0.0, { 0.0, 0.0 } }; + cmd.force = length_pid.computeCommand(desired_length - leg_pos[0], period) + feedforward_force; + cmd.torque = angle_vel_pid.computeCommand(desired_leg_angle_vel - leg_spd[1], period); + leg_conv(cmd.force, 10 * desired_leg_angle_vel + cmd.torque, leg_angle[0], leg_angle[1], cmd.input); + return cmd; +} + +inline LegCommand computePidAngleLegCommand(double desired_length, double desired_leg_angle, double leg_pos[2], + control_toolbox::Pid& length_pid, control_toolbox::Pid& angle_pid, + const double* leg_angle, const ros::Duration& period, + double feedforward_force = 0.0f) +{ + LegCommand cmd{ 0.0, 0.0, { 0.0, 0.0 } }; + cmd.force = length_pid.computeCommand(desired_length - leg_pos[0], period) + feedforward_force; + cmd.torque = angle_pid.computeCommand(-angles::shortest_angular_distance(desired_leg_angle, leg_pos[1]), period); + leg_conv(cmd.force, cmd.torque, leg_angle[0], leg_angle[1], cmd.input); + return cmd; +} +inline LegCommand computePidLenLegCommand(double desired_length, double leg_pos[2], control_toolbox::Pid& length_pid, + const double* leg_angle, const ros::Duration& period, + double feedforward_force = 0.0f) +{ + LegCommand cmd{ 0.0, 0.0, { 0.0, 0.0 } }; + cmd.force = length_pid.computeCommand(desired_length - leg_pos[0], period) + feedforward_force; + leg_conv(cmd.force, 0.0, leg_angle[0], leg_angle[1], cmd.input); + return cmd; +} /** * Set joint commands to the joint handles * @param joints diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/series_legged_vmc_controller.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/series_legged_vmc_controller.h index 01298526..a2d06bfa 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/series_legged_vmc_controller.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/series_legged_vmc_controller.h @@ -18,6 +18,8 @@ #include "bipedal_wheel_controller/vmc/leg_pos.h" #include "bipedal_wheel_controller/vmc/leg_spd.h" +#include "bipedal_wheel_controller/vmc/VMC.h" + #include namespace rm_chassis_controllers @@ -48,6 +50,8 @@ class VMCController : public controller_interface::MultiInterfaceController vmcPtr_; + ros::Publisher statePublisher_, jointCmdStatePublisher_; ros::Subscriber cmdLegLengthSubscriber_, cmdLegAngleSubscriber_; }; diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h new file mode 100644 index 00000000..9e2b7fcc --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h @@ -0,0 +1,22 @@ +// +// Created by wk on 2026/2/25. +// + +namespace rm_chassis_controllers +{ +class VMC +{ +public: + explicit VMC(double l1, double l2, double l5) : l1_(l1), l2_(l2), l3_(l2), l4_(l1), l5_(l5){}; + ~VMC() = default; + + void leg_pos(double phi1, double phi4, double pos[2]) const; + void leg_spd(double dphi1, double dphi4, double phi1, double phi4, double spd[2]); + void leg_conv(double F, double Tp, double phi1, double phi4, double T[2]); + +private: + void calc_jacobian(double phi1, double phi4, double J[2][2]); + + double l1_, l2_, l3_, l4_, l5_; +}; +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h index e979da0c..de527cda 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -100,7 +100,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController * @param time The current time. * @param period The time passed since the last call to update. */ - void follow(const ros::Time& time, const ros::Duration& period); + virtual void follow(const ros::Time& time, const ros::Duration& period); /** @brief The mode TWIST: Just moving chassis. * * The mode TWIST: Chassis will move independent and will not effect by gimbal's move. diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp index ef542792..c23d406b 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp @@ -40,9 +40,10 @@ bool BipedalController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan model_params_ = std::make_shared(); control_params_ = std::make_shared(); bias_params_ = std::make_shared(); + leg_threshold_params_ = std::make_shared(); if (!setupModelParams(controller_nh) || !setupLQR(controller_nh) || !setupBiasParams(controller_nh) || - !setupControlParams(controller_nh)) + !setupControlParams(controller_nh) || !setupThresholdParams(controller_nh)) return false; auto legCmdCallback = [this](const rm_msgs::LegCmd::ConstPtr& msg) { @@ -52,7 +53,7 @@ bool BipedalController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan leg_cmd_sub_ = controller_nh.subscribe("/leg_cmd", 1, legCmdCallback); unstick_pub_ = controller_nh.advertise("unstick", 1); - leg_len_status_pub_ = controller_nh.advertise("leg_len_status", 1); + upstair_status_pub_ = controller_nh.advertise("upstair_status", 1); legged_chassis_status_pub_ = controller_nh.advertise("legged_chassis_status", 1); legged_chassis_mode_pub_ = controller_nh.advertise("legged_chassis_mode", 1); lqr_status_pub_ = controller_nh.advertise("lqr_status", 1); @@ -60,10 +61,12 @@ bool BipedalController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan x_right_.setZero(); // Slippage detection - A_ << 1, 0, 0, 1; + A_ << 1, 0.0, 0, 1; H_ << 1, 0, 0, 1; Q_ << 1, 0, 0, 1; R_ << 200, 0, 0, 200; + // Q_ << 25, 0, 0, 2000; + // R_ << 800, 0, 0, 0.01; R_wheel_ = R_(0, 0); slip_R_wheel_ = slip_alpha_ * R_wheel_; B_.setZero(); @@ -72,6 +75,11 @@ bool BipedalController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan kalmanFilterPtr_ = std::make_shared>(A_, B_, H_, Q_, R_); kalmanFilterPtr_->clear(X_); + // left_leg_angle_lpFilterPtr_ = std::make_shared(100); + // right_leg_angle_lpFilterPtr_ = std::make_shared(100); + + left_leg_angle_vel_lpFilterPtr_ = std::make_shared(60); + right_leg_angle_vel_lpFilterPtr_ = std::make_shared(60); return true; } @@ -91,7 +99,7 @@ void BipedalController::moveJoint(const ros::Time& time, const ros::Duration& pe void BipedalController::clearStatus() { - x_left_(2) = x_right_(2) = 0; + x_left_(2) = x_right_(2) = -bias_params_->x; } void BipedalController::updateEstimation(const ros::Time& time, const ros::Duration& period) @@ -129,8 +137,7 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat tf2::Vector3 z_body(0, 0, 1); tf2::Vector3 z_world = tf2::quatRotate(odom2base.getRotation(), z_body); - overturn_ = z_world.z() < -5.0; - overturn_ = !(abs(pitch) < 0.9); + overturn_ = (abs(pitch) > 0.65 || abs(roll) > 0.4) && z_world.z() < 0.0; } catch (tf2::TransformException& ex) { @@ -142,16 +149,16 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat // vmc double left_angle[2]{}, right_angle[2]{}, left_pos[2]{}, left_spd[2]{}, right_pos[2]{}, right_spd[2]{}; // [0]:hip_vmc_joint [1]:knee_vmc_joint - // left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; - // left_angle[1] = left_knee_joint_handle_.getPosition(); - // right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; - // right_angle[1] = right_knee_joint_handle_.getPosition(); + left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI; + left_angle[1] = left_knee_joint_handle_.getPosition(); + right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI; + right_angle[1] = right_knee_joint_handle_.getPosition(); // gazebo - left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; - left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; - right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; - right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; + // left_angle[0] = left_hip_joint_handle_.getPosition() + M_PI_2; + // left_angle[1] = left_knee_joint_handle_.getPosition() - M_PI_2; + // right_angle[0] = right_hip_joint_handle_.getPosition() + M_PI_2; + // right_angle[1] = right_knee_joint_handle_.getPosition() - M_PI_2; // [0] is length, [1] is angle leg_pos(left_angle[0], left_angle[1], left_pos); @@ -160,9 +167,18 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat left_spd); leg_spd(right_hip_joint_handle_.getVelocity(), right_knee_joint_handle_.getVelocity(), right_angle[0], right_angle[1], right_spd); + left_leg_angle_vel_lpFilterPtr_->input(left_spd[1]); + right_leg_angle_vel_lpFilterPtr_->input(right_spd[1]); + // left_leg_angle_lpFilterPtr_->input(left_pos[1]); + // right_leg_angle_lpFilterPtr_->input(right_pos[1]); + + // left_pos[1] = left_leg_angle_lpFilterPtr_->output(); + // right_pos[1] = right_leg_angle_lpFilterPtr_->output(); + left_spd[1] = left_leg_angle_vel_lpFilterPtr_->output(); + right_spd[1] = right_leg_angle_vel_lpFilterPtr_->output(); // Slippage_detection - leftWheelVel = (left_wheel_joint_handle_.getVelocity() - angular_vel_base.y + left_spd[1]) * wheel_radius_; + 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_); @@ -176,20 +192,20 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat i = 0; X_(0) = wheel_vel_aver; X_(1) = linear_acc_base.x; - kalmanFilterPtr_->predict(U_, R_); - kalmanFilterPtr_->update(X_); + kalmanFilterPtr_->predict(U_); + kalmanFilterPtr_->update(X_, R_); } else { - kalmanFilterPtr_->predict(U_, R_); + kalmanFilterPtr_->predict(U_); i++; } auto x_hat_vel = kalmanFilterPtr_->getState(); - slip_flag_ = abs(x_hat_vel(0) - wheel_vel_aver) > 1.5; + slip_flag_ = abs(x_hat_vel(0) - wheel_vel_aver) > 3.0; // update state x_left_[3] = state_ != RAW ? x_hat_vel(0) : 0; - if (abs(x_left_[3]) <= 0.5f && abs(vel_cmd_.x) <= 0.1f) + if (abs(x_left_[3]) <= 0.6f && abs(vel_cmd_.x) <= 0.1f) { x_left_[2] += state_ != RAW ? x_left_[3] * period.toSec() : 0; } @@ -207,8 +223,7 @@ void BipedalController::updateEstimation(const ros::Time& time, const ros::Durat // ros msg rm_msgs::LeggedChassisStatus legged_chassis_status_msg; - legged_chassis_status_msg.roll = - (left_wheel_joint_handle_.getVelocity() + right_wheel_joint_handle_.getVelocity()) * model_params_->r / 2.0; + legged_chassis_status_msg.roll = wheel_vel_aver; legged_chassis_status_msg.pitch = x_left_[4]; legged_chassis_status_msg.d_pitch = x_left_[5]; legged_chassis_status_msg.yaw = yaw; @@ -244,7 +259,7 @@ void BipedalController::pubState() void BipedalController::stopping(const ros::Time& time) { - balance_mode_ = BalanceMode::RECOVER; + balance_mode_ = BalanceMode::SIT_DOWN; balance_state_changed_ = false; setJointCommands(joint_handles_, { 0, 0, { 0., 0. } }, { 0, 0, { 0., 0. } }); @@ -328,6 +343,7 @@ bool BipedalController::setupBiasParams(ros::NodeHandle& controller_nh) { const std::pair tbl[] = { { "x_bias", &bias_params_->x }, + { "theta_bias", &bias_params_->theta }, { "pitch_bias", &bias_params_->pitch }, { "roll_bias", &bias_params_->roll }, }; @@ -341,6 +357,7 @@ bool BipedalController::setupBiasParams(ros::NodeHandle& controller_nh) return true; } +// [will unused] bool BipedalController::setupControlParams(ros::NodeHandle& controller_nh) { if (!controller_nh.getParam("jumpOverTime", control_params_->jumpOverTime_) || @@ -353,6 +370,24 @@ bool BipedalController::setupControlParams(ros::NodeHandle& controller_nh) return true; } +bool BipedalController::setupThresholdParams(ros::NodeHandle& controller_nh) +{ + if (!controller_nh.getParam("under_lower_threshold", leg_threshold_params_->under_lower) || + !controller_nh.getParam("under_upper_threshold", leg_threshold_params_->under_upper) || + !controller_nh.getParam("front_lower_threshold", leg_threshold_params_->front_lower) || + !controller_nh.getParam("front_upper_threshold", leg_threshold_params_->front_upper) || + !controller_nh.getParam("behind_lower_threshold", leg_threshold_params_->behind_lower) || + !controller_nh.getParam("behind_upper_threshold", leg_threshold_params_->behind_upper) || + !controller_nh.getParam("upstair_exit_threshold", leg_threshold_params_->upstair_exit_threshold) || + !controller_nh.getParam("upstair_des_theta", leg_threshold_params_->upstair_des_theta)) + { + ROS_ERROR("Load threshold param fail, check the resist of " + "under_threshold, front_threshold, behind_threshold"); + return false; + } + return true; +} + void BipedalController::polyfit(const std::vector>& Ks, const std::vector& L0s, Eigen::Matrix& coeffs) { @@ -409,12 +444,58 @@ void BipedalController::pubLQRStatus(Eigen::Matrix left_er lqr_status_pub_.publish(msg); } -void BipedalController::pubLegLenStatus(const bool& is_high_leg_flag) +void BipedalController::pubLegLenStatus(const bool& upstair_flag) { - std_msgs::Bool msg; - msg.data = is_high_leg_flag; - leg_len_status_pub_.publish(msg); + rm_msgs::LeggedUpstairStatus msg; + msg.upstair_flag = upstair_flag; + upstair_status_pub_.publish(msg); } +// void BipedalController::follow(const ros::Time& time, const ros::Duration& period) +//{ +// static bool follow_source_frame_changed_{ false }; +// static std::string last_follow_source_frame_{ follow_source_frame_ }; +// if (state_changed_) +// { +// state_changed_ = false; +// ROS_INFO("[Chassis] Enter FOLLOW"); +// +// ChassisBase::recovery(); +// pid_follow_.reset(); +// } +// +// tfVelToBase(command_source_frame_); +// try +// { +// double roll{}, pitch{}, yaw{}; +// // double target_yaw_bias{ 0 }; +// quatToRPY(robot_state_handle_.lookupTransform("base_link", follow_source_frame_, ros::Time(0)).transform.rotation, +// roll, pitch, yaw); +// double yawForwardError = angles::shortest_angular_distance(0, yaw); +// double yawInverseError = angles::shortest_angular_distance(M_PI, yaw); +// double yawError = abs(yawForwardError) < abs(yawInverseError) ? yawForwardError : yawInverseError; +// if (follow_source_frame_ != last_follow_source_frame_) +// { +// follow_source_frame_changed_ = true; +// } +// if (follow_source_frame_changed_) +// { +// yawError = yawForwardError; +// if (abs(yawError) < 0.1) +// { +// follow_source_frame_changed_ = false; +// } +// } +// pid_follow_.computeCommand(yawError, period); +// vel_cmd_.z = pid_follow_.getCurrentCmd() + cmd_rt_buffer_.readFromRT()->cmd_chassis_.follow_vel_des; +// } +// catch (tf2::TransformException& ex) +// { +// ROS_WARN("%s", ex.what()); +// } +// last_follow_source_frame_ = follow_source_frame_; +// } + } // namespace rm_chassis_controllers PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::BipedalController, controller_interface::ControllerBase) diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp index 7b835985..0c31ea7a 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp @@ -42,8 +42,8 @@ ModeManager::ModeManager(ros::NodeHandle& controller_nh, &pid_theta_diff_, &pid_roll_))); mode_map_.insert( std::make_pair(BalanceMode::STAND_UP, std::make_unique(joint_handles, pid_legs_stand_up_, pid_thetas_))); - mode_map_.insert( - std::make_pair(BalanceMode::RECOVER, std::make_unique(joint_handles, pid_legs_stand_up_, pid_thetas_))); + mode_map_.insert(std::make_pair(BalanceMode::RECOVER, std::make_unique(joint_handles, pid_legs_stand_up_, + pid_thetas_, &pid_theta_diff_))); mode_map_.insert(std::make_pair(BalanceMode::SIT_DOWN, std::make_unique(joint_handles, pid_wheels_))); mode_map_.insert(std::make_pair(BalanceMode::UPSTAIRS, std::make_unique(joint_handles, pid_legs_stand_up_, pid_thetas_))); diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp index c4c0d35c..6345267d 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp @@ -82,10 +82,10 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const x_left_ref(3) = x_right_ref(3) = friction_circle_alpha * vel_cmd_.x; leg_length_des = controller->getLegCmd(); } - x_left(0) -= 0.07; - x_right(0) -= 0.07; - x_left(4) -= 0.; - x_right(4) -= 0.; + x_left(0) -= bias_params_->theta; + x_right(0) -= bias_params_->theta; + x_left(4) -= bias_params_->pitch; + x_right(4) -= bias_params_->pitch; x_left -= x_left_ref; x_right -= x_right_ref; @@ -230,13 +230,9 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const right_cmd = { F_leg[1], u_right[1], { right_T[0], right_T[1] } }; // upstairs - // if (((x_left[4] < -0.25 && (x_left(0) + x_right(0) / 2.0f) > 0.25) || - // (((left_pos_[1] + right_pos_[1]) / 2.0f) > 0.5)) && - // controller->getCompleteStand() && abs(vel_cmd_.x) > 1.0 && abs(x_left(3)) > 0.2 && - // ((left_pos_[0] + right_pos_[0]) / 2.0f) > 0.34) - if (jump_phase_ == JumpPhase::IDLE && linear_acc_base_.z < -3.0 && controller->getCompleteStand() && - abs(vel_cmd_.x) > 1.0 && abs(x_left(3)) > 0.2 && ((left_pos_[0] + right_pos_[0]) / 2.0f) > 0.34 && - leg_length_des > 0.34) + if (jump_phase_ == JumpPhase::IDLE && linear_acc_base_.z < -7.0 && controller->getCompleteStand() && + abs(vel_cmd_.x) > 0.1 && abs(x_left(3)) > 0.1 && ((left_pos_[0] + right_pos_[0]) / 2.0f) > 0.30 && + leg_length_des > 0.30) { leg_length_des = controller->getDefaultLegLength(); controller->setMode(BalanceMode::UPSTAIRS); @@ -247,7 +243,7 @@ void Normal::execute(BipedalController* controller, const ros::Time& time, const } // Protection - if ((controller->getCompleteStand() && (abs(x_left(4)) > 0.95 || abs(x_left(0)) > 0.8 || abs(roll_) > 1.3)) || + if (abs(x_left(4)) > 0.6 || abs(x_left(0)) > 1.0 || abs(x_right(0)) > 1.0 || abs(roll_) > 1.0 || controller->getOverturn() || controller->getBaseState() == 4) { leg_length_des = controller->getDefaultLegLength(); diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp index 7eef77c4..ca568c13 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp @@ -9,8 +9,8 @@ namespace rm_chassis_controllers { Recover::Recover(const std::vector& joint_handles, const std::vector& pid_legs, - const std::vector& pid_thetas) - : joint_handles_(joint_handles), pid_legs_(pid_legs), pid_thetas_(pid_thetas) + const std::vector& pid_thetas, control_toolbox::Pid* pid_theta_diff) + : joint_handles_(joint_handles), pid_legs_(pid_legs), pid_thetas_(pid_thetas), pid_theta_diff_(pid_theta_diff) { } @@ -19,31 +19,45 @@ void Recover::execute(BipedalController* controller, const ros::Time& time, cons if (!controller->getStateChange()) { ROS_INFO("[balance] Enter RECOVER"); - detectLegRelState(left_pos_[1], left_leg_state); - detectLegRelState(right_pos_[1], right_leg_state); + detectd_flag = false; controller->setStateChange(true); } - int left_leg_state, right_leg_state; - double left_desired_angle{ left_pos_[1] }, right_desired_angle{ right_pos_[1] }; - left_desired_angle = left_pos_[1]; - right_desired_angle = right_pos_[1]; + // until chassis + if (!detectd_flag && abs(x_left_[1]) < 0.1 && abs(x_left_[5]) < 0.1) + { + detectChassisStateToRecover(); + detectd_flag = true; + leg_recovery_velocity_ = + recovery_chassis_state_ == BackwardSlip ? -leg_recovery_velocity_const_ : leg_recovery_velocity_const_; + } + LegCommand left_cmd = { 0, 0, { 0., 0. } }, right_cmd = { 0, 0, { 0., 0. } }; - setUpLegMotion(x_left_, right_leg_state, left_pos_[0], left_pos_[1], left_leg_state, left_desired_angle); - setUpLegMotion(x_right_, left_leg_state, right_pos_[0], right_pos_[1], right_leg_state, right_desired_angle); + leg_theta_diff_ = angles::shortest_angular_distance(left_pos_[1], right_pos_[1]); + double T_theta_diff{ 0.0 }, feedforward_force{ 0.0 }; if (controller->getBaseState() != 4) { - left_cmd = - computePidLegCommand(0.36, left_desired_angle, left_pos_, left_spd_, *pid_legs_[0], *pid_thetas_[0], - *pid_thetas_[2], left_angle_, left_leg_state, period, 0.0, controller->getOverturn()); - right_cmd = - computePidLegCommand(0.36, right_desired_angle, right_pos_, right_spd_, *pid_legs_[1], *pid_thetas_[1], - *pid_thetas_[3], right_angle_, right_leg_state, period, 0.0, controller->getOverturn()); + const auto& model_params_ = controller->getModelParams(); + feedforward_force = model_params_->f_spring; + T_theta_diff = pid_theta_diff_->computeCommand(leg_theta_diff_, period); + left_cmd.force = pid_legs_[0]->computeCommand(desired_leg_length_ - left_pos_[0], period) + feedforward_force; + right_cmd.force = pid_legs_[1]->computeCommand(desired_leg_length_ - right_pos_[0], period) + feedforward_force; + leg_conv(left_cmd.force, T_theta_diff, left_angle_[0], left_angle_[1], left_cmd.input); + leg_conv(right_cmd.force, -T_theta_diff, right_angle_[0], right_angle_[1], right_cmd.input); + if (abs(leg_theta_diff_) < 0.1) + { + left_cmd.torque = pid_thetas_[2]->computeCommand(leg_recovery_velocity_ - left_spd_[1], period); + right_cmd.torque = pid_thetas_[3]->computeCommand(leg_recovery_velocity_ - right_spd_[1], period); + leg_conv(left_cmd.force, 10 * leg_recovery_velocity_ + left_cmd.torque + T_theta_diff, left_angle_[0], + left_angle_[1], left_cmd.input); + leg_conv(right_cmd.force, 10 * leg_recovery_velocity_ + right_cmd.torque - T_theta_diff, right_angle_[0], + right_angle_[1], right_cmd.input); + } } setJointCommands(joint_handles_, left_cmd, right_cmd); // Exit - if (!controller->getOverturn() && abs(x_left_[5]) < 0.2 && abs(x_left_[1]) < 0.2 && abs(x_right_[1]) < 0.2) + if (abs(pitch_) < 0.2 && linear_acc_base_.z > 5.0 && !controller->getOverturn()) { controller->setMode(BalanceMode::STAND_UP); controller->setStateChange(false); @@ -51,54 +65,18 @@ void Recover::execute(BipedalController* controller, const ros::Time& time, cons } } -void Recover::setUpLegMotion(const Eigen::Matrix& x, const int& other_leg_state, - const double& leg_length, const double& leg_theta, int& leg_state, double& theta_des) +void Recover::detectChassisStateToRecover() { - switch (leg_state) + // pitch_ is base_link pitch not model pitch + if (pitch_ > 0.45 && pitch_ < M_PI) { - case LegState::UNDER: - theta_des = leg_theta; - if (leg_length > 0.35) - { - leg_state = LegState::BEHIND; - } - break; - case LegState::FRONT: - theta_des = leg_theta; - if (other_leg_state == LegState::FRONT) - { - theta_des = M_PI / 2 - 0.35; - } - break; - case LegState::BEHIND: - theta_des = leg_theta; - if ((leg_theta < -M_PI / 2 + 0.7 && leg_theta > -M_PI) || (leg_theta < M_PI && leg_theta > M_PI - 0.5)) - { - leg_state = LegState::FRONT; - } - break; + ROS_INFO("forward"); + recovery_chassis_state_ = RecoveryChassisState::ForwardSlip; } -} - -void Recover::detectLegRelState(const double& leg_theta, int& leg_state) -{ - if (leg_theta > -M_PI / 2 + 0.7 && leg_theta < (M_PI / 2 - 1.4)) - leg_state = LegState::UNDER; - else if ((leg_theta < -M_PI / 2 + 0.7 && leg_theta > -M_PI) || (leg_theta < M_PI && leg_theta > M_PI - 0.5)) - leg_state = LegState::FRONT; - else if (leg_theta > (M_PI / 2 - 1.4) && leg_theta < M_PI - 0.5) - leg_state = LegState::BEHIND; - switch (leg_state) + else if (pitch_ < -0.45 && pitch_ > -M_PI) { - case LegState::UNDER: - ROS_INFO("[balance] leg_theta: %.3f Leg state: UNDER", leg_theta); - break; - case LegState::FRONT: - ROS_INFO("[balance] leg_theta: %.3f Leg state: FRONT", leg_theta); - break; - case LegState::BEHIND: - ROS_INFO("[balance] leg_theta: %.3f Leg state: BEHIND", leg_theta); - break; + ROS_INFO("back"); + recovery_chassis_state_ = RecoveryChassisState::BackwardSlip; } } } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp index 5cab5c59..e2286e9a 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp @@ -29,7 +29,7 @@ void SitDown::execute(BipedalController* controller, const ros::Time& time, cons // Exit if (abs(x_left_(1)) < 0.1 && abs(x_left_(5)) < 0.2 && controller->getBaseState() != 4) { - if (!controller->getOverturn() && abs(x_left_(4)) < 0.1) + if (!controller->getOverturn()) controller->setMode(BalanceMode::STAND_UP); else controller->setMode(BalanceMode::RECOVER); diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp index f5114cf8..f921e412 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp @@ -22,20 +22,20 @@ void StandUp::execute(BipedalController* controller, const ros::Time& time, cons ROS_INFO("[balance] Enter STAND_UP"); controller->setStateChange(true); controller->setCompleteStand(false); - detectLegState(x_left_, left_leg_state); - detectLegState(x_right_, right_leg_state); + leg_state_threshold_ = controller->getLegThresholdParams(); + StandUp::detectLegState(x_left_, left_leg_state); + StandUp::detectLegState(x_right_, right_leg_state); } auto model_params_ = controller->getModelParams(); - double spring_force = -model_params_->f_spring; - double theta_des_l, theta_des_r, length_des_l, length_des_r; + spring_force_ = -model_params_->f_spring; LegCommand left_cmd = { 0, 0, { 0., 0. } }, right_cmd = { 0, 0, { 0., 0. } }; setUpLegMotion(x_left_, right_leg_state, left_pos_[0], left_pos_[1], left_leg_state, theta_des_l, length_des_l); setUpLegMotion(x_right_, left_leg_state, right_pos_[0], right_pos_[1], right_leg_state, theta_des_r, length_des_r); left_cmd = computePidLegCommand(length_des_l, theta_des_l, left_pos_, left_spd_, *pid_legs_[0], *pid_thetas_[0], - *pid_thetas_[2], left_angle_, left_leg_state, period, spring_force); + *pid_thetas_[2], left_angle_, left_leg_state, period, spring_force_); right_cmd = computePidLegCommand(length_des_r, theta_des_r, right_pos_, right_spd_, *pid_legs_[1], *pid_thetas_[1], - *pid_thetas_[3], right_angle_, right_leg_state, period, spring_force); + *pid_thetas_[3], right_angle_, right_leg_state, period, spring_force_); setJointCommands(joint_handles_, left_cmd, right_cmd); @@ -58,30 +58,64 @@ void StandUp::setUpLegMotion(const Eigen::Matrix& x, const switch (leg_state) { case LegState::UNDER: - theta_des = M_PI / 2 - 0.35; - length_des = 0.36; - if (leg_length > 0.35) + if (spring_force_ > 0) { - leg_state = LegState::FRONT; + theta_des = M_PI / 2 - 0.35; + length_des = 0.36; + if (leg_length > 0.35) + { + leg_state = LegState::FRONT; + } + } + else + { + theta_des = 0.0; + length_des = 0.18; } break; case LegState::FRONT: - theta_des = M_PI / 2 - 0.35; + theta_des = M_PI_2 - 0.35; length_des = 0.36; - if (abs(x[0] - theta_des) < 0.3 && abs(x[4]) < 0.3) + if ((abs(x[0] - theta_des) < 0.3 && abs(x[4]) < 0.3) || (abs(x[1]) < 0.1 && x[0] > M_PI_2)) leg_state = LegState::BEHIND; break; case LegState::BEHIND: theta_des = leg_theta; length_des = leg_length; - if (other_leg_state == LegState::BEHIND) + if (other_leg_state != LegState::FRONT) { length_des = 0.18; + if (leg_length < 0.3) + theta_des = 0.0; } - if (leg_length < 0.2) - { - theta_des = 0; - } + break; + } +} + +inline void StandUp::detectLegState(const Eigen::Matrix& x, int& leg_state) +{ + if (!leg_state_threshold_) + { + ROS_ERROR_THROTTLE(1.0, "LegUtils threshold params not initialized!"); + return; + } + if (x[0] > leg_state_threshold_->under_lower && x[0] < leg_state_threshold_->under_upper) + leg_state = LegState::UNDER; + else if ((x[0] < leg_state_threshold_->front_lower && x[0] > -M_PI) || + (x[0] < M_PI && x[0] > leg_state_threshold_->front_upper)) + leg_state = LegState::FRONT; + else if (x[0] > leg_state_threshold_->behind_lower && x[0] < leg_state_threshold_->behind_upper) + leg_state = LegState::BEHIND; + switch (leg_state) + { + case LegState::UNDER: + ROS_INFO("[balance] x[0]: %.3f Leg state: UNDER", x[0]); + break; + case LegState::FRONT: + ROS_INFO("[balance] x[0]: %.3f Leg state: FRONT", x[0]); + break; + case LegState::BEHIND: + ROS_INFO("[balance] x[0]: %.3f Leg state: BEHIND", x[0]); break; } } diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp index a8976794..2f8c3026 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp @@ -22,11 +22,13 @@ void Upstairs::execute(BipedalController* controller, const ros::Time& time, con ROS_INFO("[balance] Enter Upstairs"); controller->setStateChange(true); controller->setCompleteStand(false); + leg_state_threshold_ = controller->getLegThresholdParams(); detectLegState(x_left_, left_leg_state); detectLegState(x_right_, right_leg_state); } - double theta_des_l{ M_PI_2 - 0.3 }, theta_des_r{ M_PI_2 - 0.3 }, length_des_l{ 0.18 }, length_des_r{ 0.18 }; + double theta_des_l{ M_PI_2 - 0.6 }, theta_des_r{ M_PI_2 - 0.6 }, length_des_l{ 0.18 }, length_des_r{ 0.18 }; + 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); @@ -35,12 +37,41 @@ void Upstairs::execute(BipedalController* controller, const ros::Time& time, con setJointCommands(joint_handles_, left_cmd, right_cmd); // Exit - if (left_pos_[0] < 0.2 && left_pos_[1] > (M_PI_2 - 0.5) && right_pos_[0] < 0.2 && right_pos_[1] > (M_PI_2 - 0.5)) + if (left_pos_[0] < 0.2 && left_pos_[1] > leg_state_threshold_->upstair_exit_threshold && right_pos_[0] < 0.2 && + right_pos_[1] > leg_state_threshold_->upstair_exit_threshold) { - controller->pubLegLenStatus(false); + controller->pubLegLenStatus(true); controller->setMode(BalanceMode::STAND_UP); controller->setStateChange(false); ROS_INFO("[balance] Exit Upstairs"); } } + +inline void Upstairs::detectLegState(const Eigen::Matrix& x, int& leg_state) +{ + if (!leg_state_threshold_) + { + ROS_ERROR_THROTTLE(1.0, "LegUtils threshold params not initialized!"); + return; + } + if (x[0] > leg_state_threshold_->under_lower && x[0] < leg_state_threshold_->under_upper) + leg_state = LegState::UNDER; + else if ((x[0] < leg_state_threshold_->front_lower && x[0] > -M_PI) || + (x[0] < M_PI && x[0] > leg_state_threshold_->front_upper)) + leg_state = LegState::FRONT; + else if (x[0] > leg_state_threshold_->behind_lower && x[0] < leg_state_threshold_->behind_upper) + leg_state = LegState::BEHIND; + switch (leg_state) + { + case LegState::UNDER: + ROS_INFO("[balance] x[0]: %.3f Leg state: UNDER", x[0]); + break; + case LegState::FRONT: + ROS_INFO("[balance] x[0]: %.3f Leg state: FRONT", x[0]); + break; + case LegState::BEHIND: + ROS_INFO("[balance] x[0]: %.3f Leg state: BEHIND", x[0]); + break; + } +} } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp index bc6e3da7..18f3abea 100644 --- a/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp @@ -49,6 +49,14 @@ bool VMCController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ROS_ERROR("Load param fail, check the resist of spring_force"); return false; } + double l1, l2; + if (!controller_nh.getParam("l1", l1) || !controller_nh.getParam("l2", l2)) + { + ROS_ERROR("Load param fail, check the resist of l1 or l2"); + return false; + } + vmcPtr_ = std::make_unique(l1, l2, 0); + jointThigh_ = robot_hw->get()->getHandle(thighJoint); jointKnee_ = robot_hw->get()->getHandle(kneeJoint); return true; @@ -66,14 +74,14 @@ void VMCController::update(const ros::Time& time, const ros::Duration& period) // series leg vmc // gazebo - // thigh_angle = jointThigh_.getPosition() + M_PI_2; - // knee_angle = jointKnee_.getPosition() - M_PI_2; + thigh_angle = jointThigh_.getPosition() + M_PI_2; + knee_angle = jointKnee_.getPosition() - M_PI_2; // five link vmc - thigh_angle = jointThigh_.getPosition() + M_PI; - knee_angle = jointKnee_.getPosition(); - leg_pos(thigh_angle, knee_angle, position); - leg_spd(jointThigh_.getVelocity(), jointKnee_.getVelocity(), thigh_angle, knee_angle, speed); + // thigh_angle = jointThigh_.getPosition() + M_PI; + // knee_angle = jointKnee_.getPosition(); + vmcPtr_->leg_pos(thigh_angle, knee_angle, position); + vmcPtr_->leg_spd(jointThigh_.getVelocity(), jointKnee_.getVelocity(), thigh_angle, knee_angle, speed); double effortCmd[2], jointCmd[2]; static double angleSinCmd_ = 0; @@ -88,7 +96,7 @@ void VMCController::update(const ros::Time& time, const ros::Duration& period) effortCmd[0] = pidLength_.computeCommand(lengthCmd_ - position[0], period) - spring_force_; effortCmd[1] = pidAngle_.computeCommand(angle_error, period); - leg_conv(effortCmd[0], effortCmd[1], thigh_angle, knee_angle, jointCmd); + vmcPtr_->leg_conv(effortCmd[0], effortCmd[1], thigh_angle, knee_angle, jointCmd); std_msgs::Float64MultiArray state; state.data.push_back(thigh_angle); state.data.push_back(knee_angle); diff --git a/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/VMC.cpp b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/VMC.cpp new file mode 100644 index 00000000..a53bf332 --- /dev/null +++ b/rm_chassis_controllers/src/bipedal_wheel_controller/vmc/VMC.cpp @@ -0,0 +1,90 @@ +// +// Created by wk on 2026/2/25. +// + +#include "bipedal_wheel_controller/vmc/VMC.h" +#include + +namespace rm_chassis_controllers +{ +void VMC::leg_pos(double phi1, double phi4, double pos[2]) const +{ + double a_tmp; + double t4; + /* This function was generated by the Symbolic Math Toolbox version 9.2. + */ + /* 15-Sep-2025 20:28:02 */ + t4 = phi1 + phi4; + a_tmp = cos(phi1) * l1_ + cos(t4) * l2_; + t4 = sin(phi1) * l1_ + sin(t4) * l2_; + pos[0] = sqrt(a_tmp * a_tmp + t4 * t4); + pos[1] = atan2(t4, a_tmp); +} + +void VMC::leg_spd(double dphi1, double dphi4, double phi1, double phi4, double* spd) +{ + double J[2][2]; + + calc_jacobian(phi1, phi4, J); + + // 速度映射 + spd[0] = J[0][0] * dphi1 + J[0][1] * dphi4; // dl0 + spd[1] = J[1][0] * dphi1 + J[1][1] * dphi4; // dphi0 +} + +void VMC::leg_conv(double F, double Tp, double phi1, double phi4, double* T) +{ + double J[2][2]; + + calc_jacobian(phi1, phi4, J); + + // J^T * [F; Tp] + T[0] = J[0][0] * F + J[1][0] * Tp; + T[1] = J[0][1] * F + J[1][1] * Tp; +} + +void VMC::calc_jacobian(double phi1, double phi4, double J[2][2]) +{ + const double l1 = 0.218; + const double l2 = 0.26; + + double phi2 = phi1 + phi4; + + double c1 = cos(phi1); + double s1 = sin(phi1); + double c2 = cos(phi2); + double s2 = sin(phi2); + + // 末端笛卡尔坐标 + double Cx = l1 * c1 + l2 * c2; + double Cy = l1 * s1 + l2 * s2; + + double L0 = sqrt(Cx * Cx + Cy * Cy); + + // 防止奇异 + if (L0 < 1e-8) + { + J[0][0] = J[0][1] = 0.0; + J[1][0] = J[1][1] = 0.0; + return; + } + + // 偏导 + double dx_dphi1 = -l1 * s1 - l2 * s2; + double dy_dphi1 = l1 * c1 + l2 * c2; + + double dx_dphi4 = -l2 * s2; + double dy_dphi4 = l2 * c2; + + double inv_L0 = 1.0 / L0; + double inv_L0_sq = 1.0 / (L0 * L0); + + // 第一行：dl0/dphi + J[0][0] = (Cx * dx_dphi1 + Cy * dy_dphi1) * inv_L0; + J[0][1] = (Cx * dx_dphi4 + Cy * dy_dphi4) * inv_L0; + + // 第二行：dphi0/dphi + J[1][0] = (Cx * dy_dphi1 - Cy * dx_dphi1) * inv_L0_sq; + J[1][1] = (Cx * dy_dphi4 - Cy * dx_dphi4) * inv_L0_sq; +} +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/test/vmc_controller.yaml b/rm_chassis_controllers/test/vmc_controller.yaml index 2a4f21e0..ef0a4a37 100644 --- a/rm_chassis_controllers/test/vmc_controller.yaml +++ b/rm_chassis_controllers/test/vmc_controller.yaml @@ -8,8 +8,10 @@ controllers: vmc_controller: type: rm_chassis_controllers/VMCController vmc_bias_angle: 1.57 - spring_force: 75.0 - pid_length: { p: 800.0, i: 0, d: 20, i_clamp_max: 20.0, i_clamp_min: -20.0, antiwindup: true, publish_state: true } + spring_force: 0.0 + pid_length: { p: 800.0, i: 0, d: 25, i_clamp_max: 20.0, i_clamp_min: -20.0, antiwindup: true, publish_state: true } pid_angle: { p: 30.0, i: 0, d: 2, i_clamp_max: 15.0, i_clamp_min: -15.0, antiwindup: true, publish_state: true } thigh_joint: left_hip_joint knee_joint: left_knee_joint + l1: 0.218 + l2: 0.26