Add linear acceleration max magnitude constraint

include/trajopt/constraint/Constraint.hpp

@@ -11,6 +11,7 @@
 
 #include "trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp"
 #include "trajopt/constraint/LinePointConstraint.hpp"
+#include "trajopt/constraint/LinearAccelerationMaxMagnitudeConstraint.hpp"
 #include "trajopt/constraint/LinearVelocityDirectionConstraint.hpp"
 #include "trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp"
 #include "trajopt/constraint/PointAtConstraint.hpp"
@@ -48,6 +49,7 @@ concept ConstraintType =
 
 static_assert(ConstraintType<AngularVelocityMaxMagnitudeConstraint>);
 static_assert(ConstraintType<LinePointConstraint>);
+static_assert(ConstraintType<LinearAccelerationMaxMagnitudeConstraint>);
 static_assert(ConstraintType<LinearVelocityDirectionConstraint>);
 static_assert(ConstraintType<LinearVelocityMaxMagnitudeConstraint>);
 static_assert(ConstraintType<PointAtConstraint>);
@@ -58,6 +60,7 @@ static_assert(ConstraintType<TranslationEqualityConstraint>);
 
 using Constraint =
     std::variant<AngularVelocityMaxMagnitudeConstraint, LinePointConstraint,
+                 LinearAccelerationMaxMagnitudeConstraint,
                  LinearVelocityDirectionConstraint,
                  LinearVelocityMaxMagnitudeConstraint, PointAtConstraint,
                  PointLineConstraint, PointPointConstraint,

include/trajopt/constraint/LinearAccelerationMaxMagnitudeConstraint.hpp

@@ -0,0 +1,61 @@
+// Copyright (c) TrajoptLib contributors
+
+#pragma once
+
+#include <cassert>
+
+#include <sleipnir/autodiff/Variable.hpp>
+#include <sleipnir/optimization/OptimizationProblem.hpp>
+
+#include "trajopt/geometry/Pose2.hpp"
+#include "trajopt/geometry/Translation2.hpp"
+#include "trajopt/util/SymbolExports.hpp"
+
+namespace trajopt {
+
+/**
+ * Linear acceleration max magnitude inequality constraint.
+ */
+class TRAJOPT_DLLEXPORT LinearAccelerationMaxMagnitudeConstraint {
+ public:
+  /**
+   * Constructs a LinearAccelerationMaxMagnitudeConstraint.
+   *
+   * @param maxMagnitude The maximum linear acceleration magnitude. Must be
+   *     nonnegative.
+   */
+  explicit LinearAccelerationMaxMagnitudeConstraint(double maxMagnitude)
+      : m_maxMagnitude{maxMagnitude} {
+    assert(maxMagnitude >= 0.0);
+  }
+
+  /**
+   * Applies this constraint to the given problem.
+   *
+   * @param problem The optimization problem.
+   * @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,
+             [[maybe_unused]] const sleipnir::Variable& angularVelocity,
+             const Translation2v& linearAcceleration,
+             [[maybe_unused]] const sleipnir::Variable& angularAcceleration) {
+    if (m_maxMagnitude == 0.0) {
+      problem.SubjectTo(linearAcceleration.X() == 0.0);
+      problem.SubjectTo(linearAcceleration.Y() == 0.0);
+    } else {
+      problem.SubjectTo(linearAcceleration.SquaredNorm() <=
+                        m_maxMagnitude * m_maxMagnitude);
+    }
+  }
+
+ private:
+  double m_maxMagnitude;
+};
+
+}  // namespace trajopt

src/lib.rs

@@ -93,6 +93,11 @@ mod ffi {
             index: usize,
             angular_velocity: f64,
         );
+        fn wpt_linear_acceleration_max_magnitude(
+            self: Pin<&mut SwervePathBuilderImpl>,
+            index: usize,
+            magnitude: f64,
+        );
         fn wpt_point_at(
             self: Pin<&mut SwervePathBuilderImpl>,
             index: usize,
@@ -119,6 +124,12 @@ mod ffi {
             to_index: usize,
             angular_velocity: f64,
         );
+        fn sgmt_linear_acceleration_max_magnitude(
+            self: Pin<&mut SwervePathBuilderImpl>,
+            from_index: usize,
+            to_index: usize,
+            magnitude: f64,
+        );
         fn sgmt_point_at(
             self: Pin<&mut SwervePathBuilderImpl>,
             from_index: usize,
@@ -246,6 +257,14 @@ impl SwervePathBuilder {
         );
     }
 
+    pub fn wpt_linear_acceleration_max_magnitude(&mut self, index: usize, magnitude: f64) {
+        crate::ffi::SwervePathBuilderImpl::wpt_linear_acceleration_max_magnitude(
+            self.path.pin_mut(),
+            index,
+            magnitude,
+        );
+    }
+
     pub fn wpt_point_at(
         &mut self,
         index: usize,
@@ -304,6 +323,20 @@ impl SwervePathBuilder {
         );
     }
 
+    pub fn sgmt_linear_acceleration_max_magnitude(
+        &mut self,
+        from_index: usize,
+        to_index: usize,
+        magnitude: f64,
+    ) {
+        crate::ffi::SwervePathBuilderImpl::sgmt_linear_acceleration_max_magnitude(
+            self.path.pin_mut(),
+            from_index,
+            to_index,
+            magnitude,
+        );
+    }
+
     pub fn sgmt_point_at(
         &mut self,
         from_index: usize,

src/trajoptlibrust.cpp

@@ -11,6 +11,7 @@
 
 #include "trajopt/SwerveTrajectoryGenerator.hpp"
 #include "trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp"
+#include "trajopt/constraint/LinearAccelerationMaxMagnitudeConstraint.hpp"
 #include "trajopt/constraint/LinearVelocityDirectionConstraint.hpp"
 #include "trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp"
 #include "trajopt/constraint/PointAtConstraint.hpp"
@@ -134,6 +135,12 @@ void SwervePathBuilderImpl::wpt_angular_velocity_max_magnitude(
       index, trajopt::AngularVelocityMaxMagnitudeConstraint{angular_velocity});
 }
 
+void SwervePathBuilderImpl::wpt_linear_acceleration_max_magnitude(
+    size_t index, double magnitude) {
+  path.WptConstraint(
+      index, trajopt::LinearAccelerationMaxMagnitudeConstraint{magnitude});
+}
+
 void SwervePathBuilderImpl::wpt_point_at(size_t index, double field_point_x,
                                          double field_point_y,
                                          double heading_tolerance) {
@@ -163,6 +170,13 @@ void SwervePathBuilderImpl::sgmt_angular_velocity_max_magnitude(
       trajopt::AngularVelocityMaxMagnitudeConstraint{angular_velocity});
 }
 
+void SwervePathBuilderImpl::sgmt_linear_acceleration_max_magnitude(
+    size_t from_index, size_t to_index, double magnitude) {
+  path.SgmtConstraint(
+      from_index, to_index,
+      trajopt::LinearAccelerationMaxMagnitudeConstraint{magnitude});
+}
+
 void SwervePathBuilderImpl::sgmt_point_at(size_t from_index, size_t to_index,
                                           double field_point_x,
                                           double field_point_y,

src/trajoptlibrust.hpp

@@ -35,6 +35,7 @@ class SwervePathBuilderImpl {
   void wpt_linear_velocity_max_magnitude(size_t index, double magnitude);
   void wpt_angular_velocity_max_magnitude(size_t index,
                                           double angular_velocity);
+  void wpt_linear_acceleration_max_magnitude(size_t index, double magnitude);
   void wpt_point_at(size_t index, double field_point_x, double field_point_y,
                     double heading_tolerance);
 
@@ -44,6 +45,9 @@ class SwervePathBuilderImpl {
                                           double magnitude);
   void sgmt_angular_velocity_max_magnitude(size_t from_index, size_t to_index,
                                            double angular_velocity);
+  void sgmt_linear_acceleration_max_magnitude(size_t from_index,
+                                              size_t to_index,
+                                              double magnitude);
   void sgmt_point_at(size_t from_index, size_t to_index, double field_point_x,
                      double field_point_y, double heading_tolerance);