From fc2d3ec0c0c3791734ca471b678cf3260bc11e54 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sat, 7 Mar 2026 09:36:20 +0000 Subject: [PATCH 01/36] Merge ZY's codes. --- rm_chassis_controllers/CMakeLists.txt | 6 + rm_chassis_controllers/cfg/PowerLimit.cfg | 15 + .../rm_chassis_controllers/chassis_base.h | 122 ++++-- .../include/rm_chassis_controllers/swerve.h | 5 +- rm_chassis_controllers/package.xml | 1 + rm_chassis_controllers/src/balance.cpp | 4 +- rm_chassis_controllers/src/chassis_base.cpp | 403 ++++++++++-------- rm_chassis_controllers/src/omni.cpp | 2 +- rm_chassis_controllers/src/sentry.cpp | 4 +- rm_chassis_controllers/src/swerve.cpp | 66 ++- 10 files changed, 410 insertions(+), 218 deletions(-) create mode 100644 rm_chassis_controllers/cfg/PowerLimit.cfg diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt index ec9391d0..9476246b 100644 --- a/rm_chassis_controllers/CMakeLists.txt +++ b/rm_chassis_controllers/CMakeLists.txt @@ -21,10 +21,15 @@ find_package(catkin REQUIRED tf2_geometry_msgs nav_msgs angles + dynamic_reconfigure ) find_package(Eigen3 REQUIRED) +generate_dynamic_reconfigure_options( + cfg/PowerLimit.cfg +) + ################################### ## catkin specific configuration ## ################################### @@ -48,6 +53,7 @@ catkin_package( tf2_geometry_msgs nav_msgs angles + dynamic_reconfigure LIBRARIES ${PROJECT_NAME} ) diff --git a/rm_chassis_controllers/cfg/PowerLimit.cfg b/rm_chassis_controllers/cfg/PowerLimit.cfg new file mode 100644 index 00000000..58e474de --- /dev/null +++ b/rm_chassis_controllers/cfg/PowerLimit.cfg @@ -0,0 +1,15 @@ +#!/usr/bin/env python3 +PACKAGE = "rm_chassis_controllers" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("velocity_coeff", double_t, 0, "velocity coefficient for power limit", 0.0, 0.0, 20.0) +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) +gen.add("pivot_max_power", double_t, 0, "maximum power for pivot joints", 0.0, 0.0, 100.0) +gen.add("pivot_effort_coeff", double_t, 0, "effort coefficient for pivot power limit", 0.0, 0.0, 20.0) +gen.add("pivot_velocity_coeff", double_t, 0, "velocity coefficient for pivot power limit", 0.0, 0.0, 20.0) + +exit(gen.generate(PACKAGE, "chassis_controller", "PowerLimit")) \ No newline at end of file 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..8ada3443 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -48,6 +48,17 @@ #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include namespace rm_chassis_controllers { @@ -57,6 +68,13 @@ struct Command rm_msgs::ChassisCmd cmd_chassis_; ros::Time stamp_; }; + +struct PowerLimitParams +{ + double velocity_coeff{}; + double effort_coeff{}; + double power_offset{}; +}; template class ChassisBase : public controller_interface::MultiInterfaceController { @@ -129,7 +147,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController * * Receive power limit from command. Set max_effort command to chassis to avoid exceed power limit. */ - void powerLimit(); + virtual void powerLimit(); /** @brief Write current command from rm_msgs::ChassisCmd. * * @param msg This message contains various state parameter settings for basic chassis control @@ -140,46 +158,86 @@ class ChassisBase : public controller_interface::MultiInterfaceController * @param msg This expresses velocity in free space broken into its linear and angular parts. */ void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg); - void outsideOdomCallback(const nav_msgs::Odometry::ConstPtr& msg); + + void initialize_parameters(ros::NodeHandle& controller_nh); + void slamCallback(const nav_msgs::Odometry::ConstPtr& msg); + void localizationCallback(const geometry_msgs::TransformStamped::ConstPtr& msg); rm_control::RobotStateHandle robot_state_handle_{}; hardware_interface::EffortJointInterface* effort_joint_interface_{}; - std::vector joint_handles_{}; - - double wheel_radius_{}, publish_rate_{}, twist_angular_{}, timeout_{}, effort_coeff_{}, velocity_coeff_{}, - power_offset_{}; - double roll_ = 0., pitch_ = 0., yaw_ = 0.; - double pitch_angle_threshold_ = 0., scale_ = 0.; - double max_odom_vel_; - bool enable_uphill_acceleration_ = false; - bool enable_odom_tf_ = false; - bool topic_update_ = false; - bool publish_odom_tf_ = false; - bool state_changed_ = true; + std::vector wheel_joint_handles_{}; + realtime_tools::RealtimeBuffer cmd_rt_buffer_{}; + realtime_tools::RealtimeBuffer slam_rt_buffer_{}; + realtime_tools::RealtimeBuffer localization_rt_buffer_{}; + realtime_tools::RealtimeBuffer power_limit_rt_buffer_{}; + std::unique_ptr> odometry_rt_pub_; + std::unique_ptr> wheel_power_pub_; + dynamic_reconfigure::Server* power_limit_srv_{}; + + rm_common::TfRtBroadcaster brcst4global_map2robot_odom_{}; + rm_common::TfRtBroadcaster brcst4robot_odom2robot_base_{}; + + geometry_msgs::TransformStamped global_map2robot_odom_{}; + geometry_msgs::TransformStamped robot_odom2robot_base_{}; + geometry_msgs::TransformStamped robot_base2lidar_base_{}; + + tf2::Transform T_global_map2robot_odom_{}; + tf2::Transform T_robot_odom_2robot_base_{}; + tf2::Transform T_lidar_odom2lidar_base_{}; + tf2::Transform T_robot_base2lidar_base_{}; + tf2::Transform T_global_map2lidar_odom_{}; + + ros::Subscriber cmd_vel_sub_; + ros::Subscriber cmd_chassis_sub_; + ros::Subscriber slam_sub_; + ros::Subscriber localization_sub_; + + std::unique_ptr> ramp_x_{ nullptr }; + std::unique_ptr> ramp_y_{ nullptr }; + std::unique_ptr> ramp_w_{ nullptr }; + + double publish_rate_{ 100.0 }; + bool publish_map_tf_{ false }; + bool publish_odom_tf_{ false }; + + double velocity_coeff_{ 0.0 }; + double effort_coeff_{ 0.0 }; + double power_offset_{ 0.0 }; + bool power_limit_reconfig_initialized_{ false }; + + double wheel_radius_{ 0.02 }; + double twist_angular_{ M_PI / 6 }; + double max_odom_vel_{ 10.0 }; + double timeout_{ 0.1 }; + + bool odom_initialized_{ false }; + bool slam_updated_{ false }; + bool localization_updated_{ false }; + bool state_changed_{ true }; + int state_{ RAW }; + + std::string follow_source_frame_{}; + std::string command_source_frame_{}; + std::string global_map_frame_id_{ "map" }; + std::string robot_odom_frame_id_{ "odom" }; + std::string robot_base_frame_id_{ "base_link" }; + std::string lidar_base_frame_id_{ "livox_frame" }; + std::string slam_topic_{ "/Odometry" }; + std::string localization_topic_{ "/hdl_global_localization/result" }; + + ros::Time last_publish_time_{}; + geometry_msgs::Vector3 vel_cmd_{}; // x, y + control_toolbox::Pid pid_follow_{}; + + Command cmd_struct_{}; + PowerLimitParams power_config_{}; + enum { RAW, FOLLOW, TWIST }; - int state_ = RAW; - RampFilter*ramp_x_{}, *ramp_y_{}, *ramp_w_{}; - std::string follow_source_frame_{}, command_source_frame_{}; - - ros::Time last_publish_time_; - geometry_msgs::TransformStamped odom2base_{}; - tf2::Transform world2odom_; - geometry_msgs::Vector3 vel_cmd_{}; // x, y - control_toolbox::Pid pid_follow_; - - std::shared_ptr > odom_pub_; - rm_common::TfRtBroadcaster tf_broadcaster_{}; - ros::Subscriber outside_odom_sub_; - ros::Subscriber cmd_chassis_sub_; - ros::Subscriber cmd_vel_sub_; - Command cmd_struct_; - realtime_tools::RealtimeBuffer cmd_rt_buffer_; - realtime_tools::RealtimeBuffer odom_buffer_; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h index 23ddfb94..5165ff34 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h @@ -61,7 +61,10 @@ class SwerveController : public ChassisBase modules_; + void powerLimit() override; + std::vector modules_{}; + std::vector pivot_joint_handles_{}; + double pivot_max_power_{},pivot_effort_coeff_{},pivot_velocity_coeff_{}; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/package.xml b/rm_chassis_controllers/package.xml index bed77aad..8ae776a5 100644 --- a/rm_chassis_controllers/package.xml +++ b/rm_chassis_controllers/package.xml @@ -21,6 +21,7 @@ tf2_geometry_msgs nav_msgs angles + dynamic_reconfigure` robot_localization diff --git a/rm_chassis_controllers/src/balance.cpp b/rm_chassis_controllers/src/balance.cpp index 93b9bb73..dc5f0d7c 100644 --- a/rm_chassis_controllers/src/balance.cpp +++ b/rm_chassis_controllers/src/balance.cpp @@ -32,8 +32,8 @@ bool BalanceController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan } left_wheel_joint_handle_ = robot_hw->get()->getHandle(left_wheel_joint); right_wheel_joint_handle_ = robot_hw->get()->getHandle(right_wheel_joint); - joint_handles_.push_back(left_wheel_joint_handle_); - joint_handles_.push_back(right_wheel_joint_handle_); + wheel_joint_handles_.push_back(left_wheel_joint_handle_); + wheel_joint_handles_.push_back(right_wheel_joint_handle_); left_momentum_block_joint_handle_ = robot_hw->get()->getHandle(left_momentum_block_joint); right_momentum_block_joint_handle_ = diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 627b4831..d763c6b5 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -35,12 +35,6 @@ // Created by huakang on 2021/3/21. // #include "rm_chassis_controllers/chassis_base.h" -#include -#include -#include -#include -#include -#include namespace rm_chassis_controllers { @@ -49,73 +43,86 @@ template class ChassisBase; template -bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, - ros::NodeHandle& controller_nh) +void ChassisBase::initialize_parameters(ros::NodeHandle& controller_nh) { - if (!controller_nh.getParam("publish_rate", publish_rate_) || !controller_nh.getParam("timeout", timeout_) || - !controller_nh.getParam("power/vel_coeff", velocity_coeff_) || - !controller_nh.getParam("power/effort_coeff", effort_coeff_) || - !controller_nh.getParam("power/power_offset", power_offset_)) + try { - ROS_ERROR("Some chassis params doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; + controller_nh.getParam("publish_rate", publish_rate_); + controller_nh.getParam("publish_map_tf", publish_map_tf_); + controller_nh.getParam("publish_odom_tf", publish_odom_tf_); + controller_nh.getParam("slam_topic", slam_topic_); + controller_nh.getParam("localization_topic", localization_topic_); + + controller_nh.getParam("power/vel_coeff", velocity_coeff_); + controller_nh.getParam("power/effort_coeff", effort_coeff_); + controller_nh.getParam("power/power_offset", power_offset_); + + controller_nh.getParam("wheel_radius", wheel_radius_); + controller_nh.getParam("twist_angular", twist_angular_); + controller_nh.getParam("max_odom_vel", max_odom_vel_); + controller_nh.getParam("timeout", timeout_); + + if (controller_nh.hasParam("pid_follow")) + pid_follow_.init(ros::NodeHandle(controller_nh, "pid_follow")); } - pitch_angle_threshold_ = getParam(controller_nh, "pitch_angle_threshold", -0.25); - scale_ = getParam(controller_nh, "scale", 1.); - enable_uphill_acceleration_ = getParam(controller_nh, "enable_uphill_acceleration", false); - wheel_radius_ = getParam(controller_nh, "wheel_radius", 0.02); - twist_angular_ = getParam(controller_nh, "twist_angular", M_PI / 6); - max_odom_vel_ = getParam(controller_nh, "max_odom_vel", 0); - enable_odom_tf_ = getParam(controller_nh, "enable_odom_tf", true); - publish_odom_tf_ = getParam(controller_nh, "publish_odom_tf", false); - - // Get and check params for covariances - XmlRpc::XmlRpcValue twist_cov_list; - controller_nh.getParam("twist_covariance_diagonal", twist_cov_list); - ROS_ASSERT(twist_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray); - ROS_ASSERT(twist_cov_list.size() == 6); - for (int i = 0; i < twist_cov_list.size(); ++i) - ROS_ASSERT(twist_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble); - + catch (std::exception& e) + { + ROS_ERROR("Chassis parameter initialization failed: %s", e.what()); + } +} +template +bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh) +{ + initialize_parameters(controller_nh); robot_state_handle_ = robot_hw->get()->getHandle("robot_state"); effort_joint_interface_ = robot_hw->get(); + cmd_vel_sub_ = root_nh.subscribe("cmd_vel", 1, &ChassisBase::cmdVelCallback, this); + cmd_chassis_sub_ = + controller_nh.subscribe("/cmd_chassis", 1, &ChassisBase::cmdChassisCallback, this); + slam_sub_ = controller_nh.subscribe(slam_topic_, 10, &ChassisBase::slamCallback, this); + localization_sub_ = controller_nh.subscribe( + localization_topic_, 10, &ChassisBase::localizationCallback, this); + + // power publisher + auto power_publisher = + std::make_unique>(controller_nh, "wheel_power", 100); + this->wheel_power_pub_ = std::move(power_publisher); + // Setup odometry realtime publisher + odom message constant fields - odom_pub_.reset(new realtime_tools::RealtimePublisher(root_nh, "odom", 100)); - odom_pub_->msg_.header.frame_id = "odom"; - odom_pub_->msg_.child_frame_id = "base_link"; - odom_pub_->msg_.twist.covariance = { static_cast(twist_cov_list[0]), 0., 0., 0., 0., 0., 0., - static_cast(twist_cov_list[1]), 0., 0., 0., 0., 0., 0., - static_cast(twist_cov_list[2]), 0., 0., 0., 0., 0., 0., - static_cast(twist_cov_list[3]), 0., 0., 0., 0., 0., 0., - static_cast(twist_cov_list[4]), 0., 0., 0., 0., 0., 0., - static_cast(twist_cov_list[5]) }; - - ramp_x_ = new RampFilter(0, 0.001); - ramp_y_ = new RampFilter(0, 0.001); - ramp_w_ = new RampFilter(0, 0.001); - - // init odom tf - if (enable_odom_tf_) + auto odometry_publisher = + std::make_unique>(root_nh, "odom", 100); + this->odometry_rt_pub_ = std::move(odometry_publisher); + odometry_rt_pub_->msg_.header.frame_id = robot_odom_frame_id_; + odometry_rt_pub_->msg_.child_frame_id = robot_base_frame_id_; + odometry_rt_pub_->msg_.twist.covariance = { 0.001, 0., 0., 0., 0., 0., 0., 0.001, 0., 0., 0., 0., + 0., 0., 0.001, 0., 0., 0., 0., 0., 0., 0.001, 0., 0., + 0., 0., 0., 0., 0.001, 0., 0., 0., 0., 0., 0., 0.001 }; + + ramp_x_ = std::make_unique>(0, 0.001); + ramp_y_ = std::make_unique>(0, 0.001); + ramp_w_ = std::make_unique>(0, 0.001); + + if (publish_map_tf_) { - odom2base_.header.frame_id = "odom"; - odom2base_.header.stamp = ros::Time::now(); - odom2base_.child_frame_id = "base_link"; - odom2base_.transform.rotation.w = 1; - tf_broadcaster_.init(root_nh); - tf_broadcaster_.sendTransform(odom2base_); + global_map2robot_odom_.header.stamp = ros::Time::now(); + global_map2robot_odom_.header.frame_id = global_map_frame_id_; + global_map2robot_odom_.child_frame_id = robot_odom_frame_id_; + global_map2robot_odom_.transform.rotation.w = 1; + brcst4global_map2robot_odom_.init(root_nh); + brcst4global_map2robot_odom_.sendTransform(global_map2robot_odom_); } - world2odom_.setRotation(tf2::Quaternion::getIdentity()); - - outside_odom_sub_ = - controller_nh.subscribe("/odometry", 10, &ChassisBase::outsideOdomCallback, this); - cmd_chassis_sub_ = - controller_nh.subscribe("/cmd_chassis", 1, &ChassisBase::cmdChassisCallback, this); - cmd_vel_sub_ = root_nh.subscribe("cmd_vel", 1, &ChassisBase::cmdVelCallback, this); - if (controller_nh.hasParam("pid_follow")) - if (!pid_follow_.init(ros::NodeHandle(controller_nh, "pid_follow"))) - return false; + if (publish_odom_tf_) + { + robot_odom2robot_base_.header.stamp = ros::Time::now(); + robot_odom2robot_base_.header.frame_id = robot_odom_frame_id_; + robot_odom2robot_base_.child_frame_id = robot_base_frame_id_; + global_map2robot_odom_.transform.rotation.w = 1; + brcst4robot_odom2robot_base_.init(root_nh); + brcst4robot_odom2robot_base_.sendTransform(robot_odom2robot_base_); + } return true; } @@ -197,8 +204,9 @@ void ChassisBase::follow(const ros::Time& time, const ros::Duration& perio try { double roll{}, pitch{}, yaw{}; - quatToRPY(robot_state_handle_.lookupTransform("base_link", follow_source_frame_, ros::Time(0)).transform.rotation, - roll, pitch, yaw); + quatToRPY( + robot_state_handle_.lookupTransform(robot_base_frame_id_, follow_source_frame_, ros::Time(0)).transform.rotation, + roll, pitch, yaw); double follow_error = angles::shortest_angular_distance(yaw, 0); pid_follow_.computeCommand(-follow_error, period); vel_cmd_.z = pid_follow_.getCurrentCmd() + cmd_rt_buffer_.readFromRT()->cmd_chassis_.follow_vel_des; @@ -224,7 +232,8 @@ void ChassisBase::twist(const ros::Time& time, const ros::Duration& period try { double roll{}, pitch{}, yaw{}; - quatToRPY(robot_state_handle_.lookupTransform("base_link", command_source_frame_, ros::Time(0)).transform.rotation, + quatToRPY(robot_state_handle_.lookupTransform(robot_base_frame_id_, command_source_frame_, ros::Time(0)) + .transform.rotation, roll, pitch, yaw); double angle[4] = { -0.785, 0.785, 2.355, -2.355 }; @@ -265,110 +274,154 @@ void ChassisBase::raw() template void ChassisBase::updateOdom(const ros::Time& time, const ros::Duration& period) { - geometry_msgs::Twist vel_base = odometry(); // on base_link frame - if (enable_odom_tf_) + if (publish_map_tf_) { - geometry_msgs::Vector3 linear_vel_odom, angular_vel_odom; - try - { - odom2base_ = robot_state_handle_.lookupTransform("odom", "base_link", ros::Time(0)); - tf2::Quaternion q; - tf2::fromMsg(odom2base_.transform.rotation, q); - tf2::Matrix3x3(q).getEulerYPR(yaw_, pitch_, roll_); - } - catch (tf2::TransformException& ex) + if (!odom_initialized_) { - tf_broadcaster_.sendTransform(odom2base_); // TODO: For some reason, the sendTransform in init sometime not work? - ROS_WARN("%s", ex.what()); - return; + try + { + geometry_msgs::TransformStamped global_map2lidar_odom = + robot_state_handle_.lookupTransform(robot_base_frame_id_, lidar_base_frame_id_, ros::Time(0)); + T_global_map2lidar_odom_.setOrigin(tf2::Vector3(global_map2lidar_odom.transform.translation.x, + global_map2lidar_odom.transform.translation.y, + global_map2lidar_odom.transform.translation.z)); + T_global_map2lidar_odom_.setRotation( + tf2::Quaternion(global_map2lidar_odom.transform.rotation.x, global_map2lidar_odom.transform.rotation.y, + global_map2lidar_odom.transform.rotation.z, global_map2lidar_odom.transform.rotation.w)); + odom_initialized_ = true; + } + catch (...) + { + ROS_WARN("Failed to init robot_odom2lidar_odom."); + } } - odom2base_.header.stamp = time; - // integral vel to pos and angle - tf2::doTransform(vel_base.linear, linear_vel_odom, odom2base_); - tf2::doTransform(vel_base.angular, angular_vel_odom, odom2base_); - double length = - std::sqrt(std::pow(linear_vel_odom.x, 2) + std::pow(linear_vel_odom.y, 2) + std::pow(linear_vel_odom.z, 2)); - if (length < max_odom_vel_) + + if (localization_updated_) { - // avoid nan vel - odom2base_.transform.translation.x += linear_vel_odom.x * period.toSec(); - odom2base_.transform.translation.y += linear_vel_odom.y * period.toSec(); - odom2base_.transform.translation.z += linear_vel_odom.z * period.toSec(); - } - length = - std::sqrt(std::pow(angular_vel_odom.x, 2) + std::pow(angular_vel_odom.y, 2) + std::pow(angular_vel_odom.z, 2)); - if (length > 0.001) - { // avoid nan quat - tf2::Quaternion odom2base_quat, trans_quat; - tf2::fromMsg(odom2base_.transform.rotation, odom2base_quat); - trans_quat.setRotation(tf2::Vector3(angular_vel_odom.x / length, angular_vel_odom.y / length, - angular_vel_odom.z / length), - length * period.toSec()); - odom2base_quat = trans_quat * odom2base_quat; - odom2base_quat.normalize(); - odom2base_.transform.rotation = tf2::toMsg(odom2base_quat); + try + { + localization_updated_ = false; + const auto& localization = localization_rt_buffer_.readFromRT(); + T_global_map2lidar_odom_.setOrigin(tf2::Vector3(localization->transform.translation.x, + localization->transform.translation.y, + localization->transform.translation.z)); + T_global_map2lidar_odom_.setRotation( + tf2::Quaternion(localization->transform.rotation.x, localization->transform.rotation.y, + localization->transform.rotation.z, localization->transform.rotation.w)); + } + catch (...) + { + ROS_WARN("Failed to update localization offset."); + } } - } - if (topic_update_) - { - auto* odom_msg = odom_buffer_.readFromRT(); - - tf2::Transform world2sensor; - world2sensor.setOrigin( - tf2::Vector3(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z)); - world2sensor.setRotation(tf2::Quaternion(odom_msg->pose.pose.orientation.x, odom_msg->pose.pose.orientation.y, - odom_msg->pose.pose.orientation.z, odom_msg->pose.pose.orientation.w)); - - if (world2odom_.getRotation() == tf2::Quaternion::getIdentity()) // First received + if (slam_updated_) { - tf2::Transform odom2sensor; try { - geometry_msgs::TransformStamped tf_msg = - robot_state_handle_.lookupTransform("odom", "livox_frame", odom_msg->header.stamp); - tf2::fromMsg(tf_msg.transform, odom2sensor); + slam_updated_ = false; + const auto& slam = slam_rt_buffer_.readFromRT(); + T_lidar_odom2lidar_base_.setOrigin( + tf2::Vector3(slam->pose.pose.position.x, slam->pose.pose.position.y, slam->pose.pose.position.z)); + T_lidar_odom2lidar_base_.setRotation( + tf2::Quaternion(slam->pose.pose.orientation.x, slam->pose.pose.orientation.y, slam->pose.pose.orientation.z, + slam->pose.pose.orientation.w)); + + robot_base2lidar_base_ = + robot_state_handle_.lookupTransform(robot_base_frame_id_, lidar_base_frame_id_, ros::Time(0)); + T_robot_base2lidar_base_.setOrigin(tf2::Vector3(robot_base2lidar_base_.transform.translation.x, + robot_base2lidar_base_.transform.translation.y, + robot_base2lidar_base_.transform.translation.z)); + T_robot_base2lidar_base_.setRotation( + tf2::Quaternion(robot_base2lidar_base_.transform.rotation.x, robot_base2lidar_base_.transform.rotation.y, + robot_base2lidar_base_.transform.rotation.z, robot_base2lidar_base_.transform.rotation.w)); + + T_robot_odom_2robot_base_.setOrigin(tf2::Vector3(robot_odom2robot_base_.transform.translation.x, + robot_odom2robot_base_.transform.translation.y, + robot_odom2robot_base_.transform.translation.z)); + T_robot_odom_2robot_base_.setRotation( + tf2::Quaternion(robot_odom2robot_base_.transform.rotation.x, robot_odom2robot_base_.transform.rotation.y, + robot_odom2robot_base_.transform.rotation.z, robot_odom2robot_base_.transform.rotation.w)); + + T_global_map2robot_odom_ = T_global_map2lidar_odom_ * T_lidar_odom2lidar_base_ * + T_robot_base2lidar_base_.inverse() * T_robot_odom_2robot_base_.inverse(); + + global_map2robot_odom_.transform = tf2::toMsg(T_global_map2robot_odom_); } - catch (tf2::TransformException& ex) + catch (...) { - ROS_WARN("%s", ex.what()); - return; + ROS_WARN("Failed to update global_map2robot_odom."); } - world2odom_ = world2sensor * odom2sensor.inverse(); } - tf2::Transform base2sensor; + global_map2robot_odom_.header.stamp = time; + } + + if (publish_odom_tf_) + { try { - geometry_msgs::TransformStamped tf_msg = - robot_state_handle_.lookupTransform("base_link", "livox_frame", odom_msg->header.stamp); - tf2::fromMsg(tf_msg.transform, base2sensor); + robot_odom2robot_base_ = + robot_state_handle_.lookupTransform(robot_odom_frame_id_, robot_base_frame_id_, ros::Time(0)); + robot_odom2robot_base_.header.stamp = time; + geometry_msgs::Twist vel_base = odometry(); // on base_link frame + geometry_msgs::Vector3 linear_vel_odom, angular_vel_odom; + tf2::doTransform(vel_base.linear, linear_vel_odom, robot_odom2robot_base_); + tf2::doTransform(vel_base.angular, angular_vel_odom, robot_odom2robot_base_); + + double length = + std::sqrt(std::pow(linear_vel_odom.x, 2) + std::pow(linear_vel_odom.y, 2) + std::pow(linear_vel_odom.z, 2)); + if (length < max_odom_vel_) + { // avoid nan vel + robot_odom2robot_base_.transform.translation.x += linear_vel_odom.x * period.toSec(); + robot_odom2robot_base_.transform.translation.y += linear_vel_odom.y * period.toSec(); + robot_odom2robot_base_.transform.translation.z += linear_vel_odom.z * period.toSec(); + } + length = std::sqrt(std::pow(angular_vel_odom.x, 2) + std::pow(angular_vel_odom.y, 2) + + std::pow(angular_vel_odom.z, 2)); + if (length > 0.001) + { // avoid nan quat + tf2::Quaternion odom2base_quat, trans_quat; + tf2::fromMsg(robot_odom2robot_base_.transform.rotation, odom2base_quat); + trans_quat.setRotation(tf2::Vector3(angular_vel_odom.x / length, angular_vel_odom.y / length, + angular_vel_odom.z / length), + length * period.toSec()); + odom2base_quat = trans_quat * odom2base_quat; + odom2base_quat.normalize(); + robot_odom2robot_base_.transform.rotation = tf2::toMsg(odom2base_quat); + } + robot_state_handle_.setTransform(robot_odom2robot_base_, "rm_chassis_controllers"); + + odometry_rt_pub_->msg_.header.stamp = time; + odometry_rt_pub_->msg_.pose.pose.position.x = robot_odom2robot_base_.transform.translation.x; + odometry_rt_pub_->msg_.pose.pose.position.y = robot_odom2robot_base_.transform.translation.y; + odometry_rt_pub_->msg_.pose.pose.orientation.x = robot_odom2robot_base_.transform.rotation.x; + odometry_rt_pub_->msg_.pose.pose.orientation.y = robot_odom2robot_base_.transform.rotation.y; + odometry_rt_pub_->msg_.pose.pose.orientation.z = robot_odom2robot_base_.transform.rotation.z; + odometry_rt_pub_->msg_.pose.pose.orientation.w = robot_odom2robot_base_.transform.rotation.w; + odometry_rt_pub_->msg_.twist.twist.linear.x = linear_vel_odom.x; + odometry_rt_pub_->msg_.twist.twist.linear.y = linear_vel_odom.y; + odometry_rt_pub_->msg_.twist.twist.angular.z = angular_vel_odom.z; } - catch (tf2::TransformException& ex) + catch (...) { - ROS_WARN("%s", ex.what()); - return; + ROS_WARN("Failed to update robot_odom2robot_base."); } - tf2::Transform odom2base = world2odom_.inverse() * world2sensor * base2sensor.inverse(); - odom2base_.transform.translation.x = odom2base.getOrigin().x(); - odom2base_.transform.translation.y = odom2base.getOrigin().y(); - odom2base_.transform.translation.z = odom2base.getOrigin().z(); - topic_update_ = false; } - robot_state_handle_.setTransform(odom2base_, "rm_chassis_controllers"); - if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) { - if (odom_pub_->trylock()) + if (publish_map_tf_) + { + brcst4global_map2robot_odom_.sendTransform(global_map2robot_odom_); + } + + if (publish_odom_tf_) { - odom_pub_->msg_.header.stamp = time; - odom_pub_->msg_.twist.twist.linear.x = vel_base.linear.x; - odom_pub_->msg_.twist.twist.linear.y = vel_base.linear.y; - odom_pub_->msg_.twist.twist.angular.z = vel_base.angular.z; - odom_pub_->unlockAndPublish(); + brcst4robot_odom2robot_base_.sendTransform(robot_odom2robot_base_); + if (odometry_rt_pub_->trylock()) + odometry_rt_pub_->unlockAndPublish(); } - if (enable_odom_tf_ && publish_odom_tf_) - tf_broadcaster_.sendTransform(odom2base_); + last_publish_time_ = time; } } @@ -387,40 +440,29 @@ void ChassisBase::powerLimit() double power_limit = cmd_rt_buffer_.readFromRT()->cmd_chassis_.power_limit; // Three coefficients of a quadratic equation in one variable double a = 0., b = 0., c = 0.; - for (const auto& joint : joint_handles_) + for (const auto& joint : wheel_joint_handles_) { double cmd_effort = joint.getCommand(); double real_vel = joint.getVelocity(); - if (joint.getName().find("wheel") != std::string::npos) // The pivot joint of swerve drive doesn't need power limit - { - a += square(cmd_effort); - b += std::abs(cmd_effort * real_vel); - c += square(real_vel); - } + a += square(cmd_effort); + b += std::abs(cmd_effort * real_vel); + c += square(real_vel); } a *= effort_coeff_; c = c * velocity_coeff_ - power_offset_ - power_limit; // Root formula for quadratic equation in one variable - double zoom_coeff = (square(b) - 4 * a * c) > 0 ? ((-b + sqrt(square(b) - 4 * a * c)) / (2 * a)) : 0.; - for (auto joint : joint_handles_) - if (joint.getName().find("wheel") != std::string::npos) - { - if (pitch_ < pitch_angle_threshold_ && enable_uphill_acceleration_) - { - if (joint.getName().find("back") != std::string::npos) - { - joint.setCommand(zoom_coeff > 1 ? joint.getCommand() : joint.getCommand() * zoom_coeff * scale_); - } - if (joint.getName().find("front") != std::string::npos) - { - joint.setCommand(zoom_coeff > 1 ? joint.getCommand() : joint.getCommand() * zoom_coeff); - } - } - else - { - joint.setCommand(zoom_coeff > 1 ? joint.getCommand() : joint.getCommand() * zoom_coeff); - } - } + double zoom_coeff_tmp = (square(b) - 4 * a * c) > 0 ? ((-b + sqrt(square(b) - 4 * a * c)) / (2 * a)) : 0.; + double zoom_coeff = std::min(zoom_coeff_tmp, 1.0); + for (auto joint : wheel_joint_handles_) + { + joint.setCommand(joint.getCommand() * zoom_coeff); + } + double wheel_power = square(zoom_coeff) * a + zoom_coeff * b + (c + power_limit + power_offset_); + if (wheel_power_pub_->trylock()) + { + wheel_power_pub_->msg_.data = wheel_power; + wheel_power_pub_->unlockAndPublish(); + } } template @@ -452,10 +494,17 @@ void ChassisBase::cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg } template -void ChassisBase::outsideOdomCallback(const nav_msgs::Odometry::ConstPtr& msg) +void ChassisBase::slamCallback(const nav_msgs::Odometry::ConstPtr& msg) +{ + slam_rt_buffer_.writeFromNonRT(*msg); + slam_updated_ = true; +} + +template +void ChassisBase::localizationCallback(const geometry_msgs::TransformStamped::ConstPtr& msg) { - odom_buffer_.writeFromNonRT(*msg); - topic_update_ = true; + localization_rt_buffer_.writeFromNonRT(*msg); + localization_updated_ = true; } -} // namespace rm_chassis_controllers +} // namespace rm_chassis_controllers \ No newline at end of file diff --git a/rm_chassis_controllers/src/omni.cpp b/rm_chassis_controllers/src/omni.cpp index 903ad007..84c27349 100644 --- a/rm_chassis_controllers/src/omni.cpp +++ b/rm_chassis_controllers/src/omni.cpp @@ -44,7 +44,7 @@ bool OmniController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle joints_.push_back(std::make_shared()); if (!joints_.back()->init(effort_joint_interface_, nh_wheel)) return false; - joint_handles_.push_back(joints_[i]->joint_); + wheel_joint_handles_.push_back(joints_[i]->joint_); i++; } diff --git a/rm_chassis_controllers/src/sentry.cpp b/rm_chassis_controllers/src/sentry.cpp index e26ad8d3..ce5e0fb7 100644 --- a/rm_chassis_controllers/src/sentry.cpp +++ b/rm_chassis_controllers/src/sentry.cpp @@ -56,8 +56,8 @@ bool SentryController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand !ctrl_catapult_joint_.init(effort_joint_interface_, nh_brake)) return false; if_catapult_ = false; - joint_handles_.push_back(effort_joint_interface_->getHandle(ctrl_wheel_.getJointName())); - joint_handles_.push_back(effort_joint_interface_->getHandle(ctrl_catapult_joint_.getJointName())); + wheel_joint_handles_.push_back(effort_joint_interface_->getHandle(ctrl_wheel_.getJointName())); + wheel_joint_handles_.push_back(effort_joint_interface_->getHandle(ctrl_catapult_joint_.getJointName())); return true; } diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index 69c21077..c851abbb 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 @@ -73,10 +73,19 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand return false; if (module.second["pivot"].hasMember("offset")) m.pivot_offset_ = module.second["pivot"]["offset"]; - joint_handles_.push_back(m.ctrl_pivot_->joint_); - joint_handles_.push_back(m.ctrl_wheel_->joint_); + pivot_joint_handles_.push_back(m.ctrl_pivot_->joint_); + wheel_joint_handles_.push_back(m.ctrl_wheel_->joint_); modules_.push_back(m); } + // power limit + if (!controller_nh.getParam("power/pivot_max_power", pivot_max_power_) || + !controller_nh.getParam("power/pivot_effort_coeff", pivot_effort_coeff_) || + !controller_nh.getParam("power/pivot_velocity_coeff", pivot_velocity_coeff_)) + { + ROS_ERROR("Some pivot power limit params doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); + return false; + } + return true; } @@ -125,5 +134,56 @@ geometry_msgs::Twist SwerveController::odometry() return vel_data; } +void SwerveController::powerLimit() +{ + double power_limit = cmd_rt_buffer_.readFromRT()->cmd_chassis_.power_limit; + double wheel_power_limit{}; + + // Avoid too much power allocated to pivot joints + double pivot_power_limit = std::min(power_limit / 2.0, pivot_max_power_); + // Three coefficients of a quadratic equation for pivot joints + double a_p{}, b_p{}, c_p = {}; + for (const auto& joint : pivot_joint_handles_) + { + double cmd_effort = joint.getCommand(); + double real_vel = joint.getVelocity(); + a_p += square(cmd_effort); + b_p += cmd_effort * real_vel; + c_p += square(real_vel); + } + a_p *= pivot_effort_coeff_; + c_p = c_p * pivot_velocity_coeff_ - pivot_power_limit; + // Root formula for quadratic equation in one variable + double zoom_pivot = + (square(b_p) - 4.0 * a_p * c_p) > 0.0 ? ((-b_p + sqrt(square(b_p) - 4.0 * a_p * c_p)) / (2.0 * a_p)) : 0.0; + double actual_zoom = zoom_pivot > 1.0 ? 1.0 : zoom_pivot; + double pivot_power = square(actual_zoom) * a_p + actual_zoom * b_p + (c_p + pivot_power_limit); + for (auto joint : pivot_joint_handles_) + { + joint.setCommand(actual_zoom * joint.getCommand()); + } + // Remaining power for wheels + wheel_power_limit = power_limit + power_offset_ - pivot_power; + + // Three coefficients of a quadratic equation for wheel joints + double a_w{}, b_w{}, c_w{}; + for (const auto& joint : wheel_joint_handles_) + { + double cmd_effort = joint.getCommand(); + double real_vel = joint.getVelocity(); + a_w += square(cmd_effort); + b_w += cmd_effort * real_vel; + c_w += square(real_vel); + } + a_w *= effort_coeff_; + c_w = c_w * velocity_coeff_ - wheel_power_limit; + // Root formula for quadratic equation in one variable + double zoom_coeff = (square(b_w) - 4 * a_w * c_w) > 0 ? ((-b_w + sqrt(square(b_w) - 4 * a_w * c_w)) / (2 * a_w)) : 0.; + for (auto joint : wheel_joint_handles_) + { + joint.setCommand(zoom_coeff > 1 ? joint.getCommand() : joint.getCommand() * zoom_coeff); + } +} + PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::SwerveController, controller_interface::ControllerBase) } // namespace rm_chassis_controllers From f559878a99473e1b08041cab2d5850fb06f98a8a Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sat, 7 Mar 2026 13:37:22 +0000 Subject: [PATCH 02/36] Fix bugs. --- rm_chassis_controllers/CMakeLists.txt | 2 +- rm_chassis_controllers/cfg/PowerLimit.cfg | 6 +- .../rm_chassis_controllers/chassis_base.h | 15 +---- .../include/rm_chassis_controllers/swerve.h | 3 +- rm_chassis_controllers/src/chassis_base.cpp | 34 +++++++---- rm_chassis_controllers/src/swerve.cpp | 59 ++++++++++++------- 6 files changed, 70 insertions(+), 49 deletions(-) diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt index 9476246b..babf8d00 100644 --- a/rm_chassis_controllers/CMakeLists.txt +++ b/rm_chassis_controllers/CMakeLists.txt @@ -27,7 +27,7 @@ find_package(catkin REQUIRED find_package(Eigen3 REQUIRED) generate_dynamic_reconfigure_options( - cfg/PowerLimit.cfg + cfg/PowerLimit.cfg ) ################################### diff --git a/rm_chassis_controllers/cfg/PowerLimit.cfg b/rm_chassis_controllers/cfg/PowerLimit.cfg index 58e474de..4a125ccd 100644 --- a/rm_chassis_controllers/cfg/PowerLimit.cfg +++ b/rm_chassis_controllers/cfg/PowerLimit.cfg @@ -5,11 +5,11 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("velocity_coeff", double_t, 0, "velocity coefficient for power limit", 0.0, 0.0, 20.0) +gen.add("vel_coeff", double_t, 0, "velocity coefficient for power limit", 0.001, 0.000, 0.100) 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) gen.add("pivot_max_power", double_t, 0, "maximum power for pivot joints", 0.0, 0.0, 100.0) gen.add("pivot_effort_coeff", double_t, 0, "effort coefficient for pivot power limit", 0.0, 0.0, 20.0) -gen.add("pivot_velocity_coeff", double_t, 0, "velocity coefficient for pivot power limit", 0.0, 0.0, 20.0) +gen.add("pivot_vel_coeff", double_t, 0, "velocity coefficient for pivot power limit", 0.001, 0.000, 0.100) -exit(gen.generate(PACKAGE, "chassis_controller", "PowerLimit")) \ No newline at end of file +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 8ada3443..aa397912 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -69,12 +69,6 @@ struct Command ros::Time stamp_; }; -struct PowerLimitParams -{ - double velocity_coeff{}; - double effort_coeff{}; - double power_offset{}; -}; template class ChassisBase : public controller_interface::MultiInterfaceController { @@ -162,6 +156,7 @@ 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 powerLimitReconfigCB(rm_chassis_controllers::PowerLimitConfig& config, uint32_t level); rm_control::RobotStateHandle robot_state_handle_{}; hardware_interface::EffortJointInterface* effort_joint_interface_{}; @@ -169,7 +164,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController realtime_tools::RealtimeBuffer cmd_rt_buffer_{}; realtime_tools::RealtimeBuffer slam_rt_buffer_{}; realtime_tools::RealtimeBuffer localization_rt_buffer_{}; - realtime_tools::RealtimeBuffer power_limit_rt_buffer_{}; + realtime_tools::RealtimeBuffer power_limit_rt_buffer_; std::unique_ptr> odometry_rt_pub_; std::unique_ptr> wheel_power_pub_; dynamic_reconfigure::Server* power_limit_srv_{}; @@ -200,11 +195,6 @@ class ChassisBase : public controller_interface::MultiInterfaceController bool publish_map_tf_{ false }; bool publish_odom_tf_{ false }; - double velocity_coeff_{ 0.0 }; - double effort_coeff_{ 0.0 }; - double power_offset_{ 0.0 }; - bool power_limit_reconfig_initialized_{ false }; - double wheel_radius_{ 0.02 }; double twist_angular_{ M_PI / 6 }; double max_odom_vel_{ 10.0 }; @@ -230,7 +220,6 @@ class ChassisBase : public controller_interface::MultiInterfaceController control_toolbox::Pid pid_follow_{}; Command cmd_struct_{}; - PowerLimitParams power_config_{}; enum { diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h index 5165ff34..36a181aa 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h @@ -62,9 +62,10 @@ class SwerveController : public ChassisBase modules_{}; std::vector pivot_joint_handles_{}; - double pivot_max_power_{},pivot_effort_coeff_{},pivot_velocity_coeff_{}; + std::unique_ptr> pivot_power_pub_; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index d763c6b5..219098dd 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -53,10 +53,6 @@ void ChassisBase::initialize_parameters(ros::NodeHandle& controller_nh) controller_nh.getParam("slam_topic", slam_topic_); controller_nh.getParam("localization_topic", localization_topic_); - controller_nh.getParam("power/vel_coeff", velocity_coeff_); - controller_nh.getParam("power/effort_coeff", effort_coeff_); - controller_nh.getParam("power/power_offset", power_offset_); - controller_nh.getParam("wheel_radius", wheel_radius_); controller_nh.getParam("twist_angular", twist_angular_); controller_nh.getParam("max_odom_vel", max_odom_vel_); @@ -85,9 +81,14 @@ bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan localization_sub_ = controller_nh.subscribe( localization_topic_, 10, &ChassisBase::localizationCallback, this); - // power publisher + // power publisher and dynamic reconfigure + power_limit_srv_ = new dynamic_reconfigure::Server( + ros::NodeHandle(controller_nh, "power")); + dynamic_reconfigure::Server::CallbackType cb = + boost::bind(&ChassisBase::powerLimitReconfigCB, this, _1, _2); + power_limit_srv_->setCallback(cb); auto power_publisher = - std::make_unique>(controller_nh, "wheel_power", 100); + std::make_unique>(controller_nh, "power/wheel_power", 100); this->wheel_power_pub_ = std::move(power_publisher); // Setup odometry realtime publisher + odom message constant fields @@ -439,6 +440,12 @@ void ChassisBase::powerLimit() { double power_limit = cmd_rt_buffer_.readFromRT()->cmd_chassis_.power_limit; // Three coefficients of a quadratic equation in one variable + const auto& power_config = *power_limit_rt_buffer_.readFromRT(); + + double vel_coeff = power_config.vel_coeff; + double effort_coeff = power_config.effort_coeff; + double power_offset = power_config.power_offset; + double a = 0., b = 0., c = 0.; for (const auto& joint : wheel_joint_handles_) { @@ -448,8 +455,8 @@ void ChassisBase::powerLimit() b += std::abs(cmd_effort * real_vel); c += square(real_vel); } - a *= effort_coeff_; - c = c * velocity_coeff_ - power_offset_ - power_limit; + a *= effort_coeff; + c = c * vel_coeff - power_offset - power_limit; // Root formula for quadratic equation in one variable double zoom_coeff_tmp = (square(b) - 4 * a * c) > 0 ? ((-b + sqrt(square(b) - 4 * a * c)) / (2 * a)) : 0.; double zoom_coeff = std::min(zoom_coeff_tmp, 1.0); @@ -457,7 +464,7 @@ void ChassisBase::powerLimit() { joint.setCommand(joint.getCommand() * zoom_coeff); } - double wheel_power = square(zoom_coeff) * a + zoom_coeff * b + (c + power_limit + power_offset_); + double wheel_power = square(zoom_coeff) * a + zoom_coeff * b + (c + power_limit + power_offset); if (wheel_power_pub_->trylock()) { wheel_power_pub_->msg_.data = wheel_power; @@ -507,4 +514,11 @@ void ChassisBase::localizationCallback(const geometry_msgs::TransformStamp localization_updated_ = true; } -} // namespace rm_chassis_controllers \ No newline at end of file +template +void ChassisBase::powerLimitReconfigCB(rm_chassis_controllers::PowerLimitConfig& config, uint32_t /*level*/) +{ + ROS_INFO("[Power Limit] Dynamic params change"); + power_limit_rt_buffer_.writeFromNonRT(config); +} + +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index c851abbb..af349640 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -47,6 +47,11 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand { if (!ChassisBase::init(robot_hw, root_nh, controller_nh)) return false; + + auto pivot_power_publisher = + std::make_unique>(controller_nh, "power/pivot_power", 100); + this->pivot_power_pub_ = std::move(pivot_power_publisher); + XmlRpc::XmlRpcValue modules; controller_nh.getParam("modules", modules); ROS_ASSERT(modules.getType() == XmlRpc::XmlRpcValue::TypeStruct); @@ -77,14 +82,6 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand wheel_joint_handles_.push_back(m.ctrl_wheel_->joint_); modules_.push_back(m); } - // power limit - if (!controller_nh.getParam("power/pivot_max_power", pivot_max_power_) || - !controller_nh.getParam("power/pivot_effort_coeff", pivot_effort_coeff_) || - !controller_nh.getParam("power/pivot_velocity_coeff", pivot_velocity_coeff_)) - { - ROS_ERROR("Some pivot power limit params doesn't given (namespace: %s)", controller_nh.getNamespace().c_str()); - return false; - } return true; } @@ -137,10 +134,17 @@ geometry_msgs::Twist SwerveController::odometry() void SwerveController::powerLimit() { double power_limit = cmd_rt_buffer_.readFromRT()->cmd_chassis_.power_limit; - double wheel_power_limit{}; + + const auto& power_config = *power_limit_rt_buffer_.readFromRT(); + double vel_coeff = power_config.vel_coeff; + double effort_coeff = power_config.effort_coeff; + double power_offset = power_config.power_offset; + double pivot_max_power = power_config.pivot_max_power; + double pivot_effort_coeff = power_config.pivot_effort_coeff; + double pivot_vel_coeff = power_config.pivot_vel_coeff; // Avoid too much power allocated to pivot joints - double pivot_power_limit = std::min(power_limit / 2.0, pivot_max_power_); + double pivot_power_limit = std::min(power_limit / 2.0, pivot_max_power); // Three coefficients of a quadratic equation for pivot joints double a_p{}, b_p{}, c_p = {}; for (const auto& joint : pivot_joint_handles_) @@ -151,19 +155,19 @@ void SwerveController::powerLimit() b_p += cmd_effort * real_vel; c_p += square(real_vel); } - a_p *= pivot_effort_coeff_; - c_p = c_p * pivot_velocity_coeff_ - pivot_power_limit; + a_p *= pivot_effort_coeff; + c_p = c_p * pivot_vel_coeff - pivot_power_limit; // Root formula for quadratic equation in one variable - double zoom_pivot = + double zoom_pivot_tmp = (square(b_p) - 4.0 * a_p * c_p) > 0.0 ? ((-b_p + sqrt(square(b_p) - 4.0 * a_p * c_p)) / (2.0 * a_p)) : 0.0; - double actual_zoom = zoom_pivot > 1.0 ? 1.0 : zoom_pivot; - double pivot_power = square(actual_zoom) * a_p + actual_zoom * b_p + (c_p + pivot_power_limit); + double zoom_pivot = std::min(zoom_pivot_tmp, 1.0); + double pivot_power = square(zoom_pivot) * a_p + zoom_pivot * b_p + (c_p + pivot_power_limit); for (auto joint : pivot_joint_handles_) { - joint.setCommand(actual_zoom * joint.getCommand()); + joint.setCommand(zoom_pivot * joint.getCommand()); } // Remaining power for wheels - wheel_power_limit = power_limit + power_offset_ - pivot_power; + double wheel_power_limit = power_limit + power_offset - pivot_power; // Three coefficients of a quadratic equation for wheel joints double a_w{}, b_w{}, c_w{}; @@ -175,13 +179,26 @@ void SwerveController::powerLimit() b_w += cmd_effort * real_vel; c_w += square(real_vel); } - a_w *= effort_coeff_; - c_w = c_w * velocity_coeff_ - wheel_power_limit; + a_w *= effort_coeff; + c_w = c_w * vel_coeff - wheel_power_limit; // Root formula for quadratic equation in one variable - double zoom_coeff = (square(b_w) - 4 * a_w * c_w) > 0 ? ((-b_w + sqrt(square(b_w) - 4 * a_w * c_w)) / (2 * a_w)) : 0.; + double zoom_wheel_tmp = + (square(b_w) - 4 * a_w * c_w) > 0 ? ((-b_w + sqrt(square(b_w) - 4 * a_w * c_w)) / (2 * a_w)) : 0.; + double zoom_wheel = std::min(zoom_wheel_tmp, 1.0); + double wheel_power = square(zoom_wheel) * a_w + zoom_wheel * b_w + (c_w + wheel_power_limit); for (auto joint : wheel_joint_handles_) { - joint.setCommand(zoom_coeff > 1 ? joint.getCommand() : joint.getCommand() * zoom_coeff); + joint.setCommand(joint.getCommand() * zoom_wheel); + } + if (wheel_power_pub_->trylock()) + { + wheel_power_pub_->msg_.data = wheel_power; + wheel_power_pub_->unlockAndPublish(); + } + if (pivot_power_pub_->trylock()) + { + pivot_power_pub_->msg_.data = pivot_power; + pivot_power_pub_->unlockAndPublish(); } } From 56e06c01df460b3fb67f61de9e522a6e96f1b5a7 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sat, 7 Mar 2026 14:08:23 +0000 Subject: [PATCH 03/36] Fix bugs. --- rm_chassis_controllers/cfg/PowerLimit.cfg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_chassis_controllers/cfg/PowerLimit.cfg b/rm_chassis_controllers/cfg/PowerLimit.cfg index 4a125ccd..92efa3e3 100644 --- a/rm_chassis_controllers/cfg/PowerLimit.cfg +++ b/rm_chassis_controllers/cfg/PowerLimit.cfg @@ -5,11 +5,11 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("vel_coeff", double_t, 0, "velocity coefficient for power limit", 0.001, 0.000, 0.100) +gen.add("vel_coeff", double_t, 0, "velocity coefficient for power limit", 0.000, 0.000, 0.100) 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) gen.add("pivot_max_power", double_t, 0, "maximum power for pivot joints", 0.0, 0.0, 100.0) gen.add("pivot_effort_coeff", double_t, 0, "effort coefficient for pivot power limit", 0.0, 0.0, 20.0) -gen.add("pivot_vel_coeff", double_t, 0, "velocity coefficient for pivot power limit", 0.001, 0.000, 0.100) +gen.add("pivot_vel_coeff", double_t, 0, "velocity coefficient for pivot power limit", 0.000, 0.000, 0.100) exit(gen.generate(PACKAGE, "chassis_controller", "PowerLimit")) From 804ca87b6e60240c5499a04d8e5b49277ce41348 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Tue, 10 Mar 2026 16:07:24 +0000 Subject: [PATCH 04/36] Update powerlimit. --- rm_chassis_controllers/cfg/PowerLimit.cfg | 1 + rm_chassis_controllers/src/swerve.cpp | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/rm_chassis_controllers/cfg/PowerLimit.cfg b/rm_chassis_controllers/cfg/PowerLimit.cfg index 92efa3e3..f770abb1 100644 --- a/rm_chassis_controllers/cfg/PowerLimit.cfg +++ b/rm_chassis_controllers/cfg/PowerLimit.cfg @@ -11,5 +11,6 @@ gen.add("power_offset", double_t, 0, "power offset for power limit", 0.0, -50.0, gen.add("pivot_max_power", double_t, 0, "maximum power for pivot joints", 0.0, 0.0, 100.0) gen.add("pivot_effort_coeff", double_t, 0, "effort coefficient for pivot power limit", 0.0, 0.0, 20.0) gen.add("pivot_vel_coeff", double_t, 0, "velocity coefficient for pivot power limit", 0.000, 0.000, 0.100) +gen.add("pivot_power_ratio", double_t, 0, "power ratio for pivot joints", 0.0, 0.0, 1.0) exit(gen.generate(PACKAGE, "chassis_controller", "PowerLimit")) diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index af349640..7ea36c4a 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -142,9 +142,10 @@ void SwerveController::powerLimit() double pivot_max_power = power_config.pivot_max_power; double pivot_effort_coeff = power_config.pivot_effort_coeff; double pivot_vel_coeff = power_config.pivot_vel_coeff; + double pivot_power_ratio = power_config.pivot_power_ratio; // Avoid too much power allocated to pivot joints - double pivot_power_limit = std::min(power_limit / 2.0, pivot_max_power); + double pivot_power_limit = std::min(power_limit * pivot_power_ratio, pivot_max_power); // Three coefficients of a quadratic equation for pivot joints double a_p{}, b_p{}, c_p = {}; for (const auto& joint : pivot_joint_handles_) From 4e67565ca13c9faab5b59ae97baa1fefd0f52cb5 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sun, 15 Mar 2026 04:22:15 +0000 Subject: [PATCH 05/36] Fix. --- rm_chassis_controllers/cfg/PowerLimit.cfg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_chassis_controllers/cfg/PowerLimit.cfg b/rm_chassis_controllers/cfg/PowerLimit.cfg index f770abb1..7ad87891 100644 --- a/rm_chassis_controllers/cfg/PowerLimit.cfg +++ b/rm_chassis_controllers/cfg/PowerLimit.cfg @@ -9,8 +9,8 @@ gen.add("vel_coeff", double_t, 0, "velocity coefficient for power limit", 0.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) gen.add("pivot_max_power", double_t, 0, "maximum power for pivot joints", 0.0, 0.0, 100.0) -gen.add("pivot_effort_coeff", double_t, 0, "effort coefficient for pivot power limit", 0.0, 0.0, 20.0) -gen.add("pivot_vel_coeff", double_t, 0, "velocity coefficient for pivot power limit", 0.000, 0.000, 0.100) +gen.add("pivot_effort_coeff", double_t, 0, "effort coefficient for pivot power limit", 0.0, 0.0, 100) +gen.add("pivot_vel_coeff", double_t, 0, "velocity coefficient for pivot power limit", 0.000, 0.000, 10) gen.add("pivot_power_ratio", double_t, 0, "power ratio for pivot joints", 0.0, 0.0, 1.0) exit(gen.generate(PACKAGE, "chassis_controller", "PowerLimit")) From 2e22dd32d92efdce2fc86c432cc38ecb8146dd9d Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Fri, 27 Mar 2026 15:11:05 +0000 Subject: [PATCH 06/36] Fix bugs. --- rm_chassis_controllers/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_chassis_controllers/package.xml b/rm_chassis_controllers/package.xml index 8ae776a5..ca9540ac 100644 --- a/rm_chassis_controllers/package.xml +++ b/rm_chassis_controllers/package.xml @@ -21,7 +21,7 @@ tf2_geometry_msgs nav_msgs angles - dynamic_reconfigure` + dynamic_reconfigure robot_localization From d69ebb04f545bf8e6991ce5538a3446797d1912c Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Tue, 31 Mar 2026 14:00:19 +0000 Subject: [PATCH 07/36] Add swerve base gyro. --- .../rm_chassis_controllers/chassis_base.h | 26 ++- .../include/rm_chassis_controllers/swerve.h | 13 +- rm_chassis_controllers/src/chassis_base.cpp | 41 +---- rm_chassis_controllers/src/swerve.cpp | 151 ++++++++++++------ 4 files changed, 142 insertions(+), 89 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 aa397912..f07babf7 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -59,6 +59,7 @@ #include #include #include +#include namespace rm_chassis_controllers { @@ -69,6 +70,24 @@ struct Command ros::Time stamp_; }; +struct PowerLimitor +{ + double vel_coeff{}; + double effort_coeff{}; + double power_offset{}; + double max_power{}; + double power_sum{}; + double ratio{ 1.0 }; + double Err_upper{}; + double Err_lower{}; + double Err_sum{}; + double A{}; + double B{}; + double C{}; + double Delta{}; + double K{}; +}; + template class ChassisBase : public controller_interface::MultiInterfaceController { @@ -142,6 +161,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController * Receive power limit from command. Set max_effort command to chassis to avoid exceed power limit. */ virtual void powerLimit(); + virtual void updatePowerStatus(); /** @brief Write current command from rm_msgs::ChassisCmd. * * @param msg This message contains various state parameter settings for basic chassis control @@ -166,7 +186,8 @@ class ChassisBase : public controller_interface::MultiInterfaceController realtime_tools::RealtimeBuffer localization_rt_buffer_{}; realtime_tools::RealtimeBuffer power_limit_rt_buffer_; std::unique_ptr> odometry_rt_pub_; - std::unique_ptr> wheel_power_pub_; + std::unique_ptr> ewheel_power_pub_; + std::unique_ptr> cwheel_power_pub_; dynamic_reconfigure::Server* power_limit_srv_{}; rm_common::TfRtBroadcaster brcst4global_map2robot_odom_{}; @@ -191,6 +212,8 @@ class ChassisBase : public controller_interface::MultiInterfaceController std::unique_ptr> ramp_y_{ nullptr }; std::unique_ptr> ramp_w_{ nullptr }; + double roll_{ 0. }, pitch_{ 0. }, yaw_{ 0. }; + double publish_rate_{ 100.0 }; bool publish_map_tf_{ false }; bool publish_odom_tf_{ false }; @@ -220,6 +243,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController control_toolbox::Pid pid_follow_{}; Command cmd_struct_{}; + PowerLimitor wheel_power_limitor_{}; enum { diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h index 36a181aa..c4ad240e 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h @@ -39,6 +39,8 @@ #include "rm_chassis_controllers/chassis_base.h" +#include +#include #include #include @@ -62,10 +64,19 @@ class SwerveController : public ChassisBase modules_{}; std::vector pivot_joint_handles_{}; - std::unique_ptr> pivot_power_pub_; + std::unique_ptr> epivot_power_pub_; + std::unique_ptr> cpivot_power_pub_; + + PowerLimitor pivot_power_limitor_{}; + + bool has_gimbal_imu_{ false }; + hardware_interface::ImuSensorHandle imu_handle_{}; + geometry_msgs::Vector3 base_gyro_{}; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 219098dd..c8ba5013 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -87,9 +87,6 @@ bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan dynamic_reconfigure::Server::CallbackType cb = boost::bind(&ChassisBase::powerLimitReconfigCB, this, _1, _2); power_limit_srv_->setCallback(cb); - auto power_publisher = - std::make_unique>(controller_nh, "power/wheel_power", 100); - this->wheel_power_pub_ = std::move(power_publisher); // Setup odometry realtime publisher + odom message constant fields auto odometry_publisher = @@ -390,6 +387,9 @@ void ChassisBase::updateOdom(const ros::Time& time, const ros::Duration& p odom2base_quat.normalize(); robot_odom2robot_base_.transform.rotation = tf2::toMsg(odom2base_quat); } + + quatToRPY(robot_odom2robot_base_.transform.rotation, roll_, pitch_, yaw_); + robot_state_handle_.setTransform(robot_odom2robot_base_, "rm_chassis_controllers"); odometry_rt_pub_->msg_.header.stamp = time; @@ -438,38 +438,11 @@ void ChassisBase::recovery() template void ChassisBase::powerLimit() { - double power_limit = cmd_rt_buffer_.readFromRT()->cmd_chassis_.power_limit; - // Three coefficients of a quadratic equation in one variable - const auto& power_config = *power_limit_rt_buffer_.readFromRT(); - - double vel_coeff = power_config.vel_coeff; - double effort_coeff = power_config.effort_coeff; - double power_offset = power_config.power_offset; +} - double a = 0., b = 0., c = 0.; - for (const auto& joint : wheel_joint_handles_) - { - double cmd_effort = joint.getCommand(); - double real_vel = joint.getVelocity(); - a += square(cmd_effort); - b += std::abs(cmd_effort * real_vel); - c += square(real_vel); - } - a *= effort_coeff; - c = c * vel_coeff - power_offset - power_limit; - // Root formula for quadratic equation in one variable - double zoom_coeff_tmp = (square(b) - 4 * a * c) > 0 ? ((-b + sqrt(square(b) - 4 * a * c)) / (2 * a)) : 0.; - double zoom_coeff = std::min(zoom_coeff_tmp, 1.0); - for (auto joint : wheel_joint_handles_) - { - joint.setCommand(joint.getCommand() * zoom_coeff); - } - double wheel_power = square(zoom_coeff) * a + zoom_coeff * b + (c + power_limit + power_offset); - if (wheel_power_pub_->trylock()) - { - wheel_power_pub_->msg_.data = wheel_power; - wheel_power_pub_->unlockAndPublish(); - } +template +void ChassisBase::updatePowerStatus() +{ } template diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index 7ea36c4a..1e150f96 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -48,9 +48,29 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand if (!ChassisBase::init(robot_hw, root_nh, controller_nh)) return false; - auto pivot_power_publisher = + auto* imu_handle = robot_hw->get(); + if (imu_handle == nullptr) + { + ROS_WARN("SwerveController requires gimbal_imu"); + } + else + { + imu_handle_ = imu_handle->getHandle("gimbal_imu"); + has_gimbal_imu_ = true; + } + + auto ewheel_power_publisher = + std::make_unique>(controller_nh, "power/ewheel_power", 100); + this->ewheel_power_pub_ = std::move(ewheel_power_publisher); + auto cwheel_power_publisher = + std::make_unique>(controller_nh, "power/cwheel_power", 100); + this->cwheel_power_pub_ = std::move(cwheel_power_publisher); + auto epivot_power_publisher = std::make_unique>(controller_nh, "power/pivot_power", 100); - this->pivot_power_pub_ = std::move(pivot_power_publisher); + this->epivot_power_pub_ = std::move(epivot_power_publisher); + auto cpivot_power_publisher = + std::make_unique>(controller_nh, "power/cpivot_power", 100); + this->cpivot_power_pub_ = std::move(cpivot_power_publisher); XmlRpc::XmlRpcValue modules; controller_nh.getParam("modules", modules); @@ -90,6 +110,7 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand void SwerveController::moveJoint(const ros::Time& time, const ros::Duration& period) { + // getBaseGyro(); Vec2 vel_center(vel_cmd_.x, vel_cmd_.y); for (auto& module : modules_) { @@ -131,75 +152,99 @@ geometry_msgs::Twist SwerveController::odometry() return vel_data; } +void SwerveController::getBaseGyro() +{ + // Todo: because swerve don't have chassis imu, we use gimbal imu to get chassis gyro. This is not good because of the + // gimbal's rotation. A better solution is to install an imu on the chassis. + if (has_gimbal_imu_) + { + geometry_msgs::Vector3 imu_gyro{}; + imu_gyro.x = imu_handle_.getAngularVelocity()[0]; + imu_gyro.y = imu_handle_.getAngularVelocity()[1]; + imu_gyro.z = imu_handle_.getAngularVelocity()[2]; + try + { + tf2::doTransform(imu_gyro, base_gyro_, + robot_state_handle_.lookupTransform("base_link", imu_handle_.getFrameId(), ros::Time(0))); + } + catch (tf2::TransformException& ex) + { + ROS_WARN_THROTTLE(1.0, "%s", ex.what()); + } + } +} + void SwerveController::powerLimit() { - double power_limit = cmd_rt_buffer_.readFromRT()->cmd_chassis_.power_limit; + updatePowerStatus(); + auto power_group = [](std::vector& joints, const PowerLimitor& limitor) { + + }; + + power_group(pivot_joint_handles_, pivot_power_limitor_); + power_group(wheel_joint_handles_, wheel_power_limitor_); +} + +void SwerveController::updatePowerStatus() +{ + double power_limit = cmd_rt_buffer_.readFromRT()->cmd_chassis_.power_limit; const auto& power_config = *power_limit_rt_buffer_.readFromRT(); - double vel_coeff = power_config.vel_coeff; - double effort_coeff = power_config.effort_coeff; - double power_offset = power_config.power_offset; - double pivot_max_power = power_config.pivot_max_power; - double pivot_effort_coeff = power_config.pivot_effort_coeff; - double pivot_vel_coeff = power_config.pivot_vel_coeff; - double pivot_power_ratio = power_config.pivot_power_ratio; - - // Avoid too much power allocated to pivot joints - double pivot_power_limit = std::min(power_limit * pivot_power_ratio, pivot_max_power); - // Three coefficients of a quadratic equation for pivot joints - double a_p{}, b_p{}, c_p = {}; + pivot_power_limitor_ = { .vel_coeff = power_config.pivot_vel_coeff, + .effort_coeff = power_config.pivot_effort_coeff, + .power_offset = 0, + .max_power = + std::min(power_config.pivot_max_power, power_limit * power_config.pivot_power_ratio), + .ratio = power_config.pivot_power_ratio }; + double epivot_power{}, cpivot_power{}; for (const auto& joint : pivot_joint_handles_) { - double cmd_effort = joint.getCommand(); + double cmd_torque = joint.getCommand(); double real_vel = joint.getVelocity(); - a_p += square(cmd_effort); - b_p += cmd_effort * real_vel; - c_p += square(real_vel); + double real_torque = joint.getEffort(); + epivot_power += real_torque * real_vel / 9.55f + pivot_power_limitor_.effort_coeff * square(real_torque) + + pivot_power_limitor_.vel_coeff * square(real_vel); + cpivot_power += cmd_torque * real_vel / 9.55f + pivot_power_limitor_.effort_coeff * square(cmd_torque) + + pivot_power_limitor_.vel_coeff * square(real_vel); } - a_p *= pivot_effort_coeff; - c_p = c_p * pivot_vel_coeff - pivot_power_limit; - // Root formula for quadratic equation in one variable - double zoom_pivot_tmp = - (square(b_p) - 4.0 * a_p * c_p) > 0.0 ? ((-b_p + sqrt(square(b_p) - 4.0 * a_p * c_p)) / (2.0 * a_p)) : 0.0; - double zoom_pivot = std::min(zoom_pivot_tmp, 1.0); - double pivot_power = square(zoom_pivot) * a_p + zoom_pivot * b_p + (c_p + pivot_power_limit); - for (auto joint : pivot_joint_handles_) - { - joint.setCommand(zoom_pivot * joint.getCommand()); - } - // Remaining power for wheels - double wheel_power_limit = power_limit + power_offset - pivot_power; + pivot_power_limitor_.power_sum = cpivot_power; - // Three coefficients of a quadratic equation for wheel joints - double a_w{}, b_w{}, c_w{}; + wheel_power_limitor_ = { .vel_coeff = power_config.vel_coeff, + .effort_coeff = power_config.effort_coeff, + .power_offset = power_config.power_offset, + .max_power = power_limit - std::min(cpivot_power, pivot_power_limitor_.max_power) }; + double ewheel_power{}, cwheel_power{}; for (const auto& joint : wheel_joint_handles_) { - double cmd_effort = joint.getCommand(); + double cmd_torque = joint.getCommand(); double real_vel = joint.getVelocity(); - a_w += square(cmd_effort); - b_w += cmd_effort * real_vel; - c_w += square(real_vel); + double real_torque = joint.getEffort(); + ewheel_power += real_torque * real_vel / 9.55f + wheel_power_limitor_.effort_coeff * square(real_torque) + + wheel_power_limitor_.vel_coeff * square(real_vel); + cwheel_power += cmd_torque * real_vel / 9.55f + wheel_power_limitor_.effort_coeff * square(cmd_torque) + + wheel_power_limitor_.vel_coeff * square(real_vel); + } + wheel_power_limitor_.power_sum = cwheel_power; + + if (epivot_power_pub_->trylock()) + { + epivot_power_pub_->msg_.data = epivot_power; + epivot_power_pub_->unlockAndPublish(); } - a_w *= effort_coeff; - c_w = c_w * vel_coeff - wheel_power_limit; - // Root formula for quadratic equation in one variable - double zoom_wheel_tmp = - (square(b_w) - 4 * a_w * c_w) > 0 ? ((-b_w + sqrt(square(b_w) - 4 * a_w * c_w)) / (2 * a_w)) : 0.; - double zoom_wheel = std::min(zoom_wheel_tmp, 1.0); - double wheel_power = square(zoom_wheel) * a_w + zoom_wheel * b_w + (c_w + wheel_power_limit); - for (auto joint : wheel_joint_handles_) + if (cpivot_power_pub_->trylock()) { - joint.setCommand(joint.getCommand() * zoom_wheel); + cpivot_power_pub_->msg_.data = cpivot_power; + cpivot_power_pub_->unlockAndPublish(); } - if (wheel_power_pub_->trylock()) + if (ewheel_power_pub_->trylock()) { - wheel_power_pub_->msg_.data = wheel_power; - wheel_power_pub_->unlockAndPublish(); + ewheel_power_pub_->msg_.data = ewheel_power; + ewheel_power_pub_->unlockAndPublish(); } - if (pivot_power_pub_->trylock()) + if (cwheel_power_pub_->trylock()) { - pivot_power_pub_->msg_.data = pivot_power; - pivot_power_pub_->unlockAndPublish(); + cwheel_power_pub_->msg_.data = cwheel_power; + cwheel_power_pub_->unlockAndPublish(); } } From 13c6cf6aa896d81c8e41d66a540ec6db8c7d4b9f Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Wed, 1 Apr 2026 03:41:17 +0000 Subject: [PATCH 08/36] Add powerlimit. --- .../rm_chassis_controllers/chassis_base.h | 15 ++- rm_chassis_controllers/src/swerve.cpp | 121 +++++++++++++++--- 2 files changed, 110 insertions(+), 26 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 f07babf7..8e39770c 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -76,15 +76,16 @@ struct PowerLimitor double effort_coeff{}; double power_offset{}; double max_power{}; + double power_in[4]{}; double power_sum{}; + double power_limit[4]{}; double ratio{ 1.0 }; - double Err_upper{}; - double Err_lower{}; - double Err_sum{}; - double A{}; - double B{}; - double C{}; - double Delta{}; + double err[4]{}; + double err_upper{}; + double err_lower{}; + double err_sum{}; + double omiga[4]{}; + double torque[4]{}; double K{}; }; diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index 1e150f96..6433d2ed 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -174,16 +174,92 @@ void SwerveController::getBaseGyro() } } +// Ref: https://gitee.com/cod_-control/rmcod2026_-sentry/tree/dev +// https://github.com/hkustenterprize/RM2024-PowerModule + void SwerveController::powerLimit() { updatePowerStatus(); - auto power_group = [](std::vector& joints, const PowerLimitor& limitor) { - - }; - - power_group(pivot_joint_handles_, pivot_power_limitor_); - power_group(wheel_joint_handles_, wheel_power_limitor_); + // 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_.power_sum; + 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_.power_sum); + 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_.power_sum > pivot_power_limitor_.max_power) + { + for (int i = 0; i < pivot_joint_handles_.size() && i < 4; ++i) + { + auto& joint = pivot_joint_handles_[i]; + double A = pivot_power_limitor_.effort_coeff; + double B = pivot_power_limitor_.omiga[i] / 9.55f; + double C = square(pivot_power_limitor_.omiga[i]) * pivot_power_limitor_.vel_coeff + + pivot_power_limitor_.power_offset - 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_.power_sum > wheel_power_limitor_.max_power) + { + for (int i = 0; i < wheel_joint_handles_.size() && i < 4; ++i) + { + auto& joint = wheel_joint_handles_[i]; + double A = wheel_power_limitor_.effort_coeff; + double B = wheel_power_limitor_.omiga[i] / 9.55f; + double C = square(wheel_power_limitor_.omiga[i]) * wheel_power_limitor_.vel_coeff + + wheel_power_limitor_.power_offset - 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)); + else + joint.setCommand((-B - Sqrt) / (2 * A)); + } + else + { + joint.setCommand((-B) / (2 * A)); + } + } + } } void SwerveController::updatePowerStatus() @@ -197,32 +273,39 @@ void SwerveController::updatePowerStatus() std::min(power_config.pivot_max_power, power_limit * power_config.pivot_power_ratio), .ratio = power_config.pivot_power_ratio }; double epivot_power{}, cpivot_power{}; - for (const auto& joint : pivot_joint_handles_) + for (size_t i = 0; i < pivot_joint_handles_.size() && i < 4; ++i) { - double cmd_torque = joint.getCommand(); - double real_vel = joint.getVelocity(); + const auto& joint = pivot_joint_handles_[i]; + double cmd_torque = pivot_power_limitor_.torque[i] = joint.getCommand(); + double real_vel = pivot_power_limitor_.omiga[i] = joint.getVelocity(); double real_torque = joint.getEffort(); + pivot_power_limitor_.power_in[i] = + cmd_torque * real_vel / 9.55f + pivot_power_limitor_.effort_coeff * square(cmd_torque) + + pivot_power_limitor_.vel_coeff * square(real_vel) + pivot_power_limitor_.power_offset; epivot_power += real_torque * real_vel / 9.55f + pivot_power_limitor_.effort_coeff * square(real_torque) + - pivot_power_limitor_.vel_coeff * square(real_vel); - cpivot_power += cmd_torque * real_vel / 9.55f + pivot_power_limitor_.effort_coeff * square(cmd_torque) + - pivot_power_limitor_.vel_coeff * square(real_vel); + pivot_power_limitor_.vel_coeff * square(real_vel) + pivot_power_limitor_.power_offset; + cpivot_power += pivot_power_limitor_.power_in[i]; } pivot_power_limitor_.power_sum = cpivot_power; wheel_power_limitor_ = { .vel_coeff = power_config.vel_coeff, .effort_coeff = power_config.effort_coeff, .power_offset = power_config.power_offset, - .max_power = power_limit - std::min(cpivot_power, pivot_power_limitor_.max_power) }; + .max_power = power_limit - std::abs(pivot_power_limitor_.power_sum) }; double ewheel_power{}, cwheel_power{}; - for (const auto& joint : wheel_joint_handles_) + for (int i = 0; i < wheel_joint_handles_.size() && i < 4; ++i) { - double cmd_torque = joint.getCommand(); - double real_vel = joint.getVelocity(); + const auto& joint = wheel_joint_handles_[i]; + + double cmd_torque = wheel_power_limitor_.torque[i] = joint.getCommand(); + double real_vel = wheel_power_limitor_.omiga[i] = joint.getVelocity(); double real_torque = joint.getEffort(); + wheel_power_limitor_.power_in[i] = + cmd_torque * real_vel / 9.55f + wheel_power_limitor_.effort_coeff * square(cmd_torque) + + wheel_power_limitor_.vel_coeff * square(real_vel) + wheel_power_limitor_.power_offset; ewheel_power += real_torque * real_vel / 9.55f + wheel_power_limitor_.effort_coeff * square(real_torque) + - wheel_power_limitor_.vel_coeff * square(real_vel); - cwheel_power += cmd_torque * real_vel / 9.55f + wheel_power_limitor_.effort_coeff * square(cmd_torque) + - wheel_power_limitor_.vel_coeff * square(real_vel); + wheel_power_limitor_.vel_coeff * square(real_vel) + wheel_power_limitor_.power_offset; + cwheel_power += wheel_power_limitor_.power_in[i]; } wheel_power_limitor_.power_sum = cwheel_power; From 5fa28787e45571d93cc662d8a2de6626d2f42620 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Wed, 1 Apr 2026 13:24:29 +0000 Subject: [PATCH 09/36] Fix power publish. --- rm_chassis_controllers/src/swerve.cpp | 33 +++++++++++---------------- 1 file changed, 13 insertions(+), 20 deletions(-) diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index 6433d2ed..bf0620bf 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -309,26 +309,19 @@ void SwerveController::updatePowerStatus() } wheel_power_limitor_.power_sum = cwheel_power; - if (epivot_power_pub_->trylock()) - { - epivot_power_pub_->msg_.data = epivot_power; - epivot_power_pub_->unlockAndPublish(); - } - if (cpivot_power_pub_->trylock()) - { - cpivot_power_pub_->msg_.data = cpivot_power; - cpivot_power_pub_->unlockAndPublish(); - } - if (ewheel_power_pub_->trylock()) - { - ewheel_power_pub_->msg_.data = ewheel_power; - ewheel_power_pub_->unlockAndPublish(); - } - if (cwheel_power_pub_->trylock()) - { - cwheel_power_pub_->msg_.data = cwheel_power; - cwheel_power_pub_->unlockAndPublish(); - } + // Publish power status. + auto publishPower = [](auto& pub, const double power) { + if (pub && pub->trylock()) + { + pub->msg_.data = power; + pub->unlockAndPublish(); + } + }; + + publishPower(epivot_power_pub_, epivot_power); + publishPower(cpivot_power_pub_, cpivot_power); + publishPower(ewheel_power_pub_, ewheel_power); + publishPower(cwheel_power_pub_, cwheel_power); } PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::SwerveController, controller_interface::ControllerBase) From 793048dae6bdcd0bcebfd7a86c2f70b3dffe9aaf Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sat, 4 Apr 2026 10:03:49 +0000 Subject: [PATCH 10/36] =?UTF-8?q?=E4=BC=98=E5=8C=96SwerveController?= =?UTF-8?q?=E7=9A=84=E5=8A=9F=E7=8E=87=E7=8A=B6=E6=80=81=E6=9B=B4=E6=96=B0?= =?UTF-8?q?=E9=80=BB=E8=BE=91=EF=BC=8C=E6=94=B9=E7=94=A8=E6=A8=A1=E5=9D=97?= =?UTF-8?q?=E6=95=B0=E7=BB=84=E6=9B=BF=E4=BB=A3=E5=85=B3=E8=8A=82=E5=8F=A5?= =?UTF-8?q?=E6=9F=84=EF=BC=8C=E5=A2=9E=E5=8A=A0=E9=94=99=E8=AF=AF=E8=AE=A1?= =?UTF-8?q?=E7=AE=97=E4=BB=A5=E6=8F=90=E5=8D=87=E5=8A=9F=E7=8E=87=E6=8E=A7?= =?UTF-8?q?=E5=88=B6=E7=B2=BE=E5=BA=A6.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- rm_chassis_controllers/src/swerve.cpp | 41 ++++++++++++++++----------- 1 file changed, 24 insertions(+), 17 deletions(-) diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index dc3520cd..0b2fd95a 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -275,12 +275,12 @@ void SwerveController::updatePowerStatus() std::min(power_config.pivot_max_power, power_limit * power_config.pivot_power_ratio), .ratio = power_config.pivot_power_ratio }; double epivot_power{}, cpivot_power{}; - for (size_t i = 0; i < pivot_joint_handles_.size() && i < 4; ++i) + for (size_t i = 0; i < modules_.size() && i < 4; ++i) { - const auto& joint = pivot_joint_handles_[i]; - double cmd_torque = pivot_power_limitor_.torque[i] = joint.getCommand(); - double real_vel = pivot_power_limitor_.omiga[i] = joint.getVelocity(); - double real_torque = joint.getEffort(); + 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(); + double real_torque = module.ctrl_pivot_->joint_.getEffort(); pivot_power_limitor_.power_in[i] = cmd_torque * real_vel / 9.55f + pivot_power_limitor_.effort_coeff * square(cmd_torque) + pivot_power_limitor_.vel_coeff * square(real_vel) + pivot_power_limitor_.power_offset; @@ -290,19 +290,26 @@ void SwerveController::updatePowerStatus() } pivot_power_limitor_.power_sum = cpivot_power; - wheel_power_limitor_ = { .vel_coeff = power_config.vel_coeff, - .effort_coeff = power_config.effort_coeff, - .power_offset = power_config.power_offset, - .max_power = power_limit - std::abs(pivot_power_limitor_.power_sum) }; + wheel_power_limitor_ = { + .vel_coeff = power_config.vel_coeff, + .effort_coeff = power_config.effort_coeff, + .power_offset = power_config.power_offset, + .max_power = power_limit - std::abs(pivot_power_limitor_.power_sum), + .err_upper = 4000, + .err_lower = 0.001, + }; + double ewheel_power{}, cwheel_power{}; - const size_t wheel_count = std::min(wheel_joint_handles_.size(), 4); - for (size_t i = 0; i < wheel_count; ++i) + for (size_t i = 0; i < modules_.size() && i < 4; ++i) { - const auto& joint = wheel_joint_handles_[i]; - - double cmd_torque = wheel_power_limitor_.torque[i] = joint.getCommand(); - double real_vel = wheel_power_limitor_.omiga[i] = joint.getVelocity(); - double real_torque = joint.getEffort(); + 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(); + double real_torque = module.ctrl_wheel_->joint_.getEffort(); + wheel_power_limitor_.err[i] = cmd_vel - real_vel; + wheel_power_limitor_.err_sum += abs(wheel_power_limitor_.err[i]); wheel_power_limitor_.power_in[i] = cmd_torque * real_vel / 9.55f + wheel_power_limitor_.effort_coeff * square(cmd_torque) + wheel_power_limitor_.vel_coeff * square(real_vel) + wheel_power_limitor_.power_offset; @@ -325,7 +332,7 @@ void SwerveController::updatePowerStatus() publishPower(cpivot_power_pub_, cpivot_power); publishPower(ewheel_power_pub_, ewheel_power); publishPower(cwheel_power_pub_, cwheel_power); -} +} // namespace rm_chassis_controllers PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::SwerveController, controller_interface::ControllerBase) } // namespace rm_chassis_controllers From 57a9c2114ef54431c06fcf6bc2b075801cd47bfc Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sat, 4 Apr 2026 13:01:14 +0000 Subject: [PATCH 11/36] Add swerve. --- rm_chassis_controllers/src/swerve.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index 0b2fd95a..44c6d28c 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -66,7 +66,7 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand std::make_unique>(controller_nh, "power/cwheel_power", 100); this->cwheel_power_pub_ = std::move(cwheel_power_publisher); auto epivot_power_publisher = - std::make_unique>(controller_nh, "power/pivot_power", 100); + std::make_unique>(controller_nh, "power/epivot_power", 100); this->epivot_power_pub_ = std::move(epivot_power_publisher); auto cpivot_power_publisher = std::make_unique>(controller_nh, "power/cpivot_power", 100); @@ -332,7 +332,7 @@ void SwerveController::updatePowerStatus() publishPower(cpivot_power_pub_, cpivot_power); publishPower(ewheel_power_pub_, ewheel_power); publishPower(cwheel_power_pub_, cwheel_power); -} // namespace rm_chassis_controllers +} PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::SwerveController, controller_interface::ControllerBase) } // namespace rm_chassis_controllers From 8c32a943ce2e884f58414ad1a71c0bcc6f08564f Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sun, 5 Apr 2026 03:51:10 +0000 Subject: [PATCH 12/36] =?UTF-8?q?=E4=BF=AE=E5=A4=8DSwerveController?= =?UTF-8?q?=E4=B8=AD=E7=9A=84=E5=8A=9F=E7=8E=87=E8=AE=A1=E7=AE=97=E9=80=BB?= =?UTF-8?q?=E8=BE=91=EF=BC=8C=E8=B0=83=E6=95=B4=E5=8A=9F=E7=8E=87=E9=99=90?= =?UTF-8?q?=E5=88=B6=E5=92=8C=E8=AF=AF=E5=B7=AE=E4=B8=8A=E9=99=90=EF=BC=8C?= =?UTF-8?q?=E4=BC=98=E5=8C=96=E5=8A=9F=E7=8E=87=E7=8A=B6=E6=80=81=E6=9B=B4?= =?UTF-8?q?=E6=96=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- rm_chassis_controllers/src/swerve.cpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index 44c6d28c..c1490335 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -227,7 +227,7 @@ void SwerveController::powerLimit() if (pivot_power_limitor_.torque[i] >= 0) joint.setCommand((-B + Sqrt) / (2 * A)); else - joint.setCommand((-B - Sqrt) / (2 * A)); + joint.setCommand((-B + Sqrt) / (2 * A)); } else { @@ -281,21 +281,21 @@ void SwerveController::updatePowerStatus() 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(); double real_torque = module.ctrl_pivot_->joint_.getEffort(); - pivot_power_limitor_.power_in[i] = - cmd_torque * real_vel / 9.55f + pivot_power_limitor_.effort_coeff * square(cmd_torque) + - pivot_power_limitor_.vel_coeff * square(real_vel) + pivot_power_limitor_.power_offset; + pivot_power_limitor_.power_in[i] = cmd_torque * real_vel / 9.55f + + pivot_power_limitor_.effort_coeff * square(cmd_torque) + + pivot_power_limitor_.vel_coeff * square(real_vel); epivot_power += real_torque * real_vel / 9.55f + pivot_power_limitor_.effort_coeff * square(real_torque) + - pivot_power_limitor_.vel_coeff * square(real_vel) + pivot_power_limitor_.power_offset; + pivot_power_limitor_.vel_coeff * square(real_vel); cpivot_power += pivot_power_limitor_.power_in[i]; } - pivot_power_limitor_.power_sum = cpivot_power; + pivot_power_limitor_.power_sum = cpivot_power + pivot_power_limitor_.power_offset; wheel_power_limitor_ = { .vel_coeff = power_config.vel_coeff, .effort_coeff = power_config.effort_coeff, .power_offset = power_config.power_offset, .max_power = power_limit - std::abs(pivot_power_limitor_.power_sum), - .err_upper = 4000, + .err_upper = 500, .err_lower = 0.001, }; @@ -310,14 +310,14 @@ void SwerveController::updatePowerStatus() double real_torque = module.ctrl_wheel_->joint_.getEffort(); wheel_power_limitor_.err[i] = cmd_vel - real_vel; wheel_power_limitor_.err_sum += abs(wheel_power_limitor_.err[i]); - wheel_power_limitor_.power_in[i] = - cmd_torque * real_vel / 9.55f + wheel_power_limitor_.effort_coeff * square(cmd_torque) + - wheel_power_limitor_.vel_coeff * square(real_vel) + wheel_power_limitor_.power_offset; + wheel_power_limitor_.power_in[i] = cmd_torque * real_vel / 9.55f + + wheel_power_limitor_.effort_coeff * square(cmd_torque) + + wheel_power_limitor_.vel_coeff * square(real_vel); ewheel_power += real_torque * real_vel / 9.55f + wheel_power_limitor_.effort_coeff * square(real_torque) + - wheel_power_limitor_.vel_coeff * square(real_vel) + wheel_power_limitor_.power_offset; + wheel_power_limitor_.vel_coeff * square(real_vel); cwheel_power += wheel_power_limitor_.power_in[i]; } - wheel_power_limitor_.power_sum = cwheel_power; + wheel_power_limitor_.power_sum = cwheel_power + wheel_power_limitor_.power_offset; // Publish power status. auto publishPower = [](auto& pub, const double power) { From c43308e276be4bf9029f9928db476875f8d7af9a Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sun, 5 Apr 2026 04:42:46 +0000 Subject: [PATCH 13/36] Final. --- rm_chassis_controllers/src/swerve.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index c1490335..7b5bc928 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -210,10 +210,10 @@ void SwerveController::powerLimit() // Set command limit according to power limit. if (pivot_power_limitor_.power_sum > pivot_power_limitor_.max_power) { - const size_t pivot_count = std::min(pivot_joint_handles_.size(), 4); - for (size_t i = 0; i < pivot_count; ++i) + for (size_t i = 0; i < modules_.size() && i < 4; ++i) { - auto& joint = pivot_joint_handles_[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] / 9.55f; double C = square(pivot_power_limitor_.omiga[i]) * pivot_power_limitor_.vel_coeff + @@ -237,10 +237,10 @@ void SwerveController::powerLimit() } if (wheel_power_limitor_.power_sum > wheel_power_limitor_.max_power) { - const size_t wheel_count = std::min(wheel_joint_handles_.size(), 4); - for (size_t i = 0; i < wheel_count; ++i) + for (size_t i = 0; i < modules_.size() && i < 4; ++i) { - auto& joint = wheel_joint_handles_[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] / 9.55f; double C = square(wheel_power_limitor_.omiga[i]) * wheel_power_limitor_.vel_coeff + @@ -251,7 +251,9 @@ void SwerveController::powerLimit() if (Delta >= 0) { double Sqrt = sqrtf(Delta); - if (wheel_power_limitor_.torque[i] >= 0) + double cmd{}; + module.ctrl_wheel_->getCommand(cmd); + if (cmd >= 0) joint.setCommand((-B + Sqrt) / (2 * A)); else joint.setCommand((-B - Sqrt) / (2 * A)); From 78015f8809c5564c3ca3f2319af7f1bf27209eaf Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sun, 5 Apr 2026 08:40:14 +0000 Subject: [PATCH 14/36] =?UTF-8?q?=E4=BC=98=E5=8C=96SwerveController?= =?UTF-8?q?=E7=9A=84=E5=8A=9F=E7=8E=87=E8=AE=A1=E7=AE=97=E9=80=BB=E8=BE=91?= =?UTF-8?q?=EF=BC=8C=E4=BF=AE=E6=AD=A3=E5=8A=9F=E7=8E=87=E8=BE=93=E5=85=A5?= =?UTF-8?q?=E5=92=8C=E6=80=BB=E5=8A=9F=E7=8E=87=E7=9A=84=E8=AE=A1=E7=AE=97?= =?UTF-8?q?=E6=96=B9=E5=BC=8F=EF=BC=8C=E4=BB=A5=E6=8F=90=E9=AB=98=E5=8A=9F?= =?UTF-8?q?=E7=8E=87=E6=8E=A7=E5=88=B6=E7=B2=BE=E5=BA=A6.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- rm_chassis_controllers/src/swerve.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index 7b5bc928..34c636e6 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -283,14 +283,14 @@ void SwerveController::updatePowerStatus() 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(); double real_torque = module.ctrl_pivot_->joint_.getEffort(); - pivot_power_limitor_.power_in[i] = cmd_torque * real_vel / 9.55f + - pivot_power_limitor_.effort_coeff * square(cmd_torque) + + pivot_power_limitor_.power_in[i] = real_torque * real_vel / 9.55f + + pivot_power_limitor_.effort_coeff * square(real_torque) + pivot_power_limitor_.vel_coeff * square(real_vel); - epivot_power += real_torque * real_vel / 9.55f + pivot_power_limitor_.effort_coeff * square(real_torque) + + epivot_power += pivot_power_limitor_.power_in[i]; + cpivot_power += cmd_torque * real_vel / 9.55f + pivot_power_limitor_.effort_coeff * square(cmd_torque) + pivot_power_limitor_.vel_coeff * square(real_vel); - cpivot_power += pivot_power_limitor_.power_in[i]; } - pivot_power_limitor_.power_sum = cpivot_power + pivot_power_limitor_.power_offset; + pivot_power_limitor_.power_sum = epivot_power + pivot_power_limitor_.power_offset; wheel_power_limitor_ = { .vel_coeff = power_config.vel_coeff, @@ -312,12 +312,12 @@ void SwerveController::updatePowerStatus() double real_torque = module.ctrl_wheel_->joint_.getEffort(); wheel_power_limitor_.err[i] = cmd_vel - real_vel; wheel_power_limitor_.err_sum += abs(wheel_power_limitor_.err[i]); - wheel_power_limitor_.power_in[i] = cmd_torque * real_vel / 9.55f + - wheel_power_limitor_.effort_coeff * square(cmd_torque) + + wheel_power_limitor_.power_in[i] = real_torque * real_vel / 9.55f + + wheel_power_limitor_.effort_coeff * square(real_torque) + wheel_power_limitor_.vel_coeff * square(real_vel); - ewheel_power += real_torque * real_vel / 9.55f + wheel_power_limitor_.effort_coeff * square(real_torque) + + ewheel_power += wheel_power_limitor_.power_in[i]; + cwheel_power += cmd_torque * real_vel / 9.55f + wheel_power_limitor_.effort_coeff * square(cmd_torque) + wheel_power_limitor_.vel_coeff * square(real_vel); - cwheel_power += wheel_power_limitor_.power_in[i]; } wheel_power_limitor_.power_sum = cwheel_power + wheel_power_limitor_.power_offset; From 05c73bd599c7528fb40466b45e35e5d130094e04 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sun, 5 Apr 2026 08:40:40 +0000 Subject: [PATCH 15/36] Fix. --- rm_chassis_controllers/src/swerve.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index 34c636e6..c70cb86a 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -319,7 +319,7 @@ void SwerveController::updatePowerStatus() cwheel_power += cmd_torque * real_vel / 9.55f + wheel_power_limitor_.effort_coeff * square(cmd_torque) + wheel_power_limitor_.vel_coeff * square(real_vel); } - wheel_power_limitor_.power_sum = cwheel_power + wheel_power_limitor_.power_offset; + wheel_power_limitor_.power_sum = ewheel_power + wheel_power_limitor_.power_offset; // Publish power status. auto publishPower = [](auto& pub, const double power) { From 6fd45988befd0a7f7a88cae46ca0c94b0bb51a6f Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sun, 5 Apr 2026 08:56:51 +0000 Subject: [PATCH 16/36] Add omni. --- .../include/rm_chassis_controllers/omni.h | 2 + rm_chassis_controllers/src/omni.cpp | 103 ++++++++++++++++++ 2 files changed, 105 insertions(+) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h index 1bc67336..939909c5 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h @@ -24,6 +24,8 @@ class OmniController : public ChassisBase 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_.power_sum); + 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_.power_sum > 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] / 9.55f; + double C = square(wheel_power_limitor_.omiga[i]) * wheel_power_limitor_.vel_coeff + + wheel_power_limitor_.power_offset - 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); + double cmd{}; + ctl->getCommand(cmd); + if (cmd >= 0) + joint.setCommand((-B + Sqrt) / (2 * A)); + else + joint.setCommand((-B - Sqrt) / (2 * A)); + } + else + { + joint.setCommand((-B) / (2 * A)); + } + } + } +} + +void OmniController::updatePowerStatus() +{ + double power_limit = cmd_rt_buffer_.readFromRT()->cmd_chassis_.power_limit; + const auto& power_config = *power_limit_rt_buffer_.readFromRT(); + + wheel_power_limitor_ = { + .vel_coeff = power_config.vel_coeff, + .effort_coeff = power_config.effort_coeff, + .power_offset = power_config.power_offset, + .max_power = power_limit, + .err_upper = 500, + .err_lower = 0.001, + }; + + 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(); + double real_torque = ctl->joint_.getEffort(); + wheel_power_limitor_.err[i] = cmd_vel - real_vel; + wheel_power_limitor_.err_sum += abs(wheel_power_limitor_.err[i]); + wheel_power_limitor_.power_in[i] = real_torque * real_vel / 9.55f + + wheel_power_limitor_.effort_coeff * square(real_torque) + + wheel_power_limitor_.vel_coeff * square(real_vel); + ewheel_power += wheel_power_limitor_.power_in[i]; + cwheel_power += cmd_torque * real_vel / 9.55f + wheel_power_limitor_.effort_coeff * square(cmd_torque) + + wheel_power_limitor_.vel_coeff * square(real_vel); + } + wheel_power_limitor_.power_sum = ewheel_power + wheel_power_limitor_.power_offset; + + // Publish power status. + auto publishPower = [](auto& pub, const double power) { + if (pub && pub->trylock()) + { + pub->msg_.data = power; + pub->unlockAndPublish(); + } + }; + publishPower(ewheel_power_pub_, ewheel_power); + publishPower(cwheel_power_pub_, cwheel_power); +} + } // namespace rm_chassis_controllers PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::OmniController, controller_interface::ControllerBase) From b8d50db1ed7e790dcad8b9871a84c4193e080a38 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sun, 5 Apr 2026 09:29:34 +0000 Subject: [PATCH 17/36] Fix omini. --- .../include/rm_chassis_controllers/omni.h | 6 +++--- rm_chassis_controllers/src/omni.cpp | 11 +++++++++-- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h index 939909c5..471f2019 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h @@ -16,14 +16,14 @@ class OmniController : public ChassisBase> joints_; Eigen::MatrixXd chassis2joints_; - -protected: - void moveJoint(const ros::Time& time, const ros::Duration& period) override; void powerLimit() override; void updatePowerStatus() override; }; diff --git a/rm_chassis_controllers/src/omni.cpp b/rm_chassis_controllers/src/omni.cpp index 99fc103a..bff6c093 100644 --- a/rm_chassis_controllers/src/omni.cpp +++ b/rm_chassis_controllers/src/omni.cpp @@ -17,6 +17,13 @@ bool OmniController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle { ChassisBase::init(robot_hw, root_nh, controller_nh); + auto ewheel_power_publisher = + std::make_unique>(controller_nh, "power/ewheel_power", 100); + this->ewheel_power_pub_ = std::move(ewheel_power_publisher); + auto cwheel_power_publisher = + std::make_unique>(controller_nh, "power/cwheel_power", 100); + this->cwheel_power_pub_ = std::move(cwheel_power_publisher); + XmlRpc::XmlRpcValue wheels; controller_nh.getParam("wheels", wheels); chassis2joints_.resize(wheels.size(), 3); @@ -144,8 +151,8 @@ void OmniController::updatePowerStatus() .effort_coeff = power_config.effort_coeff, .power_offset = power_config.power_offset, .max_power = power_limit, - .err_upper = 500, - .err_lower = 0.001, + .err_upper = 4000, + .err_lower = 10, }; double ewheel_power{}, cwheel_power{}; From df88d553a3223fac51e48c57c4805a16de95fad7 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Tue, 7 Apr 2026 15:24:55 +0000 Subject: [PATCH 18/36] Fix model. --- .../rm_chassis_controllers/chassis_base.h | 1 - .../include/rm_chassis_controllers/swerve.h | 12 +- rm_chassis_controllers/src/omni.cpp | 16 +-- rm_chassis_controllers/src/swerve.cpp | 114 ++++++++++-------- 4 files changed, 80 insertions(+), 63 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 c4b3d8f8..8f41e96e 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -190,7 +190,6 @@ class ChassisBase : public controller_interface::MultiInterfaceController realtime_tools::RealtimeBuffer power_limit_rt_buffer_; std::unique_ptr> odometry_rt_pub_; std::unique_ptr> ewheel_power_pub_; - std::unique_ptr> cwheel_power_pub_; dynamic_reconfigure::Server* power_limit_srv_{}; rm_common::TfRtBroadcaster brcst4global_map2robot_odom_{}; diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h index c4ad240e..b508ccf1 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h @@ -43,6 +43,11 @@ #include #include #include +#include +#include +#include +#include +#include namespace rm_chassis_controllers { @@ -54,7 +59,8 @@ struct Module effort_controllers::JointVelocityController* ctrl_wheel_; }; -class SwerveController : public ChassisBase +class SwerveController : public ChassisBase { public: SwerveController() = default; @@ -66,13 +72,15 @@ class SwerveController : public ChassisBase modules_{}; std::vector pivot_joint_handles_{}; std::unique_ptr> epivot_power_pub_; - std::unique_ptr> cpivot_power_pub_; + std::unique_ptr> base_gyro_pub_; PowerLimitor pivot_power_limitor_{}; + std::array, 2> motor_lp_filters_{}; bool has_gimbal_imu_{ false }; hardware_interface::ImuSensorHandle imu_handle_{}; diff --git a/rm_chassis_controllers/src/omni.cpp b/rm_chassis_controllers/src/omni.cpp index bff6c093..9110619b 100644 --- a/rm_chassis_controllers/src/omni.cpp +++ b/rm_chassis_controllers/src/omni.cpp @@ -20,9 +20,6 @@ bool OmniController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle auto ewheel_power_publisher = std::make_unique>(controller_nh, "power/ewheel_power", 100); this->ewheel_power_pub_ = std::move(ewheel_power_publisher); - auto cwheel_power_publisher = - std::make_unique>(controller_nh, "power/cwheel_power", 100); - this->cwheel_power_pub_ = std::move(cwheel_power_publisher); XmlRpc::XmlRpcValue wheels; controller_nh.getParam("wheels", wheels); @@ -117,9 +114,9 @@ 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] / 9.55f; + double B = wheel_power_limitor_.omiga[i]; double C = square(wheel_power_limitor_.omiga[i]) * wheel_power_limitor_.vel_coeff + - wheel_power_limitor_.power_offset - wheel_power_limitor_.power_limit[i]; + 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; @@ -155,23 +152,21 @@ void OmniController::updatePowerStatus() .err_lower = 10, }; - double ewheel_power{}, cwheel_power{}; + double ewheel_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(); + 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(); double real_torque = ctl->joint_.getEffort(); wheel_power_limitor_.err[i] = cmd_vel - real_vel; wheel_power_limitor_.err_sum += abs(wheel_power_limitor_.err[i]); - wheel_power_limitor_.power_in[i] = real_torque * real_vel / 9.55f + + wheel_power_limitor_.power_in[i] = real_torque * real_vel + wheel_power_limitor_.effort_coeff * square(real_torque) + wheel_power_limitor_.vel_coeff * square(real_vel); ewheel_power += wheel_power_limitor_.power_in[i]; - cwheel_power += cmd_torque * real_vel / 9.55f + wheel_power_limitor_.effort_coeff * square(cmd_torque) + - wheel_power_limitor_.vel_coeff * square(real_vel); } wheel_power_limitor_.power_sum = ewheel_power + wheel_power_limitor_.power_offset; @@ -184,7 +179,6 @@ void OmniController::updatePowerStatus() } }; publishPower(ewheel_power_pub_, ewheel_power); - publishPower(cwheel_power_pub_, cwheel_power); } } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index c70cb86a..15b996a5 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -48,29 +48,40 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand if (!ChassisBase::init(robot_hw, root_nh, controller_nh)) return false; - auto* imu_handle = robot_hw->get(); - if (imu_handle == nullptr) + auto* imu_iface = robot_hw->get(); + if (!imu_iface) + ROS_WARN("SwerveController cannot get ImuSensorInterface"); + else { - ROS_WARN("SwerveController requires gimbal_imu"); + try + { + imu_handle_ = imu_iface->getHandle(getParam(controller_nh, "imu_name", std::string("gimbal_imu"))); + has_gimbal_imu_ = true; + ROS_INFO_STREAM("SwerveController imu_name: " << imu_handle_.getName()); + } + catch (const hardware_interface::HardwareInterfaceException& e) + { + has_gimbal_imu_ = false; + ROS_WARN_STREAM("SwerveController cannot get imu handle: " << e.what()); + } } - else + + // 0 for pivot, 1 for wheel. Each module has 4 joints at most. + for (auto& filter_group : motor_lp_filters_) { - imu_handle_ = imu_handle->getHandle("gimbal_imu"); - has_gimbal_imu_ = true; + for (auto& filter : filter_group) + filter = new LowPassFilter(20); } auto ewheel_power_publisher = std::make_unique>(controller_nh, "power/ewheel_power", 100); this->ewheel_power_pub_ = std::move(ewheel_power_publisher); - auto cwheel_power_publisher = - std::make_unique>(controller_nh, "power/cwheel_power", 100); - this->cwheel_power_pub_ = std::move(cwheel_power_publisher); auto epivot_power_publisher = std::make_unique>(controller_nh, "power/epivot_power", 100); this->epivot_power_pub_ = std::move(epivot_power_publisher); - auto cpivot_power_publisher = - std::make_unique>(controller_nh, "power/cpivot_power", 100); - this->cpivot_power_pub_ = std::move(cpivot_power_publisher); + 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); @@ -116,6 +127,8 @@ 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_; + // Judge for flying slope. + // vel_angle = stateJudge(vel_angle); // 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); @@ -154,26 +167,37 @@ geometry_msgs::Twist SwerveController::odometry() void SwerveController::getBaseGyro() { - // Todo: because swerve don't have chassis imu, we use gimbal imu to get chassis gyro. This is not good because of the - // gimbal's rotation. A better solution is to install an imu on the chassis. + // Todo:use base_imu to detect real base_gyro if (has_gimbal_imu_) { geometry_msgs::Vector3 imu_gyro{}; imu_gyro.x = imu_handle_.getAngularVelocity()[0]; imu_gyro.y = imu_handle_.getAngularVelocity()[1]; imu_gyro.z = imu_handle_.getAngularVelocity()[2]; - try - { - tf2::doTransform(imu_gyro, base_gyro_, - robot_state_handle_.lookupTransform("base_link", imu_handle_.getFrameId(), ros::Time(0))); - } - catch (tf2::TransformException& ex) + + // Publish base_gyro. + if (base_gyro_pub_ && base_gyro_pub_->trylock()) { - ROS_WARN_THROTTLE(1.0, "%s", ex.what()); + base_gyro_pub_->msg_.header.stamp = ros::Time::now(); + base_gyro_pub_->msg_.header.frame_id = "base_link"; + base_gyro_pub_->msg_.vector = base_gyro_; + base_gyro_pub_->unlockAndPublish(); } } } +double SwerveController::stateJudge(double vel_angle) +{ + // Check for flying slope state based on gyro angular velocity (Roll and Pitch) + if (fabs(base_gyro_.y) > 1.5) + // Flying slope state,set pivot to 0 + { + return 0; + } + else + return vel_angle; +} + // Ref: https://gitee.com/cod_-control/rmcod2026_-sentry/tree/dev // https://github.com/hkustenterprize/RM2024-PowerModule @@ -215,9 +239,9 @@ void SwerveController::powerLimit() auto& module = modules_[i]; auto& joint = module.ctrl_pivot_->joint_; double A = pivot_power_limitor_.effort_coeff; - double B = pivot_power_limitor_.omiga[i] / 9.55f; + double B = pivot_power_limitor_.omiga[i]; double C = square(pivot_power_limitor_.omiga[i]) * pivot_power_limitor_.vel_coeff + - pivot_power_limitor_.power_offset - pivot_power_limitor_.power_limit[i]; + 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; @@ -227,7 +251,7 @@ void SwerveController::powerLimit() if (pivot_power_limitor_.torque[i] >= 0) joint.setCommand((-B + Sqrt) / (2 * A)); else - joint.setCommand((-B + Sqrt) / (2 * A)); + joint.setCommand((-B - Sqrt) / (2 * A)); } else { @@ -242,18 +266,16 @@ 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] / 9.55f; + double B = wheel_power_limitor_.omiga[i]; double C = square(wheel_power_limitor_.omiga[i]) * wheel_power_limitor_.vel_coeff + - wheel_power_limitor_.power_offset - wheel_power_limitor_.power_limit[i]; + 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); - double cmd{}; - module.ctrl_wheel_->getCommand(cmd); - if (cmd >= 0) + if (wheel_power_limitor_.torque[i] >= 0) joint.setCommand((-B + Sqrt) / (2 * A)); else joint.setCommand((-B - Sqrt) / (2 * A)); @@ -276,19 +298,17 @@ void SwerveController::updatePowerStatus() .max_power = std::min(power_config.pivot_max_power, power_limit * power_config.pivot_power_ratio), .ratio = power_config.pivot_power_ratio }; - double epivot_power{}, cpivot_power{}; + double epivot_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(); + pivot_power_limitor_.torque[i] = module.ctrl_pivot_->joint_.getCommand(); double real_vel = pivot_power_limitor_.omiga[i] = module.ctrl_pivot_->joint_.getVelocity(); - double real_torque = module.ctrl_pivot_->joint_.getEffort(); - pivot_power_limitor_.power_in[i] = real_torque * real_vel / 9.55f + - pivot_power_limitor_.effort_coeff * square(real_torque) + - pivot_power_limitor_.vel_coeff * square(real_vel); - epivot_power += pivot_power_limitor_.power_in[i]; - cpivot_power += cmd_torque * real_vel / 9.55f + pivot_power_limitor_.effort_coeff * square(cmd_torque) + + 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 * square(real_vel); + pivot_power_limitor_.power_in[i] = epivot_power; } pivot_power_limitor_.power_sum = epivot_power + pivot_power_limitor_.power_offset; @@ -297,27 +317,25 @@ void SwerveController::updatePowerStatus() .effort_coeff = power_config.effort_coeff, .power_offset = power_config.power_offset, .max_power = power_limit - std::abs(pivot_power_limitor_.power_sum), - .err_upper = 500, - .err_lower = 0.001, + .err_upper = 4000, + .err_lower = 100, }; - double ewheel_power{}, cwheel_power{}; + double ewheel_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(); + 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(); - double real_torque = module.ctrl_wheel_->joint_.getEffort(); + 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]); - wheel_power_limitor_.power_in[i] = real_torque * real_vel / 9.55f + - wheel_power_limitor_.effort_coeff * square(real_torque) + - wheel_power_limitor_.vel_coeff * square(real_vel); - ewheel_power += wheel_power_limitor_.power_in[i]; - cwheel_power += cmd_torque * real_vel / 9.55f + wheel_power_limitor_.effort_coeff * square(cmd_torque) + + ewheel_power += real_torque * real_vel + wheel_power_limitor_.effort_coeff * square(real_torque) + wheel_power_limitor_.vel_coeff * square(real_vel); + wheel_power_limitor_.power_in[i] = ewheel_power; } wheel_power_limitor_.power_sum = ewheel_power + wheel_power_limitor_.power_offset; @@ -331,9 +349,7 @@ void SwerveController::updatePowerStatus() }; publishPower(epivot_power_pub_, epivot_power); - publishPower(cpivot_power_pub_, cpivot_power); publishPower(ewheel_power_pub_, ewheel_power); - publishPower(cwheel_power_pub_, cwheel_power); } PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::SwerveController, controller_interface::ControllerBase) From adb1ea6c87e8edad5052419faf55a7082c54c71d Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sat, 11 Apr 2026 09:13:26 +0000 Subject: [PATCH 19/36] Remove imu. --- .../include/rm_chassis_controllers/swerve.h | 4 -- rm_chassis_controllers/src/swerve.cpp | 72 ++++--------------- 2 files changed, 12 insertions(+), 64 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h index b508ccf1..a6f8412a 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h @@ -81,10 +81,6 @@ class SwerveController : public ChassisBase, 2> motor_lp_filters_{}; - - bool has_gimbal_imu_{ false }; - hardware_interface::ImuSensorHandle imu_handle_{}; - geometry_msgs::Vector3 base_gyro_{}; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index 15b996a5..e19f469d 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -48,24 +48,6 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand if (!ChassisBase::init(robot_hw, root_nh, controller_nh)) return false; - auto* imu_iface = robot_hw->get(); - if (!imu_iface) - ROS_WARN("SwerveController cannot get ImuSensorInterface"); - else - { - try - { - imu_handle_ = imu_iface->getHandle(getParam(controller_nh, "imu_name", std::string("gimbal_imu"))); - has_gimbal_imu_ = true; - ROS_INFO_STREAM("SwerveController imu_name: " << imu_handle_.getName()); - } - catch (const hardware_interface::HardwareInterfaceException& e) - { - has_gimbal_imu_ = false; - ROS_WARN_STREAM("SwerveController cannot get imu handle: " << e.what()); - } - } - // 0 for pivot, 1 for wheel. Each module has 4 joints at most. for (auto& filter_group : motor_lp_filters_) { @@ -121,19 +103,22 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand void SwerveController::moveJoint(const ros::Time& time, const ros::Duration& period) { - // getBaseGyro(); 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_; - // Judge for flying slope. - // vel_angle = stateJudge(vel_angle); - // 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); - module.ctrl_wheel_->setCommand(vel.norm() / module.wheel_radius_ * std::cos(a)); + if (vel.norm() < 0.2) + { + module.ctrl_wheel_->setCommand(0); + } + else + { + double vel_angle = std::atan2(vel.y(), vel.x()) + module.pivot_offset_; + 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); + module.ctrl_wheel_->setCommand(vel.norm() / module.wheel_radius_ * std::cos(a)); + } module.ctrl_pivot_->update(time, period); module.ctrl_wheel_->update(time, period); } @@ -165,39 +150,6 @@ geometry_msgs::Twist SwerveController::odometry() return vel_data; } -void SwerveController::getBaseGyro() -{ - // Todo:use base_imu to detect real base_gyro - if (has_gimbal_imu_) - { - geometry_msgs::Vector3 imu_gyro{}; - imu_gyro.x = imu_handle_.getAngularVelocity()[0]; - imu_gyro.y = imu_handle_.getAngularVelocity()[1]; - imu_gyro.z = imu_handle_.getAngularVelocity()[2]; - - // Publish base_gyro. - if (base_gyro_pub_ && base_gyro_pub_->trylock()) - { - base_gyro_pub_->msg_.header.stamp = ros::Time::now(); - base_gyro_pub_->msg_.header.frame_id = "base_link"; - base_gyro_pub_->msg_.vector = base_gyro_; - base_gyro_pub_->unlockAndPublish(); - } - } -} - -double SwerveController::stateJudge(double vel_angle) -{ - // Check for flying slope state based on gyro angular velocity (Roll and Pitch) - if (fabs(base_gyro_.y) > 1.5) - // Flying slope state,set pivot to 0 - { - return 0; - } - else - return vel_angle; -} - // Ref: https://gitee.com/cod_-control/rmcod2026_-sentry/tree/dev // https://github.com/hkustenterprize/RM2024-PowerModule From 2ae17cd8f8ff7f5c02bde2a2812fa0e78ef75315 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sat, 11 Apr 2026 15:18:25 +0000 Subject: [PATCH 20/36] Fix bugs. --- .../rm_chassis_controllers/chassis_base.h | 2 +- .../include/rm_chassis_controllers/swerve.h | 1 - rm_chassis_controllers/src/swerve.cpp | 72 ++++++++----------- 3 files changed, 30 insertions(+), 45 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 8f41e96e..8a1ae632 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -189,7 +189,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController realtime_tools::RealtimeBuffer localization_rt_buffer_{}; realtime_tools::RealtimeBuffer power_limit_rt_buffer_; std::unique_ptr> odometry_rt_pub_; - std::unique_ptr> ewheel_power_pub_; + std::unique_ptr> power_pub_; dynamic_reconfigure::Server* power_limit_srv_{}; rm_common::TfRtBroadcaster brcst4global_map2robot_odom_{}; diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h index a6f8412a..08d2b3c3 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h @@ -76,7 +76,6 @@ class SwerveController : public ChassisBase modules_{}; std::vector pivot_joint_handles_{}; - std::unique_ptr> epivot_power_pub_; std::unique_ptr> base_gyro_pub_; PowerLimitor pivot_power_limitor_{}; diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index e19f469d..64788679 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -52,15 +52,12 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand for (auto& filter_group : motor_lp_filters_) { for (auto& filter : filter_group) - filter = new LowPassFilter(20); + filter = new LowPassFilter(5); } - auto ewheel_power_publisher = - std::make_unique>(controller_nh, "power/ewheel_power", 100); - this->ewheel_power_pub_ = std::move(ewheel_power_publisher); - auto epivot_power_publisher = - std::make_unique>(controller_nh, "power/epivot_power", 100); - this->epivot_power_pub_ = std::move(epivot_power_publisher); + auto power_publisher = + std::make_unique>(controller_nh, "power", 100); + this->power_pub_ = std::move(power_publisher); auto base_gyro_publisher = std::make_unique>( controller_nh, "base_gyro", 100); this->base_gyro_pub_ = std::move(base_gyro_publisher); @@ -107,18 +104,11 @@ void SwerveController::moveJoint(const ros::Time& time, const ros::Duration& per for (auto& module : modules_) { Vec2 vel = vel_center + vel_cmd_.z * Vec2(-module.position_.y(), module.position_.x()); - if (vel.norm() < 0.2) - { - module.ctrl_wheel_->setCommand(0); - } - else - { - double vel_angle = std::atan2(vel.y(), vel.x()) + module.pivot_offset_; - 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); - module.ctrl_wheel_->setCommand(vel.norm() / module.wheel_radius_ * std::cos(a)); - } + double vel_angle = std::atan2(vel.y(), vel.x()) + module.pivot_offset_; + 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); + module.ctrl_wheel_->setCommand(vel.norm() / module.wheel_radius_ * std::cos(a)); module.ctrl_pivot_->update(time, period); module.ctrl_wheel_->update(time, period); } @@ -258,20 +248,20 @@ void SwerveController::updatePowerStatus() 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 * square(real_vel); - pivot_power_limitor_.power_in[i] = epivot_power; + pivot_power_limitor_.power_in[i] = real_torque * real_vel + + pivot_power_limitor_.effort_coeff * square(real_torque) + + pivot_power_limitor_.vel_coeff * square(real_vel); + epivot_power += pivot_power_limitor_.power_in[i]; } pivot_power_limitor_.power_sum = epivot_power + pivot_power_limitor_.power_offset; - wheel_power_limitor_ = { - .vel_coeff = power_config.vel_coeff, - .effort_coeff = power_config.effort_coeff, - .power_offset = power_config.power_offset, - .max_power = power_limit - std::abs(pivot_power_limitor_.power_sum), - .err_upper = 4000, - .err_lower = 100, - }; + wheel_power_limitor_ = { .vel_coeff = power_config.vel_coeff, + .effort_coeff = power_config.effort_coeff, + .power_offset = power_config.power_offset, + .max_power = power_limit - std::abs(pivot_power_limitor_.power_sum), + .err_upper = 1e8, + .err_lower = 1e6, + .err_sum = 0 }; double ewheel_power{}; for (size_t i = 0; i < modules_.size() && i < 4; ++i) @@ -285,23 +275,19 @@ void SwerveController::updatePowerStatus() 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 * square(real_vel); - wheel_power_limitor_.power_in[i] = ewheel_power; + wheel_power_limitor_.power_in[i] = real_torque * real_vel + + wheel_power_limitor_.effort_coeff * square(real_torque) + + wheel_power_limitor_.vel_coeff * square(real_vel); + ewheel_power += wheel_power_limitor_.power_in[i]; } wheel_power_limitor_.power_sum = ewheel_power + wheel_power_limitor_.power_offset; // Publish power status. - auto publishPower = [](auto& pub, const double power) { - if (pub && pub->trylock()) - { - pub->msg_.data = power; - pub->unlockAndPublish(); - } - }; - - publishPower(epivot_power_pub_, epivot_power); - publishPower(ewheel_power_pub_, ewheel_power); + if (power_pub_ && power_pub_->trylock()) + { + power_pub_->msg_.data = wheel_power_limitor_.power_sum + pivot_power_limitor_.power_sum; + power_pub_->unlockAndPublish(); + } } PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::SwerveController, controller_interface::ControllerBase) From 7ce83ca4c4e084199122d5d6fcc27e0f72a98121 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sun, 12 Apr 2026 09:10:47 +0000 Subject: [PATCH 21/36] Add capacityCallback and fix bugs. --- .../rm_chassis_controllers/chassis_base.h | 7 ++- rm_chassis_controllers/src/chassis_base.cpp | 8 +++ rm_chassis_controllers/src/swerve.cpp | 62 ++++++++++++------- 3 files changed, 55 insertions(+), 22 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 8a1ae632..5f6a06ef 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -179,6 +179,7 @@ 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); void powerLimitReconfigCB(rm_chassis_controllers::PowerLimitConfig& config, uint32_t level); rm_control::RobotStateHandle robot_state_handle_{}; @@ -189,7 +190,8 @@ class ChassisBase : public controller_interface::MultiInterfaceController realtime_tools::RealtimeBuffer localization_rt_buffer_{}; realtime_tools::RealtimeBuffer power_limit_rt_buffer_; std::unique_ptr> odometry_rt_pub_; - std::unique_ptr> power_pub_; + std::unique_ptr> cpower_pub_; // command power publisher + std::unique_ptr> epower_pub_; // estimated power publisher dynamic_reconfigure::Server* power_limit_srv_{}; rm_common::TfRtBroadcaster brcst4global_map2robot_odom_{}; @@ -209,6 +211,7 @@ 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 }; @@ -224,6 +227,7 @@ 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 odom_initialized_{ false }; bool slam_updated_{ false }; @@ -239,6 +243,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController std::string lidar_base_frame_id_{ "livox_frame" }; std::string slam_topic_{ "/Odometry" }; std::string localization_topic_{ "/hdl_global_localization/result" }; + std::string capacity_topic_{ "/rm_referee/power_management/sample_and_status" }; ros::Time last_publish_time_{}; geometry_msgs::Vector3 vel_cmd_{}; // x, y diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index c8ba5013..eb278ef3 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -80,6 +80,8 @@ 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); // power publisher and dynamic reconfigure power_limit_srv_ = new dynamic_reconfigure::Server( @@ -487,6 +489,12 @@ 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; +} + template void ChassisBase::powerLimitReconfigCB(rm_chassis_controllers::PowerLimitConfig& config, uint32_t /*level*/) { diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index 64788679..6bde12fc 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -55,9 +55,12 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand filter = new LowPassFilter(5); } - auto power_publisher = + auto epower_publisher = std::make_unique>(controller_nh, "power", 100); - this->power_pub_ = std::move(power_publisher); + this->epower_pub_ = std::move(epower_publisher); + auto cpower_publisher = + std::make_unique>(controller_nh, "power", 100); + this->cpower_pub_ = std::move(cpower_publisher); auto base_gyro_publisher = std::make_unique>( controller_nh, "base_gyro", 100); this->base_gyro_pub_ = std::move(base_gyro_publisher); @@ -234,26 +237,33 @@ void SwerveController::updatePowerStatus() { double power_limit = cmd_rt_buffer_.readFromRT()->cmd_chassis_.power_limit; const auto& power_config = *power_limit_rt_buffer_.readFromRT(); + pivot_power_limitor_ = { .vel_coeff = power_config.pivot_vel_coeff, .effort_coeff = power_config.pivot_effort_coeff, .power_offset = 0, .max_power = std::min(power_config.pivot_max_power, power_limit * power_config.pivot_power_ratio), .ratio = power_config.pivot_power_ratio }; - double epivot_power{}; + + double epivot_power{}, cpivot_power{}; for (size_t i = 0; i < modules_.size() && i < 4; ++i) { auto& module = modules_[i]; - pivot_power_limitor_.torque[i] = module.ctrl_pivot_->joint_.getCommand(); + + 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(); - pivot_power_limitor_.power_in[i] = real_torque * real_vel + - pivot_power_limitor_.effort_coeff * square(real_torque) + - pivot_power_limitor_.vel_coeff * square(real_vel); - epivot_power += pivot_power_limitor_.power_in[i]; + + epivot_power += real_torque * real_vel + pivot_power_limitor_.effort_coeff * square(real_torque) + + pivot_power_limitor_.vel_coeff * square(real_vel); + cpivot_power += cmd_torque * real_vel + pivot_power_limitor_.effort_coeff * square(cmd_torque) + + pivot_power_limitor_.vel_coeff * square(real_vel); + + pivot_power_limitor_.power_in[i] = cpivot_power; } - pivot_power_limitor_.power_sum = epivot_power + pivot_power_limitor_.power_offset; + + pivot_power_limitor_.power_sum = cpivot_power + pivot_power_limitor_.power_offset; wheel_power_limitor_ = { .vel_coeff = power_config.vel_coeff, .effort_coeff = power_config.effort_coeff, @@ -263,31 +273,41 @@ void SwerveController::updatePowerStatus() .err_lower = 1e6, .err_sum = 0 }; - double ewheel_power{}; + double ewheel_power{}, cwheel_power{}; for (size_t i = 0; i < modules_.size() && i < 4; ++i) { auto& module = modules_[i]; - wheel_power_limitor_.torque[i] = module.ctrl_wheel_->joint_.getCommand(); + + 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]); - wheel_power_limitor_.power_in[i] = real_torque * real_vel + - wheel_power_limitor_.effort_coeff * square(real_torque) + - wheel_power_limitor_.vel_coeff * square(real_vel); - ewheel_power += wheel_power_limitor_.power_in[i]; + + ewheel_power += real_torque * real_vel + wheel_power_limitor_.effort_coeff * square(real_torque) + + wheel_power_limitor_.vel_coeff * square(real_vel); + cwheel_power += cmd_torque * real_vel + wheel_power_limitor_.effort_coeff * square(cmd_torque) + + wheel_power_limitor_.vel_coeff * square(real_vel); + + wheel_power_limitor_.power_in[i] = cwheel_power; } - wheel_power_limitor_.power_sum = ewheel_power + wheel_power_limitor_.power_offset; + wheel_power_limitor_.power_sum = cwheel_power + wheel_power_limitor_.power_offset; // Publish power status. - if (power_pub_ && power_pub_->trylock()) - { - power_pub_->msg_.data = wheel_power_limitor_.power_sum + pivot_power_limitor_.power_sum; - power_pub_->unlockAndPublish(); - } + auto publishPower = [](auto& pub, const double power) { + if (pub && pub->trylock()) + { + pub->msg_.data = power; + pub->unlockAndPublish(); + } + }; + + publishPower(epower_pub_, epivot_power + ewheel_power); + publishPower(cpower_pub_, cpivot_power + cwheel_power); } PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::SwerveController, controller_interface::ControllerBase) From b3924122ff1fb79203d4e94998baec0453e471a5 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sun, 12 Apr 2026 09:29:02 +0000 Subject: [PATCH 22/36] Fix omni. --- .../include/rm_chassis_controllers/omni.h | 3 + rm_chassis_controllers/src/omni.cpp | 59 +++++++++++-------- rm_chassis_controllers/src/swerve.cpp | 6 +- 3 files changed, 42 insertions(+), 26 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h index 471f2019..199d5e21 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" @@ -26,6 +27,8 @@ class OmniController : public ChassisBase motor_lp_filters_{}; }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/omni.cpp b/rm_chassis_controllers/src/omni.cpp index 9110619b..b48a5916 100644 --- a/rm_chassis_controllers/src/omni.cpp +++ b/rm_chassis_controllers/src/omni.cpp @@ -17,9 +17,17 @@ bool OmniController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle { ChassisBase::init(robot_hw, root_nh, controller_nh); - auto ewheel_power_publisher = - std::make_unique>(controller_nh, "power/ewheel_power", 100); - this->ewheel_power_pub_ = std::move(ewheel_power_publisher); + 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); + + for (auto& filter : motor_lp_filters_) + { + filter = new LowPassFilter(20); + } XmlRpc::XmlRpcValue wheels; controller_nh.getParam("wheels", wheels); @@ -123,9 +131,7 @@ void OmniController::powerLimit() if (Delta >= 0) { double Sqrt = sqrtf(Delta); - double cmd{}; - ctl->getCommand(cmd); - if (cmd >= 0) + if (wheel_power_limitor_.torque[i] >= 0) joint.setCommand((-B + Sqrt) / (2 * A)); else joint.setCommand((-B - Sqrt) / (2 * A)); @@ -143,32 +149,37 @@ void OmniController::updatePowerStatus() double power_limit = cmd_rt_buffer_.readFromRT()->cmd_chassis_.power_limit; const auto& power_config = *power_limit_rt_buffer_.readFromRT(); - wheel_power_limitor_ = { - .vel_coeff = power_config.vel_coeff, - .effort_coeff = power_config.effort_coeff, - .power_offset = power_config.power_offset, - .max_power = power_limit, - .err_upper = 4000, - .err_lower = 10, - }; + wheel_power_limitor_ = { .vel_coeff = power_config.vel_coeff, + .effort_coeff = power_config.effort_coeff, + .power_offset = power_config.power_offset, + .max_power = power_limit, + .err_upper = 4000, + .err_lower = 10, + .err_sum = 0 }; - double ewheel_power{}; + double ewheel_power{}, cwheel_power{}; for (size_t i = 0; i < joints_.size() && i < 4; ++i) { auto& ctl = joints_[i]; - wheel_power_limitor_.torque[i] = ctl->joint_.getCommand(); + + 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(); - double real_torque = ctl->joint_.getEffort(); + 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]); - wheel_power_limitor_.power_in[i] = real_torque * real_vel + - wheel_power_limitor_.effort_coeff * square(real_torque) + - wheel_power_limitor_.vel_coeff * square(real_vel); - ewheel_power += wheel_power_limitor_.power_in[i]; + + ewheel_power += real_torque * real_vel + wheel_power_limitor_.effort_coeff * square(real_torque) + + wheel_power_limitor_.vel_coeff * square(real_vel); + cwheel_power += cmd_torque * real_vel + wheel_power_limitor_.effort_coeff * square(cmd_torque) + + wheel_power_limitor_.vel_coeff * square(real_vel); + + wheel_power_limitor_.power_in[i] = cwheel_power; } - wheel_power_limitor_.power_sum = ewheel_power + wheel_power_limitor_.power_offset; + wheel_power_limitor_.power_sum = cwheel_power + wheel_power_limitor_.power_offset; // Publish power status. auto publishPower = [](auto& pub, const double power) { @@ -178,7 +189,9 @@ void OmniController::updatePowerStatus() pub->unlockAndPublish(); } }; - publishPower(ewheel_power_pub_, ewheel_power); + + publishPower(epower_pub_, ewheel_power); + publishPower(cpower_pub_, cwheel_power); } } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index 6bde12fc..2ded6049 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -52,14 +52,14 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand for (auto& filter_group : motor_lp_filters_) { for (auto& filter : filter_group) - filter = new LowPassFilter(5); + filter = new LowPassFilter(20); } auto epower_publisher = - std::make_unique>(controller_nh, "power", 100); + std::make_unique>(controller_nh, "power/estimated", 100); this->epower_pub_ = std::move(epower_publisher); auto cpower_publisher = - std::make_unique>(controller_nh, "power", 100); + std::make_unique>(controller_nh, "power/commanded", 100); this->cpower_pub_ = std::move(cpower_publisher); auto base_gyro_publisher = std::make_unique>( controller_nh, "base_gyro", 100); From 8c788a8ef33f4e1a7d70aa16c05a05993838da5b Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sun, 12 Apr 2026 12:20:03 +0000 Subject: [PATCH 23/36] Add cap flag. --- .../rm_chassis_controllers/chassis_base.h | 4 +++- rm_chassis_controllers/src/chassis_base.cpp | 1 + rm_chassis_controllers/src/omni.cpp | 11 ++++++----- rm_chassis_controllers/src/swerve.cpp | 17 +++++++++-------- 4 files changed, 19 insertions(+), 14 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 5f6a06ef..0367788d 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -79,7 +79,8 @@ struct PowerLimitor double power_offset{}; double max_power{}; double power_in[4]{}; - double power_sum{}; + double cmd_power{}; + double estimated_power{}; double power_limit[4]{}; double ratio{ 1.0 }; double err[4]{}; @@ -228,6 +229,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController double max_odom_vel_{ 10.0 }; double timeout_{ 0.1 }; double chassis_power_{ 0.0 }; + bool capacity_update_flag_{ false }; bool odom_initialized_{ false }; bool slam_updated_{ false }; diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index eb278ef3..0cfef958 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -493,6 +493,7 @@ template void ChassisBase::capacityCallback(const rm_msgs::PowerManagementSampleAndStatusData::ConstPtr& msg) { chassis_power_ = msg->chassis_power + msg->capacity_discharge_power; + capacity_update_flag_ = true; } template diff --git a/rm_chassis_controllers/src/omni.cpp b/rm_chassis_controllers/src/omni.cpp index b48a5916..0655583d 100644 --- a/rm_chassis_controllers/src/omni.cpp +++ b/rm_chassis_controllers/src/omni.cpp @@ -111,11 +111,11 @@ void OmniController::powerLimit() { 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_.power_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_.power_sum > 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) { @@ -179,7 +179,8 @@ void OmniController::updatePowerStatus() wheel_power_limitor_.power_in[i] = cwheel_power; } - wheel_power_limitor_.power_sum = cwheel_power + wheel_power_limitor_.power_offset; + wheel_power_limitor_.cmd_power = cwheel_power + wheel_power_limitor_.power_offset; + wheel_power_limitor_.estimated_power = ewheel_power + wheel_power_limitor_.power_offset; // Publish power status. auto publishPower = [](auto& pub, const double power) { @@ -190,8 +191,8 @@ void OmniController::updatePowerStatus() } }; - publishPower(epower_pub_, ewheel_power); - publishPower(cpower_pub_, cwheel_power); + publishPower(epower_pub_, wheel_power_limitor_.estimated_power); + publishPower(cpower_pub_, wheel_power_limitor_.cmd_power); } } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index 2ded6049..a8e076c2 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -167,17 +167,17 @@ void SwerveController::powerLimit() // 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_.power_sum; + 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_.power_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_.power_sum > pivot_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) { @@ -204,7 +204,7 @@ void SwerveController::powerLimit() } } } - if (wheel_power_limitor_.power_sum > wheel_power_limitor_.max_power) + if (wheel_power_limitor_.cmd_power > wheel_power_limitor_.max_power) { for (size_t i = 0; i < modules_.size() && i < 4; ++i) { @@ -263,12 +263,13 @@ void SwerveController::updatePowerStatus() pivot_power_limitor_.power_in[i] = cpivot_power; } - pivot_power_limitor_.power_sum = cpivot_power + pivot_power_limitor_.power_offset; + 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_ = { .vel_coeff = power_config.vel_coeff, .effort_coeff = power_config.effort_coeff, .power_offset = power_config.power_offset, - .max_power = power_limit - std::abs(pivot_power_limitor_.power_sum), + .max_power = power_limit - std::abs(pivot_power_limitor_.cmd_power), .err_upper = 1e8, .err_lower = 1e6, .err_sum = 0 }; @@ -306,8 +307,8 @@ void SwerveController::updatePowerStatus() } }; - publishPower(epower_pub_, epivot_power + ewheel_power); - publishPower(cpower_pub_, cpivot_power + cwheel_power); + publishPower(epower_pub_, pivot_power_limitor_.estimated_power + wheel_power_limitor_.estimated_power); + publishPower(cpower_pub_, pivot_power_limitor_.cmd_power + wheel_power_limitor_.cmd_power); } PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::SwerveController, controller_interface::ControllerBase) From c6cec04d7d878d85dd107a6576eb9485acc82f76 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Mon, 13 Apr 2026 08:46:25 +0000 Subject: [PATCH 24/36] Add rls. --- .../include/rm_chassis_controllers/swerve.h | 1 + rm_chassis_controllers/src/chassis_base.cpp | 4 +- rm_chassis_controllers/src/swerve.cpp | 87 +++++++++++++++---- 3 files changed, 71 insertions(+), 21 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h index 08d2b3c3..9bbc647d 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h @@ -80,6 +80,7 @@ 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 0cfef958..d285becc 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -492,8 +492,8 @@ void ChassisBase::localizationCallback(const geometry_msgs::TransformStamp template void ChassisBase::capacityCallback(const rm_msgs::PowerManagementSampleAndStatusData::ConstPtr& msg) { - chassis_power_ = msg->chassis_power + msg->capacity_discharge_power; - capacity_update_flag_ = true; + // chassis_power_ = msg->chassis_power + msg->capacity_discharge_power; + // capacity_update_flag_ = true; } template diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index a8e076c2..5c217f2b 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -48,6 +48,33 @@ 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); + } + 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_) { @@ -55,12 +82,14 @@ bool SwerveController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand 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); @@ -236,14 +265,8 @@ void SwerveController::powerLimit() void SwerveController::updatePowerStatus() { double power_limit = cmd_rt_buffer_.readFromRT()->cmd_chassis_.power_limit; - const auto& power_config = *power_limit_rt_buffer_.readFromRT(); - pivot_power_limitor_ = { .vel_coeff = power_config.pivot_vel_coeff, - .effort_coeff = power_config.pivot_effort_coeff, - .power_offset = 0, - .max_power = - std::min(power_config.pivot_max_power, power_limit * power_config.pivot_power_ratio), - .ratio = power_config.pivot_power_ratio }; + 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) @@ -260,19 +283,15 @@ void SwerveController::updatePowerStatus() cpivot_power += cmd_torque * real_vel + pivot_power_limitor_.effort_coeff * square(cmd_torque) + pivot_power_limitor_.vel_coeff * square(real_vel); - pivot_power_limitor_.power_in[i] = cpivot_power; + pivot_power_limitor_.power_in[i] = cmd_torque * real_vel + pivot_power_limitor_.effort_coeff * square(cmd_torque) + + pivot_power_limitor_.vel_coeff * square(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_ = { .vel_coeff = power_config.vel_coeff, - .effort_coeff = power_config.effort_coeff, - .power_offset = power_config.power_offset, - .max_power = power_limit - std::abs(pivot_power_limitor_.cmd_power), - .err_upper = 1e8, - .err_lower = 1e6, - .err_sum = 0 }; + 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) @@ -294,9 +313,39 @@ void SwerveController::updatePowerStatus() cwheel_power += cmd_torque * real_vel + wheel_power_limitor_.effort_coeff * square(cmd_torque) + wheel_power_limitor_.vel_coeff * square(real_vel); - wheel_power_limitor_.power_in[i] = cwheel_power; + wheel_power_limitor_.power_in[i] = cmd_torque * real_vel + wheel_power_limitor_.effort_coeff * square(cmd_torque) + + wheel_power_limitor_.vel_coeff * square(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_) + { + // Update Rls. + double all_in = limit(estimated_total_power, -power_limit, power_limit); + 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) = square(pivot_power_limitor_.omiga[0]) + square(pivot_power_limitor_.omiga[1]) + + square(pivot_power_limitor_.omiga[2]) + square(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) = square(wheel_power_limitor_.omiga[0]) + square(wheel_power_limitor_.omiga[1]) + + square(wheel_power_limitor_.omiga[2]) + square(wheel_power_limitor_.omiga[3]); + rls_->setU(all_in); + rls_->setX(x); + rls_->setY(estimated_total_power); // Todo: use chasssis power from capacity topic for better estimation. + rls_->update(); + auto w = rls_->getW(); + pivot_power_limitor_.effort_coeff = w(0); + pivot_power_limitor_.vel_coeff = w(1); + wheel_power_limitor_.effort_coeff = w(2); + wheel_power_limitor_.vel_coeff = w(3); + capacity_update_flag_ = false; } - wheel_power_limitor_.power_sum = cwheel_power + wheel_power_limitor_.power_offset; // Publish power status. auto publishPower = [](auto& pub, const double power) { @@ -307,8 +356,8 @@ void SwerveController::updatePowerStatus() } }; - publishPower(epower_pub_, pivot_power_limitor_.estimated_power + wheel_power_limitor_.estimated_power); - publishPower(cpower_pub_, pivot_power_limitor_.cmd_power + wheel_power_limitor_.cmd_power); + publishPower(epower_pub_, estimated_total_power); + publishPower(cpower_pub_, cmd_total_power); } PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::SwerveController, controller_interface::ControllerBase) From 6ce70a0564cf91459abe1fea738709eba8c0924a Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Mon, 13 Apr 2026 12:56:07 +0000 Subject: [PATCH 25/36] Remove dynamic config. --- rm_chassis_controllers/CMakeLists.txt | 1 - rm_chassis_controllers/cfg/PowerLimit.cfg | 16 ----- .../rm_chassis_controllers/chassis_base.h | 8 +-- .../include/rm_chassis_controllers/omni.h | 2 + rm_chassis_controllers/src/chassis_base.cpp | 61 +++++++++++-------- rm_chassis_controllers/src/omni.cpp | 37 +++++++---- 6 files changed, 67 insertions(+), 58 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 726a12e4..704e5deb 100644 --- a/rm_chassis_controllers/CMakeLists.txt +++ b/rm_chassis_controllers/CMakeLists.txt @@ -28,7 +28,6 @@ find_package(Eigen3 REQUIRED) generate_dynamic_reconfigure_options( cfg/LQRWeight.cfg - cfg/PowerLimit.cfg ) ################################### diff --git a/rm_chassis_controllers/cfg/PowerLimit.cfg b/rm_chassis_controllers/cfg/PowerLimit.cfg deleted file mode 100644 index 7ad87891..00000000 --- a/rm_chassis_controllers/cfg/PowerLimit.cfg +++ /dev/null @@ -1,16 +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, 0.100) -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) -gen.add("pivot_max_power", double_t, 0, "maximum power for pivot joints", 0.0, 0.0, 100.0) -gen.add("pivot_effort_coeff", double_t, 0, "effort coefficient for pivot power limit", 0.0, 0.0, 100) -gen.add("pivot_vel_coeff", double_t, 0, "velocity coefficient for pivot power limit", 0.000, 0.000, 10) -gen.add("pivot_power_ratio", double_t, 0, "power ratio for pivot joints", 0.0, 0.0, 1.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 0367788d..7d3101ef 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -58,8 +58,6 @@ #include #include #include -#include -#include #include namespace rm_chassis_controllers @@ -181,7 +179,6 @@ class ChassisBase : public controller_interface::MultiInterfaceController void slamCallback(const nav_msgs::Odometry::ConstPtr& msg); void localizationCallback(const geometry_msgs::TransformStamped::ConstPtr& msg); void capacityCallback(const rm_msgs::PowerManagementSampleAndStatusData::ConstPtr& msg); - void powerLimitReconfigCB(rm_chassis_controllers::PowerLimitConfig& config, uint32_t level); rm_control::RobotStateHandle robot_state_handle_{}; hardware_interface::EffortJointInterface* effort_joint_interface_{}; @@ -189,18 +186,18 @@ class ChassisBase : public controller_interface::MultiInterfaceController realtime_tools::RealtimeBuffer cmd_rt_buffer_{}; realtime_tools::RealtimeBuffer slam_rt_buffer_{}; realtime_tools::RealtimeBuffer localization_rt_buffer_{}; - realtime_tools::RealtimeBuffer power_limit_rt_buffer_; std::unique_ptr> odometry_rt_pub_; std::unique_ptr> cpower_pub_; // command power publisher std::unique_ptr> epower_pub_; // estimated power publisher - dynamic_reconfigure::Server* power_limit_srv_{}; rm_common::TfRtBroadcaster brcst4global_map2robot_odom_{}; rm_common::TfRtBroadcaster brcst4robot_odom2robot_base_{}; + rm_common::TfRtBroadcaster brcst4global_map2camera_init_{}; geometry_msgs::TransformStamped global_map2robot_odom_{}; geometry_msgs::TransformStamped robot_odom2robot_base_{}; geometry_msgs::TransformStamped robot_base2lidar_base_{}; + geometry_msgs::TransformStamped global_map2camera_init_{}; tf2::Transform T_global_map2robot_odom_{}; tf2::Transform T_robot_odom_2robot_base_{}; @@ -231,6 +228,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController double chassis_power_{ 0.0 }; bool capacity_update_flag_{ false }; + bool gravity_estimation_offset_{ false }; bool odom_initialized_{ false }; bool slam_updated_{ false }; bool localization_updated_{ false }; diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h index 199d5e21..0a70aea4 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h @@ -6,6 +6,7 @@ #include #include +#include #include "rm_chassis_controllers/chassis_base.h" @@ -29,6 +30,7 @@ class OmniController : public ChassisBase 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 d285becc..14e96c35 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -50,6 +50,7 @@ void ChassisBase::initialize_parameters(ros::NodeHandle& controller_nh) controller_nh.getParam("publish_rate", publish_rate_); controller_nh.getParam("publish_map_tf", publish_map_tf_); controller_nh.getParam("publish_odom_tf", publish_odom_tf_); + controller_nh.getParam("gravity_estimation_offset", gravity_estimation_offset_); controller_nh.getParam("slam_topic", slam_topic_); controller_nh.getParam("localization_topic", localization_topic_); @@ -83,13 +84,6 @@ bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan capacity_sub_ = root_nh.subscribe(capacity_topic_, 10, &ChassisBase::capacityCallback, this); - // power publisher and dynamic reconfigure - power_limit_srv_ = new dynamic_reconfigure::Server( - ros::NodeHandle(controller_nh, "power")); - dynamic_reconfigure::Server::CallbackType cb = - boost::bind(&ChassisBase::powerLimitReconfigCB, this, _1, _2); - power_limit_srv_->setCallback(cb); - // Setup odometry realtime publisher + odom message constant fields auto odometry_publisher = std::make_unique>(root_nh, "odom", 100); @@ -112,6 +106,13 @@ bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan global_map2robot_odom_.transform.rotation.w = 1; brcst4global_map2robot_odom_.init(root_nh); brcst4global_map2robot_odom_.sendTransform(global_map2robot_odom_); + + global_map2camera_init_.header.stamp = ros::Time::now(); + global_map2camera_init_.header.frame_id = global_map_frame_id_; + global_map2camera_init_.child_frame_id = "camera_init"; + global_map2camera_init_.transform.rotation.w = 1; + brcst4global_map2camera_init_.init(root_nh); + brcst4global_map2camera_init_.sendTransform(global_map2camera_init_); } if (publish_odom_tf_) @@ -285,9 +286,18 @@ void ChassisBase::updateOdom(const ros::Time& time, const ros::Duration& p T_global_map2lidar_odom_.setOrigin(tf2::Vector3(global_map2lidar_odom.transform.translation.x, global_map2lidar_odom.transform.translation.y, global_map2lidar_odom.transform.translation.z)); - T_global_map2lidar_odom_.setRotation( - tf2::Quaternion(global_map2lidar_odom.transform.rotation.x, global_map2lidar_odom.transform.rotation.y, - global_map2lidar_odom.transform.rotation.z, global_map2lidar_odom.transform.rotation.w)); + if (gravity_estimation_offset_) + { + T_global_map2lidar_odom_.setRotation(tf2::Quaternion(0, 0, 0, 1)); + global_map2camera_init_.transform.translation = global_map2lidar_odom.transform.translation; + } + else + { + T_global_map2lidar_odom_.setRotation( + tf2::Quaternion(global_map2lidar_odom.transform.rotation.x, global_map2lidar_odom.transform.rotation.y, + global_map2lidar_odom.transform.rotation.z, global_map2lidar_odom.transform.rotation.w)); + global_map2camera_init_.transform = global_map2lidar_odom.transform; + } odom_initialized_ = true; } catch (...) @@ -308,12 +318,14 @@ void ChassisBase::updateOdom(const ros::Time& time, const ros::Duration& p T_global_map2lidar_odom_.setRotation( tf2::Quaternion(localization->transform.rotation.x, localization->transform.rotation.y, localization->transform.rotation.z, localization->transform.rotation.w)); + global_map2camera_init_.transform = tf2::toMsg(T_global_map2lidar_odom_); } catch (...) { ROS_WARN("Failed to update localization offset."); } } + ros::Time tmp_time = ros::Time::now(); if (slam_updated_) { @@ -321,6 +333,7 @@ void ChassisBase::updateOdom(const ros::Time& time, const ros::Duration& p { slam_updated_ = false; const auto& slam = slam_rt_buffer_.readFromRT(); + tmp_time = slam->header.stamp; T_lidar_odom2lidar_base_.setOrigin( tf2::Vector3(slam->pose.pose.position.x, slam->pose.pose.position.y, slam->pose.pose.position.z)); T_lidar_odom2lidar_base_.setRotation( @@ -328,7 +341,7 @@ void ChassisBase::updateOdom(const ros::Time& time, const ros::Duration& p slam->pose.pose.orientation.w)); robot_base2lidar_base_ = - robot_state_handle_.lookupTransform(robot_base_frame_id_, lidar_base_frame_id_, ros::Time(0)); + robot_state_handle_.lookupTransform(robot_base_frame_id_, lidar_base_frame_id_, slam->header.stamp); T_robot_base2lidar_base_.setOrigin(tf2::Vector3(robot_base2lidar_base_.transform.translation.x, robot_base2lidar_base_.transform.translation.y, robot_base2lidar_base_.transform.translation.z)); @@ -336,12 +349,15 @@ void ChassisBase::updateOdom(const ros::Time& time, const ros::Duration& p tf2::Quaternion(robot_base2lidar_base_.transform.rotation.x, robot_base2lidar_base_.transform.rotation.y, robot_base2lidar_base_.transform.rotation.z, robot_base2lidar_base_.transform.rotation.w)); - T_robot_odom_2robot_base_.setOrigin(tf2::Vector3(robot_odom2robot_base_.transform.translation.x, - robot_odom2robot_base_.transform.translation.y, - robot_odom2robot_base_.transform.translation.z)); - T_robot_odom_2robot_base_.setRotation( - tf2::Quaternion(robot_odom2robot_base_.transform.rotation.x, robot_odom2robot_base_.transform.rotation.y, - robot_odom2robot_base_.transform.rotation.z, robot_odom2robot_base_.transform.rotation.w)); + auto tmp_robot_odom2robot_base = + robot_state_handle_.lookupTransform(robot_odom_frame_id_, robot_base_frame_id_, slam->header.stamp); + + T_robot_odom_2robot_base_.setOrigin(tf2::Vector3(tmp_robot_odom2robot_base.transform.translation.x, + tmp_robot_odom2robot_base.transform.translation.y, + tmp_robot_odom2robot_base.transform.translation.z)); + T_robot_odom_2robot_base_.setRotation(tf2::Quaternion( + tmp_robot_odom2robot_base.transform.rotation.x, tmp_robot_odom2robot_base.transform.rotation.y, + tmp_robot_odom2robot_base.transform.rotation.z, tmp_robot_odom2robot_base.transform.rotation.w)); T_global_map2robot_odom_ = T_global_map2lidar_odom_ * T_lidar_odom2lidar_base_ * T_robot_base2lidar_base_.inverse() * T_robot_odom_2robot_base_.inverse(); @@ -353,7 +369,8 @@ void ChassisBase::updateOdom(const ros::Time& time, const ros::Duration& p ROS_WARN("Failed to update global_map2robot_odom."); } } - global_map2robot_odom_.header.stamp = time; + global_map2robot_odom_.header.stamp = tmp_time; + global_map2camera_init_.header.stamp = tmp_time; } if (publish_odom_tf_) @@ -416,6 +433,7 @@ void ChassisBase::updateOdom(const ros::Time& time, const ros::Duration& p if (publish_map_tf_) { brcst4global_map2robot_odom_.sendTransform(global_map2robot_odom_); + brcst4global_map2camera_init_.sendTransform(global_map2camera_init_); } if (publish_odom_tf_) @@ -496,11 +514,4 @@ void ChassisBase::capacityCallback(const rm_msgs::PowerManagementSampleAnd // capacity_update_flag_ = true; } -template -void ChassisBase::powerLimitReconfigCB(rm_chassis_controllers::PowerLimitConfig& config, uint32_t /*level*/) -{ - ROS_INFO("[Power Limit] Dynamic params change"); - power_limit_rt_buffer_.writeFromNonRT(config); -} - } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/omni.cpp b/rm_chassis_controllers/src/omni.cpp index 0655583d..651cbc51 100644 --- a/rm_chassis_controllers/src/omni.cpp +++ b/rm_chassis_controllers/src/omni.cpp @@ -17,6 +17,23 @@ 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); + } + 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 = 4000; + wheel_power_limitor_.err_lower = 10; + auto epower_publisher = std::make_unique>(controller_nh, "power/estimated", 100); this->epower_pub_ = std::move(epower_publisher); @@ -147,15 +164,9 @@ void OmniController::powerLimit() void OmniController::updatePowerStatus() { double power_limit = cmd_rt_buffer_.readFromRT()->cmd_chassis_.power_limit; - const auto& power_config = *power_limit_rt_buffer_.readFromRT(); - wheel_power_limitor_ = { .vel_coeff = power_config.vel_coeff, - .effort_coeff = power_config.effort_coeff, - .power_offset = power_config.power_offset, - .max_power = power_limit, - .err_upper = 4000, - .err_lower = 10, - .err_sum = 0 }; + 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) @@ -177,11 +188,15 @@ void OmniController::updatePowerStatus() cwheel_power += cmd_torque * real_vel + wheel_power_limitor_.effort_coeff * square(cmd_torque) + wheel_power_limitor_.vel_coeff * square(real_vel); - wheel_power_limitor_.power_in[i] = cwheel_power; + wheel_power_limitor_.power_in[i] = cmd_torque * real_vel + wheel_power_limitor_.effort_coeff * square(cmd_torque) + + wheel_power_limitor_.vel_coeff * square(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; + // Publish power status. auto publishPower = [](auto& pub, const double power) { if (pub && pub->trylock()) @@ -191,8 +206,8 @@ void OmniController::updatePowerStatus() } }; - publishPower(epower_pub_, wheel_power_limitor_.estimated_power); - publishPower(cpower_pub_, wheel_power_limitor_.cmd_power); + publishPower(epower_pub_, estimated_total_power); + publishPower(cpower_pub_, cmd_total_power); } } // namespace rm_chassis_controllers From cb4207799f93af95834a44531b0e185c2cf3bbb9 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Tue, 14 Apr 2026 03:19:17 +0000 Subject: [PATCH 26/36] Add rls in omni. --- .../rm_chassis_controllers/chassis_base.h | 2 ++ rm_chassis_controllers/src/chassis_base.cpp | 14 ++++++-- rm_chassis_controllers/src/omni.cpp | 36 +++++++++++++++---- rm_chassis_controllers/src/swerve.cpp | 5 +-- 4 files changed, 47 insertions(+), 10 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 7d3101ef..feee1d50 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -189,6 +189,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController std::unique_ptr> odometry_rt_pub_; std::unique_ptr> cpower_pub_; // command power publisher std::unique_ptr> epower_pub_; // estimated power publisher + std::unique_ptr> chassis_power_pub_; // chassis power publisher rm_common::TfRtBroadcaster brcst4global_map2robot_odom_{}; rm_common::TfRtBroadcaster brcst4robot_odom2robot_base_{}; @@ -227,6 +228,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController double timeout_{ 0.1 }; double chassis_power_{ 0.0 }; bool capacity_update_flag_{ false }; + bool use_rls_{ false }; bool gravity_estimation_offset_{ false }; bool odom_initialized_{ false }; diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 14e96c35..27776d61 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -84,6 +84,11 @@ bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan 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); + // Setup odometry realtime publisher + odom message constant fields auto odometry_publisher = std::make_unique>(root_nh, "odom", 100); @@ -510,8 +515,13 @@ void ChassisBase::localizationCallback(const geometry_msgs::TransformStamp template void ChassisBase::capacityCallback(const rm_msgs::PowerManagementSampleAndStatusData::ConstPtr& msg) { - // chassis_power_ = msg->chassis_power + msg->capacity_discharge_power; - // capacity_update_flag_ = true; + 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 651cbc51..21e3fb9d 100644 --- a/rm_chassis_controllers/src/omni.cpp +++ b/rm_chassis_controllers/src/omni.cpp @@ -23,6 +23,7 @@ bool OmniController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle 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("use_rls", use_rls_, false); } catch (const std::exception& e) { @@ -30,10 +31,19 @@ bool OmniController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle return false; } - // pivot don't need err to multiply power. wheel_power_limitor_.err_upper = 4000; wheel_power_limitor_.err_lower = 10; + 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); @@ -41,11 +51,6 @@ bool OmniController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle std::make_unique>(controller_nh, "power/commanded", 100); this->cpower_pub_ = std::move(cpower_publisher); - for (auto& filter : motor_lp_filters_) - { - filter = new LowPassFilter(20); - } - XmlRpc::XmlRpcValue wheels; controller_nh.getParam("wheels", wheels); chassis2joints_.resize(wheels.size(), 3); @@ -197,6 +202,25 @@ 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_) + { + // Update Rls. + double all_in = limit(estimated_total_power, -power_limit, power_limit); + 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) = square(wheel_power_limitor_.omiga[0]) + square(wheel_power_limitor_.omiga[1]) + + square(wheel_power_limitor_.omiga[2]) + square(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 = w(0); + wheel_power_limitor_.vel_coeff = w(1); + capacity_update_flag_ = false; + } + // Publish power status. auto publishPower = [](auto& pub, const double power) { if (pub && pub->trylock()) diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index 5c217f2b..f4f54d91 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -58,6 +58,7 @@ 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("use_rls", use_rls_, false); } catch (const std::exception& e) { @@ -322,7 +323,7 @@ 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_) + if (capacity_update_flag_ && use_rls_) { // Update Rls. double all_in = limit(estimated_total_power, -power_limit, power_limit); @@ -337,7 +338,7 @@ void SwerveController::updatePowerStatus() square(wheel_power_limitor_.omiga[2]) + square(wheel_power_limitor_.omiga[3]); rls_->setU(all_in); rls_->setX(x); - rls_->setY(estimated_total_power); // Todo: use chasssis power from capacity topic for better estimation. + rls_->setY(chassis_power_); rls_->update(); auto w = rls_->getW(); pivot_power_limitor_.effort_coeff = w(0); From 0e5b95b1af9d61b7efe2abe7631a02bf612a315e Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Tue, 14 Apr 2026 07:51:18 +0000 Subject: [PATCH 27/36] Add uphill. --- .../rm_chassis_controllers/chassis_base.h | 1 + .../include/rm_chassis_controllers/swerve.h | 2 +- rm_chassis_controllers/src/swerve.cpp | 60 ++++++++++++++++++- 3 files changed, 59 insertions(+), 4 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 feee1d50..7680b056 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -88,6 +88,7 @@ struct PowerLimitor double omiga[4]{}; double torque[4]{}; double K{}; + double K_angle[4]{}; }; template diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h index 9bbc647d..bdf775b3 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/swerve.h @@ -72,7 +72,7 @@ class SwerveController : public ChassisBase modules_{}; std::vector pivot_joint_handles_{}; diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index f4f54d91..6cfe6d1e 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -133,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_) { @@ -173,6 +174,50 @@ geometry_msgs::Twist SwerveController::odometry() return vel_data; } +void SwerveController::stateJudge() +{ + double cos_pitch{}, sin_pitch{}; + ROS_INFO("pitch: %f,yaw: %f,roll: %f", pitch_, yaw_, roll_); + if (abs(pitch_) > 0.12) + { + cos_pitch = cos(pitch_); + sin_pitch = sin(pitch_); + } + else + { + cos_pitch = 1.0; + 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] = cos_pitch + sin_pitch; + } + if (module.ctrl_wheel_->getJointName().find("right") != std::string::npos) + { + wheel_power_limitor_.K_angle[i] = cos_pitch + 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] = cos_pitch - sin_pitch; + } + if (module.ctrl_wheel_->getJointName().find("right") != std::string::npos) + { + wheel_power_limitor_.K_angle[i] = cos_pitch - sin_pitch; + } + } + } +} + // Ref: https://gitee.com/cod_-control/rmcod2026_-sentry/tree/dev // https://github.com/hkustenterprize/RM2024-PowerModule @@ -251,16 +296,25 @@ void SwerveController::powerLimit() { double Sqrt = sqrtf(Delta); if (wheel_power_limitor_.torque[i] >= 0) - joint.setCommand((-B + Sqrt) / (2 * A)); + joint.setCommand(((-B + Sqrt) / (2 * A)) * wheel_power_limitor_.K_angle[i]); else - joint.setCommand((-B - Sqrt) / (2 * A)); + joint.setCommand(((-B - Sqrt) / (2 * A)) * wheel_power_limitor_.K_angle[i]); } else { - joint.setCommand((-B) / (2 * A)); + 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() From b4c14fe9f1e1536cb9480e4ee0aa29f40544261f Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sat, 18 Apr 2026 03:48:52 +0000 Subject: [PATCH 28/36] Add omni uphill. --- .../rm_chassis_controllers/chassis_base.h | 1 + .../include/rm_chassis_controllers/omni.h | 1 + rm_chassis_controllers/src/chassis_base.cpp | 2 +- rm_chassis_controllers/src/omni.cpp | 71 +++++++++++++++++-- rm_chassis_controllers/src/swerve.cpp | 12 +++- 5 files changed, 80 insertions(+), 7 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 7680b056..190aabcc 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -230,6 +230,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController double chassis_power_{ 0.0 }; bool capacity_update_flag_{ false }; bool use_rls_{ false }; + bool use_K_angle_{ false }; bool gravity_estimation_offset_{ false }; bool odom_initialized_{ false }; diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h index 0a70aea4..048de675 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h @@ -28,6 +28,7 @@ class OmniController : public ChassisBase motor_lp_filters_{}; std::unique_ptr> rls_{}; diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 27776d61..94e37b00 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -86,7 +86,7 @@ bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan // Setup real_power from capacity publishers. auto chassis_power_publisher = std::make_unique>( - controller_nh, "/power/chassis_power", 100); + controller_nh, "power/chassis_power", 100); this->chassis_power_pub_ = std::move(chassis_power_publisher); // Setup odometry realtime publisher + odom message constant fields diff --git a/rm_chassis_controllers/src/omni.cpp b/rm_chassis_controllers/src/omni.cpp index 21e3fb9d..9a86127c 100644 --- a/rm_chassis_controllers/src/omni.cpp +++ b/rm_chassis_controllers/src/omni.cpp @@ -23,7 +23,8 @@ bool OmniController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle 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("use_rls", use_rls_, false); + 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) { @@ -111,9 +112,62 @@ 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 cos_pitch{}, sin_pitch{}; + if (abs(pitch_) > 0.12) + { + cos_pitch = cos(pitch_); + sin_pitch = sin(pitch_); + } + else + { + cos_pitch = 1.0; + 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] = cos_pitch + sin_pitch; + } + if (ctl->getJointName().find("right") != std::string::npos) + { + wheel_power_limitor_.K_angle[i] = cos_pitch + 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] = cos_pitch - sin_pitch; + } + if (ctl->getJointName().find("right") != std::string::npos) + { + wheel_power_limitor_.K_angle[i] = cos_pitch - 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) { @@ -154,16 +208,25 @@ void OmniController::powerLimit() { double Sqrt = sqrtf(Delta); if (wheel_power_limitor_.torque[i] >= 0) - joint.setCommand((-B + Sqrt) / (2 * A)); + joint.setCommand(((-B + Sqrt) / (2 * A)) * wheel_power_limitor_.K_angle[i]); else - joint.setCommand((-B - Sqrt) / (2 * A)); + joint.setCommand(((-B - Sqrt) / (2 * A)) * wheel_power_limitor_.K_angle[i]); } else { - joint.setCommand((-B) / (2 * A)); + 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() diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index 6cfe6d1e..d706dec6 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -58,7 +58,8 @@ 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("use_rls", use_rls_, false); + 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) { @@ -176,8 +177,15 @@ geometry_msgs::Twist SwerveController::odometry() 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 cos_pitch{}, sin_pitch{}; - ROS_INFO("pitch: %f,yaw: %f,roll: %f", pitch_, yaw_, roll_); if (abs(pitch_) > 0.12) { cos_pitch = cos(pitch_); From 6ae7b6b5e852c3649b43e76eaf57957dc7f039df Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sat, 25 Apr 2026 08:33:57 +0000 Subject: [PATCH 29/36] Fix error. --- .../include/rm_chassis_controllers/chassis_base.h | 1 - rm_chassis_controllers/include/rm_chassis_controllers/omni.h | 2 +- rm_chassis_controllers/src/chassis_base.cpp | 4 ++-- rm_chassis_controllers/src/omni.cpp | 1 - 4 files changed, 3 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 190aabcc..d309b6c7 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -66,7 +66,6 @@ struct Command { geometry_msgs::Twist cmd_vel_; rm_msgs::ChassisCmd cmd_chassis_; - rm_msgs::ChassisActiveSusCmd cmd_active_sus_; ros::Time stamp_; }; diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h index 048de675..8c482f03 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h @@ -28,7 +28,7 @@ class OmniController : public ChassisBase motor_lp_filters_{}; std::unique_ptr> rls_{}; diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 94e37b00..3dce6b4e 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -85,8 +85,8 @@ bool ChassisBase::init(hardware_interface::RobotHW* robot_hw, ros::NodeHan &ChassisBase::capacityCallback, this); // Setup real_power from capacity publishers. - auto chassis_power_publisher = std::make_unique>( - controller_nh, "power/chassis_power", 100); + auto chassis_power_publisher = + std::make_unique>(controller_nh, "power/chassis_power", 100); this->chassis_power_pub_ = std::move(chassis_power_publisher); // Setup odometry realtime publisher + odom message constant fields diff --git a/rm_chassis_controllers/src/omni.cpp b/rm_chassis_controllers/src/omni.cpp index 9a86127c..0b29a7ba 100644 --- a/rm_chassis_controllers/src/omni.cpp +++ b/rm_chassis_controllers/src/omni.cpp @@ -137,7 +137,6 @@ void OmniController::stateJudge() for (size_t i = 0; i < joints_.size() && i < 4; ++i) { auto& ctl = joints_[i]; - ; if (ctl->joint_.getName().find("front") != std::string::npos) { From 8723e33b0a3881778f28f2f412fb53e4baac2e6b Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Thu, 30 Apr 2026 15:26:29 +0000 Subject: [PATCH 30/36] Fix. --- rm_gimbal_controllers/src/ballistic_solver.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rm_gimbal_controllers/src/ballistic_solver.cpp b/rm_gimbal_controllers/src/ballistic_solver.cpp index 97131f3b..f9c8284d 100644 --- a/rm_gimbal_controllers/src/ballistic_solver.cpp +++ b/rm_gimbal_controllers/src/ballistic_solver.cpp @@ -64,9 +64,9 @@ void BallisticSolver::solver(const geometry_msgs::TransformStamped& base2gimbal, double error_after_step = error_function(current_pitch + config.newton_pitch_epsilon); double error_derivative = (error_after_step - error) / config.newton_pitch_epsilon; double pitch_adjustment = error / error_derivative; - pitch_adjustment = (pitch_adjustment < -config.max_newton_step) ? -config.max_newton_step : - (pitch_adjustment > config.max_newton_step) ? config.max_newton_step : - pitch_adjustment; + pitch_adjustment = (pitch_adjustment < -config.max_newton_step) ? + -config.max_newton_step : + (pitch_adjustment > config.max_newton_step) ? config.max_newton_step : pitch_adjustment; double update_pitch = current_pitch - pitch_adjustment; double update_error = error_function(update_pitch); if (std::abs(update_error) < config.newton_convergence_tol) From 6c66deddf5bc5896df9614b4b3d21ae8c5814131 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Mon, 11 May 2026 04:24:18 +0000 Subject: [PATCH 31/36] Fix rls. Co-authored-by: Copilot --- rm_chassis_controllers/src/omni.cpp | 12 ++++++------ rm_chassis_controllers/src/swerve.cpp | 15 +++++++-------- 2 files changed, 13 insertions(+), 14 deletions(-) diff --git a/rm_chassis_controllers/src/omni.cpp b/rm_chassis_controllers/src/omni.cpp index 0b29a7ba..fac9d1d5 100644 --- a/rm_chassis_controllers/src/omni.cpp +++ b/rm_chassis_controllers/src/omni.cpp @@ -32,8 +32,8 @@ bool OmniController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle return false; } - wheel_power_limitor_.err_upper = 4000; - wheel_power_limitor_.err_lower = 10; + 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; @@ -268,18 +268,18 @@ void OmniController::updatePowerStatus() { // 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) = square(wheel_power_limitor_.omiga[0]) + square(wheel_power_limitor_.omiga[1]) + square(wheel_power_limitor_.omiga[2]) + square(wheel_power_limitor_.omiga[3]); - rls_->setU(all_in); rls_->setX(x); - rls_->setY(chassis_power_); + rls_->setY(chassis_power_ - all_in); rls_->update(); auto w = rls_->getW(); - wheel_power_limitor_.effort_coeff = w(0); - wheel_power_limitor_.vel_coeff = w(1); + wheel_power_limitor_.effort_coeff = std::max(w(0), 1e-5); + wheel_power_limitor_.vel_coeff = std::max(w(1), 1e-5); capacity_update_flag_ = false; } diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index d706dec6..d3d5cbc0 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -387,8 +387,8 @@ void SwerveController::updatePowerStatus() if (capacity_update_flag_ && use_rls_) { - // 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(pivot_power_limitor_.torque[0]) + square(pivot_power_limitor_.torque[1]) + square(pivot_power_limitor_.torque[2]) + square(pivot_power_limitor_.torque[3]); @@ -398,15 +398,14 @@ void SwerveController::updatePowerStatus() square(wheel_power_limitor_.torque[2]) + square(wheel_power_limitor_.torque[3]); x(3) = square(wheel_power_limitor_.omiga[0]) + square(wheel_power_limitor_.omiga[1]) + square(wheel_power_limitor_.omiga[2]) + square(wheel_power_limitor_.omiga[3]); - rls_->setU(all_in); rls_->setX(x); - rls_->setY(chassis_power_); - rls_->update(); + rls_->setY(chassis_power_ - all_in); + rls_->update(); // Internally computes predicted output as x^T * w auto w = rls_->getW(); - pivot_power_limitor_.effort_coeff = w(0); - pivot_power_limitor_.vel_coeff = w(1); - wheel_power_limitor_.effort_coeff = w(2); - wheel_power_limitor_.vel_coeff = w(3); + pivot_power_limitor_.effort_coeff = std::max(w(0), 1e-5); + pivot_power_limitor_.vel_coeff = std::max(w(1), 1e-5); + wheel_power_limitor_.effort_coeff = std::max(w(2), 1e-5); + wheel_power_limitor_.vel_coeff = std::max(w(3), 1e-5); capacity_update_flag_ = false; } From 916d52fb4195faa77b7097789827d46803509ade Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Mon, 11 May 2026 17:49:37 +0000 Subject: [PATCH 32/36] Fix rls. --- rm_chassis_controllers/src/omni.cpp | 7 ++++--- rm_chassis_controllers/src/swerve.cpp | 12 +++++++----- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/rm_chassis_controllers/src/omni.cpp b/rm_chassis_controllers/src/omni.cpp index fac9d1d5..bd26a30c 100644 --- a/rm_chassis_controllers/src/omni.cpp +++ b/rm_chassis_controllers/src/omni.cpp @@ -274,12 +274,13 @@ void OmniController::updatePowerStatus() square(wheel_power_limitor_.torque[2]) + square(wheel_power_limitor_.torque[3]); x(1) = square(wheel_power_limitor_.omiga[0]) + square(wheel_power_limitor_.omiga[1]) + square(wheel_power_limitor_.omiga[2]) + square(wheel_power_limitor_.omiga[3]); + rls_->setU(all_in); rls_->setX(x); - rls_->setY(chassis_power_ - all_in); + rls_->setY(chassis_power_); rls_->update(); auto w = rls_->getW(); - wheel_power_limitor_.effort_coeff = std::max(w(0), 1e-5); - wheel_power_limitor_.vel_coeff = std::max(w(1), 1e-5); + 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; } diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index d3d5cbc0..3f624e53 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -388,6 +388,7 @@ void SwerveController::updatePowerStatus() 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]) + @@ -398,14 +399,15 @@ void SwerveController::updatePowerStatus() square(wheel_power_limitor_.torque[2]) + square(wheel_power_limitor_.torque[3]); x(3) = square(wheel_power_limitor_.omiga[0]) + square(wheel_power_limitor_.omiga[1]) + square(wheel_power_limitor_.omiga[2]) + square(wheel_power_limitor_.omiga[3]); + rls_->setU(all_in); rls_->setX(x); - rls_->setY(chassis_power_ - all_in); + 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-5); - pivot_power_limitor_.vel_coeff = std::max(w(1), 1e-5); - wheel_power_limitor_.effort_coeff = std::max(w(2), 1e-5); - wheel_power_limitor_.vel_coeff = std::max(w(3), 1e-5); + 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; } From c468b5a9a66ece688c13ebeba1e159c078f42d2a Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sun, 28 Jun 2026 16:25:00 +0000 Subject: [PATCH 33/36] Update controller. --- rm_chassis_controllers/README.md | 5 + .../rm_chassis_controllers/chassis_base.h | 3 +- rm_chassis_controllers/src/chassis_base.cpp | 23 +- rm_chassis_controllers/src/omni.cpp | 28 +- rm_chassis_controllers/src/swerve.cpp | 36 +- rm_chassis_controllers/test/omni.yaml | 1 + rm_chassis_controllers/test/swerve.yaml | 1 + rm_gimbal_controllers/CMakeLists.txt | 2 +- rm_gimbal_controllers/cfg/BulletSolver.cfg | 14 +- rm_gimbal_controllers/cfg/GimbalBase.cfg | 1 + .../rm_gimbal_controllers/bullet_solver.h | 111 +-- .../dual_yaw_controller.h | 38 + .../rm_gimbal_controllers/gimbal_base.h | 75 +- .../rm_gimbal_controllers_plugins.xml | 7 + rm_gimbal_controllers/src/bullet_solver.cpp | 690 ++++++++++-------- .../src/dual_yaw_controller.cpp | 154 ++++ rm_gimbal_controllers/src/gimbal_base.cpp | 469 ++++++++---- 17 files changed, 1078 insertions(+), 580 deletions(-) create mode 100644 rm_gimbal_controllers/include/rm_gimbal_controllers/dual_yaw_controller.h create mode 100644 rm_gimbal_controllers/src/dual_yaw_controller.cpp diff --git a/rm_chassis_controllers/README.md b/rm_chassis_controllers/README.md index 0b6a81ae..800750bf 100644 --- a/rm_chassis_controllers/README.md +++ b/rm_chassis_controllers/README.md @@ -119,6 +119,10 @@ sudo rosdep install --from-paths src Allowed period (in s) between two commands. If the time is exceed this period, the speed of chassis will be set 0. +* **`raw_yaw_feedforward_k`** (double, default: 0.0) + + Yaw feedforward time constant in seconds. Only used in `raw` mode, where the command-frame rotation uses `yaw + k * w` to compensate direction-change lag. + * **`power_offset`** (double) Fix the difference between theoretical power and actual power. @@ -245,6 +249,7 @@ sudo rosdep install --from-paths src power_offset: -8.41 twist_angular: 0.5233 timeout: 0.1 + raw_yaw_feedforward_k: 0.0 pid_follow: { p: 5.0, i: 0, d: 0.3, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h index d309b6c7..86441f95 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -157,7 +157,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController * * @param from The father frame. */ - void tfVelToBase(const std::string& from); + void tfVelToBase(const std::string& from, double yaw_offset = 0.); /** @brief To limit the chassis power according to current power limit. * * Receive power limit from command. Set max_effort command to chassis to avoid exceed power limit. @@ -226,6 +226,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController double twist_angular_{ M_PI / 6 }; double max_odom_vel_{ 10.0 }; double timeout_{ 0.1 }; + double raw_yaw_feedforward_k_{ 0.0 }; double chassis_power_{ 0.0 }; bool capacity_update_flag_{ false }; bool use_rls_{ false }; diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 3dce6b4e..a77d2fc7 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -58,6 +58,7 @@ void ChassisBase::initialize_parameters(ros::NodeHandle& controller_nh) controller_nh.getParam("twist_angular", twist_angular_); controller_nh.getParam("max_odom_vel", max_odom_vel_); controller_nh.getParam("timeout", timeout_); + raw_yaw_feedforward_k_ = getParam(controller_nh, "raw_yaw_feedforward_k", 0.0); if (controller_nh.hasParam("pid_follow")) pid_follow_.init(ros::NodeHandle(controller_nh, "pid_follow")); @@ -274,7 +275,12 @@ void ChassisBase::raw() recovery(); } - tfVelToBase(command_source_frame_); + double yaw_offset; + if (command_source_frame_ == "yaw") + yaw_offset = raw_yaw_feedforward_k_ * vel_cmd_.z; + else + yaw_offset = 0.; + tfVelToBase(command_source_frame_, yaw_offset); } template @@ -471,11 +477,22 @@ void ChassisBase::updatePowerStatus() } template -void ChassisBase::tfVelToBase(const std::string& from) +void ChassisBase::tfVelToBase(const std::string& from, double yaw_offset) { try { - tf2::doTransform(vel_cmd_, vel_cmd_, robot_state_handle_.lookupTransform("base_link", from, ros::Time(0))); + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform("base_link", from, ros::Time(0)); + if (std::abs(yaw_offset) > 1e-9) + { + tf2::Quaternion rotation; + tf2::fromMsg(transform.transform.rotation, rotation); + tf2::Quaternion yaw_feedforward; + yaw_feedforward.setRPY(0., 0., yaw_offset); + rotation *= yaw_feedforward; + rotation.normalize(); + transform.transform.rotation = tf2::toMsg(rotation); + } + tf2::doTransform(vel_cmd_, vel_cmd_, transform); } catch (tf2::TransformException& ex) { diff --git a/rm_chassis_controllers/src/omni.cpp b/rm_chassis_controllers/src/omni.cpp index bd26a30c..2582cc1f 100644 --- a/rm_chassis_controllers/src/omni.cpp +++ b/rm_chassis_controllers/src/omni.cpp @@ -2,6 +2,8 @@ // Created by qiayuan on 2022/7/29. // +#include +#include #include #include @@ -122,15 +124,13 @@ void OmniController::stateJudge() } return; } - double cos_pitch{}, sin_pitch{}; + double sin_pitch{}; if (abs(pitch_) > 0.12) { - cos_pitch = cos(pitch_); sin_pitch = sin(pitch_); } else { - cos_pitch = 1.0; sin_pitch = 0.0; } @@ -142,22 +142,22 @@ void OmniController::stateJudge() { if (ctl->getJointName().find("left") != std::string::npos) { - wheel_power_limitor_.K_angle[i] = cos_pitch + sin_pitch; + wheel_power_limitor_.K_angle[i] = 1 + sin_pitch; } if (ctl->getJointName().find("right") != std::string::npos) { - wheel_power_limitor_.K_angle[i] = cos_pitch + sin_pitch; + 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] = cos_pitch - sin_pitch; + wheel_power_limitor_.K_angle[i] = 1 - sin_pitch; } if (ctl->getJointName().find("right") != std::string::npos) { - wheel_power_limitor_.K_angle[i] = cos_pitch - sin_pitch; + wheel_power_limitor_.K_angle[i] = 1 - sin_pitch; } } } @@ -198,7 +198,7 @@ void OmniController::powerLimit() auto& joint = ctl->joint_; double A = wheel_power_limitor_.effort_coeff; double B = wheel_power_limitor_.omiga[i]; - double C = square(wheel_power_limitor_.omiga[i]) * wheel_power_limitor_.vel_coeff + + 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) @@ -251,12 +251,12 @@ void OmniController::updatePowerStatus() 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 * square(real_vel); + 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 * square(real_vel); + 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 * square(real_vel); + 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; @@ -264,7 +264,7 @@ 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_) + if (capacity_update_flag_ && use_rls_ && estimated_total_power > 0) { // Update Rls. double all_in = limit(estimated_total_power, -power_limit, power_limit); @@ -272,8 +272,8 @@ void OmniController::updatePowerStatus() 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) = square(wheel_power_limitor_.omiga[0]) + square(wheel_power_limitor_.omiga[1]) + - square(wheel_power_limitor_.omiga[2]) + square(wheel_power_limitor_.omiga[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_); diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index 3f624e53..9f54947e 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -185,15 +185,13 @@ void SwerveController::stateJudge() } return; } - double cos_pitch{}, sin_pitch{}; + double sin_pitch{}; if (abs(pitch_) > 0.12) { - cos_pitch = cos(pitch_); sin_pitch = sin(pitch_); } else { - cos_pitch = 1.0; sin_pitch = 0.0; } @@ -205,22 +203,22 @@ void SwerveController::stateJudge() { if (module.ctrl_wheel_->getJointName().find("left") != std::string::npos) { - wheel_power_limitor_.K_angle[i] = cos_pitch + sin_pitch; + 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] = cos_pitch + sin_pitch; + 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] = cos_pitch - sin_pitch; + 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] = cos_pitch - sin_pitch; + wheel_power_limitor_.K_angle[i] = 1 - sin_pitch; } } } @@ -268,7 +266,7 @@ void SwerveController::powerLimit() auto& joint = module.ctrl_pivot_->joint_; double A = pivot_power_limitor_.effort_coeff; double B = pivot_power_limitor_.omiga[i]; - double C = square(pivot_power_limitor_.omiga[i]) * pivot_power_limitor_.vel_coeff + + 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) @@ -295,7 +293,7 @@ void SwerveController::powerLimit() auto& joint = module.ctrl_wheel_->joint_; double A = wheel_power_limitor_.effort_coeff; double B = wheel_power_limitor_.omiga[i]; - double C = square(wheel_power_limitor_.omiga[i]) * wheel_power_limitor_.vel_coeff + + 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) @@ -342,12 +340,12 @@ void SwerveController::updatePowerStatus() 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 * square(real_vel); + 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 * square(real_vel); + 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 * square(real_vel); + pivot_power_limitor_.vel_coeff * abs(real_vel); } pivot_power_limitor_.cmd_power = cpivot_power + pivot_power_limitor_.power_offset; @@ -372,12 +370,12 @@ void SwerveController::updatePowerStatus() 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 * square(real_vel); + 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 * square(real_vel); + 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 * square(real_vel); + 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; @@ -393,12 +391,12 @@ void SwerveController::updatePowerStatus() 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) = square(pivot_power_limitor_.omiga[0]) + square(pivot_power_limitor_.omiga[1]) + - square(pivot_power_limitor_.omiga[2]) + square(pivot_power_limitor_.omiga[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) = square(wheel_power_limitor_.omiga[0]) + square(wheel_power_limitor_.omiga[1]) + - square(wheel_power_limitor_.omiga[2]) + square(wheel_power_limitor_.omiga[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_); diff --git a/rm_chassis_controllers/test/omni.yaml b/rm_chassis_controllers/test/omni.yaml index 564ecd64..ce352f09 100644 --- a/rm_chassis_controllers/test/omni.yaml +++ b/rm_chassis_controllers/test/omni.yaml @@ -10,6 +10,7 @@ controllers: power_offset: 0 twist_angular: 0.5233 timeout: 0.1 + raw_yaw_feedforward_k: 0.0 pid_follow: { p: 8, i: 0, d: 4.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] diff --git a/rm_chassis_controllers/test/swerve.yaml b/rm_chassis_controllers/test/swerve.yaml index 2c1bc975..9dc0a189 100644 --- a/rm_chassis_controllers/test/swerve.yaml +++ b/rm_chassis_controllers/test/swerve.yaml @@ -9,6 +9,7 @@ controllers: vel_coeff: 0.0048 power_offset: -3 timeout: 0.1 + raw_yaw_feedforward_k: 0.0 twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] pid_follow: { p: 5, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } # SwerveController diff --git a/rm_gimbal_controllers/CMakeLists.txt b/rm_gimbal_controllers/CMakeLists.txt index 98e8736b..a92c2890 100644 --- a/rm_gimbal_controllers/CMakeLists.txt +++ b/rm_gimbal_controllers/CMakeLists.txt @@ -65,7 +65,7 @@ include_directories( ) ## Declare a cpp library -add_library(${PROJECT_NAME} src/gimbal_base.cpp src/bullet_solver.cpp src/ballistic_solver.cpp) +add_library(${PROJECT_NAME} src/gimbal_base.cpp src/dual_yaw_controller.cpp src/bullet_solver.cpp src/ballistic_solver.cpp) ## Specify libraries to link executable targets against target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 3cf38627..0abda770 100644 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -10,22 +10,26 @@ gen.add("resistance_coff_qd_15", double_t, 0, "Air resistance divided by mass of gen.add("resistance_coff_qd_16", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) gen.add("resistance_coff_qd_18", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) gen.add("resistance_coff_qd_30", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) +gen.add("resistance_coff_qd_800", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) gen.add("g", double_t, 0, "Air resistance divided by mass", 9.8, 9.6, 10.0) gen.add("delay", double_t, 0, "Delay of bullet firing", 0.0, 0, 0.5) +gen.add("outpost_delay", double_t, 0, "Delay of bullet firing of outpost", 0.0, 0, 0.5) gen.add("center_delay", double_t, 0, "Delay of shooting this target when in center mode", 0.105, -0.5, 0.5) gen.add("max_switch_angle", double_t, 0, "Max switch angle", 40.0, 0.0, 90.0) gen.add("min_switch_angle", double_t, 0, "Min switch angle", 2.0, 0.0, 90.0) gen.add("switch_angle_offset", double_t, 0, "Switch angle offset", 0.0, -20.0, 20) -gen.add("switch_duration_scale",double_t, 0,"A param of gimbal switch duration", 0.0, -99.0, 99.0) -gen.add("switch_duration_rate",double_t, 0,"A param of gimbal switch duration", 0.0, -99.0, 99.0) -gen.add("switch_duration_offset",double_t, 0,"A param of gimbal switch duration", 0.0, -99.0, 99.0) -gen.add("min_fit_switch_count", int_t, 0, "Min count that current angle fit switch angle", 0, 3, 99) -gen.add("max_selected_armor_", int_t, 0, "Max selected armor number", 0, -4, 4) +gen.add("outpost_switch_angle_offset", double_t, 0,"Switch angle offset", 0.0, -20.0, 20) +gen.add("min_fit_switch_count", int_t, 0, "Min count that current angle fit switch angle", 0, 1, 99) +gen.add("traject_start_fit_", int_t, 0, "The time of traject start ahead of switch", 0, 1, 99) gen.add("min_shoot_beforehand_vel", double_t, 0, "Min velocity to shoot beforehand", 3.0, 0.0, 20) gen.add("track_rotate_target_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0) gen.add("track_move_target_delay", double_t, 0, "Used to estimate current armor's position when moving", 0.0, -1.0, 1.0) +gen.add("yaw_max_acc", double_t, 0, "the max acc of yaw joint" , 0.0, -400.0, 400.0) +gen.add("track_rotate_outpost_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0) gen.add("traject_ahead_",double_t, 0, "Used to switch in advance ", 0.0, -5.0, 5.0) gen.add("clean_shoot_num_", int_t, 0, "Used to clean shoot_num", 0, 0, 1) gen.add("end_pos_offset",double_t, 0, "The offset of the end pos", 0.0 , -1.0, 1.0) +gen.add("traject_k_effort",double_t, 0, "The offset of the traject_switch_time", 0.0 , -10.0, 10.0) +gen.add("traject_k_vel",double_t, 0, "The offset of the traject_switch_time", 0.0 , -10.0, 10.0) exit(gen.generate(PACKAGE, "bullet_solver", "BulletSolver")) diff --git a/rm_gimbal_controllers/cfg/GimbalBase.cfg b/rm_gimbal_controllers/cfg/GimbalBase.cfg index 5f614c33..6c6a3107 100644 --- a/rm_gimbal_controllers/cfg/GimbalBase.cfg +++ b/rm_gimbal_controllers/cfg/GimbalBase.cfg @@ -13,5 +13,6 @@ gen.add("chassis_comp_a_",double_t, 0,"A param of chassis_compensation", 0.0, 0, gen.add("chassis_comp_b_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0) gen.add("chassis_comp_c_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0) gen.add("chassis_comp_d_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0) +gen.add("moment_of_inertia_", double_t, 0, "Moment of inertia of gimbal", 0.0, 0, 999.0) exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase")) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h index 163f8475..d0b3cb32 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -57,15 +57,15 @@ namespace rm_gimbal_controllers { struct Config { - double resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18, - resistance_coff_qd_30, g, delay, center_delay, max_switch_angle, switch_angle_offset, switch_duration_scale, - switch_duration_rate, switch_duration_offset, min_shoot_beforehand_vel, track_rotate_target_delay, - track_move_target_delay; - int min_fit_switch_count; - int max_selected_armor_; + double resistance_coff_qd_1, resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, + resistance_coff_qd_18, resistance_coff_qd_30, resistance_coff_qd_800, g, delay, outpost_delay, center_delay, + max_switch_angle, switch_angle_offset, outpost_switch_angle_offset, min_shoot_beforehand_vel, + track_rotate_target_delay, track_move_target_delay, yaw_max_acc, track_rotate_outpost_delay; + int min_fit_switch_count, traject_start_fit_; double traject_ahead_; int clean_shoot_num_; double end_pos_offset; + double traject_k_effort, traject_k_vel_; }; struct TrajectoryFunctionCoefficients { @@ -82,22 +82,27 @@ class BulletSolver explicit BulletSolver(ros::NodeHandle& controller_nh); bool solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, double v_yaw, - double r1, double r2, double dz, int armors_num, double start_vel); + double r1, double r2, double dz, double armors_num, double start_vel, double track_id); double getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1, - double r2, double dz, int armors_num, double yaw_real, double pitch_real, double bullet_speed); - double getResistanceCoefficient(double bullet_speed) const; + double r2, double dz, double armors_num, double yaw_real, double pitch_real, + double bullet_speed); + double getResistanceCoefficient(double target_distance) const; double getYaw() const { - return output_yaw_; + return output_yaw_[0]; } double getPitch() const { - return -output_pitch_; + return -output_pitch_[0]; } double getTrajectYaw() const { return traject_output_yaw_; } + double getTrajectVel() const + { + return traject_vel_; + } bool getUsingtraject() const { return using_traject_; @@ -110,26 +115,32 @@ class BulletSolver { return traject_effort_ff_; } - bool getTrackTarget() + bool getTrackTarget() const { return track_target_; } - double getGimbalSwitchDuration(double v_yaw); + void CleanTrackCount() + { + track_count_ = 0; + } + double getFlyTime() + { + return fly_time_[0]; + } void getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel, geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, - double r1, double r2, double dz, int armors_num); - void judgeShootBeforehand(const ros::Time& time, double v_yaw); + double r1, double r2, double dz, double armors_num); + uint8_t judgeShootBeforehand(double v_yaw, int id); void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); void identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg); void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); - double planningPoint(ros::Time& time, ros::Time& start_trajectory_time_); + double planningPoint(ros::Time& time, ros::Time& start_trajectory_time_, double v_yaw); void heatCB(const rm_msgs::LocalHeatStateConstPtr& msg); ~BulletSolver() = default; private: std::shared_ptr> path_desire_pub_; std::shared_ptr> path_real_pub_; - std::shared_ptr> shoot_beforehand_cmd_pub_; std::shared_ptr> fly_time_pub_; std::shared_ptr> bullet_solver_pub; ros::Subscriber identified_target_change_sub_; @@ -138,46 +149,56 @@ class BulletSolver realtime_tools::RealtimeBuffer config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; Config config_{}; - double max_track_target_vel_; - double output_yaw_{}, output_pitch_{}, traject_output_yaw_{}; + double yaw_[150], pos_x[150], pos_y[150]; + double max_track_target_vel_{}; + double output_yaw_[150], output_pitch_[150], traject_output_yaw_{}; double bullet_speed_{}, resistance_coff_{}; - double fly_time_; - double switch_hysteresis_; + double fly_time_[150]; + double switch_hysteresis_{}; double last_yaw_{}, filtered_yaw_{}; double gimbal_switch_duration_{}; - double yaw_subtract_; - double switch_armor_angle; + double yaw_subtract_{}; + double switch_armor_angle{}; double filtered_v_yaw_{}; - double switchtime; - double traject_effort_ff_; - double traject_switch_time_; - double switche_time_yaw_; - double traject_max_acc_; - int shoot_num_ = 0; - int shoot_beforehand_cmd_{}; - int count_; - int traject_count_; - int ban_shoot_count_ = 0; - int selected_armor_ = 0; + double switchtime{}; + double traject_effort_ff_{}; + double traject_switch_time_{}; + double switche_time_yaw_{}; + double traject_max_acc_{}; + double last_output_yaw_{}; + double traject_vel_{}; + double r_traject_{}; + double intital_yaw_{}; - bool track_target_ = true; + int shoot_num_{}; + int shoot_beforehand_cmd_{}; + int count_[150]{}; + int next_count_[150]{}; + int ban_shoot_count_{}; + int selected_armor_[150] = {}; + int last_selected_armor_ = {}; + bool track_target_ = false; + int track_count_{}; bool identified_target_change_ = true; - bool is_in_delay_before_switch_{}; bool dynamic_reconfig_initialized_{}; - bool change_armor = false; - bool using_traject_; - bool last_shoot_state_; + bool using_traject_{}; + bool last_shoot_state_{}; + bool is_aheading_two_[150]{}; + bool start_traject_{}; + double filtered_vel_des{}; geometry_msgs::Point after_traject_output_yaw_{}; - geometry_msgs::Point target_pos_{}; + geometry_msgs::Point target_pos_[150]{}; visualization_msgs::Marker marker_desire_; visualization_msgs::Marker marker_real_; - ros::Time start_using_traject_time; - ros::Time ban_shoot_time_; + ros::Time start_using_traject_time{}; + ros::Time ban_shoot_time_{}; + ros::Time last_output_time_{}; + ros::Time ban_shoot_start_time_{}; - TrajectoryFunctionCoefficients trajectory_function_coefficients; - TrajectoryLimitParams stauts_limit_; + TrajectoryFunctionCoefficients trajectory_function_coefficients{}; + TrajectoryLimitParams stauts_limit_{}; - mutable std::mutex heat_mutex_; + mutable std::mutex heat_mutex_{}; }; } // namespace rm_gimbal_controllers diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/dual_yaw_controller.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/dual_yaw_controller.h new file mode 100644 index 00000000..85dccb45 --- /dev/null +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/dual_yaw_controller.h @@ -0,0 +1,38 @@ +/******************************************************************************* + * BSD 3-Clause License + ******************************************************************************/ + +#pragma once + +#include + +namespace rm_gimbal_controllers +{ +class DualYawController : public Controller +{ +public: + DualYawController() = default; + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; + +protected: + bool shouldInitializeController(const std::string& name, const urdf::JointConstSharedPtr& joint_urdf, + int axis) const override; + bool shouldApplyJointLimit(int axis) const override; + std::string getBaseFrameID(const std::unordered_map& joint_urdfs) override; + void updateYawJoint(const ros::Time& time, const ros::Duration& period, const geometry_msgs::Vector3& angular_vel, + const double pos_real[3], const double pos_des[3], const double pos_des_temp[3], + const double vel_des[3], const double angle_error[3], const double traject_pos_des[3], + const double traject_angle_error[3]) override; + +private: + bool initBaseYaw(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& controller_nh); + bool getTrackArmorSetPoint(const ros::Time& time, double& armor_set_point); + void publishBaseYawState(const ros::Time& time, double base_yaw_pos_real, double base_yaw_set_point, + const double vel_des[3], double base_yaw_error); + + urdf::JointConstSharedPtr base_yaw_joint_; + std::unique_ptr base_yaw_ctrl_; + std::unique_ptr base_yaw_pid_pos_; + std::unique_ptr> base_yaw_pos_state_pub_; +}; +} // namespace rm_gimbal_controllers diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index 50c3eff2..c736107c 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -44,12 +44,12 @@ #include #include #include +#include #include #include #include #include #include -#include #include #include #include @@ -57,22 +57,22 @@ #include #include #include -#include -#include -#include +#include namespace rm_gimbal_controllers { struct GimbalConfig { double yaw_k_v_{}, pitch_k_v_{}, accel_pitch_{}, accel_yaw_{}; - double chassis_comp_a_{}, chassis_comp_b_{}, chassis_comp_c_{}, chassis_comp_d_{}; + double chassis_comp_a_{}, chassis_comp_b_{}, chassis_comp_c_{}, + chassis_comp_d_{}; // sine wave compensation, a * sin(b * chassis_angular_z + c) + d + double moment_of_inertia_{}; }; class ChassisVel { public: - explicit ChassisVel(const ros::NodeHandle& nh) + ChassisVel(const ros::NodeHandle& nh) { double num_data; nh.param("num_data", num_data, 20.0); @@ -141,26 +141,44 @@ class Controller : public controller_interface::MultiInterfaceController& joint_urdfs); + virtual std::string getBaseFrameID(const std::unordered_map& joint_urdfs); + virtual void updatePitchJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double vel_des[3], + const double angle_error[3]); + virtual void updateYawJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double pos_real[3], + const double pos_des[3], const double pos_des_temp[3], const double vel_des[3], + const double angle_error[3], const double traject_pos_des[3], + const double traject_angle_error[3]); + void updateGimbalYawController(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, + effort_controllers::JointVelocityController& ctrl, control_toolbox::Pid& pid_pos, + double k_v, const double vel_des[3], const double angle_error[3], + const double traject_angle_error[3]); void rate(const ros::Time& time, const ros::Duration& period); - void track(const ros::Time& time); + void track(const ros::Time& time, const ros::Duration& period); + void externalAimTrack(const ros::Time& time); void direct(const ros::Time& time); void traj(const ros::Time& time); + bool externalAimIsFresh(const ros::Time& time) const; bool setDesIntoLimit(double& angle, const urdf::JointConstSharedPtr& joint_urdf, bool update, double current_angle); void moveJoint(const ros::Time& time, const ros::Duration& period); - void updateChassisVel(); - void updateBallisticSolution(const ros::Time& time); double gravityFeedForward(const ros::Time& time); + void updateChassisVel(); double updateCompensation(double chassis_vel_angular_z); + void publishShootBeforehand(const ros::Time& time, uint8_t cmd); void commandCB(const rm_msgs::GimbalCmdConstPtr& msg); void trackCB(const rm_msgs::TrackDataConstPtr& msg); - void ballisticSolverRequestCB(const std_msgs::BoolConstPtr& msg); + void mpcGimbalAimCB(const rm_msgs::MPCGimbalAimCmdConstPtr& msg); void reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t); - std::string getGimbalFrameID(std::unordered_map joint_urdfs); - std::string getBaseFrameID(std::unordered_map joint_urdfs); rm_control::RobotStateHandle robot_state_handle_; hardware_interface::ImuSensorHandle imu_sensor_handle_; @@ -169,34 +187,41 @@ class Controller : public controller_interface::MultiInterfaceController joint_urdfs_; std::unordered_map pos_des_in_limit_; bool has_imu_ = true; - double ballistic_yaw_{}, ballistic_pitch_{}; std::shared_ptr bullet_solver_; - std::shared_ptr ballistic_solver_; + double gimbal_real_z_vel_{}; + enum class TrackSolverType + { + BULLET_SOLVER, + EXTERNAL_MPC_AIM + }; + TrackSolverType track_solver_type_{ TrackSolverType::BULLET_SOLVER }; + double external_aim_timeout_{ 0.1 }; + bool external_aim_active_{ false }; // ROS Interface ros::Time last_publish_time_{}; - ros::Time last_ballistic_publish_time_{}; + ros::Time last_track_time_{}; std::unordered_map>> pos_state_pub_; std::shared_ptr> error_pub_; - std::shared_ptr> ballistic_solution_pub_; + std::shared_ptr> shoot_beforehand_cmd_pub_; ros::Subscriber cmd_gimbal_sub_; ros::Subscriber data_track_sub_; - ros::Subscriber ballistic_solver_request_sub_; + ros::Subscriber mpc_gimbal_aim_sub_; realtime_tools::RealtimeBuffer cmd_rt_buffer_; realtime_tools::RealtimeBuffer track_rt_buffer_; - realtime_tools::RealtimeBuffer ballistic_track_rt_buffer_; + realtime_tools::RealtimeBuffer gimbal_aim_rt_buffer_; rm_msgs::GimbalCmd cmd_gimbal_; rm_msgs::TrackData data_track_; + rm_msgs::MPCGimbalAimCmd gimbal_aim_; std::string gimbal_des_frame_id_{}, imu_name_{}, gimbal_traject_des_frame_id_; double publish_rate_{}; bool state_changed_{}; int loop_count_{}; // Transform - geometry_msgs::TransformStamped odom2gimbal_des_, odom2gimbal_, odom2base_, last_odom2base_, base2gimbal_, - odom2gimbal_traject_des_; + geometry_msgs::TransformStamped odom2gimbal_des_, odom2gimbal_, odom2base_, last_odom2base_, odom2gimbal_traject_des_; // Gravity Compensation geometry_msgs::Vector3 mass_origin_; @@ -221,9 +246,11 @@ class Controller : public controller_interface::MultiInterfaceController + + + The DualYawController controls base_yaw, yaw and pitch from one GimbalCmd source. + + + diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 06f330f3..a604342d 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -40,6 +40,7 @@ #include #include #include +#include namespace rm_gimbal_controllers { @@ -51,22 +52,25 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .resistance_coff_qd_16 = getParam(controller_nh, "resistance_coff_qd_16", 0.), .resistance_coff_qd_18 = getParam(controller_nh, "resistance_coff_qd_18", 0.), .resistance_coff_qd_30 = getParam(controller_nh, "resistance_coff_qd_30", 0.), + .resistance_coff_qd_800 = getParam(controller_nh, "resistance_coff_qd_800", 0.001), .g = getParam(controller_nh, "g", 0.), .delay = getParam(controller_nh, "delay", 0.), + .outpost_delay = getParam(controller_nh, "outpost_delay", 0.), .center_delay = getParam(controller_nh, "center_delay", 0.0), - .max_switch_angle = getParam(controller_nh, "max_switch_angle", 40.0), .switch_angle_offset = getParam(controller_nh, "switch_angle_offset", 0.0), - .switch_duration_scale = getParam(controller_nh, "switch_duration_scale", 0.), - .switch_duration_rate = getParam(controller_nh, "switch_duration_rate", 0.), - .switch_duration_offset = getParam(controller_nh, "switch_duration_offset", 0.1), - .min_shoot_beforehand_vel = getParam(controller_nh, "min_shoot_beforehand_vel", 3.0), + .outpost_switch_angle_offset = getParam(controller_nh, "outpost_switch_angle_offset", 0.0), + .min_shoot_beforehand_vel = getParam(controller_nh, "min_shoot_beforehand_vel", 2.5), .track_rotate_target_delay = getParam(controller_nh, "track_rotate_target_delay", 0.), .track_move_target_delay = getParam(controller_nh, "track_move_target_delay", 0.), + .yaw_max_acc = getParam(controller_nh, "yaw_max_acc", 60.), + .track_rotate_outpost_delay = getParam(controller_nh, "track_rotate_outpost_delay", 0.), .min_fit_switch_count = getParam(controller_nh, "min_fit_switch_count", 3), - .max_selected_armor_ = getParam(controller_nh, "max_selecteed_armor", 3), + .traject_start_fit_ = getParam(controller_nh, "traject_start_fit_", 45), .traject_ahead_ = getParam(controller_nh, "traject_ahead_", 1.), .clean_shoot_num_ = getParam(controller_nh, "clean_shoot_num_", 1), .end_pos_offset = getParam(controller_nh, "end_pos_offset", 0.), + .traject_k_effort = getParam(controller_nh, "traject_k_effort", 0.0), + .traject_k_vel_ = getParam(controller_nh, "traject_k_vel", 0.65), }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); switch_hysteresis_ = getParam(controller_nh, "switch_hysteresis", 1.0); @@ -96,8 +100,6 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) new realtime_tools::RealtimePublisher(controller_nh, "model_desire", 10)); path_real_pub_.reset( new realtime_tools::RealtimePublisher(controller_nh, "model_real", 10)); - shoot_beforehand_cmd_pub_.reset( - new realtime_tools::RealtimePublisher(controller_nh, "shoot_beforehand_cmd", 10)); fly_time_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "fly_time", 10)); bullet_solver_pub.reset( new realtime_tools::RealtimePublisher(controller_nh, "bullet_solver_data", 10)); @@ -107,352 +109,385 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) &BulletSolver::heatCB, this); } -double BulletSolver::getResistanceCoefficient(double bullet_speed) const +double BulletSolver::getResistanceCoefficient(double target_distance) const { - // bullet_speed have 5 value:10,15,16,18,30 double resistance_coff; - if (bullet_speed < 12.5) - resistance_coff = config_.resistance_coff_qd_10; - else if (bullet_speed < 15.5) - resistance_coff = config_.resistance_coff_qd_15; - else if (bullet_speed < 17) - resistance_coff = config_.resistance_coff_qd_16; - else if (bullet_speed < 24) - resistance_coff = config_.resistance_coff_qd_18; - else - resistance_coff = config_.resistance_coff_qd_30; + // hero + resistance_coff = config_.resistance_coff_qd_800; return resistance_coff; } bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, - double v_yaw, double r1, double r2, double dz, int armors_num, double start_vel) + double v_yaw, double r1, double r2, double dz, double armors_num, double start_vel, + double track_id) { + while (yaw > M_PI / 2) + yaw -= M_PI; + while (yaw < -M_PI / 2) + yaw += M_PI; + if (armors_num == 3) + { + armors_num = 3.6; + } config_ = *config_rt_buffer_.readFromRT(); bullet_speed_ = bullet_speed; - resistance_coff_ = getResistanceCoefficient(bullet_speed_) != 0 ? getResistanceCoefficient(bullet_speed_) : 0.001; if (abs(yaw - last_yaw_) > 1.) filtered_yaw_ = yaw; else if (last_yaw_ != yaw) - { filtered_yaw_ = filtered_yaw_ + (yaw - filtered_yaw_) * (0.001 / (0.01 + 0.001)); - } last_yaw_ = yaw; filtered_v_yaw_ = filtered_v_yaw_ + (v_yaw - filtered_v_yaw_) * 0.05; - double temp_z = pos.z; - double target_rho = std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)); - double output_yaw_central = std::atan2(pos.y, pos.x); - output_pitch_ = std::atan2(temp_z, std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2))); - double rough_fly_time = - (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / resistance_coff_; - double r = r1; - double z = pos.z; - if (track_target_) - yaw += filtered_v_yaw_ * config_.track_rotate_target_delay; - pos.x += vel.x * config_.track_move_target_delay; - pos.y += vel.y * config_.track_move_target_delay; - if (track_target_) + if (track_count_ <= 300) { - if (std::abs(filtered_v_yaw_) >= max_track_target_vel_ + switch_hysteresis_) - track_target_ = false; + track_target_ = false; + track_count_++; } - else + if (track_count_ > 300) { - if (std::abs(filtered_v_yaw_) <= max_track_target_vel_ - switch_hysteresis_) + if (std::abs(filtered_v_yaw_) >= max_track_target_vel_ + switch_hysteresis_) + track_target_ = false; + else if (std::abs(filtered_v_yaw_) <= max_track_target_vel_ - switch_hysteresis_) track_target_ = true; - } - switch_armor_angle = acos(r1 / target_rho) - config_.switch_angle_offset; - traject_switch_time_ = 1.321 * exp(-0.289 * abs(filtered_v_yaw_ + 1.0)) * config_.traject_ahead_; - // traject_switch_time_ = (switch_armor_angle - config_.traject_ahead_ - switche_time_yaw_) / filtered_v_yaw_; - // yaw_subtract_ = filtered_yaw_ - output_yaw_central; - yaw_subtract_ = filtered_yaw_ - output_yaw_central; - while (yaw_subtract_ > M_PI) - yaw_subtract_ -= 2 * M_PI; - while (yaw_subtract_ < -M_PI) - yaw_subtract_ += 2 * M_PI; - double after_fly_yaw_subtract_ = yaw_subtract_ + v_yaw * rough_fly_time; - if (((after_fly_yaw_subtract_ < switch_armor_angle - 0.3 && filtered_v_yaw_ > 1.) || - (after_fly_yaw_subtract_ > switch_armor_angle + 0.3 && filtered_v_yaw_ < -1.)) && - track_target_) - { - selected_armor_ = 0; - } - else if (!track_target_) - { - selected_armor_ = 0; + if (sqrt(pow(pos.x, 2) + pow(pos.y, 2)) < 1.2) + track_target_ = false; } - if ((after_fly_yaw_subtract_ > switch_armor_angle && filtered_v_yaw_ > 1.) || - (after_fly_yaw_subtract_ < switch_armor_angle && filtered_v_yaw_ < -1.)) + if (identified_target_change_) { - count_++; - if (change_armor) + for (int i = 0; i < 150; i++) { - count_ = 0; - change_armor = false; - switche_time_yaw_ = after_fly_yaw_subtract_ + selected_armor_ * 2 * M_PI / armors_num; + count_[i] = 0; + next_count_[i] = 0; } - if (count_ >= config_.min_fit_switch_count) + identified_target_change_ = false; + } + for (int i = 0; i < 150; i++) + { + double temp_z = pos.z; + if (track_target_) { - selected_armor_ = v_yaw > 0. ? -1 : 1; - r = armors_num == 4 ? r2 : r1; - z = pos.z + dz; - if ((after_fly_yaw_subtract_ - 2 * M_PI / armors_num > switch_armor_angle) && v_yaw > 0.) - { - selected_armor_ = -2; - } - else if ((after_fly_yaw_subtract_ + 2 * M_PI / armors_num < switch_armor_angle) && v_yaw < 0.) + if (track_id == 6) { - selected_armor_ = 2; + yaw_[i] = yaw + filtered_v_yaw_ * (config_.track_rotate_outpost_delay + i * 0.001); } - if (count_ == config_.min_fit_switch_count) + else { - switch_armor_time_ = ros::Time::now(); - traject_count_ = 0; + yaw_[i] = yaw + filtered_v_yaw_ * (config_.track_rotate_target_delay + i * 0.001); } } - } - if (ros::Time::now().toSec() > (switch_armor_time_.toSec() + traject_switch_time_) && abs(filtered_v_yaw_) > 2. && - count_ > 2) - { - traject_count_++; - if (traject_count_ == 2) + else + yaw_[i] = yaw; + pos_x[i] = pos.x + vel.x * (config_.track_move_target_delay + i * 0.001); + pos_y[i] = pos.y + vel.y * (config_.track_move_target_delay + i * 0.001); + double target_rho = std::sqrt(std::pow(pos_x[i], 2) + std::pow(pos_y[i], 2)); + resistance_coff_ = getResistanceCoefficient(target_rho) != 0 ? getResistanceCoefficient(target_rho) : 0.001; + double output_yaw_central = std::atan2(pos_y[i], pos_x[i]); + output_pitch_[i] = std::atan2(temp_z, std::sqrt(std::pow(pos_x[i], 2) + std::pow(pos_y[i], 2))); + double rough_fly_time = + (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_[i])))) / + resistance_coff_; + if (bullet_speed == 0.0) { - using_traject_ = true; - start_using_traject_time = ros::Time::now(); + ROS_ERROR("solve bullet speed is zero"); + return false; } - } - double filtered_aim_armor_yaw = filtered_yaw_ + selected_armor_ * 1.57; - double aim_yaw_subtract = filtered_aim_armor_yaw - output_yaw_central; - while (aim_yaw_subtract > M_PI) - aim_yaw_subtract -= 2 * M_PI; - while (aim_yaw_subtract < -M_PI) - aim_yaw_subtract += 2 * M_PI; - is_in_delay_before_switch_ = - ((((aim_yaw_subtract + filtered_v_yaw_ * config_.delay) > switch_armor_angle) && filtered_v_yaw_ > 1.) || - (((aim_yaw_subtract + filtered_v_yaw_ * config_.delay) < -switch_armor_angle) && filtered_v_yaw_ < -1.)) && - track_target_; - int count{}; - double error = 999; - if (track_target_) - { - target_pos_.x = pos.x - r * cos(yaw + selected_armor_ * 1.57); - target_pos_.y = pos.y - r * sin(yaw + selected_armor_ * 1.57); - } - else - { - target_pos_.x = pos.x - r * cos(atan2(pos.y, pos.x)); - target_pos_.y = pos.y - r * sin(atan2(pos.y, pos.x)); - if ((filtered_v_yaw_ > 1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay)) > 0.5) || - (filtered_v_yaw_ < -1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay)) < -0.5)) + double r = r1; + double z = pos.z; + + switch_armor_angle = acos(r1 / target_rho) - config_.switch_angle_offset; + if (track_id == 6) { - selected_armor_ = filtered_v_yaw_ > 0. ? -1 : 1; + switch_armor_angle = acos(r1 / target_rho) - config_.outpost_switch_angle_offset; } - if (((filtered_v_yaw_ > 1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 1 * 2 * M_PI / armors_num) > 0.5) || - (filtered_v_yaw_ < -1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 1 * 2 * M_PI / armors_num) < -0.5))) + yaw_subtract_ = yaw_[i] - output_yaw_central; + // while (yaw_subtract_ > M_PI) + // yaw_subtract_ -= 2 * M_PI; + // while (yaw_subtract_ < -M_PI) + // yaw_subtract_ += 2 * M_PI; + double after_fly_yaw_subtract_ = yaw_subtract_ + v_yaw * rough_fly_time; + if (abs(v_yaw) < 2.) + selected_armor_[i] = 0; + else if ((after_fly_yaw_subtract_ < switch_armor_angle - 0.4 && filtered_v_yaw_ > 2.) || + (after_fly_yaw_subtract_ > -switch_armor_angle + 0.4 && filtered_v_yaw_ < -2.)) + selected_armor_[i] = 0; + + if ((after_fly_yaw_subtract_ > switch_armor_angle && filtered_v_yaw_ > 2.) || + (after_fly_yaw_subtract_ < -switch_armor_angle && filtered_v_yaw_ < -2.)) { - selected_armor_ = filtered_v_yaw_ > 0. ? -2 : 2; + count_[i]++; + if (count_[i] >= config_.min_fit_switch_count) + { + selected_armor_[i] = v_yaw > 0. ? -1 : 1; + r = armors_num == 4 ? r2 : r1; + z = pos.z + dz; + if ((after_fly_yaw_subtract_ - 2 * M_PI / armors_num > switch_armor_angle) && v_yaw > 0.) + { + selected_armor_[i] = -2; + r = armors_num == 4 ? r1 : r2; + z = pos.z; + next_count_[i]++; + if (next_count_[i] == config_.min_fit_switch_count) + { + switch_armor_time_ = ros::Time::now(); + is_aheading_two_[i] = true; + } + } + else if ((after_fly_yaw_subtract_ + 2 * M_PI / armors_num < -switch_armor_angle) && v_yaw < 0.) + { + selected_armor_[i] = 2; + r = armors_num == 4 ? r1 : r2; + z = pos.z; + next_count_[i]++; + if (next_count_[i] == config_.min_fit_switch_count) + { + switch_armor_time_ = ros::Time::now(); + is_aheading_two_[i] = true; + } + } + if (count_[i] == config_.min_fit_switch_count) + { + if (is_aheading_two_[i]) + { + is_aheading_two_[i] = false; + } + else + { + switch_armor_time_ = ros::Time::now(); + } + } + } } - if (((filtered_v_yaw_ > 1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 2 * 2 * M_PI / armors_num) > 0.5) || - (filtered_v_yaw_ < -1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 2 * 2 * M_PI / armors_num) < -0.5))) + int count{}; + double error = 999; + if (track_target_) { - selected_armor_ = filtered_v_yaw_ > 0. ? -3 : 3; + target_pos_[i].x = pos_x[i] - r * cos(yaw_[i] + selected_armor_[i] * 2 * M_PI / armors_num); + target_pos_[i].y = pos_y[i] - r * sin(yaw_[i] + selected_armor_[i] * 2 * M_PI / armors_num); } - if ((selected_armor_ > 0 && selected_armor_ > config_.max_selected_armor_) || - (selected_armor_ < 0 && selected_armor_ < -config_.max_selected_armor_)) + else { - selected_armor_ = selected_armor_ > 0 ? config_.max_selected_armor_ : -config_.max_selected_armor_; + target_pos_[i].x = pos_x[i] - r * cos(atan2(pos_y[i], pos_x[i])); + target_pos_[i].y = pos_y[i] - r * sin(atan2(pos_y[i], pos_x[i])); + selected_armor_[i] = 0; + if ((filtered_v_yaw_ > 2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay)) > 0.7) || + (filtered_v_yaw_ < -2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay)) < -0.7)) + { + selected_armor_[i] = filtered_v_yaw_ > 0. ? -1 : 1; + } + if (((filtered_v_yaw_ > 2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) - + 1 * 2 * M_PI / armors_num) > 0.7) || + (filtered_v_yaw_ < -2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) + + 1 * 2 * M_PI / armors_num) < -0.7))) + { + selected_armor_[i] = filtered_v_yaw_ > 0. ? -2 : 2; + } + if (((filtered_v_yaw_ > 2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) - + 2 * 2 * M_PI / armors_num) > 0.7) || + (filtered_v_yaw_ < -2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) + + 2 * 2 * M_PI / armors_num) < -0.7))) + { + selected_armor_[i] = filtered_v_yaw_ > 0. ? -3 : 3; + } + + if (selected_armor_[i] % 2 == 0) + { + r = r1; + z = pos.z; + } + else + { + r = r2; + z = pos.z + dz; + } } - if (selected_armor_ % 2 == 0) + target_pos_[i].z = z; + while (error >= 0.001) { - r = r1; - z = pos.z; + output_yaw_[i] = std::atan2(target_pos_[i].y, target_pos_[i].x); + output_pitch_[i] = std::atan2(temp_z, std::sqrt(std::pow(target_pos_[i].x, 2) + std::pow(target_pos_[i].y, 2))); + target_rho = std::sqrt(std::pow(target_pos_[i].x, 2) + std::pow(target_pos_[i].y, 2)); + fly_time_[i] = (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_[i])))) / + resistance_coff_; + double real_z = (bullet_speed_ * std::sin(output_pitch_[i]) + (config_.g / resistance_coff_)) * + (1 - std::exp(-resistance_coff_ * fly_time_[i])) / resistance_coff_ - + config_.g * fly_time_[i] / resistance_coff_; + if (track_target_) + { + target_pos_[i].x = pos_x[i] + vel.x * fly_time_[i] - + r * cos(yaw_[i] + v_yaw * fly_time_[i] + selected_armor_[i] * 2 * M_PI / armors_num); + target_pos_[i].y = pos_y[i] + vel.y * fly_time_[i] - + r * sin(yaw_[i] + v_yaw * fly_time_[i] + selected_armor_[i] * 2 * M_PI / armors_num); + } + else + { + double target_pos_after_fly_time[2]; + target_pos_after_fly_time[0] = pos_x[i] + vel.x * fly_time_[i]; + target_pos_after_fly_time[1] = pos_y[i] + vel.y * fly_time_[i]; + target_pos_[i].x = + target_pos_after_fly_time[0] - r * cos(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); + target_pos_[i].y = + target_pos_after_fly_time[1] - r * sin(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); + } + target_pos_[i].z = z + vel.z * fly_time_[i]; + + double target_yaw = std::atan2(target_pos_[i].y, target_pos_[i].x); + double error_theta = target_yaw - output_yaw_[i]; + double error_z = target_pos_[i].z - real_z; + temp_z += error_z; + error = std::sqrt(std::pow(error_theta * target_rho, 2) + std::pow(error_z, 2)); + count++; + if (count >= 20 || std::isnan(error)) + return false; } } - target_pos_.z = z; - double r_trajcet_; - if (selected_armor_ == 1 || selected_armor_ == -1) + if (fly_time_pub_->trylock()) { - r_trajcet_ = r1; + fly_time_pub_->msg_.data = fly_time_[0]; + fly_time_pub_->unlockAndPublish(); + } + if (selected_armor_[0] == 1 || selected_armor_[0] == -1 || selected_armor_[0] == 3 || selected_armor_[0] == -3) + { + r_traject_ = r1; } else { - r_trajcet_ = r2; + r_traject_ = r2; } - - if (!using_traject_) + for (int i = 1; i <= config_.traject_start_fit_; i++) { - switchtime = 0.08; - - if (filtered_v_yaw_ > 0) - { - after_traject_output_yaw_.x = - pos.x + vel.x * (fly_time_ + switchtime) - - r_trajcet_ * cos(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ - 1) * 2 * M_PI / armors_num); - after_traject_output_yaw_.y = - pos.y + vel.y * (fly_time_ + switchtime) - - r_trajcet_ * sin(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ - 1) * 2 * M_PI / armors_num); - } - else + if (selected_armor_[i] != selected_armor_[0] && using_traject_ == false && abs(v_yaw) > 4.5) { - after_traject_output_yaw_.x = - pos.x + vel.x * (fly_time_ + switchtime) - - r_trajcet_ * cos(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ + 1) * 2 * M_PI / armors_num); - after_traject_output_yaw_.y = - pos.y + vel.y * (fly_time_ + switchtime) - - r_trajcet_ * sin(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ + 1) * 2 * M_PI / armors_num); + start_traject_ = true; + using_traject_ = true; + start_using_traject_time = ros::Time::now(); + break; } - stauts_limit_.start_pos = output_yaw_; - stauts_limit_.start_vel = start_vel; - stauts_limit_.end_pos = - std::atan2(after_traject_output_yaw_.y, after_traject_output_yaw_.x) + config_.end_pos_offset; - stauts_limit_.end_vel = stauts_limit_.start_vel; + } - if (switchtime > 0.2) + if (start_traject_ && using_traject_) + { + traject_max_acc_ = 400.; + switchtime = 0.05; + while (abs(traject_max_acc_) > config_.yaw_max_acc) { - using_traject_ = false; - } - trajectory_function_coefficients.a0 = stauts_limit_.start_pos; - trajectory_function_coefficients.a1 = stauts_limit_.start_vel; - - Eigen::Matrix2d A; - A << std::pow(switchtime, 2), std::pow(switchtime, 3), 2 * switchtime, 3 * std::pow(switchtime, 2); - Eigen::Vector2d B; - B << stauts_limit_.end_pos - (stauts_limit_.start_pos + stauts_limit_.start_vel * switchtime), - stauts_limit_.end_vel - stauts_limit_.start_vel; + switchtime += 0.005; + if (filtered_v_yaw_ > 0) + { + after_traject_output_yaw_.x = pos_x[0] + vel.x * (fly_time_[0] + switchtime) - + r_traject_ * cos(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] - 1) * 2 * M_PI / armors_num); + after_traject_output_yaw_.y = pos_y[0] + vel.y * (fly_time_[0] + switchtime) - + r_traject_ * sin(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] - 1) * 2 * M_PI / armors_num); + } + else + { + after_traject_output_yaw_.x = pos_x[0] + vel.x * (fly_time_[0] + switchtime) - + r_traject_ * cos(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] + 1) * 2 * M_PI / armors_num); + after_traject_output_yaw_.y = pos_y[0] + vel.y * (fly_time_[0] + switchtime) - + r_traject_ * sin(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] + 1) * 2 * M_PI / armors_num); + } + stauts_limit_.start_pos = output_yaw_[0]; + stauts_limit_.start_vel = start_vel; + stauts_limit_.end_pos = + std::atan2(after_traject_output_yaw_.y, after_traject_output_yaw_.x) + config_.end_pos_offset; + stauts_limit_.end_pos = + stauts_limit_.start_pos + angles::shortest_angular_distance(stauts_limit_.start_pos, stauts_limit_.end_pos); + stauts_limit_.end_vel = stauts_limit_.start_vel; - Eigen::Vector2d X = A.colPivHouseholderQr().solve(B); + if (switchtime > 0.15) + { + break; + } + trajectory_function_coefficients.a0 = stauts_limit_.start_pos; + trajectory_function_coefficients.a1 = stauts_limit_.start_vel; - trajectory_function_coefficients.a2 = X(0); - trajectory_function_coefficients.a3 = X(1); - traject_max_acc_ = 2 * trajectory_function_coefficients.a2; - } + Eigen::Matrix2d A; + A << std::pow(switchtime, 2), std::pow(switchtime, 3), 2 * switchtime, 3 * std::pow(switchtime, 2); + Eigen::Vector2d B; + B << stauts_limit_.end_pos - (stauts_limit_.start_pos + stauts_limit_.start_vel * switchtime), + stauts_limit_.end_vel - stauts_limit_.start_vel; - while (error >= 0.001) - { - output_yaw_ = std::atan2(target_pos_.y, target_pos_.x); - output_pitch_ = std::atan2(temp_z, std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2))); - target_rho = std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2)); - fly_time_ = - (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / resistance_coff_; - double real_z = (bullet_speed_ * std::sin(output_pitch_) + (config_.g / resistance_coff_)) * - (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_ - - config_.g * fly_time_ / resistance_coff_; + Eigen::Vector2d X = A.colPivHouseholderQr().solve(B); - if (track_target_) - { - target_pos_.x = - pos.x + vel.x * fly_time_ - r * cos(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num); - target_pos_.y = - pos.y + vel.y * fly_time_ - r * sin(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num); + trajectory_function_coefficients.a2 = X(0); + trajectory_function_coefficients.a3 = X(1); + traject_max_acc_ = abs(2 * trajectory_function_coefficients.a2); } - else - { - double target_pos_after_fly_time[2]; - target_pos_after_fly_time[0] = pos.x + vel.x * fly_time_; - target_pos_after_fly_time[1] = pos.y + vel.y * fly_time_; - target_pos_.x = - target_pos_after_fly_time[0] - r * cos(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); - target_pos_.y = - target_pos_after_fly_time[1] - r * sin(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); - } - target_pos_.z = z + vel.z * fly_time_; - - double target_yaw = std::atan2(target_pos_.y, target_pos_.x); - double error_theta = target_yaw - output_yaw_; - double error_z = target_pos_.z - real_z; - temp_z += error_z; - error = std::sqrt(std::pow(error_theta * target_rho, 2) + std::pow(error_z, 2)); - count++; - - if (count >= 20 || std::isnan(error)) - return false; - } - if (fly_time_pub_->trylock()) - { - fly_time_pub_->msg_.data = fly_time_; - fly_time_pub_->unlockAndPublish(); + start_traject_ = false; } if (using_traject_) { ros::Time temp = ros::Time::now(); - traject_output_yaw_ = planningPoint(temp, start_using_traject_time); + traject_output_yaw_ = planningPoint(temp, start_using_traject_time, v_yaw); + while (traject_output_yaw_ > M_PI) + traject_output_yaw_ -= 2 * M_PI; + while (traject_output_yaw_ < -M_PI) + traject_output_yaw_ += 2 * M_PI; if (ros::Time::now() - start_using_traject_time > ros::Duration(switchtime)) { using_traject_ = false; } } - else + if (!using_traject_ || !track_target_) { - traject_output_yaw_ = output_yaw_; + traject_output_yaw_ = output_yaw_[0]; } + double vel_des = (traject_output_yaw_ - last_output_yaw_) / (ros::Time::now().toSec() - last_output_time_.toSec()); + filtered_vel_des = filtered_vel_des + (vel_des - filtered_vel_des) * (0.001 / (0.01 + 0.001)); + last_output_time_ = ros::Time::now(); + last_output_yaw_ = traject_output_yaw_; if (bullet_solver_pub->trylock()) { - bullet_solver_pub->msg_.selected_armor_ = selected_armor_; - bullet_solver_pub->msg_.yaw_subtract_ = yaw_subtract_; - bullet_solver_pub->msg_.output_yaw_central = output_yaw_central; - bullet_solver_pub->msg_.after_fly_yaw_subtract_ = after_fly_yaw_subtract_; - bullet_solver_pub->msg_.switch_armor_angle = switch_armor_angle; - bullet_solver_pub->msg_.test_number = traject_switch_time_; - bullet_solver_pub->msg_.switche_time_yaw_ = switche_time_yaw_; - bullet_solver_pub->msg_.traject_acc_ = traject_max_acc_; - bullet_solver_pub->msg_.test_number = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 2 * 2 * M_PI / armors_num); - if (v_yaw > 1.0) + for (int i = 0; i < 150; ++i) { - bullet_solver_pub->msg_.center_this_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 0 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 1 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_diagonal_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 2 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_triple_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 3 * 2 * M_PI / armors_num); - } - else if (v_yaw < -1.0) - { - bullet_solver_pub->msg_.center_this_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 0 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 1 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_diagonal_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 2 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_triple_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 3 * 2 * M_PI / armors_num); + bullet_solver_pub->msg_.selected_armor_[i] = static_cast(selected_armor_[i]); + bullet_solver_pub->msg_.count_number_[i] = static_cast(count_[i]); } + bullet_solver_pub->msg_.start_vel = stauts_limit_.start_vel; + bullet_solver_pub->msg_.switchtime = switchtime; + bullet_solver_pub->msg_.traject_output_yaw_ = traject_output_yaw_; + bullet_solver_pub->msg_.traject_acc_ = traject_effort_ff_; + bullet_solver_pub->msg_.traject_vel_ = filtered_vel_des; + bullet_solver_pub->msg_.using_traject_ = using_traject_; + bullet_solver_pub->msg_.test_number[0] = yaw_[0]; bullet_solver_pub->unlockAndPublish(); } return true; } + void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel, geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, - double v_yaw, double r1, double r2, double dz, int armors_num) + double v_yaw, double r1, double r2, double dz, double armors_num) { + if (armors_num == 3) + { + armors_num = 3.6; + } double r = r1, z = pos.z; - if (armors_num == 4 && selected_armor_ != 0 && armors_num == 3) + if (armors_num == 4 && selected_armor_[0] % 2 != 0) { r = r2; z = pos.z + dz; } - pos.x += vel.x * (config_.track_move_target_delay + fly_time_); - pos.y += vel.y * (config_.track_move_target_delay + fly_time_); + pos.x += vel.x * (config_.track_move_target_delay + fly_time_[0]); + pos.y += vel.y * (config_.track_move_target_delay + fly_time_[0]); if (track_target_) { - armor_pos.x = pos.x - r * cos(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + - selected_armor_ * 2 * M_PI / armors_num); - armor_pos.y = pos.y - r * sin(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + - selected_armor_ * 2 * M_PI / armors_num); + armor_pos.x = pos.x - r * cos(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 2 * M_PI / armors_num); + armor_pos.y = pos.y - r * sin(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 2 * M_PI / armors_num); armor_pos.z = z; armor_vel.x = vel.x + v_yaw * r * - sin(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + - selected_armor_ * 2 * M_PI / armors_num); + sin(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 2 * M_PI / armors_num); armor_vel.y = vel.y - v_yaw * r * - cos(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + - selected_armor_ * 2 * M_PI / armors_num); + cos(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 2 * M_PI / armors_num); armor_vel.z = vel.z; } else @@ -470,18 +505,18 @@ void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pi double roll{}, pitch{}, yaw{}; quatToRPY(odom2pitch.transform.rotation, roll, pitch, yaw); geometry_msgs::Point point_desire{}, point_real{}; - double target_rho = std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2)); + double target_rho = std::sqrt(std::pow(target_pos_[0].x, 2) + std::pow(target_pos_[0].y, 2)); int point_num = int(target_rho * 20); for (int i = 0; i <= point_num; i++) { double rt_bullet_rho = target_rho * i / point_num; - double fly_time = (-std::log(1 - rt_bullet_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / + double fly_time = (-std::log(1 - rt_bullet_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_[0])))) / resistance_coff_; - double rt_bullet_z = (bullet_speed_ * std::sin(output_pitch_) + (config_.g / resistance_coff_)) * + double rt_bullet_z = (bullet_speed_ * std::sin(output_pitch_[0]) + (config_.g / resistance_coff_)) * (1 - std::exp(-resistance_coff_ * fly_time)) / resistance_coff_ - config_.g * fly_time / resistance_coff_; - point_desire.x = rt_bullet_rho * std::cos(output_yaw_) + odom2pitch.transform.translation.x; - point_desire.y = rt_bullet_rho * std::sin(output_yaw_) + odom2pitch.transform.translation.y; + point_desire.x = rt_bullet_rho * std::cos(output_yaw_[0]) + odom2pitch.transform.translation.x; + point_desire.y = rt_bullet_rho * std::sin(output_yaw_[0]) + odom2pitch.transform.translation.y; point_desire.z = rt_bullet_z + odom2pitch.transform.translation.z; marker_desire_.points.push_back(point_desire); } @@ -513,14 +548,18 @@ void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pi } double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, - double r1, double r2, double dz, int armors_num, double yaw_real, double pitch_real, - double bullet_speed) + double r1, double r2, double dz, double armors_num, double yaw_real, + double pitch_real, double bullet_speed) { + if (armors_num == 3) + { + armors_num = 3.6; + } config_ = *config_rt_buffer_.readFromRT(); double delay; delay = track_target_ ? 0. : config_.center_delay; double r, z; - if (selected_armor_ % 2 == 0) + if (selected_armor_[0] % 2 == 0) { r = r1; z = pos.z; @@ -534,28 +573,28 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec if (track_target_) { double bullet_rho = - bullet_speed * std::cos(pitch_real) * (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_; + bullet_speed * std::cos(pitch_real) * (1 - std::exp(-resistance_coff_ * fly_time_[0])) / resistance_coff_; double bullet_x = bullet_rho * std::cos(yaw_real); double bullet_y = bullet_rho * std::sin(yaw_real); double bullet_z = (bullet_speed * std::sin(pitch_real) + (config_.g / resistance_coff_)) * - (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_ - - config_.g * fly_time_ / resistance_coff_; - error = std::sqrt(std::pow(target_pos_.x - bullet_x, 2) + std::pow(target_pos_.y - bullet_y, 2) + - std::pow(target_pos_.z - bullet_z, 2)); + (1 - std::exp(-resistance_coff_ * fly_time_[0])) / resistance_coff_ - + config_.g * fly_time_[0] / resistance_coff_; + error = std::sqrt(std::pow(target_pos_[0].x - bullet_x, 2) + std::pow(target_pos_[0].y - bullet_y, 2) + + std::pow(target_pos_[0].z - bullet_z, 2)); } else { geometry_msgs::Point target_pos_after_fly_time_and_delay{}; target_pos_after_fly_time_and_delay.x = - pos.x + vel.x * (fly_time_ + delay) - - r * cos(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num); + pos.x + vel.x * (fly_time_[0] + delay) - + r * cos(yaw + v_yaw * (fly_time_[0] + delay) + selected_armor_[0] * 2 * M_PI / armors_num); target_pos_after_fly_time_and_delay.y = - pos.y + vel.y * (fly_time_ + delay) - - r * sin(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num); - target_pos_after_fly_time_and_delay.z = z + vel.z * (fly_time_ + delay); - error = std::sqrt(std::pow(target_pos_.x - target_pos_after_fly_time_and_delay.x, 2) + - std::pow(target_pos_.y - target_pos_after_fly_time_and_delay.y, 2) + - std::pow(target_pos_.z - target_pos_after_fly_time_and_delay.z, 2)); + pos.y + vel.y * (fly_time_[0] + delay) - + r * sin(yaw + v_yaw * (fly_time_[0] + delay) + selected_armor_[0] * 2 * M_PI / armors_num); + target_pos_after_fly_time_and_delay.z = z + vel.z * (fly_time_[0] + delay); + error = std::sqrt(std::pow(target_pos_[0].x - target_pos_after_fly_time_and_delay.x, 2) + + std::pow(target_pos_[0].y - target_pos_after_fly_time_and_delay.y, 2) + + std::pow(target_pos_[0].z - target_pos_after_fly_time_and_delay.z, 2)); } return error; } @@ -566,47 +605,53 @@ void BulletSolver::identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg) identified_target_change_ = true; else if (!msg->data && identified_target_change_) { - change_armor = true; identified_target_change_ = false; } } -double BulletSolver::getGimbalSwitchDuration(double v_yaw) -{ - gimbal_switch_duration_ = - config_.switch_duration_scale * std::exp(config_.switch_duration_rate * (v_yaw * 60 / (2 * M_PI))) + - config_.switch_duration_offset; - return gimbal_switch_duration_; -} - -void BulletSolver::judgeShootBeforehand(const ros::Time& time, double v_yaw) +uint8_t BulletSolver::judgeShootBeforehand(double v_yaw, int id) { if (!track_target_) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; else if (std::abs(v_yaw) > config_.min_shoot_beforehand_vel) { - if ((ros::Time::now().toSec() > (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay)) && - (ros::Time::now().toSec() < (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay) + switchtime)) + int ban_shoot_delay_number = static_cast(55 + config_.delay * 1000); + if (id == 6) + { + ban_shoot_delay_number = static_cast(55 + config_.outpost_delay * 1000); + } + ban_shoot_delay_number = ban_shoot_delay_number > 149 ? 149 : ban_shoot_delay_number; + if (selected_armor_[ban_shoot_delay_number] != selected_armor_[0]) + { + ban_shoot_count_++; + shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; + if (ban_shoot_count_ == 1) + { + ban_shoot_start_time_ = ros::Time::now(); + } + } + else if (using_traject_ && ros::Time::now().toSec() < ban_shoot_start_time_.toSec() + switchtime) + { shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; - else if (ros::Time::now().toSec() > - (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay) + switchtime && - ros::Time::now().toSec() < - (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay) + switchtime + config_.delay) + } + else if (ros::Time::now().toSec() > ban_shoot_start_time_.toSec() + switchtime && + ros::Time::now().toSec() < ban_shoot_start_time_.toSec() + switchtime + config_.delay) + { shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT; + ban_shoot_count_ = 0; + } else + { shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; + ban_shoot_count_ = 0; + } } else shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; - if (shoot_beforehand_cmd_pub_->trylock()) - { - shoot_beforehand_cmd_pub_->msg_.stamp = time; - shoot_beforehand_cmd_pub_->msg_.cmd = shoot_beforehand_cmd_; - shoot_beforehand_cmd_pub_->unlockAndPublish(); - } + return static_cast(shoot_beforehand_cmd_); } -double BulletSolver::planningPoint(ros::Time& time, ros::Time& start_trajectory_time_) +double BulletSolver::planningPoint(ros::Time& time, ros::Time& start_trajectory_time_, double v_yaw) { double a0 = trajectory_function_coefficients.a0; double a1 = trajectory_function_coefficients.a1; @@ -614,8 +659,8 @@ double BulletSolver::planningPoint(ros::Time& time, ros::Time& start_trajectory_ double a3 = trajectory_function_coefficients.a3; double n_time_ = (time - start_trajectory_time_).toSec(); double plansetpoint = a0 + a1 * n_time_ + a2 * pow(n_time_, 2) + a3 * pow(n_time_, 3); - // traject_effort_ff_ = 1.4 - 1.4 * n_time_/switchtime; - traject_effort_ff_ = 0.0; + traject_effort_ff_ = (2 * a2 + 6 * a3 * n_time_) / (2 * a2) * 1.0 * config_.traject_k_effort; + traject_vel_ = (a1 + 2 * a2 * n_time_ + 3 * a3 * pow(n_time_, 2)) * config_.traject_k_vel_; return plansetpoint; } @@ -646,20 +691,22 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.resistance_coff_qd_30 = init_config.resistance_coff_qd_30; config.g = init_config.g; config.delay = init_config.delay; + config.outpost_delay = init_config.outpost_delay; config.center_delay = init_config.center_delay; - config.max_switch_angle = init_config.max_switch_angle; config.switch_angle_offset = init_config.switch_angle_offset; - config.switch_duration_scale = init_config.switch_duration_scale; - config.switch_duration_rate = init_config.switch_duration_rate; - config.switch_duration_offset = init_config.switch_duration_offset; + config.outpost_switch_angle_offset = init_config.outpost_switch_angle_offset; config.min_shoot_beforehand_vel = init_config.min_shoot_beforehand_vel; config.track_rotate_target_delay = init_config.track_rotate_target_delay; config.track_move_target_delay = init_config.track_move_target_delay; + config.yaw_max_acc = init_config.yaw_max_acc; + config.track_rotate_outpost_delay = init_config.track_rotate_outpost_delay; config.min_fit_switch_count = init_config.min_fit_switch_count; - config.max_selected_armor_ = init_config.max_selected_armor_; + config.traject_start_fit_ = init_config.traject_start_fit_; config.traject_ahead_ = init_config.traject_ahead_; config.clean_shoot_num_ = init_config.clean_shoot_num_; config.end_pos_offset = init_config.end_pos_offset; + config.traject_k_effort = init_config.traject_k_effort; + config.traject_k_vel = init_config.traject_k_vel_; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .resistance_coff_qd_10 = config.resistance_coff_qd_10, @@ -667,22 +714,25 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .resistance_coff_qd_16 = config.resistance_coff_qd_16, .resistance_coff_qd_18 = config.resistance_coff_qd_18, .resistance_coff_qd_30 = config.resistance_coff_qd_30, + .resistance_coff_qd_800 = config.resistance_coff_qd_800, .g = config.g, .delay = config.delay, + .outpost_delay = config.outpost_delay, .center_delay = config.center_delay, - .max_switch_angle = config.max_switch_angle, .switch_angle_offset = config.switch_angle_offset, - .switch_duration_scale = config.switch_duration_scale, - .switch_duration_rate = config.switch_duration_rate, - .switch_duration_offset = config.switch_duration_offset, + .outpost_switch_angle_offset = config.outpost_switch_angle_offset, .min_shoot_beforehand_vel = config.min_shoot_beforehand_vel, .track_rotate_target_delay = config.track_rotate_target_delay, .track_move_target_delay = config.track_move_target_delay, + .yaw_max_acc = config.yaw_max_acc, + .track_rotate_outpost_delay = config.track_rotate_outpost_delay, .min_fit_switch_count = config.min_fit_switch_count, - .max_selected_armor_ = config.max_selected_armor_, + .traject_start_fit_ = config.traject_start_fit_, .traject_ahead_ = config.traject_ahead_, .clean_shoot_num_ = config.clean_shoot_num_, - .end_pos_offset = config.end_pos_offset }; + .end_pos_offset = config.end_pos_offset, + .traject_k_effort = config.traject_k_effort, + .traject_k_vel_ = config.traject_k_vel }; config_rt_buffer_.writeFromNonRT(config_non_rt); } } // namespace rm_gimbal_controllers diff --git a/rm_gimbal_controllers/src/dual_yaw_controller.cpp b/rm_gimbal_controllers/src/dual_yaw_controller.cpp new file mode 100644 index 00000000..a8be8e8e --- /dev/null +++ b/rm_gimbal_controllers/src/dual_yaw_controller.cpp @@ -0,0 +1,154 @@ +#include "rm_gimbal_controllers/dual_yaw_controller.h" + +#include +#include +#include +#include +#include +#include +#include + +namespace rm_gimbal_controllers +{ +bool DualYawController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh) +{ + if (!initBaseYaw(robot_hw, controller_nh)) + return false; + return Controller::init(robot_hw, root_nh, controller_nh); +} + +bool DualYawController::initBaseYaw(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& controller_nh) +{ + ros::NodeHandle base_yaw_nh(controller_nh, "controllers/base_yaw"); + ros::NodeHandle base_yaw_pid_nh(controller_nh, "controllers/base_yaw/pid_pos"); + + urdf::Model urdf; + if (!urdf.initParamWithNodeHandle("robot_description", controller_nh)) + { + ROS_ERROR("Failed to parse urdf file"); + return false; + } + base_yaw_joint_ = urdf.getJoint(getParam(base_yaw_nh, "joint", std::string("base_yaw_joint"))); + if (!base_yaw_joint_) + { + ROS_ERROR("Could not find base_yaw joint in urdf"); + return false; + } + + hardware_interface::EffortJointInterface* effort_joint_interface = + robot_hw->get(); + base_yaw_ctrl_ = std::make_unique(); + base_yaw_pid_pos_ = std::make_unique(); + base_yaw_pos_state_pub_ = + std::make_unique>(base_yaw_nh, "pos_state", 1); + return base_yaw_ctrl_->init(effort_joint_interface, base_yaw_nh) && base_yaw_pid_pos_->init(base_yaw_pid_nh); +} + +bool DualYawController::shouldInitializeController(const std::string& /*name*/, + const urdf::JointConstSharedPtr& joint_urdf, int axis) const +{ + return !(axis == 2 && base_yaw_joint_ && joint_urdf->name == base_yaw_joint_->name); +} + +bool DualYawController::shouldApplyJointLimit(int axis) const +{ + return axis != 2; +} + +std::string DualYawController::getBaseFrameID(const std::unordered_map& joint_urdfs) +{ + if (base_yaw_joint_) + return base_yaw_joint_->parent_link_name.c_str(); + return Controller::getBaseFrameID(joint_urdfs); +} + +void DualYawController::updateYawJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double pos_real[3], + const double pos_des[3], const double /*pos_des_temp*/[3], + const double vel_des[3], const double angle_error[3], + const double traject_pos_des[3], const double traject_angle_error[3]) +{ + if (pid_pos_.find(2) != pid_pos_.end() && ctrls_.find(2) != ctrls_.end()) + { + updateGimbalYawController(time, period, angular_vel, *ctrls_.at(2), *pid_pos_.at(2), config_.yaw_k_v_, vel_des, + angle_error, traject_angle_error); + } + + if (!base_yaw_ctrl_ || !base_yaw_pid_pos_) + return; + + double base_yaw_pos_real = 0.; + try + { + geometry_msgs::TransformStamped odom2base_yaw = + robot_state_handle_.lookupTransform("odom", base_yaw_joint_->child_link_name, time); + double roll{}, pitch{}; + quatToRPY(odom2base_yaw.transform.rotation, roll, pitch, base_yaw_pos_real); + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return; + } + + double armor_set_point{}; + double base_yaw_set_point = getTrackArmorSetPoint(time, armor_set_point) ? armor_set_point : pos_des[2]; + double base_yaw_error = angles::shortest_angular_distance(base_yaw_pos_real, base_yaw_set_point); + base_yaw_pid_pos_->computeCommand(base_yaw_error, period); + base_yaw_ctrl_->setCommand(base_yaw_pid_pos_->getCurrentCmd() + base_yaw_ctrl_->joint_.getVelocity() - angular_vel.z); + base_yaw_ctrl_->update(time, period); + publishBaseYawState(time, base_yaw_pos_real, base_yaw_set_point, vel_des, base_yaw_error); +} + +bool DualYawController::getTrackArmorSetPoint(const ros::Time& time, double& armor_set_point) +{ + if (state_ != TRACK || !base_yaw_joint_ || data_track_.header.stamp.isZero()) + return false; + + geometry_msgs::Point target_pos = data_track_.position; + if (!std::isfinite(target_pos.x) || !std::isfinite(target_pos.y) || !std::isfinite(target_pos.z)) + return false; + + try + { + geometry_msgs::TransformStamped odom2base_yaw = + robot_state_handle_.lookupTransform("odom", base_yaw_joint_->child_link_name, time); + geometry_msgs::Point base_yaw_pos_odom; + base_yaw_pos_odom.x = odom2base_yaw.transform.translation.x; + base_yaw_pos_odom.y = odom2base_yaw.transform.translation.y; + base_yaw_pos_odom.z = odom2base_yaw.transform.translation.z; + + armor_set_point = std::atan2(target_pos.y - base_yaw_pos_odom.y, target_pos.x - base_yaw_pos_odom.x); + if (!std::isfinite(armor_set_point)) + return false; + + return true; + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return false; + } +} + +void DualYawController::publishBaseYawState(const ros::Time& time, double base_yaw_pos_real, double base_yaw_set_point, + const double vel_des[3], double base_yaw_error) +{ + if (base_yaw_pos_state_pub_ && loop_count_ % 10 == 0 && base_yaw_pos_state_pub_->trylock()) + { + base_yaw_pos_state_pub_->msg_.header.stamp = time; + base_yaw_pos_state_pub_->msg_.set_point = base_yaw_set_point; + base_yaw_pos_state_pub_->msg_.traject_set_point = base_yaw_set_point; + base_yaw_pos_state_pub_->msg_.set_point_dot = vel_des[2]; + base_yaw_pos_state_pub_->msg_.process_value = base_yaw_pos_real; + base_yaw_pos_state_pub_->msg_.error = base_yaw_error; + base_yaw_pos_state_pub_->msg_.command = base_yaw_pid_pos_->getCurrentCmd(); + base_yaw_pos_state_pub_->msg_.shoot_number = bullet_solver_->getShootnum(); + base_yaw_pos_state_pub_->unlockAndPublish(); + } +} + +} // namespace rm_gimbal_controllers + +PLUGINLIB_EXPORT_CLASS(rm_gimbal_controllers::DualYawController, controller_interface::ControllerBase) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index e7c67730..92269905 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -36,6 +36,7 @@ // #include "rm_gimbal_controllers/gimbal_base.h" +#include #include #include #include @@ -48,27 +49,6 @@ namespace rm_gimbal_controllers { bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) { - ros::NodeHandle chassis_vel_nh(controller_nh, "chassis_vel"); - chassis_vel_ = std::make_shared(chassis_vel_nh); - ros::NodeHandle nh_bullet_solver = ros::NodeHandle(controller_nh, "bullet_solver"); - bullet_solver_ = std::make_shared(nh_bullet_solver); - ros::NodeHandle nh_ballistic_solver = ros::NodeHandle(controller_nh, "ballistic_solver"); - ballistic_solver_ = std::make_shared(nh_ballistic_solver); - - config_ = { .yaw_k_v_ = getParam(controller_nh, "controllers/yaw/k_v", 0.), - .pitch_k_v_ = getParam(controller_nh, "controllers/pitch/k_v", 0.), - .accel_pitch_ = getParam(controller_nh, "controllers/pitch/accel", 99.), - .accel_yaw_ = getParam(controller_nh, "controllers/yaw/accel", 99.), - .chassis_comp_a_ = getParam(controller_nh, "controllers/yaw/chassis_comp_a", 0.), - .chassis_comp_b_ = getParam(controller_nh, "controllers/yaw/chassis_comp_b", 0.), - .chassis_comp_c_ = getParam(controller_nh, "controllers/yaw/chassis_comp_c", 0.), - .chassis_comp_d_ = getParam(controller_nh, "controllers/yaw/chassis_comp_d", 0.) }; - config_rt_buffer_.initRT(config_); - d_srv_ = new dynamic_reconfigure::Server(controller_nh); - dynamic_reconfigure::Server::CallbackType cb = - [this](auto&& PH1, auto&& PH2) { reconfigCB(PH1, PH2); }; - d_srv_->setCallback(cb); - XmlRpc::XmlRpcValue xml_rpc_value; const bool enable_feedforward = controller_nh.getParam("feedforward", xml_rpc_value); if (enable_feedforward) @@ -82,7 +62,51 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro gravity_ = enable_feedforward ? (double)xml_rpc_value["gravity"] : 0.; enable_gravity_compensation_ = enable_feedforward && (bool)xml_rpc_value["enable_gravity_compensation"]; - auto* effort_joint_interface = robot_hw->get(); + ros::NodeHandle chassis_vel_nh(controller_nh, "chassis_vel"); + chassis_vel_ = std::make_shared(chassis_vel_nh); + ros::NodeHandle nh_bullet_solver = ros::NodeHandle(controller_nh, "bullet_solver"); + bullet_solver_ = std::make_shared(nh_bullet_solver); + std::string track_solver_type; + controller_nh.param("track_solver_type", track_solver_type, "bullet_solver"); + if (track_solver_type == "external_aim") + { + track_solver_type_ = TrackSolverType::EXTERNAL_MPC_AIM; + ROS_WARN("[Gimbal] track_solver_type=external_aim is deprecated, using external_mpc_aim instead"); + } + else if (track_solver_type == "external_mpc_aim") + { + track_solver_type_ = TrackSolverType::EXTERNAL_MPC_AIM; + ROS_INFO("[Gimbal] TRACK mode uses external MPC aim solver"); + } + else if (track_solver_type != "bullet_solver") + { + track_solver_type_ = TrackSolverType::BULLET_SOLVER; + ROS_WARN("[Gimbal] Unknown track_solver_type '%s', falling back to bullet_solver", track_solver_type.c_str()); + } + controller_nh.param("external_aim_timeout", external_aim_timeout_, 0.1); + std::string external_mpc_aim_topic; + controller_nh.param("external_mpc_aim_topic", external_mpc_aim_topic, "/sp_vision/mpc_gimbal_aim"); + + config_ = { + .yaw_k_v_ = getParam(controller_nh, "controllers/yaw/k_v", 0.), + .pitch_k_v_ = getParam(controller_nh, "controllers/pitch/k_v", 0.), + .accel_pitch_ = getParam(controller_nh, "controllers/pitch/accel", 99.), + .accel_yaw_ = getParam(controller_nh, "controllers/yaw/accel", 99.), + .chassis_comp_a_ = getParam(controller_nh, "controllers/yaw/chassis_comp_a", 0.), + .chassis_comp_b_ = getParam(controller_nh, "controllers/yaw/chassis_comp_b", 0.), + .chassis_comp_c_ = getParam(controller_nh, "controllers/yaw/chassis_comp_c", 0.), + .chassis_comp_d_ = getParam(controller_nh, "controllers/yaw/chassis_comp_d", 0.), + .moment_of_inertia_ = getParam(controller_nh, "controllers/moment_of_inertia", 0.), + }; + + config_rt_buffer_.initRT(config_); + d_srv_ = new dynamic_reconfigure::Server(controller_nh); + dynamic_reconfigure::Server::CallbackType cb = + [this](auto&& PH1, auto&& PH2) { reconfigCB(PH1, PH2); }; + d_srv_->setCallback(cb); + + hardware_interface::EffortJointInterface* effort_joint_interface = + robot_hw->get(); if (controller_nh.getParam("controllers", xml_rpc_value)) { // Get URDF info about joint @@ -103,12 +127,15 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro return false; } const int axis = (joint_urdf->axis.x == 1) * 0 + (joint_urdf->axis.y == 1) * 1 + (joint_urdf->axis.z == 1) * 2; - joint_urdfs_.emplace(axis, std::move(joint_urdf)); - ctrls_.emplace(axis, std::make_unique()); - pid_pos_.emplace(axis, std::make_unique()); - pos_des_in_limit_.emplace(axis, true); - pos_state_pub_.emplace( - axis, std::make_unique>(nh, "pos_state", 1)); + if (!shouldInitializeController(it.first, joint_urdf, axis)) + continue; + joint_urdfs_.insert(std::make_pair(axis, joint_urdf)); + ctrls_.insert(std::make_pair(axis, std::make_unique())); + pid_pos_.insert(std::make_pair(axis, std::make_unique())); + pos_des_in_limit_.insert(std::make_pair(axis, true)); + pos_state_pub_.insert(std::make_pair( + axis, std::make_unique>(nh, "pos_state", 1))); + if (!ctrls_.at(axis)->init(effort_joint_interface, nh) || !pid_pos_.at(axis)->init(nh_pid_pos)) return false; } @@ -119,8 +146,10 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro if (has_imu_) { imu_name_ = getParam(controller_nh, "imu_name", static_cast("gimbal_imu")); - auto* imu_sensor_interface = robot_hw->get(); + hardware_interface::ImuSensorInterface* imu_sensor_interface = + robot_hw->get(); imu_sensor_handle_ = imu_sensor_interface->getHandle(imu_name_); + ROS_WARN_ONCE("has imu"); } else { @@ -137,18 +166,26 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro odom2base_.header.frame_id = "odom"; odom2base_.child_frame_id = getBaseFrameID(joint_urdfs_); odom2base_.transform.rotation.w = 1.; + odom2gimbal_traject_des_.header.frame_id = "odom"; odom2gimbal_traject_des_.child_frame_id = gimbal_traject_des_frame_id_; odom2gimbal_traject_des_.transform.rotation.w = 1.; + rm_msgs::GimbalCmd default_cmd; + rm_msgs::TrackData default_track; + rm_msgs::MPCGimbalAimCmd default_aim; + cmd_rt_buffer_.initRT(default_cmd); + track_rt_buffer_.initRT(default_track); + gimbal_aim_rt_buffer_.initRT(default_aim); + cmd_gimbal_sub_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); - data_track_sub_ = controller_nh.subscribe("/track", 1, &Controller::trackCB, this); - ballistic_solver_request_sub_ = controller_nh.subscribe("/ballistic_solver_request", 1, - &Controller::ballisticSolverRequestCB, this); + data_track_sub_ = controller_nh.subscribe("/sp_vision/track", 1, &Controller::trackCB, this); + mpc_gimbal_aim_sub_ = + controller_nh.subscribe(external_mpc_aim_topic, 1, &Controller::mpcGimbalAimCB, this); publish_rate_ = getParam(controller_nh, "publish_rate", 100.); error_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "error", 100)); - ballistic_solution_pub_.reset( - new realtime_tools::RealtimePublisher(controller_nh, "ballistic_solution", 100)); + shoot_beforehand_cmd_pub_.reset( + new realtime_tools::RealtimePublisher(controller_nh, "shoot_beforehand_cmd", 10)); return true; } @@ -164,19 +201,18 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) { cmd_gimbal_ = *cmd_rt_buffer_.readFromRT(); data_track_ = *track_rt_buffer_.readFromNonRT(); + gimbal_aim_ = *gimbal_aim_rt_buffer_.readFromRT(); config_ = *config_rt_buffer_.readFromRT(); try { odom2gimbal_ = robot_state_handle_.lookupTransform("odom", odom2gimbal_.child_frame_id, time); odom2base_ = robot_state_handle_.lookupTransform("odom", odom2base_.child_frame_id, time); - base2gimbal_ = robot_state_handle_.lookupTransform(odom2base_.child_frame_id, odom2gimbal_.child_frame_id, time); } catch (tf2::TransformException& ex) { ROS_WARN_THROTTLE(5, "%s\n", ex.what()); return; } - updateBallisticSolution(time); updateChassisVel(); if (state_ != cmd_gimbal_.mode) { @@ -189,7 +225,10 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) rate(time, period); break; case TRACK: - track(time); + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM) + externalAimTrack(time); + else + track(time, period); break; case DIRECT: direct(time); @@ -201,24 +240,40 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) moveJoint(time, period); } +bool Controller::shouldInitializeController(const std::string& /*name*/, + const urdf::JointConstSharedPtr& /*joint_urdf*/, int /*axis*/) const +{ + return true; +} + +bool Controller::shouldApplyJointLimit(int /*axis*/) const +{ + return true; +} + void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des, double traject_yaw_des, bool update_yaw, bool update_pitch) { - tf2::Quaternion odom2base, odom2gimbal_des, base2gimbal_des, base2gimbal_traject_des; + tf2::Quaternion odom2base, odom2gimbal_des_q, base2gimbal_des, base2gimbal_traject_des; tf2::fromMsg(odom2base_.transform.rotation, odom2base); - tf2::fromMsg(odom2gimbal_des_.transform.rotation, odom2gimbal_des); + tf2::fromMsg(odom2gimbal_des_.transform.rotation, odom2gimbal_des_q); double current_rpy[3], des_rpy[3], traject_rpy[3]; - quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des), current_rpy[0], current_rpy[1], current_rpy[2]); - odom2gimbal_des.setRPY(0, pitch_des, yaw_des); - quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des), des_rpy[0], des_rpy[1], des_rpy[2]); - odom2gimbal_des.setRPY(0, pitch_des, traject_yaw_des); - quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des), traject_rpy[0], traject_rpy[1], traject_rpy[2]); + quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des_q), current_rpy[0], current_rpy[1], current_rpy[2]); + odom2gimbal_des_q.setRPY(0, pitch_des, yaw_des); + quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des_q), des_rpy[0], des_rpy[1], des_rpy[2]); + odom2gimbal_des_q.setRPY(0, pitch_des, traject_yaw_des); + quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des_q), traject_rpy[0], traject_rpy[1], traject_rpy[2]); + for (const auto& it : joint_urdfs_) { - bool update = (it.first != 1 && it.first != 2) || (it.first == 1 && update_pitch) || (it.first == 2 && update_yaw); - pos_des_in_limit_[it.first] = setDesIntoLimit(des_rpy[it.first], it.second, update, current_rpy[it.first]); + bool update = (it.first == 1) ? update_pitch : ((it.first == 2) ? update_yaw : true); + if (shouldApplyJointLimit(it.first)) + pos_des_in_limit_[it.first] = setDesIntoLimit(des_rpy[it.first], it.second, update, current_rpy[it.first]); + else + pos_des_in_limit_[it.first] = true; } + base2gimbal_des.setRPY(des_rpy[0], des_rpy[1], des_rpy[2]); base2gimbal_traject_des.setRPY(traject_rpy[0], traject_rpy[1], traject_rpy[2]); odom2gimbal_des_.transform.rotation = tf2::toMsg(odom2base * base2gimbal_des); @@ -229,6 +284,7 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des, void Controller::rate(const ros::Time& time, const ros::Duration& period) { + bullet_solver_->CleanTrackCount(); if (state_changed_) { // on enter state_changed_ = false; @@ -254,8 +310,16 @@ void Controller::rate(const ros::Time& time, const ros::Duration& period) } } -void Controller::track(const ros::Time& time) +void Controller::track(const ros::Time& time, const ros::Duration& period) { + external_aim_active_ = false; + if (data_track_.header.stamp.toSec() - time.toSec() > 0.5) + { + state_ = RATE; + state_changed_ = true; + rate(time, period); + return; + } if (state_changed_) { // on enter state_changed_ = false; @@ -273,7 +337,7 @@ void Controller::track(const ros::Time& time) { if (!data_track_.header.frame_id.empty()) { - auto transform = + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform("odom", data_track_.header.frame_id, data_track_.header.stamp); tf2::doTransform(target_pos, target_pos, transform); tf2::doTransform(target_vel, target_vel, transform); @@ -296,8 +360,8 @@ void Controller::track(const ros::Time& time) target_vel.z -= chassis_vel_->linear_->z(); bool solve_success = bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, yaw, data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, data_track_.dz, - data_track_.armors_num, ctrls_.at(2)->joint_.getVelocity()); - bullet_solver_->judgeShootBeforehand(time, data_track_.v_yaw); + data_track_.armors_num, gimbal_real_z_vel_, data_track_.id); + publishShootBeforehand(time, bullet_solver_->judgeShootBeforehand(data_track_.v_yaw, data_track_.id)); if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) { @@ -324,6 +388,58 @@ void Controller::track(const ros::Time& time) } } +bool Controller::externalAimIsFresh(const ros::Time& time) const +{ + if (!gimbal_aim_.valid || gimbal_aim_.header.stamp.isZero()) + return false; + const double age = (time - gimbal_aim_.header.stamp).toSec(); + return age >= -external_aim_timeout_ && age <= external_aim_timeout_; +} + +void Controller::externalAimTrack(const ros::Time& time) +{ + if (state_changed_) + { + state_changed_ = false; + ROS_INFO("[Gimbal] Enter TRACK with external aim"); + } + + external_aim_active_ = externalAimIsFresh(time); + if (external_aim_active_) + { + setDes(time, gimbal_aim_.target_yaw, gimbal_aim_.pitch, gimbal_aim_.yaw); + publishShootBeforehand(time, gimbal_aim_.shoot_cmd == 1 ? rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT : + rm_msgs::ShootBeforehandCmd::BAN_SHOOT); + } + else + { + odom2gimbal_des_.header.stamp = time; + robot_state_handle_.setTransform(odom2gimbal_des_, "rm_gimbal_controllers"); + publishShootBeforehand(time, rm_msgs::ShootBeforehandCmd::BAN_SHOOT); + } + + if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) + { + if (error_pub_->trylock()) + { + error_pub_->msg_.stamp = time; + error_pub_->msg_.error = external_aim_active_ && std::isfinite(gimbal_aim_.error) ? gimbal_aim_.error : 1.0; + error_pub_->unlockAndPublish(); + } + last_publish_time_ = time; + } +} + +void Controller::publishShootBeforehand(const ros::Time& time, uint8_t cmd) +{ + if (shoot_beforehand_cmd_pub_ && shoot_beforehand_cmd_pub_->trylock()) + { + shoot_beforehand_cmd_pub_->msg_.stamp = time; + shoot_beforehand_cmd_pub_->msg_.cmd = cmd; + shoot_beforehand_cmd_pub_->unlockAndPublish(); + } +} + void Controller::direct(const ros::Time& time) { if (state_changed_) @@ -335,11 +451,9 @@ void Controller::direct(const ros::Time& time) try { if (!cmd_gimbal_.target_pos.header.frame_id.empty()) - { - auto transform = robot_state_handle_.lookupTransform("odom", cmd_gimbal_.target_pos.header.frame_id, - cmd_gimbal_.target_pos.header.stamp); - tf2::doTransform(aim_point_odom, aim_point_odom, transform); - } + tf2::doTransform(aim_point_odom, aim_point_odom, + robot_state_handle_.lookupTransform("odom", cmd_gimbal_.target_pos.header.frame_id, + cmd_gimbal_.target_pos.header.stamp)); } catch (tf2::TransformException& ex) { @@ -419,9 +533,9 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) gyro.z = imu_sensor_handle_.getAngularVelocity()[2]; try { - auto transform = - robot_state_handle_.lookupTransform(odom2gimbal_.child_frame_id, imu_sensor_handle_.getFrameId(), time); - tf2::doTransform(gyro, angular_vel, transform); + tf2::doTransform(gyro, angular_vel, + robot_state_handle_.lookupTransform(odom2gimbal_.child_frame_id, imu_sensor_handle_.getFrameId(), + time)); } catch (tf2::TransformException& ex) { @@ -438,6 +552,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) if (ctrls_.find(2) != ctrls_.end()) angular_vel.z = ctrls_.at(2)->joint_.getVelocity(); } + gimbal_real_z_vel_ = angular_vel.z; quatToRPY(odom2gimbal_des_.transform.rotation, pos_des[0], pos_des[1], pos_des[2]); quatToRPY(odom2gimbal_.transform.rotation, pos_real[0], pos_real[1], pos_real[2]); quatToRPY(odom2gimbal_traject_des_.transform.rotation, traject_pos_des[0], traject_pos_des[1], traject_pos_des[2]); @@ -453,79 +568,83 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } else if (state_ == TRACK) { - geometry_msgs::Point target_pos; - geometry_msgs::Vector3 target_vel; - if (data_track_.id != 12) + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM) { - geometry_msgs::Point pos = data_track_.position; - double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec()); - pos.x += data_track_.velocity.x * (time - data_track_.header.stamp).toSec(); - pos.y += data_track_.velocity.y * (time - data_track_.header.stamp).toSec(); - pos.z += data_track_.velocity.z * (time - data_track_.header.stamp).toSec(); - bullet_solver_->getSelectedArmorPosAndVel(target_pos, target_vel, pos, data_track_.velocity, yaw, - data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, - data_track_.dz, data_track_.armors_num); + if (external_aim_active_ && externalAimIsFresh(time)) + { + vel_des[2] = gimbal_aim_.yaw_rate; + vel_des[1] = gimbal_aim_.pitch_rate; + } + else + { + vel_des[2] = 0.; + vel_des[1] = 0.; + } } else { - target_pos = data_track_.position; - target_vel = data_track_.velocity; - } - target_vel.x -= chassis_vel_->linear_->x(); - target_vel.y -= chassis_vel_->linear_->y(); - target_vel.z -= chassis_vel_->linear_->z(); - tf2::Vector3 target_pos_tf, target_vel_tf; - try - { - if (joint_urdfs_.find(2) != joint_urdfs_.end()) + geometry_msgs::Point target_pos; + geometry_msgs::Vector3 target_vel; + if (data_track_.id != 12) { - auto transform = robot_state_handle_.lookupTransform(odom2base_.child_frame_id, data_track_.header.frame_id, - data_track_.header.stamp); - tf2::doTransform(target_pos, target_pos, transform); - tf2::doTransform(target_vel, target_vel, transform); - tf2::fromMsg(target_pos, target_pos_tf); - tf2::fromMsg(target_vel, target_vel_tf); - vel_des[2] = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); + geometry_msgs::Point pos = data_track_.position; + double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec()); + pos.x += data_track_.velocity.x * (time - data_track_.header.stamp).toSec(); + pos.y += data_track_.velocity.y * (time - data_track_.header.stamp).toSec(); + pos.z += data_track_.velocity.z * (time - data_track_.header.stamp).toSec(); + bullet_solver_->getSelectedArmorPosAndVel(target_pos, target_vel, pos, data_track_.velocity, yaw, + data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, + data_track_.dz, data_track_.armors_num); } - if (joint_urdfs_.find(1) != joint_urdfs_.end()) + else { - auto transform = robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->parent_link_name, - data_track_.header.frame_id, data_track_.header.stamp); - tf2::doTransform(target_pos, target_pos, transform); - tf2::doTransform(target_vel, target_vel, transform); - tf2::fromMsg(target_pos, target_pos_tf); - tf2::fromMsg(target_vel, target_vel_tf); - vel_des[1] = target_pos_tf.cross(target_vel_tf).y() / std::pow((target_pos_tf.length()), 2); + target_pos = data_track_.position; + target_vel = data_track_.velocity; + } + target_vel.x -= chassis_vel_->linear_->x(); + target_vel.y -= chassis_vel_->linear_->y(); + target_vel.z -= chassis_vel_->linear_->z(); + tf2::Vector3 target_pos_tf, target_vel_tf; + try + { + if (joint_urdfs_.find(2) != joint_urdfs_.end()) + { + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( + odom2base_.child_frame_id, data_track_.header.frame_id, data_track_.header.stamp); + tf2::doTransform(target_pos, target_pos, transform); + tf2::doTransform(target_vel, target_vel, transform); + tf2::fromMsg(target_pos, target_pos_tf); + tf2::fromMsg(target_vel, target_vel_tf); + vel_des[2] = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); + } + if (bullet_solver_->getUsingtraject() && bullet_solver_->getTrackTarget()) + { + vel_des[2] = bullet_solver_->getTrajectVel(); + } + if (joint_urdfs_.find(1) != joint_urdfs_.end()) + { + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( + joint_urdfs_.at(1)->parent_link_name, data_track_.header.frame_id, data_track_.header.stamp); + tf2::doTransform(target_pos, target_pos, transform); + tf2::doTransform(target_vel, target_vel, transform); + tf2::fromMsg(target_pos, target_pos_tf); + tf2::fromMsg(target_vel, target_vel_tf); + vel_des[1] = target_pos_tf.cross(target_vel_tf).y() / std::pow((target_pos_tf.length()), 2); + } + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); } - } - catch (tf2::TransformException& ex) - { - ROS_WARN("%s", ex.what()); } } + for (const auto& in_limit : pos_des_in_limit_) if (!in_limit.second) vel_des[in_limit.first] = 0.; - - if (pid_pos_.find(1) != pid_pos_.end() && ctrls_.find(1) != ctrls_.end()) - { - pid_pos_.at(1)->computeCommand(angle_error[1], period); - ctrls_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * vel_des[1] + - ctrls_.at(1)->joint_.getVelocity() - angular_vel.y); - ctrls_.at(1)->update(time, period); - ctrls_.at(1)->joint_.setCommand(ctrls_.at(1)->joint_.getCommand() + gravityFeedForward(time)); - } - if (pid_pos_.find(2) != pid_pos_.end() && ctrls_.find(2) != ctrls_.end()) - { - pid_pos_.at(2)->computeCommand(bullet_solver_->getUsingtraject() ? traject_angle_error[2] : angle_error[2], period); - double cmd = pid_pos_.at(2)->getCurrentCmd() - - updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + - config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z; - if (state_ == TRACK && bullet_solver_->getUsingtraject() && bullet_solver_->getTrackTarget()) - cmd += bullet_solver_->getTrajectEffortff(); - ctrls_.at(2)->setCommand(cmd); - ctrls_.at(2)->update(time, period); - } + updatePitchJoint(time, period, angular_vel, vel_des, angle_error); + updateYawJoint(time, period, angular_vel, pos_real, pos_des, pos_des_temp, vel_des, angle_error, traject_pos_des, + traject_angle_error); // publish state if (loop_count_ % 10 == 0) @@ -561,9 +680,9 @@ double Controller::gravityFeedForward(const ros::Time& time) if (enable_gravity_compensation_) { Eigen::Vector3d gravity_compensation(0, 0, gravity_); - auto transform = robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->child_link_name, - joint_urdfs_.at(1)->parent_link_name, time); - tf2::doTransform(gravity_compensation, gravity_compensation, transform); + tf2::doTransform(gravity_compensation, gravity_compensation, + robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->child_link_name, + joint_urdfs_.at(1)->parent_link_name, time)); feedforward -= mass_origin.cross(gravity_compensation).y(); } return feedforward; @@ -591,42 +710,97 @@ void Controller::updateChassisVel() last_odom2base_ = odom2base_; } -void Controller::updateBallisticSolution(const ros::Time& time) +void Controller::updatePitchJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double vel_des[3], + const double angle_error[3]) { - if (publish_rate_ > 0.0 && last_ballistic_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) + if (pid_pos_.find(1) != pid_pos_.end() && ctrls_.find(1) != ctrls_.end()) { - if (ballistic_solution_pub_->trylock()) + pid_pos_.at(1)->computeCommand(angle_error[1], period); + ctrls_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * vel_des[1] + + ctrls_.at(1)->joint_.getVelocity() - angular_vel.y); + ctrls_.at(1)->update(time, period); + ctrls_.at(1)->joint_.setCommand(ctrls_.at(1)->joint_.getCommand() + gravityFeedForward(time)); + } +} + +void Controller::updateYawJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double /*pos_real*/[3], + const double /*pos_des*/[3], const double /*pos_des_temp*/[3], const double vel_des[3], + const double angle_error[3], const double /*traject_pos_des*/[3], + const double traject_angle_error[3]) +{ + if (pid_pos_.find(2) == pid_pos_.end() || ctrls_.find(2) == ctrls_.end()) + return; + + updateGimbalYawController(time, period, angular_vel, *ctrls_.at(2), *pid_pos_.at(2), config_.yaw_k_v_, vel_des, + angle_error, traject_angle_error); +} + +void Controller::updateGimbalYawController(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, + effort_controllers::JointVelocityController& ctrl, + control_toolbox::Pid& pid_pos, double k_v, const double vel_des[3], + const double angle_error[3], const double traject_angle_error[3]) +{ + if (state_ == TRACK) + { + pid_pos_.at(2)->computeCommand(traject_angle_error[2], period); + } + else + { + pid_pos_.at(2)->computeCommand(angle_error[2], period); + } + if (state_ == TRACK) + { + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM) + { + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); + ctrls_.at(2)->update(time, period); + auto cmd = ctrls_.at(2)->joint_.getCommand(); + cmd = config_.moment_of_inertia_ * gimbal_aim_.yaw_acc + cmd; + ROS_WARN("cmd is %f, yaw_acc is %f", cmd, gimbal_aim_.yaw_acc); + ctrls_.at(2)->joint_.setCommand(cmd); + } + else { - std_msgs::Float32MultiArray data; - data.data.emplace_back(ballistic_yaw_); - data.data.emplace_back(ballistic_pitch_); - ballistic_solution_pub_->msg_.data = data.data; - ballistic_solution_pub_->unlockAndPublish(); + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); } - last_ballistic_publish_time_ = time; } + else + { + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); + } + + ctrls_.at(2)->update(time, period); } -std::string Controller::getGimbalFrameID(std::unordered_map joint_urdfs) +std::string Controller::getGimbalFrameID(const std::unordered_map& joint_urdfs) { if (joint_urdfs.find(1) != joint_urdfs.end()) - return joint_urdfs.at(1)->child_link_name; + return joint_urdfs.at(1)->child_link_name.c_str(); if (joint_urdfs.find(0) != joint_urdfs.end()) - return joint_urdfs.at(0)->child_link_name; + return joint_urdfs.at(0)->child_link_name.c_str(); if (joint_urdfs.find(2) != joint_urdfs.end()) - return joint_urdfs.at(2)->child_link_name; - return {}; + return joint_urdfs.at(2)->child_link_name.c_str(); + return std::string(); } -std::string Controller::getBaseFrameID(std::unordered_map joint_urdfs) +std::string Controller::getBaseFrameID(const std::unordered_map& joint_urdfs) { if (joint_urdfs.find(2) != joint_urdfs.end()) - return joint_urdfs.at(2)->parent_link_name; + return joint_urdfs.at(2)->parent_link_name.c_str(); if (joint_urdfs.find(0) != joint_urdfs.end()) - return joint_urdfs.at(0)->parent_link_name; + return joint_urdfs.at(0)->parent_link_name.c_str(); if (joint_urdfs.find(1) != joint_urdfs.end()) - return joint_urdfs.at(1)->parent_link_name; - return {}; + return joint_urdfs.at(1)->parent_link_name.c_str(); + return std::string(); } double Controller::updateCompensation(double chassis_vel_angular_z) @@ -649,12 +823,9 @@ void Controller::trackCB(const rm_msgs::TrackDataConstPtr& msg) track_rt_buffer_.writeFromNonRT(*msg); } -void Controller::ballisticSolverRequestCB(const std_msgs::BoolConstPtr& msg) +void Controller::mpcGimbalAimCB(const rm_msgs::MPCGimbalAimCmdConstPtr& msg) { - ballistic_track_rt_buffer_.writeFromNonRT(*msg); - bool ballistic_solver_request = msg->data; - if (ballistic_solver_request) - ballistic_solver_->solver(base2gimbal_, ballistic_yaw_, ballistic_pitch_); + gimbal_aim_rt_buffer_.writeFromNonRT(*msg); } void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t /*unused*/) @@ -665,12 +836,13 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin GimbalConfig init_config = *config_rt_buffer_.readFromNonRT(); // config init use yaml config.yaw_k_v_ = init_config.yaw_k_v_; config.pitch_k_v_ = init_config.pitch_k_v_; - config.accel_pitch_ = init_config.accel_pitch_; - config.accel_yaw_ = init_config.accel_yaw_; config.chassis_comp_a_ = init_config.chassis_comp_a_; config.chassis_comp_b_ = init_config.chassis_comp_b_; config.chassis_comp_c_ = init_config.chassis_comp_c_; config.chassis_comp_d_ = init_config.chassis_comp_d_; + config.accel_pitch_ = init_config.accel_pitch_; + config.accel_yaw_ = init_config.accel_yaw_; + config.moment_of_inertia_ = init_config.moment_of_inertia_; dynamic_reconfig_initialized_ = true; } GimbalConfig config_non_rt{ .yaw_k_v_ = config.yaw_k_v_, @@ -680,7 +852,8 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin .chassis_comp_a_ = config.chassis_comp_a_, .chassis_comp_b_ = config.chassis_comp_b_, .chassis_comp_c_ = config.chassis_comp_c_, - .chassis_comp_d_ = config.chassis_comp_d_ }; + .chassis_comp_d_ = config.chassis_comp_d_, + .moment_of_inertia_ = config.moment_of_inertia_ }; config_rt_buffer_.writeFromNonRT(config_non_rt); } From 26919450d8164ad2566715f1237c1a4edf4adf22 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sat, 11 Jul 2026 07:36:34 +0000 Subject: [PATCH 34/36] Finish dual yaw. --- rm_gimbal_controllers/src/dual_yaw_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_gimbal_controllers/src/dual_yaw_controller.cpp b/rm_gimbal_controllers/src/dual_yaw_controller.cpp index a8be8e8e..69ffa1b4 100644 --- a/rm_gimbal_controllers/src/dual_yaw_controller.cpp +++ b/rm_gimbal_controllers/src/dual_yaw_controller.cpp @@ -96,7 +96,7 @@ void DualYawController::updateYawJoint(const ros::Time& time, const ros::Duratio double base_yaw_set_point = getTrackArmorSetPoint(time, armor_set_point) ? armor_set_point : pos_des[2]; double base_yaw_error = angles::shortest_angular_distance(base_yaw_pos_real, base_yaw_set_point); base_yaw_pid_pos_->computeCommand(base_yaw_error, period); - base_yaw_ctrl_->setCommand(base_yaw_pid_pos_->getCurrentCmd() + base_yaw_ctrl_->joint_.getVelocity() - angular_vel.z); + base_yaw_ctrl_->setCommand(base_yaw_pid_pos_->getCurrentCmd() - chassis_vel_->angular_->z()); base_yaw_ctrl_->update(time, period); publishBaseYawState(time, base_yaw_pos_real, base_yaw_set_point, vel_des, base_yaw_error); } From 91d5806ba15418d464672cd6505c3816d53fcdf0 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Sat, 1 Aug 2026 16:53:39 +0000 Subject: [PATCH 35/36] Add fallen mode. --- .../rm_chassis_controllers/chassis_base.h | 4 +++- rm_chassis_controllers/src/chassis_base.cpp | 20 +++++++++++++++++++ 2 files changed, 23 insertions(+), 1 deletion(-) 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 86441f95..e8f62e59 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -153,6 +153,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController /** @brief Set chassis velocity to zero. */ void recovery(); + void fallen(); /** @brief Transform tf velocity to base link frame. * * @param from The father frame. @@ -260,7 +261,8 @@ class ChassisBase : public controller_interface::MultiInterfaceController { RAW, FOLLOW, - TWIST + TWIST, + FALLEN }; }; diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index a77d2fc7..800c9be2 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -185,6 +185,9 @@ void ChassisBase::update(const ros::Time& time, const ros::Duration& perio case TWIST: twist(time, period); break; + case FALLEN: + fallen(); + break; } ramp_w_->setAcc(cmd_chassis.accel.angular.z); @@ -283,6 +286,23 @@ void ChassisBase::raw() tfVelToBase(command_source_frame_, yaw_offset); } +template +void ChassisBase::fallen() +{ + if (state_changed_) + { + state_changed_ = false; + ROS_INFO("[Chassis] Enter FALLEN"); + } + + ramp_x_->clear(); + ramp_y_->clear(); + ramp_w_->clear(); + vel_cmd_.x = 0.; + vel_cmd_.y = 0.; + vel_cmd_.z = 0.; +} + template void ChassisBase::updateOdom(const ros::Time& time, const ros::Duration& period) { From 4ea7ef1d58d9bae70aaccba7f8a37c6670166a2b Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Tue, 4 Aug 2026 11:27:27 +0000 Subject: [PATCH 36/36] Add Fallen. --- .../include/rm_chassis_controllers/chassis_base.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 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 e8f62e59..e0b91226 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -259,10 +259,10 @@ class ChassisBase : public controller_interface::MultiInterfaceController enum { - RAW, - FOLLOW, - TWIST, - FALLEN + RAW = 0, + FOLLOW = 1, + TWIST = 2, + FALLEN = 4 }; };