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 old mode 100755 new mode 100644 index 9a95c74c..43206c6a --- 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 @@ -178,7 +177,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,9 +185,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> cmd_power_pub_; + std::unique_ptr> estimated_power_pub_; rm_common::TfRtBroadcaster brcst4global_map2robot_odom_{}; rm_common::TfRtBroadcaster brcst4global_map2camera_init_{}; @@ -210,7 +207,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 }; @@ -228,9 +224,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 }; @@ -248,8 +241,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 1bc67336..c9a5f468 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h @@ -5,6 +5,7 @@ #pragma once #include +#include #include "rm_chassis_controllers/chassis_base.h" @@ -16,14 +17,19 @@ 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_{}; }; } // 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..3684330d 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h @@ -41,6 +41,7 @@ #include #include +#include namespace rm_chassis_controllers { @@ -61,7 +62,17 @@ 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_{}; }; } // 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 903ad007..0e1f59b0 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,34 @@ bool OmniController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle { ChassisBase::init(robot_hw, root_nh, controller_nh); + 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_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; + + for (auto& filter : motor_lp_filters_) + { + filter = new LowPassFilter(20); + } + + auto estimated_power_publisher = + std::make_unique>(controller_nh, "power/estimated", 100); + this->estimated_power_pub_ = std::move(estimated_power_publisher); + auto cmd_power_publisher = + std::make_unique>(controller_nh, "power/commanded", 100); + this->cmd_power_pub_ = std::move(cmd_power_publisher); + XmlRpc::XmlRpcValue wheels; controller_nh.getParam("wheels", wheels); chassis2joints_.resize(wheels.size(), 3); @@ -77,5 +107,167 @@ 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(); + + 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); + } + + 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 estimated_wheel_power{}, cmd_wheel_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]); + + estimated_wheel_power += real_torque * real_vel + wheel_power_limitor_.effort_coeff * square(real_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 = 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; + double cmd_total_power = wheel_power_limitor_.cmd_power; + + auto publishPower = [](auto& pub, const double power) { + if (pub && pub->trylock()) + { + pub->msg_.data = power; + pub->unlockAndPublish(); + } + }; + + publishPower(estimated_power_pub_, estimated_total_power); + publishPower(cmd_power_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..b4e8ded0 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,44 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand { if (!ChassisBase::init(robot_hw, root_nh, controller_nh)) return false; + + 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_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; + + for (auto& filter_group : motor_lp_filters_) + { + for (auto& filter : filter_group) + filter = new LowPassFilter(20); + } + + auto estimated_power_publisher = + std::make_unique>(controller_nh, "power/estimated", 100); + this->estimated_power_pub_ = std::move(estimated_power_publisher); + auto cmd_power_publisher = + std::make_unique>(controller_nh, "power/commanded", 100); + 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); + 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 +111,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,12 +122,13 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand void SwerveController::moveJoint(const ros::Time& time, const ros::Duration& period) { + stateJudge(); Vec2 vel_center(vel_cmd_.x, vel_cmd_.y); for (auto& module : modules_) { Vec2 vel = vel_center + vel_cmd_.z * Vec2(-module.position_.y(), module.position_.x()); double vel_angle = std::atan2(vel.y(), vel.x()) + module.pivot_offset_; - // Direction flipping and Stray module mitigation + double a = angles::shortest_angular_distance(module.ctrl_pivot_->joint_.getPosition(), vel_angle); double b = angles::shortest_angular_distance(module.ctrl_pivot_->joint_.getPosition(), vel_angle + M_PI); module.ctrl_pivot_->setCommand(std::abs(a) < std::abs(b) ? vel_angle : vel_angle + M_PI); @@ -125,5 +164,224 @@ 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(); + + 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); + } + + 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; + } + + 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 estimated_pivot_power{}, cmd_pivot_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(); + + estimated_pivot_power += real_torque * real_vel + pivot_power_limitor_.effort_coeff * square(real_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 = 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{}, cmd_wheel_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]); + + estimated_wheel_power += real_torque * real_vel + wheel_power_limitor_.effort_coeff * square(real_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 = 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; + double cmd_total_power = pivot_power_limitor_.cmd_power + wheel_power_limitor_.cmd_power; + + auto publishPower = [](auto& pub, const double power) { + if (pub && pub->trylock()) + { + pub->msg_.data = power; + pub->unlockAndPublish(); + } + }; + + publishPower(estimated_power_pub_, estimated_total_power); + publishPower(cmd_power_pub_, cmd_total_power); +} + PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::SwerveController, controller_interface::ControllerBase) } // namespace rm_chassis_controllers