137 lines
6.1 KiB
C
137 lines
6.1 KiB
C
|
#pragma once
|
|||
|
|
|
|||
|
|
#include "CoordinateTypes.h"
|
|||
|
|
#include "SG_baseDataType.h"
|
|||
|
|
|
|||
|
|
#include <array>
|
|||
|
|
|
|||
|
|
struct HandEyeExtrinsic;
|
|||
|
|
struct RobotPose6D;
|
|||
|
|
|
|||
|
|
// 砂轮盘孔 / 砂轮盘架子姿态计算工具集。
|
|||
|
|
//
|
|||
|
|
// 解决两类问题：
|
|||
|
|
// 1) 当主轴接近世界 Z 时，原 Y = Z_world x 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 matrix16[16]);
|
|||
|
|
|
|||
|
|
// 按 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 x 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);
|
|||
|
|
|
|||
|
|
// ============ 完整流水线（DetectDiscHole / DetectDiscRack 共用）============
|
|||
|
|
|
|||
|
|
struct PoseAngles
|
|||
|
|
{
|
|||
|
|
double rollDeg = 0.0;
|
|||
|
|
double pitchDeg = 0.0;
|
|||
|
|
double yawDeg = 0.0;
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
// 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);
|
|||
|
|
|
|||
|
|
// ============ 砂轮盘孔定位专用工具 ============
|
|||
|
|
|
|||
|
|
// 从 SSG_pointPose 的三轴向量 (pose_x/y/z) 构造旋转矩阵，经手眼变换流水线提取欧拉角。
|
|||
|
|
// 用于砂轮盘架子检测（算法直接给出完整姿态三元组）。
|
|||
|
|
bool PointPoseToEuler(const SSG_pointPose& pointPose,
|
|||
|
|
const double handEyeMatrix[16],
|
|||
|
|
const HandEyeExtrinsic& extrinsic,
|
|||
|
|
const RobotPose6D& robotPose,
|
|||
|
|
double& outX,
|
|||
|
|
double& outY,
|
|||
|
|
double& outZ,
|
|||
|
|
double& outRoll,
|
|||
|
|
double& outPitch,
|
|||
|
|
double& outYaw,
|
|||
|
|
int poseOutputOrder);
|
|||
|
|
|
|||
|
|
// 从单一法向量 normDir 构造完整姿态（Z 轴 = normDir，X/Y 自动补全），
|
|||
|
|
// 经手眼变换流水线提取欧拉角。
|
|||
|
|
// 用于砂轮盘孔检测（算法只给出孔中心和法向量）。
|
|||
|
|
bool NormalDirToPoseAngles(const SVzNL3DPoint& center,
|
|||
|
|
const SVzNL3DPoint& normDir,
|
|||
|
|
const double handEyeMatrix[16],
|
|||
|
|
const HandEyeExtrinsic& extrinsic,
|
|||
|
|
const RobotPose6D& robotPose,
|
|||
|
|
double& outX,
|
|||
|
|
double& outY,
|
|||
|
|
double& outZ,
|
|||
|
|
double& outRoll,
|
|||
|
|
double& outPitch,
|
|||
|
|
double& outYaw,
|
|||
|
|
int poseOutputOrder);
|
|||
|
|
|
|||
|
|
} // namespace PoseAxesBuilder
|