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-10 00:49:52 +08:00
parent 7c078bdb54
commit e0e6b33791
5 changed files with 195 additions and 188 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
User/component/toolbox/robotics.h
View File

@ -211,12 +211,11 @@ class Serial_Link {
return J_; return J_;
} }
// inverse kinematic, numerical solution(Newton method) // inverse kinematic, Levenberg-Marquardt method
// param[in] T: homogeneous transformation matrix of end effector // param[in] Td: target homogeneous transformation matrix of end effector
// param[in] q: initial joint variable vector(q0) for Newton method's // param[in] q: initial joint variable vector(q0) for iteration
// iteration // param[in] tol: tolerance of error (norm of twist error vector)
// param[in] tol: tolerance of error (norm(error of twist vector)) // param[in] max_iter: maximum iterations, default 50
// param[in] max_iter: maximum iterations, default 30
// param[out] q: joint variable vector // param[out] q: joint variable vector
Matrixf<_n, 1> ikine(Matrixf<4, 4> Td, Matrixf<_n, 1> ikine(Matrixf<4, 4> Td,
Matrixf<_n, 1> q = matrixf::zeros<_n, 1>(), Matrixf<_n, 1> q = matrixf::zeros<_n, 1>(),
@ -225,57 +224,65 @@ class Serial_Link {
Matrixf<3, 1> pe, we; Matrixf<3, 1> pe, we;
Matrixf<6, 1> err, new_err; Matrixf<6, 1> err, new_err;
Matrixf<_n, 1> dq; Matrixf<_n, 1> dq;
float step = 1; float lambda = 1.0f; // LM 阻尼因子
for (int i = 0; i < max_iter; i++) { constexpr float lambda_up = 10.0f; // 发散时增大
constexpr float lambda_down = 0.1f; // 收敛时减小
constexpr float lambda_max = 1e6f;
constexpr float lambda_min = 1e-6f;
for (int iter = 0; iter < max_iter; iter++) {
T = fkine(q); T = fkine(q);
pe = t2p(Td) - t2p(T); pe = t2p(Td) - t2p(T);
// angvec(Td*T^-1), transform angular vector(T->Td) in world coordinate
we = t2twist(Td * invT(T)).block<3, 1>(3, 0); we = t2twist(Td * invT(T)).block<3, 1>(3, 0);
for (int i = 0; i < 3; i++) { for (int k = 0; k < 3; k++) {
err[i][0] = pe[i][0]; err[k][0] = pe[k][0];
err[i + 3][0] = we[i][0]; err[k + 3][0] = we[k][0];
} }
if (err.norm() < tol) float err_norm = err.norm();
if (err_norm < tol)
return q; return q;
// adjust iteration step
Matrixf<6, _n> J = jacob(q); Matrixf<6, _n> J = jacob(q);
for (int j = 0; j < 5; j++) { Matrixf<_n, _n> JtJ = J.trans() * J;
dq = matrixf::inv(J.trans() * J) * (J.trans() * err) * step; Matrixf<_n, 1> Jte = J.trans() * err;
if (dq[0][0] == INFINITY) // J'*J singular
{ // LM 步：dq = (J'J + λI)^{-1} J'e
dq = matrixf::inv(J.trans() * J + 0.1f * matrixf::eye<_n, _n>()) * dq = matrixf::inv(JtJ + lambda * matrixf::eye<_n, _n>()) * Jte;
J.trans() * err * step;
// SVD<6, _n> JTJ_svd(J.trans() * J); // 检查求逆是否成功
// dq = JTJ_svd.solve(err) * step * 5e-2f; if (dq[0][0] == INFINITY || dq[0][0] != dq[0][0]) {
q += dq; lambda *= lambda_up;
if (lambda > lambda_max) return q;
continue;
}
// 评估新误差
Matrixf<_n, 1> q_new = q + dq;
// 关节角折叠到有效区间
for (int i = 0; i < _n; i++) { for (int i = 0; i < _n; i++) {
if (links_[i].type() == R) if (links_[i].type() == R)
q[i][0] = math::loopLimit(q[i][0], -PI, PI); q_new[i][0] = math::loopLimit(q_new[i][0], links_[i].qmin_, links_[i].qmax_);
} }
break; T = fkine(q_new);
}
T = fkine(q + dq);
pe = t2p(Td) - t2p(T); pe = t2p(Td) - t2p(T);
we = t2twist(Td * invT(T)).block<3, 1>(3, 0); we = t2twist(Td * invT(T)).block<3, 1>(3, 0);
for (int i = 0; i < 3; i++) { for (int k = 0; k < 3; k++) {
new_err[i][0] = pe[i][0]; new_err[k][0] = pe[k][0];
new_err[i + 3][0] = we[i][0]; new_err[k + 3][0] = we[k][0];
} }
if (new_err.norm() < err.norm()) { float new_err_norm = new_err.norm();
q += dq;
for (int i = 0; i < _n; i++) { if (new_err_norm < err_norm) {
if (links_[i].type() == robotics::Joint_Type_e::R) { // 误差减小：接受步长，减小阻尼（趋近 Gauss-Newton，加速收敛）
q[i][0] = math::loopLimit(q[i][0], -PI, PI); q = q_new;
} lambda *= lambda_down;
} if (lambda < lambda_min) lambda = lambda_min;
break;
} else { } else {
step /= 2.0f; // 误差增大：拒绝步长，增大阻尼（趋近梯度下降，缩小步幅）
lambda *= lambda_up;
if (lambda > lambda_max) return q; // 阻尼过大，无法继续
} }
} }
if (step < 1e-3f)
return q;
}
return q; return q;
} }

113
User/module/arm_oop.hpp
View File

@ -196,94 +196,94 @@ public:
} }
/** /**
* @brief 轨迹规划核心：每帧插值一小步并对插值点求IK * @brief 轨迹规划核心：基于固定的 start→goal 路径，由 traj_.t 驱动进度
*
* 设计原则：轨迹由 traj_.start / traj_.goal 固定，traj_.t 按速度限制逐帧递增。
* - IK目标点由 traj_.t 插值得到，与当前末端位姿无关，手动扰动不影响轨迹目标
* - IK初始猜测优先使用上一帧轨迹解（连续性好），首帧用当前关节角
* *
* 每帧执行流程： * 每帧执行流程：
* 1. 按速度限制计算本帧 t 步进量 Δt = v_max * dt / 总距离 * 1. 计算 start→goal 的总位移/角位移向量（固定）
* 2. P(t) = start + t*(goal-start) 线性插值得到本帧中间目标 * 2. dt = min(v_max*dt/total, 1.0) 按速度限制计算本帧进度增量
* 3. IK(P(t), q_init=当前关节角) → 初始猜测与结果相差极小，必然收敛 * 3. traj_.t += dt，clamp 到 [0, 1]
* 4. 验证 + 写入 inverseKinematics_.angles * 4. interp = start + t * (goal - start)
* 5. IK(interp, q_init=上一帧解 或 当前关节角) → 写入 traj_.angles
*/ */
void StepTrajectory() { void StepTrajectory() {
if (!traj_.active) return; if (!traj_.active) return;
constexpr float TWO_PI = 2.0f * (float)M_PI; constexpr float TWO_PI = 2.0f * (float)M_PI;
// 计算总距离（用于将速度限制转换为 t 步进量） // 计算 start→goal 的固定总位移向量（不随手动扰动变化）
float dx = traj_.goal.x - traj_.start.x; float tdx = traj_.goal.x - traj_.start.x;
float dy = traj_.goal.y - traj_.start.y; float tdy = traj_.goal.y - traj_.start.y;
float dz = traj_.goal.z - traj_.start.z; float tdz = traj_.goal.z - traj_.start.z;
float pos_dist = sqrtf(dx*dx + dy*dy + dz*dz); float total_dist = sqrtf(tdx*tdx + tdy*tdy + tdz*tdz);
float dr = traj_.goal.roll - traj_.start.roll; float tdr = traj_.goal.roll - traj_.start.roll;
float dp = traj_.goal.pitch - traj_.start.pitch; float tdp = traj_.goal.pitch - traj_.start.pitch;
float dyw = traj_.goal.yaw - traj_.start.yaw; float tdyw = traj_.goal.yaw - traj_.start.yaw;
// 姿态差 wrap 到 (-π, π]：确保插值走最短路径，避免跨越 r2rpy 值域边界时绕大弯 // 姿态差 wrap 到 (-π, π]：走最短路径
dr -= roundf(dr / TWO_PI) * TWO_PI; tdr -= roundf(tdr / TWO_PI) * TWO_PI;
dp -= roundf(dp / TWO_PI) * TWO_PI; tdp -= roundf(tdp / TWO_PI) * TWO_PI;
dyw -= roundf(dyw / TWO_PI) * TWO_PI; tdyw -= roundf(tdyw / TWO_PI) * TWO_PI;
float ang_dist = sqrtf(dr*dr + dp*dp + dyw*dyw); float total_ang = sqrtf(tdr*tdr + tdp*tdp + tdyw*tdyw);
// Δt = v_max * dt / 总距离 // 按速度限制计算本帧进度增量，位置/姿态约束取严格者
// 物理意义：以限定速度匀速运动，谁先到达限制谁决定步进 float dt_p = (total_dist > 1e-6f) ? (traj_.max_lin_vel * timer_.dt / total_dist) : 1.0f;
// 例：v=0.15m/s，dt=2ms，dist=0.3m → Δt=0.001，需1000帧=2s走完 float dt_a = (total_ang > 1e-6f) ? (traj_.max_ang_vel * timer_.dt / total_ang) : 1.0f;
float dt_pos = (pos_dist > 1e-6f) ? (traj_.max_lin_vel * timer_.dt / pos_dist) : 1.0f; float dt = dt_p < dt_a ? dt_p : dt_a;
float dt_ang = (ang_dist > 1e-6f) ? (traj_.max_ang_vel * timer_.dt / ang_dist) : 1.0f; if (dt > 1.0f) dt = 1.0f;
float dt_step = (dt_pos < dt_ang) ? dt_pos : dt_ang; // 取小值（慢者为约束）
traj_.t += dt_step; traj_.t += dt;
if (traj_.t >= 1.0f) { if (traj_.t >= 1.0f) {
traj_.t = 1.0f; traj_.t = 1.0f;
traj_.active = false; // 到达终点 traj_.active = false;
} }
// 线性插值：P(t) = start + t*(goal-start) // 线性插值：start + t * (goal - start)，轨迹固定，不受手动扰动影响
Arm6dof_Pose_t interp; Arm6dof_Pose_t interp;
interp.x = traj_.start.x + traj_.t * dx; interp.x = traj_.start.x + traj_.t * tdx;
interp.y = traj_.start.y + traj_.t * dy; interp.y = traj_.start.y + traj_.t * tdy;
interp.z = traj_.start.z + traj_.t * dz; interp.z = traj_.start.z + traj_.t * tdz;
interp.roll = traj_.start.roll + traj_.t * dr; interp.roll = traj_.start.roll + traj_.t * tdr;
interp.pitch = traj_.start.pitch + traj_.t * dp; interp.pitch = traj_.start.pitch + traj_.t * tdp;
interp.yaw = traj_.start.yaw + traj_.t * dyw; interp.yaw = traj_.start.yaw + traj_.t * tdyw;
// IK初始猜测 = 当前关节角（与上一帧解相差极小，牛顿法必然收敛到正确分支） // IK初始猜测：优先用上一帧轨迹IK解（连续性好，不受手动扰动影响）
// traj_.valid=false 表示首帧或上帧IK失败，改用当前关节角
Arm6dof_JointAngles_t q_init; Arm6dof_JointAngles_t q_init;
if (traj_.valid) {
q_init = traj_.angles;
} else {
for (size_t i = 0; i < JOINT_NUM; ++i) { for (size_t i = 0; i < JOINT_NUM; ++i) {
q_init.q[i] = joints_[i] ? joints_[i]->GetCurrentAngle() : 0.0f; q_init.q[i] = joints_[i] ? joints_[i]->GetCurrentAngle() : 0.0f;
} }
}
if (Arm6dof_InverseKinematics(&interp, &q_init, &traj_.angles, 0.01f, 50) != 0) { if (Arm6dof_InverseKinematics(&interp, &q_init, &traj_.angles, 0.001f, 50) != 0) {
traj_.valid = false; traj_.valid = false;
traj_.active = false; traj_.active = false;
control_.state = MotionState::ERROR; control_.state = MotionState::ERROR;
return; return;
} }
// 角度连续性修正：将IK解调整到与当前关节角最近的等效角 // 角度连续性修正：仅对范围跨度 > 2π 的关节（如 J1: [-15.7, 15.7]）
// 仅对范围跨度 > 2π 的关节（如 J1: [-15.7, 15.7]）有意义： // IK 可能返回与 q_init 差 2π 整数倍的等效解导致跳变，wrap 到最近分支
// IK 求解器对多圈关节可能返回与当前角差 2π 整数倍的等效解，导致跳变。
// 通过将差值 wrap 到 (-π, π] 选取最近分支解决此问题。
// 对范围 ≤ 2π 的关节（如 J4/J6: [0, 2π]）：
// 每个角度值在物理上唯一，不存在 2π 等效分支，直接信任 IK 结果，跳过修正。
for (size_t i = 0; i < JOINT_NUM; ++i) { for (size_t i = 0; i < JOINT_NUM; ++i) {
if (!joints_[i]) continue; if (!joints_[i]) continue;
const auto& dh = joints_[i]->GetDHParams(); const auto& dh = joints_[i]->GetDHParams();
// 关节范围跨度不足 2π——无等效分支，无需修正 if ((dh.qmax - dh.qmin) <= TWO_PI + 0.1f) continue; // 范围 ≤ 2π，无等效分支
if ((dh.qmax - dh.qmin) <= TWO_PI + 0.1f) continue; float diff = traj_.angles.q[i] - q_init.q[i];
float q_ik = traj_.angles.q[i];
float q_cur = q_init.q[i];
float diff = q_ik - q_cur;
// roundf(diff / 2π) * 2π = 最近的 2π 整数倍，减去后差值落在 (-π, π]
diff -= roundf(diff / TWO_PI) * TWO_PI; diff -= roundf(diff / TWO_PI) * TWO_PI;
traj_.angles.q[i] = q_cur + diff; traj_.angles.q[i] = q_init.q[i] + diff;
} }
// 限位检查 // 限位检查
for (size_t i = 0; i < JOINT_NUM; ++i) { for (size_t i = 0; i < JOINT_NUM; ++i) {
if (joints_[i]) { if (joints_[i]) {
const auto& dh = joints_[i]->GetDHParams(); const auto& dh = joints_[i]->GetDHParams();
if (traj_.angles.q[i] < (dh.qmin-qLimitTolerance) || if (traj_.angles.q[i] < (dh.qmin - qLimitTolerance) ||
traj_.angles.q[i] > (dh.qmax+qLimitTolerance)) { traj_.angles.q[i] > (dh.qmax + qLimitTolerance)) {
traj_.valid = false; traj_.valid = false;
traj_.active = false; traj_.active = false;
control_.state = MotionState::ERROR; control_.state = MotionState::ERROR;
@ -292,13 +292,10 @@ public:
} }
} }
// 突变检查：轨迹模式下每帧步长极小，若仍触发突变说明严重异常 // 突变检查：以上一帧轨迹解（q_init）为参考，检查轨迹自身连续性
// 阈值比直接IK时更严：max_lin_vel*dt对应的关节角变化通常远小于0.3rad
static constexpr float TRAJ_MAX_JOINT_DELTA = 0.3f; // rad ≈ 17°/帧 static constexpr float TRAJ_MAX_JOINT_DELTA = 0.3f; // rad ≈ 17°/帧
for (size_t i = 0; i < JOINT_NUM; ++i) { for (size_t i = 0; i < JOINT_NUM; ++i) {
if (joints_[i]) { float delta = fabsf(traj_.angles.q[i] - q_init.q[i]);
float delta = fabsf(traj_.angles.q[i] -
joints_[i]->GetCurrentAngle());
if (delta > TRAJ_MAX_JOINT_DELTA) { if (delta > TRAJ_MAX_JOINT_DELTA) {
traj_.valid = false; traj_.valid = false;
traj_.active = false; traj_.active = false;
@ -306,7 +303,6 @@ public:
return; return;
} }
} }
}
traj_.valid = true; traj_.valid = true;
end_effector_.target = interp; end_effector_.target = interp;
@ -546,14 +542,13 @@ public:
fabsf(goal.roll - traj_.goal.roll) > ANG_THRESH || fabsf(goal.roll - traj_.goal.roll) > ANG_THRESH ||
fabsf(goal.pitch - traj_.goal.pitch) > ANG_THRESH || fabsf(goal.pitch - traj_.goal.pitch) > ANG_THRESH ||
fabsf(goal.yaw - traj_.goal.yaw) > ANG_THRESH; fabsf(goal.yaw - traj_.goal.yaw) > ANG_THRESH;
if (goal_changed) { if (goal_changed) {
// 从当前末端位姿出发重新规划轨迹 // 从当前末端位姿出发重新规划轨迹
traj_.start = end_effector_.current; traj_.start = end_effector_.current;
traj_.goal = goal; traj_.goal = goal;
traj_.t = 0.0f; traj_.t = 0.0f;
traj_.active = true; traj_.active = true;
traj_.valid = true; // 允许Control()进入，等待第一帧StepTrajectory traj_.valid = false; // 首帧由 StepTrajectory 用当前关节角作IK初值
control_.state = MotionState::MOVING; control_.state = MotionState::MOVING;
} }
return 0; return 0;

7
User/module/cmd/cmd.c
View File

@ -262,6 +262,10 @@ static void CMD_RC_BuildArmCmd(CMD_t *ctx) {
* DOWN -> enable = true，set_target_as_current = true（目标位姿吸附为当前位姿） * DOWN -> enable = true，set_target_as_current = true（目标位姿吸附为当前位姿）
*/ */
ctx->output.arm.cmd.set_target_as_current = 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;
}
switch (ctx->input.rc.sw[0]) { switch (ctx->input.rc.sw[0]) {
case CMD_SW_MID: case CMD_SW_MID:
ctx->output.arm.cmd.enable = true; ctx->output.arm.cmd.enable = true;
@ -274,9 +278,6 @@ static void CMD_RC_BuildArmCmd(CMD_t *ctx) {
goto end; goto end;
break; break;
} }
if (ctx->input.rc.sw[1]==CMD_SW_DOWN) {
ctx->output.arm.cmd.set_target_as_current = true;
}
/* 遥控器模式使用笛卡尔位姿控制 */ /* 遥控器模式使用笛卡尔位姿控制 */
ctx->output.arm.cmd.ctrl_type = ARM_CTRL_REMOTE_CARTESIAN; ctx->output.arm.cmd.ctrl_type = ARM_CTRL_REMOTE_CARTESIAN;

112
User/task/arm_main.cpp
View File

@ -65,8 +65,7 @@ MotorLZ* motors_lz[3] = {nullptr}; // 用于调试查看
MotorDM* motors_dm[3] = {nullptr}; // 用于调试查看 MotorDM* motors_dm[3] = {nullptr}; // 用于调试查看
Joint* joints[6] = {nullptr}; Joint* joints[6] = {nullptr};
RoboticArm* robot_arm = nullptr; RoboticArm* robot_arm = nullptr;
Arm_CMD_t arm_cmd; // 当前机械臂控制命令 Arm_CMD_t arm_cmd; // 当前机械臂控制命令（含 target_pose / set_target_as_current）
Arm6dof_Pose_t target_pose = {0}; // 控制输入：目标末端位姿，单位 m/rad（内部计算全链路SI单位）
// ============================================================================ // ============================================================================
// 测试用调试变量（可在调试器Watch窗口直接观察和修改） // 测试用调试变量（可在调试器Watch窗口直接观察和修改）
@ -91,15 +90,10 @@ float gravity_comp_scale = 1.0f;
// 轨迹进度观察 [0.0~1.0]，1.0=已到达目标；调试器中可观察运动是否平滑完成 // 轨迹进度观察 [0.0~1.0]，1.0=已到达目标；调试器中可观察运动是否平滑完成
float traj_progress_dbg = 0.0f; float traj_progress_dbg = 0.0f;
// 轨迹速度设置：可在调试器中修改这两个值，重启case4生效 // 轨迹速度设置：可在调试器中修改这两个值，重启case4生效
float traj_lin_vel = 0.05f; // 末端线速度 (m/s)，默认 150mm/s float traj_lin_vel = 0.15f; // 末端线速度 (m/s)，默认 150mm/s
float traj_ang_vel = 0.3f; // 末端角速度 (rad/s)，默认 ~57°/s float traj_ang_vel = 1.0f; // 末端角速度 (rad/s)，默认 ~57°/s
// 同步控制：设置为1时将target_pose同步为当前位姿（世界系，供MoveCartesian使用），同步后自动清零
int sync_target_to_current = 0;
// 航向系同步：设置为1时将target_pose同步为当前位姿的航向系表示（供MoveCartesianHeading使用），同步后自动清零
// 位置：p_h = Rz(-yaw)*p_w
// 姿态：R_h = Rz(-yaw)*R_cur 提取RPY，同步后直接修改 yaw/pitch/roll 即可直观控制末端各轴旋转
int sync_heading_to_current = 0;
uint8_t control_frame = 1; // 选择在何种坐标系下控制 1= 世界系，2=工具系，3=航向系
// ============================================================================ // ============================================================================
// IK测试调试变量 // IK测试调试变量
// ============================================================================ // ============================================================================
@ -109,10 +103,9 @@ int sync_heading_to_current = 0;
int ik_test_enable = 0; int ik_test_enable = 0;
Arm6dof_JointAngles_t ik_test_result = {0}; // IK解算结果（rad），调试器Watch观察 Arm6dof_JointAngles_t ik_test_result = {0}; // IK解算结果（rad），调试器Watch观察
int ik_test_ret = 0; // IK返回码：0=成功，-1=无解或未初始化 int ik_test_ret = 0; // IK返回码：0=成功，-1=无解或未初始化
// 工具系同步：设置为1时将target_pose同步为当前位姿的工具系表示（供MoveCartesianTool使用），同步后自动清零
// 位置：p_tool = R_cur^T * p_world // cmd task 中的机械臂命令指针（cmd.cpp 中定义），通过 extern 访问以便写回初始位姿
// 姿态：roll=pitch=yaw=0 即 R_target=I，机械臂保持当前末端姿态不变 extern Arm_CMD_t *cmd_for_arm;
int sync_tool_to_current = 0;
extern "C" { extern "C" {
@ -167,11 +160,13 @@ void Task_arm_main(void* argument) {
// JOINT_POSITION: 关节位置控制 + 重力补偿前馈 // JOINT_POSITION: 关节位置控制 + 重力补偿前馈
robot_arm->SetMode(ControlMode::GRAVITY_COMP); robot_arm->SetMode(ControlMode::GRAVITY_COMP);
// 读取当前末端位姿，同步到 arm_cmd 和 cmd 侧累积 target_pose，防止上电时大范围跳变
// 将 target_pose 初始化为当前末端位姿，防止 stage4 首次进入时进行大范围跳变
osDelay(100); osDelay(100);
robot_arm->Update(); // 先跨一帧读取当前状态 robot_arm->Update();
target_pose = robot_arm->GetEndPose(); arm_cmd.target_pose = robot_arm->GetEndPose();
if (cmd_for_arm) {
cmd_for_arm->target_pose = arm_cmd.target_pose;
}
while (1) { while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */ tick += delay_tick; /* 计算下一个唤醒时刻 */
@ -198,53 +193,62 @@ void Task_arm_main(void* argument) {
osMessageQueueGet(task_runtime.msgq.arm.cmd, &arm_cmd, NULL, 0); osMessageQueueGet(task_runtime.msgq.arm.cmd, &arm_cmd, NULL, 0);
if (!arm_cmd.enable) { robot_arm->Enable(arm_cmd.enable);
robot_arm->Enable(false);
}
if (sync_target_to_current) { // set_target_as_current：将目标位姿同步为当前实际末端位姿
target_pose = robot_arm->GetEndPose(); // 同时写回 cmd_for_arm->target_pose，使 cmd 侧增量累积从正确基准继续
sync_target_to_current = 0; // 自动清零 if (arm_cmd.set_target_as_current) {
}
// 航向系同步：将target_pose设为当前位姿的航向系表示，供MoveCartesianHeading使用
// 位置：Rz(-yaw) * p_world → 航向系坐标
// 姿态：R_heading = Rz(-yaw) * R_cur，提取RPY后写入target_pose
// 同步后 yaw/pitch/roll 即为末端在航向系下的当前姿态，修改哪个轴就转哪个轴
if (sync_heading_to_current) {
const Arm6dof_Pose_t& ep = robot_arm->GetEndPose(); const Arm6dof_Pose_t& ep = robot_arm->GetEndPose();
Arm6dof_Pose_t pose;
switch (control_frame) {
case 1: { // 世界系
pose = ep;
arm_cmd.target_pose = pose;
if (cmd_for_arm) {
cmd_for_arm->target_pose = pose;
}
break;
}
case 2: { // 工具系（航向系：Rz(-yaw)旋转水平分量）
float cy = cosf(ep.yaw), sy = sinf(ep.yaw); float cy = cosf(ep.yaw), sy = sinf(ep.yaw);
// 位置逆变换 pose.x = cy * ep.x + sy * ep.y;
target_pose.x = cy * ep.x + sy * ep.y; pose.y = -sy * ep.x + cy * ep.y;
target_pose.y = -sy * ep.x + cy * ep.y; pose.z = ep.z;
target_pose.z = ep.z;
// 姿态逆变换：R_h = Rz(-yaw) * R_cur，再提取RPY
float rpy_cur_arr[3] = {ep.yaw, ep.pitch, ep.roll}; float rpy_cur_arr[3] = {ep.yaw, ep.pitch, ep.roll};
float rpy_mzy_arr[3] = {-ep.yaw, 0.0f, 0.0f}; // Rz(-yaw) float rpy_mzy_arr[3] = {-ep.yaw, 0.0f, 0.0f}; // Rz(-yaw)
Matrixf<3,3> R_h = robotics::rpy2r(Matrixf<3,1>(rpy_mzy_arr)) Matrixf<3,3> R_h = robotics::rpy2r(Matrixf<3,1>(rpy_mzy_arr))
* robotics::rpy2r(Matrixf<3,1>(rpy_cur_arr)); * robotics::rpy2r(Matrixf<3,1>(rpy_cur_arr));
Matrixf<3,1> rpy_h = robotics::r2rpy(R_h); Matrixf<3,1> rpy_h = robotics::r2rpy(R_h);
target_pose.yaw = rpy_h[0][0]; pose.yaw = rpy_h[0][0];
target_pose.pitch = rpy_h[1][0]; pose.pitch = rpy_h[1][0];
target_pose.roll = rpy_h[2][0]; pose.roll = rpy_h[2][0];
sync_heading_to_current = 0; // 自动清零 arm_cmd.target_pose = pose;
if (cmd_for_arm) {
cmd_for_arm->target_pose = pose;
} }
// 工具系同步：将target_pose设为当前位姿的工具系表示，供MoveCartesianTool使用 break;
// 位置：p_tool = R_cur^T * p_world }
// 姿态：roll=pitch=yaw=0 即 R_target=I → R_world=R_cur，保持当前末端姿态不变 case 3: { // 工具系（R_cur^T * p_world，姿态置零）
if (sync_tool_to_current) {
const Arm6dof_Pose_t& ep = robot_arm->GetEndPose();
float rpy_cur_arr[3] = {ep.yaw, ep.pitch, ep.roll}; float rpy_cur_arr[3] = {ep.yaw, ep.pitch, ep.roll};
Matrixf<3,3> R = robotics::rpy2r(Matrixf<3,1>(rpy_cur_arr)); Matrixf<3,3> R = robotics::rpy2r(Matrixf<3,1>(rpy_cur_arr));
float p_arr[3] = {ep.x, ep.y, ep.z}; float p_arr[3] = {ep.x, ep.y, ep.z};
Matrixf<3,1> p_tool = R.trans() * Matrixf<3,1>(p_arr); // R^T * p_world Matrixf<3,1> p_tool = R.trans() * Matrixf<3,1>(p_arr);
target_pose.x = p_tool[0][0]; pose.x = p_tool[0][0];
target_pose.y = p_tool[1][0]; pose.y = p_tool[1][0];
target_pose.z = p_tool[2][0]; pose.z = p_tool[2][0];
target_pose.yaw = 0.0f; // R_target = I，保持姿态 pose.yaw = 0.0f;
target_pose.pitch = 0.0f; pose.pitch = 0.0f;
target_pose.roll = 0.0f; pose.roll = 0.0f;
sync_tool_to_current = 0; // 自动清零 arm_cmd.target_pose = pose;
if (cmd_for_arm) {
cmd_for_arm->target_pose = pose;
}
break;
}
default:
break;
}
} }
// 每帧同步调试观察变量 // 每帧同步调试观察变量
@ -299,7 +303,7 @@ void Task_arm_main(void* argument) {
robot_arm->SetMode(ControlMode::CARTESIAN_POSITION); robot_arm->SetMode(ControlMode::CARTESIAN_POSITION);
// robot_arm->MoveCartesianTool(target_pose); // robot_arm->MoveCartesianTool(target_pose);
// robot_arm->MoveCartesianHeading(target_pose); // robot_arm->MoveCartesianHeading(target_pose);
robot_arm->MoveCartesian(target_pose); robot_arm->MoveCartesian(arm_cmd.target_pose);
// robot_arm->StepTrajectory(); // robot_arm->StepTrajectory();
@ -314,7 +318,7 @@ void Task_arm_main(void* argument) {
// 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 = Arm6dof_InverseKinematics(&target_pose, &current_angles, ik_test_ret = Arm6dof_InverseKinematics(&arm_cmd.target_pose, &current_angles,
&ik_test_result, 0.001f, 50); &ik_test_result, 0.001f, 50);
ik_test_ret++; ik_test_ret++;
} }