diff --git a/src/graph/validation.cc b/src/graph/validation.cc index 3cad101963db3a2583a8ec6a1b3c5ed72953b588..e2ceaafef62a0bcb7cf6ae82f8d629b2a2a79bdb 100644 --- a/src/graph/validation.cc +++ b/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> @@ -64,11 +66,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 +91,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 +144,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; }