diff --git a/include/hpp/manipulation/device.hh b/include/hpp/manipulation/device.hh index fefbe58aacdd8bdf16aa7f8d923f2fd71eeaac59..fec0b6ac73605a3e5d41e70e72d72ac40cb10e1c 100644 --- a/include/hpp/manipulation/device.hh +++ b/include/hpp/manipulation/device.hh @@ -67,6 +67,13 @@ namespace hpp { return core::Container <Element>::get (name); } + /// Check if a Container has a key. + template <typename Element> + bool has (const std::string& name) const + { + return core::Container <Element>::has (name); + } + /// Get the keys of a container template <typename Element, typename ReturnType> ReturnType getKeys () const diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh index a30be804ff298055e8475a69c6a658c7a45a2ac0..91c0fcd155d9ff166b6211c03df0c0001e87f784 100644 --- a/include/hpp/manipulation/problem-solver.hh +++ b/include/hpp/manipulation/problem-solver.hh @@ -136,6 +136,13 @@ namespace hpp { return Container <Element>::get (name); } + /// Check if a Container has a key. + template <typename Element> + bool has (const std::string& name) const + { + return core::Container <Element>::has (name); + } + /// Add an element to a container template <typename Element> void add (const std::string& name, const Element& element) diff --git a/src/problem-solver.cc b/src/problem-solver.cc index 8772e0097366fa19ecd4aa361f208f7fb5b7365f..6700497b2afa6d581f0e677961505464031f8a69 100644 --- a/src/problem-solver.cc +++ b/src/problem-solver.cc @@ -161,21 +161,21 @@ namespace hpp { ConvexShapeContactComplementPtr_t > constraints (ConvexShapeContactComplement::createPair (name, complementName, robot_)); - JointAndShapes_t l = robot_->get <JointAndShapes_t> (surface1); - if (l.empty ()) throw std::runtime_error - ("First list of triangles not found."); + if (!robot_->has <JointAndShapes_t> (surface1)) + throw std::runtime_error ("First list of triangles not found."); + JointAndShapes_t l = robot_->get <JointAndShapes_t> (surface1); for (JointAndShapes_t::const_iterator it = l.begin (); it != l.end(); ++it) { constraints.first->addObject (ConvexShape (it->second, it->first)); } + // Search first robot triangles - l = robot_->get <JointAndShapes_t> (surface2); - if (l.empty ()) { + if (robot_->has <JointAndShapes_t> (surface2)) + l = robot_->get <JointAndShapes_t> (surface2); // and then environment triangles. + else if (has <JointAndShapes_t> (surface2)) l = get <JointAndShapes_t> (surface2); - if (l.empty ()) throw std::runtime_error - ("Second list of triangles not found."); - } + else throw std::runtime_error ("Second list of triangles not found."); for (JointAndShapes_t::const_iterator it = l.begin (); it != l.end(); ++it) { constraints.first->addFloor (ConvexShape (it->second, it->first));