From 9f0bdd92e65c81f011e47078246d081eab5210bd Mon Sep 17 00:00:00 2001 From: dzran1210 Date: Tue, 3 Nov 2026 14:00:08 +0000 Subject: [PATCH 1/5] Update powerlimit for omni and swerve. --- .../rm_chassis_controllers/chassis_base.h | 2 + .../include/rm_chassis_controllers/omni.h | 12 +- .../include/rm_chassis_controllers/swerve.h | 16 +- rm_chassis_controllers/src/omni.cpp | 226 +++++++++++++ rm_chassis_controllers/src/swerve.cpp | 301 +++++++++++++++++- 5 files changed, 552 insertions(+), 5 deletions(-) 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 9a95c74c..f5372fd0 100755 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -190,6 +190,8 @@ class ChassisBase : public controller_interface::MultiInterfaceController 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 + std::unique_ptr> effort_coeff_pub_; //effort coeff publisher on rls + std::unique_ptr> velocity_coeff_pub_; //velocity coeff publisher on rls rm_common::TfRtBroadcaster brcst4global_map2robot_odom_{}; rm_common::TfRtBroadcaster brcst4global_map2camera_init_{}; 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..bee01bd1 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h @@ -39,8 +39,11 @@ #include "rm_chassis_controllers/chassis_base.h" +#include +#include #include #include +#include namespace rm_chassis_controllers { @@ -61,7 +64,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/omni.cpp b/rm_chassis_controllers/src/omni.cpp index 903ad007..b2d35696 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,47 @@ 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); + auto effort_coeff_publisher = + std::make_unique>(controller_nh, "rls/effort_coeff", 50); + this->effort_coeff_pub_ = std::move(effort_coeff_publisher); + auto velocity_coeff_publisher = + std::make_unique>(controller_nh, "rls/velocity_coeff", 50); + this->velocity_coeff_pub_ = std::move(velocity_coeff_publisher); + XmlRpc::XmlRpcValue wheels; controller_nh.getParam("wheels", wheels); chassis2joints_.resize(wheels.size(), 3); @@ -77,5 +120,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/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index 69c21077..ccc2dad1 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,7 +122,7 @@ 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_); + pivot_joint_handles_.push_back(m.ctrl_pivot_->joint_); joint_handles_.push_back(m.ctrl_wheel_->joint_); modules_.push_back(m); } @@ -84,6 +133,7 @@ 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_) { @@ -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 From bc6ced9832d7246a81e17f368dc509671adddc95 Mon Sep 17 00:00:00 2001 From: dzran1210 Date: Wed, 4 Nov 2026 18:08:08 +0000 Subject: [PATCH 2/5] Remove rls. --- .../rm_chassis_controllers/chassis_base.h | 14 +-- .../include/rm_chassis_controllers/omni.h | 2 - .../include/rm_chassis_controllers/swerve.h | 3 - rm_chassis_controllers/src/chassis_base.cpp | 19 --- rm_chassis_controllers/src/omni.cpp | 72 +++-------- rm_chassis_controllers/src/swerve.cpp | 115 ++++++------------ 6 files changed, 59 insertions(+), 166 deletions(-) mode change 100755 => 100644 rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h old mode 100755 new mode 100644 index f5372fd0..0be71236 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -178,7 +178,6 @@ class ChassisBase : public controller_interface::MultiInterfaceController 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_{}; @@ -187,11 +186,8 @@ class ChassisBase : public controller_interface::MultiInterfaceController realtime_tools::RealtimeBuffer slam_rt_buffer_{}; realtime_tools::RealtimeBuffer localization_rt_buffer_{}; std::unique_ptr> odom_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 - std::unique_ptr> effort_coeff_pub_; //effort coeff publisher on rls - std::unique_ptr> velocity_coeff_pub_; //velocity coeff publisher on rls + std::unique_ptr> command_power_pub_; + std::unique_ptr> estimated_power_pub_; rm_common::TfRtBroadcaster brcst4global_map2robot_odom_{}; rm_common::TfRtBroadcaster brcst4global_map2camera_init_{}; @@ -212,7 +208,6 @@ class ChassisBase : public controller_interface::MultiInterfaceController 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 }; @@ -230,9 +225,6 @@ class ChassisBase : public controller_interface::MultiInterfaceController double twist_angular_{ M_PI / 6 }; double max_odom_vel_{ 10.0 }; double timeout_{ 0.1 }; - double chassis_power_{ 0.0 }; - bool capacity_update_flag_{ false }; - bool use_rls_{ false }; bool use_K_angle_{ false }; bool odom_initialized_{ false }; @@ -250,8 +242,6 @@ class ChassisBase : public controller_interface::MultiInterfaceController std::string slam_topic_{ "/slam/odometry" }; std::string localization_topic_{ "/shinji/result" }; - std::string capacity_topic_{ "/rm_referee/power_management/sample_and_status" }; - ros::Time last_publish_time_{}; geometry_msgs::Vector3 vel_cmd_{}; // x, y std::array twist_covariance_{}; diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h index 8c482f03..c9a5f468 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h @@ -6,7 +6,6 @@ #include #include -#include #include "rm_chassis_controllers/chassis_base.h" @@ -31,7 +30,6 @@ class OmniController : public ChassisBase 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 bee01bd1..3684330d 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h @@ -39,8 +39,6 @@ #include "rm_chassis_controllers/chassis_base.h" -#include -#include #include #include #include @@ -75,7 +73,6 @@ class SwerveController : public ChassisBase, 2> motor_lp_filters_{}; - std::unique_ptr> rls_{}; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 42c3e5ca..1d688257 100755 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -105,13 +105,6 @@ bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan 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); ramp_x_ = std::make_unique>(0, 0.001); ramp_y_ = std::make_unique>(0, 0.001); @@ -515,16 +508,4 @@ void ChassisBase::localizationCallback(const geometry_msgs::TransformStamp 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 b2d35696..5055e426 100644 --- a/rm_chassis_controllers/src/omni.cpp +++ b/rm_chassis_controllers/src/omni.cpp @@ -19,13 +19,11 @@ 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) @@ -37,28 +35,17 @@ bool OmniController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle 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 = + auto estimated_power_publisher = std::make_unique>(controller_nh, "power/estimated", 100); - this->epower_pub_ = std::move(epower_publisher); - auto cpower_publisher = + this->estimated_power_pub_ = std::move(estimated_power_publisher); + auto command_power_publisher = std::make_unique>(controller_nh, "power/commanded", 100); - this->cpower_pub_ = std::move(cpower_publisher); - auto effort_coeff_publisher = - std::make_unique>(controller_nh, "rls/effort_coeff", 50); - this->effort_coeff_pub_ = std::move(effort_coeff_publisher); - auto velocity_coeff_publisher = - std::make_unique>(controller_nh, "rls/velocity_coeff", 50); - this->velocity_coeff_pub_ = std::move(velocity_coeff_publisher); + this->command_power_pub_ = std::move(command_power_publisher); XmlRpc::XmlRpcValue wheels; controller_nh.getParam("wheels", wheels); @@ -173,7 +160,7 @@ 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; @@ -187,7 +174,7 @@ void OmniController::powerLimit() 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 = @@ -202,24 +189,24 @@ void OmniController::powerLimit() { 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 + + 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 delta = square(b) - 4 * a * c; + if (!std::isfinite(delta) || delta < 0.0) + delta = 0.0; + if (delta >= 0) { - double Sqrt = sqrtf(Delta); + double sqrt = sqrtf(delta); if (wheel_power_limitor_.torque[i] >= 0) - joint.setCommand(((-B + Sqrt) / (2 * A)) * wheel_power_limitor_.K_angle[i]); + 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]); + joint.setCommand(((-b - sqrt) / (2 * a)) * wheel_power_limitor_.K_angle[i]); } else { - joint.setCommand(((-B) / (2 * A)) * wheel_power_limitor_.K_angle[i]); + joint.setCommand(((-b) / (2 * a)) * wheel_power_limitor_.K_angle[i]); } } } @@ -270,27 +257,6 @@ void OmniController::updatePowerStatus() 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()) { @@ -299,8 +265,8 @@ void OmniController::updatePowerStatus() } }; - publishPower(epower_pub_, estimated_total_power); - publishPower(cpower_pub_, cmd_total_power); + publishPower(estimated_power_pub_, estimated_total_power); + publishPower(command_power_pub_, cmd_total_power); } } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index ccc2dad1..e353faf7 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -48,7 +48,6 @@ 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); @@ -58,7 +57,6 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand 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) @@ -67,30 +65,21 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand 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 = + auto estimated_power_publisher = std::make_unique>(controller_nh, "power/estimated", 100); - this->epower_pub_ = std::move(epower_publisher); - auto cpower_publisher = + this->estimated_power_pub_ = std::move(estimated_power_publisher); + auto command_power_publisher = std::make_unique>(controller_nh, "power/commanded", 100); - this->cpower_pub_ = std::move(cpower_publisher); + this->command_power_pub_ = std::move(command_power_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); @@ -139,7 +128,7 @@ void SwerveController::moveJoint(const ros::Time& time, const ros::Duration& per { 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); @@ -231,7 +220,6 @@ 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; @@ -245,7 +233,7 @@ void SwerveController::powerLimit() 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; @@ -257,31 +245,31 @@ void SwerveController::powerLimit() 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 + + 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 delta = square(b) - 4 * a * c; + if (!std::isfinite(delta) || delta < 0.0) + delta = 0.0; + if (delta >= 0) { - double Sqrt = sqrtf(Delta); + double Sqrt = sqrtf(delta); if (pivot_power_limitor_.torque[i] >= 0) - joint.setCommand((-B + Sqrt) / (2 * A)); + joint.setCommand((-b + Sqrt) / (2 * a)); else - joint.setCommand((-B - Sqrt) / (2 * A)); + joint.setCommand((-b - Sqrt) / (2 * a)); } else { - joint.setCommand((-B) / (2 * A)); + joint.setCommand((-b) / (2 * a)); } } } @@ -291,24 +279,24 @@ void SwerveController::powerLimit() { 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 + + 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 delta = square(b) - 4 * a * c; + if (!std::isfinite(delta) || delta < 0.0) + delta = 0.0; + if (delta >= 0) { - double Sqrt = sqrtf(Delta); + double sqrt = sqrtf(delta); if (wheel_power_limitor_.torque[i] >= 0) - joint.setCommand(((-B + Sqrt) / (2 * A)) * wheel_power_limitor_.K_angle[i]); + 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]); + joint.setCommand(((-b - sqrt) / (2 * a)) * wheel_power_limitor_.K_angle[i]); } else { - joint.setCommand(((-B) / (2 * A)) * wheel_power_limitor_.K_angle[i]); + joint.setCommand(((-b) / (2 * a)) * wheel_power_limitor_.K_angle[i]); } } } @@ -329,7 +317,7 @@ void SwerveController::updatePowerStatus() pivot_power_limitor_.max_power = pivot_power_limitor_.ratio * power_limit; - double epivot_power{}, cpivot_power{}; + double estimated_pivot_power{}, command_pivot_power{}; for (size_t i = 0; i < modules_.size() && i < 4; ++i) { auto& module = modules_[i]; @@ -339,17 +327,17 @@ void SwerveController::updatePowerStatus() 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); + estimated_pivot_power += real_torque * real_vel + pivot_power_limitor_.effort_coeff * square(real_torque) + + pivot_power_limitor_.vel_coeff * abs(real_vel); + command_pivot_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; + pivot_power_limitor_.cmd_power = command_pivot_power + pivot_power_limitor_.power_offset; + pivot_power_limitor_.estimated_power = estimated_pivot_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; @@ -383,33 +371,6 @@ void SwerveController::updatePowerStatus() 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()) { @@ -418,8 +379,8 @@ void SwerveController::updatePowerStatus() } }; - publishPower(epower_pub_, estimated_total_power); - publishPower(cpower_pub_, cmd_total_power); + publishPower(estimated_power_pub_, estimated_total_power); + publishPower(command_power_pub_, cmd_total_power); } PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::SwerveController, controller_interface::ControllerBase) From 499b7779e8fe3802fdf732795166f889dbf505c0 Mon Sep 17 00:00:00 2001 From: dzran1210 Date: Thu, 5 Nov 2026 05:27:34 +0000 Subject: [PATCH 3/5] Fix format problem. --- rm_chassis_controllers/src/omni.cpp | 14 +++++++------- rm_chassis_controllers/src/swerve.cpp | 14 +++++++------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/rm_chassis_controllers/src/omni.cpp b/rm_chassis_controllers/src/omni.cpp index 5055e426..417d8d0b 100644 --- a/rm_chassis_controllers/src/omni.cpp +++ b/rm_chassis_controllers/src/omni.cpp @@ -228,7 +228,7 @@ void OmniController::updatePowerStatus() wheel_power_limitor_.max_power = power_limit; wheel_power_limitor_.err_sum = 0; - double ewheel_power{}, cwheel_power{}; + double estimated_wheel_power{}, command_wheel_power{}; for (size_t i = 0; i < joints_.size() && i < 4; ++i) { auto& ctl = joints_[i]; @@ -243,16 +243,16 @@ void OmniController::updatePowerStatus() 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); + estimated_wheel_power += real_torque * real_vel + wheel_power_limitor_.effort_coeff * square(real_torque) + + wheel_power_limitor_.vel_coeff * abs(real_vel); + command_wheel_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; + wheel_power_limitor_.cmd_power = command_wheel_power + wheel_power_limitor_.power_offset; + wheel_power_limitor_.estimated_power = estimated_wheel_power + wheel_power_limitor_.power_offset; double estimated_total_power = wheel_power_limitor_.estimated_power; double cmd_total_power = wheel_power_limitor_.cmd_power; diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index e353faf7..6ff6b480 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -342,7 +342,7 @@ void SwerveController::updatePowerStatus() 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{}; + double estimated_wheel_power{}, command_wheel_power{}; for (size_t i = 0; i < modules_.size() && i < 4; ++i) { auto& module = modules_[i]; @@ -357,16 +357,16 @@ void SwerveController::updatePowerStatus() 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); + estimated_wheel_power += real_torque * real_vel + wheel_power_limitor_.effort_coeff * square(real_torque) + + wheel_power_limitor_.vel_coeff * abs(real_vel); + command_wheel_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; + wheel_power_limitor_.cmd_power = command_wheel_power + wheel_power_limitor_.power_offset; + wheel_power_limitor_.estimated_power = estimated_wheel_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; From 397ff96641e102d45118f390277ed0f83db69c35 Mon Sep 17 00:00:00 2001 From: dzran1210 Date: Thu, 5 Nov 2026 05:46:56 +0000 Subject: [PATCH 4/5] Fix formatting issues. --- .../rm_chassis_controllers/chassis_base.h | 2 +- rm_chassis_controllers/src/omni.cpp | 14 ++++++------ rm_chassis_controllers/src/swerve.cpp | 22 +++++++++---------- 3 files changed, 19 insertions(+), 19 deletions(-) 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 0be71236..3caa0c14 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -186,7 +186,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController realtime_tools::RealtimeBuffer slam_rt_buffer_{}; realtime_tools::RealtimeBuffer localization_rt_buffer_{}; std::unique_ptr> odom_pub_; - std::unique_ptr> command_power_pub_; + std::unique_ptr> cmd_power_pub_; std::unique_ptr> estimated_power_pub_; rm_common::TfRtBroadcaster brcst4global_map2robot_odom_{}; diff --git a/rm_chassis_controllers/src/omni.cpp b/rm_chassis_controllers/src/omni.cpp index 417d8d0b..0e1f59b0 100644 --- a/rm_chassis_controllers/src/omni.cpp +++ b/rm_chassis_controllers/src/omni.cpp @@ -43,9 +43,9 @@ bool OmniController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle auto estimated_power_publisher = std::make_unique>(controller_nh, "power/estimated", 100); this->estimated_power_pub_ = std::move(estimated_power_publisher); - auto command_power_publisher = + auto cmd_power_publisher = std::make_unique>(controller_nh, "power/commanded", 100); - this->command_power_pub_ = std::move(command_power_publisher); + this->cmd_power_pub_ = std::move(cmd_power_publisher); XmlRpc::XmlRpcValue wheels; controller_nh.getParam("wheels", wheels); @@ -228,7 +228,7 @@ void OmniController::updatePowerStatus() wheel_power_limitor_.max_power = power_limit; wheel_power_limitor_.err_sum = 0; - double estimated_wheel_power{}, command_wheel_power{}; + double estimated_wheel_power{}, cmd_wheel_power{}; for (size_t i = 0; i < joints_.size() && i < 4; ++i) { auto& ctl = joints_[i]; @@ -245,13 +245,13 @@ void OmniController::updatePowerStatus() estimated_wheel_power += real_torque * real_vel + wheel_power_limitor_.effort_coeff * square(real_torque) + wheel_power_limitor_.vel_coeff * abs(real_vel); - command_wheel_power += cmd_torque * real_vel + wheel_power_limitor_.effort_coeff * square(cmd_torque) + - wheel_power_limitor_.vel_coeff * abs(real_vel); + cmd_wheel_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 = command_wheel_power + wheel_power_limitor_.power_offset; + wheel_power_limitor_.cmd_power = cmd_wheel_power + wheel_power_limitor_.power_offset; wheel_power_limitor_.estimated_power = estimated_wheel_power + wheel_power_limitor_.power_offset; double estimated_total_power = wheel_power_limitor_.estimated_power; @@ -266,7 +266,7 @@ void OmniController::updatePowerStatus() }; publishPower(estimated_power_pub_, estimated_total_power); - publishPower(command_power_pub_, cmd_total_power); + publishPower(cmd_power_pub_, cmd_total_power); } } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index 6ff6b480..b4e8ded0 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -77,9 +77,9 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand auto estimated_power_publisher = std::make_unique>(controller_nh, "power/estimated", 100); this->estimated_power_pub_ = std::move(estimated_power_publisher); - auto command_power_publisher = + auto cmd_power_publisher = std::make_unique>(controller_nh, "power/commanded", 100); - this->command_power_pub_ = std::move(command_power_publisher); + this->cmd_power_pub_ = std::move(cmd_power_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); @@ -317,7 +317,7 @@ void SwerveController::updatePowerStatus() pivot_power_limitor_.max_power = pivot_power_limitor_.ratio * power_limit; - double estimated_pivot_power{}, command_pivot_power{}; + double estimated_pivot_power{}, cmd_pivot_power{}; for (size_t i = 0; i < modules_.size() && i < 4; ++i) { auto& module = modules_[i]; @@ -329,20 +329,20 @@ void SwerveController::updatePowerStatus() estimated_pivot_power += real_torque * real_vel + pivot_power_limitor_.effort_coeff * square(real_torque) + pivot_power_limitor_.vel_coeff * abs(real_vel); - command_pivot_power += cmd_torque * real_vel + pivot_power_limitor_.effort_coeff * square(cmd_torque) + - pivot_power_limitor_.vel_coeff * abs(real_vel); + cmd_pivot_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 = command_pivot_power + pivot_power_limitor_.power_offset; + pivot_power_limitor_.cmd_power = cmd_pivot_power + pivot_power_limitor_.power_offset; pivot_power_limitor_.estimated_power = estimated_pivot_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 estimated_wheel_power{}, command_wheel_power{}; + double estimated_wheel_power{}, cmd_wheel_power{}; for (size_t i = 0; i < modules_.size() && i < 4; ++i) { auto& module = modules_[i]; @@ -359,13 +359,13 @@ void SwerveController::updatePowerStatus() estimated_wheel_power += real_torque * real_vel + wheel_power_limitor_.effort_coeff * square(real_torque) + wheel_power_limitor_.vel_coeff * abs(real_vel); - command_wheel_power += cmd_torque * real_vel + wheel_power_limitor_.effort_coeff * square(cmd_torque) + - wheel_power_limitor_.vel_coeff * abs(real_vel); + cmd_wheel_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 = command_wheel_power + wheel_power_limitor_.power_offset; + wheel_power_limitor_.cmd_power = cmd_wheel_power + wheel_power_limitor_.power_offset; wheel_power_limitor_.estimated_power = estimated_wheel_power + wheel_power_limitor_.power_offset; double estimated_total_power = pivot_power_limitor_.estimated_power + wheel_power_limitor_.estimated_power; @@ -380,7 +380,7 @@ void SwerveController::updatePowerStatus() }; publishPower(estimated_power_pub_, estimated_total_power); - publishPower(command_power_pub_, cmd_total_power); + publishPower(cmd_power_pub_, cmd_total_power); } PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::SwerveController, controller_interface::ControllerBase) From c9daa1c59516b6e3e9f02c479b15ed33a3dc5504 Mon Sep 17 00:00:00 2001 From: dzran1210 Date: Thu, 5 Nov 2026 06:37:16 +0000 Subject: [PATCH 5/5] Remove powerlimit dynamic_reconfigure. --- rm_chassis_controllers/CMakeLists.txt | 1 - rm_chassis_controllers/cfg/PowerLimit.cfg | 12 ------------ .../include/rm_chassis_controllers/chassis_base.h | 1 - 3 files changed, 14 deletions(-) delete mode 100644 rm_chassis_controllers/cfg/PowerLimit.cfg diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt index a61205e2..d6ffb1a7 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/cfg/PowerLimit.cfg b/rm_chassis_controllers/cfg/PowerLimit.cfg deleted file mode 100644 index 52247c4d..00000000 --- a/rm_chassis_controllers/cfg/PowerLimit.cfg +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python3 -PACKAGE = "rm_chassis_controllers" - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -gen.add("vel_coeff", double_t, 0, "velocity coefficient for power limit", 0.000, 0.000, 1.000) -gen.add("effort_coeff", double_t, 0, "effort coefficient for power limit", 0.0, 0.0, 20.0) -gen.add("power_offset", double_t, 0, "power offset for power limit", 0.0, -50.0, 50.0) - -exit(gen.generate(PACKAGE, "chassis_controller", "PowerLimit")) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h index 3caa0c14..43206c6a 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -48,7 +48,6 @@ #include #include #include -#include #include #include #include