diff --git a/include/hpp/manipulation/graph/graph-component.hh b/include/hpp/manipulation/graph/graph-component.hh
index d286f552621dccfac746a0f299b988c52d20fa83..1644fe43dd4f4459f3b68d891378be760c8280bf 100644
--- a/include/hpp/manipulation/graph/graph-component.hh
+++ b/include/hpp/manipulation/graph/graph-component.hh
@@ -57,10 +57,12 @@ namespace hpp {
virtual void addLockedDofConstraint (const LockedDofPtr_t& constraint);
/// Insert the numerical constraints in a ConfigProjector
- void insertNumericalConstraints (ConfigProjectorPtr_t& proj) const;
+ /// \return true is at least one DifferentiableFunctionPtr_t was inserted.
+ bool insertNumericalConstraints (ConfigProjectorPtr_t& proj) const;
/// Insert the LockedDof constraints in a ConstraintSet
- void insertLockedDofs (ConstraintSetPtr_t cs) const;
+ /// \return true is at least one LockedDofPtr_t was inserted.
+ bool insertLockedDofs (ConstraintSetPtr_t cs) const;
/// Get a reference to the DifferentiableFunctions_t
const DifferentiableFunctions_t& numericalConstraints() const;
diff --git a/include/hpp/manipulation/graph/node.hh b/include/hpp/manipulation/graph/node.hh
index 9a48f7e7ab0d232b3b9c238f5b12e193260f0707..f778b9e7cabd8facfbe4ecc5aa1b15329c15d2a0 100644
--- a/include/hpp/manipulation/graph/node.hh
+++ b/include/hpp/manipulation/graph/node.hh
@@ -95,11 +95,13 @@ namespace hpp {
}
/// Insert the numerical constraints in a ConfigProjector
- void insertNumericalConstraintsForPath (ConfigProjectorPtr_t& proj) const
+ /// \return true is at least one DifferentiableFunctionPtr_t was inserted.
+ bool insertNumericalConstraintsForPath (ConfigProjectorPtr_t& proj) const
{
for (DifferentiableFunctions_t::const_iterator it = numericalConstraintsForPath_.begin();
it != numericalConstraintsForPath_.end(); it++)
proj->addConstraint (it->first, it->second);
+ return !numericalConstraintsForPath_.empty ();
}
/// Get a reference to the DifferentiableFunctions_t
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 7a66ad06c1484aac2f81435a7a5ed9afa14b1597..6cad6c047a68314a49e51e54bb5d16a645026bbf 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -104,10 +104,11 @@ namespace hpp {
ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
- g->insertNumericalConstraints (proj);
- insertNumericalConstraints (proj);
- to ()->insertNumericalConstraints (proj);
- constraint->addConstraint (proj);
+ // If at least one DifferentiableFunctionPtr_t was inserted, the add the projector.
+ if (g->insertNumericalConstraints (proj)
+ | insertNumericalConstraints (proj)
+ | to ()->insertNumericalConstraints (proj))
+ constraint->addConstraint (proj);
g->insertLockedDofs (constraint);
insertLockedDofs (constraint);
@@ -131,10 +132,11 @@ namespace hpp {
ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
- g->insertNumericalConstraints (proj);
- insertNumericalConstraints (proj);
- node ()->insertNumericalConstraintsForPath (proj);
- constraint->addConstraint (proj);
+ // If at least one DifferentiableFunctionPtr_t was inserted, the add the projector.
+ if (g->insertNumericalConstraints (proj)
+ | insertNumericalConstraints (proj)
+ | node ()->insertNumericalConstraintsForPath (proj))
+ constraint->addConstraint (proj);
g->insertLockedDofs (constraint);
insertLockedDofs (constraint);
@@ -161,15 +163,19 @@ namespace hpp {
bool Edge::applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const
{
- configConstraint ()->offsetFromConfig (qoffset);
- if (configConstraint ()->apply (q))
+ ConstraintSetPtr_t c = configConstraint ();
+ c->offsetFromConfig (qoffset);
+ ConfigProjectorPtr_t proj = c->configProjector ();
+ if (c->apply (q)) {
return true;
- typedef ::hpp::statistics::SuccessStatistics SuccessStatistics;
- SuccessStatistics& ss = configConstraint ()->configProjector ()->statistics ();
- if (ss.nbFailure () > ss.nbSuccess ()) {
- hppDout (warning, configConstraint ()->name () << " fails often." << std::endl << ss);
- } else {
- hppDout (warning, configConstraint ()->name () << " succeeds at rate " << (double)(ss.nbSuccess ()) / ss.numberOfObservations () << ".");
+ }
+ if (proj) {
+ ::hpp::statistics::SuccessStatistics& ss = proj->statistics ();
+ if (ss.nbFailure () > ss.nbSuccess ()) {
+ hppDout (warning, c->name () << " fails often." << std::endl << ss);
+ } else {
+ hppDout (warning, c->name () << " succeeds at rate " << (double)(ss.nbSuccess ()) / ss.numberOfObservations () << ".");
+ }
}
return false;
}
@@ -304,8 +310,8 @@ namespace hpp {
ConstraintSetPtr_t cs = extraConfigConstraint ();
cs->offsetFromConfig (q_offset);
- if (cs->configProjector ()) {
- const ConfigProjectorPtr_t cp = cs->configProjector ();
+ const ConfigProjectorPtr_t cp = cs->configProjector ();
+ if (cp) {
vector_t offset = cp->offsetFromConfig (q_offset);
size_t row = 0, nbRows = 0;
for (DifferentiableFunctions_t::const_iterator it = extraNumericalFunctions_.begin ();
@@ -329,12 +335,13 @@ namespace hpp {
// Eventually, do the projection.
if (cs->apply (q))
return true;
- typedef ::hpp::statistics::SuccessStatistics SuccessStatistics;
- SuccessStatistics& ss = cs->configProjector ()->statistics ();
- if (ss.nbFailure () > ss.nbSuccess ()) {
- hppDout (warning, configConstraint ()->name () << " fails often." << std::endl << ss);
- } else {
- hppDout (warning, configConstraint ()->name () << " succeeds at rate " << (double)(ss.nbSuccess ()) / ss.numberOfObservations () << ".");
+ if (cp) {
+ ::hpp::statistics::SuccessStatistics& ss = cp->statistics ();
+ if (ss.nbFailure () > ss.nbSuccess ()) {
+ hppDout (warning, cs->name () << " fails often." << std::endl << ss);
+ } else {
+ hppDout (warning, cs->name () << " succeeds at rate " << (double)(ss.nbSuccess ()) / ss.numberOfObservations () << ".");
+ }
}
return false;
}
@@ -392,14 +399,15 @@ namespace hpp {
ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
- g->insertNumericalConstraints (proj);
+ bool gHasDiffFunc = g->insertNumericalConstraints (proj);
for (DifferentiableFunctions_t::const_iterator it = extraNumericalFunctions_.begin ();
it != extraNumericalFunctions_.end (); it++) {
proj->addConstraint (it->first, it->second);
}
- insertNumericalConstraints (proj);
- to ()->insertNumericalConstraints (proj);
- constraint->addConstraint (proj);
+ if (gHasDiffFunc | !extraNumericalFunctions_.empty ()
+ | insertNumericalConstraints (proj)
+ | to ()->insertNumericalConstraints (proj))
+ constraint->addConstraint (proj);
g->insertLockedDofs (constraint);
for (LockedDofs_t::const_iterator it = extraLockedDofs_.begin ();
diff --git a/src/graph/graph-component.cc b/src/graph/graph-component.cc
index 02f511150c68229aa907bec16079d155cde2558f..98a3f4fd0537919e1c57198a2576290e6d7aca12 100644
--- a/src/graph/graph-component.cc
+++ b/src/graph/graph-component.cc
@@ -76,18 +76,20 @@ namespace hpp {
lockedDofConstraints_.push_back (constraint);
}
- void GraphComponent::insertNumericalConstraints (ConfigProjectorPtr_t& proj) const
+ bool GraphComponent::insertNumericalConstraints (ConfigProjectorPtr_t& proj) const
{
for (DifferentiableFunctions_t::const_iterator it = numericalConstraints_.begin();
it != numericalConstraints_.end(); it++)
proj->addConstraint (it->first, it->second);
+ return !numericalConstraints_.empty ();
}
- void GraphComponent::insertLockedDofs (ConstraintSetPtr_t cs) const
+ bool GraphComponent::insertLockedDofs (ConstraintSetPtr_t cs) const
{
for (LockedDofs_t::const_iterator it = lockedDofConstraints_.begin();
it != lockedDofConstraints_.end(); it++)
cs->addConstraint (*it);
+ return !lockedDofConstraints_.empty ();
}
const DifferentiableFunctions_t& GraphComponent::numericalConstraints() const
diff --git a/src/graph/node.cc b/src/graph/node.cc
index 950f2a4e9f9402b26dab61180407e1dbdb433484..dae220105a4017e5de717587358d2e09c3290fc1 100644
--- a/src/graph/node.cc
+++ b/src/graph/node.cc
@@ -85,9 +85,10 @@ namespace hpp {
ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj " + n, g->errorThreshold(), g->maxIterations());
- g->insertNumericalConstraints (proj);
- insertNumericalConstraints (proj);
- constraint->addConstraint (proj);
+ // If at least one DifferentiableFunctionPtr_t was inserted, then add the projector.
+ if (g->insertNumericalConstraints (proj)
+ | insertNumericalConstraints (proj))
+ constraint->addConstraint (proj);
g->insertLockedDofs (constraint);
insertLockedDofs (constraint);