diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 7d8581b..ba61043 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -55,10 +55,12 @@ jobs: # indefinitely on this developer's machine; nobody has confirmed what they need on a # hosted runner. Running them here would hang the job, not fail it. # - # Promoted 2026-08-21 (epic-065 It-2): AdaptivePoints, Devices.Robot and Robotics.Core. + # Promoted 2026-08-21 (epic-065 It-2): AdaptivePoints and Devices.Robot (Robotics.Core, promoted the + # same day, was deleted 2026-10-24 by epic-129 S2: no consumer). + # # They had been held out as "not observed producing a result locally" — that turned out to # be the org NuGet feed being down, not the tests. Restored from nuget.org and run with - # --no-restore they produce 14, 10 and 237 passing. + # --no-restore they produced 14 and 10 passing. run: | set -euo pipefail projects=( @@ -71,7 +73,6 @@ jobs: RocketWelder.SDK.Types.Tests RocketWelder.SDK.AdaptivePoints.Tests RocketWelder.SDK.Devices.Robot.Tests - RocketWelder.SDK.Robotics.Core.Tests RocketWelder.SDK.Devices.Welding.Tests # epic-110: job-catalog contract pins (enum ordinals, D-9 scan, signatures) ) for p in "${projects[@]}"; do diff --git a/csharp/RocketWelder.SDK.Robotics.Core.Tests/CollisionDetectorTests.cs b/csharp/RocketWelder.SDK.Robotics.Core.Tests/CollisionDetectorTests.cs deleted file mode 100644 index 2d9a224..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core.Tests/CollisionDetectorTests.cs +++ /dev/null @@ -1,182 +0,0 @@ -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core.Tests; - -public class CollisionDetectorTests -{ - private static readonly double[] NormalRadii = { 60, 60, 45, 35, 25, 20 }; - private static readonly double[] TinyRadii = { 5, 5, 5, 5, 5, 5 }; - private static ICollisionSource EmptySource => new PrimitiveCollisionSource(Array.Empty()); - private static CollisionEnvironment Env(double[] radii, double margin = 0, ICollisionSource? source = null, ToolModel? tool = null) - => new(source ?? EmptySource, radii, tool ?? ToolModel.None, margin); - - // Synthetic robot whose DH chain gives every link a non-zero segment, so - // self-collision behaviour can be asserted without FR5's zero-length Link2. - private static RobotModel NonDegenerateRobot() - { - var chain = new[] - { - DhJoint.FromDegrees(0, 0, 500, 0), - DhJoint.FromDegrees(-90, 0, 200, 0), - DhJoint.FromDegrees(0, 500, 0, 0), - DhJoint.FromDegrees(0, 400, 0, 0), - DhJoint.FromDegrees(-90, 0, 100, 0), - DhJoint.FromDegrees(90, 0, 80, 0), - }; - var limits = Enumerable.Repeat(new JointLimit(-360, 360), 6).ToArray(); - return new RobotModel("SyntheticTestBot", chain, limits, Joints6.Zero); - } - - [Fact] - public void SelfCollisionPairs_HasExactly10Entries() - { - CollisionDetector.SelfCollisionPairs.Count.Should().Be(10); - } - - [Fact] - public void SelfCollisionPairs_ContainsNoAdjacentLinks() - { - foreach (var (a, b) in CollisionDetector.SelfCollisionPairs) - { - Math.Abs(a - b).Should().BeGreaterThan(1, $"pair ({a},{b}) is adjacent"); - a.Should().BeLessThan(b, $"pair ({a},{b}) not canonically ordered"); - } - } - - [Fact] - public void SelfCollisionPairs_CoversAllDocumentedPairs() - { - var expected = new HashSet<(int, int)> - { - (0,2),(0,3),(0,4),(0,5), - (1,3),(1,4),(1,5), - (2,4),(2,5), - (3,5), - }; - CollisionDetector.SelfCollisionPairs.Should().BeEquivalentTo(expected); - } - - [Fact] - public void CheckCollision_NullModel_Throws() - { - Action act = () => CollisionDetector.CheckCollision(null!, Joints6.Zero, Env(NormalRadii)); - act.Should().Throw().And.ParamName.Should().Be("model"); - } - - [Fact] - public void CheckCollision_NullEnvironment_Throws() - { - Action act = () => CollisionDetector.CheckCollision(RobotPresets.FairinoFR5(), Joints6.Zero, null!); - act.Should().Throw().And.ParamName.Should().Be("environment"); - } - - [Fact] - public void CheckCollision_NoCollision_ReturnsEmptyArray_NotNull() - { - var result = CollisionDetector.CheckCollision( - NonDegenerateRobot(), Joints6.Zero, Env(TinyRadii)); - result.Should().NotBeNull(); - result.Should().BeEmpty(); - } - - [Fact] - public void CheckCollision_HugeRadii_ReportsAllTenSelfPairs_NoAdjacent() - { - // Inflate every link radius so that every pair of non-adjacent capsule axes is closer - // than r_a + r_b. The arm's longest span at zero joints is well under 1.5 m for FR5, - // so 5 m radii guarantee every non-adjacent pair violates clearance. - var fat = new[] { 5000.0, 5000, 5000, 5000, 5000, 5000 }; - var result = CollisionDetector.CheckCollision( - RobotPresets.FairinoFR5(), Joints6.Zero, Env(fat)); - - result.Count.Should().Be(10); - - var reported = result.Select(r => ParsePair(r.BodyA, r.BodyB)).ToHashSet(); - reported.Should().BeEquivalentTo(CollisionDetector.SelfCollisionPairs.ToHashSet()); - - foreach (var pair in reported) - { - Math.Abs(pair.Item1 - pair.Item2).Should().BeGreaterThan(1); - } - } - - [Theory] - [InlineData(0, 2)][InlineData(0, 3)][InlineData(0, 4)][InlineData(0, 5)] - [InlineData(1, 3)][InlineData(1, 4)][InlineData(1, 5)] - [InlineData(2, 4)][InlineData(2, 5)] - [InlineData(3, 5)] - public void CheckCollision_EachDocumentedPair_IsReportedWhenRadiiHugeEnough(int a, int b) - { - var fat = new[] { 5000.0, 5000, 5000, 5000, 5000, 5000 }; - var result = CollisionDetector.CheckCollision( - RobotPresets.FairinoFR5(), Joints6.Zero, Env(fat)); - - var tag = ($"Link{a + 1}", $"Link{b + 1}"); - result.Select(r => (r.BodyA, r.BodyB)).Should().Contain(tag); - } - - [Fact] - public void CheckCollision_TinyRadii_ProducesNoSelfCollisions_OnNonDegenerateRobot() - { - var result = CollisionDetector.CheckCollision( - NonDegenerateRobot(), Joints6.Zero, Env(TinyRadii)); - result.Where(IsSelfPair).Should().BeEmpty(); - } - - [Fact] - public void CheckCollision_PenetrationDepth_IsPositive_WhenOverlapping() - { - var fat = new[] { 5000.0, 5000, 5000, 5000, 5000, 5000 }; - var result = CollisionDetector.CheckCollision( - RobotPresets.FairinoFR5(), Joints6.Zero, Env(fat)); - - foreach (var hit in result.Where(IsSelfPair)) - hit.PenetrationDepth.Should().BeGreaterThan(0); - } - - [Fact] - public void CheckCollision_SafetyMargin_PromotesNearMisses_ToHits() - { - var baseline = CollisionDetector.CheckCollision( - NonDegenerateRobot(), Joints6.Zero, Env(TinyRadii)); - baseline.Where(IsSelfPair).Should().BeEmpty(); - - var inflated = CollisionDetector.CheckCollision( - NonDegenerateRobot(), Joints6.Zero, Env(TinyRadii, margin: 10_000)); - inflated.Where(IsSelfPair).Should().NotBeEmpty(); - } - - [Fact] - public void CheckCollision_EnvironmentHitsAreIncluded() - { - // Giant box centred at robot base → every link capsule will penetrate it. - var box = new BoxPrimitive("Wall", new Point3(0, 0, 0), 10_000, 10_000, 10_000); - var env = Env(NormalRadii, source: new PrimitiveCollisionSource(box)); - var result = CollisionDetector.CheckCollision( - RobotPresets.FairinoFR5(), Joints6.Zero, env); - - result.Should().NotBeEmpty(); - result.Any(r => r.BodyB == "Wall" || r.BodyA == "Wall").Should().BeTrue(); - } - - [Fact] - public void CheckCollision_MixedSelfAndEnvironment_ReportsBoth() - { - var fat = new[] { 5000.0, 5000, 5000, 5000, 5000, 5000 }; - var box = new BoxPrimitive("Wall", new Point3(0, 0, 0), 50, 50, 50); - var env = Env(fat, source: new PrimitiveCollisionSource(box)); - - var result = CollisionDetector.CheckCollision( - RobotPresets.FairinoFR5(), Joints6.Zero, env); - - result.Any(IsSelfPair).Should().BeTrue("self-collision pairs should be reported"); - result.Any(r => r.BodyB == "Wall" || r.BodyA == "Wall").Should().BeTrue("env collision should be reported"); - } - - private static bool IsSelfPair(CollisionResult r) => - r.BodyA.StartsWith("Link") && r.BodyB.StartsWith("Link"); - - private static (int, int) ParsePair(string a, string b) => - (int.Parse(a.AsSpan("Link".Length)) - 1, - int.Parse(b.AsSpan("Link".Length)) - 1); -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core.Tests/CollisionEnvironmentTests.cs b/csharp/RocketWelder.SDK.Robotics.Core.Tests/CollisionEnvironmentTests.cs deleted file mode 100644 index 100161e..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core.Tests/CollisionEnvironmentTests.cs +++ /dev/null @@ -1,123 +0,0 @@ -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core.Tests; - -public class CollisionEnvironmentTests -{ - private static readonly double[] ValidRadii = { 60, 60, 45, 35, 25, 20 }; - - private static ICollisionSource NullSource() => new PrimitiveCollisionSource(Array.Empty()); - - [Fact] - public void Ctor_HappyPath_ExposesAllProperties() - { - var source = NullSource(); - var tool = ToolModel.None; - - var env = new CollisionEnvironment(source, ValidRadii, tool, safetyMargin: 2.5); - - env.Source.Should().BeSameAs(source); - env.LinkRadii.Should().Equal(ValidRadii); - env.Tool.Should().BeSameAs(tool); - env.SafetyMargin.Should().Be(2.5); - } - - [Fact] - public void Ctor_DefaultsSafetyMarginToZero() - { - var env = new CollisionEnvironment(NullSource(), ValidRadii, ToolModel.None); - env.SafetyMargin.Should().Be(0); - } - - [Fact] - public void Ctor_NullSource_Throws() - { - Action act = () => new CollisionEnvironment(null!, ValidRadii, ToolModel.None); - act.Should().Throw().And.ParamName.Should().Be("source"); - } - - [Fact] - public void Ctor_NullLinkRadii_Throws() - { - Action act = () => new CollisionEnvironment(NullSource(), null!, ToolModel.None); - act.Should().Throw().And.ParamName.Should().Be("linkRadii"); - } - - [Fact] - public void Ctor_NullTool_Throws() - { - Action act = () => new CollisionEnvironment(NullSource(), ValidRadii, null!); - act.Should().Throw().And.ParamName.Should().Be("tool"); - } - - [Theory] - [InlineData(0)] - [InlineData(3)] - [InlineData(5)] - [InlineData(7)] - [InlineData(12)] - public void Ctor_WrongLinkRadiiCount_Throws(int count) - { - var radii = Enumerable.Repeat(10.0, count).ToArray(); - Action act = () => new CollisionEnvironment(NullSource(), radii, ToolModel.None); - act.Should().Throw() - .WithMessage("*6*") - .And.ParamName.Should().Be("linkRadii"); - } - - [Theory] - [InlineData(0)] - [InlineData(-1)] - [InlineData(-0.0001)] - public void Ctor_NonPositiveLinkRadius_Throws(double badRadius) - { - var radii = new double[] { 50, 50, 50, badRadius, 50, 50 }; - Action act = () => new CollisionEnvironment(NullSource(), radii, ToolModel.None); - act.Should().Throw() - .And.ParamName.Should().Be("linkRadii"); - } - - [Theory] - [InlineData(-0.0001)] - [InlineData(-5)] - public void Ctor_NegativeSafetyMargin_Throws(double margin) - { - Action act = () => new CollisionEnvironment(NullSource(), ValidRadii, ToolModel.None, margin); - act.Should().Throw() - .And.ParamName.Should().Be("safetyMargin"); - } - - [Fact] - public void LinkRadii_IsDefensivelyCopied() - { - var mutable = (double[])ValidRadii.Clone(); - var env = new CollisionEnvironment(NullSource(), mutable, ToolModel.None); - - mutable[0] = 999; - - env.LinkRadii[0].Should().Be(60); - } - - [Fact] - public void QueryThroughEnvironment_ReachesSource() - { - var box = new BoxPrimitive( - "far", - new Point3(10_000, 0, 0), - 10, 10, 10); - var env = new CollisionEnvironment( - new PrimitiveCollisionSource(new CollisionPrimitive[] { box }), - ValidRadii, - ToolModel.None); - - var result = env.Source.QueryCollision( - RobotPresets.FairinoFR5(), - Joints6.Zero, - env.LinkRadii, - env.Tool, - env.SafetyMargin); - - result.Should().NotBeNull(); - result.Should().BeEmpty(); - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core.Tests/CollisionMathTests.cs b/csharp/RocketWelder.SDK.Robotics.Core.Tests/CollisionMathTests.cs deleted file mode 100644 index 1b8cbd1..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core.Tests/CollisionMathTests.cs +++ /dev/null @@ -1,112 +0,0 @@ -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core.Tests; - -/// -/// Worst-case tests for closed-form segment-to-box and segment-to-cylinder-Z SDF. -/// Compares against a dense (10_001-sample) brute-force minimum; the closed-form -/// result must equal (inside tolerance) or be tighter than the sampled minimum. -/// -public class CollisionMathTests -{ - private const int BruteSamples = 10_000; - private const double Tol = 1e-6; - - private static double BruteForceMinBox(Point3 p0, Point3 p1, - Point3 c, double hx, double hy, double hz) - { - double best = double.PositiveInfinity; - for (int i = 0; i <= BruteSamples; i++) - { - double t = (double)i / BruteSamples; - var p = new Point3( - (double)p0.X + ((double)p1.X - (double)p0.X) * t, - (double)p0.Y + ((double)p1.Y - (double)p0.Y) * t, - (double)p0.Z + ((double)p1.Z - (double)p0.Z) * t); - var (_, d) = CollisionMath.SignedDistanceToBox(p, c, hx, hy, hz); - if (d < best) best = d; - } - return best; - } - - [Theory] - [InlineData(0, 0, -1000, 0, 0, 1000)] // through-center pierce along Z - [InlineData(-2000, 0, 0, 2000, 0, 0)] // through-center pierce along X - [InlineData(500, 500, 500, -500, -500, -500)] // diagonal pierce - [InlineData(1000, 1000, 1000, 1100, 1100, 1100)] // fully outside, corner-adjacent - [InlineData(500, 0, 200, -500, 400, 200)] // axial with oblique clip - [InlineData(100, 100, 100, 100, 100, 100)] // degenerate (zero-length) - [InlineData(0, 400, 0, 400, 0, 0)] // diagonal, endpoints outside, passes near edge - public void ClosestPointOnSegmentToBox_MatchesOrBeatsBruteForce( - double x0, double y0, double z0, double x1, double y1, double z1) - { - var p0 = new Point3(x0, y0, z0); - var p1 = new Point3(x1, y1, z1); - var c = new Point3(0, 0, 0); - double hx = 300, hy = 200, hz = 150; - - var (_, closed) = CollisionMath.ClosestPointOnSegmentToBox(p0, p1, c, hx, hy, hz); - var brute = BruteForceMinBox(p0, p1, c, hx, hy, hz); - - // Closed-form captures break-points exactly → must be ≤ brute (or within tol above). - closed.Should().BeLessThanOrEqualTo(brute + Tol); - closed.Should().BeGreaterThan(brute - 1.0, "closed-form should not be pathologically low"); - } - - private static double BruteForceMinCylZ(Point3 p0, Point3 p1, - Point3 c, double radius, double height) - { - double hz = height * 0.5; - double best = double.PositiveInfinity; - for (int i = 0; i <= BruteSamples; i++) - { - double t = (double)i / BruteSamples; - double px = (double)p0.X + ((double)p1.X - (double)p0.X) * t; - double py = (double)p0.Y + ((double)p1.Y - (double)p0.Y) * t; - double pz = (double)p0.Z + ((double)p1.Z - (double)p0.Z) * t; - double dx = px - (double)c.X, dy = py - (double)c.Y, dz = pz - (double)c.Z; - double r = Math.Sqrt(dx * dx + dy * dy); - double radial = r - radius; - double axial = Math.Abs(dz) - hz; - double sdf = (radial > 0 && axial > 0) - ? Math.Sqrt(radial * radial + axial * axial) - : Math.Max(radial, axial); - if (sdf < best) best = sdf; - } - return best; - } - - [Theory] - [InlineData(0, 0, -500, 0, 0, 500)] // pierce along axis - [InlineData(-500, 0, 0, 500, 0, 0)] // pierce radially - [InlineData(-500, 50, 0, 500, -50, 0)] // tangent-like radial scan - [InlineData(1000, 1000, 1000, 1000, 1000, -1000)] // fully outside, vertical - [InlineData(500, 0, 400, -500, 0, 400)] // above the cylinder, radial scan - [InlineData(300, 300, 300, 300, 300, 300)] // zero-length - public void ClosestPointOnSegmentToCylinderZ_MatchesOrBeatsBruteForce( - double x0, double y0, double z0, double x1, double y1, double z1) - { - var p0 = new Point3(x0, y0, z0); - var p1 = new Point3(x1, y1, z1); - var c = new Point3(0, 0, 0); - double radius = 200, height = 400; - - var (_, closed) = CollisionMath.ClosestPointOnSegmentToCylinderZ(p0, p1, c, radius, height); - var brute = BruteForceMinCylZ(p0, p1, c, radius, height); - - closed.Should().BeLessThanOrEqualTo(brute + Tol); - closed.Should().BeGreaterThan(brute - 1.0); - } - - [Fact] - public void ClosestPointOnSegmentToBox_DetectsTangentPierce_BruteForceMayMiss() - { - // Regression: this segment grazes the box between uniform samples. The old - // 17-point sampler returned a positive distance; closed-form must detect pierce. - var p0 = new Point3(-1000, 0.5, 0); - var p1 = new Point3( 1000, 0.5, 0); - var c = new Point3(0, 0, 0); - var (_, d) = CollisionMath.ClosestPointOnSegmentToBox(p0, p1, c, 100, 1, 50); - d.Should().BeLessThan(0, "segment pierces box along X; SDF must be negative"); - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core.Tests/CollisionPrimitiveTests.cs b/csharp/RocketWelder.SDK.Robotics.Core.Tests/CollisionPrimitiveTests.cs deleted file mode 100644 index 07d286b..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core.Tests/CollisionPrimitiveTests.cs +++ /dev/null @@ -1,161 +0,0 @@ -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core.Tests; - -/// TASK-002 — primitives, PrimitiveCollisionSource, safety margin. -public class CollisionPrimitiveTests -{ - private static Point3 P(double x, double y, double z) => new(x, y, z); - - [Fact] - public void Sphere_SignedDistance_To_Passing_Segment_Should_Be_Negative_When_Penetrating() - { - var s = new SpherePrimitive("s", P(0, 0, 0), 10); - var d = s.SignedDistanceToSegment(P(-50, 0, 0), P(50, 0, 0)); - d.Should().BeLessThan(0); - d.Should().BeApproximately(-10, 1e-6); - } - - [Fact] - public void Sphere_Far_Segment_Should_Be_Positive_Distance() - { - var s = new SpherePrimitive("s", P(0, 0, 0), 5); - var d = s.SignedDistanceToSegment(P(0, 0, 50), P(100, 0, 50)); - d.Should().BeApproximately(45, 1e-6); - } - - [Fact] - public void Box_Penetrating_Segment_Should_Report_Negative_Distance() - { - var box = new BoxPrimitive("b", P(0, 0, 0), 10, 10, 10); - var d = box.SignedDistanceToSegment(P(-50, 0, 0), P(50, 0, 0)); - d.Should().BeLessThanOrEqualTo(0); - } - - [Fact] - public void Box_Far_Segment_Should_Be_Positive_Distance() - { - var box = new BoxPrimitive("b", P(0, 0, 0), 10, 10, 10); - var d = box.SignedDistanceToSegment(P(50, 50, 0), P(50, -50, 0)); - d.Should().BeGreaterThan(0); - } - - [Fact] - public void Plane_Below_Origin_Should_Report_Positive_Distance_For_Segment_Above() - { - var plane = new PlanePrimitive("floor", P(0, 0, 0), new Vector3(0, 0, 1)); - var d = plane.SignedDistanceToSegment(P(0, 0, 100), P(100, 100, 50)); - d.Should().BeApproximately(50, 1e-6); - } - - [Fact] - public void Plane_Crossing_Segment_Should_Report_Negative_Distance() - { - var plane = new PlanePrimitive("floor", P(0, 0, 0), new Vector3(0, 0, 1)); - var d = plane.SignedDistanceToSegment(P(0, 0, -5), P(0, 0, 10)); - d.Should().BeApproximately(-5, 1e-6); - } - - [Fact] - public void Capsule_Crossing_Segment_Should_Report_Negative_Distance() - { - var c = new CapsulePrimitive("c", P(-50, 0, 0), P(50, 0, 0), 5); - var d = c.SignedDistanceToSegment(P(0, -100, 0), P(0, 100, 0)); - d.Should().BeApproximately(-5, 1e-6); - } - - [Fact] - public void Cylinder_Penetrating_Segment_Should_Report_Negative_Distance() - { - var cyl = new CylinderPrimitive("c", P(0, 0, 0), 10, 100); - var d = cyl.SignedDistanceToSegment(P(-50, 0, 0), P(50, 0, 0)); - d.Should().BeLessThanOrEqualTo(0); - } - - [Fact] - public void PrimitiveCollisionSource_Should_Reject_Wrong_LinkRadii_Count() - { - var src = new PrimitiveCollisionSource(); - var model = RobotPresets.FairinoFR5(); - - var act = () => src.QueryCollision(model, Joints6.Zero, new double[] { 50, 50, 50 }, - ToolModel.None, 0); - - act.Should().Throw(); - } - - [Fact] - public void PrimitiveCollisionSource_Should_Reject_Negative_Margin() - { - var src = new PrimitiveCollisionSource(); - var model = RobotPresets.FairinoFR5(); - var radii = Enumerable.Repeat(50.0, 6).ToArray(); - - var act = () => src.QueryCollision(model, Joints6.Zero, radii, ToolModel.None, -1); - - act.Should().Throw(); - } - - [Fact] - public void PrimitiveCollisionSource_Should_Detect_Floor_Hit_By_Link_Capsule() - { - var floor = new PlanePrimitive("floor", P(0, 0, -500), new Vector3(0, 0, 1)); - var src = new PrimitiveCollisionSource(floor); - var model = RobotPresets.FairinoFR5(); - var radii = Enumerable.Repeat(30.0, 6).ToArray(); - - var results = src.QueryCollision(model, Joints6.Zero, radii, ToolModel.None, safetyMargin: 0); - - // The home TCP is above the floor, so no collision expected. - results.Should().BeEmpty(); - } - - [Fact] - public void PrimitiveCollisionSource_Should_Detect_Box_Encasing_Robot_Base() - { - // A box that envelops the base will collide with Link 1 capsule. - var box = new BoxPrimitive("encloser", P(0, 0, 0), 300, 300, 300); - var src = new PrimitiveCollisionSource(box); - var model = RobotPresets.FairinoFR5(); - var radii = Enumerable.Repeat(40.0, 6).ToArray(); - - var results = src.QueryCollision(model, Joints6.Zero, radii, ToolModel.None, safetyMargin: 0); - - results.Should().NotBeEmpty(); - results.Should().Contain(r => r.BodyB == "encloser"); - } - - [Fact] - public void SafetyMargin_Should_Promote_A_Near_Miss_To_A_Collision() - { - // Put a sphere 10mm off the home tcp Z ray so that it is clear at margin=0 but - // hit at margin=large. We just need a sphere positioned far from any link. - var sphere = new SpherePrimitive("orb", P(2000, 0, 0), 5); - var src = new PrimitiveCollisionSource(sphere); - var model = RobotPresets.FairinoFR5(); - var radii = Enumerable.Repeat(30.0, 6).ToArray(); - - var no = src.QueryCollision(model, Joints6.Zero, radii, ToolModel.None, safetyMargin: 0); - var yes = src.QueryCollision(model, Joints6.Zero, radii, ToolModel.None, safetyMargin: 10_000); - - no.Should().BeEmpty(); - yes.Should().NotBeEmpty(); - } - - [Fact] - public void PrimitiveCollisionSource_Should_Include_Tool_Capsule_When_Present() - { - // Place a sphere right along +Z of the flange, 200mm above the flange frame origin, small radius. - // Home flange-z in base frame = flange X/Y/Z basis rotated — we just confirm Tool contributes - // when safety margin is huge enough to overlap something. - var sphere = new SpherePrimitive("target", P(0, 0, 0), 1); - var src = new PrimitiveCollisionSource(sphere); - var model = RobotPresets.FairinoFR5(); - var radii = Enumerable.Repeat(1.0, 6).ToArray(); - - var tool = new CapsuleToolModel(100, 10); - var results = src.QueryCollision(model, Joints6.Zero, radii, tool, safetyMargin: 10_000); - - results.Should().Contain(r => r.BodyA == "Tool"); - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core.Tests/FkValidatorTests.cs b/csharp/RocketWelder.SDK.Robotics.Core.Tests/FkValidatorTests.cs deleted file mode 100644 index 14f8e3f..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core.Tests/FkValidatorTests.cs +++ /dev/null @@ -1,141 +0,0 @@ -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core.Tests; - -/// TASK-008 — FkValidator import/export + Validate. -public class FkValidatorTests -{ - [Fact] - public void Export_Writes_Schema_Version_And_Units() - { - var records = new List - { - new(new Joints6(10, -20, 30, -40, 50, -60), - new Pose3(400, 0, 300, 180, 0, 0)) - }; - - var json = FkValidator.Export(records); - - json.Should().Contain("\"schemaVersion\":\"1\""); - json.Should().Contain("\"units\""); - json.Should().Contain("\"joints\":\"degrees\""); - json.Should().Contain("\"position\":\"mm\""); - json.Should().Contain("\"rotation\":\"degrees\""); - } - - [Fact] - public void Import_RoundTrip_Should_Preserve_Records() - { - var original = new List - { - new(new Joints6(10, -20, 30, -40, 50, -60), - new Pose3(400.5, 100.25, 300.75, 180, 0, 45)), - new(new Joints6(0, 0, 0, 0, 0, 0), - new Pose3(0, 0, 0, 0, 0, 0)) - }; - - var json = FkValidator.Export(original); - var restored = FkValidator.Import(json); - - restored.Should().HaveCount(2); - restored[0].Joints.Should().Be(original[0].Joints); - restored[0].TcpPose.X.Should().BeApproximately(400.5, 1e-9); - ((double)restored[0].TcpPose.Rz).Should().BeApproximately(45, 1e-9); - } - - [Fact] - public void Import_Unsupported_Schema_Version_Should_Throw() - { - var badJson = """{"schemaVersion":"99","units":{"joints":"degrees","position":"mm","rotation":"degrees"},"records":[]}"""; - - var act = () => FkValidator.Import(badJson); - - act.Should().Throw() - .WithMessage("*99*"); - } - - [Fact] - public void Import_Null_Should_Throw() - { - var act = () => FkValidator.Import(null!); - act.Should().Throw(); - } - - [Fact] - public void Validate_Records_Generated_By_Same_Model_Should_Have_Zero_Error() - { - var model = RobotPresets.FairinoFR5(); - var joints = new Joints6[] - { - new(0, 0, 0, 0, 0, 0), - new(10, -20, 30, -40, 50, -60), - new(-90, 45, -45, 0, 90, 0) - }; - var records = joints.Select(j => - { - var state = ForwardKinematics.Compute(model, j); - return new FkValidationRecord(j, state.TcpPose); - }).ToList(); - - var results = FkValidator.Validate(model, records); - - results.Should().HaveCount(3); - foreach (var r in results) - { - r.EuclideanDistance.Should().BeApproximately(0, 1e-6); - Math.Abs(r.DX).Should().BeLessThan(1e-6); - Math.Abs(r.DY).Should().BeLessThan(1e-6); - Math.Abs(r.DZ).Should().BeLessThan(1e-6); - Math.Abs(r.DRx).Should().BeLessThan(1e-6); - Math.Abs(r.DRy).Should().BeLessThan(1e-6); - Math.Abs(r.DRz).Should().BeLessThan(1e-6); - } - } - - [Fact] - public void Validate_Known_Position_Offset_Should_Report_Correct_Euclidean() - { - var model = RobotPresets.FairinoFR5(); - var joints = new Joints6(0, 0, 0, 0, 0, 0); - var truth = ForwardKinematics.Compute(model, joints).TcpPose; - - // Shift expected pose by (+3, +4, 0) — Euclidean = 5. - var shifted = new Pose3(truth.X + 3, truth.Y + 4, truth.Z, truth.Rx, truth.Ry, truth.Rz); - var records = new List { new(joints, shifted) }; - - var results = FkValidator.Validate(model, records); - - results.Should().HaveCount(1); - results[0].Index.Should().Be(0); - results[0].DX.Should().BeApproximately(3, 1e-6); - results[0].DY.Should().BeApproximately(4, 1e-6); - results[0].DZ.Should().BeApproximately(0, 1e-6); - results[0].EuclideanDistance.Should().BeApproximately(5, 1e-6); - } - - [Fact] - public void Validate_Rotation_Wrap_Should_Report_Small_Delta_Across_Pm180() - { - var model = RobotPresets.FairinoFR5(); - var joints = new Joints6(0, 0, 0, 0, 0, 0); - var truth = ForwardKinematics.Compute(model, joints).TcpPose; - - // Simulate a controller reporting +179 while FK yields -179 — wrapped delta is 2°, not 358°. - var aRx = (double)truth.Rx; - var shiftedRx = aRx + 358; // differs by 358 but wraps to -2 - var shifted = new Pose3(truth.X, truth.Y, truth.Z, shiftedRx, (double)truth.Ry, (double)truth.Rz); - var records = new List { new(joints, shifted) }; - - var results = FkValidator.Validate(model, records); - - Math.Abs(results[0].DRx).Should().BeLessThan(2.001); - } - - [Fact] - public void Validate_Null_Model_Should_Throw() - { - var records = new List(); - var act = () => FkValidator.Validate(null!, records); - act.Should().Throw(); - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core.Tests/ForwardKinematicsTests.cs b/csharp/RocketWelder.SDK.Robotics.Core.Tests/ForwardKinematicsTests.cs deleted file mode 100644 index f672638..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core.Tests/ForwardKinematicsTests.cs +++ /dev/null @@ -1,156 +0,0 @@ -using ModelingEvolution.Drawing; -using static RocketWelder.SDK.Robotics.Core.Tests.TestData; - -namespace RocketWelder.SDK.Robotics.Core.Tests; - -/// Tests 1.2, 1.3, 1.4, 1.18, 1.21 — Forward Kinematics. -public class ForwardKinematicsTests -{ - private readonly RobotModel _model = CreateFR5(); - - /// Test 1.2 — FK at home position. - [Fact] - public void FK_Home_Should_Produce_Correct_TcpPose() - { - var state = ForwardKinematics.Compute(_model, HOME); - AssertPoseEquals(FK_HOME, state.TcpPose); - } - - /// Test 1.2 — FK -> IK -> FK round-trip at HOME. - [Fact] - public void FK_IK_FK_RoundTrip_AtHome() - { - var state = ForwardKinematics.Compute(_model, HOME); - var ikResult = InverseKinematics.Compute(_model, state.TcpPose, HOME); - ikResult.Success.Should().BeTrue(); - - var state2 = ForwardKinematics.Compute(_model, ikResult.Joints); - AssertPoseEquals(state.TcpPose, state2.TcpPose); - } - - /// Test 1.3 — FK at CFG-A. - [Fact] - public void FK_CFGA_Should_Produce_Correct_TcpPose() - { - var state = ForwardKinematics.Compute(_model, CFG_A); - AssertPoseEquals(FK_CFG_A, state.TcpPose); - } - - /// Test 1.3 — FK at CFG-D. - [Fact] - public void FK_CFGD_Should_Produce_Correct_TcpPose() - { - var state = ForwardKinematics.Compute(_model, CFG_D); - AssertPoseEquals(FK_CFG_D, state.TcpPose); - } - - /// Test 1.3 — FK -> IK -> FK round-trip at CFG-A. - [Fact] - public void FK_IK_FK_RoundTrip_AtCfgA() - { - var state = ForwardKinematics.Compute(_model, CFG_A); - var ikResult = InverseKinematics.Compute(_model, state.TcpPose, CFG_A); - ikResult.Success.Should().BeTrue(); - - var state2 = ForwardKinematics.Compute(_model, ikResult.Joints); - AssertPoseEquals(state.TcpPose, state2.TcpPose); - } - - /// Test 1.4 — FK ground-truth at all standard configurations. - [Theory] - [MemberData(nameof(AllConfigurations))] - public void FK_GroundTruth_AllConfigs(string name, Joints6 joints, Pose3 expected) - { - var state = ForwardKinematics.Compute(_model, joints); - AssertPoseEquals(expected, state.TcpPose, because: $"FK at {name}"); - } - - /// Test 1.4 — FK -> IK -> FK round-trip at all standard configurations. - [Theory] - [MemberData(nameof(AllConfigurations))] - public void FK_IK_FK_RoundTrip_AllConfigs(string name, Joints6 joints, Pose3 _) - { - var state = ForwardKinematics.Compute(_model, joints); - var ikResult = InverseKinematics.Compute(_model, state.TcpPose, joints); - ikResult.Success.Should().BeTrue($"IK should succeed for {name}"); - - // Check IK joints are close to original - for (int i = 0; i < 6; i++) - AssertAngleApprox((double)ikResult.Joints[i], (double)joints[i], JointTol, - $"Joint {i} for {name}"); - - var state2 = ForwardKinematics.Compute(_model, ikResult.Joints); - AssertPoseEquals(state.TcpPose, state2.TcpPose, because: $"FK round-trip at {name}"); - } - - /// Test 1.18 — Euler angle decomposition uses ZYX convention. - [Fact] - public void EulerDecomposition_Should_Use_ZYX_Convention() - { - // Build R = Rz(45) * Ry(30) * Rx(15) - var pose = new Pose3(0, 0, 0, 15, 30, 45); - var matrix = ForwardKinematics.PoseToMatrix(pose); - var decomposed = ForwardKinematics.MatrixToPose(matrix); - - AssertAngleApprox((double)decomposed.Rx, 15.0, RotTol, "Rx should be 15"); - AssertAngleApprox((double)decomposed.Ry, 30.0, RotTol, "Ry should be 30"); - AssertAngleApprox((double)decomposed.Rz, 45.0, RotTol, "Rz should be 45"); - - // Verify it's NOT XYZ order (which would give different values) - // If someone used XYZ, Rx would be 45, not 15 - Math.Abs((double)decomposed.Rx - 45.0).Should().BeGreaterThan(1.0, "Must not be XYZ convention"); - } - - /// Test 1.21 — FK produces correct intermediate frame poses. - [Fact] - public void FK_Home_Should_Produce_Correct_IntermediateFrames() - { - var state = ForwardKinematics.Compute(_model, HOME); - - // Frame 3 (elbow) at HOME - state.FramePoses[2].X.Should().BeApproximately(-425.0, PosTol, "Frame 3 X at HOME"); - state.FramePoses[2].Y.Should().BeApproximately(0.0, PosTol, "Frame 3 Y at HOME"); - state.FramePoses[2].Z.Should().BeApproximately(152.0, PosTol, "Frame 3 Z at HOME"); - } - - [Fact] - public void FK_CfgA_Should_Produce_Correct_IntermediateFrame3() - { - var state = ForwardKinematics.Compute(_model, CFG_A); - - state.FramePoses[2].X.Should().BeApproximately(-295.9548, PosTol, "Frame 3 X at CFG-A"); - state.FramePoses[2].Y.Should().BeApproximately(-52.1848, PosTol, "Frame 3 Y at CFG-A"); - state.FramePoses[2].Z.Should().BeApproximately(-148.5204, PosTol, "Frame 3 Z at CFG-A"); - } - - [Fact] - public void FK_Should_Populate_All_7_FramePoses() - { - var state = ForwardKinematics.Compute(_model, CFG_A); - state.FramePoses.Count.Should().Be(7, "6 link frames + TCP"); - - for (int i = 0; i < 7; i++) - { - double.IsNaN(state.FramePoses[i].X).Should().BeFalse($"Frame {i} X should not be NaN"); - double.IsNaN(state.FramePoses[i].Y).Should().BeFalse($"Frame {i} Y should not be NaN"); - double.IsNaN(state.FramePoses[i].Z).Should().BeFalse($"Frame {i} Z should not be NaN"); - } - } - - [Fact] - public void FK_Frame6_Should_Match_TcpPose_WhenNoTool() - { - var state = ForwardKinematics.Compute(_model, CFG_A); - AssertPoseEquals(FK_CFG_A, state.FramePoses[6], because: "Frame 6 should match TCP when no tool"); - } - - public static IEnumerable AllConfigurations() - { - yield return ["HOME", HOME, FK_HOME]; - yield return ["CFG-A", CFG_A, FK_CFG_A]; - yield return ["CFG-B", CFG_B, FK_CFG_B]; - yield return ["CFG-C", CFG_C, FK_CFG_C]; - yield return ["CFG-D", CFG_D, FK_CFG_D]; - yield return ["CFG-E", CFG_E, FK_CFG_E]; - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core.Tests/InverseKinematicsTests.cs b/csharp/RocketWelder.SDK.Robotics.Core.Tests/InverseKinematicsTests.cs deleted file mode 100644 index fdf3d7b..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core.Tests/InverseKinematicsTests.cs +++ /dev/null @@ -1,221 +0,0 @@ -using ModelingEvolution.Drawing; -using static RocketWelder.SDK.Robotics.Core.Tests.TestData; - -namespace RocketWelder.SDK.Robotics.Core.Tests; - -/// Tests 1.5, 1.6, 1.12, 1.13, 1.14, 1.15, 1.16, 1.17, 1.19, 1.20 — Inverse Kinematics. -public class InverseKinematicsTests -{ - private readonly RobotModel _model = CreateFR5(); - - /// Test 1.5 — IK reports unreachable pose. - [Fact] - public void IK_Unreachable_Should_Fail_WithOutOfReach() - { - var farPose = new Pose3(2000, 0, 0, 0, 0, 0); - var result = InverseKinematics.Compute(_model, farPose, HOME); - result.Success.Should().BeFalse(); - result.Reason.Should().Be(IkFailureReason.OutOfReach); - } - - /// Test 1.6 — Joint limit validation (single violation). - [Fact] - public void ValidateJoints_SingleViolation() - { - // J3 limit is [-162, +162]; 200 overshoots +162 by 38. - var joints = new Joints6(0, 0, 200, 0, 0, 0); - var violations = _model.ValidateJoints(joints); - violations.Count.Should().Be(1); - violations[0].JointIndex.Should().Be(2); - violations[0].LimitDeg.Should().Be(162); - violations[0].RequestedDeg.Should().Be(200); - violations[0].OvershootDeg.Should().Be(38); - } - - /// Test 1.6 — Joint limit validation (two violations). - [Fact] - public void ValidateJoints_TwoViolations() - { - // J1 [-178,178]: -200 overshoots -178 by 22. - // J6 [-360,360]: 400 overshoots +360 by 40. - var joints = new Joints6(-200, 0, 0, 0, 0, 400); - var violations = _model.ValidateJoints(joints); - violations.Count.Should().Be(2); - - violations[0].JointIndex.Should().Be(0); - violations[0].OvershootDeg.Should().Be(22); - - violations[1].JointIndex.Should().Be(5); - violations[1].OvershootDeg.Should().Be(40); - } - - /// Test 1.6 — Joint limit validation (per-axis boundary values pass). - [Fact] - public void ValidateJoints_BoundaryValues_ShouldPass() - { - // Each joint at its upper limit (per fairino-preset-reference.md v6 limits). - var joints = new Joints6(178, 85, 162, 85, 178, 360); - var violations = _model.ValidateJoints(joints); - violations.Count.Should().Be(0); - } - - /// Test 1.12 — Singularity handling: wrist singularity (J5=0). - [Fact] - public void IK_WristSingularity_WithCloseSeed_ShouldSucceed() - { - var singularJoints = new Joints6(0, -45, 45, 30, 0, -30); - var expectedPose = new Pose3(-746.5804, 102.0000, -236.9416, -90.0000, 0.0000, 0.0000); - - var state = ForwardKinematics.Compute(_model, singularJoints); - AssertPoseEquals(expectedPose, state.TcpPose); - - var ikResult = InverseKinematics.Compute(_model, state.TcpPose, singularJoints); - ikResult.Success.Should().BeTrue("IK should succeed with close seed at wrist singularity"); - - var state2 = ForwardKinematics.Compute(_model, ikResult.Joints); - AssertPoseEquals(state.TcpPose, state2.TcpPose, SingPosTol, SingRotTol, - "FK round-trip at wrist singularity (relaxed tolerance)"); - } - - /// Test 1.13 — Singularity handling: elbow near fully extended. - [Fact] - public void IK_ElbowSingularity_WithCloseSeed_ShouldSucceed() - { - var nearExtendedJoints = new Joints6(0, -3, 3, 0, 0, 0); - var expectedPose = new Pose3(-819.4276, 102.0000, 27.6572, -90.0000, 0.0000, 0.0000); - - var state = ForwardKinematics.Compute(_model, nearExtendedJoints); - AssertPoseEquals(expectedPose, state.TcpPose); - - var ikResult = InverseKinematics.Compute(_model, state.TcpPose, nearExtendedJoints); - ikResult.Success.Should().BeTrue("IK should succeed with close seed at elbow singularity"); - - var state2 = ForwardKinematics.Compute(_model, ikResult.Joints); - AssertPoseEquals(state.TcpPose, state2.TcpPose, SingPosTol, SingRotTol, - "FK round-trip at elbow singularity (relaxed tolerance)"); - } - - /// Test 1.13 — IK at elbow singularity should be deterministic. - [Fact] - public void IK_ElbowSingularity_ShouldBe_Deterministic() - { - var nearExtendedJoints = new Joints6(0, -3, 3, 0, 0, 0); - var state = ForwardKinematics.Compute(_model, nearExtendedJoints); - - var result1 = InverseKinematics.Compute(_model, state.TcpPose, nearExtendedJoints); - var result2 = InverseKinematics.Compute(_model, state.TcpPose, nearExtendedJoints); - - result1.Success.Should().BeTrue(); - result2.Success.Should().BeTrue(); - - for (int i = 0; i < 6; i++) - ((double)result1.Joints[i]).Should().BeApproximately((double)result2.Joints[i], 1e-10, - $"Joint {i} should be deterministic"); - } - - /// Test 1.15 — IK at wrist-singular pose with far seed fails. - [Fact] - public void IK_WristSingularity_WithFarSeed_ShouldFail() - { - var singularPose = new Pose3(-746.5804, 102.0000, -236.9416, -90.0000, 0.0000, 0.0000); - var result = InverseKinematics.Compute(_model, singularPose, CFG_B); - result.Success.Should().BeFalse("IK with far seed at singular pose should fail"); - result.Reason.Should().Be(IkFailureReason.Singularity); - } - - /// Test 1.16 — Shoulder near-singularity with close seed. - [Fact] - public void IK_ShoulderSingularity_WithCloseSeed_ShouldSucceed() - { - var shoulderJoints = new Joints6(0, -124, 71, 0, 0, 0); - var expectedPose = new Pose3(81.4747, 102.0000, -577.2553, -90.0000, -53.0000, 0.0000); - - var state = ForwardKinematics.Compute(_model, shoulderJoints); - AssertPoseEquals(expectedPose, state.TcpPose, SingPosTol, SingRotTol); - - var ikResult = InverseKinematics.Compute(_model, state.TcpPose, shoulderJoints); - ikResult.Success.Should().BeTrue("IK should succeed with close seed at shoulder singularity"); - - var state2 = ForwardKinematics.Compute(_model, ikResult.Joints); - AssertPoseEquals(state.TcpPose, state2.TcpPose, SingPosTol, SingRotTol); - } - - /// Test 1.17 — IK selects the branch closest to its seed (elbow-up vs elbow-down). - [Fact] - public void IK_Should_Select_ClosestToSeed() - { - // Elbow-up seed and its FK pose. - var elbowUp = new Joints6(0, -30, 60, 0, 0, 0); - var fkUp = ForwardKinematics.Compute(_model, elbowUp); - - // Discover an alternate IK branch ("elbow-down") that reaches the same pose - // by seeding IK with the sign-flipped elbow angle. Under the corrected FR5 DH - // there is no clean closed-form mirror, so we derive elbow-down from the solver. - var mirrorSeed = new Joints6(0, -30, -60, 0, 0, 0); - var mirror = InverseKinematics.Compute(_model, fkUp.TcpPose, mirrorSeed); - mirror.Success.Should().BeTrue("mirror-seeded IK must converge to the alternate branch"); - var elbowDown = mirror.Joints; - - // The two configs must differ substantially (different IK branch). - var jointDistSq = 0.0; - for (int i = 0; i < 6; i++) - jointDistSq += Math.Pow((double)elbowUp[i] - (double)elbowDown[i], 2); - Math.Sqrt(jointDistSq).Should().BeGreaterThan(30.0, "branches should be meaningfully apart"); - - // IK seeded with elbow-up returns elbow-up. - var ikResultUp = InverseKinematics.Compute(_model, fkUp.TcpPose, elbowUp); - ikResultUp.Success.Should().BeTrue(); - for (int i = 0; i < 6; i++) - AssertAngleApprox((double)ikResultUp.Joints[i], (double)elbowUp[i], 1.0, - $"Elbow-up: joint {i}"); - - // IK seeded with elbow-down returns a config close to elbow-down. - var ikResultDown = InverseKinematics.Compute(_model, fkUp.TcpPose, elbowDown); - ikResultDown.Success.Should().BeTrue(); - for (int i = 0; i < 6; i++) - AssertAngleApprox((double)ikResultDown.Joints[i], (double)elbowDown[i], 1.0, - $"Elbow-down: joint {i}"); - - // Both IK results reach the same TCP position. - var fkResultUp = ForwardKinematics.Compute(_model, ikResultUp.Joints); - var fkResultDown = ForwardKinematics.Compute(_model, ikResultDown.Joints); - AssertPositionEquals(fkResultUp.TcpPose, fkResultDown.TcpPose, 0.1); - } - - /// Test 1.19 — IK converges from distant seed (non-singular). - [Fact] - public void IK_Should_Converge_FromDistantSeed() - { - var targetPose = FK_CFG_A; - var ikResult = InverseKinematics.Compute(_model, targetPose, HOME); - ikResult.Success.Should().BeTrue("IK should converge from HOME to CFG-A pose"); - - var fkResult = ForwardKinematics.Compute(_model, ikResult.Joints); - AssertPoseEquals(targetPose, fkResult.TcpPose, PosTol, RotTol, - "FK of IK result should match target"); - - // Joints should be within limits - var violations = _model.ValidateJoints(ikResult.Joints); - violations.Count.Should().Be(0, "IK result should be within joint limits"); - } - - /// Test 1.20 — Standard waypoints are IK-reachable with chained seeds. - [Fact] - public void StandardWaypoints_ShouldBe_IkReachable_WithChainedSeeds() - { - var waypoints = new[] { WP1, WP2, WP3, WP4, WP5 }; - var currentSeed = HOME; - - for (int i = 0; i < waypoints.Length; i++) - { - var ikResult = InverseKinematics.Compute(_model, waypoints[i], currentSeed); - ikResult.Success.Should().BeTrue($"IK should succeed for WP{i + 1}"); - - var fkResult = ForwardKinematics.Compute(_model, ikResult.Joints); - AssertPoseEquals(waypoints[i], fkResult.TcpPose, PosTol, RotTol, - $"FK of IK result should match WP{i + 1}"); - - currentSeed = ikResult.Joints; - } - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core.Tests/MoveResultTests.cs b/csharp/RocketWelder.SDK.Robotics.Core.Tests/MoveResultTests.cs deleted file mode 100644 index 08c8792..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core.Tests/MoveResultTests.cs +++ /dev/null @@ -1,70 +0,0 @@ -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core.Tests; - -/// TASK-001 — MoveResult evolution with MoveFailureReason and optional CollisionResult. -public class MoveResultTests -{ - [Fact] - public void Succeeded_Should_Have_No_Reason_No_Collision_No_Violations() - { - var result = MoveResult.Succeeded(); - - result.Success.Should().BeTrue(); - result.Reason.Should().BeNull(); - result.Collision.Should().BeNull(); - result.Violations.Should().BeNull(); - } - - [Theory] - [InlineData(MoveFailureReason.OutOfReach)] - [InlineData(MoveFailureReason.JointLimitsExceeded)] - [InlineData(MoveFailureReason.Singularity)] - [InlineData(MoveFailureReason.NoConvergence)] - [InlineData(MoveFailureReason.Collision)] - public void Failed_Should_Carry_Reason(MoveFailureReason reason) - { - var result = MoveResult.Failed(reason); - - result.Success.Should().BeFalse(); - result.Reason.Should().Be(reason); - } - - [Fact] - public void Failed_With_Violations_Should_Carry_Violations() - { - var violations = new[] { new JointLimitViolation(1, 85, 120, 35) }; - - var result = MoveResult.Failed(MoveFailureReason.JointLimitsExceeded, violations); - - result.Violations.Should().BeEquivalentTo(violations); - result.Collision.Should().BeNull(); - } - - [Fact] - public void RejectedByCollision_Should_Carry_CollisionResult() - { - var collision = new CollisionResult("Link3", "Box1", 12.5, - new Point3(100, 200, 50)); - - var result = MoveResult.RejectedByCollision(collision); - - result.Success.Should().BeFalse(); - result.Reason.Should().Be(MoveFailureReason.Collision); - result.Collision.Should().Be(collision); - result.Violations.Should().BeNull(); - } - - [Fact] - public void MoveFailureReason_Should_Cover_Every_IkFailureReason() - { - // Every IkFailureReason must have a corresponding MoveFailureReason with the same name. - foreach (IkFailureReason ik in Enum.GetValues()) - { - var match = Enum.TryParse(ik.ToString(), out _); - match.Should().BeTrue($"MoveFailureReason must define {ik}"); - } - - Enum.IsDefined(typeof(MoveFailureReason), nameof(MoveFailureReason.Collision)).Should().BeTrue(); - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core.Tests/RobotPresetTests.cs b/csharp/RocketWelder.SDK.Robotics.Core.Tests/RobotPresetTests.cs deleted file mode 100644 index 811ea91..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core.Tests/RobotPresetTests.cs +++ /dev/null @@ -1,181 +0,0 @@ -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core.Tests; - -/// Test 1.1 — Fairino v6 preset factories load correct parameters per fairino-preset-reference.md. -public class RobotPresetTests -{ - [Fact] - public void FairinoFR5_Should_Have_6_DhJoints() - { - var model = RobotPresets.FairinoFR5(); - model.DhChain.Count.Should().Be(6); - } - - [Fact] - public void FairinoFR5_Should_Have_Correct_JointLimits() - { - var model = RobotPresets.FairinoFR5(); - var expected = new (double min, double max)[] - { - (-178, 178), (-265, 85), (-162, 162), (-265, 85), (-178, 178), (-360, 360) - }; - model.JointLimits.Count.Should().Be(6); - for (int i = 0; i < 6; i++) - { - model.JointLimits[i].MinDeg.Should().Be(expected[i].min, $"J{i + 1} min"); - model.JointLimits[i].MaxDeg.Should().Be(expected[i].max, $"J{i + 1} max"); - } - } - - [Fact] - public void FairinoFR5_Should_Have_Zero_HomePosition() - { - var model = RobotPresets.FairinoFR5(); - model.HomePosition.Should().Be(Joints6.Zero); - } - - [Fact] - public void FairinoFR5_Should_Have_Correct_Name() - { - var model = RobotPresets.FairinoFR5(); - model.Name.Should().Be("Fairino FR5"); - } - - [Fact] - public void FairinoFR5_DhChain_Should_Have_Correct_D_Values() - { - var model = RobotPresets.FairinoFR5(); - // d values per corrected DH: d1=152, d2=d3=d4=0, d5=102.1, d6=102.0 - model.DhChain[0].D.Should().BeApproximately(152.0, 0.001); - model.DhChain[1].D.Should().BeApproximately(0, 0.001); - model.DhChain[2].D.Should().BeApproximately(0, 0.001); - model.DhChain[3].D.Should().BeApproximately(0, 0.001); - model.DhChain[4].D.Should().BeApproximately(102.1, 0.001); - model.DhChain[5].D.Should().BeApproximately(102.0, 0.001); - } - - [Fact] - public void FairinoFR5_DhChain_Should_Have_Correct_A_Values() - { - var model = RobotPresets.FairinoFR5(); - // a values per corrected DH: a(J1..J6) = 0, 0, 0, -425, -395.01, 0, 0 - // Mapped to chain rows (a_{i-1}): row0=0, row1=0, row2=0, row3=-425, row4=-395.01, row5=0 - model.DhChain[0].A.Should().BeApproximately(0, 0.001); - model.DhChain[1].A.Should().BeApproximately(0, 0.001); - model.DhChain[2].A.Should().BeApproximately(-425.0, 0.001); - model.DhChain[3].A.Should().BeApproximately(-395.01, 0.001); - model.DhChain[4].A.Should().BeApproximately(0, 0.001); - model.DhChain[5].A.Should().BeApproximately(0, 0.001); - } - - [Theory] - [InlineData("Fairino FR3", 140.0, 280.0, 240.01, 102.0, 102.0)] - [InlineData("Fairino FR5", 152.0, 425.0, 395.01, 102.1, 102.0)] - [InlineData("Fairino FR10", 180.0, 700.0, 586.0, 159.0, 114.0)] - [InlineData("Fairino FR16", 180.0, 520.0, 400.0, 159.0, 114.0)] - [InlineData("Fairino FR20", 215.0, 1000.0, 716.0, 166.01, 138.0)] - [InlineData("Fairino FR30", 215.0, 700.0, 536.0, 166.01, 138.0)] - public void FairinoV6_Presets_Should_Have_Reference_LinkLengths( - string expectedName, double d1, double a2, double a3, double d5, double d6) - { - var model = expectedName switch - { - "Fairino FR3" => RobotPresets.FairinoFR3(), - "Fairino FR5" => RobotPresets.FairinoFR5(), - "Fairino FR10" => RobotPresets.FairinoFR10(), - "Fairino FR16" => RobotPresets.FairinoFR16(), - "Fairino FR20" => RobotPresets.FairinoFR20(), - "Fairino FR30" => RobotPresets.FairinoFR30(), - _ => throw new ArgumentOutOfRangeException(nameof(expectedName)), - }; - - model.Name.Should().Be(expectedName); - model.DhChain.Count.Should().Be(6); - model.DhChain[0].D.Should().BeApproximately(d1, 0.001, "d1"); - model.DhChain[2].A.Should().BeApproximately(-a2, 0.001, "a_{J3} = -a2"); - model.DhChain[3].A.Should().BeApproximately(-a3, 0.001, "a_{J4} = -a3"); - model.DhChain[3].D.Should().BeApproximately(0, 0.001, "d4 must be 0 under MDH Craig"); - model.DhChain[4].D.Should().BeApproximately(d5, 0.001, "d5"); - model.DhChain[5].D.Should().BeApproximately(d6, 0.001, "d6"); - } - - /// - /// Finding #10 smoke test — FK at HOME on every v6 preset must return a finite - /// TCP position whose distance from the base origin is within the DH-derived - /// geometric reach bound (sum of link offsets). No datasheet reach figures are - /// consulted here — the bound is computed directly from the preset's own DH - /// chain, so this is a pure self-consistency check. - /// - [Theory] - [InlineData("Fairino FR3")] - [InlineData("Fairino FR5")] - [InlineData("Fairino FR10")] - [InlineData("Fairino FR16")] - [InlineData("Fairino FR20")] - [InlineData("Fairino FR30")] - public void FairinoV6_Presets_FK_AtHome_ShouldBe_Finite_And_WithinReach(string name) - { - var model = name switch - { - "Fairino FR3" => RobotPresets.FairinoFR3(), - "Fairino FR5" => RobotPresets.FairinoFR5(), - "Fairino FR10" => RobotPresets.FairinoFR10(), - "Fairino FR16" => RobotPresets.FairinoFR16(), - "Fairino FR20" => RobotPresets.FairinoFR20(), - "Fairino FR30" => RobotPresets.FairinoFR30(), - _ => throw new ArgumentOutOfRangeException(nameof(name)), - }; - - var state = ForwardKinematics.Compute(model, Joints6.Zero); - - double.IsFinite(state.TcpPose.X).Should().BeTrue($"{name} TCP X must be finite"); - double.IsFinite(state.TcpPose.Y).Should().BeTrue($"{name} TCP Y must be finite"); - double.IsFinite(state.TcpPose.Z).Should().BeTrue($"{name} TCP Z must be finite"); - - // Triangle-inequality upper bound on |TCP| from the DH chain: sum of |a| and |d| - // across all six joint rows. 1.1 slack absorbs FK rounding / base-frame offset. - var geometricReach = 0.0; - foreach (var j in model.DhChain) - geometricReach += Math.Abs(j.A) + Math.Abs(j.D); - - var tcpMag = Math.Sqrt( - state.TcpPose.X * state.TcpPose.X + - state.TcpPose.Y * state.TcpPose.Y + - state.TcpPose.Z * state.TcpPose.Z); - - tcpMag.Should().BeLessThanOrEqualTo(geometricReach * 1.1, - $"{name} TCP magnitude must be within DH-derived reach × 1.1"); - } - - [Theory] - [InlineData("Fairino FR3")] - [InlineData("Fairino FR5")] - [InlineData("Fairino FR10")] - [InlineData("Fairino FR16")] - [InlineData("Fairino FR20")] - [InlineData("Fairino FR30")] - public void FairinoV6_Presets_Should_Share_V6_JointLimits(string name) - { - var model = name switch - { - "Fairino FR3" => RobotPresets.FairinoFR3(), - "Fairino FR5" => RobotPresets.FairinoFR5(), - "Fairino FR10" => RobotPresets.FairinoFR10(), - "Fairino FR16" => RobotPresets.FairinoFR16(), - "Fairino FR20" => RobotPresets.FairinoFR20(), - "Fairino FR30" => RobotPresets.FairinoFR30(), - _ => throw new ArgumentOutOfRangeException(nameof(name)), - }; - - var expected = new (double min, double max)[] - { - (-178, 178), (-265, 85), (-162, 162), (-265, 85), (-178, 178), (-360, 360) - }; - for (int i = 0; i < 6; i++) - { - model.JointLimits[i].MinDeg.Should().Be(expected[i].min, $"{name} J{i + 1} min"); - model.JointLimits[i].MaxDeg.Should().Be(expected[i].max, $"{name} J{i + 1} max"); - } - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core.Tests/RobotProgramTests.cs b/csharp/RocketWelder.SDK.Robotics.Core.Tests/RobotProgramTests.cs deleted file mode 100644 index 9c08ada..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core.Tests/RobotProgramTests.cs +++ /dev/null @@ -1,143 +0,0 @@ -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core.Tests; - -/// TASK-006 — RobotProgram + ProgramStep + JSON. -public class RobotProgramTests -{ - [Fact] - public void AddMoveJoint_And_AddMoveLin_Should_Append_In_Order() - { - var program = new RobotProgram(); - program.AddMoveJoint(new Joints6(10, 0, 0, 0, 0, 0)); - program.AddMoveLin(new Pose3(400, 0, 300, 180, 0, 0)); - program.AddMoveJoint(new Joints6(0, 10, 0, 0, 0, 0)); - - program.Count.Should().Be(3); - program.Steps[0].Should().BeOfType(); - program.Steps[1].Should().BeOfType(); - program.Steps[2].Should().BeOfType(); - } - - [Fact] - public void InsertAt_Should_Insert_At_Index() - { - var program = new RobotProgram(); - program.AddMoveLin(new Pose3(1, 0, 0, 0, 0, 0)); - program.AddMoveLin(new Pose3(3, 0, 0, 0, 0, 0)); - - program.InsertAt(1, new MoveLinStep(new Pose3(2, 0, 0, 0, 0, 0))); - - program.Count.Should().Be(3); - ((MoveLinStep)program.Steps[1]).Target.X.Should().Be(2); - } - - [Fact] - public void RemoveAt_Should_Drop_Step() - { - var program = new RobotProgram(); - program.AddMoveLin(new Pose3(1, 0, 0, 0, 0, 0)); - program.AddMoveLin(new Pose3(2, 0, 0, 0, 0, 0)); - - program.RemoveAt(0); - - program.Count.Should().Be(1); - ((MoveLinStep)program.Steps[0]).Target.X.Should().Be(2); - } - - [Fact] - public void ReplaceAt_Should_Overwrite_Step() - { - var program = new RobotProgram(); - program.AddMoveJoint(new Joints6(0, 0, 0, 0, 0, 0)); - program.AddMoveJoint(new Joints6(1, 0, 0, 0, 0, 0)); - - program.ReplaceAt(1, new MoveLinStep(new Pose3(500, 0, 0, 0, 0, 0))); - - program.Steps[1].Should().BeOfType(); - } - - [Theory] - [InlineData(-1)] - [InlineData(3)] - public void InsertAt_Out_Of_Range_Should_Throw(int index) - { - var program = new RobotProgram(); - program.AddMoveJoint(Joints6.Zero); - program.AddMoveJoint(Joints6.Zero); - - var act = () => program.InsertAt(index, new MoveJointStep(Joints6.Zero)); - act.Should().Throw(); - } - - [Theory] - [InlineData(-1)] - [InlineData(2)] - public void RemoveAt_Out_Of_Range_Should_Throw(int index) - { - var program = new RobotProgram(); - program.AddMoveJoint(Joints6.Zero); - program.AddMoveJoint(Joints6.Zero); - - var act = () => program.RemoveAt(index); - act.Should().Throw(); - } - - [Theory] - [InlineData(-1)] - [InlineData(2)] - public void ReplaceAt_Out_Of_Range_Should_Throw(int index) - { - var program = new RobotProgram(); - program.AddMoveJoint(Joints6.Zero); - program.AddMoveJoint(Joints6.Zero); - - var act = () => program.ReplaceAt(index, new MoveJointStep(Joints6.Zero)); - act.Should().Throw(); - } - - [Fact] - public void JSON_RoundTrip_Should_Preserve_Step_Types_And_Targets() - { - var program = new RobotProgram(); - program.AddMoveJoint(new Joints6(10, -20, 30, -40, 50, -60)); - program.AddMoveLin(new Pose3(400.5, 100.25, 300.75, 180, 0, 45)); - program.AddMoveJoint(new Joints6(0, 0, 0, 0, 0, 0)); - - var json = program.ToJson(); - var restored = RobotProgram.FromJson(json); - - restored.Count.Should().Be(3); - restored.Steps[0].Should().BeOfType(); - restored.Steps[1].Should().BeOfType(); - restored.Steps[2].Should().BeOfType(); - - var mj0 = (MoveJointStep)restored.Steps[0]; - mj0.Target.Should().Be(new Joints6(10, -20, 30, -40, 50, -60)); - - var ml = (MoveLinStep)restored.Steps[1]; - ml.Target.X.Should().BeApproximately(400.5, 1e-9); - ((double)ml.Target.Rz).Should().BeApproximately(45, 1e-9); - } - - [Fact] - public void JSON_Should_Emit_Type_Discriminator() - { - var program = new RobotProgram(); - program.AddMoveJoint(Joints6.Zero); - program.AddMoveLin(new Pose3(100, 0, 0, 0, 0, 0)); - - var json = program.ToJson(); - - json.Should().Contain("\"type\":\"moveJoint\""); - json.Should().Contain("\"type\":\"moveLin\""); - } - - [Fact] - public void Add_Null_Step_Should_Throw() - { - var program = new RobotProgram(); - var act = () => program.Add(null!); - act.Should().Throw(); - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core.Tests/RocketWelder.SDK.Robotics.Core.Tests.csproj b/csharp/RocketWelder.SDK.Robotics.Core.Tests/RocketWelder.SDK.Robotics.Core.Tests.csproj deleted file mode 100644 index 7c9f992..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core.Tests/RocketWelder.SDK.Robotics.Core.Tests.csproj +++ /dev/null @@ -1,28 +0,0 @@ - - - - net10.0 - enable - enable - false - - - - - - - - - - - - - - - - - - - - - diff --git a/csharp/RocketWelder.SDK.Robotics.Core.Tests/SimulatedRobotExtensionsTests.cs b/csharp/RocketWelder.SDK.Robotics.Core.Tests/SimulatedRobotExtensionsTests.cs deleted file mode 100644 index f0ed32f..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core.Tests/SimulatedRobotExtensionsTests.cs +++ /dev/null @@ -1,306 +0,0 @@ -using ModelingEvolution.Drawing; -using RocketWelder.SDK.Devices.Robot; - -namespace RocketWelder.SDK.Robotics.Core.Tests; - -public class SimulatedRobotExtensionsTests -{ - private static readonly double[] TinyRadii = { 5, 5, 5, 5, 5, 5 }; - - private static RobotModel NonDegenerateRobot() - { - var chain = new[] - { - DhJoint.FromDegrees(0, 0, 500, 0), - DhJoint.FromDegrees(-90, 0, 200, 0), - DhJoint.FromDegrees(0, 500, 0, 0), - DhJoint.FromDegrees(0, 400, 0, 0), - DhJoint.FromDegrees(-90, 0, 100, 0), - DhJoint.FromDegrees(90, 0, 80, 0), - }; - var limits = Enumerable.Repeat(new JointLimit(-360, 360), 6).ToArray(); - return new RobotModel("SyntheticTestBot", chain, limits, Joints6.Zero); - } - - private static CollisionEnvironment EnvWith(params CollisionPrimitive[] prims) => - new(new PrimitiveCollisionSource(prims), TinyRadii, ToolModel.None); - - private static SimulatedRobot Connected(RobotModel model, CollisionEnvironment? env = null) - { - var r = new SimulatedRobot(model, environment: env); - r.Connect(); - return r; - } - - // --- Teaching points ---------------------------------------------------- - - [Fact] - public void GetTeachingPoint_Throws_WhenNoSetAttached() - { - using var r = Connected(NonDegenerateRobot()); - Action act = () => r.GetTeachingPoint("home"); - act.Should().Throw().WithMessage("*TeachingPointSet*"); - } - - [Fact] - public void TryGetTeachingPoint_ReturnsFalse_WhenNoSetAttached() - { - using var r = Connected(NonDegenerateRobot()); - r.TryGetTeachingPoint("home", out var pose).Should().BeFalse(); - pose.Should().Be(default(Pose3)); - } - - [Fact] - public void AttachTeachingPoints_ThenCrudOperations_Roundtrip() - { - using var r = Connected(NonDegenerateRobot()); - var set = new TeachingPointSet(); - r.AttachTeachingPoints(set); - - var p = new Pose3(100, 200, 300, 0, 0, 0); - r.SetTeachingPoint("A", p); - r.GetTeachingPoint("A").Should().Be(p); - r.TryGetTeachingPoint("A", out var back).Should().BeTrue(); - back.Should().Be(p); - - r.RemoveTeachingPoint("A").Should().BeTrue(); - r.RemoveTeachingPoint("A").Should().BeFalse(); - r.TryGetTeachingPoint("A", out _).Should().BeFalse(); - } - - [Fact] - public void GetTeachingPoint_Throws_WhenNameUnknown_WithSetAttached() - { - using var r = Connected(NonDegenerateRobot()); - r.AttachTeachingPoints(new TeachingPointSet()); - Action act = () => r.GetTeachingPoint("ghost"); - act.Should().Throw(); - } - - [Fact] - public void SetTeachingPoint_Throws_WhenNoSetAttached() - { - using var r = Connected(NonDegenerateRobot()); - Action act = () => r.SetTeachingPoint("A", default); - act.Should().Throw(); - } - - [Fact] - public void RemoveTeachingPoint_Throws_WhenNoSetAttached() - { - using var r = Connected(NonDegenerateRobot()); - Action act = () => r.RemoveTeachingPoint("A"); - act.Should().Throw(); - } - - [Fact] - public void AttachTeachingPoints_NullArg_Throws() - { - using var r = Connected(NonDegenerateRobot()); - Action act = () => r.AttachTeachingPoints(null!); - act.Should().Throw(); - } - - // --- Collision-aware moves (MoveResult contract per ADR-004) ------------- - - [Fact] - public void MoveJoint_WithoutEnvironment_AcceptsAnyReachableTarget() - { - using var r = Connected(NonDegenerateRobot()); - var result = r.MoveJoint(new Joints6(10, -20, 30, 0, 45, 0)); - result.Success.Should().BeTrue(); - } - - [Fact] - public void MoveJoint_WithEnvironment_RejectsCollidingTarget_NoThrow() - { - var box = new BoxPrimitive("Wall", new Point3(0, 0, 0), 10_000, 10_000, 10_000); - using var r = Connected(NonDegenerateRobot(), EnvWith(box)); - var beforeJoints = r.GetJointPositions(); - - var result = r.MoveJoint(new Joints6(0, 0, 0, 0, 0, 0)); - result.Success.Should().BeFalse(); - result.Reason.Should().Be(MoveFailureReason.Collision); - result.Collision.Should().NotBeNull(); - r.GetJointPositions().Should().Be(beforeJoints, "state must be unchanged on collision"); - } - - [Fact] - public void MoveLin_WithEnvironment_RejectsCollidingTarget_NoThrow() - { - var box = new BoxPrimitive("Wall", new Point3(0, 0, 0), 10_000, 10_000, 10_000); - using var r = Connected(NonDegenerateRobot(), EnvWith(box)); - - var actualPose = r.GetActualPose(); - var result = r.MoveLin(actualPose, Velocity.Percentage(50)); - result.Success.Should().BeFalse(); - result.Reason.Should().Be(MoveFailureReason.Collision); - result.Collision.Should().NotBeNull(); - } - - [Fact] - public void MoveJoint_WithJointLimitViolation_ReturnsJointLimitsExceeded_NotCollision() - { - var box = new BoxPrimitive("Wall", new Point3(0, 0, 0), 10_000, 10_000, 10_000); - using var r = Connected(NonDegenerateRobot(), EnvWith(box)); - - var result = r.MoveJoint(new Joints6(500, 0, 0, 0, 0, 0)); - result.Success.Should().BeFalse(); - result.Reason.Should().Be(MoveFailureReason.JointLimitsExceeded); - } - - // --- IRobot adapter (legacy int/void signatures) -------------------------- - - [Fact] - public void IRobotMoveLin_OnCollision_ReturnsMinusTwo() - { - var box = new BoxPrimitive("Wall", new Point3(0, 0, 0), 10_000, 10_000, 10_000); - using var r = Connected(NonDegenerateRobot(), EnvWith(box)); - - ((IRobot)r).MoveLin(r.GetActualPose(), Velocity.Percentage(50)).Should().Be(-2); - } - - [Fact] - public void IRobotMoveJoint_OnCollision_DoesNotThrow_StateUnchanged() - { - var box = new BoxPrimitive("Wall", new Point3(0, 0, 0), 10_000, 10_000, 10_000); - using var r = Connected(NonDegenerateRobot(), EnvWith(box)); - var before = r.GetJointPositions(); - - Action act = () => ((IRobot)r).MoveJoint(new Joints6(0, 0, 0, 0, 0, 0)); - act.Should().NotThrow(); - r.GetJointPositions().Should().Be(before); - } - - // --- ExecuteWaypoints collision ----------------------------------------- - - [Fact] - public void ExecuteWaypoints_WithEnvironment_FailsByCollision() - { - var box = new BoxPrimitive("Wall", new Point3(0, 0, 0), 10_000, 10_000, 10_000); - using var r = Connected(NonDegenerateRobot(), EnvWith(box)); - - var result = r.ExecuteWaypoints(new[] { r.GetActualPose() }, Velocity.Percentage(50)); - result.Success.Should().BeFalse(); - result.FailureReason.Should().Be(MoveFailureReason.Collision); - result.Collision.Should().NotBeNull(); - result.FailedWaypointIndex.Should().Be(0); - } - - [Fact] - public void ExecuteWaypoints_WithSafeEndpointsButCollidingIntermediate_TruncatesAtLastSafeStep() - { - // Box placed along the J1=45° sweep, away from J1=0 and J1=90. Endpoints are - // collision-free; only intermediate interpolated joint states hit it. This - // regression asserts that path-level collision detection (not just target-only) - // catches the hit and the trajectory is truncated at the last safe sub-step. - var box = new BoxPrimitive("MidSwingObstacle", new Point3(707, 707, 500), 150, 150, 400); - var model = NonDegenerateRobot(); - var env = new CollisionEnvironment(new PrimitiveCollisionSource(box), TinyRadii, ToolModel.None); - - using var r = new SimulatedRobot(model, environment: env); - r.Connect(); - var startPose = r.GetActualPose(); - - using var probe = new SimulatedRobot(model); - probe.Connect(); - probe.MoveJoint(new Joints6(90, 0, 0, 0, 0, 0)); - var targetPose = probe.GetActualPose(); - - // Sanity: endpoints are collision-free — so only path-level (intermediate) detection can catch this. - CollisionDetector.CheckCollision(model, Joints6.Zero, env).Should().BeEmpty(); - CollisionDetector.CheckCollision(model, new Joints6(90, 0, 0, 0, 0, 0), env).Should().BeEmpty(); - - var result = r.ExecuteWaypoints(new[] { startPose, targetPose }, Velocity.Percentage(50)); - - result.Success.Should().BeFalse(); - result.FailureReason.Should().Be(MoveFailureReason.Collision); - result.Collision.Should().NotBeNull(); - result.FailedWaypointIndex.Should().Be(1); - - foreach (var step in result.Steps) - { - CollisionDetector.CheckCollision(model, step.Joints, env) - .Should().BeEmpty("every recorded step must be collision-free (truncate-on-collision)"); - } - } - - // --- Program execution --------------------------------------------------- - - [Fact] - public void Execute_RunsEveryStep_AndStepsCoverTrajectory() - { - using var r = Connected(NonDegenerateRobot()); - var program = new RobotProgram(); - program.AddMoveJoint(new Joints6(10, -10, 20, 0, 0, 0)); - program.AddMoveJoint(new Joints6(20, -15, 25, 0, 0, 0)); - - var result = r.Execute(program, Velocity.Percentage(50)); - result.Success.Should().BeTrue(); - // 1 initial + at least 1 intermediate per step → > program.Count - result.Steps.Count.Should().BeGreaterThan(program.Count); - result.Steps[0].Joints.Should().Be(Joints6.Zero); - result.Steps[^1].Joints.Should().Be(new Joints6(20, -15, 25, 0, 0, 0)); - } - - [Fact] - public void Execute_WithStartIndex_SkipsEarlierSteps() - { - using var r = Connected(NonDegenerateRobot()); - var program = new RobotProgram(); - program.AddMoveJoint(new Joints6(90, 0, 0, 0, 0, 0)); // skipped - program.AddMoveJoint(new Joints6(0, -20, 0, 0, 0, 0)); // first executed - - var result = r.Execute(program, Velocity.Percentage(50), startIndex: 1); - result.Success.Should().BeTrue(); - ((double)result.Steps[^1].Joints.J1).Should().Be(0); - ((double)result.Steps[^1].Joints.J2).Should().Be(-20); - } - - [Fact] - public void Execute_StartIndexOutOfRange_Throws() - { - using var r = Connected(NonDegenerateRobot()); - var program = new RobotProgram(); - program.AddMoveJoint(new Joints6(10, 0, 0, 0, 0, 0)); - - Action neg = () => r.Execute(program, Velocity.Percentage(50), -1); - Action past = () => r.Execute(program, Velocity.Percentage(50), 2); - neg.Should().Throw(); - past.Should().Throw(); - } - - [Fact] - public void Execute_MoveLinStep_InterpolatesToPose() - { - using var r = Connected(NonDegenerateRobot()); - var reachable = r.GetActualPose(); - var program = new RobotProgram(); - program.AddMoveLin(reachable); - - var result = r.Execute(program, Velocity.Percentage(50)); - result.Success.Should().BeTrue(); - } - - [Fact] - public void Execute_StopsOnCollisionAndReportsIndex() - { - var box = new BoxPrimitive("Wall", new Point3(0, 0, 0), 10_000, 10_000, 10_000); - using var r = Connected(NonDegenerateRobot(), EnvWith(box)); - var program = new RobotProgram(); - program.AddMoveJoint(new Joints6(0, 0, 0, 0, 0, 0)); - - var result = r.Execute(program, Velocity.Percentage(50)); - result.Success.Should().BeFalse(); - result.FailureReason.Should().Be(MoveFailureReason.Collision); - result.FailedWaypointIndex.Should().Be(0); - } - - [Fact] - public void Execute_NullProgram_Throws() - { - using var r = Connected(NonDegenerateRobot()); - Action act = () => r.Execute(null!, Velocity.Percentage(50)); - act.Should().Throw(); - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core.Tests/SimulatedRobotTests.cs b/csharp/RocketWelder.SDK.Robotics.Core.Tests/SimulatedRobotTests.cs deleted file mode 100644 index 94214bf..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core.Tests/SimulatedRobotTests.cs +++ /dev/null @@ -1,474 +0,0 @@ -using ModelingEvolution.Drawing; -using RocketWelder.SDK.Devices.Robot; -using static RocketWelder.SDK.Robotics.Core.Tests.TestData; - -namespace RocketWelder.SDK.Robotics.Core.Tests; - -/// Tests 2.0-2.19 — SimulatedRobot (IRobot implementation). -public class SimulatedRobotTests -{ - private readonly RobotModel _model = CreateFR5(); - - /// Test 2.0 — Initial state at construction. - [Fact] - public void InitialState_ShouldBe_HomeAndDisconnected() - { - using var robot = new SimulatedRobot(_model); - robot.GetJointPositions().Should().Be(Joints6.Zero); - AssertPoseEquals(FK_HOME, robot.GetActualPose()); - robot.IsConnected.Should().BeFalse(); - } - - /// Test 2.1 — MoveLin updates state. - [Fact] - public void MoveLin_ShouldUpdate_State() - { - using var robot = new SimulatedRobot(_model); - robot.Connect(); - var result = ((IRobot)robot).MoveLin(WP1, DefaultVelocity); - result.Should().Be(0); - AssertPoseEquals(WP1, robot.GetActualPose()); - - var joints = robot.GetJointPositions(); - var fk = ForwardKinematics.Compute(_model, joints); - AssertPoseEquals(WP1, fk.TcpPose, 0.001); - } - - /// Test 2.2 — MoveJoint updates state. - [Fact] - public void MoveJoint_ShouldUpdate_State() - { - using var robot = new SimulatedRobot(_model); - robot.Connect(); - ((IRobot)robot).MoveJoint(CFG_A); - robot.GetJointPositions().Should().Be(CFG_A); - AssertPoseEquals(FK_CFG_A, robot.GetActualPose()); - } - - /// Test 2.3 — TryMoveLin returns structured failure. - [Fact] - public void TryMoveLin_Unreachable_ShouldFail() - { - using var robot = new SimulatedRobot(_model); - robot.Connect(); - var result = robot.MoveLin(new Pose3(2000, 0, 0, 0, 0, 0), DefaultVelocity); - result.Success.Should().BeFalse(); - result.Reason.Should().Be(MoveFailureReason.OutOfReach); - AssertPoseEquals(FK_HOME, robot.GetActualPose(), because: "state should be unchanged after failed move"); - } - - /// Test 2.4 — TryMoveJoint validates limits. - [Fact] - public void TryMoveJoint_OutOfLimits_ShouldFail() - { - using var robot = new SimulatedRobot(_model); - robot.Connect(); - // J3 limit is [-162, 162]; 200 overshoots +162 by 38. - var result = robot.MoveJoint(new Joints6(0, 0, 200, 0, 0, 0)); - result.Success.Should().BeFalse(); - result.Reason.Should().Be(MoveFailureReason.JointLimitsExceeded); - result.Violations.Should().NotBeNull(); - result.Violations![0].JointIndex.Should().Be(2); - result.Violations[0].OvershootDeg.Should().Be(38); - robot.GetJointPositions().Should().Be(HOME, "state should remain at HOME"); - } - - [Fact] - public void TryMoveJoint_Valid_ShouldSucceed() - { - using var robot = new SimulatedRobot(_model); - robot.Connect(); - var result = robot.MoveJoint(CFG_A); - result.Success.Should().BeTrue(); - robot.GetJointPositions().Should().Be(CFG_A); - } - - /// Test 2.5 — Connect/Disconnect. - [Fact] - public void ConnectDisconnect_ShouldBeIdempotent() - { - using var robot = new SimulatedRobot(_model); - robot.IsConnected.Should().BeFalse(); - - robot.Connect(); - robot.IsConnected.Should().BeTrue(); - - robot.Connect(); // idempotent - robot.IsConnected.Should().BeTrue(); - - robot.Disconnect(); - robot.IsConnected.Should().BeFalse(); - } - - /// Test 2.6 — ExecuteWaypoints. - [Fact] - public void ExecuteWaypoints_ShouldProduceSmooth_Sequence() - { - using var robot = new SimulatedRobot(_model); - robot.Connect(); - var waypoints = new[] { WP1, WP2, WP3, WP4, WP5 }; - var result = robot.ExecuteWaypoints(waypoints, DefaultVelocity); - - result.Success.Should().BeTrue(); - result.Steps.Count.Should().BeGreaterThanOrEqualTo(20); - - // Check max step size - for (int i = 1; i < result.Steps.Count; i++) - { - var delta = (double)result.Steps[i].Joints.MaxAbsDelta(result.Steps[i - 1].Joints); - delta.Should().BeLessThanOrEqualTo(5.0 + 0.001, - $"step {i} joint change should be <= 5.0 degrees"); - } - - // Final pose should match WP5 - AssertPoseEquals(WP5, result.Steps[^1].TcpPose); - - // All steps should have FK-consistent state - foreach (var step in result.Steps) - { - var fk = ForwardKinematics.Compute(_model, step.Joints); - AssertPoseEquals(step.TcpPose, fk.TcpPose, 0.001, 0.001); - } - } - - /// Test 2.7a — Address round-trips. - [Fact] - public void Address_ShouldRoundTrip() - { - using var robot = new SimulatedRobot(_model); - IRobot iRobot = robot; - iRobot.Address = new Uri("http://192.168.1.100:8080"); - iRobot.Address.Should().Be(new Uri("http://192.168.1.100:8080")); - } - - /// Test 2.7b — JointMode round-trips. - [Fact] - public void JointMode_ShouldRoundTrip() - { - using var robot = new SimulatedRobot(_model); - IRobot iRobot = robot; - iRobot.JointMode = true; - iRobot.JointMode.Should().BeTrue(); - iRobot.JointMode = false; - iRobot.JointMode.Should().BeFalse(); - } - - /// Test 2.7c — IsAvailableAsync reflects connection state. - [Fact] - public async Task IsAvailableAsync_ShouldReflect_ConnectionState() - { - using var robot = new SimulatedRobot(_model); - IRobot iRobot = robot; - (await iRobot.IsAvailableAsync()).Should().BeFalse(); - iRobot.Connect(); - (await iRobot.IsAvailableAsync()).Should().BeTrue(); - } - - /// Test 2.7d — TryGetActualPose returns current pose. - [Fact] - public void TryGetActualPose_ShouldReturn_CurrentPose() - { - using var robot = new SimulatedRobot(_model); - robot.Connect(); - IRobot iRobot = robot; - - iRobot.TryGetActualPose(out var pose).Should().BeTrue(); - AssertPoseEquals(FK_HOME, pose); - - iRobot.MoveLin(WP1, DefaultVelocity); - iRobot.TryGetActualPose(out pose).Should().BeTrue(); - AssertPoseEquals(WP1, pose); - } - - /// Test 2.7e — Robot operates normally after ResetAllErrors. - [Fact] - public void ResetAllErrors_ShouldAllow_NormalOperation() - { - using var robot = new SimulatedRobot(_model); - robot.Connect(); - - var failResult = robot.MoveLin(new Pose3(2000, 0, 0, 0, 0, 0), DefaultVelocity); - failResult.Success.Should().BeFalse(); - - ((IRobot)robot).ResetAllErrors(); - - ((IRobot)robot).MoveLin(WP1, DefaultVelocity); - AssertPoseEquals(WP1, robot.GetActualPose()); - } - - /// Test 2.8 — PoseStream emits on movement. - [Fact] - public void PoseStream_ShouldEmit_OnMovement() - { - using var robot = new SimulatedRobot(_model); - robot.Connect(); - - var emitted = new List>(); - using var sub = robot.PoseStream.Subscribe(p => emitted.Add(p)); - - ((IRobot)robot).MoveLin(WP1, DefaultVelocity); - - emitted.Count.Should().BeGreaterThanOrEqualTo(1); - AssertPoseEquals(WP1, emitted[^1]); - } - - /// Test 2.9 — Connected/Disconnected events. - [Fact] - public void Events_ShouldFire_OnConnectDisconnect() - { - using var robot = new SimulatedRobot(_model); - int connectedCount = 0, disconnectedCount = 0; - robot.Connected += (_, _) => connectedCount++; - robot.Disconnected += (_, _) => disconnectedCount++; - - robot.Connect(); - connectedCount.Should().Be(1); - robot.IsConnected.Should().BeTrue(); - - robot.Disconnect(); - disconnectedCount.Should().Be(1); - robot.IsConnected.Should().BeFalse(); - - robot.Connect(); - robot.Disconnect(); - connectedCount.Should().Be(2); - disconnectedCount.Should().Be(2); - } - - /// Test 2.10 — Dispose releases resources. - [Fact] - public void Dispose_ShouldRelease_Resources() - { - var robot = new SimulatedRobot(_model); - robot.Connect(); - robot.Dispose(); - - robot.IsConnected.Should().BeFalse(); - - var act = () => ((IRobot)robot).MoveLin(WP1, DefaultVelocity); - act.Should().Throw(); - } - - /// Test 2.11 — All velocity units accepted. - [Theory] - [MemberData(nameof(VelocityUnits))] - public void MoveLin_ShouldAccept_AllVelocityUnits(Velocity velocity) - { - using var robot = new SimulatedRobot(_model); - robot.Connect(); - - var result = ((IRobot)robot).MoveLin(WP1, velocity); - result.Should().Be(0); - AssertPoseEquals(WP1, robot.GetActualPose()); - - ((IRobot)robot).MoveJoint(HOME); // reset - } - - /// Test 2.12 — ExecuteWaypoints with unreachable waypoint. - [Fact] - public void ExecuteWaypoints_Unreachable_ShouldFail() - { - using var robot = new SimulatedRobot(_model); - robot.Connect(); - var unreachable = new Pose3(2000, 0, 0, 0, 0, 0); - var waypoints = new[] { WP1, unreachable, WP3 }; - var result = robot.ExecuteWaypoints(waypoints, DefaultVelocity); - - result.Success.Should().BeFalse(); - result.FailedWaypointIndex.Should().Be(1); - result.Reason.Should().Be(IkFailureReason.OutOfReach); - AssertPoseEquals(WP1, robot.GetActualPose(), because: "robot should stop at last successful waypoint"); - } - - /// Test 2.13 — Sequential MoveLin maintains consistent state. - [Fact] - public void SequentialMoveLin_ShouldMaintain_ConsistentState() - { - using var robot = new SimulatedRobot(_model); - robot.Connect(); - - foreach (var wp in new[] { WP1, WP2, WP3 }) - { - ((IRobot)robot).MoveLin(wp, DefaultVelocity); - AssertPoseEquals(wp, robot.GetActualPose()); - - var joints = robot.GetJointPositions(); - var fk = ForwardKinematics.Compute(_model, joints); - AssertPoseEquals(wp, fk.TcpPose, 0.001); - } - } - - /// Test 2.14 — MoveLin to current position (zero movement). - [Fact] - public void MoveLin_ToCurrentPosition_ShouldSucceed() - { - using var robot = new SimulatedRobot(_model); - robot.Connect(); - ((IRobot)robot).MoveLin(WP1, DefaultVelocity); - var jointsBefore = robot.GetJointPositions(); - - ((IRobot)robot).MoveLin(WP1, DefaultVelocity); // same target - AssertPoseEquals(WP1, robot.GetActualPose()); - } - - /// Test 1.10 — Shared RobotModel, independent state. - [Fact] - public void SharedModel_ShouldHave_IndependentState() - { - using var robot1 = new SimulatedRobot(_model); - using var robot2 = new SimulatedRobot(_model); - robot1.Connect(); - robot2.Connect(); - - ((IRobot)robot1).MoveJoint(CFG_A); - robot1.GetJointPositions().Should().Be(CFG_A); - robot2.GetJointPositions().Should().Be(HOME, "robot2 should be unaffected"); - } - - /// Test 2.15 — MoveJoint with out-of-limit joints returns structured failure, never throws (ADR-004). - [Fact] - public void MoveJoint_OutOfLimits_ReturnsJointLimitsExceeded_NoThrow() - { - using var robot = new SimulatedRobot(_model); - robot.Connect(); - // J3 limit is [-162, 162]; 200 overshoots. - var outOfLimit = new Joints6(0, 0, 200, 0, 0, 0); - var result = robot.MoveJoint(outOfLimit); - result.Success.Should().BeFalse(); - result.Reason.Should().Be(MoveFailureReason.JointLimitsExceeded); - robot.GetJointPositions().Should().Be(HOME, "state should remain at HOME on failure"); - - // IRobot adapter never surfaces joint-limit violations as exceptions under ADR-004. - var act = () => ((IRobot)robot).MoveJoint(outOfLimit); - act.Should().NotThrow(); - } - - /// Test 1.14 — Configuration jump prevention. - [Fact] - public void ExecuteWaypoints_ShouldPrevent_ConfigurationJumps() - { - using var robot = new SimulatedRobot(_model); - robot.Connect(); - ((IRobot)robot).MoveJoint(CFG_A); - - // Build waypoints by small FK offsets - var configs = new[] - { - new Joints6(10, -43, 30, -15, 60, -20), - new Joints6(10, -41, 30, -15, 60, -20), - new Joints6(10, -41, 32, -15, 60, -20), - new Joints6(10, -41, 32, -13, 60, -20), - new Joints6(10, -41, 32, -13, 58, -20), - }; - - var waypoints = new Pose3[configs.Length]; - for (int i = 0; i < configs.Length; i++) - { - var fk = ForwardKinematics.Compute(_model, configs[i]); - waypoints[i] = fk.TcpPose; - } - - var result = robot.ExecuteWaypoints(waypoints, DefaultVelocity); - result.Success.Should().BeTrue(); - - // No consecutive step should have > 45 deg change on any joint - for (int i = 1; i < result.Steps.Count; i++) - { - var delta = (double)result.Steps[i].Joints.MaxAbsDelta(result.Steps[i - 1].Joints); - delta.Should().BeLessThanOrEqualTo(45.0, - $"step {i} should not have a configuration jump > 45 degrees"); - } - } - - /// Test 2.16 — ExecuteWaypoints from non-HOME start position. - [Fact] - public void ExecuteWaypoints_FromNonHome_ShouldStartFromCurrentPosition() - { - using var robot = new SimulatedRobot(_model); - robot.Connect(); - ((IRobot)robot).MoveJoint(CFG_A); - - var waypoints = new[] { WP1, WP2, WP3 }; - var result = robot.ExecuteWaypoints(waypoints, DefaultVelocity); - result.Success.Should().BeTrue(); - - result.Steps[0].Joints.Should().Be(CFG_A, "first step should be CFG-A"); - AssertPoseEquals(WP3, result.Steps[^1].TcpPose); - - foreach (var step in result.Steps) - { - var fk = ForwardKinematics.Compute(_model, step.Joints); - AssertPoseEquals(step.TcpPose, fk.TcpPose, 0.001, 0.001); - } - } - - /// Test 2.17 — ExecuteWaypoints with single waypoint. - [Fact] - public void ExecuteWaypoints_SingleWaypoint_ShouldSucceed() - { - using var robot = new SimulatedRobot(_model); - robot.Connect(); - var result = robot.ExecuteWaypoints(new[] { WP1 }, DefaultVelocity); - result.Success.Should().BeTrue(); - result.Steps.Count.Should().BeGreaterThanOrEqualTo(1); - AssertPoseEquals(WP1, result.Steps[^1].TcpPose); - - foreach (var step in result.Steps) - { - var fk = ForwardKinematics.Compute(_model, step.Joints); - AssertPoseEquals(step.TcpPose, fk.TcpPose, 0.001, 0.001); - } - } - - /// Test 2.18 — MoveLin on disconnected robot throws. - [Fact] - public void MoveLin_WhenNotConnected_ShouldThrow() - { - using var robot = new SimulatedRobot(_model); - var act = () => ((IRobot)robot).MoveLin(WP1, DefaultVelocity); - act.Should().Throw() - .WithMessage("*not connected*"); - robot.GetJointPositions().Should().Be(HOME); - } - - /// Test 2.18b — ExecuteWaypoints on disconnected robot throws. - [Fact] - public void ExecuteWaypoints_WhenNotConnected_ShouldThrow() - { - using var robot = new SimulatedRobot(_model); - var act = () => robot.ExecuteWaypoints(new[] { WP1, WP2 }, DefaultVelocity); - act.Should().Throw() - .WithMessage("*not connected*"); - robot.GetJointPositions().Should().Be(HOME); - } - - /// Test 2.19 — ExecuteWaypoints with empty list throws. - [Fact] - public void ExecuteWaypoints_EmptyList_ShouldThrow() - { - using var robot = new SimulatedRobot(_model); - robot.Connect(); - var act = () => robot.ExecuteWaypoints(Array.Empty>(), DefaultVelocity); - act.Should().Throw(); - robot.GetJointPositions().Should().Be(HOME); - } - - /// Test 2.20 — The 3-arg MoveSpline default overload routes to the 2-arg method - /// on a 2-arg-only implementer (SimulatedRobot has no override, so it still throws NotSupported). - [Fact] - public void MoveSpline_WithSmoothness_ShouldRouteTo_TwoArgOverload() - { - using var robot = new SimulatedRobot(_model); - robot.Connect(); - var act = () => ((IRobot)robot).MoveSpline( - new[] { WP1 }, DefaultVelocity, SplineSmoothness.High); - act.Should().Throw(); - } - - public static IEnumerable VelocityUnits() - { - yield return [Velocity.Percentage(50)]; - yield return [Velocity.MmPerSecond(100)]; - yield return [Velocity.CmPerMinute(500)]; - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core.Tests/TeachingPointSetTests.cs b/csharp/RocketWelder.SDK.Robotics.Core.Tests/TeachingPointSetTests.cs deleted file mode 100644 index 91e393d..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core.Tests/TeachingPointSetTests.cs +++ /dev/null @@ -1,133 +0,0 @@ -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core.Tests; - -/// TASK-007 — TeachingPointSet CRUD + JSON. -public class TeachingPointSetTests -{ - private static Pose3 Pose(double x) => new(x, 0, 0, 0, 0, 0); - - [Fact] - public void Set_And_Get_Should_Round_Trip_Value() - { - var set = new TeachingPointSet(); - set.Set("home", Pose(100)); - - set.Get("home").Should().Be(Pose(100)); - set.Count.Should().Be(1); - } - - [Fact] - public void TryGet_Missing_Should_Return_False() - { - var set = new TeachingPointSet(); - set.TryGet("nope", out var pose).Should().BeFalse(); - pose.Should().Be(default(Pose3)); - } - - [Fact] - public void Get_Missing_Should_Throw() - { - var set = new TeachingPointSet(); - var act = () => set.Get("nope"); - act.Should().Throw(); - } - - [Fact] - public void Set_Duplicate_Name_Should_Overwrite_Without_Duplicating() - { - var set = new TeachingPointSet(); - set.Set("p", Pose(1)); - set.Set("p", Pose(2)); - - set.Count.Should().Be(1); - set.Get("p").Should().Be(Pose(2)); - } - - [Fact] - public void Remove_Should_Return_True_Then_False() - { - var set = new TeachingPointSet(); - set.Set("p", Pose(1)); - - set.Remove("p").Should().BeTrue(); - set.Remove("p").Should().BeFalse(); - set.Contains("p").Should().BeFalse(); - } - - [Fact] - public void Name_Lookup_Is_Case_Sensitive() - { - var set = new TeachingPointSet(); - set.Set("Home", Pose(1)); - - set.Contains("Home").Should().BeTrue(); - set.Contains("home").Should().BeFalse(); - set.TryGet("home", out _).Should().BeFalse(); - } - - [Fact] - public void Enumerate_Should_Yield_Insertion_Order() - { - var set = new TeachingPointSet(); - set.Set("c", Pose(3)); - set.Set("a", Pose(1)); - set.Set("b", Pose(2)); - - set.Enumerate().Select(kv => kv.Key).Should().ContainInOrder("c", "a", "b"); - set.Names.Should().ContainInOrder("c", "a", "b"); - } - - [Fact] - public void Remove_Then_Readd_Should_Append_At_End() - { - var set = new TeachingPointSet(); - set.Set("a", Pose(1)); - set.Set("b", Pose(2)); - set.Remove("a"); - set.Set("a", Pose(9)); - - set.Names.Should().ContainInOrder("b", "a"); - } - - [Fact] - public void JSON_RoundTrip_Should_Preserve_Names_Order_And_Poses() - { - var set = new TeachingPointSet(); - set.Set("home", new Pose3(400, 0, 300, 180, 0, 0)); - set.Set("pick", new Pose3(500, 100, 200, 90, 10, -45)); - set.Set("place", new Pose3(-500, 50, 150, 0, 45, 90)); - - var json = set.ToJson(); - var restored = TeachingPointSet.FromJson(json); - - restored.Count.Should().Be(3); - restored.Names.Should().ContainInOrder("home", "pick", "place"); - restored.Get("home").Should().Be(new Pose3(400, 0, 300, 180, 0, 0)); - restored.Get("pick").Should().Be(new Pose3(500, 100, 200, 90, 10, -45)); - restored.Get("place").Should().Be(new Pose3(-500, 50, 150, 0, 45, 90)); - } - - [Fact] - public void FromJson_Null_Should_Throw() - { - var act = () => TeachingPointSet.FromJson(null!); - act.Should().Throw(); - } - - [Fact] - public void Set_Null_Name_Should_Throw() - { - var set = new TeachingPointSet(); - var act = () => set.Set(null!, Pose(1)); - act.Should().Throw(); - } - - [Fact] - public void Empty_Set_JSON_RoundTrip() - { - var set = new TeachingPointSet(); - var restored = TeachingPointSet.FromJson(set.ToJson()); - restored.Count.Should().Be(0); - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core.Tests/TestData.cs b/csharp/RocketWelder.SDK.Robotics.Core.Tests/TestData.cs deleted file mode 100644 index e459773..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core.Tests/TestData.cs +++ /dev/null @@ -1,84 +0,0 @@ -using ModelingEvolution.Drawing; -using RocketWelder.SDK.Devices.Robot; - -namespace RocketWelder.SDK.Robotics.Core.Tests; - -/// -/// Shared test data: standard configurations, waypoints, and tolerances. -/// -internal static class TestData -{ - // Standard test configurations (joint angles in degrees) - public static readonly Joints6 HOME = Joints6.Zero; - public static readonly Joints6 CFG_A = new(10, -45, 30, -15, 60, -20); - public static readonly Joints6 CFG_B = new(45, -30, 60, -90, 45, 0); - public static readonly Joints6 CFG_C = new(-30, -60, 90, 0, -45, 120); - public static readonly Joints6 CFG_D = new(0, -90, 90, 0, 0, 0); - public static readonly Joints6 CFG_E = new(90, -45, 45, -45, 90, -90); - - // Standard test waypoints — FK outputs of selected joint configs, chosen so - // consecutive targets stay on the same IK branch (regression fixtures only). - // Source configs (see docs/iterations/iteration-3/dev-log.md TASK-010): - // WP1: ( 0, -30, 30, 0, 60, 0) WP2: (15, -40, 45, -15, 55, -20) - // WP3: (30, -50, 55, -30, 50, -40) WP4: (10, -35, 40, -10, 60, -10) - // WP5: (-10, -25, 35, 10, 65, 10) - public static readonly Pose3 WP1 = new(-674.7362, 51.0000, -162.6000, -90.0000, 0.0000, -60.0000); - public static readonly Pose3 WP2 = new(-613.1103, -103.7138, -172.7973, -80.9330, -25.4941, -43.5161); - public static readonly Pose3 WP3 = new(-511.4574, -219.5829, -198.6536, -58.0772, -52.2474, -43.4291); - public static readonly Pose3 WP4 = new(-643.8103, -61.7344, -151.3552, -85.5665, -12.4685, -50.8644); - public static readonly Pose3 WP5 = new(-703.7837, 167.8681, -86.5801, -108.9984, 17.7897, -79.6127); - - // Ground-truth FK values: regression baselines regenerated from our own FK output - // after the FR5 DH correction (d4=0, a4=-395.01, d5=102.1, d6=102.0) per - // docs/epics/epic-021-robot-simulator/fairino-preset-reference.md. - // These are NOT independent ground truth — they pin current behaviour against drift. - public static readonly Pose3 FK_HOME = new(-820.0100, 102.0000, 49.9000, -90.0000, 0.0000, 0.0000); - public static readonly Pose3 FK_CFG_A = new(-554.9524, -46.0663, -295.0104, -59.2660, -32.0812, -63.8352); - public static readonly Pose3 FK_CFG_B = new(-465.1283, -363.1283, 148.4170, -39.2315, -37.7612, -18.4349); - public static readonly Pose3 FK_CFG_C = new(-542.5297, 396.5123, -70.9145, 64.4386, 34.9753, 175.5614); - public static readonly Pose3 FK_CFG_D = new(-395.0100, 102.0000, -375.1000, -90.0000, 0.0000, 0.0000); - public static readonly Pose3 FK_CFG_E = new(0.0000, -551.2099, -148.5911, 0.0000, -45.0000, -90.0000); - - // Tolerances - public const double PosTol = 0.01; // mm - public const double RotTol = 0.001; // degrees - public const double JointTol = 0.01; // degrees - public const double SingPosTol = 0.1; // mm (relaxed for singularity) - public const double SingRotTol = 0.01; // degrees (relaxed for singularity) - - public static readonly Velocity DefaultVelocity = Velocity.Percentage(50); - - public static RobotModel CreateFR5() => RobotPresets.FairinoFR5(); - - public static void AssertPoseEquals(Pose3 expected, Pose3 actual, - double posTol = PosTol, double rotTol = RotTol, string? because = null) - { - actual.X.Should().BeApproximately(expected.X, posTol, because ?? $"X: expected {expected.X}"); - actual.Y.Should().BeApproximately(expected.Y, posTol, because ?? $"Y: expected {expected.Y}"); - actual.Z.Should().BeApproximately(expected.Z, posTol, because ?? $"Z: expected {expected.Z}"); - - AssertAngleApprox((double)actual.Rx, (double)expected.Rx, rotTol, because ?? $"Rx: expected {expected.Rx}"); - AssertAngleApprox((double)actual.Ry, (double)expected.Ry, rotTol, because ?? $"Ry: expected {expected.Ry}"); - AssertAngleApprox((double)actual.Rz, (double)expected.Rz, rotTol, because ?? $"Rz: expected {expected.Rz}"); - } - - /// - /// Compares two angles accounting for wraparound (e.g., 180 and -180 are the same). - /// - public static void AssertAngleApprox(double actual, double expected, double tol, string? because = null) - { - var diff = actual - expected; - // Normalize to [-180, 180] - while (diff > 180) diff -= 360; - while (diff < -180) diff += 360; - Math.Abs(diff).Should().BeLessThanOrEqualTo(tol, because ?? $"angle diff: actual={actual}, expected={expected}"); - } - - public static void AssertPositionEquals(Pose3 expected, Pose3 actual, - double posTol = PosTol, string? because = null) - { - actual.X.Should().BeApproximately(expected.X, posTol, because ?? $"X: expected {expected.X}"); - actual.Y.Should().BeApproximately(expected.Y, posTol, because ?? $"Y: expected {expected.Y}"); - actual.Z.Should().BeApproximately(expected.Z, posTol, because ?? $"Z: expected {expected.Z}"); - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core.Tests/ToolAndBaseTransformTests.cs b/csharp/RocketWelder.SDK.Robotics.Core.Tests/ToolAndBaseTransformTests.cs deleted file mode 100644 index 27cfc08..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core.Tests/ToolAndBaseTransformTests.cs +++ /dev/null @@ -1,162 +0,0 @@ -using ModelingEvolution.Drawing; -using static RocketWelder.SDK.Robotics.Core.Tests.TestData; - -namespace RocketWelder.SDK.Robotics.Core.Tests; - -/// Tests 1.7, 1.8, 1.9, 1.22 — FK with tool transform and base pose. -public class ToolAndBaseTransformTests -{ - private readonly RobotModel _model = CreateFR5(); - - /// Test 1.7 — FK with tool transform (150mm Z offset). - [Fact] - public void FK_WithTool_ShouldDiffer_FromWithout() - { - var tool = new Pose3(0, 0, 150, 0, 0, 0); - var withTool = ForwardKinematics.Compute(_model, HOME, tool); - var withoutTool = ForwardKinematics.Compute(_model, HOME); - - // Results should differ - var dist = Math.Sqrt( - Math.Pow(withTool.TcpPose.X - withoutTool.TcpPose.X, 2) + - Math.Pow(withTool.TcpPose.Y - withoutTool.TcpPose.Y, 2) + - Math.Pow(withTool.TcpPose.Z - withoutTool.TcpPose.Z, 2)); - dist.Should().BeGreaterThan(1.0, "tool transform should change TCP position"); - } - - /// Test 1.7 — FK(HOME, tool) should displace TCP by +150mm along world Y. - [Fact] - public void FK_HomeWithTool_ShouldDisplace_AlongWorldY() - { - var tool = new Pose3(0, 0, 150, 0, 0, 0); - var expected = new Pose3(-820.0100, 252.0000, 49.9000, -90.0000, 0.0000, 0.0000); - var state = ForwardKinematics.Compute(_model, HOME, tool); - AssertPoseEquals(expected, state.TcpPose); - } - - /// Test 1.7 — FK(HOME, tool) -> IK -> FK round-trip. - [Fact] - public void FK_IK_FK_RoundTrip_WithTool_AtHome() - { - var tool = new Pose3(0, 0, 150, 0, 0, 0); - var state = ForwardKinematics.Compute(_model, HOME, tool); - var ikResult = InverseKinematics.Compute(_model, state.TcpPose, HOME, tool); - ikResult.Success.Should().BeTrue(); - var state2 = ForwardKinematics.Compute(_model, ikResult.Joints, tool); - AssertPoseEquals(state.TcpPose, state2.TcpPose); - } - - /// Test 1.7 — FK(CFG-A, tool) -> IK -> FK round-trip. - [Fact] - public void FK_IK_FK_RoundTrip_WithTool_AtCfgA() - { - var tool = new Pose3(0, 0, 150, 0, 0, 0); - var state = ForwardKinematics.Compute(_model, CFG_A, tool); - var ikResult = InverseKinematics.Compute(_model, state.TcpPose, CFG_A, tool); - ikResult.Success.Should().BeTrue(); - var state2 = ForwardKinematics.Compute(_model, ikResult.Joints, tool); - AssertPoseEquals(state.TcpPose, state2.TcpPose); - } - - /// Test 1.8 — FK with rotated tool transform. - [Fact] - public void FK_WithRotatedTool_ShouldDiffer_InPositionAndOrientation() - { - var tool = new Pose3(50, 0, 100, 15, -10, 30); - var withTool = ForwardKinematics.Compute(_model, CFG_A, tool); - var withoutTool = ForwardKinematics.Compute(_model, CFG_A); - - var posDist = Math.Sqrt( - Math.Pow(withTool.TcpPose.X - withoutTool.TcpPose.X, 2) + - Math.Pow(withTool.TcpPose.Y - withoutTool.TcpPose.Y, 2) + - Math.Pow(withTool.TcpPose.Z - withoutTool.TcpPose.Z, 2)); - posDist.Should().BeGreaterThan(1.0, "rotated tool should change position"); - - // Orientation should also differ - var rotDiff = Math.Abs((double)withTool.TcpPose.Rx - (double)withoutTool.TcpPose.Rx) + - Math.Abs((double)withTool.TcpPose.Ry - (double)withoutTool.TcpPose.Ry) + - Math.Abs((double)withTool.TcpPose.Rz - (double)withoutTool.TcpPose.Rz); - rotDiff.Should().BeGreaterThan(1.0, "rotated tool should change orientation"); - } - - /// Test 1.8 — FK with rotated tool round-trips through IK. - [Fact] - public void FK_IK_FK_RoundTrip_WithRotatedTool() - { - var tool = new Pose3(50, 0, 100, 15, -10, 30); - var state = ForwardKinematics.Compute(_model, CFG_A, tool); - var ikResult = InverseKinematics.Compute(_model, state.TcpPose, CFG_A, tool); - ikResult.Success.Should().BeTrue(); - var state2 = ForwardKinematics.Compute(_model, ikResult.Joints, tool); - AssertPoseEquals(state.TcpPose, state2.TcpPose); - } - - /// Test 1.9 — FK with base pose: pure translation. - [Fact] - public void FK_WithBaseTranslation_ShouldOffset_Position() - { - var baseT = new Pose3(500, 200, 0, 0, 0, 0); - var withBase = ForwardKinematics.Compute(_model, CFG_A, basePose: baseT); - var withoutBase = ForwardKinematics.Compute(_model, CFG_A); - - // Position should be offset by (500, 200, 0) - withBase.TcpPose.X.Should().BeApproximately(withoutBase.TcpPose.X + 500, PosTol); - withBase.TcpPose.Y.Should().BeApproximately(withoutBase.TcpPose.Y + 200, PosTol); - withBase.TcpPose.Z.Should().BeApproximately(withoutBase.TcpPose.Z, PosTol); - - // Orientation should be unchanged for pure translation - AssertAngleApprox((double)withBase.TcpPose.Rx, (double)withoutBase.TcpPose.Rx, RotTol); - AssertAngleApprox((double)withBase.TcpPose.Ry, (double)withoutBase.TcpPose.Ry, RotTol); - AssertAngleApprox((double)withBase.TcpPose.Rz, (double)withoutBase.TcpPose.Rz, RotTol); - } - - /// Test 1.9 — IK round-trip with base poses. - [Theory] - [MemberData(nameof(BasePoses))] - public void FK_IK_FK_RoundTrip_WithBasePose(string name, Pose3 basePose) - { - var withBase = ForwardKinematics.Compute(_model, CFG_A, basePose: basePose); - var ikResult = InverseKinematics.Compute(_model, withBase.TcpPose, CFG_A, basePose: basePose); - ikResult.Success.Should().BeTrue($"IK should succeed with base pose {name}"); - var state2 = ForwardKinematics.Compute(_model, ikResult.Joints, basePose: basePose); - AssertPoseEquals(withBase.TcpPose, state2.TcpPose, because: $"round-trip with base {name}"); - } - - /// Test 1.9 — Intermediate frame 3 with base translation offset. - [Fact] - public void FK_IntermediateFrame3_WithBaseTranslation() - { - var baseT = new Pose3(500, 200, 0, 0, 0, 0); - var withBase = ForwardKinematics.Compute(_model, CFG_A, basePose: baseT); - var withoutBase = ForwardKinematics.Compute(_model, CFG_A); - - withBase.FramePoses[2].X.Should().BeApproximately(withoutBase.FramePoses[2].X + 500, PosTol); - withBase.FramePoses[2].Y.Should().BeApproximately(withoutBase.FramePoses[2].Y + 200, PosTol); - withBase.FramePoses[2].Z.Should().BeApproximately(withoutBase.FramePoses[2].Z, PosTol); - } - - /// Test 1.22 — FK with combined tool and base pose. - [Fact] - public void FK_WithToolAndBase_ShouldProduceCorrectResult() - { - var tool = new Pose3(0, 0, 150, 0, 0, 0); - var baseT = new Pose3(500, 200, 0, 0, 0, 0); - - var combined = ForwardKinematics.Compute(_model, CFG_A, tool, baseT); - var expected = new Pose3(42.8149, 247.3297, -230.0585, -59.2660, -32.0812, -63.8352); - AssertPoseEquals(expected, combined.TcpPose); - - // Verify: combined = FK(CFG-A, tool_only) + base translation - var toolOnly = ForwardKinematics.Compute(_model, CFG_A, tool); - combined.TcpPose.X.Should().BeApproximately(toolOnly.TcpPose.X + 500, PosTol); - combined.TcpPose.Y.Should().BeApproximately(toolOnly.TcpPose.Y + 200, PosTol); - combined.TcpPose.Z.Should().BeApproximately(toolOnly.TcpPose.Z, PosTol); - } - - public static IEnumerable BasePoses() - { - yield return ["Base-T", new Pose3(500, 200, 0, 0, 0, 0)]; - yield return ["Base-R", new Pose3(0, 0, 0, 0, 0, 90)]; - yield return ["Base-TR", new Pose3(300, -100, 50, 15, -10, 45)]; - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core.Tests/ToolModelTests.cs b/csharp/RocketWelder.SDK.Robotics.Core.Tests/ToolModelTests.cs deleted file mode 100644 index 638dd57..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core.Tests/ToolModelTests.cs +++ /dev/null @@ -1,66 +0,0 @@ -namespace RocketWelder.SDK.Robotics.Core.Tests; - -/// TASK-003 — ToolModel hierarchy. -public class ToolModelTests -{ - [Fact] - public void CapsuleToolModel_Should_Carry_Dimensions() - { - var tool = new CapsuleToolModel(120.0, 15.0); - - tool.Length.Should().Be(120.0); - tool.Radius.Should().Be(15.0); - } - - [Fact] - public void CapsuleToolModel_Equality_Should_Compare_By_Value() - { - var a = new CapsuleToolModel(50, 10); - var b = new CapsuleToolModel(50, 10); - var c = new CapsuleToolModel(50, 11); - - a.Should().Be(b); - a.Should().NotBe(c); - } - - [Fact] - public void MeshToolModel_Should_Carry_Opaque_Id() - { - var tool = new MeshToolModel("torch-v3.glb"); - - tool.Id.Should().Be("torch-v3.glb"); - } - - [Fact] - public void MeshToolModel_Equality_Should_Compare_By_Id() - { - var a = new MeshToolModel("torch"); - var b = new MeshToolModel("torch"); - var c = new MeshToolModel("gripper"); - - a.Should().Be(b); - a.Should().NotBe(c); - } - - [Fact] - public void ToolModel_None_Should_Be_A_ToolModel() - { - ToolModel.None.Should().NotBeNull(); - ToolModel.None.Should().BeAssignableTo(); - } - - [Fact] - public void ToolModel_Variants_Should_Be_Pattern_Matchable() - { - ToolModel tool = new CapsuleToolModel(100, 20); - - var result = tool switch - { - CapsuleToolModel c => $"capsule:{c.Length}:{c.Radius}", - MeshToolModel m => $"mesh:{m.Id}", - _ => "none" - }; - - result.Should().Be("capsule:100:20"); - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core.Tests/WeavingRobotTests.cs b/csharp/RocketWelder.SDK.Robotics.Core.Tests/WeavingRobotTests.cs deleted file mode 100644 index 71848e3..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core.Tests/WeavingRobotTests.cs +++ /dev/null @@ -1,141 +0,0 @@ -using NSubstitute; -using RocketWelder.SDK.Devices.Robot; -using static RocketWelder.SDK.Robotics.Core.Tests.TestData; - -namespace RocketWelder.SDK.Robotics.Core.Tests; - -/// -/// Contract + SimulatedRobot tests for the vendor-agnostic weave capability (Epic 091, SDK half). -/// Covers the / enums, the capability cast-probe -/// pattern the run orchestrator relies on, and SimulatedRobot's weave-state behaviour. -/// -public class WeavingRobotTests -{ - private static WeaveProfile SampleProfile( - WeaveInstruction instruction = WeaveInstruction.Swinging, - WeaveSwingType swingType = WeaveSwingType.VerticalLSine) => - new( - SwingType: swingType, - Instruction: instruction, - FrequencyHz: 3.5, - AmplitudeMm: 6, - IncludeDwell: true, - LeftDwellMs: 120, - RightDwellMs: 80, - StationaryWait: false, - AzimuthDeg: 45, - RollDeg: -15); - - /// U-4 — WeaveSwingType int values equal the Fairino weaveType codes; none maps to 7. - [Fact] - public void WeaveSwingType_IntValues_ShouldEqual_FairinoWeaveTypeCodes() - { - // Arrange & Act & Assert — the ints are load-bearing (== weaveType), so pin each one. - ((int)WeaveSwingType.Triangular).Should().Be(0); - ((int)WeaveSwingType.VerticalLTriangular).Should().Be(1); - ((int)WeaveSwingType.CircularCw).Should().Be(2); - ((int)WeaveSwingType.CircularCcw).Should().Be(3); - ((int)WeaveSwingType.Sine).Should().Be(4); - ((int)WeaveSwingType.VerticalLSine).Should().Be(5); - ((int)WeaveSwingType.VerticalTriangle).Should().Be(6); - - var codes = Enum.GetValues().Select(v => (int)v); - codes.Should().BeEquivalentTo(new[] { 0, 1, 2, 3, 4, 5, 6 }); - codes.Should().NotContain(7, because: "Fairino code 7 (second vertical sine) is not exposed by the editor"); - } - - /// U-2 — WeaveInstruction has exactly the four in-scope families; Gradient is out of scope. - [Fact] - public void WeaveInstruction_ShouldHave_FourFamilies_WithoutGradient() - { - var names = Enum.GetNames(); - - names.Should().BeEquivalentTo("Swinging", "Simulation", "TrajectoryWarning", "FixedPoint"); - names.Should().NotContain("Gradient", because: "swing gradient is deferred (ADR-9) and has no member"); - } - - /// SimulatedRobot exposes the weave capability; a robot that lacks it probes to null (cast-probe pattern). - [Fact] - public void WeavingCapability_ShouldBeReachable_ByCastProbe_WhenSupported() - { - // Arrange - using var weaving = new SimulatedRobot(CreateFR5()); - var nonWeaving = Substitute.For(); - - // Act - var weavingProbe = weaving as IWeavingRobot; - var nonWeavingProbe = nonWeaving as IWeavingRobot; - - // Assert — same probe, opposite conditions: one supports weaving, the other does not. - weavingProbe.Should().NotBeNull(); - nonWeavingProbe.Should().BeNull(because: "a robot that does not implement IWeavingRobot cannot weave (FR-9)"); - } - - /// BeginWeave records the active profile and turns weaving on. - [Fact] - public void BeginWeave_ShouldSet_ActiveProfile_AndWeavingOn() - { - // Arrange - using var robot = new SimulatedRobot(CreateFR5()); - var profile = SampleProfile(); - - // Act - var code = ((IWeavingRobot)robot).BeginWeave(profile); - - // Assert - code.Should().Be(0); - robot.IsWeaving.Should().BeTrue(); - robot.ActiveWeaveProfile.Should().Be(profile); - } - - /// EndWeave turns weaving off and clears the active profile. - [Fact] - public void EndWeave_ShouldClear_WeavingState() - { - // Arrange - using var robot = new SimulatedRobot(CreateFR5()); - var weaving = (IWeavingRobot)robot; - weaving.BeginWeave(SampleProfile()); - robot.IsWeaving.Should().BeTrue(because: "the anchor: weaving must be on before EndWeave can turn it off"); - - // Act - var code = weaving.EndWeave(WeaveInstruction.Swinging); - - // Assert - code.Should().Be(0); - robot.IsWeaving.Should().BeFalse(); - robot.ActiveWeaveProfile.Should().BeNull(); - } - - /// Each instruction family round-trips through the active profile carried by BeginWeave. - [Theory] - [InlineData(WeaveInstruction.Swinging)] - [InlineData(WeaveInstruction.Simulation)] - [InlineData(WeaveInstruction.TrajectoryWarning)] - [InlineData(WeaveInstruction.FixedPoint)] - public void BeginWeave_ShouldRoundTrip_EachInstructionFamily(WeaveInstruction family) - { - // Arrange - using var robot = new SimulatedRobot(CreateFR5()); - var profile = SampleProfile(instruction: family); - - // Act - ((IWeavingRobot)robot).BeginWeave(profile); - - // Assert — the profile the robot holds carries the family and every param unchanged. - robot.ActiveWeaveProfile.Should().NotBeNull(); - robot.ActiveWeaveProfile!.Instruction.Should().Be(family); - robot.ActiveWeaveProfile.Should().Be(profile); - } - - /// BeginWeave rejects a null profile. - [Fact] - public void BeginWeave_ShouldThrow_OnNullProfile() - { - using var robot = new SimulatedRobot(CreateFR5()); - - var act = () => ((IWeavingRobot)robot).BeginWeave(null!); - - act.Should().Throw(); - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core/CollisionDetector.cs b/csharp/RocketWelder.SDK.Robotics.Core/CollisionDetector.cs deleted file mode 100644 index 2941f99..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/CollisionDetector.cs +++ /dev/null @@ -1,86 +0,0 @@ -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Pure static collision queries: self-collision (capsule-capsule over non-adjacent link pairs) -/// plus environment collision delegated to . -/// Thread-safe. -/// -public static class CollisionDetector -{ - /// - /// Non-adjacent link pairs checked for self-collision. Adjacent pairs share a joint - /// and would report spurious contacts at the shared origin, so they are excluded. - /// Indices are zero-based (L1 = 0 ... L6 = 5). - /// - public static readonly IReadOnlyList<(int A, int B)> SelfCollisionPairs = new (int, int)[] - { - (0, 2), (0, 3), (0, 4), (0, 5), - (1, 3), (1, 4), (1, 5), - (2, 4), (2, 5), - (3, 5), - }; - - /// - /// Reports every self-collision pair and every environment collision for the given pose. - /// Returns an empty list when no contact. Never returns null. - /// - public static IReadOnlyList CheckCollision( - RobotModel model, - Joints6 joints, - CollisionEnvironment environment) - { - ArgumentNullException.ThrowIfNull(model); - ArgumentNullException.ThrowIfNull(environment); - - var state = ForwardKinematics.Compute(model, joints); - Span> p0s = stackalloc Point3[6]; - Span> p1s = stackalloc Point3[6]; - for (int i = 0; i < 6; i++) - { - p0s[i] = i == 0 ? new Point3(0, 0, 0) : FramePosition(state, i - 1); - p1s[i] = FramePosition(state, i); - } - - var radii = environment.LinkRadii; - var margin = environment.SafetyMargin; - - List? hits = null; - - for (int k = 0; k < SelfCollisionPairs.Count; k++) - { - var (a, b) = SelfCollisionPairs[k]; - var (pa, pb, dist) = CollisionMath.ClosestPointsOnSegments( - p0s[a], p1s[a], p0s[b], p1s[b]); - - var clearance = dist - radii[a] - radii[b]; - if (clearance < margin) - { - hits ??= new List(4); - var midpoint = new Point3( - (pa.X + pb.X) * 0.5, - (pa.Y + pb.Y) * 0.5, - (pa.Z + pb.Z) * 0.5); - hits.Add(new CollisionResult( - $"Link{a + 1}", $"Link{b + 1}", -clearance, midpoint)); - } - } - - var envHits = environment.Source.QueryCollision(model, joints, radii, environment.Tool, margin); - if (envHits.Count == 0) - return hits ?? (IReadOnlyList)Array.Empty(); - - if (hits is null) return envHits; - - for (int i = 0; i < envHits.Count; i++) - hits.Add(envHits[i]); - return hits; - } - - private static Point3 FramePosition(RobotState state, int linkIndex) - { - var pose = state.FramePoses[linkIndex]; - return new Point3(pose.X, pose.Y, pose.Z); - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core/CollisionEnvironment.cs b/csharp/RocketWelder.SDK.Robotics.Core/CollisionEnvironment.cs deleted file mode 100644 index 01d73fe..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/CollisionEnvironment.cs +++ /dev/null @@ -1,50 +0,0 @@ -using System.Collections.Immutable; - -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Immutable configuration aggregating the collision dependencies used by -/// the collision detector and collision-aware robot moves. -/// -public sealed class CollisionEnvironment -{ - /// Environment geometry provider. - public ICollisionSource Source { get; } - - /// Capsule radii (mm) for each of the six robot links. Indexed 0..5. - public ImmutableArray LinkRadii { get; } - - /// Tool geometry attached to the robot flange. - public ToolModel Tool { get; } - - /// Non-negative inflation (mm) applied to every bounding volume during checks. - public double SafetyMargin { get; } - - public CollisionEnvironment( - ICollisionSource source, - IReadOnlyList linkRadii, - ToolModel tool, - double safetyMargin = 0) - { - ArgumentNullException.ThrowIfNull(source); - ArgumentNullException.ThrowIfNull(linkRadii); - ArgumentNullException.ThrowIfNull(tool); - - if (linkRadii.Count != 6) - throw new ArgumentException("LinkRadii must have exactly 6 entries.", nameof(linkRadii)); - - for (int i = 0; i < 6; i++) - { - if (!(linkRadii[i] > 0)) - throw new ArgumentException($"LinkRadii[{i}] must be > 0; got {linkRadii[i]}.", nameof(linkRadii)); - } - - if (safetyMargin < 0) - throw new ArgumentOutOfRangeException(nameof(safetyMargin), safetyMargin, "SafetyMargin must be non-negative."); - - Source = source; - LinkRadii = ImmutableArray.CreateRange(linkRadii); - Tool = tool; - SafetyMargin = safetyMargin; - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core/CollisionMath.cs b/csharp/RocketWelder.SDK.Robotics.Core/CollisionMath.cs deleted file mode 100644 index 6bb16c0..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/CollisionMath.cs +++ /dev/null @@ -1,320 +0,0 @@ -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Closed-form helpers for capsule vs primitive distance queries. -/// All units are millimetres. Thread-safe (pure static). -/// -internal static class CollisionMath -{ - private const double Eps = 1e-10; - - /// - /// Returns (closestPointOnSegment, distanceFromPoint). - /// - public static (Point3 Closest, double Distance) ClosestPointOnSegment( - in Point3 a, in Point3 b, in Point3 p) - { - double abx = (double)b.X - (double)a.X; - double aby = (double)b.Y - (double)a.Y; - double abz = (double)b.Z - (double)a.Z; - - double apx = (double)p.X - (double)a.X; - double apy = (double)p.Y - (double)a.Y; - double apz = (double)p.Z - (double)a.Z; - - double denom = abx * abx + aby * aby + abz * abz; - double t = denom < Eps ? 0.0 : Math.Clamp((apx * abx + apy * aby + apz * abz) / denom, 0.0, 1.0); - - double cx = (double)a.X + abx * t; - double cy = (double)a.Y + aby * t; - double cz = (double)a.Z + abz * t; - - double dx = (double)p.X - cx; - double dy = (double)p.Y - cy; - double dz = (double)p.Z - cz; - - return (new Point3(cx, cy, cz), Math.Sqrt(dx * dx + dy * dy + dz * dz)); - } - - /// - /// Returns the closest points on two line segments and the minimum distance between them. - /// - public static (Point3 ClosestA, Point3 ClosestB, double Distance) ClosestPointsOnSegments( - in Point3 p1, in Point3 p2, in Point3 p3, in Point3 p4) - { - double d1x = (double)p2.X - (double)p1.X, d1y = (double)p2.Y - (double)p1.Y, d1z = (double)p2.Z - (double)p1.Z; - double d2x = (double)p4.X - (double)p3.X, d2y = (double)p4.Y - (double)p3.Y, d2z = (double)p4.Z - (double)p3.Z; - double rx = (double)p1.X - (double)p3.X, ry = (double)p1.Y - (double)p3.Y, rz = (double)p1.Z - (double)p3.Z; - - double a = d1x * d1x + d1y * d1y + d1z * d1z; - double e = d2x * d2x + d2y * d2y + d2z * d2z; - double f = d2x * rx + d2y * ry + d2z * rz; - - double s, t; - if (a <= Eps && e <= Eps) - { - s = t = 0; - } - else if (a <= Eps) - { - s = 0; - t = Math.Clamp(f / e, 0, 1); - } - else - { - double c = d1x * rx + d1y * ry + d1z * rz; - if (e <= Eps) - { - t = 0; - s = Math.Clamp(-c / a, 0, 1); - } - else - { - double b = d1x * d2x + d1y * d2y + d1z * d2z; - double denom = a * e - b * b; - s = denom != 0 ? Math.Clamp((b * f - c * e) / denom, 0, 1) : 0; - t = (b * s + f) / e; - if (t < 0) { t = 0; s = Math.Clamp(-c / a, 0, 1); } - else if (t > 1) { t = 1; s = Math.Clamp((b - c) / a, 0, 1); } - } - } - - var cA = new Point3((double)p1.X + d1x * s, (double)p1.Y + d1y * s, (double)p1.Z + d1z * s); - var cB = new Point3((double)p3.X + d2x * t, (double)p3.Y + d2y * t, (double)p3.Z + d2z * t); - - double dx = (double)cA.X - (double)cB.X; - double dy = (double)cA.Y - (double)cB.Y; - double dz = (double)cA.Z - (double)cB.Z; - - return (cA, cB, Math.Sqrt(dx * dx + dy * dy + dz * dz)); - } - - /// - /// Minimum signed distance from a segment to an axis-aligned box. Returns (closestPointOnBox, signedDistance). - /// Negative when the segment penetrates the box. - /// - /// - /// Closed-form: the SDF along the segment is piecewise smooth with break-points at - /// axis-center crossings (q_i(t)=0) and face crossings (|q_i(t)|=h_i). Within each - /// piece the sign pattern is fixed, so the active-axis SDF is either a constant, a - /// single |linear|−h (piecewise linear) or √Σ(linear)² (smooth, convex). The minimum - /// is either at a piece endpoint (break-point) or at the unique stationary point of - /// the outside-quadratic in the piece. We enumerate all such candidates — at most - /// 2 endpoints + 3 center crossings + 6 face crossings + 1 quadratic stationary per - /// piece. No numerical sampling; no tolerance on minimum location. - /// - public static (Point3 Closest, double Distance) ClosestPointOnSegmentToBox( - in Point3 p0, in Point3 p1, - in Point3 center, double hx, double hy, double hz) - { - double vx = (double)p1.X - (double)p0.X; - double vy = (double)p1.Y - (double)p0.Y; - double vz = (double)p1.Z - (double)p0.Z; - double q0x = (double)p0.X - (double)center.X; - double q0y = (double)p0.Y - (double)center.Y; - double q0z = (double)p0.Z - (double)center.Z; - - Span ts = stackalloc double[48]; - int count = 0; - ts[count++] = 0.0; - ts[count++] = 1.0; - - AddIfIn01(ts, ref count, -q0x, vx); // qx(t)=0 - AddIfIn01(ts, ref count, hx - q0x, vx); // qx(t)=+hx - AddIfIn01(ts, ref count, -hx - q0x, vx); // qx(t)=-hx - AddIfIn01(ts, ref count, -q0y, vy); - AddIfIn01(ts, ref count, hy - q0y, vy); - AddIfIn01(ts, ref count, -hy - q0y, vy); - AddIfIn01(ts, ref count, -q0z, vz); - AddIfIn01(ts, ref count, hz - q0z, vz); - AddIfIn01(ts, ref count, -hz - q0z, vz); - - // Stationary t of the outside quadratic for every non-empty subset of active - // axes, using the sign pattern of each endpoint. For a subset S (active=outside) - // f(t) = Σ_{i∈S}(q_i(t) − s_i·h_i)², t* = Σv_i(s_i h_i − q0_i) / Σv_i². - // Covers the eight sign cells; in-piece candidates are validated by bounds check. - for (int sx = -1; sx <= 1; sx++) - for (int sy = -1; sy <= 1; sy++) - for (int sz = -1; sz <= 1; sz++) - { - if (sx == 0 && sy == 0 && sz == 0) continue; - double num = 0, den = 0; - if (sx != 0) { num += vx * (sx * hx - q0x); den += vx * vx; } - if (sy != 0) { num += vy * (sy * hy - q0y); den += vy * vy; } - if (sz != 0) { num += vz * (sz * hz - q0z); den += vz * vz; } - if (den < Eps) continue; - double tStar = num / den; - if (tStar > 0 && tStar < 1) ts[count++] = tStar; - } - - double best = double.PositiveInfinity; - Point3 bestClosest = p0; - for (int i = 0; i < count; i++) - { - double t = ts[i]; - var p = new Point3( - (double)p0.X + vx * t, - (double)p0.Y + vy * t, - (double)p0.Z + vz * t); - var (closest, d) = SignedDistanceToBox(p, center, hx, hy, hz); - if (d < best) { best = d; bestClosest = closest; } - } - return (bestClosest, best); - } - - private static void AddIfIn01(Span ts, ref int count, double rhs, double v) - { - if (Math.Abs(v) < Eps) return; - double t = rhs / v; - if (t > 0 && t < 1) ts[count++] = t; - } - - /// - /// Signed distance from a point to an axis-aligned box. Returns closest surface point and signed distance. - /// - public static (Point3 Closest, double Distance) SignedDistanceToBox( - in Point3 p, in Point3 center, double hx, double hy, double hz) - { - double dx = (double)p.X - (double)center.X; - double dy = (double)p.Y - (double)center.Y; - double dz = (double)p.Z - (double)center.Z; - - double qx = Math.Abs(dx) - hx; - double qy = Math.Abs(dy) - hy; - double qz = Math.Abs(dz) - hz; - - double outside = Math.Sqrt(Math.Max(qx, 0) * Math.Max(qx, 0) + - Math.Max(qy, 0) * Math.Max(qy, 0) + - Math.Max(qz, 0) * Math.Max(qz, 0)); - double inside = Math.Min(Math.Max(qx, Math.Max(qy, qz)), 0); - double sdf = outside + inside; - - double cx = (double)center.X + Math.Clamp(dx, -hx, hx); - double cy = (double)center.Y + Math.Clamp(dy, -hy, hy); - double cz = (double)center.Z + Math.Clamp(dz, -hz, hz); - - return (new Point3(cx, cy, cz), sdf); - } - - /// - /// Closest-point and signed distance from a segment to an axis-aligned cylinder aligned with world Z. - /// Negative when the segment penetrates the cylinder. - /// - /// - /// Closed-form: radial r²(t) is quadratic in t, axial z(t) linear. SDF break-points - /// along the segment occur at (a) axial face crossings z=±hz, (b) axial center crossing - /// z=0, (c) radial surface crossings r=R (roots of a quadratic in t), (d) radial - /// minimum t* (vertex of the quadratic). Within each piece both sign patterns are - /// fixed, so the single-axis SDF is monotone (axial) or unimodal-convex (radial) — - /// its minimum is at a break-point or at t*. For the rare "both outside" diagonal - /// piece, the SDF² is strictly convex; the radial minimum t* is added as its - /// stationary-candidate proxy (tight: error bounded by the chord-arc gap over that - /// piece, which for robot-scale geometry is below 1e-6 mm). - /// - public static (Point3 Closest, double Distance) ClosestPointOnSegmentToCylinderZ( - in Point3 p0, in Point3 p1, - in Point3 center, double radius, double height) - { - double hz = height * 0.5; - double vx = (double)p1.X - (double)p0.X; - double vy = (double)p1.Y - (double)p0.Y; - double vz = (double)p1.Z - (double)p0.Z; - double q0x = (double)p0.X - (double)center.X; - double q0y = (double)p0.Y - (double)center.Y; - double q0z = (double)p0.Z - (double)center.Z; - - Span ts = stackalloc double[16]; - int count = 0; - ts[count++] = 0.0; - ts[count++] = 1.0; - - AddIfIn01(ts, ref count, hz - q0z, vz); - AddIfIn01(ts, ref count, -hz - q0z, vz); - AddIfIn01(ts, ref count, -q0z, vz); - - // r²(t) = (q0x+t·vx)² + (q0y+t·vy)² = A·t² + B·t + C - double a = vx * vx + vy * vy; - double b = 2 * (q0x * vx + q0y * vy); - double c = q0x * q0x + q0y * q0y; - - if (a > Eps) - { - double tProj = -b / (2 * a); - if (tProj > 0 && tProj < 1) ts[count++] = tProj; - - double disc = b * b - 4 * a * (c - radius * radius); - if (disc >= 0) - { - double sq = Math.Sqrt(disc); - double t1 = (-b - sq) / (2 * a); - double t2 = (-b + sq) / (2 * a); - if (t1 > 0 && t1 < 1) ts[count++] = t1; - if (t2 > 0 && t2 < 1) ts[count++] = t2; - } - } - - double best = double.PositiveInfinity; - Point3 bestClosest = p0; - for (int i = 0; i < count; i++) - { - double t = ts[i]; - double px = (double)p0.X + vx * t; - double py = (double)p0.Y + vy * t; - double pz = (double)p0.Z + vz * t; - - double dx = px - (double)center.X; - double dy = py - (double)center.Y; - double dz = pz - (double)center.Z; - - double r = Math.Sqrt(dx * dx + dy * dy); - double radial = r - radius; - double axial = Math.Abs(dz) - hz; - - double sdf; - if (radial > 0 && axial > 0) - sdf = Math.Sqrt(radial * radial + axial * axial); - else - sdf = Math.Max(radial, axial); - - if (sdf < best) - { - best = sdf; - double ratio = r > Eps ? radius / r : 0; - double cx = (double)center.X + dx * ratio; - double cy = (double)center.Y + dy * ratio; - double cz = (double)center.Z + Math.Clamp(dz, -hz, hz); - bestClosest = new Point3(cx, cy, cz); - } - } - return (bestClosest, best); - } - - /// - /// Signed distance from a point to an oriented plane defined by a point + normal. - /// - public static double SignedDistanceToPlane(in Point3 p, in Point3 pointOnPlane, in Vector3 normal) - { - double nx = (double)normal.X, ny = (double)normal.Y, nz = (double)normal.Z; - double norm = Math.Sqrt(nx * nx + ny * ny + nz * nz); - if (norm < Eps) return double.PositiveInfinity; - nx /= norm; ny /= norm; nz /= norm; - return nx * ((double)p.X - (double)pointOnPlane.X) - + ny * ((double)p.Y - (double)pointOnPlane.Y) - + nz * ((double)p.Z - (double)pointOnPlane.Z); - } - - /// - /// Orthogonal projection of a point onto a plane. - /// - public static Point3 ProjectOntoPlane(in Point3 p, in Point3 pointOnPlane, in Vector3 normal) - { - double nx = (double)normal.X, ny = (double)normal.Y, nz = (double)normal.Z; - double norm = Math.Sqrt(nx * nx + ny * ny + nz * nz); - if (norm < Eps) return p; - nx /= norm; ny /= norm; nz /= norm; - double d = SignedDistanceToPlane(p, pointOnPlane, normal); - return new Point3((double)p.X - nx * d, (double)p.Y - ny * d, (double)p.Z - nz * d); - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core/CollisionPrimitive.cs b/csharp/RocketWelder.SDK.Robotics.Core/CollisionPrimitive.cs deleted file mode 100644 index 908d711..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/CollisionPrimitive.cs +++ /dev/null @@ -1,121 +0,0 @@ -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Immutable environment primitive. Provides a closed-form distance from a line segment -/// (the swept axis of a robot link or tool capsule) to this primitive's surface. -/// -public abstract record CollisionPrimitive(string Name) -{ - /// - /// Minimum signed distance from the segment [p0, p1] to this primitive's surface, in mm. - /// Negative when the segment penetrates the primitive. - /// - public abstract double SignedDistanceToSegment(in Point3 p0, in Point3 p1); - - /// - /// Returns the closest point on the primitive surface to the given segment, used as a contact-point witness. - /// - public abstract Point3 ClosestPointToSegment(in Point3 p0, in Point3 p1); -} - -/// Axis-aligned box centred at with half-extents , , (mm). -public sealed record BoxPrimitive(string Name, Point3 Center, - double HalfSizeX, double HalfSizeY, double HalfSizeZ) : CollisionPrimitive(Name) -{ - /// - public override double SignedDistanceToSegment(in Point3 p0, in Point3 p1) - { - var (closest, d) = CollisionMath.ClosestPointOnSegmentToBox(p0, p1, Center, HalfSizeX, HalfSizeY, HalfSizeZ); - _ = closest; - return d; - } - - /// - public override Point3 ClosestPointToSegment(in Point3 p0, in Point3 p1) - { - var (closest, _) = CollisionMath.ClosestPointOnSegmentToBox(p0, p1, Center, HalfSizeX, HalfSizeY, HalfSizeZ); - return closest; - } -} - -/// Sphere of the given (mm) at . -public sealed record SpherePrimitive(string Name, Point3 Center, double Radius) : CollisionPrimitive(Name) -{ - /// - public override double SignedDistanceToSegment(in Point3 p0, in Point3 p1) - { - var (_, d) = CollisionMath.ClosestPointOnSegment(p0, p1, Center); - return d - Radius; - } - - /// - public override Point3 ClosestPointToSegment(in Point3 p0, in Point3 p1) - { - var (closest, _) = CollisionMath.ClosestPointOnSegment(p0, p1, Center); - return closest; - } -} - -/// Capsule between and with (mm). -public sealed record CapsulePrimitive(string Name, Point3 A, Point3 B, double Radius) : CollisionPrimitive(Name) -{ - /// - public override double SignedDistanceToSegment(in Point3 p0, in Point3 p1) - { - var (_, _, d) = CollisionMath.ClosestPointsOnSegments(p0, p1, A, B); - return d - Radius; - } - - /// - public override Point3 ClosestPointToSegment(in Point3 p0, in Point3 p1) - { - var (_, q, _) = CollisionMath.ClosestPointsOnSegments(p0, p1, A, B); - return q; - } -} - -/// -/// Cylinder aligned with world Z, centered at () with the given and total (mm). -/// -public sealed record CylinderPrimitive(string Name, Point3 Center, double Radius, double Height) : CollisionPrimitive(Name) -{ - /// - public override double SignedDistanceToSegment(in Point3 p0, in Point3 p1) - { - var (_, d) = CollisionMath.ClosestPointOnSegmentToCylinderZ(p0, p1, Center, Radius, Height); - return d; - } - - /// - public override Point3 ClosestPointToSegment(in Point3 p0, in Point3 p1) - { - var (closest, _) = CollisionMath.ClosestPointOnSegmentToCylinderZ(p0, p1, Center, Radius, Height); - return closest; - } -} - -/// -/// Infinite plane defined by a point on the plane () and a unit . -/// Positive side of the plane is where the normal points. -/// -public sealed record PlanePrimitive(string Name, Point3 PointOnPlane, Vector3 Normal) : CollisionPrimitive(Name) -{ - /// - public override double SignedDistanceToSegment(in Point3 p0, in Point3 p1) - { - var d0 = CollisionMath.SignedDistanceToPlane(p0, PointOnPlane, Normal); - var d1 = CollisionMath.SignedDistanceToPlane(p1, PointOnPlane, Normal); - return Math.Min(d0, d1); - } - - /// - public override Point3 ClosestPointToSegment(in Point3 p0, in Point3 p1) - { - var d0 = CollisionMath.SignedDistanceToPlane(p0, PointOnPlane, Normal); - var d1 = CollisionMath.SignedDistanceToPlane(p1, PointOnPlane, Normal); - var closer = d0 <= d1 ? p0 : p1; - return CollisionMath.ProjectOntoPlane(closer, PointOnPlane, Normal); - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core/CollisionResult.cs b/csharp/RocketWelder.SDK.Robotics.Core/CollisionResult.cs deleted file mode 100644 index f1c1f64..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/CollisionResult.cs +++ /dev/null @@ -1,16 +0,0 @@ -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Structured report of a single collision pair. Immutable. -/// -/// Identifier of the first colliding body (e.g., "Link3", "Tool", primitive id). -/// Identifier of the second colliding body. -/// Signed overlap in millimetres. Positive means the shapes interpenetrate. -/// Representative contact point in robot-base coordinates (mm). -public readonly record struct CollisionResult( - string BodyA, - string BodyB, - double PenetrationDepth, - Point3 ContactPoint); diff --git a/csharp/RocketWelder.SDK.Robotics.Core/DhJoint.cs b/csharp/RocketWelder.SDK.Robotics.Core/DhJoint.cs deleted file mode 100644 index e1ad3b0..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/DhJoint.cs +++ /dev/null @@ -1,21 +0,0 @@ -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Modified Denavit-Hartenberg (Craig convention) parameters for a single joint. -/// Transform: T_i = Rot_x(alpha_{i-1}) * Trans_x(a_{i-1}) * Rot_z(theta_i) * Trans_z(d_i) -/// -public readonly record struct DhJoint( - double Alpha, // alpha_{i-1} in radians (twist angle from previous link) - double A, // a_{i-1} in mm (link length from previous link) - double D, // d_i in mm (link offset along Z) - double ThetaOffset // theta offset in radians (added to joint variable) -) -{ - /// - /// Creates a DhJoint from parameters in degrees/mm (the common datasheet format). - /// - public static DhJoint FromDegrees(double alphaDeg, double aMm, double dMm, double thetaOffsetDeg) => - new(alphaDeg * Math.PI / 180.0, aMm, dMm, thetaOffsetDeg * Math.PI / 180.0); -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core/FkValidator.cs b/csharp/RocketWelder.SDK.Robotics.Core/FkValidator.cs deleted file mode 100644 index 096b31a..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/FkValidator.cs +++ /dev/null @@ -1,144 +0,0 @@ -using System.Text.Json; -using System.Text.Json.Serialization; -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// One recorded pairing of joint angles (degrees) and the TCP pose (mm, degrees) the -/// robot controller reported for those joints. Used for FK validation against a -/// . -/// -public sealed record FkValidationRecord(Joints6 Joints, Pose3 TcpPose); - -/// -/// Per-record FK validation outcome. Deltas are expected minus actual (record minus FK); -/// position units are mm, rotation units are degrees, and -/// is the positional Euclidean error in mm. -/// -public sealed record FkValidationResult( - int Index, - double DX, - double DY, - double DZ, - double DRx, - double DRy, - double DRz, - double EuclideanDistance); - -/// -/// Pure static utility for importing, exporting, and validating FK records. -/// Thread-safe. -/// -public static class FkValidator -{ - /// Current supported JSON schema version. - public const string SchemaVersion = "1"; - - /// Serialize a list of records to JSON with schema version. - public static string Export(IReadOnlyList records, JsonSerializerOptions? options = null) - { - ArgumentNullException.ThrowIfNull(records); - var items = new RecordDto[records.Count]; - for (int i = 0; i < records.Count; i++) - items[i] = new RecordDto { Joints = records[i].Joints, TcpPose = records[i].TcpPose }; - var dto = new FkValidationFileDto - { - SchemaVersion = SchemaVersion, - Units = new UnitsDto(), - Records = items - }; - return JsonSerializer.Serialize(dto, options ?? DefaultJson); - } - - /// Parse records from JSON. Throws on unsupported schema version. - public static IReadOnlyList Import(string json, JsonSerializerOptions? options = null) - { - ArgumentNullException.ThrowIfNull(json); - var dto = JsonSerializer.Deserialize(json, options ?? DefaultJson) - ?? throw new InvalidOperationException("FK validation JSON deserialised to null."); - if (!string.Equals(dto.SchemaVersion, SchemaVersion, StringComparison.Ordinal)) - throw new NotSupportedException( - $"Unsupported FK validation schema version '{dto.SchemaVersion}'. Expected '{SchemaVersion}'."); - var records = dto.Records ?? Array.Empty(); - var result = new FkValidationRecord[records.Length]; - for (int i = 0; i < records.Length; i++) - result[i] = new FkValidationRecord(records[i].Joints, records[i].TcpPose); - return result; - } - - /// - /// Compute FK for each record using and compare to the - /// recorded TCP pose. Rotation deltas are wrapped to (-180, 180] degrees so that - /// 359° vs 1° reports a 2° error instead of 358°. - /// - public static IReadOnlyList Validate( - RobotModel model, - IReadOnlyList records, - Pose3? toolTransform = null, - Pose3? basePose = null) - { - ArgumentNullException.ThrowIfNull(model); - ArgumentNullException.ThrowIfNull(records); - - var results = new FkValidationResult[records.Count]; - for (int i = 0; i < records.Count; i++) - { - var record = records[i]; - var state = ForwardKinematics.Compute(model, record.Joints, toolTransform, basePose); - var actual = state.TcpPose; - var expected = record.TcpPose; - - var dx = expected.X - actual.X; - var dy = expected.Y - actual.Y; - var dz = expected.Z - actual.Z; - var dRx = WrapDegrees((double)expected.Rx - (double)actual.Rx); - var dRy = WrapDegrees((double)expected.Ry - (double)actual.Ry); - var dRz = WrapDegrees((double)expected.Rz - (double)actual.Rz); - var euclid = Math.Sqrt(dx * dx + dy * dy + dz * dz); - - results[i] = new FkValidationResult(i, dx, dy, dz, dRx, dRy, dRz, euclid); - } - return results; - } - - private static double WrapDegrees(double d) - { - d = ((d + 180.0) % 360.0 + 360.0) % 360.0 - 180.0; - return d == -180.0 ? 180.0 : d; - } - - internal static readonly JsonSerializerOptions DefaultJson = new() - { - WriteIndented = false, - PropertyNamingPolicy = JsonNamingPolicy.CamelCase - }; - - private sealed class FkValidationFileDto - { - [JsonPropertyName("schemaVersion")] - public string SchemaVersion { get; set; } = "1"; - - [JsonPropertyName("units")] - public UnitsDto Units { get; set; } = new(); - - [JsonPropertyName("records")] - public RecordDto[]? Records { get; set; } - } - - private sealed class UnitsDto - { - [JsonPropertyName("joints")] public string Joints { get; set; } = "degrees"; - [JsonPropertyName("position")] public string Position { get; set; } = "mm"; - [JsonPropertyName("rotation")] public string Rotation { get; set; } = "degrees"; - } - - private sealed class RecordDto - { - [JsonPropertyName("joints")] - public Joints6 Joints { get; set; } - - [JsonPropertyName("tcpPose")] - public Pose3 TcpPose { get; set; } - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core/ForwardKinematics.cs b/csharp/RocketWelder.SDK.Robotics.Core/ForwardKinematics.cs deleted file mode 100644 index 80f7f52..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/ForwardKinematics.cs +++ /dev/null @@ -1,153 +0,0 @@ -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Pure static forward kinematics using Modified DH (Craig convention). -/// Thread-safe, no instance state. -/// -public static class ForwardKinematics -{ - /// - /// Computes the forward kinematics for the given model and joint angles. - /// Returns a RobotState with TCP pose and all intermediate frame poses. - /// - public static RobotState Compute(RobotModel model, Joints6 joints, - Pose3? toolTransform = null, Pose3? basePose = null) - { - var framePoses = new Pose3[7]; // 6 link frames + TCP (which may include tool) - var cumulativeMatrix = Matrix4x4d.Identity; - - // Apply base pose if provided - if (basePose.HasValue && !basePose.Value.IsIdentity) - cumulativeMatrix = PoseToMatrix(basePose.Value); - - for (int i = 0; i < 6; i++) - { - var dh = model.DhChain[i]; - var theta = (double)joints[i] * Math.PI / 180.0 + dh.ThetaOffset; - var jointMatrix = ComputeDhMatrix(dh.Alpha, dh.A, dh.D, theta); - cumulativeMatrix = Multiply(cumulativeMatrix, jointMatrix); - framePoses[i] = MatrixToPose(cumulativeMatrix); - } - - // Frame 6 is the flange frame. TCP = flange * tool (if provided). - if (toolTransform.HasValue && !toolTransform.Value.IsIdentity) - { - var toolMatrix = PoseToMatrix(toolTransform.Value); - var tcpMatrix = Multiply(cumulativeMatrix, toolMatrix); - framePoses[6] = MatrixToPose(tcpMatrix); - } - else - { - framePoses[6] = framePoses[5]; // TCP = flange when no tool - } - - return RobotState.Create(joints, framePoses[6], framePoses); - } - - /// - /// Computes Modified DH (Craig convention) transform matrix for a single joint. - /// T_i = Rot_x(alpha_{i-1}) * Trans_x(a_{i-1}) * Rot_z(theta_i) * Trans_z(d_i) - /// - public static Matrix4x4d ComputeDhMatrix(double alpha, double a, double d, double theta) - { - var ca = Math.Cos(alpha); - var sa = Math.Sin(alpha); - var ct = Math.Cos(theta); - var st = Math.Sin(theta); - - return new Matrix4x4d( - ct, -st, 0, a, - st * ca, ct * ca, -sa, -sa * d, - st * sa, ct * sa, ca, ca * d, - 0, 0, 0, 1 - ); - } - - /// - /// Converts a Pose3 (X,Y,Z in mm, Rx,Ry,Rz in degrees as ZYX Euler) to a 4x4 homogeneous matrix. - /// R = Rz(rz) * Ry(ry) * Rx(rx) — ZYX intrinsic (Tait-Bryan). - /// - public static Matrix4x4d PoseToMatrix(Pose3 pose) - { - var rx = (double)pose.Rx * Math.PI / 180.0; - var ry = (double)pose.Ry * Math.PI / 180.0; - var rz = (double)pose.Rz * Math.PI / 180.0; - - var cx = Math.Cos(rx); var sx = Math.Sin(rx); - var cy = Math.Cos(ry); var sy = Math.Sin(ry); - var cz = Math.Cos(rz); var sz = Math.Sin(rz); - - // R = Rz * Ry * Rx - return new Matrix4x4d( - cz * cy, cz * sy * sx - sz * cx, cz * sy * cx + sz * sx, pose.X, - sz * cy, sz * sy * sx + cz * cx, sz * sy * cx - cz * sx, pose.Y, - -sy, cy * sx, cy * cx, pose.Z, - 0, 0, 0, 1 - ); - } - - /// - /// Extracts Pose3 (X,Y,Z, Rx,Ry,Rz in ZYX Euler degrees) from a 4x4 homogeneous matrix. - /// - public static Pose3 MatrixToPose(Matrix4x4d m) - { - double x = m.M03; - double y = m.M13; - double z = m.M23; - - // ZYX Euler decomposition - double ry, rx, rz; - - var sy = -m.M20; - if (Math.Abs(sy) >= 1.0 - 1e-10) - { - // Gimbal lock - ry = Math.CopySign(Math.PI / 2.0, sy); - rx = 0; - rz = Math.Atan2(-m.M01, m.M11); - } - else - { - ry = Math.Asin(sy); - rx = Math.Atan2(m.M21, m.M22); - rz = Math.Atan2(m.M10, m.M00); - } - - return new Pose3( - x, y, z, - rx * 180.0 / Math.PI, - ry * 180.0 / Math.PI, - rz * 180.0 / Math.PI - ); - } - - /// - /// Multiplies two 4x4 matrices. - /// - public static Matrix4x4d Multiply(Matrix4x4d a, Matrix4x4d b) - { - return new Matrix4x4d( - a.M00 * b.M00 + a.M01 * b.M10 + a.M02 * b.M20 + a.M03 * b.M30, - a.M00 * b.M01 + a.M01 * b.M11 + a.M02 * b.M21 + a.M03 * b.M31, - a.M00 * b.M02 + a.M01 * b.M12 + a.M02 * b.M22 + a.M03 * b.M32, - a.M00 * b.M03 + a.M01 * b.M13 + a.M02 * b.M23 + a.M03 * b.M33, - - a.M10 * b.M00 + a.M11 * b.M10 + a.M12 * b.M20 + a.M13 * b.M30, - a.M10 * b.M01 + a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31, - a.M10 * b.M02 + a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32, - a.M10 * b.M03 + a.M11 * b.M13 + a.M12 * b.M23 + a.M13 * b.M33, - - a.M20 * b.M00 + a.M21 * b.M10 + a.M22 * b.M20 + a.M23 * b.M30, - a.M20 * b.M01 + a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31, - a.M20 * b.M02 + a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32, - a.M20 * b.M03 + a.M21 * b.M13 + a.M22 * b.M23 + a.M23 * b.M33, - - a.M30 * b.M00 + a.M31 * b.M10 + a.M32 * b.M20 + a.M33 * b.M30, - a.M30 * b.M01 + a.M31 * b.M11 + a.M32 * b.M21 + a.M33 * b.M31, - a.M30 * b.M02 + a.M31 * b.M12 + a.M32 * b.M22 + a.M33 * b.M32, - a.M30 * b.M03 + a.M31 * b.M13 + a.M32 * b.M23 + a.M33 * b.M33 - ); - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core/ICollisionSource.cs b/csharp/RocketWelder.SDK.Robotics.Core/ICollisionSource.cs deleted file mode 100644 index 70f9720..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/ICollisionSource.cs +++ /dev/null @@ -1,26 +0,0 @@ -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Pluggable backend supplying environment geometry for collision detection. -/// Headless default: . CAD/CAM sources resolve mesh-id tools. -/// -public interface ICollisionSource -{ - /// - /// Reports all environment and tool-vs-environment collisions for the given robot pose. - /// - /// Robot kinematic model (for FK). - /// Joint configuration in degrees. - /// Per-link capsule radii (mm); 6 entries. - /// Tool geometry attached to the flange. - /// Non-negative inflation in mm applied to every primitive and to the tool/link surface. - /// A list of collision reports. Empty when no contact. - IReadOnlyList QueryCollision( - RobotModel model, - Joints6 joints, - IReadOnlyList linkRadii, - ToolModel tool, - double safetyMargin); -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core/IkResult.cs b/csharp/RocketWelder.SDK.Robotics.Core/IkResult.cs deleted file mode 100644 index 78b359c..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/IkResult.cs +++ /dev/null @@ -1,55 +0,0 @@ -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Reason for IK failure. -/// -public enum IkFailureReason -{ - /// Target is beyond the robot's kinematic reach. - OutOfReach, - - /// A solution exists but violates joint limits. - JointLimitsExceeded, - - /// The Jacobian is ill-conditioned (singularity detected). - Singularity, - - /// The solver did not converge within the maximum number of iterations. - NoConvergence -} - -/// -/// Result of an inverse kinematics computation. Either success with joint angles, or failure with reason. -/// -public readonly record struct IkResult -{ - /// Whether the computation succeeded. - public bool Success { get; } - - /// The joint angles that reach the target pose (only valid when Success is true). - public Joints6 Joints { get; } - - /// The failure reason (only valid when Success is false). - public IkFailureReason? Reason { get; } - - /// Joint limit violations (only populated when Reason is JointLimitsExceeded). - public IReadOnlyList? Violations { get; } - - private IkResult(bool success, Joints6 joints, IkFailureReason? reason, IReadOnlyList? violations) - { - Success = success; - Joints = joints; - Reason = reason; - Violations = violations; - } - - /// Creates a successful IK result. - public static IkResult Succeeded(Joints6 joints) => - new(true, joints, null, null); - - /// Creates a failed IK result. - public static IkResult Failed(IkFailureReason reason, IReadOnlyList? violations = null) => - new(false, Joints6.Zero, reason, violations); -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core/InverseKinematics.cs b/csharp/RocketWelder.SDK.Robotics.Core/InverseKinematics.cs deleted file mode 100644 index a4c04fe..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/InverseKinematics.cs +++ /dev/null @@ -1,407 +0,0 @@ -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Numerical inverse kinematics using Levenberg-Marquardt (damped least-squares). -/// Pure static functions — thread-safe, no instance state. -/// -public static class InverseKinematics -{ - private const int MaxIterations = 500; - private const double PositionTolerance = 1e-6; // mm - private const double RotationTolerance = 1e-7; // radians (~0.006 deg) - private const double JacobianDelta = 1e-6; // radians for numerical differentiation - private const double InitialDamping = 0.01; - private const double MaxReachMultiplier = 1.15; - private const double SingularityCondThreshold = 1e6; - - // Exploratory seeds covering common 6-DOF arm configurations. - private static readonly double[][] ExploratorySeedsRad = - [ - [0, -Math.PI/2, 0, 0, -Math.PI/2, 0], - [0, -Math.PI/3, Math.PI/3, 0, Math.PI/2, 0], - [0, -Math.PI/4, Math.PI/4, 0, -Math.PI/2, 0], - [0, -Math.PI/2, Math.PI/2, 0, 0, 0], - [0, -Math.PI/2, Math.PI/2, 0, -Math.PI/2, 0], - [0, 0, 0, 0, Math.PI/2, 0], - ]; - - /// - /// Computes inverse kinematics for the given target pose, seeded from the given joint configuration. - /// Returns the solution closest to the seed joints. - /// - public static IkResult Compute(RobotModel model, Pose3 target, Joints6 seed, - Pose3? toolTransform = null, Pose3? basePose = null) - { - // Quick reach check - var targetInRobotFrame = target; - if (basePose.HasValue && !basePose.Value.IsIdentity) - { - var baseMatrix = ForwardKinematics.PoseToMatrix(basePose.Value); - var baseInv = baseMatrix.InvertRigid(); - var targetMatrix = ForwardKinematics.PoseToMatrix(target); - var localTarget = ForwardKinematics.Multiply(baseInv, targetMatrix); - targetInRobotFrame = ForwardKinematics.MatrixToPose(localTarget); - } - - var maxReach = ComputeMaxReach(model); - var targetDist = Math.Sqrt( - targetInRobotFrame.X * targetInRobotFrame.X + - targetInRobotFrame.Y * targetInRobotFrame.Y + - targetInRobotFrame.Z * targetInRobotFrame.Z); - - if (targetDist > maxReach * MaxReachMultiplier) - return IkResult.Failed(IkFailureReason.OutOfReach); - - var targetMat = ForwardKinematics.PoseToMatrix(target); - - var seedRad = new double[6]; - for (int i = 0; i < 6; i++) - seedRad[i] = (double)seed[i] * Math.PI / 180.0; - - var (result, singularityOnPath) = SolveLM(model, targetMat, seedRad, toolTransform, basePose); - if (result.Success) - { - // If the converged result is at a singular configuration AND the seed was far, - // report singularity. A far seed reaching a singular solution implies the solver - // traversed through the singular region. - if (singularityOnPath) - { - // Check if the result is geometrically at a singularity. - // For 6-DOF arms: wrist singularity = J5 near 0. - var j5 = Math.Abs((double)result.Joints[4]); - var seedJ5 = Math.Abs((double)seed[4]); - // If result J5 is near zero but seed J5 was far from zero, - // the solver crossed the wrist singularity. - if (j5 < 2.0 && Math.Abs(seedJ5 - j5) > 20.0) - return IkResult.Failed(IkFailureReason.Singularity); - } - return result; - } - - // If the solver encountered singularity on the path from seed to target, - // report singularity — don't attempt exploratory seeds. - if (singularityOnPath) - return IkResult.Failed(IkFailureReason.Singularity); - - // Primary seed failed without singularity (NoConvergence). - // Try exploratory seeds and pick the solution closest to the original seed. - IkResult? bestResult = null; - double bestDist = double.MaxValue; - - foreach (var exploSeed in ExploratorySeedsRad) - { - var (exploResult, exploSingularity) = SolveLM(model, targetMat, exploSeed, toolTransform, basePose); - if (exploResult.Success && !exploSingularity) - { - double dist = 0; - for (int i = 0; i < 6; i++) - { - var diff = (double)exploResult.Joints[i] - (double)seed[i]; - dist += diff * diff; - } - if (dist < bestDist) - { - bestDist = dist; - bestResult = exploResult; - } - } - } - - return bestResult ?? result; - } - - private static (IkResult result, bool singularityOnPath) SolveLM( - RobotModel model, Matrix4x4d targetMatrix, double[] seedRad, - Pose3? toolTransform, Pose3? basePose) - { - var currentJointsRad = (double[])seedRad.Clone(); - var damping = InitialDamping; - var prevError = double.MaxValue; - int stagnationCount = 0; - bool singularityDetected = false; - int singularityHits = 0; - - for (int iter = 0; iter < MaxIterations; iter++) - { - var currentJointsDeg = RadToJoints(currentJointsRad); - var fkState = ForwardKinematics.Compute(model, currentJointsDeg, toolTransform, basePose); - var currentMatrix = ForwardKinematics.PoseToMatrix(fkState.TcpPose); - - var error = ComputeTaskError(currentMatrix, targetMatrix); - var errorNorm = VectorNorm(error); - - if (IsConverged(error)) - { - var resultJoints = NormalizeAngles(currentJointsRad); - var violations = model.ValidateJoints(resultJoints); - if (violations.Count > 0) - return (IkResult.Failed(IkFailureReason.JointLimitsExceeded, violations), singularityDetected); - - // Check if converged result is at a singularity - var resultJacobian = ComputeJacobian(model, currentJointsRad, currentMatrix, toolTransform, basePose); - var resultCond = EstimateConditionNumber(resultJacobian); - if (resultCond > SingularityCondThreshold) - singularityDetected = true; - - return (IkResult.Succeeded(resultJoints), singularityDetected); - } - - var jacobian = ComputeJacobian(model, currentJointsRad, currentMatrix, toolTransform, basePose); - - var condNumber = EstimateConditionNumber(jacobian); - if (condNumber > SingularityCondThreshold) - { - singularityDetected = true; - singularityHits++; - damping = Math.Max(damping, 1.0); - - if (singularityHits > 50) - return (IkResult.Failed(IkFailureReason.Singularity), true); - } - - var delta = SolveDls(jacobian, error, damping); - var maxStep = singularityDetected ? 0.05 : 0.5; - ClampVector(delta, maxStep); - - for (int i = 0; i < 6; i++) - currentJointsRad[i] += delta[i]; - - if (errorNorm < prevError * 0.9999) - { - damping *= 0.5; - if (damping < 1e-8) damping = 1e-8; - stagnationCount = 0; - } - else - { - damping *= 3.0; - if (damping > 1e8) damping = 1e8; - stagnationCount++; - } - - if (stagnationCount > 80) - return (IkResult.Failed(singularityDetected - ? IkFailureReason.Singularity - : IkFailureReason.NoConvergence), singularityDetected); - - prevError = errorNorm; - } - - return (IkResult.Failed(singularityDetected - ? IkFailureReason.Singularity - : IkFailureReason.NoConvergence), singularityDetected); - } - - private static double ComputeMaxReach(RobotModel model) - { - double reach = 0; - foreach (var dh in model.DhChain) - reach += Math.Abs(dh.A) + Math.Abs(dh.D); - return reach; - } - - private static double[] ComputeTaskError(Matrix4x4d current, Matrix4x4d target) - { - var error = new double[6]; - - error[0] = target.M03 - current.M03; - error[1] = target.M13 - current.M13; - error[2] = target.M23 - current.M23; - - // R_error = R_target * R_current^T, then vee(R_error - R_error^T) / 2 - var re21 = target.M20*current.M10 + target.M21*current.M11 + target.M22*current.M12; - var re12 = target.M10*current.M20 + target.M11*current.M21 + target.M12*current.M22; - var re02 = target.M00*current.M20 + target.M01*current.M21 + target.M02*current.M22; - var re20 = target.M20*current.M00 + target.M21*current.M01 + target.M22*current.M02; - var re10 = target.M10*current.M00 + target.M11*current.M01 + target.M12*current.M02; - var re01 = target.M00*current.M10 + target.M01*current.M11 + target.M02*current.M12; - - error[3] = 0.5 * (re21 - re12); - error[4] = 0.5 * (re02 - re20); - error[5] = 0.5 * (re10 - re01); - - return error; - } - - private static bool IsConverged(double[] error) - { - var posErr = Math.Sqrt(error[0] * error[0] + error[1] * error[1] + error[2] * error[2]); - var rotErr = Math.Sqrt(error[3] * error[3] + error[4] * error[4] + error[5] * error[5]); - return posErr < PositionTolerance && rotErr < RotationTolerance; - } - - private static double VectorNorm(double[] v) - { - double sum = 0; - for (int i = 0; i < v.Length; i++) sum += v[i] * v[i]; - return Math.Sqrt(sum); - } - - private static double[,] ComputeJacobian(RobotModel model, double[] jointsRad, - Matrix4x4d currentMatrix, Pose3? toolTransform, Pose3? basePose) - { - var jacobian = new double[6, 6]; - for (int j = 0; j < 6; j++) - { - var perturbedRad = (double[])jointsRad.Clone(); - perturbedRad[j] += JacobianDelta; - var perturbedJointsDeg = RadToJoints(perturbedRad); - var perturbedState = ForwardKinematics.Compute(model, perturbedJointsDeg, toolTransform, basePose); - var pm = ForwardKinematics.PoseToMatrix(perturbedState.TcpPose); - - jacobian[0, j] = (pm.M03 - currentMatrix.M03) / JacobianDelta; - jacobian[1, j] = (pm.M13 - currentMatrix.M13) / JacobianDelta; - jacobian[2, j] = (pm.M23 - currentMatrix.M23) / JacobianDelta; - - var rd21 = pm.M20*currentMatrix.M10 + pm.M21*currentMatrix.M11 + pm.M22*currentMatrix.M12; - var rd12 = pm.M10*currentMatrix.M20 + pm.M11*currentMatrix.M21 + pm.M12*currentMatrix.M22; - var rd02 = pm.M00*currentMatrix.M20 + pm.M01*currentMatrix.M21 + pm.M02*currentMatrix.M22; - var rd20 = pm.M20*currentMatrix.M00 + pm.M21*currentMatrix.M01 + pm.M22*currentMatrix.M02; - var rd10 = pm.M10*currentMatrix.M00 + pm.M11*currentMatrix.M01 + pm.M12*currentMatrix.M02; - var rd01 = pm.M00*currentMatrix.M10 + pm.M01*currentMatrix.M11 + pm.M02*currentMatrix.M12; - - jacobian[3, j] = 0.5 * (rd21 - rd12) / JacobianDelta; - jacobian[4, j] = 0.5 * (rd02 - rd20) / JacobianDelta; - jacobian[5, j] = 0.5 * (rd10 - rd01) / JacobianDelta; - } - return jacobian; - } - - private static double EstimateConditionNumber(double[,] jacobian) - { - var jtj = new double[6, 6]; - for (int i = 0; i < 6; i++) - for (int j = 0; j < 6; j++) - { - double sum = 0; - for (int k = 0; k < 6; k++) sum += jacobian[k, i] * jacobian[k, j]; - jtj[i, j] = sum; - } - - double trace = 0; - for (int i = 0; i < 6; i++) trace += jtj[i, i]; - if (trace < 1e-15) return double.MaxValue; - - var v = new double[] { 1, 0, 0, 0, 0, 0 }; - double maxEig = 0; - for (int iter = 0; iter < 30; iter++) - { - var w = MatVec6(jtj, v); - maxEig = VectorNorm(w); - if (maxEig < 1e-15) return double.MaxValue; - for (int i = 0; i < 6; i++) v[i] = w[i] / maxEig; - } - var minEig = EstimateMinEigenvalue(jtj); - if (minEig < 1e-15) return double.MaxValue; - return Math.Sqrt(maxEig / minEig); - } - - private static double EstimateMinEigenvalue(double[,] m) - { - var v = new double[] { 0, 0, 0, 0, 0, 1 }; - double eigenvalue = 0; - for (int iter = 0; iter < 30; iter++) - { - var w = SolveLinear6x6(m, v); - if (w == null) return 0; - var norm = VectorNorm(w); - if (norm < 1e-15) return 0; - eigenvalue = 1.0 / norm; - for (int i = 0; i < 6; i++) v[i] = w[i] / norm; - } - return eigenvalue; - } - - private static double[] MatVec6(double[,] m, double[] v) - { - var r = new double[6]; - for (int i = 0; i < 6; i++) - { - double sum = 0; - for (int j = 0; j < 6; j++) sum += m[i, j] * v[j]; - r[i] = sum; - } - return r; - } - - private static double[] SolveDls(double[,] jacobian, double[] error, double lambda) - { - var jte = new double[6]; - for (int i = 0; i < 6; i++) - { - double sum = 0; - for (int k = 0; k < 6; k++) sum += jacobian[k, i] * error[k]; - jte[i] = sum; - } - var jtjLambda = new double[6, 6]; - for (int i = 0; i < 6; i++) - for (int j = 0; j < 6; j++) - { - double sum = 0; - for (int k = 0; k < 6; k++) sum += jacobian[k, i] * jacobian[k, j]; - jtjLambda[i, j] = sum + (i == j ? lambda : 0); - } - return SolveLinear6x6(jtjLambda, jte) ?? new double[6]; - } - - private static double[]? SolveLinear6x6(double[,] a, double[] b) - { - var n = 6; - var aug = new double[n, n + 1]; - for (int i = 0; i < n; i++) - { - for (int j = 0; j < n; j++) aug[i, j] = a[i, j]; - aug[i, n] = b[i]; - } - for (int col = 0; col < n; col++) - { - int maxRow = col; - double maxVal = Math.Abs(aug[col, col]); - for (int row = col + 1; row < n; row++) - if (Math.Abs(aug[row, col]) > maxVal) - { maxVal = Math.Abs(aug[row, col]); maxRow = row; } - if (maxVal < 1e-15) return null; - if (maxRow != col) - for (int j = 0; j <= n; j++) - (aug[col, j], aug[maxRow, j]) = (aug[maxRow, j], aug[col, j]); - for (int row = col + 1; row < n; row++) - { - var factor = aug[row, col] / aug[col, col]; - for (int j = col; j <= n; j++) aug[row, j] -= factor * aug[col, j]; - } - } - var x = new double[n]; - for (int i = n - 1; i >= 0; i--) - { - x[i] = aug[i, n]; - for (int j = i + 1; j < n; j++) x[i] -= aug[i, j] * x[j]; - x[i] /= aug[i, i]; - } - return x; - } - - private static void ClampVector(double[] v, double maxMag) - { - for (int i = 0; i < v.Length; i++) - v[i] = Math.Clamp(v[i], -maxMag, maxMag); - } - - private static Joints6 RadToJoints(double[] rads) => - new(rads[0] * 180.0 / Math.PI, rads[1] * 180.0 / Math.PI, - rads[2] * 180.0 / Math.PI, rads[3] * 180.0 / Math.PI, - rads[4] * 180.0 / Math.PI, rads[5] * 180.0 / Math.PI); - - private static Joints6 NormalizeAngles(double[] rads) - { - var deg = new double[6]; - for (int i = 0; i < 6; i++) - { - deg[i] = rads[i] * 180.0 / Math.PI; - while (deg[i] > 180.0) deg[i] -= 360.0; - while (deg[i] <= -180.0) deg[i] += 360.0; - } - return new Joints6(deg[0], deg[1], deg[2], deg[3], deg[4], deg[5]); - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core/JointLimit.cs b/csharp/RocketWelder.SDK.Robotics.Core/JointLimit.cs deleted file mode 100644 index 3a1b9a6..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/JointLimit.cs +++ /dev/null @@ -1,23 +0,0 @@ -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Joint angle limits in degrees. -/// -public readonly record struct JointLimit(double MinDeg, double MaxDeg) -{ - /// - /// Returns true if the given angle (degrees) is within limits (inclusive). - /// - public bool Contains(double angleDeg) => angleDeg >= MinDeg && angleDeg <= MaxDeg; - - /// - /// Returns the overshoot amount in degrees (0 if within limits). - /// Positive means above max, negative means below min. - /// - public double Overshoot(double angleDeg) - { - if (angleDeg > MaxDeg) return angleDeg - MaxDeg; - if (angleDeg < MinDeg) return angleDeg - MinDeg; - return 0; - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core/JointLimitViolation.cs b/csharp/RocketWelder.SDK.Robotics.Core/JointLimitViolation.cs deleted file mode 100644 index 3af0251..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/JointLimitViolation.cs +++ /dev/null @@ -1,11 +0,0 @@ -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Details of a single joint limit violation. -/// -public readonly record struct JointLimitViolation( - int JointIndex, - double LimitDeg, - double RequestedDeg, - double OvershootDeg -); diff --git a/csharp/RocketWelder.SDK.Robotics.Core/Matrix4x4d.cs b/csharp/RocketWelder.SDK.Robotics.Core/Matrix4x4d.cs deleted file mode 100644 index d1b3d61..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/Matrix4x4d.cs +++ /dev/null @@ -1,54 +0,0 @@ -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// 4x4 homogeneous transformation matrix using double precision. -/// Row-major layout: M[row][col]. -/// -public readonly record struct Matrix4x4d( - double M00, double M01, double M02, double M03, - double M10, double M11, double M12, double M13, - double M20, double M21, double M22, double M23, - double M30, double M31, double M32, double M33 -) -{ - public static readonly Matrix4x4d Identity = new( - 1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1 - ); - - /// - /// Returns the transpose of this matrix. - /// - public Matrix4x4d Transpose() => new( - M00, M10, M20, M30, - M01, M11, M21, M31, - M02, M12, M22, M32, - M03, M13, M23, M33 - ); - - /// - /// Inverts a homogeneous transformation matrix [R|t; 0 0 0 1]. - /// Inverse is [R^T | -R^T*t; 0 0 0 1]. - /// - public Matrix4x4d InvertRigid() - { - // R^T - var r00 = M00; var r01 = M10; var r02 = M20; - var r10 = M01; var r11 = M11; var r12 = M21; - var r20 = M02; var r21 = M12; var r22 = M22; - - // -R^T * t - var tx = -(r00 * M03 + r01 * M13 + r02 * M23); - var ty = -(r10 * M03 + r11 * M13 + r12 * M23); - var tz = -(r20 * M03 + r21 * M13 + r22 * M23); - - return new Matrix4x4d( - r00, r01, r02, tx, - r10, r11, r12, ty, - r20, r21, r22, tz, - 0, 0, 0, 1 - ); - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core/MoveFailureReason.cs b/csharp/RocketWelder.SDK.Robotics.Core/MoveFailureReason.cs deleted file mode 100644 index c7269aa..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/MoveFailureReason.cs +++ /dev/null @@ -1,35 +0,0 @@ -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Reason why a move was rejected. Superset of plus collision. -/// -public enum MoveFailureReason -{ - /// Target is beyond the robot's kinematic reach. - OutOfReach, - - /// A solution exists but violates joint limits. - JointLimitsExceeded, - - /// The Jacobian is ill-conditioned (singularity detected). - Singularity, - - /// The solver did not converge within the maximum number of iterations. - NoConvergence, - - /// The target configuration collides with self or the environment. - Collision -} - -/// Conversion helpers between and . -public static class MoveFailureReasonExtensions -{ - public static MoveFailureReason ToMoveReason(this IkFailureReason reason) => reason switch - { - IkFailureReason.OutOfReach => MoveFailureReason.OutOfReach, - IkFailureReason.JointLimitsExceeded => MoveFailureReason.JointLimitsExceeded, - IkFailureReason.Singularity => MoveFailureReason.Singularity, - IkFailureReason.NoConvergence => MoveFailureReason.NoConvergence, - _ => throw new ArgumentOutOfRangeException(nameof(reason), reason, "Unknown IkFailureReason") - }; -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core/MoveResult.cs b/csharp/RocketWelder.SDK.Robotics.Core/MoveResult.cs deleted file mode 100644 index 39ddc76..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/MoveResult.cs +++ /dev/null @@ -1,41 +0,0 @@ -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Result of a TryMoveLin or TryMoveJoint operation. -/// -public readonly record struct MoveResult -{ - /// Whether the move succeeded. - public bool Success { get; } - - /// Failure reason (null on success). - public MoveFailureReason? Reason { get; } - - /// Joint limit violations (populated when Reason is JointLimitsExceeded). - public IReadOnlyList? Violations { get; } - - /// Collision details (populated when Reason is Collision). - public CollisionResult? Collision { get; } - - private MoveResult(bool success, MoveFailureReason? reason, - IReadOnlyList? violations, CollisionResult? collision) - { - Success = success; - Reason = reason; - Violations = violations; - Collision = collision; - } - - /// Creates a successful move result. - public static MoveResult Succeeded() => new(true, null, null, null); - - /// Creates a failed move result. - public static MoveResult Failed(MoveFailureReason reason, - IReadOnlyList? violations = null, - CollisionResult? collision = null) => - new(false, reason, violations, collision); - - /// Creates a collision-rejected move result. - public static MoveResult RejectedByCollision(CollisionResult collision) => - new(false, MoveFailureReason.Collision, null, collision); -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core/PrimitiveCollisionSource.cs b/csharp/RocketWelder.SDK.Robotics.Core/PrimitiveCollisionSource.cs deleted file mode 100644 index 7834028..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/PrimitiveCollisionSource.cs +++ /dev/null @@ -1,113 +0,0 @@ -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Immutable backed by a fixed list of s. -/// Thread-safe; one instance can be shared across simulators. -/// -public sealed class PrimitiveCollisionSource : ICollisionSource -{ - private readonly IReadOnlyList _primitives; - - /// Creates a source backed by the given primitives. The list is captured by reference; callers must not mutate it. - public PrimitiveCollisionSource(IReadOnlyList primitives) - { - ArgumentNullException.ThrowIfNull(primitives); - _primitives = primitives; - } - - /// Convenience: create from a params array. - public PrimitiveCollisionSource(params CollisionPrimitive[] primitives) - : this((IReadOnlyList)primitives) { } - - /// The underlying primitive list. - public IReadOnlyList Primitives => _primitives; - - /// - public IReadOnlyList QueryCollision( - RobotModel model, - Joints6 joints, - IReadOnlyList linkRadii, - ToolModel tool, - double safetyMargin) - { - ArgumentNullException.ThrowIfNull(model); - ArgumentNullException.ThrowIfNull(linkRadii); - ArgumentNullException.ThrowIfNull(tool); - if (linkRadii.Count != 6) - throw new ArgumentException("Link radii must have exactly 6 entries.", nameof(linkRadii)); - if (safetyMargin < 0) - throw new ArgumentOutOfRangeException(nameof(safetyMargin), "Safety margin must be non-negative."); - - var state = ForwardKinematics.Compute(model, joints); - var results = new List(); - - // Link capsules: segment between consecutive frame origins, with configured radius. - for (int link = 0; link < 6; link++) - { - var p0 = link == 0 ? new Point3(0, 0, 0) : FramePosition(state, link - 1); - var p1 = FramePosition(state, link); - double r = linkRadii[link]; - var linkId = $"Link{link + 1}"; - - foreach (var prim in _primitives) - { - var sdf = prim.SignedDistanceToSegment(p0, p1); - var clearance = sdf - r; - if (clearance < safetyMargin) - { - var contact = prim.ClosestPointToSegment(p0, p1); - results.Add(new CollisionResult(linkId, prim.Name, -clearance, contact)); - } - } - } - - // Tool capsule: from flange origin along the flange +Z axis (only when a CapsuleToolModel is attached). - if (tool is CapsuleToolModel capsule) - { - var flangePos = FramePosition(state, 5); - var tipPos = ExtrudeAlongFlangeZ(state, capsule.Length); - foreach (var prim in _primitives) - { - var sdf = prim.SignedDistanceToSegment(flangePos, tipPos); - var clearance = sdf - capsule.Radius; - if (clearance < safetyMargin) - { - var contact = prim.ClosestPointToSegment(flangePos, tipPos); - results.Add(new CollisionResult("Tool", prim.Name, -clearance, contact)); - } - } - } - - return results; - } - - private static Point3 FramePosition(RobotState state, int linkIndex) - { - var pose = state.FramePoses[linkIndex]; - return new Point3(pose.X, pose.Y, pose.Z); - } - - private static Point3 ExtrudeAlongFlangeZ(RobotState state, double length) - { - // Use frame 6 (flange) pose; rotate +Z by its ZYX euler to get the flange Z axis direction in base frame. - var flange = state.FramePoses[5]; - double rx = (double)flange.Rx * Math.PI / 180.0; - double ry = (double)flange.Ry * Math.PI / 180.0; - double rz = (double)flange.Rz * Math.PI / 180.0; - double cx = Math.Cos(rx), sx = Math.Sin(rx); - double cy = Math.Cos(ry), sy = Math.Sin(ry); - double cz = Math.Cos(rz), sz = Math.Sin(rz); - - // R * (0,0,1) column = (cz*sy*cx + sz*sx, sz*sy*cx - cz*sx, cy*cx) - double zx = cz * sy * cx + sz * sx; - double zy = sz * sy * cx - cz * sx; - double zz = cy * cx; - - return new Point3( - (double)flange.X + zx * length, - (double)flange.Y + zy * length, - (double)flange.Z + zz * length); - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core/ProgramStep.cs b/csharp/RocketWelder.SDK.Robotics.Core/ProgramStep.cs deleted file mode 100644 index 4c43685..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/ProgramStep.cs +++ /dev/null @@ -1,19 +0,0 @@ -using System.Text.Json.Serialization; -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Base type for a single step in a . Immutable; step variants are records. -/// JSON discriminator: type. -/// -[JsonPolymorphic(TypeDiscriminatorPropertyName = "type")] -[JsonDerivedType(typeof(MoveJointStep), "moveJoint")] -[JsonDerivedType(typeof(MoveLinStep), "moveLin")] -public abstract record ProgramStep; - -/// Move to the given joint configuration (degrees). -public sealed record MoveJointStep(Joints6 Target) : ProgramStep; - -/// Move the TCP linearly to the given pose (mm, degrees). -public sealed record MoveLinStep(Pose3 Target) : ProgramStep; diff --git a/csharp/RocketWelder.SDK.Robotics.Core/RobotModel.cs b/csharp/RocketWelder.SDK.Robotics.Core/RobotModel.cs deleted file mode 100644 index b453e35..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/RobotModel.cs +++ /dev/null @@ -1,63 +0,0 @@ -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Immutable definition of a 6-DOF articulated robot arm. -/// Created once, shared across SimulatedRobot instances. Thread-safe. -/// -public sealed class RobotModel -{ - /// Name of this robot model (e.g., "Fairino FR5"). - public string Name { get; } - - /// Modified DH (Craig convention) parameters for all 6 joints. - public IReadOnlyList DhChain { get; } - - /// Joint angle limits for all 6 joints. - public IReadOnlyList JointLimits { get; } - - /// Home position (all joints at their default angles). - public Joints6 HomePosition { get; } - - public RobotModel( - string name, - IReadOnlyList dhChain, - IReadOnlyList jointLimits, - Joints6 homePosition) - { - ArgumentNullException.ThrowIfNull(name); - ArgumentNullException.ThrowIfNull(dhChain); - ArgumentNullException.ThrowIfNull(jointLimits); - - if (dhChain.Count != 6) - throw new ArgumentException("DH chain must have exactly 6 joints.", nameof(dhChain)); - if (jointLimits.Count != 6) - throw new ArgumentException("Joint limits must have exactly 6 entries.", nameof(jointLimits)); - - Name = name; - DhChain = dhChain; - JointLimits = jointLimits; - HomePosition = homePosition; - } - - /// - /// Validates joint angles against limits. Returns an empty list if all joints are within limits. - /// - public IReadOnlyList ValidateJoints(Joints6 joints) - { - var violations = new List(); - for (int i = 0; i < 6; i++) - { - var angleDeg = (double)joints[i]; - var limit = JointLimits[i]; - var overshoot = limit.Overshoot(angleDeg); - if (overshoot != 0) - { - var limitValue = overshoot > 0 ? limit.MaxDeg : limit.MinDeg; - violations.Add(new JointLimitViolation(i, limitValue, angleDeg, Math.Abs(overshoot))); - } - } - return violations; - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core/RobotPresets.cs b/csharp/RocketWelder.SDK.Robotics.Core/RobotPresets.cs deleted file mode 100644 index 3f93217..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/RobotPresets.cs +++ /dev/null @@ -1,64 +0,0 @@ -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Factory for known robot models. Convenience presets — the library has no dependency on any robot brand. -/// DH values derived from FAIR-INNOVATION/frcobot_ros2 URDFs (Modified DH, Craig convention). -/// See docs/epics/epic-021-robot-simulator/fairino-preset-reference.md for provenance. -/// -public static class RobotPresets -{ - // Shared joint limits for all Fairino v6 6-DOF arms (FR3/FR5/FR10/FR16/FR20/FR30). - private static readonly JointLimit[] FairinoV6Limits = - { - new(-178, 178), - new(-265, 85), - new(-162, 162), - new(-265, 85), - new(-178, 178), - new(-360, 360), - }; - - /// - /// Builds a Fairino v6 6-DOF RobotModel from the per-model link-length parameters. - /// d4 is always 0 for v6 arms (j3/j4 axes parallel under MDH Craig). - /// - private static RobotModel FairinoV6(string name, double d1, double a2, double a3, double d5, double d6) - { - var chain = new DhJoint[] - { - DhJoint.FromDegrees( 0, 0, d1, 0), // Joint 1 - DhJoint.FromDegrees(-90, 0, 0, 0), // Joint 2 - DhJoint.FromDegrees( 0, -a2, 0, 0), // Joint 3 - DhJoint.FromDegrees( 0, -a3, 0, 0), // Joint 4 (d4 = 0) - DhJoint.FromDegrees(-90, 0, d5, 0), // Joint 5 - DhJoint.FromDegrees( 90, 0, d6, 0), // Joint 6 - }; - return new RobotModel(name, chain, FairinoV6Limits, Joints6.Zero); - } - - /// Fairino FR3 6-DOF collaborative robot. See fairino-preset-reference.md. - public static RobotModel FairinoFR3() => - FairinoV6("Fairino FR3", d1: 140, a2: 280, a3: 240.01, d5: 102, d6: 102); - - /// Fairino FR5 6-DOF collaborative robot. See fairino-preset-reference.md. - public static RobotModel FairinoFR5() => - FairinoV6("Fairino FR5", d1: 152, a2: 425, a3: 395.01, d5: 102.1, d6: 102); - - /// Fairino FR10 6-DOF collaborative robot. See fairino-preset-reference.md. - public static RobotModel FairinoFR10() => - FairinoV6("Fairino FR10", d1: 180, a2: 700, a3: 586, d5: 159, d6: 114); - - /// Fairino FR16 6-DOF collaborative robot. See fairino-preset-reference.md. - public static RobotModel FairinoFR16() => - FairinoV6("Fairino FR16", d1: 180, a2: 520, a3: 400, d5: 159, d6: 114); - - /// Fairino FR20 6-DOF collaborative robot. See fairino-preset-reference.md. - public static RobotModel FairinoFR20() => - FairinoV6("Fairino FR20", d1: 215, a2: 1000, a3: 716, d5: 166.01, d6: 138); - - /// Fairino FR30 6-DOF collaborative robot. See fairino-preset-reference.md. - public static RobotModel FairinoFR30() => - FairinoV6("Fairino FR30", d1: 215, a2: 700, a3: 536, d5: 166.01, d6: 138); -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core/RobotProgram.cs b/csharp/RocketWelder.SDK.Robotics.Core/RobotProgram.cs deleted file mode 100644 index 4de9922..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/RobotProgram.cs +++ /dev/null @@ -1,79 +0,0 @@ -using System.Text.Json; -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Mutable ordered sequence of s. Not thread-safe: one instance per agent. -/// -public sealed class RobotProgram -{ - private readonly List _steps = new(); - - /// All steps in order. - public IReadOnlyList Steps => _steps; - - /// Step count. - public int Count => _steps.Count; - - /// Append a joint-space move. - public void AddMoveJoint(Joints6 target) => _steps.Add(new MoveJointStep(target)); - - /// Append a linear-pose move. - public void AddMoveLin(Pose3 target) => _steps.Add(new MoveLinStep(target)); - - /// Append a prebuilt step. - public void Add(ProgramStep step) - { - ArgumentNullException.ThrowIfNull(step); - _steps.Add(step); - } - - /// Insert a step at the given index. - public void InsertAt(int index, ProgramStep step) - { - ArgumentNullException.ThrowIfNull(step); - if (index < 0 || index > _steps.Count) - throw new ArgumentOutOfRangeException(nameof(index)); - _steps.Insert(index, step); - } - - /// Remove the step at the given index. - public void RemoveAt(int index) - { - if (index < 0 || index >= _steps.Count) - throw new ArgumentOutOfRangeException(nameof(index)); - _steps.RemoveAt(index); - } - - /// Replace the step at the given index. - public void ReplaceAt(int index, ProgramStep step) - { - ArgumentNullException.ThrowIfNull(step); - if (index < 0 || index >= _steps.Count) - throw new ArgumentOutOfRangeException(nameof(index)); - _steps[index] = step; - } - - /// Serialize this program to JSON. - public string ToJson(JsonSerializerOptions? options = null) => - JsonSerializer.Serialize(_steps, options ?? DefaultJson); - - /// Parse a program from JSON. - public static RobotProgram FromJson(string json, JsonSerializerOptions? options = null) - { - ArgumentNullException.ThrowIfNull(json); - var steps = JsonSerializer.Deserialize>(json, options ?? DefaultJson) - ?? throw new InvalidOperationException("Program JSON deserialised to null."); - var program = new RobotProgram(); - foreach (var step in steps) - program.Add(step); - return program; - } - - internal static readonly JsonSerializerOptions DefaultJson = new() - { - WriteIndented = false, - PropertyNamingPolicy = JsonNamingPolicy.CamelCase - }; -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core/RobotState.cs b/csharp/RocketWelder.SDK.Robotics.Core/RobotState.cs deleted file mode 100644 index 30da65c..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/RobotState.cs +++ /dev/null @@ -1,21 +0,0 @@ -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Immutable snapshot of a robot's kinematic state. Thread-safe, pure data. -/// Created by FK computation. -/// -public sealed record RobotState( - Joints6 Joints, - Pose3 TcpPose, - IReadOnlyList> FramePoses, - DateTimeOffset Timestamp -) -{ - /// - /// Creates a RobotState with the current timestamp. - /// - public static RobotState Create(Joints6 joints, Pose3 tcpPose, IReadOnlyList> framePoses) => - new(joints, tcpPose, framePoses is Pose3[] arr ? Array.AsReadOnly(arr) : framePoses, DateTimeOffset.UtcNow); -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core/RocketWelder.SDK.Robotics.Core.csproj b/csharp/RocketWelder.SDK.Robotics.Core/RocketWelder.SDK.Robotics.Core.csproj deleted file mode 100644 index e730a16..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/RocketWelder.SDK.Robotics.Core.csproj +++ /dev/null @@ -1,32 +0,0 @@ - - - - net10.0 - enable - enable - true - $(NoWarn);CS1591 - RocketWelder.SDK.Robotics.Core - ModelingEvolution - ModelingEvolution - Headless kinematic simulation engine: DH forward/inverse kinematics, SimulatedRobot (IRobot drop-in replacement), robot presets. - rocketwelder;robotics;kinematics;simulation;sdk - MIT - https://github.com/modelingevolution/rocket-welder-sdk - - - - - - - - - - - - - - - - - diff --git a/csharp/RocketWelder.SDK.Robotics.Core/SimulatedRobot.cs b/csharp/RocketWelder.SDK.Robotics.Core/SimulatedRobot.cs deleted file mode 100644 index 1be512f..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/SimulatedRobot.cs +++ /dev/null @@ -1,437 +0,0 @@ -using System.Reactive.Linq; -using System.Reactive.Subjects; -using ModelingEvolution.Drawing; -using RocketWelder.SDK.Abstractions; -using RocketWelder.SDK.Devices.Robot; - -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Mutable IRobot implementation backed by forward/inverse kinematics. -/// Each agent/user needs their own instance. Not thread-safe. -/// MoveLin/MoveJoint update internal state instantaneously (no simulated motion time). -/// -public sealed class SimulatedRobot : IRobot, IWeavingRobot -{ - private const double MaxStepDegrees = 5.0; - - private readonly RobotModel _model; - private readonly Pose3? _toolTransform; - private readonly Pose3? _basePose; - private readonly CollisionEnvironment? _collisionEnv; - private readonly Subject> _poseSubject = new(); - - private RobotState _currentState; - private TeachingPointSet? _teachingPoints; - private bool _isConnected; - private bool _isDisposed; - private Uri _address = new("sim://localhost"); - private bool _jointMode; - private WeaveProfile? _activeWeaveProfile; - - /// - /// Creates a SimulatedRobot at the model's home position. When - /// is supplied, every move checks the target configuration for self and environment collisions - /// before committing. - /// - /// Stable robot identity (Epic 029, FR-1.1). Defaults to a fresh - /// DeviceId.New("simulated") so each simulator gets a unique catalogue scope. - public SimulatedRobot( - RobotModel model, - Pose3? toolTransform = null, - Pose3? basePose = null, - CollisionEnvironment? environment = null, - DeviceId? id = null) - { - _model = model ?? throw new ArgumentNullException(nameof(model)); - _toolTransform = toolTransform; - _basePose = basePose; - _collisionEnv = environment; - _currentState = ForwardKinematics.Compute(model, model.HomePosition, toolTransform, basePose); - Id = id ?? DeviceId.New("simulated"); - } - - /// The robot model backing this simulator. - public RobotModel Model => _model; - - #region IRobot Implementation - - /// - public DeviceId Id { get; } - - public Uri Address - { - get => _address; - set => _address = value; - } - - public bool IsConnected => _isConnected; - - public Task IsAvailableAsync(CancellationToken ct = default) - { - ThrowIfDisposed(); - return Task.FromResult(_isConnected); - } - - public int Connect() - { - ThrowIfDisposed(); - if (!_isConnected) - { - _isConnected = true; - Connected?.Invoke(this, EventArgs.Empty); - } - return 0; - } - - public void Disconnect() - { - ThrowIfDisposed(); - if (_isConnected) - { - _isConnected = false; - Disconnected?.Invoke(this, EventArgs.Empty); - } - } - - public event EventHandler? Connected; - public event EventHandler? Disconnected; - - public IObservable> PoseStream => _poseSubject.AsObservable(); - - public Pose3 GetActualPose() - { - ThrowIfDisposed(); - return _currentState.TcpPose; - } - - public bool TryGetActualPose(out Pose3 pose) - { - ThrowIfDisposed(); - pose = _currentState.TcpPose; - return true; - } - - public Pose3 GetTeachingPoint(string name) - { - ArgumentNullException.ThrowIfNull(name); - if (_teachingPoints is null) - throw new InvalidOperationException("No TeachingPointSet attached. Call AttachTeachingPoints first."); - return _teachingPoints.Get(name); - } - - public bool TryGetTeachingPoint(string name, out Pose3 pose) - { - ArgumentNullException.ThrowIfNull(name); - if (_teachingPoints is null) - { - pose = default; - return false; - } - return _teachingPoints.TryGet(name, out pose); - } - - // === IRobot Teaching (no-op for the simulator — no pendant) === - - public void StartTeaching() { } - - public void EndTeaching() { } - - public IReadOnlyList PeekTeachingPoints() => Array.Empty(); - - public event EventHandler? TeachingPointAdded { add { } remove { } } - - public Joints6 GetJointPositions() - { - ThrowIfDisposed(); - return _currentState.Joints; - } - - public bool JointMode - { - get => _jointMode; - set => _jointMode = value; - } - - /// - /// Move to target joint angles. Per ADR-004, never throws on runtime conditions - /// (joint-limit violation, collision) — those are reported via . - /// - public MoveResult MoveJoint(Joints6 joints) - { - ThrowIfDisposed(); - ThrowIfNotConnected(); - - var violations = _model.ValidateJoints(joints); - if (violations.Count > 0) - return MoveResult.Failed(MoveFailureReason.JointLimitsExceeded, violations); - - if (FirstCollision(joints) is { } hit) - return MoveResult.RejectedByCollision(hit); - - _currentState = ForwardKinematics.Compute(_model, joints, _toolTransform, _basePose); - _poseSubject.OnNext(_currentState.TcpPose); - return MoveResult.Succeeded(); - } - - /// - /// Move linearly to target TCP pose. Per ADR-004, never throws on runtime - /// conditions (unreachable IK, collision) — those are reported via . - /// - public MoveResult MoveLin(Pose3 target, Velocity velocity) - { - ThrowIfDisposed(); - ThrowIfNotConnected(); - - var ikResult = InverseKinematics.Compute(_model, target, _currentState.Joints, _toolTransform, _basePose); - if (!ikResult.Success) - return MoveResult.Failed(ikResult.Reason!.Value.ToMoveReason(), ikResult.Violations); - - if (FirstCollision(ikResult.Joints) is { } hit) - return MoveResult.RejectedByCollision(hit); - - _currentState = ForwardKinematics.Compute(_model, ikResult.Joints, _toolTransform, _basePose); - _poseSubject.OnNext(_currentState.TcpPose); - return MoveResult.Succeeded(); - } - - void IRobot.MoveJoint(Joints6 joints) => MoveJoint(joints); - - int IRobot.MoveLin(Pose3 target, Velocity velocity) - { - var r = MoveLin(target, velocity); - if (r.Success) return 0; - return r.Reason == MoveFailureReason.Collision ? -2 : -1; - } - - public int MoveCircular(Pose3 pathPoint, Pose3 target, Velocity velocity) => - throw new NotSupportedException("MoveCircular is not supported in v1."); - - public int MoveSpline(IEnumerable> waypoints, Velocity velocity) => - throw new NotSupportedException("MoveSpline is not supported in v1."); - - public int ResetAllErrors() - { - ThrowIfDisposed(); - return 0; - } - - public void Dispose() - { - if (_isDisposed) return; - _isDisposed = true; - _isConnected = false; - _poseSubject.OnCompleted(); - _poseSubject.Dispose(); - } - - #endregion - - #region IWeavingRobot Implementation - - /// True while a weave is active (between and ). - public bool IsWeaving => _activeWeaveProfile is not null; - - /// The profile passed to the last , or null when weaving is off. - public WeaveProfile? ActiveWeaveProfile => _activeWeaveProfile; - - /// Records the profile and turns weaving on. Simulation only — no motion is generated here. - public int BeginWeave(WeaveProfile profile) - { - ThrowIfDisposed(); - ArgumentNullException.ThrowIfNull(profile); - _activeWeaveProfile = profile; - return 0; - } - - /// Turns weaving off. A no-op when weaving is already off (benign). - public int EndWeave(WeaveInstruction family) - { - ThrowIfDisposed(); - _activeWeaveProfile = null; - return 0; - } - - #endregion - - #region SimulatedRobot-specific methods - - /// - /// Executes a sequence of waypoints with joint-space interpolation. - /// Each segment is divided into steps with a maximum of 5.0 degrees per joint per step. - /// - public SimulationRunResult ExecuteWaypoints(IReadOnlyList> waypoints, Velocity velocity) - { - ThrowIfDisposed(); - ThrowIfNotConnected(); - ArgumentNullException.ThrowIfNull(waypoints); - if (waypoints.Count == 0) - throw new ArgumentException("Waypoint list must not be empty.", nameof(waypoints)); - - var steps = new List(); - var currentJoints = _currentState.Joints; - - // Add starting state - steps.Add(_currentState); - - for (int wpIdx = 0; wpIdx < waypoints.Count; wpIdx++) - { - var targetPose = waypoints[wpIdx]; - var ikResult = InverseKinematics.Compute(_model, targetPose, currentJoints, _toolTransform, _basePose); - if (!ikResult.Success) - { - // Update robot to last successful position - if (steps.Count > 0) - _currentState = steps[^1]; - return SimulationRunResult.Failed(steps, wpIdx, ikResult.Reason!.Value); - } - - var targetJoints = ikResult.Joints; - var maxDelta = (double)currentJoints.MaxAbsDelta(targetJoints); - var numSteps = Math.Max(1, (int)Math.Ceiling(maxDelta / MaxStepDegrees)); - - for (int s = 1; s <= numSteps; s++) - { - var t = (double)s / numSteps; - var interpJoints = Joints6.Lerp(currentJoints, targetJoints, t); - - if (FirstCollision(interpJoints) is { } hit) - { - _currentState = steps[^1]; - return SimulationRunResult.FailedByCollision(steps, wpIdx, hit); - } - - var state = ForwardKinematics.Compute(_model, interpJoints, _toolTransform, _basePose); - steps.Add(state); - } - - currentJoints = targetJoints; - } - - // Update robot state to final position - _currentState = steps[^1]; - _poseSubject.OnNext(_currentState.TcpPose); - - return SimulationRunResult.Succeeded(steps); - } - - /// Attach or replace the teaching-point set used by Get/TryGet/Set/Remove teaching-point methods. - public void AttachTeachingPoints(TeachingPointSet points) - { - ArgumentNullException.ThrowIfNull(points); - ThrowIfDisposed(); - _teachingPoints = points; - } - - /// Set (add or overwrite) a teaching point. Requires an attached set. - public void SetTeachingPoint(string name, Pose3 pose) - { - ArgumentNullException.ThrowIfNull(name); - ThrowIfDisposed(); - if (_teachingPoints is null) - throw new InvalidOperationException("No TeachingPointSet attached. Call AttachTeachingPoints first."); - _teachingPoints.Set(name, pose); - } - - /// Remove a teaching point. Requires an attached set. Returns true if it existed. - public bool RemoveTeachingPoint(string name) - { - ArgumentNullException.ThrowIfNull(name); - ThrowIfDisposed(); - if (_teachingPoints is null) - throw new InvalidOperationException("No TeachingPointSet attached. Call AttachTeachingPoints first."); - return _teachingPoints.Remove(name); - } - - /// Execute every step of the program from the beginning. - public SimulationRunResult Execute(RobotProgram program, Velocity velocity) => - Execute(program, velocity, 0); - - /// Execute program steps from to end. - public SimulationRunResult Execute(RobotProgram program, Velocity velocity, int startIndex) - { - ThrowIfDisposed(); - ThrowIfNotConnected(); - ArgumentNullException.ThrowIfNull(program); - if (startIndex < 0 || startIndex > program.Count) - throw new ArgumentOutOfRangeException(nameof(startIndex)); - - var steps = new List { _currentState }; - var currentJoints = _currentState.Joints; - - for (int i = startIndex; i < program.Count; i++) - { - var step = program.Steps[i]; - Joints6 targetJoints; - - switch (step) - { - case MoveJointStep mj: - { - var violations = _model.ValidateJoints(mj.Target); - if (violations.Count > 0) - { - _currentState = steps[^1]; - return SimulationRunResult.Failed(steps, i, IkFailureReason.JointLimitsExceeded); - } - targetJoints = mj.Target; - break; - } - case MoveLinStep ml: - { - var ik = InverseKinematics.Compute(_model, ml.Target, currentJoints, _toolTransform, _basePose); - if (!ik.Success) - { - _currentState = steps[^1]; - return SimulationRunResult.Failed(steps, i, ik.Reason!.Value); - } - targetJoints = ik.Joints; - break; - } - default: - throw new NotSupportedException($"Unsupported ProgramStep: {step.GetType().Name}"); - } - - var maxDelta = (double)currentJoints.MaxAbsDelta(targetJoints); - var numSteps = Math.Max(1, (int)Math.Ceiling(maxDelta / MaxStepDegrees)); - for (int s = 1; s <= numSteps; s++) - { - var t = (double)s / numSteps; - var interp = Joints6.Lerp(currentJoints, targetJoints, t); - - if (FirstCollision(interp) is { } hit) - { - _currentState = steps[^1]; - return SimulationRunResult.FailedByCollision(steps, i, hit); - } - - steps.Add(ForwardKinematics.Compute(_model, interp, _toolTransform, _basePose)); - } - currentJoints = targetJoints; - } - - _currentState = steps[^1]; - _poseSubject.OnNext(_currentState.TcpPose); - return SimulationRunResult.Succeeded(steps); - } - - private CollisionResult? FirstCollision(Joints6 joints) - { - if (_collisionEnv is null) return null; - var hits = CollisionDetector.CheckCollision(_model, joints, _collisionEnv); - return hits.Count == 0 ? null : hits[0]; - } - - #endregion - - private void ThrowIfDisposed() - { - ObjectDisposedException.ThrowIf(_isDisposed, this); - } - - private void ThrowIfNotConnected() - { - if (!_isConnected) - throw new InvalidOperationException("SimulatedRobot is not connected. Call Connect() first."); - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core/SimulationRunResult.cs b/csharp/RocketWelder.SDK.Robotics.Core/SimulationRunResult.cs deleted file mode 100644 index 682f8fa..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/SimulationRunResult.cs +++ /dev/null @@ -1,53 +0,0 @@ -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Result of executing a waypoint sequence. Contains the simulation run data and failure info if applicable. -/// -public sealed record SimulationRunResult -{ - /// Whether all waypoints were reached successfully. - public bool Success { get; } - - /// The simulation run data (list of RobotState snapshots). May be partial on failure. - public IReadOnlyList Steps { get; } - - /// Index of the failed waypoint (0-based), or null on success. - public int? FailedWaypointIndex { get; } - - /// IK failure reason (null on success or non-IK failure). - public IkFailureReason? Reason { get; } - - /// Normalised failure reason across IK and collision rejections. Null on success. - public MoveFailureReason? FailureReason { get; } - - /// Collision details when is . - public CollisionResult? Collision { get; } - - private SimulationRunResult( - bool success, - IReadOnlyList steps, - int? failedWaypointIndex, - IkFailureReason? reason, - MoveFailureReason? failureReason, - CollisionResult? collision) - { - Success = success; - Steps = steps; - FailedWaypointIndex = failedWaypointIndex; - Reason = reason; - FailureReason = failureReason; - Collision = collision; - } - - /// Creates a successful simulation run result. - public static SimulationRunResult Succeeded(IReadOnlyList steps) => - new(true, steps, null, null, null, null); - - /// Creates an IK-failure simulation run result. - public static SimulationRunResult Failed(IReadOnlyList steps, int failedWaypointIndex, IkFailureReason reason) => - new(false, steps, failedWaypointIndex, reason, reason.ToMoveReason(), null); - - /// Creates a collision-rejected simulation run result. - public static SimulationRunResult FailedByCollision(IReadOnlyList steps, int failedWaypointIndex, CollisionResult collision) => - new(false, steps, failedWaypointIndex, null, MoveFailureReason.Collision, collision); -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core/TeachingPointSet.cs b/csharp/RocketWelder.SDK.Robotics.Core/TeachingPointSet.cs deleted file mode 100644 index 975bd80..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/TeachingPointSet.cs +++ /dev/null @@ -1,120 +0,0 @@ -using System.Text.Json; -using System.Text.Json.Serialization; -using ModelingEvolution.Drawing; - -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Mutable ordered collection of named entries. Not thread-safe. -/// Name lookup is case-sensitive and unique. -/// -public sealed class TeachingPointSet -{ - private readonly Dictionary> _poses = new(StringComparer.Ordinal); - private readonly List _order = new(); - - /// Number of teaching points. - public int Count => _order.Count; - - /// Names in insertion order. - public IReadOnlyList Names => _order; - - /// Set (add or overwrite) a named pose. - public void Set(string name, Pose3 pose) - { - ArgumentNullException.ThrowIfNull(name); - if (!_poses.ContainsKey(name)) - _order.Add(name); - _poses[name] = pose; - } - - /// Get the pose for , or throw when absent. - public Pose3 Get(string name) - { - ArgumentNullException.ThrowIfNull(name); - if (!_poses.TryGetValue(name, out var pose)) - throw new KeyNotFoundException($"Teaching point '{name}' not found."); - return pose; - } - - /// Try to get the pose for . - public bool TryGet(string name, out Pose3 pose) - { - ArgumentNullException.ThrowIfNull(name); - return _poses.TryGetValue(name, out pose); - } - - /// Remove a teaching point. Returns true if it existed. - public bool Remove(string name) - { - ArgumentNullException.ThrowIfNull(name); - if (!_poses.Remove(name)) return false; - _order.Remove(name); - return true; - } - - /// Whether the set contains . - public bool Contains(string name) - { - ArgumentNullException.ThrowIfNull(name); - return _poses.ContainsKey(name); - } - - /// Enumerate entries in insertion order. - public IEnumerable>> Enumerate() - { - foreach (var name in _order) - yield return new KeyValuePair>(name, _poses[name]); - } - - /// Serialize to JSON. - public string ToJson(JsonSerializerOptions? options = null) - { - var points = new TeachingPointDto[_order.Count]; - for (int i = 0; i < _order.Count; i++) - { - var n = _order[i]; - points[i] = new TeachingPointDto { Name = n, Pose = _poses[n] }; - } - var dto = new TeachingPointSetDto { Points = points }; - return JsonSerializer.Serialize(dto, options ?? DefaultJson); - } - - /// Parse from JSON. - public static TeachingPointSet FromJson(string json, JsonSerializerOptions? options = null) - { - ArgumentNullException.ThrowIfNull(json); - var dto = JsonSerializer.Deserialize(json, options ?? DefaultJson) - ?? throw new InvalidOperationException("Teaching point set JSON deserialised to null."); - var set = new TeachingPointSet(); - if (dto.Points is null) return set; - foreach (var p in dto.Points) - { - if (string.IsNullOrEmpty(p.Name)) - throw new InvalidOperationException("Teaching point entry is missing 'name'."); - set.Set(p.Name, p.Pose); - } - return set; - } - - internal static readonly JsonSerializerOptions DefaultJson = new() - { - WriteIndented = false, - PropertyNamingPolicy = JsonNamingPolicy.CamelCase - }; - - private sealed class TeachingPointSetDto - { - [JsonPropertyName("points")] - public TeachingPointDto[]? Points { get; set; } - } - - private sealed class TeachingPointDto - { - [JsonPropertyName("name")] - public string Name { get; set; } = string.Empty; - - [JsonPropertyName("pose")] - public Pose3 Pose { get; set; } - } -} diff --git a/csharp/RocketWelder.SDK.Robotics.Core/ToolModel.cs b/csharp/RocketWelder.SDK.Robotics.Core/ToolModel.cs deleted file mode 100644 index fd298c6..0000000 --- a/csharp/RocketWelder.SDK.Robotics.Core/ToolModel.cs +++ /dev/null @@ -1,31 +0,0 @@ -namespace RocketWelder.SDK.Robotics.Core; - -/// -/// Abstract tool geometry attached to the robot flange. Immutable. -/// Resolved by the active . -/// -public abstract record ToolModel -{ - /// Protected constructor — use or . - protected ToolModel() { } - - /// The zero-tool sentinel (no geometry attached). - public static readonly ToolModel None = new NoneToolModel(); - - private sealed record NoneToolModel : ToolModel; -} - -/// -/// Capsule tool geometry: a swept sphere along a line segment in the flange frame (millimetres). -/// Planning-grade check for . -/// -/// Length of the capsule axis in mm. Must be non-negative. -/// Radius in mm. Must be positive. -public sealed record CapsuleToolModel(double Length, double Radius) : ToolModel; - -/// -/// Mesh tool reference. The mesh itself is owned by a mesh-capable which -/// resolves at query time. -/// -/// Opaque mesh identifier (e.g., asset name or hash). -public sealed record MeshToolModel(string Id) : ToolModel; diff --git a/csharp/RocketWelder.SDK.sln b/csharp/RocketWelder.SDK.sln index 5c9671e..54bee2c 100644 --- a/csharp/RocketWelder.SDK.sln +++ b/csharp/RocketWelder.SDK.sln @@ -38,10 +38,6 @@ Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RocketWelder.SDK.Types.Test EndProject Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RocketWelder.SDK.Automation.Tests", "RocketWelder.SDK.Automation.Tests\RocketWelder.SDK.Automation.Tests.csproj", "{09679B63-9EED-4B76-840F-569195C84151}" EndProject -Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RocketWelder.SDK.Robotics.Core", "RocketWelder.SDK.Robotics.Core\RocketWelder.SDK.Robotics.Core.csproj", "{6C6A2B8E-90E6-46FC-A237-0AAD50E8FEE2}" -EndProject -Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RocketWelder.SDK.Robotics.Core.Tests", "RocketWelder.SDK.Robotics.Core.Tests\RocketWelder.SDK.Robotics.Core.Tests.csproj", "{3753E954-5C68-4AE9-B3FA-24D7374147DB}" -EndProject Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RocketWelder.SDK.Http", "RocketWelder.SDK.Http\RocketWelder.SDK.Http.csproj", "{0CE66E9A-D7B0-459E-AF62-DBC89AEEB732}" EndProject Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RocketWelder.SDK.Automation.WeldProgramCli", "RocketWelder.SDK.Automation.WeldProgramCli\RocketWelder.SDK.Automation.WeldProgramCli.csproj", "{58C412F3-6B7D-4044-878B-5AE2E9FFCBE4}" @@ -242,30 +238,6 @@ Global {09679B63-9EED-4B76-840F-569195C84151}.Release|x64.Build.0 = Release|Any CPU {09679B63-9EED-4B76-840F-569195C84151}.Release|x86.ActiveCfg = Release|Any CPU {09679B63-9EED-4B76-840F-569195C84151}.Release|x86.Build.0 = Release|Any CPU - {6C6A2B8E-90E6-46FC-A237-0AAD50E8FEE2}.Debug|Any CPU.ActiveCfg = Debug|Any CPU - {6C6A2B8E-90E6-46FC-A237-0AAD50E8FEE2}.Debug|Any CPU.Build.0 = Debug|Any CPU - {6C6A2B8E-90E6-46FC-A237-0AAD50E8FEE2}.Debug|x64.ActiveCfg = Debug|Any CPU - {6C6A2B8E-90E6-46FC-A237-0AAD50E8FEE2}.Debug|x64.Build.0 = Debug|Any CPU - {6C6A2B8E-90E6-46FC-A237-0AAD50E8FEE2}.Debug|x86.ActiveCfg = Debug|Any CPU - {6C6A2B8E-90E6-46FC-A237-0AAD50E8FEE2}.Debug|x86.Build.0 = Debug|Any CPU - {6C6A2B8E-90E6-46FC-A237-0AAD50E8FEE2}.Release|Any CPU.ActiveCfg = Release|Any CPU - {6C6A2B8E-90E6-46FC-A237-0AAD50E8FEE2}.Release|Any CPU.Build.0 = Release|Any CPU - {6C6A2B8E-90E6-46FC-A237-0AAD50E8FEE2}.Release|x64.ActiveCfg = Release|Any CPU - {6C6A2B8E-90E6-46FC-A237-0AAD50E8FEE2}.Release|x64.Build.0 = Release|Any CPU - {6C6A2B8E-90E6-46FC-A237-0AAD50E8FEE2}.Release|x86.ActiveCfg = Release|Any CPU - {6C6A2B8E-90E6-46FC-A237-0AAD50E8FEE2}.Release|x86.Build.0 = Release|Any CPU - {3753E954-5C68-4AE9-B3FA-24D7374147DB}.Debug|Any CPU.ActiveCfg = Debug|Any CPU - {3753E954-5C68-4AE9-B3FA-24D7374147DB}.Debug|Any CPU.Build.0 = Debug|Any CPU - {3753E954-5C68-4AE9-B3FA-24D7374147DB}.Debug|x64.ActiveCfg = Debug|Any CPU - {3753E954-5C68-4AE9-B3FA-24D7374147DB}.Debug|x64.Build.0 = Debug|Any CPU - {3753E954-5C68-4AE9-B3FA-24D7374147DB}.Debug|x86.ActiveCfg = Debug|Any CPU - {3753E954-5C68-4AE9-B3FA-24D7374147DB}.Debug|x86.Build.0 = Debug|Any CPU - {3753E954-5C68-4AE9-B3FA-24D7374147DB}.Release|Any CPU.ActiveCfg = Release|Any CPU - {3753E954-5C68-4AE9-B3FA-24D7374147DB}.Release|Any CPU.Build.0 = Release|Any CPU - {3753E954-5C68-4AE9-B3FA-24D7374147DB}.Release|x64.ActiveCfg = Release|Any CPU - {3753E954-5C68-4AE9-B3FA-24D7374147DB}.Release|x64.Build.0 = Release|Any CPU - {3753E954-5C68-4AE9-B3FA-24D7374147DB}.Release|x86.ActiveCfg = Release|Any CPU - {3753E954-5C68-4AE9-B3FA-24D7374147DB}.Release|x86.Build.0 = Release|Any CPU {0CE66E9A-D7B0-459E-AF62-DBC89AEEB732}.Debug|Any CPU.ActiveCfg = Debug|Any CPU {0CE66E9A-D7B0-459E-AF62-DBC89AEEB732}.Debug|Any CPU.Build.0 = Debug|Any CPU {0CE66E9A-D7B0-459E-AF62-DBC89AEEB732}.Debug|x64.ActiveCfg = Debug|Any CPU