IMP logo
IMP Reference Guide  2.6.0
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-2016 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 #include <boost/shared_ptr.hpp>
23 
24 IMPKINEMATICS_BEGIN_NAMESPACE
25 
26 // TODO: should move out of kinematics, or be in kinematic_algorithms
27 
28 /** A Simple implementation of the Rapidly-exploring Random Trees
29  algorithm
30 */
31 class IMPKINEMATICSEXPORT RRT : public IMP::Sampler {
32  public:
34 
35  // simple RRT node implementation
36  // we may replace it with something from boost so we can use boost graph
37  class RRTNode {
38  public:
39  RRTNode(const DOFValues& vec) : vec_(vec) {
40  id_ = node_counter_;
41  node_counter_++;
42  score_ = 0;
43  }
44 
45  // Accessors
46  const DOFValues& get_DOFValues() const { return vec_; }
47 
48  const std::vector<std::pair<const RRTNode*, float> >& get_edges() const {
49  return edges_;
50  }
51 
52  int get_id() const { return id_; }
53 
54  float get_score() const { return score_; }
55 
56  bool is_edge(const RRTNode* node) const {
57  for (unsigned int i = 0; i < edges_.size(); i++)
58  if (node == edges_[i].first) return true;
59  return false;
60  }
61 
62  // Modifiers
63  void add_edge(const RRTNode* node, float distance) {
64  if (id_ > node->id_) std::cerr << "wrong direction edge" << std::endl;
65  edges_.push_back(std::make_pair(node, distance));
66  }
67 
68  void set_score(float score) { score_ = score; }
69 
70  struct less_node {
71  bool operator()(const RRTNode* n1, const RRTNode* n2) {
72  return n1->get_score() < n2->get_score();
73  }
74  };
75 
76  friend std::ostream& operator<<(std::ostream& s, const RRTNode& n);
77 
78  private:
79  const DOFValues vec_;
80  // a node and a distance between two dofs
81  std::vector<std::pair<const RRTNode*, float> > edges_;
82  int id_;
83  float score_;
84  static int node_counter_;
85  };
86 
87  private:
88  // counters for stop condition...
89  class Parameters {
90  public:
91  Parameters()
92  : number_of_iterations_(0),
93  actual_tree_size_(0),
94  tree_size_(0),
95  number_of_collisions_(0) {}
96 
97  Parameters(unsigned int number_of_iterations,
98  unsigned int actual_tree_size = 100,
99  unsigned int tree_size = 100,
100  unsigned int number_of_collisions = 10000)
101  : number_of_iterations_(number_of_iterations),
102  actual_tree_size_(actual_tree_size),
103  tree_size_(tree_size),
104  number_of_collisions_(number_of_collisions) {}
105 
106  unsigned int number_of_iterations_;
107  unsigned int actual_tree_size_; // not including path nodes
108  unsigned int tree_size_; // all nodes
109  unsigned int number_of_collisions_;
110  };
111 
112  friend std::ostream& operator<<(std::ostream& s, const Parameters& p);
113 
114  public:
115  // Constructor
116  RRT(Model* m, DOFsSampler* sampler, LocalPlanner* planner,
117  const DOFs& cspace_dofs, unsigned int iteration_number = 1000,
118  unsigned int tree_size = 100,
119  unsigned int number_of_sampled_dofs = 0); // 0 for all dofs
120 
121  // function required by Sampler
122  // TODO: think how to save configurations in internal coords
123  // to be more memory efficient
124  IMP::ConfigurationSet* do_sample() const {
125  const_cast<RRT*>(this)->run();
126  return nullptr;
127  }
128 
129  // run the specified number of iterations
130  // if zero, will run for default iteration number
131  // this option allows to output the already sampled
132  // configurations during the sampling run
133  bool run(unsigned int number_of_iterations = 0);
134 
135  std::vector<DOFValues> get_DOFValues();
136 
137  /* Parameters for controlling RRT stop condition */
138 
139  // number of RRT iterations
140  void set_number_of_iterations(unsigned int num) {
141  default_parameters_.number_of_iterations_ = num;
142  }
143 
144  // tree size
145  void set_tree_size(unsigned int num) { default_parameters_.tree_size_ = num; }
146 
147  // actual tree size - not including path nodes
148  void set_actual_tree_size(unsigned int num) {
149  default_parameters_.actual_tree_size_ = num;
150  }
151 
152  // number of collisions
153  void set_number_of_collisions(unsigned int num) {
154  default_parameters_.number_of_collisions_ = num;
155  }
156 
157  private:
158  RRTNode* get_q_near(const DOFValues& q_rand) const;
159 
160  void check_initial_configuration(ScoringFunction *sf) const;
161 
162  void add_nodes(RRTNode* q_near, const std::vector<DOFValues>& new_nodes);
163 
164  // TODO: make it more general
165  bool is_stop_condition(const Parameters& parameters,
166  const Parameters& counters) {
167  return (counters.number_of_iterations_ > parameters.number_of_iterations_ ||
168  counters.actual_tree_size_ > parameters.actual_tree_size_ ||
169  counters.tree_size_ > parameters.tree_size_ ||
170  counters.number_of_collisions_ > parameters.number_of_collisions_);
171  }
172 
173  private:
174  DOFsSampler* dofs_sampler_;
175  LocalPlanner* local_planner_;
176  typedef boost::shared_ptr<RRTNode> RRTNodePtr;
177  typedef std::vector<RRTNodePtr> RRTTree;
178  RRTTree tree_;
179  DOFs cspace_dofs_; // configuration space dofs
180  Parameters default_parameters_; // limits for stop condition
181  Parameters current_counters_;
182 
183  // for sampling subsets of dofs
184  unsigned int number_of_sampled_dofs_;
185  std::vector<bool> active_dofs_;
186 };
187 
188 IMPKINEMATICS_END_NAMESPACE
189 
190 #endif /* IMPKINEMATICS_RR_T_H */
a simple class for storage of DOF values.
single degree of freedom
IMP::Vector< IMP::Pointer< DOF > > DOFs
Definition: DOF.h:70
#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:307
Base class for all samplers.
Definition: Sampler.h:31
Base class for all samplers.
A class to store a set of configurations of a model.
Store a set of configurations of the model.