diff --git a/docs/source/tutorials/2_can_linear_constraints.rst b/docs/source/tutorials/2_can_linear_constraints.rst index c5b1caa0..aeb76e47 100644 --- a/docs/source/tutorials/2_can_linear_constraints.rst +++ b/docs/source/tutorials/2_can_linear_constraints.rst @@ -86,7 +86,7 @@ object, readied to be used with TOPP-RA. See below for code example: way_pts = np.random.randn(N_samples, dof) path = ta.SplineInterpolator(np.linspace(0, 1, 5), way_pts) - c = toppra.constraint.CanonicalLinearSecondOrderConstraint(inverse_dynamic, F, g) + c = toppra.constraint.SecondOrderConstraint(inverse_dynamic, F, g) instance = algo.TOPPRA([c], path) traj = instance.compute_trajectory() @@ -173,7 +173,7 @@ the final constraint object is :code:`pc_cart_acc`. def g(q): return g_q - pc_cart_acc = constraint.CanonicalLinearSecondOrderConstraint( + pc_cart_acc = constraint.SecondOrderConstraint( inverse_dynamics, F, g, dof=7) diff --git a/toppra/constraint/linear_second_order.py b/toppra/constraint/linear_second_order.py index 09327a39..ecf6e77b 100644 --- a/toppra/constraint/linear_second_order.py +++ b/toppra/constraint/linear_second_order.py @@ -1,8 +1,12 @@ """This module implements the general Second-Order constraints.""" +import logging import numpy as np + from .linear_constraint import LinearConstraint, canlinear_colloc_to_interpolate from .constraint import DiscretizationType +logger = logging.getLogger(__name__) + class SecondOrderConstraint(LinearConstraint): """This class represents the linear generalized Second-order constraints. @@ -73,6 +77,7 @@ def __init__(self, inv_dyn, cnst_F, cnst_g, dof, friction=None, discretization_s if friction is None: self.friction = lambda s: np.zeros(self.dof) else: + logger.warn("Friction is not handled due to a bug.") self.friction = friction self._format_string = " Kind: Generalized Second-order constraint\n" self._format_string = " Dimension:\n" @@ -115,8 +120,8 @@ def compute_constraint_params(self, path, gridpoints, scaling): a_vec = np.array([self.inv_dyn(_p, v_zero, _ps) for _p, _ps in zip(p_vec, ps_vec)]) - c_vec b_vec = np.array([self.inv_dyn(_p, _ps, pss_) for _p, _ps, pss_ in zip(p_vec, ps_vec, pss_vec)]) - c_vec - for i, (_p, _ps) in enumerate(zip(p_vec, ps_vec)): - c_vec[i] = c_vec[i] + np.sign(_ps) * self.friction(_p) + # for i, (_p, _ps) in enumerate(zip(p_vec, ps_vec)): + # c_vec[i] = c_vec[i] + np.sign(_ps) * self.friction(_p) if self.discretization_type == DiscretizationType.Collocation: return a_vec, b_vec, c_vec, F_vec, g_vec, None, None