1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35 /* Author: Ioan Sucan */
36
37 #define BOOST_TEST_MODULE "StateSpaces"
38 #include <boost/test/unit_test.hpp>
39 #include <iostream>
40
41 #include "ompl/base/ScopedState.h"
42 #include "ompl/base/SpaceInformation.h"
43
44 #include "ompl/base/spaces/TimeStateSpace.h"
45 #include "ompl/base/spaces/RealVectorStateSpace.h"
46 #include "ompl/base/spaces/SO2StateSpace.h"
47 #include "ompl/base/spaces/SO3StateSpace.h"
48 #include "ompl/base/spaces/SE2StateSpace.h"
49 #include "ompl/base/spaces/SE3StateSpace.h"
50 #include "ompl/base/spaces/DiscreteStateSpace.h"
51 #include "ompl/base/spaces/ReedsSheppStateSpace.h"
52 #include "ompl/base/spaces/DubinsStateSpace.h"
53
54 #include <boost/math/constants/constants.hpp>
55
56 #include "StateSpaceTest.h"
57
58 using namespace ompl;
59
60 // define a convenience macro
61 #define BOOST_OMPL_EXPECT_NEAR(a, b, diff) BOOST_CHECK_SMALL((a) - (b), diff)
62
63 const double PI = boost::math::constants::pi<double>();
64
BOOST_AUTO_TEST_CASE(Dubins_Simple)65 BOOST_AUTO_TEST_CASE(Dubins_Simple)
66 {
67 auto d(std::make_shared<base::DubinsStateSpace>());
68 auto dsym(std::make_shared<base::DubinsStateSpace>(1., true));
69
70 base::RealVectorBounds bounds2(2);
71 bounds2.setLow(-3);
72 bounds2.setHigh(3);
73 d->setBounds(bounds2);
74 dsym->setBounds(bounds2);
75
76 d->setup();
77 d->sanityChecks();
78
79 dsym->setup();
80 dsym->sanityChecks();
81 }
82
BOOST_AUTO_TEST_CASE(ReedsShepp_Simple)83 BOOST_AUTO_TEST_CASE(ReedsShepp_Simple)
84 {
85 auto d(std::make_shared<base::ReedsSheppStateSpace>());
86
87 base::RealVectorBounds bounds2(2);
88 bounds2.setLow(-3);
89 bounds2.setHigh(3);
90 d->setBounds(bounds2);
91
92 d->setup();
93 d->sanityChecks();
94 }
95
BOOST_AUTO_TEST_CASE(Discrete_Simple)96 BOOST_AUTO_TEST_CASE(Discrete_Simple)
97 {
98 auto d(std::make_shared<base::DiscreteStateSpace>(0, 2));
99 base::DiscreteStateSpace &dm = *d;
100 d->setup();
101 d->sanityChecks();
102
103 BOOST_CHECK_EQUAL(d->getDimension(), 1u);
104 BOOST_CHECK_EQUAL(d->getMaximumExtent(), 2);
105 BOOST_CHECK_EQUAL(dm.getStateCount(), 3u);
106 base::ScopedState<base::DiscreteStateSpace> s1(d);
107 base::ScopedState<base::DiscreteStateSpace> s2(d);
108 base::ScopedState<base::DiscreteStateSpace> s3(d);
109 s1->value = 0;
110 s2->value = 2;
111 s3->value = 0;
112 BOOST_CHECK_EQUAL(s1, s3);
113 BOOST_CHECK_EQUAL(s2->value, 2);
114 d->interpolate(s1.get(), s2.get(), 0.3, s3.get());
115 BOOST_CHECK_EQUAL(s3->value, 1);
116 d->interpolate(s1.get(), s2.get(), 0.2, s3.get());
117 BOOST_CHECK_EQUAL(s3->value, 0);
118 }
119
BOOST_AUTO_TEST_CASE(SO2_Simple)120 BOOST_AUTO_TEST_CASE(SO2_Simple)
121 {
122 auto m(std::make_shared<base::SO2StateSpace>());
123 m->setup();
124 m->sanityChecks();
125
126 StateSpaceTest mt(m, 1000, 1e-15);
127 mt.test();
128
129 BOOST_CHECK_EQUAL(m->getDimension(), 1u);
130 BOOST_CHECK_EQUAL(m->getMaximumExtent(), PI);
131
132 base::ScopedState<base::SO2StateSpace> s1(m);
133 base::ScopedState<base::SO2StateSpace> s2(m);
134 base::ScopedState<base::SO2StateSpace> s3(m);
135
136 s1->value = PI - 0.1;
137 s2->value = -PI + 0.1;
138 BOOST_OMPL_EXPECT_NEAR(m->distance(s2.get(), s1.get()), 0.2, 1e-3);
139 BOOST_OMPL_EXPECT_NEAR(m->distance(s1.get(), s2.get()), 0.2, 1e-3);
140 BOOST_OMPL_EXPECT_NEAR(m->distance(s1.get(), s1.get()), 0.0, 1e-3);
141
142 s1->value = PI - 0.08;
143 m->interpolate(s1.get(), s2.get(), 0.5, s3.get());
144 BOOST_OMPL_EXPECT_NEAR(s3->value, -PI + 0.01, 1e-3);
145
146 s1->value = PI - 0.1;
147 s2->value = 0.1;
148 BOOST_OMPL_EXPECT_NEAR(m->distance(s2.get(), s1.get()), PI - 0.2, 1e-3);
149
150 m->interpolate(s1.get(), s2.get(), 0.5, s3.get());
151 BOOST_OMPL_EXPECT_NEAR(s3->value, PI / 2.0, 1e-3);
152
153 s2 = s1;
154 m->interpolate(s1.get(), s1.get(), 0.5, s1.get());
155 BOOST_CHECK_EQUAL(s1, s2);
156
157 m->interpolate(s1.get(), s2.get(), 0.5, s1.get());
158 BOOST_CHECK_EQUAL(s1, s2);
159 s1->value = 0.5;
160 s2->value = 1.5;
161
162 m->interpolate(s1.get(), s2.get(), 0.0, s3.get());
163 BOOST_OMPL_EXPECT_NEAR(s3->value, s1->value, 1e-3);
164 m->interpolate(s1.get(), s2.get(), 1.0, s3.get());
165 BOOST_OMPL_EXPECT_NEAR(s3->value, s2->value, 1e-3);
166 }
167
BOOST_AUTO_TEST_CASE(SO2_Projection)168 BOOST_AUTO_TEST_CASE(SO2_Projection)
169 {
170 auto m(std::make_shared<base::SO2StateSpace>());
171 m->setup();
172
173 base::ProjectionEvaluatorPtr proj = m->getDefaultProjection();
174 BOOST_CHECK_EQUAL(proj->getDimension(), 1u);
175
176 Eigen::VectorXd p(proj->getDimension());
177 base::ScopedState<base::SO2StateSpace> s(m);
178 proj->project(s.get(), p);
179 BOOST_CHECK_EQUAL(p[0], s->value);
180 }
181
BOOST_AUTO_TEST_CASE(SO2_Sampler)182 BOOST_AUTO_TEST_CASE(SO2_Sampler)
183 {
184 auto m(std::make_shared<base::SO2StateSpace>());
185 m->setup();
186 base::StateSamplerPtr s = m->allocStateSampler();
187 base::ScopedState<base::SO2StateSpace> x(m);
188 base::ScopedState<base::SO2StateSpace> y(m);
189 x.random();
190 for (int i = 0; i < 100; ++i)
191 {
192 s->sampleUniformNear(y.get(), x.get(), 10000);
193 BOOST_CHECK(y.satisfiesBounds());
194 }
195 }
196
BOOST_AUTO_TEST_CASE(SO3_Simple)197 BOOST_AUTO_TEST_CASE(SO3_Simple)
198 {
199 auto m(std::make_shared<base::SO3StateSpace>());
200 m->setup();
201 m->sanityChecks();
202
203 StateSpaceTest mt(m, 1000, 1e-12);
204 mt.test();
205
206 BOOST_CHECK_EQUAL(m->getDimension(), 3u);
207 BOOST_CHECK_EQUAL(m->getMaximumExtent(), .5 * PI);
208
209 base::ScopedState<base::SO3StateSpace> s1(m);
210 base::ScopedState<base::SO3StateSpace> s2(m);
211
212 s1.random();
213 s2 = s1;
214
215 BOOST_OMPL_EXPECT_NEAR(m->distance(s1.get(), s2.get()), 0.0, 1e-3);
216 BOOST_CHECK_EQUAL(s1, s2);
217
218 s2.random();
219
220 base::SpaceInformation si(m);
221 si.setStateValidityChecker([](const base::State *) { return true; });
222 si.setup();
223
224 std::vector<base::State *> states;
225 unsigned int ns = 100;
226 unsigned int count = si.getMotionStates(s1.get(), s2.get(), states, ns, true, true);
227 BOOST_CHECK(states.size() == count);
228 BOOST_CHECK(ns + 2 == count);
229
230 for (auto &state : states)
231 {
232 double nrm = m->as<base::SO3StateSpace>()->norm(state->as<base::SO3StateSpace::StateType>());
233 BOOST_OMPL_EXPECT_NEAR(nrm, 1.0, 1e-15);
234 BOOST_CHECK(m->satisfiesBounds(state));
235 si.freeState(state);
236 }
237
238 base::ProjectionEvaluatorPtr proj = m->getDefaultProjection();
239 BOOST_CHECK_EQUAL(proj->getDimension(), 3u);
240 }
241
BOOST_AUTO_TEST_CASE(RealVector_Bounds)242 BOOST_AUTO_TEST_CASE(RealVector_Bounds)
243 {
244 base::RealVectorBounds bounds1(1);
245 bounds1.setLow(0);
246 bounds1.setHigh(1);
247 base::RealVectorStateSpace rsm1(1);
248 rsm1.setBounds(bounds1);
249 rsm1.setup();
250 BOOST_CHECK_EQUAL(rsm1.getDimension(), 1u);
251 BOOST_OMPL_EXPECT_NEAR(rsm1.getMaximumExtent(), 1.0, 1e-3);
252
253 base::RealVectorBounds bounds3(3);
254 bounds3.setLow(0);
255 bounds3.setHigh(1);
256 base::RealVectorStateSpace rsm3(3);
257 rsm3.setBounds(bounds3);
258 rsm3.setup();
259 BOOST_CHECK(rsm3.getDimension() == 3);
260 BOOST_OMPL_EXPECT_NEAR(rsm3.getMaximumExtent(), sqrt(3.0), 1e-3);
261
262 BOOST_CHECK(rsm3.getBounds().low == bounds3.low);
263 BOOST_CHECK(rsm3.getBounds().high == bounds3.high);
264 }
265
BOOST_AUTO_TEST_CASE(RealVector_Simple)266 BOOST_AUTO_TEST_CASE(RealVector_Simple)
267 {
268 base::RealVectorBounds bounds3(3);
269 bounds3.setLow(0);
270 bounds3.setHigh(1);
271 auto m(std::make_shared<base::RealVectorStateSpace>(3));
272 base::RealVectorStateSpace &rm = *m->as<base::RealVectorStateSpace>();
273 rm.setBounds(bounds3);
274 rm.setDimensionName(2, "testDim");
275 m->setup();
276 m->sanityChecks();
277
278 StateSpaceTest mt(m, 1000, 1e-12);
279 mt.test();
280
281 base::ScopedState<base::RealVectorStateSpace> s0(m);
282 (*s0)[0] = 0;
283 (*s0)[1] = 0;
284 (*s0)[2] = 0;
285
286 BOOST_CHECK(m->satisfiesBounds(s0.get()));
287
288 base::ScopedState<base::RealVectorStateSpace> s1 = s0;
289 BOOST_CHECK_EQUAL(s0, s1);
290 BOOST_OMPL_EXPECT_NEAR(m->distance(s0.get(), s1.get()), 0.0, 1e-3);
291 m->interpolate(s0.get(), s0.get(), 0.6, s0.get());
292 BOOST_CHECK_EQUAL(s0, s1);
293 s1->values[2] = 1.0;
294
295 BOOST_CHECK(m->satisfiesBounds(s1.get()));
296 m->interpolate(s0.get(), s1.get(), 0.5, s0.get());
297
298 BOOST_OMPL_EXPECT_NEAR((*s0)[rm.getDimensionIndex("testDim")], 0.5, 1e-3);
299
300 auto m2(std::make_shared<base::RealVectorStateSpace>());
301 m2->addDimension(1, 2);
302 m2->setup();
303 BOOST_OMPL_EXPECT_NEAR(m2->getMaximumExtent(), 1.0, 1e-3);
304 BOOST_CHECK_EQUAL(m2->getDimension(), 1u);
305 }
306
BOOST_AUTO_TEST_CASE(Time_Bounds)307 BOOST_AUTO_TEST_CASE(Time_Bounds)
308 {
309 base::TimeStateSpace t;
310 t.setup();
311
312 BOOST_CHECK_EQUAL(t.getDimension(), 1u);
313 BOOST_CHECK_EQUAL(t.isBounded(), false);
314 BOOST_OMPL_EXPECT_NEAR(t.getMaximumExtent(), 1.0, 1e-3);
315
316 t.setBounds(-1, 1);
317 BOOST_CHECK(t.isBounded());
318 BOOST_OMPL_EXPECT_NEAR(t.getMaximumExtent(), 2.0, 1e-3);
319 }
320
BOOST_AUTO_TEST_CASE(Time_Simple)321 BOOST_AUTO_TEST_CASE(Time_Simple)
322 {
323 auto t(std::make_shared<base::TimeStateSpace>());
324 t->setup();
325 t->sanityChecks();
326 t->params()["valid_segment_count_factor"] = 1;
327
328 StateSpaceTest mt(t, 1000, 1e-12);
329 mt.test();
330
331 base::ScopedState<base::TimeStateSpace> ss(t);
332 ss.random();
333 BOOST_CHECK_EQUAL(ss->position, 0.0);
334 BOOST_CHECK(t->satisfiesBounds(ss.get()));
335
336 t->as<base::TimeStateSpace>()->setBounds(-1, 1);
337 t->setup();
338
339 for (int i = 0; i < 100; ++i)
340 {
341 ss.random();
342 if (fabs(ss->position) > 0.0)
343 break;
344 }
345 BOOST_CHECK(fabs(ss->position) > 0.0);
346
347 ss->position = 2.0;
348 BOOST_CHECK_EQUAL(t->satisfiesBounds(ss.get()), false);
349 t->enforceBounds(ss.get());
350 BOOST_OMPL_EXPECT_NEAR(ss->position, 1.0, 1e-3);
351 BOOST_CHECK(t->satisfiesBounds(ss.get()));
352
353 base::ScopedState<base::TimeStateSpace> s0 = ss;
354 s0->position = 0.5;
355 t->interpolate(s0.get(), ss.get(), 0.5, ss.get());
356 BOOST_OMPL_EXPECT_NEAR(ss->position, 0.75, 1e-3);
357 BOOST_CHECK_NE(s0, ss);
358 ss = s0;
359 BOOST_CHECK(s0 == ss);
360 }
361
BOOST_AUTO_TEST_CASE(Compound_Simple)362 BOOST_AUTO_TEST_CASE(Compound_Simple)
363 {
364 auto m1(std::make_shared<base::SE2StateSpace>());
365 auto m2(std::make_shared<base::SE3StateSpace>());
366 auto m3(std::make_shared<base::SO2StateSpace>());
367 auto m4(std::make_shared<base::SO3StateSpace>());
368
369 BOOST_CHECK(m1 + m1 == m1);
370
371 base::StateSpacePtr s = m1 + m2 + m3;
372 BOOST_CHECK(s + s == s);
373 BOOST_CHECK(s - s + s == s);
374 BOOST_CHECK(s + s - s + s == s);
375 BOOST_CHECK(s * s == s);
376 BOOST_CHECK(s * m2 == m2);
377 BOOST_CHECK(m1 * s == m1);
378 BOOST_CHECK(m1 + s == s);
379 BOOST_CHECK(s + m2 == s);
380 BOOST_CHECK(m1 + s + m2 == s);
381 BOOST_CHECK(m1 + s + m2 - "x" == s);
382 BOOST_CHECK(s - "random" == s);
383 BOOST_CHECK(m3 + m3 == m3);
384 BOOST_CHECK(m3 + s * m3 * s - m2 - m1 == m3 * s);
385
386 BOOST_CHECK(base::StateSpacePtr() + m1 == m1);
387 BOOST_CHECK(m1 + base::StateSpacePtr() == m1);
388 BOOST_CHECK_EQUAL(m1 + base::StateSpacePtr() == m2 * s, false);
389 BOOST_CHECK(base::StateSpacePtr() * s + m1 == m1);
390 BOOST_CHECK(m3 * base::StateSpacePtr() + m1 == m1);
391 BOOST_CHECK(base::StateSpacePtr() * base::StateSpacePtr() + m4 == m4);
392 BOOST_CHECK(base::StateSpacePtr() + base::StateSpacePtr() + m4 == m4);
393 BOOST_CHECK(base::StateSpacePtr() - base::StateSpacePtr() + m4 == m4);
394 BOOST_CHECK((base::StateSpacePtr() - "")->getDimension() == 0);
395
396 BOOST_CHECK_EQUAL(s->getDimension(), m1->getDimension() + m2->getDimension() + m3->getDimension());
397 base::StateSpacePtr d = s - m2;
398 BOOST_CHECK_EQUAL(d->getDimension(), m1->getDimension() + m3->getDimension());
399 BOOST_CHECK((s + d)->getDimension() == s->getDimension());
400
401 m4->setName("test");
402 BOOST_CHECK(m4->getName() == "test");
403
404 base::StateSpacePtr t = m1 + m4;
405 BOOST_CHECK_EQUAL((t - "test")->getDimension(), m1->getDimension());
406 BOOST_CHECK_EQUAL((m1 - m1)->getDimension(), 0u);
407 t->setName(t->getName());
408 base::ScopedState<> st(t);
409 BOOST_CHECK(t->getValueAddressAtIndex(st.get(), 10000) == nullptr);
410 BOOST_CHECK(t->includes(m1));
411 BOOST_CHECK_EQUAL(t->includes(m2), false);
412 BOOST_CHECK_EQUAL(m1->includes(t), false);
413 BOOST_CHECK(m3->includes(m3));
414 BOOST_CHECK(t->includes(t));
415 }
416