/// <summary>
/// 伺服运动模块中计算下一周期的Tcp位置
/// </summary>
/// <param name="tcpRealPosition">实时Tcp坐标</param>
/// <param name="referenceForce">参考力信号</param>
/// <returns>返回下一周期的Tcp位置</returns>
protected override double[] ServoMotionNextTcpPosition(double[] tcpRealPosition, double[] referenceForce, double[] moreInfo = null)
{
int refFlag = 0;
double[] refDist = new double[] { 0.0, 0.0 };
double[] refPosture = new double[] { 0.0, 0.0, 0.0 };
double refForce = 0.0;
lock (refLocker)
{
refFlag = servoMotionRefFlag;
refDist[0] = servoMotionRefPos[0];
refDist[1] = servoMotionRefPos[1];
refPosture[0] = servoMotionRefPos[2];
refPosture[1] = servoMotionRefPos[3];
refPosture[2] = servoMotionRefPos[4];
refForce = servoMotionRefPos[5];
}
double[] nextTcpPosition = (double[])tcpRealPosition.Clone();
// 暂停运动
if (servoMotionActivePause)
{
return(nextTcpPosition);
}
// 姿态变化，单周期幅值限制
double[] currentPosture = new double[] { tcpRealPosition[3], tcpRealPosition[4], tcpRealPosition[5] };
double[] transitPosture = URMath.Quatnum2AxisAngle(
URMath.FindTransitQuatnum(
URMath.AxisAngle2Quatnum(currentPosture),
URMath.AxisAngle2Quatnum(refPosture)));
double transitAngle = URMath.LengthOfArray(transitPosture);
double[] transitAxis = new double[] { transitPosture[0] / transitAngle,
transitPosture[1] / transitAngle,
transitPosture[2] / transitAngle };
if (transitAngle <= servoMotionMaxAngleIncrement)
{
nextTcpPosition[3] = refPosture[0];
nextTcpPosition[4] = refPosture[1];
nextTcpPosition[5] = refPosture[2];
}
else
{
double[] newPosture = URMath.Quatnum2AxisAngle(
URMath.QuatnumRotate(new Quatnum[] {
URMath.AxisAngle2Quatnum(currentPosture),
URMath.AxisAngle2Quatnum(new double[] { transitAxis[0] * servoMotionMaxAngleIncrement,
transitAxis[1] * servoMotionMaxAngleIncrement,
transitAxis[2] * servoMotionMaxAngleIncrement })
}));
nextTcpPosition[3] = newPosture[0];
nextTcpPosition[4] = newPosture[1];
nextTcpPosition[5] = newPosture[2];
}
// 位移变化
double[] currentPosition = new double[] { tcpRealPosition[0], tcpRealPosition[1], tcpRealPosition[2] };
double[] initialPosition = new double[] { servoMotionBeginTcpPosition[0], servoMotionBeginTcpPosition[1], servoMotionBeginTcpPosition[2] };
double[] initialPosture = new double[] { servoMotionBeginTcpPosition[3], servoMotionBeginTcpPosition[4], servoMotionBeginTcpPosition[5] };
double[] aimPointAtPlane = new double[] {
initialPosition[0] + servoMotionToolDirectionXAtBase[0] * refDist[0] + servoMotionToolDirectionYAtBase[0] * refDist[1],
initialPosition[1] + servoMotionToolDirectionXAtBase[1] * refDist[0] + servoMotionToolDirectionYAtBase[1] * refDist[1],
initialPosition[2] + servoMotionToolDirectionXAtBase[2] * refDist[0] + servoMotionToolDirectionYAtBase[2] * refDist[1]
};
double[] currentZAxisAtBase = internalProcessor.ZDirectionOfTcpAtBaseReference(tcpRealPosition);
double enlongedCoeff =
(currentZAxisAtBase[0] * (currentPosition[0] - aimPointAtPlane[0]) +
currentZAxisAtBase[1] * (currentPosition[1] - aimPointAtPlane[1]) +
currentZAxisAtBase[2] * (currentPosition[2] - aimPointAtPlane[2])) /
(currentZAxisAtBase[0] * servoMotionToolDirectionZAtBase[0] +
currentZAxisAtBase[1] * servoMotionToolDirectionZAtBase[1] +
currentZAxisAtBase[2] * servoMotionToolDirectionZAtBase[2]);
double[] aimPointAtSurf = new double[] {
aimPointAtPlane[0] + enlongedCoeff * servoMotionToolDirectionZAtBase[0],
aimPointAtPlane[1] + enlongedCoeff * servoMotionToolDirectionZAtBase[1],
aimPointAtPlane[2] + enlongedCoeff * servoMotionToolDirectionZAtBase[2]
};
double[] refDelta = new double[] {
aimPointAtSurf[0] - currentPosition[0],
aimPointAtSurf[1] - currentPosition[1],
aimPointAtSurf[2] - currentPosition[2]
};
double refDeltaDistance = URMath.LengthOfArray(refDelta);
double[] refDeltaDirection = new double[] {
refDelta[0] / refDeltaDistance,
refDelta[1] / refDeltaDistance,
refDelta[2] / refDeltaDistance
};
if (refDeltaDistance > servoMotionMaxDistanceIncrement)
{
aimPointAtSurf[0] = currentPosition[0] + servoMotionMaxDistanceIncrement * refDeltaDirection[0];
aimPointAtSurf[1] = currentPosition[1] + servoMotionMaxDistanceIncrement * refDeltaDirection[1];
aimPointAtSurf[2] = currentPosition[2] + servoMotionMaxDistanceIncrement * refDeltaDirection[2];
}
nextTcpPosition[0] = aimPointAtSurf[0];
nextTcpPosition[1] = aimPointAtSurf[1];
nextTcpPosition[2] = aimPointAtSurf[2];
// 力变化
double forceDirectionZIncrement = 0.0;
bool ifContinueForceTrack;
lock (enableLocker)
{
ifContinueForceTrack = servoMotionForceTrackEnable;
}
if (ifContinueForceTrack)
{
double differenceForceZ = referenceForce[2] - (-refForce);
if (Math.Abs(differenceForceZ) <= servoMotionMinAvailableForce)
{
forceDirectionZIncrement = 0.0;
}
else if (Math.Abs(differenceForceZ) >= servoMotionMaxAvailableForce)
{
forceDirectionZIncrement = Math.Sign(differenceForceZ) * servoMotionMaxIncrement;
}
else
{
forceDirectionZIncrement = Math.Sign(differenceForceZ) * ((Math.Abs(differenceForceZ) - servoMotionMinAvailableForce) / (servoMotionMaxAvailableForce - servoMotionMinAvailableForce) * (servoMotionMaxIncrement - servoMotionMinIncrement) + servoMotionMinIncrement);
}
nextTcpPosition[0] += (currentZAxisAtBase[0] * forceDirectionZIncrement);
nextTcpPosition[1] += (currentZAxisAtBase[1] * forceDirectionZIncrement);
nextTcpPosition[2] += (currentZAxisAtBase[2] * forceDirectionZIncrement);
}
// 记录数据
servoMotionRecordDatas.Add(new double[] { tcpRealPosition[0],
tcpRealPosition[1],
tcpRealPosition[2],
tcpRealPosition[3],
tcpRealPosition[4],
tcpRealPosition[5],
referenceForce[0],
referenceForce[1],
referenceForce[2],
referenceForce[3],
referenceForce[4],
referenceForce[5],
DateTime.Now.Year,
DateTime.Now.Month,
DateTime.Now.Day,
DateTime.Now.Hour,
DateTime.Now.Minute,
DateTime.Now.Second,
DateTime.Now.Millisecond,
DateTime.Now.Hour * 3600 * 1000 +
DateTime.Now.Minute * 60 * 1000 +
DateTime.Now.Second * 1000 +
DateTime.Now.Millisecond,
refFlag });
return(nextTcpPosition);
}
/// <summary>
/// 伺服运动模块中计算下一周期的Tcp位置
/// </summary>
/// <param name="tcpRealPosition">实时Tcp坐标</param>
/// <param name="referenceForce">参考力信号</param>
/// <returns>返回下一周期的Tcp位置</returns>
protected override double[] ServoMotionNextTcpPosition(double[] tcpRealPosition, double[] referenceForce, double[] moreInfo = null)
{
double[] nextTcpPosition = (double[])tcpRealPosition.Clone();
// 暂停运动
if (servoMotionActivePause)
{
return(nextTcpPosition);
}
// 计算当前位置在球面上的最佳投影位置
double[] realArray = new double[] { tcpRealPosition[0] - servoMotionPointedPosition[0], tcpRealPosition[1] - servoMotionPointedPosition[1], tcpRealPosition[2] - servoMotionPointedPosition[2] };
double realArrayMagnitude = URMath.LengthOfArray(realArray);
double[] realDirection = new double[] { realArray[0] / realArrayMagnitude, realArray[1] / realArrayMagnitude, realArray[2] / realArrayMagnitude };
double[] realLatitudeDirection = new double[] { realDirection[0], realDirection[1], 0.0 };
double realLongitudeAngle = servoMotionLongitudeRefAngle - ((Math.Abs(realDirection[2]) > 1.0) ? Math.Acos((double)Math.Sign(realDirection[2])) : Math.Acos(realDirection[2]));
double realLatitudeAngleMagnitudeCos = URMath.VectorDotMultiply(realLatitudeDirection, servoMotionLatitudeRefAngleArray) / URMath.LengthOfArray(realLatitudeDirection) / URMath.LengthOfArray(servoMotionLatitudeRefAngleArray);
double realLatitudeAngleMagnitude = Math.Abs(realLatitudeAngleMagnitudeCos) > 1.0 ? Math.Acos((double)Math.Sign(realLatitudeAngleMagnitudeCos)) : Math.Acos(realLatitudeAngleMagnitudeCos);
double[] realCrossResult = URMath.VectorCrossMultiply(realLatitudeDirection, servoMotionLatitudeRefAngleArray);
double realLatitudeAngle = realCrossResult[2] > 0 ? -realLatitudeAngleMagnitude : realLatitudeAngleMagnitude;
// 计算力传感器给出的移动角度
double longitudeForce = 0.0;
double latitudeForce = 0.0;
switch (servoMotionLongitudeForce)
{
case ServoOccupiedForce.ForceX:
longitudeForce = referenceForce[0] * servoMotionLongtitudeForceSign;
break;
case ServoOccupiedForce.ForceY:
longitudeForce = referenceForce[1] * servoMotionLongtitudeForceSign;
break;
default:
longitudeForce = 0.0;
break;
}
switch (servoMotionLatitudeForce)
{
case ServoOccupiedForce.ForceX:
latitudeForce = referenceForce[0] * servoMotionLatitudeForceSign;
break;
case ServoOccupiedForce.ForceY:
latitudeForce = referenceForce[1] * servoMotionLatitudeForceSign;
break;
default:
latitudeForce = 0.0;
break;
}
double longitudeAngle = 0.0;
double latitudeAngle = 0.0;
if (Math.Abs(longitudeForce) < servoMotionMinAvailableForce)
{
longitudeAngle = 0.0;
}
else if (Math.Abs(longitudeForce) > servoMotionMaxAvailableForce)
{
longitudeAngle = Math.Sign(longitudeForce) * servoMotionAngleMaxIncrement;
}
else
{
longitudeAngle = Math.Sign(longitudeForce) * (Math.Abs(longitudeForce) - servoMotionMinAvailableForce) / (servoMotionMaxAvailableForce - servoMotionMinAvailableForce) * servoMotionAngleMaxIncrement;
}
if (Math.Abs(latitudeForce) < servoMotionMinAvailableForce)
{
latitudeAngle = 0.0;
}
else if (Math.Abs(latitudeForce) > servoMotionMaxAvailableForce)
{
latitudeAngle = Math.Sign(latitudeForce) * servoMotionAngleMaxIncrement;
}
else
{
latitudeAngle = Math.Sign(latitudeForce) * (Math.Abs(latitudeForce) - servoMotionMinAvailableForce) / (servoMotionMaxAvailableForce - servoMotionMinAvailableForce) * servoMotionAngleMaxIncrement;
}
// 根据角度限制目标角度
double aimLongitudeAngle = realLongitudeAngle + longitudeAngle;
aimLongitudeAngle = aimLongitudeAngle > servoMotionUpBiasAngle ? servoMotionUpBiasAngle : aimLongitudeAngle;
aimLongitudeAngle = aimLongitudeAngle < -servoMotionDownBiasAngle ? -servoMotionDownBiasAngle : aimLongitudeAngle;
double aimLatitudeAngle = realLatitudeAngle + latitudeAngle;
aimLatitudeAngle = aimLatitudeAngle > servoMotionRightBiasAngle ? servoMotionRightBiasAngle : aimLatitudeAngle;
aimLatitudeAngle = aimLatitudeAngle < -servoMotionLeftBiasAngle ? -servoMotionLeftBiasAngle : aimLatitudeAngle;
// 计算目标位置
double longitudeRotateAngle = (Math.PI / 2.0 - servoMotionLongitudeRefAngle) + aimLongitudeAngle;
double radiusAfterLongitudeTransfer = servoMotionKeepDistance * Math.Cos(longitudeRotateAngle);
double[] nextPosition = new double[] {
servoMotionPointedPosition[0] + servoMotionLatitudeRefAngleArray[0] * radiusAfterLongitudeTransfer *Math.Cos(aimLatitudeAngle) + servoMotionLatitudeTangentialArray[0] * radiusAfterLongitudeTransfer *Math.Sin(aimLatitudeAngle),
servoMotionPointedPosition[1] + servoMotionLatitudeRefAngleArray[1] * radiusAfterLongitudeTransfer *Math.Cos(aimLatitudeAngle) + servoMotionLatitudeTangentialArray[1] * radiusAfterLongitudeTransfer *Math.Sin(aimLatitudeAngle),
servoMotionPointedPosition[2] + servoMotionKeepDistance * Math.Sin(longitudeRotateAngle)
};
// 计算目标姿态
Quatnum qFirstRotateFromBaseToTcp = URMath.AxisAngle2Quatnum(new double[] { servoMotionBeginTcpPosition[3], servoMotionBeginTcpPosition[4], servoMotionBeginTcpPosition[5] });
Quatnum qSecondRotateLongitudeDirection = URMath.AxisAngle2Quatnum(new double[] { -servoMotionLatitudeTangentialArray[0] * aimLongitudeAngle, -servoMotionLatitudeTangentialArray[1] * aimLongitudeAngle, -servoMotionLatitudeTangentialArray[2] * aimLongitudeAngle });
Quatnum qThirdRotateLatitudeDirection = URMath.AxisAngle2Quatnum(new double[] { 0.0, 0.0, aimLatitudeAngle });
double[] nextPosture = URMath.Quatnum2AxisAngle(
URMath.QuatnumRotate(new Quatnum[] { qFirstRotateFromBaseToTcp, qSecondRotateLongitudeDirection, qThirdRotateLatitudeDirection }));
// 是否更新目标值
if (longitudeAngle != 0.0 || latitudeAngle != 0.0)
{
servoMotionLastNextPositionOnSphere[0] = nextPosition[0];
servoMotionLastNextPositionOnSphere[1] = nextPosition[1];
servoMotionLastNextPositionOnSphere[2] = nextPosition[2];
servoMotionLastNextPositionOnSphere[3] = nextPosture[0];
servoMotionLastNextPositionOnSphere[4] = nextPosture[1];
servoMotionLastNextPositionOnSphere[5] = nextPosture[2];
}
// 计算下一个位姿
nextTcpPosition[0] = servoMotionLastNextPositionOnSphere[0];
nextTcpPosition[1] = servoMotionLastNextPositionOnSphere[1];
nextTcpPosition[2] = servoMotionLastNextPositionOnSphere[2];
nextTcpPosition[3] = servoMotionLastNextPositionOnSphere[3];
nextTcpPosition[4] = servoMotionLastNextPositionOnSphere[4];
nextTcpPosition[5] = servoMotionLastNextPositionOnSphere[5];
// 记录数据
if (servoMotionRecordDatas.Count >= 15000)
{
servoMotionRecordDatas.RemoveAt(0);
}
servoMotionRecordDatas.Add(new double[] { tcpRealPosition[0],
tcpRealPosition[1],
tcpRealPosition[2],
tcpRealPosition[3],
tcpRealPosition[4],
tcpRealPosition[5],
referenceForce[0],
referenceForce[1],
referenceForce[2],
referenceForce[3],
referenceForce[4],
referenceForce[5] });
return(nextTcpPosition);
}
/// <summary>
/// 伺服运动模块中计算下一周期的Tcp位置
/// </summary>
/// <param name="tcpRealPosition">实时Tcp坐标</param>
/// <param name="referenceForce">参考力信号</param>
/// <returns>返回下一周期的Tcp位置</returns>
protected override double[] ServoMotionNextTcpPosition(double[] tcpRealPosition, double[] referenceForce, double[] moreInfo = null)
{
double[] nextTcpPosition = (double[])tcpRealPosition.Clone();
// 暂停运动
if (servoMotionActivePause)
{
return(nextTcpPosition);
}
// 获得实时运动方向在Base系的表示
double[] tcpToBase = URMath.ReverseReferenceRelationship(tcpRealPosition);
Quatnum qTcpToBase = URMath.AxisAngle2Quatnum(new double[] { tcpToBase[3], tcpToBase[4], tcpToBase[5] });
double[] movingDirectionAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(servoMotionMovingDirectionArrayAtTcp, qTcpToBase);
double[] detectingDirectionAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(servoMotionDetectingDirectionArrayAtTcp, qTcpToBase);
// 运动方向上加一定的增量
for (int k = 0; k < 3; k++)
{
nextTcpPosition[k] += movingDirectionAtBase[k] * servoMotionMovingPeriodicalIncrement;
}
// 力保持方向上加一定的增量
double differenceForce = URMath.VectorDotMultiply(referenceForce, servoMotionDetectingDirectionArrayAtTcp) - servoMotionPreservedForce[0];
double forceDirectionIncrement = 0.0;
if (Math.Abs(differenceForce) <= servoMotionDetectingMinAvailableForce)
{
forceDirectionIncrement = 0.0;
}
else if (Math.Abs(differenceForce) >= servoMotionDetectingMaxAvailableForce)
{
forceDirectionIncrement = Math.Sign(differenceForce) * servoMotionDetectingPeriodicalMaxIncrement;
}
else
{
forceDirectionIncrement = Math.Sign(differenceForce) * ((Math.Abs(differenceForce) - servoMotionDetectingMinAvailableForce) / (servoMotionDetectingMaxAvailableForce - servoMotionDetectingMinAvailableForce) * (servoMotionDetectingPeriodicalMaxIncrement - servoMotionDetectingPeriodicalMinIncrement) + servoMotionDetectingPeriodicalMinIncrement);
}
for (int k = 0; k < 3; k++)
{
nextTcpPosition[k] += detectingDirectionAtBase[k] * forceDirectionIncrement;
}
for (int k = 0; k < 3; k++)
{
nextTcpPosition[k] += detectingDirectionAtBase[k] * forceDirectionIncrement;
}
// 当前运动坐标系中的坐标获取
double[] arrayP = new double[] { tcpRealPosition[0] - servoMotionBeginTcpPosition[0], tcpRealPosition[1] - servoMotionBeginTcpPosition[1], tcpRealPosition[2] - servoMotionBeginTcpPosition[2] };
double coordinateV = URMath.VectorDotMultiply(arrayP, servoMotionInitialMovingDirectionArrayAtBase);
double coordinateD = URMath.VectorDotMultiply(arrayP, servoMotionInitialDetectingDirectionArrayAtBase);
// 当前角度获得
double currentAngle = Math.Acos(URMath.VectorDotMultiply(detectingDirectionAtBase, servoMotionInitialDetectingDirectionArrayAtBase));
currentAngle = currentAngle < 0.0 ? 0.0 : currentAngle;
currentAngle = currentAngle > Math.PI / 2.0 ? Math.PI / 2.0 : currentAngle;
// 保存关键数据
servoMotionMovingDirectionCordinates.Add(coordinateV);
servoMotionDetectingForceDirectionCordinates.Add(coordinateD);
servoMotionVibratingAngleCordinates.Add(currentAngle);
int currentIndex = servoMotionVibratingAngleCordinates.Count - 1;
double[] distanceJudgeArray = new double[] { coordinateV - servoMotionMovingDirectionCordinates[servoMotionLastIndex], coordinateD - servoMotionDetectingForceDirectionCordinates[servoMotionLastIndex], 0.0 };
// 矫正摩擦力控制 离散式
double deltaForce = 0.0;
double judgeThisRound = 0.0;
double theta = 0.0;
if (!servoMotionVibrateAngleIgnoreRegionBeyond && URMath.LengthOfArray(distanceJudgeArray) > servoMotionIgnoreJudgeSpan)
{
servoMotionVibrateAngleIgnoreRegionBeyond = true;
servoMotionLastIndex = currentIndex;
}
if (!servoMotionVibrateAngleModifyBegin && URMath.LengthOfArray(distanceJudgeArray) > servoMotionJudgeStep)
{
servoMotionVibrateAngleModifyBegin = true;
}
if (servoMotionIfRecorrectRotateAngle && servoMotionVibrateAngleModifyBegin)
{
if (URMath.LengthOfArray(distanceJudgeArray) > servoMotionJudgeStep)
{
theta = GetPredictAngleAfterLimitation(servoMotionLastIndex, currentIndex);
deltaForce = TuneFrictionTranslation(servoMotionVibratingAnglePredictAfterAccum.Count - 1, servoMotionVibratingAnglePredictAfterAccum.Count - 1, servoMotionLastIndex, currentIndex);
judgeThisRound = 1.0;
servoMotionLastIndex = (int)Math.Round(currentIndex - (currentIndex - servoMotionLastIndex) * servoMotionOverlappingRate);
}
}
// 当前正压力和摩擦力
double currentFraction = -URMath.VectorDotMultiply(referenceForce, servoMotionMovingDirectionArrayAtTcp);
double currentNormForce = -URMath.VectorDotMultiply(referenceForce, servoMotionDetectingDirectionArrayAtTcp);
// 摩擦力控制摆动
double factor = 1.0 / (1.0 + Math.Exp(-8.0 * currentNormForce + 20.0));
double refFraction = (1 - factor) * (0.07632 * currentNormForce + 0.02814) + factor * (0.1404 * currentNormForce + 0.02702) + servoMotionAdjustBias;
double diffFraction = refFraction - currentFraction;
double deltaAngle = diffFraction * servoMotionFromFrictionToAngle;
// 摆动速度限幅
if (Math.Abs(deltaAngle) > servoMotionVibratingPeriodicalMaxIncrement)
{
deltaAngle = Math.Sign(deltaAngle) * servoMotionVibratingPeriodicalMaxIncrement;
}
double predictAngle = currentAngle + deltaAngle - servoMotionInitialAngle;
if (predictAngle < 0)
{
predictAngle = 0;
}
if (predictAngle > servoMotionUpBoundAngle - servoMotionInitialAngle)
{
predictAngle = servoMotionUpBoundAngle - servoMotionInitialAngle;
}
double[] nextPosture = URMath.Quatnum2AxisAngle(
URMath.QuatnumRotate(new Quatnum[] {
URMath.AxisAngle2Quatnum(new double[] { servoMotionBeginTcpPosition[3], servoMotionBeginTcpPosition[4], servoMotionBeginTcpPosition[5] }),
URMath.AxisAngle2Quatnum(new double[] { predictAngle *servoMotionInitialVibratingDirectionArrayAtBase[0], predictAngle *servoMotionInitialVibratingDirectionArrayAtBase[1], predictAngle *servoMotionInitialVibratingDirectionArrayAtBase[2] })
}));
if (servoMotionIfAttitudeChange && servoMotionVibrateAngleIgnoreRegionBeyond)
{
nextTcpPosition[3] = nextPosture[0];
nextTcpPosition[4] = nextPosture[1];
nextTcpPosition[5] = nextPosture[2];
}
// 记录数据
servoMotionRecordDatas.Add(new double[] { tcpRealPosition[0],
tcpRealPosition[1],
tcpRealPosition[2],
tcpRealPosition[3],
tcpRealPosition[4],
tcpRealPosition[5],
referenceForce[0],
referenceForce[1],
referenceForce[2],
referenceForce[3],
referenceForce[4],
referenceForce[5],
coordinateV,
coordinateD,
currentAngle,
judgeThisRound,
predictAngle,
deltaForce,
servoMotionAdjustBias,
servoMotionInstantBias,
servoMotionLastIndex,
theta,
differenceForce,
forceDirectionIncrement });
return(nextTcpPosition);
}
/// <summary>
/// 伺服运动模块旋转一定的初始角度
/// </summary>
protected async void RotateAboutInitialAngle()
{
if (servoMotionIfFindInitialAngle)
{
await Task.Delay(500);
await Task.Run(new Action(() =>
{
double[] forces = internalProcessor.ServoGetAverageForces();
double forceOnMoving = URMath.VectorDotMultiply(servoMotionMovingDirectionArrayAtTcp, new double[] { forces[0], forces[1], forces[2] });
double forceOnDetecting = URMath.VectorDotMultiply(servoMotionDetectingDirectionArrayAtTcp, new double[] { forces[0], forces[1], forces[2] });
if (forceOnMoving < 0.0)
{
servoMotionInitialAngle = 0.0;
}
else
{
double theta = Math.Atan2(Math.Abs(forceOnMoving), Math.Abs(forceOnDetecting));
if (theta < 0.0 || theta > Math.PI / 2.0)
{
servoMotionInitialAngle = 0.0;
}
servoMotionInitialAngle = theta;
}
}));
await Task.Run(new Action(() =>
{
double[] tcpCurrentPosition = internalProcessor.PositionsTcpActual;
double[] nextPosition = new double[] { tcpCurrentPosition[0], tcpCurrentPosition[1], tcpCurrentPosition[2] };
for (int k = 0; k < 3; k++)
{
nextPosition[k] += servoMotionInitialDetectingDirectionArrayAtBase[k] * (-servoMotionDetectForceBackDistance);
}
double[] nextPosture = URMath.Quatnum2AxisAngle(
URMath.QuatnumRotate(new Quatnum[] {
URMath.AxisAngle2Quatnum(new double[] { tcpCurrentPosition[3], tcpCurrentPosition[4], tcpCurrentPosition[5] }),
URMath.AxisAngle2Quatnum(new double[] { servoMotionInitialAngle *servoMotionInitialVibratingDirectionArrayAtBase[0], servoMotionInitialAngle *servoMotionInitialVibratingDirectionArrayAtBase[1], servoMotionInitialAngle *servoMotionInitialVibratingDirectionArrayAtBase[2] })
}));
internalProcessor.SendURCommanderMoveL(new double[] { nextPosition[0], nextPosition[1], nextPosition[2], nextPosture[0], nextPosture[1], nextPosture[2] }, 0.1, 0.1);
}));
await Task.Run(new Action(() =>
{
Thread.Sleep(2000);
while (internalProcessor.ProgramState == (double)URDataProcessor.RobotProgramStatus.Running)
{
Thread.Sleep(200);
}
// 执行扫查力度校验，方向根据所测得的力定
Thread.Sleep(200);
internalProcessor.nonServoFindForceTranslationModule.NonServoMotionSetAndBegin((URNonServo.NonServoFindForceTranslation.NonServoDirectionAtTcp)((byte)servoMotionDetectingDirectionAtTcp),
internalProcessor.PositionsTcpActual,
0.0005, 0.005,
servoMotionTargetForceKeep, true);
Thread.Sleep(800);
while (internalProcessor.ProgramState == (double)URDataProcessor.RobotProgramStatus.Running)
{
Thread.Sleep(200);
}
}));
}
// 初始化力保持的值
servoMotionPreservedForce[0] = 0.0;
// 初始化姿态实时相对角度
servoMotionVibratingCurrentAngle = 0.0;
// 初始化其余必要控制参数
servoMotionAdjustBias = servoMotionAdjustFrictionBiasIni;
servoMotionInstantBias = servoMotionAdjustFrictionBiasIni;
servoMotionMovingDirectionCordinates.Clear();
servoMotionDetectingForceDirectionCordinates.Clear();
servoMotionVibratingAngleCordinates.Clear();
servoMotionVibratingAnglePredict.Clear();
servoMotionVibratingAnglePredictAfterFilter.Clear();
servoMotionVibratingAnglePredictAfterAccum.Clear();
servoMotionLastIndex = 0;
servoMotionVibrateAngleModifyBegin = false;
servoMotionVibrateAngleIgnoreRegionBeyond = false;
servoMotionMeanAngleChange = 0.0;
servoMotionAccmuErr = 0.0;
// 打开伺服模块时对一般逻辑的处理
internalProcessor.ServoSwitchMode(true);
// 开始本伺服模式
ServoMotionBegin();
}
/// <summary>
/// 伺服运动模块中计算下一周期的Tcp位置
/// </summary>
/// <param name="tcpRealPosition">实时Tcp坐标</param>
/// <param name="referenceForce">参考力信号</param>
/// <returns>返回下一周期的Tcp位置</returns>
protected override double[] ServoMotionNextTcpPosition(double[] tcpRealPosition, double[] referenceForce, double[] moreInfo = null)
{
double[] nextTcpPosition = (double[])tcpRealPosition.Clone();
// 暂停运动
if (servoMotionActivePause)
{
return(nextTcpPosition);
}
// 运动方向上加一定的增量
for (int k = 0; k < 3; k++)
{
nextTcpPosition[k] += servoMotionMovingDirectionAtBase[k] * servoMotionMovingPeriodicalIncrement;
}
// 力保持方向上加一定的增量
double differenceForce = URMath.VectorDotMultiply(referenceForce, servoMotionDetectingDirectionAtBase) - servoMotionPreservedForce[0];
double forceDirectionIncrement = 0.0;
if (Math.Abs(differenceForce) <= servoMotionDetectingMinAvailableForce)
{
forceDirectionIncrement = 0.0;
}
else if (Math.Abs(differenceForce) >= servoMotionDetectingMaxAvailableForce)
{
forceDirectionIncrement = Math.Sign(differenceForce) * servoMotionDetectingPeriodicalMaxIncrement;
}
else
{
forceDirectionIncrement = Math.Sign(differenceForce) * ((Math.Abs(differenceForce) - servoMotionDetectingMinAvailableForce) / (servoMotionDetectingMaxAvailableForce - servoMotionDetectingMinAvailableForce) * (servoMotionDetectingPeriodicalMaxIncrement - servoMotionDetectingPeriodicalMinIncrement) + servoMotionDetectingPeriodicalMinIncrement);
}
for (int k = 0; k < 3; k++)
{
nextTcpPosition[k] += servoMotionDetectingDirectionAtBase[k] * forceDirectionIncrement;
}
double predictAngleBeforeNormalization = 0.0;
double predictAngle = 0.0;
// 姿态矫正
if (servoMotionIfAttitudeChange)
{
double[] moveArray = new double[] { tcpRealPosition[0] - servoMotionBeginTcpPosition[0], tcpRealPosition[1] - servoMotionBeginTcpPosition[1], tcpRealPosition[2] - servoMotionBeginTcpPosition[2] };
double motionProportion = 0.0;
if (servoMotionAngleInterpolationRelyDirection == 'm')
{
motionProportion = URMath.VectorDotMultiply(moveArray, servoMotionMovingDirectionAtBase) / servoMotionMovingDirectionStopDistance;
}
else
{
motionProportion = URMath.VectorDotMultiply(moveArray, servoMotionDetectingDirectionAtBase) / servoMotionDetectingDirectionStopDistance;
}
if (motionProportion < 0.0)
{
motionProportion = 0.0;
}
if (motionProportion > 1.0)
{
motionProportion = 1.0;
}
predictAngleBeforeNormalization = motionProportion * servoMotionEndAngle;
if (Math.Abs(predictAngleBeforeNormalization) < Math.Abs(servoMotionProcessAngle))
{
predictAngle = servoMotionProcessAngle;
}
else
{
predictAngle = predictAngleBeforeNormalization;
servoMotionProcessAngle = predictAngle;
}
double[] nextPosture = URMath.Quatnum2AxisAngle(
URMath.QuatnumRotate(new Quatnum[] {
URMath.AxisAngle2Quatnum(new double[] { servoMotionBeginTcpPosition[3], servoMotionBeginTcpPosition[4], servoMotionBeginTcpPosition[5] }),
URMath.AxisAngle2Quatnum(new double[] { predictAngle *servoMotionVibratingDirectionAtBase[0], predictAngle *servoMotionVibratingDirectionAtBase[1], predictAngle *servoMotionVibratingDirectionAtBase[2] })
}));
nextTcpPosition[3] = nextPosture[0];
nextTcpPosition[4] = nextPosture[1];
nextTcpPosition[5] = nextPosture[2];
}
// 记录数据
servoMotionRecordDatas.Add(new double[] { tcpRealPosition[0],
tcpRealPosition[1],
tcpRealPosition[2],
tcpRealPosition[3],
tcpRealPosition[4],
tcpRealPosition[5],
referenceForce[0],
referenceForce[1],
referenceForce[2],
referenceForce[3],
referenceForce[4],
referenceForce[5],
predictAngleBeforeNormalization,
predictAngle });
return(nextTcpPosition);
}
/// <summary>
/// 更新控制目标参考值
/// </summary>
/// <param name="flag">运动标号</param>
/// <param name="values">运动参考值</param>
public void RefreshAimPostion(int flag, double[] values)
{
if (workingStatus == WorkStatus.WorkRunning)
{
double posFactor, attFactor, fosFactor;
bool posEnable, attEnable, fosKeepEnable, fosTrackEnable;
lock (controlParamLocker)
{
posFactor = positionOverride;
attFactor = angleOverride;
fosFactor = forceOverride;
posEnable = ifEnableTranslationTrack;
attEnable = ifEnableAttitudeTrack;
fosKeepEnable = ifEnableForceKeep;
fosTrackEnable = ifEnableForceTrack;
}
int nextFlag = flag;
double xDist = values[0] * posFactor;
double yDist = values[1] * posFactor;
double[] posture = new double[] { values[2] * attFactor,
values[3] * attFactor,
values[4] * attFactor };
double force = values[5] * fosFactor;
// 1. 检验距离是否太大
if (URMath.LengthOfArray(new double[] { xDist, yDist, 0 }) > maxAvailableRadius)
{
double theta = Math.Atan2(yDist, xDist);
xDist = maxAvailableRadius * Math.Cos(theta);
yDist = maxAvailableRadius * Math.Sin(theta);
}
// 2. 检测转角是否过大
double recAngle = URMath.LengthOfArray(posture);
if (recAngle > maxAvailableAngle)
{
posture[0] = posture[0] / recAngle * maxAvailableAngle;
posture[1] = posture[1] / recAngle * maxAvailableAngle;
posture[2] = posture[2] / recAngle * maxAvailableAngle;
}
// 3. 转换到当前的目标姿态
Quatnum initialQ = URMath.AxisAngle2Quatnum(
new double[] { startTcpPostion[3], startTcpPostion[4], startTcpPostion[5] });
double rotateAngle = URMath.LengthOfArray(posture);
double[] rotateAxisAtBase;
if (Math.Abs(rotateAngle) < 0.001)
{
rotateAxisAtBase = new double[] { 0.0, 0.0, 0.0 }
}
;
else
{
rotateAxisAtBase =
URMath.FindDirectionToSecondReferenceFromFirstReference
(new double[] { posture[0] / rotateAngle, posture[1] / rotateAngle, posture[2] / rotateAngle },
URMath.InvQuatnum(initialQ));
}
Quatnum rotateQ = URMath.AxisAngle2Quatnum(new double[] { rotateAxisAtBase[0] * rotateAngle, rotateAxisAtBase[1] * rotateAngle, rotateAxisAtBase[2] * rotateAngle });
double[] aimAttitude = URMath.Quatnum2AxisAngle(
URMath.QuatnumRotate(new Quatnum[] { initialQ, rotateQ }));
// 4. 检测目标姿态是否过限
double[] aimZDirection =
URMath.FindDirectionToSecondReferenceFromFirstReference(
new double[] { 0.0, 0.0, 1.0 },
URMath.AxisAngle2Quatnum(aimAttitude));
if (URMath.VectorDotMultiply(
aimZDirection, new double[] { 0.0, 0.0, 1.0 }) > 0)
{
double aimAttitudeAngle = URMath.LengthOfArray(aimAttitude);
double[] aimAttitudeAxis = new double[] {
aimAttitude[0] / aimAttitudeAngle,
aimAttitude[1] / aimAttitudeAngle,
aimAttitude[2] / aimAttitudeAngle
};
double newSin = 0.0;
if (Math.Pow(aimAttitudeAxis[2], 2) > 0.5)
{
newSin = 1.0;
}
else
{
newSin = Math.Sqrt(0.5 / (1 - Math.Pow(aimAttitudeAxis[2], 2)));
}
if (Math.Abs(newSin) > 1.0)
{
newSin = Math.Sign(newSin) * 1.0;
}
double newAngle = 2.0 * Math.Asin(newSin);
if (newAngle == System.Double.NaN)
{
newAngle = Math.Sign(newSin) * Math.PI;
}
aimAttitude[0] = aimAttitudeAxis[0] * newAngle;
aimAttitude[1] = aimAttitudeAxis[1] * newAngle;
aimAttitude[2] = aimAttitudeAxis[2] * newAngle;
}
// 5. 将参数输入模块
// 输入模块的参数为：
// 相对起始扫描点的两维坐标，
// 相对基座原点的姿态，
// 接触压力
internalProcessor.servoTrackTranslationModule.ServoMotionRefreshRefValues(
nextFlag, new double?[] {
posEnable ? new double?(xDist) : null,
posEnable ? new double?(yDist) : null,
attEnable ? new double?(aimAttitude[0]) : null,
attEnable ? new double?(aimAttitude[1]) : null,
attEnable ? new double?(aimAttitude[2]) : null,
fosTrackEnable ? force : detectingBasicPreservedForce
});
// 6. 力保持使能更改
internalProcessor.servoTrackTranslationModule.ServoMotionForceTrackEnable = fosKeepEnable;
}
}