Directory: | ./ |
---|---|
File: | tests/test-spline-gradient-based.cc |
Date: | 2024-12-13 16:14:03 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 50 | 50 | 100.0% |
Branches: | 160 | 314 | 51.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | #define BOOST_TEST_MODULE spline_gradient_based | ||
2 | #include <boost/mpl/list.hpp> | ||
3 | #include <boost/test/included/unit_test.hpp> | ||
4 | #include <hpp/core/path-optimization/spline-gradient-based.hh> | ||
5 | #include <hpp/core/problem.hh> | ||
6 | #include <hpp/core/straight-path.hh> | ||
7 | #include <hpp/pinocchio/urdf/util.hh> | ||
8 | #include <hpp/util/debug.hh> | ||
9 | |||
10 | using namespace hpp::pinocchio; | ||
11 | using namespace hpp::core; | ||
12 | |||
13 | 9 | DevicePtr_t createNDoFRobot(int ndof) { | |
14 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | std::ostringstream oss; |
15 | oss << "<robot name='test'>" | ||
16 |
2/4✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
|
9 | << "<link name='link0'/>"; |
17 |
2/2✓ Branch 0 taken 9 times.
✓ Branch 1 taken 9 times.
|
18 | for (int i = 0; i < ndof; ++i) { |
18 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | oss << "<joint name='joint" << i << "' type='prismatic'>" |
19 |
3/6✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
|
9 | << "<parent link='link" << i << "'/>" |
20 |
3/6✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
|
9 | << "<child link='link" << i + 1 << "'/>" |
21 | << "<limit effort='30' velocity='1.0' lower='-4' upper='4'/>" | ||
22 | << "</joint>" | ||
23 |
7/14✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 9 times.
✗ Branch 20 not taken.
|
9 | << "<link name='link" << i + 1 << "'/>"; |
24 | } | ||
25 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | oss << "</robot>"; |
26 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | std::string urdf(oss.str()); |
27 | |||
28 |
2/4✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
|
18 | DevicePtr_t robot = Device::create("test"); |
29 |
5/10✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 9 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 9 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
|
9 | urdf::loadModelFromString(robot, 0, "", "anchor", urdf, ""); |
30 | 18 | return robot; | |
31 | 9 | } | |
32 | |||
33 | typedef pathOptimization::SplineGradientBased<path::BernsteinBasis, 3> | ||
34 | SplineGradientBased_bezier3; | ||
35 | typedef pathOptimization::SplineGradientBased<path::BernsteinBasis, 5> | ||
36 | SplineGradientBased_bezier5; | ||
37 | typedef pathOptimization::SplineGradientBased<path::BernsteinBasis, 7> | ||
38 | SplineGradientBased_bezier7; | ||
39 | |||
40 | template <typename OptimizerType> | ||
41 | 18 | void test_spline_gradient_based(size_type costOrder) { | |
42 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
18 | DevicePtr_t dev = createNDoFRobot(1); |
43 |
1/2✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
|
18 | ProblemPtr_t problem = Problem::create(dev); |
44 |
3/6✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 9 times.
✗ Branch 10 not taken.
|
18 | problem->setParameter("SplineGradientBased/returnOptimum", Parameter(true)); |
45 |
3/6✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 9 times.
✗ Branch 10 not taken.
|
18 | problem->setParameter("SplineGradientBased/costOrder", Parameter(costOrder)); |
46 | |||
47 |
1/2✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
|
18 | typename OptimizerType::Ptr_t opt = OptimizerType::create(problem); |
48 | |||
49 |
3/6✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 9 times.
✗ Branch 10 not taken.
|
18 | PathVectorPtr_t pv = PathVector::create(dev->configSize(), dev->numberDof()); |
50 | |||
51 |
4/8✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 9 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 9 times.
✗ Branch 13 not taken.
|
18 | Configuration_t q0(dev->configSize()), q1(dev->configSize()); |
52 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
18 | q0 << 0.0; |
53 |
2/4✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
|
18 | vector_t subpathTimes(vector_t::LinSpaced(4, 0.0, 3.0)); |
54 |
2/2✓ Branch 1 taken 27 times.
✓ Branch 2 taken 9 times.
|
72 | for (int i = 1; i < subpathTimes.size(); ++i) { |
55 |
2/4✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
|
54 | q1 << subpathTimes[i]; |
56 |
6/12✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 27 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 27 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 27 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 27 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 27 times.
✗ Branch 20 not taken.
|
54 | pv->appendPath(StraightPath::create(dev, q0, q1, (q1 - q0)[0])); |
57 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
54 | q0 = q1; |
58 | } | ||
59 | |||
60 |
1/2✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
|
18 | PathVectorPtr_t optPath = opt->optimize(pv); |
61 | |||
62 |
2/4✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
|
18 | vector_t ts(vector_t::LinSpaced(subpathTimes.size() * 10, 0.0, 3.0)); |
63 |
2/4✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
|
18 | Configuration_t qi(dev->configSize()); |
64 |
4/8✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 9 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 9 times.
✗ Branch 13 not taken.
|
18 | vector_t vi(dev->numberDof()), ai(dev->numberDof()); |
65 |
3/6✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
|
18 | std::cout << "d_order" << OptimizerType::SplineOrder << "_cost" << costOrder |
66 |
2/4✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
|
18 | << "=["; |
67 |
2/2✓ Branch 1 taken 360 times.
✓ Branch 2 taken 9 times.
|
738 | for (int i = 0; i < ts.size(); ++i) { |
68 |
3/6✓ Branch 2 taken 360 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 360 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 360 times.
✗ Branch 9 not taken.
|
720 | optPath->eval(qi, ts[i]); |
69 |
3/6✓ Branch 2 taken 360 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 360 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 360 times.
✗ Branch 9 not taken.
|
720 | optPath->derivative(vi, ts[i], 1); |
70 |
3/6✓ Branch 2 taken 360 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 360 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 360 times.
✗ Branch 9 not taken.
|
720 | optPath->derivative(ai, ts[i], 2); |
71 |
10/20✓ Branch 1 taken 360 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 360 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 360 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 360 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 360 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 360 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 360 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 360 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 360 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 360 times.
✗ Branch 29 not taken.
|
720 | std::cout << '[' << ts[i] << ',' << qi << ',' << vi << ',' << ai << "],"; |
72 | } | ||
73 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
18 | std::cout << "]\n"; |
74 | 18 | } | |
75 | |||
76 | typedef boost::mpl::list<SplineGradientBased_bezier3, | ||
77 | SplineGradientBased_bezier5, | ||
78 | SplineGradientBased_bezier7> | ||
79 | SplineGradientBaseds_t; | ||
80 | |||
81 |
33/66✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 3 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 3 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 3 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 3 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 3 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 3 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 3 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 3 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 3 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 3 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 3 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 3 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 3 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 3 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 3 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 3 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 3 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 3 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 3 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 3 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 3 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 3 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 3 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 3 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 3 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 3 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 3 times.
✗ Branch 117 not taken.
|
12 | BOOST_AUTO_TEST_CASE_TEMPLATE(spline_gradient_based, SplineGradientBasedType, |
82 | SplineGradientBaseds_t) { | ||
83 | 6 | hpp::debug::setVerbosityLevel(50); | |
84 | 6 | test_spline_gradient_based<SplineGradientBasedType>(1); | |
85 | 6 | test_spline_gradient_based<SplineGradientBasedType>(2); | |
86 | 6 | test_spline_gradient_based<SplineGradientBasedType>(3); | |
87 | 6 | } | |
88 | |||
89 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(fake_spline_gradient_based_get_python_plot_function) { |
90 | 2 | std::cout << R"( | |
91 | import numpy as np | ||
92 | import matplotlib.pyplot as plt | ||
93 | |||
94 | def plot_q(dd): | ||
95 | d = np.array(dd) | ||
96 | plt.title("position") | ||
97 | plt.plot(d[:,0], d[:,1]) | ||
98 | plt.plot([d[0,0], d[-1,0]], [d[0,1], d[-1,1]]) | ||
99 | |||
100 | plt.show() | ||
101 | |||
102 | def plot_qva(dd): | ||
103 | d = np.array(dd) | ||
104 | plt.subplot(3,1,1) | ||
105 | plt.title("position") | ||
106 | plt.plot(d[:,0], d[:,1]) | ||
107 | |||
108 | plt.subplot(3,1,2) | ||
109 | plt.title("velocity") | ||
110 | plt.plot(d[:,0], d[:,2]) | ||
111 | |||
112 | plt.subplot(3,1,3) | ||
113 | plt.title("acceleration") | ||
114 | plt.plot(d[:,0], d[:,3]) | ||
115 | |||
116 | plt.show() | ||
117 | )"; | ||
118 | 2 | } | |
119 |