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