diff --git a/CMakeLists.txt b/CMakeLists.txt
index d1f102191f3c1e3617e6af15ea935064b4b2e3c9..3ab5ef5eefe86996331b2c03ec27e086147d567f 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,27 +242,25 @@ 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/fcl.hxx
+ 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
)
- LIST(APPEND ${PROJECT_NAME}_MULTIBODY_HEADERS
- multibody/geometry.hpp
- multibody/geometry.hxx
- )
- LIST(APPEND ${PROJECT_NAME}_ALGORITHM_HEADERS
- algorithm/geometry.hpp
- algorithm/geometry.hxx
- )
ENDIF(HPP_FCL_FOUND)
IF(LUA5_1_FOUND)
diff --git a/benchmark/CMakeLists.txt b/benchmark/CMakeLists.txt
index f11f2a0cd58931377e992e57995412ca37ca7ba5..1d254b4ecb6d5650a8230af0087446dfaa9b31f7 100644
--- a/benchmark/CMakeLists.txt
+++ b/benchmark/CMakeLists.txt
@@ -37,9 +37,13 @@ ENDIF(BUILD_PYTHON_INTERFACE)
IF(${URDFDOM_FOUND})
PKG_CONFIG_USE_DEPENDENCY(timings urdfdom)
ENDIF(${URDFDOM_FOUND})
+IF(HPP_FCL_FOUND)
+ PKG_CONFIG_USE_DEPENDENCY(timings hpp-fcl)
+ ADD_TEST_CFLAGS(timings "-DWITH_HPP_FCL")
+ENDIF(HPP_FCL_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)
@@ -48,6 +52,7 @@ ELSE(BUILD_BENCHMARK)
ENDIF(BUILD_BENCHMARK)
PKG_CONFIG_USE_DEPENDENCY(timings-eigen eigen3)
+
# geomTimings
#
IF(URDFDOM_FOUND AND HPP_FCL_FOUND)
@@ -56,16 +61,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 fd79b8672c7d81b679bf03b4df5e63c911b8b29c..c34ccea678c75405a19981f24624730fdcbe12df 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/CMakeLists.txt b/src/CMakeLists.txt
index 28d4e58ad7f1339198066e147805efad0598d4d5..9eee54a0714b03c493b09cd2a8be11c3a8e353ca 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -62,11 +62,9 @@ IF(URDFDOM_FOUND)
parsers/urdf/model.cpp
)
- IF(HPP_FCL_FOUND)
LIST(APPEND ${PROJECT_NAME}_PARSERS_SOURCES
parsers/urdf/geometry.cpp
)
- ENDIF(HPP_FCL_FOUND)
ENDIF(URDFDOM_FOUND)
IF(LUA5_1_FOUND)
diff --git a/src/algorithm/geometry.hpp b/src/algorithm/geometry.hpp
index f7303a1451a6350b17880e6942e9404eb847f47c..b99352e61b9855c389214859a24652986b70ee43 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 6b3db72e24fb5f0cb02fc51b38c01f412be6ea0b..3865d4e6147706fc2778452b595870e75356d949 100644
--- a/src/algorithm/geometry.hxx
+++ b/src/algorithm/geometry.hxx
@@ -40,15 +40,17 @@ namespace se3
GeometryData & data_geom
)
{
- for (GeometryData::GeomIndex i=0; i < (GeometryData::GeomIndex) data_geom.model_geom.ngeoms; ++i)
+ for (GeomIndex i=0; i < (GeomIndex) data_geom.model_geom.ngeoms; ++i)
{
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/fcl.hpp b/src/multibody/fcl.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..0ba8b92aca7f34c2f64071970ccf52889cd7aa10
--- /dev/null
+++ b/src/multibody/fcl.hpp
@@ -0,0 +1,238 @@
+//
+// Copyright (c) 2015-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_fcl_hpp__
+#define __se3_fcl_hpp__
+
+#include "pinocchio/multibody/fwd.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>
+#include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"
+#endif
+
+#include <iostream>
+#include <map>
+#include <list>
+#include <utility>
+#include <assert.h>
+
+#include <boost/foreach.hpp>
+#include <boost/shared_ptr.hpp>
+
+namespace se3
+{
+ struct CollisionPair: public std::pair<GeomIndex, GeomIndex>
+ {
+
+ typedef std::pair<GeomIndex, GeomIndex> Base;
+
+ ///
+ /// \brief Default constructor of a collision pair from two collision object indexes.
+ /// The indexes must be ordered such that co1 < co2. If not, the constructor reverts the indexes.
+ ///
+ /// \param[in] co1 Index of the first collision object
+ /// \param[in] co2 Index of the second collision object
+ ///
+ CollisionPair(const GeomIndex co1, const GeomIndex co2) : Base(co1,co2)
+ {
+ assert(co1 != co2 && "The index of collision objects must not be equal.");
+ }
+
+ bool operator== (const CollisionPair& rhs) const
+ {
+ return (first == rhs.first && second == rhs.second)
+ || (first == rhs.second && second == rhs.first);
+ }
+
+ void disp(std::ostream & os) const { os << "collision pair (" << first << "," << second << ")\n"; }
+ friend std::ostream & operator << (std::ostream & os, const CollisionPair & X);
+
+ }; // struct CollisionPair
+ typedef std::vector<CollisionPair> CollisionPairsVector_t;
+
+#ifdef WITH_HPP_FCL
+ /**
+ * @brief Result of distance computation between two CollisionObjects
+ */
+ struct DistanceResult
+ {
+
+ DistanceResult() : fcl_distance_result(), object1(0), object2(0) {}
+ DistanceResult(fcl::DistanceResult dist_fcl, const GeomIndex co1, const GeomIndex co2)
+ : fcl_distance_result(dist_fcl), object1(co1), object2(co2)
+ {}
+
+
+ ///
+ /// @brief Return the minimal distance between two geometry objects
+ ///
+ double distance () const;
+
+ ///
+ /// \brief Return the witness point on the inner object expressed in global frame.
+ ///
+ Eigen::Vector3d closestPointInner () const;
+
+ ///
+ /// \brief Return the witness point on the outer object expressed in global frame.
+ ///
+ Eigen::Vector3d closestPointOuter () const;
+
+ bool operator == (const DistanceResult & other) const
+ {
+ return (distance() == other.distance()
+ && closestPointInner() == other.closestPointInner()
+ && closestPointOuter() == other.closestPointOuter()
+ && object1 == other.object1
+ && object2 == other.object2);
+ }
+
+ /// \brief The FCL result of the distance computation
+ fcl::DistanceResult fcl_distance_result;
+
+ /// \brief Index of the first colision object
+ GeomIndex object1;
+
+ /// \brief Index of the second colision object
+ GeomIndex object2;
+
+ }; // struct DistanceResult
+
+
+ /**
+ * @brief Result of collision computation between two CollisionObjects
+ */
+ struct CollisionResult
+ {
+
+ /**
+ * @brief Default constrcutor of a CollisionResult
+ *
+ * @param[in] coll_fcl The FCL collision result
+ * @param[in] co1 Index of the first geometry object involved in the computation
+ * @param[in] co2 Index of the second geometry object involved in the computation
+ */
+ CollisionResult(fcl::CollisionResult coll_fcl, const GeomIndex co1, const GeomIndex co2)
+ : fcl_collision_result(coll_fcl), object1(co1), object2(co2)
+ {}
+ CollisionResult() : fcl_collision_result(), object1(0), object2(0) {}
+
+ bool operator == (const CollisionResult & other) const
+ {
+ return (fcl_collision_result == other.fcl_collision_result
+ && object1 == other.object1
+ && object2 == other.object2);
+ }
+
+ /// \brief The FCL result of the collision computation
+ fcl::CollisionResult fcl_collision_result;
+
+ /// \brief Index of the first collision object
+ GeomIndex object1;
+
+ /// \brief Index of the second collision object
+ GeomIndex object2;
+
+ }; // struct CollisionResult
+
+#else
+
+ namespace fcl
+ {
+
+ struct FakeCollisionGeometry
+ {
+ FakeCollisionGeometry(){};
+ };
+
+ struct AABB
+ {
+ AABB(): min_(0), max_(1){};
+
+ int min_;
+ int max_;
+ };
+ typedef FakeCollisionGeometry CollisionGeometry;
+
+ }
+
+#endif // WITH_HPP_FCL
+
+enum GeometryType
+{
+ VISUAL,
+ COLLISION,
+ NONE
+};
+
+struct GeometryObject
+{
+
+ /// \brief Name of the geometry object
+ std::string name;
+
+ /// \brief Index of the parent joint
+ JointIndex parent;
+
+ /// \brief The actual cloud of points representing the collision mesh of the object
+ boost::shared_ptr<fcl::CollisionGeometry> collision_geometry;
+
+ /// \brief Position of geometry object in parent joint's frame
+ SE3 placement;
+
+ /// \brief Absolute path to the mesh file
+ std::string mesh_path;
+
+
+ GeometryObject(const std::string & name, const JointIndex parent, const boost::shared_ptr<fcl::CollisionGeometry> & collision,
+ const SE3 & placement, const std::string & mesh_path)
+ : name(name)
+ , parent(parent)
+ , collision_geometry(collision)
+ , placement(placement)
+ , mesh_path(mesh_path)
+ {}
+
+ GeometryObject & operator=(const GeometryObject & other)
+ {
+ name = other.name;
+ parent = other.parent;
+ collision_geometry = other.collision_geometry;
+ placement = other.placement;
+ mesh_path = other.mesh_path;
+ return *this;
+ }
+
+ friend std::ostream & operator<< (std::ostream & os, const GeometryObject & geom_object);
+};
+
+
+
+
+} // namespace se3
+
+/* --- Details -------------------------------------------------------------- */
+/* --- Details -------------------------------------------------------------- */
+/* --- Details -------------------------------------------------------------- */
+#include "pinocchio/multibody/fcl.hxx"
+
+
+#endif // ifndef __se3_fcl_hpp__
diff --git a/src/multibody/fcl.hxx b/src/multibody/fcl.hxx
new file mode 100644
index 0000000000000000000000000000000000000000..4a5a4070b43b794a77793c6c7c601d57e19888eb
--- /dev/null
+++ b/src/multibody/fcl.hxx
@@ -0,0 +1,87 @@
+//
+// Copyright (c) 2015-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_fcl_hxx__
+#define __se3_fcl_hxx__
+
+
+#include <iostream>
+
+
+namespace se3
+{
+
+ inline std::ostream & operator << (std::ostream & os, const CollisionPair & X)
+ {
+ X.disp(os); return os;
+ }
+
+
+#ifdef WITH_HPP_FCL
+
+
+ inline double DistanceResult::distance () const
+ {
+ return fcl_distance_result.min_distance;
+ }
+
+ inline Eigen::Vector3d DistanceResult::closestPointInner () const
+ {
+ return toVector3d(fcl_distance_result.nearest_points [0]);
+ }
+
+ inline Eigen::Vector3d DistanceResult::closestPointOuter () const
+ {
+ return toVector3d(fcl_distance_result.nearest_points [1]);
+ }
+
+ inline bool operator == (const fcl::CollisionObject & lhs, const fcl::CollisionObject & rhs)
+ {
+ return lhs.collisionGeometry() == rhs.collisionGeometry()
+ && lhs.getAABB().min_ == rhs.getAABB().min_
+ && lhs.getAABB().max_ == rhs.getAABB().max_;
+ }
+
+#endif // WITH_HPP_FCL
+
+
+ inline bool operator==(const GeometryObject & lhs, const GeometryObject & rhs)
+ {
+ return ( lhs.name == rhs.name
+ && lhs.parent == rhs.parent
+ && lhs.collision_geometry == rhs.collision_geometry
+ && lhs.placement == rhs.placement
+ && lhs.mesh_path == rhs.mesh_path
+ );
+ }
+
+ inline std::ostream & operator<< (std::ostream & os, const GeometryObject & geom_object)
+ {
+ os << "Name: \t \n" << geom_object.name << "\n"
+ << "Parent ID: \t \n" << geom_object.parent << "\n"
+ // << "collision object: \t \n" << geom_object.collision_geometry << "\n"
+ << "Position in parent frame: \t \n" << geom_object.placement << "\n"
+ << "Absolute path to mesh file: \t \n" << geom_object.mesh_path << "\n"
+ << std::endl;
+ return os;
+ }
+
+
+} // namespace se3
+
+
+#endif // ifndef __se3_fcl_hxx__
diff --git a/src/multibody/geometry.hpp b/src/multibody/geometry.hpp
index a7f818740c0941d25fab46376d8886ad68ff5561..0a2aedd22e0825d6aa540216f9ea49a63cf43f4a 100644
--- a/src/multibody/geometry.hpp
+++ b/src/multibody/geometry.hpp
@@ -19,19 +19,11 @@
#define __se3_geom_hpp__
-#include "pinocchio/spatial/fwd.hpp"
-#include "pinocchio/spatial/se3.hpp"
-#include "pinocchio/spatial/force.hpp"
-#include "pinocchio/spatial/motion.hpp"
-#include "pinocchio/spatial/inertia.hpp"
-#include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"
+#include "pinocchio/multibody/fcl.hpp"
#include "pinocchio/multibody/model.hpp"
-#include "pinocchio/multibody/joint/joint-variant.hpp"
+
#include <iostream>
-#include <hpp/fcl/collision_object.h>
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/distance.h>
#include <boost/foreach.hpp>
#include <map>
#include <list>
@@ -41,210 +33,9 @@
namespace se3
{
- struct CollisionPair: public std::pair<Model::GeomIndex, Model::GeomIndex>
- {
- typedef Model::Index Index;
- typedef Model::GeomIndex GeomIndex;
- typedef std::pair<Model::GeomIndex, Model::GeomIndex> Base;
-
- ///
- /// \brief Default constructor of a collision pair from two collision object indexes.
- /// The indexes must be ordered such that co1 < co2. If not, the constructor reverts the indexes.
- ///
- /// \param[in] co1 Index of the first collision object
- /// \param[in] co2 Index of the second collision object
- ///
- CollisionPair(const GeomIndex co1, const GeomIndex co2) : Base(co1,co2)
- {
- assert(co1 != co2 && "The index of collision objects must not be equal.");
- }
-
- bool operator== (const CollisionPair& rhs) const
- {
- return (first == rhs.first && second == rhs.second)
- || (first == rhs.second && second == rhs.first);
- }
-
- void disp(std::ostream & os) const { os << "collision pair (" << first << "," << second << ")\n"; }
- friend std::ostream & operator << (std::ostream & os, const CollisionPair & X)
- {
- X.disp(os); return os;
- }
- }; // struct CollisionPair
- typedef std::vector<CollisionPair> CollisionPairsVector_t;
-
- /**
- * @brief Result of distance computation between two CollisionObjects
- */
- struct DistanceResult
- {
- typedef Model::Index Index;
- typedef Model::GeomIndex GeomIndex;
-
- DistanceResult() : fcl_distance_result(), object1(0), object2(0) {}
- DistanceResult(fcl::DistanceResult dist_fcl, const GeomIndex co1, const GeomIndex co2)
- : fcl_distance_result(dist_fcl), object1(co1), object2(co2)
- {}
-
-
- ///
- /// @brief Return the minimal distance between two geometry objects
- ///
- double distance () const { return fcl_distance_result.min_distance; }
-
- ///
- /// \brief Return the witness point on the inner object expressed in global frame.
- ///
- Eigen::Vector3d closestPointInner () const { return toVector3d(fcl_distance_result.nearest_points [0]); }
-
- ///
- /// \brief Return the witness point on the outer object expressed in global frame.
- ///
- Eigen::Vector3d closestPointOuter () const { return toVector3d(fcl_distance_result.nearest_points [1]); }
-
- bool operator == (const DistanceResult & other) const
- {
- return (distance() == other.distance()
- && closestPointInner() == other.closestPointInner()
- && closestPointOuter() == other.closestPointOuter()
- && object1 == other.object1
- && object2 == other.object2);
- }
-
- /// \brief The FCL result of the distance computation
- fcl::DistanceResult fcl_distance_result;
-
- /// \brief Index of the first colision object
- GeomIndex object1;
-
- /// \brief Index of the second colision object
- GeomIndex object2;
-
- }; // struct DistanceResult
-
- /**
- * @brief Result of collision computation between two CollisionObjects
- */
- struct CollisionResult
- {
- typedef Model::Index Index;
- typedef Model::GeomIndex GeomIndex;
-
- /**
- * @brief Default constrcutor of a CollisionResult
- *
- * @param[in] coll_fcl The FCL collision result
- * @param[in] co1 Index of the first geometry object involved in the computation
- * @param[in] co2 Index of the second geometry object involved in the computation
- */
- CollisionResult(fcl::CollisionResult coll_fcl, const GeomIndex co1, const GeomIndex co2)
- : fcl_collision_result(coll_fcl), object1(co1), object2(co2)
- {}
- CollisionResult() : fcl_collision_result(), object1(0), object2(0) {}
-
- bool operator == (const CollisionResult & other) const
- {
- return (fcl_collision_result == other.fcl_collision_result
- && object1 == other.object1
- && object2 == other.object2);
- }
-
- /// \brief The FCL result of the collision computation
- fcl::CollisionResult fcl_collision_result;
-
- /// \brief Index of the first collision object
- GeomIndex object1;
-
- /// \brief Index of the second collision object
- GeomIndex object2;
-
- }; // struct CollisionResult
-
-/// \brief Return true if the intrinsic geometry of the two CollisionObject is the same
-inline bool operator == (const fcl::CollisionObject & lhs, const fcl::CollisionObject & rhs)
-{
- return lhs.collisionGeometry() == rhs.collisionGeometry()
- && lhs.getAABB().min_ == rhs.getAABB().min_
- && lhs.getAABB().max_ == rhs.getAABB().max_;
-}
-enum GeometryType
-{
- VISUAL,
- COLLISION,
- NONE
-};
-
-struct GeometryObject
-{
- typedef Model::Index Index;
- typedef Model::JointIndex JointIndex;
- typedef Model::GeomIndex GeomIndex;
-
-
- /// \brief Name of the geometry object
- std::string name;
-
- /// \brief Index of the parent joint
- JointIndex parent;
-
- /// \brief The actual cloud of points representing the collision mesh of the object
- boost::shared_ptr<fcl::CollisionGeometry> collision_geometry;
-
- /// \brief Position of geometry object in parent joint's frame
- SE3 placement;
-
- /// \brief Absolute path to the mesh file
- std::string mesh_path;
-
-
- GeometryObject(const std::string & name, const JointIndex parent, const boost::shared_ptr<fcl::CollisionGeometry> & collision,
- const SE3 & placement, const std::string & mesh_path)
- : name(name)
- , parent(parent)
- , collision_geometry(collision)
- , placement(placement)
- , mesh_path(mesh_path)
- {}
-
- GeometryObject & operator=(const GeometryObject & other)
- {
- name = other.name;
- parent = other.parent;
- collision_geometry = other.collision_geometry;
- placement = other.placement;
- mesh_path = other.mesh_path;
- return *this;
- }
-
-};
-
- inline bool operator==(const GeometryObject & lhs, const GeometryObject & rhs)
- {
- return ( lhs.name == rhs.name
- && lhs.parent == rhs.parent
- && lhs.collision_geometry == rhs.collision_geometry
- && lhs.placement == rhs.placement
- && lhs.mesh_path == rhs.mesh_path
- );
- }
-
- inline std::ostream & operator<< (std::ostream & os, const GeometryObject & geom_object)
- {
- os << "Name: \t \n" << geom_object.name << "\n"
- << "Parent ID: \t \n" << geom_object.parent << "\n"
- // << "collision object: \t \n" << geom_object.collision_geometry << "\n"
- << "Position in parent frame: \t \n" << geom_object.placement << "\n"
- << "Absolute path to mesh file: \t \n" << geom_object.mesh_path << "\n"
- << std::endl;
- return os;
- }
-
struct GeometryModel
{
- typedef Model::Index Index;
- typedef Model::JointIndex JointIndex;
- typedef Model::GeomIndex GeomIndex;
typedef std::vector<GeomIndex> GeomIndexList;
@@ -326,7 +117,7 @@ struct GeometryObject
*/
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.
@@ -372,6 +163,7 @@ struct GeometryObject
/// \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
@@ -388,15 +180,12 @@ struct GeometryObject
* @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
struct GeometryData
{
- typedef Model::Index Index;
- typedef Model::GeomIndex GeomIndex;
-
+
///
/// \brief A const reference to the model storing all the geometries.
/// See class GeometryModel.
@@ -411,7 +200,7 @@ struct GeometryObject
/// 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).
///
@@ -440,7 +229,6 @@ struct GeometryObject
/// attached to the body from the joint center.
///
std::vector<double> radius;
-
GeometryData(const GeometryModel & modelGeom)
: model_geom(modelGeom)
, oMg(model_geom.ngeoms)
@@ -455,9 +243,16 @@ struct GeometryObject
{ 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);
@@ -499,7 +294,7 @@ struct GeometryObject
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 fa6a4e27690a63e0e4e7e99ace533f5e1624516a..759575dcd146ff71f24c8ce11d1a1efce769a083 100644
--- a/src/multibody/geometry.hxx
+++ b/src/multibody/geometry.hxx
@@ -19,19 +19,10 @@
#define __se3_geometry_hxx__
-#include "pinocchio/spatial/fwd.hpp"
-#include "pinocchio/spatial/se3.hpp"
-#include "pinocchio/spatial/force.hpp"
-#include "pinocchio/spatial/motion.hpp"
-#include "pinocchio/spatial/inertia.hpp"
-#include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"
-#include "pinocchio/multibody/model.hpp"
-#include "pinocchio/multibody/joint/joint-variant.hpp"
+
#include <iostream>
-#include <hpp/fcl/collision_object.h>
-#include <hpp/fcl/collision.h>
-#include <hpp/fcl/distance.h>
+
#include <map>
#include <list>
#include <utility>
@@ -41,11 +32,11 @@
namespace se3
{
- inline GeometryModel::GeomIndex GeometryModel::addGeometryObject(const JointIndex parent,
- const boost::shared_ptr<fcl::CollisionGeometry> & co,
- const SE3 & placement,
- const std::string & geom_name,
- const std::string & mesh_path) throw(std::invalid_argument)
+ inline GeomIndex GeometryModel::addGeometryObject(const JointIndex parent,
+ const boost::shared_ptr<fcl::CollisionGeometry> & co,
+ const SE3 & placement,
+ const std::string & geom_name,
+ const std::string & mesh_path) throw(std::invalid_argument)
{
Index idx = (Index) (ngeoms ++);
@@ -57,14 +48,14 @@ namespace se3
}
- inline GeometryModel::GeomIndex GeometryModel::getGeometryId(const std::string & name) const
+ inline GeomIndex GeometryModel::getGeometryId(const std::string & name) const
{
std::vector<GeometryObject>::const_iterator it = std::find_if(geometryObjects.begin(),
geometryObjects.end(),
boost::bind(&GeometryObject::name, _1) == name
);
- return GeometryModel::GeomIndex(it - geometryObjects.begin());
+ return GeomIndex(it - geometryObjects.begin());
}
@@ -108,7 +99,7 @@ namespace se3
{
os << "Nb geometry objects = " << model_geom.ngeoms << std::endl;
- for(GeometryModel::Index i=0;i<(GeometryModel::Index)(model_geom.ngeoms);++i)
+ for(Index i=0;i<(Index)(model_geom.ngeoms);++i)
{
os << model_geom.geometryObjects[i] <<std::endl;
}
@@ -118,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(GeometryData::Index i=0;i<(GeometryData::Index)(data_geom.activeCollisionPairs.size());++i)
+ 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) );
@@ -161,7 +159,7 @@ namespace se3
pair) != collisionPairs.end());
}
- inline GeometryModel::Index GeometryModel::findCollisionPair (const CollisionPair & pair) const
+ inline Index GeometryModel::findCollisionPair (const CollisionPair & pair) const
{
CollisionPairsVector_t::const_iterator it = std::find(collisionPairs.begin(),
collisionPairs.end(),
@@ -255,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 45c3a9b48146330b329f73438ca5aa23448e4523..4eb54c72ffe211b1a60bfb912d3fa3190e05e629 100644
--- a/src/parsers/urdf.hpp
+++ b/src/parsers/urdf.hpp
@@ -21,12 +21,7 @@
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/deprecated.hh"
-#ifdef WITH_HPP_FCL
- #include "pinocchio/multibody/geometry.hpp"
- #include <hpp/fcl/collision_object.h>
- #include <hpp/fcl/collision.h>
- #include <hpp/fcl/shape/geometric_shapes.h>
-#endif
+#include "pinocchio/multibody/geometry.hpp"
#include <urdf_model/model.h>
@@ -105,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
@@ -124,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,
@@ -146,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,
@@ -155,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 71a8f097f10545aff86d4018d11aa16d5f446049..93b9b434559c3fc37bc963a01810d77a3bbdfcff 100644
--- a/src/parsers/urdf/geometry.cpp
+++ b/src/parsers/urdf/geometry.cpp
@@ -15,6 +15,7 @@
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
+
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/urdf/utils.hpp"
#include "pinocchio/parsers/utils.hpp"
@@ -27,7 +28,9 @@
#include <iomanip>
#include <boost/foreach.hpp>
+#ifdef WITH_HPP_FCL
#include <hpp/fcl/mesh_loader/assimp.h>
+#endif // WITH_HPP_FCL
namespace se3
{
@@ -35,6 +38,7 @@ namespace se3
{
namespace details
{
+#ifdef WITH_HPP_FCL
typedef fcl::BVHModel< fcl::OBBRSS > PolyhedronType;
typedef boost::shared_ptr <PolyhedronType> PolyhedronPtrType;
@@ -118,6 +122,7 @@ namespace se3
return geometry;
}
+#endif // WITH_HPP_FCL
/**
* @brief Get the first geometry attached to a link
@@ -196,7 +201,16 @@ 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
+ mesh_path = retrieveResourcePath(boost::dynamic_pointer_cast< ::urdf::Mesh> ((*i)->geometry)->filename, package_dirs);
const boost::shared_ptr<fcl::CollisionGeometry> geometry = retrieveCollisionGeometry((*i)->geometry, package_dirs, mesh_path);
+#else
+ boost::shared_ptr < ::urdf::Mesh> urdf_mesh = boost::dynamic_pointer_cast< ::urdf::Mesh> ((*i)->geometry);
+ if (urdf_mesh) mesh_path = retrieveResourcePath(urdf_mesh->filename, package_dirs);
+
+ 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 f4a3be720d0e8ffb9938a92ed140e62bbf7f1bb4..847c2fe5166275830f23b98f3a4ee53b79da871e 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 69b2cad47d781a2d92c541a9b80d0c98b3f3ed57..5d4a67bfb036d892fb0390f0ab8cfebd7ee7ffa3 100644
--- a/src/python/geometry-data.hpp
+++ b/src/python/geometry-data.hpp
@@ -40,8 +40,7 @@ namespace se3
struct CollisionPairPythonVisitor
: public boost::python::def_visitor<CollisionPairPythonVisitor>
{
- typedef CollisionPair::Index Index;
- typedef CollisionPair::GeomIndex GeomIndex;
+
static void expose()
{
bp::class_<CollisionPair> ("CollisionPair",
@@ -69,8 +68,6 @@ namespace se3
struct GeometryDataPythonVisitor
: public boost::python::def_visitor< GeometryDataPythonVisitor >
{
- typedef GeometryData::Index Index;
- typedef GeometryData::GeomIndex GeomIndex;
typedef eigenpy::UnalignedEquivalent<SE3>::type SE3_fx;
/* --- Convert From C++ to Python ------------------------------------- */
@@ -99,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<>()),
@@ -135,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)
;
}
@@ -146,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; }
@@ -165,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(); }
@@ -173,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 8f01432fc2afe31df56b8c6635f71d29fa051b53..13c6c9bca0d5c24ebf294d137b88ffe11d5b3248 100644
--- a/src/python/geometry-model.hpp
+++ b/src/python/geometry-model.hpp
@@ -40,9 +40,7 @@ namespace se3
: public boost::python::def_visitor< GeometryModelPythonVisitor >
{
public:
- typedef GeometryModel::Index Index;
- typedef GeometryModel::JointIndex JointIndex;
- typedef GeometryModel::GeomIndex GeomIndex;
+
typedef eigenpy::UnalignedEquivalent<SE3>::type SE3_fx;
public:
@@ -72,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<>()),
@@ -97,15 +95,15 @@ 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")
;
}
- static GeometryModel::Index ngeoms( GeometryModelHandler & m ) { return m->ngeoms; }
+ static Index ngeoms( GeometryModelHandler & m ) { return m->ngeoms; }
- static Model::GeomIndex getGeometryId( const GeometryModelHandler & gmodelPtr, const std::string & name )
+ static GeomIndex getGeometryId( const GeometryModelHandler & gmodelPtr, const std::string & name )
{ return gmodelPtr->getGeometryId(name); }
static bool existGeometryName(const GeometryModelHandler & gmodelPtr, const std::string & name)
{ return gmodelPtr->existGeometryName(name);}
@@ -113,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; }
@@ -132,10 +130,10 @@ namespace se3
static bool existCollisionPair (const GeometryModelHandler & m, const GeomIndex co1, const GeomIndex co2)
{ return m->existCollisionPair(CollisionPair(co1,co2)); }
- static GeometryModel::Index findCollisionPair (const GeometryModelHandler & m, const GeomIndex co1,
+ 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 b54508dde7c7e3e3827490a2f04be38d5bc63748..b29c5064e7c6ace10c714116ef2ec3eb0dd01cfc 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/src/python/python.cpp b/src/python/python.cpp
index cad794b906567db1514f58fb163ecbd77757fdba..bb42329b8a0761836370ac47f4aefed16155c038 100644
--- a/src/python/python.cpp
+++ b/src/python/python.cpp
@@ -32,11 +32,9 @@
#include "pinocchio/python/parsers.hpp"
#include "pinocchio/python/explog.hpp"
-#ifdef WITH_HPP_FCL
#include "pinocchio/python/geometry-object.hpp"
#include "pinocchio/python/geometry-model.hpp"
#include "pinocchio/python/geometry-data.hpp"
-#endif
namespace se3
{
@@ -74,12 +72,10 @@ namespace se3
FramePythonVisitor::expose();
ModelPythonVisitor::expose();
DataPythonVisitor::expose();
-#ifdef WITH_HPP_FCL
GeometryObjectPythonVisitor::expose();
CollisionPairPythonVisitor::expose();
GeometryModelPythonVisitor::expose();
GeometryDataPythonVisitor::expose();
-#endif
}
void exposeAlgorithms()
{
diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt
index f3615c35b25e22dd5fefe93174701d1c70f2c6f4..a9798562b1fbc6922ca945e5073e6b9248e361f8 100644
--- a/unittest/CMakeLists.txt
+++ b/unittest/CMakeLists.txt
@@ -28,8 +28,15 @@ MACRO(ADD_UNIT_TEST NAME PKGS)
PKG_CONFIG_USE_DEPENDENCY(${NAME} ${PKG})
ENDFOREACH(PKG)
+ IF(HPP_FCL_FOUND)
+ PKG_CONFIG_USE_DEPENDENCY(${NAME} hpp-fcl)
+ ADD_TEST_CFLAGS(${NAME} "-DWITH_HPP_FCL")
+ ENDIF(HPP_FCL_FOUND)
+
TARGET_LINK_LIBRARIES(${NAME} ${PROJECT_NAME})
+
IF(BUILD_PYTHON_INTERFACE)
+ TARGET_LINK_BOOST_PYTHON(${NAME})
TARGET_LINK_LIBRARIES(${NAME} ${PYTHON_LIBRARIES})
ENDIF(BUILD_PYTHON_INTERFACE)
@@ -68,9 +75,8 @@ IF(URDFDOM_FOUND)
ADD_UNIT_TEST(value "eigen3;urdfdom")
ADD_TEST_CFLAGS(value '-DPINOCCHIO_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"')
IF(HPP_FCL_FOUND)
- ADD_UNIT_TEST(geom "eigen3;urdfdom;hpp-fcl")
+ ADD_UNIT_TEST(geom "eigen3;urdfdom")
ADD_TEST_CFLAGS(geom '-DPINOCCHIO_SOURCE_DIR=\\\"${${PROJECT_NAME}_SOURCE_DIR}\\\"')
- ADD_TEST_CFLAGS(geom "-DWITH_HPP_FCL")
IF(BUILD_TESTS_WITH_HPP)
ADD_OPTIONAL_DEPENDENCY("hpp-model-urdf")
IF(HPP_MODEL_URDF_FOUND)
diff --git a/unittest/geom.cpp b/unittest/geom.cpp
index dc9b53ed5e5b2ef79f3e3a0bb253d4befc0ebcd1..b997f0fd43c2b21d52f972a242a7a25b2cf19c4e 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;
diff --git a/utils/CMakeLists.txt b/utils/CMakeLists.txt
index 66faac4139fc5c0387b88e72ccb32d5ed322b7ad..66bbd35710d7f967093ac3396a1805d639977139 100644
--- a/utils/CMakeLists.txt
+++ b/utils/CMakeLists.txt
@@ -38,7 +38,8 @@ MACRO(ADD_UTIL NAME UTIL_SRC PKGS)
IF(BUILD_UTILS)
INSTALL(TARGETS ${NAME} DESTINATION bin)
- ENDIF(BUILD_UTILS)
+ ENDIF(BUILD_UTILS)
+
ENDMACRO(ADD_UTIL)