diff --git a/CMakeLists.txt b/CMakeLists.txt
index a52b2ea0099bfc576bd80565d54164059c5f556d..f77206ac8a2a5df7ec2da6da41ce93f13118be30 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/include/hpp/manipulation/graph/fwd.hh b/include/hpp/manipulation/graph/fwd.hh
index 425bd85d2149e2ce7c38dcb95fd78866737a8a26..5806bde298bcec447467651f2f0186b609b1e512 100644
--- a/include/hpp/manipulation/graph/fwd.hh
+++ b/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
diff --git a/include/hpp/manipulation/graph/validation.hh b/include/hpp/manipulation/graph/validation.hh
new file mode 100644
index 0000000000000000000000000000000000000000..62f53b54c3d7253b8e219968cb981efbc12ec359
--- /dev/null
+++ b/include/hpp/manipulation/graph/validation.hh
@@ -0,0 +1,108 @@
+// 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
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 22dd3287aff4aec63da19bbe194b4a2247ce4b3f..0ae2c0ecb6b5ff76d5a8902ebd461fa6a955c59d 100644
--- a/src/CMakeLists.txt
+++ b/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
diff --git a/src/graph/validation.cc b/src/graph/validation.cc
new file mode 100644
index 0000000000000000000000000000000000000000..2c36740d6ac4d91366a73c0053c54dd732ae6d2a
--- /dev/null
+++ b/src/graph/validation.cc
@@ -0,0 +1,165 @@
+// 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