Add toString for geometry and trajectory classes (#1991)

wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java

@@ -196,6 +196,11 @@ public class Pose2d {
     return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
   }
 
+  @Override
+  public String toString() {
+    return String.format("Pose2d(%s, %s)", m_translation, m_rotation);
+  }
+
   /**
    * Checks equality between this Pose2d and another object.
    *

wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java

@@ -180,6 +180,11 @@ public class Rotation2d {
     return m_sin / m_cos;
   }
 
+  @Override
+  public String toString() {
+    return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value));
+  }
+
   /**
    * Checks equality between this Rotation2d and another object.
    *

wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java

@@ -79,6 +79,11 @@ public class Transform2d {
     return m_rotation;
   }
 
+  @Override
+  public String toString() {
+    return String.format("Transform2d(%s, %s)", m_translation, m_rotation);
+  }
+
   /**
    * Checks equality between this Transform2d and another object.
    *

wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java

@@ -165,6 +165,11 @@ public class Translation2d {
     return new Translation2d(m_x / scalar, m_y / scalar);
   }
 
+  @Override
+  public String toString() {
+    return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y);
+  }
+
   /**
    * Checks equality between this Translation2d and another object.
    *

wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java

@@ -48,6 +48,11 @@ public class Twist2d {
     this.dtheta = dtheta;
   }
 
+  @Override
+  public String toString() {
+    return String.format("Twist2d(dX: %.2f, dY: %.2f, dTheta: %.2f)", dx, dy, dtheta);
+  }
+
   /**
    * Checks equality between this Twist2d and another object.
    *

wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java

@@ -8,6 +8,7 @@
 package edu.wpi.first.wpilibj.trajectory;
 
 import java.util.List;
+import java.util.stream.Collectors;
 
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 
@@ -219,5 +220,19 @@ public class Trajectory {
           lerp(curvatureRadPerMeter, endValue.curvatureRadPerMeter, interpolationFrac)
       );
     }
+
+    @Override
+    public String toString() {
+      return String.format(
+        "State(Sec: %.2f, Vel m/s: %.2f, Accel m/s/s: %.2f, Pose: %s, Curvature: %.2f)",
+        timeSeconds, velocityMetersPerSecond, accelerationMetersPerSecondSq,
+        poseMeters, curvatureRadPerMeter);
+    }
+  }
+
+  @Override
+  public String toString() {
+    String stateList = m_states.stream().map(State::toString).collect(Collectors.joining(", \n"));
+    return String.format("Trajectory - Seconds: %.2f, States:\n%s", m_totalTimeSeconds, stateList);
   }
 }