diff --git a/include/hpp/manipulation/graph/helper.hh b/include/hpp/manipulation/graph/helper.hh
index 2455440371b2e752b32b83644bae2070d251b5e0..c3b7b832f5eaa9b2ed3319d1e499e21011d8b28e 100644
--- a/include/hpp/manipulation/graph/helper.hh
+++ b/include/hpp/manipulation/graph/helper.hh
@@ -20,6 +20,8 @@
# include <string>
# include <algorithm>
+# include <boost/tuple/tuple.hpp>
+
# include "hpp/manipulation/config.hh"
# include "hpp/manipulation/fwd.hh"
# include "hpp/manipulation/graph/fwd.hh"
@@ -111,6 +113,59 @@ namespace hpp {
const bool levelSetGrasp, const bool levelSetPlace,
const FoliatedManifold& submanifoldDef = FoliatedManifold ()
);
+
+ /// Create a waypoint edge taking into account:
+ /// \li grasp
+ /// \li placement
+ /// \li preplacement
+
+ /// Create a waypoint edge taking into account
+ /// \li grasp
+ /// \li pregrasp
+ /// \li placement
+
+ /// \todo when the handle is a free flying object, add the robot DOFs
+ /// as passive dofs to the numerical constraints for paths
+ void graspManifold (
+ const GripperPtr_t& gripper, const HandlePtr_t& handle,
+ FoliatedManifold& grasp, FoliatedManifold& pregrasp);
+
+ /// The placement foliation constraint is built using
+ /// hpp::constraints::ConvexShapeMatcherComplement
+ void strictPlacementManifold (
+ const NumericalConstraintPtr_t placement,
+ const NumericalConstraintPtr_t preplacement,
+ const NumericalConstraintPtr_t placementComplement,
+ FoliatedManifold& place, FoliatedManifold& preplace);
+
+ /// The placement foliation constraint is built locked joints
+ /// It is faster than strictPlacementManifold but the foliation
+ /// parametrisation is redundant.
+ void relaxedPlacementManifold (
+ const NumericalConstraintPtr_t placement,
+ const NumericalConstraintPtr_t preplacement,
+ const LockedJoints_t objectLocks,
+ FoliatedManifold& place, FoliatedManifold& preplace);
+
+ typedef boost::tuple <NumericalConstraintPtr_t,
+ NumericalConstraintPtr_t,
+ LockedJoints_t>
+ PlacementConstraint_t;
+ typedef std::vector <HandlePtr_t> Handles_t;
+ typedef std::vector <GripperPtr_t> Grippers_t;
+ typedef boost::tuple <PlacementConstraint_t, Handles_t> Object_t;
+ typedef std::vector <Object_t> Objects_t;
+
+ /// Fill a Graph
+ ///
+ /// \note It is assumed that a gripper can grasp only one handle and each
+ /// handle cannot be grasped by several grippers at the same time.
+ ///
+ /// \param[in,out] graph must be an initialized empty Graph.
+ void graphBuilder (
+ const Objects_t& objects,
+ const Grippers_t& grippers,
+ GraphPtr_t graph);
/// \}
} // namespace helper
} // namespace graph
diff --git a/src/graph/helper.cc b/src/graph/helper.cc
index 41cf89775d2ba15b8601c33e32d9fbbbea6fc0cc..0d982688f3edc51cdcdfded2c47e2cb233efeb05 100644
--- a/src/graph/helper.cc
+++ b/src/graph/helper.cc
@@ -18,10 +18,16 @@
#include <hpp/util/debug.hh>
+#include <hpp/model/gripper.hh>
+
+#include <hpp/constraints/differentiable-function.hh>
+
#include <hpp/manipulation/graph/node.hh>
#include <hpp/manipulation/graph/edge.hh>
#include <hpp/manipulation/graph/node-selector.hh>
+#include <hpp/manipulation/handle.hh>
+
namespace hpp {
namespace manipulation {
namespace graph {
@@ -429,6 +435,126 @@ namespace hpp {
return std::make_pair (eForw, eBack);
}
+
+ void graspManifold (
+ const GripperPtr_t& gripper, const HandlePtr_t& handle,
+ FoliatedManifold& grasp, FoliatedManifold& pregrasp)
+ {
+ NumericalConstraintPtr_t gc = handle->createGrasp (gripper);
+ grasp.nc.nc.push_back (gc);
+ grasp.nc.pdof.push_back (SizeIntervals_t ());
+ grasp.nc_path.nc.push_back (gc);
+ // TODO: see function declaration
+ grasp.nc_path.pdof.push_back (SizeIntervals_t ());
+ NumericalConstraintPtr_t gcc = handle->createGraspComplement (gripper);
+ if (gcc->function ().outputSize () > 0) {
+ grasp.nc_fol.nc.push_back (gcc);
+ grasp.nc_fol.pdof.push_back (SizeIntervals_t());
+ }
+
+ const value_type c = handle->clearance () + gripper->clearance ();
+ NumericalConstraintPtr_t pgc = handle->createPreGrasp (gripper, c);
+ pregrasp.nc.nc.push_back (pgc);
+ pregrasp.nc.pdof.push_back (SizeIntervals_t());
+ pregrasp.nc_path.nc.push_back (pgc);
+ pregrasp.nc_path.pdof.push_back (SizeIntervals_t());
+ }
+
+ void strictPlacementManifold (
+ const NumericalConstraintPtr_t placement,
+ const NumericalConstraintPtr_t preplacement,
+ const NumericalConstraintPtr_t placementComplement,
+ FoliatedManifold& place, FoliatedManifold& preplace)
+ {
+ place.nc.nc.push_back (placement);
+ place.nc.pdof.push_back (SizeIntervals_t());
+ place.nc_path.nc.push_back (placement);
+ place.nc_path.pdof.push_back (SizeIntervals_t());
+ if (placementComplement && placementComplement->function().outputSize () > 0) {
+ place.nc_fol.nc.push_back (placementComplement);
+ place.nc_fol.pdof.push_back (SizeIntervals_t());
+ }
+
+ preplace.nc.nc.push_back (preplacement);
+ preplace.nc.pdof.push_back (SizeIntervals_t());
+ preplace.nc_path.nc.push_back (preplacement);
+ preplace.nc_path.pdof.push_back (SizeIntervals_t());
+ }
+
+ void relaxedPlacementManifold (
+ const NumericalConstraintPtr_t placement,
+ const NumericalConstraintPtr_t preplacement,
+ const LockedJoints_t objectLocks,
+ FoliatedManifold& place, FoliatedManifold& preplace)
+ {
+ place.nc.nc.push_back (placement);
+ place.nc.pdof.push_back (SizeIntervals_t());
+ place.nc_path.nc.push_back (placement);
+ place.nc_path.pdof.push_back (SizeIntervals_t());
+ std::copy (objectLocks.begin(), objectLocks.end(), place.lj_fol.end());
+
+ preplace.nc.nc.push_back (preplacement);
+ preplace.nc.pdof.push_back (SizeIntervals_t());
+ preplace.nc_path.nc.push_back (preplacement);
+ preplace.nc_path.pdof.push_back (SizeIntervals_t());
+ }
+
+ typedef std::vector <std::size_t> IndexV_t;
+ typedef std::list <std::size_t> IndexL_t;
+ typedef std::pair <std::size_t, std::size_t> Grasp_t;
+ typedef std::vector <std::size_t, std::size_t> GraspV_t;
+
+ void makeEdge (const Grippers_t& grippers, const IndexV_t& idx_g,
+ const Objects_t& objects, const IndexV_t& idx_oh,
+ const GraspV_t& grasps
+
+ /// idx are the available grippers
+ void recurseGrippers (const Grippers_t& grippers, const IndexV_t& idx_g,
+ const Objects_t& objects, const IndexV_t& idx_oh,
+ const GraspV_t& grasps)
+ {
+ for (IndexV_t::const_iterator itx_g = idx_g.begin ();
+ itx_g != idx_g.end (); ++itx_g) {
+ IndexV_t nIdx_g (idx_g.size() - 1);
+ // Copy all element except itx_g
+ std::copy (boost::next (itx_g), idx_g.end (),
+ std::copy (idx_g.begin (), itx_g, nIdx_g.begin ())
+ );
+ for (IndexV_t::const_iterator itx_oh = idx_oh.begin ();
+ itx_oh != idx_oh.end (); ++itx_oh) {
+ IndexV_t nIdx_oh (idx_oh.size() - 1);
+ // Copy all element except itx_oh
+ std::copy (boost::next (itx_oh), idx_oh.end (),
+ std::copy (idx_oh.begin (), itx_oh, nIdx_oh.begin ())
+ );
+ GraspV_t nGrasps = grasps;
+ grasps.push_back (Grasp_t (*itx_g, *itx_oh));
+ makeEdge (grippers, objects, grasps, nGrasps);
+ recurseGrippers (grippers, nIdx_oh, objects, nIdx_oh, nGrasps);
+ }
+ }
+ }
+
+ void graphBuilder (
+ const Objects_t& objects,
+ const Grippers_t& grippers,
+ GraphPtr_t graph)
+ {
+ if (!graph) throw std::logic_error ("The graph must be initialized");
+ NodeSelectorPtr_t ns = graph->nodeSelector ();
+ if (!ns) throw std::logic_error ("The graph does not have a NodeSelector");
+
+ // Max number of handles that can be grasped
+ std::size_t nGrippers = grippers.size ();
+ for (Objects_t::const_iterator begin = objects.begin ();
+ begin != objects.end (); ++begin) {
+ for (
+ }
+ for (Grippers_t::const_iterator begin = grippers.begin ();
+ begin != grippers.end (); ++begin) {
+ }
+ }
+ } // namespace helper
} // namespace graph
} // namespace manipulation
} // namespace hpp