diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
index cf6ebb5b43..de1b2d05b2 100644
--- a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
+++ b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,7 +10,8 @@
 using namespace frc;
 
 MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
-    const ChassisSpeeds& chassisSpeeds, const Translation2d& centerOfRotation) {
+    const ChassisSpeeds& chassisSpeeds,
+    const Translation2d& centerOfRotation) const {
   // We have a new center of rotation. We need to compute the matrix again.
   if (centerOfRotation != m_previousCoR) {
     auto fl = m_frontLeftWheel - centerOfRotation;
@@ -39,7 +40,7 @@ MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
 }
 
 ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
-    const MecanumDriveWheelSpeeds& wheelSpeeds) {
+    const MecanumDriveWheelSpeeds& wheelSpeeds) const {
   Eigen::Matrix<double, 4, 1> wheelSpeedsMatrix;
   // clang-format off
   wheelSpeedsMatrix << wheelSpeeds.frontLeft.to<double>(), wheelSpeeds.frontRight.to<double>(),
@@ -57,7 +58,7 @@ ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
 void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
                                                   Translation2d fr,
                                                   Translation2d rl,
-                                                  Translation2d rr) {
+                                                  Translation2d rr) const {
   // clang-format off
   m_inverseKinematics << 1, -1, (-(fl.X() + fl.Y())).template to<double>(),
                          1,  1, (fr.X() - fr.Y()).template to<double>(),
diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
index c3e367a033..b43a54779d 100644
--- a/wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
+++ b/wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
@@ -15,7 +15,7 @@ CentripetalAccelerationConstraint::CentripetalAccelerationConstraint(
 
 units::meters_per_second_t CentripetalAccelerationConstraint::MaxVelocity(
     const Pose2d& pose, units::curvature_t curvature,
-    units::meters_per_second_t velocity) {
+    units::meters_per_second_t velocity) const {
   // ac = v^2 / r
   // k (curvature) = 1 / r
 
@@ -33,7 +33,7 @@ units::meters_per_second_t CentripetalAccelerationConstraint::MaxVelocity(
 TrajectoryConstraint::MinMax
 CentripetalAccelerationConstraint::MinMaxAcceleration(
     const Pose2d& pose, units::curvature_t curvature,
-    units::meters_per_second_t speed) {
+    units::meters_per_second_t speed) const {
   // The acceleration of the robot has no impact on the centripetal acceleration
   // of the robot.
   return {};
diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
index bd02bcf25d..200f21905e 100644
--- a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
+++ b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
@@ -15,7 +15,7 @@ DifferentialDriveKinematicsConstraint::DifferentialDriveKinematicsConstraint(
 
 units::meters_per_second_t DifferentialDriveKinematicsConstraint::MaxVelocity(
     const Pose2d& pose, units::curvature_t curvature,
-    units::meters_per_second_t velocity) {
+    units::meters_per_second_t velocity) const {
   auto wheelSpeeds =
       m_kinematics.ToWheelSpeeds({velocity, 0_mps, velocity * curvature});
   wheelSpeeds.Normalize(m_maxSpeed);
@@ -26,6 +26,6 @@ units::meters_per_second_t DifferentialDriveKinematicsConstraint::MaxVelocity(
 TrajectoryConstraint::MinMax
 DifferentialDriveKinematicsConstraint::MinMaxAcceleration(
     const Pose2d& pose, units::curvature_t curvature,
-    units::meters_per_second_t speed) {
+    units::meters_per_second_t speed) const {
   return {};
 }
diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
index 7019e63243..652577d4bf 100644
--- a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
+++ b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
@@ -23,14 +23,14 @@ DifferentialDriveVoltageConstraint::DifferentialDriveVoltageConstraint(
 
 units::meters_per_second_t DifferentialDriveVoltageConstraint::MaxVelocity(
     const Pose2d& pose, units::curvature_t curvature,
-    units::meters_per_second_t velocity) {
+    units::meters_per_second_t velocity) const {
   return units::meters_per_second_t(std::numeric_limits<double>::max());
 }
 
 TrajectoryConstraint::MinMax
 DifferentialDriveVoltageConstraint::MinMaxAcceleration(
     const Pose2d& pose, units::curvature_t curvature,
-    units::meters_per_second_t speed) {
+    units::meters_per_second_t speed) const {
   auto wheelSpeeds =
       m_kinematics.ToWheelSpeeds({speed, 0_mps, speed * curvature});
 
diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp
index c70496dd99..52d0074a02 100644
--- a/wpilibc/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp
+++ b/wpilibc/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp
@@ -13,7 +13,7 @@ using namespace frc;
 
 EllipticalRegionConstraint::EllipticalRegionConstraint(
     const Translation2d& center, units::meter_t xWidth, units::meter_t yWidth,
-    const Rotation2d& rotation, TrajectoryConstraint& constraint)
+    const Rotation2d& rotation, const TrajectoryConstraint& constraint)
     : m_center(center),
       m_radii(xWidth / 2.0, yWidth / 2.0),
       m_constraint(constraint) {
@@ -22,7 +22,7 @@ EllipticalRegionConstraint::EllipticalRegionConstraint(
 
 units::meters_per_second_t EllipticalRegionConstraint::MaxVelocity(
     const Pose2d& pose, units::curvature_t curvature,
-    units::meters_per_second_t velocity) {
+    units::meters_per_second_t velocity) const {
   if (IsPoseInRegion(pose)) {
     return m_constraint.MaxVelocity(pose, curvature, velocity);
   } else {
@@ -32,7 +32,7 @@ units::meters_per_second_t EllipticalRegionConstraint::MaxVelocity(
 
 TrajectoryConstraint::MinMax EllipticalRegionConstraint::MinMaxAcceleration(
     const Pose2d& pose, units::curvature_t curvature,
-    units::meters_per_second_t speed) {
+    units::meters_per_second_t speed) const {
   if (IsPoseInRegion(pose)) {
     return m_constraint.MinMaxAcceleration(pose, curvature, speed);
   } else {
@@ -40,7 +40,7 @@ TrajectoryConstraint::MinMax EllipticalRegionConstraint::MinMaxAcceleration(
   }
 }
 
-bool EllipticalRegionConstraint::IsPoseInRegion(const Pose2d& pose) {
+bool EllipticalRegionConstraint::IsPoseInRegion(const Pose2d& pose) const {
   // The region (disk) bounded by the ellipse is given by the equation:
   // ((x-h)^2)/Rx^2) + ((y-k)^2)/Ry^2) <= 1
   // If the inequality is satisfied, then it is inside the ellipse; otherwise
diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
index 17008ad90e..5b8a3a13bb 100644
--- a/wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
+++ b/wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
@@ -15,7 +15,7 @@ MecanumDriveKinematicsConstraint::MecanumDriveKinematicsConstraint(
 
 units::meters_per_second_t MecanumDriveKinematicsConstraint::MaxVelocity(
     const Pose2d& pose, units::curvature_t curvature,
-    units::meters_per_second_t velocity) {
+    units::meters_per_second_t velocity) const {
   auto xVelocity = velocity * pose.Rotation().Cos();
   auto yVelocity = velocity * pose.Rotation().Sin();
   auto wheelSpeeds =
@@ -30,6 +30,6 @@ units::meters_per_second_t MecanumDriveKinematicsConstraint::MaxVelocity(
 TrajectoryConstraint::MinMax
 MecanumDriveKinematicsConstraint::MinMaxAcceleration(
     const Pose2d& pose, units::curvature_t curvature,
-    units::meters_per_second_t speed) {
+    units::meters_per_second_t speed) const {
   return {};
 }
diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp
index a5927aecf0..7d76c70397 100644
--- a/wpilibc/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp
+++ b/wpilibc/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp
@@ -13,14 +13,14 @@ using namespace frc;
 
 RectangularRegionConstraint::RectangularRegionConstraint(
     const Translation2d& bottomLeftPoint, const Translation2d& topRightPoint,
-    TrajectoryConstraint& constraint)
+    const TrajectoryConstraint& constraint)
     : m_bottomLeftPoint(bottomLeftPoint),
       m_topRightPoint(topRightPoint),
       m_constraint(constraint) {}
 
 units::meters_per_second_t RectangularRegionConstraint::MaxVelocity(
     const Pose2d& pose, units::curvature_t curvature,
-    units::meters_per_second_t velocity) {
+    units::meters_per_second_t velocity) const {
   if (IsPoseInRegion(pose)) {
     return m_constraint.MaxVelocity(pose, curvature, velocity);
   } else {
@@ -30,7 +30,7 @@ units::meters_per_second_t RectangularRegionConstraint::MaxVelocity(
 
 TrajectoryConstraint::MinMax RectangularRegionConstraint::MinMaxAcceleration(
     const Pose2d& pose, units::curvature_t curvature,
-    units::meters_per_second_t speed) {
+    units::meters_per_second_t speed) const {
   if (IsPoseInRegion(pose)) {
     return m_constraint.MinMaxAcceleration(pose, curvature, speed);
   } else {
@@ -38,7 +38,7 @@ TrajectoryConstraint::MinMax RectangularRegionConstraint::MinMaxAcceleration(
   }
 }
 
-bool RectangularRegionConstraint::IsPoseInRegion(const Pose2d& pose) {
+bool RectangularRegionConstraint::IsPoseInRegion(const Pose2d& pose) const {
   return pose.Translation().X() >= m_bottomLeftPoint.X() &&
          pose.Translation().X() <= m_topRightPoint.X() &&
          pose.Translation().Y() >= m_bottomLeftPoint.Y() &&
diff --git a/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
index 3fcdebf008..6f1ef66536 100644
--- a/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
+++ b/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
@@ -68,7 +68,8 @@ class SimpleMotorFeedforward {
    * @return The maximum possible velocity at the given acceleration.
    */
   constexpr units::unit_t<Velocity> MaxAchievableVelocity(
-      units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
+      units::volt_t maxVoltage,
+      units::unit_t<Acceleration> acceleration) const {
     // Assume max velocity is positive
     return (maxVoltage - kS - kA * acceleration) / kV;
   }
@@ -85,7 +86,8 @@ class SimpleMotorFeedforward {
    * @return The minimum possible velocity at the given acceleration.
    */
   constexpr units::unit_t<Velocity> MinAchievableVelocity(
-      units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
+      units::volt_t maxVoltage,
+      units::unit_t<Acceleration> acceleration) const {
     // Assume min velocity is positive, ks flips sign
     return (-maxVoltage + kS - kA * acceleration) / kV;
   }
@@ -102,7 +104,7 @@ class SimpleMotorFeedforward {
    * @return The maximum possible acceleration at the given velocity.
    */
   constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
-      units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
+      units::volt_t maxVoltage, units::unit_t<Velocity> velocity) const {
     return (maxVoltage - kS * wpi::sgn(velocity) - kV * velocity) / kA;
   }
 
@@ -118,7 +120,7 @@ class SimpleMotorFeedforward {
    * @return The minimum possible acceleration at the given velocity.
    */
   constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
-      units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
+      units::volt_t maxVoltage, units::unit_t<Velocity> velocity) const {
     return MaxAchievableAcceleration(-maxVoltage, velocity);
   }
 
diff --git a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
index 47b1b341d0..ccddc871ed 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
+++ b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -101,7 +101,7 @@ class MecanumDriveKinematics {
    */
   MecanumDriveWheelSpeeds ToWheelSpeeds(
       const ChassisSpeeds& chassisSpeeds,
-      const Translation2d& centerOfRotation = Translation2d());
+      const Translation2d& centerOfRotation = Translation2d()) const;
 
   /**
    * Performs forward kinematics to return the resulting chassis state from the
@@ -113,17 +113,18 @@ class MecanumDriveKinematics {
    *
    * @return The resulting chassis speed.
    */
-  ChassisSpeeds ToChassisSpeeds(const MecanumDriveWheelSpeeds& wheelSpeeds);
+  ChassisSpeeds ToChassisSpeeds(
+      const MecanumDriveWheelSpeeds& wheelSpeeds) const;
 
  private:
-  Eigen::Matrix<double, 4, 3> m_inverseKinematics;
+  mutable Eigen::Matrix<double, 4, 3> m_inverseKinematics;
   Eigen::HouseholderQR<Eigen::Matrix<double, 4, 3>> m_forwardKinematics;
   Translation2d m_frontLeftWheel;
   Translation2d m_frontRightWheel;
   Translation2d m_rearLeftWheel;
   Translation2d m_rearRightWheel;
 
-  Translation2d m_previousCoR;
+  mutable Translation2d m_previousCoR;
 
   /**
    * Construct inverse kinematics matrix from wheel locations.
@@ -138,7 +139,7 @@ class MecanumDriveKinematics {
    *           center of the robot.
    */
   void SetInverseKinematics(Translation2d fl, Translation2d fr,
-                            Translation2d rl, Translation2d rr);
+                            Translation2d rl, Translation2d rr) const;
 };
 
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
index f8893639c3..9717c7a058 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
+++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -108,7 +108,7 @@ class SwerveDriveKinematics {
    */
   std::array<SwerveModuleState, NumModules> ToSwerveModuleStates(
       const ChassisSpeeds& chassisSpeeds,
-      const Translation2d& centerOfRotation = Translation2d());
+      const Translation2d& centerOfRotation = Translation2d()) const;
 
   /**
    * Performs forward kinematics to return the resulting chassis state from the
@@ -123,7 +123,7 @@ class SwerveDriveKinematics {
    * @return The resulting chassis speed.
    */
   template <typename... ModuleStates>
-  ChassisSpeeds ToChassisSpeeds(ModuleStates&&... wheelStates);
+  ChassisSpeeds ToChassisSpeeds(ModuleStates&&... wheelStates) const;
 
   /**
    * Performs forward kinematics to return the resulting chassis state from the
@@ -139,7 +139,7 @@ class SwerveDriveKinematics {
    * @return The resulting chassis speed.
    */
   ChassisSpeeds ToChassisSpeeds(
-      std::array<SwerveModuleState, NumModules> moduleStates);
+      std::array<SwerveModuleState, NumModules> moduleStates) const;
 
   /**
    * Normalizes the wheel speeds using some max attainable speed. Sometimes,
@@ -158,12 +158,12 @@ class SwerveDriveKinematics {
       units::meters_per_second_t attainableMaxSpeed);
 
  private:
-  Eigen::Matrix<double, NumModules * 2, 3> m_inverseKinematics;
+  mutable Eigen::Matrix<double, NumModules * 2, 3> m_inverseKinematics;
   Eigen::HouseholderQR<Eigen::Matrix<double, NumModules * 2, 3>>
       m_forwardKinematics;
   std::array<Translation2d, NumModules> m_modules;
 
-  Translation2d m_previousCoR;
+  mutable Translation2d m_previousCoR;
 };
 }  // namespace frc
 
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
index b362aa0916..40442a18d5 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
+++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -18,7 +18,8 @@ SwerveDriveKinematics(Translation2d, Wheels...)
 template <size_t NumModules>
 std::array<SwerveModuleState, NumModules>
 SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
-    const ChassisSpeeds& chassisSpeeds, const Translation2d& centerOfRotation) {
+    const ChassisSpeeds& chassisSpeeds,
+    const Translation2d& centerOfRotation) const {
   // We have a new center of rotation. We need to compute the matrix again.
   if (centerOfRotation != m_previousCoR) {
     for (size_t i = 0; i < NumModules; i++) {
@@ -57,7 +58,7 @@ SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
 template <size_t NumModules>
 template <typename... ModuleStates>
 ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
-    ModuleStates&&... wheelStates) {
+    ModuleStates&&... wheelStates) const {
   static_assert(sizeof...(wheelStates) == NumModules,
                 "Number of modules is not consistent with number of wheel "
                 "locations provided in constructor.");
@@ -69,7 +70,7 @@ ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
 
 template <size_t NumModules>
 ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
-    std::array<SwerveModuleState, NumModules> moduleStates) {
+    std::array<SwerveModuleState, NumModules> moduleStates) const {
   Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix;
 
   for (size_t i = 0; i < NumModules; i++) {
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h
index 8e7d52d790..e46404ef4e 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h
@@ -29,10 +29,10 @@ class CentripetalAccelerationConstraint : public TrajectoryConstraint {
 
   units::meters_per_second_t MaxVelocity(
       const Pose2d& pose, units::curvature_t curvature,
-      units::meters_per_second_t velocity) override;
+      units::meters_per_second_t velocity) const override;
 
   MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
-                            units::meters_per_second_t speed) override;
+                            units::meters_per_second_t speed) const override;
 
  private:
   units::meters_per_second_squared_t m_maxCentripetalAcceleration;
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
index fe863318fa..260fc4c42f 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
@@ -26,10 +26,10 @@ class DifferentialDriveKinematicsConstraint : public TrajectoryConstraint {
 
   units::meters_per_second_t MaxVelocity(
       const Pose2d& pose, units::curvature_t curvature,
-      units::meters_per_second_t velocity) override;
+      units::meters_per_second_t velocity) const override;
 
   MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
-                            units::meters_per_second_t speed) override;
+                            units::meters_per_second_t speed) const override;
 
  private:
   DifferentialDriveKinematics m_kinematics;
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
index 4906ee5ec9..ae8d9adb44 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
@@ -38,10 +38,10 @@ class DifferentialDriveVoltageConstraint : public TrajectoryConstraint {
 
   units::meters_per_second_t MaxVelocity(
       const Pose2d& pose, units::curvature_t curvature,
-      units::meters_per_second_t velocity) override;
+      units::meters_per_second_t velocity) const override;
 
   MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
-                            units::meters_per_second_t speed) override;
+                            units::meters_per_second_t speed) const override;
 
  private:
   SimpleMotorFeedforward<units::meter> m_feedforward;
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
index 03a13f0798..1e5e673ccb 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
@@ -31,14 +31,14 @@ class EllipticalRegionConstraint : public TrajectoryConstraint {
    */
   EllipticalRegionConstraint(const Translation2d& center, units::meter_t xWidth,
                              units::meter_t yWidth, const Rotation2d& rotation,
-                             TrajectoryConstraint& constraint);
+                             const TrajectoryConstraint& constraint);
 
   units::meters_per_second_t MaxVelocity(
       const Pose2d& pose, units::curvature_t curvature,
-      units::meters_per_second_t velocity) override;
+      units::meters_per_second_t velocity) const override;
 
   MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
-                            units::meters_per_second_t speed) override;
+                            units::meters_per_second_t speed) const override;
 
   /**
    * Returns whether the specified robot pose is within the region that the
@@ -47,11 +47,11 @@ class EllipticalRegionConstraint : public TrajectoryConstraint {
    * @param pose The robot pose.
    * @return Whether the robot pose is within the constraint region.
    */
-  bool IsPoseInRegion(const Pose2d& pose);
+  bool IsPoseInRegion(const Pose2d& pose) const;
 
  private:
   Translation2d m_center;
   Translation2d m_radii;
-  TrajectoryConstraint& m_constraint;
+  const TrajectoryConstraint& m_constraint;
 };
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h
index b0f9fbefd5..042340e0ea 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h
@@ -29,12 +29,12 @@ class MaxVelocityConstraint : public TrajectoryConstraint {
 
   units::meters_per_second_t MaxVelocity(
       const Pose2d& pose, units::curvature_t curvature,
-      units::meters_per_second_t velocity) override {
+      units::meters_per_second_t velocity) const override {
     return m_maxVelocity;
   }
 
   MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
-                            units::meters_per_second_t speed) override {
+                            units::meters_per_second_t speed) const override {
     return {};
   }
 
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
index cb2e8db8c8..00e66c9710 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
@@ -28,10 +28,10 @@ class MecanumDriveKinematicsConstraint : public TrajectoryConstraint {
 
   units::meters_per_second_t MaxVelocity(
       const Pose2d& pose, units::curvature_t curvature,
-      units::meters_per_second_t velocity) override;
+      units::meters_per_second_t velocity) const override;
 
   MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
-                            units::meters_per_second_t speed) override;
+                            units::meters_per_second_t speed) const override;
 
  private:
   MecanumDriveKinematics m_kinematics;
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
index 250f769e50..134f0555e6 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
@@ -31,14 +31,14 @@ class RectangularRegionConstraint : public TrajectoryConstraint {
    */
   RectangularRegionConstraint(const Translation2d& bottomLeftPoint,
                               const Translation2d& topRightPoint,
-                              TrajectoryConstraint& constraint);
+                              const TrajectoryConstraint& constraint);
 
   units::meters_per_second_t MaxVelocity(
       const Pose2d& pose, units::curvature_t curvature,
-      units::meters_per_second_t velocity) override;
+      units::meters_per_second_t velocity) const override;
 
   MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
-                            units::meters_per_second_t speed) override;
+                            units::meters_per_second_t speed) const override;
 
   /**
    * Returns whether the specified robot pose is within the region that the
@@ -47,11 +47,11 @@ class RectangularRegionConstraint : public TrajectoryConstraint {
    * @param pose The robot pose.
    * @return Whether the robot pose is within the constraint region.
    */
-  bool IsPoseInRegion(const Pose2d& pose);
+  bool IsPoseInRegion(const Pose2d& pose) const;
 
  private:
   Translation2d m_bottomLeftPoint;
   Translation2d m_topRightPoint;
-  TrajectoryConstraint& m_constraint;
+  const TrajectoryConstraint& m_constraint;
 };
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
index 5107640458..e7b852f0f9 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
@@ -31,10 +31,10 @@ class SwerveDriveKinematicsConstraint : public TrajectoryConstraint {
 
   units::meters_per_second_t MaxVelocity(
       const Pose2d& pose, units::curvature_t curvature,
-      units::meters_per_second_t velocity) override;
+      units::meters_per_second_t velocity) const override;
 
   MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
-                            units::meters_per_second_t speed) override;
+                            units::meters_per_second_t speed) const override;
 
  private:
   frc::SwerveDriveKinematics<NumModules> m_kinematics;
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc b/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc
index 7fbb03c89e..63ac59a55b 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc
@@ -26,7 +26,7 @@ template <size_t NumModules>
 units::meters_per_second_t
 SwerveDriveKinematicsConstraint<NumModules>::MaxVelocity(
     const Pose2d& pose, units::curvature_t curvature,
-    units::meters_per_second_t velocity) {
+    units::meters_per_second_t velocity) const {
   auto xVelocity = velocity * pose.Rotation().Cos();
   auto yVelocity = velocity * pose.Rotation().Sin();
   auto wheelSpeeds = m_kinematics.ToSwerveModuleStates(
@@ -42,7 +42,7 @@ template <size_t NumModules>
 TrajectoryConstraint::MinMax
 SwerveDriveKinematicsConstraint<NumModules>::MinMaxAcceleration(
     const Pose2d& pose, units::curvature_t curvature,
-    units::meters_per_second_t speed) {
+    units::meters_per_second_t speed) const {
   return {};
 }
 
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h
index cf85cf11e5..c9de6761fc 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h
@@ -60,7 +60,7 @@ class TrajectoryConstraint {
    */
   virtual units::meters_per_second_t MaxVelocity(
       const Pose2d& pose, units::curvature_t curvature,
-      units::meters_per_second_t velocity) = 0;
+      units::meters_per_second_t velocity) const = 0;
 
   /**
    * Returns the minimum and maximum allowable acceleration for the trajectory
@@ -74,6 +74,6 @@ class TrajectoryConstraint {
    */
   virtual MinMax MinMaxAcceleration(const Pose2d& pose,
                                     units::curvature_t curvature,
-                                    units::meters_per_second_t speed) = 0;
+                                    units::meters_per_second_t speed) const = 0;
 };
 }  // namespace frc