Add class hpp::manipulation::graph::Validation

CMakeLists.txt

@@ -87,6 +87,7 @@ SET (${PROJECT_NAME}_HEADERS
   include/hpp/manipulation/graph/fwd.hh
   include/hpp/manipulation/graph/dot.hh
   include/hpp/manipulation/graph/helper.hh
+  include/hpp/manipulation/graph/validation.hh
 
   include/hpp/manipulation/path-optimization/small-steps.hh
   include/hpp/manipulation/path-optimization/enforce-transition-semantic.hh

include/hpp/manipulation/graph/fwd.hh

@@ -60,6 +60,9 @@ namespace hpp {
       typedef boost::shared_ptr <StateHistogram> StateHistogramPtr_t;
       typedef boost::shared_ptr <LeafHistogram> LeafHistogramPtr_t;
       typedef std::list < HistogramPtr_t > Histograms_t;
+
+      class Validation;
+      typedef boost::shared_ptr < Validation > ValidationPtr_t;
     } // namespace graph
   } // namespace manipulation
 } // namespace hpp

include/hpp/manipulation/graph/validation.hh

+// Copyright (c) 2019, LAAS-CNRS
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+//
+// This file is part of hpp-manipulation.
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Lesser Public License for more details.  You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef HPP_MANIPULATION_GRAPH_VALIDATION_REPORT_HH
+# define HPP_MANIPULATION_GRAPH_VALIDATION_REPORT_HH
+
+# include <string>
+# include <vector>
+# include <hpp/core/validation-report.hh>
+
+# include <hpp/manipulation/config.hh>
+# include <hpp/manipulation/fwd.hh>
+# include <hpp/manipulation/graph/fwd.hh>
+
+namespace hpp {
+  namespace manipulation {
+    namespace graph {
+      /// \addtogroup constraint_graph
+      /// \{
+
+      /// Check that graph components are valid.
+      ///
+      /// A stringified validation report can be obtained via
+      /// Validation::print or operator<< (std::ostream&, const Validation&).
+      class HPP_MANIPULATION_DLLAPI Validation
+      {
+        public:
+          Validation(const core::ProblemPtr_t& problem)
+            : problem_ (problem) {}
+
+          void clear ()
+          {
+            warnings_.clear();
+            errors_.clear();
+          }
+
+          bool hasWarnings () const { return !warnings_.empty(); }
+
+          bool hasErrors () const { return !errors_.empty(); }
+
+          virtual std::ostream& print (std::ostream& os) const;
+
+          /// Validate a graph component.
+          /// It dynamically casts in order to call the right function among
+          /// the validation method below.
+          ///
+          /// \return true if the component could not be proven infeasible.
+          /// \note Even if true is returned, the report can contain warnings.
+          bool validate (const GraphComponentPtr_t& comp);
+
+          /// Validate a state
+          /// \return true if the state could not be proven infeasible.
+          /// \note Even if true is returned, the report can contain warnings.
+          bool validateState (const StatePtr_t& state);
+
+          /// Validate an edge
+          /// \return true if the edge could not be proven infeasible.
+          /// \note Even if true is returned, the report can contain warnings.
+          bool validateEdge  (const EdgePtr_t & edge);
+
+          /// Validate an graph
+          /// \return true if no component of the graph could not be proven infeasible.
+          /// \note Even if true is returned, the report can contain warnings.
+          bool validateGraph (const GraphPtr_t& graph);
+
+
+        private:
+          void addWarning (const GraphComponentPtr_t& c, const std::string& w)
+          {
+            warnings_.push_back (Message (c, w));
+          }
+
+          void addError (const GraphComponentPtr_t& c, const std::string& w)
+          {
+            errors_.push_back (Message (c, w));
+          }
+
+          typedef std::pair<GraphComponentPtr_t, std::string> Message;
+          std::vector<Message> warnings_, errors_;
+
+          core::ProblemPtr_t problem_;
+      };
+
+      inline std::ostream& operator<< (std::ostream& os, const Validation& v)
+      {
+        return v.print(os);
+      }
+
+      /// \}
+    } // namespace graph
+  } // namespace manipulation
+
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_GRAPH_VALIDATION_REPORT_HH

src/CMakeLists.txt

@@ -44,6 +44,7 @@ SET(SOURCES
   graph/helper.cc
 
   graph/dot.cc
+  graph/validation.cc
 
   path-optimization/random-shortcut.cc
   path-optimization/enforce-transition-semantic.cc

src/graph/validation.cc

+// Copyright (c) 2019, LAAS-CNRS
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+//
+// This file is part of hpp-manipulation.
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Lesser Public License for more details.  You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
+
+#define HPP_DEBUG 1
+
+#include "hpp/manipulation/graph/validation.hh"
+
+#include <sstream>
+
+#include <hpp/util/indent.hh>
+
+#include <hpp/core/collision-validation.hh>
+#include <hpp/core/configuration-shooter.hh>
+#include <hpp/core/relative-motion.hh>
+
+#include "hpp/manipulation/problem.hh"
+#include "hpp/manipulation/graph/graph.hh"
+#include "hpp/manipulation/graph/edge.hh"
+#include "hpp/manipulation/graph/state.hh"
+#include "hpp/manipulation/graph/state-selector.hh"
+
+namespace hpp {
+  namespace manipulation {
+    namespace graph {
+      std::ostream& Validation::print (std::ostream& os) const
+      {
+        for (std::size_t i = 0; i < warnings_.size(); ++i) {
+          os << "Warning " << i
+            << " (" << (warnings_[i].first ? warnings_[i].first->name() : "no component")
+            << ')' << incendl << warnings_[i].second << decendl;
+        }
+        for (std::size_t i = 0; i < errors_.size(); ++i) {
+          os << "Error " << i
+            << " (" << (errors_[i].first ? errors_[i].first->name() : "no component")
+            << ')' << incendl << errors_[i].second << decendl;
+        }
+        return os;
+      }
+
+      bool Validation::validate (const GraphComponentPtr_t& comp)
+      {
+        if      (HPP_DYNAMIC_PTR_CAST(State, comp))
+          return validateState (HPP_DYNAMIC_PTR_CAST(State, comp));
+        else if (HPP_DYNAMIC_PTR_CAST(Edge, comp))
+          return validateEdge  (HPP_DYNAMIC_PTR_CAST(Edge, comp));
+        else if (HPP_DYNAMIC_PTR_CAST(Graph, comp))
+          return validateGraph (HPP_DYNAMIC_PTR_CAST(Graph, comp));
+        else
+          return true;
+      }
+
+      bool Validation::validateState (const StatePtr_t& state)
+      {
+        std::ostringstream oss;
+        oss << incindent;
+        bool success = true;
+
+        // 1. try to generate a configuration in this state.
+        bool projOk;
+        Configuration_t q;
+        std::size_t i, Nrand = 100;
+
+        for (i = 0; i < Nrand; i++) {
+          problem_->configurationShooter()->shoot(q);
+          projOk = state->configConstraint()->apply (q);
+          if (projOk) break;
+        }
+        if (!projOk) {
+          oss << "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 << '.';
+          addWarning (state, oss.str());
+          oss.clear();
+        }
+
+        // 2. check the collision pairs which will be disabled because of the
+        //    constraint.
+        core::CollisionValidationPtr_t colValidation (
+            core::CollisionValidation::create (problem_->robot()));
+        for (std::size_t i = 0; i < problem_->collisionObstacles().size(); ++i)
+          colValidation->addObstacle (problem_->collisionObstacles()[i]);
+        colValidation->computeAllContacts (true);
+
+        typedef core::RelativeMotion RelativeMotion;
+        RelativeMotion::matrix_type relMotion = RelativeMotion::matrix (problem_->robot());
+        RelativeMotion::fromConstraint (relMotion, problem_->robot(),
+            state->configConstraint());
+
+        // Invert the relative motions.
+        hppDout (info, "Relative motion matrix:\n" << relMotion);
+        for (size_type r = 0; r < relMotion.rows(); ++r)
+          for (size_type c = 0; c < r; ++c) {
+            if (relMotion(r,c) != relMotion(c,r)) {
+              hppDout (error, "Relative motion matrix not symmetric.");
+            }
+            switch (relMotion(r,c)) {
+              case RelativeMotion::Constrained:
+                relMotion(r,c) = relMotion(c,r) = RelativeMotion::Unconstrained;
+                break;
+              case RelativeMotion::Parameterized:
+              case RelativeMotion::Unconstrained:
+                relMotion(r,c) = relMotion(c,r) = RelativeMotion::Constrained;
+                break;
+              default:
+                throw std::logic_error ("Relative motion not understood");
+            }
+          }
+        hppDout (info, "Relative motion matrix:\n" << relMotion);
+
+        colValidation->filterCollisionPairs (relMotion);
+        core::ValidationReportPtr_t colReport;
+        bool colOk = colValidation->validate (q, colReport);
+
+        if (!colOk) {
+          oss << "The following collision pairs will always collide." << incendl << *colReport << decindent;
+          addError (state, oss.str());
+          success = false;
+        }
+
+        return success;
+      }
+
+      bool Validation::validateEdge  (const EdgePtr_t &)
+      {
+        return true;
+      }
+
+      bool Validation::validateGraph (const GraphPtr_t& graph)
+      {
+        if (!graph) return false;
+        bool success = true;
+
+        States_t states = graph->stateSelector()->getStates();
+        for (States_t::const_iterator _state = states.begin();
+            _state != states.end(); ++_state) {
+          if (!validateState(*_state)) success = false;
+        }
+
+        states = graph->stateSelector()->getWaypointStates();
+        for (States_t::const_iterator _state = states.begin();
+            _state != states.end(); ++_state) {
+          if (!validateState(*_state)) success = false;
+        }
+
+        return success;
+      }
+    } // namespace graph
+  } // namespace manipulation
+} // namespace hpp