From 0c14630d40f2cf8b715e46dfc78c54350be42b78 Mon Sep 17 00:00:00 2001
From: Ryan Brott <rcbrott@gmail.com>
Date: Wed, 19 Jul 2023 23:13:58 -0700
Subject: [PATCH] Bump RR

---
 TeamCode/build.gradle                         |  4 +-
 .../ftc/teamcode/MecanumDrive.java            | 71 +++++++++----------
 .../firstinspires/ftc/teamcode/TankDrive.java | 68 +++++++++---------
 .../ftc/teamcode/ThreeDeadWheelLocalizer.java |  8 +--
 .../ftc/teamcode/TwoDeadWheelLocalizer.java   |  8 +--
 .../ftc/teamcode/tuning/DriveView.java        |  4 +-
 .../teamcode/tuning/ForwardRampLogger.java    |  2 +-
 .../ftc/teamcode/tuning/LocalizationTest.java | 17 ++---
 .../tuning/ManualFeedforwardTuner.java        |  5 +-
 .../ftc/teamcode/util/Localizer.java          |  4 +-
 .../ftc/teamcode/util/LogFiles.java           | 12 ++--
 .../ftc/teamcode/util/Paths.java              |  4 +-
 12 files changed, 105 insertions(+), 102 deletions(-)

diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle
index ac72b746b0..dd806f291d 100644
--- a/TeamCode/build.gradle
+++ b/TeamCode/build.gradle
@@ -34,8 +34,8 @@ dependencies {
     annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
 
     implementation 'com.acmerobotics.dashboard:dashboard:0.4.9'
-    implementation 'com.acmerobotics.roadrunner:core:1.0.0-beta1'
-    implementation 'com.acmerobotics.roadrunner:actions:1.0.0-beta1'
+    implementation 'com.acmerobotics.roadrunner:core:1.0.0-beta2'
+    implementation 'com.acmerobotics.roadrunner:actions:1.0.0-beta2'
 
     implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7'
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
index 27e0c19f2b..6a8009f1d9 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
@@ -20,9 +20,7 @@
 import com.acmerobotics.roadrunner.TimeTurn;
 import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
 import com.acmerobotics.roadrunner.TurnConstraints;
-import com.acmerobotics.roadrunner.Twist2d;
 import com.acmerobotics.roadrunner.Twist2dDual;
-import com.acmerobotics.roadrunner.Twist2dIncrDual;
 import com.acmerobotics.roadrunner.Vector2d;
 import com.acmerobotics.roadrunner.VelConstraint;
 import com.qualcomm.hardware.lynx.LynxModule;
@@ -51,6 +49,7 @@ public final class MecanumDrive {
     // drive model parameters
     public static double IN_PER_TICK = 0;
     public static double LATERAL_IN_PER_TICK = 1;
+    public static double LATERAL_MULTIPLIER = IN_PER_TICK / LATERAL_IN_PER_TICK;
     public static double TRACK_WIDTH_TICKS = 0;
 
     // feedforward parameters in tick units
@@ -77,8 +76,7 @@ public final class MecanumDrive {
     public static double HEADING_VEL_GAIN = 0.0; // shared with turn
 
     public final MecanumKinematics kinematics = new MecanumKinematics(
-            IN_PER_TICK * TRACK_WIDTH_TICKS,
-            IN_PER_TICK / LATERAL_IN_PER_TICK);
+            IN_PER_TICK * TRACK_WIDTH_TICKS, LATERAL_MULTIPLIER);
 
     public final MotorFeedforward feedforward = new MotorFeedforward(kS, kV / IN_PER_TICK, kA / IN_PER_TICK);
 
@@ -126,7 +124,7 @@ public DriveLocalizer() {
         }
 
         @Override
-        public Twist2dIncrDual<Time> update() {
+        public Twist2dDual<Time> update() {
             Encoder.PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity();
             Encoder.PositionVelocityPair leftRearPosVel = leftRear.getPositionAndVelocity();
             Encoder.PositionVelocityPair rightRearPosVel = rightRear.getPositionAndVelocity();
@@ -135,7 +133,7 @@ public Twist2dIncrDual<Time> update() {
             Rotation2d heading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
             double headingDelta = heading.minus(lastHeading);
 
-            Twist2dIncrDual<Time> twist = kinematics.forward(new MecanumKinematics.WheelIncrements<>(
+            Twist2dDual<Time> twist = kinematics.forward(new MecanumKinematics.WheelIncrements<>(
                     new DualNum<Time>(new double[]{
                             (leftFrontPosVel.position - lastLeftFrontPos) + kinematics.trackWidth * headingDelta,
                             leftFrontPosVel.velocity,
@@ -161,9 +159,9 @@ public Twist2dIncrDual<Time> update() {
 
             lastHeading = heading;
 
-            return new Twist2dIncrDual<>(
-                    twist.transIncr,
-                    twist.rotIncr.drop(1).addFront(headingDelta)
+            return new Twist2dDual<>(
+                    twist.line,
+                    DualNum.cons(headingDelta, twist.angle.drop(1))
             );
         }
     }
@@ -199,8 +197,9 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) {
         localizer = new DriveLocalizer();
     }
 
-    public void setDrivePowers(Twist2d powers) {
-        MecanumKinematics.WheelVelocities<Time> wheelVels = new MecanumKinematics(1).inverse(Twist2dDual.constant(powers, 1));
+    public void setDrivePowers(PoseVelocity2d powers) {
+        MecanumKinematics.WheelVelocities<Time> wheelVels = new MecanumKinematics(1).inverse(
+                PoseVelocity2dDual.constant(powers, 1));
 
         double maxPowerMag = 1;
         for (DualNum<Time> power : wheelVels.all()) {
@@ -229,8 +228,8 @@ public FollowTrajectoryAction(TimeTrajectory t) {
             yPoints = new double[disps.size()];
             for (int i = 0; i < disps.size(); i++) {
                 Pose2d p = t.path.get(disps.get(i), 1).value();
-                xPoints[i] = p.trans.x;
-                yPoints[i] = p.trans.y;
+                xPoints[i] = p.position.x;
+                yPoints[i] = p.position.y;
             }
         }
 
@@ -255,9 +254,9 @@ public boolean run(@NonNull TelemetryPacket p) {
 
             Pose2dDual<Time> txWorldTarget = timeTrajectory.get(t);
 
-            Twist2d robotVelRobot = updatePoseEstimate();
+            PoseVelocity2d robotVelRobot = updatePoseEstimate();
 
-            Twist2dDual<Time> command = new HolonomicController(
+            PoseVelocity2dDual<Time> command = new HolonomicController(
                     AXIAL_GAIN, LATERAL_GAIN, HEADING_GAIN,
                     AXIAL_VEL_GAIN, LATERAL_VEL_GAIN, HEADING_VEL_GAIN
             )
@@ -272,14 +271,14 @@ public boolean run(@NonNull TelemetryPacket p) {
 
             LogFiles.recordTargetPose(txWorldTarget.value());
 
-            p.put("x", pose.trans.x);
-            p.put("y", pose.trans.y);
-            p.put("heading (deg)", Math.toDegrees(pose.rot.log()));
+            p.put("x", pose.position.x);
+            p.put("y", pose.position.y);
+            p.put("heading (deg)", Math.toDegrees(pose.heading.log()));
 
             Pose2d error = txWorldTarget.value().minusExp(pose);
-            p.put("xError", error.trans.x);
-            p.put("yError", error.trans.y);
-            p.put("headingError (deg)", Math.toDegrees(error.rot.log()));
+            p.put("xError", error.position.x);
+            p.put("yError", error.position.y);
+            p.put("headingError (deg)", Math.toDegrees(error.heading.log()));
 
             // only draw when active; only one drive action should be active at a time
             Canvas c = p.fieldOverlay();
@@ -336,9 +335,9 @@ public boolean run(@NonNull TelemetryPacket p) {
 
             Pose2dDual<Time> txWorldTarget = turn.get(t);
 
-            Twist2d robotVelRobot = updatePoseEstimate();
+            PoseVelocity2d robotVelRobot = updatePoseEstimate();
 
-            Twist2dDual<Time> command = new HolonomicController(
+            PoseVelocity2dDual<Time> command = new HolonomicController(
                     AXIAL_GAIN, LATERAL_GAIN, HEADING_GAIN,
                     AXIAL_VEL_GAIN, LATERAL_VEL_GAIN, HEADING_VEL_GAIN
             )
@@ -363,7 +362,7 @@ public boolean run(@NonNull TelemetryPacket p) {
             drawRobot(c, pose);
 
             c.setStroke("#7C4DFFFF");
-            c.fillCircle(turn.beginPose.trans.x, turn.beginPose.trans.y, 2);
+            c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
 
             return true;
         }
@@ -371,13 +370,13 @@ public boolean run(@NonNull TelemetryPacket p) {
         @Override
         public void preview(Canvas c) {
             c.setStroke("#7C4DFF7A");
-            c.fillCircle(turn.beginPose.trans.x, turn.beginPose.trans.y, 2);
+            c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
         }
     }
 
-    public Twist2d updatePoseEstimate() {
-        Twist2dIncrDual<Time> incr = localizer.update();
-        pose = pose.plus(incr.value());
+    public PoseVelocity2d updatePoseEstimate() {
+        Twist2dDual<Time> twist = localizer.update();
+        pose = pose.plus(twist.value());
 
         poseHistory.add(pose);
         while (poseHistory.size() > 100) {
@@ -386,7 +385,7 @@ public Twist2d updatePoseEstimate() {
 
         LogFiles.recordPose(pose);
 
-        return incr.velocity().value();
+        return twist.velocity().value();
     }
 
     private void drawPoseHistory(Canvas c) {
@@ -395,8 +394,8 @@ private void drawPoseHistory(Canvas c) {
 
         int i = 0;
         for (Pose2d t : poseHistory) {
-            xPoints[i] = t.trans.x;
-            yPoints[i] = t.trans.y;
+            xPoints[i] = t.position.x;
+            yPoints[i] = t.position.y;
 
             i++;
         }
@@ -410,10 +409,10 @@ private static void drawRobot(Canvas c, Pose2d t) {
         final double ROBOT_RADIUS = 9;
 
         c.setStrokeWidth(1);
-        c.strokeCircle(t.trans.x, t.trans.y, ROBOT_RADIUS);
+        c.strokeCircle(t.position.x, t.position.y, ROBOT_RADIUS);
 
-        Vector2d halfv = t.rot.vec().times(0.5 * ROBOT_RADIUS);
-        Vector2d p1 = t.trans.plus(halfv);
+        Vector2d halfv = t.heading.vec().times(0.5 * ROBOT_RADIUS);
+        Vector2d p1 = t.position.plus(halfv);
         Vector2d p2 = p1.plus(halfv);
         c.strokeLine(p1.x, p1.y, p2.x, p2.y);
     }
@@ -422,10 +421,10 @@ public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) {
         return new TrajectoryActionBuilder(
                 TurnAction::new,
                 FollowTrajectoryAction::new,
-                beginPose, 1e-6,
+                beginPose, 1e-6, 0.0,
                 defaultTurnConstraints,
                 defaultVelConstraint, defaultAccelConstraint,
-                0.25
+                0.25, 0.1
         );
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
index dd89d9c9ad..69c692903a 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
@@ -15,6 +15,8 @@
 import com.acmerobotics.roadrunner.MotorFeedforward;
 import com.acmerobotics.roadrunner.Pose2d;
 import com.acmerobotics.roadrunner.Pose2dDual;
+import com.acmerobotics.roadrunner.PoseVelocity2d;
+import com.acmerobotics.roadrunner.PoseVelocity2dDual;
 import com.acmerobotics.roadrunner.ProfileAccelConstraint;
 import com.acmerobotics.roadrunner.RamseteController;
 import com.acmerobotics.roadrunner.TankKinematics;
@@ -25,7 +27,6 @@
 import com.acmerobotics.roadrunner.TurnConstraints;
 import com.acmerobotics.roadrunner.Twist2d;
 import com.acmerobotics.roadrunner.Twist2dDual;
-import com.acmerobotics.roadrunner.Twist2dIncrDual;
 import com.acmerobotics.roadrunner.Vector2d;
 import com.acmerobotics.roadrunner.Vector2dDual;
 import com.acmerobotics.roadrunner.VelConstraint;
@@ -135,7 +136,7 @@ public DriveLocalizer() {
         }
 
         @Override
-        public Twist2dIncrDual<Time> update() {
+        public Twist2dDual<Time> update() {
             double meanLeftPos = 0.0, meanLeftVel = 0.0;
             for (Encoder e : leftEncs) {
                 Encoder.PositionVelocityPair p = e.getPositionAndVelocity();
@@ -154,7 +155,7 @@ public Twist2dIncrDual<Time> update() {
             meanRightPos /= rightEncs.size();
             meanRightVel /= rightEncs.size();
 
-            TankKinematics.WheelIncrements<Time> incrs = new TankKinematics.WheelIncrements<>(
+            TankKinematics.WheelIncrements<Time> twist = new TankKinematics.WheelIncrements<>(
                     new DualNum<Time>(new double[] {
                             meanLeftPos - lastLeftPos,
                             meanLeftVel
@@ -168,7 +169,7 @@ public Twist2dIncrDual<Time> update() {
             lastLeftPos = meanLeftPos;
             lastRightPos = meanRightPos;
 
-            return kinematics.forward(incrs);
+            return kinematics.forward(twist);
         }
     }
 
@@ -203,8 +204,9 @@ public TankDrive(HardwareMap hardwareMap, Pose2d pose) {
         localizer = new TankDrive.DriveLocalizer();
     }
 
-    public void setDrivePowers(Twist2d powers) {
-        TankKinematics.WheelVelocities<Time> wheelVels = new TankKinematics(2).inverse(Twist2dDual.constant(powers, 1));
+    public void setDrivePowers(PoseVelocity2d powers) {
+        TankKinematics.WheelVelocities<Time> wheelVels = new TankKinematics(2).inverse(
+                PoseVelocity2dDual.constant(powers, 1));
 
         double maxPowerMag = 1;
         for (DualNum<Time> power : wheelVels.all()) {
@@ -235,8 +237,8 @@ public FollowTrajectoryAction(TimeTrajectory t) {
             yPoints = new double[disps.size()];
             for (int i = 0; i < disps.size(); i++) {
                 Pose2d p = t.path.get(disps.get(i), 1).value();
-                xPoints[i] = p.trans.x;
-                yPoints[i] = p.trans.y;
+                xPoints[i] = p.position.x;
+                yPoints[i] = p.position.y;
             }
         }
 
@@ -267,7 +269,7 @@ public boolean run(@NonNull TelemetryPacket p) {
 
             updatePoseEstimate();
 
-            Twist2dDual<Time> command = new RamseteController(kinematics.trackWidth, RAMSETE_ZETA, RAMSETE_BBAR)
+            PoseVelocity2dDual<Time> command = new RamseteController(kinematics.trackWidth, RAMSETE_ZETA, RAMSETE_BBAR)
                     .compute(x, txWorldTarget, pose);
 
             TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
@@ -279,14 +281,14 @@ public boolean run(@NonNull TelemetryPacket p) {
                 m.setPower(feedforward.compute(wheelVels.right) / voltage);
             }
 
-            p.put("x", pose.trans.x);
-            p.put("y", pose.trans.y);
-            p.put("heading (deg)", Math.toDegrees(pose.rot.log()));
+            p.put("x", pose.position.x);
+            p.put("y", pose.position.y);
+            p.put("heading (deg)", Math.toDegrees(pose.heading.log()));
 
             Pose2d error = txWorldTarget.value().minusExp(pose);
-            p.put("xError", error.trans.x);
-            p.put("yError", error.trans.y);
-            p.put("headingError (deg)", Math.toDegrees(error.rot.log()));
+            p.put("xError", error.position.x);
+            p.put("yError", error.position.y);
+            p.put("headingError (deg)", Math.toDegrees(error.heading.log()));
 
             LogFiles.recordTargetPose(txWorldTarget.value());
 
@@ -347,13 +349,13 @@ public boolean run(@NonNull TelemetryPacket p) {
 
             Pose2dDual<Time> txWorldTarget = turn.get(t);
 
-            Twist2d robotVelRobot = updatePoseEstimate();
+            PoseVelocity2d robotVelRobot = updatePoseEstimate();
 
-            Twist2dDual<Time> command = new Twist2dDual<>(
+            PoseVelocity2dDual<Time> command = new PoseVelocity2dDual<>(
                     Vector2dDual.constant(new Vector2d(0, 0), 3),
-                    txWorldTarget.rot.velocity().plus(
-                            TURN_GAIN * pose.rot.minus(txWorldTarget.rot.value()) +
-                            TURN_VEL_GAIN * (robotVelRobot.rotVel - txWorldTarget.rot.velocity().value())
+                    txWorldTarget.heading.velocity().plus(
+                            TURN_GAIN * pose.heading.minus(txWorldTarget.heading.value()) +
+                            TURN_VEL_GAIN * (robotVelRobot.angVel - txWorldTarget.heading.velocity().value())
                     )
             );
 
@@ -378,7 +380,7 @@ public boolean run(@NonNull TelemetryPacket p) {
             drawRobot(c, pose);
 
             c.setStroke("#7C4DFFFF");
-            c.fillCircle(turn.beginPose.trans.x, turn.beginPose.trans.y, 2);
+            c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
 
             return true;
         }
@@ -386,13 +388,13 @@ public boolean run(@NonNull TelemetryPacket p) {
         @Override
         public void preview(Canvas c) {
             c.setStroke("#7C4DFF7A");
-            c.fillCircle(turn.beginPose.trans.x, turn.beginPose.trans.y, 2);
+            c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
         }
     }
 
-    public Twist2d updatePoseEstimate() {
-        Twist2dIncrDual<Time> incr = localizer.update();
-        pose = pose.plus(incr.value());
+    public PoseVelocity2d updatePoseEstimate() {
+        Twist2dDual<Time> twist = localizer.update();
+        pose = pose.plus(twist.value());
 
         poseHistory.add(pose);
         while (poseHistory.size() > 100) {
@@ -401,7 +403,7 @@ public Twist2d updatePoseEstimate() {
 
         LogFiles.recordPose(pose);
 
-        return incr.velocity().value();
+        return twist.velocity().value();
     }
 
     private void drawPoseHistory(Canvas c) {
@@ -410,8 +412,8 @@ private void drawPoseHistory(Canvas c) {
 
         int i = 0;
         for (Pose2d t : poseHistory) {
-            xPoints[i] = t.trans.x;
-            yPoints[i] = t.trans.y;
+            xPoints[i] = t.position.x;
+            yPoints[i] = t.position.y;
 
             i++;
         }
@@ -425,10 +427,10 @@ private static void drawRobot(Canvas c, Pose2d t) {
         final double ROBOT_RADIUS = 9;
 
         c.setStrokeWidth(1);
-        c.strokeCircle(t.trans.x, t.trans.y, ROBOT_RADIUS);
+        c.strokeCircle(t.position.x, t.position.y, ROBOT_RADIUS);
 
-        Vector2d halfv = t.rot.vec().times(0.5 * ROBOT_RADIUS);
-        Vector2d p1 = t.trans.plus(halfv);
+        Vector2d halfv = t.heading.vec().times(0.5 * ROBOT_RADIUS);
+        Vector2d p1 = t.position.plus(halfv);
         Vector2d p2 = p1.plus(halfv);
         c.strokeLine(p1.x, p1.y, p2.x, p2.y);
     }
@@ -437,10 +439,10 @@ public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) {
         return new TrajectoryActionBuilder(
                 TurnAction::new,
                 FollowTrajectoryAction::new,
-                beginPose, 1e-6,
+                beginPose, 1e-6, 0.0,
                 defaultTurnConstraints,
                 defaultVelConstraint, defaultAccelConstraint,
-                0.25
+                0.25, 0.1
         );
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java
index 407dee9a6d..7ba0de6bb4 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java
@@ -3,7 +3,7 @@
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.roadrunner.DualNum;
 import com.acmerobotics.roadrunner.Time;
-import com.acmerobotics.roadrunner.Twist2dIncrDual;
+import com.acmerobotics.roadrunner.Twist2dDual;
 import com.acmerobotics.roadrunner.Vector2dDual;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.HardwareMap;
@@ -36,7 +36,7 @@ public ThreeDeadWheelLocalizer(HardwareMap hardwareMap, double inPerTick) {
         this.inPerTick = inPerTick;
     }
 
-    public Twist2dIncrDual<Time> update() {
+    public Twist2dDual<Time> update() {
         Encoder.PositionVelocityPair par0PosVel = par0.getPositionAndVelocity();
         Encoder.PositionVelocityPair par1PosVel = par1.getPositionAndVelocity();
         Encoder.PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
@@ -45,7 +45,7 @@ public Twist2dIncrDual<Time> update() {
         int par1PosDelta = par1PosVel.position - lastPar1Pos;
         int perpPosDelta = perpPosVel.position - lastPerpPos;
 
-        Twist2dIncrDual<Time> twistIncr = new Twist2dIncrDual<>(
+        Twist2dDual<Time> twist = new Twist2dDual<>(
                 new Vector2dDual<>(
                         new DualNum<Time>(new double[] {
                                 (PAR0_Y_TICKS * par1PosDelta - PAR1_Y_TICKS * par0PosDelta) / (PAR0_Y_TICKS - PAR1_Y_TICKS),
@@ -66,6 +66,6 @@ public Twist2dIncrDual<Time> update() {
         lastPar1Pos = par1PosVel.position;
         lastPerpPos = perpPosVel.position;
 
-        return twistIncr;
+        return twist;
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java
index c9c9cfcfcb..317293098e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java
@@ -4,7 +4,7 @@
 import com.acmerobotics.roadrunner.DualNum;
 import com.acmerobotics.roadrunner.Rotation2d;
 import com.acmerobotics.roadrunner.Time;
-import com.acmerobotics.roadrunner.Twist2dIncrDual;
+import com.acmerobotics.roadrunner.Twist2dDual;
 import com.acmerobotics.roadrunner.Vector2dDual;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.HardwareMap;
@@ -40,7 +40,7 @@ public TwoDeadWheelLocalizer(HardwareMap hardwareMap, IMU imu, double inPerTick)
         this.inPerTick = inPerTick;
     }
 
-    public Twist2dIncrDual<Time> update() {
+    public Twist2dDual<Time> update() {
         Encoder.PositionVelocityPair parPosVel = par.getPositionAndVelocity();
         Encoder.PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
         Rotation2d heading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
@@ -51,7 +51,7 @@ public Twist2dIncrDual<Time> update() {
 
         double headingVel = imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate;
 
-        Twist2dIncrDual<Time> twistIncr = new Twist2dIncrDual<>(
+        Twist2dDual<Time> twist = new Twist2dDual<>(
                 new Vector2dDual<>(
                         new DualNum<Time>(new double[] {
                                 parPosDelta - PAR_Y_TICKS * headingDelta,
@@ -72,6 +72,6 @@ public Twist2dIncrDual<Time> update() {
         lastPerpPos = perpPosVel.position;
         lastHeading = heading;
 
-        return twistIncr;
+        return twist;
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveView.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveView.java
index a18ef76f65..8648462201 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveView.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveView.java
@@ -2,7 +2,7 @@
 
 import com.acmerobotics.roadrunner.MotorFeedforward;
 import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.Twist2d;
+import com.acmerobotics.roadrunner.PoseVelocity2d;
 import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.HardwareMap;
@@ -176,7 +176,7 @@ public MotorFeedforward feedforward() {
         throw new AssertionError();
     }
 
-    public void setDrivePowers(Twist2d powers) {
+    public void setDrivePowers(PoseVelocity2d powers) {
         if (md != null) {
             md.setDrivePowers(powers);
             return;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ForwardRampLogger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ForwardRampLogger.java
index aed0588401..e2edaf316b 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ForwardRampLogger.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ForwardRampLogger.java
@@ -13,7 +13,7 @@
 import java.util.List;
 import java.util.Map;
 
-public final class ForwardRampLogger extends LinearOpMode {
+public final class  ForwardRampLogger extends LinearOpMode {
     private static double power(double seconds) {
         return Math.min(0.1 * seconds, 0.9);
     }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java
index 131dde8387..a5b5bbc5f9 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java
@@ -1,6 +1,7 @@
 package org.firstinspires.ftc.teamcode.tuning;
 
 import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.PoseVelocity2d;
 import com.acmerobotics.roadrunner.Twist2d;
 import com.acmerobotics.roadrunner.Vector2d;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@@ -17,7 +18,7 @@ public void runOpMode() throws InterruptedException {
             waitForStart();
 
             while (opModeIsActive()) {
-                drive.setDrivePowers(new Twist2d(
+                drive.setDrivePowers(new PoseVelocity2d(
                         new Vector2d(
                                 -gamepad1.left_stick_y,
                                 -gamepad1.left_stick_x
@@ -27,9 +28,9 @@ public void runOpMode() throws InterruptedException {
 
                 drive.updatePoseEstimate();
 
-                telemetry.addData("x", drive.pose.trans.x);
-                telemetry.addData("y", drive.pose.trans.y);
-                telemetry.addData("heading", drive.pose.rot);
+                telemetry.addData("x", drive.pose.position.x);
+                telemetry.addData("y", drive.pose.position.y);
+                telemetry.addData("heading", drive.pose.heading);
                 telemetry.update();
             }
         } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
@@ -38,7 +39,7 @@ public void runOpMode() throws InterruptedException {
             waitForStart();
 
             while (opModeIsActive()) {
-                drive.setDrivePowers(new Twist2d(
+                drive.setDrivePowers(new PoseVelocity2d(
                         new Vector2d(
                                 -gamepad1.left_stick_y,
                                 0.0
@@ -48,9 +49,9 @@ public void runOpMode() throws InterruptedException {
 
                 drive.updatePoseEstimate();
 
-                telemetry.addData("x", drive.pose.trans.x);
-                telemetry.addData("y", drive.pose.trans.y);
-                telemetry.addData("heading", drive.pose.rot);
+                telemetry.addData("x", drive.pose.position.x);
+                telemetry.addData("y", drive.pose.position.y);
+                telemetry.addData("heading", drive.pose.heading);
                 telemetry.update();
             }
         } else {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedforwardTuner.java
index 742bacd6b6..b891727b58 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedforwardTuner.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedforwardTuner.java
@@ -4,6 +4,7 @@
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.DualNum;
+import com.acmerobotics.roadrunner.PoseVelocity2d;
 import com.acmerobotics.roadrunner.Profiles;
 import com.acmerobotics.roadrunner.Time;
 import com.acmerobotics.roadrunner.TimeProfile;
@@ -72,7 +73,7 @@ public void runOpMode() {
                     telemetry.addData("vref", v.get(0));
 
                     double power = view.feedforward().compute(v) / view.voltageSensor.getVoltage();
-                    view.setDrivePowers(new Twist2d(new Vector2d(power, 0), 0));
+                    view.setDrivePowers(new PoseVelocity2d(new Vector2d(power, 0), 0));
 
                     break;
                 }
@@ -84,7 +85,7 @@ public void runOpMode() {
                     }
 
                     view.setDrivePowers(
-                            new Twist2d(
+                            new PoseVelocity2d(
                                     new Vector2d(
                                             -gamepad1.left_stick_y,
                                             -gamepad1.left_stick_x
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Localizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Localizer.java
index 69a9750e00..c04ad80385 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Localizer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Localizer.java
@@ -1,8 +1,8 @@
 package org.firstinspires.ftc.teamcode.util;
 
 import com.acmerobotics.roadrunner.Time;
-import com.acmerobotics.roadrunner.Twist2dIncrDual;
+import com.acmerobotics.roadrunner.Twist2dDual;
 
 public interface Localizer {
-    Twist2dIncrDual<Time> update();
+    Twist2dDual<Time> update();
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java
index 180759699f..158b9b1b29 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java
@@ -106,18 +106,18 @@ public LogFile(String opModeName) {
     }
 
     public static void recordTargetPose(Pose2d targetPose) {
-        log.targetXs.add(targetPose.trans.x);
-        log.targetYs.add(targetPose.trans.y);
-        log.targetHeadings.add(targetPose.rot.log());
+        log.targetXs.add(targetPose.position.x);
+        log.targetYs.add(targetPose.position.y);
+        log.targetHeadings.add(targetPose.heading.log());
     }
 
     public static void recordPose(Pose2d pose) {
         // arbitrarily add time here
         log.nsTimes.add(System.nanoTime());
 
-        log.xs.add(pose.trans.x);
-        log.ys.add(pose.trans.y);
-        log.headings.add(pose.rot.log());
+        log.xs.add(pose.position.x);
+        log.ys.add(pose.position.y);
+        log.headings.add(pose.heading.log());
     }
 
     private static final OpModeManagerNotifier.Notifications notifHandler = new OpModeManagerNotifier.Notifications() {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Paths.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Paths.java
index 3abe87bd04..9077a39e90 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Paths.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Paths.java
@@ -16,11 +16,11 @@ public static double project(PosePath path, Vector2d query) {
         List<Double> xs = com.acmerobotics.roadrunner.Math.range(
                 0.0, path.length(), (int) Math.ceil(path.length() / 6.0));
         double champDisp = Curves.project(path, query, xs.get(0));
-        final Vector2d initVec = path.get(champDisp, 1).trans.value();
+        final Vector2d initVec = path.get(champDisp, 1).position.value();
         double champSqrNorm = initVec.minus(query).sqrNorm();
         for (int i = 1; i < xs.size(); i++) {
             double disp = Curves.project(path, query, xs.get(i));
-            Vector2d vec = path.get(disp, 1).trans.value();
+            Vector2d vec = path.get(disp, 1).position.value();
             double sqrNorm = vec.minus(query).sqrNorm();
 
             if (sqrNorm < champSqrNorm) {