init varying accel

hungpham2511 · Oct 26, 2023 · 2d6394d · 2d6394d
1 parent 730aa8a
commit 2d6394d
Show file tree

Hide file tree

Showing 2 changed files with 35 additions and 9 deletions.
diff --git 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
@@ -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