11 #ifndef IMPKINEMATICS_RR_T_H
12 #define IMPKINEMATICS_RR_T_H
14 #include <IMP/kinematics/kinematics_config.h>
22 #include <boost/shared_ptr.hpp>
24 IMPKINEMATICS_BEGIN_NAMESPACE
39 RRTNode(
const DOFValues& vec) : vec_(vec) {
46 const DOFValues& get_DOFValues()
const {
return vec_; }
48 const std::vector<std::pair<const RRTNode*, float> >&
get_edges()
const {
52 int get_id()
const {
return id_; }
54 float get_score()
const {
return score_; }
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;
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));
68 void set_score(
float score) { score_ = score; }
71 bool operator()(
const RRTNode* n1,
const RRTNode* n2) {
72 return n1->get_score() < n2->get_score();
76 friend std::ostream& operator<<(std::ostream& s,
const RRTNode& n);
81 std::vector<std::pair<const RRTNode*, float> > edges_;
84 static int node_counter_;
92 : number_of_iterations_(0),
95 number_of_collisions_(0) {}
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) {}
106 unsigned int number_of_iterations_;
107 unsigned int actual_tree_size_;
108 unsigned int tree_size_;
109 unsigned int number_of_collisions_;
112 friend std::ostream& operator<<(std::ostream& s,
const Parameters& p);
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);
125 const_cast<RRT*
>(
this)->run();
133 bool run(
unsigned int number_of_iterations = 0);
135 std::vector<DOFValues> get_DOFValues();
140 void set_number_of_iterations(
unsigned int num) {
141 default_parameters_.number_of_iterations_ = num;
145 void set_tree_size(
unsigned int num) { default_parameters_.tree_size_ = num; }
148 void set_actual_tree_size(
unsigned int num) {
149 default_parameters_.actual_tree_size_ = num;
153 void set_number_of_collisions(
unsigned int num) {
154 default_parameters_.number_of_collisions_ = num;
158 RRTNode* get_q_near(
const DOFValues& q_rand)
const;
160 void check_initial_configuration(ScoringFunction *sf)
const;
162 void add_nodes(RRTNode* q_near,
const std::vector<DOFValues>& new_nodes);
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_);
174 DOFsSampler* dofs_sampler_;
175 LocalPlanner* local_planner_;
176 typedef boost::shared_ptr<RRTNode> RRTNodePtr;
177 typedef std::vector<RRTNodePtr> RRTTree;
180 Parameters default_parameters_;
181 Parameters current_counters_;
184 unsigned int number_of_sampled_dofs_;
185 std::vector<bool> active_dofs_;
188 IMPKINEMATICS_END_NAMESPACE
a simple class for storage of DOF values.
IMP::Vector< IMP::Pointer< DOF > > DOFs
#define IMP_OBJECT_METHODS(Name)
Define the basic things needed by any Object.
IntPairs get_edges(const BoundingBoxD< 3 > &)
Return the edges of the box as indices into the vertices list.
Base class for all samplers.
Base class for all samplers.
A class to store a set of configurations of a model.
Store a set of configurations of the model.