update functions

src/robot_control/CMakeLists.txt

@@ -44,6 +44,7 @@ set(SOURCES
   src/controllers/wheel_control.cpp
   src/controllers/waist_control.cpp
   src/utils/trapezoidal_trajectory.cpp
+  src/utils/s_curve_trajectory.cpp
   src/main.cpp
 )
 

src/robot_control/include/controllers/arm_control.hpp

@@ -11,6 +11,7 @@
 #include <memory>
 #include <string>
 #include <vector>
+#include <map>
 #include "geometry_msgs/msg/pose.hpp"
 
 // 前向声明
@@ -24,34 +25,28 @@ namespace moveit {
     }
 }
 
-namespace moveit {
-    namespace planning_interface {
-        class MoveGroupInterface;
-    }
-}
-
 namespace robot_control
 {
     /**
      * @class ArmControl
      * @brief 手臂控制器类
      * 
-     * 继承自ControlBase，提供手臂特定的控制功能。
-     * 使用MoveIt进行轨迹规划。
+     * 继承自ControlBase，使用MoveIt进行轨迹规划。
+     * 结合MotionParameters中的配置信息和MoveIt的URDF/SRDF配置实现手臂控制。
      */
     class ArmControl : public ControlBase
     {
     public:
         /**
          * @brief 构造函数
-         * @param total_joints 关节总数
-         * @param lengthParameters 长度参数
-         * @param maxSpeed 最大速度
-         * @param maxAcc 最大加速度
-         * @param minLimits 最小限位
-         * @param maxLimits 最大限位
-         * @param home_positions 回零位置
-         * @param zero_positions 零位置
+         * @param total_joints 关节总数（每个臂6个关节，总共12个）
+         * @param lengthParameters 长度参数（从MotionParameters中的left_arm_length_或right_arm_length_获取）
+         * @param maxSpeed 最大速度（度/秒）
+         * @param maxAcc 最大加速度（度/秒²）
+         * @param minLimits 最小限位（度）
+         * @param maxLimits 最大限位（度）
+         * @param home_positions 回零位置（弧度）
+         * @param zero_positions 零位置（弧度）
          * @param joint_directions 关节方向
          */
         ArmControl(
@@ -72,11 +67,11 @@ namespace robot_control
 
         /**
          * @brief 初始化MoveIt（需要在ROS节点创建后调用）
+         * 初始化三个MoveGroup: arm_left, arm_right, dual_arm
          * @param node ROS 2节点指针
-         * @param move_group_name MoveGroup名称（如 "arm_left", "arm_right"）
-         * @return true 初始化成功，false 初始化失败
+         * @return true 所有初始化成功，false 初始化失败
          */
-        bool InitializeMoveIt(rclcpp::Node* node, const std::string& move_group_name);
+        bool InitializeMoveIt(rclcpp::Node* node);
 
         /**
          * @brief 规划关节空间运动（MOVEJ）
@@ -112,10 +107,45 @@ namespace robot_control
 
         /**
          * @brief 检查MoveIt是否已初始化
-         * @return true 已初始化，false 未初始化
+         * @return true 所有move group已初始化，false 未初始化
          */
-        bool IsMoveItInitialized() const { return move_group_ != nullptr; }
+        bool IsMoveItInitialized() const;
 
+        // ==================== 重写ControlBase的方法（采用多态方式处理） ====================
+        
+        /**
+         * @brief 移动到目标关节位置（重写ControlBase的方法）
+         * 使用MoveIt规划轨迹，然后通过插补执行。如果MoveIt未初始化，回退到基类实现。
+         * @param joint_positions 输出：当前关节位置（会被更新）
+         * @param target_joint 目标关节位置（弧度）
+         * @param dt 时间步长（秒）
+         * @return true 到达目标，false 运动进行中
+         */
+        bool MoveToTargetJoint(
+            std::vector<double>& joint_positions,
+            const std::vector<double>& target_joint,
+            double dt);
+
+        /**
+         * @brief 回零运动（重写ControlBase的方法）
+         * 使用MoveIt规划轨迹，然后通过插补执行。如果MoveIt未初始化，回退到基类实现。
+         * @param joint_positions 输出：当前关节位置（会被更新）
+         * @param dt 时间步长（秒）
+         * @return true 回零完成，false 回零进行中
+         */
+        bool GoHome(std::vector<double>& joint_positions, double dt);
+
+        /**
+         * @brief 停止运动（重写ControlBase的方法）
+         * 清除MoveIt轨迹并调用基类的停止方法。
+         * @param joint_positions 输出：当前关节位置（会被更新）
+         * @param dt 时间步长（秒）
+         * @return true 完全停止，false 正在停止
+         */
+        bool Stop(std::vector<double>& joint_positions, double dt);
+
+        // ==================== MoveIt特定功能 ====================
+        
         /**
          * @brief 存储规划好的轨迹
          * @param trajectory_points 轨迹点（关节角度，弧度）
@@ -146,13 +176,46 @@ namespace robot_control
         void ClearTrajectory(uint8_t arm_id);
 
     private:
-        std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;  ///< MoveIt MoveGroup接口
-        std::string move_group_name_;  ///< MoveGroup名称
+        // MoveGroup接口（三个move group）
+        std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_left_;   ///< 左臂MoveGroup接口
+        std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_right_;  ///< 右臂MoveGroup接口
+        std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_dual_;   ///< 双臂MoveGroup接口
+        
         rclcpp::Node* node_;  ///< ROS节点指针（用于MoveIt初始化）
+        
+        uint8_t current_arm_id_;  ///< 当前控制的臂ID（0=左臂，1=右臂，2=双臂）
+
+        /**
+         * @brief 根据arm_id获取对应的MoveGroup接口
+         * @param arm_id 手臂ID（0=左臂，1=右臂，2=双臂）
+         * @return MoveGroup接口指针，如果arm_id无效返回nullptr
+         */
+        std::shared_ptr<moveit::planning_interface::MoveGroupInterface> GetMoveGroup(uint8_t arm_id) const;
+        
+        /**
+         * @brief 将度数转换为弧度
+         * @param degrees 度数
+         * @return 弧度
+         */
+        static double DegToRad(double degrees) { return degrees * M_PI / 180.0; }
+        
+        /**
+         * @brief 将弧度转换为度数
+         * @param radians 弧度
+         * @return 度数
+         */
+        static double RadToDeg(double radians) { return radians * 180.0 / M_PI; }
 
         // 轨迹执行相关
+        struct TrajectoryPoint {
+            std::vector<double> positions;     ///< 关节位置（弧度）
+            std::vector<double> velocities;    ///< 关节速度（rad/s）
+            std::vector<double> accelerations; ///< 关节加速度（rad/s²）
+            double time_from_start;            ///< 从轨迹开始的时间（秒）
+        };
+        
         struct TrajectoryState {
-            std::vector<std::vector<double>> trajectory_points;  ///< 轨迹点（关节角度，弧度）
+            std::vector<TrajectoryPoint> trajectory_points;  ///< 完整轨迹点信息
             size_t current_index;  ///< 当前轨迹点索引
             double elapsed_time;   ///< 已执行时间
             bool is_active;        ///< 是否激活

src/robot_control/include/controllers/control_base.hpp

@@ -16,9 +16,17 @@
 #include <vector>
 #include <memory>
 #include "utils/trapezoidal_trajectory.hpp"
+#include "utils/s_curve_trajectory.hpp"
 
 namespace robot_control
 {
+    /**
+     * @brief 轨迹规划器类型枚举
+     */
+    enum class TrajectoryPlannerType {
+        TRAPEZOIDAL = 0,  ///< 梯形轨迹规划器（默认）
+        S_CURVE = 1       ///< S型曲线轨迹规划器
+    };
     /**
      * @class ControlBase
      * @brief 控制器基类
@@ -121,6 +129,18 @@ namespace robot_control
          */
         void SetHomePositions(const std::vector<double>& home_positions);
 
+        /**
+         * @brief 设置轨迹规划器类型
+         * @param planner_type 规划器类型（TRAPEZOIDAL 或 S_CURVE）
+         */
+        void SetTrajectoryPlannerType(TrajectoryPlannerType planner_type);
+
+        /**
+         * @brief 获取当前轨迹规划器类型
+         * @return 当前规划器类型
+         */
+        TrajectoryPlannerType GetTrajectoryPlannerType() const { return planner_type_; }
+
     protected:
         size_t total_joints_;  ///< 关节总数
 
@@ -142,6 +162,8 @@ namespace robot_control
         bool is_target_set_;                        ///< 目标是否已设置
 
         TrapezoidalTrajectory* trapezoidalTrajectory_;  ///< 梯形轨迹规划器
+        S_CurveTrajectory* s_curveTrajectory_;          ///< S型曲线轨迹规划器
+        TrajectoryPlannerType planner_type_;            ///< 当前使用的规划器类型
 
         bool is_moving_;          ///< 是否在运动
         bool is_stopping_;        ///< 是否在停止

src/robot_control/include/core/motion_parameters.hpp

@@ -122,6 +122,15 @@ namespace robot_control
             waist_length_ = {
                 0.1,    // 腰部长度
             };
+            
+            // 手臂长度参数（单位：米，每个臂6个关节对应的连杆长度）
+            // 注意：这些参数主要用于传统运动学计算，MoveIt会从URDF/SRDF加载实际参数
+            left_arm_length_ = {
+                0.1, 0.2, 0.15, 0.1, 0.05, 0.02  // 默认值，实际应从URDF加载
+            };
+            right_arm_length_ = {
+                0.1, 0.2, 0.15, 0.1, 0.05, 0.02  // 默认值，实际应从URDF加载
+            };
 
             // ==================== 速度参数 ====================
             // 轮子速度参数
@@ -143,12 +152,19 @@ namespace robot_control
             wheel_joint_indices_ = {0, 1}; 
             waist_joint_indices_ = {0, 1}; 
             leg_joint_indices_ = {2, 3, 4, 5, 6, 7};
+            // 左臂关节索引（从索引9开始，6个关节）
+            left_arm_joint_indices_ = {9, 10, 11, 12, 13, 14};
+            // 右臂关节索引（从索引15开始，6个关节）
+            right_arm_joint_indices_ = {15, 16, 17, 18, 19, 20};
             total_joints_ = 8; 
 
             // ==================== 关节方向 ====================
             wheel_joint_directions_ = {1, -1};  
             waist_joint_directions_ = {1, 1};
-            leg_joint_directions_ = {-1, 1, 1, -1, -1, 1};  
+            leg_joint_directions_ = {-1, 1, 1, -1, -1, 1};
+            // 手臂关节方向（每个臂6个关节，默认都是1）
+            left_arm_joint_directions_ = {1, 1, 1, 1, 1, 1};
+            right_arm_joint_directions_ = {1, 1, 1, 1, 1, 1};  
 
             // ==================== 关节传动参数 ====================
             joint_gear_ratio_ = {100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0};  // 传动比
@@ -187,6 +203,16 @@ namespace robot_control
             leg_max_velocity_.resize(leg_joint_indices_.size());
             leg_max_acceleration_.resize(leg_joint_indices_.size());
             
+            // 初始化手臂参数
+            left_arm_max_limit_.resize(left_arm_joint_indices_.size());
+            left_arm_min_limit_.resize(left_arm_joint_indices_.size());
+            left_arm_max_velocity_.resize(left_arm_joint_indices_.size());
+            left_arm_max_acceleration_.resize(left_arm_joint_indices_.size());
+            right_arm_max_limit_.resize(right_arm_joint_indices_.size());
+            right_arm_min_limit_.resize(right_arm_joint_indices_.size());
+            right_arm_max_velocity_.resize(right_arm_joint_indices_.size());
+            right_arm_max_acceleration_.resize(right_arm_joint_indices_.size());
+            
             // 从 joint_limits_ 中提取各部分的限位和速度参数
             for (size_t i = 0; i < waist_joint_indices_.size(); i++)
             {
@@ -204,6 +230,59 @@ namespace robot_control
                 leg_max_acceleration_[i] = max_joint_acceleration_[leg_joint_indices_[i]];
             }
             
+            // 从 joint_limits_ 中提取手臂参数（注意：如果关节索引超出范围，使用默认值）
+            // 左臂参数（从索引9开始）
+            for (size_t i = 0; i < left_arm_joint_indices_.size(); i++)
+            {
+                int joint_idx = left_arm_joint_indices_[i];
+                if (joint_idx < static_cast<int>(joint_limits_.size()))
+                {
+                    left_arm_max_limit_[i] = joint_limits_[joint_idx].max;
+                    left_arm_min_limit_[i] = joint_limits_[joint_idx].min;
+                }
+                else
+                {
+                    left_arm_max_limit_[i] = 180.0;  // 默认值（度）
+                    left_arm_min_limit_[i] = -180.0;
+                }
+                if (joint_idx < static_cast<int>(max_joint_velocity_.size()))
+                {
+                    left_arm_max_velocity_[i] = max_joint_velocity_[joint_idx];
+                    left_arm_max_acceleration_[i] = max_joint_acceleration_[joint_idx];
+                }
+                else
+                {
+                    left_arm_max_velocity_[i] = 30.0;  // 默认值（度/秒）
+                    left_arm_max_acceleration_[i] = 80.0;  // 默认值（度/秒²）
+                }
+            }
+            
+            // 右臂参数（从索引15开始）
+            for (size_t i = 0; i < right_arm_joint_indices_.size(); i++)
+            {
+                int joint_idx = right_arm_joint_indices_[i];
+                if (joint_idx < static_cast<int>(joint_limits_.size()))
+                {
+                    right_arm_max_limit_[i] = joint_limits_[joint_idx].max;
+                    right_arm_min_limit_[i] = joint_limits_[joint_idx].min;
+                }
+                else
+                {
+                    right_arm_max_limit_[i] = 180.0;  // 默认值（度）
+                    right_arm_min_limit_[i] = -180.0;
+                }
+                if (joint_idx < static_cast<int>(max_joint_velocity_.size()))
+                {
+                    right_arm_max_velocity_[i] = max_joint_velocity_[joint_idx];
+                    right_arm_max_acceleration_[i] = max_joint_acceleration_[joint_idx];
+                }
+                else
+                {
+                    right_arm_max_velocity_[i] = 30.0;  // 默认值（度/秒）
+                    right_arm_max_acceleration_[i] = 80.0;  // 默认值（度/秒²）
+                }
+            }
+            
             // ==================== 零点位置 ====================
             waist_zero_positions_ = {
                 350.0,
@@ -223,6 +302,10 @@ namespace robot_control
                 0.0,
                 0.0
             };
+            
+            // 手臂零点位置（弧度，每个臂6个关节）
+            left_arm_zero_positions_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+            right_arm_zero_positions_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
 
             // ==================== 初始位置（Home位置） ====================
             waist_home_positions_ = {
@@ -246,6 +329,10 @@ namespace robot_control
                 0.0,
                 0.0
             };
+            
+            // 手臂Home位置（弧度，每个臂6个关节，默认与zero位置相同）
+            left_arm_home_positions_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+            right_arm_home_positions_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
 
             // ==================== 转换因子 ====================
             pulse_to_degree_.resize(joint_resolution_.size());
@@ -266,23 +353,30 @@ namespace robot_control
         // ---------- 关节索引和方向 ----------
         size_t total_joints_;                          ///< 总关节数
         std::vector<int> leg_joint_indices_;           ///< 腿部关节索引
+        std::vector<int> arm_joint_indices_;           ///< 手臂关节索引（所有手臂关节）
+        std::vector<int> left_arm_joint_indices_;      ///< 左臂关节索引
+        std::vector<int> right_arm_joint_indices_;     ///< 右臂关节索引
         std::vector<int> wheel_joint_indices_;         ///< 轮子关节索引
         std::vector<int> waist_joint_indices_;         ///< 腰部关节索引
-        std::vector<int> real_waist_joint_indices_;    ///< 实际腰部关节索引
-        std::vector<int> real_leg_joint_indices_;      ///< 实际腿部关节索引
 
         std::vector<int> wheel_joint_directions_;      ///< 轮子关节方向
         std::vector<int> waist_joint_directions_;      ///< 腰部关节方向
         std::vector<int> leg_joint_directions_;        ///< 腿部关节方向
+        std::vector<int> left_arm_joint_directions_;   ///< 左臂关节方向
+        std::vector<int> right_arm_joint_directions_;  ///< 右臂关节方向
 
         // ---------- 位置参数 ----------
         std::vector<double> leg_home_positions_;       ///< 腿部初始位置（Home位置）
         std::vector<double> waist_home_positions_;     ///< 腰部初始位置（Home位置）
         std::vector<double> wheel_home_positions_;     ///< 轮子初始位置（Home位置）
+        std::vector<double> left_arm_home_positions_;  ///< 左臂初始位置（Home位置，弧度）
+        std::vector<double> right_arm_home_positions_; ///< 右臂初始位置（Home位置，弧度）
 
         std::vector<double> waist_zero_positions_;     ///< 腰部零点位置
         std::vector<double> leg_zero_positions_;       ///< 腿部零点位置
         std::vector<double> wheel_zero_positions_;     ///< 轮子零点位置
+        std::vector<double> left_arm_zero_positions_;  ///< 左臂零点位置（弧度）
+        std::vector<double> right_arm_zero_positions_; ///< 右臂零点位置（弧度）
 
         // ---------- 限位参数 ----------
         std::vector<JointLimit> joint_limits_;         ///< 关节限制（包含所有关节）
@@ -292,18 +386,26 @@ namespace robot_control
         std::vector<double> leg_max_limit_;            ///< 腿部最大限位
         std::vector<double> wheel_min_limit_;          ///< 轮子最小限位
         std::vector<double> wheel_max_limit_;          ///< 轮子最大限位
+        std::vector<double> left_arm_min_limit_;       ///< 左臂最小限位（度）
+        std::vector<double> left_arm_max_limit_;       ///< 左臂最大限位（度）
+        std::vector<double> right_arm_min_limit_;      ///< 右臂最小限位（度）
+        std::vector<double> right_arm_max_limit_;      ///< 右臂最大限位（度）
 
         // ---------- 速度参数 ----------
         std::vector<double> max_joint_velocity_;       ///< 所有关节的最大速度
         std::vector<double> waist_max_velocity_;       ///< 腰部最大速度
         std::vector<double> leg_max_velocity_;         ///< 腿部最大速度
         std::vector<double> wheel_max_velocity_;       ///< 轮子最大速度
+        std::vector<double> left_arm_max_velocity_;    ///< 左臂最大速度（度/秒）
+        std::vector<double> right_arm_max_velocity_;   ///< 右臂最大速度（度/秒）
 
         // ---------- 加速度参数 ----------
         std::vector<double> max_joint_acceleration_;   ///< 所有关节的最大加速度
         std::vector<double> waist_max_acceleration_;   ///< 腰部最大加速度
         std::vector<double> leg_max_acceleration_;     ///< 腿部最大加速度
         std::vector<double> wheel_max_acceleration_;   ///< 轮子最大加速度
+        std::vector<double> left_arm_max_acceleration_;  ///< 左臂最大加速度（度/秒²）
+        std::vector<double> right_arm_max_acceleration_; ///< 右臂最大加速度（度/秒²）
 
         // ---------- 轮子控制参数 ----------
         double wheel_radius_;              ///< 轮子半径（米）
@@ -316,6 +418,8 @@ namespace robot_control
         std::vector<double> leg_length_;   ///< 腿部长度参数（米）
         std::vector<double> waist_length_; ///< 腰部长度参数（米）
         std::vector<double> wheel_length_; ///< 轮子长度参数（米）
+        std::vector<double> left_arm_length_;  ///< 左臂长度参数（米）
+        std::vector<double> right_arm_length_; ///< 右臂长度参数（米）
 
         // ---------- 关节传动参数 ----------
         std::vector<double> joint_gear_ratio_;     ///< 关节传动比

src/robot_control/include/core/robot_control_manager.hpp

@@ -329,7 +329,7 @@ namespace robot_control
         std::unique_ptr<LegControl> leg_controller_;     ///< 腿部控制器
         std::unique_ptr<WheelControl> wheel_controller_; ///< 轮子控制器
         std::unique_ptr<WaistControl> waist_controller_; ///< 腰部控制器
-        std::unique_ptr<ArmControl> arm_controller_;     ///< 手臂控制器（可选）
+        std::unique_ptr<ArmControl> arm_controller_;     ///< 手臂控制器
 
         // 控制器启用标志
         bool leg_controller_enabled_;     ///< 腿部控制器是否启用

src/robot_control/include/core/robot_control_node.hpp

@@ -115,13 +115,8 @@ namespace robot_control
 
         // ==================== 控制状态 ====================
         
-        bool isStopping_;    ///< 是否正在停止
-        bool isJogMode_;     ///< 是否处于点动模式
-        int jogDirection_;   ///< 点动方向（-1, 0, 1）
-        size_t jogIndex_;    ///< 当前点动的关节索引
         double jogValue_;    ///< 点动值
         double lastSpeed_;   ///< 上次速度
-        std::string lastJoyCommand_;  ///< 上次处理的遥控器命令（用于去重）
 
         // ==================== 回调函数 ====================
         

src/robot_control/include/utils/s_curve_trajectory.hpp

@@ -0,0 +1,210 @@
+#pragma once
+
+#include <vector>
+#include <cmath>
+#include <algorithm>
+
+namespace robot_control
+{
+
+/**
+ * @brief 多轴S型曲线轨迹规划器（支持各轴独立速度/加速度/Jerk约束）
+ * 保证所有轴同时启动、同时结束，各阶段时间完全同步
+ * S型曲线包含7个阶段：加加速、匀加速、减加速、匀速、加减速、匀减速、减减速
+ */
+class S_CurveTrajectory
+{
+public:
+    /**
+     * @brief 构造函数（多轴版本）
+     * @param max_velocities 各轴最大速度约束（向量，长度=轴数）
+     * @param max_accelerations 各轴最大加速度约束（向量，长度=轴数）
+     * @param max_jerks 各轴最大Jerk约束（向量，长度=轴数），默认值为max_acceleration*10
+     */
+    S_CurveTrajectory(const std::vector<double>& max_velocities, 
+                     const std::vector<double>& max_accelerations,
+                     const std::vector<double>& max_jerks = std::vector<double>());
+    
+    /**
+     * @brief 构造函数（单轴版本）
+     * @param max_velocity 最大速度约束
+     * @param max_acceleration 最大加速度约束
+     * @param max_jerk 最大Jerk约束，默认值为max_acceleration*10
+     */
+    S_CurveTrajectory(double max_velocity = 1.0, 
+                     double max_acceleration = 0.5, 
+                     double max_jerk = 5.0);
+    
+    /**
+     * @brief 析构函数
+     */
+    ~S_CurveTrajectory() = default;
+    
+    /**
+     * @brief 初始化多轴轨迹规划
+     * @param start_pos 起始位置（向量，长度=轴数）
+     * @param target_pos 目标位置（向量，长度=轴数）
+     */
+    void init(const std::vector<double>& start_pos, const std::vector<double>& target_pos);
+    
+    /**
+     * @brief 初始化单轴轨迹规划
+     * @param start_pos 起始位置
+     * @param target_pos 目标位置
+     */
+    void init(double start_pos, double target_pos);
+    
+    /**
+     * @brief 更新多轴轨迹，计算当前时间的位置
+     * @param time 已运动时间（s）
+     * @return 当前位置向量
+     */
+    std::vector<double> update(double time);
+    
+    /**
+     * @brief 更新单轴轨迹，计算当前时间的位置
+     * @param time 已运动时间（s）
+     * @return 当前位置
+     */
+    double updateSingle(double time);
+    
+    /**
+     * @brief 获取当前速度（多轴）
+     */
+    std::vector<double> getVelocity() const { return current_velocity_; }
+    
+    /**
+     * @brief 获取当前速度（单轴）
+     */
+    double getSingleVelocity() const { return current_velocity_[0]; }
+    
+    /**
+     * @brief 获取当前加速度（多轴）
+     */
+    std::vector<double> getAcceleration() const { return current_acceleration_; }
+    
+    /**
+     * @brief 获取当前加速度（单轴）
+     */
+    double getSingleAcceleration() const { return current_acceleration_[0]; }
+    
+    /**
+     * @brief 检查轨迹是否已完成
+     * @param time 已运动时间（s）
+     * @return 完成返回true
+     */
+    bool isFinished(double time) const { return time >= total_time_; }
+
+    /**
+     * @brief 设置各轴最大速度
+     * @param max_velocities 新的最大速度向量（长度=轴数）
+     */
+    void setMaxVelocities(const std::vector<double>& max_velocities);
+    
+    /**
+     * @brief 设置单轴最大速度
+     * @param max_velocity 新的最大速度
+     */
+    void setMaxVelocity(double max_velocity);
+    
+    /**
+     * @brief 设置各轴最大加速度
+     * @param max_accelerations 新的最大加速度向量（长度=轴数）
+     */
+    void setMaxAccelerations(const std::vector<double>& max_accelerations);
+    
+    /**
+     * @brief 设置单轴最大加速度
+     * @param max_acceleration 新的最大加速度
+     */
+    void setMaxAcceleration(double max_acceleration);
+
+    /**
+     * @brief 计算急停轨迹参数（当前状态下减速到停止）
+     */
+    void calculateStopTrajectory();
+    
+    /**
+     * @brief 更新急停轨迹
+     * @param dt 从急停开始的时间（s）
+     * @return 当前位置
+     */
+    std::vector<double> updateStop(double dt);
+    
+    /**
+     * @brief 检查急停是否完成
+     * @param dt 从急停开始的时间（s）
+     * @return 完成返回true
+     */
+    bool isStopFinished(double dt) const { return dt >= stop_total_time_; }
+
+private:
+    /**
+     * @brief 计算轨迹核心参数（统一时间+各轴参数）
+     */
+    void calculateTrajectoryParams();
+    
+    /**
+     * @brief 计算单轴的S型曲线阶段时间
+     * @param dist 位移距离（绝对值）
+     * @param max_v 最大速度
+     * @param max_a 最大加速度
+     * @param max_j 最大Jerk
+     * @param[out] T1 加加速阶段时间
+     * @param[out] T2 匀加速阶段时间
+     * @param[out] T3 减加速阶段时间
+     * @param[out] T4 匀速阶段时间
+     * @param[out] T5 加减速阶段时间
+     * @param[out] T6 匀减速阶段时间
+     * @param[out] T7 减减速阶段时间
+     * @param[out] v_max 实际达到的最大速度
+     */
+    void calculateScurveTimes(double dist, double max_v, double max_a, double max_j,
+                              double& T1, double& T2, double& T3, double& T4,
+                              double& T5, double& T6, double& T7, double& v_max);
+
+    std::vector<double> start_pos_;         // 起始位置（各轴）
+    std::vector<double> target_pos_;        // 目标位置（各轴）
+    std::vector<double> total_distance_;    // 总位移绝对值（各轴）
+    std::vector<double> current_pos_;       // 当前位置（各轴）
+    std::vector<double> current_velocity_;  // 当前速度（各轴）
+    std::vector<double> current_acceleration_; // 当前加速度（各轴）
+
+    // 轨迹参数（各轴独立）
+    std::vector<double> max_velocity_;      // 最大速度（各轴独立）
+    std::vector<double> max_acceleration_;  // 最大加速度（各轴独立）
+    std::vector<double> max_jerk_;          // 最大Jerk（各轴独立）
+    
+    // S曲线阶段时间（各轴独立，但会统一到最大值）
+    std::vector<double> T1_;  // 加加速阶段时间
+    std::vector<double> T2_;  // 匀加速阶段时间
+    std::vector<double> T3_;  // 减加速阶段时间
+    std::vector<double> T4_;  // 匀速阶段时间
+    std::vector<double> T5_;  // 加减速阶段时间
+    std::vector<double> T6_;  // 匀减速阶段时间
+    std::vector<double> T7_;  // 减减速阶段时间
+    std::vector<double> v_max_;  // 实际达到的最大速度（各轴）
+
+    // 统一时间参数（所有轴共用，取各轴最大值）
+    double total_time_;         // 总运动时间
+    double T1_total_;           // 加加速阶段时间
+    double T2_total_;           // 匀加速阶段时间
+    double T3_total_;           // 减加速阶段时间
+    double T4_total_;           // 匀速阶段时间
+    double T5_total_;           // 加减速阶段时间
+    double T6_total_;           // 匀减速阶段时间
+    double T7_total_;           // 减减速阶段时间
+
+    // 急停相关参数
+    bool is_stopping_;              // 是否处于急停状态
+    double stop_total_time_;        // 急停总时间（统一）
+    std::vector<double> stop_start_pos_;  // 急停起始位置
+    std::vector<double> stop_start_vel_;  // 急停起始速度
+    std::vector<double> stop_start_acc_;  // 急停起始加速度
+    std::vector<double> stop_jerk_;       // 急停Jerk（各轴独立）
+
+    bool is_single_joint_;  // 是否为单轴模式
+};
+
+}  // namespace robot_control
+

src/robot_control/src/controllers/arm_control.cpp

@@ -3,47 +3,51 @@
 #include <stdexcept>
 #include <vector>
 #include <memory>
+#include <cmath>
+#include <algorithm>
 #include "rclcpp/rclcpp.hpp"
 #include "moveit/move_group_interface/move_group_interface.h"
 #include "moveit/planning_interface/planning_interface.h"
 #include "moveit_msgs/msg/move_it_error_codes.hpp"
 
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
 namespace robot_control
 {
 
 ArmControl::ArmControl(
-    size_t total_joints, 
-    const std::vector<double>& lengthParameters, 
+    size_t total_joints,
+    const std::vector<double>& lengthParameters,
     const std::vector<double>& maxSpeed,
     const std::vector<double>& maxAcc,
-    const std::vector<double>& minLimits, 
-    const std::vector<double>& maxLimits, 
+    const std::vector<double>& minLimits,
+    const std::vector<double>& maxLimits,
     const std::vector<double>& home_positions,
     const std::vector<double>& zero_positions,
-    const std::vector<int>& joint_directions
-): ControlBase(
-    total_joints, 
-    lengthParameters, 
-    maxSpeed, maxAcc, 
-    minLimits, 
-    maxLimits, 
-    home_positions, 
-    zero_positions,
-    joint_directions)
-    , move_group_(nullptr)
+    const std::vector<int>& joint_directions)
+    : ControlBase(total_joints, lengthParameters, maxSpeed, maxAcc,
+                  minLimits, maxLimits, home_positions, zero_positions, joint_directions)
+    , move_group_left_(nullptr)
+    , move_group_right_(nullptr)
+    , move_group_dual_(nullptr)
     , node_(nullptr)
+    , current_arm_id_(0)
 {
-    std::cout << "ArmControl Constructor" << std::endl;
-    // 注意：ControlBase已经初始化了所有基本成员变量
+    std::cout << "ArmControl Constructor: total_joints=" << total_joints << std::endl;
+    // MoveIt配置从SRDF/URDF文件加载，但基础参数从MotionParameters获取
 }
 
 ArmControl::~ArmControl()
 {
     // MoveGroupInterface是shared_ptr，会自动释放
-    move_group_.reset();
+    move_group_left_.reset();
+    move_group_right_.reset();
+    move_group_dual_.reset();
 }
 
-bool ArmControl::InitializeMoveIt(rclcpp::Node* node, const std::string& move_group_name)
+bool ArmControl::InitializeMoveIt(rclcpp::Node* node)
 {
     if (!node) {
         std::cerr << "ArmControl::InitializeMoveIt: node is null" << std::endl;
@@ -51,7 +55,6 @@ bool ArmControl::InitializeMoveIt(rclcpp::Node* node, const std::string& move_gr
     }
 
     node_ = node;
-    move_group_name_ = move_group_name;
 
     try {
         // 创建一个shared_ptr包装node（MoveGroupInterface需要shared_ptr）
@@ -59,16 +62,33 @@ bool ArmControl::InitializeMoveIt(rclcpp::Node* node, const std::string& move_gr
             // 空的删除器，因为我们不拥有node的所有权
         });
 
-        move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
-            node_shared, move_group_name);
-
-        if (!move_group_) {
-            std::cerr << "ArmControl::InitializeMoveIt: Failed to create MoveGroupInterface" << std::endl;
+        // 初始化左臂move group
+        move_group_left_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
+            node_shared, "arm_left");
+        if (!move_group_left_) {
+            std::cerr << "ArmControl::InitializeMoveIt: Failed to create MoveGroupInterface for arm_left" << std::endl;
             return false;
         }
+        std::cout << "ArmControl::InitializeMoveIt: MoveGroupInterface initialized for arm_left" << std::endl;
+
+        // 初始化右臂move group
+        move_group_right_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
+            node_shared, "arm_right");
+        if (!move_group_right_) {
+            std::cerr << "ArmControl::InitializeMoveIt: Failed to create MoveGroupInterface for arm_right" << std::endl;
+            return false;
+        }
+        std::cout << "ArmControl::InitializeMoveIt: MoveGroupInterface initialized for arm_right" << std::endl;
+
+        // 初始化双臂move group
+        move_group_dual_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
+            node_shared, "dual_arm");
+        if (!move_group_dual_) {
+            std::cerr << "ArmControl::InitializeMoveIt: Failed to create MoveGroupInterface for dual_arm" << std::endl;
+            return false;
+        }
+        std::cout << "ArmControl::InitializeMoveIt: MoveGroupInterface initialized for dual_arm" << std::endl;
 
-        std::cout << "ArmControl::InitializeMoveIt: MoveGroupInterface initialized for " 
-                  << move_group_name << std::endl;
         return true;
     } catch (const std::exception& e) {
         std::cerr << "ArmControl::InitializeMoveIt: Exception: " << e.what() << std::endl;
@@ -76,49 +96,108 @@ bool ArmControl::InitializeMoveIt(rclcpp::Node* node, const std::string& move_gr
     }
 }
 
+std::shared_ptr<moveit::planning_interface::MoveGroupInterface> ArmControl::GetMoveGroup(uint8_t arm_id) const
+{
+    switch (arm_id) {
+        case 0:  // ARM_LEFT
+            return move_group_left_;
+        case 1:  // ARM_RIGHT
+            return move_group_right_;
+        case 2:  // ARM_DUAL
+            return move_group_dual_;
+        default:
+            std::cerr << "ArmControl::GetMoveGroup: Invalid arm_id " << static_cast<int>(arm_id) << std::endl;
+            return nullptr;
+    }
+}
+
+bool ArmControl::IsMoveItInitialized() const
+{
+    return move_group_left_ != nullptr && 
+           move_group_right_ != nullptr && 
+           move_group_dual_ != nullptr;
+}
+
 bool ArmControl::PlanJointMotion(
-    uint8_t /* arm_id */,
+    uint8_t arm_id,
     const std::vector<double>& target_joints,
     double velocity_scaling,
     double acceleration_scaling,
     std::vector<std::vector<double>>& trajectory_points)
 {
-    if (!move_group_) {
-        std::cerr << "ArmControl::PlanJointMotion: MoveIt not initialized" << std::endl;
+    auto move_group = GetMoveGroup(arm_id);
+    if (!move_group) {
+        std::cerr << "ArmControl::PlanJointMotion: Invalid arm_id or MoveIt not initialized" << std::endl;
         return false;
     }
 
     try {
         // 设置速度和加速度缩放因子
-        move_group_->setMaxVelocityScalingFactor(velocity_scaling);
-        move_group_->setMaxAccelerationScalingFactor(acceleration_scaling);
+        move_group->setMaxVelocityScalingFactor(velocity_scaling);
+        move_group->setMaxAccelerationScalingFactor(acceleration_scaling);
 
         // 设置目标关节值（弧度）
-        move_group_->setJointValueTarget(target_joints);
+        move_group->setJointValueTarget(target_joints);
 
         // 规划
         moveit::planning_interface::MoveGroupInterface::Plan plan;
-        moveit::core::MoveItErrorCode error_code = move_group_->plan(plan);
+        moveit::core::MoveItErrorCode error_code = move_group->plan(plan);
 
         if (error_code != moveit::core::MoveItErrorCode::SUCCESS) {
             std::cerr << "ArmControl::PlanJointMotion: Planning failed with error code: " 
-                      << error_code.val << std::endl;
+                      << error_code.val << " for arm_id " << static_cast<int>(arm_id) << std::endl;
             return false;
         }
 
-        // 提取轨迹点
+        // 提取轨迹点（为了向后兼容，仍返回位置信息）
         trajectory_points.clear();
-        if (plan.trajectory_.joint_trajectory.points.size() > 0) {
-            for (const auto& point : plan.trajectory_.joint_trajectory.points) {
+        const auto& trajectory = plan.trajectory_.joint_trajectory;
+        if (trajectory.points.size() > 0) {
+            // 存储完整轨迹信息到TrajectoryState
+            TrajectoryState* traj_state = (arm_id == 0) ? &left_arm_trajectory_ : &right_arm_trajectory_;
+            traj_state->trajectory_points.clear();
+            size_t num_joints = trajectory.points[0].positions.size();
+            
+            for (const auto& point : trajectory.points) {
+                // 提取位置信息（用于返回）
                 trajectory_points.push_back(point.positions);
+                
+                // 存储完整轨迹信息（位置、速度、加速度、时间）
+                TrajectoryPoint traj_point;
+                traj_point.positions = point.positions;
+                
+                // 提取速度信息（如果存在）
+                if (point.velocities.size() == num_joints) {
+                    traj_point.velocities = point.velocities;
+                } else {
+                    traj_point.velocities.resize(num_joints, 0.0);
+                }
+                
+                // 提取加速度信息（如果存在）
+                if (point.accelerations.size() == num_joints) {
+                    traj_point.accelerations = point.accelerations;
+                } else {
+                    traj_point.accelerations.resize(num_joints, 0.0);
+                }
+                
+                // 提取时间信息
+                traj_point.time_from_start = point.time_from_start.sec + 
+                                            point.time_from_start.nanosec * 1e-9;
+                
+                traj_state->trajectory_points.push_back(traj_point);
             }
+            
+            traj_state->current_index = 0;
+            traj_state->elapsed_time = 0.0;
+            traj_state->is_active = true;
         } else {
             std::cerr << "ArmControl::PlanJointMotion: Trajectory is empty" << std::endl;
             return false;
         }
 
-        std::cout << "ArmControl::PlanJointMotion: Planning successful, " 
-                  << trajectory_points.size() << " trajectory points generated" << std::endl;
+        std::cout << "ArmControl::PlanJointMotion: Planning successful for arm_id " 
+                  << static_cast<int>(arm_id) << ", " << trajectory_points.size() 
+                  << " trajectory points generated" << std::endl;
         return true;
     } catch (const std::exception& e) {
         std::cerr << "ArmControl::PlanJointMotion: Exception: " << e.what() << std::endl;
@@ -127,48 +206,85 @@ bool ArmControl::PlanJointMotion(
 }
 
 bool ArmControl::PlanCartesianMotion(
-    uint8_t /* arm_id */,
+    uint8_t arm_id,
     const geometry_msgs::msg::Pose& target_pose,
     double velocity_scaling,
     double acceleration_scaling,
     std::vector<std::vector<double>>& trajectory_points)
 {
-    if (!move_group_) {
-        std::cerr << "ArmControl::PlanCartesianMotion: MoveIt not initialized" << std::endl;
+    auto move_group = GetMoveGroup(arm_id);
+    if (!move_group) {
+        std::cerr << "ArmControl::PlanCartesianMotion: Invalid arm_id or MoveIt not initialized" << std::endl;
         return false;
     }
 
     try {
         // 设置速度和加速度缩放因子
-        move_group_->setMaxVelocityScalingFactor(velocity_scaling);
-        move_group_->setMaxAccelerationScalingFactor(acceleration_scaling);
+        move_group->setMaxVelocityScalingFactor(velocity_scaling);
+        move_group->setMaxAccelerationScalingFactor(acceleration_scaling);
 
         // 设置目标位姿
-        move_group_->setPoseTarget(target_pose);
+        move_group->setPoseTarget(target_pose);
 
         // 规划
         moveit::planning_interface::MoveGroupInterface::Plan plan;
-        moveit::core::MoveItErrorCode error_code = move_group_->plan(plan);
+        moveit::core::MoveItErrorCode error_code = move_group->plan(plan);
 
         if (error_code != moveit::core::MoveItErrorCode::SUCCESS) {
             std::cerr << "ArmControl::PlanCartesianMotion: Planning failed with error code: " 
-                      << error_code.val << std::endl;
+                      << error_code.val << " for arm_id " << static_cast<int>(arm_id) << std::endl;
             return false;
         }
 
-        // 提取轨迹点
+        // 提取轨迹点（为了向后兼容，仍返回位置信息）
         trajectory_points.clear();
-        if (plan.trajectory_.joint_trajectory.points.size() > 0) {
-            for (const auto& point : plan.trajectory_.joint_trajectory.points) {
+        const auto& trajectory = plan.trajectory_.joint_trajectory;
+        if (trajectory.points.size() > 0) {
+            // 存储完整轨迹信息到TrajectoryState
+            TrajectoryState* traj_state = (arm_id == 0) ? &left_arm_trajectory_ : &right_arm_trajectory_;
+            traj_state->trajectory_points.clear();
+            size_t num_joints = trajectory.points[0].positions.size();
+            
+            for (const auto& point : trajectory.points) {
+                // 提取位置信息（用于返回）
                 trajectory_points.push_back(point.positions);
+                
+                // 存储完整轨迹信息（位置、速度、加速度、时间）
+                TrajectoryPoint traj_point;
+                traj_point.positions = point.positions;
+                
+                // 提取速度信息（如果存在）
+                if (point.velocities.size() == num_joints) {
+                    traj_point.velocities = point.velocities;
+                } else {
+                    traj_point.velocities.resize(num_joints, 0.0);
+                }
+                
+                // 提取加速度信息（如果存在）
+                if (point.accelerations.size() == num_joints) {
+                    traj_point.accelerations = point.accelerations;
+                } else {
+                    traj_point.accelerations.resize(num_joints, 0.0);
+                }
+                
+                // 提取时间信息
+                traj_point.time_from_start = point.time_from_start.sec + 
+                                            point.time_from_start.nanosec * 1e-9;
+                
+                traj_state->trajectory_points.push_back(traj_point);
             }
+            
+            traj_state->current_index = 0;
+            traj_state->elapsed_time = 0.0;
+            traj_state->is_active = true;
         } else {
             std::cerr << "ArmControl::PlanCartesianMotion: Trajectory is empty" << std::endl;
             return false;
         }
 
-        std::cout << "ArmControl::PlanCartesianMotion: Planning successful, " 
-                  << trajectory_points.size() << " trajectory points generated" << std::endl;
+        std::cout << "ArmControl::PlanCartesianMotion: Planning successful for arm_id " 
+                  << static_cast<int>(arm_id) << ", " << trajectory_points.size() 
+                  << " trajectory points generated" << std::endl;
         return true;
     } catch (const std::exception& e) {
         std::cerr << "ArmControl::PlanCartesianMotion: Exception: " << e.what() << std::endl;
@@ -178,15 +294,37 @@ bool ArmControl::PlanCartesianMotion(
 
 void ArmControl::StoreTrajectory(const std::vector<std::vector<double>>& trajectory_points, uint8_t arm_id)
 {
+    // 注意：此函数保留用于向后兼容，但现在主要使用 StoreTrajectoryFromPlan
+    // 如果只提供位置信息，速度和加速度将被设置为0
     TrajectoryState* traj_state = (arm_id == 0) ? &left_arm_trajectory_ : &right_arm_trajectory_;
     
-    traj_state->trajectory_points = trajectory_points;
+    traj_state->trajectory_points.clear();
+    
+    if (trajectory_points.empty()) {
+        traj_state->is_active = false;
+        return;
+    }
+    
+    size_t num_joints = trajectory_points[0].size();
+    double time_step = 0.0;  // 如果没有时间信息，使用索引作为时间
+    
+    // 将位置向量转换为完整的轨迹点（速度和加速度设为0）
+    for (size_t i = 0; i < trajectory_points.size(); ++i) {
+        TrajectoryPoint traj_point;
+        traj_point.positions = trajectory_points[i];
+        traj_point.velocities.resize(num_joints, 0.0);
+        traj_point.accelerations.resize(num_joints, 0.0);
+        traj_point.time_from_start = time_step * i;  // 简单的时间估计
+        
+        traj_state->trajectory_points.push_back(traj_point);
+    }
+    
     traj_state->current_index = 0;
     traj_state->elapsed_time = 0.0;
-    traj_state->is_active = !trajectory_points.empty();
+    traj_state->is_active = true;
     
     std::cout << "ArmControl::StoreTrajectory: Stored " << trajectory_points.size() 
-              << " trajectory points for arm_id " << static_cast<int>(arm_id) << std::endl;
+              << " trajectory points (positions only) for arm_id " << static_cast<int>(arm_id) << std::endl;
 }
 
 bool ArmControl::GetInterpolatedPoint(uint8_t arm_id, double dt, std::vector<double>& joint_positions)
@@ -203,8 +341,20 @@ bool ArmControl::GetInterpolatedPoint(uint8_t arm_id, double dt, std::vector<dou
         return false;
     }
 
-    // 直接返回当前索引的点位（简单实现，也可以做插值）
-    joint_positions = traj_state->trajectory_points[traj_state->current_index];
+    // 获取当前轨迹点
+    const TrajectoryPoint& current_point = traj_state->trajectory_points[traj_state->current_index];
+    
+    // 提取位置、速度、加速度信息
+    joint_positions = current_point.positions;
+    
+    // 更新基类的速度和加速度数据成员
+    if (current_point.velocities.size() == joint_velocity_.size()) {
+        joint_velocity_ = current_point.velocities;
+    }
+    
+    if (current_point.accelerations.size() == joint_acceleration_.size()) {
+        joint_acceleration_ = current_point.accelerations;
+    }
     
     // 更新索引和时间
     traj_state->current_index++;
@@ -238,4 +388,88 @@ void ArmControl::ClearTrajectory(uint8_t arm_id)
               << static_cast<int>(arm_id) << std::endl;
 }
 
+// ==================== 重写ControlBase的方法 ====================
+
+bool ArmControl::MoveToTargetJoint(
+    std::vector<double>& joint_positions,
+    const std::vector<double>& target_joint,
+    double dt)
+{
+    // 如果没有MoveIt初始化，回退到基类的实现（使用梯形轨迹）
+    if (!IsMoveItInitialized())
+    {
+        return ControlBase::MoveToTargetJoint(joint_positions, target_joint, dt);
+    }
+    
+    // 使用MoveIt规划轨迹
+    // 如果当前没有活动轨迹，进行规划
+    if (!is_moving_)
+    {
+        std::vector<std::vector<double>> trajectory_points;
+        
+        // 使用默认的arm_id（左臂，如果需要可以根据target_joint判断）
+        uint8_t arm_id = current_arm_id_;
+        
+        // 规划关节空间运动
+        // 注意：PlanJointMotion 内部已经调用了 StoreTrajectoryFromPlan 存储完整轨迹信息
+        if (PlanJointMotion(arm_id, target_joint, 0.5, 0.5, trajectory_points))
+        {
+            is_moving_ = true;
+            movingDurationTime_ = 0.0;
+        }
+        else
+        {
+            // 规划失败，回退到基类实现
+            return ControlBase::MoveToTargetJoint(joint_positions, target_joint, dt);
+        }
+    }
+    
+    // 执行轨迹插补
+    std::vector<double> interpolated_joints;
+    
+    if (GetInterpolatedPoint(current_arm_id_, dt, interpolated_joints))
+    {
+        joint_positions = interpolated_joints;
+        joint_commands_ = interpolated_joints;
+        // 注意：GetInterpolatedPoint 已经更新了 joint_velocity_ 和 joint_acceleration_
+        movingDurationTime_ += dt;
+        return false;  // 还在运动
+    }
+    else
+    {
+        // 轨迹执行完成
+        joint_positions = target_joint;
+        joint_commands_ = target_joint;
+        // 轨迹完成时，将速度和加速度设置为0
+        std::fill(joint_velocity_.begin(), joint_velocity_.end(), 0.0);
+        std::fill(joint_acceleration_.begin(), joint_acceleration_.end(), 0.0);
+        is_moving_ = false;
+        movingDurationTime_ = 0.0;
+        ClearTrajectory(current_arm_id_);
+        return true;  // 到达目标
+    }
+}
+
+bool ArmControl::GoHome(std::vector<double>& joint_positions, double dt)
+{
+    // 使用基类的home位置
+    return MoveToTargetJoint(joint_positions, joint_home_positions_, dt);
+}
+
+bool ArmControl::Stop(std::vector<double>& joint_positions, double dt)
+{
+    // 清除MoveIt轨迹
+    if (left_arm_trajectory_.is_active)
+    {
+        ClearTrajectory(0);
+    }
+    if (right_arm_trajectory_.is_active)
+    {
+        ClearTrajectory(1);
+    }
+    
+    // 调用基类的停止方法
+    return ControlBase::Stop(joint_positions, dt);
+}
+
 }  // namespace robot_control

src/robot_control/src/controllers/control_base.cpp

@@ -24,7 +24,8 @@ ControlBase::ControlBase(
     lengthParameters_(lengthParameters),
     joint_directions_(joint_directions),
     joint_zero_positions_(zero_positions),
-    is_target_set_(false)
+    is_target_set_(false),
+    planner_type_(TrajectoryPlannerType::TRAPEZOIDAL)  // 默认使用梯形轨迹规划器
 {
     if (total_joints_ <= 0)
         throw std::invalid_argument("Total joints must be positive");
@@ -39,6 +40,9 @@ ControlBase::ControlBase(
     
     // 初始化梯形轨迹规划器（使用多轴版本）
     trapezoidalTrajectory_ = new TrapezoidalTrajectory(maxSpeed, maxAcc);
+    
+    // 初始化S型曲线轨迹规划器（使用多轴版本，默认Jerk为加速度的10倍）
+    s_curveTrajectory_ = new S_CurveTrajectory(maxSpeed, maxAcc);
 
     is_moving_ = false;
     is_stopping_ = false;
@@ -51,6 +55,7 @@ ControlBase::ControlBase(
 ControlBase::~ControlBase()
 {
     delete trapezoidalTrajectory_;
+    delete s_curveTrajectory_;
 }
 
 void ControlBase::SetHomePositions(const std::vector<double>& home_positions)
@@ -75,18 +80,31 @@ bool ControlBase::MoveToTargetJoint(std::vector<double>& joint_positions, const
             return true; 
         }
         
-        // 从当前关节位置开始规划轨迹
-        trapezoidalTrajectory_->init(joint_position_, target_joint);
+        // 从当前关节位置开始规划轨迹（根据规划器类型选择）
+        if (planner_type_ == TrajectoryPlannerType::TRAPEZOIDAL) {
+            trapezoidalTrajectory_->init(joint_position_, target_joint);
+        } else {
+            s_curveTrajectory_->init(joint_position_, target_joint);
+        }
         movingDurationTime_ = 0.0;       
         is_moving_ = true;     
     }
 
     movingDurationTime_ += dt;
 
-    std::vector<double> desired_pos = trapezoidalTrajectory_->update(movingDurationTime_);
+    // 根据规划器类型更新轨迹
+    std::vector<double> desired_pos;
+    bool is_finished;
+    if (planner_type_ == TrajectoryPlannerType::TRAPEZOIDAL) {
+        desired_pos = trapezoidalTrajectory_->update(movingDurationTime_);
+        is_finished = trapezoidalTrajectory_->isFinished(movingDurationTime_);
+    } else {
+        desired_pos = s_curveTrajectory_->update(movingDurationTime_);
+        is_finished = s_curveTrajectory_->isFinished(movingDurationTime_);
+    }
 
     // 检查是否到达目标位置
-    if (trapezoidalTrajectory_->isFinished(movingDurationTime_))
+    if (is_finished)
     {
         joint_positions = target_joint;
         joint_commands_ = target_joint;
@@ -98,8 +116,15 @@ bool ControlBase::MoveToTargetJoint(std::vector<double>& joint_positions, const
 
     joint_commands_ = desired_pos;
     joint_positions = desired_pos;
-    joint_velocity_ = trapezoidalTrajectory_->getVelocity();
-    joint_acceleration_ = trapezoidalTrajectory_->getAcceleration();
+    
+    // 获取速度和加速度（根据规划器类型）
+    if (planner_type_ == TrajectoryPlannerType::TRAPEZOIDAL) {
+        joint_velocity_ = trapezoidalTrajectory_->getVelocity();
+        joint_acceleration_ = trapezoidalTrajectory_->getAcceleration();
+    } else {
+        joint_velocity_ = s_curveTrajectory_->getVelocity();
+        joint_acceleration_ = s_curveTrajectory_->getAcceleration();
+    }
 
     return false;
 }
@@ -120,15 +145,29 @@ bool ControlBase::Stop(std::vector<double>& joint_positions, double dt)
     if (is_moving_ && !is_stopping_)
     {
         is_stopping_ = true;
-        trapezoidalTrajectory_->calculateStopTrajectory();
+        // 根据规划器类型计算急停轨迹
+        if (planner_type_ == TrajectoryPlannerType::TRAPEZOIDAL) {
+            trapezoidalTrajectory_->calculateStopTrajectory();
+        } else {
+            s_curveTrajectory_->calculateStopTrajectory();
+        }
         stopDurationTime_ = 0.0;
     }
 
     if (is_stopping_)
     {
         stopDurationTime_ += dt;
-        joint_positions = trapezoidalTrajectory_->updateStop(stopDurationTime_);
-        if (trapezoidalTrajectory_->isStopFinished(stopDurationTime_))
+        // 根据规划器类型更新急停轨迹
+        bool is_stop_finished;
+        if (planner_type_ == TrajectoryPlannerType::TRAPEZOIDAL) {
+            joint_positions = trapezoidalTrajectory_->updateStop(stopDurationTime_);
+            is_stop_finished = trapezoidalTrajectory_->isStopFinished(stopDurationTime_);
+        } else {
+            joint_positions = s_curveTrajectory_->updateStop(stopDurationTime_);
+            is_stop_finished = s_curveTrajectory_->isStopFinished(stopDurationTime_);
+        }
+        
+        if (is_stop_finished)
         {
             is_moving_ = false;
             is_stopping_ = false;
@@ -184,6 +223,11 @@ bool ControlBase::IsReached(const std::vector<double>& target_joint)
     return result;
 }
 
+void ControlBase::SetTrajectoryPlannerType(TrajectoryPlannerType planner_type)
+{
+    planner_type_ = planner_type;
+}
+
 bool ControlBase::checkJointLimits(std::vector<double>& joint_positions)
 {
     for (size_t i = 0; i < joint_positions.size() && i < joint_min_limits_.size() && i < joint_max_limits_.size(); i++)

src/robot_control/src/core/controller_factory.cpp

@@ -36,11 +36,87 @@ std::unique_ptr<ControlBase> ControllerFactory::create_controller(
                 motion_params.leg_zero_positions_,
                 motion_params.leg_joint_directions_);
 
-        case ControllerType::ARM_CONTROLLER:
-            // ARM controller not currently implemented in MotionParameters
-            // Return nullptr or throw exception if ARM is requested but not configured
-            // TODO: Add ARM parameters to MotionParameters if needed
-            return nullptr;
+        case ControllerType::ARM_CONTROLLER: {
+            // ARM controller 继承自 ControlBase，需要从 MotionParameters 获取参数
+            // 每个臂6个关节，总共12个关节（左臂+右臂）
+            const size_t arm_total_joints = motion_params.left_arm_joint_indices_.size() + 
+                                            motion_params.right_arm_joint_indices_.size();
+            
+            // 合并左臂和右臂的参数
+            std::vector<double> arm_length_params;
+            arm_length_params.insert(arm_length_params.end(), 
+                                     motion_params.left_arm_length_.begin(), 
+                                     motion_params.left_arm_length_.end());
+            arm_length_params.insert(arm_length_params.end(), 
+                                     motion_params.right_arm_length_.begin(), 
+                                     motion_params.right_arm_length_.end());
+            
+            std::vector<double> arm_max_speed;
+            arm_max_speed.insert(arm_max_speed.end(), 
+                                 motion_params.left_arm_max_velocity_.begin(), 
+                                 motion_params.left_arm_max_velocity_.end());
+            arm_max_speed.insert(arm_max_speed.end(), 
+                                 motion_params.right_arm_max_velocity_.begin(), 
+                                 motion_params.right_arm_max_velocity_.end());
+            
+            std::vector<double> arm_max_acc;
+            arm_max_acc.insert(arm_max_acc.end(), 
+                               motion_params.left_arm_max_acceleration_.begin(), 
+                               motion_params.left_arm_max_acceleration_.end());
+            arm_max_acc.insert(arm_max_acc.end(), 
+                               motion_params.right_arm_max_acceleration_.begin(), 
+                               motion_params.right_arm_max_acceleration_.end());
+            
+            // 限位（度转弧度）
+            std::vector<double> arm_min_limits, arm_max_limits;
+            for (size_t i = 0; i < motion_params.left_arm_min_limit_.size(); i++)
+            {
+                arm_min_limits.push_back(DEG2RAD(motion_params.left_arm_min_limit_[i]));
+                arm_max_limits.push_back(DEG2RAD(motion_params.left_arm_max_limit_[i]));
+            }
+            for (size_t i = 0; i < motion_params.right_arm_min_limit_.size(); i++)
+            {
+                arm_min_limits.push_back(DEG2RAD(motion_params.right_arm_min_limit_[i]));
+                arm_max_limits.push_back(DEG2RAD(motion_params.right_arm_max_limit_[i]));
+            }
+            
+            // Home和Zero位置（已经是弧度）
+            std::vector<double> arm_home_positions;
+            arm_home_positions.insert(arm_home_positions.end(), 
+                                      motion_params.left_arm_home_positions_.begin(), 
+                                      motion_params.left_arm_home_positions_.end());
+            arm_home_positions.insert(arm_home_positions.end(), 
+                                      motion_params.right_arm_home_positions_.begin(), 
+                                      motion_params.right_arm_home_positions_.end());
+            
+            std::vector<double> arm_zero_positions;
+            arm_zero_positions.insert(arm_zero_positions.end(), 
+                                      motion_params.left_arm_zero_positions_.begin(), 
+                                      motion_params.left_arm_zero_positions_.end());
+            arm_zero_positions.insert(arm_zero_positions.end(), 
+                                      motion_params.right_arm_zero_positions_.begin(), 
+                                      motion_params.right_arm_zero_positions_.end());
+            
+            // 关节方向
+            std::vector<int> arm_joint_directions;
+            arm_joint_directions.insert(arm_joint_directions.end(), 
+                                        motion_params.left_arm_joint_directions_.begin(), 
+                                        motion_params.left_arm_joint_directions_.end());
+            arm_joint_directions.insert(arm_joint_directions.end(), 
+                                        motion_params.right_arm_joint_directions_.begin(), 
+                                        motion_params.right_arm_joint_directions_.end());
+            
+            return std::make_unique<ArmControl>(
+                arm_total_joints,
+                arm_length_params,
+                arm_max_speed,
+                arm_max_acc,
+                arm_min_limits,
+                arm_max_limits,
+                arm_home_positions,
+                arm_zero_positions,
+                arm_joint_directions);
+        }
 
         case ControllerType::WHEEL_CONTROLLER:
             return std::make_unique<WheelControl>(

src/robot_control/src/core/robot_control_manager.cpp

@@ -157,14 +157,8 @@ bool RobotControlManager::InitializeArmMoveIt(rclcpp::Node* node)
         return false;
     }
 
-    // 初始化左臂和右臂的MoveIt
-    // 注意：当前ArmControl只支持一个move_group，如果需要支持双臂，
-    // 可能需要两个ArmControl实例，或者修改ArmControl支持多个move_group
-    // 这里先假设只有一个arm_controller，使用"arm_left"作为默认名称
-    // TODO: 根据实际需求调整，可能需要支持"arm_left"和"arm_right"
-    
-    std::string move_group_name = "arm_left";  // 默认使用左臂
-    return arm_controller_->InitializeMoveIt(node, move_group_name);
+    // 初始化三个MoveGroup: arm_left, arm_right, dual_arm
+    return arm_controller_->InitializeMoveIt(node);
 }
 
 bool RobotControlManager::PlanArmMotion(

src/robot_control/src/core/robot_control_node.cpp

@@ -16,7 +16,6 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
 {
     RCLCPP_INFO(this->get_logger(), "robot_control_node Node is creating");
     lastSpeed_ = 51;
-    // 注意：isJogMode_, jogDirection_, jogIndex_, isStopping_, lastJoyCommand_ 已迁移到 RemoteController
  
     // 初始化数据文件（设置路径，确保目录存在）
     #if RECORD_FLAG
@@ -42,6 +41,11 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
     joyCommandSub_ = this->create_subscription<std_msgs::msg::String>("/gamepad_msg", 10,std::bind(&RobotControlNode::JoyCommandCallback, this, std::placeholders::_1));
     controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(CYCLE_TIME),std::bind(&RobotControlNode::ControlLoop, this));
 
+    // 初始化 RemoteController（需要在 ActionManager 之前，因为 ActionManager 需要 is_jog_mode_func）
+    remoteController_ = std::make_unique<RemoteController>(
+        this,
+        robotControlManager_);
+
     // 初始化 ActionManager（在创建发布器和客户端之后）
     // 注意：is_jog_mode_func 使用一个临时的 lambda，稍后会被 RemoteController 覆盖
     auto is_jog_mode_func = [this]() -> bool { 
@@ -55,11 +59,6 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
         motorParamClient_);
     action_manager_->initialize();
 
-    // 初始化 RemoteController（需要在 ActionManager 之前，因为 ActionManager 需要 is_jog_mode_func）
-    remoteController_ = std::make_unique<RemoteController>(
-        this,
-        robotControlManager_);
-
     lastTime_ = this->now();  // 初始化时间
     std::cout << "RobotFsm Node is created finished!" << std::endl;
 }
@@ -81,17 +80,17 @@ void RobotControlNode::ControlLoop()
     lastTime_ = currentTime;
 
     // 处理停止请求
-    if (isStopping_)
+    if (remoteController_ && remoteController_->HasStopRequest())
     {
         action_manager_->set_move_home_executing(false);
         action_manager_->set_move_leg_executing(false);
         action_manager_->set_move_waist_executing(false);
         action_manager_->set_move_wheel_executing(false);
-        isJogMode_ = false;
+        remoteController_->SetJogMode(false);
 
         if (robotControlManager_.Stop(dt_sec))
         {
-            isStopping_ = false;
+            remoteController_->ClearStopRequest();
         }
     }
 

src/robot_control/src/utils/s_curve_trajectory.cpp

@@ -0,0 +1,432 @@
+#include "utils/s_curve_trajectory.hpp"
+#include <cmath>
+#include <stdexcept>
+#include <algorithm>
+#include <iostream>
+
+namespace robot_control
+{
+
+// 多轴构造函数
+S_CurveTrajectory::S_CurveTrajectory(const std::vector<double>& max_velocities, 
+                                     const std::vector<double>& max_accelerations,
+                                     const std::vector<double>& max_jerks)
+    : is_stopping_(false), is_single_joint_(false)
+{
+    if (max_velocities.empty() || max_accelerations.empty())
+        throw std::invalid_argument("Max velocities and accelerations cannot be empty");
+    if (max_velocities.size() != max_accelerations.size())
+        throw std::invalid_argument("Max velocities and accelerations must have same size");
+    
+    for (double v : max_velocities)
+        if (v <= 0) throw std::invalid_argument("Max velocity must be positive");
+    for (double a : max_accelerations)
+        if (a <= 0) throw std::invalid_argument("Max acceleration must be positive");
+    
+    max_velocity_ = max_velocities;
+    max_acceleration_ = max_accelerations;
+    
+    // 如果没有提供Jerk，使用默认值（加速度的10倍）
+    if (max_jerks.empty()) {
+        max_jerk_.resize(max_velocities.size());
+        for (size_t i = 0; i < max_velocities.size(); ++i) {
+            max_jerk_[i] = max_accelerations[i] * 10.0;
+        }
+    } else {
+        if (max_jerks.size() != max_velocities.size())
+            throw std::invalid_argument("Max jerks size must match velocities size");
+        for (double j : max_jerks)
+            if (j <= 0) throw std::invalid_argument("Max jerk must be positive");
+        max_jerk_ = max_jerks;
+    }
+}
+
+// 单轴构造函数
+S_CurveTrajectory::S_CurveTrajectory(double max_velocity, double max_acceleration, double max_jerk)
+    : is_stopping_(false), is_single_joint_(true)
+{
+    if (max_velocity <= 0)
+        throw std::invalid_argument("Max velocity must be positive");
+    if (max_acceleration <= 0)
+        throw std::invalid_argument("Max acceleration must be positive");
+    if (max_jerk <= 0)
+        throw std::invalid_argument("Max jerk must be positive");
+    
+    max_velocity_ = {max_velocity};
+    max_acceleration_ = {max_acceleration};
+    max_jerk_ = {max_jerk};
+}
+
+// 多轴初始化
+void S_CurveTrajectory::init(const std::vector<double>& start_pos, 
+                            const std::vector<double>& target_pos)
+{
+    if (start_pos.size() != target_pos.size())
+        throw std::invalid_argument("Start and target positions must have same size");
+    if (start_pos.size() != max_velocity_.size())
+        throw std::invalid_argument("Position size does not match max velocity size");
+    
+    is_single_joint_ = false;
+    is_stopping_ = false;
+    start_pos_ = start_pos;
+    target_pos_ = target_pos;
+    size_t dof = start_pos.size();
+    
+    // 初始化向量大小
+    total_distance_.resize(dof);
+    current_pos_.resize(dof);
+    current_velocity_.resize(dof);
+    current_acceleration_.resize(dof);
+    T1_.resize(dof);
+    T2_.resize(dof);
+    T3_.resize(dof);
+    T4_.resize(dof);
+    T5_.resize(dof);
+    T6_.resize(dof);
+    T7_.resize(dof);
+    v_max_.resize(dof);
+    
+    // 计算各轴总位移
+    for (size_t i = 0; i < dof; ++i)
+        total_distance_[i] = std::fabs(target_pos[i] - start_pos[i]);
+    
+    calculateTrajectoryParams();
+}
+
+// 单轴初始化
+void S_CurveTrajectory::init(double start_pos, double target_pos)
+{
+    is_single_joint_ = true;
+    is_stopping_ = false;
+    start_pos_ = {start_pos};
+    target_pos_ = {target_pos};
+    total_distance_ = {std::fabs(target_pos - start_pos)};
+    
+    current_pos_.resize(1);
+    current_velocity_.resize(1);
+    current_acceleration_.resize(1);
+    T1_.resize(1);
+    T2_.resize(1);
+    T3_.resize(1);
+    T4_.resize(1);
+    T5_.resize(1);
+    T6_.resize(1);
+    T7_.resize(1);
+    v_max_.resize(1);
+    
+    calculateTrajectoryParams();
+}
+
+// 计算S型曲线阶段时间
+void S_CurveTrajectory::calculateScurveTimes(double dist, double max_v, double max_a, double max_j,
+                                              double& T1, double& T2, double& T3, double& T4,
+                                              double& T5, double& T6, double& T7, double& v_max)
+{
+    // 简化计算：假设对称的S型曲线（T1=T3=T5=T7, T2=T6）
+    // 阶段1和3：加加速和减加速，时间各为 T1=T3 = max_a/max_j
+    double T_acc_ramp = max_a / max_j;  // 加速斜坡时间
+    double T_dec_ramp = T_acc_ramp;     // 减速斜坡时间
+    
+    // 阶段2：匀加速阶段，从0加速到max_a的时间已过，需要加速到最大速度
+    // v_max = max_a * T1 + max_a * T2
+    // 如果v_max > max_v，需要限制v_max = max_v，并重新计算T2
+    
+    // 首先假设能达到最大速度
+    v_max = max_v;
+    
+    // 计算加速到max_v所需的匀加速时间
+    // v_max = max_a * T1 + max_a * T2
+    // T2 = (v_max - max_a*T1) / max_a
+    double v_after_T1 = max_a * T_acc_ramp;  // T1结束时的速度
+    double T2_calc = 0.0;
+    if (v_after_T1 < max_v) {
+        T2_calc = (max_v - v_after_T1) / max_a;
+    }
+    
+    T1 = T_acc_ramp;
+    T2 = T2_calc;
+    T3 = T_acc_ramp;  // 减加速阶段，加速度从max_a减到0
+    T4 = 0.0;  // 匀速阶段时间，稍后计算
+    T5 = T_dec_ramp;  // 加减速阶段（加速度从0到-max_a）
+    T6 = T2_calc;     // 匀减速阶段
+    T7 = T_dec_ramp;  // 减减速阶段（加速度从-max_a到0）
+    
+    // 计算实际达到的最大速度（如果T2=0，则v_max = max_a*T1）
+    if (T2 <= 1e-9) {
+        v_max = v_after_T1;
+    }
+    
+    // 计算各阶段的位移
+    double s1 = 0.5 * max_j * T1 * T1 * T1 / 6.0;  // 加加速阶段位移
+    double s2 = v_after_T1 * T2 + 0.5 * max_a * T2 * T2;  // 匀加速阶段位移
+    double s3 = v_max * T3 - 0.5 * max_a * T3 * T3;  // 减加速阶段位移（简化）
+    double s_acc_total = s1 + s2 + s3;
+    
+    double s_dec_total = s_acc_total;  // 减速阶段总位移（对称）
+    
+    // 计算匀速阶段时间
+    if (dist > s_acc_total + s_dec_total) {
+        T4 = (dist - s_acc_total - s_dec_total) / v_max;
+    } else {
+        // 距离不足以达到最大速度，需要重新计算
+        // 简化处理：使用梯形轨迹近似
+        T1 = T_acc_ramp;
+        T2 = 0.0;
+        T3 = T_acc_ramp;
+        T4 = 0.0;
+        T5 = T_dec_ramp;
+        T6 = 0.0;
+        T7 = T_dec_ramp;
+        
+        // 重新计算能达到的最大速度
+        double s_min = 2 * s1 + 2 * s3;  // 最小位移（只有加速和减速阶段）
+        if (dist < s_min) {
+            // 距离太短，简化处理：按比例缩放时间
+            double scale = std::sqrt(dist / s_min);
+            T1 *= scale;
+            T3 *= scale;
+            T5 *= scale;
+            T7 *= scale;
+            v_max = max_a * T1;
+        } else {
+            v_max = (dist - 2 * s1) / (2 * T1 + T4);
+        }
+    }
+}
+
+// 计算轨迹核心参数
+void S_CurveTrajectory::calculateTrajectoryParams()
+{
+    size_t dof = start_pos_.size();
+    
+    // 计算各轴的阶段时间
+    for (size_t i = 0; i < dof; ++i) {
+        double dist = total_distance_[i];
+        if (dist < 1e-9) {
+            T1_[i] = T2_[i] = T3_[i] = T4_[i] = T5_[i] = T6_[i] = T7_[i] = 0.0;
+            v_max_[i] = 0.0;
+            continue;
+        }
+        
+        calculateScurveTimes(dist, max_velocity_[i], max_acceleration_[i], max_jerk_[i],
+                            T1_[i], T2_[i], T3_[i], T4_[i], T5_[i], T6_[i], T7_[i], v_max_[i]);
+    }
+    
+    // 统一时间参数（取各轴最大值，保证同步）
+    T1_total_ = *std::max_element(T1_.begin(), T1_.end());
+    T2_total_ = *std::max_element(T2_.begin(), T2_.end());
+    T3_total_ = *std::max_element(T3_.begin(), T3_.end());
+    T4_total_ = *std::max_element(T4_.begin(), T4_.end());
+    T5_total_ = *std::max_element(T5_.begin(), T5_.end());
+    T6_total_ = *std::max_element(T6_.begin(), T6_.end());
+    T7_total_ = *std::max_element(T7_.begin(), T7_.end());
+    
+    total_time_ = T1_total_ + T2_total_ + T3_total_ + T4_total_ + T5_total_ + T6_total_ + T7_total_;
+}
+
+// 多轴位置更新
+std::vector<double> S_CurveTrajectory::update(double time)
+{
+    if (is_single_joint_)
+        throw std::runtime_error("Call updateSingle for single joint mode");
+    if (is_stopping_)
+        throw std::runtime_error("In stop mode, use updateStop instead");
+
+    size_t dof = start_pos_.size();
+    double t = std::clamp(time, 0.0, total_time_);
+
+    for (size_t i = 0; i < dof; ++i) {
+        double dist = total_distance_[i];
+        if (dist < 1e-9) {
+            current_pos_[i] = start_pos_[i];
+            current_velocity_[i] = 0.0;
+            current_acceleration_[i] = 0.0;
+            continue;
+        }
+
+        double dir = (target_pos_[i] - start_pos_[i]) > 0 ? 1.0 : -1.0;
+        double j = max_jerk_[i];
+        double a_max = max_acceleration_[i];
+        double v_max_axis = v_max_[i];
+        
+        double pos_offset = 0.0;
+        double vel = 0.0;
+        double acc = 0.0;
+        
+        // 使用统一时间，但各轴的阶段时间可能不同
+        double T1 = T1_[i], T2 = T2_[i], T3 = T3_[i], T4 = T4_[i];
+        double T5 = T5_[i], T6 = T6_[i];
+        
+        if (t <= T1) {
+            // 阶段1：加加速（jerk = +j）
+            double dt = t;
+            acc = dir * j * dt;
+            vel = 0.5 * dir * j * dt * dt;
+            pos_offset = dir * j * dt * dt * dt / 6.0;
+        } else if (t <= T1 + T2) {
+            // 阶段2：匀加速（jerk = 0, acc = max_a）
+            double dt = t - T1;
+            acc = dir * a_max;
+            vel = 0.5 * dir * j * T1 * T1 + dir * a_max * dt;
+            pos_offset = dir * j * T1 * T1 * T1 / 6.0 + 0.5 * dir * j * T1 * T1 * dt + 0.5 * dir * a_max * dt * dt;
+        } else if (t <= T1 + T2 + T3) {
+            // 阶段3：减加速（jerk = -j）
+            double dt = t - T1 - T2;
+            acc = dir * a_max - dir * j * dt;
+            double v_T2 = 0.5 * dir * j * T1 * T1 + dir * a_max * T2;
+            vel = v_T2 + dir * a_max * dt - 0.5 * dir * j * dt * dt;
+            double s_T2 = dir * j * T1 * T1 * T1 / 6.0 + 0.5 * dir * j * T1 * T1 * T2 + 0.5 * dir * a_max * T2 * T2;
+            pos_offset = s_T2 + v_T2 * dt + 0.5 * dir * a_max * dt * dt - dir * j * dt * dt * dt / 6.0;
+        } else if (t <= T1 + T2 + T3 + T4) {
+            // 阶段4：匀速（jerk = 0, acc = 0）
+            double dt = t - T1 - T2 - T3;
+            acc = 0.0;
+            vel = dir * v_max_axis;
+            double s_T3 = dir * j * T1 * T1 * T1 / 6.0 + 0.5 * dir * j * T1 * T1 * (T2 + T3) + 0.5 * dir * a_max * (T2 * T2 + 2 * T2 * T3) - dir * j * T3 * T3 * T3 / 6.0;
+            pos_offset = s_T3 + dir * v_max_axis * dt;
+        } else if (t <= T1 + T2 + T3 + T4 + T5) {
+            // 阶段5：加减速（jerk = -j，加速度从0到-max_a）
+            double dt = t - T1 - T2 - T3 - T4;
+            acc = -dir * j * dt;
+            vel = dir * v_max_axis - 0.5 * dir * j * dt * dt;
+            double s_T4 = dir * j * T1 * T1 * T1 / 6.0 + 0.5 * dir * j * T1 * T1 * (T2 + T3) + 0.5 * dir * a_max * (T2 * T2 + 2 * T2 * T3) - dir * j * T3 * T3 * T3 / 6.0 + dir * v_max_axis * T4;
+            pos_offset = s_T4 + dir * v_max_axis * dt - dir * j * dt * dt * dt / 6.0;
+        } else if (t <= T1 + T2 + T3 + T4 + T5 + T6) {
+            // 阶段6：匀减速（jerk = 0, acc = -max_a）
+            double dt = t - T1 - T2 - T3 - T4 - T5;
+            acc = -dir * a_max;
+            double v_T5 = dir * v_max_axis - 0.5 * dir * j * T5 * T5;
+            vel = v_T5 - dir * a_max * dt;
+            double s_T5 = dir * j * T1 * T1 * T1 / 6.0 + 0.5 * dir * j * T1 * T1 * (T2 + T3) + 0.5 * dir * a_max * (T2 * T2 + 2 * T2 * T3) - dir * j * T3 * T3 * T3 / 6.0 + dir * v_max_axis * T4 - dir * j * T5 * T5 * T5 / 6.0;
+            pos_offset = s_T5 + v_T5 * dt - 0.5 * dir * a_max * dt * dt;
+        } else {
+            // 阶段7：减减速（jerk = +j，加速度从-max_a到0）
+            double dt = t - T1 - T2 - T3 - T4 - T5 - T6;
+            acc = -dir * a_max + dir * j * dt;
+            double v_T6 = dir * v_max_axis - 0.5 * dir * j * T5 * T5 - dir * a_max * T6;
+            vel = v_T6 - dir * a_max * dt + 0.5 * dir * j * dt * dt;
+            // 简化：直接使用目标位置
+            pos_offset = dist;
+            acc = 0.0;
+            vel = 0.0;
+        }
+
+        current_pos_[i] = start_pos_[i] + pos_offset;
+        current_velocity_[i] = vel;
+        current_acceleration_[i] = acc;
+    }
+
+    return current_pos_;
+}
+
+// 单轴位置更新
+double S_CurveTrajectory::updateSingle(double time)
+{
+    if (!is_single_joint_)
+        throw std::runtime_error("Call update for multi-joint mode");
+    if (is_stopping_)
+        throw std::runtime_error("In stop mode, use updateStop instead");
+
+    std::vector<double> result = update(time);
+    return result[0];
+}
+
+// 设置各轴最大速度
+void S_CurveTrajectory::setMaxVelocities(const std::vector<double>& max_velocities)
+{
+    if (max_velocities.size() != max_velocity_.size())
+        throw std::invalid_argument("Velocity size mismatch");
+    for (double v : max_velocities)
+        if (v <= 0) throw std::invalid_argument("Max velocity must be positive");
+    max_velocity_ = max_velocities;
+}
+
+void S_CurveTrajectory::setMaxVelocity(double max_velocity)
+{
+    if (max_velocity <= 0)
+        throw std::invalid_argument("Max velocity must be positive");
+    max_velocity_[0] = max_velocity;
+}
+
+void S_CurveTrajectory::setMaxAccelerations(const std::vector<double>& max_accelerations)
+{
+    if (max_accelerations.size() != max_acceleration_.size())
+        throw std::invalid_argument("Acceleration size mismatch");
+    for (double a : max_accelerations)
+        if (a <= 0) throw std::invalid_argument("Max acceleration must be positive");
+    max_acceleration_ = max_accelerations;
+}
+
+void S_CurveTrajectory::setMaxAcceleration(double max_acceleration)
+{
+    if (max_acceleration <= 0)
+        throw std::invalid_argument("Max acceleration must be positive");
+    max_acceleration_[0] = max_acceleration;
+}
+
+// 计算急停轨迹参数（简化版本，使用梯形急停）
+void S_CurveTrajectory::calculateStopTrajectory()
+{
+    size_t dof = current_pos_.size();
+    is_stopping_ = true;
+    stop_start_pos_ = current_pos_;
+    stop_start_vel_ = current_velocity_;
+    stop_start_acc_ = current_acceleration_;
+    stop_jerk_.resize(dof);
+    
+    // 使用最大Jerk进行急停
+    for (size_t i = 0; i < dof; ++i) {
+        double vel = std::fabs(stop_start_vel_[i]);
+        if (vel < 1e-6) {
+            stop_jerk_[i] = 0.0;
+            continue;
+        }
+        // 使用最大Jerk，方向与速度相反
+        stop_jerk_[i] = -std::copysign(max_jerk_[i], stop_start_vel_[i]);
+    }
+    
+    // 简化：计算急停时间（使用最大减速度）
+    std::vector<double> stop_times(dof, 0.0);
+    for (size_t i = 0; i < dof; ++i) {
+        if (std::fabs(stop_start_vel_[i]) < 1e-6) {
+            stop_times[i] = 0.0;
+        } else {
+            stop_times[i] = std::fabs(stop_start_vel_[i]) / max_acceleration_[i];
+        }
+    }
+    stop_total_time_ = *std::max_element(stop_times.begin(), stop_times.end());
+}
+
+// 更新急停轨迹（简化版本）
+std::vector<double> S_CurveTrajectory::updateStop(double dt)
+{
+    if (!is_stopping_)
+        throw std::runtime_error("Not in stop mode, call calculateStopTrajectory first");
+
+    size_t dof = stop_start_pos_.size();
+    double t = std::clamp(dt, 0.0, stop_total_time_);
+
+    for (size_t i = 0; i < dof; ++i) {
+        double vel0 = stop_start_vel_[i];
+        if (std::fabs(vel0) < 1e-6) {
+            current_pos_[i] = stop_start_pos_[i];
+            current_velocity_[i] = 0.0;
+            current_acceleration_[i] = 0.0;
+            continue;
+        }
+
+        // 简化：使用匀减速
+        double decel = -std::copysign(max_acceleration_[i], vel0);
+        current_pos_[i] = stop_start_pos_[i] + vel0 * t + 0.5 * decel * t * t;
+        current_velocity_[i] = vel0 + decel * t;
+        
+        if (std::fabs(current_velocity_[i]) < 1e-6)
+            current_velocity_[i] = 0.0;
+        current_acceleration_[i] = decel;
+    }
+
+    return current_pos_;
+}
+
+}  // namespace robot_control
+