diff --git a/.github/scripts/grep-gate.sh b/.github/scripts/grep-gate.sh new file mode 100755 index 0000000..4d741a7 --- /dev/null +++ b/.github/scripts/grep-gate.sh @@ -0,0 +1,29 @@ +#!/usr/bin/env bash +# Grep gate (epic-129 test-scenarios TS-129-48). Each row is: id extended regex pathspecs. +# A row passes when `git grep -nE` finds 0 lines. Add a row per slice as the G-rows are switched on. +# Same shape as drawing's .github/scripts/grep-gate.sh. +set -uo pipefail +cd "$(git rev-parse --show-toplevel)" + +ROWS=( + # G-18 (S3): the Pose3 maths members are deleted in Drawing 2.0; Frame3.Interpolate/Distance/FromSurface replace them. + $'G-18\t\\bPose3(d|<[^>]*>)?\\.(Lerp|Distance|FromSurface)\\b\tcsharp/' + # G-18a (S3, epic-129 test-scenarios numbering): no Euler rotation algebra on Rotation3 and no direction → Euler shortcut + # (Rotation3.Combine/Slerp and Vector3.RotationTo are deleted; use Quaternion.Slerp / Quaternion.FromTo). + $'G-18a\t\\bRotation3(d|<[^>]*>)?\\.(Combine|Slerp|Inverse|Rotate|ToMatrix3x3)\\b|\\bRotationTo\\s*\\(\tcsharp/' +) + +failed=0 +for row in "${ROWS[@]}"; do + IFS=$'\t' read -r id pattern paths <<<"$row" + # shellcheck disable=SC2086 # pathspecs are space-separated on purpose + hits=$(git grep -nE "$pattern" -- $paths) + count=$(printf '%s' "$hits" | grep -c . || true) + echo "$id: $count line(s) [git grep -nE '$pattern' -- $paths]" + if [ "$count" -ne 0 ]; then + printf '%s\n' "$hits" + echo "::error::grep gate $id found $count line(s)" + failed=1 + fi +done +exit $failed diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index ba61043..430b206 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -11,15 +11,28 @@ on: paths: - 'csharp/**' - '.github/workflows/test.yml' + - '.github/scripts/grep-gate.sh' pull_request: paths: - 'csharp/**' - '.github/workflows/test.yml' + - '.github/scripts/grep-gate.sh' permissions: contents: read jobs: + grep-gate: + # epic-129 TS-129-48: deleted names cannot come back. One row per G-row switched on; a row fails on any line. + name: grep-gate + runs-on: ubuntu-latest + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Grep gate + run: .github/scripts/grep-gate.sh + csharp: name: C# tests runs-on: ubuntu-latest diff --git a/csharp/Directory.Build.props b/csharp/Directory.Build.props index a9e78c2..bb192ae 100644 --- a/csharp/Directory.Build.props +++ b/csharp/Directory.Build.props @@ -26,7 +26,7 @@ - 3.2.0 + 3.3.0 - + diff --git a/csharp/RocketWelder.SDK.Devices.Robot/RobotMotionExtensions.cs b/csharp/RocketWelder.SDK.Devices.Robot/RobotMotionExtensions.cs index b3194b3..514abe9 100644 --- a/csharp/RocketWelder.SDK.Devices.Robot/RobotMotionExtensions.cs +++ b/csharp/RocketWelder.SDK.Devices.Robot/RobotMotionExtensions.cs @@ -91,12 +91,17 @@ private static (Pose3 Entry, Pose3 Via, Pose3 Exit)? Com var centre = vertex.Position + (radiusMm / Math.Sin(halfAngle)) * bisector; var via = centre - radiusMm * bisector; - var entryRotation = Rotation3.Slerp(prev.Rotation, vertex.Rotation, (edgeIn - setback) / edgeIn); - var exitRotation = Rotation3.Slerp(vertex.Rotation, next.Rotation, setback / edgeOut); - var viaRotation = Rotation3.Slerp(entryRotation, exitRotation, 0.5); + // Rotation maths on quaternions (epic-129 design rule 2); Rotation3 is the wire format, converted once + // in and once out. + var qPrev = prev.Rotation.ToQuaternion(); + var qVertex = vertex.Rotation.ToQuaternion(); + var qNext = next.Rotation.ToQuaternion(); + var entryRotation = Quaternion.Slerp(qPrev, qVertex, (edgeIn - setback) / edgeIn); + var exitRotation = Quaternion.Slerp(qVertex, qNext, setback / edgeOut); + var viaRotation = Quaternion.Slerp(entryRotation, exitRotation, 0.5); - return (new Pose3(entryTangent, entryRotation), - new Pose3(via, viaRotation), - new Pose3(exitTangent, exitRotation)); + return (new Pose3(entryTangent, Rotation3.FromQuaternion(entryRotation)), + new Pose3(via, Rotation3.FromQuaternion(viaRotation)), + new Pose3(exitTangent, Rotation3.FromQuaternion(exitRotation))); } } diff --git a/csharp/RocketWelder.SDK.Devices.Robot/RocketWelder.SDK.Devices.Robot.csproj b/csharp/RocketWelder.SDK.Devices.Robot/RocketWelder.SDK.Devices.Robot.csproj index 302a106..8fa7f03 100644 --- a/csharp/RocketWelder.SDK.Devices.Robot/RocketWelder.SDK.Devices.Robot.csproj +++ b/csharp/RocketWelder.SDK.Devices.Robot/RocketWelder.SDK.Devices.Robot.csproj @@ -16,7 +16,7 @@ - + diff --git a/csharp/RocketWelder.SDK.Devices.Welding/RocketWelder.SDK.Devices.Welding.csproj b/csharp/RocketWelder.SDK.Devices.Welding/RocketWelder.SDK.Devices.Welding.csproj index eee0be5..cce5c4e 100644 --- a/csharp/RocketWelder.SDK.Devices.Welding/RocketWelder.SDK.Devices.Welding.csproj +++ b/csharp/RocketWelder.SDK.Devices.Welding/RocketWelder.SDK.Devices.Welding.csproj @@ -19,7 +19,7 @@ - + diff --git a/csharp/RocketWelder.SDK.Hmi/RocketWelder.SDK.Hmi.csproj b/csharp/RocketWelder.SDK.Hmi/RocketWelder.SDK.Hmi.csproj index d5ebb2f..da7baef 100644 --- a/csharp/RocketWelder.SDK.Hmi/RocketWelder.SDK.Hmi.csproj +++ b/csharp/RocketWelder.SDK.Hmi/RocketWelder.SDK.Hmi.csproj @@ -16,7 +16,7 @@ - + diff --git a/csharp/RocketWelder.SDK.Localization/RocketWelder.SDK.Localization.csproj b/csharp/RocketWelder.SDK.Localization/RocketWelder.SDK.Localization.csproj index f4fe37d..ff30027 100644 --- a/csharp/RocketWelder.SDK.Localization/RocketWelder.SDK.Localization.csproj +++ b/csharp/RocketWelder.SDK.Localization/RocketWelder.SDK.Localization.csproj @@ -16,7 +16,7 @@ - + diff --git a/csharp/RocketWelder.SDK.Operations.Welding/RocketWelder.SDK.Operations.Welding.csproj b/csharp/RocketWelder.SDK.Operations.Welding/RocketWelder.SDK.Operations.Welding.csproj index 9372d68..177484b 100644 --- a/csharp/RocketWelder.SDK.Operations.Welding/RocketWelder.SDK.Operations.Welding.csproj +++ b/csharp/RocketWelder.SDK.Operations.Welding/RocketWelder.SDK.Operations.Welding.csproj @@ -16,7 +16,7 @@ - + diff --git a/csharp/RocketWelder.SDK.Runtime/RocketWelder.SDK.Runtime.csproj b/csharp/RocketWelder.SDK.Runtime/RocketWelder.SDK.Runtime.csproj index ca8323f..c7f9834 100644 --- a/csharp/RocketWelder.SDK.Runtime/RocketWelder.SDK.Runtime.csproj +++ b/csharp/RocketWelder.SDK.Runtime/RocketWelder.SDK.Runtime.csproj @@ -17,7 +17,7 @@ - + diff --git a/csharp/RocketWelder.SDK.Vision/CameraProjector.cs b/csharp/RocketWelder.SDK.Vision/CameraProjector.cs index 64ca235..cd6f1ac 100644 --- a/csharp/RocketWelder.SDK.Vision/CameraProjector.cs +++ b/csharp/RocketWelder.SDK.Vision/CameraProjector.cs @@ -6,6 +6,8 @@ using Vector3d = ModelingEvolution.Drawing.Vector3; using Pointd = ModelingEvolution.Drawing.Point; using Triangle3d = ModelingEvolution.Drawing.Triangle3; +using Frame3d = ModelingEvolution.Drawing.Frame3; +using Quaterniond = ModelingEvolution.Drawing.Quaternion; namespace RocketWelder.SDK.Vision; @@ -13,10 +15,19 @@ namespace RocketWelder.SDK.Vision; /// Projects 2D pixel coordinates to 3D points on a surface using camera intrinsics /// and hand-eye calibration. /// +/// +/// Pose maths composes (epic-129 design.md, "Rules the design enforces", rule 2); +/// is the wire format of this API and is converted once at its edge. Every length +/// in and out of the projector is in millimetres (poses, points, distances), so the frames are kept in +/// millimetres too: the conversion scale is 1. Projection through the intrinsics divides by Z and is +/// scale-free. +/// public class CameraProjector : ICameraProjector { private readonly CameraIntrinsics _intrinsics; - private readonly Pose3d _cameraToGripper; + private const double LengthScale = 1.0; // Pose3 wire (mm) → Frame3 (mm): see the class remarks. + + private readonly Frame3d _cameraToGripper; private readonly Func _getCurrentPosition; /// @@ -28,7 +39,7 @@ public class CameraProjector : ICameraProjector public CameraProjector(CameraIntrinsics intrinsics, Pose3d cameraToGripper, Func getCurrentPosition) { _intrinsics = intrinsics; - _cameraToGripper = cameraToGripper; + _cameraToGripper = Frame3d.FromPose3(cameraToGripper, LengthScale); _getCurrentPosition = getCurrentPosition; } @@ -48,13 +59,16 @@ public Pose3d ProjectPose(Pointd pixel, Pose3d surface) /// Projects a pixel coordinate onto a surface defined by a triangle. /// public Pose3d ProjectPose(Pointd pixel, Triangle3d surface) - => ProjectPose(pixel, surface.ToPose()); + => ProjectPose(pixel, surface.ToFrame()); /// /// Projects a pixel coordinate onto a surface defined by three points (right-hand rule for normal). /// public Pose3d ProjectPose(Pointd pixel, Point3d a, Point3d b, Point3d c) - => ProjectPose(pixel, Pose3d.FromSurface(a, b, c)); + => ProjectPose(pixel, Frame3d.FromSurface(a, b, c)); + + private Pose3d ProjectPose(Pointd pixel, in Frame3d surface) + => (surface with { Position = ProjectPoint(pixel, surface) }).ToPose3(LengthScale); /// /// Projects a pixel coordinate to a point at a fixed distance along the ray. @@ -67,11 +81,8 @@ public Pose3d ProjectPoseAt(Pointd pixel, double distance) // 1. Pixel to undistorted ray direction in camera frame var dirCam = PixelToRay(pixel); - // 2. Get current gripper pose - var gripperToBase = _getCurrentPosition(); - - // 3. Compose transforms: camera → gripper → base - var cameraToBase = gripperToBase * _cameraToGripper; + // 2-3. Current gripper pose; compose camera → gripper → base + var cameraToBase = CameraToBase(); // 4. Transform ray to base frame var rayOrigin = cameraToBase.Position; @@ -81,9 +92,9 @@ public Pose3d ProjectPoseAt(Pointd pixel, double distance) var position = rayOrigin + rayDir * distance; // 6. Build orientation with Z along ray direction - var rotation = Vector3d.EZ.RotationTo(rayDir); + var rotation = Quaterniond.FromTo(Vector3d.EZ, rayDir); - return new Pose3d(position, rotation); + return new Frame3d(position, rotation).ToPose3(LengthScale); } /// @@ -109,24 +120,22 @@ public Vector3d ProjectVector(Pointd actualLocation, Pointd expectedLocation, do /// Surface pose in robot base frame (Z-axis is surface normal). /// Correction vector (actual - expected) to apply to robot position. public Vector3d ProjectVector(Pointd actualLocation, Pointd expectedLocation, Pose3d surface) - { - var actualPoint = ProjectPoint(actualLocation, surface); - var expectedPoint = ProjectPoint(expectedLocation, surface); - - return actualPoint - expectedPoint; - } + => ProjectVector(actualLocation, expectedLocation, Frame3d.FromPose3(surface, LengthScale)); /// /// Computes the 3D correction vector projected onto a surface defined by a triangle. /// public Vector3d ProjectVector(Pointd actualLocation, Pointd expectedLocation, Triangle3d surface) - => ProjectVector(actualLocation, expectedLocation, surface.ToPose()); + => ProjectVector(actualLocation, expectedLocation, surface.ToFrame()); /// /// Computes the 3D correction vector projected onto a surface defined by three points. /// public Vector3d ProjectVector(Pointd actualLocation, Pointd expectedLocation, Point3d a, Point3d b, Point3d c) - => ProjectVector(actualLocation, expectedLocation, Pose3d.FromSurface(a, b, c)); + => ProjectVector(actualLocation, expectedLocation, Frame3d.FromSurface(a, b, c)); + + private Vector3d ProjectVector(Pointd actualLocation, Pointd expectedLocation, in Frame3d surface) + => ProjectPoint(actualLocation, surface) - ProjectPoint(expectedLocation, surface); /// /// Projects a pixel coordinate onto a surface, returning the 3D point in robot base frame. @@ -135,15 +144,15 @@ public Vector3d ProjectVector(Pointd actualLocation, Pointd expectedLocation, Po /// Surface pose in robot base frame (Z-axis is surface normal). /// 3D point on surface in robot base frame. public Point3d ProjectPoint(Pointd pixel, Pose3d surface) + => ProjectPoint(pixel, Frame3d.FromPose3(surface, LengthScale)); + + private Point3d ProjectPoint(Pointd pixel, in Frame3d surface) { // 1. Pixel to undistorted ray direction in camera frame var dirCam = PixelToRay(pixel); - // 2. Get current gripper pose - var gripperToBase = _getCurrentPosition(); - - // 3. Compose transforms: camera → gripper → base - var cameraToBase = gripperToBase * _cameraToGripper; + // 2-3. Current gripper pose; compose camera → gripper → base + var cameraToBase = CameraToBase(); // 4. Transform ray to base frame var rayOrigin = cameraToBase.Position; @@ -151,7 +160,7 @@ public Point3d ProjectPoint(Pointd pixel, Pose3d surface) // 5. Get plane from surface pose (point + normal) var planePoint = surface.Position; - var planeNormal = surface.Rotation.Rotate(Vector3d.EZ); + var planeNormal = surface.TransformVector(Vector3d.EZ); // 6. Ray-plane intersection: t = ((P0 - O) · N) / (D · N) var originToPlane = planePoint - rayOrigin; @@ -173,16 +182,19 @@ public Point3d ProjectPoint(Pointd pixel, Pose3d surface) /// Projects a pixel coordinate onto a surface defined by a triangle. /// public Point3d ProjectPoint(Pointd pixel, Triangle3d surface) - => ProjectPoint(pixel, surface.ToPose()); + => ProjectPoint(pixel, surface.ToFrame()); /// /// Projects a pixel coordinate onto a surface defined by three points (right-hand rule for normal). /// public Point3d ProjectPoint(Pointd pixel, Point3d a, Point3d b, Point3d c) - => ProjectPoint(pixel, Pose3d.FromSurface(a, b, c)); + => ProjectPoint(pixel, Frame3d.FromSurface(a, b, c)); /// public Polyline3 ProjectPoints(Polyline pixels, Pose3d surface) + => ProjectPoints(pixels, Frame3d.FromPose3(surface, LengthScale)); + + private Polyline3 ProjectPoints(Polyline pixels, in Frame3d surface) { var span = pixels.AsSpan(); var points = new Point3d[span.Length]; @@ -196,10 +208,13 @@ public Polyline3 ProjectPoints(Polyline pixels, Pose3d surface) /// public Polyline3 ProjectPoints(Polyline pixels, Triangle3d surface) - => ProjectPoints(pixels, surface.ToPose()); + => ProjectPoints(pixels, surface.ToFrame()); /// public Polyline3 ProjectPoints(Polyline pixels, Pose3d surface) + => ProjectPoints(pixels, Frame3d.FromPose3(surface, LengthScale)); + + private Polyline3 ProjectPoints(Polyline pixels, in Frame3d surface) { var span = pixels.AsSpan(); var points = new Point3d[span.Length]; @@ -210,16 +225,16 @@ public Polyline3 ProjectPoints(Polyline pixels, Pose3d surface) /// public Polyline3 ProjectPoints(Polyline pixels, Triangle3d surface) - => ProjectPoints(pixels, surface.ToPose()); + => ProjectPoints(pixels, surface.ToFrame()); /// - public Pose3d GetTcpForCameraPose(Pose3d cameraPose) => cameraPose * _cameraToGripper.Inverse(); + public Pose3d GetTcpForCameraPose(Pose3d cameraPose) + => (Frame3d.FromPose3(cameraPose, LengthScale) * _cameraToGripper.Inverse()).ToPose3(LengthScale); /// public bool TryProjectToPixel(Point3d basePoint, out Pointd pixel) { - var gripperToBase = _getCurrentPosition(); - var cameraToBase = gripperToBase * _cameraToGripper; + var cameraToBase = CameraToBase(); var pointInCamera = cameraToBase.Inverse().TransformPoint(basePoint); @@ -233,5 +248,8 @@ public bool TryProjectToPixel(Point3d basePoint, out Pointd pixel) return true; } + /// Camera → gripper → base, from the live gripper pose (the Pose3 wire edge). + private Frame3d CameraToBase() => Frame3d.FromPose3(_getCurrentPosition(), LengthScale) * _cameraToGripper; + private Vector3d PixelToRay(Pointd pixel) => _intrinsics.PixelToRay(pixel); } diff --git a/csharp/RocketWelder.SDK.Vision/RocketWelder.SDK.Vision.csproj b/csharp/RocketWelder.SDK.Vision/RocketWelder.SDK.Vision.csproj index d7ffdbd..fa87931 100644 --- a/csharp/RocketWelder.SDK.Vision/RocketWelder.SDK.Vision.csproj +++ b/csharp/RocketWelder.SDK.Vision/RocketWelder.SDK.Vision.csproj @@ -16,7 +16,7 @@ - + diff --git a/csharp/RocketWelder.SDK.Vision/docs/CameraProjector.md b/csharp/RocketWelder.SDK.Vision/docs/CameraProjector.md index a33a23c..86fcad9 100644 --- a/csharp/RocketWelder.SDK.Vision/docs/CameraProjector.md +++ b/csharp/RocketWelder.SDK.Vision/docs/CameraProjector.md @@ -106,12 +106,12 @@ var cornerC = new Point3(400, 200, 50); // defines plane // Create surface pose using right-hand rule // Z-axis points up from surface (a->b->c counter-clockwise) -var surface = Pose3.FromSurface(cornerA, cornerB, cornerC); +var surface = Frame3.FromSurface(cornerA, cornerB, cornerC).ToPose3(1.0); // mm in, mm out (scale 1) ``` **Note**: Point order determines Z direction via right-hand rule: -- `FromSurface(a, b, c)` - Z points "up" (counter-clockwise winding) -- `FromSurface(c, b, a)` - Z points "down" (clockwise winding) +- `Frame3.FromSurface(a, b, c)` - Z points "up" (counter-clockwise winding) +- `Frame3.FromSurface(c, b, a)` - Z points "down" (clockwise winding) ### Surface Definition Options @@ -119,7 +119,7 @@ All surface-based methods accept three interchangeable surface definitions: ```csharp // Option 1: Pose3 (pre-computed) -var surface = Pose3.FromSurface(cornerA, cornerB, cornerC); +var surface = Frame3.FromSurface(cornerA, cornerB, cornerC).ToPose3(1.0); // mm in, mm out (scale 1) var point = projector.ProjectPoint(pixel, surface); // Option 2: Triangle3 (when you have a triangle) @@ -150,7 +150,7 @@ Console.WriteLine($"Detected at: X={worldPoint.X:F2}, Y={worldPoint.Y:F2}, Z={wo // Setup (once at startup) var imageProcessor = new ImageProcessor("http://127.0.0.1:5000"); var projector = new CameraProjector(intrinsics, cameraToGripper, () => cobot.GetActualPose()); -var surface = Pose3.FromSurface(cornerA, cornerB, cornerC); +var surface = Frame3.FromSurface(cornerA, cornerB, cornerC).ToPose3(1.0); // mm in, mm out (scale 1) const double torchHeight = 15.0; // mm above surface diff --git a/csharp/RocketWelder.SDK/RocketWelder.SDK.csproj b/csharp/RocketWelder.SDK/RocketWelder.SDK.csproj index 67788c2..c22e582 100644 --- a/csharp/RocketWelder.SDK/RocketWelder.SDK.csproj +++ b/csharp/RocketWelder.SDK/RocketWelder.SDK.csproj @@ -45,7 +45,7 @@ - + diff --git a/csharp/examples/SimpleClient/SimpleClient.csproj b/csharp/examples/SimpleClient/SimpleClient.csproj index 39eaa65..0aab68c 100644 --- a/csharp/examples/SimpleClient/SimpleClient.csproj +++ b/csharp/examples/SimpleClient/SimpleClient.csproj @@ -20,7 +20,7 @@ - +