Commit 9e0f9d3b authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[Python] Remove duplication with hpp-fcl.

parent dfaf425c
......@@ -79,9 +79,6 @@ REMOVE_PATH_FROM_LIST("${${PROJECT_NAME}_PYTHON_SOURCES}" "${CMAKE_CURRENT_SOURC
# Python exposition of FCL
IF(NOT BUILD_WITH_HPP_FCL_SUPPORT)
LIST(REMOVE_ITEM ${PROJECT_NAME}_PYTHON_HEADERS
multibody/fcl/contact.hpp
multibody/fcl/collision-result.hpp
multibody/fcl/distance-result.hpp
multibody/fcl/transform.hpp
)
......
//
// Copyright (c) 2017 CNRS
//
#ifndef __pinocchio_python_fcl_collision_result_hpp__
#define __pinocchio_python_fcl_collision_result_hpp__
#include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"
#include <hpp/fcl/collision_data.h>
#include <boost/python/copy_const_reference.hpp>
#include <boost/python/return_internal_reference.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
namespace pinocchio
{
namespace python
{
namespace fcl
{
namespace bp = boost::python;
struct CollisionResultPythonVisitor : public bp::def_visitor<CollisionResultPythonVisitor>
{
typedef ::hpp::fcl::CollisionResult CollisionResult;
template<class PyClass>
void visit(PyClass& cl) const
{
cl
.def(bp::init<>("Default constructor"))
.def("addContact",&CollisionResult::addContact,bp::arg("contact"),"Adds one contact into result structure")
.def("isCollision",&CollisionResult::isCollision,"Returns binary collision result")
.def("numContacts",&CollisionResult::numContacts,"Returns the number of contacts found")
.def("clear",&CollisionResult::clear,"Clears the results obtained")
.def("getContacts",&CollisionResult::getContacts,"Get all the contacts",bp::return_internal_reference<>())
.def("getContact",&CollisionResult::getContact,bp::arg("index"),"Get the i-th contact calculated",bp::return_value_policy<bp::copy_const_reference>())
.def_readwrite("distance_lower_bound",&CollisionResult::distance_lower_bound,"Lower bound on distance between objects if they are disjoint (computed only on request).")
;
}
static void expose()
{
bp::class_<CollisionResult>("CollisionResult",
"Contact information returned by collision.",
bp::no_init)
.def(CollisionResultPythonVisitor())
;
}
private:
};
} // namespace fcl
} // namespace python
} // namespace pinocchio
#endif // namespace __pinocchio_python_fcl_collision_result_hpp__
//
// Copyright (c) 2017 CNRS
//
#ifndef __pinocchio_python_fcl_contact_hpp__
#define __pinocchio_python_fcl_contact_hpp__
#include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"
#include <hpp/fcl/collision_data.h>
#include <boost/python.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
namespace pinocchio
{
namespace python
{
namespace fcl
{
namespace bp = boost::python;
struct ContactPythonVisitor : public bp::def_visitor<ContactPythonVisitor>
{
typedef ::hpp::fcl::Contact Contact;
typedef ::hpp::fcl::CollisionGeometry CollisionGeometry;
template<class PyClass>
void visit(PyClass& cl) const
{
cl
.def(bp::init<>("Default constructor"))
// TODO: add a kind of bp::with_custodian_and_ward_postcall<0,1>() for managing arguments lifetime
.def("__init__",bp::make_constructor(&make_contact,
bp::default_call_policies(),
bp::args("o1","o2","b1","b2")),
"Constructor from two Collision Geometry objects")
.def_readwrite("b1", &Contact::b1, "Contact primitive in object 1.\n"
"if object 1 is mesh or point cloud, it is the triangle or point id\n"
"if object 1 is geometry shape, it is NONE (-1),\n"
"if object 1 is octree, it is the id of the cell.")
.def_readwrite("b2", &Contact::b2, "Contact primitive in object 2.\n"
"if object 2 is mesh or point cloud, it is the triangle or point id\n"
"if object 2 is geometry shape, it is NONE (-1),\n"
"if object 2 is octree, it is the id of the cell.")
.add_property("normal", &getNormal, &setNormal,
"Contact normal, pointing from o1 to o2.")
.add_property("pos", &getPos, &setPos,
"Contact position, in world space.")
.def_readwrite("penetration_depth",&Contact::penetration_depth,"Penetration depth.")
.add_property("o1",bp::make_function(&getObject1,bp::return_value_policy<bp::copy_const_reference>()),"Returns a copy of collision object 1")
.add_property("o2",bp::make_function(&getObject2,bp::return_value_policy<bp::copy_const_reference>()),"Returns a copy of collision object 2.")
;
}
static void expose()
{
bp::class_<Contact>("Contact",
"Contact information returned by collision.",
bp::no_init)
.def(ContactPythonVisitor())
;
}
private:
static Contact * make_contact(const CollisionGeometry & o1, const CollisionGeometry & o2, int b1, int b2)
{ return new Contact(&o1,&o2,b1,b2); }
static Eigen::Vector3d getNormal(const Contact & self)
{ return self.normal; }
static void setNormal(Contact & self, const Eigen::Vector3d & normal)
{ self.normal = normal; }
static Eigen::Vector3d getPos(const Contact & self)
{ return self.pos; }
static void setPos(Contact & self, const Eigen::Vector3d & pos)
{ self.pos = pos; }
static const CollisionGeometry & getObject1(const Contact & self)
{ return *self.o1; }
static const CollisionGeometry & getObject2(const Contact & self)
{ return *self.o2; }
};
} // namespace fcl
} // namespace python
} // namespace pinocchio
#endif // namespace __pinocchio_python_fcl_contact_hpp__
//
// Copyright (c) 2018 CNRS
//
#ifndef __pinocchio_python_fcl_distance_result_hpp__
#define __pinocchio_python_fcl_distance_result_hpp__
#include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"
#include <hpp/fcl/collision_data.h>
#include <boost/python/copy_const_reference.hpp>
#include <boost/python/return_internal_reference.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
namespace pinocchio
{
namespace python
{
namespace fcl
{
namespace bp = boost::python;
struct DistanceResultPythonVisitor : public bp::def_visitor<DistanceResultPythonVisitor>
{
typedef ::hpp::fcl::DistanceResult DistanceResult;
template<class PyClass>
void visit(PyClass& cl) const
{
cl
.def(bp::init<>("Default constructor"))
.def_readonly("min_distance",&DistanceResult::min_distance,
"minimum distance between two objects.\n"
"If two objects are in collision, min_distance <= 0.")
.def("getNearestPoint1",getNearestPoint1,
"Returns the nearest point on collision object 1.")
.def("getNearestPoint2",getNearestPoint2,
"Returns the nearest point on collision object 2.")
;
}
static void expose()
{
bp::class_<DistanceResult>("DistanceResult",
"Contact information returned by collision.",
bp::no_init)
.def(DistanceResultPythonVisitor())
;
}
private:
static ::hpp::fcl::Vec3f getNearestPoint1(const DistanceResult & res) { return res.nearest_points[0]; }
static ::hpp::fcl::Vec3f getNearestPoint2(const DistanceResult & res) { return res.nearest_points[1]; }
};
} // namespace fcl
} // namespace python
} // namespace pinocchio
#endif // namespace __pinocchio_python_fcl_distance_result_hpp__
......@@ -3,88 +3,18 @@
//
#include "pinocchio/bindings/python/fwd.hpp"
#include "pinocchio/bindings/python/multibody/fcl/contact.hpp"
#include "pinocchio/bindings/python/multibody/fcl/collision-result.hpp"
#include "pinocchio/bindings/python/multibody/fcl/distance-result.hpp"
#include "pinocchio/bindings/python/multibody/fcl/transform.hpp"
#include "pinocchio/bindings/python/utils/std-vector.hpp"
#include <eigenpy/registration.hpp>
namespace pinocchio
{
namespace python
{
void exposeFCL()
{
using namespace pinocchio::python::fcl;
namespace bp = boost::python;
if(!eigenpy::register_symbolic_link_to_registered_type< ::hpp::fcl::OBJECT_TYPE>())
{
bp::enum_< ::hpp::fcl::OBJECT_TYPE >("OBJECT_TYPE")
.value("OT_UNKNOWN",::hpp::fcl::OT_UNKNOWN)
.value("OT_BVH",::hpp::fcl::OT_BVH)
.value("OT_GEOM",::hpp::fcl::OT_GEOM)
.value("OT_OCTREE",::hpp::fcl::OT_OCTREE)
.value("OT_COUNT",::hpp::fcl::OT_COUNT)
;
}
if(!eigenpy::register_symbolic_link_to_registered_type< ::hpp::fcl::NODE_TYPE>())
{
bp::enum_< ::hpp::fcl::NODE_TYPE >("NODE_TYPE")
.value("BV_UNKNOWN",::hpp::fcl::BV_UNKNOWN)
.value("BV_AABB",::hpp::fcl::BV_AABB)
.value("BV_OBB",::hpp::fcl::BV_OBB)
.value("BV_RSS",::hpp::fcl::BV_RSS)
.value("BV_kIOS",::hpp::fcl::BV_kIOS)
.value("BV_OBBRSS",::hpp::fcl::BV_OBBRSS)
.value("BV_KDOP16",::hpp::fcl::BV_KDOP16)
.value("BV_KDOP18",::hpp::fcl::BV_KDOP18)
.value("BV_KDOP24",::hpp::fcl::BV_KDOP24)
.value("GEOM_BOX",::hpp::fcl::GEOM_BOX)
.value("GEOM_SPHERE",::hpp::fcl::GEOM_SPHERE)
.value("GEOM_CAPSULE",::hpp::fcl::GEOM_CAPSULE)
.value("GEOM_CONE",::hpp::fcl::GEOM_CONE)
.value("GEOM_CYLINDER",::hpp::fcl::GEOM_CYLINDER)
.value("GEOM_CONVEX",::hpp::fcl::GEOM_CONVEX)
.value("GEOM_PLANE",::hpp::fcl::GEOM_PLANE)
.value("GEOM_HALFSPACE",::hpp::fcl::GEOM_HALFSPACE)
.value("GEOM_TRIANGLE",::hpp::fcl::GEOM_TRIANGLE)
.value("GEOM_OCTREE",::hpp::fcl::GEOM_OCTREE)
.value("NODE_COUNT",::hpp::fcl::NODE_COUNT)
;
}
if(!eigenpy::register_symbolic_link_to_registered_type< ::hpp::fcl::Contact>())
{
ContactPythonVisitor::expose();
}
if(!eigenpy::register_symbolic_link_to_registered_type< std::vector< ::hpp::fcl::Contact> >())
{
StdVectorPythonVisitor<ContactPythonVisitor::Contact>::expose("StdVec_Contact");
}
if(!eigenpy::register_symbolic_link_to_registered_type< ::hpp::fcl::CollisionResult>())
{
CollisionResultPythonVisitor::expose();
}
if(!eigenpy::register_symbolic_link_to_registered_type< std::vector< ::hpp::fcl::CollisionResult> >())
{
StdVectorPythonVisitor<CollisionResultPythonVisitor::CollisionResult>::expose("StdVec_CollisionResult");
}
if(!eigenpy::register_symbolic_link_to_registered_type< ::hpp::fcl::DistanceResult>())
{
DistanceResultPythonVisitor::expose();
}
if(!eigenpy::register_symbolic_link_to_registered_type< std::vector< ::hpp::fcl::DistanceResult> >())
{
StdVectorPythonVisitor<DistanceResultPythonVisitor::DistanceResult>::expose("StdVec_DistanceResult");
}
typedef ::hpp::fcl::Transform3f Transform3f;
// Register implicit conversion SE3 <=> ::hpp::fcl::Transform3f
......
Markdown is supported
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