private void CheckA23Singularity(ITxRobot irobot) { TxRobot robot = irobot as TxRobot; if (robot != null) { double?jointA3HomeValue = null; foreach (TxPose pose in robot.PoseList) { if (pose.Name.Equals("HOME")) { TxPoseData poseDataHome = pose.PoseData; robot.CurrentPose = poseDataHome; jointA3HomeValue = (double)pose.Device.CurrentPose.JointValues[2]; break; } } if (jointA3HomeValue != null) { double a3SingularityValue = RwMathUtilities.Rad2Deg((double)jointA3HomeValue) - 90.0; double a3CurrentValue = RwMathUtilities.Rad2Deg((double)currentJointValues[2]); if (RwMathUtilities.Abs(a3CurrentValue - a3SingularityValue) < deltaA23Singularity) { AxisLimitsResult = string.IsNullOrEmpty(AxisLimitsResult) ? $"A23 Singularity: {a3CurrentValue:F1}" : string.Join("; ", AxisLimitsResult, $"A23 Singularity: {a3CurrentValue:F1}"); } } } }
/// <summary> /// Assign the joint values of the robot at the second location /// </summary> /// <param name="jointValues">The joint values of the robot in radians</param> private void AssignLocation2Values(ArrayList jointValues) { Axis1ValueLocation2 = RwMathUtilities.Rad2Deg((double)jointValues[0]); Axis2ValueLocation2 = RwMathUtilities.Rad2Deg((double)jointValues[1]); Axis3ValueLocation2 = RwMathUtilities.Rad2Deg((double)jointValues[2]); Axis4ValueLocation2 = RwMathUtilities.Rad2Deg((double)jointValues[3]); Axis5ValueLocation2 = RwMathUtilities.Rad2Deg((double)jointValues[4]); Axis6ValueLocation2 = RwMathUtilities.Rad2Deg((double)jointValues[5]); CalculateAxisDeltaValues(); }
private void CheckA5Singularity() { double a5Value = RwMathUtilities.Rad2Deg((double)currentJointValues[4]); if (RwMathUtilities.Abs(a5Value) < deltaA5Singularity) { AxisLimitsResult = string.IsNullOrEmpty(AxisLimitsResult) ? $"A5 Singularity: {a5Value:F1}" : string.Join("; ", AxisLimitsResult, $"A5 Singularity: {a5Value:F1}"); } }
private void CheckAxisLimits() { for (int i = 0; i < currentJointValues.Count; i++) { double upperValue = RwMathUtilities.Abs(RwMathUtilities.Rad2Deg((double)upperLimits[i] - (double)currentJointValues[i])); double lowerValue = RwMathUtilities.Abs(RwMathUtilities.Rad2Deg((double)lowerLimits[i] - (double)currentJointValues[i])); if (upperValue < deltaRotationalAxisLimit) { AxisLimitsResult = string.IsNullOrEmpty(AxisLimitsResult) ? $"A{i}: {upperValue:F1}" : string.Join("; ", AxisLimitsResult, $"A{i}: {upperValue:F1}"); } if (lowerValue < deltaRotationalAxisLimit) { AxisLimitsResult = string.IsNullOrEmpty(AxisLimitsResult) ? $"A{i}: {lowerValue:F1}" : string.Join("; ", AxisLimitsResult, $"A{i}: {lowerValue:F1}"); } } }