Merge pull request #37 from hungpham2511/hung-dev

Automatic scaling for qpoases solver-wrapper

Author: hungpham2511

.gitignore

@@ -101,4 +101,7 @@ venv.bak/
 /site
 
 # mypy
-.mypy_cache/
+.mypy_cache/
+
+# emacs temp file
+*~

.travis.yml

@@ -21,7 +21,7 @@ before_install:
   - ./install-fcl.sh
   - ./install-openrave.sh
   - sudo apt install python-tk
-  - pip install --ignore-installed cython matplotlib coverage python-coveralls cvxopt --user
+  - pip install --ignore-installed cython matplotlib coverage python-coveralls cvxopt tabulate --user
 
 install:
   # Install dependencies
@@ -34,6 +34,7 @@ install:
 script:
   - cd $CI_SOURCE_PATH
   - pip install -r requirements.txt --user
+  - pip install pandas  --user # for testing
   - python setup.py develop --user  # need to run the develop command to have coverage working. install command (below) should work just fine.
   # - python setup.py install --user
   - py.test --cov=toppra tests -Wonce

requirements.txt

@@ -3,7 +3,4 @@ cython>=0.22.0
 scipy>0.18
 coloredlogs
 enum34
-cvxpy>=0.4.11, <1.0.0
-pytest
-pytest-cov
 matplotlib<3.0.0

setup.py

@@ -4,11 +4,12 @@
 import numpy as np
 
 NAME = "toppra"
-VERSION = "0.2"
+VERSION = "0.3.0"
 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."
 URL = "https://github.com/hungpham2511/toppra"
+
 with open("requirements.txt", "r") as f:
     REQUIRES = [line.strip() for line in f if line.strip()]
 

tests/__init__.py

@@ -1,10 +1,6 @@
-try:
-    import openravepy as orpy
-except:
-    # Unable to find openrave
-    FOUND_OPENRAVEPY = False
+import toppra
 import pytest
-
+import openravepy as orpy
 
 @pytest.fixture(scope="session")
 def rave_env():
@@ -13,3 +9,24 @@ def rave_env():
     env.Destroy()
     orpy.RaveDestroy()
 
+
+def pytest_addoption(parser):
+    parser.addoption(
+        "--loglevel", action="store", default="WARNING",
+        help="Set toppra loglevel during testing."
+    )
+
+    parser.addoption(
+        "--robust_regex", action="store", default=".*oa.*",
+        help="Regex to choose problems to test when running test_robustness_main.py. "
+             "Select '.*oa.*' to run only tests for hotqpoases."
+    )
+
+    parser.addoption(
+        "--visualize", action="store_true", default=False,
+        help="If True visualize test instance."
+    )
+
+
+def pytest_collection_modifyitems(config, items):
+    toppra.setup_logging(config.getoption("--loglevel"))

tests/conftest.py

@@ -4,7 +4,7 @@
 import numpy.testing as npt
 import toppra as ta
 import toppra.constraint as constraint
-from toppra.constants import TINY, JVEL_MAXSD
+from toppra.constants import TINY, JVEL_MAXSD, SMALL
 from scipy.interpolate import CubicSpline
 
 
@@ -67,15 +67,18 @@ def test_constraint_satisfaction(self, velocity_pc_data):
                            qs[i] * sd >= vlim[:, 0],
                            sd >= 0, sd <= JVEL_MAXSD]
             prob = cvx.Problem(cvx.Maximize(sd), constraints)
-            prob.solve(solver=cvx.ECOS, abstol=1e-9)
-            xmax = sd.value ** 2
-
-            prob = cvx.Problem(cvx.Minimize(sd), constraints)
-            prob.solve(solver=cvx.ECOS, abstol=1e-9)
-            xmin = sd.value ** 2
+            try:
+                prob.solve(solver=cvx.ECOS, abstol=1e-9)
+                xmax = sd.value ** 2
+                
+                prob = cvx.Problem(cvx.Minimize(sd), constraints)
+                prob.solve(solver=cvx.ECOS, abstol=1e-9)
+                xmin = sd.value ** 2
+            except cvx.SolverError:
+                continue
 
             # 3. They should agree
-            npt.assert_allclose([xmin, xmax], xlimit[i], atol=TINY)
+            npt.assert_allclose([xmin, xmax], xlimit[i], atol=SMALL)
 
             # Assert non-negativity
             assert xlimit[i, 0] >= 0
@@ -123,16 +126,20 @@ def test_jnt_vel_varying_basic():
         constraints = [qs[i] * sd <= vlim[:, 1],
                        qs[i] * sd >= vlim[:, 0],
                        sd >= 0, sd <= JVEL_MAXSD]
-        prob = cvx.Problem(cvx.Maximize(sd), constraints)
-        prob.solve(solver=cvx.ECOS, abstol=1e-9)
-        xmax = sd.value ** 2
 
-        prob = cvx.Problem(cvx.Minimize(sd), constraints)
-        prob.solve(solver=cvx.ECOS, abstol=1e-9)
-        xmin = sd.value ** 2
+        try:
+            prob = cvx.Problem(cvx.Maximize(sd), constraints)
+            prob.solve(solver=cvx.ECOS, abstol=1e-9)
+            xmax = sd.value ** 2
+            
+            prob = cvx.Problem(cvx.Minimize(sd), constraints)
+            prob.solve(solver=cvx.ECOS, abstol=1e-9)
+            xmin = sd.value ** 2
+        except cvx.SolverError:
+            continue  # ECOS can't solve this problem.
 
         # 3. they should agree
-        npt.assert_allclose([xmin, xmax], xlimit[i], atol=TINY)
+        npt.assert_allclose([xmin, xmax], xlimit[i], atol=SMALL)
 
         # assert non-negativity
         assert xlimit[i, 0] >= 0

tests/constraint/test_joint_velocity.py

@@ -11,18 +11,6 @@
 
 toppra.setup_logging(level="INFO")
 
-try:
-    import mosek
-    FOUND_MOSEK = True
-except ImportError:
-    FOUND_MOSEK = False
-
-try:
-    import cvxpy
-    FOUND_CXPY = True
-except ImportError:
-    FOUND_CXPY = False
-
 
 @pytest.mark.parametrize("solver_wrapper", ["cvxpy", "qpoases", "hotqpoases", "seidel"])
 def test_toppra_linear(basic_constraints, basic_path, solver_wrapper):

tests/retime/test_general_cases.py

@@ -5,18 +5,6 @@
 
 toppra.setup_logging(level="INFO")
 
-try:
-    import mosek
-    FOUND_MOSEK = True
-except ImportError:
-    FOUND_MOSEK = False
-
-try:
-    import cvxpy
-    FOUND_CXPY = True
-except ImportError:
-    FOUND_CXPY = False
-
 
 @pytest.fixture(params=[(0, 0)])
 def vel_accel_robustaccel(request):

tests/retime/test_retime_wconic_constraints.py

@@ -7,7 +7,6 @@
 except ImportError:
     FOUND_OPENRAVE = False
 
-toppra.setup_logging("DEBUG")
 
 @pytest.fixture(scope='module')
 def robot_fixture():

tests/retime/test_retime_with_openrave.py

@@ -0,0 +1,9 @@
+Robustness test suites
+=======================
+
+The test suites in this folder ensure that TOPP-RA work correctly for
+different problem inputs. The `.yaml` files provide problem inputs,
+which are read and solved by `test_robustness_main.py`.
+
+
+

tests/retime_robustness/README.md

@@ -0,0 +1,100 @@
+small_joint_1:
+  waypoints:
+    - [-0.00038593, -0.00038593, -0.00038593]
+    - [ 0.00013093, 0.00013093, 0.00013093]
+    - [ 0.00064752, 0.00064752, 0.00064752]
+    - [ 0.00116383, 0.00116383, 0.00116383]
+    - [ 0.00167987, 0.00167987, 0.00167987]
+    - [ 0.00219563, 0.00219563, 0.00219563]
+    - [ 0.00271112, 0.00271112, 0.00271112]
+    - [ 0.00322633, 0.00322633, 0.00322633]
+    - [ 0.00374127, 0.00374127, 0.00374127]
+    - [ 0.00425594, 0.00425594, 0.00425594]
+
+  ss_waypoints: [0, 1]
+  nb_gridpoints: [51, 101]
+  vlim: [100, 100, 100]
+  alim: [100, 100, 100]
+
+  desired_duration: [0, 1.0]
+  solver_wrapper: ['hotqpoases', 'seidel']
+
+small_joint_2:
+  waypoints:
+    - [0, 0, 0]
+    - [0, 1e-5, 0]
+    - [0, 0, 0]
+
+  ss_waypoints: [0, 1]
+  nb_gridpoints: [51, 101]
+  vlim: [100, 10000, 100]
+  alim: [100, 10000, 100]
+
+  desired_duration: [0]
+  solver_wrapper: ['hotqpoases', 'seidel']
+
+small_joint_3:
+  waypoints:
+    - [0, 0, 0]
+    - [0, 1e-7, 0]
+    - [0, 0, 0]
+
+  ss_waypoints: [0, 1]
+  nb_gridpoints: [51, 101]
+  vlim: [100, 1000, 100]
+  alim: [100, 1000, 100]
+
+  desired_duration: [0]
+  solver_wrapper: ['hotqpoases', 'seidel']
+
+
+issue26:
+  remark: "From JadBadMobile. https://github.com/hungpham2511/toppra/issues/26"
+  waypoints:
+    - [-3.85929168e-04, -3.85929168e-04, -3.85929168e-04]
+    - [-2.13610365e-04, -2.13610365e-04, -2.13610365e-04]
+    - [-4.13223272e-05, -4.13223272e-05, -4.13223272e-05]
+    - [ 1.30934967e-04, 1.30934967e-04, 1.30934967e-04]
+    - [ 3.03161539e-04, 3.03161539e-04, 3.03161539e-04]
+    - [ 4.75357412e-04, 4.75357412e-04, 4.75357412e-04]
+    - [ 6.47522605e-04, 6.47522605e-04, 6.47522605e-04]
+    - [ 8.19657141e-04, 8.19657141e-04, 8.19657141e-04]
+    - [ 9.91761041e-04, 9.91761041e-04, 9.91761041e-04]
+    - [ 1.16383433e-03, 1.16383433e-03, 1.16383433e-03]
+
+  ss_waypoints: [0, 1]
+  nb_gridpoints: [51, 101]
+  vlim: [1000, 1000, 1000]
+  alim: [1000, 2000, 10000]
+
+  desired_duration: [0, 1.0]
+  solver_wrapper: ['hotqpoases', 'seidel']
+
+two_points_1:
+  waypoints:
+    - [0, 0, 0]
+    - [0, 1, 0]
+
+  ss_waypoints: [0, 1]
+  nb_gridpoints: [51, 101, 501]
+  vlim: [100, 100, 100]
+  alim: [100, 100, 100]
+
+  desired_duration: [0, 1.0]
+  optimal_duration: 0.2
+  solver_wrapper: ['hotqpoases', 'seidel']
+
+two_points_2:
+  waypoints:
+    - [0, 0, 0]
+    - [0, 1e-4, 0]
+
+  ss_waypoints: [0, 1]
+  nb_gridpoints: [51, 101, 1001]
+  vlim: [100, 100, 100]
+  alim: [100, 100, 100]
+
+  desired_duration: [0]
+  optimal_duration: 0.002
+  solver_wrapper: ['hotqpoases', 'seidel']
+