准备加夹爪
This commit is contained in:
parent
edc200bdf0
commit
d5933612aa
@ -552,7 +552,14 @@ public:
|
|||||||
int8_t Control() {
|
int8_t Control() {
|
||||||
// 更新时间
|
// 更新时间
|
||||||
uint64_t now_us = BSP_TIME_Get_us();
|
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;
|
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_.last_wakeup = now_us;
|
||||||
timer_.now = now_us / 1000000.0f;
|
timer_.now = now_us / 1000000.0f;
|
||||||
|
|
||||||
|
|||||||
@ -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) {
|
static void MoveBySelectedFrame(const Arm6dof_Pose_t& target) {
|
||||||
switch (control_frame) {
|
switch (control_frame) {
|
||||||
case 2:
|
case 2:
|
||||||
@ -252,6 +258,48 @@ static void RunJointControlFromCmd() {
|
|||||||
traj_progress_dbg = 0.0f;
|
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" {
|
extern "C" {
|
||||||
|
|
||||||
void Task_arm_main(void* argument) {
|
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,
|
end_pose_mm_dbg = { ep.x * 1000.0f, ep.y * 1000.0f, ep.z * 1000.0f,
|
||||||
ep.roll, ep.pitch, ep.yaw };
|
ep.roll, ep.pitch, ep.yaw };
|
||||||
|
|
||||||
switch (test_stage) {
|
RunTestStageControl();
|
||||||
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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// IK测试：以当前角度为初始猜测，对 target_pose 求逆运动学
|
// IK测试：以当前角度为初始猜测，对 target_pose 求逆运动学
|
||||||
// 在调试器中将 ik_test_enable 置1启用，观察 ik_test_result 和 ik_test_ret
|
// 在调试器中将 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,
|
||||||
¤t_angles);
|
// ¤t_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,
|
||||||
¤t_angles,
|
// ¤t_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); /* 运行结束，等待下一次唤醒 */
|
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user