| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// Copyright (C) 2019 LAAS-CNRS |
||
2 |
// Author: Pierre Fernbach |
||
3 |
// |
||
4 |
// This file is part of the hpp-rbprm. |
||
5 |
// |
||
6 |
// hpp-rbprm is free software: you can redistribute it and/or modify |
||
7 |
// it under the terms of the GNU Lesser General Public License as published by |
||
8 |
// the Free Software Foundation, either version 3 of the License, or |
||
9 |
// (at your option) any later version. |
||
10 |
// |
||
11 |
// test-hpp is distributed in the hope that it will be useful, |
||
12 |
// but WITHOUT ANY WARRANTY; without even the implied warranty of |
||
13 |
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||
14 |
// GNU Lesser General Public License for more details. |
||
15 |
// |
||
16 |
// You should have received a copy of the GNU Lesser General Public License |
||
17 |
// along with hpp-core. If not, see <http://www.gnu.org/licenses/>. |
||
18 |
|||
19 |
#define BOOST_TEST_MODULE test - kinodynamic |
||
20 |
#include <boost/test/included/unit_test.hpp> |
||
21 |
#include <hpp/core/path-vector.hh> |
||
22 |
#include <hpp/core/problem-solver.hh> |
||
23 |
#include <hpp/pinocchio/configuration.hh> |
||
24 |
#include <hpp/rbprm/rbprm-device.hh> |
||
25 |
#include <pinocchio/fwd.hpp> |
||
26 |
|||
27 |
#include "tools-fullbody.hh" |
||
28 |
#include "tools-obstacle.hh" |
||
29 |
|||
30 |
using namespace hpp; |
||
31 |
using namespace rbprm; |
||
32 |
|||
33 |
62 |
bool checkPathVector(core::PathPtr_t path) { |
|
34 |
124 |
core::PathVectorPtr_t pv = dynamic_pointer_cast<core::PathVector>(path); |
|
35 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
62 |
BOOST_CHECK(pv->numberPaths() > 0); |
36 |
✓✓ | 62 |
if (pv->numberPaths() == 1) return true; |
37 |
|||
38 |
size_type idAcc = |
||
39 |
37 |
path->outputSize() - |
|
40 |
37 |
3; // because acceleration may be set à 0 in init/end config of paths |
|
41 |
|||
42 |
✓✗ | 74 |
core::PathPtr_t previousPath = pv->pathAtRank(0); |
43 |
bool successPath; |
||
44 |
✓✗✓✗ ✓✗ |
37 |
if (previousPath->initial().head(idAcc) != |
45 |
✓✗✓✗ ✗✓ |
74 |
(*previousPath)(0., successPath).head(idAcc)) { |
46 |
std::cout << "init config not equal to config at time 0" << std::endl; |
||
47 |
std::cout << "init config : " |
||
48 |
<< hpp::pinocchio::displayConfig(previousPath->initial()) |
||
49 |
<< std::endl; |
||
50 |
std::cout << "t=0 config : " |
||
51 |
<< hpp::pinocchio::displayConfig((*previousPath)(0., successPath)) |
||
52 |
<< std::endl; |
||
53 |
return false; |
||
54 |
} |
||
55 |
✓✗✓✗ ✓✗ |
37 |
if (previousPath->end().head(idAcc) != |
56 |
✓✗✓✗ ✓✗✗✓ |
74 |
(*previousPath)(previousPath->length(), successPath).head(idAcc)) { |
57 |
std::cout << "end config not equal to config at time length()" << std::endl; |
||
58 |
std::cout << "end config : " |
||
59 |
<< hpp::pinocchio::displayConfig(previousPath->end()) |
||
60 |
<< std::endl; |
||
61 |
std::cout << "t=l config : " |
||
62 |
<< hpp::pinocchio::displayConfig( |
||
63 |
(*previousPath)(previousPath->length(), successPath)) |
||
64 |
<< std::endl; |
||
65 |
return false; |
||
66 |
} |
||
67 |
|||
68 |
37 |
core::PathPtr_t currentPath; |
|
69 |
✓✓ | 383 |
for (size_t i = 1; i < pv->numberPaths(); ++i) { |
70 |
✓✗ | 346 |
currentPath = pv->pathAtRank(i); |
71 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
346 |
if (previousPath->end().head(idAcc) != currentPath->initial().head(idAcc)) { |
72 |
std::cout << "previous path end not equal to current init, for id" << i |
||
73 |
<< std::endl; |
||
74 |
std::cout << "end previous : " |
||
75 |
<< hpp::pinocchio::displayConfig(previousPath->end()) |
||
76 |
<< std::endl; |
||
77 |
std::cout << "init current : " |
||
78 |
<< hpp::pinocchio::displayConfig(currentPath->initial()) |
||
79 |
<< std::endl; |
||
80 |
return false; |
||
81 |
} |
||
82 |
✓✗✓✗ ✓✗ |
346 |
if (currentPath->initial().head(idAcc) != |
83 |
✓✗✓✗ ✗✓ |
692 |
(*currentPath)(0., successPath).head(idAcc)) { |
84 |
std::cout << "init config not equal to config at time 0 at index " << i |
||
85 |
<< std::endl; |
||
86 |
std::cout << "init config : " |
||
87 |
<< hpp::pinocchio::displayConfig(currentPath->initial()) |
||
88 |
<< std::endl; |
||
89 |
std::cout << "t=0 config : " |
||
90 |
<< hpp::pinocchio::displayConfig( |
||
91 |
(*currentPath)(0., successPath)) |
||
92 |
<< std::endl; |
||
93 |
return false; |
||
94 |
} |
||
95 |
✓✗✓✗ ✓✗ |
346 |
if (currentPath->end().head(idAcc) != |
96 |
✓✗✓✗ ✓✗✗✓ |
692 |
(*currentPath)(currentPath->length(), successPath).head(idAcc)) { |
97 |
std::cout << "end config not equal to config at time length() at index " |
||
98 |
<< i << std::endl; |
||
99 |
std::cout << "end config : " |
||
100 |
<< hpp::pinocchio::displayConfig(currentPath->end()) |
||
101 |
<< std::endl; |
||
102 |
std::cout << "t=l config : " |
||
103 |
<< hpp::pinocchio::displayConfig( |
||
104 |
(*currentPath)(currentPath->length(), successPath)) |
||
105 |
<< std::endl; |
||
106 |
return false; |
||
107 |
} |
||
108 |
346 |
previousPath = currentPath; |
|
109 |
} |
||
110 |
37 |
return true; |
|
111 |
} |
||
112 |
|||
113 |
62 |
bool checkPath(core::PathPtr_t path, double v, double dt = 0.01) { |
|
114 |
62 |
double t = dt; |
|
115 |
62 |
double maxD = v * sqrt(3.) * dt; |
|
116 |
bool successPath; |
||
117 |
✓✗✓✗ ✓✗ |
62 |
vector3_t a = (*path)(0., successPath).head<3>(); |
118 |
✗✓ | 62 |
if (!successPath) return false; |
119 |
✓✗ | 62 |
vector3_t b; |
120 |
double norm; |
||
121 |
✓✗✓✓ |
79214 |
while (t < path->length()) { |
122 |
✓✗✓✗ ✓✗ |
79152 |
b = (*path)(t, successPath).head<3>(); |
123 |
✗✓ | 79152 |
if (!successPath) return false; |
124 |
✓✗✓✗ |
79152 |
norm = (a - b).norm(); |
125 |
✗✓ | 79152 |
if (norm > maxD) return false; |
126 |
79152 |
t += dt; |
|
127 |
✓✗ | 79152 |
a = b; |
128 |
} |
||
129 |
62 |
return true; |
|
130 |
} |
||
131 |
|||
132 |
BOOST_AUTO_TEST_SUITE(rbrrt_kinodynamic) |
||
133 |
|||
134 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(load_abstract_model_hyq) { |
135 |
✓✗ | 4 |
hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = loadHyQAbsract(); |
136 |
// for(size_t i = 0 ; i < rbprmDevice->data().mass.size() ; ++i) |
||
137 |
// std::cout<<"mass : "<<i<<" = "<<rbprmDevice->data().mass[i]<<std::endl; |
||
138 |
// BOOST_CHECK_CLOSE(rbprmDevice->mass(),70.,1e-2); // FIXME : need to |
||
139 |
// investigate and open an issue |
||
140 |
✓✗ | 2 |
hpp::core::ProblemSolverPtr_t ps = hpp::core::ProblemSolver::create(); |
141 |
✓✗ | 2 |
ps->robot(rbprmDevice); |
142 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(rbprmDevice->mass(), 76.07, 1e-2); |
143 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(ps->robot()->mass(), 76.07, 1e-2); |
144 |
// for(size_t i = 0 ; i < rbprmDevice->data().mass.size() ; ++i) |
||
145 |
// std::cout<<"mass : "<<i<<" = "<<rbprmDevice->data().mass[i]<<std::endl; |
||
146 |
2 |
} |
|
147 |
|||
148 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(load_abstract_model_SimpleHumanoid) { |
149 |
✓✗ | 4 |
hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = loadSimpleHumanoidAbsract(); |
150 |
// for(size_t i = 0 ; i < rbprmDevice->data().mass.size() ; ++i) |
||
151 |
// std::cout<<"mass : "<<i<<" = "<<rbprmDevice->data().mass[i]<<std::endl; |
||
152 |
// BOOST_CHECK_CLOSE(rbprmDevice->mass(),70.,1e-2); // FIXME : need to |
||
153 |
// investigate and open an issue |
||
154 |
✓✗ | 2 |
hpp::core::ProblemSolverPtr_t ps = hpp::core::ProblemSolver::create(); |
155 |
✓✗ | 2 |
ps->robot(rbprmDevice); |
156 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(rbprmDevice->mass(), 70., 1e-2); |
157 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(ps->robot()->mass(), 70., 1e-2); |
158 |
// for(size_t i = 0 ; i < rbprmDevice->data().mass.size() ; ++i) |
||
159 |
// std::cout<<"mass : "<<i<<" = "<<rbprmDevice->data().mass[i]<<std::endl; |
||
160 |
2 |
} |
|
161 |
|||
162 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(straight_line) { |
163 |
✓✗ | 4 |
hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = loadSimpleHumanoidAbsract(); |
164 |
✓✗ | 2 |
rbprmDevice->setDimensionExtraConfigSpace(6); |
165 |
4 |
BindShooter bShooter; |
|
166 |
4 |
std::vector<double> boundsSO3; |
|
167 |
✓✗ | 2 |
boundsSO3.push_back(-1.7); |
168 |
✓✗ | 2 |
boundsSO3.push_back(1.7); |
169 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
170 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
171 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
172 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
173 |
✓✗ | 2 |
bShooter.so3Bounds_ = boundsSO3; |
174 |
hpp::core::ProblemSolverPtr_t ps = |
||
175 |
✓✗ | 2 |
configureRbprmProblemSolverForSupportLimbs(rbprmDevice, bShooter); |
176 |
2 |
hpp::core::ProblemSolver& pSolver = *ps; |
|
177 |
✓✗✓✗ |
2 |
loadObstacleWithAffordance(pSolver, std::string("hpp_environments"), |
178 |
✓✗ | 4 |
std::string("multicontact/ground"), |
179 |
✓✗ | 4 |
std::string("planning")); |
180 |
// configure planner |
||
181 |
✓✗✓✗ |
2 |
pSolver.addPathOptimizer(std::string("RandomShortcutDynamic")); |
182 |
✓✗✓✗ |
2 |
pSolver.configurationShooterType(std::string("RbprmShooter")); |
183 |
✓✗✓✗ |
2 |
pSolver.pathValidationType(std::string("RbprmPathValidation"), 0.05); |
184 |
✓✗✓✗ |
2 |
pSolver.distanceType(std::string("Kinodynamic")); |
185 |
✓✗✓✗ |
2 |
pSolver.steeringMethodType(std::string("RBPRMKinodynamic")); |
186 |
✓✗✓✗ |
2 |
pSolver.pathPlannerType(std::string("DynamicPlanner")); |
187 |
|||
188 |
// set problem parameters : |
||
189 |
2 |
double aMax = 0.1; |
|
190 |
2 |
double vMax = 0.3; |
|
191 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/velocityBound"), |
192 |
✓✗ | 4 |
core::Parameter(vMax)); |
193 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/accelerationBound"), |
194 |
✓✗ | 4 |
core::Parameter(aMax)); |
195 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootX"), |
196 |
✓✗ | 4 |
core::Parameter(0.2)); |
197 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootY"), |
198 |
✓✗ | 4 |
core::Parameter(0.12)); |
199 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/friction"), |
200 |
✓✗ | 4 |
core::Parameter(0.5)); |
201 |
✓✗ | 4 |
pSolver.problem()->setParameter( |
202 |
✓✗ | 4 |
std::string("ConfigurationShooter/sampleExtraDOF"), |
203 |
✓✗ | 4 |
core::Parameter(false)); |
204 |
|||
205 |
✓✓ | 6 |
for (size_type i = 0; i < 2; ++i) { |
206 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -vMax; |
207 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = vMax; |
208 |
} |
||
209 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(2) = 0.; |
210 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(2) = 0.; |
211 |
✓✓ | 6 |
for (size_type i = 3; i < 5; ++i) { |
212 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -aMax; |
213 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = aMax; |
214 |
} |
||
215 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(5) = 0.; |
216 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(5) = 0.; |
217 |
|||
218 |
// define the planning problem : |
||
219 |
✓✗✓✗ |
4 |
core::Configuration_t q_init(rbprmDevice->configSize()); |
220 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
q_init << 0, 0, 1.0, 0, 0, 0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
221 |
|||
222 |
✓✗ | 4 |
core::Configuration_t q_goal = q_init; |
223 |
✓✗ | 2 |
q_goal(0) = 1.5; |
224 |
|||
225 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.initConfig(ConfigurationPtr_t(new core::Configuration_t(q_init))); |
226 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.addGoalConfig(ConfigurationPtr_t(new core::Configuration_t(q_goal))); |
227 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.robot()->mass(), 70., 1e-2); |
228 |
✓✗ | 2 |
bool success = pSolver.prepareSolveStepByStep(); |
229 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(success); |
230 |
✓✗ | 2 |
pSolver.finishSolveStepByStep(); |
231 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPathVector(pSolver.paths().back())); |
232 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPath(pSolver.paths().back(), 0.5)); |
233 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 1); |
234 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 8., 1e-6); |
235 |
core::PathVectorPtr_t pv = |
||
236 |
4 |
dynamic_pointer_cast<core::PathVector>(pSolver.paths().back()); |
|
237 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pv->numberPaths(), 1); |
238 |
✓✗ | 2 |
pSolver.solve(); |
239 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 3); |
240 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 8., 1e-6); |
241 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPathVector(pSolver.paths().back())); |
242 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPath(pSolver.paths().back(), 0.5)); |
243 |
2 |
pv = dynamic_pointer_cast<core::PathVector>(pSolver.paths().back()); |
|
244 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pv->numberPaths(), 1); |
245 |
✓✗ | 2 |
pSolver.optimizePath(pSolver.paths().back()); |
246 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 4); |
247 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 8., 1e-6); |
248 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPathVector(pSolver.paths().back())); |
249 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPath(pSolver.paths().back(), 0.5)); |
250 |
2 |
pv = dynamic_pointer_cast<core::PathVector>(pSolver.paths().back()); |
|
251 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pv->numberPaths(), 1); |
252 |
2 |
} |
|
253 |
|||
254 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(square_v0) { |
255 |
✓✗ | 4 |
hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = loadSimpleHumanoidAbsract(); |
256 |
✓✗ | 2 |
rbprmDevice->setDimensionExtraConfigSpace(6); |
257 |
4 |
BindShooter bShooter; |
|
258 |
4 |
std::vector<double> boundsSO3; |
|
259 |
✓✗ | 2 |
boundsSO3.push_back(-1.7); |
260 |
✓✗ | 2 |
boundsSO3.push_back(1.7); |
261 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
262 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
263 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
264 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
265 |
✓✗ | 2 |
bShooter.so3Bounds_ = boundsSO3; |
266 |
hpp::core::ProblemSolverPtr_t ps = |
||
267 |
✓✗ | 2 |
configureRbprmProblemSolverForSupportLimbs(rbprmDevice, bShooter); |
268 |
2 |
hpp::core::ProblemSolver& pSolver = *ps; |
|
269 |
✓✗✓✗ |
2 |
loadObstacleWithAffordance(pSolver, std::string("hpp_environments"), |
270 |
✓✗ | 4 |
std::string("multicontact/ground"), |
271 |
✓✗ | 4 |
std::string("planning")); |
272 |
// configure planner |
||
273 |
✓✗✓✗ |
2 |
pSolver.addPathOptimizer(std::string("RandomShortcutDynamic")); |
274 |
✓✗✓✗ |
2 |
pSolver.configurationShooterType(std::string("RbprmShooter")); |
275 |
✓✗✓✗ |
2 |
pSolver.pathValidationType(std::string("RbprmPathValidation"), 0.05); |
276 |
✓✗✓✗ |
2 |
pSolver.distanceType(std::string("Kinodynamic")); |
277 |
✓✗✓✗ |
2 |
pSolver.steeringMethodType(std::string("RBPRMKinodynamic")); |
278 |
✓✗✓✗ |
2 |
pSolver.pathPlannerType(std::string("DynamicPlanner")); |
279 |
|||
280 |
// set problem parameters : |
||
281 |
2 |
double aMax = 0.5; |
|
282 |
2 |
double vMax = 1.; |
|
283 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/velocityBound"), |
284 |
✓✗ | 4 |
core::Parameter(vMax)); |
285 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/accelerationBound"), |
286 |
✓✗ | 4 |
core::Parameter(aMax)); |
287 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootX"), |
288 |
✓✗ | 4 |
core::Parameter(0.2)); |
289 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootY"), |
290 |
✓✗ | 4 |
core::Parameter(0.12)); |
291 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/friction"), |
292 |
✓✗ | 4 |
core::Parameter(0.5)); |
293 |
✓✗ | 4 |
pSolver.problem()->setParameter( |
294 |
✓✗ | 4 |
std::string("ConfigurationShooter/sampleExtraDOF"), |
295 |
✓✗ | 4 |
core::Parameter(false)); |
296 |
|||
297 |
✓✓ | 6 |
for (size_type i = 0; i < 2; ++i) { |
298 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -vMax; |
299 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = vMax; |
300 |
} |
||
301 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(2) = 0.; |
302 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(2) = 0.; |
303 |
✓✓ | 6 |
for (size_type i = 3; i < 5; ++i) { |
304 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -aMax; |
305 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = aMax; |
306 |
} |
||
307 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(5) = 0.; |
308 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(5) = 0.; |
309 |
|||
310 |
// define the planning problem : |
||
311 |
✓✗✓✗ |
4 |
core::Configuration_t q_init(rbprmDevice->configSize()); |
312 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
q_init << 0, 0, 1.0, 0, 0, 0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
313 |
|||
314 |
✓✗ | 4 |
core::Configuration_t q_goal = q_init; |
315 |
✓✗ | 2 |
q_goal(0) = 1.; |
316 |
✓✗ | 2 |
q_goal(1) = 1.; |
317 |
|||
318 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.initConfig(ConfigurationPtr_t(new core::Configuration_t(q_init))); |
319 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.addGoalConfig(ConfigurationPtr_t(new core::Configuration_t(q_goal))); |
320 |
✓✗ | 2 |
bool success = pSolver.prepareSolveStepByStep(); |
321 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(success); |
322 |
✓✗ | 2 |
pSolver.finishSolveStepByStep(); |
323 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 1); |
324 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 2.8284271247461898, |
325 |
1e-10); |
||
326 |
✓✗ | 2 |
pSolver.optimizePath(pSolver.paths().back()); |
327 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 2); |
328 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 2.8284271247461898, |
329 |
1e-10); |
||
330 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL( |
331 |
dynamic_pointer_cast<core::PathVector>(pSolver.paths().back()) |
||
332 |
->numberPaths(), |
||
333 |
1); |
||
334 |
|||
335 |
✓✗ | 2 |
pSolver.resetGoalConfigs(); |
336 |
✓✗ | 2 |
q_goal(0) = 0.; |
337 |
✓✗ | 2 |
q_goal(1) = 1.; |
338 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.addGoalConfig(ConfigurationPtr_t(new core::Configuration_t(q_goal))); |
339 |
✓✗ | 2 |
success = pSolver.prepareSolveStepByStep(); |
340 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(success); |
341 |
✓✗ | 2 |
pSolver.finishSolveStepByStep(); |
342 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 3); |
343 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 2.8284271247461898, |
344 |
1e-10); |
||
345 |
✓✗ | 2 |
pSolver.optimizePath(pSolver.paths().back()); |
346 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 4); |
347 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 2.8284271247461898, |
348 |
1e-10); |
||
349 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL( |
350 |
dynamic_pointer_cast<core::PathVector>(pSolver.paths().back()) |
||
351 |
->numberPaths(), |
||
352 |
1); |
||
353 |
|||
354 |
✓✗ | 2 |
pSolver.resetGoalConfigs(); |
355 |
✓✗ | 2 |
q_goal(0) = -1.; |
356 |
✓✗ | 2 |
q_goal(1) = 1.; |
357 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.addGoalConfig(ConfigurationPtr_t(new core::Configuration_t(q_goal))); |
358 |
✓✗ | 2 |
success = pSolver.prepareSolveStepByStep(); |
359 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(success); |
360 |
✓✗ | 2 |
pSolver.finishSolveStepByStep(); |
361 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 5); |
362 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 2.8284271247461898, |
363 |
1e-10); |
||
364 |
✓✗ | 2 |
pSolver.optimizePath(pSolver.paths().back()); |
365 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 6); |
366 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 2.8284271247461898, |
367 |
1e-10); |
||
368 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL( |
369 |
dynamic_pointer_cast<core::PathVector>(pSolver.paths().back()) |
||
370 |
->numberPaths(), |
||
371 |
1); |
||
372 |
|||
373 |
✓✗ | 2 |
pSolver.resetGoalConfigs(); |
374 |
✓✗ | 2 |
q_goal(0) = -1.; |
375 |
✓✗ | 2 |
q_goal(1) = 0.; |
376 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.addGoalConfig(ConfigurationPtr_t(new core::Configuration_t(q_goal))); |
377 |
✓✗ | 2 |
success = pSolver.prepareSolveStepByStep(); |
378 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(success); |
379 |
✓✗ | 2 |
pSolver.finishSolveStepByStep(); |
380 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 7); |
381 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 2.8284271247461898, |
382 |
1e-10); |
||
383 |
✓✗ | 2 |
pSolver.optimizePath(pSolver.paths().back()); |
384 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 8); |
385 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 2.8284271247461898, |
386 |
1e-10); |
||
387 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL( |
388 |
dynamic_pointer_cast<core::PathVector>(pSolver.paths().back()) |
||
389 |
->numberPaths(), |
||
390 |
1); |
||
391 |
|||
392 |
✓✗ | 2 |
pSolver.resetGoalConfigs(); |
393 |
✓✗ | 2 |
q_goal(0) = -1.5; |
394 |
✓✗ | 2 |
q_goal(1) = -1.; |
395 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.addGoalConfig(ConfigurationPtr_t(new core::Configuration_t(q_goal))); |
396 |
✓✗ | 2 |
success = pSolver.prepareSolveStepByStep(); |
397 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(success); |
398 |
✓✗ | 2 |
pSolver.finishSolveStepByStep(); |
399 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 9); |
400 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 3.4641016151377544, |
401 |
1e-10); |
||
402 |
✓✗ | 2 |
pSolver.optimizePath(pSolver.paths().back()); |
403 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 10); |
404 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 3.4641016151377544, |
405 |
1e-10); |
||
406 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL( |
407 |
dynamic_pointer_cast<core::PathVector>(pSolver.paths().back()) |
||
408 |
->numberPaths(), |
||
409 |
1); |
||
410 |
|||
411 |
✓✗ | 2 |
pSolver.resetGoalConfigs(); |
412 |
✓✗ | 2 |
q_goal(0) = 0.; |
413 |
✓✗ | 2 |
q_goal(1) = -1; |
414 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.addGoalConfig(ConfigurationPtr_t(new core::Configuration_t(q_goal))); |
415 |
✓✗ | 2 |
success = pSolver.prepareSolveStepByStep(); |
416 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(success); |
417 |
✓✗ | 2 |
pSolver.finishSolveStepByStep(); |
418 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 11); |
419 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 2.8284271247461898, |
420 |
1e-10); |
||
421 |
✓✗ | 2 |
pSolver.optimizePath(pSolver.paths().back()); |
422 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 12); |
423 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 2.8284271247461898, |
424 |
1e-10); |
||
425 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL( |
426 |
dynamic_pointer_cast<core::PathVector>(pSolver.paths().back()) |
||
427 |
->numberPaths(), |
||
428 |
1); |
||
429 |
|||
430 |
✓✗ | 2 |
pSolver.resetGoalConfigs(); |
431 |
✓✗ | 2 |
q_goal(0) = 1; |
432 |
✓✗ | 2 |
q_goal(1) = -1; |
433 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.addGoalConfig(ConfigurationPtr_t(new core::Configuration_t(q_goal))); |
434 |
✓✗ | 2 |
success = pSolver.prepareSolveStepByStep(); |
435 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(success); |
436 |
✓✗ | 2 |
pSolver.finishSolveStepByStep(); |
437 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 13); |
438 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 2.8284271247461898, |
439 |
1e-10); |
||
440 |
✓✗ | 2 |
pSolver.optimizePath(pSolver.paths().back()); |
441 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 14); |
442 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 2.8284271247461898, |
443 |
1e-10); |
||
444 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL( |
445 |
dynamic_pointer_cast<core::PathVector>(pSolver.paths().back()) |
||
446 |
->numberPaths(), |
||
447 |
1); |
||
448 |
|||
449 |
✓✗ | 2 |
pSolver.resetGoalConfigs(); |
450 |
✓✗ | 2 |
q_goal(0) = 1.3; |
451 |
✓✗ | 2 |
q_goal(1) = -1.2; |
452 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.addGoalConfig(ConfigurationPtr_t(new core::Configuration_t(q_goal))); |
453 |
✓✗ | 2 |
success = pSolver.prepareSolveStepByStep(); |
454 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(success); |
455 |
✓✗ | 2 |
pSolver.finishSolveStepByStep(); |
456 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 15); |
457 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 3.2249030993194197, |
458 |
1e-10); |
||
459 |
✓✗ | 2 |
pSolver.optimizePath(pSolver.paths().back()); |
460 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 16); |
461 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 3.2249030993194197, |
462 |
1e-10); |
||
463 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL( |
464 |
dynamic_pointer_cast<core::PathVector>(pSolver.paths().back()) |
||
465 |
->numberPaths(), |
||
466 |
1); |
||
467 |
2 |
} |
|
468 |
|||
469 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(straight_velocity) { |
470 |
✓✗ | 4 |
hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = loadSimpleHumanoidAbsract(); |
471 |
✓✗ | 2 |
rbprmDevice->setDimensionExtraConfigSpace(6); |
472 |
4 |
BindShooter bShooter; |
|
473 |
4 |
std::vector<double> boundsSO3; |
|
474 |
✓✗ | 2 |
boundsSO3.push_back(-1.7); |
475 |
✓✗ | 2 |
boundsSO3.push_back(1.7); |
476 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
477 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
478 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
479 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
480 |
✓✗ | 2 |
bShooter.so3Bounds_ = boundsSO3; |
481 |
hpp::core::ProblemSolverPtr_t ps = |
||
482 |
✓✗ | 2 |
configureRbprmProblemSolverForSupportLimbs(rbprmDevice, bShooter); |
483 |
2 |
hpp::core::ProblemSolver& pSolver = *ps; |
|
484 |
✓✗✓✗ |
2 |
loadObstacleWithAffordance(pSolver, std::string("hpp_environments"), |
485 |
✓✗ | 4 |
std::string("multicontact/ground"), |
486 |
✓✗ | 4 |
std::string("planning")); |
487 |
// configure planner |
||
488 |
✓✗✓✗ |
2 |
pSolver.addPathOptimizer(std::string("RandomShortcutDynamic")); |
489 |
✓✗✓✗ |
2 |
pSolver.configurationShooterType(std::string("RbprmShooter")); |
490 |
✓✗✓✗ |
2 |
pSolver.pathValidationType(std::string("RbprmPathValidation"), 0.05); |
491 |
✓✗✓✗ |
2 |
pSolver.distanceType(std::string("Kinodynamic")); |
492 |
✓✗✓✗ |
2 |
pSolver.steeringMethodType(std::string("RBPRMKinodynamic")); |
493 |
✓✗✓✗ |
2 |
pSolver.pathPlannerType(std::string("DynamicPlanner")); |
494 |
|||
495 |
// set problem parameters : |
||
496 |
2 |
double aMax = 0.5; |
|
497 |
2 |
double vMax = 1.5; |
|
498 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/velocityBound"), |
499 |
✓✗ | 4 |
core::Parameter(vMax)); |
500 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/accelerationBound"), |
501 |
✓✗ | 4 |
core::Parameter(aMax)); |
502 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootX"), |
503 |
✓✗ | 4 |
core::Parameter(0.2)); |
504 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootY"), |
505 |
✓✗ | 4 |
core::Parameter(0.12)); |
506 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/friction"), |
507 |
✓✗ | 4 |
core::Parameter(0.5)); |
508 |
✓✗ | 4 |
pSolver.problem()->setParameter( |
509 |
✓✗ | 4 |
std::string("ConfigurationShooter/sampleExtraDOF"), |
510 |
✓✗ | 4 |
core::Parameter(false)); |
511 |
|||
512 |
✓✓ | 6 |
for (size_type i = 0; i < 2; ++i) { |
513 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -vMax; |
514 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = vMax; |
515 |
} |
||
516 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(2) = 0.; |
517 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(2) = 0.; |
518 |
✓✓ | 6 |
for (size_type i = 3; i < 5; ++i) { |
519 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -aMax; |
520 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = aMax; |
521 |
} |
||
522 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(5) = 0.; |
523 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(5) = 0.; |
524 |
|||
525 |
// define the planning problem : |
||
526 |
✓✗✓✗ |
4 |
core::Configuration_t q_init(rbprmDevice->configSize()); |
527 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
q_init << 0, 0, 1.0, 0, 0, 0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
528 |
✓✗ | 2 |
q_init(7) = 0.5; // init velocity along x |
529 |
✓✗ | 4 |
core::Configuration_t q_goal = q_init; |
530 |
✓✗ | 2 |
q_goal(0) = 1.; |
531 |
|||
532 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.initConfig(ConfigurationPtr_t(new core::Configuration_t(q_init))); |
533 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.addGoalConfig(ConfigurationPtr_t(new core::Configuration_t(q_goal))); |
534 |
✓✗ | 2 |
bool success = pSolver.prepareSolveStepByStep(); |
535 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(success); |
536 |
✓✗ | 2 |
pSolver.finishSolveStepByStep(); |
537 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 1); |
538 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 1.4641016151377546, |
539 |
1e-10); |
||
540 |
✓✗ | 2 |
pSolver.optimizePath(pSolver.paths().back()); |
541 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 2); |
542 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 1.4641016151377546, |
543 |
1e-10); |
||
544 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPathVector(pSolver.paths().back())); |
545 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPath(pSolver.paths().back(), 1.7)); |
546 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL( |
547 |
dynamic_pointer_cast<core::PathVector>(pSolver.paths().back()) |
||
548 |
->numberPaths(), |
||
549 |
1); |
||
550 |
|||
551 |
✓✗ | 2 |
pSolver.resetGoalConfigs(); |
552 |
✓✗ | 2 |
q_goal(0) = 0.; |
553 |
✓✗ | 2 |
q_goal(7) = 0.1; |
554 |
✓✗ | 2 |
q_goal(8) = -0.2; // final velocity |
555 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.addGoalConfig(ConfigurationPtr_t(new core::Configuration_t(q_goal))); |
556 |
✓✗ | 2 |
success = pSolver.prepareSolveStepByStep(); |
557 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(success); |
558 |
✓✗ | 2 |
pSolver.finishSolveStepByStep(); |
559 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 3); |
560 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 2.642220510185596, 1e-10); |
561 |
✓✗ | 2 |
pSolver.optimizePath(pSolver.paths().back()); |
562 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 4); |
563 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 2.642220510185596, 1e-10); |
564 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPathVector(pSolver.paths().back())); |
565 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPath(pSolver.paths().back(), 1.7)); |
566 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL( |
567 |
dynamic_pointer_cast<core::PathVector>(pSolver.paths().back()) |
||
568 |
->numberPaths(), |
||
569 |
1); |
||
570 |
|||
571 |
✓✗ | 2 |
pSolver.resetGoalConfigs(); |
572 |
✓✗ | 2 |
q_goal(0) = 1.; |
573 |
✓✗ | 2 |
q_goal(7) = -0.3; |
574 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.addGoalConfig(ConfigurationPtr_t(new core::Configuration_t(q_goal))); |
575 |
✓✗ | 2 |
success = pSolver.prepareSolveStepByStep(); |
576 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(success); |
577 |
✓✗ | 2 |
pSolver.finishSolveStepByStep(); |
578 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 5); |
579 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 2.8741411087489799, |
580 |
1e-10); |
||
581 |
✓✗ | 2 |
pSolver.optimizePath(pSolver.paths().back()); |
582 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 6); |
583 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 2.8741411087489799, |
584 |
1e-10); |
||
585 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPathVector(pSolver.paths().back())); |
586 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPath(pSolver.paths().back(), 1.7)); |
587 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL( |
588 |
dynamic_pointer_cast<core::PathVector>(pSolver.paths().back()) |
||
589 |
->numberPaths(), |
||
590 |
1); |
||
591 |
|||
592 |
✓✗ | 2 |
pSolver.resetGoalConfigs(); |
593 |
✓✗ | 2 |
q_goal(0) = -1.; |
594 |
✓✗ | 2 |
q_goal(7) = 0.; |
595 |
✓✗ | 2 |
q_goal(8) = 0.3; |
596 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.addGoalConfig(ConfigurationPtr_t(new core::Configuration_t(q_goal))); |
597 |
✓✗ | 2 |
success = pSolver.prepareSolveStepByStep(); |
598 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(success); |
599 |
✓✗ | 2 |
pSolver.finishSolveStepByStep(); |
600 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 7); |
601 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 4.16227766016838, 1e-10); |
602 |
✓✗ | 2 |
pSolver.optimizePath(pSolver.paths().back()); |
603 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 8); |
604 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 4.16227766016838, 1e-10); |
605 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPathVector(pSolver.paths().back())); |
606 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPath(pSolver.paths().back(), 1.7)); |
607 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL( |
608 |
dynamic_pointer_cast<core::PathVector>(pSolver.paths().back()) |
||
609 |
->numberPaths(), |
||
610 |
1); |
||
611 |
2 |
} |
|
612 |
|||
613 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(straight_line_amax_mu05) { |
614 |
✓✗ | 4 |
hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = loadSimpleHumanoidAbsract(); |
615 |
✓✗ | 2 |
rbprmDevice->setDimensionExtraConfigSpace(6); |
616 |
4 |
BindShooter bShooter; |
|
617 |
4 |
std::vector<double> boundsSO3; |
|
618 |
✓✗ | 2 |
boundsSO3.push_back(-1.7); |
619 |
✓✗ | 2 |
boundsSO3.push_back(1.7); |
620 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
621 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
622 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
623 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
624 |
✓✗ | 2 |
bShooter.so3Bounds_ = boundsSO3; |
625 |
hpp::core::ProblemSolverPtr_t ps = |
||
626 |
✓✗ | 2 |
configureRbprmProblemSolverForSupportLimbs(rbprmDevice, bShooter); |
627 |
2 |
hpp::core::ProblemSolver& pSolver = *ps; |
|
628 |
✓✗✓✗ |
2 |
loadObstacleWithAffordance(pSolver, std::string("hpp_environments"), |
629 |
✓✗ | 4 |
std::string("multicontact/ground"), |
630 |
✓✗ | 4 |
std::string("planning")); |
631 |
// configure planner |
||
632 |
✓✗✓✗ |
2 |
pSolver.addPathOptimizer(std::string("RandomShortcutDynamic")); |
633 |
✓✗✓✗ |
2 |
pSolver.configurationShooterType(std::string("RbprmShooter")); |
634 |
✓✗✓✗ |
2 |
pSolver.pathValidationType(std::string("RbprmPathValidation"), 0.05); |
635 |
✓✗✓✗ |
2 |
pSolver.distanceType(std::string("Kinodynamic")); |
636 |
✓✗✓✗ |
2 |
pSolver.steeringMethodType(std::string("RBPRMKinodynamic")); |
637 |
✓✗✓✗ |
2 |
pSolver.pathPlannerType(std::string("DynamicPlanner")); |
638 |
|||
639 |
// set problem parameters : |
||
640 |
2 |
double aMax = 10.; |
|
641 |
2 |
double vMax = 1.; |
|
642 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/velocityBound"), |
643 |
✓✗ | 4 |
core::Parameter(vMax)); |
644 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/accelerationBound"), |
645 |
✓✗ | 4 |
core::Parameter(aMax)); |
646 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootX"), |
647 |
✓✗ | 4 |
core::Parameter(0.2)); |
648 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootY"), |
649 |
✓✗ | 4 |
core::Parameter(0.12)); |
650 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/friction"), |
651 |
✓✗ | 4 |
core::Parameter(0.5)); |
652 |
✓✗ | 4 |
pSolver.problem()->setParameter( |
653 |
✓✗ | 4 |
std::string("ConfigurationShooter/sampleExtraDOF"), |
654 |
✓✗ | 4 |
core::Parameter(false)); |
655 |
|||
656 |
✓✓ | 6 |
for (size_type i = 0; i < 2; ++i) { |
657 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -vMax; |
658 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = vMax; |
659 |
} |
||
660 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(2) = 0.; |
661 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(2) = 0.; |
662 |
✓✓ | 6 |
for (size_type i = 3; i < 5; ++i) { |
663 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -aMax; |
664 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = aMax; |
665 |
} |
||
666 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(5) = 0.; |
667 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(5) = 0.; |
668 |
|||
669 |
// define the planning problem : |
||
670 |
✓✗✓✗ |
4 |
core::Configuration_t q_init(rbprmDevice->configSize()); |
671 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
q_init << 0, 0, 1.0, 0, 0, 0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
672 |
|||
673 |
✓✗ | 4 |
core::Configuration_t q_goal = q_init; |
674 |
✓✗ | 2 |
q_goal(0) = 1.5; |
675 |
|||
676 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.initConfig(ConfigurationPtr_t(new core::Configuration_t(q_init))); |
677 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.addGoalConfig(ConfigurationPtr_t(new core::Configuration_t(q_goal))); |
678 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.robot()->mass(), 70., 1e-2); |
679 |
✓✗ | 2 |
bool success = pSolver.prepareSolveStepByStep(); |
680 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(success); |
681 |
✓✗ | 2 |
pSolver.finishSolveStepByStep(); |
682 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 1); |
683 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 2.5298682146914024, 1e-6); |
684 |
2 |
} |
|
685 |
|||
686 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(straight_line_amax_mu005) { |
687 |
✓✗ | 4 |
hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = loadSimpleHumanoidAbsract(); |
688 |
✓✗ | 2 |
rbprmDevice->setDimensionExtraConfigSpace(6); |
689 |
4 |
BindShooter bShooter; |
|
690 |
4 |
std::vector<double> boundsSO3; |
|
691 |
✓✗ | 2 |
boundsSO3.push_back(-1.7); |
692 |
✓✗ | 2 |
boundsSO3.push_back(1.7); |
693 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
694 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
695 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
696 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
697 |
✓✗ | 2 |
bShooter.so3Bounds_ = boundsSO3; |
698 |
hpp::core::ProblemSolverPtr_t ps = |
||
699 |
✓✗ | 2 |
configureRbprmProblemSolverForSupportLimbs(rbprmDevice, bShooter); |
700 |
2 |
hpp::core::ProblemSolver& pSolver = *ps; |
|
701 |
✓✗✓✗ |
2 |
loadObstacleWithAffordance(pSolver, std::string("hpp_environments"), |
702 |
✓✗ | 4 |
std::string("multicontact/ground"), |
703 |
✓✗ | 4 |
std::string("planning")); |
704 |
// configure planner |
||
705 |
✓✗✓✗ |
2 |
pSolver.addPathOptimizer(std::string("RandomShortcutDynamic")); |
706 |
✓✗✓✗ |
2 |
pSolver.configurationShooterType(std::string("RbprmShooter")); |
707 |
✓✗✓✗ |
2 |
pSolver.pathValidationType(std::string("RbprmPathValidation"), 0.05); |
708 |
✓✗✓✗ |
2 |
pSolver.distanceType(std::string("Kinodynamic")); |
709 |
✓✗✓✗ |
2 |
pSolver.steeringMethodType(std::string("RBPRMKinodynamic")); |
710 |
✓✗✓✗ |
2 |
pSolver.pathPlannerType(std::string("DynamicPlanner")); |
711 |
|||
712 |
// set problem parameters : |
||
713 |
2 |
double aMax = 10.; |
|
714 |
2 |
double vMax = 1.; |
|
715 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/velocityBound"), |
716 |
✓✗ | 4 |
core::Parameter(vMax)); |
717 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/accelerationBound"), |
718 |
✓✗ | 4 |
core::Parameter(aMax)); |
719 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootX"), |
720 |
✓✗ | 4 |
core::Parameter(0.2)); |
721 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootY"), |
722 |
✓✗ | 4 |
core::Parameter(0.12)); |
723 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/friction"), |
724 |
✓✗ | 4 |
core::Parameter(0.05)); |
725 |
✓✗ | 4 |
pSolver.problem()->setParameter( |
726 |
✓✗ | 4 |
std::string("ConfigurationShooter/sampleExtraDOF"), |
727 |
✓✗ | 4 |
core::Parameter(false)); |
728 |
|||
729 |
✓✓ | 6 |
for (size_type i = 0; i < 2; ++i) { |
730 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -vMax; |
731 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = vMax; |
732 |
} |
||
733 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(2) = 0.; |
734 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(2) = 0.; |
735 |
✓✓ | 6 |
for (size_type i = 3; i < 5; ++i) { |
736 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -aMax; |
737 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = aMax; |
738 |
} |
||
739 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(5) = 0.; |
740 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(5) = 0.; |
741 |
|||
742 |
// define the planning problem : |
||
743 |
✓✗✓✗ |
4 |
core::Configuration_t q_init(rbprmDevice->configSize()); |
744 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
q_init << 0, 0, 1.0, 0, 0, 0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
745 |
|||
746 |
✓✗ | 4 |
core::Configuration_t q_goal = q_init; |
747 |
✓✗ | 2 |
q_goal(0) = 1.5; |
748 |
|||
749 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.initConfig(ConfigurationPtr_t(new core::Configuration_t(q_init))); |
750 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.addGoalConfig(ConfigurationPtr_t(new core::Configuration_t(q_goal))); |
751 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.robot()->mass(), 70., 1e-2); |
752 |
✓✗ | 2 |
bool success = pSolver.prepareSolveStepByStep(); |
753 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(success); |
754 |
✓✗ | 2 |
pSolver.finishSolveStepByStep(); |
755 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 1); |
756 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 3.5337445642816432, 1e-6); |
757 |
2 |
} |
|
758 |
|||
759 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(straight_line_amax_mu001) { |
760 |
✓✗ | 4 |
hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = loadSimpleHumanoidAbsract(); |
761 |
✓✗ | 2 |
rbprmDevice->setDimensionExtraConfigSpace(6); |
762 |
4 |
BindShooter bShooter; |
|
763 |
4 |
std::vector<double> boundsSO3; |
|
764 |
✓✗ | 2 |
boundsSO3.push_back(-1.7); |
765 |
✓✗ | 2 |
boundsSO3.push_back(1.7); |
766 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
767 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
768 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
769 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
770 |
✓✗ | 2 |
bShooter.so3Bounds_ = boundsSO3; |
771 |
hpp::core::ProblemSolverPtr_t ps = |
||
772 |
✓✗ | 2 |
configureRbprmProblemSolverForSupportLimbs(rbprmDevice, bShooter); |
773 |
2 |
hpp::core::ProblemSolver& pSolver = *ps; |
|
774 |
✓✗✓✗ |
2 |
loadObstacleWithAffordance(pSolver, std::string("hpp_environments"), |
775 |
✓✗ | 4 |
std::string("multicontact/ground"), |
776 |
✓✗ | 4 |
std::string("planning")); |
777 |
// configure planner |
||
778 |
✓✗✓✗ |
2 |
pSolver.addPathOptimizer(std::string("RandomShortcutDynamic")); |
779 |
✓✗✓✗ |
2 |
pSolver.configurationShooterType(std::string("RbprmShooter")); |
780 |
✓✗✓✗ |
2 |
pSolver.pathValidationType(std::string("RbprmPathValidation"), 0.05); |
781 |
✓✗✓✗ |
2 |
pSolver.distanceType(std::string("Kinodynamic")); |
782 |
✓✗✓✗ |
2 |
pSolver.steeringMethodType(std::string("RBPRMKinodynamic")); |
783 |
✓✗✓✗ |
2 |
pSolver.pathPlannerType(std::string("DynamicPlanner")); |
784 |
|||
785 |
// set problem parameters : |
||
786 |
2 |
double aMax = 10.; |
|
787 |
2 |
double vMax = 1.; |
|
788 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/velocityBound"), |
789 |
✓✗ | 4 |
core::Parameter(vMax)); |
790 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/accelerationBound"), |
791 |
✓✗ | 4 |
core::Parameter(aMax)); |
792 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootX"), |
793 |
✓✗ | 4 |
core::Parameter(0.2)); |
794 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootY"), |
795 |
✓✗ | 4 |
core::Parameter(0.12)); |
796 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/friction"), |
797 |
✓✗ | 4 |
core::Parameter(0.01)); |
798 |
✓✗ | 4 |
pSolver.problem()->setParameter( |
799 |
✓✗ | 4 |
std::string("ConfigurationShooter/sampleExtraDOF"), |
800 |
✓✗ | 4 |
core::Parameter(false)); |
801 |
|||
802 |
✓✓ | 6 |
for (size_type i = 0; i < 2; ++i) { |
803 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -vMax; |
804 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = vMax; |
805 |
} |
||
806 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(2) = 0.; |
807 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(2) = 0.; |
808 |
✓✓ | 6 |
for (size_type i = 3; i < 5; ++i) { |
809 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -aMax; |
810 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = aMax; |
811 |
} |
||
812 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(5) = 0.; |
813 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(5) = 0.; |
814 |
|||
815 |
// define the planning problem : |
||
816 |
✓✗✓✗ |
4 |
core::Configuration_t q_init(rbprmDevice->configSize()); |
817 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
q_init << 0, 0, 1.0, 0, 0, 0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
818 |
|||
819 |
✓✗ | 4 |
core::Configuration_t q_goal = q_init; |
820 |
✓✗ | 2 |
q_goal(0) = 1.5; |
821 |
|||
822 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.initConfig(ConfigurationPtr_t(new core::Configuration_t(q_init))); |
823 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.addGoalConfig(ConfigurationPtr_t(new core::Configuration_t(q_goal))); |
824 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.robot()->mass(), 70., 1e-2); |
825 |
✓✗ | 2 |
bool success = pSolver.prepareSolveStepByStep(); |
826 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(success); |
827 |
✓✗ | 2 |
pSolver.finishSolveStepByStep(); |
828 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 1); |
829 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 8.2531857104894222, 1e-6); |
830 |
2 |
} |
|
831 |
|||
832 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(straight_line_amax_mu5) { |
833 |
✓✗ | 4 |
hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = loadSimpleHumanoidAbsract(); |
834 |
✓✗ | 2 |
rbprmDevice->setDimensionExtraConfigSpace(6); |
835 |
4 |
BindShooter bShooter; |
|
836 |
4 |
std::vector<double> boundsSO3; |
|
837 |
✓✗ | 2 |
boundsSO3.push_back(-1.7); |
838 |
✓✗ | 2 |
boundsSO3.push_back(1.7); |
839 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
840 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
841 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
842 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
843 |
✓✗ | 2 |
bShooter.so3Bounds_ = boundsSO3; |
844 |
hpp::core::ProblemSolverPtr_t ps = |
||
845 |
✓✗ | 2 |
configureRbprmProblemSolverForSupportLimbs(rbprmDevice, bShooter); |
846 |
2 |
hpp::core::ProblemSolver& pSolver = *ps; |
|
847 |
✓✗✓✗ |
2 |
loadObstacleWithAffordance(pSolver, std::string("hpp_environments"), |
848 |
✓✗ | 4 |
std::string("multicontact/ground"), |
849 |
✓✗ | 4 |
std::string("planning")); |
850 |
// configure planner |
||
851 |
✓✗✓✗ |
2 |
pSolver.addPathOptimizer(std::string("RandomShortcutDynamic")); |
852 |
✓✗✓✗ |
2 |
pSolver.configurationShooterType(std::string("RbprmShooter")); |
853 |
✓✗✓✗ |
2 |
pSolver.pathValidationType(std::string("RbprmPathValidation"), 0.05); |
854 |
✓✗✓✗ |
2 |
pSolver.distanceType(std::string("Kinodynamic")); |
855 |
✓✗✓✗ |
2 |
pSolver.steeringMethodType(std::string("RBPRMKinodynamic")); |
856 |
✓✗✓✗ |
2 |
pSolver.pathPlannerType(std::string("DynamicPlanner")); |
857 |
|||
858 |
// set problem parameters : |
||
859 |
2 |
double aMax = 10.; |
|
860 |
2 |
double vMax = 1.; |
|
861 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/velocityBound"), |
862 |
✓✗ | 4 |
core::Parameter(vMax)); |
863 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/accelerationBound"), |
864 |
✓✗ | 4 |
core::Parameter(aMax)); |
865 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootX"), |
866 |
✓✗ | 4 |
core::Parameter(0.2)); |
867 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootY"), |
868 |
✓✗ | 4 |
core::Parameter(0.12)); |
869 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/friction"), |
870 |
✓✗ | 4 |
core::Parameter(5.)); |
871 |
✓✗ | 4 |
pSolver.problem()->setParameter( |
872 |
✓✗ | 4 |
std::string("ConfigurationShooter/sampleExtraDOF"), |
873 |
✓✗ | 4 |
core::Parameter(false)); |
874 |
|||
875 |
✓✓ | 6 |
for (size_type i = 0; i < 2; ++i) { |
876 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -vMax; |
877 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = vMax; |
878 |
} |
||
879 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(2) = 0.; |
880 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(2) = 0.; |
881 |
✓✓ | 6 |
for (size_type i = 3; i < 5; ++i) { |
882 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -aMax; |
883 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = aMax; |
884 |
} |
||
885 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(5) = 0.; |
886 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(5) = 0.; |
887 |
|||
888 |
// define the planning problem : |
||
889 |
✓✗✓✗ |
4 |
core::Configuration_t q_init(rbprmDevice->configSize()); |
890 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
q_init << 0, 0, 1.0, 0, 0, 0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
891 |
|||
892 |
✓✗ | 4 |
core::Configuration_t q_goal = q_init; |
893 |
✓✗ | 2 |
q_goal(0) = 1.5; |
894 |
|||
895 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.initConfig(ConfigurationPtr_t(new core::Configuration_t(q_init))); |
896 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.addGoalConfig(ConfigurationPtr_t(new core::Configuration_t(q_goal))); |
897 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.robot()->mass(), 70., 1e-2); |
898 |
✓✗ | 2 |
bool success = pSolver.prepareSolveStepByStep(); |
899 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(success); |
900 |
✓✗ | 2 |
pSolver.finishSolveStepByStep(); |
901 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 1); |
902 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 2.5298682886765311, 1e-6); |
903 |
2 |
} |
|
904 |
|||
905 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(straight_line_amax_feetChange) { |
906 |
✓✗ | 4 |
hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = loadSimpleHumanoidAbsract(); |
907 |
✓✗ | 2 |
rbprmDevice->setDimensionExtraConfigSpace(6); |
908 |
4 |
BindShooter bShooter; |
|
909 |
4 |
std::vector<double> boundsSO3; |
|
910 |
✓✗ | 2 |
boundsSO3.push_back(-1.7); |
911 |
✓✗ | 2 |
boundsSO3.push_back(1.7); |
912 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
913 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
914 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
915 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
916 |
✓✗ | 2 |
bShooter.so3Bounds_ = boundsSO3; |
917 |
hpp::core::ProblemSolverPtr_t ps = |
||
918 |
✓✗ | 2 |
configureRbprmProblemSolverForSupportLimbs(rbprmDevice, bShooter); |
919 |
2 |
hpp::core::ProblemSolver& pSolver = *ps; |
|
920 |
✓✗✓✗ |
2 |
loadObstacleWithAffordance(pSolver, std::string("hpp_environments"), |
921 |
✓✗ | 4 |
std::string("multicontact/ground"), |
922 |
✓✗ | 4 |
std::string("planning")); |
923 |
// configure planner |
||
924 |
✓✗✓✗ |
2 |
pSolver.addPathOptimizer(std::string("RandomShortcutDynamic")); |
925 |
✓✗✓✗ |
2 |
pSolver.configurationShooterType(std::string("RbprmShooter")); |
926 |
✓✗✓✗ |
2 |
pSolver.pathValidationType(std::string("RbprmPathValidation"), 0.05); |
927 |
✓✗✓✗ |
2 |
pSolver.distanceType(std::string("Kinodynamic")); |
928 |
✓✗✓✗ |
2 |
pSolver.steeringMethodType(std::string("RBPRMKinodynamic")); |
929 |
✓✗✓✗ |
2 |
pSolver.pathPlannerType(std::string("DynamicPlanner")); |
930 |
|||
931 |
// set problem parameters : |
||
932 |
2 |
double aMax = 10.; |
|
933 |
2 |
double vMax = 1.; |
|
934 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/velocityBound"), |
935 |
✓✗ | 4 |
core::Parameter(vMax)); |
936 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/accelerationBound"), |
937 |
✓✗ | 4 |
core::Parameter(aMax)); |
938 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootX"), |
939 |
✓✗ | 4 |
core::Parameter(0.05)); |
940 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootY"), |
941 |
✓✗ | 4 |
core::Parameter(0.05)); |
942 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/friction"), |
943 |
✓✗ | 4 |
core::Parameter(0.5)); |
944 |
✓✗ | 4 |
pSolver.problem()->setParameter( |
945 |
✓✗ | 4 |
std::string("ConfigurationShooter/sampleExtraDOF"), |
946 |
✓✗ | 4 |
core::Parameter(false)); |
947 |
|||
948 |
✓✓ | 6 |
for (size_type i = 0; i < 2; ++i) { |
949 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -vMax; |
950 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = vMax; |
951 |
} |
||
952 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(2) = 0.; |
953 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(2) = 0.; |
954 |
✓✓ | 6 |
for (size_type i = 3; i < 5; ++i) { |
955 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -aMax; |
956 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = aMax; |
957 |
} |
||
958 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(5) = 0.; |
959 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(5) = 0.; |
960 |
|||
961 |
// define the planning problem : |
||
962 |
✓✗✓✗ |
4 |
core::Configuration_t q_init(rbprmDevice->configSize()); |
963 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
q_init << 0, 0, 1.0, 0, 0, 0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; |
964 |
|||
965 |
✓✗ | 4 |
core::Configuration_t q_goal = q_init; |
966 |
✓✗ | 2 |
q_goal(0) = 1.5; |
967 |
|||
968 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.initConfig(ConfigurationPtr_t(new core::Configuration_t(q_init))); |
969 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.addGoalConfig(ConfigurationPtr_t(new core::Configuration_t(q_goal))); |
970 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.robot()->mass(), 70., 1e-2); |
971 |
✓✗ | 2 |
bool success = pSolver.prepareSolveStepByStep(); |
972 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(success); |
973 |
✓✗ | 2 |
pSolver.finishSolveStepByStep(); |
974 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 1); |
975 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.paths().back()->length(), 5.0502312883507487, 1e-6); |
976 |
2 |
} |
|
977 |
|||
978 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(nav_bauzil) { |
979 |
std::cout |
||
980 |
✓✗ | 2 |
<< "start nav_bauzil test case, this may take a couple of minutes ..." |
981 |
✓✗ | 2 |
<< std::endl; |
982 |
// this test case may take up to a minute to execute. Usually after ~5 minutes |
||
983 |
// it should be considered as a failure. |
||
984 |
✓✗ | 4 |
hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = loadSimpleHumanoidAbsract(); |
985 |
✓✗ | 2 |
rbprmDevice->setDimensionExtraConfigSpace(6); |
986 |
4 |
BindShooter bShooter; |
|
987 |
4 |
std::vector<double> boundsSO3; |
|
988 |
✓✗ | 2 |
boundsSO3.push_back(-4); |
989 |
✓✗ | 2 |
boundsSO3.push_back(4); |
990 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
991 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
992 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
993 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
994 |
✓✗ | 2 |
bShooter.so3Bounds_ = boundsSO3; |
995 |
hpp::core::ProblemSolverPtr_t ps = |
||
996 |
✓✗ | 2 |
configureRbprmProblemSolverForSupportLimbs(rbprmDevice, bShooter); |
997 |
2 |
hpp::core::ProblemSolver& pSolver = *ps; |
|
998 |
✓✗✓✗ |
2 |
loadObstacleWithAffordance(pSolver, std::string("hpp_environments"), |
999 |
✓✗ | 4 |
std::string("multicontact/floor_bauzil"), |
1000 |
✓✗ | 4 |
std::string("planning")); |
1001 |
// configure planner |
||
1002 |
✓✗✓✗ |
2 |
pSolver.addPathOptimizer(std::string("RandomShortcutDynamic")); |
1003 |
✓✗✓✗ |
2 |
pSolver.configurationShooterType(std::string("RbprmShooter")); |
1004 |
✓✗✓✗ |
2 |
pSolver.pathValidationType(std::string("RbprmPathValidation"), 0.05); |
1005 |
✓✗✓✗ |
2 |
pSolver.distanceType(std::string("Kinodynamic")); |
1006 |
✓✗✓✗ |
2 |
pSolver.steeringMethodType(std::string("RBPRMKinodynamic")); |
1007 |
✓✗✓✗ |
2 |
pSolver.pathPlannerType(std::string("DynamicPlanner")); |
1008 |
|||
1009 |
// set problem parameters : |
||
1010 |
2 |
double aMax = 0.1; |
|
1011 |
2 |
double vMax = 0.3; |
|
1012 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/velocityBound"), |
1013 |
✓✗ | 4 |
core::Parameter(vMax)); |
1014 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/accelerationBound"), |
1015 |
✓✗ | 4 |
core::Parameter(aMax)); |
1016 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootX"), |
1017 |
✓✗ | 4 |
core::Parameter(0.2)); |
1018 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootY"), |
1019 |
✓✗ | 4 |
core::Parameter(0.12)); |
1020 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/friction"), |
1021 |
✓✗ | 4 |
core::Parameter(0.5)); |
1022 |
✓✗ | 4 |
pSolver.problem()->setParameter( |
1023 |
✓✗ | 4 |
std::string("ConfigurationShooter/sampleExtraDOF"), |
1024 |
✓✗ | 4 |
core::Parameter(false)); |
1025 |
✓✗ | 4 |
pSolver.problem()->setParameter( |
1026 |
✓✗ | 4 |
std::string("PathOptimization/RandomShortcut/NumberOfLoops"), |
1027 |
✓✗ | 4 |
core::Parameter((core::size_type)50)); |
1028 |
|||
1029 |
✓✓ | 6 |
for (size_type i = 0; i < 2; ++i) { |
1030 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -vMax; |
1031 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = vMax; |
1032 |
} |
||
1033 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(2) = 0.; |
1034 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(2) = 0.; |
1035 |
✓✓ | 6 |
for (size_type i = 3; i < 5; ++i) { |
1036 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -aMax; |
1037 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = aMax; |
1038 |
} |
||
1039 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(5) = 0.; |
1040 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(5) = 0.; |
1041 |
|||
1042 |
// define the planning problem : |
||
1043 |
✓✗✓✗ |
4 |
core::Configuration_t q_init(rbprmDevice->configSize()); |
1044 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
q_init << -0.9, 1.5, 1., 0.0, 0.0, 0.0, 1.0, 0.07, 0, 0, 0.0, 0.0, 0.0; |
1045 |
✓✗✓✗ |
4 |
core::Configuration_t q_goal(rbprmDevice->configSize()); |
1046 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
q_goal << 2, 2.6, 1., 0.0, 0.0, 0.0, 1.0, 0.1, 0, 0, 0.0, 0.0, 0.0; |
1047 |
|||
1048 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.initConfig(ConfigurationPtr_t(new core::Configuration_t(q_init))); |
1049 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.addGoalConfig(ConfigurationPtr_t(new core::Configuration_t(q_goal))); |
1050 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.robot()->mass(), 70., 1e-2); |
1051 |
|||
1052 |
✓✗ | 2 |
pSolver.solve(); |
1053 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 2); |
1054 |
std::cout |
||
1055 |
✓✗ | 2 |
<< "Solve complete, start optimization. This may take few minutes ..." |
1056 |
✓✗ | 2 |
<< std::endl; |
1057 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPathVector(pSolver.paths().back())); |
1058 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPath(pSolver.paths().back(), 0.5)); |
1059 |
✓✓ | 22 |
for (size_t i = 0; i < 10; ++i) { |
1060 |
✓✗ | 20 |
pSolver.optimizePath(pSolver.paths().back()); |
1061 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 3 + i); |
1062 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
BOOST_CHECK(checkPathVector(pSolver.paths().back())); |
1063 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
BOOST_CHECK(checkPath(pSolver.paths().back(), 0.5)); |
1064 |
✓✗✓✗ ✓✗✓✗ |
20 |
std::cout << "(" << i + 1 << "/10) " << std::flush; |
1065 |
} |
||
1066 |
✓✗ | 2 |
std::cout << std::endl; |
1067 |
2 |
} |
|
1068 |
|||
1069 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(nav_bauzil_oriented) { |
1070 |
std::cout << "start nav_bauzil_oriented test case, this may take a couple of " |
||
1071 |
✓✗ | 2 |
"minutes ..." |
1072 |
✓✗ | 2 |
<< std::endl; |
1073 |
// this test case may take up to a minute to execute. Usually after ~5 minutes |
||
1074 |
// it should be considered as a failure. |
||
1075 |
✓✗ | 4 |
hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = loadSimpleHumanoidAbsract(); |
1076 |
✓✗ | 2 |
rbprmDevice->setDimensionExtraConfigSpace(6); |
1077 |
4 |
BindShooter bShooter; |
|
1078 |
4 |
std::vector<double> boundsSO3; |
|
1079 |
✓✗ | 2 |
boundsSO3.push_back(-4); |
1080 |
✓✗ | 2 |
boundsSO3.push_back(4); |
1081 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
1082 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
1083 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
1084 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
1085 |
✓✗ | 2 |
bShooter.so3Bounds_ = boundsSO3; |
1086 |
hpp::core::ProblemSolverPtr_t ps = |
||
1087 |
✓✗ | 2 |
configureRbprmProblemSolverForSupportLimbs(rbprmDevice, bShooter); |
1088 |
2 |
hpp::core::ProblemSolver& pSolver = *ps; |
|
1089 |
✓✗✓✗ |
2 |
loadObstacleWithAffordance(pSolver, std::string("hpp_environments"), |
1090 |
✓✗ | 4 |
std::string("multicontact/floor_bauzil"), |
1091 |
✓✗ | 4 |
std::string("planning")); |
1092 |
// configure planner |
||
1093 |
✓✗✓✗ |
2 |
pSolver.addPathOptimizer(std::string("RandomShortcutDynamic")); |
1094 |
✓✗✓✗ |
2 |
pSolver.configurationShooterType(std::string("RbprmShooter")); |
1095 |
✓✗✓✗ |
2 |
pSolver.pathValidationType(std::string("RbprmPathValidation"), 0.05); |
1096 |
✓✗✓✗ |
2 |
pSolver.distanceType(std::string("Kinodynamic")); |
1097 |
✓✗✓✗ |
2 |
pSolver.steeringMethodType(std::string("RBPRMKinodynamic")); |
1098 |
✓✗✓✗ |
2 |
pSolver.pathPlannerType(std::string("DynamicPlanner")); |
1099 |
|||
1100 |
// set problem parameters : |
||
1101 |
2 |
double aMax = 0.1; |
|
1102 |
2 |
double vMax = 0.3; |
|
1103 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/velocityBound"), |
1104 |
✓✗ | 4 |
core::Parameter(vMax)); |
1105 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/accelerationBound"), |
1106 |
✓✗ | 4 |
core::Parameter(aMax)); |
1107 |
✓✗ | 4 |
pSolver.problem()->setParameter( |
1108 |
✓✗✓✗ |
4 |
std::string("Kinodynamic/forceAllOrientation"), core::Parameter(true)); |
1109 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootX"), |
1110 |
✓✗ | 4 |
core::Parameter(0.2)); |
1111 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootY"), |
1112 |
✓✗ | 4 |
core::Parameter(0.12)); |
1113 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/friction"), |
1114 |
✓✗ | 4 |
core::Parameter(0.5)); |
1115 |
✓✗ | 4 |
pSolver.problem()->setParameter( |
1116 |
✓✗ | 4 |
std::string("ConfigurationShooter/sampleExtraDOF"), |
1117 |
✓✗ | 4 |
core::Parameter(false)); |
1118 |
✓✗ | 4 |
pSolver.problem()->setParameter( |
1119 |
✓✗ | 4 |
std::string("PathOptimization/RandomShortcut/NumberOfLoops"), |
1120 |
✓✗ | 4 |
core::Parameter((core::size_type)50)); |
1121 |
|||
1122 |
✓✓ | 6 |
for (size_type i = 0; i < 2; ++i) { |
1123 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -vMax; |
1124 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = vMax; |
1125 |
} |
||
1126 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(2) = 0.; |
1127 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(2) = 0.; |
1128 |
✓✓ | 6 |
for (size_type i = 3; i < 5; ++i) { |
1129 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -aMax; |
1130 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = aMax; |
1131 |
} |
||
1132 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(5) = 0.; |
1133 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(5) = 0.; |
1134 |
|||
1135 |
// define the planning problem : |
||
1136 |
✓✗✓✗ |
4 |
core::Configuration_t q_init(rbprmDevice->configSize()); |
1137 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
q_init << -0.9, 1.5, 1., 0.0, 0.0, 0.0, 1.0, 0.07, 0, 0, 0.0, 0.0, 0.0; |
1138 |
✓✗✓✗ |
4 |
core::Configuration_t q_goal(rbprmDevice->configSize()); |
1139 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
q_goal << 2, 2.6, 1., 0.0, 0.0, 0.0, 1.0, 0.1, 0, 0, 0.0, 0.0, 0.0; |
1140 |
|||
1141 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.initConfig(ConfigurationPtr_t(new core::Configuration_t(q_init))); |
1142 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.addGoalConfig(ConfigurationPtr_t(new core::Configuration_t(q_goal))); |
1143 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.robot()->mass(), 70., 1e-2); |
1144 |
|||
1145 |
✓✗ | 2 |
pSolver.solve(); |
1146 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 2); |
1147 |
std::cout |
||
1148 |
✓✗ | 2 |
<< "Solve complete, start optimization. This may take few minutes ..." |
1149 |
✓✗ | 2 |
<< std::endl; |
1150 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPathVector(pSolver.paths().back())); |
1151 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPath(pSolver.paths().back(), 0.5)); |
1152 |
✓✓ | 22 |
for (size_t i = 0; i < 10; ++i) { |
1153 |
✓✗ | 20 |
pSolver.optimizePath(pSolver.paths().back()); |
1154 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 3 + i); |
1155 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
BOOST_CHECK(checkPathVector(pSolver.paths().back())); |
1156 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
BOOST_CHECK(checkPath(pSolver.paths().back(), 0.5)); |
1157 |
✓✗✓✗ ✓✗✓✗ |
20 |
std::cout << "(" << i + 1 << "/10) " << std::flush; |
1158 |
} |
||
1159 |
✓✗ | 2 |
std::cout << std::endl; |
1160 |
2 |
} |
|
1161 |
|||
1162 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(nav_bauzil_oriented_kino) { |
1163 |
std::cout << "start nav_bauzil_oriented_kino test case, this may take a " |
||
1164 |
✓✗ | 2 |
"couple of minutes ..." |
1165 |
✓✗ | 2 |
<< std::endl; |
1166 |
// this test case may take up to a minute to execute. Usually after ~5 minutes |
||
1167 |
// it should be considered as a failure. |
||
1168 |
✓✗ | 4 |
hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = loadSimpleHumanoidAbsract(); |
1169 |
✓✗ | 2 |
rbprmDevice->setDimensionExtraConfigSpace(6); |
1170 |
4 |
BindShooter bShooter; |
|
1171 |
4 |
std::vector<double> boundsSO3; |
|
1172 |
✓✗ | 2 |
boundsSO3.push_back(-4); |
1173 |
✓✗ | 2 |
boundsSO3.push_back(4); |
1174 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
1175 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
1176 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
1177 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
1178 |
✓✗ | 2 |
bShooter.so3Bounds_ = boundsSO3; |
1179 |
hpp::core::ProblemSolverPtr_t ps = |
||
1180 |
✓✗ | 2 |
configureRbprmProblemSolverForSupportLimbs(rbprmDevice, bShooter); |
1181 |
2 |
hpp::core::ProblemSolver& pSolver = *ps; |
|
1182 |
✓✗✓✗ |
2 |
loadObstacleWithAffordance(pSolver, std::string("hpp_environments"), |
1183 |
✓✗ | 4 |
std::string("multicontact/floor_bauzil"), |
1184 |
✓✗ | 4 |
std::string("planning")); |
1185 |
// configure planner |
||
1186 |
✓✗✓✗ |
2 |
pSolver.addPathOptimizer(std::string("RandomShortcutDynamic")); |
1187 |
✓✗✓✗ |
2 |
pSolver.configurationShooterType(std::string("RbprmShooter")); |
1188 |
✓✗✓✗ |
2 |
pSolver.pathValidationType(std::string("RbprmPathValidation"), 0.05); |
1189 |
✓✗✓✗ |
2 |
pSolver.distanceType(std::string("Kinodynamic")); |
1190 |
✓✗✓✗ |
2 |
pSolver.steeringMethodType(std::string("RBPRMKinodynamic")); |
1191 |
✓✗✓✗ |
2 |
pSolver.pathPlannerType(std::string("DynamicPlanner")); |
1192 |
|||
1193 |
// set problem parameters : |
||
1194 |
2 |
double aMax = 0.1; |
|
1195 |
2 |
double vMax = 0.3; |
|
1196 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/velocityBound"), |
1197 |
✓✗ | 4 |
core::Parameter(vMax)); |
1198 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/accelerationBound"), |
1199 |
✓✗ | 4 |
core::Parameter(aMax)); |
1200 |
✓✗ | 4 |
pSolver.problem()->setParameter( |
1201 |
✓✗✓✗ |
4 |
std::string("Kinodynamic/forceAllOrientation"), core::Parameter(true)); |
1202 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootX"), |
1203 |
✓✗ | 4 |
core::Parameter(0.2)); |
1204 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootY"), |
1205 |
✓✗ | 4 |
core::Parameter(0.12)); |
1206 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/friction"), |
1207 |
✓✗ | 4 |
core::Parameter(0.5)); |
1208 |
✓✗ | 4 |
pSolver.problem()->setParameter( |
1209 |
✓✗ | 4 |
std::string("ConfigurationShooter/sampleExtraDOF"), |
1210 |
✓✗ | 4 |
core::Parameter(true)); |
1211 |
✓✗ | 4 |
pSolver.problem()->setParameter( |
1212 |
✓✗ | 4 |
std::string("PathOptimization/RandomShortcut/NumberOfLoops"), |
1213 |
✓✗ | 4 |
core::Parameter((core::size_type)50)); |
1214 |
|||
1215 |
✓✓ | 6 |
for (size_type i = 0; i < 2; ++i) { |
1216 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -vMax; |
1217 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = vMax; |
1218 |
} |
||
1219 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(2) = 0.; |
1220 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(2) = 0.; |
1221 |
✓✓ | 6 |
for (size_type i = 3; i < 5; ++i) { |
1222 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -aMax; |
1223 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = aMax; |
1224 |
} |
||
1225 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(5) = 0.; |
1226 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(5) = 0.; |
1227 |
|||
1228 |
// define the planning problem : |
||
1229 |
✓✗✓✗ |
4 |
core::Configuration_t q_init(rbprmDevice->configSize()); |
1230 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
q_init << -0.9, 1.5, 1., 0.0, 0.0, 0.0, 1.0, 0.07, 0, 0, 0.0, 0.0, 0.0; |
1231 |
✓✗✓✗ |
4 |
core::Configuration_t q_goal(rbprmDevice->configSize()); |
1232 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
q_goal << 2, 2.6, 1., 0.0, 0.0, 0.0, 1.0, 0.1, 0, 0, 0.0, 0.0, 0.0; |
1233 |
|||
1234 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.initConfig(ConfigurationPtr_t(new core::Configuration_t(q_init))); |
1235 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.addGoalConfig(ConfigurationPtr_t(new core::Configuration_t(q_goal))); |
1236 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.robot()->mass(), 70., 1e-2); |
1237 |
|||
1238 |
✓✗ | 2 |
pSolver.solve(); |
1239 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 2); |
1240 |
std::cout |
||
1241 |
✓✗ | 2 |
<< "Solve complete, start optimization. This may take few minutes ..." |
1242 |
✓✗ | 2 |
<< std::endl; |
1243 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPathVector(pSolver.paths().back())); |
1244 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPath(pSolver.paths().back(), 0.5)); |
1245 |
✓✓ | 22 |
for (size_t i = 0; i < 10; ++i) { |
1246 |
✓✗ | 20 |
pSolver.optimizePath(pSolver.paths().back()); |
1247 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 3 + i); |
1248 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
BOOST_CHECK(checkPathVector(pSolver.paths().back())); |
1249 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
BOOST_CHECK(checkPath(pSolver.paths().back(), 0.5)); |
1250 |
✓✗✓✗ ✓✗✓✗ |
20 |
std::cout << "(" << i + 1 << "/10) " << std::flush; |
1251 |
} |
||
1252 |
✓✗ | 2 |
std::cout << std::endl; |
1253 |
2 |
} |
|
1254 |
|||
1255 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(nav_bauzil_hyq) { |
1256 |
std::cout |
||
1257 |
✓✗ | 2 |
<< "start nav_bauzil_hyq test case, this may take a couple of minutes ..." |
1258 |
✓✗ | 2 |
<< std::endl; |
1259 |
// this test case may take up to a minute to execute. Usually after ~5 minutes |
||
1260 |
// it should be considered as a failure. |
||
1261 |
✓✗ | 4 |
hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = loadHyQAbsract(); |
1262 |
✓✗✓✗ |
2 |
rbprmDevice->rootJoint()->lowerBound(2, 0.62); |
1263 |
✓✗✓✗ |
2 |
rbprmDevice->rootJoint()->upperBound(2, 0.62); |
1264 |
✓✗ | 2 |
rbprmDevice->setDimensionExtraConfigSpace(6); |
1265 |
4 |
BindShooter bShooter; |
|
1266 |
4 |
std::vector<double> boundsSO3; |
|
1267 |
✓✗ | 2 |
boundsSO3.push_back(-4); |
1268 |
✓✗ | 2 |
boundsSO3.push_back(4); |
1269 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
1270 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
1271 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
1272 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
1273 |
✓✗ | 2 |
bShooter.so3Bounds_ = boundsSO3; |
1274 |
hpp::core::ProblemSolverPtr_t ps = |
||
1275 |
✓✗ | 2 |
configureRbprmProblemSolverForSupportLimbs(rbprmDevice, bShooter); |
1276 |
2 |
hpp::core::ProblemSolver& pSolver = *ps; |
|
1277 |
✓✗✓✗ |
2 |
loadObstacleWithAffordance(pSolver, std::string("hpp_environments"), |
1278 |
✓✗ | 4 |
std::string("multicontact/floor_bauzil"), |
1279 |
✓✗ | 4 |
std::string("planning")); |
1280 |
// configure planner |
||
1281 |
✓✗✓✗ |
2 |
pSolver.addPathOptimizer(std::string("RandomShortcutDynamic")); |
1282 |
✓✗✓✗ |
2 |
pSolver.configurationShooterType(std::string("RbprmShooter")); |
1283 |
✓✗✓✗ |
2 |
pSolver.pathValidationType(std::string("RbprmPathValidation"), 0.05); |
1284 |
✓✗✓✗ |
2 |
pSolver.distanceType(std::string("Kinodynamic")); |
1285 |
✓✗✓✗ |
2 |
pSolver.steeringMethodType(std::string("RBPRMKinodynamic")); |
1286 |
✓✗✓✗ |
2 |
pSolver.pathPlannerType(std::string("DynamicPlanner")); |
1287 |
|||
1288 |
// set problem parameters : |
||
1289 |
2 |
double aMax = 0.5; |
|
1290 |
2 |
double vMax = 0.3; |
|
1291 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/velocityBound"), |
1292 |
✓✗ | 4 |
core::Parameter(vMax)); |
1293 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/accelerationBound"), |
1294 |
✓✗ | 4 |
core::Parameter(aMax)); |
1295 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootX"), |
1296 |
✓✗ | 4 |
core::Parameter(0.01)); |
1297 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootY"), |
1298 |
✓✗ | 4 |
core::Parameter(0.01)); |
1299 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/friction"), |
1300 |
✓✗ | 4 |
core::Parameter(0.5)); |
1301 |
✓✗ | 4 |
pSolver.problem()->setParameter( |
1302 |
✓✗ | 4 |
std::string("ConfigurationShooter/sampleExtraDOF"), |
1303 |
✓✗ | 4 |
core::Parameter(false)); |
1304 |
✓✗ | 4 |
pSolver.problem()->setParameter( |
1305 |
✓✗ | 4 |
std::string("PathOptimization/RandomShortcut/NumberOfLoops"), |
1306 |
✓✗ | 4 |
core::Parameter((core::size_type)50)); |
1307 |
|||
1308 |
✓✓ | 6 |
for (size_type i = 0; i < 2; ++i) { |
1309 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -vMax; |
1310 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = vMax; |
1311 |
} |
||
1312 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(2) = 0.; |
1313 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(2) = 0.; |
1314 |
✓✓ | 6 |
for (size_type i = 3; i < 5; ++i) { |
1315 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -aMax; |
1316 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = aMax; |
1317 |
} |
||
1318 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(5) = 0.; |
1319 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(5) = 0.; |
1320 |
|||
1321 |
// define the planning problem : |
||
1322 |
✓✗✓✗ |
4 |
core::Configuration_t q_init(rbprmDevice->configSize()); |
1323 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
q_init << -0.2, 1.9, 0.62, 0.0, 0.0, 0.0, 1.0, 0.05, 0, 0, 0.0, 0.0, 0.0; |
1324 |
✓✗✓✗ |
4 |
core::Configuration_t q_goal(rbprmDevice->configSize()); |
1325 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
q_goal << 3.7, 0., 0.62, 0, 0, -0.7071, 0.7071, 0., -0.1, 0, 0.0, 0.0, 0.0; |
1326 |
✓✗✓✗ ✓✗✓✗ |
2 |
hpp::pinocchio::normalize(rbprmDevice, q_goal.head(rbprmDevice->model().nq)); |
1327 |
|||
1328 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.initConfig(ConfigurationPtr_t(new core::Configuration_t(q_init))); |
1329 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.addGoalConfig(ConfigurationPtr_t(new core::Configuration_t(q_goal))); |
1330 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.robot()->mass(), 76.07, 1e-2); |
1331 |
|||
1332 |
✓✗ | 2 |
pSolver.solve(); |
1333 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 2); |
1334 |
std::cout |
||
1335 |
✓✗ | 2 |
<< "Solve complete, start optimization. This may take few minutes ..." |
1336 |
✓✗ | 2 |
<< std::endl; |
1337 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPathVector(pSolver.paths().back())); |
1338 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPath(pSolver.paths().back(), 0.5)); |
1339 |
✓✓ | 22 |
for (size_t i = 0; i < 10; ++i) { |
1340 |
✓✗ | 20 |
pSolver.optimizePath(pSolver.paths().back()); |
1341 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 3 + i); |
1342 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
BOOST_CHECK(checkPathVector(pSolver.paths().back())); |
1343 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
BOOST_CHECK(checkPath(pSolver.paths().back(), 0.5)); |
1344 |
✓✗✓✗ ✓✗✓✗ |
20 |
std::cout << "(" << i + 1 << "/10) " << std::flush; |
1345 |
} |
||
1346 |
✓✗ | 2 |
std::cout << std::endl; |
1347 |
2 |
} |
|
1348 |
|||
1349 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(nav_bauzil_oriented_hyq) { |
1350 |
std::cout << "start nav_bauzil_oriented_hyq test case, this may take a " |
||
1351 |
✓✗ | 2 |
"couple of minutes ..." |
1352 |
✓✗ | 2 |
<< std::endl; |
1353 |
// this test case may take up to a minute to execute. Usually after ~5 minutes |
||
1354 |
// it should be considered as a failure. |
||
1355 |
✓✗ | 4 |
hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = loadHyQAbsract(); |
1356 |
✓✗✓✗ |
2 |
rbprmDevice->rootJoint()->lowerBound(2, 0.62); |
1357 |
✓✗✓✗ |
2 |
rbprmDevice->rootJoint()->upperBound(2, 0.62); |
1358 |
✓✗ | 2 |
rbprmDevice->setDimensionExtraConfigSpace(6); |
1359 |
4 |
BindShooter bShooter; |
|
1360 |
4 |
std::vector<double> boundsSO3; |
|
1361 |
✓✗ | 2 |
boundsSO3.push_back(-4); |
1362 |
✓✗ | 2 |
boundsSO3.push_back(4); |
1363 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
1364 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
1365 |
✓✗ | 2 |
boundsSO3.push_back(-0.1); |
1366 |
✓✗ | 2 |
boundsSO3.push_back(0.1); |
1367 |
✓✗ | 2 |
bShooter.so3Bounds_ = boundsSO3; |
1368 |
hpp::core::ProblemSolverPtr_t ps = |
||
1369 |
✓✗ | 2 |
configureRbprmProblemSolverForSupportLimbs(rbprmDevice, bShooter); |
1370 |
2 |
hpp::core::ProblemSolver& pSolver = *ps; |
|
1371 |
✓✗✓✗ |
2 |
loadObstacleWithAffordance(pSolver, std::string("hpp_environments"), |
1372 |
✓✗ | 4 |
std::string("multicontact/floor_bauzil"), |
1373 |
✓✗ | 4 |
std::string("planning")); |
1374 |
// configure planner |
||
1375 |
✓✗✓✗ |
2 |
pSolver.addPathOptimizer(std::string("RandomShortcutDynamic")); |
1376 |
✓✗✓✗ |
2 |
pSolver.configurationShooterType(std::string("RbprmShooter")); |
1377 |
✓✗✓✗ |
2 |
pSolver.pathValidationType(std::string("RbprmPathValidation"), 0.05); |
1378 |
✓✗✓✗ |
2 |
pSolver.distanceType(std::string("Kinodynamic")); |
1379 |
✓✗✓✗ |
2 |
pSolver.steeringMethodType(std::string("RBPRMKinodynamic")); |
1380 |
✓✗✓✗ |
2 |
pSolver.pathPlannerType(std::string("DynamicPlanner")); |
1381 |
|||
1382 |
// set problem parameters : |
||
1383 |
2 |
double aMax = 0.5; |
|
1384 |
2 |
double vMax = 0.3; |
|
1385 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/velocityBound"), |
1386 |
✓✗ | 4 |
core::Parameter(vMax)); |
1387 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("Kinodynamic/accelerationBound"), |
1388 |
✓✗ | 4 |
core::Parameter(aMax)); |
1389 |
✓✗ | 4 |
pSolver.problem()->setParameter( |
1390 |
✓✗✓✗ |
4 |
std::string("Kinodynamic/forceAllOrientation"), core::Parameter(true)); |
1391 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootX"), |
1392 |
✓✗ | 4 |
core::Parameter(0.01)); |
1393 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootY"), |
1394 |
✓✗ | 4 |
core::Parameter(0.01)); |
1395 |
✓✗✓✗ |
4 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/friction"), |
1396 |
✓✗ | 4 |
core::Parameter(0.5)); |
1397 |
✓✗ | 4 |
pSolver.problem()->setParameter( |
1398 |
✓✗ | 4 |
std::string("ConfigurationShooter/sampleExtraDOF"), |
1399 |
✓✗ | 4 |
core::Parameter(false)); |
1400 |
✓✗ | 4 |
pSolver.problem()->setParameter( |
1401 |
✓✗ | 4 |
std::string("PathOptimization/RandomShortcut/NumberOfLoops"), |
1402 |
✓✗ | 4 |
core::Parameter((core::size_type)50)); |
1403 |
|||
1404 |
✓✓ | 6 |
for (size_type i = 0; i < 2; ++i) { |
1405 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -vMax; |
1406 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = vMax; |
1407 |
} |
||
1408 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(2) = 0.; |
1409 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(2) = 0.; |
1410 |
✓✓ | 6 |
for (size_type i = 3; i < 5; ++i) { |
1411 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().lower(i) = -aMax; |
1412 |
✓✗ | 4 |
rbprmDevice->extraConfigSpace().upper(i) = aMax; |
1413 |
} |
||
1414 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().lower(5) = 0.; |
1415 |
✓✗ | 2 |
rbprmDevice->extraConfigSpace().upper(5) = 0.; |
1416 |
|||
1417 |
// define the planning problem : |
||
1418 |
✓✗✓✗ |
4 |
core::Configuration_t q_init(rbprmDevice->configSize()); |
1419 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
q_init << -0.2, 1.9, 0.62, 0.0, 0.0, 0.0, 1.0, 0.05, 0, 0, 0.0, 0.0, 0.0; |
1420 |
✓✗✓✗ |
4 |
core::Configuration_t q_goal(rbprmDevice->configSize()); |
1421 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
q_goal << 3.7, 0., 0.62, 0, 0, -0.7071, 0.7071, 0., -0.1, 0, 0.0, 0.0, 0.0; |
1422 |
✓✗✓✗ ✓✗✓✗ |
2 |
hpp::pinocchio::normalize(rbprmDevice, q_goal.head(rbprmDevice->model().nq)); |
1423 |
|||
1424 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.initConfig(ConfigurationPtr_t(new core::Configuration_t(q_init))); |
1425 |
✓✗✓✗ ✓✗✓✗ |
2 |
pSolver.addGoalConfig(ConfigurationPtr_t(new core::Configuration_t(q_goal))); |
1426 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(pSolver.robot()->mass(), 76.07, 1e-2); |
1427 |
|||
1428 |
✓✗ | 2 |
pSolver.solve(); |
1429 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 2); |
1430 |
std::cout |
||
1431 |
✓✗ | 2 |
<< "Solve complete, start optimization. This may take few minutes ..." |
1432 |
✓✗ | 2 |
<< std::endl; |
1433 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPathVector(pSolver.paths().back())); |
1434 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(checkPath(pSolver.paths().back(), 0.5)); |
1435 |
✓✓ | 22 |
for (size_t i = 0; i < 10; ++i) { |
1436 |
✓✗ | 20 |
pSolver.optimizePath(pSolver.paths().back()); |
1437 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
BOOST_CHECK_EQUAL(pSolver.paths().size(), 3 + i); |
1438 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
BOOST_CHECK(checkPathVector(pSolver.paths().back())); |
1439 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
BOOST_CHECK(checkPath(pSolver.paths().back(), 0.5)); |
1440 |
✓✗✓✗ ✓✗✓✗ |
20 |
std::cout << "(" << i + 1 << "/10) " << std::flush; |
1441 |
} |
||
1442 |
✓✗ | 2 |
std::cout << std::endl; |
1443 |
2 |
} |
|
1444 |
|||
1445 |
/* |
||
1446 |
// too slow to be added in the test suite ... |
||
1447 |
BOOST_AUTO_TEST_CASE (nav_bauzil_hard) { |
||
1448 |
std::cout<<"start nav_bauzil_hard test case, this may take several minutes |
||
1449 |
..."<<std::endl; |
||
1450 |
// this test case may take up to 5 minute to execute. Usually after ~10 |
||
1451 |
minutes it should be considered as a failure. hpp::pinocchio::RbPrmDevicePtr_t |
||
1452 |
rbprmDevice = loadSimpleHumanoidAbsract(); |
||
1453 |
rbprmDevice->rootJoint()->lowerBound(0, -2.3); |
||
1454 |
rbprmDevice->rootJoint()->lowerBound(1, -1.5); |
||
1455 |
rbprmDevice->rootJoint()->lowerBound(2, 0.98); |
||
1456 |
rbprmDevice->rootJoint()->upperBound(0, 4.6); |
||
1457 |
rbprmDevice->rootJoint()->upperBound(1, 3.3); |
||
1458 |
rbprmDevice->rootJoint()->upperBound(2, 0.98); |
||
1459 |
rbprmDevice->setDimensionExtraConfigSpace(6); |
||
1460 |
BindShooter bShooter; |
||
1461 |
std::vector<double> boundsSO3; |
||
1462 |
boundsSO3.push_back(-4); |
||
1463 |
boundsSO3.push_back(4); |
||
1464 |
boundsSO3.push_back(-0.1); |
||
1465 |
boundsSO3.push_back(0.1); |
||
1466 |
boundsSO3.push_back(-0.1); |
||
1467 |
boundsSO3.push_back(0.1); |
||
1468 |
bShooter.so3Bounds_ = boundsSO3; |
||
1469 |
hpp::core::ProblemSolverPtr_t ps = |
||
1470 |
configureRbprmProblemSolverForSupportLimbs(rbprmDevice, bShooter); |
||
1471 |
hpp::core::ProblemSolver& pSolver = *ps; |
||
1472 |
loadObstacleWithAffordance(pSolver, std::string("hpp_environments"), |
||
1473 |
std::string("multicontact/floor_bauzil"),std::string("planning")); |
||
1474 |
// configure planner |
||
1475 |
pSolver.addPathOptimizer(std::string("RandomShortcutDynamic")); |
||
1476 |
pSolver.configurationShooterType(std::string("RbprmShooter")); |
||
1477 |
pSolver.pathValidationType(std::string("RbprmPathValidation"),0.05); |
||
1478 |
pSolver.distanceType(std::string("Kinodynamic")); |
||
1479 |
pSolver.steeringMethodType(std::string("RBPRMKinodynamic")); |
||
1480 |
pSolver.pathPlannerType(std::string("DynamicPlanner")); |
||
1481 |
|||
1482 |
// set problem parameters : |
||
1483 |
double aMax = 0.1; |
||
1484 |
double vMax = 0.3; |
||
1485 |
pSolver.problem()->setParameter(std::string("Kinodynamic/velocityBound"),core::Parameter(vMax)); |
||
1486 |
pSolver.problem()->setParameter(std::string("Kinodynamic/accelerationBound"),core::Parameter(aMax)); |
||
1487 |
pSolver.problem()->setParameter(std::string("Kinodynamic/forceAllOrientation"),core::Parameter(true)); |
||
1488 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootX"),core::Parameter(0.2)); |
||
1489 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/sizeFootY"),core::Parameter(0.12)); |
||
1490 |
pSolver.problem()->setParameter(std::string("DynamicPlanner/friction"),core::Parameter(0.5)); |
||
1491 |
pSolver.problem()->setParameter(std::string("ConfigurationShooter/sampleExtraDOF"),core::Parameter(false)); |
||
1492 |
pSolver.problem()->setParameter(std::string("PathOptimization/RandomShortcut/NumberOfLoops"),core::Parameter((core::size_type)10)); |
||
1493 |
|||
1494 |
|||
1495 |
for(size_type i = 0 ; i < 2 ; ++i){ |
||
1496 |
rbprmDevice->extraConfigSpace().lower(i)=-vMax; |
||
1497 |
rbprmDevice->extraConfigSpace().upper(i)=vMax; |
||
1498 |
} |
||
1499 |
rbprmDevice->extraConfigSpace().lower(2)=0.; |
||
1500 |
rbprmDevice->extraConfigSpace().upper(2)=0.; |
||
1501 |
for(size_type i = 3 ; i < 5 ; ++i){ |
||
1502 |
rbprmDevice->extraConfigSpace().lower(i)=-aMax; |
||
1503 |
rbprmDevice->extraConfigSpace().upper(i)=aMax; |
||
1504 |
} |
||
1505 |
rbprmDevice->extraConfigSpace().lower(5)=0.; |
||
1506 |
rbprmDevice->extraConfigSpace().upper(5)=0.; |
||
1507 |
|||
1508 |
// define the planning problem : |
||
1509 |
core::Configuration_t q_init(rbprmDevice->configSize()); |
||
1510 |
q_init << -0.7, 2., 0.98, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.07, 0, 0, 0.0, |
||
1511 |
0.0, 0.0; core::Configuration_t q_goal(rbprmDevice->configSize()); q_goal << 0., |
||
1512 |
-1., 0.98, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.1, 0, 0, 0.0, 0.0, 0.0; |
||
1513 |
|||
1514 |
pSolver.initConfig(ConfigurationPtr_t(new core::Configuration_t(q_init))); |
||
1515 |
pSolver.addGoalConfig(ConfigurationPtr_t(new |
||
1516 |
core::Configuration_t(q_goal))); |
||
1517 |
BOOST_CHECK_CLOSE(pSolver.robot()->mass(),70.,1e-2); |
||
1518 |
|||
1519 |
pSolver.solve(); |
||
1520 |
BOOST_CHECK_EQUAL(pSolver.paths().size(),2); |
||
1521 |
std::cout<<"Solve complete, start optimization. This may take few minutes |
||
1522 |
..."<<std::endl; BOOST_CHECK(checkPathVector(pSolver.paths().back())); |
||
1523 |
BOOST_CHECK(checkPath(pSolver.paths().back(),0.5)); |
||
1524 |
for(size_t i = 0 ; i < 10 ; ++i){ |
||
1525 |
pSolver.optimizePath(pSolver.paths().back()); |
||
1526 |
BOOST_CHECK_EQUAL(pSolver.paths().size(),3+i); |
||
1527 |
BOOST_CHECK(checkPathVector(pSolver.paths().back())); |
||
1528 |
BOOST_CHECK(checkPath(pSolver.paths().back(),0.5)); |
||
1529 |
std::cout<<"("<<i+1<<"/10) "<<std::flush; |
||
1530 |
} |
||
1531 |
std::cout<<std::endl; |
||
1532 |
} |
||
1533 |
*/ |
||
1534 |
|||
1535 |
BOOST_AUTO_TEST_SUITE_END() |
| Generated by: GCOVR (Version 4.2) |