LEMUR Packages: ompl_lemur or_lemur pr_bgl prpy_lemur
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Groups Pages
RoadmapHalton.h
Go to the documentation of this file.
1 
7 namespace ompl_lemur
8 {
9 
10 template <class RoadmapArgs>
11 class RoadmapHalton : public Roadmap<RoadmapArgs>
12 {
13  typedef boost::graph_traits<typename RoadmapArgs::Graph> GraphTypes;
14  typedef typename GraphTypes::vertex_descriptor Vertex;
15  typedef typename GraphTypes::edge_descriptor Edge;
16 
17  // set on construction
18  unsigned int _dim;
20 
21  // params
22  unsigned int _num;
23  double _radius;
24 
25 public:
26  RoadmapHalton(RoadmapArgs & args):
27  Roadmap<RoadmapArgs>(args, "Halton", 1),
28  _dim(0),
29  _bounds(0),
30  _num(0),
31  _radius(0.0)
32  {
33  // check that we're in a real vector state space
34  if (this->space->getType() != ompl::base::STATE_SPACE_REAL_VECTOR)
35  throw std::runtime_error("RoadmapHalton only supports rel vector state spaces!");
36  _dim = this->space->getDimension();
37  if (0 == ompl_lemur::util::get_prime(_dim-1))
38  throw std::runtime_error("not enough primes hardcoded!");
39  ompl::base::StateSpacePtr myspace(this->space);
40  _bounds = myspace->as<ompl::base::RealVectorStateSpace>()->getBounds();
41 
42  this->template declareParam<unsigned int>("num", this,
45  this->template declareParam<double>("radius", this,
46  &RoadmapHalton::setRadius,
47  &RoadmapHalton::getRadius);
48  }
49 
50  void setNum(unsigned int num)
51  {
52  if (num == _num)
53  return;
54  if (this->initialized)
55  throw std::runtime_error("cannot set num, already initialized!");
56  _num = num;
57  }
58 
59  unsigned int getNum() const
60  {
61  return _num;
62  }
63 
64  void setRadius(double radius)
65  {
66  if (radius == _radius)
67  return;
68  if (this->initialized)
69  throw std::runtime_error("cannot set radius, already initialized!");
70  _radius = radius;
71  }
72 
73  double getRadius() const
74  {
75  return _radius;
76  }
77 
78  void initialize()
79  {
80  std::vector<std::string> missings;
81  if (_num == 0)
82  missings.push_back("num");
83  if (_radius == 0.0)
84  missings.push_back("radius");
85  if (missings.size())
86  {
87  std::string str = "Cannot initialize, parameters not set:";
88  for (unsigned int ui=0; ui<missings.size(); ui++)
89  str += " " + missings[ui];
90  throw std::runtime_error(str);
91  }
92  this->initialized = true;
93  }
94 
95  void deserialize(const std::string & ser_data)
96  {
97  throw std::runtime_error("RoadmapHalton deserialize from ser_data not supported!");
98  }
99 
100  // should be stateless
101  double root_radius(std::size_t i_batch)
102  {
103  return _radius;
104  }
105 
106  // sets all of these maps
107  // generates one additional batch
108  void generate()
109  {
110  if (this->max_batches < this->num_batches_generated + 1)
111  throw std::runtime_error("this roadmap gen doesnt support that many batches!");
112  for (std::size_t v_index=num_vertices(this->g); v_index<_num; v_index++)
113  {
114  Vertex v_new = add_vertex(this->g);
115 
116  put(this->vertex_batch_map, v_new, this->num_batches_generated);
117  put(this->is_shadow_map, v_new, false);
118 
119  // allocate a new state for this vertex
120  put(this->state_map, v_new, this->space->allocState());
121  ompl::base::State * v_state = get(this->state_map, v_new);
122  double * values = v_state->as<ompl::base::RealVectorStateSpace::StateType>()->values;
123  for (unsigned int ui=0; ui<_dim; ui++)
124  {
125  values[ui] = _bounds.low[ui] + (_bounds.high[ui] - _bounds.low[ui])
126  * ompl_lemur::util::halton(
127  ompl_lemur::util::get_prime(ui), v_index);
128  }
129  this->nn->add(v_new);
130 
131  // allocate new undirected edges
132  std::vector<Vertex> vs_near;
133  this->nn->nearestR(v_new, _radius, vs_near);
134  for (unsigned int ui=0; ui<vs_near.size(); ui++)
135  {
136  if (vs_near[ui] == v_new)
137  continue;
138  Edge e = add_edge(v_new, vs_near[ui], this->g).first;
139  ompl::base::State * vnear_state = get(this->state_map,vs_near[ui]);
140  put(this->distance_map, e, this->space->distance(v_state,vnear_state));
141  put(this->edge_batch_map, e, this->num_batches_generated);
142  }
143  }
144  this->num_batches_generated++;
145  }
146 
147  void serialize(std::string & ser_data)
148  {
149  throw std::runtime_error("RoadmapHalton serialize to ser_data not supported!");
150  }
151 };
152 
153 } // namespace ompl_lemur
double root_radius(std::size_t i_batch)
Calcuate the root radius to be used for connecting to potential root vertices.
Definition: RoadmapHalton.h:101
std::vector< double > low
void generate()
Generates one additional batch.
Definition: RoadmapHalton.h:108
Interface for generating roadmaps over OMPL state spaces into Boost Graph objects.
Definition: Roadmap.h:76
void serialize(std::string &ser_data)
Serialize the internal generator state into the passed string.
Definition: RoadmapHalton.h:147
std::vector< double > high
void initialize()
Initialize roadmap; must be called once after setting parameters.
Definition: RoadmapHalton.h:78
void deserialize(const std::string &ser_data)
Re-constitute the internal generator state from serialized data.
Definition: RoadmapHalton.h:95
const T * as() const
Definition: Roadmap.h:11
Definition: RoadmapHalton.h:11