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);
}