From 474126541aa39c9b82f70917990fbc2a0c8b6772 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Sun, 31 May 2015 21:45:59 +0200
Subject: [PATCH] LeafHistogram uses class Foliation.
Conflicts:
include/hpp/manipulation/graph/edge.hh
include/hpp/manipulation/graph/statistics.hh
src/graph/edge.cc
src/graph/statistics.cc
---
include/hpp/manipulation/graph/edge.hh | 12 +--
include/hpp/manipulation/graph/statistics.hh | 22 ++--
include/hpp/manipulation/roadmap.hh | 3 -
src/graph/edge.cc | 108 ++++++++++++-------
src/graph/statistics.cc | 33 +++---
src/roadmap.cc | 5 -
6 files changed, 100 insertions(+), 83 deletions(-)
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 135edea0..d430efb4 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -268,7 +268,7 @@ namespace hpp {
virtual bool applyConstraints (core::NodePtr_t n_offset, ConfigurationOut_t q) const;
- void buildHistogram ();
+ void histogram (LeafHistogramPtr_t hist);
LeafHistogramPtr_t histogram () const;
@@ -304,14 +304,8 @@ namespace hpp {
/// See pathConstraint member function.
Constraint_t* extraConstraints_;
- virtual ConstraintSetPtr_t extraConfigConstraint () const;
-
- /// Extra NumericalConstraints_t
- NumericalConstraints_t extraNumericalConstraints_;
- IntervalsContainer_t extraPassiveDofs_;
-
- /// Extra LockedJoints_t
- LockedJoints_t extraLockedJoints_;
+ ConstraintSetPtr_t extraConfigConstraint () const;
+ void buildExtraConfigConstraint () const;
/// This histogram will be used to find a good level set.
LeafHistogramPtr_t hist_;
diff --git a/include/hpp/manipulation/graph/statistics.hh b/include/hpp/manipulation/graph/statistics.hh
index 163876ff..06005935 100644
--- a/include/hpp/manipulation/graph/statistics.hh
+++ b/include/hpp/manipulation/graph/statistics.hh
@@ -142,18 +142,14 @@ namespace hpp {
{
public:
typedef ::hpp::statistics::Statistics < LeafBin > Parent;
- /// Constructor
- /// \param constraint The constraint that create the foliation being
- /// studied.
- LeafHistogram (const ConstraintSetPtr_t& constraint);
+
+ static LeafHistogramPtr_t create (const Foliation f);
/// Insert an occurence of a value in the histogram
void add (const RoadmapNodePtr_t& n);
std::ostream& print (std::ostream& os) const;
- const ConstraintSetPtr_t& constraint () const;
-
virtual HistogramPtr_t clone () const;
statistics::DiscreteDistribution < RoadmapNodePtr_t > getDistribOutOfConnectedComponent (
@@ -163,9 +159,19 @@ namespace hpp {
void clear () { Parent::clear(); }
+ const Foliation& foliation () const {
+ return f_;
+ }
+
+ protected:
+ /// Constructor
+ /// \param node defines the submanifold containing the foliation.
+ /// \param constraint The constraint that create the foliation being
+ /// studied.
+ LeafHistogram (const Foliation f);
+
private:
- /// The constraint that creates the foliation.
- ConstraintSetPtr_t constraint_;
+ Foliation f_;
/// Threshold used for equality between offset values.
value_type threshold_;
diff --git a/include/hpp/manipulation/roadmap.hh b/include/hpp/manipulation/roadmap.hh
index 7d1300ee..68091f23 100644
--- a/include/hpp/manipulation/roadmap.hh
+++ b/include/hpp/manipulation/roadmap.hh
@@ -39,9 +39,6 @@ namespace hpp {
/// Return a shared pointer to a new instance
static RoadmapPtr_t create (const core::DistancePtr_t& distance, const core::DevicePtr_t& robot);
- /// Add a ConstraintSet that creates a foliation.
- void statAddFoliation (ConstraintSetPtr_t constraint);
-
/// Register histogram so that each time a node is added to the roadmap,
/// it is also added to the histogram
void insertHistogram (const graph::HistogramPtr_t hist);
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 6cc0705f..dfbf798c 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -459,12 +459,15 @@ namespace hpp {
GraphComponent::populateTooltip (tp);
tp.addLine ("");
tp.addLine ("Extra numerical constraints are:");
- for (NumericalConstraints_t::const_iterator it = extraNumericalConstraints_.begin ();
- it != extraNumericalConstraints_.end (); ++it) {
+ ConfigProjectorPtr_t param = hist_->foliation().parametrizer();
+ const NumericalConstraints_t& nc = param->numericalConstraints();
+ const LockedJoints_t& lj = param->lockedJoints ();
+ for (NumericalConstraints_t::const_iterator it = nc.begin ();
+ it != nc.end (); ++it) {
tp.addLine ("- " + (*it)->function ().name ());
}
- for (LockedJoints_t::const_iterator it = extraLockedJoints_.begin ();
- it != extraLockedJoints_.end (); ++it) {
+ for (LockedJoints_t::const_iterator it = lj.begin ();
+ it != lj.end (); ++it) {
tp.addLine ("- " + (*it)->jointName ());
}
}
@@ -473,6 +476,10 @@ namespace hpp {
{
// First, get an offset from the histogram
statistics::DiscreteDistribution < RoadmapNodePtr_t > distrib = hist_->getDistrib ();
+ if (distrib.size () == 0) {
+ hppDout (warning, "Edge " << name() << ": Distrib is empty");
+ return false;
+ }
const Configuration_t& qlevelset = *(distrib ()->configuration ());
return applyConstraintsWithOffset (qoffset, qlevelset, q);
@@ -482,6 +489,10 @@ namespace hpp {
{
// 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 ());
+ if (distrib.size () == 0) {
+ hppDout (warning, "Edge " << name() << ": Distrib is empty");
+ return false;
+ }
const Configuration_t& qlevelset = *(distrib ()->configuration ()),
qoffset = *(n_offset->configuration ());
@@ -491,6 +502,7 @@ namespace hpp {
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 ();
@@ -505,6 +517,25 @@ namespace hpp {
it != extraLockedJoints_.end (); ++it) {
(*it)->rightHandSideFromConfig (qlevelset);
}
+ // */
+ //*
+ ConfigProjectorPtr_t param = hist_->foliation().parametrizer();
+ const NumericalConstraints_t& nc = param->numericalConstraints();
+ const LockedJoints_t& lj = param->lockedJoints ();
+
+ // Then, set the offset.
+ ConstraintSetPtr_t cs = extraConfigConstraint ();
+ const ConfigProjectorPtr_t cp = cs->configProjector ();
+ assert (cp);
+ cp->rightHandSideFromConfig (q_offset);
+ for (NumericalConstraints_t::const_iterator it = nc.begin ();
+ it != nc.end (); ++it) {
+ (*it)->rightHandSideFromConfig (levelsetTarget);
+ }
+ for (LockedJoints_t::const_iterator it = lj.begin ();
+ it != lj.end (); ++it) {
+ (*it)->rightHandSideFromConfig (levelsetTarget);
+ } //*/
cp->updateRightHandSide ();
// Eventually, do the projection.
@@ -539,39 +570,55 @@ namespace hpp {
return shPtr;
}
- void LevelSetEdge::buildHistogram ()
+ void LevelSetEdge::histogram (LeafHistogramPtr_t hist)
{
- std::string n = "(" + name () + ")";
+ hist_ = hist;
+ }
+
+ LeafHistogramPtr_t LevelSetEdge::histogram () const
+ {
+ return hist_;
+ }
+
+ void LevelSetEdge::buildExtraConfigConstraint () const
+ {
+ /// First get the numerical constraints
+ ConfigProjectorPtr_t param = hist_->foliation().parametrizer();
+ const NumericalConstraints_t& nc = param->numericalConstraints();
+ const LockedJoints_t& lj = param->lockedJoints ();
+
+ /// Build the constraint set.
+ std::string n = "(" + name () + "_extra)";
GraphPtr_t g = graph_.lock ();
ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
- IntervalsContainer_t::const_iterator itpdof = extraPassiveDofs_.begin ();
- for (NumericalConstraints_t::const_iterator it = extraNumericalConstraints_.begin ();
- it != extraNumericalConstraints_.end (); ++it) {
- proj->add (*it, *itpdof);
- ++itpdof;
- }
-
- for (LockedJoints_t::const_iterator it = extraLockedJoints_.begin ();
- it != extraLockedJoints_.end (); ++it)
+ g->insertNumericalConstraints (proj);
+ for (NumericalConstraints_t::const_iterator it = nc.begin ();
+ it != nc.end (); ++it) {
proj->add (*it);
+ }
+ insertNumericalConstraints (proj);
+ to ()->insertNumericalConstraints (proj);
constraint->addConstraint (proj);
constraint->edge (wkPtr_.lock ());
- hist_ = graph::LeafHistogramPtr_t (new graph::LeafHistogram (constraint));
- }
-
- LeafHistogramPtr_t LevelSetEdge::histogram () const
- {
- return hist_;
+ g->insertLockedJoints (proj);
+ for (LockedJoints_t::const_iterator it = lj.begin ();
+ it != lj.end (); ++it) {
+ proj->add (*it);
+ }
+ insertLockedJoints (proj);
+ to ()->insertLockedJoints (proj);
+ extraConstraints_->set (constraint);
}
ConstraintSetPtr_t LevelSetEdge::extraConfigConstraint () const
{
if (!*extraConstraints_) {
+ /*
std::string n = "(" + name () + "_extra)";
GraphPtr_t g = graph_.lock ();
@@ -600,27 +647,12 @@ namespace hpp {
constraint->edge (wkPtr_.lock ());
extraConstraints_->set (constraint);
+ */
+ buildExtraConfigConstraint ();
}
return extraConstraints_->get ();
}
- void LevelSetEdge::insertConfigConstraint (const NumericalConstraintPtr_t& nm,
- const SizeIntervals_t& passiveDofs)
- {
- extraNumericalConstraints_.push_back (nm);
- extraPassiveDofs_.push_back (passiveDofs);
- }
-
- void LevelSetEdge::insertConfigConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypePtr_t ineq)
- {
- insertConfigConstraint (NumericalConstraint::create (function, ineq));
- }
-
- void LevelSetEdge::insertConfigConstraint (const LockedJointPtr_t lockedJoint)
- {
- extraLockedJoints_.push_back (lockedJoint);
- }
-
LevelSetEdge::LevelSetEdge
(const std::string& name) :
Edge (name), extraConstraints_ (new Constraint_t())
diff --git a/src/graph/statistics.cc b/src/graph/statistics.cc
index 1b473b45..970bbafd 100644
--- a/src/graph/statistics.cc
+++ b/src/graph/statistics.cc
@@ -155,10 +155,15 @@ namespace hpp {
return os << "NodeBin (" << node_->name () << ")";
}
- LeafHistogram::LeafHistogram (const ConstraintSetPtr_t& constraint) :
- constraint_ (constraint), threshold_ (0)
+ LeafHistogramPtr_t LeafHistogram::create (const Foliation f)
{
- ConfigProjectorPtr_t p = constraint_->configProjector ();
+ return LeafHistogramPtr_t (new LeafHistogram (f));
+ }
+
+ LeafHistogram::LeafHistogram (const Foliation f) :
+ f_ (f), threshold_ (0)
+ {
+ ConfigProjectorPtr_t p = f_.parametrizer ();
if (p) {
threshold_ = p->errorThreshold () /
sqrt((double)p->rightHandSide ().size ());
@@ -167,15 +172,9 @@ namespace hpp {
void LeafHistogram::add (const RoadmapNodePtr_t& n)
{
- iterator it;
- if (constraint_->configProjector ()) {
- it = insert
- (LeafBin (constraint_->configProjector ()->rightHandSideFromConfig
- (*n->configuration ()),
- &threshold_));
- } else {
- it = insert (LeafBin (vector_t (), &threshold_));
- }
+ if (!f_.contains (*n->configuration())) return;
+ iterator it = insert (LeafBin (f_.parameter (*n->configuration()),
+ &threshold_));
it->push_back (n);
if (numberOfObservations()%10 == 0) {
hppDout (info, *this);
@@ -184,19 +183,13 @@ namespace hpp {
std::ostream& LeafHistogram::print (std::ostream& os) const
{
- os << "Histogram contains ConstraintSet: "
- << constraint_->name () << std::endl;
+ os << "Leaf Histogram of foliation " << f_.condition()->name() << std::endl;
return Parent::print (os);
}
- const ConstraintSetPtr_t& LeafHistogram::constraint () const
- {
- return constraint_;
- }
-
HistogramPtr_t LeafHistogram::clone () const
{
- return HistogramPtr_t (new LeafHistogram (constraint_));
+ return HistogramPtr_t (new LeafHistogram (f_));
}
NodeHistogram::NodeHistogram (const graph::GraphPtr_t& graph) :
diff --git a/src/roadmap.cc b/src/roadmap.cc
index 1fb44ec6..9b4dcd41 100644
--- a/src/roadmap.cc
+++ b/src/roadmap.cc
@@ -58,11 +58,6 @@ namespace hpp {
}
}
- void Roadmap::statAddFoliation (ConstraintSetPtr_t constraint)
- {
- insertHistogram (graph::HistogramPtr_t (new graph::LeafHistogram (constraint)));
- }
-
void Roadmap::insertHistogram (const graph::HistogramPtr_t hist)
{
histograms_.push_back (hist);
--
GitLab