Import upstream snapshot 80e710700aac9573a2230f74f7ce9e094833a0bc

Upstream: https://github.com/Rhoban/onshape-to-robot
Upstream-Commit: 80e710700aac9573a2230f74f7ce9e094833a0bc
Upstream-Branch: master

.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

.gitignore

@@ -0,0 +1,10 @@
+**.pyc
+.vscode
+robots
+data
+**/cache/
+dist
+build
+onshape_to_robot.egg-info
+.env
+uv.lock

.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

LICENSE

@@ -0,0 +1,7 @@
+Copyright 2019-2099 Rhoban Team <Grégoire Passault>
+
+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.

MANIFEST.in

@@ -0,0 +1,2 @@
+include README-pypi.md
+

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

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)
+

README.md

@@ -0,0 +1,12 @@
+# Onshape to Robot (URDF, SDF, MuJoCo)
+
+<p align="center">
+<img src="docs/source/_static/img/main.png" />
+</p>
+
+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)

docs/.gitignore

@@ -0,0 +1 @@
+build

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)

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

docs/requirements.txt

@@ -0,0 +1 @@
+sphinx-rtd-theme

docs/source/_static/css/custom.css

@@ -0,0 +1,4 @@
+
+img.padding {
+    margin-bottom: 20px !important;
+}

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

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'

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 <exporter_urdf>`
+    * :doc:`SDF specific entries <exporter_sdf>`
+    * :doc:`MuJoCo specific entries <exporter_mujoco>`
+* :doc:`Processors <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 <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.

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 <https://github.com/Rhoban/onshape-to-robot/blob/master/onshape_to_robot/processors.py>`_. 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 <https://github.com/Rhoban/onshape-to-robot/blob/master/onshape_to_robot/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 <https://github.com/Rhoban/onshape-to-robot/blob/master/onshape_to_robot/processors.py>`_.

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 <https://cad.onshape.com/documents/7adc786257f47ce24706bb32/w/774dd3de6bd5bfd65fb4462b/e/c60f72b9088ac4e5058b8904?renderMode=0&uiState=67b64076077d3a02bf5e1c0f>`_
+
+.. 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 <draw-frames>` to ``true`` in the :doc:`config <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<processor_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 ``<mimic>`` in :doc:`URDF <exporter_urdf>` and :doc:`SDF <exporter_sdf>` formats, and as equality constraints in :doc:`MuJoCo <exporter_mujoco>`.
+
+.. image:: _static/img/gear.png
+    :align: center

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 <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 ``<joint ...>`` attributes:
+
+    * ``frictionloss``
+    * ``damping``
+    * ``armature``
+    * ``stiffness``
+
+* The following are reflected as actuator (``<position ...>`` 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 ``<geom ...>`` 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

docs/source/exporter_sdf.rst

@@ -0,0 +1,107 @@
+SDF
+===
+
+Introduction
+-------------
+
+The `SDF format <http://sdformat.org/>`_ is an extension of URDF extensively used in ROS.
+
+
+* When using SDF, frames will be exported as ``<frame>`` 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 ``<joint effort=...>`` tag)
+* ``max_velocity``: The maximum velocity of the joint (added in the ``<joint velocity=...>`` tag)
+* ``friction``: The friction of the joint (added in the ``<joint_properties friction=...>`` tag)
+* ``type``: Sets the joint type (changing the ``<joint type="...">`` 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 ``<visual>`` or ``<collision>`` 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
+

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 ``<joint effort=...>`` tag)
+* ``max_velocity``: The maximum velocity of the joint (added in the ``<joint velocity=...>`` tag)
+* ``friction``: The friction of the joint (added in the ``<joint_properties friction=...>`` tag)
+* ``type``: Sets the joint type (changing the ``<joint type="...">`` 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 ``<visual>`` or ``<collision>`` 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 ``<robot>`` 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 <fixed-robot>` to the world.
+
+.. note::
+
+    In PyBullet, such bodies are indeed recognized as fixed

docs/source/getting_started.rst

@@ -0,0 +1,116 @@
+Getting started
+===============
+
+Installing the package
+----------------------
+
+Run the following to install onshape-to-robot from `pypi <https://pypi.org/project/onshape-to-robot/>`_:
+
+.. 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 <https://cad.onshape.com/user/developer/apiKeys>`_
+
+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 <https://onshape-public.github.io/docs/auth/oauth/>`_. 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 <design>`.
+* There are some options you might want to specify in the :doc:`config.json <config>` file.
+* Have a look at the `examples <https://github.com/rhoban/onshape-to-robot-examples>`_ available on GitHub.

docs/source/index.rst

@@ -0,0 +1,40 @@
+Onshape-to-robot documentation
+==============================
+
+.. raw:: html
+
+    <center>
+    <video width="70%" controls autoplay muted loop>
+    <source src="https://github.com/Rhoban/onshape-to-robot-examples/raw/refs/heads/master/videos/sigmaban.mp4" type="video/mp4">
+    </video>
+    </center>
+    <br/>
+
+
+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 <https://github.com/rhoban/onshape-to-robot/>`_
+* `Robots examples GitHub repository <https://github.com/rhoban/onshape-to-robot-examples/>`_
+* `onshape-to-robot on pypi <https://pypi.org/project/onshape-to-robot/>`_
+* `video tutorial <https://www.youtube.com/watch?v=C8oK4uUmbRw>`_ (some information may be outdated)
+
+.. toctree::
+   :maxdepth: 2
+   :caption: Contents:
+
+   getting_started
+   design
+   config
+   exporter_urdf
+   exporter_sdf
+   exporter_mujoco
+   kinematic_loops
+   processors
+   cache

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
+
+    <center>
+    <video width="70%" controls>
+    <source src="https://github.com/Rhoban/placo-examples/raw/master/kinematics/videos/planar_2dof_trajectory.mp4" type="video/mp4">
+    </video>
+    </center>
+    <br/>
+
+
+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 <custom-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 ``<equality>`` in MuJoCo
+------------------------------------
+
+When using the :ref:`MuJoCo <exporter-mujoco>` format, ``onshape-to-robot`` will add ``<equality>`` 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
+
+    <center>
+    <video width="70%" controls>
+    <source src="https://github.com/Rhoban/onshape-to-robot-examples/raw/refs/heads/master/videos/planar_2dof_mujoco.mp4" type="video/mp4">
+    </video>
+    </center>
+    <br/>
+
+
+Ressources
+----------
+
+Here are some ressources on how to handle kinematic loops in software:
+
+* `Onshape assembly <https://cad.onshape.com/documents/04b05c47de7576f35c0e99b3/w/68041f3f5c827a258b40039c/e/db543f501b01adf8144064e3?renderMode=0&uiState=665d8e3a6de6705d2f788204>`_ for the above example robot.
+* MuJoCo `equality <https://mujoco.readthedocs.io/en/stable/computation/index.html#coequality>`_ constraints.
+* In `pyBullet <https://pybullet.org/wordpress/>`_, you can use `createConstraint` method to add the relevant constraint.
+* In the `PlaCo <https://placo.readthedocs.io/>`_ solver, you can create a `RelativePositionTask`. See the `kinematics loop documentation section <https://placo.readthedocs.io/en/latest/kinematics/loop_closures.html>`_ for  more details. Some examples created with onshape-to-robot can be found in the `example gallery <https://placo.readthedocs.io/en/latest/kinematics/examples_gallery.html>`_.

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``.

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.

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 <https://github.com/SarahWeiii/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.

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. 

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.

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.

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.

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 <https://www.youtube.com/watch?v=C8oK4uUmbRw>`_. 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 <path_to_stl>
+
+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.

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.

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 <https://github.com/Rhoban/onshape-to-robot/blob/master/onshape_to_robot/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 <https://github.com/Rhoban/onshape-to-robot/blob/master/onshape_to_robot/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

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

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

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}")

onshape_to_robot/assets/scene.xml

@@ -0,0 +1,24 @@
+<mujoco model="scene">
+    <include file="{robot_filename}" />
+
+    <visual>
+        <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
+        <rgba haze="0.15 0.25 0.35 1" />
+        <global azimuth="160" elevation="-20" />
+    </visual>
+
+    <asset>
+        <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512"
+            height="3072" />
+        <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4"
+            rgb2="0.1 0.2 0.3"
+            markrgb="0.8 0.8 0.8" width="300" height="300" />
+        <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5"
+            reflectance="0.2" />
+    </asset>
+
+    <worldbody>
+        <light pos="0 0 3.5" dir="0 0 -1" directional="true" />
+        <geom name="floor" size="0 0 0.05" pos="0 0 0" type="plane" material="groundplane" />
+    </worldbody>
+</mujoco>

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()

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

onshape_to_robot/bullet/plane.urdf

@@ -0,0 +1,29 @@
+<?xml version="0.0" ?>
+<robot name="plane">
+  <link name="planeLink">
+  <contact>
+      <lateral_friction value="1"/>
+  </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".0"/>
+       <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+				<mesh filename="plane.obj" scale="1 1 1"/>
+      </geometry>
+       <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -5"/>
+      <geometry>
+	 	<box size="30 30 10"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

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()

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))

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)

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()

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()

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)}"))

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"<!-- Additional {xml_file} -->"
+        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('<?xml version="1.0" ?>')
+        self.append("<!-- Generated using onshape-to-robot -->")
+        if self.config:
+            self.append(f"<!-- Onshape {self.config.printable_version()} -->")
+        self.append(f'<mujoco model="{robot.name}">')
+        self.append(
+            f'<compiler angle="radian" meshdir="{self.config.assets_directory}" autolimits="true" />'
+        )
+
+        # Boilerplate
+        self.default_class = robot.name
+        self.append("<default>")
+        self.append(f'<default class="{self.default_class}">')
+        self.append('<joint frictionloss="0.1" armature="0.005"/>')
+        self.append('<position kp="50" dampratio="1" />')
+        self.append('<default class="visual">')
+        self.append('<geom type="mesh" contype="0" conaffinity="0" group="2"/>')
+        self.append("</default>")
+        self.append('<default class="collision">')
+        self.append('<geom group="3"/>')
+        self.append("</default>")
+        self.append("</default>")
+        self.append("</default>")
+
+        if self.additional_xml:
+            self.append(self.additional_xml)
+
+        # Adding robot links
+        self.append("<worldbody>")
+        for base_link in robot.base_links:
+            self.add_link(robot, base_link)
+        self.append("</worldbody>")
+
+        # Asset (mesh & materials)
+        self.append("<asset>")
+        for mesh_file in set(self.meshes):
+            self.append(f'<mesh file="{mesh_file}" />')
+        for material_name, color in self.materials.items():
+            color_str = "%g %g %g %g" % self.config.round(tuple(color))
+            self.append(f'<material name="{material_name}" rgba="{color_str}" />')
+        self.append("</asset>")
+
+        # Adding actuators
+        self.add_actuators(robot)
+
+        # Adding equalities (loop closure)
+        self.add_equalities(robot)
+
+        self.append("</mujoco>")
+
+        return self.xml
+
+    def add_actuators(self, robot: Robot):
+        self.append("<actuator>")
+
+        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("</actuator>")
+
+    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("<equality>")
+        for closure in robot.closures:
+            attributes = self.get_equality_attributes(closure)
+
+            if closure.closure_type == Closure.FIXED:
+                self.append(
+                    f'<weld site1="{closure.frame1}" site2="{closure.frame2}" {attributes}/>'
+                )
+            elif closure.closure_type == Closure.REVOLUTE:
+                self.append(
+                    f'<connect site1="{closure.frame1}" site2="{closure.frame2}" {attributes}/>'
+                )
+            elif closure.closure_type == Closure.BALL:
+                self.append(
+                    f'<connect site1="{closure.frame1}" site2="{closure.frame2}" {attributes}/>'
+                )
+            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'<joint joint1="{joint.name}" joint2="{joint.relation.source_joint}" polycoef="0 {joint.relation.ratio} 0 0 0" />'
+                )
+
+        self.append("</equality>")
+
+    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 = "<inertial "
+        inertial += 'pos="%g %g %g" ' % self.config.round(tuple(com))
+        inertial += 'mass="%g" ' % self.config.round(mass)
+        inertial += 'fullinertia="%g %g %g %g %g %g" ' % self.config.round(
+            (
+                inertia[0, 0],
+                inertia[1, 1],
+                inertia[2, 2],
+                inertia[0, 1],
+                inertia[0, 2],
+                inertia[1, 2],
+            )
+        )
+        inertial += " />"
+        self.append(inertial)
+
+    def add_mesh(self, part: Part, class_: str, T_world_link: np.ndarray, mesh: Mesh):
+        """
+        Add a mesh node (e.g. STL) to the MuJoCo file
+        """
+        # Retrieving mesh file and material name
+        mesh_file = os.path.relpath(self.config.output_directory + "/" + mesh.filename, self.config.asset_path(""))
+        mesh_file_no_ext = ".".join(os.path.basename(mesh_file).split(".")[:-1])
+        material_name = mesh_file_no_ext + "_material"
+
+        # Relative frame
+        T_link_part = np.linalg.inv(T_world_link) @ part.T_world_part
+
+        # Adding the geom node
+        geom = f'<geom type="mesh" class="{class_}" '
+        geom += self.pos_quat(T_link_part) + " "
+        geom += f'mesh="{xml_escape(mesh_file_no_ext)}" '
+        geom += f'material="{xml_escape(material_name)}" '
+
+        # Apply properties based on class (visual or collision)
+        properties = mesh.visual_properties if class_ == "visual" else mesh.collision_properties
+        for key, value in properties.items():
+            geom += f'{key}="{xml_escape(str(value))}" '
+
+        geom += " />"
+
+        # Adding the mesh and material to appear in the assets section
+        self.meshes.append(mesh_file)
+        self.materials[material_name] = mesh.color
+
+        self.append(geom)
+
+    def add_shape(
+        self, part: Part, class_: str, T_world_link: np.ndarray, shape: Shape
+    ):
+        """
+        Add pure shape geometry.
+        """
+        geom = f'<geom class="{class_}" '
+
+        T_link_shape = (
+            np.linalg.inv(T_world_link) @ part.T_world_part @ shape.T_part_shape
+        )
+        geom += self.pos_quat(T_link_shape) + " "
+
+        if isinstance(shape, Box):
+            geom += 'type="box" size="%g %g %g" ' % self.config.round(tuple(shape.size / 2))
+        elif isinstance(shape, Cylinder):
+            geom += 'type="cylinder" size="%g %g" ' % (
+                shape.radius,
+                shape.length / 2,
+            )
+        elif isinstance(shape, Sphere):
+            geom += 'type="sphere" size="%g" ' % self.config.round(shape.radius)
+
+        if class_ == "visual":
+            material_name = f"{part.name}_material"
+            self.materials[material_name] = shape.color
+            geom += f'material="{xml_escape(material_name)}" '
+
+        # Apply properties based on class (visual or collision)
+        properties = shape.visual_properties if class_ == "visual" else shape.collision_properties
+        for key, value in properties.items():
+            geom += f'{key}="{xml_escape(str(value))}" '
+
+        geom += " />"
+        self.append(geom)
+
+    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):
+        self.append(f"<!-- Joint from {joint.parent.name} to {joint.child.name} -->")
+        if joint.joint_type == "fixed":
+            self.append(f'<!-- Joint is "fixed", skipping it -->')
+            return
+
+        joint_xml: str = "<joint "
+        joint_xml += 'axis="%g %g %g" ' % self.config.round(tuple(joint.axis))
+        joint_xml += f'name="{joint.name}" '
+        if joint.joint_type == Joint.REVOLUTE:
+            joint_xml += 'type="hinge" '
+        elif joint.joint_type == Joint.PRISMATIC:
+            joint_xml += 'type="slide" '
+        elif joint.joint_type == Joint.BALL:
+            joint_xml += 'type="ball" '
+        else:
+            print(warning(f"Unknown joint type: {joint.joint_type}"))
+
+        joint_limits = joint.properties.get("limits", joint.limits)
+        if joint_limits is not None and joint.properties.get("range", True):
+            joint_xml += f'range="{joint_limits[0]} {joint_limits[1]}" '
+
+        for key in (
+            "class",
+            "frictionloss",
+            "armature",
+            "damping",
+            "stiffness",
+        ):
+            if key in joint.properties:
+                joint_xml += f'{key}="{joint.properties[key]}" '
+
+        joint_xml += " />"
+        self.append(joint_xml)
+
+    def add_frame(
+        self,
+        frame: str,
+        T_world_link: np.ndarray,
+        T_world_frame: np.ndarray,
+        group: int = 0,
+    ):
+        self.append(f"<!-- Frame {frame} -->")
+        T_link_frame = np.linalg.inv(T_world_link) @ T_world_frame
+
+        site: str = f'<site group="{group}" name="{frame}" '
+        site += self.pos_quat(T_link_frame) + " "
+        site += " />"
+        self.append(site)
+
+    def add_link(
+        self,
+        robot: Robot,
+        link: Link,
+        parent_joint: Joint | None = None,
+        T_world_parent: np.ndarray = np.eye(4),
+    ):
+        """
+        Adds a link recursively to the URDF file
+        """
+        if parent_joint is None:
+            T_world_link = np.eye(4)
+        else:
+            T_world_link = parent_joint.T_world_joint
+
+        childclass = ""
+        if parent_joint is None:
+            childclass = f'childclass="{self.default_class}" '
+        self.append(f"<!-- Link {link.name} -->")
+        T_parent_link = np.linalg.inv(T_world_parent) @ T_world_link
+        self.append(
+            f'<body name="{link.name}" {self.pos_quat(T_parent_link)} {childclass}>'
+        )
+
+        if parent_joint is None:
+            if not link.fixed:
+                self.append(f'<freejoint name="{link.name}_freejoint" />')
+        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"<!-- Part {part.name} -->")
+            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("</body>")
+
+    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"))

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 = """
+<?xml version="1.0" ?>
+<model>
+    <name>%s</name>
+    <version>1.0</version>
+    <sdf version="1.7">%s</sdf>
+    <author>
+        <name></name>
+        <email></email>
+    </author>
+    <description></description>
+</model>
+"""
+
+
+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"<!-- Additional {xml_file} -->"
+        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('<?xml version="1.0" ?>')
+        self.append("<!-- Generated using onshape-to-robot -->")
+        if self.config:
+            self.append(f"<!-- Onshape {self.config.printable_version()} -->")
+        self.append('<sdf version="1.7">')
+        self.append(f'<model name="{xml_escape(robot.name)}">')
+
+        for base_link in robot.base_links:
+            self.add_link(robot, base_link)
+
+        if self.additional_xml:
+            self.append(self.additional_xml)
+
+        self.append("</model>")
+        self.append("</sdf>")
+
+        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("<inertial>")
+        self.append(
+            '<pose>%g %g %g 0 0 0</pose>'
+            % self.config.round((
+                com[0],
+                com[1],
+                com[2],
+            ))
+        )
+        self.append("<mass>%g</mass>" % self.config.round(mass))
+        self.append(
+            "<inertia><ixx>%g</ixx><ixy>%g</ixy><ixz>%g</ixz><iyy>%g</iyy><iyz>%g</iyz><izz>%g</izz></inertia>"
+            % self.config.round((
+                inertia[0, 0],
+                inertia[0, 1],
+                inertia[0, 2],
+                inertia[1, 1],
+                inertia[1, 2],
+                inertia[2, 2],
+            ))
+        )
+        self.append("</inertial>")
+
+    def append_material(self, color: np.ndarray):
+        self.append(f"<material>")
+        self.append("<ambient>%g %g %g %g</ambient>" % self.config.round((color[0], color[1], color[2], color[3])))
+        self.append("<diffuse>%g %g %g %g</diffuse>" % self.config.round((color[0], color[1], color[2], color[3])))
+        self.append("<specular>0.1 0.1 0.1 1</specular>")
+        self.append("<emissive>0 0 0 0</emissive>")
+        self.append("</material>")
+
+    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("<geometry>")
+        self.append(
+            f"<mesh><uri>model://{self.config.robot_name}/{xml_escape(mesh_file)}</uri></mesh>"
+        )
+        self.append("</geometry>")
+
+        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))}</{key}>')
+
+        self.append(f"</{node}>")
+
+    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("<geometry>")
+        if isinstance(shape, Box):
+            self.append("<box><size>%g %g %g</size></box>" % self.config.round(tuple(shape.size)))
+        elif isinstance(shape, Cylinder):
+            self.append(
+                "<cylinder><length>%g</length><radius>%g</radius></cylinder>"
+                % self.config.round((shape.length, shape.radius))
+            )
+        elif isinstance(shape, Sphere):
+            self.append("<sphere><radius>%g</radius></sphere>" % self.config.round((shape.radius,)))
+        self.append("</geometry>")
+
+        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))}</{key}>')
+
+        self.append(f"</{node}>")
+
+    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 from {joint.parent.name} to {joint.child.name} -->")
+
+        joint_type = joint.properties.get("type", joint.joint_type)
+        self.append(f'<joint name="{joint.name}" type="{joint_type}">')
+
+        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"<parent>{joint.parent.name}</parent>")
+        self.append(f"<child>{joint.child.name}</child>")
+        self.append("<axis>")
+        self.append("<xyz>%g %g %g</xyz>" % self.config.round(tuple(joint.axis)))
+
+        self.append("<limit>")
+        if "max_effort" in joint.properties:
+            self.append("<effort>%g</effort>" % self.config.round(joint.properties["max_effort"]))
+        else:
+            self.append("<effort>10</effort>")
+
+        if "max_velocity" in joint.properties:
+            self.append("<velocity>%g</velocity>" % self.config.round(joint.properties["max_velocity"]))
+        else:
+            self.append("<velocity>10</velocity>")
+
+        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"<lower>{joint_limits[0]}</lower>")
+        self.append(f"<upper>{joint_limits[1]}</upper>")
+        self.append("</limit>")
+
+        if joint.relation is not None:
+            self.append(
+                f'<mimic joint="{joint.relation.source_joint}"><multiplier>{joint.relation.ratio}</multiplier></mimic>'
+            )
+
+        self.append("<dynamics>")
+        self.append(f"<friction>{joint.properties.get('friction', 0.)}</friction>")
+        self.append(f"<damping>{joint.properties.get('damping', 0.)}</damping>")
+        self.append("</dynamics>")
+
+        self.append("</axis>")
+
+        self.append("</joint>")
+
+    def add_frame(
+        self,
+        link: Link,
+        frame: str,
+        T_world_link: np.ndarray,
+        T_world_frame: np.ndarray,
+    ):
+        self.append(f"<!-- Frame {frame} -->")
+        T_link_frame = np.linalg.inv(T_world_link) @ T_world_frame
+
+        self.append(f'<frame name="{frame}">')
+        self.append(self.pose(T_link_frame, relative_to=link.name))
+        self.append("</frame>")
+
+    def add_link(self, robot: Robot, link: Link, joint: Joint = None):
+        """
+        Adds a link recursively to the SDF file
+        """
+        self.append(f"<!-- Link {link.name} -->")
+        self.append(f'<link name="{link.name}">')
+
+        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"<!-- Part {part.name} -->")
+            self.add_geometries(link, part, T_world_link)
+
+        self.append("</link>")
+
+        if link.fixed:
+            self.append(f'<joint name="{link.name}_fixed_world" type="fixed">')
+            self.append(f"<parent>world</parent>")
+            self.append(f"<child>{link.name}</child>")
+            self.append("</joint>")
+
+        # 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 = "<pose%s>%g %g %g %g %g %g</pose>"
+
+        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"))

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"<!-- Additional {xml_file} -->"
+        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('<?xml version="1.0" ?>')
+        self.append("<!-- Generated using onshape-to-robot -->")
+        if self.config:
+            self.append(f"<!-- Onshape {self.config.printable_version()} -->")
+        self.append(f'<robot name="{robot.name}">')
+
+        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("</robot>")
+
+        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("<inertial>")
+        self.append(
+            '<origin xyz="%g %g %g" rpy="0 0 0"/>'
+            % self.config.round(
+                (
+                    com[0],
+                    com[1],
+                    com[2],
+                )
+            )
+        )
+        self.append('<mass value="%g" />' % self.config.round(mass))
+        self.append(
+            '<inertia ixx="%g" ixy="%g"  ixz="%g" iyy="%g" iyz="%g" izz="%g" />'
+            % self.config.round(
+                (
+                    inertia[0, 0],
+                    inertia[0, 1],
+                    inertia[0, 2],
+                    inertia[1, 1],
+                    inertia[1, 2],
+                    inertia[2, 2],
+                )
+            )
+        )
+        self.append("</inertial>")
+
+    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("<geometry>")
+        self.append(f'<mesh filename="package://{xml_escape(mesh_file)}" />')
+        self.append("</geometry>")
+
+        if node == "visual":
+            material_name = f"{part.name}_material"
+            self.append(f'<material name="{xml_escape(material_name)}">')
+            self.append(
+                '<color rgba="%g %g %g %g"/>'
+                % self.config.round(
+                    (mesh.color[0], mesh.color[1], mesh.color[2], mesh.color[3])
+                )
+            )
+            self.append("</material>")
+
+        # 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))}</{key}>')
+
+        self.append(f"</{node}>")
+
+    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("<geometry>")
+        if isinstance(shape, Box):
+            self.append(
+                '<box size="%g %g %g" />' % self.config.round(tuple(shape.size))
+            )
+        elif isinstance(shape, Cylinder):
+            self.append(
+                '<cylinder length="%g" radius="%g" />'
+                % self.config.round((shape.length, shape.radius))
+            )
+        elif isinstance(shape, Sphere):
+            self.append('<sphere radius="%g" />' % self.config.round((shape.radius,)))
+        self.append("</geometry>")
+
+        if node == "visual":
+            material_name = f"{part.name}_material"
+            self.append(f'<material name="{xml_escape(material_name)}">')
+            self.append(
+                '<color rgba="%g %g %g %g"/>'
+                % self.config.round(
+                    (shape.color[0], shape.color[1], shape.color[2], shape.color[3])
+                )
+            )
+            self.append("</material>")
+
+        # 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))}</{key}>')
+
+        self.append(f"</{node}>")
+
+    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 from {joint.parent.name} to {joint.child.name} -->")
+
+        joint_type = joint.properties.get("type", joint.joint_type)
+        self.append(f'<joint name="{joint.name}" type="{joint_type}">')
+
+        T_link_joint = np.linalg.inv(T_world_link) @ joint.T_world_joint
+        self.append(self.origin(T_link_joint))
+
+        self.append(f'<parent link="{joint.parent.name}" />')
+        self.append(f'<child link="{joint.child.name}" />')
+        self.append('<axis xyz="%g %g %g"/>' % 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"<limit {limits}/>")
+
+        if "friction" in joint.properties:
+            self.append(
+                f'<joint_properties friction="{joint.properties["friction"]}"/>'
+            )
+
+        if joint.relation is not None:
+            self.append(
+                f'<mimic joint="{joint.relation.source_joint}" multiplier="{joint.relation.ratio}" />'
+            )
+
+        self.append("</joint>")
+
+    def add_frame(
+        self,
+        link: Link,
+        frame: str,
+        T_world_link: np.ndarray,
+        T_world_frame: np.ndarray,
+    ):
+        self.append(f"<!-- Frame {frame} (dummy link + fixed joint) -->")
+        T_link_frame = np.linalg.inv(T_world_link) @ T_world_frame
+
+        # Adding a dummy link to the assembly
+        self.append(f'<link name="{frame}">')
+        self.append(self.origin(np.eye(4)))
+
+        self.append("<inertial>")
+        self.append('<origin xyz="0 0 0" rpy="0 0 0" />')
+        if self.no_dynamics:
+            self.append('<mass value="0" />')
+        else:
+            self.append('<mass value="1e-9" />')
+        self.append('<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />')
+        self.append("</inertial>")
+
+        self.append("</link>")
+
+        # Attaching this dummy link to the parent frame using a fixed joint
+        self.append(f'<joint name="{frame}_frame" type="fixed">')
+        self.append(self.origin(T_link_frame))
+        self.append(f'<parent link="{link.name}" />')
+        self.append(f'<child link="{frame}" />')
+        self.append('<axis xyz="0 0 0"/>')
+        self.append("</joint>")
+
+    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"<!-- Link {link.name} -->")
+        self.append(f'<link name="{link.name}">')
+
+        # 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"<!-- Part {part.name} -->")
+            self.add_geometries(part, T_world_link)
+
+        self.append("</link>")
+
+        # 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 = '<origin xyz="%g %g %g" rpy="%g %g %g" />'
+
+        return urdf % self.config.round(
+            (*matrix[:3, 3], *rotation_matrix_to_rpy(matrix))
+        )

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])

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)"))

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

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

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()

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']

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

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,
+            },
+        )

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

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)

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

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)

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()

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.")
+            )

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]

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,
+                    )
+                )

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,
+            )
+        )

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()

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)

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)