Merge branch 'develop' into master

.circleci/config.yml

@@ -92,12 +92,11 @@ jobs:
           name: test
           command: |
             . venv3/bin/activate
-            export PYTHONPATH=$PYTHONPATH:`openrave-config --python-dir`
-            pytest tests -W ignore::PendingDeprecationWarning
+            mkdir test-reports
+            pytest -q tests --durations=3 --junitxml=test-reports/junit.xml
 
-      - store_artifacts:
+      - store_test_results:
           path: test-reports
-          destination: test-reports
 
   test-python-2:
     docker:
@@ -143,16 +142,15 @@ jobs:
           command: |
             . venv/bin/activate
             export PYTHONPATH=$PYTHONPATH:`openrave-config --python-dir`
-            pytest tests -W ignore::PendingDeprecationWarning
+            mkdir test-reports
+            pytest -q tests --durations=3 --junitxml=test-reports/junit.xml
 
-      - store_artifacts:
+      - store_test_results:
           path: test-reports
-          destination: test-reports
-
 
 workflows:
   version: 2
-  test:
+  test-and-lint:
     jobs:
       - test-python-2
       - test-python-3

.gitignore

@@ -1,3 +1,6 @@
+# profiling
+*.prof
+
 # Byte-compiled / optimized / DLL files
 __pycache__/
 *.py[cod]

.pylintrc

@@ -343,7 +343,9 @@ good-names=i,
 	   logger,
 	   dx, dy, dz, dq, q,
 	   x, y, z, s, dt, F, g, a, b, c,
-	   N, ds
+	   N, ds, q, qd, qdd, qddd, qs, qss, qsss,
+	   us, xs, vs,  # special toppra-specific variables
+	   constraint_F, F_vec
 
 
 # Include a hint for the correct naming format with invalid-name

Makefile

@@ -6,3 +6,18 @@ lint:
 doc: 
 	echo "Buidling toppra docs"
 	sphinx-build -b html docs/source docs/build
+
+coverage: 
+	python -m pytest -q --cov-report term --cov-report xml --cov=toppra tests
+
+
+# todos before publishing:
+# - increment version in setup.py
+# - increment version in doc
+# - create a new release to master
+# - run publish on master
+publish:
+	pip install 'twine>=1.5.0'
+	python setup.py sdist
+	twine upload dist/*
+	rm -fr build dist .egg requests.egg-info

README.md

@@ -14,6 +14,20 @@ In general, given the inputs:
 TOPP-RA returns the time-optimal path parameterization: `s_dot (s)`, from which the fastest trajectory `q(t)` that satisfies the given
 constraints can be found.
 
+## Quick-start
+
+To install toppra, simple clone the repo and install with pip
+
+``` shell
+git clone https://github.com/hungpham2511/toppra
+cd toppra && pip install .
+```
+
+To install depencidencies for development, replace the second command with:
+``` shell
+cd toppra && pip install -e .[dev]
+```
+
 ## Citing TOPP-RA
 If you use this library for your research, we encourage you to 
 

VERSION

@@ -0,0 +1 @@
+0.2.2a

docs/source/FAQs.rst

@@ -29,4 +29,17 @@ As a general rule of thumb, the number of gridpoints should be at
 least twice the number of waypoints in the given path.
 
 
+2. Minimum requirement on path smoothness
+-------------------------------------------------
+
+TOPPRA requires the input path to be sufficiently smooth to work
+properly. An example of a noisy path that will be very difficult to
+work with can be seen below:
+
+.. image:: medias/faq_figure.png
+
+All toppra interpolators try to match all given waypoints, and hence
+it can lead to large fluctuation if the waypoints change rapidly. In
+this case, it is recommended to smooth the waypoints prior to using
+toppra using for example `scipy.interpolation`.
 

docs/source/conf.py

@@ -19,6 +19,7 @@
 import os
 import sys
 import toppra
+import pathlib2
 # sys.path.insert(0, os.path.abspath('../../toppra/'))
 
 
@@ -64,13 +65,10 @@
 
 # General information about the project.
 project = 'TOPP-RA'
-copyright = '2018, Hung Pham'
+copyright = '2019, Hung Pham'
 author = 'Hung Pham'
 
-
-version = 'v0.2.2'
-# The full version, including alpha/beta/rc tags.
-release = 'v0.2.2'
+version = pathlib2.Path('./../../VERSION').read_text()
 
 # The language for content autogenerated by Sphinx. Refer to documentation
 # for a list of supported languages.
@@ -116,8 +114,7 @@
     'fixed_sidebar': True,
     "sidebar_width": "270px",
     "page_width": "1240px",
-    "show_related": True
-    }
+    "show_related": True}
 
 # Add any paths that contain custom static files (such as style sheets) here,
 # relative to this directory. They are copied after the builtin static files,

docs/source/tutorials.rst

@@ -8,7 +8,8 @@ Some tutorials to get you used to TOPP-RA
    :maxdepth: 1
 
    tutorials/tut1_basic_example.ipynb
-   tutorials/0a_trapezoidal_scalar
+   tutorials/tut2_using_toppra_output.ipynb
+   tutorials/tut3_non_zero_velocities.ipynb
    tutorials/1_geometric_path
    tutorials/2_can_linear_constraints
 

examples/robust_kinematics.py

@@ -66,7 +66,7 @@ def main():
     plt.show()
 
     jnt_traj, aux_traj = instance.compute_trajectory(0, 0)
-    ts_sample = np.linspace(0, jnt_traj.get_duration(), 100)
+    ts_sample = np.linspace(0, jnt_traj.duration, 100)
     qs_sample = jnt_traj.evaldd(ts_sample)
 
     plt.plot(ts_sample, qs_sample)

pytest.ini

@@ -0,0 +1,3 @@
+[pytest]
+filterwarnings =
+    ignore::PendingDeprecationWarning

requirements.txt

@@ -1,7 +1,7 @@
 cython>=0.22.0
 PyYAML
 numpy
-scipy>0.18
+scipy==0.18.0
 coloredlogs
 enum34
 matplotlib<3.0.0

setup.py

@@ -5,7 +5,8 @@
 import sys
 
 NAME = "toppra"
-VERSION = "0.2.2"
+with open("VERSION", "r") as file_:
+    VERSION = file_.read()
 DESCR = "An implementation of TOPP-RA (TOPP via Reachability Analysis) for time-parametrizing" \
         "trajectories for robots subject to kinematic (velocity and acceleration) and dynamic" \
         "(torque) constraints. Some other kinds of constraints are also supported."
@@ -45,6 +46,16 @@
 
 if __name__ == "__main__":
     setup(install_requires=REQUIRES,
+          setup_requires=["numpy", "cython"],
+          extras_require={
+              'dev': [
+                  'pytest',
+                  'pytest-pep8',
+                  'pytest-cov',
+                  'tabulate',
+                  'cvxpy'
+              ]
+          },
           packages=PACKAGES,
           zip_safe=False,
           name=NAME,

tests/constraint/test_create_rave_torque.py

@@ -21,7 +21,7 @@ def test_shape(barret_robot, dof):
     np.random.seed(0)
     path = toppra.SplineInterpolator(np.linspace(0, 1, 5), np.random.rand(5, dof))
     a, b, c, F, g, _, _ = constraint.compute_constraint_params(
-        path, np.linspace(0, path.get_duration(), 5), 1.0)
+        path, np.linspace(0, path.duration, 5), 1.0)
 
     assert a.shape[1] == dof
     assert b.shape[1] == dof

tests/constraint/test_joint_velocity.py

@@ -89,7 +89,7 @@ def test_wrong_dimension(self, velocity_pc_data):
         with pytest.raises(ValueError) as e_info:
             pc.compute_constraint_params(path_wrongdim, [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
+            pc.dof, 10
         )
 
 

tests/constraint/test_robust_can_linear.py

@@ -28,7 +28,7 @@ def test_basic(accel_constraint, dist_scheme):
     assert ro_cnst.get_dof() == 5
 
     a, b, c, P, _, _ = ro_cnst.compute_constraint_params(
-        path, np.linspace(0, path.get_duration(), 10), 1.0)
+        path, np.linspace(0, path.duration, 10), 1.0)
 
     # assert a.shape == (10, 2 * path.get_dof())
     # assert b.shape == (10, 2 * path.get_dof())
@@ -38,7 +38,7 @@ def test_basic(accel_constraint, dist_scheme):
     # Linear params
     cnst.set_discretization_type(dist_scheme)
     a0, b0, c0, F0, g0, _, _ = cnst.compute_constraint_params(
-        path, np.linspace(0, path.get_duration(), 10), 1.0)
+        path, np.linspace(0, path.duration, 10), 1.0)
 
     # Assert values
     for i in range(10):

tests/constraint/test_second_order.py

@@ -28,33 +28,30 @@ def F(q):
     def g(q):
         return np.array([100, 200])
 
+    def inv_dyn(q, qd, qdd):
+        return A(q).dot(qdd) + np.dot(qd.T, np.dot(B(q), qd)) + C(q)
+
     np.random.seed(0)
     path = toppra.SplineInterpolator([0, 1, 2, 4], np.random.randn(4, 2))
-    return A, B, C, F, g, path
+    return A, B, C, F, g, path, inv_dyn
 
 
 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)
+    A, B, C, cnst_F, cnst_g, path, inv_dyn = coefficients_functions
     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(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.dof, 10
-    )
+        constraint.dof, 10)
 
 
 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)
+    A, B, C, cnst_F, cnst_g, path, inv_dyn = coefficients_functions
     constraint = toppra.constraint.SecondOrderConstraint(
         inv_dyn, cnst_F, cnst_g, dof=2,
         discretization_scheme=toppra.constraint.DiscretizationType.Collocation)
@@ -77,53 +74,26 @@ def inv_dyn(q, qd, qdd):
         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):
+@pytest.fixture
+def friction():
+    def randomized_friction(q):
         """Randomize with fixed input/output."""
-        np.random.seed(int(np.sum(q) * 1000))
+        np.random.seed(int(abs(np.sum(q)) * 1000))
         return 2 + np.sin(q) + np.random.rand(len(q))
+    yield randomized_friction
 
-    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):
+def test_joint_torque(coefficients_functions, friction):
     """ Same as the above test, but has frictional effect.
     """
     # setup
-    A, B, C, cnst_F, cnst_g, path = coefficients_functions
+    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))
+    friction = np.random.rand(2)
     taulim = np.random.randn(2, 2)
-
     constraint = toppra.constraint.SecondOrderConstraint.joint_torque_constraint(
-        inv_dyn, taulim, friction=friction)
+        inv_dyn, taulim, friction)
     constraint.set_discretization_type(0)
     a, b, c, F, g, _, _ = constraint.compute_constraint_params(
         path, np.linspace(0, path.duration, 10), 1.0)
@@ -140,7 +110,7 @@ def friction(q):
     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])
+        ci_ = C(p_vec[i]) + np.sign(ps_vec[i]) * friction
         np.testing.assert_allclose(ai_, a[i])
         np.testing.assert_allclose(bi_, b[i])
         np.testing.assert_allclose(ci_, c[i])

tests/interpolators/test_poly_interpolator.py

@@ -11,7 +11,7 @@ def test_scalar():
     npt.assert_allclose(pi.eval([0, 0.5, 1]), [1, 2.75, 6])
     npt.assert_allclose(pi.evald([0, 0.5, 1]), [2, 5, 8])
     npt.assert_allclose(pi.evaldd([0, 0.5, 1]), [6, 6, 6])
-    npt.assert_allclose(pi.get_path_interval(), np.r_[0, 2])
+    npt.assert_allclose(pi.path_interval, np.r_[0, 2])
 
 
 def test_2_dof():

tests/interpolators/test_rave_trajectory.py

@@ -48,7 +48,7 @@ def test_consistency(request, env, traj_string):
     spec = traj.GetConfigurationSpecification()
 
     N = 100
-    ss = np.linspace(0, path.get_duration(), N)
+    ss = np.linspace(0, path.duration, N)
 
     # Openrave samples
     qs_rave = []