Merge pull request #70 from jmirabel/devel

Improve graph validation + fix dynamic cast issue.

include/hpp/manipulation/graph/graph-component.hh

@@ -89,6 +89,8 @@ namespace hpp {
           const IntervalsContainer_t& passiveDofs() const;
 
           /// Get a reference to the LockedJoints_t
+          /// \deprecated LockedJoint are handled as classical explicit
+          ///             constraint
           const LockedJoints_t& lockedJoints () const
             HPP_MANIPULATION_DEPRECATED;
 
@@ -117,7 +119,7 @@ namespace hpp {
           NumericalConstraints_t numericalConstraints_;
           /// Stores the passive dofs for each numerical constraints.
           IntervalsContainer_t passiveDofs_;
-          /// List of LockedJoint constraints: to be removed
+          /// List of LockedJoint constraints: \todo to be removed
           const LockedJoints_t lockedJoints_;
           /// A weak pointer to the parent graph.
           GraphWkPtr_t graph_;

include/hpp/manipulation/graph/state-selector.hh

@@ -27,7 +27,7 @@ namespace hpp {
     namespace graph {
       /// This class is used to get the state of a configuration. States have to
       /// be ordered because a configuration can be in several states.
-      class HPP_MANIPULATION_DLLAPI StateSelector : public GraphComponent
+      class HPP_MANIPULATION_DLLAPI StateSelector
       {
         public:
           virtual ~StateSelector () {};
@@ -35,6 +35,11 @@ namespace hpp {
           /// Create a new StateSelector.
           static StateSelectorPtr_t create(const std::string& name);
 
+          const std::string& name() const
+          {
+            return name_;
+          }
+
           /// Create an empty state
           StatePtr_t createState (const std::string& name, bool waypoint = false,
               const int w = 0);
@@ -54,29 +59,21 @@ namespace hpp {
           /// Select randomly an outgoing edge of the given state.
           virtual EdgePtr_t chooseEdge(RoadmapNodePtr_t from) const;
 
-          /// Should never be called.
-          void addNumericalConstraint (const constraints::ImplicitPtr_t& /* function */,
-              const segments_t& /* passiveDofs */ = segments_t ())
-          {
-            throw std::logic_error ("StateSelector component does not have constraints.");
-          }
-
-          /// Should never be called.
-          void addLockedJointConstraint
-	    (const constraints::LockedJoint& /* constraint */)
-          {
-            throw std::logic_error ("StateSelector component does not have constraints.");
-          }
-
           /// Print the object in a stream.
           virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
 
+          /// Set the parent graph.
+          void parentGraph(const GraphWkPtr_t& parent);
+
+          /// Set the parent graph.
+          GraphPtr_t parentGraph() const;
+
         protected:
           /// Initialization of the object.
           void init (const StateSelectorPtr_t& weak);
 
           /// Constructor
-          StateSelector (const std::string& name) : GraphComponent (name)
+          StateSelector (const std::string& name) : name_ (name)
           {}
 
           /// Print the object in a stream.
@@ -88,12 +85,21 @@ namespace hpp {
           WeighedStates_t orderedStates_;
           States_t waypoints_;
 
-          virtual void initialize () { isInit_ = true; };
-
         private:
+          /// Name of the component.
+          std::string name_;
+          /// A weak pointer to the parent graph.
+          GraphWkPtr_t graph_;
           /// Weak pointer to itself.
           StateSelectorPtr_t wkPtr_;
+
+          friend std::ostream& operator<< (std::ostream& os, const StateSelector& ss);
       }; // Class StateSelector
+
+      inline std::ostream& operator<< (std::ostream& os, const StateSelector& ss)
+      {
+        return ss.print(os);
+      }
     } // namespace graph
   } // namespace manipulation
 } // namespace hpp

include/hpp/manipulation/problem.hh

@@ -57,12 +57,7 @@ namespace hpp {
         void pathValidation (const PathValidationPtr_t& pathValidation);
 
         /// Get the steering method as a SteeringMethod
-        SteeringMethodPtr_t steeringMethod () const;
-
-        void steeringMethod (core::SteeringMethodPtr_t sm)
-        {
-          Parent::steeringMethod (sm);
-        }
+        SteeringMethodPtr_t manipulationSteeringMethod () const;
 
         /// Build a new path validation
         /// \note Current obstacles are added to the created object.

src/graph/edge.cc

@@ -279,7 +279,7 @@ namespace hpp {
 
         // Build steering method
         const ProblemPtr_t& problem (g->problem());
-        steeringMethod_ = problem->steeringMethod()->innerSteeringMethod()->copy();
+        steeringMethod_ = problem->manipulationSteeringMethod()->innerSteeringMethod()->copy();
         steeringMethod_->constraints (constraint);
         // Build path validation and relative motion matrix
         // TODO this path validation will not contain obstacles added after

src/graph/guided-state-selector.cc

@@ -138,12 +138,7 @@ namespace hpp {
 
       std::ostream& GuidedStateSelector::print (std::ostream& os) const
       {
-        os << "|-- ";
-        GraphComponent::print (os) << std::endl;
-        for (WeighedStates_t::const_iterator it = orderedStates_.begin();
-            orderedStates_.end() != it; ++it)
-          os << it->first << " " << *it->second;
-        return os;
+        return StateSelector::print (os);
       }
     } // namespace graph
   } // namespace manipulation

src/graph/state-selector.cc

@@ -35,7 +35,6 @@ namespace hpp {
 
       void StateSelector::init (const StateSelectorPtr_t& weak)
       {
-        GraphComponent::init (weak);
         wkPtr_ = weak;
       }
 
@@ -129,13 +128,24 @@ namespace hpp {
 
       std::ostream& StateSelector::print (std::ostream& os) const
       {
-        os << "|-- ";
-        GraphComponent::print (os) << std::endl;
         for (WeighedStates_t::const_iterator it = orderedStates_.begin();
             orderedStates_.end() != it; ++it)
           os << it->first << " " << *it->second;
         return os;
       }
+
+      GraphPtr_t StateSelector::parentGraph() const
+      {
+        return graph_.lock ();
+      }
+
+      void StateSelector::parentGraph(const GraphWkPtr_t& parent)
+      {
+        graph_ = parent;
+        GraphPtr_t g = graph_.lock();
+        assert(g);
+      }
+
     } // namespace graph
   } // namespace manipulation
 } // namespace hpp

src/graph/validation.cc

@@ -22,6 +22,8 @@
 
 #include <hpp/util/indent.hh>
 
+#include <hpp/constraints/differentiable-function.hh>
+
 #include <hpp/core/collision-validation.hh>
 #include <hpp/core/configuration-shooter.hh>
 #include <hpp/core/relative-motion.hh>
@@ -35,6 +37,17 @@
 namespace hpp {
   namespace manipulation {
     namespace graph {
+      bool stateAIncludedInStateB (const StatePtr_t& A, const StatePtr_t& B)
+      {
+        const NumericalConstraints_t& Ancs = A->numericalConstraints();
+        const NumericalConstraints_t& Bncs = B->numericalConstraints();
+        for (NumericalConstraints_t::const_iterator _nc = Ancs.begin();
+            _nc != Ancs.end(); ++_nc)
+          if (std::find (Bncs.begin(), Bncs.end(), *_nc) == Bncs.end())
+            return false;
+        return true;
+      }
+
       std::ostream& Validation::print (std::ostream& os) const
       {
         for (std::size_t i = 0; i < warnings_.size(); ++i) {
@@ -64,11 +77,21 @@ namespace hpp {
 
       bool Validation::validateState (const StatePtr_t& state)
       {
-        std::ostringstream oss;
-        oss << incindent;
         bool success = true;
 
-        // 1. try to generate a configuration in this state.
+        // 1. Check that all the constraint are not parameterizable.
+        const NumericalConstraints_t& ncs = state->numericalConstraints();
+        for (NumericalConstraints_t::const_iterator _nc = ncs.begin();
+            _nc != ncs.end(); ++_nc)
+          if ((*_nc)->parameterSize() > 0) {
+            std::ostringstream oss;
+            oss << incindent << "Constraint " << (*_nc)->function().name() <<
+              " has a varying right hand side while constraints of a state must"
+              " have a constant one.";
+            addError (state, oss.str());
+          }
+
+        // 2. try to generate a configuration in this state.
         bool projOk;
         Configuration_t q;
         std::size_t i, Nrand = 100;
@@ -79,17 +102,21 @@ namespace hpp {
           if (projOk) break;
         }
         if (!projOk) {
-          oss << "Failed to apply the constraints to " << Nrand << "random configurations.";
+          std::ostringstream oss;
+          oss << incindent << "Failed to apply the constraints to " << Nrand <<
+            "random configurations.";
           addError (state, oss.str());
           return false;
         }
         if (4 * i > 3 * Nrand) {
-          oss << "Success rate of constraint projection is " << i / Nrand << '.';
+          std::ostringstream oss;
+          oss << incindent << "Success rate of constraint projection is " <<
+            i / Nrand << '.';
           addWarning (state, oss.str());
           oss.clear();
         }
 
-        // 2. check the collision pairs which will be disabled because of the
+        // 3. check the collision pairs which will be disabled because of the
         //    constraint.
         core::CollisionValidationPtr_t colValidation (
             core::CollisionValidation::create (problem_->robot()));
@@ -128,7 +155,9 @@ namespace hpp {
         bool colOk = colValidation->validate (q, colReport);
 
         if (!colOk) {
-          oss << "The following collision pairs will always collide." << incendl << *colReport << decindent;
+          std::ostringstream oss;
+          oss << incindent << "The following collision pairs will always "
+            "collide." << incendl << *colReport << decindent;
           addError (state, oss.str());
           success = false;
         }
@@ -146,10 +175,25 @@ namespace hpp {
         if (!graph) return false;
         bool success = true;
 
-        States_t states = graph->stateSelector()->getStates();
         for (std::size_t i = 1; i < graph->nbComponents(); ++i)
           if (!validate(graph->get(i).lock())) success = false;
 
+        // Check that no state is included in a state which has a higher priority.
+        States_t states = graph->stateSelector()->getStates();
+        for (States_t::const_iterator _state = states.begin();
+            _state != states.end(); ++_state) {
+          for (States_t::const_iterator _stateHO = states.begin();
+              _stateHO != _state; ++_stateHO) {
+            if (stateAIncludedInStateB (*_state, *_stateHO)) {
+              std::ostringstream oss;
+              oss << "State " << (*_state)->name() << " is included in state "
+                << (*_stateHO)->name() << " but the latter has a higher priority.";
+              addError (*_state, oss.str());
+              success = false;
+            }
+          }
+        }
+
         return success;
       }
     } // namespace graph

src/problem.cc

@@ -84,7 +84,7 @@ namespace hpp {
       return pv;
     }
 
-    SteeringMethodPtr_t Problem::steeringMethod () const
+    SteeringMethodPtr_t Problem::manipulationSteeringMethod () const
     {
       return HPP_DYNAMIC_PTR_CAST (SteeringMethod,
           Parent::steeringMethod());