Add geometry classes (#1766)

These classes introduce ways to represent poses and provide easy ways to transform, rotate, and translate poses across 2d space. This classes will be especially useful for a planned odometry and kinematics suite. Furthermore, these classes can also be used to simply represent waypoints on a field, do superstructure motion planning, etc.
2026-06-21 01:01:43 +00:00 · 2019-07-24 02:57:39 -04:00
parent 48fe54271a
commit ee24101696
22 changed files with 1898 additions and 0 deletions
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class Pose2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testTransformBy() {
+    var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
+    var transformation = new Transform2d(new Translation2d(5.0, 0.0),
+        Rotation2d.fromDegrees(5.0));
+
+    var transformed = initial.plus(transformation);
+
+    assertAll(
+        () -> assertEquals(transformed.getTranslation().getX(), 1 + 5.0 / Math.sqrt(2.0), kEpsilon),
+        () -> assertEquals(transformed.getTranslation().getY(), 2 + 5.0 / Math.sqrt(2.0), kEpsilon),
+        () -> assertEquals(transformed.getRotation().getDegrees(), 50.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testRelativeTo() {
+    var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0));
+    var last = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(45.0));
+
+    var finalRelativeToInitial = last.relativeTo(initial);
+
+    assertAll(
+        () -> assertEquals(finalRelativeToInitial.getTranslation().getX(), 5.0 * Math.sqrt(2.0),
+            kEpsilon),
+        () -> assertEquals(finalRelativeToInitial.getTranslation().getY(), 0.0, kEpsilon),
+        () -> assertEquals(finalRelativeToInitial.getRotation().getDegrees(), 0.0, kEpsilon)
+    );
+  }
+}
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class Rotation2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testRadiansToDegrees() {
+    var one = new Rotation2d(Math.PI / 3);
+    var two = new Rotation2d(Math.PI / 4);
+
+    assertAll(
+        () -> assertEquals(one.getDegrees(), 60.0, kEpsilon),
+        () -> assertEquals(two.getDegrees(), 45.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testRadiansAndDegrees() {
+    var one = Rotation2d.fromDegrees(45.0);
+    var two = Rotation2d.fromDegrees(30.0);
+
+    assertAll(
+        () -> assertEquals(one.getRadians(), Math.PI / 4, kEpsilon),
+        () -> assertEquals(two.getRadians(), Math.PI / 6, kEpsilon)
+    );
+  }
+
+  @Test
+  void testRotateByFromZero() {
+    var zero = new Rotation2d();
+    var rotated = zero.rotateBy(Rotation2d.fromDegrees(90.0));
+
+    assertAll(
+        () -> assertEquals(rotated.getRadians(), Math.PI / 2.0, kEpsilon),
+        () -> assertEquals(rotated.getDegrees(), 90.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testRotateByNonZero() {
+    var rot = Rotation2d.fromDegrees(90.0);
+    rot = rot.plus(Rotation2d.fromDegrees(30.0));
+
+    assertEquals(rot.getDegrees(), 120.0, kEpsilon);
+  }
+
+  @Test
+  void testMinus() {
+    var one = Rotation2d.fromDegrees(70.0);
+    var two = Rotation2d.fromDegrees(30.0);
+
+    assertEquals(one.minus(two).getDegrees(), 40.0, kEpsilon);
+  }
+}
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java
@@ -0,0 +1,100 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class Translation2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testSum() {
+    var one = new Translation2d(1.0, 3.0);
+    var two = new Translation2d(2.0, 5.0);
+
+    var sum = one.plus(two);
+
+    assertAll(
+        () -> assertEquals(sum.getX(), 3.0, kEpsilon),
+        () -> assertEquals(sum.getY(), 8.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testDifference() {
+    var one = new Translation2d(1.0, 3.0);
+    var two = new Translation2d(2.0, 5.0);
+
+    var difference = one.minus(two);
+
+    assertAll(
+        () -> assertEquals(difference.getX(), -1.0, kEpsilon),
+        () -> assertEquals(difference.getY(), -2.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testRotateBy() {
+    var another = new Translation2d(3.0, 0.0);
+    var rotated = another.rotateBy(Rotation2d.fromDegrees(90.0));
+
+    assertAll(
+        () -> assertEquals(rotated.getX(), 0.0, kEpsilon),
+        () -> assertEquals(rotated.getY(), 3.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testMultiplication() {
+    var original = new Translation2d(3.0, 5.0);
+    var mult = original.times(3);
+
+    assertAll(
+        () -> assertEquals(mult.getX(), 9.0, kEpsilon),
+        () -> assertEquals(mult.getY(), 15.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testDivision() {
+    var original = new Translation2d(3.0, 5.0);
+    var div = original.div(2);
+
+    assertAll(
+        () -> assertEquals(div.getX(), 1.5, kEpsilon),
+        () -> assertEquals(div.getY(), 2.5, kEpsilon)
+    );
+  }
+
+  @Test
+  void testNorm() {
+    var one = new Translation2d(3.0, 5.0);
+    assertEquals(one.getNorm(), Math.hypot(3.0, 5.0), kEpsilon);
+  }
+
+  @Test
+  void testDistance() {
+    var one = new Translation2d(1, 1);
+    var two = new Translation2d(6, 6);
+    assertEquals(one.getDistance(two), 5 * Math.sqrt(2), kEpsilon);
+  }
+
+  @Test
+  void testUnaryMinus() {
+    var original = new Translation2d(-4.5, 7);
+    var inverted = original.unaryMinus();
+
+    assertAll(
+        () -> assertEquals(inverted.getX(), 4.5, kEpsilon),
+        () -> assertEquals(inverted.getY(), -7, kEpsilon)
+    );
+  }
+}
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class Twist2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testStraightLineTwist() {
+    var straight = new Twist2d(5.0, 0.0, 0.0);
+    var straightPose = new Pose2d().exp(straight);
+
+    assertAll(
+        () -> assertEquals(straightPose.getTranslation().getX(), 5.0, kEpsilon),
+        () -> assertEquals(straightPose.getTranslation().getY(), 0.0, kEpsilon),
+        () -> assertEquals(straightPose.getRotation().getRadians(), 0.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testQuarterCirleTwist() {
+    var quarterCircle = new Twist2d(5.0 / 2.0 * Math.PI, 0, Math.PI / 2.0);
+    var quarterCirclePose = new Pose2d().exp(quarterCircle);
+
+    assertAll(
+        () -> assertEquals(quarterCirclePose.getTranslation().getX(), 5.0, kEpsilon),
+        () -> assertEquals(quarterCirclePose.getTranslation().getY(), 5.0, kEpsilon),
+        () -> assertEquals(quarterCirclePose.getRotation().getDegrees(), 90.0, kEpsilon)
+    );
+  }
+
+  @Test
+  void testDiagonalNoDtheta() {
+    var diagonal = new Twist2d(2.0, 2.0, 0.0);
+    var diagonalPose = new Pose2d().exp(diagonal);
+
+    assertAll(
+        () -> assertEquals(diagonalPose.getTranslation().getX(), 2.0, kEpsilon),
+        () -> assertEquals(diagonalPose.getTranslation().getY(), 2.0, kEpsilon),
+        () -> assertEquals(diagonalPose.getRotation().getDegrees(), 0.0, kEpsilon)
+    );
+  }
+}