From d5933612aa869257c2ccfcc1347f801849a74da7 Mon Sep 17 00:00:00 2001 From: xxxxm <2389287465@qq.com> Date: Fri, 20 Mar 2026 02:33:10 +0800 Subject: [PATCH] =?UTF-8?q?=E5=87=86=E5=A4=87=E5=8A=A0=E5=A4=B9=E7=88=AA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/arm_oop.hpp | 9 ++- User/task/arm_main.cpp | 158 +++++++++++++++++++--------------------- 2 files changed, 82 insertions(+), 85 deletions(-) diff --git a/User/module/arm_oop.hpp b/User/module/arm_oop.hpp index d55ae8b..752e5f1 100644 --- a/User/module/arm_oop.hpp +++ b/User/module/arm_oop.hpp @@ -552,7 +552,14 @@ public: int8_t Control() { // 更新时间 uint64_t now_us = BSP_TIME_Get_us(); - timer_.dt = (now_us - timer_.last_wakeup) / 1000000.0f; + 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; diff --git a/User/task/arm_main.cpp b/User/task/arm_main.cpp index d5d6fb7..1c0e9d6 100644 --- a/User/task/arm_main.cpp +++ b/User/task/arm_main.cpp @@ -203,6 +203,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: @@ -252,6 +258,48 @@ static void RunJointControlFromCmd() { 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) { @@ -363,94 +411,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 模式中，完全与原轨迹规划流程接轨 - // ================================================================================= - if (arm_cmd.ctrl_type == ARM_CTRL_CUSTOM_JOINT) { - RunJointControlFromCmd(); - } else { - 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, - ¤t_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, + // ¤t_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, - ¤t_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, + // ¤t_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); /* 运行结束，等待下一次唤醒 */ }