diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index d430efb4d977bc57e13369e1faf79aced12db69d..7c78acfcaf0504c54db98ce50d37b13559f8a064 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -60,7 +60,7 @@ namespace hpp {
{
public:
/// Destructor
- ~Edge ();
+ virtual ~Edge ();
/// Create a new empty Edge.
static EdgePtr_t create
@@ -256,7 +256,7 @@ namespace hpp {
class HPP_MANIPULATION_DLLAPI LevelSetEdge : public Edge
{
public:
- ~LevelSetEdge ();
+ virtual ~LevelSetEdge ();
/// Create a new LevelSetEdge.
static LevelSetEdgePtr_t create
@@ -268,17 +268,36 @@ namespace hpp {
virtual bool applyConstraints (core::NodePtr_t n_offset, ConfigurationOut_t q) const;
- void histogram (LeafHistogramPtr_t hist);
+ void buildHistogram ();
LeafHistogramPtr_t histogram () const;
- void insertConfigConstraint (const NumericalConstraintPtr_t& nm,
+ /// \name Foliation definition
+ /// \{
+
+ /// Insert a NumericalConstraint that parametrizes the foliation
+ void insertParamConstraint (const NumericalConstraintPtr_t& nm,
const SizeIntervals_t& passiveDofs = SizeIntervals_t ());
- void insertConfigConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypePtr_t ineq)
+ void insertParamConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypePtr_t ineq)
HPP_MANIPULATION_DEPRECATED;
- void insertConfigConstraint (const LockedJointPtr_t lockedJoint);
+ /// Insert a LockedJoint that parametrizes the foliation
+ void insertParamConstraint (const LockedJointPtr_t lockedJoint);
+
+ /// Insert a NumericalConstraint that defines the foliation
+ ///
+ /// The manifold represented the foliation is defined by this
+ /// constraints.
+ void insertConditionConstraint (const NumericalConstraintPtr_t& nm,
+ const SizeIntervals_t& passiveDofs = SizeIntervals_t ());
+
+ /// Insert a LockedJoint that defines the foliation
+ ///
+ /// The manifold represented the foliation is defined by this
+ /// constraints.
+ void insertConditionConstraint (const LockedJointPtr_t lockedJoint);
+ /// \}
/// Print the object in a stream.
virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
@@ -300,12 +319,19 @@ namespace hpp {
bool applyConstraintsWithOffset (ConfigurationIn_t qoffset,
ConfigurationIn_t qlevelset, ConfigurationOut_t q) const;
- typedef Cache < ConstraintSetPtr_t > Constraint_t;
-
- /// See pathConstraint member function.
- Constraint_t* extraConstraints_;
- ConstraintSetPtr_t extraConfigConstraint () const;
- void buildExtraConfigConstraint () const;
+ // Parametrizer
+ // NumericalConstraints_t
+ NumericalConstraints_t paramNumericalConstraints_;
+ IntervalsContainer_t paramPassiveDofs_;
+ // LockedJoints_t
+ LockedJoints_t paramLockedJoints_;
+
+ // Condition
+ // NumericalConstraints_t
+ NumericalConstraints_t condNumericalConstraints_;
+ IntervalsContainer_t condPassiveDofs_;
+ // LockedJoints_t
+ LockedJoints_t condLockedJoints_;
/// 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 06005935a78b79e79056f534a5e7d3b68e018e48..46e19eb50f9a61b1fa4387600ce4ff0028eea7ef 100644
--- a/include/hpp/manipulation/graph/statistics.hh
+++ b/include/hpp/manipulation/graph/statistics.hh
@@ -117,24 +117,17 @@ namespace hpp {
/// Whether the configuration is the submanifold $\mathcal{M}$
vector_t parameter (ConfigurationIn_t q) const;
- void condition (const ConfigProjectorPtr_t c);
- ConfigProjectorPtr_t condition () const;
- void parametrizer (const ConfigProjectorPtr_t p);
- ConfigProjectorPtr_t parametrizer () const;
+ void condition (const ConstraintSetPtr_t c);
+ ConstraintSetPtr_t condition () const;
+ void parametrizer (const ConstraintSetPtr_t p);
+ ConstraintSetPtr_t parametrizer () const;
private:
// condition_ contains the constraints defining the submanifold
// containing all the leaf.
// parametrizer_ contains the constraints providing a parametrization
// of the foliation.
- //struct {
- // NumericalConstraints_t nc;
- // This should be not be used as we are never resolving equations.
- // We are only testing if they are equations are true or false.
- // IntervalsContainer_t pdofs;
- // LockedJoints_t lj;
- //} condition_, parametrizer_;
- ConfigProjectorPtr_t condition_, parametrizer_;
+ ConstraintSetPtr_t condition_, parametrizer_;
};
class HPP_MANIPULATION_DLLAPI LeafHistogram : public ::hpp::statistics::Statistics < LeafBin >
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index dfbf798c6122476140d4958cc2ce66299d28f5f8..78fad10c7fac174d6e8f00cb17fa6be5dd725406 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -458,16 +458,22 @@ namespace hpp {
{
GraphComponent::populateTooltip (tp);
tp.addLine ("");
- tp.addLine ("Extra numerical constraints are:");
- 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 ("Foliation condition constraints:");
+ for (NumericalConstraints_t::const_iterator it = condNumericalConstraints_.begin ();
+ it != condNumericalConstraints_.end (); ++it) {
tp.addLine ("- " + (*it)->function ().name ());
}
- for (LockedJoints_t::const_iterator it = lj.begin ();
- it != lj.end (); ++it) {
+ for (LockedJoints_t::const_iterator it = condLockedJoints_.begin ();
+ it != condLockedJoints_.end (); ++it) {
+ tp.addLine ("- " + (*it)->jointName ());
+ }
+ tp.addLine ("Foliation parametrization constraints:");
+ for (NumericalConstraints_t::const_iterator it = paramNumericalConstraints_.begin ();
+ it != paramNumericalConstraints_.end (); ++it) {
+ tp.addLine ("- " + (*it)->function ().name ());
+ }
+ for (LockedJoints_t::const_iterator it = paramLockedJoints_.begin ();
+ it != paramLockedJoints_.end (); ++it) {
tp.addLine ("- " + (*it)->jointName ());
}
}
@@ -502,40 +508,20 @@ namespace hpp {
bool LevelSetEdge::applyConstraintsWithOffset (ConfigurationIn_t qoffset,
ConfigurationIn_t qlevelset, ConfigurationOut_t q) const
{
- /*
// First, set the offset.
- ConstraintSetPtr_t cs = extraConfigConstraint ();
+ ConstraintSetPtr_t cs = hist_->foliation().parametrizer ();
const ConfigProjectorPtr_t cp = cs->configProjector ();
assert (cp);
cp->rightHandSideFromConfig (qoffset);
for (NumericalConstraints_t::const_iterator it =
- extraNumericalConstraints_.begin ();
- it != extraNumericalConstraints_.end (); ++it) {
+ paramNumericalConstraints_.begin ();
+ it != paramNumericalConstraints_.end (); ++it) {
(*it)->rightHandSideFromConfig (qlevelset);
}
- for (LockedJoints_t::const_iterator it = extraLockedJoints_.begin ();
- it != extraLockedJoints_.end (); ++it) {
+ for (LockedJoints_t::const_iterator it = paramLockedJoints_.begin ();
+ it != paramLockedJoints_.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.
@@ -570,99 +556,104 @@ namespace hpp {
return shPtr;
}
- void LevelSetEdge::histogram (LeafHistogramPtr_t hist)
- {
- hist_ = hist;
- }
-
LeafHistogramPtr_t LevelSetEdge::histogram () const
{
return hist_;
}
- void LevelSetEdge::buildExtraConfigConstraint () const
+ void LevelSetEdge::buildHistogram ()
{
- /// First get the numerical constraints
- ConfigProjectorPtr_t param = hist_->foliation().parametrizer();
- const NumericalConstraints_t& nc = param->numericalConstraints();
- const LockedJoints_t& lj = param->lockedJoints ();
+ Foliation f;
/// Build the constraint set.
- std::string n = "(" + name () + "_extra)";
+ std::string n = "(" + name () + ")";
GraphPtr_t g = graph_.lock ();
- ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
+ // The parametrizer
+ ConstraintSetPtr_t param = ConstraintSet::create (g->robot (), "Set " + n);
- ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
+ ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "projParam_" + n, g->errorThreshold(), g->maxIterations());
g->insertNumericalConstraints (proj);
- for (NumericalConstraints_t::const_iterator it = nc.begin ();
- it != nc.end (); ++it) {
- proj->add (*it);
+ IntervalsContainer_t::const_iterator itpdof = paramPassiveDofs_.begin ();
+ for (NumericalConstraints_t::const_iterator it = paramNumericalConstraints_.begin ();
+ it != paramNumericalConstraints_.end (); ++it) {
+ proj->add (*it, *itpdof);
+ ++itpdof;
}
+ assert (itpdof == paramPassiveDofs_.end ());
insertNumericalConstraints (proj);
to ()->insertNumericalConstraints (proj);
- constraint->addConstraint (proj);
- constraint->edge (wkPtr_.lock ());
+ param->addConstraint (proj);
+ param->edge (wkPtr_.lock ());
g->insertLockedJoints (proj);
- for (LockedJoints_t::const_iterator it = lj.begin ();
- it != lj.end (); ++it) {
+ for (LockedJoints_t::const_iterator it = paramLockedJoints_.begin ();
+ it != paramLockedJoints_.end (); ++it) {
proj->add (*it);
}
insertLockedJoints (proj);
to ()->insertLockedJoints (proj);
- extraConstraints_->set (constraint);
+
+ f.parametrizer (param);
+
+ // The codition
+ ConstraintSetPtr_t cond = ConstraintSet::create (g->robot (), "Set " + n);
+ proj = ConfigProjector::create(g->robot(), "projCond_" + n, g->errorThreshold(), g->maxIterations());
+ itpdof = condPassiveDofs_.begin ();
+ for (NumericalConstraints_t::const_iterator it = condNumericalConstraints_.begin ();
+ it != condNumericalConstraints_.end (); ++it) {
+ proj->add (*it, *itpdof);
+ ++itpdof;
+ }
+ assert (itpdof == condPassiveDofs_.end ());
+ for (LockedJoints_t::const_iterator it = condLockedJoints_.begin ();
+ it != condLockedJoints_.end (); ++it) {
+ proj->add (*it);
+ }
+ f.condition (cond);
+ cond->addConstraint (proj);
+
+ hist_ = LeafHistogram::create (f);
}
- ConstraintSetPtr_t LevelSetEdge::extraConfigConstraint () const
+ void LevelSetEdge::insertParamConstraint (const NumericalConstraintPtr_t& nm,
+ const SizeIntervals_t& passiveDofs)
{
- if (!*extraConstraints_) {
- /*
- std::string n = "(" + name () + "_extra)";
- GraphPtr_t g = graph_.lock ();
+ paramNumericalConstraints_.push_back (nm);
+ paramPassiveDofs_.push_back (passiveDofs);
+ }
- ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
+ void LevelSetEdge::insertParamConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypePtr_t ineq)
+ {
+ insertParamConstraint (NumericalConstraint::create (function, ineq));
+ }
- ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
- g->insertNumericalConstraints (proj);
- IntervalsContainer_t::const_iterator itpdof = extraPassiveDofs_.begin ();
- for (NumericalConstraints_t::const_iterator it = extraNumericalConstraints_.begin ();
- it != extraNumericalConstraints_.end (); ++it) {
- proj->add (*it, *itpdof);
- ++itpdof;
- }
- assert (itpdof == extraPassiveDofs_.end ());
- insertNumericalConstraints (proj);
- to ()->insertNumericalConstraints (proj);
- constraint->addConstraint (proj);
-
- g->insertLockedJoints (proj);
- for (LockedJoints_t::const_iterator it = extraLockedJoints_.begin ();
- it != extraLockedJoints_.end (); ++it) {
- proj->add (*it);
- }
- insertLockedJoints (proj);
- to ()->insertLockedJoints (proj);
+ void LevelSetEdge::insertParamConstraint (const LockedJointPtr_t lockedJoint)
+ {
+ paramLockedJoints_.push_back (lockedJoint);
+ }
- constraint->edge (wkPtr_.lock ());
- extraConstraints_->set (constraint);
- */
- buildExtraConfigConstraint ();
- }
- return extraConstraints_->get ();
+ void LevelSetEdge::insertConditionConstraint (const NumericalConstraintPtr_t& nm,
+ const SizeIntervals_t& passiveDofs)
+ {
+ condNumericalConstraints_.push_back (nm);
+ condPassiveDofs_.push_back (passiveDofs);
+ }
+
+ void LevelSetEdge::insertConditionConstraint (const LockedJointPtr_t lockedJoint)
+ {
+ condLockedJoints_.push_back (lockedJoint);
}
LevelSetEdge::LevelSetEdge
(const std::string& name) :
- Edge (name), extraConstraints_ (new Constraint_t())
+ Edge (name)
{
}
LevelSetEdge::~LevelSetEdge ()
- {
- if (extraConstraints_ ) delete extraConstraints_;
- }
+ {}
} // namespace graph
} // namespace manipulation
} // namespace hpp
diff --git a/src/graph/statistics.cc b/src/graph/statistics.cc
index 970bbafdf9689021e9e6d140e3a7e8c6f651c6a0..b4ee42c1d7f9922cb97e5dfecd0ee26afab2a390 100644
--- a/src/graph/statistics.cc
+++ b/src/graph/statistics.cc
@@ -163,7 +163,7 @@ namespace hpp {
LeafHistogram::LeafHistogram (const Foliation f) :
f_ (f), threshold_ (0)
{
- ConfigProjectorPtr_t p = f_.parametrizer ();
+ ConfigProjectorPtr_t p = f_.parametrizer ()->configProjector();
if (p) {
threshold_ = p->errorThreshold () /
sqrt((double)p->rightHandSide ().size ());
@@ -270,25 +270,25 @@ namespace hpp {
if (!condition_->isSatisfied (q)) {
hppDout (error, "Configuration not in the foliation");
}
- return parametrizer_->rightHandSideFromConfig (q);
+ return parametrizer_->configProjector()->rightHandSideFromConfig (q);
}
- ConfigProjectorPtr_t Foliation::condition () const
+ ConstraintSetPtr_t Foliation::condition () const
{
return condition_;
}
- void Foliation::condition (const ConfigProjectorPtr_t c)
+ void Foliation::condition (const ConstraintSetPtr_t c)
{
condition_ = c;
}
- ConfigProjectorPtr_t Foliation::parametrizer () const
+ ConstraintSetPtr_t Foliation::parametrizer () const
{
return parametrizer_;
}
- void Foliation::parametrizer (const ConfigProjectorPtr_t p)
+ void Foliation::parametrizer (const ConstraintSetPtr_t p)
{
parametrizer_ = p;
}