减速bug优化

eli_cs_controllers/include/eli_cs_controllers/scaled_joint_trajectory_controller.hpp

@@ -1,58 +1,57 @@
 #ifndef _ELITE_CS_CONTROLLER__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_
 #define _ELITE_CS_CONTROLLER__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_
-
+#include <limits>
+#include <memory>
+#include <vector>
 #include "angles/angles.h"
+#include "eli_cs_controllers/scaled_joint_trajectory_controller_parameters.hpp"
 #include "joint_trajectory_controller/joint_trajectory_controller.hpp"
 #include "joint_trajectory_controller/trajectory.hpp"
 #include "rclcpp/duration.hpp"
 #include "rclcpp/time.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
-#include "eli_cs_controllers/scaled_joint_trajectory_controller_parameters.hpp"
+#include "realtime_tools/realtime_buffer.hpp"
 
 namespace ELITE_CS_CONTROLLER {
+
 class ScaledJointTrajectoryController : public joint_trajectory_controller::JointTrajectoryController {
    public:
     ScaledJointTrajectoryController() = default;
     ~ScaledJointTrajectoryController() override = default;
 
     controller_interface::InterfaceConfiguration state_interface_configuration() const override;
-
     controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& state) override;
-
     controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
-
     CallbackReturn on_init() override;
 
    protected:
     struct TimeData {
-        TimeData() : time(0.0), period(rclcpp::Duration::from_nanoseconds(0.0)), uptime(0.0) {}
+        TimeData() : time(0.0), period(rclcpp::Duration::from_nanoseconds(0)), uptime(0.0) {}
         rclcpp::Time time;
         rclcpp::Duration period;
         rclcpp::Time uptime;
     };
 
    private:
-    double scaling_factor_{};
+    double scaling_factor_{1.0};
+    bool has_scaling_interface_{false};
+    size_t scaling_state_interface_index_{std::numeric_limits<size_t>::max()};
+
     realtime_tools::RealtimeBuffer<TimeData> time_data_;
 
     std::shared_ptr<scaled_joint_trajectory_controller::ParamListener> scaled_param_listener_;
     scaled_joint_trajectory_controller::Params scaled_params_;
 
-    /**
-     * @brief Assigns the values from a trajectory point interface to a joint interface.
-     *
-     * @tparam T The type of the joint interface.
-     * @param[out] joint_interface The reference_wrapper to assign the values to
-     * @param[in] trajectory_point_interface Containing the values to assign.
-     * @todo: Use auto in parameter declaration with c++20
-     */
+    double read_and_clamp_scaling_factor_();
+
     template <typename T>
     void assign_interface_from_point(const T& joint_interface, const std::vector<double>& trajectory_point_interface) {
         for (size_t index = 0; index < dof_; ++index) {
             joint_interface[index].get().set_value(trajectory_point_interface[index]);
         }
     }
 };
+
 }  // namespace ELITE_CS_CONTROLLER
 
 #endif

eli_cs_controllers/src/scaled_joint_trajectory_controller.cpp

@@ -1,8 +1,10 @@
+#include "eli_cs_controllers/scaled_joint_trajectory_controller.hpp"
+
+#include <algorithm>
+#include <cmath>
 #include <memory>
 #include <vector>
 
-#include "eli_cs_controllers/scaled_joint_trajectory_controller.hpp"
-
 #include "lifecycle_msgs/msg/state.hpp"
 
 namespace ELITE_CS_CONTROLLER {
@@ -12,35 +14,78 @@ controller_interface::CallbackReturn ScaledJointTrajectoryController::on_init()
     scaled_param_listener_ = std::make_shared<scaled_joint_trajectory_controller::ParamListener>(get_node());
     scaled_params_ = scaled_param_listener_->get_params();
 
+    scaling_factor_ = 1.0;
+    has_scaling_interface_ = false;
+    scaling_state_interface_index_ = std::numeric_limits<size_t>::max();
+
     return JointTrajectoryController::on_init();
 }
 
 controller_interface::InterfaceConfiguration ScaledJointTrajectoryController::state_interface_configuration() const {
-    controller_interface::InterfaceConfiguration conf;
-    conf = JointTrajectoryController::state_interface_configuration();
+    auto conf = JointTrajectoryController::state_interface_configuration();
     conf.names.push_back(scaled_params_.speed_scaling_interface_name);
-
     return conf;
 }
 
 controller_interface::CallbackReturn ScaledJointTrajectoryController::on_activate(const rclcpp_lifecycle::State& state) {
-    TimeData time_data;
-    time_data.time = get_node()->now();
-    time_data.period = rclcpp::Duration::from_nanoseconds(0);
-    time_data.uptime = get_node()->now();
-    time_data_.initRT(time_data);
-    return JointTrajectoryController::on_activate(state);
+    TimeData td;
+    td.time = get_node()->now();
+    td.period = rclcpp::Duration::from_nanoseconds(0);
+    td.uptime = get_node()->now();
+    time_data_.initRT(td);
+
+    const auto ret = JointTrajectoryController::on_activate(state);
+    if (ret != controller_interface::CallbackReturn::SUCCESS) {
+        return ret;
+    }
+
+    has_scaling_interface_ = false;
+    scaling_state_interface_index_ = std::numeric_limits<size_t>::max();
+
+    for (size_t i = 0; i < state_interfaces_.size(); ++i) {
+        if (state_interfaces_[i].get_name() == scaled_params_.speed_scaling_interface_name) {
+            has_scaling_interface_ = true;
+            scaling_state_interface_index_ = i;
+            break;
+        }
+    }
+
+    if (!has_scaling_interface_) {
+        RCLCPP_ERROR(get_node()->get_logger(),
+                     "Speed scaling interface (%s) not found among state interfaces. "
+                     "Controller will run with scaling_factor=1.0.",
+                     scaled_params_.speed_scaling_interface_name.c_str());
+        scaling_factor_ = 1.0;
+    }
+
+    return controller_interface::CallbackReturn::SUCCESS;
 }
 
-controller_interface::return_type ScaledJointTrajectoryController::update(const rclcpp::Time& time,
-                                                                          const rclcpp::Duration& period) {
-    if (state_interfaces_.back().get_name() == scaled_params_.speed_scaling_interface_name) {
-        scaling_factor_ = state_interfaces_.back().get_value();
+double ScaledJointTrajectoryController::read_and_clamp_scaling_factor_() {
+    double s = 1.0;
+
+    if (has_scaling_interface_ && scaling_state_interface_index_ < state_interfaces_.size()) {
+        s = state_interfaces_[scaling_state_interface_index_].get_value();
     } else {
-        RCLCPP_ERROR(get_node()->get_logger(), "Speed scaling interface (%s) not found in hardware interface.",
-                     scaled_params_.speed_scaling_interface_name.c_str());
+        s = 1.0;
     }
 
+    if (!std::isfinite(s)) {
+        s = 1.0;
+    }
+    s = std::clamp(s, 0.0, 1.0);
+    return s;
+}
+
+controller_interface::return_type ScaledJointTrajectoryController::update(const rclcpp::Time& time,
+                                                                          const rclcpp::Duration& period) {
+    (void)period;
+
+    scaling_factor_ = read_and_clamp_scaling_factor_();
+
+    // RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 2000,
+    //                      "ScaledJointTrajectoryController speed scaling: %.3f", scaling_factor_);
+    //解开注释可打印当前速度缩放因子
     if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {
         return controller_interface::return_type::OK;
     }
@@ -81,18 +126,21 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
     state_current_.time_from_start.set__sec(0);
     read_state_from_state_interfaces(state_current_);
 
-    // currently carrying out a trajectory
-    if (has_active_trajectory()) {
-        // Adjust time with scaling factor
-        TimeData time_data;
-        time_data.time = time;
-        rcl_duration_value_t t_period = (time_data.time - time_data_.readFromRT()->time).nanoseconds();
-        time_data.period = rclcpp::Duration::from_nanoseconds(scaling_factor_ * t_period);
-        time_data.uptime = time_data_.readFromRT()->uptime + time_data.period;
-        rclcpp::Time traj_time = time_data_.readFromRT()->uptime + rclcpp::Duration::from_nanoseconds(t_period);
-        time_data_.reset();
-        time_data_.initRT(time_data);
+    const TimeData* last_td = time_data_.readFromRT();
+    const rcl_duration_value_t real_dt_ns = (time - last_td->time).nanoseconds();
+    const rcl_duration_value_t scaled_dt_ns = static_cast<rcl_duration_value_t>(static_cast<double>(real_dt_ns) * scaling_factor_);
 
+    TimeData new_td;
+    new_td.time = time;
+    new_td.period = rclcpp::Duration::from_nanoseconds(std::max<rcl_duration_value_t>(0, scaled_dt_ns));
+    new_td.uptime = last_td->uptime + new_td.period;
+
+    const rclcpp::Time traj_time = new_td.uptime;
+
+    time_data_.reset();
+    time_data_.initRT(new_td);
+
+    if (has_active_trajectory()) {
         bool first_sample = false;
         // if sampling the first time, set the point before you sample
         if (!traj_external_point_ptr_->is_sampled_already()) {
@@ -126,8 +174,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
 
             // have we reached the end, are not holding position, and is a timeout configured?
             // Check independently of other tolerances
-            if (!before_last_point && !rt_is_holding_ && cmd_timeout_ > 0.0 &&
-                time_difference > cmd_timeout_) {
+            if (!before_last_point && !rt_is_holding_ && cmd_timeout_ > 0.0 && time_difference > cmd_timeout_) {
                 RCLCPP_WARN(get_node()->get_logger(), "Aborted due to command timeout");
 
                 traj_msg_external_point_ptr_.reset();
@@ -136,7 +183,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
 
             // Check state/goal tolerance
             for (size_t index = 0; index < dof_; ++index) {
-                compute_error_for_joint(state_error_, index, state_current_, state_desired_);
+                compute_error_for_joint(state_error_, static_cast<int>(index), state_current_, state_desired_);
 
                 // Always check the state tolerance on the first sample in case the first sample
                 // is the last point
@@ -150,7 +197,6 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
                     !check_state_tolerance_per_joint(state_error_, index, default_tolerances_.goal_state_tolerance[index], false) &&
                     !rt_is_holding_) {
                     outside_goal_tolerance = true;
-
                     if (default_tolerances_.goal_time_tolerance != 0.0) {
                         if (time_difference > default_tolerances_.goal_time_tolerance) {
                             within_goal_time = false;
@@ -159,14 +205,15 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
                 }
             }
 
-            // set values for next hardware write() if tolerance is met
+            const uint64_t pid_dt_ns = static_cast<uint64_t>(std::max<rcl_duration_value_t>(1, scaled_dt_ns));
+
             if (!tolerance_violated_while_moving && within_goal_time) {
                 if (use_closed_loop_pid_adapter_) {
                     // Update PIDs
                     for (auto i = 0ul; i < dof_; ++i) {
-                        tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) +
-                                          pids_[i]->computeCommand(state_error_.positions[i], state_error_.velocities[i],
-                                                                   (uint64_t)period.nanoseconds());
+                        tmp_command_[i] =
+                            (state_desired_.velocities[i] * ff_velocity_scale_[i]) +
+                            pids_[i]->computeCommand(state_error_.positions[i], state_error_.velocities[i], pid_dt_ns);
                     }
                 }
 
@@ -197,7 +244,6 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
                 auto feedback = std::make_shared<FollowJTrajAction::Feedback>();
                 feedback->header.stamp = time;
                 feedback->joint_names = params_.joints;
-
                 feedback->actual = state_current_;
                 feedback->desired = state_desired_;
                 feedback->error = state_error_;
@@ -240,8 +286,8 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
                         rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
                         rt_has_pending_goal_ = false;
 
-                        RCLCPP_WARN(get_node()->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds",
-                                    time_difference);
+                        RCLCPP_WARN(get_node()->get_logger(),
+                                    "Aborted due goal_time_tolerance exceeding by %f seconds (scaled time)", time_difference);
 
                         traj_msg_external_point_ptr_.reset();
                         traj_msg_external_point_ptr_.initRT(set_hold_position());
@@ -250,12 +296,10 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
             } else if (tolerance_violated_while_moving && !rt_has_pending_goal_) {
                 // we need to ensure that there is no pending goal -> we get a race condition otherwise
                 RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation");
-
                 traj_msg_external_point_ptr_.reset();
                 traj_msg_external_point_ptr_.initRT(set_hold_position());
             } else if (!before_last_point && !within_goal_time && !rt_has_pending_goal_) {
                 RCLCPP_ERROR(get_node()->get_logger(), "Exceeded goal_time_tolerance: holding position...");
-
                 traj_msg_external_point_ptr_.reset();
                 traj_msg_external_point_ptr_.initRT(set_hold_position());
             }
@@ -272,4 +316,4 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
 }  // namespace ELITE_CS_CONTROLLER
 
 #include "pluginlib/class_list_macros.hpp"
-PLUGINLIB_EXPORT_CLASS(ELITE_CS_CONTROLLER::ScaledJointTrajectoryController, controller_interface::ControllerInterface)
+PLUGINLIB_EXPORT_CLASS(ELITE_CS_CONTROLLER::ScaledJointTrajectoryController, controller_interface::ControllerInterface)

eli_cs_robot_description/urdf/cs.ros2_control.xacro

@@ -9,6 +9,7 @@
     headless_mode:=false
     initial_positions:=${dict(shoulder_pan_joint=0.0,shoulder_lift_joint=-1.57,elbow_joint=0.0,wrist_1_joint=-1.57,wrist_2_joint=0.0,wrist_3_joint=0.0)}
     use_tool_communication:=false
+    controller_update_rate:=125.0
     script_filename
     output_recipe_filename
     input_recipe_filename
@@ -50,9 +51,10 @@
           <param name="script_command_port">${script_command_port}</param>
           <param name="trajectory_port">${trajectory_port}</param>
           <param name="tf_prefix">${tf_prefix}</param>
+          <param name="controller_update_rate">${controller_update_rate}</param>
           <param name="servoj_gain">1000</param>
           <param name="servoj_lookahead_time">0.03</param>
-          <param name="servoj_time">0.008</param>
+          <param name="servoj_time">${1.0 / controller_update_rate}</param>
           <param name="use_tool_communication">${use_tool_communication}</param>
           <param name="tool_voltage">${tool_voltage}</param>
           <param name="tool_parity">${tool_parity}</param>

eli_cs_robot_description/urdf/cs.urdf.xacro

@@ -30,6 +30,7 @@
    <xacro:arg name="reverse_port" default="50001"/>
    <xacro:arg name="script_sender_port" default="50002"/>
    <xacro:arg name="trajectory_port" default="50003"/>
+   <xacro:arg name="controller_update_rate" default="125.0"/>
    <!--   tool communication related parameters-->
    <xacro:arg name="use_tool_communication" default="false" />
    <xacro:arg name="tool_voltage" default="0" />
@@ -83,6 +84,7 @@
      script_filename="$(arg script_filename)"
      output_recipe_filename="$(arg output_recipe_filename)"
      input_recipe_filename="$(arg input_recipe_filename)"
+     controller_update_rate="$(arg controller_update_rate)"
      local_ip="$(arg local_ip)"
      script_command_port="$(arg script_command_port)"
      reverse_port="$(arg reverse_port)"

eli_cs_robot_description/urdf/cs_macro.xacro

@@ -55,6 +55,7 @@
     script_filename:=to_be_filled_by_cs_robot_driver
     output_recipe_filename:=to_be_filled_by_cs_robot_driver
     input_recipe_filename:=to_be_filled_by_cs_robot_driver
+    controller_update_rate:=125.0
     reverse_port:=50001
     script_sender_port:=50002
     local_ip:=0.0.0.0
@@ -87,6 +88,7 @@
         script_filename="${script_filename}"
         output_recipe_filename="${output_recipe_filename}"
         input_recipe_filename="${input_recipe_filename}"
+        controller_update_rate="${controller_update_rate}"
         tf_prefix="${tf_prefix}"
         robot_ip="${robot_ip}"
         use_tool_communication="${use_tool_communication}"

eli_cs_robot_driver/include/eli_cs_robot_driver/hardware_interface.hpp

@@ -123,6 +123,7 @@ class EliteCSPositionHardwareInterface : public hardware_interface::SystemInterf
     double tool_output_voltage_copy_;
     double tool_output_current_;
     double tool_temperature_;
+    double controller_update_rate_;
 
     // copy of non double values
     std::array<double, STANDARD_DIG_GPIO_NUM> standard_dig_out_;

eli_cs_robot_driver/src/hardware_interface.cpp

@@ -362,8 +362,10 @@ hardware_interface::CallbackReturn EliteCSPositionHardwareInterface::on_configur
     // Specify lookahead time for servoing to position in joint space.
     // A longer lookahead time can smooth the trajectory.
     const double servoj_lookahead_time = stod(info_.hardware_parameters["servoj_lookahead_time"]);
-    // Time of servoj run
+    // Time of servoj run (aligned with controller update rate)
     const double servoj_time = stod(info_.hardware_parameters["servoj_time"]);
+    // Controller loop frequency, shared with RTSI and servoj timing
+    controller_update_rate_ = stod(info_.hardware_parameters["controller_update_rate"]);
 
     // const bool use_tool_communication = (info_.hardware_parameters["use_tool_communication"] == "true") ||
     //                                     (info_.hardware_parameters["use_tool_communication"] == "True");
@@ -436,7 +438,9 @@ bool EliteCSPositionHardwareInterface::rtsiInit(const std::string& ip) {
     // Path to the file containing the recipe used for requesting RTSI inputs.
     const std::string input_recipe_filename = info_.hardware_parameters["input_recipe_filename"];
 
-    rtsi_interface_ = std::make_unique<ELITE::RtsiIOInterface>(output_recipe_filename, input_recipe_filename, 125);
+    // Keep RTSI polling frequency aligned with controller update rate to avoid mismatched cycles
+    rtsi_interface_ = std::make_unique<ELITE::RtsiIOInterface>(output_recipe_filename, input_recipe_filename,
+                                                               static_cast<int>(controller_update_rate_));
     if (!rtsi_interface_->connect(ip)) {
         return false;
     }

eli_cs_robot_moveit_config/launch/cs_moveit.launch.py

@@ -153,8 +153,8 @@ def launch_setup(context, *args, **kwargs):
 
     trajectory_execution = {
         "moveit_manage_controllers": False,
-        "trajectory_execution.allowed_execution_duration_scaling": 1.2,
-        "trajectory_execution.allowed_goal_duration_margin": 0.5,
+        "trajectory_execution.allowed_execution_duration_scaling": 50.0,
+        "trajectory_execution.allowed_goal_duration_margin": 50.0,
         "trajectory_execution.allowed_start_tolerance": 0.01,
     }