/// <summary>
/// 非伺服运动模块设置并开始
/// </summary>
/// <param name="MoveDirectionAtTcp">相对Tcp坐标系的运动方向</param>
/// <param name="tcpCurrentPosition">实时Tcp坐标</param>
/// <param name="Velocity">寻力方向运动速度</param>
/// <param name="Acceleration">寻力方向运动加速度</param>
/// <param name="DetectForce">寻力方向终止力大小</param>
/// <param name="IfAutoFindDirectionInDetail">是否自动寻找运动方向正负</param>
public void NonServoMotionSetAndBegin(NonServoDirectionAtTcp MoveDirectionAtTcp,
double[] tcpCurrentPosition,
double Velocity,
double Acceleration,
double DetectForce,
bool IfAutoFindDirectionInDetail = false)
{
// 设置是否自动寻找力正负方向
nonServoAutoMovingDirectionJudge = IfAutoFindDirectionInDetail;
// 设置寻力方向
nonServoMovingDirectionAtTcp = MoveDirectionAtTcp;
// 获得寻力方向在Base系中的表示
double[] tcpToBase = URMath.ReverseReferenceRelationship(tcpCurrentPosition);
Quatnum qTcpToBase = URMath.AxisAngle2Quatnum(new double[] { tcpToBase[3], tcpToBase[4], tcpToBase[5] });
switch (nonServoMovingDirectionAtTcp)
{
case NonServoDirectionAtTcp.PositiveX:
nonServoMovingDirectionArrayAtTcp = new double[] { 1.0, 0.0, 0.0 };
break;
case NonServoDirectionAtTcp.NegativeX:
nonServoMovingDirectionArrayAtTcp = new double[] { -1.0, 0.0, 0.0 };
break;
case NonServoDirectionAtTcp.PositiveY:
nonServoMovingDirectionArrayAtTcp = new double[] { 0.0, 1.0, 0.0 };
break;
case NonServoDirectionAtTcp.NegativeY:
nonServoMovingDirectionArrayAtTcp = new double[] { 0.0, -1.0, 0.0 };
break;
case NonServoDirectionAtTcp.PositiveZ:
nonServoMovingDirectionArrayAtTcp = new double[] { 0.0, 0.0, 1.0 };
break;
case NonServoDirectionAtTcp.NegativeZ:
nonServoMovingDirectionArrayAtTcp = new double[] { 0.0, 0.0, -1.0 };
break;
default:
break;
}
nonServoMovingDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(nonServoMovingDirectionArrayAtTcp, qTcpToBase);
// 设置寻力运动参数
nonServoMotionMovingSpeed = Velocity;
nonServoMotionMovingAcceleration = Acceleration;
// 设置终止力大小
nonServoMotionDetectingForce = DetectForce;
// 开启本非伺服模式
NonServoMotionBegin();
}
/// <summary>
/// 非伺服运动的准备工作
/// </summary>
/// <param name="tcpRealPosition">实时Tcp坐标</param>
/// <param name="referenceForce">参考力信号</param>
protected override void NonServoMotionGetReady(double[] tcpRealPosition, double[] referenceForce)
{
switch (nonServoMotionOpenRound)
{
case 0:
nonServoMovingDirectionReverse = false;
if (nonServoAutoMovingDirectionJudge)
{
double forceMagnitude = URMath.VectorDotMultiply(referenceForce, nonServoMovingDirectionArrayAtTcp);
if (Math.Abs(forceMagnitude) >= nonServoMotionDetectingForce)
{
nonServoMovingDirectionReverse = true;
}
}
break;
case 1:
case 2:
if (nonServoAutoMovingDirectionJudge && nonServoMovingDirectionReverse)
{
double forceMagnitude = URMath.VectorDotMultiply(referenceForce, nonServoMovingDirectionArrayAtTcp);
if (Math.Abs(forceMagnitude) >= nonServoMotionDetectingForce)
{
nonServoMovingDirectionReverse = true;
}
else
{
nonServoMovingDirectionReverse = false;
}
}
break;
case 3: // 第四个周期 更新移动指令数组
if (nonServoMovingDirectionReverse)
{
nonServoMotionMovingArray[0] = -nonServoMovingDirectionArrayAtBase[0] * nonServoMotionMovingSpeed;
nonServoMotionMovingArray[1] = -nonServoMovingDirectionArrayAtBase[1] * nonServoMotionMovingSpeed;
nonServoMotionMovingArray[2] = -nonServoMovingDirectionArrayAtBase[2] * nonServoMotionMovingSpeed;
}
else
{
nonServoMotionMovingArray[0] = nonServoMovingDirectionArrayAtBase[0] * nonServoMotionMovingSpeed;
nonServoMotionMovingArray[1] = nonServoMovingDirectionArrayAtBase[1] * nonServoMotionMovingSpeed;
nonServoMotionMovingArray[2] = nonServoMovingDirectionArrayAtBase[2] * nonServoMotionMovingSpeed;
}
nonServoMotionMovingArray[3] = 0.0;
nonServoMotionMovingArray[4] = 0.0;
nonServoMotionMovingArray[5] = 0.0;
break;
case 4: // 第五个周期 下达下位机指令并运行
internalProcessor.SendURCommanderSpeedL(nonServoMotionMovingArray, nonServoMotionMovingAcceleration, 3600);
break;
default:
break;
}
}
/// <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.VectorDotMultiply(moveArray, servoMotionMainStopDirectionArrayAtBase) > servoMotionMainStopDirectionStopDistance)
{
Logger.HistoryPrinting(Logger.Level.INFO, MethodBase.GetCurrentMethod().DeclaringType.FullName, "Main stop direction limitation reached.");
return(true);
}
if (servoMotionActiveDistanceOrRecurrentCondition == ServoStopMode.DistanceCondition)
{
// 副运动方向距离终止
if (Math.Abs(URMath.VectorDotMultiply(moveArray, servoMotionSubStopDirectionArrayAtBase)) > servoMotionSubStopDirectionStopDistance)
{
Logger.HistoryPrinting(Logger.Level.INFO, MethodBase.GetCurrentMethod().DeclaringType.FullName, "Sub stop direction limitation reached.");
return(true);
}
}
else
{
// 副运动方向回环终止
if (servoMotionOpenRound > servoMotionSubStopDirectionRecurrentCheckStartRound)
{
if (servoMotionSubStopDirectionRecurrentCheckSign * URMath.VectorDotMultiply(moveArray, servoMotionSubStopDirectionArrayAtBase) < servoMotionSubStopDirectionRecurrentCheckStopDistance)
{
Logger.HistoryPrinting(Logger.Level.INFO, MethodBase.GetCurrentMethod().DeclaringType.FullName, "Sub stop direction recurrent reached.");
return(true);
}
}
else if (servoMotionOpenRound == servoMotionSubStopDirectionRecurrentCheckStartRound)
{
servoMotionSubStopDirectionRecurrentCheckSign = Math.Sign(URMath.VectorDotMultiply(moveArray, servoMotionSubStopDirectionArrayAtBase));
servoMotionOpenRound++;
}
else
{
servoMotionOpenRound++;
}
}
return(false);
}
/// <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();
}
servoMotionPreservedForce[0] += URMath.VectorDotMultiply(referenceForce, servoMotionDetectingDirectionArrayAtTcp);
break;
case 1:
servoMotionPreservedForce[0] += URMath.VectorDotMultiply(referenceForce, servoMotionDetectingDirectionArrayAtTcp);
break;
case 2: // 第三个周期 清空数据记录
servoMotionRecordDatas.Clear();
servoMotionPreservedForce[0] += URMath.VectorDotMultiply(referenceForce, servoMotionDetectingDirectionArrayAtTcp);
break;
case 3: // 第四个周期 获得Tcp位置并下发传参端口作为初始值
servoMotionBeginTcpPosition = (double[])tcpRealPosition.Clone();
if (ifPort30004Used)
{
internalProcessor.SendURServorInputDatas(servoMotionBeginTcpPosition);
}
else
{
internalProcessor.SendURModbusInputDatas(servoMotionBeginTcpPosition);
}
servoMotionPreservedForce[0] += URMath.VectorDotMultiply(referenceForce, servoMotionDetectingDirectionArrayAtTcp);
break;
case 4: // 第五个周期 下达下位机指令并运行
servoMotionPreservedForce[0] += URMath.VectorDotMultiply(referenceForce, servoMotionDetectingDirectionArrayAtTcp);
servoMotionPreservedForce[0] /= servoMotionInitialRound;
servoMotionRecordDatas.Add(ServoMotionOutputConfiguration());
internalProcessor.SendURCommanderControllerCode();
break;
default:
break;
}
}
/// <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>
/// 非伺服运动模块是否到达终止条件
/// </summary>
/// <param name="tcpRealPosition">实时Tcp坐标</param>
/// <param name="referenceForce">参考力信号</param>
/// <returns>返回是否终止</returns>
protected override bool NonServoMotionIfFinished(double[] tcpRealPosition, double[] referenceForce)
{
// 寻力方向力大小终止
double forceMagnitude = URMath.VectorDotMultiply(referenceForce, nonServoMovingDirectionArrayAtTcp);
if (nonServoMovingDirectionReverse)
{
if (Math.Abs(forceMagnitude) <= nonServoMotionDetectingForce)
{
nonServoMotionEndForce = forceMagnitude;
return(true);
}
}
else
{
if (Math.Abs(forceMagnitude) >= nonServoMotionDetectingForce)
{
nonServoMotionEndForce = forceMagnitude;
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>
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>
/// 伺服运动模块设置并开始
/// </summary>
/// <param name="MoveDirectionAtTcp">相对Tcp坐标系的既定运动方向</param>
/// <param name="DetectDirectionAtTcp">相对Tcp坐标系的既定力保持方向</param>
/// <param name="tcpCurrentPosition">实时Tcp坐标</param>
/// <param name="MainStopDirectionAtTcp">相对Tcp坐标系的既定主运动停止方向</param>
/// <param name="SubStopDirectionAtTcp">相对Tcp坐标系的既定副运动停止方向</param>
/// <param name="MainStopDirectionStopDistance">在主运动方向上的终止距离</param>
/// <param name="SubStopDirectionStopDistance">在副运动方向上的终止距离</param>
/// <param name="SubStopDirectionRecurrentStopDistance">在副运动方向上的回环检查的终止距离</param>
/// <param name="StopWay">使用的终止条件</param>
/// <param name="ForwardSpeed">在运动方向上的速度</param>
/// <param name="ForceMaxSpeed">在力保持方向上的最大速度</param>
/// <param name="ForceMinSpeed">在力保持方向上的最小速度</param>
/// <param name="ForceMaxValue">在力保持方向上的最大可接受力值</param>
/// <param name="ForceMinValue">在力保持方向上的最小可接受力值</param>
/// <param name="VibrateMaxValue">在摆动方向上的最大速度</param>
/// <param name="VibrateForceToAngle">在摆动方向上的力误差对应的角度偏差</param>
/// <param name="VibrateAngleToForce">在摆动方向上的角度误差对应的力偏差</param>
/// <param name="TargetForceKeep">目标保持力</param>
/// <param name="IfRotate">是否改变姿态</param>
/// <param name="UpAngle">姿态改变角度上限</param>
/// <param name="IfFindInitialAngle">是否寻找初始姿态角</param>
/// <param name="IfRecorrectRotateAngle">是否矫正姿态角</param>
/// <param name="JudgeStep">矫正步长，隔多少距离矫正一次</param>
/// <param name="IgnoreJudgeSpan">忽略矫正长度，开始的时候隔多少距离开始进入矫正状态</param>
/// <param name="OverlappingRate">矫正采样重叠范围</param>
/// <param name="ControlPeriod">伺服运动周期</param>
/// <param name="LookAheadTime">伺服运动预计时间</param>
/// <param name="Gain">伺服运动增益</param>
public void ServoMotionSetAndBegin(ServoDirectionAtTcp MoveDirectionAtTcp,
ServoDirectionAtTcp DetectDirectionAtTcp,
double[] tcpCurrentPosition,
ServoDirectionAtTcp MainStopDirectionAtTcp,
ServoDirectionAtTcp SubStopDirectionAtTcp,
double MainStopDirectionStopDistance,
double SubStopDirectionStopDistance,
double SubStopDirectionRecurrentStopDistance,
ServoStopMode StopWay,
double ForwardSpeed,
double ForceMaxSpeed,
double ForceMinSpeed,
double ForceMaxValue,
double ForceMinValue,
double VibrateMaxValue,
double VibrateForceToAngle,
double VibrateAngleToForce,
double TargetForceKeep,
bool IfRotate,
double UpAngle,
bool IfFindInitialAngle = false,
bool IfRecorrectRotateAngle = false,
double IgnoreJudgeSpan = 0.001,
double JudgeStep = 0.003,
double OverlappingRate = 0.5,
double ControlPeriod = 0.008,
double LookAheadTime = 0.1,
double Gain = 200)
{
// 设定运动和力保持方向
servoMotionMovingDirectionAtTcp = MoveDirectionAtTcp;
servoMotionDetectingDirectionAtTcp = DetectDirectionAtTcp;
// 获得运动和力保持方向的Tcp系和Base系表示
double[] tcpToBase = URMath.ReverseReferenceRelationship(tcpCurrentPosition);
Quatnum qTcpToBase = URMath.AxisAngle2Quatnum(new double[] { tcpToBase[3], tcpToBase[4], tcpToBase[5] });
switch (servoMotionMovingDirectionAtTcp)
{
case ServoDirectionAtTcp.PositiveX:
servoMotionMovingDirectionArrayAtTcp = new double[] { 1.0, 0.0, 0.0 };
break;
case ServoDirectionAtTcp.NegativeX:
servoMotionMovingDirectionArrayAtTcp = new double[] { -1.0, 0.0, 0.0 };
break;
case ServoDirectionAtTcp.PositiveY:
servoMotionMovingDirectionArrayAtTcp = new double[] { 0.0, 1.0, 0.0 };
break;
case ServoDirectionAtTcp.NegativeY:
servoMotionMovingDirectionArrayAtTcp = new double[] { 0.0, -1.0, 0.0 };
break;
case ServoDirectionAtTcp.PositiveZ:
servoMotionMovingDirectionArrayAtTcp = new double[] { 0.0, 0.0, 1.0 };
break;
case ServoDirectionAtTcp.NegativeZ:
servoMotionMovingDirectionArrayAtTcp = new double[] { 0.0, 0.0, -1.0 };
break;
default:
break;
}
servoMotionInitialMovingDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(servoMotionMovingDirectionArrayAtTcp, qTcpToBase);
switch (servoMotionDetectingDirectionAtTcp)
{
case ServoDirectionAtTcp.PositiveX:
case ServoDirectionAtTcp.NegativeX:
servoMotionDetectingDirectionArrayAtTcp = new double[] { 1.0, 0.0, 0.0 };
break;
case ServoDirectionAtTcp.PositiveY:
case ServoDirectionAtTcp.NegativeY:
servoMotionDetectingDirectionArrayAtTcp = new double[] { 0.0, 1.0, 0.0 };
break;
case ServoDirectionAtTcp.PositiveZ:
case ServoDirectionAtTcp.NegativeZ:
servoMotionDetectingDirectionArrayAtTcp = new double[] { 0.0, 0.0, 1.0 };
break;
default:
break;
}
servoMotionInitialDetectingDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(servoMotionDetectingDirectionArrayAtTcp, qTcpToBase);
// 设定摆动方向
servoMotionVibratingDirectionArrayAtTcp = URMath.VectorCrossMultiply(servoMotionMovingDirectionArrayAtTcp, servoMotionDetectingDirectionArrayAtTcp);
servoMotionInitialVibratingDirectionArrayAtBase = URMath.VectorCrossMultiply(servoMotionInitialMovingDirectionArrayAtBase, servoMotionInitialDetectingDirectionArrayAtBase);
// 设定主运动停止方向和副运动停止方向
servoMotionMainStopDirectionAtTcp = MainStopDirectionAtTcp;
servoMotionSubStopDirectionAtTcp = SubStopDirectionAtTcp;
// 获得主运动和副运动停止方向的Base系表示
switch (servoMotionMainStopDirectionAtTcp)
{
case ServoDirectionAtTcp.PositiveX:
servoMotionMainStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 1.0, 0.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.NegativeX:
servoMotionMainStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { -1.0, 0.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.PositiveY:
servoMotionMainStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, 1.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.NegativeY:
servoMotionMainStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, -1.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.PositiveZ:
servoMotionMainStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, 0.0, 1.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.NegativeZ:
servoMotionMainStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, 0.0, -1.0 }, qTcpToBase);
break;
default:
break;
}
switch (servoMotionSubStopDirectionAtTcp)
{
case ServoDirectionAtTcp.PositiveX:
servoMotionSubStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 1.0, 0.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.NegativeX:
servoMotionSubStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { -1.0, 0.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.PositiveY:
servoMotionSubStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, 1.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.NegativeY:
servoMotionSubStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, -1.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.PositiveZ:
servoMotionSubStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, 0.0, 1.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.NegativeZ:
servoMotionSubStopDirectionArrayAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, 0.0, -1.0 }, qTcpToBase);
break;
default:
break;
}
// 设定主运动和副运动停止方向的最大行进距离、回环停止距离以及停止条件的选择
servoMotionMainStopDirectionStopDistance = MainStopDirectionStopDistance;
servoMotionSubStopDirectionStopDistance = SubStopDirectionStopDistance;
servoMotionSubStopDirectionRecurrentCheckStopDistance = SubStopDirectionRecurrentStopDistance;
servoMotionActiveDistanceOrRecurrentCondition = StopWay;
// 设定运动速度
servoMotionMovingPeriodicalIncrement = ForwardSpeed;
// 设定力保持方向速度和力限制
servoMotionDetectingPeriodicalMaxIncrement = ForceMaxSpeed;
servoMotionDetectingPeriodicalMinIncrement = ForceMinSpeed;
servoMotionDetectingMaxAvailableForce = ForceMaxValue;
servoMotionDetectingMinAvailableForce = ForceMinValue;
// 设定摆动方向的速度限制
servoMotionVibratingPeriodicalMaxIncrement = VibrateMaxValue;
// 设定摆动方向的角度与力之间的转换关系
servoMotionFromFrictionToAngle = VibrateForceToAngle;
servoMotionFromAngleToFriction = VibrateAngleToForce;
// 设定目标保持力
servoMotionTargetForceKeep = TargetForceKeep;
// 设定姿态是否更改，以及更改的上下限
servoMotionIfAttitudeChange = IfRotate;
servoMotionUpBoundAngle = UpAngle;
// 设定是否寻找初始姿态角
servoMotionIfFindInitialAngle = IfFindInitialAngle;
// 设定是否矫正姿态角
servoMotionIfRecorrectRotateAngle = IfRecorrectRotateAngle;
servoMotionIgnoreJudgeSpan = IgnoreJudgeSpan;
servoMotionJudgeStep = JudgeStep;
servoMotionOverlappingRate = OverlappingRate;
// 设置伺服参数并重写下位机控制程序
SetServoMotionParameters(ControlPeriod, LookAheadTime, Gain);
internalProcessor.WriteStringToControlCode(ControlPeriod, LookAheadTime, Gain);
// 准备查找初始姿态角
RotateAboutInitialAngle();
}
/// <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="MoveDirectionAtTcp">相对Tcp坐标系的既定运动方向</param>
/// <param name="DetectDirectionAtTcp">相对Tcp坐标系的既定力保持方向</param>
/// <param name="tcpCurrentPosition">实时Tcp坐标</param>
/// <param name="MoveDirectionStopDistance">在运动方向上的终止距离</param>
/// <param name="DetectDirectionStopDistance">在力保持方向上的终止距离</param>
/// <param name="DetectDirectionRecurrentStopDistance">在力保持方向上的回环检查的终止距离</param>
/// <param name="StopWay">使用的终止条件</param>
/// <param name="ForwardSpeed">在运动方向上的速度</param>
/// <param name="ForceMaxSpeed">在力保持方向上的最大速度</param>
/// <param name="ForceMinSpeed">在力保持方向上的最小速度</param>
/// <param name="ForceMaxValue">在力保持方向上的最大可接受力值</param>
/// <param name="ForceMinValue">在力保持方向上的最小可接受力值</param>
/// <param name="IfRotate">是否改变姿态</param>
/// <param name="ThatAngle">终点姿态改变角度</param>
/// <param name="RelyDirection">姿态改变依赖方向，可填m或者d表示运动方向或者力保持方向</param>
/// <param name="ControlPeriod">伺服运动周期</param>
/// <param name="LookAheadTime">伺服运动预计时间</param>
/// <param name="Gain">伺服运动增益</param>
public void ServoMotionSetAndBegin(ServoDirectionAtTcp MoveDirectionAtTcp,
ServoDirectionAtTcp DetectDirectionAtTcp,
double[] tcpCurrentPosition,
double MoveDirectionStopDistance,
double DetectDirectionStopDistance,
double DetectDirectionRecurrentStopDistance,
ServoStopMode StopWay,
double ForwardSpeed,
double ForceMaxSpeed,
double ForceMinSpeed,
double ForceMaxValue,
double ForceMinValue,
bool IfRotate,
double ThatAngle,
char RelyDirection,
double ControlPeriod = 0.008,
double LookAheadTime = 0.1,
double Gain = 200)
{
// 设定运动和力保持方向
servoMotionMovingDirectionAtTcp = MoveDirectionAtTcp;
servoMotionDetectingDirectionAtTcp = DetectDirectionAtTcp;
// 将运动和力保持方向转换到Base坐标系中
double[] tcpToBase = URMath.ReverseReferenceRelationship(tcpCurrentPosition);
Quatnum qTcpToBase = URMath.AxisAngle2Quatnum(new double[] { tcpToBase[3], tcpToBase[4], tcpToBase[5] });
double[] moveDirectionAtBase = new double[3];
double[] detectDirectionAtBase = new double[3];
switch (servoMotionMovingDirectionAtTcp)
{
case ServoDirectionAtTcp.PositiveX:
moveDirectionAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 1.0, 0.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.NegativeX:
moveDirectionAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { -1.0, 0.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.PositiveY:
moveDirectionAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, 1.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.NegativeY:
moveDirectionAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, -1.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.PositiveZ:
moveDirectionAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, 0.0, 1.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.NegativeZ:
moveDirectionAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, 0.0, -1.0 }, qTcpToBase);
break;
default:
break;
}
switch (servoMotionDetectingDirectionAtTcp)
{
case ServoDirectionAtTcp.PositiveX:
case ServoDirectionAtTcp.NegativeX:
detectDirectionAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 1.0, 0.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.PositiveY:
case ServoDirectionAtTcp.NegativeY:
detectDirectionAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, 1.0, 0.0 }, qTcpToBase);
break;
case ServoDirectionAtTcp.PositiveZ:
case ServoDirectionAtTcp.NegativeZ:
detectDirectionAtBase = URMath.FindDirectionToSecondReferenceFromFirstReference(new double[] { 0.0, 0.0, 1.0 }, qTcpToBase);
break;
default:
break;
}
// 伺服运动模块设置并开始
ServoMotionSetAndBegin(moveDirectionAtBase,
detectDirectionAtBase,
MoveDirectionStopDistance,
DetectDirectionStopDistance,
DetectDirectionRecurrentStopDistance,
StopWay,
ForwardSpeed,
ForceMaxSpeed,
ForceMinSpeed,
ForceMaxValue,
ForceMinValue,
IfRotate,
ThatAngle,
RelyDirection,
ControlPeriod,
LookAheadTime,
Gain);
}
/// <summary>
/// 伺服运动模块设置并开始
/// </summary>
/// <param name="MoveDirectionAtBase">相对Base坐标系的既定运动方向</param>
/// <param name="DetectDirectionAtBase">相对Base坐标系的既定力保持方向，需注意和运动方向正交</param>
/// <param name="MoveDirectionStopDistance">在运动方向上的终止距离</param>
/// <param name="DetectDirectionStopDistance">在力保持方向上的终止距离</param>
/// <param name="DetectDirectionRecurrentStopDistance">在力保持方向上的回环检查的终止距离</param>
/// <param name="StopWay">使用的终止条件</param>
/// <param name="ForwardSpeed">在运动方向上的速度</param>
/// <param name="ForceMaxSpeed">在力保持方向上的最大速度</param>
/// <param name="ForceMinSpeed">在力保持方向上的最小速度</param>
/// <param name="ForceMaxValue">在力保持方向上的最大可接受力值</param>
/// <param name="ForceMinValue">在力保持方向上的最小可接受力值</param>
/// <param name="IfRotate">是否改变姿态</param>
/// <param name="ThatAngle">终点姿态改变角度</param>
/// <param name="RelyDirection">姿态改变依赖方向，可填m或者d表示运动方向或者力保持方向</param>
/// <param name="ControlPeriod">伺服运动周期</param>
/// <param name="LookAheadTime">伺服运动预计时间</param>
/// <param name="Gain">伺服运动增益</param>
public void ServoMotionSetAndBegin(double[] MoveDirectionAtBase,
double[] DetectDirectionAtBase,
double MoveDirectionStopDistance,
double DetectDirectionStopDistance,
double DetectDirectionRecurrentStopDistance,
ServoStopMode StopWay,
double ForwardSpeed,
double ForceMaxSpeed,
double ForceMinSpeed,
double ForceMaxValue,
double ForceMinValue,
bool IfRotate,
double ThatAngle,
char RelyDirection,
double ControlPeriod = 0.008,
double LookAheadTime = 0.1,
double Gain = 200)
{
// 设定运动和力保持方向
servoMotionMovingDirectionAtBase = (double[])MoveDirectionAtBase.Clone();
servoMotionDetectingDirectionAtBase = (double[])DetectDirectionAtBase.Clone();
// 设定运动和力保持方向的最大行进距离、回环停止距离以及停止条件的选择
servoMotionMovingDirectionStopDistance = MoveDirectionStopDistance;
servoMotionDetectingDirectionStopDistance = DetectDirectionStopDistance;
servoMotionDetectingDirectionRecurrentCheckStopDistance = DetectDirectionRecurrentStopDistance;
servoMotionActiveDistanceOrRecurrentCondition = StopWay;
// 设定摆动方向
servoMotionVibratingDirectionAtBase = URMath.VectorCrossMultiply(servoMotionMovingDirectionAtBase, servoMotionDetectingDirectionAtBase);
// 设定运动速度
servoMotionMovingPeriodicalIncrement = ForwardSpeed;
// 设定力保持方向速度和力限制
servoMotionDetectingPeriodicalMaxIncrement = ForceMaxSpeed;
servoMotionDetectingPeriodicalMinIncrement = ForceMinSpeed;
servoMotionDetectingMaxAvailableForce = ForceMaxValue;
servoMotionDetectingMinAvailableForce = ForceMinValue;
// 设定姿态是否更改，以及更改的末值
servoMotionIfAttitudeChange = IfRotate;
servoMotionEndAngle = ThatAngle;
// 设定姿态更改依赖的方向
servoMotionAngleInterpolationRelyDirection = RelyDirection;
// 初始化力保持的值
servoMotionPreservedForce[0] = 0.0;
// 初始化过程相对角度
servoMotionProcessAngle = 0.0;
// 设置伺服参数并重写下位机控制程序
SetServoMotionParameters(ControlPeriod, LookAheadTime, Gain);
internalProcessor.WriteStringToControlCode(ControlPeriod, LookAheadTime, Gain);
// 打开伺服模块时对一般逻辑的处理
internalProcessor.ServoSwitchMode(true);
// 开始本伺服模式
ServoMotionBegin();
}
/// <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;
}
}
