moveL function added

src/robot_control/include/actions/dual_arm_action.hpp

@@ -11,6 +11,7 @@
 #include "control_msgs/action/follow_joint_trajectory.hpp"
 #include "interfaces/action/dual_arm.hpp"
 #include "trajectory_msgs/msg/joint_trajectory.hpp"
+#include "geometry_msgs/msg/pose.hpp"
 
 #include "actions/action_context.hpp"
 
@@ -44,7 +45,7 @@ private:
   void handle_accepted_(const std::shared_ptr<GoalHandleDualArm> goal_handle);
   void execute_(const std::shared_ptr<GoalHandleDualArm> goal_handle);
 
-  bool collect_movej_targets_(
+  bool collect_dual_arm_joint_targets_(
     const DualArm::Goal& goal,
     size_t arm_dof,
     bool* need_left,
@@ -53,6 +54,10 @@ private:
     std::vector<double>* right_target,
     std::string* error_msg) const;
 
+  bool validate_movel_pose_(
+    const geometry_msgs::msg::Pose& pose,
+    std::string* error_msg) const;
+
   bool export_and_normalize_trajectory_(
     const MotionParameters& motion_params,
     trajectory_msgs::msg::JointTrajectory* out,
@@ -160,6 +165,9 @@ private:
 
   bool save_trajectory_enabled_{true};
   bool trajectory_execution_enabled_{true};
+  bool movel_sync_use_s_curve_progress_{true};
+  double movel_sync_min_dt_sec_{0.01};
+  int movel_sync_max_samples_{500};
 
 };
 }  // namespace robot_control

src/robot_control/include/utils/trajectory_utils.hpp

@@ -9,6 +9,7 @@
 #include "trajectory_msgs/msg/joint_trajectory.hpp"
 #include <algorithm>
 #include <cmath>
+#include <string>
 #include <vector>
 
 namespace robot_control
@@ -143,4 +144,226 @@ inline trajectory_msgs::msg::JointTrajectory make_deceleration_trajectory(
   return traj;
 }
 
+/**
+ * @brief 将值限制到 [0, 1]
+ */
+inline double clamp01(double v)
+{
+  if (v < 0.0) return 0.0;
+  if (v > 1.0) return 1.0;
+  return v;
+}
+
+/**
+ * @brief Quintic smoothstep 进度映射（S型）
+ *
+ * 输入/输出均为 [0,1]，且在 0/1 两端的一阶、二阶导均为 0，
+ * 适合做起停平滑的时间重标定。
+ */
+inline double s_curve_progress(double u)
+{
+  u = clamp01(u);
+  const double u2 = u * u;
+  const double u3 = u2 * u;
+  const double u4 = u3 * u;
+  const double u5 = u4 * u;
+  return 6.0 * u5 - 15.0 * u4 + 10.0 * u3;
+}
+
+/**
+ * @brief 在指定时刻对轨迹做关节位置线性插值
+ *
+ * - t 早于首点时，返回首点位置
+ * - t 晚于尾点时，返回尾点位置
+ */
+inline bool interpolate_positions_at_time(
+  const trajectory_msgs::msg::JointTrajectory& traj,
+  double t_sec,
+  std::vector<double>* out_positions)
+{
+  if (!out_positions || traj.points.empty()) return false;
+  const size_t dof = traj.joint_names.size();
+  if (dof == 0) return false;
+  out_positions->assign(dof, 0.0);
+
+  const auto time_of = [](const trajectory_msgs::msg::JointTrajectoryPoint& p) {
+    return static_cast<double>(p.time_from_start.sec) +
+           static_cast<double>(p.time_from_start.nanosec) * 1e-9;
+  };
+
+  const double t0 = time_of(traj.points.front());
+  const double tN = time_of(traj.points.back());
+  if (traj.points.front().positions.size() < dof ||
+      traj.points.back().positions.size() < dof) {
+    return false;
+  }
+
+  if (t_sec <= t0 + 1e-9) {
+    *out_positions = traj.points.front().positions;
+    return true;
+  }
+  if (t_sec >= tN - 1e-9) {
+    *out_positions = traj.points.back().positions;
+    return true;
+  }
+
+  for (size_t i = 0; i + 1 < traj.points.size(); ++i) {
+    const double ta = time_of(traj.points[i]);
+    const double tb = time_of(traj.points[i + 1]);
+    if (t_sec <= tb + 1e-9) {
+      if (traj.points[i].positions.size() < dof ||
+          traj.points[i + 1].positions.size() < dof) {
+        return false;
+      }
+      const double den = std::max(1e-9, tb - ta);
+      const double a = clamp01((t_sec - ta) / den);
+      for (size_t j = 0; j < dof; ++j) {
+        const double q0 = traj.points[i].positions[j];
+        const double q1 = traj.points[i + 1].positions[j];
+        (*out_positions)[j] = q0 + a * (q1 - q0);
+      }
+      return true;
+    }
+  }
+
+  *out_positions = traj.points.back().positions;
+  return true;
+}
+
+/**
+ * @brief 基于位置点列回填速度与加速度（数值差分）
+ */
+inline void fill_velocity_acceleration(
+  trajectory_msgs::msg::JointTrajectory* traj)
+{
+  if (!traj || traj->points.empty()) return;
+  const size_t n = traj->points.size();
+  const size_t dof = traj->joint_names.size();
+  if (dof == 0) return;
+
+  const auto time_of = [](const trajectory_msgs::msg::JointTrajectoryPoint& p) {
+    return static_cast<double>(p.time_from_start.sec) +
+           static_cast<double>(p.time_from_start.nanosec) * 1e-9;
+  };
+
+  for (auto& p : traj->points) {
+    if (p.positions.size() < dof) return;
+    p.velocities.assign(dof, 0.0);
+    p.accelerations.assign(dof, 0.0);
+  }
+  if (n < 2) return;
+
+  for (size_t i = 0; i < n; ++i) {
+    const size_t i0 = (i == 0) ? 0 : (i - 1);
+    const size_t i1 = (i + 1 >= n) ? (n - 1) : (i + 1);
+    const double t0 = time_of(traj->points[i0]);
+    const double t1 = time_of(traj->points[i1]);
+    const double dt = std::max(1e-6, t1 - t0);
+    for (size_t j = 0; j < dof; ++j) {
+      const double q0 = traj->points[i0].positions[j];
+      const double q1 = traj->points[i1].positions[j];
+      traj->points[i].velocities[j] = (q1 - q0) / dt;
+    }
+  }
+  for (size_t i = 0; i < n; ++i) {
+    const size_t i0 = (i == 0) ? 0 : (i - 1);
+    const size_t i1 = (i + 1 >= n) ? (n - 1) : (i + 1);
+    const double t0 = time_of(traj->points[i0]);
+    const double t1 = time_of(traj->points[i1]);
+    const double dt = std::max(1e-6, t1 - t0);
+    for (size_t j = 0; j < dof; ++j) {
+      const double v0 = traj->points[i0].velocities[j];
+      const double v1 = traj->points[i1].velocities[j];
+      traj->points[i].accelerations[j] = (v1 - v0) / dt;
+    }
+  }
+}
+
+/**
+ * @brief 双臂轨迹统一时间轴重采样（可选S型进度）
+ *
+ * 目标是让左右臂输出轨迹拥有相同的时间戳序列，实现同步执行。
+ * @param use_s_curve_progress true 时对进度使用 S 型映射，降低起停冲击
+ */
+inline bool synchronize_dual_arm_trajectory_timebase(
+  const trajectory_msgs::msg::JointTrajectory& left_in,
+  const trajectory_msgs::msg::JointTrajectory& right_in,
+  trajectory_msgs::msg::JointTrajectory* left_out,
+  trajectory_msgs::msg::JointTrajectory* right_out,
+  double* out_total_time,
+  std::string* error_msg = nullptr,
+  bool use_s_curve_progress = true,
+  double min_dt = 0.01,
+  size_t max_samples = 500)
+{
+  if (!left_out || !right_out || !out_total_time) {
+    if (error_msg) *error_msg = "Null output pointer";
+    return false;
+  }
+  if (left_in.points.empty() || right_in.points.empty()) {
+    if (error_msg) *error_msg = "Empty left/right trajectory";
+    return false;
+  }
+
+  const auto total_time = [](const trajectory_msgs::msg::JointTrajectory& t) {
+    const auto& p = t.points.back();
+    return static_cast<double>(p.time_from_start.sec) +
+           static_cast<double>(p.time_from_start.nanosec) * 1e-9;
+  };
+  const double left_T = total_time(left_in);
+  const double right_T = total_time(right_in);
+  const double T = std::max(left_T, right_T);
+  *out_total_time = T;
+
+  left_out->header = left_in.header;
+  right_out->header = right_in.header;
+  left_out->joint_names = left_in.joint_names;
+  right_out->joint_names = right_in.joint_names;
+  left_out->points.clear();
+  right_out->points.clear();
+
+  if (T <= 1e-6) {
+    left_out->points.push_back(left_in.points.back());
+    right_out->points.push_back(right_in.points.back());
+    fill_velocity_acceleration(left_out);
+    fill_velocity_acceleration(right_out);
+    return true;
+  }
+
+  min_dt = std::max(0.001, min_dt);
+  max_samples = std::max<size_t>(3, max_samples);
+
+  size_t samples = std::max(left_in.points.size(), right_in.points.size());
+  samples = std::max<size_t>(samples, 3);
+  const size_t min_samples_by_dt = static_cast<size_t>(std::ceil(T / min_dt)) + 1;
+  if (samples < min_samples_by_dt) samples = min_samples_by_dt;
+  if (samples > max_samples) samples = max_samples;
+
+  left_out->points.reserve(samples);
+  right_out->points.reserve(samples);
+  for (size_t k = 0; k < samples; ++k) {
+    const double u = (samples == 1) ? 1.0 : (static_cast<double>(k) / static_cast<double>(samples - 1));
+    const double progress = use_s_curve_progress ? s_curve_progress(u) : u;
+    const double sample_t = progress * T;
+    const double output_t = u * T;
+
+    trajectory_msgs::msg::JointTrajectoryPoint lp;
+    trajectory_msgs::msg::JointTrajectoryPoint rp;
+    if (!interpolate_positions_at_time(left_in, sample_t, &lp.positions) ||
+        !interpolate_positions_at_time(right_in, sample_t, &rp.positions)) {
+      if (error_msg) *error_msg = "Trajectory interpolation failed";
+      return false;
+    }
+
+    sec_to_duration(output_t, &lp.time_from_start);
+    sec_to_duration(output_t, &rp.time_from_start);
+    left_out->points.push_back(std::move(lp));
+    right_out->points.push_back(std::move(rp));
+  }
+
+  fill_velocity_acceleration(left_out);
+  fill_velocity_acceleration(right_out);
+  return true;
+}
+
 }  // namespace robot_control

src/robot_control/src/actions/dual_arm_action.cpp

@@ -35,6 +35,40 @@ 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");
+
+    if (!ctx_.node->has_parameter("dual_arm.movel_sync.use_s_curve_progress")) {
+      movel_sync_use_s_curve_progress_ =
+        ctx_.node->declare_parameter<bool>(
+          "dual_arm.movel_sync.use_s_curve_progress",
+          movel_sync_use_s_curve_progress_);
+    } else {
+      (void)ctx_.node->get_parameter(
+        "dual_arm.movel_sync.use_s_curve_progress",
+        movel_sync_use_s_curve_progress_);
+    }
+    if (!ctx_.node->has_parameter("dual_arm.movel_sync.min_dt_sec")) {
+      movel_sync_min_dt_sec_ =
+        ctx_.node->declare_parameter<double>(
+          "dual_arm.movel_sync.min_dt_sec",
+          movel_sync_min_dt_sec_);
+    } else {
+      (void)ctx_.node->get_parameter(
+        "dual_arm.movel_sync.min_dt_sec",
+        movel_sync_min_dt_sec_);
+    }
+    if (!ctx_.node->has_parameter("dual_arm.movel_sync.max_samples")) {
+      movel_sync_max_samples_ =
+        ctx_.node->declare_parameter<int>(
+          "dual_arm.movel_sync.max_samples",
+          movel_sync_max_samples_);
+    } else {
+      (void)ctx_.node->get_parameter(
+        "dual_arm.movel_sync.max_samples",
+        movel_sync_max_samples_);
+    }
+
+    movel_sync_min_dt_sec_ = std::max(0.001, movel_sync_min_dt_sec_);
+    movel_sync_max_samples_ = std::max(3, movel_sync_max_samples_);
   }
 
   server_ = rclcpp_action::create_server<DualArm>(
@@ -43,7 +77,12 @@ void DualArmAction::initialize()
     std::bind(&DualArmAction::handle_goal_, this, _1, _2),
     std::bind(&DualArmAction::handle_cancel_, this, _1),
     std::bind(&DualArmAction::handle_accepted_, this, _1));
-  RCLCPP_INFO(ctx_.node->get_logger(), "DualArm action server is ready");
+  RCLCPP_INFO(
+    ctx_.node->get_logger(),
+    "DualArm action server is ready (movel_sync: s_curve=%s, min_dt=%.4f, max_samples=%d)",
+    movel_sync_use_s_curve_progress_ ? "true" : "false",
+    movel_sync_min_dt_sec_,
+    movel_sync_max_samples_);
 }
 
 rclcpp_action::GoalResponse DualArmAction::handle_goal_(
@@ -111,20 +150,26 @@ rclcpp_action::GoalResponse DualArmAction::handle_goal_(
       return rclcpp_action::GoalResponse::REJECT;
     }
 
-    // 目前只支持 MOVEJ
-    if (arm_param.motion_type != arm_param.MOVEJ) {
-      RCLCPP_ERROR(ctx_.node->get_logger(),
-        "DualArm goal rejected: only MOVEJ is supported for now (got motion_type=%d for arm_id=%d)",
-        arm_param.motion_type, arm_param.arm_id);
-      return rclcpp_action::GoalResponse::REJECT;
-    }
-
-    // 关节限位检查
     std::string err;
-    if (!ctx_.robot_control_manager->ValidateArmJointTarget(arm_param.arm_id, arm_param.target_joints, &err)) {
+    if (arm_param.motion_type == arm_param.MOVEJ) {
+      if (!ctx_.robot_control_manager->ValidateArmJointTarget(
+            arm_param.arm_id, arm_param.target_joints, &err)) {
+        RCLCPP_ERROR(ctx_.node->get_logger(),
+          "DualArm goal rejected: MOVEJ target_joints invalid for arm_id %d: %s",
+          arm_param.arm_id, err.c_str());
+        return rclcpp_action::GoalResponse::REJECT;
+      }
+    } else if (arm_param.motion_type == arm_param.MOVEL) {
+      if (!validate_movel_pose_(arm_param.target_pose, &err)) {
+        RCLCPP_ERROR(ctx_.node->get_logger(),
+          "DualArm goal rejected: MOVEL target_pose invalid for arm_id %d: %s",
+          arm_param.arm_id, err.c_str());
+        return rclcpp_action::GoalResponse::REJECT;
+      }
+    } else {
       RCLCPP_ERROR(ctx_.node->get_logger(),
-        "DualArm goal rejected: MOVEJ target_joints exceed limits for arm_id %d: %s",
-        arm_param.arm_id, err.c_str());
+        "DualArm goal rejected: unsupported motion_type=%d for arm_id=%d",
+        arm_param.motion_type, arm_param.arm_id);
       return rclcpp_action::GoalResponse::REJECT;
     }
 
@@ -152,7 +197,7 @@ rclcpp_action::GoalResponse DualArmAction::handle_goal_(
       return rclcpp_action::GoalResponse::REJECT;
     }
 
-    // 双臂模式：两只手都不能在执行，且只允许 MOVEJ
+    // 双臂模式：两只手都不能在执行
     if (executing_left_.load() || executing_right_.load()) {
       RCLCPP_ERROR(ctx_.node->get_logger(),
         "DualArm goal rejected: left or right arm is executing another goal");
@@ -160,17 +205,26 @@ rclcpp_action::GoalResponse DualArmAction::handle_goal_(
     }
 
     for (const auto& arm_param : goal->arm_motion_params) {
-      if (arm_param.motion_type != arm_param.MOVEJ) {
-        RCLCPP_ERROR(ctx_.node->get_logger(),
-          "DualArm goal rejected: only MOVEJ is supported for now (got motion_type=%d for arm_id=%d)",
-          arm_param.motion_type, arm_param.arm_id);
-        return rclcpp_action::GoalResponse::REJECT;
-      }
       std::string err;
-      if (!ctx_.robot_control_manager->ValidateArmJointTarget(arm_param.arm_id, arm_param.target_joints, &err)) {
+      if (arm_param.motion_type == arm_param.MOVEJ) {
+        if (!ctx_.robot_control_manager->ValidateArmJointTarget(
+              arm_param.arm_id, arm_param.target_joints, &err)) {
+          RCLCPP_ERROR(ctx_.node->get_logger(),
+            "DualArm goal rejected: MOVEJ target_joints invalid for arm_id %d: %s",
+            arm_param.arm_id, err.c_str());
+          return rclcpp_action::GoalResponse::REJECT;
+        }
+      } else if (arm_param.motion_type == arm_param.MOVEL) {
+        if (!validate_movel_pose_(arm_param.target_pose, &err)) {
+          RCLCPP_ERROR(ctx_.node->get_logger(),
+            "DualArm goal rejected: MOVEL target_pose invalid for arm_id %d: %s",
+            arm_param.arm_id, err.c_str());
+          return rclcpp_action::GoalResponse::REJECT;
+        }
+      } else {
         RCLCPP_ERROR(ctx_.node->get_logger(),
-          "DualArm goal rejected: MOVEJ target_joints exceed limits for arm_id %d: %s",
-          arm_param.arm_id, err.c_str());
+          "DualArm goal rejected: unsupported motion_type=%d for arm_id=%d",
+          arm_param.motion_type, arm_param.arm_id);
         return rclcpp_action::GoalResponse::REJECT;
       }
     }
@@ -196,7 +250,7 @@ void DualArmAction::handle_accepted_(const std::shared_ptr<GoalHandleDualArm> go
   std::thread{std::bind(&DualArmAction::execute_, this, _1), goal_handle}.detach();
 }
 
-bool DualArmAction::collect_movej_targets_(
+bool DualArmAction::collect_dual_arm_joint_targets_(
   const DualArm::Goal& goal,
   size_t arm_dof,
   bool* need_left,
@@ -216,25 +270,67 @@ bool DualArmAction::collect_movej_targets_(
   right_target->clear();
 
   for (const auto& arm_param : goal.arm_motion_params) {
-    if (arm_param.motion_type != arm_param.MOVEJ) {
-      if (error_msg) *error_msg = "Only MOVEJ is supported for DualArm";
-      return false;
-    }
-    if (arm_param.target_joints.size() != arm_dof) {
+    std::vector<double> target_joint;
+    if (arm_param.motion_type == arm_param.MOVEJ) {
+      target_joint = arm_param.target_joints;
+      if (target_joint.size() != arm_dof) {
+        if (error_msg) {
+          *error_msg =
+            "target_joints size mismatch for arm_id " + std::to_string(arm_param.arm_id) +
+            " (expected " + std::to_string(arm_dof) +
+            ", got " + std::to_string(target_joint.size()) + ")";
+        }
+        return false;
+      }
+    } else if (arm_param.motion_type == arm_param.MOVEL) {
+      std::string err;
+      if (!validate_movel_pose_(arm_param.target_pose, &err)) {
+        if (error_msg) {
+          *error_msg = "MOVEL target_pose invalid for arm_id " +
+                       std::to_string(arm_param.arm_id) + ": " + err;
+        }
+        return false;
+      }
+      if (!ctx_.robot_control_manager->SolveArmInverseKinematics(
+            arm_param.arm_id, arm_param.target_pose, &target_joint, &err)) {
+        if (error_msg) {
+          *error_msg = "MOVEL IK failed for arm_id " +
+                       std::to_string(arm_param.arm_id) + ": " + err;
+        }
+        return false;
+      }
+      if (target_joint.size() != arm_dof) {
+        if (error_msg) {
+          *error_msg =
+            "MOVEL IK output size mismatch for arm_id " + std::to_string(arm_param.arm_id) +
+            " (expected " + std::to_string(arm_dof) +
+            ", got " + std::to_string(target_joint.size()) + ")";
+        }
+        return false;
+      }
+    } else {
       if (error_msg) {
-        *error_msg =
-          "target_joints size mismatch for arm_id " + std::to_string(arm_param.arm_id) +
-          " (expected " + std::to_string(arm_dof) +
-          ", got " + std::to_string(arm_param.target_joints.size()) + ")";
+        *error_msg = "Unsupported motion_type for arm_id " + std::to_string(arm_param.arm_id);
       }
       return false;
     }
+
+    std::string limit_err;
+    if (!ctx_.robot_control_manager->ValidateArmJointTarget(
+          arm_param.arm_id, target_joint, &limit_err)) {
+      if (error_msg) {
+        *error_msg = "Target joint out of limits for arm_id " +
+                     std::to_string(arm_param.arm_id) + ": " + limit_err;
+      }
+      return false;
+    }
+
     if (arm_param.arm_id == 0) {
       *need_left = true;
-      *left_target = arm_param.target_joints;
+      *left_target = target_joint;
     } else if (arm_param.arm_id == 1) {
       *need_right = true;
-      *right_target = arm_param.target_joints;
+      *right_target = target_joint;
     } else {
       if (error_msg) *error_msg = "Invalid arm_id (expected 0=left, 1=right)";
       return false;
@@ -248,6 +344,29 @@ bool DualArmAction::collect_movej_targets_(
   return true;
 }
 
+bool DualArmAction::validate_movel_pose_(
+  const geometry_msgs::msg::Pose& pose,
+  std::string* error_msg) const
+{
+  auto finite = [](double v) { return std::isfinite(v); };
+  if (!finite(pose.position.x) || !finite(pose.position.y) || !finite(pose.position.z) ||
+      !finite(pose.orientation.x) || !finite(pose.orientation.y) ||
+      !finite(pose.orientation.z) || !finite(pose.orientation.w)) {
+    if (error_msg) *error_msg = "Pose contains NaN/Inf";
+    return false;
+  }
+
+  const double qn2 = pose.orientation.x * pose.orientation.x +
+                     pose.orientation.y * pose.orientation.y +
+                     pose.orientation.z * pose.orientation.z +
+                     pose.orientation.w * pose.orientation.w;
+  if (qn2 < 1e-8) {
+    if (error_msg) *error_msg = "Quaternion norm is too small";
+    return false;
+  }
+  return true;
+}
+
 bool DualArmAction::export_and_normalize_trajectory_(
   const MotionParameters& motion_params,
   trajectory_msgs::msg::JointTrajectory* out,
@@ -620,52 +739,117 @@ void DualArmAction::execute_dual_arm_(
   std::vector<double> left_target;
   std::vector<double> right_target;
   std::string err;
-  if (!collect_movej_targets_(*goal, ARM_DOF, &need_left, &left_target, &need_right, &right_target, &err)) {
+  const ArmMotionParams* left_param = nullptr;
+  const ArmMotionParams* right_param = nullptr;
+  bool has_movel = false;
+  for (const auto& arm_param : goal->arm_motion_params) {
+    if (arm_param.arm_id == ArmMotionParams::ARM_LEFT) {
+      left_param = &arm_param;
+    } else if (arm_param.arm_id == ArmMotionParams::ARM_RIGHT) {
+      right_param = &arm_param;
+    }
+    if (arm_param.motion_type == arm_param.MOVEL) {
+      has_movel = true;
+    }
+  }
+  if (!left_param || !right_param) {
     planner_busy_.store(false);
     result->success = false;
-    result->message = std::string("DualArm goal invalid: ") + err;
+    result->message = "DualArm goal invalid: missing left or right arm params";
     goal_handle->abort(result);
     return;
   }
-  if (!need_left || !need_right) {
-    planner_busy_.store(false);
-    result->success = false;
-    result->message = "DualArm goal invalid: dual-arm requires both left and right targets";
-    goal_handle->abort(result);
-    return;
-  }
-
   const double velocity_scaling = accepted_velocity_scale_;
+  trajectory_msgs::msg::JointTrajectory traj_left;
+  trajectory_msgs::msg::JointTrajectory traj_right;
+  double total_time = 0.0;
 
-  if (!ctx_.robot_control_manager->PlanDualArmJointMotion(
-        need_left, left_target, need_right, right_target,
-        velocity_scaling, acceleration_scaling))
-  {
-    planner_busy_.store(false);
-    result->success = false;
-    result->message = "DualArm MoveIt planning failed (dual_arm group)";
-    goal_handle->abort(result);
-    return;
+  if (!has_movel) {
+    if (!collect_dual_arm_joint_targets_(
+          *goal, ARM_DOF, &need_left, &left_target, &need_right, &right_target, &err)) {
+      planner_busy_.store(false);
+      result->success = false;
+      result->message = std::string("DualArm goal invalid: ") + err;
+      goal_handle->abort(result);
+      return;
+    }
+    if (!need_left || !need_right) {
+      planner_busy_.store(false);
+      result->success = false;
+      result->message = "DualArm goal invalid: dual-arm requires both left and right targets";
+      goal_handle->abort(result);
+      return;
+    }
+
+    if (!ctx_.robot_control_manager->PlanDualArmJointMotion(
+          need_left, left_target, need_right, right_target,
+          velocity_scaling, acceleration_scaling))
+    {
+      planner_busy_.store(false);
+      result->success = false;
+      result->message = "DualArm MoveIt planning failed (dual_arm group, MOVEJ)";
+      goal_handle->abort(result);
+      return;
+    }
+
+    trajectory_msgs::msg::JointTrajectory traj;
+    if (!export_and_normalize_trajectory_(motion_params, &traj, &total_time, &err)) {
+      planner_busy_.store(false);
+      result->success = false;
+      result->message = std::string("Export/normalize trajectory failed: ") + err;
+      goal_handle->abort(result);
+      return;
+    }
+    split_dual_arm_trajectory_(motion_params, ARM_DOF, traj, &traj_left, &traj_right);
+  } else {
+    // MOVEL 需要保持笛卡尔直线路径：分别对左右臂做单臂笛卡尔/关节规划，再统一时间轴同步执行
+    if (!ctx_.robot_control_manager->PlanArmMotion(
+          *left_param, velocity_scaling, acceleration_scaling)) {
+      planner_busy_.store(false);
+      result->success = false;
+      result->message = "DualArm planning failed: left arm PlanArmMotion failed";
+      goal_handle->abort(result);
+      return;
+    }
+    if (!ctx_.robot_control_manager->PlanArmMotion(
+          *right_param, velocity_scaling, acceleration_scaling)) {
+      planner_busy_.store(false);
+      result->success = false;
+      result->message = "DualArm planning failed: right arm PlanArmMotion failed";
+      goal_handle->abort(result);
+      return;
+    }
+
+    trajectory_msgs::msg::JointTrajectory left_raw;
+    trajectory_msgs::msg::JointTrajectory right_raw;
+    if (!ctx_.robot_control_manager->ExportArmPlannedTrajectory(ArmMotionParams::ARM_LEFT, left_raw) ||
+        !ctx_.robot_control_manager->ExportArmPlannedTrajectory(ArmMotionParams::ARM_RIGHT, right_raw)) {
+      planner_busy_.store(false);
+      result->success = false;
+      result->message = "DualArm planning failed: export single-arm trajectory failed";
+      goal_handle->abort(result);
+      return;
+    }
+
+    if (!synchronize_dual_arm_trajectory_timebase(
+          left_raw, right_raw, &traj_left, &traj_right, &total_time, &err,
+          movel_sync_use_s_curve_progress_,
+          movel_sync_min_dt_sec_,
+          static_cast<size_t>(movel_sync_max_samples_))) {
+      planner_busy_.store(false);
+      result->success = false;
+      result->message = std::string("DualArm MOVEL sync failed: ") + err;
+      goal_handle->abort(result);
+      return;
+    }
   }
 
   planner_busy_.store(false);
-
   feedback->status = 1;
 
-  trajectory_msgs::msg::JointTrajectory traj;
-  double total_time = 0.0;
-  if (!export_and_normalize_trajectory_(motion_params, &traj, &total_time, &err)) {
-    result->success = false;
-    result->message = std::string("Export/normalize trajectory failed: ") + err;
-    goal_handle->abort(result);
-    return;
-  }
-
-  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);
+  total_time = std::max(trajectory_total_time_sec_(traj_left), trajectory_total_time_sec_(traj_right));
   save_trajectory_to_file_(traj_left, "dual_left");
   save_trajectory_to_file_(traj_right, "dual_right");
   if (!trajectory_execution_enabled_) {

src/robot_control/src/controllers/arm_control.cpp

@@ -12,6 +12,7 @@
 #include "moveit/move_group_interface/move_group_interface.h"
 #include "moveit/robot_state/robot_state.h"
 #include "moveit_msgs/msg/move_it_error_codes.hpp"
+#include "moveit_msgs/msg/robot_trajectory.hpp"
 
 #ifndef M_PI
 #define M_PI 3.14159265358979323846
@@ -286,19 +287,37 @@ bool ArmControl::PlanCartesianMotion(
         move_group->setMaxVelocityScalingFactor(velocity_scaling);
         move_group->setMaxAccelerationScalingFactor(acceleration_scaling);
 
-        // 设置目标位姿
-        move_group->setPoseTarget(target_pose);
+        // 使用 computeCartesianPath 生成真实笛卡尔直线路径（而非仅末端位姿目标）
+        std::vector<geometry_msgs::msg::Pose> waypoints;
+        waypoints.reserve(2);
+        waypoints.push_back(move_group->getCurrentPose().pose);
+        waypoints.push_back(target_pose);
 
-        // 规划
-        moveit::planning_interface::MoveGroupInterface::Plan plan;
-        moveit::core::MoveItErrorCode error_code = move_group->plan(plan);
+        moveit_msgs::msg::RobotTrajectory cartesian_traj;
+        constexpr double kEefStep = 0.01;      // 1cm 离散步长
+        constexpr double kJumpThreshold = 0.0; // 禁用 jump 判据
+        const double fraction = move_group->computeCartesianPath(
+            waypoints, kEefStep, kJumpThreshold, cartesian_traj, true);
 
-        if (error_code != moveit::core::MoveItErrorCode::SUCCESS) {
+        if (fraction < 0.999) {
             if (node_) {
-                RCLCPP_ERROR(node_->get_logger(), "PlanCartesianMotion failed: error_code=%d arm_id=%d", error_code.val, static_cast<int>(arm_id));
+                RCLCPP_ERROR(
+                    node_->get_logger(),
+                    "PlanCartesianMotion failed: Cartesian path fraction=%.3f arm_id=%d",
+                    fraction,
+                    static_cast<int>(arm_id));
             }
             return false;
         }
+        if (cartesian_traj.joint_trajectory.points.empty()) {
+            if (node_) {
+                RCLCPP_ERROR(node_->get_logger(), "PlanCartesianMotion failed: empty cartesian trajectory arm_id=%d", static_cast<int>(arm_id));
+            }
+            return false;
+        }
+
+        moveit::planning_interface::MoveGroupInterface::Plan plan;
+        plan.trajectory_ = cartesian_traj;
         return timeParameterizeAndStore_(
             arm_id, plan, velocity_scaling, acceleration_scaling);
     } catch (const std::exception& e) {