diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh index ba664e6feac1d52b326991543043d0023b0d11b1..503966d203e49398acaacc427c68cee79d735d97 100644 --- a/include/hpp/manipulation/fwd.hh +++ b/include/hpp/manipulation/fwd.hh @@ -23,6 +23,10 @@ # include <map> # include <hpp/constraints/fwd.hh> +namespace fcl { + class TriangleP; +} + namespace hpp { namespace manipulation { typedef model::Device Device; @@ -92,6 +96,10 @@ namespace hpp { typedef boost::shared_ptr <Grasp_t> GraspPtr_t; typedef std::map <DifferentiableFunctionPtr_t, GraspPtr_t> GraspsMap_t; typedef std::map <std::string, LockedDofPtr_t> LockedDofConstraintMap_t; + + typedef fcl::TriangleP Triangle; + typedef std::list <Triangle> TriangleList; + typedef std::map <std::string, TriangleList> TriangleMap; } // namespace manipulation } // namespace hpp diff --git a/include/hpp/manipulation/object.hh b/include/hpp/manipulation/object.hh index f5e420e72a1d1057ad94c319ad9987f22f92566a..24fe19c644eb2d9f4e8f20ef268d9d8d1a1c09f4 100644 --- a/include/hpp/manipulation/object.hh +++ b/include/hpp/manipulation/object.hh @@ -49,6 +49,21 @@ namespace hpp { return shPtr; } + void addContactTriangle (std::string name, const TriangleList& t) + { + contacts_[name] = t; + } + + TriangleList contactTriangles (std::string name) + { + return contacts_ [name]; + } + + const TriangleMap& contactTriangles () const + { + return contacts_; + } + /// \name Object handles /// \{ @@ -90,6 +105,7 @@ namespace hpp { private: Handles_t handles_; ObjectWkPtr_t weakPtr_; + TriangleMap contacts_; }; // class Object } // namespace manipulation } // namespace hpp diff --git a/include/hpp/manipulation/problem-solver.hh b/include/hpp/manipulation/problem-solver.hh index 423b64a688e85aca7ba4b704bf2ee2285b01f241..01e0b3da5f74434e093f6d421ba36342fb17c32f 100644 --- a/include/hpp/manipulation/problem-solver.hh +++ b/include/hpp/manipulation/problem-solver.hh @@ -161,6 +161,12 @@ namespace hpp { return problem_; } + void addContactTriangles (const std::string name, const TriangleList triangles); + + TriangleList contactTriangles (const std::string name); + + const TriangleMap& contactTriangles () const; + protected: void initializeProblem (ProblemPtr_t problem); @@ -174,6 +180,7 @@ namespace hpp { RobotsandObjects_t robotsAndObjects_; GraspsMap_t graspsMap_; LockedDofConstraintMap_t lockedDofConstraintMap_; + TriangleMap contactTriangles_; }; // class ProblemSolver } // namespace manipulation } // namespace hpp diff --git a/src/problem-solver.cc b/src/problem-solver.cc index 194c5cf5938bfe6b8e89cd710214f64fd0707923..49a99be77cf5306d5de3b80da1f9141204981b24 100644 --- a/src/problem-solver.cc +++ b/src/problem-solver.cc @@ -179,5 +179,20 @@ namespace hpp { if (constraintGraph_) r->constraintGraph (constraintGraph_); roadmap (r); } + + void ProblemSolver::addContactTriangles (const std::string name, const TriangleList triangles) + { + contactTriangles_ [name] = triangles; + } + + TriangleList ProblemSolver::contactTriangles (const std::string name) + { + return contactTriangles_ [name]; + } + + const TriangleMap& ProblemSolver::contactTriangles () const + { + return contactTriangles_; + } } // namespace manipulation } // namespace hpp