Model motor torque across wheel speed

choreolib/py/choreo/test/choreolib_test.py

@@ -5,7 +5,7 @@
 TRAJECTORY = """
 {
  "name":"New Path",
- "version":3,
+ "version":4,
  "snapshot":{
   "waypoints":[
     {"x":0.0, "y":0.0, "heading":0.0, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},

choreolib/py/choreo/test/resources/swerve_test.traj

@@ -1,6 +1,6 @@
 {
  "name":"test",
- "version":3,
+ "version":4,
  "snapshot":{
   "waypoints":[
     {"x":2.6185336112976074, "y":6.034867286682129, "heading":0.0, "intervals":16, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},

choreolib/py/choreo/util/traj_schema_version.py

@@ -1,2 +1,2 @@
 # Auto-generated by update_traj_schema.py
-TRAJ_SCHEMA_VERSION = 3
+TRAJ_SCHEMA_VERSION = 4

choreolib/src/main/java/choreo/util/TrajSchemaVersion.java

@@ -7,7 +7,7 @@
 /** Internal autogenerated class for storing the current trajectory schema version. */
 public class TrajSchemaVersion {
   /** The current trajectory schema version. */
-  public static final int TRAJ_SCHEMA_VERSION = 3;
+  public static final int TRAJ_SCHEMA_VERSION = 4;
 
   /** Utility class. */
   private TrajSchemaVersion() {}

choreolib/src/main/native/include/choreo/util/TrajSchemaVersion.hpp

@@ -8,6 +8,6 @@
 
 namespace choreo {
 
-inline constexpr uint32_t kTrajSchemaVersion = 3;
+inline constexpr uint32_t kTrajSchemaVersion = 4;
 
 }  // namespace choreo

choreolib/src/test/java/choreo/ChoreoTests.java

@@ -15,7 +15,7 @@ public class ChoreoTests {
 """
 {
  "name":"New Path",
- "version":3,
+ "version":4,
  "snapshot":{
   "waypoints":[
     {"x":0.0, "y":0.0, "heading":0.0, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},

choreolib/src/test/native/cpp/TrajectoryFileTests.cpp

@@ -14,7 +14,7 @@ using namespace choreo;
 constexpr std::string_view swerveTrajectoryString =
     R"({
  "name":"New Path",
- "version":3,
+ "version":4,
  "snapshot":{
   "waypoints":[
     {"x":0.0, "y":0.0, "heading":0.0, "intervals":9, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},

docs/document-settings/robot-configuration.md

@@ -34,18 +34,30 @@ This panel asks for details about the drive motors used to propel the robot arou
 
 These values should be determined by consulting the motor's documentation.
 
-- **Motor Max Speed** $[\text{RPM}]$: The maximum speed of each drive motor
+- **Motor Speed Limit** $[\text{RPM}]$: The planner speed limit for each drive motor
 
-!!! tip "Choosing a Motor Max Speed"
+!!! tip "Choosing a Motor Speed Limit"
 
-    A reasonable choice of Motor Max Speed is ~80% of the free speed of the drive motor(s). Although your motors have more speed available, this headroom helps ensure that your robot is able to close any errors and return to the planned trajectory.
+    A reasonable Motor Speed Limit is ~80% of the free speed of the drive motor(s). Although your motors have more speed available, this headroom helps ensure that your robot is able to close any errors and return to the planned trajectory.
 
 - **Motor Max Torque** $[N * m]$: The maximum torque applied by each drive motor
 
 !!! tip "Choosing a Max Torque"
 
     A reasonable choice of Max Torque is that corresponding to a current draw of approximately `1.5 * BreakerValue` experienced at the drive motor(s). Although your motors have more torque available, this headroom helps ensure that your robot is able to close any errors and return to the planned trajectory. Use the motor's manual or published performance curves to determine an appropriate value.
 
+- **Use torque-speed curve**: Limits the voltage required by the planned wheel torque and speed to the motor's nominal voltage. Leave this disabled only when preserving the behavior of a project created before this setting was available.
+- **Motor Free Speed** $[\text{RPM}]$: The physical no-load motor speed at nominal voltage
+- **Motor Stall Torque** $[N * m]$: The physical motor torque at zero speed and nominal voltage
+
+The torque-speed curve is applied in addition to Motor Speed Limit and Motor Max Torque. Those existing values remain independent planning and current limits.
+
+[![REV NEO Vortex motor curve](https://4148826207-files.gitbook.io/~/files/v0/b/gitbook-x-prod.appspot.com/o/spaces%2Fe0CWwhMSoCEH7NLVoLhF%2Fuploads%2FhZxihxChU3uObbZTT9Cq%2FREV%20NEO%20Vortex%20Motor%20Curve.svg?alt=media&token=497ab0b0-ad33-496d-9d68-fc049463179e)](https://docs.revrobotics.com/brushless/neo/vortex#neo-vortex-motor-curve)
+
+_Source: [REV Robotics NEO Vortex documentation](https://docs.revrobotics.com/brushless/neo/vortex#neo-vortex-motor-curve)._
+
+The blue torque curve is the relevant line for trajectory generation: torque starts at the stall value at zero speed and decreases toward zero as the motor approaches free speed. The red current curve falls with it, while the orange power curve peaks around the middle of the speed range. Choreo reconstructs the ideal torque-speed line from Motor Stall Torque and Motor Free Speed, then combines it with the independently configured Motor Max Torque current limit. This keeps torque flat while the current limit is lower, then follows the falling voltage-limited curve at higher speed. Power and efficiency are shown for context but are not separate constraints in this model.
+
 ## Theoretical
 
 This panel displays calculated metrics about your robot, for reference and validation.
@@ -54,7 +66,7 @@ This panel displays calculated metrics about your robot, for reference and valid
 
 - **Traction Accel Limit**  $[m/s^2]$: The robot's maximum acceleration before wheels begin slipping
 - **Motor Accel Limit** $[m/s^2]$: The robot's maximum acceleration based on motor torque
-- **Floor Speed** $[m/s]$: The maximum speed reached by the robot when driving in a straight line and not rotating
+- **Floor Speed** $[m/s]$: The straight-line robot speed corresponding to the Motor Speed Limit
 - **Floor Accel** $[m/s^2]$: The maximum acceleration reached by the robot when driving in a straight line and not rotating. The minimum of traction and motor limits.
 - **Ang Speed** $[rad/s]$: The robot's maximum angular speed when spinning in place
 - **Ang Accel** $[rad/s^2]$: The robot's maximum angular acceleration when spinning in place

src-core/src/generation/intervals.rs

@@ -12,6 +12,10 @@ pub fn guess_control_interval_counts(
         return Err(ChoreoError::sign("Wheel max torque", "positive"));
     } else if config.wheel_max_velocity() <= 0.0 {
         return Err(ChoreoError::sign("Wheel max velocity", "positive"));
+    } else if config.motor_curve_enabled && config.wheel_stall_torque() <= 0.0 {
+        return Err(ChoreoError::sign("Wheel stall torque", "positive"));
+    } else if config.motor_curve_enabled && config.wheel_free_speed() <= 0.0 {
+        return Err(ChoreoError::sign("Wheel free speed", "positive"));
     } else if config.mass <= 0.0 {
         return Err(ChoreoError::sign("Robot mass", "positive"));
     } else if config.radius <= 0.0 {

src-core/src/generation/transformers/drivetrain_and_bumpers.rs

@@ -29,6 +29,9 @@ impl SwerveGenerationTransformer for DrivetrainAndBumpersSetter {
             wheel_max_torque: config.tmax * config.gearing,
             wheel_cof: config.cof,
             modules: config.module_translations(),
+            motor_curve_enabled: config.motor_curve_enabled,
+            wheel_free_angular_velocity: config.wheel_free_speed(),
+            wheel_stall_torque: config.wheel_stall_torque(),
         };
 
         generator.set_drivetrain(&drivetrain);
@@ -59,6 +62,9 @@ impl DifferentialGenerationTransformer for DrivetrainAndBumpersSetter {
             wheel_max_torque: config.tmax * config.gearing,
             wheel_cof: config.cof,
             trackwidth: config.differential_track_width,
+            motor_curve_enabled: config.motor_curve_enabled,
+            wheel_free_angular_velocity: config.wheel_free_speed(),
+            wheel_stall_torque: config.wheel_stall_torque(),
         };
 
         generator.set_drivetrain(&drivetrain);

src-core/src/spec/project.rs

@@ -93,6 +93,11 @@ pub struct RobotConfig<T: SnapshottableType> {
     pub vmax: T, // motor rad/s
     /// motor N*m
     pub tmax: T, // N*m
+    pub motor_curve_enabled: bool,
+    /// motor rad/s
+    pub motor_free_speed: T,
+    /// motor N*m
+    pub motor_stall_torque: T,
     pub cof: T,
     pub bumper: Bumper<T>,
     pub differential_track_width: T,
@@ -109,6 +114,9 @@ impl<T: SnapshottableType> RobotConfig<T> {
             radius: self.radius.snapshot(),
             vmax: self.vmax.snapshot(),
             tmax: self.tmax.snapshot(),
+            motor_curve_enabled: self.motor_curve_enabled,
+            motor_free_speed: self.motor_free_speed.snapshot(),
+            motor_stall_torque: self.motor_stall_torque.snapshot(),
             cof: self.cof.snapshot(),
             bumper: self.bumper.snapshot(),
             differential_track_width: self.differential_track_width.snapshot(),
@@ -122,6 +130,12 @@ impl<T: SnapshottableType> RobotConfig<T> {
     pub fn wheel_max_velocity(&self) -> f64 {
         self.vmax.snapshot() / self.gearing.snapshot()
     }
+    pub fn wheel_free_speed(&self) -> f64 {
+        self.motor_free_speed.snapshot() / self.gearing.snapshot()
+    }
+    pub fn wheel_stall_torque(&self) -> f64 {
+        self.motor_stall_torque.snapshot() * self.gearing.snapshot()
+    }
 }
 impl RobotConfig<f64> {
     pub fn module_translations(&self) -> Vec<Translation2d> {
@@ -203,6 +217,9 @@ impl Default for ProjectFile {
                 radius: Expr::new("2 in", 0.0508),
                 vmax: Expr::new("6000.0 RPM", (6000.0 / 60.0) * std::f64::consts::TAU),
                 tmax: Expr::new("1.2 N*m", 1.2),
+                motor_curve_enabled: true,
+                motor_free_speed: Expr::new("6000.0 RPM", (6000.0 / 60.0) * std::f64::consts::TAU),
+                motor_stall_torque: Expr::new("7.09 N*m", 7.09),
                 front_left: Module {
                     x: Expr::new("11 in", 0.2794),
                     y: Expr::new("11 in", 0.2794),

src-core/src/spec/project_schema_version.rs

@@ -1,2 +1,2 @@
 // Auto-generated by update_project_schema.py
-pub const PROJECT_SCHEMA_VERSION: u32 = 2;
+pub const PROJECT_SCHEMA_VERSION: u32 = 3;

src-core/src/spec/traj_schema_version.rs

@@ -1,2 +1,2 @@
 // Auto-generated by update_traj_schema.py
-pub const TRAJ_SCHEMA_VERSION: u32 = 3;
+pub const TRAJ_SCHEMA_VERSION: u32 = 4;

src-core/src/spec/upgraders.rs

@@ -18,6 +18,7 @@ mod traj_file {
         upgrader.add_version_action(up_0_1);
         upgrader.add_version_action(up_1_2);
         upgrader.add_version_action(up_2_3);
+        upgrader.add_version_action(up_3_4);
         // Ensure the new upgrader is added here
         upgrader
     }
@@ -53,6 +54,20 @@ mod traj_file {
         clear_generation_result(editor)
     }
 
+    fn up_3_4(editor: &mut Editor) -> ChoreoResult<()> {
+        if editor
+            .get_path_raw("trajectory.config")
+            .is_some_and(|config| !config.is_null())
+        {
+            let max_speed: f64 = editor.get_path("trajectory.config.vmax")?;
+            let max_torque: f64 = editor.get_path("trajectory.config.tmax")?;
+            editor.set_path("trajectory.config.motorCurveEnabled", false)?;
+            editor.set_path("trajectory.config.motorFreeSpeed", max_speed)?;
+            editor.set_path("trajectory.config.motorStallTorque", max_torque)?;
+        }
+        Ok(())
+    }
+
     #[cfg(test)]
     mod tests {
         use crate::ChoreoResult;
@@ -102,11 +117,20 @@ mod traj_file {
         }
         #[test]
         pub fn test_3_swerve() -> ChoreoResult<()> {
-            test_trajectory("3", "swerve")
+            let file = load_trajectory("3", "swerve")?;
+            let config = file.trajectory.config.expect("config snapshot");
+            assert!(!config.motor_curve_enabled);
+            assert_eq!(config.motor_free_speed, config.vmax);
+            assert_eq!(config.motor_stall_torque, config.tmax);
+            Ok(())
         }
 
         /// Tests that the file upgrades to the current version and deserializes properly.
         fn test_trajectory(version: &str, file_name: &str) -> ChoreoResult<()> {
+            load_trajectory(version, file_name).map(|_| ())
+        }
+
+        fn load_trajectory(version: &str, file_name: &str) -> ChoreoResult<TrajectoryFile> {
             let contents = get_contents(FileType::Trajectory, version, file_name);
             let file = TrajectoryFile::from_content(&(contents))?;
             assert!(
@@ -115,7 +139,7 @@ mod traj_file {
                 file.version,
                 TRAJ_SCHEMA_VERSION
             );
-            Ok(())
+            Ok(file)
         }
     }
 }
@@ -174,6 +198,7 @@ mod project_file {
         let mut upgrader = Upgrader::new(PROJECT_SCHEMA_VERSION);
         upgrader.add_version_action(up_0_1);
         upgrader.add_version_action(up_1_2);
+        upgrader.add_version_action(up_2_3);
         upgrader
     }
     // Naming convention: up_[old version]_[new_version]
@@ -188,6 +213,13 @@ mod project_file {
         editor.set_path("codegen.useChoreoLib", true)?;
         Ok(())
     }
+    fn up_2_3(editor: &mut Editor) -> ChoreoResult<()> {
+        let max_speed: Expr = editor.get_path("config.vmax")?;
+        let max_torque: Expr = editor.get_path("config.tmax")?;
+        editor.set_path("config.motorCurveEnabled", false)?;
+        editor.set_path_serialize("config.motorFreeSpeed", max_speed)?;
+        editor.set_path_serialize("config.motorStallTorque", max_torque)
+    }
 
     #[cfg(test)]
     mod tests {
@@ -240,7 +272,20 @@ mod project_file {
         }
         #[test]
         pub fn test_2_swerve() -> ChoreoResult<()> {
-            test_project("2", "swerve")
+            let file = load_project("2", "swerve")?;
+            assert!(!file.config.motor_curve_enabled);
+            assert_eq!(file.config.motor_free_speed.exp, file.config.vmax.exp);
+            assert_eq!(file.config.motor_free_speed.val, file.config.vmax.val);
+            assert_eq!(file.config.motor_stall_torque.exp, file.config.tmax.exp);
+            assert_eq!(file.config.motor_stall_torque.val, file.config.tmax.val);
+            Ok(())
+        }
+
+        fn load_project(version: &str, file_name: &str) -> ChoreoResult<ProjectFile> {
+            let contents = get_contents(FileType::Project, version, file_name);
+            let file = ProjectFile::from_content(&(contents))?;
+            assert_eq!(file.version, PROJECT_SCHEMA_VERSION);
+            Ok(file)
         }
     }
 }

src/components/config/robotconfig/ModuleConfigPanel.tsx

@@ -1,4 +1,5 @@
 import { observer } from "mobx-react";
+import { Checkbox, FormControlLabel } from "@mui/material";
 import { Component } from "react";
 import { doc } from "../../../document/DocumentManager";
 import ExpressionInput from "../../input/ExpressionInput";
@@ -38,12 +39,12 @@ class RobotConfigPanel extends Component<Props, State> {
           titleTooltip="Gearing between motor shaft and wheel axle (>1)"
         />
         <ExpressionInput
-          title="Motor Max Speed"
+          title="Motor Speed Limit"
           enabled={true}
           roundingPrecision={0}
           number={config.vmax}
           maxWidthCharacters={8}
-          titleTooltip="Actual motor speed at 12V"
+          titleTooltip="Planner motor speed limit (typically 80% of free speed)"
         />
 
         <ExpressionInput
@@ -54,6 +55,33 @@ class RobotConfigPanel extends Component<Props, State> {
           maxWidthCharacters={8}
           titleTooltip="Motor torque as current-limited"
         />
+        <FormControlLabel
+          sx={{ gridColumn: "1 / 3", justifySelf: "end", marginRight: 0 }}
+          control={
+            <Checkbox
+              size="small"
+              checked={config.motorCurveEnabled}
+              onChange={(_, enabled) => config.setMotorCurveEnabled(enabled)}
+            />
+          }
+          label="Use torque-speed curve"
+        />
+        <ExpressionInput
+          title="Motor Free Speed"
+          enabled={config.motorCurveEnabled}
+          roundingPrecision={0}
+          number={config.motorFreeSpeed}
+          maxWidthCharacters={8}
+          titleTooltip="Physical no-load motor speed at nominal voltage"
+        />
+        <ExpressionInput
+          title="Motor Stall Torque"
+          enabled={config.motorCurveEnabled}
+          roundingPrecision={3}
+          number={config.motorStallTorque}
+          maxWidthCharacters={8}
+          titleTooltip="Physical motor stall torque at nominal voltage"
+        />
       </ExpressionInputList>
     );
   }

src/components/config/robotconfig/MotorCurves.tsx

@@ -1,4 +1,9 @@
-export type MotorCurve = { name: string; vmax: number; kt: number };
+export type MotorCurve = {
+  name: string;
+  vmax: number;
+  stallTorque: number;
+  kt: number;
+};
 
 // Data here is from Recalc's motor data at
 // https://github.com/tervay/recalc/blob/main/src/common/models/data/motors.json
@@ -19,36 +24,43 @@ export const MotorCurves: Record<
   Falcon500: {
     name: "Falcon 500",
     vmax: 6380 * rpm,
+    stallTorque: 4.69,
     kt: 4.69 / 257.0
   },
   FalconFOC: {
     name: "Falcon with FOC",
     vmax: 6080 * rpm,
+    stallTorque: 5.84,
     kt: 5.84 / 304
   },
   NEO: {
     name: "NEO",
     vmax: 5880 * rpm,
+    stallTorque: 3.28,
     kt: 3.28 / 181
   },
   NEOVortex: {
     name: "NEO Vortex",
     vmax: 6784 * rpm,
+    stallTorque: 3.6,
     kt: 3.6 / 211
   },
   KrakenX60: {
     name: "Kraken X60",
     vmax: 6000 * rpm,
+    stallTorque: 7.09,
     kt: 7.09 / 366
   },
   KrakenFOC: {
     name: "Kraken with FOC",
     vmax: 5800 * rpm,
+    stallTorque: 9.37,
     kt: 9.37 / 483
   },
   CIM: {
     name: "CIM",
     vmax: 5330 * rpm,
+    stallTorque: 2.41,
     kt: 2.41 / 131
   }
 };