diff --git a/HISTORY.md b/HISTORY.md index 0168f879..f5663ade 100644 --- a/HISTORY.md +++ b/HISTORY.md @@ -1,5 +1,11 @@ # Changelog +## Unreleased + +### Added + +- feat(constraint): time varying acceleration constraint (#213) + ## 0.6.2 (Sept 19 2023) ### Changed diff --git a/cpp/src/toppra/constraint/linear_joint_acceleration.cpp b/cpp/src/toppra/constraint/linear_joint_acceleration.cpp index 0435cfbe..85e409cb 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(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; + } } } diff --git a/cpp/src/toppra/constraint/linear_joint_acceleration.hpp b/cpp/src/toppra/constraint/linear_joint_acceleration.hpp index 2da8fda3..81a11553 100644 --- a/cpp/src/toppra/constraint/linear_joint_acceleration.hpp +++ b/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 diff --git a/examples/plot_varying_constraint.py b/examples/plot_varying_constraint.py new file mode 100644 index 00000000..979f5284 --- /dev/null +++ b/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() diff --git a/toppra/constraint/__init__.py b/toppra/constraint/__init__.py index 72e642ee..ae58cb57 100644 --- a/toppra/constraint/__init__.py +++ b/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, diff --git a/toppra/constraint/linear_joint_acceleration.py b/toppra/constraint/linear_joint_acceleration.py index 8eabcde7..8818b2a4 100644 --- a/toppra/constraint/linear_joint_acceleration.py +++ b/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!")