|
|
@@ -62,7 +62,11 @@ namespace TeamAAS_VP.Services
|
|
|
Matrix<double> RobotCameraRotationMatrix = null;
|
|
|
|
|
|
//移动机器人至待机位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
|
|
|
+ bool isSuccess = await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0, 0, 0);
|
|
|
@@ -109,7 +113,11 @@ namespace TeamAAS_VP.Services
|
|
|
RPoint RobotU = calibrationInfo.CenterPoint.Clone();
|
|
|
RobotU.U += (float)calibrationInfo.Angle;
|
|
|
//移动机器人中心点
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.CenterPoint, null);
|
|
|
+ isSuccess=await robot.CalibMotionAsync(calibrationInfo.CenterPoint, null);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0, 0, 0);
|
|
|
@@ -118,7 +126,11 @@ namespace TeamAAS_VP.Services
|
|
|
await robot.CalibOutIOAsync(true);
|
|
|
|
|
|
//机器人U轴旋转
|
|
|
- await robot.CalibMotionAsync(RobotU, null);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(RobotU, null);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0, 0, 0);
|
|
|
@@ -127,7 +139,11 @@ namespace TeamAAS_VP.Services
|
|
|
//关闭吸气
|
|
|
await robot.CalibOutIOAsync(false);
|
|
|
//移动机器人至待机位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0, 0, 0);
|
|
|
@@ -149,7 +165,11 @@ namespace TeamAAS_VP.Services
|
|
|
double offsety = (P0.Y - curPixel.Y) * pixelScaleY;
|
|
|
var pos = RobotCameraRotationMatrix.Inverse() * Vector<double>.Build.Dense(new double[] { offsetx, offsety });
|
|
|
//移动机器人取标定块
|
|
|
- await robot.CalibMotionAsync(RobotU, null);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(RobotU, null);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0, 0, 0);
|
|
|
@@ -160,7 +180,11 @@ namespace TeamAAS_VP.Services
|
|
|
RobotU.X += (float)pos[0];
|
|
|
RobotU.Y += (float)pos[1];
|
|
|
//机器人移动
|
|
|
- await robot.CalibMotionAsync(RobotU, null);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(RobotU, null);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0, 0, 0);
|
|
|
@@ -169,7 +193,11 @@ namespace TeamAAS_VP.Services
|
|
|
//关闭吸气
|
|
|
await robot.CalibOutIOAsync(false);
|
|
|
//移动机器人至待机位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0, 0, 0);
|
|
|
@@ -187,7 +215,11 @@ namespace TeamAAS_VP.Services
|
|
|
if (Math.Abs(offsetx) < 1 && Math.Abs(offsetx) < 1)
|
|
|
{
|
|
|
//移动机器人取标定块
|
|
|
- await robot.CalibMotionAsync(RobotU, null);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(RobotU, null);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0, 0, 0);
|
|
|
@@ -195,7 +227,11 @@ namespace TeamAAS_VP.Services
|
|
|
//打开吸气
|
|
|
await robot.CalibOutIOAsync(true);
|
|
|
//移动机器人至待机位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
IsFinish = true;
|
|
|
break;
|
|
|
}
|
|
|
@@ -247,7 +283,11 @@ namespace TeamAAS_VP.Services
|
|
|
Matrix<double> RobotCameraRotationMatrix = null;
|
|
|
|
|
|
//移动机器人至中心位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.CenterPoint, null);
|
|
|
+ bool isSuccess = await robot.CalibMotionAsync(calibrationInfo.CenterPoint, null);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
|
|
|
//计算相机与机器人坐标系的旋转矩阵
|
|
|
var robotCameraRotationMatrix = await AutoIdentifyRobotCameraRotationMatrix(calibrationInfo, robot, cancellationToken);
|
|
|
@@ -272,7 +312,11 @@ namespace TeamAAS_VP.Services
|
|
|
RobotU.U += (float)calibrationInfo.Angle;
|
|
|
|
|
|
//机器人U轴旋转
|
|
|
- await robot.CalibMotionAsync(RobotU, RobotU.Z);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(RobotU, RobotU.Z);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0, 0, 0);
|
|
|
@@ -314,7 +358,11 @@ namespace TeamAAS_VP.Services
|
|
|
calibrationInfo.Tool.Y = toolCoord.Y;
|
|
|
|
|
|
//移动机器人至中心位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.CenterPoint, calibrationInfo.CenterPoint.Z);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(calibrationInfo.CenterPoint, calibrationInfo.CenterPoint.Z);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
|
|
|
return (true, RobotCameraRotationMatrix, calibrationInfo.Tool.X, calibrationInfo.Tool.Y, pixelScaleX, pixelScaleY);
|
|
|
}
|
|
|
@@ -334,7 +382,11 @@ namespace TeamAAS_VP.Services
|
|
|
Matrix<double> RobotCameraRotationMatrix = null;
|
|
|
|
|
|
//移动机器人至中心位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.CenterPoint, null);
|
|
|
+ bool isSuccess = await robot.CalibMotionAsync(calibrationInfo.CenterPoint, null);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
|
|
|
if (calibrationInfo.Pick.PickPlaceModel == Enums.PickPlaceModel.Inhal)
|
|
|
{
|
|
|
@@ -380,7 +432,11 @@ namespace TeamAAS_VP.Services
|
|
|
RobotU.U += (float)calibrationInfo.Angle;
|
|
|
|
|
|
//机器人U轴旋转
|
|
|
- await robot.CalibMotionAsync(RobotU, RobotU.Z);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(RobotU, RobotU.Z);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0, 0, 0);
|
|
|
@@ -422,7 +478,11 @@ namespace TeamAAS_VP.Services
|
|
|
calibrationInfo.Tool.Y = toolCoord.Y;
|
|
|
|
|
|
//移动机器人至中心位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.CenterPoint, calibrationInfo.CenterPoint.Z);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(calibrationInfo.CenterPoint, calibrationInfo.CenterPoint.Z);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
|
|
|
return (true, RobotCameraRotationMatrix, calibrationInfo.Tool.X, calibrationInfo.Tool.Y, pixelScaleX, pixelScaleY);
|
|
|
}
|
|
|
@@ -708,7 +768,7 @@ namespace TeamAAS_VP.Services
|
|
|
else
|
|
|
{
|
|
|
//移动机器人至P1
|
|
|
- isFinished= await robot.CalibMotionAsync(RobotP1, RobotP1.Z);
|
|
|
+ isFinished = await robot.CalibMotionAsync(RobotP1, RobotP1.Z);
|
|
|
if (!isFinished)
|
|
|
{
|
|
|
return (false, null, 0, 0);
|
|
|
@@ -732,7 +792,7 @@ namespace TeamAAS_VP.Services
|
|
|
if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown && !calibrationInfo.FixedDownCameraNoReturnToHome)
|
|
|
{
|
|
|
//移动机器人至P1
|
|
|
- isFinished= await robot.CalibMotionAsync(RobotP1, null);
|
|
|
+ isFinished = await robot.CalibMotionAsync(RobotP1, null);
|
|
|
if (!isFinished)
|
|
|
{
|
|
|
return (false, null, 0, 0);
|
|
|
@@ -744,7 +804,7 @@ namespace TeamAAS_VP.Services
|
|
|
//打开吸气
|
|
|
await robot.CalibOutIOAsync(true);
|
|
|
//移动机器人至P2
|
|
|
- isFinished= await robot.CalibMotionAsync(RobotP2, null);
|
|
|
+ isFinished = await robot.CalibMotionAsync(RobotP2, null);
|
|
|
if (!isFinished)
|
|
|
{
|
|
|
return (false, null, 0, 0);
|
|
|
@@ -756,7 +816,7 @@ namespace TeamAAS_VP.Services
|
|
|
//关闭吸气
|
|
|
await robot.CalibOutIOAsync(false);
|
|
|
//移动机器人至待机位置
|
|
|
- isFinished= await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
|
|
|
+ isFinished = await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
|
|
|
if (!isFinished)
|
|
|
{
|
|
|
return (false, null, 0, 0);
|
|
|
@@ -765,7 +825,7 @@ namespace TeamAAS_VP.Services
|
|
|
else
|
|
|
{
|
|
|
//移动机器人至P2
|
|
|
- isFinished= await robot.CalibMotionAsync(RobotP2, RobotP2.Z);
|
|
|
+ isFinished = await robot.CalibMotionAsync(RobotP2, RobotP2.Z);
|
|
|
if (!isFinished)
|
|
|
{
|
|
|
return (false, null, 0, 0);
|
|
|
@@ -789,7 +849,7 @@ namespace TeamAAS_VP.Services
|
|
|
if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown && !calibrationInfo.FixedDownCameraNoReturnToHome)
|
|
|
{
|
|
|
//移动机器人至P2
|
|
|
- isFinished= await robot.CalibMotionAsync(RobotP2, null);
|
|
|
+ isFinished = await robot.CalibMotionAsync(RobotP2, null);
|
|
|
if (!isFinished)
|
|
|
{
|
|
|
return (false, null, 0, 0);
|
|
|
@@ -801,7 +861,7 @@ namespace TeamAAS_VP.Services
|
|
|
//打开吸气
|
|
|
await robot.CalibOutIOAsync(true);
|
|
|
//移动机器人至P0
|
|
|
- isFinished= await robot.CalibMotionAsync(RobotP0, null);
|
|
|
+ isFinished = await robot.CalibMotionAsync(RobotP0, null);
|
|
|
if (!isFinished)
|
|
|
{
|
|
|
return (false, null, 0, 0);
|
|
|
@@ -813,7 +873,7 @@ namespace TeamAAS_VP.Services
|
|
|
//关闭吸气
|
|
|
await robot.CalibOutIOAsync(false);
|
|
|
//移动机器人至待机位置
|
|
|
- isFinished= await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
|
|
|
+ isFinished = await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
|
|
|
if (!isFinished)
|
|
|
{
|
|
|
return (false, null, 0, 0);
|
|
|
@@ -822,7 +882,7 @@ namespace TeamAAS_VP.Services
|
|
|
else
|
|
|
{
|
|
|
//移动机器人至P0
|
|
|
- isFinished= await robot.CalibMotionAsync(RobotP0, RobotP0.Z);
|
|
|
+ isFinished = await robot.CalibMotionAsync(RobotP0, RobotP0.Z);
|
|
|
if (!isFinished)
|
|
|
{
|
|
|
return (false, null, 0, 0);
|
|
|
@@ -910,7 +970,11 @@ namespace TeamAAS_VP.Services
|
|
|
curpos.X += (float)pos[0];
|
|
|
curpos.Y += (float)pos[1];
|
|
|
//机器人移动
|
|
|
- await robot.CalibMotionAsync(curpos, curpos.Z);
|
|
|
+ bool isSuccess = await robot.CalibMotionAsync(curpos, curpos.Z);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, 0, 0, 0);
|
|
|
+ }
|
|
|
await Task.Delay(200);
|
|
|
|
|
|
result = await CaptureAndProcessCallback();
|
|
|
@@ -963,7 +1027,7 @@ namespace TeamAAS_VP.Services
|
|
|
{
|
|
|
if (calibrationInfo == null) return (false, null, null, null);
|
|
|
if (robot == null || !robot.IsConnected) return (false, null, null, null);
|
|
|
-
|
|
|
+ bool isSuccess = false;
|
|
|
|
|
|
//发送校准的参数
|
|
|
await robot.CalibParameAsync(calibrationInfo.Pick, calibrationInfo.Speed, calibrationInfo.Accel, calibrationInfo.Power, calibrationInfo.WaitSuction, calibrationInfo.WaitBlow);
|
|
|
@@ -973,7 +1037,11 @@ namespace TeamAAS_VP.Services
|
|
|
{
|
|
|
|
|
|
//移动机器人至中心位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.CenterPoint, null);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(calibrationInfo.CenterPoint, null);
|
|
|
+ if (isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, null, null);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, null, null);
|
|
|
@@ -1002,7 +1070,11 @@ namespace TeamAAS_VP.Services
|
|
|
else if (calibrationInfo.CameraMount == Enums.CameraMount.MobileDown_XYPlatform)
|
|
|
{
|
|
|
//移动机器人至中心位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.CenterPoint, null);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(calibrationInfo.CenterPoint, null);
|
|
|
+ if (isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, null, null);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, null, null);
|
|
|
@@ -1203,7 +1275,11 @@ namespace TeamAAS_VP.Services
|
|
|
if (calibrationInfo.CameraMount == Enums.CameraMount.MobileJ4)
|
|
|
{
|
|
|
//移动机器人至中心位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.CenterPoint, null);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(calibrationInfo.CenterPoint, null);
|
|
|
+ if (isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, null, null);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, null, null);
|
|
|
@@ -1229,7 +1305,11 @@ namespace TeamAAS_VP.Services
|
|
|
//移动机器人至i
|
|
|
if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
|
|
|
{
|
|
|
- await robot.CalibMotionAsync(rPoints[i], null);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(rPoints[i], null);
|
|
|
+ if (isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, null, null);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, null, null);
|
|
|
@@ -1237,11 +1317,19 @@ namespace TeamAAS_VP.Services
|
|
|
//关闭吸气
|
|
|
await robot.CalibOutIOAsync(false);
|
|
|
//移动机器人至待机位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
|
|
|
+ if (isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, null, null);
|
|
|
+ }
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- await robot.CalibMotionAsync(rPoints[i], rPoints[i].Z);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(rPoints[i], rPoints[i].Z);
|
|
|
+ if (isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, null, null);
|
|
|
+ }
|
|
|
}
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
@@ -1260,7 +1348,11 @@ namespace TeamAAS_VP.Services
|
|
|
if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
|
|
|
{
|
|
|
//移动机器人至i
|
|
|
- await robot.CalibMotionAsync(rPoints[i], null);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(rPoints[i], null);
|
|
|
+ if (isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, null, null);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, null, null);
|
|
|
@@ -1283,7 +1375,11 @@ namespace TeamAAS_VP.Services
|
|
|
if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
|
|
|
{
|
|
|
//移动机器人至待机位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(calibrationInfo.HomePoint, null);
|
|
|
+ if (isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, null, null);
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
@@ -1528,7 +1624,11 @@ namespace TeamAAS_VP.Services
|
|
|
if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
|
|
|
{
|
|
|
//移动机器人至i
|
|
|
- await robot.CalibMotionAsync(rPoints[i], null);
|
|
|
+ isSucceed= await robot.CalibMotionAsync(rPoints[i], null);
|
|
|
+ if (isSucceed)
|
|
|
+ {
|
|
|
+ return (false, null);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null);
|
|
|
@@ -1795,7 +1895,11 @@ namespace TeamAAS_VP.Services
|
|
|
Matrix<double> RobotCameraRotationMatrix = null;
|
|
|
|
|
|
// 2.移动机器人至中心位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.CenterPoint, null);
|
|
|
+ bool isSuccess = await robot.CalibMotionAsync(calibrationInfo.CenterPoint, null);
|
|
|
+ if (isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, null);
|
|
|
+ }
|
|
|
|
|
|
// 3.计算相机与机器人坐标系的旋转矩阵
|
|
|
var robotCameraRotationMatrix = await AutoIdentifyRobotCameraRotationMatrix(calibrationInfo, robot, cancellationToken);
|
|
|
@@ -1856,32 +1960,32 @@ namespace TeamAAS_VP.Services
|
|
|
//计算;两点的间距
|
|
|
double wordLength = Common.CalculateDistance2D(P1.X, P1.Y, P2.X, P2.Y);
|
|
|
double localLength = Common.CalculateDistance2D(P11.X, P11.Y, P12.X, P12.Y);
|
|
|
- if (Math.Abs(wordLength- localLength)>0.1)
|
|
|
+ if (Math.Abs(wordLength - localLength) > 0.1)
|
|
|
{
|
|
|
- MessageBox.Show("检测下来绝对坐标系移动距离和独立步进坐标系的移动距离差大于0.1mm,请检查哥轴的实际移动距离是否正确!"+ Math.Abs(wordLength - localLength).ToString()+"mm");
|
|
|
+ MessageBox.Show("检测下来绝对坐标系移动距离和独立步进坐标系的移动距离差大于0.1mm,请检查哥轴的实际移动距离是否正确!" + Math.Abs(wordLength - localLength).ToString() + "mm");
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
MessageBox.Show($"检测下来两个坐标系的移动距离正确,实际距离差:{Math.Abs(wordLength - localLength)}mm");
|
|
|
}
|
|
|
|
|
|
- // 9.计算刚体变换矩阵
|
|
|
- ////得到旋转矩阵
|
|
|
- //var rigidMatrix = local.InvR;
|
|
|
- ////平移矩阵
|
|
|
- //var translation = local.T;
|
|
|
- //double[,] TransformationMatrix = new double[3, 3];
|
|
|
- //TransformationMatrix[0, 0] = rigidMatrix[0, 0];
|
|
|
- //TransformationMatrix[0, 1] = rigidMatrix[0, 1];
|
|
|
- //TransformationMatrix[0, 2] = translation[0, 0];
|
|
|
- //TransformationMatrix[1, 0] = rigidMatrix[1, 0];
|
|
|
- //TransformationMatrix[1, 1] = rigidMatrix[1, 1];
|
|
|
- //TransformationMatrix[1, 2] = translation[1, 0];
|
|
|
- //TransformationMatrix[2, 0] = 0;
|
|
|
- //TransformationMatrix[2, 1] = 0;
|
|
|
- //TransformationMatrix[2, 2] = 1;
|
|
|
-
|
|
|
- return (true, local.R.ToArray(), local.T.ToArray());
|
|
|
+ // 9.计算刚体变换矩阵
|
|
|
+ ////得到旋转矩阵
|
|
|
+ //var rigidMatrix = local.InvR;
|
|
|
+ ////平移矩阵
|
|
|
+ //var translation = local.T;
|
|
|
+ //double[,] TransformationMatrix = new double[3, 3];
|
|
|
+ //TransformationMatrix[0, 0] = rigidMatrix[0, 0];
|
|
|
+ //TransformationMatrix[0, 1] = rigidMatrix[0, 1];
|
|
|
+ //TransformationMatrix[0, 2] = translation[0, 0];
|
|
|
+ //TransformationMatrix[1, 0] = rigidMatrix[1, 0];
|
|
|
+ //TransformationMatrix[1, 1] = rigidMatrix[1, 1];
|
|
|
+ //TransformationMatrix[1, 2] = translation[1, 0];
|
|
|
+ //TransformationMatrix[2, 0] = 0;
|
|
|
+ //TransformationMatrix[2, 1] = 0;
|
|
|
+ //TransformationMatrix[2, 2] = 1;
|
|
|
+
|
|
|
+ return (true, local.R.ToArray(), local.T.ToArray());
|
|
|
}
|
|
|
|
|
|
public void Dispose()
|