helper.cc

            const value_type& prePlaceWidth)
        {
          const Device& robot = *(ps->robot ());
          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
            if (!envNames.empty() && !od.shapes.empty ()) {
              const std::string placeN = "place_" + od.name;
              ps->createPlacementConstraint (placeN,
                  od.shapes, envNames, margin);
              objects[i].get<0> ().get<0> () =
                ps->core::ProblemSolver::get <NumericalConstraintPtr_t> (placeN);
              if (prePlace) {
                ps->createPrePlacementConstraint ("pre" + placeN,
                    od.shapes, envNames, margin, prePlaceWidth);
                objects[i].get<0> ().get<1> () =
                  ps->core::ProblemSolver::get <NumericalConstraintPtr_t> ("pre" + placeN);
              }
            }
            // Create object lock
            using model::JointVector_t;
            assert (robot.has <JointVector_t> (od.name));
            BOOST_FOREACH (const JointPtr_t& oj, robot.get<JointVector_t> (od.name)) {
              LockedJointPtr_t lj = core::LockedJoint::create (oj,
                  robot.currentConfiguration()
                  .segment (oj->rankInConfiguration (), oj->configSize ()));
              ps->ProblemSolver::ThisC_t::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->nodeSelector (
              GuidedNodeSelector::create ("nodeSelector",
              ps->roadmap ()));
          graph->maxIterations  (ps->maxIterations ());
          graph->errorThreshold (ps->errorThreshold ());

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