IMP logo
IMP Reference Guide  2.11.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-2019 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 //! Simple implementation of the Rapidly-exploring Random Trees algorithm
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(Model* m, DOFsSampler* sampler, LocalPlanner* planner,
115  const DOFs& cspace_dofs, unsigned int iteration_number = 1000,
116  unsigned int tree_size = 100,
117  unsigned int number_of_sampled_dofs = 0); // 0 for all dofs
118 
119  // function required by Sampler
120  // TODO: think how to save configurations in internal coords
121  // to be more memory efficient
122  IMP::ConfigurationSet* do_sample() const {
123  const_cast<RRT*>(this)->run();
124  return nullptr;
125  }
126 
127  //! Run the specified number of iterations
128  /** If zero, will run for default iteration number.
129  This option allows to output the already sampled
130  configurations during the sampling run. */
131  bool run(unsigned int number_of_iterations = 0);
132 
133  std::vector<DOFValues> get_DOFValues();
134 
135  /* Parameters for controlling RRT stop condition */
136 
137  //! Set number of RRT iterations
138  void set_number_of_iterations(unsigned int num) {
139  default_parameters_.number_of_iterations_ = num;
140  }
141 
142  //! Set tree size
143  void set_tree_size(unsigned int num) { default_parameters_.tree_size_ = num; }
144 
145  //! Set the actual tree size - not including path nodes
146  void set_actual_tree_size(unsigned int num) {
147  default_parameters_.actual_tree_size_ = num;
148  }
149 
150  //! Set the number of collisions
151  void set_number_of_collisions(unsigned int num) {
152  default_parameters_.number_of_collisions_ = num;
153  }
154 
155  private:
156  RRTNode* get_q_near(const DOFValues& q_rand) const;
157 
158  void check_initial_configuration(ScoringFunction *sf) const;
159 
160  void add_nodes(RRTNode* q_near, const std::vector<DOFValues>& new_nodes);
161 
162  // TODO: make it more general
163  bool is_stop_condition(const Parameters& parameters,
164  const Parameters& counters) {
165  return (counters.number_of_iterations_ > parameters.number_of_iterations_ ||
166  counters.actual_tree_size_ > parameters.actual_tree_size_ ||
167  counters.tree_size_ > parameters.tree_size_ ||
168  counters.number_of_collisions_ > parameters.number_of_collisions_);
169  }
170 
171  private:
172  PointerMember<DOFsSampler> dofs_sampler_;
173  PointerMember<LocalPlanner> local_planner_;
174  typedef boost::shared_ptr<RRTNode> RRTNodePtr;
175  typedef std::vector<RRTNodePtr> RRTTree;
176  RRTTree tree_;
177  DOFs cspace_dofs_; // configuration space dofs
178  Parameters default_parameters_; // limits for stop condition
179  Parameters current_counters_;
180 
181  // for sampling subsets of dofs
182  unsigned int number_of_sampled_dofs_;
183  std::vector<bool> active_dofs_;
184 };
185 
186 IMPKINEMATICS_END_NAMESPACE
187 
188 #endif /* IMPKINEMATICS_RR_T_H */
void set_actual_tree_size(unsigned int num)
Set the actual tree size - not including path nodes.
Definition: RRT.h:146
Simple RRT node implementation.
Definition: RRT.h:35
Simple implementation of the Rapidly-exploring Random Trees algorithm.
Definition: RRT.h:29
a simple class for storage of DOF values.
single degree of freedom
IMP::Vector< IMP::Pointer< DOF > > DOFs
Definition: DOF.h:71
void set_number_of_iterations(unsigned int num)
Set number of RRT iterations.
Definition: RRT.h:138
#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 that holds DOF values for DOFs.
Definition: DOFValues.h:20
A smart pointer to a ref-counted Object that is a class member.
Definition: Pointer.h:146
A class to store a set of configurations of a model.
void set_number_of_collisions(unsigned int num)
Set the number of collisions.
Definition: RRT.h:151
Represents a scoring function on the model.
Store a set of configurations of the model.
void set_tree_size(unsigned int num)
Set tree size.
Definition: RRT.h:143