Unverified Commit 3852a0f8 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #116 from jcarpent/topic/python

Improve bindings + remove borring warnings + fix
parents f46b141f 0fef5b66
...@@ -41,6 +41,9 @@ set(PROJECT_DESCRIPTION ...@@ -41,6 +41,9 @@ set(PROJECT_DESCRIPTION
) )
SET(PROJECT_USE_CMAKE_EXPORT TRUE) SET(PROJECT_USE_CMAKE_EXPORT TRUE)
SET(CMAKE_C_STANDARD 99)
SET(CMAKE_CXX_STANDARD 98)
# Do not support CMake older than 2.8.12 # Do not support CMake older than 2.8.12
CMAKE_POLICY(SET CMP0022 NEW) CMAKE_POLICY(SET CMP0022 NEW)
SET(PROJECT_USE_KEYWORD_LINK_LIBRARIES TRUE) SET(PROJECT_USE_KEYWORD_LINK_LIBRARIES TRUE)
...@@ -55,6 +58,7 @@ include(cmake/boost.cmake) ...@@ -55,6 +58,7 @@ include(cmake/boost.cmake)
include(cmake/python.cmake) include(cmake/python.cmake)
include(cmake/hpp.cmake) include(cmake/hpp.cmake)
include(cmake/apple.cmake) include(cmake/apple.cmake)
include(cmake/ide.cmake)
# If needed, fix CMake policy for APPLE systems # If needed, fix CMake policy for APPLE systems
APPLY_DEFAULT_APPLE_CONFIGURATION() APPLY_DEFAULT_APPLE_CONFIGURATION()
......
Subproject commit 308d3c947a3e276b06c5fa79894119346635de4d Subproject commit a99dc925bde7b976ae1ed286d0ab38906c73dff5
...@@ -38,14 +38,15 @@ ...@@ -38,14 +38,15 @@
#ifndef HPP_FCL_AABB_H #ifndef HPP_FCL_AABB_H
#define HPP_FCL_AABB_H #define HPP_FCL_AABB_H
#include <stdexcept>
#include <hpp/fcl/data_types.h> #include <hpp/fcl/data_types.h>
namespace hpp namespace hpp
{ {
namespace fcl namespace fcl
{ {
class CollisionRequest;
struct CollisionRequest;
/// @defgroup Bounding_Volume /// @defgroup Bounding_Volume
/// regroup class of differents types of bounding volume. /// regroup class of differents types of bounding volume.
/// @{ /// @{
......
...@@ -62,7 +62,7 @@ template<typename BV1, typename BV2> ...@@ -62,7 +62,7 @@ template<typename BV1, typename BV2>
class Converter class Converter
{ {
private: private:
static void convert(const BV1& bv1, const Transform3f& tf1, BV2& bv2) static void convert(const BV1& /*bv1*/, const Transform3f& /*tf1*/, BV2& /*bv2*/)
{ {
// should only use the specialized version, so it is private. // should only use the specialized version, so it is private.
} }
......
...@@ -107,11 +107,15 @@ struct BVNode : public BVNodeBase ...@@ -107,11 +107,15 @@ struct BVNode : public BVNodeBase
return bv.distance(other.bv, P1, P2); return bv.distance(other.bv, P1, P2);
} }
/// @brief Access the center of the BV /// @brief Access to the center of the BV
Vec3f getCenter() const { return bv.center(); } Vec3f getCenter() const { return bv.center(); }
/// @brief Access the orientation of the BV /// @brief Access to the orientation of the BV
const Matrix3f& getOrientation() const { return Matrix3f::Identity(); } const Matrix3f& getOrientation() const
{
static const Matrix3f id3 = Matrix3f::Identity();
return id3;
}
}; };
template<> template<>
......
...@@ -44,7 +44,8 @@ namespace hpp ...@@ -44,7 +44,8 @@ namespace hpp
{ {
namespace fcl namespace fcl
{ {
class CollisionRequest;
struct CollisionRequest;
/// @addtogroup Bounding_Volume /// @addtogroup Bounding_Volume
/// @{ /// @{
......
...@@ -46,7 +46,8 @@ namespace hpp ...@@ -46,7 +46,8 @@ namespace hpp
{ {
namespace fcl namespace fcl
{ {
class CollisionRequest;
struct CollisionRequest;
/// @addtogroup Bounding_Volume /// @addtogroup Bounding_Volume
/// @{ /// @{
......
...@@ -38,7 +38,6 @@ ...@@ -38,7 +38,6 @@
#ifndef HPP_FCL_RSS_H #ifndef HPP_FCL_RSS_H
#define HPP_FCL_RSS_H #define HPP_FCL_RSS_H
#include <stdexcept>
#include <hpp/fcl/data_types.h> #include <hpp/fcl/data_types.h>
#include <boost/math/constants/constants.hpp> #include <boost/math/constants/constants.hpp>
...@@ -47,7 +46,8 @@ namespace hpp ...@@ -47,7 +46,8 @@ namespace hpp
namespace fcl namespace fcl
{ {
class CollisionRequest; struct CollisionRequest;
/// @addtogroup Bounding_Volume /// @addtogroup Bounding_Volume
/// @{ /// @{
...@@ -164,4 +164,4 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, ...@@ -164,4 +164,4 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
} // namespace hpp } // namespace hpp
#endif #endif
\ No newline at end of file
...@@ -46,7 +46,7 @@ namespace hpp ...@@ -46,7 +46,7 @@ namespace hpp
namespace fcl namespace fcl
{ {
class CollisionRequest; struct CollisionRequest;
/// @addtogroup Bounding_Volume /// @addtogroup Bounding_Volume
/// @{ /// @{
......
...@@ -45,7 +45,8 @@ namespace hpp ...@@ -45,7 +45,8 @@ namespace hpp
{ {
namespace fcl namespace fcl
{ {
class CollisionRequest;
struct CollisionRequest;
/// @addtogroup Bounding_Volume /// @addtogroup Bounding_Volume
/// @{ /// @{
......
...@@ -129,6 +129,11 @@ struct Contact ...@@ -129,6 +129,11 @@ struct Contact
&& pos == other.pos && pos == other.pos
&& penetration_depth == other.penetration_depth; && penetration_depth == other.penetration_depth;
} }
bool operator != (const Contact& other) const
{
return !(*this == other);
}
}; };
struct CollisionResult; struct CollisionResult;
......
...@@ -206,4 +206,5 @@ bool isEqual(const Eigen::MatrixBase<Derived>& lhs, const Eigen::MatrixBase<Othe ...@@ -206,4 +206,5 @@ bool isEqual(const Eigen::MatrixBase<Derived>& lhs, const Eigen::MatrixBase<Othe
} }
} // namespace hpp } // namespace hpp
#endif #endif
\ No newline at end of file
...@@ -181,4 +181,5 @@ public: ...@@ -181,4 +181,5 @@ public:
/// @endcond /// @endcond
#endif #endif
\ No newline at end of file
...@@ -55,17 +55,6 @@ static inline std::ostream& operator << (std::ostream& o, const Quaternion3f& q) ...@@ -55,17 +55,6 @@ static inline std::ostream& operator << (std::ostream& o, const Quaternion3f& q)
return o; return o;
} }
inline bool isQuatIdentity (const Quaternion3f& q)
{
return (q.w() == 1 || q.w() == -1) && q.x() == 0 && q.y() == 0 && q.z() == 0;
}
inline bool areQuatEquals (const Quaternion3f& q1, const Quaternion3f& q2)
{
return (q1.w() == q2.w() && q1.x() == q2.x() && q1.y() == q2.y() && q1.z() == q2.z())
|| (q1.w() == -q2.w() && q1.x() == -q2.x() && q1.y() == -q2.y() && q1.z() == -q2.z());
}
/// @brief Simple transform class used locally by InterpMotion /// @brief Simple transform class used locally by InterpMotion
class Transform3f class Transform3f
{ {
...@@ -140,8 +129,9 @@ public: ...@@ -140,8 +129,9 @@ public:
} }
/// @brief set transform from rotation and translation /// @brief set transform from rotation and translation
template <typename Matrixx3Like, typename Vector3Like> template <typename Matrix3Like, typename Vector3Like>
inline void setTransform(const Eigen::MatrixBase<Matrixx3Like>& R_, const Eigen::MatrixBase<Vector3Like>& T_) inline void setTransform(const Eigen::MatrixBase<Matrix3Like>& R_,
const Eigen::MatrixBase<Vector3Like>& T_)
{ {
R.noalias() = R_; R.noalias() = R_;
T.noalias() = T_; T.noalias() = T_;
...@@ -242,7 +232,7 @@ public: ...@@ -242,7 +232,7 @@ public:
template<typename Derived> template<typename Derived>
inline Quaternion3f fromAxisAngle(const Eigen::MatrixBase<Derived>& axis, FCL_REAL angle) inline Quaternion3f fromAxisAngle(const Eigen::MatrixBase<Derived>& axis, FCL_REAL angle)
{ {
return Quaternion3f (Eigen::AngleAxis<double>(angle, axis)); return Quaternion3f (Eigen::AngleAxis<FCL_REAL>(angle, axis));
} }
} }
......
...@@ -39,13 +39,22 @@ SET(LIBRARY_NAME hppfcl) ...@@ -39,13 +39,22 @@ SET(LIBRARY_NAME hppfcl)
INCLUDE_DIRECTORIES("${Boost_INCLUDE_DIRS}" ${PYTHON_INCLUDE_DIRS}) INCLUDE_DIRECTORIES("${Boost_INCLUDE_DIRS}" ${PYTHON_INCLUDE_DIRS})
ADD_LIBRARY(${LIBRARY_NAME} SHARED SET(${LIBRARY_NAME}_HEADERS
fcl.hh
)
SET(${LIBRARY_NAME}_SOURCES
version.cc version.cc
math.cc math.cc
collision-geometries.cc collision-geometries.cc
collision.cc collision.cc
distance.cc distance.cc
fcl.cc) fcl.cc
)
ADD_LIBRARY(${LIBRARY_NAME} SHARED ${${LIBRARY_NAME}_SOURCES} ${${LIBRARY_NAME}_HEADERS})
ADD_HEADER_GROUP(${LIBRARY_NAME}_HEADER)
ADD_SOURCE_GROUP(${LIBRARY_NAME}_SOURCES)
TARGET_LINK_BOOST_PYTHON(${LIBRARY_NAME} PUBLIC) TARGET_LINK_BOOST_PYTHON(${LIBRARY_NAME} PUBLIC)
TARGET_LINK_LIBRARIES(${LIBRARY_NAME} PUBLIC ${PROJECT_NAME} ${BOOST_system_LIBRARY}) TARGET_LINK_LIBRARIES(${LIBRARY_NAME} PUBLIC ${PROJECT_NAME} ${BOOST_system_LIBRARY})
......
// //
// Software License Agreement (BSD License) // Software License Agreement (BSD License)
// //
// Copyright (c) 2019 CNRS-LAAS // Copyright (c) 2019 CNRS-LAAS INRIA
// Author: Joseph Mirabel // Author: Joseph Mirabel
// All rights reserved. // All rights reserved.
// //
...@@ -34,6 +34,8 @@ ...@@ -34,6 +34,8 @@
#include <boost/python.hpp> #include <boost/python.hpp>
#include <eigenpy/registration.hpp>
#include "fcl.hh" #include "fcl.hh"
#include <hpp/fcl/fwd.hh> #include <hpp/fcl/fwd.hh>
...@@ -183,48 +185,59 @@ void exposeShapes () ...@@ -183,48 +185,59 @@ void exposeShapes ()
void exposeCollisionGeometries () void exposeCollisionGeometries ()
{ {
enum_<OBJECT_TYPE>("OBJECT_TYPE")
.value ("OT_UNKNOWN", OT_UNKNOWN) if(!eigenpy::register_symbolic_link_to_registered_type<OBJECT_TYPE>())
.value ("OT_BVH" , OT_BVH) {
.value ("OT_GEOM" , OT_GEOM) enum_<OBJECT_TYPE>("OBJECT_TYPE")
.value ("OT_OCTREE" , OT_OCTREE) .value ("OT_UNKNOWN", OT_UNKNOWN)
; .value ("OT_BVH" , OT_BVH)
enum_<NODE_TYPE>("NODE_TYPE") .value ("OT_GEOM" , OT_GEOM)
.value ("BV_UNKNOWN", BV_UNKNOWN) .value ("OT_OCTREE" , OT_OCTREE)
.value ("BV_AABB" , BV_AABB) ;
.value ("BV_OBB" , BV_OBB) }
.value ("BV_RSS" , BV_RSS)
.value ("BV_kIOS" , BV_kIOS) if(!eigenpy::register_symbolic_link_to_registered_type<NODE_TYPE>())
.value ("BV_OBBRSS", BV_OBBRSS) {
.value ("BV_KDOP16", BV_KDOP16) enum_<NODE_TYPE>("NODE_TYPE")
.value ("BV_KDOP18", BV_KDOP18) .value ("BV_UNKNOWN", BV_UNKNOWN)
.value ("BV_KDOP24", BV_KDOP24) .value ("BV_AABB" , BV_AABB)
.value ("GEOM_BOX" , GEOM_BOX) .value ("BV_OBB" , BV_OBB)
.value ("GEOM_SPHERE" , GEOM_SPHERE) .value ("BV_RSS" , BV_RSS)
.value ("GEOM_CAPSULE" , GEOM_CAPSULE) .value ("BV_kIOS" , BV_kIOS)
.value ("GEOM_CONE" , GEOM_CONE) .value ("BV_OBBRSS", BV_OBBRSS)
.value ("GEOM_CYLINDER" , GEOM_CYLINDER) .value ("BV_KDOP16", BV_KDOP16)
.value ("GEOM_CONVEX" , GEOM_CONVEX) .value ("BV_KDOP18", BV_KDOP18)
.value ("GEOM_PLANE" , GEOM_PLANE) .value ("BV_KDOP24", BV_KDOP24)
.value ("GEOM_HALFSPACE", GEOM_HALFSPACE) .value ("GEOM_BOX" , GEOM_BOX)
.value ("GEOM_TRIANGLE" , GEOM_TRIANGLE) .value ("GEOM_SPHERE" , GEOM_SPHERE)
.value ("GEOM_OCTREE" , GEOM_OCTREE) .value ("GEOM_CAPSULE" , GEOM_CAPSULE)
; .value ("GEOM_CONE" , GEOM_CONE)
.value ("GEOM_CYLINDER" , GEOM_CYLINDER)
.value ("GEOM_CONVEX" , GEOM_CONVEX)
.value ("GEOM_PLANE" , GEOM_PLANE)
.value ("GEOM_HALFSPACE", GEOM_HALFSPACE)
.value ("GEOM_TRIANGLE" , GEOM_TRIANGLE)
.value ("GEOM_OCTREE" , GEOM_OCTREE)
;
}
class_ <CollisionGeometry, CollisionGeometryPtr_t, noncopyable> if(!eigenpy::register_symbolic_link_to_registered_type<CollisionGeometry>())
("CollisionGeometry", no_init) {
.def ("getObjectType", &CollisionGeometry::getObjectType) class_ <CollisionGeometry, CollisionGeometryPtr_t, noncopyable>
.def ("getNodeType", &CollisionGeometry::getNodeType) ("CollisionGeometry", no_init)
.def ("getObjectType", &CollisionGeometry::getObjectType)
.def ("getNodeType", &CollisionGeometry::getNodeType)
.def ("computeLocalAABB", &CollisionGeometry::computeLocalAABB) .def ("computeLocalAABB", &CollisionGeometry::computeLocalAABB)
.def ("computeCOM", &CollisionGeometry::computeCOM) .def ("computeCOM", &CollisionGeometry::computeCOM)
.def ("computeMomentofInertia", &CollisionGeometry::computeMomentofInertia) .def ("computeMomentofInertia", &CollisionGeometry::computeMomentofInertia)
.def ("computeVolume", &CollisionGeometry::computeVolume) .def ("computeVolume", &CollisionGeometry::computeVolume)
.def ("computeMomentofInertiaRelatedToCOM", &CollisionGeometry::computeMomentofInertiaRelatedToCOM) .def ("computeMomentofInertiaRelatedToCOM", &CollisionGeometry::computeMomentofInertiaRelatedToCOM)
.def_readwrite("aabb_radius",&CollisionGeometry::aabb_radius,"AABB radius") .def_readwrite("aabb_radius",&CollisionGeometry::aabb_radius,"AABB radius")
; ;
}
exposeShapes(); exposeShapes();
......
// //
// Software License Agreement (BSD License) // Software License Agreement (BSD License)
// //
// Copyright (c) 2019 CNRS-LAAS // Copyright (c) 2019 CNRS-LAAS INRIA
// Author: Joseph Mirabel // Author: Joseph Mirabel
// All rights reserved. // All rights reserved.
// //
...@@ -35,6 +35,8 @@ ...@@ -35,6 +35,8 @@
#include <boost/python.hpp> #include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp> #include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <eigenpy/registration.hpp>
#include "fcl.hh" #include "fcl.hh"
#include <hpp/fcl/fwd.hh> #include <hpp/fcl/fwd.hh>
...@@ -43,58 +45,75 @@ ...@@ -43,58 +45,75 @@
using namespace boost::python; using namespace boost::python;
using namespace hpp::fcl; using namespace hpp::fcl;
using boost::shared_ptr;
using boost::noncopyable;
void exposeCollisionAPI () void exposeCollisionAPI ()
{ {
enum_ <CollisionRequestFlag> ("CollisionRequestFlag") if(!eigenpy::register_symbolic_link_to_registered_type<CollisionRequestFlag>())
.value ("CONTACT", CONTACT) {
.value ("DISTANCE_LOWER_BOUND", DISTANCE_LOWER_BOUND) enum_ <CollisionRequestFlag> ("CollisionRequestFlag")
.value ("NO_REQUEST", NO_REQUEST) .value ("CONTACT", CONTACT)
; .value ("DISTANCE_LOWER_BOUND", DISTANCE_LOWER_BOUND)
.value ("NO_REQUEST", NO_REQUEST)
class_ <CollisionRequest> ("CollisionRequest", init<>()) ;
.def (init<CollisionRequestFlag, size_t>()) }
.def_readwrite ("num_max_contacts" , &CollisionRequest::num_max_contacts) if(!eigenpy::register_symbolic_link_to_registered_type<CollisionRequest>())
.def_readwrite ("enable_contact" , &CollisionRequest::enable_contact) {
.def_readwrite ("enable_distance_lower_bound", &CollisionRequest::enable_distance_lower_bound) class_ <CollisionRequest> ("CollisionRequest", init<>())
.def_readwrite ("enable_cached_gjk_guess" , &CollisionRequest::enable_cached_gjk_guess) .def (init<CollisionRequestFlag, size_t>())
.def_readwrite ("cached_gjk_guess" , &CollisionRequest::cached_gjk_guess)
.def_readwrite ("security_margin" , &CollisionRequest::security_margin) .def_readwrite ("num_max_contacts" , &CollisionRequest::num_max_contacts)
.def_readwrite ("break_distance" , &CollisionRequest::break_distance) .def_readwrite ("enable_contact" , &CollisionRequest::enable_contact)
; .def_readwrite ("enable_distance_lower_bound", &CollisionRequest::enable_distance_lower_bound)
.def_readwrite ("enable_cached_gjk_guess" , &CollisionRequest::enable_cached_gjk_guess)
.def_readwrite ("cached_gjk_guess" , &CollisionRequest::cached_gjk_guess)
.def_readwrite ("security_margin" , &CollisionRequest::security_margin)
.def_readwrite ("break_distance" , &CollisionRequest::break_distance)
;
}
class_ <Contact> ("Contact", init<>()) if(!eigenpy::register_symbolic_link_to_registered_type<Contact>())
//.def(init<CollisionGeometryPtr_t, CollisionGeometryPtr_t, int, int>()) {
//.def(init<CollisionGeometryPtr_t, CollisionGeometryPtr_t, int, int, Vec3f, Vec3f, FCL_REAL>()) class_ <Contact> ("Contact", init<>())
.def_readonly ("o1", &Contact::o1) //.def(init<CollisionGeometryPtr_t, CollisionGeometryPtr_t, int, int>())
.def_readonly ("o2", &Contact::o2) //.def(init<CollisionGeometryPtr_t, CollisionGeometryPtr_t, int, int, Vec3f, Vec3f, FCL_REAL>())
.def_readwrite ("b1", &Contact::b1) .def_readonly ("o1", &Contact::o1)
.def_readwrite ("b2", &Contact::b2) .def_readonly ("o2", &Contact::o2)
.def_readwrite ("normal", &Contact::normal) .def_readwrite ("b1", &Contact::b1)
.def_readwrite ("pos", &Contact::pos) .def_readwrite ("b2", &Contact::b2)
.def_readwrite ("penetration_depth", &Contact::penetration_depth) .def_readwrite ("normal", &Contact::normal)
.def (self == self) .def_readwrite ("pos", &Contact::pos)
; .def_readwrite ("penetration_depth", &Contact::penetration_depth)
.def (self == self)
.def (self != self)
;
}
class_< std::vector<Contact> >("StdVec_Contact") if(!eigenpy::register_symbolic_link_to_registered_type< std::vector<Contact> >())
.def(vector_indexing_suite< std::vector<Contact> >()) {
; class_< std::vector<Contact> >("StdVec_Contact")
.def(vector_indexing_suite< std::vector<Contact> >())
;
}
class_ <CollisionResult> ("CollisionResult", init<>()) if(!eigenpy::register_symbolic_link_to_registered_type< std::vector<CollisionResult> >())
.def ("isCollision", &CollisionResult::isCollision) {
.def ("numContacts", &CollisionResult::numContacts) class_ <CollisionResult> ("CollisionResult", init<>())
.def ("getContact" , &CollisionResult::getContact , return_value_policy<copy_const_reference>()) .def ("isCollision", &CollisionResult::isCollision)
.def ("getContacts", &CollisionResult::getContacts, return_internal_reference<>()) .def ("numContacts", &CollisionResult::numContacts)
.def ("addContact" , &CollisionResult::addContact ) .def ("getContact" , &CollisionResult::getContact , return_value_policy<copy_const_reference>())
.def ("clear", &CollisionResult::clear) .def ("getContacts", &CollisionResult::getContacts, return_internal_reference<>())
; .def ("addContact" , &CollisionResult::addContact )
.def ("clear", &CollisionResult::clear)
;
}
class_< std::vector<CollisionResult> >("CollisionResult") if(!eigenpy::register_symbolic_link_to_registered_type< std::vector<CollisionResult> >())
.def(vector_indexing_suite< std::vector<CollisionResult> >()) {
; class_< std::vector<CollisionResult> >("CollisionResult")
.def(vector_indexing_suite< std::vector<CollisionResult> >())
;
}
def ("collide", static_cast< std::size_t (*)(const CollisionObject*, const CollisionObject*, def ("collide", static_cast< std::size_t (*)(const CollisionObject*, const CollisionObject*,
const CollisionRequest&, CollisionResult&) > (&collide)); const CollisionRequest&, CollisionResult&) > (&collide));
......
// //
// Software License Agreement (BSD License) // Software License Agreement (BSD License)
// //
// Copyright (c) 2019 CNRS-LAAS // Copyright (c) 2019 CNRS-LAAS INRIA
// Author: Joseph Mirabel // Author: Joseph Mirabel
// All rights reserved. // All rights reserved.
// //
...@@ -35,6 +35,8 @@ ...@@ -35,6 +35,8 @@
#include <boost/python.hpp>