Skip to content
Snippets Groups Projects
Commit 46097521 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by Joseph Mirabel
Browse files

Make exception more explicit.

parent 0a1e552a
No related branches found
No related tags found
No related merge requests found
...@@ -16,6 +16,8 @@ ...@@ -16,6 +16,8 @@
#include "hpp/manipulation/graph-path-validation.hh" #include "hpp/manipulation/graph-path-validation.hh"
#include <hpp/pinocchio/configuration.hh>
#include <hpp/core/path.hh> #include <hpp/core/path.hh>
#include <hpp/core/path-vector.hh> #include <hpp/core/path-vector.hh>
...@@ -94,6 +96,7 @@ namespace hpp { ...@@ -94,6 +96,7 @@ namespace hpp {
bool GraphPathValidation::impl_validate (const PathPtr_t& path, bool GraphPathValidation::impl_validate (const PathPtr_t& path,
bool reverse, PathPtr_t& validPart, PathValidationReportPtr_t& report) bool reverse, PathPtr_t& validPart, PathValidationReportPtr_t& report)
{ {
using pinocchio::displayConfig;
PathVectorPtr_t pathVector = HPP_DYNAMIC_PTR_CAST(PathVector, path); PathVectorPtr_t pathVector = HPP_DYNAMIC_PTR_CAST(PathVector, path);
if (pathVector) if (pathVector)
return impl_validate (pathVector, reverse, validPart, report); return impl_validate (pathVector, reverse, validPart, report);
...@@ -147,11 +150,27 @@ namespace hpp { ...@@ -147,11 +150,27 @@ namespace hpp {
validPart = path->extract (std::make_pair (oldTR.first,oldTR.first)); validPart = path->extract (std::make_pair (oldTR.first,oldTR.first));
return false; return false;
} }
if (!oldPath (q, oldTR.first)) if (!oldPath (q, oldTR.first)) {
throw std::logic_error ("Initial configuration of the path to be validated cannot be projected."); std::stringstream oss;
oss << "Initial configuration of the path to be validated failed to"
" be projected. After maximal number of iterations, q="
<< displayConfig (q) << "; error=";
vector_t error;
oldPath.constraints ()->isSatisfied (q, error);
oss << displayConfig (error) << ".";
throw std::logic_error (oss.str ().c_str ());
}
const graph::StatePtr_t& oldOstate = constraintGraph_->getState (q); const graph::StatePtr_t& oldOstate = constraintGraph_->getState (q);
if (!oldPath (q, oldTR.second)) if (!oldPath (q, oldTR.second)) {
throw std::logic_error ("End configuration of the path to be validated cannot be projected."); std::stringstream oss;
oss << "End configuration of the path to be validated failed to"
" be projected. After maximal number of iterations, q="
<< displayConfig (q) << "; error=";
vector_t error;
oldPath.constraints ()->isSatisfied (q, error);
oss << displayConfig (error) << ".";
throw std::logic_error (oss.str ().c_str ());
}
const graph::StatePtr_t& oldDstate = constraintGraph_->getState (q); const graph::StatePtr_t& oldDstate = constraintGraph_->getState (q);
if (origState == oldOstate && destState == oldDstate) { if (origState == oldOstate && destState == oldDstate) {
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment