diff --git a/include/hpp/manipulation/device.hh b/include/hpp/manipulation/device.hh index a2cd02a19b867888a00994a075c9d87f8f6067ab..8834f25b2c923da2d6823e6cbd862d2baf4c869c 100644 --- a/include/hpp/manipulation/device.hh +++ b/include/hpp/manipulation/device.hh @@ -35,23 +35,11 @@ namespace hpp { /// it is compatible with hpp::pinocchio::urdf::loadHumanoidRobot /// /// This class also contains pinocchio::Gripper, Handle and \ref JointAndShapes_t - class HPP_MANIPULATION_DLLAPI Device : - public pinocchio::HumanoidRobot, - public core::Containers< - boost::mpl::vector < HandlePtr_t, - GripperPtr_t, - JointAndShapes_t, - FrameIndices_t> > + class HPP_MANIPULATION_DLLAPI Device : public pinocchio::HumanoidRobot { public: typedef pinocchio::HumanoidRobot Parent_t; - typedef core::Containers< - boost::mpl::vector < HandlePtr_t, - pinocchio::GripperPtr_t, - JointAndShapes_t, - FrameIndices_t> > Containers_t; - /// Constructor /// \param name of the new instance, static DevicePtr_t create (const std::string& name) @@ -82,6 +70,11 @@ namespace hpp { /// \} + core::Container <HandlePtr_t> handles; + core::Container <GripperPtr_t> grippers; + core::Container <JointAndShapes_t> jointAndShapes; + core::Container <FrameIndices_t> frameIndices; + protected: /// Constructor /// \param name of the new instance, diff --git a/src/device.cc b/src/device.cc index b641f991bae504f0dd36f052f3537e3a12ebeb0a..d4f01097a99d17ae8b4814dca13fe3f719d6e73d 100644 --- a/src/device.cc +++ b/src/device.cc @@ -43,7 +43,7 @@ namespace hpp { void Device::setRobotRootPosition (const std::string& rn, const Transform3f& t) { - FrameIndices_t idxs = get<JointIndices_t> (rn); + const FrameIndices_t& idxs = frameIndices.get (rn); pinocchio::Model& m = model(); pinocchio::GeomModel& gm = geomModel(); // The root frame should be the first frame. @@ -93,11 +93,11 @@ namespace hpp { } frameCacheSize_ = model().frames.size(); - if (has<FrameIndices_t>(name)) { - const FrameIndices_t& old = get<FrameIndices_t>(name); + if (frameIndices.has(name)) { + const FrameIndices_t& old = frameIndices.get(name); newF.insert(newF.begin(), old.begin(), old.end()); } - add (name, newF); + frameIndices.add (name, newF); createData(); createGeomData(); } @@ -107,10 +107,10 @@ namespace hpp { Parent_t::print (os); // print handles os << "Handles:" << std::endl; - Containers_t::print <HandlePtr_t> (os); + handles.print (os); // print grippers os << "Grippers:" << std::endl; - Containers_t::print <GripperPtr_t> (os); + grippers.print (os); return os; } } // namespace manipulation diff --git a/src/graph/helper.cc b/src/graph/helper.cc index a1ff2a520a16dbb2dabc6238ee2e05eab4191a12..224c71d802ecf9ad3f0f0f231f6a2c181e56ef24 100644 --- a/src/graph/helper.cc +++ b/src/graph/helper.cc @@ -663,8 +663,6 @@ namespace hpp { inline boost::array<NumericalConstraintPtr_t,3>& graspConstraint ( const index_t& iG, const index_t& iOH) { - typedef core::ProblemSolver CPS_t; - boost::array<NumericalConstraintPtr_t,3>& gcs = graspCs [iG * nOH + iOH]; if (!gcs[0]) { @@ -673,16 +671,16 @@ namespace hpp { const GripperPtr_t& g (gs[iG]); const HandlePtr_t& h (handle (iOH)); const std::string& grasp = g->name() + " grasps " + h->name(); - if (!ps->CPS_t::has<NumericalConstraintPtr_t>(grasp)) { + if (!ps->numericalConstraints.has(grasp)) { ps->createGraspConstraint (grasp, g->name(), h->name()); } - gcs[0] = ps->CPS_t::get<NumericalConstraintPtr_t>(grasp); - gcs[1] = ps->CPS_t::get<NumericalConstraintPtr_t>(grasp + "/complement"); + gcs[0] = ps->numericalConstraints.get(grasp); + gcs[1] = ps->numericalConstraints.get(grasp + "/complement"); const std::string& pregrasp = g->name() + " pregrasps " + h->name(); - if (!ps->CPS_t::has<NumericalConstraintPtr_t>(pregrasp)) { + if (!ps->numericalConstraints.has(pregrasp)) { ps->createPreGraspConstraint (pregrasp, g->name(), h->name()); } - gcs[2] = ps->CPS_t::get<NumericalConstraintPtr_t>(pregrasp); + gcs[2] = ps->numericalConstraints.get(pregrasp); } return gcs; } @@ -1050,7 +1048,7 @@ namespace hpp { Grippers_t grippers (griNames.size()); index_t i = 0; BOOST_FOREACH (const std::string& gn, griNames) { - grippers[i] = robot.get <GripperPtr_t> (gn); + grippers[i] = robot.grippers.get (gn); ++i; } Objects_t objects (objs.size()); @@ -1063,7 +1061,7 @@ namespace hpp { 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 = robot.handles.get (hn); ++it; } // Create placement @@ -1076,30 +1074,30 @@ namespace hpp { // 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)) { + if (ps->numericalConstraints.has(placeN)) { objects[i].get<0> ().get<0> () = - ps->core::ProblemSolver::get <NumericalConstraintPtr_t> (placeN); - if (ps->core::ProblemSolver::has<NumericalConstraintPtr_t>(preplaceN)) { + ps->numericalConstraints.get (placeN); + if (ps->numericalConstraints.has(preplaceN)) { objects[i].get<0> ().get<1> () = - ps->core::ProblemSolver::get <NumericalConstraintPtr_t> (preplaceN); + 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->core::ProblemSolver::get <NumericalConstraintPtr_t> (placeN); + ps->numericalConstraints.get (placeN); if (prePlace) { ps->createPrePlacementConstraint (preplaceN, od.shapes, envNames, margin, prePlaceWidth); objects[i].get<0> ().get<1> () = - ps->core::ProblemSolver::get <NumericalConstraintPtr_t> (preplaceN); + ps->numericalConstraints.get (preplaceN); } } // Create object lock // Loop over all frames of object, detect joint and create locked // joint. - assert (robot.has <FrameIndices_t> (od.name)); - BOOST_FOREACH (const se3::FrameIndex& f, robot.get<FrameIndices_t> (od.name)) { + 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)); @@ -1108,7 +1106,7 @@ namespace hpp { .segment (oj->rankInConfiguration (), oj->configSize ()), space); LockedJointPtr_t lj = core::LockedJoint::create (oj, lge); - ps->add <LockedJointPtr_t> ("lock_" + oj->name (), lj); + ps->lockedJoints.add ("lock_" + oj->name (), lj); objects[i].get<0> ().get<2> ().push_back (lj); } ++i; diff --git a/src/problem-solver.cc b/src/problem-solver.cc index e443b8dd1b637e14900ea2d6f8077cc55c396d86..9f14011c8d60a27637dfd3ba6e90dd110043d030 100644 --- a/src/problem-solver.cc +++ b/src/problem-solver.cc @@ -111,69 +111,64 @@ namespace hpp { ProblemSolver::ProblemSolver () : core::ProblemSolver (), robot_ (), problem_ (0x0) { - parent_t::add<core::RobotBuilder_t> ("hpp::manipulation::Device", - hpp::manipulation::Device::create); - parent_t::robotType ("hpp::manipulation::Device"); - parent_t::add<core::PathPlannerBuilder_t> - ("M-RRT", ManipulationPlanner::create); - parent_t::add<core::PathPlannerBuilder_t> - ("SymbolicPlanner", SymbolicPlanner::create); - using core::PathOptimizerBuilder_t; - parent_t::add <PathOptimizerBuilder_t> ("Graph-RandomShortcut", + robots.add ("hpp::manipulation::Device", manipulation::Device::create); + robotType ("hpp::manipulation::Device"); + + pathPlanners.add ("M-RRT", ManipulationPlanner::create); + pathPlanners.add ("SymbolicPlanner", SymbolicPlanner::create); + + pathOptimizers.add ("Graph-RandomShortcut", GraphOptimizer::create <core::RandomShortcut>); - parent_t::add <PathOptimizerBuilder_t> ("PartialShortcut", core::pathOptimization:: + pathOptimizers.add ("PartialShortcut", core::pathOptimization:: PartialShortcut::createWithTraits <PartialShortcutTraits>); - parent_t::add <PathOptimizerBuilder_t> ("Graph-PartialShortcut", + pathOptimizers.add ("Graph-PartialShortcut", GraphOptimizer::create <core::pathOptimization::PartialShortcut>); - parent_t::add <PathOptimizerBuilder_t> ("ConfigOptimization", + pathOptimizers.add ("ConfigOptimization", core::pathOptimization::ConfigOptimization::createWithTraits <pathOptimization::ConfigOptimizationTraits>); - parent_t::add <PathOptimizerBuilder_t> ("Graph-ConfigOptimization", + pathOptimizers.add ("Graph-ConfigOptimization", GraphOptimizer::create < GraphConfigOptimizationTraits <pathOptimization::ConfigOptimizationTraits> >); - parent_t::add <core::PathProjectorBuilder_t> ("Progressive", + pathProjectors.add ("Progressive", createPathProjector <core::pathProjector::Progressive>); - parent_t::add <core::PathProjectorBuilder_t> ("Dichotomy", + pathProjectors.add ("Dichotomy", createPathProjector <core::pathProjector::Dichotomy>); - parent_t::add <core::PathProjectorBuilder_t> ("Global", + pathProjectors.add ("Global", createPathProjector <core::pathProjector::Global>); - parent_t::add <core::PathProjectorBuilder_t> ("RecursiveHermite", + pathProjectors.add ("RecursiveHermite", createPathProjector <core::pathProjector::RecursiveHermite>); - // parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_cannonical1",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 1>::createFromCore); - // parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_cannonical2",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 2>::createFromCore); - // parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_cannonical3",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 3>::createFromCore); - parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_bezier1",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 1>::createFromCore); - // parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_bezier2",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 2>::createFromCore); - parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_bezier3",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 3>::createFromCore); + // pathOptimizers.add ("SplineGradientBased_cannonical1",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 1>::createFromCore); + // pathOptimizers.add ("SplineGradientBased_cannonical2",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 2>::createFromCore); + // pathOptimizers.add ("SplineGradientBased_cannonical3",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 3>::createFromCore); + pathOptimizers.add ("SplineGradientBased_bezier1",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 1>::createFromCore); + // pathOptimizers.add ("SplineGradientBased_bezier2",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 2>::createFromCore); + pathOptimizers.add ("SplineGradientBased_bezier3",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 3>::createFromCore); - using core::SteeringMethodBuilder_t; - parent_t::add <SteeringMethodBuilder_t> ("Graph-SteeringMethodStraight", + steeringMethods.add ("Graph-SteeringMethodStraight", GraphSteeringMethod::create <core::SteeringMethodStraight>); - parent_t::add <SteeringMethodBuilder_t> ("Graph-Straight", + steeringMethods.add ("Graph-Straight", GraphSteeringMethod::create <core::steeringMethod::Straight>); - parent_t::add <SteeringMethodBuilder_t> ("Graph-Hermite", + steeringMethods.add ("Graph-Hermite", GraphSteeringMethod::create <core::steeringMethod::Hermite>); - parent_t::add <SteeringMethodBuilder_t> ("CrossStateOptimization", + steeringMethods.add ("CrossStateOptimization", steeringMethod::CrossStateOptimization::createFromCore); - parent_t::add <SteeringMethodBuilder_t> ("Graph-ReedsShepp", + steeringMethods.add ("Graph-ReedsShepp", createSteeringMethodWithGuess <core::steeringMethod::ReedsShepp>); - parent_t::add <SteeringMethodBuilder_t> ("Graph-Dubins", + steeringMethods.add ("Graph-Dubins", createSteeringMethodWithGuess <core::steeringMethod::Dubins>); - parent_t::add <SteeringMethodBuilder_t> ("Graph-Snibud", + steeringMethods.add ("Graph-Snibud", createSteeringMethodWithGuess <core::steeringMethod::Snibud>); - parent_t::add <PathOptimizerBuilder_t> ("KeypointsShortcut", + pathOptimizers.add ("KeypointsShortcut", pathOptimization::Keypoints::create); #if HPP_MANIPULATION_HAS_WHOLEBODY_STEP - parent_t::add <PathOptimizerBuilder_t> - ("Walkgen", wholebodyStep::SmallSteps::create); - parent_t::add <PathOptimizerBuilder_t> - ("Graph-Walkgen", pathOptimization::SmallSteps::create); + pathOptimizers.add ("Walkgen", wholebodyStep::SmallSteps::create); + pathOptimizers.add ("Graph-Walkgen", pathOptimization::SmallSteps::create); #endif pathPlannerType ("M-RRT"); @@ -238,9 +233,9 @@ namespace hpp { JointAndShapes_t l; for (StringList_t::const_iterator it1 = surface1.begin (); it1 != surface1.end(); ++it1) { - if (!robot_->has <JointAndShapes_t> (*it1)) + if (!robot_->jointAndShapes.has (*it1)) throw std::runtime_error ("First list of triangles not found."); - l = robot_->get <JointAndShapes_t> (*it1); + l = robot_->jointAndShapes.get (*it1); for (JointAndShapes_t::const_iterator it = l.begin (); it != l.end(); ++it) { constraints.first->addObject (ConvexShape (it->second, it->first)); @@ -250,11 +245,11 @@ namespace hpp { for (StringList_t::const_iterator it2 = surface2.begin (); it2 != surface2.end(); ++it2) { // Search first robot triangles - if (robot_->has <JointAndShapes_t> (*it2)) - l = robot_->get <JointAndShapes_t> (*it2); + if (robot_->jointAndShapes.has (*it2)) + l = robot_->jointAndShapes.get (*it2); // and then environment triangles. - else if (core::ProblemSolver::has <JointAndShapes_t> (*it2)) - l = core::ProblemSolver::get <JointAndShapes_t> (*it2); + else if (jointAndShapes.has (*it2)) + l = jointAndShapes.get (*it2); else throw std::runtime_error ("Second list of triangles not found."); for (JointAndShapes_t::const_iterator it = l.begin (); it != l.end(); ++it) { @@ -289,9 +284,9 @@ namespace hpp { JointAndShapes_t l; for (StringList_t::const_iterator it1 = surface1.begin (); it1 != surface1.end(); ++it1) { - if (!robot_->has <JointAndShapes_t> (*it1)) + if (!robot_->jointAndShapes.has (*it1)) throw std::runtime_error ("First list of triangles not found."); - l = robot_->get <JointAndShapes_t> (*it1); + l = robot_->jointAndShapes.get (*it1); for (JointAndShapes_t::const_iterator it = l.begin (); it != l.end(); ++it) { @@ -302,11 +297,11 @@ namespace hpp { for (StringList_t::const_iterator it2 = surface2.begin (); it2 != surface2.end(); ++it2) { // Search first robot triangles - if (robot_->has <JointAndShapes_t> (*it2)) - l = robot_->get <JointAndShapes_t> (*it2); + if (robot_->jointAndShapes.has (*it2)) + l = robot_->jointAndShapes.get (*it2); // and then environment triangles. - else if (has <JointAndShapes_t> (*it2)) - l = get <JointAndShapes_t> (*it2); + else if (jointAndShapes.has (*it2)) + l = jointAndShapes.get (*it2); else throw std::runtime_error ("Second list of triangles not found."); for (JointAndShapes_t::const_iterator it = l.begin (); @@ -327,9 +322,9 @@ namespace hpp { if (!constraintGraph ()) { throw std::runtime_error ("The graph is not defined."); } - GripperPtr_t g = robot_->get <GripperPtr_t> (gripper); + GripperPtr_t g = robot_->grippers.get (gripper, GripperPtr_t()); if (!g) throw std::runtime_error ("No gripper with name " + gripper + "."); - HandlePtr_t h = robot_->get <HandlePtr_t> (handle); + HandlePtr_t h = robot_->handles.get (handle, HandlePtr_t()); if (!h) throw std::runtime_error ("No handle with name " + handle + "."); const std::string cname = name + "/complement"; const std::string bname = name + "/hold"; @@ -346,9 +341,9 @@ namespace hpp { (const std::string& name, const std::string& gripper, const std::string& handle) { - GripperPtr_t g = robot_->get <GripperPtr_t> (gripper); + GripperPtr_t g = robot_->grippers.get (gripper, GripperPtr_t()); if (!g) throw std::runtime_error ("No gripper with name " + gripper + "."); - HandlePtr_t h = robot_->get <HandlePtr_t> (handle); + HandlePtr_t h = robot_->handles.get (handle, HandlePtr_t()); if (!h) throw std::runtime_error ("No handle with name " + handle + "."); value_type c = h->clearance () + g->clearance (); @@ -362,7 +357,7 @@ namespace hpp { parent_t::pathValidationType(type, tolerance); assert (problem_); problem_->setPathValidationFactory ( - parent_t::get<core::PathValidationBuilder_t>(type), + pathValidations.get(type), tolerance); }