Commit 2d927754 authored by jcarpent's avatar jcarpent
Browse files

[Python] Start to expose FCL objects

parent 4e08213c
#
# Copyright (c) 2015-2016 CNRS
# Copyright (c) 2015-2017 CNRS
# Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
#
# This file is part of Pinocchio
......@@ -83,6 +83,10 @@ SET(${PROJECT_NAME}_PYTHON_HEADERS
multibody/joint/joints-variant.hpp
multibody/joint/joint.hpp
multibody/joint/joint-derived.hpp
multibody/fcl/contact.hpp
multibody/fcl/collision-result.hpp
multibody/fcl/distance-result.hpp
multibody/fcl/collision-geometry.hpp
algorithm/algorithms.hpp
parsers/parsers.hpp
)
......@@ -99,6 +103,7 @@ SET(${PROJECT_NAME}_PYTHON_SOURCES
multibody/expose-data.cpp
multibody/expose-geometry.cpp
multibody/joint/expose-joints.cpp
multibody/fcl/expose-fcl.cpp
algorithm/expose-algorithms.cpp
algorithm/expose-com.cpp
algorithm/expose-kinematics.cpp
......@@ -131,6 +136,7 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python"
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/spatial")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/multibody")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/multibody/joint")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/multibody/fcl")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/parsers")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/algorithm")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/utils")
......
//
// Copyright (c) 2015-2016 CNRS
// Copyright (c) 2015-2017 CNRS
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
......@@ -44,6 +44,10 @@ namespace se3
// Expose algorithms
void exposeAlgorithms();
#ifdef WITH_HPP_FCL
void exposeFCL();
#endif // WITH_HPP_FCL
} // namespace python
} // namespace se3
......
......@@ -56,5 +56,9 @@ BOOST_PYTHON_MODULE(libpinocchio_pywrap)
exposeAlgorithms();
exposeParsers();
#ifdef WITH_HPP_FCL
exposeFCL();
#endif // WITH_HPP_FCL
}
//
// Copyright (c) 2017 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_python_fcl_collision_geometry_hpp__
#define __se3_python_fcl_collision_geometry_hpp__
#include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"
#include <hpp/fcl/collision_object.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 se3
{
namespace python
{
namespace fcl
{
namespace bp = boost::python;
struct CollisionGeometryPythonVisitor : public bp::def_visitor<CollisionGeometryPythonVisitor>
{
typedef ::fcl::CollisionGeometry CollisionGeometry;
template<class PyClass>
void visit(PyClass& cl) const
{
cl
.def("getObjectType",&CollisionGeometry::getObjectType,"Get the type of the object.")
.def("getNodeType",&CollisionGeometry::getNodeType,"Get the node type.")
.def("computeLocalAABB",&CollisionGeometry::computeLocalAABB)
.def("isOccupied",&CollisionGeometry::isOccupied)
.def("isFree",&CollisionGeometry::isFree)
.def("isUncertain",&CollisionGeometry::isUncertain)
.def_readwrite("aabb_radius",&CollisionGeometry::aabb_radius,"AABB radius")
;
}
static void expose()
{
bp::class_<CollisionGeometry,boost::noncopyable>("CollisionGeometry",
"The geometry for the object for collision or distance computation.",
bp::no_init)
.def(CollisionGeometryPythonVisitor())
;
bp::enum_< ::fcl::OBJECT_TYPE >("OBJECT_TYPE")
.value("OT_UNKNOWN",::fcl::OT_UNKNOWN)
.value("OT_BVH",::fcl::OT_BVH)
.value("OT_GEOM",::fcl::OT_GEOM)
.value("OT_OCTREE",::fcl::OT_OCTREE)
.value("OT_COUNT",::fcl::OT_COUNT)
;
bp::enum_< ::fcl::NODE_TYPE >("NODE_TYPE")
.value("BV_UNKNOWN",::fcl::BV_UNKNOWN)
.value("BV_AABB",::fcl::BV_AABB)
.value("BV_OBB",::fcl::BV_OBB)
.value("BV_RSS",::fcl::BV_RSS)
.value("BV_kIOS",::fcl::BV_kIOS)
.value("BV_OBBRSS",::fcl::BV_OBBRSS)
.value("BV_KDOP16",::fcl::BV_KDOP16)
.value("BV_KDOP18",::fcl::BV_KDOP18)
.value("BV_KDOP24",::fcl::BV_KDOP24)
.value("GEOM_BOX",::fcl::GEOM_BOX)
.value("GEOM_SPHERE",::fcl::GEOM_SPHERE)
.value("GEOM_CAPSULE",::fcl::GEOM_CAPSULE)
.value("GEOM_CONE",::fcl::GEOM_CONE)
.value("GEOM_CYLINDER",::fcl::GEOM_CYLINDER)
.value("GEOM_CONVEX",::fcl::GEOM_CONVEX)
.value("GEOM_PLANE",::fcl::GEOM_PLANE)
.value("GEOM_HALFSPACE",::fcl::GEOM_HALFSPACE)
.value("GEOM_TRIANGLE",::fcl::GEOM_TRIANGLE)
.value("GEOM_OCTREE",::fcl::GEOM_OCTREE)
.value("NODE_COUNT",::fcl::NODE_COUNT)
;
}
private:
};
} // namespace fcl
} // namespace python
} // namespace se3
#endif // namespace __se3_python_fcl_collision_geometry_hpp__
//
// Copyright (c) 2017 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_python_fcl_collision_result_hpp__
#define __se3_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 se3
{
namespace python
{
namespace fcl
{
namespace bp = boost::python;
struct CollisionResultPythonVisitor : public bp::def_visitor<CollisionResultPythonVisitor>
{
typedef ::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("numCostSources",&CollisionResult::numCostSources,"Returns the number of cost sources 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 se3
#endif // namespace __se3_python_fcl_collision_result_hpp__
//
// Copyright (c) 2017 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_python_fcl_contact_hpp__
#define __se3_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 se3
{
namespace python
{
namespace fcl
{
namespace bp = boost::python;
struct ContactPythonVisitor : public bp::def_visitor<ContactPythonVisitor>
{
typedef ::fcl::Contact Contact;
typedef ::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 toVector3d(self.normal); }
static void setNormal(Contact & self, const Eigen::Vector3d & normal)
{ self.normal = toFclVec3f(normal); }
static Eigen::Vector3d getPos(const Contact & self)
{ return toVector3d(self.pos); }
static void setPos(Contact & self, const Eigen::Vector3d & pos)
{ self.pos = toFclVec3f(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 se3
#endif // namespace __se3_python_fcl_contact_hpp__
//
// Copyright (c) 2017 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/>.
#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/collision-geometry.hpp"
#include "pinocchio/bindings/python/utils/std-vector.hpp"
namespace se3
{
namespace python
{
void exposeFCL()
{
using namespace se3::python::fcl;
ContactPythonVisitor::expose();
StdVectorPythonVisitor<ContactPythonVisitor::Contact>::expose("StdVect_Contact");
CollisionResultPythonVisitor::expose();
CollisionGeometryPythonVisitor::expose();
}
} // namespace python
} // namespace se3
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