Commit 02b9b7b0 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

convert boost/std CollisionGeometryPtr

ref. https://github.com/humanoid-path-planner/hpp-pinocchio/pull/158
parent c46079f8
Pipeline #18511 failed with stage
in 1 minute and 26 seconds
......@@ -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)
......
......@@ -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);
}
}
......
......@@ -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));
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment