123321

CMakeLists.txt

@@ -81,6 +81,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/module/cmd/cmd_adapter.c
     User/module/cmd/cmd_behavior.c
     User/module/cmd/cmd_example.c
+    User/module/gripper_rm.cpp
     # User/task sources
     User/task/arm_main.cpp
     User/task/blink.c

User/component/arm_kinematics/arm6dof.h

@@ -66,6 +66,8 @@ typedef enum {
 typedef struct {
     bool                  enable;               /* 使能开关 */
     bool                  set_target_as_current; /* true: 将目标位姿同步为当前实际位姿 */
+    bool                  gripper_toggle;        /* 夹爪状态切换置位（单帧脉冲） */
+    bool                  gripper_close;         /* 夹爪目标状态：true=夹紧, false=松开 */
     Arm_CtrlType_t        ctrl_type;             /* 控制类型 */
     Arm6dof_Pose_t        target_pose;           /* 目标末端位姿（世界坐标系，供调试观察） */
     Arm6dof_JointAngles_t target_joints;         /* 目标六个关节角度（自定义控制器模式） */

User/module/arm_oop.hpp

@@ -552,7 +552,14 @@ public:
     int8_t Control() {
         // 更新时间
         uint64_t now_us = BSP_TIME_Get_us();
+        if (timer_.last_wakeup == 0 || now_us <= timer_.last_wakeup) {
+            timer_.dt = 0.001f;
+        } else {
             timer_.dt = (now_us - timer_.last_wakeup) / 1000000.0f;
+            // 异常调度抖动保护，避免单帧超大/超小 dt 造成控制突变。
+            if (timer_.dt < 0.0002f) timer_.dt = 0.0002f;
+            if (timer_.dt > 0.02f)   timer_.dt = 0.02f;
+        }
         timer_.last_wakeup = now_us;
         timer_.now = now_us / 1000000.0f;
         
@@ -592,7 +599,23 @@ public:
                 }
                 
                 if(control_.mode == ControlMode::JOINT_POSITION) {
-   
+                    // JOINT_POSITION 模式由上层写入目标角，这里做边界校验与状态同步
+                    for (size_t i = 0; i < JOINT_NUM; ++i) {
+                        if (!joints_[i]) {
+                            continue;
+                        }
+                        const float target = joints_[i]->GetTargetAngle();
+                        const auto& p = joints_[i]->GetParams();
+                        if (!std::isfinite(target) || target < p.qmin || target > p.qmax) {
+                            RelaxAllJoints();
+                            control_.state = MotionState::ERROR;
+                            return -1;
+                        }
+                        inverseKinematics_.angles.q[i] = target;
+                    }
+                    inverseKinematics_.valid = true;
+                    traj_.valid = true;
+                    traj_.active = false;
                 }
 
                 // GRAVITY_COMP 模式：保持当前位置 + 重力补偿

User/module/cmd/cmd.c

@@ -3,6 +3,7 @@
  */
 #include "cmd.h"
 #include "bsp/time.h"
+#include <math.h>
 #include <stdint.h>
 #include <string.h>
    
@@ -16,7 +17,7 @@ static void CMD_RC_BuildChassisCmd(CMD_t *ctx) {
   CMD_RCModeMap_t *map = &ctx->config->rc_mode_map;
   
   /* 根据左拨杆位置选择模式 */
-  switch (ctx->input.rc.sw[1]) {
+  switch (ctx->input.rc.sw[0]) {
       case CMD_SW_UP:
           ctx->output.chassis.cmd.mode = map->sw_left_up;
           break;
@@ -256,74 +257,92 @@ static void CMD_NUC_BuildShootCmd(CMD_t *ctx) {
 #if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_ARM
 static void CMD_RC_BuildArmCmd(CMD_t *ctx) {
   /*
-   * 遥控器控制机械臂末端位姿 —— 笛卡尔增量方案
+  * 遥控器机械臂行为组（按需求定制）：
   *
-   * 左拨杆 SW_L (sw[0]) —— 整体使能:
-   *   UP   → 失能（电机松弛/重力补偿）
-   *   MID  → 使能，笛卡尔增量控制
-   *   DOWN → 使能（保留备用）
+  * 左拨杆 SW_L：
+  *   UP   → 机械臂 relax
+  *   MID  → 机械臂关节控制分组（由 SW_R 选择）
+  *   DOWN → 仅控制夹爪
   *
-   * 右拨杆 SW_R (sw[1]) —— 自由度分组:
+  * SW_L=MID 且 SW_R=UP：
+  *   LY → J2/J3 同向联动增量（底盘摇杆控制由底盘模块保持）
   *
-   *   UP  【位置模式】3轴平移 + 偏航
-   *         右摇杆X (RX) → X 平移
-   *         右摇杆Y (RY) → Y 平移
-   *         左摇杆Y (LY) → Z 平移（升降）
-   *         左摇杆X (LX) → Yaw 偏航旋转
+  * SW_L=MID 且 SW_R=MID：
+  *   LX → J1, LY → J2, RY → J3
   *
-   *   MID 【姿态模式】俯仰/横滚/偏航 + 升降
-   *         右摇杆X (RX) → Yaw  偏航旋转
-   *         右摇杆Y (RY) → Pitch 俯仰旋转
-   *         左摇杆X (LX) → Roll  横滚旋转
-   *         左摇杆Y (LY) → Z 平移（升降）
+  * SW_L=MID 且 SW_R=DOWN：
+  *   RX → J4, RY → J5, LY → J6
   *
-   *   DOWN → set_target_as_current（目标吸附当前实际位姿，消除累积漂移）
+  * SW_L=DOWN（夹爪控制）：
+  *   SW_R=UP   → 松开
+  *   SW_R=MID  → 保持当前
+  *   SW_R=DOWN → 夹紧
    */
   ctx->output.arm.cmd.set_target_as_current = false;
-  if (ctx->input.rc.sw[1] == CMD_SW_DOWN) {
-    ctx->output.arm.cmd.set_target_as_current = true;
-  }
+  ctx->output.arm.cmd.gripper_toggle = false;
 
-  switch (ctx->input.rc.sw[0]) {
-    case CMD_SW_MID:
-    case CMD_SW_DOWN:
-      ctx->output.arm.cmd.enable = true;
-      break;
-    default:
-      ctx->output.arm.cmd.enable = false;
-      goto end;
-  }
-
-  /* 遥控器模式使用笛卡尔位姿累积控制 */
-  ctx->output.arm.cmd.ctrl_type = ARM_CTRL_REMOTE_CARTESIAN;
-
-  /* set_target_as_current 置位时不叠加摇杆增量，由上层负责同步位姿基准 */
-  if (ctx->output.arm.cmd.set_target_as_current) return;
-
-  /* 摇杆速度直接积分到 target_pose（坐标系语义由 arm_main 的 control_frame 决定） */
-  const float pos_scale = 0.3f;   /* 末端线速度上限 (m/s) */
-  const float rot_scale = 1.0f;   /* 末端角速度上限 (rad/s) */
+  /* 按控制类型分别处理增量输入 */
+  const float joint_scale = 1.0f; /* 关节角速度上限 (rad/s) */
   const float dt = ctx->timer.dt;
 
-  switch (ctx->input.rc.sw[1]) {
+  switch (ctx->input.rc.sw[0]) {
     case CMD_SW_UP:
-      /* 位置模式：3轴平移 + 偏航 */
-      ctx->output.arm.cmd.target_pose.x   += ctx->input.rc.joy_right.x * pos_scale * dt;
-      ctx->output.arm.cmd.target_pose.y   += ctx->input.rc.joy_right.y * pos_scale * dt;
-      ctx->output.arm.cmd.target_pose.z   += ctx->input.rc.joy_left.y  * pos_scale * dt;
-      ctx->output.arm.cmd.target_pose.yaw += ctx->input.rc.joy_left.x  * rot_scale * dt;
+      ctx->output.arm.cmd.enable = false;
       break;
+
     case CMD_SW_MID:
-      /* 姿态模式：俯仰 + 横滚 + 偏航 + 升降 */
-      ctx->output.arm.cmd.target_pose.yaw   += ctx->input.rc.joy_right.x * rot_scale * dt;
-      ctx->output.arm.cmd.target_pose.pitch += ctx->input.rc.joy_right.y * rot_scale * dt;
-      ctx->output.arm.cmd.target_pose.roll  += ctx->input.rc.joy_left.x  * rot_scale * dt;
-      ctx->output.arm.cmd.target_pose.z     += ctx->input.rc.joy_left.y  * pos_scale * dt;
+      ctx->output.arm.cmd.enable = true;
+      ctx->output.arm.cmd.ctrl_type = ARM_CTRL_CUSTOM_JOINT;
+
+      switch (ctx->input.rc.sw[1]) {
+        case CMD_SW_UP: {
+          const float d = ctx->input.rc.joy_left.y * joint_scale * dt;
+          ctx->output.arm.cmd.target_joints.q[1] += d;
+          ctx->output.arm.cmd.target_joints.q[2] += d;
+          break;
+        }
+        case CMD_SW_MID:
+          ctx->output.arm.cmd.target_joints.q[0] += ctx->input.rc.joy_left.x  * joint_scale * dt;
+          ctx->output.arm.cmd.target_joints.q[1] += ctx->input.rc.joy_left.y  * joint_scale * dt;
+          ctx->output.arm.cmd.target_joints.q[2] += ctx->input.rc.joy_right.y * joint_scale * dt;
+          break;
+        case CMD_SW_DOWN:
+          ctx->output.arm.cmd.target_joints.q[3] += ctx->input.rc.joy_right.x * joint_scale * dt;
+          ctx->output.arm.cmd.target_joints.q[4] += ctx->input.rc.joy_right.y * joint_scale * dt;
+          ctx->output.arm.cmd.target_joints.q[5] += ctx->input.rc.joy_left.y  * joint_scale * dt;
           break;
         default:
           break;
       }
-  end:
+      break;
+
+    case CMD_SW_DOWN: {
+      // 左拨杆下：只控制夹爪，机械臂保持使能但不注入新的关节目标
+      ctx->output.arm.cmd.enable = true;
+      ctx->output.arm.cmd.ctrl_type = ARM_CTRL_CUSTOM_JOINT;
+
+      bool has_cmd = false;
+      bool target_close = ctx->output.arm.cmd.gripper_close;
+      if (ctx->input.rc.sw[1] == CMD_SW_UP) {
+        has_cmd = true;
+        target_close = false;
+      } else if (ctx->input.rc.sw[1] == CMD_SW_DOWN) {
+        has_cmd = true;
+        target_close = true;
+      }
+
+      if (has_cmd && target_close != ctx->output.arm.cmd.gripper_close) {
+        ctx->output.arm.cmd.gripper_close = target_close;
+        ctx->output.arm.cmd.gripper_toggle = true;
+      }
+      break;
+    }
+
+    default:
+      ctx->output.arm.cmd.enable = false;
+      break;
+  }
+
   return;
 }
 #endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_ARM */

User/module/gripper_rm.cpp

@@ -0,0 +1,192 @@
+#include "module/gripper_rm.hpp"
+
+#include <cmath>
+
+#include "task/user_task.h"
+
+namespace arm {
+
+GripperState gripper_state_dbg = GripperState::OPEN_HOLD;
+bool gripper_target_close = false;
+float gripper_output_dbg = 0.0f;
+
+float gripper_out_open = -0.35f;
+float gripper_out_close_fast = 0.55f;
+float gripper_out_preload = 0.72f;
+float gripper_out_hold = 0.46f;
+float gripper_out_boost = 0.64f;
+
+float gripper_contact_current_th = 4200.0f;
+float gripper_contact_speed_th = 2.0f;
+float gripper_slip_speed_th = 1.6f;
+
+uint16_t gripper_contact_debounce_ms = 18;
+uint16_t gripper_preload_ms = 180;
+uint16_t gripper_boost_ms = 120;
+uint16_t gripper_open_timeout_ms = 1200;
+uint16_t gripper_close_timeout_ms = 1500;
+
+static MOTOR_RM_Param_t g_gripper_rm_param = {};
+static bool g_gripper_inited = false;
+
+static uint16_t GripperMsToTicks(uint16_t ms) {
+  const uint32_t freq = ARM_MAIN_FREQ;
+  uint32_t ticks = ((uint32_t)ms * freq + 999u) / 1000u;
+  if (ticks == 0u) {
+    ticks = 1u;
+  }
+  return (uint16_t)ticks;
+}
+
+static void GripperSetOutput(float out_norm) {
+  if (out_norm > 1.0f) out_norm = 1.0f;
+  if (out_norm < -1.0f) out_norm = -1.0f;
+  gripper_output_dbg = out_norm;
+  (void)MOTOR_RM_SetOutput(&g_gripper_rm_param, out_norm);
+}
+
+static void GripperStateMachineStep(bool target_close) {
+  static uint16_t state_ticks = 0;
+  static uint16_t contact_ticks = 0;
+  static uint16_t slip_ticks = 0;
+  static uint16_t boost_ticks = 0;
+  static bool prev_target_close = false;
+
+  const bool target_changed = (target_close != prev_target_close);
+  prev_target_close = target_close;
+
+  MOTOR_RM_t* gm = MOTOR_RM_GetMotor(&g_gripper_rm_param);
+  const bool online = (gm != nullptr) && gm->motor.header.online;
+  const float fb_speed = online ? gm->feedback.rotor_speed : 0.0f;
+  const float fb_current = online ? gm->feedback.torque_current : 0.0f;
+
+  const bool contact_now = fabsf(fb_current) > gripper_contact_current_th &&
+                           fabsf(fb_speed) < gripper_contact_speed_th;
+  const bool slip_now = fabsf(fb_speed) > gripper_slip_speed_th;
+
+  if (target_changed) {
+    state_ticks = 0;
+    contact_ticks = 0;
+    slip_ticks = 0;
+    boost_ticks = 0;
+    gripper_state_dbg = target_close ? GripperState::CLOSING : GripperState::OPENING;
+  }
+
+  if (!online) {
+    gripper_state_dbg = GripperState::FAULT;
+    GripperSetOutput(0.0f);
+    return;
+  }
+
+  ++state_ticks;
+
+  switch (gripper_state_dbg) {
+    case GripperState::OPENING:
+      GripperSetOutput(gripper_out_open);
+      if (target_close) {
+        gripper_state_dbg = GripperState::CLOSING;
+        state_ticks = 0;
+        contact_ticks = 0;
+      } else if (state_ticks >= GripperMsToTicks(gripper_open_timeout_ms)) {
+        gripper_state_dbg = GripperState::OPEN_HOLD;
+        state_ticks = 0;
+      }
+      break;
+
+    case GripperState::OPEN_HOLD:
+      GripperSetOutput(0.0f);
+      if (target_close) {
+        gripper_state_dbg = GripperState::CLOSING;
+        state_ticks = 0;
+        contact_ticks = 0;
+      }
+      break;
+
+    case GripperState::CLOSING:
+      GripperSetOutput(gripper_out_close_fast);
+      if (!target_close) {
+        gripper_state_dbg = GripperState::OPENING;
+        state_ticks = 0;
+        contact_ticks = 0;
+        break;
+      }
+
+      contact_ticks = contact_now ? (uint16_t)(contact_ticks + 1u) : 0u;
+      if (contact_ticks >= GripperMsToTicks(gripper_contact_debounce_ms)) {
+        gripper_state_dbg = GripperState::PRELOAD;
+        state_ticks = 0;
+      } else if (state_ticks >= GripperMsToTicks(gripper_close_timeout_ms)) {
+        // 稳夹优先：闭合超时后转入保持而不是松开
+        gripper_state_dbg = GripperState::CLOSE_HOLD;
+        state_ticks = 0;
+      }
+      break;
+
+    case GripperState::PRELOAD:
+      GripperSetOutput(gripper_out_preload);
+      if (!target_close) {
+        gripper_state_dbg = GripperState::OPENING;
+        state_ticks = 0;
+      } else if (state_ticks >= GripperMsToTicks(gripper_preload_ms)) {
+        gripper_state_dbg = GripperState::CLOSE_HOLD;
+        state_ticks = 0;
+      }
+      break;
+
+    case GripperState::CLOSE_HOLD:
+      if (!target_close) {
+        gripper_state_dbg = GripperState::OPENING;
+        state_ticks = 0;
+        slip_ticks = 0;
+        boost_ticks = 0;
+        break;
+      }
+
+      if (boost_ticks > 0) {
+        GripperSetOutput(gripper_out_boost);
+        --boost_ticks;
+      } else {
+        GripperSetOutput(gripper_out_hold);
+      }
+
+      // 检测到可能打滑时短时增压，提高稳夹能力
+      if (slip_now) {
+        ++slip_ticks;
+      } else {
+        slip_ticks = 0;
+      }
+      if (slip_ticks >= GripperMsToTicks(30)) {
+        boost_ticks = GripperMsToTicks(gripper_boost_ms);
+        slip_ticks = 0;
+      }
+      break;
+
+    case GripperState::FAULT:
+    default:
+      GripperSetOutput(0.0f);
+      if (online) {
+        gripper_state_dbg = target_close ? GripperState::CLOSING : GripperState::OPENING;
+        state_ticks = 0;
+      }
+      break;
+  }
+}
+
+int8_t Gripper_Init(const MOTOR_RM_Param_t& param) {
+  g_gripper_rm_param = param;
+  g_gripper_inited = (MOTOR_RM_Register(&g_gripper_rm_param) == DEVICE_OK);
+  return g_gripper_inited ? DEVICE_OK : DEVICE_ERR;
+}
+
+void Gripper_Step(bool target_close) {
+  if (!g_gripper_inited) {
+    return;
+  }
+
+  gripper_target_close = target_close;
+  (void)MOTOR_RM_Update(&g_gripper_rm_param);
+  GripperStateMachineStep(target_close);
+  (void)MOTOR_RM_Ctrl(&g_gripper_rm_param);
+}
+
+}  // namespace arm

User/module/gripper_rm.hpp

@@ -0,0 +1,44 @@
+#pragma once
+
+#include <stdint.h>
+
+#include "device/motor_rm.h"
+
+namespace arm {
+
+enum class GripperState : uint8_t {
+  OPENING = 0,
+  OPEN_HOLD,
+  CLOSING,
+  PRELOAD,
+  CLOSE_HOLD,
+  FAULT,
+};
+
+// 调试变量与可调参数，保留为全局便于Watch实时观察/修改。
+extern GripperState gripper_state_dbg;
+extern bool gripper_target_close;
+extern float gripper_output_dbg;
+
+// 夹爪输出（MOTOR_RM_SetOutput 归一化范围：[-1, 1]）
+extern float gripper_out_open;
+extern float gripper_out_close_fast;
+extern float gripper_out_preload;
+extern float gripper_out_hold;
+extern float gripper_out_boost;
+
+// 接触/防滑判据
+extern float gripper_contact_current_th;
+extern float gripper_contact_speed_th;
+extern float gripper_slip_speed_th;
+
+extern uint16_t gripper_contact_debounce_ms;
+extern uint16_t gripper_preload_ms;
+extern uint16_t gripper_boost_ms;
+extern uint16_t gripper_open_timeout_ms;
+extern uint16_t gripper_close_timeout_ms;
+
+int8_t Gripper_Init(const MOTOR_RM_Param_t& param);
+void Gripper_Step(bool target_close);
+
+}  // namespace arm

User/task/arm_main.cpp

@@ -7,6 +7,7 @@
 #include "task/user_task.h"
 
 #include "module/arm_oop.hpp"
+#include "module/gripper_rm.hpp"
 #include "module/motor_base.hpp"
 #include "module/joint.hpp"
 #include "bsp/can.h"
@@ -66,6 +67,16 @@ RoboticArm* robot_arm = nullptr;
 
 Arm_CMD_t arm_cmd;  // 当前机械臂控制命令（含 target_pose / set_target_as_current）
 
+static MOTOR_RM_Param_t MakeGripperRMParam() {
+  MOTOR_RM_Param_t param{};
+  param.can = BSP_CAN_1;
+  param.id = 0x201;
+  param.module = MOTOR_M3508;
+  param.reverse = false;
+  param.gear = true;
+  return param;
+}
+
 // ============================================================================
 // 测试用调试变量（可在调试器Watch窗口直接观察和修改）
 // ============================================================================
@@ -154,10 +165,26 @@ static Arm6dof_Pose_t SyncTargetToFrame(uint8_t frame, const Arm6dof_Pose_t& cur
 }
 
 static void SyncCommandTargetFromCurrent() {
-  Arm6dof_Pose_t sync = SyncTargetToFrame(control_frame, robot_arm->GetEndPose());
+  if (!robot_arm) {
+    return;
+  }
+
+  const Arm6dof_Pose_t sync = SyncTargetToFrame(control_frame, robot_arm->GetEndPose());
+  Arm6dof_JointAngles_t sync_joints = arm_cmd.target_joints;
+  for (int i = 0; i < 6; ++i) {
+    if (joints[i]) {
+      sync_joints.q[i] = joints[i]->GetCurrentAngle();
+    }
+  }
+
   arm_cmd.target_pose = sync;
+  arm_cmd.target_joints = sync_joints;
+
+  // 同步上游CMD中的目标值，避免下一帧被旧目标覆盖导致跳变。
+  // 注意：这里只同步目标量，不修改夹爪toggle等一次性控制位。
   if (cmd_for_arm) {
     cmd_for_arm->target_pose = sync;
+    cmd_for_arm->target_joints = sync_joints;
   }
 }
 
@@ -187,6 +214,12 @@ static void RelaxAllMotors() {
   }
 }
 
+static void ApplyJointTorques(const float torques[6]) {
+  for (int i = 0; i < 6; ++i) {
+    joints[i]->TorqueControl(torques[i]);
+  }
+}
+
 static void MoveBySelectedFrame(const Arm6dof_Pose_t& target) {
   switch (control_frame) {
     case 2:
@@ -219,6 +252,65 @@ static void RunCartesianControl(ControlMode mode) {
   traj_progress_dbg = robot_arm->GetTrajProgress();
 }
 
+static void RunJointControlFromCmd() {
+  float target_joints[6];
+  for (int i = 0; i < 6; ++i) {
+    target_joints[i] = arm_cmd.target_joints.q[i];
+  }
+
+  robot_arm->SetMode(ControlMode::JOINT_POSITION);
+  if (robot_arm->MoveJoint(target_joints) != 0) {
+    // 超限时回收错误并把命令吸附到当前角，避免持续触发错误
+    robot_arm->ResetError();
+    SyncCommandTargetFromCurrent();
+  }
+  robot_arm->Control();
+  ik_from_fk_result = robot_arm->GetIkAngles();
+  traj_progress_dbg = 0.0f;
+}
+
+static void RunTestStageControl() {
+  switch (test_stage) {
+    case 0: {
+      // 检测电机正电流旋转方向与urdf规定是否一致
+      static float rotationDirectionCheck[6] = {0.0f};
+      ApplyJointTorques(rotationDirectionCheck);
+      break;
+    }
+
+    case 1:
+      // 阶段1：仅计算，不输出。观察 gravity_torques_dbg[0~5] 是否量级合理
+      RelaxAllMotors();
+      break;
+
+    case 2:
+      // 阶段2：输出重力补偿力矩，验证各轴方向与量级
+      ApplyJointTorques(gravity_torques_dbg);
+      break;
+
+    case 4:
+      // 笛卡尔空间轨迹规划控制：基于数值逆解(雅可比)
+      RunCartesianControl(ControlMode::CARTESIAN_POSITION);
+      break;
+
+    case 5:
+      // 2.5D 解析降维控制（支持关节/笛卡尔两种上层指令）
+      if (arm_cmd.ctrl_type == ARM_CTRL_CUSTOM_JOINT) {
+        RunJointControlFromCmd();
+      } else {
+        RunCartesianControl(ControlMode::CARTESIAN_ANALYTICAL);
+      }
+      break;
+
+    case 3:
+    default:
+      // 默认：全部六轴 GRAVITY_COMP 模式（位置保持 + 重力补偿前馈）
+      robot_arm->SetMode(ControlMode::GRAVITY_COMP);
+      robot_arm->Control();
+      break;
+  }
+}
+
 extern "C" {
 
 void Task_arm_main(void* argument) {
@@ -230,6 +322,7 @@ void Task_arm_main(void* argument) {
 
   BSP_CAN_Init();
   MOTOR_LZ_Init();  // 注册LZ电机ID解析器
+  Gripper_Init(MakeGripperRMParam());
   
   // 初始化运动学模型
   Arm6dof_Init(dh_params);
@@ -290,7 +383,15 @@ void Task_arm_main(void* argument) {
     // 更新机械臂状态（读取关节角度、FK、重力补偿计算）
     robot_arm->Update();
 
-    osMessageQueueGet(task_runtime.msgq.arm.cmd, &arm_cmd, NULL, 0);  
+    const osStatus_t arm_cmd_rx = osMessageQueueGet(task_runtime.msgq.arm.cmd, &arm_cmd, NULL, 0);
+    if (arm_cmd_rx != osOK) {
+      // 未收到新命令时清理单帧脉冲，避免重复消费上一帧toggle类信号
+      arm_cmd.gripper_toggle = false;
+      arm_cmd.set_target_as_current = false;
+    }
+
+    // 夹爪两态控制：直接跟随 gripper_close，内部状态机负责稳夹优先策略
+    Gripper_Step(arm_cmd.gripper_close);
     
     robot_arm->Enable(arm_cmd.enable);
 
@@ -306,6 +407,13 @@ void Task_arm_main(void* argument) {
       SyncCommandTargetFromCurrent();
     }
 
+    // 控制类型切换沿：同步目标，避免 pose/joint 切换时命令继承旧目标导致突变
+    static Arm_CtrlType_t last_ctrl_type = ARM_CTRL_REMOTE_CARTESIAN;
+    if (arm_cmd.enable && arm_cmd.ctrl_type != last_ctrl_type) {
+      SyncCommandTargetFromCurrent();
+    }
+    last_ctrl_type = arm_cmd.ctrl_type;
+
     
     // 每帧同步调试观察变量
     for (int i = 0; i < 6; ++i) {
@@ -318,90 +426,36 @@ void Task_arm_main(void* argument) {
     end_pose_mm_dbg = { ep.x * 1000.0f, ep.y * 1000.0f, ep.z * 1000.0f,
                         ep.roll, ep.pitch, ep.yaw };
 
-      switch (test_stage) {
-        case 0:
-          //检测电机正电流旋转方向与urdf规定是否一致
-          static float rotationDirectionCheck[6] = {0.0f};
-          joints[0]->TorqueControl(rotationDirectionCheck[0]);  
-          joints[1]->TorqueControl(rotationDirectionCheck[1]);  
-          joints[2]->TorqueControl(rotationDirectionCheck[2]);  
-          joints[3]->TorqueControl(rotationDirectionCheck[3]); 
-          joints[4]->TorqueControl(rotationDirectionCheck[4]); 
-          joints[5]->TorqueControl(rotationDirectionCheck[5]); 
-          break;
-
-        case 1:
-          // 阶段1：仅计算，不输出。观察 gravity_torques_dbg[0~5] 是否量级合理
-          RelaxAllMotors();
-          break;
-
-        case 2:
-          // 阶段1：测试单个关节力矩输出（用于验证力矩计算）
-          // 修改下面的索引来测试不同关节：j2=1, j3=2, j5=4
-          // motors[0]->Relax();
-          // motors[1]->Relax();
-          // motors[2]->Relax();
-          // motors[3]->Relax();
-          // motors[4]->Relax();
-          // motors[5]->Relax();
-          joints[1]->TorqueControl(gravity_torques_dbg[1]);  // j2大臂
-          joints[2]->TorqueControl(gravity_torques_dbg[2]);  // j3小臂
-          joints[4]->TorqueControl(gravity_torques_dbg[4]);  // j5腕部
-
-          joints[0]->TorqueControl(gravity_torques_dbg[0]); 
-          joints[3]->TorqueControl(gravity_torques_dbg[3]); 
-          joints[5]->TorqueControl(gravity_torques_dbg[5]); 
-          break;
-        case 3:
-        default:
-          // 阶段4：全部六轴 GRAVITY_COMP 模式（位置保持 + 重力补偿前馈）
-          robot_arm->SetMode(ControlMode::GRAVITY_COMP);
-          robot_arm->Control();
-          break;
-
-        case 4:
-          // 笛卡尔空间轨迹规划控制：基于数值逆解(雅可比)
-          RunCartesianControl(ControlMode::CARTESIAN_POSITION);
-          break;
-
-        case 5: {
-          // =================================================================================
-          // ★ 2.5D 解析降维控制算法 ★ 
-          // 已经集成到底层 arm_oop 的 CARTESIAN_ANALYTICAL 模式中，完全与原轨迹规划流程接轨
-          // =================================================================================
-          RunCartesianControl(ControlMode::CARTESIAN_ANALYTICAL); // 启用降维解析数学模式
-          break;
-        }
-      }
+    RunTestStageControl();
 
     // IK测试：以当前角度为初始猜测，对 target_pose 求逆运动学
     // 在调试器中将 ik_test_enable 置1启用，观察 ik_test_result 和 ik_test_ret
-    if (ik_test_enable) {
-      ik_test_ret_analytical = robot_arm->InverseKinematicsAnalytical(&arm_cmd.target_pose,
-                                                                       &ik_test_result,
-                                                                       &current_angles);
-      ik_test_ret_numerical = 0;
-      ik_test_solver_dbg = -1;
+    // if (ik_test_enable) {
+    //   ik_test_ret_analytical = robot_arm->InverseKinematicsAnalytical(&arm_cmd.target_pose,
+    //                                                                    &ik_test_result,
+    //                                                                    &current_angles);
+    //   ik_test_ret_numerical = 0;
+    //   ik_test_solver_dbg = -1;
 
-      // 解析解失败时回退到数值IK，便于调试观察并区分“无解”与“分支失败”。
-      if (ik_test_ret_analytical == 0) {
-        ik_test_ret = 0;
-        ik_test_solver_dbg = 0;
-      } else {
-        ik_test_ret_numerical = Arm6dof_InverseKinematics(&arm_cmd.target_pose,
-                                                          &current_angles,
-                                                          &ik_test_result,
-                                                          0.001f,
-                                                          50);
-        if (ik_test_ret_numerical == 0) {
-          ik_test_ret = 0;
-          ik_test_solver_dbg = 1;
-        } else {
-          ik_test_ret = -1;
-          ik_test_solver_dbg = -1;
-        }
-      }
-    }
+    //   // 解析解失败时回退到数值IK，便于调试观察并区分“无解”与“分支失败”。
+    //   if (ik_test_ret_analytical == 0) {
+    //     ik_test_ret = 0;
+    //     ik_test_solver_dbg = 0;
+    //   } else {
+    //     ik_test_ret_numerical = Arm6dof_InverseKinematics(&arm_cmd.target_pose,
+    //                                                       &current_angles,
+    //                                                       &ik_test_result,
+    //                                                       0.001f,
+    //                                                       50);
+    //     if (ik_test_ret_numerical == 0) {
+    //       ik_test_ret = 0;
+    //       ik_test_solver_dbg = 1;
+    //     } else {
+    //       ik_test_ret = -1;
+    //       ik_test_solver_dbg = -1;
+    //     }
+    //   }
+    // }
 
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }