commit bb87928bce7b79872b8b0cbfc2d73e6941c1e616 Author: Upstream Snapshot Date: Fri Aug 28 15:42:31 2026 +0800 Import upstream snapshot 80e710700aac9573a2230f74f7ce9e094833a0bc Upstream: https://github.com/Rhoban/onshape-to-robot Upstream-Commit: 80e710700aac9573a2230f74f7ce9e094833a0bc Upstream-Branch: master diff --git a/.github/workflows/wheels.yml b/.github/workflows/wheels.yml new file mode 100644 index 0000000..aed220e --- /dev/null +++ b/.github/workflows/wheels.yml @@ -0,0 +1,38 @@ +name: Build and upload to PyPI + +on: + release: + types: + - published + +jobs: + build_sdist: + name: Build source distribution + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + + - name: Build sdist + run: pipx run build --sdist + + - uses: actions/upload-artifact@v4 + with: + name: cibw-sdist + path: dist/*.tar.gz + + upload_pypi: + needs: [build_sdist] + runs-on: ubuntu-latest + environment: pypi + permissions: + id-token: write + if: github.event_name == 'release' && github.event.action == 'published' + steps: + - uses: actions/download-artifact@v4 + with: + # unpacks all CIBW artifacts into dist/ + pattern: cibw-* + path: dist + merge-multiple: true + + - uses: pypa/gh-action-pypi-publish@release/v1 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..4154c5f --- /dev/null +++ b/.gitignore @@ -0,0 +1,10 @@ +**.pyc +.vscode +robots +data +**/cache/ +dist +build +onshape_to_robot.egg-info +.env +uv.lock diff --git a/.readthedocs.yaml b/.readthedocs.yaml new file mode 100644 index 0000000..303f2a3 --- /dev/null +++ b/.readthedocs.yaml @@ -0,0 +1,22 @@ +# .readthedocs.yaml +# Read the Docs configuration file +# See https://docs.readthedocs.io/en/stable/config-file/v2.html for details + +# Required +version: 2 + +# Set the version of Python and other tools you might need +build: + os: ubuntu-22.04 + tools: + python: "3.11" + +# Build documentation in the docs/ directory with Sphinx +sphinx: + configuration: docs/source/conf.py + +# We recommend specifying your dependencies to enable reproducible builds: +# https://docs.readthedocs.io/en/stable/guides/reproducible-builds.html +python: + install: + - requirements: docs/requirements.txt diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..e5bdf8f --- /dev/null +++ b/LICENSE @@ -0,0 +1,7 @@ +Copyright 2019-2099 Rhoban Team + +Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. \ No newline at end of file diff --git a/MANIFEST.in b/MANIFEST.in new file mode 100644 index 0000000..0386774 --- /dev/null +++ b/MANIFEST.in @@ -0,0 +1,2 @@ +include README-pypi.md + diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..3e74d87 --- /dev/null +++ b/Makefile @@ -0,0 +1,13 @@ + +all: + @rm -rf dist/* + python3 setup.py sdist bdist_wheel + +upload: + python3 -m twine upload --repository pypi dist/* + +upload-test: + python3 -m twine upload --repository testpypi dist/* + +clean: + rm -rf build dist onshape_to_robot.egg-info diff --git a/README-pypi.md b/README-pypi.md new file mode 100644 index 0000000..e324bba --- /dev/null +++ b/README-pypi.md @@ -0,0 +1,10 @@ +# Onshape to robot (URDF, SDF, MuJoCo) + +This tool is based on the [Onshape API](https://dev-portal.onshape.com/) to retrieve +informations from an assembly and build a robot description (URDF, SDF, MuJoCo) suitable for physics +simulation. + +* Check out the [official documentation](https://onshape-to-robot.readthedocs.io/) +* [GitHub repository](https://github.com/rhoban/onshape-to-robot/) +* [Robots examples](https://github.com/rhoban/onshape-to-robot-examples) + diff --git a/README.md b/README.md new file mode 100644 index 0000000..25a6406 --- /dev/null +++ b/README.md @@ -0,0 +1,12 @@ +# Onshape to Robot (URDF, SDF, MuJoCo) + +

+ +

+ +This tool is based on the [Onshape API](https://dev-portal.onshape.com/) to retrieve +informations from an assembly and build a robot description (URDF, SDF, MuJoCo ) suitable +for physics simulation. + +* [Documentation](https://onshape-to-robot.readthedocs.io/) +* [Examples](https://github.com/rhoban/onshape-to-robot-examples) diff --git a/docs/.gitignore b/docs/.gitignore new file mode 100644 index 0000000..378eac2 --- /dev/null +++ b/docs/.gitignore @@ -0,0 +1 @@ +build diff --git a/docs/Makefile b/docs/Makefile new file mode 100644 index 0000000..d0c3cbf --- /dev/null +++ b/docs/Makefile @@ -0,0 +1,20 @@ +# Minimal makefile for Sphinx documentation +# + +# You can set these variables from the command line, and also +# from the environment for the first two. +SPHINXOPTS ?= +SPHINXBUILD ?= sphinx-build +SOURCEDIR = source +BUILDDIR = build + +# Put it first so that "make" without argument is like "make help". +help: + @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) + +.PHONY: help Makefile + +# Catch-all target: route all unknown targets to Sphinx using the new +# "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). +%: Makefile + @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) diff --git a/docs/make.bat b/docs/make.bat new file mode 100644 index 0000000..6247f7e --- /dev/null +++ b/docs/make.bat @@ -0,0 +1,35 @@ +@ECHO OFF + +pushd %~dp0 + +REM Command file for Sphinx documentation + +if "%SPHINXBUILD%" == "" ( + set SPHINXBUILD=sphinx-build +) +set SOURCEDIR=source +set BUILDDIR=build + +if "%1" == "" goto help + +%SPHINXBUILD% >NUL 2>NUL +if errorlevel 9009 ( + echo. + echo.The 'sphinx-build' command was not found. Make sure you have Sphinx + echo.installed, then set the SPHINXBUILD environment variable to point + echo.to the full path of the 'sphinx-build' executable. Alternatively you + echo.may add the Sphinx directory to PATH. + echo. + echo.If you don't have Sphinx installed, grab it from + echo.http://sphinx-doc.org/ + exit /b 1 +) + +%SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% +goto end + +:help +%SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% + +:end +popd diff --git a/docs/requirements.txt b/docs/requirements.txt new file mode 100644 index 0000000..6c5d5d4 --- /dev/null +++ b/docs/requirements.txt @@ -0,0 +1 @@ +sphinx-rtd-theme diff --git a/docs/source/_static/architecture.svg b/docs/source/_static/architecture.svg new file mode 100644 index 0000000..689fc02 --- /dev/null +++ b/docs/source/_static/architecture.svg @@ -0,0 +1,2525 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/docs/source/_static/css/custom.css b/docs/source/_static/css/custom.css new file mode 100644 index 0000000..f823861 --- /dev/null +++ b/docs/source/_static/css/custom.css @@ -0,0 +1,4 @@ + +img.padding { + margin-bottom: 20px !important; +} \ No newline at end of file diff --git a/docs/source/_static/dof_tree.svg b/docs/source/_static/dof_tree.svg new file mode 100644 index 0000000..336bb3f --- /dev/null +++ b/docs/source/_static/dof_tree.svg @@ -0,0 +1,1558 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/docs/source/_static/fixed.svg b/docs/source/_static/fixed.svg new file mode 100644 index 0000000..32f533b --- /dev/null +++ b/docs/source/_static/fixed.svg @@ -0,0 +1,945 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/docs/source/_static/frame.svg b/docs/source/_static/frame.svg new file mode 100644 index 0000000..1dd1a79 --- /dev/null +++ b/docs/source/_static/frame.svg @@ -0,0 +1,1637 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/docs/source/_static/img/after.png b/docs/source/_static/img/after.png new file mode 100644 index 0000000..fdbb076 Binary files /dev/null and b/docs/source/_static/img/after.png differ diff --git a/docs/source/_static/img/architecture.png b/docs/source/_static/img/architecture.png new file mode 100644 index 0000000..245a89a Binary files /dev/null and b/docs/source/_static/img/architecture.png differ diff --git a/docs/source/_static/img/before.png b/docs/source/_static/img/before.png new file mode 100644 index 0000000..189eb71 Binary files /dev/null and b/docs/source/_static/img/before.png differ diff --git a/docs/source/_static/img/configuration.png b/docs/source/_static/img/configuration.png new file mode 100644 index 0000000..2d669cd Binary files /dev/null and b/docs/source/_static/img/configuration.png differ diff --git a/docs/source/_static/img/design.png b/docs/source/_static/img/design.png new file mode 100644 index 0000000..743b019 Binary files /dev/null and b/docs/source/_static/img/design.png differ diff --git a/docs/source/_static/img/design.xcf b/docs/source/_static/img/design.xcf new file mode 100644 index 0000000..5bad493 Binary files /dev/null and b/docs/source/_static/img/design.xcf differ diff --git a/docs/source/_static/img/fixed.png b/docs/source/_static/img/fixed.png new file mode 100644 index 0000000..44bd9d6 Binary files /dev/null and b/docs/source/_static/img/fixed.png differ diff --git a/docs/source/_static/img/frame.png b/docs/source/_static/img/frame.png new file mode 100644 index 0000000..24bd489 Binary files /dev/null and b/docs/source/_static/img/frame.png differ diff --git a/docs/source/_static/img/frames.png b/docs/source/_static/img/frames.png new file mode 100644 index 0000000..e9c33f5 Binary files /dev/null and b/docs/source/_static/img/frames.png differ diff --git a/docs/source/_static/img/gear.png b/docs/source/_static/img/gear.png new file mode 100644 index 0000000..647cf79 Binary files /dev/null and b/docs/source/_static/img/gear.png differ diff --git a/docs/source/_static/img/loop.png b/docs/source/_static/img/loop.png new file mode 100644 index 0000000..7e543ba Binary files /dev/null and b/docs/source/_static/img/loop.png differ diff --git a/docs/source/_static/img/main.png b/docs/source/_static/img/main.png new file mode 100644 index 0000000..d4284f9 Binary files /dev/null and b/docs/source/_static/img/main.png differ diff --git a/docs/source/_static/img/opened_chain.png b/docs/source/_static/img/opened_chain.png new file mode 100644 index 0000000..abf02a5 Binary files /dev/null and b/docs/source/_static/img/opened_chain.png differ diff --git a/docs/source/_static/img/pure-shape.png b/docs/source/_static/img/pure-shape.png new file mode 100644 index 0000000..4cc75a8 Binary files /dev/null and b/docs/source/_static/img/pure-shape.png differ diff --git a/docs/source/_static/img/shape-approx.png b/docs/source/_static/img/shape-approx.png new file mode 100644 index 0000000..4dc73d3 Binary files /dev/null and b/docs/source/_static/img/shape-approx.png differ diff --git a/docs/source/_static/img/shape-approx.xcf b/docs/source/_static/img/shape-approx.xcf new file mode 100644 index 0000000..f042550 Binary files /dev/null and b/docs/source/_static/img/shape-approx.xcf differ diff --git a/docs/source/_static/img/smalls/after.png b/docs/source/_static/img/smalls/after.png new file mode 100644 index 0000000..c573f51 Binary files /dev/null and b/docs/source/_static/img/smalls/after.png differ diff --git a/docs/source/_static/img/smalls/before.png b/docs/source/_static/img/smalls/before.png new file mode 100644 index 0000000..23992bf Binary files /dev/null and b/docs/source/_static/img/smalls/before.png differ diff --git a/docs/source/_static/img/smalls/design.png b/docs/source/_static/img/smalls/design.png new file mode 100644 index 0000000..b2ec3e1 Binary files /dev/null and b/docs/source/_static/img/smalls/design.png differ diff --git a/docs/source/_static/img/smalls/frame.png b/docs/source/_static/img/smalls/frame.png new file mode 100644 index 0000000..febc7ba Binary files /dev/null and b/docs/source/_static/img/smalls/frame.png differ diff --git a/docs/source/_static/img/smalls/main.png b/docs/source/_static/img/smalls/main.png new file mode 100644 index 0000000..03f209a Binary files /dev/null and b/docs/source/_static/img/smalls/main.png differ diff --git a/docs/source/_static/img/smalls/pure-shape.png b/docs/source/_static/img/smalls/pure-shape.png new file mode 100644 index 0000000..fa124b8 Binary files /dev/null and b/docs/source/_static/img/smalls/pure-shape.png differ diff --git a/docs/source/_static/img/smalls/shape-approx.png b/docs/source/_static/img/smalls/shape-approx.png new file mode 100644 index 0000000..82f263b Binary files /dev/null and b/docs/source/_static/img/smalls/shape-approx.png differ diff --git a/docs/source/_static/img/zaxis.png b/docs/source/_static/img/zaxis.png new file mode 100644 index 0000000..b8bca60 Binary files /dev/null and b/docs/source/_static/img/zaxis.png differ diff --git a/docs/source/_static/loop.svg b/docs/source/_static/loop.svg new file mode 100644 index 0000000..7ae5a6c --- /dev/null +++ b/docs/source/_static/loop.svg @@ -0,0 +1,1062 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/docs/source/_static/main.svg b/docs/source/_static/main.svg new file mode 100644 index 0000000..7da3829 --- /dev/null +++ b/docs/source/_static/main.svg @@ -0,0 +1,1653 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/docs/source/cache.rst b/docs/source/cache.rst new file mode 100644 index 0000000..97290a7 --- /dev/null +++ b/docs/source/cache.rst @@ -0,0 +1,20 @@ +Cache +===== + +Presentation +------------ + +In order to re-issue the same requests to the Onshape API, ``onshape-to-robot`` caches the result of most of the requests. + +.. note:: + + All requests involving a **workspace** can't be cached, since they rely on live version that are subject to change. + +Clearing the cache +------------------ + +You can clear the cache using the following command: + +.. code-block:: bash + + onshape-to-robot-clear-cache \ No newline at end of file diff --git a/docs/source/conf.py b/docs/source/conf.py new file mode 100644 index 0000000..321dae8 --- /dev/null +++ b/docs/source/conf.py @@ -0,0 +1,61 @@ +# Configuration file for the Sphinx documentation builder. +# +# This file only contains a selection of the most common options. For a full +# list see the documentation: +# https://www.sphinx-doc.org/en/master/usage/configuration.html + +# -- Path setup -------------------------------------------------------------- + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +# +# import os +# import sys +# sys.path.insert(0, os.path.abspath('.')) + + +# -- Project information ----------------------------------------------------- + +project = 'Onshape to robot' +copyright = '2025, Rhoban' +author = 'Rhoban' + +# The full version, including alpha/beta/rc tags +release = 'latest' + + +# -- General configuration --------------------------------------------------- + +# Add any Sphinx extension module names here, as strings. They can be +# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom +# ones. +extensions = [ +] + +# Add any paths that contain templates here, relative to this directory. +templates_path = ['_templates'] + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +# This pattern also affects html_static_path and html_extra_path. +exclude_patterns = [] + + +# -- Options for HTML output ------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +# +html_theme = "sphinx_rtd_theme" + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = ['_static'] + +html_css_files = [ + 'css/custom.css', +] + +master_doc = 'index' \ No newline at end of file diff --git a/docs/source/config.rst b/docs/source/config.rst new file mode 100644 index 0000000..62b4a75 --- /dev/null +++ b/docs/source/config.rst @@ -0,0 +1,277 @@ +Configuration (config.json) +=========================== + +Specific entries +---------------- + +Below are the global configuration entries. +You might also want to check out the following documentation for more specific entries: + +* Exporters + * :doc:`URDF specific entries ` + * :doc:`SDF specific entries ` + * :doc:`MuJoCo specific entries ` +* :doc:`Processors ` can define their own specific entries + + +``config.json`` entries +----------------------- + +Here is an example of complete ``config.json`` file, with details below: + +.. code-block:: javascript + + // config.json general options + // for urdf or mujoco specific options, see documentation + { + // Onshape assembly URL + "url": "https://cad.onshape.com/documents/11a7f59e37f711d732274fca/w/7807518dc67487ad405722c8/e/5233c6445c575366a6cc0d50", + // Output format: urdf or mujoco (required) + "output_format": "urdf", + // Output filename (default: "robot") + // Extension (.urdf, .xml) will be added automatically + "output_filename": "robot", + // Assets directory (default: "assets") + "assets_directory": "assets", + + // If you don't use "url", you can alternatively specify the following + // The Onshape document id to parse, see "getting started" (optional) + "document_id": "document-id", + // The document version id (optional) + "version_id": "version-id", + // The workspace id (optional) + "workspace_id": "workspace-id", + // Element id (optional) + "element_id": "element-id", + // Assembly name to use in the document (optional) + "assembly_name": "robot", + + // Onshape configuration to use (default: "default") + "configuration": "Configuration=BigFoot;RodLength=50mm", + // Robot name (default: "onshape") + "robot_name": "robot", + + // Ignore limits (default: false) + "ignore_limits": true, + + // Parts to ignore (default: {}) + "ignore": { + // Ignore visual for visual + "part1": "visual", + "screw*": "visual", + + // Ignore everything expect "leg" for collision + "*" : "collision" + "!leg": "collision" + }, + + // Whether to keep frame links (default: false) + "draw_frames": true, + // Override the color of all links (default: None) + "color": [0.5, 0.1, 0.1], + + // Disable dynamics retrieval (default: false) + "no_dynamics": true, + + // Whether to include configuration suffix to part (stl) files (default: true) + "include_configuration_suffix": false, + + // Post import commands (default: []) + "post_import_commands" [ + "echo 'Import done'", + "echo 'Do something else'" + ], + + // Custom processors + "processors": [ + "my_project.my_custom_processor:MyCustomProcessor" + ], + + // Number of decimals to round numerical values (default: 12) + "round_decimals": 12 + + // More options available in specific exporters (URDF, SDF, MuJoCo) + // More options available in processors + } + +.. note:: + + Comments are supported in the ``config.json`` file. + +.. note:: + + Since ``1.0.0``, all configuration entries are now snake case. For backward compatibility reasons, the old + camel case entries are still supported. (for example, ``document_id`` and ``documentId`` are equivalent). + +``url`` *(required)* +~~~~~~~~~~~~~~~~~~~~ + +The Onshape URL of the assembly to be exported. Be sure you are on the correct tab when copying the URL. + +``output_format`` *(required)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +**required** + +This should be either ``urdf`` or ``mujoco`` to specify which output format is wanted for robot description +created by the export. + +``output_filename`` *(default: robot)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +This is the name of the output file without extension. By default "robot" (for example: ``robot.urdf``, ``robot.sdf`` or ``robot.xml``). + +``assets_directory`` *(default: "assets")* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +This is the directory where the assets (like meshes) will be stored. + +``assembly_name`` *(optional)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +This can be used to specify the name of the assembly (in the Onshape document) to be used for robot export. + +If this is not provided, ``onshape-to-robot`` will list the assemblies. If more than one assembly is found, +an error will be raised. + +``document_id`` *(optional)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +If you don't specify the URL, this is the onshape ID of the document to be imported. It can be found in the Onshape URL, +just after ``document/``. + +.. code-block:: bash + + https://cad.onshape.com/documents/XXXXXXXXX/w/YYYYYYYY/e/ZZZZZZZZ + ^^^^^^^^^ + This is the document id + +``version_id`` *(optional)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +If you don't specify the URL, this argument can be used to use a specific version of the document instead of the last one. The version ID +can be found in URL, after the ``/v/`` part when selecting a specific version in the tree. + +If it is not specified, the workspace will be retrieved and the live version will be used. + +``workspace_id`` *(optional)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +If you don't specify the URL, this argument can be used to use a specific workspace of the document. This can be used for specific branches +ofr your robot without making a version. +The workspace ID can be found in URL, after the ``/w/`` part when selecting a specific version in the tree. + +``element_id`` *(optional)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +If you don't specify the URL, this argument can be used to use a specific element of the document. +The element ID can be found in URL, after the ``/e/`` part when selecting a specific version in the tree. + +``configuration`` *(default: "default")* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +This is the robot configuration string that will be passed to Onshape. Lists, booleans and quantities are allowed. For example: + +.. image:: _static/img/configuration.png + :width: 300px + :align: center + +Should be written as the following: + +.. code-block:: text + + Configuration=Long;RemovePart=true;Length=30mm + +.. note:: + + Alternatively, you can specify the configuration as a dictionary: + + .. code-block:: json + + { + // ... + "configuration": { + "Configuration": "Long", + "RemovePart": true, + "Length": "30mm" + } + } + + +``robot_name`` *(default: "dirname")* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Specifies the robot name. This value is typically present in the header of the exported files. + +If it is not specified, the directory name will be used. + +``ignore_limits`` *(default: false)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +If set to ``true``, the joint limits coming from Onshape will be ignored during export. + +``ignore`` *(default: {})* +~~~~~~~~~~~~~~~~~~~~~~~~~~ + +This can be a list of parts that you want to be ignored during the export. + +Alternatively, you can use a dict, where the values are either ``all``, ``visual`` or ``collision``. The rules will apply in order of appearance. + +You can use wildcards ``*`` to match multiple parts. + +You can prefix the part name with ``!`` to exclude it from the rule. For example, the following will ignore all parts for visual, except the ``leg`` part, turning the ignore list to a whitelist: + +.. code-block:: json + + { + // Ignore everything from visual + "*": "collision", + // Except the leg part + "!leg": "collision" + } + +.. note:: + + The dynamics of the part will not be ignored, but the visual and collision aspect will. + +.. _draw-frames: + +``draw_frames`` *(default: false)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +When , the part that is used for positionning the frame is +by default excluded from the output description (a dummy link is kept instead). Passing this option to ``true`` will +keep it instead. + +``no_dynamics`` *(default: false)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +This flag can be set if there is no dynamics. In that case all masses and inertia will be set to 0. +In pyBullet, this will result in static object (think of some environment for example). + + +``color`` *(default: None)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Can override the color for parts (should be an array: ``[r, g, b]`` with numbers from 0 to 1) + +``include_configuration_suffix`` *(default: true)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +When this flag is set to ``true`` (default), configurations will be added as a suffix to the part names and STL files. + +``post_import_commands`` *(default: [])* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +This is an array of commands that will be executed after the import is done. It can be used to be sure that +some processing scripts are run everytime you run onshape-to-robot. + +``processors`` *(default: None)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +See :ref:`custom processors ` for more information. + +``round_decimals`` *(default: 12)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Numbers displayed in export will be rounded up using `round()` method. The number of decimals that are kept can be controlled using this parameters. diff --git a/docs/source/custom_processors.rst b/docs/source/custom_processors.rst new file mode 100644 index 0000000..fe44b1b --- /dev/null +++ b/docs/source/custom_processors.rst @@ -0,0 +1,56 @@ +.. _custom_processors: + +Writing & registering custom Processor +====================================== + +Introduction +------------ + +In this documentation, you can find the description of all processors that are `registered by default `_. You can also write your own processor, by writing a class and registering it in the ``config.json`` file as described below. + + +Minimal processor example +------------------------- + +Below is a minimal example of processor you can write: + +.. code-block:: python + + # my_project/my_custom_processor.py + from onshape_to_robot.processor import Processor + from onshape_to_robot.config import Config + from onshape_to_robot.robot import Robot + + class MyCustomProcessor(Processor): + def __init__(self, config: Config): + super().__init__(config) + + self.use_my_custom: bool = config.get("use_my_custom", False) + + def process(self, robot: Robot): + if self.use_my_custom: + print(f"Custom processing for {robot.name} with custom processor.") + +Remember that processors processes the robot intermediate representation, that you can find in `robot.py `_. + +Registering your processor(s) +----------------------------- + +To register your processor, use the ``processors`` entry in ``config.json``: + +.. code-block:: javascript + + { + "processors": [ + // Custom processor + "my_project.my_custom_processor:MyCustomProcessor", + // Default processors + "ProcessorScad", + "ProcessorMergeParts", + "ProcessorNoCollisionMeshes" + ] + } + +.. note:: + + The list of existing default processors can be found in `processors.py `_. \ No newline at end of file diff --git a/docs/source/design.rst b/docs/source/design.rst new file mode 100644 index 0000000..0be80ce --- /dev/null +++ b/docs/source/design.rst @@ -0,0 +1,120 @@ +Design-time considerations +========================== + +Workflow overview +----------------- + +In order to make your robot possible to export, you need to follow some conventions. The summary is as follows: + +* ``onshape-to-robot`` exports an **assembly** of the robot, +* Be sure this assembly is a **top-level assembly**, where instances are robot links (they can be parts or sub-assemblies), +* The **first instance** in the assembly list will be considered as the base link, +* All the instances in the assembly will become links in the export +* **Mate connectors** should have special names (see below for details): + + * ``dof_name``: for degrees of freedom + * ``frame_name``: to create a frame (site in MuJoCo) + * ``fix_name``: fix two links together, causing ``onshape-to-robot`` to merge them + * ``closing_name``: to close a kinematic loop (see :ref:`kinematic-loops`) + * Other mates are not considered by ``onshape-to-robot`` + +* Orphaned links (that are not part of the kinematic chain) will be **fixed to the base link**, with a warning + +.. image:: _static/img/design.png + :align: center + +Specifying degrees of freedom +----------------------------- + +To create a degree of freedom, you should use the ``dof_`` prefix when placing a mate connector. + +* If the mate connector is **cylindrical** or **revolute**, a ``revolute`` joint will be issued +* If the mate connector is a **slider**, a ``prismatic`` joint will be issued +* If the mate connector is **fastened**, a ``fixed`` joint will be issued + +.. note:: + + You can specify joint limits in Onshape, they will be understood and exported + +Inverting axis orientation +-------------------------- + +You sometime might want your robot joint to rotate in the opposite direction than the one in the Onshape assembly. + +To that end, use the ``inv`` suffix in the mate connector name. For instance, ``dof_head_pitch_inv`` will result in a joint named ``head_pitch`` having the axis inverted with the one from the Onshape assembly. + +Naming links +------------ + +If you create a mate connector and name it ``link_something``, the link corresponding to the instance +on which it is attached will be named ``something`` in the resulting export. + +.. _custom-frames: + +Adding custom frames in your model +---------------------------------- + +You can add your own custom frames (such as the end effector or the tip of a leg) to your model. + +* In URDF, it will produce a *dummy link* connected to the parent link with a fixed joint +* In SDF, a ``frame`` element will be added +* In MuJoCo, it will result in a *site* + +To do so, either: + +* Add a mate connector where you want your frame to be, and name it ``frame_something``, where ``something`` is the name of your frame + +**OR** + +* Add any relation between a body representing your frame and the body you want to attach it to. Name this relation ``frame_something``. + +.. image:: _static/img/frames.png + :align: center + :class: padding + + +Here is a document that can be used (be sure to turn on "composite parts" when inserting it, use the ``frame`` composite part): `Onshape frame part `_ + +.. note:: + + The instance used for frame representation is only here for visualization purpose and is excluded from the robot. + You can however include it by setting :ref:`draw_frames ` to ``true`` in the :doc:`config ` file, mostly for debugging purposes. + +Joint frames +------------ + +Joint frames are the ones you see in Onshape when you click on the joint in the tree on the left. +Thus, they are always revolving around the z axis, or translating along the *z axis*. + +.. image:: _static/img/zaxis.png + :align: center + +.. _fixed-robot: + +Fixed robot +----------- + +If you want to export a robot that is fixed to the ground, use the "Fixed" feture of Onshape: + +.. image:: _static/img/fixed.png + :align: center + :class: padding + +Robot with multiple base links +------------------------------ + +The robot can have multiple links. In that case, the first instance appearing on the list will be considered as a separate base link. + +.. note:: + + MuJoCo and SDF both supports multiple base links, while URDF doesn't. + + In that case, you might consider using multiple URDF files, or :ref:`adding a dummy base link`. However, this will fix all the base links to the base link without freedom. + +Gear relations +-------------- + +Gear relations are exported by onshape-to-robot. Be sure to click the **source joint** first, and then the **target joint**. They will be exported as ```` in :doc:`URDF ` and :doc:`SDF ` formats, and as equality constraints in :doc:`MuJoCo `. + +.. image:: _static/img/gear.png + :align: center diff --git a/docs/source/exporter_mujoco.rst b/docs/source/exporter_mujoco.rst new file mode 100644 index 0000000..5e5b279 --- /dev/null +++ b/docs/source/exporter_mujoco.rst @@ -0,0 +1,144 @@ +.. _exporter-mujoco: + +MuJoCo +====== + +Introduction +------------- + +MuJoCo is a standard physics simulator, coming with an extensive description format. + +* Frames will be added as ``site`` tags in the MuJoCo XML file. +* *Actuators* will be created for all actuated joints (see below). +* When :ref:`kinematic loops ` are present, they will be enforced using equality constraints. + + * If the loop is achieved using a ``fixed`` connector, a ``weld`` constraint will be added. + * If a ``ball`` joint is used, a ``connect`` constraint will be added + * If a ``revolute`` joint is used, two ``connect`` constraints will be used + +* Additionally to the ``robot.xml`` file, a ``scene.xml`` file will be produced, adding floor and lighting useful for testing purpose. + +``config.json`` entries (MuJoCo) +-------------------------------- + +Here is an example of complete ``config.json`` file, with details below: + +.. code-block:: javascript + + { + "url": "document-url", + "output_format": "mujoco", + // ... + // General import options (see config.json documentation) + // ... + + // Additional XML file to be included in the URDF (default: "") + "additional_xml": "my_custom_file.xml", + + // Override joint properties (default: {}) + "joint_properties": { + // Default properties for all joints + "*": { + "actuated": true, + "forcerange": 10.0, + "frictionloss": 0.5, + "limits": [0.5, 1.2] + // ... + }, + // Set the properties for a specific joint + "joint_name": { + "forcerange": 20.0, + "frictionloss": 0.1 + // ... + } + }, + + // Override geometry properties (default: {}) + "geom_properties": { + // Set properties for specific links using pattern matching + "foot": { + "collision": { + "name": "left_foot_collision", + "friction": "1.2 0.005 0.0001" + } + }, + // Wildcard patterns are supported + "leg_*": { + "collision": { + "solimp": "0.9 0.95 0.001", + "solref": "0.02 1" + }, + "visual": { + "rgba": "1 0 0 1" + } + } + }, + + // Override equality attributes + "equalities": { + "closing_branch*": { + "solref": "0.002 1", + "solimp": "0.99 0.999 0.0005 0.5 2" + } + } + } + +``joint_properties`` *(default: {})* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Allow to specify the properties of the joints produced in the URDF output. The key should be joint names. The special ``default`` key will set default values for each joints. + +Possible values are: + +* ``actuated``: *(default: true)* whether an actuator should be associated to this joint, +* ``class``: a ``class="..."`` to be added to the joint (and actuator) +* ``type`` *(default: position)* defines the actuator that will be produced +* ``range`` *(default: true)*: if ``true``, the joint limits are reflected on the joint ``range`` attribute +* ``limits``: Override the joint limits, should be a list of two values (min, max) + +* The following are reflected as ```` attributes: + + * ``frictionloss`` + * ``damping`` + * ``armature`` + * ``stiffness`` + +* The following are reflected as actuator (```` or other) attributes: + + * ``kp``, ``kv`` and ``dampratio`` gains + * ``forcerange`` + +``geom_properties`` *(default: {})* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Allow to specify the properties of the geometries (collision and visual) produced in the MuJoCo output. The key should be part names with support for wildcard pattern matching. + +Properties can be specified separately for ``visual`` and ``collision`` geometries, or applied to both if not nested. + +Wildcard patterns (``*``, ``?``, ``[seq]``) are supported for matching part names. When multiple patterns match, properties are merged in order with later matches overriding earlier ones. + +All properties are added as XML attributes to the ```` tag. Common MuJoCo geom attributes include: + +* ``name``: Override the geometry name +* ``friction``: Friction coefficients (e.g., ``"1.2 0.005 0.0001"``) +* ``solimp``: Solver impedance parameters (e.g., ``"0.9 0.95 0.001"``) +* ``solref``: Solver reference parameters (e.g., ``"0.02 1"``) +* ``contype``: Contact type bitmask (e.g., ``"1"``) +* ``conaffinity``: Contact affinity bitmask (e.g., ``"1"``) +* ``rgba``: Color and transparency (e.g., ``"1 0 0 1"``) + +``equalities`` *(default: {})* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +This entry allows to override the equality attributes of the MuJoCo XML file. The key should be the equality name (which might contains wildcards ``*``), and the value should be a dictionary of attributes. + +This can be used to adjust the ``solref`` and ``solimp`` attributes of the equality constraints. + +``additional_xml`` *(default: "")* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +If you want to include additional XML in the URDF, you can specify the path to the file here. This file will be included in the produced XML. + +.. note:: + + Alternatively, ``additional_xml`` can be a list of files diff --git a/docs/source/exporter_sdf.rst b/docs/source/exporter_sdf.rst new file mode 100644 index 0000000..9cde323 --- /dev/null +++ b/docs/source/exporter_sdf.rst @@ -0,0 +1,107 @@ +SDF +=== + +Introduction +------------- + +The `SDF format `_ is an extension of URDF extensively used in ROS. + + +* When using SDF, frames will be exported as ```` items. +* The links and joints always using ``relative_to`` attribute to specify the parent frame, keeping the same coordinates system as in URDF. +* Additionally to the ``robot.sdf`` file, a ``model.config`` file will be produced, adding metadata useful for Gazebo. + +``config.json`` entries (SDF) +----------------------------- + +Here is an example of complete ``config.json`` file, with details below: + +.. code-block:: javascript + + { + "url": "document-url", + "output_format": "sdf", + // ... + // General import options (see config.json documentation) + // ... + + // Additional XML file to be included in the URDF (default: "") + "additional_xml": "my_custom_file.xml", + + // Override joint properties (default: {}) + "joint_properties": { + // Default properties for all joints + "*": { + "max_effort": 10.0, + "max_velocity": 6.0, + "friction": 0.5 + }, + // Set the properties for a specific joint + "joint_name": { + "max_effort": 20.0, + "max_velocity": 10.0, + "friction": 0.1, + "limits": [0.5, 1.2] + }, + "wheel": { + "type": "continuous" + } + }, + + // Override geometry properties (default: {}) + "geom_properties": { + // Set properties for specific links using pattern matching + "tibia": { + "collision": { + "mu": "1.2", + "mu2": "0.8" + } + }, + // Wildcard patterns are supported + "leg_*": { + "collision": { + "bounce": "0.5", + "max_contacts": "10" + } + } + }, + } + +``joint_properties`` *(default: {})* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Allow to specify the properties of the joints produced in the URDF output. The key should be joint names. The special ``default`` key will set default values for each joints. + +Possible values are: + +* ``max_effort``: The maximum effort that can be applied to the joint (added in the ```` tag) +* ``max_velocity``: The maximum velocity of the joint (added in the ```` tag) +* ``friction``: The friction of the joint (added in the ```` tag) +* ``type``: Sets the joint type (changing the ```` tag) +* ``limits``: Override the joint limits, should be a list of two values (min, max) + +``geom_properties`` *(default: {})* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Allow to specify the properties of the geometries (collision and visual) produced in the SDF output. The key should be part names with support for wildcard pattern matching. + +Properties can be specified separately for ``visual`` and ``collision`` geometries, or applied to both if not nested. + +Wildcard patterns (``*``, ``?``, ``[seq]``) are supported for matching part names. When multiple patterns match, properties are merged in order with later matches overriding earlier ones. + +All properties are added as nested XML elements within the ```` or ```` tags. Common SDF geometry properties include: + +* ``mu``, ``mu2``: Friction coefficients for collision geometries +* ``bounce``: Restitution coefficient for collision geometries +* ``max_contacts``: Maximum number of contact points for collision geometries +* ``kp``, ``kd``: Contact stiffness and damping parameters + +``additional_xml`` *(default: "")* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +If you want to include additional XML in the URDF, you can specify the path to the file here. This file will be included in the produced SDF file. + +.. note:: + + Alternatively, ``additional_xml`` can be a list of files + diff --git a/docs/source/exporter_urdf.rst b/docs/source/exporter_urdf.rst new file mode 100644 index 0000000..4f6152e --- /dev/null +++ b/docs/source/exporter_urdf.rst @@ -0,0 +1,119 @@ +URDF +==== + +Introduction +------------- + +URDF is a very standard format that can be exported by ``onshape-to-robot``. Below are the specific configuration entries that can be specified when using this format. + +When using this exporter, frames will be added as a *dummy links* attached to their body using a fixed joint + +``config.json`` entries (URDF) +------------------------------ + +Here is an example of complete ``config.json`` file, with details below: + +.. code-block:: javascript + + { + "url": "document-url", + "output_format": "urdf", + // ... + // General import options (see config.json documentation) + // ... + + // Package name (for ROS) (default: "") + "package_name": "my_robot", + // Additional XML file to be included in the URDF (default: "") + "additional_xml": "my_custom_file.xml", + // Exclude inertial data for fixed bodies (default: false) + "set_zero_mass_to_fixed": true, + + // Override joint properties (default: {}) + "joint_properties": { + // Default properties for all joints + "*": { + "max_effort": 10.0, + "max_velocity": 6.0, + "friction": 0.5 + }, + // Set the properties for a specific joint + "joint_name": { + "max_effort": 20.0, + "max_velocity": 10.0, + "friction": 0.1, + "limits": [0.5, 1.2] + }, + "wheel": { + "type": "continuous" + } + }, + + // Override geometry properties (default: {}) + "geom_properties": { + // Set properties for specific links using pattern matching + "tibia": { + "collision": { + "mu1": "1.2", + "mu2": "0.8" + } + }, + // Wildcard patterns are supported + "leg_*": { + "visual": { + "material": "leg_material" + } + } + }, + } + +``joint_properties`` *(default: {})* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Allow to specify the properties of the joints produced in the URDF output. The key should be joint names. The special ``default`` key will set default values for each joints. + +Possible values are: + +* ``max_effort``: The maximum effort that can be applied to the joint (added in the ```` tag) +* ``max_velocity``: The maximum velocity of the joint (added in the ```` tag) +* ``friction``: The friction of the joint (added in the ```` tag) +* ``type``: Sets the joint type (changing the ```` tag) +* ``limits``: Override the joint limits, should be a list of two values (min, max) + +``geom_properties`` *(default: {})* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Allow to specify the properties of the geometries (collision and visual) produced in the URDF output. The key should be part names with support for wildcard pattern matching. + +Properties can be specified separately for ``visual`` and ``collision`` geometries, or applied to both if not nested. + +Wildcard patterns (``*``, ``?``, ``[seq]``) are supported for matching part names. When multiple patterns match, properties are merged in order with later matches overriding earlier ones. + +All properties are added as nested XML elements within the ```` or ```` tags. Common URDF geometry properties include: + +* ``mu1``, ``mu2``: Friction coefficients for collision geometries +* ``kp``, ``kd``: Contact stiffness and damping parameters +* ``material``: Material reference for visual geometries + +``package_name`` *(default: "")* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +If you are exporting a URDF for ROS, you can specify the package name here. This will be used in the ```` tag. + +``additional_xml`` *(default: "")* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +If you want to include additional XML in the URDF, you can specify the path to the file here. This file will be included in the produced URDF file. + +.. note:: + + Alternatively, ``additional_xml`` can be a list of files + +``set_zero_mass_to_fixed`` *(default: false)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +This option sets the mass to 0 for bodies that are :ref:`fixed ` to the world. + +.. note:: + + In PyBullet, such bodies are indeed recognized as fixed diff --git a/docs/source/getting_started.rst b/docs/source/getting_started.rst new file mode 100644 index 0000000..1d54133 --- /dev/null +++ b/docs/source/getting_started.rst @@ -0,0 +1,116 @@ +Getting started +=============== + +Installing the package +---------------------- + +Run the following to install onshape-to-robot from `pypi `_: + +.. code-block:: bash + + pip install onshape-to-robot + +.. _api-key: + +Setting up your Authentication +------------------------------ + +Developer Keys +^^^^^^^^^^^^^^ + +You will need to obtain API key and secret from the +`My Account > Developer menu `_ + +API key must be set as environment variables. + +Using `.bashrc` +~~~~~~~~~~~~~~~ + +You can add something like this in your ``.bashrc``: + +.. code-block:: bash + + # .bashrc + # Obtained at https://dev-portal.onshape.com/keys + export ONSHAPE_API=https://cad.onshape.com + export ONSHAPE_ACCESS_KEY=Your_Access_Key + export ONSHAPE_SECRET_KEY=Your_Secret_Key + +Using `.env` file +~~~~~~~~~~~~~~~~~ + +Alternatively, you can also create a ``.env`` file in the root of your project: + +.. code-block:: bash + + # .env + # Obtained at https://dev-portal.onshape.com/keys + ONSHAPE_API=https://cad.onshape.com + ONSHAPE_ACCESS_KEY=Your_Access_Key + ONSHAPE_SECRET_KEY=Your_Secret_Key + +OAuth2 +^^^^^^ + +Onshape alternatively supports `OAuth2 authentication `_. To authenticate this app with an OAuth token, you +can instead set the following in `.barshrc` or `.env`: + +.. code-block:: bash + + ONSHAPE_API=https://cad.onshape.com + ONSHAPE_SECRET_BEARER=Your_Access_Key + +Setting up your export +---------------------- + +To export your own robot, first create a directory: + +.. code-block:: bash + + mkdir my-robot + +Then edit ``my-robot/config.json``, here is the minimum example: + +.. code-block:: json + + { + // Onshape URL of the assembly + "url": "https://cad.onshape.com/documents/11a7f59e37f711d732274fca/w/7807518dc67487ad405722c8/e/5233c6445c575366a6cc0d50", + // Output format + "output_format": "urdf" + } + +.. note:: + + The Onshape URL should be the one of your assembly. Be sure to be on the right tab when you copy it. + +Once this is done, run the following command: + +.. code-block:: bash + + onshape-to-robot my-robot + + +Testing your export +------------------- + +You can test your export by running (PyBullet): + +.. code-block:: bash + + onshape-to-robot-bullet my-robot + +Or (MuJoCo): + +.. code-block:: bash + + onshape-to-robot-mujoco my-robot + +What's next ? +------------- + +Before you can actually enjoy your export, you need to pay attention to the following: + +* ``onshape-to-robot`` comes with some conventions to follow, in order to understand what in your robot is a degree of freedom, a link, a frame, etc. Make sure to read the :doc:`design-time considerations `. +* There are some options you might want to specify in the :doc:`config.json ` file. +* Have a look at the `examples `_ available on GitHub. \ No newline at end of file diff --git a/docs/source/index.rst b/docs/source/index.rst new file mode 100644 index 0000000..c7ea311 --- /dev/null +++ b/docs/source/index.rst @@ -0,0 +1,40 @@ +Onshape-to-robot documentation +============================== + +.. raw:: html + +
+ +
+
+ + +What is this ? +~~~~~~~~~~~~~~ + +.. image:: _static/img/main.png + +``onshape-to-robot`` is a tool that allows you to export robots designed from the **Onshape CAD** software +to descriptions format like **URDF**, **SDF** or **MuJoCo**, so that you can use them for physics simulation or in your running code +(requesting frames, computing dynamics etc.) + +* `onshape-to-robot GitHub repository `_ +* `Robots examples GitHub repository `_ +* `onshape-to-robot on pypi `_ +* `video tutorial `_ (some information may be outdated) + +.. toctree:: + :maxdepth: 2 + :caption: Contents: + + getting_started + design + config + exporter_urdf + exporter_sdf + exporter_mujoco + kinematic_loops + processors + cache diff --git a/docs/source/kinematic_loops.rst b/docs/source/kinematic_loops.rst new file mode 100644 index 0000000..82f863e --- /dev/null +++ b/docs/source/kinematic_loops.rst @@ -0,0 +1,87 @@ +.. _kinematic-loops: + +Handling kinematic loops +======================== + +Some robots have *kinematic loops*, meaning that the kinematic chain is not a tree but a graph. + +Introduction +------------ + +Here is a 2D planar robot with kinematic loop, we assume the two first joints to be actuated and the others to +be passive: + +.. raw:: html + +
+ +
+
+ + +However, robot description are usually **trees**. To model this type of robot, we break it down to a tree. We attach **frames** to this tree, and need to enforce **run-time constraints**. + +.. image:: _static/img/opened_chain.png + :width: 300px + :align: center + + +Specifying closing constraints +------------------------------ + +While you could manually add :ref:`frames `, ``onshape-to-robot`` provides a more convenient way to handle kinematic loops: **mate connectors**. + +To achieve that, add a **mate** with the name ``closing_something``: + +.. image:: _static/img/loop.png + :align: center + :class: padding + + +Support for ```` in MuJoCo +------------------------------------ + +When using the :ref:`MuJoCo ` format, ``onshape-to-robot`` will add ```` constraints to enforce the kinematic loop. + +For example, the above robot can be exported using the following ``config.json``: + +.. code-block:: javascript + + { + // Document URL, MuJoCo output + "url": "https://cad.onshape.com/documents/04b05c47de7576f35c0e99b3/w/68041f3f5c827a258b40039c/e/db543f501b01adf8144064e3", + "output_format": "mujoco", + + // Disable the freejoint to fix the robot + "freejoint": false, + + // Don't create actuators for passive joints + "joint_properties": { + "passive1": {"actuated": false}, + "passive2": {"actuated": false} + } + } + +Here is the result of the export: + +.. raw:: html + +
+ +
+
+ + +Ressources +---------- + +Here are some ressources on how to handle kinematic loops in software: + +* `Onshape assembly `_ for the above example robot. +* MuJoCo `equality `_ constraints. +* In `pyBullet `_, you can use `createConstraint` method to add the relevant constraint. +* In the `PlaCo `_ solver, you can create a `RelativePositionTask`. See the `kinematics loop documentation section `_ for more details. Some examples created with onshape-to-robot can be found in the `example gallery `_. diff --git a/docs/source/processor_ball_to_euler.rst b/docs/source/processor_ball_to_euler.rst new file mode 100644 index 0000000..4de41f3 --- /dev/null +++ b/docs/source/processor_ball_to_euler.rst @@ -0,0 +1,43 @@ +Ball to Euler +============= + +Introduction +------------ + +This processor turns the ``ball`` joints to three revolute joints. This can be convenient if your downstream simulator or tool can't handle ``ball`` joints. + +``config.json`` entries +----------------------- + +.. code-block:: javascript + + { + // ... + // General import options (see config.json documentation) + // ... + + // Convert balls to Euler (default: false) + "ball_to_euler": true, + // Euler angles order (default: "xyz") + "ball_to_euler_order": "xyz", + } + +``ball_to_euler`` *(default: false)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +If set to ``true``, all ball joints will be converted to Euler. + +If you don't want to convert all your ball joints, you can specify a list of joints as follows: + +.. code-block:: javascript + + { + // Using specific joint lists, with wildcards + "ball_to_euler": ["joint1", "shoulder_*"], + } + +``ball_to_euler_order`` *(default: "xyz")* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +If set, the order of the euler angles will be set to the given value. The default is ``xyz``. +The order should be ``xyz``, ``xzy``, ``zyx``, ``zxy``, ``yxz`` or ``yzx``. \ No newline at end of file diff --git a/docs/source/processor_collision_as_visual.rst b/docs/source/processor_collision_as_visual.rst new file mode 100644 index 0000000..85726aa --- /dev/null +++ b/docs/source/processor_collision_as_visual.rst @@ -0,0 +1,32 @@ +Use collisions as visual +======================== + +Introduction +------------ + +If this processor is enabled, the items from collision will be also used as visual items. + +This can be used for: + +* Debugging purpose, when you have no convenient way to visualize collisions in your downstream tools +* Creating a model that is lighter to load + + +``config.json`` entries +----------------------- + +.. code-block:: javascript + + { + // ... + // General import options (see config.json documentation) + // ... + + // Removes collision meshes + "collisions_as_visual": true + } + +``collisions_as_visual`` *(default: false)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +If set to ``true``, collision will be used as visual. \ No newline at end of file diff --git a/docs/source/processor_convex_decomposition.rst b/docs/source/processor_convex_decomposition.rst new file mode 100644 index 0000000..53a5dce --- /dev/null +++ b/docs/source/processor_convex_decomposition.rst @@ -0,0 +1,34 @@ +Convex decomposition (CoACD) +============================ + +Introduction +------------ + +If this processor is enabled, the collision meshes will be decomposed using `CoACD `_ convex decomposition. + + +``config.json`` entries +----------------------- + +.. code-block:: javascript + + { + // ... + // General import options (see config.json documentation) + // ... + + // Enables convex decomposition + "convex_decomposition": true + // Use Rainbow colors instead of the part color + "rainbow_colors": true + } + +``convex_decomposition`` *(default: false)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +If set to ``true``, collision meshes will be decomposed using CoACD. + +``rainbow_colors`` *(default: false)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +If set to ``true``, the collision meshes will be colored using rainbow colors instead of the part color. \ No newline at end of file diff --git a/docs/source/processor_dummy_base_link.rst b/docs/source/processor_dummy_base_link.rst new file mode 100644 index 0000000..c58fb39 --- /dev/null +++ b/docs/source/processor_dummy_base_link.rst @@ -0,0 +1,28 @@ +.. _processor_dummy_base_link: + +Adding dummy base link +====================== + +Introduction +------------ + +If this processor is enabled, a dummy base called ``base_link`` will be added in your robot. The base will be attached to this link using a ``fixed`` joint. + +``config.json`` entries +----------------------- + +.. code-block:: javascript + + { + // ... + // General import options (see config.json documentation) + // ... + + // Add a dummy base link (default: false) + "add_dummy_base_link": true + } + +``add_dummy_base_link`` *(default: false)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +If set to ``true``, a dummy base link will be added to the robot. \ No newline at end of file diff --git a/docs/source/processor_fixed_links.rst b/docs/source/processor_fixed_links.rst new file mode 100644 index 0000000..9d3e283 --- /dev/null +++ b/docs/source/processor_fixed_links.rst @@ -0,0 +1,32 @@ +Using fixed links +================= + +Introduction +------------ + +If this processor is enabled, all parts will be separated in a link, associated with its parent using a ``fixed`` link. + +.. note:: + + Doing this is likely to result in poor performance in physics engine, but can be useful for debugging. + +``config.json`` entries +----------------------- + +.. code-block:: javascript + + { + // ... + // General import options (see config.json documentation) + // ... + + // Adding fixed links, resulting in one link per part + "use_fixed_links": true + } + +``use_fixed_links`` *(default: false)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +If set to ``true``, a dummy base link will be added to the robot. + +Alternatively, ``use_fixed_links`` can be a list of the links for which you want the fixed links to be added. \ No newline at end of file diff --git a/docs/source/processor_merge_parts.rst b/docs/source/processor_merge_parts.rst new file mode 100644 index 0000000..395a2a2 --- /dev/null +++ b/docs/source/processor_merge_parts.rst @@ -0,0 +1,30 @@ +.. _processor-merge-parts: + +Merge STLs +========== + +Introduction +------------ + +This processor merge the meshes of all parts in the links into a single one. + +``config.json`` entries +----------------------- + +.. code-block:: javascript + + { + // ... + // General import options (see config.json documentation) + // ... + + // Merge STL meshes (default: false) + "merge_stls": true + } + +``merge_stls`` *(default: false)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +If set to ``true``, each link parts meshes will be merged in a single one. The resulting part will be named after the link. + +You can also set it to ``"visual"`` to merge only the visual parts, or to ``"collision"`` to merge only the collision parts. \ No newline at end of file diff --git a/docs/source/processor_no_collision_meshes.rst b/docs/source/processor_no_collision_meshes.rst new file mode 100644 index 0000000..b137387 --- /dev/null +++ b/docs/source/processor_no_collision_meshes.rst @@ -0,0 +1,41 @@ +Removing collision meshes +========================= + +Introduction +------------ + +This processor ensure no collision meshes are rendered. + +.. note:: + + Alternatively, you can use ignore lists to ignore meshes from being processed: + + .. code-block:: json + + { + "ignore": { + "*": "collision" + } + } + + However, this will prevent the meshes from being available to other processors (for example, to be approximated with pure shapes). + + +``config.json`` entries +----------------------- + +.. code-block:: javascript + + { + // ... + // General import options (see config.json documentation) + // ... + + // Removes collision meshes + "no_collision_meshes": true + } + +``no_collision_meshes`` *(default: false)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +If set to ``true``, collision meshes will be removed. \ No newline at end of file diff --git a/docs/source/processor_scad.rst b/docs/source/processor_scad.rst new file mode 100644 index 0000000..8481fbe --- /dev/null +++ b/docs/source/processor_scad.rst @@ -0,0 +1,69 @@ +OpenSCAD pure shapes approximation +================================== + +Introduction +------------ + +This processor provides you a way to manually approximate your robot into pure shapes. + +You can follow the following `video tutorial `_. Some of the steps are outdated, but the general idea is still the same. + +Requirements +------------ + +For this processor to work, you need to install the OpenSCAD package: + +.. code-block:: bash + + sudo apt-get install openscad + +Process +------- + +When the OpenSCAD processor is enabled (see below), it will check for the presence of ``.scad`` files in the output directory. If some are present, they will be parsed and pure shapes will be exported. + +.. note:: + + By default, exporters will use pure shapes for collisions instead of meshes. + +You can use the following convenient command to run OpenSCAD on a specific ``.stl`` you want to approximate: + +.. code-block:: bash + + onshape-to-robot-edit-shape + +This will open a window similar to the following: + +.. image:: _static/img/pure-shape.png + :align: center + :width: 400px + +Editing the ``.scad`` file with the same name as the ``.stl`` file. Pure shapes present here will be used as approximation. + +``config.json`` entries +----------------------- + +.. code-block:: javascript + + { + // ... + // General import options (see config.json documentation) + // ... + + // Simplify STL meshes (default: false) + "use_scads": true, + // Can be used to enlarge/shrink the pure shapes (default: 0.0) + "pure_shape_dilatation": 0.0 + } + +``use_scads`` *(default: false)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +If set to ``true``, the processor will use OpenSCAD pure shapes approximation (see above) + +``pure_shape_dilatation`` *(default: 0.0)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +A float that can be used to enlarge or shrink the pure shapes. This can be useful to avoid collisions between parts. + +Use a negative value to shrink the shapes. \ No newline at end of file diff --git a/docs/source/processor_simplify_stls.rst b/docs/source/processor_simplify_stls.rst new file mode 100644 index 0000000..bdfd4c3 --- /dev/null +++ b/docs/source/processor_simplify_stls.rst @@ -0,0 +1,45 @@ +Simplify STLs +============= + +Introduction +------------ + +This processor will simplify the STLs files so that their size don't exceed a predefined limit. +Used with :ref:`processor-merge-parts`, it will simplify the merged STLs. + +Requirements +------------ + +For this processor to work, ensure pymeshlab is installed: + +.. code-block:: bash + + pip install pymeshlab + +``config.json`` entries +----------------------- + +.. code-block:: javascript + + { + // ... + // General import options (see config.json documentation) + // ... + + // Simplify STL meshes (default: false) + "simplify_stls": true, + // Maximum size of the STL files in MB (default: 3) + "max_stl_size": 1 + } + +``simplify_stls`` *(default: false)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +If set to ``true``, the STL files will be simplified. + +``max_stl_size`` *(default: 3)* +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The maximum size of the STL files in MB. If the size of a file exceeds this limit, it will be simplified. + +You can also set it to ``"visual"`` to simplify only the visual parts, or to ``"collision"`` to simplify only the collision parts. \ No newline at end of file diff --git a/docs/source/processors.rst b/docs/source/processors.rst new file mode 100644 index 0000000..04b772c --- /dev/null +++ b/docs/source/processors.rst @@ -0,0 +1,44 @@ +Processors +========== + +Introduction +~~~~~~~~~~~~ + +Here is an overview of ``onshape-to-robot`` pipeline: + +.. image:: _static/img/architecture.png + +* **(1)**: The assembly is retrieved from Onshape, to produce an intermediate representation of the robot. See the `robot.py `_ from the source code. +* **(2)**: Some operations can be applied on this representation, those are the **processors**, some of them are listed below. +* **(3)**: The robot is exported to the desired format (URDF, MuJoCo, etc.) using an **exporter**. + +.. note:: + + If you want to tweak your robot in the process, do not hesitate to have a look at the `export.py `_ script, which is the entry point of the ``onshape-to-robot`` command, and summarize the above-listed steps. + +Retrieve and Convert Modes +~~~~~~~~~~~~~~~~~~~~~~~~~~ + +It is possible to only execute the retrieval step **(1)**, by passing the ``--retrieve`` argument to the ``onshape-to-robot`` command. This will save the intermediate representation of the robot to a file named ``robot.pkl`` in the output directory. + +Similarly, steps **(2)** and **(3)** can be executed by passing the ``--convert`` argument, which will load the robot from the ``robot.pkl`` file. Processors will then be executed and the exported will produce the final output. This can be convenient to avoid sending API requests to Onshape while tweaking processors or exporters. + +You can also use the ``--save-pickle`` argument to save the robot data after retrieval while still proceeding to conversion. + +Processors list +~~~~~~~~~~~~~~~ + +.. toctree:: + :maxdepth: 2 + :caption: Processors: + + processor_ball_to_euler + processor_merge_parts + processor_simplify_stls + processor_scad + processor_dummy_base_link + processor_no_collision_meshes + processor_collision_as_visual + processor_convex_decomposition + processor_fixed_links + custom_processors diff --git a/docs/watch.sh b/docs/watch.sh new file mode 100644 index 0000000..4f54082 --- /dev/null +++ b/docs/watch.sh @@ -0,0 +1,15 @@ +#!/bin/bash + +make html +echo "Starting watching..." +killall php +cd build/html +php -S localhost:8080 & +cd ../.. + +while [ true ] +do + inotifywait source/*.rst + make html + sleep 0.5 +done \ No newline at end of file diff --git a/onshape_to_robot/__init__.py b/onshape_to_robot/__init__.py new file mode 100644 index 0000000..9017b3c --- /dev/null +++ b/onshape_to_robot/__init__.py @@ -0,0 +1,3 @@ +"""Export models from Onshape to other robotics formats.""" + +from . import export, bullet, mujoco, clear_cache, edit_shape, pure_sketch diff --git a/onshape_to_robot/assembly.py b/onshape_to_robot/assembly.py new file mode 100644 index 0000000..fd0fffc --- /dev/null +++ b/onshape_to_robot/assembly.py @@ -0,0 +1,951 @@ +from __future__ import annotations +import numpy as np +from .config import Config +from .message import error, info, bright, success, warning +from .onshape_api.client import Client +from .robot import Joint +from .expression import ExpressionParser + +INSTANCE_IGNORE = -1 + + +class Frame: + """ + Represents a frame attached + """ + + def __init__(self, body_id: int, name: str, T_world_frame: np.ndarray): + self.body_id: int = body_id + self.name: str = name + self.T_world_frame: np.ndarray = T_world_frame + + +class DOF: + """ + Represents a DOF + """ + + def __init__( + self, + body1_id: int, + body2_id: int, + name: str, + joint_type: str, + T_world_mate: np.ndarray, + limits: tuple | None, + axis: np.ndarray = np.array([0.0, 0.0, 1.0]), + ): + self.body1_id: int = body1_id + self.body2_id: int = body2_id + self.name: str = name + self.joint_type: str = joint_type + self.T_world_mate: np.ndarray = T_world_mate + self.limits: tuple | None = limits + self.axis: np.ndarray = axis + + def flip(self, flip_limits: bool = True): + if flip_limits and self.limits is not None: + self.limits = (-self.limits[1], -self.limits[0]) + + # Flipping the joint around X axis + flip = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]) + self.T_world_mate[:3, :3] = self.T_world_mate[:3, :3] @ flip + + def other_body(self, body_id: int): + if body_id == self.body1_id: + return self.body2_id + elif body_id == self.body2_id: + return self.body1_id + else: + raise Exception(f"ERROR: body {body_id} is not part of this DOF") + + +class Assembly: + """ + Main entry point to process an assembly + """ + + def __init__(self, config: Config): + self.config: Config = config + + # Creating Onshape API client + self.client = Client(logging=False, creds=self.config.config_file) + self.expression_parser = ExpressionParser() + self.expression_parser.variables_lazy_loading = self.load_variables + + self.document_id: str = config.document_id + self.workspace_id: str | None = config.workspace_id + self.version_id: str | None = config.version_id + + # All (raw) data from assembly + self.assembly_data: dict = {} + # Map a (top-level) instance id to a body id + self.current_body_id: int = 0 + self.instance_body: dict[str, int] = {} + # Frames object + self.frames: list[Frame] = [] + # Loop closure constraints + self.closures: list = [] + # Degrees of freedom + self.dofs: list[DOF] = [] + # Features data + self.features: dict = {} + # Configuration values + self.configuration_parameters: dict = {} + # Dictionnary mapping items to their children in the tree + self.tree_children: dict = {} + # Root nodes + self.root_nodes: list = [] + # Overriden link names + self.link_names: dict[int, str] = {} + # Relation indexed by target joints, values are [source joint, ratio] + self.relations: dict = {} + + self.ensure_workspace_or_version() + self.find_assembly() + self.check_configuration() + self.retrieve_assembly() + self.find_instances() + self.load_features() + self.load_configuration() + self.process_mates() + self.build_trees() + self.find_relations() + print("") + + def ensure_workspace_or_version(self): + """ + Ensure either a workspace id or a version id is set + If none, try to retrieve the current workspace ID from API + """ + if self.version_id: + print(bright(f"* Using configuration version ID {self.version_id} ...")) + elif self.workspace_id: + print(bright(f"* Using configuration workspace ID {self.workspace_id} ...")) + else: + print( + bright( + "* Not workspace ID specified, retrieving the current workspace ..." + ) + ) + document = self.client.get_document(self.config.document_id) + self.workspace_id = document["defaultWorkspace"]["id"] + print(success(f"+ Using workspace id: {self.workspace_id}")) + + def find_assembly(self): + """ + Find the wanted assembly from the document + """ + if self.config.element_id: + print( + bright(f"* Using configuration element ID {self.config.element_id} ...") + ) + self.element_id = self.config.element_id + return + + print( + bright( + "\n* Retrieving elements in the document, searching for the assembly..." + ) + ) + + elements = self.client.list_elements( + self.document_id, + self.version_id if self.version_id else self.workspace_id, + "v" if self.version_id else "w", + ) + + self.element_id = None + assemblies: dict = {} + for element in elements: + if element["type"] == "Assembly": + assemblies[element["name"]] = element["id"] + + if self.config.assembly_name: + if self.config.assembly_name in assemblies: + self.element_id = assemblies[self.config.assembly_name] + else: + raise Exception( + f"ERROR: Unable to find required assembly {self.config.assembly_name} in this document" + ) + else: + if len(assemblies) == 0: + raise Exception("ERROR: No assembly found in this document\n") + elif len(assemblies) == 1: + self.element_id = list(assemblies.values())[0] + else: + raise Exception( + f"ERROR: Multiple assemblies found, please specify the assembly name\n" + + ' to export (use "assemblyName" in the configuration file)\n' + + f" Available assemblies: {', '.join(assemblies.keys())}" + ) + + if self.element_id == None: + raise Exception(f"ERROR: Unable to find assembly in this document") + + def check_configuration(self): + """ + Retrieve configuration items for given assembly and parsing config configuration + """ + + if self.config.configuration != "default": + # Retrieving available config parameters + elements = self.client.elements_configuration( + self.document_id, + self.version_id if self.version_id else self.workspace_id, + self.element_id, + wmv=("v" if self.version_id else "w"), + ) + + parameters = {} + for entry in elements["configurationParameters"]: + type_name = entry["typeName"] + message = entry["message"] + + if type_name.startswith("BTMConfigurationParameterEnum"): + # The very first label typed is kept as the internal name for the enum, under the "option" + # key. However, the user label that can be changed later is "optionName" + option_names = [ + option["message"]["optionName"] for option in message["options"] + ] + options = [ + option["message"]["option"] for option in message["options"] + ] + parameters[message["parameterName"]] = [ + "enum", + message["parameterId"], + option_names, + options, + ] + elif type_name.startswith("BTMConfigurationParameterBoolean"): + parameters[message["parameterName"]] = ["bool"] + elif type_name.startswith("BTMConfigurationParameterQuantity"): + parameters[message["parameterName"]] = ["quantity"] + + # Parsing configuration + parts = self.config.configuration.split(";") + processed_configuration = [] + for part in parts: + kv = part.split("=") + if len(kv) == 2: + key, value = kv + if key not in parameters: + raise Exception( + f'ERROR: Unknown configuration parameter "{key}" in the configuration' + ) + if parameters[key][0] == "enum": + if value not in parameters[key][2]: + raise Exception( + f'ERROR: Unknown value "{value}" for configuration parameter "{key}"' + ) + + value = parameters[key][3][parameters[key][2].index(value)] + key = parameters[key][1] + processed_configuration.append(f"{key}={value.replace(' ', '+')}") + + # Re-writing the configuration + self.config.configuration = ";".join(processed_configuration) + + def retrieve_assembly(self): + """ + Retrieve all assembly data + """ + print(bright(f"* Retrieving assembly with id {self.element_id}")) + + self.assembly_data: dict = self.client.get_assembly( + self.document_id, + self.version_id if self.version_id else self.workspace_id, + self.element_id, + wmv=("v" if self.version_id else "w"), + configuration=self.config.configuration, + ) + + self.microversion_id: str = self.assembly_data["rootAssembly"][ + "documentMicroversion" + ] + self.occurrences: dict = {} + for occurrence in self.assembly_data["rootAssembly"]["occurrences"]: + self.occurrences[tuple(occurrence["path"])] = occurrence + + def find_instances(self, prefix: list = [], instances=None): + """ + Walking all the instances and associating them with their occurrences + """ + if instances is None: + instances = self.assembly_data["rootAssembly"]["instances"] + + for instance in instances: + if "type" in instance: + path = prefix + [instance["id"]] + self.get_occurrence(path)["instance"] = instance + + if instance["type"] == "Assembly": + if not instance["suppressed"]: + d = instance["documentId"] + m = instance["documentMicroversion"] + e = instance["elementId"] + c = instance["configuration"] + for sub_assembly in self.assembly_data["subAssemblies"]: + if ( + sub_assembly["documentId"] == d + and sub_assembly["documentMicroversion"] == m + and sub_assembly["elementId"] == e + and sub_assembly["configuration"] == c + ): + self.find_instances( + prefix + [instance["id"]], sub_assembly["instances"] + ) + + def load_features(self): + """ + Load features + """ + + self.features = self.client.get_features( + self.document_id, + self.microversion_id, + self.element_id, + wmv="m", + configuration=self.config.configuration, + ) + + self.matevalues = self.client.matevalues( + self.document_id, + self.version_id if self.version_id else self.workspace_id, + self.element_id, + wmv="v" if self.version_id else "w", + configuration=self.config.configuration, + ) + + def load_configuration(self): + """ + Load configuration parameters + """ + + self.variable_values = None + + # Extracting configuration variables + parts = self.assembly_data["rootAssembly"]["fullConfiguration"].split(";") + for part in parts: + key_value = part.split("=") + if len(key_value) == 2: + key, value = key_value + value = value.replace("+", " ") + self.configuration_parameters[key] = value + try: + param_value = self.expression_parser.eval_expr(value) + self.expression_parser.variables[key] = param_value + except ValueError: + pass + + def load_variables(self): + """ + Load variables values (only if needed) in the expression parser + """ + variables = self.client.get_variables( + self.document_id, + self.version_id if self.version_id else self.workspace_id, + self.element_id, + wmv="v" if self.version_id else "w", + configuration=self.config.configuration, + ) + for entry in variables: + for variable in entry["variables"]: + if variable["value"] is not None: + self.expression_parser.variables[variable["name"]] = ( + self.expression_parser.eval_expr(variable["value"]) + ) + + def get_occurrence(self, path: list): + """ + Retrieve occurrence from its path + """ + return self.occurrences[tuple(path)] + + def get_occurrence_transform(self, path: list) -> np.ndarray: + """ + Retrieve occurrence transform from its path + """ + T_world_part = np.array(self.get_occurrence(path)["transform"]).reshape(4, 4) + + return T_world_part + + def cs_to_transformation(self, cs: dict) -> np.ndarray: + """ + Convert a coordinate system to a transformation matrix + """ + T = np.eye(4) + T[:3, :3] = np.stack( + ( + np.array(cs["xAxis"]), + np.array(cs["yAxis"]), + np.array(cs["zAxis"]), + ) + ).T + T[:3, 3] = cs["origin"] + + return T + + def get_mate_transform(self, mated_entity: dict): + return self.cs_to_transformation(mated_entity["matedCS"]) + + def make_body(self, id: str): + """ + Make the given instance id a body + """ + self.instance_body[id] = self.current_body_id + self.current_body_id += 1 + + def merge_bodies(self, occurrence_A: str, occurrence_B: str): + # Ensure occurrences are body + if occurrence_A not in self.instance_body: + self.make_body(occurrence_A) + if occurrence_B not in self.instance_body: + self.make_body(occurrence_B) + + # Merging bodies + body1_id = self.instance_body[occurrence_A] + body2_id = self.instance_body[occurrence_B] + if body1_id > body2_id: + body1_id, body2_id = body2_id, body1_id + + for occurrence in self.instance_body: + if self.instance_body[occurrence] == body2_id: + self.instance_body[occurrence] = body1_id + + for dof in self.dofs: + if dof.body1_id == body2_id: + dof.body1_id = body1_id + if dof.body2_id == body2_id: + dof.body2_id = body1_id + + def translation(self, x: float, y: float, z: float) -> np.ndarray: + return np.array( + [ + [1, 0, 0, x], + [0, 1, 0, y], + [0, 0, 1, z], + [0, 0, 0, 1], + ] + ) + + def process_mates(self): + """ + Pre-assign all top-level instances to a separate body id + """ + top_level_instances = self.assembly_data["rootAssembly"]["instances"] + self.make_body(top_level_instances[0]["id"]) + + # We first search for DOFs + for data, occurrence_A, occurrence_B in self.feature_mating_two_occurrences(): + if data["name"].startswith("dof_"): + # Process the DOF name, removing dof prefix and inv suffix + parts = data["name"].split("_") + del parts[0] + data["inverted"] = False + if parts[-1] == "inv" or parts[-1] == "inverted": + data["inverted"] = True + del parts[-1] + name = "_".join(parts) + + if name == "": + raise Exception( + f"ERROR: the following dof should have a name {data['name']}" + ) + + # Finding joint type and limits + limits = None + if data["mateType"] == "REVOLUTE" or data["mateType"] == "CYLINDRICAL": + if "wheel" in parts or "continuous" in parts: + joint_type = Joint.CONTINUOUS + else: + joint_type = Joint.REVOLUTE + + if not self.config.ignore_limits: + limits = self.get_limits(joint_type, data["name"]) + elif data["mateType"] == "SLIDER": + joint_type = Joint.PRISMATIC + if not self.config.ignore_limits: + limits = self.get_limits(joint_type, data["name"]) + elif data["mateType"] == "FASTENED": + joint_type = Joint.FIXED + elif data["mateType"] == "BALL": + joint_type = Joint.BALL + if not self.config.ignore_limits: + limits = self.get_limits(joint_type, data["name"]) + else: + raise Exception( + f"ERROR: {name} is declared as a DOF but the mate type is {data['mateType']}\n" + + " Only REVOLUTE, CYLINDRICAL, SLIDER and FASTENED are supported" + ) + + # We compute the axis in the world frame + mated_entity = data["matedEntities"][0] + T_world_part = self.get_occurrence_transform( + mated_entity["matedOccurrence"] + ) + + # jointToPart is the (rotation only) matrix from joint to the part + # it is attached to + T_part_mate = self.get_mate_transform(mated_entity) + + T_world_mate = T_world_part @ T_part_mate + + limits_str = "" + if limits is not None: + limits_str = f"[{round(limits[0], 3)}: {round(limits[1], 3)}]" + print(success(f"+ Found DOF: {name} ({joint_type}) {limits_str}")) + + # Ensure occurrences are body + if occurrence_A not in self.instance_body: + self.make_body(occurrence_A) + if occurrence_B not in self.instance_body: + self.make_body(occurrence_B) + + dof = DOF( + self.instance_body[occurrence_A], + self.instance_body[occurrence_B], + name, + joint_type, + T_world_mate, + limits, + ) + + if data["inverted"]: + dof.flip() + + self.dofs.append(dof) + + # Merging fixed links + for data, occurrence_A, occurrence_B in self.feature_mating_two_occurrences(): + if data["name"].startswith("fix_") or ( + data["mateType"] == "FASTENED" + and not data["name"].startswith("dof_") + and not data["name"].startswith("closing_") + and not data["name"].startswith("frame_") + ): + self.merge_bodies(occurrence_A, occurrence_B) + + # Merging mate gorups + for group in self.feature_mate_groups(): + for k in range(1, len(group)): + occurrence_A = group[0] + occurrence_B = group[k] + + self.merge_bodies(occurrence_A, occurrence_B) + + # Processing frame mates + for data, occurrence_A, occurrence_B in self.feature_mating_two_occurrences(): + if data["name"].startswith("frame_"): + name = "_".join(data["name"].split("_")[1:]) + if ( + occurrence_A not in self.instance_body + and occurrence_B in self.instance_body + ): + parent, child = occurrence_B, occurrence_A + mated_entity = data["matedEntities"][0] + elif ( + occurrence_B not in self.instance_body + and occurrence_A in self.instance_body + ): + parent, child = occurrence_A, occurrence_B + mated_entity = data["matedEntities"][1] + else: + raise Exception( + f"Frame {name} should mate an orphan body to a body in the kinematics tree" + ) + + T_world_part = self.get_occurrence_transform( + mated_entity["matedOccurrence"] + ) + + self.frames.append( + Frame(self.instance_body[parent], name, T_world_part) + ) + + if self.config.draw_frames: + self.merge_bodies(parent, child) + else: + self.instance_body[child] = INSTANCE_IGNORE + + # Checking that all intances are assigned to a body + for instance in self.assembly_data["rootAssembly"]["instances"]: + if instance["id"] not in self.instance_body and not instance["suppressed"]: + self.make_body(instance["id"]) + + # Processing loop closing frames + for data, occurrence_A, occurrence_B in self.feature_mating_two_occurrences(): + is_hinge_closure = data["mateType"] == "REVOLUTE" + + if data["name"].startswith("closing_"): + for k in 0, 1: + mated_entity = data["matedEntities"][k] + occurrence = mated_entity["matedOccurrence"][0] + + T_world_part = self.get_occurrence_transform( + mated_entity["matedOccurrence"] + ) + T_part_mate = self.get_mate_transform(mated_entity) + T_world_mate = T_world_part @ T_part_mate + + self.frames.append( + Frame( + self.instance_body[occurrence], + f"{data['name']}_{k+1}", + T_world_mate, + ) + ) + + if is_hinge_closure: + self.frames.append( + Frame( + self.instance_body[occurrence], + f"{data['name']}_{k+1}_z", + T_world_mate @ self.translation(0, 0, 0.1), + ) + ) + + closure_types = { + "FASTENED": "fixed", + "REVOLUTE": "revolute", + "BALL": "ball", + "SLIDER": "slider", + } + + self.closures.append( + [ + closure_types.get(data["mateType"], "unknown"), + f"{data['name']}_1", + f"{data['name']}_2", + ] + ) + if is_hinge_closure: + self.closures.append( + [ + closure_types.get(data["mateType"], "unknown"), + f"{data['name']}_1_z", + f"{data['name']}_2_z", + ] + ) + + # Search for mate connector named "link_..." to override link names + for feature in self.assembly_data["rootAssembly"]["features"]: + # Suppressed mate connectors reference occurrences that may no longer + # exist in the assembly, so skip them like mates and mate groups do. + if feature.get("suppressed"): + continue + + if feature["featureType"] == "mateConnector" and feature["featureData"][ + "name" + ].startswith("link_"): + link_name = "_".join(feature["featureData"]["name"].split("_")[1:]) + body_id = self.instance_body[feature["featureData"]["occurrence"][0]] + self.link_names[body_id] = link_name + + if feature["featureType"] == "mateConnector" and feature["featureData"][ + "name" + ].startswith("frame_"): + name = "_".join(feature["featureData"]["name"].split("_")[1:]) + occurrence = feature["featureData"]["occurrence"] + T_world_occurrence = self.get_occurrence_transform(occurrence) + body_id = self.instance_body[occurrence[0]] + T_occurrence_mate = self.cs_to_transformation( + feature["featureData"]["mateConnectorCS"] + ) + T_world_mate = T_world_occurrence @ T_occurrence_mate + self.frames.append(Frame(body_id, name, T_world_mate)) + + print(success(f"* Found total {len(self.dofs)} degrees of freedom")) + + def build_trees(self): + """ + Perform checks on the produced tree + """ + self.body_in_tree = [] + for body_id in self.instance_body.values(): + if body_id != INSTANCE_IGNORE and body_id not in self.body_in_tree: + self.build_tree(body_id) + + print(success(f"* Found {len(self.root_nodes)} root nodes:")) + for root_node in self.root_nodes: + print(success(f" - {self.body_instance(root_node)['name']}")) + + def build_tree(self, root_node: int): + """ + Building a tree starting a root_node + """ + # Append the root node + self.root_nodes.append(root_node) + + # Checking that the graph is actually a tree (no loop) + exploring = [root_node] + dofs = self.dofs.copy() + while len(exploring) > 0: + current = exploring.pop() + self.body_in_tree.append(current) + + children = [] + dofs_to_remove = [] + for dof in dofs: + if dof.body1_id == current: + dof.flip(flip_limits=False) + children.append(dof.body2_id) + dofs_to_remove.append(dof) + elif dof.body2_id == current: + children.append(dof.body1_id) + dofs_to_remove.append(dof) + for dof in dofs_to_remove: + dofs.remove(dof) + + self.tree_children[current] = children + for child in children: + if child in self.body_in_tree: + raise Exception( + "The DOF graph is not a tree, check for loops in your DOFs" + ) + elif child not in exploring: + exploring.append(child) + + def feature_mating_two_occurrences(self): + """ + Iterate over all valid mating feature with two occurrences + """ + for feature in self.assembly_data["rootAssembly"]["features"]: + if feature["featureType"] == "mate" and not feature["suppressed"]: + data = feature["featureData"] + + if ( + "matedEntities" not in data + or len(data["matedEntities"]) != 2 + or len(data["matedEntities"][0]["matedOccurrence"]) == 0 + or len(data["matedEntities"][1]["matedOccurrence"]) == 0 + ): + continue + + occurrence_A = data["matedEntities"][0]["matedOccurrence"][0] + occurrence_B = data["matedEntities"][1]["matedOccurrence"][0] + + yield data, occurrence_A, occurrence_B + + def feature_mate_groups(self): + """ + Find mate groups in the assembly + """ + groups = [] + + for feature in self.assembly_data["rootAssembly"]["features"]: + group = [] + if feature["featureType"] == "mateGroup" and not feature["suppressed"]: + data = feature["featureData"] + + for occurrence in data["occurrences"]: + group.append(occurrence["occurrence"][0]) + groups.append(group) + + return groups + + def get_feature_by_id(self, feature_id: str): + """ + Find a specific feature by its ID + """ + for feature in self.features["features"]: + if feature["message"]["featureId"] == feature_id: + return feature + + return None + + def find_relations(self): + """ + Finding relations features in the assembly + """ + for feature in self.features["features"]: + if feature["typeName"] == "BTMMateRelation": + relation_name = feature["message"]["name"] + + mated_dofs = None + ratio = None + reverse = None + for parameter in feature["message"]["parameters"]: + if parameter["message"]["parameterId"] == "matesQuery": + queries = parameter["message"]["queries"] + if len(queries) == 2: + dof1_feature = self.get_feature_by_id( + queries[0]["message"]["featureId"] + ) + dof2_feature = self.get_feature_by_id( + queries[1]["message"]["featureId"] + ) + if dof1_feature is not None and dof2_feature is not None: + dof1 = dof1_feature["message"]["name"] + dof2 = dof2_feature["message"]["name"] + if dof1.startswith("dof_") and dof2.startswith("dof_"): + mated_dofs = [dof1[4:], dof2[4:]] + elif parameter["message"]["parameterId"] == "relationRatio": + ratio = self.read_expression(parameter["message"]["expression"]) + elif parameter["message"]["parameterId"] == "reverseDirection": + reverse = parameter["message"]["value"] + + if mated_dofs is not None and ratio is not None and reverse is not None: + if not reverse: + ratio = -ratio + + print( + success( + f"+ Found relation {relation_name} mating {mated_dofs} with ratio {ratio}" + ) + ) + if mated_dofs[1] in self.relations: + print( + warning( + f"Multiple relations found with {mated_dofs[1]} as target" + ) + ) + + self.relations[mated_dofs[1]] = [mated_dofs[0], ratio] + + def read_parameter_value(self, parameter: str, name: str): + """ + Try to read a parameter value from Onshape + """ + + # This is an expression + if parameter["typeName"] == "BTMParameterNullableQuantity": + return self.read_expression(parameter["message"]["expression"]) + if parameter["typeName"] == "BTMParameterConfigured": + message = parameter["message"] + parameterValue = self.configuration_parameters[ + message["configurationParameterId"] + ] + + for value in message["values"]: + if value["typeName"] == "BTMConfiguredValueByBoolean": + booleanValue = parameterValue == "true" + if value["message"]["booleanValue"] == booleanValue: + return self.read_expression( + value["message"]["value"]["message"]["expression"] + ) + elif value["typeName"] == "BTMConfiguredValueByEnum": + if value["message"]["enumValue"] == parameterValue: + return self.read_expression( + value["message"]["value"]["message"]["expression"] + ) + else: + raise Exception( + "Can't read value of parameter {name} configured with {value['typeName']}" + ) + + print(error(f"Coud not find the value for {name}")) + else: + raise Exception(f"Unknown feature type for {name}: {parameter['typeName']}") + + def read_expression(self, expression: str): + """ + Reading an expression from Onshape + """ + return self.expression_parser.eval_expr(expression) + + def get_offset(self, name: str): + """ + Retrieve the offset from current joint position in the assembly + Currently, this only works with workspace in the API + """ + if self.matevalues is None: + return None + + for entry in self.matevalues["mateValues"]: + if entry["mateName"] == name: + if "rotationZ" in entry: + return entry["rotationZ"] + elif "translationZ" in entry: + return entry["translationZ"] + else: + print(warning(f"Unknown offset type for {name}")) + return None + + def get_limits(self, joint_type: str, name: str): + """ + Retrieve (low, high) limits for a given joint, if any + """ + enabled = False + minimum, maximum = 0, 0 + for feature in self.features["features"]: + # Find coresponding joint + if name == feature["message"]["name"]: + # Find min and max values + for parameter in feature["message"]["parameters"]: + if parameter["message"]["parameterId"] == "limitsEnabled": + enabled = parameter["message"]["value"] + + if enabled: + for parameter in feature["message"]["parameters"]: + if joint_type == Joint.REVOLUTE: + if parameter["message"]["parameterId"] == "limitAxialZMin": + minimum = self.read_parameter_value(parameter, name) + if parameter["message"]["parameterId"] == "limitAxialZMax": + maximum = self.read_parameter_value(parameter, name) + elif joint_type == Joint.PRISMATIC: + if parameter["message"]["parameterId"] == "limitZMin": + minimum = self.read_parameter_value(parameter, name) + if parameter["message"]["parameterId"] == "limitZMax": + maximum = self.read_parameter_value(parameter, name) + elif joint_type == Joint.BALL: + if ( + parameter["message"]["parameterId"] + == "limitEulerConeAngleMax" + ): + minimum = 0 + maximum = self.read_parameter_value(parameter, name) + else: + print( + warning( + f"WARNING: Can't read limits for a joint of type {joint_type}" + ) + ) + print(parameter) + if enabled: + if joint_type != Joint.BALL: + offset = self.get_offset(name) + if offset is not None: + minimum -= offset + maximum -= offset + return (minimum, maximum) + else: + if joint_type != Joint.CONTINUOUS: + print( + warning(f"WARNING: joint {name} of type {joint_type} has no limits") + ) + return None + + def body_instance(self, body_id: int): + """ + Get the (first) instance associated with a given body + """ + for instance in self.assembly_data["rootAssembly"]["instances"]: + if ( + instance["id"] in self.instance_body + and self.instance_body[instance["id"]] == body_id + ): + return instance + + return None + + def body_occurrences(self, body_id: int): + """ + Retrieve all occurrences associated to a given body id + """ + for occurrence in self.assembly_data["rootAssembly"]["occurrences"]: + key = occurrence["path"][0] + if key in self.instance_body and self.instance_body[key] == body_id: + yield occurrence + + def get_dof(self, body1_id: int, body2_id: int): + """ + Get a DOF for given bodies + """ + for dof in self.dofs: + if (dof.body1_id == body1_id and dof.body2_id == body2_id) or ( + dof.body1_id == body2_id and dof.body2_id == body1_id + ): + return dof + + raise Exception(f"ERROR: no DOF found between {body1_id} and {body2_id}") diff --git a/onshape_to_robot/assets/scene.xml b/onshape_to_robot/assets/scene.xml new file mode 100644 index 0000000..92f8123 --- /dev/null +++ b/onshape_to_robot/assets/scene.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/onshape_to_robot/bullet.py b/onshape_to_robot/bullet.py new file mode 100644 index 0000000..5f88eda --- /dev/null +++ b/onshape_to_robot/bullet.py @@ -0,0 +1,71 @@ +def main(): + import math + import sys + import os + import time + import argparse + import pybullet as p + from .simulation import Simulation + + parser = argparse.ArgumentParser(prog="onshape-to-robot-bullet") + parser.add_argument("-f", "--fixed", action="store_true") + parser.add_argument("-n", "--no-self-collisions", action="store_true") + parser.add_argument("-x", "--x", type=float, default=0) + parser.add_argument("-y", "--y", type=float, default=0) + parser.add_argument("-z", "--z", type=float, default=0) + parser.add_argument("directory") + args = parser.parse_args() + + robotPath = args.directory + if not robotPath.endswith(".urdf"): + robotPath += "/robot.urdf" + + sim = Simulation( + robotPath, + gui=True, + panels=True, + fixed=args.fixed, + ignore_self_collisions=args.no_self_collisions, + ) + pos, rpy = sim.getRobotPose() + _, orn = p.getBasePositionAndOrientation(sim.robot) + sim.setRobotPose([pos[0] + args.x, pos[1] + args.y, pos[2] + args.z], orn) + + controls = {} + for name in sim.getJoints(): + if name.endswith("_speed"): + controls[name] = p.addUserDebugParameter(name, -math.pi * 3, math.pi * 3, 0) + else: + infos = sim.getJointsInfos(name) + low = -math.pi + high = math.pi + if "lowerLimit" in infos: + low = infos["lowerLimit"] + if "upperLimit" in infos: + high = infos["upperLimit"] + controls[name] = p.addUserDebugParameter(name, low, high, 0) + + lastPrint = 0 + while True: + targets = {} + for name in controls.keys(): + targets[name] = p.readUserDebugParameter(controls[name]) + sim.setJoints(targets) + + if time.time() - lastPrint > 0.05: + lastPrint = time.time() + os.system("clear") + frames = sim.getFrames() + for frame in frames: + print(frame) + print("- x=%f\ty=%f\tz=%f" % frames[frame][0]) + print("- r=%f\tp=%f\ty=%f" % frames[frame][1]) + print("") + print("Center of mass:") + print(sim.getCenterOfMassPosition()) + + sim.tick() + + +if __name__ == "__main__": + main() diff --git a/onshape_to_robot/bullet/plane.obj b/onshape_to_robot/bullet/plane.obj new file mode 100644 index 0000000..6062095 --- /dev/null +++ b/onshape_to_robot/bullet/plane.obj @@ -0,0 +1,18 @@ +# Blender v2.66 (sub 1) OBJ File: '' +# www.blender.org +mtllib plane.mtl +o Plane +v 15.000000 -15.000000 0.000000 +v 15.000000 15.000000 0.000000 +v -15.000000 15.000000 0.000000 +v -15.000000 -15.000000 0.000000 + +vt 15.000000 0.000000 +vt 15.000000 15.000000 +vt 0.000000 15.000000 +vt 0.000000 0.000000 + +usemtl Material +s off +f 1/1 2/2 3/3 +f 1/1 3/3 4/4 diff --git a/onshape_to_robot/bullet/plane.urdf b/onshape_to_robot/bullet/plane.urdf new file mode 100644 index 0000000..b04f67e --- /dev/null +++ b/onshape_to_robot/bullet/plane.urdf @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/onshape_to_robot/clear_cache.py b/onshape_to_robot/clear_cache.py new file mode 100644 index 0000000..0a21446 --- /dev/null +++ b/onshape_to_robot/clear_cache.py @@ -0,0 +1,16 @@ +"""Clear the onshape-to-robot cache.""" + + +def main(): + import shutil + + from .onshape_api.cache import get_cache_path + + """Clear the onshape-to-robot cache.""" + cache_dir = get_cache_path() + print("Removing cache directory: {}".format(cache_dir)) + shutil.rmtree(cache_dir, ignore_errors=True) + + +if __name__ == "__main__": + main() diff --git a/onshape_to_robot/config.py b/onshape_to_robot/config.py new file mode 100644 index 0000000..9ba6e3e --- /dev/null +++ b/onshape_to_robot/config.py @@ -0,0 +1,209 @@ +from __future__ import annotations +import numpy as np +import re +import os +import commentjson as json + + +class Config: + def __init__(self, robot_path: str, safe: bool = False): + self.safe: bool = safe + self.config_file: str = robot_path + + if os.path.isdir(robot_path): + self.config_file += os.path.sep + "config.json" + + # Loading JSON configuration + if not os.path.exists(self.config_file): + raise Exception(f"ERROR: The file {self.config_file} can't be found") + with open(self.config_file, "r", encoding="utf8") as stream: + self.config: dict = json.load(stream) + + # Loaded processors + self.processors: list = [] + + self.read_configuration() + + # Output directory, making it if it doesn't exists + self.output_directory: str = os.path.dirname(os.path.abspath(self.config_file)) + + if self.robot_name is None: + self.robot_name = os.path.dirname(os.path.abspath(self.config_file)).split( + "/" + )[-1] + + try: + os.makedirs(self.output_directory) + except OSError: + pass + + def to_camel_case(self, snake_str: str) -> str: + """ + Converts a string to camel case + """ + components = snake_str.split("_") + return components[0] + "".join(x.title() for x in components[1:]) + + def get(self, name: str, default=None, required: bool = True, values_list=None): + """ + Gets an entry from the configuration + + Args: + name (str): entry name + default: default fallback value if the entry is not present. Defaults to None. + required (bool, optional): whether the configuration entry is required. Defaults to False. + values_list: list of allowed values. Defaults to None. + """ + camel_name = self.to_camel_case(name) + + if name in self.config or camel_name in self.config: + if name in self.config: + value = self.config[name] + else: + value = self.config[camel_name] + + if values_list is not None and value not in values_list: + raise Exception( + f"Value for {name} should be onf of: {','.join(values_list)}" + ) + return value + elif required and default is None: + raise Exception(f"ERROR: missing required key {name} in config") + + return default + + def printable_version(self) -> str: + if self.url is not None: + return self.url + else: + version = f"document_id: {self.document_id}" + if self.version_id: + version += f" / version_id: {self.version_id}" + elif self.workspace_id: + version += f" / workspace_id: {self.workspace_id}" + + return version + + def parse_url(self): + pattern = "https://(.*)/(.*)/([wv])/(.*)/e/(.*)" + match = re.match(pattern, self.url) + + if match is None: + raise Exception(f"Invalid URL: {self.url}") + + match_groups = match.groups() + self.document_id = match_groups[1] + if match_groups[2] == "w": + self.workspace_id = match_groups[3] + elif match_groups[2] == "v": + self.version_id = match_groups[3] + self.element_id = match_groups[4] + + def asset_path(self, asset_name: str) -> str: + return f"{self.output_directory}/{self.assets_directory}/{asset_name}" + + def read_configuration(self): + """ + Load and check configuration entries + """ + + # Robot name + self.robot_name: str = self.get("robot_name", None, required=False) + self.output_filename: str = self.get("output_filename", "robot") + # Securing filename + self.output_filename = "".join( + c for c in self.output_filename if c.isalnum() or c in ("_", "-") + ).rstrip() + self.assets_directory: str = self.get("assets_directory", "assets") + + # Main settings + self.document_id: str = self.get("document_id", required=False) + self.version_id: str | None = self.get("version_id", required=False) + self.workspace_id: str | None = self.get("workspace_id", required=False) + self.element_id: str | None = self.get("element_id", required=False) + + if self.version_id and self.workspace_id: + raise Exception("You can't specify workspace_id and version_id") + + self.url: str = self.get("url", None, required=False) + if self.url is not None: + self.parse_url() + + if self.url is None and self.document_id is None: + raise Exception("You need to specify either a url or a document_id") + + self.draw_frames: bool = self.get("draw_frames", False) + + self.assembly_name: str = self.get("assembly_name", required=False) + self.output_format: str = self.get("output_format") + self.configuration: str | dict = self.get("configuration", "default") + self.ignore_limits: bool = self.get("ignore_limits", False) + + if isinstance(self.configuration, dict): + self.configuration = ";".join( + [f"{k}={v}" for k, v in self.configuration.items()] + ) + + # Joint specs + self.joint_properties: dict = self.get("joint_properties", {}) + self.geom_properties: dict = self.get("geom_properties", {}) + self.no_dynamics: bool = self.get("no_dynamics", False) + + # Ignore / whitelists + self.ignore: list[str] = self.get("ignore", {}) + if isinstance(self.ignore, list): + self.ignore = {entry: "all" for entry in self.ignore} + + # Color override + self.color: str | None = self.get("color", required=False) + + # Post-import commands + self.post_import_commands: list[str] = self.get("post_import_commands", []) + + # Whether to include configuration suffix in part names + self.include_configuration_suffix: bool = self.get( + "include_configuration_suffix", True + ) + + # Number of decimals to keep for small numbers + self.round_decimals = self.get("round_decimals", 12) + + # Loading processors + from . import processors + + loaded_modules = {} + processors_list: list[str] | None = self.get("processors", None, required=False) + if processors_list is None or self.safe: + self.processors = [ + processor(self) + for processor in processors.default_processors + if (processor.is_safe or not self.safe) + ] + else: + for entry in processors_list: + parts = entry.split(":") + + if len(parts) == 1: + processor = eval(f"processors.{entry}") + else: + module, cls = parts + if module not in loaded_modules: + loaded_modules[module] = __import__(module, fromlist=[cls]) + processor = getattr(loaded_modules[module], cls) + + if processor is None: + raise Exception(f"ERROR: Processor {entry} not found") + + self.processors.append(processor(self)) + + def round(self, object: float | list | tuple | np.ndarray): + """ + Round the given number or list of numbers using the configuration decimals + """ + if isinstance(object, float): + return round(object, self.round_decimals) + elif isinstance(object, np.ndarray): + return object.round(self.round_decimals) + else: + original_type = type(object) + return original_type(np.array(object).round(self.round_decimals)) \ No newline at end of file diff --git a/onshape_to_robot/csg.py b/onshape_to_robot/csg.py new file mode 100644 index 0000000..fe89727 --- /dev/null +++ b/onshape_to_robot/csg.py @@ -0,0 +1,123 @@ +import json +import re +import os +import numpy as np + +""" +These functions are responsible for parsing CSG files (constructive solid geometry), which are files +produced by OpenSCAD, containing no loop, variables etc. +""" + + +def multmatrix_parse(parameters): + matrix = np.array(json.loads(parameters), dtype=float) + matrix[0, 3] /= 1000.0 + matrix[1, 3] /= 1000.0 + matrix[2, 3] /= 1000.0 + return matrix + + +def cube_parse(parameters, dilatation): + results = re.findall(r'^size = (.+), center = (.+)$', parameters) + if len(results) != 1: + print("! Can't parse CSG cube parameters: "+parameters) + exit() + extra = np.array([dilatation]*3) + return (extra + np.array(json.loads(results[0][0]), dtype=float)/1000.0), results[0][1] == 'true' + + +def cylinder_parse(parameters, dilatation): + results = re.findall( + r'h = (.+), r1 = (.+), r2 = (.+), center = (.+)', parameters) + if len(results) != 1: + print("! Can't parse CSG cylinder parameters: "+parameters) + exit() + result = results[0] + extra = np.array([dilatation/2, dilatation]) + return (extra + np.array([result[0], result[1]], dtype=float)/1000.0), result[3] == 'true' + + +def sphere_parse(parameters, dilatation): + results = re.findall(r'r = (.+)$', parameters) + if len(results) != 1: + print("! Can't parse CSG sphere parameters: "+parameters) + exit() + return dilatation + float(results[0])/1000.0 + + +def extract_node_parameters(line): + line = line.strip() + parts = line.split('(', 1) + node = parts[0] + parameters = parts[1] + if parameters[-1] == ';': + parameters = parameters[:-2] + if parameters[-1] == '{': + parameters = parameters[:-3] + return node, parameters + + +def T(x, y, z): + m = np.eye(4) + m.T[3, :3] = [x, y, z] + + return m + + +def parse_csg(data, dilatation): + shapes = [] + lines = data.split("\n") + matrices = [] + for line in lines: + line = line.strip() + if line != '': + if line[-1] == '{': + node, parameters = extract_node_parameters(line) + if node == 'multmatrix': + matrix = multmatrix_parse(parameters) + else: + matrix = np.array(np.identity(4)) + matrices.append(matrix) + elif line[-1] == '}': + matrices.pop() + else: + node, parameters = extract_node_parameters(line) + transform = np.array(np.identity(4)) + for entry in matrices: + transform = transform@entry + if node == 'cube': + size, center = cube_parse(parameters, dilatation) + if not center: + transform = transform @ \ + T(size[0]/2.0, size[1]/2.0, size[2]/2.0) + shapes.append({ + 'type': 'cube', + 'parameters': size, + 'transform': transform + }) + if node == 'cylinder': + size, center = cylinder_parse(parameters, dilatation) + if not center: + transform = transform @ T(0, 0, size[0]/2.0) + shapes.append({ + 'type': 'cylinder', + 'parameters': size, + 'transform': transform + }) + if node == 'sphere': + shapes.append({ + 'type': 'sphere', + 'parameters': sphere_parse(parameters, dilatation), + 'transform': transform + }) + return shapes + + +def process(filename, dilatation): + tmp_data = os.getcwd()+'/_tmp_data.csg' + os.system('openscad '+filename+' -o '+tmp_data) + with open(tmp_data, "r", encoding="utf-8") as stream: + data = stream.read() + os.system('rm '+tmp_data) + + return parse_csg(data, dilatation) diff --git a/onshape_to_robot/edit_shape.py b/onshape_to_robot/edit_shape.py new file mode 100644 index 0000000..7d7bef5 --- /dev/null +++ b/onshape_to_robot/edit_shape.py @@ -0,0 +1,26 @@ +def main(): + import os + import sys + + if len(sys.argv) < 2: + print("Usage: onshape-to-robot-edit-shape {STL file}") + else: + fileName = sys.argv[1] + parts = fileName.split(".") + parts[-1] = "scad" + fileName = ".".join(parts) + if not os.path.exists(fileName): + scad = '% scale(1000) import("' + os.path.basename(sys.argv[1]) + '");\n' + scad += "\n" + scad += "// Append pure shapes (cube, cylinder and sphere), e.g:\n" + scad += "// cube([10, 10, 10], center=true);\n" + scad += "// cylinder(r=10, h=10, center=true);\n" + scad += "// sphere(10);\n" + with open(fileName, "w", encoding="utf-8") as stream: + stream.write(scad) + directory = os.path.dirname(fileName) + os.system("cd " + directory + "; openscad " + os.path.basename(fileName)) + + +if __name__ == "__main__": + main() diff --git a/onshape_to_robot/export.py b/onshape_to_robot/export.py new file mode 100644 index 0000000..972a37e --- /dev/null +++ b/onshape_to_robot/export.py @@ -0,0 +1,117 @@ +def main(): + import os + import sys + import pickle + import argparse + from dotenv import load_dotenv, find_dotenv + from .config import Config + from .message import error, info + from .robot_builder import RobotBuilder + from .exporter_urdf import ExporterURDF + from .exporter_sdf import ExporterSDF + from .exporter_mujoco import ExporterMuJoCo + + """ + This is the entry point of the export script, i.e the "onshape-to-robot" command. + """ + load_dotenv(find_dotenv(usecwd=True)) + + def get_version(): + # Get version from package + try: + from importlib.metadata import version, PackageNotFoundError + + return version("onshape-to-robot") + except PackageNotFoundError: + return "unknown" + + try: + # Retrieving robot path + arg_parser = argparse.ArgumentParser() + arg_parser.add_argument( + "robot_path", type=str, help="Path to the robot directory" + ) + arg_parser.add_argument( + "--version", action="version", version=f"onshape-to-robot {get_version()}" + ) + arg_parser.add_argument( + "--retrieve", + action="store_true", + help="Only retrieve data and produce robot.pkl", + ) + arg_parser.add_argument( + "--save-pickle", + action="store_true", + help="Save the robot data to robot.pkl", + ) + arg_parser.add_argument( + "--convert", + action="store_true", + help="Only convert robot.pkl to the desired format", + ) + arg_parser.add_argument( + "--safe", + action="store_true", + help="Disable features involving custom commands or imports", + ) + args = arg_parser.parse_args() + + print(info(f"* onshape-to-robot version {get_version()}")) + + robot_path: str = args.robot_path + + # Loading configuration + config = Config(robot_path, safe=args.safe) + + # Building exporter beforehand, so that the configuration gets checked + if config.output_format == "urdf": + exporter = ExporterURDF(config) + elif config.output_format == "sdf": + exporter = ExporterSDF(config) + elif config.output_format == "mujoco": + exporter = ExporterMuJoCo(config) + else: + raise Exception(f"Unsupported output format: {config.output_format}") + + if not args.convert: + # Building the robot + robot_builder = RobotBuilder(config) + robot = robot_builder.robot + + # Can be used for debugging + pkl_filename = config.output_directory + "/robot.pkl" + if args.retrieve or args.save_pickle: + pickle.dump(robot, open(pkl_filename, "wb")) + print(info(f"* Robot data saved to {pkl_filename}")) + + if args.convert: + print(info(f"* Loading robot data from {pkl_filename}")) + robot = pickle.load(open(pkl_filename, "rb")) + + if not args.retrieve: + # Applying processors + for processor in config.processors: + processor.process(robot) + + exporter.write_xml( + robot, + config.output_directory + + "/" + + config.output_filename + + "." + + exporter.ext, + ) + + if not args.safe: + # Executing post-import commands + for command in config.post_import_commands: + print(info(f"* Running command: {command}")) + os.system(command) + + except Exception as e: + print(error(f"ERROR: {e}")) + raise e + + +if __name__ == "__main__": + main() diff --git a/onshape_to_robot/exporter.py b/onshape_to_robot/exporter.py new file mode 100644 index 0000000..edfa985 --- /dev/null +++ b/onshape_to_robot/exporter.py @@ -0,0 +1,40 @@ +import os +from .message import success +import xml.dom.minidom +from .robot import Robot + + +class Exporter: + def __init__(self): + self.xml: str = "" + self.ext: str = "xml" + + def build(self): + raise Exception("This exporter should implement build() method") + + def get_xml(self, robot: Robot) -> str: + self.build(robot) + return self.xml + + def remove_empty_text_nodes(self, node: xml.dom.Node): + to_delete = [] + + for child_node in node.childNodes: + if isinstance(child_node, xml.dom.minidom.Text): + child_node.data = child_node.data.strip() + if child_node.data == "": + to_delete.append(child_node) + else: + self.remove_empty_text_nodes(child_node) + + for child_node in to_delete: + node.childNodes.remove(child_node) + + def write_xml(self, robot: Robot, filename: str) -> str: + with open(filename, "w") as file: + self.build(robot) + dom = xml.dom.minidom.parseString(self.xml) + self.remove_empty_text_nodes(dom) + xml_output = dom.toprettyxml(indent=" ") + file.write(xml_output) + print(success(f"* Writing {os.path.basename(filename)}")) diff --git a/onshape_to_robot/exporter_mujoco.py b/onshape_to_robot/exporter_mujoco.py new file mode 100644 index 0000000..49ad80b --- /dev/null +++ b/onshape_to_robot/exporter_mujoco.py @@ -0,0 +1,414 @@ +from __future__ import annotations +import numpy as np +import os +import fnmatch +from .message import success, warning, info +from .robot import Robot, Link, Part, Joint, Closure +from .config import Config +from .geometry import Box, Cylinder, Sphere, Mesh, Shape +from .exporter import Exporter +from .exporter_utils import xml_escape, rotation_matrix_to_rpy +from transforms3d.quaternions import mat2quat + + +class ExporterMuJoCo(Exporter): + def __init__(self, config: Config | None = None): + super().__init__() + self.config: Config = config + + self.no_dynamics: bool = False + self.additional_xml: str = "" + self.meshes: list = [] + self.materials: dict = {} + + if config is not None: + self.equalities = self.config.get("equalities", {}) + self.no_dynamics = config.no_dynamics + additional_xml_file = config.get("additional_xml", None, required=False) + if isinstance(additional_xml_file, str): + self.add_additional_xml(additional_xml_file) + elif isinstance(additional_xml_file, list): + for filename in additional_xml_file: + self.add_additional_xml(filename) + + def add_additional_xml(self, xml_file: str): + self.additional_xml += f"" + with open(self.config.output_directory + "/" + xml_file, "r") as file: + self.additional_xml += file.read() + + def append(self, line: str): + self.xml += line + + def build(self, robot: Robot): + self.xml = "" + self.append('') + self.append("") + if self.config: + self.append(f"") + self.append(f'') + self.append( + f'' + ) + + # Boilerplate + self.default_class = robot.name + self.append("") + self.append(f'') + self.append('') + self.append('') + self.append('') + self.append('') + self.append("") + self.append('') + self.append('') + self.append("") + self.append("") + self.append("") + + if self.additional_xml: + self.append(self.additional_xml) + + # Adding robot links + self.append("") + for base_link in robot.base_links: + self.add_link(robot, base_link) + self.append("") + + # Asset (mesh & materials) + self.append("") + for mesh_file in set(self.meshes): + self.append(f'') + for material_name, color in self.materials.items(): + color_str = "%g %g %g %g" % self.config.round(tuple(color)) + self.append(f'') + self.append("") + + # Adding actuators + self.add_actuators(robot) + + # Adding equalities (loop closure) + self.add_equalities(robot) + + self.append("") + + return self.xml + + def add_actuators(self, robot: Robot): + self.append("") + + for joint in robot.joints: + if joint.joint_type == "fixed": + continue + + # Suppose joints with relation equality is not actuated, unless specified + guess_actuated = joint.relation is None + + if ( + joint.properties.get("actuated", guess_actuated) + and joint.joint_type != Joint.BALL + ): + type = joint.properties.get("type", "position") + actuator_class = joint.properties.get("class", self.default_class) + actuator: str = f'<{type} class="{actuator_class}" name="{joint.name}" joint="{joint.name}" ' + + for key in "kp", "kv", "dampratio": + if key in joint.properties: + actuator += f'{key}="{joint.properties[key]}" ' + + if "forcerange" in joint.properties: + actuator += f'forcerange="-{joint.properties["forcerange"]} {joint.properties["forcerange"]}" ' + + joint_limits = joint.properties.get("limits", joint.limits) + limits_are_set = joint.properties.get("limits", False) != False + if joint_limits and (type == "position" or limits_are_set): + if joint.properties.get("range", True) and type == "position": + actuator += f'inheritrange="1" ' + else: + actuator += f'ctrlrange="{joint_limits[0]} {joint_limits[1]}" ' + + actuator += "/>" + self.append(actuator) + + self.append("") + + def get_equality_attributes(self, closure: Closure) -> str: + all_attributes = {} + for name, attributes in self.equalities.items(): + if fnmatch.fnmatch(closure.frame1, name) and fnmatch.fnmatch( + closure.frame2, name + ): + all_attributes.update(attributes) + + if len(all_attributes) > 0: + return ( + " ".join([f'{key}="{value}"' for key, value in all_attributes.items()]) + + " " + ) + + return "" + + def add_equalities(self, robot: Robot): + self.append("") + for closure in robot.closures: + attributes = self.get_equality_attributes(closure) + + if closure.closure_type == Closure.FIXED: + self.append( + f'' + ) + elif closure.closure_type == Closure.REVOLUTE: + self.append( + f'' + ) + elif closure.closure_type == Closure.BALL: + self.append( + f'' + ) + else: + print( + warning( + f"Closure type: {closure.closure_type} is not supported with MuJoCo equality constraints" + ) + ) + + for joint in robot.joints: + if joint.relation is not None: + self.append( + f'' + ) + + self.append("") + + def add_inertial(self, mass: float, com: np.ndarray, inertia: np.ndarray): + # Ensuring epsilon masses and inertias + mass = max(1e-9, mass) + inertia[0, 0] = max(1e-9, inertia[0, 0]) + inertia[1, 1] = max(1e-9, inertia[1, 1]) + inertia[2, 2] = max(1e-9, inertia[2, 2]) + + # Populating body inertial properties + # https://mujoco.readthedocs.io/en/stable/XMLreference.html#body-inertial + inertial: str = "") + if joint.joint_type == "fixed": + self.append(f'') + return + + joint_xml: str = "") + T_link_frame = np.linalg.inv(T_world_link) @ T_world_frame + + site: str = f'") + T_parent_link = np.linalg.inv(T_world_parent) @ T_world_link + self.append( + f'' + ) + + if parent_joint is None: + if not link.fixed: + self.append(f'') + else: + self.add_joint(parent_joint) + + # Adding inertial properties + mass, com, inertia = link.get_dynamics(T_world_link) + self.add_inertial(mass, com, inertia) + + # Adding geometry objects + for part in link.parts: + self.append(f"") + self.add_geometries(part, T_world_link) + + # Adding frames attached to current link + for frame, T_world_frame in link.frames.items(): + self.add_frame(frame, T_world_link, T_world_frame, group=3) + + # Adding joints and children links + for joint in robot.get_link_joints(link): + self.add_link(robot, joint.child, joint, T_world_link) + + self.append("") + + def pos_quat(self, matrix: np.ndarray) -> str: + """ + Turn a transformation matrix into 'pos="..." quat="..."' attributes + """ + pos = matrix[:3, 3] + quat = mat2quat(matrix[:3, :3]) + xml = 'pos="%g %g %g" quat="%g %g %g %g"' % self.config.round((*pos, *quat)) + + return xml + + def write_xml(self, robot: Robot, filename: str) -> str: + super().write_xml(robot, filename) + + dirname = os.path.dirname(filename) + scene_filename = dirname + "/scene.xml" + if not os.path.exists(scene_filename): + scene_xml: str = ( + os.path.dirname(os.path.realpath(__file__)) + "/assets/scene.xml" + ) + scene_xml = open(scene_xml, "r").read() + scene_xml = scene_xml.format(robot_filename=os.path.basename(filename)) + with open(scene_filename, "w") as file: + file.write(scene_xml) + print(success(f"* Writing scene.xml")) + else: + print(info(f"* scene.xml already exists, not over-writing it")) diff --git a/onshape_to_robot/exporter_sdf.py b/onshape_to_robot/exporter_sdf.py new file mode 100644 index 0000000..e289a23 --- /dev/null +++ b/onshape_to_robot/exporter_sdf.py @@ -0,0 +1,338 @@ +from __future__ import annotations +import numpy as np +import os +from .message import success +from .robot import Robot, Link, Part, Joint +from .config import Config +from .geometry import Box, Cylinder, Sphere, Mesh, Shape +from .exporter import Exporter +from .exporter_utils import xml_escape, rotation_matrix_to_rpy + +MODEL_CONFIG_XML = """ + + + %s + 1.0 + %s + + + + + + +""" + + +class ExporterSDF(Exporter): + def __init__(self, config: Config | None = None): + super().__init__() + self.config: Config = config + + self.ext: str = "sdf" + self.no_dynamics: bool = False + self.additional_xml: str = "" + + if config is not None: + self.no_dynamics = config.no_dynamics + additional_xml_file = config.get("additional_xml", None, required=False) + if isinstance(additional_xml_file, str): + self.add_additional_xml(additional_xml_file) + elif isinstance(additional_xml_file, list): + for filename in additional_xml_file: + self.add_additional_xml(filename) + + def add_additional_xml(self, xml_file: str): + self.additional_xml += f"" + with open(self.config.output_directory + "/" + xml_file, "r") as file: + self.additional_xml += file.read() + + def append(self, line: str): + self.xml += line + + def build(self, robot: Robot): + self.xml = "" + self.append('') + self.append("") + if self.config: + self.append(f"") + self.append('') + self.append(f'') + + for base_link in robot.base_links: + self.add_link(robot, base_link) + + if self.additional_xml: + self.append(self.additional_xml) + + self.append("") + self.append("") + + return self.xml + + def add_inertial( + self, mass: float, com: np.ndarray, inertia: np.ndarray, frame: str = "" + ): + # Unless "no_dynamics" is set, we make sure that mass and inertia + # are not zero + if not self.no_dynamics: + mass = max(1e-9, mass) + inertia[0, 0] = max(1e-9, inertia[0, 0]) + inertia[1, 1] = max(1e-9, inertia[1, 1]) + inertia[2, 2] = max(1e-9, inertia[2, 2]) + + self.append("") + self.append( + '%g %g %g 0 0 0' + % self.config.round(( + com[0], + com[1], + com[2], + )) + ) + self.append("%g" % self.config.round(mass)) + self.append( + "%g%g%g%g%g%g" + % self.config.round(( + inertia[0, 0], + inertia[0, 1], + inertia[0, 2], + inertia[1, 1], + inertia[1, 2], + inertia[2, 2], + )) + ) + self.append("") + + def append_material(self, color: np.ndarray): + self.append(f"") + self.append("%g %g %g %g" % self.config.round((color[0], color[1], color[2], color[3]))) + self.append("%g %g %g %g" % self.config.round((color[0], color[1], color[2], color[3]))) + self.append("0.1 0.1 0.1 1") + self.append("0 0 0 0") + self.append("") + + def add_mesh( + self, + link: Link, + part: Part, + node: str, + T_world_link: np.ndarray, + mesh: Mesh, + mesh_n: int, + ): + """ + Add a mesh node (e.g. STL) to the SDF file + """ + self.append(f'<{node} name="{part.name}_{node}_mesh_{mesh_n}">') + + T_link_part = np.linalg.inv(T_world_link) @ part.T_world_part + self.append(self.pose(T_link_part, relative_to=link.name)) + + mesh_file = mesh.filename + + self.append("") + self.append( + f"model://{self.config.robot_name}/{xml_escape(mesh_file)}" + ) + self.append("") + + if node == "visual": + self.append_material(mesh.color) + + # Apply properties based on node type (visual or collision) + properties = mesh.visual_properties if node == "visual" else mesh.collision_properties + for key, value in properties.items(): + self.append(f'<{key}>{xml_escape(str(value))}') + + self.append(f"") + + def add_shape( + self, + link: Link, + part: Part, + node: str, + T_world_link: np.ndarray, + shape: Shape, + shape_n: int, + ): + """ + Add shapes (box, sphere and cylinder) nodes to the SDF. + """ + self.append(f'<{node} name="{part.name}_{node}_shapes_{shape_n}">') + + T_link_shape = ( + np.linalg.inv(T_world_link) @ part.T_world_part @ shape.T_part_shape + ) + self.append(self.pose(T_link_shape, relative_to=link.name)) + + self.append("") + if isinstance(shape, Box): + self.append("%g %g %g" % self.config.round(tuple(shape.size))) + elif isinstance(shape, Cylinder): + self.append( + "%g%g" + % self.config.round((shape.length, shape.radius)) + ) + elif isinstance(shape, Sphere): + self.append("%g" % self.config.round((shape.radius,))) + self.append("") + + if node == "visual": + self.append_material(shape.color) + + # Apply properties based on node type (visual or collision) + properties = shape.visual_properties if node == "visual" else shape.collision_properties + for key, value in properties.items(): + self.append(f'<{key}>{xml_escape(str(value))}') + + self.append(f"") + + def add_geometries(self, link: Link, part: Part, T_world_link: np.ndarray): + """ + Add a part geometries + """ + shape_n = 0 + for shape in part.shapes: + if shape.visual: + shape_n += 1 + self.add_shape(link, part, "visual", T_world_link, shape, shape_n) + if shape.collision: + shape_n += 1 + self.add_shape(link, part, "collision", T_world_link, shape, shape_n) + + mesh_n = 0 + for mesh in part.meshes: + if mesh.visual: + mesh_n += 1 + self.add_mesh(link, part, "visual", T_world_link, mesh, mesh_n) + if mesh.collision: + mesh_n += 1 + self.add_mesh(link, part, "collision", T_world_link, mesh, mesh_n) + + def add_joint(self, joint: Joint, T_world_link: np.ndarray): + self.append(f"") + + joint_type = joint.properties.get("type", joint.joint_type) + self.append(f'') + + T_link_joint = np.linalg.inv(T_world_link) @ joint.T_world_joint + self.append(self.pose(T_link_joint, relative_to=joint.parent.name)) + + self.append(f"{joint.parent.name}") + self.append(f"{joint.child.name}") + self.append("") + self.append("%g %g %g" % self.config.round(tuple(joint.axis))) + + self.append("") + if "max_effort" in joint.properties: + self.append("%g" % self.config.round(joint.properties["max_effort"])) + else: + self.append("10") + + if "max_velocity" in joint.properties: + self.append("%g" % self.config.round(joint.properties["max_velocity"])) + else: + self.append("10") + + joint_limits = joint.properties.get("limits", joint.limits) + if joint_limits is None: + if joint_type == "revolute": + joint_limits = [-np.pi, np.pi] + if joint_type == "prismatic": + joint_limits = [-1, 1] + + self.append(f"{joint_limits[0]}") + self.append(f"{joint_limits[1]}") + self.append("") + + if joint.relation is not None: + self.append( + f'{joint.relation.ratio}' + ) + + self.append("") + self.append(f"{joint.properties.get('friction', 0.)}") + self.append(f"{joint.properties.get('damping', 0.)}") + self.append("") + + self.append("") + + self.append("") + + def add_frame( + self, + link: Link, + frame: str, + T_world_link: np.ndarray, + T_world_frame: np.ndarray, + ): + self.append(f"") + T_link_frame = np.linalg.inv(T_world_link) @ T_world_frame + + self.append(f'') + self.append(self.pose(T_link_frame, relative_to=link.name)) + self.append("") + + def add_link(self, robot: Robot, link: Link, joint: Joint = None): + """ + Adds a link recursively to the SDF file + """ + self.append(f"") + self.append(f'') + + T_world_link = np.eye(4) + if joint is not None: + T_world_link = joint.T_world_joint + + # Adding inertial properties + mass, com, inertia = link.get_dynamics(T_world_link) + self.add_inertial(mass, com, inertia, link.name) + + relative_to = "" + if joint is not None: + relative_to = joint.name + self.append(self.pose(np.eye(4), relative_to=relative_to)) + + # Adding geometry objects + for part in link.parts: + self.append(f"") + self.add_geometries(link, part, T_world_link) + + self.append("") + + if link.fixed: + self.append(f'') + self.append(f"world") + self.append(f"{link.name}") + self.append("") + + # Adding frames attached to current link + for frame, T_world_frame in link.frames.items(): + self.add_frame(link, frame, T_world_link, T_world_frame) + + # Adding joints and children links + for joint in robot.get_link_joints(link): + self.add_link(robot, joint.child, joint) + self.add_joint(joint, T_world_link) + + def pose(self, matrix: np.ndarray, relative_to: str = ""): + """ + Transforms a transformation matrix into a SDF pose tag + """ + relative = "" + if relative_to: + relative = f' relative_to="{relative_to}"' + + sdf = "%g %g %g %g %g %g" + + return sdf % (relative, *self.config.round((*matrix[:3, 3], *rotation_matrix_to_rpy(matrix)))) + + def write_xml(self, robot: Robot, filename: str) -> str: + model_config = MODEL_CONFIG_XML % (robot.name, os.path.basename(filename)) + + super().write_xml(robot, filename) + + dirname = os.path.dirname(filename) + with open(dirname + "/model.config", "w") as file: + file.write(model_config) + print(success(f"* Writing model.config")) diff --git a/onshape_to_robot/exporter_urdf.py b/onshape_to_robot/exporter_urdf.py new file mode 100644 index 0000000..5b77535 --- /dev/null +++ b/onshape_to_robot/exporter_urdf.py @@ -0,0 +1,326 @@ +from __future__ import annotations +import numpy as np +import os +from .message import warning +from .robot import Robot, Link, Part, Joint +from .config import Config +from .geometry import Box, Cylinder, Sphere, Shape, Mesh +from .exporter import Exporter +from .exporter_utils import xml_escape, rotation_matrix_to_rpy + + +class ExporterURDF(Exporter): + def __init__(self, config: Config | None = None): + super().__init__() + self.config: Config = config + + self.ext: str = "urdf" + self.no_dynamics: bool = False + self.package_name: str = "" + self.additional_xml: str = "" + self.set_zero_mass_to_fixed: bool = False + + if config is not None: + self.no_dynamics = config.no_dynamics + self.package_name: str = config.get("package_name", "") + self.set_zero_mass_to_fixed: bool = config.get( + "set_zero_mass_to_fixed", False + ) + additional_xml_file = config.get("additional_xml", None, required=False) + if isinstance(additional_xml_file, str): + self.add_additional_xml(additional_xml_file) + elif isinstance(additional_xml_file, list): + for filename in additional_xml_file: + self.add_additional_xml(filename) + + def add_additional_xml(self, xml_file: str): + self.additional_xml += f"" + with open(self.config.output_directory + "/" + xml_file, "r") as file: + self.additional_xml += file.read() + + def append(self, line: str): + self.xml += line + + def build(self, robot: Robot): + self.xml = "" + self.append('') + self.append("") + if self.config: + self.append(f"") + self.append(f'') + + if len(robot.base_links) > 1: + print( + warning( + "WARNING: Multiple base links detected, which is not supported by URDF." + ) + ) + print(warning("Only the first base link will be considered.")) + + if len(robot.base_links) > 0: + self.add_link(robot, robot.base_links[0]) + + if self.additional_xml: + self.append(self.additional_xml) + + self.append("") + + return self.xml + + def add_inertial( + self, mass: float, com: np.ndarray, inertia: np.ndarray, fixed: str = False + ): + # Unless "no_dynamics" is set, we make sure that mass and inertia + # are not zero + if not self.no_dynamics: + mass = max(1e-9, mass) + inertia[0, 0] = max(1e-9, inertia[0, 0]) + inertia[1, 1] = max(1e-9, inertia[1, 1]) + inertia[2, 2] = max(1e-9, inertia[2, 2]) + if fixed and self.set_zero_mass_to_fixed: + # To mark an object as fixed in the world, sets its dynamics to zero + mass = 0 + com = np.zeros(3) + inertia = np.zeros((3, 3)) + + self.append("") + self.append( + '' + % self.config.round( + ( + com[0], + com[1], + com[2], + ) + ) + ) + self.append('' % self.config.round(mass)) + self.append( + '' + % self.config.round( + ( + inertia[0, 0], + inertia[0, 1], + inertia[0, 2], + inertia[1, 1], + inertia[1, 2], + inertia[2, 2], + ) + ) + ) + self.append("") + + def add_mesh(self, part: Part, node: str, T_world_link: np.ndarray, mesh: Mesh): + """ + Add a mesh node (e.g. STL) to the URDF file + """ + self.append(f"<{node}>") + + T_link_part = np.linalg.inv(T_world_link) @ part.T_world_part + self.append(self.origin(T_link_part)) + + mesh_file = mesh.filename + if self.package_name: + mesh_file = self.package_name + "/" + mesh_file + + self.append("") + self.append(f'') + self.append("") + + if node == "visual": + material_name = f"{part.name}_material" + self.append(f'') + self.append( + '' + % self.config.round( + (mesh.color[0], mesh.color[1], mesh.color[2], mesh.color[3]) + ) + ) + self.append("") + + # Apply properties based on node type (visual or collision) + properties = mesh.visual_properties if node == "visual" else mesh.collision_properties + for key, value in properties.items(): + self.append(f'<{key}>{xml_escape(str(value))}') + + self.append(f"") + + def add_shape(self, part: Part, node: str, T_world_link: np.ndarray, shape: Shape): + """ + Add shapes (box, sphere and cylinder) nodes to the URDF. + """ + self.append(f"<{node}>") + + T_link_shape = ( + np.linalg.inv(T_world_link) @ part.T_world_part @ shape.T_part_shape + ) + self.append(self.origin(T_link_shape)) + + self.append("") + if isinstance(shape, Box): + self.append( + '' % self.config.round(tuple(shape.size)) + ) + elif isinstance(shape, Cylinder): + self.append( + '' + % self.config.round((shape.length, shape.radius)) + ) + elif isinstance(shape, Sphere): + self.append('' % self.config.round((shape.radius,))) + self.append("") + + if node == "visual": + material_name = f"{part.name}_material" + self.append(f'') + self.append( + '' + % self.config.round( + (shape.color[0], shape.color[1], shape.color[2], shape.color[3]) + ) + ) + self.append("") + + # Apply properties based on node type (visual or collision) + properties = shape.visual_properties if node == "visual" else shape.collision_properties + for key, value in properties.items(): + self.append(f'<{key}>{xml_escape(str(value))}') + + self.append(f"") + + def add_geometries(self, part: Part, T_world_link: np.ndarray): + """ + Add a part geometries + """ + for shape in part.shapes: + if shape.visual: + self.add_shape(part, "visual", T_world_link, shape) + if shape.collision: + self.add_shape(part, "collision", T_world_link, shape) + + for mesh in part.meshes: + if mesh.visual: + self.add_mesh(part, "visual", T_world_link, mesh) + if mesh.collision: + self.add_mesh(part, "collision", T_world_link, mesh) + + def add_joint(self, joint: Joint, T_world_link: np.ndarray): + self.append(f"") + + joint_type = joint.properties.get("type", joint.joint_type) + self.append(f'') + + T_link_joint = np.linalg.inv(T_world_link) @ joint.T_world_joint + self.append(self.origin(T_link_joint)) + + self.append(f'') + self.append(f'') + self.append('' % self.config.round(tuple(joint.axis))) + + limits = "" + if "max_effort" in joint.properties: + limits += 'effort="%g" ' % self.config.round(joint.properties["max_effort"]) + else: + limits += 'effort="10" ' + + if "max_velocity" in joint.properties: + limits += 'velocity="%g" ' % self.config.round( + joint.properties["max_velocity"] + ) + else: + limits += 'velocity="10" ' + + joint_limits = joint.properties.get("limits", joint.limits) + if joint_limits is not None: + limits += 'lower="%g" upper="%g" ' % self.config.round( + (joint_limits[0], joint_limits[1]) + ) + elif joint_type == "revolute": + limits += f'lower="{-np.pi}" upper="{np.pi}" ' + elif joint_type == "prismatic": + limits += 'lower="-1" upper="1" ' + + if limits: + self.append(f"") + + if "friction" in joint.properties: + self.append( + f'' + ) + + if joint.relation is not None: + self.append( + f'' + ) + + self.append("") + + def add_frame( + self, + link: Link, + frame: str, + T_world_link: np.ndarray, + T_world_frame: np.ndarray, + ): + self.append(f"") + T_link_frame = np.linalg.inv(T_world_link) @ T_world_frame + + # Adding a dummy link to the assembly + self.append(f'') + self.append(self.origin(np.eye(4))) + + self.append("") + self.append('') + if self.no_dynamics: + self.append('') + else: + self.append('') + self.append('') + self.append("") + + self.append("") + + # Attaching this dummy link to the parent frame using a fixed joint + self.append(f'') + self.append(self.origin(T_link_frame)) + self.append(f'') + self.append(f'') + self.append('') + self.append("") + + def add_link(self, robot: Robot, link: Link, T_world_link: np.ndarray = np.eye(4)): + """ + Adds a link recursively to the URDF file + """ + self.append(f"") + self.append(f'') + + # Adding inertial properties + mass, com, inertia = link.get_dynamics(T_world_link) + self.add_inertial(mass, com, inertia, link.fixed) + + # Adding geometry objects + for part in link.parts: + self.append(f"") + self.add_geometries(part, T_world_link) + + self.append("") + + # Adding frames attached to current link + for frame, T_world_frame in link.frames.items(): + self.add_frame(link, frame, T_world_link, T_world_frame) + + # Adding joints and children links + for joint in robot.get_link_joints(link): + self.add_link(robot, joint.child, joint.T_world_joint) + self.add_joint(joint, T_world_link) + + def origin(self, matrix: np.ndarray): + """ + Transforms a transformation matrix into a URDF origin tag + """ + urdf = '' + + return urdf % self.config.round( + (*matrix[:3, 3], *rotation_matrix_to_rpy(matrix)) + ) diff --git a/onshape_to_robot/exporter_utils.py b/onshape_to_robot/exporter_utils.py new file mode 100644 index 0000000..161709f --- /dev/null +++ b/onshape_to_robot/exporter_utils.py @@ -0,0 +1,30 @@ +import math +import numpy as np +from xml.sax.saxutils import escape + + +def xml_escape(unescaped: str) -> str: + """ + Escapes XML characters in a string so that it can be safely added to an XML file + """ + return escape(unescaped, entities={"'": "'", '"': """}) + + +def rotation_matrix_to_rpy(R): + """ + Converts a rotation matrix to rpy Euler angles + """ + sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0]) + + singular = sy < 1e-6 + + if not singular: + x = math.atan2(R[2, 1], R[2, 2]) + y = math.atan2(-R[2, 0], sy) + z = math.atan2(R[1, 0], R[0, 0]) + else: + x = math.atan2(-R[1, 2], R[1, 1]) + y = math.atan2(-R[2, 0], sy) + z = 0 + + return np.array([x, y, z]) diff --git a/onshape_to_robot/expression.py b/onshape_to_robot/expression.py new file mode 100644 index 0000000..92709ad --- /dev/null +++ b/onshape_to_robot/expression.py @@ -0,0 +1,119 @@ +import numpy as np +import ast +import operator as op + + +class ExpressionParser: + """ + Evaluate Onshape expression + See https://cad.onshape.com/help/Content/numeric-fields.htm + + This parser is based on ast module. + """ + + def __init__(self): + self.variables_lazy_loading = None + self.variables = { + "pi": np.pi, "Pi": np.pi, "PI": np.pi + } + + # Supported operators + operators = { + ast.Add: op.add, + ast.Sub: op.sub, + ast.Mult: op.mul, + ast.Div: op.truediv, + ast.Pow: op.pow, + ast.BitXor: op.xor, + ast.USub: op.neg, + ast.Mod: op.mod, + } + + # Supporter functions + functions = { + "cos": np.cos, + "sin": np.sin, + "tan": np.tan, + "acos": np.arccos, + "asin": np.arcsin, + "atan": np.arctan, + "atan2": np.arctan2, + "cosh": np.cosh, + "sinh": np.sinh, + "tanh": np.tanh, + "asinh": np.arcsinh, + "acosh": np.arccosh, + "atanh": np.arctanh, + "ceil": np.ceil, + "floor": np.floor, + "round": np.round, + "exp": np.exp, + "sqrt": np.sqrt, + "abs": np.abs, + "max": np.max, + "min": np.min, + "log": np.log, + "log10": np.log10, + } + + def eval_expr(self, expr): + # Length units. Converting everything to meter / radian + units = { + "millimeter": 1e-3, + "mm": 1e-3, + "centimeter": 1e-2, + "cm": 1e-2, + "meter": 1.0, + "inch": 0.0254, + "in": 0.0254, + "foot": 0.3048, + "ft": 0.3048, + "yard": 0.9144, + "yd": 0.9144, + "radian": 1.0, + "rad": 1.0, + "degree": np.pi / 180, + "deg": np.pi / 180, + "m": 1.0 + } + for unit, factor in units.items(): + expr = expr.replace(f" {unit}", f"*{factor}") + + expr = expr.replace("#", "") + expr = expr.replace("^", "**") + + return self.eval_(ast.parse(expr, mode="eval").body) + + def eval_(self, node): + if isinstance(node, ast.Constant): + return float(node.value) + elif isinstance(node, ast.BinOp): + return self.operators[type(node.op)]( + self.eval_(node.left), self.eval_(node.right) + ) + elif isinstance(node, ast.UnaryOp): # e.g., -1 + return self.operators[type(node.op)](self.eval_(node.operand)) + elif isinstance(node, ast.Name): + if ( + node.id not in self.variables + and self.variables_lazy_loading is not None + ): + self.variables_lazy_loading() + self.variables_lazy_loading = None + if node.id not in self.variables: + raise ValueError(f"Unknown variable in expression: {node.id}") + return self.variables[node.id] + elif isinstance(node, ast.Call): + if node.func.id not in self.functions: + raise ValueError(f"Unknown function in expression: {node.func.id}") + return self.functions[node.func.id](*[self.eval_(arg) for arg in node.args]) + else: + raise TypeError(node) + + +if __name__ == "__main__": + ep = ExpressionParser() + ep.variables["x"] = 5 + + print(ep.eval_expr("(cos(5 deg)) mm + #x inch")) + print(ep.eval_expr("-sin(3/(2^2) deg)")) diff --git a/onshape_to_robot/geometry.py b/onshape_to_robot/geometry.py new file mode 100644 index 0000000..b2fb8b9 --- /dev/null +++ b/onshape_to_robot/geometry.py @@ -0,0 +1,87 @@ +import numpy as np + + +class Geometry: + def __init__( + self, + color: np.ndarray = np.array([0.5, 0.5, 0.5, 1.0]), + visual: bool = True, + collision: bool = True, + ): + self.color: np.ndarray = color + self.visual: bool = visual + self.collision: bool = collision + self.visual_properties: dict = {} + self.collision_properties: dict = {} + + def is_type(self, what: str): + if what == "visual": + return self.visual + elif what == "collision": + return self.collision + return False + + +class Mesh(Geometry): + def __init__( + self, + filename: str, + color: np.ndarray = np.array([0.5, 0.5, 0.5, 1.0]), + visual: bool = True, + collision: bool = True, + ): + super().__init__(color, visual, collision) + self.filename: str = filename + + +class Shape(Geometry): + def __init__( + self, + T_part_shape: np.ndarray, + color: np.ndarray = np.array([0.5, 0.5, 0.5, 1.0]), + visual: bool = True, + collision: bool = True, + ): + super().__init__(color, visual, collision) + self.T_part_shape: np.ndarray = T_part_shape + + +class Box(Shape): + def __init__( + self, + T_part_shape: np.ndarray, + size: np.ndarray, + color: np.ndarray = np.array([0.5, 0.5, 0.5, 1.0]), + visual: bool = True, + collision: bool = True, + ): + super().__init__(T_part_shape, color, visual, collision) + self.size: np.ndarray = size + + +class Cylinder(Shape): + def __init__( + self, + T_part_shape: np.ndarray, + length: float, + radius: float, + color: np.ndarray = np.array([0.5, 0.5, 0.5, 1.0]), + visual: bool = True, + collision: bool = True, + ): + super().__init__(T_part_shape, color, visual, collision) + self.length: float = length + self.radius: float = radius + + +class Sphere(Shape): + def __init__( + self, + T_part_shape: np.ndarray, + radius: float, + color: np.ndarray = np.array([0.5, 0.5, 0.5, 1.0]), + visual: bool = True, + collision: bool = True, + ): + super().__init__(T_part_shape, color, visual, collision) + self.radius: float = radius diff --git a/onshape_to_robot/message.py b/onshape_to_robot/message.py new file mode 100644 index 0000000..a02d0eb --- /dev/null +++ b/onshape_to_robot/message.py @@ -0,0 +1,21 @@ +from colorama import Fore, Back, Style, just_fix_windows_console + +just_fix_windows_console() + +def error(text: str): + return Fore.RED + text + Style.RESET_ALL + +def bright(text: str): + return Style.BRIGHT + text + Style.RESET_ALL + +def info(text: str): + return Fore.BLUE + text + Style.RESET_ALL + +def success(text: str): + return Fore.GREEN + text + Style.RESET_ALL + +def warning(text: str): + return Fore.YELLOW + text + Style.RESET_ALL + +def dim(text: str): + return Style.DIM + text + Style.RESET_ALL diff --git a/onshape_to_robot/mujoco.py b/onshape_to_robot/mujoco.py new file mode 100644 index 0000000..27522ef --- /dev/null +++ b/onshape_to_robot/mujoco.py @@ -0,0 +1,38 @@ +def main(): + import time + import mujoco + import argparse + import mujoco.viewer + + parser = argparse.ArgumentParser(prog="onshape-to-robot-mujoco") + parser.add_argument("--sim", action="store_true") + parser.add_argument("--x", type=float, default=0) + parser.add_argument("--y", type=float, default=0) + parser.add_argument("--z", type=float, default=0.5) + parser.add_argument("directory") + args = parser.parse_args() + + robot_path = args.directory + if not robot_path.endswith(".xml"): + robot_path += "/scene.xml" + + model: mujoco.MjModel = mujoco.MjModel.from_xml_path(robot_path) + data: mujoco.MjData = mujoco.MjData(model) + + # Check for root existence + if len(model.jnt_type) and model.jnt_type[0] == mujoco.mjtJoint.mjJNT_FREE: + data.qpos[:3] = [args.x, args.y, args.z] + + viewer = mujoco.viewer.launch_passive(model, data) + while viewer.is_running(): + step_start = time.time() + mujoco.mj_step(model, data) + viewer.sync() + + time_until_next_step = model.opt.timestep - (time.time() - step_start) + if time_until_next_step > 0: + time.sleep(time_until_next_step) + + +if __name__ == "__main__": + main() diff --git a/onshape_to_robot/onshape_api/__init__.py b/onshape_to_robot/onshape_api/__init__.py new file mode 100644 index 0000000..6d3966e --- /dev/null +++ b/onshape_to_robot/onshape_api/__init__.py @@ -0,0 +1,11 @@ +''' +onshape_api, based on onshape "apikey" +====== + +Demonstrates usage of API keys for the Onshape REST API +''' + +__copyright__ = 'Copyright (c) 2016 Onshape, Inc.' +__license__ = 'All rights reserved.' +__title__ = 'onshape_api' +__all__ = ['onshape', 'client', 'utils'] diff --git a/onshape_to_robot/onshape_api/cache.py b/onshape_to_robot/onshape_api/cache.py new file mode 100644 index 0000000..7db0e87 --- /dev/null +++ b/onshape_to_robot/onshape_api/cache.py @@ -0,0 +1,54 @@ +import os +import inspect +import hashlib +from pathlib import Path +import pickle + + +def get_cache_path() -> Path: + """ + Return the path to the user cache. + """ + path = Path.home() / ".cache" / "onshape-to-robot" + path.mkdir(parents=True, exist_ok=True) + return path + + +def can_cache(method, *args, **kwargs) -> bool: + """ + Check if the cache can be used. + When using wmv=w, the current workspace is used, which make it impossible to cache. + """ + signature = inspect.signature(method) + wmv = None + if "wmv" in signature.parameters: + wmv = signature.parameters["wmv"].default + if "wmv" in kwargs: + wmv = kwargs["wmv"] + return wmv != "w" + + +def cache_response(method): + """ + Decorator that caches the response of a method. + """ + + def cached_call(*args, **kwargs): + # Checking if the method can be cached + if not can_cache(method, *args, **kwargs): + return method(*args, **kwargs) + + # Building filename that is unique for method/args combination + method_name = method.__qualname__ + arguments = {"args": args, "kwargs": kwargs} + arguments_hash = hashlib.sha1(pickle.dumps(arguments)).hexdigest() + filename = f"{get_cache_path()}/{method_name}_{arguments_hash}.pkl" + + if not os.path.exists(filename): + result = method(*args, **kwargs) + with open(filename, "wb") as f: + pickle.dump(result, f) + + return pickle.load(open(filename, "rb")) + + return cached_call diff --git a/onshape_to_robot/onshape_api/client.py b/onshape_to_robot/onshape_api/client.py new file mode 100644 index 0000000..f002543 --- /dev/null +++ b/onshape_to_robot/onshape_api/client.py @@ -0,0 +1,254 @@ +""" +client +====== + +Convenience functions for working with the Onshape API +""" + +from .onshape import Onshape +from .cache import cache_response + + +def escape(s): + return s.replace("/", "%2f").replace("+", "%2b") + + +class Client: + """ + Defines methods for testing the Onshape API. Comes with several methods: + + - Create a document + - Delete a document + - Get a list of documents + + Attributes: + - stack (str, default='https://cad.onshape.com'): Base URL + - logging (bool, default=True): Turn logging on or off + """ + + def __init__( + self, stack="https://cad.onshape.com", logging=True, creds="./config.json" + ): + """ + Instantiates a new Onshape client. + + Args: + - stack (str, default='https://cad.onshape.com'): Base URL + - logging (bool, default=True): Turn logging on or off + """ + + self._metadata_cache = {} + self._massproperties_cache = {} + self._stack = stack + self._api = Onshape(stack=stack, logging=logging, creds=creds) + + def request(self, url, **kwargs): + return self._api.request("get", url, **kwargs).json() + + def request_binary(self, url, **kwargs): + return self._api.request("get", url, **kwargs).content + + @cache_response + def get_document(self, did): + """ + Get details for a specified document. + + Args: + - did (str): Document ID + + Returns: + - requests.Response: Onshape response data + """ + return self.request(f"/api/documents/{escape(did)}") + + @cache_response + def list_elements(self, did, wid, wmv="w"): + """ + Get the list of elements in a given document + """ + + return self.request( + f"/api/documents/d/{escape(did)}/{escape(wmv)}/{escape(wid)}/elements" + ) + + @cache_response + def get_assembly(self, did, wmvid, eid, wmv="w", configuration="default"): + """ + Retrieve the assembly structure for a specified document / workspace / element. + """ + return self.request( + f"/api/assemblies/d/{escape(did)}/{escape(wmv)}/{escape(wmvid)}/e/{escape(eid)}", + query={ + "includeMateFeatures": "true", + "includeMateConnectors": "true", + "includeNonSolids": "true", + "configuration": configuration, + }, + ) + + @cache_response + def get_features(self, did, wvid, eid, wmv="w", configuration="default"): + """ + Gets the feature list for specified document / workspace / part studio. + + Args: + - did (str): Document ID + - mid (str): Microversion + - eid (str): Element ID + + Returns: + - requests.Response: Onshape response data + """ + + return self.request( + f"/api/assemblies/d/{escape(did)}/{escape(wmv)}/{escape(wvid)}/e/{escape(eid)}/features", + query={"configuration": configuration}, + ) + + @cache_response + def get_sketches(self, did, mid, eid, configuration): + """ + Get sketches for a given document / microversion / element. + """ + return self.request( + f"/api/partstudios/d/{escape(did)}/m/{escape(mid)}/e/{escape(eid)}/sketches", + query={"includeGeometry": "true", "configuration": configuration}, + ) + + @cache_response + def get_parts(self, did, mid, eid, configuration): + """ + Get parts for a given document / microversion / element. + """ + return self.request( + f"/api/parts/d/{escape(did)}/m/{escape(mid)}/e/{escape(eid)}", + query={"configuration": configuration}, + ) + + def find_new_partid( + self, did, mid, eid, partid, configuration_before, configuration + ): + before = self.get_parts(did, mid, eid, configuration_before) + name = None + for entry in before: + if entry["partId"] == partid: + name = entry["name"] + + if name is not None: + after = self.get_parts(did, mid, eid, configuration) + for entry in after: + if entry["name"] == name: + return entry["partId"] + else: + print("Onshape ERROR: Can't find new partid for " + str(partid)) + + return partid + + @cache_response + def part_studio_stl_m( + self, + did, + wmvid, + eid, + partid="", + wmv="m", + configuration="default", + linked_document_id=None, + ): + req_headers = {"Accept": "*/*"} + query = { + "mode": "binary", + "units": "meter", + "configuration": configuration, + } + if linked_document_id is not None: + query["linkDocumentId"] = linked_document_id + return self.request_binary( + f"/api/parts/d/{escape(did)}/{escape(wmv)}/{escape(wmvid)}/e/{escape(eid)}/partid/{escape(partid)}/stl", + query=query, + headers=req_headers, + ) + + @cache_response + def matevalues(self, did, wmvid, eid, wmv="w", configuration="default"): + return self.request( + f"/api/assemblies/d/{escape(did)}/{wmv}/{escape(wmvid)}/e/{escape(eid)}/matevalues", + query={"configuration": configuration}, + ) + + @cache_response + def part_get_metadata( + self, + did, + wmvid, + eid, + partid, + wmv="m", + configuration="default", + linked_document_id=None, + ): + query = {"configuration": configuration} + if linked_document_id is not None: + query["linkDocumentId"] = linked_document_id + return self.request( + f"/api/metadata/d/{escape(did)}/{escape(wmv)}/{escape(wmvid)}/e/{escape(eid)}/p/{escape(partid)}", + query=query, + ) + + @cache_response + def part_mass_properties( + self, + did, + wmvid, + eid, + partid, + wmv="m", + configuration="default", + linked_document_id=None, + ): + query = { + "configuration": configuration, + "useMassPropertyOverrides": True, + } + if linked_document_id is not None: + query["linkDocumentId"] = linked_document_id + return self.request( + f"/api/parts/d/{escape(did)}/{escape(wmv)}/{escape(wmvid)}/e/{escape(eid)}/partid/{escape(partid)}/massproperties", + query=query, + ) + + @cache_response + def standard_cont_mass_properties( + self, did, vid, eid, partid, linked_document_id, configuration + ): + return self.request( + f"/api/parts/d/{escape(did)}/v/{escape(vid)}/e/{escape(eid)}/partid/{escape(partid)}/massproperties", + query={ + "configuration": configuration, + "useMassPropertyOverrides": True, + "linkDocumentId": linked_document_id, + "inferMetadataOwner": True, + }, + ) + + @cache_response + def elements_configuration( + self, did, wmvid, eid, wmv, linked_document_id=None, configuration=None + ): + query = {} + if linked_document_id is not None: + query["linkDocumentId"] = linked_document_id + return self.request( + f"/api/elements/d/{escape(did)}/{escape(wmv)}/{escape(wmvid)}/e/{escape(eid)}/configuration", + query=query, + ) + + @cache_response + def get_variables(self, did, wvid, eid, wmv, configuration): + return self.request( + f"/api/variables/d/{escape(did)}/{escape(wmv)}/{escape(wvid)}/e/{escape(eid)}/variables", + query={ + "configuration": configuration, + "includeValuesAndReferencedVariables": True, + }, + ) diff --git a/onshape_to_robot/onshape_api/onshape.py b/onshape_to_robot/onshape_api/onshape.py new file mode 100644 index 0000000..82c41b3 --- /dev/null +++ b/onshape_to_robot/onshape_api/onshape.py @@ -0,0 +1,247 @@ +''' +onshape +====== + +Provides access to the Onshape REST API +''' + +from . import utils + +import os +import random +import string +import commentjson as json +import hmac +import hashlib +import base64 +import urllib +import datetime +import requests +from colorama import Fore, Back, Style +from urllib.parse import urlparse +from urllib.parse import parse_qs + +__all__ = [ + 'Onshape' +] + + +class Onshape(): + ''' + Provides access to the Onshape REST API. + + Attributes: + - stack (str): Base URL + - creds (str, default='./creds.json'): Credentials location + - logging (bool, default=True): Turn logging on or off + ''' + + def __init__(self, stack, creds='./config.json', logging=True): + ''' + Instantiates an instance of the Onshape class. Reads credentials from a JSON file + of this format: + + { + "http://cad.onshape.com": { + "access_key": "YOUR KEY HERE", + "secret_key": "YOUR KEY HERE" + }, + etc... add new object for each stack to test on + } + + The creds.json file should be stored in the root project folder; optionally, + you can specify the location of a different file. + + Args: + - stack (str): Base URL + - creds (str, default='./config.json'): Credentials location + ''' + + if not os.path.isfile(creds): + raise IOError('%s is not a file' % creds) + + self._logging = logging + + with open(creds, "r", encoding="utf-8") as stream: + try: + config = json.load(stream) + except TypeError as ex: + raise ValueError('%s is not valid json' % creds) from ex + + try: + # Trying to retrieve from config.json, this is deprecated but kept for backwards compatibility + self._url = config["onshape_api"] + self._access_key = config['onshape_access_key'].encode('utf-8') + self._secret_key = config['onshape_secret_key'].encode('utf-8') + + print(Fore.YELLOW + 'WARNING: Storing Onshape credentials in config.json is deprecated, please use environment variables instead' + Style.RESET_ALL) + except KeyError: + self._url = os.getenv('ONSHAPE_API') + self._secret_bearer = os.getenv('ONSHAPE_SECRET_BEARER') + self._access_key = os.getenv('ONSHAPE_ACCESS_KEY') + self._secret_key = os.getenv('ONSHAPE_SECRET_KEY') + + if self._url and self._secret_bearer: + self._secret_bearer = self._secret_bearer.encode('utf-8') + elif self._url and self._access_key and self._secret_key: + self._access_key = self._access_key.encode('utf-8') + self._secret_key = self._secret_key.encode('utf-8') + else: + print(Fore.RED + 'ERROR: No Onshape API access key are set' + Style.RESET_ALL) + print() + print(Fore.BLUE + 'TIP: Connect to https://dev-portal.onshape.com/keys, and edit your .bashrc file:' + Style.RESET_ALL) + print(Fore.BLUE + 'export ONSHAPE_API=https://cad.onshape.com' + Style.RESET_ALL) + print(Fore.BLUE + 'export ONSHAPE_ACCESS_KEY=Your_Access_Key' + Style.RESET_ALL) + print(Fore.BLUE + 'export ONSHAPE_SECRET_KEY=Your_Secret_Key' + Style.RESET_ALL) + exit(1) + + if self._logging: + utils.log('onshape instance created: url = %s, access key = %s' % (self._url, self._access_key)) + + def _make_nonce(self): + ''' + Generate a unique ID for the request, 25 chars in length + + Returns: + - str: Cryptographic nonce + ''' + + chars = string.digits + string.ascii_letters + nonce = ''.join(random.choice(chars) for i in range(25)) + + if self._logging: + utils.log('nonce created: %s' % nonce) + + return nonce + + def _append_auth(self, output_headers: dict, method, date, path, query={}, ctype='application/json'): + ''' + Create the request signature to authenticate + + Args: + - method (str): HTTP method + - date (str): HTTP date header string + - nonce (str): Cryptographic nonce + - path (str): URL pathname + - query (dict, default={}): URL query string in key-value pairs + - ctype (str, default='application/json'): HTTP Content-Type + ''' + + if self._secret_bearer: + # Access using simple bearer token + output_headers['Authorization'] = 'Bearer ' + self._secret_bearer.decode('utf-8') + else: + # Access using API key + nonce = self._make_nonce() + query = urllib.parse.urlencode(query) + + hmac_str = (method + '\n' + nonce + '\n' + date + '\n' + ctype + '\n' + path + + '\n' + query + '\n').lower().encode('utf-8') + + signature = base64.b64encode(hmac.new(self._secret_key, hmac_str, digestmod=hashlib.sha256).digest()) + auth = 'On ' + self._access_key.decode('utf-8') + ':HmacSHA256:' + signature.decode('utf-8') + + if self._logging: + utils.log({ + 'query': query, + 'hmac_str': hmac_str, + 'signature': signature, + 'auth': auth + }) + + output_headers['On-Nonce'] = nonce + output_headers['Authorization'] = auth + + def _make_headers(self, method, path, query={}, headers={}): + ''' + Creates a headers object to sign the request + + Args: + - method (str): HTTP method + - path (str): Request path, e.g. /api/documents. No query string + - query (dict, default={}): Query string in key-value format + - headers (dict, default={}): Other headers to pass in + + Returns: + - dict: Dictionary containing all headers + ''' + + date = datetime.datetime.utcnow().strftime('%a, %d %b %Y %H:%M:%S GMT') + ctype = headers.get('Content-Type') if headers.get('Content-Type') else 'application/json' + + req_headers = { + 'Content-Type': 'application/json', + 'Date': date, + 'User-Agent': 'Onshape Python Sample App', + 'Accept': 'application/json' + } + + self._append_auth(req_headers, method, date, path, query=query, ctype=ctype) + + # add in user-defined headers + for h in headers: + req_headers[h] = headers[h] + + return req_headers + + def request(self, method, path, query={}, headers={}, body={}, base_url=None): + ''' + Issues a request to Onshape + + Args: + - method (str): HTTP method + - path (str): Path e.g. /api/documents/:id + - query (dict, default={}): Query params in key-value pairs + - headers (dict, default={}): Key-value pairs of headers + - body (dict, default={}): Body for POST request + - base_url (str, default=None): Host, including scheme and port (if different from creds file) + + Returns: + - requests.Response: Object containing the response from Onshape + ''' + + req_headers = self._make_headers(method, path, query, headers) + if base_url is None: + base_url = self._url + url = base_url + path + '?' + urllib.parse.urlencode(query) + + if self._logging: + utils.log(body) + utils.log(req_headers) + utils.log('request url: ' + url) + + # only parse as json string if we have to + body = json.dumps(body) if type(body) == dict else body + + res = requests.request(method, url, headers=req_headers, data=body, allow_redirects=False, stream=True) + + if res.status_code == 307: + location = urlparse(res.headers["Location"]) + querystring = parse_qs(location.query) + + if self._logging: + utils.log('request redirected to: ' + location.geturl()) + + new_query = {} + new_base_url = location.scheme + '://' + location.netloc + + for key in querystring: + new_query[key] = querystring[key][0] # won't work for repeated query params + + return self.request(method, location.path, query=new_query, headers=headers, base_url=new_base_url) + elif not 200 <= res.status_code <= 206: + print(url) + print('! ERROR ('+str(res.status_code)+') while using Onshape API') + if res.text: + print('! '+res.text) + + if res.status_code == 403: + print('HINT: Check that your access rights are correct, and that the clock on your computer is set correctly') + exit() + if self._logging: + utils.log('request failed, details: ' + res.text, level=1) + else: + if self._logging: + utils.log('request succeeded, details: ' + res.text) + + return res diff --git a/onshape_to_robot/onshape_api/utils.py b/onshape_to_robot/onshape_api/utils.py new file mode 100644 index 0000000..105b60f --- /dev/null +++ b/onshape_to_robot/onshape_api/utils.py @@ -0,0 +1,74 @@ +''' +utils +===== + +Handy functions for API key sample app +''' + +import logging +from logging.config import dictConfig + +__all__ = [ + 'log' +] + + +def log(msg, level=0): + ''' + Logs a message to the console, with optional level paramater + + Args: + - msg (str): message to send to console + - level (int): log level; 0 for info, 1 for error (default = 0) + ''' + + red = '\033[91m' + endc = '\033[0m' + + # configure the logging module + cfg = { + 'version': 1, + 'disable_existing_loggers': False, + 'formatters': { + 'stdout': { + 'format': '[%(levelname)s]: %(asctime)s - %(message)s', + 'datefmt': '%x %X' + }, + 'stderr': { + 'format': red + '[%(levelname)s]: %(asctime)s - %(message)s' + endc, + 'datefmt': '%x %X' + } + }, + 'handlers': { + 'stdout': { + 'class': 'logging.StreamHandler', + 'level': 'DEBUG', + 'formatter': 'stdout' + }, + 'stderr': { + 'class': 'logging.StreamHandler', + 'level': 'ERROR', + 'formatter': 'stderr' + } + }, + 'loggers': { + 'info': { + 'handlers': ['stdout'], + 'level': 'INFO', + 'propagate': True + }, + 'error': { + 'handlers': ['stderr'], + 'level': 'ERROR', + 'propagate': False + } + } + } + + dictConfig(cfg) + + lg = 'info' if level == 0 else 'error' + lvl = 20 if level == 0 else 40 + + logger = logging.getLogger(lg) + logger.log(lvl, msg) diff --git a/onshape_to_robot/processor.py b/onshape_to_robot/processor.py new file mode 100644 index 0000000..64890af --- /dev/null +++ b/onshape_to_robot/processor.py @@ -0,0 +1,13 @@ +from .config import Config +from .robot import Robot + + +class Processor: + is_safe: bool = True + + def __init__(self, config: Config): + self.config: Config = config + pass + + def process(self, robot: Robot): + pass diff --git a/onshape_to_robot/processor_ball_to_euler.py b/onshape_to_robot/processor_ball_to_euler.py new file mode 100644 index 0000000..968f603 --- /dev/null +++ b/onshape_to_robot/processor_ball_to_euler.py @@ -0,0 +1,75 @@ +import numpy as np +from .processor import Processor +from .config import Config +from .robot import Robot, Link, Joint +from .message import info +import fnmatch + + +class ProcessorBallToEuler(Processor): + """ + Turn ball joints into euler roll/pitch/yaw joints. + """ + + def __init__(self, config: Config): + super().__init__(config) + + # Check if it is enabled in configuration + self.ball_to_euler: bool | list = config.get("ball_to_euler", False) + self.ball_to_euler_order: bool | list = config.get( + "ball_to_euler_order", + "xyz", + values_list=["xyz", "xzy", "zyx", "zxy", "yxz", "yzx"], + ) + + def should_replace(self, joint: Joint) -> bool: + if self.ball_to_euler == True: + return True + elif isinstance(self.ball_to_euler, list): + for entry in self.ball_to_euler: + if fnmatch.fnmatch(entry, joint.name): + return True + return False + + def process(self, robot: Robot): + if self.ball_to_euler: + print(info(f"Replacing balls to euler ({self.ball_to_euler})")) + replaced_joints: list[Joint] = [] + + # Searching for ball joints to replace + for joint in robot.joints: + if joint.joint_type == Joint.BALL and self.should_replace(joint): + parent = joint.parent + children = {} + for letter in self.ball_to_euler_order[:-1]: + body_name = f"{joint.name}_link_{letter}" + new_link = Link(body_name) + children[letter] = new_link + robot.links.append(new_link) + children[self.ball_to_euler_order[-1]] = joint.child + + for letter in self.ball_to_euler_order: + if letter == "x": + axis = np.array([1.0, 0.0, 0.0]) + elif letter == "y": + axis = np.array([0.0, 1.0, 0.0]) + elif letter == "z": + axis = np.array([0.0, 0.0, 1.0]) + + new_joint = Joint( + name=f"{joint.name}_{letter}", + joint_type=Joint.REVOLUTE, + parent=parent, + child=children[letter], + T_world_joint=joint.T_world_joint, + properties=joint.properties.copy(), + axis=axis, + ) + robot.joints.append(new_joint) + parent = children[letter] + + replaced_joints.append(joint) + + # Removing replaced joints + for joint in replaced_joints: + robot.joints.remove(joint) diff --git a/onshape_to_robot/processor_collision_as_visual.py b/onshape_to_robot/processor_collision_as_visual.py new file mode 100644 index 0000000..123218a --- /dev/null +++ b/onshape_to_robot/processor_collision_as_visual.py @@ -0,0 +1,35 @@ +from pathlib import Path +from .message import bright, info, error, warning +from .processor import Processor +from .config import Config +from .robot import Robot, Part +from .geometry import Mesh +import numpy as np + + +class ProcessorCollisionAsVisual(Processor): + """ + This processor will update the part mesh and shapes to turn every collision into visual. + + Can be useful for debugging + """ + + def __init__(self, config: Config): + super().__init__(config) + + # OpenSCAD pure shapes + self.collisions_as_visual: bool = config.get("collisions_as_visual", False) + + def process(self, robot: Robot): + """ + Runs the processor + """ + if self.collisions_as_visual: + print(info("+ Converting collisions to visual")) + for link in robot.links: + for part in link.parts: + for mesh in part.meshes: + mesh.visual = mesh.collision + for shape in part.shapes: + shape.visual = shape.collision + part.prune_unused_geometry() diff --git a/onshape_to_robot/processor_convex_decomposition.py b/onshape_to_robot/processor_convex_decomposition.py new file mode 100644 index 0000000..a5f2425 --- /dev/null +++ b/onshape_to_robot/processor_convex_decomposition.py @@ -0,0 +1,120 @@ +import hashlib +import os +from pathlib import Path +from .message import bright, info, error, warning +from .processor import Processor +from .config import Config +from .robot import Robot, Part +from .geometry import Mesh +import numpy as np +import pickle + + +class ProcessorConvexDecomposition(Processor): + """ + Convex decomposition processor. Runs CoACD algorithm on collision meshes to use a convex approximation. + """ + + is_safe: bool = False + + def __init__(self, config: Config): + super().__init__(config) + + # Enable convex decomposition + self.convex_decomposition: bool = config.get("convex_decomposition", False) + self.rainbow_colors: bool = config.get("rainbow_colors", False) + + self.check_coacd() + + def get_cache_path(self) -> Path: + """ + Return the path to the user cache. + """ + path = Path.home() / ".cache" / "onshape-to-robot-convex-decomposition" + path.mkdir(parents=True, exist_ok=True) + return path + + def check_coacd(self): + if self.convex_decomposition: + print(bright("* Checking CoACD presence...")) + try: + import coacd + import trimesh + except ImportError: + print(bright("Can't import CoACD, disabling convex decomposition.")) + print(info("TIP: consider installing CoACD:")) + print(info("pip install coacd trimesh")) + self.convex_decomposition = False + + def process(self, robot: Robot): + if self.convex_decomposition: + os.makedirs(self.config.asset_path("convex_decomposition"), exist_ok=True) + getcwd = os.getcwd() + os.chdir(self.config.output_directory) + + for link in robot.links: + for part in link.parts: + self.convex_decompose(part) + + os.chdir(getcwd) + + def convex_decompose(self, part: Part): + import coacd + import trimesh + + collision_meshes = [mesh for mesh in part.meshes if mesh.collision] + if len(collision_meshes) > 0: + if len(collision_meshes) > 1: + print( + warning( + f"* Skipping convex decomposition for part {part.name} as it already has multiple collision meshes." + ) + ) + + collision_mesh = collision_meshes[0] + + # Retrieving file SHA1 + sha1 = hashlib.sha1(open(collision_mesh.filename, "rb").read()).hexdigest() + cache_filename = f"{self.get_cache_path()}/{sha1}.pkl" + + if os.path.exists(cache_filename): + print( + info( + f"* Loading cached CoACD decomposition cache for part {part.name}" + ) + ) + with open(cache_filename, "rb") as f: + meshes = pickle.load(f) + else: + mesh = trimesh.load(collision_mesh.filename, force="mesh") + mesh = coacd.Mesh(mesh.vertices, mesh.faces) + meshes = coacd.run_coacd(mesh, max_convex_hull=16) + with open(cache_filename, "wb") as f: + pickle.dump(meshes, f) + + part.collision_meshes = [] + filename = self.config.asset_path( + f"convex_decomposition/{part.name}_%05d.stl" + ) + for k, mesh in enumerate(meshes): + mesh = trimesh.Trimesh(vertices=mesh[0], faces=mesh[1]) + mesh.export(filename % k) + color = ( + np.concatenate([np.random.rand(3), [1.0]]) if self.rainbow_colors else collision_mesh.color + ) + part.meshes.append( + Mesh( + filename % k, + color, + visual=False, + collision=True, + ) + ) + part.collision_meshes.append(filename % k) + + collision_mesh.collision = False + part.prune_unused_geometry() + + print( + info(f"* Decomposed part {part.name} into {len(meshes)} convex shapes.") + ) diff --git a/onshape_to_robot/processor_dummy_base_link.py b/onshape_to_robot/processor_dummy_base_link.py new file mode 100644 index 0000000..d98e2c1 --- /dev/null +++ b/onshape_to_robot/processor_dummy_base_link.py @@ -0,0 +1,36 @@ +import numpy as np +from .processor import Processor +from .config import Config +from .robot import Robot, Link, Joint + + +class ProcessorDummyBaseLink(Processor): + """ + Fixed links processor. + When enabled, crawl all the links, and split them into sublinks containing all one part. + """ + + def __init__(self, config: Config): + super().__init__(config) + + # Check if it is enabled in configuration + self.add_dummy_base_link: bool = config.get("add_dummy_base_link", False) + + def process(self, robot: Robot): + if self.add_dummy_base_link: + new_base_link = Link("base_link") + new_base_link.fixed = True + robot.links.append(new_base_link) + + for base_link in robot.base_links: + robot.joints.append( + Joint( + "base_link_to_" + base_link.name, + "fixed", + new_base_link, + base_link, + np.eye(4), + ) + ) + + robot.base_links = [new_base_link] diff --git a/onshape_to_robot/processor_fixed_links.py b/onshape_to_robot/processor_fixed_links.py new file mode 100644 index 0000000..fb468e1 --- /dev/null +++ b/onshape_to_robot/processor_fixed_links.py @@ -0,0 +1,51 @@ +from .processor import Processor +from .config import Config +from .robot import Robot, Link, Joint +from .message import info +import fnmatch + + +class ProcessorFixedLinks(Processor): + """ + Fixed links processor. + When enabled, crawl all the links, and split them into sublinks containing all one part. + """ + + def __init__(self, config: Config): + super().__init__(config) + + # Check if it is enabled in configuration + self.use_fixed_links: bool | list = config.get("use_fixed_links", False) + + def should_fix_links(self, link_name: str) -> bool: + if self.use_fixed_links == True: + return True + elif isinstance(self.use_fixed_links, list): + for entry in self.use_fixed_links: + if fnmatch.fnmatch(link_name, entry): + return True + return False + + def process(self, robot: Robot): + if self.use_fixed_links: + print(info(f"Using fixed links ({self.use_fixed_links})")) + new_links = [] + for link in robot.links: + if self.should_fix_links(link.name): + for part in link.parts: + part_link = Link(f"{link.name}_{part.name}") + part_link.parts = [part] + new_links.append([link, part_link]) + link.parts = [] + + for parent_link, new_link in new_links: + robot.links.append(new_link) + robot.joints.append( + Joint( + f"{new_link.name}_fixed", + Joint.FIXED, + parent_link, + new_link, + new_link.parts[0].T_world_part, + ) + ) diff --git a/onshape_to_robot/processor_merge_parts.py b/onshape_to_robot/processor_merge_parts.py new file mode 100644 index 0000000..4c2f4b3 --- /dev/null +++ b/onshape_to_robot/processor_merge_parts.py @@ -0,0 +1,159 @@ +import numpy as np +import os +from .config import Config +from .robot import Robot, Link, Part +from .processor import Processor +from .geometry import Mesh +from .message import bright, info, error +from stl import mesh, Mode + + +class ProcessorMergeParts(Processor): + """ + This processor merge all parts into a single one, combining the STL + """ + + def __init__(self, config: Config): + super().__init__(config) + self.merge_stls = config.get("merge_stls", False) + + def process(self, robot: Robot): + if self.merge_stls: + os.makedirs(self.config.asset_path("merged"), exist_ok=True) + getcwd = os.getcwd() + os.chdir(self.config.output_directory) + for link in robot.links: + self.merge_parts(link) + os.chdir(getcwd) + + def load_mesh(self, stl_file: str) -> mesh.Mesh: + return mesh.Mesh.from_file(stl_file) + + def save_mesh(self, mesh: mesh.Mesh, stl_file: str): + # Tweaking STL header to avoid timestamp + # This ensures that same process will result in same STL file + def get_header(name): + header = "onshape-to-robot" + return header[:80].ljust(80, " ") + + mesh.get_header = get_header + mesh.save(stl_file, mode=Mode.BINARY) + + def transform_mesh(self, mesh: mesh.Mesh, matrix: np.ndarray): + rotation = matrix[:3, :3] + translation = matrix[:3, 3] + + def transform(points): + return (rotation @ points.T).T + translation + + mesh.v0 = transform(mesh.v0) + mesh.v1 = transform(mesh.v1) + mesh.v2 = transform(mesh.v2) + mesh.normals = transform(mesh.normals) + + def combine_meshes(self, m1: mesh.Mesh, m2: mesh.Mesh): + return mesh.Mesh(np.concatenate([m1.data, m2.data])) + + def merge_parts(self, link: Link): + print(info(f"+ Merging parts for {link.name}")) + + merge_everything = ( + self.merge_stls != "collision" and self.merge_stls != "visual" + ) + + # Computing the frame where the new part will be located at + _, com, __ = link.get_dynamics() + T_world_com = np.eye(4) + T_world_com[:3, 3] = com + + # Computing a new color, weighting by masses + color = np.zeros(4) + total_mass = 0 + for part in link.parts: + if len(part.meshes): + meshes_color = np.mean([mesh.color for mesh in part.meshes], axis=0) + color += meshes_color * part.mass + total_mass += part.mass + + color /= total_mass + + # Changing shapes frame + merged_shapes = [] + for part in link.parts: + if part.shapes is not None: + for shape in part.shapes: + if merge_everything or shape.is_type(self.merge_stls): + # Changing the shape frame + T_world_shape = part.T_world_part @ shape.T_part_shape + shape.T_part_shape = np.linalg.inv(T_world_com) @ T_world_shape + merged_shapes.append(shape) + + # Merging STL files + def accumulate_meshes(which: str): + mesh = None + for part in link.parts: + for part_mesh in part.meshes: + if part_mesh.is_type(which): + if which == "visual": + part_mesh.visual = False + else: + part_mesh.collision = False + + # Retrieving meshes + part_mesh = self.load_mesh(part_mesh.filename) + + # Expressing meshes in the merged frame + T_com_part = np.linalg.inv(T_world_com) @ part.T_world_part + self.transform_mesh(part_mesh, T_com_part) + + if mesh is None: + mesh = part_mesh + else: + mesh = self.combine_meshes(mesh, part_mesh) + return mesh + + merged_meshes = [] + + if self.merge_stls != "collision": + visual_mesh = accumulate_meshes("visual") + if visual_mesh is not None: + filename = self.config.asset_path( + "merged/" + "/" + link.name + "_visual.stl" + ) + self.save_mesh(visual_mesh, filename) + merged_meshes.append( + Mesh(os.path.relpath(filename, self.config.output_directory), color, visual=True, collision=False) + ) + + if self.merge_stls != "visual": + collision_mesh = accumulate_meshes("collision") + if collision_mesh is not None: + filename = self.config.asset_path( + "merged/" + "/" + link.name + "_collision.stl" + ) + self.save_mesh(collision_mesh, filename) + merged_meshes.append( + Mesh(os.path.relpath(filename, self.config.output_directory), color, visual=False, collision=True) + ) + + mass, com, inertia = link.get_dynamics(T_world_com) + if merge_everything: + # Remove all parts + link.parts = [] + else: + # We keep the existing parts and add a massless part with merged meshes + mass = 0 + inertia *= 0 + + # Replacing parts with a single one + link.parts.append( + Part( + f"{link.name}_parts", + T_world_com, + mass, + com, + inertia, + merged_meshes, + merged_shapes, + ) + ) diff --git a/onshape_to_robot/processor_no_collision_meshes.py b/onshape_to_robot/processor_no_collision_meshes.py new file mode 100644 index 0000000..31fb79d --- /dev/null +++ b/onshape_to_robot/processor_no_collision_meshes.py @@ -0,0 +1,29 @@ +from pathlib import Path +from .message import bright, info, error, warning +from .processor import Processor +from .config import Config +from .robot import Robot, Part + + +class ProcessorNoCollisionMeshes(Processor): + """ + This processor ensures no collision meshes are present in the robot + """ + + def __init__(self, config: Config): + super().__init__(config) + + # OpenSCAD pure shapes + self.no_collision_meshes: bool = config.get("no_collision_meshes", False) + + def process(self, robot: Robot): + """ + Runs the processor + """ + if self.no_collision_meshes: + print(info("+ Removing collision meshes")) + for link in robot.links: + for part in link.parts: + for mesh in part.meshes: + mesh.collision = False + part.prune_unused_geometry() diff --git a/onshape_to_robot/processor_scad.py b/onshape_to_robot/processor_scad.py new file mode 100644 index 0000000..ff7b6e8 --- /dev/null +++ b/onshape_to_robot/processor_scad.py @@ -0,0 +1,192 @@ +import subprocess +import re +import json +import os +from .message import bright, info, error +from .processor import Processor +from .config import Config +from .robot import Robot +from .geometry import Box, Cylinder, Sphere, Shape +import numpy as np + + +class ProcessorScad(Processor): + """ + Scad processor. This processor will parse OpenSCAD files to create pure shapes when available. + + The code is a naive parser of the intermediate CSG file produced by OpenSCAD, gathering Box, Sphere and Cylinders. + """ + + is_safe: bool = False + + def __init__(self, config: Config): + super().__init__(config) + + # OpenSCAD pure shapes + self.use_scads: bool = config.get("use_scads", False) + self.pure_shape_dilatation: float = config.get("pure_shape_dilatation", 0.0) + + if self.use_scads: + self.check_openscad() + + def check_openscad(self): + if self.use_scads: + print(bright("* Checking OpenSCAD presence...")) + try: + subprocess.run(["openscad", "-v"]) + except FileNotFoundError: + print(bright("Can't run openscad -v, disabling OpenSCAD support")) + print(info("TIP: consider installing openscad:")) + print(info("Linux:")) + print(info("sudo add-apt-repository ppa:openscad/releases")) + print(info("sudo apt-get update")) + print(info("sudo apt-get install openscad")) + print(info("Windows:")) + print(info("go to: https://openscad.org/downloads.html ")) + self.use_scads = False + + def process(self, robot: Robot): + if self.use_scads: + getcwd = os.getcwd() + os.chdir(self.config.output_directory) + print(info("+ Parsing OpenSCAD files...")) + for link in robot.links: + for part in link.parts: + converted_meshes = [] + for mesh in part.meshes: + if mesh.collision: + scad_file = mesh.filename.replace(".stl", ".scad") + if os.path.exists(scad_file): + part.shapes += self.parse_scad(scad_file, mesh.color) + converted_meshes.append(mesh) + + for converted_mesh in converted_meshes: + converted_mesh.collision = False + part.prune_unused_geometry() + os.chdir(getcwd) + + def multmatrix_parse(self, parameters: str): + matrix = np.array(json.loads(parameters), dtype=float) + matrix[0, 3] /= 1000.0 + matrix[1, 3] /= 1000.0 + matrix[2, 3] /= 1000.0 + + return matrix + + def cube_parse(self, parameters: str): + results = re.findall(r"^size = (.+), center = (.+)$", parameters) + if len(results) != 1: + raise Exception(f"! Can't parse CSG cube parameters: {parameters}") + extra = np.array([self.pure_shape_dilatation] * 3) + + return ( + extra + np.array(json.loads(results[0][0]), dtype=float) / 1000.0 + ), results[0][1] == "true" + + def cylinder_parse(self, parameters: str): + results = re.findall( + r"h = (.+), r1 = (.+), r2 = (.+), center = (.+)", parameters + ) + if len(results) != 1: + raise Exception(f"! Can't parse CSG cylinder parameters: {parameters}") + result = results[0] + extra = np.array([self.pure_shape_dilatation / 2, self.pure_shape_dilatation]) + + return (extra + np.array([result[0], result[1]], dtype=float) / 1000.0), result[ + 3 + ] == "true" + + def sphere_parse(self, parameters: str): + results = re.findall(r"r = (.+)$", parameters) + if len(results) != 1: + raise Exception(f"! Can't parse CSG sphere parameters: {parameters}") + + return self.pure_shape_dilatation + float(results[0]) / 1000.0 + + def extract_node_parameters(self, line: str): + line = line.strip() + parts = line.split("(", 1) + node = parts[0] + parameters = parts[1] + if parameters[-1] == ";": + parameters = parameters[:-2] + if parameters[-1] == "{": + parameters = parameters[:-3] + return node, parameters + + def translation(self, x: float, y: float, z: float): + m = np.eye(4) + m[:3, 3] = [x, y, z] + + return m + + def parse_csg(self, csg_data: str, color): + shapes: list[Shape] = [] + lines = csg_data.split("\n") + matrices = [] + + for line in lines: + line = line.strip() + if line != "": + if line[-1] == "{": + node, parameters = self.extract_node_parameters(line) + if node == "multmatrix": + matrix = self.multmatrix_parse(parameters) + else: + matrix = np.eye(4) + matrices.append(matrix) + elif line[-1] == "}": + matrices.pop() + else: + node, parameters = self.extract_node_parameters(line) + + transform = np.eye(4) + for matrix in matrices: + transform = transform @ matrix + + if node == "cube": + size, center = self.cube_parse(parameters) + if not center: + transform = transform @ self.translation( + size[0] / 2.0, size[1] / 2.0, size[2] / 2.0 + ) + shapes.append( + Box(transform, size, color, visual=False, collision=True) + ) + if node == "cylinder": + size, center = self.cylinder_parse(parameters) + if not center: + transform = transform @ self.translation( + 0, 0, size[0] / 2.0 + ) + shapes.append( + Cylinder( + transform, + size[0], + size[1], + color, + visual=False, + collision=True, + ) + ) + if node == "sphere": + shapes.append( + Sphere( + transform, + self.sphere_parse(parameters), + color, + visual=False, + collision=True, + ) + ) + + return shapes + + def parse_scad(self, scad_file: str, color: np.ndarray): + tmp_data = os.getcwd() + "/_tmp_data.csg" + os.system("openscad " + scad_file + " -o " + tmp_data) + with open(tmp_data, "r", encoding="utf-8") as stream: + data = stream.read() + os.system("rm " + tmp_data) + + return self.parse_csg(data, color) diff --git a/onshape_to_robot/processor_simplify_stls.py b/onshape_to_robot/processor_simplify_stls.py new file mode 100644 index 0000000..4f9451b --- /dev/null +++ b/onshape_to_robot/processor_simplify_stls.py @@ -0,0 +1,89 @@ +import os +from .config import Config +from .robot import Robot +from .processor import Processor +from .message import bright, info, error + + +class ProcessorSimplifySTLs(Processor): + """ + Allow for mesh simplifications using MeshLab + """ + + def __init__(self, config: Config): + super().__init__(config) + + # STL merge / simplification + self.simplify_stls = config.get("simplify_stls", False) + self.max_stl_size = config.get("max_stl_size", 3) + + if self.simplify_stls: + self.pymeshlab = self.check_meshlab() + + def check_meshlab(self): + print(bright("* Checking pymeshlab presence...")) + try: + import pymeshlab + + return pymeshlab + except ImportError: + self.simplify_stls = False + print(error("No pymeshlab, disabling STL simplification support")) + print(info("TIP: consider installing pymeshlab:")) + print(info("pip install pymeshlab")) + + def process(self, robot: Robot): + if self.simplify_stls: + simplify_all = ( + self.simplify_stls != "visual" and self.simplify_stls != "collision" + ) + simplified = set() + getcwd = os.getcwd() + # Changing directory to output to have relative paths working + os.chdir(self.config.output_directory) + for link in robot.links: + for part in link.parts: + for mesh in part.meshes: + if ( + simplify_all or mesh.is_type(self.simplify_stls) + ) and mesh.filename not in simplified: + simplified.add(mesh.filename) + self.simplify_stl(mesh.filename) + os.chdir(getcwd) + + def reduce_faces(self, filename: str, reduction: float = 0.9): + mesh_set = self.pymeshlab.MeshSet() + + # Add input mesh + mesh_set.load_new_mesh(filename) + + # Apply filter + mesh_set.apply_filter( + "meshing_decimation_quadric_edge_collapse", + targetperc=reduction, + qualitythr=0.5, + preserveboundary=False, + boundaryweight=1, + preservenormal=True, + preservetopology=False, + optimalplacement=True, + planarquadric=True, + qualityweight=False, + planarweight=0.001, + autoclean=True, + selected=False, + ) + + # Save mesh + mesh_set.save_current_mesh(filename) + + def simplify_stl(self, filename: str): + size_M = os.path.getsize(filename) / (1024 * 1024) + + if size_M > self.max_stl_size: + print( + info( + f"+ {os.path.basename(filename)} is {size_M:.2f} M, running mesh simplification" + ) + ) + self.reduce_faces(filename, self.max_stl_size / size_M) diff --git a/onshape_to_robot/processors.py b/onshape_to_robot/processors.py new file mode 100644 index 0000000..512ba77 --- /dev/null +++ b/onshape_to_robot/processors.py @@ -0,0 +1,21 @@ +from .processor_merge_parts import ProcessorMergeParts +from .processor_scad import ProcessorScad +from .processor_simplify_stls import ProcessorSimplifySTLs +from .processor_fixed_links import ProcessorFixedLinks +from .processor_dummy_base_link import ProcessorDummyBaseLink +from .processor_convex_decomposition import ProcessorConvexDecomposition +from .processor_collision_as_visual import ProcessorCollisionAsVisual +from .processor_no_collision_meshes import ProcessorNoCollisionMeshes +from .processor_ball_to_euler import ProcessorBallToEuler + +default_processors = [ + ProcessorBallToEuler, + ProcessorScad, + ProcessorMergeParts, + ProcessorSimplifySTLs, + ProcessorFixedLinks, + ProcessorDummyBaseLink, + ProcessorConvexDecomposition, + ProcessorNoCollisionMeshes, + ProcessorCollisionAsVisual, +] diff --git a/onshape_to_robot/pure_sketch.py b/onshape_to_robot/pure_sketch.py new file mode 100644 index 0000000..74d80f5 --- /dev/null +++ b/onshape_to_robot/pure_sketch.py @@ -0,0 +1,157 @@ +def main(): + import numpy as np + import math + import commentjson as json + import os + import sys, os + from dotenv import load_dotenv, find_dotenv + from colorama import Fore, Back, Style + + load_dotenv(find_dotenv(usecwd=True)) + + if len(sys.argv) < 2: + print("Usage: onshape-to-robot-pure-shape {STL file} [prefix=PureShapes]") + else: + fileName = sys.argv[1] + robotDir = os.path.dirname(fileName) + configFile = os.path.join(robotDir, "config.json") + prefix = "PureShapes" + if len(sys.argv) > 2: + prefix = sys.argv[2] + + from .onshape_api.client import Client + + client = Client(logging=False, creds=configFile) + + parts = fileName.split(".") + parts[-1] = "part" + partFileName = ".".join(parts) + parts[-1] = "scad" + scadFileName = ".".join(parts) + + with open(partFileName, "r", encoding="utf-8") as stream: + part = json.load(stream) + partid = part["partId"] + result = client.get_sketches( + part["documentId"], + part["documentMicroversion"], + part["elementId"], + part["configuration"], + ) + + scad = '% scale(1000) import("' + os.path.basename(fileName) + '");\n' + + sketchDatas = [] + for sketch in result["sketches"]: + if sketch["sketch"].startswith(prefix): + parts = sketch["sketch"].split(" ") + if len(parts) >= 2: + sketch["thickness"] = float(parts[1]) + else: + print( + Fore.RED + + 'ERROR: The sketch name should contain extrusion size (e.g "PureShapes 5.3")' + + Style.RESET_ALL + ) + exit(0) + sketchDatas.append(sketch) + + if len(sketchDatas): + print( + Fore.GREEN + + "* Found " + + str(len(sketchDatas)) + + " PureShapes sketches" + + Style.RESET_ALL + ) + for sketchData in sketchDatas: + # Retrieving sketch transform matrix + m = sketchData["transformMatrix"] + mm = [m[0:4], m[4:8], m[8:12], m[12:16]] + mm[0][3] *= 1000 + mm[1][3] *= 1000 + mm[2][3] *= 1000 + scad += "\n" + scad += "// Sketch " + sketchData["sketch"] + "\n" + scad += "multmatrix(" + str(mm) + ") {" + "\n" + scad += "thickness = %f;\n" % sketchData["thickness"] + scad += "translate([0, 0, -thickness]) {\n" + + boxes = {} + + def boxSet(id, pointName, point): + if id not in boxes: + boxes[id] = {} + boxes[id][pointName] = point + + for entry in sketchData["geomEntities"]: + if entry["entityType"] == "circle": + center = entry["center"] + scad += " translate([%f, %f, 0]) {\n" % ( + center[0] * 1000, + center[1] * 1000, + ) + scad += " cylinder(r=%f,h=thickness);\n" % ( + entry["radius"] * 1000 + ) + scad += " }\n" + if entry["entityType"] == "point": + parts = entry["id"].split(".") + if len(parts) == 3: + if parts[1] == "top" and parts[2] == "start": + boxSet(parts[0], "A", entry["point"]) + if parts[1] == "top" and parts[2] == "end": + boxSet(parts[0], "B", entry["point"]) + if parts[1] == "bottom" and parts[2] == "start": + boxSet(parts[0], "C", entry["point"]) + if parts[1] == "bottom" and parts[2] == "end": + boxSet(parts[0], "D", entry["point"]) + + for id in boxes: + if len(boxes[id]) == 4: + A, B = np.array(boxes[id]["A"]), np.array(boxes[id]["B"]) + C, D = np.array(boxes[id]["C"]), np.array(boxes[id]["D"]) + AB = B - A + + # Making sure that the orientation of the square is correct + AB90 = np.array([-AB[1], AB[0]]) + side = AB90.dot(C - A) + width = np.linalg.norm(B - A) + height = np.linalg.norm(B - D) + if side < 0: + A, B, C, D = C, D, A, B + + AB = B - A + alpha = np.rad2deg(math.atan2(AB[1], AB[0])) + scad += " translate([%f, %f, 0]) {\n" % ( + A[0] * 1000, + A[1] * 1000, + ) + scad += " rotate([0, 0, " + str(alpha) + "]) {" + "\n" + scad += " cube([%f, %f, thickness]);\n" % ( + width * 1000, + height * 1000, + ) + scad += " }\n" + scad += " }\n" + + scad += "}\n" + scad += "}\n" + + with open(scadFileName, "w", encoding="utf-8") as stream: + stream.write(scad) + + directory = os.path.dirname(fileName) + os.system( + "cd " + directory + "; openscad " + os.path.basename(scadFileName) + ) + else: + print( + Fore.RED + + "ERROR: Can't find pure shape sketch in this part" + + Style.RESET_ALL + ) + + +if __name__ == "__main__": + main() diff --git a/onshape_to_robot/robot.py b/onshape_to_robot/robot.py new file mode 100644 index 0000000..53baffc --- /dev/null +++ b/onshape_to_robot/robot.py @@ -0,0 +1,170 @@ +from __future__ import annotations +from copy import deepcopy +import numpy as np +from .geometry import Shape, Mesh + + +class Part: + """ + A part is a single component of a link. + """ + + def __init__( + self, + name: str, + T_world_part: np.ndarray, + mass: float, + com: np.ndarray, + inertia: np.ndarray, + meshes: list[Mesh] = [], + shapes: list[Shape] = [], + ): + self.name: str = name + self.T_world_part: np.ndarray = T_world_part + self.mass: float = mass + self.com: np.ndarray = com + self.inertia: np.ndarray = inertia + self.meshes: list[Mesh] = deepcopy(meshes) + self.shapes: list[Shape] = deepcopy(shapes) + + def prune_unused_geometry(self): + """ + Remove meshes or shapes that are neither visual nor collision. + """ + self.meshes = [mesh for mesh in self.meshes if (mesh.visual or mesh.collision)] + self.shapes = [ + shape for shape in self.shapes if (shape.visual or shape.collision) + ] + + +class Link: + """ + A link of a robot. + """ + + def __init__(self, name: str): + self.name = name + self.parts: list[Part] = [] + self.frames: dict[str, np.ndarray] = {} + self.fixed: bool = False + + def get_dynamics(self, T_world_frame: np.ndarray = np.eye(4)): + """ + Returns the dynamics (mass, com, inertia) in a given frame. + The CoM is expressed in the required frame. + Inertia is expressed around the CoM, aligned with the required frame. + """ + mass = 0 + com = np.zeros(3) + inertia = np.zeros((3, 3)) + T_frame_world = np.linalg.inv(T_world_frame) + + for part in self.parts: + T_frame_part = T_frame_world @ part.T_world_part + com_frame = (T_frame_part @ [*part.com, 1])[:3] + com += com_frame * part.mass + mass += part.mass + + if mass > 1e-9: + com /= mass + + for part in self.parts: + T_frame_part = T_frame_world @ part.T_world_part + com_frame = (T_frame_part @ [*part.com, 1])[:3] + R = T_frame_part[:3, :3] + q = (com_frame - com).reshape((3, 1)) + # See Modern Robotics, (8.26) & (8.27) + inertia += ( + R @ part.inertia @ R.T + ((q.T @ q) * np.eye(3) - q @ q.T) * part.mass + ) + + return mass, com, inertia + + +class Relation: + """ + Represents a relation (for example a gear) with a source joint + """ + + def __init__(self, source_joint: str, ratio: float): + self.source_joint: str = source_joint + self.ratio: float = ratio + + +class Joint: + """ + A joint connects two links. + """ + + # Joint types + FIXED = "fixed" + REVOLUTE = "revolute" + PRISMATIC = "prismatic" + CONTINUOUS = "continuous" + BALL = "ball" + + def __init__( + self, + name: str, + joint_type: str, + parent: Link, + child: Link, + T_world_joint: np.ndarray, + properties: dict = {}, + limits: tuple[float, float] | None = None, + axis: np.ndarray = np.array([0.0, 0.0, 1.0]), + ): + self.name: str = name + self.joint_type: str = joint_type + self.properties: dict = properties + self.parent: Link = parent + self.child: Link = child + self.limits: tuple[float, float] | None = limits + self.axis: np.ndarray = axis + self.T_world_joint: np.ndarray = T_world_joint + self.relation: Relation | None = None + + +class Closure: + """ + A kinematics closure + """ + + FIXED = "fixed" + REVOLUTE = "revolute" + BALL = "ball" + SLIDER = "slider" + + def __init__(self, closure_type: str, frame1: str, frame2: str): + self.closure_type: str = closure_type + self.frame1: str = frame1 + self.frame2: str = frame2 + + +class Robot: + """ + Robot representation produced after requesting Onshape API, and before + exporting (e.g URDF, MuJoCo). + """ + + def __init__(self, name: str): + self.name: str = name + self.links: list[Link] = [] + self.base_links: list[Link] = [] + self.joints: list[Joint] = [] + self.closures: list[Closure] = [] + + def get_link(self, name: str): + for link in self.links: + if link.name == name: + return link + raise ValueError(f"Link {name} not found") + + def get_joint(self, name: str): + for joint in self.joints: + if joint.name == name: + return joint + raise ValueError(f"Joint {name} not found") + + def get_link_joints(self, link: Link): + return [joint for joint in self.joints if joint.parent == link] diff --git a/onshape_to_robot/robot_builder.py b/onshape_to_robot/robot_builder.py new file mode 100644 index 0000000..05f1f17 --- /dev/null +++ b/onshape_to_robot/robot_builder.py @@ -0,0 +1,456 @@ +import numpy as np +import os +import hashlib +import json +import fnmatch +from .geometry import Mesh +from .message import warning, info, success, error, dim, bright +from .assembly import Assembly +from .config import Config +from .robot import Part, Joint, Link, Robot, Relation, Closure +from .csg import process as csg_process + + +class RobotBuilder: + def __init__(self, config: Config): + self.config: Config = config + self.assembly: Assembly = Assembly(config) + self.robot: Robot = Robot(config.robot_name) + + for closure_type, frame1, frame2 in self.assembly.closures: + self.robot.closures.append(Closure(closure_type, frame1, frame2)) + + self.unique_names = {} + self.stl_filenames: dict = {} + + for node in self.assembly.root_nodes: + link = self.build_robot(node) + self.robot.base_links.append(link) + + def part_is_ignored(self, name: str, what: str) -> bool: + """ + Checks if a given part should be ignored by config + """ + ignored = False + + # Removing <1>, <2> etc. suffix + name = "<".join(name.split("<")[:-1]).strip() + + for entry in self.config.ignore: + to_ignore = True + match_entry = entry + if entry[0] == "!": + to_ignore = False + match_entry = entry[1:] + + if fnmatch.fnmatch(name.lower(), match_entry.lower()): + if ( + self.config.ignore[entry] == "all" + or self.config.ignore[entry] == what + ): + ignored = to_ignore + + return ignored + + def slugify(self, value: str) -> str: + """ + Turns a value into a slug + """ + return "".join(c if c.isalnum() else "_" for c in value).strip("_") + + def printable_configuration(self, instance: dict) -> str: + """ + Retrieve configuration enums to replace "List_..." with proper enum names + """ + configuration = instance["configuration"] + + if instance["configuration"] != "default": + if "documentVersion" in instance: + version = instance["documentVersion"] + wmv = "v" + else: + version = instance["documentMicroversion"] + wmv = "m" + elements = self.assembly.client.elements_configuration( + instance["documentId"], + version, + instance["elementId"], + wmv=wmv, + linked_document_id=self.config.document_id, + ) + for entry in elements["configurationParameters"]: + type_name = entry["typeName"] + message = entry["message"] + + if type_name.startswith("BTMConfigurationParameterEnum"): + parameter_name = message["parameterName"] + parameter_id = message["parameterId"] + configuration = configuration.replace(parameter_id, parameter_name) + + return configuration + + def part_name(self, part: dict, include_configuration: bool = False) -> str: + """ + Retrieve the name from a part. + i.e "Base link <1>" -> "base_link" + """ + name = part["name"] + parts = name.split(" ") + del parts[-1] + base_part_name = self.slugify("_".join(parts).lower()) + + if not include_configuration: + return base_part_name + + # Only add configuration to name if its not default and not a very long configuration (which happens for library parts like screws) + configuration = self.printable_configuration(part) + if configuration != "default" and self.config.include_configuration_suffix: + if len(configuration) < 40: + parts += ["_" + configuration.replace("=", "_").replace(" ", "_")] + else: + parts += ["_" + hashlib.md5(configuration.encode("utf-8")).hexdigest()] + + return self.slugify("_".join(parts).lower()) + + def unique_name(self, part: dict, type: str): + """ + Get unique part name (plate, plate_2, plate_3, ...) + In the case where multiple parts have the same name in Onshape, they will result in different names in the URDF + """ + while True: + name = self.part_name(part, include_configuration=True) + + if type not in self.unique_names: + self.unique_names[type] = {} + + if name in self.unique_names[type]: + self.unique_names[type][name] += 1 + name = f"{name}_{self.unique_names[type][name]}" + else: + self.unique_names[type][name] = 1 + name = name + + if name not in [frame.name for frame in self.assembly.frames]: + return name + + def instance_request_params(self, instance: dict) -> dict: + """ + Build parameters to make an API call for a given instance + """ + params = {} + + if "documentVersion" in instance: + params["wmvid"] = instance["documentVersion"] + params["wmv"] = "v" + else: + params["wmvid"] = instance["documentMicroversion"] + params["wmv"] = "m" + + params["did"] = instance["documentId"] + params["eid"] = instance["elementId"] + params["linked_document_id"] = self.config.document_id + params["configuration"] = instance["configuration"] + + return params + + def get_stl_filename(self, instance: dict) -> str: + """ + Get a STL filename unique to the instance + """ + exact_instance = ( + instance["documentId"], + instance["documentMicroversion"], + instance["elementId"], + instance["configuration"], + instance["partId"], + ) + + if exact_instance not in self.stl_filenames: + part_name_config = self.part_name(instance, True) + stl_filename = part_name_config + k = 1 + while stl_filename in self.stl_filenames.values(): + k += 1 + stl_filename = part_name_config + f"__{k}" + if k != 1: + print( + warning( + f'WARNING: Parts with same name "{part_name_config}", incrementing STL name to "{stl_filename}"' + ) + ) + self.stl_filenames[exact_instance] = stl_filename + + return self.stl_filenames[exact_instance] + + def get_stl(self, instance: dict) -> str: + """ + Download and store STL file + """ + os.makedirs(self.config.asset_path(""), exist_ok=True) + + stl_filename = self.get_stl_filename(instance) + filename = stl_filename + ".stl" + + params = self.instance_request_params(instance) + stl = self.assembly.client.part_studio_stl_m( + **params, + partid=instance["partId"], + ) + with open(self.config.asset_path(filename), "wb") as stream: + stream.write(stl) + + # Storing metadata for imported instances in the .part file + stl_metadata = stl_filename + ".part" + with open( + self.config.asset_path(stl_metadata), "w", encoding="utf-8" + ) as stream: + json.dump(instance, stream, indent=4, sort_keys=True) + + return self.config.asset_path(filename) + + def get_color(self, instance: dict) -> np.ndarray: + """ + Retrieve the color of a part + """ + if self.config.color is not None: + color = np.array(self.config.color) + else: + params = self.instance_request_params(instance) + metadata = self.assembly.client.part_get_metadata( + **params, + partid=instance["partId"], + ) + + color = np.array([0.5, 0.5, 0.5, 1.0]) + + # XXX: There must be a better way to retrieve the part color + for entry in metadata["properties"]: + if ( + "value" in entry + and type(entry["value"]) is dict + and "color" in entry["value"] + ): + rgb = entry["value"]["color"] + a = entry["value"]["opacity"] + color = np.array([rgb["red"], rgb["green"], rgb["blue"], a]) / 255.0 + + return color + + def get_dynamics(self, instance: dict) -> tuple: + """ + Retrieve the dynamics (mass, com, inertia) of a given instance + """ + if self.config.no_dynamics: + mass = 0 + com = [0] * 3 + inertia = [0] * 12 + else: + if instance["isStandardContent"]: + mass_properties = self.assembly.client.standard_cont_mass_properties( + instance["documentId"], + instance["documentVersion"], + instance["elementId"], + instance["partId"], + configuration=instance["configuration"], + linked_document_id=self.config.document_id, + ) + else: + params = self.instance_request_params(instance) + mass_properties = self.assembly.client.part_mass_properties( + **params, + partid=instance["partId"], + ) + + body = mass_properties["bodies"].get(instance["partId"]) + + # Surfaces (sheet bodies) have no volume and thus no dynamics. Onshape + # reports them with "hasMass": false (and may omit them from "bodies" + # entirely), so relying on presence in "bodies" alone misses them. + if body is None or not body.get("hasMass", True): + print( + warning( + f"WARNING: part {instance['name']} has no dynamics (maybe it is a surface)" + ) + ) + return 0.0, np.zeros(3), np.zeros((3, 3)) + + mass = body["mass"][0] + com = body["centroid"] + inertia = body["inertia"] + + if abs(mass) < 1e-9: + print( + warning( + f"WARNING: part {instance['name']} has no mass, maybe you should assign a material to it ?" + ) + ) + + return mass, com[:3], np.reshape(inertia[:9], (3, 3)) + + def add_part(self, occurrence: dict): + """ + Add a part to the current link + """ + instance = occurrence["instance"] + + if instance["suppressed"]: + return + + if instance["partId"] == "": + print(warning(f"WARNING: Part '{instance['name']}' has no partId")) + return + + part_name = instance["name"] + extra = "" + if instance["configuration"] != "default": + extra = dim( + " (configuration: " + self.printable_configuration(instance) + ")" + ) + symbol = "+" + if self.part_is_ignored(part_name, "visual") or self.part_is_ignored( + part_name, "collision" + ): + symbol = "-" + extra += dim(" / ") + if self.part_is_ignored(part_name, "visual"): + extra += dim("(ignoring visual)") + if self.part_is_ignored(part_name, "collision"): + extra += dim(" (ignoring collision)") + + print(success(f"{symbol} Adding part {part_name}{extra}")) + + if self.part_is_ignored(part_name, "visual") and self.part_is_ignored( + part_name, "collision" + ): + stl_file = None + else: + stl_file = self.get_stl(instance) + + # Obtain metadatas about part to retrieve color + color = self.get_color(instance) + + # Obtain the instance dynamics + mass, com, inertia = self.get_dynamics(instance) + + # Obtain part pose + T_world_part = np.array(occurrence["transform"]).reshape(4, 4) + + # Adding non-ignored meshes + meshes = [] + stl_relpath = ( + os.path.relpath(stl_file, self.config.output_directory) + if stl_file is not None + else None + ) + mesh = Mesh(stl_relpath, color) + if self.part_is_ignored(part_name, "visual"): + mesh.visual = False + if self.part_is_ignored(part_name, "collision"): + mesh.collision = False + if mesh.visual or mesh.collision: + meshes.append(mesh) + + # Get unique part name (with _2, _3 suffixes for duplicates) + unique_part_name = self.unique_name(instance, "part") + + # Apply geom_properties based on unique part name pattern matching + for mesh in meshes: + visual_properties = {} + collision_properties = {} + + for pattern_name in self.config.geom_properties: + if fnmatch.fnmatch(unique_part_name, pattern_name): + pattern_props = self.config.geom_properties[pattern_name] + + # Check for nested visual/collision structure + has_nested = ( + "visual" in pattern_props or "collision" in pattern_props + ) + + if has_nested: + visual_properties = { + **visual_properties, + **pattern_props.get("visual", {}), + } + collision_properties = { + **collision_properties, + **pattern_props.get("collision", {}), + } + else: + # Apply to both if not nested + visual_properties = {**visual_properties, **pattern_props} + collision_properties = {**collision_properties, **pattern_props} + + mesh.visual_properties = visual_properties + mesh.collision_properties = collision_properties + + part = Part( + unique_part_name, + T_world_part, + mass, + com, + inertia, + meshes, + ) + + self.robot.links[-1].parts.append(part) + + def build_robot(self, body_id: int): + """ + Add recursively body nodes to the robot description. + """ + instance = self.assembly.body_instance(body_id) + + if body_id in self.assembly.link_names: + link_name = self.assembly.link_names[body_id] + else: + link_name = self.unique_name(instance, "link") + + # Adding all the parts in the current link + link = Link(link_name) + self.robot.links.append(link) + for occurrence in self.assembly.body_occurrences(body_id): + if occurrence["instance"]["type"] == "Part": + self.add_part(occurrence) + if occurrence["fixed"]: + link.fixed = True + + # Adding frames to the link + for frame in self.assembly.frames: + if frame.body_id == body_id: + self.robot.links[-1].frames[frame.name] = frame.T_world_frame + + for children_body in self.assembly.tree_children[body_id]: + dof = self.assembly.get_dof(body_id, children_body) + child_body = dof.other_body(body_id) + T_world_axis = dof.T_world_mate.copy() + + properties = self.config.joint_properties.get("default", {}) + for joint_name in self.config.joint_properties: + if fnmatch.fnmatch(dof.name, joint_name): + properties = { + **properties, + **self.config.joint_properties[joint_name], + } + + joint = Joint( + dof.name, + dof.joint_type, + link, + None, + T_world_axis, + properties, + dof.limits, + dof.axis, + ) + if dof.name in self.assembly.relations: + source, ratio = self.assembly.relations[dof.name] + joint.relation = Relation(source, ratio) + + # The joint is added before the recursive call, ensuring items in robot.joints has the + # same order as recursive calls on the tree + self.robot.joints.append(joint) + + joint.child = self.build_robot(child_body) + + return link diff --git a/onshape_to_robot/simulation.py b/onshape_to_robot/simulation.py new file mode 100644 index 0000000..91f8154 --- /dev/null +++ b/onshape_to_robot/simulation.py @@ -0,0 +1,541 @@ +from transforms3d.quaternions import mat2quat, quat2mat +import math +import sys +import time +import numpy as np +import pybullet as p +from time import sleep +import os +import re + + +class Simulation: + """ + A Bullet simulation involving Onshape to robot model + """ + + def __init__(self, robotPath, floor=True, fixed=False, transparent=False, gui=True, ignore_self_collisions=False, + realTime=True, panels=False, useUrdfInertia=True, dt=0.002, physicsClient = None): + """Creates an instance of humanoid simulation + + Keyword Arguments: + field {bool} -- enable the display of the field (default: {False}) + fixed {bool} -- makes the base of the robot floating/fixed (default: {False}) + transparent {bool} -- makes the robot transparent (default: {False}) + gui {bool} -- enables the gui visualizer, if False it will runs headless (default {True}) + realTime {bool} -- try to have simulation in real time (default {True}) + panels {bool} -- show/hide the user interaction pyBullet panels (default {False}) + useUrdfInertia {bool} -- use URDF from URDF file (default {True}) + dt {float} -- time step (default {0.002}) + """ + + self.dir = os.path.dirname(os.path.abspath(__file__)) + self.gui = gui + self.realTime = realTime + self.t = 0 + self.start = time.time() + self.dt = dt + self.mass = None + + # Debug lines drawing + self.lines = [] + self.currentLine = 0 + self.lastLinesDraw = 0 + self.lineColors = [[1, 0, 0], [0, 1, 0], [ + 0, 0, 1], [1, 1, 0], [1, 0, 1], [0, 1, 1]] + + # Instanciating bullet + if physicsClient is None: + if gui: + physicsClient = p.connect(p.GUI) + else: + physicsClient = p.connect(p.DIRECT) + p.setGravity(0, 0, -9.81) + + # Light GUI + if not panels: + p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) + p.configureDebugVisualizer( + p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0) + p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0) + p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0) + + p.configureDebugVisualizer(p.COV_ENABLE_MOUSE_PICKING, 1) + + # Loading floor and/or plane ground + if floor: + self.floor = p.loadURDF(self.dir+'/bullet/plane.urdf') + else: + self.floor = None + + # Loading robot + startPos = [0, 0, 0] + if not fixed: + startPos[2] = 1 + startOrientation = p.getQuaternionFromEuler([0, 0, 0]) + flags = 0 if ignore_self_collisions else p.URDF_USE_SELF_COLLISION + if useUrdfInertia: + flags += p.URDF_USE_INERTIA_FROM_FILE + self.robot = p.loadURDF(robotPath, + startPos, startOrientation, + flags=flags, useFixedBase=fixed) + + # Setting frictions parameters to default ones + self.setFloorFrictions() + + # Engine parameters + p.setPhysicsEngineParameter(fixedTimeStep=self.dt, maxNumCmdPer1ms=0) + # p.setRealTimeSimulation(0) + # p.setPhysicsEngineParameter(numSubSteps=1) + + # Retrieving joints and frames + self.joints = {} + self.passive_joints = {} + self.jointsInfos = {} + self.jointsIndexes = {} + self.frames = {} + self.maxTorques = {} + + # Collecting the available joints + n = 0 + for k in range(p.getNumJoints(self.robot)): + jointInfo = p.getJointInfo(self.robot, k) + name = jointInfo[1].decode('utf-8') + + if 'passive' in name: + self.passive_joints[name] = k + elif not name.endswith('_fixed'): + if '_frame' in name: + self.frames[name] = k + else: + self.jointsIndexes[name] = n + n += 1 + self.joints[name] = k + self.jointsInfos[name] = { + 'type': jointInfo[2] + } + if jointInfo[8] < jointInfo[9]: + self.jointsInfos[name]['lowerLimit'] = jointInfo[8] + self.jointsInfos[name]['upperLimit'] = jointInfo[9] + + # Changing robot opacity if transparent set to true + if transparent: + for k in range(p.getNumJoints(self.robot)): + p.changeVisualShape(self.robot, k, rgbaColor=[ + 0.3, 0.3, 0.3, 0.3]) + + print('* Found '+str(len(self.joints))+' DOFs') + print('* Found '+str(len(self.frames))+' frames') + + def setFloorFrictions(self, lateral=1, spinning=-1, rolling=-1): + """Sets the frictions with the plane object + + Keyword Arguments: + lateral {float} -- lateral friction (default: {1.0}) + spinning {float} -- spinning friction (default: {-1.0}) + rolling {float} -- rolling friction (default: {-1.0}) + """ + if self.floor is not None: + p.changeDynamics(self.floor, -1, lateralFriction=lateral, + spinningFriction=spinning, rollingFriction=rolling) + + def lookAt(self, target): + """Control the look of the visualizer camera + + Arguments: + target {tuple} -- target as (x,y,z) tuple + """ + if self.gui: + params = p.getDebugVisualizerCamera() + p.resetDebugVisualizerCamera( + params[10], params[8], params[9], target) + + def getRobotPose(self): + """Gets the robot (origin) position + + Returns: + (tuple(3), tuple(3)) -- (x,y,z), (roll, pitch, yaw) + """ + pose = p.getBasePositionAndOrientation(self.robot) + return (pose[0], p.getEulerFromQuaternion(pose[1])) + + def frameToWorldMatrix(self, frame): + """Gets the given frame to world matrix transformation. can be a frame name + from URDF/SDF or "origin" for the part origin + + Arguments: + frame {str} -- frame name + + Returns: + np.matrix -- a 4x4 matrix + """ + + if frame == 'origin': + frameToWorldPose = p.getBasePositionAndOrientation(self.robot) + else: + frameToWorldPose = p.getLinkState(self.robot, self.frames[frame]) + + return self.poseToMatrix(frameToWorldPose) + + def transformation(self, frameA, frameB): + """Transformation matrix AtoB + + Arguments: + frameA {str} -- frame A name + frameB {str} -- frame B name + + Returns: + np.matrix -- A 4x4 matrix + """ + AtoWorld = self.frameToWorldMatrix(frameA) + BtoWorld = self.frameToWorldMatrix(frameB) + + return np.linalg.inv(BtoWorld) * AtoWorld + + def poseToMatrix(self, pose): + """Converts a pyBullet pose to a transformation matrix""" + translation = pose[0] + quaternion = pose[1] + + # NOTE: PyBullet quaternions are x, y, z, w + rotation = quat2mat([quaternion[3], quaternion[0], + quaternion[1], quaternion[2]]) + + m = np.identity(4) + m[0:3, 0:3] = rotation + m.T[3, 0:3] = translation + + return np.matrix(m) + + def matrixToPose(self, matrix): + """Converts a transformation matrix to a pyBullet pose""" + arr = np.array(matrix) + translation = list(arr.T[3, 0:3]) + quaternion = mat2quat(arr[0:3, 0:3]) + + # NOTE: PyBullet quaternions are x, y, z, w + quaternion = [quaternion[1], quaternion[2], + quaternion[3], quaternion[0]] + + return translation, quaternion + + def setRobotPose(self, pos, orn): + """Sets the robot (origin) pose + + Arguments: + pos {tuple} -- (x,y,z) position + orn {tuple} -- (x,y,z,w) quaternions + """ + p.resetBasePositionAndOrientation(self.robot, pos, orn) + + def reset(self, height=0.5, orientation='straight'): + """Resets the robot for experiment (joints, robot position, simulator time) + + Keyword Arguments: + height {float} -- height of the reset (m) (default: {0.55}) + orientation {str} -- orientation (straight, front or back) of the robot (default: {'straight'}) + """ + self.lines = [] + self.t = 0 + self.start = time.time() + + # Resets the robot position + orn = [0, 0, 0] + if orientation == 'front': + orn = [0, math.pi/2, 0] + elif orientation == 'back': + orn = [0, -math.pi/2, 0] + self.resetPose([0, 0, height], p.getQuaternionFromEuler(orn)) + + # Reset the joints to 0 + for entry in self.joints.values(): + p.resetJointState(self.robot, entry, 0) + + def resetPose(self, pos, orn): + """Called by reset() with the robot pose + + Arguments: + pos {tuple} -- (x,y,z) position + orn {tuple} -- (x,y,z,w) quaternions + """ + self.setRobotPose(pos, orn) + + def getFrame(self, frame): + """Gets the given frame + + Arguments: + frame {str} -- frame name + + Returns: + tuple -- (pos, orn), where pos is (x, y, z) and orn is quaternions (x, y, z, w) + """ + jointState = p.getLinkState(self.robot, self.frames[frame]) + return (jointState[0], jointState[1]) + + def getFrames(self): + """Gets the available frames in the current robot model + + Returns: + dict -- dict of str -> (pos, orientation) + """ + frames = {} + + for name in self.frames.keys(): + jointState = p.getLinkState(self.robot, self.frames[name]) + pos = jointState[0] + orientation = p.getEulerFromQuaternion(jointState[1]) + frames[name] = [pos, orientation] + + return frames + + def getVelocity(self, frame): + """Gets the velocity of the given frame + + Arguments: + frame {str} -- frame name + + Returns: + tuple -- (linear, angular) + """ + jointState = p.getLinkState(self.robot, self.frames[frame], computeLinkVelocity=True) + return (jointState[6], jointState[7]) + + def resetJoints(self, joints): + """Reset all the joints to a given position + + Arguments: + joints {dict} -- dict of joint name -> angle (float, radian) + """ + for name in joints: + p.resetJointState(self.robot, self.joints[name], joints[name]) + + def setJoints(self, joints): + """Set joint targets for motor control in simulation + + Arguments: + joints {dict} -- dict of joint name -> angle (float, radian) + + Raises: + Exception: if a joint is not found, exception is raised + + Returns: + applied {dict} -- dict of joint states (position, velocity, reaction forces, applied torque) + """ + applied = {} + + for name in self.passive_joints: + p.setJointMotorControl2(self.robot, self.passive_joints[name], controlMode=p.VELOCITY_CONTROL, force=0) + + for name in joints.keys(): + if name in self.joints: + if name.endswith('_speed'): + p.setJointMotorControl2( + self.robot, self.joints[name], p.VELOCITY_CONTROL, targetVelocity=joints[name]) + else: + if name in self.maxTorques: + maxTorque = self.maxTorques[name] + p.setJointMotorControl2( + self.robot, self.joints[name], p.POSITION_CONTROL, joints[name], force=maxTorque) + else: + p.setJointMotorControl2( + self.robot, self.joints[name], p.POSITION_CONTROL, joints[name]) + + applied[name] = p.getJointState(self.robot, self.joints[name]) + else: + raise Exception("Can't find joint %s" % name) + + return applied + + def getJoints(self): + """Get all the joints names + + Returns: + list -- list of str, with joint names + """ + return self.joints.keys() + + def getJointsInfos(self, name): + """Get informations about a joint + + Return: + list -- a list with key type, lowerLimit & upperLimit (if defined) + """ + + return self.jointsInfos[name] + + def getRobotMass(self): + """Returns the robot mass + + Returns: + float -- the robot mass (kg) + """ + if self.mass is None: + k = -1 + self.mass = 0 + while True: + if k == -1 or p.getLinkState(self.robot, k) is not None: + d = p.getDynamicsInfo(self.robot, k) + self.mass += d[0] + else: + break + k += 1 + + return self.mass + + def getCenterOfMassPosition(self): + """Returns center of mass of the robot + + Returns: + pos -- (x, y, z) robot center of mass + """ + + k = -1 + mass = 0 + com = np.array([0., 0., 0.]) + while True: + if k == -1: + pos, _ = p.getBasePositionAndOrientation(self.robot) + else: + res = p.getLinkState(self.robot, k) + if res is None: + break + pos = res[0] + + d = p.getDynamicsInfo(self.robot, k) + m = d[0] + com += np.array(pos) * m + mass += m + + k += 1 + + return com / mass + + def addDebugPosition(self, position, color=None, duration=30): + """Adds a debug position to be drawn as a line + + Arguments: + position {tuple} -- (x,y,z) (m) + + Keyword Arguments: + color {tuple} -- (r,g,b) (0->1) (default: {None}) + duration {float} -- line duration on screen before disapearing (default: {30}) + """ + if color is None: + color = self.lineColors[self.currentLine % len(self.lineColors)] + + if self.currentLine >= len(self.lines): + self.lines.append({}) + + self.lines[self.currentLine]['update'] = True + self.lines[self.currentLine]['to'] = position + self.lines[self.currentLine]['color'] = color + self.lines[self.currentLine]['duration'] = duration + + self.currentLine += 1 + + def drawDebugLines(self): + """Updates the drawing of debug lines""" + self.currentLine = 0 + if time.time() - self.lastLinesDraw > 0.05: + for line in self.lines: + if 'from' in line: + if line['update'] == True: + p.addUserDebugLine( + line['from'], line['to'], line['color'], 2, line['duration']) + line['update'] = False + else: + del line['from'] + line['from'] = line['to'] + + self.lastLinesDraw = time.time() + + def contactPoints(self): + """Gets all contact points and forces + + Returns: + list -- list of entries (link_name, position in m, normal force vector, force in N) + """ + result = [] + contacts = p.getContactPoints(bodyA=self.floor, bodyB=self.robot) + for contact in contacts: + link_index = contact[4] + if link_index >= 0: + link_name = (p.getJointInfo( + self.robot, link_index)[12]).decode() + else: + link_name = 'base' + result.append((link_name, contact[6], contact[7], contact[9])) + + return result + + def autoCollisions(self): + """Returns the total amount of N in autocollisions (not with ground) + + Returns: + float -- Newtons of collisions not with ground + """ + total = 0 + for k in range(1, p.getNumJoints(self.robot)): + contacts = p.getContactPoints(bodyA=k) + for contact in contacts: + if contact[2] != self.floor: + total += contact[9] + return total + + def addConstraint(self, frameA, frameB, constraint = p.JOINT_POINT2POINT): + """Adds a constraint between two given frames + + Args: + frameA (str): frame A name + frameB (str): frame A name + constraint (int, optional): pyBullet joint type. Defaults to p.JOINT_POINT2POINT. + + Returns: + int: returns from pybullet createConstraint + """ + infosA = p.getJointInfo(self.robot, self.frames[frameA]) + infosB = p.getJointInfo(self.robot, self.frames[frameB]) + + st = p.getLinkState(self.robot, infosA[16]) + T_world_parentA = self.poseToMatrix(st[:2]) + T_world_childA = self.poseToMatrix(self.getFrame(frameA)) + T_parentA_childA = np.linalg.inv(T_world_parentA) * T_world_childA + childApose = self.matrixToPose(T_parentA_childA) + + st = p.getLinkState(self.robot, infosB[16]) + T_world_parentB = self.poseToMatrix(st[:2]) + T_world_childB = self.poseToMatrix(self.getFrame(frameB)) + T_parentB_childB = np.linalg.inv(T_world_parentB) * T_world_childB + childBpose = self.matrixToPose(T_parentB_childB) + + c = p.createConstraint( + self.robot, + infosA[16], + self.robot, + infosB[16], + constraint, + [0.0, 0.0, 0.0], + childApose[0], + childBpose[0], + childApose[1], + childBpose[1], + ) + + p.changeConstraint(c, maxForce=1e3) + + return c + + def execute(self): + """Executes the simulaiton infinitely (blocks)""" + while True: + self.tick() + + def tick(self): + """Ticks one step of simulation. If realTime is True, sleeps to compensate real time""" + self.t += self.dt + self.drawDebugLines() + + p.stepSimulation() + delay = self.t - (time.time() - self.start) + if delay > 0 and self.realTime: + time.sleep(delay) diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..a2d8d9c --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,44 @@ +[build-system] +requires = ["uv_build>=0.9.9,<0.10.0"] +build-backend = "uv_build" + +[project] +name = "onshape_to_robot" +version = "1.8.3" +description = "Converting Onshape assembly to robot definition (URDF, SDF, MuJoCo) through Onshape API" +readme = "README.md" +requires-python = ">=3.9" +classifiers = [ + "Programming Language :: Python :: 3", + "License :: OSI Approved :: MIT License", + "Operating System :: OS Independent", +] +dependencies = [ + "numpy", + "requests", + "commentjson", + "colorama>=0.4.6", + "numpy-stl", + "transforms3d", + "python-dotenv", +] + +[project.urls] +Homepage = "https://onshape-to-robot.readthedocs.io/" +Repository = "https://github.com/rhoban/onshape-to-robot/" + +[project.scripts] +onshape-to-robot = "onshape_to_robot:export.main" +onshape-to-robot-bullet = "onshape_to_robot:bullet.main" +onshape-to-robot-mujoco = "onshape_to_robot:mujoco.main" +onshape-to-robot-clear-cache = "onshape_to_robot:clear_cache.main" +onshape-to-robot-edit-shape = "onshape_to_robot:edit_shape.main" +onshape-to-robot-pure-sketch = "onshape_to_robot:pure_sketch.main" + +[project.optional-dependencies] +pymeshlab = ["pymeshlab"] +pybullet = ["pybullet"] +mujoco = ["mujoco"] + +[tool.uv.build-backend] +module-root = "" diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..08f1687 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,15 @@ +# Core dependencies +colorama>=0.4.6 +commentjson +numpy +numpy-stl +pybullet +mujoco +requests +sphinx +sphinx-rtd-theme +transforms3d +python-dotenv + +# Optional dependencies +# pymeshlab \ No newline at end of file