From edc200bdf04bf40c93a80fa8332b7e96ffb48a9f Mon Sep 17 00:00:00 2001 From: xxxxm <2389287465@qq.com> Date: Fri, 20 Mar 2026 01:11:33 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0pose/joint=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/component/arm_kinematics/arm6dof.h | 2 + User/module/arm_oop.hpp | 18 ++++- User/module/cmd/cmd.c | 96 ++++++++++++++++--------- User/task/arm_main.cpp | 55 +++++++++++++- 4 files changed, 134 insertions(+), 37 deletions(-) diff --git a/User/component/arm_kinematics/arm6dof.h b/User/component/arm_kinematics/arm6dof.h index d81f469..7a60f05 100644 --- a/User/component/arm_kinematics/arm6dof.h +++ b/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; /* 目标六个关节角度（自定义控制器模式） */ diff --git a/User/module/arm_oop.hpp b/User/module/arm_oop.hpp index 06b4262..d55ae8b 100644 --- a/User/module/arm_oop.hpp +++ b/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 模式：保持当前位置 + 重力补偿 diff --git a/User/module/cmd/cmd.c b/User/module/cmd/cmd.c index 0e5b980..f2541d0 100644 --- a/User/module/cmd/cmd.c +++ b/User/module/cmd/cmd.c @@ -3,6 +3,7 @@ */ #include "cmd.h" #include "bsp/time.h" +#include #include #include @@ -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 → 使能，笛卡尔增量控制 - * DOWN → 使能（保留备用） + * MID → 使能，POSE 笛卡尔增量控制 + * 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.set_target_as_current = true; + ctx->output.arm.cmd.gripper_toggle = false; + + /* 右拨杆 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]) { - 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; - 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; - break; - default: - break; + if (ctx->output.arm.cmd.ctrl_type == ARM_CTRL_REMOTE_CARTESIAN) { + switch (ctx->input.rc.sw[1]) { + 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; + 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; + 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; diff --git a/User/task/arm_main.cpp b/User/task/arm_main.cpp index 76b471d..d5d6fb7 100644 --- a/User/task/arm_main.cpp +++ b/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; } }