/// <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);
}
