Add acceleration to constrainable variables

include/trajopt/constraint/AngularVelocityEqualityConstraint.hpp

@@ -2,6 +2,7 @@
 
 #pragma once
 
+#include <sleipnir/autodiff/Variable.hpp>
 #include <sleipnir/optimization/OptimizationProblem.hpp>
 
 #include "trajopt/geometry/Pose2.hpp"
@@ -30,11 +31,15 @@ class TRAJOPT_DLLEXPORT AngularVelocityEqualityConstraint {
    * @param pose The robot's pose.
    * @param linearVelocity The robot's linear velocity.
    * @param angularVelocity The robot's angular velocity.
+   * @param linearAcceleration The robot's linear acceleration.
+   * @param angularAcceleration The robot's angular acceleration.
    */
   void Apply(sleipnir::OptimizationProblem& problem,
              [[maybe_unused]] const Pose2v& pose,
              [[maybe_unused]] const Translation2v& linearVelocity,
-             const sleipnir::Variable& angularVelocity) {
+             const sleipnir::Variable& angularVelocity,
+             [[maybe_unused]] const Translation2v& linearAcceleration,
+             [[maybe_unused]] const sleipnir::Variable& angularAcceleration) {
     problem.SubjectTo(angularVelocity == m_angularVelocity);
   }
 

include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp

@@ -4,6 +4,7 @@
 
 #include <cassert>
 
+#include <sleipnir/autodiff/Variable.hpp>
 #include <sleipnir/optimization/OptimizationProblem.hpp>
 
 #include "trajopt/geometry/Pose2.hpp"
@@ -35,11 +36,15 @@ class TRAJOPT_DLLEXPORT AngularVelocityMaxMagnitudeConstraint {
    * @param pose The robot's pose.
    * @param linearVelocity The robot's linear velocity.
    * @param angularVelocity The robot's angular velocity.
+   * @param linearAcceleration The robot's linear acceleration.
+   * @param angularAcceleration The robot's angular acceleration.
    */
   void Apply(sleipnir::OptimizationProblem& problem,
              [[maybe_unused]] const Pose2v& pose,
              [[maybe_unused]] const Translation2v& linearVelocity,
-             const sleipnir::Variable& angularVelocity) {
+             const sleipnir::Variable& angularVelocity,
+             [[maybe_unused]] const Translation2v& linearAcceleration,
+             [[maybe_unused]] const sleipnir::Variable& angularAcceleration) {
     if (m_maxMagnitude == 0.0) {
       problem.SubjectTo(angularVelocity == 0.0);
     } else {

include/trajopt/constraint/Constraint.hpp

@@ -5,6 +5,7 @@
 #include <utility>
 #include <variant>
 
+#include <sleipnir/autodiff/Variable.hpp>
 #include <sleipnir/optimization/OptimizationProblem.hpp>
 
 #include "trajopt/constraint/AngularVelocityEqualityConstraint.hpp"
@@ -28,8 +29,17 @@ namespace trajopt {
 template <typename T>
 concept ConstraintType = requires(T t) {
   {
+    // Parameters:
+    //
+    // sleipnir::OptimizationProblem& problem
+    // const Pose2v& pose
+    // const Translation2v& linearVelocity
+    // const sleipnir::Variable& angularVelocity
+    // const Translation2v& linearAcceleration
+    // const sleipnir::Variable& angularAcceleration
     t.Apply(std::declval<sleipnir::OptimizationProblem>(),
             std::declval<Pose2v>(), std::declval<Translation2v>(),
+            std::declval<sleipnir::Variable>(), std::declval<Translation2v>(),
             std::declval<sleipnir::Variable>())
   };  // NOLINT(readability/braces)
 };

include/trajopt/constraint/LinePointConstraint.hpp

@@ -5,6 +5,7 @@
 #include <cassert>
 #include <utility>
 
+#include <sleipnir/autodiff/Variable.hpp>
 #include <sleipnir/optimization/OptimizationProblem.hpp>
 
 #include "trajopt/constraint/detail/LinePointDistance.hpp"
@@ -48,10 +49,14 @@ class TRAJOPT_DLLEXPORT LinePointConstraint {
    * @param pose The robot's pose.
    * @param linearVelocity The robot's linear velocity.
    * @param angularVelocity The robot's angular velocity.
+   * @param linearAcceleration The robot's linear acceleration.
+   * @param angularAcceleration The robot's angular acceleration.
    */
   void Apply(sleipnir::OptimizationProblem& problem, const Pose2v& pose,
              [[maybe_unused]] const Translation2v& linearVelocity,
-             [[maybe_unused]] const sleipnir::Variable& angularVelocity) {
+             [[maybe_unused]] const sleipnir::Variable& angularVelocity,
+             [[maybe_unused]] const Translation2v& linearAcceleration,
+             [[maybe_unused]] const sleipnir::Variable& angularAcceleration) {
     auto lineStart =
         pose.Translation() + m_robotLineStart.RotateBy(pose.Rotation());
     auto lineEnd =

include/trajopt/constraint/LinearVelocityDirectionConstraint.hpp

@@ -2,6 +2,7 @@
 
 #pragma once
 
+#include <sleipnir/autodiff/Variable.hpp>
 #include <sleipnir/optimization/OptimizationProblem.hpp>
 
 #include "trajopt/geometry/Pose2.hpp"
@@ -29,11 +30,15 @@ class TRAJOPT_DLLEXPORT LinearVelocityDirectionConstraint {
    * @param pose The robot's pose.
    * @param linearVelocity The robot's linear velocity.
    * @param angularVelocity The robot's angular velocity.
+   * @param linearAcceleration The robot's linear acceleration.
+   * @param angularAcceleration The robot's angular acceleration.
    */
   void Apply([[maybe_unused]] sleipnir::OptimizationProblem& problem,
              [[maybe_unused]] const Pose2v& pose,
              [[maybe_unused]] const Translation2v& linearVelocity,
-             [[maybe_unused]] const sleipnir::Variable& angularVelocity) {
+             [[maybe_unused]] const sleipnir::Variable& angularVelocity,
+             [[maybe_unused]] const Translation2v& linearAcceleration,
+             [[maybe_unused]] const sleipnir::Variable& angularAcceleration) {
     // <v_x, v_y> and <u_x, u_y> must be parallel
     //
     //   (v ⋅ u)/‖v‖ = 1

include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp

@@ -4,6 +4,7 @@
 
 #include <cassert>
 
+#include <sleipnir/autodiff/Variable.hpp>
 #include <sleipnir/optimization/OptimizationProblem.hpp>
 
 #include "trajopt/geometry/Pose2.hpp"
@@ -35,11 +36,15 @@ class TRAJOPT_DLLEXPORT LinearVelocityMaxMagnitudeConstraint {
    * @param pose The robot's pose.
    * @param linearVelocity The robot's linear velocity.
    * @param angularVelocity The robot's angular velocity.
+   * @param linearAcceleration The robot's linear acceleration.
+   * @param angularAcceleration The robot's angular acceleration.
    */
   void Apply(sleipnir::OptimizationProblem& problem,
              [[maybe_unused]] const Pose2v& pose,
              const Translation2v& linearVelocity,
-             [[maybe_unused]] const sleipnir::Variable& angularVelocity) {
+             [[maybe_unused]] const sleipnir::Variable& angularVelocity,
+             [[maybe_unused]] const Translation2v& linearAcceleration,
+             [[maybe_unused]] const sleipnir::Variable& angularAcceleration) {
     if (m_maxMagnitude == 0.0) {
       problem.SubjectTo(linearVelocity.X() == 0.0);
       problem.SubjectTo(linearVelocity.Y() == 0.0);

include/trajopt/constraint/PointAtConstraint.hpp

@@ -5,6 +5,7 @@
 #include <cassert>
 #include <utility>
 
+#include <sleipnir/autodiff/Variable.hpp>
 #include <sleipnir/optimization/OptimizationProblem.hpp>
 
 #include "trajopt/geometry/Pose2.hpp"
@@ -40,10 +41,14 @@ class TRAJOPT_DLLEXPORT PointAtConstraint {
    * @param pose The robot's pose.
    * @param linearVelocity The robot's linear velocity.
    * @param angularVelocity The robot's angular velocity.
+   * @param linearAcceleration The robot's linear acceleration.
+   * @param angularAcceleration The robot's angular acceleration.
    */
   void Apply(sleipnir::OptimizationProblem& problem, const Pose2v& pose,
              [[maybe_unused]] const Translation2v& linearVelocity,
-             [[maybe_unused]] const sleipnir::Variable& angularVelocity) {
+             [[maybe_unused]] const sleipnir::Variable& angularVelocity,
+             [[maybe_unused]] const Translation2v& linearAcceleration,
+             [[maybe_unused]] const sleipnir::Variable& angularAcceleration) {
     // dx,dy = desired heading
     // ux,uy = unit vector of desired heading
     // hx,hy = heading

include/trajopt/constraint/PointLineConstraint.hpp

@@ -5,6 +5,7 @@
 #include <cassert>
 #include <utility>
 
+#include <sleipnir/autodiff/Variable.hpp>
 #include <sleipnir/optimization/OptimizationProblem.hpp>
 
 #include "trajopt/constraint/detail/LinePointDistance.hpp"
@@ -48,10 +49,14 @@ class TRAJOPT_DLLEXPORT PointLineConstraint {
    * @param pose The robot's pose.
    * @param linearVelocity The robot's linear velocity.
    * @param angularVelocity The robot's angular velocity.
+   * @param linearAcceleration The robot's linear acceleration.
+   * @param angularAcceleration The robot's angular acceleration.
    */
   void Apply(sleipnir::OptimizationProblem& problem, const Pose2v& pose,
              [[maybe_unused]] const Translation2v& linearVelocity,
-             [[maybe_unused]] const sleipnir::Variable& angularVelocity) {
+             [[maybe_unused]] const sleipnir::Variable& angularVelocity,
+             [[maybe_unused]] const Translation2v& linearAcceleration,
+             [[maybe_unused]] const sleipnir::Variable& angularAcceleration) {
     auto point = pose.Translation() + m_robotPoint.RotateBy(pose.Rotation());
     auto dist =
         detail::LinePointDistance(m_fieldLineStart, m_fieldLineEnd, point);

include/trajopt/constraint/PointPointConstraint.hpp

@@ -5,6 +5,7 @@
 #include <cassert>
 #include <utility>
 
+#include <sleipnir/autodiff/Variable.hpp>
 #include <sleipnir/optimization/OptimizationProblem.hpp>
 
 #include "trajopt/geometry/Pose2.hpp"
@@ -44,10 +45,14 @@ class TRAJOPT_DLLEXPORT PointPointConstraint {
    * @param pose The robot's pose.
    * @param linearVelocity The robot's linear velocity.
    * @param angularVelocity The robot's angular velocity.
+   * @param linearAcceleration The robot's linear acceleration.
+   * @param angularAcceleration The robot's angular acceleration.
    */
   void Apply(sleipnir::OptimizationProblem& problem, const Pose2v& pose,
              [[maybe_unused]] const Translation2v& linearVelocity,
-             [[maybe_unused]] const sleipnir::Variable& angularVelocity) {
+             [[maybe_unused]] const sleipnir::Variable& angularVelocity,
+             [[maybe_unused]] const Translation2v& linearAcceleration,
+             [[maybe_unused]] const sleipnir::Variable& angularAcceleration) {
     auto bumperCorner =
         pose.Translation() + m_robotPoint.RotateBy(pose.Rotation());
     auto dx = m_fieldPoint.X() - bumperCorner.X();

include/trajopt/constraint/PoseEqualityConstraint.hpp

@@ -2,6 +2,7 @@
 
 #pragma once
 
+#include <sleipnir/autodiff/Variable.hpp>
 #include <sleipnir/optimization/OptimizationProblem.hpp>
 
 #include "trajopt/geometry/Pose2.hpp"
@@ -32,10 +33,14 @@ class TRAJOPT_DLLEXPORT PoseEqualityConstraint {
    * @param pose The robot's pose.
    * @param linearVelocity The robot's linear velocity.
    * @param angularVelocity The robot's angular velocity.
+   * @param linearAcceleration The robot's linear acceleration.
+   * @param angularAcceleration The robot's angular acceleration.
    */
   void Apply(sleipnir::OptimizationProblem& problem, const Pose2v& pose,
              [[maybe_unused]] const Translation2v& linearVelocity,
-             [[maybe_unused]] const sleipnir::Variable& angularVelocity) {
+             [[maybe_unused]] const sleipnir::Variable& angularVelocity,
+             [[maybe_unused]] const Translation2v& linearAcceleration,
+             [[maybe_unused]] const sleipnir::Variable& angularAcceleration) {
     problem.SubjectTo(pose == m_pose);
   }
 

include/trajopt/constraint/TranslationEqualityConstraint.hpp

@@ -2,6 +2,7 @@
 
 #pragma once
 
+#include <sleipnir/autodiff/Variable.hpp>
 #include <sleipnir/optimization/OptimizationProblem.hpp>
 
 #include "trajopt/geometry/Pose2.hpp"
@@ -30,10 +31,14 @@ class TRAJOPT_DLLEXPORT TranslationEqualityConstraint {
    * @param pose The robot's pose.
    * @param linearVelocity The robot's linear velocity.
    * @param angularVelocity The robot's angular velocity.
+   * @param linearAcceleration The robot's linear acceleration.
+   * @param angularAcceleration The robot's angular acceleration.
    */
   void Apply(sleipnir::OptimizationProblem& problem, const Pose2v& pose,
              [[maybe_unused]] const Translation2v& linearVelocity,
-             [[maybe_unused]] const sleipnir::Variable& angularVelocity) {
+             [[maybe_unused]] const sleipnir::Variable& angularVelocity,
+             [[maybe_unused]] const Translation2v& linearAcceleration,
+             [[maybe_unused]] const sleipnir::Variable& angularAcceleration) {
     problem.SubjectTo(pose.Translation() == m_translation);
   }
 

src/SwerveTrajectoryGenerator.cpp

@@ -232,10 +232,13 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator(
           x.at(index), y.at(index), {thetacos.at(index), thetasin.at(index)}};
       Translation2v linearVelocity{vx.at(index), vy.at(index)};
       auto angularVelocity = omega.at(index);
+      Translation2v linearAcceleration{ax.at(index), ay.at(index)};
+      auto angularAcceleration = alpha.at(index);
 
       std::visit(
           [&](auto&& arg) {
-            arg.Apply(problem, pose, linearVelocity, angularVelocity);
+            arg.Apply(problem, pose, linearVelocity, angularVelocity,
+                      linearAcceleration, angularAcceleration);
           },
           constraint);
     }
@@ -252,10 +255,13 @@ SwerveTrajectoryGenerator::SwerveTrajectoryGenerator(
             x.at(index), y.at(index), {thetacos.at(index), thetasin.at(index)}};
         Translation2v linearVelocity{vx.at(index), vy.at(index)};
         auto angularVelocity = omega.at(index);
+        Translation2v linearAcceleration{ax.at(index), ay.at(index)};
+        auto angularAcceleration = alpha.at(index);
 
         std::visit(
             [&](auto&& arg) {
-              arg.Apply(problem, pose, linearVelocity, angularVelocity);
+              arg.Apply(problem, pose, linearVelocity, angularVelocity,
+                        linearAcceleration, angularAcceleration);
             },
             constraint);
       }