helper.cc

            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.get <GripperPtr_t> (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.get <HandlePtr_t> (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->core::ProblemSolver::has<NumericalConstraintPtr_t>(placeN)) {
              objects[i].get<0> ().get<0> () =
                ps->core::ProblemSolver::get <NumericalConstraintPtr_t> (placeN);
              if (ps->core::ProblemSolver::has<NumericalConstraintPtr_t>(preplaceN)) {
                objects[i].get<0> ().get<1> () =
                  ps->core::ProblemSolver::get <NumericalConstraintPtr_t> (preplaceN);
              }
            } else if (!envNames.empty() && !od.shapes.empty ()) {
              ps->createPlacementConstraint (placeN,
                  od.shapes, envNames, margin);
              objects[i].get<0> ().get<0> () =
                ps->core::ProblemSolver::get <NumericalConstraintPtr_t> (placeN);
              if (prePlace) {
                ps->createPrePlacementConstraint (preplaceN,
                    od.shapes, envNames, margin, prePlaceWidth);
                objects[i].get<0> ().get<1> () =
                  ps->core::ProblemSolver::get <NumericalConstraintPtr_t> (preplaceN);
              }
            }
            // Create object lock
	    // Loop over all frames of object, detect joint and create locked
	    // joint.
            assert (robot.has <FrameIndexes_t> (od.name));
            BOOST_FOREACH (const se3::FrameIndex& f, robot.get<FrameIndexes_t> (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));
              LockedJointPtr_t lj = core::LockedJoint::create (oj,
                  robot.currentConfiguration()
                  .segment (oj->rankInConfiguration (), oj->configSize ()));
              ps->add <LockedJointPtr_t> ("lock_" + oj->name (), lj);
              objects[i].get<0> ().get<2> ().push_back (lj);
            }
            ++i;
          }
          GraphPtr_t graph = Graph::create (graphName,
              ps->robot(), ps->problem());
          graph->stateSelector (
              GuidedStateSelector::create ("stateSelector",
              ps->roadmap ()));
          graph->maxIterations  (ps->maxIterProjection ());
          graph->errorThreshold (ps->errorThreshold ());

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