/// <summary>
/// Returns a KRL FRAME representation of the current state of the cursor.
/// Note POS also accept T and S parameters for unambiguous arm configuration def. @TODO: implement?
/// </summary>
/// <returns></returns>
internal string GetPositionTargetValue(RobotCursor cursor)
{
YawPitchRoll euler = cursor.rotation.Q.ToYawPitchRoll(); // @TODO: does this actually work...?
return(string.Format("{{POS: X {0}, Y {1}, Z {2}, A {3}, B {4}, C {5}}}",
Math.Round(cursor.position.X, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(cursor.position.Y, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(cursor.position.Z, Geometry.STRING_ROUND_DECIMALS_MM),
// note reversed ZYX order
Math.Round(euler.ZAngle, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(euler.YAngle, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(euler.XAngle, Geometry.STRING_ROUND_DECIMALS_DEGS)));
}
/// <summary>
/// Returns a KRL representation of a Tool object
/// </summary>
/// <param name="cursor"></param>
/// <returns></returns>
internal string GetToolValue(RobotCursor cursor)
{
if (cursor.tool == null)
{
throw new Exception("Cursor has no tool attached");
}
YawPitchRoll euler = cursor.tool.TCPOrientation.Q.ToYawPitchRoll();
return(string.Format("{{X {0}, Y {1}, Z {2}, A {3}, B {4}, C {5}}}",
Math.Round(cursor.tool.TCPPosition.X, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(cursor.tool.TCPPosition.Y, Geometry.STRING_ROUND_DECIMALS_MM),
Math.Round(cursor.tool.TCPPosition.Z, Geometry.STRING_ROUND_DECIMALS_MM),
// note reversed ZYX order
Math.Round(euler.ZAngle, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(euler.YAngle, Geometry.STRING_ROUND_DECIMALS_DEGS),
Math.Round(euler.XAngle, Geometry.STRING_ROUND_DECIMALS_DEGS)));
}
/// <summary>
/// Test if this YawPitchRoll is approximately equal to another.
/// </summary>
/// <param name="other"></param>
/// <returns></returns>
public bool IsSimilar(YawPitchRoll other)
{
return(Math.Abs(this.XAngle - other.XAngle) < EPSILON &&
Math.Abs(this.YAngle - other.YAngle) < EPSILON &&
Math.Abs(this.ZAngle - other.ZAngle) < EPSILON);
}
/// <summary>
/// Is this rotation equivalent to another? I.e. is the resulting orientation the same?
/// </summary>
/// <param name="other"></param>
/// <returns></returns>
public bool IsEquivalent(YawPitchRoll other)
{
// Quick and dirty (and expensive?) test, compare underlying Quaternions...
return(this.ToQuaternion().IsEquivalent(other.ToQuaternion()));
}
