Bump RR

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:core:1.0.0-beta2'
-    implementation 'com.acmerobotics.roadrunner:actions:1.0.0-beta1'
+    implementation 'com.acmerobotics.roadrunner:actions:1.0.0-beta2'
 
     implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7'
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java

@@ -20,9 +20,7 @@ import com.acmerobotics.roadrunner.TimeTrajectory;
 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 * TRACK_WIDTH_TICKS, LATERAL_MULTIPLIER);
-            IN_PER_TICK / LATERAL_IN_PER_TICK);
 
     public final MotorFeedforward feedforward = new MotorFeedforward(kS, kV / IN_PER_TICK, kA / IN_PER_TICK);
 
@@ -126,7 +124,7 @@ public final class MecanumDrive {
         }
 
         @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 final class MecanumDrive {
             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 final class MecanumDrive {
 
             lastHeading = heading;
 
-            return new Twist2dIncrDual<>(
+            return new Twist2dDual<>(
-                    twist.transIncr,
+                    twist.line,
-                    twist.rotIncr.drop(1).addFront(headingDelta)
+                    DualNum.cons(headingDelta, twist.angle.drop(1))
             );
         }
     }
@@ -199,8 +197,9 @@ public final class MecanumDrive {
         localizer = new DriveLocalizer();
     }
 
-    public void setDrivePowers(Twist2d powers) {
+    public void setDrivePowers(PoseVelocity2d powers) {
-        MecanumKinematics.WheelVelocities<Time> wheelVels = new MecanumKinematics(1).inverse(Twist2dDual.constant(powers, 1));
+        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 final class MecanumDrive {
             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;
+                xPoints[i] = p.position.x;
-                yPoints[i] = p.trans.y;
+                yPoints[i] = p.position.y;
             }
         }
 
@@ -255,9 +254,9 @@ public final class MecanumDrive {
 
             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 final class MecanumDrive {
 
             LogFiles.recordTargetPose(txWorldTarget.value());
 
-            p.put("x", pose.trans.x);
+            p.put("x", pose.position.x);
-            p.put("y", pose.trans.y);
+            p.put("y", pose.position.y);
-            p.put("heading (deg)", Math.toDegrees(pose.rot.log()));
+            p.put("heading (deg)", Math.toDegrees(pose.heading.log()));
 
             Pose2d error = txWorldTarget.value().minusExp(pose);
-            p.put("xError", error.trans.x);
+            p.put("xError", error.position.x);
-            p.put("yError", error.trans.y);
+            p.put("yError", error.position.y);
-            p.put("headingError (deg)", Math.toDegrees(error.rot.log()));
+            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 final class MecanumDrive {
 
             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 final class MecanumDrive {
             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 final class MecanumDrive {
         @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() {
+    public PoseVelocity2d updatePoseEstimate() {
-        Twist2dIncrDual<Time> incr = localizer.update();
+        Twist2dDual<Time> twist = localizer.update();
-        pose = pose.plus(incr.value());
+        pose = pose.plus(twist.value());
 
         poseHistory.add(pose);
         while (poseHistory.size() > 100) {
@@ -386,7 +385,7 @@ public final class MecanumDrive {
 
         LogFiles.recordPose(pose);
 
-        return incr.velocity().value();
+        return twist.velocity().value();
     }
 
     private void drawPoseHistory(Canvas c) {
@@ -395,8 +394,8 @@ public final class MecanumDrive {
 
         int i = 0;
         for (Pose2d t : poseHistory) {
-            xPoints[i] = t.trans.x;
+            xPoints[i] = t.position.x;
-            yPoints[i] = t.trans.y;
+            yPoints[i] = t.position.y;
 
             i++;
         }
@@ -410,10 +409,10 @@ public final class MecanumDrive {
         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 halfv = t.heading.vec().times(0.5 * ROBOT_RADIUS);
-        Vector2d p1 = t.trans.plus(halfv);
+        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 final class MecanumDrive {
         return new TrajectoryActionBuilder(
                 TurnAction::new,
                 FollowTrajectoryAction::new,
-                beginPose, 1e-6,
+                beginPose, 1e-6, 0.0,
                 defaultTurnConstraints,
                 defaultVelConstraint, defaultAccelConstraint,
-                0.25
+                0.25, 0.1
         );
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java

@@ -15,6 +15,8 @@ import com.acmerobotics.roadrunner.MinVelConstraint;
 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.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.Vector2dDual;
 import com.acmerobotics.roadrunner.VelConstraint;
@@ -135,7 +136,7 @@ public final class TankDrive {
         }
 
         @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 final class TankDrive {
             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 final class TankDrive {
             lastLeftPos = meanLeftPos;
             lastRightPos = meanRightPos;
 
-            return kinematics.forward(incrs);
+            return kinematics.forward(twist);
         }
     }
 
@@ -203,8 +204,9 @@ public final class TankDrive {
         localizer = new TankDrive.DriveLocalizer();
     }
 
-    public void setDrivePowers(Twist2d powers) {
+    public void setDrivePowers(PoseVelocity2d powers) {
-        TankKinematics.WheelVelocities<Time> wheelVels = new TankKinematics(2).inverse(Twist2dDual.constant(powers, 1));
+        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 final class TankDrive {
             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;
+                xPoints[i] = p.position.x;
-                yPoints[i] = p.trans.y;
+                yPoints[i] = p.position.y;
             }
         }
 
@@ -267,7 +269,7 @@ public final class TankDrive {
 
             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 final class TankDrive {
                 m.setPower(feedforward.compute(wheelVels.right) / voltage);
             }
 
-            p.put("x", pose.trans.x);
+            p.put("x", pose.position.x);
-            p.put("y", pose.trans.y);
+            p.put("y", pose.position.y);
-            p.put("heading (deg)", Math.toDegrees(pose.rot.log()));
+            p.put("heading (deg)", Math.toDegrees(pose.heading.log()));
 
             Pose2d error = txWorldTarget.value().minusExp(pose);
-            p.put("xError", error.trans.x);
+            p.put("xError", error.position.x);
-            p.put("yError", error.trans.y);
+            p.put("yError", error.position.y);
-            p.put("headingError (deg)", Math.toDegrees(error.rot.log()));
+            p.put("headingError (deg)", Math.toDegrees(error.heading.log()));
 
             LogFiles.recordTargetPose(txWorldTarget.value());
 
@@ -347,13 +349,13 @@ public final class TankDrive {
 
             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(
+                    txWorldTarget.heading.velocity().plus(
-                            TURN_GAIN * pose.rot.minus(txWorldTarget.rot.value()) +
+                            TURN_GAIN * pose.heading.minus(txWorldTarget.heading.value()) +
-                            TURN_VEL_GAIN * (robotVelRobot.rotVel - txWorldTarget.rot.velocity().value())
+                            TURN_VEL_GAIN * (robotVelRobot.angVel - txWorldTarget.heading.velocity().value())
                     )
             );
 
@@ -378,7 +380,7 @@ public final class TankDrive {
             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 final class TankDrive {
         @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() {
+    public PoseVelocity2d updatePoseEstimate() {
-        Twist2dIncrDual<Time> incr = localizer.update();
+        Twist2dDual<Time> twist = localizer.update();
-        pose = pose.plus(incr.value());
+        pose = pose.plus(twist.value());
 
         poseHistory.add(pose);
         while (poseHistory.size() > 100) {
@@ -401,7 +403,7 @@ public final class TankDrive {
 
         LogFiles.recordPose(pose);
 
-        return incr.velocity().value();
+        return twist.velocity().value();
     }
 
     private void drawPoseHistory(Canvas c) {
@@ -410,8 +412,8 @@ public final class TankDrive {
 
         int i = 0;
         for (Pose2d t : poseHistory) {
-            xPoints[i] = t.trans.x;
+            xPoints[i] = t.position.x;
-            yPoints[i] = t.trans.y;
+            yPoints[i] = t.position.y;
 
             i++;
         }
@@ -425,10 +427,10 @@ public final class TankDrive {
         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 halfv = t.heading.vec().times(0.5 * ROBOT_RADIUS);
-        Vector2d p1 = t.trans.plus(halfv);
+        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 final class TankDrive {
         return new TrajectoryActionBuilder(
                 TurnAction::new,
                 FollowTrajectoryAction::new,
-                beginPose, 1e-6,
+                beginPose, 1e-6, 0.0,
                 defaultTurnConstraints,
                 defaultVelConstraint, defaultAccelConstraint,
-                0.25
+                0.25, 0.1
         );
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java

@@ -3,7 +3,7 @@ package org.firstinspires.ftc.teamcode;
 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 final class ThreeDeadWheelLocalizer implements Localizer {
         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 final class ThreeDeadWheelLocalizer implements Localizer {
         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 final class ThreeDeadWheelLocalizer implements Localizer {
         lastPar1Pos = par1PosVel.position;
         lastPerpPos = perpPosVel.position;
 
-        return twistIncr;
+        return twist;
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java

@@ -4,7 +4,7 @@ import com.acmerobotics.dashboard.config.Config;
 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 final class TwoDeadWheelLocalizer implements Localizer {
         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 final class TwoDeadWheelLocalizer implements Localizer {
 
         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 final class TwoDeadWheelLocalizer implements Localizer {
         lastPerpPos = perpPosVel.position;
         lastHeading = heading;
 
-        return twistIncr;
+        return twist;
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveView.java

@@ -2,7 +2,7 @@ package org.firstinspires.ftc.teamcode.tuning;
 
 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 @@ final class DriveView {
         throw new AssertionError();
     }
 
-    public void setDrivePowers(Twist2d powers) {
+    public void setDrivePowers(PoseVelocity2d powers) {
         if (md != null) {
             md.setDrivePowers(powers);
             return;

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 class LocalizationTest extends LinearOpMode {
             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 class LocalizationTest extends LinearOpMode {
 
                 drive.updatePoseEstimate();
 
-                telemetry.addData("x", drive.pose.trans.x);
+                telemetry.addData("x", drive.pose.position.x);
-                telemetry.addData("y", drive.pose.trans.y);
+                telemetry.addData("y", drive.pose.position.y);
-                telemetry.addData("heading", drive.pose.rot);
+                telemetry.addData("heading", drive.pose.heading);
                 telemetry.update();
             }
         } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
@@ -38,7 +39,7 @@ public class LocalizationTest extends LinearOpMode {
             waitForStart();
 
             while (opModeIsActive()) {
-                drive.setDrivePowers(new Twist2d(
+                drive.setDrivePowers(new PoseVelocity2d(
                         new Vector2d(
                                 -gamepad1.left_stick_y,
                                 0.0
@@ -48,9 +49,9 @@ public class LocalizationTest extends LinearOpMode {
 
                 drive.updatePoseEstimate();
 
-                telemetry.addData("x", drive.pose.trans.x);
+                telemetry.addData("x", drive.pose.position.x);
-                telemetry.addData("y", drive.pose.trans.y);
+                telemetry.addData("y", drive.pose.position.y);
-                telemetry.addData("heading", drive.pose.rot);
+                telemetry.addData("heading", drive.pose.heading);
                 telemetry.update();
             }
         } else {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedforwardTuner.java

@@ -4,6 +4,7 @@ import com.acmerobotics.dashboard.FtcDashboard;
 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 class ManualFeedforwardTuner extends LinearOpMode {
                     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 class ManualFeedforwardTuner extends LinearOpMode {
                     }
 
                     view.setDrivePowers(
-                            new Twist2d(
+                            new PoseVelocity2d(
                                     new Vector2d(
                                             -gamepad1.left_stick_y,
                                             -gamepad1.left_stick_x

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();
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java

@@ -106,18 +106,18 @@ public final class LogFiles {
     }
 
     public static void recordTargetPose(Pose2d targetPose) {
-        log.targetXs.add(targetPose.trans.x);
+        log.targetXs.add(targetPose.position.x);
-        log.targetYs.add(targetPose.trans.y);
+        log.targetYs.add(targetPose.position.y);
-        log.targetHeadings.add(targetPose.rot.log());
+        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.xs.add(pose.position.x);
-        log.ys.add(pose.trans.y);
+        log.ys.add(pose.position.y);
-        log.headings.add(pose.rot.log());
+        log.headings.add(pose.heading.log());
     }
 
     private static final OpModeManagerNotifier.Notifications notifHandler = new OpModeManagerNotifier.Notifications() {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Paths.java

@@ -16,11 +16,11 @@ public final class Paths {
         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) {