/// <summary>
/// Returns an UR pose representation of the current state of the cursor.
/// </summary>
/// <returns></returns>
internal static string GetPoseTargetValue(RobotCursor cursor)
{
RotationVector axisAng = cursor.rotation.GetRotationVector(true);
return(string.Format("p[{0},{1},{2},{3},{4},{5}]",
Math.Round(0.001 * cursor.position.X, Geometry.STRING_ROUND_DECIMALS_M),
Math.Round(0.001 * cursor.position.Y, Geometry.STRING_ROUND_DECIMALS_M),
Math.Round(0.001 * cursor.position.Z, Geometry.STRING_ROUND_DECIMALS_M),
Math.Round(axisAng.X, Geometry.STRING_ROUND_DECIMALS_RADS),
Math.Round(axisAng.Y, Geometry.STRING_ROUND_DECIMALS_RADS),
Math.Round(axisAng.Z, Geometry.STRING_ROUND_DECIMALS_RADS)));
}
/// <summary>
/// Returns a UR representation of a Tool object.
/// </summary>
/// <param name="cursor"></param>
/// <returns></returns>
internal static string GetToolValue(RobotCursor cursor) //TODO: wouldn't it be just better to pass the Tool object? Inconsistent with the rest of the API...
{
if (cursor.tool == null)
{
throw new Exception("Cursor has no tool attached");
}
RotationVector axisAng = cursor.tool.TCPOrientation.Q.ToRotationVector(true);
return(string.Format("p[{0},{1},{2},{3},{4},{5}]",
Math.Round(0.001 * cursor.tool.TCPPosition.X, Geometry.STRING_ROUND_DECIMALS_M),
Math.Round(0.001 * cursor.tool.TCPPosition.Y, Geometry.STRING_ROUND_DECIMALS_M),
Math.Round(0.001 * cursor.tool.TCPPosition.Z, Geometry.STRING_ROUND_DECIMALS_M),
Math.Round(axisAng.X, Geometry.STRING_ROUND_DECIMALS_RADS),
Math.Round(axisAng.Y, Geometry.STRING_ROUND_DECIMALS_RADS),
Math.Round(axisAng.Z, Geometry.STRING_ROUND_DECIMALS_RADS)));
}
/// <summary>
/// Test if this RotationVector is approximately equal to another.
/// </summary>
/// <param name="other"></param>
/// <returns></returns>
public bool IsSimilar(RotationVector other)
{
return(Math.Abs(this.X - other.X) < EPSILON2 &&
Math.Abs(this.Y - other.Y) < EPSILON2 &&
Math.Abs(this.Z - other.Z) < EPSILON2);
}
/// <summary>
/// Is this rotation equivalent to a given one?
/// Equivalence is defined as rotations around vectors sharing the same axis (including opposite directions)
/// and an angle with the same modulated equivalence. This in turn means the same spatial orientation after transformation.
/// See <see cref="AxisAngle.IsEquivalent(AxisAngle)"/>
/// </summary>
/// <param name="rv"></param>
/// <returns></returns>
public bool IsEquivalent(RotationVector rv)
{
return(this.ToAxisAngle().IsEquivalent(rv.ToAxisAngle()));
}