-
-
Notifications
You must be signed in to change notification settings - Fork 172
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
37 changed files
with
608 additions
and
281 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,35 @@ | ||
| import pytest | ||
| import numpy as np | ||
| import toppra | ||
| import toppra.constraint as constraint | ||
|
|
||
|
|
||
| @pytest.fixture(params=[(0, 0)]) | ||
| def basic_constraints(request): | ||
| """ Return a set of relatively simple constraints. | ||
| """ | ||
| dtype_a, dtype_ra = request.param | ||
| vlims = np.array([[-1, 1], [-1, 2], [-1, 4], [-3, 4], [-2, 4], [-3, 4], [-2, 5]], | ||
| dtype=float) * 10 | ||
| alims = np.array([[-1, 1], [-1, 2], [-1, 4], [-3, 4], [-2, 4], [-3, 4], [-2, 5]], | ||
| dtype=float) * 10 | ||
|
|
||
| vel_cnst = constraint.JointVelocityConstraint(vlims) | ||
| accl_cnst = constraint.JointAccelerationConstraint(alims, dtype_a) | ||
| robust_accl_cnst = constraint.RobustCanonicalLinearConstraint( | ||
| accl_cnst, [1e-4, 1e-4, 5e-4], dtype_ra) | ||
| yield vel_cnst, accl_cnst, robust_accl_cnst | ||
|
|
||
|
|
||
| @pytest.fixture(params=["spline", "poly"]) | ||
| def basic_path(request): | ||
| """ Return a generic path. | ||
| """ | ||
| if request.param == "spline": | ||
| np.random.seed(1) | ||
| path = toppra.SplineInterpolator(np.linspace(0, 1, 5), np.random.randn(5, 7)) | ||
| elif request.param == "poly": | ||
| np.random.seed(1) | ||
| coeffs = np.random.randn(7, 3) # 7 random quadratic equations | ||
| path = toppra.PolynomialPath(coeffs) | ||
| yield path |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,84 @@ | ||
| """General cases | ||
| This test suite guarantees that the basic functionalities provided by | ||
| toppra and the many interfaces to optimization solvers contained with | ||
| it work. | ||
| """ | ||
| import pytest | ||
| import numpy as np | ||
| import toppra | ||
| import toppra.constraint as constraint | ||
|
|
||
| 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): | ||
| """Solve some basic problem instances. | ||
| Passing this test guaranetees that the basic functionalities are | ||
| inplace. | ||
| """ | ||
| vel_c, acc_c, ro_acc_c = basic_constraints | ||
| instance = toppra.algorithm.TOPPRA( | ||
| [vel_c, acc_c], basic_path, solver_wrapper=solver_wrapper) | ||
| X = instance.compute_feasible_sets() | ||
| assert np.all(X >= 0) | ||
| assert not np.any(np.isnan(X)) | ||
|
|
||
| K = instance.compute_controllable_sets(0, 0) | ||
| assert np.all(K >= 0) | ||
| assert not np.any(np.isnan(K)) | ||
|
|
||
| traj, _ = instance.compute_trajectory(0, 0) | ||
| assert traj is not None | ||
|
|
||
|
|
||
| @pytest.mark.parametrize("solver_wrapper", [ | ||
| "cvxpy,qpoases", | ||
| "qpoases,hotqpoases", | ||
| "qpoases,seidel", | ||
| "hotqpoases,seidel" | ||
| ]) | ||
| def test_toppra_linear_compare(basic_constraints, basic_path, solver_wrapper): | ||
| """ Compare the output of the algorithm for basic instances. | ||
| """ | ||
| print("compare {:} and {:}".format(*solver_wrapper)) | ||
| solver_wrapper = solver_wrapper.split(",") | ||
| vel_c, acc_c, ro_acc_c = basic_constraints | ||
| instance = toppra.algorithm.TOPPRA( | ||
| [vel_c, acc_c], basic_path, solver_wrapper=solver_wrapper[0]) | ||
| instance2 = toppra.algorithm.TOPPRA( | ||
| [vel_c, acc_c], basic_path, solver_wrapper=solver_wrapper[1]) | ||
|
|
||
| K = instance.compute_controllable_sets(0, 0) | ||
| K2 = instance2.compute_controllable_sets(0, 0) | ||
| for i in range(instance._N, -1, -1): | ||
| np.testing.assert_allclose(K[i], K2[i], atol=1e-6, rtol=1e-2, | ||
| err_msg="Mismatched at i={:} / N={:}".format(i, instance._N)) | ||
|
|
||
| X = instance.compute_feasible_sets() | ||
| X2 = instance2.compute_feasible_sets() | ||
| for i in range(instance._N, -1, -1): | ||
| np.testing.assert_allclose(X[i], X2[i], atol=1e-6, rtol=1e-2, | ||
| err_msg="Mismatched at i={:} / N={:}".format(i, instance._N)) | ||
|
|
||
| sd, sdd, _ = instance.compute_parameterization(0, 0) | ||
| sd2, sdd2, _ = instance2.compute_parameterization(0, 0) | ||
| for i in range(instance._N - 1, -1, -1): | ||
| np.testing.assert_allclose(sd[i], sd2[i], atol=1e-6, rtol=1e-2, | ||
| err_msg="Mismatched at i={:} / N={:}".format(i, instance._N)) | ||
|
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,29 @@ | ||
| """This test suite ensures that problem scaling does not affect | ||
| solution quality, in the sense that a very short path (~1e-2) and a | ||
| very long path (~1e2) can be both parameterized. | ||
| """ | ||
| import pytest | ||
| import numpy as np | ||
| import toppra | ||
| import toppra.constraint as constraint | ||
|
|
||
| toppra.setup_logging(level="INFO") | ||
|
|
||
|
|
||
| @pytest.mark.parametrize("solver_wrapper", ["cvxpy", "hotqpoases", "seidel"]) | ||
| def test_linear_case1(basic_constraints, basic_path, solver_wrapper): | ||
| """A generic test case. | ||
| Compare scaling between 0.5 and 1.0. Since the scaling factor is | ||
| quite small, resulting trajectories should have similar durations. | ||
| """ | ||
| vel_c, acc_c, ro_acc_c = basic_constraints | ||
| instance_scale1 = toppra.algorithm.TOPPRA( | ||
| [vel_c, acc_c], basic_path, solver_wrapper=solver_wrapper, scaling=1.0) | ||
| instance_scale05 = toppra.algorithm.TOPPRA( | ||
| [vel_c, acc_c], basic_path, solver_wrapper=solver_wrapper, scaling=5) | ||
| traj1, _ = instance_scale1.compute_trajectory() | ||
| traj05, _ = instance_scale05.compute_trajectory() | ||
| # accurate up to 0.1% | ||
| np.testing.assert_allclose(traj1.duration, traj05.duration, rtol=1e-3) |
Oops, something went wrong.