feat: Varying Acceleration Constraint

HISTORY.md

@@ -1,5 +1,11 @@
 # Changelog
 
+## Unreleased
+
+### Added
+
+- feat(constraint): time varying acceleration constraint (#213)
+
 ## 0.6.2 (Sept 19 2023)
 
 ### Changed

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(times[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;
+    }
   }
 }
 

cpp/src/toppra/constraint/linear_joint_acceleration.hpp

@@ -19,6 +19,25 @@ 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();
+    }
+
+    /**
+      \brief Computes the acceleration limit at time \c time.
+
+      The result must be stored into attributes
+      LinearJointAcceleration::m_lower and LinearJointAcceleration::m_upper.
+      */
+    virtual void computeAccelerationLimits(value_type time) { (void)time; }
+
+    Vector m_lower, m_upper;
+
   private:
     void check();
 
@@ -28,7 +47,6 @@ class LinearJointAcceleration : public LinearConstraint {
         Matrices& F, Vectors& g,
         Bounds& ubound, Bounds& xbound);
 
-    Vector m_lower, m_upper;
 }; // class LinearJointAcceleration
 } // namespace constraint
 } // namespace toppra

examples/plot_varying_constraint.py

@@ -0,0 +1,96 @@
+"""Retime a path subject to time varying constraints
+====================================================
+
+In this example, we will see how can we retime a generic spline-based
+path subject to time varying constraints.
+"""
+
+import toppra as ta
+import toppra.constraint as constraint
+import toppra.algorithm as algo
+import numpy as np
+import matplotlib.pyplot as plt
+import time
+
+ta.setup_logging("INFO")
+
+################################################################################
+# We generate a path with some random waypoints.
+
+
+def generate_new_problem(seed=42):
+    # Parameters
+    N_samples = 5
+    dof = 7
+    np.random.seed(seed)
+    way_pts = np.random.randn(N_samples, dof)
+    return (
+        np.linspace(0, 1, 5),
+        way_pts,
+        10 + np.random.rand(dof) * 20,
+        10 + np.random.rand(dof) * 2,
+    )
+
+
+ss, way_pts, vlims, alims = generate_new_problem()
+
+
+def vlim_func(t):
+    vlim = np.array(vlims, dtype=float)
+    if len(vlim.shape) == 1:
+        vlim = np.vstack((-np.array(vlim), np.array(vlim))).T
+    else:
+        vlim = np.array(vlim, dtype=float)
+    return vlim * abs(np.sin(2 * np.pi * t))
+
+
+def alim_func(t):
+    """Varying acceleration limits
+    with t between 0, 1, limit is 0 at start and end
+    """
+    alim = np.array(alims, dtype=float)
+    if len(alim.shape) == 1:
+        alim = np.vstack((-np.array(alim), np.array(alim))).T
+    else:
+        alim = np.array(alim, dtype=float)
+    return alim * (0.25 + np.sin(np.pi * t))
+
+
+################################################################################
+# Define the geometric path and two constraints.
+path = ta.SplineInterpolator(ss, way_pts)
+pc_vel = constraint.JointVelocityConstraintVarying(vlim_func)
+pc_acc = constraint.JointAccelerationConstraintVarying(alim_func)
+
+################################################################################
+# We solve the parametrization problem using the
+# `ParametrizeConstAccel` parametrizer. This parametrizer is the
+# classical solution, guarantee constraint and boundary conditions
+# satisfaction.
+instance = algo.TOPPRA([pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel")
+jnt_traj = instance.compute_trajectory()
+
+################################################################################
+# The output trajectory is an instance of
+# :class:`toppra.interpolator.AbstractGeometricPath`.
+ts_sample = np.linspace(0, jnt_traj.duration, 100)
+qs_sample = jnt_traj(ts_sample)
+qds_sample = jnt_traj(ts_sample, 1)
+qdds_sample = jnt_traj(ts_sample, 2)
+fig, axs = plt.subplots(3, 1, sharex=True)
+for i in range(path.dof):
+    # plot the i-th joint trajectory
+    axs[0].plot(ts_sample, qs_sample[:, i], c="C{:d}".format(i))
+    axs[1].plot(ts_sample, qds_sample[:, i], c="C{:d}".format(i))
+    axs[2].plot(ts_sample, qdds_sample[:, i], c="C{:d}".format(i))
+axs[2].set_xlabel("Time (s)")
+axs[0].set_ylabel("Position (rad)")
+axs[1].set_ylabel("Velocity (rad/s)")
+axs[2].set_ylabel("Acceleration (rad/s2)")
+plt.show()
+
+
+################################################################################
+# Optionally, we can inspect the output.
+instance.compute_feasible_sets()
+instance.inspect()

toppra/constraint/__init__.py

@@ -60,6 +60,13 @@
    :special-members:
    :show-inheritance:
 
+JointAccelerationConstraintVarying
+^^^^^^^^^^^^^^^^^^^^^^^^^
+.. autoclass:: toppra.constraint.JointAccelerationConstraintVarying
+   :members:
+   :special-members:
+   :show-inheritance:
+
 RobustLinearConstraint
 ^^^^^^^^^^^^^^^^^^^^^^^^^^
 .. autoclass:: toppra.constraint.RobustLinearConstraint
@@ -79,7 +86,10 @@
 """
 from .constraint import ConstraintType, DiscretizationType, Constraint
 from .joint_torque import JointTorqueConstraint
-from .linear_joint_acceleration import JointAccelerationConstraint
+from .linear_joint_acceleration import (
+    JointAccelerationConstraint,
+    JointAccelerationConstraintVarying,
+)
 from .linear_joint_velocity import (
     JointVelocityConstraint,
     JointVelocityConstraintVarying,

toppra/constraint/linear_joint_acceleration.py

@@ -46,7 +46,7 @@ def __init__(self, alim, discretization_scheme=DiscretizationType.Interpolation)
         super(JointAccelerationConstraint, self).__init__()
         alim = np.array(alim, dtype=float)
         if np.isnan(alim).any():
-            raise ValueError("Bad velocity given: %s" % alim)
+            raise ValueError("Bad acceleration given: %s" % alim)
         if len(alim.shape) == 1:
             self.alim = np.vstack((-np.array(alim), np.array(alim))).T
         else:
@@ -102,3 +102,71 @@ def compute_constraint_params(
             )
         else:
             raise NotImplementedError("Other form of discretization not supported!")
+
+
+class JointAccelerationConstraintVarying(LinearConstraint):
+    """A Joint acceleration Constraint class.
+
+    This class handle acceleration constraints that vary along the path.
+
+    Parameters
+    ----------
+    alim_func: (float) -> np.ndarray
+        A function that receives a scalar (float) and produce an array
+        with shape (dof, 2). The lower and upper acceleration bounds of
+        the j-th joint are given by out[j, 0] and out[j, 1]
+        respectively.
+    """
+
+    def __init__(self, alim_func):
+        super(JointAccelerationConstraintVarying, self).__init__()
+        self.dof = alim_func(0).shape[0]
+        self._format_string = "    Varying Acceleration limit: \n"
+        self.alim_func = alim_func
+        self.identical = False
+
+    def compute_constraint_params(self, path, gridpoints):
+        if path.dof != self.dof:
+            raise ValueError(
+                "Wrong dimension: constraint dof ({:d}) not equal to path dof ({:d})".format(
+                    self.dof, path.dof
+                )
+            )
+        ps_vec = (path(gridpoints, order=1)).reshape((-1, path.dof))
+        pss_vec = (path(gridpoints, order=2)).reshape((-1, path.dof))
+        dof = path.dof
+        n_grid = gridpoints.shape[0]
+
+        stacked_eyes = np.vstack((np.eye(dof), -np.eye(dof)))
+        f_vec = np.repeat(stacked_eyes[np.newaxis, :, :], n_grid, axis=0)
+
+        # compute accel limits over gridpoints
+        alim_grid = np.array([self.alim_func(s) for s in gridpoints])
+        g_vec = np.zeros((n_grid, dof * 2))
+        g_vec[:, 0:dof] = alim_grid[:, :, 1]
+        g_vec[:, dof:] = -alim_grid[:, :, 0]
+
+        if self.discretization_type == DiscretizationType.Collocation:
+            return (
+                ps_vec,
+                pss_vec,
+                np.zeros_like(ps_vec),
+                f_vec,
+                g_vec,
+                None,
+                None,
+            )
+        elif self.discretization_type == DiscretizationType.Interpolation:
+            return canlinear_colloc_to_interpolate(
+                ps_vec,
+                pss_vec,
+                np.zeros_like(ps_vec),
+                f_vec,
+                g_vec,
+                None,
+                None,
+                gridpoints,
+                identical=False,
+            )
+        else:
+            raise NotImplementedError("Other form of discretization not supported!")