11 #ifndef IMPKINEMATICS_RR_T_H
12 #define IMPKINEMATICS_RR_T_H
14 #include <IMP/kinematics/kinematics_config.h>
24 IMPKINEMATICS_BEGIN_NAMESPACE
44 const DOFValues& get_DOFValues()
const {
return vec_; }
46 const std::vector<std::pair<const RRTNode*, float> >&
get_edges()
const {
50 int get_id()
const {
return id_; }
52 float get_score()
const {
return score_; }
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;
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));
66 void set_score(
float score) { score_ = score; }
70 return n1->get_score() < n2->get_score();
74 friend std::ostream& operator<<(std::ostream& s,
const RRTNode& n);
79 std::vector<std::pair<const RRTNode*, float> > edges_;
82 static int node_counter_;
90 : number_of_iterations_(0),
93 number_of_collisions_(0) {}
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) {}
104 unsigned int number_of_iterations_;
105 unsigned int actual_tree_size_;
106 unsigned int tree_size_;
107 unsigned int number_of_collisions_;
111 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);
141 default_parameters_.number_of_iterations_ = num;
145 void set_tree_size(
unsigned int num) { default_parameters_.tree_size_ = num; }
149 default_parameters_.actual_tree_size_ = num;
154 default_parameters_.number_of_collisions_ = num;
160 RRTNode* get_q_near(
const DOFValues& q_rand)
const;
164 void add_nodes(RRTNode* q_near,
const std::vector<DOFValues>& new_nodes);
167 bool is_stop_condition(
const Parameters& parameters,
168 const Parameters& counters) {
169 return (counters.number_of_iterations_ > parameters.number_of_iterations_ ||
170 counters.actual_tree_size_ > parameters.actual_tree_size_ ||
171 counters.tree_size_ > parameters.tree_size_ ||
172 counters.number_of_collisions_ > parameters.number_of_collisions_);
178 typedef std::shared_ptr<RRTNode> RRTNodePtr;
179 typedef std::vector<RRTNodePtr> RRTTree;
182 Parameters default_parameters_;
183 Parameters current_counters_;
186 unsigned int number_of_sampled_dofs_;
187 std::vector<bool> active_dofs_;
190 IMPKINEMATICS_END_NAMESPACE
void set_actual_tree_size(unsigned int num)
Set the actual tree size - not including path nodes.
Simple RRT node implementation.
Simple implementation of the Rapidly-exploring Random Trees algorithm.
a simple class for storage of DOF values.
IMP::Vector< IMP::Pointer< DOF > > DOFs
void set_number_of_iterations(unsigned int num)
Set number of RRT iterations.
#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.
IMP::Vector< DOFValues > DOFValuesList
Base class for all samplers.
A class that holds DOF values for DOFs.
A smart pointer to a ref-counted Object that is a class member.
A class to store a set of configurations of a model.
void set_number_of_collisions(unsigned int num)
Set the number of collisions.
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.