CMakeLists.txt

@@ -81,7 +81,6 @@ 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,8 +66,6 @@ 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,14 +552,7 @@ 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;
         
@@ -599,23 +592,7 @@ 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,7 +3,6 @@
  */
 #include "cmd.h"
 #include "bsp/time.h"
-#include <math.h>
 #include <stdint.h>
 #include <string.h>
    
@@ -17,7 +16,7 @@ static void CMD_RC_BuildChassisCmd(CMD_t *ctx) {
   CMD_RCModeMap_t *map = &ctx->config->rc_mode_map;
   
   /* 根据左拨杆位置选择模式 */
-  switch (ctx->input.rc.sw[0]) {
+  switch (ctx->input.rc.sw[1]) {
       case CMD_SW_UP:
           ctx->output.chassis.cmd.mode = map->sw_left_up;
           break;
@@ -257,92 +256,74 @@ 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_L (sw[0]) —— 整体使能:
-  *   UP   → 机械臂 relax
+   *   UP   → 失能（电机松弛/重力补偿）
-  *   MID  → 机械臂关节控制分组（由 SW_R 选择）
+   *   MID  → 使能，笛卡尔增量控制
-  *   DOWN → 仅控制夹爪
+   *   DOWN → 使能（保留备用）
    *
-  * SW_L=MID 且 SW_R=UP：
+   * 右拨杆 SW_R (sw[1]) —— 自由度分组:
-  *   LY → J2/J3 同向联动增量（底盘摇杆控制由底盘模块保持）
    *
-  * SW_L=MID 且 SW_R=MID：
+   *   UP  【位置模式】3轴平移 + 偏航
-  *   LX → J1, LY → J2, RY → J3
+   *         右摇杆X (RX) → X 平移
+   *         右摇杆Y (RY) → Y 平移
+   *         左摇杆Y (LY) → Z 平移（升降）
+   *         左摇杆X (LX) → Yaw 偏航旋转
    *
-  * SW_L=MID 且 SW_R=DOWN：
+   *   MID 【姿态模式】俯仰/横滚/偏航 + 升降
-  *   RX → J4, RY → J5, LY → J6
+   *         右摇杆X (RX) → Yaw  偏航旋转
+   *         右摇杆Y (RY) → Pitch 俯仰旋转
+   *         左摇杆X (LX) → Roll  横滚旋转
+   *         左摇杆Y (LY) → Z 平移（升降）
    *
-  * SW_L=DOWN（夹爪控制）：
+   *   DOWN → set_target_as_current（目标吸附当前实际位姿，消除累积漂移）
-  *   SW_R=UP   → 松开
-  *   SW_R=MID  → 保持当前
-  *   SW_R=DOWN → 夹紧
    */
   ctx->output.arm.cmd.set_target_as_current = false;
-  ctx->output.arm.cmd.gripper_toggle = false;
+  if (ctx->input.rc.sw[1] == CMD_SW_DOWN) {
-
+    ctx->output.arm.cmd.set_target_as_current = true;
-  /* 按控制类型分别处理增量输入 */
+  }
-  const float joint_scale = 1.0f; /* 关节角速度上限 (rad/s) */
-  const float dt = ctx->timer.dt;
 
   switch (ctx->input.rc.sw[0]) {
-    case CMD_SW_UP:
-      ctx->output.arm.cmd.enable = false;
-      break;
-
     case CMD_SW_MID:
+    case CMD_SW_DOWN:
       ctx->output.arm.cmd.enable = true;
-      ctx->output.arm.cmd.ctrl_type = ARM_CTRL_CUSTOM_JOINT;
+      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 dt = ctx->timer.dt;
 
   switch (ctx->input.rc.sw[1]) {
-        case CMD_SW_UP: {
+    case CMD_SW_UP:
-          const float d = ctx->input.rc.joy_left.y * joint_scale * dt;
+      /* 位置模式：3轴平移 + 偏航 */
-          ctx->output.arm.cmd.target_joints.q[1] += d;
+      ctx->output.arm.cmd.target_pose.x   += ctx->input.rc.joy_right.x * pos_scale * dt;
-          ctx->output.arm.cmd.target_joints.q[2] += d;
+      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;
       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_pose.yaw   += ctx->input.rc.joy_right.x * rot_scale * dt;
-          ctx->output.arm.cmd.target_joints.q[2] += ctx->input.rc.joy_right.y * joint_scale * dt;
+      ctx->output.arm.cmd.target_pose.pitch += ctx->input.rc.joy_right.y * rot_scale * dt;
-          break;
+      ctx->output.arm.cmd.target_pose.roll  += ctx->input.rc.joy_left.x  * rot_scale * dt;
-        case CMD_SW_DOWN:
+      ctx->output.arm.cmd.target_pose.z     += ctx->input.rc.joy_left.y  * pos_scale * dt;
-          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;
   }
-      break;
+  end:
-
-    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

@@ -1,192 +0,0 @@
-#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

@@ -1,44 +0,0 @@
-#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,7 +7,6 @@
 #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"
@@ -67,16 +66,6 @@ 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窗口直接观察和修改）
 // ============================================================================
@@ -165,26 +154,10 @@ static Arm6dof_Pose_t SyncTargetToFrame(uint8_t frame, const Arm6dof_Pose_t& cur
 }
 
 static void SyncCommandTargetFromCurrent() {
-  if (!robot_arm) {
+  Arm6dof_Pose_t sync = SyncTargetToFrame(control_frame, robot_arm->GetEndPose());
-    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;
   }
 }
 
@@ -214,12 +187,6 @@ 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:
@@ -252,65 +219,6 @@ 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) {
@@ -322,7 +230,6 @@ void Task_arm_main(void* argument) {
 
   BSP_CAN_Init();
   MOTOR_LZ_Init();  // 注册LZ电机ID解析器
-  Gripper_Init(MakeGripperRMParam());
   
   // 初始化运动学模型
   Arm6dof_Init(dh_params);
@@ -383,15 +290,7 @@ void Task_arm_main(void* argument) {
     // 更新机械臂状态（读取关节角度、FK、重力补偿计算）
     robot_arm->Update();
 
-    const osStatus_t arm_cmd_rx = osMessageQueueGet(task_runtime.msgq.arm.cmd, &arm_cmd, NULL, 0);
+    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);
 
@@ -407,13 +306,6 @@ 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) {
@@ -426,36 +318,90 @@ 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 };
 
-    RunTestStageControl();
+      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;
+        }
+      }
 
     // IK测试：以当前角度为初始猜测，对 target_pose 求逆运动学
     // 在调试器中将 ik_test_enable 置1启用，观察 ik_test_result 和 ik_test_ret
-    // if (ik_test_enable) {
+    if (ik_test_enable) {
-    //   ik_test_ret_analytical = robot_arm->InverseKinematicsAnalytical(&arm_cmd.target_pose,
+      ik_test_ret_analytical = robot_arm->InverseKinematicsAnalytical(&arm_cmd.target_pose,
-    //                                                                    &ik_test_result,
+                                                                       &ik_test_result,
-    //                                                                    &current_angles);
+                                                                       &current_angles);
-    //   ik_test_ret_numerical = 0;
+      ik_test_ret_numerical = 0;
-    //   ik_test_solver_dbg = -1;
+      ik_test_solver_dbg = -1;
 
-    //   // 解析解失败时回退到数值IK，便于调试观察并区分“无解”与“分支失败”。
+      // 解析解失败时回退到数值IK，便于调试观察并区分“无解”与“分支失败”。
-    //   if (ik_test_ret_analytical == 0) {
+      if (ik_test_ret_analytical == 0) {
-    //     ik_test_ret = 0;
+        ik_test_ret = 0;
-    //     ik_test_solver_dbg = 0;
+        ik_test_solver_dbg = 0;
-    //   } else {
+      } else {
-    //     ik_test_ret_numerical = Arm6dof_InverseKinematics(&arm_cmd.target_pose,
+        ik_test_ret_numerical = Arm6dof_InverseKinematics(&arm_cmd.target_pose,
-    //                                                       &current_angles,
+                                                          &current_angles,
-    //                                                       &ik_test_result,
+                                                          &ik_test_result,
-    //                                                       0.001f,
+                                                          0.001f,
-    //                                                       50);
+                                                          50);
-    //     if (ik_test_ret_numerical == 0) {
+        if (ik_test_ret_numerical == 0) {
-    //       ik_test_ret = 0;
+          ik_test_ret = 0;
-    //       ik_test_solver_dbg = 1;
+          ik_test_solver_dbg = 1;
-    //     } else {
+        } else {
-    //       ik_test_ret = -1;
+          ik_test_ret = -1;
-    //       ik_test_solver_dbg = -1;
+          ik_test_solver_dbg = -1;
-    //     }
+        }
-    //   }
+      }
-    // }
+    }
 
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }