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}"); } } } }
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}"); } } }
private void CheckA1Singularity(ITxRobot irobot) { TxTransformation currentWorkingFrame = TxApplication.ActiveDocument.WorkingFrame; TxRobot robot = irobot as TxRobot; if (robot != null) { if (location is ITxLocatableObject locateableLocation) { TxApplication.ActiveDocument.WorkingFrame = robot.AbsoluteLocation; double locationXvalue = locateableLocation.LocationRelativeToWorkingFrame.Translation.X; double locationYvalue = locateableLocation.LocationRelativeToWorkingFrame.Translation.Y; double xyValue = RwMathUtilities.Length(locationXvalue, locationYvalue, 0.0); if (RwMathUtilities.Abs(xyValue) < deltaA1Singularity) { AxisLimitsResult = string.IsNullOrEmpty(AxisLimitsResult) ? $"A1 Singularity: {xyValue:F1}" : string.Join("; ", AxisLimitsResult, $"A1 Singularity: {xyValue:F1}"); } TxApplication.ActiveDocument.WorkingFrame = currentWorkingFrame; } } }