IMP  2.3.1
The Integrative Modeling Platform
RRT.h
Go to the documentation of this file.
1 /**
2  * \file IMP/kinematics/RRT.h
3  * \brief simple RRT implementation
4  *
5  * \authors Dina Schneidman, Barak Raveh
6  *
7  * Copyright 2007-2014 IMP Inventors. All rights reserved.
8  *
9  */
10 
11 #ifndef IMPKINEMATICS_RR_T_H
12 #define IMPKINEMATICS_RR_T_H
13 
14 #include <IMP/kinematics/kinematics_config.h>
15 #include <IMP/kinematics/DOF.h>
18 
19 #include <IMP/Sampler.h>
20 #include <IMP/ConfigurationSet.h>
21 
22 IMPKINEMATICS_BEGIN_NAMESPACE
23 
24 // TODO: should move out of kinematics, or be in kinematic_algorithms
25 
26 /** A Simple implementation of the Rapidly-exploring Random Trees
27  algorithm
28 */
29 class IMPKINEMATICSEXPORT RRT : public IMP::Sampler {
30  public:
32 
33  // simple RRT node implementation
34  // we may replace it with something from boost so we can use boost graph
35  class RRTNode {
36  public:
37  RRTNode(const DOFValues& vec) : vec_(vec) {
38  id_ = node_counter_;
39  node_counter_++;
40  score_ = 0;
41  }
42 
43  // Accessors
44  const DOFValues& get_DOFValues() const { return vec_; }
45 
46  const std::vector<std::pair<const RRTNode*, float> >& get_edges() const {
47  return edges_;
48  }
49 
50  int get_id() const { return id_; }
51 
52  float get_score() const { return score_; }
53 
54  bool is_edge(const RRTNode* node) const {
55  for (unsigned int i = 0; i < edges_.size(); i++)
56  if (node == edges_[i].first) return true;
57  return false;
58  }
59 
60  // Modifiers
61  void add_edge(const RRTNode* node, float distance) {
62  if (id_ > node->id_) std::cerr << "wrong direction edge" << std::endl;
63  edges_.push_back(std::make_pair(node, distance));
64  }
65 
66  void set_score(float score) { score_ = score; }
67 
68  struct less_node {
69  bool operator()(const RRTNode* n1, const RRTNode* n2) {
70  return n1->get_score() < n2->get_score();
71  }
72  };
73 
74  friend std::ostream& operator<<(std::ostream& s, const RRTNode& n);
75 
76  private:
77  const DOFValues vec_;
78  // a node and a distance between two dofs
79  std::vector<std::pair<const RRTNode*, float> > edges_;
80  int id_;
81  float score_;
82  static int node_counter_;
83  };
84 
85  private:
86  // counters for stop condition...
87  class Parameters {
88  public:
89  Parameters()
90  : number_of_iterations_(0),
91  actual_tree_size_(0),
92  tree_size_(0),
93  number_of_collisions_(0) {}
94 
95  Parameters(unsigned int number_of_iterations,
96  unsigned int actual_tree_size = 100,
97  unsigned int tree_size = 100,
98  unsigned int number_of_collisions = 10000)
99  : number_of_iterations_(number_of_iterations),
100  actual_tree_size_(actual_tree_size),
101  tree_size_(tree_size),
102  number_of_collisions_(number_of_collisions) {}
103 
104  unsigned int number_of_iterations_;
105  unsigned int actual_tree_size_; // not including path nodes
106  unsigned int tree_size_; // all nodes
107  unsigned int number_of_collisions_;
108  };
109 
110  friend std::ostream& operator<<(std::ostream& s, const Parameters& p);
111 
112  public:
113  // Constructor
114  RRT(kernel::Model* m, DOFsSampler* sampler, LocalPlanner* planner,
115  const DOFs& cspace_dofs, unsigned int iteration_number = 1000,
116  unsigned int tree_size = 100);
117 
118  // function required by Sampler
119  // TODO: think how to save configurations in internal coords
120  // to be more memory efficient
121  IMP::ConfigurationSet* do_sample() const {
122  const_cast<RRT*>(this)->run();
123  return nullptr;
124  }
125 
126  void run();
127 
128  std::vector<DOFValues> get_DOFValues();
129 
130  /* Parameters for controlling RRT stop condition */
131 
132  // number of RRT iterations
133  void set_number_of_iterations(unsigned int num) {
134  default_parameters_.number_of_iterations_ = num;
135  }
136 
137  // tree size
138  void set_tree_size(unsigned int num) { default_parameters_.tree_size_ = num; }
139 
140  // actual tree size - not including path nodes
141  void set_actual_tree_size(unsigned int num) {
142  default_parameters_.actual_tree_size_ = num;
143  }
144 
145  // number of collisions
146  void set_number_of_collisions(unsigned int num) {
147  default_parameters_.number_of_collisions_ = num;
148  }
149 
150  private:
151  RRTNode* get_q_near(const DOFValues& q_rand) const;
152 
153  void add_nodes(RRTNode* q_near, const std::vector<DOFValues>& new_nodes);
154 
155  // TODO: make it more general
156  bool is_stop_condition(const Parameters& parameters,
157  const Parameters& counters) {
158  return (counters.number_of_iterations_ > parameters.number_of_iterations_ ||
159  counters.actual_tree_size_ > parameters.actual_tree_size_ ||
160  counters.tree_size_ > parameters.tree_size_ ||
161  counters.number_of_collisions_ > parameters.number_of_collisions_);
162  }
163 
164  private:
165  DOFsSampler* dofs_sampler_;
166  LocalPlanner* local_planner_;
167  typedef std::vector<RRTNode*> RRTTree;
168  RRTTree tree_;
169  DOFs cspace_dofs_; // configuration space dofs
170  Parameters default_parameters_; // limits for stop condition
171 };
172 
173 IMPKINEMATICS_END_NAMESPACE
174 
175 #endif /* IMPKINEMATICS_RR_T_H */
A class to store a set of configurations of a model.
a simple class for storage of DOF values.
single degree of freedom
#define IMP_OBJECT_METHODS(Name)
Define the basic things needed by any Object.
Definition: object_macros.h:25
IntPairs get_edges(const BoundingBoxD< 3 > &)
Return the edges of the box as indices into the vertices list.
Definition: BoundingBoxD.h:289
IMP::kernel::Model Model
Import IMP/kernel/Sampler.h in the namespace.
Base class for all samplers.
Import IMP/kernel/ConfigurationSet.h in the namespace.
IMP::base::Vector< IMP::base::Pointer< DOF > > DOFs
Definition: DOF.h:70