code optimization

src/hc_dual_arm_motion/CMakeLists.txt

@@ -1,8 +1,5 @@
 cmake_minimum_required(VERSION 3.10)
 
-cmake_policy(SET CMP0167 NEW)  
-add_compile_options(-Wall -Wextra -Wpedantic -Wno-dev) 
-
 project(hc_dual_arm_motion)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")

src/hc_dual_arm_motion/config/hc_dual_arm_motion_config.yaml

@@ -1,4 +1,3 @@
-# 注意：根节点名必须和Launch中Node的name参数完全一致（hc_dual_arm_motion）
 hc_dual_arm_motion_server:
   ros__parameters:
     dual_arm_move_group: "dual_arm"

src/hc_dual_arm_motion/include/hc_dual_arm_motion_server.hpp

@@ -109,15 +109,15 @@ private:
 
   void handle_accepted(const shared_ptr<GoalHandleRobotMotion> goal_handle);
   void execute(const shared_ptr<GoalHandleRobotMotion> goal_handle);
-  bool execute_arm_motion(const std::shared_ptr<const HcDualArmAction::Goal> goal, std::shared_ptr<HcDualArmAction::Feedback> feedback, std::shared_ptr<HcDualArmAction::Result> result);
+  bool execute_arm_motion(const std::shared_ptr<const HcDualArmAction::Goal> goal, std::shared_ptr<HcDualArmAction::Feedback> feedback, std::shared_ptr<HcDualArmAction::Result> result, const std::shared_ptr<GoalHandleRobotMotion> goal_handle);
 
   MoveGroupInterface::Plan single_arm_plan(MoveGroupInterface& move_group, ArmMotionParams motion_Params, double step_value);
   RobotTrajectory mergeTrajectories(const moveit_msgs::msg::RobotTrajectory& left_traj,const moveit_msgs::msg::RobotTrajectory& right_traj);
   RobotTrajectory scaleTrajectoryTime(const moveit_msgs::msg::RobotTrajectory& traj, double target_duration);
 
   // 运动执行函数
-  bool execute_single_arm_motion(const shared_ptr<const HcDualArmAction::Goal> goal, shared_ptr<HcDualArmAction::Feedback> feedback, shared_ptr<HcDualArmAction::Result> result, ArmType arm_type);
-  bool execute_dual_arm_motion(const shared_ptr<const HcDualArmAction::Goal> goal, shared_ptr<HcDualArmAction::Feedback> feedback, shared_ptr<HcDualArmAction::Result> result);
+  bool execute_single_arm_motion(const shared_ptr<const HcDualArmAction::Goal> goal, shared_ptr<HcDualArmAction::Feedback> feedback, shared_ptr<HcDualArmAction::Result> result, ArmType arm_type, const std::shared_ptr<GoalHandleRobotMotion> goal_handle);
+  bool execute_dual_arm_motion(const shared_ptr<const HcDualArmAction::Goal> goal, shared_ptr<HcDualArmAction::Feedback> feedback, shared_ptr<HcDualArmAction::Result> result, const std::shared_ptr<GoalHandleRobotMotion> goal_handle);
 
   // 遥控器回调
   void joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg);

src/hc_dual_arm_motion/src/hc_dual_arm_motion_server.cpp

@@ -157,11 +157,18 @@ void HcDualArmMotionServer::handle_accepted(const std::shared_ptr<GoalHandleRobo
 void HcDualArmMotionServer::execute(const std::shared_ptr<GoalHandleRobotMotion> goal_handle)
 {
   RCLCPP_INFO(this->get_logger(), "Starting motion execution");
+  
+  // 设置运动状态标志
+  is_moving = true;
+  
   const auto goal = goal_handle->get_goal();
   auto feedback = std::make_shared<HcDualArmAction::Feedback>();
   auto result = std::make_shared<HcDualArmAction::Result>();
 
-  bool success = execute_arm_motion(goal, feedback, result);
+  bool success = execute_arm_motion(goal, feedback, result, goal_handle);
+  
+  // 重置运动状态标志
+  is_moving = false;
   
   // 设置Action结果
   if (rclcpp::ok()) 
@@ -182,7 +189,8 @@ void HcDualArmMotionServer::execute(const std::shared_ptr<GoalHandleRobotMotion>
 bool HcDualArmMotionServer::execute_arm_motion(
   const std::shared_ptr<const HcDualArmAction::Goal> goal,
   std::shared_ptr<HcDualArmAction::Feedback> feedback,
-  std::shared_ptr<HcDualArmAction::Result> result)
+  std::shared_ptr<HcDualArmAction::Result> result,
+  const std::shared_ptr<GoalHandleRobotMotion> goal_handle)
 {
   bool success = false;
   const auto arm_type = static_cast<ArmType>(goal->arm_type);
@@ -190,15 +198,15 @@ bool HcDualArmMotionServer::execute_arm_motion(
   switch (arm_type)
   {
   case ArmType::LEFT:
-    success = execute_single_arm_motion(goal, feedback, result, arm_type);
+    success = execute_single_arm_motion(goal, feedback, result, arm_type, goal_handle);
     break;
   case ArmType::RIGHT:
     current_move_group_ptr_ = right_arm_move_group_ptr_;
-    success = execute_single_arm_motion(goal, feedback, result, arm_type);
+    success = execute_single_arm_motion(goal, feedback, result, arm_type, goal_handle);
     break;
   case ArmType::DUAL:
     current_move_group_ptr_ = dual_arm_move_group_ptr_;
-    success = execute_dual_arm_motion(goal, feedback, result);
+    success = execute_dual_arm_motion(goal, feedback, result, goal_handle);
     break;
   default:
     break;
@@ -212,7 +220,8 @@ bool HcDualArmMotionServer::execute_single_arm_motion(
   const std::shared_ptr<const HcDualArmAction::Goal> goal,
   std::shared_ptr<HcDualArmAction::Feedback> feedback,
   std::shared_ptr<HcDualArmAction::Result> result,
-  ArmType arm_type)
+  ArmType arm_type,
+  const std::shared_ptr<GoalHandleRobotMotion> goal_handle)
 {
   if (goal -> arm_motion_params.size() != 1)
     {
@@ -230,7 +239,7 @@ bool HcDualArmMotionServer::execute_single_arm_motion(
   }
 
   current_move_group_ptr_->setMaxVelocityScalingFactor(goal->velocity_scaling);
-  current_move_group_ptr_->setMaxAccelerationScalingFactor(goal->velocity_scaling);
+  current_move_group_ptr_->setMaxAccelerationScalingFactor(goal->acceleration_scaling);
 
   auto temp_motion_param = goal->arm_motion_params[0];
 
@@ -266,6 +275,20 @@ bool HcDualArmMotionServer::execute_single_arm_motion(
     feedback->progress = 0.5;
     feedback->status = static_cast<uint8_t>(MotionStatus::EXECUTING);
     
+    // 更新反馈中的关节角度
+    try {
+      auto current_state = current_move_group_ptr_->getCurrentState();
+      std::vector<double> current_joints = current_move_group_ptr_->getCurrentJointValues();
+      if (arm_type == ArmType::LEFT) {
+        feedback->joints_left = current_joints;
+      } else if (arm_type == ArmType::RIGHT) {
+        feedback->joints_right = current_joints;
+      }
+    } catch (const std::exception& e) {
+      RCLCPP_WARN(this->get_logger(), "Failed to get current joint values for feedback: %s", e.what());
+    }
+    goal_handle->publish_feedback(feedback);
+    
     current_move_group_ptr_->execute(plan);
 
     // 5. 设置结果
@@ -306,6 +329,19 @@ bool HcDualArmMotionServer::execute_single_arm_motion(
     feedback->progress = 0.5;
     feedback->status = static_cast<uint8_t>(MotionStatus::EXECUTING);
     
+    // 更新反馈中的关节角度
+    try {
+      std::vector<double> current_joints = current_move_group_ptr_->getCurrentJointValues();
+      if (arm_type == ArmType::LEFT) {
+        feedback->joints_left = current_joints;
+      } else if (arm_type == ArmType::RIGHT) {
+        feedback->joints_right = current_joints;
+      }
+    } catch (const std::exception& e) {
+      RCLCPP_WARN(this->get_logger(), "Failed to get current joint values for feedback: %s", e.what());
+    }
+    goal_handle->publish_feedback(feedback);
+    
     MoveGroupInterface::Plan plan;
     plan.trajectory_ = trajectory;
     current_move_group_ptr_->execute(plan);
@@ -323,12 +359,12 @@ bool HcDualArmMotionServer::execute_single_arm_motion(
   return false;
 }
 
-bool HcDualArmMotionServer::execute_dual_arm_motion(const shared_ptr<const HcDualArmAction::Goal> goal, shared_ptr<HcDualArmAction::Feedback> feedback, shared_ptr<HcDualArmAction::Result> result)
+bool HcDualArmMotionServer::execute_dual_arm_motion(const shared_ptr<const HcDualArmAction::Goal> goal, shared_ptr<HcDualArmAction::Feedback> feedback, shared_ptr<HcDualArmAction::Result> result, const std::shared_ptr<GoalHandleRobotMotion> goal_handle)
 {
   current_move_group_ptr_ = dual_arm_move_group_ptr_;
 
   current_move_group_ptr_->setMaxVelocityScalingFactor(goal->velocity_scaling);
-  current_move_group_ptr_->setMaxAccelerationScalingFactor(goal->velocity_scaling);
+  current_move_group_ptr_->setMaxAccelerationScalingFactor(goal->acceleration_scaling);
 
   if (goal->arm_motion_params.size() != 2)
   {
@@ -380,6 +416,20 @@ bool HcDualArmMotionServer::execute_dual_arm_motion(const shared_ptr<const HcDua
     // 4. 执行运动
     feedback->progress = 0.5;
     feedback->status = static_cast<uint8_t>(MotionStatus::EXECUTING);
+    
+    // 更新反馈中的关节角度
+    try {
+      std::vector<double> total_joints = current_move_group_ptr_->getCurrentJointValues();
+      size_t left_joint_count = left_arm_joint_names_.size();
+      if (total_joints.size() >= left_joint_count + right_arm_joint_names_.size()) {
+        feedback->joints_left.assign(total_joints.begin(), total_joints.begin() + left_joint_count);
+        feedback->joints_right.assign(total_joints.begin() + left_joint_count, total_joints.end());
+      }
+    } catch (const std::exception& e) {
+      RCLCPP_WARN(this->get_logger(), "Failed to get current joint values for feedback: %s", e.what());
+    }
+    goal_handle->publish_feedback(feedback);
+    
     current_move_group_ptr_->execute(plan);
     feedback->progress = 1.0;
     feedback->status = static_cast<uint8_t>(MotionStatus::DONE);
@@ -467,7 +517,17 @@ bool HcDualArmMotionServer::execute_dual_arm_motion(const shared_ptr<const HcDua
 
     feedback->progress = 0.5;
     feedback->status = static_cast<uint8_t>(MotionStatus::EXECUTING);
-
+    
+    // 更新反馈中的关节角度
+    try {
+      std::vector<double> left_joints = left_arm_move_group_ptr_->getCurrentJointValues();
+      std::vector<double> right_joints = right_arm_move_group_ptr_->getCurrentJointValues();
+      feedback->joints_left = left_joints;
+      feedback->joints_right = right_joints;
+    } catch (const std::exception& e) {
+      RCLCPP_WARN(this->get_logger(), "Failed to get current joint values for feedback: %s", e.what());
+    }
+    goal_handle->publish_feedback(feedback);
 
     // 方式2：精准同步（合并轨迹，需整体规划组，可选）
     moveit_msgs::msg::RobotTrajectory merged_traj = mergeTrajectories(left_plan.trajectory_, right_plan.trajectory_);
@@ -550,9 +610,17 @@ moveit_msgs::msg::RobotTrajectory HcDualArmMotionServer::mergeTrajectories(
     right_traj.joint_trajectory.joint_names.end()
   );
 
-  // 2. 对齐时间戳（取最长轨迹时长）
-  double left_duration = left_traj.joint_trajectory.points.back().time_from_start.sec;
-  double right_duration = right_traj.joint_trajectory.points.back().time_from_start.sec;
+  // 2. 对齐时间戳（取最长轨迹时长，使用完整时间戳包括纳秒）
+  if (left_traj.joint_trajectory.points.empty() || right_traj.joint_trajectory.points.empty()) {
+    RCLCPP_ERROR(this->get_logger(), "Empty trajectory in mergeTrajectories");
+    return merged_traj;
+  }
+  
+  // 计算完整时间戳（包括纳秒）
+  double left_duration = left_traj.joint_trajectory.points.back().time_from_start.sec + 
+                         left_traj.joint_trajectory.points.back().time_from_start.nanosec * 1e-9;
+  double right_duration = right_traj.joint_trajectory.points.back().time_from_start.sec + 
+                           right_traj.joint_trajectory.points.back().time_from_start.nanosec * 1e-9;
   double max_duration = std::max(left_duration, right_duration);
 
   // 3. 缩放轨迹时间（使左右臂轨迹时长一致）
@@ -560,30 +628,47 @@ moveit_msgs::msg::RobotTrajectory HcDualArmMotionServer::mergeTrajectories(
   moveit_msgs::msg::RobotTrajectory scaled_right = scaleTrajectoryTime(right_traj, max_duration);
 
   // 4. 合并轨迹点（逐点拼接关节值）
-  for (size_t i = 0; i < scaled_left.joint_trajectory.points.size(); ++i) {
+  // 检查轨迹点数是否一致，如果不一致则进行插值处理
+  size_t left_size = scaled_left.joint_trajectory.points.size();
+  size_t right_size = scaled_right.joint_trajectory.points.size();
+  size_t max_size = std::max(left_size, right_size);
+  
+  if (left_size != right_size) {
+    RCLCPP_WARN(this->get_logger(), "Trajectory point count mismatch: left=%zu, right=%zu, will interpolate", left_size, right_size);
+  }
+  
+  for (size_t i = 0; i < max_size; ++i) {
+    // 如果轨迹点数不同，使用最后一个点进行插值
+    size_t left_idx = (i < left_size) ? i : left_size - 1;
+    size_t right_idx = (i < right_size) ? i : right_size - 1;
+    
     trajectory_msgs::msg::JointTrajectoryPoint merged_point;
     // 左手臂关节值
-    merged_point.positions = scaled_left.joint_trajectory.points[i].positions;
-    merged_point.velocities = scaled_left.joint_trajectory.points[i].velocities;
-    merged_point.accelerations = scaled_left.joint_trajectory.points[i].accelerations;
+    merged_point.positions = scaled_left.joint_trajectory.points[left_idx].positions;
+    merged_point.velocities = scaled_left.joint_trajectory.points[left_idx].velocities;
+    merged_point.accelerations = scaled_left.joint_trajectory.points[left_idx].accelerations;
     // 右手臂关节值
     merged_point.positions.insert(
       merged_point.positions.end(),
-      scaled_right.joint_trajectory.points[i].positions.begin(),
-      scaled_right.joint_trajectory.points[i].positions.end()
+      scaled_right.joint_trajectory.points[right_idx].positions.begin(),
+      scaled_right.joint_trajectory.points[right_idx].positions.end()
     );
     merged_point.velocities.insert(
       merged_point.velocities.end(),
-      scaled_right.joint_trajectory.points[i].velocities.begin(),
-      scaled_right.joint_trajectory.points[i].velocities.end()
+      scaled_right.joint_trajectory.points[right_idx].velocities.begin(),
+      scaled_right.joint_trajectory.points[right_idx].velocities.end()
     );
     merged_point.accelerations.insert(
       merged_point.accelerations.end(),
-      scaled_right.joint_trajectory.points[i].accelerations.begin(),
-      scaled_right.joint_trajectory.points[i].accelerations.end()
+      scaled_right.joint_trajectory.points[right_idx].accelerations.begin(),
+      scaled_right.joint_trajectory.points[right_idx].accelerations.end()
     );
-    // 时间戳
-    merged_point.time_from_start = scaled_left.joint_trajectory.points[i].time_from_start;
+    // 时间戳（使用较长的轨迹时间）
+    if (i < left_size) {
+      merged_point.time_from_start = scaled_left.joint_trajectory.points[left_idx].time_from_start;
+    } else {
+      merged_point.time_from_start = scaled_right.joint_trajectory.points[right_idx].time_from_start;
+    }
     merged_traj.joint_trajectory.points.push_back(merged_point);
   }
 
@@ -594,11 +679,35 @@ moveit_msgs::msg::RobotTrajectory HcDualArmMotionServer::scaleTrajectoryTime(
   const moveit_msgs::msg::RobotTrajectory& traj, double target_duration)
 {
   moveit_msgs::msg::RobotTrajectory scaled_traj = traj;
-  double original_duration = traj.joint_trajectory.points.back().time_from_start.sec;
+  
+  if (traj.joint_trajectory.points.empty()) {
+    RCLCPP_ERROR(this->get_logger(), "Empty trajectory in scaleTrajectoryTime");
+    return scaled_traj;
+  }
+  
+  // 使用完整时间戳（包括纳秒）计算原始时长
+  double original_duration = traj.joint_trajectory.points.back().time_from_start.sec + 
+                              traj.joint_trajectory.points.back().time_from_start.nanosec * 1e-9;
+  
+  if (original_duration <= 0.0) {
+    RCLCPP_ERROR(this->get_logger(), "Invalid original duration: %f", original_duration);
+    return scaled_traj;
+  }
+  
   double scale = target_duration / original_duration;
 
   for (auto& point : scaled_traj.joint_trajectory.points) {
-    point.time_from_start = rclcpp::Duration::from_seconds(point.time_from_start.sec * scale);
+    // 计算完整时间戳（秒+纳秒）
+    double point_time = point.time_from_start.sec + point.time_from_start.nanosec * 1e-9;
+    double scaled_time = point_time * scale;
+    
+    // 设置缩放后的时间戳
+    int64_t sec = static_cast<int64_t>(scaled_time);
+    int64_t nanosec = static_cast<int64_t>((scaled_time - sec) * 1e9);
+    point.time_from_start.sec = sec;
+    point.time_from_start.nanosec = nanosec;
+    
+    // 缩放速度和加速度
     for (auto& vel : point.velocities) vel /= scale;
     for (auto& acc : point.accelerations) acc /= (scale * scale);
   }
@@ -614,10 +723,22 @@ void HcDualArmMotionServer::joy_callback(const sensor_msgs::msg::Joy::SharedPtr
   static bool last_btn2 = false;
   static bool last_btn3 = false;
 
+  // 检查按钮数组大小
+  if (msg->buttons.size() < 4) {
+    RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Joy message has insufficient buttons");
+    return;
+  }
+
   bool trigger_movej = (msg->buttons[0] == 1) && !last_btn0;
   bool trigger_movel = (msg->buttons[1] == 1) && !last_btn1;
   bool trigger_movel2 = (msg->buttons[3] == 1) && !last_btn3;
   bool trigger_cancel = (msg->buttons[2] == 1) && !last_btn2;
+  
+  // 如果正在运动，只允许取消操作
+  if (is_moving && (trigger_movej || trigger_movel || trigger_movel2)) {
+    RCLCPP_WARN(this->get_logger(), "Robot is moving, ignoring new motion command");
+    return;
+  }
 
 
 
@@ -641,18 +762,19 @@ void HcDualArmMotionServer::joy_callback(const sensor_msgs::msg::Joy::SharedPtr
     ArmMotionParams tempParam;
     tempParam.motion_type = static_cast<uint8_t>(MotionType::MOVEJ);
     tempParam.arm_id = 0;
-    tempParam.target_joints = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+    tempParam.target_joints = preset_movej_joints_;
 
     ArmMotionParams tempParam2;
     tempParam2.motion_type = static_cast<uint8_t>(MotionType::MOVEJ);
     tempParam2.arm_id = 1;
-    tempParam2.target_joints = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+    tempParam2.target_joints = preset_movej_joints_;
 
     auto goal_msg = HcDualArmAction::Goal();
     goal_msg.arm_type = static_cast<uint8_t>(ArmType::DUAL);
     goal_msg.arm_motion_params.push_back(tempParam);
     goal_msg.arm_motion_params.push_back(tempParam2);
     goal_msg.velocity_scaling = velocity_scaling_;
+    goal_msg.acceleration_scaling = max_acceleration_scaling_;
     goal_msg.cartesian_step = cartesian_step_;
 
     // 配置Goal发送选项（可选：添加回调监听结果）
@@ -688,25 +810,25 @@ void HcDualArmMotionServer::joy_callback(const sensor_msgs::msg::Joy::SharedPtr
   // 触发MoveL：通过Action Client发送Goal
   if (trigger_movel) {
     RCLCPP_INFO(this->get_logger(), "Joy trigger: execute MoveL");
-    ArmMotionParams tempParam;
-    tempParam.motion_type = static_cast<uint8_t>(MotionType::MOVEL);
-    tempParam.arm_id = 0;
-    current_move_group_ptr_ = dual_arm_move_group_ptr_;
-    auto current_pose = current_move_group_ptr_->getCurrentPose(end_effector_links_[0]).pose;
-    current_pose.position.z += 0.2;
-    tempParam.target_pose = current_pose;
-
-    ArmMotionParams tempParam2;
-    tempParam2.motion_type = static_cast<uint8_t>(MotionType::MOVEL);
-    tempParam2.arm_id = 1;
-    current_move_group_ptr_ = dual_arm_move_group_ptr_;
-    auto current_pose2 = current_move_group_ptr_->getCurrentPose(end_effector_links_[1]).pose;
-    current_pose2.position.z += 0.3;
-    tempParam2.target_pose = current_pose2;
     
     auto goal_msg = HcDualArmAction::Goal();
-    try 
-    {
+    try {
+      current_move_group_ptr_ = dual_arm_move_group_ptr_;
+      
+      ArmMotionParams tempParam;
+      tempParam.motion_type = static_cast<uint8_t>(MotionType::MOVEL);
+      tempParam.arm_id = 0;
+      auto current_pose = current_move_group_ptr_->getCurrentPose(end_effector_links_[0]).pose;
+      current_pose.position.z += 0.2;
+      tempParam.target_pose = current_pose;
+
+      ArmMotionParams tempParam2;
+      tempParam2.motion_type = static_cast<uint8_t>(MotionType::MOVEL);
+      tempParam2.arm_id = 1;
+      auto current_pose2 = current_move_group_ptr_->getCurrentPose(end_effector_links_[1]).pose;
+      current_pose2.position.z += 0.3;
+      tempParam2.target_pose = current_pose2;
+      
       goal_msg.arm_type = static_cast<uint8_t>(ArmType::DUAL);
       goal_msg.arm_motion_params.push_back(tempParam);
       goal_msg.arm_motion_params.push_back(tempParam2);
@@ -715,6 +837,7 @@ void HcDualArmMotionServer::joy_callback(const sensor_msgs::msg::Joy::SharedPtr
       return;
     }
     goal_msg.velocity_scaling = velocity_scaling_;
+    goal_msg.acceleration_scaling = max_acceleration_scaling_;
     goal_msg.cartesian_step = cartesian_step_;
 
     // 配置Goal发送选项
@@ -747,26 +870,26 @@ void HcDualArmMotionServer::joy_callback(const sensor_msgs::msg::Joy::SharedPtr
   }
 
   if (trigger_movel2) {
-    RCLCPP_INFO(this->get_logger(), "Joy trigger: execute MoveL");
-    ArmMotionParams tempParam;
-    tempParam.motion_type = static_cast<uint8_t>(MotionType::MOVEL);
-    tempParam.arm_id = 0;
-    current_move_group_ptr_ = dual_arm_move_group_ptr_;
-    auto current_pose = current_move_group_ptr_->getCurrentPose(end_effector_links_[0]).pose;
-    current_pose.position.z -= 0.2;
-    tempParam.target_pose = current_pose;
-
-    ArmMotionParams tempParam2;
-    tempParam2.motion_type = static_cast<uint8_t>(MotionType::MOVEL);
-    tempParam2.arm_id = 1;
-    current_move_group_ptr_ = dual_arm_move_group_ptr_;
-    auto current_pose2 = current_move_group_ptr_->getCurrentPose(end_effector_links_[1]).pose;
-    current_pose2.position.z -= 0.3;
-    tempParam2.target_pose = current_pose2;
+    RCLCPP_INFO(this->get_logger(), "Joy trigger: execute MoveL (down)");
     
     auto goal_msg = HcDualArmAction::Goal();
-    try 
-    {
+    try {
+      current_move_group_ptr_ = dual_arm_move_group_ptr_;
+      
+      ArmMotionParams tempParam;
+      tempParam.motion_type = static_cast<uint8_t>(MotionType::MOVEL);
+      tempParam.arm_id = 0;
+      auto current_pose = current_move_group_ptr_->getCurrentPose(end_effector_links_[0]).pose;
+      current_pose.position.z -= 0.2;
+      tempParam.target_pose = current_pose;
+
+      ArmMotionParams tempParam2;
+      tempParam2.motion_type = static_cast<uint8_t>(MotionType::MOVEL);
+      tempParam2.arm_id = 1;
+      auto current_pose2 = current_move_group_ptr_->getCurrentPose(end_effector_links_[1]).pose;
+      current_pose2.position.z -= 0.3;
+      tempParam2.target_pose = current_pose2;
+      
       goal_msg.arm_type = static_cast<uint8_t>(ArmType::DUAL);
       goal_msg.arm_motion_params.push_back(tempParam);
       goal_msg.arm_motion_params.push_back(tempParam2);
@@ -775,6 +898,7 @@ void HcDualArmMotionServer::joy_callback(const sensor_msgs::msg::Joy::SharedPtr
       return;
     }
     goal_msg.velocity_scaling = velocity_scaling_;
+    goal_msg.acceleration_scaling = max_acceleration_scaling_;
     goal_msg.cartesian_step = cartesian_step_;
 
     // 配置Goal发送选项