movej for single arm and dual arm

src/dual_arm_moveit_config/config/moveit_controllers.yaml

@@ -4,11 +4,11 @@ moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControll
 
 moveit_simple_controller_manager:
   controller_names:
-    - arm_left_controller
-    - arm_right_controller
+    - left_arm_controller
+    - right_arm_controller
     # - dual_arm_controller
 
-  arm_left_controller:
+  left_arm_controller:
     type: FollowJointTrajectory
     joints:
       - left_arm_joint_1
@@ -19,7 +19,7 @@ moveit_simple_controller_manager:
       - left_arm_joint_6
     action_ns: follow_joint_trajectory
     default: true
-  arm_right_controller:
+  right_arm_controller:
     type: FollowJointTrajectory
     joints:
       - right_arm_joint_1

src/dual_arm_moveit_config/config/ros2_controllers.yaml

@@ -3,11 +3,11 @@ controller_manager:
   ros__parameters:
     update_rate: 100  # Hz
 
-    arm_left_controller:
+    left_arm_controller:
       type: joint_trajectory_controller/JointTrajectoryController
 
 
-    arm_right_controller:
+    right_arm_controller:
       type: joint_trajectory_controller/JointTrajectoryController
 
 
@@ -18,7 +18,7 @@ controller_manager:
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
-arm_left_controller:
+left_arm_controller:
   ros__parameters:
     joints:
       - left_arm_joint_1
@@ -32,7 +32,14 @@ arm_left_controller:
     state_interfaces:
       - position
       - velocity
-arm_right_controller:
+    state_publish_rate: 50.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    stop_trajectory_duration: 0.25
+    constraints:
+      goal_time: 0.5
+      stopped_velocity_tolerance: 0.01
+right_arm_controller:
   ros__parameters:
     joints:
       - right_arm_joint_1
@@ -46,6 +53,13 @@ arm_right_controller:
     state_interfaces:
       - position
       - velocity
+    state_publish_rate: 50.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    stop_trajectory_duration: 0.25
+    constraints:
+      goal_time: 0.5
+      stopped_velocity_tolerance: 0.01
 # dual_arm_controller:
 #   ros__parameters:
 #     joints:

src/robot_control/include/actions/dual_arm_action.hpp

@@ -117,7 +117,7 @@ private:
   rclcpp_action::Client<FollowJTA>::SharedPtr right_fjt_client_;
 
   bool save_trajectory_enabled_{true};
-  bool trajectory_execution_enabled_{false};
+  bool trajectory_execution_enabled_{true};
 
 };
 }  // namespace robot_control

src/robot_control/include/controllers/arm_control.hpp

@@ -195,6 +195,9 @@ namespace robot_control
         
         uint8_t current_arm_id_;  ///< 当前控制的臂ID（0=左臂，1=右臂，2=双臂）
 
+        // 是否启用 S 型曲线时间参数化（默认关闭，便于对比测试）
+        bool enable_s_curve_time_parameterization_ {true};
+
         /**
          * @brief 根据arm_id获取对应的MoveGroup接口
          * @param arm_id 手臂ID（0=左臂，1=右臂，2=双臂）

src/robot_control/launch/right_arm_joint_test.launch.py

@@ -18,7 +18,7 @@ def generate_launch_description():
     
     action_name_arg = DeclareLaunchArgument(
         'action_name',
-        default_value='/arm_right_controller/follow_joint_trajectory',
+        default_value='/right_arm_controller/follow_joint_trajectory',
         description='Action server name for FollowJointTrajectory'
     )
     

src/robot_control/scripts/dual_arm_action_client.py

@@ -35,13 +35,13 @@ class DualArmActionClient(Node):
         self.get_logger().info('Action server已就绪')
         return True
     
-    def send_goal(self, arm_motion_params_list, velocity_scaling=0.5):
+    def send_goal(self, arm_motion_params_list, velocity_scaling=50):
         """
         发送运动目标
         
         Args:
             arm_motion_params_list: ArmMotionParams消息列表
-            velocity_scaling: 速度缩放因子 [0,1]
+            velocity_scaling: 速度缩放因子 [1,100]
         """
         # reset per-goal futures
         self._goal_handle = None
@@ -122,7 +122,7 @@ class DualArmActionClient(Node):
     def send_goal_and_wait(
         self,
         arm_motion_params_list,
-        velocity_scaling=0.5,
+        velocity_scaling=50,
         result_timeout_sec=30.0,
     ):
         """
@@ -158,7 +158,7 @@ class DualArmActionClient(Node):
         return bool(result.success), str(result.message)
 
 
-def create_arm_motion_param(arm_id, motion_type, target_joints=None, target_pose=None, blend_radius=0.0):
+def create_arm_motion_param(arm_id, motion_type, target_joints=None, target_pose=None, blend_radius=0):
     """
     创建ArmMotionParams消息
     
@@ -223,7 +223,7 @@ def example_left_and_right_arm_joint_motion():
         )
         ok, msg = client.send_goal_and_wait(
             arm_motion_params_list=[left_param_up],
-            velocity_scaling=0.5,
+            velocity_scaling= 50,
             result_timeout_sec=30.0,
         )
         client.get_logger().info(f'左臂 move-to 结果: ok={ok}, msg={msg}')
@@ -238,7 +238,7 @@ def example_left_and_right_arm_joint_motion():
         )
         ok, msg = client.send_goal_and_wait(
             arm_motion_params_list=[left_param_home],
-            velocity_scaling=0.5,
+            velocity_scaling=50,
             result_timeout_sec=30.0,
         )
         client.get_logger().info(f'左臂回零结果: ok={ok}, msg={msg}')
@@ -260,7 +260,7 @@ def example_left_and_right_arm_joint_motion():
         )
         ok, msg = client.send_goal_and_wait(
             arm_motion_params_list=[right_param_up],
-            velocity_scaling=0.5,
+            velocity_scaling=50,
             result_timeout_sec=30.0,
         )
         client.get_logger().info(f'右臂 move-to 结果: ok={ok}, msg={msg}')
@@ -274,7 +274,7 @@ def example_left_and_right_arm_joint_motion():
         )
         ok, msg = client.send_goal_and_wait(
             arm_motion_params_list=[right_param_home],
-            velocity_scaling=0.5,
+            velocity_scaling=50,
             result_timeout_sec=30.0,
         )
         client.get_logger().info(f'右臂回零结果: ok={ok}, msg={msg}')
@@ -301,7 +301,7 @@ def example_left_and_right_arm_joint_motion():
 
         ok, msg = client.send_goal_and_wait(
             arm_motion_params_list=[left_param_dual, right_param_dual],
-            velocity_scaling=0.5,
+            velocity_scaling=50,
             result_timeout_sec=30.0,
         )
         client.get_logger().info(f'双臂同时运动结果: ok={ok}, msg={msg}')

src/robot_control/scripts/plot_joint_trajectory_multi.py

@@ -63,6 +63,6 @@ if __name__ == "__main__":
         print("用法: python plot_joint_trajectory_multi.py <数据文件路径>")
         sys.exit(1)
     
-    file_path = sys.argv[1]
+    file_path = "/tmp/trajectory_dual_right_20260226_151304.csv"
     plot_joint_trajectories(file_path)
 

src/robot_control/src/actions/dual_arm_action.cpp

@@ -5,6 +5,7 @@
 #include <chrono>
 #include <ctime>
 #include <algorithm>
+#include <cmath>
 #include <fstream>
 #include <iomanip>
 #include <sstream>
@@ -13,6 +14,129 @@
 #include "core/robot_control_manager.hpp"
 #include "core/common_enum.hpp"
 
+namespace
+{
+inline double duration_to_sec(const builtin_interfaces::msg::Duration& d)
+{
+  return static_cast<double>(d.sec) + static_cast<double>(d.nanosec) * 1e-9;
+}
+
+inline void sec_to_duration(double sec, builtin_interfaces::msg::Duration* out)
+{
+  if (!out) return;
+  if (sec < 0.0) sec = 0.0;
+  const int64_t s = static_cast<int64_t>(std::floor(sec));
+  const double rem = sec - static_cast<double>(s);
+  const uint32_t ns = static_cast<uint32_t>(std::round(rem * 1e9));
+  out->sec = static_cast<int32_t>(s);
+  out->nanosec = ns;
+}
+
+inline bool goal_velocity_to_scale(double goal_value, double* out_scale, std::string* error_msg)
+{
+  if (!out_scale) {
+    if (error_msg) *error_msg = "Null out_scale";
+    return false;
+  }
+
+  if (!std::isfinite(goal_value)) {
+    if (error_msg) *error_msg = "velocity_scaling is NaN/Inf";
+    return false;
+  }
+
+  // Backward compatibility:
+  // - Some clients may send [0,1] scaling.
+  // - Some clients may send [1,100] percent.
+  // The action definition in repo says int32 percent, but generated type is currently double.
+  if (goal_value > 1.0) {
+    if (goal_value < 1.0 || goal_value > 100.0) {
+      if (error_msg) *error_msg = "velocity_scaling percent out of range [1,100]: " + std::to_string(goal_value);
+      return false;
+    }
+    *out_scale = std::clamp(goal_value / 100.0, 0.01, 1.0);
+    return true;
+  }
+
+  if (goal_value <= 0.0 || goal_value > 1.0) {
+    if (error_msg) *error_msg = "velocity_scaling scale out of range (0,1]: " + std::to_string(goal_value);
+    return false;
+  }
+  *out_scale = std::clamp(goal_value, 0.01, 1.0);
+  return true;
+}
+
+inline void ensure_nonzero_start_time(trajectory_msgs::msg::JointTrajectory* traj, double min_start_sec)
+{
+  if (!traj || traj->points.empty()) return;
+  if (min_start_sec <= 0.0) min_start_sec = 0.01;
+
+  // If first point is at t=0 and we have more than one point, drop it (common cause of startup jerk).
+  const double t0 = duration_to_sec(traj->points.front().time_from_start);
+  if (t0 <= 1e-9 && traj->points.size() > 1) {
+    traj->points.erase(traj->points.begin());
+  }
+  if (traj->points.empty()) return;
+
+  const double t_first = duration_to_sec(traj->points.front().time_from_start);
+  if (t_first >= min_start_sec) return;
+
+  const double shift = (min_start_sec - t_first);
+  for (auto& p : traj->points) {
+    const double t = duration_to_sec(p.time_from_start) + shift;
+    sec_to_duration(t, &p.time_from_start);
+  }
+}
+
+inline bool get_current_positions(
+  const robot_control::RobotControlManager* mgr,
+  const std::vector<std::string>& joint_names,
+  std::vector<double>* out_positions,
+  std::string* error_msg)
+{
+  if (!mgr || !out_positions) {
+    if (error_msg) *error_msg = "Null manager or out_positions";
+    return false;
+  }
+  out_positions->clear();
+  out_positions->reserve(joint_names.size());
+
+  const auto& mp = mgr->GetMotionParameters();
+  const auto& jp = mgr->GetJointPositions();
+  for (const auto& jn : joint_names) {
+    const auto it = mp.joint_name_to_index_.find(jn);
+    if (it == mp.joint_name_to_index_.end()) {
+      if (error_msg) *error_msg = "Joint name not mapped: " + jn;
+      return false;
+    }
+    const size_t idx = it->second;
+    if (idx >= jp.size()) {
+      if (error_msg) *error_msg = "Joint index out of range for " + jn;
+      return false;
+    }
+    out_positions->push_back(jp[idx]);
+  }
+  return true;
+}
+
+inline trajectory_msgs::msg::JointTrajectory make_hold_trajectory(
+  const std::vector<std::string>& joint_names,
+  const std::vector<double>& positions,
+  double hold_time_sec)
+{
+  trajectory_msgs::msg::JointTrajectory traj;
+  traj.joint_names = joint_names;
+
+  trajectory_msgs::msg::JointTrajectoryPoint p;
+  p.positions = positions;
+  p.velocities.assign(joint_names.size(), 0.0);
+  p.accelerations.assign(joint_names.size(), 0.0);
+  sec_to_duration(std::max(0.05, hold_time_sec), &p.time_from_start);
+
+  traj.points.push_back(std::move(p));
+  return traj;
+}
+}  // namespace
+
 namespace robot_control
 {
 DualArmAction::DualArmAction(const ActionContext& ctx)
@@ -30,28 +154,6 @@ void DualArmAction::initialize()
       ctx_.node, "/left_arm_controller/follow_joint_trajectory");
     right_fjt_client_ = rclcpp_action::create_client<FollowJTA>(
       ctx_.node, "/right_arm_controller/follow_joint_trajectory");
-
-    const std::string save_param = "save_trajectory_enabled";
-    try {
-      ctx_.node->declare_parameter<bool>(save_param, save_trajectory_enabled_);
-    } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException&) {
-      // Parameter already declared elsewhere; keep going.
-    }
-    if (ctx_.node->get_parameter(save_param, save_trajectory_enabled_)) {
-      RCLCPP_INFO(ctx_.node->get_logger(),
-        "save_trajectory_enabled=%s", save_trajectory_enabled_ ? "true" : "false");
-    }
-
-    const std::string exec_param = "arm_trajectory_execution_enabled";
-    try {
-      ctx_.node->declare_parameter<bool>(exec_param, trajectory_execution_enabled_);
-    } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException&) {
-      // Parameter already declared elsewhere; keep going.
-    }
-    if (ctx_.node->get_parameter(exec_param, trajectory_execution_enabled_)) {
-      RCLCPP_INFO(ctx_.node->get_logger(),
-        "arm_trajectory_execution_enabled=%s", trajectory_execution_enabled_ ? "true" : "false");
-    }
   }
 
   server_ = rclcpp_action::create_server<DualArm>(
@@ -72,9 +174,24 @@ rclcpp_action::GoalResponse DualArmAction::handle_goal_(
   if (!ctx_.robot_control_manager) return rclcpp_action::GoalResponse::REJECT;
 
   RCLCPP_INFO(ctx_.node->get_logger(),
-    "Received DualArm goal: velocity_scaling=%.2f, arm_motion_params_count=%zu",
+    "Received DualArm goal: velocity_scaling=%d, arm_motion_params_count=%zu",
     goal->velocity_scaling, goal->arm_motion_params.size());
 
+  // 关节状态未稳定前进行规划/执行，容易出现起点跳变（看起来像启停抖动）
+  if (!ctx_.robot_control_manager->RobotInitFinished()) {
+    RCLCPP_ERROR(ctx_.node->get_logger(),
+      "DualArm goal rejected: robot init not finished (joint_states not stabilized yet)");
+    return rclcpp_action::GoalResponse::REJECT;
+  }
+
+  double scale = 0.0;
+  std::string scale_err;
+  if (!goal_velocity_to_scale(goal->velocity_scaling, &scale, &scale_err)) {
+    RCLCPP_ERROR(ctx_.node->get_logger(), "DualArm goal rejected: %s", scale_err.c_str());
+    return rclcpp_action::GoalResponse::REJECT;
+  }
+  RCLCPP_INFO(ctx_.node->get_logger(), "DualArm derived velocity_scale=%.3f", scale);
+
   // 统一检查 MoveIt 规划器占用：同一时刻只能进行一组规划
   if (planner_busy_.load()) {
     RCLCPP_ERROR(ctx_.node->get_logger(), "DualArm goal rejected: MoveIt planner is busy");
@@ -398,7 +515,7 @@ void DualArmAction::save_trajectory_to_file_(
   auto now = std::chrono::system_clock::now();
   auto time_t = std::chrono::system_clock::to_time_t(now);
   std::stringstream ss;
-  ss << "/tmp/trajectory_" << tag << "_"
+  ss << "trajectory_" << tag << "_"
      << std::put_time(std::localtime(&time_t), "%Y%m%d_%H%M%S") << ".csv";
   const std::string filename = ss.str();
 
@@ -462,10 +579,19 @@ void DualArmAction::execute_single_arm_(
 
   // 使用单臂规划接口
   ArmMotionParams arm_params = arm_param;
+  double velocity_scaling = 0.0;
+  std::string scale_err;
+  if (!goal_velocity_to_scale(goal->velocity_scaling, &velocity_scaling, &scale_err)) {
+    planner_busy_.store(false);
+    result->success = false;
+    result->message = std::string("Invalid velocity_scaling: ") + scale_err;
+    goal_handle->abort(result);
+    return;
+  }
 
   if (!ctx_.robot_control_manager->PlanArmMotion(
         arm_params,
-        goal->velocity_scaling,
+        velocity_scaling,
         acceleration_scaling)) {
     planner_busy_.store(false);
     result->success = false;
@@ -483,6 +609,8 @@ void DualArmAction::execute_single_arm_(
     goal_handle->abort(result);
     return;
   }
+  // Normalize for startup smoothness (avoid t=0 jump)
+  ensure_nonzero_start_time(&traj_single, 0.05);
   save_trajectory_to_file_(traj_single, (arm_id == 0) ? "left_arm" : "right_arm");
   if (!trajectory_execution_enabled_) {
     result->success = false;
@@ -534,7 +662,19 @@ void DualArmAction::execute_single_arm_(
         [&res_future]() {
           return res_future.wait_for(std::chrono::milliseconds(1)) == std::future_status::ready;
         },
-        [&fjt_client, &goal_handle_arm, &arm_id, this]() {
+        [&fjt_client, &goal_handle_arm, &arm_id, joint_names = traj_single.joint_names, this]() {
+          // Soft stop: send a short "hold current position" trajectory to reduce jerk on cancel.
+          if (ctx_.robot_control_manager) {
+            std::vector<double> cur;
+            std::string err;
+            if (get_current_positions(ctx_.robot_control_manager, joint_names, &cur, &err)) {
+              FollowJTA::Goal stop_goal;
+              stop_goal.trajectory = make_hold_trajectory(joint_names, cur, 0.2);
+              (void)fjt_client->async_send_goal(stop_goal);
+            } else if (ctx_.node) {
+              RCLCPP_WARN(ctx_.node->get_logger(), "Soft stop skipped (single arm): %s", err.c_str());
+            }
+          }
           (void)fjt_client->async_cancel_goal(goal_handle_arm);
           if (arm_id == 0) executing_left_.store(false);
           else executing_right_.store(false);
@@ -615,9 +755,19 @@ void DualArmAction::execute_dual_arm_(
     return;
   }
 
+  double velocity_scaling = 0.0;
+  std::string scale_err;
+  if (!goal_velocity_to_scale(goal->velocity_scaling, &velocity_scaling, &scale_err)) {
+    planner_busy_.store(false);
+    result->success = false;
+    result->message = std::string("Invalid velocity_scaling: ") + scale_err;
+    goal_handle->abort(result);
+    return;
+  }
+
   if (!ctx_.robot_control_manager->PlanDualArmJointMotion(
         need_left, left_target, need_right, right_target,
-        goal->velocity_scaling, acceleration_scaling))
+        velocity_scaling, acceleration_scaling))
   {
     planner_busy_.store(false);
     result->success = false;
@@ -642,6 +792,8 @@ void DualArmAction::execute_dual_arm_(
   trajectory_msgs::msg::JointTrajectory traj_left;
   trajectory_msgs::msg::JointTrajectory traj_right;
   split_dual_arm_trajectory_(motion_params, ARM_DOF, traj, &traj_left, &traj_right);
+  ensure_nonzero_start_time(&traj_left, 0.05);
+  ensure_nonzero_start_time(&traj_right, 0.05);
   save_trajectory_to_file_(traj_left, "dual_left");
   save_trajectory_to_file_(traj_right, "dual_right");
   if (!trajectory_execution_enabled_) {
@@ -712,7 +864,27 @@ void DualArmAction::execute_dual_arm_(
             (right_res_future.wait_for(std::chrono::milliseconds(1)) == std::future_status::ready);
           return left_done && right_done;
         },
-        [&left_handle, &right_handle, this]() {
+        [&left_handle, &right_handle, left_joint_names = traj_left.joint_names, right_joint_names = traj_right.joint_names, this]() {
+          // Soft stop for both arms before cancel.
+          if (ctx_.robot_control_manager) {
+            std::vector<double> cur_left;
+            std::vector<double> cur_right;
+            std::string err;
+            if (get_current_positions(ctx_.robot_control_manager, left_joint_names, &cur_left, &err)) {
+              FollowJTA::Goal stop_goal;
+              stop_goal.trajectory = make_hold_trajectory(left_joint_names, cur_left, 0.2);
+              (void)left_fjt_client_->async_send_goal(stop_goal);
+            } else if (ctx_.node) {
+              RCLCPP_WARN(ctx_.node->get_logger(), "Soft stop skipped (left arm): %s", err.c_str());
+            }
+            if (get_current_positions(ctx_.robot_control_manager, right_joint_names, &cur_right, &err)) {
+              FollowJTA::Goal stop_goal;
+              stop_goal.trajectory = make_hold_trajectory(right_joint_names, cur_right, 0.2);
+              (void)right_fjt_client_->async_send_goal(stop_goal);
+            } else if (ctx_.node) {
+              RCLCPP_WARN(ctx_.node->get_logger(), "Soft stop skipped (right arm): %s", err.c_str());
+            }
+          }
           (void)left_fjt_client_->async_cancel_goal(left_handle);
           (void)right_fjt_client_->async_cancel_goal(right_handle);
           executing_.store(false);

src/robot_control/src/controllers/arm_control.cpp

@@ -16,6 +16,75 @@
 namespace robot_control
 {
 
+namespace
+{
+inline double duration_to_sec(const builtin_interfaces::msg::Duration& d)
+{
+    return static_cast<double>(d.sec) + static_cast<double>(d.nanosec) * 1e-9;
+}
+
+inline void sec_to_duration(double sec, builtin_interfaces::msg::Duration* out)
+{
+    if (!out) return;
+    if (!std::isfinite(sec) || sec < 0.0) sec = 0.0;
+    const int64_t s = static_cast<int64_t>(std::floor(sec));
+    const double rem = sec - static_cast<double>(s);
+    const uint32_t ns = static_cast<uint32_t>(std::round(rem * 1e9));
+    out->sec = static_cast<int32_t>(s);
+    out->nanosec = ns;
+}
+
+inline void normalize_and_fix_time_from_start(trajectory_msgs::msg::JointTrajectory* traj, double default_dt)
+{
+    if (!traj || traj->points.empty()) return;
+    if (!std::isfinite(default_dt) || default_dt <= 0.0) default_dt = 0.01;
+
+    // Shift so that the first point starts at t=0 (GetInterpolatedPoint assumes this).
+    const double t0 = duration_to_sec(traj->points.front().time_from_start);
+    bool need_rebuild = (!std::isfinite(t0) || t0 < 0.0);
+
+    std::vector<double> t;
+    t.reserve(traj->points.size());
+    for (const auto& p : traj->points) {
+        const double ti = duration_to_sec(p.time_from_start);
+        t.push_back(ti);
+        if (!std::isfinite(ti) || ti < 0.0) {
+            need_rebuild = true;
+        }
+    }
+
+    if (!need_rebuild) {
+        for (auto& ti : t) {
+            ti -= t0;
+        }
+        // Ensure strictly increasing and non-zero duration.
+        for (size_t i = 1; i < t.size(); ++i) {
+            if (!(t[i] > t[i - 1] + 1e-9)) {
+                need_rebuild = true;
+                break;
+            }
+        }
+        if (t.size() >= 2 && t.back() <= default_dt * 0.5) {
+            need_rebuild = true;
+        }
+    }
+
+    if (need_rebuild) {
+        t.clear();
+        t.reserve(traj->points.size());
+        double acc = 0.0;
+        for (size_t i = 0; i < traj->points.size(); ++i) {
+            t.push_back(acc);
+            acc += default_dt;
+        }
+    }
+
+    for (size_t i = 0; i < traj->points.size(); ++i) {
+        sec_to_duration(t[i], &traj->points[i].time_from_start);
+    }
+}
+}  // namespace
+
 ArmControl::ArmControl(
     const std::vector<std::string>& joint_names,
     const std::map<std::string, JointInfo>& joints_info_map,
@@ -258,27 +327,34 @@ bool ArmControl::timeParameterizeAndStore_(
     double velocity_scaling,
     double acceleration_scaling)
 {
-    // 1) 使用 S 型曲线参数化（MoveIt 路径 + S-curve time scaling）
-    bool s_curve_ok = false;
-    if (!plan.trajectory_.joint_trajectory.points.empty()) {
-        trajectory_msgs::msg::JointTrajectory s_curve_traj;
-        s_curve_ok = sCurveTimeParameterize_(
-            arm_id,
-            plan.trajectory_.joint_trajectory,
-            s_curve_traj,
-            velocity_scaling,
-            acceleration_scaling);
-        if (s_curve_ok) {
-            plan.trajectory_.joint_trajectory = s_curve_traj;
+    // 1) 时间参数化（可选：S 型曲线）
+    if (enable_s_curve_time_parameterization_) {
+        bool s_curve_ok = false;
+        if (!plan.trajectory_.joint_trajectory.points.empty()) {
+            trajectory_msgs::msg::JointTrajectory s_curve_traj;
+            s_curve_ok = sCurveTimeParameterize_(
+                arm_id,
+                plan.trajectory_.joint_trajectory,
+                s_curve_traj,
+                velocity_scaling,
+                acceleration_scaling);
+            if (s_curve_ok) {
+                plan.trajectory_.joint_trajectory = s_curve_traj;
+            }
         }
-    }
-    if (!s_curve_ok) {
-        if (node_) {
-            RCLCPP_ERROR(node_->get_logger(),
-                         "S-curve time parameterization failed for arm_id %d",
-                         static_cast<int>(arm_id));
+        if (!s_curve_ok) {
+            if (node_) {
+                RCLCPP_ERROR(node_->get_logger(),
+                             "S-curve time parameterization failed for arm_id %d",
+                             static_cast<int>(arm_id));
+            }
+            return false;
         }
-        return false;
+    } else {
+        // 不进行 S 曲线：直接使用 MoveIt 规划结果自带的 time_from_start
+        // 但要保证：从 0 开始、严格递增、总时长 > 0（否则插补会瞬间结束导致跳变）。
+        const double default_dt = std::max(0.001, static_cast<double>(CYCLE_TIME) / 1000.0);
+        normalize_and_fix_time_from_start(&plan.trajectory_.joint_trajectory, default_dt);
     }
 
     // 2) 存入内部 TrajectoryState（使用 msg 内的 time_from_start，并保留 vel/acc）
@@ -361,8 +437,10 @@ bool ArmControl::sCurveTimeParameterize_(
         out_traj.points.clear();
         trajectory_msgs::msg::JointTrajectoryPoint point;
         point.positions = waypoints.front();
-        point.velocities.assign(dof, 0.0);
-        point.accelerations.assign(dof, 0.0);
+        // Leave vel/acc empty so downstream controller does not attempt
+        // higher-order interpolation with potentially inconsistent derivatives.
+        point.velocities.clear();
+        point.accelerations.clear();
         point.time_from_start = rclcpp::Duration::from_seconds(0.0);
         out_traj.points.push_back(std::move(point));
         return true;
@@ -397,8 +475,8 @@ bool ArmControl::sCurveTimeParameterize_(
         out_traj.points.clear();
         trajectory_msgs::msg::JointTrajectoryPoint point;
         point.positions = waypoints.back();
-        point.velocities.assign(dof, 0.0);
-        point.accelerations.assign(dof, 0.0);
+        point.velocities.clear();
+        point.accelerations.clear();
         point.time_from_start = rclcpp::Duration::from_seconds(0.0);
         out_traj.points.push_back(std::move(point));
         return true;
@@ -449,10 +527,15 @@ bool ArmControl::sCurveTimeParameterize_(
     out_traj.joint_names = in_traj.joint_names;
     out_traj.points.clear();
 
-    size_t seg_idx = 0;
+    // First pass: sample the scalar path progress s(t).
+    std::vector<double> t_samples;
+    std::vector<double> s_samples;
+
     double t = 0.0;
     bool done = false;
     const double min_time = total_length / std::max(1e-6, s_max_vel);
+    t_samples.reserve(static_cast<size_t>(std::ceil(min_time / dt)) + 5);
+    s_samples.reserve(t_samples.capacity());
     const size_t max_iter =
         static_cast<size_t>(std::ceil((min_time * 5.0) / dt)) + 1000;
 
@@ -461,8 +544,34 @@ bool ArmControl::sCurveTimeParameterize_(
         double s = s_vec.empty() ? 0.0 : s_vec[0];
         if (s > total_length) s = total_length;
 
-        const double s_dot = s_curve.getVelocity()[0];
-        const double s_ddot = s_curve.getAcceleration()[0];
+        t_samples.push_back(t);
+        s_samples.push_back(s);
+
+        if (s_curve.isFinished(t) || (total_length - s) <= 1e-6) {
+            done = true;
+            break;
+        }
+        t += dt;
+    }
+
+    if (!done || t_samples.empty() || s_samples.empty()) {
+        return false;
+    }
+
+    // If the scalar profile under-travels (s_end < total_length) we scale s(t)
+    // so that the last sample exactly reaches total_length, avoiding the last-cycle jump.
+    const double s_end = s_samples.back();
+    if (!(s_end > kEps)) {
+        return false;
+    }
+    const double scale = total_length / s_end;
+
+    // Second pass: convert scaled s(t) to joint positions along piecewise-linear path.
+    size_t seg_idx = 0;
+    out_traj.points.reserve(t_samples.size());
+    for (size_t i = 0; i < t_samples.size(); ++i) {
+        double s = s_samples[i] * scale;
+        if (s > total_length) s = total_length;
 
         while (seg_idx + 1 < s_cum.size() && s > s_cum[seg_idx + 1]) {
             ++seg_idx;
@@ -480,41 +589,25 @@ bool ArmControl::sCurveTimeParameterize_(
 
         trajectory_msgs::msg::JointTrajectoryPoint point;
         point.positions.resize(dof, 0.0);
-        point.velocities.resize(dof, 0.0);
-        point.accelerations.resize(dof, 0.0);
+        point.velocities.clear();
+        point.accelerations.clear();
 
         for (size_t j = 0; j < dof; ++j) {
             const double q0 = waypoints[seg_idx][j];
             const double q1 = waypoints[seg_idx + 1][j];
             const double dq = q1 - q0;
             point.positions[j] = q0 + alpha * dq;
-            if (seg_len > kEps) {
-                const double dqds = dq / seg_len;
-                point.velocities[j] = s_dot * dqds;
-                point.accelerations[j] = s_ddot * dqds;
-            }
         }
 
-        point.time_from_start = rclcpp::Duration::from_seconds(t);
+        point.time_from_start = rclcpp::Duration::from_seconds(t_samples[i]);
         out_traj.points.push_back(std::move(point));
-
-        if (s_curve.isFinished(t) || (total_length - s) <= 1e-6) {
-            done = true;
-            break;
-        }
-        t += dt;
     }
 
-    if (!done || out_traj.points.empty()) {
-        return false;
+    // Ensure the last point is exactly the last waypoint (numerical guard).
+    if (!out_traj.points.empty()) {
+        out_traj.points.back().positions = waypoints.back();
     }
 
-    // 确保末点速度/加速度为 0
-    auto& last = out_traj.points.back();
-    last.positions = waypoints.back();
-    last.velocities.assign(dof, 0.0);
-    last.accelerations.assign(dof, 0.0);
-
     return true;
 }
 

src/robot_control/src/core/robot_control_node.cpp

@@ -1,5 +1,6 @@
 #include <thread>
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp/executors/single_threaded_executor.hpp"
 #include "core/robot_control_node.hpp"
 #include "actions/action_manager.hpp"
 #include "core/motion_parameters.hpp"
@@ -51,8 +52,6 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
         RCLCPP_ERROR(this->get_logger(), "Failed to activate right_arm_controller: trajectory_controller");
     }
 
-    
-    motor_enable(0, 1);
 
     // 初始化MoveIt（需要在ROS节点创建后调用）
     if (robotControlManager_.InitializeArmMoveIt(this)) {
@@ -145,7 +144,30 @@ bool RobotControlNode::activateController(const std::string& controller_name)
     request->timeout.nanosec = 0;
 
     auto future = switch_client_->async_send_request(request);
+    rclcpp::executors::SingleThreadedExecutor exec;
+    exec.add_node(this->get_node_base_interface());
 
+    const auto ret = exec.spin_until_future_complete(future, std::chrono::seconds(5));
+    if (ret != rclcpp::FutureReturnCode::SUCCESS) {
+        RCLCPP_ERROR(this->get_logger(),
+                     "activateController: switch_controller timeout/failure for '%s'",
+                     controller_name.c_str());
+        return false;
+    }
+
+    const auto resp = future.get();
+    if (!resp) {
+        RCLCPP_ERROR(this->get_logger(),
+                     "activateController: null response for '%s'",
+                     controller_name.c_str());
+        return false;
+    }
+    if (!resp->ok) {
+        RCLCPP_ERROR(this->get_logger(),
+                     "activateController: switch_controller returned ok=false for '%s'",
+                     controller_name.c_str());
+        return false;
+    }
     return true;
 }
 
@@ -241,8 +263,3 @@ void RobotControlNode::motor_reset_fault_all()
     motor_fault(0, 0);
 }
 
-void RobotControlNode::motor_enable_all(double enable)
-{
-    motor_enable(0, enable);
-    usleep(20000);
-}

src/robot_control/src/utils/s_curve_trajectory.cpp

@@ -247,8 +247,6 @@ std::vector<double> S_CurveTrajectory::update(double time)
         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;
@@ -256,61 +254,103 @@ std::vector<double> S_CurveTrajectory::update(double time)
         // 使用统一时间，但各轴的阶段时间可能不同
         double T1 = T1_[i], T2 = T2_[i], T3 = T3_[i], T4 = T4_[i];
         double T5 = T5_[i], T6 = T6_[i];
+        double T7 = T7_[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;
+        // Use progress variables in positive direction, then apply sign.
+        const double j_abs = std::abs(j);
+        const double a_abs = std::abs(a_max);
+        const double sign = (dir >= 0.0) ? 1.0 : -1.0;
+
+        auto clamp01 = [](double x) { return std::clamp(x, 0.0, 1.0); };
+        (void)clamp01;
+
+        const double t1 = T1;
+        const double t2 = T2;
+        const double t3 = T3;
+        const double t4 = T4;
+        const double t5 = T5;
+        const double t6 = T6;
+        const double t7 = T7;
+
+        // Boundary values (positive direction)
+        const double s1 = j_abs * t1 * t1 * t1 / 6.0;
+        const double v1 = 0.5 * j_abs * t1 * t1;
+
+        const double s2 = s1 + v1 * t2 + 0.5 * a_abs * t2 * t2;
+        const double v2 = v1 + a_abs * t2;
+
+        const double s3 = s2 + v2 * t3 + 0.5 * a_abs * t3 * t3 - j_abs * t3 * t3 * t3 / 6.0;
+        const double v3 = v2 + a_abs * t3 - 0.5 * j_abs * t3 * t3;
+
+        const double s4 = s3 + v3 * t4;
+
+        const double s5 = s4 + v3 * t5 - j_abs * t5 * t5 * t5 / 6.0;
+        const double v5 = v3 - 0.5 * j_abs * t5 * t5;
+
+        const double s6 = s5 + v5 * t6 - 0.5 * a_abs * t6 * t6;
+        const double v6 = v5 - a_abs * t6;
+
+        double s_prog = 0.0;  // [0, dist]
+        double v_prog = 0.0;
+        double a_prog = 0.0;
+
+        if (t <= t1) {
+            // Phase 1: jerk +j
+            const double dt = t;
+            a_prog = j_abs * dt;
+            v_prog = 0.5 * j_abs * dt * dt;
+            s_prog = j_abs * dt * dt * dt / 6.0;
+        } else if (t <= t1 + t2) {
+            // Phase 2: const acc +a
+            const double dt = t - t1;
+            a_prog = a_abs;
+            v_prog = v1 + a_abs * dt;
+            s_prog = s1 + v1 * dt + 0.5 * a_abs * dt * dt;
+        } else if (t <= t1 + t2 + t3) {
+            // Phase 3: jerk -j
+            const double dt = t - t1 - t2;
+            a_prog = a_abs - j_abs * dt;
+            v_prog = v2 + a_abs * dt - 0.5 * j_abs * dt * dt;
+            s_prog = s2 + v2 * dt + 0.5 * a_abs * dt * dt - j_abs * dt * dt * dt / 6.0;
+        } else if (t <= t1 + t2 + t3 + t4) {
+            // Phase 4: const vel
+            const double dt = t - t1 - t2 - t3;
+            a_prog = 0.0;
+            v_prog = v3;
+            s_prog = s3 + v3 * dt;
+        } else if (t <= t1 + t2 + t3 + t4 + t5) {
+            // Phase 5: jerk -j (acc 0 -> -a)
+            const double dt = t - t1 - t2 - t3 - t4;
+            a_prog = -j_abs * dt;
+            v_prog = v3 - 0.5 * j_abs * dt * dt;
+            s_prog = s4 + v3 * dt - j_abs * dt * dt * dt / 6.0;
+        } else if (t <= t1 + t2 + t3 + t4 + t5 + t6) {
+            // Phase 6: const acc -a
+            const double dt = t - t1 - t2 - t3 - t4 - t5;
+            a_prog = -a_abs;
+            v_prog = v5 - a_abs * dt;
+            s_prog = s5 + v5 * dt - 0.5 * a_abs * 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;
+            // Phase 7: jerk +j (acc -a -> 0)
+            double dt = t - t1 - t2 - t3 - t4 - t5 - t6;
+            if (dt < 0.0) dt = 0.0;
+            if (t7 > 0.0) dt = std::min(dt, t7);
+            a_prog = -a_abs + j_abs * dt;
+            v_prog = v6 - a_abs * dt + 0.5 * j_abs * dt * dt;
+            s_prog = s6 + v6 * dt - 0.5 * a_abs * dt * dt + j_abs * dt * dt * dt / 6.0;
         }
 
+        if (!std::isfinite(s_prog) || !std::isfinite(v_prog) || !std::isfinite(a_prog)) {
+            s_prog = 0.0;
+            v_prog = 0.0;
+            a_prog = 0.0;
+        }
+        s_prog = std::clamp(s_prog, 0.0, dist);
+
+        pos_offset = sign * s_prog;
+        vel = sign * v_prog;
+        acc = sign * a_prog;
+
         current_pos_[i] = start_pos_[i] + pos_offset;
         current_velocity_[i] = vel;
         current_acceleration_[i] = acc;