OMPL
Overview
Download
Documentation
Primer
Installation
Tutorials
Demos
Python Bindings
Available Planners
Available State Spaces
FAQ
External links:
OMPL ROS Interface
OMPL ROS Tutorial
Code
API Overview
Classes
Files
Browse Repository
TeamCity Build Server
Issues
Community
Developers
Contributions
Education
Gallery
About
License
Citations
Acknowledgments
Contact Us
Blog
All
Classes
Namespaces
Functions
Variables
Typedefs
Enumerations
Enumerator
Friends
All
Classes
Namespaces
Files
Functions
Variables
Typedefs
Enumerations
Enumerator
Friends
Groups
Pages
src
ompl
control
planners
kpiece
KPIECE1.h
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
#ifndef OMPL_CONTROL_PLANNERS_KPIECE_KPIECE1_
38
#define OMPL_CONTROL_PLANNERS_KPIECE_KPIECE1_
39
40
#include "ompl/control/planners/PlannerIncludes.h"
41
#include "ompl/base/ProjectionEvaluator.h"
42
#include "ompl/datastructures/GridB.h"
43
#include <vector>
44
#include <set>
45
46
namespace
ompl
47
{
48
49
namespace
control
50
{
51
77
class
KPIECE1
:
public
base::Planner
78
{
79
public
:
80
82
KPIECE1
(
const
SpaceInformationPtr
&si);
83
84
virtual
~
KPIECE1
(
void
);
85
86
virtual
base::PlannerStatus
solve
(
const
base::PlannerTerminationCondition
&ptc);
87
88
virtual
void
clear
(
void
);
89
97
void
setGoalBias
(
double
goalBias)
98
{
99
goalBias_
= goalBias;
100
}
101
103
double
getGoalBias
(
void
)
const
104
{
105
return
goalBias_
;
106
}
107
114
void
setBorderFraction
(
double
bp)
115
{
116
selectBorderFraction_
= bp;
117
}
118
121
double
getBorderFraction
(
void
)
const
122
{
123
return
selectBorderFraction_
;
124
}
125
132
void
setCellScoreFactor
(
double
good,
double
bad)
133
{
134
setGoodCellScoreFactor
(good);
135
setBadCellScoreFactor
(bad);
136
}
137
139
void
setBadCellScoreFactor
(
double
bad)
140
{
141
badScoreFactor_
= bad;
142
}
143
145
void
setGoodCellScoreFactor
(
double
good)
146
{
147
goodScoreFactor_
= good;
148
}
149
152
double
getGoodCellScoreFactor
(
void
)
const
153
{
154
return
goodScoreFactor_
;
155
}
156
159
double
getBadCellScoreFactor
(
void
)
const
160
{
161
return
badScoreFactor_
;
162
}
163
166
void
setMaxCloseSamplesCount
(
unsigned
int
nCloseSamples)
167
{
168
nCloseSamples_
= nCloseSamples;
169
}
170
172
unsigned
int
getMaxCloseSamplesCount
(
void
)
const
173
{
174
return
nCloseSamples_
;
175
}
176
179
void
setProjectionEvaluator
(
const
base::ProjectionEvaluatorPtr
&projectionEvaluator)
180
{
181
projectionEvaluator_
= projectionEvaluator;
182
}
183
186
void
setProjectionEvaluator
(
const
std::string &name)
187
{
188
projectionEvaluator_
=
si_
->getStateSpace()->getProjection(name);
189
}
190
192
const
base::ProjectionEvaluatorPtr
&
getProjectionEvaluator
(
void
)
const
193
{
194
return
projectionEvaluator_
;
195
}
196
197
virtual
void
setup
(
void
);
198
virtual
void
getPlannerData
(
base::PlannerData
&data)
const
;
199
200
protected
:
201
203
struct
Motion
204
{
205
Motion
(
void
) :
state
(NULL),
control
(NULL),
steps
(0),
parent
(NULL)
206
{
207
}
208
210
Motion
(
const
SpaceInformation
*si) :
state
(si->allocState()),
control
(si->allocControl()),
steps
(0),
parent
(NULL)
211
{
212
}
213
214
~
Motion
(
void
)
215
{
216
}
217
219
base::State
*
state
;
220
222
Control
*
control
;
223
225
unsigned
int
steps
;
226
228
Motion
*
parent
;
229
};
230
232
struct
CellData
233
{
234
CellData
(
void
) :
coverage
(0.0),
selections
(1),
score
(1.0),
iteration
(0),
importance
(0.0)
235
{
236
}
237
238
~
CellData
(
void
)
239
{
240
}
241
243
std::vector<Motion*>
motions
;
244
248
double
coverage
;
249
252
unsigned
int
selections
;
253
257
double
score
;
258
260
unsigned
int
iteration
;
261
263
double
importance
;
264
};
265
268
struct
OrderCellsByImportance
269
{
270
bool
operator()(
const
CellData
*
const
a,
const
CellData
*
const
b)
const
271
{
272
return
a->
importance
> b->
importance
;
273
}
274
};
275
277
typedef
GridB<CellData*, OrderCellsByImportance>
Grid
;
278
280
struct
CloseSample
281
{
283
CloseSample
(
Grid::Cell
*c,
Motion
*m,
double
d) :
cell
(c),
motion
(m),
distance
(d)
284
{
285
}
286
288
Grid::Cell
*
cell
;
289
291
Motion
*
motion
;
292
294
double
distance
;
295
297
bool
operator<
(
const
CloseSample
&other)
const
298
{
299
return
distance
< other.
distance
;
300
}
301
};
302
304
struct
CloseSamples
305
{
307
CloseSamples
(
unsigned
int
size) :
maxSize
(size)
308
{
309
}
310
316
bool
consider
(
Grid::Cell
*cell,
Motion
*motion,
double
distance);
317
323
bool
selectMotion
(
Motion
* &smotion,
Grid::Cell
* &scell);
324
326
bool
canSample
(
void
)
const
327
{
328
return
samples
.size() > 0;
329
}
330
332
unsigned
int
maxSize
;
333
335
std::set<CloseSample>
samples
;
336
};
337
338
340
struct
TreeData
341
{
342
TreeData
(
void
) :
grid
(0),
size
(0),
iteration
(1)
343
{
344
}
345
348
Grid
grid
;
349
352
unsigned
int
size
;
353
355
unsigned
int
iteration
;
356
};
357
361
static
void
computeImportance
(
Grid::Cell
*cell,
void
*)
362
{
363
CellData
&cd = *(cell->data);
364
cd.
importance
= cd.
score
/ ((cell->neighbors + 1) * cd.
coverage
* cd.
selections
);
365
}
366
368
void
freeMemory
(
void
);
369
371
void
freeGridMotions
(
Grid
&grid);
372
374
void
freeCellData
(CellData *cdata);
375
377
void
freeMotion
(Motion *motion);
378
384
Grid::Cell
*
addMotion
(Motion* motion,
double
dist);
385
389
bool
selectMotion
(Motion* &smotion,
Grid::Cell
* &scell);
390
394
unsigned
int
findNextMotion
(
const
std::vector<Grid::Coord> &coords,
unsigned
int
index,
unsigned
int
count);
395
397
ControlSamplerPtr
controlSampler_
;
398
400
TreeData
tree_
;
401
403
const
SpaceInformation
*
siC_
;
404
408
base::ProjectionEvaluatorPtr
projectionEvaluator_
;
409
413
double
goodScoreFactor_
;
414
418
double
badScoreFactor_
;
419
423
unsigned
int
nCloseSamples_
;
424
427
double
selectBorderFraction_
;
428
430
double
goalBias_
;
431
433
RNG
rng_
;
434
436
Motion
*
lastGoalMotion_
;
437
};
438
439
}
440
}
441
442
#endif