Fix wheel power constraint

spacey-sooty · Jul 13, 2024 · 9d95bbe · 9d95bbe
1 parent a0cddec
commit 9d95bbe
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 3 deletions.
diff --git a/trajoptlib/examples/Differential/src/Main.cpp b/trajoptlib/examples/Differential/src/Main.cpp
@@ -171,7 +171,7 @@ int main() {
 
     path.WptConstraint(0, zeroLinearVelocity);
     path.WptConstraint(1, zeroLinearVelocity);
-    path.ControlIntervalCounts({40});
+    path.ControlIntervalCounts({50});
 
     trajopt::DifferentialTrajectoryGenerator generator{path};
     [[maybe_unused]]

diff --git a/trajoptlib/src/DifferentialTrajectoryGenerator.cpp b/trajoptlib/src/DifferentialTrajectoryGenerator.cpp
@@ -132,15 +132,15 @@ DifferentialTrajectoryGenerator::DifferentialTrajectoryGenerator(
     double r = path.drivetrain.trackwidth / 2.0;
     auto tau_net = r * FR.at(index) - r * FL.at(index);
 
-    // Apply module power constraints
+    // Apply wheel power constraints
     {
       auto vWrtRobot = v.RotateBy(-theta);
       const auto& [wheelRadius, wheelMaxAngularVelocity, wheelMaxTorque] =
           path.drivetrain.left;
 
       Translation2v vWheelWrtRobot{
           vWrtRobot.X() - path.drivetrain.trackwidth / 2.0 * angularVelocity,
-          0.0};
+          vWrtRobot.Y()};
       double maxWheelVelocity = wheelRadius * wheelMaxAngularVelocity;
       problem.SubjectTo(vWheelWrtRobot.SquaredNorm() <=
                         maxWheelVelocity * maxWheelVelocity);