helper.cc

          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 Strings_t& griNames,
            const std::vector <ObjectDef_t>& objs,
            const Strings_t& envNames,
	    const std::vector <Rule>& rules,
            const value_type& prePlaceWidth)
        {
          if (ps->graphs.has (graphName))
            throw std::invalid_argument ("A graph named " + graphName + " already exists.");

          const Device& robot = *(ps->robot ());
          const pinocchio::Model& model = robot.model();
          Grippers_t grippers (griNames.size());
          index_t i = 0;
          for (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);
          for (const ObjectDef_t& od : objs) {
            // Create handles
            std::get<2>(objects[i]) = i;
            std::get<1>(objects[i]).resize (od.handles.size());
            Handles_t::iterator it = std::get<1>(objects[i]).begin();
            for (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
            auto& pc (std::get<0>(objects[i]));
            if (ps->numericalConstraints.has(placeN)) {
              std::get<0>(pc) = ps->numericalConstraints.get (placeN);
              if (ps->numericalConstraints.has(preplaceN)) {
                std::get<1>(pc) = ps->numericalConstraints.get (preplaceN);
              }
            } else if (!envNames.empty() && !od.shapes.empty ()) {
              ps->createPlacementConstraint (placeN,
                  od.shapes, envNames, margin);
              std::get<0>(pc) = ps->numericalConstraints.get (placeN);
              if (prePlace) {
                ps->createPrePlacementConstraint (preplaceN,
                    od.shapes, envNames, margin, prePlaceWidth);
                std::get<1>(pc) = ps->numericalConstraints.get (preplaceN);
              }
            }
            // Create object lock
	    // Loop over all frames of object, detect joint and create locked
	    // joint.
            assert (robot.robotFrames (od.name).size () != 0);
            for (const FrameIndex& f : robot.robotFrames (od.name)) {
              if (model.frames[f].type != ::pinocchio::JOINT) continue;
              const JointIndex j = model.frames[f].parent;
              JointPtr_t oj (Joint::create (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->numericalConstraints.add ("lock_" + oj->name (), lj);
              std::get<2>(pc).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