diff --git a/src/graph/helper.cc b/src/graph/helper.cc index a2d965607e0911139d814a5271765359bbcf5d16..04dfc6d9b93e4088edff6c3bbbce2c5dc363be14 100644 --- a/src/graph/helper.cc +++ b/src/graph/helper.cc @@ -40,6 +40,8 @@ #include <hpp/manipulation/graph/guided-state-selector.hh> #include <hpp/manipulation/problem-solver.hh> +#define CASE_TO_STRING(var, value) ( (var & value) ? std::string(#value) : std::string() ) + namespace hpp { namespace manipulation { namespace graph { @@ -139,6 +141,17 @@ namespace hpp { static inline const StatePtr_t& Nintersec (const StateArray& n) { assert (intersec); return n[1 + (pregrasp?1:0)]; } static inline const StatePtr_t& Npreplace (const StateArray& n) { assert (preplace); return n[1 + (pregrasp?1:0) + (intersec?1:0)]; } + static inline std::string caseToString () + { + return CASE_TO_STRING (gCase, NoGrasp) + + CASE_TO_STRING (gCase, GraspOnly) + + CASE_TO_STRING (gCase, WithPreGrasp) + + " - " + + CASE_TO_STRING (gCase, NoPlace) + + CASE_TO_STRING (gCase, PlaceOnly) + + CASE_TO_STRING (gCase, WithPrePlace); + } + static inline EdgePtr_t makeWE ( const std::string& name, const StatePtr_t& from, const StatePtr_t& to, @@ -331,6 +344,8 @@ namespace hpp { const FoliatedManifold& submanifoldDef) { typedef CaseTraits<gCase> T; + hppDout (info, "Creating edges " << forwName << " and " << backName + << "\ncase is " << T::caseToString ()); assert (T::valid && "Not a valid case."); typedef typename T::StateArray StateArray; typedef typename T::EdgeArray EdgeArray;