Merge pull request #45 from hungpham2511/feat/dry-friction

Implement dry friction as a subclass of the SecondOrder Constraint class

.pylintrc

@@ -138,7 +138,7 @@ disable=print-statement,
 	superfluous-parens,
 	no-member,
 	too-few-public-methods,
-	fixme
+	fixme,anomalous-backslash-in-string
 
 
 # Enable the message, report, category or checker with the given id(s). You can
@@ -342,7 +342,8 @@ good-names=i,
            _,
 	   logger,
 	   dx, dy, dz, dq, q,
-	   x, y, z, s, dt
+	   x, y, z, s, dt, F, g, a, b, c
+
 
 # Include a hint for the correct naming format with invalid-name
 include-naming-hint=no

docs/source/modules.rst

@@ -6,51 +6,53 @@ Module references
 Interpolators
 -------------
 
+Interpolator base class
+^^^^^^^^^^^^^^^^^^^^^^^^^^^
+.. autoclass:: toppra.Interpolator
+   :members:
 
-SplineInterplator
+Spline Interplator
 ^^^^^^^^^^^^^^^^^^^
 .. autoclass:: toppra.SplineInterpolator
    :members:
 
-RaveTrajectoryWrapper
-^^^^^^^^^^^^^^^^^^^^^^
+Rave Trajectory Wrapper
+^^^^^^^^^^^^^^^^^^^^^^^^^^
 .. autoclass:: toppra.RaveTrajectoryWrapper
    :members:
 
-Interpolator (base class)
-^^^^^^^^^^^^^^^^^^^^^^^^^^^
-.. autoclass:: toppra.Interpolator
-   :members:
-
 Constraints
 ------------
 
-JointAccelerationConstraint
+Joint Acceleration Constraint
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 .. autoclass:: toppra.constraint.JointAccelerationConstraint
    :members:
+   :special-members:
 
-JointVelocityConstraint
+Joint Velocity Constraint
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 .. autoclass:: toppra.constraint.JointVelocityConstraint
    :members:
+   :special-members:
 
-SecondOrderConstraint
+Second Order Constraint
 ^^^^^^^^^^^^^^^^^^^^^^^^^
-.. autoclass:: toppra.constraint.CanonicalLinearSecondOrderConstraint
-   :members:
-
-CanonicalLinearConstraint (base class)
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-.. autoclass:: toppra.constraint.CanonicalLinearConstraint
+.. autoclass:: toppra.constraint.SecondOrderConstraint
    :members:
+   :special-members:
 
-RobustLinearConstraint
+Robust Linear Constraint
 ^^^^^^^^^^^^^^^^^^^^^^^^^^
+.. autoclass:: toppra.constraint.RobustLinearConstraint
+   :members:
 
-.. autoclass:: toppra.constraint.RobustCanonicalLinearConstraint
+Canonical Linear Constraint (base class)
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+.. autoclass:: toppra.constraint.LinearConstraint
    :members:
+   :special-members:
+
 
 Constraints (base class)
 ^^^^^^^^^^^^^^^^^^^^^^^^^

tests/constraint/test_joint_acceleration.py

@@ -6,7 +6,7 @@
 from toppra.constants import JACC_MAXU
 
 
-@pytest.fixture(scope="class", params=[1, 2, 6], name='acceleration_pc_data')
+@pytest.fixture(params=[1, 2, 6], name='acceleration_pc_data')
 def create_acceleration_pc_fixtures(request):
     """ Parameterized Acceleration path constraint.
 
@@ -45,59 +45,49 @@ def create_acceleration_pc_fixtures(request):
         return data, pc_vel
 
 
-class TestClass_JointAccelerationConstraint(object):
+def test_constraint_type(acceleration_pc_data):
+    """ Syntactic correct.
     """
+    data, pc = acceleration_pc_data
+    assert pc.get_constraint_type() == constraint.ConstraintType.CanonicalLinear
 
-    Tests:
-    ------
 
-    1. syntactic: the object return should have correct dimension.
+def test_constraint_params(acceleration_pc_data):
+    """ Test constraint satisfaction with cvxpy.
+    """
+    data, constraint = acceleration_pc_data
+    path, ss, alim = data
 
-    2. constraint satisfaction: the `PathConstraint` returned should
-    be consistent with the data.
+    # An user of the class
+    a, b, c, F, g, ubound, xbound = constraint.compute_constraint_params(path, ss, 1.0)
+    assert xbound is None
 
-    """
-    def test_constraint_type(self, acceleration_pc_data):
-        """ Syntactic correct.
-        """
-        data, pc = acceleration_pc_data
-        assert pc.get_constraint_type() == constraint.ConstraintType.CanonicalLinear
-
-    def test_constraint_params(self, acceleration_pc_data):
-        """ Test constraint satisfaction with cvxpy.
-        """
-        data, constraint = acceleration_pc_data
-        path, ss, alim = data
-
-        # An user of the class
-        a, b, c, F, g, ubound, xbound = constraint.compute_constraint_params(path, ss, 1.0)
+    N = ss.shape[0] - 1
+    dof = path.dof
+
+    ps = path.evald(ss)
+    pss = path.evaldd(ss)
+
+    F_actual = np.vstack((np.eye(dof), - np.eye(dof)))
+    g_actual = np.hstack((alim[:, 1], - alim[:, 0]))
+
+    npt.assert_allclose(F, F_actual)
+    npt.assert_allclose(g, g_actual)
+    for i in range(0, N + 1):
+        npt.assert_allclose(a[i], ps[i])
+        npt.assert_allclose(b[i], pss[i])
+        npt.assert_allclose(c[i], np.zeros_like(ps[i]))
+        assert ubound is None
         assert xbound is None
 
-        N = ss.shape[0] - 1
-        dof = path.get_dof()
-
-        ps = path.evald(ss)
-        pss = path.evaldd(ss)
-
-        F_actual = np.vstack((np.eye(dof), - np.eye(dof)))
-        g_actual = np.hstack((alim[:, 1], - alim[:, 0]))
-
-        npt.assert_allclose(F, F_actual)
-        npt.assert_allclose(g, g_actual)
-        for i in range(0, N + 1):
-            npt.assert_allclose(a[i], ps[i])
-            npt.assert_allclose(b[i], pss[i])
-            npt.assert_allclose(c[i], np.zeros_like(ps[i]))
-            assert ubound is None
-            assert xbound is None
-
-    def test_wrong_dimension(self, acceleration_pc_data):
-        data, pc = acceleration_pc_data
-        path_wrongdim = ta.SplineInterpolator(np.linspace(0, 1, 5), np.random.randn(5, 10))
-        with pytest.raises(ValueError) as e_info:
-            pc.compute_constraint_params(path_wrongdim, np.r_[0, 0.5, 1], 1.0)
-        assert e_info.value.args[0] == "Wrong dimension: constraint dof ({:d}) not equal to path dof ({:d})".format(
-            pc.get_dof(), 10
-        )
+
+def test_wrong_dimension(acceleration_pc_data):
+    data, pc = acceleration_pc_data
+    path_wrongdim = ta.SplineInterpolator(np.linspace(0, 1, 5), np.random.randn(5, 10))
+    with pytest.raises(ValueError) as e_info:
+        pc.compute_constraint_params(path_wrongdim, np.r_[0, 0.5, 1], 1.0)
+    assert e_info.value.args[0] == "Wrong dimension: constraint dof ({:d}) not equal to path dof ({:d})".format(
+        pc.get_dof(), 10
+    )
 
 

tests/constraint/test_second_order.py

@@ -34,35 +34,37 @@ def g(q):
 
 
 def test_wrong_dimension(coefficients_functions):
+    """If the given path has wrong dimension, raise error."""
     A, B, C, cnst_F, cnst_g, path = coefficients_functions
     def inv_dyn(q, qd, qdd):
         return A(q).dot(qdd) + np.dot(qd.T, np.dot(B(q), qd)) + C(q)
     constraint = toppra.constraint.SecondOrderConstraint(inv_dyn, cnst_F, cnst_g, dof=2)
     path_wrongdim = toppra.SplineInterpolator(np.linspace(0, 1, 5), np.random.randn(5, 10))
-    with pytest.raises(AssertionError) as e_info:
+    with pytest.raises(ValueError) as e_info:
         constraint.compute_constraint_params(path_wrongdim, np.r_[0, 0.5, 1], 1.0)
     assert e_info.value.args[0] == "Wrong dimension: constraint dof ({:d}) not equal to path dof ({:d})".format(
-        constraint.get_dof(), 10
+        constraint.dof, 10
     )
 
 
-def test_assemble_ABCFg(coefficients_functions):
+def test_correctness(coefficients_functions):
     """ For randomly set A, B, C, F, g functions. The generated parameters must equal
     those given by equations.
     """
     A, B, C, cnst_F, cnst_g, path = coefficients_functions
 
     def inv_dyn(q, qd, qdd):
         return A(q).dot(qdd) + np.dot(qd.T, np.dot(B(q), qd)) + C(q)
-    constraint = toppra.constraint.SecondOrderConstraint(inv_dyn, cnst_F, cnst_g, dof=2)
-    constraint.set_discretization_type(0)
+    constraint = toppra.constraint.SecondOrderConstraint(
+        inv_dyn, cnst_F, cnst_g, dof=2,
+        discretization_scheme=toppra.constraint.DiscretizationType.Collocation)
     a, b, c, F, g, _, _ = constraint.compute_constraint_params(
-        path, np.linspace(0, path.get_duration(), 10), 1.0)
+        path, np.linspace(0, path.duration, 10), 1.0)
 
     # Correct params
-    q_vec = path.eval(np.linspace(0, path.get_duration(), 10))
-    qs_vec = path.evald(np.linspace(0, path.get_duration(), 10))
-    qss_vec = path.evaldd(np.linspace(0, path.get_duration(), 10))
+    q_vec = path.eval(np.linspace(0, path.duration, 10))
+    qs_vec = path.evald(np.linspace(0, path.duration, 10))
+    qss_vec = path.evaldd(np.linspace(0, path.duration, 10))
 
     for i in range(10):
         ai_ = A(q_vec[i]).dot(qs_vec[i])
@@ -73,3 +75,74 @@ def inv_dyn(q, qd, qdd):
         np.testing.assert_allclose(ci_, c[i])
         np.testing.assert_allclose(cnst_F(q_vec[i]), F[i])
         np.testing.assert_allclose(cnst_g(q_vec[i]), g[i])
+
+
+def test_correctness_friction(coefficients_functions):
+    """ Same as the above test, but has frictional effect.
+    """
+    # setup
+    A, B, C, cnst_F, cnst_g, path = coefficients_functions
+    def inv_dyn(q, qd, qdd):
+        return A(q).dot(qdd) + np.dot(qd.T, np.dot(B(q), qd)) + C(q)
+    def friction(q):
+        """Randomize with fixed input/output."""
+        np.random.seed(int(np.sum(q) * 1000))
+        return 2 + np.sin(q) + np.random.rand(len(q))
+
+    constraint = toppra.constraint.SecondOrderConstraint(inv_dyn, cnst_F, cnst_g, 2, friction=friction)
+    constraint.set_discretization_type(0)
+    a, b, c, F, g, _, _ = constraint.compute_constraint_params(
+        path, np.linspace(0, path.duration, 10), 1.0)
+
+    # Correct params
+    p_vec = path.eval(np.linspace(0, path.duration, 10))
+    ps_vec = path.evald(np.linspace(0, path.duration, 10))
+    pss_vec = path.evaldd(np.linspace(0, path.duration, 10))
+
+    for i in range(10):
+        ai_ = A(p_vec[i]).dot(ps_vec[i])
+        bi_ = A(p_vec[i]).dot(pss_vec[i]) + np.dot(ps_vec[i].T, B(p_vec[i]).dot(ps_vec[i]))
+        ci_ = C(p_vec[i]) + np.sign(ps_vec[i]) * friction(p_vec[i])
+        np.testing.assert_allclose(ai_, a[i])
+        np.testing.assert_allclose(bi_, b[i])
+        np.testing.assert_allclose(ci_, c[i])
+        np.testing.assert_allclose(cnst_F(p_vec[i]), F[i])
+
+
+def test_joint_force(coefficients_functions):
+    """ Same as the above test, but has frictional effect.
+    """
+    # setup
+    A, B, C, cnst_F, cnst_g, path = coefficients_functions
+    def inv_dyn(q, qd, qdd):
+        return A(q).dot(qdd) + np.dot(qd.T, np.dot(B(q), qd)) + C(q)
+    def friction(q):
+        """Randomize with fixed input/output."""
+        np.random.seed(int(np.sum(q) * 1000))
+        return 2 + np.sin(q) + np.random.rand(len(q))
+    taulim = np.random.randn(2, 2)
+
+    constraint = toppra.constraint.SecondOrderConstraint.joint_torque_constraint(
+        inv_dyn, taulim, friction=friction)
+    constraint.set_discretization_type(0)
+    a, b, c, F, g, _, _ = constraint.compute_constraint_params(
+        path, np.linspace(0, path.duration, 10), 1.0)
+
+    # Correct params
+    p_vec = path.eval(np.linspace(0, path.duration, 10))
+    ps_vec = path.evald(np.linspace(0, path.duration, 10))
+    pss_vec = path.evaldd(np.linspace(0, path.duration, 10))
+
+    dof = 2
+    F_actual = np.vstack((np.eye(dof), - np.eye(dof)))
+    g_actual = np.hstack((taulim[:, 1], - taulim[:, 0]))
+
+    for i in range(10):
+        ai_ = A(p_vec[i]).dot(ps_vec[i])
+        bi_ = A(p_vec[i]).dot(pss_vec[i]) + np.dot(ps_vec[i].T, B(p_vec[i]).dot(ps_vec[i]))
+        ci_ = C(p_vec[i]) + np.sign(ps_vec[i]) * friction(p_vec[i])
+        np.testing.assert_allclose(ai_, a[i])
+        np.testing.assert_allclose(bi_, b[i])
+        np.testing.assert_allclose(ci_, c[i])
+        np.testing.assert_allclose(F_actual, F[i])
+        np.testing.assert_allclose(g_actual, g[i])

toppra/__init__.py

@@ -6,14 +6,12 @@
 This package produces routines for creation and handling path constraints
 using the algorithm `TOPP-RA`.
 """
-# import constraint
-# from .TOPP import *
-from .interpolator import *
-# from .constraints import *
+from .interpolator import RaveTrajectoryWrapper, SplineInterpolator,\
+    UnivariateSplineInterpolator, PolynomialPath, Interpolator
 from .utils import smooth_singularities, setup_logging
-# from . import postprocess
-# from .postprocess import compute_trajectory_gridpoints, compute_trajectory_uniform
-from .planning_utils import retime_active_joints_kinematics, create_rave_torque_path_constraint
-from . import constraint as constraint
-from . import algorithm as algorithm
-from . import solverwrapper as solverwrapper
+from .planning_utils import retime_active_joints_kinematics,\
+    create_rave_torque_path_constraint
+
+from . import constraint
+from . import algorithm
+from . import solverwrapper

toppra/constants.py

@@ -13,13 +13,12 @@
 # TODO: What are these constant used for?
 MAXU = 10000  # Max limit for `u`
 MAXX = 10000  # Max limit for `x`
-MAXSD = 100   # square root of maxx
+MAXSD = 100  # square root of maxx
 
 # constraint creation
-JVEL_MAXSD = 1e8   # max sd when creating joint velocity constraints
+JVEL_MAXSD = 1e8  # max sd when creating joint velocity constraints
 JACC_MAXU = 1e16  # max u when creating joint acceleration constraint
 
-
 # solver wrapper related constants.
 # NOTE: Read the wrapper's documentation for more details.
 

toppra/constraint/__init__.py

@@ -1,4 +1,4 @@
-"""toppra modules."""
+"""This module defines different dynamic constraints."""
 from .constraint import ConstraintType, DiscretizationType, Constraint
 from .joint_torque import JointTorqueConstraint
 from .linear_joint_acceleration import JointAccelerationConstraint