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>
/// Get the joint values of the robot assigned to a location
/// </summary>
/// <param name="location">The location from which to get the joint values</param>
/// <returns>The joint values</returns>
private ArrayList GetRobotPoseDataFromLocation(ITxRoboticLocationOperation location)
{
ITxRobot robot = location.ParentRoboticOperation.Robot;
if (robot != null)
{
TxPoseData pose = robot.GetPoseAtLocation(location);
if (pose != null)
{
return(pose.JointValues);
}
}
return(null);
}
