roadmap-1.cc 9.73 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
// Copyright (C) 2014 LAAS-CNRS
// Author: Mathieu Geisert
//
// This file is part of the hpp-core.
//
// hpp-core is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// test-hpp is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with hpp-core.  If not, see <http://www.gnu.org/licenses/>.

#include <boost/assign.hpp>

#include <hpp/util/debug.hh>
#include <hpp/model/device.hh>
#include <hpp/model/joint.hh>
#include <hpp/core/fwd.hh>
#include <hpp/core/roadmap.hh>
#include <hpp/core/weighed-distance.hh>
#include "hpp/core/basic-configuration-shooter.hh"
#include <hpp/core/connected-component.hh>
#include <hpp/core/node.hh>
#include <hpp/model/joint-configuration.hh>

#include <hpp/core/steering-method-straight.hh>
#include <hpp/core/weighed-distance.hh>

#define BOOST_TEST_MODULE roadmap-1
#include <boost/test/included/unit_test.hpp>

using hpp::model::Configuration_t;
using hpp::core::ConfigurationPtr_t;
using hpp::model::JointPtr_t;
using hpp::model::Device;
using hpp::model::DevicePtr_t;
using hpp::model::JointTranslation;
using hpp::core::SteeringMethodStraight;
45
using hpp::core::SteeringMethodStraightPtr_t;
46
47
48
49
50
51
52
using hpp::core::RoadmapPtr_t;
using hpp::core::Roadmap;
using hpp::core::NodePtr_t;
using hpp::core::WeighedDistance;

BOOST_AUTO_TEST_SUITE( test_hpp_core )

53
54
55
56
57
58
59
60
61
void addEdge (const hpp::core::RoadmapPtr_t& r,
	      const hpp::core::SteeringMethod& sm,
	      const std::vector <NodePtr_t>& nodes,
	      std::size_t i, std::size_t j)
{
  r->addEdge (nodes [i], nodes [j], sm (*(nodes [i]->configuration ()),
					*(nodes [j]->configuration ())));
}

62
63
64
BOOST_AUTO_TEST_CASE (Roadmap1) {
  // Build robot
  DevicePtr_t robot = Device::create("robot");
65
  JointPtr_t xJoint = new JointTranslation <1> (fcl::Transform3f());
66
67
68
  xJoint->isBounded(0,1);
  xJoint->lowerBound(0,-3.);
  xJoint->upperBound(0,3.);
69
  JointPtr_t yJoint = new JointTranslation <1>
70
71
72
73
74
75
76
77
78
    (fcl::Transform3f(fcl::Quaternion3f (sqrt (2)/2, 0, 0, sqrt(2)/2)));
  yJoint->isBounded(0,1);
  yJoint->lowerBound(0,-3.);
  yJoint->upperBound(0,3.);

  robot->rootJoint (xJoint);
  xJoint->addChildJoint (yJoint);

  // Create steering method
79
  SteeringMethodStraightPtr_t sm = SteeringMethodStraight::create (robot);
80
81
82
83
  // create roadmap
  RoadmapPtr_t r = Roadmap::create (WeighedDistance::create
				    (robot, boost::assign::list_of (1)(1)),
				    robot);
84
85
86
87
88
89

  std::vector <NodePtr_t> nodes;

  // nodes [0]
  ConfigurationPtr_t q (new Configuration_t (robot->configSize ()));
  (*q) [0] = 0; (*q) [1] = 0;
90
  // Set init node
91
92
  r->initNode (q);
  nodes.push_back (r->initNode ());
93

94
95
96
97
  // nodes [1]
  q = ConfigurationPtr_t (new Configuration_t (robot->configSize ()));
  (*q) [0] = 1; (*q) [1] = 0;
  nodes.push_back (r->addNode (q));
98

99
100
101
102
  // nodes [2]
  q = ConfigurationPtr_t (new Configuration_t (robot->configSize ()));
  (*q) [0] = 0.5; (*q) [1] = 0.9;
  nodes.push_back (r->addNode (q));
103

104
105
106
107
  // nodes [3]
  q = ConfigurationPtr_t (new Configuration_t (robot->configSize ()));
  (*q) [0] = -0.1; (*q) [1] = -0.9;
  nodes.push_back (r->addNode (q));
108

109
110
111
112
  // nodes [4]
  q = ConfigurationPtr_t (new Configuration_t (robot->configSize ()));
  (*q) [0] = 1.5; (*q) [1] = 2.9;
  nodes.push_back (r->addNode (q));
113

114
115
116
117
118
  // nodes [5]
  q = ConfigurationPtr_t (new Configuration_t (robot->configSize ()));
  (*q) [0] = 2.5; (*q) [1] = 2.9;
  nodes.push_back (r->addNode (q));
  r->addGoalNode (nodes [5]->configuration ());
119

120
121
122
123
124
125
126
127
128
129
130
131
  std::cout << *r << std::endl;
  BOOST_CHECK_EQUAL (r->connectedComponents ().size (), 6);
  for (std::size_t i=0; i < nodes.size (); ++i) {
    for (std::size_t j=i+1; j < nodes.size (); ++j) {
      BOOST_CHECK_MESSAGE (nodes [i]->connectedComponent () !=
			   nodes [j]->connectedComponent (),
			   "nodes " << i << " and " << j << "are in the same"
			   << " connected component");
    }
  }
  BOOST_CHECK (!r->pathExists ());
  // 0 -> 1
132
  addEdge (r, *sm, nodes, 0, 1);
133
134
135
136
137
138
139
140
141
142
143
  BOOST_CHECK_EQUAL (r->connectedComponents ().size (), 6);
  for (std::size_t i=0; i < nodes.size (); ++i) {
    for (std::size_t j=i+1; j < nodes.size (); ++j) {
      BOOST_CHECK_MESSAGE (nodes [i]->connectedComponent () !=
			   nodes [j]->connectedComponent (),
			   "nodes " << i << " and " << j << "are in the same"
			   << " connected component");
    }
  }
  BOOST_CHECK (!r->pathExists ());
  std::cout << *r << std::endl;
144

145
  // 1 -> 0
146
  addEdge (r, *sm, nodes, 1, 0);
147

148
149
150
151
152
153
154
155
156
157
158
159
160
161
  BOOST_CHECK_EQUAL (r->connectedComponents ().size (), 5);
  BOOST_CHECK (nodes [0]->connectedComponent () == 
	       nodes [1]->connectedComponent ());
  for (std::size_t i=1; i < nodes.size (); ++i) {
    for (std::size_t j=i+1; j < nodes.size (); ++j) {
      BOOST_CHECK_MESSAGE (nodes [i]->connectedComponent () !=
			   nodes [j]->connectedComponent (),
			   "nodes " << i << " and " << j << "are in the same"
			   << " connected component");
    }
  }
  BOOST_CHECK (!r->pathExists ());
  std::cout << *r << std::endl; 
  // 1 -> 2
162
  addEdge (r, *sm, nodes, 1, 2);
163
164
165
166
167
168
169
170
171
172
173
174
  BOOST_CHECK_EQUAL (r->connectedComponents ().size (), 5);
  BOOST_CHECK (nodes [0]->connectedComponent () == 
	       nodes [1]->connectedComponent ());
  for (std::size_t i=1; i < nodes.size (); ++i) {
    for (std::size_t j=i+1; j < nodes.size (); ++j) {
      BOOST_CHECK_MESSAGE (nodes [i]->connectedComponent () !=
			   nodes [j]->connectedComponent (),
			   "nodes " << i << " and " << j << "are in the same"
			   << " connected component");
    }
  }
  BOOST_CHECK (!r->pathExists ());
175
  std::cout << *r << std::endl;
176
177

  // 2 -> 0
178
  addEdge (r, *sm, nodes, 2, 0);
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
  BOOST_CHECK_EQUAL (r->connectedComponents ().size (), 4);
  BOOST_CHECK (nodes [0]->connectedComponent () == 
	       nodes [1]->connectedComponent ());
  BOOST_CHECK (nodes [0]->connectedComponent () == 
	       nodes [2]->connectedComponent ());
  for (std::size_t i=2; i < nodes.size (); ++i) {
    for (std::size_t j=i+1; j < nodes.size (); ++j) {
      BOOST_CHECK_MESSAGE (nodes [i]->connectedComponent () !=
			   nodes [j]->connectedComponent (),
			   "nodes " << i << " and " << j << "are in the same"
			   << " connected component");
    }
  }
  BOOST_CHECK (!r->pathExists ());
  std::cout << *r << std::endl;

  // 2 -> 3
196
  addEdge (r, *sm, nodes, 2, 3);
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
  BOOST_CHECK_EQUAL (r->connectedComponents ().size (), 4);
  BOOST_CHECK (nodes [0]->connectedComponent () == 
	       nodes [1]->connectedComponent ());
  BOOST_CHECK (nodes [0]->connectedComponent () == 
	       nodes [2]->connectedComponent ());
  for (std::size_t i=2; i < nodes.size (); ++i) {
    for (std::size_t j=i+1; j < nodes.size (); ++j) {
      BOOST_CHECK_MESSAGE (nodes [i]->connectedComponent () !=
			   nodes [j]->connectedComponent (),
			   "nodes " << i << " and " << j << "are in the same"
			   << " connected component");
    }
  }
  BOOST_CHECK (!r->pathExists ());
  std::cout << *r << std::endl;

  // 2 -> 4
214
  addEdge (r, *sm, nodes, 2, 4);
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
  BOOST_CHECK_EQUAL (r->connectedComponents ().size (), 4);
  BOOST_CHECK (nodes [0]->connectedComponent () == 
	       nodes [1]->connectedComponent ());
  BOOST_CHECK (nodes [0]->connectedComponent () == 
	       nodes [2]->connectedComponent ());
  for (std::size_t i=2; i < nodes.size (); ++i) {
    for (std::size_t j=i+1; j < nodes.size (); ++j) {
      BOOST_CHECK_MESSAGE (nodes [i]->connectedComponent () !=
			   nodes [j]->connectedComponent (),
			   "nodes " << i << " and " << j << "are in the same"
			   << " connected component");
    }
  }
  BOOST_CHECK (!r->pathExists ());
  std::cout << *r << std::endl;

  // 3 -> 5
232
  addEdge (r, *sm, nodes, 3, 5);
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
  BOOST_CHECK_EQUAL (r->connectedComponents ().size (), 4);
  BOOST_CHECK (nodes [0]->connectedComponent () == 
	       nodes [1]->connectedComponent ());
  BOOST_CHECK (nodes [0]->connectedComponent () == 
	       nodes [2]->connectedComponent ());
  for (std::size_t i=2; i < nodes.size (); ++i) {
    for (std::size_t j=i+1; j < nodes.size (); ++j) {
      BOOST_CHECK_MESSAGE (nodes [i]->connectedComponent () !=
			   nodes [j]->connectedComponent (),
			   "nodes " << i << " and " << j << "are in the same"
			   << " connected component");
    }
  }
  BOOST_CHECK (r->pathExists ());
  std::cout << *r << std::endl;

  // 4 -> 5
250
  addEdge (r, *sm, nodes, 4, 5);
251
252
253
254
255
256
257
258
259
260
261
262
263
  BOOST_CHECK_EQUAL (r->connectedComponents ().size (), 4);
  BOOST_CHECK (nodes [0]->connectedComponent () == 
	       nodes [1]->connectedComponent ());
  BOOST_CHECK (nodes [0]->connectedComponent () == 
	       nodes [2]->connectedComponent ());
  for (std::size_t i=2; i < nodes.size (); ++i) {
    for (std::size_t j=i+1; j < nodes.size (); ++j) {
      BOOST_CHECK_MESSAGE (nodes [i]->connectedComponent () !=
			   nodes [j]->connectedComponent (),
			   "nodes " << i << " and " << j << "are in the same"
			   << " connected component");
    }
  }
264
  BOOST_CHECK (r->pathExists ());
265
266
267
  std::cout << *r << std::endl;

  // 5 -> 0
268
  addEdge (r, *sm, nodes, 5, 0);
269
270
271
272
273
274
275
276
277
278
279
280
281
282
  BOOST_CHECK_EQUAL (r->connectedComponents ().size (), 1);
  BOOST_CHECK (nodes [0]->connectedComponent () == 
	       nodes [1]->connectedComponent ());
  BOOST_CHECK (nodes [0]->connectedComponent () == 
	       nodes [2]->connectedComponent ());
  BOOST_CHECK (nodes [0]->connectedComponent () == 
	       nodes [3]->connectedComponent ());
  BOOST_CHECK (nodes [0]->connectedComponent () == 
	       nodes [4]->connectedComponent ());
  BOOST_CHECK (nodes [0]->connectedComponent () == 
	       nodes [5]->connectedComponent ());
  BOOST_CHECK (r->pathExists ());
  std::cout << *r << std::endl;

283
284
285
286
287
}
BOOST_AUTO_TEST_SUITE_END()