Add new pose reset for initial paths

src/main/java/frc/robot/RobotContainer.java

@@ -288,8 +288,8 @@ public RobotContainer() {
         autoChooser_ = new LoggedDashboardChooser<>("Auto Choices");
 
         autoChooser_.onChange(auto -> {
-            System.err.println("Auto \"" + auto.getName() + "\" selected!");
-            // This should be used to set up robot position setting, initialization, etc.
+            System.out.println("Auto \"" + auto.getName() + "\" selected!");
+            // Anything you may want to do when the auto is selected.
         });
 
         // Test Bindings

src/main/java/frc/robot/commands/drive/DriveCommands.java

@@ -62,6 +62,7 @@
 import frc.robot.Constants.FieldConstants;
 import frc.robot.Constants.Mode;
 import frc.robot.subsystems.drive.Drive;
+import frc.robot.util.MapleSimUtil;
 
 public class DriveCommands {
   private static final double kStoppedVelocity = 0.15 ;
@@ -593,10 +594,15 @@ public static Command initialFollowPathCommand(Drive drive, String pathName, boo
         return Commands.none();
       }
 
+      var startingPose = initPosePath.getStartingHolonomicPose().orElseThrow();
+
       return Commands.sequence(
           setPoseCommand(
               drive,
-              initPosePath.getStartingHolonomicPose().orElseThrow(), false),
+              startingPose,
+              false
+          ),
+          Commands.runOnce(() -> MapleSimUtil.placeRobotOnField(startingPose)),
           AutoBuilder.followPath(path.get()));
     }
 

src/main/java/frc/robot/util/MapleSimUtil.java

@@ -137,6 +137,10 @@ public static Pose2d getPosition() {
         return drivebaseSimulation.getSimulatedDriveTrainPose();
     }
 
+    public static void placeRobotOnField(Pose2d pose) {
+        drivebaseSimulation.setSimulationWorldPose(pose);
+    }
+
     public static ChassisSpeeds getFieldChassisSpeeds() {
         return drivebaseSimulation.getDriveTrainSimulatedChassisSpeedsFieldRelative();
     }