133 lines
6.3 KiB
C++
133 lines
6.3 KiB
C++
#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
|