From ff59a07d30f84bea275d0be75dc7dbe210dfa2b5 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Thu, 7 Jan 2016 10:30:31 +0100
Subject: [PATCH] LevelSetEdge does not throw when apply constraints from a
offset configuration.
---
include/hpp/manipulation/graph/edge.hh | 3 ++
include/hpp/manipulation/graph/statistics.hh | 2 ++
src/graph/edge.cc | 29 ++++++++++++++------
src/graph/statistics.cc | 12 ++++++++
4 files changed, 37 insertions(+), 9 deletions(-)
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 4b5f3286..135edea0 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -297,6 +297,9 @@ namespace hpp {
virtual void populateTooltip (dot::Tooltip& tp) const;
private:
+ bool applyConstraintsWithOffset (ConfigurationIn_t qoffset,
+ ConfigurationIn_t qlevelset, ConfigurationOut_t q) const;
+
typedef Cache < ConstraintSetPtr_t > Constraint_t;
/// See pathConstraint member function.
diff --git a/include/hpp/manipulation/graph/statistics.hh b/include/hpp/manipulation/graph/statistics.hh
index fea288a3..ed93750a 100644
--- a/include/hpp/manipulation/graph/statistics.hh
+++ b/include/hpp/manipulation/graph/statistics.hh
@@ -122,6 +122,8 @@ namespace hpp {
statistics::DiscreteDistribution < RoadmapNodePtr_t > getDistribOutOfConnectedComponent (
const core::ConnectedComponentPtr_t& cc) const;
+ statistics::DiscreteDistribution < RoadmapNodePtr_t > getDistrib () const;
+
private:
/// The constraint that creates the foliation.
ConstraintSetPtr_t constraint_;
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 1c2089f9..6cc0705f 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -469,35 +469,46 @@ namespace hpp {
}
}
- bool LevelSetEdge::applyConstraints (ConfigurationIn_t, ConfigurationOut_t) const
+ bool LevelSetEdge::applyConstraints (ConfigurationIn_t qoffset, ConfigurationOut_t q) const
{
- throw std::logic_error ("I need to know which connected component we wish to use.");
+ // First, get an offset from the histogram
+ statistics::DiscreteDistribution < RoadmapNodePtr_t > distrib = hist_->getDistrib ();
+ const Configuration_t& qlevelset = *(distrib ()->configuration ());
+
+ return applyConstraintsWithOffset (qoffset, qlevelset, q);
}
bool LevelSetEdge::applyConstraints (core::NodePtr_t n_offset, ConfigurationOut_t q) const
{
// First, get an offset from the histogram that is not in the same connected component.
statistics::DiscreteDistribution < RoadmapNodePtr_t > distrib = hist_->getDistribOutOfConnectedComponent (n_offset->connectedComponent ());
- const Configuration_t& levelsetTarget = *(distrib ()->configuration ()),
- q_offset = *(n_offset->configuration ());
- // Then, set the offset.
+ const Configuration_t& qlevelset = *(distrib ()->configuration ()),
+ qoffset = *(n_offset->configuration ());
+
+ return applyConstraintsWithOffset (qoffset, qlevelset, q);
+ }
+
+ bool LevelSetEdge::applyConstraintsWithOffset (ConfigurationIn_t qoffset,
+ ConfigurationIn_t qlevelset, ConfigurationOut_t q) const
+ {
+ // First, set the offset.
ConstraintSetPtr_t cs = extraConfigConstraint ();
const ConfigProjectorPtr_t cp = cs->configProjector ();
assert (cp);
- cp->rightHandSideFromConfig (q_offset);
+ cp->rightHandSideFromConfig (qoffset);
for (NumericalConstraints_t::const_iterator it =
extraNumericalConstraints_.begin ();
it != extraNumericalConstraints_.end (); ++it) {
- (*it)->rightHandSideFromConfig (levelsetTarget);
+ (*it)->rightHandSideFromConfig (qlevelset);
}
for (LockedJoints_t::const_iterator it = extraLockedJoints_.begin ();
it != extraLockedJoints_.end (); ++it) {
- (*it)->rightHandSideFromConfig (levelsetTarget);
+ (*it)->rightHandSideFromConfig (qlevelset);
}
cp->updateRightHandSide ();
// Eventually, do the projection.
- if (isShort_) q = q_offset;
+ if (isShort_) q = qoffset;
if (cs->apply (q)) return true;
::hpp::statistics::SuccessStatistics& ss = cp->statistics ();
if (ss.nbFailure () > ss.nbSuccess ()) {
diff --git a/src/graph/statistics.cc b/src/graph/statistics.cc
index 3bdde7c1..e50c4321 100644
--- a/src/graph/statistics.cc
+++ b/src/graph/statistics.cc
@@ -250,6 +250,18 @@ namespace hpp {
return distrib;
}
+ statistics::DiscreteDistribution < RoadmapNodePtr_t > LeafHistogram::getDistrib () const
+ {
+ statistics::DiscreteDistribution < RoadmapNodePtr_t > distrib;
+ for (const_iterator bin = begin(); bin != end (); ++bin) {
+ unsigned int w = bin->freq ();
+ if (w == 0)
+ continue;
+ distrib.insert (bin->nodes ().front (), w);
+ }
+ return distrib;
+ }
+
const LeafBin::RoadmapNodes_t& LeafBin::nodes () const
{
return nodes_;
--
GitLab