添加pose/joint控制

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

@@ -592,7 +592,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>
    
@@ -256,16 +257,17 @@ 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) {
   /*
-   * 遥控器控制机械臂末端位姿 —— 笛卡尔增量方案
+  * 遥控器控制机械臂：拨杆选择 pose 或 joint
    *
-   * 左拨杆 SW_L (sw[0]) —— 整体使能:
+  * 左拨杆 SW_L (sw[0]) —— 使能与控制类型:
    *   UP   → 失能（电机松弛/重力补偿）
-   *   MID  → 使能，笛卡尔增量控制
+  *   MID  → 使能，POSE 笛卡尔增量控制
-   *   DOWN → 使能（保留备用）
+  *   DOWN → 使能，JOINT 关节角增量控制
    *
-   * 右拨杆 SW_R (sw[1]) —— 自由度分组:
+  * 右拨杆 SW_R (sw[1]) —— 子模式/分组 + 夹爪状态切换:
    *
-   *   UP  【位置模式】3轴平移 + 偏航
+  * POSE(ctrl_type=ARM_CTRL_REMOTE_CARTESIAN):
+  *   UP  【位置模式】3轴平移 + 偏航
    *         右摇杆X (RX) → X 平移
    *         右摇杆Y (RY) → Y 平移
    *         左摇杆Y (LY) → Z 平移（升降）
@@ -277,51 +279,79 @@ static void CMD_RC_BuildArmCmd(CMD_t *ctx) {
    *         左摇杆X (LX) → Roll  横滚旋转
    *         左摇杆Y (LY) → Z 平移（升降）
    *
-   *   DOWN → set_target_as_current（目标吸附当前实际位姿，消除累积漂移）
+   * JOINT(ctrl_type=ARM_CTRL_CUSTOM_JOINT):
+   *   UP  → J1/J2/J3 由 RX/RY/LY 增量控制
+   *   MID → J4/J5/J6 由 RX/RY/LY 增量控制
+   *
+   * 通用:
+   *   SW_R 进入 DOWN 的边沿触发一次 gripper_toggle，
+   *   并翻转 gripper_close（true=夹紧, false=松开）。
    */
   ctx->output.arm.cmd.set_target_as_current = false;
-  if (ctx->input.rc.sw[1] == CMD_SW_DOWN) {
+  ctx->output.arm.cmd.gripper_toggle = false;
-    ctx->output.arm.cmd.set_target_as_current = true;
+
+  /* 右拨杆 DOWN 边沿：夹爪状态切换置位（仅输出控制量，不在此执行夹爪动作） */
+  if (ctx->input.rc.sw[1] == CMD_SW_DOWN && ctx->last_input.rc.sw[1] != CMD_SW_DOWN) {
+    ctx->output.arm.cmd.gripper_close = !ctx->output.arm.cmd.gripper_close;
+    ctx->output.arm.cmd.gripper_toggle = true;
   }
 
   switch (ctx->input.rc.sw[0]) {
     case CMD_SW_MID:
+      ctx->output.arm.cmd.enable = true;
+      ctx->output.arm.cmd.ctrl_type = ARM_CTRL_REMOTE_CARTESIAN;
+      break;
     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 joint_scale = 1.0f; /* 关节角速度上限 (rad/s) */
   const float dt = ctx->timer.dt;
 
-  switch (ctx->input.rc.sw[1]) {
+  if (ctx->output.arm.cmd.ctrl_type == ARM_CTRL_REMOTE_CARTESIAN) {
-    case CMD_SW_UP:
+    switch (ctx->input.rc.sw[1]) {
-      /* 位置模式：3轴平移 + 偏航 */
+      case CMD_SW_UP:
-      ctx->output.arm.cmd.target_pose.x   += ctx->input.rc.joy_right.x * pos_scale * dt;
+        /* 位置模式：3轴平移 + 偏航 */
-      ctx->output.arm.cmd.target_pose.y   += ctx->input.rc.joy_right.y * pos_scale * dt;
+        ctx->output.arm.cmd.target_pose.x   += ctx->input.rc.joy_right.x * 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.y   += ctx->input.rc.joy_right.y * pos_scale * dt;
-      ctx->output.arm.cmd.target_pose.yaw += 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;
-      break;
+        ctx->output.arm.cmd.target_pose.yaw += ctx->input.rc.joy_left.x  * rot_scale * dt;
-    case CMD_SW_MID:
+        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.yaw   += ctx->input.rc.joy_right.x * 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.pitch += ctx->input.rc.joy_right.y * rot_scale * dt;
-      ctx->output.arm.cmd.target_pose.z     += ctx->input.rc.joy_left.y  * pos_scale * dt;
+        ctx->output.arm.cmd.target_pose.roll  += ctx->input.rc.joy_left.x  * rot_scale * dt;
-      break;
+        ctx->output.arm.cmd.target_pose.z     += ctx->input.rc.joy_left.y  * pos_scale * dt;
-    default:
+        break;
-      break;
+      default:
+        /* SW_R=DOWN 预留给夹爪切换，不做机械臂增量 */
+        break;
+    }
+  } else {
+    switch (ctx->input.rc.sw[1]) {
+      case CMD_SW_UP:
+        ctx->output.arm.cmd.target_joints.q[0] += ctx->input.rc.joy_right.x * joint_scale * dt;
+        ctx->output.arm.cmd.target_joints.q[1] += ctx->input.rc.joy_right.y * joint_scale * dt;
+        ctx->output.arm.cmd.target_joints.q[2] += ctx->input.rc.joy_left.y  * joint_scale * dt;
+        break;
+      case CMD_SW_MID:
+        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:
+        /* SW_R=DOWN 预留给夹爪切换，不做机械臂增量 */
+        break;
+    }
   }
   end:
   return;

User/task/arm_main.cpp

@@ -154,10 +154,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;
   }
 }
 
@@ -219,6 +235,23 @@ 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;
+}
+
 extern "C" {
 
 void Task_arm_main(void* argument) {
@@ -290,7 +323,12 @@ 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;
+    }
     
     robot_arm->Enable(arm_cmd.enable);
 
@@ -306,6 +344,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) {
@@ -369,7 +414,11 @@ void Task_arm_main(void* argument) {
           // ★ 2.5D 解析降维控制算法 ★ 
           // 已经集成到底层 arm_oop 的 CARTESIAN_ANALYTICAL 模式中，完全与原轨迹规划流程接轨
           // =================================================================================
-          RunCartesianControl(ControlMode::CARTESIAN_ANALYTICAL); // 启用降维解析数学模式
+          if (arm_cmd.ctrl_type == ARM_CTRL_CUSTOM_JOINT) {
+            RunJointControlFromCmd();
+          } else {
+            RunCartesianControl(ControlMode::CARTESIAN_ANALYTICAL); // 启用降维解析数学模式
+          }
           break;
         }
       }