Add Maple-Sim

build.gradle

@@ -99,4 +99,4 @@ wpi.java.configureTestTasks(test)
 // Configure string concat to always inline compile
 tasks.withType(JavaCompile) {
     options.compilerArgs.add '-XDstringConcat=inline'
-}
+}

simgui-ds.json

@@ -98,7 +98,7 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "Keyboard0"
+      "guid": "030000004c050000e60c000000000000"
     },
     {
       "guid": "030000004c050000c405000000000000"

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

@@ -96,6 +96,10 @@ public RobotContainer()
    */
   private void configureBindings()
   {
+    if (Robot.isSimulation())
+    {
+      driverXbox.start().onTrue(Commands.runOnce(() -> drivebase.resetOdometry(new Pose2d(3, 3, new Rotation2d()))));
+    }
     if (DriverStation.isTest())
     {
       driverXbox.b().whileTrue(drivebase.sysIdDriveMotorCommand());

src/main/java/frc/robot/subsystems/swervedrive/Vision.java

@@ -129,10 +129,16 @@ public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset)
    */
   public void updatePoseEstimation(SwerveDrive swerveDrive)
   {
-    if (SwerveDriveTelemetry.isSimulation)
+    if (SwerveDriveTelemetry.isSimulation && swerveDrive.getSimulationDriveTrainPose().isPresent())
     {
-      visionSim.update(swerveDrive.getPose());
-
+      /*
+       * In the maple-sim, odometry is simulated using encoder values, accounting for factors like skidding and drifting.
+       * As a result, the odometry may not always be 100% accurate.
+       * However, the vision system should be able to provide a reasonably accurate pose estimation, even when odometry is incorrect.
+       * (This is why teams implement vision system to correct odometry.)
+       * Therefore, we must ensure that the actual robot pose is provided in the simulator when updating the vision simulation during the simulation.
+       */
+      visionSim.update(swerveDrive.getSimulationDriveTrainPose().get());
     }
     for (Cameras camera : Cameras.values())
     {

src/main/java/swervelib/SwerveDrive.java

@@ -21,10 +21,8 @@
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.*;
 import edu.wpi.first.wpilibj.Alert.AlertType;
-import edu.wpi.first.wpilibj.Notifier;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import java.util.ArrayList;
@@ -34,6 +32,7 @@
 import java.util.Optional;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
+
 import org.ironmaple.simulation.SimulatedArena;
 import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
 import org.ironmaple.simulation.drivesims.SwerveModuleSimulation;
@@ -51,6 +50,8 @@
 import swervelib.telemetry.SwerveDriveTelemetry;
 import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
 
+import static edu.wpi.first.units.Units.*;
+
 /**
  * Swerve Drive class representing and controlling the swerve drive.
  */
@@ -199,48 +200,51 @@ public SwerveDrive(
     kinematics = new SwerveDriveKinematics(config.moduleLocationsMeters);
     odometryThread = new Notifier(this::updateOdometry);
 
+    this.swerveModules = config.modules;
+
     // Create an integrator for angle if the robot is being simulated to emulate an IMU
     // If the robot is real, instantiate the IMU instead.
     if (SwerveDriveTelemetry.isSimulation)
     {
-      simIMU = new SwerveIMUSimulation();
-      imuReadingCache = new Cache<>(simIMU::getGyroRotation3d, 5L);
+      DriveTrainSimulationConfig simulationConfig = DriveTrainSimulationConfig.Default()
+              .withBumperSize(
+                      Meters.of(config.getTracklength()).plus(Inches.of(5)),
+                      Meters.of(config.getTrackwidth()).plus(Inches.of(5)))
+              .withRobotMass(Kilograms.of(config.physicalCharacteristics.robotMassKg))
+              .withCustomModuleTranslations(config.moduleLocationsMeters)
+              .withGyro(config.getGyroSim())
+              .withSwerveModule(() -> new SwerveModuleSimulation(
+                      config.getDriveMotorSim(),
+                      config.getAngleMotorSim(),
+                      config.physicalCharacteristics.conversionFactor.drive.gearRatio,
+                      config.physicalCharacteristics.conversionFactor.angle.gearRatio,
+                      Amps.of(config.physicalCharacteristics.driveMotorCurrentLimit),
+                      Amps.of(20),
+                      Volts.of(config.physicalCharacteristics.driveFrictionVoltage),
+                      Volts.of(config.physicalCharacteristics.angleFrictionVoltage),
+                      Inches.of(config.physicalCharacteristics.conversionFactor.drive.diameter/2),
+                      KilogramSquareMeters.of(0.02),
+                      config.physicalCharacteristics.wheelGripCoefficientOfFriction));
+
+      mapleSimDrive = new SwerveDriveSimulation(simulationConfig, startingPose);
+
+      // feed module simulation instances to modules
+      for (int i = 0; i < swerveModules.length; i++)
+      {
+        this.swerveModules[i].configureModuleSimulation(mapleSimDrive.getModules()[i]);
+      }
 
-      // Setup MapleSim
-      SwerveModuleSimulation simModule = new SwerveModuleSimulation(config.getDriveMotorSim(),
-                                                                    config.getAngleMotorSim(),
-                                                                    config.physicalCharacteristics.driveMotorCurrentLimit,
-                                                                    config.physicalCharacteristics.conversionFactor.drive.gearRatio,
-                                                                    config.physicalCharacteristics.conversionFactor.angle.gearRatio,
-                                                                    config.physicalCharacteristics.driveFrictionVoltage,
-                                                                    config.physicalCharacteristics.angleFrictionVoltage,
-                                                                    config.physicalCharacteristics.wheelGripCoefficientOfFriction,
-                                                                    config.physicalCharacteristics.conversionFactor.drive.diameter /
-                                                                    2,
-                                                                    config.physicalCharacteristics.steerRotationalInertia);
-      DriveTrainSimulationConfig simCfg = new DriveTrainSimulationConfig(config.physicalCharacteristics.robotMassKg,
-                                                                         config.getTracklength() +
-                                                                         Units.inchesToMeters(5),
-                                                                         config.getTrackwidth() +
-                                                                         Units.inchesToMeters(5),
-                                                                         config.getTracklength(),
-                                                                         config.getTrackwidth(),
-                                                                         () -> {
-                                                                           return simModule;
-                                                                         },
-                                                                         config.getGyroSim());
-      mapleSimDrive = new SwerveDriveSimulation(simCfg, startingPose);
       // register the drivetrain simulation
       SimulatedArena.getInstance().addDriveTrainSimulation(mapleSimDrive);
-    } else
-    {
+
+      simIMU = new SwerveIMUSimulation(mapleSimDrive.getGyroSimulation());
+      imuReadingCache = new Cache<>(simIMU::getGyroRotation3d, 5L);
+    } else {
       imu = config.imu;
       imu.factoryDefault();
       imuReadingCache = new Cache<>(imu::getRotation3d, 5L);
     }
 
-    this.swerveModules = config.modules;
-
     //    odometry = new SwerveDriveOdometry(kinematics, getYaw(), getModulePositions());
     swerveDrivePoseEstimator =
         new SwerveDrivePoseEstimator(
@@ -287,7 +291,7 @@ public SwerveDrive(
       SwerveDriveTelemetry.measuredStatesObj = new SwerveModuleState[SwerveDriveTelemetry.moduleCount];
     }
 
-    odometryThread.startPeriodic(SwerveDriveTelemetry.isSimulation ? 0.01 : 0.02);
+    setOdometryPeriod(SwerveDriveTelemetry.isSimulation ? 0.004 : 0.02);
 
     checkIfTunerXCompatible();
 
@@ -344,6 +348,7 @@ private void checkIfTunerXCompatible()
   public void setOdometryPeriod(double period)
   {
     odometryThread.stop();
+    SimulatedArena.overrideSimulationTimings(Seconds.of(period), 1);
     odometryThread.startPeriodic(period);
   }
 
@@ -353,6 +358,7 @@ public void setOdometryPeriod(double period)
   public void stopOdometryThread()
   {
     odometryThread.stop();
+    SimulatedArena.overrideSimulationTimings(Seconds.of(TimedRobot.kDefaultPeriod), 5);
   }
 
   /**
@@ -699,7 +705,7 @@ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds)
   }
 
   /**
-   * Gets the current pose (position and rotation) of the robot, as reported by odometry.
+   * Gets the measured pose (position and rotation) of the robot, as reported by odometry.
    *
    * @return The robot's pose
    */
@@ -713,7 +719,25 @@ public Pose2d getPose()
   }
 
   /**
-   * Gets the current field-relative velocity (x, y and omega) of the robot
+   * Gets the actual pose of the drivetrain during simulation
+   *
+   * @return an optional Pose2d, representing the drivetrain pose during simulation, or an empty optional when running on real robot
+   * */
+  public Optional<Pose2d> getSimulationDriveTrainPose()
+  {
+    if (SwerveDriveTelemetry.isSimulation)
+    {
+      odometryLock.lock();
+      Pose2d simulationPose = mapleSimDrive.getSimulatedDriveTrainPose();
+      odometryLock.unlock();
+      return Optional.of(simulationPose);
+    }
+
+    return Optional.empty();
+  }
+
+  /**
+   * Gets the measured field-relative robot velocity (x, y and omega)
    *
    * @return A ChassisSpeeds object of the current field-relative velocity
    */
@@ -727,6 +751,26 @@ public ChassisSpeeds getFieldVelocity()
         kinematics.toChassisSpeeds(getStates()), getOdometryHeading().unaryMinus());
   }
 
+  /**
+   * Gets the actual field-relative robot velocity (x, y and omega) during simulation
+   *
+   * @return An optional ChassisSpeeds representing the actual field-relative velocity of the robot, or an empty optional when running on real robot
+   * @deprecated for testing version of maple-sim only
+   */
+  @Deprecated
+  public Optional<ChassisSpeeds> getSimulationFieldVelocity()
+  {
+    if (SwerveDriveTelemetry.isSimulation)
+    {
+      odometryLock.lock();
+      ChassisSpeeds simulationFieldRelativeVelocity = mapleSimDrive.getDriveTrainSimulatedChassisSpeedsFieldRelative();
+      odometryLock.unlock();
+      return Optional.of(simulationFieldRelativeVelocity);
+    }
+
+    return Optional.empty();
+  }
+
   /**
    * Gets the current robot-relative velocity (x, y and omega) of the robot
    *
@@ -737,6 +781,26 @@ public ChassisSpeeds getRobotVelocity()
     return kinematics.toChassisSpeeds(getStates());
   }
 
+  /**
+   * Gets the actual robot-relative robot velocity (x, y and omega) during simulation
+   *
+   * @return An optional ChassisSpeeds representing the actual robot-relative velocity of the robot, or an empty optional when running on real robot
+   * @deprecated for testing version of maple-sim only
+   */
+  @Deprecated
+  public Optional<ChassisSpeeds> getSimulationRobotVelocity()
+  {
+    if (SwerveDriveTelemetry.isSimulation)
+    {
+      odometryLock.lock();
+      ChassisSpeeds simulationFieldRelativeVelocity = mapleSimDrive.getDriveTrainSimulatedChassisSpeedsRobotRelative();
+      odometryLock.unlock();
+      return Optional.of(simulationFieldRelativeVelocity);
+    }
+
+    return Optional.empty();
+  }
+
   /**
    * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when calling this
    * method. However, if either gyro angle or module position is reset, this must be called in order for odometry to
@@ -748,8 +812,13 @@ public void resetOdometry(Pose2d pose)
   {
     odometryLock.lock();
     swerveDrivePoseEstimator.resetPosition(getYaw(), getModulePositions(), pose);
+    if (SwerveDriveTelemetry.isSimulation)
+    {
+      mapleSimDrive.setSimulationWorldPose(pose);
+    }
     odometryLock.unlock();
     kinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, getYaw()));
+
   }
 
   /**
@@ -1048,13 +1117,17 @@ public void updateOdometry()
       // Update odometry
       swerveDrivePoseEstimator.update(getYaw(), getModulePositions());
 
+      if (SwerveDriveTelemetry.isSimulation)
+      {
+        SimulatedArena.getInstance().simulationPeriodic();
+      }
+
       // Update angle accumulator if the robot is simulated
       if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
       {
         Pose2d[] modulePoses = getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition());
         if (SwerveDriveTelemetry.isSimulation)
         {
-          SimulatedArena.getInstance().simulationPeriodic();
           simIMU.updateOdometry(
               kinematics,
               getStates(),
@@ -1073,7 +1146,14 @@ public void updateOdometry()
 
       if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal())
       {
-        field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition());
+        if (SwerveDriveTelemetry.isSimulation)
+        {
+          field.setRobotPose(mapleSimDrive.getSimulatedDriveTrainPose());
+          field.getObject("OdometryPose").setPose(swerveDrivePoseEstimator.getEstimatedPosition());
+        } else
+        {
+          field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition());
+        }
       }
 
       double sumVelocity = 0;

src/main/java/swervelib/SwerveModule.java

@@ -171,6 +171,11 @@ public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat
       absoluteEncoder.configure(moduleConfiguration.absoluteEncoderInverted);
     }
 
+    if (SwerveDriveTelemetry.isSimulation)
+    {
+      simModule = new SwerveModuleSimulation();
+    }
+
     // Setup the cache for the absolute encoder position.
     absolutePositionCache = new Cache<>(this::getRawAbsolutePosition, 20);
 
@@ -525,6 +530,13 @@ public double getAbsolutePosition()
    */
   public double getRawAbsolutePosition()
   {
+    /* During simulation, when no absolute encoders are available, we return the state from the simulation module instead. */
+    if (SwerveDriveTelemetry.isSimulation)
+    {
+      Rotation2d absolutePosition = simModule.getState().angle;
+      return absolutePosition.getDegrees();
+    }
+
     double angle;
     if (absoluteEncoder != null)
     {
@@ -709,4 +721,19 @@ public void invalidateCache()
     drivePositionCache.update();
     driveVelocityCache.update();
   }
+
+  /**
+   * Obtains the {@link SwerveModuleSimulation} used in simulation.
+   *
+   * @return the module simulation, <b>null</b> if this method is called on a real robot
+   * */
+  public SwerveModuleSimulation getSimModule()
+  {
+    return simModule;
+  }
+
+  public void configureModuleSimulation(org.ironmaple.simulation.drivesims.SwerveModuleSimulation swerveModuleSimulation)
+  {
+    this.simModule.configureSimModule(swerveModuleSimulation);
+  }
 }