├── docs ├── .gitignore ├── requirements.txt ├── _static │ └── img │ │ ├── frame.png │ │ ├── zaxis.png │ │ ├── design.png │ │ ├── design.xcf │ │ ├── opened_chain.png │ │ └── smalls │ │ ├── design.png │ │ ├── frame.png │ │ └── main.png ├── Makefile ├── make.bat ├── installation.rst ├── index.rst ├── conf.py ├── kinematic_loops.rst ├── design.rst └── config.rst ├── onshape_urdf_exporter ├── __init__.py ├── onshape_api │ ├── __init__.py │ ├── utils.py │ ├── onshape.py │ └── client.py ├── stl_utils.py ├── config_file.py ├── features.py ├── __main__.py ├── robot_description.py └── load_robot.py ├── .gitignore ├── .readthedocs.yaml ├── README.md ├── .github └── workflows │ ├── lint.yaml │ └── release.yaml ├── pyproject.toml └── LICENSE /docs/.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | -------------------------------------------------------------------------------- /onshape_urdf_exporter/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /docs/requirements.txt: -------------------------------------------------------------------------------- 1 | sphinx-rtd-theme 2 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | output/ 2 | 3 | # IDE stuff 4 | .idea 5 | 6 | # Python stuff 7 | *.pyc 8 | -------------------------------------------------------------------------------- /docs/_static/img/frame.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UrbanMachine/onshape-urdf-exporter/HEAD/docs/_static/img/frame.png -------------------------------------------------------------------------------- /docs/_static/img/zaxis.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UrbanMachine/onshape-urdf-exporter/HEAD/docs/_static/img/zaxis.png -------------------------------------------------------------------------------- /docs/_static/img/design.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UrbanMachine/onshape-urdf-exporter/HEAD/docs/_static/img/design.png -------------------------------------------------------------------------------- /docs/_static/img/design.xcf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UrbanMachine/onshape-urdf-exporter/HEAD/docs/_static/img/design.xcf -------------------------------------------------------------------------------- /docs/_static/img/opened_chain.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UrbanMachine/onshape-urdf-exporter/HEAD/docs/_static/img/opened_chain.png -------------------------------------------------------------------------------- /docs/_static/img/smalls/design.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UrbanMachine/onshape-urdf-exporter/HEAD/docs/_static/img/smalls/design.png -------------------------------------------------------------------------------- /docs/_static/img/smalls/frame.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UrbanMachine/onshape-urdf-exporter/HEAD/docs/_static/img/smalls/frame.png -------------------------------------------------------------------------------- /docs/_static/img/smalls/main.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UrbanMachine/onshape-urdf-exporter/HEAD/docs/_static/img/smalls/main.png -------------------------------------------------------------------------------- /onshape_urdf_exporter/onshape_api/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | onshape_api, based on onshape "apikey" 3 | ====== 4 | 5 | Demonstrates usage of API keys for the Onshape REST API 6 | """ 7 | 8 | __copyright__ = "Copyright (c) 2016 Onshape, Inc." 9 | __license__ = "All rights reserved." 10 | __title__ = "onshape_api" 11 | __all__ = ["onshape", "client", "utils"] 12 | -------------------------------------------------------------------------------- /.readthedocs.yaml: -------------------------------------------------------------------------------- 1 | # Read the Docs configuration file 2 | # See https://docs.readthedocs.io/en/stable/config-file/v2.html for details 3 | 4 | version: 2 5 | 6 | build: 7 | os: ubuntu-22.04 8 | tools: 9 | python: "3.12" 10 | 11 | sphinx: 12 | configuration: docs/conf.py 13 | 14 | python: 15 | install: 16 | - requirements: docs/requirements.txt 17 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # OnShape URDF Exporter 2 | 3 | [Read the docs here](https://onshape-urdf-exporter.readthedocs.io/en/latest/) 4 | 5 | Exports OnShape assemblies into URDF files with STL meshes. This is based off 6 | of the URDF exporting functionality provided by onshape-to-robot, but with 7 | some improvements: 8 | 9 | - XML document creation is done using Python's XML library, fixing a number of 10 | bugs related to characters not being properly escaped 11 | - Files created by this tool always have valid filenames, even on Windows 12 | - Uses Open3D for STL simplification instead of MeshLab 13 | -------------------------------------------------------------------------------- /.github/workflows/lint.yaml: -------------------------------------------------------------------------------- 1 | name: Lint 2 | on: [push] 3 | 4 | jobs: 5 | lint: 6 | runs-on: ubuntu-latest 7 | steps: 8 | - name: Setup Python 9 | uses: actions/setup-python@v4 10 | with: 11 | python-version: '3.10' 12 | - name: Install Poetry 13 | uses: snok/install-poetry@v1 14 | - name: Checkout 15 | uses: actions/checkout@v4 16 | - name: Install Dependencies 17 | run: poetry install --no-interaction 18 | - name: Black 19 | run: poetry run black --check onshape_urdf_exporter 20 | - name: Isort 21 | run: poetry run isort --check onshape_urdf_exporter 22 | -------------------------------------------------------------------------------- /.github/workflows/release.yaml: -------------------------------------------------------------------------------- 1 | name: Release 2 | 3 | on: 4 | release: 5 | types: 6 | - created 7 | 8 | jobs: 9 | pypi: 10 | runs-on: ubuntu-22.04 11 | steps: 12 | - uses: actions/checkout@v4 13 | - name: Set up python 14 | id: setup-python 15 | uses: actions/setup-python@v5 16 | with: 17 | python-version: '3.10' 18 | - name: Install Poetry 19 | uses: snok/install-poetry@v1 20 | - name: Install Project 21 | run: poetry install 22 | - name: Build and Upload 23 | run: | 24 | poetry config pypi-token.pypi ${{ secrets.PYPI_API_TOKEN }} 25 | poetry publish --build 26 | -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line, and also 5 | # from the environment for the first two. 6 | SPHINXOPTS ?= 7 | SPHINXBUILD ?= sphinx-build 8 | SOURCEDIR = . 9 | BUILDDIR = build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 21 | -------------------------------------------------------------------------------- /docs/make.bat: -------------------------------------------------------------------------------- 1 | @ECHO OFF 2 | 3 | pushd %~dp0 4 | 5 | REM Command file for Sphinx documentation 6 | 7 | if "%SPHINXBUILD%" == "" ( 8 | set SPHINXBUILD=sphinx-build 9 | ) 10 | set SOURCEDIR=source 11 | set BUILDDIR=build 12 | 13 | if "%1" == "" goto help 14 | 15 | %SPHINXBUILD% >NUL 2>NUL 16 | if errorlevel 9009 ( 17 | echo. 18 | echo.The 'sphinx-build' command was not found. Make sure you have Sphinx 19 | echo.installed, then set the SPHINXBUILD environment variable to point 20 | echo.to the full path of the 'sphinx-build' executable. Alternatively you 21 | echo.may add the Sphinx directory to PATH. 22 | echo. 23 | echo.If you don't have Sphinx installed, grab it from 24 | echo.http://sphinx-doc.org/ 25 | exit /b 1 26 | ) 27 | 28 | %SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 29 | goto end 30 | 31 | :help 32 | %SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 33 | 34 | :end 35 | popd 36 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [tool.poetry] 2 | name = "onshape-urdf-exporter" 3 | version = "1.0.0" 4 | description = "Exports an OnShape assembly to a URDF file with STL meshes" 5 | authors = ["Urban Machine "] 6 | license = "MIT" 7 | readme = "README.md" 8 | packages = [{include = "onshape_urdf_exporter"}] 9 | 10 | [tool.poetry.scripts] 11 | onshape-urdf-exporter = "onshape_urdf_exporter.__main__:main" 12 | 13 | [tool.poetry.dependencies] 14 | python = "^3.10" 15 | requests = "^2.31.0" 16 | numpy = "^1.26.2" 17 | colorama = "^0.4.6" 18 | # Poetry can't figure out dependencies for Open3D 0.17.x 19 | open3d = "0.16.0" 20 | pydantic = "^2.5.2" 21 | pydantic-yaml = "^1.2.0" 22 | 23 | [tool.poetry.group.dev.dependencies] 24 | black = "^23.11.0" 25 | isort = "^5.12.0" 26 | mypy = "^1.7.1" 27 | 28 | [build-system] 29 | requires = ["poetry-core"] 30 | build-backend = "poetry.core.masonry.api" 31 | 32 | [tool.isort] 33 | profile = "black" 34 | 35 | [tool.mypy] 36 | strict = true 37 | ignore_missing_imports = true 38 | -------------------------------------------------------------------------------- /docs/installation.rst: -------------------------------------------------------------------------------- 1 | 2 | Installation & requirements 3 | =========================== 4 | 5 | Requirements 6 | ------------- 7 | 8 | You will need an Onshape account and Python 3. 9 | 10 | Installation 11 | ------------ 12 | 13 | Pipx_ is the recommended way to install Onshape URDF Exporter. Just run the 14 | following command: 15 | 16 | .. code-block:: bash 17 | 18 | pipx install onshape-urdf-exporter 19 | 20 | .. _Pipx: https://pipx.pypa.io/stable/installation/ 21 | 22 | .. _api-key: 23 | 24 | Setting up your API key 25 | ----------------------- 26 | 27 | To authenticate the tool with Onshape, you will need to obtain an API key and 28 | secret from the `Onshape developer portal`_. 29 | 30 | These keys are provided to the tool using environment variables. Declare them 31 | in your ``.bashrc`` (or shell equivalent) by adding these lines: 32 | 33 | .. code-block:: bash 34 | 35 | # Obtained at https://dev-portal.onshape.com/keys 36 | export ONSHAPE_API=https://cad.onshape.com 37 | export ONSHAPE_ACCESS_KEY=Your_Access_Key 38 | export ONSHAPE_SECRET_KEY=Your_Secret_Key 39 | 40 | .. _Onshape developer portal: https://dev-portal.onshape.com/keys 41 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2019-2099 Rhoban Team 2 | Copyright 2023 Urban Machine 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy of 5 | this software and associated documentation files (the "Software"), to deal in 6 | the Software without restriction, including without limitation the rights to 7 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 8 | of the Software, and to permit persons to whom the Software is furnished to do 9 | so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in all 12 | copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | SOFTWARE. 21 | -------------------------------------------------------------------------------- /docs/index.rst: -------------------------------------------------------------------------------- 1 | Onshape URDF Exporter Documentation 2 | =================================== 3 | 4 | Onshape URDF Exporter is a tool that that exports Onshape robot designs as 5 | URDF files. These files can be used with tools like RViz or Gazebo to 6 | calculate inverse kinematics, run physics simulations, and provide 7 | visualizations. 8 | 9 | This project is based on onshape-to-robot_, but with some 10 | different design decisions and a number of bug fixes: 11 | 12 | - This tool focuses on doing one thing well: Exporting URDFs 13 | - XML document creation is done using Python's XML library, fixing a number of 14 | bugs related to characters not being properly escaped 15 | - Files created by this tool always have valid filenames, even on Windows 16 | - Uses Open3D for STL simplification instead of MeshLab, which makes 17 | installation easier 18 | - Uses YAML files for configuration, instead of the non-standard commentjson 19 | format 20 | 21 | .. _onshape-to-robot: https://github.com/Rhoban/onshape-to-robot 22 | 23 | * `Onshape URDF Exporter GitHub repository `_ 24 | * `Onshape URDF Exporter on PyPI `_ 25 | 26 | .. toctree:: 27 | :maxdepth: 2 28 | :caption: Contents: 29 | 30 | installation 31 | design 32 | kinematic_loops 33 | config 34 | -------------------------------------------------------------------------------- /onshape_urdf_exporter/stl_utils.py: -------------------------------------------------------------------------------- 1 | from pathlib import Path 2 | 3 | import open3d as o3d 4 | from colorama import Fore, Style 5 | 6 | 7 | def simplify_stl(path: Path) -> None: 8 | """Optimize a single STL file in-place, if it isn't already optimized.""" 9 | 10 | print(f"Optimizing {str(path)}") 11 | simple_mesh: o3d.geometry.TriangleMesh = o3d.io.read_triangle_mesh(str(path)) 12 | simple_mesh = simple_mesh.compute_triangle_normals() 13 | simple_mesh = simple_mesh.simplify_vertex_clustering( 14 | voxel_size=0.0025, contraction=o3d.geometry.SimplificationContraction.Quadric 15 | ) 16 | simple_mesh = simple_mesh.merge_close_vertices(eps=0.001) 17 | simple_mesh = simple_mesh.remove_duplicated_vertices() 18 | simple_mesh = simple_mesh.remove_duplicated_triangles() 19 | simple_mesh = simple_mesh.remove_degenerate_triangles() 20 | simple_mesh = simple_mesh.remove_non_manifold_edges() 21 | simple_mesh = simple_mesh.compute_triangle_normals() 22 | simple_mesh = simple_mesh.compute_vertex_normals() 23 | 24 | success = o3d.io.write_triangle_mesh( 25 | str(path), 26 | simple_mesh, 27 | write_vertex_normals=False, 28 | write_triangle_uvs=False, 29 | compressed=False, 30 | write_vertex_colors=False, 31 | ) 32 | if not success: 33 | print( 34 | f"{Fore.YELLOW}" 35 | f"WARNING: Failed to simplify STL {path}. See above logs for " 36 | f"clues as to why." 37 | f"{Style.RESET_ALL}" 38 | ) 39 | -------------------------------------------------------------------------------- /onshape_urdf_exporter/onshape_api/utils.py: -------------------------------------------------------------------------------- 1 | """ 2 | utils 3 | ===== 4 | 5 | Handy functions for API key sample app 6 | """ 7 | 8 | import logging 9 | from logging.config import dictConfig 10 | 11 | __all__ = ["log"] 12 | 13 | 14 | def log(msg, level=0): 15 | """ 16 | Logs a message to the console, with optional level paramater 17 | 18 | Args: 19 | - msg (str): message to send to console 20 | - level (int): log level; 0 for info, 1 for error (default = 0) 21 | """ 22 | 23 | red = "\033[91m" 24 | endc = "\033[0m" 25 | 26 | # configure the logging module 27 | cfg = { 28 | "version": 1, 29 | "disable_existing_loggers": False, 30 | "formatters": { 31 | "stdout": { 32 | "format": "[%(levelname)s]: %(asctime)s - %(message)s", 33 | "datefmt": "%x %X", 34 | }, 35 | "stderr": { 36 | "format": red + "[%(levelname)s]: %(asctime)s - %(message)s" + endc, 37 | "datefmt": "%x %X", 38 | }, 39 | }, 40 | "handlers": { 41 | "stdout": { 42 | "class": "logging.StreamHandler", 43 | "level": "DEBUG", 44 | "formatter": "stdout", 45 | }, 46 | "stderr": { 47 | "class": "logging.StreamHandler", 48 | "level": "ERROR", 49 | "formatter": "stderr", 50 | }, 51 | }, 52 | "loggers": { 53 | "info": {"handlers": ["stdout"], "level": "INFO", "propagate": True}, 54 | "error": {"handlers": ["stderr"], "level": "ERROR", "propagate": False}, 55 | }, 56 | } 57 | 58 | dictConfig(cfg) 59 | 60 | lg = "info" if level == 0 else "error" 61 | lvl = 20 if level == 0 else 40 62 | 63 | logger = logging.getLogger(lg) 64 | logger.log(lvl, msg) 65 | -------------------------------------------------------------------------------- /onshape_urdf_exporter/config_file.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from pathlib import Path 3 | from typing import Any, Optional 4 | 5 | from colorama import Fore, Style 6 | from pydantic import BaseModel, ValidationError 7 | from pydantic_yaml import parse_yaml_file_as 8 | 9 | 10 | class Configuration(BaseModel): 11 | document_id: str 12 | version_id: str = "" 13 | workspace_id: str = "" 14 | draw_frames: bool = False 15 | draw_collisions: bool = False 16 | assembly_name: str = "" 17 | use_fixed_links: bool = False 18 | configuration: str = "default" 19 | ignore_limits: bool = False 20 | 21 | # Dynamics 22 | joint_max_effort: float = 1.0 23 | joint_max_velocity: float = 20.0 24 | no_dynamics: bool = False 25 | 26 | # Ignore list 27 | ignore: list[str] = [] 28 | whitelist: list[str] | None = None 29 | 30 | # Color override 31 | color: list[float] | None = None 32 | 33 | # STL configuration 34 | simplify_stls: bool = False 35 | 36 | # Post-import commands to execute 37 | post_import_commands: list[str] = [] 38 | 39 | dynamics_override: dict[str, Any] = {} 40 | 41 | # Add collisions=true configuration on parts 42 | use_collisions_configurations: bool = True 43 | 44 | # ROS support 45 | package_name: str = "" 46 | add_dummy_base_link: bool = False 47 | robot_name: str = "onshape" 48 | 49 | additional_urdf_file: str = "" 50 | additional_xml: str = "" 51 | 52 | dynamics: dict[str, Any] = {} 53 | 54 | class Config: 55 | arbitrary_types_allowed = True 56 | 57 | singleton: Optional["Configuration"] = None 58 | 59 | @classmethod 60 | def from_file(cls, robot_directory: Path) -> "Configuration": 61 | try: 62 | cls.singleton = parse_yaml_file_as( 63 | Configuration, robot_directory / "config.yaml" 64 | ) 65 | except ValidationError as ex: 66 | print(f"{Fore.RED}{ex}{Style.RESET_ALL}") 67 | sys.exit(1) 68 | return cls.singleton 69 | -------------------------------------------------------------------------------- /docs/conf.py: -------------------------------------------------------------------------------- 1 | # Configuration file for the Sphinx documentation builder. 2 | # 3 | # This file only contains a selection of the most common options. For a full 4 | # list see the documentation: 5 | # https://www.sphinx-doc.org/en/master/usage/configuration.html 6 | 7 | # -- Path setup -------------------------------------------------------------- 8 | 9 | # If extensions (or modules to document with autodoc) are in another directory, 10 | # add these directories to sys.path here. If the directory is relative to the 11 | # documentation root, use os.path.abspath to make it absolute, like shown here. 12 | # 13 | # import os 14 | # import sys 15 | # sys.path.insert(0, os.path.abspath('.')) 16 | 17 | 18 | # -- Project information ----------------------------------------------------- 19 | 20 | project = 'Onshape URDF Exporter' 21 | copyright = '2020, Rhoban & 2023, Urban Machine' 22 | author = 'Rhoban' 23 | 24 | # The full version, including alpha/beta/rc tags 25 | release = '1' 26 | 27 | 28 | # -- General configuration --------------------------------------------------- 29 | 30 | # Add any Sphinx extension module names here, as strings. They can be 31 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 32 | # ones. 33 | extensions = [ 34 | ] 35 | 36 | # Add any paths that contain templates here, relative to this directory. 37 | templates_path = ['_templates'] 38 | 39 | # List of patterns, relative to source directory, that match files and 40 | # directories to ignore when looking for source files. 41 | # This pattern also affects html_static_path and html_extra_path. 42 | exclude_patterns = [] 43 | 44 | 45 | # -- Options for HTML output ------------------------------------------------- 46 | 47 | # The theme to use for HTML and HTML Help pages. See the documentation for 48 | # a list of builtin themes. 49 | # 50 | html_theme = "sphinx_rtd_theme" 51 | 52 | # Add any paths that contain custom static files (such as style sheets) here, 53 | # relative to this directory. They are copied after the builtin static files, 54 | # so a file named "default.css" will overwrite the builtin "default.css". 55 | html_static_path = ['_static'] 56 | 57 | master_doc = 'index' 58 | -------------------------------------------------------------------------------- /docs/kinematic_loops.rst: -------------------------------------------------------------------------------- 1 | Handling Kinematic Loops 2 | ======================== 3 | 4 | Some robots have *kinematic loops*, meaning that the kinematic chain is not a tree but a graph. 5 | Here is an example: 6 | 7 | Introduction 8 | ------------ 9 | 10 | Here is a 2D planar robot with kinematic loop, we assume the two first joints to be actuated and the others to 11 | be passive: 12 | 13 | .. raw:: html 14 | 15 |
16 | 19 |
20 |
21 | 22 | 23 | Here, the two branches are connected together, thus this can't be represented as a tree. 24 | URDF doesn't allow to directly represent this type of structure. We need to break it down in a tree, and enforce the 25 | closing constraints in software during execution. 26 | To enforce the constraints, frames can be placed at relevant positions, here is an example for the above robot: 27 | 28 | .. image:: _static/img/opened_chain.png 29 | :width: 200px 30 | :align: center 31 | 32 | Using Onshape URDF Exporter 33 | --------------------------- 34 | 35 | The above mentioned example can be achieved by adding :ref:`frames `. However, this would require to 36 | manually place two frames at each closing position. Onshape URDF Exporter comes with a more convenient way to achieve this, 37 | by using mate connectors. 38 | 39 | If you add a relation with ``closing_something`` as name, two frames will be added to your URDF 40 | (``closing_something_1`` and ``closing_something_2``) that will be attached to the two parts mated. 41 | 42 | This way, the closure is handled in Onshape exactly the way it should appear in the final mechanism, and will result 43 | in the two frames being placed at the correct position in the URDF. 44 | 45 | Here is the complete `Onshape assembly `_ for the above example robot. 46 | 47 | Handling constraints on execution time 48 | -------------------------------------- 49 | 50 | Here are some resources on how to handle kinematic loops in software: 51 | 52 | * In MuJoCo, you can add an `equality `_ constraint in your XML model. 53 | * In `pyBullet `_, you can use `createConstraint` method to add the relevant constraint. 54 | * In the `PlaCo `_ solver, you can create a 55 | ``RelativePositionTask``. See the 56 | `kinematics loop documentation section `_ 57 | for more details. 58 | -------------------------------------------------------------------------------- /docs/design.rst: -------------------------------------------------------------------------------- 1 | Design-time considerations 2 | ========================== 3 | 4 | .. note:: 5 | Try to make your robot assembly mostly based on sub pre-assembled components (avoid to have a lot of 6 | constraints that are not relevant for the export). In this main assembly, do not use features 7 | such as sub-assemblies network. 8 | 9 | Specifying degrees of freedom 10 | ----------------------------- 11 | 12 | * Degree of freedoms should be slider, cylindrical or revolute mate connectors named ``dof_something``, where 13 | ``something`` will be used to name the joint in the final document 14 | 15 | * If the mate connector is **cylindrical** or **revolute**, a ``revolute`` joint will be issued by default 16 | 17 | * To make a ``continuous`` joint, add ``continuous`` or ``wheel`` in the name of the joint. For instance, a **revolute** mate named 18 | ``dof_front_left_wheel`` will result in a ``continuous`` joint named ``front_left_wheel`` in the resulting URDF. 19 | * If the mate connector is a **slider**, a ``prismatic`` joint will be issued 20 | * If the mate connector is **fastened**, a ``fixed`` joint will be issued 21 | * When doing this connection, click the children joint first. This will be used to find the trunk of the robot (part with children but no parent) 22 | 23 | .. image:: _static/img/smalls/design.png 24 | :align: center 25 | 26 | Inverting axis orientation 27 | -------------------------- 28 | 29 | It is possible to invert the axis for convenience by adding ``_inv`` at the end of the name. For instance 30 | ``dof_head_pitch_inv`` will result in a joint named ``head_pitch`` having the axis inverted with the one 31 | from the OnShape assembly. 32 | 33 | Naming links 34 | ------------ 35 | 36 | If you create a mate connector and name it ``link_something``, the link corresponding to the part 37 | on which it is attached will be named ``something`` in the resulting URDF. 38 | 39 | .. _custom-frames: 40 | 41 | Adding custom frames in your model 42 | ---------------------------------- 43 | 44 | If you want to track some frames on your robot, you can do the following: 45 | 46 | * Connect any part to your robot using mate relations in OnShape 47 | * Name one of these relations ``frame_something``, when ``something`` will be the name of 48 | the frame (a link) in the resulting ``sdf`` or ``urdf`` 49 | 50 | .. image:: _static/img/smalls/frame.png 51 | :align: center 52 | 53 | 54 | Here is a `link `_ of a document that can be used as a frame (note: the center cube is 5mm side, so 55 | you might need 2.5mm offset to center it). 56 | 57 | Joint frames 58 | ------------ 59 | 60 | Joint frames are the ones you see in OnShape when you click on the joint in the tree on the left. 61 | Thus, they are always revolving around the z axis, or translating along the z axis, even if the 62 | ``_inv`` suffix is added. 63 | 64 | .. image:: _static/img/zaxis.png 65 | :align: center 66 | -------------------------------------------------------------------------------- /onshape_urdf_exporter/features.py: -------------------------------------------------------------------------------- 1 | import math 2 | import sys 3 | from typing import Any 4 | 5 | from colorama import Fore, Style 6 | 7 | from .config_file import Configuration 8 | from .onshape_api.client import Client 9 | 10 | joint_features: dict[str, Any] = {} 11 | configuration_parameters: dict[str, str] = {} 12 | 13 | 14 | def init( 15 | client: Client, 16 | config: Configuration, 17 | root: dict[str, str], 18 | workspace_id: str, 19 | assembly_id: str, 20 | ) -> None: 21 | global configuration_parameters, joint_features 22 | 23 | # Load joint features to get limits later 24 | if config.version_id == "": 25 | joint_features = client.get_features( 26 | config.document_id, workspace_id, assembly_id 27 | ) 28 | else: 29 | joint_features = client.get_features( 30 | config.document_id, config.version_id, assembly_id, type="v" 31 | ) 32 | 33 | # Retrieving root configuration parameters 34 | configuration_parameters = {} 35 | parts = root["fullConfiguration"].split(";") 36 | for part in parts: 37 | kv = part.split("=") 38 | if len(kv) == 2: 39 | configuration_parameters[kv[0]] = kv[1].replace("+", " ") 40 | 41 | 42 | def read_expression(expression: str) -> float: 43 | # Expression can itself be a variable from configuration 44 | # XXX: This doesn't handle all expression, only values and variables 45 | if expression[0] == "#": 46 | expression = configuration_parameters[expression[1:]] 47 | if expression[0:2] == "-#": 48 | expression = "-" + configuration_parameters[expression[2:]] 49 | 50 | parts = expression.split(" ") 51 | 52 | # Checking the unit, returning only radians and meters 53 | if parts[1] == "deg": 54 | return math.radians(float(parts[0])) 55 | elif parts[1] in ["radian", "rad"]: 56 | return float(parts[0]) 57 | elif parts[1] == "mm": 58 | return float(parts[0]) / 1000.0 59 | elif parts[1] == "m": 60 | return float(parts[0]) 61 | else: 62 | print(Fore.RED + "Unknown unit: " + parts[1] + Style.RESET_ALL) 63 | sys.exit(1) 64 | 65 | 66 | def read_parameter_value(parameter: dict[str, Any], name: str) -> float: 67 | # This is an expression 68 | if parameter["typeName"] == "BTMParameterNullableQuantity": 69 | return read_expression(parameter["message"]["expression"]) 70 | if parameter["typeName"] == "BTMParameterConfigured": 71 | message = parameter["message"] 72 | parameter_value = configuration_parameters[message["configurationParameterId"]] 73 | 74 | for value in message["values"]: 75 | if value["typeName"] == "BTMConfiguredValueByBoolean": 76 | boolean_value = parameter_value == "true" 77 | if value["message"]["booleanValue"] == boolean_value: 78 | return read_expression( 79 | value["message"]["value"]["message"]["expression"] 80 | ) 81 | elif value["typeName"] == "BTMConfiguredValueByEnum": 82 | if value["message"]["enumValue"] == parameter_value: 83 | return read_expression( 84 | value["message"]["value"]["message"]["expression"] 85 | ) 86 | else: 87 | print( 88 | Fore.RED 89 | + "Can't read value of parameter " 90 | + name 91 | + " configured with " 92 | + value["typeName"] 93 | + Style.RESET_ALL 94 | ) 95 | sys.exit(1) 96 | 97 | print(Fore.RED + "Could not find the value for " + name + Style.RESET_ALL) 98 | sys.exit(1) 99 | else: 100 | print( 101 | Fore.RED 102 | + "Unknown feature type for " 103 | + name 104 | + ": " 105 | + parameter["typeName"] 106 | + Style.RESET_ALL 107 | ) 108 | sys.exit(1) 109 | 110 | 111 | def get_limits(joint_type: str, name: str) -> tuple[float, float] | None: 112 | """Gets the limits of a given joint""" 113 | enabled = False 114 | minimum, maximum = 0.0, 0.0 115 | for feature in joint_features["features"]: 116 | # Find corresponding joint 117 | if name == feature["message"]["name"]: 118 | # Find min and max values 119 | for parameter in feature["message"]["parameters"]: 120 | if parameter["message"]["parameterId"] == "limitsEnabled": 121 | enabled = parameter["message"]["value"] 122 | 123 | if joint_type == "revolute": 124 | if parameter["message"]["parameterId"] == "limitAxialZMin": 125 | minimum = read_parameter_value(parameter, name) 126 | if parameter["message"]["parameterId"] == "limitAxialZMax": 127 | maximum = read_parameter_value(parameter, name) 128 | elif joint_type == "prismatic": 129 | if parameter["message"]["parameterId"] == "limitZMin": 130 | minimum = read_parameter_value(parameter, name) 131 | if parameter["message"]["parameterId"] == "limitZMax": 132 | maximum = read_parameter_value(parameter, name) 133 | if enabled: 134 | return minimum, maximum 135 | else: 136 | if joint_type != "continuous": 137 | print( 138 | Fore.YELLOW 139 | + "WARNING: joint " 140 | + name 141 | + " of type " 142 | + joint_type 143 | + " has no limits " 144 | + Style.RESET_ALL 145 | ) 146 | return None 147 | -------------------------------------------------------------------------------- /docs/config.rst: -------------------------------------------------------------------------------- 1 | Export your own robot (writing config.yaml) 2 | =========================================== 3 | 4 | To export your own robot, first create a directory: 5 | 6 | .. code-block:: bash 7 | 8 | mkdir my-robot 9 | 10 | Then edit ``my-robot/config.yaml``, here is the minimum example: 11 | 12 | .. code-block:: yaml 13 | 14 | document_id: document-id 15 | 16 | The ``document_id`` is the number (below XXXXXXXXX) you can find in Onshape URL: 17 | 18 | .. code-block:: bash 19 | 20 | https://cad.onshape.com/documents/XXXXXXXXX/w/YYYYYYYY/e/ZZZZZZZZ 21 | 22 | Once this is done, if you properly :doc:`installed and setup your API key `, just run: 23 | 24 | .. code-block:: bash 25 | 26 | onshape-urdf-exporter my-robot 27 | 28 | ``config.yaml`` entries 29 | ----------------------- 30 | 31 | Here is the full list of possible entries for this configuration. 32 | 33 | ``document_id`` 34 | ~~~~~~~~~~~~~~~ 35 | 36 | This is the Onshape ID of the document to be imported. It can be found in the Onshape URL, 37 | just after ``document/``. 38 | 39 | 40 | ``assembly_name`` 41 | ~~~~~~~~~~~~~~~~~ 42 | 43 | *optional* 44 | 45 | This can be used to specify the name of the assembly (in the Onshape document) to be used for robot export. If none 46 | is used, the first assembly found will be used. 47 | 48 | ``workspace_id`` 49 | ~~~~~~~~~~~~~~~~ 50 | 51 | *optional, no default* 52 | 53 | This argument can be used to use a specific workspace of the document. This can be used for specific branches 54 | ofr your robot without making a version. 55 | The workspace ID can be found in URL, after the ``/w/`` part when selecting a specific version in the tree. 56 | 57 | ``version_id`` 58 | ~~~~~~~~~~~~~~ 59 | 60 | *optional, no default* 61 | 62 | This argument can be used to use a specific version of the document instead of the last one. The version ID 63 | can be found in URL, after the ``/v/`` part when selecting a specific version in the tree. 64 | 65 | If it is not specified, the very last version will be used for import. 66 | 67 | ``configuration`` 68 | ~~~~~~~~~~~~~~~~~ 69 | 70 | *optional, default: "default"* 71 | 72 | This is the robot configuration string that will be passed to Onshape. An example of format: 73 | 74 | .. code-block:: js 75 | 76 | left_motor_angle=3+radian;enable_yaw=true 77 | 78 | ``draw_frames`` 79 | ~~~~~~~~~~~~~~~ 80 | 81 | *optional, default: false* 82 | 83 | When :ref:`adding custom frames to your model `, the part that is used for positioning the frame is 84 | by default excluded from the output description (a dummy link is kept instead). Passing this option to ``true`` will 85 | keep it instead. 86 | 87 | ``draw_collisions`` 88 | ~~~~~~~~~~~~~~~~~~~ 89 | 90 | *optional, default: false* 91 | 92 | Controls if collision shapes are shown visually. 93 | 94 | ``joint_max_effort`` and ``joint_max_velocity`` 95 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 96 | 97 | *optional, default: 1 and 20* 98 | 99 | Those parameters can be used to specify the values that will be included in the ``joint`` entries. 100 | 101 | Alternatively, they can be dictionaries associating named joints to the values. 102 | 103 | 104 | ``dynamics`` 105 | ~~~~~~~~~~~~ 106 | 107 | *optional, default: {}* 108 | 109 | This ``dict`` can be used to override the mass and inertia computed by Onshape for a specific part. 110 | See :ref:`example ` below. 111 | 112 | 113 | ``no_dynamics`` 114 | ~~~~~~~~~~~~~~~ 115 | 116 | *optional, default: false* 117 | 118 | This flag can be set if there is no dynamics. In that case all masses and inertia will be set to 0. 119 | 120 | ``ignore`` 121 | ~~~~~~~~~~ 122 | 123 | *optional, default: []* 124 | 125 | This can be a list of parts that you want to be ignored during the export. 126 | 127 | Note: the dynamics of the part will not be ignored, but the visual and collision aspect will. 128 | 129 | ``whitelist`` 130 | ~~~~~~~~~~~~~ 131 | 132 | *optional, default: None* 133 | 134 | This can be used as the opposed of ``ignore``, to import only some items listed in the configuration 135 | (all items not listed in ``whitelist`` will be ignored if it is not ``None``) 136 | 137 | ``color`` 138 | ~~~~~~~~~ 139 | 140 | *optional, default: None* 141 | 142 | Can override the color for parts (should be an array: ``[r, g, b]`` with numbers from 0 to 1) 143 | 144 | ``package_name`` 145 | ~~~~~~~~~~~~~~~~ 146 | 147 | *optional* 148 | 149 | Prepends a string to the paths of STL files. This is helpful for ROS users as they often need to specify their 150 | ``robot_description`` package. 151 | 152 | ``add_dummy_base_link`` 153 | ~~~~~~~~~~~~~~~~~~~~~~~ 154 | 155 | *optional* 156 | 157 | Adds a ``base_link`` without inertia as root. This is often necessary for ROS users. 158 | 159 | ``robot_name`` 160 | ~~~~~~~~~~~~~~ 161 | 162 | *optional* 163 | 164 | Specifies the robot name. 165 | 166 | ``additional_urdf_file`` 167 | ~~~~~~~~~~~~~~~~~~~~~~~~ 168 | 169 | *optional* 170 | 171 | Specifies a file with XML content that is inserted into the URDF at the end of the file. Useful to add things that can't be modelled in Onshape, e.g. simulated sensors. 172 | 173 | ``use_fixed_links`` 174 | ~~~~~~~~~~~~~~~~~~~ 175 | 176 | *optional, default: false* 177 | 178 | With this option, visual parts will be added through fixed links to each part of the robot. 179 | 180 | ``simplify_stls`` 181 | ~~~~~~~~~~~~~~~~~ 182 | 183 | *optional, default: "no"* 184 | 185 | Can be "no", "visual", "collision" or "all". 186 | 187 | If this is set, the complexity of the STL files will be reduced. This can be 188 | good for file size and visualization performance. 189 | 190 | ``use_collisions_configurations`` 191 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 192 | 193 | *optional, default: true* 194 | 195 | With this option (enabled by default), the collisions=true configuration will be passed when exporting STL 196 | meshes (and NOT dynamics), in order to retrieve simplified mesh parts from Onshape. 197 | 198 | This is a way to approximate your robot with simpler meshes. 199 | 200 | ``post_import_commands`` 201 | ~~~~~~~~~~~~~~~~~~~~~~~~ 202 | 203 | *optional, default: []* 204 | 205 | This is an array of commands that will be executed after the import is done. It can be used to be sure that 206 | some processing scripts are run everytime you run the tool. 207 | 208 | .. _example-config: 209 | 210 | Example ``config.yaml`` file 211 | ---------------------------- 212 | 213 | Here is an example of configuration: 214 | 215 | .. code-block:: yaml 216 | 217 | # Can be found in the URL when editing the assembly 218 | document_id: 483c803918afc4d52e2647f0 219 | # If not specified, the first assembly will be used 220 | assembly_name: robot 221 | # The frames parts are kept in the final file 222 | draw_frames: false 223 | # Collisions (pure shapes) are also used in the visual section 224 | draw_collisions: false 225 | # Masses, com and inertias will be zero (can be used if you import a static 226 | # field for example) 227 | no_dynamics: false 228 | # Should we simplify STLs files? 229 | simplify_stls: false 230 | 231 | # Those can be used to configure the joint max efforts and velocity, and 232 | # overriden for specific joints 233 | joint_max_effort: 234 | default: 1.5 235 | head_pitch: 0.5 236 | joint_max_velocity: 22 237 | 238 | # This can be used to override the dynamics of some part (suppose it's a compound 239 | # which dynamics is well specified) 240 | dynamics: 241 | motorcase: 242 | mass: 0.5 243 | com: [0, 0.1, 0] 244 | inertia: [0.1, 0, 0, 0, 0.1, 0, 0, 0, 0.1] 245 | 246 | # "fixed" can be used to assign a null mass to the object, which makes it fixed (non-dynamics) 247 | base: fixed 248 | 249 | # Some parts can be totally ignored during import 250 | ignore: 251 | - small_screw 252 | - small_nut 253 | -------------------------------------------------------------------------------- /onshape_urdf_exporter/onshape_api/onshape.py: -------------------------------------------------------------------------------- 1 | """ 2 | onshape 3 | ====== 4 | 5 | Provides access to the Onshape REST API 6 | """ 7 | 8 | import base64 9 | import datetime 10 | import hashlib 11 | import hmac 12 | import json 13 | import os 14 | import random 15 | import string 16 | import urllib 17 | from urllib.parse import parse_qs, urlparse 18 | 19 | import requests 20 | from colorama import Fore, Style 21 | 22 | from . import utils 23 | 24 | __all__ = ["Onshape"] 25 | 26 | 27 | class Onshape: 28 | """Provides access to the Onshape REST API""" 29 | 30 | def __init__(self, stack, logging=True): 31 | """ 32 | :param stack: Base API URL 33 | """ 34 | self._logging = logging 35 | 36 | self._url = os.getenv("ONSHAPE_API") 37 | self._access_key = os.getenv("ONSHAPE_ACCESS_KEY") 38 | self._secret_key = os.getenv("ONSHAPE_SECRET_KEY") 39 | 40 | if self._url is None or self._access_key is None or self._secret_key is None: 41 | print( 42 | Fore.RED + "ERROR: No OnShape API access key are set" + Style.RESET_ALL 43 | ) 44 | print() 45 | print( 46 | Fore.BLUE 47 | + "TIP: Connect to https://dev-portal.onshape.com/keys, and edit your .bashrc file:" 48 | + Style.RESET_ALL 49 | ) 50 | print( 51 | Fore.BLUE 52 | + "export ONSHAPE_API=https://cad.onshape.com" 53 | + Style.RESET_ALL 54 | ) 55 | print( 56 | Fore.BLUE 57 | + "export ONSHAPE_ACCESS_KEY=Your_Access_Key" 58 | + Style.RESET_ALL 59 | ) 60 | print( 61 | Fore.BLUE 62 | + "export ONSHAPE_SECRET_KEY=Your_Secret_Key" 63 | + Style.RESET_ALL 64 | ) 65 | exit(1) 66 | 67 | self._access_key = self._access_key.encode("utf-8") 68 | self._secret_key = self._secret_key.encode("utf-8") 69 | 70 | if self._logging: 71 | utils.log( 72 | "onshape instance created: url = %s, access key = %s" 73 | % (self._url, self._access_key) 74 | ) 75 | 76 | def _make_nonce(self): 77 | """ 78 | Generate a unique ID for the request, 25 chars in length 79 | 80 | Returns: 81 | - str: Cryptographic nonce 82 | """ 83 | 84 | chars = string.digits + string.ascii_letters 85 | nonce = "".join(random.choice(chars) for i in range(25)) 86 | 87 | if self._logging: 88 | utils.log("nonce created: %s" % nonce) 89 | 90 | return nonce 91 | 92 | def _make_auth(self, method, date, nonce, path, query={}, ctype="application/json"): 93 | """ 94 | Create the request signature to authenticate 95 | 96 | Args: 97 | - method (str): HTTP method 98 | - date (str): HTTP date header string 99 | - nonce (str): Cryptographic nonce 100 | - path (str): URL pathname 101 | - query (dict, default={}): URL query string in key-value pairs 102 | - ctype (str, default='application/json'): HTTP Content-Type 103 | """ 104 | 105 | query = urllib.parse.urlencode(query) 106 | 107 | hmac_str = ( 108 | ( 109 | method 110 | + "\n" 111 | + nonce 112 | + "\n" 113 | + date 114 | + "\n" 115 | + ctype 116 | + "\n" 117 | + path 118 | + "\n" 119 | + query 120 | + "\n" 121 | ) 122 | .lower() 123 | .encode("utf-8") 124 | ) 125 | 126 | signature = base64.b64encode( 127 | hmac.new(self._secret_key, hmac_str, digestmod=hashlib.sha256).digest() 128 | ) 129 | auth = ( 130 | "On " 131 | + self._access_key.decode("utf-8") 132 | + ":HmacSHA256:" 133 | + signature.decode("utf-8") 134 | ) 135 | 136 | if self._logging: 137 | utils.log( 138 | { 139 | "query": query, 140 | "hmac_str": hmac_str, 141 | "signature": signature, 142 | "auth": auth, 143 | } 144 | ) 145 | 146 | return auth 147 | 148 | def _make_headers(self, method, path, query={}, headers={}): 149 | """ 150 | Creates a headers object to sign the request 151 | 152 | Args: 153 | - method (str): HTTP method 154 | - path (str): Request path, e.g. /api/documents. No query string 155 | - query (dict, default={}): Query string in key-value format 156 | - headers (dict, default={}): Other headers to pass in 157 | 158 | Returns: 159 | - dict: Dictionary containing all headers 160 | """ 161 | 162 | date = datetime.datetime.utcnow().strftime("%a, %d %b %Y %H:%M:%S GMT") 163 | nonce = self._make_nonce() 164 | ctype = ( 165 | headers.get("Content-Type") 166 | if headers.get("Content-Type") 167 | else "application/json" 168 | ) 169 | 170 | auth = self._make_auth(method, date, nonce, path, query=query, ctype=ctype) 171 | 172 | req_headers = { 173 | "Content-Type": "application/json", 174 | "Date": date, 175 | "On-Nonce": nonce, 176 | "Authorization": auth, 177 | "User-Agent": "Onshape Python Sample App", 178 | "Accept": "application/json", 179 | } 180 | 181 | # add in user-defined headers 182 | for h in headers: 183 | req_headers[h] = headers[h] 184 | 185 | return req_headers 186 | 187 | def request(self, method, path, query={}, headers={}, body={}, base_url=None): 188 | """ 189 | Issues a request to Onshape 190 | 191 | Args: 192 | - method (str): HTTP method 193 | - path (str): Path e.g. /api/documents/:id 194 | - query (dict, default={}): Query params in key-value pairs 195 | - headers (dict, default={}): Key-value pairs of headers 196 | - body (dict, default={}): Body for POST request 197 | - base_url (str, default=None): Host, including scheme and port (if different from creds file) 198 | 199 | Returns: 200 | - requests.Response: Object containing the response from Onshape 201 | """ 202 | 203 | req_headers = self._make_headers(method, path, query, headers) 204 | if base_url is None: 205 | base_url = self._url 206 | url = base_url + path + "?" + urllib.parse.urlencode(query) 207 | 208 | if self._logging: 209 | utils.log(body) 210 | utils.log(req_headers) 211 | utils.log("request url: " + url) 212 | 213 | # only parse as json string if we have to 214 | body = json.dumps(body) if type(body) == dict else body 215 | 216 | res = requests.request( 217 | method, 218 | url, 219 | headers=req_headers, 220 | data=body, 221 | allow_redirects=False, 222 | stream=True, 223 | ) 224 | 225 | if res.status_code == 307: 226 | location = urlparse(res.headers["Location"]) 227 | querystring = parse_qs(location.query) 228 | 229 | if self._logging: 230 | utils.log("request redirected to: " + location.geturl()) 231 | 232 | new_query = {} 233 | new_base_url = location.scheme + "://" + location.netloc 234 | 235 | for key in querystring: 236 | new_query[key] = querystring[key][ 237 | 0 238 | ] # won't work for repeated query params 239 | 240 | return self.request( 241 | method, 242 | location.path, 243 | query=new_query, 244 | headers=headers, 245 | base_url=new_base_url, 246 | ) 247 | elif not 200 <= res.status_code <= 206: 248 | print(url) 249 | print("! ERROR (" + str(res.status_code) + ") while using OnShape API") 250 | if res.text: 251 | print("! " + res.text) 252 | 253 | if res.status_code == 403: 254 | print( 255 | "HINT: Check that your access rights are correct, and that the clock on your computer is set correctly" 256 | ) 257 | raise ConnectionRefusedError( 258 | "HINT: Check that your access rights are correct, and that the clock on your computer is set correctly" 259 | ) 260 | if self._logging: 261 | utils.log("request failed, details: " + res.text, level=1) 262 | exit() 263 | 264 | else: 265 | if self._logging: 266 | utils.log("request succeeded, details: " + res.text) 267 | 268 | return res 269 | -------------------------------------------------------------------------------- /onshape_urdf_exporter/__main__.py: -------------------------------------------------------------------------------- 1 | import hashlib 2 | import json 3 | import os 4 | from argparse import ArgumentParser 5 | from pathlib import Path 6 | 7 | import numpy as np 8 | from colorama import Fore, Style 9 | 10 | from .config_file import Configuration 11 | from .robot_description import RobotURDF 12 | from .stl_utils import simplify_stl 13 | 14 | partNames = {} 15 | 16 | 17 | def main(): 18 | parser = ArgumentParser( 19 | description="Exports an OnShape assembly to a URDF file with STL meshes" 20 | ) 21 | parser.add_argument( 22 | "robot_directory", 23 | type=Path, 24 | help="A directory containing the config.yaml", 25 | ) 26 | args = parser.parse_args() 27 | 28 | config = Configuration.from_file(args.robot_directory) 29 | 30 | # Loading configuration, collecting occurrences and building robot tree 31 | from .load_robot import client, frames, getOccurrence, occurrences, tree 32 | 33 | robot = RobotURDF(config.robot_name, config) 34 | 35 | def partIsIgnore(name): 36 | if config.whitelist is None: 37 | return name in config.ignore 38 | else: 39 | return name not in config.whitelist 40 | 41 | # Adds a part to the current robot link 42 | 43 | def addPart(occurrence, matrix): 44 | part = occurrence["instance"] 45 | 46 | if part["suppressed"]: 47 | return 48 | 49 | if part["partId"] == "": 50 | print( 51 | Fore.YELLOW 52 | + "WARNING: Part " 53 | + part["name"] 54 | + " has no partId" 55 | + Style.RESET_ALL 56 | ) 57 | return 58 | 59 | # Importing STL file for this part 60 | justPart, prefix = extractPartName(part["name"], part["configuration"]) 61 | 62 | extra = "" 63 | if occurrence["instance"]["configuration"] != "default": 64 | extra = ( 65 | Style.DIM 66 | + " (configuration: " 67 | + occurrence["instance"]["configuration"] 68 | + ")" 69 | ) 70 | symbol = "+" 71 | if partIsIgnore(justPart): 72 | symbol = "-" 73 | extra += Style.DIM + " / ignoring visual and collision" 74 | 75 | print( 76 | Fore.GREEN 77 | + symbol 78 | + " Adding part " 79 | + occurrence["instance"]["name"] 80 | + extra 81 | + Style.RESET_ALL 82 | ) 83 | 84 | stl_file: Path | None 85 | if partIsIgnore(justPart): 86 | stl_file = None 87 | else: 88 | stl_file = args.robot_directory / sanitize_filename(prefix + ".stl") 89 | # shorten the configuration to a maximum number of chars to prevent errors. 90 | # Necessary for standard parts like screws 91 | if len(part["configuration"]) > 40: 92 | shortend_configuration = hashlib.md5( 93 | part["configuration"].encode("utf-8") 94 | ).hexdigest() 95 | else: 96 | shortend_configuration = part["configuration"] 97 | 98 | try: 99 | stl = client.part_studio_stl_m( 100 | part["documentId"], 101 | part["documentMicroversion"], 102 | part["elementId"], 103 | part["partId"], 104 | shortend_configuration, 105 | ) 106 | except ConnectionRefusedError as e: 107 | print( 108 | Fore.RED 109 | + f"Skipping part {part['name']}, because of connection error: {e}" 110 | + Style.RESET_ALL 111 | ) 112 | return 113 | 114 | stl_file.write_bytes(stl) 115 | if config.simplify_stls: 116 | simplify_stl(stl_file) 117 | 118 | stlMetadata = sanitize_filename(prefix + ".part") 119 | with open( 120 | args.robot_directory / stlMetadata, "w", encoding="utf-8" 121 | ) as stream: 122 | json.dump(part, stream, indent=4, sort_keys=True) 123 | 124 | # Obtain metadatas about part to retrieve color 125 | if config.color is not None: 126 | color = config.color 127 | else: 128 | metadata = client.part_get_metadata( 129 | part["documentId"], 130 | part["documentMicroversion"], 131 | part["elementId"], 132 | part["partId"], 133 | part["configuration"], 134 | ) 135 | 136 | color = [0.5, 0.5, 0.5] 137 | 138 | # XXX: There must be a better way to retrieve the part color 139 | for entry in metadata["properties"]: 140 | if ( 141 | "value" in entry 142 | and type(entry["value"]) is dict 143 | and "color" in entry["value"] 144 | ): 145 | rgb = entry["value"]["color"] 146 | color = np.array([rgb["red"], rgb["green"], rgb["blue"]]) / 255.0 147 | 148 | # Obtain mass properties about that part 149 | if config.no_dynamics: 150 | mass = 0 151 | com = [0] * 3 152 | inertia = [0] * 12 153 | else: 154 | if prefix in config.dynamics_override: 155 | entry = config.dynamics_override[prefix] 156 | mass = entry["mass"] 157 | com = entry["com"] 158 | inertia = entry["inertia"] 159 | else: 160 | massProperties = client.part_mass_properties( 161 | part["documentId"], 162 | part["documentMicroversion"], 163 | part["elementId"], 164 | part["partId"], 165 | part["configuration"], 166 | ) 167 | 168 | if part["partId"] not in massProperties["bodies"]: 169 | print( 170 | Fore.YELLOW 171 | + "WARNING: part " 172 | + part["name"] 173 | + " has no dynamics (maybe it is a surface)" 174 | + Style.RESET_ALL 175 | ) 176 | return 177 | massProperties = massProperties["bodies"][part["partId"]] 178 | mass = massProperties["mass"][0] 179 | com = massProperties["centroid"] 180 | inertia = massProperties["inertia"] 181 | 182 | if abs(mass) < 1e-9: 183 | print( 184 | Fore.YELLOW 185 | + "WARNING: part " 186 | + part["name"] 187 | + " has no mass, maybe you should assign a material to it ?" 188 | + Style.RESET_ALL 189 | ) 190 | 191 | pose = occurrence["transform"] 192 | if robot.relative: 193 | pose = np.linalg.inv(matrix) * pose 194 | 195 | robot.add_part(pose, stl_file, mass, com, inertia, color, prefix) 196 | 197 | partNames = {} 198 | 199 | def extractPartName(name, configuration): 200 | parts = name.split(" ") 201 | del parts[-1] 202 | basePartName = "_".join(parts).lower() 203 | 204 | # only add configuration to name if its not default and not a very long configuration (which happens for library parts like screws) 205 | if configuration != "default" and len(configuration) < 40: 206 | parts += ["_" + configuration.replace("=", "_").replace(" ", "_")] 207 | 208 | return basePartName, "_".join(parts).lower() 209 | 210 | def processPartName(name, configuration, overrideName=None): 211 | if overrideName is None: 212 | global partNames 213 | _, name = extractPartName(name, configuration) 214 | 215 | if name in partNames: 216 | partNames[name] += 1 217 | else: 218 | partNames[name] = 1 219 | 220 | if partNames[name] == 1: 221 | return name 222 | else: 223 | return name + "_" + str(partNames[name]) 224 | else: 225 | return overrideName 226 | 227 | def buildRobot(tree, matrix): 228 | occurrence = getOccurrence([tree["id"]]) 229 | instance = occurrence["instance"] 230 | print( 231 | Fore.BLUE 232 | + Style.BRIGHT 233 | + "* Adding top-level instance [" 234 | + instance["name"] 235 | + "]" 236 | + Style.RESET_ALL 237 | ) 238 | 239 | # Build a part name that is unique but still informative 240 | link = processPartName( 241 | instance["name"], instance["configuration"], occurrence["linkName"] 242 | ) 243 | 244 | # Create the link, collecting all children in the tree assigned to this top-level part 245 | robot.start_link(link) 246 | for occurrence in occurrences.values(): 247 | if ( 248 | occurrence["assignation"] == tree["id"] 249 | and "type" in occurrence["instance"] 250 | and occurrence["instance"]["type"] == "Part" 251 | ): 252 | addPart(occurrence, matrix) 253 | robot.end_link() 254 | 255 | # Adding the frames (linkage is relative to parent) 256 | if tree["id"] in frames: 257 | for name, partOrFrame in frames[tree["id"]]: 258 | if type(partOrFrame) == list: 259 | frame = getOccurrence(partOrFrame)["transform"] 260 | else: 261 | frame = partOrFrame 262 | 263 | if robot.relative: 264 | frame = np.linalg.inv(matrix) * frame 265 | robot.add_frame(name, frame) 266 | 267 | # Following the children in the tree, calling this function recursively 268 | k = 0 269 | for child in tree["children"]: 270 | worldAxisFrame = child["axis_frame"] 271 | zAxis = child["z_axis"] 272 | jointType = child["jointType"] 273 | jointLimits = child["jointLimits"] 274 | 275 | if robot.relative: 276 | axisFrame = np.linalg.inv(matrix) * worldAxisFrame 277 | childMatrix = worldAxisFrame 278 | else: 279 | # In SDF format, everything is expressed in the world frame, in this case 280 | # childMatrix will be always identity 281 | axisFrame = worldAxisFrame 282 | childMatrix = matrix 283 | 284 | subLink = buildRobot(child, childMatrix) 285 | robot.add_joint( 286 | jointType, 287 | link, 288 | subLink, 289 | axisFrame, 290 | child["dof_name"], 291 | jointLimits, 292 | zAxis, 293 | ) 294 | 295 | return link 296 | 297 | # Start building the robot 298 | buildRobot(tree, np.matrix(np.identity(4))) 299 | 300 | print( 301 | "\n" 302 | + Style.BRIGHT 303 | + "* Writing " 304 | + robot.ext.upper() 305 | + " file" 306 | + Style.RESET_ALL 307 | ) 308 | with open( 309 | args.robot_directory / f"robot.{robot.ext}", "w", encoding="utf-8" 310 | ) as stream: 311 | robot.write_to(stream) 312 | 313 | if len(config.post_import_commands): 314 | print( 315 | "\n" + Style.BRIGHT + "* Executing post-import commands" + Style.RESET_ALL 316 | ) 317 | for command in config.post_import_commands: 318 | print("* " + command) 319 | os.system(command) 320 | 321 | 322 | def sanitize_filename(value: str) -> str: 323 | """Escapes characters that are invalid in filenames on Linux filesystems and NTFS. 324 | The escape sequences mimic URL encoding. 325 | 326 | Based on info from: https://stackoverflow.com/a/31976060 327 | """ 328 | invalid_chars = [chr(i) for i in range(32)] + [ 329 | "<", 330 | ">", 331 | ":", 332 | '"', 333 | "/", 334 | "\\", 335 | "|", 336 | "?", 337 | "*", 338 | ] 339 | 340 | for char in invalid_chars: 341 | value = value.replace(char, hex(ord(char)).replace("0x", "%")) 342 | 343 | return value 344 | 345 | 346 | if __name__ == "__main__": 347 | main() 348 | -------------------------------------------------------------------------------- /onshape_urdf_exporter/robot_description.py: -------------------------------------------------------------------------------- 1 | import math 2 | from pathlib import Path 3 | from typing import Any, TextIO 4 | from xml.etree import ElementTree as ET 5 | 6 | import numpy as np 7 | import numpy.typing as npt 8 | 9 | from .config_file import Configuration 10 | 11 | 12 | def rotation_matrix_to_euler_angles( 13 | r: npt.NDArray[np.float64], 14 | ) -> npt.NDArray[np.float64]: 15 | sy = math.sqrt(r[0, 0] * r[0, 0] + r[1, 0] * r[1, 0]) 16 | 17 | singular = sy < 1e-6 18 | 19 | if not singular: 20 | x = math.atan2(r[2, 1], r[2, 2]) 21 | y = math.atan2(-r[2, 0], sy) 22 | z = math.atan2(r[1, 0], r[0, 0]) 23 | else: 24 | x = math.atan2(-r[1, 2], r[1, 1]) 25 | y = math.atan2(-r[2, 0], sy) 26 | z = 0 27 | 28 | return np.array([x, y, z]) 29 | 30 | 31 | def add_origin_element(parent: ET.Element, matrix: npt.NDArray[np.float64]) -> None: 32 | x = matrix[0, 3] 33 | y = matrix[1, 3] 34 | z = matrix[2, 3] 35 | xyz_str = "%.20g %.20g %.20g" % (x, y, z) 36 | 37 | r, p, y = rotation_matrix_to_euler_angles(matrix) 38 | rpy_str = "%.20g %.20g %.20g" % (r, p, y) 39 | 40 | ET.SubElement(parent, "origin", xyz=xyz_str, rpy=rpy_str) 41 | 42 | 43 | class RobotDescription: 44 | def __init__(self, name: str, config: Configuration): 45 | self.config = config 46 | self.relative = True 47 | 48 | self._color = np.array([0.0, 0.0, 0.0]) 49 | self._color_mass = 0.0 50 | self._link_childs = 0 51 | self._visuals: list[list[Any]] = [] 52 | self._dynamics: list[dict[str, Any]] = [] 53 | 54 | def joint_max_effort_for(self, joint_name: str) -> float: 55 | if isinstance(self.config.joint_max_effort, dict): 56 | if joint_name in self.config.joint_max_effort: 57 | return self.config.joint_max_effort[joint_name] 58 | else: 59 | return self.config.joint_max_effort["default"] 60 | else: 61 | return self.config.joint_max_effort 62 | 63 | def joint_max_velocity_for(self, joint_name: str) -> float: 64 | if isinstance(self.config.joint_max_velocity, dict): 65 | if joint_name in self.config.joint_max_velocity: 66 | return self.config.joint_max_velocity[joint_name] 67 | else: 68 | return self.config.joint_max_velocity["default"] 69 | else: 70 | return self.config.joint_max_velocity 71 | 72 | def reset_link(self) -> None: 73 | self._color = np.array([0.0, 0.0, 0.0]) 74 | self._color_mass = 0 75 | self._link_childs = 0 76 | self._visuals = [] 77 | self._dynamics = [] 78 | 79 | def add_link_dynamics( 80 | self, 81 | matrix: npt.NDArray[np.float64], 82 | mass: float, 83 | com: npt.NDArray[np.float64], 84 | inertia: npt.NDArray[np.float64], 85 | ) -> None: 86 | # Inertia 87 | I = np.matrix(np.reshape(inertia[:9], (3, 3))) 88 | R = matrix[:3, :3] 89 | # Expressing COM in the link frame 90 | com = np.array((matrix * np.matrix([com[0], com[1], com[2], 1]).T).T)[0][:3] 91 | # Expressing inertia in the link frame 92 | inertia = R * I * R.T 93 | 94 | self._dynamics.append({"mass": mass, "com": com, "inertia": inertia}) 95 | 96 | def link_dynamics( 97 | self, 98 | ) -> tuple[float, npt.NDArray[np.float64], npt.NDArray[np.float64]]: 99 | mass = 0 100 | com = np.array([0.0] * 3) 101 | inertia = np.matrix(np.zeros((3, 3))) 102 | identity = np.matrix(np.eye(3)) 103 | 104 | for dynamic in self._dynamics: 105 | mass += dynamic["mass"] 106 | com += dynamic["com"] * dynamic["mass"] 107 | 108 | if mass > 0: 109 | com /= mass 110 | 111 | # https://pybullet.org/Bullet/phpBB3/viewtopic.php?t=246 112 | for dynamic in self._dynamics: 113 | r = dynamic["com"] - com 114 | p = np.matrix(r) 115 | inertia += ( # type: ignore[misc] 116 | dynamic["inertia"] 117 | + (np.dot(r, r) * identity - p.T * p) * dynamic["mass"] 118 | ) 119 | 120 | return mass, com, inertia 121 | 122 | 123 | class RobotURDF(RobotDescription): 124 | def __init__(self, name: str, config: Configuration): 125 | super().__init__(name, config) 126 | self.ext = "urdf" 127 | self.xml_root = ET.Element("robot", name=self.config.robot_name) 128 | self._active_link: ET.Element | None = None 129 | 130 | def add_dummy_link( 131 | self, 132 | name: str, 133 | visual_matrix: npt.NDArray[np.float64] | None = None, 134 | visual_stl: str | None = None, 135 | visual_color: npt.NDArray[np.float64] | None = None, 136 | ) -> None: 137 | link = ET.SubElement(self.xml_root, "link", name=name) 138 | inertial = ET.SubElement(link, "inertial") 139 | ET.SubElement(inertial, "origin", xyz="0 0 0", rpy="0 0 0") 140 | # XXX: We use a low mass because PyBullet consider mass 0 as world fixed 141 | if self.config.no_dynamics: 142 | ET.SubElement(inertial, "mass", value="0") 143 | else: 144 | ET.SubElement(inertial, "mass", value="1e-9") 145 | ET.SubElement( 146 | inertial, "inertia", ixx="0", ixy="0", ixz="0", iyy="0", iyz="0", izz="0" 147 | ) 148 | if visual_stl is not None: 149 | if visual_matrix is None or visual_color is None: 150 | raise RuntimeError( 151 | "visual_matrix, visual_stl, and visual_color must all be set if " 152 | "any one are set" 153 | ) 154 | 155 | self.add_stl( 156 | link, 157 | visual_matrix, 158 | visual_stl, 159 | visual_color, 160 | name + "_visual", 161 | "visual", 162 | ) 163 | 164 | def add_dummy_base_link_method(self, name: str) -> None: 165 | # adds a dummy base_link for ROS users 166 | ET.SubElement(self.xml_root, "link", name="base_link") 167 | joint = ET.SubElement( 168 | self.xml_root, "joint", name="base_link_to_base", type="fixed" 169 | ) 170 | ET.SubElement(joint, "parent", link="base_link") 171 | ET.SubElement(joint, "child", link=name) 172 | ET.SubElement(joint, "origin", rpy="0.0 0 0", xyz="0 0 0") 173 | 174 | def add_fixed_joint( 175 | self, 176 | parent: str, 177 | child: str, 178 | matrix: npt.NDArray[np.float64], 179 | name: str | None = None, 180 | ) -> None: 181 | if name is None: 182 | name = parent + "_" + child + "_fixing" 183 | 184 | joint = ET.SubElement(self.xml_root, "joint", name=name, type="fixed") 185 | add_origin_element(joint, matrix) 186 | ET.SubElement(joint, "parent", link=parent) 187 | ET.SubElement(joint, "child", link=child) 188 | ET.SubElement(joint, "axis", xyz="0 0 0") 189 | 190 | def start_link(self, name: str) -> None: 191 | self._link_name = name 192 | self.reset_link() 193 | 194 | if self.config.add_dummy_base_link: 195 | self.add_dummy_base_link_method(name) 196 | self.add_dummy_base_link = False 197 | self._active_link = ET.SubElement(self.xml_root, "link", name=name) 198 | 199 | def end_link(self) -> None: 200 | if self._active_link is None: 201 | raise RuntimeError("start_link must be called before end_link") 202 | 203 | mass, com, inertia = self.link_dynamics() 204 | 205 | inertial = ET.SubElement(self._active_link, "inertial") 206 | ET.SubElement( 207 | inertial, "origin", xyz="%.20g %.20g %.20g" % (com[0], com[1], com[2]) 208 | ) 209 | ET.SubElement(inertial, "mass", value="%.20g" % mass) 210 | ET.SubElement( 211 | inertial, 212 | "inertia", 213 | ixx="%.20g" % inertia[0, 0], 214 | ixy="%.20g" % inertia[0, 1], 215 | ixz="%.20g" % inertia[0, 2], 216 | iyy="%.20g" % inertia[1, 1], 217 | iyz="%.20g" % inertia[1, 2], 218 | izz="%.20g" % inertia[2, 2], 219 | ) 220 | 221 | if self.config.use_fixed_links: 222 | visual_elem = ET.SubElement(self._active_link, "visual") 223 | geometry = ET.SubElement(visual_elem, "geometry") 224 | ET.SubElement(geometry, "box", size="0 0 0") 225 | 226 | self._active_link = None 227 | 228 | if self.config.use_fixed_links: 229 | n = 0 230 | for visual in self._visuals: 231 | n += 1 232 | visual_name = "%s_%d" % (self._link_name, n) 233 | self.add_dummy_link(visual_name, visual[0], visual[1], visual[2]) 234 | self.add_joint( 235 | "fixed", 236 | self._link_name, 237 | visual_name, 238 | np.eye(4), 239 | visual_name + "_fixing", 240 | None, 241 | ) 242 | 243 | def add_frame(self, name: str, matrix: npt.NDArray[np.float64]) -> None: 244 | # Adding a dummy link 245 | self.add_dummy_link(name) 246 | 247 | # Linking it with last link with a fixed link 248 | self.add_fixed_joint(self._link_name, name, matrix, name + "_frame") 249 | 250 | def add_stl( 251 | self, 252 | parent: ET.Element, 253 | matrix: npt.NDArray[np.float64], 254 | stl: str, 255 | color: npt.NDArray[np.float64], 256 | name: str, 257 | node: str = "visual", 258 | ) -> None: 259 | stl_file = self.config.package_name.strip("/") + "/" + stl 260 | 261 | material_name = name + "_material" 262 | 263 | element = ET.SubElement(parent, node) 264 | add_origin_element(element, matrix) 265 | geometry = ET.SubElement(element, "geometry") 266 | ET.SubElement(geometry, "mesh", filename=f"package://{stl_file}") 267 | 268 | if node == "visual": 269 | material = ET.SubElement(element, "material", name=material_name) 270 | ET.SubElement( 271 | material, 272 | "color", 273 | rgba="%.20g %.20g %.20g 1.0" % (color[0], color[1], color[2]), 274 | ) 275 | 276 | def add_part( 277 | self, 278 | matrix: npt.NDArray[np.float64], 279 | stl: Path | None, 280 | mass: float, 281 | com: npt.NDArray[np.float64], 282 | inertia: npt.NDArray[np.float64], 283 | color: npt.NDArray[np.float64], 284 | name: str = "", 285 | ) -> None: 286 | if self._active_link is None: 287 | raise RuntimeError("Cannot call addPart before calling start_link") 288 | 289 | if stl is not None: 290 | if not self.config.draw_collisions: 291 | if self.config.use_fixed_links: 292 | self._visuals.append( 293 | [matrix, self.config.package_name + stl.name, color] 294 | ) 295 | else: 296 | self.add_stl( 297 | self._active_link, 298 | matrix, 299 | stl.name, 300 | color, 301 | name, 302 | "visual", 303 | ) 304 | 305 | entries = ["collision"] 306 | if self.config.draw_collisions: 307 | entries.append("visual") 308 | for entry in entries: 309 | self.add_stl( 310 | self._active_link, 311 | matrix, 312 | stl.name, 313 | color, 314 | name, 315 | entry, 316 | ) 317 | 318 | self.add_link_dynamics(matrix, mass, com, inertia) 319 | 320 | def add_joint( 321 | self, 322 | joint_type: str, 323 | link_from: str, 324 | link_to: str, 325 | transform: npt.NDArray[np.float64], 326 | name: str, 327 | joint_limits: list[float] | None, 328 | z_axis: list[float] | None = None, 329 | ) -> None: 330 | z_axis = [0, 0, 1] if z_axis is None else z_axis 331 | 332 | joint = ET.SubElement(self.xml_root, "joint", name=name, type=joint_type) 333 | add_origin_element(joint, transform) 334 | ET.SubElement(joint, "parent", link=link_from) 335 | ET.SubElement(joint, "child", link=link_to) 336 | ET.SubElement(joint, "axis", xyz="%.20g %.20g %.20g" % tuple(z_axis)) 337 | 338 | limit_elem = ET.SubElement( 339 | joint, 340 | "limit", 341 | effort="%.20g" % self.joint_max_effort_for(name), 342 | velocity="%.20g" % self.joint_max_velocity_for(name), 343 | ) 344 | if joint_limits is not None: 345 | limit_elem.set("lower", "%.20g" % joint_limits[0]) 346 | limit_elem.set("upper", "%.20g" % joint_limits[1]) 347 | ET.SubElement(joint, "joint_properties", friction="0.0") 348 | 349 | def write_to(self, stream: TextIO) -> None: 350 | stream.write(ET.tostring(self.xml_root, encoding="unicode")) 351 | -------------------------------------------------------------------------------- /onshape_urdf_exporter/onshape_api/client.py: -------------------------------------------------------------------------------- 1 | """ 2 | client 3 | ====== 4 | 5 | Convenience functions for working with the Onshape API 6 | """ 7 | 8 | import hashlib 9 | import json 10 | import mimetypes 11 | import os 12 | import random 13 | import string 14 | from pathlib import Path 15 | 16 | from .onshape import Onshape 17 | 18 | 19 | def escape_url(s): 20 | return s.replace("/", "%2f").replace("+", "%2b") 21 | 22 | 23 | class Client: 24 | """ 25 | Defines methods for testing the Onshape API. Comes with several methods: 26 | 27 | - Create a document 28 | - Delete a document 29 | - Get a list of documents 30 | 31 | Attributes: 32 | - stack (str, default='https://cad.onshape.com'): Base URL 33 | - logging (bool, default=True): Turn logging on or off 34 | """ 35 | 36 | def __init__(self, stack="https://cad.onshape.com", logging=True): 37 | """ 38 | Instantiates a new Onshape client. 39 | 40 | Args: 41 | - stack (str, default='https://cad.onshape.com'): Base URL 42 | - logging (bool, default=True): Turn logging on or off 43 | """ 44 | 45 | self._metadata_cache = {} 46 | self._massproperties_cache = {} 47 | self._stack = stack 48 | self._api = Onshape(stack=stack, logging=logging) 49 | self.useCollisionsConfigurations = True 50 | 51 | @staticmethod 52 | def get_cache_path() -> Path: 53 | """Return the path to the user cache.""" 54 | path = Path.home() / ".cache" / "onshape-urdf-exporter" 55 | path.mkdir(parents=True, exist_ok=True) 56 | return path 57 | 58 | def new_document(self, name="Test Document", owner_type=0, public=False): 59 | """ 60 | Create a new document. 61 | 62 | Args: 63 | - name (str, default='Test Document'): The doc name 64 | - owner_type (int, default=0): 0 for user, 1 for company, 2 for team 65 | - public (bool, default=False): Whether or not to make doc public 66 | 67 | Returns: 68 | - requests.Response: Onshape response data 69 | """ 70 | 71 | payload = {"name": name, "ownerType": owner_type, "isPublic": public} 72 | 73 | return self._api.request("post", "/api/documents", body=payload) 74 | 75 | def rename_document(self, did, name): 76 | """ 77 | Renames the specified document. 78 | 79 | Args: 80 | - did (str): Document ID 81 | - name (str): New document name 82 | 83 | Returns: 84 | - requests.Response: Onshape response data 85 | """ 86 | 87 | payload = {"name": name} 88 | 89 | return self._api.request("post", "/api/documents/" + did, body=payload) 90 | 91 | def del_document(self, did): 92 | """ 93 | Delete the specified document. 94 | 95 | Args: 96 | - did (str): Document ID 97 | 98 | Returns: 99 | - requests.Response: Onshape response data 100 | """ 101 | 102 | return self._api.request("delete", "/api/documents/" + did) 103 | 104 | def get_document(self, did): 105 | """ 106 | Get details for a specified document. 107 | 108 | Args: 109 | - did (str): Document ID 110 | 111 | Returns: 112 | - requests.Response: Onshape response data 113 | """ 114 | return self._api.request("get", "/api/documents/" + did) 115 | 116 | def cache_get(self, method, key, callback, isString=False): 117 | if type(key) == tuple: 118 | key = "_".join(list(key)) 119 | fileName = method + "__" + key 120 | dirName = self.get_cache_path() 121 | 122 | m = hashlib.sha1() 123 | m.update(fileName.encode("utf-8")) 124 | fileName = m.hexdigest() 125 | 126 | fileName = dirName / fileName 127 | 128 | if os.path.exists(fileName): 129 | with open(fileName, "rb") as stream: 130 | result = stream.read() 131 | else: 132 | result = callback().content 133 | with open(fileName, "wb") as stream: 134 | stream.write(result) 135 | if isString and type(result) == bytes: 136 | result = result.decode("utf-8") 137 | return result 138 | 139 | def list_documents(self): 140 | """ 141 | Get list of documents for current user. 142 | 143 | Returns: 144 | - requests.Response: Onshape response data 145 | """ 146 | 147 | return self._api.request("get", "/api/documents") 148 | 149 | def list_elements(self, did, wid, type="w"): 150 | """ 151 | Get the list of elements in a given document 152 | """ 153 | 154 | return self._api.request( 155 | "get", "/api/documents/d/" + did + "/" + type + "/" + wid + "/elements" 156 | ) 157 | 158 | def create_assembly(self, did, wid, name="My Assembly"): 159 | """ 160 | Creates a new assembly element in the specified document / workspace. 161 | 162 | Args: 163 | - did (str): Document ID 164 | - wid (str): Workspace ID 165 | - name (str, default='My Assembly') 166 | 167 | Returns: 168 | - requests.Response: Onshape response data 169 | """ 170 | 171 | payload = {"name": name} 172 | 173 | return self._api.request( 174 | "post", "/api/assemblies/d/" + did + "/w/" + wid, body=payload 175 | ) 176 | 177 | def get_assembly(self, did, wid, eid, type="w", configuration="default"): 178 | return self._api.request( 179 | "get", 180 | "/api/assemblies/d/" + did + "/" + type + "/" + wid + "/e/" + eid, 181 | query={ 182 | "includeMateFeatures": "true", 183 | "includeMateConnectors": "true", 184 | "includeNonSolids": "true", 185 | "configuration": configuration, 186 | }, 187 | ).json() 188 | 189 | def get_features(self, did, wid, eid, type="w"): 190 | """ 191 | Gets the feature list for specified document / workspace / part studio. 192 | 193 | Args: 194 | - did (str): Document ID 195 | - wid (str): Workspace ID 196 | - eid (str): Element ID 197 | 198 | Returns: 199 | - requests.Response: Onshape response data 200 | """ 201 | 202 | return self._api.request( 203 | "get", 204 | "/api/assemblies/d/" 205 | + did 206 | + "/" 207 | + type 208 | + "/" 209 | + wid 210 | + "/e/" 211 | + eid 212 | + "/features", 213 | ).json() 214 | 215 | def get_assembly_features(self, did, wid, eid): 216 | """ 217 | Gets the feature list for specified document / workspace / part studio. 218 | 219 | Args: 220 | - did (str): Document ID 221 | - wid (str): Workspace ID 222 | - eid (str): Element ID 223 | 224 | Returns: 225 | - requests.Response: Onshape response data 226 | """ 227 | 228 | return self._api.request( 229 | "get", "/api/assemblies/d/" + did + "/w/" + wid + "/e/" + eid + "/features" 230 | ) 231 | 232 | def get_partstudio_tessellatededges(self, did, wid, eid): 233 | """ 234 | Gets the tessellation of the edges of all parts in a part studio. 235 | 236 | Args: 237 | - did (str): Document ID 238 | - wid (str): Workspace ID 239 | - eid (str): Element ID 240 | 241 | Returns: 242 | - requests.Response: Onshape response data 243 | """ 244 | 245 | return self._api.request( 246 | "get", 247 | "/api/partstudios/d/" 248 | + did 249 | + "/w/" 250 | + wid 251 | + "/e/" 252 | + eid 253 | + "/tessellatededges", 254 | ) 255 | 256 | def upload_blob(self, did, wid, filepath="./blob.json"): 257 | """ 258 | Uploads a file to a new blob element in the specified doc. 259 | 260 | Args: 261 | - did (str): Document ID 262 | - wid (str): Workspace ID 263 | - filepath (str, default='./blob.json'): Blob element location 264 | 265 | Returns: 266 | - requests.Response: Onshape response data 267 | """ 268 | 269 | chars = string.ascii_letters + string.digits 270 | boundary_key = "".join(random.choice(chars) for i in range(8)) 271 | 272 | mimetype = mimetypes.guess_type(filepath)[0] 273 | encoded_filename = os.path.basename(filepath) 274 | file_content_length = str(os.path.getsize(filepath)) 275 | with open(filepath, "r", encoding="utf-8") as stream: 276 | blob = stream.read() 277 | 278 | req_headers = { 279 | "Content-Type": 'multipart/form-data; boundary="%s"' % boundary_key 280 | } 281 | 282 | # build request body 283 | payload = ( 284 | "--" 285 | + boundary_key 286 | + '\r\nContent-Disposition: form-data; name="encodedFilename"\r\n\r\n' 287 | + encoded_filename 288 | + "\r\n" 289 | ) 290 | payload += ( 291 | "--" 292 | + boundary_key 293 | + '\r\nContent-Disposition: form-data; name="fileContentLength"\r\n\r\n' 294 | + file_content_length 295 | + "\r\n" 296 | ) 297 | payload += ( 298 | "--" 299 | + boundary_key 300 | + '\r\nContent-Disposition: form-data; name="file"; filename="' 301 | + encoded_filename 302 | + '"\r\n' 303 | ) 304 | payload += "Content-Type: " + mimetype + "\r\n\r\n" 305 | payload += blob 306 | payload += "\r\n--" + boundary_key + "--" 307 | 308 | return self._api.request( 309 | "post", 310 | "/api/blobelements/d/" + did + "/w/" + wid, 311 | headers=req_headers, 312 | body=payload, 313 | ) 314 | 315 | def part_studio_stl(self, did, wid, eid): 316 | """ 317 | Exports STL export from a part studio 318 | 319 | Args: 320 | - did (str): Document ID 321 | - wid (str): Workspace ID 322 | - eid (str): Element ID 323 | 324 | Returns: 325 | - requests.Response: Onshape response data 326 | """ 327 | 328 | req_headers = {"Accept": "*/*"} 329 | return self._api.request( 330 | "get", 331 | "/api/partstudios/d/" + did + "/w/" + wid + "/e/" + eid + "/stl", 332 | headers=req_headers, 333 | ) 334 | 335 | def hash_partid(self, data): 336 | m = hashlib.sha1() 337 | m.update(data.encode("utf-8")) 338 | return m.hexdigest() 339 | 340 | def get_sketches(self, did, mid, eid, configuration): 341 | def invoke(): 342 | return self._api.request( 343 | "get", 344 | "/api/partstudios/d/" + did + "/m/" + mid + "/e/" + eid + "/sketches", 345 | query={"includeGeometry": "true", "configuration": configuration}, 346 | ) 347 | 348 | return json.loads( 349 | self.cache_get("sketches", (did, mid, eid, configuration), invoke) 350 | ) 351 | 352 | def get_parts(self, did, mid, eid, configuration): 353 | def invoke(): 354 | return self._api.request( 355 | "get", 356 | "/api/parts/d/" + did + "/m/" + mid + "/e/" + eid, 357 | query={"configuration": configuration}, 358 | ) 359 | 360 | return json.loads( 361 | self.cache_get("parts_list", (did, mid, eid, configuration), invoke) 362 | ) 363 | 364 | def find_new_partid( 365 | self, did, mid, eid, partid, configuration_before, configuration 366 | ): 367 | before = self.get_parts(did, mid, eid, configuration_before) 368 | name = None 369 | for entry in before: 370 | if entry["partId"] == partid: 371 | name = entry["name"] 372 | 373 | if name is not None: 374 | after = self.get_parts(did, mid, eid, configuration) 375 | for entry in after: 376 | if entry["name"] == name: 377 | return entry["partId"] 378 | else: 379 | print("OnShape ERROR: Can't find new partid for " + str(partid)) 380 | 381 | return partid 382 | 383 | def part_studio_stl_m(self, did, mid, eid, partid, configuration="default"): 384 | if self.useCollisionsConfigurations: 385 | configuration_before = configuration 386 | parts = configuration.split(";") 387 | partIdChanged = False 388 | result = "" 389 | for k, part in enumerate(parts): 390 | kv = part.split("=") 391 | if len(kv) == 2: 392 | if kv[0] == "collisions": 393 | kv[1] = "true" 394 | partIdChanged = True 395 | parts[k] = "=".join(kv) 396 | configuration = ";".join(parts) 397 | 398 | if partIdChanged: 399 | partid = self.find_new_partid( 400 | did, mid, eid, partid, configuration_before, configuration 401 | ) 402 | 403 | def invoke(): 404 | req_headers = {"Accept": "*/*"} 405 | return self._api.request( 406 | "get", 407 | "/api/parts/d/" 408 | + did 409 | + "/m/" 410 | + mid 411 | + "/e/" 412 | + eid 413 | + "/partid/" 414 | + escape_url(partid) 415 | + "/stl", 416 | query={ 417 | "mode": "binary", 418 | "units": "meter", 419 | "configuration": configuration, 420 | }, 421 | headers=req_headers, 422 | ) 423 | 424 | return self.cache_get( 425 | "part_stl", (did, mid, eid, self.hash_partid(partid), configuration), invoke 426 | ) 427 | 428 | def part_get_metadata(self, did, mid, eid, partid, configuration="default"): 429 | def invoke(): 430 | return self._api.request( 431 | "get", 432 | "/api/metadata/d/" 433 | + did 434 | + "/m/" 435 | + mid 436 | + "/e/" 437 | + eid 438 | + "/p/" 439 | + escape_url(partid), 440 | query={"configuration": configuration}, 441 | ) 442 | 443 | return json.loads( 444 | self.cache_get( 445 | "metadata", 446 | (did, mid, eid, self.hash_partid(partid), configuration), 447 | invoke, 448 | True, 449 | ) 450 | ) 451 | 452 | def part_mass_properties(self, did, mid, eid, partid, configuration="default"): 453 | def invoke(): 454 | return self._api.request( 455 | "get", 456 | "/api/parts/d/" 457 | + did 458 | + "/m/" 459 | + mid 460 | + "/e/" 461 | + eid 462 | + "/partid/" 463 | + escape_url(partid) 464 | + "/massproperties", 465 | query={ 466 | "configuration": configuration, 467 | "useMassPropertyOverrides": True, 468 | }, 469 | ) 470 | 471 | return json.loads( 472 | self.cache_get( 473 | "massproperties", 474 | (did, mid, eid, self.hash_partid(partid), configuration), 475 | invoke, 476 | True, 477 | ) 478 | ) 479 | -------------------------------------------------------------------------------- /onshape_urdf_exporter/load_robot.py: -------------------------------------------------------------------------------- 1 | from collections import defaultdict 2 | from sys import exit 3 | 4 | import numpy as np 5 | from colorama import Fore, Style 6 | 7 | from .config_file import Configuration 8 | from .onshape_api.client import Client 9 | 10 | config = Configuration.singleton 11 | if config is None: 12 | raise RuntimeError("Configuration has not been loaded yet") 13 | 14 | # OnShape API client 15 | workspaceId = None 16 | client = Client(logging=False) 17 | client.useCollisionsConfigurations = config.use_collisions_configurations 18 | 19 | # If a versionId is provided, it will be used, else the main workspace is retrieved 20 | if config.version_id != "": 21 | print( 22 | "\n" 23 | + Style.BRIGHT 24 | + "* Using configuration version ID " 25 | + config.version_id 26 | + " ..." 27 | + Style.RESET_ALL 28 | ) 29 | elif config.workspace_id != "": 30 | print( 31 | "\n" 32 | + Style.BRIGHT 33 | + "* Using configuration workspace ID " 34 | + config.workspace_id 35 | + " ..." 36 | + Style.RESET_ALL 37 | ) 38 | workspaceId = config.workspace_id 39 | else: 40 | print("\n" + Style.BRIGHT + "* Retrieving workspace ID ..." + Style.RESET_ALL) 41 | document = client.get_document(config.document_id).json() 42 | workspaceId = document["defaultWorkspace"]["id"] 43 | print(Fore.GREEN + "+ Using workspace id: " + workspaceId + Style.RESET_ALL) 44 | 45 | # Now, finding the assembly, according to given name in configuration, or else the first possible one 46 | print( 47 | "\n" 48 | + Style.BRIGHT 49 | + "* Retrieving elements in the document, searching for the assembly..." 50 | + Style.RESET_ALL 51 | ) 52 | if config.version_id != "": 53 | elements = client.list_elements(config.document_id, config.version_id, "v").json() 54 | else: 55 | elements = client.list_elements(config.document_id, workspaceId).json() 56 | assemblyId = None 57 | assemblyName = "" 58 | for element in elements: 59 | if element["type"] == "Assembly" and ( 60 | config.assembly_name is False or element["name"] == config.assembly_name 61 | ): 62 | print( 63 | Fore.GREEN 64 | + "+ Found assembly, id: " 65 | + element["id"] 66 | + ', name: "' 67 | + element["name"] 68 | + '"' 69 | + Style.RESET_ALL 70 | ) 71 | assemblyName = element["name"] 72 | assemblyId = element["id"] 73 | 74 | if assemblyId == None: 75 | print( 76 | Fore.RED + "ERROR: Unable to find assembly in this document" + Style.RESET_ALL 77 | ) 78 | exit(1) 79 | 80 | # Retrieving the assembly 81 | print( 82 | "\n" 83 | + Style.BRIGHT 84 | + '* Retrieving assembly "' 85 | + assemblyName 86 | + '" with id ' 87 | + assemblyId 88 | + Style.RESET_ALL 89 | ) 90 | if config.version_id != "": 91 | assembly = client.get_assembly( 92 | config.document_id, 93 | config.version_id, 94 | assemblyId, 95 | "v", 96 | configuration=config.configuration, 97 | ) 98 | else: 99 | assembly = client.get_assembly( 100 | config.document_id, 101 | workspaceId, 102 | assemblyId, 103 | configuration=config.configuration, 104 | ) 105 | 106 | root = assembly["rootAssembly"] 107 | 108 | # Finds a (leaf) instance given the full path, typically A B C where A and B would be subassemblies and C 109 | # the final part 110 | 111 | 112 | def findInstance(path, instances=None): 113 | global assembly 114 | 115 | if instances is None: 116 | instances = assembly["rootAssembly"]["instances"] 117 | 118 | for instance in instances: 119 | if instance["id"] == path[0]: 120 | if len(path) == 1: 121 | # If the length of remaining path is 1, the part is in the current assembly/subassembly 122 | return instance 123 | else: 124 | # Else, we need to find the matching sub assembly to find the proper part (recursively) 125 | d = instance["documentId"] 126 | m = instance["documentMicroversion"] 127 | e = instance["elementId"] 128 | for asm in assembly["subAssemblies"]: 129 | if ( 130 | asm["documentId"] == d 131 | and asm["documentMicroversion"] == m 132 | and asm["elementId"] == e 133 | ): 134 | return findInstance(path[1:], asm["instances"]) 135 | 136 | print(Fore.RED + "Could not find instance for " + str(path) + Style.RESET_ALL) 137 | 138 | 139 | # Collecting occurrences, the path is the assembly / sub assembly chain 140 | occurrences = {} 141 | for occurrence in root["occurrences"]: 142 | occurrence["assignation"] = None 143 | occurrence["instance"] = findInstance(occurrence["path"]) 144 | occurrence["transform"] = np.matrix(np.reshape(occurrence["transform"], (4, 4))) 145 | occurrence["linkName"] = None 146 | occurrences[tuple(occurrence["path"])] = occurrence 147 | 148 | # Gets an occurrence given its full path 149 | 150 | 151 | def getOccurrence(path): 152 | return occurrences[tuple(path)] 153 | 154 | 155 | # Assignations are pieces that will be in the same link. Note that this is only for top-level 156 | # item of the path (all sub assemblies and parts in assemblies are naturally in the same link as 157 | # the parent), but other parts that can be connected with mates in top assemblies are then assigned to 158 | # the link 159 | assignations = {} 160 | 161 | # Frames (mated with frame_ name) will be special links in the output file allowing to track some specific 162 | # manually identified frames 163 | frames = defaultdict(list) 164 | 165 | 166 | def assignParts(root, parent): 167 | assignations[root] = parent 168 | for occurrence in occurrences.values(): 169 | if occurrence["path"][0] == root: 170 | occurrence["assignation"] = parent 171 | 172 | 173 | from .features import get_limits 174 | from .features import init as features_init 175 | 176 | features_init(client, config, root, workspaceId, assemblyId) 177 | 178 | 179 | def get_T_part_mate(matedEntity: dict): 180 | T_part_mate = np.eye(4) 181 | T_part_mate[:3, :3] = np.stack( 182 | ( 183 | np.array(matedEntity["matedCS"]["xAxis"]), 184 | np.array(matedEntity["matedCS"]["yAxis"]), 185 | np.array(matedEntity["matedCS"]["zAxis"]), 186 | ) 187 | ).T 188 | T_part_mate[:3, 3] = matedEntity["matedCS"]["origin"] 189 | 190 | return T_part_mate 191 | 192 | 193 | # First, features are scanned to find the DOFs. Links that they connects are then tagged 194 | print( 195 | "\n" 196 | + Style.BRIGHT 197 | + "* Getting assembly features, scanning for DOFs..." 198 | + Style.RESET_ALL 199 | ) 200 | trunk = None 201 | relations = {} 202 | features = root["features"] 203 | for feature in features: 204 | if feature["featureType"] == "mateConnector": 205 | name = feature["featureData"]["name"] 206 | if name[0:5] == "link_": 207 | name = name[5:] 208 | occurrences[(feature["featureData"]["occurrence"][0],)]["linkName"] = name 209 | else: 210 | if feature["suppressed"]: 211 | continue 212 | 213 | data = feature["featureData"] 214 | 215 | if ( 216 | "matedEntities" not in data 217 | or len(data["matedEntities"]) != 2 218 | or len(data["matedEntities"][0]["matedOccurrence"]) == 0 219 | or len(data["matedEntities"][1]["matedOccurrence"]) == 0 220 | ): 221 | continue 222 | 223 | child = data["matedEntities"][0]["matedOccurrence"][0] 224 | parent = data["matedEntities"][1]["matedOccurrence"][0] 225 | 226 | if data["name"].startswith("closing_"): 227 | for k in 0, 1: 228 | matedEntity = data["matedEntities"][k] 229 | occurrence = matedEntity["matedOccurrence"][0] 230 | 231 | T_world_part = getOccurrence(matedEntity["matedOccurrence"])[ 232 | "transform" 233 | ] 234 | T_part_mate = get_T_part_mate(matedEntity) 235 | T_world_mate = T_world_part * T_part_mate 236 | 237 | frames[occurrence].append([f"{data['name']}_{k+1}", T_world_mate]) 238 | elif data["name"].startswith("dof_"): 239 | parts = data["name"].split("_") 240 | del parts[0] 241 | data["inverted"] = False 242 | if parts[-1] == "inv" or parts[-1] == "inverted": 243 | data["inverted"] = True 244 | del parts[-1] 245 | name = "_".join(parts) 246 | if name == "": 247 | print( 248 | Fore.RED 249 | + "ERROR: a DOF dones't have any name (\"" 250 | + data["name"] 251 | + '" should be "dof_...")' 252 | + Style.RESET_ALL 253 | ) 254 | exit() 255 | 256 | limits = None 257 | if data["mateType"] == "REVOLUTE" or data["mateType"] == "CYLINDRICAL": 258 | if "wheel" in parts or "continuous" in parts: 259 | jointType = "continuous" 260 | else: 261 | jointType = "revolute" 262 | 263 | if not config.ignore_limits: 264 | limits = get_limits(jointType, data["name"]) 265 | elif data["mateType"] == "SLIDER": 266 | jointType = "prismatic" 267 | if not config.ignore_limits: 268 | limits = get_limits(jointType, data["name"]) 269 | elif data["mateType"] == "FASTENED": 270 | jointType = "fixed" 271 | else: 272 | print( 273 | Fore.RED 274 | + 'ERROR: "' 275 | + name 276 | + '" is declared as a DOF but the mate type is ' 277 | + data["mateType"] 278 | + "" 279 | ) 280 | print( 281 | " Only REVOLUTE, CYLINDRICAL, SLIDER and FASTENED are supported" 282 | + Style.RESET_ALL 283 | ) 284 | exit(1) 285 | 286 | # We compute the axis in the world frame 287 | matedEntity = data["matedEntities"][0] 288 | T_world_part = getOccurrence(matedEntity["matedOccurrence"])["transform"] 289 | 290 | # jointToPart is the (rotation only) matrix from joint to the part 291 | # it is attached to 292 | T_part_mate = get_T_part_mate(matedEntity) 293 | 294 | if data["inverted"]: 295 | if limits is not None: 296 | limits = (-limits[1], -limits[0]) 297 | 298 | # Flipping the joint around X axis 299 | flip = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]) 300 | T_part_mate[:3, :3] = T_part_mate[:3, :3] @ flip 301 | 302 | T_world_mate = T_world_part * T_part_mate 303 | 304 | limitsStr = "" 305 | if limits is not None: 306 | limitsStr = ( 307 | "[" 308 | + str(round(limits[0], 3)) 309 | + ": " 310 | + str(round(limits[1], 3)) 311 | + "]" 312 | ) 313 | print( 314 | Fore.GREEN 315 | + "+ Found DOF: " 316 | + name 317 | + " " 318 | + Style.DIM 319 | + "(" 320 | + jointType 321 | + ")" 322 | + limitsStr 323 | + Style.RESET_ALL 324 | ) 325 | 326 | if child in relations: 327 | print(Fore.RED) 328 | print( 329 | "Error, the relation " 330 | + name 331 | + " is connected a child that is already connected" 332 | ) 333 | print("Be sure you ordered properly your relations, see:") 334 | print( 335 | "https://onshape-to-robot.readthedocs.io/en/latest/design.html#specifying-degrees-of-freedom" 336 | ) 337 | print(Style.RESET_ALL) 338 | exit() 339 | 340 | relations[child] = { 341 | "parent": parent, 342 | "worldAxisFrame": T_world_mate, 343 | "zAxis": np.array([0, 0, 1]), 344 | "name": name, 345 | "type": jointType, 346 | "limits": limits, 347 | } 348 | 349 | assignParts(child, child) 350 | assignParts(parent, parent) 351 | 352 | print( 353 | Fore.GREEN 354 | + Style.BRIGHT 355 | + "* Found total " 356 | + str(len(relations)) 357 | + " DOFs" 358 | + Style.RESET_ALL 359 | ) 360 | 361 | # If we have no DOF 362 | if len(relations) == 0: 363 | trunk = root["instances"][0]["id"] 364 | assignParts(trunk, trunk) 365 | 366 | 367 | def connectParts(child, parent): 368 | assignParts(child, parent) 369 | 370 | 371 | # Spreading parts assignations, this parts mainly does two things: 372 | # 1. Finds the parts of the top level assembly that are not directly in a sub assembly and try to assign them 373 | # to an existing link that was identified before 374 | # 2. Among those parts, finds the ones that are frames (connected with a frame_* connector) 375 | changed = True 376 | while changed: 377 | changed = False 378 | for feature in features: 379 | if feature["featureType"] != "mate" or feature["suppressed"]: 380 | continue 381 | 382 | data = feature["featureData"] 383 | 384 | if ( 385 | len(data["matedEntities"]) != 2 386 | or len(data["matedEntities"][0]["matedOccurrence"]) == 0 387 | or len(data["matedEntities"][1]["matedOccurrence"]) == 0 388 | ): 389 | continue 390 | 391 | occurrenceA = data["matedEntities"][0]["matedOccurrence"][0] 392 | occurrenceB = data["matedEntities"][1]["matedOccurrence"][0] 393 | 394 | if (occurrenceA not in assignations) != (occurrenceB not in assignations): 395 | if data["name"].startswith("frame_"): 396 | # In case of a constraint naemd "frame_", we add it as a frame, we draw it if drawFrames is True 397 | name = "_".join(data["name"].split("_")[1:]) 398 | if occurrenceA in assignations: 399 | frames[occurrenceA].append( 400 | [name, data["matedEntities"][1]["matedOccurrence"]] 401 | ) 402 | assignParts( 403 | occurrenceB, 404 | {True: assignations[occurrenceA], False: "frame"}[ 405 | config.draw_frames 406 | ], 407 | ) 408 | else: 409 | frames[occurrenceB].append( 410 | [name, data["matedEntities"][0]["matedOccurrence"]] 411 | ) 412 | assignParts( 413 | occurrenceA, 414 | {True: assignations[occurrenceB], False: "frame"}[ 415 | config.draw_frames 416 | ], 417 | ) 418 | changed = True 419 | else: 420 | if occurrenceA in assignations: 421 | connectParts(occurrenceB, assignations[occurrenceA]) 422 | changed = True 423 | else: 424 | connectParts(occurrenceA, assignations[occurrenceB]) 425 | changed = True 426 | 427 | # Building and checking robot tree, here we: 428 | # 1. Search for robot trunk (which will be the top-level link) 429 | # 2. Scan for orphaned parts (if you add something floating with no mate to anything) 430 | # that are then assigned to trunk by default 431 | # 3. Collect all the pieces of the robot tree 432 | print("\n" + Style.BRIGHT + "* Building robot tree" + Style.RESET_ALL) 433 | 434 | for childId in relations: 435 | entry = relations[childId] 436 | if entry["parent"] not in relations: 437 | trunk = entry["parent"] 438 | break 439 | trunkOccurrence = getOccurrence([trunk]) 440 | print( 441 | Style.BRIGHT + "* Trunk is " + trunkOccurrence["instance"]["name"] + Style.RESET_ALL 442 | ) 443 | 444 | for occurrence in occurrences.values(): 445 | if occurrence["assignation"] is None: 446 | print( 447 | Fore.YELLOW 448 | + "WARNING: part (" 449 | + occurrence["instance"]["name"] 450 | + ") has no assignation, connecting it with trunk" 451 | + Style.RESET_ALL 452 | ) 453 | child = occurrence["path"][0] 454 | connectParts(child, trunk) 455 | 456 | # If a sub-assembly is suppressed, we also mark as suppressed the parts in this sub-assembly 457 | for occurrence in occurrences.values(): 458 | if not occurrence["instance"].get("suppressed", True): 459 | for k in range(len(occurrence["path"]) - 1): 460 | upper_path = tuple(occurrence["path"][0 : k + 1]) 461 | if ( 462 | upper_path in occurrences 463 | and occurrences[upper_path]["instance"]["suppressed"] 464 | ): 465 | occurrence["instance"]["suppressed"] = True 466 | 467 | 468 | def collect(id): 469 | part = {} 470 | part["id"] = id 471 | part["children"] = [] 472 | for childId in relations: 473 | entry = relations[childId] 474 | if entry["parent"] == id: 475 | child = collect(childId) 476 | child["axis_frame"] = entry["worldAxisFrame"] 477 | child["z_axis"] = entry["zAxis"] 478 | child["dof_name"] = entry["name"] 479 | child["jointType"] = entry["type"] 480 | child["jointLimits"] = entry["limits"] 481 | part["children"].append(child) 482 | return part 483 | 484 | 485 | tree = collect(trunk) 486 | --------------------------------------------------------------------------------