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