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; //读版本号