diff --git a/src/graph/validation.cc b/src/graph/validation.cc index e2ceaafef62a0bcb7cf6ae82f8d629b2a2a79bdb..562e307ac0042b4116710a611bb276ecd13bbaee 100644 --- a/src/graph/validation.cc +++ b/src/graph/validation.cc @@ -37,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) { @@ -164,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