Merge pull request #50 from hungpham2511/bug-2ndordercnst

A hotfix for a bug in second-order constraint class. Will need to fix nxt release.

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)
 
 

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