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}");
}
}
}
