xm/arm
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

准备加夹爪

Browse Source
This commit is contained in:
xxxxm 2026-03-20 02:33:10 +08:00
parent edc200bdf0
commit d5933612aa
2 changed files with 82 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
User/module/arm_oop.hpp
View File

@ -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;

158
User/task/arm_main.cpp
View File

@ -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,
&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); /* 运行结束，等待下一次唤醒 */
}