diff --git a/.github/workflows/push.yml b/.github/workflows/push.yml new file mode 100644 index 0000000..7858377 --- /dev/null +++ b/.github/workflows/push.yml @@ -0,0 +1,18 @@ +name: Build + +on: [push, pull_request] + +jobs: + build_wheels: + name: Build wheel on ${{matrix.platform}} + runs-on: ${{matrix.platform}} + strategy: + matrix: + platform: [ubuntu-latest, macos-latest, windows-latest] + steps: + - uses: actions/checkout@v2 + - name: Build wheels + uses: pypa/cibuildwheel@v2.0.0 + - uses: actions/upload-artifact@v2 + with: + path: ./wheelhouse/*.whl \ No newline at end of file diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml new file mode 100644 index 0000000..c2a68c1 --- /dev/null +++ b/.github/workflows/release.yml @@ -0,0 +1,27 @@ +name: Release + +on: release + +jobs: + build_wheels: + name: Build wheel on ${{matrix.platform}} + runs-on: ${{matrix.platform}} + strategy: + matrix: + platform: [ubuntu-latest, macos-latest, windows-latest] + steps: + - uses: actions/checkout@v2 + - name: Build wheels + uses: pypa/cibuildwheel@v2.0.0 + - uses: actions/upload-artifact@v2 + with: + path: ./wheelhouse/*.whl + - uses: xresloader/upload-to-github-release@v1 + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + with: + file: ./wheelhouse/*.whl + overwrite: true + draft: false + update_latest_release: true + tag_name: v0.6.1 \ No newline at end of file diff --git a/.gitignore b/.gitignore index db59b2b..2cd1861 100644 --- a/.gitignore +++ b/.gitignore @@ -25,6 +25,7 @@ wheels/ .installed.cfg *.egg MANIFEST +deps/ # PyInstaller # Usually these files are written by a python script from a template @@ -103,3 +104,6 @@ venv.bak/ # mypy .mypy_cache/ + +# vscode settings +.vscode/ diff --git a/.travis.yml b/.travis.yml index 76785fe..5172712 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,11 +1,5 @@ env: global: - - CIBW_BEFORE_BUILD="yum install -y gcc && yum install -y glibc.i686 && pip install - numpy cython && cd requirements && bash install_cmake.bash && bash clone.bash - && bash build.bash" - - CIBW_TEST_REQUIRES="nose2" - - CIBW_TEST_COMMAND="cd {project}/test && nose2" - - CIBW_SKIP="cp33-* cp34-*" - TWINE_USERNAME=mmatl matrix: include: diff --git a/MANIFEST.in b/MANIFEST.in deleted file mode 100644 index d616172..0000000 --- a/MANIFEST.in +++ /dev/null @@ -1,5 +0,0 @@ -# Include the license -include LICENSE -include fcl/fcl_defs.pxd -include fcl/octomap_defs.pxd -include fcl/std_defs.pxd diff --git a/README.md b/README.md index 9297ebe..bfe9822 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,9 @@ # python-fcl ### Python Interface for the Flexible Collision Library -[![Build Status](https://travis-ci.org/BerkeleyAutomation/python-fcl.svg?branch=master)](https://travis-ci.org/BerkeleyAutomation/python-fcl) - Python-FCL is an (unofficial) Python interface for the [Flexible Collision Library (FCL)](https://github.com/flexible-collision-library/fcl), an excellent C++ library for performing proximity and collision queries on pairs of geometric models. -Currently, this package is targeted for FCL 0.5.0. +Currently, this package is targeted for FCL 0.6.1. This package supports three types of proximity queries for pairs of geometric models: * __Collision Detection__: Detecting whether two models overlap (and optionally where). diff --git a/requirements/Dockerfile b/build_dependencies/Dockerfile similarity index 59% rename from requirements/Dockerfile rename to build_dependencies/Dockerfile index eaccf63..da9d08f 100644 --- a/requirements/Dockerfile +++ b/build_dependencies/Dockerfile @@ -6,28 +6,16 @@ # ln -sf requirements/Dockerfile . # docker build . -t pythonfcl -FROM quay.io/pypa/manylinux1_x86_64:latest - - -RUN yum install -y gcc - -# install cmake 2.8.12 -COPY requirements/install_cmake.bash . -RUN bash install_cmake.bash - -# clone FCL and libccd -# the exact checkouts are in clone.bash -COPY requirements/clone.bash . -RUN bash clone.bash +FROM quay.io/pypa/manylinux2010_x86_64:latest # build and install libccd and fcl using cmake -COPY requirements/build.bash . -RUN bash build.bash +COPY build_dependencies/install_linux.sh . +RUN bash install_linux.sh # manylinux includes a bunch of pythons # to test with others change this env variable #ENV PATH=/opt/python/cp27-cp27m/bin:$PATH -ENV PATH=/opt/python/cp36-cp36m/bin:$PATH +ENV PATH=/opt/python/cp39-cp39/bin:$PATH # we need numpy to build python-fcl # since we set our path we'll be using the right pip @@ -35,4 +23,7 @@ RUN pip install numpy cython # build the python-fcl module COPY . /python_fcl -RUN cd /python_fcl && python setup.py build_ext +RUN pip wheel /python_fcl --no-deps -w wheelhouse/ +RUN pip install /python_fcl --no-index -f /wheelhouse +RUN ls /wheelhouse +RUN auditwheel repair wheelhouse/python_fcl-0.6.1-cp39-cp39-linux_x86_64.whl --plat manylinux2010_x86_64 -w /wheelhouse diff --git a/build_dependencies/install_linux.sh b/build_dependencies/install_linux.sh new file mode 100644 index 0000000..022f08c --- /dev/null +++ b/build_dependencies/install_linux.sh @@ -0,0 +1,41 @@ +mkdir -p deps +cd deps +get eigen +curl -OL https://gitlab.com/libeigen/eigen/-/archive/3.3.9/eigen-3.3.9.tar.gz +tar -zxf eigen-3.3.9.tar.gz + +rm -rf libccd +git clone --depth 1 --branch v2.1 https://github.com/danfis/libccd.git + +rm -rf octomap +git clone --depth 1 --branch v1.8.0 https://github.com/OctoMap/octomap.git + +rm -rf fcl +git clone --depth 1 --branch v0.6.1 https://github.com/flexible-collision-library/fcl.git + +echo "Install eigen" +cmake -B build -S eigen-3.3.9 +cmake --install build + +echo "Build and install libccd" +cd libccd +cmake . +make -j4 +make install +cd .. + +echo "Build and install octomap" +cd octomap +cmake . +make -j4 +make install +cd .. + +echo "Build and install fcl" +cd fcl +cmake . +make -j4 +make install +cd .. + +cd .. \ No newline at end of file diff --git a/build_dependencies/install_macos.sh b/build_dependencies/install_macos.sh new file mode 100644 index 0000000..9d89609 --- /dev/null +++ b/build_dependencies/install_macos.sh @@ -0,0 +1,23 @@ +brew update > /dev/null + +# brew install git +# brew install cmake +# brew install eigen +# brew install libccd +brew install fcl + +# mkdir -p deps +# cd deps +# # Octomap +# git clone https://github.com/OctoMap/octomap +# cd octomap +# git checkout tags/v1.8.0 +# mkdir build +# cd build +# cmake .. +# make +# sudo make install + +# cd .. +# cd .. +# cd .. \ No newline at end of file diff --git a/build_dependencies/install_windows.ps1 b/build_dependencies/install_windows.ps1 new file mode 100644 index 0000000..d2822cd --- /dev/null +++ b/build_dependencies/install_windows.ps1 @@ -0,0 +1,104 @@ +<# +Originally based on script written by Pebaz (https://github.com/Pebaz/python-fcl/blob/master/requirements/build_win32.ps1) +but with many modification in order to use fcl 0.6.1 and install dependencies without admin rights. + +This script builds fcl and it's dependencies for python-fcl on Windows. + +It downloads, builds, installs: + * fcl + * libccd + * eigen + * octomap +#> + +# Remember starting location for future usage +$base_dir = Get-Location + +# Create a directory that encapsulates all dependencies +mkdir -p deps; Set-Location deps + +# Build options +$generator = "Visual Studio 16 2019" + +# All compiled depencies will be install in following folder +$install_dir = "$base_dir\deps\install" + + +#------------------------------------------------------------------------------ +# Eigen +Write-Host "Building Eigen" +$eigen_ver = "3.3.9" +Invoke-WebRequest -Uri https://gitlab.com/libeigen/eigen/-/archive/$eigen_ver/eigen-$eigen_ver.tar.gz -Outfile eigen-$eigen_ver.tar.gz +tar -zxf "eigen-$eigen_ver.tar.gz" +Set-Location "eigen-$eigen_ver" + +cmake -B build ` + -D CMAKE_BUILD_TYPE=Release ` + -G $generator ` + -D BUILD_SHARED_LIBS=ON ` + -D CMAKE_INSTALL_PREFIX=$install_dir +cmake --install build + +Set-Location .. + + +# ------------------------------------------------------------------------------ +# LibCCD +Write-Host "Building LibCCD" +git clone --depth 1 --branch v2.1 https://github.com/danfis/libccd +Set-Location libccd + +cmake -B build ` + -D CMAKE_BUILD_TYPE=Release ` + -G $generator ` + -D BUILD_SHARED_LIBS=ON ` + -D CMAKE_INSTALL_PREFIX=$install_dir +cmake --build build --config Release --target install + +Set-Location .. + + +# ------------------------------------------------------------------------------ +# Octomap +Write-Host "Building Octomap" +git clone --depth 1 --branch v1.8.0 https://github.com/OctoMap/octomap +Set-Location octomap + +cmake -B build ` + -D CMAKE_PREFIX_PATH=$install_dir ` + -D CMAKE_BUILD_TYPE=Release ` + -G $generator ` + -D BUILD_SHARED_LIBS=ON ` + -D CMAKE_INSTALL_PREFIX=$install_dir ` + -D BUILD_OCTOVIS_SUBPROJECT=OFF ` + -D BUILD_DYNAMICETD3D_SUBPROJECT=OFF +cmake --build build --config Release +cmake --build build --config Release --target install + +Set-Location .. + +# ------------------------------------------------------------------------------ +# FCL +Write-Host "Building FCL" +git clone --depth 1 --branch v0.6.1 https://github.com/flexible-collision-library/fcl +Set-Location fcl + +cmake -B build ` + -D CMAKE_PREFIX_PATH=$install_dir ` + -D CMAKE_BUILD_TYPE=Release ` + -G $generator ` + -D CMAKE_INSTALL_PREFIX=$install_dir + +cmake --build build --config Release --target install +Set-Location .. + +# ------------------------------------------------------------------------------ +# Python-FCL + +Write-Host "Copying dependent DLLs" +Copy-Item $install_dir\bin\octomap.dll $base_dir\src\fcl +Copy-Item $install_dir\bin\octomath.dll $base_dir\src\fcl +Copy-Item $install_dir\bin\ccd.dll $base_dir\src\fcl + +Set-Location $base_dir +Write-Host "All done!" diff --git a/build_dependencies/install_windows_ci.ps1 b/build_dependencies/install_windows_ci.ps1 new file mode 100644 index 0000000..bec5ebb --- /dev/null +++ b/build_dependencies/install_windows_ci.ps1 @@ -0,0 +1,29 @@ +<# +This script install precompiled dependencies to build python-fcl on Windows: + * fcl + * libccd + * eigen + * octomap +#> + +# Remember starting location for future usage +$base_dir = Get-Location +# Binaries folder +$install_dir = "$base_dir\deps\install" + + +$file_name = "PrecompiledDependenciesWindows.zip" +Invoke-WebRequest -Uri "https://github.com/CyrilWaechter/python-fcl/releases/download/v0.6.1/$file_name" -Outfile $file_name +Expand-Archive $file_name -DestinationPath deps + + +# ------------------------------------------------------------------------------ +# Python-FCL + +Write-Host "Copying dependent DLLs" +Copy-Item $install_dir\bin\octomap.dll $base_dir\src\fcl +Copy-Item $install_dir\bin\octomath.dll $base_dir\src\fcl +Copy-Item $install_dir\bin\ccd.dll $base_dir\src\fcl + +Set-Location $base_dir +Write-Host "All done!" diff --git a/fcl/__init__.py b/fcl/__init__.py deleted file mode 100644 index e852be0..0000000 --- a/fcl/__init__.py +++ /dev/null @@ -1,5 +0,0 @@ -from .fcl import CollisionObject, CollisionGeometry, Transform, TriangleP, Box, Sphere, Ellipsoid, Capsule, Cone, Cylinder, Halfspace, Plane, BVHModel, OcTree, DynamicAABBTreeCollisionManager, collide, continuousCollide, distance, defaultCollisionCallback, defaultDistanceCallback - -from .collision_data import OBJECT_TYPE, NODE_TYPE, CCDMotionType, CCDSolverType, GJKSolverType, Contact, CostSource, CollisionRequest, CollisionResult, ContinuousCollisionRequest, ContinuousCollisionResult, DistanceRequest, DistanceResult, CollisionData, DistanceData - -from .version import __version__ diff --git a/fcl/fcl_defs.pxd b/fcl/fcl_defs.pxd deleted file mode 100644 index d5461fd..0000000 --- a/fcl/fcl_defs.pxd +++ /dev/null @@ -1,409 +0,0 @@ -from libcpp cimport bool -from libcpp.string cimport string -from libcpp.vector cimport vector -from libcpp.set cimport set -from libcpp.memory cimport shared_ptr -cimport octomap_defs as octomap - -cdef extern from "Python.h": - ctypedef struct PyObject - void Py_INCREF(PyObject *obj) - void Py_DECREF(PyObject *obj) - object PyObject_CallObject(object obj, object args) - object PySequence_Concat(object obj1, object obj2) - -#cdef extern from "boost/shared_ptr.hpp" namespace "boost": -# cppclass shared_ptr[T]: -# shared_ptr() except + -# shared_ptr(T*) except + -# T* get() - -cdef extern from "fcl/data_types.h" namespace "fcl": - ctypedef double FCL_REAL - -cdef extern from "fcl/math/vec_3f.h" namespace "fcl": - cdef cppclass Vec3f: - Vec3f() except + - Vec3f(FCL_REAL x, FCL_REAL y, FCL_REAL z) except + - FCL_REAL& operator[](size_t i) - -cdef extern from "fcl/math/matrix_3f.h" namespace "fcl": - cdef cppclass Matrix3f: - Matrix3f() except + - Matrix3f(FCL_REAL xx, FCL_REAL xy, FCL_REAL xz, - FCL_REAL yx, FCL_REAL yy, FCL_REAL yz, - FCL_REAL zx, FCL_REAL zy, FCL_REAL zz) except + - FCL_REAL operator()(size_t i, size_t j) - -cdef extern from "fcl/math/transform.h" namespace "fcl": - cdef cppclass Quaternion3f: - Quaternion3f() except + - Quaternion3f(FCL_REAL a, FCL_REAL b, - FCL_REAL c, FCL_REAL d) except + - void fromRotation(Matrix3f& R) - void fromAxisAngle(Vec3f& axis, FCL_REAL angle) - FCL_REAL& getW() - FCL_REAL& getX() - FCL_REAL& getY() - FCL_REAL& getZ() - - cdef cppclass Transform3f: - Transform3f() except + - Transform3f(Matrix3f& R_, Vec3f& T_) - Transform3f(Quaternion3f& q_, Vec3f& T_) - Transform3f(Matrix3f& R_) - Transform3f(Quaternion3f& q_) - Transform3f(Vec3f& T_) - Transform3f(Transform3f& tf_) - Matrix3f& getRotation() - Vec3f& getTranslation() - Quaternion3f& getQuatRotation() - void setRotation(Matrix3f& R_) - void setTranslation(Vec3f& T_) - void setQuatRotation(Quaternion3f & q_) - -cdef extern from "fcl/collision_data.h" namespace "fcl": - - cdef enum CCDMotionType: - CCDM_TRANS, CCDM_LINEAR, CCDM_SCREW, CCDM_SPLINE - - cdef enum CCDSolverType: - CCDC_NAIVE, CCDC_CONSERVATIVE_ADVANCEMENT, CCDC_RAY_SHOOTING, CCDC_POLYNOMIAL_SOLVER - - cdef enum GJKSolverType: - GST_LIBCCD, GST_INDEP - - cdef cppclass Contact: - CollisionGeometry *o1 - CollisionGeometry *o2 - int b1 - int b2 - Vec3f normal - Vec3f pos - FCL_REAL penetration_depth - Contact() except + - Contact(CollisionGeometry* o1_, - CollisionGeometry* o2_, - int b1_, int b2_) except + - - cdef cppclass CostSource: - Vec3f aabb_min - Vec3f aabb_max - FCL_REAL cost_density - FCL_REAL total_cost - - cdef cppclass CollisionResult: - CollisionResult() except + - bool isCollision() - void getContacts(vector[Contact]& contacts_) - void getCostSources(vector[CostSource]& cost_sources_) - - cdef cppclass ContinuousCollisionResult: - ContinuousCollisionResult() except + - bool is_collide - FCL_REAL time_of_contact - Transform3f contact_tf1, contact_tf2 - - cdef cppclass CollisionRequest: - size_t num_max_contacts - bool enable_contact - size_t num_max_cost_sources - bool enable_cost - bool use_approximate_cost - GJKSolverType gjk_solver_type - CollisionRequest(size_t num_max_contacts_, - bool enable_contact_, - size_t num_max_cost_sources_, - bool enable_cost_, - bool use_approximate_cost_, - GJKSolverType gjk_solver_type_) - - cdef cppclass ContinuousCollisionRequest: - size_t num_max_iterations_, - FCL_REAL toc_err_, - CCDMotionType ccd_motion_type_, - GJKSolverType gjk_solver_type_, - GJKSolverType ccd_solver_type_ - - ContinuousCollisionRequest( - size_t num_max_iterations_, - FCL_REAL toc_err_, - CCDMotionType ccd_motion_type_, - GJKSolverType gjk_solver_type_, - CCDSolverType ccd_solver_type_ ) - - cdef cppclass DistanceResult: - FCL_REAL min_distance - Vec3f* nearest_points - CollisionGeometry* o1 - CollisionGeometry* o2 - int b1 - int b2 - DistanceResult(FCL_REAL min_distance_) except + - DistanceResult() except + - - cdef cppclass DistanceRequest: - bool enable_nearest_points - GJKSolverType gjk_solver_type - DistanceRequest(bool enable_nearest_points_, GJKSolverType gjk_solver_type_) except + - -cdef extern from "fcl/collision_object.h" namespace "fcl": - cdef enum OBJECT_TYPE: - OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE, OT_COUNT - cdef enum NODE_TYPE: - BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_kIOS, BV_OBBRSS, BV_KDOP16, BV_KDOP18, BV_KDOP24, - GEOM_BOX, GEOM_SPHERE, GEOM_ELLIPSOID, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, - GEOM_PLANE, GEOM_HALFSPACE, GEOM_TRIANGLE, GEOM_OCTREE, NODE_COUNT - - cdef cppclass CollisionGeometry: - CollisionGeometry() except + - OBJECT_TYPE getObjectType() - NODE_TYPE getNodeType() - void computeLocalAABB() - Vec3f aabb_center - FCL_REAL aabb_radius - FCL_REAL cost_density - FCL_REAL threshold_occupied - FCL_REAL threshold_free - - cdef cppclass CollisionObject: - CollisionObject() except + - CollisionObject(shared_ptr[CollisionGeometry]& cgeom_) except + - CollisionObject(shared_ptr[CollisionGeometry]& cgeom_, Transform3f& tf) except + - OBJECT_TYPE getObjectType() - NODE_TYPE getNodeType() - Vec3f& getTranslation() - Matrix3f& getRotation() - Quaternion3f& getQuatRotation() - Transform3f& getTransform() - CollisionGeometry* getCollisionGeometry() - void setTranslation(Vec3f& T) - void setRotation(Matrix3f& M) - void setQuatRotation(Quaternion3f& q) - void setTransform(Quaternion3f& q, Vec3f& T) - void setTransform(Matrix3f& q, Vec3f& T) - void setTransform(Transform3f& tf) - void setUserData(void *data) - void computeAABB() - void *getUserData() - bool isOccupied() - bool isFree() - bool isUncertain() - - ctypedef CollisionGeometry const_CollisionGeometry "const fcl::CollisionGeometry" - ctypedef CollisionObject const_CollisionObject "const fcl::CollisionObject" - -cdef extern from "fcl/shape/geometric_shapes.h" namespace "fcl": - cdef cppclass ShapeBase(CollisionGeometry): - ShapeBase() except + - - cdef cppclass TriangleP(ShapeBase): - TriangleP(Vec3f& a_, Vec3f& b_, Vec3f& c_) except + - Vec3f a, b, c - - cdef cppclass Box(ShapeBase): - Box(FCL_REAL x, FCL_REAL y, FCL_REAL z) except + - Vec3f side - - cdef cppclass Sphere(ShapeBase): - Sphere(FCL_REAL radius_) except + - FCL_REAL radius - - cdef cppclass Ellipsoid(ShapeBase): - Ellipsoid(FCL_REAL a_, FCL_REAL b_, FCL_REAL c_) except + - Vec3f radii - - cdef cppclass Capsule(ShapeBase): - Capsule(FCL_REAL radius_, FCL_REAL lz_) except + - FCL_REAL radius - FCL_REAL lz - - cdef cppclass Cone(ShapeBase): - Cone(FCL_REAL radius_, FCL_REAL lz_) except + - FCL_REAL radius - FCL_REAL lz - - cdef cppclass Cylinder(ShapeBase): - Cylinder(FCL_REAL radius_, FCL_REAL lz_) except + - FCL_REAL radius - FCL_REAL lz - - cdef cppclass Convex(ShapeBase): - Convex(Vec3f* plane_nomals_, - FCL_REAL* plane_dis_, - int num_planes, - Vec3f* points_, - int num_points_, - int* polygons_) except + - - cdef cppclass Halfspace(ShapeBase): - Halfspace(Vec3f& n_, FCL_REAL d_) except + - Vec3f n - FCL_REAL d - - cdef cppclass Plane(ShapeBase): - Plane(Vec3f& n_, FCL_REAL d_) except + - Vec3f n - FCL_REAL d - -cdef extern from "fcl/broadphase/broadphase.h" namespace "fcl": - ctypedef bool (*CollisionCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata) - ctypedef bool (*DistanceCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& dist) - -cdef extern from "fcl/broadphase/broadphase_dynamic_AABB_tree.h" namespace "fcl": - cdef cppclass DynamicAABBTreeCollisionManager: - DynamicAABBTreeCollisionManager() except + - void registerObjects(vector[CollisionObject*]& other_objs) - void registerObject(CollisionObject* obj) - void unregisterObject(CollisionObject* obj) - void collide(DynamicAABBTreeCollisionManager* mgr, void* cdata, CollisionCallBack callback) - void distance(DynamicAABBTreeCollisionManager* mgr, void* cdata, DistanceCallBack callback) - void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) - void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) - void collide(void* cdata, CollisionCallBack callback) - void distance(void* cdata, DistanceCallBack callback) - void setup() - void update() - void update(CollisionObject* updated_obj) - void update(vector[CollisionObject*] updated_objs) - void clear() - bool empty() - size_t size() - int max_tree_nonbalanced_level - int tree_incremental_balance_pass - int& tree_topdown_balance_threshold - int& tree_topdown_level - int tree_init_level - bool octree_as_geometry_collide - bool octree_as_geometry_distance - -cdef extern from "fcl/collision.h" namespace "fcl": - size_t collide(CollisionObject* o1, CollisionObject* o2, - CollisionRequest& request, - CollisionResult& result) - - size_t collide(CollisionGeometry* o1, Transform3f& tf1, - CollisionGeometry* o2, Transform3f& tf2, - CollisionRequest& request, - CollisionResult& result) - -cdef extern from "fcl/continuous_collision.h" namespace "fcl": - FCL_REAL continuousCollide(CollisionGeometry* o1, Transform3f& tf1_beg, Transform3f& tf1_end, - CollisionGeometry* o2, Transform3f& tf2_beg, Transform3f& tf2_end, - ContinuousCollisionRequest& request, - ContinuousCollisionResult& result) - - FCL_REAL continuousCollide(CollisionObject* o1, Transform3f& tf1_end, - CollisionObject* o2, Transform3f& tf2_end, - ContinuousCollisionRequest& request, - ContinuousCollisionResult& result) - - -cdef extern from "fcl/distance.h" namespace "fcl": - FCL_REAL distance(CollisionObject* o1, CollisionObject* o2, - DistanceRequest& request, DistanceResult& result) - FCL_REAL distance(CollisionGeometry* o1, Transform3f& tf1, - CollisionGeometry* o2, Transform3f& tf2, - DistanceRequest& request, DistanceResult& result) - -cdef extern from "fcl/BVH/BVH_internal.h" namespace "fcl": - cdef enum BVHModelType: - BVH_MODEL_UNKNOWN, # unknown model type - BVH_MODEL_TRIANGLES, # triangle model - BVH_MODEL_POINTCLOUD # point cloud model - -cdef extern from "fcl/BVH/BVH_internal.h" namespace "fcl": - cdef enum BVHReturnCode: - BVH_OK = 0, # BVH is valid - BVH_ERR_MODEL_OUT_OF_MEMORY = -1, # Cannot allocate memory for vertices and triangles - BVH_ERR_BUILD_OUT_OF_SEQUENCE = -2, # BVH construction does not follow correct sequence - BVH_ERR_BUILD_EMPTY_MODEL = -3, # BVH geometry is not prepared - BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME = -4, # BVH geometry in previous frame is not prepared - BVH_ERR_UNSUPPORTED_FUNCTION = -5, # BVH funtion is not supported - BVH_ERR_UNUPDATED_MODEL = -6, # BVH model update failed - BVH_ERR_INCORRECT_DATA = -7, # BVH data is not valid - BVH_ERR_UNKNOWN = -8 # Unknown failure - - -cdef extern from "fcl/BVH/BVH_internal.h" namespace "fcl": - cdef enum BVHBuildState: - BVH_BUILD_STATE_EMPTY, # empty state, immediately after constructor - BVH_BUILD_STATE_BEGUN, # after beginModel(), state for adding geometry primitives - BVH_BUILD_STATE_PROCESSED, # after tree has been build, ready for cd use - BVH_BUILD_STATE_UPDATE_BEGUN, # after beginUpdateModel(), state for updating geometry primitives - BVH_BUILD_STATE_UPDATED, # after tree has been build for updated geometry, ready for ccd use - BVH_BUILD_STATE_REPLACE_BEGUN, # after beginReplaceModel(), state for replacing geometry primitives - -cdef extern from "fcl/data_types.h" namespace "fcl": - cdef cppclass Triangle: - Triangle() except + - Triangle(size_t p1, size_t p2, size_t p3) except + - size_t vids[3] - -cdef extern from "fcl/BVH/BV_splitter.h" namespace "fcl": - cdef cppclass BVSplitterBase: - pass - -cdef extern from "fcl/BVH/BV_fitter.h" namespace "fcl": - cdef cppclass BVFitterBase: - pass - -cdef extern from "fcl/BVH/BVH_model.h" namespace "fcl": - # Cython only accepts type template parameters. - # see https://groups.google.com/forum/#!topic/cython-users/xAZxdCFw6Xs - cdef cppclass BVHModel "fcl::BVHModel" ( CollisionGeometry ): - # Constructing an empty BVH - BVHModel() except + - BVHModel(BVHModel& other) except + - # - #Geometry point data - Vec3f* vertices - # - #Geometry triangle index data, will be NULL for point clouds - Triangle* tri_indices - # - #Geometry point data in previous frame - Vec3f* prev_vertices - # - #Number of triangles - int num_tris - # - #Number of points - int num_vertices - # - #The state of BVH building process - BVHBuildState build_state - # - # # #Split rule to split one BV node into two children - # - # boost::shared_ptr > bv_splitter - shared_ptr[BVSplitterBase] bv_splitter - # boost::shared_ptr > bv_fitter - shared_ptr[BVFitterBase] bv_fitter - - int beginModel(int num_tris_, int num_vertices_) - - int addVertex(const Vec3f& p) - - int addTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3) - - #int addSubModel(const std::vector& ps) - # void getCostSources(vector[CostSource]& cost_sources_) - - #int addSubModel(const vector[Vec3f]& ps) - # - int addSubModel(const vector[Vec3f]& ps, const vector[Triangle]& ts) - - int endModel() - - int buildTree() - - # void computeLocalAABB() - - -cdef extern from "fcl/octree.h" namespace "fcl": - cdef cppclass OcTree(CollisionGeometry): - # Constructing - OcTree(FCL_REAL resolution) except + - OcTree(shared_ptr[octomap.OcTree]& tree_) except + - diff --git a/fcl/version.py b/fcl/version.py deleted file mode 100644 index b459ff2..0000000 --- a/fcl/version.py +++ /dev/null @@ -1 +0,0 @@ -__version__ = '0.0.12' diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..ef88f68 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,19 @@ +[build-system] +requires = ["setuptools", "wheel", "oldest-supported-numpy", "Cython"] +build-backend = "setuptools.build_meta" + +[tool.cibuildwheel] +skip = "pp*" +test-requires = "pytest" +test-command = "pytest {package}/test" + +[tool.cibuildwheel.linux] +before-all = "bash build_dependencies/install_linux.sh" +archs = ["x86_64"] + +[tool.cibuildwheel.macos] +before-all = "bash build_dependencies/install_macos.sh" + +[tool.cibuildwheel.windows] +before-all = "powershell build_dependencies\\install_windows_ci.ps1" +archs = ["AMD64"] \ No newline at end of file diff --git a/requirements/build.bash b/requirements/build.bash deleted file mode 100644 index 16b7091..0000000 --- a/requirements/build.bash +++ /dev/null @@ -1,17 +0,0 @@ -cd libccd -cmake . -make -j4 -make install -cd .. - -cd octomap -cmake . -make -j4 -make install -cd .. - -cd fcl -cmake . -make -j4 -make install -cd .. diff --git a/requirements/clone.bash b/requirements/clone.bash deleted file mode 100644 index fc7d56a..0000000 --- a/requirements/clone.bash +++ /dev/null @@ -1,24 +0,0 @@ -rm -rf libccd -git clone https://github.com/danfis/libccd.git -cd libccd -git pull -git checkout 64f02f741ac94fccd0fb660a5bffcbe6d01d9939 -cd .. - -rm -rf octomap -git clone https://github.com/OctoMap/octomap.git -cd octomap -git pull -git checkout b8c1d62a7a64ce0a5df278503f31d73acafa97e4 -cd .. - -rm -rf fcl -git clone https://github.com/flexible-collision-library/fcl.git -cd fcl -git pull -git checkout 22f375f333beccc10c527974cef96784f0841649 -cd .. - -# get eigen -#curl -OL https://github.com/RLovelett/eigen/archive/3.3.4.tar.gz -#tar -zxvf 3.3.4.tar.gz diff --git a/requirements/install_cmake.bash b/requirements/install_cmake.bash deleted file mode 100644 index 06072cd..0000000 --- a/requirements/install_cmake.bash +++ /dev/null @@ -1,5 +0,0 @@ -# install cmake using their shell script -# we do this because fcl needs cmake >= 2.8.12, but centos 5 -# from the manylinux image has only cmake 2.8.11 -curl -OL https://cmake.org/files/v2.8/cmake-2.8.12.2-Linux-i386.sh -bash cmake-2.8.12.2-Linux-i386.sh --skip-license --prefix=/usr diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..4b431f7 --- /dev/null +++ b/setup.cfg @@ -0,0 +1,44 @@ +[metadata] +name = python-fcl +version = attr: fcl.__version__ +description = Python bindings for the Flexible Collision Library +long_description = file: README.md +long_description_content_type = text/markdown +url = https://github.com/CyrilWaechter/python-fcl +author = Jelle Feringa, Matthew Matl, Shirokuma, Michael Dawson-Haggerty, See contributor list +author_email = jelleferinga@gmail.com, mmatl@eecs.berkeley.edu, rootstock_acg@yahoo.co.jp +maintainer = Matthew Matl +maintainer_email = mmatl@eecs.berkeley.edu +license = BSD +license_files = LICENSE +classifiers = + Development Status :: 3 - Alpha + License :: OSI Approved :: BSD License + Operating System :: POSIX :: Linux + Operating System :: MacOS + Operating System :: Microsoft :: Windows + Programming Language :: Python :: 3 + Programming Language :: Python :: 3.6 + Programming Language :: Python :: 3.7 + Programming Language :: Python :: 3.8 + Programming Language :: Python :: 3.9 + Programming Language :: Python :: Implementation :: CPython +keywords = fcl collision distance + +[options] +include_package_data = True +package_dir = + = src +packages = find: +setup_requires = + numpy + Cython +install_requires = + numpy + Cython + +[options.package_data] +* = *.pyx, *.pxd, *.dll + +[options.packages.find] +where = src \ No newline at end of file diff --git a/setup.py b/setup.py index 4dedd51..a7fb666 100644 --- a/setup.py +++ b/setup.py @@ -1,86 +1,78 @@ import os import sys -import inspect from setuptools import Extension, setup +from Cython.Build import cythonize +INSTALL_PREFIX_WIN = "deps\\install" + + +def is_nix_platform(platform): + for prefix in ["darwin", "linux", "bsd"]: + if prefix in sys.platform: + return True + return False + + +def get_include_dirs(): + if is_nix_platform(sys.platform): + include_dirs = [ + "/usr/include", + "/usr/local/include", + "/usr/include/eigen3", + "/usr/local/include/eigen3", + ] + + if "CPATH" in os.environ: + include_dirs += os.environ["CPATH"].split(":") + + elif sys.platform == "win32": + include_dirs = [ + f"{INSTALL_PREFIX_WIN}\\include", + f"{INSTALL_PREFIX_WIN}\\include\\eigen3", + ] + else: + raise NotImplementedError(sys.platform) + + # get the numpy include path from numpy + import numpy + + include_dirs.append(numpy.get_include()) + return include_dirs + + +def get_libraries_dir(): + if is_nix_platform(sys.platform): + lib_dirs = ["/usr/lib", "/usr/local/lib"] + + if "LD_LIBRARY_PATH" in os.environ: + lib_dirs += os.environ["LD_LIBRARY_PATH"].split(":") + return lib_dirs + if sys.platform == "win32": + return [f"{INSTALL_PREFIX_WIN}\\lib"] -# get current directory of file in case someone -# called setup.py from elsewhere -cwd = os.path.dirname(os.path.abspath( - inspect.getfile(inspect.currentframe()))) - -# load __version__ -exec(open(os.path.join(cwd, - 'fcl/version.py'), 'r').read()) - -platform_supported = False -for prefix in ['darwin', 'linux', 'bsd']: - if prefix in sys.platform: - platform_supported = True - include_dirs = ['/usr/include', - '/usr/local/include', - '/usr/include/eigen3'] - lib_dirs = ['/usr/lib', - '/usr/local/lib'] - - if 'CPATH' in os.environ: - include_dirs += os.environ['CPATH'].split(':') - if 'LD_LIBRARY_PATH' in os.environ: - lib_dirs += os.environ['LD_LIBRARY_PATH'].split(':') - - try: - # get the numpy include path from numpy - import numpy - include_dirs.append(numpy.get_include()) - except: - pass - - break - -if sys.platform == "win32": - platform_supported = False - -if not platform_supported: raise NotImplementedError(sys.platform) + +def get_libraries(): + libraries = ["fcl", "octomap"] + if sys.platform == "win32": + libraries.extend(["octomath", "ccd", "vcruntime"]) + return libraries + + setup( - name='python-fcl', - version=__version__, - description='Python bindings for the Flexible Collision Library', - long_description='Python bindings for the Flexible Collision Library', - url='https://github.com/BerkeleyAutomation/python-fcl', - author='Matthew Matl', - author_email='mmatl@eecs.berkeley.edu', - license = "BSD", - classifiers=[ - 'Development Status :: 3 - Alpha', - 'License :: OSI Approved :: BSD License', - 'Operating System :: POSIX :: Linux', - 'Programming Language :: Python :: 2', - 'Programming Language :: Python :: 2.7', - 'Programming Language :: Python :: 3', - 'Programming Language :: Python :: 3.0', - 'Programming Language :: Python :: 3.1', - 'Programming Language :: Python :: 3.2', - 'Programming Language :: Python :: 3.3', - 'Programming Language :: Python :: 3.4', - 'Programming Language :: Python :: 3.5', - 'Programming Language :: Python :: 3.6', - ], - keywords='fcl collision distance', - packages=['fcl'], - setup_requires=['cython'], - install_requires=['numpy', 'cython'], - ext_modules=[Extension( - "fcl.fcl", - ["fcl/fcl.pyx"], - include_dirs = include_dirs, - library_dirs = lib_dirs, - libraries=[ - "fcl","octomap" - ], - language="c++", - extra_compile_args = ["-std=c++11"] - )] + ext_modules=cythonize( + [ + Extension( + "fcl.fcl", + ["src/fcl/fcl.pyx"], + include_dirs=get_include_dirs(), + library_dirs=get_libraries_dir(), + libraries=get_libraries(), + language="c++", + extra_compile_args=["-std=c++11"], + ) + ], + ) ) diff --git a/src/fcl/__init__.py b/src/fcl/__init__.py new file mode 100644 index 0000000..5a6e4af --- /dev/null +++ b/src/fcl/__init__.py @@ -0,0 +1,48 @@ +try: + from .fcl import ( + CollisionObject, + CollisionGeometry, + Transform, + TriangleP, + Box, + Sphere, + Ellipsoid, + Capsule, + Cone, + Cylinder, + Halfspace, + Plane, + BVHModel, + OcTree, + DynamicAABBTreeCollisionManager, + collide, + continuousCollide, + distance, + defaultCollisionCallback, + defaultDistanceCallback, + ) +except ModuleNotFoundError: + import traceback + + traceback.print_exc() + print("Failed to import fcl.fcl. It is probably not correctly compiled.") + +from .collision_data import ( + OBJECT_TYPE, + NODE_TYPE, + CCDMotionType, + CCDSolverType, + GJKSolverType, + Contact, + CostSource, + CollisionRequest, + CollisionResult, + ContinuousCollisionRequest, + ContinuousCollisionResult, + DistanceRequest, + DistanceResult, + CollisionData, + DistanceData, +) + +from .version import __version__ diff --git a/fcl/collision_data.py b/src/fcl/collision_data.py similarity index 100% rename from fcl/collision_data.py rename to src/fcl/collision_data.py diff --git a/fcl/fcl.pyx b/src/fcl/fcl.pyx similarity index 67% rename from fcl/fcl.pyx rename to src/fcl/fcl.pyx index ab28ca8..ff8209e 100644 --- a/fcl/fcl.pyx +++ b/src/fcl/fcl.pyx @@ -16,26 +16,51 @@ cimport octomap_defs as octomap cimport std_defs as std from collision_data import Contact, CostSource, CollisionRequest, ContinuousCollisionRequest, CollisionResult, ContinuousCollisionResult, DistanceRequest, DistanceResult +""" +Eigen::Transform linear and translation parts are returned as Eigen::Block +It can be an rvalue and an lvalue, so in C++ you could assign something to translation() like: + `tf.translation() = (Vector3d (0., 0., 50));` +In python and cython however, a function call is never an lvalue, so we workaround with the following macro +""" +cdef extern from *: + """ + /* Verbatim C as a workaround for assingment to lvalue-returning functions*/ + #define ASSIGN(a, b) a = b + """ + void ASSIGN(defs.Vector3d&, defs.Vector3d) + void ASSIGN(defs.Matrix3d&, defs.Matrix3d) + #void ASSIGN[T](T&, T) # This doesn't work somehow + ############################################################################### # Transforms ############################################################################### cdef class Transform: - cdef defs.Transform3f *thisptr + cdef defs.Transform3d *thisptr def __cinit__(self, *args): if len(args) == 0: - self.thisptr = new defs.Transform3f() + self.thisptr = new defs.Transform3d() + self.thisptr.setIdentity() elif len(args) == 1: if isinstance(args[0], Transform): - self.thisptr = new defs.Transform3f(deref(( args[0]).thisptr)) + self.thisptr = new defs.Transform3d(deref(( args[0]).thisptr)) else: data = numpy.array(args[0]) if data.shape == (3,3): - self.thisptr = new defs.Transform3f(numpy_to_mat3f(data)) + self.thisptr = new defs.Transform3d() + self.thisptr.setIdentity() + ASSIGN(self.thisptr.linear(), + numpy_to_mat3d(data)) elif data.shape == (4,): - self.thisptr = new defs.Transform3f(numpy_to_quaternion3f(data)) + self.thisptr = new defs.Transform3d() + self.thisptr.setIdentity() + ASSIGN(self.thisptr.linear(), + numpy_to_quaternion3d(data).toRotationMatrix()) elif data.shape == (3,): - self.thisptr = new defs.Transform3f(numpy_to_vec3f(data)) + self.thisptr = new defs.Transform3d() + self.thisptr.setIdentity() + ASSIGN(self.thisptr.translation(), + numpy_to_vec3d(data)) else: raise ValueError('Invalid input to Transform().') elif len(args) == 2: @@ -45,9 +70,19 @@ cdef class Transform: raise ValueError('Translation must be (3,).') if rot.shape == (3,3): - self.thisptr = new defs.Transform3f(numpy_to_mat3f(rot), numpy_to_vec3f(trans)) + self.thisptr = new defs.Transform3d() + self.thisptr.setIdentity() + ASSIGN(self.thisptr.linear(), + numpy_to_mat3d(rot)) + ASSIGN(self.thisptr.translation(), + numpy_to_vec3d(trans)) elif rot.shape == (4,): - self.thisptr = new defs.Transform3f(numpy_to_quaternion3f(rot), numpy_to_vec3f(trans)) + self.thisptr = new defs.Transform3d() + self.thisptr.setIdentity() + ASSIGN(self.thisptr.linear(), + numpy_to_quaternion3d(rot).toRotationMatrix()) + ASSIGN(self.thisptr.translation(), + numpy_to_vec3d(trans)) else: raise ValueError('Invalid input to Transform().') else: @@ -58,29 +93,33 @@ cdef class Transform: free(self.thisptr) def getRotation(self): - return mat3f_to_numpy(self.thisptr.getRotation()) + return mat3d_to_numpy(self.thisptr.linear()) def getTranslation(self): - return vec3f_to_numpy(self.thisptr.getTranslation()) + return vec3d_to_numpy(self.thisptr.translation()) def getQuatRotation(self): - return quaternion3f_to_numpy(self.thisptr.getQuatRotation()) + cdef defs.Quaterniond quaternion = defs.Quaterniond(self.thisptr.linear()) + return quaternion3d_to_numpy(quaternion) def setRotation(self, R): - self.thisptr.setRotation(numpy_to_mat3f(R)) + ASSIGN(self.thisptr.linear(), + numpy_to_mat3d(R)) def setTranslation(self, T): - self.thisptr.setTranslation(numpy_to_vec3f(T)) + ASSIGN(self.thisptr.translation(), + numpy_to_vec3d(T)) def setQuatRotation(self, q): - self.thisptr.setQuatRotation(numpy_to_quaternion3f(q)) + ASSIGN(self.thisptr.linear(), + numpy_to_quaternion3d(q).toRotationMatrix()) ############################################################################### # Collision objects and geometries ############################################################################### cdef class CollisionObject: - cdef defs.CollisionObject *thisptr + cdef defs.CollisionObjectd *thisptr cdef defs.PyObject *geom cdef bool _no_instance @@ -92,9 +131,9 @@ cdef class CollisionObject: self._no_instance = _no_instance if geom.getNodeType() is not None and not self._no_instance: if tf is not None: - self.thisptr = new defs.CollisionObject(defs.shared_ptr[defs.CollisionGeometry](geom.thisptr), deref(tf.thisptr)) + self.thisptr = new defs.CollisionObjectd(defs.shared_ptr[defs.CollisionGeometryd](geom.thisptr), deref(tf.thisptr)) else: - self.thisptr = new defs.CollisionObject(defs.shared_ptr[defs.CollisionGeometry](geom.thisptr)) + self.thisptr = new defs.CollisionObjectd(defs.shared_ptr[defs.CollisionGeometryd](geom.thisptr)) self.thisptr.setUserData( self.geom) # Save the python geometry object for later retrieval else: if not self._no_instance: @@ -112,24 +151,24 @@ cdef class CollisionObject: return self.thisptr.getNodeType() def getTranslation(self): - return vec3f_to_numpy(self.thisptr.getTranslation()) + return vec3d_to_numpy(self.thisptr.getTranslation()) def setTranslation(self, vec): - self.thisptr.setTranslation(numpy_to_vec3f(vec)) + self.thisptr.setTranslation(numpy_to_vec3d(vec)) self.thisptr.computeAABB() def getRotation(self): - return mat3f_to_numpy(self.thisptr.getRotation()) + return mat3d_to_numpy(self.thisptr.getRotation()) def setRotation(self, mat): - self.thisptr.setRotation(numpy_to_mat3f(mat)) + self.thisptr.setRotation(numpy_to_mat3d(mat)) self.thisptr.computeAABB() def getQuatRotation(self): - return quaternion3f_to_numpy(self.thisptr.getQuatRotation()) + return quaternion3d_to_numpy(self.thisptr.getQuatRotation()) def setQuatRotation(self, q): - self.thisptr.setQuatRotation(numpy_to_quaternion3f(q)) + self.thisptr.setQuatRotation(numpy_to_quaternion3d(q)) self.thisptr.computeAABB() def getTransform(self): @@ -151,7 +190,7 @@ cdef class CollisionObject: return self.thisptr.isUncertain() cdef class CollisionGeometry: - cdef defs.CollisionGeometry *thisptr + cdef defs.CollisionGeometryd *thisptr def __cinit__(self): pass @@ -175,7 +214,7 @@ cdef class CollisionGeometry: property aabb_center: def __get__(self): if self.thisptr: - return vec3f_to_numpy(self.thisptr.aabb_center) + return vec3d_to_numpy(self.thisptr.aabb_center) else: return None def __set__(self, value): @@ -188,153 +227,149 @@ cdef class CollisionGeometry: cdef class TriangleP(CollisionGeometry): def __cinit__(self, a, b, c): - self.thisptr = new defs.TriangleP(numpy_to_vec3f(a), numpy_to_vec3f(b), numpy_to_vec3f(c)) + self.thisptr = new defs.TrianglePd(numpy_to_vec3d(a), numpy_to_vec3d(b), numpy_to_vec3d(c)) property a: def __get__(self): - return vec3f_to_numpy(( self.thisptr).a) + return vec3d_to_numpy(( self.thisptr).a) def __set__(self, value): - ( self.thisptr).a[0] = value[0] - ( self.thisptr).a[1] = value[1] - ( self.thisptr).a[2] = value[2] + ( self.thisptr).a[0] = value[0] + ( self.thisptr).a[1] = value[1] + ( self.thisptr).a[2] = value[2] property b: def __get__(self): - return vec3f_to_numpy(( self.thisptr).b) + return vec3d_to_numpy(( self.thisptr).b) def __set__(self, value): - ( self.thisptr).b[0] = value[0] - ( self.thisptr).b[1] = value[1] - ( self.thisptr).b[2] = value[2] + ( self.thisptr).b[0] = value[0] + ( self.thisptr).b[1] = value[1] + ( self.thisptr).b[2] = value[2] property c: def __get__(self): - return vec3f_to_numpy(( self.thisptr).c) + return vec3d_to_numpy(( self.thisptr).c) def __set__(self, value): - ( self.thisptr).c[0] = value[0] - ( self.thisptr).c[1] = value[1] - ( self.thisptr).c[2] = value[2] + ( self.thisptr).c[0] = value[0] + ( self.thisptr).c[1] = value[1] + ( self.thisptr).c[2] = value[2] cdef class Box(CollisionGeometry): def __cinit__(self, x, y, z): - self.thisptr = new defs.Box(x, y, z) + self.thisptr = new defs.Boxd(x, y, z) property side: def __get__(self): - return vec3f_to_numpy(( self.thisptr).side) + return vec3d_to_numpy(( self.thisptr).side) def __set__(self, value): - ( self.thisptr).side[0] = value[0] - ( self.thisptr).side[1] = value[1] - ( self.thisptr).side[2] = value[2] + ( self.thisptr).side[0] = value[0] + ( self.thisptr).side[1] = value[1] + ( self.thisptr).side[2] = value[2] cdef class Sphere(CollisionGeometry): def __cinit__(self, radius): - self.thisptr = new defs.Sphere(radius) + self.thisptr = new defs.Sphered(radius) property radius: def __get__(self): - return ( self.thisptr).radius + return ( self.thisptr).radius def __set__(self, value): - ( self.thisptr).radius = value + ( self.thisptr).radius = value cdef class Ellipsoid(CollisionGeometry): def __cinit__(self, a, b, c): - self.thisptr = new defs.Ellipsoid( a, b, c) + self.thisptr = new defs.Ellipsoidd( a, b, c) property radii: def __get__(self): - return vec3f_to_numpy(( self.thisptr).radii) + return vec3d_to_numpy(( self.thisptr).radii) def __set__(self, values): - ( self.thisptr).radii = numpy_to_vec3f(values) + ( self.thisptr).radii = numpy_to_vec3d(values) cdef class Capsule(CollisionGeometry): def __cinit__(self, radius, lz): - self.thisptr = new defs.Capsule(radius, lz) + self.thisptr = new defs.Capsuled(radius, lz) property radius: def __get__(self): - return ( self.thisptr).radius + return ( self.thisptr).radius def __set__(self, value): - ( self.thisptr).radius = value + ( self.thisptr).radius = value property lz: def __get__(self): - return ( self.thisptr).lz + return ( self.thisptr).lz def __set__(self, value): - ( self.thisptr).lz = value + ( self.thisptr).lz = value cdef class Cone(CollisionGeometry): def __cinit__(self, radius, lz): - self.thisptr = new defs.Cone(radius, lz) + self.thisptr = new defs.Coned(radius, lz) property radius: def __get__(self): - return ( self.thisptr).radius + return ( self.thisptr).radius def __set__(self, value): - ( self.thisptr).radius = value + ( self.thisptr).radius = value property lz: def __get__(self): - return ( self.thisptr).lz + return ( self.thisptr).lz def __set__(self, value): - ( self.thisptr).lz = value + ( self.thisptr).lz = value cdef class Cylinder(CollisionGeometry): def __cinit__(self, radius, lz): - self.thisptr = new defs.Cylinder(radius, lz) + self.thisptr = new defs.Cylinderd(radius, lz) property radius: def __get__(self): - return ( self.thisptr).radius + return ( self.thisptr).radius def __set__(self, value): - ( self.thisptr).radius = value + ( self.thisptr).radius = value property lz: def __get__(self): - return ( self.thisptr).lz + return ( self.thisptr).lz def __set__(self, value): - ( self.thisptr).lz = value + ( self.thisptr).lz = value cdef class Halfspace(CollisionGeometry): - def __cinit__(self, n, d): - self.thisptr = new defs.Halfspace(defs.Vec3f( n[0], - n[1], - n[2]), + def __cinit__(self, np.ndarray[double, ndim=1] n, d): + self.thisptr = new defs.Halfspaced(defs.Vector3d(&n[0]), d) property n: def __get__(self): - return vec3f_to_numpy(( self.thisptr).n) + return vec3d_to_numpy(( self.thisptr).n) def __set__(self, value): - ( self.thisptr).n[0] = value[0] - ( self.thisptr).n[1] = value[1] - ( self.thisptr).n[2] = value[2] + ( self.thisptr).n[0] = value[0] + ( self.thisptr).n[1] = value[1] + ( self.thisptr).n[2] = value[2] property d: def __get__(self): - return ( self.thisptr).d + return ( self.thisptr).d def __set__(self, value): - ( self.thisptr).d = value + ( self.thisptr).d = value cdef class Plane(CollisionGeometry): - def __cinit__(self, n, d): - self.thisptr = new defs.Plane(defs.Vec3f( n[0], - n[1], - n[2]), + def __cinit__(self, np.ndarray[double, ndim=1] n, d): + self.thisptr = new defs.Planed(defs.Vector3d(&n[0]), d) property n: def __get__(self): - return vec3f_to_numpy(( self.thisptr).n) + return vec3d_to_numpy(( self.thisptr).n) def __set__(self, value): - ( self.thisptr).n[0] = value[0] - ( self.thisptr).n[1] = value[1] - ( self.thisptr).n[2] = value[2] + ( self.thisptr).n[0] = value[0] + ( self.thisptr).n[1] = value[1] + ( self.thisptr).n[2] = value[2] property d: def __get__(self): - return ( self.thisptr).d + return ( self.thisptr).d def __set__(self, value): - ( self.thisptr).d = value + ( self.thisptr).d = value cdef class BVHModel(CollisionGeometry): def __cinit__(self): @@ -355,20 +390,21 @@ cdef class BVHModel(CollisionGeometry): return n def addVertex(self, x, y, z): - n = ( self.thisptr).addVertex(defs.Vec3f( x, y, z)) + cdef np.ndarray[double, ndim=1] n = numpy.array([x, y, z]) + n = ( self.thisptr).addVertex(defs.Vector3d(&n[0])) return self._check_ret_value(n) def addTriangle(self, v1, v2, v3): - n = ( self.thisptr).addTriangle(numpy_to_vec3f(v1), - numpy_to_vec3f(v2), - numpy_to_vec3f(v3)) + n = ( self.thisptr).addTriangle(numpy_to_vec3d(v1), + numpy_to_vec3d(v2), + numpy_to_vec3d(v3)) return self._check_ret_value(n) def addSubModel(self, verts, triangles): - cdef vector[defs.Vec3f] ps + cdef vector[defs.Vector3d] ps cdef vector[defs.Triangle] tris for vert in verts: - ps.push_back(numpy_to_vec3f(vert)) + ps.push_back(numpy_to_vec3d(vert)) for tri in triangles: tris.push_back(defs.Triangle( tri[0], tri[1], tri[2])) n = ( self.thisptr).addSubModel(ps, tris) @@ -406,7 +442,7 @@ cdef class OcTree(CollisionGeometry): self.tree = new octomap.OcTree(r) self.tree.readBinaryData(ss) - self.thisptr = new defs.OcTree(defs.shared_ptr[octomap.OcTree](self.tree)) + self.thisptr = new defs.OcTreed(defs.shared_ptr[octomap.OcTree](self.tree)) ############################################################################### @@ -414,11 +450,11 @@ cdef class OcTree(CollisionGeometry): ############################################################################### cdef class DynamicAABBTreeCollisionManager: - cdef defs.DynamicAABBTreeCollisionManager *thisptr + cdef defs.DynamicAABBTreeCollisionManagerd *thisptr cdef list objs def __cinit__(self): - self.thisptr = new defs.DynamicAABBTreeCollisionManager() + self.thisptr = new defs.DynamicAABBTreeCollisionManagerd() self.objs = [] def __dealloc__(self): @@ -426,7 +462,7 @@ cdef class DynamicAABBTreeCollisionManager: del self.thisptr def registerObjects(self, other_objs): - cdef vector[defs.CollisionObject*] pobjs + cdef vector[defs.CollisionObjectd*] pobjs for obj in other_objs: self.objs.append(obj) pobjs.push_back(( obj).thisptr) @@ -445,7 +481,7 @@ cdef class DynamicAABBTreeCollisionManager: self.thisptr.setup() def update(self, arg=None): - cdef vector[defs.CollisionObject*] objs + cdef vector[defs.CollisionObjectd*] objs if hasattr(arg, "__len__"): for a in arg: objs.push_back(( a).thisptr) @@ -547,10 +583,10 @@ def collide(CollisionObject o1, CollisionObject o2, if result is None: result = CollisionResult() - cdef defs.CollisionResult cresult + cdef defs.CollisionResultd cresult cdef size_t ret = defs.collide(o1.thisptr, o2.thisptr, - defs.CollisionRequest( + defs.CollisionRequestd( request.num_max_contacts, request.enable_contact, request.num_max_cost_sources, @@ -562,12 +598,12 @@ def collide(CollisionObject o1, CollisionObject o2, result.is_collision = result.is_collision or cresult.isCollision() - cdef vector[defs.Contact] contacts + cdef vector[defs.Contactd] contacts cresult.getContacts(contacts) for idx in range(contacts.size()): result.contacts.append(c_to_python_contact(contacts[idx], o1, o2)) - cdef vector[defs.CostSource] costs + cdef vector[defs.CostSourced] costs cresult.getCostSources(costs) for idx in range(costs.size()): result.cost_sources.append(c_to_python_costsource(costs[idx])) @@ -583,13 +619,13 @@ def continuousCollide(CollisionObject o1, Transform tf1_end, if result is None: result = ContinuousCollisionResult() - cdef defs.ContinuousCollisionResult cresult + cdef defs.ContinuousCollisionResultd cresult - cdef defs.FCL_REAL ret = defs.continuousCollide(o1.thisptr, deref(tf1_end.thisptr), + cdef double ret = defs.continuousCollide(o1.thisptr, deref(tf1_end.thisptr), o2.thisptr, deref(tf2_end.thisptr), - defs.ContinuousCollisionRequest( + defs.ContinuousCollisionRequestd( request.num_max_iterations, - request.toc_err, + request.toc_err, request.ccd_motion_type, request.gjk_solver_type, request.ccd_solver_type, @@ -609,18 +645,18 @@ def distance(CollisionObject o1, CollisionObject o2, if result is None: result = DistanceResult() - cdef defs.DistanceResult cresult + cdef defs.DistanceResultd cresult cdef double dis = defs.distance(o1.thisptr, o2.thisptr, - defs.DistanceRequest( + defs.DistanceRequestd( request.enable_nearest_points, request.gjk_solver_type ), cresult) result.min_distance = min(cresult.min_distance, result.min_distance) - result.nearest_points = [vec3f_to_numpy(cresult.nearest_points[0]), - vec3f_to_numpy(cresult.nearest_points[1])] + result.nearest_points = [vec3d_to_numpy(cresult.nearest_points[0]), + vec3d_to_numpy(cresult.nearest_points[1])] result.o1 = c_to_python_collision_geometry(cresult.o1, o1, o2) result.o2 = c_to_python_collision_geometry(cresult.o2, o1, o2) result.b1 = cresult.b1 @@ -670,7 +706,7 @@ cdef class CollisionFunction: self.py_func = py_func self.py_args = py_args - cdef bool eval_func(self, defs.CollisionObject*o1, defs.CollisionObject*o2): + cdef bool eval_func(self, defs.CollisionObjectd*o1, defs.CollisionObjectd*o2): cdef object py_r = defs.PyObject_CallObject(self.py_func, (copy_ptr_collision_object(o1), copy_ptr_collision_object(o2), @@ -686,18 +722,18 @@ cdef class DistanceFunction: self.py_func = py_func self.py_args = py_args - cdef bool eval_func(self, defs.CollisionObject*o1, defs.CollisionObject*o2, defs.FCL_REAL& dist): + cdef bool eval_func(self, defs.CollisionObjectd*o1, defs.CollisionObjectd*o2, double& dist): cdef object py_r = defs.PyObject_CallObject(self.py_func, (copy_ptr_collision_object(o1), copy_ptr_collision_object(o2), self.py_args)) - (&dist)[0] = py_r[1] + (&dist)[0] = py_r[1] return py_r[0] -cdef inline bool CollisionCallBack(defs.CollisionObject*o1, defs.CollisionObject*o2, void*cdata): +cdef inline bool CollisionCallBack(defs.CollisionObjectd*o1, defs.CollisionObjectd*o2, void*cdata): return ( cdata).eval_func(o1, o2) -cdef inline bool DistanceCallBack(defs.CollisionObject*o1, defs.CollisionObject*o2, void*cdata, defs.FCL_REAL& dist): +cdef inline bool DistanceCallBack(defs.CollisionObjectd*o1, defs.CollisionObjectd*o2, void*cdata, double& dist): return ( cdata).eval_func(o1, o2, dist) @@ -705,58 +741,59 @@ cdef inline bool DistanceCallBack(defs.CollisionObject*o1, defs.CollisionObject* # Helper Functions ############################################################################### -cdef quaternion3f_to_numpy(defs.Quaternion3f q): - return numpy.array([q.getW(), q.getX(), q.getY(), q.getZ()]) +cdef quaternion3d_to_numpy(defs.Quaterniond q): + return numpy.array([q.w(), q.x(), q.y(), q.z()]) -cdef defs.Quaternion3f numpy_to_quaternion3f(a): - return defs.Quaternion3f( a[0], a[1], a[2], a[3]) +cdef defs.Quaterniond numpy_to_quaternion3d(a): + return defs.Quaterniond( a[0], a[1], a[2], a[3]) -cdef vec3f_to_numpy(defs.Vec3f vec): +cdef vec3d_to_numpy(defs.Vector3d vec): return numpy.array([vec[0], vec[1], vec[2]]) -cdef defs.Vec3f numpy_to_vec3f(a): - return defs.Vec3f( a[0], a[1], a[2]) +cdef defs.Vector3d numpy_to_vec3d(arr): + cdef double[:] memview = arr.astype(numpy.float64) + return defs.Vector3d(&memview[0]) -cdef mat3f_to_numpy(defs.Matrix3f m): +cdef mat3d_to_numpy(defs.Matrix3d m): return numpy.array([[m(0,0), m(0,1), m(0,2)], [m(1,0), m(1,1), m(1,2)], [m(2,0), m(2,1), m(2,2)]]) -cdef defs.Matrix3f numpy_to_mat3f(a): - return defs.Matrix3f( a[0][0], a[0][1], a[0][2], - a[1][0], a[1][1], a[1][2], - a[2][0], a[2][1], a[2][2]) +cdef defs.Matrix3d numpy_to_mat3d(arr): + # NOTE Eigen defaults to column-major storage, + # which corresponds to non-default Fortran mode of ordering in numpy + cdef double[:, :] memview = arr.astype(numpy.float64, order='F') + return defs.Matrix3d(&memview[0, 0]) -cdef c_to_python_collision_geometry(defs.const_CollisionGeometry*geom, CollisionObject o1, CollisionObject o2): - cdef CollisionGeometry o1_py_geom = (( o1.thisptr).getUserData()) - cdef CollisionGeometry o2_py_geom = (( o2.thisptr).getUserData()) - if geom == o1_py_geom.thisptr: +cdef c_to_python_collision_geometry(defs.const_CollisionGeometryd*geom, CollisionObject o1, CollisionObject o2): + cdef CollisionGeometry o1_py_geom = (( o1.thisptr).getUserData()) + cdef CollisionGeometry o2_py_geom = (( o2.thisptr).getUserData()) + if geom == o1_py_geom.thisptr: return o1_py_geom else: return o2_py_geom -cdef c_to_python_contact(defs.Contact contact, CollisionObject o1, CollisionObject o2): +cdef c_to_python_contact(defs.Contactd contact, CollisionObject o1, CollisionObject o2): c = Contact() c.o1 = c_to_python_collision_geometry(contact.o1, o1, o2) c.o2 = c_to_python_collision_geometry(contact.o2, o1, o2) c.b1 = contact.b1 c.b2 = contact.b2 - c.normal = vec3f_to_numpy(contact.normal) - c.pos = vec3f_to_numpy(contact.pos) + c.normal = vec3d_to_numpy(contact.normal) + c.pos = vec3d_to_numpy(contact.pos) c.penetration_depth = contact.penetration_depth return c -cdef c_to_python_costsource(defs.CostSource cost_source): +cdef c_to_python_costsource(defs.CostSourced cost_source): c = CostSource() - c.aabb_min = vec3f_to_numpy(cost_source.aabb_min) - c.aabb_max = vec3f_to_numpy(cost_source.aabb_max) + c.aabb_min = vec3d_to_numpy(cost_source.aabb_min) + c.aabb_max = vec3d_to_numpy(cost_source.aabb_max) c.cost_density = cost_source.cost_density c.total_cost = cost_source.total_cost return c -cdef copy_ptr_collision_object(defs.CollisionObject*cobj): +cdef copy_ptr_collision_object(defs.CollisionObjectd*cobj): geom = cobj.getUserData() co = CollisionObject(geom, _no_instance=True) ( co).thisptr = cobj return co - diff --git a/src/fcl/fcl_defs.pxd b/src/fcl/fcl_defs.pxd new file mode 100644 index 0000000..2bbe78a --- /dev/null +++ b/src/fcl/fcl_defs.pxd @@ -0,0 +1,411 @@ +from libcpp cimport bool +from libcpp.string cimport string +from libcpp.vector cimport vector +from libcpp.set cimport set +from libcpp.memory cimport shared_ptr +cimport octomap_defs as octomap + +cdef extern from "Python.h": + ctypedef struct PyObject + void Py_INCREF(PyObject *obj) + void Py_DECREF(PyObject *obj) + object PyObject_CallObject(object obj, object args) + object PySequence_Concat(object obj1, object obj2) + +#cdef extern from "boost/shared_ptr.hpp" namespace "boost": +# cppclass shared_ptr[T]: +# shared_ptr() except + +# shared_ptr(T*) except + +# T* get() + +cdef extern from "fcl/common/types.h" namespace "fcl": + cdef cppclass Vector3d: + Vector3d() except + + Vector3d(double *data) except + + double& operator[](size_t i) + + cdef cppclass Matrix3d: + Matrix3d() except + + Matrix3d(double *data) + double operator()(size_t i, size_t j) + + cdef cppclass Quaterniond: + Quaterniond() except + + Quaterniond(double a, double b, double c, double d) except + + Quaterniond(Matrix3d& R) except + + double& w() + double& x() + double& y() + double& z() + Matrix3d& toRotationMatrix() + + cdef cppclass Transform3d: + Transform3d() except + + Transform3d(Transform3d& tf_) + void setIdentity() + Matrix3d& linear() + Vector3d& translation() + +cdef extern from "fcl/narrowphase/continuous_collision_request.h" namespace "fcl": + cdef enum CCDMotionType: + CCDM_TRANS, CCDM_LINEAR, CCDM_SCREW, CCDM_SPLINE + + cdef enum CCDSolverType: + CCDC_NAIVE, CCDC_CONSERVATIVE_ADVANCEMENT, CCDC_RAY_SHOOTING, CCDC_POLYNOMIAL_SOLVER + + cdef cppclass ContinuousCollisionRequestd: + size_t num_max_iterations_, + double toc_err_, + CCDMotionType ccd_motion_type_, + GJKSolverType gjk_solver_type_, + GJKSolverType ccd_solver_type_ + + ContinuousCollisionRequestd( + size_t num_max_iterations_, + double toc_err_, + CCDMotionType ccd_motion_type_, + GJKSolverType gjk_solver_type_, + CCDSolverType ccd_solver_type_ ) + +cdef extern from "fcl/narrowphase/gjk_solver_type.h" namespace "fcl": + cdef enum GJKSolverType: + GST_LIBCCD, GST_INDEP + +cdef extern from "fcl/narrowphase/contact.h" namespace "fcl": + cdef cppclass Contactd: + CollisionGeometryd *o1 + CollisionGeometryd *o2 + int b1 + int b2 + Vector3d normal + Vector3d pos + double penetration_depth + Contactd() except + + Contactd(CollisionGeometryd* o1_, + CollisionGeometryd* o2_, + int b1_, int b2_) except + + +cdef extern from "fcl/narrowphase/cost_source.h" namespace "fcl": + cdef cppclass CostSourced: + Vector3d aabb_min + Vector3d aabb_max + double cost_density + double total_cost + +cdef extern from "fcl/narrowphase/collision_result.h" namespace "fcl": + cdef cppclass CollisionResultd: + CollisionResultd() except + + bool isCollision() + void getContacts(vector[Contactd]& contacts_) + void getCostSources(vector[CostSourced]& cost_sources_) + +cdef extern from "fcl/narrowphase/continuous_collision_result.h" namespace "fcl": + cdef cppclass ContinuousCollisionResultd: + ContinuousCollisionResultd() except + + bool is_collide + double time_of_contact + Transform3d contact_tf1, contact_tf2 + +cdef extern from "fcl/narrowphase/collision_request.h" namespace "fcl": + cdef cppclass CollisionRequestd: + size_t num_max_contacts + bool enable_contact + size_t num_max_cost_sources + bool enable_cost + bool use_approximate_cost + GJKSolverType gjk_solver_type + CollisionRequestd(size_t num_max_contacts_, + bool enable_contact_, + size_t num_max_cost_sources_, + bool enable_cost_, + bool use_approximate_cost_, + GJKSolverType gjk_solver_type_) + +cdef extern from "fcl/narrowphase/distance_result.h" namespace "fcl": + cdef cppclass DistanceResultd: + double min_distance + Vector3d* nearest_points + CollisionGeometryd* o1 + CollisionGeometryd* o2 + int b1 + int b2 + DistanceResultd(double min_distance_) except + + DistanceResultd() except + + +cdef extern from "fcl/narrowphase/distance_request.h" namespace "fcl": + cdef cppclass DistanceRequestd: + bool enable_nearest_points + GJKSolverType gjk_solver_type + DistanceRequestd(bool enable_nearest_points_, GJKSolverType gjk_solver_type_) except + + +cdef extern from "fcl/geometry/collision_geometry.h" namespace "fcl": + cdef enum OBJECT_TYPE: + OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE, OT_COUNT + cdef enum NODE_TYPE: + BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_kIOS, BV_OBBRSS, BV_KDOP16, BV_KDOP18, BV_KDOP24, + GEOM_BOX, GEOM_SPHERE, GEOM_ELLIPSOID, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, + GEOM_PLANE, GEOM_HALFSPACE, GEOM_TRIANGLE, GEOM_OCTREE, NODE_COUNT + + cdef cppclass CollisionGeometryd: + CollisionGeometryd() except + + OBJECT_TYPE getObjectType() + NODE_TYPE getNodeType() + void computeLocalAABB() + Vector3d aabb_center + double aabb_radius + double cost_density + double threshold_occupied + double threshold_free + + ctypedef CollisionGeometryd const_CollisionGeometryd "const fcl::CollisionGeometryd" + +cdef extern from "fcl/narrowphase/collision_object.h" namespace "fcl": + cdef cppclass CollisionObjectd: + CollisionObjectd() except + + CollisionObjectd(shared_ptr[CollisionGeometryd]& cgeom_) except + + CollisionObjectd(shared_ptr[CollisionGeometryd]& cgeom_, Transform3d& tf) except + + OBJECT_TYPE getObjectType() + NODE_TYPE getNodeType() + Vector3d& getTranslation() + Matrix3d& getRotation() + Quaterniond& getQuatRotation() + Transform3d& getTransform() + CollisionGeometryd* getCollisionGeometry() + void setTranslation(Vector3d& T) + void setRotation(Matrix3d& M) + void setQuatRotation(Quaterniond& q) + void setTransform(Quaterniond& q, Vector3d& T) + void setTransform(Matrix3d& q, Vector3d& T) + void setTransform(Transform3d& tf) + void setUserData(void *data) + void computeAABB() + void *getUserData() + bool isOccupied() + bool isFree() + bool isUncertain() + + ctypedef CollisionObjectd const_CollisionObjectd "const fcl::CollisionObjectd" + +cdef extern from "fcl/geometry/shape/shape_base.h" namespace "fcl": + cdef cppclass ShapeBased(CollisionGeometryd): + ShapeBased() except + + +cdef extern from "fcl/geometry/shape/triangle_p.h" namespace "fcl": + cdef cppclass TrianglePd(ShapeBased): + TrianglePd(Vector3d& a_, Vector3d& b_, Vector3d& c_) except + + Vector3d a, b, c + +cdef extern from "fcl/geometry/shape/box.h" namespace "fcl": + cdef cppclass Boxd(ShapeBased): + Boxd(double x, double y, double z) except + + Vector3d side + +cdef extern from "fcl/geometry/shape/sphere.h" namespace "fcl": + cdef cppclass Sphered(ShapeBased): + Sphered(double radius_) except + + double radius + +cdef extern from "fcl/geometry/shape/ellipsoid.h" namespace "fcl": + cdef cppclass Ellipsoidd(ShapeBased): + Ellipsoidd(double a_, double b_, double c_) except + + Vector3d radii + +cdef extern from "fcl/geometry/shape/capsule.h" namespace "fcl": + cdef cppclass Capsuled(ShapeBased): + Capsuled(double radius_, double lz_) except + + double radius + double lz + +cdef extern from "fcl/geometry/shape/cone.h" namespace "fcl": + cdef cppclass Coned(ShapeBased): + Coned(double radius_, double lz_) except + + double radius + double lz + +cdef extern from "fcl/geometry/shape/cylinder.h" namespace "fcl": + cdef cppclass Cylinderd(ShapeBased): + Cylinderd(double radius_, double lz_) except + + double radius + double lz + +cdef extern from "fcl/geometry/shape/convex.h" namespace "fcl": + cdef cppclass Convexd(ShapeBased): + Convexd(Vector3d* plane_nomals_, + double* plane_dis_, + int num_planes, + Vector3d* points_, + int num_points_, + int* polygons_) except + + +cdef extern from "fcl/geometry/shape/halfspace.h" namespace "fcl": + cdef cppclass Halfspaced(ShapeBased): + Halfspaced(Vector3d& n_, double d_) except + + Vector3d n + double d + +cdef extern from "fcl/geometry/shape/plane.h" namespace "fcl": + cdef cppclass Planed(ShapeBased): + Planed(Vector3d& n_, double d_) except + + Vector3d n + double d + +cdef extern from "fcl/broadphase/broadphase_collision_manager.h" namespace "fcl": + ctypedef bool (*CollisionCallBack)(CollisionObjectd* o1, CollisionObjectd* o2, void* cdata) + ctypedef bool (*DistanceCallBack)(CollisionObjectd* o1, CollisionObjectd* o2, void* cdata, double& dist) + +cdef extern from "fcl/broadphase/broadphase_dynamic_AABB_tree.h" namespace "fcl": + cdef cppclass DynamicAABBTreeCollisionManagerd: + DynamicAABBTreeCollisionManagerd() except + + void registerObjects(vector[CollisionObjectd*]& other_objs) + void registerObject(CollisionObjectd* obj) + void unregisterObject(CollisionObjectd* obj) + void collide(DynamicAABBTreeCollisionManagerd* mgr, void* cdata, CollisionCallBack callback) + void distance(DynamicAABBTreeCollisionManagerd* mgr, void* cdata, DistanceCallBack callback) + void collide(CollisionObjectd* obj, void* cdata, CollisionCallBack callback) + void distance(CollisionObjectd* obj, void* cdata, DistanceCallBack callback) + void collide(void* cdata, CollisionCallBack callback) + void distance(void* cdata, DistanceCallBack callback) + void setup() + void update() + void update(CollisionObjectd* updated_obj) + void update(vector[CollisionObjectd*] updated_objs) + void clear() + bool empty() + size_t size() + int max_tree_nonbalanced_level + int tree_incremental_balance_pass + int& tree_topdown_balance_threshold + int& tree_topdown_level + int tree_init_level + bool octree_as_geometry_collide + bool octree_as_geometry_distance + +cdef extern from "fcl/narrowphase/collision.h" namespace "fcl": + size_t collide(CollisionObjectd* o1, CollisionObjectd* o2, + CollisionRequestd& request, + CollisionResultd& result) + + size_t collide(CollisionGeometryd* o1, Transform3d& tf1, + CollisionGeometryd* o2, Transform3d& tf2, + CollisionRequestd& request, + CollisionResultd& result) + +cdef extern from "fcl/narrowphase/continuous_collision.h" namespace "fcl": + double continuousCollide(CollisionGeometryd* o1, Transform3d& tf1_beg, Transform3d& tf1_end, + CollisionGeometryd* o2, Transform3d& tf2_beg, Transform3d& tf2_end, + ContinuousCollisionRequestd& request, + ContinuousCollisionResultd& result) + + double continuousCollide(CollisionObjectd* o1, Transform3d& tf1_end, + CollisionObjectd* o2, Transform3d& tf2_end, + ContinuousCollisionRequestd& request, + ContinuousCollisionResultd& result) + + +cdef extern from "fcl/narrowphase/distance.h" namespace "fcl": + double distance(CollisionObjectd* o1, CollisionObjectd* o2, + DistanceRequestd& request, DistanceResultd& result) + double distance(CollisionGeometryd* o1, Transform3d& tf1, + CollisionGeometryd* o2, Transform3d& tf2, + DistanceRequestd& request, DistanceResultd& result) + +cdef extern from "fcl/geometry/bvh/BVH_internal.h" namespace "fcl": + cdef enum BVHModelType: + BVH_MODEL_UNKNOWN, # unknown model type + BVH_MODEL_TRIANGLES, # triangle model + BVH_MODEL_POINTCLOUD # point cloud model + + cdef enum BVHReturnCode: + BVH_OK = 0, # BVH is valid + BVH_ERR_MODEL_OUT_OF_MEMORY = -1, # Cannot allocate memory for vertices and triangles + BVH_ERR_BUILD_OUT_OF_SEQUENCE = -2, # BVH construction does not follow correct sequence + BVH_ERR_BUILD_EMPTY_MODEL = -3, # BVH geometry is not prepared + BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME = -4, # BVH geometry in previous frame is not prepared + BVH_ERR_UNSUPPORTED_FUNCTION = -5, # BVH funtion is not supported + BVH_ERR_UNUPDATED_MODEL = -6, # BVH model update failed + BVH_ERR_INCORRECT_DATA = -7, # BVH data is not valid + BVH_ERR_UNKNOWN = -8 # Unknown failure + + + cdef enum BVHBuildState: + BVH_BUILD_STATE_EMPTY, # empty state, immediately after constructor + BVH_BUILD_STATE_BEGUN, # after beginModel(), state for adding geometry primitives + BVH_BUILD_STATE_PROCESSED, # after tree has been build, ready for cd use + BVH_BUILD_STATE_UPDATE_BEGUN, # after beginUpdateModel(), state for updating geometry primitives + BVH_BUILD_STATE_UPDATED, # after tree has been build for updated geometry, ready for ccd use + BVH_BUILD_STATE_REPLACE_BEGUN, # after beginReplaceModel(), state for replacing geometry primitives + +cdef extern from "fcl/math/triangle.h" namespace "fcl": + cdef cppclass Triangle: + Triangle() except + + Triangle(size_t p1, size_t p2, size_t p3) except + + size_t vids[3] + +# TODO what about these guys? +cdef extern from "fcl/geometry/bvh/detail/BV_splitter_base.h" namespace "fcl": + cdef cppclass BVSplitterBase: + pass + +cdef extern from "fcl/geometry/bvh/detail/BV_fitter_base.h" namespace "fcl": + cdef cppclass BVFitterBase: + pass + +cdef extern from "fcl/geometry/bvh/BVH_model.h" namespace "fcl": + # Cython only accepts type template parameters. + # see https://groups.google.com/forum/#!topic/cython-users/xAZxdCFw6Xs + cdef cppclass BVHModel "fcl::BVHModel" ( CollisionGeometryd ): + # Constructing an empty BVH + BVHModel() except + + BVHModel(BVHModel& other) except + + # + #Geometry point data + Vector3d* vertices + # + #Geometry triangle index data, will be NULL for point clouds + Triangle* tri_indices + # + #Geometry point data in previous frame + Vector3d* prev_vertices + # + #Number of triangles + int num_tris + # + #Number of points + int num_vertices + # + #The state of BVH building process + BVHBuildState build_state + # + # # #Split rule to split one BV node into two children + # + # boost::shared_ptr > bv_splitter + shared_ptr[BVSplitterBase] bv_splitter + # boost::shared_ptr > bv_fitter + shared_ptr[BVFitterBase] bv_fitter + + int beginModel(int num_tris_, int num_vertices_) + + int addVertex(const Vector3d& p) + + int addTriangle(const Vector3d& p1, const Vector3d& p2, const Vector3d& p3) + + #int addSubModel(const std::vector& ps) + # void getCostSources(vector[CostSourced]& cost_sources_) + + #int addSubModel(const vector[Vector3d]& ps) + # + int addSubModel(const vector[Vector3d]& ps, const vector[Triangle]& ts) + + int endModel() + + int buildTree() + + # void computeLocalAABB() + + +cdef extern from "fcl/geometry/octree/octree.h" namespace "fcl": + cdef cppclass OcTreed(CollisionGeometryd): + # Constructing + OcTreed(double resolution) except + + OcTreed(shared_ptr[octomap.OcTree]& tree_) except + + diff --git a/fcl/octomap_defs.pxd b/src/fcl/octomap_defs.pxd similarity index 100% rename from fcl/octomap_defs.pxd rename to src/fcl/octomap_defs.pxd diff --git a/fcl/std_defs.pxd b/src/fcl/std_defs.pxd similarity index 100% rename from fcl/std_defs.pxd rename to src/fcl/std_defs.pxd diff --git a/src/fcl/version.py b/src/fcl/version.py new file mode 100644 index 0000000..8411e55 --- /dev/null +++ b/src/fcl/version.py @@ -0,0 +1 @@ +__version__ = '0.6.1'