From c972a892436ab64205589e4bd8c30f33dafb6e5f Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Mon, 10 Nov 2014 11:14:36 +0100
Subject: [PATCH] Do not insert an empty projector in ConstraintSet
---
.../hpp/manipulation/graph/graph-component.hh | 6 +-
include/hpp/manipulation/graph/node.hh | 4 +-
src/graph/edge.cc | 64 +++++++++++--------
src/graph/graph-component.cc | 6 +-
src/graph/node.cc | 7 +-
5 files changed, 51 insertions(+), 36 deletions(-)
diff --git a/include/hpp/manipulation/graph/graph-component.hh b/include/hpp/manipulation/graph/graph-component.hh
index d286f55..1644fe4 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 9a48f7e..f778b9e 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 7a66ad0..6cad6c0 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 02f5111..98a3f4f 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 950f2a4..dae2201 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);
--
GitLab