jerryzeng/algoLib
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

workpieceHolePositioning v1.0.1

Browse Source 
添加了工件姿态（欧拉角）输出
This commit is contained in:
jerryzeng 2026-01-30 00:10:14 +08:00
parent 14c99bb830
commit 7a4be26602
2 changed files with 36 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
sourceCode/workpieceHolePositioning.cpp
View File

@ -6,7 +6,8 @@
#include <limits>
//version 1.0.0 : base version release to customer
std::string m_strVersion = "1.0.1";
//version 1.0.2 : 添加了工件姿态（欧拉角输出)
std::string m_strVersion = "1.0.2";
const char* wd_workpieceHolePositioningVersion(void)
{
return m_strVersion.c_str();
@ -514,6 +515,7 @@ void wd_workpieceHolePositioning(
y_dir = { y_dir.x / modLen, y_dir.y / modLen, 0 };
a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z };
a_workpiece.z_dir = { a_workpiece.center.x, a_workpiece.center.y, a_workpiece.center.z - 20 };
workpiecePositioning.push_back(a_workpiece);
}
@ -535,6 +537,38 @@ void wd_workpieceHolePositioning(
rpt = _ptRotate(workpiecePositioning[i].holesDir[j], groundCalibPara.invRMatrix);
workpiecePositioning[i].holesDir[j] = rpt;
}
SVzNL3DPoint vector_z = { workpiecePositioning[i].z_dir.x - workpiecePositioning[i].center.x,
workpiecePositioning[i].z_dir.y - workpiecePositioning[i].center.y,
workpiecePositioning[i].z_dir.z - workpiecePositioning[i].center.z };
SVzNL3DPoint vector_y = { workpiecePositioning[i].y_dir.x - workpiecePositioning[i].center.x,
workpiecePositioning[i].y_dir.y - workpiecePositioning[i].center.y,
workpiecePositioning[i].y_dir.z - workpiecePositioning[i].center.z };
double mod_vz = sqrt(pow(vector_z.x, 2) + pow(vector_z.y, 2) + pow(vector_z.z, 2));
vector_z = { vector_z.x / mod_vz, vector_z.y / mod_vz, vector_z.z / mod_vz }; //归一化
double mod_vy = sqrt(pow(vector_y.x, 2) + pow(vector_y.y, 2) + pow(vector_y.z, 2));
vector_y = { vector_y.x / mod_vy, vector_y.y / mod_vy, vector_y.z / mod_vy }; //归一化
//叉乘出vector_x
SVzNL3DPoint vector_x;
vector_x.x = vector_y.y * vector_z.z - vector_z.y * vector_y.z;
vector_x.y = vector_y.z * vector_z.x - vector_z.z * vector_y.x;
vector_x.z = vector_y.x * vector_z.y - vector_z.x * vector_y.y;
//得到旋转矩阵
double R[3][3];
R[0][0] = vector_x.x;
R[1][0] = vector_x.y;
R[2][0] = vector_x.z;
R[0][1] = vector_y.x;
R[1][1] = vector_y.y;
R[2][1] = vector_y.z;
R[0][2] = vector_z.x;
R[1][2] = vector_z.y;
R[2][2] = vector_z.z;
SSG_EulerAngles eulerAngle = rotationMatrixToEulerZYX(R);
workpiecePositioning[i].workpiecePose = eulerAngle;
}
return;

1
sourceCode/workpieceHolePositioning_Export.h
View File

@ -21,6 +21,7 @@ typedef struct
SVzNL3DPoint center;
SVzNL3DPoint z_dir;
SVzNL3DPoint y_dir;
SSG_EulerAngles workpiecePose;
}WD_workpieceInfo;
//读版本号