helper.cc

                std::copy (boost::next (itx_oh), idx_oh.end (),
                    std::copy (idx_oh.begin (), itx_oh, nIdx_oh.begin ())
                    );
                // Do all the possible combination below this new grasp
                recurseGrippers (r, nIdx_g, nIdx_oh, nGrasps, depth + 2);
              }
            }
          }
        }

        void graphBuilder (
            const ProblemSolverPtr_t& ps,
            const Objects_t& objects,
            const Grippers_t& grippers,
            GraphPtr_t graph,
            const Rules_t& rules)
        {
          if (!graph) throw std::logic_error ("The graph must be initialized");
          StateSelectorPtr_t ns = graph->stateSelector ();
          if (!ns) throw std::logic_error ("The graph does not have a StateSelector");

          Result r (ps, grippers, objects, graph);
          r.setRules (rules);

          IndexV_t availG (r.nG), availOH (r.nOH);
          for (index_t i = 0; i < r.nG; ++i) availG[i] = i;
          for (index_t i = 0; i < r.nOH; ++i) availOH[i] = i;

          GraspV_t iG (r.nG, r.nOH);

          recurseGrippers (r, availG, availOH, iG, 0);

          hppDout (info, "Created a graph with " << r.states.size() << " states "
              "and " << r.edges.size() << " edges.");
        }

        GraphPtr_t graphBuilder (
            const ProblemSolverPtr_t& ps,
            const std::string& graphName,
            const StringList_t& griNames,
            const std::list <ObjectDef_t>& objs,
            const StringList_t& envNames,
	    const std::vector <Rule>& rules,
            const value_type& prePlaceWidth)
        {
          const Device& robot = *(ps->robot ());
          const pinocchio::Model& model = robot.model();
          Grippers_t grippers (griNames.size());
          index_t i = 0;
          BOOST_FOREACH (const std::string& gn, griNames) {
            grippers[i] = robot.grippers.get (gn);
            ++i;
          }
          Objects_t objects (objs.size());
          i = 0;
          const value_type margin = 1e-3;
          bool prePlace = (prePlaceWidth > 0);
          BOOST_FOREACH (const ObjectDef_t& od, objs) {
            // Create handles
            objects[i].get<2> () = i;
            objects[i].get<1> ().resize (od.handles.size());
            Handles_t::iterator it = objects[i].get<1> ().begin();
            BOOST_FOREACH (const std::string hn, od.handles) {
              *it = robot.handles.get (hn);
              ++it;
            }
            // Create placement
            const std::string placeN = "place_" + od.name;
            const std::string preplaceN = "pre" + placeN;
            // If user provides constraint "place_objectName",
            // then
            //   use this as placement and use "preplace_objectName" for
            //   pre-placement if defined.
            // else if contact surfaces are defined and selected
            //   create default placement constraint using the ProblemSolver
            //   methods createPlacementConstraint and createPrePlacementConstraint
            if (ps->numericalConstraints.has(placeN)) {
              objects[i].get<0> ().get<0> () =
                ps->numericalConstraints.get (placeN);
              if (ps->numericalConstraints.has(preplaceN)) {
                objects[i].get<0> ().get<1> () =
                  ps->numericalConstraints.get (preplaceN);
              }
            } else if (!envNames.empty() && !od.shapes.empty ()) {
              ps->createPlacementConstraint (placeN,
                  od.shapes, envNames, margin);
              objects[i].get<0> ().get<0> () =
                ps->numericalConstraints.get (placeN);
              if (prePlace) {
                ps->createPrePlacementConstraint (preplaceN,
                    od.shapes, envNames, margin, prePlaceWidth);
                objects[i].get<0> ().get<1> () =
                  ps->numericalConstraints.get (preplaceN);
              }
            }
            // Create object lock
	    // Loop over all frames of object, detect joint and create locked
	    // joint.
            assert (robot.frameIndices.has (od.name));
            BOOST_FOREACH (const se3::FrameIndex& f, robot.frameIndices.get (od.name)) {
              if (model.frames[f].type != se3::JOINT) continue;
              const JointIndex j = model.frames[f].parent;
              JointPtr_t oj (new Joint (ps->robot(), j));
              LiegroupSpacePtr_t space (oj->configurationSpace ());
              LiegroupElement lge (robot.currentConfiguration()
                                   .segment (oj->rankInConfiguration (),
                                             oj->configSize ()), space);
              LockedJointPtr_t lj = core::LockedJoint::create (oj, lge);
              ps->lockedJoints.add ("lock_" + oj->name (), lj);
              objects[i].get<0> ().get<2> ().push_back (lj);
            }
            ++i;
          }
          GraphPtr_t graph = Graph::create (graphName,
              ps->robot(), ps->problem());
          ps->graphs.add (graphName, graph);
          ps->constraintGraph (graphName);
          graph->stateSelector (
              GuidedStateSelector::create ("stateSelector",
              ps->roadmap ()));
          graph->maxIterations  (ps->maxIterProjection ());
          graph->errorThreshold (ps->errorThreshold ());

          graphBuilder (ps, objects, grippers, graph, rules);
          return graph;
        }
      } // namespace helper
    } // namespace graph
  } // namespace manipulation
} // namespace hpp