diff --git a/doc/constraint-graph.png b/doc/constraint-graph.png
new file mode 100644
index 0000000000000000000000000000000000000000..c28a8da7e529fc2e9ccc490756027554127f1c53
Binary files /dev/null and b/doc/constraint-graph.png differ
diff --git a/include/hpp/manipulation/graph/edge.hh b/include/hpp/manipulation/graph/edge.hh
index 40403654678c9b80740950c2875bf8b37fe5a200..d69dcd0277768470588eb0de89eb74b6fa1c90c2 100644
--- a/include/hpp/manipulation/graph/edge.hh
+++ b/include/hpp/manipulation/graph/edge.hh
@@ -376,7 +376,78 @@ namespace hpp {
WaypointEdgeWkPtr_t wkPtr_;
}; // class WaypointEdge
- /// Edge that find intersection of level set.
+ /// Edge that handles crossed foliations
+ ///
+ /// Let us consider the following simple constraint graph
+ /// corresponding to a robot grasping an object with one gripper.
+ ///
+ /// \image html constraint-graph.png "Simple constraint graph corresponding to a robot grasping an object."
+ ///
+ /// In order to disambiguate, we assume here that
+ /// \li transition <b>Grasp object</b> is in \b Placement state,
+ /// \li transition <b>Release object</b> is in \b Grasp state.
+ ///
+ /// If state \b Placement is defined by the object lying on a planar
+ /// polygonal surface, then
+ /// \li state \b Placement,
+ /// \li transition \b Transit, and
+ /// \li transition <b>Grasp object</b>
+ ///
+ /// are all constrained in a foliated manifold parameterized by the
+ /// position of the box on the surface.
+ ///
+ /// Likewise, if the object is cylindrical the grasp may have a degree
+ /// of freedom corresponding to the angle around z-axis of the gripper
+ /// with respect to the object. See classes
+ /// \link hpp::manipulation::Handle Handle\endlink and
+ /// \link hpp::pinocchio::Gripper Gripper\endlink for details.
+ /// In this latter case,
+ /// \li state \b Grasp,
+ /// \li transition \b Transfer, and
+ /// \li transition <b>Release object</b>
+ ///
+ /// are all constrained in a foliated manifold parameterized by the
+ /// angle around z-axis of the gripper with respect to the object.
+ ///
+ /// Let us denote
+ /// \li \c grasp the numerical constraint defining state \b Grasp,
+ /// \li \c placement the numerical constraint defining state \b Placement,
+ /// \li \c grasp_comp the parameterized constraint defining a leaf
+ /// of \c Transfer (the angle between the gripper and the
+ /// object),
+ /// \li \c placement_comp the parameterized constraint defining a leaf
+ /// of \b Placement (the position of the object on the contact
+ /// surface).
+ ///
+ /// As explained in <a
+ /// href="https://hal.archives-ouvertes.fr/hal-01358767">this
+ /// paper </a>, we are in the crossed foliation case and manipulation RRT
+ /// will never be able to connect trees expanding in different leaves of
+ /// the foliation.
+ ///
+ /// This class solves this issue in the following way by creating an
+ /// instance of LevelSetEdge between \b Placement and \b Grasp.
+ ///
+ /// When extending a configuration \f$\mathbf{q}_{start}\f$ in state
+ /// \b Placement, this transition will produce a target configuration
+ /// (method \link LevelSetEdge::generateTargetConfig generateTargetConfig)
+ /// \endlink as follows.
+ ///
+ /// \li pick a random configuration \f$\mathbf{q}_rand\f$, in the edge
+ /// histogram (see method \link LevelSetEdge::histogram histogram\endlink)
+ /// \li compute right hand side of \c grasp_comp with
+ /// \f$\mathbf{q}_{rand}\f$,
+ /// \li compute right hand side of \c placement_comp with
+ /// \f$\mathbf{q}_{start}\f$,
+ /// \li solve (\c grasp, \c placement, \c placement_comp, \c grasp_comp)
+ /// using input configuration \f$\mathbf{q}\f$. Note that the
+ /// parent method Edge::generateTargetConfig does the same without
+ /// adding \c grasp_comp.
+ ///
+ /// The constraints parameterizing the target state foliation
+ /// (\c graps_comp in our example) are passed to class instances
+ /// using method \link LevelSetEdge::insertParamConstraint
+ /// insertParamConstraint\endlink.
class HPP_MANIPULATION_DLLAPI LevelSetEdge : public Edge
{
public:
@@ -443,18 +514,39 @@ namespace hpp {
/// Build path and target state constraints
virtual ConstraintSetPtr_t buildTargetConstraint();
+ /// Build the histogram
+ /// \sa LevelSetEdge::histogram.
void buildHistogram ();
+ /// Return pointer on histogram of the edge
+ ///
+ /// The edge histogram is a container of configurations defined by
+ /// a set of constraints called the <b>condition constraints</b>
+ /// that a configuration should satisfy to be inserted in the
+ /// histogram.
+ ///
+ /// The histogram is passed to the Roadmap via the graph (method
+ /// Graph::insertHistogram). The roadmap then populates the histogram
+ /// with all new configurations satisfying the condition constraints.
+ ///
+ /// The condition constraints should therefore be the constraints of
+ /// the target state of the level set edge.
+ ///
+ /// \sa LevelSetEdge::insertConditionConstraint
LeafHistogramPtr_t histogram () const;
/// \name Foliation definition
/// \{
- /// Insert a numerical constraint that parametrizes the foliation
+ /// Insert a constraints parameterizing the target state foliation
+ /// \param nm the numerical constraint,
+ /// \param passiveDofs the passive degrees of freedom of the
+ /// constraint.
void insertParamConstraint (const ImplicitPtr_t& nm,
const segments_t& passiveDofs = segments_t ());
- /// Insert a numerical constraint that defines the foliation
+ /// Insert a condition constraint
+ /// \sa LevelSetEdge::histogram
void insertConditionConstraint (const ImplicitPtr_t& nm,
const segments_t& passiveDofs = segments_t ());
diff --git a/src/graph/edge.cc b/src/graph/edge.cc
index 6545f9941e891df1a3e3da3ae069f1d5e196da9b..d59e75beec7b5ce73284aefe3855f09e099cc858 100644
--- a/src/graph/edge.cc
+++ b/src/graph/edge.cc
@@ -203,7 +203,7 @@ namespace hpp {
return targetConstraints_;
}
- // Merge constraints of several graph components into a config projectors
+ // Merge constraints of several graph components into a config projector
// Replace constraints and complement by combination of both when
// necessary.
static void mergeConstraintsIntoConfigProjector
@@ -289,6 +289,11 @@ namespace hpp {
if (state () != stateTo ()) {
components.push_back (state ());
}
+ // Copy constraints from
+ // - graph,
+ // - this edge,
+ // - the destination state,
+ // - the state in which the transition lies if different
mergeConstraintsIntoConfigProjector (proj, components, parentGraph ());
constraint->addConstraint (proj);
@@ -654,7 +659,10 @@ namespace hpp {
const ConfigProjectorPtr_t cp = cs->configProjector ();
assert (cp);
+ // Set right hand side of edge constraints with qStart
cp->rightHandSideFromConfig (qStart);
+ // Set right hand side of constraints parameterizing the target state
+ // foliation with qLeaf.
for (NumericalConstraints_t::const_iterator it =
paramNumericalConstraints_.begin ();
it != paramNumericalConstraints_.end (); ++it) {
@@ -778,6 +786,13 @@ namespace hpp {
ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
+ // Copy constraints from
+ // - graph,
+ // - param numerical constraints
+ // - this edge,
+ // - the destination state,
+ // - the state in which the transition lies if different
+
g->insertNumericalConstraints (proj);
IntervalsContainer_t::const_iterator itpdof = paramPassiveDofs_.begin ();
for (NumericalConstraints_t::const_iterator it = paramNumericalConstraints_.begin ();