From ae26b50febd6253a802788ab028c7d8e365b700c Mon Sep 17 00:00:00 2001 From: Ruthie-FRC Date: Thu, 2 Jul 2026 10:39:35 -0500 Subject: [PATCH 1/5] attempt to fix collisions --- .../main/java/jsim/api/SimBodyBuilder.java | 38 ++++---- .../main/java/jsim/collision/NarrowPhase.java | 96 ++++++++++++------- .../java/jsim/collision/NarrowPhaseTest.java | 14 ++- 3 files changed, 90 insertions(+), 58 deletions(-) diff --git a/vendordep/src/main/java/jsim/api/SimBodyBuilder.java b/vendordep/src/main/java/jsim/api/SimBodyBuilder.java index 9df8634..0ac87e7 100644 --- a/vendordep/src/main/java/jsim/api/SimBodyBuilder.java +++ b/vendordep/src/main/java/jsim/api/SimBodyBuilder.java @@ -37,6 +37,7 @@ public final class SimBodyBuilder { private double mass = 1.0; // kg private double ixx = 1.0, iyy = 1.0, izz = 1.0; // kg·m² + private boolean inertiaExplicit = false; private double posX, posY, posZ; // metres private double qW = 1, qX, qY, qZ; // unit quaternion (w,x,y,z) @@ -144,7 +145,9 @@ public SimBodyBuilder angularVelocity(double ox, double oy, double oz) { * @return this builder */ public SimBodyBuilder inertia(double ixx, double iyy, double izz) { - this.ixx = ixx; this.iyy = iyy; this.izz = izz; return this; + this.ixx = ixx; this.iyy = iyy; this.izz = izz; + this.inertiaExplicit = true; + return this; } // Collider shortcuts @@ -157,7 +160,7 @@ public SimBodyBuilder inertia(double ixx, double iyy, double izz) { */ public SimBodyBuilder sphereCollider(double radius) { collider = new SphereCollider(radius); - return estimateInertiaIfNeeded(radius); + return this; } /** @@ -170,7 +173,7 @@ public SimBodyBuilder sphereCollider(double radius) { */ public SimBodyBuilder boxCollider(double halfX, double halfY, double halfZ) { collider = new BoxCollider(halfX, halfY, halfZ); - return estimateInertiaIfNeeded(halfX, halfY, halfZ); + return this; } /** @@ -258,6 +261,19 @@ SimBody build() { body.omX = omX; body.omY = omY; body.omZ = omZ; body.collider = collider; + if (!inertiaExplicit && collider != null) { + if (collider instanceof SphereCollider) { + double r = ((SphereCollider) collider).radius; + double v = 0.4 * mass * r * r; + ixx = iyy = izz = v; + } else if (collider instanceof BoxCollider) { + BoxCollider bc = (BoxCollider) collider; + ixx = mass / 3.0 * (bc.halfY * bc.halfY + bc.halfZ * bc.halfZ); + iyy = mass / 3.0 * (bc.halfX * bc.halfX + bc.halfZ * bc.halfZ); + izz = mass / 3.0 * (bc.halfX * bc.halfX + bc.halfY * bc.halfY); + } + } + if (RigidBodyFlags.isSet(flags, RigidBodyFlags.STATIC)) { body.setStatic(); } else { @@ -268,20 +284,4 @@ SimBody build() { return new SimBody(body, actuator, robotId); } - // Inertia estimation - - private SimBodyBuilder estimateInertiaIfNeeded(double radius) { - // Solid sphere: I = 2/5 * m * r² - double v = 0.4 * mass * radius * radius; - ixx = iyy = izz = v; - return this; - } - - private SimBodyBuilder estimateInertiaIfNeeded(double hx, double hy, double hz) { - // Solid box: I_xx = m/12 * (4hy² + 4hz²) with half-extents - ixx = mass / 3.0 * (hy * hy + hz * hz); - iyy = mass / 3.0 * (hx * hx + hz * hz); - izz = mass / 3.0 * (hx * hx + hy * hy); - return this; - } } diff --git a/vendordep/src/main/java/jsim/collision/NarrowPhase.java b/vendordep/src/main/java/jsim/collision/NarrowPhase.java index ad82efc..cf735f4 100644 --- a/vendordep/src/main/java/jsim/collision/NarrowPhase.java +++ b/vendordep/src/main/java/jsim/collision/NarrowPhase.java @@ -114,24 +114,15 @@ private static void boxPlane(RigidBody box, BoxCollider bc, wp[0] += plane.posX; wp[1] += plane.posY; wp[2] += plane.posZ; double[] Rb = Mat3.fromQuaternion(box.qW, box.qX, box.qY, box.qZ); - double[] corners = {-1, 1}; - double deepest = Double.POSITIVE_INFINITY; - double[] deepestCorner = null; - - for (double sx : corners) for (double sy : corners) for (double sz : corners) { - // Corner in body space + double[] signs = {-1, 1}; + for (double sx : signs) for (double sy : signs) for (double sz : signs) { double[] lc = Vec3.of(sx * bc.halfX, sy * bc.halfY, sz * bc.halfZ); - // Corner in world space double[] wc = Mat3.mulVec(Rb, lc); wc[0] += box.posX; wc[1] += box.posY; wc[2] += box.posZ; - // Signed distance to plane double d = Vec3.dot(Vec3.sub(wc, wp), wn); - if (d < deepest) { deepest = d; deepestCorner = wc; } - } - - if (deepest < 0 && deepestCorner != null) { - emit(box, plane, deepestCorner[0], deepestCorner[1], deepestCorner[2], - wn[0], wn[1], wn[2], -deepest, out); + if (d < 0) { + emit(box, plane, wc[0], wc[1], wc[2], wn[0], wn[1], wn[2], -d, out); + } } } @@ -158,24 +149,32 @@ private static void boxSphere(RigidBody box, BoxCollider bc, double dist = Math.sqrt(distSq); double[] localNormal; + double pen; if (dist < 1e-8) { - // Sphere centre inside box — push along shallowest axis + // Sphere centre inside box — push out along the shallowest face axis. + // Penetration = sphere radius + distance from centre to that face. double dx = bc.halfX - Math.abs(local[0]); double dy = bc.halfY - Math.abs(local[1]); double dz = bc.halfZ - Math.abs(local[2]); - if (dx <= dy && dx <= dz) localNormal = Vec3.of(Math.signum(local[0]), 0, 0); - else if (dy <= dz) localNormal = Vec3.of(0, Math.signum(local[1]), 0); - else localNormal = Vec3.of(0, 0, Math.signum(local[2])); + if (dx <= dy && dx <= dz) { + localNormal = Vec3.of(Math.signum(local[0]), 0, 0); + pen = sc.radius + dx; + } else if (dy <= dz) { + localNormal = Vec3.of(0, Math.signum(local[1]), 0); + pen = sc.radius + dy; + } else { + localNormal = Vec3.of(0, 0, Math.signum(local[2])); + pen = sc.radius + dz; + } } else { localNormal = Vec3.scale(diff, 1.0 / dist); + pen = sc.radius - dist; } // Transform normal back to world frame (sphere is bodyA for convention) double[] wn = Mat3.mulVec(Rb, localNormal); double[] wClosest = Mat3.mulVec(Rb, closest); wClosest[0] += box.posX; wClosest[1] += box.posY; wClosest[2] += box.posZ; - - double pen = sc.radius - dist; // Normal points from box toward sphere (sphere = bodyA) emit(sphere, box, wClosest[0], wClosest[1], wClosest[2], wn[0], wn[1], wn[2], pen, out); } @@ -190,7 +189,6 @@ private static void boxBox(RigidBody a, BoxCollider ba, double[] t = Vec3.of(b.posX - a.posX, b.posY - a.posY, b.posZ - a.posZ); - // Build axes for SAT: 3 face-normals from A, 3 from B (9 edge cross axes omitted for brevity) double[][] axesA = {colOf(Ra, 0), colOf(Ra, 1), colOf(Ra, 2)}; double[][] axesB = {colOf(Rb, 0), colOf(Rb, 1), colOf(Rb, 2)}; double[] halfsA = {ba.halfX, ba.halfY, ba.halfZ}; @@ -200,26 +198,21 @@ private static void boxBox(RigidBody a, BoxCollider ba, double[] bestAxis = null; int bestAxisSign = 1; - // Test A's face normals + // Test A's 3 face normals for (int i = 0; i < 3; i++) { double[] ax = axesA[i]; - double projA = halfsA[i]; - double projB = projectBox(ax, Rb, halfsB); - double dist = Math.abs(Vec3.dot(t, ax)) - (projA + projB); - if (dist > 0) return; // separating axis found + double dist = Math.abs(Vec3.dot(t, ax)) - (halfsA[i] + projectBox(ax, Rb, halfsB)); + if (dist > 0) return; if (-dist < minPen) { minPen = -dist; bestAxis = ax; - // normal must point from B toward A: negate axis if it aligns with t (A→B) bestAxisSign = (Vec3.dot(t, ax) < 0) ? 1 : -1; } } - // Test B's face normals + // Test B's 3 face normals for (int i = 0; i < 3; i++) { double[] ax = axesB[i]; - double projA = projectBox(ax, Ra, halfsA); - double projB = halfsB[i]; - double dist = Math.abs(Vec3.dot(t, ax)) - (projA + projB); + double dist = Math.abs(Vec3.dot(t, ax)) - (projectBox(ax, Ra, halfsA) + halfsB[i]); if (dist > 0) return; if (-dist < minPen) { minPen = -dist; @@ -227,14 +220,49 @@ private static void boxBox(RigidBody a, BoxCollider ba, bestAxisSign = (Vec3.dot(t, ax) < 0) ? 1 : -1; } } + // Test 9 edge cross-product axes (required for edge-edge contacts on rotated boxes) + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + double[] ax = Vec3.cross(axesA[i], axesB[j]); + double len = Vec3.length(ax); + if (len < 1e-8) continue; // parallel edges — already covered by face axes + ax = Vec3.scale(ax, 1.0 / len); + double dist = Math.abs(Vec3.dot(t, ax)) + - (projectBox(ax, Ra, halfsA) + projectBox(ax, Rb, halfsB)); + if (dist > 0) return; + if (-dist < minPen) { + minPen = -dist; + bestAxis = ax; + bestAxisSign = (Vec3.dot(t, ax) < 0) ? 1 : -1; + } + } + } if (bestAxis == null) return; // Contact normal points from B to A double[] n = Vec3.scale(bestAxis, bestAxisSign); - // Contact point: deepest corner of B in direction n (toward A) - double[] corner = deepestCorner(b, Rb, bb, n); - emit(a, b, corner[0], corner[1], corner[2], n[0], n[1], n[2], minPen, out); + + // Emit one contact point per corner of B that lies inside A along n. + // pen_corner = dot(c_B - a.pos, n) + projA gives per-corner penetration depth. + double projA = projectBox(n, Ra, halfsA); + boolean emitted = false; + double[] sv = {-1.0, 1.0}; + for (double sx : sv) for (double sy : sv) for (double sz : sv) { + double[] localB = Vec3.of(sx * bb.halfX, sy * bb.halfY, sz * bb.halfZ); + double[] wc = Mat3.mulVec(Rb, localB); + wc[0] += b.posX; wc[1] += b.posY; wc[2] += b.posZ; + double pen = Vec3.dot(Vec3.of(wc[0] - a.posX, wc[1] - a.posY, wc[2] - a.posZ), n) + projA; + if (pen > 0) { + emit(a, b, wc[0], wc[1], wc[2], n[0], n[1], n[2], pen, out); + emitted = true; + } + } + // Fallback for edge-edge contacts where no corner of B is inside A + if (!emitted) { + double[] corner = deepestCorner(b, Rb, bb, n); + emit(a, b, corner[0], corner[1], corner[2], n[0], n[1], n[2], minPen, out); + } } // SAT helpers diff --git a/vendordep/src/test/java/jsim/collision/NarrowPhaseTest.java b/vendordep/src/test/java/jsim/collision/NarrowPhaseTest.java index 4d4a399..fa42184 100644 --- a/vendordep/src/test/java/jsim/collision/NarrowPhaseTest.java +++ b/vendordep/src/test/java/jsim/collision/NarrowPhaseTest.java @@ -147,15 +147,17 @@ void spherePlane_normalPointsUp() { // Box vs Plane @Test void boxPlane_cornerBelowFloor_contact() { - // Box at z=0.4, half-height 0.5 → bottom corners at z=-0.1 (penetrating) + // Box at z=0.4, half-height 0.5 → all 4 bottom corners at z=-0.1 (penetrating) RigidBody b = box(0, 0, 0.4, 0.3, 0.3, 0.5); RigidBody floor = staticFloor(); List pairs = new ArrayList<>(); pairs.add(new CollisionPair(b, floor)); List out = new ArrayList<>(); NarrowPhase.generateContacts(pairs, out); - assertEquals(1, out.size()); - assertTrue(out.get(0).penetration > 0, "box corner should be penetrating floor"); + assertEquals(4, out.size(), "All 4 bottom corners should generate contacts"); + for (ContactPoint cp : out) { + assertTrue(cp.penetration > 0, "each contact should have positive penetration"); + } } @Test @@ -203,8 +205,10 @@ void boxBox_overlapping_contact() { pairs.add(new CollisionPair(a, b)); List out = new ArrayList<>(); NarrowPhase.generateContacts(pairs, out); - assertEquals(1, out.size()); - assertTrue(out.get(0).penetration > 0); + assertTrue(out.size() >= 1, "Should generate at least one contact"); + for (ContactPoint cp : out) { + assertTrue(cp.penetration > 0, "each contact should have positive penetration"); + } } @Test From c5a8b395166835dadcd0c5d0c655f822f835e5e6 Mon Sep 17 00:00:00 2001 From: Ruthie-FRC Date: Fri, 3 Jul 2026 09:19:22 -0500 Subject: [PATCH 2/5] Add FRC field simulation classes and tests - Implement FieldLayout for adding static field geometry (floor and walls) to SimWorld. - Create FieldSimulator to manage game pieces and robot interactions on the field. - Introduce GamePiece and GamePieceGripper classes for handling game piece dynamics and robot intake. - Develop comprehensive unit tests for FieldLayout, FieldSimulator, GamePiece, and GamePieceGripper to ensure functionality and correctness. --- .../src/main/java/jsim/field/FieldLayout.java | 110 +++++++++ .../main/java/jsim/field/FieldSimulator.java | 217 ++++++++++++++++++ .../src/main/java/jsim/field/GamePiece.java | 57 +++++ .../java/jsim/field/GamePieceGripper.java | 123 ++++++++++ .../test/java/jsim/field/FieldLayoutTest.java | 148 ++++++++++++ .../java/jsim/field/FieldSimulatorTest.java | 197 ++++++++++++++++ .../java/jsim/field/GamePieceGripperTest.java | 168 ++++++++++++++ 7 files changed, 1020 insertions(+) create mode 100644 vendordep/src/main/java/jsim/field/FieldLayout.java create mode 100644 vendordep/src/main/java/jsim/field/FieldSimulator.java create mode 100644 vendordep/src/main/java/jsim/field/GamePiece.java create mode 100644 vendordep/src/main/java/jsim/field/GamePieceGripper.java create mode 100644 vendordep/src/test/java/jsim/field/FieldLayoutTest.java create mode 100644 vendordep/src/test/java/jsim/field/FieldSimulatorTest.java create mode 100644 vendordep/src/test/java/jsim/field/GamePieceGripperTest.java diff --git a/vendordep/src/main/java/jsim/field/FieldLayout.java b/vendordep/src/main/java/jsim/field/FieldLayout.java new file mode 100644 index 0000000..f5c5369 --- /dev/null +++ b/vendordep/src/main/java/jsim/field/FieldLayout.java @@ -0,0 +1,110 @@ +package jsim.field; + +import jsim.api.SimBodyBuilder; +import jsim.api.SimWorld; +import jsim.material.Material; + +/** + * Utility for populating a {@link SimWorld} with FRC field geometry: a carpet floor + * (infinite plane) and four perimeter wall segments (box colliders). + * + *

All bodies added by this class are static — they deflect game pieces and robots + * but cannot themselves be moved. + * + *

Coordinate frame: X points from the blue alliance wall toward the red alliance wall; + * Y points across the field; Z points up. This matches AdvantageScope's field rendering. + */ +public final class FieldLayout { + /** FRC 2025/2026 field length (blue to red) in metres (54 ft). */ + public static final double FRC_LENGTH_M = 16.4592; + /** FRC 2025/2026 field width in metres (27 ft). */ + public static final double FRC_WIDTH_M = 8.2296; + + private static final double WALL_THICK = 0.05; + private static final double WALL_HEIGHT = 0.5; + + private FieldLayout() {} + + /** + * Add a standard FRC field (carpet floor + four perimeter walls) to {@code world}. + * + * @param world the world to populate + */ + public static void addFrcField(SimWorld world) { + addField(world, FRC_LENGTH_M, FRC_WIDTH_M); + } + + /** + * Add a configurable rectangular field (carpet floor + four perimeter walls) to {@code world}. + * + * @param world the world to populate + * @param lengthM field length along X (metres) + * @param widthM field width along Y (metres) + */ + public static void addField(SimWorld world, double lengthM, double widthM) { + addFloor(world); + addWalls(world, lengthM, widthM, WALL_THICK, WALL_HEIGHT); + } + + /** + * Add just the carpet floor (infinite plane at Z = 0) to {@code world}. + * + * @param world the world to populate + */ + public static void addFloor(SimWorld world) { + world.addBody(new SimBodyBuilder("Field/Floor") + .planeCollider(0, 0, 1, 0) + .material(Material.CARPET)); + } + + /** + * Add four perimeter wall segments to {@code world}. + * + *

The blue and red end walls are extended in Y to cover the field corners, + * so no gap exists at any corner of the boundary. + * + * @param world the world to populate + * @param lengthM field length along X (metres) + * @param widthM field width along Y (metres) + * @param thickM wall thickness (metres) + * @param heightM wall height above the floor (metres) + */ + public static void addWalls(SimWorld world, + double lengthM, double widthM, + double thickM, double heightM) { + double t2 = thickM / 2.0; + double h2 = heightM / 2.0; + double l2 = lengthM / 2.0; + double w2 = widthM / 2.0; + + // Blue alliance wall — inner face at x = 0, box extends into x < 0. + // halfY is w2+thickM so it overlaps the adjacent Near/Far wall thickness, + // closing the corner gap. + world.addBody(new SimBodyBuilder("Field/WallBlue") + .position(-t2, w2, h2) + .boxCollider(t2, w2 + thickM, h2) + .material(Material.WALL) + .isStatic()); + + // Red alliance wall — inner face at x = lengthM. + world.addBody(new SimBodyBuilder("Field/WallRed") + .position(lengthM + t2, w2, h2) + .boxCollider(t2, w2 + thickM, h2) + .material(Material.WALL) + .isStatic()); + + // Near wall — inner face at y = 0, box extends into y < 0. + world.addBody(new SimBodyBuilder("Field/WallNear") + .position(l2, -t2, h2) + .boxCollider(l2, t2, h2) + .material(Material.WALL) + .isStatic()); + + // Far wall — inner face at y = widthM. + world.addBody(new SimBodyBuilder("Field/WallFar") + .position(l2, widthM + t2, h2) + .boxCollider(l2, t2, h2) + .material(Material.WALL) + .isStatic()); + } +} diff --git a/vendordep/src/main/java/jsim/field/FieldSimulator.java b/vendordep/src/main/java/jsim/field/FieldSimulator.java new file mode 100644 index 0000000..e783b3c --- /dev/null +++ b/vendordep/src/main/java/jsim/field/FieldSimulator.java @@ -0,0 +1,217 @@ +package jsim.field; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.LinkedHashMap; +import java.util.List; +import java.util.Map; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Translation3d; +import jsim.api.SimBody; +import jsim.api.SimBodyBuilder; +import jsim.api.SimWorld; +import jsim.core.SimConstants; + +/** + * High-level FRC field simulator that combines a {@link SimWorld} with pre-built + * field geometry and game-piece management tailored to AdvantageScope integration. + * + *

The field is static — it deflects game pieces and robots but cannot itself be + * moved. Game pieces are dynamic rigid bodies that collide with the field and each + * other. Robots are dynamic bodies that the user drives externally (e.g. via + * {@code SimBody.getActuator()} or {@code SimBody.setPose()}). + * + *

Typical usage: + * 

{@code
+ * FieldSimulator sim = new FieldSimulator();
+ *
+ * SimBody robot = sim.addRobot(new SimBodyBuilder("Robot")
+ *     .pose(new Pose3d(2, 4, 0.05, new Rotation3d()))
+ *     .mass(50)
+ *     .boxCollider(0.45, 0.45, 0.3)
+ *     .material(Material.CARPET)
+ *     .noGravity()
+ *     .fixedRotation());
+ *
+ * GamePieceGripper gripper = sim.createGripper(robot, new Translation3d(0.5, 0, 0.1));
+ *
+ * GamePiece note = sim.spawnGamePiece("Note", new SimBodyBuilder("Note0")
+ *     .position(8, 4, 0.18)
+ *     .mass(0.235)
+ *     .sphereCollider(0.18)
+ *     .material(Material.RUBBER));
+ *
+ * // In simulationPeriodic():
+ * sim.step();
+ * Pose3d[] notePoses = sim.getGamePiecePoses("Note");
+ * Logger.recordOutput("FieldSimulation/NotePoses", notePoses);
+ * }
+ */ +public final class FieldSimulator { + private final SimWorld world; + private final List pieces = new ArrayList<>(); + private final List grippers = new ArrayList<>(); + + /** + * Create a simulator with the standard FRC field (16.46 m × 8.23 m) and the + * default 20 ms fixed timestep. + */ + public FieldSimulator() { + this(SimConstants.DEFAULT_DT); + } + + /** + * Create a simulator with the standard FRC field and a custom fixed timestep. + * + * @param fixedTimestepSeconds physics timestep in seconds + */ + public FieldSimulator(double fixedTimestepSeconds) { + this(fixedTimestepSeconds, FieldLayout.FRC_LENGTH_M, FieldLayout.FRC_WIDTH_M); + } + + /** + * Create a simulator with a custom field size and timestep. + * + * @param fixedTimestepSeconds physics timestep in seconds + * @param fieldLengthM field length along X (metres) + * @param fieldWidthM field width along Y (metres) + */ + public FieldSimulator(double fixedTimestepSeconds, double fieldLengthM, double fieldWidthM) { + world = new SimWorld(fixedTimestepSeconds); + FieldLayout.addField(world, fieldLengthM, fieldWidthM); + } + + // Body management + + /** + * Add a robot body to the simulation. + * + *

Robots are typically configured with {@code .noGravity().fixedRotation()} and + * driven by setting their pose or actuator forces each tick. + * + * @param builder configured body builder + * @return the resulting {@link SimBody} + */ + public SimBody addRobot(SimBodyBuilder builder) { + return world.addBody(builder); + } + + /** + * Spawn a game piece in the world. The piece collides with the field (floor and walls) + * and any robot bodies, and can be picked up by a {@link GamePieceGripper}. + * + *

The {@code variant} string must match the gamepiece key in the AdvantageScope + * asset configuration (e.g. {@code "Note"} for 2024 Crescendo, {@code "Coral"} for + * 2025 Reefscape). + * + * @param variant AdvantageScope gamepiece variant name + * @param builder configured body builder for physics properties + * @return the new {@link GamePiece} handle + */ + public GamePiece spawnGamePiece(String variant, SimBodyBuilder builder) { + SimBody body = world.addBody(builder); + GamePiece piece = new GamePiece(body, variant); + pieces.add(piece); + return piece; + } + + /** + * Remove a game piece from the simulation, releasing it from any gripper that + * holds it first. + * + * @param piece the piece to remove + */ + public void removeGamePiece(GamePiece piece) { + // Detach from any holding gripper before removing the body. + for (GamePieceGripper g : grippers) { + if (g.getHeld() == piece) { + g.release(); + break; + } + } + world.removeBody(piece.getBody()); + pieces.remove(piece); + } + + /** + * Create a {@link GamePieceGripper} that mounts on {@code robot} at {@code intakeOffset} + * (in robot-local frame). The gripper's {@link GamePieceGripper#update()} is called + * automatically by {@link #step()}. + * + * @param robot the robot body the gripper is mounted on + * @param intakeOffset hold-point position in the robot's local frame (metres) + * @return a new {@link GamePieceGripper} + */ + public GamePieceGripper createGripper(SimBody robot, Translation3d intakeOffset) { + GamePieceGripper gripper = new GamePieceGripper(robot, intakeOffset); + grippers.add(gripper); + return gripper; + } + + // Stepping + + /** + * Advance physics by the configured fixed timestep, then update all active grippers + * so held pieces track the robot. + * + *

Call once per robot loop iteration from {@code simulationPeriodic()}. + */ + public void step() { + world.step(); + for (GamePieceGripper g : grippers) g.update(); + } + + // AdvantageScope output + + /** + * Return the current {@link Pose3d} of every game piece matching {@code variant}. + * + *

Log the returned array to AdvantageScope via AdvantageKit or NT4: + * 

{@code
+     * Logger.recordOutput("FieldSimulation/NotePoses", sim.getGamePiecePoses("Note"));
+     * }
+ * + * @param variant the variant name to filter by (case-sensitive) + * @return array of poses, one per matching piece, in spawn order + */ + public Pose3d[] getGamePiecePoses(String variant) { + List poses = new ArrayList<>(); + for (GamePiece p : pieces) { + if (p.getVariant().equals(variant)) poses.add(p.getPose()); + } + return poses.toArray(new Pose3d[0]); + } + + /** + * Return all game piece poses grouped by variant. Useful when multiple game + * piece types exist on the field simultaneously. + * + * @return map from variant name to pose array, in spawn order within each variant + */ + public Map getAllGamePiecePoses() { + Map> grouped = new LinkedHashMap<>(); + for (GamePiece p : pieces) { + grouped.computeIfAbsent(p.getVariant(), k -> new ArrayList<>()).add(p.getPose()); + } + Map result = new LinkedHashMap<>(); + grouped.forEach((k, v) -> result.put(k, v.toArray(new Pose3d[0]))); + return result; + } + + /** + * Unmodifiable view of all game pieces currently in the simulation. + * + * @return all game pieces in spawn order + */ + public List getGamePieces() { + return Collections.unmodifiableList(pieces); + } + + /** + * Direct access to the underlying {@link SimWorld} for advanced use — custom force + * generators, additional bodies, sensors, etc. + * + * @return the underlying physics world + */ + public SimWorld getWorld() { return world; } +} diff --git a/vendordep/src/main/java/jsim/field/GamePiece.java b/vendordep/src/main/java/jsim/field/GamePiece.java new file mode 100644 index 0000000..0145858 --- /dev/null +++ b/vendordep/src/main/java/jsim/field/GamePiece.java @@ -0,0 +1,57 @@ +package jsim.field; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Translation3d; +import jsim.api.SimBody; + +/** + * A dynamic game piece backed by a {@link SimBody}, with a named variant that + * maps to an AdvantageScope gamepiece model (e.g. {@code "Note"}, {@code "Coral"}). + * + *

Obtain instances from {@link FieldSimulator#spawnGamePiece}. All physics state + * (pose, velocity, mass, collider) is managed through the underlying {@link SimBody}. + */ +public final class GamePiece { + private final SimBody body; + private final String variant; + private boolean held; + + GamePiece(SimBody body, String variant) { + this.body = body; + this.variant = variant; + } + + /** + * AdvantageScope gamepiece variant name. Must match the key used in the + * AdvantageScope asset configuration (e.g. {@code "Note"} for 2024 Crescendo, + * {@code "Coral"} for 2025 Reefscape). + * + * @return the variant name + */ + public String getVariant() { return variant; } + + /** + * The underlying physics body. Use this for direct velocity/force manipulation + * or to attach force generators via {@link jsim.api.SimWorld#addForceGenerator}. + * + * @return the backing SimBody + */ + public SimBody getBody() { return body; } + + /** @return current world-frame pose */ + public Pose3d getPose() { return body.getPose(); } + + /** @return current linear velocity in world frame (m/s) */ + public Translation3d getLinearVelocity() { return body.getLinearVelocity(); } + + /** + * Whether this game piece is currently attached to a {@link GamePieceGripper}. + * While held, the piece follows the robot's intake point and does not respond + * to gravity or collisions independently. + * + * @return {@code true} if held by a gripper + */ + public boolean isHeld() { return held; } + + void setHeld(boolean held) { this.held = held; } +} diff --git a/vendordep/src/main/java/jsim/field/GamePieceGripper.java b/vendordep/src/main/java/jsim/field/GamePieceGripper.java new file mode 100644 index 0000000..5bc0de5 --- /dev/null +++ b/vendordep/src/main/java/jsim/field/GamePieceGripper.java @@ -0,0 +1,123 @@ +package jsim.field; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Translation3d; +import jsim.api.SimBody; + +/** + * Attaches a {@link GamePiece} to a robot body's intake point so it is carried + * rigidly in robot-local space each tick. + * + *

While a game piece is held, its pose and linear velocity are overridden each + * tick (by {@link #update()}) to track the robot. The piece's physics body is not + * removed from the world — it just gets corrected immediately after every physics + * step so it always appears at the intake position. + * + *

Obtain instances from {@link FieldSimulator#createGripper}. {@link #update()} + * is called automatically by {@link FieldSimulator#step()}, so manual calls are + * only needed when using the gripper with a raw {@link jsim.api.SimWorld}. + * + *

Example: + * 

{@code
+ * GamePieceGripper gripper = sim.createGripper(robot, new Translation3d(0.45, 0, 0.1));
+ *
+ * // Robot drives up to a note and intakes it
+ * gripper.grab(note);
+ *
+ * // ... robot drives to the speaker, then shoots ...
+ * gripper.eject(0, 0, 8);   // launch straight up at 8 m/s
+ * }
+ */ +public final class GamePieceGripper { + private final SimBody robot; + /** Hold-point offset in the robot's local frame. */ + private final Translation3d intakeOffset; + private GamePiece held; + + GamePieceGripper(SimBody robot, Translation3d intakeOffset) { + this.robot = robot; + this.intakeOffset = intakeOffset; + } + + /** + * Attach a game piece to this gripper. The piece is immediately snapped to the + * intake position and its velocity is matched to the robot's. + * + *

Returns {@code false} (no-op) if: + *

    + *
  • this gripper is already holding a piece, or + *
  • {@code piece} is already held by another gripper. + *
+ * + * @param piece the game piece to grab + * @return {@code true} if the grab succeeded + */ + public boolean grab(GamePiece piece) { + if (held != null || piece.isHeld()) return false; + held = piece; + piece.setHeld(true); + snapToIntake(); + return true; + } + + /** + * Release the held piece, giving it the robot's current linear velocity. + * No-op if nothing is held. + */ + public void release() { + if (held == null) return; + held.getBody().setLinearVelocity(robot.getLinearVelocity()); + held.getBody().setAngularVelocity(new Translation3d()); + held.setHeld(false); + held = null; + } + + /** + * Eject the held piece with a custom world-frame velocity. Use this to model + * a shooter or roller that launches the piece at a specific velocity. + * No-op if nothing is held. + * + * @param vx ejection velocity X component (m/s) + * @param vy ejection velocity Y component (m/s) + * @param vz ejection velocity Z component (m/s) + */ + public void eject(double vx, double vy, double vz) { + if (held == null) return; + held.getBody().setLinearVelocity(vx, vy, vz); + held.getBody().setAngularVelocity(new Translation3d()); + held.setHeld(false); + held = null; + } + + /** + * Teleport the held piece to the current intake position and match its velocity + * to the robot. Called automatically by {@link FieldSimulator#step()} after + * each physics tick. + */ + void update() { + if (held == null) return; + snapToIntake(); + } + + /** @return the currently held game piece, or {@code null} if nothing is held */ + public GamePiece getHeld() { return held; } + + /** @return {@code true} if this gripper is currently holding a piece */ + public boolean isHolding() { return held != null; } + + // Internals + + private void snapToIntake() { + held.getBody().setPose(holdPose()); + held.getBody().setLinearVelocity(robot.getLinearVelocity()); + held.getBody().setAngularVelocity(new Translation3d()); + } + + private Pose3d holdPose() { + Pose3d robotPose = robot.getPose(); + // Rotate the local-frame intake offset into world frame, then add to robot position. + Translation3d worldOffset = intakeOffset.rotateBy(robotPose.getRotation()); + Translation3d worldPos = robotPose.getTranslation().plus(worldOffset); + return new Pose3d(worldPos, robotPose.getRotation()); + } +} diff --git a/vendordep/src/test/java/jsim/field/FieldLayoutTest.java b/vendordep/src/test/java/jsim/field/FieldLayoutTest.java new file mode 100644 index 0000000..fd3e1ae --- /dev/null +++ b/vendordep/src/test/java/jsim/field/FieldLayoutTest.java @@ -0,0 +1,148 @@ +package jsim.field; + +import static org.junit.jupiter.api.Assertions.*; +import jsim.api.SimBody; +import jsim.api.SimBodyBuilder; +import jsim.api.SimWorld; +import jsim.material.Material; +import org.junit.jupiter.api.Test; + +class FieldLayoutTest { + + @Test + void addFrcField_addsFiveStaticBodies() { + SimWorld world = new SimWorld(0.02, 10); + int before = world.getBodies().size(); + FieldLayout.addFrcField(world); + // floor + 4 walls = 5 bodies + assertEquals(before + 5, world.getBodies().size()); + } + + @Test + void allFieldBodiesAreStatic() { + SimWorld world = new SimWorld(0.02, 10); + FieldLayout.addFrcField(world); + for (SimBody b : world.getBodies()) { + assertTrue(b.isStatic(), "Field body '" + b.getName() + "' must be static"); + } + } + + @Test + void fieldBodiesDoNotMoveAfterSteps() { + SimWorld world = new SimWorld(0.02, 10); + FieldLayout.addFrcField(world); + + // Record initial positions + double[] xs = world.getBodies().stream().mapToDouble(b -> b.getPosition().getX()).toArray(); + double[] ys = world.getBodies().stream().mapToDouble(b -> b.getPosition().getY()).toArray(); + double[] zs = world.getBodies().stream().mapToDouble(b -> b.getPosition().getZ()).toArray(); + + for (int i = 0; i < 100; i++) world.step(); + + int i = 0; + for (SimBody b : world.getBodies()) { + assertEquals(xs[i], b.getPosition().getX(), 1e-9, + b.getName() + " X must not change"); + assertEquals(ys[i], b.getPosition().getY(), 1e-9, + b.getName() + " Y must not change"); + assertEquals(zs[i], b.getPosition().getZ(), 1e-9, + b.getName() + " Z must not change"); + i++; + } + } + + @Test + void floorPreventsSphereFallingThrough() { + SimWorld world = new SimWorld(0.02, 10); + FieldLayout.addFrcField(world); + + SimBody ball = world.addBody(new SimBodyBuilder("Ball") + .position(8, 4, 2) + .mass(0.235) + .sphereCollider(0.18) + .material(Material.RUBBER)); + + for (int i = 0; i < 200; i++) world.step(); + + assertTrue(ball.getPosition().getZ() >= -0.1, + "Ball Z must not go far below floor: " + ball.getPosition().getZ()); + } + + @Test + void redWallStopsFastProjectile() { + SimWorld world = new SimWorld(0.02, 10); + FieldLayout.addFrcField(world); + + // Launch a sphere toward the red (high-X) wall at very high speed + SimBody ball = world.addBody(new SimBodyBuilder("Ball") + .position(8, 4, 0.25) + .linearVelocity(30, 0, 0) + .mass(0.235) + .sphereCollider(0.18) + .noGravity() + .material(Material.RUBBER)); + + for (int i = 0; i < 200; i++) world.step(); + + assertTrue(ball.getPosition().getX() < FieldLayout.FRC_LENGTH_M + 1.0, + "Ball must not escape past the red wall: x=" + ball.getPosition().getX()); + } + + @Test + void blueWallStopsFastProjectile() { + SimWorld world = new SimWorld(0.02, 10); + FieldLayout.addFrcField(world); + + SimBody ball = world.addBody(new SimBodyBuilder("Ball") + .position(8, 4, 0.25) + .linearVelocity(-30, 0, 0) + .mass(0.235) + .sphereCollider(0.18) + .noGravity() + .material(Material.RUBBER)); + + for (int i = 0; i < 200; i++) world.step(); + + assertTrue(ball.getPosition().getX() > -1.0, + "Ball must not escape past the blue wall: x=" + ball.getPosition().getX()); + } + + @Test + void nearAndFarWallsContainBall() { + SimWorld world = new SimWorld(0.02, 10); + FieldLayout.addFrcField(world); + + // Launch diagonally toward near-Y wall then far-Y wall + SimBody ball = world.addBody(new SimBodyBuilder("Ball") + .position(8, 4, 0.25) + .linearVelocity(0, 30, 0) + .mass(0.235) + .sphereCollider(0.18) + .noGravity() + .material(Material.RUBBER)); + + for (int i = 0; i < 200; i++) world.step(); + + assertTrue(ball.getPosition().getY() < FieldLayout.FRC_WIDTH_M + 1.0, + "Ball must not escape past far wall: y=" + ball.getPosition().getY()); + } + + @Test + void addField_customDimensions() { + SimWorld world = new SimWorld(0.02, 10); + FieldLayout.addField(world, 10.0, 5.0); + // Should still add 5 bodies without throwing + assertEquals(5, world.getBodies().size()); + for (SimBody b : world.getBodies()) { + assertTrue(b.isStatic()); + } + } + + @Test + void addFloor_addsOnlyOneBody() { + SimWorld world = new SimWorld(0.02, 10); + FieldLayout.addFloor(world); + assertEquals(1, world.getBodies().size()); + assertTrue(world.getBodies().get(0).isStatic()); + } +} diff --git a/vendordep/src/test/java/jsim/field/FieldSimulatorTest.java b/vendordep/src/test/java/jsim/field/FieldSimulatorTest.java new file mode 100644 index 0000000..f6eb92d --- /dev/null +++ b/vendordep/src/test/java/jsim/field/FieldSimulatorTest.java @@ -0,0 +1,197 @@ +package jsim.field; + +import static org.junit.jupiter.api.Assertions.*; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import jsim.api.SimBody; +import jsim.api.SimBodyBuilder; +import jsim.material.Material; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import java.util.Map; + +class FieldSimulatorTest { + private FieldSimulator sim; + + @BeforeEach + void setUp() { + sim = new FieldSimulator(); + } + + @Test + void defaultConstructorCreatesWorkingWorld() { + assertNotNull(sim.getWorld()); + // 5 field bodies (floor + 4 walls) should already exist + assertTrue(sim.getWorld().getBodies().size() >= 5); + } + + @Test + void gamePieceFallsAndStaysAboveFloor() { + sim.spawnGamePiece("Note", new SimBodyBuilder("Note0") + .position(8, 4, 3) + .mass(0.235) + .sphereCollider(0.18) + .material(Material.RUBBER)); + + for (int i = 0; i < 200; i++) sim.step(); + + GamePiece note = sim.getGamePieces().get(0); + assertTrue(note.getPose().getZ() >= -0.1, + "Note Z must stay above floor: " + note.getPose().getZ()); + } + + @Test + void gamePieceBouncesOffRedWall() { + GamePiece note = sim.spawnGamePiece("Note", new SimBodyBuilder("Note0") + .position(8, 4, 0.18) + .linearVelocity(30, 0, 0) + .mass(0.235) + .sphereCollider(0.18) + .noGravity() + .material(new Material(0.0, 0.5))); + + for (int i = 0; i < 200; i++) sim.step(); + + assertTrue(note.getPose().getX() < FieldLayout.FRC_LENGTH_M + 1.0, + "Note must not escape past red wall: x=" + note.getPose().getX()); + } + + @Test + void gamePieceBouncesOffBlueWall() { + GamePiece note = sim.spawnGamePiece("Note", new SimBodyBuilder("Note0") + .position(8, 4, 0.18) + .linearVelocity(-30, 0, 0) + .mass(0.235) + .sphereCollider(0.18) + .noGravity() + .material(new Material(0.0, 0.5))); + + for (int i = 0; i < 200; i++) sim.step(); + + assertTrue(note.getPose().getX() > -1.0, + "Note must not escape past blue wall: x=" + note.getPose().getX()); + } + + @Test + void getGamePiecePoses_returnsCorrectCount() { + sim.spawnGamePiece("Note", new SimBodyBuilder("N0").position(2, 4, 0.18) + .mass(0.235).sphereCollider(0.18).material(Material.RUBBER)); + sim.spawnGamePiece("Note", new SimBodyBuilder("N1").position(4, 4, 0.18) + .mass(0.235).sphereCollider(0.18).material(Material.RUBBER)); + sim.spawnGamePiece("Note", new SimBodyBuilder("N2").position(6, 4, 0.18) + .mass(0.235).sphereCollider(0.18).material(Material.RUBBER)); + + assertEquals(3, sim.getGamePiecePoses("Note").length); + } + + @Test + void getGamePiecePoses_filtersVariant() { + sim.spawnGamePiece("Note", new SimBodyBuilder("N0").position(2, 4, 0.18) + .mass(0.235).sphereCollider(0.18).material(Material.RUBBER)); + sim.spawnGamePiece("Note", new SimBodyBuilder("N1").position(4, 4, 0.18) + .mass(0.235).sphereCollider(0.18).material(Material.RUBBER)); + sim.spawnGamePiece("Coral", new SimBodyBuilder("C0").position(6, 4, 0.18) + .mass(0.26).sphereCollider(0.15).material(Material.RUBBER)); + + assertEquals(2, sim.getGamePiecePoses("Note").length); + assertEquals(1, sim.getGamePiecePoses("Coral").length); + assertEquals(0, sim.getGamePiecePoses("Algae").length); + } + + @Test + void getAllGamePiecePoses_groupsByVariant() { + sim.spawnGamePiece("Note", new SimBodyBuilder("N0").position(2, 4, 0.18) + .mass(0.235).sphereCollider(0.18).material(Material.RUBBER)); + sim.spawnGamePiece("Coral", new SimBodyBuilder("C0").position(6, 4, 0.18) + .mass(0.26).sphereCollider(0.15).material(Material.RUBBER)); + + Map all = sim.getAllGamePiecePoses(); + assertEquals(2, all.size()); + assertEquals(1, all.get("Note").length); + assertEquals(1, all.get("Coral").length); + } + + @Test + void removeGamePiece_reducesWorldBodyCount() { + GamePiece note = sim.spawnGamePiece("Note", new SimBodyBuilder("N0") + .position(8, 4, 0.18).mass(0.235).sphereCollider(0.18).material(Material.RUBBER)); + + int before = sim.getWorld().getBodies().size(); + sim.removeGamePiece(note); + + assertEquals(before - 1, sim.getWorld().getBodies().size()); + assertEquals(0, sim.getGamePieces().size()); + } + + @Test + void removeGamePiece_releasesFromGripper() { + SimBody robot = sim.addRobot(new SimBodyBuilder("Robot") + .position(0, 0, 0.15).mass(50).boxCollider(0.45, 0.45, 0.15) + .noGravity().fixedRotation().material(Material.CARPET)); + + GamePieceGripper gripper = sim.createGripper(robot, new Translation3d(0.5, 0, 0)); + + GamePiece note = sim.spawnGamePiece("Note", new SimBodyBuilder("N0") + .position(0.5, 0, 0.15).mass(0.235).sphereCollider(0.18).material(Material.RUBBER)); + + gripper.grab(note); + assertTrue(gripper.isHolding()); + + sim.removeGamePiece(note); + assertFalse(gripper.isHolding()); + } + + @Test + void gripperAndStepIntegration() { + SimBody robot = sim.addRobot(new SimBodyBuilder("Robot") + .position(0, 0, 0.15).mass(50).boxCollider(0.45, 0.45, 0.15) + .noGravity().fixedRotation().material(Material.CARPET)); + + GamePieceGripper gripper = sim.createGripper(robot, new Translation3d(0.5, 0, 0)); + + GamePiece note = sim.spawnGamePiece("Note", new SimBodyBuilder("N0") + .position(0.5, 0, 0.15).mass(0.235).sphereCollider(0.18).material(Material.RUBBER)); + + gripper.grab(note); + + // Move robot; step should teleport the note to follow + robot.setPose(new Pose3d(new Translation3d(3, 2, 0.15), new Rotation3d())); + sim.step(); + + assertEquals(3.5, note.getPose().getX(), 0.01, "Note should follow robot in X"); + assertEquals(2.0, note.getPose().getY(), 0.01, "Note should follow robot in Y"); + + // Eject upward + gripper.eject(0, 0, 5.0); + assertFalse(note.isHeld()); + assertEquals(5.0, note.getLinearVelocity().getZ(), 1e-6); + } + + @Test + void gamePiecePosesReflectPhysics() { + sim.spawnGamePiece("Note", new SimBodyBuilder("N0") + .position(8, 4, 2).mass(0.235).sphereCollider(0.18).material(Material.RUBBER)); + + Pose3d[] before = sim.getGamePiecePoses("Note"); + for (int i = 0; i < 10; i++) sim.step(); + Pose3d[] after = sim.getGamePiecePoses("Note"); + + // After 10 ticks with gravity, the note should have fallen + assertTrue(after[0].getZ() < before[0].getZ(), + "Note should have fallen under gravity"); + } + + @Test + void customTimestep() { + FieldSimulator custom = new FieldSimulator(0.005); + assertEquals(0.005, custom.getWorld().getTimestep(), 1e-12); + } + + @Test + void customFieldDimensions() { + FieldSimulator custom = new FieldSimulator(0.02, 10.0, 5.0); + // Should create world without throwing and have 5 field bodies + assertTrue(custom.getWorld().getBodies().size() >= 5); + } +} diff --git a/vendordep/src/test/java/jsim/field/GamePieceGripperTest.java b/vendordep/src/test/java/jsim/field/GamePieceGripperTest.java new file mode 100644 index 0000000..5b3845f --- /dev/null +++ b/vendordep/src/test/java/jsim/field/GamePieceGripperTest.java @@ -0,0 +1,168 @@ +package jsim.field; + +import static org.junit.jupiter.api.Assertions.*; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import jsim.api.SimBody; +import jsim.api.SimBodyBuilder; +import jsim.api.SimWorld; +import jsim.material.Material; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +class GamePieceGripperTest { + private SimWorld world; + private SimBody robot; + private GamePiece piece; + private GamePieceGripper gripper; + + @BeforeEach + void setUp() { + world = new SimWorld(0.02, 10); + + robot = world.addBody(new SimBodyBuilder("Robot") + .position(0, 0, 0.15) + .mass(50) + .boxCollider(0.45, 0.45, 0.15) + .noGravity().fixedRotation() + .material(Material.CARPET)); + + SimBody pieceBody = world.addBody(new SimBodyBuilder("Note") + .position(5, 0, 0.18) + .mass(0.235) + .sphereCollider(0.18) + .material(Material.RUBBER)); + + piece = new GamePiece(pieceBody, "Note"); + gripper = new GamePieceGripper(robot, new Translation3d(0.5, 0, 0)); + } + + @Test + void initiallyNotHolding() { + assertFalse(gripper.isHolding()); + assertNull(gripper.getHeld()); + assertFalse(piece.isHeld()); + } + + @Test + void grabAttachesPiece() { + assertTrue(gripper.grab(piece)); + assertTrue(gripper.isHolding()); + assertSame(piece, gripper.getHeld()); + assertTrue(piece.isHeld()); + } + + @Test + void grabSnapsToIntakePosition() { + gripper.grab(piece); + // Intake offset is (0.5, 0, 0) in robot-local frame; robot is at (0,0,0.15) facing +X + Pose3d pose = piece.getPose(); + assertEquals(0.5, pose.getX(), 1e-6, "piece X should equal robot X + intake offset"); + assertEquals(0.0, pose.getY(), 1e-6); + assertEquals(0.15, pose.getZ(), 1e-6); + } + + @Test + void grabMatchesRobotVelocity() { + robot.setLinearVelocity(2.0, 1.0, 0); + gripper.grab(piece); + Translation3d pv = piece.getLinearVelocity(); + assertEquals(2.0, pv.getX(), 1e-6); + assertEquals(1.0, pv.getY(), 1e-6); + assertEquals(0.0, pv.getZ(), 1e-6); + } + + @Test + void grabReturnsFalseIfAlreadyHolding() { + SimBody otherBody = world.addBody(new SimBodyBuilder("Other") + .position(3, 0, 0.18).mass(0.235).sphereCollider(0.18).material(Material.RUBBER)); + GamePiece other = new GamePiece(otherBody, "Note"); + + gripper.grab(piece); + assertFalse(gripper.grab(other), "Second grab should fail while already holding"); + assertFalse(other.isHeld()); + } + + @Test + void grabReturnsFalseIfPieceAlreadyHeld() { + GamePieceGripper gripper2 = new GamePieceGripper(robot, new Translation3d(-0.5, 0, 0)); + gripper.grab(piece); + assertFalse(gripper2.grab(piece), "Cannot grab a piece already held elsewhere"); + } + + @Test + void releaseDetachesPiece() { + gripper.grab(piece); + gripper.release(); + assertFalse(gripper.isHolding()); + assertNull(gripper.getHeld()); + assertFalse(piece.isHeld()); + } + + @Test + void releaseGivesRobotVelocity() { + robot.setLinearVelocity(3.0, 0.5, 0); + gripper.grab(piece); + gripper.release(); + Translation3d pv = piece.getLinearVelocity(); + assertEquals(3.0, pv.getX(), 1e-6); + assertEquals(0.5, pv.getY(), 1e-6); + } + + @Test + void releaseNoOpIfNotHolding() { + assertDoesNotThrow(() -> gripper.release()); + assertFalse(gripper.isHolding()); + } + + @Test + void ejectSetsCustomVelocity() { + gripper.grab(piece); + gripper.eject(0, 0, 8.0); + assertFalse(piece.isHeld()); + assertNull(gripper.getHeld()); + Translation3d pv = piece.getLinearVelocity(); + assertEquals(0.0, pv.getX(), 1e-6); + assertEquals(0.0, pv.getY(), 1e-6); + assertEquals(8.0, pv.getZ(), 1e-6); + } + + @Test + void ejectNoOpIfNotHolding() { + assertDoesNotThrow(() -> gripper.eject(0, 0, 8)); + } + + @Test + void updateTeleportsPieceWhenRobotMoves() { + gripper.grab(piece); + + // Move robot to a new position + robot.setPose(new Pose3d(new Translation3d(4, 2, 0.15), new Rotation3d())); + gripper.update(); + + // Piece should be at robot + (0.5, 0, 0) + assertEquals(4.5, piece.getPose().getX(), 1e-6, "Piece X should track robot"); + assertEquals(2.0, piece.getPose().getY(), 1e-6); + assertEquals(0.15, piece.getPose().getZ(), 1e-6); + } + + @Test + void updateNoOpIfNotHolding() { + assertDoesNotThrow(() -> gripper.update()); + } + + @Test + void intakeOffsetRotatesWithRobot() { + // Rotate robot 90° about Z so its +X axis becomes world +Y + robot.setPose(new Pose3d( + new Translation3d(0, 0, 0.15), + new Rotation3d(0, 0, Math.PI / 2))); + + gripper.grab(piece); + + // Intake offset (0.5, 0, 0) in robot frame → (0, 0.5, 0) in world frame + assertEquals(0.0, piece.getPose().getX(), 1e-6, "X should stay at 0"); + assertEquals(0.5, piece.getPose().getY(), 1e-6, "Y should be 0.5 (rotated offset)"); + } +} From 30afed1fbe79b93fa5c928e2586dcd4ef6250291 Mon Sep 17 00:00:00 2001 From: Ruthie-FRC Date: Fri, 3 Jul 2026 13:47:34 -0500 Subject: [PATCH 3/5] attempts --- .../src/main/java/jsim/field/FieldLayout.java | 86 +++++++++---------- .../test/java/jsim/field/FieldLayoutTest.java | 11 ++- 2 files changed, 49 insertions(+), 48 deletions(-) diff --git a/vendordep/src/main/java/jsim/field/FieldLayout.java b/vendordep/src/main/java/jsim/field/FieldLayout.java index f5c5369..4079da2 100644 --- a/vendordep/src/main/java/jsim/field/FieldLayout.java +++ b/vendordep/src/main/java/jsim/field/FieldLayout.java @@ -6,13 +6,17 @@ /** * Utility for populating a {@link SimWorld} with FRC field geometry: a carpet floor - * (infinite plane) and four perimeter wall segments (box colliders). + * and four perimeter wall planes. * *

All bodies added by this class are static — they deflect game pieces and robots * but cannot themselves be moved. * - *

Coordinate frame: X points from the blue alliance wall toward the red alliance wall; - * Y points across the field; Z points up. This matches AdvantageScope's field rendering. + *

Coordinate frame: X points from the blue alliance wall toward the red alliance + * wall; Y points across the field; Z points up. This matches AdvantageScope's field rendering. + * + *

Wall implementation: Each wall is an infinite half-space {@link jsim.collision.PlaneCollider}. + * Infinite planes are always in the broadphase and use signed-distance narrowphase, so they are + * completely immune to tunneling even when game pieces are launched at very high velocities. */ public final class FieldLayout { /** FRC 2025/2026 field length (blue to red) in metres (54 ft). */ @@ -20,13 +24,10 @@ public final class FieldLayout { /** FRC 2025/2026 field width in metres (27 ft). */ public static final double FRC_WIDTH_M = 8.2296; - private static final double WALL_THICK = 0.05; - private static final double WALL_HEIGHT = 0.5; - private FieldLayout() {} /** - * Add a standard FRC field (carpet floor + four perimeter walls) to {@code world}. + * Add a standard FRC field (carpet floor + four perimeter wall planes) to {@code world}. * * @param world the world to populate */ @@ -35,7 +36,8 @@ public static void addFrcField(SimWorld world) { } /** - * Add a configurable rectangular field (carpet floor + four perimeter walls) to {@code world}. + * Add a configurable rectangular field (carpet floor + four perimeter wall planes) to + * {@code world}. * * @param world the world to populate * @param lengthM field length along X (metres) @@ -43,7 +45,7 @@ public static void addFrcField(SimWorld world) { */ public static void addField(SimWorld world, double lengthM, double widthM) { addFloor(world); - addWalls(world, lengthM, widthM, WALL_THICK, WALL_HEIGHT); + addWalls(world, lengthM, widthM); } /** @@ -58,53 +60,43 @@ public static void addFloor(SimWorld world) { } /** - * Add four perimeter wall segments to {@code world}. + * Add four perimeter wall planes to {@code world}. * - *

The blue and red end walls are extended in Y to cover the field corners, - * so no gap exists at any corner of the boundary. + *

Each wall is a {@link jsim.collision.PlaneCollider} (infinite half-space) anchored + * at the corresponding field boundary and oriented so its normal points into the field. + * Plane colliders have unbounded AABBs and use signed-distance collision tests, making + * them immune to tunneling at any projectile speed. * - * @param world the world to populate - * @param lengthM field length along X (metres) - * @param widthM field width along Y (metres) - * @param thickM wall thickness (metres) - * @param heightM wall height above the floor (metres) + * @param world the world to populate + * @param lengthM field length along X (metres) + * @param widthM field width along Y (metres) */ - public static void addWalls(SimWorld world, - double lengthM, double widthM, - double thickM, double heightM) { - double t2 = thickM / 2.0; - double h2 = heightM / 2.0; - double l2 = lengthM / 2.0; - double w2 = widthM / 2.0; - - // Blue alliance wall — inner face at x = 0, box extends into x < 0. - // halfY is w2+thickM so it overlaps the adjacent Near/Far wall thickness, - // closing the corner gap. + public static void addWalls(SimWorld world, double lengthM, double widthM) { + // Blue alliance wall — inner face at x = 0, normal points +X into field. + // Blocks the half-space x < 0. world.addBody(new SimBodyBuilder("Field/WallBlue") - .position(-t2, w2, h2) - .boxCollider(t2, w2 + thickM, h2) - .material(Material.WALL) - .isStatic()); + .planeCollider(1, 0, 0, 0) + .material(Material.WALL)); - // Red alliance wall — inner face at x = lengthM. + // Red alliance wall — inner face at x = lengthM, normal points -X into field. + // Body is placed at (lengthM, 0, 0); offset = 0, so the plane passes through + // that point. Blocks the half-space x > lengthM. world.addBody(new SimBodyBuilder("Field/WallRed") - .position(lengthM + t2, w2, h2) - .boxCollider(t2, w2 + thickM, h2) - .material(Material.WALL) - .isStatic()); + .position(lengthM, 0, 0) + .planeCollider(-1, 0, 0, 0) + .material(Material.WALL)); - // Near wall — inner face at y = 0, box extends into y < 0. + // Near wall — inner face at y = 0, normal points +Y into field. + // Blocks the half-space y < 0. world.addBody(new SimBodyBuilder("Field/WallNear") - .position(l2, -t2, h2) - .boxCollider(l2, t2, h2) - .material(Material.WALL) - .isStatic()); + .planeCollider(0, 1, 0, 0) + .material(Material.WALL)); - // Far wall — inner face at y = widthM. + // Far wall — inner face at y = widthM, normal points -Y into field. + // Blocks the half-space y > widthM. world.addBody(new SimBodyBuilder("Field/WallFar") - .position(l2, widthM + t2, h2) - .boxCollider(l2, t2, h2) - .material(Material.WALL) - .isStatic()); + .position(0, widthM, 0) + .planeCollider(0, -1, 0, 0) + .material(Material.WALL)); } } diff --git a/vendordep/src/test/java/jsim/field/FieldLayoutTest.java b/vendordep/src/test/java/jsim/field/FieldLayoutTest.java index fd3e1ae..beb64ca 100644 --- a/vendordep/src/test/java/jsim/field/FieldLayoutTest.java +++ b/vendordep/src/test/java/jsim/field/FieldLayoutTest.java @@ -131,13 +131,22 @@ void nearAndFarWallsContainBall() { void addField_customDimensions() { SimWorld world = new SimWorld(0.02, 10); FieldLayout.addField(world, 10.0, 5.0); - // Should still add 5 bodies without throwing assertEquals(5, world.getBodies().size()); for (SimBody b : world.getBodies()) { assertTrue(b.isStatic()); } } + @Test + void addWalls_addsExactlyFourBodies() { + SimWorld world = new SimWorld(0.02, 10); + FieldLayout.addWalls(world, 16.0, 8.0); + assertEquals(4, world.getBodies().size()); + for (SimBody b : world.getBodies()) { + assertTrue(b.isStatic()); + } + } + @Test void addFloor_addsOnlyOneBody() { SimWorld world = new SimWorld(0.02, 10); From 739696d1320e617f5f62839e079c5cb8e652fbb1 Mon Sep 17 00:00:00 2001 From: Ruthie-FRC Date: Tue, 7 Jul 2026 14:30:50 -0500 Subject: [PATCH 4/5] stuff and things --- mkdocs/docs/walkthroughs/gamepiece_simulation.md | 2 +- vendordep/src/main/java/jsim/field/GamePieceGripper.java | 6 +++--- vendordep/src/test/java/jsim/field/FieldSimulatorTest.java | 1 + 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/mkdocs/docs/walkthroughs/gamepiece_simulation.md b/mkdocs/docs/walkthroughs/gamepiece_simulation.md index 030f548..35beb01 100644 --- a/mkdocs/docs/walkthroughs/gamepiece_simulation.md +++ b/mkdocs/docs/walkthroughs/gamepiece_simulation.md @@ -10,7 +10,7 @@ simWorld.addBody(new SimBodyBuilder("Floor") .planeCollider(0, 0, 1, 0) .material(Material.CARPET)); -// 2025 Crescendo Note — 36 cm diameter ring, ~0.23 kg +// 2024 Crescendo Note — 36 cm diameter ring, ~0.23 kg SimBody note = simWorld.addBody(new SimBodyBuilder("Note") .position(4.5, 2.0, 0.18) // 18 cm off the floor (radius) .mass(0.235) diff --git a/vendordep/src/main/java/jsim/field/GamePieceGripper.java b/vendordep/src/main/java/jsim/field/GamePieceGripper.java index 5bc0de5..2b055e8 100644 --- a/vendordep/src/main/java/jsim/field/GamePieceGripper.java +++ b/vendordep/src/main/java/jsim/field/GamePieceGripper.java @@ -21,11 +21,11 @@ * 

{@code
  * GamePieceGripper gripper = sim.createGripper(robot, new Translation3d(0.45, 0, 0.1));
  *
- * // Robot drives up to a note and intakes it
- * gripper.grab(note);
+ * // Robot drives up to a game piece and intakes it
+ * gripper.grab(gamePiece);
  *
  * // ... robot drives to the speaker, then shoots ...
- * gripper.eject(0, 0, 8);   // launch straight up at 8 m/s
+ * gripper.eject(0, 0, 8);   launch straight up at 8 m/s
  * }
*/ public final class GamePieceGripper { diff --git a/vendordep/src/test/java/jsim/field/FieldSimulatorTest.java b/vendordep/src/test/java/jsim/field/FieldSimulatorTest.java index f6eb92d..0f849c0 100644 --- a/vendordep/src/test/java/jsim/field/FieldSimulatorTest.java +++ b/vendordep/src/test/java/jsim/field/FieldSimulatorTest.java @@ -113,6 +113,7 @@ void getAllGamePiecePoses_groupsByVariant() { } @Test + void removeGamePiece_reducesWorldBodyCount() { GamePiece note = sim.spawnGamePiece("Note", new SimBodyBuilder("N0") .position(8, 4, 0.18).mass(0.235).sphereCollider(0.18).material(Material.RUBBER)); From d5df1c989a5bfaa3928cfacea5f43a940f7be6ca Mon Sep 17 00:00:00 2001 From: Ruthie-FRC Date: Thu, 9 Jul 2026 13:36:30 -0500 Subject: [PATCH 5/5] Implement intake and eject commands for game pieces in SwerveSubsystem; add field loader and 2025 Reefscape field definition --- .../main/java/frc/robot/RobotContainer.java | 4 + .../frc/robot/subsystems/SwerveSubsystem.java | 107 ++++++++++-- .../src/main/java/jsim/field/FieldLoader.java | 159 ++++++++++++++++++ .../resources/jsim/field/2025-reefscape.json | 66 ++++++++ 4 files changed, 319 insertions(+), 17 deletions(-) create mode 100644 vendordep/src/main/java/jsim/field/FieldLoader.java create mode 100644 vendordep/src/main/resources/jsim/field/2025-reefscape.json diff --git a/examples/java/simple_robot/src/main/java/frc/robot/RobotContainer.java b/examples/java/simple_robot/src/main/java/frc/robot/RobotContainer.java index 6c17c0e..833ad56 100644 --- a/examples/java/simple_robot/src/main/java/frc/robot/RobotContainer.java +++ b/examples/java/simple_robot/src/main/java/frc/robot/RobotContainer.java @@ -42,6 +42,10 @@ private void configureBindings() Meters.of(6), Rotation2d.fromDegrees(70)))); + // A — grab the nearest free game piece within intake range. + // B — eject / shoot the held piece forward with a loft. + xboxController.a().onTrue(drive.intakeNearest()); + xboxController.b().onTrue(drive.ejectPiece()); } public Command getAutonomousCommand() diff --git a/examples/java/simple_robot/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/examples/java/simple_robot/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 4ef687b..6511fbd 100644 --- a/examples/java/simple_robot/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/examples/java/simple_robot/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -23,8 +23,11 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructArrayPublisher; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearVelocity; @@ -35,7 +38,10 @@ import jsim.api.RobotId; import jsim.api.SimBody; import jsim.api.SimBodyBuilder; -import jsim.api.SimWorld; +import jsim.field.FieldLoader; +import jsim.field.FieldSimulator; +import jsim.field.GamePiece; +import jsim.field.GamePieceGripper; import jsim.material.Material; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -51,14 +57,26 @@ public class SwerveSubsystem extends SubsystemBase { + // Intake geometry — front-center of the robot, just above carpet. + private static final double INTAKE_FORWARD_M = 0.35; + private static final double INTAKE_HEIGHT_M = 0.05; + // Any free game piece whose centre is within this distance of the intake point is grabbable. + private static final double INTAKE_RADIUS_M = 0.55; + // Eject velocity: forward at 3.5 m/s, lofted at 1.5 m/s upward. + private static final double EJECT_SPEED_XY = 3.5; + private static final double EJECT_SPEED_Z = 1.5; + private final Pigeon2 gyro = new Pigeon2(14); private final SwerveDrive drive; private final Field2d field = new Field2d(); - // JSim physics world — owns the floor and the robot body. - private final SimWorld simWorld; - // Handle to the robot's physics body, tagged as Blue alliance station 1. - private final SimBody simBody; + // JSim field simulator — owns the FRC perimeter, robot body, and all game pieces. + private final FieldSimulator fieldSim; + private final SimBody simBody; + private final GamePieceGripper gripper; + // NT4 Pose3d[] publishers that AdvantageScope reads for game-piece 3D rendering. + private final StructArrayPublisher coralPublisher; + private final StructArrayPublisher algaePublisher; private AngularVelocity maximumChassisSpeedsAngularVelocity = DegreesPerSecond.of(360); private LinearVelocity maximumChassisSpeedsLinearVelocity = MetersPerSecond.of(1); @@ -162,21 +180,20 @@ public SwerveSubsystem() new Translation2d(Inches.of(-24).in(Meters), Inches.of(-24).in(Meters))); SwerveDriveConfig config = new SwerveDriveConfig(this, fl, fr, bl, br) .withGyro(gyro.getYaw().asSupplier()) - .withStartingPose(new Pose2d(0, 0, Rotation2d.fromDegrees(0))) + .withStartingPose(new Pose2d(2.0, 2.0, Rotation2d.fromDegrees(0))) .withTranslationController(new PIDController(1, 0, 0)) .withRotationController(new PIDController(1, 0, 0)); drive = new SwerveDrive(config); - // Build the JSim world: carpet floor + the robot body tagged as Blue 1. - simWorld = new SimWorld(); - simWorld.addBody(new SimBodyBuilder("Floor") - .planeCollider(0, 0, 1, 0) - .material(Material.CARPET)); + // Load the 2025 Reefscape field: perimeter walls, reef hitboxes, and pre-staged + // game pieces (Coral + Algae) from the bundled JSON resource. + fieldSim = FieldLoader.fromResource("2025-reefscape"); + // 24"×24" chassis (~0.6 m square), centre height 0.1 m, 54 kg typical swerve. // noGravity + fixedRotation: we drive kinematically by setting velocity each tick. - simBody = simWorld.addBody(new SimBodyBuilder("Robot") + simBody = fieldSim.addRobot(new SimBodyBuilder("Robot") .robotId(RobotId.BLUE_1) - .position(0, 0, 0.1) + .position(2.0, 2.0, 0.1) .mass(Kilograms.of(54)) .boxCollider(Inches.of(24).in(Meters) / 2, Inches.of(24).in(Meters) / 2, @@ -185,9 +202,62 @@ public SwerveSubsystem() .fixedRotation() .material(Material.CARPET)); + // Gripper mounted on the robot's front face, just off the carpet. + gripper = fieldSim.createGripper(simBody, + new Translation3d(INTAKE_FORWARD_M, 0, INTAKE_HEIGHT_M)); + + // NT4 struct-array publishers — AdvantageScope reads these as Pose3d[] for + // game-piece rendering. In the 3D view, drag the key onto the field and choose + // the appropriate game-piece model (Coral / Algae from the 2025 asset pack). + var nt = NetworkTableInstance.getDefault(); + coralPublisher = nt.getStructArrayTopic("FieldSimulation/Coral", Pose3d.struct).publish(); + algaePublisher = nt.getStructArrayTopic("FieldSimulation/Algae", Pose3d.struct).publish(); + SmartDashboard.putData("Field", field); } + /** + * Find the closest free game piece within {@link #INTAKE_RADIUS_M} of the robot's intake + * point and grab it. No-op if the gripper is already holding a piece. + * + * @return a one-shot {@link Command} suitable for binding to a button + */ + public Command intakeNearest() + { + return runOnce(() -> { + if (gripper.isHolding()) return; + Pose3d robotPose = simBody.getPose(); + Translation3d intakePt = new Translation3d(INTAKE_FORWARD_M, 0, INTAKE_HEIGHT_M) + .rotateBy(robotPose.getRotation()) + .plus(robotPose.getTranslation()); + GamePiece closest = null; + double minDist = INTAKE_RADIUS_M; + for (GamePiece gp : fieldSim.getGamePieces()) { + if (gp.isHeld()) continue; + double dist = gp.getPose().getTranslation().getDistance(intakePt); + if (dist < minDist) { minDist = dist; closest = gp; } + } + if (closest != null) gripper.grab(closest); + }); + } + + /** + * Eject the currently held game piece forward in the robot's heading direction with a + * slight upward loft. No-op if nothing is held. + * + * @return a one-shot {@link Command} suitable for binding to a button + */ + public Command ejectPiece() + { + return runOnce(() -> { + Rotation2d heading = simBody.getPose().getRotation().toRotation2d(); + gripper.eject( + heading.getCos() * EJECT_SPEED_XY, + heading.getSin() * EJECT_SPEED_XY, + EJECT_SPEED_Z); + }); + } + /** * Drive the {@link SwerveDrive} object with field relative chassis speeds. * @@ -232,7 +302,6 @@ public void simulationPeriodic() drive.simIterate(); // Push the odometry chassis speeds (robot-relative) into the physics body. - // Rotate to world frame using the current heading reported by the drive. ChassisSpeeds speeds = drive.getRobotRelativeSpeed(); Rotation2d heading = drive.getPose().getRotation(); double cosH = heading.getCos(); @@ -242,12 +311,16 @@ public void simulationPeriodic() simBody.setLinearVelocity(worldVx, worldVy, 0); simBody.setAngularVelocity(0, 0, speeds.omegaRadiansPerSecond); - // Advance the physics simulation one fixed timestep (default 20 ms). - simWorld.step(); + // Advance physics one fixed timestep; gripper update is included inside step(). + fieldSim.step(); - // Read the physics pose back and feed it to the field widget. + // Mirror physics robot pose back to the 2D field widget. Pose3d p = simBody.getPose(); Pose2d jsimPose = new Pose2d(p.getX(), p.getY(), p.getRotation().toRotation2d()); field.getObject("JSimPose").setPose(jsimPose); + + // Publish game-piece pose arrays for AdvantageScope 3D rendering. + coralPublisher.set(fieldSim.getGamePiecePoses("Coral")); + algaePublisher.set(fieldSim.getGamePiecePoses("Algae")); } } diff --git a/vendordep/src/main/java/jsim/field/FieldLoader.java b/vendordep/src/main/java/jsim/field/FieldLoader.java new file mode 100644 index 0000000..459bd20 --- /dev/null +++ b/vendordep/src/main/java/jsim/field/FieldLoader.java @@ -0,0 +1,159 @@ +package jsim.field; + +import com.fasterxml.jackson.databind.JsonNode; +import com.fasterxml.jackson.databind.ObjectMapper; +import java.io.IOException; +import java.io.InputStream; +import java.nio.file.Path; +import jsim.api.SimBodyBuilder; +import jsim.core.SimConstants; +import jsim.material.Material; + +/** + * Loads a JSim field definition from a JSON resource or file, producing a fully-configured + * {@link FieldSimulator} that includes: + *
    + *
  • the correct perimeter walls for the field dimensions declared in the JSON, + *
  • static obstacle hitboxes (reef structures, scoring targets, etc.), and + *
  • pre-spawned game pieces at their starting positions. + *
+ * + *

Bundled season definitions ship with JSim as classpath resources under + * {@code jsim/field/.json}. Load them by name: + * 

{@code
+ * FieldSimulator sim = FieldLoader.fromResource("2025-reefscape");
+ * }
+ * + *

Custom field files can be loaded from disk: + * 

{@code
+ * FieldSimulator sim = FieldLoader.fromFile(Path.of("my-field.json"));
+ * }
+ * + *

JSON schema

+ * 
{@code
+ * {
+ *   "year": 2025,
+ *   "game": "Reefscape",
+ *   "field": {
+ *     "lengthMeters": 17.548,
+ *     "widthMeters": 8.052
+ *   },
+ *   "obstacles": [
+ *     {
+ *       "name": "ReefBlue",
+ *       "x": 4.49,  "y": 4.03,  "z": 1.00,
+ *       "halfX": 0.85, "halfY": 0.85, "halfZ": 1.00
+ *     }
+ *   ],
+ *   "gamePieces": [
+ *     {
+ *       "variant": "Coral",
+ *       "radius": 0.057,
+ *       "mass": 0.227,
+ *       "friction": 0.5,
+ *       "restitution": 0.3,
+ *       "poses": [
+ *         { "x": 1.15, "y": 0.65, "z": 0.057 }
+ *       ]
+ *     }
+ *   ]
+ * }
+ * }
+ * + *

Each {@code obstacle} becomes a static {@link jsim.collision.BoxCollider} body in the + * physics world — robots and game pieces bounce off it but cannot push it. {@code gamePieces} + * become dynamic sphere bodies tracked by {@link FieldSimulator#getGamePiecePoses(String)} for + * AdvantageScope output. + */ +public final class FieldLoader { + private static final ObjectMapper MAPPER = new ObjectMapper(); + + private FieldLoader() {} + + /** + * Load a bundled field definition by short name, e.g. {@code "2025-reefscape"}. + * The classpath resource must exist at {@code /jsim/field/.json}. + * + * @param name resource name, without path prefix or {@code .json} extension + * @return a fully configured {@link FieldSimulator} + * @throws IllegalArgumentException if no bundled resource matches {@code name} + * @throws RuntimeException if the resource cannot be read or parsed + */ + public static FieldSimulator fromResource(String name) { + String resourcePath = "/jsim/field/" + name + ".json"; + try (InputStream is = FieldLoader.class.getResourceAsStream(resourcePath)) { + if (is == null) { + throw new IllegalArgumentException( + "No bundled field resource found at classpath:" + resourcePath); + } + return parse(MAPPER.readTree(is)); + } catch (IOException e) { + throw new RuntimeException("Failed to load field resource: " + resourcePath, e); + } + } + + /** + * Load a field definition from a JSON file on disk. + * + * @param jsonPath path to the {@code .json} field definition file + * @return a fully configured {@link FieldSimulator} + * @throws RuntimeException if the file cannot be read or parsed + */ + public static FieldSimulator fromFile(Path jsonPath) { + try { + return parse(MAPPER.readTree(jsonPath.toFile())); + } catch (IOException e) { + throw new RuntimeException("Failed to load field file: " + jsonPath, e); + } + } + + private static FieldSimulator parse(JsonNode root) { + // Field dimensions — fall back to standard FRC constants if absent. + JsonNode fieldNode = root.path("field"); + double lengthM = fieldNode.path("lengthMeters").asDouble(FieldLayout.FRC_LENGTH_M); + double widthM = fieldNode.path("widthMeters").asDouble(FieldLayout.FRC_WIDTH_M); + + FieldSimulator sim = new FieldSimulator(SimConstants.DEFAULT_DT, lengthM, widthM); + + // Static obstacle hitboxes (reef structures, coral stations, etc.). + // Each becomes a static box body that absorbs collisions without moving. + for (JsonNode obs : root.path("obstacles")) { + String name = obs.path("name").asText("Obstacle"); + double x = obs.path("x").asDouble(); + double y = obs.path("y").asDouble(); + double z = obs.path("z").asDouble(); + double halfX = obs.path("halfX").asDouble(); + double halfY = obs.path("halfY").asDouble(); + double halfZ = obs.path("halfZ").asDouble(); + sim.getWorld().addBody(new SimBodyBuilder(name) + .position(x, y, z) + .boxCollider(halfX, halfY, halfZ) + .isStatic() + .material(Material.WALL)); + } + + // Dynamic game piece bodies tracked for AdvantageScope output. + for (JsonNode type : root.path("gamePieces")) { + String variant = type.path("variant").asText(); + double radius = type.path("radius").asDouble(); + double mass = type.path("mass").asDouble(); + double friction = type.path("friction").asDouble(0.5); + double restitution = type.path("restitution").asDouble(0.3); + Material mat = new Material(friction, restitution); + int i = 0; + for (JsonNode pose : type.path("poses")) { + double px = pose.path("x").asDouble(); + double py = pose.path("y").asDouble(); + double pz = pose.path("z").asDouble(); + sim.spawnGamePiece(variant, new SimBodyBuilder(variant + i) + .position(px, py, pz) + .mass(mass) + .sphereCollider(radius) + .material(mat)); + i++; + } + } + + return sim; + } +} diff --git a/vendordep/src/main/resources/jsim/field/2025-reefscape.json b/vendordep/src/main/resources/jsim/field/2025-reefscape.json new file mode 100644 index 0000000..838a95b --- /dev/null +++ b/vendordep/src/main/resources/jsim/field/2025-reefscape.json @@ -0,0 +1,66 @@ +{ + "year": 2025, + "game": "Reefscape", + "field": { + "lengthMeters": 17.548, + "widthMeters": 8.052 + }, + "obstacles": [ + { + "name": "ReefBlue", + "x": 4.490, + "y": 4.026, + "z": 1.000, + "halfX": 0.850, + "halfY": 0.850, + "halfZ": 1.000 + }, + { + "name": "ReefRed", + "x": 13.059, + "y": 4.026, + "z": 1.000, + "halfX": 0.850, + "halfY": 0.850, + "halfZ": 1.000 + } + ], + "gamePieces": [ + { + "variant": "Coral", + "radius": 0.057, + "mass": 0.227, + "friction": 0.50, + "restitution": 0.30, + "poses": [ + { "x": 1.15, "y": 0.65, "z": 0.057 }, + { "x": 1.15, "y": 1.45, "z": 0.057 }, + { "x": 1.15, "y": 6.60, "z": 0.057 }, + { "x": 1.15, "y": 7.40, "z": 0.057 }, + { "x": 2.50, "y": 1.75, "z": 0.057 }, + { "x": 2.50, "y": 6.30, "z": 0.057 }, + { "x": 16.40, "y": 0.65, "z": 0.057 }, + { "x": 16.40, "y": 1.45, "z": 0.057 }, + { "x": 16.40, "y": 6.60, "z": 0.057 }, + { "x": 16.40, "y": 7.40, "z": 0.057 }, + { "x": 15.05, "y": 1.75, "z": 0.057 }, + { "x": 15.05, "y": 6.30, "z": 0.057 } + ] + }, + { + "variant": "Algae", + "radius": 0.203, + "mass": 0.260, + "friction": 0.50, + "restitution": 0.55, + "poses": [ + { "x": 3.25, "y": 4.026, "z": 0.203 }, + { "x": 4.490, "y": 2.80, "z": 0.203 }, + { "x": 4.490, "y": 5.25, "z": 0.203 }, + { "x": 14.30, "y": 4.026, "z": 0.203 }, + { "x": 13.059, "y": 2.80, "z": 0.203 }, + { "x": 13.059, "y": 5.25, "z": 0.203 } + ] + } + ] +}