From 1210ef602e369b65337ad51608960ef8b3742bee Mon Sep 17 00:00:00 2001
From: Valenza Florian <fvalenza@laas.fr>
Date: Fri, 29 Jul 2016 19:15:05 +0200
Subject: [PATCH] [C++][Python] One can contruct a GeometryModel and Data even
if hpp-fcl is not found. All stuff related to collisions is not available but
geometryPlacements are ( for visualisation in the viewer especially).
---
CMakeLists.txt | 39 +++++++++++++++---------------
benchmark/CMakeLists.txt | 22 +++++++++--------
benchmark/timings-geometry.cpp | 7 ++++--
src/algorithm/geometry.hpp | 4 ++--
src/algorithm/geometry.hxx | 6 +++--
src/multibody/fake-fcl.hpp | 43 ++++++++++++++++++++++++++++++++++
src/multibody/fcl.hpp | 8 ++++++-
src/multibody/geometry.hpp | 18 +++++++++-----
src/multibody/geometry.hxx | 9 ++++++-
src/parsers/urdf.hpp | 9 ++++---
src/parsers/urdf/geometry.cpp | 7 ++++++
src/python/algorithms.hpp | 23 +++++++++---------
src/python/geometry-data.hpp | 10 ++++----
src/python/geometry-model.hpp | 8 +++----
src/python/parsers.hpp | 6 ++---
unittest/geom.cpp | 5 ++++
16 files changed, 151 insertions(+), 73 deletions(-)
create mode 100644 src/multibody/fake-fcl.hpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7b3a2d855..f71a7a523 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -213,13 +213,13 @@ IF(BUILD_PYTHON_INTERFACE)
python/explog.hpp
)
- IF(HPP_FCL_FOUND)
- LIST(APPEND ${PROJECT_NAME}_PYTHON_HEADERS
- python/geometry-object.hpp
- python/geometry-model.hpp
- python/geometry-data.hpp
- )
- ENDIF(HPP_FCL_FOUND)
+
+LIST(APPEND ${PROJECT_NAME}_PYTHON_HEADERS
+ python/geometry-object.hpp
+ python/geometry-model.hpp
+ python/geometry-data.hpp
+ )
+
ELSE(BUILD_PYTHON_INTERFACE)
SET(${PROJECT_NAME}_PYTHON_HEADERS "")
@@ -242,28 +242,27 @@ IF(URDFDOM_FOUND)
parsers/urdf.hpp
parsers/urdf/utils.hpp
)
-
- IF(HPP_FCL_FOUND)
- LIST(APPEND ${PROJECT_NAME}_PARSERS_HEADERS
- )
- ENDIF(HPP_FCL_FOUND)
+
ADD_DEFINITIONS(-DWITH_URDFDOM)
ENDIF(URDFDOM_FOUND)
+LIST(APPEND ${PROJECT_NAME}_MULTIBODY_HEADERS
+ multibody/fcl.hpp
+ multibody/geometry.hpp
+ multibody/geometry.hxx
+ )
+LIST(APPEND ${PROJECT_NAME}_ALGORITHM_HEADERS
+ algorithm/geometry.hpp
+ algorithm/geometry.hxx
+ )
IF(HPP_FCL_FOUND)
LIST(APPEND ${PROJECT_NAME}_SPATIAL_HEADERS
spatial/fcl-pinocchio-conversions.hpp
)
+ELSE(HPP_FCL_FOUND)
LIST(APPEND ${PROJECT_NAME}_MULTIBODY_HEADERS
- multibody/fcl.hpp
- multibody/geometry.hpp
- multibody/geometry.hxx
- )
- LIST(APPEND ${PROJECT_NAME}_ALGORITHM_HEADERS
- algorithm/geometry.hpp
- algorithm/geometry.hxx
- )
+ multibody/fake-fcl.hpp)
ENDIF(HPP_FCL_FOUND)
IF(LUA5_1_FOUND)
diff --git a/benchmark/CMakeLists.txt b/benchmark/CMakeLists.txt
index f11f2a0cd..bf1b0d56b 100644
--- a/benchmark/CMakeLists.txt
+++ b/benchmark/CMakeLists.txt
@@ -39,7 +39,7 @@ IF(${URDFDOM_FOUND})
ENDIF(${URDFDOM_FOUND})
SET_TARGET_PROPERTIES (timings PROPERTIES COMPILE_DEFINITIONS PINOCCHIO_SOURCE_DIR="${${PROJECT_NAME}_SOURCE_DIR}")
-# timings
+# timings-eigen
#
IF(BUILD_BENCHMARK)
ADD_EXECUTABLE(timings-eigen timings-eigen.cpp)
@@ -56,16 +56,18 @@ IF(URDFDOM_FOUND AND HPP_FCL_FOUND)
ELSE(BUILD_BENCHMARK)
ADD_EXECUTABLE(geomTimings EXCLUDE_FROM_ALL timings-geometry.cpp)
ENDIF(BUILD_BENCHMARK)
- ADD_TEST_CFLAGS(geomTimings "-DWITH_HPP_FCL")
- IF(BUILD_TESTS_WITH_HPP)
- ADD_OPTIONAL_DEPENDENCY("hpp-model-urdf")
- IF(HPP_MODEL_URDF_FOUND)
- PKG_CONFIG_USE_DEPENDENCY(geomTimings hpp-model-urdf)
- ADD_TEST_CFLAGS(geomTimings "-DWITH_HPP_MODEL_URDF")
- ENDIF(HPP_MODEL_URDF_FOUND)
- ENDIF(BUILD_TESTS_WITH_HPP)
- TARGET_LINK_LIBRARIES (geomTimings ${PROJECT_NAME})
+
+ ADD_TEST_CFLAGS(geomTimings "-DWITH_HPP_FCL")
+ IF(BUILD_TESTS_WITH_HPP)
+ ADD_OPTIONAL_DEPENDENCY("hpp-model-urdf")
+ IF(HPP_MODEL_URDF_FOUND)
+ PKG_CONFIG_USE_DEPENDENCY(geomTimings hpp-model-urdf)
+ ADD_TEST_CFLAGS(geomTimings "-DWITH_HPP_MODEL_URDF")
+ ENDIF(HPP_MODEL_URDF_FOUND)
+ ENDIF(BUILD_TESTS_WITH_HPP)
PKG_CONFIG_USE_DEPENDENCY(geomTimings hpp-fcl)
+
+ TARGET_LINK_LIBRARIES (geomTimings ${PROJECT_NAME})
IF(BUILD_PYTHON_INTERFACE)
TARGET_LINK_LIBRARIES(geomTimings ${PYTHON_LIBRARIES})
ENDIF(BUILD_PYTHON_INTERFACE)
diff --git a/benchmark/timings-geometry.cpp b/benchmark/timings-geometry.cpp
index fd79b8672..c34ccea67 100644
--- a/benchmark/timings-geometry.cpp
+++ b/benchmark/timings-geometry.cpp
@@ -72,7 +72,9 @@ int main()
se3::Model model = se3::urdf::buildModel(romeo_filename, se3::JointModelFreeFlyer());
se3::GeometryModel geom_model = se3::urdf::buildGeom(model, romeo_filename, package_dirs, COLLISION);
+#ifdef WITH_HPP_FCL
geom_model.addAllCollisionPairs();
+#endif // WITH_HPP_FCL
Data data(model);
GeometryData geom_data(geom_model);
@@ -104,6 +106,7 @@ int main()
double update_col_time = timer.toc(StackTicToc::US)/NBT - geom_time;
std::cout << "Update Collision Geometry < false > = \t" << update_col_time << " " << StackTicToc::unitName(StackTicToc::US) << std::endl;
+#ifdef WITH_HPP_FCL
timer.tic();
SMOOTH(NBT)
{
@@ -277,7 +280,7 @@ hpp::model::HumanoidRobotPtr_t humanoidRobot =
std::cout << "HPP - Update + Compute distances (K+D) " << humanoidRobot->distanceResults().size() << " col pairs\t" << hpp_compute_distances
<< " " << StackTicToc::unitName(StackTicToc::US) << std::endl;
-#endif
-
+#endif // WITH_HPP_MODEL_URDF
+#endif // WITH_HPP_FCL
return 0;
}
diff --git a/src/algorithm/geometry.hpp b/src/algorithm/geometry.hpp
index f7303a145..b99352e61 100644
--- a/src/algorithm/geometry.hpp
+++ b/src/algorithm/geometry.hpp
@@ -57,7 +57,7 @@ namespace se3
const GeometryModel & geom,
GeometryData & geom_data
);
-
+#ifdef WITH_HPP_FCL
inline bool computeCollisions(const Model & model,
Data & data,
const GeometryModel & model_geom,
@@ -92,7 +92,7 @@ namespace se3
inline void computeBodyRadius(const Model & model,
const GeometryModel & geomModel,
GeometryData & geomData);
-
+#endif // WITH_HPP_FCL
} // namespace se3
/* --- Details -------------------------------------------------------------------- */
diff --git a/src/algorithm/geometry.hxx b/src/algorithm/geometry.hxx
index 75f85cc0b..3865d4e61 100644
--- a/src/algorithm/geometry.hxx
+++ b/src/algorithm/geometry.hxx
@@ -45,10 +45,12 @@ namespace se3
const Model::JointIndex & parent = model_geom.geometryObjects[i].parent;
if (parent>0) data_geom.oMg[i] = (data.oMi[parent] * model_geom.geometryObjects[i].placement);
else data_geom.oMg[i] = model_geom.geometryObjects[i].placement;
+#ifdef WITH_HPP_FCL
data_geom.collisionObjects[i].setTransform( toFclTransform3f(data_geom.oMg[i]) );
+#endif // WITH_HPP_FCL
}
}
-
+#ifdef WITH_HPP_FCL
inline bool computeCollisions(GeometryData & data_geom,
const bool stopAtFirstCollision
)
@@ -177,7 +179,7 @@ namespace se3
}
#undef SE3_GEOM_AABB
-
+#endif // WITH_HPP_FCL
} // namespace se3
diff --git a/src/multibody/fake-fcl.hpp b/src/multibody/fake-fcl.hpp
new file mode 100644
index 000000000..6290c77ca
--- /dev/null
+++ b/src/multibody/fake-fcl.hpp
@@ -0,0 +1,43 @@
+//
+// Copyright (c) 2016 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef __se3_fake_fcl_hpp__
+#define __se3_fake_fcl_hpp__
+
+
+namespace se3
+{
+ namespace fcl
+ {
+
+ struct FakeCollisionGeometry
+ {
+ FakeCollisionGeometry(){};
+ };
+
+ struct AABB
+ {
+ AABB(): min_(0), max_(1){};
+
+ int min_;
+ int max_;
+ };
+ typedef FakeCollisionGeometry CollisionGeometry;
+
+ }
+}
+#endif // __se3_fake_fcl_hpp__
diff --git a/src/multibody/fcl.hpp b/src/multibody/fcl.hpp
index 5bfdef125..9730358ca 100644
--- a/src/multibody/fcl.hpp
+++ b/src/multibody/fcl.hpp
@@ -21,10 +21,14 @@
#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"
+#ifdef WITH_HPP_FCL
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/collision.h>
#include <hpp/fcl/distance.h>
#include <hpp/fcl/shape/geometric_shapes.h>
+#else
+#include "pinocchio/multibody/fake-fcl.hpp"
+#endif
#include <iostream>
#include <map>
@@ -66,7 +70,8 @@ namespace se3
}
}; // struct CollisionPair
typedef std::vector<CollisionPair> CollisionPairsVector_t;
-
+
+#ifdef WITH_HPP_FCL
/**
* @brief Result of distance computation between two CollisionObjects
*/
@@ -158,6 +163,7 @@ inline bool operator == (const fcl::CollisionObject & lhs, const fcl::CollisionO
&& lhs.getAABB().min_ == rhs.getAABB().min_
&& lhs.getAABB().max_ == rhs.getAABB().max_;
}
+#endif // WITH_HPP_FCL
enum GeometryType
{
VISUAL,
diff --git a/src/multibody/geometry.hpp b/src/multibody/geometry.hpp
index a5b4c1e93..b4047e8f8 100644
--- a/src/multibody/geometry.hpp
+++ b/src/multibody/geometry.hpp
@@ -117,7 +117,7 @@ namespace se3
*/
const std::string & getGeometryName(const GeomIndex index) const;
-
+#ifdef WITH_HPP_FCL
///
/// \brief Add a collision pair into the vector of collision_pairs.
/// The method check before if the given CollisionPair is already included.
@@ -163,6 +163,7 @@ namespace se3
/// \brief Display on std::cout the list of pairs (is it really useful?).
void displayCollisionPairs() const;
+#endif // WITH_HPP_FCL
/**
* @brief Associate a GeometryObject of type COLLISION to a joint's inner objects list
@@ -179,7 +180,6 @@ namespace se3
* @param[in] inner_object Index of the GeometryObject that will be an outer object
*/
void addOutterObject(const JointIndex joint, const GeomIndex outer_object);
-
friend std::ostream& operator<<(std::ostream & os, const GeometryModel & model_geom);
}; // struct GeometryModel
@@ -200,7 +200,7 @@ namespace se3
/// for fcl (collision) computation. The copy is done in collisionObjects[i]->setTransform(.)
///
std::vector<se3::SE3> oMg;
-
+#ifdef WITH_HPP_FCL
///
/// \brief Collision objects (ie a fcl placed geometry).
///
@@ -229,7 +229,6 @@ namespace se3
/// attached to the body from the joint center.
///
std::vector<double> radius;
-
GeometryData(const GeometryModel & modelGeom)
: model_geom(modelGeom)
, oMg(model_geom.ngeoms)
@@ -246,9 +245,16 @@ namespace se3
{ collisionObjects.push_back
(fcl::CollisionObject(geom.collision_geometry)); }
}
+#else
+ GeometryData(const GeometryModel & modelGeom)
+ : model_geom(modelGeom)
+ , oMg(model_geom.ngeoms)
+ {}
+#endif // WITH_HPP_FCL
- ~GeometryData() {};
+ ~GeometryData() {};
+#ifdef WITH_HPP_FCL
void activateCollisionPair(const Index pairId,const bool flag=true);
void deactivateCollisionPair(const Index pairId);
@@ -290,7 +296,7 @@ namespace se3
void computeAllDistances();
void resetDistances();
-
+#endif //WITH_HPP_FCL
friend std::ostream & operator<<(std::ostream & os, const GeometryData & data_geom);
}; // struct GeometryData
diff --git a/src/multibody/geometry.hxx b/src/multibody/geometry.hxx
index 5398b297e..759575dcd 100644
--- a/src/multibody/geometry.hxx
+++ b/src/multibody/geometry.hxx
@@ -109,16 +109,23 @@ namespace se3
inline std::ostream & operator<< (std::ostream & os, const GeometryData & data_geom)
{
+#ifdef WITH_HPP_FCL
os << "Nb collision pairs = " << data_geom.activeCollisionPairs.size() << std::endl;
for(Index i=0;i<(Index)(data_geom.activeCollisionPairs.size());++i)
{
os << "collision object in position " << data_geom.model_geom.collisionPairs[i] << std::endl;
}
+#else
+ os << "WARNING** Without fcl, no collision computations are possible. Only Positions can be computed" << std::endl;
+ os << "Nb of geometry objects = " << data_geom.oMg.size() << std::endl;
+#endif
return os;
}
+#ifdef WITH_HPP_FCL
+
inline void GeometryModel::addCollisionPair (const CollisionPair & pair)
{
assert( (pair.first < ngeoms) && (pair.second < ngeoms) );
@@ -246,7 +253,7 @@ namespace se3
{
std::fill(distance_results.begin(), distance_results.end(), DistanceResult( fcl::DistanceResult(), 0, 0) );
}
-
+#endif //WITH_HPP_FCL
} // namespace se3
/// @endcond
diff --git a/src/parsers/urdf.hpp b/src/parsers/urdf.hpp
index 9df51afcd..4eb54c72f 100644
--- a/src/parsers/urdf.hpp
+++ b/src/parsers/urdf.hpp
@@ -21,9 +21,7 @@
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/deprecated.hh"
-#ifdef WITH_HPP_FCL
- #include "pinocchio/multibody/geometry.hpp"
-#endif
+#include "pinocchio/multibody/geometry.hpp"
#include <urdf_model/model.h>
@@ -102,7 +100,6 @@ namespace se3
throw (std::invalid_argument)
{ Model m; return buildModel(filename,m,verbose); }
-#ifdef WITH_HPP_FCL
/**
* @brief Build The GeometryModel from a URDF file. Search for meshes
@@ -121,6 +118,8 @@ namespace se3
*
* @return Returns the reference on geom model for convenience.
*
+ * \warning If hpp-fcl has not been found during compilation, COLLISION types can not be loaded
+ *
*/
GeometryModel& buildGeom(const Model & model,
const std::string & filename,
@@ -143,6 +142,7 @@ namespace se3
*
* @return The GeometryModel associated to the urdf file and the given Model.
*
+ * \warning If hpp-fcl has not been found during compilation, COLLISION types can not be loaded
*/
PINOCCHIO_DEPRECATED
inline GeometryModel buildGeom(const Model & model,
@@ -152,7 +152,6 @@ namespace se3
throw (std::invalid_argument)
{ GeometryModel g; return buildGeom (model,filename,type,g,package_dirs); }
-#endif
} // namespace urdf
} // namespace se3
diff --git a/src/parsers/urdf/geometry.cpp b/src/parsers/urdf/geometry.cpp
index aaf0c5249..e5e2c15d7 100644
--- a/src/parsers/urdf/geometry.cpp
+++ b/src/parsers/urdf/geometry.cpp
@@ -36,6 +36,7 @@ namespace se3
{
namespace details
{
+#ifdef WITH_HPP_FCL
typedef fcl::BVHModel< fcl::OBBRSS > PolyhedronType;
typedef boost::shared_ptr <PolyhedronType> PolyhedronPtrType;
@@ -119,6 +120,7 @@ namespace se3
return geometry;
}
+#endif // WITH_HPP_FCL
/**
* @brief Get the first geometry attached to a link
@@ -197,7 +199,12 @@ namespace se3
std::size_t objectId = 0;
for (typename std::vector< boost::shared_ptr< T > >::const_iterator i = geometries_array.begin();i != geometries_array.end(); ++i)
{
+#ifdef WITH_HPP_FCL
const boost::shared_ptr<fcl::CollisionGeometry> geometry = retrieveCollisionGeometry((*i)->geometry, package_dirs, mesh_path);
+#else
+ const boost::shared_ptr<fcl::CollisionGeometry> geometry(new fcl::CollisionGeometry());
+#endif // WITH_HPP_FCL
+
SE3 geomPlacement = convertFromUrdf((*i)->origin);
std::ostringstream geometry_object_suffix;
geometry_object_suffix << "_" << objectId;
diff --git a/src/python/algorithms.hpp b/src/python/algorithms.hpp
index f4a3be720..847c2fe51 100644
--- a/src/python/algorithms.hpp
+++ b/src/python/algorithms.hpp
@@ -36,12 +36,11 @@
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
-#ifdef WITH_HPP_FCL
- #include "pinocchio/multibody/geometry.hpp"
- #include "pinocchio/python/geometry-model.hpp"
- #include "pinocchio/python/geometry-data.hpp"
- #include "pinocchio/algorithm/geometry.hpp"
-#endif
+#include "pinocchio/multibody/geometry.hpp"
+#include "pinocchio/python/geometry-model.hpp"
+#include "pinocchio/python/geometry-data.hpp"
+#include "pinocchio/algorithm/geometry.hpp"
+
namespace se3
{
@@ -302,7 +301,7 @@ namespace se3
return randomConfiguration(*model, lowerPosLimit, upperPosLimit);
}
-#ifdef WITH_HPP_FCL
+
static void updateGeometryPlacements_proxy(const ModelHandler & model,
DataHandler & data,
@@ -313,7 +312,7 @@ namespace se3
{
return updateGeometryPlacements(*model, *data, *geom_model, *geom_data, q);
}
-
+#ifdef WITH_HPP_FCL
static bool computeCollisions_proxy(GeometryDataHandler & data_geom,
const bool stopAtFirstCollision)
{
@@ -345,7 +344,7 @@ namespace se3
computeDistances(*model, *data, *model_geom, *data_geom, q);
}
-#endif
+#endif // WITH_HPP_FCL
/* --- Expose --------------------------------------------------------- */
@@ -540,14 +539,14 @@ namespace se3
// bp::def("randomConfiguration",randomConfiguration_proxy,
// bp::args("Model"),
// "Generate a random configuration ensuring Model's joint limits are respected ");
-#ifdef WITH_HPP_FCL
bp::def("updateGeometryPlacements",updateGeometryPlacements_proxy,
bp::args("Model", "Data", "GeometryModel", "GeometryData", "Configuration q (size Model::nq)"),
"Update the placement of the collision objects according to the current configuration."
"The algorithm also updates the current placement of the joint in Data."
);
-
+
+#ifdef WITH_HPP_FCL
bp::def("computeCollisions",computeCollisions_proxy,
bp::args("GeometryData","bool"),
"Determine if collision pairs are effectively in collision."
@@ -570,7 +569,7 @@ namespace se3
"compute the distance between each collision pair"
);
-#endif
+#endif // WITH_HPP_FCL
}
};
diff --git a/src/python/geometry-data.hpp b/src/python/geometry-data.hpp
index d3f433ea0..5d4a67bfb 100644
--- a/src/python/geometry-data.hpp
+++ b/src/python/geometry-data.hpp
@@ -96,6 +96,7 @@ namespace se3
bp::make_function(&GeometryDataPythonVisitor::oMg,
bp::return_internal_reference<>()),
"Vector of collision objects placement relative to the world.")
+#ifdef WITH_HPP_FCL
.add_property("distance_results",
bp::make_function(&GeometryDataPythonVisitor::distance_results,
bp::return_internal_reference<>()),
@@ -132,7 +133,7 @@ namespace se3
.def("computeAllDistances",&GeometryDataPythonVisitor::computeAllDistances,
"Same as computeDistance. It applies computeDistance to all collision pairs contained collision_pairs."
"The results are stored in collision_distances.")
-
+#endif // WITH_HPP_FCL
.def("__str__",&GeometryDataPythonVisitor::toString)
;
}
@@ -143,6 +144,7 @@ namespace se3
}
static std::vector<SE3> & oMg(GeometryDataHandler & m) { return m->oMg; }
+#ifdef WITH_HPP_FCL
static std::vector<DistanceResult> & distance_results( GeometryDataHandler & m ) { return m->distance_results; }
static std::vector<CollisionResult> & collision_results( GeometryDataHandler & m ) { return m->collision_results; }
static std::vector<bool> & activeCollisionPairs(GeometryDataHandler & m) { return m->activeCollisionPairs; }
@@ -162,7 +164,7 @@ namespace se3
return m->computeDistance(CollisionPair(co1, co2));
}
static void computeAllDistances(GeometryDataHandler & m) { m->computeAllDistances(); }
-
+#endif // WITH_HPP_FCL
static std::string toString(const GeometryDataHandler& m)
{ std::ostringstream s; s << *m; return s.str(); }
@@ -170,13 +172,13 @@ namespace se3
/* --- Expose --------------------------------------------------------- */
static void expose()
{
-
+#ifdef WITH_HPP_FCL
bp::class_< std::vector<DistanceResult> >("StdVec_DistanceResult")
.def(bp::vector_indexing_suite< std::vector<DistanceResult> >());
bp::class_< std::vector<CollisionResult> >("StdVec_CollisionResult")
.def(bp::vector_indexing_suite< std::vector<CollisionResult> >());
-
+#endif // WITH_HPP_FCL
bp::class_<GeometryDataHandler>("GeometryData",
"Geometry data linked to a geometry model and data struct.",
bp::no_init)
diff --git a/src/python/geometry-model.hpp b/src/python/geometry-model.hpp
index 3ba6a860e..13c6c9bca 100644
--- a/src/python/geometry-model.hpp
+++ b/src/python/geometry-model.hpp
@@ -70,7 +70,7 @@ namespace se3
bp::make_function(&GeometryModelPythonVisitor::geometryObjects,
bp::return_internal_reference<>()) )
.def("__str__",&GeometryModelPythonVisitor::toString)
-
+#ifdef WITH_HPP_FCL
.add_property("collision_pairs",
bp::make_function(&GeometryModelPythonVisitor::collision_pairs,
bp::return_internal_reference<>()),
@@ -95,7 +95,7 @@ namespace se3
bp::args("co1 (index)","co2 (index)"),
"Return the index of a collision pair given by the index of the two collision objects exists or not."
" Remark: co1 < co2")
-
+#endif // WITH_HPP_FCL
.def("BuildGeometryModel",&GeometryModelPythonVisitor::maker_default)
.staticmethod("BuildGeometryModel")
;
@@ -111,7 +111,7 @@ namespace se3
{ return gmodelPtr->getGeometryName(index);}
static std::vector<GeometryObject> & geometryObjects( GeometryModelHandler & m ) { return m->geometryObjects; }
-
+#ifdef WITH_HPP_FCL
static std::vector<CollisionPair> & collision_pairs( GeometryModelHandler & m )
{ return m->collisionPairs; }
@@ -133,7 +133,7 @@ namespace se3
static Index findCollisionPair (const GeometryModelHandler & m, const GeomIndex co1,
const GeomIndex co2)
{ return m->findCollisionPair( CollisionPair(co1,co2) ); }
-
+#endif // WITH_HPP_FCL
static GeometryModelHandler maker_default()
{ return GeometryModelHandler(new GeometryModel(), true); }
diff --git a/src/python/parsers.hpp b/src/python/parsers.hpp
index b54508dde..b29c5064e 100644
--- a/src/python/parsers.hpp
+++ b/src/python/parsers.hpp
@@ -26,11 +26,9 @@
#ifdef WITH_URDFDOM
#include "pinocchio/parsers/urdf.hpp"
-#ifdef WITH_HPP_FCL
#include "pinocchio/python/geometry-model.hpp"
#include "pinocchio/python/geometry-data.hpp"
#endif
-#endif
#ifdef WITH_LUA
#include "pinocchio/parsers/lua.hpp"
@@ -73,7 +71,6 @@ namespace se3
}
-#ifdef WITH_HPP_FCL
typedef std::pair<ModelHandler, GeometryModelHandler> ModelGeometryHandlerPair_t;
static GeometryModelHandler
@@ -102,6 +99,7 @@ namespace se3
return GeometryModelHandler(geometry_model, true);
}
+#ifdef WITH_HPP_FCL
static void removeCollisionPairsFromSrdf(ModelHandler & model,
GeometryModelHandler& geometry_model,
const std::string & filename,
@@ -157,7 +155,6 @@ namespace se3
"(remember to create the corresponding data structure)."
);
-#ifdef WITH_HPP_FCL
bp::to_python_converter<std::pair<ModelHandler, GeometryModelHandler>, PairToTupleConverter<ModelHandler, GeometryModelHandler> >();
@@ -174,6 +171,7 @@ namespace se3
"Parse the urdf file given in input looking for the geometry of the given Model and return a proper pinocchio geometry model "
"(remember to create the corresponding data structures).");
+#ifdef WITH_HPP_FCL
bp::def("removeCollisionPairsFromSrdf",removeCollisionPairsFromSrdf,
bp::args("Model", "GeometryModel (where pairs are removed)","srdf filename (string)", "verbosity"
),
diff --git a/unittest/geom.cpp b/unittest/geom.cpp
index 059432e40..d42efa613 100644
--- a/unittest/geom.cpp
+++ b/unittest/geom.cpp
@@ -136,6 +136,11 @@ void loadHumanoidPathPlanerModel (const hpp::model::HumanoidRobotPtr_t& robot,
BOOST_AUTO_TEST_SUITE ( GeomTest )
+BOOST_AUTO_TEST_CASE ( GeomNoFcl )
+{
+
+}
+
BOOST_AUTO_TEST_CASE ( simple_boxes )
{
using namespace se3;
--
GitLab