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

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

wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp

@@ -98,6 +98,8 @@ void DifferentialDrivetrainSim::SetPose(const frc::Pose2d& pose) {
   m_x(State::kX) = pose.X().to<double>();
   m_x(State::kY) = pose.Y().to<double>();
   m_x(State::kHeading) = pose.Rotation().Radians().to<double>();
+  m_x(State::kLeftPosition) = 0;
+  m_x(State::kRightPosition) = 0;
 }
 
 Eigen::Matrix<double, 7, 1> DifferentialDrivetrainSim::Dynamics(

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java

@@ -192,6 +192,8 @@ public class DifferentialDrivetrainSim {
     m_x.set(State.kX.value, 0, pose.getX());
     m_x.set(State.kY.value, 0, pose.getY());
     m_x.set(State.kHeading.value, 0, pose.getRotation().getRadians());
+    m_x.set(State.kLeftPosition.value, 0, 0);
+    m_x.set(State.kRightPosition.value, 0, 0);
   }
 
   @SuppressWarnings({"DuplicatedCode", "LocalVariableName"})

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