convert boost/std CollisionGeometryPtr

ref. https://github.com/humanoid-path-planner/hpp-pinocchio/pull/158

CMakeLists.txt

@@ -48,7 +48,7 @@ SET(CMAKE_MODULE_PATH
 COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
 PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
 
-ADD_PROJECT_DEPENDENCY(hpp-pinocchio)
+ADD_PROJECT_DEPENDENCY(hpp-pinocchio 4.13.0)
 ADD_PROJECT_DEPENDENCY(hpp-util)
 ADD_PROJECT_DEPENDENCY(hpp-statistics)
 ADD_PROJECT_DEPENDENCY(hpp-constraints)

src/problem-solver.cc

@@ -74,6 +74,7 @@
 #include <hpp/core/weighed-distance.hh>
 #include <hpp/pinocchio/collision-object.hh>
 #include <hpp/pinocchio/joint-collection.hh>
+#include <hpp/pinocchio/shared-ptr.hh>
 #include <hpp/util/debug.hh>
 #include <hpp/util/exception-factory.hh>
 #include <iterator>
@@ -847,7 +848,8 @@ void ProblemSolver::addObstacle(const std::string& name,
 
   ::pinocchio::GeomIndex id = obstacleModel_->addGeometryObject(
       ::pinocchio::GeometryObject(
-          name, 1, 0, inObject.collisionGeometry(),
+          name, 1, 0,
+          hpp::pinocchio::as_boost_shared_ptr(inObject.collisionGeometry()),
           ::pinocchio::toPinocchioSE3(inObject.getTransform()), "",
           vector3_t::Ones()),
       *obstacleRModel_);
@@ -914,8 +916,8 @@ void ProblemSolver::cutObstacle(const std::string& name,
   ::pinocchio::GeomIndex id = obstacleModel_->getGeometryId(name);
 
   fcl::Transform3f oMg = ::pinocchio::toFclTransform3f(obstacleData_->oMg[id]);
-  fcl::CollisionGeometryPtr_t fclgeom =
-      obstacleModel_->geometryObjects[id].geometry;
+  fcl::CollisionGeometryPtr_t fclgeom = hpp::pinocchio::as_std_shared_ptr(
+      obstacleModel_->geometryObjects[id].geometry);
   fcl::CollisionObject fclobj(fclgeom, oMg);
   fclobj.computeAABB();
   if (!fclobj.getAABB().overlap(aabb)) {
@@ -928,7 +930,8 @@ void ProblemSolver::cutObstacle(const std::string& name,
     // No intersection. Geom should be removed.
     removeObstacle(name);
   } else {
-    obstacleModel_->geometryObjects[id].geometry = newgeom;
+    obstacleModel_->geometryObjects[id].geometry =
+        hpp::pinocchio::as_boost_shared_ptr(newgeom);
   }
 }
 

tests/test-solid-solid-collision.cc

@@ -35,6 +35,7 @@
 #include <hpp/core/continuous-validation/solid-solid-collision.hh>
 #include <hpp/pinocchio/device.hh>
 #include <hpp/pinocchio/joint.hh>
+#include <hpp/pinocchio/shared-ptr.hh>
 #include <hpp/pinocchio/simple-device.hh>
 #include <limits>
 #include <pinocchio/fwd.hpp>
@@ -95,8 +96,9 @@ BOOST_AUTO_TEST_CASE(solid_solid_collision_1) {
   pinocchio::FrameIndex frame_id = robot->model().addFrame(
       ::pinocchio::Frame("base_link", 0, 0, I3, ::pinocchio::BODY));
   GeomIndex idObj = robot->geomModel().addGeometryObject(
-      ::pinocchio::GeometryObject("obstacle", frame_id, 0, box, I3, "",
-                                  vector3_t::Ones()),
+      ::pinocchio::GeometryObject("obstacle", frame_id, 0,
+                                  hpp::pinocchio::as_boost_shared_ptr(box), I3,
+                                  "", vector3_t::Ones()),
       robot->model());
   CollisionObjectPtr_t collObj(
       new CollisionObject(robot->geomModelPtr(), robot->geomDataPtr(), idObj));