From e0e6b33791f2b3427389aa76131a9ecc950bb51c Mon Sep 17 00:00:00 2001 From: xxxxm <2389287465@qq.com> Date: Tue, 10 Mar 2026 00:49:52 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=9D=E5=AD=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/component/toolbox/robotics.h | 105 ++++++++++++----------- User/module/arm_oop.hpp | 129 ++++++++++++++-------------- User/module/cmd/cmd.c | 9 +- User/task/arm_main.cpp | 138 +++++++++++++++--------------- User/task/cmd.cpp | 2 +- 5 files changed, 195 insertions(+), 188 deletions(-) diff --git a/User/component/toolbox/robotics.h b/User/component/toolbox/robotics.h index d5f8839..17dac17 100644 --- a/User/component/toolbox/robotics.h +++ b/User/component/toolbox/robotics.h @@ -211,12 +211,11 @@ class Serial_Link { return J_; } - // inverse kinematic, numerical solution(Newton method) - // param[in] T: homogeneous transformation matrix of end effector - // param[in] q: initial joint variable vector(q0) for Newton method's - // iteration - // param[in] tol: tolerance of error (norm(error of twist vector)) - // param[in] max_iter: maximum iterations, default 30 + // inverse kinematic, Levenberg-Marquardt method + // param[in] Td: target homogeneous transformation matrix of end effector + // param[in] q: initial joint variable vector(q0) for iteration + // param[in] tol: tolerance of error (norm of twist error vector) + // param[in] max_iter: maximum iterations, default 50 // param[out] q: joint variable vector Matrixf<_n, 1> ikine(Matrixf<4, 4> Td, Matrixf<_n, 1> q = matrixf::zeros<_n, 1>(), @@ -225,56 +224,64 @@ class Serial_Link { Matrixf<3, 1> pe, we; Matrixf<6, 1> err, new_err; Matrixf<_n, 1> dq; - float step = 1; - for (int i = 0; i < max_iter; i++) { + float lambda = 1.0f; // LM 阻尼因子 + 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); 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); - for (int i = 0; i < 3; i++) { - err[i][0] = pe[i][0]; - err[i + 3][0] = we[i][0]; + for (int k = 0; k < 3; k++) { + err[k][0] = pe[k][0]; + err[k + 3][0] = we[k][0]; } - if (err.norm() < tol) + float err_norm = err.norm(); + if (err_norm < tol) return q; - // adjust iteration step + Matrixf<6, _n> J = jacob(q); - for (int j = 0; j < 5; j++) { - dq = matrixf::inv(J.trans() * J) * (J.trans() * err) * step; - if (dq[0][0] == INFINITY) // J'*J singular - { - dq = matrixf::inv(J.trans() * J + 0.1f * matrixf::eye<_n, _n>()) * - J.trans() * err * step; - // SVD<6, _n> JTJ_svd(J.trans() * J); - // dq = JTJ_svd.solve(err) * step * 5e-2f; - q += dq; - for (int i = 0; i < _n; i++) { - if (links_[i].type() == R) - q[i][0] = math::loopLimit(q[i][0], -PI, PI); - } - break; - } - T = fkine(q + dq); - pe = t2p(Td) - t2p(T); - we = t2twist(Td * invT(T)).block<3, 1>(3, 0); - for (int i = 0; i < 3; i++) { - new_err[i][0] = pe[i][0]; - new_err[i + 3][0] = we[i][0]; - } - if (new_err.norm() < err.norm()) { - q += dq; - for (int i = 0; i < _n; i++) { - if (links_[i].type() == robotics::Joint_Type_e::R) { - q[i][0] = math::loopLimit(q[i][0], -PI, PI); - } - } - break; - } else { - step /= 2.0f; - } + Matrixf<_n, _n> JtJ = J.trans() * J; + Matrixf<_n, 1> Jte = J.trans() * err; + + // LM 步：dq = (J'J + λI)^{-1} J'e + dq = matrixf::inv(JtJ + lambda * matrixf::eye<_n, _n>()) * Jte; + + // 检查求逆是否成功 + if (dq[0][0] == INFINITY || dq[0][0] != dq[0][0]) { + lambda *= lambda_up; + if (lambda > lambda_max) return q; + continue; + } + + // 评估新误差 + Matrixf<_n, 1> q_new = q + dq; + // 关节角折叠到有效区间 + for (int i = 0; i < _n; i++) { + if (links_[i].type() == R) + q_new[i][0] = math::loopLimit(q_new[i][0], links_[i].qmin_, links_[i].qmax_); + } + T = fkine(q_new); + pe = t2p(Td) - t2p(T); + we = t2twist(Td * invT(T)).block<3, 1>(3, 0); + for (int k = 0; k < 3; k++) { + new_err[k][0] = pe[k][0]; + new_err[k + 3][0] = we[k][0]; + } + float new_err_norm = new_err.norm(); + + if (new_err_norm < err_norm) { + // 误差减小：接受步长，减小阻尼（趋近 Gauss-Newton，加速收敛） + q = q_new; + lambda *= lambda_down; + if (lambda < lambda_min) lambda = lambda_min; + } else { + // 误差增大：拒绝步长，增大阻尼（趋近梯度下降，缩小步幅） + lambda *= lambda_up; + if (lambda > lambda_max) return q; // 阻尼过大，无法继续 } - if (step < 1e-3f) - return q; } return q; } diff --git a/User/module/arm_oop.hpp b/User/module/arm_oop.hpp index b8ec614..01821dc 100644 --- a/User/module/arm_oop.hpp +++ b/User/module/arm_oop.hpp @@ -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 / 总距离 - * 2. P(t) = start + t*(goal-start) 线性插值得到本帧中间目标 - * 3. IK(P(t), q_init=当前关节角) → 初始猜测与结果相差极小，必然收敛 - * 4. 验证 + 写入 inverseKinematics_.angles + * 1. 计算 start→goal 的总位移/角位移向量（固定） + * 2. dt = min(v_max*dt/total, 1.0) 按速度限制计算本帧进度增量 + * 3. traj_.t += dt，clamp 到 [0, 1] + * 4. interp = start + t * (goal - start) + * 5. IK(interp, q_init=上一帧解 或 当前关节角) → 写入 traj_.angles */ void StepTrajectory() { if (!traj_.active) return; constexpr float TWO_PI = 2.0f * (float)M_PI; - // 计算总距离（用于将速度限制转换为 t 步进量） - float dx = traj_.goal.x - traj_.start.x; - float dy = traj_.goal.y - traj_.start.y; - float dz = traj_.goal.z - traj_.start.z; - float pos_dist = sqrtf(dx*dx + dy*dy + dz*dz); + // 计算 start→goal 的固定总位移向量（不随手动扰动变化） + float tdx = traj_.goal.x - traj_.start.x; + float tdy = traj_.goal.y - traj_.start.y; + float tdz = traj_.goal.z - traj_.start.z; + float total_dist = sqrtf(tdx*tdx + tdy*tdy + tdz*tdz); - float dr = traj_.goal.roll - traj_.start.roll; - float dp = traj_.goal.pitch - traj_.start.pitch; - float dyw = traj_.goal.yaw - traj_.start.yaw; - // 姿态差 wrap 到 (-π, π]：确保插值走最短路径，避免跨越 r2rpy 值域边界时绕大弯 - dr -= roundf(dr / TWO_PI) * TWO_PI; - dp -= roundf(dp / TWO_PI) * TWO_PI; - dyw -= roundf(dyw / TWO_PI) * TWO_PI; - float ang_dist = sqrtf(dr*dr + dp*dp + dyw*dyw); + float tdr = traj_.goal.roll - traj_.start.roll; + float tdp = traj_.goal.pitch - traj_.start.pitch; + float tdyw = traj_.goal.yaw - traj_.start.yaw; + // 姿态差 wrap 到 (-π, π]：走最短路径 + tdr -= roundf(tdr / TWO_PI) * TWO_PI; + tdp -= roundf(tdp / TWO_PI) * TWO_PI; + tdyw -= roundf(tdyw / TWO_PI) * TWO_PI; + float total_ang = sqrtf(tdr*tdr + tdp*tdp + tdyw*tdyw); - // Δt = v_max * dt / 总距离 - // 物理意义：以限定速度匀速运动，谁先到达限制谁决定步进 - // 例：v=0.15m/s，dt=2ms，dist=0.3m → Δt=0.001，需1000帧=2s走完 - float dt_pos = (pos_dist > 1e-6f) ? (traj_.max_lin_vel * timer_.dt / pos_dist) : 1.0f; - float dt_ang = (ang_dist > 1e-6f) ? (traj_.max_ang_vel * timer_.dt / ang_dist) : 1.0f; - float dt_step = (dt_pos < dt_ang) ? dt_pos : dt_ang; // 取小值（慢者为约束） + // 按速度限制计算本帧进度增量，位置/姿态约束取严格者 + float dt_p = (total_dist > 1e-6f) ? (traj_.max_lin_vel * timer_.dt / total_dist) : 1.0f; + float dt_a = (total_ang > 1e-6f) ? (traj_.max_ang_vel * timer_.dt / total_ang) : 1.0f; + float dt = dt_p < dt_a ? dt_p : dt_a; + if (dt > 1.0f) dt = 1.0f; - traj_.t += dt_step; + traj_.t += dt; if (traj_.t >= 1.0f) { - traj_.t = 1.0f; - traj_.active = false; // 到达终点 + traj_.t = 1.0f; + traj_.active = false; } - // 线性插值：P(t) = start + t*(goal-start) + // 线性插值：start + t * (goal - start)，轨迹固定，不受手动扰动影响 Arm6dof_Pose_t interp; - interp.x = traj_.start.x + traj_.t * dx; - interp.y = traj_.start.y + traj_.t * dy; - interp.z = traj_.start.z + traj_.t * dz; - interp.roll = traj_.start.roll + traj_.t * dr; - interp.pitch = traj_.start.pitch + traj_.t * dp; - interp.yaw = traj_.start.yaw + traj_.t * dyw; + interp.x = traj_.start.x + traj_.t * tdx; + interp.y = traj_.start.y + traj_.t * tdy; + interp.z = traj_.start.z + traj_.t * tdz; + interp.roll = traj_.start.roll + traj_.t * tdr; + interp.pitch = traj_.start.pitch + traj_.t * tdp; + interp.yaw = traj_.start.yaw + traj_.t * tdyw; - // IK初始猜测 = 当前关节角（与上一帧解相差极小，牛顿法必然收敛到正确分支） + // IK初始猜测：优先用上一帧轨迹IK解（连续性好，不受手动扰动影响） + // traj_.valid=false 表示首帧或上帧IK失败，改用当前关节角 Arm6dof_JointAngles_t q_init; - for (size_t i = 0; i < JOINT_NUM; ++i) { - q_init.q[i] = joints_[i] ? joints_[i]->GetCurrentAngle() : 0.0f; + if (traj_.valid) { + q_init = traj_.angles; + } else { + for (size_t i = 0; i < JOINT_NUM; ++i) { + 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_.active = false; control_.state = MotionState::ERROR; return; } - // 角度连续性修正：将IK解调整到与当前关节角最近的等效角 - // 仅对范围跨度 > 2π 的关节（如 J1: [-15.7, 15.7]）有意义： - // IK 求解器对多圈关节可能返回与当前角差 2π 整数倍的等效解，导致跳变。 - // 通过将差值 wrap 到 (-π, π] 选取最近分支解决此问题。 - // 对范围 ≤ 2π 的关节（如 J4/J6: [0, 2π]）： - // 每个角度值在物理上唯一，不存在 2π 等效分支，直接信任 IK 结果，跳过修正。 + // 角度连续性修正：仅对范围跨度 > 2π 的关节（如 J1: [-15.7, 15.7]） + // IK 可能返回与 q_init 差 2π 整数倍的等效解导致跳变，wrap 到最近分支 for (size_t i = 0; i < JOINT_NUM; ++i) { if (!joints_[i]) continue; const auto& dh = joints_[i]->GetDHParams(); - // 关节范围跨度不足 2π——无等效分支，无需修正 - if ((dh.qmax - dh.qmin) <= TWO_PI + 0.1f) continue; - - 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π 整数倍，减去后差值落在 (-π, π] + if ((dh.qmax - dh.qmin) <= TWO_PI + 0.1f) continue; // 范围 ≤ 2π，无等效分支 + float diff = traj_.angles.q[i] - q_init.q[i]; 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) { if (joints_[i]) { const auto& dh = joints_[i]->GetDHParams(); - if (traj_.angles.q[i] < (dh.qmin-qLimitTolerance) || - traj_.angles.q[i] > (dh.qmax+qLimitTolerance)) { + if (traj_.angles.q[i] < (dh.qmin - qLimitTolerance) || + traj_.angles.q[i] > (dh.qmax + qLimitTolerance)) { traj_.valid = false; traj_.active = false; control_.state = MotionState::ERROR; @@ -292,19 +292,15 @@ public: } } - // 突变检查：轨迹模式下每帧步长极小，若仍触发突变说明严重异常 - // 阈值比直接IK时更严：max_lin_vel*dt对应的关节角变化通常远小于0.3rad + // 突变检查：以上一帧轨迹解（q_init）为参考，检查轨迹自身连续性 static constexpr float TRAJ_MAX_JOINT_DELTA = 0.3f; // rad ≈ 17°/帧 for (size_t i = 0; i < JOINT_NUM; ++i) { - if (joints_[i]) { - float delta = fabsf(traj_.angles.q[i] - - joints_[i]->GetCurrentAngle()); - if (delta > TRAJ_MAX_JOINT_DELTA) { - traj_.valid = false; - traj_.active = false; - control_.state = MotionState::ERROR; - return; - } + float delta = fabsf(traj_.angles.q[i] - q_init.q[i]); + if (delta > TRAJ_MAX_JOINT_DELTA) { + traj_.valid = false; + traj_.active = false; + control_.state = MotionState::ERROR; + return; } } @@ -546,14 +542,13 @@ public: fabsf(goal.roll - traj_.goal.roll) > ANG_THRESH || fabsf(goal.pitch - traj_.goal.pitch) > ANG_THRESH || fabsf(goal.yaw - traj_.goal.yaw) > ANG_THRESH; - if (goal_changed) { // 从当前末端位姿出发重新规划轨迹 traj_.start = end_effector_.current; traj_.goal = goal; traj_.t = 0.0f; traj_.active = true; - traj_.valid = true; // 允许Control()进入，等待第一帧StepTrajectory + traj_.valid = false; // 首帧由 StepTrajectory 用当前关节角作IK初值 control_.state = MotionState::MOVING; } return 0; diff --git a/User/module/cmd/cmd.c b/User/module/cmd/cmd.c index ee81f72..67d696f 100644 --- a/User/module/cmd/cmd.c +++ b/User/module/cmd/cmd.c @@ -262,7 +262,11 @@ static void CMD_RC_BuildArmCmd(CMD_t *ctx) { * DOWN -> enable = true，set_target_as_current = true（目标位姿吸附为当前位姿） */ ctx->output.arm.cmd.set_target_as_current = false; - switch (ctx->input.rc.sw[0]) { + if (ctx->input.rc.sw[1]==CMD_SW_DOWN) { + ctx->output.arm.cmd.set_target_as_current = true; + } + + switch (ctx->input.rc.sw[0]) { case CMD_SW_MID: ctx->output.arm.cmd.enable = true; break; @@ -274,9 +278,6 @@ static void CMD_RC_BuildArmCmd(CMD_t *ctx) { goto end; 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; diff --git a/User/task/arm_main.cpp b/User/task/arm_main.cpp index 0c95379..075f381 100644 --- a/User/task/arm_main.cpp +++ b/User/task/arm_main.cpp @@ -65,8 +65,7 @@ MotorLZ* motors_lz[3] = {nullptr}; // 用于调试查看 MotorDM* motors_dm[3] = {nullptr}; // 用于调试查看 Joint* joints[6] = {nullptr}; RoboticArm* robot_arm = nullptr; -Arm_CMD_t arm_cmd; // 当前机械臂控制命令 -Arm6dof_Pose_t target_pose = {0}; // 控制输入：目标末端位姿，单位 m/rad（内部计算全链路SI单位） +Arm_CMD_t arm_cmd; // 当前机械臂控制命令（含 target_pose / set_target_as_current） // ============================================================================ // 测试用调试变量（可在调试器Watch窗口直接观察和修改） @@ -91,15 +90,10 @@ float gravity_comp_scale = 1.0f; // 轨迹进度观察 [0.0~1.0]，1.0=已到达目标；调试器中可观察运动是否平滑完成 float traj_progress_dbg = 0.0f; // 轨迹速度设置：可在调试器中修改这两个值，重启case4生效 -float traj_lin_vel = 0.05f; // 末端线速度 (m/s)，默认 150mm/s -float traj_ang_vel = 0.3f; // 末端角速度 (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; +float traj_lin_vel = 0.15f; // 末端线速度 (m/s)，默认 150mm/s +float traj_ang_vel = 1.0f; // 末端角速度 (rad/s)，默认 ~57°/s +uint8_t control_frame = 1; // 选择在何种坐标系下控制 1= 世界系，2=工具系，3=航向系 // ============================================================================ // IK测试调试变量 // ============================================================================ @@ -109,10 +103,9 @@ int sync_heading_to_current = 0; int ik_test_enable = 0; Arm6dof_JointAngles_t ik_test_result = {0}; // IK解算结果（rad），调试器Watch观察 int ik_test_ret = 0; // IK返回码：0=成功，-1=无解或未初始化 -// 工具系同步：设置为1时将target_pose同步为当前位姿的工具系表示（供MoveCartesianTool使用），同步后自动清零 -// 位置：p_tool = R_cur^T * p_world -// 姿态：roll=pitch=yaw=0 即 R_target=I，机械臂保持当前末端姿态不变 -int sync_tool_to_current = 0; + +// cmd task 中的机械臂命令指针（cmd.cpp 中定义），通过 extern 访问以便写回初始位姿 +extern Arm_CMD_t *cmd_for_arm; extern "C" { @@ -167,12 +160,14 @@ void Task_arm_main(void* argument) { // JOINT_POSITION: 关节位置控制 + 重力补偿前馈 robot_arm->SetMode(ControlMode::GRAVITY_COMP); - - // 将 target_pose 初始化为当前末端位姿，防止 stage4 首次进入时进行大范围跳变 + // 读取当前末端位姿，同步到 arm_cmd 和 cmd 侧累积 target_pose，防止上电时大范围跳变 osDelay(100); - robot_arm->Update(); // 先跨一帧读取当前状态 - target_pose = robot_arm->GetEndPose(); - + robot_arm->Update(); + arm_cmd.target_pose = robot_arm->GetEndPose(); + if (cmd_for_arm) { + cmd_for_arm->target_pose = arm_cmd.target_pose; + } + while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /*---------------------------零点校准---------------------------*/ @@ -198,55 +193,64 @@ void Task_arm_main(void* argument) { osMessageQueueGet(task_runtime.msgq.arm.cmd, &arm_cmd, NULL, 0); - if (!arm_cmd.enable) { - robot_arm->Enable(false); - } + robot_arm->Enable(arm_cmd.enable); - if (sync_target_to_current) { - target_pose = robot_arm->GetEndPose(); - sync_target_to_current = 0; // 自动清零 + // set_target_as_current：将目标位姿同步为当前实际末端位姿 + // 同时写回 cmd_for_arm->target_pose，使 cmd 侧增量累积从正确基准继续 + if (arm_cmd.set_target_as_current) { + 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); + pose.x = cy * ep.x + sy * ep.y; + pose.y = -sy * ep.x + cy * ep.y; + pose.z = ep.z; + float rpy_cur_arr[3] = {ep.yaw, ep.pitch, ep.roll}; + 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)) + * robotics::rpy2r(Matrixf<3,1>(rpy_cur_arr)); + Matrixf<3,1> rpy_h = robotics::r2rpy(R_h); + pose.yaw = rpy_h[0][0]; + pose.pitch = rpy_h[1][0]; + pose.roll = rpy_h[2][0]; + arm_cmd.target_pose = pose; + if (cmd_for_arm) { + cmd_for_arm->target_pose = pose; + } + break; + } + case 3: { // 工具系（R_cur^T * p_world，姿态置零） + float rpy_cur_arr[3] = {ep.yaw, ep.pitch, ep.roll}; + Matrixf<3,3> R = robotics::rpy2r(Matrixf<3,1>(rpy_cur_arr)); + float p_arr[3] = {ep.x, ep.y, ep.z}; + Matrixf<3,1> p_tool = R.trans() * Matrixf<3,1>(p_arr); + pose.x = p_tool[0][0]; + pose.y = p_tool[1][0]; + pose.z = p_tool[2][0]; + pose.yaw = 0.0f; + pose.pitch = 0.0f; + pose.roll = 0.0f; + arm_cmd.target_pose = pose; + if (cmd_for_arm) { + cmd_for_arm->target_pose = pose; + } + break; + } + default: + break; } - // 航向系同步：将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(); - float cy = cosf(ep.yaw), sy = sinf(ep.yaw); - // 位置逆变换 - target_pose.x = cy * ep.x + sy * ep.y; - target_pose.y = -sy * ep.x + cy * ep.y; - 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_mzy_arr[3] = {-ep.yaw, 0.0f, 0.0f}; // Rz(-yaw) - Matrixf<3,3> R_h = robotics::rpy2r(Matrixf<3,1>(rpy_mzy_arr)) - * robotics::rpy2r(Matrixf<3,1>(rpy_cur_arr)); - Matrixf<3,1> rpy_h = robotics::r2rpy(R_h); - target_pose.yaw = rpy_h[0][0]; - target_pose.pitch = rpy_h[1][0]; - target_pose.roll = rpy_h[2][0]; - sync_heading_to_current = 0; // 自动清零 - } - // 工具系同步：将target_pose设为当前位姿的工具系表示，供MoveCartesianTool使用 - // 位置：p_tool = R_cur^T * p_world - // 姿态：roll=pitch=yaw=0 即 R_target=I → R_world=R_cur，保持当前末端姿态不变 - if (sync_tool_to_current) { - const Arm6dof_Pose_t& ep = robot_arm->GetEndPose(); - float rpy_cur_arr[3] = {ep.yaw, ep.pitch, ep.roll}; - Matrixf<3,3> R = robotics::rpy2r(Matrixf<3,1>(rpy_cur_arr)); - 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 - target_pose.x = p_tool[0][0]; - target_pose.y = p_tool[1][0]; - target_pose.z = p_tool[2][0]; - target_pose.yaw = 0.0f; // R_target = I，保持姿态 - target_pose.pitch = 0.0f; - target_pose.roll = 0.0f; - sync_tool_to_current = 0; // 自动清零 - } - + } + // 每帧同步调试观察变量 for (int i = 0; i < 6; ++i) { current_angles.q[i] = joints[i]->GetCurrentAngle(); @@ -299,7 +303,7 @@ void Task_arm_main(void* argument) { robot_arm->SetMode(ControlMode::CARTESIAN_POSITION); // robot_arm->MoveCartesianTool(target_pose); // robot_arm->MoveCartesianHeading(target_pose); - robot_arm->MoveCartesian(target_pose); + robot_arm->MoveCartesian(arm_cmd.target_pose); // robot_arm->StepTrajectory(); @@ -314,7 +318,7 @@ void Task_arm_main(void* argument) { // IK测试：以当前角度为初始猜测，对 target_pose 求逆运动学 // 在调试器中将 ik_test_enable 置1启用，观察 ik_test_result 和 ik_test_ret if (ik_test_enable) { - ik_test_ret = Arm6dof_InverseKinematics(&target_pose, ¤t_angles, + ik_test_ret = Arm6dof_InverseKinematics(&arm_cmd.target_pose, ¤t_angles, &ik_test_result, 0.001f, 50); ik_test_ret++; } diff --git a/User/task/cmd.cpp b/User/task/cmd.cpp index 97eb014..bcaeb29 100644 --- a/User/task/cmd.cpp +++ b/User/task/cmd.cpp @@ -62,7 +62,7 @@ void Task_cmd(void *argument) { /* 获取命令发送到各模块 */ cmd_for_chassis = CMD_GetChassisCmd(&cmd); cmd_for_arm = CMD_GetArmCmd(&cmd); - osMessageQueueReset(task_runtime.msgq.chassis.cmd); + osMessageQueueReset(task_runtime.msgq.chassis.cmd); osMessageQueuePut(task_runtime.msgq.chassis.cmd, cmd_for_chassis, 0, 0); osMessageQueuePut(task_runtime.msgq.arm.cmd, cmd_for_arm, 0, 0);