diff --git a/.github/workflows/push.yml b/.github/workflows/push.yml new file mode 100644 index 0000000..a60fa69 --- /dev/null +++ b/.github/workflows/push.yml @@ -0,0 +1,35 @@ +name: Build Pull Requests + +on: [pull_request] + +jobs: + formatting: + name: Check Formatting + runs-on: ubuntu-latest + steps: + - name: Check out source repository + uses: actions/checkout@v3 + - name: Set up Python environment + uses: actions/setup-python@v4 + with: + python-version: "3.12" + - name: ruff + run: | + pip install ruff==0.9.6 + ruff check . + + build_wheels: + name: Build wheel on ${{matrix.platform}} + runs-on: ${{matrix.platform}} + strategy: + fail-fast: false + matrix: + platform: [ubuntu-latest, ubuntu-24.04-arm, macos-latest, windows-latest, macos-15-intel] + steps: + - uses: actions/checkout@v4 + - name: Build wheels + uses: pypa/cibuildwheel@v3.2.1 + - uses: actions/upload-artifact@v4 + with: + path: ./wheelhouse/*.whl + name: wheels-${{ matrix.platform }} diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml new file mode 100644 index 0000000..854583c --- /dev/null +++ b/.github/workflows/release.yml @@ -0,0 +1,65 @@ +name: Release To Pypi + +on: + push: + branches: + - master + +jobs: + build_wheels: + name: Build wheel on ${{matrix.platform}} + runs-on: ${{matrix.platform}} + strategy: + matrix: + platform: [ubuntu-latest, ubuntu-24.04-arm, macos-latest, windows-latest, macos-15-intel] + steps: + - uses: actions/checkout@v4 + - name: Build wheels + uses: pypa/cibuildwheel@v3.2.1 + - uses: actions/upload-artifact@v4 + with: + path: ./wheelhouse/*.whl + name: wheels-${{ matrix.platform }} + + upload_pypi: + name: Release To PyPi + needs: [build_wheels] + runs-on: ubuntu-latest + permissions: + id-token: write + steps: + - uses: actions/setup-python@v4 + with: + python-version: "3.12" + + - uses: actions/download-artifact@v4 + with: + path: dist + pattern: wheels-* + merge-multiple: true + - uses: pypa/gh-action-pypi-publish@v1.13.0 + with: + password: ${{ secrets.PYPI_API_TOKEN }} + create_release: + name: Create GitHub Release + runs-on: ubuntu-latest + needs: [build_wheels, upload_pypi] + steps: + - name: Checkout code + uses: actions/checkout@v4 + with: + fetch-depth: 2 + - name: Tag Version + id: set_tag + run: | + export VER=v$(python -c "exec(open('src/fcl/version.py','r').read());print(__version__)") + echo "tag_name=${VER}" >> $GITHUB_OUTPUT + - uses: actions/download-artifact@v4 + with: + path: dist + pattern: wheels-* + merge-multiple: true + - uses: ncipollo/release-action@v1.14.0 + with: + artifacts: "dist/*" + tag: ${{ steps.set_tag.outputs.tag_name }} diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..2cd1861 --- /dev/null +++ b/.gitignore @@ -0,0 +1,109 @@ +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so +*.cpp + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST +deps/ + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +.hypothesis/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +.static_storage/ +.media/ +local_settings.py + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# pyenv +.python-version + +# celery beat schedule file +celerybeat-schedule + +# SageMath parsed files +*.sage.py + +# Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ + +# vscode settings +.vscode/ diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..41fd22c --- /dev/null +++ b/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2017, Matthew Matl +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/README.md b/README.md new file mode 100644 index 0000000..5948444 --- /dev/null +++ b/README.md @@ -0,0 +1,365 @@ +# python-fcl +### Python Interface for the Flexible Collision Library + +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.7.0. + +This package supports three types of proximity queries for pairs of geometric models: +* __Collision Detection__: Detecting whether two models overlap (and optionally where). +* __Distance Computation__: Computing the minimum distance between a pair of models. +* __Continuous Collision Detection__: Detecting whether two models overlap during motion (and optionally the time of contact). + +This package also supports most of FCL's object shapes, including: +* TriangleP +* Box +* Sphere +* Ellipsoid +* Capsule +* Cone +* Convex +* Cylinder +* Half-Space +* Plane +* Mesh +* OcTree + +## Installation + +First, install [octomap](https://github.com/OctoMap/octomap), which is necessary to use OcTree. For Ubuntu, use `sudo apt-get install liboctomap-dev`. +Second, install FCL using the instructions provided [here](https://github.com/flexible-collision-library/fcl). +If you're on Ubuntu 17.04 or newer, you can install FCL using `sudo apt-get install libfcl-dev`. +Otherwise, just compile FCL from source -- it's quick and easy, and its dependencies are all easily installed via `apt` or `brew`. +Note: the provided install scripts (under `build_dependencies`) can automate this process as well. + +In order to install the Python wrappers for FCL, simply run +```shell +pip install python-fcl +``` + +## Objects + +### Collision Objects +The primary construct in FCL is the `CollisionObject`, which forms the backbone of all collision and distance computations. +A `CollisionObject` consists of two components -- its geometry, defined by a `CollisionGeometry` object, and its pose, defined by a `Transform` object. + +#### Collision Geometries +There are two main types of `CollisionGeometry` objects -- geometric primitives, such as boxes and spheres, +and arbitrary triangular meshes. +Here's some examples of how to instantiate geometric primitives. +Note that the box, sphere, ellipsoid, capsule, cone, and cylinder are all centered at the origin. + +```python +import numpy as np +import fcl + +v1 = np.array([1.0, 2.0, 3.0]) +v2 = np.array([2.0, 1.0, 3.0]) +v3 = np.array([3.0, 2.0, 1.0]) +x, y, z = 1, 2, 3 +rad, lz = 1.0, 3.0 +n = np.array([1.0, 0.0, 0.0]) +d = 5.0 + +t = fcl.TriangleP(v1, v2, v3) # Triangle defined by three points +b = fcl.Box(x, y, z) # Axis-aligned box with given side lengths +s = fcl.Sphere(rad) # Sphere with given radius +e = fcl.Ellipsoid(x, y, z) # Axis-aligned ellipsoid with given radii +c = fcl.Capsule(rad, lz) # Capsule with given radius and height along z-axis +c = fcl.Cone(rad, lz) # Cone with given radius and cylinder height along z-axis +c = fcl.Cylinder(rad, lz) # Cylinder with given radius and height along z-axis +h = fcl.Halfspace(n, d) # Half-space defined by {x : < d} +p = fcl.Plane(n, d) # Plane defined by {x : = d} +``` + +Triangular meshes are wrapped by the `BVHModel` class, and they are instantiated a bit differently. +```python +verts = np.array([[1.0, 1.0, 1.0], + [2.0, 1.0, 1.0], + [1.0, 2.0, 1.0], + [1.0, 1.0, 2.0]]) +tris = np.array([[0,2,1], + [0,3,2], + [0,1,3], + [1,2,3]]) + +m = fcl.BVHModel() +m.beginModel(len(verts), len(tris)) +m.addSubModel(verts, tris) +m.endModel() +``` + +If the mesh is convex, such as the example above, you can also wrap it in the `Convex` class. Note that the instantiation is a bit different because the `Convex` class supports arbitrary polygons for each face of the convex object. +```python +verts = np.array([[1.0, 1.0, 1.0], + [2.0, 1.0, 1.0], + [1.0, 2.0, 1.0], + [1.0, 1.0, 2.0]]) +tris = np.array([[0,2,1], + [0,3,2], + [0,1,3], + [1,2,3]]) +faces = np.concatenate((3 * np.ones((len(tris), 1), dtype=np.int64), tris), axis=1).flatten() +c = fcl.Convex(verts, len(tris), faces) +``` + +#### Transforms +In addition to a `CollisionGeometry`, a `CollisionObject` requires a `Transform`, which tells FCL where the `CollisionGeometry` is actually located in the world. +All `Transform` objects specify a rigid transformation (i.e. a rotation and a translation). +The translation is always a 3-entry vector, while the rotation can be specified by a 3x3 rotation matrix or a 4-entry quaternion. + +Here are some examples of possible ways to instantiate and manipulate a `Transform`. + +```python +R = np.array([[0.0, -1.0, 0.0], + [1.0, 0.0, 0.0], + [0.0, 0.0, 1.0]]) +T = np.array([1.0, 2.0, 3.0]) +q = np.array([0.707, 0.0, 0.0, 0.707]) + +tf = fcl.Transform() # Default gives identity transform +tf = fcl.Transform(q) # Quaternion rotation, zero translation +tf = fcl.Transform(R) # Matrix rotation, zero translation +tf = fcl.Transform(T) # Translation, identity rotation +tf = fcl.Transform(q, T) # Quaternion rotation and translation +tf = fcl.Transform(R, T) # Matrix rotation and translation +tf1 = fcl.Transform(tf) # Can also initialize with another Transform +``` + +Now, given a `CollisionGeometry` and a `Transform`, we can create a `CollisionObject`: + +```python +t = fcl.Transform(R, T) +b = fcl.Box(x, y, z) +obj = fcl.CollisionObject(b, t) +``` + +The transform of a collision object can be modified in-place: +```python +t1 = fcl.Transform(R1, T1) +obj.setTransform(t1) # Using a transform +obj.setRotation(R2) # Specifying components individually +obj.setTranslation(T2) +obj.setQuatRotation(q2) +``` +## Commands + +### Pairwise Operations + +Given a pair of collision objects, this library supports three types of queries: +* __Collision Detection__ +* __Distance Computation__ +* __Continuous Collision Detection__ + +The interfaces for each of these operations follow a common pipeline. +First, a query request data structure is initialized and populated with parameters. +Then, an empty query response structure is initialized. +Finally, the query function is called with the two `CollisionObject` items, the request structure, and the response structure as arguments. +The query function returns a scalar result, and any additional information is stored in the query result data structure. +Examples of all three operations are shown below. + +#### Collision Checking + +```python +g1 = fcl.Box(1,2,3) +t1 = fcl.Transform() +o1 = fcl.CollisionObject(g1, t1) + +g2 = fcl.Cone(1,3) +t2 = fcl.Transform() +o2 = fcl.CollisionObject(g2, t2) + +request = fcl.CollisionRequest() +result = fcl.CollisionResult() + +ret = fcl.collide(o1, o2, request, result) +``` + +After calling `fcl.collide()`, `ret` contains the number of contacts generated between the two objects, +and `result` contains information about the collision and contacts. +For more information about available parameters for collision requests and results, +see `fcl/collision_data.py`. + +#### Distance Checking + +```python +g1 = fcl.Box(1,2,3) +t1 = fcl.Transform() +o1 = fcl.CollisionObject(g1, t1) + +g2 = fcl.Cone(1,3) +t2 = fcl.Transform() +o2 = fcl.CollisionObject(g2, t2) + +request = fcl.DistanceRequest() +result = fcl.DistanceResult() + +ret = fcl.distance(o1, o2, request, result) +``` + +After calling `fcl.distance()`, `ret` contains the minimum distance between the two objects +and `result` contains information about the closest points on the objects. +If `ret` is negative, the objects are in collision. +For more information about available parameters for distance requests and results, +see `fcl/collision_data.py`. + +#### Continuous Collision Checking + +```python +g1 = fcl.Box(1,2,3) +t1 = fcl.Transform() +o1 = fcl.CollisionObject(g1, t1) +t1_final = fcl.Transform(np.array([1.0, 0.0, 0.0])) + +g2 = fcl.Cone(1,3) +t2 = fcl.Transform() +o2 = fcl.CollisionObject(g2, t2) +t2_final = fcl.Transform(np.array([-1.0, 0.0, 0.0])) + +request = fcl.ContinuousCollisionRequest() +result = fcl.ContinuousCollisionResult() + +ret = fcl.continuousCollide(o1, t1_final, o2, t2_final, request, result) +``` + +After calling `fcl.continuousCollide()`, `ret` contains the time of contact in (0,1), or 1.0 if the objects did not collide during movement from their initial poses to their final poses. +Additionally, `result` contains information about the collision time and status. +For more information about available parameters for continuous collision requests and results, +see `fcl/collision_data.py`. + +### Broadphase Checking +In addition to pairwise checks, FCL supports broadphase collision/distance queries between groups of objects and can avoid n-squared complexity. +Specifically, `CollisionObject` items are registered with a `DynamicAABBTreeCollisionManager` before collision or distance checking is performed. + +Three types of checks are possible: +* One-to-many: Collision/distance checking between a stand-alone `CollisionObject` and all objects managed by a manager. +* Internal many-to-many: Pairwise collision/distance checking between all pairs of objects managed by a manager. +* Group many-to-many: Pairwise collision/distance checking between items from two managers. + +In general, the collision methods can return all contact pairs, while the distance methods will just return the single closest distance between any pair of objects. +Here are some examples of managed collision checking. +The methods take a callback function -- use the defaults from `python-fcl` unless you have a special use case -- and a wrapper object, either `CollisionData` or `DistanceData`, that wraps a request-response pair. This object also has a field, `done`, that tells the recursive collision checker when to quit. +Be sure to use a new `Data` object for each request or set the `done` attribute to `False` before reusing one. + +```python +objs1 = [fcl.CollisionObject(box), fcl.CollisionObject(sphere)] +objs2 = [fcl.CollisionObject(cone), fcl.CollisionObject(mesh)] + +manager1 = fcl.DynamicAABBTreeCollisionManager() +manager2 = fcl.DynamicAABBTreeCollisionManager() + +manager1.registerObjects(objs1) +manager2.registerObjects(objs2) + +manager1.setup() +manager2.setup() + +#===================================================================== +# Managed internal (sub-n^2) collision checking +#===================================================================== +cdata = fcl.CollisionData() +manager1.collide(cdata, fcl.defaultCollisionCallback) +print 'Collision within manager 1?: {}'.format(cdata.result.is_collision) + +##===================================================================== +## Managed internal (sub-n^2) distance checking +##===================================================================== +ddata = fcl.DistanceData() +manager1.distance(ddata, fcl.defaultDistanceCallback) +print 'Closest distance within manager 1?: {}'.format(ddata.result.min_distance) + +#===================================================================== +# Managed one to many collision checking +#===================================================================== +req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True) +rdata = fcl.CollisionData(request = req) + +manager1.collide(fcl.CollisionObject(mesh), rdata, fcl.defaultCollisionCallback) +print 'Collision between manager 1 and Mesh?: {}'.format(rdata.result.is_collision) +print 'Contacts:' +for c in rdata.result.contacts: + print '\tO1: {}, O2: {}'.format(c.o1, c.o2) + +#===================================================================== +# Managed many to many collision checking +#===================================================================== +rdata = fcl.CollisionData(request = req) +manager1.collide(manager2, rdata, fcl.defaultCollisionCallback) +print 'Collision between manager 1 and manager 2?: {}'.format(rdata.result.is_collision) +print 'Contacts:' +for c in rdata.result.contacts: + print '\tO1: {}, O2: {}'.format(c.o1, c.o2) +``` + +### Extracting Which Objects Are In Collision + +To determine which objects are actually in collision, you'll need parse the collision data's contacts and use an additional external data structure. + +Specifically, the `fcl.CollisionData` object that is passed into any `collide()` call has an internal set of contacts, stored in `cdata.result.contacts`. +This object is a simple list of `Contact` objects, each of which represents a contact point between two objects. +Each contact object has two attributes, `o1` and `o2`, that store references to the original `fcl.CollisionGeometry` objects were created for the two `fcl.CollisionObject` objects that are in collision. +This is a bit wonky, but it's part of the FCL API. + +Therefore, all you have to do is make a map from the `id` of each `fcl.CollisionGeometry` object to either the actual `fcl.CollisionObject` it corresponds to or to some string identifier for each object. +Then, you can iterate over `cdata.result.contacts`, extract `o1` and `o2`, apply the built-in `id()` function to each, and find the corresponding data you want in your map. + +Here's an example. + +```python +import fcl +import numpy as np + +# Create collision geometry and objects +geom1 = fcl.Cylinder(1.0, 1.0) +obj1 = fcl.CollisionObject(geom1) + +geom2 = fcl.Cylinder(1.0, 1.0) +obj2 = fcl.CollisionObject(geom2, fcl.Transform(np.array([0.0, 0.0, 0.3]))) + +geom3 = fcl.Cylinder(1.0, 1.0) +obj3 = fcl.CollisionObject(geom3, fcl.Transform(np.array([0.0, 0.0, 3.0]))) + +geoms = [geom1, geom2, geom3] +objs = [obj1, obj2, obj3] +names = ['obj1', 'obj2', 'obj3'] + +# Create map from geometry IDs to objects +geom_id_to_obj = { id(geom) : obj for geom, obj in zip(geoms, objs) } + +# Create map from geometry IDs to string names +geom_id_to_name = { id(geom) : name for geom, name in zip(geoms, names) } + +# Create manager +manager = fcl.DynamicAABBTreeCollisionManager() +manager.registerObjects(objs) +manager.setup() + +# Create collision request structure +crequest = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True) +cdata = fcl.CollisionData(crequest, fcl.CollisionResult()) + +# Run collision request +manager.collide(cdata, fcl.defaultCollisionCallback) + +# Extract collision data from contacts and use that to infer set of +# objects that are in collision +objs_in_collision = set() + +for contact in cdata.result.contacts: + # Extract collision geometries that are in contact + coll_geom_0 = contact.o1 + coll_geom_1 = contact.o2 + + # Get their names + coll_names = [geom_id_to_name[id(coll_geom_0)], geom_id_to_name[id(coll_geom_1)]] + coll_names = tuple(sorted(coll_names)) + objs_in_collision.add(coll_names) + +for coll_pair in objs_in_collision: + print('Object {} in collision with object {}!'.format(coll_pair[0], coll_pair[1])) +``` + +``` +>>> Object obj1 in collision with object obj2! +``` +For more examples, see `examples/example.py`. diff --git a/README.rst b/README.rst deleted file mode 100644 index 294f1ea..0000000 --- a/README.rst +++ /dev/null @@ -1,24 +0,0 @@ -python-fcl -========= - -About ------ -This library is a Python binding of FCL library. - -Build ------ -Building **python-fcl requires FCL headers and libraries**. -When building, you can specify their location with the --include-dirs -and --library-dirs command line options: - - $ python setup.py build_ext --include-dirs /path/to/includes --library-dirs /path/to/libraries - -If you use ubuntu 12.04 and install ros-fcl at /opt directory, you can use the following command: - - $ python setup.py - -Install -------- -You can install python-fcl using the normal distutils install command: - - $ python setup.py install diff --git a/build_dependencies/Dockerfile b/build_dependencies/Dockerfile new file mode 100644 index 0000000..f1f19d2 --- /dev/null +++ b/build_dependencies/Dockerfile @@ -0,0 +1,25 @@ +# NOTE: this docker file is only for local testing of the build +# example of testing a build locally: +# docker build . -t pythonfcl -f build_dependencies/Dockerfile + +FROM quay.io/pypa/manylinux2010_x86_64:latest + +# build and install libccd and fcl using cmake +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/cp39-cp39/bin:$PATH + +# we need numpy to build python-fcl +# since we set our path we'll be using the right pip +RUN pip install numpy cython + +# build the python-fcl module +COPY . /python_fcl +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.7.0-cp39-cp39-linux_x86_64.whl -w /wheelhouse diff --git a/build_dependencies/install_linux.sh b/build_dependencies/install_linux.sh new file mode 100644 index 0000000..ad899cf --- /dev/null +++ b/build_dependencies/install_linux.sh @@ -0,0 +1,52 @@ +# exit immediately on any failed step +set -xe + +# get cmake from pip so we can version-lock +pip install cmake==3.31.6 +# Set CMAKE variable to use the cmake in the same directory as python +export CMAKE="$(dirname $(which python))/cmake" + +$CMAKE --version + +mkdir -p deps +cd deps + +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.9.8 https://github.com/OctoMap/octomap.git + +rm -rf fcl +git clone --depth 1 --branch v0.7.0 https://github.com/ambi-robotics/fcl.git + +# Install eigen +$CMAKE -B build -S eigen-3.3.9 +$CMAKE --install build + +# Build and install libccd +cd libccd +$CMAKE . -DENABLE_DOUBLE_PRECISION=ON +make -j4 +make install +cd .. + +# Build and install octomap +cd octomap +$CMAKE . -DCMAKE_BUILD_TYPE=Release -DBUILD_OCTOVIS_SUBPROJECT=OFF -DBUILD_DYNAMICETD3D_SUBPROJECT=OFF -DCMAKE_CXX_STANDARD=17 -DCMAKE_CXX_STANDARD_REQUIRED=ON -DCMAKE_CXX_FLAGS="-std=c++17 -Wno-error=template-id-cdtor" +make -j4 +make install +cd .. + +# Build and install fcl +cd fcl +$CMAKE . +make -j4 +make install +cd .. + +# Update library cache so the system can find the newly installed libraries +ldconfig diff --git a/build_dependencies/install_macos.sh b/build_dependencies/install_macos.sh new file mode 100644 index 0000000..0ab672c --- /dev/null +++ b/build_dependencies/install_macos.sh @@ -0,0 +1,51 @@ +# exit immediately on any failed step +set -xe +pip install cmake==3.31.6 + +# Set CMAKE variable to use the cmake in the same directory as python +export CMAKE="$(dirname $(which python))/cmake" + +$CMAKE --version + +mkdir -p deps +cd deps + +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 +mv eigen-3.3.9 eigen + +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.9.8 https://github.com/OctoMap/octomap.git + +rm -rf fcl +git clone --depth 1 --branch v0.7.0 https://github.com/ambi-robotics/fcl.git + +# Install eigen +$CMAKE -B build -S eigen +sudo $CMAKE --install build + +# Build and install libccd +cd libccd +$CMAKE . -D ENABLE_DOUBLE_PRECISION=ON +make -j4 +sudo make install +cd .. + +# Build and install octomap +cd octomap +$CMAKE . -D CMAKE_BUILD_TYPE=Release -D BUILD_OCTOVIS_SUBPROJECT=OFF -D BUILD_DYNAMICETD3D_SUBPROJECT=OFF +make -j4 +sudo make install +cd .. + +# Build and install fcl +cd fcl +$CMAKE . +make -j4 +sudo make install +cd .. + +cd .. diff --git a/build_dependencies/install_windows.ps1 b/build_dependencies/install_windows.ps1 new file mode 100644 index 0000000..e0d9f77 --- /dev/null +++ b/build_dependencies/install_windows.ps1 @@ -0,0 +1,105 @@ +<# +Originally based on script written by Pebaz (https://github.com/Pebaz/python-fcl/blob/master/requirements/build_win32.ps1) +but with many modifications in order to use fcl 0.7.0 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 17 2022" + +# All compiled dependencies will be installed 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 ENABLE_DOUBLE_PRECISION=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.9.8 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.7.0 https://github.com/ambi-robotics/fcl.git +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/example/example.py b/example/example.py deleted file mode 100644 index c15a07a..0000000 --- a/example/example.py +++ /dev/null @@ -1,66 +0,0 @@ -from fcl import fcl, transform - -objs = [fcl.CollisionObject(fcl.Box(1.0, 2.0, 3.0)), - fcl.CollisionObject(fcl.Sphere(4.0)), - fcl.CollisionObject(fcl.Cone(5.0, 6.0))] - -# Register objects to DynamicAABBTreeCollisionManager -manager = fcl.DynamicAABBTreeCollisionManager() -print "Before resgister: ", manager.size() -manager.registerObjects(objs) -print "After register 1 : ", manager.size() -manager.registerObject(fcl.CollisionObject(fcl.Cylinder(7.0, 8.0))) -print "After register 2 : ", manager.size() - -# Use Callback function -def cb_func(obj1, obj2, res): - print "cb_func start" - ret, res = fcl.collide(obj1, obj2, fcl.CollisionRequest()) - print "result: ", ret - return ret -res = fcl.CollisionResult() -manager.collide(res, cb_func) - -# Collision calcuration -ret, result = fcl.collide(fcl.CollisionObject(fcl.Box(1.0, 2.0, 3.0), - transform.Transform(transform.Quaternion(), [10.0, 0.0, 0.0])), - fcl.CollisionObject(fcl.Sphere(4.0), - transform.Transform(transform.Quaternion(), [-10.0, 0.0, 0.0])), - fcl.CollisionRequest()) - -print "-- Collision result: ", ret -for contact in result.contacts: - print contact.o1 - print contact.o2 -for cost_source in result.cost_sources: - print cost_source - -dis, result = fcl.distance(fcl.CollisionObject(fcl.Box(1.0, 2.0, 3.0), - transform.Transform(transform.Quaternion(), [10.0, 0.0, 0.0])), - fcl.CollisionObject(fcl.Sphere(4.0), - transform.Transform(transform.Quaternion(), [-10.0, 0.0, 0.0])), - fcl.DistanceRequest(True)) - -print "-- Distance result: ", dis -print result.nearest_points - -ret, result = fcl.collide(fcl.CollisionObject(fcl.Box(1.0, 2.0, 3.0), - transform.Transform(transform.Quaternion())), - fcl.CollisionObject(fcl.Sphere(4.0), - transform.Transform(transform.Quaternion(), [0.0, 0.0, 0.0])), - fcl.CollisionRequest()) -print "-- Collision result: ", ret -for contact in result.contacts: - print contact.o1 - print contact.o2 -for cost_source in result.cost_sources: - print cost_source - -dis, result = fcl.distance(fcl.CollisionObject(fcl.Box(1.0, 2.0, 3.0), - transform.Transform(transform.Quaternion())), - fcl.CollisionObject(fcl.Sphere(4.0), - transform.Transform(transform.Quaternion(), [0.0, 0.0, 0.0])), - fcl.DistanceRequest(True)) - -print "-- Distance result: ", dis -print result.nearest_points diff --git a/examples/example.py b/examples/example.py new file mode 100644 index 0000000..0b50f0e --- /dev/null +++ b/examples/example.py @@ -0,0 +1,212 @@ +import numpy as np + +import fcl + + +def print_collision_result(o1_name, o2_name, result): + print(f"Collision between {o1_name} and {o2_name}:") + print("-" * 30) + print(f"Collision?: {result.is_collision}") + print(f"Number of contacts: {len(result.contacts)}") + print("") + + +def print_continuous_collision_result(o1_name, o2_name, result): + print(f"Continuous collision between {o1_name} and {o2_name}:") + print("-" * 30) + print(f"Collision?: {result.is_collide}") + print(f"Time of collision: {result.time_of_contact}") + print("") + + +def print_distance_result(o1_name, o2_name, result): + print(f"Distance between {o1_name} and {o2_name}:") + print("-" * 30) + print(f"Distance: {result.min_distance}") + print("Closest Points:") + print(result.nearest_points[0]) + print(result.nearest_points[1]) + print("") + + +# Create simple geometries +box = fcl.Box(1.0, 2.0, 3.0) +sphere = fcl.Sphere(4.0) +cone = fcl.Cone(5.0, 6.0) +cyl = fcl.Cylinder(2.0, 2.0) + +verts = np.array([[1.0, 1.0, 1.0], [2.0, 1.0, 1.0], [1.0, 2.0, 1.0], [1.0, 1.0, 2.0]]) +tris = np.array([[0, 2, 1], [0, 3, 2], [0, 1, 3], [1, 2, 3]]) + +# Create mesh geometry +mesh = fcl.BVHModel() +mesh.beginModel(len(verts), len(tris)) +mesh.addSubModel(verts, tris) +mesh.endModel() + +# ===================================================================== +# Pairwise collision checking +# ===================================================================== +print("=" * 60) +print("Testing Pairwise Collision Checking") +print("=" * 60) +print("") + +req = fcl.CollisionRequest(enable_contact=True) +res = fcl.CollisionResult() + +n_contacts = fcl.collide( + fcl.CollisionObject(box, fcl.Transform()), + fcl.CollisionObject(cone, fcl.Transform()), + req, + res, +) +print_collision_result("Box", "Cone", res) + +n_contacts = fcl.collide( + fcl.CollisionObject(box, fcl.Transform()), + fcl.CollisionObject(cyl, fcl.Transform(np.array([6.0, 0.0, 0.0]))), + req, + res, +) +print_collision_result("Box", "Cylinder", res) + +n_contacts = fcl.collide( + fcl.CollisionObject(mesh, fcl.Transform(np.array([0.0, 0.0, -1.0]))), + fcl.CollisionObject(cyl, fcl.Transform()), + req, + res, +) +print_collision_result("Box", "Mesh", res) + +# ===================================================================== +# Pairwise distance checking +# ===================================================================== +print("=" * 60) +print("Testing Pairwise Distance Checking") +print("=" * 60) +print("") + +req = fcl.DistanceRequest(enable_nearest_points=True, enable_signed_distance=True) +res = fcl.DistanceResult() + +dist = fcl.distance( + fcl.CollisionObject(box, fcl.Transform()), + fcl.CollisionObject(cone, fcl.Transform()), + req, + res, +) +print_distance_result("Box", "Cone", res) + +dist = fcl.distance( + fcl.CollisionObject(box, fcl.Transform()), + fcl.CollisionObject(cyl, fcl.Transform(np.array([6.0, 0.0, 0.0]))), + req, + res, +) +print_distance_result("Box", "Cylinder", res) + +dist = fcl.distance( + fcl.CollisionObject(box, fcl.Transform()), + fcl.CollisionObject(box, fcl.Transform(np.array([1.01, 0.0, 0.0]))), + req, + res, +) +print_distance_result("Box", "Box", res) + +# ===================================================================== +# Pairwise continuous collision checking +# ===================================================================== +print("=" * 60) +print("Testing Pairwise Continuous Collision Checking") +print("=" * 60) +print("") + +req = fcl.ContinuousCollisionRequest() +res = fcl.ContinuousCollisionResult() + +dist = fcl.continuousCollide( + fcl.CollisionObject(box, fcl.Transform()), + fcl.Transform(np.array([5.0, 0.0, 0.0])), + fcl.CollisionObject(cyl, fcl.Transform(np.array([5.0, 0.0, 0.0]))), + fcl.Transform(np.array([0.0, 0.0, 0.0])), + req, + res, +) +print_continuous_collision_result("Box", "Cylinder", res) + +# ===================================================================== +# Managed collision checking +# ===================================================================== +print("=" * 60) +print("Testing Managed Collision and Distance Checking") +print("=" * 60) +print("") +objs1 = [ + fcl.CollisionObject(box, fcl.Transform(np.array([20, 0, 0]))), + fcl.CollisionObject(sphere), +] +objs2 = [fcl.CollisionObject(cone), fcl.CollisionObject(mesh)] +objs3 = [fcl.CollisionObject(box), fcl.CollisionObject(sphere)] + +manager1 = fcl.DynamicAABBTreeCollisionManager() +manager2 = fcl.DynamicAABBTreeCollisionManager() +manager3 = fcl.DynamicAABBTreeCollisionManager() + +manager1.registerObjects(objs1) +manager2.registerObjects(objs2) +manager3.registerObjects(objs3) + +manager1.setup() +manager2.setup() +manager3.setup() + +# ===================================================================== +# Managed internal (n^2) collision checking +# ===================================================================== +cdata = fcl.CollisionData() +manager1.collide(cdata, fcl.defaultCollisionCallback) +print(f"Collision within manager 1?: {cdata.result.is_collision}") +print("") + +cdata = fcl.CollisionData() +manager2.collide(cdata, fcl.defaultCollisionCallback) +print(f"Collision within manager 2?: {cdata.result.is_collision}") +print("") + +# ===================================================================== +# Managed internal (n^2) distance checking +# ===================================================================== +ddata = fcl.DistanceData() +manager1.distance(ddata, fcl.defaultDistanceCallback) +print(f"Closest distance within manager 1?: {ddata.result.min_distance}") +print("") + +ddata = fcl.DistanceData() +manager2.distance(ddata, fcl.defaultDistanceCallback) +print(f"Closest distance within manager 2?: {ddata.result.min_distance}") +print("") + +# ===================================================================== +# Managed one to many collision checking +# ===================================================================== +req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True) +rdata = fcl.CollisionData(request=req) + +manager1.collide(fcl.CollisionObject(mesh), rdata, fcl.defaultCollisionCallback) +print(f"Collision between manager 1 and Mesh?: {rdata.result.is_collision}") +print("Contacts:") +for c in rdata.result.contacts: + print(f"\tO1: {c.o1}, O2: {c.o2}") +print("") + +# ===================================================================== +# Managed many to many collision checking +# ===================================================================== +rdata = fcl.CollisionData(request=req) +manager3.collide(manager2, rdata, fcl.defaultCollisionCallback) +print(f"Collision between manager 2 and manager 3?: {rdata.result.is_collision}") +print("Contacts:") +for c in rdata.result.contacts: + print(f"\tO1: {c.o1}, O2: {c.o2}") +print("") diff --git a/fcl/__init__.py b/fcl/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/fcl/collision_data.py b/fcl/collision_data.py deleted file mode 100644 index a68ea66..0000000 --- a/fcl/collision_data.py +++ /dev/null @@ -1,51 +0,0 @@ -import sys - -class OBJECT_TYPE: - OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE, OT_COUNT = range(5) - def __init__(self): - pass - -class 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_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE,\ - GEOM_HALFSPACE, GEOM_TRIANGLE, GEOM_OCTREE, NODE_COUNT = range(20) - def __init__(self): - pass - -class CostSource: - def __init__(self): - self.aabb_min = [0.0, 0.0, 0.0] - self.cost_density = 0.0 - self.total_cost = 0.0 - -class CollisionResult: - def __init__(self): - self.contacts = [] - self.cost_sources = [] - -class CollisionRequest: - def __init__(self, - num_max_contacts = 1, - enable_contact = False, - num_max_cost_sources = 1, - enable_cost = False, - use_approximate_cost = True): - self.num_max_contacts = num_max_contacts - self.enable_contact = enable_contact - self.num_max_cost_sources = num_max_cost_sources - self.enable_cost = enable_cost - self.use_approximate_cost = use_approximate_cost - -class DistanceResult: - def __init__(self, min_distance_ = sys.float_info.max): - self.min_distance = min_distance_ - self.nearest_points = [None, None] - self.o1 = None - self.o2 = None - self.b1 = -1 - self.b2 = -1 - -class DistanceRequest: - def __init__(self, - enable_nearest_points_ = False): - self.enable_nearest_points = enable_nearest_points_ diff --git a/fcl/fcl.pyx b/fcl/fcl.pyx deleted file mode 100644 index e4a9468..0000000 --- a/fcl/fcl.pyx +++ /dev/null @@ -1,474 +0,0 @@ - -from libcpp cimport bool -from libcpp.string cimport string -from libcpp.vector cimport vector -from libc.stdlib cimport free -from libc.string cimport memcpy -from cython.operator cimport dereference as deref, preincrement as inc, address -cimport fcl_defs as defs -import inspect -import transform as tf -from collision_data import * -cimport numpy as np -ctypedef np.float64_t DOUBLE_t - -cdef class CollisionFunction: - cdef: - object py_func - object py_args - - def __init__(self, py_func, py_args): - self.py_func = py_func - self.py_args = py_args - cdef bool eval_func(self, defs.CollisionObject* o1, defs.CollisionObject* o2): - cdef object py_r = defs.PyObject_CallObject(self.py_func, - (copy_ptr_collision_object(o1), - copy_ptr_collision_object(o2), - self.py_args)) - return py_r - -cdef class DistanceFunction: - cdef: - object py_func - object py_args - - def __init__(self, py_func, py_args): - 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 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] - return py_r[0] - -cdef inline bool CollisionCallBack(defs.CollisionObject* o1, defs.CollisionObject* 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): - return (cdata).eval_func(o1, o2, dist) - -cdef vec3f_to_tuple(defs.Vec3f vec): - return (vec[0], vec[1], vec[2]) - -cdef vec3f_to_list(defs.Vec3f vec): - return [vec[0], vec[1], vec[2]] - -cdef c_to_python_quaternion(defs.Quaternion3f q): - return tf.Quaternion(q.getW(), q.getX(), q.getY(), q.getZ()) - -cdef class CollisionObject: - cdef defs.CollisionObject *thisptr - cdef defs.PyObject *geom - cdef bool _no_instance - def __cinit__(self, CollisionGeometry geom=CollisionGeometry(), tf=None, _no_instance=False): - defs.Py_INCREF(geom) - self.geom = geom - self._no_instance = _no_instance - if not geom.getNodeType() is None: - if not tf is None: - self.thisptr = new defs.CollisionObject(defs.shared_ptr[defs.CollisionGeometry](geom.thisptr), - defs.Transform3f(defs.Quaternion3f(tf.q.w, - tf.q.x, - tf.q.y, - tf.q.z), - defs.Vec3f(tf.t[0], - tf.t[1], - tf.t[2]))) - else: - self.thisptr = new defs.CollisionObject(defs.shared_ptr[defs.CollisionGeometry](geom.thisptr)) - else: - if not self._no_instance: - raise ValueError - def __dealloc__(self): - if self.thisptr and not self._no_instance: - free(self.thisptr) - defs.Py_DECREF(self.geom) - def getObjectType(self): - return self.thisptr.getObjectType() - def getNodeType(self): - return self.thisptr.getNodeType() - def getTranslation(self): - return vec3f_to_tuple(self.thisptr.getTranslation()) - def getQuatRotation(self): - cdef defs.Quaternion3f quat = self.thisptr.getQuatRotation() - return c_to_python_quaternion(quat) - def setTranslation(self, vec): - self.thisptr.setTranslation(defs.Vec3f(vec[0], vec[1], vec[2])) - def setQuatRotation(self, q): - self.thisptr.setQuatRotation(defs.Quaternion3f(q[0], q[1], q[2], q[3])) - def setTransform(self, q, vec): - self.thisptr.setTransform(defs.Quaternion3f(q[0], q[1], q[2], q[3]), - defs.Vec3f(vec[0], vec[1], vec[2])) - def isOccupied(self): - return self.thisptr.isOccupied() - def isFree(self): - return self.thisptr.isFree() - def isUncertain(self): - return self.thisptr.isUncertain() - -cdef class CollisionGeometry: - cdef defs.CollisionGeometry *thisptr - def __cinit__(self): - pass - def __dealloc__(self): - if self.thisptr: - del self.thisptr - def getNodeType(self): - if self.thisptr: - return self.thisptr.getNodeType() - else: - return None - def computeLocalAABB(self): - if self.thisptr: - self.thisptr.computeLocalAABB() - else: - return None - property aabb_center: - def __get__(self): - if self.thisptr: - return vec3f_to_tuple(self.thisptr.aabb_center) - else: - return None - def __set__(self, value): - if self.thisptr: - self.thisptr.aabb_center[0] = value[0] - self.thisptr.aabb_center[1] = value[1] - self.thisptr.aabb_center[2] = value[2] - else: - raise ReferenceError - -cdef class ShapeBase(CollisionGeometry): - def __cinit__(self): - pass - -cdef class TriangleP(ShapeBase): - def __cinit__(self, a, b, c): - self.thisptr = new defs.TriangleP(defs.Vec3f(a[0], a[1], a[2]), - defs.Vec3f(b[0], b[1], b[2]), - defs.Vec3f(c[0], c[1], c[2])) - property a: - def __get__(self): - return vec3f_to_tuple((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] - property b: - def __get__(self): - return vec3f_to_tuple((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] - property c: - def __get__(self): - return vec3f_to_tuple((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] - -cdef class Box(ShapeBase): - def __cinit__(self, x, y, z): - self.thisptr = new defs.Box(x, y, z) - property side: - def __get__(self): - return vec3f_to_tuple((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] - -cdef class Sphere(ShapeBase): - def __cinit__(self, radius): - self.thisptr = new defs.Sphere(radius) - property radius: - def __get__(self): - return (self.thisptr).radius - def __set__(self, value): - (self.thisptr).radius = value - -cdef class Capsule(ShapeBase): - def __cinit__(self, radius, lz): - self.thisptr = new defs.Capsule(radius, lz) - property radius: - def __get__(self): - return (self.thisptr).radius - def __set__(self, value): - (self.thisptr).radius = value - property lz: - def __get__(self): - return (self.thisptr).lz - def __set__(self, value): - (self.thisptr).lz = value - -cdef class Cone(ShapeBase): - def __cinit__(self, radius, lz): - self.thisptr = new defs.Cone(radius, lz) - property radius: - def __get__(self): - return (self.thisptr).radius - def __set__(self, value): - (self.thisptr).radius = value - property lz: - def __get__(self): - return (self.thisptr).lz - def __set__(self, value): - (self.thisptr).lz = value - -cdef class Cylinder(ShapeBase): - def __cinit__(self, radius, lz): - self.thisptr = new defs.Cylinder(radius, lz) - property radius: - def __get__(self): - return (self.thisptr).radius - def __set__(self, value): - (self.thisptr).radius = value - property lz: - def __get__(self): - return (self.thisptr).lz - def __set__(self, value): - (self.thisptr).lz = value - -cdef class Halfspace(ShapeBase): - def __cinit__(self, n, d): - self.thisptr = new defs.Halfspace(defs.Vec3f(n[0], - n[1], - n[2]), - d) - property n: - def __get__(self): - return vec3f_to_tuple((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] - property d: - def __get__(self): - return (self.thisptr).d - def __set__(self, value): - (self.thisptr).d = value - -cdef class Plane(ShapeBase): - def __cinit__(self, n, d): - self.thisptr = new defs.Plane(defs.Vec3f(n[0], - n[1], - n[2]), - d) - property n: - def __get__(self): - return vec3f_to_tuple((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] - property d: - def __get__(self): - return (self.thisptr).d - def __set__(self, value): - (self.thisptr).d = value - -class Contact: - def __init__(self): - self.o1 = CollisionGeometry() - self.o2 = CollisionGeometry() - self.b1 = 0 - self.b2 = 0 - self.normal = [0.0, 0.0, 0.0] - self.pos = [0.0, 0.0, 0.0] - self.penetration_depth = 0.0 - -cdef class DynamicAABBTreeCollisionManager: - cdef defs.DynamicAABBTreeCollisionManager *thisptr - cdef vector[defs.PyObject*]* objs - def __cinit__(self): - self.thisptr = new defs.DynamicAABBTreeCollisionManager() - self.objs = new vector[defs.PyObject* ]() - def __dealloc__(self): - if self.thisptr: - del self.thisptr - for idx in range(self.objs.size()): - defs.Py_DECREF(deref(self.objs)[idx]) - def registerObjects(self, other_objs): - cdef vector[defs.CollisionObject*] pobjs - for o in other_objs: - defs.Py_INCREF(o) - self.objs.push_back(o) - pobjs.push_back((o).thisptr) - self.thisptr.registerObjects(pobjs) - def registerObject(self, CollisionObject obj): - defs.Py_INCREF(obj) - self.objs.push_back(obj) - self.thisptr.registerObject(obj.thisptr) - def collide(self, *args): - if len(args) == 2 and inspect.isfunction(args[1]): - fn = CollisionFunction(args[1], args[0]) - self.thisptr.collide(fn, CollisionCallBack) - elif len(args) == 3 and inspect.isfunction(args[2]): - fn = CollisionFunction(args[2], args[1]) - self.thisptr.collide((args[0]).thisptr, fn, CollisionCallBack) - else: - raise ValueError - - def distance(self, *args): - if len(args) == 2 and inspect.isfunction(args[1]): - fn = DistanceFunction(args[1], args[0]) - self.thisptr.distance( fn, DistanceCallBack) - elif len(args) == 3 and inspect.isfunction(args[2]): - fn = DistanceFunction(args[2], args[1]) - self.thisptr.distance(( args[0]).thisptr, fn, DistanceCallBack) - else: - raise ValueError - - def setup(self): - self.thisptr.setup() - - def update(self, arg=None): - cdef vector[defs.CollisionObject*] objs - if hasattr(arg, "__len__"): - for a in arg: - objs.push_back((a).thisptr) - self.thisptr.update(objs) - elif arg is None: - self.thisptr.update() - else: - self.thisptr.update((arg).thisptr) - def clear(self): - self.thisptr.clear() - def empty(self): - return self.thisptr.empty() - def size(self): - return self.thisptr.size() - property max_tree_nonbalanced_level: - def __get__(self): - return self.thisptr.max_tree_nonbalanced_level - def __set__(self, value): - self.thisptr.max_tree_nonbalanced_level = value - property tree_incremental_balance_pass: - def __get__(self): - return self.thisptr.tree_incremental_balance_pass - def __set__(self, value): - self.thisptr.tree_incremental_balance_pass = value - property tree_topdown_balance_threshold: - def __get__(self): - return self.thisptr.tree_topdown_balance_threshold - def __set__(self, value): - self.thisptr.tree_topdown_balance_threshold = value - property tree_topdown_level: - def __get__(self): - return self.thisptr.tree_topdown_level - def __set__(self, value): - self.thisptr.tree_topdown_level = value - property tree_init_level: - def __get__(self): - return self.thisptr.tree_init_level - def __set__(self, value): - self.thisptr.tree_init_level = value - property octree_as_geometry_collide: - def __get__(self): - return self.thisptr.octree_as_geometry_collide - def __set__(self, value): - self.thisptr.octree_as_geometry_collide = value - property octree_as_geometry_distance: - def __get__(self): - return self.thisptr.octree_as_geometry_distance - def __set__(self, value): - self.thisptr.octree_as_geometry_distance = value - -cdef c_to_python_collision_geometry(defs.const_CollisionGeometry* geom): - if geom.getNodeType() == defs.GEOM_BOX: - obj = Box(0, 0, 0) - memcpy(obj.thisptr, geom, sizeof(defs.Box)) - return obj - elif geom.getNodeType() == defs.GEOM_SPHERE: - obj = Sphere(0) - memcpy(obj.thisptr, geom, sizeof(defs.Sphere)) - return obj - elif geom.getNodeType() == defs.GEOM_CAPSULE: - obj = Capsule(0, 0) - memcpy(obj.thisptr, geom, sizeof(defs.Capsule)) - return obj - elif geom.getNodeType() == defs.GEOM_CONE: - obj = Cone(0, 0) - memcpy(obj.thisptr, geom, sizeof(defs.Cone)) - return obj - elif geom.getNodeType() == defs.GEOM_CYLINDER: - obj = Cylinder(0, 0) - memcpy(obj.thisptr, geom, sizeof(defs.Cylinder)) - return obj - elif geom.getNodeType() == defs.GEOM_TRIANGLE: - obj = TriangleP(np.zeros(3), np.zeros(3), np.zeros(3)) - memcpy(obj.thisptr, geom, sizeof(defs.TriangleP)) - return obj - elif geom.getNodeType() == defs.GEOM_HALFSPACE: - obj = Halfspace(np.zeros(3), 0) - memcpy(obj.thisptr, geom, sizeof(defs.Halfspace)) - return obj - elif geom.getNodeType() == defs.GEOM_PLANE: - obj = Plane(np.zeros(3), 0) - memcpy(obj.thisptr, geom, sizeof(defs.Plane)) - return obj - -cdef copy_ptr_collision_object(defs.CollisionObject* cobj): - co = CollisionObject(_no_instance=True) - (co).thisptr = cobj - return co - -cdef c_to_python_contact(defs.Contact contact): - c = Contact() - c.o1 = c_to_python_collision_geometry(contact.o1) - c.o2 = c_to_python_collision_geometry(contact.o2) - c.b1 = contact.b1 - c.b2 = contact.b2 - c.normal = vec3f_to_list(contact.normal) - c.pos = vec3f_to_list(contact.pos) - c.penetration_depth = contact.penetration_depth - return c - -cdef c_to_python_costsource(defs.CostSource cost_source): - c = CostSource() - c.aabb_min = vec3f_to_list(cost_source.aabb_min) - c.aabb_max = vec3f_to_list(cost_source.aabb_max) - c.cost_density = cost_source.cost_density - c.total_cost = cost_source.total_cost - return c - -def collide(CollisionObject o1, CollisionObject o2, request): - cdef defs.CollisionResult result - cdef size_t ret = defs.collide(o1.thisptr, - o2.thisptr, - defs.CollisionRequest(request.num_max_contacts, - request.enable_contact, - request.num_max_cost_sources, - request.enable_cost, - request.use_approximate_cost), - result) - col_res = CollisionResult() - cdef vector[defs.Contact] contacts - result.getContacts(contacts) - cdef vector[defs.CostSource] costs - result.getCostSources(costs) - for idx in range(contacts.size()): - col_res.contacts.append(c_to_python_contact(contacts[idx])) - for idx in range(costs.size()): - col_res.cost_sources.append(c_to_python_costsource(costs[idx])) - return ret, col_res - -def distance(CollisionObject o1, CollisionObject o2, request): - cdef defs.DistanceResult result - cdef double dis = defs.distance(o1.thisptr, - o2.thisptr, - defs.DistanceRequest(request.enable_nearest_points), - result) - dis_res = DistanceResult() - dis_res.min_distance = result.min_distance - dis_res.nearest_points = [vec3f_to_tuple(result.nearest_points[0]), - vec3f_to_tuple(result.nearest_points[1])] - dis_res.o1 = c_to_python_collision_geometry(result.o1) - dis_res.o2 = c_to_python_collision_geometry(result.o2) - dis_res.b1 = result.b1 - dis_res.b2 = result.b2 - return dis, dis_res diff --git a/fcl/fcl_defs.pxd b/fcl/fcl_defs.pxd deleted file mode 100644 index 0984491..0000000 --- a/fcl/fcl_defs.pxd +++ /dev/null @@ -1,230 +0,0 @@ -from libcpp cimport bool -from libcpp.string cimport string -from libcpp.vector cimport vector -from libcpp.set cimport set - -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_) - -cdef extern from "fcl/collision_data.h" namespace "fcl": - 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 + - void getContacts(vector[Contact]& contacts_) - void getCostSources(vector[CostSource]& cost_sources_) - cdef cppclass CollisionRequest: - size_t num_max_contacts - bool enable_contact - size_t num_max_cost_sources - bool enable_cost - bool use_approximate_cost - CollisionRequest(size_t num_max_contacts_, - bool enable_contact_, - size_t num_max_cost_sources_, - bool enable_cost_, - bool use_approximate_cost_) - 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 - DistanceRequest(bool enable_nearest_points_) 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_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() - CollisionGeometry* getCollisionGeometry() - void setTranslation(Vec3f& T) - void setQuatRotation(Quaternion3f& q) - void setTransform(Quaternion3f& q, Vec3f& T) - 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 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(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/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) diff --git a/fcl/interval.py b/fcl/interval.py deleted file mode 100644 index 1a18964..0000000 --- a/fcl/interval.py +++ /dev/null @@ -1,213 +0,0 @@ -import numpy as np - -class Interval: - def __init__(self, left=None, right=None): - if left is None: - self.i = np.zeros(2) - elif getattr(left, '__iter__', False): - self.i = np.array(left[:2]) - else: - self.i = np.zeros(2) - self.i[0] = left - if right is None: - self.i[1] = left - else: - self.i[1] = right - self.i.sort() - - def setValue(self, a, b=None): - if getattr(left, '__iter__', False): - self.i = np.array(a[:2]) - else: - self.i[0] = a - if b is None: - self.i[1] = a - else: - self.i[1] = b - self.i.sort() - - def __getitem__(self, idx): - return self.i[idx] - - def __setitem__(self, idx, value): - self.i[idx] = value - - def __iter__(self): - for i in self.i: - yield i - - def __eq__(self, other): - return np.array_equal(other) - - def __add__(self, other): - if isinstance(other, Interval): - return Interval(self.i + other.i) - else: - return Interval(self.i[0] + other, self.i[1] + other) - - __radd__ = __add__ - __iadd__ = __add__ - - def __sub__(self, other): - if isinstance(other, Interval): - return Interval(self.i[0] - other.i[1], - self.i[1] - other.i[0]) - else: - return Interval(self.i[0] - other, self.i[1] - other) - - def __rsub__(self, other): - return Interval(other - self.i[0], other - self.i[1]) - - __isub__ = __sub__ - - def __mul__(self, other): - if isinstance(other, Interval): - if other.i[0] >= 0: - if self.i[0] >= 0: - return Interval(self.i[0] * other.i[0], self.i[1] * other.i[1]) - if self.i[1] <= 0: - return Interval(self.i[0] * other.i[1], self.i[1] * other.i[0]) - return Interval(self.i[0] * other.i[1], self.i[1] * other.i[1]) - if other.i[1] <= 0: - if self.i[0] >= 0: - return Interval(self.i[1] * other.i[0], self.i[0] * other.i[1]) - if self.i[1] <= 0: - return Interval(self.i[1] * other.i[1], self.i[0] * other.i[0]) - return Interval(self.i[1] * other.i[0], self.i[0] * other.i[0]) - - if self.i[0] >= 0: - return Interval(self.i[1] * other.i[0], self.i[1] * other.i[1]) - - if self.i[1] <= 0: - return Interval(self.i[0] * other.i[1], self.i[0] * other.i[0]) - - v00 = self.i[0] * other.i[0] - v11 = self.i[1] * other.i[1] - if v00 <= v11: - v01 = self.i[0] * other.i[1] - v10 = self.i[1] * other.i[0] - if v01 < v10: - return Interval(v01, v11) - return Interval(v10, v11) - - v01 = self.i[0] * other.i[1] - v10 = self.i[1] * other.i[0] - if v01 < v10: - return Interval(v01, v00) - return Interval(v10, v00) - - else: - if other >= 0: - return Interval(self.i * other) - else: - return Interval(self.i * other) - - __rmul__ = __mul__ - - __imul__ = __mul__ - - def __div__(self, other): - if isinstance(other, Interval): - return self * Interval(1.0 / other.i[1], 1.0 / other.i[0]) - else: - return self * (1.0 / other) - - def __rdiv__(self, other): - return Interval(other / self.i[1], other / self.i[0]) - - __idiv__ = __div__ - - def __pow__(self, other): - ans = Interval(self) - for _ in range(other - 1): - ans *= self - return ans - - def overlap(self, other): - if self.i[1] < other.i[0]: - return False - if self.i[0] > other.i[1]: - return False - return True - - def intersect(self, other): - if self.i[1] < other.i[0]: - return False - if self.i[0] > other.i[1]: - return False - if self.i[1] > other.i[1]: - self.i[1] = other.i[1] - if self.i[0] < other.i[0]: - self.i[0] = other.i[0] - return True - - def __neg__(self): - return Interval(-self.i[1], -self.i[0]) - - def getAbsLower(self): - if self.i[0] >= 0: - return self.i[0] - if self.i[1] >= 0: - return 0 - return -self.i[1] - - def getAbsUpper(self): - if self.i.sum() >= 0: - return self.i[1] - return self.i[0] - - def contains(self, v): - if v < self.i[0]: - return False - if v > self.i[1]: - return False - return True - - def bound(self, other): - if isinstance(other, Interval): - if other.i[0] < self.i[0]: - self.i[0] = other.i[0] - if other.i[1] > self.i[1]: - self.i[1] = other.i[1] - return self - else: - if other < self.i[0]: - self.i[0] = other - if other > self.i[1]: - self.i[1] = other - return self - - def __str__(self): - return str(self.i) - - def center(self): - return self.i.mean() - - def diameter(self): - return self.i[1] - self.i[0] - -def bound(i, other): - res = Interval(i) - if isinstance(other, Interval): - if other.i[0] < res.i[0]: - res.i[0] = other.i[0] - if other.i[1] > res.i[1]: - res.i[1] = other.i[1] - return res - else: - if other < res.i[0]: - res.i[0] = other - if other > res.i[1]: - res.i[1] = other - return res - -class TimeInterval: - def __init__(self, l, r): - self.setValue(l, r) - def setValue(self, l, r): - self.t = Interval(l, r) - self.t2 = Interval(l * self.t[0], r * self.t[1]) - self.t3 = Interval(l * self.t2[0], r * self.t2[1]) - self.t4 = Interval(l * self.t3[0], r * self.t3[1]) - self.t5 = Interval(l * self.t4[0], r * self.t4[1]) - self.t6 = Interval(l * self.t5[0], r * self.t5[1]) diff --git a/fcl/transform.py b/fcl/transform.py deleted file mode 100644 index 3a1972e..0000000 --- a/fcl/transform.py +++ /dev/null @@ -1,173 +0,0 @@ -import numpy as np - -def rotation_to_quaternion(rot): - next_idx = (1, 2, 0) - data = np.zeros(4) - trace = np.trace(rot) - if trace > 0.0: - root = np.sqrt(trace + 1.0) - data[0] = 0.5 * root - root = 0.5 / root - data[1] = (rot[2, 1] - rot[1, 2]) * root - data[2] = (rot[0, 2] - rot[2, 0]) * root - data[3] = (rot[1, 0] - rot[0, 1]) * root - else: - i = 0 - if rot[1, 1] > rot[0, 0]: - i = 1 - if rot[2, 2] > rot[i, i]: - i = 2 - j = next_idx[i] - k = next_idx[j] - - root = np.sqrt(rot[i, i] - rot[j, j] - rot[k, k] + 1.0) - data[i + 1] = 0.5 * root - root = 0.5 / root - data[0] = (rot[k, j] - rot[j, k]) * root - data[j + 1] = (rot[j, i] + rot[i, j]) * root - data[k + 1] = (rot[k, i] + rot[i, k]) * root - return Quaternion(data) - -def quaternion_to_rotation(quat): - data = quat._data - twoX = 2.0 * data[1] - twoY = 2.0 * data[2] - twoZ = 2.0 * data[3] - twoWX = twoX * data[0] - twoWY = twoY * data[0] - twoWZ = twoZ * data[0] - twoXX = twoX * data[1] - twoXY = twoY * data[1] - twoXZ = twoZ * data[1] - twoYY = twoY * data[2] - twoYZ = twoZ * data[2] - twoZZ = twoZ * data[3] - - return np.array([[1.0 - (twoYY + twoZZ), twoXY - twoWZ, twoXZ + twoWY], - [twoXY + twoWZ, 1.0 - (twoXX + twoZZ), twoYZ - twoWX], - [twoXZ - twoWY, twoYZ + twoWX, 1.0 - (twoXX + twoYY)]]) - -def axisangle_to_quaternion(axis, angle): - half_angle = 0.5 * angle - sn = np.sin(half_angle) - data = np.zeros(4) - data[0] = np.cos(half_angle) - data[1:] = sn * axis - return Quaternion(data) - -def quaternion_to_axisangle(quat): - data = quat._data - sqr_length = sum(np.square(data[1:])) - axis = np.zeros(3) - if sqr_length > 0: - angle = 2.0 * np.acos(data[0]) - inv_length = 1.0 / np.sqrt(sqr_length) - axis = inv_length * data[1:] - else: - angle = 0 - axis[0] = 1 - axis[1] = 0 - axis[2] = 0 - return axis, angle - -class Quaternion(object): - def __init__(self, *args): - if len(args) == 0: - self._data = np.zeros(4) - self._data[0] = 1.0 - elif len(args) == 4: - self._data = np.array(args) - elif len(args) == 1 and len(args[0]) == 4: - self._data = np.array(args[0]) - - @property - def w(self): - return self._data[0] - @w.setter - def w(self, value): - self._data[0] = value - - @property - def x(self): - return self._data[1] - @x.setter - def x(self, value): - self._data[1] = value - - @property - def y(self): - return self._data[2] - @y.setter - def y(self, value): - self._data[2] = value - - @property - def z(self): - return self._data[3] - @z.setter - def z(self, value): - self._data[3] = value - - @property - def v(self): - return self._data[1:] - @v.setter - def v(self, value): - self._data[1:] = value - - def __getitem__(self, idx): - return self._data[idx] - def __setitem__(self, idx, value): - self._data[idx] = value - - def isIdentity(self): - return self._data[0] == 1 and \ - all((d == 0 for d in self._data[1:])) - def __add__(self, other): - return Quaternion(self._data + other._data) - def __sub__(self, other): - return Quaternion(self._data - other._data) - def __neg__(self): - return Quaternion(-self._data) - def __mul__(self, other): - v = self.w * other.v + other.w * self.v + np.cross(self.v, other.v) - return Quaternion(self.w * other.w - np.dot(self.v, other.v), - v[0], v[1], v[2]) - def dot(self, other): - return Quaternion(np.dot(self._data, other._data)) - - def conj(self): - return Quaternion(self.w, -self.x, -self.y, -self.z) - - def __str__(self): - return str(self._data) - - def inverse(self): - data = np.zeros(4) - sqr_length = sum(np.square(data)) - if sqr_length > 0: - inv_length = 1 / np.sqrt(sqr_length); - data[0] *= inv_length - data[1:] *= (-inv_length) - else: - data[1:] *= (-1) - return Quaternion(data) - - def transform(vec): - r = self * Quaternion(0.0, vec[0], vec[1], vec[2]) * self.conj() - return r.v - - -class Transform: - def __init__(self, rot=None, pos=None): - if not pos is None: - self.t = np.array(pos) - else: - self.t = np.zeros(3) - - if rot is None: - self.q = Quaternion() - elif isinstance(rot ,Quaternion): - self.q = rot - else: - self.q = rotation_to_quaternion(rot) diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..30750c6 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,113 @@ +[build-system] +requires = [ + "setuptools>=70.0.0", + "Cython>=3.0", + "numpy; python_version<'3.12'", + "numpy>=1.26.0b1; python_version>='3.12'"] +build-backend = "setuptools.build_meta" + +[project] +name = "python-fcl" +description = "Python bindings for the Flexible Collision Library" +requires-python = ">=3.9" +authors = [ + {name = "Jelle Feringa", email = "jelleferinga@gmail.com"}, + {name = "Matthew Matl", email = "mmatl@eecs.berkeley.edu"}, + {name = "Shirokuma", email = "rootstock_acg@yahoo.co.jp"}, + {name = "Michael Dawson-Haggerty"}, + {name = "See contributor list"}, +] +maintainers = [{name = "Matthew Matl", email = "mmatl@eecs.berkeley.edu"}] +license = {text = "BSD"} +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.7", + "Programming Language :: Python :: 3.8", + "Programming Language :: Python :: 3.9", + "Programming Language :: Python :: 3.10", + "Programming Language :: Python :: 3.11", + "Programming Language :: Python :: 3.12", + "Programming Language :: Python :: Implementation :: CPython", +] +keywords = ["fcl collision distance"] +dependencies = [ + "numpy; python_version<'3.12'", + "numpy>=1.26.0b1; python_version>='3.12'", + "Cython", +] +dynamic = ["version"] + +[project.readme] +file = "README.md" +content-type = "text/markdown" + +[project.urls] +Homepage = "https://github.com/berkeleyautomation/python-fcl" + + + + +[tool.setuptools] +include-package-data = true +package-dir = {"" = "src"} +license-files = ["LICENSE"] + +[tool.setuptools.package-data] +"*" = ["*.pyx", "*.pxd", "*.dll"] + +[tool.setuptools.packages.find] +where = ["src"] +namespaces = false + +[tool.setuptools.dynamic] +version = {attr = "fcl.__version__"} + +[tool.ruff] +target-version = "py37" +line-length = 90 + +[tool.ruff.lint] +# See https://github.com/charliermarsh/ruff#rules for error code definitions. +select = [ + "B", # bugbear + "C", # comprehensions + "E", # style errors + "F", # flakes + "I", # import sorting + "RUF100", # meta + "U", # upgrade + "W", # style warnings + "YTT", # sys.version +] + +ignore = [ + "C901", # Comprehension is too complex (11 > 10) + "N802", # Function name should be lowercase + "N806", # Variable in function should be lowercase + "E501", # Line too long ({width} > {limit} characters) + "B904", # raise ... from err + "B905", # zip() without an explicit strict= parameter +] + + +[tool.cibuildwheel] +skip = ["*musllinux*"] +test-requires = "pytest" +test-command = "pytest {package}/tests" + +[tool.cibuildwheel.linux] +before-all = "bash build_dependencies/install_linux.sh" +repair-wheel-command = "LD_LIBRARY_PATH=/usr/local/lib64 auditwheel repair -w {dest_dir} {wheel}" + +[tool.cibuildwheel.macos] +before-all = "bash build_dependencies/install_macos.sh" +repair-wheel-command = "delocate-wheel -w {dest_dir} -v {wheel}" + +[tool.cibuildwheel.windows] +before-all = "powershell build_dependencies\\install_windows.ps1" +archs = ["AMD64"] diff --git a/setup.py b/setup.py index 0dca873..9ebe01f 100644 --- a/setup.py +++ b/setup.py @@ -1,45 +1,78 @@ import os import sys -from distutils.core import Extension, setup -from Cython.Distutils import build_ext -platform_supported = False -for prefix in ['darwin', 'linux', 'bsd']: - if prefix in sys.platform: - platform_supported = True +from Cython.Build import cythonize +from setuptools import Extension, setup + +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", + "/usr/local/include", + "/usr/include/eigen3", + "/usr/local/include/eigen3", ] - lib_dirs = [ - '/usr/lib', - '/usr/local/lib', + + 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", ] - 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(':') - break + else: + raise NotImplementedError(sys.platform) -if sys.platform == "win32": - platform_supported = False + # 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", "/usr/lib64", "/usr/local/lib64"] + + 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"] -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="fcl", - version="0.1", - license = "BSD", - packages=["fcl"], - ext_modules=[Extension( - "fcl.fcl", - ["fcl/fcl.pyx"], - include_dirs = include_dirs, - library_dirs = lib_dirs, - libraries=[ - "fcl" - ], - language="c++")], - cmdclass={'build_ext': build_ext}, + 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..6fa2688 --- /dev/null +++ b/src/fcl/__init__.py @@ -0,0 +1,88 @@ +try: + from .fcl import ( + Box, + BVHModel, + Capsule, + CollisionGeometry, + CollisionObject, + Cone, + Convex, + Cylinder, + DynamicAABBTreeCollisionManager, + Ellipsoid, + Halfspace, + OcTree, + Plane, + Sphere, + Transform, + TriangleP, + collide, + continuousCollide, + defaultCollisionCallback, + defaultDistanceCallback, + distance, + ) +except ModuleNotFoundError: + import traceback + + traceback.print_exc() + print("Failed to import fcl.fcl. It is probably not correctly compiled.") + +from .collision_data import ( + NODE_TYPE, + OBJECT_TYPE, + CCDMotionType, + CCDSolverType, + CollisionData, + CollisionRequest, + CollisionResult, + Contact, + ContinuousCollisionRequest, + ContinuousCollisionResult, + CostSource, + DistanceData, + DistanceRequest, + DistanceResult, + GJKSolverType, +) +from .version import __version__ + +__all__ = [ + "CollisionObject", + "CollisionGeometry", + "Transform", + "TriangleP", + "Box", + "Sphere", + "Ellipsoid", + "Capsule", + "Cone", + "Convex", + "Cylinder", + "Halfspace", + "Plane", + "BVHModel", + "OcTree", + "DynamicAABBTreeCollisionManager", + "collide", + "continuousCollide", + "distance", + "defaultCollisionCallback", + "defaultDistanceCallback", + "OBJECT_TYPE", + "NODE_TYPE", + "CCDMotionType", + "CCDSolverType", + "GJKSolverType", + "Contact", + "CostSource", + "CollisionRequest", + "CollisionResult", + "ContinuousCollisionRequest", + "ContinuousCollisionResult", + "DistanceRequest", + "DistanceResult", + "CollisionData", + "DistanceData", + "__version__", +] diff --git a/src/fcl/collision_data.py b/src/fcl/collision_data.py new file mode 100644 index 0000000..0297173 --- /dev/null +++ b/src/fcl/collision_data.py @@ -0,0 +1,160 @@ +import sys + +import numpy as np + + +class OBJECT_TYPE: + OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE, OT_COUNT = range(5) + + +class 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, + ) = range(21) + + +class CCDMotionType: + CCDM_TRANS, CCDM_LINEAR, CCDM_SCREW, CCDM_SPLINE = range(4) + + +class CCDSolverType: + ( + CCDC_NAIVE, + CCDC_CONSERVATIVE_ADVANCEMENT, + CCDC_RAY_SHOOTING, + CCDC_POLYNOMIAL_SOLVER, + ) = range(4) + + +class GJKSolverType: + GST_LIBCCD, GST_INDEP = range(2) + + +class Contact: + def __init__(self): + self.o1 = None + self.o2 = None + self.b1 = 0 + self.b2 = 0 + self.normal = np.array([0.0, 0.0, 0.0]) + self.pos = np.array([0.0, 0.0, 0.0]) + self.penetration_depth = 0.0 + + +class CostSource: + def __init__(self): + self.aabb_min = np.array([0.0, 0.0, 0.0]) + self.aabb_max = np.array([0.0, 0.0, 0.0]) + self.cost_density = 0.0 + self.total_cost = 0.0 + + +class CollisionRequest: + def __init__( + self, + num_max_contacts=1, + enable_contact=False, + num_max_cost_sources=1, + enable_cost=False, + use_approximate_cost=True, + gjk_solver_type=GJKSolverType.GST_LIBCCD, + ): + self.num_max_contacts = num_max_contacts + self.enable_contact = enable_contact + self.num_max_cost_sources = num_max_cost_sources + self.enable_cost = enable_cost + self.use_approximate_cost = use_approximate_cost + self.gjk_solver_type = gjk_solver_type + + +class CollisionResult: + def __init__(self, is_collision=False): + self.is_collision = False + self.contacts = [] + self.cost_sources = [] + + +class ContinuousCollisionRequest: + def __init__( + self, + num_max_iterations=10, + toc_err=0.0001, + ccd_motion_type=CCDMotionType.CCDM_TRANS, + gjk_solver_type=GJKSolverType.GST_LIBCCD, + ccd_solver_type=CCDSolverType.CCDC_CONSERVATIVE_ADVANCEMENT, + ): + self.num_max_iterations = num_max_iterations + self.toc_err = toc_err + self.ccd_motion_type = ccd_motion_type + self.gjk_solver_type = gjk_solver_type + self.ccd_solver_type = ccd_solver_type + + +class ContinuousCollisionResult: + def __init__(self, is_collide=False, time_of_contact=1.0): + self.is_collide = is_collide + self.time_of_contact = time_of_contact + + +class DistanceRequest: + def __init__( + self, + enable_nearest_points=False, + enable_signed_distance=False, + gjk_solver_type=GJKSolverType.GST_LIBCCD, + ): + self.enable_nearest_points = enable_nearest_points + self.enable_signed_distance = enable_signed_distance + self.gjk_solver_type = gjk_solver_type + + +class DistanceResult: + def __init__(self, min_distance_=sys.float_info.max): + self.min_distance = min_distance_ + self.nearest_points = [None, None] + self.o1 = None + self.o2 = None + self.b1 = -1 + self.b2 = -1 + + +class CollisionData: + def __init__(self, request=None, result=None): + if request is None: + request = CollisionRequest() + if result is None: + result = CollisionResult() + self.request = request + self.result = result + self.done = False + + +class DistanceData: + def __init__(self, request=None, result=None): + if request is None: + request = DistanceRequest() + if result is None: + result = DistanceResult() + self.request = request + self.result = result + self.done = False diff --git a/src/fcl/fcl.pyx b/src/fcl/fcl.pyx new file mode 100644 index 0000000..169bbb2 --- /dev/null +++ b/src/fcl/fcl.pyx @@ -0,0 +1,830 @@ +# cython: language_level=2 +from libc.stdlib cimport free +from libc.string cimport memcpy +from libcpp cimport bool +from libcpp.string cimport string +from libcpp.vector cimport vector + +import inspect + +cimport numpy as np +from cython.operator cimport address +from cython.operator cimport dereference as deref +from cython.operator cimport preincrement as inc + +import numpy + +ctypedef np.float64_t DOUBLE_t + +cimport fcl_defs as defs +cimport octomap_defs as octomap +cimport std_defs as std + +from collision_data import ( + CollisionRequest, + CollisionResult, + Contact, + ContinuousCollisionRequest, + ContinuousCollisionResult, + CostSource, + 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.Transform3d *thisptr + + def __cinit__(self, *args): + if len(args) == 0: + self.thisptr = new defs.Transform3d() + self.thisptr.setIdentity() + elif len(args) == 1: + if isinstance(args[0], Transform): + self.thisptr = new defs.Transform3d(deref(( args[0]).thisptr)) + else: + data = numpy.array(args[0]) + if data.shape == (3,3): + self.thisptr = new defs.Transform3d() + self.thisptr.setIdentity() + ASSIGN(self.thisptr.linear(), + numpy_to_mat3d(data)) + elif data.shape == (4,): + 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.Transform3d() + self.thisptr.setIdentity() + ASSIGN(self.thisptr.translation(), + numpy_to_vec3d(data)) + else: + raise ValueError('Invalid input to Transform().') + elif len(args) == 2: + rot = numpy.array(args[0]) + trans = numpy.array(args[1]).squeeze() + if not trans.shape == (3,): + raise ValueError('Translation must be (3,).') + + if rot.shape == (3,3): + 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.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: + raise ValueError('Too many arguments to Transform().') + + def __dealloc__(self): + if self.thisptr: + free(self.thisptr) + + def getRotation(self): + return mat3d_to_numpy(self.thisptr.linear()) + + def getTranslation(self): + return vec3d_to_numpy(self.thisptr.translation()) + + def getQuatRotation(self): + cdef defs.Quaterniond quaternion = defs.Quaterniond(self.thisptr.linear()) + return quaternion3d_to_numpy(quaternion) + + def setRotation(self, R): + ASSIGN(self.thisptr.linear(), + numpy_to_mat3d(R)) + + def setTranslation(self, T): + ASSIGN(self.thisptr.translation(), + numpy_to_vec3d(T)) + + def setQuatRotation(self, q): + ASSIGN(self.thisptr.linear(), + numpy_to_quaternion3d(q).toRotationMatrix()) + +############################################################################### +# Collision objects and geometries +############################################################################### + +cdef class CollisionObject: + cdef defs.CollisionObjectd *thisptr + cdef defs.PyObject *geom + cdef bool _no_instance + + def __cinit__(self, CollisionGeometry geom=None, Transform tf=None, _no_instance=False): + if geom is None: + geom = CollisionGeometry() + defs.Py_INCREF( geom) + self.geom = geom + 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.CollisionObjectd(defs.shared_ptr[defs.CollisionGeometryd](geom.thisptr), deref(tf.thisptr)) + else: + 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: + raise ValueError + + def __dealloc__(self): + if self.thisptr and not self._no_instance: + free(self.thisptr) + defs.Py_DECREF(self.geom) + + def getObjectType(self): + return self.thisptr.getObjectType() + + def getNodeType(self): + return self.thisptr.getNodeType() + + def getTranslation(self): + return vec3d_to_numpy(self.thisptr.getTranslation()) + + def setTranslation(self, vec): + self.thisptr.setTranslation(numpy_to_vec3d(vec)) + self.thisptr.computeAABB() + + def getRotation(self): + return mat3d_to_numpy(self.thisptr.getRotation()) + + def setRotation(self, mat): + self.thisptr.setRotation(numpy_to_mat3d(mat)) + self.thisptr.computeAABB() + + def getQuatRotation(self): + return quaternion3d_to_numpy(self.thisptr.getQuatRotation()) + + def setQuatRotation(self, q): + self.thisptr.setQuatRotation(numpy_to_quaternion3d(q)) + self.thisptr.computeAABB() + + def getTransform(self): + rot = self.getRotation() + trans = self.getTranslation() + return Transform(rot, trans) + + def setTransform(self, tf): + self.thisptr.setTransform(deref(( tf).thisptr)) + self.thisptr.computeAABB() + + def isOccupied(self): + return self.thisptr.isOccupied() + + def isFree(self): + return self.thisptr.isFree() + + def isUncertain(self): + return self.thisptr.isUncertain() + +cdef class CollisionGeometry: + cdef defs.CollisionGeometryd *thisptr + + def __cinit__(self): + pass + + def __dealloc__(self): + if self.thisptr: + del self.thisptr + + def getNodeType(self): + if self.thisptr: + return self.thisptr.getNodeType() + else: + return None + + def computeLocalAABB(self): + if self.thisptr: + self.thisptr.computeLocalAABB() + else: + return None + + property aabb_center: + def __get__(self): + if self.thisptr: + return vec3d_to_numpy(self.thisptr.aabb_center) + else: + return None + def __set__(self, value): + if self.thisptr: + self.thisptr.aabb_center[0] = value[0] + self.thisptr.aabb_center[1] = value[1] + self.thisptr.aabb_center[2] = value[2] + else: + raise ReferenceError + +cdef class TriangleP(CollisionGeometry): + def __cinit__(self, a, b, c): + self.thisptr = new defs.TrianglePd(numpy_to_vec3d(a), numpy_to_vec3d(b), numpy_to_vec3d(c)) + + property a: + def __get__(self): + 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] + + property b: + def __get__(self): + 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] + + property c: + def __get__(self): + 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] + +cdef class Box(CollisionGeometry): + def __cinit__(self, x, y, z): + self.thisptr = new defs.Boxd(x, y, z) + + property side: + def __get__(self): + 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] + +cdef class Sphere(CollisionGeometry): + def __cinit__(self, radius): + self.thisptr = new defs.Sphered(radius) + + property radius: + def __get__(self): + return ( self.thisptr).radius + def __set__(self, value): + ( self.thisptr).radius = value + +cdef class Ellipsoid(CollisionGeometry): + def __cinit__(self, a, b, c): + self.thisptr = new defs.Ellipsoidd( a, b, c) + + property radii: + def __get__(self): + return vec3d_to_numpy(( self.thisptr).radii) + def __set__(self, values): + ( self.thisptr).radii = numpy_to_vec3d(values) + +cdef class Capsule(CollisionGeometry): + def __cinit__(self, radius, lz): + self.thisptr = new defs.Capsuled(radius, lz) + + property radius: + def __get__(self): + return ( self.thisptr).radius + def __set__(self, value): + ( self.thisptr).radius = value + + property lz: + def __get__(self): + return ( self.thisptr).lz + def __set__(self, value): + ( self.thisptr).lz = value + +cdef class Cone(CollisionGeometry): + def __cinit__(self, radius, lz): + self.thisptr = new defs.Coned(radius, lz) + + property radius: + def __get__(self): + return ( self.thisptr).radius + def __set__(self, value): + ( self.thisptr).radius = value + + property lz: + def __get__(self): + return ( self.thisptr).lz + def __set__(self, value): + ( self.thisptr).lz = value + +cdef class Convex(CollisionGeometry): + def __cinit__(self, vertices, num_faces, faces): + cdef vector[defs.Vector3d] vs + cdef vector[int] fs + for vert in vertices: + vs.push_back(numpy_to_vec3d(vert)) + for face in faces: + fs.push_back(face) + self.thisptr = new defs.Convexd(defs.make_shared[vector[defs.Vector3d]](vs), num_faces, defs.make_shared[vector[int]](fs)) + + property num_faces: + def __get__(self): + return ( self.thisptr).getFaceCount() + +cdef class Cylinder(CollisionGeometry): + def __cinit__(self, radius, lz): + self.thisptr = new defs.Cylinderd(radius, lz) + + property radius: + def __get__(self): + return ( self.thisptr).radius + def __set__(self, value): + ( self.thisptr).radius = value + + property lz: + def __get__(self): + return ( self.thisptr).lz + def __set__(self, value): + ( self.thisptr).lz = value + +cdef class Halfspace(CollisionGeometry): + 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 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] + + property d: + def __get__(self): + return ( self.thisptr).d + def __set__(self, value): + ( self.thisptr).d = value + +cdef class Plane(CollisionGeometry): + 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 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] + + property d: + def __get__(self): + return ( self.thisptr).d + def __set__(self, value): + ( self.thisptr).d = value + +cdef class BVHModel(CollisionGeometry): + def __cinit__(self): + self.thisptr = new defs.BVHModel() + + def num_tries_(self): + return ( self.thisptr).num_tris + + def buildState(self): + return ( self.thisptr).build_state + + def beginModel(self, num_tris_=0, num_vertices_=0): + n = ( self.thisptr).beginModel( num_tris_, num_vertices_) + return n + + def endModel(self): + n = ( self.thisptr).endModel() + return n + + def addVertex(self, 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_vec3d(v1), + numpy_to_vec3d(v2), + numpy_to_vec3d(v3)) + return self._check_ret_value(n) + + def addSubModel(self, verts, triangles): + cdef vector[defs.Vector3d] ps + cdef vector[defs.Triangle] tris + for vert in verts: + 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) + return self._check_ret_value(n) + + def _check_ret_value(self, n): + if n == defs.BVH_OK: + return True + elif n == defs.BVH_ERR_MODEL_OUT_OF_MEMORY: + raise MemoryError("Cannot allocate memory for vertices and triangles") + elif n == defs.BVH_ERR_BUILD_OUT_OF_SEQUENCE: + raise ValueError("BVH construction does not follow correct sequence") + elif n == defs.BVH_ERR_BUILD_EMPTY_MODEL: + raise ValueError("BVH geometry is not prepared") + elif n == defs.BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME: + raise ValueError("BVH geometry in previous frame is not prepared") + elif n == defs.BVH_ERR_UNSUPPORTED_FUNCTION: + raise ValueError("BVH funtion is not supported") + elif n == defs.BVH_ERR_UNUPDATED_MODEL: + raise ValueError("BVH model update failed") + elif n == defs.BVH_ERR_INCORRECT_DATA: + raise ValueError("BVH data is not valid") + elif n == defs.BVH_ERR_UNKNOWN: + raise ValueError("Unknown failure") + else: + return False + +cdef class OcTree(CollisionGeometry): + cdef octomap.OcTree* tree + + def __cinit__(self, r, data): + cdef std.stringstream ss + cdef vector[char] vd = data + ss.write(vd.data(), len(data)) + + self.tree = new octomap.OcTree(r) + self.tree.readBinaryData(ss) + self.thisptr = new defs.OcTreed(defs.shared_ptr[octomap.OcTree](self.tree)) + + +############################################################################### +# Collision managers +############################################################################### + +cdef class DynamicAABBTreeCollisionManager: + cdef defs.DynamicAABBTreeCollisionManagerd *thisptr + cdef list objs + + def __cinit__(self): + self.thisptr = new defs.DynamicAABBTreeCollisionManagerd() + self.objs = [] + + def __dealloc__(self): + if self.thisptr: + del self.thisptr + + def registerObjects(self, other_objs): + cdef vector[defs.CollisionObjectd*] pobjs + for obj in other_objs: + self.objs.append(obj) + pobjs.push_back(( obj).thisptr) + self.thisptr.registerObjects(pobjs) + + def registerObject(self, obj): + self.objs.append(obj) + self.thisptr.registerObject(( obj).thisptr) + + def unregisterObject(self, obj): + if obj in self.objs: + self.objs.remove(obj) + self.thisptr.unregisterObject(( obj).thisptr) + + def setup(self): + self.thisptr.setup() + + def update(self, arg=None): + cdef vector[defs.CollisionObjectd*] objs + if hasattr(arg, "__len__"): + for a in arg: + objs.push_back(( a).thisptr) + self.thisptr.update(objs) + elif arg is None: + self.thisptr.update() + else: + self.thisptr.update(( arg).thisptr) + + def getObjects(self): + return list(self.objs) + + def collide(self, *args): + if len(args) == 2 and inspect.isroutine(args[1]): + fn = CollisionFunction(args[1], args[0]) + self.thisptr.collide( fn, CollisionCallBack) + elif len(args) == 3 and isinstance(args[0], DynamicAABBTreeCollisionManager): + fn = CollisionFunction(args[2], args[1]) + self.thisptr.collide(( args[0]).thisptr, fn, CollisionCallBack) + elif len(args) == 3 and inspect.isroutine(args[2]): + fn = CollisionFunction(args[2], args[1]) + self.thisptr.collide(( args[0]).thisptr, fn, CollisionCallBack) + else: + raise ValueError + + def distance(self, *args): + if len(args) == 2 and inspect.isroutine(args[1]): + fn = DistanceFunction(args[1], args[0]) + self.thisptr.distance( fn, DistanceCallBack) + elif len(args) == 3 and isinstance(args[0], DynamicAABBTreeCollisionManager): + fn = DistanceFunction(args[2], args[1]) + self.thisptr.distance(( args[0]).thisptr, fn, DistanceCallBack) + elif len(args) == 3 and inspect.isroutine(args[2]): + fn = DistanceFunction(args[2], args[1]) + self.thisptr.distance(( args[0]).thisptr, fn, DistanceCallBack) + else: + raise ValueError + + def clear(self): + self.thisptr.clear() + + def empty(self): + return self.thisptr.empty() + + def size(self): + return self.thisptr.size() + + property max_tree_nonbalanced_level: + def __get__(self): + return self.thisptr.max_tree_nonbalanced_level + def __set__(self, value): + self.thisptr.max_tree_nonbalanced_level = value + + property tree_incremental_balance_pass: + def __get__(self): + return self.thisptr.tree_incremental_balance_pass + def __set__(self, value): + self.thisptr.tree_incremental_balance_pass = value + + property tree_topdown_balance_threshold: + def __get__(self): + return self.thisptr.tree_topdown_balance_threshold + def __set__(self, value): + self.thisptr.tree_topdown_balance_threshold = value + + property tree_topdown_level: + def __get__(self): + return self.thisptr.tree_topdown_level + def __set__(self, value): + self.thisptr.tree_topdown_level = value + + property tree_init_level: + def __get__(self): + return self.thisptr.tree_init_level + def __set__(self, value): + self.thisptr.tree_init_level = value + + property octree_as_geometry_collide: + def __get__(self): + return self.thisptr.octree_as_geometry_collide + def __set__(self, value): + self.thisptr.octree_as_geometry_collide = value + + property octree_as_geometry_distance: + def __get__(self): + return self.thisptr.octree_as_geometry_distance + def __set__(self, value): + self.thisptr.octree_as_geometry_distance = value + +############################################################################### +# Collision and distance functions +############################################################################### + +def collide(CollisionObject o1, CollisionObject o2, + request=None, result=None): + + if request is None: + request = CollisionRequest() + if result is None: + result = CollisionResult() + + cdef defs.CollisionResultd cresult + + cdef size_t ret = defs.collide(o1.thisptr, o2.thisptr, + defs.CollisionRequestd( + request.num_max_contacts, + request.enable_contact, + request.num_max_cost_sources, + request.enable_cost, + request.use_approximate_cost, + request.gjk_solver_type + ), + cresult) + + result.is_collision = result.is_collision or cresult.isCollision() + + 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.CostSourced] costs + cresult.getCostSources(costs) + for idx in range(costs.size()): + result.cost_sources.append(c_to_python_costsource(costs[idx])) + + return ret + +def continuousCollide(CollisionObject o1, Transform tf1_end, + CollisionObject o2, Transform tf2_end, + request = None, result = None): + + if request is None: + request = ContinuousCollisionRequest() + if result is None: + result = ContinuousCollisionResult() + + cdef defs.ContinuousCollisionResultd cresult + + cdef double ret = defs.continuousCollide(o1.thisptr, deref(tf1_end.thisptr), + o2.thisptr, deref(tf2_end.thisptr), + defs.ContinuousCollisionRequestd( + request.num_max_iterations, + request.toc_err, + request.ccd_motion_type, + request.gjk_solver_type, + request.ccd_solver_type, + + ), + cresult) + + result.is_collide = result.is_collide or cresult.is_collide + result.time_of_contact = min(cresult.time_of_contact, result.time_of_contact) + return ret + +def distance(CollisionObject o1, CollisionObject o2, + request = None, result=None): + + if request is None: + request = DistanceRequest() + if result is None: + result = DistanceResult() + + cdef defs.DistanceResultd cresult + + cdef double dis = defs.distance(o1.thisptr, o2.thisptr, + defs.DistanceRequestd( + request.enable_nearest_points, + request.enable_signed_distance, + request.gjk_solver_type + ), + cresult) + + if result.min_distance > cresult.min_distance: + result.min_distance = cresult.min_distance + 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 + result.b2 = cresult.b2 + return dis + +############################################################################### +# Collision and Distance Callback Functions +############################################################################### + +def defaultCollisionCallback(CollisionObject o1, CollisionObject o2, cdata): + request = cdata.request + result = cdata.result + + if cdata.done: + return True + + collide(o1, o2, request, result) + + if (not request.enable_cost and result.is_collision and len(result.contacts) > request.num_max_contacts): + cdata.done = True + + return cdata.done + +def defaultDistanceCallback(CollisionObject o1, CollisionObject o2, cdata): + request = cdata.request + result = cdata.result + + if cdata.done: + return True, result.min_distance + + distance(o1, o2, request, result) + + dist = result.min_distance + + if dist <= 0: + return True, dist + + return cdata.done, dist + +cdef class CollisionFunction: + cdef: + object py_func + object py_args + + def __init__(self, py_func, py_args): + self.py_func = py_func + self.py_args = py_args + + 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), + self.py_args)) + return py_r + +cdef class DistanceFunction: + cdef: + object py_func + object py_args + + def __init__(self, py_func, py_args): + self.py_func = py_func + self.py_args = py_args + + 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] + return py_r[0] + +cdef inline bool CollisionCallBack(defs.CollisionObjectd*o1, defs.CollisionObjectd*o2, void*cdata) noexcept: + return ( cdata).eval_func(o1, o2) + +cdef inline bool DistanceCallBack(defs.CollisionObjectd*o1, defs.CollisionObjectd*o2, void*cdata, double& dist) noexcept: + return ( cdata).eval_func(o1, o2, dist) + + +############################################################################### +# Helper Functions +############################################################################### + +cdef quaternion3d_to_numpy(defs.Quaterniond q): + return numpy.array([q.w(), q.x(), q.y(), q.z()]) + +cdef defs.Quaterniond numpy_to_quaternion3d(a): + return defs.Quaterniond( a[0], a[1], a[2], a[3]) + +cdef vec3d_to_numpy(defs.Vector3d vec): + return numpy.array([vec[0], vec[1], vec[2]]) + +cdef defs.Vector3d numpy_to_vec3d(arr): + cdef double[:] memview = arr.astype(numpy.float64) + return defs.Vector3d(&memview[0]) + +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.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_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.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 = 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.CostSourced cost_source): + c = CostSource() + 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.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..cb1205c --- /dev/null +++ b/src/fcl/fcl_defs.pxd @@ -0,0 +1,408 @@ +cimport octomap_defs as octomap +from libcpp cimport bool +from libcpp.memory cimport make_shared, shared_ptr +from libcpp.set cimport set +from libcpp.string cimport string +from libcpp.vector cimport vector + + +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 + bool enable_signed_distance + GJKSolverType gjk_solver_type + DistanceRequestd(bool enable_nearest_points_, bool enable_signed_distance, 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(const shared_ptr[const vector[Vector3d]]& vertices, int num_faces, const shared_ptr[const vector[int]]& faces) except + + int getFaceCount() + +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/src/fcl/octomap_defs.pxd b/src/fcl/octomap_defs.pxd new file mode 100644 index 0000000..196b37c --- /dev/null +++ b/src/fcl/octomap_defs.pxd @@ -0,0 +1,19 @@ +cimport std_defs as std + + +cdef extern from "octomap/OccupancyOcTreeBase.h" namespace "octomap": + # Cython only accepts type template parameters. + # see https://groups.google.com/forum/#!topic/cython-users/xAZxdCFw6Xs + cdef cppclass OccupancyOcTreeBase "octomap::OccupancyOcTreeBase" : + # Constructing + OccupancyOcTreeBase(double resolution) except + + + # Reads only the data (=complete tree structure) from the input stream. + # The tree needs to be constructed with the proper header information + # beforehand, see readBinary(). + std.istream& readBinaryData(std.istream &s) except + + +cdef extern from "octomap/OcTree.h" namespace "octomap": + cdef cppclass OcTree(OccupancyOcTreeBase): + # Constructing + OcTree(double resolution) except + diff --git a/src/fcl/std_defs.pxd b/src/fcl/std_defs.pxd new file mode 100644 index 0000000..5b2cd68 --- /dev/null +++ b/src/fcl/std_defs.pxd @@ -0,0 +1,10 @@ + +cdef extern from "" namespace "std": + cdef cppclass istream: + istream& write(const char*, int) except + + + cdef cppclass iostream(istream): + iostream() except + + + cdef cppclass stringstream(iostream): + stringstream() except + diff --git a/src/fcl/version.py b/src/fcl/version.py new file mode 100644 index 0000000..11a4304 --- /dev/null +++ b/src/fcl/version.py @@ -0,0 +1 @@ +__version__ = "0.7.0.11" diff --git a/test/fcl_unit_test.py b/test/fcl_unit_test.py deleted file mode 100644 index bad363a..0000000 --- a/test/fcl_unit_test.py +++ /dev/null @@ -1,132 +0,0 @@ -import sys -from unittest import TestCase -from fcl import fcl, transform - - -class Test_FCL(TestCase): - def setUp(self): - self.objs = [fcl.CollisionObject(fcl.Box(1.0, 2.0, 3.0)), - fcl.CollisionObject(fcl.Sphere(4.0)), - fcl.CollisionObject(fcl.Cone(5.0, 6.0))] - - self.manager = fcl.DynamicAABBTreeCollisionManager() - self.res = fcl.CollisionResult() - # self.manager.collide(self.res, self.collide_callback_func) - - self._side = (1.0, 2.0, 3.0) - self._radius = 4.0 - self._nearest_points = [(-0.5, 0.0, 0.0), (4.0, 0.0, 0.0)] - self._ref_dist = 15.5 - self.pt1 = [10.0, 0.0, 0.0] - self.pt2 = [-10.0, 0.0, 0.0] - - self.box = fcl.Box(*self._side) - self.sphere = fcl.Sphere(self._radius) - self.cyl = fcl.Cylinder(7.0, 8.0) - - self.trans1 = transform.Transform(transform.Quaternion(), self.pt1) - self.trans2 = transform.Transform(transform.Quaternion(), self.pt2) - - def test_dynamic_aabb_tree_collision_manager(self): - _size = 0 - self.assertTrue(self.manager.size() == 0) - self.manager.registerObjects(self.objs) - _size += len(self.objs) - self.assertTrue(self.manager.size() == _size) - self.manager.registerObject(fcl.CollisionObject(self.cyl)) - _size += 1 - self.assertTrue(self.manager.size() == _size) - - def collide_callback_func(obj1, obj2, res): - ret, res = fcl.collide(obj1, obj2, fcl.CollisionRequest()) - return ret - - def test_distance_box_sphere_translated(self): - ret, result = fcl.collide(fcl.CollisionObject(self.box, self.trans1), - fcl.CollisionObject(self.sphere, self.trans2), - fcl.CollisionRequest() - ) - - self.assertEqual(result.contacts, []) - self.assertEqual(result.cost_sources, []) - - dis, result = fcl.distance(fcl.CollisionObject(self.box, self.trans1), - fcl.CollisionObject(self.sphere, self.trans2), - fcl.DistanceRequest(True) - ) - - self.assertEqual(dis, self._ref_dist) - self.assertEqual(result.min_distance, self._ref_dist) - self.assertEqual(result.nearest_points, self._nearest_points) - self.assertEqual(result.o2.radius, self._radius) - self.assertEqual(result.o1.side, self._side) - - def test_collision_box_sphere(self): - ret, result = fcl.collide(fcl.CollisionObject(fcl.Box(*self._side), - transform.Transform(transform.Quaternion())), - fcl.CollisionObject(fcl.Sphere(self._radius), - transform.Transform(transform.Quaternion(), [0.0, 0.0, 0.0])), - fcl.CollisionRequest()) - - self.assertTrue(len(result.contacts) == 1) - self.assertTrue(len(result.cost_sources) == 0) - - con = result.contacts[0] - self.assertAlmostEqual(con.penetration_depth, 0.0) - self.assertTrue(con.penetration_depth < sys.float_info.min) - self.assertTrue(isinstance(con.o1, fcl.Box)) - self.assertTrue(isinstance(con.o2, fcl.Sphere)) - - def test_collision_and_distance_box_sphere(self): - dis, result = fcl.distance(fcl.CollisionObject(self.box, self.trans1), - fcl.CollisionObject(self.sphere, self.trans2), - fcl.DistanceRequest(True) - ) - - self.assertEqual(dis, self._ref_dist) - self.assertEqual(result.min_distance, self._ref_dist) - self.assertEqual(result.nearest_points, self._nearest_points) - self.assertEqual(result.b1, -1) - self.assertEqual(result.b2, -1) - - dis, result = fcl.distance(fcl.CollisionObject(self.box, transform.Transform(transform.Quaternion())), - fcl.CollisionObject(self.sphere, - transform.Transform(transform.Quaternion(), [0.0, 0.0, 0.0])), - fcl.DistanceRequest(True) - ) - - self.assertEqual(dis, -1.0) - self.assertEqual(result.min_distance, -1.0) - - for i in result.nearest_points: - for j in i: - self.assertAlmostEqual(0.0, j) - - def test_triangle(self): - """ - - TriangleP is not well supported... - - Perhaps not much of an issue, since BVHModel will deal with meshes / triangle soup - So, what'evs? - - """ - import numpy as np - - li = [[0, 100, 100], [0, 0, 0], [100, 0, 100], ] - arr = np.array(li, "f") - _p = fcl.TriangleP(arr[0], arr[1], arr[2]) - p = fcl.CollisionObject(_p) - - collision_object = fcl.CollisionObject(self.box, transform.Transform(transform.Quaternion())) - - # TODO: segfault! - # ....Warning: distance function between node type 9 and node type 17 is not supported - # dis, result = fcl.distance(collision_object, p, fcl.DistanceRequest(True)) - - ret, result = fcl.collide(collision_object, p, fcl.CollisionRequest()) - # Warning: collision function between node type 9 and node type 17 is not supported - self.assertTrue(ret==0) - - - diff --git a/tests/test_fcl.py b/tests/test_fcl.py new file mode 100644 index 0000000..07b9c43 --- /dev/null +++ b/tests/test_fcl.py @@ -0,0 +1,500 @@ +import unittest + +import numpy as np + +import fcl + + +class TestFCL(unittest.TestCase): + def setUp(self): + verts = np.array( + [[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] + ) + tris = np.array([[0, 2, 1], [0, 3, 2], [0, 1, 3], [1, 2, 3]]) + mesh = fcl.BVHModel() + mesh.beginModel(len(verts), len(tris)) + mesh.addSubModel(verts, tris) + mesh.endModel() + + verts = np.array( + [ + [-0.5, -0.5, -0.5], + [-0.5, 0.5, -0.5], + [-0.5, 0.5, 0.5], + [-0.5, -0.5, 0.5], + [0.5, -0.5, -0.5], + [0.5, 0.5, -0.5], + [0.5, 0.5, 0.5], + [0.5, -0.5, 0.5], + ] + ) + quads = np.array( + [ + [0, 3, 2, 1], + [4, 5, 6, 7], + [4, 7, 3, 0], + [5, 1, 2, 6], + [4, 0, 1, 5], + [7, 6, 2, 3], + ] + ) + + faces = np.concatenate( + (np.ones((len(quads), 1), dtype=np.int32) * 4, quads), axis=1 + ).flatten() + convex = fcl.Convex(verts, len(quads), faces) + + self.geometry = { + "box": fcl.Box(1.0, 1.0, 1.0), + "sphere": fcl.Sphere(1.0), + "cone": fcl.Cone(1.0, 1.0), + "cylinder": fcl.Cylinder(1.0, 1.0), + "mesh": mesh, + "convex": convex, + } + + self.crequest = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True) + self.drequest = fcl.DistanceRequest( + enable_nearest_points=True, enable_signed_distance=True + ) + + self.x_axis_rot = np.array([[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]]) + + def test_pairwise_collisions(self): + result = fcl.CollisionResult() + + box = fcl.CollisionObject(self.geometry["box"]) + cone = fcl.CollisionObject(self.geometry["cone"]) + mesh = fcl.CollisionObject(self.geometry["mesh"]) + convex = fcl.CollisionObject(self.geometry["convex"]) + + result = fcl.CollisionResult() + ret = fcl.collide(box, cone, self.crequest, result) + self.assertTrue(ret > 0) + self.assertTrue(result.is_collision) + + result = fcl.CollisionResult() + ret = fcl.collide(convex, cone, self.crequest, result) + self.assertTrue(ret > 0) + self.assertTrue(result.is_collision) + + result = fcl.CollisionResult() + ret = fcl.collide(box, mesh, self.crequest, result) + self.assertTrue(ret > 0) + self.assertTrue(result.is_collision) + + result = fcl.CollisionResult() + ret = fcl.collide(convex, mesh, self.crequest, result) + self.assertTrue(ret > 0) + self.assertTrue(result.is_collision) + + result = fcl.CollisionResult() + ret = fcl.collide(cone, mesh, self.crequest, result) + self.assertTrue(ret > 0) + self.assertTrue(result.is_collision) + + cone.setTranslation(np.array([0.0, 0.0, -0.6])) + + result = fcl.CollisionResult() + ret = fcl.collide(box, cone, self.crequest, result) + self.assertTrue(ret > 0) + self.assertTrue(result.is_collision) + + result = fcl.CollisionResult() + ret = fcl.collide(convex, cone, self.crequest, result) + self.assertTrue(ret > 0) + self.assertTrue(result.is_collision) + + result = fcl.CollisionResult() + ret = fcl.collide(cone, mesh, self.crequest, result) + self.assertTrue(ret == 0) + self.assertFalse(result.is_collision) + + cone.setTranslation(np.array([0.0, -0.9, 0.0])) + cone.setRotation(self.x_axis_rot) + + result = fcl.CollisionResult() + ret = fcl.collide(box, cone, self.crequest, result) + self.assertTrue(ret > 0) + self.assertTrue(result.is_collision) + + result = fcl.CollisionResult() + ret = fcl.collide(convex, cone, self.crequest, result) + self.assertTrue(ret > 0) + self.assertTrue(result.is_collision) + + cone.setTranslation(np.array([0.0, -1.1, 0.0])) + + result = fcl.CollisionResult() + ret = fcl.collide(box, cone, self.crequest, result) + self.assertTrue(ret == 0) + self.assertFalse(result.is_collision) + + result = fcl.CollisionResult() + ret = fcl.collide(convex, cone, self.crequest, result) + self.assertTrue(ret == 0) + self.assertFalse(result.is_collision) + + def test_pairwise_distances(self): + result = fcl.DistanceResult() + + box = fcl.CollisionObject(self.geometry["box"]) + cone = fcl.CollisionObject(self.geometry["cone"]) + mesh = fcl.CollisionObject(self.geometry["mesh"]) + convex = fcl.CollisionObject(self.geometry["convex"]) + + result = fcl.DistanceResult() + ret_box = fcl.distance(box, cone, self.drequest, result) + self.assertTrue(ret_box < 0) + + result = fcl.DistanceResult() + ret_convex = fcl.distance(convex, cone, self.drequest, result) + self.assertTrue(ret_convex < 0) + self.assertAlmostEqual(ret_convex, ret_box, places=6) + + result = fcl.DistanceResult() + ret_box = fcl.distance(box, mesh, self.drequest, result) + self.assertTrue(ret_box < 0) + + result = fcl.DistanceResult() + ret_convex = fcl.distance(convex, mesh, self.drequest, result) + self.assertTrue(ret_convex < 0) + self.assertAlmostEqual(ret_convex, ret_box, places=6) + + result = fcl.DistanceResult() + ret = fcl.distance(cone, mesh, self.drequest, result) + self.assertTrue(ret < 0) + + cone.setTranslation(np.array([0.0, 0.0, -0.6])) + + result = fcl.DistanceResult() + ret = fcl.distance(box, cone, self.drequest, result) + self.assertTrue(ret < 0) + + result = fcl.DistanceResult() + ret = fcl.distance(cone, mesh, self.drequest, result) + self.assertAlmostEqual(ret, 0.1, places=6) + + cone.setTranslation(np.array([0.0, -0.9, 0.0])) + cone.setRotation(self.x_axis_rot) + + result = fcl.DistanceResult() + ret_box = fcl.distance(box, cone, self.drequest, result) + self.assertTrue(ret_box < 0) + + result = fcl.DistanceResult() + ret_convex = fcl.distance(convex, cone, self.drequest, result) + self.assertTrue(ret_convex < 0) + self.assertAlmostEqual(ret_box, ret_convex, places=6) + + cone.setTranslation(np.array([0.0, -1.1, 0.0])) + + result = fcl.DistanceResult() + ret = fcl.distance(box, cone, self.drequest, result) + self.assertAlmostEqual(ret, 0.1, places=6) + + result = fcl.DistanceResult() + ret = fcl.distance(convex, cone, self.drequest, result) + self.assertAlmostEqual(ret, 0.1, places=6) + + def test_pairwise_continuous_collisions(self): + request = fcl.ContinuousCollisionRequest() + result = fcl.ContinuousCollisionResult() + + box = fcl.CollisionObject(self.geometry["box"]) + cone = fcl.CollisionObject( + self.geometry["cone"], fcl.Transform(np.array([0.0, 0.0, -2.0])) + ) + + _ret = fcl.continuousCollide( + box, fcl.Transform(), cone, fcl.Transform(), request, result + ) + + """ + ## WHY DOES THIS FAIL ## + self.assertTrue(result.is_collide) + self.assertAlmostEqual(0.625, ret) + """ + + def test_managed_collisions(self): + manager1 = fcl.DynamicAABBTreeCollisionManager() + manager2 = fcl.DynamicAABBTreeCollisionManager() + manager3 = fcl.DynamicAABBTreeCollisionManager() + + objs1 = [ + fcl.CollisionObject(self.geometry["box"]), + fcl.CollisionObject(self.geometry["cylinder"]), + ] + objs2 = [ + fcl.CollisionObject( + self.geometry["cone"], fcl.Transform(np.array([0.0, 0.0, 5.0])) + ), + fcl.CollisionObject( + self.geometry["cylinder"], fcl.Transform(np.array([0.0, 0.0, -5.0])) + ), + ] + objs3 = [ + fcl.CollisionObject(self.geometry["mesh"]), + fcl.CollisionObject( + self.geometry["convex"], fcl.Transform(np.array([0.0, 0.0, -5.0])) + ), + ] + + manager1.registerObjects(objs1) + manager2.registerObjects(objs2) + manager3.registerObjects(objs3) + + manager1.setup() + manager2.setup() + manager3.setup() + + self.assertTrue(len(manager1.getObjects()) == 2) + self.assertTrue(len(manager2.getObjects()) == 2) + self.assertTrue(len(manager3.getObjects()) == 2) + + # One-to-many + o1 = fcl.CollisionObject(self.geometry["box"]) + o2 = fcl.CollisionObject( + self.geometry["cylinder"], fcl.Transform(np.array([0.0, 0.0, -4.6])) + ) + + cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) + manager1.collide(o1, cdata, fcl.defaultCollisionCallback) + self.assertTrue(cdata.result.is_collision) + + cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) + manager1.collide(o2, cdata, fcl.defaultCollisionCallback) + self.assertFalse(cdata.result.is_collision) + + cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) + manager2.collide(o1, cdata, fcl.defaultCollisionCallback) + self.assertFalse(cdata.result.is_collision) + + cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) + manager2.collide(o2, cdata, fcl.defaultCollisionCallback) + self.assertTrue(cdata.result.is_collision) + + # Many-to-many, internal + cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) + manager1.collide(cdata, fcl.defaultCollisionCallback) + self.assertTrue(cdata.result.is_collision) + + cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) + manager2.collide(cdata, fcl.defaultCollisionCallback) + self.assertFalse(cdata.result.is_collision) + + # Many-to-many, grouped + cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) + manager1.collide(manager2, cdata, fcl.defaultCollisionCallback) + self.assertFalse(cdata.result.is_collision) + + cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) + manager2.collide(manager3, cdata, fcl.defaultCollisionCallback) + self.assertTrue(cdata.result.is_collision) + + cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) + manager1.collide(manager3, cdata, fcl.defaultCollisionCallback) + self.assertTrue(cdata.result.is_collision) + + def test_updates(self): + manager = fcl.DynamicAABBTreeCollisionManager() + + objs = [ + fcl.CollisionObject(self.geometry["sphere"]), + fcl.CollisionObject( + self.geometry["sphere"], fcl.Transform(np.array([0.0, 0.0, -5.0])) + ), + ] + + manager.registerObjects(objs) + manager.setup() + + self.assertTrue(len(manager.getObjects()) == 2) + + # Many-to-many, internal + cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) + manager.collide(cdata, fcl.defaultCollisionCallback) + self.assertFalse(cdata.result.is_collision) + + objs[1].setTranslation(np.array([0.0, 0.0, -0.3])) + cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) + manager.update(objs[1]) + manager.collide(cdata, fcl.defaultCollisionCallback) + self.assertTrue(cdata.result.is_collision) + + def test_many_objects(self): + manager = fcl.DynamicAABBTreeCollisionManager() + + objs = [ + fcl.CollisionObject(self.geometry["sphere"]), + fcl.CollisionObject( + self.geometry["sphere"], fcl.Transform(np.array([0.0, 0.0, -5.0])) + ), + fcl.CollisionObject( + self.geometry["sphere"], fcl.Transform(np.array([0.0, 0.0, 5.0])) + ), + ] + + manager.registerObjects(objs) + manager.setup() + + self.assertTrue(len(manager.getObjects()) == 3) + + # Many-to-many, internal + cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) + manager.collide(cdata, fcl.defaultCollisionCallback) + self.assertFalse(cdata.result.is_collision) + + objs[1].setTranslation(np.array([0.0, 0.0, -0.3])) + manager.update(objs[1]) + cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) + manager.collide(cdata, fcl.defaultCollisionCallback) + self.assertTrue(cdata.result.is_collision) + + objs[1].setTranslation(np.array([0.0, 0.0, -5.0])) + manager.update(objs[1]) + cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) + manager.collide(cdata, fcl.defaultCollisionCallback) + self.assertFalse(cdata.result.is_collision) + + objs[2].setTranslation(np.array([0.0, 0.0, 0.3])) + manager.update(objs[2]) + cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) + manager.collide(cdata, fcl.defaultCollisionCallback) + self.assertTrue(cdata.result.is_collision) + + objs[2].setTranslation(np.array([0.0, 0.0, 5.0])) + manager.update(objs[2]) + cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) + manager.collide(cdata, fcl.defaultCollisionCallback) + self.assertFalse(cdata.result.is_collision) + + def test_managed_distances(self): + manager1 = fcl.DynamicAABBTreeCollisionManager() + manager2 = fcl.DynamicAABBTreeCollisionManager() + manager3 = fcl.DynamicAABBTreeCollisionManager() + + objs1 = [ + fcl.CollisionObject(self.geometry["box"]), + fcl.CollisionObject(self.geometry["cylinder"]), + ] + objs2 = [ + fcl.CollisionObject( + self.geometry["cone"], fcl.Transform(np.array([0.0, 0.0, 5.0])) + ), + fcl.CollisionObject( + self.geometry["cylinder"], fcl.Transform(np.array([0.0, 0.0, -5.0])) + ), + ] + objs3 = [ + fcl.CollisionObject(self.geometry["mesh"]), + fcl.CollisionObject( + self.geometry["convex"], fcl.Transform(np.array([0.0, 0.0, -5.0])) + ), + ] + + manager1.registerObjects(objs1) + manager2.registerObjects(objs2) + manager3.registerObjects(objs3) + + manager1.setup() + manager2.setup() + manager3.setup() + + self.assertTrue(len(manager1.getObjects()) == 2) + self.assertTrue(len(manager2.getObjects()) == 2) + self.assertTrue(len(manager3.getObjects()) == 2) + + # One-to-many + o1 = fcl.CollisionObject(self.geometry["box"]) + o2 = fcl.CollisionObject( + self.geometry["cylinder"], fcl.Transform(np.array([0.0, 0.0, -4.6])) + ) + + cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult()) + manager1.distance(o1, cdata, fcl.defaultDistanceCallback) + self.assertTrue(cdata.result.min_distance < 0) + + cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult()) + manager1.distance(o2, cdata, fcl.defaultDistanceCallback) + self.assertAlmostEqual(cdata.result.min_distance, 3.6, places=6) + + cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult()) + manager2.distance(o1, cdata, fcl.defaultDistanceCallback) + self.assertAlmostEqual(cdata.result.min_distance, 4.0, places=6) + + cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult()) + manager2.distance(o2, cdata, fcl.defaultDistanceCallback) + self.assertTrue(cdata.result.min_distance < 0) + + # Many-to-many, internal + cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult()) + manager1.distance(cdata, fcl.defaultDistanceCallback) + self.assertTrue(cdata.result.min_distance < 0) + + cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult()) + manager2.distance(cdata, fcl.defaultDistanceCallback) + self.assertAlmostEqual(cdata.result.min_distance, 9.0, places=6) + + # Many-to-many, grouped + cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult()) + manager1.distance(manager2, cdata, fcl.defaultDistanceCallback) + self.assertAlmostEqual(cdata.result.min_distance, 4.0, places=6) + + cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult()) + manager2.distance(manager3, cdata, fcl.defaultDistanceCallback) + self.assertTrue(cdata.result.min_distance < 0) + + cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult()) + manager1.distance(manager3, cdata, fcl.defaultDistanceCallback) + self.assertTrue(cdata.result.min_distance < 0) + + # Tests with boxes to make sure nearest points are independent of manager order + def test_nearest_points(self): + box1, box1_t = (20.0, 5.0), (-2.0, 0.0) + box2, box2_t = (5, 5), (-5.0, 10.0) + box3, box3_t = (5, 5), (-15.0, 15.0) + q1 = np.array([0.8660254, 0.0, 0.0, 0.5]) # np.pi / 3 z-ax rotation + q2 = np.array([1.0, 0.0, 0.0, 0.0]) # no rotation + + h = 1000 + g1 = fcl.Box(*box1, h) + t1 = fcl.Transform(q1, [*box1_t, 0]) + o1 = fcl.CollisionObject(g1, t1) + g2 = fcl.Box(*box2, h) + t2 = fcl.Transform(q2, [*box2_t, 0]) + o2 = fcl.CollisionObject(g2, t2) + g3 = fcl.Box(*box3, h) + t3 = fcl.Transform(q2, [*box3_t, 0]) + o3 = fcl.CollisionObject(g3, t3) + + manager1 = fcl.DynamicAABBTreeCollisionManager() + manager1.registerObjects([o1]) + manager1.setup() + + manager2 = fcl.DynamicAABBTreeCollisionManager() + manager2r = fcl.DynamicAABBTreeCollisionManager() + manager2.registerObjects([o2, o3]) + manager2r.registerObjects([o3, o2]) + manager2.setup() + manager2r.setup() + + ddata = fcl.DistanceData(request=fcl.DistanceRequest(enable_nearest_points=True)) + manager1.distance(manager2, ddata, fcl.defaultDistanceCallback) + self.assertAlmostEqual( + ddata.result.min_distance, + np.linalg.norm(np.subtract(*ddata.result.nearest_points)), + places=6, + ) + + ddata = fcl.DistanceData(request=fcl.DistanceRequest(enable_nearest_points=True)) + manager1.distance(manager2r, ddata, fcl.defaultDistanceCallback) + self.assertAlmostEqual( + ddata.result.min_distance, + np.linalg.norm(np.subtract(*ddata.result.nearest_points)), + places=6, + ) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/test_precision.py b/tests/test_precision.py new file mode 100644 index 0000000..a695cb2 --- /dev/null +++ b/tests/test_precision.py @@ -0,0 +1,203 @@ +import unittest + +import numpy as np + +import fcl + + +# These test cases were added because there was a very sneaky crash that would occur +# in the signed distance functions if all libs were not compiled with double precision +class TestPrecision(unittest.TestCase): + def setUp(self): + self.act_dist = -0.00735518 + + # Set up first mesh + f1 = np.array( + [ + [0, 1, 2], + [0, 2, 3], + [3, 4, 5], + [3, 5, 0], + [2, 6, 4], + [2, 4, 3], + [1, 7, 6], + [1, 6, 2], + [7, 5, 4], + [7, 4, 6], + [0, 5, 7], + [0, 7, 1], + ] + ) + tris1 = np.concatenate( + (3 * np.ones((len(f1), 1), dtype=np.int64), f1), axis=1 + ).flatten() + v1 = np.array( + [ + [-1.5596524477005005, -1.4861732721328735, 0.0], + [-1.55965256690979, -1.3408161401748657, 0.0], + [0.8114118576049805, -1.3408160209655762, 0.0], + [0.8114122152328491, -1.4861732721328735, 0.0], + [0.8114122152328491, -1.4861732721328735, 2.566922187805176], + [-1.559652328491211, -1.4861699342727661, 2.566922187805176], + [0.8114122152328491, -1.3408160209655762, 2.566922187805176], + [-1.5596498250961304, -1.3408160209655762, 2.566922187805176], + ] + ) + t1 = np.zeros(3) + r1 = np.eye(3) + + # Set up second mesh + f2 = np.array( + [ + [0, 1, 2], + [0, 2, 3], + [3, 2, 4], + [3, 4, 5], + [5, 4, 6], + [5, 6, 7], + [7, 6, 1], + [7, 1, 0], + [3, 5, 7], + [3, 7, 0], + [4, 2, 1], + [4, 1, 6], + ] + ) + tris2 = np.concatenate( + (3 * np.ones((len(f2), 1), dtype=np.int64), f2), axis=1 + ).flatten() + v2 = np.array( + [ + [-0.035486817359924316, -0.15411892533302307, 0.08323988318443298], + [-0.035486817359924316, -0.034327179193496704, 0.13931824266910553], + [-0.035486817359924316, -0.004236131906509399, 0.07503926753997803], + [-0.035486817359924316, -0.12402787804603577, 0.018960915505886078], + [0.035486817359924316, -0.004236131906509399, 0.07503926753997803], + [0.035486817359924316, -0.12402787804603577, 0.018960915505886078], + [0.035486817359924316, -0.034327179193496704, 0.13931824266910553], + [0.035486817359924316, -0.15411892533302307, 0.08323988318443298], + ] + ) + t2 = np.array([-0.17816561357553456, -1.1899346069644454, 1.1061233975645952]) + r2 = np.array( + [ + [-0.7731251991739131, 0.3586043541062879, -0.5231446679631834], + [0.4540350514442685, 0.8888443213778688, -0.06170854409493361], + [0.4428652147801832, -0.28523444667558645, -0.8500068893646534], + ] + ) + + # Wrap meshes in FCL Convex objects + c1 = fcl.Convex(v1, len(f1), tris1) + c2 = fcl.Convex(v2, len(f2), tris2) + + # Wrap in CollisionObjects + self.o1 = fcl.CollisionObject(c1, fcl.Transform(r1, t1)) + self.o2 = fcl.CollisionObject(c2, fcl.Transform(r2, t2)) + + # Create managers + self.mgr1 = fcl.DynamicAABBTreeCollisionManager() + self.mgr2 = fcl.DynamicAABBTreeCollisionManager() + + self.mgr1.registerObjects([self.o1]) + self.mgr2.registerObjects([self.o2]) + + self.mgr1.setup() + self.mgr2.setup() + + def test_obj_obj_coll(self): + request = fcl.CollisionRequest() + result = fcl.CollisionResult() + ret = fcl.collide(self.o1, self.o2, request, result) + assert ret == 1 # Objects are in collision + + request = fcl.CollisionRequest() + result = fcl.CollisionResult() + ret = fcl.collide(self.o2, self.o1, request, result) + assert ret == 1 # Objects are in collision + + def test_obj_obj_simple_distance(self): + request = fcl.DistanceRequest() + result = fcl.DistanceResult() + ret = fcl.distance(self.o1, self.o2, request, result) + assert ret == -1 + + request = fcl.DistanceRequest() + result = fcl.DistanceResult() + ret = fcl.distance(self.o2, self.o1, request, result) + assert ret == -1 + + def test_obj_obj_signed_distance(self): + request = fcl.DistanceRequest( + enable_nearest_points=True, enable_signed_distance=True + ) + result = fcl.DistanceResult() + ret = fcl.distance(self.o1, self.o2, request, result) + assert np.isclose(ret, self.act_dist) + assert np.isclose(np.linalg.norm(np.subtract(*result.nearest_points)), abs(ret)) + + request = fcl.DistanceRequest( + enable_nearest_points=True, enable_signed_distance=True + ) + result = fcl.DistanceResult() + ret = fcl.distance(self.o2, self.o1, request, result) + assert np.isclose(ret, self.act_dist) + assert np.isclose(np.linalg.norm(np.subtract(*result.nearest_points)), abs(ret)) + + def test_mgr_obj_coll(self): + req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True) + rdata = fcl.CollisionData(request=req) + self.mgr1.collide(self.o2, rdata, fcl.defaultCollisionCallback) + assert rdata.result.is_collision + + rdata = fcl.CollisionData(request=req) + self.mgr2.collide(self.o1, rdata, fcl.defaultCollisionCallback) + assert rdata.result.is_collision + + def test_mgr_mgr_coll(self): + req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True) + rdata = fcl.CollisionData(request=req) + self.mgr1.collide(self.mgr2, rdata, fcl.defaultCollisionCallback) + assert rdata.result.is_collision + + rdata = fcl.CollisionData(request=req) + self.mgr2.collide(self.mgr1, rdata, fcl.defaultCollisionCallback) + assert rdata.result.is_collision + + def test_mgr_obj_signed_distance(self): + req = fcl.DistanceRequest(enable_signed_distance=True, enable_nearest_points=True) + ddata = fcl.DistanceData(req) + self.mgr1.distance(self.o2, ddata, fcl.defaultDistanceCallback) + assert np.isclose(ddata.result.min_distance, self.act_dist) + assert np.isclose( + np.linalg.norm(np.subtract(*ddata.result.nearest_points)), + abs(ddata.result.min_distance), + ) + + req = fcl.DistanceRequest(enable_signed_distance=True, enable_nearest_points=True) + ddata = fcl.DistanceData(req) + self.mgr2.distance(self.o1, ddata, fcl.defaultDistanceCallback) + assert np.isclose(ddata.result.min_distance, self.act_dist) + assert np.isclose( + np.linalg.norm(np.subtract(*ddata.result.nearest_points)), + abs(ddata.result.min_distance), + ) + + def test_mgr_mgr_signed_distance(self): + req = fcl.DistanceRequest(enable_signed_distance=True, enable_nearest_points=True) + ddata = fcl.DistanceData(req) + self.mgr1.distance(self.mgr2, ddata, fcl.defaultDistanceCallback) + assert np.isclose(ddata.result.min_distance, self.act_dist) + assert np.isclose( + np.linalg.norm(np.subtract(*ddata.result.nearest_points)), + abs(ddata.result.min_distance), + ) + + req = fcl.DistanceRequest(enable_signed_distance=True, enable_nearest_points=True) + ddata = fcl.DistanceData(req) + self.mgr2.distance(self.mgr1, ddata, fcl.defaultDistanceCallback) + assert np.isclose(ddata.result.min_distance, self.act_dist) + assert np.isclose( + np.linalg.norm(np.subtract(*ddata.result.nearest_points)), + abs(ddata.result.min_distance), + )