From 7a4be26602f6d4bea1059e34ae9be51c9cf37b62 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Fri, 30 Jan 2026 00:10:14 +0800 Subject: [PATCH] =?UTF-8?q?workpieceHolePositioning=20v1.0.1=20=E6=B7=BB?= =?UTF-8?q?=E5=8A=A0=E4=BA=86=E5=B7=A5=E4=BB=B6=E5=A7=BF=E6=80=81=EF=BC=88?= =?UTF-8?q?=E6=AC=A7=E6=8B=89=E8=A7=92=EF=BC=89=E8=BE=93=E5=87=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sourceCode/workpieceHolePositioning.cpp | 36 +++++++++++++++++++- sourceCode/workpieceHolePositioning_Export.h | 1 + 2 files changed, 36 insertions(+), 1 deletion(-) diff --git a/sourceCode/workpieceHolePositioning.cpp b/sourceCode/workpieceHolePositioning.cpp index 7953ed3..a4a1c12 100644 --- a/sourceCode/workpieceHolePositioning.cpp +++ b/sourceCode/workpieceHolePositioning.cpp @@ -6,7 +6,8 @@ #include //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; diff --git a/sourceCode/workpieceHolePositioning_Export.h b/sourceCode/workpieceHolePositioning_Export.h index 03fc106..e7653fa 100644 --- a/sourceCode/workpieceHolePositioning_Export.h +++ b/sourceCode/workpieceHolePositioning_Export.h @@ -21,6 +21,7 @@ typedef struct SVzNL3DPoint center; SVzNL3DPoint z_dir; SVzNL3DPoint y_dir; + SSG_EulerAngles workpiecePose; }WD_workpieceInfo; //读版本号