[wpilib] Fix DiffDriveSim pose reset and example (#2837)

Calling the resetPosition method on an odometry instance expects encoder positions to be reset to zero.

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java

@@ -130,6 +130,9 @@ public class RobotContainer {
         m_robotDrive
     );
 
+    // Reset odometry to starting pose of trajectory.
+    m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
+
     // Run path following command, then stop at the end.
     return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVolts(0, 0));
   }

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java

@@ -163,6 +163,7 @@ public class DriveSubsystem extends SubsystemBase {
    */
   public void resetOdometry(Pose2d pose) {
     resetEncoders();
+    m_drivetrainSimulator.setPose(pose);
     m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading()));
   }