修正螺杆的手眼转换; 工件孔的更新算法
This commit is contained in:
杰仔
parent
fde0fb3eee
commit
c745c36743
@ -0,0 +1,132 @@
|
||||
#pragma once
|
||||
|
||||
#include "CoordinateTypes.h"
|
||||
#include <array>
|
||||
|
||||
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<CTVec3D, 3>& axes,
|
||||
double minPerpendicularity = 0.1);
|
||||
|
||||
// ============ 三元组运算 ============
|
||||
|
||||
// 在三轴自身坐标系内施加 R = Rx(rotX)·Ry(rotY)·Rz(rotZ)（角度）。
|
||||
// 物理含义：把"算法工具系"二次旋转到"机器人期望 TCP 系"或远离万向锁的位姿。
|
||||
void ApplyAxesRotation(std::array<CTVec3D, 3>& axes,
|
||||
double rotXDeg, double rotYDeg, double rotZDeg);
|
||||
|
||||
// 把 srcAxes 经齐次变换的旋转部分搬到 dstAxes（不平移），并归一化。
|
||||
// 任一轴归一化失败返回 false。
|
||||
bool TransformAxes(const std::array<CTVec3D, 3>& srcAxes,
|
||||
const CTHomogeneousMatrix& transform,
|
||||
std::array<CTVec3D, 3>& dstAxes);
|
||||
|
||||
// 把三轴当列向量组装成 3x3 旋转矩阵。
|
||||
CTRotationMatrix BuildRotationMatrix(const std::array<CTVec3D, 3>& 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<CTVec3D, 3>& eyeAxes,
|
||||
const CTHomogeneousMatrix& eyeInHandTransform,
|
||||
const HandEyeExtrinsic& extrinsic,
|
||||
CTEulerOrder eulerOrder,
|
||||
double refRollDeg,
|
||||
double refYawDeg,
|
||||
PoseAngles& outAngles,
|
||||
PoseDebugInfo* outDebugInfo = nullptr);
|
||||
|
||||
} // namespace PoseAxesBuilder
|
||||
@ -1,21 +1,14 @@
|
||||
#include "DetectPresenter.h"
|
||||
#include "AlgorithmParamConverter.h"
|
||||
#include "CoordinateTransform.h"
|
||||
#include "PoseAxesBuilder.h"
|
||||
#include "ScrewPositionTCPProtocol.h"
|
||||
#include "rodAndBarDetection_Export.h"
|
||||
#include <QColor>
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
|
||||
#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<std::vector<SVzNL3DPosition>>& xyzData,
|
||||
const std::vector<SSX_rodPoseInfo>& screwInfo,
|
||||
const std::vector<std::pair<double, double>>* approachXY = nullptr)
|
||||
@ -67,7 +60,6 @@ QImage BuildToolDiskPointCloudImage(const std::vector<std::vector<SVzNL3DPositio
|
||||
const QColor normalColor(0, 180, 255); // 蓝色：法向 Z
|
||||
const QColor xAxisColor(255, 0, 0); // 红色：X 轴
|
||||
const QColor yAxisColor(0, 255, 0); // 绿色：Y 轴
|
||||
const QColor textColor(255, 255, 0);
|
||||
const QColor approachColor(255, 128, 0); // 橙色：接近点
|
||||
|
||||
// 绘制定位盘中心点
|
||||
@ -114,389 +106,11 @@ void SaveDebugImageIfNeeded(int cameraIndex,
|
||||
image.save(QString::fromStdString(fileName));
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
// 将角度归一化到 (-180°, 180°]
|
||||
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);
|
||||
}
|
||||
|
||||
// 统一走 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<CTVec3D, 3>& 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<CTVec3D, 3>& 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<CTVec3D, 3>& eyeAxes,
|
||||
const CTHomogeneousMatrix& transform,
|
||||
std::array<CTVec3D, 3>& 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<CTVec3D, 3>& 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<CTEulerOrder>(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<int>(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<std::pair<double, double>> 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<CTVec3D, 3> 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<int>(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<int>(order), eyeRollAfter, eyePitchAfter, eyeYawAfter);
|
||||
}
|
||||
std::array<CTVec3D, 3> 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<int>(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<int>(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<CTEulerOrder>(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<double, double> approachEyeXY(eyeApproach.x, eyeApproach.y);
|
||||
|
||||
// 姿态：三轴直接取算法输出 → 标定补偿 → 变换到 Robot 系 → 提欧拉角。
|
||||
std::array<CTVec3D, 3> 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<int>(order), eyeRollBefore, eyePitchBefore, eyeYawBefore);
|
||||
// 锚点式三元组：算法已给出完整 X/Y/Z，但仍统一走 BuildAnchoredFrame —— 它对算法输出做
|
||||
// Gram-Schmidt 正交化（算法本就正交时是恒等），保证后续欧拉提取数值稳定。
|
||||
std::array<CTVec3D, 3> 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<CTVec3D, 3> 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<int>(order), eyeRoll, eyePitch, eyeYaw);
|
||||
LOG_INFO("[Algo Thread] ToolDisk Eye Euler before compensation (order=%d): Roll=%.3f, Pitch=%.3f, Yaw=%.3f\n",
|
||||
static_cast<int>(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<int>(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<int>(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);
|
||||
|
||||
@ -0,0 +1,360 @@
|
||||
#include "PoseAxesBuilder.h"
|
||||
|
||||
#include "CoordinateTransform.h"
|
||||
#include "DetectPresenter.h" // for HandEyeExtrinsic
|
||||
#include "ScrewPositionTCPProtocol.h" // for RobotPose6D
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#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<CTVec3D, 3>& 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<CTVec3D, 3>& 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<CTVec3D, 3>& srcAxes,
|
||||
const CTHomogeneousMatrix& transform,
|
||||
std::array<CTVec3D, 3>& 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<CTVec3D, 3>& 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<CTVec3D, 3>& eyeAxes,
|
||||
const CTHomogeneousMatrix& eyeInHandTransform,
|
||||
const HandEyeExtrinsic& extrinsic,
|
||||
CTEulerOrder eulerOrder,
|
||||
double refRollDeg,
|
||||
double refYawDeg,
|
||||
PoseAngles& outAngles,
|
||||
PoseDebugInfo* outDebugInfo)
|
||||
{
|
||||
std::array<CTVec3D, 3> 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<CTVec3D, 3> 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
|
||||
@ -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 \
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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. 更新算法：螺杆检测
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -1,3 +1,8 @@
|
||||
# 1.1.6
|
||||
## build_1 2026-05-19
|
||||
1. 更新算法：修正一层的时候无结果
|
||||
|
||||
|
||||
# 1.1.5
|
||||
## build_1 2026-05-05
|
||||
1. 更新算法：异物检测修正
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@ -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<SSG_RUN>& 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<SWD_rodArcFeature>& line_rodArcs //环
|
||||
);
|
||||
|
||||
/// <summary>
|
||||
/// 提取激光线上的圆环的上半段弧。
|
||||
/// seg端点：z距离大于门限
|
||||
/// nPointIdx被重新定义成Feature类型
|
||||
/// 算法流程：
|
||||
/// （1）逐段计算前向角和后向角
|
||||
/// （2）搜索同方向的拐角连续段
|
||||
/// （2）搜索Z极值，
|
||||
/// （2）逐点计算拐角，顺时针为负，逆时针为正
|
||||
/// （3）搜索正拐角的极大值。
|
||||
/// （4）判断拐角是否为跳变
|
||||
///
|
||||
/// </summary>
|
||||
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<SWD_rodArcFeature>& line_rodArcs //环
|
||||
);
|
||||
|
||||
//提取corner极值（较早实现函数可以使用此函数进行代码优化）
|
||||
SG_APISHARED_EXPORT void wd_searchCornerPeaks(
|
||||
std::vector< SSG_pntDirAngle>& corners,
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -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();
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user