/// <summary>
/// 伺服运动模块是否到达终止条件
/// </summary>
/// <param name="tcpRealPosition">实时Tcp坐标</param>
/// <returns>返回是否终止</returns>
protected override bool ServoMotionIfFinished(double[] tcpRealPosition)
{
// 继承基类判断
if (base.ServoMotionIfFinished(tcpRealPosition))
{
return(true);
}
// 可能的运动方向终止
double[] moveArray = new double[] { tcpRealPosition[0] - servoMotionBeginTcpPosition[0], tcpRealPosition[1] - servoMotionBeginTcpPosition[1], tcpRealPosition[2] - servoMotionBeginTcpPosition[2] };
if (URMath.LengthOfArray(moveArray) > servoMotionStopDistance)
{
Logger.HistoryPrinting(Logger.Level.INFO, MethodBase.GetCurrentMethod().DeclaringType.FullName, "Movement edge reached.");
return(true);
}
return(false);
}
/// <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>
/// 伺服运动的准备工作，包括可能的伺服数据交换设置，保持力记录，初始位置记录和发送，并开始下位机程序
/// </summary>
/// <param name="tcpRealPosition">实时Tcp坐标</param>
/// <param name="referenceForce">参考力信号</param>
protected override void ServoMotionGetReady(double[] tcpRealPosition, double[] referenceForce)
{
switch (servoMotionOpenRound)
{
case 0: // 第一个周期 传参端口设置
if (ifPort30004Used)
{
internalProcessor.SendURServorInputSetup();
}
break;
case 1:
break;
case 2: // 第三个周期 清空数据记录 获得Tcp位置，记录并下发传参端口作为初始值
servoMotionRecordDatas.Clear();
servoMotionBeginTcpPosition = (double[])tcpRealPosition.Clone();
servoMotionLastNextPositionOnSphere[0] = servoMotionBeginTcpPosition[0];
servoMotionLastNextPositionOnSphere[1] = servoMotionBeginTcpPosition[1];
servoMotionLastNextPositionOnSphere[2] = servoMotionBeginTcpPosition[2];
servoMotionLastNextPositionOnSphere[3] = servoMotionBeginTcpPosition[3];
servoMotionLastNextPositionOnSphere[4] = servoMotionBeginTcpPosition[4];
servoMotionLastNextPositionOnSphere[5] = servoMotionBeginTcpPosition[5];
if (ifPort30004Used)
{
internalProcessor.SendURServorInputDatas(servoMotionBeginTcpPosition);
}
else
{
internalProcessor.SendURModbusInputDatas(servoMotionBeginTcpPosition);
}
break;
case 3: // 第四个周期 计算基准角度或向量
servoMotionToolDirectionXAtBase = internalProcessor.XDirectionOfTcpAtBaseReference(servoMotionBeginTcpPosition);
servoMotionToolDirectionYAtBase = internalProcessor.YDirectionOfTcpAtBaseReference(servoMotionBeginTcpPosition);
servoMotionToolDirectionZAtBase = internalProcessor.ZDirectionOfTcpAtBaseReference(servoMotionBeginTcpPosition);
double[] initialArray = new double[] { servoMotionBeginTcpPosition[0] - servoMotionPointedPosition[0], servoMotionBeginTcpPosition[1] - servoMotionPointedPosition[1], servoMotionBeginTcpPosition[2] - servoMotionPointedPosition[2] };
servoMotionKeepDistance = URMath.LengthOfArray(initialArray);
servoMotionAngleMaxIncrement = servoMotionMaxIncrement / servoMotionKeepDistance;
double[] initialDirection = new double[] { initialArray[0] / servoMotionKeepDistance, initialArray[1] / servoMotionKeepDistance, initialArray[2] / servoMotionKeepDistance };
servoMotionLongitudeRefAngle = (Math.Abs(initialDirection[2]) > 1.0) ? Math.Acos((double)Math.Sign(initialDirection[2])) : Math.Acos(initialDirection[2]);
double servoMotionLatitudeRefAngleArrayMagnitude = URMath.LengthOfArray(new double[] { initialDirection[0], initialDirection[1], 0.0 });
servoMotionLatitudeRefAngleArray[0] = initialDirection[0] / servoMotionLatitudeRefAngleArrayMagnitude;
servoMotionLatitudeRefAngleArray[1] = initialDirection[1] / servoMotionLatitudeRefAngleArrayMagnitude;
servoMotionLatitudeRefAngleArray[2] = 0.0;
byte rollMode = internalProcessor.RollFlag;
switch (rollMode)
{
case 0:
servoMotionLongitudeForce = ServoOccupiedForce.ForceX;
servoMotionLatitudeForce = ServoOccupiedForce.ForceY;
servoMotionLongtitudeForceSign = 1;
servoMotionLatitudeForceSign = 1;
servoMotionLatitudeTangentialArray = internalProcessor.YDirectionOfTcpAtBaseReference(servoMotionBeginTcpPosition);
break;
case 1:
servoMotionLongitudeForce = ServoOccupiedForce.ForceY;
servoMotionLatitudeForce = ServoOccupiedForce.ForceX;
servoMotionLongtitudeForceSign = -1;
servoMotionLatitudeForceSign = 1;
servoMotionLatitudeTangentialArray = internalProcessor.XDirectionOfTcpAtBaseReference(servoMotionBeginTcpPosition);
break;
case 2:
servoMotionLongitudeForce = ServoOccupiedForce.ForceX;
servoMotionLatitudeForce = ServoOccupiedForce.ForceY;
servoMotionLongtitudeForceSign = -1;
servoMotionLatitudeForceSign = -1;
servoMotionLatitudeTangentialArray = internalProcessor.YDirectionOfTcpAtBaseReference(servoMotionBeginTcpPosition);
servoMotionLatitudeTangentialArray[0] = -servoMotionLatitudeTangentialArray[0];
servoMotionLatitudeTangentialArray[1] = -servoMotionLatitudeTangentialArray[1];
servoMotionLatitudeTangentialArray[2] = -servoMotionLatitudeTangentialArray[2];
break;
case 3:
servoMotionLongitudeForce = ServoOccupiedForce.ForceY;
servoMotionLatitudeForce = ServoOccupiedForce.ForceX;
servoMotionLongtitudeForceSign = 1;
servoMotionLatitudeForceSign = -1;
servoMotionLatitudeTangentialArray = internalProcessor.XDirectionOfTcpAtBaseReference(servoMotionBeginTcpPosition);
servoMotionLatitudeTangentialArray[0] = -servoMotionLatitudeTangentialArray[0];
servoMotionLatitudeTangentialArray[1] = -servoMotionLatitudeTangentialArray[1];
servoMotionLatitudeTangentialArray[2] = -servoMotionLatitudeTangentialArray[2];
break;
default:
servoMotionLongitudeForce = ServoOccupiedForce.ForceX;
servoMotionLatitudeForce = ServoOccupiedForce.ForceY;
servoMotionLongtitudeForceSign = 1;
servoMotionLatitudeForceSign = 1;
servoMotionLatitudeTangentialArray = internalProcessor.YDirectionOfTcpAtBaseReference(servoMotionBeginTcpPosition);
break;
}
break;
case 4: // 第五个周期 下达下位机指令并运行
servoMotionPreservedForce[0] = 0.0;
servoMotionPreservedForce[1] = 0.0;
servoMotionPreservedForce[2] = 0.0;
servoMotionRecordDatas.Add(ServoMotionOutputConfiguration());
internalProcessor.SendURCommanderControllerCode();
break;
default:
break;
}
}
/// <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>
/// <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;
}
}
