diff --git a/CMakeLists.txt b/CMakeLists.txt index 2504d9e9654119a3a35c0109a3afbd5dbc952a58..29306c552d3ae134f3d0a3862de83409f0a495c7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -81,7 +81,7 @@ SET (${PROJECT_NAME}_HEADERS include/hpp/manipulation/roadmap.hh include/hpp/manipulation/roadmap-node.hh include/hpp/manipulation/connected-component.hh - include/hpp/manipulation/symbolic-component.hh + include/hpp/manipulation/leaf-connected-comp.hh include/hpp/manipulation/symbolic-planner.hh include/hpp/manipulation/manipulation-planner.hh include/hpp/manipulation/graph-path-validation.hh diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh index 16cb9a31c44c3f8532cfa8e03fa2367a98e1dd44..d0b5de0ddafd618cc7ab57268e39b3fb64d075d6 100644 --- a/include/hpp/manipulation/fwd.hh +++ b/include/hpp/manipulation/fwd.hh @@ -60,11 +60,11 @@ namespace hpp { typedef std::vector<RoadmapNodePtr_t> RoadmapNodes_t; HPP_PREDEF_CLASS (ConnectedComponent); typedef boost::shared_ptr<ConnectedComponent> ConnectedComponentPtr_t; - HPP_PREDEF_CLASS (SymbolicComponent); - typedef boost::shared_ptr<SymbolicComponent> SymbolicComponentPtr_t; - typedef std::set<SymbolicComponentPtr_t> SymbolicComponents_t; - HPP_PREDEF_CLASS (WeighedSymbolicComponent); - typedef boost::shared_ptr<WeighedSymbolicComponent> WeighedSymbolicComponentPtr_t; + HPP_PREDEF_CLASS (LeafConnectedComp); + typedef boost::shared_ptr<LeafConnectedComp> LeafConnectedCompPtr_t; + typedef std::set<LeafConnectedCompPtr_t> LeafConnectedComps_t; + HPP_PREDEF_CLASS (WeighedLeafConnectedComp); + typedef boost::shared_ptr<WeighedLeafConnectedComp> WeighedLeafConnectedCompPtr_t; HPP_PREDEF_CLASS (WeighedDistance); typedef boost::shared_ptr<WeighedDistance> WeighedDistancePtr_t; typedef constraints::RelativeOrientation RelativeOrientation; diff --git a/include/hpp/manipulation/symbolic-component.hh b/include/hpp/manipulation/leaf-connected-comp.hh similarity index 71% rename from include/hpp/manipulation/symbolic-component.hh rename to include/hpp/manipulation/leaf-connected-comp.hh index 0648b0818ee86093159773a56cf499587b79de98..b3f932e320224241f9f45d3c129711e83e4f08b3 100644 --- a/include/hpp/manipulation/symbolic-component.hh +++ b/include/hpp/manipulation/leaf-connected-comp.hh @@ -16,8 +16,8 @@ // hpp-manipulation If not, see // <http://www.gnu.org/licenses/>. -#ifndef HPP_MANIPULATION_SYMBOLIC_COMPONENT_HH -#define HPP_MANIPULATION_SYMBOLIC_COMPONENT_HH +#ifndef HPP_MANIPULATION_LEAF_CONNECTED_COMP_HH +#define HPP_MANIPULATION_LEAF_CONNECTED_COMP_HH #include <hpp/manipulation/config.hh> #include <hpp/manipulation/fwd.hh> @@ -27,29 +27,29 @@ namespace hpp { namespace manipulation { - /// Set of configurations accessible to each others by a single edge, + /// Set of configurations accessible to each others by a single transition, /// with the same right hand side. /// /// This assumes the roadmap is not directed. - class HPP_MANIPULATION_DLLAPI SymbolicComponent + class HPP_MANIPULATION_DLLAPI LeafConnectedComp { public: - typedef std::set<SymbolicComponent*> SymbolicComponents_t; + typedef std::set<LeafConnectedComp*> LeafConnectedComps_t; /// return a shared pointer to new instance - static SymbolicComponentPtr_t create (const RoadmapPtr_t& roadmap); + static LeafConnectedCompPtr_t create (const RoadmapPtr_t& roadmap); /// Merge two symbolic components /// \param other manipulation symbolic component to merge into this one. /// \note other will be empty after calling this method. - virtual void merge (SymbolicComponentPtr_t otherCC); + virtual void merge (LeafConnectedCompPtr_t otherCC); - bool canMerge (const SymbolicComponentPtr_t& otherCC) const; + bool canMerge (const LeafConnectedCompPtr_t& otherCC) const; /// Add otherCC to the list of reachable components /// /// This also add this object to the list of ancestor of otherCC. - void canReach (const SymbolicComponentPtr_t& otherCC); + void canReach (const LeafConnectedCompPtr_t& otherCC); /// Add roadmap node to connected component /// \param roadmap node to be added @@ -69,10 +69,10 @@ namespace hpp { } protected: - SymbolicComponent(const RoadmapPtr_t& r) + LeafConnectedComp(const RoadmapPtr_t& r) : roadmap_(r) {} - void init (const SymbolicComponentWkPtr_t& shPtr) + void init (const LeafConnectedCompWkPtr_t& shPtr) { weak_ = shPtr; } @@ -82,19 +82,19 @@ namespace hpp { private: RoadmapWkPtr_t roadmap_; - SymbolicComponents_t to_, from_; - SymbolicComponentWkPtr_t weak_; - }; // class SymbolicComponent + LeafConnectedComps_t to_, from_; + LeafConnectedCompWkPtr_t weak_; + }; // class LeafConnectedComp - class HPP_MANIPULATION_DLLAPI WeighedSymbolicComponent : - public SymbolicComponent + class HPP_MANIPULATION_DLLAPI WeighedLeafConnectedComp : + public LeafConnectedComp { public: - void merge (SymbolicComponentPtr_t otherCC); + void merge (LeafConnectedCompPtr_t otherCC); void setFirstNode (const RoadmapNodePtr_t& node); - static WeighedSymbolicComponentPtr_t create (const RoadmapPtr_t& roadmap); + static WeighedLeafConnectedCompPtr_t create (const RoadmapPtr_t& roadmap); std::size_t indexOf (const graph::EdgePtr_t e) const; @@ -110,11 +110,11 @@ namespace hpp { std::vector<graph::EdgePtr_t> edges_; protected: - WeighedSymbolicComponent(const RoadmapPtr_t& r) - : SymbolicComponent(r), weight_(1) {} + WeighedLeafConnectedComp(const RoadmapPtr_t& r) + : LeafConnectedComp(r), weight_(1) {} private: - }; // class SymbolicComponent + }; // class LeafConnectedComp } // namespace manipulation } // namespace hpp -#endif // HPP_MANIPULATION_SYMBOLIC_COMPONENT_HH +#endif // HPP_MANIPULATION_LEAF_CONNECTED_COMP_HH diff --git a/include/hpp/manipulation/roadmap-node.hh b/include/hpp/manipulation/roadmap-node.hh index 439677d58b9343d5fd39a278f58c6981e61070e3..e55a92b31ffd26c7bc6be47c17f77ac1d81732ac 100644 --- a/include/hpp/manipulation/roadmap-node.hh +++ b/include/hpp/manipulation/roadmap-node.hh @@ -92,12 +92,12 @@ namespace hpp { } /// \} - void symbolicComponent (const SymbolicComponentPtr_t& sc) + void symbolicComponent (const LeafConnectedCompPtr_t& sc) { symbolicCC_ = sc; } - SymbolicComponentPtr_t symbolicComponent () const + LeafConnectedCompPtr_t symbolicComponent () const { return symbolicCC_; } @@ -106,7 +106,7 @@ namespace hpp { CachingSystem cacheSystem_; graph::StateWkPtr_t state_; - SymbolicComponentPtr_t symbolicCC_; + LeafConnectedCompPtr_t symbolicCC_; }; } // namespace manipulation } // namespace hpp diff --git a/include/hpp/manipulation/roadmap.hh b/include/hpp/manipulation/roadmap.hh index 685b1c1aa0b37eaa7e3fd5fd5597af6f534c5ea3..56de8879e3565d1f0e5bcc4fdb7e95eb0a9c9304 100644 --- a/include/hpp/manipulation/roadmap.hh +++ b/include/hpp/manipulation/roadmap.hh @@ -67,7 +67,7 @@ namespace hpp { graph::StatePtr_t getState(RoadmapNodePtr_t node); /// Get the symbolic components - const SymbolicComponents_t& symbolicComponents () const + const LeafConnectedComps_t& symbolicComponents () const { return symbolicCCs_; } @@ -97,7 +97,7 @@ namespace hpp { Histograms_t histograms_; graph::GraphPtr_t graph_; RoadmapWkPtr_t weak_; - SymbolicComponents_t symbolicCCs_; + LeafConnectedComps_t symbolicCCs_; }; /// \} } // namespace manipulation diff --git a/include/hpp/manipulation/symbolic-planner.hh b/include/hpp/manipulation/symbolic-planner.hh index 3c0211649039c2a0bcac170949b8b303ddc0892c..2078d17c096188a01d6bef91603601757f358b64 100644 --- a/include/hpp/manipulation/symbolic-planner.hh +++ b/include/hpp/manipulation/symbolic-planner.hh @@ -104,11 +104,11 @@ namespace hpp { /// \param alpha should be between 0 and 1. /// /// \note Theoretically, the described operation preserves the - /// normalization of the sum. WeighedSymbolicComponent::normalizeProba + /// normalization of the sum. WeighedLeafConnectedComp::normalizeProba /// is called to avoid numerical errors. static void updateEdgeProba ( const graph::EdgePtr_t edge, - WeighedSymbolicComponentPtr_t wsc, + WeighedLeafConnectedCompPtr_t wsc, const value_type alpha); /// Configuration shooter diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ecb8644bfc75c921001c9f865d9beb7ffdcc2381..445be2a074c546d95fd70b07aa74a5c4429a4401 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -27,7 +27,7 @@ SET(SOURCES problem-solver.cc roadmap.cc connected-component.cc - symbolic-component.cc + leaf-connected-comp.cc constraint-set.cc roadmap-node.cc device.cc diff --git a/src/symbolic-component.cc b/src/leaf-connected-comp.cc similarity index 72% rename from src/symbolic-component.cc rename to src/leaf-connected-comp.cc index 3eb5ed2028c227ceb12be9eca823dbd64af02936..e7204bc8404861c783cb546670730c83d27cb757 100644 --- a/src/symbolic-component.cc +++ b/src/leaf-connected-comp.cc @@ -14,44 +14,44 @@ // received a copy of the GNU Lesser General Public License along with // hpp-manipulation. If not, see <http://www.gnu.org/licenses/>. -#include <hpp/manipulation/symbolic-component.hh> +#include <hpp/manipulation/leaf-connected-comp.hh> #include <hpp/manipulation/roadmap.hh> #include <hpp/manipulation/graph/state.hh> namespace hpp { namespace manipulation { - SymbolicComponentPtr_t SymbolicComponent::create (const RoadmapPtr_t& roadmap) + LeafConnectedCompPtr_t LeafConnectedComp::create (const RoadmapPtr_t& roadmap) { - SymbolicComponentPtr_t shPtr = - SymbolicComponentPtr_t(new SymbolicComponent(roadmap)); + LeafConnectedCompPtr_t shPtr = + LeafConnectedCompPtr_t(new LeafConnectedComp(roadmap)); shPtr->init(shPtr); return shPtr; } - void SymbolicComponent::addNode (const RoadmapNodePtr_t& node) + void LeafConnectedComp::addNode (const RoadmapNodePtr_t& node) { assert(state_); graph::StatePtr_t state = roadmap_.lock()->getState(node); // Sanity check if (state_ == state) // Oops - throw std::runtime_error ("RoadmapNode of SymbolicComponent must be in" + throw std::runtime_error ("RoadmapNode of LeafConnectedComp must be in" " the same state"); nodes_.push_back(node); } - void SymbolicComponent::setFirstNode (const RoadmapNodePtr_t& node) + void LeafConnectedComp::setFirstNode (const RoadmapNodePtr_t& node) { assert(!state_); state_ = roadmap_.lock()->getState(node); nodes_.push_back(node); } - bool SymbolicComponent::canMerge (const SymbolicComponentPtr_t& otherCC) const + bool LeafConnectedComp::canMerge (const LeafConnectedCompPtr_t& otherCC) const { if (otherCC->state_ != state_) return false; - SymbolicComponents_t::const_iterator it = std::find + LeafConnectedComps_t::const_iterator it = std::find (to_.begin(), to_.end(), otherCC.get()); if (it == to_.end()) return false; it = std::find @@ -60,13 +60,13 @@ namespace hpp { return true; } - void SymbolicComponent::canReach (const SymbolicComponentPtr_t& otherCC) + void LeafConnectedComp::canReach (const LeafConnectedCompPtr_t& otherCC) { to_.insert(otherCC.get()); otherCC->from_.insert(this); } - void SymbolicComponent::merge (SymbolicComponentPtr_t other) + void LeafConnectedComp::merge (LeafConnectedCompPtr_t other) { assert (canMerge(other)); if (other == weak_.lock()) return; @@ -87,28 +87,28 @@ namespace hpp { to_.insert (other->to_.begin(), other->to_.end()); } - WeighedSymbolicComponentPtr_t WeighedSymbolicComponent::create (const RoadmapPtr_t& roadmap) + WeighedLeafConnectedCompPtr_t WeighedLeafConnectedComp::create (const RoadmapPtr_t& roadmap) { - WeighedSymbolicComponentPtr_t shPtr = WeighedSymbolicComponentPtr_t - (new WeighedSymbolicComponent(roadmap)); + WeighedLeafConnectedCompPtr_t shPtr = WeighedLeafConnectedCompPtr_t + (new WeighedLeafConnectedComp(roadmap)); shPtr->init(shPtr); return shPtr; } - void WeighedSymbolicComponent::merge (SymbolicComponentPtr_t otherCC) + void WeighedLeafConnectedComp::merge (LeafConnectedCompPtr_t otherCC) { - WeighedSymbolicComponentPtr_t other = - HPP_DYNAMIC_PTR_CAST(WeighedSymbolicComponent, otherCC); + WeighedLeafConnectedCompPtr_t other = + HPP_DYNAMIC_PTR_CAST(WeighedLeafConnectedComp, otherCC); value_type r = ((value_type)nodes_.size()) / (value_type)(nodes_.size() + other->nodes_.size()); - SymbolicComponent::merge(otherCC); + LeafConnectedComp::merge(otherCC); weight_ *= other->weight_; // TODO a geometric mean would be more natural. p_ = r * p_ + (1 - r) * other->p_; } - void WeighedSymbolicComponent::setFirstNode (const RoadmapNodePtr_t& node) + void WeighedLeafConnectedComp::setFirstNode (const RoadmapNodePtr_t& node) { - SymbolicComponent::setFirstNode(node); + LeafConnectedComp::setFirstNode(node); std::vector<value_type> p = state_->neighbors().probabilities(); p_.resize(p.size()); edges_ = state_->neighbors().values(); @@ -116,7 +116,7 @@ namespace hpp { p_[i] = p[i]; } - std::size_t WeighedSymbolicComponent::indexOf (const graph::EdgePtr_t e) const + std::size_t WeighedLeafConnectedComp::indexOf (const graph::EdgePtr_t e) const { std::size_t i = 0; for (; i < edges_.size(); ++i) diff --git a/src/roadmap.cc b/src/roadmap.cc index a505ed48ff0d8423c2b925550c208d184025b27f..28e67f5ab8586a6e00d4533c1a218a5ad8c2e12f 100644 --- a/src/roadmap.cc +++ b/src/roadmap.cc @@ -23,7 +23,7 @@ #include <hpp/manipulation/roadmap.hh> #include <hpp/manipulation/roadmap-node.hh> -#include <hpp/manipulation/symbolic-component.hh> +#include <hpp/manipulation/leaf-connected-comp.hh> #include <hpp/manipulation/graph/state.hh> #include <hpp/manipulation/graph/statistics.hh> @@ -118,7 +118,7 @@ namespace hpp { { // call RoadmapNode constructor with new manipulation connected component RoadmapNodePtr_t node = new RoadmapNode (q, ConnectedComponent::create(weak_)); - SymbolicComponentPtr_t sc = WeighedSymbolicComponent::create (weak_.lock()); + LeafConnectedCompPtr_t sc = WeighedLeafConnectedComp::create (weak_.lock()); node->symbolicComponent (sc); sc->setFirstNode(node); return node; @@ -134,8 +134,8 @@ namespace hpp { Parent::addEdge(edge); const RoadmapNodePtr_t& f = static_cast <const RoadmapNodePtr_t> (edge->from()); const RoadmapNodePtr_t& t = static_cast <const RoadmapNodePtr_t> (edge->to()); - SymbolicComponentPtr_t scf = f->symbolicComponent(); - SymbolicComponentPtr_t sct = t->symbolicComponent(); + LeafConnectedCompPtr_t scf = f->symbolicComponent(); + LeafConnectedCompPtr_t sct = t->symbolicComponent(); scf->canReach(sct); if (scf->canMerge(sct)) { if (scf->nodes().size() > sct->nodes().size()) { diff --git a/src/symbolic-planner.cc b/src/symbolic-planner.cc index 9b9d26897782520065155a12db69aaf49e39778a..f0ae1b3c8467dba727976b9c317be031df4cef99 100644 --- a/src/symbolic-planner.cc +++ b/src/symbolic-planner.cc @@ -42,15 +42,15 @@ #include "hpp/manipulation/problem.hh" #include "hpp/manipulation/roadmap.hh" #include "hpp/manipulation/roadmap-node.hh" -#include "hpp/manipulation/symbolic-component.hh" +#include "hpp/manipulation/leaf-connected-comp.hh" #include "hpp/manipulation/graph-path-validation.hh" #include "hpp/manipulation/graph/edge.hh" #include "hpp/manipulation/graph/graph.hh" #include "hpp/manipulation/graph/state-selector.hh" #define CastToWSC_ptr(var, scPtr) \ - WeighedSymbolicComponentPtr_t var = \ - HPP_DYNAMIC_PTR_CAST(WeighedSymbolicComponent,scPtr) + WeighedLeafConnectedCompPtr_t var = \ + HPP_DYNAMIC_PTR_CAST(WeighedLeafConnectedComp,scPtr) namespace hpp { namespace manipulation { @@ -74,22 +74,22 @@ namespace hpp { HPP_DEFINE_TIMECOUNTER(projectPath); HPP_DEFINE_TIMECOUNTER(validatePath); - struct WeighedSymbolicComponentComp { - bool operator() (const SymbolicComponentPtr_t& lhs, const SymbolicComponentPtr_t& rhs) + struct WeighedLeafConnectedCompComp { + bool operator() (const LeafConnectedCompPtr_t& lhs, const LeafConnectedCompPtr_t& rhs) { - return HPP_DYNAMIC_PTR_CAST(WeighedSymbolicComponent, lhs)->weight_ - > HPP_DYNAMIC_PTR_CAST(WeighedSymbolicComponent, rhs)->weight_; + return HPP_DYNAMIC_PTR_CAST(WeighedLeafConnectedComp, lhs)->weight_ + > HPP_DYNAMIC_PTR_CAST(WeighedLeafConnectedComp, rhs)->weight_; } }; - typedef std::list<WeighedSymbolicComponentPtr_t> SymbolicComponentList_t; - SymbolicComponentList_t sorted_list (const SymbolicComponents_t& sc) + typedef std::list<WeighedLeafConnectedCompPtr_t> LeafConnectedCompList_t; + LeafConnectedCompList_t sorted_list (const LeafConnectedComps_t& sc) { - SymbolicComponentList_t l; - WeighedSymbolicComponentComp comp; - for (SymbolicComponents_t::const_iterator _sc = sc.begin(); + LeafConnectedCompList_t l; + WeighedLeafConnectedCompComp comp; + for (LeafConnectedComps_t::const_iterator _sc = sc.begin(); _sc != sc.end(); ++_sc) l.insert (std::upper_bound(l.begin(), l.end(), *_sc, comp), - HPP_DYNAMIC_PTR_CAST(WeighedSymbolicComponent, *_sc)); + HPP_DYNAMIC_PTR_CAST(WeighedLeafConnectedComp, *_sc)); return l; } @@ -179,7 +179,7 @@ namespace hpp { // Get the roadmap and the symbolic components RoadmapPtr_t rdm = HPP_DYNAMIC_PTR_CAST(Roadmap, roadmap()); HPP_ASSERT(rdm); - SymbolicComponentList_t scs = sorted_list (rdm->symbolicComponents()); + LeafConnectedCompList_t scs = sorted_list (rdm->symbolicComponents()); core::Nodes_t newNodes; core::PathPtr_t path; @@ -199,8 +199,8 @@ namespace hpp { itcc != rdm->connectedComponents ().end (); ++itcc) { // Find the symbolic component in this connected component which has // the highest value. - SymbolicComponentPtr_t sc; - for (SymbolicComponentList_t::iterator _sc = scs.begin(); + LeafConnectedCompPtr_t sc; + for (LeafConnectedCompList_t::iterator _sc = scs.begin(); _sc != scs.end(); ++_sc) { sc = *_sc; if (sc->connectedComponent() == *itcc) break; @@ -300,10 +300,10 @@ namespace hpp { HPP_START_TIMECOUNTER (chooseEdge); // This code should go into a NodeSelector derived class. - WeighedSymbolicComponentPtr_t wscPtr = HPP_DYNAMIC_PTR_CAST - (WeighedSymbolicComponent, n_near->symbolicComponent()); + WeighedLeafConnectedCompPtr_t wscPtr = HPP_DYNAMIC_PTR_CAST + (WeighedLeafConnectedComp, n_near->symbolicComponent()); if (wscPtr) { - WeighedSymbolicComponent wsc = *wscPtr; + WeighedLeafConnectedComp wsc = *wscPtr; value_type R = rand() / RAND_MAX; std::size_t i = 0; for (value_type sum = wsc.p_[0]; sum < R; sum += wsc.p_[i]) { ++i; } @@ -632,7 +632,7 @@ namespace hpp { void SymbolicPlanner::updateEdgeProba ( const graph::EdgePtr_t edge, - WeighedSymbolicComponentPtr_t wsc, + WeighedLeafConnectedCompPtr_t wsc, const value_type alpha) { size_type i = wsc->indexOf (edge); assert(i < wsc->p_.size());