/// <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>
/// 伺服运动模块中计算下一周期的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="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>
/// 伺服运动模块设置并开始
/// </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);
}