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