diff --git a/App/ScrewPosition/ScrewPositionApp/Presenter/Inc/PoseAxesBuilder.h b/App/ScrewPosition/ScrewPositionApp/Presenter/Inc/PoseAxesBuilder.h new file mode 100644 index 00000000..415033a7 --- /dev/null +++ b/App/ScrewPosition/ScrewPositionApp/Presenter/Inc/PoseAxesBuilder.h @@ -0,0 +1,132 @@ +#pragma once + +#include "CoordinateTypes.h" +#include + +struct HandEyeExtrinsic; +struct RobotPose6D; + +// 螺杆 / 工具盘姿态计算工具集。 +// +// 解决两类问题： +// 1) 当主轴接近世界 Z 时，原 Y = Z_world × X 构造的副轴对 X 的微小抖动敏感（叉乘量级 +// 退化到 sin(夹角)），会被放大成 ~10° 的方向摆动。BuildAnchoredFrame 用「参考方向 +// 投影到 X 垂直平面」的 Gram-Schmidt 方式锚定 Y 轴，对主轴抖动鲁棒。 +// 2) 「Eye 三轴 → Eye 补偿 → Eye→Robot 变换 → Robot 补偿 → 提欧拉 → 万向锁消歧」 +// 的流水线在螺杆和工具盘场景重复出现，封装为 ComputeRobotPoseAngles 单次调用。 +namespace PoseAxesBuilder +{ + +// ============ 基础数值工具 ============ + +CTVec3D NormalizeVector(const CTVec3D& v); +bool IsValidVector(const CTVec3D& v); + +// ============ 手眼标定相关 ============ + +// 从 16 元素行优先数组构建手眼标定矩阵。 +// 约定：clibMatrix 表达 T_flange_camera（即 Eye 系下的点经其变换得到 Flange 系下的点）， +// 与 CCoordinateTransform::sixAxisEyeInHandBuildTransform 入参一致。 +CTHomogeneousMatrix BuildHandEyeMatrix(const double clibMatrix[16]); + +// 把法兰系下的局部方向（例如 (0,1,0) = 法兰 Y 轴）转换为 Eye 系下的方向向量。 +// 用于得到稳定的 Y 锚点：法兰姿态固定时其轴在 Eye 系下的方向也稳定。 +// 内部走 handEyeMatrix.rotation^T，仅旋转、不平移。 +CTVec3D FlangeAxisToEye(const CTHomogeneousMatrix& handEyeMatrix, + const CTVec3D& flangeLocalAxis); + +// 按 poseOutputOrder 把 RobotPose6D.a/b/c 重排成真实绕轴角度 rx/ry/rz（度）。 +void ResolveRobotPoseAnglesDegrees(const RobotPose6D& robotPose, + int poseOutputOrder, + double& rxDeg, + double& ryDeg, + double& rzDeg); + +// ============ 锚点式正交三元组构建 ============ + +// 给定主轴 primary 和参考方向 referenceY，构建正交三元组： +// X = primary（归一化） +// Y = (referenceY - (referenceY·X) X) 归一化 （Gram-Schmidt 投影） +// Z = X × Y（归一化） +// +// 典型用法： +// - 螺杆：primary = 螺杆轴向, referenceY = FlangeAxisToEye(handEye, (0,1,0)) +// - 工具盘：primary = 算法 X 轴, referenceY = 算法 Y 轴（已正交时退化为恒等） +// +// 退化判定：投影后 |Y_proj| < minPerpendicularity（即 primary 与 referenceY 近共线）， +// 此时副轴无法稳定确定，返回 false；调用方应换用另一参考方向或跳过该目标。 +bool BuildAnchoredFrame(const CTVec3D& primary, + const CTVec3D& referenceY, + std::array& axes, + double minPerpendicularity = 0.1); + +// ============ 三元组运算 ============ + +// 在三轴自身坐标系内施加 R = Rx(rotX)·Ry(rotY)·Rz(rotZ)（角度）。 +// 物理含义：把"算法工具系"二次旋转到"机器人期望 TCP 系"或远离万向锁的位姿。 +void ApplyAxesRotation(std::array& axes, + double rotXDeg, double rotYDeg, double rotZDeg); + +// 把 srcAxes 经齐次变换的旋转部分搬到 dstAxes（不平移），并归一化。 +// 任一轴归一化失败返回 false。 +bool TransformAxes(const std::array& srcAxes, + const CTHomogeneousMatrix& transform, + std::array& dstAxes); + +// 把三轴当列向量组装成 3x3 旋转矩阵。 +CTRotationMatrix BuildRotationMatrix(const std::array& axes); + +// ============ 欧拉提取与消歧 ============ + +// 旋转矩阵 → 欧拉角（按 order），自动把 pitch 归一化到 [-90°, 90°]。 +void RotationMatrixToConfiguredEulerDegrees(const CTRotationMatrix& rotation, + CTEulerOrder order, + double& rollDeg, + double& pitchDeg, + double& yawDeg); + +// 万向锁附近（|pitch| 接近 90°）把 yaw/roll 锚定到 (refYaw/refRoll) 周围，让多帧输出 +// 稳定落在机器人当前法兰姿态附近。|pitch| < 80° 时不做任何修改。 +void ResolveGimbalAmbiguity(double& rollDeg, + double pitchDeg, + double& yawDeg, + double refRollDeg, + double refYawDeg); + +// ============ 完整流水线（DetectScrew / DetectToolDisk 共用）============ + +struct PoseAngles +{ + double rollDeg = 0.0; + double pitchDeg = 0.0; + double yawDeg = 0.0; +}; + +// 中间状态调试信息；不需要可传 nullptr。 +struct PoseDebugInfo +{ + PoseAngles eyeEulerBefore; // ApplyAxesRotation(rotX/Y/Z) 之前的 Eye 系欧拉角 + PoseAngles eyeEulerAfter; // 同上之后的 Eye 系欧拉角 + PoseAngles robotEulerBeforeOutRot; // Eye→Robot 变换后、ApplyAxesRotation(outRotX/Y/Z) 之前的 Robot 系欧拉角 + // —— 标定 outRot 时把它粘进 CloudView「姿态补偿」的"当前"栏 + CTRotationMatrix eyeRotationAfter; // Eye 补偿后的旋转矩阵 + CTRotationMatrix robotRotation; // 最终 Robot 系旋转矩阵 +}; + +// Eye 系三轴 → Robot 系欧拉角完整流水线： +// 1) Eye 系内补偿：ApplyAxesRotation(extrinsic.rotX/Y/Z) +// 2) Eye → Robot 变换：TransformAxes(eyeInHandTransform) +// 3) Robot 系内补偿：ApplyAxesRotation(extrinsic.outRotX/Y/Z) +// 4) 提欧拉角 + 万向锁消歧（refRollDeg / refYawDeg 通常取机器人当前法兰姿态） +// +// 返回 false：变换链中任一步退化（向量长度为零等）。 +bool ComputeRobotPoseAngles(const std::array& eyeAxes, + const CTHomogeneousMatrix& eyeInHandTransform, + const HandEyeExtrinsic& extrinsic, + CTEulerOrder eulerOrder, + double refRollDeg, + double refYawDeg, + PoseAngles& outAngles, + PoseDebugInfo* outDebugInfo = nullptr); + +} // namespace PoseAxesBuilder diff --git a/App/ScrewPosition/ScrewPositionApp/Presenter/Src/DetectPresenter.cpp b/App/ScrewPosition/ScrewPositionApp/Presenter/Src/DetectPresenter.cpp index 4c330106..c8894bdc 100644 --- a/App/ScrewPosition/ScrewPositionApp/Presenter/Src/DetectPresenter.cpp +++ b/App/ScrewPosition/ScrewPositionApp/Presenter/Src/DetectPresenter.cpp @@ -1,21 +1,14 @@ #include "DetectPresenter.h" #include "AlgorithmParamConverter.h" #include "CoordinateTransform.h" +#include "PoseAxesBuilder.h" #include "ScrewPositionTCPProtocol.h" #include "rodAndBarDetection_Export.h" #include #include -#include - -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif namespace { -constexpr double kVectorEpsilon = 1e-8; -constexpr double kRotationEpsilon = 1e-9; - QImage BuildScrewPointCloudImage(const std::vector>& xyzData, const std::vector& screwInfo, const std::vector>* approachXY = nullptr) @@ -67,7 +60,6 @@ QImage BuildToolDiskPointCloudImage(const std::vector 180.0) { - deg -= 360.0; - } - while (deg <= -180.0) { - deg += 360.0; - } - return deg; -} - -// 对 Tait-Bryan 欧拉角做等价归一化，使 pitch ∈ [-90°, 90°] -// 依据：(α, β, γ) ≡ (α±180°, 180°-β, γ±180°) 以及 (α±180°, -180°-β, γ±180°) -// 适用于 CTEulerOrder 中所有 12 种 Tait-Bryan 顺序（内旋/外旋一致） -void NormalizePitchRange(double& rollDeg, double& pitchDeg, double& yawDeg) -{ - pitchDeg = WrapDegreesTo180(pitchDeg); - if (pitchDeg > 90.0) { - pitchDeg = 180.0 - pitchDeg; - rollDeg += 180.0; - yawDeg += 180.0; - } else if (pitchDeg < -90.0) { - pitchDeg = -180.0 - pitchDeg; - rollDeg += 180.0; - yawDeg += 180.0; - } - rollDeg = WrapDegreesTo180(rollDeg); - yawDeg = WrapDegreesTo180(yawDeg); -} - -// 统一走 CCoordinateTransform::rotationMatrixToEuler（与 eulerToRotationMatrix 成对互逆）， -// 再把 pitch 压入 [-90°, 90°]，保证对机器人侧输出的姿态语义稳定。 -void RotationMatrixToConfiguredEulerDegrees(const CTRotationMatrix& rotation, - CTEulerOrder order, - double& rollDeg, - double& pitchDeg, - double& yawDeg) -{ - const CTEulerAngles euler = CCoordinateTransform::rotationMatrixToEuler(rotation, order); - euler.toDegrees(rollDeg, pitchDeg, yawDeg); - NormalizePitchRange(rollDeg, pitchDeg, yawDeg); -} - -// 万向锁消歧：当 |pitch| 接近 90° 时，Rz(yaw)·Ry(pitch)·Rx(roll) 的分解退化成一个自由度。 -// 严格展开可得： -// pitch = +90° → R 只依赖 (roll - yaw)；任意满足 roll - yaw = const 的组合都表示同一 R -// pitch = -90° → R 只依赖 (roll + yaw)；任意满足 roll + yaw = const 的组合都表示同一 R -// 本函数把 yaw 锚定到 refYaw 附近，把剩余自由度全部放到 roll， -// 让不同帧的输出稳定落在参考（通常是机器人法兰的 rx、rz）附近。 -// -// 【强消歧配置】：阈值覆盖 80°-85°，优先保证数值稳定性，代价是 pitch∈[80°,85°] -// 的过渡带会改变真实旋转矩阵（X 轴可能偏 3°-5°）。如需精确定姿请收窄阈值或换欧拉顺序。 -// |pitch| < kSoftThreshold：完全不改（保留真实分解） -// |pitch| ≥ kHardThreshold：完全锁到参考（对 R 影响最小） -// kSoftThreshold ≤ |pitch| < kHardThreshold：线性插值过渡 -void ResolveGimbalAmbiguity(double& rollDeg, double pitchDeg, double& yawDeg, - double refRollDeg, double refYawDeg) -{ - constexpr double kSoftThresholdDeg = 80.0; - constexpr double kHardThresholdDeg = 85.0; - const double absPitch = std::abs(pitchDeg); - if (absPitch < kSoftThresholdDeg) { - return; - } - - // 不变量方向与 pitch 符号的关系（由 Rz·Ry·Rx 展开式导出）： - // pitch > 0 → inv = roll - yaw - // pitch < 0 → inv = roll + yaw - const bool positivePitch = (pitchDeg >= 0.0); - const double sum = positivePitch ? (rollDeg - yawDeg) : (rollDeg + yawDeg); - const double refSum = positivePitch ? (refRollDeg - refYawDeg) : (refRollDeg + refYawDeg); - - double adjustedSum = sum; - while (adjustedSum - refSum > 180.0) { adjustedSum -= 360.0; } - while (adjustedSum - refSum < -180.0) { adjustedSum += 360.0; } - - const double fullYaw = WrapDegreesTo180(refYawDeg); - // 反解 roll：positive 时 roll = sum + yaw；negative 时 roll = sum - yaw（与 sum 方向匹配） - const double fullRoll = WrapDegreesTo180(positivePitch ? (adjustedSum + fullYaw) - : (adjustedSum - fullYaw)); - - if (absPitch >= kHardThresholdDeg) { - rollDeg = fullRoll; - yawDeg = fullYaw; - return; - } - - // 过渡带：alpha 从 0（kSoftThreshold）线性增到 1（kHardThreshold） - const double alpha = (absPitch - kSoftThresholdDeg) / - (kHardThresholdDeg - kSoftThresholdDeg); - auto lerpAngle = [alpha](double from, double to) { - const double diff = WrapDegreesTo180(to - from); - return WrapDegreesTo180(from + alpha * diff); - }; - rollDeg = lerpAngle(rollDeg, fullRoll); - yawDeg = lerpAngle(yawDeg, fullYaw); -} - -CTVec3D NormalizeVector(const CTVec3D& vector) -{ - const double length = vector.norm(); - if (length < kVectorEpsilon) { - return CTVec3D(); - } - return vector * (1.0 / length); -} - -double DotProduct(const CTVec3D& a, const CTVec3D& b) -{ - return a.x * b.x + a.y * b.y + a.z * b.z; -} - -CTVec3D CrossProduct(const CTVec3D& a, const CTVec3D& b) -{ - return CTVec3D(a.y * b.z - a.z * b.y, - a.z * b.x - a.x * b.z, - a.x * b.y - a.y * b.x); -} - -bool IsValidVector(const CTVec3D& vector) -{ - return vector.norm() >= kVectorEpsilon; -} - CTVec3D ToCTVec3D(const SVzNL3DPoint& point) { return CTVec3D(point.x, point.y, point.z); } -bool TryProjectedAxis(const CTVec3D& reference, const CTVec3D& primary, CTVec3D& axis) -{ - const CTVec3D candidate = NormalizeVector(reference - primary * DotProduct(reference, primary)); - if (!IsValidVector(candidate)) { - return false; - } - axis = candidate; - return true; -} - -bool BuildFrameFromXAxis(const CTVec3D& primary, std::array& axes) -{ - const CTVec3D xAxis = NormalizeVector(primary); - if (!IsValidVector(xAxis)) { - return false; - } - - CTVec3D yAxis = NormalizeVector(CrossProduct(CTVec3D(0.0, 0.0, 1.0), xAxis)); - if (!IsValidVector(yAxis) && - !TryProjectedAxis(CTVec3D(0.0, 1.0, 0.0), xAxis, yAxis) && - !TryProjectedAxis(CTVec3D(1.0, 0.0, 0.0), xAxis, yAxis)) { - return false; - } - - const CTVec3D zAxis = NormalizeVector(CrossProduct(xAxis, yAxis)); - if (!IsValidVector(zAxis)) { - return false; - } - - axes = {xAxis, yAxis, zAxis}; - return true; -} - -void ApplyAxesRotation(std::array& axes, - double rotXDeg, double rotYDeg, double rotZDeg) -{ - if (std::fabs(rotXDeg) < kRotationEpsilon && - std::fabs(rotYDeg) < kRotationEpsilon && - std::fabs(rotZDeg) < kRotationEpsilon) { - return; - } - - const double rx = rotXDeg * M_PI / 180.0; - const double ry = rotYDeg * M_PI / 180.0; - const double rz = rotZDeg * M_PI / 180.0; - - const double cx = std::cos(rx), sx = std::sin(rx); - const double cy = std::cos(ry), sy = std::sin(ry); - const double cz = std::cos(rz), sz = std::sin(rz); - - // R = Rx(rx) * Ry(ry) * Rz(rz)；对 Eye 坐标系下的工具轴组合（列向量）右乘 R， - // 等价于在工具坐标系内按内旋 XYZ 顺序施加补偿旋转。 - double R[3][3]; - R[0][0] = cy * cz; - R[0][1] = -cy * sz; - R[0][2] = sy; - R[1][0] = sx * sy * cz + cx * sz; - R[1][1] = -sx * sy * sz + cx * cz; - R[1][2] = -sx * cy; - R[2][0] = -cx * sy * cz + sx * sz; - R[2][1] = cx * sy * sz + sx * cz; - R[2][2] = cx * cy; - - const CTVec3D oldX = axes[0]; - const CTVec3D oldY = axes[1]; - const CTVec3D oldZ = axes[2]; - - axes[0] = oldX * R[0][0] + oldY * R[1][0] + oldZ * R[2][0]; - axes[1] = oldX * R[0][1] + oldY * R[1][1] + oldZ * R[2][1]; - axes[2] = oldX * R[0][2] + oldY * R[1][2] + oldZ * R[2][2]; -} - -bool TransformAxes(const std::array& eyeAxes, - const CTHomogeneousMatrix& transform, - std::array& robotAxes) -{ - for (size_t i = 0; i < eyeAxes.size(); ++i) { - robotAxes[i] = NormalizeVector(transform.transformVector(eyeAxes[i])); - if (!IsValidVector(robotAxes[i])) { - return false; - } - } - return true; -} - -CTRotationMatrix BuildRotationMatrix(const std::array& axes) -{ - CTRotationMatrix rotation; - rotation.at(0, 0) = axes[0].x; - rotation.at(0, 1) = axes[1].x; - rotation.at(0, 2) = axes[2].x; - rotation.at(1, 0) = axes[0].y; - rotation.at(1, 1) = axes[1].y; - rotation.at(1, 2) = axes[2].y; - rotation.at(2, 0) = axes[0].z; - rotation.at(2, 1) = axes[1].z; - rotation.at(2, 2) = axes[2].z; - return rotation; -} - -// ============ ZXY 欧拉角分解/重建（绕开 ZYX 奇异区） ============ - -// ZXY 分解：R = Rz(α)·Rx(β)·Ry(γ) -// ZXY 奇异点在 β=±90°（R(2,1)=±1），与 ZYX 奇异点 pitch≈±90° 不重叠。 -static void DecomposeRotationZXY(const CTRotationMatrix& R, - double& alpha, double& beta, double& gamma) -{ - const double sb = std::max(-1.0, std::min(1.0, R.at(2, 1))); - const double betaRad = std::asin(sb); - const double cb = std::cos(betaRad); - - if (std::fabs(cb) > 1e-7) { - alpha = std::atan2(-R.at(0, 1), R.at(1, 1)) * 180.0 / M_PI; - gamma = std::atan2(-R.at(2, 0), R.at(2, 2)) * 180.0 / M_PI; - } else { - alpha = std::atan2(R.at(1, 0), R.at(0, 0)) * 180.0 / M_PI; - gamma = 0.0; - } - beta = betaRad * 180.0 / M_PI; -} - -static CTRotationMatrix RebuildRotationZXY(double alphaDeg, double betaDeg, double gammaDeg) -{ - const double a = alphaDeg * M_PI / 180.0; - const double b = betaDeg * M_PI / 180.0; - const double g = gammaDeg * M_PI / 180.0; - const double ca = std::cos(a), sa = std::sin(a); - const double cb = std::cos(b), sb = std::sin(b); - const double cg = std::cos(g), sg = std::sin(g); - - CTRotationMatrix R; - R.at(0,0) = ca*cg + sa*sb*sg; R.at(0,1) = -sa*cb; R.at(0,2) = ca*sg + sa*sb*cg; - R.at(1,0) = sa*cg - ca*sb*sg; R.at(1,1) = ca*cb; R.at(1,2) = sa*sg - ca*sb*cg; - R.at(2,0) = -cb*sg; R.at(2,1) = sb; R.at(2,2) = cb*cg; - return R; -} - -// 直接 ZYX 分解：R = Rz(yaw)·Ry(pitch)·Rx(roll) -static void DecomposeRotationZYX(const CTRotationMatrix& R, - double& roll, double& pitch, double& yaw) -{ - const double sp = std::max(-1.0, std::min(1.0, -R.at(2, 0))); - const double pitchRad = std::asin(sp); - const double cp = std::cos(pitchRad); - - if (std::fabs(cp) > 1e-7) { - roll = std::atan2(R.at(2, 1), R.at(2, 2)) * 180.0 / M_PI; - yaw = std::atan2(R.at(1, 0), R.at(0, 0)) * 180.0 / M_PI; - } else { - yaw = std::atan2(-R.at(0, 1), R.at(1, 1)) * 180.0 / M_PI; - roll = 0.0; - } - pitch = pitchRad * 180.0 / M_PI; -} - -// 稳定 ZYX 欧拉角提取： -// 1) |R(2,0)| ≤ threshold → 直接 ZYX 分解 -// 2) |R(2,0)| > threshold → ZXY 分解（奇异点不重叠）→ 重建 R' → ZYX 分解 -// 3) 用拍照法兰姿态做连续性修正（±360° 拉回参考附近） -static void ExtractStableZYXEulerDegrees(const CTRotationMatrix& R, - double& roll, double& pitch, double& yaw, - double refRoll, double refYaw) -{ - constexpr double kSingularityThreshold = 0.9998; - - const double absSinPitch = std::fabs(R.at(2, 0)); - if (absSinPitch > kSingularityThreshold) { - LOG_INFO("[Algo Thread] ZYX singularity: |R(2,0)|=%.6f > %.4f\n", - absSinPitch, kSingularityThreshold); - - // ZXY 分解 - double alpha, beta, gamma; - DecomposeRotationZXY(R, alpha, beta, gamma); - LOG_INFO("[Algo Thread] ZXY: Z=%.3f, X=%.3f, Y=%.3f\n", alpha, beta, gamma); - - // 重建 R' 并验证 - CTRotationMatrix Rp = RebuildRotationZXY(alpha, beta, gamma); - double err = 0; - for (int i = 0; i < 3; ++i) - for (int j = 0; j < 3; ++j) - err += (Rp.at(i, j) - R.at(i, j)) * (Rp.at(i, j) - R.at(i, j)); - err = std::sqrt(err); - LOG_INFO("[Algo Thread] R' rebuild error: %.10f\n", err); - - // 对 R' 做 ZYX 分解 - DecomposeRotationZYX(Rp, roll, pitch, yaw); - LOG_INFO("[Algo Thread] ZYX from R': Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n", roll, pitch, yaw); - } else { - DecomposeRotationZYX(R, roll, pitch, yaw); - } - - // 连续性修正：拉到参考姿态附近 - double yawDiff = yaw - refYaw; - while (yawDiff > 180.0) { yaw -= 360.0; yawDiff -= 360.0; } - while (yawDiff < -180.0) { yaw += 360.0; yawDiff += 360.0; } - - double rollDiff = roll - refRoll; - while (rollDiff > 180.0) { roll -= 360.0; rollDiff -= 360.0; } - while (rollDiff < -180.0) { roll += 360.0; rollDiff += 360.0; } -} - } DetectPresenter::DetectPresenter(/* args */) @@ -588,31 +202,39 @@ int DetectPresenter::DetectScrew( detectionResult.success = true; detectionResult.errorCode = 0; detectionResult.message = QStringLiteral("螺杆检测成功"); - const CTHomogeneousMatrix handEyeMatrix = BuildHandEyeMatrix(clibMatrix); + const CTHomogeneousMatrix handEyeMatrix = PoseAxesBuilder::BuildHandEyeMatrix(clibMatrix); const CTEulerOrder order = static_cast(extrinsic.eulerOrder); double rx, ry, rz; - ResolveRobotPoseAnglesDegrees(robotPose, poseOutputOrder, rx, ry, rz); + PoseAxesBuilder::ResolveRobotPoseAnglesDegrees(robotPose, poseOutputOrder, rx, ry, rz); const CTRobotPose flangePose = CTRobotPose::fromDegrees(robotPose.x, robotPose.y, robotPose.z, rx, ry, rz); // flangePose.rx/ry/rz 已经是真实轴角，可直接交给 sixAxisEyeInHandBuildTransform 按 eulerOrder 组装。 const CTHomogeneousMatrix eyeInHandTransform = CCoordinateTransform::sixAxisEyeInHandBuildTransform( flangePose, order, handEyeMatrix); + // 锚点：法兰 Y 轴在 Eye 系下的方向。法兰姿态在一次检测内固定，所以 flangeYInEye 也固定， + // 用它作 BuildAnchoredFrame 的参考方向可以消除"螺杆轴向接近 Z_world 时副轴方向被放大抖动"。 + const CTVec3D flangeYInEye = PoseAxesBuilder::FlangeAxisToEye(handEyeMatrix, CTVec3D(0.0, 1.0, 0.0)); + if (debugParam.enableDebug && debugParam.printDetailLog) { LOG_INFO("[Algo Thread] Robot flange pose fields: X=%.3f, Y=%.3f, Z=%.3f, RX=%.3f, RY=%.3f, RZ=%.3f (eulerOrder=%d)\n", robotPose.x, robotPose.y, robotPose.z, rx, ry, rz, static_cast(order)); + LOG_INFO("[Algo Thread] Flange Y axis in Eye frame (anchor): X=%.6f, Y=%.6f, Z=%.6f\n", + flangeYInEye.x, flangeYInEye.y, flangeYInEye.z); } std::vector> approachEyeXY; approachEyeXY.reserve(screwInfo.size()); + const bool needDebug = debugParam.enableDebug && debugParam.printDetailLog; + for (size_t i = 0; i < screwInfo.size(); ++i) { const auto& screw = screwInfo[i]; const CTVec3D eyeCenter = ToCTVec3D(screw.center); - const CTVec3D eyeAxialDir = NormalizeVector(ToCTVec3D(screw.axialDir)); + const CTVec3D eyeAxialDir = PoseAxesBuilder::NormalizeVector(ToCTVec3D(screw.axialDir)); const CTVec3D robotCenter = eyeInHandTransform.transformPoint(eyeCenter); - const CTVec3D robotAxialDir = NormalizeVector(eyeInHandTransform.transformVector(eyeAxialDir)); + const CTVec3D robotAxialDir = PoseAxesBuilder::NormalizeVector(eyeInHandTransform.transformVector(eyeAxialDir)); // 接近点：Eye 系沿螺杆轴偏移 offset，再经 T 变换到 Robot 系；姿态与目标点共用。 const double offset = -extrinsic.approachOffset; @@ -622,40 +244,22 @@ int DetectPresenter::DetectScrew( const CTVec3D robotApproach = eyeInHandTransform.transformPoint(eyeApproach); approachEyeXY.emplace_back(eyeApproach.x, eyeApproach.y); - // 姿态：以螺杆轴为 X 轴构建正交三元组 → 标定补偿 → 变换到 Robot 系 → 提欧拉角。 + // 锚点式三元组：X = 螺杆轴向，Y = 法兰 Y 投影到 X 垂直面，Z = X × Y。 std::array eyeAxes; - if (!BuildFrameFromXAxis(eyeAxialDir, eyeAxes)) { - LOG_WARNING("[Algo Thread] Screw %zu: 无法由轴向构建坐标系，已跳过\n", i); + if (!PoseAxesBuilder::BuildAnchoredFrame(eyeAxialDir, flangeYInEye, eyeAxes)) { + LOG_WARNING("[Algo Thread] Screw %zu: 螺杆轴向与法兰 Y 锚点近共线，无法构造稳定副轴，已跳过\n", i); continue; } - if (debugParam.enableDebug && debugParam.printDetailLog) { - const CTRotationMatrix eyeRotationBefore = BuildRotationMatrix(eyeAxes); - double eyeRollBefore = 0.0; - double eyePitchBefore = 0.0; - double eyeYawBefore = 0.0; - RotationMatrixToConfiguredEulerDegrees(eyeRotationBefore, order, eyeRollBefore, eyePitchBefore, eyeYawBefore); - LOG_INFO("[Algo Thread] Screw %zu Eye Euler before compensation (order=%d): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n", - i, static_cast(order), eyeRollBefore, eyePitchBefore, eyeYawBefore); - } - ApplyAxesRotation(eyeAxes, extrinsic.rotX, extrinsic.rotY, extrinsic.rotZ); - if (debugParam.enableDebug && debugParam.printDetailLog) { - const CTRotationMatrix eyeRotationAfter = BuildRotationMatrix(eyeAxes); - double eyeRollAfter = 0.0; - double eyePitchAfter = 0.0; - double eyeYawAfter = 0.0; - RotationMatrixToConfiguredEulerDegrees(eyeRotationAfter, order, eyeRollAfter, eyePitchAfter, eyeYawAfter); - LOG_INFO("[Algo Thread] Screw %zu Eye Euler after compensation (order=%d): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n", - i, static_cast(order), eyeRollAfter, eyePitchAfter, eyeYawAfter); - } - std::array robotAxes; - if (!TransformAxes(eyeAxes, eyeInHandTransform, robotAxes)) { + + PoseAxesBuilder::PoseAngles angles; + PoseAxesBuilder::PoseDebugInfo debugInfo; + const bool poseOk = PoseAxesBuilder::ComputeRobotPoseAngles( + eyeAxes, eyeInHandTransform, extrinsic, order, rx, rz, + angles, needDebug ? &debugInfo : nullptr); + if (!poseOk) { LOG_WARNING("[Algo Thread] Screw %zu: 轴变换到机器人坐标系失败，已跳过\n", i); continue; } - // Robot 系下对工具三轴做后补偿：把"算法工具系"修正到"机器人期望 TCP 系"， - // 同时让最终输出欧拉角远离万向节锁；默认 0 不启用。 - ApplyAxesRotation(robotAxes, extrinsic.outRotX, extrinsic.outRotY, extrinsic.outRotZ); - const CTRotationMatrix rotation = BuildRotationMatrix(robotAxes); ScrewPosition pos; pos.x = robotCenter.x + extrinsic.offsetX; @@ -664,12 +268,9 @@ int DetectPresenter::DetectScrew( pos.approachX = robotApproach.x + extrinsic.offsetX; pos.approachY = robotApproach.y + extrinsic.offsetY; pos.approachZ = robotApproach.z + extrinsic.offsetZ; -#if 0 - ExtractStableZYXEulerDegrees(rotation, pos.roll, pos.pitch, pos.yaw, rx, rz); -#else - RotationMatrixToConfiguredEulerDegrees(rotation, order, pos.roll, pos.pitch, pos.yaw); - ResolveGimbalAmbiguity(pos.roll, pos.pitch, pos.yaw, rx, rz); -#endif + pos.roll = angles.rollDeg; + pos.pitch = angles.pitchDeg; + pos.yaw = angles.yawDeg; detectionResult.positions.push_back(pos); ScrewInfo info; @@ -682,7 +283,25 @@ int DetectPresenter::DetectScrew( info.rotateAngle = pos.roll; detectionResult.screwInfoList.push_back(info); - if (debugParam.enableDebug && debugParam.printDetailLog) { + if (needDebug) { + LOG_INFO("[Algo Thread] Screw %zu Eye Euler before compensation (order=%d): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n", + i, static_cast(order), + debugInfo.eyeEulerBefore.rollDeg, + debugInfo.eyeEulerBefore.pitchDeg, + debugInfo.eyeEulerBefore.yawDeg); + LOG_INFO("[Algo Thread] Screw %zu Eye Euler after compensation (order=%d): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n", + i, static_cast(order), + debugInfo.eyeEulerAfter.rollDeg, + debugInfo.eyeEulerAfter.pitchDeg, + debugInfo.eyeEulerAfter.yawDeg); + // 给 outRot 标定用：这是「outRot 补偿前」的 Robot 系姿态。 + // 把欧拉粘进 CloudView「工具→姿态补偿计算」的"当前"栏，期望填机械臂期望的姿态， + // 矩阵补偿 + XYZ 输出顺序，反解出来的就是 outRotX/Y/Z。 + LOG_INFO("[Algo Thread] Screw %zu Robot RPY (before outRot, calibration ref): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n", + i, + debugInfo.robotEulerBeforeOutRot.rollDeg, + debugInfo.robotEulerBeforeOutRot.pitchDeg, + debugInfo.robotEulerBeforeOutRot.yawDeg); LOG_INFO("[Algo Thread] Screw %zu Eye Coords: X=%.2f, Y=%.2f, Z=%.2f\n", i, screw.center.x, screw.center.y, screw.center.z); LOG_INFO("[Algo Thread] Screw %zu Robot Coords: X=%.2f, Y=%.2f, Z=%.2f, RPY=%.2f/%.2f/%.2f\n", i, pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw); LOG_INFO("[Algo Thread] Screw %zu Axial Dir Eye: X=%.3f, Y=%.3f, Z=%.3f\n", i, screw.axialDir.x, screw.axialDir.y, screw.axialDir.z); @@ -753,21 +372,21 @@ int DetectPresenter::DetectToolDisk( detectionResult.success = true; detectionResult.errorCode = 0; detectionResult.message = QStringLiteral("工具盘检测成功"); - const CTHomogeneousMatrix handEyeMatrix = BuildHandEyeMatrix(clibMatrix); + const CTHomogeneousMatrix handEyeMatrix = PoseAxesBuilder::BuildHandEyeMatrix(clibMatrix); const CTEulerOrder order = static_cast(extrinsic.eulerOrder); double rx, ry, rz; - ResolveRobotPoseAnglesDegrees(robotPose, poseOutputOrder, rx, ry, rz); - const CTRobotPose flangePose = CTRobotPose::fromDegrees( robotPose.x, robotPose.y, robotPose.z, rx, ry, rz); + PoseAxesBuilder::ResolveRobotPoseAnglesDegrees(robotPose, poseOutputOrder, rx, ry, rz); + const CTRobotPose flangePose = CTRobotPose::fromDegrees(robotPose.x, robotPose.y, robotPose.z, rx, ry, rz); // flangePose.rx/ry/rz 已经是真实轴角，可直接交给 sixAxisEyeInHandBuildTransform 按 eulerOrder 组装。 const CTHomogeneousMatrix eyeInHandTransform = CCoordinateTransform::sixAxisEyeInHandBuildTransform( flangePose, order, handEyeMatrix); // 将定位盘中心点通过手眼标定转换为机器人坐标 - const CTVec3D robotCenter = eyeInHandTransform.transformPoint(ToCTVec3D(poseInfo.center)); - const CTVec3D eyeXAxis = NormalizeVector(ToCTVec3D(poseInfo.xDir)); - const CTVec3D eyeYAxis = NormalizeVector(ToCTVec3D(poseInfo.yDir)); - const CTVec3D eyeNormalDir = NormalizeVector(ToCTVec3D(poseInfo.normalDir)); + const CTVec3D eyeCenter = ToCTVec3D(poseInfo.center); + const CTVec3D robotCenter = eyeInHandTransform.transformPoint(eyeCenter); + const CTVec3D eyeXAxis = PoseAxesBuilder::NormalizeVector(ToCTVec3D(poseInfo.xDir)); + const CTVec3D eyeYAxis = PoseAxesBuilder::NormalizeVector(ToCTVec3D(poseInfo.yDir)); ScrewPosition pos; pos.x = robotCenter.x + extrinsic.offsetX; @@ -775,65 +394,69 @@ int DetectPresenter::DetectToolDisk( pos.z = robotCenter.z + extrinsic.offsetZ; // 接近点：Eye 系沿工具盘 X 轴偏移 offset，再经 T 变换到 Robot 系；姿态与目标点共用。 const double offset = extrinsic.approachOffset; - const CTVec3D eyeCenter = ToCTVec3D(poseInfo.center); - const CTVec3D eyeApproach(eyeCenter.x + eyeXAxis.x * offset, eyeCenter.y + eyeXAxis.y * offset, eyeCenter.z + eyeXAxis.z * offset); + const CTVec3D eyeApproach(eyeCenter.x + eyeXAxis.x * offset, + eyeCenter.y + eyeXAxis.y * offset, + eyeCenter.z + eyeXAxis.z * offset); const CTVec3D robotApproach = eyeInHandTransform.transformPoint(eyeApproach); pos.approachX = robotApproach.x + extrinsic.offsetX; pos.approachY = robotApproach.y + extrinsic.offsetY; pos.approachZ = robotApproach.z + extrinsic.offsetZ; const std::pair approachEyeXY(eyeApproach.x, eyeApproach.y); - // 姿态：三轴直接取算法输出 → 标定补偿 → 变换到 Robot 系 → 提欧拉角。 - std::array eyeAxes = {eyeXAxis, eyeYAxis, eyeNormalDir}; - if (debugParam.enableDebug && debugParam.printDetailLog) { - const CTRotationMatrix eyeRotationBefore = BuildRotationMatrix(eyeAxes); - double eyeRollBefore = 0.0; - double eyePitchBefore = 0.0; - double eyeYawBefore = 0.0; - RotationMatrixToConfiguredEulerDegrees(eyeRotationBefore, order, eyeRollBefore, eyePitchBefore, eyeYawBefore); - LOG_INFO("[Algo Thread] ToolDisk Eye Euler before compensation (order=%d): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n", - static_cast(order), eyeRollBefore, eyePitchBefore, eyeYawBefore); + // 锚点式三元组：算法已给出完整 X/Y/Z，但仍统一走 BuildAnchoredFrame —— 它对算法输出做 + // Gram-Schmidt 正交化（算法本就正交时是恒等），保证后续欧拉提取数值稳定。 + std::array eyeAxes; + if (!PoseAxesBuilder::BuildAnchoredFrame(eyeXAxis, eyeYAxis, eyeAxes)) { + LOG_WARNING("[Algo Thread] ToolDisk: 算法 X/Y 轴近共线，无法构造稳定副轴\n"); + return ERR_CODE(DEV_DATA_INVALID); } - ApplyAxesRotation(eyeAxes, extrinsic.rotX, extrinsic.rotY, extrinsic.rotZ); - std::array robotAxes; - if (!TransformAxes(eyeAxes, eyeInHandTransform, robotAxes)) { + + const bool needDebug = debugParam.enableDebug && debugParam.printDetailLog; + PoseAxesBuilder::PoseAngles angles; + PoseAxesBuilder::PoseDebugInfo debugInfo; + if (!PoseAxesBuilder::ComputeRobotPoseAngles( + eyeAxes, eyeInHandTransform, extrinsic, order, rx, rz, + angles, needDebug ? &debugInfo : nullptr)) { LOG_WARNING("[Algo Thread] ToolDisk: 轴变换到机器人坐标系失败\n"); return ERR_CODE(DEV_DATA_INVALID); } - // Robot 系下对工具三轴做后补偿：把"算法工具系"修正到"机器人期望 TCP 系"， - // 同时让最终输出欧拉角远离万向节锁；默认 0 不启用。 - ApplyAxesRotation(robotAxes, extrinsic.outRotX, extrinsic.outRotY, extrinsic.outRotZ); - const CTRotationMatrix robotRotation = BuildRotationMatrix(robotAxes); -#if 0 - ExtractStableZYXEulerDegrees(robotRotation, pos.roll, pos.pitch, pos.yaw, rx, rz); -#else - RotationMatrixToConfiguredEulerDegrees(robotRotation, order, pos.roll, pos.pitch, pos.yaw); - ResolveGimbalAmbiguity(pos.roll, pos.pitch, pos.yaw, rx, rz); -#endif + pos.roll = angles.rollDeg; + pos.pitch = angles.pitchDeg; + pos.yaw = angles.yawDeg; detectionResult.positions.push_back(pos); - if (debugParam.enableDebug && debugParam.printDetailLog) { - const CTRotationMatrix eyeRotation = BuildRotationMatrix(eyeAxes); - double eyeRoll = 0.0; - double eyePitch = 0.0; - double eyeYaw = 0.0; - RotationMatrixToConfiguredEulerDegrees(eyeRotation, order, eyeRoll, eyePitch, eyeYaw); + if (needDebug) { + const CTRotationMatrix& eyeRotationAfter = debugInfo.eyeRotationAfter; + const CTRotationMatrix& robotRotation = debugInfo.robotRotation; LOG_INFO("[Algo Thread] === Tool Disk Pose Debug ===\n"); - LOG_INFO("[Algo Thread] Eye Rotation Matrix:\n"); - LOG_INFO(" [%.6f, %.6f, %.6f]\n", eyeRotation.at(0, 0), eyeRotation.at(0, 1), eyeRotation.at(0, 2)); - LOG_INFO(" [%.6f, %.6f, %.6f]\n", eyeRotation.at(1, 0), eyeRotation.at(1, 1), eyeRotation.at(1, 2)); - LOG_INFO(" [%.6f, %.6f, %.6f]\n", eyeRotation.at(2, 0), eyeRotation.at(2, 1), eyeRotation.at(2, 2)); - LOG_INFO("[Algo Thread] Eye Pos: (%.6f, %.6f, %.6f)\n", poseInfo.center.x, poseInfo.center.y, poseInfo.center.z); - LOG_INFO("[Algo Thread] Eye Euler after compensation (order=%d ): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n", - static_cast(order), eyeRoll, eyePitch, eyeYaw); + LOG_INFO("[Algo Thread] ToolDisk Eye Euler before compensation (order=%d): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n", + static_cast(order), + debugInfo.eyeEulerBefore.rollDeg, + debugInfo.eyeEulerBefore.pitchDeg, + debugInfo.eyeEulerBefore.yawDeg); + LOG_INFO("[Algo Thread] Eye Rotation Matrix (after compensation):\n"); + LOG_INFO(" [%.6f, %.6f, %.6f]\n", eyeRotationAfter.at(0, 0), eyeRotationAfter.at(0, 1), eyeRotationAfter.at(0, 2)); + LOG_INFO(" [%.6f, %.6f, %.6f]\n", eyeRotationAfter.at(1, 0), eyeRotationAfter.at(1, 1), eyeRotationAfter.at(1, 2)); + LOG_INFO(" [%.6f, %.6f, %.6f]\n", eyeRotationAfter.at(2, 0), eyeRotationAfter.at(2, 1), eyeRotationAfter.at(2, 2)); + LOG_INFO("[Algo Thread] Eye Pos: (%.6f, %.6f, %.6f)\n", poseInfo.center.x, poseInfo.center.y, poseInfo.center.z); + LOG_INFO("[Algo Thread] Eye Euler after compensation (order=%d): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n", + static_cast(order), + debugInfo.eyeEulerAfter.rollDeg, + debugInfo.eyeEulerAfter.pitchDeg, + debugInfo.eyeEulerAfter.yawDeg); LOG_INFO("[Algo Thread] Robot Rotation Matrix:\n"); LOG_INFO(" [%.6f, %.6f, %.6f]\n", robotRotation.at(0, 0), robotRotation.at(0, 1), robotRotation.at(0, 2)); LOG_INFO(" [%.6f, %.6f, %.6f]\n", robotRotation.at(1, 0), robotRotation.at(1, 1), robotRotation.at(1, 2)); LOG_INFO(" [%.6f, %.6f, %.6f]\n", robotRotation.at(2, 0), robotRotation.at(2, 1), robotRotation.at(2, 2)); - LOG_INFO("[Algo Thread] Robot Pos: (%.6f, %.6f, %.6f)\n", robotCenter.x, robotCenter.y, robotCenter.z); - LOG_INFO("[Algo Thread] Robot Pos Euler (order=%d ): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n", + LOG_INFO("[Algo Thread] Robot Pos: (%.6f, %.6f, %.6f)\n", robotCenter.x, robotCenter.y, robotCenter.z); + // 给 outRot 标定用：outRot 补偿前的 Robot 系欧拉（粘进 CloudView「姿态补偿」"当前"栏） + LOG_INFO("[Algo Thread] ToolDisk Robot RPY (before outRot, calibration ref): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n", + debugInfo.robotEulerBeforeOutRot.rollDeg, + debugInfo.robotEulerBeforeOutRot.pitchDeg, + debugInfo.robotEulerBeforeOutRot.yawDeg); + LOG_INFO("[Algo Thread] Robot Pos Euler (order=%d): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n", static_cast(order), pos.roll, pos.pitch, pos.yaw); LOG_INFO("[Algo Thread] ToolDisk Approach (offset=%.2f): X=%.2f, Y=%.2f, Z=%.2f\n", offset, pos.approachX, pos.approachY, pos.approachZ); diff --git a/App/ScrewPosition/ScrewPositionApp/Presenter/Src/PoseAxesBuilder.cpp b/App/ScrewPosition/ScrewPositionApp/Presenter/Src/PoseAxesBuilder.cpp new file mode 100644 index 00000000..06a9b2b9 --- /dev/null +++ b/App/ScrewPosition/ScrewPositionApp/Presenter/Src/PoseAxesBuilder.cpp @@ -0,0 +1,360 @@ +#include "PoseAxesBuilder.h" + +#include "CoordinateTransform.h" +#include "DetectPresenter.h" // for HandEyeExtrinsic +#include "ScrewPositionTCPProtocol.h" // for RobotPose6D + +#include + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +namespace { + +constexpr double kVectorEpsilon = 1e-8; +constexpr double kRotationEpsilon = 1e-9; + +double DotProduct(const CTVec3D& a, const CTVec3D& b) +{ + return a.x * b.x + a.y * b.y + a.z * b.z; +} + +CTVec3D CrossProduct(const CTVec3D& a, const CTVec3D& b) +{ + return CTVec3D(a.y * b.z - a.z * b.y, + a.z * b.x - a.x * b.z, + a.x * b.y - a.y * b.x); +} + +double WrapDegreesTo180(double deg) +{ + while (deg > 180.0) { + deg -= 360.0; + } + while (deg <= -180.0) { + deg += 360.0; + } + return deg; +} + +// 对 Tait-Bryan 欧拉角做等价归一化，使 pitch ∈ [-90°, 90°]。 +// 依据：(α, β, γ) ≡ (α±180°, 180°-β, γ±180°) 以及 (α±180°, -180°-β, γ±180°)， +// 适用于 CTEulerOrder 中所有 12 种 Tait-Bryan 顺序（内旋/外旋一致）。 +void NormalizePitchRange(double& rollDeg, double& pitchDeg, double& yawDeg) +{ + pitchDeg = WrapDegreesTo180(pitchDeg); + if (pitchDeg > 90.0) { + pitchDeg = 180.0 - pitchDeg; + rollDeg += 180.0; + yawDeg += 180.0; + } else if (pitchDeg < -90.0) { + pitchDeg = -180.0 - pitchDeg; + rollDeg += 180.0; + yawDeg += 180.0; + } + rollDeg = WrapDegreesTo180(rollDeg); + yawDeg = WrapDegreesTo180(yawDeg); +} + +} // namespace + +namespace PoseAxesBuilder +{ + +CTVec3D NormalizeVector(const CTVec3D& v) +{ + const double length = v.norm(); + if (length < kVectorEpsilon) { + return CTVec3D(); + } + return v * (1.0 / length); +} + +bool IsValidVector(const CTVec3D& v) +{ + return v.norm() >= kVectorEpsilon; +} + +CTHomogeneousMatrix BuildHandEyeMatrix(const double clibMatrix[16]) +{ + CTHomogeneousMatrix handEyeMatrix; + for (int row = 0; row < 4; ++row) { + for (int col = 0; col < 4; ++col) { + handEyeMatrix.at(row, col) = clibMatrix[row * 4 + col]; + } + } + return handEyeMatrix; +} + +CTVec3D FlangeAxisToEye(const CTHomogeneousMatrix& handEyeMatrix, + const CTVec3D& flangeLocalAxis) +{ + // handEyeMatrix: T_flange_camera，旋转部分 R 把 Eye 系向量映射到 Flange 系。 + // 反向：把 Flange 系向量映射到 Eye 系 = R^T·v。 + const CTRotationMatrix rotInv = handEyeMatrix.getRotation().transpose(); + return NormalizeVector(rotInv.transform(flangeLocalAxis)); +} + +void ResolveRobotPoseAnglesDegrees(const RobotPose6D& robotPose, + int poseOutputOrder, + double& rxDeg, + double& ryDeg, + double& rzDeg) +{ + switch (poseOutputOrder) { + case 1: + rxDeg = robotPose.a; + ryDeg = robotPose.b; + rzDeg = robotPose.c; + break; + case 2: + rxDeg = robotPose.b; + ryDeg = robotPose.a; + rzDeg = robotPose.c; + break; + case 3: + rxDeg = robotPose.c; + ryDeg = robotPose.a; + rzDeg = robotPose.b; + break; + case 4: + rxDeg = robotPose.b; + ryDeg = robotPose.c; + rzDeg = robotPose.a; + break; + case 5: + rxDeg = robotPose.c; + ryDeg = robotPose.b; + rzDeg = robotPose.a; + break; + case 0: + default: + rxDeg = robotPose.a; + ryDeg = robotPose.b; + rzDeg = robotPose.c; + break; + } +} + +bool BuildAnchoredFrame(const CTVec3D& primary, + const CTVec3D& referenceY, + std::array& axes, + double minPerpendicularity) +{ + const CTVec3D xAxis = NormalizeVector(primary); + if (!IsValidVector(xAxis)) { + return false; + } + const CTVec3D refY = NormalizeVector(referenceY); + if (!IsValidVector(refY)) { + return false; + } + + // Gram-Schmidt：refY 投影到与 X 垂直的平面。 + // |yRaw| = sin(refY 与 X 的夹角)。小于阈值则两者近共线、副轴方向无意义。 + const CTVec3D yRaw = refY - xAxis * DotProduct(refY, xAxis); + if (yRaw.norm() < minPerpendicularity) { + return false; + } + const CTVec3D yAxis = NormalizeVector(yRaw); + const CTVec3D zAxis = NormalizeVector(CrossProduct(xAxis, yAxis)); + if (!IsValidVector(zAxis)) { + return false; + } + + axes = {xAxis, yAxis, zAxis}; + return true; +} + +void ApplyAxesRotation(std::array& axes, + double rotXDeg, double rotYDeg, double rotZDeg) +{ + if (std::fabs(rotXDeg) < kRotationEpsilon && + std::fabs(rotYDeg) < kRotationEpsilon && + std::fabs(rotZDeg) < kRotationEpsilon) { + return; + } + + const double rx = rotXDeg * M_PI / 180.0; + const double ry = rotYDeg * M_PI / 180.0; + const double rz = rotZDeg * M_PI / 180.0; + + const double cx = std::cos(rx), sx = std::sin(rx); + const double cy = std::cos(ry), sy = std::sin(ry); + const double cz = std::cos(rz), sz = std::sin(rz); + + // R = Rx(rx) * Ry(ry) * Rz(rz)；对工具三轴（列向量）右乘 R，等价于在工具自身坐标系 + // 内按内旋 XYZ 顺序施加补偿旋转。 + double R[3][3]; + R[0][0] = cy * cz; + R[0][1] = -cy * sz; + R[0][2] = sy; + R[1][0] = sx * sy * cz + cx * sz; + R[1][1] = -sx * sy * sz + cx * cz; + R[1][2] = -sx * cy; + R[2][0] = -cx * sy * cz + sx * sz; + R[2][1] = cx * sy * sz + sx * cz; + R[2][2] = cx * cy; + + const CTVec3D oldX = axes[0]; + const CTVec3D oldY = axes[1]; + const CTVec3D oldZ = axes[2]; + + axes[0] = oldX * R[0][0] + oldY * R[1][0] + oldZ * R[2][0]; + axes[1] = oldX * R[0][1] + oldY * R[1][1] + oldZ * R[2][1]; + axes[2] = oldX * R[0][2] + oldY * R[1][2] + oldZ * R[2][2]; +} + +bool TransformAxes(const std::array& srcAxes, + const CTHomogeneousMatrix& transform, + std::array& dstAxes) +{ + for (size_t i = 0; i < srcAxes.size(); ++i) { + dstAxes[i] = NormalizeVector(transform.transformVector(srcAxes[i])); + if (!IsValidVector(dstAxes[i])) { + return false; + } + } + return true; +} + +CTRotationMatrix BuildRotationMatrix(const std::array& axes) +{ + CTRotationMatrix rotation; + rotation.at(0, 0) = axes[0].x; + rotation.at(0, 1) = axes[1].x; + rotation.at(0, 2) = axes[2].x; + rotation.at(1, 0) = axes[0].y; + rotation.at(1, 1) = axes[1].y; + rotation.at(1, 2) = axes[2].y; + rotation.at(2, 0) = axes[0].z; + rotation.at(2, 1) = axes[1].z; + rotation.at(2, 2) = axes[2].z; + return rotation; +} + +void RotationMatrixToConfiguredEulerDegrees(const CTRotationMatrix& rotation, + CTEulerOrder order, + double& rollDeg, + double& pitchDeg, + double& yawDeg) +{ + const CTEulerAngles euler = CCoordinateTransform::rotationMatrixToEuler(rotation, order); + euler.toDegrees(rollDeg, pitchDeg, yawDeg); + NormalizePitchRange(rollDeg, pitchDeg, yawDeg); +} + +// 万向锁消歧：当 |pitch| 接近 90° 时，Rz(yaw)·Ry(pitch)·Rx(roll) 的分解退化成一个自由度。 +// 严格展开可得： +// pitch = +90° → R 只依赖 (roll - yaw)；任意满足 roll - yaw = const 的组合都表示同一 R +// pitch = -90° → R 只依赖 (roll + yaw)；任意满足 roll + yaw = const 的组合都表示同一 R +// 本函数把 yaw 锚定到 refYaw 附近，把剩余自由度放到 roll，让不同帧的输出稳定落在 +// 参考（通常是机器人法兰的 rx/rz）附近。阈值覆盖 80°-85°： +// |pitch| < 80°：完全不改（保留真实分解） +// |pitch| ≥ 85°：完全锁到参考（对 R 影响最小） +// 80° ≤ |pitch| < 85°：线性插值过渡 +void ResolveGimbalAmbiguity(double& rollDeg, double pitchDeg, double& yawDeg, + double refRollDeg, double refYawDeg) +{ + constexpr double kSoftThresholdDeg = 80.0; + constexpr double kHardThresholdDeg = 85.0; + const double absPitch = std::abs(pitchDeg); + if (absPitch < kSoftThresholdDeg) { + return; + } + + // 不变量方向与 pitch 符号的关系（由 Rz·Ry·Rx 展开式导出）： + // pitch > 0 → inv = roll - yaw + // pitch < 0 → inv = roll + yaw + const bool positivePitch = (pitchDeg >= 0.0); + const double sum = positivePitch ? (rollDeg - yawDeg) : (rollDeg + yawDeg); + const double refSum = positivePitch ? (refRollDeg - refYawDeg) : (refRollDeg + refYawDeg); + + double adjustedSum = sum; + while (adjustedSum - refSum > 180.0) { adjustedSum -= 360.0; } + while (adjustedSum - refSum < -180.0) { adjustedSum += 360.0; } + + const double fullYaw = WrapDegreesTo180(refYawDeg); + const double fullRoll = WrapDegreesTo180(positivePitch ? (adjustedSum + fullYaw) + : (adjustedSum - fullYaw)); + + if (absPitch >= kHardThresholdDeg) { + rollDeg = fullRoll; + yawDeg = fullYaw; + return; + } + + const double alpha = (absPitch - kSoftThresholdDeg) / + (kHardThresholdDeg - kSoftThresholdDeg); + auto lerpAngle = [alpha](double from, double to) { + const double diff = WrapDegreesTo180(to - from); + return WrapDegreesTo180(from + alpha * diff); + }; + rollDeg = lerpAngle(rollDeg, fullRoll); + yawDeg = lerpAngle(yawDeg, fullYaw); +} + +bool ComputeRobotPoseAngles(const std::array& eyeAxes, + const CTHomogeneousMatrix& eyeInHandTransform, + const HandEyeExtrinsic& extrinsic, + CTEulerOrder eulerOrder, + double refRollDeg, + double refYawDeg, + PoseAngles& outAngles, + PoseDebugInfo* outDebugInfo) +{ + std::array eyeAxesWorking = eyeAxes; + + if (outDebugInfo) { + const CTRotationMatrix rEyeBefore = BuildRotationMatrix(eyeAxesWorking); + RotationMatrixToConfiguredEulerDegrees(rEyeBefore, eulerOrder, + outDebugInfo->eyeEulerBefore.rollDeg, + outDebugInfo->eyeEulerBefore.pitchDeg, + outDebugInfo->eyeEulerBefore.yawDeg); + } + + // 1) Eye 系内补偿 + ApplyAxesRotation(eyeAxesWorking, extrinsic.rotX, extrinsic.rotY, extrinsic.rotZ); + + if (outDebugInfo) { + outDebugInfo->eyeRotationAfter = BuildRotationMatrix(eyeAxesWorking); + RotationMatrixToConfiguredEulerDegrees(outDebugInfo->eyeRotationAfter, eulerOrder, + outDebugInfo->eyeEulerAfter.rollDeg, + outDebugInfo->eyeEulerAfter.pitchDeg, + outDebugInfo->eyeEulerAfter.yawDeg); + } + + // 2) Eye → Robot 变换 + std::array robotAxes; + if (!TransformAxes(eyeAxesWorking, eyeInHandTransform, robotAxes)) { + return false; + } + + // 2.5) outRot 补偿之前的 Robot 系姿态：这是 CloudView「姿态补偿」工具应当读到的「当前」。 + // 把日志里打印的欧拉粘进"当前"栏，期望填机械臂期望的姿态，矩阵补偿 + XYZ 输出顺序， + // 反解出来的就是 outRotX/Y/Z。 + if (outDebugInfo) { + const CTRotationMatrix rRobotBeforeOutRot = BuildRotationMatrix(robotAxes); + RotationMatrixToConfiguredEulerDegrees(rRobotBeforeOutRot, eulerOrder, + outDebugInfo->robotEulerBeforeOutRot.rollDeg, + outDebugInfo->robotEulerBeforeOutRot.pitchDeg, + outDebugInfo->robotEulerBeforeOutRot.yawDeg); + } + + // 3) Robot 系内补偿 + ApplyAxesRotation(robotAxes, extrinsic.outRotX, extrinsic.outRotY, extrinsic.outRotZ); + + // 4) 提欧拉角 + 万向锁消歧 + const CTRotationMatrix robotRotation = BuildRotationMatrix(robotAxes); + if (outDebugInfo) { + outDebugInfo->robotRotation = robotRotation; + } + RotationMatrixToConfiguredEulerDegrees(robotRotation, eulerOrder, + outAngles.rollDeg, outAngles.pitchDeg, outAngles.yawDeg); + ResolveGimbalAmbiguity(outAngles.rollDeg, outAngles.pitchDeg, outAngles.yawDeg, + refRollDeg, refYawDeg); + return true; +} + +} // namespace PoseAxesBuilder diff --git a/App/ScrewPosition/ScrewPositionApp/ScrewPositionApp.pro b/App/ScrewPosition/ScrewPositionApp/ScrewPositionApp.pro index f174a273..5c5f254b 100644 --- a/App/ScrewPosition/ScrewPositionApp/ScrewPositionApp.pro +++ b/App/ScrewPosition/ScrewPositionApp/ScrewPositionApp.pro @@ -53,6 +53,7 @@ SOURCES += \ Presenter/Src/ConfigManager.cpp \ Presenter/Src/ScrewPositionPresenter.cpp \ Presenter/Src/DetectPresenter.cpp \ + Presenter/Src/PoseAxesBuilder.cpp \ Presenter/Src/ScrewPositionTCPProtocol.cpp \ Presenter/Src/TCPServerMethods.cpp \ main.cpp \ @@ -65,6 +66,7 @@ HEADERS += \ Presenter/Inc/ConfigManager.h \ Presenter/Inc/ScrewPositionPresenter.h \ Presenter/Inc/DetectPresenter.h \ + Presenter/Inc/PoseAxesBuilder.h \ Presenter/Inc/ScrewPositionTCPProtocol.h \ IYScrewPositionStatus.h \ mainwindow.h \ diff --git a/App/ScrewPosition/ScrewPositionApp/Version.h b/App/ScrewPosition/ScrewPositionApp/Version.h index f7ef6558..90e387f2 100644 --- a/App/ScrewPosition/ScrewPositionApp/Version.h +++ b/App/ScrewPosition/ScrewPositionApp/Version.h @@ -3,7 +3,7 @@ #define SCREWPOSITION_APP_NAME "螺杆定位" -#define SCREWPOSITION_VERSION_STRING "1.1.10" +#define SCREWPOSITION_VERSION_STRING "1.1.11" #define SCREWPOSITION_BUILD_STRING "1" #define SCREWPOSITION_FULL_VERSION_STRING "V" SCREWPOSITION_VERSION_STRING "_" SCREWPOSITION_BUILD_STRING diff --git a/App/ScrewPosition/ScrewPositionApp/Version.md b/App/ScrewPosition/ScrewPositionApp/Version.md index 115591f8..daf7aa2c 100644 --- a/App/ScrewPosition/ScrewPositionApp/Version.md +++ b/App/ScrewPosition/ScrewPositionApp/Version.md @@ -1,3 +1,11 @@ +# 1.1.11 2026-05-22 +## build_1 +1. 修改螺杆的手眼转换 + +# 1.1.10 2026-05-15 +## build_1 +1. 更新算法：螺杆检测 + # 1.1.10 2026-05-15 ## build_1 1. 更新算法：螺杆检测 diff --git a/App/WorkpieceHole/WorkpieceHoleApp/Version.h b/App/WorkpieceHole/WorkpieceHoleApp/Version.h index a2ba41bf..d78407c2 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/Version.h +++ b/App/WorkpieceHole/WorkpieceHoleApp/Version.h @@ -5,7 +5,7 @@ #define WORKPIECEHOLE_APP_NAME "工件孔定位" // 版本字符串 -#define WORKPIECEHOLE_VERSION_STRING "1.1.5" +#define WORKPIECEHOLE_VERSION_STRING "1.1.6" #define WORKPIECEHOLE_BUILD_STRING "1" #define WORKPIECEHOLE_FULL_VERSION_STRING "V" WORKPIECEHOLE_VERSION_STRING "_" WORKPIECEHOLE_BUILD_STRING diff --git a/App/WorkpieceHole/WorkpieceHoleApp/Version.md b/App/WorkpieceHole/WorkpieceHoleApp/Version.md index fba60e5d..9cdd22a4 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/Version.md +++ b/App/WorkpieceHole/WorkpieceHoleApp/Version.md @@ -1,3 +1,8 @@ +# 1.1.6 +## build_1 2026-05-19 +1. 更新算法：修正一层的时候无结果 + + # 1.1.5 ## build_1 2026-05-05 1. 更新算法：异物检测修正 diff --git a/AppAlgo/workpieceHolePositioning/Arm/aarch64/libbaseAlgorithm.so b/AppAlgo/workpieceHolePositioning/Arm/aarch64/libbaseAlgorithm.so index 39f579a7..744760d5 100644 Binary files a/AppAlgo/workpieceHolePositioning/Arm/aarch64/libbaseAlgorithm.so and b/AppAlgo/workpieceHolePositioning/Arm/aarch64/libbaseAlgorithm.so differ diff --git a/AppAlgo/workpieceHolePositioning/Arm/aarch64/libworkpieceHolePositioning.so b/AppAlgo/workpieceHolePositioning/Arm/aarch64/libworkpieceHolePositioning.so index 8dcf3301..93b34f09 100644 Binary files a/AppAlgo/workpieceHolePositioning/Arm/aarch64/libworkpieceHolePositioning.so and b/AppAlgo/workpieceHolePositioning/Arm/aarch64/libworkpieceHolePositioning.so differ diff --git a/AppAlgo/workpieceHolePositioning/Inc/SG_baseAlgo_Export.h b/AppAlgo/workpieceHolePositioning/Inc/SG_baseAlgo_Export.h index a05da66b..0920d683 100644 --- a/AppAlgo/workpieceHolePositioning/Inc/SG_baseAlgo_Export.h +++ b/AppAlgo/workpieceHolePositioning/Inc/SG_baseAlgo_Export.h @@ -166,6 +166,14 @@ SG_APISHARED_EXPORT void wd_lineDataSegment_dist( const int minSegSize ); +//ݾԷֶ +SG_APISHARED_EXPORT void wd_lineDataSegment_dist_2( + std::vector< SVzNL3DPosition>& lineData, + std::vector& segs, + const double maxDistTh, + const int minSegSize +); + SG_APISHARED_EXPORT void wd_lineDataSegment_zDist( std::vector< SVzNL3DPosition>& lineData, std::vector< SVzNL3DPosition>& vldPts, @@ -205,6 +213,29 @@ SG_APISHARED_EXPORT void wd_getRodArcFeature_peakCornerMethod( std::vector& line_rodArcs // ); +/// +/// ȡϵԲϰλ +/// seg˵㣺z +/// nPointIdx¶Feature +/// 㷨̣ +/// 1μǰǺͺ +/// 2ͬĹս +/// 2Zֵ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +/// +SG_APISHARED_EXPORT void wd_getRodArcFeature_segmentPeakCornerMethod( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + const double maxDistTh, + const int minSegSize, + const double peakChkWin, + const SSG_cornerParam cornerPara, + std::vector& line_rodArcs // +); + //ȡcornerֵʵֺʹô˺дŻ SG_APISHARED_EXPORT void wd_searchCornerPeaks( std::vector< SSG_pntDirAngle>& corners, diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.dll b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.dll index 80c4a405..ccc66155 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.dll and b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.dll differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.lib b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.lib index 64aac627..51bacf92 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.lib and b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.lib differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.pdb b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.pdb index 968a7e03..df5bada0 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.pdb and b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.pdb differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.dll b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.dll index ee4d7f16..7da16bc3 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.dll and b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.dll differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.pdb b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.pdb index a79647f5..989ce0c2 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.pdb and b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.pdb differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.dll b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.dll index 4ea60400..7a70f348 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.dll and b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.dll differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.lib b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.lib index 08279904..ae4fd837 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.lib and b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.lib differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.pdb b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.pdb index e63a47b1..53a84aac 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.pdb and b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.pdb differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.dll b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.dll index 9488ec88..072e965d 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.dll and b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.dll differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.pdb b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.pdb index 202728b8..55e8219f 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.pdb and b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.pdb differ diff --git a/AppAlgo/workpieceHolePositioning/workpieceHolePositioning_test.cpp b/AppAlgo/workpieceHolePositioning/workpieceHolePositioning_test.cpp index c2dba1ed..410602d0 100644 --- a/AppAlgo/workpieceHolePositioning/workpieceHolePositioning_test.cpp +++ b/AppAlgo/workpieceHolePositioning/workpieceHolePositioning_test.cpp @@ -566,7 +566,7 @@ void TuoPuFa_holePosition_test(void) }; SVzNLRange fileIdx[TPF_TEST_GROUP] = { - {6,6}, {1, 16}, {1,6} + {6,6}, {1, 16}, {1,19} }; const char* ver = wd_workpieceHolePositioningVersion();