361 lines
12 KiB
C++
361 lines
12 KiB
C++
#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
|