From 2d6394d2e611ef8ad1159145aa9a2f668ae33788 Mon Sep 17 00:00:00 2001
From: Chenna Kautilya <hb@xn--0mc.com>
Date: Sun, 11 Sep 2022 09:56:39 -0700
Subject: [PATCH] init varying accel

---
 .../constraint/linear_joint_acceleration.cpp  | 30 ++++++++++++++-----
 .../constraint/linear_joint_acceleration.hpp  | 14 ++++++++-
 2 files changed, 35 insertions(+), 9 deletions(-)

diff --git a/cpp/src/toppra/constraint/linear_joint_acceleration.cpp b/cpp/src/toppra/constraint/linear_joint_acceleration.cpp
index 0435cfbe..0fe7d433 100644
--- a/cpp/src/toppra/constraint/linear_joint_acceleration.cpp
+++ b/cpp/src/toppra/constraint/linear_joint_acceleration.cpp
@@ -29,22 +29,36 @@ void LinearJointAcceleration::computeParams_impl(const GeometricPath& path,
 
   Eigen::Index ndofs (nbVariables());
 
-  // Compute F and g
-  Matrix& _F = F[0];
-  _F.   topRows(ndofs).setIdentity();
-  _F.bottomRows(ndofs).setZero();
-  _F.bottomRows(ndofs).diagonal().setConstant(-1);
-  Vector& _g = g[0];
-  _g.head(ndofs) =  m_upper;
-  _g.tail(ndofs) = -m_lower;
+  if (constantF()) {
+    // Compute F and g
+    Matrix& _F = F[0];
+    _F.topRows(ndofs).setIdentity();
+    _F.bottomRows(ndofs).setZero();
+    _F.bottomRows(ndofs).diagonal().setConstant(-1);
+    Vector& _g = g[0];
+    _g.head(ndofs) =  m_upper;
+    _g.tail(ndofs) = -m_lower;
+  }
 
   assert(ndofs == path.dof());
   for (std::size_t i = 0; i < N_1; ++i) {
+    computeAccelerationLimits(gridpoints[i]);
+
     a[i] = path.eval_single(times[i], 1);
     assert(a[i].size() == ndofs);
     b[i] = path.eval_single(times[i], 2);
     assert(b[i].size() == ndofs);
     c[i].setZero();
+
+    if (!constantF()) {
+      Matrix& _F = F[i];
+      _F.topRows(ndofs).setIdentity();
+      _F.bottomRows(ndofs).setZero();
+      _F.bottomRows(ndofs).diagonal().setConstant(-1);
+      Vector& _g = g[i];
+      _g.head(ndofs) =  m_upper;
+      _g.tail(ndofs) = -m_lower;
+    }
   }
 }
 
diff --git a/cpp/src/toppra/constraint/linear_joint_acceleration.hpp b/cpp/src/toppra/constraint/linear_joint_acceleration.hpp
index 2da8fda3..703f2b17 100644
--- a/cpp/src/toppra/constraint/linear_joint_acceleration.hpp
+++ b/cpp/src/toppra/constraint/linear_joint_acceleration.hpp
@@ -19,6 +19,19 @@ class LinearJointAcceleration : public LinearConstraint {
 
     virtual std::ostream& print(std::ostream& os) const;
 
+  protected:
+    LinearJointAcceleration (const int nDof)
+      : LinearConstraint (nDof * 2, nDof, false, false, false)
+      , m_lower (nDof)
+      , m_upper (nDof)
+    {
+      check();
+    }
+
+    virtual void computeAccelerationLimits(value_type time) { (void)time; }
+
+    Vector m_lower, m_upper;
+
   private:
     void check();
 
@@ -28,7 +41,6 @@ class LinearJointAcceleration : public LinearConstraint {
         Matrices& F, Vectors& g,
         Bounds& ubound, Bounds& xbound);
 
-    Vector m_lower, m_upper;
 }; // class LinearJointAcceleration
 } // namespace constraint
 } // namespace toppra