/// <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())); }