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

hybridPosePositioning

Browse Source 
version 1.3.1 : 添加了海瑞马转子钢芯定位位置属性计算，区分出中央，边和角的位置，按照中心排序，先输出最中央的目标
This commit is contained in:
jerryzeng 2026-07-29 08:34:39 +08:00
parent fe7811b407
commit 7342a322d9
5 changed files with 284 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
bagPositioning_test/bagPositioning_test.vcxproj
View File

@ -42,13 +42,13 @@
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
<ConfigurationType>Application</ConfigurationType>
<UseDebugLibraries>true</UseDebugLibraries>
<PlatformToolset>v142</PlatformToolset>
<PlatformToolset>v143</PlatformToolset>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
<ConfigurationType>Application</ConfigurationType>
<UseDebugLibraries>false</UseDebugLibraries>
<PlatformToolset>v142</PlatformToolset>
<PlatformToolset>v143</PlatformToolset>
<WholeProgramOptimization>true</WholeProgramOptimization>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>

5
hybridPosePositioning_test/hybridPosePositioning_test.cpp
View File

@ -2110,6 +2110,10 @@ void HaiRuiMa_rotorCorePositioning_test(void)
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]);
groundCalibPara = _readCalibPara(calibFile);
//读取料筐参数
sprintf_s(calibFile, "%sbin_info.txt", dataPath[grp]);
WD_HRM_BinInfo binInfo = _readBinInfo(calibFile);
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
{
//fidx = 3;
@ -2137,6 +2141,7 @@ void HaiRuiMa_rotorCorePositioning_test(void)
wd_HRM_RotorCorePositioning(
scanLines,
objROIs,
binInfo,
groundCalibPara,
workpieceParam,
workpiecePositions,

1
sourceCode/SG_errCode.h
View File

@ -29,6 +29,7 @@
#define SX_ERR_ZERO_OBJ_TOPLAYER -2102
#define SX_ERR_ZERO_OBJ_BTMLAYER -2103
#define SX_ERR_GET_INVALID_PALTE -2104 //몰겼瓊혤댄轎
#define SX_ERR_BIN_EDGE_ERROR -2105
//뀔관
#define SX_BAG_TRAY_EMPTY -2201

283
sourceCode/hybridPosePositioning.cpp
View File

@ -11,7 +11,8 @@
//version 1.1.0 : 海瑞马码垛规划版本
//version 1.2.0 : 添加了海瑞马锥形工件定位版本（不包含二次定位）
//version 1.3.0 : 添加了海瑞马转子钢芯定位
std::string m_strVersion = "HybridPositioning 1.3.0";
//version 1.3.1 : 添加了海瑞马转子钢芯定位位置属性计算，区分出中央，边和角的位置，按照中心排序，先输出最中央的目标
std::string m_strVersion = "HybridPositioning 1.3.1";
const char* wd_hybridPositioningVersion(void)
{
return m_strVersion.c_str();
@ -799,6 +800,16 @@ typedef struct
std::vector<SWDScanPosition> mappings;//每个2D点可能对应多个3D点
}mapping2DTo3D;
typedef struct
{
double dist;
int idx;
}WD_indexingDistance;
bool _compareByDistance(WD_indexingDistance& a, WD_indexingDistance& b)
{
return a.dist < b.dist;
}
bool _compareByXValue(WD_workpieceInfo& a, WD_workpieceInfo& b)
{
return a.center.x < b.center.x;
@ -834,6 +845,7 @@ void _getYTopLine(
void wd_HRM_RotorCorePositioning(
std::vector< std::vector<SVzNLPositionD>>& scanLinesInput,
std::vector<WD_objArea2D>& objROIs,
const WD_HRM_BinInfo binInfo,
const SSG_planeCalibPara groundCalibPara,
const SWD_Cylinder workpieceParam, //圆柱形工件的标称半径和高度
std::vector< WD_workpieceInfo>& workpiecePositions,
@ -1174,15 +1186,268 @@ void wd_HRM_RotorCorePositioning(
if (allWorkpiece[i].center.z < topLayerTh)
zSortWorkpiece.push_back(allWorkpiece[i]);
}
//水平方向排序
while (zSortWorkpiece.size() > 0)
if (binInfo.binTopZ > 1e-4)
{
std::vector< WD_workpieceInfo> firstLine;
std::vector< WD_workpieceInfo> restWorkpiece;
_getYTopLine(zSortWorkpiece, firstLine, restWorkpiece, workpieceParam.radius);
workpiecePositions.insert(workpiecePositions.end(), firstLine.begin(), firstLine.end());
zSortWorkpiece.clear();
zSortWorkpiece.insert(zSortWorkpiece.end(), restWorkpiece.begin(), restWorkpiece.end());
//计算工件位置属性
std::vector<SWD_3DPointPair> linePoint;
linePoint.resize(4);
linePoint[0].pt1 = binInfo.minRectVertex[0];
linePoint[0].pt2 = binInfo.minRectVertex[1];
linePoint[1].pt1 = binInfo.minRectVertex[1];
linePoint[1].pt2 = binInfo.minRectVertex[2];
linePoint[2].pt1 = binInfo.minRectVertex[2];
linePoint[2].pt2 = binInfo.minRectVertex[3];
linePoint[3].pt1 = binInfo.minRectVertex[3];
linePoint[3].pt2 = binInfo.minRectVertex[0];
int lineTopIdx = -1;
for (int i = 0; i < (int)linePoint.size(); i++)
{
if ((linePoint[i].pt1.y < binInfo.center.y) && (linePoint[i].pt2.y < binInfo.center.y))
{
lineTopIdx = i;
break;
}
}
if (lineTopIdx < 0)
{
*errCode = SX_ERR_BIN_EDGE_ERROR;
//旋转回去
int workpieceNum = (int)workpiecePositions.size();
for (int i = 0; i < workpieceNum; i++)
{
SVzNL3DPoint rpt;
rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.invRMatrix);
workpiecePositions[i].center = rpt;
rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.invRMatrix);
workpiecePositions[i].y_dir = rpt;
rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.invRMatrix);
workpiecePositions[i].z_dir = rpt;
rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.invRMatrix);
workpiecePositions[i].x_dir = rpt;
}
return;
}
SWD_3DPointPair lineTop = linePoint[lineTopIdx];
linePoint.erase(linePoint.begin() + lineTopIdx);//去掉已经识别的edge
int lineBtmIdx = -1;
for (int i = 0; i < (int)linePoint.size(); i++)
{
if ((linePoint[i].pt1.y > binInfo.center.y) && (linePoint[i].pt2.y > binInfo.center.y))
{
lineBtmIdx = i;
break;
}
}
if (lineBtmIdx < 0)
{
*errCode = SX_ERR_BIN_EDGE_ERROR;
//旋转回去
int workpieceNum = (int)workpiecePositions.size();
for (int i = 0; i < workpieceNum; i++)
{
SVzNL3DPoint rpt;
rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.invRMatrix);
workpiecePositions[i].center = rpt;
rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.invRMatrix);
workpiecePositions[i].y_dir = rpt;
rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.invRMatrix);
workpiecePositions[i].z_dir = rpt;
rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.invRMatrix);
workpiecePositions[i].x_dir = rpt;
}
return;
}
SWD_3DPointPair lineBtm = linePoint[lineBtmIdx];
linePoint.erase(linePoint.begin() + lineBtmIdx); //去掉已经识别的edge
int lineLeftIdx = -1;
for (int i = 0; i < (int)linePoint.size(); i++)
{
if ((linePoint[i].pt1.x < binInfo.center.x) && (linePoint[i].pt2.x < binInfo.center.x))
{
lineLeftIdx = i;
break;
}
}
if (lineLeftIdx < 0)
{
*errCode = SX_ERR_BIN_EDGE_ERROR;
//旋转回去
int workpieceNum = (int)workpiecePositions.size();
for (int i = 0; i < workpieceNum; i++)
{
SVzNL3DPoint rpt;
rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.invRMatrix);
workpiecePositions[i].center = rpt;
rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.invRMatrix);
workpiecePositions[i].y_dir = rpt;
rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.invRMatrix);
workpiecePositions[i].z_dir = rpt;
rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.invRMatrix);
workpiecePositions[i].x_dir = rpt;
}
return;
}
SWD_3DPointPair lineLeft = linePoint[lineLeftIdx];
linePoint.erase(linePoint.begin() + lineLeftIdx); //去掉已经识别的edge
int lineRightIdx = -1;
for (int i = 0; i < (int)linePoint.size(); i++)
{
if ((linePoint[i].pt1.x > binInfo.center.x) && (linePoint[i].pt2.x > binInfo.center.x))
{
lineRightIdx = i;
break;
}
}
if (lineRightIdx < 0)
{
*errCode = SX_ERR_BIN_EDGE_ERROR;
//旋转回去
int workpieceNum = (int)workpiecePositions.size();
for (int i = 0; i < workpieceNum; i++)
{
SVzNL3DPoint rpt;
rpt = wd_ptRotate(workpiecePositions[i].center, groundCalibPara.invRMatrix);
workpiecePositions[i].center = rpt;
rpt = wd_ptRotate(workpiecePositions[i].y_dir, groundCalibPara.invRMatrix);
workpiecePositions[i].y_dir = rpt;
rpt = wd_ptRotate(workpiecePositions[i].z_dir, groundCalibPara.invRMatrix);
workpiecePositions[i].z_dir = rpt;
rpt = wd_ptRotate(workpiecePositions[i].x_dir, groundCalibPara.invRMatrix);
workpiecePositions[i].x_dir = rpt;
}
return;
}
SWD_3DPointPair lineRight = linePoint[lineRightIdx];
//计算工件的位置：左上、右上、左下、右下四个角；上下左右四条边：中央部分
//机械臂对不同部分的工件会采用不同的抓取策略
double diameter = workpieceParam.radius * 2;
double lineTop_a, lineTop_b, lineTop_c;
compute2ptLine(lineTop.pt1, lineTop.pt2, &lineTop_a, &lineTop_b, &lineTop_c);
double lineBtm_a, lineBtm_b, lineBtm_c;
compute2ptLine(lineBtm.pt1, lineBtm.pt2, &lineBtm_a, &lineBtm_b, &lineBtm_c);
double lineLeft_a, lineLeft_b, lineLeft_c;
compute2ptLine(lineLeft.pt1, lineLeft.pt2, &lineLeft_a, &lineLeft_b, &lineLeft_c);
double lineRight_a, lineRight_b, lineRight_c;
compute2ptLine(lineRight.pt1, lineRight.pt2, &lineRight_a, &lineRight_b, &lineRight_c);
std::vector< WD_workpieceInfo> topSide; //上边
std::vector< WD_workpieceInfo> btmSide; //下边
std::vector< WD_workpieceInfo> leftSide; //左边
std::vector< WD_workpieceInfo> rightSide; //右边
std::vector< WD_workpieceInfo> LTCorner; //左上角
std::vector< WD_workpieceInfo> RTCorner; //右上角
std::vector< WD_workpieceInfo> LBCorner; //左边
std::vector< WD_workpieceInfo> RBCorner; //右边
std::vector< WD_workpieceInfo> centerRegion; //右边
for (int i = 0; i < (int)zSortWorkpiece.size(); i++) //对上层目标进行分析
{
SVzNL3DPoint& objCenter = zSortWorkpiece[i].center;
double distTop = computePtDistToLine(objCenter.x, objCenter.y, lineTop_a, lineTop_b, lineTop_c);
double distBtm = computePtDistToLine(objCenter.x, objCenter.y, lineBtm_a, lineBtm_b, lineBtm_c);
double distLeft = computePtDistToLine(objCenter.x, objCenter.y, lineLeft_a, lineLeft_b, lineLeft_c);
double distRight = computePtDistToLine(objCenter.x, objCenter.y, lineRight_a, lineRight_b, lineRight_c);
if (distTop < diameter)
{
if (distLeft < diameter)
{
zSortWorkpiece[i].workpieceType = 5; //左上
LTCorner.push_back(zSortWorkpiece[i]);
}
else if (distRight < diameter)
{
zSortWorkpiece[i].workpieceType = 6; //右上
RTCorner.push_back(zSortWorkpiece[i]);
}
else
{
zSortWorkpiece[i].workpieceType = 1; //上
topSide.push_back(zSortWorkpiece[i]);
}
}
else if (distBtm < diameter)
{
if (distLeft < diameter)
{
zSortWorkpiece[i].workpieceType = 7; //左下
LBCorner.push_back(zSortWorkpiece[i]);
}
else if (distRight < diameter)
{
zSortWorkpiece[i].workpieceType = 8; //右下
RBCorner.push_back(zSortWorkpiece[i]);
}
else
{
zSortWorkpiece[i].workpieceType = 2; //下
btmSide.push_back(zSortWorkpiece[i]);
}
}
else if (distLeft < diameter)
{
zSortWorkpiece[i].workpieceType = 3; //左
leftSide.push_back(zSortWorkpiece[i]);
}
else if (distRight < diameter)
{
zSortWorkpiece[i].workpieceType = 4; //右
rightSide.push_back(zSortWorkpiece[i]);
}
else
{
zSortWorkpiece[i].workpieceType = 0; //中央
centerRegion.push_back(zSortWorkpiece[i]);
}
}
//对中央部分工件按距中央距离排序
if (centerRegion.size() > 0)
{
std::vector<WD_indexingDistance> distanceIndice;
distanceIndice.resize(centerRegion.size());
for (int m = 0; m < (int)centerRegion.size(); m++)
{
distanceIndice[m].dist = sqrt(pow(centerRegion[m].center.x - binInfo.center.x, 2) +
pow(centerRegion[m].center.y - binInfo.center.y, 2));
distanceIndice[m].idx = m;
}
std::sort(distanceIndice.begin(), distanceIndice.end(), _compareByDistance);
for (int m = 0; m < (int)distanceIndice.size(); m++)
{
int idx = distanceIndice[m].idx;
workpiecePositions.push_back(centerRegion[idx]);
}
}
if(topSide.size() > 0)
workpiecePositions.insert(workpiecePositions.end(), topSide.begin(), topSide.end());
if (btmSide.size() > 0)
workpiecePositions.insert(workpiecePositions.end(), btmSide.begin(), btmSide.end());
if(leftSide.size() >0)
workpiecePositions.insert(workpiecePositions.end(), leftSide.begin(), leftSide.end());
if (rightSide.size() > 0)
workpiecePositions.insert(workpiecePositions.end(), rightSide.begin(), rightSide.end());
if(LTCorner.size() >0)
workpiecePositions.insert(workpiecePositions.end(), LTCorner.begin(), LTCorner.end());
if (RTCorner.size() > 0)
workpiecePositions.insert(workpiecePositions.end(), RTCorner.begin(), RTCorner.end());
if (LBCorner.size() > 0)
workpiecePositions.insert(workpiecePositions.end(), LBCorner.begin(), LBCorner.end());
if (RBCorner.size() > 0)
workpiecePositions.insert(workpiecePositions.end(), RBCorner.begin(), RBCorner.end());
}
else
{//水平方向排序
while (zSortWorkpiece.size() > 0)
{
std::vector< WD_workpieceInfo> firstLine;
std::vector< WD_workpieceInfo> restWorkpiece;
_getYTopLine(zSortWorkpiece, firstLine, restWorkpiece, workpieceParam.radius);
workpiecePositions.insert(workpiecePositions.end(), firstLine.begin(), firstLine.end());
zSortWorkpiece.clear();
zSortWorkpiece.insert(zSortWorkpiece.end(), restWorkpiece.begin(), restWorkpiece.end());
}
}
//旋转回去

3
sourceCode/hybridPosePositioning_Export.h
View File

@ -14,7 +14,7 @@ typedef struct
typedef struct
{
int workpieceType;
int workpieceType; //0:中央; 1-上边; 2-下边; 3-左边; 4-右边; 5-左上角; 6-右上角; 7-左下角; 8-右下角;
SVzNL3DPoint center;
SVzNL3DPoint z_dir; //z方向向量（归一化）
SVzNL3DPoint y_dir; //y方向向量（归一化）
@ -60,6 +60,7 @@ SG_APISHARED_EXPORT void wd_lineDataR(
SG_APISHARED_EXPORT void wd_HRM_RotorCorePositioning(
std::vector< std::vector<SVzNLPositionD>>& scanLinesInput,
std::vector<WD_objArea2D>& objROIs,
const WD_HRM_BinInfo binInfo,
const SSG_planeCalibPara groundCalibPara,
const SWD_Cylinder workpieceParam, //圆柱形工件的标称半径和高度
std::vector< WD_workpieceInfo>& workpiecePositions,