/// <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);
}
// 计算当前位置在球面上的最佳投影位置
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="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();
}