[ { "path": ".coveragerc", "content": "[run]\nbranch = True\nsource = pytransform3d\nomit =\n */setup.py\n pytransform3d/plot_utils.py\n pytransform3d/editor.py\n pytransform3d/visualizer.py\n\n[report]\nomit =\n pytransform3d/plot_utils/*.py\n pytransform3d/editor.py\n pytransform3d/visualizer/*.py\n pytransform3d/*/_plot.py\n pytransform3d/test/*.py\n pytransform3d/*/test/*.py\nexclude_lines =\n pragma: no cover\n except ImportError:\n" }, { "path": ".github/workflows/python-package.yml", "content": "# This workflow will install Python dependencies, run tests and lint with a variety of Python versions\n# For more information see: https://help.github.com/actions/language-and-framework-guides/using-python-with-github-actions\n\nname: Python package\n\non:\n push:\n branches-ignore:\n - gh-pages\n\njobs:\n build:\n\n runs-on: ubuntu-latest\n strategy:\n matrix:\n python-version: [\"3.9\", \"3.10\", \"3.11\", \"3.12\", \"3.13\"]\n\n steps:\n - uses: actions/checkout@v2\n - name: Set up Python ${{ matrix.python-version }}\n uses: actions/setup-python@v2\n with:\n python-version: ${{ matrix.python-version }}\n - name: Install dependencies\n run: |\n sudo apt install graphviz\n python -m pip install --upgrade pip\n python -m pip install flake8\n python -m pip install -e .[test] pydot trimesh\n - name: Lint with flake8\n run: |\n # stop the build if there are Python syntax errors or undefined names\n flake8 . --count --select=E9,F63,F7,F82 --show-source --statistics\n # exit-zero treats all errors as warnings. The GitHub editor is 127 chars wide\n flake8 . --count --exit-zero --max-complexity=10 --max-line-length=127 --statistics\n - name: Test\n run: |\n MPLBACKEND=Agg pytest\n - name: Codecov\n uses: codecov/codecov-action@v1.3.2\n" }, { "path": ".gitignore", "content": ".idea/\n.coverage\n.venv/\ncover/\n*~\n*.pyc\n*.vim\n.ipynb_checkpoints/\nvenv/\nbuild/\ndoc/examples\ndoc/source/_apidoc\ndoc/source/_auto_examples\n.vscode\n*.egg-info/\n" }, { "path": "CITATION.cff", "content": "cff-version: 1.2.0\nmessage: \"If you use this software, please cite it as below.\"\nauthors:\n- family-names: Fabisch\n given-names: Alexander\ntitle: \"pytransform3d\"\nurl: https://github.com/dfki-ric/pytransform3d\ndoi: 10.5281/zenodo.2553450\npreferred-citation:\n type: article\n authors:\n - family-names: Fabisch\n given-names: Alexander\n orcid: https://orcid.org/0000-0003-2824-7956\n title: \"pytransform3d: 3D Transformations for Python\"\n doi: 10.21105/joss.01159\n journal: Journal of Open Source Software\n start: 1159\n issue: 33\n volume: 4\n month: 1\n year: 2019\n" }, { "path": "CONTRIBUTING.md", "content": "# Contributing\n\nEveryone is welcome to contribute.\n\nThere are several ways to contribute to pytransform3d: you could\n\n* send a bug report to the\n [bug tracker](http://github.com/dfki-ric/pytransform3d/issues)\n* work on one of the reported issues\n* write documentation\n* add a new feature\n* add tests\n* add an example\n\n## How to Contribute Code\n\nThis text is shamelessly copied from\n[scikit-learn's](https://scikit-learn.org/stable/developers/contributing.html)\ncontribution guidelines.\n\nThe preferred way to contribute to pytransform3d is to fork the\n[repository](http://github.com/dfki-ric/pytransform3d/) on GitHub,\nthen submit a \"pull request\" (PR):\n\n1. [Create an account](https://github.com/signup/free) on\n GitHub if you do not already have one.\n\n2. Fork the [project repository](http://github.com/dfki-ric/pytransform3d):\n click on the 'Fork' button near the top of the page. This creates a copy of\n the code under your account on the GitHub server.\n\n3. Clone this copy to your local disk:\n\n $ git clone git@github.com:YourLogin/pytransform3d.git\n\n4. Create a branch to hold your changes:\n\n $ git checkout -b my-feature\n\n and start making changes. Never work in the `main` branch!\n\n5. Work on this copy, on your computer, using Git to do the version\n control. When you're done editing, do:\n\n $ git add modified_files\n $ git commit\n\n to record your changes in Git, then push them to GitHub with:\n\n $ git push -u origin my-feature\n\nFinally, go to the web page of the your fork of the pytransform3d repository,\nand click 'Pull request' to send your changes to the maintainer for review.\nMake sure that your target branch is 'develop'.\n\nIn the above setup, your `origin` remote repository points to\nYourLogin/pytransform3d.git. If you wish to fetch/merge from the main\nrepository instead of your forked one, you will need to add another remote\nto use instead of `origin`. If we choose the name `upstream` for it, the\ncommand will be:\n\n $ git remote add upstream https://github.com/dfki-ric/pytransform3d.git\n\n(If any of the above seems like magic to you, then look up the\n[Git documentation](http://git-scm.com/documentation) on the web.)\n\n## Requirements for New Features\n\nAdding a new feature to pytransform3d requires a few other changes:\n\n* New classes or functions that are part of the public interface must be\n documented. We use [NumPy's conventions for docstrings](https://github.com/numpy/numpy/blob/master/doc/HOWTO_DOCUMENT.rst.txt).\n* An entry to the API documentation must be added [here](https://dfki-ric.github.io/pytransform3d/api.html).\n* Consider writing a simple example script.\n* Tests: Unit tests for new features are mandatory. They should cover all\n branches. Exceptions are plotting functions, debug outputs, etc. These\n are usually hard to test and are not a fundamental part of the library.\n\n## Merge Policy\n\nUsually it is not possible to push directly to the develop or main branch for\nanyone. Only tiny changes, urgent bugfixes, and maintenance commits can be\npushed directly to the develop branch by the maintainer without a review.\n\"Tiny\" means backwards compatibility is mandatory and all tests must succeed.\nNo new feature must be added.\n\nDevelopers have to submit pull requests. Those will be reviewed and merged by\na maintainer. New features must be documented and tested. Breaking changes must\nbe discussed and announced in advance with deprecation warnings.\n\n## Versioning\n\nSemantic versioning is used, that is, the major version number will be\nincremented when the API changes in a backwards incompatible way, the\nminor version will be incremented when new functionality is added in a\nbackwards compatible manner.\n" }, { "path": "LICENSE", "content": "BSD 3-Clause License\n\nCopyright 2014-2017 Alexander Fabisch (Robotics Research Group, University of Bremen),\n2017-2025 Alexander Fabisch (DFKI GmbH, Robotics Innovation Center),\nand pytransform3d contributors\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n1. Redistributions of source code must retain the above copyright notice, this\n list of conditions and the following disclaimer.\n\n2. Redistributions in binary form must reproduce the above copyright notice,\n this list of conditions and the following disclaimer in the documentation\n and/or other materials provided with the distribution.\n\n3. Neither the name of the copyright holder nor the names of its\n contributors may be used to endorse or promote products derived from\n this software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\nAND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\nDISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE\nFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\nDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR\nSERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\nCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,\nOR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n" }, { "path": "MANIFEST.in", "content": "include pytransform3d/py.typed\ninclude pytransform3d/*.pyi\ninclude pytransform3d/*/*.pyi\n" }, { "path": "README.md", "content": "\n\n[![codecov](https://codecov.io/gh/dfki-ric/pytransform3d/branch/main/graph/badge.svg?token=jB10RM3Ujj)](https://codecov.io/gh/dfki-ric/pytransform3d)\n[![Paper DOI](http://joss.theoj.org/papers/10.21105/joss.01159/status.svg)](https://doi.org/10.21105/joss.01159)\n[![Release DOI](https://zenodo.org/badge/DOI/10.5281/zenodo.2553450.svg)](https://doi.org/10.5281/zenodo.2553450)\n\n# pytransform3d\n\nA Python library for transformations in three dimensions.\n\npytransform3d offers...\n\n* operations like concatenation and inversion for most common representations\n of rotation (orientation) and translation (position)\n* conversions between those representations\n* clear documentation of transformation conventions\n* tight coupling with matplotlib to quickly visualize (or animate)\n transformations\n* the TransformManager which manages complex chains of transformations\n (with export to graph visualization as PNG, additionally requires pydot)\n* the TransformEditor which allows to modify transformations graphically\n (additionally requires PyQt4/5)\n* the UrdfTransformManager which is able to load transformations from\n [URDF](https://wiki.ros.org/urdf) files (additionally requires lxml)\n* a matplotlib-like interface to Open3D's visualizer to display and animate\n geometries and transformations (additionally requires Open3D)\n\npytransform3d is used in various domains, for example:\n\n* specifying motions of a robot\n* learning robot movements from human demonstration\n* sensor fusion for human pose estimation\n* collision detection for robots\n\nThe API documentation can be found\n[here](https://dfki-ric.github.io/pytransform3d/).\n\nI gave a talk at EuroSciPy 2023 about pytransform3d. Slides are available\n[here](https://github.com/AlexanderFabisch/pytransform3d_euroscipy2023/).\n\nIf you need similar features in JAX (on GPU, vectorized, differentiable),\nhave a look at the experimental library\n[jaxtransform3d](https://github.com/AlexanderFabisch/jaxtransform3d/).\n\n## Installation\n\nUse pip to install the package from PyPI:\n\n```bash\npip install 'pytransform3d[all]'\n```\n\nor conda:\n\n```bash\nconda install -c conda-forge pytransform3d\n```\n\nTake a look at the\n[installation instructions](https://dfki-ric.github.io/pytransform3d/install.html)\nin the documentation for more details.\n\n## Gallery\n\nThe following plots and visualizations have been generated with pytransform3d.\nThe code for most examples can be found in\n[the documentation](https://dfki-ric.github.io/pytransform3d/_auto_examples/index.html).\n\nLeft: [Nao robot](https://www.softbankrobotics.com/emea/en/nao) with URDF\nfrom [Bullet3](https://github.com/bulletphysics/bullet3).\nRight: [Kuka iiwa](https://www.kuka.com/en-de/products/robot-systems/industrial-robots/lbr-iiwa).\nThe animation is based on pytransform3d's visualization interface to\n[Open3D](http://www.open3d.org/).\n\n\n\nVisualizations based on [Open3D](http://www.open3d.org/).\n\n\n\nVarious plots based on Matplotlib.\n\n\n\nTransformation editor based on Qt.\n\n\n\n## Example\n\nThis is just one simple example. You can find more examples in the subfolder\n`examples/`.\n\n```python\nimport numpy as np\nimport matplotlib.pyplot as plt\nfrom pytransform3d import rotations as pr\nfrom pytransform3d import transformations as pt\nfrom pytransform3d.transform_manager import TransformManager\n\n\nrng = np.random.default_rng(0)\n\nee2robot = pt.transform_from_pq(\n np.hstack((np.array([0.4, -0.3, 0.5]),\n pr.random_quaternion(rng))))\ncam2robot = pt.transform_from_pq(\n np.hstack((np.array([0.0, 0.0, 0.8]), pr.q_id)))\nobject2cam = pt.transform_from(\n pr.active_matrix_from_intrinsic_euler_xyz(np.array([0.0, 0.0, -0.5])),\n np.array([0.5, 0.1, 0.1]))\n\ntm = TransformManager()\ntm.add_transform(\"end-effector\", \"robot\", ee2robot)\ntm.add_transform(\"camera\", \"robot\", cam2robot)\ntm.add_transform(\"object\", \"camera\", object2cam)\n\nee2object = tm.get_transform(\"end-effector\", \"object\")\n\nax = tm.plot_frames_in(\"robot\", s=0.1)\nax.set_xlim((-0.25, 0.75))\nax.set_ylim((-0.5, 0.5))\nax.set_zlim((0.0, 1.0))\nplt.show()\n```\n\n![output](https://dfki-ric.github.io/pytransform3d/_images/sphx_glr_plot_transform_manager_001.png)\n\n## Documentation\n\nThe API documentation can be found\n[here](https://dfki-ric.github.io/pytransform3d/).\n\nThe documentation can be found in the directory `doc`.\nTo build the documentation, run e.g. (on linux):\n\n```bash\ncd doc\nmake html\n```\n\nThe HTML documentation is now located at `doc/build/html/index.html`.\nExecute the following command in the main folder of the repository\nto install the dependencies:\n\n```bash\npip install -e '.[doc]'\n```\n\n## Tests\n\nYou can use pytest to run the tests of this project in the root directory:\n\n```bash\npytest\n```\n\nA coverage report will be located at `htmlcov/index.html`.\nNote that you have to install `pytest` to run the tests and `pytest-cov` to\nobtain the code coverage report.\n\n## Contributing\n\nIf you wish to report bugs, please use the\n[issue tracker](https://github.com/dfki-ric/pytransform3d/issues) at\nGithub. If you would like to contribute to pytransform3d, just open an issue\nor a [pull request](https://github.com/dfki-ric/pytransform3d/pulls).\nThe target branch for pull requests is the develop branch.\nThe development branch will be merged to main for new releases.\nIf you have questions about the software, you should ask them in the\n[discussion section](https://github.com/dfki-ric/pytransform3d/discussions).\n\nThe recommended workflow to add a new feature, add documentation, or fix a bug\nis the following:\n\n* Push your changes to a branch (e.g. `feature/x`, `doc/y`, or `fix/z`) of your\n fork of the pytransform3d repository.\n* Open a pull request to the latest development branch. There is usually an\n open merge request from the latest development branch to the main branch.\n* When the latest development branch is merged to the main branch, a new\n release will be made.\n\nNote that there is a\n[checklist](https://github.com/dfki-ric/pytransform3d/wiki#checklist-for-new-features)\nfor new features.\n\nIt is forbidden to directly push to the main branch. Each new version\nhas its own development branch from which a pull request will be opened to the\nmain branch. Only the maintainer of the software is allowed to merge a\ndevelopment branch to the main branch.\n\n## License\n\nThe library is distributed under the\n[3-Clause BSD license](https://github.com/dfki-ric/pytransform3d/blob/main/LICENSE).\n\n## Citation\n\nIf you use pytransform3d for a scientific publication, I would appreciate\ncitation of the following paper:\n\nFabisch, A. (2019). pytransform3d: 3D Transformations for Python.\nJournal of Open Source Software, 4(33), 1159,\n[![Paper DOI](http://joss.theoj.org/papers/10.21105/joss.01159/status.svg)](https://doi.org/10.21105/joss.01159)\n\nBibtex entry:\n\n```bibtex\n@article{Fabisch2019,\n doi = {10.21105/joss.01159},\n url = {https://doi.org/10.21105/joss.01159},\n year = {2019},\n publisher = {The Open Journal},\n volume = {4},\n number = {33},\n pages = {1159},\n author = {Alexander Fabisch},\n title = {pytransform3d: 3D Transformations for Python},\n journal = {Journal of Open Source Software}\n}\n```\n" }, { "path": "doc/Makefile", "content": "# Makefile for Sphinx documentation\n#\n\n# You can set these variables from the command line.\nSPHINXOPTS =\nSPHINXBUILD = sphinx-build\nPAPER =\nBUILDDIR = build\n\n# User-friendly check for sphinx-build\nifeq ($(shell which $(SPHINXBUILD) >/dev/null 2>&1; echo $$?), 1)\n$(error The '$(SPHINXBUILD)' command was not found. Make sure you have Sphinx installed, then set the SPHINXBUILD environment variable to point to the full path of the '$(SPHINXBUILD)' executable. Alternatively you can add the directory with the executable to your PATH. If you don't have Sphinx installed, grab it from http://sphinx-doc.org/)\nendif\n\n# Internal variables.\nPAPEROPT_a4 = -D latex_paper_size=a4\nPAPEROPT_letter = -D latex_paper_size=letter\nALLSPHINXOPTS = -d $(BUILDDIR)/doctrees $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) source\n# the i18n builder cannot share the environment and doctrees with the others\nI18NSPHINXOPTS = $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) source\n\n.PHONY: help clean html dirhtml singlehtml pickle json htmlhelp qthelp devhelp epub latex latexpdf text man changes linkcheck doctest gettext\n\nhelp:\n\t@echo \"Please use \\`make ' where is one of\"\n\t@echo \" html to make standalone HTML files\"\n\t@echo \" dirhtml to make HTML files named index.html in directories\"\n\t@echo \" singlehtml to make a single large HTML file\"\n\t@echo \" pickle to make pickle files\"\n\t@echo \" json to make JSON files\"\n\t@echo \" htmlhelp to make HTML files and a HTML help project\"\n\t@echo \" qthelp to make HTML files and a qthelp project\"\n\t@echo \" devhelp to make HTML files and a Devhelp project\"\n\t@echo \" epub to make an epub\"\n\t@echo \" latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter\"\n\t@echo \" latexpdf to make LaTeX files and run them through pdflatex\"\n\t@echo \" latexpdfja to make LaTeX files and run them through platex/dvipdfmx\"\n\t@echo \" text to make text files\"\n\t@echo \" man to make manual pages\"\n\t@echo \" texinfo to make Texinfo files\"\n\t@echo \" info to make Texinfo files and run them through makeinfo\"\n\t@echo \" gettext to make PO message catalogs\"\n\t@echo \" changes to make an overview of all changed/added/deprecated items\"\n\t@echo \" xml to make Docutils-native XML files\"\n\t@echo \" pseudoxml to make pseudoxml-XML files for display purposes\"\n\t@echo \" linkcheck to check all external links for integrity\"\n\t@echo \" doctest to run all doctests embedded in the documentation (if enabled)\"\n\nclean:\n\trm -rf $(BUILDDIR)/*\n\trm -rf source/_apidoc\n\trm -rf source/_auto_examples\n\nhtml:\n\t$(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html\n\t@echo\n\t@echo \"Build finished. The HTML pages are in $(BUILDDIR)/html.\"\n\ndirhtml:\n\t$(SPHINXBUILD) -b dirhtml $(ALLSPHINXOPTS) $(BUILDDIR)/dirhtml\n\t@echo\n\t@echo \"Build finished. The HTML pages are in $(BUILDDIR)/dirhtml.\"\n\nsinglehtml:\n\t$(SPHINXBUILD) -b singlehtml $(ALLSPHINXOPTS) $(BUILDDIR)/singlehtml\n\t@echo\n\t@echo \"Build finished. The HTML page is in $(BUILDDIR)/singlehtml.\"\n\npickle:\n\t$(SPHINXBUILD) -b pickle $(ALLSPHINXOPTS) $(BUILDDIR)/pickle\n\t@echo\n\t@echo \"Build finished; now you can process the pickle files.\"\n\njson:\n\t$(SPHINXBUILD) -b json $(ALLSPHINXOPTS) $(BUILDDIR)/json\n\t@echo\n\t@echo \"Build finished; now you can process the JSON files.\"\n\nhtmlhelp:\n\t$(SPHINXBUILD) -b htmlhelp $(ALLSPHINXOPTS) $(BUILDDIR)/htmlhelp\n\t@echo\n\t@echo \"Build finished; now you can run HTML Help Workshop with the\" \\\n\t \".hhp project file in $(BUILDDIR)/htmlhelp.\"\n\nqthelp:\n\t$(SPHINXBUILD) -b qthelp $(ALLSPHINXOPTS) $(BUILDDIR)/qthelp\n\t@echo\n\t@echo \"Build finished; now you can run \"qcollectiongenerator\" with the\" \\\n\t \".qhcp project file in $(BUILDDIR)/qthelp, like this:\"\n\t@echo \"# qcollectiongenerator $(BUILDDIR)/qthelp/pytransform3d.qhcp\"\n\t@echo \"To view the help file:\"\n\t@echo \"# assistant -collectionFile $(BUILDDIR)/qthelp/pytransform3d.qhc\"\n\ndevhelp:\n\t$(SPHINXBUILD) -b devhelp $(ALLSPHINXOPTS) $(BUILDDIR)/devhelp\n\t@echo\n\t@echo \"Build finished.\"\n\t@echo \"To view the help file:\"\n\t@echo \"# mkdir -p $$HOME/.local/share/devhelp/pytransform3d\"\n\t@echo \"# ln -s $(BUILDDIR)/devhelp $$HOME/.local/share/devhelp/pytransform3d\"\n\t@echo \"# devhelp\"\n\nepub:\n\t$(SPHINXBUILD) -b epub $(ALLSPHINXOPTS) $(BUILDDIR)/epub\n\t@echo\n\t@echo \"Build finished. The epub file is in $(BUILDDIR)/epub.\"\n\nlatex:\n\t$(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex\n\t@echo\n\t@echo \"Build finished; the LaTeX files are in $(BUILDDIR)/latex.\"\n\t@echo \"Run \\`make' in that directory to run these through (pdf)latex\" \\\n\t \"(use \\`make latexpdf' here to do that automatically).\"\n\nlatexpdf:\n\t$(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex\n\t@echo \"Running LaTeX files through pdflatex...\"\n\t$(MAKE) -C $(BUILDDIR)/latex all-pdf\n\t@echo \"pdflatex finished; the PDF files are in $(BUILDDIR)/latex.\"\n\nlatexpdfja:\n\t$(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex\n\t@echo \"Running LaTeX files through platex and dvipdfmx...\"\n\t$(MAKE) -C $(BUILDDIR)/latex all-pdf-ja\n\t@echo \"pdflatex finished; the PDF files are in $(BUILDDIR)/latex.\"\n\ntext:\n\t$(SPHINXBUILD) -b text $(ALLSPHINXOPTS) $(BUILDDIR)/text\n\t@echo\n\t@echo \"Build finished. The text files are in $(BUILDDIR)/text.\"\n\nman:\n\t$(SPHINXBUILD) -b man $(ALLSPHINXOPTS) $(BUILDDIR)/man\n\t@echo\n\t@echo \"Build finished. The manual pages are in $(BUILDDIR)/man.\"\n\ntexinfo:\n\t$(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo\n\t@echo\n\t@echo \"Build finished. The Texinfo files are in $(BUILDDIR)/texinfo.\"\n\t@echo \"Run \\`make' in that directory to run these through makeinfo\" \\\n\t \"(use \\`make info' here to do that automatically).\"\n\ninfo:\n\t$(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo\n\t@echo \"Running Texinfo files through makeinfo...\"\n\tmake -C $(BUILDDIR)/texinfo info\n\t@echo \"makeinfo finished; the Info files are in $(BUILDDIR)/texinfo.\"\n\ngettext:\n\t$(SPHINXBUILD) -b gettext $(I18NSPHINXOPTS) $(BUILDDIR)/locale\n\t@echo\n\t@echo \"Build finished. The message catalogs are in $(BUILDDIR)/locale.\"\n\nchanges:\n\t$(SPHINXBUILD) -b changes $(ALLSPHINXOPTS) $(BUILDDIR)/changes\n\t@echo\n\t@echo \"The overview file is in $(BUILDDIR)/changes.\"\n\nlinkcheck:\n\t$(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck\n\t@echo\n\t@echo \"Link check complete; look for any errors in the above output \" \\\n\t \"or in $(BUILDDIR)/linkcheck/output.txt.\"\n\ndoctest:\n\t$(SPHINXBUILD) -b doctest $(ALLSPHINXOPTS) $(BUILDDIR)/doctest\n\t@echo \"Testing of doctests in the sources finished, look at the \" \\\n\t \"results in $(BUILDDIR)/doctest/output.txt.\"\n\nxml:\n\t$(SPHINXBUILD) -b xml $(ALLSPHINXOPTS) $(BUILDDIR)/xml\n\t@echo\n\t@echo \"Build finished. The XML files are in $(BUILDDIR)/xml.\"\n\npseudoxml:\n\t$(SPHINXBUILD) -b pseudoxml $(ALLSPHINXOPTS) $(BUILDDIR)/pseudoxml\n\t@echo\n\t@echo \"Build finished. The pseudo-XML files are in $(BUILDDIR)/pseudoxml.\"\n" }, { "path": "doc/make.bat", "content": "@ECHO OFF\r\n\r\nREM Command file for Sphinx documentation\r\n\r\nif \"%SPHINXBUILD%\" == \"\" (\r\n\tset SPHINXBUILD=sphinx-build\r\n)\r\nset BUILDDIR=build\r\nset ALLSPHINXOPTS=-d %BUILDDIR%/doctrees %SPHINXOPTS% source\r\nset I18NSPHINXOPTS=%SPHINXOPTS% source\r\nif NOT \"%PAPER%\" == \"\" (\r\n\tset ALLSPHINXOPTS=-D latex_paper_size=%PAPER% %ALLSPHINXOPTS%\r\n\tset I18NSPHINXOPTS=-D latex_paper_size=%PAPER% %I18NSPHINXOPTS%\r\n)\r\n\r\nif \"%1\" == \"\" goto help\r\n\r\nif \"%1\" == \"help\" (\r\n\t:help\r\n\techo.Please use `make ^` where ^ is one of\r\n\techo. html to make standalone HTML files\r\n\techo. dirhtml to make HTML files named index.html in directories\r\n\techo. singlehtml to make a single large HTML file\r\n\techo. pickle to make pickle files\r\n\techo. json to make JSON files\r\n\techo. htmlhelp to make HTML files and a HTML help project\r\n\techo. qthelp to make HTML files and a qthelp project\r\n\techo. devhelp to make HTML files and a Devhelp project\r\n\techo. epub to make an epub\r\n\techo. latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter\r\n\techo. text to make text files\r\n\techo. man to make manual pages\r\n\techo. texinfo to make Texinfo files\r\n\techo. gettext to make PO message catalogs\r\n\techo. changes to make an overview over all changed/added/deprecated items\r\n\techo. xml to make Docutils-native XML files\r\n\techo. pseudoxml to make pseudoxml-XML files for display purposes\r\n\techo. linkcheck to check all external links for integrity\r\n\techo. doctest to run all doctests embedded in the documentation if enabled\r\n\tgoto end\r\n)\r\n\r\nif \"%1\" == \"clean\" (\r\n\tfor /d %%i in (%BUILDDIR%\\*) do rmdir /q /s %%i\r\n\tdel /q /s %BUILDDIR%\\*\r\n\tgoto end\r\n)\r\n\r\n\r\n%SPHINXBUILD% 2> nul\r\nif errorlevel 9009 (\r\n\techo.\r\n\techo.The 'sphinx-build' command was not found. Make sure you have Sphinx\r\n\techo.installed, then set the SPHINXBUILD environment variable to point\r\n\techo.to the full path of the 'sphinx-build' executable. Alternatively you\r\n\techo.may add the Sphinx directory to PATH.\r\n\techo.\r\n\techo.If you don't have Sphinx installed, grab it from\r\n\techo.http://sphinx-doc.org/\r\n\texit /b 1\r\n)\r\n\r\nif \"%1\" == \"html\" (\r\n\t%SPHINXBUILD% -b html %ALLSPHINXOPTS% %BUILDDIR%/html\r\n\tif errorlevel 1 exit /b 1\r\n\techo.\r\n\techo.Build finished. The HTML pages are in %BUILDDIR%/html.\r\n\tgoto end\r\n)\r\n\r\nif \"%1\" == \"dirhtml\" (\r\n\t%SPHINXBUILD% -b dirhtml %ALLSPHINXOPTS% %BUILDDIR%/dirhtml\r\n\tif errorlevel 1 exit /b 1\r\n\techo.\r\n\techo.Build finished. The HTML pages are in %BUILDDIR%/dirhtml.\r\n\tgoto end\r\n)\r\n\r\nif \"%1\" == \"singlehtml\" (\r\n\t%SPHINXBUILD% -b singlehtml %ALLSPHINXOPTS% %BUILDDIR%/singlehtml\r\n\tif errorlevel 1 exit /b 1\r\n\techo.\r\n\techo.Build finished. The HTML pages are in %BUILDDIR%/singlehtml.\r\n\tgoto end\r\n)\r\n\r\nif \"%1\" == \"pickle\" (\r\n\t%SPHINXBUILD% -b pickle %ALLSPHINXOPTS% %BUILDDIR%/pickle\r\n\tif errorlevel 1 exit /b 1\r\n\techo.\r\n\techo.Build finished; now you can process the pickle files.\r\n\tgoto end\r\n)\r\n\r\nif \"%1\" == \"json\" (\r\n\t%SPHINXBUILD% -b json %ALLSPHINXOPTS% %BUILDDIR%/json\r\n\tif errorlevel 1 exit /b 1\r\n\techo.\r\n\techo.Build finished; now you can process the JSON files.\r\n\tgoto end\r\n)\r\n\r\nif \"%1\" == \"htmlhelp\" (\r\n\t%SPHINXBUILD% -b htmlhelp %ALLSPHINXOPTS% %BUILDDIR%/htmlhelp\r\n\tif errorlevel 1 exit /b 1\r\n\techo.\r\n\techo.Build finished; now you can run HTML Help Workshop with the ^\r\n.hhp project file in %BUILDDIR%/htmlhelp.\r\n\tgoto end\r\n)\r\n\r\nif \"%1\" == \"qthelp\" (\r\n\t%SPHINXBUILD% -b qthelp %ALLSPHINXOPTS% %BUILDDIR%/qthelp\r\n\tif errorlevel 1 exit /b 1\r\n\techo.\r\n\techo.Build finished; now you can run \"qcollectiongenerator\" with the ^\r\n.qhcp project file in %BUILDDIR%/qthelp, like this:\r\n\techo.^> qcollectiongenerator %BUILDDIR%\\qthelp\\pytransform3d.qhcp\r\n\techo.To view the help file:\r\n\techo.^> assistant -collectionFile %BUILDDIR%\\qthelp\\pytransform3d.ghc\r\n\tgoto end\r\n)\r\n\r\nif \"%1\" == \"devhelp\" (\r\n\t%SPHINXBUILD% -b devhelp %ALLSPHINXOPTS% %BUILDDIR%/devhelp\r\n\tif errorlevel 1 exit /b 1\r\n\techo.\r\n\techo.Build finished.\r\n\tgoto end\r\n)\r\n\r\nif \"%1\" == \"epub\" (\r\n\t%SPHINXBUILD% -b epub %ALLSPHINXOPTS% %BUILDDIR%/epub\r\n\tif errorlevel 1 exit /b 1\r\n\techo.\r\n\techo.Build finished. The epub file is in %BUILDDIR%/epub.\r\n\tgoto end\r\n)\r\n\r\nif \"%1\" == \"latex\" (\r\n\t%SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex\r\n\tif errorlevel 1 exit /b 1\r\n\techo.\r\n\techo.Build finished; the LaTeX files are in %BUILDDIR%/latex.\r\n\tgoto end\r\n)\r\n\r\nif \"%1\" == \"latexpdf\" (\r\n\t%SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex\r\n\tcd %BUILDDIR%/latex\r\n\tmake all-pdf\r\n\tcd %BUILDDIR%/..\r\n\techo.\r\n\techo.Build finished; the PDF files are in %BUILDDIR%/latex.\r\n\tgoto end\r\n)\r\n\r\nif \"%1\" == \"latexpdfja\" (\r\n\t%SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex\r\n\tcd %BUILDDIR%/latex\r\n\tmake all-pdf-ja\r\n\tcd %BUILDDIR%/..\r\n\techo.\r\n\techo.Build finished; the PDF files are in %BUILDDIR%/latex.\r\n\tgoto end\r\n)\r\n\r\nif \"%1\" == \"text\" (\r\n\t%SPHINXBUILD% -b text %ALLSPHINXOPTS% %BUILDDIR%/text\r\n\tif errorlevel 1 exit /b 1\r\n\techo.\r\n\techo.Build finished. The text files are in %BUILDDIR%/text.\r\n\tgoto end\r\n)\r\n\r\nif \"%1\" == \"man\" (\r\n\t%SPHINXBUILD% -b man %ALLSPHINXOPTS% %BUILDDIR%/man\r\n\tif errorlevel 1 exit /b 1\r\n\techo.\r\n\techo.Build finished. The manual pages are in %BUILDDIR%/man.\r\n\tgoto end\r\n)\r\n\r\nif \"%1\" == \"texinfo\" (\r\n\t%SPHINXBUILD% -b texinfo %ALLSPHINXOPTS% %BUILDDIR%/texinfo\r\n\tif errorlevel 1 exit /b 1\r\n\techo.\r\n\techo.Build finished. The Texinfo files are in %BUILDDIR%/texinfo.\r\n\tgoto end\r\n)\r\n\r\nif \"%1\" == \"gettext\" (\r\n\t%SPHINXBUILD% -b gettext %I18NSPHINXOPTS% %BUILDDIR%/locale\r\n\tif errorlevel 1 exit /b 1\r\n\techo.\r\n\techo.Build finished. The message catalogs are in %BUILDDIR%/locale.\r\n\tgoto end\r\n)\r\n\r\nif \"%1\" == \"changes\" (\r\n\t%SPHINXBUILD% -b changes %ALLSPHINXOPTS% %BUILDDIR%/changes\r\n\tif errorlevel 1 exit /b 1\r\n\techo.\r\n\techo.The overview file is in %BUILDDIR%/changes.\r\n\tgoto end\r\n)\r\n\r\nif \"%1\" == \"linkcheck\" (\r\n\t%SPHINXBUILD% -b linkcheck %ALLSPHINXOPTS% %BUILDDIR%/linkcheck\r\n\tif errorlevel 1 exit /b 1\r\n\techo.\r\n\techo.Link check complete; look for any errors in the above output ^\r\nor in %BUILDDIR%/linkcheck/output.txt.\r\n\tgoto end\r\n)\r\n\r\nif \"%1\" == \"doctest\" (\r\n\t%SPHINXBUILD% -b doctest %ALLSPHINXOPTS% %BUILDDIR%/doctest\r\n\tif errorlevel 1 exit /b 1\r\n\techo.\r\n\techo.Testing of doctests in the sources finished, look at the ^\r\nresults in %BUILDDIR%/doctest/output.txt.\r\n\tgoto end\r\n)\r\n\r\nif \"%1\" == \"xml\" (\r\n\t%SPHINXBUILD% -b xml %ALLSPHINXOPTS% %BUILDDIR%/xml\r\n\tif errorlevel 1 exit /b 1\r\n\techo.\r\n\techo.Build finished. The XML files are in %BUILDDIR%/xml.\r\n\tgoto end\r\n)\r\n\r\nif \"%1\" == \"pseudoxml\" (\r\n\t%SPHINXBUILD% -b pseudoxml %ALLSPHINXOPTS% %BUILDDIR%/pseudoxml\r\n\tif errorlevel 1 exit /b 1\r\n\techo.\r\n\techo.Build finished. The pseudo-XML files are in %BUILDDIR%/pseudoxml.\r\n\tgoto end\r\n)\r\n\r\n:end\r\n" }, { "path": "doc/source/_static/custom.css", "content": ":root {\n --pst-sidebar-width: 100px;\n --pst-secondary-sidebar-width: 100px;\n}\n\n.bd-main .bd-content .bd-article-container {\n max-width: 100%; /* Allows content to expand if sidebars are smaller */\n}\n\n.bd-page-width {\n max-width: 100%; /* Ensures the overall page uses more width */\n}\n" }, { "path": "doc/source/_static/overview.tex", "content": "\\documentclass{article}\n\\usepackage{amssymb,amsmath}\n\\usepackage[pdftex,active,tightpage]{preview}\n\\setlength\\PreviewBorder{2mm}\n\n% converted to png via\n% pdflatex overview.tex\n% pdftoppm -png overview.pdf > overview.png\n\n\\usepackage{tikz}\n\\usetikzlibrary{arrows, arrows.meta, positioning}\n\n\\begin{document}\n\\begin{preview}\n\\newcommand{\\matx}{10.1}\n\\newcommand{\\maty}{14.2}\n\n\\begin{tikzpicture}\n\\node[draw,shape=rectangle,rounded corners,fill=cyan!10] (legendconversionrot) at (7.5,18.7) {\\large\\texttt{pytransform3d.rotations}};\n\n\\node[draw,shape=rectangle,rounded corners,fill=blue!10] (legendconversiontr) at (21.5,18.7) {\\large\\texttt{pytransform3d.transformations}};\n\n\\node[draw,shape=rectangle,thick,rounded corners,fill=blue!15] (tmat) at (17,15) {\\begin{minipage}{4cm}\\textbf{transformation matrix}\\\\[0.3em]\n$\\boldsymbol{T} \\in SE(3)$\\\\[0.3em]\n\\texttt{transform}\\\\[0.3em]\nshape (4, 4)\n\\end{minipage}};\n\n\\node[draw,shape=rectangle,thick,rounded corners,fill=blue!15] (dquat) at (21.5,15) {\\begin{minipage}{4cm}\\textbf{(unit) dual quaternion}\\\\[0.3em]\n$\\boldsymbol{\\sigma}$\\\\[0.3em]\n\\texttt{dual\\_quaternion}\\\\[0.3em]\nshape (8,)\n\\end{minipage}};\n\n\\node[draw,shape=rectangle,thick,rounded corners,fill=blue!15] (pq) at (26.5,15) {\\begin{minipage}{5cm}\\textbf{position and unit quaternion}\\\\[0.3em]\n$(\\boldsymbol{p}, \\boldsymbol{q}) \\in \\mathbb{R}^3 \\times S^3$\\\\[0.3em]\n\\texttt{pq}\\\\[0.3em]\nshape (7,)\n\\end{minipage}};\n\n\\node[draw,shape=rectangle,thick,rounded corners,fill=blue!15] (logtr) at (17,11) {\\begin{minipage}{4.9cm}\\textbf{logarithm of transformation}\\\\[0.3em]\n$\\left[\\mathcal{S}\\right]\\theta \\in se(3)$\\\\[0.3em]\n\\texttt{transform\\_log}\\\\[0.3em]\nshape (4, 4)\n\\end{minipage}};\n\n\\node[draw,shape=rectangle,thick,rounded corners] (screwmat) at (22.5,11) {\\begin{minipage}{3.8cm}\\textbf{screw matrix}\\\\[0.3em]\n$\\left[\\mathcal{S}\\right] \\in se(3)$\\\\[0.3em]\n\\texttt{screw\\_matrix}\\\\[0.3em]\nshape (4, 4)\n\\end{minipage}};\n\n\\node[draw,shape=rectangle,thick,rounded corners,fill=blue!15] (expcoord) at (17,7) {\\begin{minipage}{7.5cm}\\textbf{exponential coordinates of transformation}\\\\[0.3em]\n$\\mathcal{S}\\theta \\in \\mathbb{R}^6$\\\\[0.3em]\n\\texttt{exponential\\_coordinates}\\\\[0.3em]\nshape (6,)\n\\end{minipage}};\n\n\\node[draw,shape=rectangle,thick,rounded corners] (spvel) at (24,7) {\\begin{minipage}{4cm}\\textbf{twist / spatial velocity}\\\\[0.3em]\n$\\mathcal{S}\\dot{\\theta} \\in \\mathbb{R}^6$\\\\[0.3em]\nshape (6,)\n\\end{minipage}};\n\n\\node[draw,shape=rectangle,thick,rounded corners] (spacc) at (29,7) {\\begin{minipage}{3.5cm}\\textbf{spatial acceleration}\\\\[0.3em]\n$\\mathcal{S}\\ddot{\\theta} \\in \\mathbb{R}^6$\\\\[0.3em]\nshape (6,)\n\\end{minipage}};\n\n\\node[draw,shape=rectangle,thick,rounded corners] (scrax) at (17,2) {\\begin{minipage}{2.2cm}\\textbf{screw axis}\\\\[0.3em]\n$\\mathcal{S} \\in \\mathbb{R}^6$\\\\[0.3em]\n\\texttt{screw\\_axis}\\\\[0.3em]\nshape (6,)\n\\end{minipage}};\n\n\\node[draw,shape=rectangle,thick,rounded corners] (screw) at (22,2) {\\begin{minipage}{5.2cm}\\textbf{screw parameters}\\\\[0.3em]\n$(\\boldsymbol{q}, \\hat{\\boldsymbol{s}}, h) \\in \\mathbb{R}^3 \\times S^2 \\times \\mathbb{R}$\\\\[0.3em]\n\\texttt{screw\\_parameters}\\\\[0.3em]\ntuple[shape (3,), shape (3,), float]\n\\end{minipage}};\n\n\\draw[<->] (tmat) -- (logtr) node[midway,left] {exp./log. map};\n\\draw[->] (expcoord) -- (logtr) node[midway,left] {$\\left[\\cdot\\right]$};\n\\draw[->] (expcoord) -- (spvel) node[midway,above] {$\\frac{\\partial \\mathcal{S}\\theta}{\\partial t}$};\n\\draw[->] (spvel) -- (spacc) node[midway,above] {$\\frac{\\partial \\mathcal{S}\\dot{\\theta}}{\\partial t}$};\n\\draw[->] (scrax) -- (expcoord) node[midway,left] {$\\theta$};\n\\draw[->] (screwmat) -- (logtr) node[midway,above] {$\\theta$};\n\\draw[<->] (screw) -- (scrax) node[midway,above] {};\n\n\\draw [thick,rounded corners,fill=cyan!15] (\\matx-0.2,\\maty-0.5) rectangle (\\matx+3.1,\\maty+2.2);\n\\node[anchor=west] (rotmatname) at (\\matx,\\maty+1.8) {\\textbf{rotation matrix}};\n\\node[anchor=west] (rotmatsymbol) at (\\matx,\\maty+1.2) {$\\boldsymbol{R}$};\n\\node[anchor=west] (rotmatset) at (\\matx+0.4,\\maty+1.178) {$\\in SO(3)$};\n\\node[anchor=west] (rotmatpname) at (\\matx,\\maty+0.6) {\\texttt{matrix}};\n\\node[anchor=west] (rotmatarray) at (\\matx,\\maty) {shape (3, 3)};\n\n\\node (legendname) at (\\matx-2,\\maty+3) {representation};\n\\draw[red] (rotmatname) edge (legendname);\n\\node (legendsymbol) at (\\matx-3,\\maty+1.9) {mathematical symbol};\n\\draw[red] (rotmatsymbol) edge (legendsymbol);\n\\node (legendset) at (\\matx+4,\\maty+1.2) {set};\n\\draw[red] (rotmatset) edge (legendset);\n\\node (legendpname) at (\\matx-3.3,\\maty+0.7) {\\begin{minipage}{3.1cm}\\centering name in pytransform3d\\end{minipage}};\n\\draw[red] (rotmatpname) edge (legendpname);\n\\node (legendarray) at (\\matx-2,\\maty-0.5) {NumPy array};\n\\draw[red] (rotmatarray) edge (legendarray);\n\n\\node[draw,shape=rectangle,thick,rounded corners,fill=cyan!15] (quat) at (1.8,15) {\\begin{minipage}{6cm}\\textbf{(unit) quaternion / versor / rotor}\\\\[0.3em]\n$\\boldsymbol{q} \\in S^3$\\\\[0.3em]\n\\texttt{quaternion}\\\\[0.3em]\nshape (4,)\n\\end{minipage}};\n\n\\node[draw,shape=rectangle,thick,rounded corners,fill=cyan!15] (logrot) at (11.2,11) {\\begin{minipage}{3.8cm}\\textbf{logarithm of rotation}\\\\[0.3em]\n$\\left[\\boldsymbol{\\omega}\\right] \\in so(3)$\\\\[0.3em]\n\\texttt{rot\\_log}\\\\[0.3em]\nshape (3, 3)\n\\end{minipage}};\n\n\\node[draw,shape=rectangle,thick,rounded corners,fill=cyan!15] (rotvec) at (11.2,7) {\\begin{minipage}{3.3cm}\\textbf{rotation vector}\\\\[0.3em]\n$\\boldsymbol{\\omega} = \\hat{\\boldsymbol{\\omega}}\\theta \\in \\mathbb{R}^3$\\\\[0.3em]\n\\texttt{compact\\_axis\\_angle}\\\\[0.3em]\nshape (3,)\n\\end{minipage}};\n\n\\node[draw,shape=rectangle,thick,rounded corners] (angvel) at (6.9,7) {\\begin{minipage}{2.8cm}\\textbf{angular velocity}\\\\[0.3em]\n$\\dot{\\boldsymbol{\\omega}} = \\hat{\\boldsymbol{\\omega}}\\dot{\\theta} \\in \\mathbb{R}^3$\\\\[0.3em]\nshape (3,)\n\\end{minipage}};\n\n\\node[draw,shape=rectangle,thick,rounded corners] (angacc) at (2.5,7) {\\begin{minipage}{3.6cm}\\textbf{angular acceleration}\\\\[0.3em]\n$\\hat{\\boldsymbol{\\omega}}\\ddot{\\theta} \\in \\mathbb{R}^3$\\\\[0.3em]\nshape (3,)\n\\end{minipage}};\n\n\\node[draw,shape=rectangle,thick,rounded corners,fill=cyan!15] (axan) at (11.2,3.7) {\\begin{minipage}{2.4cm}\\textbf{axis-angle}\\\\[0.3em]\n$(\\hat{\\boldsymbol{\\omega}}, \\theta) \\in S^2 \\times \\mathbb{R}$\\\\[0.3em]\n\\texttt{axis\\_angle}\\\\[0.3em]\nshape (4,)\n\\end{minipage}};\n\n\\node[draw,shape=rectangle,thick,rounded corners] (rotax) at (11.2,1.5) {\\begin{minipage}{3.8cm}\\textbf{rotation axis}\\\\[0.3em]\n$\\hat{\\boldsymbol{\\omega}} \\in S^2$\n\\end{minipage}};\n\n\\node[draw,shape=rectangle,thick,rounded corners,fill=cyan!15] (euler) at (3,1) {\\begin{minipage}{7.5cm}\\textbf{proper Euler / Cardan / Tait-Bryan angles}\\\\[0.3em]\n$(\\alpha, \\beta, \\gamma) \\in \\mathbb{R}^3$\\\\[0.3em]\n\\texttt{euler}\\\\[0.3em]\nshape (3,)\n\\end{minipage}};\n\n\\node[draw,shape=rectangle,thick,rounded corners,fill=cyan!15] (mrp) at (6,3.7) {\\begin{minipage}{5.6cm}\\textbf{modified Rodrigues parameters}\\\\[0.3em]\n$\\boldsymbol{\\psi} = \\hat{\\boldsymbol{\\omega}} \\tan{\\frac{\\theta}{4}} \\in \\mathbb{R}^3$\\\\[0.3em]\n\\texttt{mrp}\\\\[0.3em]\nshape (3,)\n\\end{minipage}};\n\n\\draw[<->] (rotmatarray) -- (logrot) node[midway,left] {exp./log. map};\n\\draw[->] (rotvec) -- (logrot) node[midway,left] {cross product matrix};\n\\draw[->] (rotvec) -- (angvel) node[midway,above] {$\\frac{\\partial \\boldsymbol{\\omega}}{\\partial t}$};\n\\draw[->] (angvel) -- (angacc) node[midway,above] {$\\frac{\\partial \\dot{\\boldsymbol{\\omega}}}{\\partial t}$};\n\\draw[->] (axan) -- (rotvec) node[midway,left] {product};\n\\draw[->] (rotax) -- (axan) node[midway,left] {$\\theta$};\n\\end{tikzpicture}\n\\end{preview}\n\\end{document}" }, { "path": "doc/source/_templates/class.rst", "content": "{{ fullname | escape | underline}}\n\n.. currentmodule:: {{ module }}\n\n.. autoclass:: {{ objname }}\n :members:\n :show-inheritance:\n :inherited-members:\n\n {% block methods %}\n .. automethod:: __init__\n\n {% if methods %}\n .. rubric:: {{ _('Methods') }}\n\n .. autosummary::\n {% for item in methods %}\n ~{{ name }}.{{ item }}\n {%- endfor %}\n {% endif %}\n {% endblock %}\n\n {% block attributes %}\n {% if attributes %}\n .. rubric:: {{ _('Attributes') }}\n\n .. autosummary::\n {% for item in attributes %}\n ~{{ name }}.{{ item }}\n {%- endfor %}\n {% endif %}\n {% endblock %}\n\n.. minigallery:: {{module}}.{{objname}}\n :add-heading:\n" }, { "path": "doc/source/_templates/class_without_inherited.rst", "content": "{{ fullname | escape | underline}}\n\n.. currentmodule:: {{ module }}\n\n.. autoclass:: {{ objname }}\n :members:\n :show-inheritance:\n :no-inherited-members:\n\n {% block methods %}\n .. automethod:: __init__\n\n {% if methods %}\n .. rubric:: {{ _('Methods') }}\n\n .. autosummary::\n {% for item in methods %}\n {% if item not in inherited_members %}\n ~{{ name }}.{{ item }}\n {% endif %}\n {%- endfor %}\n {% endif %}\n {% endblock %}\n\n {% block attributes %}\n {% if attributes %}\n .. rubric:: {{ _('Attributes') }}\n\n .. autosummary::\n {% for item in attributes %}\n {% if item not in inherited_members %}\n ~{{ name }}.{{ item }}\n {% endif %}\n {%- endfor %}\n {% endif %}\n {% endblock %}\n\n.. minigallery:: {{module}}.{{objname}}\n :add-heading:\n" }, { "path": "doc/source/_templates/function.rst", "content": ":mod:`{{module}}`.{{objname}}\n{{ underline }}====================\n\n.. currentmodule:: {{ module }}\n\n.. autofunction:: {{ objname }}\n\n.. minigallery:: {{module}}.{{objname}}\n :add-heading:\n" }, { "path": "doc/source/api.rst", "content": ".. _api:\n\n=================\nAPI Documentation\n=================\n\nThis is the detailed documentation of all public classes and functions.\nYou can also search for specific modules, classes, or functions in the\n:ref:`genindex`.\n\n\n:mod:`pytransform3d.rotations`\n==============================\n\n.. automodule:: pytransform3d.rotations\n :no-members:\n :no-inherited-members:\n\nRotation Matrix\n---------------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~check_matrix\n\n ~matrix_requires_renormalization\n ~norm_matrix\n ~robust_polar_decomposition\n\n ~random_matrix\n\n ~plot_basis\n\n ~assert_rotation_matrix\n\n ~matrix_slerp\n ~matrix_power\n\n ~passive_matrix_from_angle\n ~active_matrix_from_angle\n ~matrix_from_two_vectors\n\n ~matrix_from_euler\n ~matrix_from_axis_angle\n ~matrix_from_compact_axis_angle\n ~matrix_from_quaternion\n ~matrix_from_rotor\n\nEuler Angles\n------------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~euler_near_gimbal_lock\n ~norm_euler\n\n ~assert_euler_equal\n\n ~euler_from_quaternion\n ~euler_from_matrix\n\nAxis-Angle\n----------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~check_axis_angle\n ~check_compact_axis_angle\n\n ~compact_axis_angle_near_pi\n ~norm_axis_angle\n ~norm_compact_axis_angle\n\n ~random_axis_angle\n ~random_compact_axis_angle\n\n ~plot_axis_angle\n\n ~assert_axis_angle_equal\n ~assert_compact_axis_angle_equal\n\n ~axis_angle_slerp\n\n ~axis_angle_from_two_directions\n\n ~axis_angle_from_matrix\n ~axis_angle_from_quaternion\n ~axis_angle_from_compact_axis_angle\n ~axis_angle_from_mrp\n ~compact_axis_angle\n ~compact_axis_angle_from_matrix\n ~compact_axis_angle_from_quaternion\n\nLogarithm of Rotation\n---------------------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~check_rot_log\n ~check_skew_symmetric_matrix\n\n ~rot_log_from_compact_axis_angle\n ~cross_product_matrix\n\n\nQuaternion\n----------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~check_quaternion\n ~check_quaternions\n\n ~quaternion_requires_renormalization\n\n ~quaternion_double\n ~pick_closest_quaternion\n\n ~random_quaternion\n\n ~assert_quaternion_equal\n\n ~concatenate_quaternions\n ~q_prod_vector\n ~q_conj\n ~quaternion_slerp\n ~quaternion_dist\n ~quaternion_diff\n ~quaternion_gradient\n ~quaternion_integrate\n ~quaternion_from_angle\n ~quaternion_from_euler\n ~quaternion_from_matrix\n ~quaternion_from_axis_angle\n ~quaternion_from_compact_axis_angle\n ~quaternion_from_mrp\n ~quaternion_xyzw_from_wxyz\n ~quaternion_wxyz_from_xyzw\n\nRotor\n-----\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~check_rotor\n\n ~plot_bivector\n\n ~wedge\n ~plane_normal_from_bivector\n ~geometric_product\n ~concatenate_rotors\n ~rotor_reverse\n ~rotor_apply\n ~rotor_slerp\n\n ~rotor_from_two_directions\n ~rotor_from_plane_angle\n\nModified Rodrigues Parameters\n-----------------------------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~check_mrp\n\n ~mrp_near_singularity\n ~norm_mrp\n\n ~assert_mrp_equal\n\n ~mrp_double\n\n ~concatenate_mrp\n ~mrp_prod_vector\n\n ~mrp_from_axis_angle\n ~mrp_from_quaternion\n\nJacobians\n---------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~left_jacobian_SO3\n ~left_jacobian_SO3_series\n ~left_jacobian_SO3_inv\n ~left_jacobian_SO3_inv_series\n\nUtility Functions\n-----------------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~norm_angle\n ~norm_vector\n ~perpendicular_to_vectors\n ~angle_between_vectors\n ~vector_projection\n ~plane_basis_from_normal\n ~random_vector\n\nDeprecated Functions\n--------------------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~quaternion_from_extrinsic_euler_xyz\n ~active_matrix_from_intrinsic_euler_xzx\n ~active_matrix_from_extrinsic_euler_xzx\n ~active_matrix_from_intrinsic_euler_xyx\n ~active_matrix_from_extrinsic_euler_xyx\n ~active_matrix_from_intrinsic_euler_yxy\n ~active_matrix_from_extrinsic_euler_yxy\n ~active_matrix_from_intrinsic_euler_yzy\n ~active_matrix_from_extrinsic_euler_yzy\n ~active_matrix_from_intrinsic_euler_zyz\n ~active_matrix_from_extrinsic_euler_zyz\n ~active_matrix_from_intrinsic_euler_zxz\n ~active_matrix_from_extrinsic_euler_zxz\n ~active_matrix_from_intrinsic_euler_xzy\n ~active_matrix_from_extrinsic_euler_xzy\n ~active_matrix_from_intrinsic_euler_xyz\n ~active_matrix_from_extrinsic_euler_xyz\n ~active_matrix_from_intrinsic_euler_yxz\n ~active_matrix_from_extrinsic_euler_yxz\n ~active_matrix_from_intrinsic_euler_yzx\n ~active_matrix_from_extrinsic_euler_yzx\n ~active_matrix_from_intrinsic_euler_zyx\n ~active_matrix_from_extrinsic_euler_zyx\n ~active_matrix_from_intrinsic_euler_zxy\n ~active_matrix_from_extrinsic_euler_zxy\n ~active_matrix_from_extrinsic_roll_pitch_yaw\n ~intrinsic_euler_xzx_from_active_matrix\n ~extrinsic_euler_xzx_from_active_matrix\n ~intrinsic_euler_xyx_from_active_matrix\n ~extrinsic_euler_xyx_from_active_matrix\n ~intrinsic_euler_yxy_from_active_matrix\n ~extrinsic_euler_yxy_from_active_matrix\n ~intrinsic_euler_yzy_from_active_matrix\n ~extrinsic_euler_yzy_from_active_matrix\n ~intrinsic_euler_zyz_from_active_matrix\n ~extrinsic_euler_zyz_from_active_matrix\n ~intrinsic_euler_zxz_from_active_matrix\n ~extrinsic_euler_zxz_from_active_matrix\n ~intrinsic_euler_xzy_from_active_matrix\n ~extrinsic_euler_xzy_from_active_matrix\n ~intrinsic_euler_xyz_from_active_matrix\n ~extrinsic_euler_xyz_from_active_matrix\n ~intrinsic_euler_yxz_from_active_matrix\n ~extrinsic_euler_yxz_from_active_matrix\n ~intrinsic_euler_yzx_from_active_matrix\n ~extrinsic_euler_yzx_from_active_matrix\n ~intrinsic_euler_zyx_from_active_matrix\n ~extrinsic_euler_zyx_from_active_matrix\n ~intrinsic_euler_zxy_from_active_matrix\n ~extrinsic_euler_zxy_from_active_matrix\n\n\n:mod:`pytransform3d.transformations`\n====================================\n\n.. automodule:: pytransform3d.transformations\n :no-members:\n :no-inherited-members:\n\nTransformation Matrix\n---------------------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~check_transform\n\n ~transform_requires_renormalization\n\n ~random_transform\n\n ~concat\n ~invert_transform\n ~transform\n ~vector_to_point\n ~vectors_to_points\n ~vector_to_direction\n ~vectors_to_directions\n ~transform_power\n ~transform_sclerp\n ~adjoint_from_transform\n\n ~plot_transform\n\n ~assert_transform\n\n ~transform_from\n ~translate_transform\n ~rotate_transform\n\n ~transform_from_pq\n ~transform_from_exponential_coordinates\n ~transform_from_transform_log\n ~transform_from_dual_quaternion\n\nPosition and Quaternion\n-----------------------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~check_pq\n\n ~pq_slerp\n\n ~pq_from_transform\n ~pq_from_dual_quaternion\n\nScrew Parameters\n----------------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~check_screw_parameters\n\n ~plot_screw\n\n ~assert_screw_parameters_equal\n\n ~screw_parameters_from_screw_axis\n ~screw_parameters_from_dual_quaternion\n\nScrew Axis\n----------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~check_screw_axis\n\n ~random_screw_axis\n\n ~screw_axis_from_screw_parameters\n ~screw_axis_from_exponential_coordinates\n ~screw_axis_from_screw_matrix\n\nExponential Coordinates\n-----------------------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~check_exponential_coordinates\n\n ~norm_exponential_coordinates\n\n ~random_exponential_coordinates\n\n ~assert_exponential_coordinates_equal\n\n ~exponential_coordinates_from_transform\n ~exponential_coordinates_from_screw_axis\n ~exponential_coordinates_from_transform_log\n\nScrew Matrix\n------------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~check_screw_matrix\n\n ~screw_matrix_from_screw_axis\n ~screw_matrix_from_transform_log\n\nLogarithm of Transformation\n---------------------------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~check_transform_log\n\n ~transform_log_from_exponential_coordinates\n ~transform_log_from_screw_matrix\n ~transform_log_from_transform\n\nDual Quaternion\n---------------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~check_dual_quaternion\n\n ~dual_quaternion_requires_renormalization\n ~norm_dual_quaternion\n ~dual_quaternion_squared_norm\n\n ~assert_unit_dual_quaternion\n ~assert_unit_dual_quaternion_equal\n\n ~dual_quaternion_double\n\n ~dq_conj\n ~dq_q_conj\n ~concatenate_dual_quaternions\n ~dq_prod_vector\n ~dual_quaternion_power\n ~dual_quaternion_sclerp\n\n ~dual_quaternion_from_transform\n ~dual_quaternion_from_pq\n ~dual_quaternion_from_screw_parameters\n\nJacobians\n---------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~left_jacobian_SE3\n ~left_jacobian_SE3_series\n ~left_jacobian_SE3_inv\n ~left_jacobian_SE3_inv_series\n\nDeprecated Functions\n--------------------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~scale_transform\n\n\n:mod:`pytransform3d.batch_rotations`\n====================================\n\n.. automodule:: pytransform3d.batch_rotations\n :no-members:\n :no-inherited-members:\n\nRotation Matrices\n-----------------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~active_matrices_from_angles\n\n ~active_matrices_from_intrinsic_euler_angles\n ~active_matrices_from_extrinsic_euler_angles\n ~matrices_from_compact_axis_angles\n ~matrices_from_quaternions\n\nAxis-Angle Representation\n-------------------------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~norm_axis_angles\n ~axis_angles_from_matrices\n ~axis_angles_from_quaternions\n\n ~cross_product_matrices\n\nQuaternions\n-----------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~batch_concatenate_quaternions\n ~batch_q_conj\n ~quaternion_slerp_batch\n ~smooth_quaternion_trajectory\n\n ~quaternions_from_matrices\n ~batch_quaternion_wxyz_from_xyzw\n ~batch_quaternion_xyzw_from_wxyz\n\nUtility Functions\n-----------------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~norm_vectors\n ~angles_between_vectors\n\n\n:mod:`pytransform3d.trajectories`\n=================================\n\n.. automodule:: pytransform3d.trajectories\n :no-members:\n :no-inherited-members:\n\nTransformation Matrices\n-----------------------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~random_trajectories\n\n ~invert_transforms\n ~concat_one_to_many\n ~concat_many_to_one\n ~concat_many_to_many\n ~concat_dynamic\n\n ~transforms_from_pqs\n ~transforms_from_exponential_coordinates\n ~transforms_from_dual_quaternions\n\nPositions and Quaternions\n-------------------------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~plot_trajectory\n\n ~pqs_from_transforms\n ~pqs_from_dual_quaternions\n\nScrew Parameters\n----------------\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~screw_parameters_from_dual_quaternions\n\nExponential Coordinates\n-----------------------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~mirror_screw_axis_direction\n\n ~exponential_coordinates_from_transforms\n\nDual Quaternions\n----------------\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~batch_dq_conj\n ~batch_dq_q_conj\n ~batch_concatenate_dual_quaternions\n ~batch_dq_prod_vector\n ~dual_quaternions_from_pqs\n ~dual_quaternions_power\n ~dual_quaternions_sclerp\n ~dual_quaternions_from_screw_parameters\n\n\n:mod:`pytransform3d.uncertainty`\n================================\n\n.. automodule:: pytransform3d.uncertainty\n :no-members:\n :no-inherited-members:\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~estimate_gaussian_rotation_matrix_from_samples\n ~estimate_gaussian_transform_from_samples\n ~frechet_mean\n ~invert_uncertain_transform\n ~concat_globally_uncertain_transforms\n ~concat_locally_uncertain_transforms\n ~pose_fusion\n ~to_ellipsoid\n ~to_projected_ellipsoid\n ~plot_projected_ellipsoid\n\n\n:mod:`pytransform3d.coordinates`\n================================\n\n.. automodule:: pytransform3d.coordinates\n :no-members:\n :no-inherited-members:\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~cartesian_from_cylindrical\n ~cartesian_from_spherical\n ~cylindrical_from_cartesian\n ~cylindrical_from_spherical\n ~spherical_from_cartesian\n ~spherical_from_cylindrical\n\n\n:mod:`pytransform3d.transform_manager`\n======================================\n\n.. automodule:: pytransform3d.transform_manager\n :no-members:\n :no-inherited-members:\n\n.. autosummary::\n :toctree: _apidoc/\n :template: class.rst\n\n ~TransformGraphBase\n ~TransformManager\n ~TemporalTransformManager\n ~TimeVaryingTransform\n ~StaticTransform\n ~NumpyTimeseriesTransform\n\n\n:mod:`pytransform3d.editor`\n===========================\n\n.. automodule:: pytransform3d.editor\n :no-members:\n :no-inherited-members:\n\n.. autosummary::\n :toctree: _apidoc/\n :template: class_without_inherited.rst\n\n ~TransformEditor\n\n\n:mod:`pytransform3d.urdf`\n=========================\n\n.. automodule:: pytransform3d.urdf\n :no-members:\n :no-inherited-members:\n\n.. autosummary::\n :toctree: _apidoc/\n :template: class.rst\n\n ~UrdfTransformManager\n ~Link\n ~Joint\n ~Geometry\n ~Box\n ~Sphere\n ~Cylinder\n ~Mesh\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~parse_urdf\n ~initialize_urdf_transform_manager\n\n\n:mod:`pytransform3d.camera`\n===========================\n\n.. automodule:: pytransform3d.camera\n :no-members:\n :no-inherited-members:\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~make_world_grid\n ~make_world_line\n ~cam2sensor\n ~sensor2img\n ~world2image\n ~plot_camera\n\n\n:mod:`pytransform3d.plot_utils`\n===============================\n\n.. automodule:: pytransform3d.plot_utils\n :no-members:\n :no-inherited-members:\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~make_3d_axis\n ~remove_frame\n ~plot_vector\n ~plot_length_variable\n ~plot_box\n ~plot_sphere\n ~plot_spheres\n ~plot_cylinder\n ~plot_mesh\n ~plot_ellipsoid\n ~plot_capsule\n ~plot_cone\n\n.. autosummary::\n :toctree: _apidoc/\n :template: class_without_inherited.rst\n\n ~Arrow3D\n ~Frame\n ~LabeledFrame\n ~Trajectory\n ~Camera\n\n\n:mod:`pytransform3d.visualizer`\n===============================\n\n.. automodule:: pytransform3d.visualizer\n :no-members:\n :no-inherited-members:\n\n.. autosummary::\n :toctree: _apidoc/\n :template: function.rst\n\n ~figure\n\n.. autosummary::\n :toctree: _apidoc/\n :template: class.rst\n\n ~Figure\n ~Artist\n ~Line3D\n ~PointCollection3D\n ~Vector3D\n ~Frame\n ~Trajectory\n ~Sphere\n ~Box\n ~Cylinder\n ~Mesh\n ~Ellipsoid\n ~Capsule\n ~Cone\n ~Plane\n ~Graph\n ~Camera\n" }, { "path": "doc/source/conf.py", "content": "# -*- coding: utf-8 -*-\n\nimport sys\nimport os\nimport glob\nimport shutil\nimport time\nfrom sphinx_gallery.scrapers import figure_rst\n\nsys.path.insert(0, os.path.abspath('.'))\nsys.path.insert(0, os.path.abspath('../..'))\n\nextensions = [\n \"sphinx.ext.autodoc\",\n \"sphinx.ext.autosummary\",\n \"sphinx.ext.doctest\",\n \"sphinx.ext.intersphinx\",\n \"sphinx.ext.todo\",\n \"sphinx.ext.coverage\",\n \"sphinx.ext.mathjax\",\n #\"sphinx.ext.imgmath\",\n \"sphinx.ext.ifconfig\",\n \"sphinx.ext.viewcode\",\n \"matplotlib.sphinxext.plot_directive\",\n \"numpydoc\",\n \"sphinx_gallery.gen_gallery\",\n]\n\nautodoc_default_options = {\"member-order\": \"bysource\"}\nautosummary_generate = True # generate files at doc/source/_apidoc\nclass_members_toctree = False\nnumpydoc_show_class_members = False\n\n# Options for the `::plot` directive:\n# https://matplotlib.org/stable/api/sphinxext_plot_directive_api.html\nplot_formats = [\"png\"]\nplot_include_source = False\nplot_html_show_formats = False\nplot_html_show_source_link = False\n\n# class template from https://stackoverflow.com/a/62613202/915743\ntemplates_path = [\"_templates\"]\nexclude_patterns = []\nexclude_trees = [\"_templates\", \"sphinxext\"]\nsource_suffix = '.rst'\nsource_encoding = 'utf-8-sig'\nmaster_doc = 'index'\nproject = u'pytransform3d'\ncopyright = u\"2014-{}, Alexander Fabisch, DFKI GmbH, Robotics Innovation Center\".format(time.strftime(\"%Y\"))\nversion = __import__(\"pytransform3d\").__version__\nrelease = __import__(\"pytransform3d\").__version__\nlanguage = 'en'\ntoday_fmt = '%B %d, %Y'\nadd_function_parentheses = True\nauthor = \"Alexander Fabisch\"\nshow_authors = True\npygments_style = 'sphinx'\nhtml_logo = \"_static/logo.png\"\nhtml_theme = \"pydata_sphinx_theme\"\nhtml_theme_options = {\n \"logo\": {\n \"alt_text\": f\"pytransform3d {release}\",\n },\n \"collapse_navigation\": True,\n}\nhtml_sidebars = {\n \"install\": [],\n \"api\": [],\n}\nhtml_static_path = ['_static']\nhtml_css_files = ['custom.css']\nhtml_last_updated_fmt = '%b %d, %Y'\nhtml_use_smartypants = True\nhtml_show_sourcelink = False\nhtml_show_sphinx = False\nhtml_show_copyright = True\n\nintersphinx_mapping = {\n \"python\": (f\"https://docs.python.org/{sys.version_info.major}\", None),\n \"numpy\": (\"https://numpy.org/doc/stable\", None),\n \"scipy\": (\"https://docs.scipy.org/doc/scipy/reference\", None),\n \"matplotlib\": (\"https://matplotlib.org/\", None)\n}\nintersphinx_timeout = 10\n\n\nclass Open3DScraper:\n def __repr__(self):\n return f\"<{type(self).__name__} object>\"\n\n def __call__(self, block, block_vars, gallery_conf, **kwargs):\n \"\"\"Scrape Open3D images.\n\n Parameters\n ----------\n block : tuple\n A tuple containing the (label, content, line_number) of the block.\n block_vars : dict\n Dict of block variables.\n gallery_conf : dict\n Contains the configuration of Sphinx-Gallery\n **kwargs : dict\n Additional keyword arguments to pass to\n :meth:`~matplotlib.figure.Figure.savefig`, e.g. ``format='svg'``.\n The ``format`` kwarg in particular is used to set the file extension\n of the output file (currently only 'png', 'jpg', and 'svg' are\n supported).\n\n Returns\n -------\n rst : str\n The ReSTructuredText that will be rendered to HTML containing\n the images.\n \"\"\"\n path_current_example = os.path.dirname(block_vars['src_file'])\n jpgs = sorted(glob.glob(os.path.join(\n path_current_example, \"__open3d_rendered_image.jpg\")))\n\n image_names = list()\n image_path_iterator = block_vars[\"image_path_iterator\"]\n for jpg in jpgs:\n this_image_path = image_path_iterator.next()\n image_names.append(this_image_path)\n shutil.move(jpg, this_image_path)\n return figure_rst(image_names, gallery_conf[\"src_dir\"])\n\n\ndef _get_sg_image_scraper():\n \"\"\"Return the callable scraper to be used by Sphinx-Gallery.\n\n It allows us to just use strings as they already can be for 'matplotlib'\n and 'mayavi'. Details on this implementation can be found in\n `sphinx-gallery/sphinx-gallery/494`_\n\n This is required to make the config pickable.\n\n This function must be imported into the top level namespace of\n pytransform3d.\n\n .. _sphinx-gallery/sphinx-gallery/494: https://github.com/sphinx-gallery/sphinx-gallery/pull/494\n \"\"\"\n return Open3DScraper()\n\n\n# monkeypatching pytransform3d to make the config pickable\n__import__(\"pytransform3d\")._get_sg_image_scraper = _get_sg_image_scraper\n\n\nsphinx_gallery_conf = {\n \"examples_dirs\": \"../../examples\",\n \"gallery_dirs\": \"_auto_examples\",\n \"reference_url\": {\"pytransform3d\": None},\n \"filename_pattern\": \"/(?:plot|animate|vis)_\",\n \"image_scrapers\": (\"matplotlib\", \"pytransform3d\"),\n \"matplotlib_animations\": (True, \"mp4\"),\n \"backreferences_dir\": \"_auto_examples/backreferences\",\n \"doc_module\": \"pytransform3d\",\n}\n" }, { "path": "doc/source/index.rst", "content": ".. pytransform3d documentation master file, created by\n sphinx-quickstart on Thu Nov 20 21:01:30 2014.\n\n=============\npytransform3d\n=============\n\n.. toctree::\n :hidden:\n\n install\n user_guide/index\n api\n _auto_examples/index\n\nThis documentation explains how you can work with pytransform3d and with\n3D transformations in general.\n\n\n-----\nScope\n-----\n\npytransform3d focuses on readability and debugging, not on computational\nefficiency. If you want to have an efficient implementation of some function\nfrom the library you can easily extract the relevant code and implement it\nmore efficiently in a language of your choice.\n\nThe library integrates well with the\n`scientific Python ecosystem `_\nwith its core libraries NumPy, SciPy and Matplotlib.\nWe rely on `NumPy `_ for linear algebra and on\n`Matplotlib `_ to offer plotting functionalities.\n`SciPy `_ is used if you want to\nautomatically compute transformations from a graph of transformations.\n\npytransform3d offers...\n\n* operations for most common representations of rotation / orientation and\n translation / position\n* conversions between those representations\n* clear documentation of conventions\n* functions to check for common pitfalls (e.g., singularities, ambiguities,\n discontinuities)\n* tight coupling with matplotlib to quickly visualize (or animate)\n transformations\n* the TransformManager which organizes complex chains of transformations\n* the TransformEditor which allows to modify transformations graphically\n* the UrdfTransformManager which is able to load transformations from\n `URDF `_ files\n* a matplotlib-like interface to Open3D's visualizer to display\n geometries and transformations\n\nThe design philosophy of pytransform3d is to not hide anything; there is no\nmagic: no operator overloading, no classes to store data (rotations or\ntransformations) other than NumPy arrays, and no layers of abstraction.\nIt is as transparent to the user as possible and its interface is mainly\nfunctional.\n\n--------\nCitation\n--------\n\nIf you use pytransform3d for a scientific publication, I would appreciate\ncitation of the following paper:\n\nFabisch, A. (2019). pytransform3d: 3D Transformations for Python.\nJournal of Open Source Software, 4(33), 1159,\nhttps://doi.org/10.21105/joss.01159\n" }, { "path": "doc/source/install.rst", "content": "============\nInstallation\n============\n\nYou can install pytransform3d with pip:\n\n.. code-block:: bash\n\n pip install pytransform3d\n\nor conda (since version 1.8):\n\n.. code-block:: bash\n\n conda install -c conda-forge pytransform3d\n\n(More detailed instructions\n`here `_.)\n\n---------------------\nOptional Dependencies\n---------------------\n\nWhen using pip, you can install pytransform3d with the options all, doc, and\ntest.\n\n* `all` will add support for loading meshes, the 3D visualizer of\n pytransform3d, and pydot export of `TransformManager` objects.\n* `doc` will install necessary dependencies to build this documentation.\n* `test` will install dependencies to run the unit tests.\n\nFor example, you can call\n\n.. code-block:: bash\n\n python -m pip install 'pytransform3d[all]'\n\nUnfortunately, pip cannot install all dependencies:\n\n* If you want to have support for pydot export of `TransformManager` objects,\n make sure to install graphviz (on Ubuntu: `sudo apt install graphviz`) if\n you want to use this feature.\n* If you want to have support for the Qt GUI you have to install PyQt 4 or 5\n (on Ubuntu: `sudo apt install python3-pyqt5`; conda: `conda install pyqt`).\n\n------------------------\nInstallation from Source\n------------------------\n\nYou can also install from the current git version. pytransform3d is available\nat `GitHub `_. Clone the repository\nand go to the main folder. Install dependencies with:\n\n.. code-block:: bash\n\n pip install -r requirements.txt\n\nInstall the package with:\n\n.. code-block:: bash\n\n python setup.py install\n\npip also supports installation from a git repository:\n\n.. code-block:: bash\n\n pip install git+https://github.com/dfki-ric/pytransform3d.git\n" }, { "path": "doc/source/user_guide/camera.rst", "content": "======\nCamera\n======\n\nWhen we know the 3D position of a point in the world we can easily compute\nwhere we would see it in a camera image with the pinhole camera model.\nHowever, we have to know some parameters of the camera:\n\n* camera pose\n* focal length :math:`f`\n* sensor width and height\n* image width and height\n\n.. image:: ../_static/camera.png\n :alt: Camera\n :align: center\n :width: 60%\n\n|\n\nNote that light passes through a pinhole in a real pinhole camera before it\nwill be measured from the sensor so that pixels will be mirrored in the x-y\nplane. The sensor that we show here actually corresponds to the virtual\nimage plane.\n\nThe Example :ref:`sphx_glr__auto_examples_plots_plot_camera_with_image.py`\nshows how a grid is projected on an image with the function\n:func:`~pytransform3d.camera.world2image`.\n\n.. figure:: ../_auto_examples/plots/images/sphx_glr_plot_camera_with_image_001.png\n :target: ../_auto_examples/plots/plot_camera_with_image.html\n :align: center\n\nExtrinsic and intrinsic camera parameters can be visualized in the following\nway. The extrinsic camera parameters are fully determined by a transform\nfrom world coordinates to camera coordinates or by the pose of the camera in\nthe world. In this illustration, the point indicates the camera center /\ncenter of projection, which is the position component of the pose. The\norientation determines the direction to and orientation of the virtual image\nplane. The arrow at the top of the virtual image plane shows the up direction\nof the image.\n\nThe field of view is determined from the intrinsic camera parameters. These\nare given by a matrix\n\n.. math::\n\n \\left( \\begin{array}{ccc}\n f_x & 0 & c_x\\\\\n 0 & f_y & c_y\\\\\n 0 & 0 & 1\n \\end{array} \\right),\n\nwhere :math:`f_x, f_y` are focal lengths and :math:`c_x, c_y` is the position\nof the camera center. Together with the image size we can determine the field\nof view. Values of the intrinsic camera matrix and the image size can be given\nin pixels or meters to generate the following visualization with\n:func:`~pytransform3d.camera.plot_camera` (see Example\n:ref:`sphx_glr__auto_examples_plots_plot_camera_3d.py`).\n\n.. figure:: ../_auto_examples/plots/images/sphx_glr_plot_camera_3d_001.png\n :target: ../_auto_examples/plots/plot_camera_3d.html\n :align: center\n\nYou can use this to display a trajectory of camera poses (see Example\n:ref:`sphx_glr__auto_examples_plots_plot_camera_trajectory.py`).\n\n.. figure:: ../_auto_examples/plots/images/sphx_glr_plot_camera_trajectory_001.png\n :target: ../_auto_examples/plots/plot_camera_trajectory.html\n :align: center\n" }, { "path": "doc/source/user_guide/euler_angles.rst", "content": ".. _euler_angles:\n\n============\nEuler Angles\n============\n\nSince Euler angles [1]_ are an intuitive way to specify a rotation in 3D, they\nare often exposed at user interfaces. However, there are 24 different\nconventions that could be used. Furthermore, you have to find out whether\ndegrees or radians are used to express the angles (we will only use\nradians in pytransform3d).\n\n.. figure:: ../_auto_examples/plots/images/sphx_glr_plot_euler_angles_001.png\n :target: ../_auto_examples/plots/plot_euler_angles.html\n :align: center\n\n Here we rotate about the extrinsic (fixed) x-axis, y-axis, and z-axis by\n 90 degrees.\n\n--------------\n24 Conventions\n--------------\n\nEuler angles generally refer to three consecutive rotations about basis\nvectors. There are proper Euler angles for which we can distinguish\n6 conventions: xzx, xyx, yxy, yzy, zyz, and zxz. As you can see, proper\nEuler angles rotate about the same basis vector during the first and last\nrotation and they rotate about another basis vector in the second rotation.\nIn addition, there are Cardan (or Tait-Bryan) angles that rotate about\nthree different basis vectors. There are also 6 conventions:\nxzy, xyz, yxz, yzx, zyx, and zxy.\n\nIf you read about :doc:`transformation_ambiguities`, you know that the\norder in which we concatenate rotation matrices matters. We can make\nextrinsic rotations, in which we rotate about basis vectors of a fixed\nframe, and we can make intrinsic rotations, in which we rotate about\nbasis vectors of the new, rotated frame. This increases the amount of\npossible conventions to :math:`2 (6 + 6) = 24` (if we only allow active\nrotation matrices).\n\n---------------\nRange of Angles\n---------------\n\nEuler angles rotate about three basis vectors by the angles\n:math:`\\alpha`, :math:`\\beta`, and :math:`\\gamma`. If we want to find the\nEuler angles that correspond to one rotation matrix :math:`\\boldsymbol{R}`,\nthere is an infinite number of solutions because we can always add or subtract\n:math:`2\\pi` to one of the angles and get the same result. In addition,\nfor proper Euler angles\n\n.. math::\n\n \\boldsymbol{R}(\\alpha, \\beta, \\gamma) = \\boldsymbol{R}(\\alpha + \\pi, -\\beta, \\gamma - \\pi).\n\nFor Cardan angles\n\n.. math::\n\n \\boldsymbol{R}(\\alpha, \\beta, \\gamma) = \\boldsymbol{R}(\\alpha + \\pi, \\pi - \\beta, \\gamma - \\pi).\n\nFor this reason the proper Euler angles are typically restricted to\n\n.. math::\n\n -\\pi \\leq \\alpha < \\pi, \\qquad 0 \\leq \\beta \\leq \\pi, \\qquad -\\pi \\leq \\gamma < \\pi\n\nand Cardan angles are usually restricted to\n\n.. math::\n\n -\\pi \\leq \\alpha < \\pi, \\qquad -\\frac{\\pi}{2} \\leq \\beta \\leq \\frac{\\pi}{2}, \\qquad -\\pi \\leq \\gamma < \\pi\n\nto make these representations unique (using\n:func:`~pytransform3d.rotations.norm_euler`).\n\nAn alternative convention limits the range of :math:`\\alpha` and :math:`\\gamma`\nto :math:`\\left[0, 2 \\pi\\right)`.\n\n-----------\nGimbal Lock\n-----------\n\nThe special case of a so-called gimbal lock occurs when the second angle\n:math:`\\beta` is at one of its limits. In this case the axis of rotation\nfor the first and last rotation are either the same or exactly opposite,\nthat is, an infinite number of angles :math:`\\alpha` and :math:`\\gamma`\nwill represent the same rotation even though we restricted their range\nto an interval of length :math:`2\\pi`: either all pairs of angles that\nsatisfy :math:`\\alpha + \\gamma = constant` or all pairs of angles\nthat satisfy :math:`\\alpha - \\gamma = constant`. When we reconstruct\nEuler angles from a rotation matrix, we set one of these angles to 0 to\ndetermine the other. We can check if Euler angles are close to gimbal lock\nwith :func:`~pytransform3d.rotations.euler_near_gimbal_lock`.\n\n-----------\nOther Names\n-----------\n\nThere are also other names for Euler angles. For example, the extrinsic\nxyz Cardan angles can also be called roll, pitch, and yaw (or sometimes\nthe intrinsic convention is used here as well). Roll is a rotation about\nx, pitch is a rotation about y and yaw is a rotation about z.\n\n----------\nReferences\n----------\n\n.. [1] Shuster, M. D. (1993). A Survery of Attitude Representations.\n The Journal of Astronautical Sciences, 41(4), pp. 475-476.\n http://malcolmdshuster.com/Pub_1993h_J_Repsurv_scan.pdf\n" }, { "path": "doc/source/user_guide/index.rst", "content": "==========\nUser Guide\n==========\n\n.. toctree::\n :numbered:\n :maxdepth: 2\n\n introduction\n rotations\n transformations\n transformation_ambiguities\n euler_angles\n transformation_modeling\n transform_manager\n transformation_over_time\n uncertainty\n camera\n" }, { "path": "doc/source/user_guide/introduction.rst", "content": "========================================\nIntroduction to 3D Rigid Transformations\n========================================\n\n------\nBasics\n------\n\nFollowing Waldron and Schmiedeler [1]_, we define the basic terms that we use\nthroughout this user guide.\n\n.. list-table::\n :widths: 15 35 15 35\n\n * - .. image:: ../_static/position.png\n - **Position** of a rigid body in 3D Euclidean space is expressed as a 3D\n vector.\n - .. image:: ../_static/translation.png\n - **Translation** is a displacement, in which points move along parallel\n lines by the same distance.\n * - .. image:: ../_static/frame.png\n - **Orientation** of a rigid body in 3D Euclidean space is defined by a\n set of 3 orthogonal basis vectors.\n - .. image:: ../_static/rotation.png\n - **Rotation** is a displacement, in which points move about a rotation\n axis through the origin of the reference frame (fixed point) along a\n circle by the same angle.\n * - .. image:: ../_static/position.png\n :width: 30%\n :align: center\n .. image:: ../_static/frame.png\n :width: 30%\n :align: center\n - **Pose** is a combination of position and orientation.\n - .. image:: ../_static/translation.png\n :width: 30%\n :align: center\n .. image:: ../_static/rotation.png\n :width: 30%\n :align: center\n - A (proper) **rigid transformation** is a combination of translation and\n rotation.\n\n------\nFrames\n------\n\nA (coordinate reference) frame in 3D Euclidean space is defined by an origin\n(position) and 3 orthogonal basis vectors (orientation) and it is attached to\na rigid body. The pose (position and orientation) of a rigid body (i.e., of\nits frame) is always expressed with respect to another frame.\n\n.. _Frame Notation:\n\n--------------\nFrame Notation\n--------------\n\nNotation is important for describing and understanding of physical quantities\nin 3D. Furgale [2]_ [3]_ presents one of the most consistent and clear\napproaches and we use it here.\n\nFor physical quantities we use the notation :math:`_{A}\\boldsymbol{x}_{BC}`,\nwhere :math:`\\boldsymbol{x}` is a physical quantity of frame C with\nrespect to frame B expressed in frame A. For example,\n:math:`_{A}\\boldsymbol{t}_{BC}` is the translation of C with respect to B\nmeasured in A or :math:`_{A}\\boldsymbol{\\omega}_{BC}` is the\norientation vector of C with respect to B measured in A.\n\nSince :math:`_A\\boldsymbol{t}_{BC}` represents a vector or translation from\nframe B to frame C expressed in frame A, the position of a point :math:`P`\nwith respect to a frame A in three-dimensional space can be defined by\n:math:`_A\\boldsymbol{p} := _A\\boldsymbol{t}_{AP}`.\n\nWhen we define a mapping from some frame A to another frame B that can be\nexpressed as a matrix multiplication, we use the notation\n:math:`\\boldsymbol{M}_{BA}` for the corresponding matrix. We can read this\nfrom right to left as a matrix that maps from frame A to frame B through\nmultiplication, for example, when we want to transform a point by\n\n.. math::\n\n _B\\boldsymbol{p} = \\boldsymbol{M}_{BA} {_A\\boldsymbol{p}}\n\n------------------------------------\nDuality of Transformations and Poses\n------------------------------------\n\nWe can use a transformation matrix :math:`\\boldsymbol{T}_{BA}` that represents\na transformation from frame A to frame B to represent the pose (position and\norientation) of frame A in frame B (if we use the active transformation\nconvention; see :ref:`transformation_ambiguities` for details). This is just\na different interpretation of the same matrix and similar to our interpretation\nof a vector from A to P :math:`_A\\boldsymbol{t}_{AP}` as a point\n:math:`_A\\boldsymbol{p}`.\n\n---------------\nRepresentations\n---------------\n\nAt least six numbers are required to express the pose of a rigid body or a\ntransformation between two frames, but there are also redundant\nrepresentations.\nWe can use many different representations of rotation and / or translation.\nHere is an overview of the representations that are available in pytransform3d.\nAll representations are stored in NumPy arrays, of which the corresponding\nshape is shown in this figure. You will find more details on these\nrepresentations on the following pages.\n\n\n.. figure:: ../_static/overview.png\n :alt: Overview of representations for rotations and transformations in 3D.\n :align: center\n :width: 100%\n\n----------\nReferences\n----------\n\n.. [1] Waldron, K., Schmiedeler, J. (2008). Kinematics. In: Siciliano, B., Khatib,\n O. (eds) Springer Handbook of Robotics. Springer, Berlin, Heidelberg.\n https://doi.org/10.1007/978-3-540-30301-5_2\n\n.. [2] Furgale, P. (2014). Representing Robot Pose: The good, the bad, and the\n ugly (slides). In What Sucks in Robotics and How to Fix It: Lessons Learned\n from Building Complex Systems (ICRA workshop). Note: these slides seem to be\n lost, but the blog below conveys the same message.\n http://static.squarespace.com/static/523c5c56e4b0abc2df5e163e/t/53957839e4b05045ad65021d/1402304569659/Workshop+-+Rotations_v102.key.pdf\n\n.. [3] Furgale, P. (2014). Representing Robot Pose: The good, the bad, and the\n ugly (blog).\n https://web.archive.org/web/20220420162355/http://paulfurgale.info/news/2014/6/9/representing-robot-pose-the-good-the-bad-and-the-ugly\n" }, { "path": "doc/source/user_guide/rotations.rst", "content": "===================\nSO(3): 3D Rotations\n===================\n\nThe group of all rotations in the 3D Cartesian space is called :math:`SO(3)`\n(SO: special orthogonal group). It is typically represented by 3D rotation\nmatrices [7]_. The minimum number of components that are required to describe\nany rotation from :math:`SO(3)` is 3. However, there is no representation that\nis non-redundant, continuous, and free of singularities.\n\n.. figure:: ../_static/rotations.png\n :alt: Rotations\n :width: 50%\n :align: center\n\n Overview of the representations and the conversions between them that are\n available in pytransform3d.\n\nNot all representations support all operations directly without conversion to\nanother representation. The following table is an overview. If the operation\nis not implemented in pytransform3d then it is shown in brackets.\n\n+----------------------------------------+---------------+--------------------+---------------+-----------------------+-----------------+\n| Representation | Inverse | Rotation of vector | Concatenation | Interpolation | Renormalization |\n+========================================+===============+====================+===============+=======================+=================+\n| Rotation matrix | Transpose | Yes | Yes | SLERP | Required |\n| :math:`\\pmb{R}` | | | | | |\n+----------------------------------------+---------------+--------------------+---------------+-----------------------+-----------------+\n| Axis-angle | Negative axis | No | No | SLERP | Not necessary |\n| :math:`(\\hat{\\pmb{\\omega}}, \\theta)` | | | | | |\n+----------------------------------------+---------------+--------------------+---------------+-----------------------+-----------------+\n| Rotation vector | Negative | No | No | SLERP / `(2)` / `(3)` | Not required |\n| :math:`\\pmb{\\omega}` | | | | | |\n+----------------------------------------+---------------+--------------------+---------------+-----------------------+-----------------+\n| Logarithm of rotation | Negative | No | No | SLERP / `(2)` / `(3)` | Not required |\n| :math:`\\left[\\pmb{\\omega}\\right]` | | | | | |\n+----------------------------------------+---------------+--------------------+---------------+-----------------------+-----------------+\n| Quaternion | Conjugate | Yes | Yes | SLERP | Required |\n| :math:`\\pmb{q}` | | | | | |\n+----------------------------------------+---------------+--------------------+---------------+-----------------------+-----------------+\n| Rotor | Reverse | Yes | Yes | SLERP | Required |\n| :math:`R` | | | | | |\n+----------------------------------------+---------------+--------------------+---------------+-----------------------+-----------------+\n| Euler angles | `(1)` | No | No | No | Not necessary |\n| :math:`(\\alpha, \\beta, \\gamma)` | | | | | |\n+----------------------------------------+---------------+--------------------+---------------+-----------------------+-----------------+\n| Modified Rodrigues parameters | Negative | No | Yes | No | Not required |\n| :math:`\\pmb{\\psi}` | | | | | |\n+----------------------------------------+---------------+--------------------+---------------+-----------------------+-----------------+\n\nFootnotes:\n\nSLERP means Spherical Linear intERPolation. This can either be implemented\ndirectly for two instances of the representation or sometimes involves a\nconversion to a rotation vector that represents the difference of the two\norientations.\n\n`(1)` Inversion of Euler angles in convention ABC (e.g., xyz) requires using\nanother convention CBA (e.g., zyx), reversing the order of angles, and taking\nthe negative of these.\n\n`(2)` Linear interpolation is approximately correct for small differences.\n\n`(3)` Fractions of this representation represent partial rotations, but\na conversion to another representation is required to interpolate between\norientations.\n\n---------------\nRotation Matrix\n---------------\n\nOne of the most practical representations of orientation is a rotation matrix\n\n.. math::\n\n \\boldsymbol R =\n \\left( \\begin{array}{ccc}\n r_{11} & r_{12} & r_{13}\\\\\n r_{21} & r_{22} & r_{23}\\\\\n r_{31} & r_{32} & r_{33}\\\\\n \\end{array} \\right)\n \\in SO(3).\n\nNote that this is a non-minimal representation for orientations because we\nhave 9 values but only 3 degrees of freedom. This is because\n:math:`\\boldsymbol R` is orthonormal, which results in 6 constraints\n(enforced with :func:`~pytransform3d.rotations.norm_matrix`\nand checked with\n:func:`~pytransform3d.rotations.matrix_requires_renormalization` or\n:func:`~pytransform3d.rotations.check_matrix`):\n\n* column vectors must have unit norm (3 constraints)\n* and must be orthogonal to each other (3 constraints)\n\nA more compact representation of these constraints is\n:math:`\\boldsymbol R^T \\boldsymbol R = \\boldsymbol I\n\\Leftrightarrow \\boldsymbol R^T = \\boldsymbol R^{-1}`.\n\nIn addition, :math:`\\det(\\boldsymbol R) = 1` because we use right-handed\ncoordinate system (:math:`\\det(\\boldsymbol R) = -1` for left-handed\ncoordinate systems).\n\nHence, the group :math:`SO(3)` is defined as\n\n.. math::\n\n SO(3) = \\{\\boldsymbol{R} \\in \\mathbb{R}^{3 \\times 3} |\n \\boldsymbol{R}\\boldsymbol{R}^T = \\boldsymbol{I},\n \\det(\\boldsymbol{R}) = 1\\}.\n\npytransform3d uses a numpy array of shape (3, 3) to represent rotation\nmatrices and typically we use the variable name R for a rotation matrix.\n\n.. warning::\n\n There are two conventions on how to interpret rotations: active\n or passive rotation. The standard in pytransform3d is an active rotation.\n\nWe can use a rotation matrix :math:`\\boldsymbol R_{BA}` to transform a point\n:math:`_A\\boldsymbol{p}` from frame :math:`A` to frame :math:`B`.\n\n.. warning::\n\n There are two different conventions on how to use rotation matrices to\n apply a rotation to a vector. We can either (pre-)multiply the rotation\n matrix to a column vector from the left side or we can (post-)multiply it\n to a row vector from the right side.\n We will use the **pre-multiplication** convention.\n\nThis means that we rotate a point :math:`_A\\boldsymbol{p}` by\n\n.. math::\n\n _B\\boldsymbol{p} = \\boldsymbol{R}_{BAA} \\boldsymbol{p}\n\nThis is called **linear map**.\n\nNote that with our index notation (as explained in :ref:`Frame Notation`) and\nthese conventions, the second index of the rotation matrix and the left index\nof the point have to be the same (:math:`A` in this example). The rotation is\napplied incorrectly if this is not the case.\n\n*Each column* of a rotation matrix :math:`\\boldsymbol{R}_{BA}` is a basis\nvector of frame :math:`A` with respect to frame :math:`B`. We can plot the\nbasis vectors of an orientation to visualize it. Here, we can see orientation\nrepresented by the rotation matrix\n\n.. math::\n\n \\boldsymbol R =\n \\left( \\begin{array}{ccc}\n 1 & 0 & 0\\\\\n 0 & 1 & 0\\\\\n 0 & 0 & 1\\\\\n \\end{array} \\right).\n\n.. plot::\n :include-source:\n\n from pytransform3d.rotations import plot_basis\n plot_basis()\n\n.. note::\n\n When plotting basis vectors, it is a convention to use red for the x-axis,\n green for the y-axis and blue for the z-axis (RGB for xyz).\n\nWe can easily chain multiple rotations: we can apply the rotation defined\nby :math:`\\boldsymbol R_{CB}` after the rotation :math:`\\boldsymbol R_{BA}`\nby applying the rotation\n\n.. math::\n\n \\boldsymbol R_{CA} = \\boldsymbol R_{CB} \\boldsymbol R_{BA}.\n\nNote that the indices have to align again. Otherwise rotations are not applied\nin the correct order.\n\n.. warning::\n\n There are two different conventions on how to concatenate rotation\n matrices. Suppose we have a rotation matrix :math:`R_1` and another matrix\n :math:`R_2` and we want to first rotate by :math:`R_1` and then by\n :math:`R_2`. If we want to apply both rotations in global coordinates, we\n have to concatenate them with :math:`R_2 \\cdot R_1`. We can also express\n the second rotation in terms of a local, body-fixed coordinates by\n :math:`R_1 \\cdot R_2`, which means :math:`R_1` defines new coordinates in\n which :math:`R_2` is applied. Note that this applies to both\n passive and active rotation matrices.\n\nThe easiest way to construct rotation matrices is through rotations about the\nbasis vectors with :func:`~pytransform3d.rotations.active_matrix_from_angle`.\nMultiple rotation matrices that were constructed like this can be concatenated.\nThis will be done, for instance, to obtain rotation matrices from Euler angles\n(see :doc:`euler_angles`).\n\n**Pros**\n\n* Supported operations: all except interpolation.\n* Interpretation: each column is a basis vector.\n* Singularities: none.\n* Ambiguities: none that are specific for rotation matrices.\n\n**Cons**\n\n* Representation: 9 values for 3 degrees of freedom.\n* Renormalization: expensive in comparison to quaternions.\n\n----------\nAxis-Angle\n----------\n\n.. figure:: ../_auto_examples/plots/images/sphx_glr_plot_axis_angle_001.png\n :target: ../_auto_examples/plots/plot_axis_angle.html\n :width: 50%\n :align: center\n\nEach rotation can be represented by a single rotation about one axis.\nThe axis can be represented as a three-dimensional unit vector and the angle\nby a scalar:\n\n.. math::\n\n \\left( \\hat{\\boldsymbol{\\omega}}, \\theta \\right) = \\left( \\left( \\begin{array}{c}\\omega_x\\\\\\omega_y\\\\\\omega_z\\end{array} \\right), \\theta \\right)\n\npytransform3d uses a numpy array of shape (4,) for the axis-angle\nrepresentation of a rotation, where the first 3 entries correspond to the\nunit axis of rotation and the fourth entry to the rotation angle in\nradians, and typically we use the variable name a.\n\nNote that the axis-angle representation has a singularity at\n:math:`\\theta = 0` as there is an infinite number of rotation axes that\nrepresent the identity rotation in this case. However, we can modify the\nrepresentation to avoid this singularity.\n\n---------------\nRotation Vector\n---------------\n\nSince :math:`||\\hat{\\boldsymbol{\\omega}}|| = 1`, it is possible to write this\nin a more compact way as a rotation vector [2]_\n\n.. math::\n\n \\boldsymbol{\\omega} = \\hat{\\boldsymbol{\\omega}} \\theta \\in \\mathbb{R}^3.\n\nIn code, we call this the compact axis-angle representation.\npytransform3d uses a numpy array of shape (3,) for the compact axis-angle\nrepresentation of a rotation and typically it uses the variable name a.\n\nWe can also refer to this representation as **exponential coordinates of\nrotation** [5]_. We can represent angular velocity as\n:math:`\\hat{\\boldsymbol{\\omega}} \\dot{\\theta}`\nand angular acceleration as\n:math:`\\hat{\\boldsymbol{\\omega}} \\ddot{\\theta}` so that we can easily do\ncomponent-wise integration and differentiation with this representation.\n\n**Pros**\n\n* Representation: minimal.\n* Supported operations: interpolation; can also represent angular velocity and\n acceleration.\n\n**Cons**\n\n* Ambiguities: an angle of 0 and any multiple of :math:`2\\pi` represent\n the same orientation (can be avoided with\n :func:`~pytransform3d.rotations.norm_compact_axis_angle`, which introduces\n discontinuities); when :math:`\\theta = \\pi`, the axes\n :math:`\\hat{\\boldsymbol{\\omega}}` and :math:`-\\hat{\\boldsymbol{\\omega}}`\n represent the same rotation.\n* Supported operations: concatenation and transformation of vectors requires\n conversion to another representation.\n\n---------------------\nLogarithm of Rotation\n---------------------\n\nIn addition, we can represent :math:`\\hat{\\boldsymbol{\\omega}} \\theta` by\nthe cross-product matrix (:func:`~pytransform3d.rotations.cross_product_matrix`)\n\n.. math::\n\n \\left[\\hat{\\boldsymbol{\\omega}}\\right] \\theta\n =\n \\left(\n \\begin{matrix}\n 0 & -\\omega_3 & \\omega_2\\\\\n \\omega_3 & 0 & -\\omega_1\\\\\n -\\omega_2 & \\omega_1 & 0\\\\\n \\end{matrix}\n \\right)\n \\theta\n \\in so(3)\n \\subset \\mathbb{R}^{3 \\times 3},\n\nwhere :math:`\\left[\\hat{\\boldsymbol{\\omega}}\\right] \\theta` is the matrix\nlogarithm of a rotation matrix and :math:`so(3)` is the Lie algebra of\nthe Lie group :math:`SO(3)`.\n\n-----------\nQuaternions\n-----------\n\nQuaternions are represented by a scalar / real part :math:`w`\nand a vector / imaginary part\n:math:`x \\boldsymbol{i} + y \\boldsymbol{j} + z \\boldsymbol{k}`.\n\n.. math::\n\n \\boldsymbol{q} = w + x \\boldsymbol{i} + y \\boldsymbol{j} + z \\boldsymbol{k}\n \\in \\mathbb{H}\n\n.. warning::\n\n There are two different quaternion conventions: Hamilton's convention\n defines :math:`ijk = -1` and the Shuster or JPL convention (from NASA's\n Jet Propulsion Laboratory, JPL) defines :math:`ijk = 1` [1]_.\n These two conventions result in different multiplication operations and\n conversions to other representations. We use Hamilton's convention.\n\nRead `this paper `_ for details about\nthe two conventions and why Hamilton's convention should be used. Section VI A\ngives further useful hints to identify which convention is used.\n\nThe unit quaternion space :math:`S^3` can be used to represent\norientations with an encoding based on the rotation axis and angle.\nA rotation quaternion is a four-dimensional unit vector (versor)\n:math:`\\boldsymbol{\\hat{q}}`.\nThe following equation describes its relation to axis-angle notation.\n\n.. math::\n\n \\boldsymbol{\\hat{q}} =\n \\left( \\begin{array}{c} w\\\\ x\\\\ y\\\\ z\\\\ \\end{array} \\right) =\n \\left( \\begin{array}{c}\n \\cos \\frac{\\theta}{2}\\\\\n \\omega_x \\sin \\frac{\\theta}{2}\\\\\n \\omega_y \\sin \\frac{\\theta}{2}\\\\\n \\omega_z \\sin \\frac{\\theta}{2}\\\\\n \\end{array} \\right)\n =\n \\left( \\begin{array}{c}\n \\cos \\frac{\\theta}{2}\\\\\n \\hat{\\boldsymbol{\\omega}} \\sin \\frac{\\theta}{2}\\\\\n \\end{array} \\right)\n\npytransform3d uses a numpy array of shape (4,) for quaternions and\ntypically we use the variable name q.\n\n.. warning::\n\n The scalar component :math:`w` of a quaternion is sometimes the first\n element and sometimes the last element of the versor. We will use\n the first element to store the scalar component.\n\nSince the other convention is also used often, pytransform3d provides the\nfunctions :func:`~pytransform3d.rotations.quaternion_wxyz_from_xyzw` and\n:func:`~pytransform3d.rotations.quaternion_xyzw_from_wxyz` for conversion.\n\n.. warning::\n\n The *antipodal* unit quaternions :math:`\\boldsymbol{\\hat{q}}` and\n :math:`-\\boldsymbol{\\hat{q}}`\n (:func:`~pytransform3d.rotations.quaternion_double`) represent the same\n rotation (double cover). This must be considered during operations like\n interpolation, distance calculation, or (approximate) equality checks.\n\nThe quaternion product\n(:func:`~pytransform3d.rotations.concatenate_quaternions`) can be used to\nconcatenate rotations like the matrix product can be used to concatenate\nrotations represented by rotation matrices.\n\nThe inverse of the rotation represented by the unit quaternion\n:math:`\\boldsymbol{q}` is represented by the conjugate\n:math:`\\boldsymbol{q}^*` (:func:`~pytransform3d.rotations.q_conj`).\n\nWe can rotate a vector by representing it as a pure quaternion (i.e., with\na scalar part of 0) and then left-multiplying the quaternion and\nright-multiplying its conjugate\n:math:`\\boldsymbol{q}\\boldsymbol{v}\\boldsymbol{q}^*`\nwith the quaternion product (:func:`~pytransform3d.rotations.q_prod_vector`).\n\n**Pros**\n\n* Representation: compact.\n* Renormalization: checked with\n :func:`~pytransform3d.rotations.quaternion_requires_renormalization`;\n cheap in comparison to rotation matrices (); less susceptible to round-off\n errors than matrix representation.\n* Discontinuities: none.\n* Computational efficiency: the quaternion product is cheaper than the matrix\n product.\n* Singularities: none.\n\n**Cons**\n\n* Interpretation: not straightforward.\n* Ambiguities: double cover.\n\n------------\nEuler Angles\n------------\n\nA complete rotation can be split into three rotations around basis vectors.\npytransform3d uses a numpy array of shape (3,) for Euler angles, where\neach entry corresponds to a rotation angle in radians around one basis\nvector. The basis vector that will be used and the order of rotation\nis defined by the convention that we use. See :doc:`euler_angles` for more\ninformation.\n\n.. warning::\n\n There are 24 different conventions for defining euler angles. There are\n 12 different valid ways to sequence rotation axes that can be interpreted\n as extrinsic or intrinsic rotations: XZX, XYX, YXY, YZY, ZYZ, ZXZ, XZY,\n XYZ, YXZ, YZX, ZYX, and ZXY.\n\n**Pros**\n\n* Representation: minimal.\n* Interpretation: easy to interpret for users (when the convention is clearly\n defined) in comparison to axis-angle or quaternions.\n\n**Cons**\n\n* Ambiguities: 24 different conventions, infinitely many Euler angles\n represent the same rotation without proper limits for the angles.\n* Singularity: gimbal lock.\n* Supported operations: all operations require conversion to another\n representation.\n\n\n------\nRotors\n------\n\n.. figure:: ../_auto_examples/plots/images/sphx_glr_plot_bivector_001.png\n :target: ../_auto_examples/plots/plot_bivector.html\n :width: 70%\n :align: center\n\nRotors and quaternions are very similar concepts in 3D. However, rotors are\nmore general as they can be extended to more dimensions [3]_ [4]_.\n\nThe concept of a quaternion builds on the axis-angle representation, in\nwhich we rotate by an angle about a rotation axis (see black arrow in the\nillustration above). The axis can be computed from the cross product of two\nvectors (gray arrows). A rotor builds on a plane-angle representation, in which\nwe rotate with a given direction by an angle in a plane (indicated by gray\narea). The plane can be computed from the wedge product :math:`a \\wedge b` (see\n:func:`~pytransform3d.rotations.wedge`) of two vectors :math:`a` and :math:`b`,\nwhich is a so-called bivector. Although both approaches might seem different,\nin 3D they operate with exactly the same numbers in exactly the same way.\n\n.. warning::\n\n The rotors :math:`R` and :math:`-R` represent exactly the same rotation.\n\nA rotor is a unit multivector\n\n.. math::\n\n R = (a, b_{yz}, b_{zx}, b_{xy})\n\nthat consists of a scalar :math:`a` and a bivector\n:math:`(b_{yz}, b_{zx}, b_{xy})`. The components of a bivector constructed\nby the wedge product of two vectors can be interpreted as the area of the\nparallelogram formed by the two vectors projected on the three basis planes\nyz, zx, and xy (see illustration above). These values also correspond to the\nx-, y-, and z-components of the cross product of the two vectors, which allows\ntwo different interpretations of the same numbers from which we can then derive\nquaternions on the one hand and rotors on the other hand.\n\n.. warning::\n\n In pytransform3d our convention is that we organize the components of a\n rotor in exactly the same way as we organize the components of the\n equivalent quaternion. There are other conventions. It is not just possible\n to change the order of the scalar and the bivector (similar to a\n quaterion), but also to change the order of bivector components.\n\nPros and cons for rotors are the same as for quaternions as they have the\nsame representation in 3D.\n\n-----------------------------\nModified Rodrigues Parameters\n-----------------------------\n\nAnother minimal representation of rotation are modified Rodrigues parameters\n(MRP) [6]_ [8]_\n\n.. math::\n\n \\boldsymbol{\\psi} = \\tan \\left(\\frac{\\theta}{4}\\right)\n \\hat{\\boldsymbol{\\omega}}\n\nThis representation is similar to the compact axis-angle representation.\nHowever, the angle of rotation is converted to :math:`\\tan(\\frac{\\theta}{4})`.\nHence, there is an easy conversion from unit quaternions to MRP\n(:func:`~pytransform3d.rotations.mrp_from_quaternion`):\n\n.. math::\n\n \\boldsymbol{\\psi} = \\frac{\n \\left( \\begin{array}{c} x\\\\ y\\\\ z\\\\ \\end{array} \\right)}{1 + w}\n\ngiven some quaternion with a scalar :math:`w` and a vector\n:math:`\\left(x, y, z \\right)^T`.\n\npytransform3d uses a numpy array of shape (3,) for modified Rodrigues\nparameters.\n\n.. warning::\n\n MRPs have a singuarity at :math:`2 \\pi` (see\n :func:`~pytransform3d.rotations.mrp_near_singularity`) which we can avoid\n by ensuring the angle of rotation does not exceed :math:`\\pi` (with\n :func:`~pytransform3d.rotations.norm_mrp`).\n\n.. warning::\n\n MRPs have two representations for the same rotation:\n :math:`\\boldsymbol{\\psi}` and :math:`-\\frac{1}{||\\boldsymbol{\\psi}||^2}\n \\boldsymbol{\\psi}` (:func:`~pytransform3d.rotations.mrp_double`) represent\n the same rotation and correspond to two antipodal unit quaternions [8]_.\n\n**Pros**\n\n* Representation: minimal.\n\n**Cons**\n\n* Interpretation: not straightforward.\n* Singularity: at :math:`\\theta = 2 \\pi`.\n* Ambiguity: double cover.\n* Supported operations: transformation of vectors requires conversion to\n another representation.\n\n----------\nReferences\n----------\n\n.. [1] Sommer, H., Gilitschenski, I., Bloesch, M., Weiss, S., Siegwart, R., Nieto,\n J. (2018). Why and How to Avoid the Flipped Quaternion Multiplication.\n Aerospace, 5(3), pp. 2226-4310, doi: 10.3390/aerospace5030072.\n https://arxiv.org/pdf/1801.07478.pdf\n.. [2] Gehring, C., Bellicoso, C. D., Bloesch, M., Sommer, H., Fankhauser, P.,\n Hutter, M., Siegwart, R. (2024). Kindr cheat sheet.\n https://github.com/ANYbotics/kindr/blob/master/doc/cheatsheet/cheatsheet_latest.pdf\n.. [3] ten Bosch, M. (2020). Let's remove Quaternions from every 3D Engine.\n https://marctenbosch.com/quaternions/\n.. [4] Doran, C. (2015). Applications of Geometric Algebra.\n http://geometry.mrao.cam.ac.uk/wp-content/uploads/2015/02/01ApplicationsI.pdf\n.. [5] Dai, J. S. (2015). Euler–Rodrigues formula variations, quaternion\n conjugation and intrinsic connections, Mechanism and Machine Theory, 92,\n pp. 144-152, doi: 10.1016/j.mechmachtheory.2015.03.004.\n https://doi.org/10.1016/j.mechmachtheory.2015.03.004\n.. [6] Terzakis, G., Lourakis, M., Ait-Boudaoud, D. (2017). Modified Rodrigues\n Parameters: An Efficient Representation of Orientation in 3D Vision and\n Graphics. J Math Imaging Vis, 60, pp. 422-442,\n doi: 10.1007/s10851-017-0765-x.\n.. [7] Hauser, K.: Robotic Systems (draft),\n http://motion.pratt.duke.edu/RoboticSystems/3DRotations.html\n.. [8] Shuster, M. D. (1993). A Survey of Attitude Representations.\n Journal of the Astronautical Sciences, 41, 439-517.\n http://malcolmdshuster.com/Pub_1993h_J_Repsurv_scan.pdf\n" }, { "path": "doc/source/user_guide/transform_manager.rst", "content": "=====================================\nOrganizing Transformations in a Graph\n=====================================\n\nIt is sometimes very difficult to have an overview of all the transformations\nthat are required to calculate another transformation. Suppose you have\na robot with a camera that can observe the robot's end-effector and an object\nthat we want to manipulate. We would like to know the position of the\nend-effector in the object's frame so that we can control it.\n\nWe can organize transformations in a graph like the one shown in the figure\nbelow. Each frame represents a vertex (node) and each transformation an edge\n(connection). Edges in these graphs are directed, but we can create an edge\nin the opposite direction by inverting the transformation. If we want to\ncompute a new edge indicated by the dashed red arrow, we have to use known\ntransformations, that is, we compute the shortest path through the undirected\ngraph, concatenate transformations along the path, and invert transformations\nif required as indicated by the solid red arrow.\n\n.. figure:: ../_static/transform_graph.png\n :width: 35%\n :align: center\n\n Example of a graph representing transformations that connect frames.\n\nThe :class:`~pytransform3d.transform_manager.TransformManager` does this for\nyou. For the given example, this would be the corresponding code\n(see also: :ref:`sphx_glr__auto_examples_plots_plot_transform_manager.py`).\n\n.. literalinclude:: ../../../examples/plots/plot_transform_manager.py\n :language: python\n :lines: 30-35\n\nWe can also export the underlying graph structure as a PNG with\n\n.. code-block:: python\n\n tm.write_png(filename)\n\n.. image:: ../_static/graph.png\n :width: 50%\n :align: center\n\n|\n\nA subclass of :class:`~pytransform3d.transform_manager.TransformManager` is\n:class:`~pytransform3d.urdf.UrdfTransformManager` which can load robot\ndefinitions from `URDF `_ files.\nThe same class can be used to display collision objects or visuals from URDF\nfiles. The library `trimesh `_ will be used to load\nmeshes. Here is a simple example with one visual that is used for two links:\n:ref:`sphx_glr__auto_examples_plots_plot_urdf_with_meshes.py`.\n\nNote that we can use Matplotlib or Open3D to visualize URDFs or any graph of\ntransformations. In previous versions of this library, an example using\npyrender was also included. This has been removed, as there is now another\nsoftware package that does this:\n`urdf_viz `_. urdf_viz uses\npytransform3d to load URDFs and displays them in pyrender.\n" }, { "path": "doc/source/user_guide/transformation_ambiguities.rst", "content": ".. _transformation_ambiguities:\n\n==========================================\nTransformation Ambiguities and Conventions\n==========================================\n\nHeterogeneous software systems that consist of proprietary and open source\nsoftware are often combined when we work with transformations.\nFor example, suppose you want to transfer a trajectory demonstrated by a human\nto a robot. The human trajectory could be measured from an RGB-D camera, fused\nwith IMU sensors that are attached to the human, and then translated to\njoint angles by inverse kinematics. That involves at least three different\nsoftware systems that might all use different conventions for transformations.\nSometimes even one software uses more than one convention.\nThe following aspects are of crucial importance to glue and debug\ntransformations in systems with heterogenous and often incompatible\nsoftware:\n\n* Compatibility: Compatibility between heterogenous softwares is a difficult\n topic. It might involve, for example, communicating between proprietary and\n open source software or different languages.\n* Conventions: Lots of different conventions are used for transformations\n in three dimensions. These have to be determined or specified.\n* Conversions: We need conversions between these conventions to\n communicate transformations between different systems.\n* Visualization: Finally, transformations should be visually verified\n and that should be as easy as possible.\n\npytransform3d assists in solving these issues. This documentation clearly\nstates all of the used conventions, it aims at making conversions between\nrotation and transformation conventions as easy as possible, it is tightly\ncoupled with Matplotlib to quickly visualize (or animate) transformations and\nit is written in Python with few dependencies. Python is a widely adopted\nlanguage. It is used in many domains and supports a wide spectrum of\ncommunication to other software.\n\nThere are lots of ambiguities in the world of transformations. We try to\nexplain them all here.\n\n----------------------------------------------\nRight-handed vs. Left-handed Coordinate System\n----------------------------------------------\n\nWe typically use a right-handed coordinate system, that is, the x-, y- and\nz-axis are aligned in a specific way. The name comes from the way how the\nfingers are attached to the human hand. Try to align your thumb with the\nimaginary x-axis, your index finger with the y-axis, and your middle finger\nwith the z-axis. It is possible to do this with a right hand in a\nright-handed system and with the left hand in a left-handed system.\n\n.. raw:: html\n\n \n \n \n \n \n \n \n \n \n
Right-handedLeft-handed
\n\n.. plot::\n :width: 400px\n\n import numpy as np\n import matplotlib.pyplot as plt\n from pytransform3d.plot_utils import make_3d_axis\n\n\n plt.figure()\n ax = make_3d_axis(1)\n plt.setp(ax, xlim=(-0.05, 1.05), ylim=(-0.05, 1.05), zlim=(-0.05, 1.05),\n xlabel=\"X\", ylabel=\"Y\", zlabel=\"Z\")\n\n basis = np.eye(3)\n for d, c in enumerate([\"r\", \"g\", \"b\"]):\n ax.plot([0.0, basis[0, d]],\n [0.0, basis[1, d]],\n [0.0, basis[2, d]], color=c, lw=5)\n\n plt.show()\n\n.. raw:: html\n\n \n\n.. plot::\n :width: 400px\n\n import numpy as np\n import matplotlib.pyplot as plt\n from pytransform3d.plot_utils import make_3d_axis\n\n\n plt.figure()\n ax = make_3d_axis(1)\n plt.setp(ax, xlim=(-0.05, 1.05), ylim=(-0.05, 1.05), zlim=(-1.05, 0.05),\n xlabel=\"X\", ylabel=\"Y\", zlabel=\"Z\")\n\n basis = np.eye(3)\n basis[:, 2] *= -1.0\n for d, c in enumerate([\"r\", \"g\", \"b\"]):\n ax.plot([0.0, basis[0, d]],\n [0.0, basis[1, d]],\n [0.0, basis[2, d]], color=c, lw=5)\n\n plt.show()\n\n.. raw:: html\n\n
\n\n.. note::\n\n The default in pytransform3d is a right-handed coordinate system.\n\n-------------------------------------------------\nActive (Alibi) vs. Passive (Alias) Transformation\n-------------------------------------------------\n\nThere are two different views of transformations: the active and the passive\nview [1]_ [2]_.\n\n.. image:: ../_static/active_passive.png\n :alt: Passive vs. active transformation\n :align: center\n\nAn active transformation\n\n* changes the physical position of an object\n* can be defined in the absence of a coordinate system or does not change the\n current coordinate system\n* is the only convention used by mathematicians\n\nAnother name for active transformation is alibi transformation.\n\nA passive transformation\n\n* changes the coordinate system in which the object is described\n* does not change the object\n* could be used by physicists and engineers (e.g., roboticists)\n\nAnother name for passive transformation is alias transformation.\n\nThe following illustration compares the active view with the passive view.\nThe position of the data is interpreted in the frame indicated by solid\naxes.\nWe use exactly the same transformation matrix in both plots.\nIn the active view, we see that the transformation is applied to the data.\nThe data is physically moved. The dashed basis represents a frame that is\nmoved from the base frame with the same transformation. The data is\nnow interpreted in the old frame.\nIn a passive transformation, we move the frame with the transformation.\nThe data stays at its original position but it is interpreted in the new\nframe.\n\n.. raw:: html\n\n
\n \n \n \n \n \n \n \n \n
ActivePassive
\n\n.. plot::\n :width: 400px\n\n import numpy as np\n import matplotlib.pyplot as plt\n from pytransform3d.transformations import transform, plot_transform\n from pytransform3d.plot_utils import make_3d_axis, Arrow3D\n\n\n plt.figure()\n ax = make_3d_axis(1)\n plt.setp(ax, xlim=(-1.05, 1.05), ylim=(-0.55, 1.55), zlim=(-1.05, 1.05),\n xlabel=\"X\", ylabel=\"Y\", zlabel=\"Z\")\n ax.view_init(elev=90, azim=-90)\n ax.set_xticks(())\n ax.set_yticks(())\n ax.set_zticks(())\n\n rng = np.random.default_rng(42)\n PA = np.ones((10, 4))\n PA[:, :3] = 0.1 * rng.standard_normal(size=(10, 3))\n PA[:, 0] += 0.3\n PA[:, :3] += 0.3\n\n x_translation = -0.1\n y_translation = 0.2\n z_rotation = np.pi / 4.0\n A2B = np.array([\n [np.cos(z_rotation), -np.sin(z_rotation), 0.0, x_translation],\n [np.sin(z_rotation), np.cos(z_rotation), 0.0, y_translation],\n [0.0, 0.0, 1.0, 0.0],\n [0.0, 0.0, 0.0, 1.0]\n ])\n PB = transform(A2B, PA)\n\n plot_transform(ax=ax, A2B=np.eye(4))\n ax.scatter(PA[:, 0], PA[:, 1], PA[:, 2], c=\"orange\")\n plot_transform(ax=ax, A2B=A2B, ls=\"--\", alpha=0.5)\n ax.scatter(PB[:, 0], PB[:, 1], PB[:, 2], c=\"cyan\")\n\n axis_arrow = Arrow3D(\n [0.7, 0.3],\n [0.4, 0.9],\n [0.2, 0.2],\n mutation_scale=20, lw=3, arrowstyle=\"-|>\", color=\"k\")\n ax.add_artist(axis_arrow)\n\n plt.tight_layout()\n plt.show()\n\n.. raw:: html\n\n \n\n.. plot::\n :width: 400px\n\n import numpy as np\n import matplotlib.pyplot as plt\n from mpl_toolkits.mplot3d import proj3d\n from pytransform3d.transformations import transform, plot_transform\n from pytransform3d.plot_utils import make_3d_axis, Arrow3D\n\n\n plt.figure()\n ax = make_3d_axis(1)\n plt.setp(ax, xlim=(-1.05, 1.05), ylim=(-0.55, 1.55), zlim=(-1.05, 1.05),\n xlabel=\"X\", ylabel=\"Y\", zlabel=\"Z\")\n ax.view_init(elev=90, azim=-90)\n ax.set_xticks(())\n ax.set_yticks(())\n ax.set_zticks(())\n\n rng = np.random.default_rng(42)\n PA = np.ones((10, 4))\n PA[:, :3] = 0.1 * rng.standard_normal(size=(10, 3))\n PA[:, 0] += 0.3\n PA[:, :3] += 0.3\n\n x_translation = -0.1\n y_translation = 0.2\n z_rotation = np.pi / 4.0\n A2B = np.array([\n [np.cos(z_rotation), -np.sin(z_rotation), 0.0, x_translation],\n [np.sin(z_rotation), np.cos(z_rotation), 0.0, y_translation],\n [0.0, 0.0, 1.0, 0.0],\n [0.0, 0.0, 0.0, 1.0]\n ])\n\n plot_transform(ax=ax, A2B=np.eye(4), ls=\"--\", alpha=0.5)\n ax.scatter(PA[:, 0], PA[:, 1], PA[:, 2], c=\"orange\")\n plot_transform(ax=ax, A2B=A2B)\n\n axis_arrow = Arrow3D(\n [0.0, -0.1],\n [0.0, 0.2],\n [0.2, 0.2],\n mutation_scale=20, lw=3, arrowstyle=\"-|>\", color=\"k\")\n ax.add_artist(axis_arrow)\n\n plt.tight_layout()\n plt.show()\n\n.. raw:: html\n\n
\n\nUsing the inverse transformation in the active view gives us exactly the same\nsolution as the original transformation in the passive view and vice versa.\n\nIt is usually easy to determine whether the active or the passive convention\nis used by taking a look at the rotation matrix: when we rotate\ncounter-clockwise by an angle :math:`\\theta` about the z-axis, the following\nrotation matrix is usually used in an active transformation:\n\n.. math::\n\n \\left( \\begin{array}{ccc}\n \\cos \\theta & -\\sin \\theta & 0 \\\\\n \\sin \\theta & \\cos \\theta & 0 \\\\\n 0 & 0 & 1\\\\\n \\end{array} \\right)\n\nIts transformed version is usually used for a passive transformation:\n\n.. math::\n\n \\left( \\begin{array}{ccc}\n \\cos \\theta & \\sin \\theta & 0 \\\\\n -\\sin \\theta & \\cos \\theta & 0 \\\\\n 0 & 0 & 1\\\\\n \\end{array} \\right)\n\n.. warning::\n\n The standard in pytransform3d is an active rotation.\n\n----------------------------------------\nPre-multiply vs. Post-multiply Rotations\n----------------------------------------\n\nThe same point can be represented by a column vector :math:`\\boldsymbol v` or\na row vector :math:`\\boldsymbol w`. A rotation matrix :math:`\\boldsymbol R`\ncan be used to rotate the point by pre-multiplying it to the column vector\n:math:`\\boldsymbol R \\boldsymbol v` or by post-multiplying it to the row\nvector :math:`\\boldsymbol w \\boldsymbol R`. However, for the same rotation\nmatrix, both approaches are inverse:\n:math:`\\boldsymbol R^T \\boldsymbol v = \\boldsymbol w \\boldsymbol R`.\nHence, to achieve the same effect we have to use two different rotation\nmatrices depending on how we multiply them to points.\n\n.. note::\n\n The default in pytransform3d are pre-multiplied rotation matrices.\n\n---------------------------------------\nIntrinsic vs. Extrinsic Transformations\n---------------------------------------\n\nA similar problem occurs when we want to concatenate rotations\nor transformations:\nsuppose we have a rotation matrix :math:`R_1` and another matrix\n:math:`R_2` and we want to first rotate by :math:`R_1` and then by\n:math:`R_2`. If we want to apply both rotations in global coordinates\n(global, space-fixed / extrinsic rotation), we have to concatenate them with\n:math:`R_2 \\cdot R_1`. We can also express the second rotation in terms\nof a local, body-fixed coordinates (local, body-fixed / intrinsic rotation)\nby :math:`R_1 \\cdot R_2`, which means :math:`R_1` defines new coordinates\nin which :math:`R_2` is applied. Note that this applies to both\npassive and active rotation matrices. Specifying this convention is\nparticularly relevant when we deal with Euler angles.\n\nHere is a comparison between various conventions of concatenation.\n\n.. plot:: ../../examples/plots/plot_convention_rotation_global_local.py\n\n.. warning::\n\n There are two conventions on how to concatenate rotations and\n transformations: intrinsic and extrinsic transformation.\n There is no default in pytransform3d but usually the function name\n should tell you which convention the function uses.\n\n----------------------------\nNaming and Frame Conventions\n----------------------------\n\nIn addition to transformation and rotation conventions, there are a lot of\ndifferent naming and frame conventions. Here are some examples.\n\n.. figure:: ../_static/conventions_ship.png\n :alt: Ship conventions\n :align: center\n :width: 50%\n\n For ships we can use the following convention for the body frame: the x-axis\n is called surge, the y-axis sway, and the z-axis heave. The origin of the\n frame is the center of gravity. For the orientation, the names yaw, pitch,\n and roll are used. Sometimes the body frame is rotated by 180 degrees around\n the x-axis, so that the y-axis points to the right side and the z-axis down.\n\n.. figure:: ../_static/conventions_plane.png\n :alt: Aircraft conventions\n :align: center\n :width: 50%\n\n Aircrafts sometimes use the following naming conventions for intrinsic\n rotations around the z-, y'-, and x''-axis. The rotation about the z-axis\n is called heading, rotation about the y'-axis is called elevation, and the\n rotation about the x''-axis is called bank.\n\n\n.. figure:: ../_static/conventions_camera.png\n :alt: Camera conventions\n :align: center\n\n Cameras or similar sensors are sometimes mounted on pan/tilt units.\n Typically, first the pan joint rotates the camera about an axis parallel to\n the y-axis of the camera and the tilt joint rotates the camera about an\n axis parallel to the x-axis of the camera. When the axes of rotation\n and of the camera are not perfectly aligned, the camera will also be\n translated by the rotations.\n\n-----------------------------\nConventions of Other Software\n-----------------------------\n\nThe following is an incomplete list of conventions for representations of\nrotations, orientations, transformations, or poses and coordinate frames\nother software packages use. It illustrates the diversity that you will\nfind when you combine different software systems.\n\n`Blender user interface (computer graphics) `_\n\n* Active rotations\n* Euler angles (are actually Tait-Bryan angles): external rotations, angles in degree\n* Quaternion: scalar first\n\n`XSens MVNX format (motion capture) `_\n\n* Active rotations\n* Conventions for coordinate frames\n * Axis orientation in the world (global): x north, y west, z up (NWU)\n * Axis orientation on body parts: axes are aligned with world axes when\n subject stands in T pose\n * Quaternion and rotation matrix rotate from sensor frame to world frame,\n that is, they represent the orientation of the sensor with respect to\n the world\n* Quaternion: scalar first\n* Euler angles: extrinsic roll-pitch-yaw (xyz) convention\n\n`Bullet (physics engine) `_\n\n* Active rotations\n* Euler angles: extrinsic roll-pitch-yaw (xyz) convention\n (getQuaternionFromEuler from pybullet's API)\n* Quaternion: scalar last and Hamilton multiplication\n\n`Eigen (linear algebra library) `_\n\n* Quaternion\n * Scalar first (constructor) and scalar last (internal)\n * Hamilton multiplication\n\n`Peter Corke's robotics toolbox `_\n\n* Active rotations\n* Euler angles\n * Intrinsic zyz convention\n * Roll-pitch-yaw angles correspond either to intrinsic zyx convention\n (default) or intrinsic xyz convention, which can be selected by a\n parameter\n* Quaternion: scalar first and Hamilton multiplication\n\n`Robot Operating System (ROS) `_ `(REP103) `_\n\n* Active transformations\n* Conventions for coordinate frames\n * Axis orientation on body: x forward, y left, z up\n * Axis orientation in the world: x east, y north, z up (ENU)\n * Axis orientation of optical camera frame (indicated by suffix\n in topic name): z forward, x right, y down\n* Euler angles\n * Active, extrinsic roll-pitch-yaw (xyz) convention (as used, e.g.,\n in origin tag of URDF) can be used\n * In addition, the yaw-pitch-roll (zyx) convention can be used, but\n is discouraged\n* A `PoseStamped `_\n is represented with respect to a `frame_id`\n* When interpreted as active transformation,\n `TransformStamped `_\n represents a transformation from *child frame* to its (parent) *frame*\n* `Quaternion `_:\n scalar last\n\n`Universal Robot user interface `_\n\n* Conventions for coordinate frames\n * Default axis orientation of tool center point: z forward\n (approach direction), x and y axes define the orientation\n with which we approach the target\n* Euler angles: extrinsic roll-pitch-yaw (xyz) convention\n\n----------\nReferences\n----------\n\n.. [1] Selig, J.M. (2006): Active Versus Passive Transformations in Robotics.\n IEEE Robotics and Automation Magazine, 13(1), pp. 79-84, doi:\n 10.1109/MRA.2006.1598057. https://ieeexplore.ieee.org/document/1598057\n\n.. [2] Wikipedia: Active and passive transformation.\n https://en.wikipedia.org/wiki/Active_and_passive_transformation\n" }, { "path": "doc/source/user_guide/transformation_modeling.rst", "content": "========================\nModeling Transformations\n========================\n\nWith many transformations it is not easy to keep track of the right sequence.\nHere are some simple tricks that you can use to keep track of transformations\nand concatenate them in the correct way.\n\n.. image:: ../_static/transformation_modeling.png\n :alt: Three frames\n :align: center\n\n|\n\nWhen modeling transformations in mathematical equations we often sequence them\nfrom right to left (extrinsic convention).\nHere it makes most sense to give names like :math:`\\boldsymbol T_{BA}` for\ntransformations that maps points **from frame** :math:`A` **to frame**\n:math:`B`, so we can easily recognize that\n\n.. math::\n\n \\boldsymbol T_{CA} = \\boldsymbol T_{CB} \\boldsymbol T_{BA}\n\nwhen we read the names of the frames from right to left.\nNow we can use the transformation matrix :math:`\\boldsymbol T_{CA}` to transform\na point :math:`_A\\boldsymbol{p}` from frame :math:`A` to frame\n:math:`C` by multiplication :math:`_C\\boldsymbol{p} = \\boldsymbol{T}_{CA}\\,_A\\boldsymbol{p}`.\n\nThis might look differently in code. Here we should prefer the notation `A2B`\nfor a transformation from frame `A` to frame `B`.\n\n.. code-block::\n\n from pytransform3d.transformations import concat\n A2B = ... # transformation from frame A to frame B\n B2C = ... # transformation from frame B to frame C\n A2C = concat(A2B, B2C)\n\nNow we can verify that the new transformation matrix correctly transforms from\nframe `A` to frame `C` if we read from left to right. The function `concat`\nwill correctly apply the transformation `B2C` after `A2B`. If we want to transform\na point from `C` to `A` we can now use\n\n.. code-block::\n\n from pytransform3d.transformations import vector_to_point, transform\n p_in_A = vector_to_point(...) # point in frame A\n p_in_C = transform(A2C, p_in_A)\n\nThis notation is particuarly effective when we use it in combination with\nlonger, more readable frame names, e.g., ``left_hand_palm`` or ``robot_base``.\n\nFor more complex cases the :class:`~pytransform3d.transform_manager.TransformManager`\ncan help.\n" }, { "path": "doc/source/user_guide/transformation_over_time.rst", "content": ".. _transformations_over_time:\n\n========================================\nGraphs of Time-Dependent Transformations\n========================================\n\nIn applications, where the transformations between coordinate frames are\ndynamic (i.e., changing over time), consider using\n:class:`~pytransform3d.transform_manager.TemporalTransformManager`. In contrast\nto the :class:`~pytransform3d.transform_manager.TransformManager`,\nwhich deals with static transfomations, it provides an interface for the logic\nneeded to interpolate between transformation samples available over time.\n\nWe can visualize the lifetime of two dynamic transformations (i.e., 3\ncoordinate systems) in the figure below. Each circle represents a sample\n(measurement) holding the transformation from the parent to the child frame.\n\n.. figure:: ../_static/tf-trafo-over-time.png\n :width: 60%\n :align: center\n\nA common use case is to transform points originating from system A to system B\nat a specific point in time (i.e., :math:`t_q`, where :math:`q` refers to\nquery). Imagine two moving robots A & B reporting their observations between\neach other.\n\n--------------------------------------\nPreparing the Transformation Sequences\n--------------------------------------\n\nFirst, you need to prepare the transfomation sequences using the \n:class:`~pytransform3d.transform_manager.NumpyTimeseriesTransform` class:\n\n.. literalinclude:: ../../../examples/plots/plot_interpolation_for_transform_manager.py\n :language: python\n :lines: 47-60\n\nIn this example, the screw linear interpolation (ScLERP) will be used\n(which operates on dual quaternions, refer to\n:func:`~pytransform3d.transformations.pq_from_dual_quaternion`).\n\nFor more control, you may want to add your own implementation of the\nabstract class :class:`~pytransform3d.transform_manager.TimeVaryingTransform`.\n\nNext, you need to pass the transformations to an instance of\n:class:`~pytransform3d.transform_manager.TemporalTransformManager`:\n\n.. literalinclude:: ../../../examples/plots/plot_interpolation_for_transform_manager.py\n :language: python\n :lines: 62-65\n\n------------------------------------\nTransform Between Coordinate Systems\n------------------------------------\n\nFinally, you can transform between coordinate systems at a particular time\n:math:`t_q`:\n\n.. literalinclude:: ../../../examples/plots/plot_interpolation_for_transform_manager.py\n :language: python\n :lines: 67-72\n\nThe coordinates of A's origin (blue diamond) transformed to B are visualized\nin the plot below:\n\n.. figure:: ../_auto_examples/plots/images/sphx_glr_plot_interpolation_for_transform_manager_001.png\n :target: ../_auto_examples/plots/plot_interpolation_for_transform_manager.html\n :align: center\n" }, { "path": "doc/source/user_guide/transformations.rst", "content": "=========================\nSE(3): 3D Transformations\n=========================\n\nThe group of all proper rigid transformations (rototranslations) in\n3D Cartesian space is :math:`SE(3)` (SE: special Euclidean group).\nTransformations consist of a rotation and a translation. Those can be\nrepresented in different ways just like rotations can be expressed\nin different ways. The minimum number of components that are required to\ndescribe any transformation from :math:`SE(3)` is 6.\n\n.. figure:: ../_static/transformations.png\n :alt: Conversions between representations of transformations\n :width: 50%\n :align: center\n\n Overview of the representations and the conversions between them that are\n available in pytransform3d.\n\nFor most representations of orientations we can find\nan analogous representation of transformations:\n\n* A **transformation matrix** :math:`\\boldsymbol T` is similar to a rotation\n matrix :math:`\\boldsymbol R`.\n* A **screw axis** :math:`\\mathcal S` is similar to a rotation axis\n :math:`\\hat{\\boldsymbol{\\omega}}`.\n* A **screw matrix** :math:`\\left[\\mathcal{S}\\right]` is similar to\n a cross-product matrix of a unit rotation axis\n :math:`\\left[\\hat{\\boldsymbol{\\omega}}\\right]`.\n* The **logarithm of a transformation** :math:`\\left[\\mathcal{S}\\right] \\theta`\n is similar to a cross-product matrix of the angle-axis representation\n :math:`\\left[\\hat{\\boldsymbol{\\omega}}\\right] \\theta`.\n* The **exponential coordinates** :math:`\\mathcal{S} \\theta` for rigid body\n motions are similar to exponential coordinates\n :math:`\\hat{\\boldsymbol{\\omega}} \\theta` for rotations (compact axis-angle\n representation / rotation vector).\n* A **twist** :math:`\\mathcal V = \\mathcal{S} \\dot{\\theta}` is similar to\n angular velocity :math:`\\hat{\\boldsymbol{\\omega}} \\dot{\\theta}`.\n* A (unit) **dual quaternion**\n :math:`p_w + p_x i + p_y j + p_z k + \\epsilon (q_w + q_x i + q_y j + q_z k)`\n is similar to a (unit) quaternion :math:`w + x i + y j + z k`.\n\nNot all representations support all operations directly without conversion to\nanother representation. The following table is an overview.\n\n+----------------------------------------+------------+--------------------------+---------------+------------------+-----------------+\n| Representation | Inverse | Transformation of vector | Concatenation | Interpolation | Renormalization |\n+========================================+============+==========================+===============+==================+=================+\n| Transformation matrix | Inverse | Yes | Yes | ScLERP `(1)` | Required |\n| :math:`\\pmb{R}` | | | | | |\n+----------------------------------------+------------+--------------------------+---------------+------------------+-----------------+\n| Exponential coordinates | Negative | No | No | ScLERP `(2)` | Not necessary |\n| :math:`\\mathcal{S}\\theta` | | | | | |\n+----------------------------------------+------------+--------------------------+---------------+------------------+-----------------+\n| Logarithm of transformation | Negative | No | No | ScLERP `(2)` | Not necessary |\n| :math:`\\left[\\mathcal{S}\\theta\\right]` | | | | | |\n+----------------------------------------+------------+--------------------------+---------------+------------------+-----------------+\n| Position and quaternion | No | No | No | SLERP `(3)` | Required |\n| :math:`(\\pmb{p}, \\pmb{q})` | | | | | |\n+----------------------------------------+------------+--------------------------+---------------+------------------+-----------------+\n| Dual quaternion | Quaternion | Yes | Yes | ScLERP `(1)` | Required |\n| :math:`\\boldsymbol{\\sigma}` | Conjugate | | | | |\n+----------------------------------------+------------+--------------------------+---------------+------------------+-----------------+\n\n`(1)` ScLERP means Screw Linear intERPolation. This usually requires an\ninternal conversion to exponential coordinates.\n\n`(2)` Fractions of this representation represent partial transformations, but\na conversion to another representation is required to interpolate between\nposes.\n\n`(3)` SLERP means Spherical Linear intERPolation and is applied to the\norientation while the translation is linearly interpolated.\n\n---------------------\nTransformation Matrix\n---------------------\n\nOne of the most convenient ways to represent transformations are\ntransformation matrices. A transformation matrix is a 4x4 matrix of\nthe form\n\n.. math::\n\n \\boldsymbol T =\n \\left( \\begin{array}{cc}\n \\boldsymbol R & \\boldsymbol t\\\\\n \\boldsymbol 0 & 1\\\\\n \\end{array} \\right)\n =\n \\left(\n \\begin{matrix}\n r_{11} & r_{12} & r_{13} & t_1\\\\\n r_{21} & r_{22} & r_{23} & t_2\\\\\n r_{31} & r_{32} & r_{33} & t_3\\\\\n 0 & 0 & 0 & 1\\\\\n \\end{matrix}\n \\right)\n \\in SE(3).\n\nIt is a partitioned matrix with a 3x3 rotation matrix :math:`\\boldsymbol R`\nand a column vector :math:`\\boldsymbol t` that represents the translation.\nIt is also sometimes called the homogeneous representation of a transformation.\nAll transformation matrices of this form generate the special Euclidean group\n:math:`SE(3)`, that is,\n\n.. math::\n\n SE(3) = \\{ \\boldsymbol{T} = \\left(\n \\begin{array}{cc}\n \\boldsymbol{R} & \\boldsymbol{t}\\\\\n \\boldsymbol{0} & 1\n \\end{array}\n \\right) \\in \\mathbb{R}^{4 \\times 4}\n | \\boldsymbol{R} \\in SO(3), \\boldsymbol{t} \\in \\mathbb{R}^3 \\}.\n\npytransform3d uses a numpy array of shape (4, 4) to represent transformation\nmatrices and typically we use the variable name A2B for a transformation\nmatrix, where A corrsponds to the frame from which it transforms and B to\nthe frame to which it transforms.\n\nIt is possible to transform position vectors or direction vectors with it.\nPosition vectors are represented as a column vector\n:math:`\\left( x,y,z,1 \\right)^T`.\nThis will activate the translation part of the transformation in a matrix\nmultiplication (see :func:`~pytransform3d.transformations.vector_to_point`).\nWhen we transform a direction vector, we want to deactivate the translation by\nsetting the last component to zero (see\n:func:`~pytransform3d.transformations.vector_to_direction`):\n:math:`\\left( x,y,z,0 \\right)^T`.\n\nWe can use a transformation matrix :math:`\\boldsymbol T_{BA}` to transform a\npoint :math:`{_A}\\boldsymbol{p}` from frame :math:`A` to frame :math:`B`:\n\n.. math::\n\n \\boldsymbol{T}_{BA} {_A}\\boldsymbol{p} =\n \\left( \\begin{array}{c}\n \\boldsymbol{R}_{BA} {_A}\\boldsymbol{p} + {_B}\\boldsymbol{t}_{BA}\\\\\n 1\\\\\n \\end{array} \\right) =\n {_B}\\boldsymbol{p}.\n\nYou can use :func:`~pytransform3d.transformations.transform` to apply a\ntransformation matrix to a homogeneous vector.\n\n**Pros**\n\n* Supported operations: all except interpolation.\n* Interpretation: each column represents either a basis vector or the\n translation.\n* Singularities: none.\n\n**Cons**\n\n* Rrepresentation: 16 values for 6 degrees of freedom.\n* Renormalization: inherited from rotation matrix.\n\n-----------------------\nPosition and Quaternion\n-----------------------\n\nAn alternative to transformation matrices is the representation in a\n7-dimensional vector that consists of the translation and a rotation\nquaternion:\n\n.. math::\n\n \\left( x, y, z, q_w, q_x, q_y, q_z \\right)^T\n\nThis representation is more compact than a transformation matrix and is\nparticularly useful if you want to represent a sequence of poses in\na 2D array.\n\npytransform3d uses a numpy array of shape (7,) to represent position and\nquaternion and typically we use the variable name pq.\n\n**Pros**\n\n* Representation: compact.\n\n**Cons**\n\n* Supported operation: translation and rotation component are separated and\n have to be handled individually.\n\n----------------\nScrew Parameters\n----------------\n\n.. figure:: ../_auto_examples/plots/images/sphx_glr_plot_screw_001.png\n :target: ../_auto_examples/plots/plot_screw.html\n :width: 70%\n :align: center\n\nJust like any rotation can be expressed as a rotation by an angle about a\n3D unit vector, any transformation (rotation and translation) can be expressed\nby a motion along a screw axis [2]_ [3]_ [4]_. The **screw parameters** that\ndescribe a screw axis include a point vector :math:`\\boldsymbol{q}` through\nwhich the screw axis passes, a (unit) direction vector\n:math:`\\hat{\\boldsymbol{s}}` that\nindicates the direction of the axis, and the pitch :math:`h`. The pitch\nrepresents the ratio of translation and rotation. A screw motion translates\nalong the screw axis and rotates about it.\n\npytransform3d uses two vectors q and `s_axis` of shape (3,) and a scalar\nh to represent the parameters of a screw.\n\n.. image:: ../_static/screw_axis.png\n :alt: Screw axis\n :width: 50%\n :align: center\n\n----------\nScrew Axis\n----------\n\nA **screw axis** is typically represented by\n:math:`\\mathcal{S} = \\left[\\begin{array}{c}\\boldsymbol{\\omega}\\\\\\boldsymbol{v}\\end{array}\\right] \\in \\mathbb{R}^6`,\nwhere either\n\n1. :math:`||\\boldsymbol{\\omega}|| = 1` or\n2. :math:`||\\boldsymbol{\\omega}|| = 0` and :math:`||\\boldsymbol{v}|| = 1`\n (only translation).\n\npytransform3d uses a numpy array of shape (6,) to represent a screw axis\nand typically we use the variable name S or `screw_axis`.\n\nIn case 1, we can compute the screw axis from screw parameters\n:math:`(\\boldsymbol{q}, \\hat{\\boldsymbol{s}}, h)` as\n\n.. math::\n\n \\mathcal{S} = \\left[ \\begin{array}{c}\\hat{\\boldsymbol{s}} \\\\ \\boldsymbol{q} \\times \\hat{\\boldsymbol{s}} + h \\hat{\\boldsymbol{s}}\\end{array} \\right]\n\nIn case 2, :math:`h` is infinite and we directly translate along :math:`\\hat{\\boldsymbol{s}}`.\n\n-----------------------\nExponential Coordinates\n-----------------------\n\nBy multiplication with an additional parameter :math:`\\theta` we can then\ndefine a complete transformation through its exponential coordinates\n:math:`\\mathcal{S} \\theta = \\left[\\begin{array}{c}\\boldsymbol{\\omega}\\theta\\\\\\boldsymbol{v}\\theta\\end{array}\\right] \\in \\mathbb{R}^6`.\nThis is a minimal representation as it only needs 6 values.\n\npytransform3d uses a numpy array of shape (6,) to represent a exponential\ncoordinates of transformation and typically we use the variable name Stheta.\n\n.. warning::\n\n Note that we use the screw theory definition of exponential coordinates\n and :math:`se(3)` (see next section) used by Lynch and Park (2017) [1]_,\n and Corke (2017) [2]_. They separate the parameter :math:`\\theta` from\n the screw axis. Additionally, they use the first three components to encode\n rotation and the last three components to encode translation. There is an\n alternative definition used by Eade (2017) [3]_ and Sola et al. (2018)\n [4]_. They use a different order of the 3D vector components and they do\n not separate :math:`\\theta` from the screw axis in their notation.\n\nWe say exponential coordinates of transformation because there is an\nexponential map :math:`Exp(\\mathcal{S}\\theta) = \\boldsymbol{T}` (see\n:func:`~pytransform3d.transformations.transform_from_exponential_coordinates`)\nthat converts them to a transformation matrix. The inverse operation is the\nlogarithmic map :math:`Log(\\boldsymbol{T}) = \\mathcal{S}\\theta` (see\n:func:`~pytransform3d.transformations.exponential_coordinates_from_transform`).\n\nExponential coordinates can be used to implement screw linear interpolation\n(ScLERP) because we can easily compute fractions of a transformation in\nexponential coordinates. We just have to compute the difference between two\ntransformations\n:math:`\\boldsymbol{T}_{AB} = \\boldsymbol{T}^{-1}_{WA}\\boldsymbol{T}_{WB}`,\nconvert it to exponential coordinates\n:math:`Log(\\boldsymbol{T}_{AB}) = \\mathcal{S}\\theta_{AB}`, compute a fraction\nof it :math:`t\\mathcal{S}\\theta_{AB}` with :math:`t \\in [0, 1]`, and use the\nexponential map to concatenate the fraction of the difference\n:math:`\\boldsymbol{T}_{WA} Exp(t\\mathcal{S}\\theta_{AB})`.\n\n**Pros**\n\n* Representation: minimal.\n* Supported operations: interpolation; can also represent spatial velocity and\n acceleration.\n\n**Cons**\n\n* Supported operations: concatenation and transformation of vectors requires\n conversion to another representation.\n\n---------------------------\nLogarithm of Transformation\n---------------------------\n\nAlternatively, we can represent a screw axis :math:`\\mathcal S` in a matrix\n\n.. math::\n\n \\left[\\mathcal S\\right]\n =\n \\left( \\begin{array}{cc}\n \\left[\\boldsymbol{\\omega}\\right] & \\boldsymbol v\\\\\n \\boldsymbol 0 & 0\\\\\n \\end{array} \\right)\n =\n \\left(\n \\begin{matrix}\n 0 & -\\omega_3 & \\omega_2 & v_1\\\\\n \\omega_3 & 0 & -\\omega_1 & v_2\\\\\n -\\omega_2 & \\omega_1 & 0 & v_3\\\\\n 0 & 0 & 0 & 0\\\\\n \\end{matrix}\n \\right)\n \\in se(3) \\subset \\mathbb{R}^{4 \\times 4}\n\nthat contains the cross-product matrix of its orientation part and its\ntranslation part. This is the **matrix representation of a screw axis** and\nwe will also refer to it as **screw matrix** in the API.\n\npytransform3d uses a numpy array of shape (4, 4) to represent a screw matrix\nand typically we use the variable name `screw_matrix`.\n\nBy multiplication with :math:`\\theta` we can again generate a full\ndescription of a transformation\n:math:`\\left[\\mathcal{S}\\right] \\theta \\in se(3)`, which is the **matrix\nlogarithm of a transformation matrix** and :math:`se(3)` is the Lie\nalgebra of Lie group :math:`SE(3)`.\n\npytransform3d uses a numpy array of shape (4, 4) to represent the logarithm\nof a transformation and typically we use the variable name `transform_log`.\n\nWe say logarithm of transformation because there is an\nexponential map :math:`\\exp(\\left[\\mathcal{S}\\right]\\theta) = \\boldsymbol{T}`\n(see :func:`~pytransform3d.transformations.transform_from_transform_log`)\nthat converts it to a transformation matrix. The inverse operation is the\nlogarithmic map :math:`\\log(\\boldsymbol{T}) = \\left[\\mathcal{S}\\right]\\theta`\n(see :func:`~pytransform3d.transformations.transform_log_from_transform`).\n\nThe logarithm of transformation can be used to implement screw linear\ninterpolation (ScLERP) because we can easily compute fractions of a\ntransformation. We just have to compute the difference between two\ntransformations\n:math:`\\boldsymbol{T}_{AB} = \\boldsymbol{T}^{-1}_{WA}\\boldsymbol{T}_{WB}`,\nconvert it to the logarithm\n:math:`\\log(\\boldsymbol{T}_{AB}) = \\left[\\mathcal{S}\\right]\\theta_{AB}`,\ncompute a fraction of it :math:`t\\left[\\mathcal{S}\\right]\\theta_{AB}` with\n:math:`t \\in [0, 1]`, and use the exponential map to concatenate the fraction\nof the difference\n:math:`\\boldsymbol{T}_{WA} \\exp(t\\left[\\mathcal{S}\\right]\\theta_{AB})`.\n\n-----\nTwist\n-----\n\nWe call spatial velocity (translation and rotation) **twist**. Similarly\nto the matrix logarithm, a twist :math:`\\mathcal{V} = \\mathcal{S} \\dot{\\theta}`\nis described by a screw axis :math:`\\mathcal S` and a scalar\n:math:`\\dot{\\theta}` and\n:math:`\\left[\\mathcal{V}\\right] = \\left[\\mathcal{S}\\right] \\dot{\\theta} \\in se(3)`\nis the matrix representation of a twist.\n\n----------------\nDual Quaternions\n----------------\n\nSimilarly to unit quaternions for rotations, unit dual quaternions are\nan alternative to represent transformations [5]_ [6]_ [7]_. They support\nsimilar operations as transformation matrices.\n\nA dual quaternion consists of a real quaternion and a dual quaternion:\n\n.. math::\n\n \\boldsymbol{\\sigma} = \\boldsymbol{p} + \\epsilon \\boldsymbol{q}\n = p_w + p_x i + p_y j + p_z k + \\epsilon (q_w + q_x i + q_y j + q_z k),\n\nwhere :math:`\\epsilon^2 = 0` and :math:`\\epsilon \\neq 0`.\nWe use unit dual quaternions to represent\ntransformations. In this case, the real quaternion is a unit quaternion\nand the dual quaternion is orthogonal to the real quaternion.\nThe real quaternion is used to represent the rotation and the dual\nquaternion contains information about the rotation and translation.\n\nDual quaternions support similar operations as transformation matrices:\ninversion through the conjugate of the two individual quaternions\n:func:`~pytransform3d.transformations.dq_q_conj`, concatenation\nthrough :func:`~pytransform3d.transformations.concatenate_dual_quaternions`,\nand transformation of a point by\n:func:`~pytransform3d.transformations.dq_prod_vector`.\nThey can be renormalized efficiently (with\n:func:`~pytransform3d.transformations.check_dual_quaternion`), and\ninterpolation between two dual quaternions is possible (with\n:func:`~pytransform3d.transformations.dual_quaternion_sclerp`).\n\n.. warning::\n\n The unit dual quaternions\n :math:`\\boldsymbol{\\sigma} = \\boldsymbol{p} + \\epsilon \\boldsymbol{q}` and\n :math:`-\\boldsymbol{\\sigma}` represent exactly the same transformation.\n\nThe reason for this ambiguity is that the real quaternion\n:math:`\\boldsymbol{p}` represents the orientation component, the dual\nquaternion encodes the translation component as\n:math:`\\boldsymbol{q} = 0.5 \\boldsymbol{t} \\boldsymbol{p}`, where\n:math:`\\boldsymbol{t}` is a quaternion with the translation in the vector\ncomponent and the scalar 0, and rotation quaternions have the same ambiguity.\n\n**Pros**\n\n* Representation: compact.\n* Renormalization: cheap in comparison to transformation matrix.\n* Supported operations: all, including interpolation with ScLERP.\n* Computational efficiency: the dual quaternion product is slightly\n cheaper than the matrix product.\n* Singularities: none.\n\n**Cons**\n\n* Interpretation: not straightforward.\n* Ambiguities: double cover.\n\n----------\nReferences\n----------\n\n.. [1] Lynch, K. M., Park, F. C. (2017). Modern Robotics.\n http://hades.mech.northwestern.edu/index.php/Modern_Robotics\n.. [2] Corke, P. (2017). Robotics, Vision and Control, 2nd Edition,\n https://link.springer.com/book/10.1007/978-3-319-54413-7\n.. [3] Eade, E. (2017). Lie Groups for 2D and 3D Transformations.\n https://ethaneade.com/lie.pdf\n.. [4] Sola, J., Deray, J., Atchuthan, D. (2018).\n A micro Lie theory for state estimation in robotics. Technical Report.\n http://www.iri.upc.edu/files/scidoc/2089-A-micro-Lie-theory-for-state-estimation-in-robotics.pdf\n.. [5] Wikipedia: Dual Quaternion.\n https://en.wikipedia.org/wiki/Dual_quaternion\n.. [6] Jia, Y.-B.: Dual Quaternions.\n https://faculty.sites.iastate.edu/jia/files/inline-files/dual-quaternion.pdf\n.. [7] Kenwright, B. (2012). A Beginners Guide to Dual-Quaternions: What They\n Are, How They Work, and How to Use Them for 3D Character Hierarchies. In\n 20th International Conference in Central Europe on Computer Graphics,\n Visualization and Computer Vision.\n http://wscg.zcu.cz/WSCG2012/!_WSCG2012-Communications-1.pdf\n" }, { "path": "doc/source/user_guide/uncertainty.rst", "content": "==============================\nUncertainty in Transformations\n==============================\n\nIn computer vision and robotics we are never absolutely certain about\ntransformations. It is often a good idea to express the uncertainty explicitly\nand use it. The module :mod:`pytransform3d.uncertainty` offers tools to handle\nwith uncertain transformations.\n\n----------------------------------------\nGaussian Distribution of Transformations\n----------------------------------------\n\nUncertain transformations are often represented by Gaussian distributions,\nwhere the mean of the distribution is represented by a transformation matrix\n:math:`\\boldsymbol{T} \\in SE(3)` and the covariance is defined in the tangent space\nthrough exponential coordinates\n:math:`\\boldsymbol{\\Sigma} \\in \\mathbb{R}^{6 \\times 6}`.\n\n.. warning::\n\n It makes a difference whether the uncertainty is defined in the global\n frame of reference (i.e., left-multiplied) or local frame of reference\n (i.e., right-multiplied). Unless otherwise stated, we define uncertainty\n in a global frame, that is, to sample from a Gaussian distribution of\n transformations\n :math:`\\boldsymbol{T}_{xA} \\sim \\mathcal{N}(\\boldsymbol{T}|\\boldsymbol{T}_{BA}, \\boldsymbol{\\Sigma}_{6 \\times 6}))`,\n we compute :math:`\\Delta \\boldsymbol{T}_{xB} \\boldsymbol{T}_{BA}`,\n with :math:`\\Delta \\boldsymbol{T}_{xB} = Exp(\\boldsymbol{\\xi})` and\n :math:`\\boldsymbol{\\xi} \\sim \\mathcal{N}(\\boldsymbol{0}_6, \\boldsymbol{\\Sigma}_{6 \\times 6})`.\n Hence, the uncertainty is defined in the global frame B, not in the local\n body frame A.\n\nWe can use\n:func:`~pytransform3d.uncertainty.estimate_gaussian_transform_from_samples`\nto estimate a Gaussian distribution of transformations. We can sample from\na known Gaussian distribution of transformations with\n:func:`~pytransform3d.transformations.random_transform` as illustrated in\nthe Example :ref:`sphx_glr__auto_examples_plots_plot_sample_transforms.py`.\n\n.. figure:: ../_auto_examples/plots/images/sphx_glr_plot_sample_transforms_001.png\n :target: ../_auto_examples/plots/plot_sample_transforms.html\n :align: center\n\n----------------------------\nVisualization of Uncertainty\n----------------------------\n\nA typical visual representation of Gaussian distributions in 3D coordinates\nare equiprobable ellipsoids (obtained with\n:func:`~pytransform3d.uncertainty.to_ellipsoid`). This is equivalent to showing\nthe :math:`k\\sigma, k \\in \\mathbb{R}` intervals of a 1D Gaussian distribution.\nHowever, for transformations there are also interactions between rotation and\ntranslation components so that an ellipsoid is not an appropriate\nrepresentation to visualize the distribution of transformations in 3D. We have\nto project a 6D hyper-ellipsoid to 3D (for which we can use\n:func:`~pytransform3d.uncertainty.to_projected_ellipsoid`), which\ncan result in banana-like shapes.\n\n------------------------------------------\nConcatenation of Uncertain Transformations\n------------------------------------------\n\nThere are two different ways of defining uncertainty of transformations when\nwe concatenate multiple transformations. We can define uncertainty\nin the global frame of reference or in the local frame of reference\nand it makes a difference.\n\nGlobal frame of reference\n(:func:`~pytransform3d.uncertainty.concat_globally_uncertain_transforms`):\n\n.. math::\n\n Exp(_C\\boldsymbol{\\xi'}) \\overline{\\boldsymbol{T}}_{CA} = Exp(_C\\boldsymbol{\\xi}) \\overline{\\boldsymbol{T}}_{CB} Exp(_B\\boldsymbol{\\xi}) \\overline{\\boldsymbol{T}}_{BA},\n\nwhere :math:`_B\\boldsymbol{\\xi} \\sim \\mathcal{N}(\\boldsymbol{0}, \\boldsymbol{\\Sigma}_{BA})`,\n:math:`_C\\boldsymbol{\\xi} \\sim \\mathcal{N}(\\boldsymbol{0}, \\boldsymbol{\\Sigma}_{CB})`,\nand :math:`_C\\boldsymbol{\\xi'} \\sim \\mathcal{N}(\\boldsymbol{0}, \\boldsymbol{\\Sigma}_{CA})`.\n\nThis method of concatenating uncertain transformation is used in Example\n:ref:`sphx_glr__auto_examples_plots_plot_concatenate_uncertain_transforms.py`,\nwhich illustrates how the banana distribution is generated.\n\n.. figure:: ../_auto_examples/plots/images/sphx_glr_plot_concatenate_uncertain_transforms_001.png\n :target: ../_auto_examples/plots/plot_concatenate_uncertain_transforms.html\n :align: center\n\nLocal frame of reference\n(:func:`~pytransform3d.uncertainty.concat_locally_uncertain_transforms`):\n\n.. math::\n\n \\overline{\\boldsymbol{T}}_{CA} Exp(_A\\boldsymbol{\\xi'}) = \\overline{\\boldsymbol{T}}_{CB} Exp(_B\\boldsymbol{\\xi}) \\overline{\\boldsymbol{T}}_{BA} Exp(_A\\boldsymbol{\\xi}),\n\nwhere :math:`_B\\boldsymbol{\\xi} \\sim \\mathcal{N}(\\boldsymbol{0}, \\boldsymbol{\\Sigma}_B)`,\n:math:`_A\\boldsymbol{\\xi} \\sim \\mathcal{N}(\\boldsymbol{0}, \\boldsymbol{\\Sigma}_A)`,\nand :math:`_A\\boldsymbol{\\xi'} \\sim \\mathcal{N}(\\boldsymbol{0}, \\boldsymbol{\\Sigma}_{A,total})`.\n\nThis method of concatenating uncertain transformations is used in Example\n:ref:`sphx_glr__auto_examples_visualizations_vis_probabilistic_robot_kinematics.py`,\nwhich illustrates probabilistic robot kinematics.\n\n.. figure:: ../_auto_examples/visualizations/images/sphx_glr_vis_probabilistic_robot_kinematics_001.png\n :target: ../_auto_examples/visualizations/vis_probabilistic_robot_kinematics.html\n :align: center\n\n-------------------------\nFusion of Uncertain Poses\n-------------------------\n\nFusing of multiple uncertain poses with\n:func:`~pytransform3d.uncertainty.pose_fusion` is required, for instance,\nin state estimation and sensor fusion.\nThe Example :ref:`sphx_glr__auto_examples_plots_plot_pose_fusion.py`\nillustrates this process.\n\n.. figure:: ../_auto_examples/plots/images/sphx_glr_plot_pose_fusion_001.png\n :target: ../_auto_examples/plots/plot_pose_fusion.html\n :width: 50%\n :align: center\n" }, { "path": "examples/README.rst", "content": "========\nExamples\n========\n\nThe following examples show how pytransform3d can be used.\n" }, { "path": "examples/animations/README.rst", "content": "Matplotlib Animations\n---------------------" }, { "path": "examples/animations/animate_angle_axis_interpolation.py", "content": "\"\"\"\n==============================================\nInterpolate Between Axis-Angle Representations\n==============================================\n\nWe can interpolate between two orientations that are represented by an axis and\nan angle either linearly or with slerp (spherical linear interpolation).\nHere we compare both methods and measure the angular\nvelocity between two successive steps. We can see that linear interpolation\nresults in a non-constant angular velocity. Usually it is a better idea to\ninterpolate with slerp.\n\"\"\"\n\nimport matplotlib.animation as animation\nimport matplotlib.pyplot as plt\nimport numpy as np\nfrom mpl_toolkits.mplot3d import axes3d # noqa: F401\n\nfrom pytransform3d import rotations as pr\n\nvelocity = None\nlast_a = None\nrotation_axis = None\n\n\ndef interpolate_linear(start, end, t):\n return (1 - t) * start + t * end\n\n\ndef update_lines(step, start, end, n_frames, rot, profile):\n global velocity\n global last_a\n\n if step == 0:\n velocity = []\n last_a = start\n\n if step <= n_frames / 2:\n t = step / float(n_frames / 2 - 1)\n a = pr.axis_angle_slerp(start, end, t)\n else:\n t = (step - n_frames / 2) / float(n_frames / 2 - 1)\n a = interpolate_linear(end, start, t)\n\n R = pr.matrix_from_axis_angle(a)\n\n # Draw new frame\n rot[0].set_data(np.array([0, R[0, 0]]), [0, R[1, 0]])\n rot[0].set_3d_properties([0, R[2, 0]])\n\n rot[1].set_data(np.array([0, R[0, 1]]), [0, R[1, 1]])\n rot[1].set_3d_properties([0, R[2, 1]])\n\n rot[2].set_data(np.array([0, R[0, 2]]), [0, R[1, 2]])\n rot[2].set_3d_properties([0, R[2, 2]])\n\n # Update vector in frame\n test = R.dot(np.ones(3) / np.sqrt(3.0))\n rot[3].set_data(\n np.array([test[0] / 2.0, test[0]]), [test[1] / 2.0, test[1]]\n )\n rot[3].set_3d_properties([test[2] / 2.0, test[2]])\n\n velocity.append(\n pr.angle_between_vectors(a[:3], last_a[:3]) + a[3] - last_a[3]\n )\n last_a = a\n profile.set_data(np.linspace(0, 1, n_frames)[: len(velocity)], velocity)\n\n return rot\n\n\nif __name__ == \"__main__\":\n # Generate random start and goal\n np.random.seed(3)\n start = np.array([0, 0, 0, np.pi])\n start[:3] = pr.norm_vector(np.random.randn(3))\n end = np.array([0, 0, 0, np.pi])\n end[:3] = pr.norm_vector(np.random.randn(3))\n n_frames = 100\n\n fig = plt.figure(figsize=(12, 5))\n\n ax = fig.add_subplot(121, projection=\"3d\")\n ax.set_xlim((-1, 1))\n ax.set_ylim((-1, 1))\n ax.set_zlim((-1, 1))\n ax.set_xlabel(\"X\")\n ax.set_ylabel(\"Y\")\n ax.set_zlabel(\"Z\")\n\n Rs = pr.matrix_from_axis_angle(start)\n Re = pr.matrix_from_axis_angle(end)\n\n rot = [\n ax.plot([0, 1], [0, 0], [0, 0], c=\"r\", lw=3)[0],\n ax.plot([0, 0], [0, 1], [0, 0], c=\"g\", lw=3)[0],\n ax.plot([0, 0], [0, 0], [0, 1], c=\"b\", lw=3)[0],\n ax.plot([0, 1], [0, 1], [0, 1], c=\"gray\", lw=3)[0],\n ax.plot(\n [0, Rs[0, 0]], [0, Rs[1, 0]], [0, Rs[2, 0]], c=\"r\", lw=3, alpha=0.5\n )[0],\n ax.plot(\n [0, Rs[0, 1]], [0, Rs[1, 1]], [0, Rs[2, 1]], c=\"g\", lw=3, alpha=0.5\n )[0],\n ax.plot(\n [0, Rs[0, 2]], [0, Rs[1, 2]], [0, Rs[2, 2]], c=\"b\", lw=3, alpha=0.5\n )[0],\n ax.plot(\n [0, Re[0, 0]],\n [0, Re[1, 0]],\n [0, Re[2, 0]],\n c=\"orange\",\n lw=3,\n alpha=0.5,\n )[0],\n ax.plot(\n [0, Re[0, 1]],\n [0, Re[1, 1]],\n [0, Re[2, 1]],\n c=\"turquoise\",\n lw=3,\n alpha=0.5,\n )[0],\n ax.plot(\n [0, Re[0, 2]],\n [0, Re[1, 2]],\n [0, Re[2, 2]],\n c=\"violet\",\n lw=3,\n alpha=0.5,\n )[0],\n ]\n\n ax = fig.add_subplot(122)\n ax.set_xlim((0, 1))\n ax.set_ylim((0, 1))\n profile = ax.plot(0, 0)[0]\n\n anim = animation.FuncAnimation(\n fig,\n update_lines,\n n_frames,\n fargs=(start, end, n_frames, rot, profile),\n interval=50,\n blit=False,\n )\n\n plt.show()\n" }, { "path": "examples/animations/animate_camera.py", "content": "\"\"\"\n==============\nAnimate Camera\n==============\n\nAnimate a camera moving along a circular trajectory while looking at a target.\n\"\"\"\n\nimport matplotlib.animation as animation\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d.plot_utils import Frame, Camera, make_3d_axis\nfrom pytransform3d.rotations import matrix_from_euler\nfrom pytransform3d.transformations import transform_from\n\n\ndef update_camera(step, n_frames, camera):\n phi = 2 * np.pi * step / n_frames\n tf = transform_from(\n matrix_from_euler([-0.5 * np.pi, phi, 0], 0, 1, 2, False),\n -10 * np.array([np.sin(phi), np.cos(phi), 0]),\n )\n camera.set_data(tf)\n return camera\n\n\nif __name__ == \"__main__\":\n n_frames = 50\n\n fig = plt.figure(figsize=(5, 5))\n ax = make_3d_axis(15)\n\n frame = Frame(np.eye(4), label=\"target\", s=3, draw_label_indicator=False)\n frame.add_frame(ax)\n\n fl = 3000 # [pixels]\n w, h = 1920, 1080 # [pixels]\n M = np.array(((fl, 0, w // 2), (0, fl, h // 2), (0, 0, 1)))\n camera = Camera(\n M,\n np.eye(4),\n virtual_image_distance=5,\n sensor_size=(w, h),\n c=\"c\",\n )\n camera.add_camera(ax)\n\n anim = animation.FuncAnimation(\n fig,\n update_camera,\n n_frames,\n fargs=(n_frames, camera),\n interval=50,\n blit=False,\n )\n\n plt.show()\n" }, { "path": "examples/animations/animate_quaternion_integration.py", "content": "\"\"\"\n======================\nQuaternion Integration\n======================\n\nIntegrate angular accelerations to a quaternion sequence and animate it.\n\"\"\"\n\nimport matplotlib.animation as animation\nimport matplotlib.pyplot as plt\nimport numpy as np\nfrom mpl_toolkits.mplot3d import axes3d # noqa: F401\n\nfrom pytransform3d import rotations as pr\n\n\ndef update_lines(step, Q, rot):\n R = pr.matrix_from_quaternion(Q[step])\n\n # Draw new frame\n rot[0].set_data(np.array([0, R[0, 0]]), [0, R[1, 0]])\n rot[0].set_3d_properties([0, R[2, 0]])\n\n rot[1].set_data(np.array([0, R[0, 1]]), [0, R[1, 1]])\n rot[1].set_3d_properties([0, R[2, 1]])\n\n rot[2].set_data(np.array([0, R[0, 2]]), [0, R[1, 2]])\n rot[2].set_3d_properties([0, R[2, 2]])\n\n return rot\n\n\nif __name__ == \"__main__\":\n rng = np.random.default_rng(3)\n start = pr.random_quaternion(rng)\n n_frames = 1000\n dt = 0.01\n angular_accelerations = np.empty((n_frames, 3))\n for i in range(n_frames):\n angular_accelerations[i] = pr.random_compact_axis_angle(rng)\n # Integrate angular accelerations to velocities\n angular_velocities = np.vstack(\n (np.zeros((1, 3)), np.cumsum(angular_accelerations * dt, axis=0))\n )\n # Integrate angular velocities to quaternions\n Q = pr.quaternion_integrate(angular_velocities, q0=start, dt=dt)\n\n fig = plt.figure(figsize=(4, 3))\n\n ax = fig.add_subplot(111, projection=\"3d\")\n ax.set_xlim((-1, 1))\n ax.set_ylim((-1, 1))\n ax.set_zlim((-1, 1))\n ax.set_xlabel(\"X\")\n ax.set_ylabel(\"Y\")\n ax.set_zlabel(\"Z\")\n\n R = pr.matrix_from_quaternion(start)\n\n rot = [\n ax.plot([0, 1], [0, 0], [0, 0], c=\"r\", lw=3)[0],\n ax.plot([0, 0], [0, 1], [0, 0], c=\"g\", lw=3)[0],\n ax.plot([0, 0], [0, 0], [0, 1], c=\"b\", lw=3)[0],\n ax.plot(\n [0, R[0, 0]], [0, R[1, 0]], [0, R[2, 0]], c=\"r\", lw=3, alpha=0.3\n )[0],\n ax.plot(\n [0, R[0, 1]], [0, R[1, 1]], [0, R[2, 1]], c=\"g\", lw=3, alpha=0.3\n )[0],\n ax.plot(\n [0, R[0, 2]], [0, R[1, 2]], [0, R[2, 2]], c=\"b\", lw=3, alpha=0.3\n )[0],\n ]\n\n anim = animation.FuncAnimation(\n fig, update_lines, n_frames, fargs=(Q, rot), interval=10, blit=False\n )\n\n plt.show()\n" }, { "path": "examples/animations/animate_quaternion_interpolation.py", "content": "\"\"\"\n===========================================\nInterpolate Between Quaternion Orientations\n===========================================\n\nWe can interpolate between two orientations that are represented by quaternions\neither linearly or with slerp (spherical linear interpolation).\nHere we compare both methods and measure the angular velocity between two\nsuccessive steps. We can see that linear interpolation results in a\nnon-constant angular velocity. Usually it is a better idea to interpolate with\nslerp.\n\"\"\"\n\nimport matplotlib.animation as animation\nimport matplotlib.pyplot as plt\nimport numpy as np\nfrom mpl_toolkits.mplot3d import axes3d # noqa: F401\n\nfrom pytransform3d import rotations as pr\n\nvelocity = None\nlast_R = None\n\n\ndef interpolate_linear(start, end, t):\n return (1 - t) * start + t * end\n\n\ndef update_lines(step, start, end, n_frames, rot, profile):\n global velocity\n global last_R\n\n if step == 0:\n velocity = []\n last_R = pr.matrix_from_quaternion(start)\n\n if step <= n_frames / 2:\n t = step / float(n_frames / 2 - 1)\n q = pr.quaternion_slerp(start, end, t)\n else:\n t = (step - n_frames / 2) / float(n_frames / 2 - 1)\n q = interpolate_linear(end, start, t)\n\n R = pr.matrix_from_quaternion(q)\n\n # Draw new frame\n rot[0].set_data(np.array([0, R[0, 0]]), [0, R[1, 0]])\n rot[0].set_3d_properties([0, R[2, 0]])\n\n rot[1].set_data(np.array([0, R[0, 1]]), [0, R[1, 1]])\n rot[1].set_3d_properties([0, R[2, 1]])\n\n rot[2].set_data(np.array([0, R[0, 2]]), [0, R[1, 2]])\n rot[2].set_3d_properties([0, R[2, 2]])\n\n # Update vector in frame\n test = R.dot(np.ones(3) / np.sqrt(3.0))\n rot[3].set_data(\n np.array([test[0] / 2.0, test[0]]), [test[1] / 2.0, test[1]]\n )\n rot[3].set_3d_properties([test[2] / 2.0, test[2]])\n\n velocity.append(np.linalg.norm(R - last_R))\n last_R = R\n profile.set_data(np.linspace(0, 1, n_frames)[: len(velocity)], velocity)\n\n return rot\n\n\nif __name__ == \"__main__\":\n # Generate random start and goal\n np.random.seed(3)\n start = np.array([0, 0, 0, np.pi])\n start[:3] = np.random.randn(3)\n start = pr.quaternion_from_axis_angle(start)\n end = np.array([0, 0, 0, np.pi])\n end[:3] = np.random.randn(3)\n end = pr.quaternion_from_axis_angle(end)\n n_frames = 200\n\n fig = plt.figure(figsize=(12, 5))\n\n ax = fig.add_subplot(121, projection=\"3d\")\n ax.set_xlim((-1, 1))\n ax.set_ylim((-1, 1))\n ax.set_zlim((-1, 1))\n ax.set_xlabel(\"X\")\n ax.set_ylabel(\"Y\")\n ax.set_zlabel(\"Z\")\n\n Rs = pr.matrix_from_quaternion(start)\n Re = pr.matrix_from_quaternion(end)\n\n rot = [\n ax.plot([0, 1], [0, 0], [0, 0], c=\"r\", lw=3)[0],\n ax.plot([0, 0], [0, 1], [0, 0], c=\"g\", lw=3)[0],\n ax.plot([0, 0], [0, 0], [0, 1], c=\"b\", lw=3)[0],\n ax.plot([0, 1], [0, 1], [0, 1], c=\"gray\", lw=3)[0],\n ax.plot(\n [0, Rs[0, 0]], [0, Rs[1, 0]], [0, Rs[2, 0]], c=\"r\", lw=3, alpha=0.5\n )[0],\n ax.plot(\n [0, Rs[0, 1]], [0, Rs[1, 1]], [0, Rs[2, 1]], c=\"g\", lw=3, alpha=0.5\n )[0],\n ax.plot(\n [0, Rs[0, 2]], [0, Rs[1, 2]], [0, Rs[2, 2]], c=\"b\", lw=3, alpha=0.5\n )[0],\n ax.plot(\n [0, Re[0, 0]],\n [0, Re[1, 0]],\n [0, Re[2, 0]],\n c=\"orange\",\n lw=3,\n alpha=0.5,\n )[0],\n ax.plot(\n [0, Re[0, 1]],\n [0, Re[1, 1]],\n [0, Re[2, 1]],\n c=\"turquoise\",\n lw=3,\n alpha=0.5,\n )[0],\n ax.plot(\n [0, Re[0, 2]],\n [0, Re[1, 2]],\n [0, Re[2, 2]],\n c=\"violet\",\n lw=3,\n alpha=0.5,\n )[0],\n ]\n\n ax = fig.add_subplot(122)\n ax.set_xlim((0, 1))\n ax.set_ylim((0, 1))\n profile = ax.plot(0, 0)[0]\n\n anim = animation.FuncAnimation(\n fig,\n update_lines,\n n_frames,\n fargs=(start, end, n_frames, rot, profile),\n interval=50,\n blit=False,\n )\n\n plt.show()\n" }, { "path": "examples/animations/animate_rotation.py", "content": "\"\"\"\n================\nAnimate Rotation\n================\n\nSimple animations of frames and trajectories are easy to implement with\nMatplotlib's FuncAnimation. The following example will generate a frame\nthat will be rotated about the x-axis.\n\"\"\"\n\nimport matplotlib.animation as animation\nimport matplotlib.pyplot as plt\nimport numpy as np\nfrom mpl_toolkits.mplot3d import axes3d # noqa: F401\n\nfrom pytransform3d import rotations as pr\nfrom pytransform3d.plot_utils import Frame\n\n\ndef update_frame(step, n_frames, frame):\n angle = 2.0 * np.pi * (step + 1) / n_frames\n R = pr.passive_matrix_from_angle(0, angle)\n A2B = np.eye(4)\n A2B[:3, :3] = R\n frame.set_data(A2B)\n return frame\n\n\nif __name__ == \"__main__\":\n n_frames = 50\n\n fig = plt.figure(figsize=(5, 5))\n\n ax = fig.add_subplot(111, projection=\"3d\")\n ax.set_xlim((-1, 1))\n ax.set_ylim((-1, 1))\n ax.set_zlim((-1, 1))\n ax.set_xlabel(\"X\")\n ax.set_ylabel(\"Y\")\n ax.set_zlabel(\"Z\")\n\n frame = Frame(np.eye(4), label=\"rotating frame\", s=0.5)\n frame.add_frame(ax)\n\n anim = animation.FuncAnimation(\n fig,\n update_frame,\n n_frames,\n fargs=(n_frames, frame),\n interval=50,\n blit=False,\n )\n\n plt.show()\n" }, { "path": "examples/animations/animate_trajectory.py", "content": "\"\"\"\n==================\nAnimate Trajectory\n==================\n\nAnimates a trajectory.\n\"\"\"\n\nimport matplotlib.animation as animation\nimport matplotlib.pyplot as plt\nimport numpy as np\nfrom mpl_toolkits.mplot3d import axes3d # noqa: F401\n\nfrom pytransform3d.plot_utils import Trajectory\nfrom pytransform3d.rotations import passive_matrix_from_angle, R_id\nfrom pytransform3d.transformations import transform_from, concat\n\n\ndef update_trajectory(step, n_frames, trajectory):\n progress = float(step + 1) / float(n_frames)\n H = np.zeros((100, 4, 4))\n H0 = transform_from(R_id, np.zeros(3))\n H_mod = np.eye(4)\n for i, t in enumerate(np.linspace(0, progress, len(H))):\n H0[:3, 3] = np.array([t, 0, t])\n H_mod[:3, :3] = passive_matrix_from_angle(2, 8 * np.pi * t)\n H[i] = concat(H0, H_mod)\n\n trajectory.set_data(H)\n return trajectory\n\n\nif __name__ == \"__main__\":\n n_frames = 200\n\n fig = plt.figure(figsize=(5, 5))\n\n ax = fig.add_subplot(111, projection=\"3d\")\n ax.set_xlim((-1, 1))\n ax.set_ylim((-1, 1))\n ax.set_zlim((-1, 1))\n ax.set_xlabel(\"X\")\n ax.set_ylabel(\"Y\")\n ax.set_zlabel(\"Z\")\n\n H = np.zeros((100, 4, 4))\n H[:] = np.eye(4)\n trajectory = Trajectory(H, show_direction=False, s=0.2, c=\"k\")\n trajectory.add_trajectory(ax)\n\n anim = animation.FuncAnimation(\n fig,\n update_trajectory,\n n_frames,\n fargs=(n_frames, trajectory),\n interval=50,\n blit=False,\n )\n\n plt.show()\n" }, { "path": "examples/apps/README.rst", "content": "GUI Applications\n----------------" }, { "path": "examples/apps/app_transformation_editor.py", "content": "\"\"\"\n=====================\nTransformation Editor\n=====================\n\nThe transformation editor can be used to manipulate transformations.\n\"\"\"\n\nfrom pytransform3d.editor import TransformEditor\nfrom pytransform3d.rotations import active_matrix_from_extrinsic_euler_xyx\nfrom pytransform3d.transform_manager import TransformManager\nfrom pytransform3d.transformations import transform_from\n\ntm = TransformManager()\n\ntm.add_transform(\n \"tree\",\n \"world\",\n transform_from(\n active_matrix_from_extrinsic_euler_xyx([0, 0.5, 0]), [0, 0, 0.5]\n ),\n)\ntm.add_transform(\n \"car\",\n \"world\",\n transform_from(\n active_matrix_from_extrinsic_euler_xyx([0.5, 0, 0]), [0.5, 0, 0]\n ),\n)\n\nte = TransformEditor(tm, \"world\", s=0.3)\nte.show()\nprint(\"tree to world:\")\nprint(te.transform_manager.get_transform(\"tree\", \"world\"))\n" }, { "path": "examples/plots/README.rst", "content": "Matplotlib Figures\n------------------" }, { "path": "examples/plots/plot_axis_angle.py", "content": "\"\"\"\n=====================================\nAxis-Angle Representation of Rotation\n=====================================\n\nAny rotation can be represented with a single rotation about some axis.\nHere we see a frame that is rotated in multiple steps around a rotation\naxis.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d.rotations import (\n random_axis_angle,\n matrix_from_axis_angle,\n plot_basis,\n plot_axis_angle,\n)\n\noriginal = random_axis_angle(np.random.RandomState(5))\nax = plot_axis_angle(a=original)\nfor fraction in np.linspace(0, 1, 50):\n a = original.copy()\n a[-1] = fraction * original[-1]\n R = matrix_from_axis_angle(a)\n plot_basis(ax, R, alpha=0.2)\nplt.subplots_adjust(left=0, right=1, bottom=0, top=1.1)\nax.view_init(azim=105, elev=12)\nplt.show()\n" }, { "path": "examples/plots/plot_axis_angle_from_two_vectors.py", "content": "\"\"\"\n====================================================\nAxis-Angle Representation from Two Direction Vectors\n====================================================\n\nThis example shows how we can compute the axis-angle representation of a\nrotation that transforms a direction given by a vector 'a' to a direction\ngiven by a vector 'b'. We show both vectors, the rotation about the rotation\naxis and the initial and resulting coordinate frame, where the vector 'b'\nand its corresponding frame after the rotation are represented by shorter\nlines.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d.plot_utils import make_3d_axis, plot_vector\nfrom pytransform3d.rotations import (\n axis_angle_from_two_directions,\n matrix_from_axis_angle,\n plot_axis_angle,\n plot_basis,\n)\n\na = np.array([1.0, 0.0, 0.0])\nb = np.array([0.76958075, -0.49039301, -0.40897453])\naa = axis_angle_from_two_directions(a, b)\n\nax = make_3d_axis(ax_s=1)\nplot_vector(ax, start=np.zeros(3), direction=a, s=1.0)\nplot_vector(ax, start=np.zeros(3), direction=b, s=0.5)\nplot_axis_angle(ax, aa)\nplot_basis(ax)\nplot_basis(ax, R=matrix_from_axis_angle(aa), s=0.5)\nplt.show()\n" }, { "path": "examples/plots/plot_bivector.py", "content": "\"\"\"\n=============\nPlot Bivector\n=============\n\nVisualizes a bivector constructed from the wedge product of two vectors.\nThe two vectors will be displayed in grey together with the parallelogram that\nthey form. Each component of the bivector corresponds to the area of the\nparallelogram projected on the basis planes. These parallelograms will be\nshown as well. Furthermore, one black arrow will show the rotation direction\nof the bivector and one black arrow will represent the normal of the plane\nthat can be extracted by rearranging the elements of the bivector and\nnormalizing the vector.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nimport pytransform3d.rotations as pr\n\na = np.array([0.6, 0.3, 0.9])\nb = np.array([0.4, 0.8, 0.2])\n\nax = pr.plot_basis()\npr.plot_bivector(ax=ax, a=a, b=b)\nax.view_init(azim=75, elev=40)\nax.set_xlim((-0.5, 1))\nax.set_ylim((-0.5, 1))\nax.set_zlim((-0.5, 1))\nplt.tight_layout()\nplt.show()\n" }, { "path": "examples/plots/plot_box.py", "content": "\"\"\"\n========\nPlot Box\n========\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d.plot_utils import (\n plot_box,\n plot_length_variable,\n remove_frame,\n)\nfrom pytransform3d.transformations import plot_transform\n\nplt.figure()\nax = plot_box(size=[1, 1, 1], wireframe=False, alpha=0.1, color=\"k\", ax_s=0.6)\nplot_transform(ax=ax)\nplot_box(ax=ax, size=[1, 1, 1], wireframe=True, alpha=0.3, linewidth=2)\nplot_length_variable(\n ax=ax,\n start=np.array([-0.5, -0.5, 0.55]),\n end=np.array([0.5, -0.5, 0.55]),\n name=\"a\",\n fontsize=14,\n fontfamily=\"serif\",\n)\nplot_length_variable(\n ax=ax,\n start=np.array([0.55, -0.5, 0.5]),\n end=np.array([0.55, 0.5, 0.5]),\n name=\"b\",\n fontsize=14,\n fontfamily=\"serif\",\n)\nplot_length_variable(\n ax=ax,\n start=np.array([-0.55, -0.5, -0.5]),\n end=np.array([-0.55, -0.5, 0.5]),\n name=\"c\",\n fontsize=14,\n fontfamily=\"serif\",\n)\nremove_frame(ax)\nplt.show()\n" }, { "path": "examples/plots/plot_camera_3d.py", "content": "\"\"\"\n===========================\nCamera Representation in 3D\n===========================\n\nThis visualization is inspired by Blender's camera visualization. It will\nshow the camera center, a virtual image plane at a desired distance to the\ncamera center, and the top direction of the virtual image plane.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nimport pytransform3d.camera as pc\nimport pytransform3d.transformations as pt\n\ncam2world = pt.transform_from_pq([0, 0, 0, np.sqrt(0.5), -np.sqrt(0.5), 0, 0])\n# default parameters of a camera in Blender\nsensor_size = np.array([0.036, 0.024])\nintrinsic_matrix = np.array(\n [\n [0.05, 0, sensor_size[0] / 2.0],\n [0, 0.05, sensor_size[1] / 2.0],\n [0, 0, 1],\n ]\n)\nvirtual_image_distance = 1\n\nax = pt.plot_transform(A2B=cam2world, s=0.2)\npc.plot_camera(\n ax,\n cam2world=cam2world,\n M=intrinsic_matrix,\n sensor_size=sensor_size,\n virtual_image_distance=virtual_image_distance,\n)\nplt.show()\n" }, { "path": "examples/plots/plot_camera_projection.py", "content": "\"\"\"\n=================\nCamera Projection\n=================\n\nWe can see the camera coordinate frame and a grid of points in the camera\ncoordinate system which will be projected to the sensor. From the coordinates\non the sensor we can compute the corresponding pixels.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d.camera import make_world_grid, cam2sensor, sensor2img\nfrom pytransform3d.transformations import plot_transform\n\n# %%\n# Camera Details\n# --------------\n# We have to define the focal length and sensor size of the camera in meters as\n# well as the image size in pixels. The sensor size is extraordinarily large so\n# that we can see it.\nfocal_length = 0.2\nsensor_size = (0.2, 0.15)\nimage_size = (640, 480)\n\n# %%\n# Grid\n# ----\n# We define a grid in 3D world coordinates and compute its projection to the\n# sensor in 3D.\ncam_grid = make_world_grid(n_points_per_line=11) - np.array([0, 0, -2, 0])\nimg_grid_3d = cam_grid * focal_length\n\n# %%\n# Projection\n# ----------\n# First, we project the grid from its original 3D coordinates to its projection\n# on the sensor, then we convert it to image coordinates.\nsensor_grid = cam2sensor(cam_grid, focal_length)\nimg_grid = sensor2img(sensor_grid, sensor_size, image_size)\n\n# %%\n# Plotting\n# --------\n# Now we can plot the grid in 3D, projected to the 3D sensor, and projected to\n# the image.\nplt.figure(figsize=(12, 5))\nax = plt.subplot(121, projection=\"3d\")\nax.set_title(\"Grid in 3D camera coordinate system\")\nax.set_xlim((-1, 1))\nax.set_ylim((-1, 1))\nax.set_zlim((0, 2))\nax.set_xlabel(\"X\")\nax.set_ylabel(\"Y\")\nax.set_zlabel(\"Z\")\nc = np.arange(len(cam_grid))\nax.scatter(cam_grid[:, 0], cam_grid[:, 1], cam_grid[:, 2], c=c)\nax.scatter(img_grid_3d[:, 0], img_grid_3d[:, 1], img_grid_3d[:, 2], c=c)\nplot_transform(ax)\n\nax = plt.subplot(122, aspect=\"equal\")\nax.set_title(\"Grid in 2D image coordinate system\")\nax.scatter(img_grid[:, 0], img_grid[:, 1], c=c)\nax.set_xlim((0, image_size[0]))\nax.set_ylim((0, image_size[1]))\n\nplt.show()\n" }, { "path": "examples/plots/plot_camera_trajectory.py", "content": "\"\"\"\n=================\nCamera Trajectory\n=================\n\nThe following illustration shows a camera's trajectory that has has been\nestimated from odometry. This specific trajectory has been used to reconstruct\na colored mesh from a depth camera and an RGB camera.\n\"\"\"\n\nimport os\n\nimport matplotlib.pyplot as plt\nimport numpy as np\nfrom cycler import cycle\n\nimport pytransform3d.camera as pc\nimport pytransform3d.rotations as pr\nimport pytransform3d.trajectories as ptr\nimport pytransform3d.transformations as pt\n\nBASE_DIR = \"test/test_data/\"\ndata_dir = BASE_DIR\nsearch_path = \".\"\nwhile (\n not os.path.exists(data_dir)\n and os.path.dirname(search_path) != \"pytransform3d\"\n):\n search_path = os.path.join(search_path, \"..\")\n data_dir = os.path.join(search_path, BASE_DIR)\n\nintrinsic_matrix = np.loadtxt(\n os.path.join(data_dir, \"reconstruction_camera_matrix.csv\"), delimiter=\",\"\n)\n\nP = np.loadtxt(\n os.path.join(data_dir, \"reconstruction_odometry.csv\"),\n delimiter=\",\",\n skiprows=1,\n)\nfor t in range(len(P)):\n P[t, 3:] = pr.quaternion_wxyz_from_xyzw(P[t, 3:])\ncam2world_trajectory = ptr.transforms_from_pqs(P)\n\nplt.figure(figsize=(5, 5))\nax = pt.plot_transform(s=0.3)\nax = ptr.plot_trajectory(ax, P=P, s=0.1, n_frames=10)\n\nimage_size = np.array([1920, 1440])\n\nkey_frames_indices = np.linspace(0, len(P) - 1, 10, dtype=int)\ncolors = cycle(\"rgb\")\nfor i, c in zip(key_frames_indices, colors):\n pc.plot_camera(\n ax,\n intrinsic_matrix,\n cam2world_trajectory[i],\n sensor_size=image_size,\n virtual_image_distance=0.2,\n c=c,\n )\n\npos_min = np.min(P[:, :3], axis=0)\npos_max = np.max(P[:, :3], axis=0)\ncenter = (pos_max + pos_min) / 2.0\nmax_half_extent = max(pos_max - pos_min) / 2.0\nax.set_xlim((center[0] - max_half_extent, center[0] + max_half_extent))\nax.set_ylim((center[1] - max_half_extent, center[1] + max_half_extent))\nax.set_zlim((center[2] - max_half_extent, center[2] + max_half_extent))\n\nax.view_init(azim=110, elev=40)\n\nplt.show()\n" }, { "path": "examples/plots/plot_camera_with_image.py", "content": "\"\"\"\n================\nCamera Transform\n================\n\nWe can see the camera frame and the world frame. There is a grid of points from\nwhich we know the world coordinates. If we know the location and orientation of\nthe camera in the world, we can easily compute the location of the points on\nthe image.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d.camera import make_world_grid, world2image, plot_camera\nfrom pytransform3d.plot_utils import make_3d_axis\nfrom pytransform3d.rotations import active_matrix_from_intrinsic_euler_xyz\nfrom pytransform3d.transformations import transform_from, plot_transform\n\ncam2world = transform_from(\n active_matrix_from_intrinsic_euler_xyz([-np.pi + 1, -0.1, 0.3]),\n [0.2, -1, 0.5],\n)\nfocal_length = 0.0036\nsensor_size = (0.00367, 0.00274)\nimage_size = (640, 480)\nintrinsic_camera_matrix = np.array(\n [\n [focal_length, 0, sensor_size[0] / 2],\n [0, focal_length, sensor_size[1] / 2],\n [0, 0, 1],\n ]\n)\n\nworld_grid = make_world_grid(n_points_per_line=101)\nimage_grid = world2image(\n world_grid, cam2world, sensor_size, image_size, focal_length, kappa=0.4\n)\n\nplt.figure(figsize=(12, 5))\nax = make_3d_axis(1, 121, unit=\"m\")\nax.view_init(elev=30, azim=-70)\nplot_transform(ax)\nplot_transform(ax, A2B=cam2world, s=0.3, name=\"Camera\")\nplot_camera(\n ax,\n intrinsic_camera_matrix,\n cam2world,\n sensor_size=sensor_size,\n virtual_image_distance=0.5,\n)\nax.set_title(\"Camera and world frames\")\nax.scatter(world_grid[:, 0], world_grid[:, 1], world_grid[:, 2], s=1, alpha=0.2)\nax.scatter(world_grid[-1, 0], world_grid[-1, 1], world_grid[-1, 2], color=\"r\")\nax.view_init(elev=25, azim=-130)\n\nax = plt.subplot(122, aspect=\"equal\")\nax.set_title(\"Camera image\")\nax.set_xlim(0, image_size[0])\nax.set_ylim(0, image_size[1])\nax.scatter(image_grid[:, 0], -(image_grid[:, 1] - image_size[1]))\nax.scatter(image_grid[-1, 0], -(image_grid[-1, 1] - image_size[1]), color=\"r\")\n\nplt.show()\n" }, { "path": "examples/plots/plot_collision_objects.py", "content": "\"\"\"\n===========================\nURDF with Collision Objects\n===========================\n\nThis example shows how to load a URDF description of collision objects and\ndisplay them.\n\"\"\"\n\nimport matplotlib.pyplot as plt\n\nfrom pytransform3d.urdf import UrdfTransformManager\n\nURDF = \"\"\"\n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\"\"\"\n\n\ntm = UrdfTransformManager()\ntm.load_urdf(URDF)\nax = tm.plot_frames_in(\"collision\", s=0.1)\ntm.plot_collision_objects(\"collision\", ax)\nax.set_zlim((-0.5, 1.5))\nplt.show()\n" }, { "path": "examples/plots/plot_compare_rotations.py", "content": "\"\"\"\n========================================\nCompare Various Definitions of Rotations\n========================================\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d import rotations as pr\n\nax = pr.plot_basis(R=np.eye(3), ax_s=2, lw=3)\naxis = 2\nangle = np.pi\n\np = np.array([1.0, 1.0, 1.0])\neuler = [0, 0, 0]\neuler[axis] = angle\nR = pr.active_matrix_from_intrinsic_euler_xyz(euler)\npr.plot_basis(ax, R, p)\n\np = np.array([-1.0, 1.0, 1.0])\neuler = [0, 0, 0]\neuler[2 - axis] = angle\nR = pr.active_matrix_from_intrinsic_euler_zyx(euler)\npr.plot_basis(ax, R, p)\n\np = np.array([1.0, 1.0, -1.0])\nR = pr.active_matrix_from_angle(axis, angle)\npr.plot_basis(ax, R, p)\n\np = np.array([1.0, -1.0, -1.0])\ne = [pr.unitx, pr.unity, pr.unitz][axis]\na = np.hstack((e, (angle,)))\nR = pr.matrix_from_axis_angle(a)\npr.plot_basis(ax, R, p)\npr.plot_axis_angle(ax, a, p)\n\np = np.array([-1.0, -1.0, -1.0])\nq = pr.quaternion_from_axis_angle(a)\nR = pr.matrix_from_quaternion(q)\npr.plot_basis(ax, R, p)\n\nplt.show()\n" }, { "path": "examples/plots/plot_concatenate_uncertain_transforms.py", "content": "\"\"\"\n================================\nConcatenate Uncertain Transforms\n================================\n\nIn this example, we assume that a robot is moving with constant velocity\nalong the x-axis, however, there is noise in the orientation of the robot\nthat accumulates and leads to different paths when sampling. Uncertainty\naccumulation leads to the so-called banana distribution, which does not seem\nGaussian in Cartesian space, but it is Gaussian in exponential coordinates\nof SE(3).\n\nThis example adapted and modified to 3D from Barfoot and Furgale [1]_.\nThe banana distribution was analyzed in detail by Long et al. [2]_.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nimport pytransform3d.plot_utils as ppu\nimport pytransform3d.trajectories as ptr\nimport pytransform3d.transformations as pt\nimport pytransform3d.uncertainty as pu\n\n# %%\n# We configure the example here. You can change the random seed if you want.\n# We assume :math:`\\Delta t = 1 s` and constant velocity, so that in each step\n# we concatenate the transformation :math:`\\boldsymbol{T}_{vel}` defined\n# via its exponential coordinates here. The covariance associated with\n# :math:`\\boldsymbol{T}_{vel}` is constructed from its Cholesky decomposition\n# through :math:`\\boldsymbol{LL}^T`. Since it is a diagonal matrix at\n# the moment, we just define the standard deviation of each velocity component.\n# In the default configuration we have some uncertainty in the rotational\n# components of the velocity.\nrng = np.random.default_rng(0)\ncov_pose_chol = np.diag([0, 0.02, 0.03, 0, 0, 0])\ncov_pose = np.dot(cov_pose_chol, cov_pose_chol.T)\nvelocity_vector = np.array([0, 0, 0, 1.0, 0, 0])\nT_vel = pt.transform_from_exponential_coordinates(velocity_vector)\nn_steps = 100\nn_mc_samples = 1000\nn_skip_trajectories = 1 # plot every n-th trajectory\n\n# %%\n# We compute each path with respect to the initial pose of the robot, which\n# we set to :math:`\\boldsymbol{I}_{4 \\times 4}`. The covariance of the initial\n# pose is :math:`\\boldsymbol{0}_{6 \\times 6}`. We concatenate the current\n# pose with :math:`\\boldsymbol{T}_{vel}` in each step and factor in the\n# uncertainty of the pose and the transformation. Hence, uncertainties will\n# accumulate in the covariance of the pose.\nT_est = np.eye(4)\npath = np.zeros((n_steps + 1, 6))\npath[0] = pt.exponential_coordinates_from_transform(T_est)\ncov_est = np.zeros((6, 6))\nfor t in range(n_steps):\n T_est, cov_est = pu.concat_globally_uncertain_transforms(\n T_est, cov_est, T_vel, cov_pose\n )\n path[t + 1] = pt.exponential_coordinates_from_transform(T_est)\n\n# %%\n# We perform Monte-Carlo sampling of trajectories for comparison.\n# This implementation is vectorized with NumPy.\nT = np.eye(4)\nmc_path = np.zeros((n_steps + 1, n_mc_samples, 4, 4))\nmc_path[0, :] = T\nfor t in range(n_steps):\n noise_samples = ptr.transforms_from_exponential_coordinates(\n cov_pose_chol.dot(rng.standard_normal(size=(6, n_mc_samples))).T\n )\n step_samples = ptr.concat_many_to_one(noise_samples, T_vel)\n mc_path[t + 1] = np.einsum(\"nij,njk->nik\", step_samples, mc_path[t])\n# Plot the random samples' trajectory lines (in a frame attached to the start)\n# same as mc_path[t, i, :3, :3].T.dot(mc_path[t, i, :3, 3]), but faster\nmc_path_vec = np.einsum(\n \"tinm,tin->tim\", mc_path[:, :, :3, :3], mc_path[:, :, :3, 3]\n)\n\n# %%\n# We plot the MC-sampled trajectories and final poses, mean trajectory,\n# projected equiprobably hyperellipsoid of the final distribution, and\n# the equiprobably ellipsoid of the final distribution of the position.\nax = ppu.make_3d_axis(100)\n\nfor i in range(0, mc_path_vec.shape[1], n_skip_trajectories):\n ax.plot(\n mc_path_vec[:, i, 0],\n mc_path_vec[:, i, 1],\n mc_path_vec[:, i, 2],\n lw=1,\n c=\"b\",\n alpha=0.05,\n )\nax.scatter(\n mc_path_vec[-1, :, 0],\n mc_path_vec[-1, :, 1],\n mc_path_vec[-1, :, 2],\n s=3,\n c=\"b\",\n)\n\nptr.plot_trajectory(\n ax,\n ptr.pqs_from_transforms(ptr.transforms_from_exponential_coordinates(path)),\n s=5.0,\n lw=3,\n)\n\npu.plot_projected_ellipsoid(\n ax, T_est, cov_est, wireframe=False, alpha=0.3, color=\"y\", factor=3.0\n)\npu.plot_projected_ellipsoid(\n ax, T_est, cov_est, wireframe=True, alpha=0.5, color=\"y\", factor=3.0\n)\n\nmean_mc = np.mean(mc_path_vec[-1, :], axis=0)\ncov_mc = np.cov(mc_path_vec[-1, :], rowvar=False)\n\nellipsoid2origin, radii = pu.to_ellipsoid(mean_mc, cov_mc)\nppu.plot_ellipsoid(\n ax, 3.0 * radii, ellipsoid2origin, wireframe=False, alpha=0.1, color=\"m\"\n)\nppu.plot_ellipsoid(\n ax, 3.0 * radii, ellipsoid2origin, wireframe=True, alpha=0.3, color=\"m\"\n)\n\nplt.xlim((-5, 105))\nplt.ylim((-50, 50))\nplt.xlabel(\"x\")\nplt.ylabel(\"y\")\nax.view_init(elev=70, azim=-90)\nplt.show()\n\n# %%\n# References\n# ----------\n# .. [1] Barfoot, T. D., Furgale, P. T. (2014). Associating Uncertainty With\n# Three-Dimensional Poses for Use in Estimation Problems. IEEE Transactions\n# on Robotics 30(3), pp. 679-693, doi: 10.1109/TRO.2014.2298059.\n#\n# .. [2] Long, A. W., Wolfe, K. C., Mashner, M. J., Chirikjian, G. S. (2013).\n# The Banana Distribution is Gaussian: A Localization Study with Exponential\n# Coordinates. In Robotics: Science and Systems VIII, pp. 265-272.\n# http://www.roboticsproceedings.org/rss08/p34.pdf\n" }, { "path": "examples/plots/plot_convention_rotation_global_local.py", "content": "\"\"\"\n================================================================\nConvention for Rotation: Passive / Active, Extrinsic / Intrinsic\n================================================================\n\nWe will compare all possible combinations of passive and active\nrotations and extrinsic and intrinsic concatenation of rotations.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\nfrom mpl_toolkits.mplot3d import proj3d # noqa: F401\n\nfrom pytransform3d.rotations import (\n passive_matrix_from_angle,\n active_matrix_from_angle,\n plot_basis,\n)\n\n# %%\n# Passive Extrinsic Rotations\n# ---------------------------\nplt.figure(figsize=(8, 3))\naxes = [plt.subplot(1, 3, 1 + i, projection=\"3d\") for i in range(3)]\nfor ax in axes:\n plt.setp(\n ax,\n xlim=(-1, 1),\n ylim=(-1, 1),\n zlim=(-1, 1),\n xlabel=\"X\",\n ylabel=\"Y\",\n zlabel=\"Z\",\n )\n\nRx45 = passive_matrix_from_angle(0, np.deg2rad(45))\nRz45 = passive_matrix_from_angle(2, np.deg2rad(45))\n\naxes[0].set_title(\"Passive Extrinsic Rotations\", y=0.95)\nplot_basis(ax=axes[0], R=np.eye(3))\naxes[1].set_title(\"$R_x(45^{\\circ})$\", y=0.95)\nplot_basis(ax=axes[1], R=Rx45)\naxes[2].set_title(\"$R_z(45^{\\circ}) R_x(45^{\\circ})$\", y=0.95)\nplot_basis(ax=axes[2], R=Rz45.dot(Rx45))\n\nplt.tight_layout()\n\n# %%\n# Passive Intrinsic Rotations\n# ---------------------------\nplt.figure(figsize=(8, 3))\naxes = [plt.subplot(1, 3, 1 + i, projection=\"3d\") for i in range(3)]\nfor ax in axes:\n plt.setp(\n ax,\n xlim=(-1, 1),\n ylim=(-1, 1),\n zlim=(-1, 1),\n xlabel=\"X\",\n ylabel=\"Y\",\n zlabel=\"Z\",\n )\n\naxes[0].set_title(\"Passive Intrinsic Rotations\", y=0.95)\nplot_basis(ax=axes[0], R=np.eye(3))\naxes[1].set_title(\"$R_x(45^{\\circ})$\", y=0.95)\nplot_basis(ax=axes[1], R=Rx45)\naxes[2].set_title(\"$R_x(45^{\\circ}) R_{z'}(45^{\\circ})$\", y=0.95)\nplot_basis(ax=axes[2], R=Rx45.dot(Rz45))\n\nplt.tight_layout()\n\n# %%\n# Active Extrinsic Rotations\n# --------------------------\nplt.figure(figsize=(8, 3))\naxes = [plt.subplot(1, 3, 1 + i, projection=\"3d\") for i in range(3)]\nfor ax in axes:\n plt.setp(\n ax,\n xlim=(-1, 1),\n ylim=(-1, 1),\n zlim=(-1, 1),\n xlabel=\"X\",\n ylabel=\"Y\",\n zlabel=\"Z\",\n )\n\nRx45 = active_matrix_from_angle(0, np.deg2rad(45))\nRz45 = active_matrix_from_angle(2, np.deg2rad(45))\n\naxes[0].set_title(\"Active Extrinsic Rotations\", y=0.95)\nplot_basis(ax=axes[0], R=np.eye(3))\naxes[1].set_title(\"$R_x(45^{\\circ})$\", y=0.95)\nplot_basis(ax=axes[1], R=Rx45)\naxes[2].set_title(\"$R_z(45^{\\circ}) R_x(45^{\\circ})$\", y=0.95)\nplot_basis(ax=axes[2], R=Rz45.dot(Rx45))\n\nplt.tight_layout()\n\n# %%\n# Active Intrinsic Rotations\n# --------------------------\nplt.figure(figsize=(8, 3))\naxes = [plt.subplot(1, 3, 1 + i, projection=\"3d\") for i in range(3)]\nfor ax in axes:\n plt.setp(\n ax,\n xlim=(-1, 1),\n ylim=(-1, 1),\n zlim=(-1, 1),\n xlabel=\"X\",\n ylabel=\"Y\",\n zlabel=\"Z\",\n )\n\naxes[0].set_title(\"Active Intrinsic Rotations\", y=0.95)\nplot_basis(ax=axes[0], R=np.eye(3))\naxes[1].set_title(\"$R_x(45^{\\circ})$\", y=0.95)\nplot_basis(ax=axes[1], R=Rx45)\naxes[2].set_title(\"$R_x(45^{\\circ}) R_{z'}(45^{\\circ})$\", y=0.95)\nplot_basis(ax=axes[2], R=Rx45.dot(Rz45))\n\nplt.tight_layout()\n\nplt.show()\n" }, { "path": "examples/plots/plot_cylinder.py", "content": "\"\"\"\n==========================\nPlot Transformed Cylinders\n==========================\n\nPlots surfaces of transformed cylindrical shells.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d.plot_utils import plot_cylinder, remove_frame\nfrom pytransform3d.rotations import random_axis_angle, matrix_from_axis_angle\nfrom pytransform3d.transformations import transform_from, plot_transform\n\nrng = np.random.default_rng(44)\nA2B = transform_from(\n R=matrix_from_axis_angle(random_axis_angle(rng)),\n p=rng.standard_normal(size=3),\n)\nax = plot_cylinder(\n length=1.0, radius=0.3, thickness=0.1, wireframe=False, alpha=0.2\n)\nplot_transform(ax=ax, A2B=np.eye(4), s=0.3, lw=3)\nplot_cylinder(\n ax=ax,\n length=1.0,\n radius=0.3,\n thickness=0.1,\n A2B=A2B,\n wireframe=False,\n alpha=0.2,\n)\nplot_transform(ax=ax, A2B=A2B, s=0.3, lw=3)\nremove_frame(ax)\nax.set_xlim((0, 1.5))\nax.set_ylim((-1.5, 0))\nax.set_zlim((-0.8, 0.7))\nplt.show()\n" }, { "path": "examples/plots/plot_dual_quaternion_interpolation.py", "content": "\"\"\"\n=============================\nDual Quaternion Interpolation\n=============================\n\nThis example shows interpolated trajectories between two random poses.\nThe red line corresponds to linear interpolation with exponential coordinates,\nthe green line corresponds to linear interpolation with dual quaternions,\nand the blue line corresponds to screw linear interpolation (ScLERP) with\ndual quaternions. The true screw motion from pose 1 to pose 2 is shown by\na thick, transparent black line in the background of the ScLERP interpolation.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nimport pytransform3d.plot_utils as ppu\nimport pytransform3d.trajectories as ptr\nimport pytransform3d.transformations as pt\n\n# %%\n# Setup\n# -----\n# We generate two random transformation matrices to represent two poses\n# between we want to interpolate. These will also be converted to dual\n# quaternions, exponential coordinates, and position and quaternion.\nrng = np.random.default_rng(25)\npose1 = pt.random_transform(rng)\npose2 = pt.random_transform(rng)\ndq1 = pt.dual_quaternion_from_transform(pose1)\ndq2 = -pt.dual_quaternion_from_transform(pose2)\nStheta1 = pt.exponential_coordinates_from_transform(pose1)\nStheta2 = pt.exponential_coordinates_from_transform(pose2)\npq1 = pt.pq_from_transform(pose1)\npq2 = pt.pq_from_transform(pose2)\n\nn_steps = 100\n\n# %%\n# Ground Truth: Screw Motion\n# --------------------------\n# The ground truth for interpolation of poses is a linear interpolation of\n# rotation about and translation along the screw axis. We will represent the\n# difference between the two poses as exponential coordinates, a product of the\n# screw axis and the magnitude of the transformation. We can use fractions of\n# the exponential coordinates to smoothly interpolate between the two poses.\npose12pose2 = pt.concat(pose2, pt.invert_transform(pose1))\nStheta = pt.exponential_coordinates_from_transform(pose12pose2)\noffsets = ptr.transforms_from_exponential_coordinates(\n Stheta[np.newaxis] * np.linspace(0, 1, n_steps)[:, np.newaxis]\n)\ninterpolated_poses = ptr.concat_many_to_one(offsets, pose1)\n\n# %%\n# Approximation: Linear Interpolation of Dual Quaternions\n# -------------------------------------------------------\n# An approximately correct solution is linear interpolation and subsequent\n# normalization of dual quaternions. The problem with dual quaternions is that\n# they have a double cover and the path of the interpolation might be different\n# depending on which of the two representation of the pose is selected. In this\n# case the path does not match the ground truth path, but when we switch from\n# dq1 to pt.dual_quaternion_double(dq1)---its alternative representation---the\n# interpolation path is very close to the ground truth.\ninterpolated_dqs = (\n np.linspace(1, 0, n_steps)[:, np.newaxis] * dq1\n + np.linspace(0, 1, n_steps)[:, np.newaxis] * dq2\n)\n# renormalization (not required here because it will be done with conversion)\ninterpolated_dqs /= np.linalg.norm(interpolated_dqs[:, :4], axis=1)[\n :, np.newaxis\n]\ninterpolated_poses_from_dqs = ptr.transforms_from_dual_quaternions(\n interpolated_dqs\n)\n\n# %%\n# Exact Solution: Screw Linear Interpolation (ScLERP)\n# ---------------------------------------------------\n# Dual quaternions also support screw linear interpolation (ScLERP) which is\n# implemented with the dual quaternion power. The dual quaternion power\n# internally uses the screw parameters of the pose difference to smoothly\n# interpolate along the screw axis.\nsclerp_interpolated_dqs = np.vstack(\n [pt.dual_quaternion_sclerp(dq1, dq2, t) for t in np.linspace(0, 1, n_steps)]\n)\nsclerp_interpolated_poses_from_dqs = ptr.transforms_from_dual_quaternions(\n sclerp_interpolated_dqs\n)\n\n# %%\n# Approximation: Linear Interpolation of Exponential Coordinates\n# --------------------------------------------------------------\n# A more crude approximation is the linear interpolation of exponential\n# coordinates.\ninterpolated_ecs = (\n np.linspace(1, 0, n_steps)[:, np.newaxis] * Stheta1\n + np.linspace(0, 1, n_steps)[:, np.newaxis] * Stheta2\n)\ninterpolates_poses_from_ecs = ptr.transforms_from_exponential_coordinates(\n interpolated_ecs\n)\n\n# %%\n# Linear Interpolation of Position + SLERP\n# ----------------------------------------\n# A completly different solution can be obtained by decomposing the poses into\n# positions and orientations and then using spherical linear interpolation\n# (SLERP) of the orientation (in this case: quaternions).\ninterpolated_pqs = np.vstack(\n [pt.pq_slerp(pq1, pq2, t) for t in np.linspace(0, 1, n_steps)]\n)\ninterpolated_poses_from_pqs = ptr.transforms_from_pqs(interpolated_pqs)\n\n# %%\n# Plotting\n# --------\n# We show all solutions in one 3D plot.\nax = pt.plot_transform(A2B=pose1, s=0.3, ax_s=2)\npt.plot_transform(A2B=pose2, s=0.3, ax=ax)\ntraj = ppu.Trajectory(\n interpolated_poses, s=0.1, c=\"k\", lw=5, alpha=0.5, show_direction=True\n)\ntraj.add_trajectory(ax)\ntraj_from_dqs = ppu.Trajectory(\n interpolated_poses_from_dqs, s=0.1, c=\"g\", show_direction=False\n)\ntraj_from_dqs.add_trajectory(ax)\ntraj_from_ecs = ppu.Trajectory(\n interpolates_poses_from_ecs, s=0.1, c=\"r\", show_direction=False\n)\ntraj_from_ecs.add_trajectory(ax)\ntraj_from_dqs_sclerp = ppu.Trajectory(\n sclerp_interpolated_poses_from_dqs, s=0.1, c=\"b\", show_direction=False\n)\ntraj_from_dqs_sclerp.add_trajectory(ax)\ntraj_from_pq_slerp = ppu.Trajectory(\n interpolated_poses_from_pqs, s=0.1, c=\"c\", show_direction=False\n)\ntraj_from_pq_slerp.add_trajectory(ax)\nplt.legend(\n [\n traj.trajectory,\n traj_from_dqs.trajectory,\n traj_from_ecs.trajectory,\n traj_from_dqs_sclerp.trajectory,\n traj_from_pq_slerp.trajectory,\n ],\n [\n \"Screw interpolation\",\n \"Linear dual quaternion interpolation\",\n \"Linear interpolation of exp. coordinates\",\n \"Dual quaternion ScLERP\",\n \"Linear interpolation of position + SLERP of quaternions\",\n ],\n loc=\"best\",\n)\nplt.show()\n" }, { "path": "examples/plots/plot_euler_angles.py", "content": "\"\"\"\n============\nEuler Angles\n============\n\nAny rotation can be represented by three consecutive rotations about three\nbasis vectors. Here we use the extrinsic xyz convention.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d import rotations as pr\nfrom pytransform3d.plot_utils import remove_frame\n\nalpha, beta, gamma = 0.5 * np.pi, 0.5 * np.pi, 0.5 * np.pi\np = np.array([1, 1, 1])\n\nplt.figure(figsize=(5, 5))\n\nax = pr.plot_basis(R=np.eye(3), p=-1.5 * p, ax_s=2)\npr.plot_axis_angle(ax, [1, 0, 0, alpha], -1.5 * p)\n\npr.plot_basis(\n ax, pr.active_matrix_from_extrinsic_euler_xyz([alpha, 0, 0]), -0.5 * p\n)\npr.plot_axis_angle(ax, [0, 1, 0, beta], p=-0.5 * p)\n\npr.plot_basis(\n ax, pr.active_matrix_from_extrinsic_euler_xyz([alpha, beta, 0]), 0.5 * p\n)\npr.plot_axis_angle(ax, [0, 0, 1, gamma], 0.5 * p)\n\npr.plot_basis(\n ax,\n pr.active_matrix_from_extrinsic_euler_xyz([alpha, beta, gamma]),\n 1.5 * p,\n lw=5,\n)\n\nremove_frame(ax)\n\nplt.show()\n" }, { "path": "examples/plots/plot_frames.py", "content": "\"\"\"\n===============================================\nPlot with Respect to Different Reference Frames\n===============================================\n\nIn this example, we will demonstrate how to use the TransformManager.\nWe will add several transforms to the manager and plot all frames in\ntwo reference frames ('world' and 'A').\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d.plot_utils import make_3d_axis\nfrom pytransform3d.transform_manager import TransformManager\nfrom pytransform3d.transformations import random_transform\n\nrng = np.random.default_rng(5)\nA2world = random_transform(rng)\nB2world = random_transform(rng)\nA2C = random_transform(rng)\nD2B = random_transform(rng)\n\ntm = TransformManager()\ntm.add_transform(\"A\", \"world\", A2world)\ntm.add_transform(\"B\", \"world\", B2world)\ntm.add_transform(\"A\", \"C\", A2C)\ntm.add_transform(\"D\", \"B\", D2B)\n\nplt.figure(figsize=(10, 5))\n\nax = make_3d_axis(2, 121)\nax = tm.plot_frames_in(\"world\", ax=ax, alpha=0.6)\nax.view_init(30, 20)\n\nax = make_3d_axis(3, 122)\nax = tm.plot_frames_in(\"A\", ax=ax, alpha=0.6)\nax.view_init(30, 20)\n\nplt.show()\n" }, { "path": "examples/plots/plot_interpolation_for_transform_manager.py", "content": "\"\"\"\n==================================\nManaging Transformations over Time\n==================================\n\nIn this example, given two trajectories of 3D rigid transformations, we will\ninterpolate both and use the transform manager for the target timestep.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d import rotations as pr\nfrom pytransform3d import transformations as pt\nfrom pytransform3d.transform_manager import (\n TemporalTransformManager,\n NumpyTimeseriesTransform,\n)\n\n\ndef create_sinusoidal_movement(\n duration_sec, sample_period, x_velocity, y_start_offset, start_time\n):\n \"\"\"Create a planar (z=0) sinusoidal movement around x-axis.\"\"\"\n time = np.arange(0, duration_sec, sample_period) + start_time\n n_steps = len(time)\n x = np.linspace(0, x_velocity * duration_sec, n_steps)\n\n spatial_freq = 1.0 / 5.0 # 1 sinus per 5m\n omega = 2.0 * np.pi * spatial_freq\n y = np.sin(omega * x)\n y += y_start_offset\n\n dydx = omega * np.cos(omega * x)\n yaw = np.arctan2(dydx, np.ones_like(dydx))\n\n pqs = []\n for i in range(n_steps):\n R = pr.active_matrix_from_extrinsic_euler_zyx([yaw[i], 0, 0])\n T = pt.transform_from(R, [x[i], y[i], 0])\n pq = pt.pq_from_transform(T)\n pqs.append(pq)\n\n return time, np.array(pqs)\n\n\n# create entities A and B together with their transformations from world\nduration = 10.0 # [s]\nsample_period = 0.5 # [s]\nvelocity_x = 1 # [m/s]\ntime_A, pqs_A = create_sinusoidal_movement(\n duration, sample_period, velocity_x, y_start_offset=2.0, start_time=0.1\n)\ntime_B, pqs_B = create_sinusoidal_movement(\n duration, sample_period, velocity_x, y_start_offset=-2.0, start_time=0.35\n)\n\n# package them into an instance of `TimeVaryingTransform` abstract class\ntransform_WA = NumpyTimeseriesTransform(time_A, pqs_A)\ntransform_WB = NumpyTimeseriesTransform(time_B, pqs_B)\n\ntm = TemporalTransformManager()\n\ntm.add_transform(\"A\", \"world\", transform_WA)\ntm.add_transform(\"B\", \"world\", transform_WB)\n\nquery_time = 4.9 # [s] or an array of times np.array([4.9, 5.2])\nA2B_at_query_time = tm.get_transform_at_time(\"A\", \"B\", query_time)\n\n# transform the origin of A in A (x=0, y=0, z=0) to B\norigin_of_A_pos = pt.vector_to_point([0, 0, 0])\norigin_of_A_in_B_xyz = pt.transform(A2B_at_query_time, origin_of_A_pos)[:-1]\n\n# for visualization purposes\npq_A = pt.pq_from_transform(transform_WA.as_matrix(query_time))\npq_B = pt.pq_from_transform(transform_WB.as_matrix(query_time))\n\nplt.figure(figsize=(8, 8))\nplt.plot(pqs_A[:, 0], pqs_A[:, 1], \"bo--\", label=\"trajectory $A(t)$\")\nplt.plot(pqs_B[:, 0], pqs_B[:, 1], \"yo--\", label=\"trajectory $B(t)$\")\nplt.scatter(pq_A[0], pq_A[1], color=\"b\", s=120, marker=\"d\", label=\"$A(t_q)$\")\nplt.scatter(pq_B[0], pq_B[1], color=\"y\", s=120, marker=\"^\", label=\"$B(t_q)$\")\nplt.text(\n pq_A[0] + 0.3,\n pq_A[1] - 0.3,\n f\"origin of A in B:\\n(x={origin_of_A_in_B_xyz[0]:.2f}m,\"\n + f\" y={origin_of_A_in_B_xyz[1]:.2f}m)\",\n)\nplt.xlabel(\"x [m]\")\nplt.ylabel(\"y [m]\")\nplt.xlim(0, 10)\nplt.ylim(-5, 5)\nplt.grid()\nplt.legend()\nplt.show()\n" }, { "path": "examples/plots/plot_invert_uncertain_transform.py", "content": "\"\"\"\n==========================\nInvert Uncertain Transform\n==========================\n\nWe sample from the original transform distribution and from the inverse\ndistribution. Samples are then projected to all 2D planes and plotted.\nThe color indicates the pose distribution. Green is the original\ndistribution and red is the inverse.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nimport pytransform3d.rotations as pr\nimport pytransform3d.trajectories as ptr\nimport pytransform3d.transformations as pt\nimport pytransform3d.uncertainty as pu\n\nn_mc_samples = 1000\nrng = np.random.default_rng(1)\nalpha = 2.0\n\nA2B = pt.transform_from(\n R=pr.matrix_from_euler([1.5, 0.5, 1.3], 0, 1, 2, True), p=[10.0, -7.0, -5.0]\n)\nvariances = alpha * np.array([0.1, 0.2, 0.1, 2.0, 1.0, 1.0])\nstd_devs = np.sqrt(variances)\ncorrelations = np.array(\n [\n [0.5, 0, 0, 0, 0, 0],\n [0.1, 0.5, 0, 0, 0, 0],\n [-0.3, 0.1, 0.5, 0, 0, 0],\n [-0.1, 0.2, 0.3, 0.5, 0, 0],\n [-0.3, -0.1, -0.2, 0.3, 0.5, 0],\n [-0.1, 0.1, 0.2, 0.1, 0.3, 0.5],\n ]\n)\ncorrelations += correlations.T\ncov_A2B = correlations * np.outer(std_devs, std_devs)\nx_A2B = pt.exponential_coordinates_from_transform(A2B)\nsamples_A2B = ptr.exponential_coordinates_from_transforms(\n [pt.random_transform(rng, A2B, cov_A2B) for _ in range(n_mc_samples)]\n)\n\nB2A, cov_B2A = pu.invert_uncertain_transform(A2B, cov_A2B)\nx_B2A = pt.exponential_coordinates_from_transform(B2A)\nsamples_B2A = ptr.exponential_coordinates_from_transforms(\n [pt.random_transform(rng, B2A, cov_B2A) for _ in range(n_mc_samples)]\n)\n\n_, axes = plt.subplots(nrows=6, ncols=6, squeeze=True, figsize=(10, 10))\nfor i in range(6):\n for j in range(6):\n if i == j:\n continue\n\n indices = np.array([i, j])\n ax = axes[i][j]\n\n ax.scatter(\n samples_A2B[:, i], samples_A2B[:, j], color=\"g\", s=1, alpha=0.3\n )\n ax.scatter(\n samples_B2A[:, i], samples_B2A[:, j], color=\"r\", s=1, alpha=0.3\n )\n\n ax.scatter(0, 0, color=\"k\")\n\nplt.show()\n" }, { "path": "examples/plots/plot_matrix_from_two_vectors.py", "content": "\"\"\"\n==========================================\nConstruct Rotation Matrix from Two Vectors\n==========================================\n\nWe compute rotation matrix from two vectors that form a plane. The x-axis will\npoint in the same direction as the first vector, the y-axis corresponds to the\nnormalized vector rejection of b on a, and the z-axis is the cross product of\nthe other basis vectors.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d.plot_utils import plot_vector\nfrom pytransform3d.rotations import (\n matrix_from_two_vectors,\n plot_basis,\n random_vector,\n)\n\nrng = np.random.default_rng(1)\na = random_vector(rng, 3) * 0.3\nb = random_vector(rng, 3) * 0.3\nR = matrix_from_two_vectors(a, b)\n\nax = plot_vector(direction=a, color=\"r\")\nplot_vector(ax=ax, direction=b, color=\"g\")\nplot_basis(ax=ax, R=R)\nplt.show()\n" }, { "path": "examples/plots/plot_mesh.py", "content": "\"\"\"\n=========\nPlot Mesh\n=========\n\nThis example shows how to load an STL mesh. This example must be\nrun from within the main folder because it uses a\nhard-coded path to the STL file.\n\"\"\"\n\nimport os\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d.plot_utils import plot_mesh\nfrom pytransform3d.transformations import plot_transform\n\nBASE_DIR = \"test/test_data/\"\ndata_dir = BASE_DIR\nsearch_path = \".\"\nwhile (\n not os.path.exists(data_dir)\n and os.path.dirname(search_path) != \"pytransform3d\"\n):\n search_path = os.path.join(search_path, \"..\")\n data_dir = os.path.join(search_path, BASE_DIR)\n\nax = plot_mesh(\n filename=os.path.join(data_dir, \"cone.stl\"), s=5 * np.ones(3), alpha=0.3\n)\nplot_transform(ax=ax, A2B=np.eye(4), s=0.3, lw=3)\nplt.show()\n" }, { "path": "examples/plots/plot_polar_decomposition.py", "content": "\"\"\"\n========================\nPlot Polar Decomposition\n========================\n\nRobust polar decomposition orthonormalizes basis vectors (i.e., rotation\nmatrices). It is more expensive than standard Gram-Schmidt orthonormalization,\nbut it spreads the error more evenly over all basis vectors. The top row of\nthese plots shows the unnormalized bases that were obtained by randomly\nrotating one of the columns of the identity matrix. The middle row shows\nGram-Schmidt orthonormalization and the bottom row shows orthonormalization\nthrough robust polar decomposition. For comparison, we show the unnormalized\nbasis with dashed lines in the last two rows.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d import rotations as pr\n\nn_cases = 4\nfig, axes = plt.subplots(\n 3, n_cases, subplot_kw={\"projection\": \"3d\"}, figsize=(8, 8)\n)\nax_s = 1.0\nplot_center = np.array([-0.2, -0.2, -0.2])\nfor ax in axes.flat:\n ax.set_xticks([])\n ax.set_yticks([])\n ax.set_zticks([])\n ax.set_xlim(-ax_s, ax_s)\n ax.set_ylim(-ax_s, ax_s)\n ax.set_zlim(-ax_s, ax_s)\n\ntitles = [\"Unnormalized Bases\", \"Gram-Schmidt\", \"Polar Decomposition\"]\nfor ax, title in zip(axes[:, 0], titles):\n ax.set_title(title)\n\nrng = np.random.default_rng(46)\nfor i in range(n_cases):\n random_axis = rng.integers(0, 3)\n R_unnormalized = np.eye(3)\n R_unnormalized[:, random_axis] = np.dot(\n pr.random_matrix(rng, cov=0.1 * np.eye(3)),\n R_unnormalized[:, random_axis],\n )\n pr.plot_basis(axes[0, i], R_unnormalized, p=plot_center, strict_check=False)\n\n R_gs = pr.norm_matrix(R_unnormalized)\n pr.plot_basis(\n axes[1, i], R_unnormalized, p=plot_center, strict_check=False, ls=\"--\"\n )\n pr.plot_basis(axes[1, i], R_gs, p=plot_center)\n\n R_pd = pr.robust_polar_decomposition(R_unnormalized)\n pr.plot_basis(\n axes[2, i], R_unnormalized, p=plot_center, strict_check=False, ls=\"--\"\n )\n pr.plot_basis(axes[2, i], R_pd, p=plot_center)\n\nplt.tight_layout()\nplt.show()\n" }, { "path": "examples/plots/plot_pose_fusion.py", "content": "\"\"\"\n============\nFuse 3 Poses\n============\n\nEach of the poses has an associated covariance that is considered during\nthe fusion. Each of the plots shows a projection of the distributions in\nexponential coordinate space to two dimensions. Red, green, and blue ellipses\nrepresent the uncertain poses that will be fused. A black ellipse indicates\nthe fused pose's distribution.\n\nThis example is from\n\nBarfoot, Furgale: Associating Uncertainty With Three-Dimensional Poses for Use\nin Estimation Problems, http://ncfrn.mcgill.ca/members/pubs/barfoot_tro14.pdf\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nimport pytransform3d.transformations as pt\nimport pytransform3d.uncertainty as pu\n\n\n# %%\n# Helper Functions\n# ----------------\n# Here you find some helper functions for plotting.\ndef to_ellipse(cov, factor=1.0):\n \"\"\"Compute error ellipse.\n\n An error ellipse shows equiprobable points of a 2D Gaussian distribution.\n\n Parameters\n ----------\n cov : array-like, shape (2, 2)\n Covariance of the Gaussian distribution.\n\n factor : float, optional (default: 1)\n One means standard deviation.\n\n Returns\n -------\n angle : float\n Rotation angle of the ellipse.\n\n width : float\n Width of the ellipse (semi axis, not diameter).\n\n height : float\n Height of the ellipse (semi axis, not diameter).\n \"\"\"\n from scipy import linalg\n\n vals, vecs = linalg.eigh(cov)\n order = vals.argsort()[::-1]\n vals, vecs = vals[order], vecs[:, order]\n angle = np.arctan2(*vecs[:, 0][::-1])\n width, height = factor * np.sqrt(vals)\n return angle, width, height\n\n\ndef plot_error_ellipse(\n ax, mean, cov, color=None, alpha=0.25, factors=np.linspace(0.25, 2.0, 8)\n):\n \"\"\"Plot error ellipse of MVN.\n\n Parameters\n ----------\n ax : axis\n Matplotlib axis.\n\n mean : array-like, shape (2,)\n Mean of the Gaussian distribution.\n\n cov : array-like, shape (2, 2)\n Covariance of the Gaussian distribution.\n\n color : str, optional (default: None)\n Color in which the ellipse should be plotted\n\n alpha : float, optional (default: 0.25)\n Alpha value for ellipse\n\n factors : array, optional (default: np.linspace(0.25, 2.0, 8))\n Multiples of the standard deviations that should be plotted.\n \"\"\"\n from matplotlib.patches import Ellipse\n\n for factor in factors:\n angle, width, height = to_ellipse(cov, factor)\n ell = Ellipse(\n xy=mean,\n width=2.0 * width,\n height=2.0 * height,\n angle=np.degrees(angle),\n )\n ell.set_alpha(alpha)\n if color is not None:\n ell.set_color(color)\n ax.add_artist(ell)\n\n\n# %%\n# Example Configuration\n# ---------------------\n# A ground truth pose is known in this example. We sample 3 poses with\n# different covariances around the ground truth pose.\nx_true = np.array([1.0, 0.0, 0.0, 0.0, 0.0, np.pi / 6.0])\nT_true = pt.transform_from_exponential_coordinates(x_true)\nalpha = 5.0\ncov1 = alpha * np.diag([0.1, 0.2, 0.1, 2.0, 1.0, 1.0])\ncov2 = alpha * np.diag([0.1, 0.1, 0.2, 1.0, 3.0, 1.0])\ncov3 = alpha * np.diag([0.2, 0.1, 0.1, 1.0, 1.0, 5.0])\n\nrng = np.random.default_rng(0)\n\nT1 = np.dot(\n pt.transform_from_exponential_coordinates(\n pt.random_exponential_coordinates(rng=rng, cov=cov1)\n ),\n T_true,\n)\nT2 = np.dot(\n pt.transform_from_exponential_coordinates(\n pt.random_exponential_coordinates(rng=rng, cov=cov2)\n ),\n T_true,\n)\nT3 = np.dot(\n pt.transform_from_exponential_coordinates(\n pt.random_exponential_coordinates(rng=rng, cov=cov3)\n ),\n T_true,\n)\n\nx1 = pt.exponential_coordinates_from_transform(T1)\nx2 = pt.exponential_coordinates_from_transform(T2)\nx3 = pt.exponential_coordinates_from_transform(T3)\n\n# %%\n# Fusion\n# ------\n# We can then fuse the 3 sampled poses with their associated covariances.\nT_est, cov_est, V = pu.pose_fusion([T1, T2, T3], [cov1, cov2, cov3])\n\n# %%\n# Plotting\n# --------\n# Each subplot shows two dimensions of the exponential coordinate space.\n# Each sampled pose and its associated covariance matrix is displayed as an\n# equiprobable ellipse. The fused pose estimate is also shown with its\n# associated covariance as an equiprobable ellipse. For comparison, the ground\n# truth pose is shown as a point.\nx_est = pt.exponential_coordinates_from_transform(T_est)\n_, axes = plt.subplots(\n nrows=6, ncols=6, sharex=True, sharey=True, squeeze=True, figsize=(10, 10)\n)\nfactors = [1.0]\nfor i in range(6):\n for j in range(6):\n if i == j:\n continue\n\n indices = np.array([i, j])\n ax = axes[i][j]\n\n for x, cov, color in zip([x1, x2, x3], [cov1, cov2, cov3], \"rgb\"):\n plot_error_ellipse(\n ax,\n x[indices],\n cov[indices][:, indices],\n color=color,\n alpha=0.4,\n factors=factors,\n )\n\n plot_error_ellipse(\n ax,\n x_est[indices],\n cov_est[indices][:, indices],\n color=\"k\",\n alpha=1,\n factors=factors,\n )\n\n ax.scatter(x_true[i], x_true[j])\n\n ax.set_xlim((-10, 10))\n ax.set_ylim((-10, 10))\n\nplt.show()\n" }, { "path": "examples/plots/plot_pose_trajectory.py", "content": "\"\"\"\n===============\nPose Trajectory\n===============\n\nPlotting pose trajectories with pytransform3d is easy.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d.batch_rotations import quaternion_slerp_batch\nfrom pytransform3d.rotations import q_id\nfrom pytransform3d.trajectories import plot_trajectory\n\nn_steps = 100000\nP = np.empty((n_steps, 7))\nP[:, 0] = np.cos(np.linspace(-2 * np.pi, 2 * np.pi, n_steps))\nP[:, 1] = np.sin(np.linspace(-2 * np.pi, 2 * np.pi, n_steps))\nP[:, 2] = np.linspace(-1, 1, n_steps)\nq_end = np.array([0.0, 0.0, np.sqrt(0.5), np.sqrt(0.5)])\nP[:, 3:] = quaternion_slerp_batch(q_id, q_end, np.linspace(0, 1, n_steps))\n\nax = plot_trajectory(\n P=P, s=0.3, n_frames=100, normalize_quaternions=False, lw=2, c=\"k\"\n)\nplt.show()\n" }, { "path": "examples/plots/plot_quaternion_integrate.py", "content": "\"\"\"\n======================\nQuaternion Integration\n======================\n\nIntegrate angular velocities to a sequence of quaternions.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d.rotations import (\n quaternion_integrate,\n matrix_from_quaternion,\n plot_basis,\n)\n\nangular_velocities = np.empty((21, 3))\nangular_velocities[:, :] = np.array([np.sqrt(0.5), np.sqrt(0.5), 0.0])\nangular_velocities *= np.pi\n\nQ = quaternion_integrate(angular_velocities, dt=0.1)\nax = None\nfor t in range(len(Q)):\n R = matrix_from_quaternion(Q[t])\n p = 2 * (t / (len(Q) - 1) - 0.5) * np.ones(3)\n ax = plot_basis(ax=ax, s=0.15, R=R, p=p)\nplt.show()\n" }, { "path": "examples/plots/plot_quaternion_slerp.py", "content": "\"\"\"\n================\nQuaternion SLERP\n================\n\nFor small rotations, linear interpolation of quaternions gives almost the same\nresults as spherical linear interpolation (SLERP). For larger angles there are\nsignificant differences as you can see in this example. The outer circle uses\nlinear interpolation and the inner circle uses SLERP. You can play around with\nthe value of 'end_angle' in this example.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d.rotations import (\n matrix_from_axis_angle,\n quaternion_from_matrix,\n quaternion_slerp,\n)\nfrom pytransform3d.trajectories import plot_trajectory\n\n# %%\n# We assume the array T represents something like time.\n# The position follows a sigmoid profile on a circular path over time, hence\n# velocity is not constant.\nT = np.linspace(0, 1, 1001)\nsigmoid = 0.5 * (np.tanh(1.5 * np.pi * (T - 0.5)) + 1.0)\nradius = 0.5\nstart_angle = np.deg2rad(0.0)\nend_angle = np.deg2rad(350.0)\n\nR1 = matrix_from_axis_angle([0, 0, 1, 0.5 * np.pi])\nR2_start = matrix_from_axis_angle([1, 0, 0, start_angle])\nR2_end = matrix_from_axis_angle([1, 0, 0, end_angle])\nq_start = quaternion_from_matrix(R1.dot(R2_start))\nq_end = quaternion_from_matrix(R1.dot(R2_end))\n\n# %%\n# The naive linear interpolation method computes a time-weighted average\n# between the orientation at the start and the orientation at the end.\nlerp = np.zeros((len(T), 7))\nlerp[:, 0] = radius * np.cos(np.deg2rad(90) - end_angle * sigmoid)\nlerp[:, 2] = radius * np.sin(np.deg2rad(90) - end_angle * sigmoid)\nif end_angle > np.pi:\n q_end *= -1.0\nlerp[:, 3:] = (1.0 - T)[:, np.newaxis] * q_start + T[:, np.newaxis] * q_end\n\n# %%\n# SLERP is the exact method to interpolate the orientations.\nslerp = np.zeros((len(T), 7))\nslerp[:, 0] = 0.7 * radius * np.cos(np.deg2rad(90) - end_angle * sigmoid)\nslerp[:, 2] = 0.7 * radius * np.sin(np.deg2rad(90) - end_angle * sigmoid)\nfor i, t in enumerate(T):\n slerp[i, 3:] = quaternion_slerp(q_start, q_end, t)\n\n# %%\n# The following 3D plot compares the two approaches.\nax = plot_trajectory(\n P=lerp, show_direction=False, n_frames=40, s=0.05, ax_s=0.7\n)\nax = plot_trajectory(P=slerp, show_direction=False, n_frames=40, s=0.05, ax=ax)\nax.text(0.1, 0, 0, \"SLERP\")\nax.text(0.4, 0, 0.6, \"Naive linear interpolation\")\nax.view_init(elev=10, azim=90)\nplt.show()\n" }, { "path": "examples/plots/plot_random_geometries.py", "content": "\"\"\"\n======================\nPlot Random Geometries\n======================\n\nPlotting of several geometric shapes is directly supported by the library.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d.plot_utils import (\n make_3d_axis,\n plot_box,\n plot_sphere,\n plot_cylinder,\n plot_ellipsoid,\n plot_capsule,\n plot_cone,\n)\nfrom pytransform3d.transformations import (\n random_transform,\n plot_transform,\n translate_transform,\n)\n\nrng = np.random.default_rng(2832)\n\n# %%\n# A box is defined by its size along the three main axes and its pose.\nbox2origin = random_transform(rng)\nbox_size = rng.random(size=3) * 3\n\n# %%\n# A sphere is defined by its position and radius.\nsphere_position = rng.standard_normal(size=3)\nsphere_radius = float(rng.random())\n\n# %%\n# A cylinder is defined by its length, radius, and pose.\ncylinder2origin = random_transform(rng)\nlength = float(rng.random()) * 5\ncylinder_radius = float(rng.random())\n\n# %%\n# An ellipsoid is defined by its 3 radii and pose.\nellipsoid2origin = random_transform(rng)\nradii = rng.random(size=3) * 3\n\n# %%\n# A capsule is defined by its height, radius, and pose.\ncapsule2origin = random_transform(rng)\ncapsule_height = float(rng.random()) * 2\ncapsule_radius = float(rng.random())\n\n# %%\n# A cone is defined by its height, radius, and pose.\ncone2origin = random_transform(rng)\ncone_height = float(rng.random()) * 5\ncone_radius = float(rng.random())\n\n# %%\n# The following part shows pytransform3d's 3D plotting functions.\nax = make_3d_axis(2)\n\nplot_transform(ax=ax, A2B=box2origin, s=0.3)\nplot_box(\n ax=ax, A2B=box2origin, size=box_size, color=\"b\", alpha=0.5, wireframe=False\n)\n\nplot_transform(\n ax=ax, A2B=translate_transform(np.eye(4), sphere_position), s=0.3\n)\nplot_sphere(\n ax=ax,\n p=sphere_position,\n radius=sphere_radius,\n color=\"y\",\n alpha=0.5,\n wireframe=False,\n)\n\nplot_transform(ax=ax, A2B=cylinder2origin, s=0.3)\nplot_cylinder(\n ax=ax,\n A2B=cylinder2origin,\n length=length,\n radius=cylinder_radius,\n color=\"g\",\n alpha=0.5,\n wireframe=False,\n)\n\nplot_transform(ax=ax, A2B=ellipsoid2origin, s=0.3)\nplot_ellipsoid(\n ax=ax,\n A2B=ellipsoid2origin,\n radii=radii,\n color=\"m\",\n alpha=0.5,\n wireframe=False,\n)\n\nplot_transform(ax=ax, A2B=capsule2origin, s=0.3)\nplot_capsule(\n ax=ax,\n A2B=capsule2origin,\n height=capsule_height,\n radius=capsule_radius,\n color=\"r\",\n alpha=0.5,\n wireframe=False,\n)\n\nplot_transform(ax=ax, A2B=cone2origin, s=0.3)\nplot_cone(\n ax=ax,\n A2B=cone2origin,\n height=cone_height,\n radius=cone_radius,\n color=\"c\",\n alpha=0.5,\n wireframe=False,\n)\n\nplt.show()\n" }, { "path": "examples/plots/plot_random_trajectories.py", "content": "\"\"\"\n===================\nRandom Trajectories\n===================\n\nThese plots show several randomly generated trajectories. Each row shows a\ndifferent trajectory. On the left side you can see the position and orientation\nrepresented by small coordinate frames. On the right side you can see the\npositions over time.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nimport pytransform3d.trajectories as ptr\nimport pytransform3d.transformations as pt\n\n# %%\n# We sample three random trajectories.\nn_trajectories = 3\ntrajectories = ptr.random_trajectories(\n rng=np.random.default_rng(5),\n n_trajectories=n_trajectories,\n n_steps=1001,\n start=np.eye(4),\n goal=pt.transform_from(R=np.eye(3), p=0.3 * np.ones(3)),\n scale=[200] * 3 + [50] * 3,\n)\n\n# %%\n# We plot the trajectory in 3D on the left and in 2D on the right.\nplt.figure(figsize=(8, 8))\nfor i in range(n_trajectories):\n ax = plt.subplot(n_trajectories, 2, 1 + 2 * i, projection=\"3d\")\n plt.setp(\n ax,\n xlim=(-0.1, 0.5),\n ylim=(-0.1, 0.5),\n zlim=(-0.1, 0.5),\n xlabel=\"X\",\n ylabel=\"Y\",\n )\n ax.set_xticks(())\n ax.set_yticks(())\n ax.set_zticks(())\n ptr.plot_trajectory(\n ax=ax, P=ptr.pqs_from_transforms(trajectories[i]), s=0.1\n )\n\n ax = plt.subplot(n_trajectories, 2, 2 + 2 * i)\n for d in range(3):\n plt.plot(trajectories[i, :, d, 3].T, label=\"XYZ\"[d])\n if i != n_trajectories - 1:\n ax.set_xticks(())\n else:\n ax.set_xlabel(\"Time step\")\n ax.legend(loc=\"best\")\n ax.set_ylabel(\"Position\")\n ax.set_xlim((0, trajectories.shape[1] - 1))\n\nplt.tight_layout()\nplt.show()\n" }, { "path": "examples/plots/plot_robot.py", "content": "\"\"\"\n=====\nRobot\n=====\n\nWe see a 6-DOF robot arm with visuals.\n\"\"\"\n\nimport os\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d.urdf import UrdfTransformManager\n\nBASE_DIR = \"test/test_data/\"\ndata_dir = BASE_DIR\nsearch_path = \".\"\nwhile (\n not os.path.exists(data_dir)\n and os.path.dirname(search_path) != \"pytransform3d\"\n):\n search_path = os.path.join(search_path, \"..\")\n data_dir = os.path.join(search_path, BASE_DIR)\n\ntm = UrdfTransformManager()\nfilename = os.path.join(data_dir, \"robot_with_visuals.urdf\")\nwith open(filename, \"r\") as f:\n robot_urdf = f.read()\n tm.load_urdf(robot_urdf, mesh_path=data_dir)\ntm.set_joint(\"joint2\", 0.2 * np.pi)\ntm.set_joint(\"joint3\", 0.2 * np.pi)\ntm.set_joint(\"joint5\", 0.1 * np.pi)\ntm.set_joint(\"joint6\", 0.5 * np.pi)\n\ntm.plot_visuals(\"robot_arm\", ax_s=0.6, alpha=0.7)\nplt.show()\n" }, { "path": "examples/plots/plot_rotate_cylinder.py", "content": "\"\"\"\n===============\nRotate Cylinder\n===============\n\nIn this example, we apply a constant torque (tau) to a cylinder at its\ncenter of gravity and plot it at several steps during the acceleration.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d.plot_utils import plot_cylinder\nfrom pytransform3d.rotations import matrix_from_compact_axis_angle\nfrom pytransform3d.transformations import transform_from, plot_transform\n\n\ndef inertia_of_cylinder(mass, length, radius):\n I_xx = I_yy = 0.25 * mass * radius**2 + 1.0 / 12.0 * mass * length**2\n I_zz = 0.5 * mass * radius**2\n return np.eye(3) * np.array([I_xx, I_yy, I_zz])\n\n\nA2B = np.eye(4)\n\nlength = 1.0\nradius = 0.1\nmass = 1.0\ndt = 0.2\ninertia = inertia_of_cylinder(mass, length, radius)\ntau = np.array([0.05, 0.05, 0.0])\nangular_velocity = np.zeros(3)\norientation = np.zeros(3)\n\nax = None\nfor p_xy in np.linspace(-2, 2, 21):\n A2B = transform_from(\n R=matrix_from_compact_axis_angle(orientation),\n p=np.array([p_xy, p_xy, 0.0]),\n )\n ax = plot_cylinder(\n length=length,\n radius=radius,\n A2B=A2B,\n wireframe=False,\n alpha=0.2,\n ax_s=2.0,\n ax=ax,\n )\n plot_transform(ax=ax, A2B=A2B, s=radius, lw=3)\n\n angular_acceleration = np.linalg.inv(inertia).dot(tau)\n angular_velocity += dt * angular_acceleration\n orientation += dt * angular_velocity\nax.view_init(elev=30, azim=70)\nplt.show()\n" }, { "path": "examples/plots/plot_sample_rotations.py", "content": "\"\"\"\n=================================\nCompare Rotation Sampling Methods\n=================================\n\nThere are different ways of sampling rotations. We draw random samples of\nrotations, convert them to rotation matrices, and apply these to the basis\nvector (1, 0, 0) to obtain points on the unit sphere.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nimport pytransform3d.batch_rotations as pbr\nimport pytransform3d.plot_utils as ppu\nimport pytransform3d.rotations as pr\n\nrng = np.random.default_rng(1223532)\nn_samples = 2000\nv = np.array([1.0, 0.0, 0.0])\n\nplt.figure(figsize=(9, 5))\n\nax1 = plt.subplot(121, projection=\"3d\")\nax1.set_title(\"Sampled rotation vectors (axis-angle)\")\nppu.plot_sphere(ax1, radius=1, n_steps=100)\nrotations = np.vstack(\n [pr.random_compact_axis_angle(rng) for _ in range(n_samples)]\n)\nR = pbr.matrices_from_compact_axis_angles(rotations)\nv_R = np.einsum(\"nij,j->ni\", R, v)\nax1.scatter(v_R[:, 0], v_R[:, 1], v_R[:, 2])\n\nax2 = plt.subplot(122, projection=\"3d\")\nax2.set_title(\"Sampled quaternions\")\nppu.plot_sphere(ax2, radius=1, n_steps=100)\nrotations = np.vstack([pr.random_quaternion(rng) for _ in range(n_samples)])\nR = pbr.matrices_from_quaternions(rotations)\nv_R = np.einsum(\"nij,j->ni\", R, v)\nax2.scatter(v_R[:, 0], v_R[:, 1], v_R[:, 2])\n\nplt.tight_layout()\nplt.show()\n" }, { "path": "examples/plots/plot_sample_transforms.py", "content": "\"\"\"\n=================\nSample Transforms\n=================\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nimport pytransform3d.transformations as pt\n\nmean = pt.transform_from(R=np.eye(3), p=np.array([0.0, 0.0, 0.5]))\ncov = np.diag([0.001, 0.001, 0.5, 0.001, 0.001, 0.001])\nrng = np.random.default_rng(0)\nax = None\nposes = np.array(\n [pt.random_transform(rng=rng, mean=mean, cov=cov) for _ in range(1000)]\n)\nfor pose in poses:\n ax = pt.plot_transform(ax=ax, A2B=pose, s=0.3)\nplt.show()\n" }, { "path": "examples/plots/plot_screw.py", "content": "\"\"\"\n========================================\nPlot Transformation through Screw Motion\n========================================\n\nA screw axis is represented by the parameters (q, s_axis, h). We can represent\nany transformation with a screw axis and an additional parameter theta that\nencodes the rotation angle and through h * theta the translation. Here we\nvisualize a screw axis and the transformation generated from a specific\ntheta.\n\nThe larger coordinate frame represents the origin of the transformation\nand the smaller frame represents the transformed frame. The red point\nindicates the position of q, which is a point on the screw axis. A straight\narrow shows the direction of the screw axis. The spiral path represents\na displacement of length theta along the screw axis.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d.rotations import active_matrix_from_extrinsic_roll_pitch_yaw\nfrom pytransform3d.transformations import (\n plot_transform,\n plot_screw,\n screw_axis_from_screw_parameters,\n transform_from_exponential_coordinates,\n concat,\n transform_from,\n)\n\n# Screw parameters\nq = np.array([-0.2, -0.1, -0.5])\ns_axis = np.array([0, 0, 1])\nh = 0.05\ntheta = 5.5 * np.pi\n\nStheta = screw_axis_from_screw_parameters(q, s_axis, h) * theta\nA2B = transform_from_exponential_coordinates(Stheta)\n\norigin = transform_from(\n active_matrix_from_extrinsic_roll_pitch_yaw([0.5, -0.3, 0.2]),\n np.array([0.0, 0.1, 0.1]),\n)\n\nax = plot_transform(A2B=origin, s=0.4)\nplot_transform(ax=ax, A2B=concat(A2B, origin), s=0.2)\nplot_screw(\n ax=ax, q=q, s_axis=s_axis, h=h, theta=theta, A2B=origin, s=1.5, alpha=0.6\n)\nax.view_init(elev=40, azim=170)\nplt.subplots_adjust(0, 0, 1, 1)\nplt.show()\n" }, { "path": "examples/plots/plot_sphere.py", "content": "\"\"\"\n===========\nPlot Sphere\n===========\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d.plot_utils import plot_sphere, remove_frame\nfrom pytransform3d.transformations import plot_transform\n\nax = plot_sphere(radius=0.5, wireframe=False, alpha=0.1, color=\"k\", ax_s=0.5)\nplot_sphere(ax=ax, radius=0.5, wireframe=True)\nplot_transform(ax=ax, A2B=np.eye(4), s=0.3, lw=3)\nremove_frame(ax)\nplt.show()\n" }, { "path": "examples/plots/plot_spheres.py", "content": "\"\"\"\r\n=====================\r\nPlot Multiple Spheres\r\n=====================\r\n\r\nBenchmarks plotting of multiple spheres at once and compares it to plotting\r\neach sphere individually.\r\n\"\"\"\r\n\r\nimport time\r\n\r\nimport matplotlib.pyplot as plt\r\nimport numpy as np\r\n\r\nfrom pytransform3d.plot_utils import plot_sphere, plot_spheres\r\n\r\nn_spheres = 50\r\nrandom_state = np.random.default_rng(0)\r\nP = 2 * random_state.random((n_spheres, 3)) - 1\r\nradii = random_state.random(n_spheres) / 2\r\ncolors = random_state.random((n_spheres, 3))\r\nalphas = random_state.random(n_spheres)\r\n\r\nstart = time.time()\r\nplot_spheres(p=P, radius=radii, color=colors, alpha=alphas, wireframe=False)\r\nend = time.time()\r\ntime_multi = end - start\r\n\r\nstart = time.time()\r\nfor p, radius, color, alpha in zip(P, radii, colors, alphas):\r\n plot_sphere(p=p, radius=radius, color=color, alpha=alpha, wireframe=False)\r\nend = time.time()\r\ntime_single = end - start\r\n\r\nspeedup = time_single / time_multi\r\n\r\nprint(\"n_spheres\", \"single\", \"\\t\", \"multi\", \"\\t\", \"speedup\", sep=\"\\t\")\r\nprint(n_spheres, \"\", time_single, time_multi, speedup, sep=\"\\t\")\r\n\r\nplt.show()\r\n" }, { "path": "examples/plots/plot_spherical_grid.py", "content": "\"\"\"\n==============\nSpherical Grid\n==============\n\nPlot a grid in spherical coordinates with rho = 1 as Cartesian points.\nWe can see that the Cartesian distances between points are not regular and\nthere are many points that were converted to the same Cartesian point at the\npoles of the sphere.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nimport pytransform3d.coordinates as pc\nfrom pytransform3d.plot_utils import make_3d_axis\n\nthetas, phis = np.meshgrid(\n np.linspace(0, np.pi, 11), np.linspace(-np.pi, np.pi, 21)\n)\nrhos = np.ones_like(thetas)\nspherical_grid = np.column_stack(\n (rhos.reshape(-1), thetas.reshape(-1), phis.reshape(-1))\n)\ncartesian_grid = pc.cartesian_from_spherical(spherical_grid)\n\nax = make_3d_axis(ax_s=1, unit=\"m\", n_ticks=6)\nax.scatter(cartesian_grid[:, 0], cartesian_grid[:, 1], cartesian_grid[:, 2])\nax.plot(cartesian_grid[:, 0], cartesian_grid[:, 1], cartesian_grid[:, 2])\nplt.show()\n" }, { "path": "examples/plots/plot_straight_line_path.py", "content": "\"\"\"\n========================\nPlot Straight Line Paths\n========================\n\nWe will compose a trajectory of multiple straight line paths in exponential\ncoordinates. This is a demonstration of batch conversion from exponential\ncoordinates to transformation matrices.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d.plot_utils import Trajectory, make_3d_axis, remove_frame\nfrom pytransform3d.rotations import active_matrix_from_angle\nfrom pytransform3d.trajectories import transforms_from_exponential_coordinates\nfrom pytransform3d.transformations import (\n exponential_coordinates_from_transform,\n transform_from,\n)\n\n\ndef time_scaling(t, t_max):\n \"\"\"Linear time scaling.\"\"\"\n return np.asarray(t) / t_max\n\n\ndef straight_line_path(start, goal, s):\n \"\"\"Compute straight line path in exponential coordinates.\"\"\"\n start = np.asarray(start)\n goal = np.asarray(goal)\n return (\n start[np.newaxis] * (1.0 - s)[:, np.newaxis]\n + goal[np.newaxis] * s[:, np.newaxis]\n )\n\n\ns = time_scaling(np.linspace(0.0, 5.0, 50001), 5.0)\nstart = transform_from(\n R=active_matrix_from_angle(1, -0.4 * np.pi), p=np.array([-1, -2.5, 0])\n)\ngoal1 = transform_from(\n R=active_matrix_from_angle(1, -0.1 * np.pi), p=np.array([-1, 1, 0])\n)\ngoal2 = transform_from(\n R=active_matrix_from_angle(2, -np.pi), p=np.array([-0.65, -0.75, 0])\n)\npath1 = straight_line_path(\n exponential_coordinates_from_transform(start),\n exponential_coordinates_from_transform(goal1),\n s,\n)\npath2 = straight_line_path(\n exponential_coordinates_from_transform(goal1),\n exponential_coordinates_from_transform(goal2),\n s,\n)\nH = transforms_from_exponential_coordinates(np.vstack((path1, path2)))\nax = make_3d_axis(1.0)\ntrajectory = Trajectory(H, n_frames=1000, show_direction=False, s=0.3)\ntrajectory.add_trajectory(ax)\nax.view_init(azim=-95, elev=70)\nax.set_xlim((-2.2, 1.3))\nax.set_ylim((-2.5, 1))\nremove_frame(ax)\nplt.show()\n" }, { "path": "examples/plots/plot_transform.py", "content": "\"\"\"\n===================\nPlot Transformation\n===================\n\nWe can display transformations by plotting the basis vectors of the\ncorresponding coordinate frame.\n\"\"\"\n\nimport matplotlib.pyplot as plt\n\nfrom pytransform3d.plot_utils import make_3d_axis\nfrom pytransform3d.transformations import plot_transform\n\nax = make_3d_axis(ax_s=1, unit=\"m\", n_ticks=6)\nplot_transform(ax=ax)\nplt.tight_layout()\nplt.show()\n" }, { "path": "examples/plots/plot_transform_concatenation.py", "content": "\"\"\"\n=======================\nTransform Concatenation\n=======================\n\nIn this example, we have a point p that is defined in a frame C, we know\nthe transform C2B and B2A. We can construct a transform C2A to extract the\nposition of p in frame A.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nimport pytransform3d.rotations as pyrot\nimport pytransform3d.transformations as pytr\n\np = np.array([0.0, 0.0, -0.5])\na = np.array([0.0, 0.0, 1.0, np.pi])\nB2A = pytr.transform_from(pyrot.matrix_from_axis_angle(a), p)\n\np = np.array([0.3, 0.4, 0.5])\na = np.array([0.0, 0.0, 1.0, -np.pi / 2.0])\nC2B = pytr.transform_from(pyrot.matrix_from_axis_angle(a), p)\n\nC2A = pytr.concat(C2B, B2A)\np = pytr.transform(C2A, np.ones(4))\n\nax = pytr.plot_transform(A2B=B2A)\npytr.plot_transform(ax, A2B=C2A)\nax.scatter(p[0], p[1], p[2])\nplt.show()\n" }, { "path": "examples/plots/plot_transform_manager.py", "content": "\"\"\"\n======================\nTransformation Manager\n======================\n\nIn this example, we will use the TransformManager to infer a transformation\nautomatically.\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d import rotations as pr\nfrom pytransform3d import transformations as pt\nfrom pytransform3d.transform_manager import TransformManager\n\nrng = np.random.default_rng(1)\n\nee2robot = pt.transform_from_pq(\n np.hstack((np.array([0.4, -0.3, 0.5]), pr.random_quaternion(rng)))\n)\ncam2robot = pt.transform_from_pq(\n np.hstack((np.array([0.0, 0.0, 0.8]), pr.q_id))\n)\nobject2cam = pt.transform_from(\n pr.active_matrix_from_intrinsic_euler_xyz(np.array([0.0, 0.0, -0.5])),\n np.array([0.5, 0.1, 0.1]),\n)\n\ntm = TransformManager()\ntm.add_transform(\"end-effector\", \"robot\", ee2robot)\ntm.add_transform(\"camera\", \"robot\", cam2robot)\ntm.add_transform(\"object\", \"camera\", object2cam)\n\nee2object = tm.get_transform(\"end-effector\", \"object\")\n\nax = tm.plot_frames_in(\"robot\", s=0.1)\nax.set_xlim((-0.25, 0.75))\nax.set_ylim((-0.5, 0.5))\nax.set_zlim((0.0, 1.0))\nplt.show()\n" }, { "path": "examples/plots/plot_urdf.py", "content": "\"\"\"\n===========\nURDF Joints\n===========\n\nThis example shows how to load a URDF description of a robot, set some joint\nangles and display relevant frames.\n\"\"\"\n\nimport matplotlib.pyplot as plt\n\nfrom pytransform3d.urdf import UrdfTransformManager\n\n# %%\n# URDF\n# ----\n# URDFs have to be provided as strings. These can be loaded from files.\nCOMPI_URDF = \"\"\"\n\n \n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n\n \n \n \n \n \n \n\n \n \n \n \n \n \n\n \n \n \n \n \n \n\n \n \n \n \n \n \n\n \n \n \n \n \n \n\n \n \n \n \n \n \n\"\"\"\n\ntm = UrdfTransformManager()\ntm.load_urdf(COMPI_URDF)\n\n# %%\n# Set Joints\n# ----------\n# In this example, we define joint angles manually. Joints are identified by\n# their string names extracted from the URDF.\njoint_names = [\"joint%d\" % i for i in range(1, 7)]\njoint_angles = [0, 0.5, 0.5, 0, 0.5, 0]\nfor name, angle in zip(joint_names, joint_angles):\n tm.set_joint(name, angle)\n\n# %%\n# Visualization\n# -------------\n# We use matplotlib to visualize the link frames of the URDF and connections\n# between these.\nax = tm.plot_frames_in(\n \"compi\",\n whitelist=[\"link%d\" % d for d in range(1, 7)],\n s=0.05,\n show_name=True,\n)\nax = tm.plot_connections_in(\"compi\", ax=ax)\nax.set_xlim((-0.2, 0.8))\nax.set_ylim((-0.5, 0.5))\nax.set_zlim((-0.2, 0.8))\nplt.show()\n" }, { "path": "examples/plots/plot_urdf_with_meshes.py", "content": "\"\"\"\n================\nURDF with Meshes\n================\n\nThis example shows how to load a URDF with STL meshes. This example must be\nrun from within the examples folder or the main folder because it uses a\nhard-coded path to the URDF file and the meshes.\n\"\"\"\n\nimport os\n\nimport matplotlib.pyplot as plt\n\nfrom pytransform3d.urdf import UrdfTransformManager\n\nBASE_DIR = \"test/test_data/\"\ndata_dir = BASE_DIR\nsearch_path = \".\"\nwhile (\n not os.path.exists(data_dir)\n and os.path.dirname(search_path) != \"pytransform3d\"\n):\n search_path = os.path.join(search_path, \"..\")\n data_dir = os.path.join(search_path, BASE_DIR)\n\ntm = UrdfTransformManager()\nwith open(os.path.join(data_dir, \"simple_mechanism.urdf\"), \"r\") as f:\n tm.load_urdf(f.read(), mesh_path=data_dir)\ntm.set_joint(\"joint\", -1.1)\nax = tm.plot_frames_in(\n \"lower_cone\", s=0.1, whitelist=[\"upper_cone\", \"lower_cone\"], show_name=True\n)\nax = tm.plot_connections_in(\"lower_cone\", ax=ax)\ntm.plot_visuals(\"lower_cone\", ax=ax)\nax.set_xlim((-0.1, 0.15))\nax.set_ylim((-0.1, 0.15))\nax.set_zlim((0.0, 0.25))\nplt.show()\n" }, { "path": "examples/plots/plot_vector.py", "content": "\"\"\"\n===========\nPlot Vector\n===========\n\"\"\"\n\nimport matplotlib.pyplot as plt\nimport numpy as np\n\nfrom pytransform3d.plot_utils import plot_vector\n\nplot_vector(\n # A vector is defined by start, direction, and s (scaling)\n start=np.array([-0.3, -0.2, -0.3]),\n direction=np.array([1.0, 1.0, 1.0]),\n s=0.5,\n ax_s=0.5, # Scaling of 3D axes\n lw=0, # Remove line around arrow\n color=\"orange\",\n)\nplt.show()\n" }, { "path": "examples/visualizations/README.rst", "content": "3D Visualizations\n-----------------" }, { "path": "examples/visualizations/vis_add_remove_artist.py", "content": "\"\"\"\n=====================\nAdd and Remove Artist\n=====================\n\nA demonstration of how previously added artists can be removed.\n\"\"\"\n\nimport os\n\nimport pytransform3d.visualizer as pv\nfrom pytransform3d.urdf import UrdfTransformManager\n\nBASE_DIR = \"test/test_data/\"\ndata_dir = BASE_DIR\nsearch_path = \".\"\nwhile (\n not os.path.exists(data_dir)\n and os.path.dirname(search_path) != \"pytransform3d\"\n):\n search_path = os.path.join(search_path, \"..\")\n data_dir = os.path.join(search_path, BASE_DIR)\n\ntm = UrdfTransformManager()\nwith open(data_dir + \"simple_mechanism.urdf\", \"r\") as f:\n tm.load_urdf(f.read(), mesh_path=data_dir)\ntm.set_joint(\"joint\", -1.1)\n\nfig = pv.figure()\n\n# add graph, box, and sphere\ngraph = fig.plot_graph(\n tm,\n \"lower_cone\",\n s=0.1,\n show_frames=True,\n whitelist=[\"upper_cone\", \"lower_cone\"],\n show_connections=True,\n show_visuals=True,\n show_name=False,\n)\nbox = fig.plot_box([1, 1, 1])\nsphere = fig.plot_sphere(2)\n\n# remove graph and box\nfig.remove_artist(graph)\nfig.remove_artist(box)\n\n# add and remove box again\nbox.add_artist(fig)\nfig.remove_artist(box)\n\nfig.view_init()\nfig.set_zoom(1.2)\nif \"__file__\" in globals():\n fig.show()\nelse:\n fig.save_image(\"__open3d_rendered_image.jpg\")\n" }, { "path": "examples/visualizations/vis_basis.py", "content": "\"\"\"\n==========================\nVisualize Coordinate Frame\n==========================\n\"\"\"\n\nimport numpy as np\n\nimport pytransform3d.visualizer as pv\n\nfig = pv.figure()\nfig.plot_basis(R=np.eye(3), p=[0.1, 0.2, 0.3])\nfig.view_init(azim=15, elev=30)\nfig.set_zoom(1.0)\nif \"__file__\" in globals():\n fig.show()\nelse:\n fig.save_image(\"__open3d_rendered_image.jpg\")\n" }, { "path": "examples/visualizations/vis_box.py", "content": "\"\"\"\n========\nPlot Box\n========\n\"\"\"\n\nimport numpy as np\n\nimport pytransform3d.visualizer as pv\nfrom pytransform3d.rotations import random_axis_angle, matrix_from_axis_angle\nfrom pytransform3d.transformations import transform_from\n\nrng = np.random.default_rng(42)\nfig = pv.figure()\nA2B = transform_from(\n R=matrix_from_axis_angle(random_axis_angle(rng)), p=np.zeros(3)\n)\nfig.plot_box(size=[0.2, 0.5, 1], A2B=A2B)\nfig.plot_transform(A2B=A2B)\nfig.view_init()\nif \"__file__\" in globals():\n fig.show()\nelse:\n fig.save_image(\"__open3d_rendered_image.jpg\")\n" }, { "path": "examples/visualizations/vis_camera_3d.py", "content": "\"\"\"\n===========================\nCamera Representation in 3D\n===========================\n\nThis visualization is inspired by Blender's camera visualization. It will\nshow the camera center, a virtual image plane at a desired distance to the\ncamera center, and the top direction of the virtual image plane.\n\"\"\"\n\nimport numpy as np\n\nimport pytransform3d.transformations as pt\nimport pytransform3d.visualizer as pv\n\ncam2world = pt.transform_from_pq([0, 0, 0, np.sqrt(0.5), -np.sqrt(0.5), 0, 0])\n# default parameters of a camera in Blender\nsensor_size = np.array([0.036, 0.024])\nintrinsic_matrix = np.array(\n [\n [0.05, 0, sensor_size[0] / 2.0],\n [0, 0.05, sensor_size[1] / 2.0],\n [0, 0, 1],\n ]\n)\nvirtual_image_distance = 1\n\nfig = pv.Figure()\nfig.plot_transform(A2B=cam2world, s=0.2)\nfig.plot_camera(\n cam2world=cam2world,\n M=intrinsic_matrix,\n sensor_size=sensor_size,\n virtual_image_distance=virtual_image_distance,\n)\nif \"__file__\" in globals():\n fig.show()\nelse:\n fig.save_image(\"__open3d_rendered_image.jpg\")\n" }, { "path": "examples/visualizations/vis_capsule.py", "content": "\"\"\"\n=================\nVisualize Capsule\n=================\n\"\"\"\n\nimport numpy as np\n\nimport pytransform3d.visualizer as pv\n\nfig = pv.figure()\nfig.plot_capsule(height=0.5, radius=0.1, c=(1, 0, 0))\nfig.plot_transform(A2B=np.eye(4))\nfig.view_init()\nif \"__file__\" in globals():\n fig.show()\nelse:\n fig.save_image(\"__open3d_rendered_image.jpg\")\n" }, { "path": "examples/visualizations/vis_cone.py", "content": "\"\"\"\n==============\nVisualize Cone\n==============\n\"\"\"\n\nimport numpy as np\n\nimport pytransform3d.visualizer as pv\n\nfig = pv.figure()\nfig.plot_cone(height=1.0, radius=0.3, c=(0, 0, 0.5))\nfig.plot_transform(A2B=np.eye(4))\nfig.view_init()\nif \"__file__\" in globals():\n fig.show()\nelse:\n fig.save_image(\"__open3d_rendered_image.jpg\")\n" }, { "path": "examples/visualizations/vis_cylinder.py", "content": "\"\"\"\n===============================\nVisualize Transformed Cylinders\n===============================\n\nPlots transformed cylinders.\n\"\"\"\n\nimport numpy as np\n\nimport pytransform3d.visualizer as pv\nfrom pytransform3d.rotations import random_axis_angle, matrix_from_axis_angle\nfrom pytransform3d.transformations import transform_from\n\nfig = pv.figure()\nrng = np.random.default_rng(42)\nA2B = transform_from(\n R=matrix_from_axis_angle(random_axis_angle(rng)),\n p=rng.standard_normal(size=3),\n)\nfig.plot_cylinder(length=1.0, radius=0.3)\nfig.plot_transform(A2B=np.eye(4))\nfig.plot_cylinder(length=1.0, radius=0.3, A2B=A2B)\nfig.plot_transform(A2B=A2B)\nfig.view_init()\nfig.set_zoom(2)\nif \"__file__\" in globals():\n fig.show()\nelse:\n fig.save_image(\"__open3d_rendered_image.jpg\")\n" }, { "path": "examples/visualizations/vis_ee_wrench.py", "content": "\"\"\"\n================\nVisualize Wrench\n================\n\nWe see a 6-DOF robot arm, and we assume that we have a force/torque sensor\nthat measures the force of a spherical mass (gray sphere) at the tool center\npoint (TCP). We can draw the screw representation of the wrench in the TCP\nframe as a force along a screw axis from the red sphere to the green sphere.\nThen we use the adjoint representation of the transformation from the base\nto the TCP to transform the wrench to the robot's base frame. This wrench\nhas a force component and a torque component, which we can also visualize\nas a screw: the red sphere indicates the point q on the screw axis, the\nstraight black line shows the screw axis, the red line indicates the\ndirection to the initial configuration and the green line indicates the\ndirection to the displaced configuration in which the instantaneous\nwrench would move the base.\n\"\"\"\n\nimport os\n\nimport numpy as np\n\nimport pytransform3d.transformations as pt\nimport pytransform3d.visualizer as pv\nfrom pytransform3d.urdf import UrdfTransformManager\n\n\ndef plot_screw(\n figure,\n q=np.zeros(3),\n s_axis=np.array([1.0, 0.0, 0.0]),\n h=1.0,\n theta=1.0,\n A2B=None,\n s=1.0,\n):\n \"\"\"Plot transformation about and along screw axis.\n\n Parameters\n ----------\n figure : Figure\n Interface to Open3D's visualizer\n\n q : array-like, shape (3,), optional (default: [0, 0, 0])\n Vector to a point on the screw axis\n\n s_axis : array-like, shape (3,), optional (default: [1, 0, 0])\n Direction vector of the screw axis\n\n h : float, optional (default: 1)\n Pitch of the screw. The pitch is the ratio of translation and rotation\n of the screw axis. Infinite pitch indicates pure translation.\n\n theta : float, optional (default: 1)\n Rotation angle. h * theta is the translation.\n\n A2B : array-like, shape (4, 4), optional (default: I)\n Origin of the screw\n\n s : float, optional (default: 1)\n Scaling of the axis and angle that will be drawn\n \"\"\"\n from pytransform3d.rotations import (\n vector_projection,\n angle_between_vectors,\n perpendicular_to_vectors,\n slerp_weights,\n )\n from pytransform3d.transformations import (\n check_screw_parameters,\n transform,\n translate_transform,\n vector_to_point,\n vector_to_direction,\n vectors_to_points,\n )\n\n if A2B is None:\n A2B = np.eye(4)\n\n q, s_axis, h = check_screw_parameters(q, s_axis, h)\n\n origin_projected_on_screw_axis = q + vector_projection(-q, s_axis)\n\n pure_translation = np.isinf(h)\n\n if not pure_translation:\n screw_axis_to_old_frame = -origin_projected_on_screw_axis\n screw_axis_to_rotated_frame = perpendicular_to_vectors(\n s_axis, screw_axis_to_old_frame\n )\n screw_axis_to_translated_frame = h * s_axis\n\n arc = np.empty((100, 3))\n angle = angle_between_vectors(\n screw_axis_to_old_frame, screw_axis_to_rotated_frame\n )\n for i, t in enumerate(\n zip(\n np.linspace(0, 2 * theta / np.pi, len(arc)),\n np.linspace(0.0, 1.0, len(arc)),\n )\n ):\n t1, t2 = t\n w1, w2 = slerp_weights(angle, t1)\n arc[i] = (\n origin_projected_on_screw_axis\n + w1 * screw_axis_to_old_frame\n + w2 * screw_axis_to_rotated_frame\n + screw_axis_to_translated_frame * t2 * theta\n )\n\n q = transform(A2B, vector_to_point(q))[:3]\n s_axis = transform(A2B, vector_to_direction(s_axis))[:3]\n if not pure_translation:\n arc = transform(A2B, vectors_to_points(arc))[:, :3]\n origin_projected_on_screw_axis = transform(\n A2B, vector_to_point(origin_projected_on_screw_axis)\n )[:3]\n\n # Screw axis\n Q = translate_transform(np.eye(4), q)\n fig.plot_sphere(radius=s * 0.02, A2B=Q, c=[1, 0, 0])\n if pure_translation:\n s_axis *= theta\n Q_plus_S_axis = translate_transform(np.eye(4), q + s_axis)\n fig.plot_sphere(radius=s * 0.02, A2B=Q_plus_S_axis, c=[0, 1, 0])\n P = np.array(\n [\n [q[0] - s * s_axis[0], q[1] - s * s_axis[1], q[2] - s * s_axis[2]],\n [\n q[0] + (1 + s) * s_axis[0],\n q[1] + (1 + s) * s_axis[1],\n q[2] + (1 + s) * s_axis[2],\n ],\n ]\n )\n figure.plot(P=P, c=[0, 0, 0])\n\n if not pure_translation:\n # Transformation\n figure.plot(arc, c=[0, 0, 0])\n\n for i, c in zip([0, -1], [[1, 0, 0], [0, 1, 0]]):\n arc_bound = np.vstack((origin_projected_on_screw_axis, arc[i]))\n figure.plot(arc_bound, c=c)\n\n\nBASE_DIR = \"test/test_data/\"\ndata_dir = BASE_DIR\nsearch_path = \".\"\nwhile (\n not os.path.exists(data_dir)\n and os.path.dirname(search_path) != \"pytransform3d\"\n):\n search_path = os.path.join(search_path, \"..\")\n data_dir = os.path.join(search_path, BASE_DIR)\n\ntm = UrdfTransformManager()\nfilename = os.path.join(data_dir, \"robot_with_visuals.urdf\")\nwith open(filename, \"r\") as f:\n robot_urdf = f.read()\n tm.load_urdf(robot_urdf, mesh_path=data_dir)\ntm.set_joint(\"joint2\", 0.2 * np.pi)\ntm.set_joint(\"joint3\", 0.2 * np.pi)\ntm.set_joint(\"joint5\", 0.1 * np.pi)\ntm.set_joint(\"joint6\", 0.5 * np.pi)\n\nee2base = tm.get_transform(\"tcp\", \"robot_arm\")\nbase2ee = tm.get_transform(\"robot_arm\", \"tcp\")\n\nmass = 1.0\nwrench_in_ee = np.array([0.0, 0.0, 0.0, 0.0, -9.81, 0.0]) * mass\nwrench_in_base = np.dot(pt.adjoint_from_transform(base2ee).T, wrench_in_ee)\n\nfig = pv.figure()\n\nfig.plot_graph(tm, \"robot_arm\", s=0.1, show_visuals=True)\n\nfig.plot_transform(s=0.4)\nfig.plot_transform(A2B=ee2base, s=0.1)\n\nmass2base = np.copy(ee2base)\nmass2base[2, 3] += 0.075\nfig.plot_sphere(radius=0.025, A2B=mass2base)\n\nS, theta = pt.screw_axis_from_exponential_coordinates(wrench_in_base)\nq, s, h = pt.screw_parameters_from_screw_axis(S)\nplot_screw(fig, q, s, h, theta * 0.05)\n\nS, theta = pt.screw_axis_from_exponential_coordinates(wrench_in_ee)\nq, s, h = pt.screw_parameters_from_screw_axis(S)\nplot_screw(fig, q, s, h, theta * 0.05, A2B=ee2base)\n\nfig.view_init()\nif \"__file__\" in globals():\n fig.show()\nelse:\n fig.save_image(\"__open3d_rendered_image.jpg\")\n" }, { "path": "examples/visualizations/vis_ellipsoid.py", "content": "\"\"\"\n===================\nVisualize Ellipsoid\n===================\n\"\"\"\n\nimport numpy as np\n\nimport pytransform3d.visualizer as pv\n\nfig = pv.figure()\nfig.plot_ellipsoid(radii=[0.2, 1, 0.5], c=(0.5, 0.5, 0))\nfig.plot_transform(A2B=np.eye(4))\nif \"__file__\" in globals():\n fig.show()\nelse:\n fig.save_image(\"__open3d_rendered_image.jpg\")\n" }, { "path": "examples/visualizations/vis_mesh.py", "content": "\"\"\"\n==============\nVisualize Mesh\n==============\n\nThis example shows how to load an STL mesh. This example must be\nrun from within the main folder because it uses a\nhard-coded path to the STL file. Press 'H' to print the viewer's\nhelp message to stdout.\n\"\"\"\n\nimport os\n\nimport numpy as np\n\nfrom pytransform3d import visualizer as pv\n\nBASE_DIR = \"test/test_data/\"\ndata_dir = BASE_DIR\nsearch_path = \".\"\nwhile (\n not os.path.exists(data_dir)\n and os.path.dirname(search_path) != \"pytransform3d\"\n):\n search_path = os.path.join(search_path, \"..\")\n data_dir = os.path.join(search_path, BASE_DIR)\n\n\nfig = pv.figure()\nfig.plot_mesh(filename=os.path.join(data_dir, \"scan.stl\"), s=np.ones(3))\nfig.plot_transform(A2B=np.eye(4), s=0.3)\nif \"__file__\" in globals():\n fig.show()\nelse:\n fig.save_image(\"__open3d_rendered_image.jpg\")\n" }, { "path": "examples/visualizations/vis_moving_basis.py", "content": "\"\"\"\n================\nAnimate Rotation\n================\n\nAnimates a rotation about the x-axis.\n\"\"\"\n\nimport numpy as np\n\nimport pytransform3d.visualizer as pv\nfrom pytransform3d import rotations as pr\n\n\ndef animation_callback(step, n_frames, frame):\n angle = 2.0 * np.pi * (step + 1) / n_frames\n R = pr.passive_matrix_from_angle(0, angle)\n A2B = np.eye(4)\n A2B[:3, :3] = R\n frame.set_data(A2B)\n return frame\n\n\nfig = pv.figure(width=500, height=500)\nframe = fig.plot_basis(R=np.eye(3), s=0.5)\nfig.view_init()\n\nn_frames = 100\nif \"__file__\" in globals():\n fig.animate(\n animation_callback, n_frames, fargs=(n_frames, frame), loop=True\n )\n fig.show()\nelse:\n fig.save_image(\"__open3d_rendered_image.jpg\")\n" }, { "path": "examples/visualizations/vis_moving_cylinder_with_wrench.py", "content": "\"\"\"\n==============================\nVisualize Cylinder with Wrench\n==============================\n\nWe apply a constant body-fixed wrench to a cylinder and integrate\nacceleration to twist and exponential coordinates of transformation\nto finally compute the new pose of the cylinder.\n\"\"\"\n\nimport numpy as np\n\nimport pytransform3d.visualizer as pv\nfrom pytransform3d.transformations import transform_from_exponential_coordinates\n\n\ndef spatial_inertia_of_cylinder(mass, length, radius):\n I_xx = I_yy = 0.25 * mass * radius**2 + 1.0 / 12.0 * mass * length**2\n I_zz = 0.5 * mass * radius**2\n inertia = np.eye(6)\n inertia[:3, :3] *= np.array([I_xx, I_yy, I_zz])\n inertia[3:, 3:] *= mass\n return inertia\n\n\ndef animation_callback(\n step, cylinder, cylinder_frame, prev_cylinder2world, Stheta_dot, inertia_inv\n):\n if step == 0: # Reset cylinder state\n prev_cylinder2world[:, :] = np.eye(4)\n Stheta_dot[:] = 0.0\n\n # Apply constant wrench\n wrench_in_cylinder = np.array([0.1, 0.001, 0.001, 0.01, 1.0, 1.0])\n dt = 0.0005\n\n Stheta_ddot = np.dot(inertia_inv, wrench_in_cylinder)\n Stheta_dot += dt * Stheta_ddot\n cylinder2world = transform_from_exponential_coordinates(\n dt * Stheta_dot\n ).dot(prev_cylinder2world)\n\n # Update visualization\n cylinder_frame.set_data(cylinder2world)\n cylinder.set_data(cylinder2world)\n\n prev_cylinder2world[:, :] = cylinder2world\n\n return cylinder_frame, cylinder\n\n\nfig = pv.figure()\n\n# Definition of cylinder\nmass = 1.0\nlength = 0.5\nradius = 0.1\ninertia_inv = np.linalg.inv(\n spatial_inertia_of_cylinder(mass=mass, length=length, radius=radius)\n)\n\n# State of cylinder\ncylinder2world = np.eye(4)\ntwist = np.zeros(6)\n\ncylinder = fig.plot_cylinder(length=length, radius=radius, c=[1, 0.5, 0])\ncylinder_frame = fig.plot_transform(A2B=cylinder2world, s=0.5)\n\nfig.plot_transform(A2B=np.eye(4), s=0.5)\n\nfig.view_init()\n\nif \"__file__\" in globals():\n fig.animate(\n animation_callback,\n n_frames=10000,\n fargs=(cylinder, cylinder_frame, cylinder2world, twist, inertia_inv),\n loop=True,\n )\n fig.show()\nelse:\n fig.save_image(\"__open3d_rendered_image.jpg\")\n" }, { "path": "examples/visualizations/vis_moving_line.py", "content": "\"\"\"\n============\nAnimate Line\n============\n\nAnimates a line.\n\"\"\"\n\nimport numpy as np\n\nimport pytransform3d.visualizer as pv\n\n\ndef animation_callback(step, n_frames, line):\n t = step / n_frames\n P = np.empty((100, 3))\n for d in range(P.shape[1]):\n P[:, d] = np.linspace(0, 1 - t, len(P))\n line.set_data(P)\n return line\n\n\nfig = pv.figure()\nP = np.zeros((100, 3))\ncolors = np.empty((99, 3))\nfor d in range(colors.shape[1]):\n P[:, d] = np.linspace(0, 1, len(P))\n colors[:, d] = np.linspace(0, 1, len(colors))\nline = fig.plot(P, colors)\nframe = fig.plot_basis(R=np.eye(3), s=0.5)\nfig.view_init()\n\nn_frames = 100\nif \"__file__\" in globals():\n fig.animate(animation_callback, n_frames, fargs=(n_frames, line), loop=True)\n fig.show()\nelse:\n fig.save_image(\"__open3d_rendered_image.jpg\")\n" }, { "path": "examples/visualizations/vis_moving_robot.py", "content": "\"\"\"\n==============\nAnimated Robot\n==============\n\nIn this example we animate a 6-DOF robot arm with cylindrical visuals.\n\"\"\"\n\nimport os\n\nimport numpy as np\n\nimport pytransform3d.visualizer as pv\nfrom pytransform3d.urdf import UrdfTransformManager\n\n\ndef animation_callback(step, n_frames, tm, graph, joint_names):\n angle = 0.5 * np.cos(2.0 * np.pi * (step / n_frames))\n for joint_name in joint_names:\n tm.set_joint(joint_name, angle)\n graph.set_data()\n return graph\n\n\nBASE_DIR = \"test/test_data/\"\ndata_dir = BASE_DIR\nsearch_path = \".\"\nwhile (\n not os.path.exists(data_dir)\n and os.path.dirname(search_path) != \"pytransform3d\"\n):\n search_path = os.path.join(search_path, \"..\")\n data_dir = os.path.join(search_path, BASE_DIR)\n\ntm = UrdfTransformManager()\nfilename = os.path.join(data_dir, \"robot_with_visuals.urdf\")\nwith open(filename, \"r\") as f:\n robot_urdf = f.read()\n tm.load_urdf(robot_urdf, mesh_path=data_dir)\njoint_names = [\"joint%d\" % i for i in range(1, 7)]\nfor joint_name in joint_names:\n tm.set_joint(joint_name, 0.5)\n\nfig = pv.figure()\ngraph = fig.plot_graph(\n tm, \"robot_arm\", s=0.1, show_frames=True, show_visuals=True\n)\nfig.view_init()\nfig.set_zoom(1.5)\nn_frames = 100\nif \"__file__\" in globals():\n fig.animate(\n animation_callback,\n n_frames,\n loop=True,\n fargs=(n_frames, tm, graph, joint_names),\n )\n fig.show()\nelse:\n fig.save_image(\"__open3d_rendered_image.jpg\")\n" }, { "path": "examples/visualizations/vis_moving_trajectory.py", "content": "\"\"\"\n==================\nAnimate Trajectory\n==================\n\nAnimates a trajectory.\n\"\"\"\n\nimport numpy as np\n\nimport pytransform3d.visualizer as pv\nfrom pytransform3d.rotations import passive_matrix_from_angle, R_id\nfrom pytransform3d.transformations import transform_from, concat\n\n\ndef update_trajectory(step, n_frames, trajectory):\n progress = 1 - float(step + 1) / float(n_frames)\n H = np.zeros((100, 4, 4))\n H0 = transform_from(R_id, np.zeros(3))\n H_mod = np.eye(4)\n for i, t in enumerate(np.linspace(0, progress, len(H))):\n H0[:3, 3] = np.array([t, 0, t])\n H_mod[:3, :3] = passive_matrix_from_angle(2, 8 * np.pi * t)\n H[i] = concat(H0, H_mod)\n\n trajectory.set_data(H)\n return trajectory\n\n\nn_frames = 200\n\nfig = pv.figure()\n\nH = np.empty((100, 4, 4))\nH[:] = np.eye(4)\n# set initial trajectory to extend view box\nH[:, 0, 3] = np.linspace(-2, 2, len(H))\nH[:, 1, 3] = np.linspace(-2, 2, len(H))\nH[:, 2, 3] = np.linspace(0, 4, len(H))\ntrajectory = pv.Trajectory(H, s=0.2, c=[0, 0, 0])\ntrajectory.add_artist(fig)\nfig.view_init()\nfig.set_zoom(0.5)\n\nif \"__file__\" in globals():\n fig.animate(\n update_trajectory, n_frames, fargs=(n_frames, trajectory), loop=True\n )\n fig.show()\nelse:\n fig.save_image(\"__open3d_rendered_image.jpg\")\n" }, { "path": "examples/visualizations/vis_moving_urdf_with_meshes.py", "content": "\"\"\"\n=========================\nAnimated URDF with Meshes\n=========================\n\nThis example shows how to load a URDF with STL meshes and animate it.\nThis example must be run from within the examples folder or the main\nfolder because it uses a hard-coded path to the URDF file and the meshes.\n\"\"\"\n\nimport os\n\nimport numpy as np\n\nimport pytransform3d.visualizer as pv\nfrom pytransform3d.urdf import UrdfTransformManager\n\n\ndef animation_callback(step, n_frames, tm, graph):\n angle = 2.79253 * np.sin(2.0 * np.pi * (step / n_frames))\n tm.set_joint(\"joint\", angle)\n graph.set_data()\n return graph\n\n\nBASE_DIR = \"test/test_data/\"\ndata_dir = BASE_DIR\nsearch_path = \".\"\nwhile (\n not os.path.exists(data_dir)\n and os.path.dirname(search_path) != \"pytransform3d\"\n):\n search_path = os.path.join(search_path, \"..\")\n data_dir = os.path.join(search_path, BASE_DIR)\n\n\ntm = UrdfTransformManager()\nwith open(data_dir + \"simple_mechanism.urdf\", \"r\") as f:\n tm.load_urdf(f.read(), mesh_path=data_dir)\n\nfig = pv.figure(\"URDF with meshes\")\ngraph = fig.plot_graph(\n tm, \"lower_cone\", s=0.1, show_connections=True, show_visuals=True\n)\nfig.view_init()\nfig.set_zoom(1.2)\nn_frames = 100\nif \"__file__\" in globals():\n fig.animate(\n animation_callback, n_frames, loop=True, fargs=(n_frames, tm, graph)\n )\n fig.show()\nelse:\n fig.save_image(\"__open3d_rendered_image.jpg\")\n" }, { "path": "examples/visualizations/vis_plane.py", "content": "\"\"\"\n===============\nVisualize Plane\n===============\n\nVisualizes one plane in Hesse normal form and one plane defined by point and\nnormal.\n\"\"\"\n\nimport numpy as np\n\nimport pytransform3d.rotations as pr\nimport pytransform3d.visualizer as pv\n\nfig = pv.figure()\nfig.plot_transform(np.eye(4))\nrng = np.random.default_rng(8853)\nfig.plot_plane(\n pr.norm_vector(rng.standard_normal(3)), rng.standard_normal(), c=(1, 0.5, 0)\n)\nfig.plot_plane(\n pr.norm_vector(rng.standard_normal(3)),\n point_in_plane=rng.standard_normal(3),\n c=(0, 1, 1),\n)\nif \"__file__\" in globals():\n fig.show()\nelse:\n fig.save_image(\"__open3d_rendered_image.jpg\")\n" }, { "path": "examples/visualizations/vis_probabilistic_robot_kinematics.py", "content": "\"\"\"\n=====================================\nProbabilistic Product of Exponentials\n=====================================\n\nWe compute the probabilistic forward kinematics of a robot with flexible\nlinks or joints and visualize the projected equiprobably ellipsoid of the\nend-effector's pose distribution.\n\"\"\"\n\nimport os\n\nimport numpy as np\nimport open3d as o3d\nfrom matplotlib import cbook\n\nimport pytransform3d.trajectories as ptr\nimport pytransform3d.transformations as pt\nimport pytransform3d.uncertainty as pu\nimport pytransform3d.visualizer as pv\nfrom pytransform3d.urdf import UrdfTransformManager\n\n\n# %%\n# Probabilistic Robot Kinematics\n# ------------------------------\n#\n# The end-effector's pose distribution is computed based on the Probabilistic\n# Product of Exponentials PPOE [1]_.\n#\n# Our ProbabilisticRobotKinematics class is a subclass of\n# :class:`~pytransform3d.urdf.UrdfTransformManager`, which loads a description\n# of a robot from the URDF format.\n#\n# The complicated part of this example is the conversion of kinematics\n# parameters from URDF data to screw axes that are needed for the product\n# of exponentials formulation of forward kinematics.\n#\n# Once we have this information, the implementation of the probabilistic\n# product of exponentials is straightforward:\n#\n# 1. We multiply the screw axis of each joint with the corresponding joint\n# angle to obtain the exponential coordinates of each relative joint\n# displacement.\n# 2. We concatenate the relative joint displacements and the base pose to\n# obtain the end-effector's pose. This is the original product of\n# exponentials.\n# 3. The PPOE modifies the original product of exponentials by transforming\n# and concatenating the covariances of each transformation.\nclass ProbabilisticRobotKinematics(UrdfTransformManager):\n \"\"\"Probabilistic robot kinematics.\n\n Parameters\n ----------\n robot_urdf : str\n URDF description of robot\n\n ee_frame : str\n Name of the end-effector frame\n\n base_frame : str\n Name of the base frame\n\n joint_names : list\n Names of joints in order from base to end effector\n\n mesh_path : str, optional (default: None)\n Path in which we search for meshes that are defined in the URDF.\n Meshes will be ignored if it is set to None and no 'package_dir'\n is given.\n\n package_dir : str, optional (default: None)\n Some URDFs start file names with 'package://' to refer to the ROS\n package in which these files (textures, meshes) are located. This\n variable defines to which path this prefix will be resolved.\n \"\"\"\n\n def __init__(\n self,\n robot_urdf,\n ee_frame,\n base_frame,\n joint_names,\n mesh_path=None,\n package_dir=None,\n ):\n super(ProbabilisticRobotKinematics, self).__init__(check=False)\n self.load_urdf(robot_urdf, mesh_path=mesh_path, package_dir=package_dir)\n self.ee2base_home, self.screw_axes_home = self._get_screw_axes(\n ee_frame, base_frame, joint_names\n )\n self.joint_limits = np.array(\n [self.get_joint_limits(jn) for jn in joint_names]\n )\n\n def _get_screw_axes(self, ee_frame, base_frame, joint_names):\n \"\"\"Get screw axes of joints in space frame at robot's home position.\n\n Parameters\n ----------\n ee_frame : str\n Name of the end-effector frame\n\n base_frame : str\n Name of the base frame\n\n joint_names : list\n Names of joints in order from base to end effector\n\n Returns\n -------\n ee2base_home : array, shape (4, 4)\n The home configuration (position and orientation) of the\n end-effector.\n\n screw_axes_home : array, shape (n_joints, 6)\n The joint screw axes in the space frame when the manipulator is at\n the home position.\n \"\"\"\n ee2base_home = self.get_transform(ee_frame, base_frame)\n screw_axes_home = []\n for jn in joint_names:\n ln, _, _, s_axis, limits, joint_type = self._joints[jn]\n link2base = self.get_transform(ln, base_frame)\n s_axis = np.dot(link2base[:3, :3], s_axis)\n q = link2base[:3, 3]\n\n if joint_type == \"revolute\":\n h = 0.0\n elif joint_type == \"prismatic\":\n h = np.inf\n else:\n raise NotImplementedError(\n \"Joint type %s not supported.\" % joint_type\n )\n\n screw_axis = pt.screw_axis_from_screw_parameters(q, s_axis, h)\n screw_axes_home.append(screw_axis)\n screw_axes_home = np.row_stack(screw_axes_home)\n return ee2base_home, screw_axes_home\n\n def probabilistic_forward_kinematics(self, thetas, covs):\n \"\"\"Compute probabilistic forward kinematics.\n\n This is based on the probabilistic product of exponentials.\n\n Parameters\n ----------\n thetas : array, shape (n_joints,)\n A list of joint coordinates.\n\n covs : array, shape (n_joints, 6, 6)\n Covariances of joint transformations.\n\n Returns\n -------\n ee2base : array, shape (4, 4)\n A homogeneous transformation matrix representing the end-effector\n frame when the joints are at the specified coordinates.\n\n cov : array, shape (6, 6)\n Covariance of the pose in tangent space.\n \"\"\"\n assert len(thetas) == self.screw_axes_home.shape[0]\n thetas = np.clip(\n thetas, self.joint_limits[:, 0], self.joint_limits[:, 1]\n )\n\n Sthetas = self.screw_axes_home * thetas[:, np.newaxis]\n joint_displacements = ptr.transforms_from_exponential_coordinates(\n Sthetas\n )\n\n T = np.eye(4)\n cov = np.zeros((6, 6))\n for i in range(len(thetas)):\n T, cov = pu.concat_locally_uncertain_transforms(\n joint_displacements[i], T, covs[i], cov\n )\n\n T = T.dot(self.ee2base_home)\n ad = pt.adjoint_from_transform(self.ee2base_home)\n cov = ad.dot(cov).dot(ad.T)\n\n return T, cov\n\n\n# %%\n# Mesh Visualization\n# ------------------\n# To visualize the 6D covariance in the tangent space of SE(3), we project its\n# equiprobable hyper-ellipsoid to 3D and represent it as a mesh. We can then\n# visualize the mesh with this class.\nclass Surface(pv.Artist):\n \"\"\"Surface to be visualized with Open3D.\n\n Parameters\n ----------\n x : array, shape (n_steps, n_steps)\n Coordinates on x-axis of grid on surface.\n\n y : array, shape (n_steps, n_steps)\n Coordinates on y-axis of grid on surface.\n\n z : array, shape (n_steps, n_steps)\n Coordinates on z-axis of grid on surface.\n\n c : array-like, shape (3,), optional (default: None)\n Color\n \"\"\"\n\n def __init__(self, x, y, z, c=None):\n self.c = c\n self.mesh = o3d.geometry.TriangleMesh()\n self.set_data(x, y, z)\n\n def set_data(self, x, y, z):\n \"\"\"Update data.\n\n Parameters\n ----------\n x : array, shape (n_steps, n_steps)\n Coordinates on x-axis of grid on surface.\n\n y : array, shape (n_steps, n_steps)\n Coordinates on y-axis of grid on surface.\n\n z : array, shape (n_steps, n_steps)\n Coordinates on z-axis of grid on surface.\n \"\"\"\n polys = np.stack(\n [cbook._array_patch_perimeters(a, 1, 1) for a in (x, y, z)], axis=-1\n )\n vertices = polys.reshape(-1, 3)\n triangles = (\n [[4 * i + 0, 4 * i + 1, 4 * i + 2] for i in range(len(polys))]\n + [[4 * i + 2, 4 * i + 3, 4 * i + 0] for i in range(len(polys))]\n + [[4 * i + 0, 4 * i + 3, 4 * i + 2] for i in range(len(polys))]\n + [[4 * i + 2, 4 * i + 1, 4 * i + 0] for i in range(len(polys))]\n )\n self.mesh.vertices = o3d.utility.Vector3dVector(vertices)\n self.mesh.triangles = o3d.utility.Vector3iVector(triangles)\n if self.c is not None:\n self.mesh.paint_uniform_color(self.c)\n self.mesh.compute_vertex_normals()\n\n @property\n def geometries(self):\n \"\"\"Expose geometries.\n\n Returns\n -------\n geometries : list\n List of geometries that can be added to the visualizer.\n \"\"\"\n return [self.mesh]\n\n\n# %%\n# Then we define a callback to animate the visualization.\ndef animation_callback(\n step, n_frames, tm, graph, joint_names, thetas, covs, surface\n):\n angle = 0.5 * np.cos(2.0 * np.pi * (0.5 + step / n_frames))\n thetas_t = angle * thetas\n for joint_name, value in zip(joint_names, thetas_t):\n tm.set_joint(joint_name, value)\n graph.set_data()\n\n T, cov = tm.probabilistic_forward_kinematics(thetas_t, covs)\n x, y, z = pu.to_projected_ellipsoid(T, cov, factor=1, n_steps=50)\n surface.set_data(x, y, z)\n\n return graph, surface\n\n\n# %%\n# Setup\n# -----\n# We load the URDF file,\nBASE_DIR = \"test/test_data/\"\ndata_dir = BASE_DIR\nsearch_path = \".\"\nwhile (\n not os.path.exists(data_dir)\n and os.path.dirname(search_path) != \"pytransform3d\"\n):\n search_path = os.path.join(search_path, \"..\")\n data_dir = os.path.join(search_path, BASE_DIR)\nfilename = os.path.join(data_dir, \"robot_with_visuals.urdf\")\nwith open(filename, \"r\") as f:\n robot_urdf = f.read()\n\n# %%\n# define the kinematic chain that we are interested in,\njoint_names = [\"joint%d\" % i for i in range(1, 7)]\ntm = ProbabilisticRobotKinematics(\n robot_urdf, \"tcp\", \"linkmount\", joint_names, mesh_path=data_dir\n)\n\n# %%\n# define the joint angles,\nthetas = np.array([1, 1, 1, 0, 1, 0])\ncurrent_thetas = -0.5 * thetas\nfor joint_name, theta in zip(joint_names, current_thetas):\n tm.set_joint(joint_name, theta)\n\n# %%\n# and define the covariances of the joints.\ncovs = np.zeros((len(thetas), 6, 6))\ncovs[0] = np.diag([0, 0, 1, 0, 0, 0])\ncovs[1] = np.diag([0, 1, 0, 0, 0, 0])\ncovs[2] = np.diag([0, 1, 0, 0, 0, 0])\ncovs[4] = np.diag([0, 1, 0, 0, 0, 0])\ncovs *= 0.05\n\n# %%\n# PPOE and Visualization\n# ----------------------\n#\n# Then we can finally use PPOE to compute the end-effector pose and its\n# covariance.\nT, cov = tm.probabilistic_forward_kinematics(current_thetas, covs)\n\n# %%\n# We compute the 3D projection of the 6D covariance matrix.\nx, y, z = pu.to_projected_ellipsoid(T, cov, factor=1, n_steps=50)\n\n# %%\n# The following code visualizes the result.\nfig = pv.figure()\ngraph = fig.plot_graph(tm, \"robot_arm\", show_visuals=True)\nfig.plot_transform(np.eye(4), s=0.3)\nsurface = Surface(x, y, z, c=(0, 0.5, 0.5))\nsurface.add_artist(fig)\nfig.view_init(elev=5, azim=50)\nn_frames = 200\nif \"__file__\" in globals():\n fig.animate(\n animation_callback,\n n_frames,\n loop=True,\n fargs=(n_frames, tm, graph, joint_names, thetas, covs, surface),\n )\n fig.show()\nelse:\n fig.save_image(\"__open3d_rendered_image.jpg\")\n\n# %%\n# References\n# ----------\n#\n# .. [1] Meyer, Strobl, Triebel (2022): The Probabilistic Robot Kinematics\n# Model and its Application to Sensor Fusion. In IEEE/RSJ International\n# Conference on Intelligent Robots and Systems (IROS), Kyoto, Japan, 2022,\n# pp. 3263-3270, doi: 10.1109/IROS47612.2022.9981399.\n# https://elib.dlr.de/191928/1/202212_ELIB_PAPER_VERSION_with_copyright.pdf\n" }, { "path": "examples/visualizations/vis_scatter.py", "content": "\"\"\"\n============\nScatter Plot\n============\n\nVisualizes a point collection.\n\"\"\"\n\nimport numpy as np\n\nimport pytransform3d.visualizer as pv\n\nfig = pv.figure()\nrng = np.random.default_rng(41)\nP = rng.standard_normal(size=(100, 3))\ncolors = np.empty((100, 3))\nfor d in range(colors.shape[1]):\n colors[:, d] = np.linspace(0, 1, len(colors))\nfig.scatter(P, c=colors)\nfig.plot_basis(R=np.eye(3), s=0.5)\nfig.view_init()\n\nif \"__file__\" in globals():\n fig.show()\nelse:\n fig.save_image(\"__open3d_rendered_image.jpg\")\n" }, { "path": "examples/visualizations/vis_sphere.py", "content": "\"\"\"\n================\nVisualize Sphere\n================\n\"\"\"\n\nimport numpy as np\n\nimport pytransform3d.visualizer as pv\n\nfig = pv.figure()\nfig.plot_sphere(radius=0.5)\nfig.plot_transform(A2B=np.eye(4))\nif \"__file__\" in globals():\n fig.show()\nelse:\n fig.save_image(\"__open3d_rendered_image.jpg\")\n" }, { "path": "examples/visualizations/vis_urdf.py", "content": "\"\"\"\n===========\nURDF Joints\n===========\n\nThis example shows how to load a URDF description of a robot, set some joint\nangles and display relevant frames.\n\"\"\"\n\nimport pytransform3d.visualizer as pv\nfrom pytransform3d.urdf import UrdfTransformManager\n\nCOMPI_URDF = \"\"\"\n\n \n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n\n \n \n \n \n \n \n\n \n \n \n \n \n \n\n \n \n \n \n \n \n\n \n \n \n \n \n \n\n \n \n \n \n \n \n\n \n \n \n \n \n \n\"\"\"\n\ntm = UrdfTransformManager()\ntm.load_urdf(COMPI_URDF)\njoint_names = [\"joint%d\" % i for i in range(1, 7)]\njoint_angles = [0, 0.5, 0.5, 0, 0.5, 0]\nfor name, angle in zip(joint_names, joint_angles):\n tm.set_joint(name, angle)\nfig = pv.figure(\"URDF\")\nfig.plot_graph(\n tm,\n \"compi\",\n show_frames=True,\n show_connections=True,\n whitelist=[\"link%d\" % d for d in range(1, 7)],\n s=0.05,\n)\nfig.view_init()\nif \"__file__\" in globals():\n fig.show()\nelse:\n fig.save_image(\"__open3d_rendered_image.jpg\")\n" }, { "path": "examples/visualizations/vis_urdf_with_meshes.py", "content": "\"\"\"\n================\nURDF with Meshes\n================\n\nThis example shows how to load a URDF with STL meshes. This example must be\nrun from within the examples folder or the main folder because it uses a\nhard-coded path to the URDF file and the meshes.\n\"\"\"\n\nimport os\n\nimport pytransform3d.visualizer as pv\nfrom pytransform3d.urdf import UrdfTransformManager\n\nBASE_DIR = \"test/test_data/\"\ndata_dir = BASE_DIR\nsearch_path = \".\"\nwhile (\n not os.path.exists(data_dir)\n and os.path.dirname(search_path) != \"pytransform3d\"\n):\n search_path = os.path.join(search_path, \"..\")\n data_dir = os.path.join(search_path, BASE_DIR)\n\ntm = UrdfTransformManager()\nwith open(data_dir + \"simple_mechanism.urdf\", \"r\") as f:\n tm.load_urdf(f.read(), mesh_path=data_dir)\ntm.set_joint(\"joint\", -1.1)\n\nfig = pv.figure(\"URDF with meshes\")\nfig.plot_graph(\n tm,\n \"lower_cone\",\n s=0.1,\n show_frames=True,\n whitelist=[\"upper_cone\", \"lower_cone\"],\n show_connections=True,\n show_visuals=True,\n show_name=False,\n)\nfig.view_init()\nfig.set_zoom(1.2)\nif \"__file__\" in globals():\n fig.show()\nelse:\n fig.save_image(\"__open3d_rendered_image.jpg\")\n" }, { "path": "examples/visualizations/vis_vector.py", "content": "\"\"\"\n================\nVisualize Vector\n================\n\"\"\"\n\nimport numpy as np\n\nimport pytransform3d.visualizer as pv\n\nfig = pv.figure()\nfig.plot_vector(\n start=np.array([0.0, 0.0, 0.0]),\n direction=np.array([1.0, 1.0, 0.0]),\n c=(1.0, 0.5, 0.0),\n)\nfig.plot_transform(A2B=np.eye(4))\nfig.view_init()\nif \"__file__\" in globals():\n fig.show()\nelse:\n fig.save_image(\"__open3d_rendered_image.jpg\")\n" }, { "path": "manifest.xml", "content": "\n \n A Python library for transformations in three dimensions. It makes conversions between rotation and transformation conventions as easy as possible. The library focuses on readability and debugging, not on computational efficiency. If you want to have an efficient implementation of some function from the library you can easily extract the relevant code and implement it more efficiently in a language of your choice.\n \n Alexander Fabisch/Alexander.Fabisch@dfki.de\n Alexander Fabisch/Alexander.Fabisch@dfki.de\n BSD-3-clause\n https://github.com/dfki-ric/pytransform3d\n https://raw.githubusercontent.com/dfki-ric/pytransform3d/main/doc/source/_static/logo.png\n\n \n \n \n \n \n \n\n \n Utilities\n \n 2\n multi project\n active\n \n python\n ubuntu\n windows\n \n\n" }, { "path": "paper/paper.bib", "content": "@ARTICLE{Gutzeit2018,\nAUTHOR={Gutzeit, Lisa and Fabisch, Alexander and Otto, Marc and Metzen, Jan Hendrik and Hansen, Jonas and Kirchner, Frank and Kirchner, Elsa Andrea},\nTITLE={The BesMan Learning Platform for Automated Robot Skill Learning},\nJOURNAL={Frontiers in Robotics and AI},\nVOLUME={5},\nPAGES={43},\nYEAR={2018},\nURL={https://www.frontiersin.org/article/10.3389/frobt.2018.00043},\nDOI={10.3389/frobt.2018.00043},\nISSN={2296-9144}\n}\n\n@Misc{SciPyLectureNotes,\ntitle={Scipy Lecture Notes},\nauthor={Ga{\\\"e}l Varoquaux and Emmanuelle Gouillart and Olav Vahtras},\nurl=\"https://www.scipy-lectures.org/\",\nyear=2018,\nnote = {[Online; accessed <3 November 2018>]}\n}\n\n@ARTICLE{Walt2011,\nauthor={S. van der Walt and S. C. Colbert and G. Varoquaux},\njournal={Computing in Science Engineering},\ntitle={The NumPy Array: A Structure for Efficient Numerical Computation},\nyear={2011},\nvolume={13},\nnumber={2},\npages={22-30},\ndoi={10.1109/MCSE.2011.37},\nISSN={1521-9615},\nmonth={March},\n}\n\n@Misc{Jones2001,\nauthor = {Eric Jones and Travis Oliphant and Pearu Peterson and others},\ntitle = {{SciPy}: Open source scientific tools for {Python}},\nyear = {2001--},\nurl = \"http://www.scipy.org/\",\nnote = {[Online; accessed <3 November 2018>]}\n}\n\n@Article{Hunter2007,\nAuthor = {Hunter, J. D.},\nTitle = {Matplotlib: A 2D graphics environment},\nJournal = {Computing In Science \\& Engineering},\nVolume = {9},\nNumber = {3},\nPages = {90--95},\npublisher = {IEEE COMPUTER SOC},\ndoi = {10.1109/MCSE.2007.55},\nyear = 2007\n}\n\n@misc{Foote2009,\nauthor={Tully Foote and Eitan Marder-Eppstein and Wim Meeussen and others},\ntitle={ROS tf2},\nyear={2009--},\nurl=\"http://wiki.ros.org/tf2\",\nnote = {[Online; accessed <4 November 2018>]}\n}\n\n@inproceedings{HidalgoCarrio2016,\nauthor={Javier Hidalgo Carri{\\'o} and Sascha Arnold and Arne B{\\\"o}ckmann and Anna Born and Ra{\\'u}l Dom{\\'i}nguez and Daniel Hennes and Christoph Hertzberg and Janosch Machowinski and Jakob Schwendner and Yong-Ho Yoo and Frank Kirchner},\ntitle={EnviRe -- Environment Representation for Long-term Autonomy},\nbooktitle={AI for Long-term Autonomy Workshop on the International Conference on Robotics and Automation (ICRA)},\nyear=2016\n}\n\n@misc{Brett2009,\nauthor = \"Matthew Brett and Christoph Gohlke\",\ntitle = \"transforms3d\",\nyear = {2009--},\nurl = \"https://github.com/matthew-brett/transforms3d\",\nnote = {[Online; accessed <4 November 2018>]}\n}\n\n@article{Ramasubramani2018,\nauthor={Vyas Ramasubramani and Sharon C. Glotzer},\ntitle={rowan: A Python package for working with quaternions},\njournal={Journal of Open Source Software},\nyear=2018,\ndoi={10.21105/joss.00787},\nurl={https://doi.org/10.21105/joss.00787},\nvolume=3,\nissue=27\n}\n" }, { "path": "paper/paper.md", "content": "---\ntitle: 'pytransform3d: 3D Transformations for Python'\ntags:\n - transformations\nauthors:\n - name: Alexander Fabisch\n orcid: 0000-0003-2824-7956\n affiliation: 1\naffiliations:\n - name: Robotics Innovation Center, DFKI GmbH\n index: 1\ndate: 3 November 2018\nbibliography: paper.bib\n---\n\n# Summary\n\npytransform3d is a Python library for transformations in three dimensions.\nHeterogenous software systems that consist of proprietary and open source\nsoftware are often combined when we work with transformations.\nFor example, suppose you want to transfer a trajectory demonstrated by a human\nto a robot. The human trajectory could be measured from an RGB-D camera, fused\nwith inertial measurement units that are attached to the human, and then\ntranslated to joint angles by inverse kinematics. That involves at least\nthree different software systems that might all use different conventions for\ntransformations. Sometimes even one software uses more than one convention.\nThe following aspects are of crucial importance to glue and debug\ntransformations in systems with heterogenous and often incompatible software:\n\n* Compatibility: Compatibility between heterogenous softwares is a difficult\n topic. It might involve, for example, communicating between proprietary and\n open source software or different languages.\n* Conventions: Lots of different conventions are used for transformations\n in three dimensions. These have to be determined or specified.\n* Conversions: We need conversions between these conventions to\n communicate transformations between different systems.\n* Visualization: Finally, transformations should be visually verified\n and that should be as easy as possible.\n\npytransform3d assists in solving these issues. Its documentation clearly\nstates all of the used conventions, it makes conversions between rotation\nand transformation conventions as easy as possible, it is tightly coupled\nwith Matplotlib to quickly visualize (or animate) transformations and it\nis written in Python with few dependencies. Python is a widely adopted\nlanguage. It is used in many domains and supports a wide spectrum of\ncommunication to other software.\n\nThe library focuses on readability and debugging, not on computational\nefficiency.\nIf you want to have an efficient implementation of some function from the\nlibrary you can easily extract the relevant code and implement it more\nefficiently in a language of your choice.\n\nThe library integrates well with the scientific Python ecosystem\n[@SciPyLectureNotes] with its core libraries Numpy, Scipy and Matplotlib.\nWe rely on Numpy [@Walt2011] for linear algebra and on Matplotlib\n[@Hunter2007] to offer plotting functionalities.\nScipy [@Jones2001] is used if you want to automatically\ncompute new transformations from a graph of existing transformations.\n\nMore advanced features of the library are the TransformManager which manages\ncomplex chains of transformations, the TransformEditor which allows to modify\ntransformations graphically (additionally requires PyQt4), and the\nUrdfTransformManager which is able to load transformations from\nthe Unified Robot Description Format (URDF) (additionally requires\nbeautifulsoup4).\n\n![Transformation Manager: The TransformManager builds a graph of transformations that can be used to automatically infer previously unknown transformations.](plot_transform_manager.png)\n\n![Transformation Editor: The TransformEditor based on PyQt4 can be used to visually modify transformations with a minimal number dependencies.](app_transformation_editor.png)\n\n![A simple URDF file loaded with pytransform3d and displayed in Matplotlib.](plot_urdf.png)\n\nOne of the strengths of pytransform3d in comparison to most other libraries\nis its rigorous approach to testing. Unit tests have 100% branch coverage\nfor code that is not related to visualization, there is more test code than\nlibrary code, there are additional examples, and continuous integration\nruns with Python 2.7, 3.4, 3.5, and 3.6. The maintainer ensures that this\nlevel of quality will not be sacrificed for new features.\n\nThere are several similar software packages. ROS tf2 [@Foote2009] is probably\nthe most widely used of them. It offers functionality that is similar to\nthe TransformManager of pytransform3d, but also considers temporal aspects\nof transformations. It also provides conversions between various conventions\nand visualization can be done with ROS' rviz package.\nEnviRe [@HidalgoCarrio2016] provides similar functionality.\nHowever, both libraries come with a number of dependencies and require\ncomplex build tools. Hence, it is not easy to set them up quickly in\na new environment with minimum impact to the system, while pytransform3d\nis a lightweight library that only requires the basic scientific Python\nsoftware stack that runs on any machine that supports CPython.\nThere are other lightweight Python libraries that offer transformations\nand conversions between conventions, for example, transforms3d [@Brett2009]\nand rowan [@Ramasubramani2018], but these do not directly support\nvisualization.\n\n## Research\n\npytransform3d is used in various domains, for example,\nspecifying motions of a robot, learning robot movements from human\ndemonstration, and sensor fusion for human pose estimation.\npytransform3d has been used by @Gutzeit2018 in the context of\nlearning robot behaviors from demonstration.\n\n# Acknowledgements\n\nWe would like to thank Manuel Meder and Hendrik Wiese who have given\nvaluable feedback as users to improve pytransform3d.\nThis work was supported through two grants of the German Federal\nMinistry of Economics and Technology (BMWi, FKZ 50 RA 1216 and FKZ 50 RA 1217)\nand two grants from the European Union's Horizon 2020 research and innovation\nprogram (723853 and 730014).\n\n# References" }, { "path": "pyproject.toml", "content": "[tool.black]\nline-length = 80\ntarget-version = [\"py38\", \"py39\", \"py310\", \"py311\", \"py312\", \"py313\"]\ninclude = '''\n/(\n pytransform3d\n | examples\n)\\/.*\\.pyi?$\n'''\nexclude = '''\n/(\n .git\n | __pycache__\n | doc\n | venv\n | build\n | dist\n)/\n'''\n\n[tool.ruff]\nline-length = 80\ntarget-version = \"py38\"\ninclude = [\n \"pytransform3d/**/*.py\",\n \"examples//**/*.py\",\n]\n\n[tool.ruff.lint]\nselect = [\n # pycodestyle\n \"E\",\n # Pyflakes\n \"F\",\n # pyupgrade\n \"UP\",\n # flake8-bugbear\n \"B\",\n # flake8-simplify\n \"SIM\",\n # isort\n \"I\",\n]\n# all rules can be found here: https://beta.ruff.rs/docs/rules/\nignore=[\n \"B008\", # Do not perform function calls in argument defaults\n \"I001\", # isort found an import in the wrong position\n \"SIM108\", # Use ternary operator\n # old Python style, should be deprecated soon\n \"UP004\", # Class inherits from `object`\n \"UP008\", # Use `super()` instead of `super(__class__, self)`\n \"UP015\", # Unnecessary mode argument\n \"UP031\", # Use format specifiers instead of percent format\n \"SIM118\", # Use `key in dict` instead of `key in dict.keys()`\n]\n\n[tool.ruff.format]\nquote-style = \"double\"\n" }, { "path": "pytransform3d/__init__.py", "content": "\"\"\"3D transformations for Python.\"\"\"\n\n__version__ = \"3.15.0\"\n" }, { "path": "pytransform3d/_geometry.py", "content": "\"\"\"Basic functionality for geometrical shapes.\"\"\"\n\nimport numpy as np\n\nfrom .transformations import check_transform\n\n\ndef unit_sphere_surface_grid(n_steps):\n \"\"\"Create grid on the surface of a unit sphere in 3D.\n\n Parameters\n ----------\n n_steps : int\n Number of discrete steps in each dimension of the surface.\n\n Returns\n -------\n x : array, shape (n_steps, n_steps)\n x-coordinates of grid points.\n\n y : array, shape (n_steps, n_steps)\n y-coordinates of grid points.\n\n z : array, shape (n_steps, n_steps)\n z-coordinates of grid points.\n \"\"\"\n phi, theta = np.mgrid[\n 0.0 : np.pi : n_steps * 1j, 0.0 : 2.0 * np.pi : n_steps * 1j\n ]\n sin_phi = np.sin(phi)\n\n x = sin_phi * np.cos(theta)\n y = sin_phi * np.sin(theta)\n z = np.cos(phi)\n\n return x, y, z\n\n\ndef transform_surface(surface2origin, x, y, z):\n \"\"\"Transform surface grid.\n\n Parameters\n ----------\n surface2origin : array-like, shape (4, 4)\n Pose: transformation that will be applied to the surface grid.\n\n x : array, shape (n_steps, n_steps)\n x-coordinates of grid points.\n\n y : array, shape (n_steps, n_steps)\n y-coordinates of grid points.\n\n z : array, shape (n_steps, n_steps)\n z-coordinates of grid points.\n\n Returns\n -------\n x : array, shape (n_steps, n_steps)\n x-coordinates of transformed grid points.\n\n y : array, shape (n_steps, n_steps)\n y-coordinates of transformed grid points.\n\n z : array, shape (n_steps, n_steps)\n z-coordinates of transformed grid points.\n \"\"\"\n surface2origin = check_transform(surface2origin)\n x = np.asarray(x)\n y = np.asarray(y)\n z = np.asarray(z)\n\n shape = x.shape\n\n P = np.column_stack((x.reshape(-1), y.reshape(-1), z.reshape(-1)))\n P = P.dot(surface2origin[:3, :3].T) + surface2origin[np.newaxis, :3, 3]\n\n x = P[:, 0].reshape(*shape)\n y = P[:, 1].reshape(*shape)\n z = P[:, 2].reshape(*shape)\n return x, y, z\n" }, { "path": "pytransform3d/_geometry.pyi", "content": "from typing import Tuple\n\nimport numpy as np\nimport numpy.typing as npt\n\ndef unit_sphere_surface_grid(\n n_steps: int,\n) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: ...\ndef transform_surface(\n pose: npt.ArrayLike, x: npt.ArrayLike, y: npt.ArrayLike, z: npt.ArrayLike\n) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: ...\n" }, { "path": "pytransform3d/_mesh_loader.py", "content": "\"\"\"Common interface to load meshes.\"\"\"\n\nimport abc\n\nimport numpy as np\n\n\ndef load_mesh(filename):\n \"\"\"Load mesh from file.\n\n This feature relies on optional dependencies. It can use trimesh or\n Open3D to load meshes. If both are not available, it will fail.\n Furthermore, some mesh formats require additional dependencies. For\n example, loading collada files ('.dae' file ending) requires pycollada\n and trimesh.\n\n Parameters\n ----------\n filename : str\n File in which the mesh is stored.\n\n Returns\n -------\n mesh : MeshBase\n Mesh instance.\n \"\"\"\n mesh_loaded = False\n\n # since trimesh does not support color for STL files, we try Open3D first\n if filename.endswith(\".stl\"):\n mesh = _Open3DMesh(filename)\n mesh_loaded = mesh.load()\n\n # trimesh is usually better for other formats\n if not mesh_loaded: # pragma: no cover\n mesh = _Trimesh(filename)\n mesh_loaded = mesh.load()\n\n if not mesh_loaded: # pragma: no cover\n mesh = _Open3DMesh(filename)\n mesh_loaded = mesh.load()\n\n if not mesh_loaded: # pragma: no cover\n raise ImportError(\n \"Could not load mesh from '%s'. Please install one of the \"\n \"optional dependencies 'trimesh' or 'open3d'.\" % filename\n )\n\n return mesh\n\n\nclass MeshBase(abc.ABC):\n \"\"\"Abstract base class of meshes.\n\n Parameters\n ----------\n filename : str\n File in which the mesh is stored.\n \"\"\"\n\n def __init__(self, filename):\n self.filename = filename\n\n @abc.abstractmethod\n def load(self):\n \"\"\"Load mesh from file.\n\n Returns\n -------\n loader_available : bool\n Is the mesh loader available?\n \"\"\"\n\n @abc.abstractmethod\n def convex_hull(self):\n \"\"\"Compute convex hull of mesh.\"\"\"\n\n @abc.abstractmethod\n def get_open3d_mesh(self):\n \"\"\"Return Open3D mesh.\n\n Returns\n -------\n mesh : open3d.geometry.TriangleMesh\n Open3D mesh.\n \"\"\"\n\n @property\n @abc.abstractmethod\n def vertices(self):\n \"\"\"Vertices.\"\"\"\n\n @property\n @abc.abstractmethod\n def triangles(self):\n \"\"\"Triangles.\"\"\"\n\n\nclass _Trimesh(MeshBase):\n def __init__(self, filename):\n super(_Trimesh, self).__init__(filename)\n self.mesh = None\n\n def load(self):\n try:\n import trimesh\n except ImportError as e: # pragma: no cover\n if e.name == \"trimesh\":\n return False\n else:\n raise e\n self.mesh = trimesh.load_mesh(self.filename)\n if isinstance(self.mesh, trimesh.Scene):\n open3d_mesh = self._scene_to_open3d_mesh(self.mesh)\n self.mesh = self._open3d_mesh_to_trimesh(open3d_mesh)\n return True\n\n def _open3d_mesh_to_trimesh(self, open3d_mesh): # pragma: no cover\n import trimesh\n\n if len(open3d_mesh.vertex_colors) == 0:\n vertex_colors = None\n else:\n vertex_colors = open3d_mesh.vertex_colors * 255.0\n return trimesh.Trimesh(\n vertices=np.asarray(open3d_mesh.vertices),\n faces=np.asarray(open3d_mesh.triangles),\n vertex_colors=vertex_colors,\n )\n\n def convex_hull(self):\n self.mesh = self.mesh.convex_hull\n\n def get_open3d_mesh(self): # pragma: no cover\n return self._trimesh_to_open3d_mesh(self.mesh)\n\n def _scene_to_open3d_mesh(self, scene): # pragma: no cover\n import open3d\n import trimesh\n\n mesh = open3d.geometry.TriangleMesh()\n for d in scene.dump():\n if isinstance(d, trimesh.Trimesh):\n mesh += self._trimesh_to_open3d_mesh(d)\n return mesh\n\n def _trimesh_to_open3d_mesh(self, tri_mesh): # pragma: no cover\n import open3d\n import trimesh\n\n mesh = open3d.geometry.TriangleMesh(\n open3d.utility.Vector3dVector(tri_mesh.vertices),\n open3d.utility.Vector3iVector(tri_mesh.faces),\n )\n if isinstance(tri_mesh.visual, trimesh.visual.ColorVisuals):\n mesh.vertex_colors = open3d.utility.Vector3dVector(\n tri_mesh.visual.vertex_colors[:, :3] / 255.0\n )\n return mesh\n\n @property\n def vertices(self):\n return self.mesh.vertices\n\n @property\n def triangles(self):\n return self.mesh.faces\n\n\nclass _Open3DMesh(MeshBase): # pragma: no cover\n def __init__(self, filename):\n super(_Open3DMesh, self).__init__(filename)\n self.mesh = None\n\n def load(self):\n try:\n import open3d\n except ImportError as e:\n if e.name == \"open3d\":\n return False\n else:\n raise e\n self.mesh = open3d.io.read_triangle_mesh(self.filename)\n return True\n\n def convex_hull(self):\n assert self.mesh is not None\n self.mesh = self.mesh.compute_convex_hull()[0]\n\n def get_open3d_mesh(self):\n return self.mesh\n\n @property\n def vertices(self):\n return np.asarray(self.mesh.vertices)\n\n @property\n def triangles(self):\n return np.asarray(self.mesh.triangles)\n" }, { "path": "pytransform3d/_mesh_loader.pyi", "content": "import abc\nfrom typing import Any\n\nimport numpy.typing as npt\n\nclass MeshBase(abc.ABC):\n filename: str\n\n def __init__(self, filename: str): ...\n @abc.abstractmethod\n def load(self) -> bool: ...\n @abc.abstractmethod\n def convex_hull(self): ...\n @abc.abstractmethod\n def get_open3d_mesh(self) -> Any: ...\n @property\n @abc.abstractmethod\n def vertices(self) -> npt.ArrayLike: ...\n @property\n @abc.abstractmethod\n def triangles(self) -> npt.ArrayLike: ...\n\ndef load_mesh(filename: str) -> MeshBase: ...\n" }, { "path": "pytransform3d/batch_rotations/__init__.py", "content": "\"\"\"Batch operations on rotations in three dimensions - SO(3).\n\nConversions from this module operate on batches of orientations or rotations\nand can be orders of magnitude faster than a loop of individual conversions.\n\nAll functions operate on nd arrays, where the last dimension (vectors) or\nthe last two dimensions (matrices) contain individual rotations.\n\"\"\"\n\nfrom ._angle import (\n active_matrices_from_angles,\n)\nfrom ._axis_angle import (\n norm_axis_angles,\n matrices_from_compact_axis_angles,\n)\nfrom ._euler import (\n active_matrices_from_intrinsic_euler_angles,\n active_matrices_from_extrinsic_euler_angles,\n)\nfrom ._matrix import (\n axis_angles_from_matrices,\n quaternions_from_matrices,\n)\nfrom ._quaternion import (\n smooth_quaternion_trajectory,\n batch_concatenate_quaternions,\n batch_q_conj,\n quaternion_slerp_batch,\n axis_angles_from_quaternions,\n matrices_from_quaternions,\n batch_quaternion_wxyz_from_xyzw,\n batch_quaternion_xyzw_from_wxyz,\n)\nfrom ._utils import (\n norm_vectors,\n angles_between_vectors,\n cross_product_matrices,\n)\n\n__all__ = [\n \"norm_vectors\",\n \"norm_axis_angles\",\n \"angles_between_vectors\",\n \"cross_product_matrices\",\n \"active_matrices_from_angles\",\n \"active_matrices_from_intrinsic_euler_angles\",\n \"active_matrices_from_extrinsic_euler_angles\",\n \"matrices_from_compact_axis_angles\",\n \"axis_angles_from_matrices\",\n \"quaternions_from_matrices\",\n \"smooth_quaternion_trajectory\",\n \"batch_concatenate_quaternions\",\n \"batch_q_conj\",\n \"quaternion_slerp_batch\",\n \"axis_angles_from_quaternions\",\n \"matrices_from_quaternions\",\n \"batch_quaternion_xyzw_from_wxyz\",\n \"batch_quaternion_wxyz_from_xyzw\",\n]\n" }, { "path": "pytransform3d/batch_rotations/_angle.py", "content": "\"\"\"Batch operations for angles.\"\"\"\n\nimport numpy as np\n\n\ndef active_matrices_from_angles(basis, angles, out=None):\n \"\"\"Compute active rotation matrices from rotation about basis vectors.\n\n Parameters\n ----------\n basis : int from [0, 1, 2]\n The rotation axis (0: x, 1: y, 2: z)\n\n angles : array-like, shape (...)\n Rotation angles\n\n out : array, shape (..., 3, 3), optional (default: new array)\n Output array to which we write the result\n\n Returns\n -------\n Rs : array, shape (..., 3, 3)\n Rotation matrices\n \"\"\"\n angles = np.asarray(angles)\n c = np.cos(angles)\n s = np.sin(angles)\n\n R_shape = angles.shape + (3, 3)\n if out is None:\n out = np.empty(R_shape)\n\n out[..., basis, :] = 0.0\n out[..., :, basis] = 0.0\n out[..., basis, basis] = 1.0\n basisp1 = (basis + 1) % 3\n basisp2 = (basis + 2) % 3\n out[..., basisp1, basisp1] = c\n out[..., basisp2, basisp2] = c\n out[..., basisp1, basisp2] = -s\n out[..., basisp2, basisp1] = s\n\n return out\n" }, { "path": "pytransform3d/batch_rotations/_angle.pyi", "content": "import numpy as np\nimport numpy.typing as npt\nfrom typing import Union\n\ndef active_matrices_from_angles(\n basis: int, angles: npt.ArrayLike, out: Union[npt.ArrayLike, None] = ...\n) -> np.ndarray: ...\n" }, { "path": "pytransform3d/batch_rotations/_axis_angle.py", "content": "\"\"\"Batch operations for axis-angle representation.\"\"\"\n\nimport numpy as np\n\nfrom ..rotations import norm_angle\nfrom ._utils import norm_vectors\n\n\ndef norm_axis_angles(a):\n \"\"\"Normalize axis-angle representation.\n\n Parameters\n ----------\n a : array-like, shape (..., 4)\n Axis of rotation and rotation angle: (x, y, z, angle)\n\n Returns\n -------\n a : array, shape (..., 4)\n Axis of rotation and rotation angle: (x, y, z, angle). The length\n of the axis vector is 1 and the angle is in [0, pi). No rotation\n is represented by [1, 0, 0, 0].\n \"\"\"\n a = np.asarray(a)\n\n # Handle the case of only one axis-angle instance\n only_one = a.ndim == 1\n a = np.atleast_2d(a)\n\n angles = a[..., 3]\n norm = np.linalg.norm(a[..., :3], axis=-1)\n\n no_rot_mask = (angles == 0.0) | (norm == 0.0)\n rot_mask = ~no_rot_mask\n\n res = np.empty_like(a)\n res[no_rot_mask, :] = np.array([1.0, 0.0, 0.0, 0.0])\n res[rot_mask, :3] = a[rot_mask, :3] / norm[rot_mask, np.newaxis]\n\n angle_normalized = norm_angle(angles)\n\n negative_angle_mask = angle_normalized < 0.0\n res[negative_angle_mask, :3] *= -1.0\n angle_normalized[negative_angle_mask] *= -1.0\n\n res[rot_mask, 3] = angle_normalized[rot_mask]\n\n if only_one:\n res = res[0]\n return res\n\n\ndef matrices_from_compact_axis_angles(A=None, axes=None, angles=None, out=None):\n \"\"\"Compute rotation matrices from compact axis-angle representations.\n\n This is called exponential map or Rodrigues' formula.\n\n This typically results in an active rotation matrix.\n\n Parameters\n ----------\n A : array-like, shape (..., 3)\n Axes of rotation and rotation angles in compact representation:\n angle * (x, y, z)\n\n axes : array, shape (..., 3)\n If the unit axes of rotation have been precomputed, you can pass them\n here.\n\n angles : array, shape (...)\n If the angles have been precomputed, you can pass them here.\n\n out : array, shape (..., 3, 3), optional (default: new array)\n Output array to which we write the result\n\n Returns\n -------\n Rs : array, shape (..., 3, 3)\n Rotation matrices\n \"\"\"\n if angles is None:\n thetas = np.linalg.norm(A, axis=-1)\n else:\n thetas = np.asarray(angles)\n\n if axes is None:\n omega_unit = norm_vectors(A)\n else:\n omega_unit = axes\n\n c = np.cos(thetas)\n s = np.sin(thetas)\n ci = 1.0 - c\n ux = omega_unit[..., 0]\n uy = omega_unit[..., 1]\n uz = omega_unit[..., 2]\n\n uxs = ux * s\n uys = uy * s\n uzs = uz * s\n ciux = ci * ux\n ciuy = ci * uy\n ciuxuy = ciux * uy\n ciuxuz = ciux * uz\n ciuyuz = ciuy * uz\n\n if out is None:\n out = np.empty(A.shape[:-1] + (3, 3))\n\n out[..., 0, 0] = ciux * ux + c\n out[..., 0, 1] = ciuxuy - uzs\n out[..., 0, 2] = ciuxuz + uys\n out[..., 1, 0] = ciuxuy + uzs\n out[..., 1, 1] = ciuy * uy + c\n out[..., 1, 2] = ciuyuz - uxs\n out[..., 2, 0] = ciuxuz - uys\n out[..., 2, 1] = ciuyuz + uxs\n out[..., 2, 2] = ci * uz * uz + c\n\n return out\n" }, { "path": "pytransform3d/batch_rotations/_axis_angle.pyi", "content": "import numpy as np\nimport numpy.typing as npt\nfrom typing import Union\n\ndef norm_axis_angles(a: npt.ArrayLike) -> np.ndarray: ...\ndef matrices_from_compact_axis_angles(\n A: Union[npt.ArrayLike, None] = ...,\n axes: Union[np.ndarray, None] = ...,\n angles: Union[np.ndarray, None] = ...,\n out: Union[np.ndarray, None] = ...,\n) -> np.ndarray: ...\n" }, { "path": "pytransform3d/batch_rotations/_euler.py", "content": "\"\"\"Euler angles.\"\"\"\n\nimport numpy as np\n\nfrom ._angle import active_matrices_from_angles\n\n\ndef active_matrices_from_intrinsic_euler_angles(\n basis1, basis2, basis3, e, out=None\n):\n \"\"\"Compute active rotation matrices from intrinsic Euler angles.\n\n Parameters\n ----------\n basis1 : int\n Basis vector of first rotation. 0 corresponds to x axis, 1 to y axis,\n and 2 to z axis.\n\n basis2 : int\n Basis vector of second rotation. 0 corresponds to x axis, 1 to y axis,\n and 2 to z axis.\n\n basis3 : int\n Basis vector of third rotation. 0 corresponds to x axis, 1 to y axis,\n and 2 to z axis.\n\n e : array-like, shape (..., 3)\n Euler angles\n\n out : array, shape (..., 3, 3), optional (default: new array)\n Output array to which we write the result\n\n Returns\n -------\n Rs : array, shape (..., 3, 3)\n Rotation matrices\n \"\"\"\n e = np.asarray(e)\n R_shape = e.shape + (3,)\n R_alpha = active_matrices_from_angles(basis1, e[..., 0].flat)\n R_beta = active_matrices_from_angles(basis2, e[..., 1].flat)\n R_gamma = active_matrices_from_angles(basis3, e[..., 2].flat)\n\n if out is None:\n out = np.empty(R_shape)\n\n out[:] = np.einsum(\n \"nij,njk->nik\", np.einsum(\"nij,njk->nik\", R_alpha, R_beta), R_gamma\n ).reshape(R_shape)\n\n return out\n\n\ndef active_matrices_from_extrinsic_euler_angles(\n basis1, basis2, basis3, e, out=None\n):\n \"\"\"Compute active rotation matrices from extrinsic Euler angles.\n\n Parameters\n ----------\n basis1 : int\n Basis vector of first rotation. 0 corresponds to x axis, 1 to y axis,\n and 2 to z axis.\n\n basis2 : int\n Basis vector of second rotation. 0 corresponds to x axis, 1 to y axis,\n and 2 to z axis.\n\n basis3 : int\n Basis vector of third rotation. 0 corresponds to x axis, 1 to y axis,\n and 2 to z axis.\n\n e : array-like, shape (..., 3)\n Euler angles\n\n out : array, shape (..., 3, 3), optional (default: new array)\n Output array to which we write the result\n\n Returns\n -------\n Rs : array, shape (..., 3, 3)\n Rotation matrices\n \"\"\"\n e = np.asarray(e)\n R_shape = e.shape + (3,)\n R_alpha = active_matrices_from_angles(basis1, e[..., 0].flat)\n R_beta = active_matrices_from_angles(basis2, e[..., 1].flat)\n R_gamma = active_matrices_from_angles(basis3, e[..., 2].flat)\n\n if out is None:\n out = np.empty(R_shape)\n\n out[:] = np.einsum(\n \"nij,njk->nik\", np.einsum(\"nij,njk->nik\", R_gamma, R_beta), R_alpha\n ).reshape(R_shape)\n\n return out\n" }, { "path": "pytransform3d/batch_rotations/_euler.pyi", "content": "import numpy as np\nimport numpy.typing as npt\nfrom typing import Union\n\ndef active_matrices_from_intrinsic_euler_angles(\n basis1: int,\n basis2: int,\n basis3: int,\n e: npt.ArrayLike,\n out: Union[npt.ArrayLike, None] = ...,\n) -> np.ndarray: ...\ndef active_matrices_from_extrinsic_euler_angles(\n basis1: int,\n basis2: int,\n basis3: int,\n e: npt.ArrayLike,\n out: Union[npt.ArrayLike, None] = ...,\n) -> np.ndarray: ...\n" }, { "path": "pytransform3d/batch_rotations/_matrix.py", "content": "\"\"\"Batch operations for rotation matrices.\"\"\"\n\nimport numpy as np\n\n\ndef axis_angles_from_matrices(Rs, traces=None, out=None):\n \"\"\"Compute compact axis-angle representations from rotation matrices.\n\n This is called logarithmic map.\n\n Parameters\n ----------\n Rs : array-like, shape (..., 3, 3)\n Rotation matrices\n\n traces : array, shape (..., 3)\n If the traces of rotation matrices been precomputed, you can pass them\n here.\n\n out : array, shape (..., 4), optional (default: new array)\n Output array to which we write the result\n\n Returns\n -------\n A : array, shape (..., 4)\n Axes of rotation and rotation angles: (x, y, z, angle)\n \"\"\"\n Rs = np.asarray(Rs)\n\n instances_shape = Rs.shape[:-2]\n\n if traces is None:\n traces = np.einsum(\"nii\", Rs.reshape(-1, 3, 3))\n if instances_shape:\n traces = traces.reshape(*instances_shape)\n else:\n # this works because indX will be a single boolean and\n # out[True, n] = value will assign value to out[n], while\n # out[False, n] = value will not assign value to out[n]\n traces = traces[0]\n\n angles = np.arccos(np.clip((traces - 1.0) / 2.0, -1.0, 1.0))\n\n if out is None:\n out = np.empty(instances_shape + (4,))\n\n out[..., 0] = Rs[..., 2, 1] - Rs[..., 1, 2]\n out[..., 1] = Rs[..., 0, 2] - Rs[..., 2, 0]\n out[..., 2] = Rs[..., 1, 0] - Rs[..., 0, 1]\n\n # The threshold is a result from this discussion:\n # https://github.com/dfki-ric/pytransform3d/issues/43\n # The standard formula becomes numerically unstable, however,\n # Rodrigues' formula reduces to R = I + 2 (ee^T - I), with the\n # rotation axis e, that is, ee^T = 0.5 * (R + I) and we can find the\n # squared values of the rotation axis on the diagonal of this matrix.\n # We can still use the original formula to reconstruct the signs of\n # the rotation axis correctly.\n angle_close_to_pi = np.abs(angles - np.pi) < 1e-4\n angle_zero = angles == 0.0\n angle_not_zero = np.logical_not(angle_zero)\n\n Rs_diag = np.einsum(\"nii->ni\", Rs.reshape(-1, 3, 3))\n if instances_shape:\n Rs_diag = Rs_diag.reshape(*(instances_shape + (3,)))\n else:\n Rs_diag = Rs_diag[0]\n\n out[angle_close_to_pi, :3] = np.sqrt(\n 0.5 * (Rs_diag[angle_close_to_pi] + 1.0)\n ) * np.sign(out[angle_close_to_pi, :3])\n out[angle_not_zero, :3] /= np.linalg.norm(out[angle_not_zero, :3], axis=-1)[\n ..., np.newaxis\n ]\n\n out[angle_zero, 0] = 1.0\n out[angle_zero, 1:3] = 0.0\n\n out[..., 3] = angles\n\n return out\n\n\ndef quaternions_from_matrices(Rs, out=None):\n \"\"\"Compute quaternions from rotation matrices.\n\n Parameters\n ----------\n Rs : array-like, shape (..., 3, 3)\n Rotation matrices\n\n out : array, shape (..., 4), optional (default: new array)\n Output array to which we write the result\n\n Returns\n -------\n Q : array, shape (..., 4)\n Unit quaternions to represent rotations: (w, x, y, z)\n \"\"\"\n Rs = np.asarray(Rs)\n instances_shape = Rs.shape[:-2]\n\n if out is None:\n out = np.empty(instances_shape + (4,))\n\n traces = np.einsum(\"nii\", Rs.reshape(-1, 3, 3))\n if instances_shape:\n traces = traces.reshape(*instances_shape)\n else:\n # this works because indX will be a single boolean and\n # out[True, n] = value will assign value to out[n], while\n # out[False, n] = value will not assign value to out[n]\n traces = traces[0]\n ind1 = traces > 0.0\n s = 2.0 * np.sqrt(1.0 + traces[ind1])\n out[ind1, 0] = 0.25 * s\n out[ind1, 1] = (Rs[ind1, 2, 1] - Rs[ind1, 1, 2]) / s\n out[ind1, 2] = (Rs[ind1, 0, 2] - Rs[ind1, 2, 0]) / s\n out[ind1, 3] = (Rs[ind1, 1, 0] - Rs[ind1, 0, 1]) / s\n\n ind2 = np.logical_and(\n np.logical_not(ind1),\n np.logical_and(\n Rs[..., 0, 0] > Rs[..., 1, 1], Rs[..., 0, 0] > Rs[..., 2, 2]\n ),\n )\n s = 2.0 * np.sqrt(1.0 + Rs[ind2, 0, 0] - Rs[ind2, 1, 1] - Rs[ind2, 2, 2])\n out[ind2, 0] = (Rs[ind2, 2, 1] - Rs[ind2, 1, 2]) / s\n out[ind2, 1] = 0.25 * s\n out[ind2, 2] = (Rs[ind2, 1, 0] + Rs[ind2, 0, 1]) / s\n out[ind2, 3] = (Rs[ind2, 0, 2] + Rs[ind2, 2, 0]) / s\n\n ind3 = np.logical_and(np.logical_not(ind1), Rs[..., 1, 1] > Rs[..., 2, 2])\n s = 2.0 * np.sqrt(1.0 + Rs[ind3, 1, 1] - Rs[ind3, 0, 0] - Rs[ind3, 2, 2])\n out[ind3, 0] = (Rs[ind3, 0, 2] - Rs[ind3, 2, 0]) / s\n out[ind3, 1] = (Rs[ind3, 1, 0] + Rs[ind3, 0, 1]) / s\n out[ind3, 2] = 0.25 * s\n out[ind3, 3] = (Rs[ind3, 2, 1] + Rs[ind3, 1, 2]) / s\n\n ind4 = np.logical_and(\n np.logical_and(np.logical_not(ind1), np.logical_not(ind2)),\n np.logical_not(ind3),\n )\n s = 2.0 * np.sqrt(1.0 + Rs[ind4, 2, 2] - Rs[ind4, 0, 0] - Rs[ind4, 1, 1])\n out[ind4, 0] = (Rs[ind4, 1, 0] - Rs[ind4, 0, 1]) / s\n out[ind4, 1] = (Rs[ind4, 0, 2] + Rs[ind4, 2, 0]) / s\n out[ind4, 2] = (Rs[ind4, 2, 1] + Rs[ind4, 1, 2]) / s\n out[ind4, 3] = 0.25 * s\n\n return out\n" }, { "path": "pytransform3d/batch_rotations/_matrix.pyi", "content": "import numpy as np\nimport numpy.typing as npt\nfrom typing import Union\n\ndef axis_angles_from_matrices(\n Rs: npt.ArrayLike,\n traces: Union[np.ndarray, None] = ...,\n out: Union[np.ndarray, None] = ...,\n) -> np.ndarray: ...\ndef quaternions_from_matrices(\n Rs: npt.ArrayLike, out: Union[np.ndarray, None] = ...\n) -> np.ndarray: ...\n" }, { "path": "pytransform3d/batch_rotations/_quaternion.py", "content": "\"\"\"Batch operations for quaternions.\"\"\"\n\nimport numpy as np\n\nfrom ._axis_angle import norm_axis_angles\nfrom ._utils import norm_vectors\nfrom ..rotations import (\n angle_between_vectors,\n slerp_weights,\n pick_closest_quaternion,\n)\n\n\ndef smooth_quaternion_trajectory(Q, start_component_positive=\"x\"):\n \"\"\"Smooth quaternion trajectory.\n\n Quaternion q and -q represent the same rotation but cannot be\n interpolated well. This function guarantees that two successive\n quaternions q1 and q2 are closer than q1 and -q2.\n\n Parameters\n ----------\n Q : array-like, shape (n_steps, 4)\n Unit quaternions to represent rotations: (w, x, y, z)\n\n start_component_positive : str, optional (default: 'x')\n Start trajectory with quaternion that has this component positive.\n Allowed values: 'w' (scalar), 'x', 'y', and 'z'.\n\n Returns\n -------\n Q : array, shape (n_steps, 4)\n Unit quaternions to represent rotations: (w, x, y, z)\n\n Raises\n ------\n ValueError\n If Q has length 0.\n \"\"\"\n Q = np.copy(Q)\n\n if len(Q) == 0:\n raise ValueError(\"At least one quaternion is expected.\")\n\n if Q[0, \"wxyz\".index(start_component_positive)] < 0.0:\n Q[0] *= -1.0\n\n q1q2_dists = np.linalg.norm(Q[:-1] - Q[1:], axis=1)\n q1mq2_dists = np.linalg.norm(Q[:-1] + Q[1:], axis=1)\n before_jump_indices = np.where(q1q2_dists > q1mq2_dists)[0]\n\n # workaround for interpolation artifacts:\n before_smooth_jump_indices = np.isclose(q1q2_dists, q1mq2_dists)\n before_smooth_jump_indices = np.where(\n np.logical_and(\n before_smooth_jump_indices[:-1], before_smooth_jump_indices[1:]\n )\n )[0]\n before_jump_indices = np.unique(\n np.hstack((before_jump_indices, before_smooth_jump_indices))\n ).tolist()\n before_jump_indices.append(len(Q) - 1)\n\n slices_to_correct = np.array(\n list(zip(before_jump_indices[:-1], before_jump_indices[1:]))\n )[::2]\n for i, j in slices_to_correct:\n Q[i + 1 : j + 1] *= -1.0\n return Q\n\n\ndef batch_concatenate_quaternions(Q1, Q2, out=None):\n \"\"\"Concatenate two batches of quaternions.\n\n We use Hamilton's quaternion multiplication.\n\n Suppose we want to apply two extrinsic rotations given by quaternions\n q1 and q2 to a vector v. We can either apply q2 to v and then q1 to\n the result or we can concatenate q1 and q2 and apply the result to v.\n\n Parameters\n ----------\n Q1 : array-like, shape (..., 4)\n First batch of quaternions\n\n Q2 : array-like, shape (..., 4)\n Second batch of quaternions\n\n out : array, shape (..., 4), optional (default: new array)\n Output array to which we write the result\n\n Returns\n -------\n Q12 : array, shape (..., 4)\n Batch of quaternions that represents the concatenated rotations\n\n Raises\n ------\n ValueError\n If the input dimensions are incorrect\n \"\"\"\n Q1 = np.asarray(Q1)\n Q2 = np.asarray(Q2)\n\n if Q1.ndim != Q2.ndim:\n raise ValueError(\n \"Number of dimensions must be the same. \"\n \"Got %d for Q1 and %d for Q2.\" % (Q1.ndim, Q2.ndim)\n )\n for d in range(Q1.ndim - 1):\n if Q1.shape[d] != Q2.shape[d]:\n raise ValueError(\n \"Size of dimension %d does not match: %d != %d\"\n % (d + 1, Q1.shape[d], Q2.shape[d])\n )\n if Q1.shape[-1] != 4:\n raise ValueError(\n \"Last dimension of first argument does not match. A quaternion \"\n \"must have 4 entries, got %d\" % Q1.shape[-1]\n )\n if Q2.shape[-1] != 4:\n raise ValueError(\n \"Last dimension of second argument does not match. A quaternion \"\n \"must have 4 entries, got %d\" % Q2.shape[-1]\n )\n\n if out is None:\n out = np.empty_like(Q1)\n\n vector_inner_products = np.sum(Q1[..., 1:] * Q2[..., 1:], axis=-1)\n out[..., 0] = Q1[..., 0] * Q2[..., 0] - vector_inner_products\n out[..., 1:] = (\n Q1[..., 0, np.newaxis] * Q2[..., 1:]\n + Q2[..., 0, np.newaxis] * Q1[..., 1:]\n + np.cross(Q1[..., 1:], Q2[..., 1:])\n )\n return out\n\n\ndef batch_q_conj(Q):\n \"\"\"Conjugate of quaternions.\n\n The conjugate of a unit quaternion inverts the rotation represented by\n this unit quaternion. The conjugate of a quaternion q is often denoted\n as q*.\n\n Parameters\n ----------\n Q : array-like, shape (..., 4)\n Unit quaternions to represent rotations: (w, x, y, z)\n\n Returns\n -------\n Q_c : array, shape (..., 4,)\n Conjugates (w, -x, -y, -z)\n \"\"\"\n Q = np.asarray(Q)\n out = np.empty_like(Q)\n out[..., 0] = Q[..., 0]\n out[..., 1:] = -Q[..., 1:]\n return out\n\n\ndef quaternion_slerp_batch(start, end, t, shortest_path=False):\n \"\"\"Spherical linear interpolation for a batch of steps.\n\n Parameters\n ----------\n start : array-like, shape (4,)\n Start unit quaternion to represent rotation: (w, x, y, z)\n\n end : array-like, shape (4,)\n End unit quaternion to represent rotation: (w, x, y, z)\n\n t : array-like, shape (n_steps,)\n Steps between start and goal, must be in interval [0, 1]\n\n shortest_path : bool, optional (default: False)\n Resolve sign ambiguity before interpolation to find the shortest path.\n The end quaternion will be picked to be close to the start quaternion.\n\n Returns\n -------\n Q : array, shape (n_steps, 4)\n Interpolated unit quaternions\n \"\"\"\n t = np.asarray(t)\n if shortest_path:\n end = pick_closest_quaternion(end, start)\n angle = angle_between_vectors(start, end)\n w1, w2 = slerp_weights(angle, t)\n return (\n w1[:, np.newaxis] * start[np.newaxis]\n + w2[:, np.newaxis] * end[np.newaxis]\n )\n\n\ndef axis_angles_from_quaternions(qs):\n \"\"\"Compute axis-angle from quaternion.\n\n This operation is called logarithmic map.\n\n We usually assume active rotations.\n\n Parameters\n ----------\n qs : array-like, shape (..., 4)\n Unit quaternion to represent rotation: (w, x, y, z)\n\n Returns\n -------\n as : array, shape (..., 4)\n Axis of rotation and rotation angle: (x, y, z, angle). The angle is\n constrained to [0, pi) so that the mapping is unique.\n \"\"\"\n quaternion_vector_part = qs[..., 1:]\n qvec_norm = np.linalg.norm(quaternion_vector_part, axis=-1)\n\n # Vectorized branches bases on norm of the vector part\n small_p_norm_mask = qvec_norm < np.finfo(float).eps\n non_zero_mask = ~small_p_norm_mask\n\n axes = (\n quaternion_vector_part[non_zero_mask]\n / qvec_norm[non_zero_mask, np.newaxis]\n )\n\n w_clamped = np.clip(qs[non_zero_mask, 0], -1.0, 1.0)\n angles = 2.0 * np.arccos(w_clamped)\n\n result = np.empty_like(qs)\n result[non_zero_mask] = norm_axis_angles(\n np.concatenate((axes, angles[..., np.newaxis]), axis=-1)\n )\n result[small_p_norm_mask] = np.array([1.0, 0.0, 0.0, 0.0])\n\n return result\n\n\ndef matrices_from_quaternions(Q, normalize_quaternions=True, out=None):\n \"\"\"Compute rotation matrices from quaternions.\n\n Parameters\n ----------\n Q : array-like, shape (..., 4)\n Unit quaternions to represent rotations: (w, x, y, z)\n\n normalize_quaternions : bool, optional (default: True)\n Normalize quaternions before conversion\n\n out : array, shape (..., 3, 3), optional (default: new array)\n Output array to which we write the result\n\n Returns\n -------\n Rs : array, shape (..., 3, 3)\n Rotation matrices\n \"\"\"\n Q = np.asarray(Q)\n\n if normalize_quaternions:\n Q = norm_vectors(Q)\n\n w = Q[..., 0]\n x = Q[..., 1]\n y = Q[..., 2]\n z = Q[..., 3]\n\n x2 = 2.0 * x * x\n y2 = 2.0 * y * y\n z2 = 2.0 * z * z\n xy = 2.0 * x * y\n xz = 2.0 * x * z\n yz = 2.0 * y * z\n xw = 2.0 * x * w\n yw = 2.0 * y * w\n zw = 2.0 * z * w\n\n if out is None:\n out = np.empty(w.shape + (3, 3))\n\n out[..., 0, 0] = 1.0 - y2 - z2\n out[..., 0, 1] = xy - zw\n out[..., 0, 2] = xz + yw\n out[..., 1, 0] = xy + zw\n out[..., 1, 1] = 1.0 - x2 - z2\n out[..., 1, 2] = yz - xw\n out[..., 2, 0] = xz - yw\n out[..., 2, 1] = yz + xw\n out[..., 2, 2] = 1.0 - x2 - y2\n\n return out\n\n\ndef batch_quaternion_wxyz_from_xyzw(Q_xyzw, out=None):\n \"\"\"Converts from x, y, z, w to w, x, y, z convention.\n\n Parameters\n ----------\n Q_xyzw : array-like, shape (..., 4)\n Quaternions with scalar part after vector part\n\n out : array, shape (..., 4), optional (default: new array)\n Output array to which we write the result\n\n Returns\n -------\n Q_wxyz : array-like, shape (..., 4)\n Quaternions with scalar part before vector part\n \"\"\"\n Q_xyzw = np.asarray(Q_xyzw)\n if out is None:\n out = np.empty_like(Q_xyzw)\n out[..., 0] = Q_xyzw[..., 3]\n out[..., 1] = Q_xyzw[..., 0]\n out[..., 2] = Q_xyzw[..., 1]\n out[..., 3] = Q_xyzw[..., 2]\n return out\n\n\ndef batch_quaternion_xyzw_from_wxyz(Q_wxyz, out=None):\n \"\"\"Converts from w, x, y, z to x, y, z, w convention.\n\n Parameters\n ----------\n Q_wxyz : array-like, shape (..., 4)\n Quaternions with scalar part before vector part\n\n out : array, shape (..., 4), optional (default: new array)\n Output array to which we write the result\n\n Returns\n -------\n Q_xyzw : array-like, shape (..., 4)\n Quaternions with scalar part after vector part\n \"\"\"\n Q_wxyz = np.asarray(Q_wxyz)\n if out is None:\n out = np.empty_like(Q_wxyz)\n out[..., 0] = Q_wxyz[..., 1]\n out[..., 1] = Q_wxyz[..., 2]\n out[..., 2] = Q_wxyz[..., 3]\n out[..., 3] = Q_wxyz[..., 0]\n return out\n" }, { "path": "pytransform3d/batch_rotations/_quaternion.pyi", "content": "import numpy as np\nimport numpy.typing as npt\nfrom typing import Union, AnyStr\n\ndef smooth_quaternion_trajectory(\n Q: npt.ArrayLike, start_component_positive: AnyStr = ...\n) -> np.ndarray: ...\ndef batch_concatenate_quaternions(\n Q1: npt.ArrayLike, Q2: npt.ArrayLike, out: Union[np.ndarray, None] = ...\n) -> np.ndarray: ...\ndef batch_q_conj(Q: npt.ArrayLike) -> np.ndarray: ...\ndef quaternion_slerp_batch(\n start: npt.ArrayLike,\n end: npt.ArrayLike,\n t: npt.ArrayLike,\n shortest_path: bool = ...,\n) -> np.ndarray: ...\ndef axis_angles_from_quaternions(qs: npt.ArrayLike) -> np.ndarray: ...\ndef matrices_from_quaternions(\n Q: npt.ArrayLike,\n normalize_quaternions: bool = ...,\n out: Union[np.ndarray, None] = ...,\n) -> np.ndarray: ...\ndef batch_quaternion_wxyz_from_xyzw(\n Q_xyzw: npt.ArrayLike, out: Union[np.ndarray, None] = ...\n) -> np.ndarray: ...\ndef batch_quaternion_xyzw_from_wxyz(\n Q_wxyz: npt.ArrayLike, out: Union[np.ndarray, None] = ...\n) -> np.ndarray: ...\n" }, { "path": "pytransform3d/batch_rotations/_utils.py", "content": "\"\"\"Utility functions.\"\"\"\n\nimport numpy as np\n\n\ndef norm_vectors(V, out=None):\n \"\"\"Normalize vectors.\n\n Parameters\n ----------\n V : array-like, shape (..., n)\n nd vectors\n\n out : array, shape (..., n), optional (default: new array)\n Output array to which we write the result\n\n Returns\n -------\n V_unit : array, shape (..., n)\n nd unit vectors with norm 1 or zero vectors\n \"\"\"\n V = np.asarray(V)\n norms = np.linalg.norm(V, axis=-1)\n if out is None:\n out = np.empty_like(V)\n # Avoid division by zero with np.maximum(..., smallest positive float).\n # The norm is zero only when the vector is zero so this case does not\n # require further processing.\n out[...] = V / np.maximum(norms[..., np.newaxis], np.finfo(float).tiny)\n return out\n\n\ndef angles_between_vectors(A, B):\n \"\"\"Compute angle between two vectors.\n\n Parameters\n ----------\n A : array-like, shape (..., n)\n nd vectors\n\n B : array-like, shape (..., n)\n nd vectors\n\n Returns\n -------\n angles : array, shape (...)\n Angles between pairs of vectors from A and B\n \"\"\"\n A = np.asarray(A)\n B = np.asarray(B)\n n_dims = A.shape[-1]\n A_norms = np.linalg.norm(A, axis=-1)\n B_norms = np.linalg.norm(B, axis=-1)\n AdotB = np.einsum(\n \"ni,ni->n\", A.reshape(-1, n_dims), B.reshape(-1, n_dims)\n ).reshape(A.shape[:-1])\n return np.arccos(np.clip(AdotB / (A_norms * B_norms), -1.0, 1.0))\n\n\ndef cross_product_matrices(V):\n \"\"\"Generate the cross-product matrices of vectors.\n\n The cross-product matrix :math:`\\\\boldsymbol{V}` satisfies the equation\n\n .. math::\n\n \\\\boldsymbol{V} \\\\boldsymbol{w} = \\\\boldsymbol{v} \\\\times\n \\\\boldsymbol{w}\n\n It is a skew-symmetric (antisymmetric) matrix, i.e.\n :math:`-\\\\boldsymbol{V} = \\\\boldsymbol{V}^T`.\n\n Parameters\n ----------\n V : array-like, shape (..., 3)\n 3d vectors\n\n Returns\n -------\n V_cross_product_matrices : array, shape (..., 3, 3)\n Cross-product matrices of V\n \"\"\"\n V = np.asarray(V)\n\n instances_shape = V.shape[:-1]\n V_matrices = np.empty(instances_shape + (3, 3))\n\n V_matrices[..., 0, 0] = 0.0\n V_matrices[..., 0, 1] = -V[..., 2]\n V_matrices[..., 0, 2] = V[..., 1]\n V_matrices[..., 1, 0] = V[..., 2]\n V_matrices[..., 1, 1] = 0.0\n V_matrices[..., 1, 2] = -V[..., 0]\n V_matrices[..., 2, 0] = -V[..., 1]\n V_matrices[..., 2, 1] = V[..., 0]\n V_matrices[..., 2, 2] = 0.0\n\n return V_matrices\n" }, { "path": "pytransform3d/batch_rotations/_utils.pyi", "content": "import numpy as np\nimport numpy.typing as npt\nfrom typing import Union\n\ndef norm_vectors(\n V: npt.ArrayLike, out: Union[npt.ArrayLike, None] = ...\n) -> np.ndarray: ...\ndef angles_between_vectors(\n A: npt.ArrayLike, B: npt.ArrayLike\n) -> np.ndarray: ...\ndef cross_product_matrices(V: npt.ArrayLike) -> np.ndarray: ...\n" }, { "path": "pytransform3d/batch_rotations/test/test_batch_rotations.py", "content": "import numpy as np\nimport pytest\nfrom numpy.testing import assert_array_almost_equal\n\nfrom pytransform3d import batch_rotations as pbr\nfrom pytransform3d import rotations as pr\n\n\ndef test_norm_vectors_0dims():\n rng = np.random.default_rng(8380)\n V = rng.standard_normal(size=3)\n V_unit = pbr.norm_vectors(V)\n assert pytest.approx(np.linalg.norm(V_unit)) == 1.0\n\n\ndef test_norm_vectors_1dim():\n rng = np.random.default_rng(8381)\n V = rng.standard_normal(size=(100, 3))\n V_unit = pbr.norm_vectors(V)\n assert_array_almost_equal(np.linalg.norm(V_unit, axis=1), np.ones(len(V)))\n\n\ndef test_norm_vectors_1dim_output_variable():\n rng = np.random.default_rng(8381)\n V = rng.standard_normal(size=(100, 3))\n pbr.norm_vectors(V, out=V)\n assert_array_almost_equal(np.linalg.norm(V, axis=1), np.ones(len(V)))\n\n\ndef test_norm_vectors_3dims():\n rng = np.random.default_rng(8382)\n V = rng.standard_normal(size=(8, 2, 8, 3))\n V_unit = pbr.norm_vectors(V)\n assert_array_almost_equal(\n np.linalg.norm(V_unit, axis=-1), np.ones(V_unit.shape[:-1])\n )\n\n\ndef test_norm_vectors_zero():\n V = np.zeros((3, 8, 1, 2))\n V_unit = pbr.norm_vectors(V)\n assert_array_almost_equal(V_unit, V)\n\n\ndef test_norm_axis_angles():\n rng = np.random.default_rng(843)\n # create a batch of unnormalized axis-angle instances\n n_rotations = 10\n A_unnormalized = np.hstack(\n (\n rng.standard_normal(size=(n_rotations, 3)),\n np.pi + rng.uniform(size=(n_rotations, 1)),\n )\n )\n\n # cross check scalar version with vectorized version when passing\n # an 1D array\n assert_array_almost_equal(\n pbr.norm_axis_angles(A_unnormalized[0]),\n pr.norm_axis_angle(A_unnormalized[0]),\n )\n\n # cross check scalar version with vectorized version when passing\n # a 3D array\n assert_array_almost_equal(\n pbr.norm_axis_angles(np.array([[[0.0, 0.0, 0.0, np.pi]]]))[0, 0],\n pr.norm_axis_angle(np.array([0.0, 0.0, 0.0, np.pi])),\n )\n\n # 2D\n A_norm = pbr.norm_axis_angles(A_unnormalized)\n for a_unnormalized, a_norm in zip(A_unnormalized, A_norm):\n # Make sure that the unnormalized version was in fact not normalized.\n assert pytest.approx(a_unnormalized[3]) != a_norm[3]\n assert pytest.approx(np.linalg.norm(a_unnormalized[:3])) != 1.0\n assert_array_almost_equal(a_norm, pr.norm_axis_angle(a_unnormalized))\n\n # 3D\n A3D = A_unnormalized.reshape(n_rotations // 2, 2, -1)\n A3D_norm = pbr.norm_axis_angles(A3D)\n for a_unnormalized, a_norm in zip(\n A3D.reshape(n_rotations, -1), A3D_norm.reshape(n_rotations, -1)\n ):\n assert_array_almost_equal(a_norm, pr.norm_axis_angle(a_unnormalized))\n\n\ndef test_angles_between_vectors_0dims():\n rng = np.random.default_rng(228)\n A = rng.standard_normal(size=3)\n B = rng.standard_normal(size=3)\n angles = pbr.angles_between_vectors(A, B)\n angles2 = pr.angle_between_vectors(A, B)\n assert_array_almost_equal(angles, angles2)\n\n\ndef test_angles_between_vectors_1dim():\n rng = np.random.default_rng(229)\n A = rng.standard_normal(size=(100, 3))\n B = rng.standard_normal(size=(100, 3))\n angles = pbr.angles_between_vectors(A, B)\n angles2 = [pr.angle_between_vectors(a, b) for a, b in zip(A, B)]\n assert_array_almost_equal(angles, angles2)\n\n\ndef test_angles_between_vectors_3dims():\n rng = np.random.default_rng(230)\n A = rng.standard_normal(size=(2, 4, 3, 4))\n B = rng.standard_normal(size=(2, 4, 3, 4))\n angles = pbr.angles_between_vectors(A, B).ravel()\n angles2 = [\n pr.angle_between_vectors(a, b)\n for a, b in zip(A.reshape(-1, 4), B.reshape(-1, 4))\n ]\n assert_array_almost_equal(angles, angles2)\n\n\ndef test_active_matrices_from_angles_0dims():\n R = pbr.active_matrices_from_angles(0, 0.4)\n assert_array_almost_equal(R, pr.active_matrix_from_angle(0, 0.4))\n\n\ndef test_active_matrices_from_angles_1dim():\n Rs = pbr.active_matrices_from_angles(1, [0.4, 0.5, 0.6])\n assert_array_almost_equal(Rs[0], pr.active_matrix_from_angle(1, 0.4))\n assert_array_almost_equal(Rs[1], pr.active_matrix_from_angle(1, 0.5))\n assert_array_almost_equal(Rs[2], pr.active_matrix_from_angle(1, 0.6))\n\n\ndef test_active_matrices_from_angles_3dims():\n rng = np.random.default_rng(8383)\n angles = rng.standard_normal(size=(2, 3, 4))\n Rs = pbr.active_matrices_from_angles(2, angles)\n Rs = Rs.reshape(-1, 3, 3)\n Rs2 = [\n pr.active_matrix_from_angle(2, angle) for angle in angles.reshape(-1)\n ]\n assert_array_almost_equal(Rs, Rs2)\n\n\ndef test_active_matrices_from_angles_3dims_output_variable():\n rng = np.random.default_rng(8384)\n angles = rng.standard_normal(size=(2, 3, 4))\n Rs = np.empty((2, 3, 4, 3, 3))\n pbr.active_matrices_from_angles(2, angles, out=Rs)\n Rs = Rs.reshape(-1, 3, 3)\n Rs2 = [\n pr.active_matrix_from_angle(2, angle) for angle in angles.reshape(-1)\n ]\n assert_array_almost_equal(Rs, Rs2)\n\n\ndef test_active_matrices_from_intrinsic_euler_angles_0dims():\n rng = np.random.default_rng(8383)\n e = rng.standard_normal(size=3)\n R = pbr.active_matrices_from_intrinsic_euler_angles(2, 1, 0, e)\n R2 = pr.active_matrix_from_intrinsic_euler_zyx(e)\n assert_array_almost_equal(R, R2)\n\n\ndef test_active_matrices_from_intrinsic_euler_angles_1dim():\n rng = np.random.default_rng(8384)\n e = rng.standard_normal(size=(10, 3))\n Rs = pbr.active_matrices_from_intrinsic_euler_angles(2, 1, 0, e)\n for i in range(len(e)):\n Ri = pr.active_matrix_from_intrinsic_euler_zyx(e[i])\n assert_array_almost_equal(Rs[i], Ri)\n\n\ndef test_active_matrices_from_intrinsic_euler_angles_1dim_output_variables():\n rng = np.random.default_rng(8384)\n e = rng.standard_normal(size=(10, 3))\n Rs = np.empty((10, 3, 3))\n pbr.active_matrices_from_intrinsic_euler_angles(2, 1, 0, e, out=Rs)\n for i in range(len(e)):\n Ri = pr.active_matrix_from_intrinsic_euler_zyx(e[i])\n assert_array_almost_equal(Rs[i], Ri)\n\n\ndef test_active_matrices_from_intrinsic_euler_angles_3dims():\n rng = np.random.default_rng(8385)\n e = rng.standard_normal(size=(2, 3, 4, 3))\n Rs = pbr.active_matrices_from_intrinsic_euler_angles(2, 1, 0, e).reshape(\n -1, 3, 3\n )\n e = e.reshape(-1, 3)\n for i in range(len(e)):\n Ri = pr.active_matrix_from_intrinsic_euler_zyx(e[i])\n assert_array_almost_equal(Rs[i], Ri)\n\n\ndef test_active_matrices_from_extrinsic_euler_angles_0dims():\n rng = np.random.default_rng(8383)\n e = rng.standard_normal(size=3)\n R = pbr.active_matrices_from_extrinsic_euler_angles(2, 1, 0, e)\n R2 = pr.active_matrix_from_extrinsic_euler_zyx(e)\n assert_array_almost_equal(R, R2)\n\n\ndef test_active_matrices_from_extrinsic_euler_angles_1dim():\n rng = np.random.default_rng(8384)\n e = rng.standard_normal(size=(10, 3))\n Rs = pbr.active_matrices_from_extrinsic_euler_angles(2, 1, 0, e)\n for i in range(len(e)):\n Ri = pr.active_matrix_from_extrinsic_euler_zyx(e[i])\n assert_array_almost_equal(Rs[i], Ri)\n\n\ndef test_active_matrices_from_extrinsic_euler_angles_3dim():\n rng = np.random.default_rng(8385)\n e = rng.standard_normal(size=(2, 3, 4, 3))\n Rs = pbr.active_matrices_from_extrinsic_euler_angles(2, 1, 0, e).reshape(\n -1, 3, 3\n )\n e = e.reshape(-1, 3)\n for i in range(len(e)):\n Ri = pr.active_matrix_from_extrinsic_euler_zyx(e[i])\n assert_array_almost_equal(Rs[i], Ri)\n\n\ndef test_active_matrices_from_extrinsic_euler_angles_1dim_output_variable():\n rng = np.random.default_rng(8385)\n e = rng.standard_normal(size=(10, 3))\n Rs = np.empty((10, 3, 3))\n pbr.active_matrices_from_extrinsic_euler_angles(2, 1, 0, e, out=Rs)\n for i in range(len(e)):\n Ri = pr.active_matrix_from_extrinsic_euler_zyx(e[i])\n assert_array_almost_equal(Rs[i], Ri)\n\n\ndef test_cross_product_matrix():\n rng = np.random.default_rng(3820)\n v = rng.standard_normal(size=3)\n assert_array_almost_equal(\n pbr.cross_product_matrices(v), pr.cross_product_matrix(v)\n )\n\n\ndef test_cross_product_matrices():\n rng = np.random.default_rng(3820)\n V = rng.standard_normal(size=(2, 2, 3, 3))\n V_cpm = pbr.cross_product_matrices(V)\n V_cpm = V_cpm.reshape(-1, 3, 3)\n V_cpm2 = [pr.cross_product_matrix(v) for v in V.reshape(-1, 3)]\n assert_array_almost_equal(V_cpm, V_cpm2)\n\n\ndef test_matrices_from_quaternions():\n rng = np.random.default_rng(83)\n for _ in range(5):\n q = pr.random_quaternion(rng)\n R = pbr.matrices_from_quaternions(\n q[np.newaxis], normalize_quaternions=False\n )[0]\n q2 = pr.quaternion_from_matrix(R)\n pr.assert_quaternion_equal(q, q2)\n\n for _ in range(5):\n q = rng.standard_normal(size=4)\n R = pbr.matrices_from_quaternions(\n q[np.newaxis], normalize_quaternions=True\n )[0]\n q2 = pr.quaternion_from_matrix(R)\n pr.assert_quaternion_equal(q / np.linalg.norm(q), q2)\n\n\ndef test_quaternions_from_matrices():\n rng = np.random.default_rng(84)\n for _ in range(5):\n q = pr.random_quaternion(rng)\n R = pr.matrix_from_quaternion(q)\n q2 = pbr.quaternions_from_matrices(R[np.newaxis])[0]\n pr.assert_quaternion_equal(q, q2)\n\n a = np.array([1.0, 0.0, 0.0, np.pi])\n q = pr.quaternion_from_axis_angle(a)\n R = pr.matrix_from_axis_angle(a)\n q_from_R = pbr.quaternions_from_matrices(R[np.newaxis])[0]\n assert_array_almost_equal(q, q_from_R)\n\n a = np.array([0.0, 1.0, 0.0, np.pi])\n q = pr.quaternion_from_axis_angle(a)\n R = pr.matrix_from_axis_angle(a)\n q_from_R = pbr.quaternions_from_matrices(R[np.newaxis])[0]\n assert_array_almost_equal(q, q_from_R)\n\n a = np.array([0.0, 0.0, 1.0, np.pi])\n q = pr.quaternion_from_axis_angle(a)\n R = pr.matrix_from_axis_angle(a)\n q_from_R = pbr.quaternions_from_matrices(R[np.newaxis])[0]\n assert_array_almost_equal(q, q_from_R)\n\n\ndef test_quaternions_from_matrices_no_batch():\n rng = np.random.default_rng(85)\n for _ in range(5):\n q = pr.random_quaternion(rng)\n R = pr.matrix_from_quaternion(q)\n q2 = pbr.quaternions_from_matrices(R)\n pr.assert_quaternion_equal(q, q2)\n\n a = np.array([1.0, 0.0, 0.0, np.pi])\n q = pr.quaternion_from_axis_angle(a)\n R = pr.matrix_from_axis_angle(a)\n q_from_R = pbr.quaternions_from_matrices(R)\n assert_array_almost_equal(q, q_from_R)\n\n a = np.array([0.0, 1.0, 0.0, np.pi])\n q = pr.quaternion_from_axis_angle(a)\n R = pr.matrix_from_axis_angle(a)\n q_from_R = pbr.quaternions_from_matrices(R)\n assert_array_almost_equal(q, q_from_R)\n\n a = np.array([0.0, 0.0, 1.0, np.pi])\n q = pr.quaternion_from_axis_angle(a)\n R = pr.matrix_from_axis_angle(a)\n q_from_R = pbr.quaternions_from_matrices(R)\n assert_array_almost_equal(q, q_from_R)\n\n\ndef test_quaternions_from_matrices_4d():\n rng = np.random.default_rng(84)\n for _ in range(5):\n q = pr.random_quaternion(rng)\n R = pr.matrix_from_quaternion(q)\n q2 = pbr.quaternions_from_matrices([[R, R], [R, R]])\n pr.assert_quaternion_equal(q, q2[0, 0])\n pr.assert_quaternion_equal(q, q2[0, 1])\n pr.assert_quaternion_equal(q, q2[1, 0])\n pr.assert_quaternion_equal(q, q2[1, 1])\n\n\ndef test_axis_angles_from_matrices_0dims():\n rng = np.random.default_rng(84)\n A = rng.standard_normal(size=3)\n A /= np.linalg.norm(A, axis=-1)[..., np.newaxis]\n A *= rng.random() * np.pi\n\n Rs = pbr.matrices_from_compact_axis_angles(A)\n A2 = pbr.axis_angles_from_matrices(Rs)\n A2_compact = A2[:3] * A2[3]\n assert_array_almost_equal(A, A2_compact)\n\n\ndef test_axis_angles_from_matrices():\n rng = np.random.default_rng(84)\n A = rng.standard_normal(size=(2, 3, 3))\n A /= np.linalg.norm(A, axis=-1)[..., np.newaxis]\n A *= rng.random(size=(2, 3, 1)) * np.pi\n A[0, 0, :] = 0.0\n\n Rs = pbr.matrices_from_compact_axis_angles(A)\n A2 = pbr.axis_angles_from_matrices(Rs)\n A2_compact = A2[..., :3] * A2[..., 3, np.newaxis]\n assert_array_almost_equal(A, A2_compact)\n\n\ndef test_axis_angles_from_matrices_output_variable():\n rng = np.random.default_rng(84)\n A = rng.standard_normal(size=(2, 3, 3))\n A /= np.linalg.norm(A, axis=-1)[..., np.newaxis]\n A *= rng.random(size=(2, 3, 1)) * np.pi\n A[0, 0, :] = 0.0\n\n Rs = np.empty((2, 3, 3, 3))\n pbr.matrices_from_compact_axis_angles(A, out=Rs)\n A2 = np.empty((2, 3, 4))\n pbr.axis_angles_from_matrices(Rs, out=A2)\n A2_compact = A2[..., :3] * A2[..., 3, np.newaxis]\n assert_array_almost_equal(A, A2_compact)\n\n\ndef test_axis_angles_from_matrices_norot():\n a = pbr.axis_angles_from_matrices(np.eye(3))\n assert_array_almost_equal(a, [1, 0, 0, 0])\n\n A = pbr.axis_angles_from_matrices([np.eye(3), np.eye(3)])\n assert_array_almost_equal(A, [[1, 0, 0, 0], [1, 0, 0, 0]])\n\n A = pbr.axis_angles_from_matrices(\n [[np.eye(3), np.eye(3)], [np.eye(3), np.eye(3)]]\n )\n assert_array_almost_equal(\n A, [[[1, 0, 0, 0], [1, 0, 0, 0]], [[1, 0, 0, 0], [1, 0, 0, 0]]]\n )\n\n\ndef test_axis_angles_from_quaternions():\n rng = np.random.default_rng(48322)\n n_rotations = 20\n Q = pbr.norm_vectors(rng.standard_normal(size=(n_rotations, 4)))\n\n # 1D\n pr.assert_quaternion_equal(\n pbr.axis_angles_from_quaternions(Q[0]),\n pr.axis_angle_from_quaternion(Q[0]),\n )\n\n # 2D\n A = pbr.axis_angles_from_quaternions(Q)\n for a, q in zip(A, Q):\n pr.assert_quaternion_equal(a, pr.axis_angle_from_quaternion(q))\n\n # 3D\n Q3D = Q.reshape(n_rotations // 4, 4, 4)\n A3D = pbr.axis_angles_from_quaternions(Q3D)\n for a, q in zip(A3D.reshape(n_rotations, 4), Q3D.reshape(n_rotations, 4)):\n pr.assert_quaternion_equal(a, pr.axis_angle_from_quaternion(q))\n\n\ndef test_quaternion_slerp_batch_zero_angle():\n rng = np.random.default_rng(228)\n q = pr.random_quaternion(rng)\n Q = pbr.quaternion_slerp_batch(q, q, [0.5])\n pr.assert_quaternion_equal(q, Q[0])\n\n\ndef test_quaternion_slerp_batch():\n rng = np.random.default_rng(229)\n q_start = pr.random_quaternion(rng)\n q_end = pr.random_quaternion(rng)\n t = np.linspace(0, 1, 101)\n Q = pbr.quaternion_slerp_batch(q_start, q_end, t)\n for i in range(len(t)):\n qi = pr.quaternion_slerp(q_start, q_end, t[i])\n pr.assert_quaternion_equal(Q[i], qi)\n\n\ndef test_quaternion_slerp_batch_sign_ambiguity():\n n_steps = 10\n rng = np.random.default_rng(2323)\n q1 = pr.random_quaternion(rng)\n a1 = pr.axis_angle_from_quaternion(q1)\n a2 = np.r_[a1[:3], a1[3] * 1.1]\n q2 = pr.quaternion_from_axis_angle(a2)\n\n if np.sign(q1[0]) != np.sign(q2[0]):\n q2 *= -1.0\n traj_q = pbr.quaternion_slerp_batch(\n q1, q2, np.linspace(0, 1, n_steps), shortest_path=True\n )\n path_length = np.sum(\n [pr.quaternion_dist(r, s) for r, s in zip(traj_q[:-1], traj_q[1:])]\n )\n\n q2 *= -1.0\n traj_q_opposing = pbr.quaternion_slerp_batch(\n q1, q2, np.linspace(0, 1, n_steps), shortest_path=False\n )\n path_length_opposing = np.sum(\n [\n pr.quaternion_dist(r, s)\n for r, s in zip(traj_q_opposing[:-1], traj_q_opposing[1:])\n ]\n )\n\n assert path_length_opposing > path_length\n\n traj_q_opposing_corrected = pbr.quaternion_slerp_batch(\n q1, q2, np.linspace(0, 1, n_steps), shortest_path=True\n )\n path_length_opposing_corrected = np.sum(\n [\n pr.quaternion_dist(r, s)\n for r, s in zip(\n traj_q_opposing_corrected[:-1], traj_q_opposing_corrected[1:]\n )\n ]\n )\n\n assert pytest.approx(path_length_opposing_corrected) == path_length\n\n\ndef test_batch_concatenate_quaternions_mismatch():\n Q1 = np.zeros((1, 2, 4))\n Q2 = np.zeros((1, 2, 3, 4))\n with pytest.raises(\n ValueError, match=\"Number of dimensions must be the same.\"\n ):\n pbr.batch_concatenate_quaternions(Q1, Q2)\n\n Q1 = np.zeros((1, 2, 4, 4))\n Q2 = np.zeros((1, 2, 3, 4))\n with pytest.raises(ValueError, match=\"Size of dimension 3 does not match\"):\n pbr.batch_concatenate_quaternions(Q1, Q2)\n\n Q1 = np.zeros((1, 2, 3, 3))\n Q2 = np.zeros((1, 2, 3, 4))\n with pytest.raises(\n ValueError, match=\"Last dimension of first argument does not \" \"match.\"\n ):\n pbr.batch_concatenate_quaternions(Q1, Q2)\n\n Q1 = np.zeros((1, 2, 3, 4))\n Q2 = np.zeros((1, 2, 3, 3))\n with pytest.raises(\n ValueError, match=\"Last dimension of second argument does \" \"not match.\"\n ):\n pbr.batch_concatenate_quaternions(Q1, Q2)\n\n\ndef test_batch_concatenate_quaternions_1d():\n rng = np.random.default_rng(230)\n q1 = pr.random_quaternion(rng)\n q2 = pr.random_quaternion(rng)\n q12 = np.empty(4)\n pbr.batch_concatenate_quaternions(q1, q2, out=q12)\n assert_array_almost_equal(q12, pr.concatenate_quaternions(q1, q2))\n\n\ndef test_batch_q_conj_1d():\n rng = np.random.default_rng(230)\n q = pr.random_quaternion(rng)\n assert_array_almost_equal(pr.q_conj(q), pbr.batch_q_conj(q))\n\n\ndef test_batch_concatenate_q_conj():\n rng = np.random.default_rng(231)\n Q = np.array([pr.random_quaternion(rng) for _ in range(10)])\n Q = Q.reshape(2, 5, 4)\n\n Q_conj = pbr.batch_q_conj(Q)\n Q_Q_conj = pbr.batch_concatenate_quaternions(Q, Q_conj)\n\n assert_array_almost_equal(\n Q_Q_conj.reshape(-1, 4), np.array([[1, 0, 0, 0]] * 10)\n )\n\n\ndef test_batch_convert_quaternion_conventions():\n q_wxyz = np.array([[1.0, 0.0, 0.0, 0.0], [1.0, 0.0, 0.0, 0.0]])\n q_xyzw = pbr.batch_quaternion_xyzw_from_wxyz(q_wxyz)\n assert_array_almost_equal(\n q_xyzw, np.array([[0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 1.0]])\n )\n pbr.batch_quaternion_xyzw_from_wxyz(q_wxyz, out=q_xyzw)\n assert_array_almost_equal(\n q_xyzw, np.array([[0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 1.0]])\n )\n q_wxyz2 = pbr.batch_quaternion_wxyz_from_xyzw(q_xyzw)\n assert_array_almost_equal(q_wxyz, q_wxyz2)\n pbr.batch_quaternion_wxyz_from_xyzw(q_xyzw, out=q_wxyz2)\n assert_array_almost_equal(q_wxyz, q_wxyz2)\n\n rng = np.random.default_rng(42)\n q_wxyz_random = pr.random_quaternion(rng)\n q_xyzw_random = pbr.batch_quaternion_xyzw_from_wxyz(q_wxyz_random)\n assert_array_almost_equal(q_xyzw_random[:3], q_wxyz_random[1:])\n assert q_xyzw_random[3] == q_wxyz_random[0]\n q_wxyz_random2 = pbr.batch_quaternion_wxyz_from_xyzw(q_xyzw_random)\n assert_array_almost_equal(q_wxyz_random, q_wxyz_random2)\n\n\ndef test_smooth_quaternion_trajectory():\n rng = np.random.default_rng(232)\n q_start = pr.random_quaternion(rng)\n if q_start[1] < 0.0:\n q_start *= -1.0\n q_goal = pr.random_quaternion(rng)\n n_steps = 101\n Q = np.empty((n_steps, 4))\n for i, t in enumerate(np.linspace(0, 1, n_steps)):\n Q[i] = pr.quaternion_slerp(q_start, q_goal, t)\n Q_broken = Q.copy()\n Q_broken[20:23, :] *= -1.0\n Q_broken[80:, :] *= -1.0\n Q_smooth = pbr.smooth_quaternion_trajectory(Q_broken)\n assert_array_almost_equal(Q_smooth, Q)\n\n\ndef test_smooth_quaternion_trajectory_start_component_negative():\n rng = np.random.default_rng(232)\n\n for index in range(4):\n component = \"wxyz\"[index]\n q = pr.random_quaternion(rng)\n if q[index] > 0.0:\n q *= -1.0\n q_corrected = pbr.smooth_quaternion_trajectory(\n [q], start_component_positive=component\n )[0]\n assert q_corrected[index] > 0.0\n\n\ndef test_smooth_quaternion_trajectory_empty():\n with pytest.raises(\n ValueError, match=r\"At least one quaternion is expected\"\n ):\n pbr.smooth_quaternion_trajectory(np.zeros((0, 4)))\n" }, { "path": "pytransform3d/camera.py", "content": "\"\"\"Transformations related to cameras.\n\nSee :doc:`user_guide/camera` for more information.\n\"\"\"\n\nimport numpy as np\n\nfrom .transformations import invert_transform, transform, check_transform\n\n\ndef make_world_grid(\n n_lines=11, n_points_per_line=51, xlim=(-0.5, 0.5), ylim=(-0.5, 0.5)\n):\n \"\"\"Generate grid in world coordinate frame.\n\n The grid will have the form\n\n .. code::\n\n +----+----+----+----+----+\n | | | | | |\n +----+----+----+----+----+\n | | | | | |\n +----+----+----+----+----+\n | | | | | |\n +----+----+----+----+----+\n | | | | | |\n +----+----+----+----+----+\n | | | | | |\n +----+----+----+----+----+\n\n on the x-y plane with z=0 for all points.\n\n Parameters\n ----------\n n_lines : int, optional (default: 11)\n Number of lines\n\n n_points_per_line : int, optional (default: 51)\n Number of points per line\n\n xlim : tuple, optional (default: (-0.5, 0.5))\n Range on x-axis\n\n ylim : tuple, optional (default: (-0.5, 0.5))\n Range on y-axis\n\n Returns\n -------\n world_grid : array-like, shape (2 * n_lines * n_points_per_line, 4)\n Grid as homogenous coordinate vectors\n \"\"\"\n world_grid_x = np.vstack(\n [\n make_world_line([xlim[0], y], [xlim[1], y], n_points_per_line)\n for y in np.linspace(ylim[0], ylim[1], n_lines)\n ]\n )\n world_grid_y = np.vstack(\n [\n make_world_line([x, ylim[0]], [x, ylim[1]], n_points_per_line)\n for x in np.linspace(xlim[0], xlim[1], n_lines)\n ]\n )\n return np.vstack((world_grid_x, world_grid_y))\n\n\ndef make_world_line(p1, p2, n_points):\n \"\"\"Generate line in world coordinate frame.\n\n Parameters\n ----------\n p1 : array-like, shape (2 or 3,)\n Start point of the line\n\n p2 : array-like, shape (2 or 3,)\n End point of the line\n\n n_points : int\n Number of points\n\n Returns\n -------\n line : array-like, shape (n_points, 4)\n Samples from line in world frame\n \"\"\"\n if len(p1) == 2:\n p1 = [p1[0], p1[1], 0]\n if len(p2) == 2:\n p2 = [p2[0], p2[1], 0]\n return np.array(\n [\n np.linspace(p1[0], p2[0], n_points),\n np.linspace(p1[1], p2[1], n_points),\n np.linspace(p1[2], p2[2], n_points),\n np.ones(n_points),\n ]\n ).T\n\n\ndef cam2sensor(P_cam, focal_length, kappa=0.0):\n \"\"\"Project points from 3D camera coordinate system to sensor plane.\n\n Parameters\n ----------\n P_cam : array-like, shape (n_points, 3 or 4)\n Points in camera coordinates\n\n focal_length : float\n Focal length of the camera\n\n kappa : float, optional (default: 0)\n Camera distortion parameter\n\n Returns\n -------\n P_sensor : array-like, shape (n_points, 2)\n Points on the sensor plane. The result for points that are behind the\n camera will be a vector of nans.\n\n Raises\n ------\n ValueError\n If input is not valid\n \"\"\"\n n_points, n_dims = P_cam.shape\n if n_dims not in (3, 4):\n raise ValueError(\n \"Expected 3- or 4-dimensional points, got %d \" \"dimensions\" % n_dims\n )\n if focal_length <= 0.0:\n raise ValueError(\"Focal length must be greater than 0.\")\n\n P_sensor = np.empty((n_points, 2))\n ahead = P_cam[:, 2] > 0.0\n P_sensor[ahead] = P_cam[ahead][:, :2] / P_cam[ahead][:, 2, np.newaxis]\n behind = np.logical_not(ahead)\n\n for n in range(P_sensor.shape[0]):\n P_sensor[n] *= 1.0 / (1.0 + kappa * np.linalg.norm(P_sensor[n]) ** 2)\n P_sensor *= focal_length\n P_sensor[behind] = np.nan\n return P_sensor\n\n\ndef sensor2img(P_sensor, sensor_size, image_size, image_center=None):\n \"\"\"Project points from 2D sensor plane to image coordinate system.\n\n Parameters\n ----------\n P_sensor : array-like, shape (n_points, 2)\n Points on camera sensor\n\n sensor_size : array-like, shape (2,)\n Size of the sensor array\n\n image_size : array-like, shape (2,)\n Size of the camera image: (width, height)\n\n image_center : array-like, shape (2,), optional (default: image_size / 2)\n Center of the image\n\n Returns\n -------\n P_img : array-like, shape (n_points, 2)\n Points on image\n \"\"\"\n P_img = np.asarray(image_size) * P_sensor / np.asarray(sensor_size)\n if image_center is None:\n image_center = np.asarray(image_size) / 2\n P_img += np.asarray(image_center)\n return P_img\n\n\ndef world2image(\n P_world,\n cam2world,\n sensor_size,\n image_size,\n focal_length,\n image_center=None,\n kappa=0.0,\n):\n \"\"\"Project points from 3D world coordinate system to 2D image.\n\n Parameters\n ----------\n P_world : array-like, shape (n_points, 4)\n Points in world coordinates\n\n cam2world : array-like, shape (4, 4), optional (default: I)\n Camera in world frame\n\n sensor_size : array-like, shape (2,)\n Size of the sensor array\n\n image_size : array-like, shape (2,)\n Size of the camera image: (width, height)\n\n focal_length : float\n Focal length of the camera\n\n image_center : array-like, shape (2,), optional (default: image_size / 2)\n Center of the image\n\n kappa : float, optional (default: 0)\n Camera distortion parameter\n\n Returns\n -------\n P_img : array-like, shape (n_points, 2)\n Points on image\n \"\"\"\n world2cam = invert_transform(cam2world)\n P_cam = transform(world2cam, P_world)\n P_sensor = cam2sensor(P_cam, focal_length, kappa)\n P_img = sensor2img(P_sensor, sensor_size, image_size, image_center)\n return P_img\n\n\ndef plot_camera(\n ax=None,\n M=None,\n cam2world=None,\n virtual_image_distance=1.0,\n sensor_size=(1920, 1080),\n ax_s=1,\n strict_check=True,\n **kwargs,\n): # pragma: no cover\n \"\"\"Plot camera in world coordinates.\n\n This function is inspired by Blender's camera visualization. It will\n show the camera center, a virtual image plane, and the top of the virtual\n image plane.\n\n Parameters\n ----------\n ax : Matplotlib 3d axis, optional (default: None)\n If the axis is None, a new 3d axis will be created.\n\n M : array-like, shape (3, 3)\n Intrinsic camera matrix that contains the focal lengths on the diagonal\n and the center of the the image in the last column. It does not matter\n whether values are given in meters or pixels as long as the unit is the\n same as for the sensor size.\n\n cam2world : array-like, shape (4, 4), optional (default: I)\n Transformation matrix of camera in world frame. We assume that the\n position is given in meters.\n\n virtual_image_distance : float, optional (default: 1)\n Distance from pinhole to virtual image plane that will be displayed.\n We assume that this distance is given in meters. The unit has to be\n consistent with the unit of the position in cam2world.\n\n sensor_size : array-like, shape (2,), optional (default: [1920, 1080])\n Size of the image sensor: (width, height). It does not matter whether\n values are given in meters or pixels as long as the unit is the same as\n for the sensor size.\n\n ax_s : float, optional (default: 1)\n Scaling of the new matplotlib 3d axis.\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the transformation matrix is not numerically\n close enough to a real transformation matrix. Otherwise we print a\n warning.\n\n kwargs : dict, optional (default: {})\n Additional arguments for the plotting functions, e.g. alpha.\n\n Returns\n -------\n ax : Matplotlib 3d axis\n New or old axis\n\n Raises\n ------\n ValueError\n If input is not valid\n \"\"\"\n from .plot_utils import Camera\n\n if ax is None:\n from .plot_utils import make_3d_axis\n\n ax = make_3d_axis(ax_s)\n\n if M is None:\n raise ValueError(\"No intrinsic camera matrix given.\")\n\n if cam2world is None:\n cam2world = np.eye(4)\n\n cam2world = check_transform(cam2world, strict_check=strict_check)\n\n camera = Camera(M, cam2world, virtual_image_distance, sensor_size, **kwargs)\n camera.add_camera(ax)\n\n return ax\n" }, { "path": "pytransform3d/camera.pyi", "content": "from typing import Union\n\nimport numpy as np\nimport numpy.typing as npt\nfrom mpl_toolkits.mplot3d import Axes3D\n\ndef make_world_grid(\n n_lines: int = ...,\n n_points_per_line: int = ...,\n xlim: npt.ArrayLike = ...,\n ylim: npt.ArrayLike = ...,\n) -> np.ndarray: ...\ndef make_world_line(\n p1: npt.ArrayLike, p2: npt.ArrayLike, n_points: int\n) -> np.ndarray: ...\ndef cam2sensor(\n P_cam: npt.ArrayLike, focal_length: float, kappa: float = ...\n) -> np.ndarray: ...\ndef sensor2img(\n P_sensor: npt.ArrayLike,\n sensor_size: npt.ArrayLike,\n image_size: npt.ArrayLike,\n image_center: Union[npt.ArrayLike, None] = ...,\n) -> np.ndarray: ...\ndef world2image(\n P_world: npt.ArrayLike,\n cam2world: npt.ArrayLike,\n sensor_size: npt.ArrayLike,\n image_size: npt.ArrayLike,\n focal_length: float,\n image_center: Union[npt.ArrayLike, None] = ...,\n kappa: float = ...,\n) -> np.ndarray: ...\ndef plot_camera(\n ax: Union[None, Axes3D] = ...,\n M: Union[None, npt.ArrayLike] = ...,\n cam2world: Union[None, npt.ArrayLike] = ...,\n virtual_image_distance: float = ...,\n sensor_size: npt.ArrayLike = ...,\n ax_s: float = ...,\n strict_check: bool = ...,\n **kwargs,\n) -> Axes3D: ...\n" }, { "path": "pytransform3d/coordinates.py", "content": "\"\"\"Conversions between coordinate systems to represent positions.\"\"\"\n\nimport numpy as np\n\n\ndef cartesian_from_cylindrical(p):\n \"\"\"Convert cylindrical coordinates to Cartesian coordinates.\n\n Parameters\n ----------\n p : array-like, shape (..., 3)\n Cylindrical coordinates: axial / radial distance (rho), azimuth\n (phi), and axial coordinate / height (z)\n\n Returns\n -------\n q : array, shape (..., 3)\n Cartesian coordinates (x, y, z)\n \"\"\"\n p = np.asarray(p, dtype=float)\n q = np.empty_like(p)\n q[..., 0] = p[..., 0] * np.cos(p[..., 1])\n q[..., 1] = p[..., 0] * np.sin(p[..., 1])\n q[..., 2] = p[..., 2]\n return q\n\n\ndef cartesian_from_spherical(p):\n \"\"\"Convert spherical coordinates to Cartesian coordinates.\n\n Parameters\n ----------\n p : array-like, shape (..., 3)\n Spherical coordinates: radial distance (rho), inclination /\n elevation (theta), and azimuth (phi)\n\n Returns\n -------\n q : array, shape (..., 3)\n Cartesian coordinates (x, y, z)\n \"\"\"\n p = np.asarray(p, dtype=float)\n q = np.empty_like(p)\n r_sin_theta = p[..., 0] * np.sin(p[..., 1])\n q[..., 0] = np.cos(p[..., 2]) * r_sin_theta\n q[..., 1] = np.sin(p[..., 2]) * r_sin_theta\n q[..., 2] = p[..., 0] * np.cos(p[..., 1])\n return q\n\n\ndef cylindrical_from_cartesian(p):\n \"\"\"Convert Cartesian coordinates to cylindrical coordinates.\n\n Parameters\n ----------\n p : array-like, shape (..., 3)\n Cartesian coordinates (x, y, z)\n\n Returns\n -------\n q : array, shape (..., 3)\n Cylindrical coordinates: axial / radial distance (rho >= 0), azimuth\n (-pi >= phi >= pi), and axial coordinate / height (z)\n \"\"\"\n p = np.asarray(p, dtype=float)\n q = np.empty_like(p)\n q[..., 0] = np.linalg.norm(p[..., :2], axis=-1)\n q[..., 1] = np.arctan2(p[..., 1], p[..., 0])\n q[..., 2] = p[..., 2]\n return q\n\n\ndef cylindrical_from_spherical(p):\n \"\"\"Convert spherical coordinates to cylindrical coordinates.\n\n Parameters\n ----------\n p : array-like, shape (..., 3)\n Spherical coordinates: radial distance (rho), inclination /\n elevation (theta), and azimuth (phi)\n\n Returns\n -------\n q : array, shape (..., 3)\n Cylindrical coordinates: axial / radial distance (rho), azimuth\n (phi), and axial coordinate / height (z)\n \"\"\"\n p = np.asarray(p, dtype=float)\n q = np.empty_like(p)\n q[..., 0] = p[..., 0] * np.sin(p[..., 1])\n q[..., 1] = p[..., 2]\n q[..., 2] = p[..., 0] * np.cos(p[..., 1])\n return q\n\n\ndef spherical_from_cartesian(p):\n \"\"\"Convert Cartesian coordinates to spherical coordinates.\n\n Parameters\n ----------\n p : array-like, shape (..., 3)\n Cartesian coordinates (x, y, z)\n\n Returns\n -------\n q : array, shape (..., 3)\n Spherical coordinates: radial distance (rho >= 0), inclination /\n elevation (0 <= theta <= pi), and azimuth (-pi <= phi <= pi)\n \"\"\"\n p = np.asarray(p, dtype=float)\n q = np.empty_like(p)\n q[..., 0] = np.linalg.norm(p, axis=-1)\n q[..., 1] = np.arctan2(np.linalg.norm(p[..., :2], axis=-1), p[..., 2])\n q[..., 2] = np.arctan2(p[..., 1], p[..., 0])\n return q\n\n\ndef spherical_from_cylindrical(p):\n \"\"\"Convert cylindrical coordinates to spherical coordinates.\n\n Parameters\n ----------\n p : array-like, shape (..., 3)\n Cylindrical coordinates: axial / radial distance (rho), azimuth\n (phi), and axial coordinate / height (z)\n\n Returns\n -------\n q : array, shape (..., 3)\n Spherical coordinates: radial distance (rho), inclination /\n elevation (theta), and azimuth (phi)\n \"\"\"\n p = np.asarray(p, dtype=float)\n q = np.empty_like(p)\n q[..., 0] = np.linalg.norm(p[..., (0, 2)], axis=-1)\n q[..., 1] = np.arctan2(p[..., 0], p[..., 2])\n q[..., 2] = p[..., 1]\n return q\n" }, { "path": "pytransform3d/coordinates.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef cartesian_from_cylindrical(p: npt.ArrayLike) -> np.ndarray: ...\ndef cartesian_from_spherical(p: npt.ArrayLike) -> np.ndarray: ...\ndef cylindrical_from_cartesian(p: npt.ArrayLike) -> np.ndarray: ...\ndef cylindrical_from_spherical(p: npt.ArrayLike) -> np.ndarray: ...\ndef spherical_from_cartesian(p: npt.ArrayLike) -> np.ndarray: ...\ndef spherical_from_cylindrical(p: npt.ArrayLike) -> np.ndarray: ...\n" }, { "path": "pytransform3d/editor.py", "content": "\"\"\"Modify transformations visually.\"\"\"\n\nqt_available = False\nqt_version = None\ntry:\n import PyQt5.QtCore as QtCore\n from PyQt5.QtWidgets import (\n QApplication,\n QMainWindow,\n QWidget,\n QSlider,\n QDoubleSpinBox,\n QGridLayout,\n QLabel,\n QGroupBox,\n QHBoxLayout,\n QComboBox,\n QVBoxLayout,\n )\n\n qt_available = True\n qt_version = 5\nexcept ImportError:\n try:\n import PyQt4.QtCore as QtCore\n from PyQt4.QtGui import (\n QApplication,\n QMainWindow,\n QWidget,\n QSlider,\n QDoubleSpinBox,\n QGridLayout,\n QLabel,\n QGroupBox,\n QHBoxLayout,\n QComboBox,\n QVBoxLayout,\n )\n\n qt_available = True\n qt_version = 4\n except ImportError:\n import warnings\n\n warnings.warn(\n \"Cannot import PyQt. TransformEditor won't be available.\",\n ImportWarning,\n stacklevel=2,\n )\n TransformEditor = None\n\n\nif qt_available:\n import sys\n from functools import partial\n from contextlib import contextmanager\n import numpy as np\n from .transform_manager import TransformManager\n from .rotations import (\n active_matrix_from_intrinsic_euler_xyz,\n intrinsic_euler_xyz_from_active_matrix,\n )\n from .transformations import transform_from\n from mpl_toolkits.mplot3d import Axes3D # noqa: F401\n from matplotlib.figure import Figure\n\n if qt_version == 5:\n from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg\n from matplotlib.backends.backend_qt5agg import NavigationToolbar2QT\n else:\n assert qt_version == 4\n from matplotlib.backends.backend_qt4agg import FigureCanvasQTAgg\n from matplotlib.backends.backend_qt4agg import NavigationToolbar2QT\n\n @contextmanager\n def _block_signals(qobject):\n \"\"\"Block signals of a QObject in this context.\"\"\"\n signals_blocked = qobject.blockSignals(True)\n try:\n yield qobject\n finally:\n qobject.blockSignals(signals_blocked)\n\n def _internal_repr(A2B):\n \"\"\"Compute internal representation of transform.\"\"\"\n p = A2B[:3, 3]\n R = A2B[:3, :3]\n e = intrinsic_euler_xyz_from_active_matrix(R)\n return np.hstack((p, e))\n\n class PositionEulerEditor(QWidget):\n \"\"\"Frame editor that represents orientation by Euler angles (XY'Z'').\n\n Parameters\n ----------\n base_frame : string\n Name of the base frame\n\n xlim : tuple\n Lower and upper limit for the x position. Defines the range of the\n plot and the range of the slider.\n\n ylim : tuple\n Lower and upper limit for the y position. Defines the range of the\n plot and the range of the slider.\n\n zlim : tuple\n Lower and upper limit for the z position. Defines the range of the\n plot and the range of the slider.\n\n parent : QWidget, optional (default: None)\n Parent widget.\n \"\"\"\n\n frameChanged = QtCore.pyqtSignal()\n\n def __init__(self, base_frame, xlim, ylim, zlim, parent=None):\n super(PositionEulerEditor, self).__init__(parent)\n\n self.dim_labels = [\"x\", \"y\", \"z\", \"X\", \"Y'\", \"Z''\"]\n self.limits = [\n xlim,\n ylim,\n zlim,\n (-3.141, 3.141),\n (-1.570, 1.570),\n (-3.141, 3.141),\n ]\n self.n_slider_steps = [\n int(100 * (upper - lower)) + 1 for lower, upper in self.limits\n ]\n self.setLayout(self._create(base_frame))\n self.A2B = None\n\n def _create(self, base_frame):\n self.sliders = []\n self.spinboxes = []\n for i in range(len(self.dim_labels)):\n self.sliders.append(QSlider(QtCore.Qt.Horizontal))\n self.sliders[i].setRange(0, self.n_slider_steps[i])\n self.sliders[i].valueChanged.connect(partial(self._on_slide, i))\n spinbox = QDoubleSpinBox()\n spinbox.setRange(*self.limits[i])\n spinbox.setDecimals(3)\n spinbox.setSingleStep(0.001)\n self.spinboxes.append(spinbox)\n self.spinboxes[i].valueChanged.connect(\n partial(self._on_pos_edited, i)\n )\n slider_group = QGridLayout()\n slider_group.addWidget(\n QLabel(\"Position\"), 0, 0, 1, 3, QtCore.Qt.AlignCenter\n )\n slider_group.addWidget(\n QLabel(\"Orientation (Euler angles)\"),\n 0,\n 3,\n 1,\n 3,\n QtCore.Qt.AlignCenter,\n )\n for i, slider in enumerate(self.sliders):\n slider_group.addWidget(QLabel(self.dim_labels[i]), 1, i)\n slider_group.addWidget(slider, 2, i)\n slider_group.addWidget(self.spinboxes[i], 3, i)\n slider_groupbox = QGroupBox(\n \"Transformation in frame '%s'\" % base_frame\n )\n slider_groupbox.setLayout(slider_group)\n layout = QHBoxLayout()\n layout.addWidget(slider_groupbox)\n layout.addStretch(1)\n return layout\n\n def set_frame(self, A2B):\n \"\"\"Set pose of frame.\n\n Parameters\n ----------\n A2B : array\n Transformation matrix\n \"\"\"\n self.A2B = A2B\n pose = _internal_repr(self.A2B)\n\n for i in range(6):\n pos = self._pos_to_slider_pos(i, pose[i])\n with _block_signals(self.sliders[i]) as slider:\n slider.setValue(pos)\n with _block_signals(self.spinboxes[i]) as spinbox:\n spinbox.setValue(pose[i])\n\n def _on_pos_edited(self, dim, pos):\n \"\"\"Slot: value in spinbox changed.\"\"\"\n pose = _internal_repr(self.A2B)\n pose[dim] = pos\n self.A2B = transform_from(\n active_matrix_from_intrinsic_euler_xyz(pose[3:]), pose[:3]\n )\n\n for i in range(6):\n pos = self._pos_to_slider_pos(i, pose[i])\n with _block_signals(self.sliders[i]) as slider:\n slider.setValue(pos)\n\n self.frameChanged.emit()\n\n def _on_slide(self, dim, step):\n \"\"\"Slot: slider position changed.\"\"\"\n pose = _internal_repr(self.A2B)\n v = self._slider_pos_to_pos(dim, step)\n pose[dim] = v\n self.A2B = transform_from(\n active_matrix_from_intrinsic_euler_xyz(pose[3:]), pose[:3]\n )\n\n self.spinboxes[dim].setValue(v)\n\n self.frameChanged.emit()\n\n def _pos_to_slider_pos(self, dim, pos):\n \"\"\"Compute slider position from value.\"\"\"\n m = self.limits[dim][0]\n r = self.limits[dim][1] - m\n slider_pos = int((pos - m) / r * self.n_slider_steps[dim])\n slider_pos = np.clip(slider_pos, 0, self.n_slider_steps[dim])\n return slider_pos\n\n def _slider_pos_to_pos(self, dim, slider_pos):\n \"\"\"Create value from slider position.\"\"\"\n m = self.limits[dim][0]\n r = self.limits[dim][1] - m\n return m + r * float(slider_pos) / float(self.n_slider_steps[dim])\n\n class TransformEditor(QMainWindow):\n \"\"\"GUI to edit transformations.\n\n .. warning::\n\n Note that this module requires PyQt4.\n\n Parameters\n ----------\n transform_manager : TransformManager\n All nodes that are reachable from the base frame will be editable\n\n frame : string\n Name of the base frame\n\n xlim : tuple, optional (-1, 1)\n Lower and upper limit for the x position. Defines the range of the\n plot and the range of the slider.\n\n ylim : tuple, optional (-1, 1)\n Lower and upper limit for the y position. Defines the range of the\n plot and the range of the slider.\n\n zlim : tuple, optional (-1, 1)\n Lower and upper limit for the z position. Defines the range of the\n plot and the range of the slider.\n\n s : float, optional (default: 1)\n Scaling of the axis and angle that will be drawn\n\n figsize : tuple of integers, optional (default: (10, 10))\n Width, height in inches.\n\n dpi : integer, optional (default: 100)\n Resolution of the figure.\n\n parent : QWidget, optional (default: None)\n Parent widget.\n\n Attributes\n ----------\n transform_manager : TransformManager\n Result, all frames are expressed in the base frame\n \"\"\"\n\n def __init__(\n self,\n transform_manager,\n base_frame,\n xlim=(-1.0, 1.0),\n ylim=(-1.0, 1.0),\n zlim=(-1.0, 1.0),\n s=1.0,\n figsize=(10, 10),\n dpi=100,\n window_size=(500, 600),\n parent=None,\n ):\n self.app = QApplication(sys.argv)\n\n super(TransformEditor, self).__init__(parent)\n self.transform_manager = self._init_transform_manager(\n transform_manager, base_frame\n )\n self.base_frame = base_frame\n self.xlim = xlim\n self.ylim = ylim\n self.zlim = zlim\n self.s = s\n self.figsize = figsize\n self.dpi = dpi\n self.window_size = window_size\n\n self.setWindowTitle(\"Transformation Editor\")\n self.canvas = None\n self.fig = None\n self.axis = None\n\n self._create_main_frame()\n self._on_node_changed(0)\n\n def _init_transform_manager(self, transform_manager, frame):\n \"\"\"Transform all nodes into the reference frame.\"\"\"\n tm = TransformManager()\n if frame not in transform_manager.nodes:\n raise KeyError(\"Unknown frame '%s'\" % frame)\n\n for node in transform_manager.nodes:\n try:\n node2frame = transform_manager.get_transform(node, frame)\n tm.add_transform(node, frame, node2frame)\n except KeyError:\n pass # Frame is not connected to the reference frame\n\n return tm\n\n def _create_main_frame(self):\n \"\"\"Create main frame and layout.\"\"\"\n self.main_frame = QWidget()\n\n self.frame_editor = PositionEulerEditor(\n self.base_frame, self.xlim, self.ylim, self.zlim\n )\n self.frame_editor.frameChanged.connect(self._on_update)\n\n self.frame_selection = self._create_frame_selector()\n\n plot = self._create_plot()\n\n vbox = QVBoxLayout()\n vbox.addWidget(self.frame_editor)\n vbox.addWidget(self.frame_selection)\n vbox.addWidget(plot)\n\n main_layout = QHBoxLayout()\n main_layout.addLayout(vbox)\n\n self.main_frame.setLayout(main_layout)\n self.setCentralWidget(self.main_frame)\n self.setGeometry(0, 0, *self.window_size)\n\n def _create_frame_selector(self):\n frame_selection = QComboBox()\n for node in self.transform_manager.nodes:\n if node != self.base_frame:\n frame_selection.addItem(node)\n frame_selection.activated.connect(self._on_node_changed)\n return frame_selection\n\n def _create_plot(self):\n plot = QWidget()\n canvas_group = QGridLayout()\n self.fig = Figure(self.figsize, dpi=self.dpi)\n self.fig.subplots_adjust(left=0, right=1, bottom=0, top=1)\n self.canvas = FigureCanvasQTAgg(self.fig)\n self.canvas.setParent(self.main_frame)\n canvas_group.addWidget(self.canvas, 1, 0)\n mpl_toolbar = NavigationToolbar2QT(self.canvas, self.main_frame)\n canvas_group.addWidget(mpl_toolbar, 2, 0)\n plot.setLayout(canvas_group)\n return plot\n\n def _on_node_changed(self, node_idx):\n \"\"\"Slot: manipulatable node changed.\"\"\"\n self.node = self.frame_selection.itemText(node_idx)\n A2B = self.transform_manager.get_transform(\n self.node, self.base_frame\n )\n self.frame_editor.set_frame(A2B)\n self._plot()\n\n def _on_update(self):\n \"\"\"Slot: transformation changed.\"\"\"\n self.transform_manager.add_transform(\n self.node, self.base_frame, self.frame_editor.A2B\n )\n self._plot()\n\n def _plot(self):\n \"\"\"Draw plot.\"\"\"\n if self.axis is None:\n elev, azim = 30, 60\n else:\n elev, azim = self.axis.elev, self.axis.azim\n self.fig.delaxes(self.axis)\n\n self.axis = self.fig.add_subplot(111, projection=\"3d\")\n self.axis.view_init(elev, azim)\n\n self.axis.set_xlim(self.xlim)\n self.axis.set_ylim(self.ylim)\n self.axis.set_zlim(self.zlim)\n\n p = self.transform_manager.get_transform(\n self.node, self.base_frame\n )[:3, 3]\n self.axis.scatter(p[0], p[1], p[2], s=100)\n self.transform_manager.plot_frames_in(\n self.base_frame, ax=self.axis, s=self.s\n )\n\n self.canvas.draw()\n\n def show(self):\n \"\"\"Start GUI.\"\"\"\n super(TransformEditor, self).show()\n self.app.exec_()\n" }, { "path": "pytransform3d/plot_utils/__init__.py", "content": "\"\"\"Utilities for plotting.\"\"\"\n\nimport warnings\n\ntry:\n import matplotlib.pyplot as plt # noqa: F401\n from ._artists import (\n Arrow3D,\n Frame,\n LabeledFrame,\n Trajectory,\n Camera,\n )\n from ._layout import (\n make_3d_axis,\n remove_frame,\n )\n from ._plot_geometries import (\n plot_box,\n plot_sphere,\n plot_spheres,\n plot_cylinder,\n plot_mesh,\n plot_ellipsoid,\n plot_capsule,\n plot_cone,\n )\n from ._plot_helpers import (\n plot_vector,\n plot_length_variable,\n )\n\n __all__ = [\n \"Arrow3D\",\n \"Frame\",\n \"LabeledFrame\",\n \"Trajectory\",\n \"Camera\",\n \"make_3d_axis\",\n \"remove_frame\",\n \"plot_box\",\n \"plot_sphere\",\n \"plot_spheres\",\n \"plot_cylinder\",\n \"plot_mesh\",\n \"plot_ellipsoid\",\n \"plot_capsule\",\n \"plot_cone\",\n \"plot_vector\",\n \"plot_length_variable\",\n ]\nexcept ImportError as e:\n if e.name == \"matplotlib\":\n warnings.warn(\n \"Matplotlib is not installed, visualization is not \" \"available\",\n ImportWarning,\n stacklevel=2,\n )\n else:\n raise e\n" }, { "path": "pytransform3d/plot_utils/_artists.py", "content": "\"\"\"Matplotlib artists.\"\"\"\n\nimport numpy as np\nfrom matplotlib import artist\nfrom matplotlib.patches import FancyArrowPatch\nfrom mpl_toolkits.mplot3d import proj3d\nfrom mpl_toolkits.mplot3d.art3d import Line3D, Text3D\n\n\nclass Frame(artist.Artist):\n \"\"\"A Matplotlib artist that displays a frame represented by its basis.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Transform from frame A to frame B\n\n label : str, optional (default: None)\n Name of the frame\n\n s : float, optional (default: 1)\n Length of basis vectors\n\n draw_label_indicator : bool, optional (default: True)\n Controls whether the line from the frame origin to frame label is\n drawn.\n\n Other arguments except 'c' and 'color' are passed on to Line3D.\n \"\"\"\n\n def __init__(self, A2B, label=None, s=1.0, **kwargs):\n super(Frame, self).__init__()\n\n if \"c\" in kwargs:\n kwargs.pop(\"c\")\n if \"color\" in kwargs:\n kwargs.pop(\"color\")\n\n self.draw_label_indicator = kwargs.pop(\"draw_label_indicator\", True)\n\n self.s = s\n\n self.x_axis = Line3D([], [], [], color=\"r\", **kwargs)\n self.y_axis = Line3D([], [], [], color=\"g\", **kwargs)\n self.z_axis = Line3D([], [], [], color=\"b\", **kwargs)\n\n self.draw_label = label is not None\n self.label = label\n\n if self.draw_label:\n if self.draw_label_indicator:\n self.label_indicator = Line3D([], [], [], color=\"k\", **kwargs)\n self.label_text = Text3D(0, 0, 0, text=\"\", zdir=\"x\")\n\n self.set_data(A2B, label)\n\n def set_data(self, A2B, label=None):\n \"\"\"Set the transformation data.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Transform from frame A to frame B\n\n label : str, optional (default: None)\n Name of the frame\n \"\"\"\n R = A2B[:3, :3]\n p = A2B[:3, 3]\n\n for d, b in enumerate([self.x_axis, self.y_axis, self.z_axis]):\n b.set_data(\n np.array([p[0], p[0] + self.s * R[0, d]]),\n np.array([p[1], p[1] + self.s * R[1, d]]),\n )\n b.set_3d_properties(np.array([p[2], p[2] + self.s * R[2, d]]))\n\n if self.draw_label:\n if label is None:\n label = self.label\n label_pos = p + 0.5 * self.s * (R[:, 0] + R[:, 1] + R[:, 2])\n\n if self.draw_label_indicator:\n self.label_indicator.set_data(\n np.array([p[0], label_pos[0]]),\n np.array([p[1], label_pos[1]]),\n )\n self.label_indicator.set_3d_properties(\n np.array([p[2], label_pos[2]])\n )\n\n self.label_text.set_text(label)\n self.label_text.set_position([label_pos[0], label_pos[1]])\n self.label_text.set_3d_properties(label_pos[2], zdir=\"x\")\n\n @artist.allow_rasterization\n def draw(self, renderer, *args, **kwargs):\n \"\"\"Draw the artist.\"\"\"\n for b in [self.x_axis, self.y_axis, self.z_axis]:\n b.draw(renderer, *args, **kwargs)\n if self.draw_label:\n if self.draw_label_indicator:\n self.label_indicator.draw(renderer, *args, **kwargs)\n self.label_text.draw(renderer, *args, **kwargs)\n super(Frame, self).draw(renderer, *args, **kwargs)\n\n def add_frame(self, axis):\n \"\"\"Add the frame to a 3D axis.\"\"\"\n for b in [self.x_axis, self.y_axis, self.z_axis]:\n axis.add_line(b)\n if self.draw_label:\n if self.draw_label_indicator:\n axis.add_line(self.label_indicator)\n axis._add_text(self.label_text)\n\n\nclass LabeledFrame(Frame):\n \"\"\"Displays a frame represented by its basis with axis labels.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Transform from frame A to frame B\n\n label : str, optional (default: None)\n Name of the frame\n\n s : float, optional (default: 1)\n Length of basis vectors\n\n draw_label_indicator : bool, optional (default: True)\n Controls whether the line from the frame origin to frame label is\n drawn.\n\n Other arguments except 'c' and 'color' are passed on to Line3D.\n \"\"\"\n\n def __init__(self, A2B, label=None, s=1.0, **kwargs):\n self.x_label = Text3D(0, 0, 0, text=\"\", zdir=\"x\")\n self.y_label = Text3D(0, 0, 0, text=\"\", zdir=\"x\")\n self.z_label = Text3D(0, 0, 0, text=\"\", zdir=\"x\")\n super(LabeledFrame, self).__init__(A2B, label=label, s=s, **kwargs)\n\n def set_data(self, A2B, label=None):\n \"\"\"Set the transformation data.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Transform from frame A to frame B\n\n label : str, optional (default: None)\n Name of the frame\n \"\"\"\n super(LabeledFrame, self).set_data(A2B, label)\n\n R = A2B[:3, :3]\n p = A2B[:3, 3]\n x_label_location = p + 1.1 * self.s * R[:, 0]\n y_label_location = p + 1.1 * self.s * R[:, 1]\n z_label_location = p + 1.1 * self.s * R[:, 2]\n\n self.x_label.set_text(\"x\")\n self.x_label.set_position(x_label_location[:2])\n self.x_label.set_3d_properties(x_label_location[2], zdir=\"x\")\n\n self.y_label.set_text(\"y\")\n self.y_label.set_position(y_label_location[:2])\n self.y_label.set_3d_properties(y_label_location[2], zdir=\"x\")\n\n self.z_label.set_text(\"z\")\n self.z_label.set_position(z_label_location[:2])\n self.z_label.set_3d_properties(z_label_location[2], zdir=\"x\")\n\n @artist.allow_rasterization\n def draw(self, renderer, *args, **kwargs):\n \"\"\"Draw the artist.\"\"\"\n self.x_label.draw(renderer, *args, **kwargs)\n self.y_label.draw(renderer, *args, **kwargs)\n self.z_label.draw(renderer, *args, **kwargs)\n super(LabeledFrame, self).draw(renderer, *args, **kwargs)\n\n def add_frame(self, axis):\n \"\"\"Add the frame to a 3D axis.\"\"\"\n super(LabeledFrame, self).add_frame(axis)\n axis._add_text(self.x_label)\n axis._add_text(self.y_label)\n axis._add_text(self.z_label)\n\n\nclass Trajectory(artist.Artist):\n \"\"\"A Matplotlib artist that displays a trajectory.\n\n Parameters\n ----------\n H : array-like, shape (n_steps, 4, 4)\n Sequence of poses represented by homogeneous matrices\n\n show_direction : bool, optional (default: True)\n Plot an arrow to indicate the direction of the trajectory\n\n n_frames : int, optional (default: 10)\n Number of frames that should be plotted to indicate the rotation\n\n s : float, optional (default: 1)\n Scaling of the frames that will be drawn\n\n label : str, optional (default: None)\n Label of the trajectory\n\n Other arguments are passed onto Line3D.\n \"\"\"\n\n def __init__(\n self, H, show_direction=True, n_frames=10, s=1.0, label=None, **kwargs\n ):\n super(Trajectory, self).__init__()\n\n self.show_direction = show_direction\n\n self.trajectory = Line3D([], [], [], label=label, **kwargs)\n self.key_frames = [\n Frame(np.eye(4), s=s, **kwargs) for _ in range(n_frames)\n ]\n\n if self.show_direction:\n self.direction_arrow = Arrow3D(\n [0, 0],\n [0, 0],\n [0, 0],\n mutation_scale=20,\n lw=1,\n arrowstyle=\"-|>\",\n color=\"k\",\n )\n\n self.set_data(H)\n\n def set_data(self, H):\n \"\"\"Set the trajectory data.\n\n Parameters\n ----------\n H : array-like, shape (n_steps, 4, 4)\n Sequence of poses represented by homogeneous matrices\n \"\"\"\n positions = H[:, :3, 3]\n self.trajectory.set_data(positions[:, 0], positions[:, 1])\n self.trajectory.set_3d_properties(positions[:, 2])\n\n key_frames_indices = np.linspace(\n 0, len(H) - 1, len(self.key_frames), dtype=np.int64\n )\n for i, key_frame_idx in enumerate(key_frames_indices):\n self.key_frames[i].set_data(H[key_frame_idx])\n\n if self.show_direction:\n start = 0.8 * positions[0] + 0.2 * positions[-1]\n end = 0.2 * positions[0] + 0.8 * positions[-1]\n self.direction_arrow.set_data(\n [start[0], end[0]], [start[1], end[1]], [start[2], end[2]]\n )\n\n @artist.allow_rasterization\n def draw(self, renderer, *args, **kwargs):\n \"\"\"Draw the artist.\"\"\"\n self.trajectory.draw(renderer, *args, **kwargs)\n for key_frame in self.key_frames:\n key_frame.draw(renderer, *args, **kwargs)\n if self.show_direction:\n self.direction_arrow.draw(renderer)\n super(Trajectory, self).draw(renderer, *args, **kwargs)\n\n def add_trajectory(self, axis):\n \"\"\"Add the trajectory to a 3D axis.\"\"\"\n axis.add_line(self.trajectory)\n for key_frame in self.key_frames:\n key_frame.add_frame(axis)\n if self.show_direction:\n axis.add_artist(self.direction_arrow)\n\n\nclass Arrow3D(FancyArrowPatch):\n \"\"\"A Matplotlib patch that represents an arrow in 3D.\n\n Source: http://stackoverflow.com/a/11156353/915743\n \"\"\"\n\n def __init__(self, xs, ys, zs, *args, **kwargs):\n super(Arrow3D, self).__init__((0, 0), (0, 0), *args, **kwargs)\n self._verts3d = xs, ys, zs\n\n def set_data(self, xs, ys, zs):\n \"\"\"Set the arrow data.\n\n Parameters\n ----------\n xs : iterable\n List of x positions\n\n ys : iterable\n List of y positions\n\n zs : iterable\n List of z positions\n \"\"\"\n self._verts3d = xs, ys, zs\n\n def draw(self, renderer):\n \"\"\"Draw the patch.\"\"\"\n xs3d, ys3d, zs3d = self._verts3d\n try:\n M = self.axes.M\n except AttributeError:\n # deprecated since matplotlib 3.4, will be removed in 3.6\n M = renderer.M\n xs, ys, zs = proj3d.proj_transform(xs3d, ys3d, zs3d, M)\n self.set_positions((xs[0], ys[0]), (xs[1], ys[1]))\n super(Arrow3D, self).draw(renderer)\n\n def do_3d_projection(self, renderer=None):\n # This supports both matplotlib 3.4 and 3.5\n return 0\n\n\nclass Camera(artist.Artist):\n \"\"\"A Matplotlib artist that displays a camera.\n\n This function is inspired by Blender's camera visualization. It will\n show the camera center, a virtual image plane, and the top of the virtual\n image plane.\n\n Parameters\n ----------\n M : array-like, shape (3, 3)\n Intrinsic camera matrix that contains the focal lengths on the diagonal\n and the center of the the image in the last column. It does not matter\n whether values are given in meters or pixels as long as the unit is the\n same as for the sensor size.\n\n cam2world : array-like, shape (4, 4)\n Transformation matrix of camera in world frame. We assume that the\n position is given in meters.\n\n virtual_image_distance : float, optional (default: 1)\n Distance from pinhole to virtual image plane that will be displayed.\n We assume that this distance is given in meters.\n\n sensor_size : array-like, shape (2,), optional (default: [1920, 1080])\n Size of the image sensor: (width, height). It does not matter whether\n values are given in meters or pixels as long as the unit is the same as\n for the sensor size.\n\n kwargs : dict, optional (default: {})\n Additional arguments for the plotting functions, e.g. alpha.\n \"\"\"\n\n def __init__(\n self,\n M,\n cam2world,\n virtual_image_distance=1.0,\n sensor_size=(1920, 1080),\n **kwargs,\n ):\n super(Camera, self).__init__()\n\n if \"c\" in kwargs:\n color = kwargs.pop(\"c\")\n elif \"color\" in kwargs:\n color = kwargs.pop(\"color\")\n else:\n color = \"k\"\n\n self.sensor_corners = _calculate_sensor_corners_in_camera(\n M, virtual_image_distance, sensor_size\n )\n self.top_corners = _calculate_top_corners_in_camera(self.sensor_corners)\n\n self.lines_sensor = [\n Line3D([], [], [], color=color, **kwargs) for _ in range(4)\n ]\n self.line_top = Line3D([], [], [], color=color, **kwargs)\n\n self.set_data(cam2world)\n\n def set_data(self, cam2world):\n \"\"\"Set the transformation data.\n\n Parameters\n ----------\n cam2world : array-like, shape (4, 4)\n Transform from frame A to frame B\n \"\"\"\n cam2world = np.asarray(cam2world)\n sensor_in_world = np.dot(\n cam2world,\n np.vstack(\n (self.sensor_corners.T, np.ones(len(self.sensor_corners)))\n ),\n )\n for i in range(4):\n xs, ys, zs = [\n [\n cam2world[j, 3],\n sensor_in_world[j, i],\n sensor_in_world[j, (i + 1) % 4],\n ]\n for j in range(3)\n ]\n self.lines_sensor[i].set_data_3d(xs, ys, zs)\n\n top_in_world = np.dot(\n cam2world,\n np.vstack((self.top_corners.T, np.ones(len(self.top_corners)))),\n )\n xs, ys, zs, _ = top_in_world\n self.line_top.set_data_3d(xs, ys, zs)\n\n @artist.allow_rasterization\n def draw(self, renderer, *args, **kwargs):\n \"\"\"Draw the artist.\"\"\"\n for b in self.lines_sensor:\n b.draw(renderer, *args, **kwargs)\n self.line_top.draw(renderer, *args, **kwargs)\n super(Camera, self).draw(renderer, *args, **kwargs)\n\n def add_camera(self, axis):\n \"\"\"Add the camera to a 3D axis.\"\"\"\n for b in self.lines_sensor:\n axis.add_line(b)\n axis.add_line(self.line_top)\n\n\ndef _calculate_sensor_corners_in_camera(M, virtual_image_distance, sensor_size):\n \"\"\"Calculate the corners of the sensor frame in camera coordinates.\"\"\"\n focal_length = np.mean((M[0, 0], M[1, 1]))\n sensor_corners = np.array(\n [\n [0, 0, focal_length],\n [0, sensor_size[1], focal_length],\n [sensor_size[0], sensor_size[1], focal_length],\n [sensor_size[0], 0, focal_length],\n ]\n )\n sensor_corners[:, 0] -= M[0, 2]\n sensor_corners[:, 1] -= M[1, 2]\n return virtual_image_distance / focal_length * sensor_corners\n\n\ndef _calculate_top_corners_in_camera(sensor_corners):\n \"\"\"Calculate the corners of the top triangle in camera coordinates.\"\"\"\n up = sensor_corners[0] - sensor_corners[1]\n return np.array(\n [\n sensor_corners[0] + 0.1 * up,\n 0.5 * (sensor_corners[0] + sensor_corners[3]) + 0.5 * up,\n sensor_corners[3] + 0.1 * up,\n sensor_corners[0] + 0.1 * up,\n ]\n )\n" }, { "path": "pytransform3d/plot_utils/_artists.pyi", "content": "import numpy as np\nimport numpy.typing as npt\nfrom mpl_toolkits.mplot3d import Axes3D\nfrom mpl_toolkits.mplot3d.art3d import Line3D\nfrom matplotlib import artist\nfrom matplotlib.patches import FancyArrowPatch\nfrom matplotlib.backend_bases import RendererBase\nfrom typing import Union, List\n\nclass Frame(artist.Artist):\n def __init__(\n self,\n A2B: npt.ArrayLike,\n label: Union[str, None] = ...,\n s: float = ...,\n **kwargs,\n ): ...\n def set_data(self, A2B: npt.ArrayLike, label: Union[str, None] = ...): ...\n @artist.allow_rasterization\n def draw(self, renderer: RendererBase, *args, **kwargs): ...\n def add_frame(self, axis: Axes3D): ...\n\nclass LabeledFrame(Frame):\n def __init__(\n self,\n A2B: npt.ArrayLike,\n label: Union[str, None] = ...,\n s: float = ...,\n **kwargs,\n ): ...\n def set_data(self, A2B: npt.ArrayLike, label=None): ...\n @artist.allow_rasterization\n def draw(self, renderer: RendererBase, *args, **kwargs): ...\n def add_frame(self, axis: Axes3D): ...\n\nclass Trajectory(artist.Artist):\n trajectory: Line3D\n\n def __init__(\n self,\n H: npt.ArrayLike,\n show_direction: bool = ...,\n n_frames: int = ...,\n s: float = ...,\n label: Union[str, None] = ...,\n **kwargs,\n ): ...\n def set_data(self, H: npt.ArrayLike): ...\n @artist.allow_rasterization\n def draw(self, renderer: RendererBase, *args, **kwargs): ...\n def add_trajectory(self, axis: Axes3D): ...\n\nclass Arrow3D(FancyArrowPatch):\n def __init__(\n self,\n xs: npt.ArrayLike,\n ys: npt.ArrayLike,\n zs: npt.ArrayLike,\n *args,\n **kwargs,\n ): ...\n def set_data(\n self, xs: npt.ArrayLike, ys: npt.ArrayLike, zs: npt.ArrayLike\n ): ...\n def draw(self, renderer: RendererBase): ...\n\nclass Camera(artist.Artist):\n sensor_corners: np.ndarray\n top_corners: np.ndarray\n lines_sensor: List[Line3D]\n line_top: Line3D\n\n def __init__(\n self,\n M: npt.ArrayLike,\n cam2world: npt.ArrayLike,\n virtual_image_distance: float = ...,\n sensor_size: npt.ArrayLike = ...,\n **kwargs,\n ): ...\n def set_data(self, cam2world: npt.ArrayLike): ...\n @artist.allow_rasterization\n def draw(self, renderer: RendererBase, *args, **kwargs): ...\n def add_camera(self, axis: Axes3D): ...\n\ndef _calculate_sensor_corners_in_camera(\n M: npt.ArrayLike, virtual_image_distance: float, sensor_size: npt.ArrayLike\n) -> npt.ArrayLike: ...\ndef _calculate_top_corners_in_camera(\n sensor_corners: npt.ArrayLike,\n) -> npt.ArrayLike: ...\n" }, { "path": "pytransform3d/plot_utils/_layout.py", "content": "\"\"\"Layout utilities for matplotlib.\"\"\"\n\nimport matplotlib.pyplot as plt\nfrom matplotlib.ticker import MaxNLocator\n\n\ndef make_3d_axis(ax_s, pos=111, unit=None, n_ticks=5):\n \"\"\"Generate new 3D axis.\n\n Parameters\n ----------\n ax_s : float, optional (default: 1)\n Scaling of the new matplotlib 3d axis\n\n pos : int, optional (default: 111)\n Position indicator (nrows, ncols, plot_number)\n\n unit : str, optional (default: None)\n Unit of axes. For example, 'm', 'cm', 'km', ...\n The unit will be shown in the axis label, for example,\n as 'X [m]'.\n\n n_ticks : int, optional (default: 5)\n Number of ticks on each axis\n\n Returns\n -------\n ax : Matplotlib 3d axis\n New axis\n \"\"\"\n try:\n ax = plt.subplot(pos, projection=\"3d\", aspect=\"equal\")\n except NotImplementedError:\n # HACK: workaround for bug in new matplotlib versions (ca. 3.02):\n # \"It is not currently possible to manually set the aspect\"\n ax = plt.subplot(pos, projection=\"3d\")\n\n if unit is None:\n xlabel = \"X\"\n ylabel = \"Y\"\n zlabel = \"Z\"\n else:\n xlabel = \"X [%s]\" % unit\n ylabel = \"Y [%s]\" % unit\n zlabel = \"Z [%s]\" % unit\n\n plt.setp(\n ax,\n xlim=(-ax_s, ax_s),\n ylim=(-ax_s, ax_s),\n zlim=(-ax_s, ax_s),\n xlabel=xlabel,\n ylabel=ylabel,\n zlabel=zlabel,\n )\n\n for axis in [ax.xaxis, ax.yaxis, ax.zaxis]:\n axis.set_major_locator(MaxNLocator(n_ticks))\n\n try:\n for axis in [ax.xaxis, ax.yaxis, ax.zaxis]:\n axis.pane.set_color(\"white\")\n except AttributeError: # pragma: no cover\n # fallback for older versions of matplotlib, deprecated since v3.1\n for axis in [ax.w_xaxis, ax.w_yaxis, ax.w_zaxis]:\n axis.pane.set_color(\"white\")\n\n return ax\n\n\ndef remove_frame(ax, left=0.0, bottom=0.0, right=1.0, top=1.0):\n \"\"\"Remove axis and scale bbox.\n\n Parameters\n ----------\n ax : Matplotlib 3d axis\n Axis from which we remove the frame\n\n left : float, optional (default: 0)\n Position of left border (between 0 and 1)\n\n bottom : float, optional (default: 0)\n Position of bottom border (between 0 and 1)\n\n right : float, optional (default: 1)\n Position of right border (between 0 and 1)\n\n top : float, optional (default: 1)\n Position of top border (between 0 and 1)\n \"\"\"\n ax.axis(\"off\")\n plt.subplots_adjust(left=left, bottom=bottom, right=right, top=top)\n" }, { "path": "pytransform3d/plot_utils/_layout.pyi", "content": "from typing import Union\n\nfrom mpl_toolkits.mplot3d import Axes3D\n\ndef make_3d_axis(\n ax_s: float,\n pos: int = ...,\n unit: Union[str, None] = ...,\n n_ticks: int = ...,\n) -> Axes3D: ...\ndef remove_frame(\n ax: Axes3D,\n left: float = ...,\n bottom: float = ...,\n right: float = ...,\n top: float = ...,\n): ...\n" }, { "path": "pytransform3d/plot_utils/_plot_geometries.py", "content": "\"\"\"Plotting functions for geometries.\"\"\"\n\nimport warnings\n\nimport numpy as np\nfrom mpl_toolkits.mplot3d.art3d import Poly3DCollection, Line3DCollection\n\nfrom ._layout import make_3d_axis\nfrom .._geometry import unit_sphere_surface_grid, transform_surface\nfrom .._mesh_loader import load_mesh\nfrom ..transformations import transform\n\n\ndef plot_box(\n ax=None,\n size=np.ones(3),\n A2B=np.eye(4),\n ax_s=1,\n wireframe=True,\n color=\"k\",\n alpha=1.0,\n linewidth=1.0,\n):\n \"\"\"Plot box.\n\n Parameters\n ----------\n ax : Matplotlib 3d axis, optional (default: None)\n If the axis is None, a new 3d axis will be created\n\n size : array-like, shape (3,), optional (default: [1, 1, 1])\n Size of the box per dimension\n\n A2B : array-like, shape (4, 4)\n Center of the box\n\n ax_s : float, optional (default: 1)\n Scaling of the new matplotlib 3d axis\n\n wireframe : bool, optional (default: True)\n Plot wireframe of box and surface otherwise\n\n color : str, optional (default: black)\n Color in which the box should be plotted\n\n alpha : float, optional (default: 1)\n Alpha value of the mesh that will be plotted\n\n linewidth : float, optional (default: 1.0)\n Line width for wireframe plot\n\n Returns\n -------\n ax : Matplotlib 3d axis\n New or old axis\n \"\"\"\n if ax is None:\n ax = make_3d_axis(ax_s)\n\n vertices = [\n [0, 0, 0],\n [0, 0, 1],\n [0, 1, 0],\n [0, 1, 1],\n [1, 0, 0],\n [1, 0, 1],\n [1, 1, 0],\n [1, 1, 1],\n ]\n vertices = (np.array(vertices, dtype=float) - 0.5) * size\n vertices = np.hstack((vertices, np.ones((len(vertices), 1))))\n vertices = transform(A2B, vertices)[:, :3]\n\n if wireframe:\n connections = [\n [0, 1],\n [0, 2],\n [1, 3],\n [2, 3],\n [4, 5],\n [4, 6],\n [5, 7],\n [6, 7],\n [0, 4],\n [1, 5],\n [2, 6],\n [3, 7],\n ]\n surface = Line3DCollection(vertices[connections], linewidth=linewidth)\n surface.set_color(color)\n else:\n faces = [\n [0, 1, 2],\n [1, 2, 3],\n [4, 5, 6],\n [5, 6, 7],\n [0, 1, 4],\n [1, 4, 5],\n [2, 6, 7],\n [2, 3, 7],\n [0, 4, 6],\n [0, 2, 6],\n [1, 5, 7],\n [1, 3, 7],\n ]\n surface = Poly3DCollection(vertices[faces])\n surface.set_facecolor(color)\n surface.set_alpha(alpha)\n ax.add_collection3d(surface)\n\n return ax\n\n\ndef plot_sphere(\n ax=None,\n radius=1.0,\n p=np.zeros(3),\n ax_s=1,\n wireframe=True,\n n_steps=20,\n alpha=1.0,\n color=\"k\",\n linewidth=1.0,\n):\n \"\"\"Plot sphere.\n\n Parameters\n ----------\n ax : Matplotlib 3d axis, optional (default: None)\n If the axis is None, a new 3d axis will be created\n\n radius : float, optional (default: 1)\n Radius of the sphere\n\n p : array-like, shape (3,), optional (default: [0, 0, 0])\n Center of the sphere\n\n ax_s : float, optional (default: 1)\n Scaling of the new matplotlib 3d axis\n\n wireframe : bool, optional (default: True)\n Plot wireframe of sphere and surface otherwise\n\n n_steps : int, optional (default: 20)\n Number of discrete steps plotted in each dimension\n\n alpha : float, optional (default: 1)\n Alpha value of the sphere that will be plotted\n\n color : str, optional (default: black)\n Color in which the sphere should be plotted\n\n linewidth : float, optional (default: 1.0)\n Line width for wireframe plot\n\n Returns\n -------\n ax : Matplotlib 3d axis\n New or old axis\n \"\"\"\n if ax is None:\n ax = make_3d_axis(ax_s)\n\n x, y, z = unit_sphere_surface_grid(n_steps)\n x = p[0] + radius * x\n y = p[1] + radius * y\n z = p[2] + radius * z\n\n if wireframe:\n ax.plot_wireframe(\n x,\n y,\n z,\n rstride=2,\n cstride=2,\n color=color,\n alpha=alpha,\n linewidth=linewidth,\n )\n else:\n ax.plot_surface(x, y, z, color=color, alpha=alpha, linewidth=0)\n\n return ax\n\n\ndef plot_spheres(\n ax=None,\n radius=np.ones(1),\n p=np.zeros((1, 3)),\n ax_s=1,\n wireframe=True,\n n_steps=20,\n alpha=np.ones(1),\n color=np.zeros((1, 3)),\n linewidth=1.0,\n):\n \"\"\"Plot multiple spheres.\n\n Parameters\n ----------\n ax : Matplotlib 3d axis, optional (default: None)\n If the axis is None, a new 3d axis will be created\n\n radius : array-like, shape (n_spheres,), optional (default: 1)\n Radius of the sphere(s)\n\n p : array-like, shape (n_spheres, 3), optional (default: [0, 0, 0])\n Center of the sphere(s)\n\n ax_s : float, optional (default: 1)\n Scaling of the new matplotlib 3d axis\n\n wireframe : bool, optional (default: True)\n Plot wireframe of sphere(s) and surface otherwise\n\n n_steps : int, optional (default: 20)\n Number of discrete steps plotted in each dimension\n\n alpha : array-like, shape (n_spheres,), optional (default: 1)\n Alpha value of the sphere(s) that will be plotted\n\n color : array-like, shape (n_spheres, 3), optional (default: black)\n Color in which the sphere(s) should be plotted\n\n linewidth : float, optional (default: 1.0)\n Line width for wireframe plot\n\n Returns\n -------\n ax : Matplotlib 3d axis\n New or old axis\n \"\"\"\n if ax is None:\n ax = make_3d_axis(ax_s)\n\n radius = np.asarray(radius)\n p = np.asarray(p)\n\n phi, theta = np.mgrid[\n 0.0 : np.pi : n_steps * 1j, 0.0 : 2.0 * np.pi : n_steps * 1j\n ]\n sin_phi = np.sin(phi)\n verts = (\n radius[..., np.newaxis, np.newaxis, np.newaxis]\n * np.array(\n [sin_phi * np.cos(theta), sin_phi * np.sin(theta), np.cos(phi)]\n )[np.newaxis, ...]\n + p[..., np.newaxis, np.newaxis]\n )\n colors = np.resize(color, (len(verts), 3))\n alphas = np.resize(alpha, len(verts))\n\n for verts_i, color_i, alpha_i in zip(verts, colors, alphas):\n if wireframe:\n ax.plot_wireframe(\n *verts_i,\n rstride=2,\n cstride=2,\n color=color_i,\n alpha=alpha_i,\n linewidth=linewidth,\n )\n else:\n ax.plot_surface(*verts_i, color=color_i, alpha=alpha_i, linewidth=0)\n\n return ax\n\n\ndef plot_cylinder(\n ax=None,\n length=1.0,\n radius=1.0,\n thickness=0.0,\n A2B=np.eye(4),\n ax_s=1,\n wireframe=True,\n n_steps=100,\n alpha=1.0,\n color=\"k\",\n linewidth=1.0,\n):\n \"\"\"Plot cylinder.\n\n A cylinder is the volume covered by a disk moving along a line segment.\n\n Parameters\n ----------\n ax : Matplotlib 3d axis, optional (default: None)\n If the axis is None, a new 3d axis will be created\n\n length : float, optional (default: 1)\n Length of the cylinder\n\n radius : float, optional (default: 1)\n Radius of the cylinder\n\n thickness : float, optional (default: 0)\n Thickness of a cylindrical shell. It will be subtracted from the\n outer radius to obtain the inner radius. The difference must be\n greater than 0.\n\n A2B : array-like, shape (4, 4)\n Center of the cylinder\n\n ax_s : float, optional (default: 1)\n Scaling of the new matplotlib 3d axis\n\n wireframe : bool, optional (default: True)\n Plot wireframe of cylinder and surface otherwise\n\n n_steps : int, optional (default: 100)\n Number of discrete steps plotted in each dimension\n\n alpha : float, optional (default: 1)\n Alpha value of the cylinder that will be plotted\n\n color : str, optional (default: black)\n Color in which the cylinder should be plotted\n\n linewidth : float, optional (default: 1.0)\n Line width for wireframe plot\n\n Returns\n -------\n ax : Matplotlib 3d axis\n New or old axis\n\n Raises\n ------\n ValueError\n If thickness is <= 0\n \"\"\"\n if ax is None:\n ax = make_3d_axis(ax_s)\n\n inner_radius = radius - thickness\n if inner_radius <= 0.0:\n raise ValueError(\n \"Thickness of cylindrical shell results in \"\n \"invalid inner radius: %g\" % inner_radius\n )\n\n if wireframe:\n t = np.linspace(0, length, n_steps)\n else:\n t = np.array([0, length])\n angles = np.linspace(0, 2 * np.pi, n_steps)\n t, angles = np.meshgrid(t, angles)\n\n A2B = np.asarray(A2B)\n axis_start = np.dot(A2B, [0, 0, -0.5 * length, 1])[:3]\n X, Y, Z = _elongated_circular_grid(axis_start, A2B, t, radius, angles)\n if thickness > 0.0:\n A2B_left_hand = np.copy(A2B)\n A2B_left_hand[:3, 2] *= -1.0\n axis_end = np.dot(A2B, [0, 0, 0.5 * length, 1])[:3]\n X_inner, Y_inner, Z_inner = _elongated_circular_grid(\n axis_end, A2B_left_hand, t, inner_radius, angles\n )\n X = np.hstack((X, X_inner))\n Y = np.hstack((Y, Y_inner))\n Z = np.hstack((Z, Z_inner))\n\n if wireframe:\n ax.plot_wireframe(\n X,\n Y,\n Z,\n rstride=10,\n cstride=10,\n alpha=alpha,\n color=color,\n linewidth=linewidth,\n )\n else:\n ax.plot_surface(X, Y, Z, color=color, alpha=alpha, linewidth=0)\n\n return ax\n\n\ndef plot_mesh(\n ax=None,\n filename=None,\n A2B=np.eye(4),\n s=np.array([1.0, 1.0, 1.0]),\n ax_s=1,\n wireframe=False,\n convex_hull=False,\n alpha=1.0,\n color=\"k\",\n linewidth=1.0,\n):\n \"\"\"Plot mesh.\n\n Note that this function requires the additional library to load meshes\n such as trimesh or open3d.\n\n Parameters\n ----------\n ax : Matplotlib 3d axis, optional (default: None)\n If the axis is None, a new 3d axis will be created\n\n filename : str, optional (default: None)\n Path to mesh file.\n\n A2B : array-like, shape (4, 4)\n Pose of the mesh\n\n s : array-like, shape (3,), optional (default: [1, 1, 1])\n Scaling of the mesh that will be drawn\n\n ax_s : float, optional (default: 1)\n Scaling of the new matplotlib 3d axis\n\n wireframe : bool, optional (default: True)\n Plot wireframe of mesh and surface otherwise\n\n convex_hull : bool, optional (default: False)\n Show convex hull instead of the original mesh. This can be much\n faster.\n\n alpha : float, optional (default: 1)\n Alpha value of the mesh that will be plotted\n\n color : str, optional (default: black)\n Color in which the mesh should be plotted\n\n linewidth : float, optional (default: 1.0)\n Line width for wireframe plot\n\n Returns\n -------\n ax : Matplotlib 3d axis\n New or old axis\n \"\"\"\n if ax is None:\n ax = make_3d_axis(ax_s)\n\n if filename is None:\n warnings.warn(\n \"No filename given for mesh. When you use the \"\n \"UrdfTransformManager, make sure to set the mesh path or \"\n \"package directory.\",\n UserWarning,\n stacklevel=2,\n )\n return ax\n\n mesh = load_mesh(filename)\n if convex_hull:\n mesh.convex_hull()\n\n vertices = np.asarray(mesh.vertices) * s\n vertices = np.hstack((vertices, np.ones((len(vertices), 1))))\n vertices = transform(A2B, vertices)[:, :3]\n faces = vertices[mesh.triangles]\n if wireframe:\n surface = Line3DCollection(faces, linewidth=linewidth)\n surface.set_color(color)\n else:\n surface = Poly3DCollection(faces)\n surface.set_facecolor(color)\n surface.set_alpha(alpha)\n ax.add_collection3d(surface)\n return ax\n\n\ndef plot_ellipsoid(\n ax=None,\n radii=np.ones(3),\n A2B=np.eye(4),\n ax_s=1,\n wireframe=True,\n n_steps=20,\n alpha=1.0,\n color=\"k\",\n linewidth=1.0,\n):\n \"\"\"Plot ellipsoid.\n\n Parameters\n ----------\n ax : Matplotlib 3d axis, optional (default: None)\n If the axis is None, a new 3d axis will be created\n\n radii : array-like, shape (3,)\n Radii along the x-axis, y-axis, and z-axis of the ellipsoid.\n\n A2B : array-like, shape (4, 4)\n Transform from frame A to frame B\n\n ax_s : float, optional (default: 1)\n Scaling of the new matplotlib 3d axis\n\n wireframe : bool, optional (default: True)\n Plot wireframe of ellipsoid and surface otherwise\n\n n_steps : int, optional (default: 20)\n Number of discrete steps plotted in each dimension\n\n alpha : float, optional (default: 1)\n Alpha value of the ellipsoid that will be plotted\n\n color : str, optional (default: black)\n Color in which the ellipsoid should be plotted\n\n linewidth : float, optional (default: 1.0)\n Line width for wireframe plot\n\n Returns\n -------\n ax : Matplotlib 3d axis\n New or old axis\n \"\"\"\n if ax is None:\n ax = make_3d_axis(ax_s)\n\n radius_x, radius_y, radius_z = radii\n\n x, y, z = unit_sphere_surface_grid(n_steps)\n x *= radius_x\n y *= radius_y\n z *= radius_z\n\n x, y, z = transform_surface(A2B, x, y, z)\n\n if wireframe:\n ax.plot_wireframe(\n x,\n y,\n z,\n rstride=2,\n cstride=2,\n color=color,\n alpha=alpha,\n linewidth=linewidth,\n )\n else:\n ax.plot_surface(x, y, z, color=color, alpha=alpha, linewidth=0)\n\n return ax\n\n\ndef plot_capsule(\n ax=None,\n A2B=np.eye(4),\n height=1.0,\n radius=1.0,\n ax_s=1,\n wireframe=True,\n n_steps=20,\n alpha=1.0,\n color=\"k\",\n linewidth=1.0,\n):\n \"\"\"Plot capsule.\n\n A capsule is the volume covered by a sphere moving along a line segment.\n\n Parameters\n ----------\n ax : Matplotlib 3d axis, optional (default: None)\n If the axis is None, a new 3d axis will be created\n\n A2B : array-like, shape (4, 4)\n Frame of the capsule, located at the center of the line segment.\n\n height : float, optional (default: 1)\n Height of the capsule along its z-axis.\n\n radius : float, optional (default: 1)\n Radius of the capsule.\n\n ax_s : float, optional (default: 1)\n Scaling of the new matplotlib 3d axis\n\n wireframe : bool, optional (default: True)\n Plot wireframe of capsule and surface otherwise\n\n n_steps : int, optional (default: 20)\n Number of discrete steps plotted in each dimension\n\n alpha : float, optional (default: 1)\n Alpha value of the mesh that will be plotted\n\n color : str, optional (default: black)\n Color in which the capsule should be plotted\n\n linewidth : float, optional (default: 1.0)\n Line width for wireframe plot\n\n Returns\n -------\n ax : Matplotlib 3d axis\n New or old axis\n \"\"\"\n if ax is None:\n ax = make_3d_axis(ax_s)\n\n x, y, z = unit_sphere_surface_grid(n_steps)\n x *= radius\n y *= radius\n z *= radius\n z[len(z) // 2 :] -= 0.5 * height\n z[: len(z) // 2] += 0.5 * height\n\n x, y, z = transform_surface(A2B, x, y, z)\n\n if wireframe:\n ax.plot_wireframe(\n x,\n y,\n z,\n rstride=2,\n cstride=2,\n color=color,\n alpha=alpha,\n linewidth=linewidth,\n )\n else:\n ax.plot_surface(x, y, z, color=color, alpha=alpha, linewidth=0)\n\n return ax\n\n\ndef plot_cone(\n ax=None,\n height=1.0,\n radius=1.0,\n A2B=np.eye(4),\n ax_s=1,\n wireframe=True,\n n_steps=20,\n alpha=1.0,\n color=\"k\",\n linewidth=1.0,\n):\n \"\"\"Plot cone.\n\n Parameters\n ----------\n ax : Matplotlib 3d axis, optional (default: None)\n If the axis is None, a new 3d axis will be created\n\n height : float, optional (default: 1)\n Height of the cone\n\n radius : float, optional (default: 1)\n Radius of the cone\n\n A2B : array-like, shape (4, 4)\n Center of the cone\n\n ax_s : float, optional (default: 1)\n Scaling of the new matplotlib 3d axis\n\n wireframe : bool, optional (default: True)\n Plot wireframe of cone and surface otherwise\n\n n_steps : int, optional (default: 100)\n Number of discrete steps plotted in each dimension\n\n alpha : float, optional (default: 1)\n Alpha value of the cone that will be plotted\n\n color : str, optional (default: black)\n Color in which the cone should be plotted\n\n linewidth : float, optional (default: 1.0)\n Line width for wireframe plot\n\n Returns\n -------\n ax : Matplotlib 3d axis\n New or old axis\n \"\"\"\n if ax is None:\n ax = make_3d_axis(ax_s)\n\n if wireframe:\n t = np.linspace(0, height, n_steps)\n radii = np.linspace(radius, 0, n_steps)\n else:\n t = np.array([0, height])\n radii = np.array([radius, 0])\n angles = np.linspace(0, 2 * np.pi, n_steps)\n t, angles = np.meshgrid(t, angles)\n\n A2B = np.asarray(A2B)\n X, Y, Z = _elongated_circular_grid(A2B[:3, 3], A2B, t, radii, angles)\n\n if wireframe:\n ax.plot_wireframe(\n X,\n Y,\n Z,\n rstride=5,\n cstride=5,\n alpha=alpha,\n color=color,\n linewidth=linewidth,\n )\n else:\n ax.plot_surface(X, Y, Z, color=color, alpha=alpha, linewidth=0)\n\n return ax\n\n\ndef _elongated_circular_grid(\n bottom_point, A2B, height_fractions, radii, angles\n):\n return [\n bottom_point[i]\n + radii * np.sin(angles) * A2B[i, 0]\n + radii * np.cos(angles) * A2B[i, 1]\n + A2B[i, 2] * height_fractions\n for i in range(3)\n ]\n" }, { "path": "pytransform3d/plot_utils/_plot_geometries.pyi", "content": "import numpy as np\nimport numpy.typing as npt\nfrom mpl_toolkits.mplot3d import Axes3D\nfrom typing import Union\n\ndef plot_box(\n ax: Union[None, Axes3D] = ...,\n size: npt.ArrayLike = ...,\n A2B: npt.ArrayLike = ...,\n ax_s: float = ...,\n wireframe: bool = ...,\n color: str = ...,\n alpha: float = ...,\n linewidth: float = ...,\n) -> Axes3D: ...\ndef plot_sphere(\n ax: Union[None, Axes3D] = ...,\n radius: float = ...,\n p: npt.ArrayLike = ...,\n ax_s: float = ...,\n wireframe: bool = ...,\n n_steps: int = ...,\n alpha: float = ...,\n color: str = ...,\n linewidth: float = ...,\n) -> Axes3D: ...\ndef plot_spheres(\n ax: Union[None, Axes3D] = ...,\n radius: npt.ArrayLike = ...,\n p: npt.ArrayLike = ...,\n ax_s: float = ...,\n wireframe: bool = ...,\n n_steps: int = ...,\n alpha: npt.ArrayLike = ...,\n color: npt.ArrayLike = ...,\n linewidth: float = ...,\n) -> Axes3D: ...\ndef plot_cylinder(\n ax: Union[None, Axes3D] = ...,\n length: float = ...,\n radius: float = ...,\n thickness: float = ...,\n A2B: npt.ArrayLike = ...,\n ax_s: float = ...,\n wireframe: bool = ...,\n n_steps: int = ...,\n alpha: float = ...,\n color: str = ...,\n linewidth: float = ...,\n) -> Axes3D: ...\ndef plot_mesh(\n ax: Union[None, Axes3D] = ...,\n filename: Union[None, str] = ...,\n A2B: npt.ArrayLike = ...,\n s: npt.ArrayLike = ...,\n ax_s: float = ...,\n wireframe: bool = ...,\n convex_hull: bool = ...,\n alpha: float = ...,\n color: str = ...,\n linewidth: float = ...,\n) -> Axes3D: ...\ndef plot_ellipsoid(\n ax: Union[None, Axes3D] = ...,\n radii: npt.ArrayLike = ...,\n A2B: npt.ArrayLike = ...,\n ax_s: float = ...,\n wireframe: bool = ...,\n n_steps: int = ...,\n alpha: float = ...,\n color: str = ...,\n linewidth: float = ...,\n) -> Axes3D: ...\ndef plot_capsule(\n ax: Union[None, Axes3D] = ...,\n A2B: npt.ArrayLike = ...,\n height: float = ...,\n radius: float = ...,\n ax_s: float = ...,\n wireframe: bool = ...,\n n_steps: int = ...,\n alpha: float = ...,\n color: str = ...,\n linewidth: float = ...,\n) -> Axes3D: ...\ndef plot_cone(\n ax: Union[None, Axes3D] = ...,\n height: float = ...,\n radius: float = ...,\n A2B: npt.ArrayLike = ...,\n ax_s: float = ...,\n wireframe: bool = ...,\n n_steps: int = ...,\n alpha: float = ...,\n color: str = ...,\n linewidth: float = ...,\n) -> Axes3D: ...\n" }, { "path": "pytransform3d/plot_utils/_plot_helpers.py", "content": "\"\"\"Plotting functions.\"\"\"\n\nimport numpy as np\n\nfrom ._artists import Arrow3D\nfrom ._layout import make_3d_axis\nfrom ..rotations import unitx, unitz, perpendicular_to_vectors, norm_vector\n\n\ndef plot_vector(\n ax=None,\n start=np.zeros(3),\n direction=np.array([1, 0, 0]),\n s=1.0,\n arrowstyle=\"simple\",\n ax_s=1,\n **kwargs,\n):\n \"\"\"Plot Vector.\n\n Draws an arrow from start to start + s * direction.\n\n Parameters\n ----------\n ax : Matplotlib 3d axis, optional (default: None)\n If the axis is None, a new 3d axis will be created\n\n start : array-like, shape (3,), optional (default: [0, 0, 0])\n Start of the vector\n\n direction : array-like, shape (3,), optional (default: [1, 0, 0])\n Direction of the vector\n\n s : float, optional (default: 1)\n Scaling of the vector that will be drawn\n\n arrowstyle : str, or ArrowStyle, optional (default: 'simple')\n See matplotlib's documentation of arrowstyle in\n matplotlib.patches.FancyArrowPatch for more options\n\n ax_s : float, optional (default: 1)\n Scaling of the new matplotlib 3d axis\n\n kwargs : dict, optional (default: {})\n Additional arguments for the plotting functions, e.g. alpha\n\n Returns\n -------\n ax : Matplotlib 3d axis\n New or old axis\n \"\"\"\n if ax is None:\n ax = make_3d_axis(ax_s)\n\n axis_arrow = Arrow3D(\n [start[0], start[0] + s * direction[0]],\n [start[1], start[1] + s * direction[1]],\n [start[2], start[2] + s * direction[2]],\n mutation_scale=20,\n arrowstyle=arrowstyle,\n **kwargs,\n )\n ax.add_artist(axis_arrow)\n\n return ax\n\n\ndef plot_length_variable(\n ax=None,\n start=np.zeros(3),\n end=np.ones(3),\n name=\"l\",\n above=False,\n ax_s=1,\n color=\"k\",\n **kwargs,\n):\n \"\"\"Plot length with text at its center.\n\n Parameters\n ----------\n ax : Matplotlib 3d axis, optional (default: None)\n If the axis is None, a new 3d axis will be created\n\n start : array-like, shape (3,), optional (default: [0, 0, 0])\n Start point\n\n end : array-like, shape (3,), optional (default: [1, 1, 1])\n End point\n\n name : str, optional (default: 'l')\n Text in the middle\n\n above : bool, optional (default: False)\n Plot name above line\n\n ax_s : float, optional (default: 1)\n Scaling of the new matplotlib 3d axis\n\n color : str, optional (default: black)\n Color in which the cylinder should be plotted\n\n kwargs : dict, optional (default: {})\n Additional arguments for the text, e.g. fontsize\n\n Returns\n -------\n ax : Matplotlib 3d axis\n New or old axis\n \"\"\"\n if ax is None:\n ax = make_3d_axis(ax_s)\n\n direction = end - start\n length = np.linalg.norm(direction)\n\n if above:\n ax.plot(\n [start[0], end[0]],\n [start[1], end[1]],\n [start[2], end[2]],\n color=color,\n )\n else:\n mid1 = start + 0.4 * direction\n mid2 = start + 0.6 * direction\n ax.plot(\n [start[0], mid1[0]],\n [start[1], mid1[1]],\n [start[2], mid1[2]],\n color=color,\n )\n ax.plot(\n [end[0], mid2[0]], [end[1], mid2[1]], [end[2], mid2[2]], color=color\n )\n\n if np.linalg.norm(direction / length - unitz) < np.finfo(float).eps:\n axis = unitx\n else:\n axis = unitz\n\n mark = (\n norm_vector(perpendicular_to_vectors(direction, axis)) * 0.03 * length\n )\n mark_start1 = start + mark\n mark_start2 = start - mark\n mark_end1 = end + mark\n mark_end2 = end - mark\n ax.plot(\n [mark_start1[0], mark_start2[0]],\n [mark_start1[1], mark_start2[1]],\n [mark_start1[2], mark_start2[2]],\n color=color,\n )\n ax.plot(\n [mark_end1[0], mark_end2[0]],\n [mark_end1[1], mark_end2[1]],\n [mark_end1[2], mark_end2[2]],\n color=color,\n )\n text_location = start + 0.45 * direction\n if above:\n text_location[2] += 0.3 * length\n ax.text(\n text_location[0],\n text_location[1],\n text_location[2],\n name,\n zdir=\"x\",\n **kwargs,\n )\n\n return ax\n" }, { "path": "pytransform3d/plot_utils/_plot_helpers.pyi", "content": "import numpy as np\nimport numpy.typing as npt\nfrom mpl_toolkits.mplot3d import Axes3D\nfrom typing import Union\n\ndef plot_vector(\n ax: Union[None, Axes3D] = ...,\n start: npt.ArrayLike = ...,\n direction: npt.ArrayLike = ...,\n s: float = ...,\n arrowstyle: str = ...,\n ax_s: float = ...,\n **kwargs,\n) -> Axes3D: ...\ndef plot_length_variable(\n ax: Union[None, Axes3D] = ...,\n start: npt.ArrayLike = ...,\n end: npt.ArrayLike = ...,\n name: str = ...,\n above: bool = ...,\n ax_s: float = ...,\n color: str = ...,\n **kwargs,\n) -> Axes3D: ...\n" }, { "path": "pytransform3d/plot_utils/test/test_plot_utils.py", "content": "import numpy as np\nimport pytest\n\ntry:\n import matplotlib # noqa: F401\nexcept ImportError:\n pytest.skip(\"matplotlib is required for these tests\")\nfrom pytransform3d.plot_utils import (\n make_3d_axis,\n remove_frame,\n Frame,\n LabeledFrame,\n Trajectory,\n Camera,\n plot_box,\n plot_sphere,\n plot_spheres,\n plot_cylinder,\n plot_mesh,\n plot_ellipsoid,\n plot_capsule,\n plot_cone,\n plot_vector,\n plot_length_variable,\n)\nfrom numpy.testing import assert_array_almost_equal\n\n\ndef test_make_3d_axis():\n ax = make_3d_axis(2.0)\n try:\n assert ax.name == \"3d\"\n assert ax.get_xlim() == (-2, 2)\n assert ax.get_ylim() == (-2, 2)\n assert ax.get_zlim() == (-2, 2)\n finally:\n ax.remove()\n\n\ndef test_make_3d_axis_subplots():\n ax1 = make_3d_axis(1.0, 121)\n ax2 = make_3d_axis(1.0, 122)\n try:\n bounds1 = ax1.get_position().bounds\n bounds2 = ax2.get_position().bounds\n assert bounds1[0] < bounds2[0]\n assert bounds1[2] < bounds2[2]\n finally:\n ax1.remove()\n ax2.remove()\n\n\ndef test_make_3d_axis_with_units():\n ax = make_3d_axis(1.0, unit=\"m\")\n try:\n assert ax.get_xlabel() == \"X [m]\"\n assert ax.get_ylabel() == \"Y [m]\"\n assert ax.get_zlabel() == \"Z [m]\"\n finally:\n ax.remove()\n\n\ndef test_frame_removed():\n ax = make_3d_axis(1.0)\n try:\n remove_frame(ax)\n bounds = ax.get_position().bounds\n # regression test\n assert_array_almost_equal(bounds, (0.125, 0.0, 0.75, 1.0))\n finally:\n ax.remove()\n\n\ndef test_frame():\n ax = make_3d_axis(1.0)\n try:\n frame = Frame(np.eye(4), label=\"Frame\", s=0.1)\n frame.add_frame(ax)\n assert len(ax.lines) == 4 # 3 axes and black line to text\n assert len(ax.texts) == 1 # label\n finally:\n ax.remove()\n\n\ndef test_frame_no_indicator():\n ax = make_3d_axis(1.0)\n try:\n frame = Frame(\n np.eye(4), label=\"Frame\", s=0.1, draw_label_indicator=False\n )\n frame.add_frame(ax)\n assert len(ax.lines) == 3 # 3 axes and omit black line to text\n assert len(ax.texts) == 1 # label\n finally:\n ax.remove()\n\n\ndef test_labeled_frame():\n ax = make_3d_axis(1.0)\n try:\n frame = LabeledFrame(np.eye(4), label=\"Frame\", s=0.1)\n frame.add_frame(ax)\n assert len(ax.lines) == 4 # 3 axes and black line to text\n assert len(ax.texts) == 4 # label and 3 axis labels\n finally:\n ax.remove()\n\n\ndef test_trajectory():\n ax = make_3d_axis(1.0)\n try:\n trajectory = Trajectory(\n np.array([np.eye(4), np.eye(4)]), s=0.1, n_frames=2\n )\n trajectory.add_trajectory(ax)\n assert len(ax.lines) == 7 # 2 * 3 axes + connection line\n # assert_equal(len(ax.artists), 1) # arrow is not an artist anymore\n finally:\n ax.remove()\n\n\ndef test_camera():\n ax = make_3d_axis(1.0)\n try:\n w, h = 1920, 1080 # [pixels]\n M = np.array(((100, 0, 100), (0, 100, 100), (0, 0, 1)))\n camera = Camera(\n M, np.eye(4), virtual_image_distance=5, sensor_size=(w, h)\n )\n camera.add_camera(ax)\n assert len(ax.lines) == 5 # 4 egdes + 1 triangle\n finally:\n ax.remove()\n\n\ndef test_plot_box():\n ax = make_3d_axis(1.0)\n try:\n plot_box(ax, wireframe=False)\n assert len(ax.collections) == 1\n finally:\n ax.remove()\n\n\ndef test_plot_box_wireframe():\n ax = make_3d_axis(1.0)\n try:\n plot_box(ax, wireframe=True)\n assert len(ax.collections) == 1\n finally:\n ax.remove()\n\n\ndef test_plot_sphere():\n ax = make_3d_axis(1.0)\n try:\n plot_sphere(ax, wireframe=False)\n assert len(ax.collections) == 1\n finally:\n ax.remove()\n\n\ndef test_plot_sphere_wireframe():\n ax = make_3d_axis(1.0)\n try:\n plot_sphere(ax, wireframe=True)\n assert len(ax.collections) == 1\n finally:\n ax.remove()\n\n\ndef test_plot_spheres():\n ax = make_3d_axis(1.0)\n try:\n plot_spheres(ax, wireframe=False)\n assert len(ax.collections) == 1\n finally:\n ax.remove()\n\n\ndef test_plot_spheres_wireframe():\n ax = make_3d_axis(1.0)\n try:\n plot_spheres(ax, wireframe=True)\n assert len(ax.collections) == 1\n finally:\n ax.remove()\n\n\ndef test_plot_cylinder():\n ax = make_3d_axis(1.0)\n try:\n plot_cylinder(ax, wireframe=False)\n assert len(ax.collections) == 1\n finally:\n ax.remove()\n\n\ndef test_plot_cylinder_wireframe():\n ax = make_3d_axis(1.0)\n try:\n plot_cylinder(ax, wireframe=True)\n assert len(ax.collections) == 1\n finally:\n ax.remove()\n\n\ndef test_plot_mesh():\n try:\n import trimesh # noqa: F401\n except ImportError:\n pytest.skip(\"trimesh is required for this test\")\n ax = make_3d_axis(1.0)\n try:\n plot_mesh(ax, filename=\"test/test_data/cone.stl\", wireframe=False)\n assert len(ax.collections) == 1\n finally:\n ax.remove()\n\n\ndef test_plot_mesh_wireframe():\n try:\n import trimesh # noqa: F401\n except ImportError:\n pytest.skip(\"trimesh is required for this test\")\n ax = make_3d_axis(1.0)\n try:\n plot_mesh(ax, filename=\"test/test_data/cone.stl\", wireframe=True)\n assert len(ax.collections) == 1\n finally:\n ax.remove()\n\n\ndef test_plot_ellipsoid():\n ax = make_3d_axis(1.0)\n try:\n plot_ellipsoid(ax, wireframe=False)\n assert len(ax.collections) == 1\n finally:\n ax.remove()\n\n\ndef test_plot_ellipsoid_wireframe():\n ax = make_3d_axis(1.0)\n try:\n plot_ellipsoid(ax, wireframe=True)\n assert len(ax.collections) == 1\n finally:\n ax.remove()\n\n\ndef test_plot_capsule():\n ax = make_3d_axis(1.0)\n try:\n plot_capsule(ax, wireframe=False)\n assert len(ax.collections) == 1\n finally:\n ax.remove()\n\n\ndef test_plot_capsule_wireframe():\n ax = make_3d_axis(1.0)\n try:\n plot_capsule(ax, wireframe=True)\n assert len(ax.collections) == 1\n finally:\n ax.remove()\n\n\ndef test_plot_cone():\n ax = make_3d_axis(1.0)\n try:\n plot_cone(ax, wireframe=False)\n assert len(ax.collections) == 1\n finally:\n ax.remove()\n\n\ndef test_plot_cone_wireframe():\n ax = make_3d_axis(1.0)\n try:\n plot_cone(ax, wireframe=True)\n assert len(ax.collections) == 1\n finally:\n ax.remove()\n\n\ndef test_plot_vector():\n ax = make_3d_axis(1.0)\n try:\n plot_vector(ax)\n # assert_equal(len(ax.artists), 1) # arrow is not an artist anymore\n finally:\n ax.remove()\n\n\ndef test_plot_length_variable():\n ax = make_3d_axis(1.0)\n try:\n plot_length_variable(ax)\n assert len(ax.lines) >= 1\n assert len(ax.texts) == 1\n finally:\n ax.remove()\n" }, { "path": "pytransform3d/py.typed", "content": "" }, { "path": "pytransform3d/rotations/__init__.py", "content": "\"\"\"Rotations in three dimensions - SO(3).\n\nSee :doc:`user_guide/rotations` for more information.\n\"\"\"\n\nfrom ._constants import (\n eps,\n unitx,\n unity,\n unitz,\n q_i,\n q_j,\n q_k,\n q_id,\n a_id,\n R_id,\n p0,\n)\nfrom ._utils import (\n norm_vector,\n angle_between_vectors,\n perpendicular_to_vector,\n vector_projection,\n perpendicular_to_vectors,\n plane_basis_from_normal,\n)\nfrom ._angle import (\n norm_angle,\n active_matrix_from_angle,\n passive_matrix_from_angle,\n quaternion_from_angle,\n)\nfrom ._matrix import (\n matrix_requires_renormalization,\n check_matrix,\n norm_matrix,\n assert_rotation_matrix,\n matrix_from_two_vectors,\n quaternion_from_matrix,\n axis_angle_from_matrix,\n compact_axis_angle_from_matrix,\n)\nfrom ._euler import (\n norm_euler,\n euler_near_gimbal_lock,\n assert_euler_equal,\n matrix_from_euler,\n euler_from_matrix,\n euler_from_quaternion,\n)\nfrom ._axis_angle import (\n norm_axis_angle,\n norm_compact_axis_angle,\n compact_axis_angle_near_pi,\n check_axis_angle,\n check_compact_axis_angle,\n assert_axis_angle_equal,\n assert_compact_axis_angle_equal,\n axis_angle_from_two_directions,\n matrix_from_axis_angle,\n matrix_from_compact_axis_angle,\n axis_angle_from_compact_axis_angle,\n compact_axis_angle,\n quaternion_from_axis_angle,\n quaternion_from_compact_axis_angle,\n mrp_from_axis_angle,\n)\nfrom ._rot_log import (\n check_skew_symmetric_matrix,\n check_rot_log,\n cross_product_matrix,\n rot_log_from_compact_axis_angle,\n)\nfrom ._quaternion import (\n quaternion_requires_renormalization,\n check_quaternion,\n check_quaternions,\n quaternion_double,\n pick_closest_quaternion,\n assert_quaternion_equal,\n quaternion_integrate,\n quaternion_gradient,\n concatenate_quaternions,\n q_conj,\n q_prod_vector,\n quaternion_diff,\n quaternion_dist,\n quaternion_from_euler,\n matrix_from_quaternion,\n axis_angle_from_quaternion,\n compact_axis_angle_from_quaternion,\n mrp_from_quaternion,\n quaternion_wxyz_from_xyzw,\n quaternion_xyzw_from_wxyz,\n)\nfrom ._mrp import (\n norm_mrp,\n check_mrp,\n mrp_near_singularity,\n mrp_double,\n assert_mrp_equal,\n concatenate_mrp,\n mrp_prod_vector,\n quaternion_from_mrp,\n axis_angle_from_mrp,\n)\nfrom ._rotors import (\n check_rotor,\n wedge,\n geometric_product,\n rotor_apply,\n rotor_reverse,\n concatenate_rotors,\n rotor_from_plane_angle,\n rotor_from_two_directions,\n matrix_from_rotor,\n plane_normal_from_bivector,\n)\nfrom ._random import (\n random_vector,\n random_axis_angle,\n random_compact_axis_angle,\n random_quaternion,\n random_matrix,\n)\nfrom ._slerp import (\n matrix_slerp,\n matrix_power,\n slerp_weights,\n quaternion_slerp,\n axis_angle_slerp,\n rotor_slerp,\n)\nfrom ._jacobians import (\n left_jacobian_SO3,\n left_jacobian_SO3_series,\n left_jacobian_SO3_inv,\n left_jacobian_SO3_inv_series,\n)\nfrom ._polar_decomp import robust_polar_decomposition\nfrom ._plot import (\n plot_basis,\n plot_axis_angle,\n plot_bivector,\n)\nfrom ._euler_deprecated import (\n active_matrix_from_extrinsic_euler_xyx,\n active_matrix_from_intrinsic_euler_xyx,\n active_matrix_from_extrinsic_euler_xyz,\n active_matrix_from_extrinsic_euler_xzx,\n active_matrix_from_extrinsic_euler_xzy,\n active_matrix_from_extrinsic_euler_yxy,\n active_matrix_from_extrinsic_euler_yxz,\n active_matrix_from_extrinsic_euler_yzx,\n active_matrix_from_extrinsic_euler_yzy,\n active_matrix_from_extrinsic_euler_zxy,\n active_matrix_from_extrinsic_euler_zxz,\n active_matrix_from_extrinsic_euler_zyz,\n active_matrix_from_intrinsic_euler_xyz,\n active_matrix_from_intrinsic_euler_xzx,\n active_matrix_from_extrinsic_euler_zyx,\n active_matrix_from_intrinsic_euler_xzy,\n active_matrix_from_intrinsic_euler_yxy,\n active_matrix_from_intrinsic_euler_yxz,\n active_matrix_from_intrinsic_euler_yzx,\n active_matrix_from_intrinsic_euler_yzy,\n active_matrix_from_intrinsic_euler_zxy,\n active_matrix_from_intrinsic_euler_zxz,\n active_matrix_from_intrinsic_euler_zyx,\n active_matrix_from_intrinsic_euler_zyz,\n active_matrix_from_extrinsic_roll_pitch_yaw,\n intrinsic_euler_xyz_from_active_matrix,\n intrinsic_euler_xyx_from_active_matrix,\n intrinsic_euler_xzx_from_active_matrix,\n intrinsic_euler_xzy_from_active_matrix,\n intrinsic_euler_yxy_from_active_matrix,\n intrinsic_euler_yxz_from_active_matrix,\n intrinsic_euler_yzx_from_active_matrix,\n intrinsic_euler_yzy_from_active_matrix,\n intrinsic_euler_zxy_from_active_matrix,\n intrinsic_euler_zxz_from_active_matrix,\n intrinsic_euler_zyx_from_active_matrix,\n intrinsic_euler_zyz_from_active_matrix,\n extrinsic_euler_xyx_from_active_matrix,\n extrinsic_euler_xyz_from_active_matrix,\n extrinsic_euler_xzx_from_active_matrix,\n extrinsic_euler_xzy_from_active_matrix,\n extrinsic_euler_yxy_from_active_matrix,\n extrinsic_euler_yxz_from_active_matrix,\n extrinsic_euler_yzx_from_active_matrix,\n extrinsic_euler_yzy_from_active_matrix,\n extrinsic_euler_zxy_from_active_matrix,\n extrinsic_euler_zxz_from_active_matrix,\n extrinsic_euler_zyx_from_active_matrix,\n extrinsic_euler_zyz_from_active_matrix,\n quaternion_from_extrinsic_euler_xyz,\n)\n\n__all__ = [\n \"eps\",\n \"unitx\",\n \"unity\",\n \"unitz\",\n \"q_i\",\n \"q_j\",\n \"q_k\",\n \"q_id\",\n \"a_id\",\n \"R_id\",\n \"p0\",\n \"norm_angle\",\n \"norm_vector\",\n \"angle_between_vectors\",\n \"perpendicular_to_vector\",\n \"vector_projection\",\n \"perpendicular_to_vectors\",\n \"norm_axis_angle\",\n \"compact_axis_angle_near_pi\",\n \"norm_compact_axis_angle\",\n \"matrix_requires_renormalization\",\n \"norm_matrix\",\n \"quaternion_requires_renormalization\",\n \"random_vector\",\n \"random_axis_angle\",\n \"random_compact_axis_angle\",\n \"random_quaternion\",\n \"random_matrix\",\n \"check_skew_symmetric_matrix\",\n \"check_rot_log\",\n \"check_matrix\",\n \"check_quaternion\",\n \"check_quaternions\",\n \"check_axis_angle\",\n \"check_rotor\",\n \"check_compact_axis_angle\",\n \"check_mrp\",\n \"quaternion_from_axis_angle\",\n \"quaternion_from_compact_axis_angle\",\n \"quaternion_from_matrix\",\n \"quaternion_wxyz_from_xyzw\",\n \"quaternion_xyzw_from_wxyz\",\n \"quaternion_from_extrinsic_euler_xyz\",\n \"axis_angle_from_quaternion\",\n \"axis_angle_from_compact_axis_angle\",\n \"axis_angle_from_matrix\",\n \"axis_angle_from_two_directions\",\n \"compact_axis_angle\",\n \"compact_axis_angle_from_quaternion\",\n \"compact_axis_angle_from_matrix\",\n \"matrix_from_quaternion\",\n \"matrix_from_compact_axis_angle\",\n \"matrix_from_axis_angle\",\n \"matrix_from_two_vectors\",\n \"active_matrix_from_angle\",\n \"norm_euler\",\n \"euler_near_gimbal_lock\",\n \"matrix_from_euler\",\n \"active_matrix_from_extrinsic_euler_xyx\",\n \"active_matrix_from_intrinsic_euler_xyx\",\n \"active_matrix_from_extrinsic_euler_xyz\",\n \"active_matrix_from_extrinsic_euler_xzx\",\n \"active_matrix_from_extrinsic_euler_xzy\",\n \"active_matrix_from_extrinsic_euler_yxy\",\n \"active_matrix_from_extrinsic_euler_yxz\",\n \"active_matrix_from_extrinsic_euler_yzx\",\n \"active_matrix_from_extrinsic_euler_yzy\",\n \"active_matrix_from_extrinsic_euler_zxy\",\n \"active_matrix_from_extrinsic_euler_zxz\",\n \"active_matrix_from_extrinsic_euler_zyz\",\n \"active_matrix_from_intrinsic_euler_xyz\",\n \"active_matrix_from_intrinsic_euler_xzx\",\n \"active_matrix_from_extrinsic_euler_zyx\",\n \"active_matrix_from_intrinsic_euler_xzy\",\n \"active_matrix_from_intrinsic_euler_yxy\",\n \"active_matrix_from_intrinsic_euler_yxz\",\n \"active_matrix_from_intrinsic_euler_yzx\",\n \"active_matrix_from_intrinsic_euler_yzy\",\n \"active_matrix_from_intrinsic_euler_zxy\",\n \"active_matrix_from_intrinsic_euler_zxz\",\n \"active_matrix_from_intrinsic_euler_zyx\",\n \"active_matrix_from_intrinsic_euler_zyz\",\n \"active_matrix_from_extrinsic_roll_pitch_yaw\",\n \"passive_matrix_from_angle\",\n \"intrinsic_euler_xyz_from_active_matrix\",\n \"intrinsic_euler_xyx_from_active_matrix\",\n \"intrinsic_euler_xzx_from_active_matrix\",\n \"intrinsic_euler_xzy_from_active_matrix\",\n \"intrinsic_euler_yxy_from_active_matrix\",\n \"intrinsic_euler_yxz_from_active_matrix\",\n \"intrinsic_euler_yzx_from_active_matrix\",\n \"intrinsic_euler_yzy_from_active_matrix\",\n \"intrinsic_euler_zxy_from_active_matrix\",\n \"intrinsic_euler_zxz_from_active_matrix\",\n \"intrinsic_euler_zyx_from_active_matrix\",\n \"intrinsic_euler_zyz_from_active_matrix\",\n \"extrinsic_euler_xyx_from_active_matrix\",\n \"extrinsic_euler_xyz_from_active_matrix\",\n \"extrinsic_euler_xzx_from_active_matrix\",\n \"extrinsic_euler_xzy_from_active_matrix\",\n \"extrinsic_euler_yxy_from_active_matrix\",\n \"extrinsic_euler_yxz_from_active_matrix\",\n \"extrinsic_euler_yzx_from_active_matrix\",\n \"extrinsic_euler_yzy_from_active_matrix\",\n \"extrinsic_euler_zxy_from_active_matrix\",\n \"extrinsic_euler_zxz_from_active_matrix\",\n \"extrinsic_euler_zyx_from_active_matrix\",\n \"extrinsic_euler_zyz_from_active_matrix\",\n \"euler_from_matrix\",\n \"euler_from_quaternion\",\n \"quaternion_from_angle\",\n \"quaternion_from_euler\",\n \"mrp_near_singularity\",\n \"norm_mrp\",\n \"mrp_double\",\n \"concatenate_mrp\",\n \"mrp_prod_vector\",\n \"cross_product_matrix\",\n \"rot_log_from_compact_axis_angle\",\n \"mrp_from_quaternion\",\n \"quaternion_from_mrp\",\n \"mrp_from_axis_angle\",\n \"axis_angle_from_mrp\",\n \"quaternion_double\",\n \"quaternion_integrate\",\n \"quaternion_gradient\",\n \"concatenate_quaternions\",\n \"q_conj\",\n \"q_prod_vector\",\n \"quaternion_diff\",\n \"quaternion_dist\",\n \"matrix_slerp\",\n \"matrix_power\",\n \"slerp_weights\",\n \"pick_closest_quaternion\",\n \"quaternion_slerp\",\n \"axis_angle_slerp\",\n \"assert_euler_equal\",\n \"assert_quaternion_equal\",\n \"assert_axis_angle_equal\",\n \"assert_compact_axis_angle_equal\",\n \"assert_rotation_matrix\",\n \"assert_mrp_equal\",\n \"plot_basis\",\n \"plot_axis_angle\",\n \"wedge\",\n \"geometric_product\",\n \"wedge\",\n \"geometric_product\",\n \"rotor_apply\",\n \"rotor_reverse\",\n \"concatenate_rotors\",\n \"rotor_from_plane_angle\",\n \"rotor_from_two_directions\",\n \"matrix_from_rotor\",\n \"plot_bivector\",\n \"rotor_slerp\",\n \"plane_normal_from_bivector\",\n \"plane_basis_from_normal\",\n \"left_jacobian_SO3\",\n \"left_jacobian_SO3_series\",\n \"left_jacobian_SO3_inv\",\n \"left_jacobian_SO3_inv_series\",\n \"robust_polar_decomposition\",\n]\n" }, { "path": "pytransform3d/rotations/_angle.py", "content": "\"\"\"Angle operations.\"\"\"\n\nimport math\n\nimport numpy as np\n\nfrom ._constants import two_pi\n\n\ndef norm_angle(a):\n \"\"\"Normalize angle to (-pi, pi].\n\n It is worth noting that using `numpy.ceil` to normalize angles will lose\n more digits of precision as angles going larger but can keep more digits\n of precision when angles are around zero. In common use cases, for example,\n -10.0*pi to 10.0*pi, it performs well.\n\n For more discussions on numerical precision:\n https://github.com/dfki-ric/pytransform3d/pull/263\n\n Parameters\n ----------\n a : float or array-like, shape (n,)\n Angle(s) in radians\n\n Returns\n -------\n a_norm : float or array, shape (n,)\n Normalized angle(s) in radians\n \"\"\"\n a = np.asarray(a, dtype=np.float64)\n return a - (np.ceil((a + np.pi) / two_pi) - 1.0) * two_pi\n\n\ndef passive_matrix_from_angle(basis, angle):\n \"\"\"Compute passive rotation matrix from rotation about basis vector.\n\n Parameters\n ----------\n basis : int from [0, 1, 2]\n The rotation axis (0: x, 1: y, 2: z)\n\n angle : float\n Rotation angle\n\n Returns\n -------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n Raises\n ------\n ValueError\n If basis is invalid\n \"\"\"\n c = np.cos(angle)\n s = np.sin(angle)\n\n if basis == 0:\n R = np.array([[1.0, 0.0, 0.0], [0.0, c, s], [0.0, -s, c]])\n elif basis == 1:\n R = np.array([[c, 0.0, -s], [0.0, 1.0, 0.0], [s, 0.0, c]])\n elif basis == 2:\n R = np.array([[c, s, 0.0], [-s, c, 0.0], [0.0, 0.0, 1.0]])\n else:\n raise ValueError(\"Basis must be in [0, 1, 2]\")\n\n return R\n\n\ndef active_matrix_from_angle(basis, angle):\n r\"\"\"Compute active rotation matrix from rotation about basis vector.\n\n With the angle :math:`\\alpha` and :math:`s = \\sin{\\alpha}, c=\\cos{\\alpha}`,\n we construct rotation matrices about the basis vectors as follows:\n\n .. math::\n\n \\boldsymbol{R}_x(\\alpha) =\n \\left(\n \\begin{array}{ccc}\n 1 & 0 & 0\\\\\n 0 & c & -s\\\\\n 0 & s & c\n \\end{array}\n \\right)\n\n .. math::\n\n \\boldsymbol{R}_y(\\alpha) =\n \\left(\n \\begin{array}{ccc}\n c & 0 & s\\\\\n 0 & 1 & 0\\\\\n -s & 0 & c\n \\end{array}\n \\right)\n\n .. math::\n\n \\boldsymbol{R}_z(\\alpha) =\n \\left(\n \\begin{array}{ccc}\n c & -s & 0\\\\\n s & c & 0\\\\\n 0 & 0 & 1\n \\end{array}\n \\right)\n\n Parameters\n ----------\n basis : int from [0, 1, 2]\n The rotation axis (0: x, 1: y, 2: z)\n\n angle : float\n Rotation angle\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n\n Raises\n ------\n ValueError\n If basis is invalid\n \"\"\"\n c = np.cos(angle)\n s = np.sin(angle)\n\n if basis == 0:\n R = np.array([[1.0, 0.0, 0.0], [0.0, c, -s], [0.0, s, c]])\n elif basis == 1:\n R = np.array([[c, 0.0, s], [0.0, 1.0, 0.0], [-s, 0.0, c]])\n elif basis == 2:\n R = np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])\n else:\n raise ValueError(\"Basis must be in [0, 1, 2]\")\n\n return R\n\n\ndef quaternion_from_angle(basis, angle):\n \"\"\"Compute quaternion from rotation about basis vector.\n\n Parameters\n ----------\n basis : int from [0, 1, 2]\n The rotation axis (0: x, 1: y, 2: z)\n\n angle : float\n Rotation angle\n\n Returns\n -------\n q : array, shape (4,)\n Unit quaternion to represent rotation: (w, x, y, z)\n\n Raises\n ------\n ValueError\n If basis is invalid\n \"\"\"\n half_angle = 0.5 * angle\n c = math.cos(half_angle)\n s = math.sin(half_angle)\n\n if basis == 0:\n q = np.array([c, s, 0.0, 0.0])\n elif basis == 1:\n q = np.array([c, 0.0, s, 0.0])\n elif basis == 2:\n q = np.array([c, 0.0, 0.0, s])\n else:\n raise ValueError(\"Basis must be in [0, 1, 2]\")\n\n return q\n" }, { "path": "pytransform3d/rotations/_angle.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef norm_angle(a: npt.ArrayLike) -> np.ndarray: ...\ndef passive_matrix_from_angle(basis: int, angle: float) -> np.ndarray: ...\ndef active_matrix_from_angle(basis: int, angle: float) -> np.ndarray: ...\ndef quaternion_from_angle(basis: int, angle: float) -> np.ndarray: ...\n" }, { "path": "pytransform3d/rotations/_axis_angle.py", "content": "\"\"\"Axis-angle representation.\"\"\"\n\nimport math\n\nimport numpy as np\nfrom numpy.testing import assert_array_almost_equal\n\nfrom ._angle import norm_angle\nfrom ._constants import eps\nfrom ._utils import norm_vector, perpendicular_to_vector\n\n\ndef check_axis_angle(a):\n \"\"\"Input validation of axis-angle representation.\n\n Parameters\n ----------\n a : array-like, shape (4,)\n Axis of rotation and rotation angle: (x, y, z, angle)\n\n Returns\n -------\n a : array, shape (4,)\n Validated axis of rotation and rotation angle: (x, y, z, angle)\n\n Raises\n ------\n ValueError\n If input is invalid\n \"\"\"\n a = np.asarray(a, dtype=np.float64)\n if a.ndim != 1 or a.shape[0] != 4:\n raise ValueError(\n \"Expected axis and angle in array with shape (4,), \"\n \"got array-like object with shape %s\" % (a.shape,)\n )\n return norm_axis_angle(a)\n\n\ndef check_compact_axis_angle(a):\n \"\"\"Input validation of compact axis-angle representation.\n\n Parameters\n ----------\n a : array-like, shape (3,)\n Axis of rotation and rotation angle: angle * (x, y, z)\n\n Returns\n -------\n a : array, shape (3,)\n Validated axis of rotation and rotation angle: angle * (x, y, z)\n\n Raises\n ------\n ValueError\n If input is invalid\n \"\"\"\n a = np.asarray(a, dtype=np.float64)\n if a.ndim != 1 or a.shape[0] != 3:\n raise ValueError(\n \"Expected axis and angle in array with shape (3,), \"\n \"got array-like object with shape %s\" % (a.shape,)\n )\n return norm_compact_axis_angle(a)\n\n\ndef norm_axis_angle(a):\n \"\"\"Normalize axis-angle representation.\n\n Parameters\n ----------\n a : array-like, shape (4,)\n Axis of rotation and rotation angle: (x, y, z, angle)\n\n Returns\n -------\n a : array, shape (4,)\n Axis of rotation and rotation angle: (x, y, z, angle). The length\n of the axis vector is 1 and the angle is in [0, pi). No rotation\n is represented by [1, 0, 0, 0].\n \"\"\"\n angle = a[3]\n norm = np.linalg.norm(a[:3])\n if angle == 0.0 or norm == 0.0:\n return np.array([1.0, 0.0, 0.0, 0.0])\n\n res = np.empty(4)\n res[:3] = a[:3] / norm\n\n angle = norm_angle(angle)\n if angle < 0.0:\n angle *= -1.0\n res[:3] *= -1.0\n\n res[3] = angle\n\n return res\n\n\ndef norm_compact_axis_angle(a):\n \"\"\"Normalize compact axis-angle representation.\n\n Parameters\n ----------\n a : array-like, shape (3,)\n Axis of rotation and rotation angle: angle * (x, y, z)\n\n Returns\n -------\n a : array, shape (3,)\n Axis of rotation and rotation angle: angle * (x, y, z).\n The angle is in [0, pi). No rotation is represented by [0, 0, 0].\n \"\"\"\n angle = np.linalg.norm(a)\n if angle == 0.0:\n return np.zeros(3)\n axis = a / angle\n return axis * norm_angle(angle)\n\n\ndef compact_axis_angle_near_pi(a, tolerance=1e-6):\n r\"\"\"Check if angle of compact axis-angle representation is near pi.\n\n When the angle :math:`\\theta = \\pi`, both :math:`\\hat{\\boldsymbol{\\omega}}`\n and :math:`-\\hat{\\boldsymbol{\\omega}}` result in the same rotation. This\n ambiguity could lead to problems when averaging or interpolating.\n\n Parameters\n ----------\n a : array-like, shape (3,)\n Axis of rotation and rotation angle: angle * (x, y, z).\n\n tolerance : float\n Tolerance of this check.\n\n Returns\n -------\n near_pi : bool\n Angle is near pi.\n \"\"\"\n theta = np.linalg.norm(a)\n return abs(theta - np.pi) < tolerance\n\n\ndef assert_axis_angle_equal(a1, a2, *args, **kwargs):\n \"\"\"Raise an assertion if two axis-angle are not approximately equal.\n\n Usually we assume that the rotation axis is normalized to length 1 and\n the angle is within [0, pi). However, this function ignores these\n constraints and will normalize the representations before comparison.\n See numpy.testing.assert_array_almost_equal for a more detailed\n documentation of the other parameters.\n\n Parameters\n ----------\n a1 : array-like, shape (4,)\n Axis of rotation and rotation angle: (x, y, z, angle)\n\n a2 : array-like, shape (4,)\n Axis of rotation and rotation angle: (x, y, z, angle)\n\n args : tuple\n Positional arguments that will be passed to\n `assert_array_almost_equal`\n\n kwargs : dict\n Positional arguments that will be passed to\n `assert_array_almost_equal`\n \"\"\"\n a1 = norm_axis_angle(a1)\n a2 = norm_axis_angle(a2)\n # required despite normalization in case of 180 degree rotation\n if np.any(np.sign(a1) != np.sign(a2)):\n a1 = -a1\n a1 = norm_axis_angle(a1)\n assert_array_almost_equal(a1, a2, *args, **kwargs)\n\n\ndef assert_compact_axis_angle_equal(a1, a2, *args, **kwargs):\n \"\"\"Raise an assertion if two axis-angle are not approximately equal.\n\n Usually we assume that the angle is within [0, pi). However, this function\n ignores this constraint and will normalize the representations before\n comparison. See numpy.testing.assert_array_almost_equal for a more detailed\n documentation of the other parameters.\n\n Parameters\n ----------\n a1 : array-like, shape (3,)\n Axis of rotation and rotation angle: angle * (x, y, z)\n\n a2 : array-like, shape (3,)\n Axis of rotation and rotation angle: angle * (x, y, z)\n\n args : tuple\n Positional arguments that will be passed to\n `assert_array_almost_equal`\n\n kwargs : dict\n Positional arguments that will be passed to\n `assert_array_almost_equal`\n \"\"\"\n angle1 = np.linalg.norm(a1)\n angle2 = np.linalg.norm(a2)\n # required despite normalization in case of 180 degree rotation\n if (\n abs(angle1) == np.pi\n and abs(angle2) == np.pi\n and any(np.sign(a1) != np.sign(a2))\n ):\n a1 = -a1\n a1 = norm_compact_axis_angle(a1)\n a2 = norm_compact_axis_angle(a2)\n assert_array_almost_equal(a1, a2, *args, **kwargs)\n\n\ndef axis_angle_from_two_directions(a, b):\n \"\"\"Compute axis-angle representation from two direction vectors.\n\n The rotation will transform direction vector a to direction vector b.\n The direction vectors don't have to be normalized as this will be\n done internally. Note that there is more than one possible solution.\n\n Parameters\n ----------\n a : array-like, shape (3,)\n First direction vector\n\n b : array-like, shape (3,)\n Second direction vector\n\n Returns\n -------\n a : array, shape (4,)\n Axis of rotation and rotation angle: (x, y, z, angle). The angle is\n constrained to [0, pi].\n \"\"\"\n a = norm_vector(a)\n b = norm_vector(b)\n cos_angle = a.dot(b)\n if abs(-1.0 - cos_angle) < eps:\n # For 180 degree rotations we have an infinite number of solutions,\n # but we have to pick one axis.\n axis = perpendicular_to_vector(a)\n else:\n axis = np.cross(a, b)\n aa = np.empty(4)\n aa[:3] = norm_vector(axis)\n aa[3] = np.arccos(max(min(cos_angle, 1.0), -1.0))\n return norm_axis_angle(aa)\n\n\ndef matrix_from_axis_angle(a):\n r\"\"\"Compute rotation matrix from axis-angle.\n\n This is called exponential map or Rodrigues' formula.\n\n .. math::\n\n \\boldsymbol{R}(\\hat{\\boldsymbol{\\omega}}, \\theta)\n =\n Exp(\\hat{\\boldsymbol{\\omega}} \\theta)\n =\n \\cos{\\theta} \\boldsymbol{I}\n + \\sin{\\theta} \\left[\\hat{\\boldsymbol{\\omega}}\\right]\n + (1 - \\cos{\\theta})\n \\hat{\\boldsymbol{\\omega}}\\hat{\\boldsymbol{\\omega}}^T\n =\n \\boldsymbol{I}\n + \\sin{\\theta} \\left[\\hat{\\boldsymbol{\\omega}}\\right]\n + (1 - \\cos{\\theta}) \\left[\\hat{\\boldsymbol{\\omega}}\\right]^2\n\n This typically results in an active rotation matrix.\n\n Parameters\n ----------\n a : array-like, shape (4,)\n Axis of rotation and rotation angle: (x, y, z, angle)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n a = check_axis_angle(a)\n ux, uy, uz, theta = a\n c = math.cos(theta)\n s = math.sin(theta)\n ci = 1.0 - c\n R = np.array(\n [\n [ci * ux * ux + c, ci * ux * uy - uz * s, ci * ux * uz + uy * s],\n [ci * uy * ux + uz * s, ci * uy * uy + c, ci * uy * uz - ux * s],\n [ci * uz * ux - uy * s, ci * uz * uy + ux * s, ci * uz * uz + c],\n ]\n )\n\n # This is equivalent to\n # R = (np.eye(3) * np.cos(a[3]) +\n # (1.0 - np.cos(a[3])) * a[:3, np.newaxis].dot(a[np.newaxis, :3]) +\n # cross_product_matrix(a[:3]) * np.sin(a[3]))\n # or\n # w = cross_product_matrix(a[:3])\n # R = np.eye(3) + np.sin(a[3]) * w + (1.0 - np.cos(a[3])) * w.dot(w)\n\n return R\n\n\ndef matrix_from_compact_axis_angle(a):\n r\"\"\"Compute rotation matrix from compact axis-angle.\n\n This is called exponential map or Rodrigues' formula.\n\n .. math::\n\n Exp(\\hat{\\boldsymbol{\\omega}} \\theta)\n =\n \\cos{\\theta} \\boldsymbol{I}\n + \\sin{\\theta} \\left[\\hat{\\boldsymbol{\\omega}}\\right]\n + (1 - \\cos{\\theta})\n \\hat{\\boldsymbol{\\omega}}\\hat{\\boldsymbol{\\omega}}^T\n =\n \\boldsymbol{I}\n + \\sin{\\theta} \\left[\\hat{\\boldsymbol{\\omega}}\\right]\n + (1 - \\cos{\\theta}) \\left[\\hat{\\boldsymbol{\\omega}}\\right]^2\n\n This typically results in an active rotation matrix.\n\n Parameters\n ----------\n a : array-like, shape (3,)\n Axis of rotation and rotation angle: angle * (x, y, z)\n\n Returns\n -------\n R : array-like, shape (3, 3)\n Rotation matrix\n \"\"\"\n a = axis_angle_from_compact_axis_angle(a)\n return matrix_from_axis_angle(a)\n\n\ndef axis_angle_from_compact_axis_angle(a):\n \"\"\"Compute axis-angle from compact axis-angle representation.\n\n We usually assume active rotations.\n\n Parameters\n ----------\n a : array-like, shape (3,)\n Axis of rotation and rotation angle: angle * (x, y, z).\n\n Returns\n -------\n a : array, shape (4,)\n Axis of rotation and rotation angle: (x, y, z, angle). The angle is\n constrained to [0, pi].\n \"\"\"\n a = check_compact_axis_angle(a)\n angle = np.linalg.norm(a)\n\n if angle == 0.0:\n return np.array([1.0, 0.0, 0.0, 0.0])\n\n axis = a / angle\n return np.hstack((axis, (angle,)))\n\n\ndef compact_axis_angle(a):\n r\"\"\"Compute 3-dimensional axis-angle from a 4-dimensional one.\n\n In the 3-dimensional axis-angle representation, the 4th dimension (the\n rotation) is represented by the norm of the rotation axis vector, which\n means we map :math:`\\left( \\hat{\\boldsymbol{\\omega}}, \\theta \\right)` to\n :math:`\\boldsymbol{\\omega} = \\theta \\hat{\\boldsymbol{\\omega}}`.\n\n This representation is also called rotation vector or exponential\n coordinates of rotation.\n\n Parameters\n ----------\n a : array-like, shape (4,)\n Axis of rotation and rotation angle: (x, y, z, angle).\n\n Returns\n -------\n a : array, shape (3,)\n Axis of rotation and rotation angle: angle * (x, y, z) (compact\n representation).\n \"\"\"\n a = check_axis_angle(a)\n return a[:3] * a[3]\n\n\ndef quaternion_from_axis_angle(a):\n \"\"\"Compute quaternion from axis-angle.\n\n This operation is called exponential map.\n\n We usually assume active rotations.\n\n Parameters\n ----------\n a : array-like, shape (4,)\n Axis of rotation and rotation angle: (x, y, z, angle)\n\n Returns\n -------\n q : array, shape (4,)\n Unit quaternion to represent rotation: (w, x, y, z)\n \"\"\"\n a = check_axis_angle(a)\n half_angle = 0.5 * a[3]\n\n q = np.empty(4)\n q[0] = np.cos(half_angle)\n q[1:] = np.sin(half_angle) * a[:3]\n return q\n\n\ndef quaternion_from_compact_axis_angle(a):\n \"\"\"Compute quaternion from compact axis-angle (exponential map).\n\n We usually assume active rotations.\n\n Parameters\n ----------\n a : array-like, shape (4,)\n Axis of rotation and rotation angle: angle * (x, y, z)\n\n Returns\n -------\n q : array, shape (4,)\n Unit quaternion to represent rotation: (w, x, y, z)\n \"\"\"\n a = axis_angle_from_compact_axis_angle(a)\n return quaternion_from_axis_angle(a)\n\n\ndef mrp_from_axis_angle(a):\n r\"\"\"Compute modified Rodrigues parameters from axis-angle representation.\n\n .. math::\n\n \\boldsymbol{\\psi} = \\tan \\left(\\frac{\\theta}{4}\\right)\n \\hat{\\boldsymbol{\\omega}}\n\n Parameters\n ----------\n a : array-like, shape (4,)\n Axis of rotation and rotation angle: (x, y, z, angle)\n\n Returns\n -------\n mrp : array, shape (3,)\n Modified Rodrigues parameters.\n \"\"\"\n a = check_axis_angle(a)\n return np.tan(0.25 * a[3]) * a[:3]\n" }, { "path": "pytransform3d/rotations/_axis_angle.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef norm_axis_angle(a: npt.ArrayLike) -> np.ndarray: ...\ndef norm_compact_axis_angle(a: npt.ArrayLike) -> np.ndarray: ...\ndef compact_axis_angle_near_pi(\n a: npt.ArrayLike, tolerance: float = ...\n) -> bool: ...\ndef check_axis_angle(a: npt.ArrayLike) -> np.ndarray: ...\ndef check_compact_axis_angle(a: npt.ArrayLike) -> np.ndarray: ...\ndef assert_axis_angle_equal(\n a1: npt.ArrayLike, a2: npt.ArrayLike, *args, **kwargs\n): ...\ndef assert_compact_axis_angle_equal(\n a1: npt.ArrayLike, a2: npt.ArrayLike, *args, **kwargs\n): ...\ndef axis_angle_from_two_directions(\n a: npt.ArrayLike, b: npt.ArrayLike\n) -> np.ndarray: ...\ndef matrix_from_axis_angle(a: npt.ArrayLike) -> np.ndarray: ...\ndef matrix_from_compact_axis_angle(a: npt.ArrayLike) -> np.ndarray: ...\ndef axis_angle_from_compact_axis_angle(a: npt.ArrayLike) -> np.ndarray: ...\ndef compact_axis_angle(a: npt.ArrayLike) -> np.ndarray: ...\ndef quaternion_from_axis_angle(a: npt.ArrayLike) -> np.ndarray: ...\ndef quaternion_from_compact_axis_angle(a: npt.ArrayLike) -> np.ndarray: ...\ndef mrp_from_axis_angle(a: npt.ArrayLike) -> np.ndarray: ...\n" }, { "path": "pytransform3d/rotations/_constants.py", "content": "\"\"\"Constants of rotation module.\"\"\"\n\nimport numpy as np\n\n\nunitx = np.array([1.0, 0.0, 0.0])\nunity = np.array([0.0, 1.0, 0.0])\nunitz = np.array([0.0, 0.0, 1.0])\n\nR_id = np.eye(3)\na_id = np.array([1.0, 0.0, 0.0, 0.0])\nq_id = np.array([1.0, 0.0, 0.0, 0.0])\nq_i = np.array([0.0, 1.0, 0.0, 0.0])\nq_j = np.array([0.0, 0.0, 1.0, 0.0])\nq_k = np.array([0.0, 0.0, 0.0, 1.0])\np0 = np.array([0.0, 0.0, 0.0])\n\neps = 1e-7\n\ntwo_pi = 2.0 * np.pi\nhalf_pi = 0.5 * np.pi\n" }, { "path": "pytransform3d/rotations/_euler.py", "content": "\"\"\"Euler angles.\"\"\"\n\nimport math\n\nimport numpy as np\nfrom numpy.testing import assert_array_almost_equal\n\nfrom ._angle import norm_angle, active_matrix_from_angle\nfrom ._constants import half_pi, unitx, unity, unitz, eps\nfrom ._matrix import check_matrix\nfrom ._quaternion import check_quaternion\nfrom ._utils import check_axis_index\n\n\ndef norm_euler(e, i, j, k):\n \"\"\"Normalize Euler angle range.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Rotation angles in radians about the axes i, j, k in this order.\n\n i : int from [0, 1, 2]\n The first rotation axis (0: x, 1: y, 2: z)\n\n j : int from [0, 1, 2]\n The second rotation axis (0: x, 1: y, 2: z)\n\n k : int from [0, 1, 2]\n The third rotation axis (0: x, 1: y, 2: z)\n\n Returns\n -------\n e : array, shape (3,)\n Extracted rotation angles in radians about the axes i, j, k in this\n order. The first and last angle are normalized to [-pi, pi]. The middle\n angle is normalized to either [0, pi] (proper Euler angles) or\n [-pi/2, pi/2] (Cardan / Tait-Bryan angles).\n \"\"\"\n check_axis_index(\"i\", i)\n check_axis_index(\"j\", j)\n check_axis_index(\"k\", k)\n\n alpha, beta, gamma = norm_angle(e)\n\n proper_euler = i == k\n if proper_euler:\n if beta < 0.0:\n alpha += np.pi\n beta *= -1.0\n gamma -= np.pi\n elif abs(beta) > half_pi:\n alpha += np.pi\n beta = np.pi - beta\n gamma -= np.pi\n\n return norm_angle([alpha, beta, gamma])\n\n\ndef euler_near_gimbal_lock(e, i, j, k, tolerance=1e-6):\n \"\"\"Check if Euler angles are close to gimbal lock.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Rotation angles in radians about the axes i, j, k in this order.\n\n i : int from [0, 1, 2]\n The first rotation axis (0: x, 1: y, 2: z)\n\n j : int from [0, 1, 2]\n The second rotation axis (0: x, 1: y, 2: z)\n\n k : int from [0, 1, 2]\n The third rotation axis (0: x, 1: y, 2: z)\n\n tolerance : float\n Tolerance for the comparison.\n\n Returns\n -------\n near_gimbal_lock : bool\n Indicates if the Euler angles are near the gimbal lock singularity.\n \"\"\"\n e = norm_euler(e, i, j, k)\n beta = e[1]\n proper_euler = i == k\n if proper_euler:\n return abs(beta) < tolerance or abs(beta - np.pi) < tolerance\n else:\n return abs(abs(beta) - half_pi) < tolerance\n\n\ndef assert_euler_equal(e1, e2, i, j, k, *args, **kwargs):\n \"\"\"Raise an assertion if two Euler angles are not approximately equal.\n\n Parameters\n ----------\n e1 : array-like, shape (3,)\n Rotation angles in radians about the axes i, j, k in this order.\n\n e2 : array-like, shape (3,)\n Rotation angles in radians about the axes i, j, k in this order.\n\n i : int from [0, 1, 2]\n The first rotation axis (0: x, 1: y, 2: z)\n\n j : int from [0, 1, 2]\n The second rotation axis (0: x, 1: y, 2: z)\n\n k : int from [0, 1, 2]\n The third rotation axis (0: x, 1: y, 2: z)\n\n args : tuple\n Positional arguments that will be passed to\n `assert_array_almost_equal`\n\n kwargs : dict\n Positional arguments that will be passed to\n `assert_array_almost_equal`\n \"\"\"\n e1 = norm_euler(e1, i, j, k)\n e2 = norm_euler(e2, i, j, k)\n assert_array_almost_equal(e1, e2, *args, **kwargs)\n\n\ndef matrix_from_euler(e, i, j, k, extrinsic):\n \"\"\"General method to compute active rotation matrix from any Euler angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Rotation angles in radians about the axes i, j, k in this order.\n\n i : int from [0, 1, 2]\n The first rotation axis (0: x, 1: y, 2: z)\n\n j : int from [0, 1, 2]\n The second rotation axis (0: x, 1: y, 2: z)\n\n k : int from [0, 1, 2]\n The third rotation axis (0: x, 1: y, 2: z)\n\n extrinsic : bool\n Do we use extrinsic transformations? Intrinsic otherwise.\n\n Returns\n -------\n R : array, shape (3, 3)\n Active rotation matrix\n\n Raises\n ------\n ValueError\n If basis is invalid\n \"\"\"\n check_axis_index(\"i\", i)\n check_axis_index(\"j\", j)\n check_axis_index(\"k\", k)\n\n alpha, beta, gamma = e\n if not extrinsic:\n i, k = k, i\n alpha, gamma = gamma, alpha\n R = (\n active_matrix_from_angle(k, gamma)\n .dot(active_matrix_from_angle(j, beta))\n .dot(active_matrix_from_angle(i, alpha))\n )\n return R\n\n\ndef general_intrinsic_euler_from_active_matrix(\n R, n1, n2, n3, proper_euler, strict_check=True\n):\n \"\"\"General algorithm to extract intrinsic euler angles from a matrix.\n\n The implementation is based on SciPy's implementation:\n https://github.com/scipy/scipy/blob/743c283bbe79473a03ca2eddaa537661846d8a19/scipy/spatial/transform/_rotation.pyx\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Active rotation matrix\n\n n1 : array, shape (3,)\n First rotation axis (basis vector)\n\n n2 : array, shape (3,)\n Second rotation axis (basis vector)\n\n n3 : array, shape (3,)\n Third rotation axis (basis vector)\n\n proper_euler : bool\n Is this an Euler angle convention or a Cardan / Tait-Bryan convention?\n Proper Euler angles rotate about the same axis twice, for example,\n z, y', and z''.\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n euler_angles : array, shape (3,)\n Extracted intrinsic rotation angles in radians about the axes\n n1, n2, and n3 in this order. The first and last angle are\n normalized to [-pi, pi]. The middle angle is normalized to\n either [0, pi] (proper Euler angles) or [-pi/2, pi/2]\n (Cardan / Tait-Bryan angles).\n\n References\n ----------\n .. [1] Shuster, M. D., Markley, F. L. (2006).\n General Formula for Extracting the Euler Angles.\n Journal of Guidance, Control, and Dynamics, 29(1), pp 2015-221,\n doi: 10.2514/1.16622. https://arc.aiaa.org/doi/abs/10.2514/1.16622\n \"\"\"\n D = check_matrix(R, strict_check=strict_check)\n\n # Differences to the paper:\n # - we call the angles alpha, beta, and gamma\n # - we obtain angles from intrinsic rotations, thus some matrices are\n # transposed like in SciPy's implementation\n\n # Step 2\n # - Equation 5\n n1_cross_n2 = np.cross(n1, n2)\n lmbda = np.arctan2(np.dot(n1_cross_n2, n3), np.dot(n1, n3))\n # - Equation 6\n C = np.vstack((n2, n1_cross_n2, n1))\n\n # Step 3\n # - Equation 8\n CDCT = np.dot(np.dot(C, D), C.T)\n O = np.dot(CDCT, active_matrix_from_angle(0, lmbda).T)\n\n # Step 4\n # Fix numerical issue if O_22 is slightly out of range of arccos\n O_22 = max(min(O[2, 2], 1.0), -1.0)\n # - Equation 10a\n beta = lmbda + np.arccos(O_22)\n\n safe1 = abs(beta - lmbda) >= np.finfo(float).eps\n safe2 = abs(beta - lmbda - np.pi) >= np.finfo(float).eps\n if safe1 and safe2: # Default case, no gimbal lock\n # Step 5\n # - Equation 10b\n alpha = np.arctan2(O[0, 2], -O[1, 2])\n # - Equation 10c\n gamma = np.arctan2(O[2, 0], O[2, 1])\n\n # Step 7\n if proper_euler:\n valid_beta = 0.0 <= beta <= np.pi\n else: # Cardan / Tait-Bryan angles\n valid_beta = -0.5 * np.pi <= beta <= 0.5 * np.pi\n # - Equation 12\n if not valid_beta:\n alpha += np.pi\n beta = 2.0 * lmbda - beta\n gamma -= np.pi\n else:\n # Step 6 - Handle gimbal locks\n # a)\n gamma = 0.0\n if not safe1:\n # b)\n alpha = np.arctan2(O[1, 0] - O[0, 1], O[0, 0] + O[1, 1])\n else:\n # c)\n alpha = np.arctan2(O[1, 0] + O[0, 1], O[0, 0] - O[1, 1])\n euler_angles = norm_angle([alpha, beta, gamma])\n return euler_angles\n\n\ndef euler_from_matrix(R, i, j, k, extrinsic, strict_check=True):\n \"\"\"General method to extract any Euler angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Active rotation matrix\n\n i : int from [0, 1, 2]\n The first rotation axis (0: x, 1: y, 2: z)\n\n j : int from [0, 1, 2]\n The second rotation axis (0: x, 1: y, 2: z)\n\n k : int from [0, 1, 2]\n The third rotation axis (0: x, 1: y, 2: z)\n\n extrinsic : bool\n Do we use extrinsic transformations? Intrinsic otherwise.\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n euler_angles : array, shape (3,)\n Extracted rotation angles in radians about the axes i, j, k in this\n order. The first and last angle are normalized to [-pi, pi]. The middle\n angle is normalized to either [0, pi] (proper Euler angles) or\n [-pi/2, pi/2] (Cardan / Tait-Bryan angles).\n\n Raises\n ------\n ValueError\n If basis is invalid\n\n References\n ----------\n .. [1] Shuster, M. D., Markley, F. L. (2006).\n General Formula for Extracting the Euler Angles.\n Journal of Guidance, Control, and Dynamics, 29(1), pp 2015-221,\n doi: 10.2514/1.16622. https://arc.aiaa.org/doi/abs/10.2514/1.16622\n \"\"\"\n check_axis_index(\"i\", i)\n check_axis_index(\"j\", j)\n check_axis_index(\"k\", k)\n\n basis_vectors = [unitx, unity, unitz]\n proper_euler = i == k\n if extrinsic:\n i, k = k, i\n e = general_intrinsic_euler_from_active_matrix(\n R,\n basis_vectors[i],\n basis_vectors[j],\n basis_vectors[k],\n proper_euler,\n strict_check,\n )\n\n if extrinsic:\n e = e[::-1]\n\n return e\n\n\ndef euler_from_quaternion(q, i, j, k, extrinsic):\n \"\"\"General method to extract any Euler angles from quaternions.\n\n Parameters\n ----------\n q : array-like, shape (4,)\n Unit quaternion to represent rotation: (w, x, y, z)\n\n i : int from [0, 1, 2]\n The first rotation axis (0: x, 1: y, 2: z)\n\n j : int from [0, 1, 2]\n The second rotation axis (0: x, 1: y, 2: z)\n\n k : int from [0, 1, 2]\n The third rotation axis (0: x, 1: y, 2: z)\n\n extrinsic : bool\n Do we use extrinsic transformations? Intrinsic otherwise.\n\n Returns\n -------\n euler_angles : array, shape (3,)\n Extracted rotation angles in radians about the axes i, j, k in this\n order. The first and last angle are normalized to [-pi, pi]. The middle\n angle is normalized to either [0, pi] (proper Euler angles) or\n [-pi/2, pi/2] (Cardan / Tait-Bryan angles).\n\n Raises\n ------\n ValueError\n If basis is invalid\n\n References\n ----------\n .. [1] Bernardes, E., Viollet, S. (2022). Quaternion to Euler angles\n conversion: A direct, general and computationally efficient method.\n PLOS ONE, 17(11), doi: 10.1371/journal.pone.0276302.\n \"\"\"\n q = check_quaternion(q)\n\n check_axis_index(\"i\", i)\n check_axis_index(\"j\", j)\n check_axis_index(\"k\", k)\n\n i += 1\n j += 1\n k += 1\n\n # The original algorithm assumes extrinsic convention. Hence, we swap\n # the order of axes for intrinsic rotation.\n if not extrinsic:\n i, k = k, i\n\n # Proper Euler angles rotate about the same axis in the first and last\n # rotation. If this is not the case, they are called Cardan or Tait-Bryan\n # angles and have to be handled differently.\n proper_euler = i == k\n if proper_euler:\n k = 6 - i - j\n\n sign = (i - j) * (j - k) * (k - i) // 2\n a = q[0]\n b = q[i]\n c = q[j]\n d = q[k] * sign\n\n if not proper_euler:\n a, b, c, d = a - c, b + d, c + a, d - b\n\n # Equation 34 is used instead of Equation 35 as atan2 it is numerically\n # more accurate than acos.\n angle_j = 2.0 * math.atan2(math.hypot(c, d), math.hypot(a, b))\n\n # Check for singularities\n if abs(angle_j) <= eps:\n singularity = 1\n elif abs(angle_j - math.pi) <= eps:\n singularity = 2\n else:\n singularity = 0\n\n # Equation 25\n # (theta_1 + theta_3) / 2\n half_sum = math.atan2(b, a)\n # (theta_1 - theta_3) / 2\n half_diff = math.atan2(d, c)\n\n if singularity == 0: # no singularity\n # Equation 32\n angle_i = half_sum + half_diff\n angle_k = half_sum - half_diff\n elif extrinsic: # singularity\n angle_k = 0.0\n if singularity == 1:\n angle_i = 2.0 * half_sum\n else:\n assert singularity == 2\n angle_i = 2.0 * half_diff\n else: # intrinsic, singularity\n angle_i = 0.0\n if singularity == 1:\n angle_k = 2.0 * half_sum\n else:\n assert singularity == 2\n angle_k = -2.0 * half_diff\n\n if not proper_euler:\n # Equation 43\n angle_j -= math.pi / 2.0\n # Equation 44\n angle_i *= sign\n\n angle_k = norm_angle(angle_k)\n angle_i = norm_angle(angle_i)\n\n if extrinsic:\n return np.array([angle_k, angle_j, angle_i])\n\n return np.array([angle_i, angle_j, angle_k])\n" }, { "path": "pytransform3d/rotations/_euler.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef norm_euler(e: npt.ArrayLike, i: int, j: int, k: int) -> np.ndarray: ...\ndef euler_near_gimbal_lock(\n e: npt.ArrayLike, i: int, j: int, k: int, tolerance: float = ...\n) -> bool: ...\ndef assert_euler_equal(\n e1: npt.ArrayLike,\n e2: npt.ArrayLike,\n i: int,\n j: int,\n k: int,\n *args,\n **kwargs,\n): ...\ndef matrix_from_euler(\n e: npt.ArrayLike, i: int, j: int, k: int, extrinsic: bool\n) -> np.ndarray: ...\ndef general_intrinsic_euler_from_active_matrix(\n R: npt.ArrayLike,\n n1: np.ndarray,\n n2: np.ndarray,\n n3: np.ndarray,\n proper_euler: bool,\n strict_check: bool = ...,\n) -> np.ndarray: ...\ndef euler_from_matrix(\n R: npt.ArrayLike,\n i: int,\n j: int,\n k: int,\n extrinsic: bool,\n strict_check: bool = ...,\n) -> np.ndarray: ...\ndef euler_from_quaternion(\n q: npt.ArrayLike, i: int, j: int, k: int, extrinsic: bool\n) -> np.ndarray: ...\n" }, { "path": "pytransform3d/rotations/_euler_deprecated.py", "content": "\"\"\"Deprecated Euler angle functions.\"\"\"\n\nimport warnings\n\nfrom ._angle import active_matrix_from_angle\nfrom ._constants import unitx, unity, unitz\nfrom ._euler import general_intrinsic_euler_from_active_matrix\nfrom ._matrix import quaternion_from_matrix\n\n\ndef active_matrix_from_intrinsic_euler_xzx(e):\n \"\"\"Compute active rotation matrix from intrinsic xzx Euler angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around x-, z'-, and x''-axes (intrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(0, alpha)\n .dot(active_matrix_from_angle(2, beta))\n .dot(active_matrix_from_angle(0, gamma))\n )\n return R\n\n\ndef active_matrix_from_extrinsic_euler_xzx(e):\n \"\"\"Compute active rotation matrix from extrinsic xzx Euler angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around x-, z-, and x-axes (extrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(0, gamma)\n .dot(active_matrix_from_angle(2, beta))\n .dot(active_matrix_from_angle(0, alpha))\n )\n return R\n\n\ndef active_matrix_from_intrinsic_euler_xyx(e):\n \"\"\"Compute active rotation matrix from intrinsic xyx Euler angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around x-, y'-, and x''-axes (intrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(0, alpha)\n .dot(active_matrix_from_angle(1, beta))\n .dot(active_matrix_from_angle(0, gamma))\n )\n return R\n\n\ndef active_matrix_from_extrinsic_euler_xyx(e):\n \"\"\"Compute active rotation matrix from extrinsic xyx Euler angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around x-, y-, and x-axes (extrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(0, gamma)\n .dot(active_matrix_from_angle(1, beta))\n .dot(active_matrix_from_angle(0, alpha))\n )\n return R\n\n\ndef active_matrix_from_intrinsic_euler_yxy(e):\n \"\"\"Compute active rotation matrix from intrinsic yxy Euler angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around y-, x'-, and y''-axes (intrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(1, alpha)\n .dot(active_matrix_from_angle(0, beta))\n .dot(active_matrix_from_angle(1, gamma))\n )\n return R\n\n\ndef active_matrix_from_extrinsic_euler_yxy(e):\n \"\"\"Compute active rotation matrix from extrinsic yxy Euler angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around y-, x-, and y-axes (extrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(1, gamma)\n .dot(active_matrix_from_angle(0, beta))\n .dot(active_matrix_from_angle(1, alpha))\n )\n return R\n\n\ndef active_matrix_from_intrinsic_euler_yzy(e):\n \"\"\"Compute active rotation matrix from intrinsic yzy Euler angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around y-, z'-, and y''-axes (intrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(1, alpha)\n .dot(active_matrix_from_angle(2, beta))\n .dot(active_matrix_from_angle(1, gamma))\n )\n return R\n\n\ndef active_matrix_from_extrinsic_euler_yzy(e):\n \"\"\"Compute active rotation matrix from extrinsic yzy Euler angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around y-, z-, and y-axes (extrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(1, gamma)\n .dot(active_matrix_from_angle(2, beta))\n .dot(active_matrix_from_angle(1, alpha))\n )\n return R\n\n\ndef active_matrix_from_intrinsic_euler_zyz(e):\n \"\"\"Compute active rotation matrix from intrinsic zyz Euler angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around z-, y'-, and z''-axes (intrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(2, alpha)\n .dot(active_matrix_from_angle(1, beta))\n .dot(active_matrix_from_angle(2, gamma))\n )\n return R\n\n\ndef active_matrix_from_extrinsic_euler_zyz(e):\n \"\"\"Compute active rotation matrix from extrinsic zyz Euler angles.\n\n .. warning::\n\n This function was not implemented correctly in versions 1.3 and 1.4\n as the order of the angles was reversed, which actually corresponds\n to intrinsic rotations. This has been fixed in version 1.5.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around z-, y-, and z-axes (extrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(2, gamma)\n .dot(active_matrix_from_angle(1, beta))\n .dot(active_matrix_from_angle(2, alpha))\n )\n return R\n\n\ndef active_matrix_from_intrinsic_euler_zxz(e):\n \"\"\"Compute active rotation matrix from intrinsic zxz Euler angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around z-, x'-, and z''-axes (intrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(2, alpha)\n .dot(active_matrix_from_angle(0, beta))\n .dot(active_matrix_from_angle(2, gamma))\n )\n return R\n\n\ndef active_matrix_from_extrinsic_euler_zxz(e):\n \"\"\"Compute active rotation matrix from extrinsic zxz Euler angles.\n\n .. warning::\n\n This function was not implemented correctly in versions 1.3 and 1.4\n as the order of the angles was reversed, which actually corresponds\n to intrinsic rotations. This has been fixed in version 1.5.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around z-, x-, and z-axes (extrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(2, gamma)\n .dot(active_matrix_from_angle(0, beta))\n .dot(active_matrix_from_angle(2, alpha))\n )\n return R\n\n\ndef active_matrix_from_intrinsic_euler_xzy(e):\n \"\"\"Compute active rotation matrix from intrinsic xzy Cardan angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around x-, z'-, and y''-axes (intrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(0, alpha)\n .dot(active_matrix_from_angle(2, beta))\n .dot(active_matrix_from_angle(1, gamma))\n )\n return R\n\n\ndef active_matrix_from_extrinsic_euler_xzy(e):\n \"\"\"Compute active rotation matrix from extrinsic xzy Cardan angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around x-, z-, and y-axes (extrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(1, gamma)\n .dot(active_matrix_from_angle(2, beta))\n .dot(active_matrix_from_angle(0, alpha))\n )\n return R\n\n\ndef active_matrix_from_intrinsic_euler_xyz(e):\n \"\"\"Compute active rotation matrix from intrinsic xyz Cardan angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around x-, y'-, and z''-axes (intrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(0, alpha)\n .dot(active_matrix_from_angle(1, beta))\n .dot(active_matrix_from_angle(2, gamma))\n )\n return R\n\n\ndef active_matrix_from_extrinsic_euler_xyz(e):\n \"\"\"Compute active rotation matrix from extrinsic xyz Cardan angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around x-, y-, and z-axes (extrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(2, gamma)\n .dot(active_matrix_from_angle(1, beta))\n .dot(active_matrix_from_angle(0, alpha))\n )\n return R\n\n\ndef active_matrix_from_intrinsic_euler_yxz(e):\n \"\"\"Compute active rotation matrix from intrinsic yxz Cardan angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around y-, x'-, and z''-axes (intrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(1, alpha)\n .dot(active_matrix_from_angle(0, beta))\n .dot(active_matrix_from_angle(2, gamma))\n )\n return R\n\n\ndef active_matrix_from_extrinsic_euler_yxz(e):\n \"\"\"Compute active rotation matrix from extrinsic yxz Cardan angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around y-, x-, and z-axes (extrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(2, gamma)\n .dot(active_matrix_from_angle(0, beta))\n .dot(active_matrix_from_angle(1, alpha))\n )\n return R\n\n\ndef active_matrix_from_intrinsic_euler_yzx(e):\n \"\"\"Compute active rotation matrix from intrinsic yzx Cardan angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around y-, z'-, and x''-axes (intrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(1, alpha)\n .dot(active_matrix_from_angle(2, beta))\n .dot(active_matrix_from_angle(0, gamma))\n )\n return R\n\n\ndef active_matrix_from_extrinsic_euler_yzx(e):\n \"\"\"Compute active rotation matrix from extrinsic yzx Cardan angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around y-, z-, and x-axes (extrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(0, gamma)\n .dot(active_matrix_from_angle(2, beta))\n .dot(active_matrix_from_angle(1, alpha))\n )\n return R\n\n\ndef active_matrix_from_intrinsic_euler_zyx(e):\n \"\"\"Compute active rotation matrix from intrinsic zyx Cardan angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around z-, y'-, and x''-axes (intrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(2, alpha)\n .dot(active_matrix_from_angle(1, beta))\n .dot(active_matrix_from_angle(0, gamma))\n )\n return R\n\n\ndef active_matrix_from_extrinsic_euler_zyx(e):\n \"\"\"Compute active rotation matrix from extrinsic zyx Cardan angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around z-, y-, and x-axes (extrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(0, gamma)\n .dot(active_matrix_from_angle(1, beta))\n .dot(active_matrix_from_angle(2, alpha))\n )\n return R\n\n\ndef active_matrix_from_intrinsic_euler_zxy(e):\n \"\"\"Compute active rotation matrix from intrinsic zxy Cardan angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around z-, x'-, and y''-axes (intrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(2, alpha)\n .dot(active_matrix_from_angle(0, beta))\n .dot(active_matrix_from_angle(1, gamma))\n )\n return R\n\n\ndef active_matrix_from_extrinsic_euler_zxy(e):\n \"\"\"Compute active rotation matrix from extrinsic zxy Cardan angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around z-, x-, and y-axes (extrinsic rotations)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n alpha, beta, gamma = e\n R = (\n active_matrix_from_angle(1, gamma)\n .dot(active_matrix_from_angle(0, beta))\n .dot(active_matrix_from_angle(2, alpha))\n )\n return R\n\n\ndef active_matrix_from_extrinsic_roll_pitch_yaw(rpy):\n \"\"\"Compute active rotation matrix from extrinsic roll, pitch, and yaw.\n\n Parameters\n ----------\n rpy : array-like, shape (3,)\n Angles for rotation around x- (roll), y- (pitch), and z-axes (yaw),\n extrinsic rotations\n\n Returns\n -------\n R : array-like, shape (3, 3)\n Rotation matrix\n \"\"\"\n warnings.warn(\n \"function is deprecated, use matrix_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n return active_matrix_from_extrinsic_euler_xyz(rpy)\n\n\ndef intrinsic_euler_xzx_from_active_matrix(R, strict_check=True):\n \"\"\"Compute intrinsic xzx Euler angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around x-, z'-, and x''-axes (intrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unitx, unitz, unitx, True, strict_check\n )\n\n\ndef extrinsic_euler_xzx_from_active_matrix(R, strict_check=True):\n \"\"\"Compute extrinsic xzx Euler angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around x-, z-, and x-axes (extrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unitx, unitz, unitx, True, strict_check\n )[::-1]\n\n\ndef intrinsic_euler_xyx_from_active_matrix(R, strict_check=True):\n \"\"\"Compute intrinsic xyx Euler angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around x-, y'-, and x''-axes (intrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unitx, unity, unitx, True, strict_check\n )\n\n\ndef extrinsic_euler_xyx_from_active_matrix(R, strict_check=True):\n \"\"\"Compute extrinsic xyx Euler angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around x-, y-, and x-axes (extrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unitx, unity, unitx, True, strict_check\n )[::-1]\n\n\ndef intrinsic_euler_yxy_from_active_matrix(R, strict_check=True):\n \"\"\"Compute intrinsic yxy Euler angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around y-, x'-, and y''-axes (intrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unity, unitx, unity, True, strict_check\n )\n\n\ndef extrinsic_euler_yxy_from_active_matrix(R, strict_check=True):\n \"\"\"Compute extrinsic yxy Euler angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around y-, x-, and y-axes (extrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unity, unitx, unity, True, strict_check\n )[::-1]\n\n\ndef intrinsic_euler_yzy_from_active_matrix(R, strict_check=True):\n \"\"\"Compute intrinsic yzy Euler angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around y-, z'-, and y''-axes (intrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unity, unitz, unity, True, strict_check\n )\n\n\ndef extrinsic_euler_yzy_from_active_matrix(R, strict_check=True):\n \"\"\"Compute extrinsic yzy Euler angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around y-, z-, and y-axes (extrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unity, unitz, unity, True, strict_check\n )[::-1]\n\n\ndef intrinsic_euler_zyz_from_active_matrix(R, strict_check=True):\n \"\"\"Compute intrinsic zyz Euler angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around z-, y'-, and z''-axes (intrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unitz, unity, unitz, True, strict_check\n )\n\n\ndef extrinsic_euler_zyz_from_active_matrix(R, strict_check=True):\n \"\"\"Compute extrinsic zyz Euler angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around z-, y-, and z-axes (extrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unitz, unity, unitz, True, strict_check\n )[::-1]\n\n\ndef intrinsic_euler_zxz_from_active_matrix(R, strict_check=True):\n \"\"\"Compute intrinsic zxz Euler angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around z-, x'-, and z''-axes (intrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unitz, unitx, unitz, True, strict_check\n )\n\n\ndef extrinsic_euler_zxz_from_active_matrix(R, strict_check=True):\n \"\"\"Compute extrinsic zxz Euler angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around z-, x-, and z-axes (extrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unitz, unitx, unitz, True, strict_check\n )[::-1]\n\n\ndef intrinsic_euler_xzy_from_active_matrix(R, strict_check=True):\n \"\"\"Compute intrinsic xzy Cardan angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around x-, z'-, and y''-axes (intrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unitx, unitz, unity, False, strict_check\n )\n\n\ndef extrinsic_euler_xzy_from_active_matrix(R, strict_check=True):\n \"\"\"Compute extrinsic xzy Cardan angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around x-, z-, and y-axes (extrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unity, unitz, unitx, False, strict_check\n )[::-1]\n\n\ndef intrinsic_euler_xyz_from_active_matrix(R, strict_check=True):\n \"\"\"Compute intrinsic xyz Cardan angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around x-, y'-, and z''-axes (intrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unitx, unity, unitz, False, strict_check\n )\n\n\ndef extrinsic_euler_xyz_from_active_matrix(R, strict_check=True):\n \"\"\"Compute extrinsic xyz Cardan angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around x-, y-, and z-axes (extrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unitz, unity, unitx, False, strict_check\n )[::-1]\n\n\ndef intrinsic_euler_yxz_from_active_matrix(R, strict_check=True):\n \"\"\"Compute intrinsic yxz Cardan angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around y-, x'-, and z''-axes (intrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unity, unitx, unitz, False, strict_check\n )\n\n\ndef extrinsic_euler_yxz_from_active_matrix(R, strict_check=True):\n \"\"\"Compute extrinsic yxz Cardan angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around y-, x-, and z-axes (extrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unitz, unitx, unity, False, strict_check\n )[::-1]\n\n\ndef intrinsic_euler_yzx_from_active_matrix(R, strict_check=True):\n \"\"\"Compute intrinsic yzx Cardan angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around y-, z'-, and x''-axes (intrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unity, unitz, unitx, False, strict_check\n )\n\n\ndef extrinsic_euler_yzx_from_active_matrix(R, strict_check=True):\n \"\"\"Compute extrinsic yzx Cardan angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around y-, z-, and x-axes (extrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unitx, unitz, unity, False, strict_check\n )[::-1]\n\n\ndef intrinsic_euler_zyx_from_active_matrix(R, strict_check=True):\n \"\"\"Compute intrinsic zyx Cardan angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around z-, y'-, and x''-axes (intrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unitz, unity, unitx, False, strict_check\n )\n\n\ndef extrinsic_euler_zyx_from_active_matrix(R, strict_check=True):\n \"\"\"Compute extrinsic zyx Cardan angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around z-, y-, and x-axes (extrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unitx, unity, unitz, False, strict_check\n )[::-1]\n\n\ndef intrinsic_euler_zxy_from_active_matrix(R, strict_check=True):\n \"\"\"Compute intrinsic zxy Cardan angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around z-, x'-, and y''-axes (intrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unitz, unitx, unity, False, strict_check\n )\n\n\ndef extrinsic_euler_zxy_from_active_matrix(R, strict_check=True):\n \"\"\"Compute extrinsic zxy Cardan angles from active rotation matrix.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n e : array, shape (3,)\n Angles for rotation around z-, x-, and y-axes (extrinsic rotations)\n \"\"\"\n warnings.warn(\n \"function is deprecated, use euler_from_matrix\",\n DeprecationWarning,\n stacklevel=2,\n )\n return general_intrinsic_euler_from_active_matrix(\n R, unity, unitx, unitz, False, strict_check\n )[::-1]\n\n\ndef quaternion_from_extrinsic_euler_xyz(e):\n \"\"\"Compute quaternion from extrinsic xyz Euler angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Angles for rotation around x-, y-, and z-axes (extrinsic rotations)\n\n Returns\n -------\n q : array, shape (4,)\n Unit quaternion to represent rotation: (w, x, y, z)\n \"\"\"\n warnings.warn(\n \"quaternion_from_extrinsic_euler_xyz is deprecated, use \"\n \"quaternion_from_euler\",\n DeprecationWarning,\n stacklevel=2,\n )\n R = active_matrix_from_extrinsic_euler_xyz(e)\n return quaternion_from_matrix(R)\n" }, { "path": "pytransform3d/rotations/_euler_deprecated.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef active_matrix_from_intrinsic_euler_xzx(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_extrinsic_euler_xzx(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_intrinsic_euler_xyx(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_extrinsic_euler_xyx(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_intrinsic_euler_yxy(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_extrinsic_euler_yxy(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_intrinsic_euler_yzy(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_extrinsic_euler_yzy(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_intrinsic_euler_zyz(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_extrinsic_euler_zyz(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_intrinsic_euler_zxz(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_extrinsic_euler_zxz(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_intrinsic_euler_xzy(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_extrinsic_euler_xzy(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_intrinsic_euler_xyz(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_extrinsic_euler_xyz(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_intrinsic_euler_yxz(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_extrinsic_euler_yxz(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_intrinsic_euler_yzx(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_extrinsic_euler_yzx(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_intrinsic_euler_zyx(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_extrinsic_euler_zyx(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_intrinsic_euler_zxy(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_extrinsic_euler_zxy(e: npt.ArrayLike) -> np.ndarray: ...\ndef active_matrix_from_extrinsic_roll_pitch_yaw(\n rpy: npt.ArrayLike,\n) -> np.ndarray: ...\ndef intrinsic_euler_xzx_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef extrinsic_euler_xzx_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef intrinsic_euler_xyx_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef extrinsic_euler_xyx_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef intrinsic_euler_yxy_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef extrinsic_euler_yxy_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef intrinsic_euler_yzy_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef extrinsic_euler_yzy_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef intrinsic_euler_zyz_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef extrinsic_euler_zyz_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef intrinsic_euler_zxz_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef extrinsic_euler_zxz_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef intrinsic_euler_xzy_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef extrinsic_euler_xzy_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef intrinsic_euler_xyz_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef extrinsic_euler_xyz_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef intrinsic_euler_yxz_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef extrinsic_euler_yxz_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef intrinsic_euler_yzx_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef extrinsic_euler_yzx_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef intrinsic_euler_zyx_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef extrinsic_euler_zyx_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef intrinsic_euler_zxy_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef extrinsic_euler_zxy_from_active_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef quaternion_from_extrinsic_euler_xyz(e: npt.ArrayLike) -> np.ndarray: ...\n" }, { "path": "pytransform3d/rotations/_jacobians.py", "content": "\"\"\"Jacobians of SO(3).\"\"\"\n\nimport math\n\nimport numpy as np\n\nfrom ._rot_log import cross_product_matrix\n\n\ndef left_jacobian_SO3(omega):\n r\"\"\"Left Jacobian of SO(3) at theta (angle of rotation).\n\n .. math::\n\n \\boldsymbol{J}(\\theta)\n =\n \\frac{\\sin{\\theta}}{\\theta} \\boldsymbol{I}\n + \\left(\\frac{1 - \\cos{\\theta}}{\\theta}\\right)\n \\left[\\hat{\\boldsymbol{\\omega}}\\right]\n + \\left(1 - \\frac{\\sin{\\theta}}{\\theta} \\right)\n \\hat{\\boldsymbol{\\omega}} \\hat{\\boldsymbol{\\omega}}^T\n\n Parameters\n ----------\n omega : array-like, shape (3,)\n Compact axis-angle representation.\n\n Returns\n -------\n J : array, shape (3, 3)\n Left Jacobian of SO(3).\n\n See also\n --------\n left_jacobian_SO3_series :\n Left Jacobian of SO(3) at theta from Taylor series.\n\n left_jacobian_SO3_inv :\n Inverse left Jacobian of SO(3) at theta (angle of rotation).\n \"\"\"\n omega = np.asarray(omega)\n theta = np.linalg.norm(omega)\n if theta < np.finfo(float).eps:\n return left_jacobian_SO3_series(omega, 10)\n omega_unit = omega / theta\n omega_matrix = cross_product_matrix(omega_unit)\n return (\n np.eye(3)\n + (1.0 - math.cos(theta)) / theta * omega_matrix\n + (1.0 - math.sin(theta) / theta) * np.dot(omega_matrix, omega_matrix)\n )\n\n\ndef left_jacobian_SO3_series(omega, n_terms):\n \"\"\"Left Jacobian of SO(3) at theta from Taylor series.\n\n Parameters\n ----------\n omega : array-like, shape (3,)\n Compact axis-angle representation.\n\n n_terms : int\n Number of terms to include in the series.\n\n Returns\n -------\n J : array, shape (3, 3)\n Left Jacobian of SO(3).\n\n See Also\n --------\n left_jacobian_SO3 : Left Jacobian of SO(3) at theta (angle of rotation).\n \"\"\"\n omega = np.asarray(omega)\n J = np.eye(3)\n pxn = np.eye(3)\n px = cross_product_matrix(omega)\n for n in range(n_terms):\n pxn = np.dot(pxn, px) / (n + 2)\n J += pxn\n return J\n\n\ndef left_jacobian_SO3_inv(omega):\n r\"\"\"Inverse left Jacobian of SO(3) at theta (angle of rotation).\n\n .. math::\n\n \\boldsymbol{J}^{-1}(\\theta)\n =\n \\frac{\\theta}{2 \\tan{\\frac{\\theta}{2}}} \\boldsymbol{I}\n - \\frac{\\theta}{2} \\left[\\hat{\\boldsymbol{\\omega}}\\right]\n + \\left(1 - \\frac{\\theta}{2 \\tan{\\frac{\\theta}{2}}}\\right)\n \\hat{\\boldsymbol{\\omega}} \\hat{\\boldsymbol{\\omega}}^T\n\n Parameters\n ----------\n omega : array-like, shape (3,)\n Compact axis-angle representation.\n\n Returns\n -------\n J_inv : array, shape (3, 3)\n Inverse left Jacobian of SO(3).\n\n See Also\n --------\n left_jacobian_SO3 : Left Jacobian of SO(3) at theta (angle of rotation).\n\n left_jacobian_SO3_inv_series :\n Inverse left Jacobian of SO(3) at theta from Taylor series.\n \"\"\"\n omega = np.asarray(omega)\n theta = np.linalg.norm(omega)\n if theta < np.finfo(float).eps:\n return left_jacobian_SO3_inv_series(omega, 10)\n omega_unit = omega / theta\n omega_matrix = cross_product_matrix(omega_unit)\n return (\n np.eye(3)\n - 0.5 * omega_matrix * theta\n + (1.0 - 0.5 * theta / np.tan(theta / 2.0))\n * np.dot(omega_matrix, omega_matrix)\n )\n\n\ndef left_jacobian_SO3_inv_series(omega, n_terms):\n \"\"\"Inverse left Jacobian of SO(3) at theta from Taylor series.\n\n Parameters\n ----------\n omega : array-like, shape (3,)\n Compact axis-angle representation.\n\n n_terms : int\n Number of terms to include in the series.\n\n Returns\n -------\n J_inv : array, shape (3, 3)\n Inverse left Jacobian of SO(3).\n\n See Also\n --------\n left_jacobian_SO3_inv :\n Inverse left Jacobian of SO(3) at theta (angle of rotation).\n \"\"\"\n from scipy.special import bernoulli\n\n omega = np.asarray(omega)\n J_inv = np.eye(3)\n pxn = np.eye(3)\n px = cross_product_matrix(omega)\n b = bernoulli(n_terms + 1)\n for n in range(n_terms):\n pxn = np.dot(pxn, px / (n + 1))\n J_inv += b[n + 1] * pxn\n return J_inv\n" }, { "path": "pytransform3d/rotations/_jacobians.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef left_jacobian_SO3(omega: npt.ArrayLike) -> np.ndarray: ...\ndef left_jacobian_SO3_series(\n omega: npt.ArrayLike, n_terms: int\n) -> np.ndarray: ...\ndef left_jacobian_SO3_inv(omega: npt.ArrayLike) -> np.ndarray: ...\ndef left_jacobian_SO3_inv_series(\n omega: npt.ArrayLike, n_terms: int\n) -> np.ndarray: ...\n" }, { "path": "pytransform3d/rotations/_matrix.py", "content": "\"\"\"Rotation matrices.\"\"\"\n\nimport warnings\n\nimport numpy as np\nfrom numpy.testing import assert_array_almost_equal\n\nfrom ._axis_angle import compact_axis_angle\nfrom ._utils import norm_vector, perpendicular_to_vectors, vector_projection\n\n\ndef check_matrix(R, tolerance=1e-6, strict_check=True):\n r\"\"\"Input validation of a rotation matrix.\n\n We check whether R multiplied by its inverse is approximately the identity\n matrix\n\n .. math::\n\n \\boldsymbol{R}\\boldsymbol{R}^T = \\boldsymbol{I}\n\n and whether the determinant is positive\n\n .. math::\n\n det(\\boldsymbol{R}) > 0\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n tolerance : float, optional (default: 1e-6)\n Tolerance threshold for checks. Default tolerance is the same as in\n assert_rotation_matrix(R).\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n R : array, shape (3, 3)\n Validated rotation matrix\n\n Raises\n ------\n ValueError\n If input is invalid\n\n See Also\n --------\n norm_matrix : Enforces orthonormality of a rotation matrix.\n robust_polar_decomposition\n A more expensive orthonormalization method that spreads the error more\n evenly between the basis vectors.\n \"\"\"\n R = np.asarray(R, dtype=np.float64)\n if R.ndim != 2 or R.shape[0] != 3 or R.shape[1] != 3:\n raise ValueError(\n \"Expected rotation matrix with shape (3, 3), got \"\n \"array-like object with shape %s\" % (R.shape,)\n )\n RRT = np.dot(R, R.T)\n if not np.allclose(RRT, np.eye(3), atol=tolerance):\n error_msg = (\n \"Expected rotation matrix, but it failed the test \"\n \"for inversion by transposition. np.dot(R, R.T) \"\n \"gives %r\" % RRT\n )\n if strict_check:\n raise ValueError(error_msg)\n warnings.warn(error_msg, UserWarning, stacklevel=2)\n R_det = np.linalg.det(R)\n if R_det < 0.0:\n error_msg = (\n \"Expected rotation matrix, but it failed the test \"\n \"for the determinant, which should be 1 but is %g; \"\n \"that is, it probably represents a rotoreflection\" % R_det\n )\n if strict_check:\n raise ValueError(error_msg)\n warnings.warn(error_msg, UserWarning, stacklevel=2)\n return R\n\n\ndef matrix_requires_renormalization(R, tolerance=1e-6):\n r\"\"\"Check if a rotation matrix needs renormalization.\n\n This function will check if :math:`R R^T \\approx I`.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix that should be orthonormal.\n\n tolerance : float, optional (default: 1e-6)\n Tolerance for check.\n\n Returns\n -------\n required : bool\n Indicates if renormalization is required.\n\n See Also\n --------\n norm_matrix : Orthonormalize rotation matrix.\n robust_polar_decomposition\n A more expensive orthonormalization method that spreads the error more\n evenly between the basis vectors.\n \"\"\"\n R = np.asarray(R, dtype=float)\n RRT = np.dot(R, R.T)\n return not np.allclose(RRT, np.eye(3), atol=tolerance)\n\n\ndef norm_matrix(R):\n r\"\"\"Orthonormalize rotation matrix.\n\n A rotation matrix is defined as\n\n .. math::\n\n \\boldsymbol R =\n \\left( \\begin{array}{ccc}\n r_{11} & r_{12} & r_{13}\\\\\n r_{21} & r_{22} & r_{23}\\\\\n r_{31} & r_{32} & r_{33}\\\\\n \\end{array} \\right)\n \\in SO(3)\n\n and must be orthonormal, which results in 6 constraints:\n\n * column vectors must have unit norm (3 constraints)\n * and must be orthogonal to each other (3 constraints)\n\n A more compact representation of these constraints is\n :math:`\\boldsymbol R^T \\boldsymbol R = \\boldsymbol I`.\n\n Because of numerical problems, a rotation matrix might not satisfy the\n constraints anymore. This function will enforce them with Gram-Schmidt\n orthonormalization optimized for 3 dimensions.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix with small numerical errors.\n\n Returns\n -------\n R : array, shape (3, 3)\n Orthonormalized rotation matrix.\n\n See Also\n --------\n check_matrix : Checks orthonormality of a rotation matrix.\n matrix_requires_renormalization\n Checks if a rotation matrix needs renormalization.\n robust_polar_decomposition\n A more expensive orthonormalization method that spreads the error more\n evenly between the basis vectors.\n \"\"\"\n R = np.asarray(R)\n c2 = R[:, 1]\n c3 = norm_vector(R[:, 2])\n c1 = norm_vector(np.cross(c2, c3))\n c2 = norm_vector(np.cross(c3, c1))\n return np.column_stack((c1, c2, c3))\n\n\ndef assert_rotation_matrix(R, *args, **kwargs):\n \"\"\"Raise an assertion if a matrix is not a rotation matrix.\n\n The two properties :math:`\\\\boldsymbol{I} = \\\\boldsymbol{R R}^T` and\n :math:`det(R) = 1` will be checked. See\n numpy.testing.assert_array_almost_equal for a more detailed documentation\n of the other parameters.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n args : tuple\n Positional arguments that will be passed to\n `assert_array_almost_equal`\n\n kwargs : dict\n Positional arguments that will be passed to\n `assert_array_almost_equal`\n \"\"\"\n assert_array_almost_equal(np.dot(R, R.T), np.eye(3), *args, **kwargs)\n assert_array_almost_equal(np.linalg.det(R), 1.0, *args, **kwargs)\n\n\ndef matrix_from_two_vectors(a, b):\n \"\"\"Compute rotation matrix from two vectors.\n\n We assume that the two given vectors form a plane so that we can compute\n a third, orthogonal vector with the cross product.\n\n The x-axis will point in the same direction as a, the y-axis corresponds\n to the normalized vector rejection of b on a, and the z-axis is the\n cross product of the other basis vectors.\n\n Parameters\n ----------\n a : array-like, shape (3,)\n First vector, must not be 0\n\n b : array-like, shape (3,)\n Second vector, must not be 0 or parallel to a\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n\n Raises\n ------\n ValueError\n If vectors are parallel or one of them is the zero vector\n \"\"\"\n if np.linalg.norm(a) == 0:\n raise ValueError(\"a must not be the zero vector.\")\n if np.linalg.norm(b) == 0:\n raise ValueError(\"b must not be the zero vector.\")\n\n c = perpendicular_to_vectors(a, b)\n if np.linalg.norm(c) == 0:\n raise ValueError(\"a and b must not be parallel.\")\n\n a = norm_vector(a)\n\n b_on_a_projection = vector_projection(b, a)\n b_on_a_rejection = b - b_on_a_projection\n b = norm_vector(b_on_a_rejection)\n\n c = norm_vector(c)\n\n return np.column_stack((a, b, c))\n\n\ndef quaternion_from_matrix(R, strict_check=True):\n \"\"\"Compute quaternion from rotation matrix.\n\n We usually assume active rotations.\n\n .. warning::\n\n When computing a quaternion from the rotation matrix there is a sign\n ambiguity: q and -q represent the same rotation.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n Returns\n -------\n q : array, shape (4,)\n Unit quaternion to represent rotation: (w, x, y, z)\n \"\"\"\n R = check_matrix(R, strict_check=strict_check)\n q = np.empty(4)\n\n # Source:\n # http://www.euclideanspace.com/maths/geometry/rotations/conversions\n trace = np.trace(R)\n if trace > 0.0:\n sqrt_trace = np.sqrt(1.0 + trace)\n q[0] = 0.5 * sqrt_trace\n q[1] = 0.5 / sqrt_trace * (R[2, 1] - R[1, 2])\n q[2] = 0.5 / sqrt_trace * (R[0, 2] - R[2, 0])\n q[3] = 0.5 / sqrt_trace * (R[1, 0] - R[0, 1])\n else:\n if R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:\n sqrt_trace = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2])\n q[0] = 0.5 / sqrt_trace * (R[2, 1] - R[1, 2])\n q[1] = 0.5 * sqrt_trace\n q[2] = 0.5 / sqrt_trace * (R[1, 0] + R[0, 1])\n q[3] = 0.5 / sqrt_trace * (R[0, 2] + R[2, 0])\n elif R[1, 1] > R[2, 2]:\n sqrt_trace = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2])\n q[0] = 0.5 / sqrt_trace * (R[0, 2] - R[2, 0])\n q[1] = 0.5 / sqrt_trace * (R[1, 0] + R[0, 1])\n q[2] = 0.5 * sqrt_trace\n q[3] = 0.5 / sqrt_trace * (R[2, 1] + R[1, 2])\n else:\n sqrt_trace = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1])\n q[0] = 0.5 / sqrt_trace * (R[1, 0] - R[0, 1])\n q[1] = 0.5 / sqrt_trace * (R[0, 2] + R[2, 0])\n q[2] = 0.5 / sqrt_trace * (R[2, 1] + R[1, 2])\n q[3] = 0.5 * sqrt_trace\n return q\n\n\ndef axis_angle_from_matrix(R, strict_check=True, check=True):\n \"\"\"Compute axis-angle from rotation matrix.\n\n This operation is called logarithmic map. Note that there are two possible\n solutions for the rotation axis when the angle is 180 degrees (pi).\n\n We usually assume active rotations.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n check : bool, optional (default: True)\n Check if rotation matrix is valid\n\n Returns\n -------\n a : array, shape (4,)\n Axis of rotation and rotation angle: (x, y, z, angle). The angle is\n constrained to [0, pi].\n \"\"\"\n if check:\n R = check_matrix(R, strict_check=strict_check)\n cos_angle = (np.trace(R) - 1.0) / 2.0\n angle = np.arccos(min(max(-1.0, cos_angle), 1.0))\n\n if angle == 0.0: # R == np.eye(3)\n return np.array([1.0, 0.0, 0.0, 0.0])\n\n a = np.empty(4)\n\n # We can usually determine the rotation axis by inverting Rodrigues'\n # formula. Subtracting opposing off-diagonal elements gives us\n # 2 * sin(angle) * e,\n # where e is the normalized rotation axis.\n axis_unnormalized = np.array(\n [R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]]\n )\n\n if abs(angle - np.pi) < 1e-4: # np.trace(R) close to -1\n # The threshold 1e-4 is a result from this discussion:\n # https://github.com/dfki-ric/pytransform3d/issues/43\n # The standard formula becomes numerically unstable, however,\n # Rodrigues' formula reduces to R = I + 2 (ee^T - I), with the\n # rotation axis e, that is, ee^T = 0.5 * (R + I) and we can find the\n # squared values of the rotation axis on the diagonal of this matrix.\n # We can still use the original formula to reconstruct the signs of\n # the rotation axis correctly.\n\n # In case of floating point inaccuracies:\n R_diag = np.clip(np.diag(R), -1.0, 1.0)\n\n eeT_diag = 0.5 * (R_diag + 1.0)\n signs = np.sign(axis_unnormalized)\n signs[signs == 0.0] = 1.0\n a[:3] = np.sqrt(eeT_diag) * signs\n else:\n a[:3] = axis_unnormalized\n # The norm of axis_unnormalized is 2.0 * np.sin(angle), that is, we\n # could normalize with a[:3] = a[:3] / (2.0 * np.sin(angle)),\n # but the following is much more precise for angles close to 0 or pi:\n a[:3] /= np.linalg.norm(a[:3])\n\n a[3] = angle\n return a\n\n\ndef compact_axis_angle_from_matrix(R, check=True):\n \"\"\"Compute compact axis-angle from rotation matrix.\n\n This operation is called logarithmic map. Note that there are two possible\n solutions for the rotation axis when the angle is 180 degrees (pi).\n\n We usually assume active rotations.\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n check : bool, optional (default: True)\n Check if rotation matrix is valid\n\n Returns\n -------\n a : array, shape (3,)\n Axis of rotation and rotation angle: angle * (x, y, z). The angle is\n constrained to [0, pi].\n \"\"\"\n a = axis_angle_from_matrix(R, check=check)\n return compact_axis_angle(a)\n" }, { "path": "pytransform3d/rotations/_matrix.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef check_matrix(\n R: npt.ArrayLike, tolerance: float = ..., strict_check: bool = ...\n) -> np.ndarray: ...\ndef matrix_requires_renormalization(\n R: npt.ArrayLike, tolerance: float = ...\n) -> bool: ...\ndef norm_matrix(R: npt.ArrayLike) -> np.ndarray: ...\ndef assert_rotation_matrix(R: npt.ArrayLike, *args, **kwargs): ...\ndef matrix_from_two_vectors(\n a: npt.ArrayLike, b: npt.ArrayLike\n) -> np.ndarray: ...\ndef quaternion_from_matrix(\n R: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef axis_angle_from_matrix(\n R: npt.ArrayLike, strict_check: bool = ..., check: bool = ...\n) -> np.ndarray: ...\ndef compact_axis_angle_from_matrix(\n R: npt.ArrayLike, check: bool = ...\n) -> np.ndarray: ...\n" }, { "path": "pytransform3d/rotations/_mrp.py", "content": "\"\"\"Modified Rodrigues parameters.\"\"\"\n\nimport numpy as np\nfrom numpy.testing import assert_array_almost_equal\n\nfrom ._angle import norm_angle\nfrom ._axis_angle import mrp_from_axis_angle\nfrom ._constants import two_pi, eps\n\n\ndef check_mrp(mrp):\n \"\"\"Input validation of modified Rodrigues parameters.\n\n Parameters\n ----------\n mrp : array-like, shape (3,)\n Modified Rodrigues parameters.\n\n Returns\n -------\n mrp : array, shape (3,)\n Validated modified Rodrigues parameters.\n\n Raises\n ------\n ValueError\n If input is invalid\n \"\"\"\n mrp = np.asarray(mrp)\n if mrp.ndim != 1 or mrp.shape[0] != 3:\n raise ValueError(\n \"Expected modified Rodrigues parameters with shape (3,), got \"\n \"array-like object with shape %s\" % (mrp.shape,)\n )\n return mrp\n\n\ndef norm_mrp(mrp):\n \"\"\"Normalize angle of modified Rodrigues parameters to range [-pi, pi].\n\n Normalization of modified Rodrigues parameters is required to avoid the\n singularity at a rotation angle of 2 * pi.\n\n Parameters\n ----------\n mrp : array-like, shape (3,)\n Modified Rodrigues parameters.\n\n Returns\n -------\n mrp : array, shape (3,)\n Modified Rodrigues parameters with angle normalized to [-pi, pi].\n \"\"\"\n mrp = check_mrp(mrp)\n a = axis_angle_from_mrp(mrp)\n a[3] = norm_angle(a[3])\n return mrp_from_axis_angle(a)\n\n\ndef mrp_near_singularity(mrp, tolerance=1e-6):\n \"\"\"Check if modified Rodrigues parameters are close to singularity.\n\n MRPs have a singularity at 2 * pi, i.e., the norm approaches infinity as\n the angle approaches 2 * pi.\n\n Parameters\n ----------\n mrp : array-like, shape (3,)\n Modified Rodrigues parameters.\n\n tolerance : float, optional (default: 1e-6)\n Tolerance for check.\n\n Returns\n -------\n near_singularity : bool\n MRPs are near singularity.\n \"\"\"\n check_mrp(mrp)\n mrp_norm = np.linalg.norm(mrp)\n angle = np.arctan(mrp_norm) * 4.0\n return abs(angle - two_pi) < tolerance\n\n\ndef mrp_double(mrp):\n r\"\"\"Other modified Rodrigues parameters representing the same orientation.\n\n MRPs have two representations for the same rotation:\n :math:`\\boldsymbol{\\psi}` and :math:`-\\frac{1}{||\\boldsymbol{\\psi}||^2}\n \\boldsymbol{\\psi}` represent the same rotation and correspond to two\n antipodal unit quaternions [1]_.\n\n No rotation is a special case, in which no second representation exists.\n Only the zero vector represents no rotation.\n\n Parameters\n ----------\n mrp : array-like, shape (3,)\n Modified Rodrigues parameters.\n\n Returns\n -------\n mrp_double : array, shape (3,)\n Different modified Rodrigues parameters that represent the same\n orientation.\n\n References\n ----------\n .. [1] Shuster, M. D. (1993). A Survey of Attitude Representations.\n Journal of the Astronautical Sciences, 41, 439-517.\n http://malcolmdshuster.com/Pub_1993h_J_Repsurv_scan.pdf\n \"\"\"\n mrp = check_mrp(mrp)\n norm = np.dot(mrp, mrp)\n if norm == 0.0:\n return mrp\n return mrp / -norm\n\n\ndef assert_mrp_equal(mrp1, mrp2, *args, **kwargs):\n \"\"\"Raise an assertion if two MRPs are not approximately equal.\n\n There are two MRPs that represent the same orientation (double cover). See\n numpy.testing.assert_array_almost_equal for a more detailed documentation\n of the other parameters.\n\n Parameters\n ----------\n mrp1 : array-like, shape (3,)\n Modified Rodrigues parameters.\n\n mrp1 : array-like, shape (3,)\n Modified Rodrigues parameters.\n\n args : tuple\n Positional arguments that will be passed to\n `assert_array_almost_equal`\n\n kwargs : dict\n Positional arguments that will be passed to\n `assert_array_almost_equal`\n \"\"\"\n try:\n assert_array_almost_equal(mrp1, mrp2, *args, **kwargs)\n except AssertionError:\n assert_array_almost_equal(mrp1, mrp_double(mrp2), *args, **kwargs)\n\n\ndef concatenate_mrp(mrp1, mrp2):\n r\"\"\"Concatenate two rotations defined by modified Rodrigues parameters.\n\n Suppose we want to apply two extrinsic rotations given by modified\n Rodrigues parameters mrp1 and mrp2 to a vector v. We can either apply mrp2\n to v and then mrp1 to the result or we can concatenate mrp1 and mrp2 and\n apply the result to v.\n\n The solution for concatenation of two rotations\n :math:`\\boldsymbol{\\psi}_1,\\boldsymbol{\\psi}_2` is given by Shuster [1]_\n (Equation 257):\n\n .. math::\n\n \\boldsymbol{\\psi} =\n \\frac{\n (1 - ||\\boldsymbol{\\psi}_1||^2) \\boldsymbol{\\psi}_2\n + (1 - ||\\boldsymbol{\\psi}_2||^2) \\boldsymbol{\\psi}_1\n - 2 \\boldsymbol{\\psi}_2 \\times \\boldsymbol{\\psi}_1}\n {1 + ||\\boldsymbol{\\psi}_2||^2 ||\\boldsymbol{\\psi}_1||^2\n - 2 \\boldsymbol{\\psi}_2 \\cdot \\boldsymbol{\\psi}_1}.\n\n Parameters\n ----------\n mrp1 : array-like, shape (3,)\n Modified Rodrigues parameters.\n\n mrp2 : array-like, shape (3,)\n Modified Rodrigues parameters.\n\n Returns\n -------\n mrp12 : array, shape (3,)\n Modified Rodrigues parameters that represent the concatenated rotation\n of mrp1 and mrp2.\n\n References\n ----------\n .. [1] Shuster, M. D. (1993). A Survey of Attitude Representations.\n Journal of the Astronautical Sciences, 41, 439-517.\n http://malcolmdshuster.com/Pub_1993h_J_Repsurv_scan.pdf\n \"\"\"\n mrp1 = check_mrp(mrp1)\n mrp2 = check_mrp(mrp2)\n norm1_sq = np.linalg.norm(mrp1) ** 2\n norm2_sq = np.linalg.norm(mrp2) ** 2\n cross_product = np.cross(mrp2, mrp1)\n scalar_product = np.dot(mrp2, mrp1)\n return (\n (1.0 - norm1_sq) * mrp2 + (1.0 - norm2_sq) * mrp1 - 2.0 * cross_product\n ) / (1.0 + norm2_sq * norm1_sq - 2.0 * scalar_product)\n\n\ndef mrp_prod_vector(mrp, v):\n r\"\"\"Apply rotation represented by MRPs to a vector.\n\n To apply the rotation defined by modified Rodrigues parameters\n :math:`\\boldsymbol{\\psi} \\in \\mathbb{R}^3` to a vector\n :math:`\\boldsymbol{v} \\in \\mathbb{R}^3`, we left-concatenate the original\n MRPs and then right-concatenate its inverted (negative) version.\n\n Parameters\n ----------\n mrp : array-like, shape (3,)\n Modified Rodrigues parameters.\n\n v : array-like, shape (3,)\n 3d vector\n\n Returns\n -------\n w : array, shape (3,)\n 3d vector\n\n See Also\n --------\n concatenate_mrp : Concatenates MRPs.\n q_prod_vector : The same operation with a quaternion.\n \"\"\"\n mrp = check_mrp(mrp)\n return concatenate_mrp(concatenate_mrp(mrp, v), -mrp)\n\n\ndef quaternion_from_mrp(mrp):\n \"\"\"Compute quaternion from modified Rodrigues parameters.\n\n Parameters\n ----------\n mrp : array-like, shape (3,)\n Modified Rodrigues parameters.\n\n Returns\n -------\n q : array, shape (4,)\n Unit quaternion to represent rotation: (w, x, y, z)\n \"\"\"\n mrp = check_mrp(mrp)\n dot_product_p1 = np.dot(mrp, mrp) + 1.0\n q = np.empty(4, dtype=float)\n q[0] = (2.0 - dot_product_p1) / dot_product_p1\n q[1:] = 2.0 * mrp / dot_product_p1\n return q\n\n\ndef axis_angle_from_mrp(mrp):\n \"\"\"Compute axis-angle representation from modified Rodrigues parameters.\n\n Parameters\n ----------\n mrp : array-like, shape (3,)\n Modified Rodrigues parameters.\n\n Returns\n -------\n a : array, shape (4,)\n Axis of rotation and rotation angle: (x, y, z, angle)\n \"\"\"\n mrp = check_mrp(mrp)\n\n mrp_norm = np.linalg.norm(mrp)\n angle = np.arctan(mrp_norm) * 4.0\n if abs(angle) < eps:\n return np.array([1.0, 0.0, 0.0, 0.0])\n\n axis = mrp / mrp_norm\n return np.hstack((axis, (angle,)))\n" }, { "path": "pytransform3d/rotations/_mrp.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef check_mrp(mrp: npt.ArrayLike) -> np.ndarray: ...\ndef norm_mrp(mrp: npt.ArrayLike) -> np.ndarray: ...\ndef mrp_near_singularity(\n mrp: npt.ArrayLike, tolerance: float = ...\n) -> bool: ...\ndef mrp_double(mrp: npt.ArrayLike) -> np.ndarray: ...\ndef assert_mrp_equal(\n mrp1: npt.ArrayLike, mrp2: npt.ArrayLike, *args, **kwargs\n): ...\ndef concatenate_mrp(mrp1: npt.ArrayLike, mrp2: npt.ArrayLike) -> np.ndarray: ...\ndef mrp_prod_vector(mrp: npt.ArrayLike, v: npt.ArrayLike) -> np.ndarray: ...\ndef quaternion_from_mrp(mrp: npt.ArrayLike) -> np.ndarray: ...\ndef axis_angle_from_mrp(mrp: npt.ArrayLike) -> np.ndarray: ...\n" }, { "path": "pytransform3d/rotations/_plot.py", "content": "\"\"\"Plotting functions.\"\"\"\n\nimport numpy as np\n\nfrom ._axis_angle import check_axis_angle\nfrom ._constants import a_id, p0, unitx, unity\nfrom ._matrix import check_matrix\nfrom ._rotors import wedge, plane_normal_from_bivector\nfrom ._slerp import slerp_weights\nfrom ._utils import perpendicular_to_vectors, angle_between_vectors\n\n\ndef plot_basis(\n ax=None, R=None, p=np.zeros(3), s=1.0, ax_s=1, strict_check=True, **kwargs\n):\n \"\"\"Plot basis of a rotation matrix.\n\n Parameters\n ----------\n ax : Matplotlib 3d axis, optional (default: None)\n If the axis is None, a new 3d axis will be created\n\n R : array-like, shape (3, 3), optional (default: I)\n Rotation matrix, each column contains a basis vector\n\n p : array-like, shape (3,), optional (default: [0, 0, 0])\n Offset from the origin\n\n s : float, optional (default: 1)\n Scaling of the frame that will be drawn\n\n ax_s : float, optional (default: 1)\n Scaling of the new matplotlib 3d axis\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically close\n enough to a real rotation matrix. Otherwise we print a warning.\n\n kwargs : dict, optional (default: {})\n Additional arguments for the plotting functions, e.g. alpha\n\n Returns\n -------\n ax : Matplotlib 3d axis\n New or old axis\n \"\"\"\n from ..plot_utils import make_3d_axis, Frame\n\n if ax is None:\n ax = make_3d_axis(ax_s)\n\n if R is None:\n R = np.eye(3)\n R = check_matrix(R, strict_check=strict_check)\n\n A2B = np.eye(4)\n A2B[:3, :3] = R\n A2B[:3, 3] = p\n\n frame = Frame(A2B, s=s, **kwargs)\n frame.add_frame(ax)\n\n return ax\n\n\ndef plot_axis_angle(ax=None, a=a_id, p=p0, s=1.0, ax_s=1, **kwargs):\n \"\"\"Plot rotation axis and angle.\n\n Parameters\n ----------\n ax : Matplotlib 3d axis, optional (default: None)\n If the axis is None, a new 3d axis will be created\n\n a : array-like, shape (4,), optional (default: [1, 0, 0, 0])\n Axis of rotation and rotation angle: (x, y, z, angle)\n\n p : array-like, shape (3,), optional (default: [0, 0, 0])\n Offset from the origin\n\n s : float, optional (default: 1)\n Scaling of the axis and angle that will be drawn\n\n ax_s : float, optional (default: 1)\n Scaling of the new matplotlib 3d axis\n\n kwargs : dict, optional (default: {})\n Additional arguments for the plotting functions, e.g. alpha\n\n Returns\n -------\n ax : Matplotlib 3d axis\n New or old axis\n \"\"\"\n from ..plot_utils import make_3d_axis, Arrow3D\n\n a = check_axis_angle(a)\n if ax is None:\n ax = make_3d_axis(ax_s)\n\n ax.add_artist(\n Arrow3D(\n [p[0], p[0] + s * a[0]],\n [p[1], p[1] + s * a[1]],\n [p[2], p[2] + s * a[2]],\n mutation_scale=20,\n lw=3,\n arrowstyle=\"-|>\",\n color=\"k\",\n )\n )\n\n arc = _arc_axis_angle(a, p, s)\n ax.plot(arc[:-5, 0], arc[:-5, 1], arc[:-5, 2], color=\"k\", lw=3, **kwargs)\n\n arrow_coords = np.vstack((arc[-1], arc[-1] + 20 * (arc[-1] - arc[-3]))).T\n ax.add_artist(\n Arrow3D(\n arrow_coords[0],\n arrow_coords[1],\n arrow_coords[2],\n mutation_scale=20,\n lw=3,\n arrowstyle=\"-|>\",\n color=\"k\",\n )\n )\n\n for i in [0, -1]:\n arc_bound = np.vstack((p + 0.5 * s * a[:3], arc[i])).T\n ax.plot(arc_bound[0], arc_bound[1], arc_bound[2], \"--\", c=\"k\")\n\n return ax\n\n\ndef _arc_axis_angle(a, p, s): # pragma: no cover\n \"\"\"Compute arc defined by a around p with scaling s.\n\n Parameters\n ----------\n a : array-like, shape (4,)\n Axis of rotation and rotation angle: (x, y, z, angle)\n\n p : array-like, shape (3,), optional (default: [0, 0, 0])\n Offset from the origin\n\n s : float, optional (default: 1)\n Scaling of the axis and angle that will be drawn\n\n Returns\n -------\n arc : array, shape (100, 3)\n Coordinates of arc\n \"\"\"\n p1 = (\n unitx\n if np.abs(a[0]) <= np.finfo(float).eps\n else perpendicular_to_vectors(unity, a[:3])\n )\n p2 = perpendicular_to_vectors(a[:3], p1)\n angle_p1p2 = angle_between_vectors(p1, p2)\n arc = np.empty((100, 3))\n for i, t in enumerate(np.linspace(0, 2 * a[3] / np.pi, len(arc))):\n w1, w2 = slerp_weights(angle_p1p2, t)\n arc[i] = p + 0.5 * s * (a[:3] + w1 * p1 + w2 * p2)\n return arc\n\n\ndef plot_bivector(ax=None, a=None, b=None, ax_s=1):\n \"\"\"Plot bivector from wedge product of two vectors a and b.\n\n The two vectors will be displayed in grey together with the parallelogram\n that they form. Each component of the bivector corresponds to the area of\n the parallelogram projected on the basis planes. These parallelograms will\n be shown as well. Furthermore, one black arrow will show the rotation\n direction of the bivector and one black arrow will represent the normal of\n the plane that can be extracted by rearranging the elements of the bivector\n and normalizing the vector.\n\n Parameters\n ----------\n ax : Matplotlib 3d axis, optional (default: None)\n If the axis is None, a new 3d axis will be created\n\n a : array-like, shape (3,), optional (default: [1, 0, 0])\n Vector\n\n b : array-like, shape (3,), optional (default: [0, 1, 0])\n Vector\n\n ax_s : float, optional (default: 1)\n Scaling of the new matplotlib 3d axis\n\n Returns\n -------\n ax : Matplotlib 3d axis\n New or old axis\n \"\"\"\n from ..plot_utils import make_3d_axis, Arrow3D, plot_vector\n\n if ax is None:\n ax = make_3d_axis(ax_s)\n\n if a is None:\n a = np.array([1, 0, 0])\n if b is None:\n b = np.array([0, 1, 0])\n\n B = wedge(a, b)\n normal = plane_normal_from_bivector(B)\n\n _plot_projected_parallelograms(ax, a, b)\n\n arc = _arc_between_vectors(a, b)\n ax.plot(arc[:-5, 0], arc[:-5, 1], arc[:-5, 2], color=\"k\", lw=2)\n\n arrow_coords = np.vstack((arc[-6], arc[-6] + 10 * (arc[-1] - arc[-3]))).T\n angle_arrow = Arrow3D(\n arrow_coords[0],\n arrow_coords[1],\n arrow_coords[2],\n mutation_scale=20,\n lw=2,\n arrowstyle=\"-|>\",\n color=\"k\",\n )\n ax.add_artist(angle_arrow)\n\n plot_vector(ax=ax, start=np.zeros(3), direction=normal, color=\"k\")\n\n return ax\n\n\ndef _plot_projected_parallelograms(ax, a, b):\n \"\"\"Plot projected parallelograms.\n\n We visualize a plane described by two vectors in 3D as a parallelogram.\n Furthermore, we project the parallelogram to the three basis planes and\n visualize these projections as parallelograms.\n\n Parameters\n ----------\n ax : Matplotlib 3d axis\n Axis\n\n a : array, shape (3,)\n Vector that defines the first side\n\n b : array, shape (3,)\n Vector that defines the second side\n \"\"\"\n _plot_parallelogram(ax, a, b, \"#aaaaaa\", 0.5)\n a_xy = np.array([a[0], a[1], 0])\n b_xy = np.array([b[0], b[1], 0])\n _plot_parallelogram(ax, a_xy, b_xy, \"#ffff00\", 0.5)\n a_xz = np.array([a[0], 0, a[2]])\n b_xz = np.array([b[0], 0, b[2]])\n _plot_parallelogram(ax, a_xz, b_xz, \"#ff00ff\", 0.5)\n a_yz = np.array([0, a[1], a[2]])\n b_yz = np.array([0, b[1], b[2]])\n _plot_parallelogram(ax, a_yz, b_yz, \"#00ffff\", 0.5)\n\n\ndef _plot_parallelogram(ax, a, b, color, alpha):\n \"\"\"Plot parallelogram.\n\n Parameters\n ----------\n ax : Matplotlib 3d axis\n Axis\n\n a : array, shape (3,)\n Vector that defines the first side\n\n b : array, shape (3,)\n Vector that defines the second side\n\n color : str\n Color\n\n alpha : float\n Alpha of surface\n \"\"\"\n from ..plot_utils import plot_vector\n from mpl_toolkits.mplot3d.art3d import Poly3DCollection\n\n plot_vector(ax, start=np.zeros(3), direction=a, color=color, alpha=alpha)\n plot_vector(ax, start=np.zeros(3), direction=b, color=color, alpha=alpha)\n verts = np.vstack((np.zeros(3), a, b, a + b, b, a))\n try: # Matplotlib < 3.5\n parallelogram = Poly3DCollection(verts)\n except ValueError: # Matplotlib >= 3.5\n parallelogram = Poly3DCollection(verts.reshape(2, 3, 3))\n parallelogram.set_facecolor(color)\n parallelogram.set_alpha(alpha)\n ax.add_collection3d(parallelogram)\n\n\ndef _arc_between_vectors(a, b): # pragma: no cover\n \"\"\"Compute arc defined by a around p with scaling s.\n\n Parameters\n ----------\n a : array-like, shape (3,)\n Vector\n\n b : array-like, shape (3,)\n Vector\n\n Returns\n -------\n arc : array, shape (100, 3)\n Coordinates of arc\n \"\"\"\n angle = angle_between_vectors(a, b)\n arc = np.empty((100, 3))\n for i, t in enumerate(np.linspace(0, angle, len(arc))):\n w1, w2 = slerp_weights(angle, t)\n arc[i] = 0.5 * (w1 * a + w2 * b)\n return arc\n" }, { "path": "pytransform3d/rotations/_plot.pyi", "content": "import numpy as np\nimport numpy.typing as npt\nfrom mpl_toolkits.mplot3d import Axes3D\nfrom typing import Union\n\ndef plot_basis(\n ax: Union[None, Axes3D] = ...,\n R: Union[None, npt.ArrayLike] = ...,\n p: npt.ArrayLike = ...,\n s: float = ...,\n ax_s: float = ...,\n strict_check: bool = ...,\n **kwargs,\n) -> Axes3D: ...\ndef plot_axis_angle(\n ax: Union[None, Axes3D] = ...,\n a: npt.ArrayLike = ...,\n p: npt.ArrayLike = ...,\n s: float = ...,\n ax_s: float = ...,\n **kwargs,\n) -> Axes3D: ...\ndef plot_bivector(\n ax: Union[None, Axes3D] = ...,\n a: Union[None, npt.ArrayLike] = ...,\n b: Union[None, npt.ArrayLike] = ...,\n ax_s: float = ...,\n): ...\n" }, { "path": "pytransform3d/rotations/_polar_decomp.py", "content": "\"\"\"Polar decomposition.\"\"\"\n\nimport numpy as np\n\nfrom ._axis_angle import matrix_from_compact_axis_angle\n\n\ndef robust_polar_decomposition(A, n_iter=20, eps=np.finfo(float).eps):\n r\"\"\"Orthonormalize rotation matrix with robust polar decomposition.\n\n Robust polar decomposition [1]_ [2]_ is a computationally more costly\n method, but it spreads the error more evenly between the basis vectors\n in comparison to Gram-Schmidt orthonormalization (as in\n :func:`norm_matrix`).\n\n Robust polar decomposition finds an orthonormal matrix that minimizes the\n Frobenius norm\n\n .. math::\n\n ||\\boldsymbol{A} - \\boldsymbol{R}||^2\n\n between the input :math:`\\boldsymbol{A}` that is not orthonormal and the\n output :math:`\\boldsymbol{R}` that is orthonormal.\n\n Parameters\n ----------\n A : array-like, shape (3, 3)\n Matrix that contains a basis vector in each column. The basis does not\n have to be orthonormal.\n\n n_iter : int, optional (default: 20)\n Maximum number of iterations for which we refine the estimation of the\n rotation matrix.\n\n eps : float, optional (default: np.finfo(float).eps)\n Precision for termination criterion of iterative refinement.\n\n Returns\n -------\n R : array, shape (3, 3)\n Orthonormalized rotation matrix.\n\n See Also\n --------\n norm_matrix\n The cheaper default orthonormalization method that uses Gram-Schmidt\n orthonormalization optimized for 3 dimensions.\n\n References\n ----------\n .. [1] Selstad, J. (2019). Orthonormalization.\n https://zalo.github.io/blog/polar-decomposition/\n\n .. [2] Müller, M., Bender, J., Chentanez, N., Macklin, M. (2016).\n A Robust Method to Extract the Rotational Part of Deformations.\n In MIG '16: Proceedings of the 9th International Conference on Motion in\n Games, pp. 55-60, doi: 10.1145/2994258.2994269.\n \"\"\"\n current_R = np.eye(3)\n for _ in range(n_iter):\n column_vector_cross_products = np.cross(\n current_R, A, axisa=0, axisb=0, axisc=1\n )\n column_vector_dot_products_sum = np.sum(current_R * A)\n omega = column_vector_cross_products.sum(axis=0) / (\n abs(column_vector_dot_products_sum) + eps\n )\n if np.linalg.norm(omega) < eps:\n break\n current_R = np.dot(matrix_from_compact_axis_angle(omega), current_R)\n return current_R\n" }, { "path": "pytransform3d/rotations/_polar_decomp.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef robust_polar_decomposition(\n A: npt.ArrayLike, n_iter: int = ..., eps: float = ...\n) -> np.ndarray: ...\n" }, { "path": "pytransform3d/rotations/_quaternion.py", "content": "\"\"\"Quaternion operations.\"\"\"\n\nimport numpy as np\nfrom numpy.testing import assert_array_almost_equal\n\nfrom ._angle import quaternion_from_angle\nfrom ._axis_angle import (\n norm_axis_angle,\n quaternion_from_compact_axis_angle,\n compact_axis_angle,\n)\nfrom ._utils import check_axis_index, norm_vector\n\n\ndef check_quaternion(q, unit=True):\n \"\"\"Input validation of quaternion representation.\n\n Parameters\n ----------\n q : array-like, shape (4,)\n Quaternion to represent rotation: (w, x, y, z)\n\n unit : bool, optional (default: True)\n Normalize the quaternion so that it is a unit quaternion\n\n Returns\n -------\n q : array, shape (4,)\n Validated quaternion to represent rotation: (w, x, y, z)\n\n Raises\n ------\n ValueError\n If input is invalid\n \"\"\"\n q = np.asarray(q, dtype=np.float64)\n if q.ndim != 1 or q.shape[0] != 4:\n raise ValueError(\n \"Expected quaternion with shape (4,), got \"\n \"array-like object with shape %s\" % (q.shape,)\n )\n if unit:\n return norm_vector(q)\n return q\n\n\ndef check_quaternions(Q, unit=True):\n \"\"\"Input validation of quaternion representation.\n\n Parameters\n ----------\n Q : array-like, shape (n_steps, 4)\n Quaternions to represent rotations: (w, x, y, z)\n\n unit : bool, optional (default: True)\n Normalize the quaternions so that they are unit quaternions\n\n Returns\n -------\n Q : array, shape (n_steps, 4)\n Validated quaternions to represent rotations: (w, x, y, z)\n\n Raises\n ------\n ValueError\n If input is invalid\n \"\"\"\n Q_checked = np.asarray(Q, dtype=np.float64)\n if Q_checked.ndim != 2 or Q_checked.shape[1] != 4:\n raise ValueError(\n \"Expected quaternion array with shape (n_steps, 4), got \"\n \"array-like object with shape %s\" % (Q_checked.shape,)\n )\n if unit:\n for i in range(len(Q)):\n Q_checked[i] = norm_vector(Q_checked[i])\n return Q_checked\n\n\ndef quaternion_requires_renormalization(q, tolerance=1e-6):\n r\"\"\"Check if a unit quaternion requires renormalization.\n\n Quaternions that represent rotations should have unit norm, so we check\n :math:`||\\boldsymbol{q}|| \\approx 1`.\n\n Parameters\n ----------\n q : array-like, shape (4,)\n Quaternion to represent rotation: (w, x, y, z)\n\n tolerance : float, optional (default: 1e-6)\n Tolerance for check.\n\n Returns\n -------\n required : bool\n Renormalization is required.\n\n See Also\n --------\n check_quaternion : Normalizes quaternion.\n \"\"\"\n return abs(np.linalg.norm(q) - 1.0) > tolerance\n\n\ndef quaternion_double(q):\n \"\"\"Create another quaternion that represents the same orientation.\n\n The unit quaternions q and -q represent the same orientation (double\n cover).\n\n Parameters\n ----------\n q : array-like, shape (4,)\n Unit quaternion.\n\n Returns\n -------\n q_double : array, shape (4,)\n -q\n\n See Also\n --------\n pick_closest_quaternion\n Picks the quaternion that is closest to another one in Euclidean space.\n \"\"\"\n return -check_quaternion(q, unit=True)\n\n\ndef pick_closest_quaternion(quaternion, target_quaternion):\n \"\"\"Resolve quaternion ambiguity and pick the closest one to the target.\n\n .. warning::\n There are always two quaternions that represent the exact same\n orientation: q and -q.\n\n Parameters\n ----------\n quaternion : array-like, shape (4,)\n Quaternion (w, x, y, z) of which we are unsure whether we want to\n select quaternion or -quaternion.\n\n target_quaternion : array-like, shape (4,)\n Target quaternion (w, x, y, z) to which we want to be close.\n\n Returns\n -------\n closest_quaternion : array, shape (4,)\n Quaternion that is closest (Euclidean norm) to the target quaternion.\n \"\"\"\n quaternion = check_quaternion(quaternion)\n target_quaternion = check_quaternion(target_quaternion)\n return pick_closest_quaternion_impl(quaternion, target_quaternion)\n\n\ndef pick_closest_quaternion_impl(quaternion, target_quaternion):\n \"\"\"Resolve quaternion ambiguity and pick the closest one to the target.\n\n This is an internal function that does not validate the inputs.\n\n Parameters\n ----------\n quaternion : array, shape (4,)\n Quaternion (w, x, y, z) of which we are unsure whether we want to\n select quaternion or -quaternion.\n\n target_quaternion : array, shape (4,)\n Target quaternion (w, x, y, z) to which we want to be close.\n\n Returns\n -------\n closest_quaternion : array, shape (4,)\n Quaternion that is closest (Euclidean norm) to the target quaternion.\n \"\"\"\n if np.linalg.norm(-quaternion - target_quaternion) < np.linalg.norm(\n quaternion - target_quaternion\n ):\n return -quaternion\n return quaternion\n\n\ndef assert_quaternion_equal(q1, q2, *args, **kwargs):\n \"\"\"Raise an assertion if two quaternions are not approximately equal.\n\n Note that quaternions are equal either if q1 == q2 or if q1 == -q2. See\n numpy.testing.assert_array_almost_equal for a more detailed documentation\n of the other parameters.\n\n Parameters\n ----------\n q1 : array-like, shape (4,)\n Unit quaternion to represent rotation: (w, x, y, z)\n\n q2 : array-like, shape (4,)\n Unit quaternion to represent rotation: (w, x, y, z)\n\n args : tuple\n Positional arguments that will be passed to\n `assert_array_almost_equal`\n\n kwargs : dict\n Positional arguments that will be passed to\n `assert_array_almost_equal`\n \"\"\"\n try:\n assert_array_almost_equal(q1, q2, *args, **kwargs)\n except AssertionError:\n assert_array_almost_equal(q1, -q2, *args, **kwargs)\n\n\ndef quaternion_integrate(Qd, q0=np.array([1.0, 0.0, 0.0, 0.0]), dt=1.0):\n \"\"\"Integrate angular velocities to quaternions.\n\n Angular velocities are given in global frame and will be left-multiplied\n to the initial or previous orientation respectively.\n\n Parameters\n ----------\n Qd : array-like, shape (n_steps, 3)\n Angular velocities in a compact axis-angle representation. Each angular\n velocity represents the rotational offset after one unit of time.\n\n q0 : array-like, shape (4,), optional (default: [1, 0, 0, 0])\n Unit quaternion to represent initial orientation: (w, x, y, z)\n\n dt : float, optional (default: 1)\n Time interval between steps.\n\n Returns\n -------\n Q : array-like, shape (n_steps, 4)\n Quaternions to represent rotations: (w, x, y, z)\n \"\"\"\n Q = np.empty((len(Qd), 4))\n Q[0] = q0\n for t in range(1, len(Qd)):\n qd = (Qd[t] + Qd[t - 1]) / 2.0\n Q[t] = concatenate_quaternions(\n quaternion_from_compact_axis_angle(dt * qd), Q[t - 1]\n )\n return Q\n\n\ndef quaternion_gradient(Q, dt=1.0):\n \"\"\"Time-derivatives of a sequence of quaternions.\n\n Note that this function does not provide the exact same functionality for\n quaternions as `NumPy's gradient function\n `_\n for positions. Gradients are always computed as central differences except\n the first and last gradient. We additionally accept a parameter dt that\n defines the time interval between each quaternion. Note that this means\n that we expect this to be constant for the whole sequence.\n\n Parameters\n ----------\n Q : array-like, shape (n_steps, 4)\n Quaternions to represent rotations: (w, x, y, z)\n\n dt : float, optional (default: 1)\n Time interval between steps. If you have non-constant dt, you can pass\n 1 and manually divide angular velocities by their corresponding time\n interval afterwards.\n\n Returns\n -------\n A : array, shape (n_steps, 3)\n Angular velocities in a compact axis-angle representation. Each angular\n velocity represents the rotational offset after one unit of time.\n Angular velocities are given in global frame and will be\n left-multiplied during integration to the initial or previous\n orientation respectively.\n \"\"\"\n Q = check_quaternions(Q)\n Qd = np.empty((len(Q), 3))\n Qd[0] = (\n compact_axis_angle_from_quaternion(\n concatenate_quaternions(Q[1], q_conj(Q[0]))\n )\n / dt\n )\n for t in range(1, len(Q) - 1):\n # divided by two because of central differences\n Qd[t] = compact_axis_angle_from_quaternion(\n concatenate_quaternions(Q[t + 1], q_conj(Q[t - 1]))\n ) / (2.0 * dt)\n Qd[-1] = (\n compact_axis_angle_from_quaternion(\n concatenate_quaternions(Q[-1], q_conj(Q[-2]))\n )\n / dt\n )\n return Qd\n\n\ndef concatenate_quaternions(q1, q2):\n r\"\"\"Concatenate two quaternions.\n\n We concatenate two quaternions by quaternion multiplication\n :math:`\\boldsymbol{q}_1\\boldsymbol{q}_2`.\n\n We use Hamilton's quaternion multiplication.\n\n If the two quaternions are divided up into scalar part and vector part\n each, i.e.,\n :math:`\\boldsymbol{q} = (w, \\boldsymbol{v}), w \\in \\mathbb{R},\n \\boldsymbol{v} \\in \\mathbb{R}^3`, then the quaternion product is\n\n .. math::\n\n \\boldsymbol{q}_{12} =\n (w_1 w_2 - \\boldsymbol{v}_1 \\cdot \\boldsymbol{v}_2,\n w_1 \\boldsymbol{v}_2 + w_2 \\boldsymbol{v}_1\n + \\boldsymbol{v}_1 \\times \\boldsymbol{v}_2)\n\n with the scalar product :math:`\\cdot` and the cross product :math:`\\times`.\n\n Parameters\n ----------\n q1 : array-like, shape (4,)\n First quaternion\n\n q2 : array-like, shape (4,)\n Second quaternion\n\n Returns\n -------\n q12 : array, shape (4,)\n Quaternion that represents the concatenated rotation q1 * q2\n\n See Also\n --------\n concatenate_rotors : Concatenate rotors, which is the same operation.\n \"\"\"\n q1 = check_quaternion(q1, unit=False)\n q2 = check_quaternion(q2, unit=False)\n q12 = np.empty(4)\n q12[0] = q1[0] * q2[0] - np.dot(q1[1:], q2[1:])\n q12[1:] = q1[0] * q2[1:] + q2[0] * q1[1:] + np.cross(q1[1:], q2[1:])\n return q12\n\n\ndef q_prod_vector(q, v):\n r\"\"\"Apply rotation represented by a quaternion to a vector.\n\n We use Hamilton's quaternion multiplication.\n\n To apply the rotation defined by a unit quaternion :math:`\\boldsymbol{q}\n \\in S^3` to a vector :math:`\\boldsymbol{v} \\in \\mathbb{R}^3`, we\n first represent the vector as a quaternion: we set the scalar part to 0 and\n the vector part is exactly the original vector\n :math:`\\left(\\begin{array}{c}0\\\\\\boldsymbol{v}\\end{array}\\right) \\in\n \\mathbb{R}^4`. Then we left-multiply the quaternion and right-multiply\n its conjugate\n\n .. math::\n\n \\left(\\begin{array}{c}0\\\\\\boldsymbol{w}\\end{array}\\right)\n =\n \\boldsymbol{q}\n \\cdot\n \\left(\\begin{array}{c}0\\\\\\boldsymbol{v}\\end{array}\\right)\n \\cdot\n \\boldsymbol{q}^*.\n\n The vector part :math:`\\boldsymbol{w}` of the resulting quaternion is\n the rotated vector.\n\n Parameters\n ----------\n q : array-like, shape (4,)\n Unit quaternion to represent rotation: (w, x, y, z)\n\n v : array-like, shape (3,)\n 3d vector\n\n Returns\n -------\n w : array, shape (3,)\n 3d vector\n\n See Also\n --------\n rotor_apply\n The same operation with a different name.\n\n concatenate_quaternions\n Hamilton's quaternion multiplication.\n \"\"\"\n q = check_quaternion(q)\n t = 2 * np.cross(q[1:], v)\n return v + q[0] * t + np.cross(q[1:], t)\n\n\ndef q_conj(q):\n r\"\"\"Conjugate of quaternion.\n\n The conjugate of a quaternion :math:`\\boldsymbol{q}` is often denoted as\n :math:`\\boldsymbol{q}^*`. For a quaternion :math:`\\boldsymbol{q} = w\n + x \\boldsymbol{i} + y \\boldsymbol{j} + z \\boldsymbol{k}` it is defined as\n\n .. math::\n\n \\boldsymbol{q}^* = w - x \\boldsymbol{i} - y \\boldsymbol{j}\n - z \\boldsymbol{k}.\n\n The conjugate of a unit quaternion inverts the rotation represented by\n this unit quaternion.\n\n Parameters\n ----------\n q : array-like, shape (4,)\n Quaternion: (w, x, y, z)\n\n Returns\n -------\n q_c : array-like, shape (4,)\n Conjugate (w, -x, -y, -z)\n\n See Also\n --------\n rotor_reverse : Reverse of a rotor, which is the same operation.\n \"\"\"\n q = check_quaternion(q, unit=False)\n return np.array([q[0], -q[1], -q[2], -q[3]])\n\n\ndef quaternion_dist(q1, q2):\n r\"\"\"Compute distance between two quaternions.\n\n We use the angular metric of :math:`S^3`, which is defined as\n\n .. math::\n\n d(q_1, q_2) = \\min(|| \\log(q_1 * \\overline{q_2})||,\n 2 \\pi - || \\log(q_1 * \\overline{q_2})||)\n\n Parameters\n ----------\n q1 : array-like, shape (4,)\n First quaternion\n\n q2 : array-like, shape (4,)\n Second quaternion\n\n Returns\n -------\n dist : float\n Distance between q1 and q2\n \"\"\"\n q1 = check_quaternion(q1)\n q2 = check_quaternion(q2)\n q12c = concatenate_quaternions(q1, q_conj(q2))\n angle = axis_angle_from_quaternion(q12c)[-1]\n return min(angle, 2.0 * np.pi - angle)\n\n\ndef quaternion_diff(q1, q2):\n r\"\"\"Compute the rotation in angle-axis format that rotates q2 into q1.\n\n .. math::\n\n (\\hat{\\boldsymbol{\\omega}}, \\theta) = \\log (q_1 \\overline{q_2})\n\n Parameters\n ----------\n q1 : array-like, shape (4,)\n First quaternion\n\n q2 : array-line, shape (4,)\n Second quaternion\n\n Returns\n -------\n a : array, shape (4,)\n The rotation in angle-axis format that rotates q2 into q1\n \"\"\"\n q1 = check_quaternion(q1)\n q2 = check_quaternion(q2)\n q1q2c = concatenate_quaternions(q1, q_conj(q2))\n return axis_angle_from_quaternion(q1q2c)\n\n\ndef quaternion_from_euler(e, i, j, k, extrinsic):\n \"\"\"General conversion to quaternion from any Euler angles.\n\n Parameters\n ----------\n e : array-like, shape (3,)\n Rotation angles in radians about the axes i, j, k in this order. The\n first and last angle are normalized to [-pi, pi]. The middle angle is\n normalized to either [0, pi] (proper Euler angles) or [-pi/2, pi/2]\n (Cardan / Tait-Bryan angles).\n\n i : int from [0, 1, 2]\n The first rotation axis (0: x, 1: y, 2: z)\n\n j : int from [0, 1, 2]\n The second rotation axis (0: x, 1: y, 2: z)\n\n k : int from [0, 1, 2]\n The third rotation axis (0: x, 1: y, 2: z)\n\n extrinsic : bool\n Do we use extrinsic transformations? Intrinsic otherwise.\n\n Returns\n -------\n q : array, shape (4,)\n Unit quaternion to represent rotation: (w, x, y, z)\n\n Raises\n ------\n ValueError\n If basis is invalid\n \"\"\"\n check_axis_index(\"i\", i)\n check_axis_index(\"j\", j)\n check_axis_index(\"k\", k)\n\n q0 = quaternion_from_angle(i, e[0])\n q1 = quaternion_from_angle(j, e[1])\n q2 = quaternion_from_angle(k, e[2])\n if not extrinsic:\n q0, q2 = q2, q0\n return concatenate_quaternions(concatenate_quaternions(q2, q1), q0)\n\n\ndef matrix_from_quaternion(q):\n \"\"\"Compute rotation matrix from quaternion.\n\n This typically results in an active rotation matrix.\n\n Parameters\n ----------\n q : array-like, shape (4,)\n Unit quaternion to represent rotation: (w, x, y, z)\n\n Returns\n -------\n R : array-like, shape (3, 3)\n Rotation matrix\n \"\"\"\n q = check_quaternion(q, unit=True)\n w, x, y, z = q\n x2 = 2.0 * x * x\n y2 = 2.0 * y * y\n z2 = 2.0 * z * z\n xy = 2.0 * x * y\n xz = 2.0 * x * z\n yz = 2.0 * y * z\n xw = 2.0 * x * w\n yw = 2.0 * y * w\n zw = 2.0 * z * w\n\n R = np.array(\n [\n [1.0 - y2 - z2, xy - zw, xz + yw],\n [xy + zw, 1.0 - x2 - z2, yz - xw],\n [xz - yw, yz + xw, 1.0 - x2 - y2],\n ]\n )\n return R\n\n\ndef axis_angle_from_quaternion(q):\n \"\"\"Compute axis-angle from quaternion.\n\n This operation is called logarithmic map.\n\n We usually assume active rotations.\n\n Parameters\n ----------\n q : array-like, shape (4,)\n Unit quaternion to represent rotation: (w, x, y, z)\n\n Returns\n -------\n a : array, shape (4,)\n Axis of rotation and rotation angle: (x, y, z, angle). The angle is\n constrained to [0, pi) so that the mapping is unique.\n \"\"\"\n q = check_quaternion(q)\n p = q[1:]\n p_norm = np.linalg.norm(p)\n\n if p_norm < np.finfo(float).eps:\n return np.array([1.0, 0.0, 0.0, 0.0])\n\n axis = p / p_norm\n w_clamped = max(min(q[0], 1.0), -1.0)\n angle = (2.0 * np.arccos(w_clamped),)\n return norm_axis_angle(np.hstack((axis, angle)))\n\n\ndef compact_axis_angle_from_quaternion(q):\n \"\"\"Compute compact axis-angle from quaternion (logarithmic map).\n\n We usually assume active rotations.\n\n Parameters\n ----------\n q : array-like, shape (4,)\n Unit quaternion to represent rotation: (w, x, y, z)\n\n Returns\n -------\n a : array, shape (3,)\n Axis of rotation and rotation angle: angle * (x, y, z). The angle is\n constrained to [0, pi].\n \"\"\"\n a = axis_angle_from_quaternion(q)\n return compact_axis_angle(a)\n\n\ndef mrp_from_quaternion(q):\n \"\"\"Compute modified Rodrigues parameters from quaternion.\n\n Parameters\n ----------\n q : array-like, shape (4,)\n Unit quaternion to represent rotation: (w, x, y, z)\n\n Returns\n -------\n mrp : array, shape (3,)\n Modified Rodrigues parameters.\n \"\"\"\n q = check_quaternion(q)\n if q[0] < 0.0:\n q = -q\n return q[1:] / (1.0 + q[0])\n\n\ndef quaternion_xyzw_from_wxyz(q_wxyz):\n \"\"\"Converts from w, x, y, z to x, y, z, w convention.\n\n Parameters\n ----------\n q_wxyz : array-like, shape (4,)\n Quaternion with scalar part before vector part\n\n Returns\n -------\n q_xyzw : array, shape (4,)\n Quaternion with scalar part after vector part\n \"\"\"\n q_wxyz = check_quaternion(q_wxyz)\n return np.array([q_wxyz[1], q_wxyz[2], q_wxyz[3], q_wxyz[0]])\n\n\ndef quaternion_wxyz_from_xyzw(q_xyzw):\n \"\"\"Converts from x, y, z, w to w, x, y, z convention.\n\n Parameters\n ----------\n q_xyzw : array-like, shape (4,)\n Quaternion with scalar part after vector part\n\n Returns\n -------\n q_wxyz : array, shape (4,)\n Quaternion with scalar part before vector part\n \"\"\"\n q_xyzw = check_quaternion(q_xyzw)\n return np.array([q_xyzw[3], q_xyzw[0], q_xyzw[1], q_xyzw[2]])\n" }, { "path": "pytransform3d/rotations/_quaternion.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef check_quaternion(q: npt.ArrayLike, unit: bool = ...) -> np.ndarray: ...\ndef check_quaternions(Q: npt.ArrayLike, unit: bool = ...) -> np.ndarray: ...\ndef quaternion_requires_renormalization(\n q: npt.ArrayLike, tolerance: float = ...\n) -> bool: ...\ndef quaternion_double(q: npt.ArrayLike) -> np.ndarray: ...\ndef pick_closest_quaternion(\n quaternion: npt.ArrayLike, target_quaternion: npt.ArrayLike\n) -> np.ndarray: ...\ndef pick_closest_quaternion_impl(\n quaternion: np.ndarray, target_quaternion: np.ndarray\n) -> np.ndarray: ...\ndef assert_quaternion_equal(\n q1: npt.ArrayLike, q2: npt.ArrayLike, *args, **kwargs\n): ...\ndef quaternion_integrate(\n Qd: npt.ArrayLike, q0: npt.ArrayLike = ..., dt: float = ...\n) -> np.ndarray: ...\ndef quaternion_gradient(Q: npt.ArrayLike, dt: float = ...) -> np.ndarray: ...\ndef concatenate_quaternions(\n q1: npt.ArrayLike, q2: npt.ArrayLike\n) -> np.ndarray: ...\ndef q_prod_vector(q: npt.ArrayLike, v: npt.ArrayLike) -> np.ndarray: ...\ndef q_conj(q: npt.ArrayLike) -> np.ndarray: ...\ndef quaternion_dist(q1: npt.ArrayLike, q2: npt.ArrayLike) -> np.ndarray: ...\ndef quaternion_diff(q1: npt.ArrayLike, q2: npt.ArrayLike) -> np.ndarray: ...\ndef quaternion_from_euler(\n e: npt.ArrayLike, i: int, j: int, k: int, extrinsic: bool\n) -> np.ndarray: ...\ndef matrix_from_quaternion(q: npt.ArrayLike) -> np.ndarray: ...\ndef axis_angle_from_quaternion(q: npt.ArrayLike) -> np.ndarray: ...\ndef compact_axis_angle_from_quaternion(q: npt.ArrayLike) -> np.ndarray: ...\ndef mrp_from_quaternion(q: npt.ArrayLike) -> np.ndarray: ...\ndef quaternion_xyzw_from_wxyz(q_wxyz: npt.ArrayLike) -> np.ndarray: ...\ndef quaternion_wxyz_from_xyzw(q_xyzw: npt.ArrayLike) -> np.ndarray: ...\n" }, { "path": "pytransform3d/rotations/_random.py", "content": "\"\"\"Random rotations.\"\"\"\n\nimport numpy as np\n\nfrom ._axis_angle import matrix_from_compact_axis_angle\nfrom ._matrix import check_matrix, norm_matrix\nfrom ._utils import norm_vector\n\n\ndef random_vector(rng=np.random.default_rng(0), n=3):\n r\"\"\"Generate an nd vector with normally distributed components.\n\n Each component will be sampled from :math:`\\mathcal{N}(\\mu=0, \\sigma=1)`.\n\n Parameters\n ----------\n rng : np.random.Generator, optional (default: random seed 0)\n Random number generator\n\n n : int, optional (default: 3)\n Number of vector components\n\n Returns\n -------\n v : array, shape (n,)\n Random vector\n \"\"\"\n return rng.standard_normal(size=n)\n\n\ndef random_axis_angle(rng=np.random.default_rng(0)):\n r\"\"\"Generate random axis-angle.\n\n The angle will be sampled uniformly from the interval :math:`[0, \\pi)`\n and each component of the rotation axis will be sampled from\n :math:`\\mathcal{N}(\\mu=0, \\sigma=1)` and then the axis will be normalized\n to length 1.\n\n Parameters\n ----------\n rng : np.random.Generator, optional (default: random seed 0)\n Random number generator\n\n Returns\n -------\n a : array, shape (4,)\n Axis of rotation and rotation angle: (x, y, z, angle)\n \"\"\"\n angle = np.pi * rng.random()\n a = np.array([0, 0, 0, angle])\n a[:3] = norm_vector(rng.standard_normal(size=3))\n return a\n\n\ndef random_compact_axis_angle(rng=np.random.default_rng(0)):\n r\"\"\"Generate random compact axis-angle.\n\n The angle will be sampled uniformly from the interval :math:`[0, \\pi)`\n and each component of the rotation axis will be sampled from\n :math:`\\mathcal{N}(\\mu=0, \\sigma=1)` and then the axis will be normalized\n to length 1.\n\n Parameters\n ----------\n rng : np.random.Generator, optional (default: random seed 0)\n Random number generator\n\n Returns\n -------\n a : array, shape (3,)\n Axis of rotation and rotation angle: angle * (x, y, z)\n \"\"\"\n a = random_axis_angle(rng)\n return a[:3] * a[3]\n\n\ndef random_quaternion(rng=np.random.default_rng(0)):\n r\"\"\"Generate uniform random quaternion.\n\n This is similar to scipy's implementation of uniform random sampling of\n rotations. It is based on Shoemake's algorithm [1]_, but we simplify the\n implementation by sampling a quaternion from a multivariate standard\n normal distribution and normalize it. In this way, we obtain a uniform\n distribution over rotation axes and a distribution proportional to\n :math:`\\sin^2(\\theta/2)` over the rotation angles, which together\n constitute the Haar measure on :math:`SO(3)`.\n\n Parameters\n ----------\n rng : np.random.Generator, optional (default: random seed 0)\n Random number generator\n\n Returns\n -------\n q : array, shape (4,)\n Unit quaternion to represent rotation: (w, x, y, z)\n\n References\n ----------\n .. [1] Shoemake, K. (1992). Uniform Random Rotations. Graphics Gems III,\n pages 124-132. Academic, New York.\n DOI: 10.1016/B978-0-08-050755-2.50036-1\n \"\"\"\n return norm_vector(rng.standard_normal(size=4))\n\n\ndef random_matrix(rng=np.random.default_rng(0), mean=np.eye(3), cov=np.eye(3)):\n r\"\"\"Generate random rotation matrix.\n\n Generate :math:`\\Delta \\boldsymbol{R}_{B_{i+1}{B_i}}\n \\boldsymbol{R}_{{B_i}A}`, with :math:`\\Delta \\boldsymbol{R}_{B_{i+1}{B_i}}\n = Exp(\\hat{\\omega} \\theta)` and :math:`\\hat{\\omega}\\theta \\sim\n \\mathcal{N}(\\boldsymbol{0}_3, \\boldsymbol{\\Sigma}_{3 \\times 3})`.\n The mean :math:`\\boldsymbol{R}_{{B_i}A}` and the covariance\n :math:`\\boldsymbol{\\Sigma}_{3 \\times 3}` are parameters of the function.\n\n Note that uncertainty is defined in the global frame B, not in the\n body frame A.\n\n Parameters\n ----------\n rng : np.random.Generator, optional (default: random seed 0)\n Random number generator.\n\n mean : array-like, shape (3, 3), optional (default: I)\n Mean rotation matrix.\n\n cov : array-like, shape (3, 3), optional (default: I)\n Covariance of noise in exponential coordinate space.\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n mean = check_matrix(mean)\n a = rng.multivariate_normal(mean=np.zeros(3), cov=cov)\n delta = matrix_from_compact_axis_angle(a)\n return norm_matrix(np.dot(delta, mean))\n" }, { "path": "pytransform3d/rotations/_random.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef random_vector(\n rng: np.random.Generator = ..., n: int = ...\n) -> np.ndarray: ...\ndef random_axis_angle(rng: np.random.Generator = ...) -> np.ndarray: ...\ndef random_compact_axis_angle(rng: np.random.Generator = ...) -> np.ndarray: ...\ndef random_quaternion(rng: np.random.Generator = ...) -> np.ndarray: ...\ndef random_matrix(\n rng: np.random.Generator = ...,\n mean: npt.ArrayLike = ...,\n cov: npt.ArrayLike = ...,\n) -> np.ndarray: ...\n" }, { "path": "pytransform3d/rotations/_rot_log.py", "content": "\"\"\"Logarithm of rotation.\"\"\"\n\nimport warnings\n\nimport numpy as np\n\n\ndef check_skew_symmetric_matrix(V, tolerance=1e-6, strict_check=True):\n \"\"\"Input validation of a skew-symmetric matrix.\n\n Check whether the transpose of the matrix is its negative:\n\n .. math::\n\n V^T = -V\n\n Parameters\n ----------\n V : array-like, shape (3, 3)\n Cross-product matrix\n\n tolerance : float, optional (default: 1e-6)\n Tolerance threshold for checks.\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if V.T is not numerically close enough to -V.\n Otherwise we print a warning.\n\n Returns\n -------\n V : array, shape (3, 3)\n Validated cross-product matrix\n\n Raises\n ------\n ValueError\n If input is invalid\n \"\"\"\n V = np.asarray(V, dtype=np.float64)\n if V.ndim != 2 or V.shape[0] != 3 or V.shape[1] != 3:\n raise ValueError(\n \"Expected skew-symmetric matrix with shape (3, 3), \"\n \"got array-like object with shape %s\" % (V.shape,)\n )\n if not np.allclose(V.T, -V, atol=tolerance):\n error_msg = (\n \"Expected skew-symmetric matrix, but it failed the test \"\n \"V.T = %r\\n-V = %r\" % (V.T, -V)\n )\n if strict_check:\n raise ValueError(error_msg)\n warnings.warn(error_msg, UserWarning, stacklevel=2)\n return V\n\n\ncheck_rot_log = check_skew_symmetric_matrix\n\n\ndef cross_product_matrix(v):\n r\"\"\"Generate the cross-product matrix of a vector.\n\n The cross-product matrix :math:`\\boldsymbol{V}` satisfies the equation\n\n .. math::\n\n \\boldsymbol{V} \\boldsymbol{w} = \\boldsymbol{v} \\times\n \\boldsymbol{w}.\n\n It is a skew-symmetric (antisymmetric) matrix, i.e.,\n :math:`-\\boldsymbol{V} = \\boldsymbol{V}^T`. Its elements are\n\n .. math::\n\n \\left[\\boldsymbol{v}\\right]\n =\n \\left[\\begin{array}{c}\n v_1\\\\ v_2\\\\ v_3\n \\end{array}\\right]\n =\n \\boldsymbol{V}\n =\n \\left(\\begin{array}{ccc}\n 0 & -v_3 & v_2\\\\\n v_3 & 0 & -v_1\\\\\n -v_2 & v_1 & 0\n \\end{array}\\right).\n\n The function can also be used to compute the logarithm of rotation from\n a compact axis-angle representation.\n\n Parameters\n ----------\n v : array-like, shape (3,)\n 3d vector\n\n Returns\n -------\n V : array, shape (3, 3)\n Cross-product matrix\n \"\"\"\n return np.array(\n [[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]]\n )\n\n\nrot_log_from_compact_axis_angle = cross_product_matrix\n" }, { "path": "pytransform3d/rotations/_rot_log.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef check_skew_symmetric_matrix(\n V: npt.ArrayLike, tolerance: float = ..., strict_check: bool = ...\n) -> np.ndarray: ...\ndef check_rot_log(\n V: npt.ArrayLike, tolerance: float = ..., strict_check: bool = ...\n) -> np.ndarray: ...\ndef cross_product_matrix(v: npt.ArrayLike) -> np.ndarray: ...\ndef rot_log_from_compact_axis_angle(v: npt.ArrayLike) -> np.ndarray: ...\n" }, { "path": "pytransform3d/rotations/_rotors.py", "content": "\"\"\"Rotor operations.\"\"\"\n\nimport numpy as np\n\nfrom ._constants import unitx, unity, unitz, eps\nfrom ._quaternion import concatenate_quaternions, q_prod_vector\nfrom ._utils import norm_vector, perpendicular_to_vector\n\n\ndef check_rotor(rotor):\n \"\"\"Input validation of rotor.\n\n Parameters\n ----------\n rotor : array-like, shape (4,)\n Rotor: (a, b_yz, b_zx, b_xy)\n\n Returns\n -------\n rotor : array, shape (4,)\n Validated rotor (with unit norm): (a, b_yz, b_zx, b_xy)\n\n Raises\n ------\n ValueError\n If input is invalid\n \"\"\"\n rotor = np.asarray(rotor, dtype=np.float64)\n if rotor.ndim != 1 or rotor.shape[0] != 4:\n raise ValueError(\n \"Expected rotor with shape (4,), got \"\n \"array-like object with shape %s\" % (rotor.shape,)\n )\n return norm_vector(rotor)\n\n\ndef wedge(a, b):\n r\"\"\"Outer product of two vectors (also exterior or wedge product).\n\n .. math::\n\n B = a \\wedge b\n\n Parameters\n ----------\n a : array-like, shape (3,)\n Vector: (x, y, z)\n\n b : array-like, shape (3,)\n Vector: (x, y, z)\n\n Returns\n -------\n B : array, shape (3,)\n Bivector that defines the plane that a and b form together:\n (b_yz, b_zx, b_xy)\n \"\"\"\n return np.cross(a, b)\n\n\ndef plane_normal_from_bivector(B):\n \"\"\"Convert bivector to normal vector of a plane.\n\n Parameters\n ----------\n B : array-like, shape (3,)\n Bivector that defines a plane: (b_yz, b_zx, b_xy)\n\n Returns\n -------\n n : array, shape (3,)\n Unit normal of the corresponding plane: (x, y, z)\n \"\"\"\n return norm_vector(B)\n\n\ndef geometric_product(a, b):\n r\"\"\"Geometric product of two vectors.\n\n The geometric product consists of the symmetric inner / dot product and the\n antisymmetric outer product (see :func:`~pytransform3d.rotations.wedge`) of\n two vectors.\n\n .. math::\n\n ab = a \\cdot b + a \\wedge b\n\n The inner product contains the cosine and the outer product contains the\n sine of the angle of rotation from a to b.\n\n Parameters\n ----------\n a : array-like, shape (3,)\n Vector: (x, y, z)\n\n b : array-like, shape (3,)\n Vector: (x, y, z)\n\n Returns\n -------\n ab : array, shape (4,)\n A multivector (a, b_yz, b_zx, b_xy) composed of scalar and bivector\n (b_yz, b_zx, b_xy) that form the geometric product of vectors a and b.\n \"\"\"\n return np.hstack(((np.dot(a, b),), wedge(a, b)))\n\n\ndef rotor_reverse(rotor):\n \"\"\"Invert rotor.\n\n Parameters\n ----------\n rotor : array-like, shape (4,)\n Rotor: (a, b_yz, b_zx, b_xy)\n\n Returns\n -------\n reverse_rotor : array, shape (4,)\n Reverse of the rotor: (a, b_yz, b_zx, b_xy)\n\n See Also\n --------\n q_conj : Quaternion conjugate, which is the same operation.\n \"\"\"\n rotor = check_rotor(rotor)\n return np.hstack(((rotor[0],), -rotor[1:]))\n\n\ndef concatenate_rotors(rotor1, rotor2):\n \"\"\"Concatenate rotors.\n\n Suppose we want to apply two extrinsic rotations given by rotors\n R1 and R2 to a vector v. We can either apply R2 to v and then R1 to\n the result or we can concatenate R1 and R2 and apply the result to v.\n\n Parameters\n ----------\n rotor1 : array-like, shape (4,)\n Rotor: (a, b_yz, b_zx, b_xy)\n\n rotor2 : array-like, shape (4,)\n Rotor: (a, b_yz, b_zx, b_xy)\n\n Returns\n -------\n rotor : array, shape (4,)\n rotor1 applied to rotor2: (a, b_yz, b_zx, b_xy)\n\n See Also\n --------\n concatenate_quaternions : Concatenate quaternions, which is the same\n operation.\n \"\"\"\n rotor1 = check_rotor(rotor1)\n rotor2 = check_rotor(rotor2)\n return concatenate_quaternions(rotor1, rotor2)\n\n\ndef rotor_apply(rotor, v):\n r\"\"\"Apply rotor to vector.\n\n .. math::\n\n v' = R v R^*\n\n Parameters\n ----------\n rotor : array-like, shape (4,)\n Rotor: (a, b_yz, b_zx, b_xy)\n\n v : array-like, shape (3,)\n Vector: (x, y, z)\n\n Returns\n -------\n v : array, shape (3,)\n Rotated vector\n\n See Also\n --------\n q_prod_vector : The same operation with a different name.\n \"\"\"\n rotor = check_rotor(rotor)\n return q_prod_vector(rotor, v)\n\n\ndef matrix_from_rotor(rotor):\n \"\"\"Compute rotation matrix from rotor.\n\n Parameters\n ----------\n rotor : array-like, shape (4,)\n Rotor: (a, b_yz, b_zx, b_xy)\n\n Returns\n -------\n R : array, shape (3, 3)\n Rotation matrix\n \"\"\"\n rotor = check_rotor(rotor)\n return np.column_stack(\n (\n rotor_apply(rotor, unitx),\n rotor_apply(rotor, unity),\n rotor_apply(rotor, unitz),\n )\n )\n\n\ndef rotor_from_two_directions(v_from, v_to):\n \"\"\"Construct the rotor that rotates one vector to another.\n\n Parameters\n ----------\n v_from : array-like, shape (3,)\n Unit vector (will be normalized internally)\n\n v_to : array-like, shape (3,)\n Unit vector (will be normalized internally)\n\n Returns\n -------\n rotor : array, shape (4,)\n Rotor: (a, b_yz, b_zx, b_xy)\n \"\"\"\n v_from = norm_vector(v_from)\n v_to = norm_vector(v_to)\n cos_angle_p1 = 1.0 + np.dot(v_from, v_to)\n if cos_angle_p1 < eps:\n # There is an infinite number of solutions for the plane of rotation.\n # This solution works with our convention, since the rotation axis is\n # the same as the plane bivector.\n plane = perpendicular_to_vector(v_from)\n else:\n plane = wedge(v_from, v_to)\n multivector = np.hstack(((cos_angle_p1,), plane))\n return norm_vector(multivector)\n\n\ndef rotor_from_plane_angle(B, angle):\n r\"\"\"Compute rotor from plane bivector and angle.\n\n Parameters\n ----------\n B : array-like, shape (3,)\n Unit bivector (b_yz, b_zx, b_xy) that represents the plane of rotation\n (will be normalized internally)\n\n angle : float\n Rotation angle\n\n Returns\n -------\n rotor : array, shape (4,)\n Rotor: (a, b_yz, b_zx, b_xy)\n \"\"\"\n a = np.cos(angle / 2.0)\n sina = np.sin(angle / 2.0)\n B = norm_vector(B)\n return np.hstack(((a,), sina * B))\n" }, { "path": "pytransform3d/rotations/_rotors.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef check_rotor(a: npt.ArrayLike) -> np.ndarray: ...\ndef wedge(a: npt.ArrayLike, b: npt.ArrayLike) -> np.ndarray: ...\ndef plane_normal_from_bivector(B: npt.ArrayLike) -> np.ndarray: ...\ndef geometric_product(a: npt.ArrayLike, b: npt.ArrayLike) -> np.ndarray: ...\ndef rotor_reverse(rotor: npt.ArrayLike) -> np.ndarray: ...\ndef concatenate_rotors(\n rotor1: npt.ArrayLike, rotor2: npt.ArrayLike\n) -> np.ndarray: ...\ndef rotor_apply(rotor: npt.ArrayLike, v: npt.ArrayLike) -> np.ndarray: ...\ndef matrix_from_rotor(rotor: npt.ArrayLike) -> np.ndarray: ...\ndef rotor_from_two_directions(\n v_from: npt.ArrayLike, v_to: npt.ArrayLike\n) -> np.ndarray: ...\ndef rotor_from_plane_angle(B: npt.ArrayLike, angle: float) -> np.ndarray: ...\n" }, { "path": "pytransform3d/rotations/_slerp.py", "content": "\"\"\"Spherical linear interpolation (SLERP).\"\"\"\n\nimport numpy as np\n\nfrom ._axis_angle import check_axis_angle, matrix_from_compact_axis_angle\nfrom ._matrix import compact_axis_angle_from_matrix\nfrom ._quaternion import check_quaternion, pick_closest_quaternion_impl\nfrom ._rotors import check_rotor\nfrom ._utils import angle_between_vectors\n\n\ndef matrix_slerp(start, end, t):\n r\"\"\"Spherical linear interpolation (SLERP) for rotation matrices.\n\n We compute the difference between two orientations\n :math:`\\boldsymbol{R}_{AB} = \\boldsymbol{R}^{-1}_{WA}\\boldsymbol{R}_{WB}`,\n convert it to a rotation vector\n :math:`Log(\\boldsymbol{R}_{AB}) = \\boldsymbol{\\omega}_{AB}`, compute a\n fraction of it :math:`t\\boldsymbol{\\omega}_{AB}` with :math:`t \\in [0, 1]`,\n and use the exponential map to concatenate the fraction of the difference\n :math:`\\boldsymbol{R}_{WA} Exp(t\\boldsymbol{\\omega}_{AB})`.\n\n Parameters\n ----------\n start : array-like, shape (3, 3)\n Rotation matrix to represent start orientation.\n\n end : array-like, shape (3, 3)\n Rotation matrix to represent end orientation.\n\n t : float in [0, 1]\n Position between start and goal.\n\n Returns\n -------\n R_t : array, shape (3, 3)\n Interpolated orientation.\n\n See Also\n --------\n axis_angle_slerp :\n SLERP axis-angle representation.\n\n quaternion_slerp :\n SLERP for quaternions.\n\n rotor_slerp :\n SLERP for rotors.\n\n pytransform3d.transformations.pq_slerp :\n SLERP for position + quaternion.\n \"\"\"\n end2start = np.dot(np.transpose(start), end)\n return np.dot(start, matrix_power(end2start, t))\n\n\ndef matrix_power(R, t):\n r\"\"\"Compute power of a rotation matrix with respect to scalar.\n\n .. math::\n\n \\boldsymbol{R}^t\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix.\n\n t : float\n Exponent.\n\n Returns\n -------\n R_t : array, shape (3, 3)\n Rotation matrix.\n \"\"\"\n a = compact_axis_angle_from_matrix(R)\n return matrix_from_compact_axis_angle(a * t)\n\n\ndef axis_angle_slerp(start, end, t):\n \"\"\"Spherical linear interpolation.\n\n Parameters\n ----------\n start : array-like, shape (4,)\n Start axis of rotation and rotation angle: (x, y, z, angle)\n\n end : array-like, shape (4,)\n Goal axis of rotation and rotation angle: (x, y, z, angle)\n\n t : float in [0, 1]\n Position between start and end\n\n Returns\n -------\n a : array, shape (4,)\n Interpolated axis of rotation and rotation angle: (x, y, z, angle)\n\n See Also\n --------\n matrix_slerp :\n SLERP for rotation matrices.\n\n quaternion_slerp :\n SLERP for quaternions.\n\n rotor_slerp :\n SLERP for rotors.\n\n pytransform3d.transformations.pq_slerp :\n SLERP for position + quaternion.\n \"\"\"\n start = check_axis_angle(start)\n end = check_axis_angle(end)\n angle = angle_between_vectors(start[:3], end[:3])\n w1, w2 = slerp_weights(angle, t)\n w1 = np.array([w1, w1, w1, (1.0 - t)])\n w2 = np.array([w2, w2, w2, t])\n return w1 * start + w2 * end\n\n\ndef quaternion_slerp(start, end, t, shortest_path=False):\n \"\"\"Spherical linear interpolation.\n\n Parameters\n ----------\n start : array-like, shape (4,)\n Start unit quaternion to represent rotation: (w, x, y, z)\n\n end : array-like, shape (4,)\n End unit quaternion to represent rotation: (w, x, y, z)\n\n t : float in [0, 1]\n Position between start and end\n\n shortest_path : bool, optional (default: False)\n Resolve sign ambiguity before interpolation to find the shortest path.\n The end quaternion will be picked to be close to the start quaternion.\n\n Returns\n -------\n q : array, shape (4,)\n Interpolated unit quaternion to represent rotation: (w, x, y, z)\n\n See Also\n --------\n matrix_slerp :\n SLERP for rotation matrices.\n\n axis_angle_slerp :\n SLERP for axis-angle representation.\n\n rotor_slerp :\n SLERP for rotors.\n\n pytransform3d.transformations.pq_slerp :\n SLERP for position + quaternion.\n \"\"\"\n start = check_quaternion(start)\n end = check_quaternion(end)\n if shortest_path:\n end = pick_closest_quaternion_impl(end, start)\n angle = angle_between_vectors(start, end)\n w1, w2 = slerp_weights(angle, t)\n return w1 * start + w2 * end\n\n\ndef rotor_slerp(start, end, t, shortest_path=False):\n \"\"\"Spherical linear interpolation.\n\n Parameters\n ----------\n start : array-like, shape (4,)\n Rotor: (a, b_yz, b_zx, b_xy)\n\n end : array-like, shape (4,)\n Rotor: (a, b_yz, b_zx, b_xy)\n\n t : float in [0, 1]\n Position between start and end\n\n shortest_path : bool, optional (default: False)\n Resolve sign ambiguity before interpolation to find the shortest path.\n The end rotor will be picked to be close to the start rotor.\n\n Returns\n -------\n rotor : array, shape (4,)\n Interpolated rotor: (a, b_yz, b_zx, b_xy)\n\n See Also\n --------\n matrix_slerp :\n SLERP for rotation matrices.\n\n axis_angle_slerp :\n SLERP for axis-angle representation.\n\n quaternion_slerp :\n SLERP for quaternions.\n\n pytransform3d.transformations.pq_slerp :\n SLERP for position + quaternion.\n \"\"\"\n start = check_rotor(start)\n end = check_rotor(end)\n return quaternion_slerp(start, end, t, shortest_path)\n\n\ndef slerp_weights(angle, t):\n \"\"\"Compute weights of start and end for spherical linear interpolation.\n\n Parameters\n ----------\n angle : float\n Rotation angle.\n\n t : float or array, shape (n_steps,)\n Position between start and end\n\n Returns\n -------\n w1 : float or array, shape (n_steps,)\n Weights for quaternion 1\n\n w2 : float or array, shape (n_steps,)\n Weights for quaternion 2\n \"\"\"\n if angle == 0.0:\n return np.ones_like(t), np.zeros_like(t)\n return (\n np.sin((1.0 - t) * angle) / np.sin(angle),\n np.sin(t * angle) / np.sin(angle),\n )\n" }, { "path": "pytransform3d/rotations/_slerp.pyi", "content": "import numpy as np\nimport numpy.typing as npt\nfrom typing import Union\n\ndef matrix_slerp(\n start: npt.ArrayLike, end: npt.ArrayLike, t: float\n) -> np.ndarray: ...\ndef matrix_power(R: npt.ArrayLike, t: float) -> np.ndarray: ...\ndef axis_angle_slerp(\n start: npt.ArrayLike, end: npt.ArrayLike, t: float\n) -> np.ndarray: ...\ndef quaternion_slerp(\n start: npt.ArrayLike,\n end: npt.ArrayLike,\n t: float,\n shortest_path: bool = ...,\n) -> np.ndarray: ...\ndef rotor_slerp(\n start: npt.ArrayLike,\n end: npt.ArrayLike,\n t: float,\n shortest_path: bool = ...,\n) -> np.ndarray: ...\ndef slerp_weights(\n angle: float, t: Union[float, npt.ArrayLike]\n) -> Union[float, np.ndarray]: ...\n" }, { "path": "pytransform3d/rotations/_utils.py", "content": "\"\"\"Utility functions for rotations.\"\"\"\n\nimport math\n\nimport numpy as np\n\nfrom ._constants import unitz, eps\n\n\ndef check_axis_index(name, i):\n \"\"\"Checks axis index.\n\n Parameters\n ----------\n name : str\n Name of the axis. Required for the error message.\n\n i : int from [0, 1, 2]\n Index of the axis (0: x, 1: y, 2: z)\n\n Raises\n ------\n ValueError\n If basis is invalid\n \"\"\"\n if i not in [0, 1, 2]:\n raise ValueError(\"Axis index %s (%d) must be in [0, 1, 2]\" % (name, i))\n\n\ndef norm_vector(v):\n \"\"\"Normalize vector.\n\n Parameters\n ----------\n v : array-like, shape (n,)\n nd vector\n\n Returns\n -------\n u : array, shape (n,)\n nd unit vector with norm 1 or the zero vector\n \"\"\"\n norm = np.linalg.norm(v)\n if norm == 0.0:\n return v\n\n return np.asarray(v) / norm\n\n\ndef perpendicular_to_vectors(a, b):\n \"\"\"Compute perpendicular vector to two other vectors.\n\n Parameters\n ----------\n a : array-like, shape (3,)\n 3d vector\n\n b : array-like, shape (3,)\n 3d vector\n\n Returns\n -------\n c : array, shape (3,)\n 3d vector that is orthogonal to a and b\n \"\"\"\n return np.cross(a, b)\n\n\ndef perpendicular_to_vector(a):\n \"\"\"Compute perpendicular vector to one other vector.\n\n There is an infinite number of solutions to this problem. Thus, we\n restrict the solutions to [1, 0, z] and return [0, 0, 1] if the\n z component of a is 0.\n\n Parameters\n ----------\n a : array-like, shape (3,)\n 3d vector\n\n Returns\n -------\n b : array, shape (3,)\n A 3d vector that is orthogonal to a. It does not necessarily have\n unit length.\n \"\"\"\n if abs(a[2]) < eps:\n return np.copy(unitz)\n # Now that we solved the problem for [x, y, 0], we can solve it for all\n # other vectors by restricting solutions to [1, 0, z] and find z.\n # The dot product of orthogonal vectors is 0, thus\n # a[0] * 1 + a[1] * 0 + a[2] * z == 0 or -a[0] / a[2] = z\n return np.array([1.0, 0.0, -a[0] / a[2]])\n\n\ndef angle_between_vectors(a, b, fast=False):\n \"\"\"Compute angle between two vectors.\n\n Parameters\n ----------\n a : array-like, shape (n,)\n nd vector\n\n b : array-like, shape (n,)\n nd vector\n\n fast : bool, optional (default: False)\n Use fast implementation instead of numerically stable solution\n\n Returns\n -------\n angle : float\n Angle between a and b\n \"\"\"\n if len(a) != 3 or fast:\n return np.arccos(\n np.clip(\n np.dot(a, b) / (np.linalg.norm(a) * np.linalg.norm(b)),\n -1.0,\n 1.0,\n )\n )\n return np.arctan2(np.linalg.norm(np.cross(a, b)), np.dot(a, b))\n\n\ndef vector_projection(a, b):\n \"\"\"Orthogonal projection of vector a on vector b.\n\n Parameters\n ----------\n a : array-like, shape (3,)\n Vector a that will be projected on vector b\n\n b : array-like, shape (3,)\n Vector b on which vector a will be projected\n\n Returns\n -------\n a_on_b : array, shape (3,)\n Vector a\n \"\"\"\n b_norm_squared = np.dot(b, b)\n if b_norm_squared == 0.0:\n return np.zeros(3)\n return np.dot(a, b) * b / b_norm_squared\n\n\ndef plane_basis_from_normal(plane_normal):\n \"\"\"Compute two basis vectors of a plane from the plane's normal vector.\n\n Note that there are infinitely many solutions because any rotation of the\n basis vectors about the normal is also a solution. This function\n deterministically picks one of the solutions.\n\n The two basis vectors of the plane together with the normal form an\n orthonormal basis in 3D space and could be used as columns to form a\n rotation matrix.\n\n Parameters\n ----------\n plane_normal : array-like, shape (3,)\n Plane normal of unit length.\n\n Returns\n -------\n x_axis : array, shape (3,)\n x-axis of the plane.\n\n y_axis : array, shape (3,)\n y-axis of the plane.\n \"\"\"\n if abs(plane_normal[0]) >= abs(plane_normal[1]):\n # x or z is the largest magnitude component, swap them\n length = math.sqrt(\n plane_normal[0] * plane_normal[0]\n + plane_normal[2] * plane_normal[2]\n )\n x_axis = np.array(\n [-plane_normal[2] / length, 0.0, plane_normal[0] / length]\n )\n y_axis = np.array(\n [\n plane_normal[1] * x_axis[2],\n plane_normal[2] * x_axis[0] - plane_normal[0] * x_axis[2],\n -plane_normal[1] * x_axis[0],\n ]\n )\n else:\n # y or z is the largest magnitude component, swap them\n length = math.sqrt(\n plane_normal[1] * plane_normal[1]\n + plane_normal[2] * plane_normal[2]\n )\n x_axis = np.array(\n [0.0, plane_normal[2] / length, -plane_normal[1] / length]\n )\n y_axis = np.array(\n [\n plane_normal[1] * x_axis[2] - plane_normal[2] * x_axis[1],\n -plane_normal[0] * x_axis[2],\n plane_normal[0] * x_axis[1],\n ]\n )\n return x_axis, y_axis\n" }, { "path": "pytransform3d/rotations/_utils.pyi", "content": "import numpy as np\nimport numpy.typing as npt\nfrom typing import Tuple\n\ndef check_axis_index(name: str, i: int): ...\ndef norm_vector(v: npt.ArrayLike) -> np.ndarray: ...\ndef perpendicular_to_vectors(\n a: npt.ArrayLike, b: npt.ArrayLike\n) -> np.ndarray: ...\ndef perpendicular_to_vector(a: npt.ArrayLike) -> np.ndarray: ...\ndef angle_between_vectors(\n a: npt.ArrayLike, b: npt.ArrayLike, fast: bool = ...\n) -> float: ...\ndef vector_projection(a: npt.ArrayLike, b: npt.ArrayLike) -> np.ndarray: ...\ndef plane_basis_from_normal(\n plane_normal: npt.ArrayLike,\n) -> Tuple[np.ndarray, np.ndarray]: ...\n" }, { "path": "pytransform3d/rotations/test/test_angle.py", "content": "import numpy as np\nimport pytest\nfrom numpy.testing import assert_array_almost_equal, assert_array_equal\n\nimport pytransform3d.rotations as pr\n\n\ndef test_norm_angle():\n \"\"\"Test normalization of angle.\"\"\"\n rng = np.random.default_rng(0)\n a_norm = rng.uniform(-np.pi, np.pi, size=(100,))\n for b in np.linspace(-10.0 * np.pi, 10.0 * np.pi, 11):\n a = a_norm + b\n assert_array_almost_equal(pr.norm_angle(a), a_norm)\n\n assert pytest.approx(pr.norm_angle(-np.pi)) == np.pi\n assert pytest.approx(pr.norm_angle(np.pi)) == np.pi\n\n\ndef test_norm_angle_precision():\n # NOTE: it would be better if angles are divided into 1e16 numbers\n # to test precision of float64 but it is limited by memory\n a_norm = np.linspace(\n np.pi, -np.pi, num=1000000, endpoint=False, dtype=np.float64\n )[::-1]\n for b in np.linspace(-10.0 * np.pi, 10.0 * np.pi, 11):\n a = a_norm + b\n assert_array_almost_equal(pr.norm_angle(a), a_norm)\n\n # eps and epsneg around zero\n a_eps = np.array([np.finfo(np.float64).eps, -np.finfo(np.float64).eps])\n a_epsneg = np.array(\n [np.finfo(np.float64).epsneg, -np.finfo(np.float64).epsneg]\n )\n\n assert_array_equal(pr.norm_angle(a_eps), a_eps)\n assert_array_equal(pr.norm_angle(a_epsneg), a_epsneg)\n\n\ndef test_passive_matrix_from_angle():\n \"\"\"Sanity checks for rotation around basis vectors.\"\"\"\n with pytest.raises(ValueError, match=\"Basis must be in\"):\n pr.passive_matrix_from_angle(-1, 0)\n with pytest.raises(ValueError, match=\"Basis must be in\"):\n pr.passive_matrix_from_angle(3, 0)\n\n R = pr.passive_matrix_from_angle(0, -0.5 * np.pi)\n assert_array_almost_equal(R, np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]]))\n R = pr.passive_matrix_from_angle(0, 0.5 * np.pi)\n assert_array_almost_equal(R, np.array([[1, 0, 0], [0, 0, 1], [0, -1, 0]]))\n\n R = pr.passive_matrix_from_angle(1, -0.5 * np.pi)\n assert_array_almost_equal(R, np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]]))\n R = pr.passive_matrix_from_angle(1, 0.5 * np.pi)\n assert_array_almost_equal(R, np.array([[0, 0, -1], [0, 1, 0], [1, 0, 0]]))\n\n R = pr.passive_matrix_from_angle(2, -0.5 * np.pi)\n assert_array_almost_equal(R, np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]))\n R = pr.passive_matrix_from_angle(2, 0.5 * np.pi)\n assert_array_almost_equal(R, np.array([[0, 1, 0], [-1, 0, 0], [0, 0, 1]]))\n\n\ndef test_active_matrix_from_angle():\n \"\"\"Sanity checks for rotation around basis vectors.\"\"\"\n with pytest.raises(ValueError, match=\"Basis must be in\"):\n pr.active_matrix_from_angle(-1, 0)\n with pytest.raises(ValueError, match=\"Basis must be in\"):\n pr.active_matrix_from_angle(3, 0)\n\n rng = np.random.default_rng(21)\n for _ in range(20):\n basis = rng.integers(0, 3)\n angle = 2.0 * np.pi * rng.random() - np.pi\n R_passive = pr.passive_matrix_from_angle(basis, angle)\n R_active = pr.active_matrix_from_angle(basis, angle)\n assert_array_almost_equal(R_active, R_passive.T)\n\n\ndef test_active_rotation_is_default():\n \"\"\"Test that rotations are active by default.\"\"\"\n Rx = pr.active_matrix_from_angle(0, 0.5 * np.pi)\n ax = np.array([1, 0, 0, 0.5 * np.pi])\n qx = pr.quaternion_from_axis_angle(ax)\n assert_array_almost_equal(Rx, pr.matrix_from_axis_angle(ax))\n assert_array_almost_equal(Rx, pr.matrix_from_quaternion(qx))\n Ry = pr.active_matrix_from_angle(1, 0.5 * np.pi)\n ay = np.array([0, 1, 0, 0.5 * np.pi])\n qy = pr.quaternion_from_axis_angle(ay)\n assert_array_almost_equal(Ry, pr.matrix_from_axis_angle(ay))\n assert_array_almost_equal(Ry, pr.matrix_from_quaternion(qy))\n Rz = pr.active_matrix_from_angle(2, 0.5 * np.pi)\n az = np.array([0, 0, 1, 0.5 * np.pi])\n qz = pr.quaternion_from_axis_angle(az)\n assert_array_almost_equal(Rz, pr.matrix_from_axis_angle(az))\n assert_array_almost_equal(Rz, pr.matrix_from_quaternion(qz))\n\n\ndef test_quaternion_from_angle():\n \"\"\"Quaternion from rotation around basis vectors.\"\"\"\n with pytest.raises(ValueError, match=\"Basis must be in\"):\n pr.quaternion_from_angle(-1, 0)\n with pytest.raises(ValueError, match=\"Basis must be in\"):\n pr.quaternion_from_angle(3, 0)\n\n rng = np.random.default_rng(22)\n for _ in range(20):\n basis = rng.integers(0, 3)\n angle = 2.0 * np.pi * rng.random() - np.pi\n R = pr.active_matrix_from_angle(basis, angle)\n q = pr.quaternion_from_angle(basis, angle)\n Rq = pr.matrix_from_quaternion(q)\n assert_array_almost_equal(R, Rq)\n" }, { "path": "pytransform3d/rotations/test/test_axis_angle.py", "content": "import numpy as np\nimport pytest\nfrom numpy.testing import assert_array_almost_equal\n\nimport pytransform3d.rotations as pr\n\n\ndef test_check_axis_angle():\n \"\"\"Test input validation for axis-angle representation.\"\"\"\n a_list = [1, 0, 0, 0]\n a = pr.check_axis_angle(a_list)\n assert_array_almost_equal(a_list, a)\n assert type(a) is np.ndarray\n assert a.dtype == np.float64\n\n rng = np.random.default_rng(0)\n a = np.empty(4)\n a[:3] = pr.random_vector(rng, 3)\n a[3] = rng.standard_normal() * 4.0 * np.pi\n a2 = pr.check_axis_angle(a)\n pr.assert_axis_angle_equal(a, a2)\n assert pytest.approx(np.linalg.norm(a2[:3])) == 1.0\n assert a2[3] > 0\n assert np.pi > a2[3]\n\n with pytest.raises(\n ValueError, match=\"Expected axis and angle in array with shape\"\n ):\n pr.check_axis_angle(np.zeros(3))\n with pytest.raises(\n ValueError, match=\"Expected axis and angle in array with shape\"\n ):\n pr.check_axis_angle(np.zeros((3, 3)))\n\n\ndef test_check_compact_axis_angle():\n \"\"\"Test input validation for compact axis-angle representation.\"\"\"\n a_list = [0, 0, 0]\n a = pr.check_compact_axis_angle(a_list)\n assert_array_almost_equal(a_list, a)\n assert type(a) is np.ndarray\n assert a.dtype == np.float64\n\n rng = np.random.default_rng(0)\n a = pr.norm_vector(pr.random_vector(rng, 3))\n a *= np.pi + rng.standard_normal() * 4.0 * np.pi\n a2 = pr.check_compact_axis_angle(a)\n pr.assert_compact_axis_angle_equal(a, a2)\n assert np.pi > np.linalg.norm(a2) > 0\n\n with pytest.raises(\n ValueError, match=\"Expected axis and angle in array with shape\"\n ):\n pr.check_compact_axis_angle(np.zeros(4))\n with pytest.raises(\n ValueError, match=\"Expected axis and angle in array with shape\"\n ):\n pr.check_compact_axis_angle(np.zeros((3, 3)))\n\n\ndef test_norm_axis_angle():\n \"\"\"Test normalization of angle-axis representation.\"\"\"\n a = np.array([1.0, 0.0, 0.0, np.pi])\n n = pr.norm_axis_angle(a)\n assert_array_almost_equal(a, n)\n\n a = np.array([0.0, 1.0, 0.0, np.pi])\n n = pr.norm_axis_angle(a)\n assert_array_almost_equal(a, n)\n\n a = np.array([0.0, 0.0, 1.0, np.pi])\n n = pr.norm_axis_angle(a)\n assert_array_almost_equal(a, n)\n\n rng = np.random.default_rng(0)\n for _ in range(5):\n a = pr.random_axis_angle(rng)\n angle = rng.uniform(-20.0, -10.0 + 2.0 * np.pi)\n a[3] = angle\n n = pr.norm_axis_angle(a)\n for angle_offset in np.arange(0.0, 10.1 * np.pi, 2.0 * np.pi):\n a[3] = angle + angle_offset\n n2 = pr.norm_axis_angle(a)\n assert_array_almost_equal(n, n2)\n\n\ndef test_compact_axis_angle_near_pi():\n assert pr.compact_axis_angle_near_pi(\n np.pi * pr.norm_vector([0.2, 0.1, -0.3])\n )\n assert pr.compact_axis_angle_near_pi(\n (1e-7 + np.pi) * pr.norm_vector([0.2, 0.1, -0.3])\n )\n assert pr.compact_axis_angle_near_pi(\n (-1e-7 + np.pi) * pr.norm_vector([0.2, 0.1, -0.3])\n )\n assert not pr.compact_axis_angle_near_pi(\n (-1e-5 + np.pi) * pr.norm_vector([0.2, 0.1, -0.3])\n )\n\n\ndef test_norm_compact_axis_angle():\n \"\"\"Test normalization of compact angle-axis representation.\"\"\"\n a = np.array([np.pi, 0.0, 0.0])\n n = pr.norm_compact_axis_angle(a)\n assert_array_almost_equal(a, n)\n\n a = np.array([0.0, np.pi, 0.0])\n n = pr.norm_compact_axis_angle(a)\n assert_array_almost_equal(a, n)\n\n a = np.array([0.0, 0.0, np.pi])\n n = pr.norm_compact_axis_angle(a)\n assert_array_almost_equal(a, n)\n\n rng = np.random.default_rng(0)\n for _ in range(5):\n a = pr.random_compact_axis_angle(rng)\n axis = a / np.linalg.norm(a)\n angle = rng.uniform(-20.0, -10.0 + 2.0 * np.pi)\n a = axis * angle\n n = pr.norm_compact_axis_angle(a)\n for angle_offset in np.arange(0.0, 10.1 * np.pi, 2.0 * np.pi):\n a = axis * (angle + angle_offset)\n n2 = pr.norm_compact_axis_angle(a)\n assert_array_almost_equal(n, n2)\n\n\ndef test_axis_angle_from_two_direction_vectors():\n \"\"\"Test calculation of axis-angle from two direction vectors.\"\"\"\n d1 = np.array([1.0, 0.0, 0.0])\n a = pr.axis_angle_from_two_directions(d1, d1)\n pr.assert_axis_angle_equal(a, np.array([1, 0, 0, 0]))\n\n a = pr.axis_angle_from_two_directions(d1, np.zeros(3))\n pr.assert_axis_angle_equal(a, np.array([1, 0, 0, 0]))\n\n a = pr.axis_angle_from_two_directions(np.zeros(3), d1)\n pr.assert_axis_angle_equal(a, np.array([1, 0, 0, 0]))\n\n d2 = np.array([0.0, 1.0, 0.0])\n a = pr.axis_angle_from_two_directions(d1, d2)\n pr.assert_axis_angle_equal(a, np.array([0, 0, 1, 0.5 * np.pi]))\n\n d3 = np.array([-1.0, 0.0, 0.0])\n a = pr.axis_angle_from_two_directions(d1, d3)\n pr.assert_axis_angle_equal(a, np.array([0, 0, 1, np.pi]))\n\n d4 = np.array([0.0, -1.0, 0.0])\n a = pr.axis_angle_from_two_directions(d2, d4)\n pr.assert_axis_angle_equal(a, np.array([0, 0, 1, np.pi]))\n\n a = pr.axis_angle_from_two_directions(d3, d4)\n pr.assert_axis_angle_equal(a, np.array([0, 0, 1, 0.5 * np.pi]))\n\n rng = np.random.default_rng(323)\n for _ in range(5):\n R = pr.matrix_from_axis_angle(pr.random_axis_angle(rng))\n v1 = pr.random_vector(rng, 3)\n v2 = R.dot(v1)\n a = pr.axis_angle_from_two_directions(v1, v2)\n assert pytest.approx(pr.angle_between_vectors(v1, a[:3])) == 0.5 * np.pi\n assert pytest.approx(pr.angle_between_vectors(v2, a[:3])) == 0.5 * np.pi\n assert_array_almost_equal(v2, pr.matrix_from_axis_angle(a).dot(v1))\n\n\ndef test_axis_angle_from_compact_axis_angle():\n \"\"\"Test conversion from compact axis-angle representation.\"\"\"\n ca = [0.0, 0.0, 0.0]\n a = pr.axis_angle_from_compact_axis_angle(ca)\n assert_array_almost_equal(a, np.array([1.0, 0.0, 0.0, 0.0]))\n\n rng = np.random.default_rng(1)\n for _ in range(5):\n ca = pr.random_compact_axis_angle(rng)\n a = pr.axis_angle_from_compact_axis_angle(ca)\n assert pytest.approx(np.linalg.norm(ca)) == a[3]\n assert_array_almost_equal(ca[:3] / np.linalg.norm(ca), a[:3])\n\n\ndef test_compact_axis_angle():\n \"\"\"Test conversion to compact axis-angle representation.\"\"\"\n a = np.array([1.0, 0.0, 0.0, 0.0])\n ca = pr.compact_axis_angle(a)\n assert_array_almost_equal(ca, np.zeros(3))\n\n rng = np.random.default_rng(0)\n for _ in range(5):\n a = pr.random_axis_angle(rng)\n ca = pr.compact_axis_angle(a)\n assert_array_almost_equal(pr.norm_vector(ca), a[:3])\n assert pytest.approx(np.linalg.norm(ca)) == a[3]\n\n\ndef test_conversions_axis_angle_quaternion():\n \"\"\"Test conversions between axis-angle and quaternion.\"\"\"\n q = np.array([1, 0, 0, 0])\n a = pr.axis_angle_from_quaternion(q)\n assert_array_almost_equal(a, np.array([1, 0, 0, 0]))\n q2 = pr.quaternion_from_axis_angle(a)\n assert_array_almost_equal(q2, q)\n\n rng = np.random.default_rng(0)\n for _ in range(5):\n a = pr.random_axis_angle(rng)\n q = pr.quaternion_from_axis_angle(a)\n\n a2 = pr.axis_angle_from_quaternion(q)\n assert_array_almost_equal(a, a2)\n\n q2 = pr.quaternion_from_axis_angle(a2)\n pr.assert_quaternion_equal(q, q2)\n\n\ndef test_conversions_compact_axis_angle_quaternion():\n \"\"\"Test conversions between compact axis-angle and quaternion.\"\"\"\n q = np.array([1, 0, 0, 0])\n a = pr.compact_axis_angle_from_quaternion(q)\n assert_array_almost_equal(a, np.array([0, 0, 0]))\n q2 = pr.quaternion_from_compact_axis_angle(a)\n assert_array_almost_equal(q2, q)\n\n rng = np.random.default_rng(0)\n for _ in range(5):\n a = pr.random_compact_axis_angle(rng)\n q = pr.quaternion_from_compact_axis_angle(a)\n\n a2 = pr.compact_axis_angle_from_quaternion(q)\n assert_array_almost_equal(a, a2)\n\n q2 = pr.quaternion_from_compact_axis_angle(a2)\n pr.assert_quaternion_equal(q, q2)\n\n\ndef test_mrp_from_axis_angle():\n rng = np.random.default_rng(98343)\n for _ in range(5):\n a = pr.random_axis_angle(rng)\n mrp = pr.mrp_from_axis_angle(a)\n q = pr.quaternion_from_axis_angle(a)\n assert_array_almost_equal(mrp, pr.mrp_from_quaternion(q))\n\n assert_array_almost_equal(\n [0.0, 0.0, 0.0], pr.mrp_from_axis_angle([1.0, 0.0, 0.0, 0.0])\n )\n assert_array_almost_equal(\n [0.0, 0.0, 0.0], pr.mrp_from_axis_angle([1.0, 0.0, 0.0, 2.0 * np.pi])\n )\n assert_array_almost_equal(\n [1.0, 0.0, 0.0], pr.mrp_from_axis_angle([1.0, 0.0, 0.0, np.pi])\n )\n" }, { "path": "pytransform3d/rotations/test/test_constants.py", "content": "from numpy.testing import assert_array_almost_equal\n\nimport pytransform3d.rotations as pr\n\n\ndef test_id_rot():\n \"\"\"Test equivalence of constants that represent no rotation.\"\"\"\n assert_array_almost_equal(pr.R_id, pr.matrix_from_axis_angle(pr.a_id))\n assert_array_almost_equal(pr.R_id, pr.matrix_from_quaternion(pr.q_id))\n" }, { "path": "pytransform3d/rotations/test/test_euler.py", "content": "import numpy as np\nimport pytest\nfrom numpy.testing import assert_array_almost_equal\n\nimport pytransform3d.rotations as pr\n\n\ndef test_norm_euler():\n rng = np.random.default_rng(94322)\n\n euler_axes = [\n [0, 2, 0],\n [0, 1, 0],\n [1, 0, 1],\n [1, 2, 1],\n [2, 1, 2],\n [2, 0, 2],\n [0, 2, 1],\n [0, 1, 2],\n [1, 0, 2],\n [1, 2, 0],\n [2, 1, 0],\n [2, 0, 1],\n ]\n for ea in euler_axes:\n for _ in range(10):\n e = np.pi + rng.random(3) * np.pi * 2.0\n e *= np.sign(rng.standard_normal(3))\n\n e_norm = pr.norm_euler(e, *ea)\n R1 = pr.matrix_from_euler(e, ea[0], ea[1], ea[2], True)\n R2 = pr.matrix_from_euler(e_norm, ea[0], ea[1], ea[2], True)\n assert_array_almost_equal(R1, R2)\n assert not np.allclose(e, e_norm)\n assert -np.pi <= e_norm[0] <= np.pi\n if ea[0] == ea[2]:\n assert 0.0 <= e_norm[1] <= np.pi\n else:\n assert -0.5 * np.pi <= e_norm[1] <= 0.5 * np.pi\n assert -np.pi <= e_norm[2] <= np.pi\n pr.assert_euler_equal(e, e_norm, *ea)\n\n\ndef test_euler_near_gimbal_lock():\n assert pr.euler_near_gimbal_lock([0, 0, 0], 1, 2, 1)\n assert pr.euler_near_gimbal_lock([0, -1e-7, 0], 1, 2, 1)\n assert pr.euler_near_gimbal_lock([0, 1e-7, 0], 1, 2, 1)\n assert pr.euler_near_gimbal_lock([0, np.pi, 0], 1, 2, 1)\n assert pr.euler_near_gimbal_lock([0, np.pi - 1e-7, 0], 1, 2, 1)\n assert pr.euler_near_gimbal_lock([0, np.pi + 1e-7, 0], 1, 2, 1)\n assert not pr.euler_near_gimbal_lock([0, 0.5, 0], 1, 2, 1)\n assert pr.euler_near_gimbal_lock([0, 0.5 * np.pi, 0], 0, 1, 2)\n assert pr.euler_near_gimbal_lock([0, 0.5 * np.pi - 1e-7, 0], 0, 1, 2)\n assert pr.euler_near_gimbal_lock([0, 0.5 * np.pi + 1e-7, 0], 0, 1, 2)\n assert pr.euler_near_gimbal_lock([0, -0.5 * np.pi, 0], 0, 1, 2)\n assert pr.euler_near_gimbal_lock([0, -0.5 * np.pi - 1e-7, 0], 0, 1, 2)\n assert pr.euler_near_gimbal_lock([0, -0.5 * np.pi + 1e-7, 0], 0, 1, 2)\n assert not pr.euler_near_gimbal_lock([0, 0, 0], 0, 1, 2)\n\n\ndef test_assert_euler_almost_equal():\n pr.assert_euler_equal(\n [0.2, 0.3, -0.5], [0.2 + np.pi, -0.3, -0.5 - np.pi], 0, 1, 0\n )\n pr.assert_euler_equal(\n [0.2, 0.3, -0.5], [0.2 + np.pi, np.pi - 0.3, -0.5 - np.pi], 0, 1, 2\n )\n\n\ndef test_general_matrix_euler_conversions():\n \"\"\"General conversion algorithms between matrix and Euler angles.\"\"\"\n rng = np.random.default_rng(22)\n\n euler_axes = [\n [0, 2, 0],\n [0, 1, 0],\n [1, 0, 1],\n [1, 2, 1],\n [2, 1, 2],\n [2, 0, 2],\n [0, 2, 1],\n [0, 1, 2],\n [1, 0, 2],\n [1, 2, 0],\n [2, 1, 0],\n [2, 0, 1],\n ]\n for ea in euler_axes:\n for extrinsic in [False, True]:\n for _ in range(5):\n e = rng.random(3)\n e[0] = 2.0 * np.pi * e[0] - np.pi\n e[1] = np.pi * e[1]\n e[2] = 2.0 * np.pi * e[2] - np.pi\n\n proper_euler = ea[0] == ea[2]\n if proper_euler:\n e[1] -= np.pi / 2.0\n\n q = pr.quaternion_from_euler(e, ea[0], ea[1], ea[2], extrinsic)\n R = pr.matrix_from_euler(e, ea[0], ea[1], ea[2], extrinsic)\n q_R = pr.quaternion_from_matrix(R)\n pr.assert_quaternion_equal(\n q, q_R, err_msg=f\"axes: {ea}, extrinsic: {extrinsic}\"\n )\n\n e_R = pr.euler_from_matrix(R, ea[0], ea[1], ea[2], extrinsic)\n e_q = pr.euler_from_quaternion(\n q, ea[0], ea[1], ea[2], extrinsic\n )\n pr.assert_euler_equal(e_R, e_q, *ea)\n\n R_R = pr.matrix_from_euler(e_R, ea[0], ea[1], ea[2], extrinsic)\n R_q = pr.matrix_from_euler(e_q, ea[0], ea[1], ea[2], extrinsic)\n assert_array_almost_equal(R_R, R_q)\n\n\ndef test_from_quaternion():\n \"\"\"Test conversion from quaternion to Euler angles.\"\"\"\n with pytest.raises(\n ValueError, match=\"Axis index i \\\\(-1\\\\) must be in \\\\[0, 1, 2\\\\]\"\n ):\n pr.euler_from_quaternion(pr.q_id, -1, 0, 2, True)\n with pytest.raises(\n ValueError, match=\"Axis index i \\\\(3\\\\) must be in \\\\[0, 1, 2\\\\]\"\n ):\n pr.euler_from_quaternion(pr.q_id, 3, 0, 2, True)\n with pytest.raises(\n ValueError, match=\"Axis index j \\\\(-1\\\\) must be in \\\\[0, 1, 2\\\\]\"\n ):\n pr.euler_from_quaternion(pr.q_id, 2, -1, 2, True)\n with pytest.raises(\n ValueError, match=\"Axis index j \\\\(3\\\\) must be in \\\\[0, 1, 2\\\\]\"\n ):\n pr.euler_from_quaternion(pr.q_id, 2, 3, 2, True)\n with pytest.raises(\n ValueError, match=\"Axis index k \\\\(-1\\\\) must be in \\\\[0, 1, 2\\\\]\"\n ):\n pr.euler_from_quaternion(pr.q_id, 2, 0, -1, True)\n with pytest.raises(\n ValueError, match=\"Axis index k \\\\(3\\\\) must be in \\\\[0, 1, 2\\\\]\"\n ):\n pr.euler_from_quaternion(pr.q_id, 2, 0, 3, True)\n\n rng = np.random.default_rng(32)\n\n euler_axes = [\n [0, 2, 0],\n [0, 1, 0],\n [1, 0, 1],\n [1, 2, 1],\n [2, 1, 2],\n [2, 0, 2],\n [0, 2, 1],\n [0, 1, 2],\n [1, 0, 2],\n [1, 2, 0],\n [2, 1, 0],\n [2, 0, 1],\n ]\n functions = [\n pr.intrinsic_euler_xzx_from_active_matrix,\n pr.extrinsic_euler_xzx_from_active_matrix,\n pr.intrinsic_euler_xyx_from_active_matrix,\n pr.extrinsic_euler_xyx_from_active_matrix,\n pr.intrinsic_euler_yxy_from_active_matrix,\n pr.extrinsic_euler_yxy_from_active_matrix,\n pr.intrinsic_euler_yzy_from_active_matrix,\n pr.extrinsic_euler_yzy_from_active_matrix,\n pr.intrinsic_euler_zyz_from_active_matrix,\n pr.extrinsic_euler_zyz_from_active_matrix,\n pr.intrinsic_euler_zxz_from_active_matrix,\n pr.extrinsic_euler_zxz_from_active_matrix,\n pr.intrinsic_euler_xzy_from_active_matrix,\n pr.extrinsic_euler_xzy_from_active_matrix,\n pr.intrinsic_euler_xyz_from_active_matrix,\n pr.extrinsic_euler_xyz_from_active_matrix,\n pr.intrinsic_euler_yxz_from_active_matrix,\n pr.extrinsic_euler_yxz_from_active_matrix,\n pr.intrinsic_euler_yzx_from_active_matrix,\n pr.extrinsic_euler_yzx_from_active_matrix,\n pr.intrinsic_euler_zyx_from_active_matrix,\n pr.extrinsic_euler_zyx_from_active_matrix,\n pr.intrinsic_euler_zxy_from_active_matrix,\n pr.extrinsic_euler_zxy_from_active_matrix,\n ]\n inverse_functions = [\n pr.active_matrix_from_intrinsic_euler_xzx,\n pr.active_matrix_from_extrinsic_euler_xzx,\n pr.active_matrix_from_intrinsic_euler_xyx,\n pr.active_matrix_from_extrinsic_euler_xyx,\n pr.active_matrix_from_intrinsic_euler_yxy,\n pr.active_matrix_from_extrinsic_euler_yxy,\n pr.active_matrix_from_intrinsic_euler_yzy,\n pr.active_matrix_from_extrinsic_euler_yzy,\n pr.active_matrix_from_intrinsic_euler_zyz,\n pr.active_matrix_from_extrinsic_euler_zyz,\n pr.active_matrix_from_intrinsic_euler_zxz,\n pr.active_matrix_from_extrinsic_euler_zxz,\n pr.active_matrix_from_intrinsic_euler_xzy,\n pr.active_matrix_from_extrinsic_euler_xzy,\n pr.active_matrix_from_intrinsic_euler_xyz,\n pr.active_matrix_from_extrinsic_euler_xyz,\n pr.active_matrix_from_intrinsic_euler_yxz,\n pr.active_matrix_from_extrinsic_euler_yxz,\n pr.active_matrix_from_intrinsic_euler_yzx,\n pr.active_matrix_from_extrinsic_euler_yzx,\n pr.active_matrix_from_intrinsic_euler_zyx,\n pr.active_matrix_from_extrinsic_euler_zyx,\n pr.active_matrix_from_intrinsic_euler_zxy,\n pr.active_matrix_from_extrinsic_euler_zxy,\n ]\n\n fi = 0\n for ea in euler_axes:\n for extrinsic in [False, True]:\n fun = functions[fi]\n inv_fun = inverse_functions[fi]\n fi += 1\n for _ in range(5):\n e = rng.random(3)\n e[0] = 2.0 * np.pi * e[0] - np.pi\n e[1] = np.pi * e[1]\n e[2] = 2.0 * np.pi * e[2] - np.pi\n\n proper_euler = ea[0] == ea[2]\n if proper_euler:\n e[1] -= np.pi / 2.0\n\n # normal case\n q = pr.quaternion_from_matrix(inv_fun(e))\n\n e1 = pr.euler_from_quaternion(q, ea[0], ea[1], ea[2], extrinsic)\n e2 = fun(pr.matrix_from_quaternion(q))\n assert_array_almost_equal(\n e1, e2, err_msg=f\"axes: {ea}, extrinsic: {extrinsic}\"\n )\n\n # first singularity\n e[1] = 0.0\n q = pr.quaternion_from_matrix(inv_fun(e))\n\n R1 = inv_fun(\n pr.euler_from_quaternion(q, ea[0], ea[1], ea[2], extrinsic)\n )\n R2 = pr.matrix_from_quaternion(q)\n assert_array_almost_equal(\n R1, R2, err_msg=f\"axes: {ea}, extrinsic: {extrinsic}\"\n )\n\n # second singularity\n e[1] = np.pi\n q = pr.quaternion_from_matrix(inv_fun(e))\n\n R1 = inv_fun(\n pr.euler_from_quaternion(q, ea[0], ea[1], ea[2], extrinsic)\n )\n R2 = pr.matrix_from_quaternion(q)\n assert_array_almost_equal(\n R1, R2, err_msg=f\"axes: {ea}, extrinsic: {extrinsic}\"\n )\n" }, { "path": "pytransform3d/rotations/test/test_euler_deprecated.py", "content": "import numpy as np\nimport pytest\nfrom numpy.testing import assert_array_almost_equal\n\nimport pytransform3d.rotations as pr\n\n\ndef test_active_matrix_from_intrinsic_euler_zxz():\n \"\"\"Test conversion from intrinsic zxz Euler angles.\"\"\"\n assert_array_almost_equal(\n pr.active_matrix_from_intrinsic_euler_zxz([0.5 * np.pi, 0, 0]),\n np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]),\n )\n assert_array_almost_equal(\n pr.active_matrix_from_intrinsic_euler_zxz(\n [0.5 * np.pi, 0, 0.5 * np.pi]\n ),\n np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]),\n )\n assert_array_almost_equal(\n pr.active_matrix_from_intrinsic_euler_zxz(\n [0.5 * np.pi, 0.5 * np.pi, 0]\n ),\n np.array([[0, 0, 1], [1, 0, 0], [0, 1, 0]]),\n )\n assert_array_almost_equal(\n pr.active_matrix_from_intrinsic_euler_zxz(\n [0.5 * np.pi, 0.5 * np.pi, 0.5 * np.pi]\n ),\n np.array([[0, 0, 1], [0, -1, 0], [1, 0, 0]]),\n )\n\n\ndef test_active_matrix_from_extrinsic_euler_zxz():\n \"\"\"Test conversion from extrinsic zxz Euler angles.\"\"\"\n assert_array_almost_equal(\n pr.active_matrix_from_extrinsic_euler_zxz([0.5 * np.pi, 0, 0]),\n np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]),\n )\n assert_array_almost_equal(\n pr.active_matrix_from_extrinsic_euler_zxz(\n [0.5 * np.pi, 0, 0.5 * np.pi]\n ),\n np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]),\n )\n assert_array_almost_equal(\n pr.active_matrix_from_extrinsic_euler_zxz(\n [0.5 * np.pi, 0.5 * np.pi, 0]\n ),\n np.array([[0, -1, 0], [0, 0, -1], [1, 0, 0]]),\n )\n assert_array_almost_equal(\n pr.active_matrix_from_extrinsic_euler_zxz(\n [0.5 * np.pi, 0.5 * np.pi, 0.5 * np.pi]\n ),\n np.array([[0, 0, 1], [0, -1, 0], [1, 0, 0]]),\n )\n\n\ndef test_active_matrix_from_intrinsic_euler_zyz():\n \"\"\"Test conversion from intrinsic zyz Euler angles.\"\"\"\n assert_array_almost_equal(\n pr.active_matrix_from_intrinsic_euler_zyz([0.5 * np.pi, 0, 0]),\n np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]),\n )\n assert_array_almost_equal(\n pr.active_matrix_from_intrinsic_euler_zyz([0.5 * np.pi, 0, 0]),\n pr.matrix_from_euler([0.5 * np.pi, 0, 0], 2, 1, 2, False),\n )\n assert_array_almost_equal(\n pr.active_matrix_from_intrinsic_euler_zyz(\n [0.5 * np.pi, 0, 0.5 * np.pi]\n ),\n np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]),\n )\n assert_array_almost_equal(\n pr.active_matrix_from_intrinsic_euler_zyz(\n [0.5 * np.pi, 0.5 * np.pi, 0]\n ),\n np.array([[0, -1, 0], [0, 0, 1], [-1, 0, 0]]),\n )\n assert_array_almost_equal(\n pr.active_matrix_from_intrinsic_euler_zyz(\n [0.5 * np.pi, 0.5 * np.pi, 0.5 * np.pi]\n ),\n np.array([[-1, 0, 0], [0, 0, 1], [0, 1, 0]]),\n )\n\n\ndef test_active_matrix_from_extrinsic_euler_zyz():\n \"\"\"Test conversion from roll, pitch, yaw.\"\"\"\n assert_array_almost_equal(\n pr.active_matrix_from_extrinsic_roll_pitch_yaw([0.5 * np.pi, 0, 0]),\n np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]]),\n )\n assert_array_almost_equal(\n pr.active_matrix_from_extrinsic_roll_pitch_yaw([0.5 * np.pi, 0, 0]),\n pr.matrix_from_euler([0.5 * np.pi, 0, 0], 0, 1, 2, True),\n )\n assert_array_almost_equal(\n pr.active_matrix_from_extrinsic_roll_pitch_yaw(\n [0.5 * np.pi, 0, 0.5 * np.pi]\n ),\n np.array([[0, 0, 1], [1, 0, 0], [0, 1, 0]]),\n )\n assert_array_almost_equal(\n pr.active_matrix_from_extrinsic_roll_pitch_yaw(\n [0.5 * np.pi, 0.5 * np.pi, 0]\n ),\n np.array([[0, 1, 0], [0, 0, -1], [-1, 0, 0]]),\n )\n assert_array_almost_equal(\n pr.active_matrix_from_extrinsic_roll_pitch_yaw(\n [0.5 * np.pi, 0.5 * np.pi, 0.5 * np.pi]\n ),\n np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]]),\n )\n\n\ndef test_active_matrix_from_intrinsic_zyx():\n \"\"\"Test conversion from intrinsic zyx Euler angles.\"\"\"\n rng = np.random.default_rng(844)\n for _ in range(5):\n euler_zyx = (rng.random(3) - 0.5) * np.array(\n [np.pi, 0.5 * np.pi, np.pi]\n )\n s = np.sin(euler_zyx)\n c = np.cos(euler_zyx)\n R_from_formula = np.array(\n [\n [\n c[0] * c[1],\n c[0] * s[1] * s[2] - s[0] * c[2],\n c[0] * s[1] * c[2] + s[0] * s[2],\n ],\n [\n s[0] * c[1],\n s[0] * s[1] * s[2] + c[0] * c[2],\n s[0] * s[1] * c[2] - c[0] * s[2],\n ],\n [-s[1], c[1] * s[2], c[1] * c[2]],\n ]\n ) # See Lynch, Park: Modern Robotics, page 576\n\n # Normal case, we can reconstruct original angles\n R = pr.active_matrix_from_intrinsic_euler_zyx(euler_zyx)\n assert_array_almost_equal(R_from_formula, R)\n euler_zyx2 = pr.intrinsic_euler_zyx_from_active_matrix(R)\n assert_array_almost_equal(euler_zyx, euler_zyx2)\n\n # Gimbal lock 1, infinite solutions with constraint\n # alpha - gamma = constant\n euler_zyx[1] = 0.5 * np.pi\n R = pr.active_matrix_from_intrinsic_euler_zyx(euler_zyx)\n euler_zyx2 = pr.intrinsic_euler_zyx_from_active_matrix(R)\n assert pytest.approx(euler_zyx2[1]) == 0.5 * np.pi\n assert (\n pytest.approx(euler_zyx[0] - euler_zyx[2])\n == euler_zyx2[0] - euler_zyx2[2]\n )\n\n # Gimbal lock 2, infinite solutions with constraint\n # alpha + gamma = constant\n euler_zyx[1] = -0.5 * np.pi\n R = pr.active_matrix_from_intrinsic_euler_zyx(euler_zyx)\n euler_zyx2 = pr.intrinsic_euler_zyx_from_active_matrix(R)\n assert pytest.approx(euler_zyx2[1]) == -0.5 * np.pi\n assert (\n pytest.approx(euler_zyx[0] + euler_zyx[2])\n == euler_zyx2[0] + euler_zyx2[2]\n )\n\n\ndef test_active_matrix_from_extrinsic_zyx():\n \"\"\"Test conversion from extrinsic zyx Euler angles.\"\"\"\n rng = np.random.default_rng(844)\n for _ in range(5):\n euler_zyx = (rng.random(3) - 0.5) * np.array(\n [np.pi, 0.5 * np.pi, np.pi]\n )\n\n # Normal case, we can reconstruct original angles\n R = pr.active_matrix_from_extrinsic_euler_zyx(euler_zyx)\n euler_zyx2 = pr.extrinsic_euler_zyx_from_active_matrix(R)\n assert_array_almost_equal(euler_zyx, euler_zyx2)\n R2 = pr.active_matrix_from_extrinsic_euler_zyx(euler_zyx2)\n assert_array_almost_equal(R, R2)\n\n # Gimbal lock 1, infinite solutions with constraint\n # alpha + gamma = constant\n euler_zyx[1] = 0.5 * np.pi\n R = pr.active_matrix_from_extrinsic_euler_zyx(euler_zyx)\n euler_zyx2 = pr.extrinsic_euler_zyx_from_active_matrix(R)\n assert pytest.approx(euler_zyx2[1]) == 0.5 * np.pi\n assert (\n pytest.approx(euler_zyx[0] + euler_zyx[2])\n == euler_zyx2[0] + euler_zyx2[2]\n )\n R2 = pr.active_matrix_from_extrinsic_euler_zyx(euler_zyx2)\n assert_array_almost_equal(R, R2)\n\n # Gimbal lock 2, infinite solutions with constraint\n # alpha - gamma = constant\n euler_zyx[1] = -0.5 * np.pi\n R = pr.active_matrix_from_extrinsic_euler_zyx(euler_zyx)\n euler_zyx2 = pr.extrinsic_euler_zyx_from_active_matrix(R)\n assert pytest.approx(euler_zyx2[1]) == -0.5 * np.pi\n assert (\n pytest.approx(euler_zyx[0] - euler_zyx[2])\n == euler_zyx2[0] - euler_zyx2[2]\n )\n R2 = pr.active_matrix_from_extrinsic_euler_zyx(euler_zyx2)\n assert_array_almost_equal(R, R2)\n\n\ndef _test_conversion_matrix_euler(\n matrix_from_euler, euler_from_matrix, proper_euler\n):\n \"\"\"Test conversions between Euler angles and rotation matrix.\"\"\"\n rng = np.random.default_rng(844)\n for _ in range(5):\n euler = (rng.random(3) - 0.5) * np.array([np.pi, 0.5 * np.pi, np.pi])\n if proper_euler:\n euler[1] += 0.5 * np.pi\n\n # Normal case, we can reconstruct original angles\n R = matrix_from_euler(euler)\n euler2 = euler_from_matrix(R)\n assert_array_almost_equal(euler, euler2)\n R2 = matrix_from_euler(euler2)\n assert_array_almost_equal(R, R2)\n\n # Gimbal lock 1\n if proper_euler:\n euler[1] = np.pi\n else:\n euler[1] = 0.5 * np.pi\n R = matrix_from_euler(euler)\n euler2 = euler_from_matrix(R)\n assert pytest.approx(euler[1]) == euler2[1]\n R2 = matrix_from_euler(euler2)\n assert_array_almost_equal(R, R2)\n\n # Gimbal lock 2\n if proper_euler:\n euler[1] = 0.0\n else:\n euler[1] = -0.5 * np.pi\n R = matrix_from_euler(euler)\n euler2 = euler_from_matrix(R)\n assert pytest.approx(euler[1]) == euler2[1]\n R2 = matrix_from_euler(euler2)\n assert_array_almost_equal(R, R2)\n\n\ndef test_all_euler_matrix_conversions():\n \"\"\"Test all conversion between Euler angles and matrices.\"\"\"\n _test_conversion_matrix_euler(\n pr.active_matrix_from_intrinsic_euler_xzx,\n pr.intrinsic_euler_xzx_from_active_matrix,\n proper_euler=True,\n )\n _test_conversion_matrix_euler(\n pr.active_matrix_from_extrinsic_euler_xzx,\n pr.extrinsic_euler_xzx_from_active_matrix,\n proper_euler=True,\n )\n _test_conversion_matrix_euler(\n pr.active_matrix_from_intrinsic_euler_xyx,\n pr.intrinsic_euler_xyx_from_active_matrix,\n proper_euler=True,\n )\n _test_conversion_matrix_euler(\n pr.active_matrix_from_extrinsic_euler_xyx,\n pr.extrinsic_euler_xyx_from_active_matrix,\n proper_euler=True,\n )\n _test_conversion_matrix_euler(\n pr.active_matrix_from_intrinsic_euler_yxy,\n pr.intrinsic_euler_yxy_from_active_matrix,\n proper_euler=True,\n )\n _test_conversion_matrix_euler(\n pr.active_matrix_from_extrinsic_euler_yxy,\n pr.extrinsic_euler_yxy_from_active_matrix,\n proper_euler=True,\n )\n _test_conversion_matrix_euler(\n pr.active_matrix_from_intrinsic_euler_yzy,\n pr.intrinsic_euler_yzy_from_active_matrix,\n proper_euler=True,\n )\n _test_conversion_matrix_euler(\n pr.active_matrix_from_extrinsic_euler_yzy,\n pr.extrinsic_euler_yzy_from_active_matrix,\n proper_euler=True,\n )\n _test_conversion_matrix_euler(\n pr.active_matrix_from_intrinsic_euler_zyz,\n pr.intrinsic_euler_zyz_from_active_matrix,\n proper_euler=True,\n )\n _test_conversion_matrix_euler(\n pr.active_matrix_from_extrinsic_euler_zyz,\n pr.extrinsic_euler_zyz_from_active_matrix,\n proper_euler=True,\n )\n _test_conversion_matrix_euler(\n pr.active_matrix_from_intrinsic_euler_zxz,\n pr.intrinsic_euler_zxz_from_active_matrix,\n proper_euler=True,\n )\n _test_conversion_matrix_euler(\n pr.active_matrix_from_extrinsic_euler_zxz,\n pr.extrinsic_euler_zxz_from_active_matrix,\n proper_euler=True,\n )\n _test_conversion_matrix_euler(\n pr.active_matrix_from_intrinsic_euler_xzy,\n pr.intrinsic_euler_xzy_from_active_matrix,\n proper_euler=False,\n )\n _test_conversion_matrix_euler(\n pr.active_matrix_from_extrinsic_euler_xzy,\n pr.extrinsic_euler_xzy_from_active_matrix,\n proper_euler=False,\n )\n _test_conversion_matrix_euler(\n pr.active_matrix_from_intrinsic_euler_xyz,\n pr.intrinsic_euler_xyz_from_active_matrix,\n proper_euler=False,\n )\n _test_conversion_matrix_euler(\n pr.active_matrix_from_extrinsic_euler_xyz,\n pr.extrinsic_euler_xyz_from_active_matrix,\n proper_euler=False,\n )\n _test_conversion_matrix_euler(\n pr.active_matrix_from_intrinsic_euler_yxz,\n pr.intrinsic_euler_yxz_from_active_matrix,\n proper_euler=False,\n )\n _test_conversion_matrix_euler(\n pr.active_matrix_from_extrinsic_euler_yxz,\n pr.extrinsic_euler_yxz_from_active_matrix,\n proper_euler=False,\n )\n _test_conversion_matrix_euler(\n pr.active_matrix_from_intrinsic_euler_yzx,\n pr.intrinsic_euler_yzx_from_active_matrix,\n proper_euler=False,\n )\n _test_conversion_matrix_euler(\n pr.active_matrix_from_extrinsic_euler_yzx,\n pr.extrinsic_euler_yzx_from_active_matrix,\n proper_euler=False,\n )\n _test_conversion_matrix_euler(\n pr.active_matrix_from_intrinsic_euler_zyx,\n pr.intrinsic_euler_zyx_from_active_matrix,\n proper_euler=False,\n )\n _test_conversion_matrix_euler(\n pr.active_matrix_from_extrinsic_euler_zyx,\n pr.extrinsic_euler_zyx_from_active_matrix,\n proper_euler=False,\n )\n _test_conversion_matrix_euler(\n pr.active_matrix_from_intrinsic_euler_zxy,\n pr.intrinsic_euler_zxy_from_active_matrix,\n proper_euler=False,\n )\n _test_conversion_matrix_euler(\n pr.active_matrix_from_extrinsic_euler_zxy,\n pr.extrinsic_euler_zxy_from_active_matrix,\n proper_euler=False,\n )\n\n\ndef test_active_matrix_from_extrinsic_roll_pitch_yaw():\n \"\"\"Test conversion from extrinsic zyz Euler angles.\"\"\"\n assert_array_almost_equal(\n pr.active_matrix_from_extrinsic_euler_zyz([0.5 * np.pi, 0, 0]),\n np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]),\n )\n assert_array_almost_equal(\n pr.active_matrix_from_extrinsic_euler_zyz(\n [0.5 * np.pi, 0, 0.5 * np.pi]\n ),\n np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]),\n )\n assert_array_almost_equal(\n pr.active_matrix_from_extrinsic_euler_zyz(\n [0.5 * np.pi, 0.5 * np.pi, 0]\n ),\n np.array([[0, 0, 1], [1, 0, 0], [0, 1, 0]]),\n )\n assert_array_almost_equal(\n pr.active_matrix_from_extrinsic_euler_zyz(\n [0.5 * np.pi, 0.5 * np.pi, 0.5 * np.pi]\n ),\n np.array([[-1, 0, 0], [0, 0, 1], [0, 1, 0]]),\n )\n\n\ndef test_quaternion_from_extrinsic_euler_xyz():\n \"\"\"Test quaternion_from_extrinsic_euler_xyz.\"\"\"\n rng = np.random.default_rng(0)\n for _ in range(10):\n e = rng.uniform(-100, 100, [3])\n q = pr.quaternion_from_extrinsic_euler_xyz(e)\n R_from_q = pr.matrix_from_quaternion(q)\n R_from_e = pr.active_matrix_from_extrinsic_euler_xyz(e)\n assert_array_almost_equal(R_from_q, R_from_e)\n\n\ndef test_euler_from_quaternion_edge_case():\n quaternion = [0.57114154, -0.41689009, -0.57114154, -0.41689009]\n matrix = pr.matrix_from_quaternion(quaternion)\n euler_xyz = pr.extrinsic_euler_xyz_from_active_matrix(matrix)\n assert not np.isnan(euler_xyz).all()\n" }, { "path": "pytransform3d/rotations/test/test_matrix.py", "content": "import warnings\n\nimport numpy as np\nimport pytest\nfrom numpy.testing import assert_array_equal, assert_array_almost_equal\n\nimport pytransform3d.rotations as pr\n\n\ndef test_check_matrix():\n \"\"\"Test input validation for rotation matrix.\"\"\"\n R_list = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]\n R = pr.check_matrix(R_list)\n assert type(R) is np.ndarray\n assert R.dtype == np.float64\n\n R_int_array = np.eye(3, dtype=int)\n R = pr.check_matrix(R_int_array)\n assert type(R) is np.ndarray\n assert R.dtype == np.float64\n\n R_array = np.eye(3)\n R = pr.check_matrix(R_array)\n assert_array_equal(R_array, R)\n\n R = np.eye(4)\n with pytest.raises(ValueError, match=\"Expected rotation matrix with shape\"):\n pr.check_matrix(R)\n\n R = np.array([[1, 0, 0], [0, 1, 0], [0, 0.1, 1]])\n with pytest.raises(ValueError, match=\"inversion by transposition\"):\n pr.check_matrix(R)\n with warnings.catch_warnings(record=True) as w:\n pr.check_matrix(R, strict_check=False)\n assert len(w) == 1\n\n R = np.array([[1, 0, 1e-16], [0, 1, 0], [0, 0, 1]])\n R2 = pr.check_matrix(R)\n assert_array_equal(R, R2)\n\n R = -np.eye(3)\n with pytest.raises(ValueError, match=\"determinant\"):\n pr.check_matrix(R)\n with warnings.catch_warnings(record=True) as w:\n pr.check_matrix(R, strict_check=False)\n assert len(w) == 1\n\n\ndef test_check_matrix_threshold():\n \"\"\"Test matrix threshold.\n\n See issue #54.\n \"\"\"\n R = np.array(\n [\n [-9.15361835e-01, 4.01808328e-01, 2.57475872e-02],\n [5.15480570e-02, 1.80374088e-01, -9.82246499e-01],\n [-3.99318925e-01, -8.97783496e-01, -1.85819250e-01],\n ]\n )\n pr.assert_rotation_matrix(R)\n pr.check_matrix(R)\n\n\ndef test_assert_rotation_matrix_behaves_like_check_matrix():\n \"\"\"Test of both checks for rotation matrix validity behave similar.\"\"\"\n rng = np.random.default_rng(2345)\n for _ in range(5):\n a = pr.random_axis_angle(rng)\n R = pr.matrix_from_axis_angle(a)\n original_value = R[2, 2]\n for error in [0, 1e-8, 1e-7, 1e-5, 1e-4, 1]:\n R[2, 2] = original_value + error\n try:\n pr.assert_rotation_matrix(R)\n pr.check_matrix(R)\n except AssertionError:\n with pytest.raises(\n ValueError, match=\"Expected rotation matrix\"\n ):\n pr.check_matrix(R)\n\n\ndef test_deactivate_rotation_matrix_precision_error():\n R = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 2.0]])\n with pytest.raises(ValueError, match=\"Expected rotation matrix\"):\n pr.check_matrix(R)\n with warnings.catch_warnings(record=True) as w:\n pr.check_matrix(R, strict_check=False)\n assert len(w) == 1\n\n\ndef test_matrix_requires_renormalization():\n R = np.eye(3)\n assert not pr.matrix_requires_renormalization(R)\n\n R[1, 0] += 1e-3\n assert pr.matrix_requires_renormalization(R)\n\n rng = np.random.default_rng(39232)\n R_total = np.eye(3)\n for _ in range(10):\n e = pr.random_vector(rng, 3)\n R = pr.active_matrix_from_extrinsic_roll_pitch_yaw(e)\n assert not pr.matrix_requires_renormalization(R, tolerance=1e-16)\n R_total = np.dot(R, R_total)\n assert pr.matrix_requires_renormalization(R_total, tolerance=1e-16)\n\n\ndef test_norm_rotation_matrix():\n R = pr.norm_matrix(np.eye(3))\n assert_array_equal(R, np.eye(3))\n\n R[1, 0] += np.finfo(float).eps\n R = pr.norm_matrix(R)\n assert_array_equal(R, np.eye(3))\n assert np.linalg.det(R) == 1.0\n\n R = np.eye(3)\n R[1, 1] += 0.3\n R_norm = pr.norm_matrix(R)\n assert pytest.approx(np.linalg.det(R_norm)) == 1.0\n assert_array_almost_equal(np.eye(3), R_norm)\n\n\ndef test_matrix_from_two_vectors():\n with pytest.raises(ValueError, match=\"a must not be the zero vector\"):\n pr.matrix_from_two_vectors(np.zeros(3), np.zeros(3))\n with pytest.raises(ValueError, match=\"b must not be the zero vector\"):\n pr.matrix_from_two_vectors(np.ones(3), np.zeros(3))\n with pytest.raises(ValueError, match=\"a and b must not be parallel\"):\n pr.matrix_from_two_vectors(np.ones(3), np.ones(3))\n\n R = pr.matrix_from_two_vectors(pr.unitx, pr.unity)\n assert_array_almost_equal(R, np.eye(3))\n\n rng = np.random.default_rng(28)\n for _ in range(5):\n a = pr.random_vector(rng, 3)\n b = pr.random_vector(rng, 3)\n R = pr.matrix_from_two_vectors(a, b)\n pr.assert_rotation_matrix(R)\n assert_array_almost_equal(pr.norm_vector(a), R[:, 0])\n assert (\n pytest.approx(pr.angle_between_vectors(b, R[:, 2])) == 0.5 * np.pi\n )\n\n\ndef test_conversions_matrix_axis_angle():\n \"\"\"Test conversions between rotation matrix and axis-angle.\"\"\"\n R = np.eye(3)\n a = pr.axis_angle_from_matrix(R)\n pr.assert_axis_angle_equal(a, np.array([1, 0, 0, 0]))\n\n R = pr.active_matrix_from_intrinsic_euler_xyz(\n np.array([-np.pi, -np.pi, 0.0])\n )\n a = pr.axis_angle_from_matrix(R)\n pr.assert_axis_angle_equal(a, np.array([0, 0, 1, np.pi]))\n\n R = pr.active_matrix_from_intrinsic_euler_xyz(\n np.array([-np.pi, 0.0, -np.pi])\n )\n a = pr.axis_angle_from_matrix(R)\n pr.assert_axis_angle_equal(a, np.array([0, 1, 0, np.pi]))\n\n R = pr.active_matrix_from_intrinsic_euler_xyz(\n np.array([0.0, -np.pi, -np.pi])\n )\n a = pr.axis_angle_from_matrix(R)\n pr.assert_axis_angle_equal(a, np.array([1, 0, 0, np.pi]))\n\n a = np.array([np.sqrt(0.5), np.sqrt(0.5), 0.0, np.pi])\n R = pr.matrix_from_axis_angle(a)\n a2 = pr.axis_angle_from_matrix(R)\n pr.assert_axis_angle_equal(a2, a)\n\n rng = np.random.default_rng(0)\n for _ in range(50):\n a = pr.random_axis_angle(rng)\n R = pr.matrix_from_axis_angle(a)\n pr.assert_rotation_matrix(R)\n\n a2 = pr.axis_angle_from_matrix(R)\n pr.assert_axis_angle_equal(a, a2)\n\n R2 = pr.matrix_from_axis_angle(a2)\n assert_array_almost_equal(R, R2)\n pr.assert_rotation_matrix(R2)\n\n\ndef test_compare_axis_angle_from_matrix_to_lynch_park():\n \"\"\"Compare log(R) to the version of Lynch, Park: Modern Robotics.\"\"\"\n R = np.eye(3)\n a = pr.axis_angle_from_matrix(R)\n pr.assert_axis_angle_equal(a, [0, 0, 0, 0])\n\n R = pr.passive_matrix_from_angle(2, np.pi)\n assert pytest.approx(np.trace(R)) == -1\n a = pr.axis_angle_from_matrix(R)\n axis = (\n 1.0\n / np.sqrt(2.0 * (1 + R[2, 2]))\n * np.array([R[0, 2], R[1, 2], 1 + R[2, 2]])\n )\n pr.assert_axis_angle_equal(a, np.hstack((axis, (np.pi,))))\n\n R = pr.passive_matrix_from_angle(1, np.pi)\n assert pytest.approx(np.trace(R)) == -1\n a = pr.axis_angle_from_matrix(R)\n axis = (\n 1.0\n / np.sqrt(2.0 * (1 + R[1, 1]))\n * np.array([R[0, 1], 1 + R[1, 1], R[2, 1]])\n )\n pr.assert_axis_angle_equal(a, np.hstack((axis, (np.pi,))))\n\n R = pr.passive_matrix_from_angle(0, np.pi)\n assert pytest.approx(np.trace(R)) == -1\n a = pr.axis_angle_from_matrix(R)\n axis = (\n 1.0\n / np.sqrt(2.0 * (1 + R[0, 0]))\n * np.array([1 + R[0, 0], R[1, 0], R[2, 0]])\n )\n pr.assert_axis_angle_equal(a, np.hstack((axis, (np.pi,))))\n\n # normal case is omitted here\n\n\ndef test_conversions_matrix_compact_axis_angle():\n \"\"\"Test conversions between rotation matrix and axis-angle.\"\"\"\n R = np.eye(3)\n a = pr.compact_axis_angle_from_matrix(R)\n pr.assert_compact_axis_angle_equal(a, np.zeros(3))\n\n R = pr.active_matrix_from_intrinsic_euler_xyz(\n np.array([-np.pi, -np.pi, 0.0])\n )\n a = pr.compact_axis_angle_from_matrix(R)\n pr.assert_compact_axis_angle_equal(a, np.array([0, 0, np.pi]))\n\n R = pr.active_matrix_from_intrinsic_euler_xyz(\n np.array([-np.pi, 0.0, -np.pi])\n )\n a = pr.compact_axis_angle_from_matrix(R)\n pr.assert_compact_axis_angle_equal(a, np.array([0, np.pi, 0]))\n\n R = pr.active_matrix_from_intrinsic_euler_xyz(\n np.array([0.0, -np.pi, -np.pi])\n )\n a = pr.compact_axis_angle_from_matrix(R)\n pr.assert_compact_axis_angle_equal(a, np.array([np.pi, 0, 0]))\n\n a = np.array([np.sqrt(0.5) * np.pi, np.sqrt(0.5) * np.pi, 0.0])\n R = pr.matrix_from_compact_axis_angle(a)\n a2 = pr.compact_axis_angle_from_matrix(R)\n pr.assert_compact_axis_angle_equal(a2, a)\n\n rng = np.random.default_rng(0)\n for _ in range(50):\n a = pr.random_compact_axis_angle(rng)\n R = pr.matrix_from_compact_axis_angle(a)\n pr.assert_rotation_matrix(R)\n\n a2 = pr.compact_axis_angle_from_matrix(R)\n pr.assert_compact_axis_angle_equal(a, a2)\n\n R2 = pr.matrix_from_compact_axis_angle(a2)\n assert_array_almost_equal(R, R2)\n pr.assert_rotation_matrix(R2)\n\n\ndef test_issue43():\n \"\"\"Test axis_angle_from_matrix() with angles close to 0 and pi.\"\"\"\n a = np.array([-1.0, 1.0, 1.0, np.pi - 5e-8])\n a[:3] = a[:3] / np.linalg.norm(a[:3])\n R = pr.matrix_from_axis_angle(a)\n a2 = pr.axis_angle_from_matrix(R)\n pr.assert_axis_angle_equal(a, a2)\n\n a = np.array([-1.0, 1.0, 1.0, 5e-8])\n a[:3] = a[:3] / np.linalg.norm(a[:3])\n R = pr.matrix_from_axis_angle(a)\n a2 = pr.axis_angle_from_matrix(R)\n pr.assert_axis_angle_equal(a, a2)\n\n a = np.array([-1.0, 1.0, 1.0, np.pi + 5e-8])\n a[:3] = a[:3] / np.linalg.norm(a[:3])\n R = pr.matrix_from_axis_angle(a)\n a2 = pr.axis_angle_from_matrix(R)\n pr.assert_axis_angle_equal(a, a2)\n\n a = np.array([-1.0, 1.0, 1.0, -5e-8])\n a[:3] = a[:3] / np.linalg.norm(a[:3])\n R = pr.matrix_from_axis_angle(a)\n a2 = pr.axis_angle_from_matrix(R)\n pr.assert_axis_angle_equal(a, a2)\n\n\ndef test_issue43_numerical_precision():\n \"\"\"Test numerical precision of angles close to 0 and pi.\"\"\"\n a = np.array([1.0, 1.0, 1.0, np.pi - 1e-7])\n a[:3] = a[:3] / np.linalg.norm(a[:3])\n R = pr.matrix_from_axis_angle(a)\n a2 = pr.axis_angle_from_matrix(R)\n axis_dist = np.linalg.norm(a[:3] - a2[:3])\n assert axis_dist < 1e-10\n assert abs(a[3] - a2[3]) < 1e-8\n\n a = np.array([1.0, 1.0, 1.0, 1e-7])\n a[:3] = a[:3] / np.linalg.norm(a[:3])\n R = pr.matrix_from_axis_angle(a)\n a2 = pr.axis_angle_from_matrix(R)\n axis_dist = np.linalg.norm(a[:3] - a2[:3])\n assert axis_dist < 1e-10\n assert abs(a[3] - a2[3]) < 1e-8\n\n\ndef test_conversions_matrix_axis_angle_continuous():\n \"\"\"Test continuous conversions between rotation matrix and axis-angle.\"\"\"\n for angle in np.arange(3.1, 3.2, 0.01):\n a = np.array([1.0, 0.0, 0.0, angle])\n R = pr.matrix_from_axis_angle(a)\n pr.assert_rotation_matrix(R)\n\n a2 = pr.axis_angle_from_matrix(R)\n pr.assert_axis_angle_equal(a, a2)\n\n R2 = pr.matrix_from_axis_angle(a2)\n assert_array_almost_equal(R, R2)\n pr.assert_rotation_matrix(R2)\n\n\ndef test_matrix_from_quaternion_hamilton():\n \"\"\"Test if the conversion from quaternion to matrix is Hamiltonian.\"\"\"\n q = np.sqrt(0.5) * np.array([1, 0, 0, 1])\n R = pr.matrix_from_quaternion(q)\n assert_array_almost_equal(np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]), R)\n\n\ndef test_quaternion_from_matrix_180():\n \"\"\"Test for bug in conversion from 180 degree rotations.\"\"\"\n a = np.array([1.0, 0.0, 0.0, np.pi])\n q = pr.quaternion_from_axis_angle(a)\n R = pr.matrix_from_axis_angle(a)\n q_from_R = pr.quaternion_from_matrix(R)\n assert_array_almost_equal(q, q_from_R)\n\n a = np.array([0.0, 1.0, 0.0, np.pi])\n q = pr.quaternion_from_axis_angle(a)\n R = pr.matrix_from_axis_angle(a)\n q_from_R = pr.quaternion_from_matrix(R)\n assert_array_almost_equal(q, q_from_R)\n\n a = np.array([0.0, 0.0, 1.0, np.pi])\n q = pr.quaternion_from_axis_angle(a)\n R = pr.matrix_from_axis_angle(a)\n q_from_R = pr.quaternion_from_matrix(R)\n assert_array_almost_equal(q, q_from_R)\n\n R = np.array([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, -1.0]])\n with pytest.raises(ValueError, match=\"Expected rotation matrix\"):\n pr.quaternion_from_matrix(R)\n\n R = np.array(\n [[-1.0, 0.0, 0.0], [0.0, 0.00000001, 1.0], [0.0, 1.0, -0.00000001]]\n )\n q_from_R = pr.quaternion_from_matrix(R)\n\n\ndef test_quaternion_from_matrix_180_not_axis_aligned():\n \"\"\"Test for bug in rotation by 180 degrees around arbitrary axes.\"\"\"\n rng = np.random.default_rng(0)\n for _ in range(10):\n a = pr.random_axis_angle(rng)\n a[3] = np.pi\n q = pr.quaternion_from_axis_angle(a)\n R = pr.matrix_from_axis_angle(a)\n q_from_R = pr.quaternion_from_matrix(R)\n pr.assert_quaternion_equal(q, q_from_R)\n\n\ndef test_axis_angle_from_matrix_cos_angle_greater_1():\n R = np.array(\n [\n [\n 1.0000000000000004,\n -1.4402617650886727e-08,\n 2.3816502339526408e-08,\n ],\n [\n 1.4402617501592725e-08,\n 1.0000000000000004,\n 1.2457848566326355e-08,\n ],\n [\n -2.3816502529500374e-08,\n -1.2457848247850049e-08,\n 0.9999999999999999,\n ],\n ]\n )\n a = pr.axis_angle_from_matrix(R)\n assert not any(np.isnan(a))\n\n\ndef test_axis_angle_from_matrix_without_check():\n R = -np.eye(3)\n with warnings.catch_warnings(record=True) as w:\n a = pr.axis_angle_from_matrix(R, check=False)\n assert len(w) == 1\n assert all(np.isnan(a[:3]))\n\n\ndef test_bug_189():\n \"\"\"Test bug #189\"\"\"\n R = np.array(\n [\n [\n -1.0000000000000004e00,\n 2.8285718503485576e-16,\n 1.0966597378775709e-16,\n ],\n [\n 1.0966597378775709e-16,\n -2.2204460492503131e-16,\n 1.0000000000000002e00,\n ],\n [\n 2.8285718503485576e-16,\n 1.0000000000000002e00,\n -2.2204460492503131e-16,\n ],\n ]\n )\n a1 = pr.compact_axis_angle_from_matrix(R)\n a2 = pr.compact_axis_angle_from_matrix(pr.norm_matrix(R))\n assert_array_almost_equal(a1, a2)\n\n\ndef test_bug_198():\n \"\"\"Test bug #198\"\"\"\n R = np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]], dtype=float)\n a = pr.compact_axis_angle_from_matrix(R)\n R2 = pr.matrix_from_compact_axis_angle(a)\n assert_array_almost_equal(R, R2)\n" }, { "path": "pytransform3d/rotations/test/test_mrp.py", "content": "import numpy as np\nimport pytest\nfrom numpy.testing import assert_array_almost_equal\n\nimport pytransform3d.rotations as pr\n\n\ndef test_check_mrp():\n with pytest.raises(\n ValueError, match=\"Expected modified Rodrigues parameters with shape\"\n ):\n pr.check_mrp([])\n with pytest.raises(\n ValueError, match=\"Expected modified Rodrigues parameters with shape\"\n ):\n pr.check_mrp(np.zeros((3, 4)))\n\n\ndef test_norm_mrp():\n mrp_norm = pr.norm_mrp(pr.mrp_from_axis_angle([1.0, 0.0, 0.0, 1.5 * np.pi]))\n assert_array_almost_equal(\n [-1.0, 0.0, 0.0, 0.5 * np.pi], pr.axis_angle_from_mrp(mrp_norm)\n )\n\n mrp_norm = pr.norm_mrp(\n pr.mrp_from_axis_angle([1.0, 0.0, 0.0, -0.5 * np.pi])\n )\n assert_array_almost_equal(\n [-1.0, 0.0, 0.0, 0.5 * np.pi], pr.axis_angle_from_mrp(mrp_norm)\n )\n\n mrp_norm = pr.norm_mrp(pr.mrp_from_axis_angle([1.0, 0.0, 0.0, 2.0 * np.pi]))\n assert_array_almost_equal(\n [1.0, 0.0, 0.0, 0.0], pr.axis_angle_from_mrp(mrp_norm)\n )\n\n mrp_norm = pr.norm_mrp(\n pr.mrp_from_axis_angle([1.0, 0.0, 0.0, -2.0 * np.pi])\n )\n assert_array_almost_equal(\n [1.0, 0.0, 0.0, 0.0], pr.axis_angle_from_mrp(mrp_norm)\n )\n\n\ndef test_mrp_near_singularity():\n axis = np.array([1.0, 0.0, 0.0])\n assert pr.mrp_near_singularity(np.tan(2.0 * np.pi / 4.0) * axis)\n assert pr.mrp_near_singularity(np.tan(2.0 * np.pi / 4.0 - 1e-7) * axis)\n assert pr.mrp_near_singularity(np.tan(2.0 * np.pi / 4.0 + 1e-7) * axis)\n assert not pr.mrp_near_singularity(np.tan(np.pi / 4.0) * axis)\n assert not pr.mrp_near_singularity(np.tan(0.0 / 4.0) * axis)\n\n\ndef test_mrp_double():\n rng = np.random.default_rng(23238)\n mrp = pr.random_vector(rng, 3)\n mrp_double = pr.mrp_double(mrp)\n q = pr.quaternion_from_mrp(mrp)\n q_double = pr.quaternion_from_mrp(mrp_double)\n pr.assert_mrp_equal(mrp, mrp_double)\n assert not np.allclose(mrp, mrp_double)\n pr.assert_quaternion_equal(q, q_double)\n assert not np.allclose(q, q_double)\n\n assert_array_almost_equal(np.zeros(3), pr.mrp_double(np.zeros(3)))\n\n\ndef test_concatenate_mrp():\n rng = np.random.default_rng(283)\n for _ in range(5):\n q1 = pr.random_quaternion(rng)\n q2 = pr.random_quaternion(rng)\n q12 = pr.concatenate_quaternions(q1, q2)\n mrp1 = pr.mrp_from_quaternion(q1)\n mrp2 = pr.mrp_from_quaternion(q2)\n mrp12 = pr.concatenate_mrp(mrp1, mrp2)\n pr.assert_quaternion_equal(q12, pr.quaternion_from_mrp(mrp12))\n\n\ndef test_mrp_prod_vector():\n rng = np.random.default_rng(2183)\n v = pr.random_vector(rng, 3)\n assert_array_almost_equal(v, pr.mrp_prod_vector([0, 0, 0], v))\n\n for _ in range(5):\n mrp = pr.random_vector(rng, 3)\n q = pr.quaternion_from_mrp(mrp)\n v_mrp = pr.mrp_prod_vector(mrp, v)\n v_q = pr.q_prod_vector(q, v)\n assert_array_almost_equal(v_mrp, v_q)\n\n\ndef test_mrp_quat_conversions():\n rng = np.random.default_rng(22)\n\n for _ in range(5):\n q = pr.random_quaternion(rng)\n mrp = pr.mrp_from_quaternion(q)\n q2 = pr.quaternion_from_mrp(mrp)\n pr.assert_quaternion_equal(q, q2)\n\n\ndef test_axis_angle_from_mrp():\n rng = np.random.default_rng(98343)\n for _ in range(5):\n mrp = pr.random_vector(rng, 3)\n a = pr.axis_angle_from_mrp(mrp)\n q = pr.quaternion_from_mrp(mrp)\n pr.assert_axis_angle_equal(a, pr.axis_angle_from_quaternion(q))\n\n pr.assert_axis_angle_equal(\n pr.axis_angle_from_mrp([np.tan(0.5 * np.pi), 0.0, 0.0]),\n [1.0, 0.0, 0.0, 0.0],\n )\n\n pr.assert_axis_angle_equal(\n pr.axis_angle_from_mrp([0.0, 0.0, 0.0]), [1.0, 0.0, 0.0, 0.0]\n )\n" }, { "path": "pytransform3d/rotations/test/test_polar_decom.py", "content": "import numpy as np\nimport pytest\nfrom numpy.testing import assert_array_almost_equal\n\nimport pytransform3d.rotations as pr\n\n\ndef test_polar_decomposition():\n # no iterations required\n R_norm = pr.robust_polar_decomposition(np.eye(3), n_iter=1, eps=1e-32)\n assert_array_almost_equal(R_norm, np.eye(3))\n\n rng = np.random.default_rng(33)\n\n # scaled valid rotation matrix\n R = pr.random_matrix(rng)\n R_norm = pr.robust_polar_decomposition(23.5 * R)\n assert_array_almost_equal(R_norm, R)\n\n # slightly rotated basis vector\n R = pr.matrix_from_euler(np.deg2rad([-45, 45, 45]), 2, 1, 0, True)\n R_unnormalized = np.copy(R)\n R_unnormalized[:, 0] = np.dot(\n pr.active_matrix_from_angle(2, np.deg2rad(1.0)), R_unnormalized[:, 0]\n )\n # norm_matrix will just fix the orthogonality with the cross product of the\n # two other basis vectors\n assert_array_almost_equal(pr.norm_matrix(R_unnormalized), R)\n # polar decomposition will spread the error more evenly among basis vectors\n errors = np.linalg.norm((R_unnormalized - R).T, axis=-1)\n assert pytest.approx(errors.tolist()) == [0.00909314, 0, 0]\n R_norm = pr.robust_polar_decomposition(R_unnormalized)\n norm_errors = np.linalg.norm((R_norm - R).T, axis=-1)\n assert np.all(norm_errors > 0)\n assert np.all(norm_errors < errors[0])\n assert np.std(norm_errors) < np.std(errors)\n\n # random rotations of random basis vectors\n for _ in range(5):\n R = pr.random_matrix(rng)\n R_unnormalized = np.copy(R)\n random_axis = rng.integers(0, 3)\n R_unnormalized[:, random_axis] = np.dot(\n pr.active_matrix_from_angle(\n rng.integers(0, 3), np.deg2rad(rng.uniform(-3, 3))\n ),\n R_unnormalized[:, random_axis],\n )\n R_norm = pr.robust_polar_decomposition(R_unnormalized)\n errors = np.linalg.norm((R_unnormalized - R).T, axis=-1)\n norm_errors = np.linalg.norm((R_norm - R).T, axis=-1)\n assert np.all(norm_errors > 0)\n assert np.all(norm_errors < errors.sum())\n assert np.std(norm_errors) < np.std(errors)\n" }, { "path": "pytransform3d/rotations/test/test_quaternion.py", "content": "import numpy as np\nimport pytest\nfrom numpy.testing import assert_array_almost_equal, assert_array_equal\n\nimport pytransform3d.rotations as pr\n\n\ndef test_check_quaternion():\n \"\"\"Test input validation for quaternion representation.\"\"\"\n q_list = [1, 0, 0, 0]\n q = pr.check_quaternion(q_list)\n assert_array_almost_equal(q_list, q)\n assert type(q) is np.ndarray\n assert q.dtype == np.float64\n\n rng = np.random.default_rng(0)\n q = rng.standard_normal(4)\n q = pr.check_quaternion(q)\n assert pytest.approx(np.linalg.norm(q)) == 1.0\n\n with pytest.raises(ValueError, match=\"Expected quaternion with shape\"):\n pr.check_quaternion(np.zeros(3))\n with pytest.raises(ValueError, match=\"Expected quaternion with shape\"):\n pr.check_quaternion(np.zeros((3, 3)))\n\n q = np.array([0.0, 1.2, 0.0, 0.0])\n q2 = pr.check_quaternion(q, unit=False)\n assert_array_almost_equal(q, q2)\n\n\ndef test_check_quaternions():\n \"\"\"Test input validation for sequence of quaternions.\"\"\"\n Q_list = [[1, 0, 0, 0]]\n Q = pr.check_quaternions(Q_list)\n assert_array_almost_equal(Q_list, Q)\n assert type(Q) is np.ndarray\n assert Q.dtype == np.float64\n assert Q.ndim == 2\n assert_array_equal(Q.shape, (1, 4))\n\n Q = np.array([[2, 0, 0, 0], [3, 0, 0, 0], [4, 0, 0, 0], [5, 0, 0, 0]])\n Q = pr.check_quaternions(Q)\n for i in range(len(Q)):\n assert pytest.approx(np.linalg.norm(Q[i])) == 1\n\n with pytest.raises(\n ValueError, match=\"Expected quaternion array with shape\"\n ):\n pr.check_quaternions(np.zeros(4))\n with pytest.raises(\n ValueError, match=\"Expected quaternion array with shape\"\n ):\n pr.check_quaternions(np.zeros((3, 3)))\n\n Q = np.array([[0.0, 1.2, 0.0, 0.0]])\n Q2 = pr.check_quaternions(Q, unit=False)\n assert_array_almost_equal(Q, Q2)\n\n\ndef test_quaternion_requires_renormalization():\n assert not pr.quaternion_requires_renormalization(pr.q_id)\n\n q = pr.q_id + np.array([1e-3, 0.0, 0.0, 0.0])\n assert pr.quaternion_requires_renormalization(q)\n\n\ndef test_quaternion_double():\n rng = np.random.default_rng(2235)\n for _ in range(5):\n q1 = pr.random_quaternion(rng)\n q2 = pr.quaternion_double(q1)\n pr.assert_quaternion_equal(q1, q2)\n\n\ndef test_pick_closest_quaternion():\n rng = np.random.default_rng(483)\n for _ in range(10):\n q = pr.random_quaternion(rng)\n assert_array_almost_equal(pr.pick_closest_quaternion(q, q), q)\n assert_array_almost_equal(pr.pick_closest_quaternion(-q, q), q)\n\n\ndef test_quaternion_gradient_integration():\n \"\"\"Test integration of quaternion gradients.\"\"\"\n n_steps = 21\n dt = 0.1\n rng = np.random.default_rng(3)\n for _ in range(5):\n q1 = pr.random_quaternion(rng)\n q2 = pr.random_quaternion(rng)\n Q = np.vstack(\n [pr.quaternion_slerp(q1, q2, t) for t in np.linspace(0, 1, n_steps)]\n )\n angular_velocities = pr.quaternion_gradient(Q, dt)\n Q2 = pr.quaternion_integrate(angular_velocities, q1, dt)\n assert_array_almost_equal(Q, Q2)\n\n\ndef test_concatenate_quaternions():\n \"\"\"Test concatenation of two quaternions.\"\"\"\n rng = np.random.default_rng(0)\n for _ in range(5):\n q1 = pr.quaternion_from_axis_angle(pr.random_axis_angle(rng))\n q2 = pr.quaternion_from_axis_angle(pr.random_axis_angle(rng))\n\n R1 = pr.matrix_from_quaternion(q1)\n R2 = pr.matrix_from_quaternion(q2)\n\n q12 = pr.concatenate_quaternions(q1, q2)\n R12 = np.dot(R1, R2)\n q12R = pr.quaternion_from_matrix(R12)\n\n pr.assert_quaternion_equal(q12, q12R)\n\n\ndef test_concatenate_quaternions_list_array():\n \"\"\"Test concatenation of two quaternions given as list and array.\"\"\"\n # Until ea9adc5, this combination of a list and a numpy array raised\n # a ValueError:\n q1 = [1, 0, 0, 0]\n q2 = np.array([0, 0, 0, 1])\n q12 = pr.concatenate_quaternions(q1, q2)\n assert_array_almost_equal(q12, np.array([0, 0, 0, 1]))\n\n\ndef test_quaternion_hamilton():\n \"\"\"Test if quaternion multiplication follows Hamilton's convention.\"\"\"\n q_ij = pr.concatenate_quaternions(pr.q_i, pr.q_j)\n assert_array_equal(pr.q_k, q_ij)\n q_ijk = pr.concatenate_quaternions(q_ij, pr.q_k)\n assert_array_equal(-pr.q_id, q_ijk)\n\n\ndef test_quaternion_rotation():\n \"\"\"Test quaternion rotation.\"\"\"\n rng = np.random.default_rng(0)\n for _ in range(5):\n q = pr.quaternion_from_axis_angle(pr.random_axis_angle(rng))\n R = pr.matrix_from_quaternion(q)\n v = pr.random_vector(rng)\n vR = np.dot(R, v)\n vq = pr.q_prod_vector(q, v)\n assert_array_almost_equal(vR, vq)\n\n\ndef test_quaternion_rotation_consistent_with_multiplication():\n \"\"\"Test if quaternion rotation and multiplication are Hamiltonian.\"\"\"\n rng = np.random.default_rng(1)\n for _ in range(5):\n v = pr.random_vector(rng)\n q = pr.random_quaternion(rng)\n v_im = np.hstack(((0.0,), v))\n qv_mult = pr.concatenate_quaternions(\n q, pr.concatenate_quaternions(v_im, pr.q_conj(q))\n )[1:]\n qv_rot = pr.q_prod_vector(q, v)\n assert_array_almost_equal(qv_mult, qv_rot)\n\n\ndef test_quaternion_conjugate():\n \"\"\"Test quaternion conjugate.\"\"\"\n rng = np.random.default_rng(0)\n for _ in range(5):\n q = pr.random_quaternion(rng)\n v = pr.random_vector(rng)\n vq = pr.q_prod_vector(q, v)\n vq2 = pr.concatenate_quaternions(\n pr.concatenate_quaternions(q, np.hstack(([0], v))), pr.q_conj(q)\n )[1:]\n assert_array_almost_equal(vq, vq2)\n\n\ndef test_quaternion_invert():\n \"\"\"Test unit quaternion inversion with conjugate.\"\"\"\n q = np.array([0.58183503, -0.75119889, -0.24622332, 0.19116072])\n q_inv = pr.q_conj(q)\n q_q_inv = pr.concatenate_quaternions(q, q_inv)\n assert_array_almost_equal(pr.q_id, q_q_inv)\n\n\ndef test_quaternion_dist():\n \"\"\"Test angular metric of quaternions.\"\"\"\n rng = np.random.default_rng(0)\n\n for _ in range(5):\n q1 = pr.quaternion_from_axis_angle(pr.random_axis_angle(rng))\n q2 = pr.quaternion_from_axis_angle(pr.random_axis_angle(rng))\n q1_to_q1 = pr.quaternion_dist(q1, q1)\n assert pytest.approx(q1_to_q1) == 0.0\n q2_to_q2 = pr.quaternion_dist(q2, q2)\n assert pytest.approx(q2_to_q2) == 0.0\n q1_to_q2 = pr.quaternion_dist(q1, q2)\n q2_to_q1 = pr.quaternion_dist(q2, q1)\n assert pytest.approx(q1_to_q2) == q2_to_q1\n assert 2.0 * np.pi > q1_to_q2\n\n\ndef test_quaternion_dist_for_identical_rotations():\n \"\"\"Test angular metric of quaternions q and -q.\"\"\"\n rng = np.random.default_rng(0)\n\n for _ in range(5):\n q = pr.quaternion_from_axis_angle(pr.random_axis_angle(rng))\n assert_array_almost_equal(\n pr.matrix_from_quaternion(q), pr.matrix_from_quaternion(-q)\n )\n assert pr.quaternion_dist(q, -q) == 0.0\n\n\ndef test_quaternion_dist_for_almost_identical_rotations():\n \"\"\"Test angular metric of quaternions q and ca. -q.\"\"\"\n rng = np.random.default_rng(0)\n\n for _ in range(5):\n a = pr.random_axis_angle(rng)\n q1 = pr.quaternion_from_axis_angle(a)\n r = 1e-4 * rng.standard_normal(4)\n q2 = -pr.quaternion_from_axis_angle(a + r)\n assert pytest.approx(pr.quaternion_dist(q1, q2), abs=1e-3) == 0.0\n\n\ndef test_quaternion_diff():\n \"\"\"Test difference of quaternions.\"\"\"\n rng = np.random.default_rng(0)\n\n for _ in range(5):\n q1 = pr.random_quaternion(rng)\n q2 = pr.random_quaternion(rng)\n a_diff = pr.quaternion_diff(q1, q2) # q1 - q2\n q_diff = pr.quaternion_from_axis_angle(a_diff)\n q3 = pr.concatenate_quaternions(q_diff, q2) # q1 - q2 + q2\n pr.assert_quaternion_equal(q1, q3)\n\n\ndef test_quaternion_from_euler():\n \"\"\"Quaternion from Euler angles.\"\"\"\n with pytest.raises(\n ValueError, match=\"Axis index i \\\\(-1\\\\) must be in \\\\[0, 1, 2\\\\]\"\n ):\n pr.quaternion_from_euler(np.zeros(3), -1, 0, 2, True)\n with pytest.raises(\n ValueError, match=\"Axis index i \\\\(3\\\\) must be in \\\\[0, 1, 2\\\\]\"\n ):\n pr.quaternion_from_euler(np.zeros(3), 3, 0, 2, True)\n with pytest.raises(\n ValueError, match=\"Axis index j \\\\(-1\\\\) must be in \\\\[0, 1, 2\\\\]\"\n ):\n pr.quaternion_from_euler(np.zeros(3), 2, -1, 2, True)\n with pytest.raises(\n ValueError, match=\"Axis index j \\\\(3\\\\) must be in \\\\[0, 1, 2\\\\]\"\n ):\n pr.quaternion_from_euler(np.zeros(3), 2, 3, 2, True)\n with pytest.raises(\n ValueError, match=\"Axis index k \\\\(-1\\\\) must be in \\\\[0, 1, 2\\\\]\"\n ):\n pr.quaternion_from_euler(np.zeros(3), 2, 0, -1, True)\n with pytest.raises(\n ValueError, match=\"Axis index k \\\\(3\\\\) must be in \\\\[0, 1, 2\\\\]\"\n ):\n pr.quaternion_from_euler(np.zeros(3), 2, 0, 3, True)\n\n euler_axes = [\n [0, 2, 0],\n [0, 1, 0],\n [1, 0, 1],\n [1, 2, 1],\n [2, 1, 2],\n [2, 0, 2],\n [0, 2, 1],\n [0, 1, 2],\n [1, 0, 2],\n [1, 2, 0],\n [2, 1, 0],\n [2, 0, 1],\n ]\n rng = np.random.default_rng(83)\n for ea in euler_axes:\n for extrinsic in [False, True]:\n for _ in range(5):\n e = rng.random(3)\n e[0] = 2.0 * np.pi * e[0] - np.pi\n e[1] = np.pi * e[1]\n e[2] = 2.0 * np.pi * e[2] - np.pi\n\n proper_euler = ea[0] == ea[2]\n if proper_euler:\n e[1] -= np.pi / 2.0\n\n q = pr.quaternion_from_euler(e, ea[0], ea[1], ea[2], extrinsic)\n e2 = pr.euler_from_quaternion(q, ea[0], ea[1], ea[2], extrinsic)\n q2 = pr.quaternion_from_euler(\n e2, ea[0], ea[1], ea[2], extrinsic\n )\n\n pr.assert_quaternion_equal(q, q2)\n\n\ndef test_conversions_matrix_quaternion():\n \"\"\"Test conversions between rotation matrix and quaternion.\"\"\"\n R = np.eye(3)\n a = pr.axis_angle_from_matrix(R)\n assert_array_almost_equal(a, np.array([1, 0, 0, 0]))\n\n rng = np.random.default_rng(0)\n for _ in range(5):\n q = pr.random_quaternion(rng)\n R = pr.matrix_from_quaternion(q)\n pr.assert_rotation_matrix(R)\n\n q2 = pr.quaternion_from_matrix(R)\n pr.assert_quaternion_equal(q, q2)\n\n R2 = pr.matrix_from_quaternion(q2)\n assert_array_almost_equal(R, R2)\n pr.assert_rotation_matrix(R2)\n\n\ndef test_quaternion_conventions():\n \"\"\"Test conversion of quaternion between wxyz and xyzw.\"\"\"\n q_wxyz = np.array([1.0, 0.0, 0.0, 0.0])\n q_xyzw = pr.quaternion_xyzw_from_wxyz(q_wxyz)\n assert_array_equal(q_xyzw, np.array([0.0, 0.0, 0.0, 1.0]))\n q_wxyz2 = pr.quaternion_wxyz_from_xyzw(q_xyzw)\n assert_array_equal(q_wxyz, q_wxyz2)\n\n rng = np.random.default_rng(42)\n q_wxyz_random = pr.random_quaternion(rng)\n q_xyzw_random = pr.quaternion_xyzw_from_wxyz(q_wxyz_random)\n assert_array_equal(q_xyzw_random[:3], q_wxyz_random[1:])\n assert q_xyzw_random[3] == q_wxyz_random[0]\n q_wxyz_random2 = pr.quaternion_wxyz_from_xyzw(q_xyzw_random)\n assert_array_equal(q_wxyz_random, q_wxyz_random2)\n" }, { "path": "pytransform3d/rotations/test/test_rot_log.py", "content": "import warnings\n\nimport numpy as np\nimport pytest\nfrom numpy.testing import assert_array_almost_equal\n\nimport pytransform3d.rotations as pr\n\n\ndef test_check_skew_symmetric_matrix():\n with pytest.raises(\n ValueError, match=\"Expected skew-symmetric matrix with shape\"\n ):\n pr.check_skew_symmetric_matrix([])\n with pytest.raises(\n ValueError, match=\"Expected skew-symmetric matrix with shape\"\n ):\n pr.check_skew_symmetric_matrix(np.zeros((3, 4)))\n with pytest.raises(\n ValueError, match=\"Expected skew-symmetric matrix with shape\"\n ):\n pr.check_skew_symmetric_matrix(np.zeros((4, 3)))\n V = np.zeros((3, 3))\n V[0, 0] = 0.001\n with pytest.raises(\n ValueError,\n match=\"Expected skew-symmetric matrix, but it failed the test\",\n ):\n pr.check_skew_symmetric_matrix(V)\n with warnings.catch_warnings(record=True) as w:\n pr.check_skew_symmetric_matrix(V, strict_check=False)\n assert len(w) == 1\n\n pr.check_skew_symmetric_matrix(np.zeros((3, 3)))\n\n\ndef test_cross_product_matrix():\n \"\"\"Test cross-product matrix.\"\"\"\n rng = np.random.default_rng(0)\n for _ in range(5):\n v = pr.random_vector(rng)\n w = pr.random_vector(rng)\n V = pr.cross_product_matrix(v)\n pr.check_skew_symmetric_matrix(V)\n r1 = np.cross(v, w)\n r2 = np.dot(V, w)\n assert_array_almost_equal(r1, r2)\n" }, { "path": "pytransform3d/rotations/test/test_rotations_jacobians.py", "content": "import numpy as np\nfrom numpy.testing import assert_array_almost_equal\n\nimport pytransform3d.rotations as pr\n\n\ndef test_jacobian_so3():\n omega = np.zeros(3)\n\n J = pr.left_jacobian_SO3(omega)\n J_series = pr.left_jacobian_SO3_series(omega, 20)\n assert_array_almost_equal(J, J_series)\n\n J_inv = pr.left_jacobian_SO3_inv(omega)\n J_inv_series = pr.left_jacobian_SO3_inv_series(omega, 20)\n assert_array_almost_equal(J_inv, J_inv_series)\n\n J_inv_J = np.dot(J_inv, J)\n assert_array_almost_equal(J_inv_J, np.eye(3))\n\n rng = np.random.default_rng(0)\n for _ in range(5):\n omega = pr.random_compact_axis_angle(rng)\n\n J = pr.left_jacobian_SO3(omega)\n J_series = pr.left_jacobian_SO3_series(omega, 20)\n assert_array_almost_equal(J, J_series)\n\n J_inv = pr.left_jacobian_SO3_inv(omega)\n J_inv_series = pr.left_jacobian_SO3_inv_series(omega, 20)\n assert_array_almost_equal(J_inv, J_inv_series)\n\n J_inv_J = np.dot(J_inv, J)\n assert_array_almost_equal(J_inv_J, np.eye(3))\n" }, { "path": "pytransform3d/rotations/test/test_rotations_random.py", "content": "import numpy as np\nfrom numpy.testing import assert_array_almost_equal\n\nimport pytransform3d.rotations as pr\n\n\ndef test_random_matrices():\n rng = np.random.default_rng(33)\n mean = pr.random_matrix(rng, np.eye(3), np.eye(3))\n cov = np.eye(3) * 1e-10\n samples = np.stack([pr.random_matrix(rng, mean, cov) for _ in range(10)])\n for sample in samples:\n assert_array_almost_equal(sample, mean, decimal=4)\n" }, { "path": "pytransform3d/rotations/test/test_rotor.py", "content": "import numpy as np\nimport pytest\nfrom numpy.testing import assert_array_almost_equal\n\nimport pytransform3d.rotations as pr\n\n\ndef test_check_rotor():\n r_list = [1, 0, 0, 0]\n r = pr.check_rotor(r_list)\n assert isinstance(r, np.ndarray)\n\n r2 = np.array([[1, 0, 0, 0]])\n with pytest.raises(ValueError, match=\"Expected rotor with shape\"):\n pr.check_rotor(r2)\n\n r3 = np.array([1, 0, 0])\n with pytest.raises(ValueError, match=\"Expected rotor with shape\"):\n pr.check_rotor(r3)\n\n r4 = np.array([2, 0, 0, 0])\n assert pytest.approx(np.linalg.norm(pr.check_rotor(r4))) == 1.0\n\n\ndef test_outer():\n rng = np.random.default_rng(82)\n for _ in range(5):\n a = rng.standard_normal(3)\n Zero = pr.wedge(a, a)\n assert_array_almost_equal(Zero, np.zeros(3))\n\n b = rng.standard_normal(3)\n A = pr.wedge(a, b)\n B = pr.wedge(b, a)\n assert_array_almost_equal(A, -B)\n\n c = rng.standard_normal(3)\n assert_array_almost_equal(pr.wedge(a, (b + c)), A + pr.wedge(a, c))\n\n\ndef test_plane_normal_from_bivector():\n rng = np.random.default_rng(82)\n for _ in range(5):\n a = rng.standard_normal(3)\n b = rng.standard_normal(3)\n B = pr.wedge(a, b)\n n = pr.plane_normal_from_bivector(B)\n assert_array_almost_equal(n, pr.norm_vector(np.cross(a, b)))\n\n\ndef test_geometric_product():\n rng = np.random.default_rng(83)\n for _ in range(5):\n a = rng.standard_normal(3)\n a2a = pr.geometric_product(a, 2 * a)\n assert_array_almost_equal(a2a, np.array([np.dot(a, 2 * a), 0, 0, 0]))\n\n b = pr.perpendicular_to_vector(a)\n ab = pr.geometric_product(a, b)\n assert_array_almost_equal(ab, np.hstack(((0.0,), pr.wedge(a, b))))\n\n\ndef test_geometric_product_creates_rotor_that_rotates_by_double_angle():\n rng = np.random.default_rng(83)\n for _ in range(5):\n a_unit = pr.norm_vector(rng.standard_normal(3))\n b_unit = pr.norm_vector(rng.standard_normal(3))\n # a geometric product of two unit vectors is a rotor\n ab = pr.geometric_product(a_unit, b_unit)\n assert pytest.approx(np.linalg.norm(ab)) == 1.0\n\n angle = pr.angle_between_vectors(a_unit, b_unit)\n c = pr.rotor_apply(ab, a_unit)\n double_angle = pr.angle_between_vectors(a_unit, c)\n\n assert pytest.approx(abs(pr.norm_angle(2.0 * angle))) == abs(\n pr.norm_angle(double_angle)\n )\n\n\ndef test_rotor_from_two_directions_special_cases():\n d1 = np.array([0, 0, 1])\n rotor = pr.rotor_from_two_directions(d1, d1)\n assert_array_almost_equal(rotor, np.array([1, 0, 0, 0]))\n\n rotor = pr.rotor_from_two_directions(d1, np.zeros(3))\n assert_array_almost_equal(rotor, np.array([1, 0, 0, 0]))\n\n d2 = np.array([0, 0, -1]) # 180 degree rotation\n rotor = pr.rotor_from_two_directions(d1, d2)\n assert_array_almost_equal(rotor, np.array([0, 1, 0, 0]))\n assert_array_almost_equal(pr.rotor_apply(rotor, d1), d2)\n\n d3 = np.array([1, 0, 0])\n d4 = np.array([-1, 0, 0])\n rotor = pr.rotor_from_two_directions(d3, d4)\n assert_array_almost_equal(pr.rotor_apply(rotor, d3), d4)\n\n\ndef test_rotor_from_two_directions():\n rng = np.random.default_rng(84)\n for _ in range(5):\n a = rng.standard_normal(3)\n b = rng.standard_normal(3)\n rotor = pr.rotor_from_two_directions(a, b)\n b2 = pr.rotor_apply(rotor, a)\n assert_array_almost_equal(pr.norm_vector(b), pr.norm_vector(b2))\n assert_array_almost_equal(np.linalg.norm(a), np.linalg.norm(b2))\n\n\ndef test_rotor_concatenation():\n rng = np.random.default_rng(85)\n for _ in range(5):\n a = rng.standard_normal(3)\n b = rng.standard_normal(3)\n c = rng.standard_normal(3)\n rotor_ab = pr.rotor_from_two_directions(a, b)\n rotor_bc = pr.rotor_from_two_directions(b, c)\n rotor_ac = pr.rotor_from_two_directions(a, c)\n rotor_ac_cat = pr.concatenate_rotors(rotor_bc, rotor_ab)\n assert_array_almost_equal(\n pr.rotor_apply(rotor_ac, a), pr.rotor_apply(rotor_ac_cat, a)\n )\n\n\ndef test_rotor_times_reverse():\n rng = np.random.default_rng(85)\n for _ in range(5):\n a = rng.standard_normal(3)\n b = rng.standard_normal(3)\n rotor = pr.rotor_from_two_directions(a, b)\n rotor_reverse = pr.rotor_reverse(rotor)\n result = pr.concatenate_rotors(rotor, rotor_reverse)\n assert_array_almost_equal(result, [1, 0, 0, 0])\n\n\ndef test_rotor_from_plane_angle():\n rng = np.random.default_rng(87)\n for _ in range(5):\n a = rng.standard_normal(3)\n b = rng.standard_normal(3)\n B = pr.wedge(a, b)\n angle = 2 * np.pi * rng.random()\n axis = np.cross(a, b)\n rotor = pr.rotor_from_plane_angle(B, angle)\n q = pr.quaternion_from_axis_angle(np.r_[axis, angle])\n v = rng.standard_normal(3)\n assert_array_almost_equal(\n pr.rotor_apply(rotor, v), pr.q_prod_vector(q, v)\n )\n\n\ndef test_matrix_from_rotor():\n rng = np.random.default_rng(88)\n for _ in range(5):\n a = rng.standard_normal(3)\n b = rng.standard_normal(3)\n B = pr.wedge(a, b)\n angle = 2 * np.pi * rng.random()\n axis = np.cross(a, b)\n rotor = pr.rotor_from_plane_angle(B, angle)\n R_rotor = pr.matrix_from_rotor(rotor)\n q = pr.quaternion_from_axis_angle(np.r_[axis, angle])\n R_q = pr.matrix_from_quaternion(q)\n assert_array_almost_equal(R_rotor, R_q)\n\n\ndef test_negative_rotor():\n rng = np.random.default_rng(89)\n for _ in range(5):\n a = rng.standard_normal(3)\n b = rng.standard_normal(3)\n rotor = pr.rotor_from_two_directions(a, b)\n neg_rotor = pr.norm_vector(-rotor)\n a2 = pr.rotor_apply(rotor, a)\n a3 = pr.rotor_apply(neg_rotor, a)\n assert_array_almost_equal(a2, a3)\n" }, { "path": "pytransform3d/rotations/test/test_slerp.py", "content": "import numpy as np\nimport pytest\nfrom numpy.testing import assert_array_almost_equal\n\nimport pytransform3d.rotations as pr\n\n\ndef test_q_R_slerps():\n \"\"\"Compare SLERP implementations for quaternions and rotation matrices.\"\"\"\n rng = np.random.default_rng(833234)\n for _ in range(20):\n q_start = pr.random_quaternion(rng)\n q_end = pr.random_quaternion(rng)\n R_start, R_end = (\n pr.matrix_from_quaternion(q_start),\n pr.matrix_from_quaternion(q_end),\n )\n t = rng.random()\n q_t = pr.quaternion_slerp(q_start, q_end, t, shortest_path=True)\n R_t = pr.matrix_slerp(R_start, R_end, t)\n assert_array_almost_equal(R_t, pr.matrix_from_quaternion(q_t))\n\n\ndef test_rotation_matrix_power():\n \"\"\"Test power of rotation matrices.\"\"\"\n R = pr.random_matrix(rng=np.random.default_rng(2844))\n angle = pr.axis_angle_from_matrix(R)[-1]\n\n R_m2 = pr.matrix_power(R, -2.0)\n assert_array_almost_equal(R_m2, R.T.dot(R.T))\n\n R_m1 = pr.matrix_power(R, -1.0)\n assert_array_almost_equal(R_m1, R.T)\n\n R_m05 = pr.matrix_power(R, -0.5)\n assert pytest.approx(angle) == 2 * pr.axis_angle_from_matrix(R_m05)[-1]\n\n R_0 = pr.matrix_power(R, 0.0)\n assert_array_almost_equal(R_0, np.eye(3))\n\n R_p05 = pr.matrix_power(R, 0.5)\n assert pytest.approx(angle) == 2 * pr.axis_angle_from_matrix(R_p05)[-1]\n\n R_p1 = pr.matrix_power(R, 1.0)\n assert_array_almost_equal(R_p1, R)\n\n R_p2 = pr.matrix_power(R, 2.0)\n assert_array_almost_equal(R_p2, R.dot(R))\n\n\ndef test_interpolate_axis_angle():\n \"\"\"Test interpolation between two axis-angle rotations with slerp.\"\"\"\n n_steps = 10\n rng = np.random.default_rng(1)\n a1 = pr.random_axis_angle(rng)\n a2 = pr.random_axis_angle(rng)\n\n traj = [pr.axis_angle_slerp(a1, a2, t) for t in np.linspace(0, 1, n_steps)]\n\n axis = pr.norm_vector(pr.perpendicular_to_vectors(a1[:3], a2[:3]))\n angle = pr.angle_between_vectors(a1[:3], a2[:3])\n traj2 = []\n for t in np.linspace(0, 1, n_steps):\n inta = np.hstack((axis, (t * angle,)))\n intaxis = pr.matrix_from_axis_angle(inta).dot(a1[:3])\n intangle = (1 - t) * a1[3] + t * a2[3]\n traj2.append(np.hstack((intaxis, (intangle,))))\n\n assert_array_almost_equal(traj, traj2)\n\n\ndef test_interpolate_same_axis_angle():\n \"\"\"Test interpolation between the same axis-angle rotation.\n\n See issue #45.\n \"\"\"\n n_steps = 3\n rng = np.random.default_rng(42)\n a = pr.random_axis_angle(rng)\n traj = [pr.axis_angle_slerp(a, a, t) for t in np.linspace(0, 1, n_steps)]\n assert len(traj) == n_steps\n assert_array_almost_equal(traj[0], a)\n assert_array_almost_equal(traj[1], a)\n assert_array_almost_equal(traj[2], a)\n\n\ndef test_interpolate_almost_same_axis_angle():\n \"\"\"Test interpolation between almost the same axis-angle rotation.\"\"\"\n n_steps = 3\n rng = np.random.default_rng(42)\n a1 = pr.random_axis_angle(rng)\n a2 = np.copy(a1)\n a2[-1] += np.finfo(\"float\").eps\n traj = [pr.axis_angle_slerp(a1, a2, t) for t in np.linspace(0, 1, n_steps)]\n assert len(traj) == n_steps\n assert_array_almost_equal(traj[0], a1)\n assert_array_almost_equal(traj[1], a1)\n assert_array_almost_equal(traj[2], a2)\n\n\ndef test_interpolate_quaternion():\n \"\"\"Test interpolation between two quaternions with slerp.\"\"\"\n n_steps = 10\n rng = np.random.default_rng(0)\n a1 = pr.random_axis_angle(rng)\n a2 = pr.random_axis_angle(rng)\n q1 = pr.quaternion_from_axis_angle(a1)\n q2 = pr.quaternion_from_axis_angle(a2)\n\n traj_q = [\n pr.quaternion_slerp(q1, q2, t) for t in np.linspace(0, 1, n_steps)\n ]\n traj_R = [pr.matrix_from_quaternion(q) for q in traj_q]\n R_diff = np.diff(traj_R, axis=0)\n R_diff_norms = [np.linalg.norm(Rd) for Rd in R_diff]\n assert_array_almost_equal(\n R_diff_norms, R_diff_norms[0] * np.ones(n_steps - 1)\n )\n\n\ndef test_interpolate_quaternion_shortest_path():\n \"\"\"Test SLERP between similar quternions with opposite sign.\"\"\"\n n_steps = 10\n rng = np.random.default_rng(2323)\n q1 = pr.random_quaternion(rng)\n a1 = pr.axis_angle_from_quaternion(q1)\n a2 = np.r_[a1[:3], a1[3] * 1.1]\n q2 = pr.quaternion_from_axis_angle(a2)\n\n if np.sign(q1[0]) != np.sign(q2[0]):\n q2 *= -1.0\n traj_q = [\n pr.quaternion_slerp(q1, q2, t) for t in np.linspace(0, 1, n_steps)\n ]\n path_length = np.sum(\n [pr.quaternion_dist(r, s) for r, s in zip(traj_q[:-1], traj_q[1:])]\n )\n\n q2 *= -1.0\n traj_q_opposing = [\n pr.quaternion_slerp(q1, q2, t) for t in np.linspace(0, 1, n_steps)\n ]\n path_length_opposing = np.sum(\n [\n pr.quaternion_dist(r, s)\n for r, s in zip(traj_q_opposing[:-1], traj_q_opposing[1:])\n ]\n )\n\n assert path_length_opposing > path_length\n\n traj_q_opposing_corrected = [\n pr.quaternion_slerp(q1, q2, t, shortest_path=True)\n for t in np.linspace(0, 1, n_steps)\n ]\n path_length_opposing_corrected = np.sum(\n [\n pr.quaternion_dist(r, s)\n for r, s in zip(\n traj_q_opposing_corrected[:-1], traj_q_opposing_corrected[1:]\n )\n ]\n )\n\n assert pytest.approx(path_length_opposing_corrected) == path_length\n\n\ndef test_interpolate_same_quaternion():\n \"\"\"Test interpolation between the same quaternion rotation.\n\n See issue #45.\n \"\"\"\n n_steps = 3\n rng = np.random.default_rng(42)\n a = pr.random_axis_angle(rng)\n q = pr.quaternion_from_axis_angle(a)\n traj = [pr.quaternion_slerp(q, q, t) for t in np.linspace(0, 1, n_steps)]\n assert len(traj) == n_steps\n assert_array_almost_equal(traj[0], q)\n assert_array_almost_equal(traj[1], q)\n assert_array_almost_equal(traj[2], q)\n\n\ndef test_interpolate_shortest_path_same_quaternion():\n \"\"\"Test interpolate along shortest path with same quaternion.\"\"\"\n rng = np.random.default_rng(8353)\n q = pr.random_quaternion(rng)\n q_interpolated = pr.quaternion_slerp(q, q, 0.5, shortest_path=True)\n assert_array_almost_equal(q, q_interpolated)\n\n q = np.array([0.0, 1.0, 0.0, 0.0])\n q_interpolated = pr.quaternion_slerp(q, -q, 0.5, shortest_path=True)\n assert_array_almost_equal(q, q_interpolated)\n\n q = np.array([0.0, 0.0, 1.0, 0.0])\n q_interpolated = pr.quaternion_slerp(q, -q, 0.5, shortest_path=True)\n assert_array_almost_equal(q, q_interpolated)\n\n q = np.array([0.0, 0.0, 0.0, 1.0])\n q_interpolated = pr.quaternion_slerp(q, -q, 0.5, shortest_path=True)\n assert_array_almost_equal(q, q_interpolated)\n\n\ndef test_rotor_slerp():\n rng = np.random.default_rng(86)\n for _ in range(5):\n a_unit = pr.norm_vector(rng.standard_normal(3))\n b_unit = pr.norm_vector(rng.standard_normal(3))\n rotor1 = pr.rotor_from_two_directions(a_unit, b_unit)\n\n axis = pr.norm_vector(np.cross(a_unit, b_unit))\n angle = pr.angle_between_vectors(a_unit, b_unit)\n q1 = pr.quaternion_from_axis_angle(np.r_[axis, angle])\n\n c_unit = pr.norm_vector(rng.standard_normal(3))\n d_unit = pr.norm_vector(rng.standard_normal(3))\n rotor2 = pr.rotor_from_two_directions(c_unit, d_unit)\n\n axis = pr.norm_vector(np.cross(c_unit, d_unit))\n angle = pr.angle_between_vectors(c_unit, d_unit)\n q2 = pr.quaternion_from_axis_angle(np.r_[axis, angle])\n\n rotor_025 = pr.rotor_slerp(rotor1, rotor2, 0.25)\n q_025 = pr.quaternion_slerp(q1, q2, 0.25)\n\n e = rng.standard_normal(3)\n assert_array_almost_equal(\n pr.rotor_apply(rotor_025, e), pr.q_prod_vector(q_025, e)\n )\n\n rotor_075 = pr.rotor_slerp(rotor1, rotor2, 0.25)\n q_075 = pr.quaternion_slerp(q1, q2, 0.25)\n\n e = rng.standard_normal(3)\n assert_array_almost_equal(\n pr.rotor_apply(rotor_075, e), pr.q_prod_vector(q_075, e)\n )\n" }, { "path": "pytransform3d/rotations/test/test_utils.py", "content": "import warnings\n\nimport numpy as np\nimport pytest\nfrom numpy.testing import assert_array_almost_equal\n\nimport pytransform3d.rotations as pr\n\n\ndef test_norm_vector():\n \"\"\"Test normalization of vectors.\"\"\"\n rng = np.random.default_rng(0)\n for n in range(1, 6):\n v = pr.random_vector(rng, n)\n u = pr.norm_vector(v)\n assert pytest.approx(np.linalg.norm(u)) == 1\n\n\ndef test_norm_zero_vector():\n \"\"\"Test normalization of zero vector.\"\"\"\n normalized = pr.norm_vector(np.zeros(3))\n assert np.isfinite(np.linalg.norm(normalized))\n\n\ndef test_perpendicular_to_vectors():\n \"\"\"Test function to compute perpendicular to vectors.\"\"\"\n rng = np.random.default_rng(0)\n a = pr.norm_vector(pr.random_vector(rng))\n a1 = pr.norm_vector(pr.random_vector(rng))\n b = pr.norm_vector(pr.perpendicular_to_vectors(a, a1))\n c = pr.norm_vector(pr.perpendicular_to_vectors(a, b))\n assert pytest.approx(pr.angle_between_vectors(a, b)) == np.pi / 2.0\n assert pytest.approx(pr.angle_between_vectors(a, c)) == np.pi / 2.0\n assert pytest.approx(pr.angle_between_vectors(b, c)) == np.pi / 2.0\n assert_array_almost_equal(pr.perpendicular_to_vectors(b, c), a)\n assert_array_almost_equal(pr.perpendicular_to_vectors(c, a), b)\n\n\ndef test_perpendicular_to_vector():\n \"\"\"Test function to compute perpendicular to vector.\"\"\"\n assert (\n pytest.approx(\n pr.angle_between_vectors(\n pr.unitx, pr.perpendicular_to_vector(pr.unitx)\n )\n )\n == np.pi / 2.0\n )\n assert (\n pytest.approx(\n pr.angle_between_vectors(\n pr.unity, pr.perpendicular_to_vector(pr.unity)\n )\n )\n == np.pi / 2.0\n )\n assert (\n pytest.approx(\n pr.angle_between_vectors(\n pr.unitz, pr.perpendicular_to_vector(pr.unitz)\n )\n )\n == np.pi / 2.0\n )\n rng = np.random.default_rng(0)\n for _ in range(5):\n a = pr.norm_vector(pr.random_vector(rng))\n assert (\n pytest.approx(\n pr.angle_between_vectors(a, pr.perpendicular_to_vector(a))\n )\n == np.pi / 2.0\n )\n b = a - np.array([a[0], 0.0, 0.0])\n assert (\n pytest.approx(\n pr.angle_between_vectors(b, pr.perpendicular_to_vector(b))\n )\n == np.pi / 2.0\n )\n c = a - np.array([0.0, a[1], 0.0])\n assert (\n pytest.approx(\n pr.angle_between_vectors(c, pr.perpendicular_to_vector(c))\n )\n == np.pi / 2.0\n )\n d = a - np.array([0.0, 0.0, a[2]])\n assert (\n pytest.approx(\n pr.angle_between_vectors(d, pr.perpendicular_to_vector(d))\n )\n == np.pi / 2.0\n )\n\n\ndef test_angle_between_vectors():\n \"\"\"Test function to compute angle between two vectors.\"\"\"\n v = np.array([1, 0, 0])\n a = np.array([0, 1, 0, np.pi / 2])\n R = pr.matrix_from_axis_angle(a)\n vR = np.dot(R, v)\n assert pytest.approx(pr.angle_between_vectors(vR, v)) == a[-1]\n v = np.array([0, 1, 0])\n a = np.array([1, 0, 0, np.pi / 2])\n R = pr.matrix_from_axis_angle(a)\n vR = np.dot(R, v)\n assert pytest.approx(pr.angle_between_vectors(vR, v)) == a[-1]\n v = np.array([0, 0, 1])\n a = np.array([1, 0, 0, np.pi / 2])\n R = pr.matrix_from_axis_angle(a)\n vR = np.dot(R, v)\n assert pytest.approx(pr.angle_between_vectors(vR, v)) == a[-1]\n\n\ndef test_angle_between_close_vectors():\n \"\"\"Test angle between close vectors.\n\n See issue #47.\n \"\"\"\n a = np.array([0.9689124217106448, 0.24740395925452294, 0.0, 0.0])\n b = np.array([0.9689124217106448, 0.247403959254523, 0.0, 0.0])\n angle = pr.angle_between_vectors(a, b)\n assert pytest.approx(angle) == 0.0\n\n\ndef test_angle_to_zero_vector_is_nan():\n \"\"\"Test angle to zero vector.\"\"\"\n a = np.array([1.0, 0.0])\n b = np.array([0.0, 0.0])\n with warnings.catch_warnings(record=True) as w:\n angle = pr.angle_between_vectors(a, b)\n assert len(w) == 1\n assert np.isnan(angle)\n\n\ndef test_vector_projection_on_zero_vector():\n \"\"\"Test projection on zero vector.\"\"\"\n rng = np.random.default_rng(23)\n for _ in range(5):\n a = pr.random_vector(rng, 3)\n a_on_b = pr.vector_projection(a, np.zeros(3))\n assert_array_almost_equal(a_on_b, np.zeros(3))\n\n\ndef test_vector_projection():\n \"\"\"Test orthogonal projection of one vector to another vector.\"\"\"\n a = np.ones(3)\n a_on_unitx = pr.vector_projection(a, pr.unitx)\n assert_array_almost_equal(a_on_unitx, pr.unitx)\n assert pytest.approx(pr.angle_between_vectors(a_on_unitx, pr.unitx)) == 0.0\n\n a2_on_unitx = pr.vector_projection(2 * a, pr.unitx)\n assert_array_almost_equal(a2_on_unitx, 2 * pr.unitx)\n assert pytest.approx(pr.angle_between_vectors(a2_on_unitx, pr.unitx)) == 0.0\n\n a_on_unity = pr.vector_projection(a, pr.unity)\n assert_array_almost_equal(a_on_unity, pr.unity)\n assert pytest.approx(pr.angle_between_vectors(a_on_unity, pr.unity)) == 0.0\n\n minus_a_on_unity = pr.vector_projection(-a, pr.unity)\n assert_array_almost_equal(minus_a_on_unity, -pr.unity)\n assert (\n pytest.approx(pr.angle_between_vectors(minus_a_on_unity, pr.unity))\n == np.pi\n )\n\n a_on_unitz = pr.vector_projection(a, pr.unitz)\n assert_array_almost_equal(a_on_unitz, pr.unitz)\n assert pytest.approx(pr.angle_between_vectors(a_on_unitz, pr.unitz)) == 0.0\n\n unitz_on_a = pr.vector_projection(pr.unitz, a)\n assert_array_almost_equal(unitz_on_a, np.ones(3) / 3.0)\n assert pytest.approx(pr.angle_between_vectors(unitz_on_a, a)) == 0.0\n\n unitx_on_unitx = pr.vector_projection(pr.unitx, pr.unitx)\n assert_array_almost_equal(unitx_on_unitx, pr.unitx)\n assert (\n pytest.approx(pr.angle_between_vectors(unitx_on_unitx, pr.unitx)) == 0.0\n )\n\n\ndef test_plane_basis_from_normal():\n x, y = pr.plane_basis_from_normal(pr.unitx)\n R = np.column_stack((x, y, pr.unitx))\n pr.assert_rotation_matrix(R)\n\n x, y = pr.plane_basis_from_normal(pr.unity)\n R = np.column_stack((x, y, pr.unity))\n pr.assert_rotation_matrix(R)\n\n x, y = pr.plane_basis_from_normal(pr.unitz)\n R = np.column_stack((x, y, pr.unitz))\n pr.assert_rotation_matrix(R)\n\n rng = np.random.default_rng(25)\n for _ in range(5):\n normal = pr.norm_vector(rng.standard_normal(3))\n x, y = pr.plane_basis_from_normal(normal)\n R = np.column_stack((x, y, normal))\n pr.assert_rotation_matrix(R)\n" }, { "path": "pytransform3d/test/test_camera.py", "content": "import numpy as np\nimport pytest\nfrom numpy.testing import assert_array_almost_equal\n\nfrom pytransform3d.camera import (\n make_world_line,\n make_world_grid,\n cam2sensor,\n sensor2img,\n world2image,\n)\nfrom pytransform3d.rotations import active_matrix_from_intrinsic_euler_xyz\nfrom pytransform3d.transformations import transform_from\n\n\ndef test_make_world_line():\n line = make_world_line(np.array([0, 0, 0]), np.array([1, 1, 1]), 11)\n assert len(line) == 11\n assert np.array([0, 0, 0, 1]) in line\n assert np.array([0.5, 0.5, 0.5, 1]) in line\n assert np.array([1, 1, 1, 1]) in line\n\n\ndef test_make_world_grid():\n grid = make_world_grid(3, 3, xlim=(-1, 1), ylim=(-1, 1))\n assert np.array([-1, -1, 0, 1]) in grid\n assert np.array([-1, 0, 0, 1]) in grid\n assert np.array([-1, 1, 0, 1]) in grid\n assert np.array([0, -1, 0, 1]) in grid\n assert np.array([0, 0, 0, 1]) in grid\n assert np.array([0, 1, 0, 1]) in grid\n assert np.array([1, -1, 0, 1]) in grid\n assert np.array([1, 0, 0, 1]) in grid\n assert np.array([1, 1, 0, 1]) in grid\n\n\ndef test_cam2sensor_wrong_dimensions():\n P_cam = np.ones((1, 2))\n with pytest.raises(ValueError, match=\"3- or 4-dimensional points\"):\n cam2sensor(P_cam, 0.1)\n P_cam = np.ones((1, 5))\n with pytest.raises(ValueError, match=\"3- or 4-dimensional points\"):\n cam2sensor(P_cam, 0.1)\n\n\ndef test_cam2sensor_wrong_focal_length():\n P_cam = np.ones((1, 3))\n with pytest.raises(ValueError, match=\"must be greater than 0\"):\n cam2sensor(P_cam, 0.0)\n\n\ndef test_cam2sensor_points_behind_camera():\n P_cam = np.ones((1, 3))\n P_cam[0, 2] = -1.0\n P_sensor = cam2sensor(P_cam, 0.1)\n assert not np.any(np.isfinite(P_sensor))\n\n\ndef test_cam2sensor_projection():\n P_cam = np.array([[-0.56776587, 0.03855521, 0.81618344, 1.0]])\n P_sensor = cam2sensor(P_cam, 0.0036)\n assert_array_almost_equal(P_sensor, np.array([[-0.00250429, 0.00017006]]))\n\n\ndef test_sensor2img():\n P_sensor = np.array(\n [[-0.00367 / 2, -0.00274 / 2], [0.0, 0.0], [0.00367 / 2, 0.00274 / 2]]\n )\n P_image = sensor2img(P_sensor, (0.00367, 0.00274), (640, 480))\n assert_array_almost_equal(\n P_image, np.array([[0, 0], [320, 240], [640, 480]])\n )\n\n P_sensor = np.array([[0.0, 0.0], [0.00367, 0.00274]])\n P_image = sensor2img(P_sensor, (0.00367, 0.00274), (640, 480), (0, 0))\n assert_array_almost_equal(P_image, np.array([[0, 0], [640, 480]]))\n\n\ndef test_world2image():\n cam2world = transform_from(\n active_matrix_from_intrinsic_euler_xyz([np.pi, 0, 0]), [0, 0, 1.5]\n )\n focal_length = 0.0036\n sensor_size = (0.00367, 0.00274)\n image_size = (640, 480)\n\n world_grid = make_world_grid()\n image_grid = world2image(\n world_grid, cam2world, sensor_size, image_size, focal_length\n )\n expected_grid = make_world_grid(\n xlim=(110.73569482, 529.26430518), ylim=(450.2189781, 29.7810219)\n )\n assert_array_almost_equal(image_grid, expected_grid[:, :2])\n" }, { "path": "pytransform3d/test/test_coordinates.py", "content": "import numpy as np\nfrom numpy.testing import assert_array_almost_equal\n\nimport pytransform3d.coordinates as pc\n\n\ndef test_cylindrical_from_cartesian_edge_cases():\n q = pc.cylindrical_from_cartesian(np.zeros(3))\n assert_array_almost_equal(q, np.zeros(3))\n\n\ndef test_cartesian_from_cylindrical_edge_cases():\n q = pc.cartesian_from_cylindrical(np.zeros(3))\n assert_array_almost_equal(q, np.zeros(3))\n\n\ndef test_convert_cartesian_cylindrical():\n rng = np.random.default_rng(0)\n for _ in range(1000):\n rho = rng.standard_normal()\n phi = rng.random() * 4 * np.pi - 2 * np.pi\n z = rng.standard_normal()\n p = np.array([rho, phi, z])\n q = pc.cartesian_from_cylindrical(p)\n r = pc.cylindrical_from_cartesian(q)\n assert r[0] >= 0\n assert -np.pi <= r[1] <= np.pi\n s = pc.cartesian_from_cylindrical(r)\n assert_array_almost_equal(q, s)\n\n\ndef test_spherical_from_cartesian_edge_cases():\n q = pc.spherical_from_cartesian(np.zeros(3))\n assert_array_almost_equal(q, np.zeros(3))\n\n\ndef test_cartesian_from_spherical_edge_cases():\n q = pc.cartesian_from_spherical(np.zeros(3))\n assert_array_almost_equal(q, np.zeros(3))\n\n\ndef test_convert_cartesian_spherical():\n rng = np.random.default_rng(1)\n for _ in range(1000):\n rho = rng.standard_normal()\n theta = rng.random() * 4 * np.pi - 2 * np.pi\n phi = rng.random() * 4 * np.pi - 2 * np.pi\n p = np.array([rho, theta, phi])\n q = pc.cartesian_from_spherical(p)\n r = pc.spherical_from_cartesian(q)\n assert r[0] >= 0\n assert 0 <= r[1] <= np.pi\n assert -np.pi <= r[2] <= np.pi\n s = pc.cartesian_from_spherical(r)\n assert_array_almost_equal(q, s)\n\n\ndef test_spherical_from_cylindrical_edge_cases():\n q = pc.spherical_from_cylindrical(np.zeros(3))\n assert_array_almost_equal(q, np.zeros(3))\n\n\ndef test_cylindrical_from_spherical_edge_cases():\n q = pc.cylindrical_from_spherical(np.zeros(3))\n assert_array_almost_equal(q, np.zeros(3))\n\n\ndef test_convert_cylindrical_spherical():\n rng = np.random.default_rng(2)\n for _ in range(1000):\n rho = rng.standard_normal()\n theta = rng.random() * 4 * np.pi - 2 * np.pi\n phi = rng.random() * 4 * np.pi - 2 * np.pi\n p = np.array([rho, theta, phi])\n q = pc.cylindrical_from_spherical(p)\n r = pc.spherical_from_cylindrical(q)\n s = pc.cylindrical_from_spherical(r)\n assert_array_almost_equal(q, s)\n\n\ndef test_integer_inputs():\n assert_array_almost_equal(\n pc.spherical_from_cylindrical([1, 0, 0]),\n pc.spherical_from_cylindrical([1.0, 0.0, 0.0]),\n )\n assert_array_almost_equal(\n pc.spherical_from_cartesian([1, 0, 0]),\n pc.spherical_from_cartesian([1.0, 0.0, 0.0]),\n )\n assert_array_almost_equal(\n pc.cylindrical_from_cartesian([0, 1, 0]),\n pc.cylindrical_from_cartesian([0.0, 1.0, 0.0]),\n )\n assert_array_almost_equal(\n pc.cartesian_from_cylindrical([1, 1, 0]),\n pc.cartesian_from_cylindrical([1.0, 1.0, 0.0]),\n )\n assert_array_almost_equal(\n pc.cartesian_from_spherical([1, 1, 0]),\n pc.cartesian_from_spherical([1.0, 1.0, 0.0]),\n )\n assert_array_almost_equal(\n pc.cylindrical_from_spherical([1, 1, 0]),\n pc.cylindrical_from_spherical([1.0, 1.0, 0.0]),\n )\n" }, { "path": "pytransform3d/test/test_geometry.py", "content": "import numpy as np\nfrom numpy.testing import assert_array_equal, assert_array_almost_equal\n\nimport pytransform3d._geometry as pg\nimport pytransform3d.transformations as pt\n\n\ndef test_unit_sphere():\n x, y, z = pg.unit_sphere_surface_grid(10)\n assert_array_equal(x.shape, (10, 10))\n assert_array_equal(y.shape, (10, 10))\n assert_array_equal(z.shape, (10, 10))\n\n P = np.column_stack((x.reshape(-1), y.reshape(-1), z.reshape(-1)))\n norms = np.linalg.norm(P, axis=1)\n assert_array_almost_equal(norms, np.ones_like(norms))\n\n\ndef test_transform_surface():\n x, y, z = pg.unit_sphere_surface_grid(10)\n\n p = np.array([0.2, -0.5, 0.7])\n pose = pt.transform_from(R=np.eye(3), p=p)\n x, y, z = pg.transform_surface(pose, x, y, z)\n\n P = np.column_stack((x.reshape(-1), y.reshape(-1), z.reshape(-1)))\n norms = np.linalg.norm(P - p[np.newaxis], axis=1)\n assert_array_almost_equal(norms, np.ones_like(norms))\n" }, { "path": "pytransform3d/test/test_mesh_loader.py", "content": "import numpy as np\n\nfrom pytransform3d import _mesh_loader\n\nimport pytest\n\n\ndef test_trimesh():\n mesh = _mesh_loader._Trimesh(\"test/test_data/cone.stl\")\n loader_available = mesh.load()\n if not loader_available:\n pytest.skip(\"trimesh is required for this test\")\n\n assert len(mesh.vertices) == 64\n assert len(mesh.triangles) == 124\n\n mesh.convex_hull()\n\n assert len(mesh.vertices) == 64\n\n\ndef test_trimesh_scene():\n mesh = _mesh_loader._Trimesh(\"test/test_data/scene.obj\")\n try:\n mesh_loaded = mesh.load()\n except ImportError as e:\n if e.name == \"open3d\":\n pytest.skip(\"open3d is required for this test\")\n else:\n raise e\n\n if not mesh_loaded:\n pytest.skip(\"trimesh is required for this test\")\n\n assert mesh_loaded\n assert len(mesh.vertices) == 16\n assert len(mesh.triangles) == 24\n\n mesh.convex_hull()\n\n assert len(mesh.vertices) == 8\n\n\ndef test_open3d():\n mesh = _mesh_loader._Open3DMesh(\"test/test_data/cone.stl\")\n loader_available = mesh.load()\n if not loader_available:\n pytest.skip(\"open3d is required for this test\")\n\n assert len(mesh.vertices) == 295\n assert len(mesh.triangles) == 124\n\n o3d_mesh = mesh.get_open3d_mesh()\n assert len(o3d_mesh.vertices) == 295\n\n mesh.convex_hull()\n\n assert len(mesh.vertices) == 64\n\n\ndef test_trimesh_with_open3d():\n mesh = _mesh_loader._Trimesh(\"test/test_data/cone.stl\")\n loader_available = mesh.load()\n if not loader_available:\n pytest.skip(\"trimesh is required for this test\")\n try:\n o3d_mesh = mesh.get_open3d_mesh()\n except ImportError:\n pytest.skip(\"open3d is required for this test\")\n assert len(o3d_mesh.vertices) == 64\n\n\ndef test_interface():\n try:\n mesh = _mesh_loader.load_mesh(\"test/test_data/cone.stl\")\n assert len(mesh.triangles) == 124\n except ImportError as e:\n if e.name in [\"open3d\", \"trimesh\"]:\n pytest.skip(\"trimesh or open3d are required for this test\")\n else:\n raise e\n\n\ndef test_ply_with_color():\n try:\n mesh = _mesh_loader.load_mesh(\"test/test_data/frame.ply\")\n vertex_colors = np.asarray(mesh.get_open3d_mesh().vertex_colors)\n except ImportError as e:\n if e.name in [\"open3d\", \"trimesh\"]:\n pytest.skip(\"trimesh and open3d are required for this test\")\n else:\n raise e\n assert len(vertex_colors) == 1134\n\n\ndef test_interface_with_scene():\n try:\n mesh = _mesh_loader.load_mesh(\"test/test_data/scene.obj\")\n assert len(mesh.triangles) == 24\n except ImportError as e:\n if e.name in [\"open3d\", \"trimesh\"]:\n pytest.skip(\"trimesh and open3d are required for this test\")\n else:\n raise e\n" }, { "path": "pytransform3d/test/test_urdf.py", "content": "try:\n import matplotlib # noqa: F401\n\n matplotlib_available = True\nexcept ImportError:\n matplotlib_available = False\nimport json\nimport os\nimport warnings\n\nimport numpy as np\nimport pytest\nfrom numpy.testing import assert_array_almost_equal\n\nfrom pytransform3d.transformations import transform_from\nfrom pytransform3d.urdf import (\n UrdfTransformManager,\n UrdfException,\n parse_urdf,\n initialize_urdf_transform_manager,\n)\n\nCOMPI_URDF = \"\"\"\n\n\n\n\n\n\n\n\n\n\n\n\n \n \n \n \n \n\n\n\n \n \n \n \n \n\n\n\n \n \n \n \n \n\n\n\n \n \n \n \n\n\n\n \n \n \n \n\n\n\n \n \n \n \n\n\n\n \n \n \n\n\n\n transmission_interface/SimpleTransmission\n \n PositionJointInterface\n \n \n 1\n \n\n\n \"\"\"\n\n\ndef test_empty():\n with pytest.raises(UrdfException):\n UrdfTransformManager().load_urdf(\"\")\n\n\ndef test_missing_robot_tag():\n with pytest.raises(UrdfException):\n UrdfTransformManager().load_urdf(\"\")\n\n\ndef test_missing_robot_name():\n with pytest.raises(UrdfException):\n UrdfTransformManager().load_urdf(\"\")\n\n\ndef test_missing_link_name():\n with pytest.raises(UrdfException):\n UrdfTransformManager().load_urdf(\n ''\n )\n\n\ndef test_missing_joint_name():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \"\"\"\n with pytest.raises(UrdfException):\n UrdfTransformManager().load_urdf(urdf)\n\n\ndef test_missing_parent():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \"\"\"\n with pytest.raises(UrdfException):\n UrdfTransformManager().load_urdf(urdf)\n\n\ndef test_missing_child():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \"\"\"\n with pytest.raises(UrdfException):\n UrdfTransformManager().load_urdf(urdf)\n\n\ndef test_missing_parent_link_name():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \"\"\"\n with pytest.raises(UrdfException):\n UrdfTransformManager().load_urdf(urdf)\n\n\ndef test_missing_child_link_name():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \"\"\"\n with pytest.raises(UrdfException):\n UrdfTransformManager().load_urdf(urdf)\n\n\ndef test_xml_namespace():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \"\"\"\n\n robot_name, links, joints = parse_urdf(urdf)\n\n assert robot_name == \"robot_name\"\n assert len(links) == 2\n assert len(joints) == 1\n\n for link in links:\n assert link.name in [\"link0\", \"link1\"]\n\n assert joints[0].joint_name == \"joint0\"\n\n\ndef test_reference_to_unknown_child():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \"\"\"\n with pytest.raises(UrdfException):\n UrdfTransformManager().load_urdf(urdf)\n\n\ndef test_reference_to_unknown_parent():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \"\"\"\n with pytest.raises(UrdfException):\n UrdfTransformManager().load_urdf(urdf)\n\n\ndef test_missing_joint_type():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \"\"\"\n with pytest.raises(UrdfException):\n UrdfTransformManager().load_urdf(urdf)\n\n\ndef test_without_origin():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \"\"\"\n tm = UrdfTransformManager()\n tm.load_urdf(urdf)\n link1_to_link0 = tm.get_transform(\"link1\", \"link0\")\n assert_array_almost_equal(link1_to_link0, np.eye(4))\n\n\ndef test_with_empty_origin():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \"\"\"\n tm = UrdfTransformManager()\n tm.load_urdf(urdf)\n link1_to_link0 = tm.get_transform(\"link1\", \"link0\")\n assert_array_almost_equal(link1_to_link0, np.eye(4))\n\n\ndef test_unsupported_joint_type():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \"\"\"\n with pytest.raises(UrdfException):\n UrdfTransformManager().load_urdf(urdf)\n\n\ndef test_unknown_joint_type():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \"\"\"\n with pytest.raises(UrdfException):\n UrdfTransformManager().load_urdf(urdf)\n\n\ndef test_with_empty_axis():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \"\"\"\n tm = UrdfTransformManager()\n tm.load_urdf(urdf)\n tm.set_joint(\"joint0\", np.pi)\n link1_to_link0 = tm.get_transform(\"link1\", \"link0\")\n assert_array_almost_equal(\n link1_to_link0,\n np.array([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]),\n )\n\n\ndef test_ee_frame():\n tm = UrdfTransformManager()\n tm.load_urdf(COMPI_URDF)\n link7_to_linkmount = tm.get_transform(\"link6\", \"linkmount\")\n assert_array_almost_equal(\n link7_to_linkmount,\n np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1.124], [0, 0, 0, 1]]),\n )\n\n\ndef test_joint_angles():\n tm = UrdfTransformManager()\n tm.load_urdf(COMPI_URDF)\n for i in range(1, 7):\n tm.set_joint(\"joint%d\" % i, 0.1 * i)\n link7_to_linkmount = tm.get_transform(\"link6\", \"linkmount\")\n assert_array_almost_equal(\n link7_to_linkmount,\n np.array(\n [\n [0.121698, -0.606672, 0.785582, 0.489351],\n [0.818364, 0.509198, 0.266455, 0.114021],\n [-0.561668, 0.610465, 0.558446, 0.924019],\n [0.0, 0.0, 0.0, 1.0],\n ]\n ),\n )\n\n\ndef test_joint_limits():\n tm = UrdfTransformManager()\n tm.load_urdf(COMPI_URDF)\n\n with pytest.raises(KeyError):\n tm.get_joint_limits(\"unknown_joint\")\n assert_array_almost_equal(tm.get_joint_limits(\"joint1\"), (-1, 1))\n assert_array_almost_equal(tm.get_joint_limits(\"joint2\"), (-1, np.inf))\n assert_array_almost_equal(tm.get_joint_limits(\"joint3\"), (-np.inf, 1))\n\n\ndef test_joint_limit_clipping():\n tm = UrdfTransformManager()\n tm.load_urdf(COMPI_URDF)\n\n tm.set_joint(\"joint1\", 2.0)\n link7_to_linkmount = tm.get_transform(\"link6\", \"linkmount\")\n assert_array_almost_equal(\n link7_to_linkmount,\n np.array(\n [\n [0.5403023, -0.8414710, 0, 0],\n [0.8414710, 0.5403023, 0, 0],\n [0, 0, 1, 1.124],\n [0, 0, 0, 1],\n ]\n ),\n )\n\n tm.set_joint(\"joint1\", -2.0)\n link7_to_linkmount = tm.get_transform(\"link6\", \"linkmount\")\n assert_array_almost_equal(\n link7_to_linkmount,\n np.array(\n [\n [0.5403023, 0.8414710, 0, 0],\n [-0.8414710, 0.5403023, 0, 0],\n [0, 0, 1, 1.124],\n [0, 0, 0, 1],\n ]\n ),\n )\n\n\ndef test_fixed_joint():\n tm = UrdfTransformManager()\n tm.load_urdf(COMPI_URDF)\n tcp_to_link0 = tm.get_transform(\"tcp\", \"linkmount\")\n assert_array_almost_equal(\n tcp_to_link0,\n np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1.174], [0, 0, 0, 1]]),\n )\n\n\ndef test_fixed_joint_unchanged_and_warning():\n tm = UrdfTransformManager()\n tm.load_urdf(COMPI_URDF)\n tcp_to_link0_before = tm.get_transform(\"tcp\", \"linkmount\")\n with warnings.catch_warnings(record=True) as w:\n tm.set_joint(\"jointtcp\", 2.0)\n assert len(w) == 1\n tcp_to_link0_after = tm.get_transform(\"tcp\", \"linkmount\")\n assert_array_almost_equal(tcp_to_link0_before, tcp_to_link0_after)\n\n\ndef test_unknown_joint():\n tm = UrdfTransformManager()\n tm.load_urdf(COMPI_URDF)\n with pytest.raises(KeyError):\n tm.set_joint(\"unknown_joint\", 0)\n\n\ndef test_visual_without_geometry():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \n \n \n \"\"\"\n with pytest.raises(UrdfException):\n UrdfTransformManager().load_urdf(urdf)\n\n\ndef test_visual():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \"\"\"\n tm = UrdfTransformManager()\n tm.load_urdf(urdf)\n link1_to_link0 = tm.get_transform(\"link1\", \"link0\")\n expected_link1_to_link0 = transform_from(np.eye(3), np.array([0, 1, 0]))\n assert_array_almost_equal(link1_to_link0, expected_link1_to_link0)\n\n link1_to_link0 = tm.get_transform(\"visual:link1/link1_visual\", \"link0\")\n expected_link1_to_link0 = transform_from(np.eye(3), np.array([0, 1, 1]))\n assert_array_almost_equal(link1_to_link0, expected_link1_to_link0)\n\n\ndef test_collision():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \"\"\"\n tm = UrdfTransformManager()\n tm.load_urdf(urdf)\n\n link1_to_link0 = tm.get_transform(\"link1\", \"link0\")\n expected_link1_to_link0 = transform_from(np.eye(3), np.array([0, 0, 1]))\n assert_array_almost_equal(link1_to_link0, expected_link1_to_link0)\n\n link1_to_link0 = tm.get_transform(\"collision:link1/0\", \"link0\")\n expected_link1_to_link0 = transform_from(np.eye(3), np.array([0, 0, 2]))\n assert_array_almost_equal(link1_to_link0, expected_link1_to_link0)\n\n\ndef test_unique_frame_names():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \"\"\"\n tm = UrdfTransformManager()\n tm.load_urdf(urdf)\n\n assert \"robot_name\" in tm.nodes\n assert \"link0\" in tm.nodes\n assert \"link1\" in tm.nodes\n assert \"visual:link1/link1_geometry\" in tm.nodes\n assert \"collision:link1/link1_geometry\" in tm.nodes\n assert len(tm.nodes) == 5\n\n\ndef test_collision_box():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \"\"\"\n tm = UrdfTransformManager()\n tm.load_urdf(urdf)\n\n assert len(tm.collision_objects) == 1\n assert_array_almost_equal(tm.collision_objects[0].size, np.array([2, 3, 4]))\n\n\ndef test_collision_box_without_size():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \"\"\"\n tm = UrdfTransformManager()\n tm.load_urdf(urdf)\n\n assert len(tm.collision_objects) == 1\n assert_array_almost_equal(tm.collision_objects[0].size, np.zeros(3))\n\n\ndef test_collision_sphere():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \"\"\"\n tm = UrdfTransformManager()\n tm.load_urdf(urdf)\n\n assert len(tm.collision_objects) == 1\n assert tm.collision_objects[0].radius == 0.123\n\n\ndef test_collision_sphere_without_radius():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \"\"\"\n with pytest.raises(UrdfException):\n UrdfTransformManager().load_urdf(urdf)\n\n\ndef test_collision_cylinder():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \"\"\"\n tm = UrdfTransformManager()\n tm.load_urdf(urdf)\n\n assert len(tm.collision_objects) == 1\n assert tm.collision_objects[0].radius == 0.123\n assert tm.collision_objects[0].length == 1.234\n\n\ndef test_collision_cylinder_without_radius():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \"\"\"\n with pytest.raises(UrdfException):\n UrdfTransformManager().load_urdf(urdf)\n\n\ndef test_collision_cylinder_without_length():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \"\"\"\n with pytest.raises(UrdfException):\n UrdfTransformManager().load_urdf(urdf)\n\n\ndef test_multiple_collision_objects():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \"\"\"\n tm = UrdfTransformManager()\n tm.load_urdf(urdf)\n\n assert len(tm.collision_objects) == 2\n\n\ndef test_multiple_visuals():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \"\"\"\n tm = UrdfTransformManager()\n tm.load_urdf(urdf)\n\n assert len(tm.visuals) == 2\n\n\ndef test_multiple_parents():\n urdf = \"\"\"\n \n \n \n \n \n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \"\"\"\n tm = UrdfTransformManager()\n tm.load_urdf(urdf)\n\n p0c = tm.get_transform(\"parent0\", \"child\")\n p1c = tm.get_transform(\"parent1\", \"child\")\n assert p0c[0, 3] == p1c[1, 3]\n\n\ndef test_mesh_missing_filename():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n\n \n \"\"\"\n with pytest.raises(UrdfException):\n UrdfTransformManager().load_urdf(urdf, mesh_path=\"\")\n\n\ndef test_plot_mesh_smoke_without_scale():\n if not matplotlib_available:\n pytest.skip(\"matplotlib is required for this test\")\n\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n\n \n \"\"\"\n BASE_DIR = \"test/test_data/\"\n tm = UrdfTransformManager()\n tm.load_urdf(urdf, mesh_path=BASE_DIR)\n tm.set_joint(\"joint\", -1.1)\n ax = tm.plot_frames_in(\n \"lower_cone\",\n s=0.1,\n whitelist=[\"upper_cone\", \"lower_cone\"],\n show_name=True,\n )\n ax = tm.plot_connections_in(\"lower_cone\", ax=ax)\n tm.plot_visuals(\"lower_cone\", ax=ax)\n\n\ndef test_continuous_joint():\n urdf = \"\"\"\n \n \n \n \n\n \n \n \n \n \n \n \n \"\"\"\n tm = UrdfTransformManager()\n tm.load_urdf(urdf)\n tm.set_joint(\"joint\", 0.5 * np.pi)\n\n c2p = tm.get_transform(\"child\", \"parent\")\n assert_array_almost_equal(\n c2p, np.array([[1, 0, 0, 0], [0, 0, -1, 0], [0, 1, 0, 0], [0, 0, 0, 1]])\n )\n\n\ndef test_prismatic_joint():\n urdf = \"\"\"\n \n \n \n \n\n \n \n \n \n \n \n \n \"\"\"\n tm = UrdfTransformManager()\n tm.load_urdf(urdf)\n tm.set_joint(\"joint\", 5.33)\n\n c2p = tm.get_transform(\"child\", \"parent\")\n assert_array_almost_equal(\n c2p,\n np.array([[1, 0, 0, 5.33], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]),\n )\n\n\ndef test_plot_mesh_smoke_with_scale():\n if not matplotlib_available:\n pytest.skip(\"matplotlib is required for this test\")\n\n BASE_DIR = \"test/test_data/\"\n tm = UrdfTransformManager()\n with open(BASE_DIR + \"simple_mechanism.urdf\", \"r\") as f:\n tm.load_urdf(f.read(), mesh_path=BASE_DIR)\n tm.set_joint(\"joint\", -1.1)\n ax = tm.plot_frames_in(\n \"lower_cone\",\n s=0.1,\n whitelist=[\"upper_cone\", \"lower_cone\"],\n show_name=True,\n )\n ax = tm.plot_connections_in(\"lower_cone\", ax=ax)\n tm.plot_visuals(\"lower_cone\", ax=ax)\n\n\ndef test_plot_without_mesh():\n if not matplotlib_available:\n pytest.skip(\"matplotlib is required for this test\")\n\n BASE_DIR = \"test/test_data/\"\n tm = UrdfTransformManager()\n with open(BASE_DIR + \"simple_mechanism.urdf\", \"r\") as f:\n tm.load_urdf(f.read(), mesh_path=None)\n tm.set_joint(\"joint\", -1.1)\n ax = tm.plot_frames_in(\n \"lower_cone\",\n s=0.1,\n whitelist=[\"upper_cone\", \"lower_cone\"],\n show_name=True,\n )\n ax = tm.plot_connections_in(\"lower_cone\", ax=ax)\n\n with warnings.catch_warnings(record=True) as w:\n tm.plot_visuals(\"lower_cone\", ax=ax)\n assert len(w) == 1\n\n\ndef test_parse_material():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \"\"\"\n tm = UrdfTransformManager()\n tm.load_urdf(urdf)\n assert_array_almost_equal(tm.visuals[0].color, np.array([0, 0, 0, 1]))\n\n\ndef test_parse_material_without_name():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \n \n \"\"\"\n with pytest.raises(UrdfException):\n UrdfTransformManager().load_urdf(urdf)\n\n\ndef test_parse_material_without_color():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \n \n \"\"\"\n tm = UrdfTransformManager()\n tm.load_urdf(urdf)\n assert tm.visuals[0].color is None\n\n\ndef test_parse_material_without_rgba():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \"\"\"\n with pytest.raises(UrdfException):\n UrdfTransformManager().load_urdf(urdf)\n\n\ndef test_parse_material_with_two_colors():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \"\"\"\n with pytest.raises(UrdfException):\n UrdfTransformManager().load_urdf(urdf)\n\n\ndef test_parse_material_local():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \n \n \n \"\"\"\n tm = UrdfTransformManager()\n tm.load_urdf(urdf)\n assert_array_almost_equal(tm.visuals[0].color, np.array([1, 0, 0, 1]))\n\n\ndef test_plot_mesh_smoke_with_package_dir():\n if not matplotlib_available:\n pytest.skip(\"matplotlib is required for this test\")\n\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \n \"\"\"\n tm = UrdfTransformManager()\n tm.load_urdf(urdf, package_dir=\"./\")\n tm.plot_visuals(\"cone\")\n\n\ndef test_load_inertial_info():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \"\"\"\n _, links, _ = parse_urdf(urdf)\n assert len(links) == 1\n assert links[0].name == \"cone\"\n assert_array_almost_equal(links[0].inertial_frame, np.eye(4))\n assert links[0].mass == 0.001\n assert_array_almost_equal(\n links[0].inertia, np.array([[1, 4, 5], [4, 2, 6], [5, 6, 3]])\n )\n\n\ndef test_load_inertial_info_sparse_matrix():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \"\"\"\n _, links, _ = parse_urdf(urdf)\n assert len(links) == 1\n assert links[0].name == \"cone\"\n assert_array_almost_equal(links[0].inertial_frame, np.eye(4))\n assert links[0].mass == 0.001\n assert_array_almost_equal(links[0].inertia, np.zeros((3, 3)))\n\n\ndef test_load_inertial_info_diagonal_matrix():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \n \"\"\"\n _, links, _ = parse_urdf(urdf)\n assert len(links) == 1\n assert links[0].name == \"cone\"\n assert_array_almost_equal(links[0].inertial_frame, np.eye(4))\n assert links[0].mass == 0.001\n assert_array_almost_equal(links[0].inertia, np.eye(3))\n\n\ndef test_load_inertial_info_without_matrix():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \"\"\"\n _, links, _ = parse_urdf(urdf)\n assert len(links) == 1\n assert links[0].name == \"cone\"\n assert_array_almost_equal(links[0].inertial_frame, np.eye(4))\n assert links[0].mass == 0.001\n assert_array_almost_equal(links[0].inertia, np.zeros((3, 3)))\n\n\ndef test_load_inertial_info_without_mass():\n urdf = \"\"\"\n \n \n \n \n \n \n \n \n \n \"\"\"\n _, links, _ = parse_urdf(urdf)\n assert len(links) == 1\n assert links[0].name == \"cone\"\n assert_array_almost_equal(links[0].inertial_frame, np.eye(4))\n assert links[0].mass == 0.0\n assert_array_almost_equal(links[0].inertia, np.zeros((3, 3)))\n\n\ndef test_load_urdf_functional():\n robot_name, links, joints = parse_urdf(COMPI_URDF)\n assert robot_name == \"compi\"\n assert len(links) == 8\n assert len(joints) == 7\n\n tm = UrdfTransformManager()\n initialize_urdf_transform_manager(tm, robot_name, links, joints)\n\n link7_to_linkmount = tm.get_transform(\"link6\", \"linkmount\")\n assert_array_almost_equal(\n link7_to_linkmount,\n np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1.124], [0, 0, 0, 1]]),\n )\n\n\ndef test_serialization():\n tm = UrdfTransformManager()\n tm.load_urdf(COMPI_URDF)\n for i in range(1, 7):\n tm.set_joint(\"joint%d\" % i, 0.1)\n\n filename = \"compi.json\"\n try:\n tm_dict = tm.to_dict()\n with open(filename, \"w\") as f:\n json.dump(tm_dict, f)\n with open(filename, \"r\") as f:\n tm_dict2 = json.load(f)\n tm2 = UrdfTransformManager.from_dict(tm_dict2)\n\n tcp2mount = tm.get_transform(\"tcp\", \"linkmount\")\n tcp2mount2 = tm2.get_transform(\"tcp\", \"linkmount\")\n assert_array_almost_equal(tcp2mount, tcp2mount2)\n finally:\n if os.path.exists(filename):\n os.remove(filename)\n\n\ndef test_rotation_matrix_conversion():\n urdf = \"\"\"\n \n \n \n \n\n \n \n \n \n \n \n \n \"\"\"\n tm = UrdfTransformManager()\n tm.load_urdf(urdf)\n\n parent2child = tm.get_transform(\"parent\", \"child\")\n assert_array_almost_equal(\n parent2child,\n np.array(\n [\n [0.23859, 0.943669, 0.229266, -0.136378],\n [-0.868696, 0.101862, 0.48476, 0.550047],\n [0.4341, -0.314821, 0.844065, -1.819024],\n [0, 0, 0, 1],\n ]\n ),\n )\n\n\ndef test_xml_comment():\n urdf = \"\"\"\n\n\n\n\n\n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n transmission_interface/SimpleTransmission\n \n PositionJointInterface\n \n \n 1\n \n \n \n transmission_interface/SimpleTransmission\n \n PositionJointInterface\n \n \n 1\n \n \n \n transmission_interface/SimpleTransmission\n \n PositionJointInterface\n \n \n 1\n \n \n \n transmission_interface/SimpleTransmission\n \n PositionJointInterface\n \n \n 1\n \n \n \n transmission_interface/SimpleTransmission\n \n PositionJointInterface\n \n \n 1\n \n \n \n transmission_interface/SimpleTransmission\n \n PositionJointInterface\n \n \n 1\n \n \n \n \n \n \n \n \n\n\"\"\"\n (robot_name, links, _) = parse_urdf(urdf)\n\n assert robot_name == \"ur10\"\n assert len(links) == 9\n assert links[0].name == \"base_link\"\n" }, { "path": "pytransform3d/trajectories/__init__.py", "content": "\"\"\"Trajectories in three dimensions - SE(3).\n\nConversions from this module operate on batches of poses or transformations\nand can be 400 to 1000 times faster than a loop of individual conversions.\n\"\"\"\n\nfrom ._dual_quaternions import (\n batch_dq_conj,\n batch_dq_q_conj,\n batch_dq_prod_vector,\n batch_concatenate_dual_quaternions,\n dual_quaternions_power,\n dual_quaternions_sclerp,\n pqs_from_dual_quaternions,\n screw_parameters_from_dual_quaternions,\n transforms_from_dual_quaternions,\n)\nfrom ._plot import (\n plot_trajectory,\n)\nfrom ._pqs import (\n transforms_from_pqs,\n dual_quaternions_from_pqs,\n)\nfrom ._random import (\n random_trajectories,\n)\nfrom ._screws import (\n mirror_screw_axis_direction,\n transforms_from_exponential_coordinates,\n dual_quaternions_from_screw_parameters,\n)\nfrom ._transforms import (\n invert_transforms,\n concat_one_to_many,\n concat_many_to_one,\n concat_many_to_many,\n concat_dynamic,\n pqs_from_transforms,\n exponential_coordinates_from_transforms,\n dual_quaternions_from_transforms,\n)\n\n__all__ = [\n \"invert_transforms\",\n \"concat_one_to_many\",\n \"concat_many_to_one\",\n \"concat_many_to_many\",\n \"concat_dynamic\",\n \"transforms_from_pqs\",\n \"pqs_from_transforms\",\n \"exponential_coordinates_from_transforms\",\n \"transforms_from_exponential_coordinates\",\n \"dual_quaternions_from_pqs\",\n \"dual_quaternions_from_transforms\",\n \"pqs_from_dual_quaternions\",\n \"screw_parameters_from_dual_quaternions\",\n \"dual_quaternions_from_screw_parameters\",\n \"dual_quaternions_power\",\n \"dual_quaternions_sclerp\",\n \"transforms_from_dual_quaternions\",\n \"batch_concatenate_dual_quaternions\",\n \"batch_dq_conj\",\n \"batch_dq_q_conj\",\n \"batch_dq_prod_vector\",\n \"mirror_screw_axis_direction\",\n \"random_trajectories\",\n \"plot_trajectory\",\n]\n" }, { "path": "pytransform3d/trajectories/_dual_quaternions.py", "content": "\"\"\"Dual quaternion operations.\"\"\"\n\nimport numpy as np\n\nfrom ._screws import (\n dual_quaternions_from_screw_parameters,\n)\nfrom ..batch_rotations import (\n batch_concatenate_quaternions,\n batch_q_conj,\n axis_angles_from_quaternions,\n matrices_from_quaternions,\n)\n\n\ndef batch_dq_conj(dqs):\n \"\"\"Conjugate of dual quaternions.\n\n There are three different conjugates for dual quaternions. The one that we\n use here converts (pw, px, py, pz, qw, qx, qy, qz) to\n (pw, -px, -py, -pz, -qw, qx, qy, qz). It is a combination of the quaternion\n conjugate and the dual number conjugate.\n\n Parameters\n ----------\n dqs : array-like, shape (..., 8)\n Dual quaternions to represent transforms:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n Returns\n -------\n dq_conjugates : array-like, shape (..., 8)\n Conjugates of dual quaternions: (pw, -px, -py, -pz, -qw, qx, qy, qz)\n \"\"\"\n out = np.empty_like(dqs)\n out[..., 0] = dqs[..., 0]\n out[..., 1:5] = -dqs[..., 1:5]\n out[..., 5:] = dqs[..., 5:]\n return out\n\n\ndef batch_dq_q_conj(dqs):\n \"\"\"Quaternion conjugate of dual quaternions.\n\n For unit dual quaternions that represent transformations,\n this function is equivalent to the inverse of the\n corresponding transformation matrix.\n\n Parameters\n ----------\n dqs : array-like, shape (..., 8)\n Unit dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n Returns\n -------\n dq_q_conjugates : array, shape (..., 8)\n Conjugate of dual quaternion: (pw, -px, -py, -pz, qw, -qx, -qy, -qz)\n\n See Also\n --------\n pytransform3d.transformations.dq_q_conj\n Quaternion conjugate of dual quaternions.\n \"\"\"\n out = np.empty_like(dqs)\n out[..., 0] = dqs[..., 0]\n out[..., 1:4] = -dqs[..., 1:4]\n out[..., 4] = dqs[..., 4]\n out[..., 5:8] = -dqs[..., 5:8]\n return out\n\n\ndef batch_concatenate_dual_quaternions(dqs1, dqs2):\n \"\"\"Concatenate dual quaternions.\n\n Suppose we want to apply two extrinsic transforms given by dual\n quaternions dq1 and dq2 to a vector v. We can either apply dq2 to v and\n then dq1 to the result or we can concatenate dq1 and dq2 and apply the\n result to v.\n\n Parameters\n ----------\n dqs1 : array-like, shape (..., 8)\n Dual quaternions to represent transforms:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n dqs2 : array-like, shape (..., 8)\n Dual quaternions to represent transforms:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n Returns\n -------\n dqs3 : array, shape (8,)\n Products of the two batches of dual quaternions:\n (pw, px, py, pz, qw, qx, qy, qz)\n \"\"\"\n dqs1 = np.asarray(dqs1)\n dqs2 = np.asarray(dqs2)\n\n out = np.empty_like(dqs1)\n out[..., :4] = batch_concatenate_quaternions(dqs1[..., :4], dqs2[..., :4])\n out[..., 4:] = batch_concatenate_quaternions(\n dqs1[..., :4], dqs2[..., 4:]\n ) + batch_concatenate_quaternions(dqs1[..., 4:], dqs2[..., :4])\n return out\n\n\ndef batch_dq_prod_vector(dqs, V):\n \"\"\"Apply transforms represented by a dual quaternions to vectors.\n\n Parameters\n ----------\n dqs : array-like, shape (..., 8)\n Unit dual quaternions\n\n V : array-like, shape (..., 3)\n 3d vectors\n\n Returns\n -------\n W : array, shape (3,)\n 3d vectors\n \"\"\"\n dqs = np.asarray(dqs)\n\n v_dqs = np.empty_like(dqs)\n v_dqs[..., 0] = 1.0\n v_dqs[..., 1:5] = 0.0\n v_dqs[..., 5:] = V\n v_dq_transformed = batch_concatenate_dual_quaternions(\n batch_concatenate_dual_quaternions(dqs, v_dqs), batch_dq_conj(dqs)\n )\n return v_dq_transformed[..., 5:]\n\n\ndef dual_quaternions_power(dqs, ts):\n \"\"\"Compute power of unit dual quaternions with respect to scalar.\n\n Parameters\n ----------\n dqs : array-like, shape (..., 8)\n Unit dual quaternions to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n t : array-like, shape (...)\n Exponent\n\n Returns\n -------\n dq_ts : array, shape (..., 8)\n Unit dual quaternions to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz) ** t\n\n See Also\n --------\n pytransform3d.transformations.dual_quaternion_power :\n Compute power of unit dual quaternion with respect to scalar.\n \"\"\"\n q, s_axis, h, theta = screw_parameters_from_dual_quaternions(dqs)\n return dual_quaternions_from_screw_parameters(q, s_axis, h, theta * ts)\n\n\ndef dual_quaternions_sclerp(starts, ends, ts):\n \"\"\"Screw linear interpolation (ScLERP) for array of dual quaternions.\n\n Parameters\n ----------\n starts : array-like, shape (..., 8)\n Unit dual quaternion to represent start poses:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n end : array-like, shape (..., 8)\n Unit dual quaternion to represent end poses:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n ts : array-like, shape (...)\n Positions between starts and goals\n\n Returns\n -------\n dq_ts : array, shape (..., 8)\n Interpolated unit dual quaternion: (pw, px, py, pz, qw, qx, qy, qz)\n\n\n See Also\n --------\n pytransform3d.transformations.dual_quaternion_sclerp :\n Screw linear interpolation (ScLERP) for dual quaternions.\n \"\"\"\n starts = np.asarray(starts)\n ends = np.asarray(ends)\n ts = np.asarray(ts)\n\n if starts.shape != ends.shape:\n raise ValueError(\n \"The 'starts' and 'ends' arrays must have the same shape.\"\n )\n\n if ts.ndim != starts.ndim - 1 or (\n ts.ndim > 0 and ts.shape != starts.shape[:-1]\n ):\n raise ValueError(\n \"ts array, shape=%s must have the same number of elements as \"\n \"starts array, shape=%s\" % (ts.shape, starts.shape)\n )\n\n diffs = batch_concatenate_dual_quaternions(batch_dq_q_conj(starts), ends)\n powers = dual_quaternions_power(diffs, ts)\n return batch_concatenate_dual_quaternions(starts, powers)\n\n\ndef pqs_from_dual_quaternions(dqs):\n \"\"\"Get positions and quaternions from dual quaternions.\n\n Parameters\n ----------\n dqs : array-like, shape (..., 8)\n Dual quaternions to represent transforms:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n Returns\n -------\n pqs : array, shape (..., 7)\n Poses represented by positions and quaternions in the\n order (x, y, z, qw, qx, qy, qz)\n \"\"\"\n dqs = np.asarray(dqs)\n instances_shape = dqs.shape[:-1]\n out = np.empty(instances_shape + (7,))\n out[..., 3:] = dqs[..., :4]\n out[..., :3] = (\n 2\n * batch_concatenate_quaternions(\n dqs[..., 4:], batch_q_conj(out[..., 3:])\n )[..., 1:]\n )\n return out\n\n\ndef screw_parameters_from_dual_quaternions(dqs):\n \"\"\"Compute screw parameters from dual quaternions.\n\n Parameters\n ----------\n dqs : array-like, shape (..., 8)\n Unit dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n Returns\n -------\n qs : array, shape (..., 3)\n Vector to a point on the screw axis\n\n s_axiss : array, shape (..., 3)\n Direction vector of the screw axis\n\n hs : array, shape (...,)\n Pitch of the screw. The pitch is the ratio of translation and rotation\n of the screw axis. Infinite pitch indicates pure translation.\n\n thetas : array, shape (...,)\n Parameter of the transformation: theta is the angle of rotation\n and h * theta the translation.\n\n See Also\n --------\n pytransform3d.transformations.screw_parameters_from_dual_quaternion :\n Compute screw parameters from dual quaternion.\n \"\"\"\n reals = dqs[..., :4]\n duals = dqs[..., 4:]\n\n a = axis_angles_from_quaternions(reals)\n s_axis = np.copy(a[..., :3])\n thetas = a[..., 3]\n\n translation = (\n 2 * batch_concatenate_quaternions(duals, batch_q_conj(reals))[..., 1:]\n )\n\n # instead of the if/else stamenets in the\n # screw_parameters_from_dual_quaternion function\n # we use mask array to enable vectorized operations\n # the name of the mask represent the according block in\n # the original function\n outer_if_mask = np.abs(thetas) < np.finfo(float).eps\n outer_else_mask = np.logical_not(outer_if_mask)\n\n ds = np.linalg.norm(translation, axis=-1)\n inner_if_mask = ds < np.finfo(float).eps\n\n outer_if_inner_if_mask = np.logical_and(outer_if_mask, inner_if_mask)\n outer_if_inner_else_mask = np.logical_and(\n outer_if_mask, np.logical_not(inner_if_mask)\n )\n\n if np.any(outer_if_inner_if_mask):\n s_axis[outer_if_inner_if_mask] = np.array([1.0, 0.0, 0.0])\n\n if np.any(outer_if_inner_else_mask):\n s_axis[outer_if_inner_else_mask] = (\n translation[outer_if_inner_else_mask]\n / ds[outer_if_inner_else_mask][..., np.newaxis]\n )\n\n qs = np.zeros(dqs.shape[:-1] + (3,))\n thetas[outer_if_mask] = ds[outer_if_mask]\n hs = np.full(dqs.shape[:-1], np.inf)\n\n if np.any(outer_else_mask):\n distance = np.einsum(\n \"ij,ij->i\", translation[outer_else_mask], s_axis[outer_else_mask]\n )\n\n moment = 0.5 * (\n np.cross(translation[outer_else_mask], s_axis[outer_else_mask])\n + (\n translation[outer_else_mask]\n - distance[..., np.newaxis] * s_axis[outer_else_mask]\n )\n / np.tan(0.5 * thetas[outer_else_mask])[..., np.newaxis]\n )\n\n qs[outer_else_mask] = np.cross(s_axis[outer_else_mask], moment)\n hs[outer_else_mask] = distance / thetas[outer_else_mask]\n\n return qs, s_axis, hs, thetas\n\n\ndef transforms_from_dual_quaternions(dqs):\n \"\"\"Get transformations from dual quaternions.\n\n Parameters\n ----------\n dqs : array-like, shape (..., 8)\n Dual quaternions to represent transforms:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n Returns\n -------\n A2Bs : array, shape (..., 4, 4)\n Poses represented by homogeneous matrices\n \"\"\"\n dqs = np.asarray(dqs)\n instances_shape = dqs.shape[:-1]\n out = np.empty(instances_shape + (4, 4))\n out[..., :3, :3] = matrices_from_quaternions(dqs[..., :4])\n out[..., :3, 3] = (\n 2\n * batch_concatenate_quaternions(\n dqs[..., 4:], batch_q_conj(dqs[..., :4])\n )[..., 1:]\n )\n out[..., 3, :3] = 0.0\n out[..., 3, 3] = 1.0\n return out\n" }, { "path": "pytransform3d/trajectories/_dual_quaternions.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef batch_dq_conj(dqs: npt.ArrayLike) -> np.ndarray: ...\ndef batch_dq_q_conj(dqs: npt.ArrayLike) -> np.ndarray: ...\ndef batch_concatenate_dual_quaternions(\n dqs1: npt.ArrayLike, dqs2: npt.ArrayLike\n) -> np.ndarray: ...\ndef batch_dq_prod_vector(\n dqs: npt.ArrayLike, V: npt.ArrayLike\n) -> np.ndarray: ...\ndef dual_quaternions_power(\n dqs: npt.ArrayLike, ts: npt.ArrayLike\n) -> np.ndarray: ...\ndef dual_quaternions_sclerp(\n starts: npt.ArrayLike, ends: npt.ArrayLike, ts: npt.ArrayLike\n) -> np.ndarray: ...\ndef pqs_from_dual_quaternions(dqs: npt.ArrayLike) -> np.ndarray: ...\ndef screw_parameters_from_dual_quaternions(\n dqs: npt.ArrayLike,\n) -> np.ndarray: ...\ndef transforms_from_dual_quaternions(dqs: npt.ArrayLike) -> np.ndarray: ...\n" }, { "path": "pytransform3d/trajectories/_plot.py", "content": "\"\"\"Plotting utilities.\"\"\"\n\nfrom ._pqs import transforms_from_pqs\n\n\ndef plot_trajectory(\n ax=None,\n P=None,\n normalize_quaternions=True,\n show_direction=True,\n n_frames=10,\n s=1.0,\n label=None,\n ax_s=1,\n **kwargs,\n): # pragma: no cover\n \"\"\"Plot pose trajectory.\n\n Parameters\n ----------\n ax : Matplotlib 3d axis, optional (default: None)\n If the axis is None, a new 3d axis will be created\n\n P : array-like, shape (n_steps, 7), optional (default: None)\n Sequence of poses represented by positions and quaternions in the\n order (x, y, z, w, vx, vy, vz) for each step\n\n normalize_quaternions : bool, optional (default: True)\n Normalize quaternions before plotting\n\n show_direction : bool, optional (default: True)\n Plot an arrow to indicate the direction of the trajectory\n\n n_frames : int, optional (default: 10)\n Number of frames that should be plotted to indicate the rotation\n\n s : float, optional (default: 1)\n Scaling of the frames that will be drawn\n\n label : str, optional (default: None)\n Label of the trajectory\n\n ax_s : float, optional (default: 1)\n Scaling of the new matplotlib 3d axis\n\n kwargs : dict, optional (default: {})\n Additional arguments for the plotting functions, e.g. alpha\n\n Returns\n -------\n ax : Matplotlib 3d axis\n New or old axis\n\n Raises\n ------\n ValueError\n If trajectory does not contain any elements.\n \"\"\"\n if P is None or len(P) == 0:\n raise ValueError(\"Trajectory does not contain any elements.\")\n\n if ax is None:\n from ..plot_utils import make_3d_axis\n\n ax = make_3d_axis(ax_s)\n\n A2Bs = transforms_from_pqs(P, normalize_quaternions)\n from ..plot_utils import Trajectory\n\n trajectory = Trajectory(A2Bs, show_direction, n_frames, s, label, **kwargs)\n trajectory.add_trajectory(ax)\n\n return ax\n" }, { "path": "pytransform3d/trajectories/_plot.pyi", "content": "import numpy as np\nimport numpy.typing as npt\nfrom mpl_toolkits.mplot3d import Axes3D\nfrom typing import Union\n\ndef plot_trajectory(\n ax: Union[None, Axes3D] = ...,\n P: Union[None, npt.ArrayLike] = ...,\n normalize_quaternions: bool = ...,\n show_direction: bool = ...,\n n_frames: int = ...,\n s: float = ...,\n label: Union[str, None] = ...,\n ax_s: float = ...,\n **kwargs,\n) -> Axes3D: ...\n" }, { "path": "pytransform3d/trajectories/_pqs.py", "content": "\"\"\"Position+quaternion operations.\"\"\"\n\nimport numpy as np\n\nfrom ..batch_rotations import (\n matrices_from_quaternions,\n batch_concatenate_quaternions,\n)\n\n\ndef transforms_from_pqs(P, normalize_quaternions=True):\n \"\"\"Get sequence of homogeneous matrices from positions and quaternions.\n\n Parameters\n ----------\n P : array-like, shape (..., 7)\n Poses represented by positions and quaternions in the\n order (x, y, z, qw, qx, qy, qz)\n\n normalize_quaternions : bool, optional (default: True)\n Normalize quaternions before conversion\n\n Returns\n -------\n A2Bs : array, shape (..., 4, 4)\n Poses represented by homogeneous matrices\n \"\"\"\n P = np.asarray(P)\n instances_shape = P.shape[:-1]\n A2Bs = np.empty(instances_shape + (4, 4))\n A2Bs[..., :3, 3] = P[..., :3]\n A2Bs[..., 3, :3] = 0.0\n A2Bs[..., 3, 3] = 1.0\n\n matrices_from_quaternions(\n P[..., 3:], normalize_quaternions, out=A2Bs[..., :3, :3]\n )\n\n return A2Bs\n\n\ndef dual_quaternions_from_pqs(pqs):\n \"\"\"Get dual quaternions from positions and quaternions.\n\n Parameters\n ----------\n pqs : array-like, shape (..., 7)\n Poses represented by positions and quaternions in the\n order (x, y, z, qw, qx, qy, qz)\n\n Returns\n -------\n dqs : array, shape (..., 8)\n Dual quaternions to represent transforms:\n (pw, px, py, pz, qw, qx, qy, qz)\n \"\"\"\n pqs = np.asarray(pqs)\n instances_shape = pqs.shape[:-1]\n out = np.empty(instances_shape + (8,))\n\n # orientation quaternion\n out[..., :4] = pqs[..., 3:]\n\n # use memory temporarily to store position\n out[..., 4] = 0\n out[..., 5:] = pqs[..., :3]\n\n out[..., 4:] = 0.5 * batch_concatenate_quaternions(\n out[..., 4:], out[..., :4]\n )\n return out\n" }, { "path": "pytransform3d/trajectories/_pqs_.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef transforms_from_pqs(\n P: npt.ArrayLike, normalize_quaternions: bool = ...\n) -> np.ndarray: ...\ndef dual_quaternions_from_pqs(pqs: npt.ArrayLike) -> np.ndarray: ...\n" }, { "path": "pytransform3d/trajectories/_random.py", "content": "\"\"\"Random trajectory generation.\"\"\"\n\nimport numpy as np\n\nfrom ._dual_quaternions import (\n transforms_from_dual_quaternions,\n dual_quaternions_sclerp,\n)\nfrom ._screws import (\n transforms_from_exponential_coordinates,\n)\nfrom ._transforms import (\n concat_many_to_many,\n dual_quaternions_from_transforms,\n)\n\n_N_EXP_COORDINATE_DIMS = 6\n\n\ndef random_trajectories(\n rng=np.random.default_rng(0),\n n_trajectories=10,\n n_steps=101,\n start=np.eye(4),\n goal=np.eye(4),\n scale=100.0 * np.ones(6),\n):\n \"\"\"Generate random trajectories.\n\n Create a smooth random trajectory with low accelerations.\n\n The generated trajectories consist of a linear movement from start to goal\n and a superimposed random movement with low accelerations. Hence, the first\n pose and last pose do not exactly equal the start and goal pose\n respectively.\n\n Parameters\n ----------\n rng : np.random.Generator, optional (default: random seed 0)\n Random number generator\n\n n_trajectories : int, optional (default: 10)\n Number of trajectories that should be generated.\n\n n_steps : int, optional (default: 101)\n Number of steps in each trajectory.\n\n start : array-like, shape (4, 4), optional (default: I)\n Start pose as transformation matrix.\n\n goal : array-like, shape (4, 4), optional (default: I)\n Goal pose as transformation matrix.\n\n scale : array-like, shape (6,), optional (default: [100] * 6)\n Scaling factor for random deviations from linear movement from start to\n goal.\n\n Returns\n -------\n trajectories : array, shape (n_trajectories, n_steps, 4, 4)\n Random trajectories between start and goal.\n \"\"\"\n dt = 1.0 / (n_steps - 1)\n linear_component = _linear_movement(start, goal, n_steps, dt)\n\n L = _acceleration_L(_N_EXP_COORDINATE_DIMS, n_steps, dt)\n samples = rng.normal(\n size=(n_trajectories, _N_EXP_COORDINATE_DIMS * n_steps)\n )\n smooth_samples = np.dot(samples, L.T)\n Sthetas = smooth_samples.reshape(\n n_trajectories, _N_EXP_COORDINATE_DIMS, n_steps\n ).transpose([0, 2, 1])\n Sthetas *= np.asarray(scale)[np.newaxis, np.newaxis]\n\n trajectories = transforms_from_exponential_coordinates(Sthetas)\n for i in range(n_trajectories):\n trajectories[i] = concat_many_to_many(trajectories[i], linear_component)\n\n return trajectories\n\n\ndef _linear_movement(start, goal, n_steps, dt):\n \"\"\"Linear movement from start to goal.\n\n Parameters\n ----------\n start : array-like, shape (4, 4)\n Start pose as transformation matrix.\n\n goal : array-like, shape (4, 4)\n Goal pose as transformation matrix.\n\n n_steps : int\n Number of steps.\n\n dt : float\n Time difference between two steps.\n\n Returns\n -------\n linear_component : array, shape (n_steps, 4, 4)\n Linear trajectory from start to goal with equal step sizes.\n \"\"\"\n time = np.arange(n_steps) * dt\n start_dq = dual_quaternions_from_transforms(start)\n goal_dq = dual_quaternions_from_transforms(goal)\n return transforms_from_dual_quaternions(\n dual_quaternions_sclerp(\n np.repeat(start_dq[np.newaxis], n_steps, axis=0),\n np.repeat(goal_dq[np.newaxis], n_steps, axis=0),\n time,\n )\n )\n\n\ndef _acceleration_L(n_dims, n_steps, dt):\n \"\"\"Cholesky decomposition of a smooth trajectory covariance.\n\n Parameters\n ----------\n n_dims : int\n Number of dimensions.\n\n n_steps : int\n Number of steps in the trajectory.\n\n dt : float\n Time difference between two steps.\n\n Returns\n -------\n L : array, shape (n_steps * n_dims, n_steps * n_dims)\n Cholesky decomposition of covariance created from finite difference\n matrix.\n \"\"\"\n A_per_dim = _create_fd_matrix_1d(n_steps, dt)\n covariance = np.linalg.inv(np.dot(A_per_dim.T, A_per_dim))\n L_per_dim = np.linalg.cholesky(covariance)\n\n # Copy L for each dimension\n L = np.zeros((n_dims * n_steps, n_dims * n_steps))\n for d in range(n_dims):\n L[d * n_steps : (d + 1) * n_steps, d * n_steps : (d + 1) * n_steps] = (\n L_per_dim\n )\n return L\n\n\ndef _create_fd_matrix_1d(n_steps, dt):\n r\"\"\"Create one-dimensional finite difference matrix for second derivative.\n\n The finite difference matrix A for the second derivative with respect to\n time is defined as:\n\n .. math::\n\n \\ddot(x) = A x\n\n Parameters\n ----------\n n_steps : int\n Number of steps in the trajectory\n\n dt : float\n Time in seconds between successive steps\n\n Returns\n -------\n A : array, shape (n_steps + 2, n_steps)\n Finite difference matrix for second derivative with respect to time\n \"\"\"\n A = np.zeros((n_steps + 2, n_steps), dtype=float)\n super_diagonal = (np.arange(n_steps), np.arange(n_steps))\n A[super_diagonal] = np.ones(n_steps)\n sub_diagonal = (np.arange(2, n_steps + 2), np.arange(n_steps))\n A[sub_diagonal] = np.ones(n_steps)\n main_diagonal = (np.arange(1, n_steps + 1), np.arange(n_steps))\n A[main_diagonal] = -2 * np.ones(n_steps)\n return A / (dt**2)\n" }, { "path": "pytransform3d/trajectories/_random.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef random_trajectories(\n rng: np.random.Generator = ...,\n n_trajectories: int = ...,\n n_steps: int = ...,\n start: npt.ArrayLike = ...,\n goal: npt.ArrayLike = ...,\n scale: npt.ArrayLike = ...,\n) -> np.ndarray: ...\ndef _linear_movement(\n start: npt.ArrayLike, goal: npt.ArrayLike, n_steps: int, dt: float\n) -> np.ndarray: ...\ndef _acceleration_L(n_dims: int, n_steps: int, dt: float) -> np.ndarray: ...\ndef _create_fd_matrix_1d(n_steps: int, dt: float) -> np.ndarray: ...\n" }, { "path": "pytransform3d/trajectories/_screws.py", "content": "\"\"\"Representations related to screw theory.\"\"\"\n\nimport numpy as np\n\nfrom ..batch_rotations import (\n matrices_from_compact_axis_angles,\n)\nfrom ..transformations import (\n screw_axis_from_exponential_coordinates,\n screw_parameters_from_screw_axis,\n screw_axis_from_screw_parameters,\n)\nfrom ..transformations import (\n transform_from_exponential_coordinates,\n)\n\n\ndef mirror_screw_axis_direction(Sthetas):\n \"\"\"Switch to the other representation of the same transformation.\n\n We take the negative of the screw axis, invert the rotation angle\n and adapt the screw pitch accordingly. For this operation we have\n to convert exponential coordinates to screw parameters first.\n\n Parameters\n ----------\n Sthetas : array-like, shape (n_steps, 6)\n Exponential coordinates of transformation:\n (omega_x, omega_y, omega_z, v_x, v_y, v_z)\n\n Returns\n -------\n Sthetas : array, shape (n_steps, 6)\n Exponential coordinates of transformation:\n (omega_x, omega_y, omega_z, v_x, v_y, v_z)\n \"\"\"\n Sthetas_new = np.empty((len(Sthetas), 6))\n for i, Stheta in enumerate(Sthetas):\n S, theta = screw_axis_from_exponential_coordinates(Stheta)\n q, s, h = screw_parameters_from_screw_axis(S)\n s_new = -s\n theta_new = 2.0 * np.pi - theta\n h_new = -h * theta / theta_new\n Stheta_new = (\n screw_axis_from_screw_parameters(q, s_new, h_new) * theta_new\n )\n Sthetas_new[i] = Stheta_new\n return Sthetas_new\n\n\ndef transforms_from_exponential_coordinates(Sthetas):\n \"\"\"Compute transformations from exponential coordinates.\n\n Parameters\n ----------\n Sthetas : array-like, shape (..., 6)\n Exponential coordinates of transformation:\n S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,\n where S is the screw axis, the first 3 components are related to\n rotation and the last 3 components are related to translation.\n Theta is the rotation angle and h * theta the translation.\n\n Returns\n -------\n A2Bs : array, shape (..., 4, 4)\n Poses represented by homogeneous matrices\n \"\"\"\n Sthetas = np.asarray(Sthetas)\n if Sthetas.ndim == 1:\n return transform_from_exponential_coordinates(Sthetas)\n\n instances_shape = Sthetas.shape[:-1]\n\n t = np.linalg.norm(Sthetas[..., :3], axis=-1)\n\n A2Bs = np.empty(instances_shape + (4, 4))\n A2Bs[..., 3, :] = (0, 0, 0, 1)\n\n ind_only_translation = t == 0.0\n\n if not np.all(ind_only_translation):\n t[ind_only_translation] = 1.0\n screw_axes = Sthetas / t[..., np.newaxis]\n\n matrices_from_compact_axis_angles(\n axes=screw_axes[..., :3], angles=t, out=A2Bs[..., :3, :3]\n )\n\n # from sympy import *\n # o0, o1, o2, vx, vy, vz, t = symbols(\"o0 o1 o2 v_x v_y v_z t\")\n # w = Matrix([[0, -o2, o1], [o2, 0, -o0], [-o1, o0, 0]])\n # v = Matrix([[vx], [vy], [vz]])\n # p = (eye(3) * t + (1 - cos(t)) * w + (t - sin(t)) * w * w) * v\n #\n # Result:\n # -v_x*(o1**2*(t - sin(t)) + o2**2*(t - sin(t)) - t)\n # + v_y*(o0*o1*(t - sin(t)) + o2*(cos(t) - 1))\n # + v_z*(o0*o2*(t - sin(t)) - o1*(cos(t) - 1))\n # v_x*(o0*o1*(t - sin(t)) - o2*(cos(t) - 1))\n # - v_y*(o0**2*(t - sin(t)) + o2**2*(t - sin(t)) - t)\n # + v_z*(o0*(cos(t) - 1) + o1*o2*(t - sin(t)))\n # v_x*(o0*o2*(t - sin(t)) + o1*(cos(t) - 1))\n # - v_y*(o0*(cos(t) - 1) - o1*o2*(t - sin(t)))\n # - v_z*(o0**2*(t - sin(t)) + o1**2*(t - sin(t)) - t)\n\n tms = t - np.sin(t)\n cm1 = np.cos(t) - 1.0\n o0 = screw_axes[..., 0]\n o1 = screw_axes[..., 1]\n o2 = screw_axes[..., 2]\n v0 = screw_axes[..., 3]\n v1 = screw_axes[..., 4]\n v2 = screw_axes[..., 5]\n o01tms = o0 * o1 * tms\n o12tms = o1 * o2 * tms\n o02tms = o0 * o2 * tms\n o0cm1 = o0 * cm1\n o1cm1 = o1 * cm1\n o2cm1 = o2 * cm1\n o00tms = o0 * o0 * tms\n o11tms = o1 * o1 * tms\n o22tms = o2 * o2 * tms\n v0 = v0.reshape(*instances_shape)\n v1 = v1.reshape(*instances_shape)\n v2 = v2.reshape(*instances_shape)\n A2Bs[..., 0, 3] = (\n -v0 * (o11tms + o22tms - t)\n + v1 * (o01tms + o2cm1)\n + v2 * (o02tms - o1cm1)\n )\n A2Bs[..., 1, 3] = (\n v0 * (o01tms - o2cm1)\n - v1 * (o00tms + o22tms - t)\n + v2 * (o0cm1 + o12tms)\n )\n A2Bs[..., 2, 3] = (\n v0 * (o02tms + o1cm1)\n - v1 * (o0cm1 - o12tms)\n - v2 * (o00tms + o11tms - t)\n )\n\n A2Bs[ind_only_translation, :3, :3] = np.eye(3)\n A2Bs[ind_only_translation, :3, 3] = Sthetas[ind_only_translation, 3:]\n\n return A2Bs\n\n\ndef dual_quaternions_from_screw_parameters(qs, s_axis, hs, thetas):\n \"\"\"Compute dual quaternions from arrays of screw parameters.\n\n Parameters\n ----------\n qs : array-like, shape (..., 3)\n Vector to a point on the screw axis\n\n s_axis : array-like, shape (..., 3)\n Direction vector of the screw axis\n\n hs : array-like, shape (...,)\n Pitch of the screw. The pitch is the ratio of translation and rotation\n of the screw axis. Infinite pitch indicates pure translation.\n\n thetas : array-like, shape (...,)\n Parameter of the transformation: theta is the angle of rotation\n and h * theta the translation.\n\n Returns\n -------\n dqs : array, shape (..., 8)\n Unit dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n See Also\n --------\n pytransform3d.transformations.dual_quaternion_from_screw_parameters :\n Compute dual quaternion from screw parameters.\n \"\"\"\n h_is_not_inf_mask = ~np.isinf(hs)\n mod_thetas = np.where(h_is_not_inf_mask, thetas, 0.0)\n ds = np.copy(thetas)\n ds[h_is_not_inf_mask] *= hs[h_is_not_inf_mask]\n\n moments = np.cross(qs, s_axis)\n half_distances = 0.5 * ds\n half_thetas = 0.5 * mod_thetas\n sin_half_angles = np.sin(half_thetas)\n cos_half_angles = np.cos(half_thetas)\n\n real_w = cos_half_angles\n real_vec = sin_half_angles[..., np.newaxis] * s_axis\n dual_w = -half_distances * sin_half_angles\n dual_vec = (\n sin_half_angles[..., np.newaxis] * moments\n + half_distances[..., np.newaxis]\n * cos_half_angles[..., np.newaxis]\n * s_axis\n )\n\n result = np.concatenate(\n [real_w[..., np.newaxis], real_vec, dual_w[..., np.newaxis], dual_vec],\n axis=-1,\n )\n return result\n" }, { "path": "pytransform3d/trajectories/_screws.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef mirror_screw_axis_direction(Sthetas: npt.ArrayLike) -> np.ndarray: ...\ndef transforms_from_exponential_coordinates(\n Sthetas: npt.ArrayLike,\n) -> np.ndarray: ...\ndef dual_quaternions_from_screw_parameters(\n qs: npt.ArrayLike,\n s_axis: npt.ArrayLike,\n hs: npt.ArrayLike,\n thetas: npt.ArrayLike,\n) -> np.ndarray: ...\n" }, { "path": "pytransform3d/trajectories/_transforms.py", "content": "\"\"\"Transformation matrices.\"\"\"\n\nimport numpy as np\n\nfrom pytransform3d.batch_rotations import (\n quaternions_from_matrices,\n axis_angles_from_matrices,\n batch_concatenate_quaternions,\n)\n\n\ndef invert_transforms(A2Bs):\n \"\"\"Invert transforms.\n\n Parameters\n ----------\n A2Bs : array-like, shape (..., 4, 4)\n Transforms from frames A to frames B\n\n Returns\n -------\n B2As : array, shape (..., 4, 4)\n Transforms from frames B to frames A\n\n See Also\n --------\n pytransform3d.transformations.invert_transform :\n Invert one transformation.\n \"\"\"\n A2Bs = np.asarray(A2Bs)\n instances_shape = A2Bs.shape[:-2]\n B2As = np.empty_like(A2Bs)\n # ( R t )^-1 ( R^T -R^T*t )\n # ( 0 1 ) = ( 0 1 )\n B2As[..., :3, :3] = A2Bs[..., :3, :3].transpose(\n tuple(range(A2Bs.ndim - 2)) + (A2Bs.ndim - 1, A2Bs.ndim - 2)\n )\n B2As[..., :3, 3] = np.einsum(\n \"nij,nj->ni\",\n -B2As[..., :3, :3].reshape(-1, 3, 3),\n A2Bs[..., :3, 3].reshape(-1, 3),\n ).reshape(*(instances_shape + (3,)))\n B2As[..., 3, :3] = 0.0\n B2As[..., 3, 3] = 1.0\n return B2As\n\n\ndef concat_one_to_many(A2B, B2Cs):\n \"\"\"Concatenate transformation A2B with multiple transformations B2C.\n\n We use the extrinsic convention, which means that B2Cs are left-multiplied\n to A2B.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Transform from frame A to frame B\n\n B2Cs : array-like, shape (n_transforms, 4, 4)\n Transforms from frame B to frame C\n\n Returns\n -------\n A2Cs : array, shape (n_transforms, 4, 4)\n Transforms from frame A to frame C\n\n See Also\n --------\n concat_many_to_one :\n Concatenate multiple transformations with one.\n\n pytransform3d.transformations.concat :\n Concatenate two transformations.\n \"\"\"\n return np.einsum(\"nij,jk->nik\", B2Cs, A2B)\n\n\ndef concat_many_to_one(A2Bs, B2C):\n \"\"\"Concatenate multiple transformations A2B with transformation B2C.\n\n We use the extrinsic convention, which means that B2C is left-multiplied\n to A2Bs.\n\n Parameters\n ----------\n A2Bs : array-like, shape (n_transforms, 4, 4)\n Transforms from frame A to frame B\n\n B2C : array-like, shape (4, 4)\n Transform from frame B to frame C\n\n Returns\n -------\n A2Cs : array, shape (n_transforms, 4, 4)\n Transforms from frame A to frame C\n\n See Also\n --------\n concat_one_to_many :\n Concatenate one transformation with multiple transformations.\n\n pytransform3d.transformations.concat :\n Concatenate two transformations.\n \"\"\"\n return np.einsum(\"ij,njk->nik\", B2C, A2Bs)\n\n\ndef concat_many_to_many(A2B, B2C):\n \"\"\"Concatenate multiple transformations A2B with transformation B2C.\n\n We use the extrinsic convention, which means that B2C is left-multiplied\n to A2Bs.\n\n Parameters\n ----------\n A2B : array-like, shape (n_transforms, 4, 4)\n Transforms from frame A to frame B\n\n B2C : array-like, shape (n_transforms, 4, 4)\n Transform from frame B to frame C\n\n Returns\n -------\n A2Cs : array, shape (n_transforms, 4, 4)\n Transforms from frame A to frame C\n\n See Also\n --------\n concat_many_to_one :\n Concatenate one transformation with multiple transformations.\n\n pytransform3d.transformations.concat :\n Concatenate two transformations.\n \"\"\"\n return np.einsum(\"ijk,ikl->ijl\", B2C, A2B)\n\n\ndef concat_dynamic(A2B, B2C):\n \"\"\"Concatenate multiple transformations A2B,B2C with different shapes.\n\n We use the extrinsic convention, which means that B2C is left-multiplied\n to A2Bs. it can handle different shapes of A2B and B2C dynamically.\n\n Parameters\n ----------\n A2B : array-like, shape (n_transforms, 4, 4) or (4, 4)\n Transforms from frame A to frame B\n\n B2C : array-like, shape (n_transforms, 4, 4) or (4, 4)\n Transform from frame B to frame C\n\n Returns\n -------\n A2Cs : array, shape (n_transforms, 4, 4) or (4, 4)\n Transforms from frame A to frame C\n\n See Also\n --------\n concat_many_to_one :\n Concatenate multiple transformations with one transformation.\n\n concat_one_to_many :\n Concatenate one transformation with multiple transformations.\n\n concat_many_to_many :\n Concatenate multiple transformations with multiple transformations.\n\n pytransform3d.transformations.concat :\n Concatenate two transformations.\n \"\"\"\n A2B = np.asarray(A2B)\n B2C = np.asarray(B2C)\n if B2C.ndim == 2 and A2B.ndim == 2:\n return B2C.dot(A2B)\n elif B2C.ndim == 2 and A2B.ndim == 3:\n return concat_many_to_one(A2B, B2C)\n elif B2C.ndim == 3 and A2B.ndim == 2:\n return concat_one_to_many(A2B, B2C)\n elif B2C.ndim == 3 and A2B.ndim == 3:\n return concat_many_to_many(A2B, B2C)\n else:\n raise ValueError(\n \"Expected ndim 2 or 3; got B2C.ndim=%d, A2B.ndim=%d\"\n % (B2C.ndim, A2B.ndim)\n )\n\n\ndef pqs_from_transforms(A2Bs):\n \"\"\"Get sequence of positions and quaternions from homogeneous matrices.\n\n Parameters\n ----------\n A2Bs : array-like, shape (..., 4, 4)\n Poses represented by homogeneous matrices\n\n Returns\n -------\n P : array, shape (n_steps, 7)\n Poses represented by positions and quaternions in the\n order (x, y, z, qw, qx, qy, qz) for each step\n \"\"\"\n A2Bs = np.asarray(A2Bs)\n instances_shape = A2Bs.shape[:-2]\n P = np.empty(instances_shape + (7,))\n P[..., :3] = A2Bs[..., :3, 3]\n quaternions_from_matrices(A2Bs[..., :3, :3], out=P[..., 3:])\n return P\n\n\ndef exponential_coordinates_from_transforms(A2Bs):\n \"\"\"Compute exponential coordinates from transformations.\n\n Parameters\n ----------\n A2Bs : array-like, shape (..., 4, 4)\n Poses represented by homogeneous matrices\n\n Returns\n -------\n Sthetas : array, shape (..., 6)\n Exponential coordinates of transformation:\n S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,\n where S is the screw axis, the first 3 components are related to\n rotation and the last 3 components are related to translation.\n Theta is the rotation angle and h * theta the translation.\n \"\"\"\n A2Bs = np.asarray(A2Bs)\n\n instances_shape = A2Bs.shape[:-2]\n\n Rs = A2Bs[..., :3, :3]\n ps = A2Bs[..., :3, 3]\n\n traces = np.einsum(\"nii\", Rs.reshape(-1, 3, 3))\n if instances_shape: # noqa: SIM108\n traces = traces.reshape(*instances_shape)\n else:\n # this works because indX will be a single boolean and\n # out[True, n] = value will assign value to out[n], while\n # out[False, n] = value will not assign value to out[n]\n traces = traces[0]\n\n Sthetas = np.empty(instances_shape + (6,))\n\n omega_thetas = axis_angles_from_matrices(Rs, traces=traces)\n Sthetas[..., :3] = omega_thetas[..., :3]\n thetas = omega_thetas[..., 3]\n\n # from sympy import *\n # o0, o1, o2, px, py, pz, t = symbols(\"o0 o1 o2 p0 p1 p2 theta\")\n # w = Matrix([[0, -o2, o1], [o2, 0, -o0], [-o1, o0, 0]])\n # p = Matrix([[px], [py], [pz]])\n # v = (eye(3) / t - 0.5 * w + (1 / t - 0.5 / tan(t / 2.0)) * w * w) * p\n\n # Result:\n # p0*(-o1**2*(-0.5/tan(0.5*t) + 1/t)\n # - o2**2*(-0.5/tan(0.5*t) + 1/t) + 1/t)\n # + p1*(o0*o1*(-0.5/tan(0.5*t) + 1/t) + 0.5*o2)\n # + p2*(o0*o2*(-0.5/tan(0.5*t) + 1/t) - 0.5*o1)\n # p0*(o0*o1*(-0.5/tan(0.5*t) + 1/t) - 0.5*o2)\n # + p1*(-o0**2*(-0.5/tan(0.5*t) + 1/t)\n # - o2**2*(-0.5/tan(0.5*t) + 1/t) + 1/t)\n # + p2*(0.5*o0 + o1*o2*(-0.5/tan(0.5*t) + 1/t))\n # p0*(o0*o2*(-0.5/tan(0.5*t) + 1/t) + 0.5*o1)\n # + p1*(-0.5*o0 + o1*o2*(-0.5/tan(0.5*t) + 1/t))\n # + p2*(-o0**2*(-0.5/tan(0.5*t) + 1/t)\n # - o1**2*(-0.5/tan(0.5*t) + 1/t) + 1/t)\n\n thetas = np.maximum(thetas, np.finfo(float).tiny)\n ti = 1.0 / thetas\n tan_term = -0.5 / np.tan(thetas / 2.0) + ti\n o0 = omega_thetas[..., 0]\n o1 = omega_thetas[..., 1]\n o2 = omega_thetas[..., 2]\n p0 = ps[..., 0]\n p1 = ps[..., 1]\n p2 = ps[..., 2]\n o00 = o0 * o0\n o01 = o0 * o1\n o02 = o0 * o2\n o11 = o1 * o1\n o12 = o1 * o2\n o22 = o2 * o2\n Sthetas[..., 3] = (\n p0 * ((-o11 - o22) * tan_term + ti)\n + p1 * (o01 * tan_term + 0.5 * o2)\n + p2 * (o02 * tan_term - 0.5 * o1)\n )\n Sthetas[..., 4] = (\n p0 * (o01 * tan_term - 0.5 * o2)\n + p1 * ((-o00 - o22) * tan_term + ti)\n + p2 * (0.5 * o0 + o12 * tan_term)\n )\n Sthetas[..., 5] = (\n p0 * (o02 * tan_term + 0.5 * o1)\n + p1 * (-0.5 * o0 + o12 * tan_term)\n + p2 * ((-o00 - o11) * tan_term + ti)\n )\n\n Sthetas *= thetas[..., np.newaxis]\n\n ind_only_translation = traces >= 3.0 - np.finfo(float).eps\n Sthetas[ind_only_translation, :3] = 0.0\n Sthetas[ind_only_translation, 3:] = ps[ind_only_translation]\n\n return Sthetas\n\n\ndef dual_quaternions_from_transforms(A2Bs):\n \"\"\"Get dual quaternions from transformations.\n\n Parameters\n ----------\n A2Bs : array-like, shape (..., 4, 4)\n Poses represented by homogeneous matrices\n\n Returns\n -------\n dqs : array, shape (..., 8)\n Dual quaternions to represent transforms:\n (pw, px, py, pz, qw, qx, qy, qz)\n \"\"\"\n A2Bs = np.asarray(A2Bs)\n instances_shape = A2Bs.shape[:-2]\n out = np.empty(instances_shape + (8,))\n\n # orientation quaternion\n out[..., :4] = quaternions_from_matrices(A2Bs[..., :3, :3])\n\n # use memory temporarily to store position\n out[..., 4] = 0\n out[..., 5:] = A2Bs[..., :3, 3]\n\n out[..., 4:] = 0.5 * batch_concatenate_quaternions(\n out[..., 4:], out[..., :4]\n )\n return out\n" }, { "path": "pytransform3d/trajectories/_transforms.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef invert_transforms(A2Bs: npt.ArrayLike) -> np.ndarray: ...\ndef concat_one_to_many(\n A2B: npt.ArrayLike, B2Cs: npt.ArrayLike\n) -> np.ndarray: ...\ndef concat_many_to_one(\n A2Bs: npt.ArrayLike, B2C: npt.ArrayLike\n) -> np.ndarray: ...\ndef concat_many_to_many(\n A2B: npt.ArrayLike, B2C: npt.ArrayLike\n) -> np.ndarray: ...\ndef concat_dynamic(A2B: npt.ArrayLike, B2C: npt.ArrayLike) -> np.ndarray: ...\ndef pqs_from_transforms(A2Bs: npt.ArrayLike) -> np.ndarray: ...\ndef exponential_coordinates_from_transforms(\n A2Bs: npt.ArrayLike,\n) -> np.ndarray: ...\ndef dual_quaternions_from_transforms(A2Bs: npt.ArrayLike) -> np.ndarray: ...\n" }, { "path": "pytransform3d/trajectories/test/test_trajectories.py", "content": "import numpy as np\nimport pytest\nfrom numpy.testing import assert_array_almost_equal\n\nimport pytransform3d.batch_rotations as pbr\nimport pytransform3d.rotations as pr\nimport pytransform3d.trajectories as ptr\nimport pytransform3d.transformations as pt\n\n\ndef test_invert_transforms_0dims():\n rng = np.random.default_rng(0)\n A2B = pt.random_transform(rng)\n B2A = pt.invert_transform(A2B)\n assert_array_almost_equal(B2A, ptr.invert_transforms(A2B))\n\n\ndef test_invert_transforms_1dims():\n rng = np.random.default_rng(1)\n A2Bs = np.empty((3, 4, 4))\n B2As = np.empty((3, 4, 4))\n for i in range(len(A2Bs)):\n A2Bs[i] = pt.random_transform(rng)\n B2As[i] = pt.invert_transform(A2Bs[i])\n assert_array_almost_equal(B2As, ptr.invert_transforms(A2Bs))\n\n\ndef test_invert_transforms_2dims():\n rng = np.random.default_rng(1)\n A2Bs = np.empty((9, 4, 4))\n B2As = np.empty((9, 4, 4))\n for i in range(len(A2Bs)):\n A2Bs[i] = pt.random_transform(rng)\n B2As[i] = pt.invert_transform(A2Bs[i])\n assert_array_almost_equal(\n B2As.reshape(3, 3, 4, 4),\n ptr.invert_transforms(A2Bs.reshape(3, 3, 4, 4)),\n )\n\n\ndef test_concat_one_to_many():\n rng = np.random.default_rng(482)\n A2B = pt.random_transform(rng)\n B2C = pt.random_transform(rng)\n A2C = pt.concat(A2B, B2C)\n assert_array_almost_equal(A2C, ptr.concat_one_to_many(A2B, [B2C])[0])\n\n B2Cs = [pt.random_transform(rng) for _ in range(5)]\n A2Cs = [pt.concat(A2B, B2C) for B2C in B2Cs]\n assert_array_almost_equal(A2Cs, ptr.concat_one_to_many(A2B, B2Cs))\n\n\ndef test_concat_many_to_one():\n rng = np.random.default_rng(482)\n A2B = pt.random_transform(rng)\n B2C = pt.random_transform(rng)\n A2C = pt.concat(A2B, B2C)\n assert_array_almost_equal(A2C, ptr.concat_many_to_one([A2B], B2C)[0])\n\n A2Bs = [pt.random_transform(rng) for _ in range(5)]\n A2Cs = [pt.concat(A2B, B2C) for A2B in A2Bs]\n assert_array_almost_equal(A2Cs, ptr.concat_many_to_one(A2Bs, B2C))\n\n\ndef test_concat_dynamic():\n rng = np.random.default_rng(84320)\n n_rotations = 5\n A2Bs = np.stack([pt.random_transform(rng) for _ in range(n_rotations)])\n B2Cs = np.stack([pt.random_transform(rng) for _ in range(n_rotations)])\n A2Cs = ptr.concat_dynamic(A2Bs, B2Cs)\n\n for i in range(len(A2Cs)):\n # check n_rotations - n_rotations case\n assert_array_almost_equal(A2Cs[i], pt.concat(A2Bs[i], B2Cs[i]))\n # check 1 - 1 case\n assert_array_almost_equal(A2Cs[i], ptr.concat_dynamic(A2Bs[i], B2Cs[i]))\n # check 1 - n_rotations case\n assert_array_almost_equal(\n ptr.concat_dynamic(A2Bs[0], B2Cs), ptr.concat_one_to_many(A2Bs[0], B2Cs)\n )\n # check n_rotations - 1 case\n assert_array_almost_equal(\n ptr.concat_dynamic(A2Bs, B2Cs[0]), ptr.concat_many_to_one(A2Bs, B2Cs[0])\n )\n\n with pytest.raises(ValueError, match=\"Expected ndim 2 or 3\"):\n ptr.concat_dynamic(A2Bs, B2Cs[np.newaxis])\n with pytest.raises(ValueError, match=\"Expected ndim 2 or 3\"):\n ptr.concat_dynamic(A2Bs[np.newaxis], B2Cs)\n\n\ndef test_concat_many_to_many():\n rng = np.random.default_rng(84320)\n n_rotations = 5\n A2Bs = np.stack([pt.random_transform(rng) for _ in range(n_rotations)])\n B2Cs = np.stack([pt.random_transform(rng) for _ in range(n_rotations)])\n A2Cs = ptr.concat_many_to_many(A2Bs, B2Cs)\n for i in range(len(A2Bs)):\n assert_array_almost_equal(A2Cs[i], pt.concat(A2Bs[i], B2Cs[i]))\n\n with pytest.raises(ValueError):\n ptr.concat_many_to_many(A2Bs, B2Cs[:-1])\n\n with pytest.raises(ValueError):\n ptr.concat_many_to_many(A2Bs, B2Cs[0])\n\n\ndef test_transforms_from_pqs_0dims():\n rng = np.random.default_rng(0)\n pq = np.empty(7)\n pq[:3] = rng.standard_normal(size=3)\n pq[3:] = pr.random_quaternion(rng)\n A2B = ptr.transforms_from_pqs(pq, False)\n assert_array_almost_equal(A2B, pt.transform_from_pq(pq))\n\n\ndef test_transforms_from_pqs_1dim():\n P = np.empty((10, 7))\n rng = np.random.default_rng(0)\n P[:, :3] = rng.standard_normal(size=(len(P), 3))\n P[:, 3:] = pbr.norm_vectors(rng.standard_normal(size=(len(P), 4)))\n\n H = ptr.transforms_from_pqs(P)\n P2 = ptr.pqs_from_transforms(H)\n\n assert_array_almost_equal(P[:, :3], H[:, :3, 3])\n assert_array_almost_equal(P[:, :3], P2[:, :3])\n\n for t in range(len(P)):\n pr.assert_quaternion_equal(\n P[t, 3:], pr.quaternion_from_matrix(H[t, :3, :3])\n )\n pr.assert_quaternion_equal(P[t, 3:], P2[t, 3:])\n\n\ndef test_transforms_from_pqs_4dims():\n rng = np.random.default_rng(0)\n P = rng.standard_normal(size=(2, 3, 4, 5, 7))\n P[..., 3:] = pbr.norm_vectors(P[..., 3:])\n\n H = ptr.transforms_from_pqs(P)\n P2 = ptr.pqs_from_transforms(H)\n\n assert_array_almost_equal(P[..., :3], H[..., :3, 3])\n assert_array_almost_equal(P[..., :3], P2[..., :3])\n\n\ndef test_transforms_from_exponential_coordinates():\n A2B = np.eye(4)\n Stheta = pt.exponential_coordinates_from_transform(A2B)\n assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0])\n A2B2 = ptr.transforms_from_exponential_coordinates(Stheta[np.newaxis])[0]\n assert_array_almost_equal(A2B, A2B2)\n A2B2 = ptr.transforms_from_exponential_coordinates(Stheta)\n assert_array_almost_equal(A2B, A2B2)\n A2B2 = ptr.transforms_from_exponential_coordinates([[Stheta], [Stheta]])[\n 0, 0\n ]\n assert_array_almost_equal(A2B, A2B2)\n\n A2B = pt.translate_transform(np.eye(4), [1.0, 5.0, 0.0])\n Stheta = pt.exponential_coordinates_from_transform(A2B)\n assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 1.0, 5.0, 0.0])\n A2B2 = ptr.transforms_from_exponential_coordinates(Stheta[np.newaxis])[0]\n assert_array_almost_equal(A2B, A2B2)\n A2B2 = ptr.transforms_from_exponential_coordinates(Stheta)\n assert_array_almost_equal(A2B, A2B2)\n A2B2 = ptr.transforms_from_exponential_coordinates([[Stheta], [Stheta]])[\n 0, 0\n ]\n assert_array_almost_equal(A2B, A2B2)\n\n A2B = pt.rotate_transform(\n np.eye(4), pr.active_matrix_from_angle(2, 0.5 * np.pi)\n )\n Stheta = pt.exponential_coordinates_from_transform(A2B)\n assert_array_almost_equal(Stheta, [0.0, 0.0, 0.5 * np.pi, 0.0, 0.0, 0.0])\n A2B2 = ptr.transforms_from_exponential_coordinates(Stheta[np.newaxis])[0]\n assert_array_almost_equal(A2B, A2B2)\n A2B2 = ptr.transforms_from_exponential_coordinates(Stheta)\n assert_array_almost_equal(A2B, A2B2)\n A2B2 = ptr.transforms_from_exponential_coordinates([[Stheta], [Stheta]])[\n 0, 0\n ]\n assert_array_almost_equal(A2B, A2B2)\n\n rng = np.random.default_rng(53)\n for _ in range(5):\n A2B = pt.random_transform(rng)\n Stheta = pt.exponential_coordinates_from_transform(A2B)\n A2B2 = ptr.transforms_from_exponential_coordinates(Stheta[np.newaxis])[\n 0\n ]\n assert_array_almost_equal(A2B, A2B2)\n A2B2 = ptr.transforms_from_exponential_coordinates(Stheta)\n assert_array_almost_equal(A2B, A2B2)\n A2B2 = ptr.transforms_from_exponential_coordinates(\n [[Stheta], [Stheta]]\n )[0, 0]\n assert_array_almost_equal(A2B, A2B2)\n\n\ndef test_exponential_coordinates_from_transforms_0dims():\n rng = np.random.default_rng(842)\n Sthetas = rng.standard_normal(size=6)\n H = ptr.transforms_from_exponential_coordinates(Sthetas)\n Sthetas2 = ptr.exponential_coordinates_from_transforms(H)\n H2 = ptr.transforms_from_exponential_coordinates(Sthetas2)\n assert_array_almost_equal(H, H2)\n\n\ndef test_exponential_coordinates_from_transforms_2dims():\n rng = np.random.default_rng(843)\n Sthetas = rng.standard_normal(size=(4, 4, 6))\n H = ptr.transforms_from_exponential_coordinates(Sthetas)\n Sthetas2 = ptr.exponential_coordinates_from_transforms(H)\n H2 = ptr.transforms_from_exponential_coordinates(Sthetas2)\n assert_array_almost_equal(H, H2)\n\n\ndef test_dual_quaternions_from_pqs_0dims():\n rng = np.random.default_rng(844)\n pq = rng.standard_normal(size=7)\n pq[3:] /= np.linalg.norm(pq[3:], axis=-1)[..., np.newaxis]\n dq = ptr.dual_quaternions_from_pqs(pq)\n pq2 = ptr.pqs_from_dual_quaternions(dq)\n assert_array_almost_equal(pq[:3], pq2[:3])\n pr.assert_quaternion_equal(pq[3:], pq2[3:])\n\n\ndef test_dual_quaternions_from_pqs_1dim():\n rng = np.random.default_rng(845)\n pqs = rng.standard_normal(size=(20, 7))\n pqs[..., 3:] /= np.linalg.norm(pqs[..., 3:], axis=-1)[..., np.newaxis]\n dqs = ptr.dual_quaternions_from_pqs(pqs)\n pqs2 = ptr.pqs_from_dual_quaternions(dqs)\n for pq, pq2 in zip(pqs, pqs2):\n assert_array_almost_equal(pq[:3], pq2[:3])\n pr.assert_quaternion_equal(pq[3:], pq2[3:])\n\n\ndef test_dual_quaternions_from_pqs_2dims():\n rng = np.random.default_rng(846)\n pqs = rng.standard_normal(size=(5, 5, 7))\n pqs[..., 3:] /= np.linalg.norm(pqs[..., 3:], axis=-1)[..., np.newaxis]\n dqs = ptr.dual_quaternions_from_pqs(pqs)\n pqs2 = ptr.pqs_from_dual_quaternions(dqs)\n for pq, pq2 in zip(pqs.reshape(-1, 7), pqs2.reshape(-1, 7)):\n assert_array_almost_equal(pq[:3], pq2[:3])\n pr.assert_quaternion_equal(pq[3:], pq2[3:])\n\n\ndef test_batch_concatenate_dual_quaternions_0dims():\n rng = np.random.default_rng(847)\n pqs = rng.standard_normal(size=(2, 7))\n pqs[..., 3:] /= np.linalg.norm(pqs[..., 3:], axis=-1)[..., np.newaxis]\n dqs = ptr.dual_quaternions_from_pqs(pqs)\n dq1 = dqs[0]\n dq2 = dqs[1]\n\n assert_array_almost_equal(\n ptr.batch_concatenate_dual_quaternions(dq1, dq2),\n pt.concatenate_dual_quaternions(dq1, dq2),\n )\n\n\ndef test_batch_concatenate_dual_quaternions_2dims():\n rng = np.random.default_rng(848)\n pqs = rng.standard_normal(size=(2, 2, 2, 7))\n pqs[..., 3:] /= np.linalg.norm(pqs[..., 3:], axis=-1)[..., np.newaxis]\n dqs = ptr.dual_quaternions_from_pqs(pqs)\n dqs1 = dqs[0]\n dqs2 = dqs[1]\n\n dqs_result = ptr.batch_concatenate_dual_quaternions(dqs1, dqs2)\n for dq_result, dq1, dq2 in zip(\n dqs_result.reshape(-1, 8), dqs1.reshape(-1, 8), dqs2.reshape(-1, 8)\n ):\n assert_array_almost_equal(\n pt.concatenate_dual_quaternions(dq1, dq2), dq_result\n )\n\n\ndef test_batch_dual_quaternion_vector_product_0dims():\n rng = np.random.default_rng(849)\n pq = rng.standard_normal(size=7)\n pq[3:] /= np.linalg.norm(pq[3:], axis=-1)[..., np.newaxis]\n dq = ptr.dual_quaternions_from_pqs(pq)\n v = rng.standard_normal(size=3)\n\n assert_array_almost_equal(\n ptr.batch_dq_prod_vector(dq, v), pt.dq_prod_vector(dq, v)\n )\n\n\ndef test_batch_dual_quaternion_vector_product_2dims():\n rng = np.random.default_rng(850)\n pqs = rng.standard_normal(size=(3, 4, 7))\n pqs[..., 3:] /= np.linalg.norm(pqs[..., 3:], axis=-1)[..., np.newaxis]\n dqs = ptr.dual_quaternions_from_pqs(pqs)\n V = rng.standard_normal(size=(3, 4, 3))\n\n V_transformed = ptr.batch_dq_prod_vector(dqs, V)\n assert V_transformed.shape == (3, 4, 3)\n for v_t, dq, v in zip(\n V_transformed.reshape(-1, 3), dqs.reshape(-1, 8), V.reshape(-1, 3)\n ):\n assert_array_almost_equal(v_t, pt.dq_prod_vector(dq, v))\n\n\ndef test_batch_conversions_dual_quaternions_transforms_0dims():\n rng = np.random.default_rng(851)\n pq = rng.standard_normal(size=7)\n pq[3:] /= np.linalg.norm(pq[3:], axis=-1)[..., np.newaxis]\n dq = ptr.dual_quaternions_from_pqs(pq)\n\n A2B = ptr.transforms_from_dual_quaternions(dq)\n dq2 = ptr.dual_quaternions_from_transforms(A2B)\n pt.assert_unit_dual_quaternion_equal(dq, dq2)\n A2B2 = ptr.transforms_from_dual_quaternions(dq2)\n assert_array_almost_equal(A2B, A2B2)\n\n\ndef test_batch_conversions_dual_quaternions_transforms_3dims():\n rng = np.random.default_rng(852)\n pqs = rng.standard_normal(size=(3, 4, 5, 7))\n pqs[..., 3:] /= np.linalg.norm(pqs[..., 3:], axis=-1)[..., np.newaxis]\n dqs = ptr.dual_quaternions_from_pqs(pqs)\n\n A2Bs = ptr.transforms_from_dual_quaternions(dqs)\n dqs2 = ptr.dual_quaternions_from_transforms(A2Bs)\n for dq1, dq2 in zip(dqs.reshape(-1, 8), dqs2.reshape(-1, 8)):\n pt.assert_unit_dual_quaternion_equal(dq1, dq2)\n A2Bs2 = ptr.transforms_from_dual_quaternions(dqs2)\n assert_array_almost_equal(A2Bs, A2Bs2)\n\n\ndef test_mirror_screw_axis():\n pose = np.array(\n [\n [0.10156069, -0.02886784, 0.99441042, 0.6753021],\n [-0.4892026, -0.87182166, 0.02465395, -0.2085889],\n [0.86623683, -0.48897203, -0.10266503, 0.30462221],\n [0.0, 0.0, 0.0, 1.0],\n ]\n )\n exponential_coordinates = pt.exponential_coordinates_from_transform(pose)\n mirror_exponential_coordinates = ptr.mirror_screw_axis_direction(\n exponential_coordinates.reshape(1, 6)\n )[0]\n pose2 = pt.transform_from_exponential_coordinates(\n mirror_exponential_coordinates\n )\n assert_array_almost_equal(pose, pose2)\n\n\ndef test_screw_parameters_from_dual_quaternions():\n case_idx0 = np.array([1, 0, 0, 0, 0, 0, 0, 0])\n case_idx1 = np.array([[1.0, 0.0, 0.0, 0.0, 0.0, 0.6, 0.65, 0.7]])\n\n dqs = np.vstack([case_idx0, case_idx1])\n q, s_axis, h, theta = ptr.screw_parameters_from_dual_quaternions(dqs)\n\n assert_array_almost_equal(q[0], np.zeros(3))\n assert_array_almost_equal(q[0], np.zeros(3))\n assert_array_almost_equal(s_axis[0], np.array([1, 0, 0]))\n assert np.isinf(h[0])\n assert pytest.approx(theta[0]) == 0\n\n assert_array_almost_equal(q[1], np.zeros(3))\n assert_array_almost_equal(\n s_axis[1], pbr.norm_vectors(np.array([1.2, 1.3, 1.4]))\n )\n assert np.isinf(h[1])\n assert pytest.approx(theta[1]) == np.linalg.norm(np.array([1.2, 1.3, 1.4]))\n\n rng = np.random.default_rng(83343)\n dqs = ptr.dual_quaternions_from_transforms(\n np.array([pt.random_transform(rng) for _ in range(10)])\n ).reshape(5, 2, -1)\n qs, s_axes, hs, thetas = ptr.screw_parameters_from_dual_quaternions(dqs)\n for q, s_axis, h, theta, dq in zip(\n qs.reshape(10, -1),\n s_axes.reshape(10, -1),\n hs.reshape(10),\n thetas.reshape(10),\n dqs.reshape(10, -1),\n ):\n q2, s_axis2, h2, theta2 = pt.screw_parameters_from_dual_quaternion(dq)\n assert_array_almost_equal(q, q2)\n assert_array_almost_equal(s_axis, s_axis2)\n assert pytest.approx(h) == h2\n assert pytest.approx(theta) == theta2\n\n q, s_axis, h, theta = ptr.screw_parameters_from_dual_quaternions(case_idx0)\n q2, s_axis2, h2, theta2 = pt.screw_parameters_from_dual_quaternion(\n case_idx0\n )\n assert_array_almost_equal(q, q2)\n assert_array_almost_equal(s_axis, s_axis2)\n assert pytest.approx(h) == h2\n assert pytest.approx(theta) == theta2\n\n\ndef test_dual_quaternions_from_screw_parameters():\n q_0 = np.zeros(3)\n s_axis_0 = np.array([1, 0, 0])\n h_0 = np.inf\n theta_0 = 0.0\n\n q_1 = np.zeros(3)\n s_axis_1 = np.array([0.55297409, 0.57701644, 0.6010588])\n h_1 = np.inf\n theta_1 = 3.6\n\n q_2 = np.zeros(3)\n s_axis_2 = np.array([0.55396089, 0.5770426, 0.6001243])\n h_2 = 0.0\n theta_2 = 4.1\n\n qs = np.vstack([q_0, q_1, q_2])\n s_axis = np.vstack([s_axis_0, s_axis_1, s_axis_2])\n hs = np.array([h_0, h_1, h_2])\n thetas = np.array([theta_0, theta_1, theta_2])\n\n dqs = ptr.dual_quaternions_from_screw_parameters(qs, s_axis, hs, thetas)\n pqs = ptr.pqs_from_dual_quaternions(dqs)\n\n assert_array_almost_equal(dqs[0], np.array([1, 0, 0, 0, 0, 0, 0, 0]))\n assert_array_almost_equal(pqs[1], np.r_[s_axis[1] * thetas[1], 1, 0, 0, 0])\n assert_array_almost_equal(pqs[2, :3], [0, 0, 0])\n\n rng = np.random.default_rng(83323)\n dqs = np.array(\n [\n ptr.dual_quaternions_from_transforms(pt.random_transform(rng))\n for _ in range(18)\n ]\n )\n\n # 1D\n screw_parameters = ptr.screw_parameters_from_dual_quaternions(dqs[0])\n pt.assert_unit_dual_quaternion_equal(\n dqs[0], ptr.dual_quaternions_from_screw_parameters(*screw_parameters)\n )\n\n # 2D\n screw_parameters = ptr.screw_parameters_from_dual_quaternions(dqs)\n dqs2 = ptr.dual_quaternions_from_screw_parameters(*screw_parameters)\n for dq1, dq2 in zip(dqs, dqs2):\n pt.assert_unit_dual_quaternion_equal(dq1, dq2)\n\n # 3D\n dqs = dqs.reshape(6, 3, 8)\n screw_parameters = ptr.screw_parameters_from_dual_quaternions(dqs)\n dqs2 = ptr.dual_quaternions_from_screw_parameters(*screw_parameters)\n for dq1, dq2 in zip(dqs.reshape(-1, 8), dqs2.reshape(-1, 8)):\n pt.assert_unit_dual_quaternion_equal(dq1, dq2)\n\n\ndef test_dual_quaternions_sclerp_same_dual_quaternions():\n rng = np.random.default_rng(19)\n\n pose0 = pt.random_transform(rng)\n dq0 = pt.dual_quaternion_from_transform(pose0)\n pose1 = pt.random_transform(rng)\n dq1 = pt.dual_quaternion_from_transform(pose1)\n dqs = np.vstack([dq0, dq1])\n\n ts = np.array([0.5, 0.8])\n\n dqs_res = ptr.dual_quaternions_sclerp(dqs, dqs, ts)\n\n assert_array_almost_equal(dqs, dqs_res)\n\n with pytest.raises(ValueError, match=\"must have the same shape\"):\n ptr.dual_quaternions_sclerp(dqs, dqs[:-1], ts)\n\n with pytest.raises(ValueError, match=\"same number of elements\"):\n ptr.dual_quaternions_sclerp(dqs, dqs, ts[:-1])\n\n with pytest.raises(ValueError, match=\"same number of elements\"):\n ptr.dual_quaternions_sclerp(dqs, dqs, ts[0])\n\n\ndef test_dual_quaternions_sclerp():\n rng = np.random.default_rng(4832238)\n Ts = np.array([pt.random_transform(rng) for _ in range(20)])\n\n # 3D\n dqs_start = ptr.dual_quaternions_from_transforms(Ts).reshape(5, 4, -1)\n dqs_end = ptr.dual_quaternions_from_transforms(Ts).reshape(5, 4, -1)\n ts = np.linspace(0, 1, 20).reshape(5, 4)\n dqs_int = ptr.dual_quaternions_sclerp(dqs_start, dqs_end, ts)\n assert dqs_int.shape == (5, 4, 8)\n for dq_start, dq_end, t, dq_int in zip(\n dqs_start.reshape(-1, 8),\n dqs_end.reshape(-1, 8),\n ts.reshape(-1),\n dqs_int.reshape(-1, 8),\n ):\n pt.assert_unit_dual_quaternion_equal(\n dq_int, pt.dual_quaternion_sclerp(dq_start, dq_end, t)\n )\n\n # 1D\n pt.assert_unit_dual_quaternion_equal(\n pt.dual_quaternion_sclerp(dqs_start[0, 0], dqs_end[0, 0], ts[0, 0]),\n ptr.dual_quaternions_sclerp(dqs_start[0, 0], dqs_end[0, 0], ts[0, 0]),\n )\n\n\ndef test_batch_dq_q_conj():\n rng = np.random.default_rng(48338)\n Ts = np.array([pt.random_transform(rng) for _ in range(20)])\n dqs = ptr.dual_quaternions_from_transforms(Ts).reshape(5, 4, -1)\n\n # 1D\n assert_array_almost_equal(\n pt.transform_from_dual_quaternion(\n pt.concatenate_dual_quaternions(\n dqs[0, 0], ptr.batch_dq_q_conj(dqs[0, 0])\n )\n ),\n np.eye(4),\n )\n\n # 3D\n dqs_inv = ptr.batch_dq_q_conj(dqs)\n dqs_id = ptr.batch_concatenate_dual_quaternions(dqs, dqs_inv)\n Is = ptr.transforms_from_dual_quaternions(dqs_id)\n for I in Is.reshape(-1, 4, 4):\n assert_array_almost_equal(I, np.eye(4))\n\n\ndef test_random_trajectories():\n rng = np.random.default_rng(3427)\n start = pt.random_transform(rng)\n goal = pt.random_transform(rng)\n\n trajectories = ptr.random_trajectories(\n rng,\n n_trajectories=7,\n n_steps=132,\n start=start,\n goal=goal,\n scale=[100] * 6,\n )\n assert trajectories.shape == (7, 132, 4, 4)\n assert_array_almost_equal(trajectories[0, 0], start, decimal=2)\n assert_array_almost_equal(trajectories[0, -1], goal, decimal=2)\n\n # check scaling, we assume start and goal are equal so that the linear\n # movement from start to goal does not interfere with the random motion\n rng = np.random.default_rng(3429)\n trajectories1 = ptr.random_trajectories(\n rng, n_trajectories=3, n_steps=20, scale=[1.0] * 6\n )\n rng = np.random.default_rng(3429)\n trajectories2 = ptr.random_trajectories(\n rng, n_trajectories=3, n_steps=20, scale=[2.0] * 6\n )\n Sthetas1 = ptr.exponential_coordinates_from_transforms(trajectories1)\n acc1 = np.abs(\n np.gradient(\n np.gradient(Sthetas1, axis=1, edge_order=1), axis=1, edge_order=1\n )\n )\n Sthetas2 = ptr.exponential_coordinates_from_transforms(trajectories2)\n acc2 = np.abs(\n np.gradient(\n np.gradient(Sthetas2, axis=1, edge_order=1), axis=1, edge_order=1\n )\n )\n assert np.all(acc1 <= acc2)\n" }, { "path": "pytransform3d/transform_manager/__init__.py", "content": "\"\"\"Manage complex chains of transformations.\n\nSee :doc:`user_guide/transform_manager` for more information.\n\"\"\"\n\nfrom ._transform_graph_base import TransformGraphBase\nfrom ._transform_manager import TransformManager\nfrom ._temporal_transform_manager import (\n StaticTransform,\n TimeVaryingTransform,\n TemporalTransformManager,\n NumpyTimeseriesTransform,\n)\n\n\n__all__ = [\n \"TransformGraphBase\",\n \"TransformManager\",\n \"TemporalTransformManager\",\n \"TimeVaryingTransform\",\n \"StaticTransform\",\n \"NumpyTimeseriesTransform\",\n]\n" }, { "path": "pytransform3d/transform_manager/_temporal_transform_manager.py", "content": "\"\"\"Temporal transform manager.\"\"\"\n\nimport abc\n\nimport numpy as np\n\nfrom ._transform_graph_base import TransformGraphBase\nfrom ..batch_rotations import norm_vectors\nfrom ..trajectories import (\n dual_quaternions_from_pqs,\n pqs_from_dual_quaternions,\n dual_quaternions_sclerp,\n transforms_from_pqs,\n concat_dynamic,\n invert_transforms,\n)\nfrom ..transformations import check_transform\n\n\nclass TimeVaryingTransform(abc.ABC):\n \"\"\"Time-varying rigid transformation.\n\n You have to inherit from this abstract base class to use the\n TemporalTransformManager. Two implementations of the interface that\n are already available are :class:`StaticTransform` and\n :class:`NumpyTimeseriesTransform`.\n \"\"\"\n\n @abc.abstractmethod\n def as_matrix(self, query_time):\n \"\"\"Get transformation matrix at given time.\n\n Parameters\n ----------\n query_time : Union[float, array-like shape (n_queries,)]\n Query time\n\n Returns\n -------\n A2B_t : array, shape (4, 4) or (n_queries, 4, 4)\n Homogeneous transformation matrix at given time or times\n \"\"\"\n\n @abc.abstractmethod\n def check_transforms(self):\n \"\"\"Checks all transformations.\n\n Returns\n -------\n self : TimeVaryingTransform\n Validated transformations.\n \"\"\"\n\n\nclass StaticTransform(TimeVaryingTransform):\n \"\"\"Transformation, which does not change over time.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Homogeneous transformation matrix.\n \"\"\"\n\n def __init__(self, A2B):\n self._A2B = A2B\n\n def as_matrix(self, query_time):\n return self._A2B\n\n def check_transforms(self):\n self._A2B = check_transform(self._A2B)\n return self\n\n\nclass NumpyTimeseriesTransform(TimeVaryingTransform):\n \"\"\"Transformation sequence, represented in a numpy array.\n\n The interpolation is computed using screw linear interpolation (ScLERP)\n method.\n\n Parameters\n ----------\n time: array, shape (n_steps,)\n Numeric timesteps corresponding to the transformation samples.\n You can use, for example, unix timestamps, relative time (starting\n with 0).\n\n pqs : array, shape (n_steps, 7)\n Time-sequence of transformations, with each row representing a single\n sample as position-quarternion (PQ) structure.\n\n time_clipping : bool, optional (default: False)\n Clip time to minimum or maximum respectively when query time is out of\n range of the time series. If this deactivated, we will raise a\n ValueError when the query time is out of range.\n \"\"\"\n\n def __init__(self, time, pqs, time_clipping=False):\n self.time = np.asarray(time)\n self._pqs = np.asarray(pqs)\n self.time_clipping = time_clipping\n\n if len(self._pqs.shape) != 2:\n raise ValueError(\"Shape of PQ array must be 2-dimensional.\")\n\n if self.time.size != self._pqs.shape[0]:\n raise ValueError(\n \"Number of timesteps does not equal to number of PQ samples\"\n )\n\n if self._pqs.shape[1] != 7:\n raise ValueError(\"`pqs` matrix shall have 7 columns.\")\n\n self._min_time = min(self.time)\n self._max_time = max(self.time)\n\n def as_matrix(self, query_time):\n \"\"\"Get transformation matrix at given time.\n\n Parameters\n ----------\n query_time : Union[float, array-like shape (...)]\n Query time\n\n Returns\n -------\n A2B_t : array, shape (4, 4) or (..., 4, 4)\n Homogeneous transformation matrix / matrices at given time / times.\n \"\"\"\n query_time = np.asarray(query_time)\n pq = self._interpolate_pq_using_sclerp(np.atleast_1d(query_time))\n transforms = transforms_from_pqs(pq)\n output_shape = list(query_time.shape) + [4, 4]\n return transforms.reshape(*output_shape)\n\n def check_transforms(self):\n self._pqs[:, 3:] = norm_vectors(self._pqs[:, 3:])\n return self\n\n def _interpolate_pq_using_sclerp(self, query_time_arr):\n # identify the index of the preceding sample\n idxs_timestep_earlier_wrt_query_time = (\n np.searchsorted(self.time, query_time_arr, side=\"right\") - 1\n )\n\n # deal with first and last timestamp\n before_start = query_time_arr < self._min_time\n after_end = query_time_arr > self._max_time\n out_of_range = np.logical_or(before_start, after_end)\n if self.time_clipping:\n min_index = 0\n max_index = self.time.shape[0] - 2\n idxs_timestep_earlier_wrt_query_time = np.clip(\n idxs_timestep_earlier_wrt_query_time, min_index, max_index\n )\n elif any(out_of_range):\n indices = np.where(out_of_range)[0]\n times = query_time_arr[indices]\n raise ValueError(\n \"Query time at indices %s, time(s): %s is/are out of range of \"\n \"time series.\" % (indices, times)\n )\n\n idxs_timestep_later_wrt_query_time = (\n idxs_timestep_earlier_wrt_query_time + 1\n )\n if self.time_clipping:\n before_or_eq_start = query_time_arr <= self._min_time\n after_or_eq_end = query_time_arr >= self._max_time\n idxs_timestep_later_wrt_query_time[before_or_eq_start] = (\n idxs_timestep_earlier_wrt_query_time[before_or_eq_start]\n )\n idxs_timestep_earlier_wrt_query_time[after_or_eq_end] = (\n idxs_timestep_later_wrt_query_time[after_or_eq_end]\n )\n\n # dual quaternion from preceding sample\n times_prev = self.time[idxs_timestep_earlier_wrt_query_time]\n pqs_prev = self._pqs[idxs_timestep_earlier_wrt_query_time]\n dqs_prev = dual_quaternions_from_pqs(pqs_prev)\n\n # dual quaternion from successive sample\n times_next = self.time[idxs_timestep_later_wrt_query_time]\n pqs_next = self._pqs[idxs_timestep_later_wrt_query_time]\n dqs_next = dual_quaternions_from_pqs(pqs_next)\n\n # For each query timestamp, determine its normalized time distance\n # between the preceeding and succeeding sample, since ScLERP works with\n # relative times t in [0, 1]. If query time equals a time sample\n # exactly, 0.0 is set.\n rel_times_between_samples = np.zeros_like(query_time_arr, dtype=float)\n interpolation_case = times_prev != times_next\n rel_times_between_samples[interpolation_case] = (\n query_time_arr[interpolation_case] - times_prev[interpolation_case]\n ) / (times_next[interpolation_case] - times_prev[interpolation_case])\n dqs_interpolated = dual_quaternions_sclerp(\n dqs_prev, dqs_next, rel_times_between_samples\n )\n return pqs_from_dual_quaternions(dqs_interpolated)\n\n\nclass TemporalTransformManager(TransformGraphBase):\n \"\"\"Manage time-varying transformations.\n\n See :ref:`transformations_over_time` for more information.\n\n Parameters\n ----------\n strict_check : bool, optional (default: True)\n Raise a ValueError if the transformation matrix is not numerically\n close enough to a real transformation matrix. Otherwise we print a\n warning.\n\n check : bool, optional (default: True)\n Check if transformation matrices are valid and requested nodes exist,\n which might significantly slow down some operations.\n \"\"\"\n\n def __init__(self, strict_check=True, check=True):\n super(TemporalTransformManager, self).__init__(strict_check, check)\n self._transforms = {}\n self._current_time = np.array([0.0])\n\n @property\n def current_time(self):\n \"\"\"Current time at which we evaluate transformations.\"\"\"\n return self._current_time\n\n @current_time.setter\n def current_time(self, time):\n \"\"\"Set current time at which we evaluate transformations.\"\"\"\n self._current_time = time\n\n @property\n def transforms(self):\n \"\"\"Rigid transformations between nodes.\"\"\"\n return {\n transform_key: transform.as_matrix(self.current_time)\n for transform_key, transform in self._transforms.items()\n }\n\n def get_transform_at_time(self, from_frame, to_frame, time):\n \"\"\"Request a transformation at a given time.\n\n Parameters\n ----------\n from_frame : Hashable\n Name of the frame for which the transformation is requested in the\n to_frame coordinate system\n\n to_frame : Hashable\n Name of the frame in which the transformation is defined\n\n time : Union[float, array-like shape (...)]\n Time or times at which we request the transformation. If the query\n time is out of bounds, it will be clipped to either the first or\n last available time.\n\n Returns\n -------\n A2B : array, shape (4, 4)\n Transformation from 'from_frame' to 'to_frame'\n\n Raises\n ------\n KeyError\n If one of the frames is unknown or there is no connection between\n them\n \"\"\"\n previous_time = self.current_time\n self.current_time = time\n\n A2B = self.get_transform(from_frame, to_frame)\n\n # revert internal state\n self.current_time = previous_time\n return A2B\n\n def get_transform(self, from_frame, to_frame):\n \"\"\"Request a transformation.\n\n The internal current_time will be used for time based transformations.\n If the query time is out of bounds, it will be clipped to either the\n first or the last available time.\n\n Parameters\n ----------\n from_frame : Hashable\n Name of the frame for which the transformation is requested in the\n to_frame coordinate system\n\n to_frame : Hashable\n Name of the frame in which the transformation is defined\n\n Returns\n -------\n A2B : Any\n Transformation from 'from_frame' to 'to_frame'\n\n Raises\n ------\n KeyError\n If one of the frames is unknown or there is no connection between\n them\n \"\"\"\n if self.check:\n if from_frame not in self.nodes:\n raise KeyError(\"Unknown frame '%s'\" % from_frame)\n if to_frame not in self.nodes:\n raise KeyError(\"Unknown frame '%s'\" % to_frame)\n\n if self._transform_available((from_frame, to_frame)):\n return self._get_transform((from_frame, to_frame))\n\n if self._transform_available((to_frame, from_frame)):\n return invert_transforms(\n self._get_transform((to_frame, from_frame))\n )\n\n i = self.nodes.index(from_frame)\n j = self.nodes.index(to_frame)\n if not np.isfinite(self.dist[i, j]):\n raise KeyError(\n \"Cannot compute path from frame '%s' to frame '%s'.\"\n % (from_frame, to_frame)\n )\n\n path = self._shortest_path(i, j)\n return self._path_transform(path)\n\n def _transform_available(self, key):\n return key in self._transforms\n\n def _set_transform(self, key, A2B):\n self._transforms[key] = A2B\n\n def _get_transform(self, key):\n return self._transforms[key].as_matrix(self._current_time)\n\n def _del_transform(self, key):\n del self._transforms[key]\n\n def _check_transform(self, A2B):\n return A2B.check_transforms()\n\n def _path_transform(self, path):\n \"\"\"Convert sequence of node names to rigid transformation.\"\"\"\n A2B = np.eye(4)\n for from_f, to_f in zip(path[:-1], path[1:]):\n A2B = concat_dynamic(A2B, self.get_transform(from_f, to_f))\n return A2B\n" }, { "path": "pytransform3d/transform_manager/_temporal_transform_manager.pyi", "content": "import abc\nfrom typing import Dict, Tuple, Hashable, Any, Union\n\nimport numpy as np\nimport numpy.typing as npt\n\nfrom ._transform_graph_base import TransformGraphBase\n\nclass TimeVaryingTransform(abc.ABC):\n @abc.abstractmethod\n def as_matrix(self, query_time: Union[float, np.ndarray]) -> np.ndarray: ...\n @abc.abstractmethod\n def check_transforms(self) -> \"TimeVaryingTransform\": ...\n\nclass StaticTransform(TimeVaryingTransform):\n _A2B: npt.ArrayLike\n\n def __init__(self, A2B: npt.ArrayLike): ...\n def as_matrix(self, query_time: Union[float, np.ndarray]) -> np.ndarray: ...\n def check_transforms(self) -> \"StaticTransform\": ...\n\nclass NumpyTimeseriesTransform(TimeVaryingTransform):\n time: np.ndarray\n _pqs: np.ndarray\n time_clipping: bool\n _min_time: float\n _max_time: float\n\n def __init__(\n self, time: npt.ArrayLike, pqs: npt.ArrayLike, time_clipping: bool = ...\n ): ...\n def as_matrix(self, query_time: Union[float, np.ndarray]) -> np.ndarray: ...\n def check_transforms(self) -> \"NumpyTimeseriesTransform\": ...\n def _interpolate_pq_using_sclerp(\n self, query_time: Union[float, np.ndarray]\n ) -> np.ndarray: ...\n\nclass TemporalTransformManager(TransformGraphBase):\n _transforms: Dict[Tuple[Hashable, Hashable], TimeVaryingTransform]\n _current_time: Union[float, np.ndarray]\n\n def __init__(self, strict_check: bool = ..., check: bool = ...): ...\n @property\n def transforms(self) -> Dict[Tuple[Hashable, Hashable], np.ndarray]: ...\n @property\n def current_time(self) -> Union[float, np.ndarray]: ...\n @current_time.setter\n def current_time(self, query_time: Union[float, np.ndarray]): ...\n def _check_transform(\n self, A2B: TimeVaryingTransform\n ) -> TimeVaryingTransform: ...\n def _transform_available(self, key: Tuple[Hashable, Hashable]) -> bool: ...\n def _set_transform(\n self, key: Tuple[Hashable, Hashable], A2B: TimeVaryingTransform\n ): ...\n def _get_transform(self, key: Tuple[Hashable, Hashable]) -> Any: ...\n def _del_transform(self, key: Tuple[Hashable, Hashable]): ...\n def add_transform(\n self,\n from_frame: Hashable,\n to_frame: Hashable,\n A2B: TimeVaryingTransform,\n ) -> \"TemporalTransformManager\": ...\n def get_transform(\n self, from_frame: Hashable, to_frame: Hashable\n ) -> np.ndarray: ...\n def get_transform_at_time(\n self,\n from_frame: Hashable,\n to_frame: Hashable,\n query_time: Union[float, npt.ArrayLike],\n ) -> np.ndarray: ...\n" }, { "path": "pytransform3d/transform_manager/_transform_graph_base.py", "content": "\"\"\"Common base class of transformation graphs.\"\"\"\n\nimport abc\nimport copy\n\nimport numpy as np\nimport scipy.sparse as sp\nfrom scipy.sparse import csgraph\n\nfrom ..transformations import concat, invert_transform\n\n\nclass TransformGraphBase(abc.ABC):\n \"\"\"Base class for all graphs of rigid transformations.\n\n Parameters\n ----------\n strict_check : bool, optional (default: True)\n Raise a ValueError if the transformation matrix is not numerically\n close enough to a real transformation matrix. Otherwise we print a\n warning.\n\n check : bool, optional (default: True)\n Check if transformation matrices are valid and requested nodes exist,\n which might significantly slow down some operations.\n \"\"\"\n\n def __init__(self, strict_check=True, check=True):\n self.strict_check = strict_check\n self.check = check\n\n # Names of nodes\n self.nodes = []\n\n # A pair (self.i[n], self.j[n]) represents indices of connected nodes\n self.i = []\n self.j = []\n # We have to store the index n associated to a transformation to be\n # able to remove the transformation later\n self.transform_to_ij_index = {}\n # Connection information as sparse matrix\n self.connections = sp.csr_matrix((0, 0))\n # Result of shortest path algorithm:\n # distance matrix (distance is the number of transformations)\n self.dist = np.empty(0)\n self.predecessors = np.empty(0, dtype=np.int32)\n\n self._cached_shortest_paths = {}\n\n @property\n @abc.abstractmethod\n def transforms(self):\n \"\"\"Rigid transformations between nodes.\"\"\"\n\n @abc.abstractmethod\n def _check_transform(self, A2B):\n \"\"\"Check validity of rigid transformation.\"\"\"\n\n def _path_transform(self, path):\n \"\"\"Convert sequence of node names to rigid transformation.\"\"\"\n A2B = np.eye(4)\n for from_f, to_f in zip(path[:-1], path[1:]):\n A2B = concat(\n A2B,\n self.get_transform(from_f, to_f),\n strict_check=self.strict_check,\n check=self.check,\n )\n return A2B\n\n @abc.abstractmethod\n def _transform_available(self, key):\n \"\"\"Check if transformation key is available.\"\"\"\n\n @abc.abstractmethod\n def _set_transform(self, key, A2B):\n \"\"\"Store transformation under given key.\"\"\"\n\n @abc.abstractmethod\n def _get_transform(self, key):\n \"\"\"Retrieve stored transformation under given key.\"\"\"\n\n @abc.abstractmethod\n def _del_transform(self, key):\n \"\"\"Delete transformation stored under given key.\"\"\"\n\n def has_frame(self, frame):\n \"\"\"Check if frame has been registered.\n\n Parameters\n ----------\n frame : Hashable\n Frame name\n\n Returns\n -------\n has_frame : bool\n Frame is registered\n \"\"\"\n return frame in self.nodes\n\n def add_transform(self, from_frame, to_frame, A2B):\n \"\"\"Register a transformation.\n\n Parameters\n ----------\n from_frame : Hashable\n Name of the frame for which the transformation is added in the\n to_frame coordinate system\n\n to_frame : Hashable\n Name of the frame in which the transformation is defined\n\n A2B : Any\n Transformation from 'from_frame' to 'to_frame'\n\n Returns\n -------\n self : TransformManager\n This object for chaining\n \"\"\"\n if self.check:\n A2B = self._check_transform(A2B)\n\n if from_frame not in self.nodes:\n self.nodes.append(from_frame)\n if to_frame not in self.nodes:\n self.nodes.append(to_frame)\n\n transform_key = (from_frame, to_frame)\n\n recompute_shortest_path = False\n if not self._transform_available(transform_key):\n ij_index = len(self.i)\n self.i.append(self.nodes.index(from_frame))\n self.j.append(self.nodes.index(to_frame))\n self.transform_to_ij_index[transform_key] = ij_index\n recompute_shortest_path = True\n\n if recompute_shortest_path:\n self._recompute_shortest_path()\n\n self._set_transform(transform_key, A2B)\n\n return self\n\n def _recompute_shortest_path(self):\n n_nodes = len(self.nodes)\n self.connections = sp.csr_matrix(\n (np.zeros(len(self.i)), (self.i, self.j)), shape=(n_nodes, n_nodes)\n )\n self.dist, self.predecessors = csgraph.shortest_path(\n self.connections,\n unweighted=True,\n directed=False,\n method=\"D\",\n return_predecessors=True,\n )\n self._cached_shortest_paths.clear()\n\n def _find_connected_transforms(self, frame):\n \"\"\"Find all transformations connected to a frame.\"\"\"\n connected_transforms = []\n for from_frame, to_frame in self.transform_to_ij_index.keys():\n if from_frame == frame or to_frame == frame:\n connected_transforms.append((from_frame, to_frame))\n return connected_transforms\n\n def remove_transform(self, from_frame, to_frame):\n \"\"\"Remove a transformation.\n\n Nothing happens if there is no such transformation.\n\n Parameters\n ----------\n from_frame : Hashable\n Name of the frame for which the transformation is added in the\n to_frame coordinate system\n\n to_frame : Hashable\n Name of the frame in which the transformation is defined\n\n Returns\n -------\n self : TransformManager\n This object for chaining\n \"\"\"\n transform_key = (from_frame, to_frame)\n if self._transform_available(transform_key):\n self._del_transform(transform_key)\n ij_index = self.transform_to_ij_index.pop(transform_key)\n self.transform_to_ij_index = {\n k: v if v < ij_index else v - 1\n for k, v in self.transform_to_ij_index.items()\n }\n del self.i[ij_index], self.j[ij_index]\n self._recompute_shortest_path()\n return self\n\n def remove_frame(self, frame):\n \"\"\"Remove a frame (node) from the graph.\n\n Parameters\n ----------\n frame : Hashable\n The frame to remove.\n\n Returns\n -------\n self : TransformManager\n This object for chaining.\n \"\"\"\n if frame not in self.nodes:\n raise KeyError(f\"Frame '{frame}' is not in the graph.\")\n\n # Remove all transformations (edges) associated with the frame\n for from_frame, to_frame in self._find_connected_transforms(frame):\n self.remove_transform(from_frame, to_frame)\n\n frame_index = self.nodes.index(frame)\n self.nodes.pop(frame_index)\n\n # Adjust the connection indices in self.i and self.j\n self.i = [\n index if index < frame_index else index - 1 for index in self.i\n ]\n self.j = [\n index if index < frame_index else index - 1 for index in self.j\n ]\n\n # Update the transform_to_ij_index dictionary\n self.transform_to_ij_index = {\n nodes: ij_index\n for nodes, ij_index in self.transform_to_ij_index.items()\n if frame not in nodes\n }\n\n self._recompute_shortest_path()\n\n return self\n\n def get_transform(self, from_frame, to_frame):\n \"\"\"Request a transformation.\n\n Parameters\n ----------\n from_frame : Hashable\n Name of the frame for which the transformation is requested in the\n to_frame coordinate system\n\n to_frame : Hashable\n Name of the frame in which the transformation is defined\n\n Returns\n -------\n A2B : Any\n Transformation from 'from_frame' to 'to_frame'\n\n Raises\n ------\n KeyError\n If one of the frames is unknown or there is no connection between\n them\n \"\"\"\n if self.check:\n if from_frame not in self.nodes:\n raise KeyError(\"Unknown frame '%s'\" % from_frame)\n if to_frame not in self.nodes:\n raise KeyError(\"Unknown frame '%s'\" % to_frame)\n\n if self._transform_available((from_frame, to_frame)):\n return self._get_transform((from_frame, to_frame))\n\n if self._transform_available((to_frame, from_frame)):\n return invert_transform(\n self._get_transform((to_frame, from_frame)),\n strict_check=self.strict_check,\n check=self.check,\n )\n\n i = self.nodes.index(from_frame)\n j = self.nodes.index(to_frame)\n if not np.isfinite(self.dist[i, j]):\n raise KeyError(\n \"Cannot compute path from frame '%s' to \"\n \"frame '%s'.\" % (from_frame, to_frame)\n )\n\n path = self._shortest_path(i, j)\n return self._path_transform(path)\n\n def _shortest_path(self, i, j):\n \"\"\"Names of nodes along the shortest path between two indices.\"\"\"\n if (i, j) in self._cached_shortest_paths:\n return self._cached_shortest_paths[(i, j)]\n\n path = []\n k = i\n while k != -9999:\n path.append(self.nodes[k])\n k = self.predecessors[j, k]\n self._cached_shortest_paths[(i, j)] = path\n return path\n\n def connected_components(self):\n \"\"\"Get number of connected components.\n\n If the number is larger than 1 there will be frames without\n connections.\n\n Returns\n -------\n n_connected_components : int\n Number of connected components.\n \"\"\"\n return csgraph.connected_components(\n self.connections, directed=False, return_labels=False\n )\n\n def check_consistency(self):\n \"\"\"Check consistency of the known transformations.\n\n The computational cost of this operation is very high.\n\n Returns\n -------\n consistent : bool\n Is the graph consistent, i.e., if there are two ways of computing\n A2B, do they give almost identical results?\n \"\"\"\n for (from_frame, to_frame), A2B in self.transforms.items():\n clone = copy.deepcopy(self)\n clone.remove_transform(from_frame, to_frame)\n try:\n A2B_from_path = clone.get_transform(from_frame, to_frame)\n if not np.allclose(A2B, A2B_from_path):\n return False\n except KeyError:\n # A2B cannot be computed in any other way\n continue\n return True\n" }, { "path": "pytransform3d/transform_manager/_transform_graph_base.pyi", "content": "import abc\nfrom typing import Dict, Tuple, List, Hashable, Any\n\nimport scipy.sparse as sp\nimport numpy as np\nimport numpy.typing as npt\n\nclass TransformGraphBase(abc.ABC):\n strict_check: bool\n check: bool\n nodes: List[Hashable]\n i: List[int]\n j: List[int]\n transform_to_ij_index = Dict[Tuple[Hashable, Hashable], int]\n connections: sp.csr_matrix\n dist: np.ndarray\n predecessors: np.ndarray\n _cached_shortest_paths: Dict[Tuple[int, int], List[Hashable]]\n\n def __init__(self, strict_check: bool = ..., check: bool = ...): ...\n @property\n @abc.abstractmethod\n def transforms(self) -> Dict[Tuple[Hashable, Hashable], Any]: ...\n @abc.abstractmethod\n def _transform_available(self, key: Tuple[Hashable, Hashable]) -> bool: ...\n @abc.abstractmethod\n def _set_transform(self, key: Tuple[Hashable, Hashable], A2B: Any): ...\n @abc.abstractmethod\n def _get_transform(self, key: Tuple[Hashable, Hashable]) -> Any: ...\n @abc.abstractmethod\n def _del_transform(self, key: Tuple[Hashable, Hashable]): ...\n @abc.abstractmethod\n def _check_transform(self, A2B: Any) -> Any: ...\n def _path_transform(self, path: List[Hashable]) -> np.ndarray: ...\n def has_frame(self, frame: Hashable) -> bool: ...\n def add_transform(\n self, from_frame: Hashable, to_frame: Hashable, A2B: Any\n ) -> \"TransformGraphBase\": ...\n def _recompute_shortest_path(self): ...\n def _find_connected_transforms(self, frame: Hashable) -> List[Hashable]: ...\n def remove_transform(\n self, from_frame: Hashable, to_frame: Hashable\n ) -> \"TransformGraphBase\": ...\n def remove_frame(self, frame: Hashable) -> \"TransformGraphBase\": ...\n def get_transform(\n self, from_frame: Hashable, to_frame: Hashable\n ) -> np.ndarray: ...\n def _shortest_path(self, i: int, j: int) -> List[Hashable]: ...\n def connected_components(self) -> int: ...\n def check_consistency(self) -> bool: ...\n" }, { "path": "pytransform3d/transform_manager/_transform_manager.py", "content": "\"\"\"Transform manager.\"\"\"\n\nimport numpy as np\nimport scipy.sparse as sp\n\nfrom ._transform_graph_base import TransformGraphBase\nfrom ..transformations import check_transform, plot_transform\n\ntry: # pragma: no cover\n import pydot\n\n PYDOT_AVAILABLE = True\nexcept ImportError:\n PYDOT_AVAILABLE = False\n\n\nclass TransformManager(TransformGraphBase):\n \"\"\"Manage transformations between frames.\n\n This is a simplified version of `ROS tf `_ [1]_\n that ignores the temporal aspect. A user can register transformations. The\n shortest path between all frames will be computed internally which enables\n us to provide transforms for any connected frames.\n\n Suppose we know the transformations A2B, D2C, and B2C. The transform\n manager can compute any transformation between the frames A, B, C and D.\n For example, you can request the transformation that represents frame D in\n frame A. The transformation manager will automatically concatenate the\n transformations D2C, C2B, and B2A, where C2B and B2A are obtained by\n inverting B2C and A2B respectively.\n\n .. warning::\n\n It is possible to introduce inconsistencies in the transformation\n manager. Adding A2B and B2A with inconsistent values will result in\n an invalid state because inconsistencies will not be checked. It seems\n to be trivial in this simple case but can be computationally complex\n for large graphs. You can check the consistency explicitly with\n :func:`TransformManager.check_consistency`.\n\n The TransformManager does not directly support serialization because\n we don't want to decide for a specific format. However, it allows\n conversion to a dict with only primitive types that is serializable,\n for instance, as JSON. If a more compact format is required, binary\n formats like msgpack can be used. Use :func:`TransformManager.to_dict`\n and :func:`TransformManager.from_dict` for this purpose.\n\n Parameters\n ----------\n strict_check : bool, optional (default: True)\n Raise a ValueError if the transformation matrix is not numerically\n close enough to a real transformation matrix. Otherwise we print a\n warning.\n\n check : bool, optional (default: True)\n Check if transformation matrices are valid and requested nodes exist,\n which might significantly slow down some operations.\n\n References\n ----------\n .. [1] Foote, T. (2013). tf: The transform library. In IEEE Conference on\n Technologies for Practical Robot Applications (TePRA), Woburn, MA, USA,\n pp. 1-6, doi: 10.1109/TePRA.2013.6556373.\n \"\"\"\n\n def __init__(self, strict_check=True, check=True):\n super(TransformManager, self).__init__(strict_check, check)\n self._transforms = {}\n\n @property\n def transforms(self):\n \"\"\"Rigid transformations between nodes.\"\"\"\n return self._transforms\n\n def _check_transform(self, A2B):\n \"\"\"Check validity of rigid transformation.\"\"\"\n return check_transform(A2B, strict_check=self.strict_check)\n\n def _transform_available(self, key):\n return key in self._transforms\n\n def _set_transform(self, key, A2B):\n self._transforms[key] = A2B\n\n def _get_transform(self, key):\n return self._transforms[key]\n\n def _del_transform(self, key):\n del self._transforms[key]\n\n def plot_frames_in(\n self,\n frame,\n ax=None,\n s=1.0,\n ax_s=1,\n show_name=True,\n whitelist=None,\n **kwargs,\n ): # pragma: no cover\n \"\"\"Plot all frames in a given reference frame.\n\n Note that frames that cannot be connected to the reference frame are\n omitted.\n\n Parameters\n ----------\n frame : Hashable\n Reference frame\n\n ax : Matplotlib 3d axis, optional (default: None)\n If the axis is None, a new 3d axis will be created\n\n s : float, optional (default: 1)\n Scaling of the frame that will be drawn\n\n ax_s : float, optional (default: 1)\n Scaling of the new matplotlib 3d axis\n\n show_name : bool, optional (default: True)\n Print node names\n\n whitelist : list, optional (default: None)\n Frames that must be plotted\n\n kwargs : dict, optional (default: {})\n Additional arguments for the plotting functions, e.g. alpha\n\n Returns\n -------\n ax : Matplotlib 3d axis\n New or old axis\n\n Raises\n ------\n KeyError\n If the frame is unknown\n \"\"\"\n if frame not in self.nodes:\n raise KeyError(\"Unknown frame '%s'\" % frame)\n\n nodes = self._whitelisted_nodes(whitelist)\n\n for node in nodes:\n try:\n node2frame = self.get_transform(node, frame)\n name = node if show_name else None\n ax = plot_transform(\n ax,\n node2frame,\n s,\n ax_s,\n name,\n strict_check=self.strict_check,\n **kwargs,\n )\n except KeyError:\n pass # Frame is not connected to the reference frame\n return ax\n\n def plot_connections_in(\n self, frame, ax=None, ax_s=1, whitelist=None, **kwargs\n ): # pragma: no cover\n \"\"\"Plot direct frame connections in a given reference frame.\n\n A line between each pair of frames for which a direct transformation\n is known will be plotted. Direct means that either A2B or B2A has been\n added to the transformation manager.\n\n Note that frames that cannot be connected to the reference frame are\n omitted.\n\n Parameters\n ----------\n frame : Hashable\n Reference frame\n\n ax : Matplotlib 3d axis, optional (default: None)\n If the axis is None, a new 3d axis will be created\n\n ax_s : float, optional (default: 1)\n Scaling of the new matplotlib 3d axis\n\n whitelist : list, optional (default: None)\n Both frames of a connection must be in the whitelist to plot the\n connection\n\n kwargs : dict, optional (default: {})\n Additional arguments for the plotting functions, e.g. alpha\n\n Returns\n -------\n ax : Matplotlib 3d axis\n New or old axis\n\n Raises\n ------\n KeyError\n If the frame is unknown\n \"\"\"\n if frame not in self.nodes:\n raise KeyError(\"Unknown frame '%s'\" % frame)\n\n if ax is None:\n from ..plot_utils import make_3d_axis\n\n ax = make_3d_axis(ax_s)\n\n nodes = self._whitelisted_nodes(whitelist)\n\n if \"c\" not in kwargs and \"color\" not in kwargs:\n kwargs[\"color\"] = \"black\"\n\n for frame_names in self._transforms:\n from_frame, to_frame = frame_names\n if from_frame in nodes and to_frame in nodes:\n try:\n from2ref = self.get_transform(from_frame, frame)\n to2ref = self.get_transform(to_frame, frame)\n ax.plot(\n (from2ref[0, 3], to2ref[0, 3]),\n (from2ref[1, 3], to2ref[1, 3]),\n (from2ref[2, 3], to2ref[2, 3]),\n **kwargs,\n )\n except KeyError:\n pass # Frame is not connected to the reference frame\n\n return ax\n\n def _whitelisted_nodes(self, whitelist):\n \"\"\"Get whitelisted nodes.\n\n Parameters\n ----------\n whitelist : list or None\n Whitelist of frames\n\n Returns\n -------\n nodes : set\n Existing whitelisted nodes\n\n Raises\n ------\n KeyError\n Will be raised if an unknown node is in the whitelist.\n \"\"\"\n nodes = set(self.nodes)\n if whitelist is not None:\n whitelist = set(whitelist)\n nodes = nodes.intersection(whitelist)\n nonwhitlisted_nodes = whitelist.difference(nodes)\n if nonwhitlisted_nodes:\n raise KeyError(\n \"Whitelist contains unknown nodes: '%s'\"\n % nonwhitlisted_nodes\n )\n return nodes\n\n def write_png(self, filename, prog=None): # pragma: no cover\n \"\"\"Create PNG from dot graph of the transformations.\n\n .. warning::\n\n Note that this method requires the Python package pydot and an\n existing installation of graphviz on your system.\n\n Parameters\n ----------\n filename : str\n Name of the output file. Should end with '.png'.\n\n prog : str, optional (default: dot)\n Name of GraphViz executable that can be found in the `$PATH` or\n absolute path to GraphViz executable. Possible options are, for\n example, 'dot', 'twopi', 'neato', 'circo', 'fdp', 'sfdp'.\n\n Raises\n ------\n ImportError\n If pydot is not available\n \"\"\"\n if not PYDOT_AVAILABLE:\n raise ImportError(\"pydot must be installed to use this feature.\")\n\n graph = pydot.Dot(graph_type=\"graph\")\n frame_color = \"#dd3322\"\n connection_color = \"#d0d0ff\"\n\n for frame in self.nodes:\n node = pydot.Node(\n _dot_display_name(str(frame)),\n style=\"filled\",\n fillcolor=frame_color,\n shape=\"egg\",\n )\n graph.add_node(node)\n for frames, A2B in self._transforms.items():\n frame_a, frame_b = frames\n connection_name = \"%s to %s\\n%s\" % (\n _dot_display_name(str(frame_a)),\n _dot_display_name(str(frame_b)),\n str(np.round(A2B, 3)),\n )\n node = pydot.Node(\n connection_name,\n style=\"filled\",\n fillcolor=connection_color,\n shape=\"note\",\n )\n graph.add_node(node)\n a_name = _dot_display_name(str(frame_a))\n a_edge = pydot.Edge(connection_name, a_name, penwidth=3)\n graph.add_edge(a_edge)\n b_name = _dot_display_name(str(frame_b))\n b_edge = pydot.Edge(connection_name, b_name, penwidth=3)\n graph.add_edge(b_edge)\n\n graph.write_png(filename, prog=prog)\n\n def to_dict(self):\n \"\"\"Convert the transform manager to a dict that is serializable.\n\n Returns\n -------\n tm_dict : dict\n Serializable dict.\n \"\"\"\n return {\n \"class\": self.__class__.__name__,\n \"strict_check\": self.strict_check,\n \"check\": self.check,\n \"transforms\": [\n (k, v.ravel().tolist()) for k, v in self._transforms.items()\n ],\n \"nodes\": self.nodes,\n \"i\": self.i,\n \"j\": self.j,\n \"transform_to_ij_index\": list(self.transform_to_ij_index.items()),\n \"connections\": {\n \"data\": self.connections.data.tolist(),\n \"indices\": self.connections.indices.tolist(),\n \"indptr\": self.connections.indptr.tolist(),\n },\n \"dist\": self.dist.tolist(),\n \"predecessors\": self.predecessors.tolist(),\n }\n\n @staticmethod\n def from_dict(tm_dict):\n \"\"\"Create transform manager from dict.\n\n Parameters\n ----------\n tm_dict : dict\n Serializable dict.\n\n Returns\n -------\n tm : TransformManager\n Deserialized transform manager.\n \"\"\"\n strict_check = tm_dict.get(\"strict_check\")\n check = tm_dict.get(\"check\")\n tm = TransformManager(strict_check=strict_check, check=check)\n tm.set_transform_manager_state(tm_dict)\n return tm\n\n def set_transform_manager_state(self, tm_dict):\n \"\"\"Set state of transform manager from dict.\n\n Parameters\n ----------\n tm_dict : dict\n Serializable dict.\n \"\"\"\n transforms = tm_dict.get(\"transforms\")\n self._transforms = {\n tuple(k): np.array(v).reshape(4, 4) for k, v in transforms\n }\n self.nodes = tm_dict.get(\"nodes\")\n self.i = tm_dict.get(\"i\")\n self.j = tm_dict.get(\"j\")\n self.transform_to_ij_index = dict(\n (tuple(k), v) for k, v in tm_dict.get(\"transform_to_ij_index\")\n )\n connections = tm_dict.get(\"connections\")\n self.connections = sp.csr_matrix(\n (connections[\"data\"], connections[\"indices\"], connections[\"indptr\"])\n )\n n_nodes = len(self.nodes)\n dist = np.array(tm_dict.get(\"dist\"))\n self.dist = dist.reshape(n_nodes, n_nodes)\n predecessors = np.array(tm_dict.get(\"predecessors\"), dtype=np.int32)\n self.predecessors = predecessors.reshape(n_nodes, n_nodes)\n\n\ndef _dot_display_name(name): # pragma: no cover\n return name.replace(\"/\", \"\")\n" }, { "path": "pytransform3d/transform_manager/_transform_manager.pyi", "content": "from typing import Dict, Tuple, List, Union, Set, Hashable, Any\n\nimport numpy as np\nimport numpy.typing as npt\nfrom mpl_toolkits.mplot3d import Axes3D\n\nfrom ._transform_graph_base import TransformGraphBase\n\nPYDOT_AVAILABLE: bool\n\nclass TransformManager(TransformGraphBase):\n _transforms: Dict[Tuple[Hashable, Hashable], np.ndarray]\n\n def __init__(self, strict_check: bool = ..., check: bool = ...): ...\n @property\n def transforms(self) -> Dict[Tuple[Hashable, Hashable], np.ndarray]: ...\n def _check_transform(self, A2B: npt.ArrayLike) -> np.ndarray: ...\n def _transform_available(self, key: Tuple[Hashable, Hashable]) -> bool: ...\n def _set_transform(self, key: Tuple[Hashable, Hashable], A2B: Any): ...\n def _get_transform(self, key: Tuple[Hashable, Hashable]) -> Any: ...\n def _del_transform(self, key: Tuple[Hashable, Hashable]): ...\n def add_transform(\n self, from_frame: Hashable, to_frame: Hashable, A2B: np.ndarray\n ) -> \"TransformManager\": ...\n def get_transform(\n self, from_frame: Hashable, to_frame: Hashable\n ) -> np.ndarray: ...\n def plot_frames_in(\n self,\n frame: Hashable,\n ax: Union[None, Axes3D] = ...,\n s: float = ...,\n ax_s: float = ...,\n show_name: bool = ...,\n whitelist: Union[List[str], None] = ...,\n **kwargs,\n ) -> Axes3D: ...\n def plot_connections_in(\n self,\n frame: Hashable,\n ax: Union[None, Axes3D] = ...,\n ax_s: float = ...,\n whitelist: Union[List[Hashable], None] = ...,\n **kwargs,\n ) -> Axes3D: ...\n def _whitelisted_nodes(\n self, whitelist: Union[None, List[Hashable]]\n ) -> Set[Hashable]: ...\n def write_png(self, filename: str, prog: Union[str, None] = ...): ...\n def to_dict(self) -> Dict[str, Any]: ...\n @staticmethod\n def from_dict(tm_dict: Dict[str, Any]) -> \"TransformManager\": ...\n def set_transform_manager_state(self, tm_dict: Dict[str, Any]): ...\n" }, { "path": "pytransform3d/transform_manager/test/test_temporal_transform_manager.py", "content": "import numpy as np\nimport pytest\nfrom numpy.testing import assert_array_almost_equal\n\nimport pytransform3d.rotations as pr\nimport pytransform3d.transformations as pt\nfrom pytransform3d.transform_manager import (\n TemporalTransformManager,\n StaticTransform,\n NumpyTimeseriesTransform,\n)\n\n\ndef create_sinusoidal_movement(\n duration_sec, sample_period, x_velocity, y_start_offset, start_time\n):\n \"\"\"Create a planar (z=0) sinusoidal movement around x-axis.\"\"\"\n time_arr = np.arange(0, duration_sec, sample_period) + start_time\n N = len(time_arr)\n x_arr = np.linspace(0, x_velocity * duration_sec, N)\n\n spatial_freq = 1 / 5 # 1 sinus per 5m\n omega = 2 * np.pi * spatial_freq\n y_arr = np.sin(omega * x_arr)\n y_arr += y_start_offset\n\n dydx_arr = omega * np.cos(omega * x_arr)\n yaw_arr = np.arctan2(dydx_arr, np.ones_like(dydx_arr))\n\n pq_arr = list()\n for i in range(N):\n R = pr.active_matrix_from_extrinsic_euler_zyx([yaw_arr[i], 0, 0])\n T = pt.transform_from(R, [x_arr[i], y_arr[i], 0])\n pq = pt.pq_from_transform(T)\n pq_arr.append(pq)\n\n return time_arr, np.array(pq_arr)\n\n\ndef test_temporal_transform():\n rng = np.random.default_rng(0)\n A2B = pt.random_transform(rng)\n\n rng = np.random.default_rng(42)\n A2C = pt.random_transform(rng)\n\n tm = TemporalTransformManager()\n\n tm.add_transform(\"A\", \"B\", StaticTransform(A2B))\n tm.add_transform(\"A\", \"C\", StaticTransform(A2C))\n\n tm.current_time = 1234.0\n B2C = tm.get_transform(\"B\", \"C\")\n\n C2B = tm.get_transform(\"C\", \"B\")\n B2C_2 = pt.invert_transform(C2B)\n assert_array_almost_equal(B2C, B2C_2)\n\n B2C_3 = tm.get_transform_at_time(\"B\", \"C\", 1234.0)\n assert_array_almost_equal(B2C_2, B2C_3)\n\n\ndef test_remove_frame_temporal_manager():\n \"\"\"Test removing a frame from the transform manager.\"\"\"\n tm = TemporalTransformManager()\n\n rng = np.random.default_rng(0)\n\n A2B = pt.random_transform(rng)\n A2D = pt.random_transform(rng)\n B2C = pt.random_transform(rng)\n D2E = pt.random_transform(rng)\n\n tm.add_transform(\"A\", \"B\", StaticTransform(A2B))\n tm.add_transform(\"A\", \"D\", StaticTransform(A2D))\n tm.add_transform(\"B\", \"C\", StaticTransform(B2C))\n tm.add_transform(\"D\", \"E\", StaticTransform(D2E))\n\n assert tm.has_frame(\"B\")\n\n A2E = tm.get_transform(\"A\", \"E\")\n\n # Check that connections are correctly represented in self.i and self.j\n assert tm.i == [\n tm.nodes.index(\"A\"),\n tm.nodes.index(\"A\"),\n tm.nodes.index(\"B\"),\n tm.nodes.index(\"D\"),\n ]\n assert tm.j == [\n tm.nodes.index(\"B\"),\n tm.nodes.index(\"D\"),\n tm.nodes.index(\"C\"),\n tm.nodes.index(\"E\"),\n ]\n\n tm.remove_frame(\"B\")\n assert not tm.has_frame(\"B\")\n\n # Ensure connections involving \"B\" are removed and the remaining\n # connections are correctly represented.\n assert tm.i == [tm.nodes.index(\"A\"), tm.nodes.index(\"D\")]\n assert tm.j == [tm.nodes.index(\"D\"), tm.nodes.index(\"E\")]\n\n with pytest.raises(KeyError, match=\"Unknown frame\"):\n tm.get_transform(\"A\", \"B\")\n with pytest.raises(KeyError, match=\"Unknown frame\"):\n tm.get_transform(\"B\", \"C\")\n\n assert tm.has_frame(\"A\")\n assert tm.has_frame(\"C\")\n assert tm.has_frame(\"D\")\n assert tm.has_frame(\"E\")\n\n # Ensure we cannot retrieve paths involving the removed frame\n with pytest.raises(KeyError, match=\"Cannot compute path\"):\n tm.get_transform(\"A\", \"C\")\n\n tm.get_transform(\"A\", \"D\")\n tm.get_transform(\"D\", \"E\")\n\n assert_array_almost_equal(A2E, tm.get_transform(\"A\", \"E\"))\n\n\ndef test_internals():\n rng = np.random.default_rng(0)\n A2B = pt.random_transform(rng)\n\n rng = np.random.default_rng(42)\n A2C = pt.random_transform(rng)\n\n tm = TemporalTransformManager()\n\n tm.add_transform(\"A\", \"B\", StaticTransform(A2B))\n tm.add_transform(\"A\", \"C\", StaticTransform(A2C))\n\n tm.remove_transform(\"A\", \"C\")\n assert (\"A\", \"C\") not in tm.transforms\n\n\ndef test_numpy_timeseries_transform():\n # create entities A and B together with their transformations from world\n duration = 10.0 # [s]\n sample_period = 0.5 # [s]\n velocity_x = 1 # [m/s]\n time_A, pq_arr_A = create_sinusoidal_movement(\n duration, sample_period, velocity_x, y_start_offset=0.0, start_time=0.1\n )\n A2world = NumpyTimeseriesTransform(time_A, pq_arr_A)\n\n time_B, pq_arr_B = create_sinusoidal_movement(\n duration, sample_period, velocity_x, y_start_offset=2.0, start_time=0.35\n )\n B2world = NumpyTimeseriesTransform(time_B, pq_arr_B)\n\n tm = TemporalTransformManager()\n\n tm.add_transform(\"A\", \"W\", A2world)\n tm.add_transform(\"B\", \"W\", B2world)\n\n query_time = time_A[0] # Start time\n A2W_at_start = pt.transform_from_pq(pq_arr_A[0, :])\n A2W_at_start_2 = tm.get_transform_at_time(\"A\", \"W\", query_time)\n assert A2W_at_start_2.ndim == 2\n assert_array_almost_equal(A2W_at_start, A2W_at_start_2, decimal=2)\n\n query_times = [time_A[0], time_A[0]] # Start times\n A2Ws_at_start_2 = tm.get_transform_at_time(\"A\", \"W\", query_times)\n assert A2Ws_at_start_2.ndim == 3\n assert_array_almost_equal(A2W_at_start, A2Ws_at_start_2[0], decimal=2)\n assert_array_almost_equal(A2W_at_start, A2Ws_at_start_2[1], decimal=2)\n\n A2Ws_at_start_2 = tm.get_transform_at_time(\"A\", \"W\", query_times)\n assert_array_almost_equal(A2W_at_start, A2Ws_at_start_2[0], decimal=2)\n assert_array_almost_equal(A2W_at_start, A2Ws_at_start_2[1], decimal=2)\n assert A2Ws_at_start_2.ndim == 3\n\n query_time = 4.9 # [s]\n A2B_at_query_time = tm.get_transform_at_time(\"A\", \"B\", query_time)\n\n origin_of_A_pos = pt.vector_to_point([0, 0, 0])\n origin_of_A_in_B_pos = pt.transform(A2B_at_query_time, origin_of_A_pos)\n origin_of_A_in_B_xyz = origin_of_A_in_B_pos[:-1]\n\n assert origin_of_A_in_B_xyz[0] == pytest.approx(-1.11, abs=1e-2)\n assert origin_of_A_in_B_xyz[1] == pytest.approx(-1.28, abs=1e-2)\n\n\ndef test_numpy_timeseries_transform_wrong_input_shapes():\n n_steps = 10\n with pytest.raises(ValueError, match=\"Number of timesteps\"):\n time = np.arange(n_steps)\n pqs = np.random.randn(n_steps + 1, 7)\n NumpyTimeseriesTransform(time, pqs)\n\n with pytest.raises(ValueError, match=\"`pqs` matrix\"):\n time = np.arange(10)\n pqs = np.random.randn(n_steps, 8)\n NumpyTimeseriesTransform(time, pqs)\n\n with pytest.raises(ValueError, match=\"Shape of PQ array\"):\n time = np.arange(10)\n pqs = np.random.randn(n_steps, 8).flatten()\n NumpyTimeseriesTransform(time, pqs)\n\n\ndef test_numpy_timeseries_transform_multiple_query_times():\n # create entities A and B together with their transformations from world\n duration = 10.0 # [s]\n sample_period = 0.5 # [s]\n velocity_x = 1 # [m/s]\n time_A, pq_arr_A = create_sinusoidal_movement(\n duration, sample_period, velocity_x, y_start_offset=0.0, start_time=0.1\n )\n A2world = NumpyTimeseriesTransform(time_A, pq_arr_A)\n\n time_B, pq_arr_B = create_sinusoidal_movement(\n duration, sample_period, velocity_x, y_start_offset=2.0, start_time=0.35\n )\n B2world = NumpyTimeseriesTransform(time_B, pq_arr_B)\n\n tm = TemporalTransformManager()\n\n tm.add_transform(\"A\", \"W\", A2world)\n tm.add_transform(\"B\", \"W\", B2world)\n\n # test if shape is conserved correctly\n A2B_at_query_time = tm.get_transform_at_time(\"A\", \"B\", 1.0)\n assert A2B_at_query_time.shape == (4, 4)\n\n A2B_at_query_time = tm.get_transform_at_time(\"A\", \"B\", np.array([1.0]))\n assert A2B_at_query_time.shape == (1, 4, 4)\n\n query_times = np.array([4.9, 5.2]) # [s]\n A2B_at_query_time = tm.get_transform_at_time(\"A\", \"B\", query_times)\n\n origin_of_A_pos = pt.vector_to_point([0, 0, 0])\n origin_of_A_in_B_pos1 = pt.transform(A2B_at_query_time[0], origin_of_A_pos)\n origin_of_A_in_B_xyz1 = origin_of_A_in_B_pos1[:-1]\n\n origin_of_A_in_B_x1, origin_of_A_in_B_y1 = (\n origin_of_A_in_B_xyz1[0],\n origin_of_A_in_B_xyz1[1],\n )\n\n assert origin_of_A_in_B_x1 == pytest.approx(-1.11, abs=1e-2)\n assert origin_of_A_in_B_y1 == pytest.approx(-1.28, abs=1e-2)\n\n\ndef test_temporal_transform_manager_incorrect_frame():\n duration = 10.0 # [s]\n sample_period = 0.5 # [s]\n velocity_x = 1 # [m/s]\n time_A, pq_arr_A = create_sinusoidal_movement(\n duration, sample_period, velocity_x, y_start_offset=0.0, start_time=0.1\n )\n A2world = NumpyTimeseriesTransform(time_A, pq_arr_A)\n\n tm = TemporalTransformManager()\n tm.add_transform(\"A\", \"W\", A2world)\n with pytest.raises(KeyError, match=\"Unknown frame\"):\n tm.get_transform(\"B\", \"W\")\n with pytest.raises(KeyError, match=\"Unknown frame\"):\n tm.get_transform(\"A\", \"B\")\n\n tm = TemporalTransformManager(check=False)\n tm.add_transform(\"A\", \"W\", A2world)\n with pytest.raises(ValueError):\n tm.get_transform(\"B\", \"W\")\n with pytest.raises(ValueError):\n tm.get_transform(\"A\", \"B\")\n\n\ndef test_temporal_transform_manager_out_of_bounds():\n duration = 10.0 # [s]\n sample_period = 0.5 # [s]\n velocity_x = 1 # [m/s]\n time_A, pq_arr_A = create_sinusoidal_movement(\n duration, sample_period, velocity_x, y_start_offset=0.0, start_time=0.0\n )\n A2world = NumpyTimeseriesTransform(time_A, pq_arr_A, time_clipping=True)\n\n time_B, pq_arr_B = create_sinusoidal_movement(\n duration, sample_period, velocity_x, y_start_offset=2.0, start_time=0.1\n )\n B2world = NumpyTimeseriesTransform(time_B, pq_arr_B, time_clipping=True)\n\n tm = TemporalTransformManager()\n tm.add_transform(\"A\", \"W\", A2world)\n tm.add_transform(\"B\", \"W\", B2world)\n\n assert min(time_A) == 0.0\n assert min(time_B) == 0.1\n A2B_at_start_time, A2B_before_start_time = tm.get_transform_at_time(\n \"A\", \"B\", [0.0, -0.1]\n )\n assert_array_almost_equal(A2B_at_start_time, A2B_before_start_time)\n\n assert max(time_A) == 9.5\n assert max(time_B) == 9.6\n A2B_at_end_time, A2B_after_end_time = tm.get_transform_at_time(\n \"A\", \"B\", [9.6, 10.0]\n )\n assert_array_almost_equal(A2B_at_end_time, A2B_after_end_time)\n\n A2world.time_clipping = False\n B2world.time_clipping = False\n\n with pytest.raises(\n ValueError, match=\"Query time at indices \\[0\\], time\\(s\\): \\[0.\\]\"\n ):\n tm.get_transform_at_time(\"A\", \"B\", [0.0, 0.1])\n\n with pytest.raises(\n ValueError,\n match=r\"Query time at indices \\[0 1\\], time\\(s\\): \\[ 9.7 10. \\]\",\n ):\n tm.get_transform_at_time(\"A\", \"B\", [9.7, 10.0])\n" }, { "path": "pytransform3d/transform_manager/test/test_transform_manager.py", "content": "import contextlib\nimport os\nimport pickle\nimport tempfile\nimport warnings\n\nimport numpy as np\nimport pytest\nfrom numpy.testing import assert_array_almost_equal\n\nimport pytransform3d.rotations as pr\nimport pytransform3d.transformations as pt\nfrom pytransform3d import transform_manager\nfrom pytransform3d.transform_manager import TransformManager\n\n\ndef test_request_added_transform():\n \"\"\"Request an added transform from the transform manager.\"\"\"\n rng = np.random.default_rng(0)\n A2B = pt.random_transform(rng)\n\n tm = TransformManager()\n assert len(tm.transforms) == 0\n tm.add_transform(\"A\", \"B\", A2B)\n assert len(tm.transforms) == 1\n A2B_2 = tm.get_transform(\"A\", \"B\")\n assert_array_almost_equal(A2B, A2B_2)\n\n\ndef test_remove_frame():\n \"\"\"Test removing a frame from the transform manager.\"\"\"\n tm = TransformManager()\n\n rng = np.random.default_rng(0)\n\n A2B = pt.random_transform(rng)\n A2D = pt.random_transform(rng)\n B2C = pt.random_transform(rng)\n D2E = pt.random_transform(rng)\n\n tm.add_transform(\"A\", \"B\", A2B)\n tm.add_transform(\"A\", \"D\", A2D)\n tm.add_transform(\"B\", \"C\", B2C)\n tm.add_transform(\"D\", \"E\", D2E)\n\n assert tm.has_frame(\"B\")\n\n A2E = tm.get_transform(\"A\", \"E\")\n\n # Check that connections are correctly represented in self.i and self.j\n assert tm.i == [\n tm.nodes.index(\"A\"),\n tm.nodes.index(\"A\"),\n tm.nodes.index(\"B\"),\n tm.nodes.index(\"D\"),\n ]\n assert tm.j == [\n tm.nodes.index(\"B\"),\n tm.nodes.index(\"D\"),\n tm.nodes.index(\"C\"),\n tm.nodes.index(\"E\"),\n ]\n\n tm.remove_frame(\"B\")\n assert not tm.has_frame(\"B\")\n\n # Ensure connections involving \"B\" are removed and the remaining\n # connections are correctly represented.\n assert tm.i == [tm.nodes.index(\"A\"), tm.nodes.index(\"D\")]\n assert tm.j == [tm.nodes.index(\"D\"), tm.nodes.index(\"E\")]\n\n with pytest.raises(KeyError, match=\"Unknown frame\"):\n tm.get_transform(\"A\", \"B\")\n with pytest.raises(KeyError, match=\"Unknown frame\"):\n tm.get_transform(\"B\", \"C\")\n\n assert tm.has_frame(\"A\")\n assert tm.has_frame(\"C\")\n assert tm.has_frame(\"D\")\n assert tm.has_frame(\"E\")\n\n # Ensure we cannot retrieve paths involving the removed frame\n with pytest.raises(KeyError, match=\"Cannot compute path\"):\n tm.get_transform(\"A\", \"C\")\n\n tm.get_transform(\"A\", \"D\")\n tm.get_transform(\"D\", \"E\")\n\n assert_array_almost_equal(A2E, tm.get_transform(\"A\", \"E\"))\n\n\ndef test_remove_frame_does_not_exist():\n tm = TransformManager()\n with pytest.raises(KeyError, match=\"not in the graph\"):\n tm.remove_frame(\"Any\")\n\n\ndef test_request_inverse_transform():\n \"\"\"Request an inverse transform from the transform manager.\"\"\"\n rng = np.random.default_rng(0)\n A2B = pt.random_transform(rng)\n\n tm = TransformManager()\n tm.add_transform(\"A\", \"B\", A2B)\n A2B_2 = tm.get_transform(\"A\", \"B\")\n assert_array_almost_equal(A2B, A2B_2)\n\n B2A = tm.get_transform(\"B\", \"A\")\n B2A_2 = pt.invert_transform(A2B)\n assert_array_almost_equal(B2A, B2A_2)\n\n\ndef test_has_frame():\n \"\"\"Check if frames have been registered with transform.\"\"\"\n tm = TransformManager()\n tm.add_transform(\"A\", \"B\", np.eye(4))\n assert tm.has_frame(\"A\")\n assert tm.has_frame(\"B\")\n assert not tm.has_frame(\"C\")\n\n\ndef test_transform_not_added():\n \"\"\"Test request for transforms that have not been added.\"\"\"\n rng = np.random.default_rng(0)\n A2B = pt.random_transform(rng)\n C2D = pt.random_transform(rng)\n\n tm = TransformManager()\n tm.add_transform(\"A\", \"B\", A2B)\n tm.add_transform(\"C\", \"D\", C2D)\n\n with pytest.raises(KeyError, match=\"Unknown frame\"):\n tm.get_transform(\"A\", \"G\")\n with pytest.raises(KeyError, match=\"Unknown frame\"):\n tm.get_transform(\"G\", \"D\")\n with pytest.raises(KeyError, match=\"Cannot compute path\"):\n tm.get_transform(\"A\", \"D\")\n\n\ndef test_request_concatenated_transform():\n \"\"\"Request a concatenated transform from the transform manager.\"\"\"\n rng = np.random.default_rng(0)\n A2B = pt.random_transform(rng)\n B2C = pt.random_transform(rng)\n F2A = pt.random_transform(rng)\n\n tm = TransformManager()\n tm.add_transform(\"A\", \"B\", A2B)\n tm.add_transform(\"B\", \"C\", B2C)\n tm.add_transform(\"D\", \"E\", np.eye(4))\n tm.add_transform(\"F\", \"A\", F2A)\n\n A2C = tm.get_transform(\"A\", \"C\")\n assert_array_almost_equal(A2C, pt.concat(A2B, B2C))\n\n C2A = tm.get_transform(\"C\", \"A\")\n assert_array_almost_equal(\n C2A, pt.concat(pt.invert_transform(B2C), pt.invert_transform(A2B))\n )\n\n F2B = tm.get_transform(\"F\", \"B\")\n assert_array_almost_equal(F2B, pt.concat(F2A, A2B))\n\n\ndef test_update_transform():\n \"\"\"Update an existing transform.\"\"\"\n rng = np.random.default_rng(0)\n A2B1 = pt.random_transform(rng)\n A2B2 = pt.random_transform(rng)\n\n tm = TransformManager()\n tm.add_transform(\"A\", \"B\", A2B1)\n tm.add_transform(\"A\", \"B\", A2B2)\n A2B = tm.get_transform(\"A\", \"B\")\n\n # Hack: test depends on internal member\n assert_array_almost_equal(A2B, A2B2)\n assert len(tm.i) == 1\n assert len(tm.j) == 1\n\n\ndef test_pickle():\n \"\"\"Test if a transform manager can be pickled.\"\"\"\n rng = np.random.default_rng(1)\n A2B = pt.random_transform(rng)\n tm = TransformManager()\n tm.add_transform(\"A\", \"B\", A2B)\n\n _, filename = tempfile.mkstemp(\".pickle\")\n try:\n with open(filename, \"wb\") as f:\n pickle.dump(tm, f)\n with open(filename, \"rb\") as f:\n tm2 = pickle.load(f)\n finally:\n if os.path.exists(filename):\n # workaround for permission problem on Windows\n with contextlib.suppress(OSError):\n os.remove(filename)\n A2B2 = tm2.get_transform(\"A\", \"B\")\n assert_array_almost_equal(A2B, A2B2)\n\n\ndef test_whitelist():\n \"\"\"Test correct handling of whitelists for plotting.\"\"\"\n rng = np.random.default_rng(2)\n A2B = pt.random_transform(rng)\n tm = TransformManager()\n tm.add_transform(\"A\", \"B\", A2B)\n\n nodes = tm._whitelisted_nodes(None)\n assert {\"A\", \"B\"} == nodes\n nodes = tm._whitelisted_nodes([\"A\"])\n assert {\"A\"} == nodes\n with pytest.raises(KeyError, match=\"unknown nodes\"):\n tm._whitelisted_nodes([\"C\"])\n\n\ndef test_check_consistency():\n \"\"\"Test correct detection of inconsistent graphs.\"\"\"\n rng = np.random.default_rng(2)\n\n tm = TransformManager()\n\n A2B = pt.random_transform(rng)\n tm.add_transform(\"A\", \"B\", A2B)\n B2A = pt.random_transform(rng)\n tm.add_transform(\"B\", \"A\", B2A)\n assert not tm.check_consistency()\n\n tm = TransformManager()\n\n A2B = pt.random_transform(rng)\n tm.add_transform(\"A\", \"B\", A2B)\n assert tm.check_consistency()\n\n C2D = pt.random_transform(rng)\n tm.add_transform(\"C\", \"D\", C2D)\n assert tm.check_consistency()\n\n B2C = pt.random_transform(rng)\n tm.add_transform(\"B\", \"C\", B2C)\n assert tm.check_consistency()\n\n A2D_over_path = tm.get_transform(\"A\", \"D\")\n\n A2D = pt.random_transform(rng)\n tm.add_transform(\"A\", \"D\", A2D)\n assert not tm.check_consistency()\n\n tm.add_transform(\"A\", \"D\", A2D_over_path)\n assert tm.check_consistency()\n\n\ndef test_connected_components():\n \"\"\"Test computation of connected components in the graph.\"\"\"\n tm = TransformManager()\n tm.add_transform(\"A\", \"B\", np.eye(4))\n assert tm.connected_components() == 1\n tm.add_transform(\"D\", \"E\", np.eye(4))\n assert tm.connected_components() == 2\n tm.add_transform(\"B\", \"C\", np.eye(4))\n assert tm.connected_components() == 2\n tm.add_transform(\"D\", \"C\", np.eye(4))\n assert tm.connected_components() == 1\n\n\ndef test_png_export():\n \"\"\"Test if the graph can be exported to PNG.\"\"\"\n rng = np.random.default_rng(0)\n\n ee2robot = pt.transform_from_pq(\n np.hstack((np.array([0.4, -0.3, 0.5]), pr.random_quaternion(rng)))\n )\n cam2robot = pt.transform_from_pq(\n np.hstack((np.array([0.0, 0.0, 0.8]), pr.q_id))\n )\n object2cam = pt.transform_from(\n pr.active_matrix_from_intrinsic_euler_xyz(np.array([0.0, 0.0, 0.5])),\n np.array([0.5, 0.1, 0.1]),\n )\n\n tm = TransformManager()\n tm.add_transform(\"end-effector\", \"robot\", ee2robot)\n tm.add_transform(\"camera\", \"robot\", cam2robot)\n tm.add_transform(\"object\", \"camera\", object2cam)\n\n _, filename = tempfile.mkstemp(\".png\")\n try:\n tm.write_png(filename)\n assert os.path.exists(filename)\n except ImportError:\n pytest.skip(\"pydot is required for this test\")\n finally:\n if os.path.exists(filename):\n # workaround for permission problem on Windows\n with contextlib.suppress(OSError):\n os.remove(filename)\n\n\ndef test_png_export_without_pydot_fails():\n \"\"\"Test graph export without pydot.\"\"\"\n pydot_available = transform_manager._transform_manager.PYDOT_AVAILABLE\n tm = TransformManager()\n try:\n transform_manager._transform_manager.PYDOT_AVAILABLE = False\n with pytest.raises(\n ImportError, match=\"pydot must be installed to use this feature.\"\n ):\n tm.write_png(\"bla\")\n finally:\n transform_manager._transform_manager.PYDOT_AVAILABLE = pydot_available\n\n\ndef test_deactivate_transform_manager_precision_error():\n A2B = np.eye(4)\n A2B[0, 0] = 2.0\n A2B[3, 0] = 3.0\n tm = TransformManager()\n with pytest.raises(ValueError, match=\"Expected rotation matrix\"):\n tm.add_transform(\"A\", \"B\", A2B)\n\n n_expected_warnings = 6\n try:\n warnings.filterwarnings(\"always\", category=UserWarning)\n with warnings.catch_warnings(record=True) as w:\n tm = TransformManager(strict_check=False)\n tm.add_transform(\"A\", \"B\", A2B)\n tm.add_transform(\"B\", \"C\", np.eye(4))\n tm.get_transform(\"C\", \"A\")\n assert len(w) == n_expected_warnings\n finally:\n warnings.filterwarnings(\"default\", category=UserWarning)\n\n\ndef test_deactivate_checks():\n tm = TransformManager(check=False)\n tm.add_transform(\"A\", \"B\", np.zeros((4, 4)))\n tm.add_transform(\"B\", \"C\", np.zeros((4, 4)))\n A2B = tm.get_transform(\"A\", \"C\")\n assert_array_almost_equal(A2B, np.zeros((4, 4)))\n\n\ndef test_remove_transform():\n tm = TransformManager()\n tm.add_transform(\"A\", \"B\", np.eye(4))\n tm.add_transform(\"C\", \"D\", np.eye(4))\n\n with pytest.raises(KeyError, match=\"Cannot compute path\"):\n tm.get_transform(\"A\", \"D\")\n\n tm.add_transform(\"B\", \"C\", np.eye(4))\n tm.get_transform(\"A\", \"C\")\n\n tm.remove_transform(\"B\", \"C\")\n tm.remove_transform(\"B\", \"C\") # nothing should happen\n with pytest.raises(KeyError, match=\"Cannot compute path\"):\n tm.get_transform(\"A\", \"D\")\n tm.get_transform(\"B\", \"A\")\n tm.get_transform(\"D\", \"C\")\n\n\ndef test_from_to_dict():\n rng = np.random.default_rng(2323)\n tm = TransformManager()\n A2B = pt.random_transform(rng)\n tm.add_transform(\"A\", \"B\", A2B)\n B2C = pt.random_transform(rng)\n tm.add_transform(\"B\", \"C\", B2C)\n C2D = pt.random_transform(rng)\n tm.add_transform(\"C\", \"D\", C2D)\n\n tm_dict = tm.to_dict()\n tm2 = TransformManager.from_dict(tm_dict)\n tm2_dict = tm2.to_dict()\n\n assert_array_almost_equal(\n tm.get_transform(\"D\", \"A\"), tm2.get_transform(\"D\", \"A\")\n )\n\n assert tm_dict == tm2_dict\n\n\ndef test_remove_twice():\n tm = TransformManager()\n tm.add_transform(\"a\", \"b\", np.eye(4))\n tm.add_transform(\"c\", \"d\", np.eye(4))\n tm.remove_transform(\"a\", \"b\")\n tm.remove_transform(\"c\", \"d\")\n\n\ndef test_remove_and_add_connection():\n rng = np.random.default_rng(5)\n A2B = pt.random_transform(rng)\n B2C = pt.random_transform(rng)\n C2D = pt.random_transform(rng)\n D2E = pt.random_transform(rng)\n\n tm = TransformManager()\n tm.add_transform(\"a\", \"b\", A2B)\n tm.add_transform(\"b\", \"c\", B2C)\n tm.add_transform(\"c\", \"d\", C2D)\n tm.add_transform(\"d\", \"e\", D2E)\n measurement1 = tm.get_transform(\"a\", \"e\")\n\n tm.remove_transform(\"b\", \"c\")\n tm.remove_transform(\"c\", \"d\")\n tm.add_transform(\"b\", \"c\", B2C)\n tm.add_transform(\"c\", \"d\", C2D)\n measurement2 = tm.get_transform(\"a\", \"e\")\n\n assert_array_almost_equal(measurement1, measurement2)\n" }, { "path": "pytransform3d/transformations/__init__.py", "content": "\"\"\"Transformations in three dimensions - SE(3).\n\nSee :doc:`user_guide/transformations` for more information.\n\"\"\"\n\nfrom ._transform import (\n transform_requires_renormalization,\n check_transform,\n assert_transform,\n transform_from,\n rotate_transform,\n translate_transform,\n pq_from_transform,\n transform_log_from_transform,\n exponential_coordinates_from_transform,\n dual_quaternion_from_transform,\n adjoint_from_transform,\n)\nfrom ._transform_operations import (\n invert_transform,\n scale_transform,\n concat,\n vector_to_point,\n vectors_to_points,\n vector_to_direction,\n vectors_to_directions,\n transform,\n transform_sclerp,\n transform_power,\n)\nfrom ._screws import (\n check_screw_parameters,\n check_screw_axis,\n check_exponential_coordinates,\n check_screw_matrix,\n check_transform_log,\n norm_exponential_coordinates,\n assert_exponential_coordinates_equal,\n assert_screw_parameters_equal,\n transform_from_transform_log,\n transform_from_exponential_coordinates,\n screw_parameters_from_screw_axis,\n screw_axis_from_screw_parameters,\n exponential_coordinates_from_screw_axis,\n screw_axis_from_exponential_coordinates,\n transform_log_from_exponential_coordinates,\n exponential_coordinates_from_transform_log,\n screw_matrix_from_screw_axis,\n screw_axis_from_screw_matrix,\n transform_log_from_screw_matrix,\n screw_matrix_from_transform_log,\n dual_quaternion_from_screw_parameters,\n)\nfrom ._pq import check_pq, pq_slerp, transform_from_pq, dual_quaternion_from_pq\nfrom ._dual_quaternion import (\n dual_quaternion_requires_renormalization,\n norm_dual_quaternion,\n dual_quaternion_squared_norm,\n check_dual_quaternion,\n assert_unit_dual_quaternion_equal,\n assert_unit_dual_quaternion,\n dual_quaternion_double,\n dq_q_conj,\n dq_conj,\n concatenate_dual_quaternions,\n dual_quaternion_sclerp,\n dual_quaternion_power,\n dq_prod_vector,\n transform_from_dual_quaternion,\n pq_from_dual_quaternion,\n screw_parameters_from_dual_quaternion,\n)\nfrom ._random import (\n random_transform,\n random_screw_axis,\n random_exponential_coordinates,\n)\nfrom ._plot import plot_transform, plot_screw\nfrom ._jacobians import (\n left_jacobian_SE3,\n left_jacobian_SE3_series,\n left_jacobian_SE3_inv,\n left_jacobian_SE3_inv_series,\n)\n\n\n__all__ = [\n \"transform_requires_renormalization\",\n \"check_transform\",\n \"check_pq\",\n \"check_screw_parameters\",\n \"check_screw_axis\",\n \"check_exponential_coordinates\",\n \"check_screw_matrix\",\n \"check_transform_log\",\n \"dual_quaternion_requires_renormalization\",\n \"check_dual_quaternion\",\n \"transform_from\",\n \"rotate_transform\",\n \"translate_transform\",\n \"pq_from_transform\",\n \"transform_from_pq\",\n \"transform_from_transform_log\",\n \"transform_log_from_transform\",\n \"transform_from_exponential_coordinates\",\n \"exponential_coordinates_from_transform\",\n \"screw_parameters_from_screw_axis\",\n \"screw_axis_from_screw_parameters\",\n \"exponential_coordinates_from_screw_axis\",\n \"screw_axis_from_exponential_coordinates\",\n \"transform_log_from_exponential_coordinates\",\n \"exponential_coordinates_from_transform_log\",\n \"screw_matrix_from_screw_axis\",\n \"screw_axis_from_screw_matrix\",\n \"transform_log_from_screw_matrix\",\n \"screw_matrix_from_transform_log\",\n \"dual_quaternion_from_transform\",\n \"transform_from_dual_quaternion\",\n \"screw_parameters_from_dual_quaternion\",\n \"dual_quaternion_from_screw_parameters\",\n \"dual_quaternion_from_pq\",\n \"pq_from_dual_quaternion\",\n \"adjoint_from_transform\",\n \"norm_exponential_coordinates\",\n \"invert_transform\",\n \"scale_transform\",\n \"concat\",\n \"vector_to_point\",\n \"vectors_to_points\",\n \"vector_to_direction\",\n \"vectors_to_directions\",\n \"transform\",\n \"transform_sclerp\",\n \"transform_power\",\n \"random_transform\",\n \"random_screw_axis\",\n \"random_exponential_coordinates\",\n \"pq_slerp\",\n \"norm_dual_quaternion\",\n \"dual_quaternion_squared_norm\",\n \"dual_quaternion_double\",\n \"dq_q_conj\",\n \"dq_conj\",\n \"concatenate_dual_quaternions\",\n \"dual_quaternion_sclerp\",\n \"dual_quaternion_power\",\n \"dq_prod_vector\",\n \"plot_transform\",\n \"plot_screw\",\n \"assert_transform\",\n \"assert_exponential_coordinates_equal\",\n \"assert_screw_parameters_equal\",\n \"assert_unit_dual_quaternion_equal\",\n \"assert_unit_dual_quaternion\",\n \"left_jacobian_SE3\",\n \"left_jacobian_SE3_series\",\n \"left_jacobian_SE3_inv\",\n \"left_jacobian_SE3_inv_series\",\n]\n" }, { "path": "pytransform3d/transformations/_dual_quaternion.py", "content": "\"\"\"Dual quaternion operations.\"\"\"\n\nimport numpy as np\nfrom numpy.testing import assert_array_almost_equal\n\nfrom ._screws import dual_quaternion_from_screw_parameters\nfrom ._transform import transform_from\nfrom ..rotations import (\n concatenate_quaternions,\n q_conj,\n axis_angle_from_quaternion,\n matrix_from_quaternion,\n)\n\n\ndef check_dual_quaternion(dq, unit=True):\n \"\"\"Input validation of dual quaternion representation.\n\n See http://web.cs.iastate.edu/~cs577/handouts/dual-quaternion.pdf\n\n A dual quaternion is defined as\n\n .. math::\n\n \\\\boldsymbol{\\\\sigma} = \\\\boldsymbol{p} + \\\\epsilon \\\\boldsymbol{q},\n\n where :math:`\\\\boldsymbol{p}` and :math:`\\\\boldsymbol{q}` are both\n quaternions and :math:`\\\\epsilon` is the dual unit with\n :math:`\\\\epsilon^2 = 0`. The first quaternion is also called the real part\n and the second quaternion is called the dual part.\n\n Parameters\n ----------\n dq : array-like, shape (8,)\n Dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n unit : bool, optional (default: True)\n Normalize the dual quaternion so that it is a unit dual quaternion.\n A unit dual quaternion has the properties\n :math:`p_w^2 + p_x^2 + p_y^2 + p_z^2 = 1` and\n :math:`p_w q_w + p_x q_x + p_y q_y + p_z q_z = 0`.\n\n Returns\n -------\n dq : array, shape (8,)\n Unit dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n Raises\n ------\n ValueError\n If input is invalid\n\n See Also\n --------\n norm_dual_quaternion\n Normalization that enforces unit norm and orthogonality of the real and\n dual quaternion.\n \"\"\"\n dq = np.asarray(dq, dtype=np.float64)\n if dq.ndim != 1 or dq.shape[0] != 8:\n raise ValueError(\n \"Expected dual quaternion with shape (8,), got \"\n \"array-like object with shape %s\" % (dq.shape,)\n )\n if unit:\n # Norm of a dual quaternion only depends on the real part because\n # the dual part vanishes with (1) epsilon ** 2 = 0 and (2) the real\n # and dual part being orthogonal, i.e., their product is 0.\n real_norm = np.linalg.norm(dq[:4])\n if real_norm == 0.0:\n return np.r_[1, 0, 0, 0, dq[4:]]\n return dq / real_norm\n return dq\n\n\ndef dual_quaternion_squared_norm(dq):\n \"\"\"Compute squared norm of dual quaternion.\n\n Parameters\n ----------\n dq : array-like, shape (8)\n Dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n Returns\n -------\n squared_norm : array, shape (2,)\n Squared norm of dual quaternion, which is a dual number with a real and\n a dual part.\n \"\"\"\n dq = np.asarray(dq)\n prod = concatenate_dual_quaternions(\n dq, dq_q_conj(dq, unit=False), unit=False\n )\n return prod[[0, 4]]\n\n\ndef dual_quaternion_requires_renormalization(dq, tolerance=1e-6):\n r\"\"\"Check if dual quaternion requires renormalization.\n\n Dual quaternions that represent transformations in 3D should have unit\n norm (:math:`1 + 0 \\epsilon`), that is the real quaternion must have unit\n norm and the real and the dual quaternion must be orthogonal (their dot\n product should be 0).\n\n This function checks unit norm and orthogonality of the real and dual part.\n\n Parameters\n ----------\n dq : array-like, shape (8,)\n Dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n tolerance : float, optional (default: 1e-6)\n Tolerance for check.\n\n Returns\n -------\n required : bool\n Indicates if renormalization is required.\n\n See Also\n --------\n check_dual_quaternion\n Input validation of dual quaternion representation. Has an option to\n normalize the dual quaternion.\n\n norm_dual_quaternion\n Normalization that enforces unit norm and orthogonality of the real and\n dual quaternion.\n\n assert_unit_dual_quaternion\n Checks unit norm and orthogonality of real and dual quaternion.\n \"\"\"\n squared_norm = dual_quaternion_squared_norm(dq)\n return (\n abs(squared_norm[0] - 1.0) > tolerance\n or abs(squared_norm[1]) > tolerance\n )\n\n\ndef norm_dual_quaternion(dq):\n \"\"\"Normalize unit dual quaternion.\n\n A unit dual quaternion has a real quaternion with unit norm and an\n orthogonal real part. Both properties are enforced by multiplying a\n normalization factor [1]_. This is not always necessary. It is often\n sufficient to only enforce the unit norm property of the real quaternion.\n This can also be done with :func:`check_dual_quaternion`.\n\n Parameters\n ----------\n dq : array-like, shape (8,)\n Dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n Returns\n -------\n dq : array, shape (8,)\n Unit dual quaternion to represent transform with orthogonal real and\n dual quaternion.\n\n See Also\n --------\n check_dual_quaternion\n Input validation of dual quaternion representation. Has an option to\n normalize the dual quaternion.\n\n dual_quaternion_requires_renormalization\n Check if normalization is required.\n\n References\n ----------\n .. [1] enki (2023). Properly normalizing a dual quaternion.\n https://stackoverflow.com/a/76313524\n \"\"\"\n # 1. ensure unit norm of real quaternion\n dq = check_dual_quaternion(dq, unit=True)\n # 2. ensure orthogonality of real and dual quaternion\n real = dq[:4]\n dual = dq[4:]\n dual = dual - np.dot(real, dual) * real\n return np.hstack((real, dual))\n\n\ndef assert_unit_dual_quaternion(dq, *args, **kwargs):\n \"\"\"Raise an assertion if the dual quaternion does not have unit norm.\n\n See numpy.testing.assert_array_almost_equal for a more detailed\n documentation of the other parameters.\n\n Parameters\n ----------\n dq : array-like, shape (8,)\n Unit dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n args : tuple\n Positional arguments that will be passed to\n `assert_array_almost_equal`\n\n kwargs : dict\n Positional arguments that will be passed to\n `assert_array_almost_equal`\n\n See Also\n --------\n check_dual_quaternion\n Input validation of dual quaternion representation. Has an option to\n normalize the dual quaternion.\n\n dual_quaternion_requires_renormalization\n Check if normalization is required.\n\n norm_dual_quaternion\n Normalization that enforces unit norm and orthogonality of the real and\n dual quaternion.\n \"\"\"\n real_sq_norm, dual_sq_norm = dual_quaternion_squared_norm(dq)\n assert_array_almost_equal(real_sq_norm, 1.0, *args, **kwargs)\n assert_array_almost_equal(dual_sq_norm, 0.0, *args, **kwargs)\n\n # The two previous checks are consequences of the unit norm requirement.\n # The norm of a dual quaternion is defined as the product of a dual\n # quaternion and its quaternion conjugate.\n dq_conj = dq_q_conj(dq)\n dq_prod_dq_conj = concatenate_dual_quaternions(dq, dq_conj)\n assert_array_almost_equal(\n dq_prod_dq_conj, [1, 0, 0, 0, 0, 0, 0, 0], *args, **kwargs\n )\n\n\ndef assert_unit_dual_quaternion_equal(dq1, dq2, *args, **kwargs):\n \"\"\"Raise an assertion if unit dual quaternions are not approximately equal.\n\n Note that unit dual quaternions are equal either if dq1 == dq2 or if\n dq1 == -dq2. See numpy.testing.assert_array_almost_equal for a more\n detailed documentation of the other parameters.\n\n Parameters\n ----------\n dq1 : array-like, shape (8,)\n Unit dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n dq2 : array-like, shape (8,)\n Unit dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n args : tuple\n Positional arguments that will be passed to\n `assert_array_almost_equal`\n\n kwargs : dict\n Positional arguments that will be passed to\n `assert_array_almost_equal`\n \"\"\"\n try:\n assert_array_almost_equal(dq1, dq2, *args, **kwargs)\n except AssertionError:\n assert_array_almost_equal(dq1, -dq2, *args, **kwargs)\n\n\ndef dual_quaternion_double(dq):\n r\"\"\"Create another dual quaternion that represents the same transformation.\n\n The unit dual quaternions\n :math:`\\boldsymbol{\\sigma} = \\boldsymbol{p} + \\epsilon \\boldsymbol{q}` and\n :math:`-\\boldsymbol{\\sigma}` represent exactly the same transformation.\n The reason for this ambiguity is that the real quaternion\n :math:`\\boldsymbol{p}` represents the orientation component, the dual\n quaternion encodes the translation component as\n :math:`\\boldsymbol{q} = 0.5 \\boldsymbol{t} \\boldsymbol{p}`, where\n :math:`\\boldsymbol{t}` is a quaternion with the translation in the vector\n component and the scalar 0, and rotation quaternions have the same\n ambiguity.\n\n Parameters\n ----------\n dq : array-like, shape (8,)\n Unit dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n Returns\n -------\n dq_double : array, shape (8,)\n -dq\n \"\"\"\n return -check_dual_quaternion(dq, unit=True)\n\n\ndef dq_conj(dq, unit=True):\n \"\"\"Conjugate of dual quaternion.\n\n There are three different conjugates for dual quaternions. The one that we\n use here converts (pw, px, py, pz, qw, qx, qy, qz) to\n (pw, -px, -py, -pz, -qw, qx, qy, qz). It is a combination of the quaternion\n conjugate and the dual number conjugate.\n\n Parameters\n ----------\n dq : array-like, shape (8,)\n Unit dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n unit : bool, optional (default: True)\n Normalize the dual quaternion so that it is a unit dual quaternion.\n A unit dual quaternion has the properties\n :math:`p_w^2 + p_x^2 + p_y^2 + p_z^2 = 1` and\n :math:`p_w q_w + p_x q_x + p_y q_y + p_z q_z = 0`.\n\n Returns\n -------\n dq_conjugate : array, shape (8,)\n Conjugate of dual quaternion: (pw, -px, -py, -pz, -qw, qx, qy, qz)\n\n See Also\n --------\n dq_q_conj\n Quaternion conjugate of dual quaternion.\n \"\"\"\n dq = check_dual_quaternion(dq, unit=unit)\n return np.r_[dq[0], -dq[1:5], dq[5:]]\n\n\ndef dq_q_conj(dq, unit=True):\n \"\"\"Quaternion conjugate of dual quaternion.\n\n For unit dual quaternions that represent transformations, this function\n is equivalent to the inverse of the corresponding transformation matrix.\n\n There are three different conjugates for dual quaternions. The one that we\n use here converts (pw, px, py, pz, qw, qx, qy, qz) to\n (pw, -px, -py, -pz, qw, -qx, -qy, -qz). It is the quaternion conjugate\n applied to each of the two quaternions.\n\n Parameters\n ----------\n dq : array-like, shape (8,)\n Unit dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n unit : bool, optional (default: True)\n Normalize the dual quaternion so that it is a unit dual quaternion.\n A unit dual quaternion has the properties\n :math:`p_w^2 + p_x^2 + p_y^2 + p_z^2 = 1` and\n :math:`p_w q_w + p_x q_x + p_y q_y + p_z q_z = 0`.\n\n Returns\n -------\n dq_q_conjugate : array, shape (8,)\n Conjugate of dual quaternion: (pw, -px, -py, -pz, qw, -qx, -qy, -qz)\n\n See Also\n --------\n dq_conj\n Conjugate of a dual quaternion.\n \"\"\"\n dq = check_dual_quaternion(dq, unit=unit)\n return np.r_[dq[0], -dq[1:4], dq[4], -dq[5:]]\n\n\ndef concatenate_dual_quaternions(dq1, dq2, unit=True):\n r\"\"\"Concatenate dual quaternions.\n\n We concatenate two dual quaternions by dual quaternion multiplication\n\n .. math::\n\n (\\boldsymbol{p}_1 + \\epsilon \\boldsymbol{q}_1)\n (\\boldsymbol{p}_2 + \\epsilon \\boldsymbol{q}_2)\n = \\boldsymbol{p}_1 \\boldsymbol{p}_2 + \\epsilon (\n \\boldsymbol{p}_1 \\boldsymbol{q}_2 + \\boldsymbol{q}_1 \\boldsymbol{p}_2)\n\n using Hamilton multiplication of quaternions.\n\n .. warning::\n\n Note that the order of arguments is different than the order in\n :func:`concat`.\n\n Parameters\n ----------\n dq1 : array-like, shape (8,)\n Dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n dq2 : array-like, shape (8,)\n Dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n unit : bool, optional (default: True)\n Normalize the dual quaternion so that it is a unit dual quaternion.\n A unit dual quaternion has the properties\n :math:`p_w^2 + p_x^2 + p_y^2 + p_z^2 = 1` and\n :math:`p_w q_w + p_x q_x + p_y q_y + p_z q_z = 0`.\n\n Returns\n -------\n dq3 : array, shape (8,)\n Product of the two dual quaternions:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n See Also\n --------\n pytransform3d.rotations.concatenate_quaternions\n Quaternion multiplication.\n \"\"\"\n dq1 = check_dual_quaternion(dq1, unit=unit)\n dq2 = check_dual_quaternion(dq2, unit=unit)\n real = concatenate_quaternions(dq1[:4], dq2[:4])\n dual = concatenate_quaternions(dq1[:4], dq2[4:]) + concatenate_quaternions(\n dq1[4:], dq2[:4]\n )\n return np.hstack((real, dual))\n\n\ndef dq_prod_vector(dq, v):\n r\"\"\"Apply transform represented by a dual quaternion to a vector.\n\n To apply the transformation defined by a unit dual quaternion\n :math:`\\boldsymbol{q}` to a point :math:`\\boldsymbol{v} \\in \\mathbb{R}^3`,\n we first represent the vector as a dual quaternion: we set the real part to\n (1, 0, 0, 0) and the dual part is a pure quaternion with the scalar part\n 0 and the vector as its vector part\n :math:`\\left(\\begin{array}{c}0\\\\\\boldsymbol{v}\\end{array}\\right) \\in\n \\mathbb{R}^4`. Then we left-multiply the dual quaternion and right-multiply\n its dual quaternion conjugate\n\n .. math::\n\n \\left(\\begin{array}{c}1\\\\0\\\\0\\\\0\\\\0\\\\\\boldsymbol{w}\\end{array}\\right)\n =\n \\boldsymbol{q}\n \\cdot\n \\left(\\begin{array}{c}1\\\\0\\\\0\\\\0\\\\0\\\\\\boldsymbol{v}\\end{array}\\right)\n \\cdot\n \\boldsymbol{q}^*.\n\n The vector part of the dual part :math:`\\boldsymbol{w}` of the resulting\n quaternion is the rotated point.\n\n Parameters\n ----------\n dq : array-like, shape (8,)\n Unit dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n v : array-like, shape (3,)\n 3d vector\n\n Returns\n -------\n w : array, shape (3,)\n 3d vector\n \"\"\"\n dq = check_dual_quaternion(dq)\n v_dq = np.r_[1, 0, 0, 0, 0, v]\n v_dq_transformed = concatenate_dual_quaternions(\n concatenate_dual_quaternions(dq, v_dq), dq_conj(dq)\n )\n return v_dq_transformed[5:]\n\n\ndef dual_quaternion_sclerp(start, end, t):\n \"\"\"Screw linear interpolation (ScLERP) for dual quaternions.\n\n Although linear interpolation of dual quaternions is possible, this does\n not result in constant velocities. If you want to generate interpolations\n with constant velocity, you have to use ScLERP.\n\n Parameters\n ----------\n start : array-like, shape (8,)\n Unit dual quaternion to represent start pose:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n end : array-like, shape (8,)\n Unit dual quaternion to represent end pose:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n t : float in [0, 1]\n Position between start and goal\n\n Returns\n -------\n dq_t : array, shape (8,)\n Interpolated unit dual quaternion: (pw, px, py, pz, qw, qx, qy, qz)\n\n References\n ----------\n .. [1] Kavan, L., Collins, S., O'Sullivan, C., Zara, J. (2006).\n Dual Quaternions for Rigid Transformation Blending, Technical report,\n Trinity College Dublin,\n https://users.cs.utah.edu/~ladislav/kavan06dual/kavan06dual.pdf\n\n See Also\n --------\n transform_sclerp :\n ScLERP for transformation matrices.\n\n pq_slerp :\n An alternative approach is spherical linear interpolation (SLERP) with\n position and quaternion.\n \"\"\"\n start = check_dual_quaternion(start)\n end = check_dual_quaternion(end)\n diff = concatenate_dual_quaternions(dq_q_conj(start), end)\n return concatenate_dual_quaternions(start, dual_quaternion_power(diff, t))\n\n\ndef dual_quaternion_power(dq, t):\n r\"\"\"Compute power of unit dual quaternion with respect to scalar.\n\n .. math::\n\n (p + \\epsilon q)^t\n\n Parameters\n ----------\n dq : array-like, shape (8,)\n Unit dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n t : float\n Exponent\n\n Returns\n -------\n dq_t : array, shape (8,)\n Unit dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz) ** t\n \"\"\"\n dq = check_dual_quaternion(dq)\n q, s_axis, h, theta = screw_parameters_from_dual_quaternion(dq)\n return dual_quaternion_from_screw_parameters(q, s_axis, h, theta * t)\n\n\ndef transform_from_dual_quaternion(dq):\n \"\"\"Compute transformation matrix from dual quaternion.\n\n Parameters\n ----------\n dq : array-like, shape (8,)\n Unit dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n Returns\n -------\n A2B : array, shape (4, 4)\n Transform from frame A to frame B\n \"\"\"\n dq = check_dual_quaternion(dq)\n real = dq[:4]\n dual = dq[4:]\n R = matrix_from_quaternion(real)\n p = 2 * concatenate_quaternions(dual, q_conj(real))[1:]\n return transform_from(R=R, p=p)\n\n\ndef pq_from_dual_quaternion(dq):\n \"\"\"Compute position and quaternion from dual quaternion.\n\n Parameters\n ----------\n dq : array-like, shape (8,)\n Unit dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n Returns\n -------\n pq : array, shape (7,)\n Position and orientation quaternion: (x, y, z, qw, qx, qy, qz)\n \"\"\"\n dq = check_dual_quaternion(dq)\n real = dq[:4]\n dual = dq[4:]\n p = 2 * concatenate_quaternions(dual, q_conj(real))[1:]\n return np.hstack((p, real))\n\n\ndef screw_parameters_from_dual_quaternion(dq):\n \"\"\"Compute screw parameters from dual quaternion.\n\n Parameters\n ----------\n dq : array-like, shape (8,)\n Unit dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n\n Returns\n -------\n q : array, shape (3,)\n Vector to a point on the screw axis\n\n s_axis : array, shape (3,)\n Direction vector of the screw axis\n\n h : float\n Pitch of the screw. The pitch is the ratio of translation and rotation\n of the screw axis. Infinite pitch indicates pure translation.\n\n theta : float\n Parameter of the transformation: theta is the angle of rotation\n and h * theta the translation.\n \"\"\"\n dq = check_dual_quaternion(dq, unit=True)\n\n real = dq[:4]\n dual = dq[4:]\n\n a = axis_angle_from_quaternion(real)\n s_axis = a[:3]\n theta = a[3]\n\n translation = 2 * concatenate_quaternions(dual, q_conj(real))[1:]\n if abs(theta) < np.finfo(float).eps:\n # pure translation\n d = np.linalg.norm(translation)\n if d < np.finfo(float).eps:\n s_axis = np.array([1, 0, 0])\n else:\n s_axis = translation / d\n q = np.zeros(3)\n theta = d\n h = np.inf\n return q, s_axis, h, theta\n\n distance = np.dot(translation, s_axis)\n moment = 0.5 * (\n np.cross(translation, s_axis)\n + (translation - distance * s_axis) / np.tan(0.5 * theta)\n )\n dual = np.cross(s_axis, moment)\n h = distance / theta\n return dual, s_axis, h, theta\n" }, { "path": "pytransform3d/transformations/_dual_quaternion.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef check_dual_quaternion(\n dq: npt.ArrayLike, unit: bool = ...\n) -> np.ndarray: ...\ndef dual_quaternion_squared_norm(dq: npt.ArrayLike) -> np.ndarray: ...\ndef dual_quaternion_requires_renormalization(\n dq: npt.ArrayLike, tolerance: float = ...\n) -> bool: ...\ndef norm_dual_quaternion(dq: npt.ArrayLike) -> np.ndarray: ...\ndef assert_unit_dual_quaternion(dq: npt.ArrayLike, *args, **kwargs): ...\ndef assert_unit_dual_quaternion_equal(\n dq1: npt.ArrayLike, dq2: npt.ArrayLike, *args, **kwargs\n): ...\ndef dual_quaternion_double(dq: npt.ArrayLike) -> np.ndarray: ...\ndef dq_conj(dq: npt.ArrayLike, unit: bool = ...) -> np.ndarray: ...\ndef dq_q_conj(dq: npt.ArrayLike, unit: bool = ...) -> np.ndarray: ...\ndef concatenate_dual_quaternions(\n dq1: npt.ArrayLike, dq2: npt.ArrayLike, unit: bool = ...\n) -> np.ndarray: ...\ndef dq_prod_vector(dq: npt.ArrayLike, v: npt.ArrayLike) -> np.ndarray: ...\ndef dual_quaternion_sclerp(\n start: npt.ArrayLike, end: npt.ArrayLike, t: float\n) -> np.ndarray: ...\ndef dual_quaternion_power(dq: npt.ArrayLike, t: float) -> np.ndarray: ...\ndef transform_from_dual_quaternion(dq: npt.ArrayLike) -> np.ndarray: ...\ndef pq_from_dual_quaternion(dq: npt.ArrayLike) -> np.ndarray: ...\ndef screw_parameters_from_dual_quaternion(dq: npt.ArrayLike) -> np.ndarray: ...\n" }, { "path": "pytransform3d/transformations/_jacobians.py", "content": "\"\"\"Jacobians of SE(3).\"\"\"\n\nimport math\n\nimport numpy as np\n\nfrom ._screws import (\n check_exponential_coordinates,\n screw_axis_from_exponential_coordinates,\n)\nfrom ..rotations import (\n cross_product_matrix,\n left_jacobian_SO3,\n left_jacobian_SO3_inv,\n)\n\n\ndef left_jacobian_SE3(Stheta):\n r\"\"\"Left Jacobian of SE(3).\n\n .. math::\n\n \\boldsymbol{\\mathcal{J}}\n =\n \\left(\n \\begin{array}{cc}\n \\boldsymbol{J} & \\boldsymbol{0}\\\\\n \\boldsymbol{Q} & \\boldsymbol{J}\n \\end{array}\n \\right),\n\n where :math:`\\boldsymbol{J}` is the left Jacobian of SO(3) and\n :math:`\\boldsymbol{Q}` is given by Barfoot and Furgale (see reference\n below).\n\n Parameters\n ----------\n Stheta : array-like, shape (6,)\n Exponential coordinates of transformation:\n S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,\n where S is the screw axis, the first 3 components are related to\n rotation and the last 3 components are related to translation.\n Theta is the rotation angle and h * theta the translation.\n\n Returns\n -------\n J : array, shape (6, 6)\n Jacobian of SE(3).\n\n See Also\n --------\n left_jacobian_SE3_series :\n Left Jacobian of SE(3) at theta from Taylor series.\n\n left_jacobian_SE3_inv : Left inverse Jacobian of SE(3).\n\n References\n ----------\n .. [1] Barfoot, T. D., Furgale, P. T. (2014).\n Associating Uncertainty With Three-Dimensional Poses for Use in\n Estimation Problems. IEEE Transactions on Robotics, 30(3), pp. 679-693,\n doi: 10.1109/TRO.2014.2298059.\n \"\"\"\n Stheta = check_exponential_coordinates(Stheta)\n\n _, theta = screw_axis_from_exponential_coordinates(Stheta)\n if theta < np.finfo(float).eps:\n return left_jacobian_SE3_series(Stheta, 10)\n\n phi = Stheta[:3]\n J = left_jacobian_SO3(phi)\n return np.block([[J, np.zeros((3, 3))], [_Q(Stheta), J]])\n\n\ndef left_jacobian_SE3_series(Stheta, n_terms):\n \"\"\"Left Jacobian of SE(3) at theta from Taylor series.\n\n Parameters\n ----------\n Stheta : array-like, shape (6,)\n Exponential coordinates of transformation:\n S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,\n where S is the screw axis, the first 3 components are related to\n rotation and the last 3 components are related to translation.\n Theta is the rotation angle and h * theta the translation.\n\n n_terms : int\n Number of terms to include in the series.\n\n Returns\n -------\n J : array, shape (3, 3)\n Left Jacobian of SE(3).\n\n See Also\n --------\n left_jacobian_SE3 : Left Jacobian of SE(3).\n \"\"\"\n Stheta = check_exponential_coordinates(Stheta)\n J = np.eye(6)\n pxn = np.eye(6)\n px = _curlyhat(Stheta)\n for n in range(n_terms):\n pxn = np.dot(pxn, px) / (n + 2)\n J += pxn\n return J\n\n\ndef left_jacobian_SE3_inv(Stheta):\n r\"\"\"Left inverse Jacobian of SE(3).\n\n .. math::\n\n \\boldsymbol{\\mathcal{J}}^{-1}\n =\n \\left(\n \\begin{array}{cc}\n \\boldsymbol{J}^{-1} & \\boldsymbol{0}\\\\\n -\\boldsymbol{J}^{-1}\\boldsymbol{Q}\\boldsymbol{J}^{-1} &\n \\boldsymbol{J}^{-1}\n \\end{array}\n \\right),\n\n where :math:`\\boldsymbol{J}` is the left Jacobian of SO(3) and\n :math:`\\boldsymbol{Q}` is given by Barfoot and Furgale (see reference\n below).\n\n Parameters\n ----------\n Stheta : array-like, shape (6,)\n Exponential coordinates of transformation:\n S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,\n where S is the screw axis, the first 3 components are related to\n rotation and the last 3 components are related to translation.\n Theta is the rotation angle and h * theta the translation.\n\n Returns\n -------\n J_inv : array, shape (6, 6)\n Inverse Jacobian of SE(3).\n\n See Also\n --------\n left_jacobian_SE3 : Left Jacobian of SE(3).\n\n left_jacobian_SE3_inv_series :\n Left inverse Jacobian of SE(3) at theta from Taylor series.\n \"\"\"\n Stheta = check_exponential_coordinates(Stheta)\n\n _, theta = screw_axis_from_exponential_coordinates(Stheta)\n if theta < np.finfo(float).eps:\n return left_jacobian_SE3_inv_series(Stheta, 10)\n\n phi = Stheta[:3]\n J_inv = left_jacobian_SO3_inv(phi)\n return np.block(\n [\n [J_inv, np.zeros((3, 3))],\n [-np.dot(J_inv, np.dot(_Q(Stheta), J_inv)), J_inv],\n ]\n )\n\n\ndef _Q(Stheta):\n rho = Stheta[3:]\n phi = Stheta[:3]\n ph = np.linalg.norm(phi)\n\n px = cross_product_matrix(phi)\n rx = cross_product_matrix(rho)\n\n ph2 = ph * ph\n ph3 = ph2 * ph\n ph4 = ph3 * ph\n ph5 = ph4 * ph\n\n cph = math.cos(ph)\n sph = math.sin(ph)\n\n t1 = 0.5 * rx\n t2 = (\n (ph - sph)\n / ph3\n * (np.dot(px, rx) + np.dot(rx, px) + np.dot(px, np.dot(rx, px)))\n )\n m3 = (1.0 - 0.5 * ph * ph - cph) / ph4\n t3 = -m3 * (\n np.dot(px, np.dot(px, rx))\n + np.dot(rx, np.dot(px, px))\n - 3 * np.dot(px, np.dot(rx, px))\n )\n m4 = 0.5 * (m3 - 3.0 * (ph - sph - ph3 / 6.0) / ph5)\n t4 = -m4 * (\n np.dot(px, np.dot(rx, np.dot(px, px)))\n + np.dot(px, np.dot(px, np.dot(rx, px)))\n )\n\n Q = t1 + t2 + t3 + t4\n\n return Q\n\n\ndef left_jacobian_SE3_inv_series(Stheta, n_terms):\n \"\"\"Left inverse Jacobian of SE(3) at theta from Taylor series.\n\n Parameters\n ----------\n Stheta : array-like, shape (6,)\n Exponential coordinates of transformation:\n S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,\n where S is the screw axis, the first 3 components are related to\n rotation and the last 3 components are related to translation.\n Theta is the rotation angle and h * theta the translation.\n\n n_terms : int\n Number of terms to include in the series.\n\n Returns\n -------\n J_inv : array, shape (3, 3)\n Inverse left Jacobian of SE(3).\n\n See Also\n --------\n left_jacobian_SE3_inv : Left inverse Jacobian of SE(3).\n \"\"\"\n from scipy.special import bernoulli\n\n Stheta = check_exponential_coordinates(Stheta)\n J_inv = np.eye(6)\n pxn = np.eye(6)\n px = _curlyhat(Stheta)\n b = bernoulli(n_terms + 1)\n for n in range(n_terms):\n pxn = np.dot(pxn, px / (n + 1))\n J_inv += b[n + 1] * pxn\n return J_inv\n\n\ndef _curlyhat(Stheta):\n omega_matrix = cross_product_matrix(Stheta[:3])\n return np.block(\n [\n [omega_matrix, np.zeros((3, 3))],\n [cross_product_matrix(Stheta[3:]), omega_matrix],\n ]\n )\n" }, { "path": "pytransform3d/transformations/_jacobians.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef left_jacobian_SE3(Stheta: npt.ArrayLike) -> np.ndarray: ...\ndef left_jacobian_SE3_series(\n Stheta: npt.ArrayLike, n_terms: int\n) -> np.ndarray: ...\ndef left_jacobian_SE3_inv(Stheta: npt.ArrayLike) -> np.ndarray: ...\ndef left_jacobian_SE3_inv_series(\n Stheta: npt.ArrayLike, n_terms: int\n) -> np.ndarray: ...\n" }, { "path": "pytransform3d/transformations/_plot.py", "content": "\"\"\"Plotting utilities.\"\"\"\n\nimport numpy as np\n\nfrom ._screws import check_screw_parameters\nfrom ._transform import check_transform\nfrom ._transform_operations import (\n transform,\n vector_to_point,\n vector_to_direction,\n vectors_to_points,\n)\n\n\ndef plot_transform(\n ax=None, A2B=None, s=1.0, ax_s=1, name=None, strict_check=True, **kwargs\n):\n \"\"\"Plot transform.\n\n Parameters\n ----------\n ax : Matplotlib 3d axis, optional (default: None)\n If the axis is None, a new 3d axis will be created\n\n A2B : array-like, shape (4, 4), optional (default: I)\n Transform from frame A to frame B\n\n s : float, optional (default: 1)\n Scaling of the axis and angle that will be drawn\n\n ax_s : float, optional (default: 1)\n Scaling of the new matplotlib 3d axis\n\n name : string, optional (default: None)\n Name of the frame, will be used for annotation\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the transformation matrix is not numerically\n close enough to a real transformation matrix. Otherwise we print a\n warning.\n\n kwargs : dict, optional (default: {})\n Additional arguments for the plotting functions, e.g. alpha\n\n Returns\n -------\n ax : Matplotlib 3d axis\n New or old axis\n \"\"\"\n from ..plot_utils import make_3d_axis, Frame\n\n if ax is None:\n ax = make_3d_axis(ax_s)\n\n if A2B is None:\n A2B = np.eye(4)\n A2B = check_transform(A2B, strict_check=strict_check)\n\n frame = Frame(A2B, name, s, **kwargs)\n frame.add_frame(ax)\n\n return ax\n\n\ndef plot_screw(\n ax=None,\n q=np.zeros(3),\n s_axis=np.array([1.0, 0.0, 0.0]),\n h=1.0,\n theta=1.0,\n A2B=None,\n s=1.0,\n ax_s=1,\n alpha=1.0,\n **kwargs,\n):\n \"\"\"Plot transformation about and along screw axis.\n\n Parameters\n ----------\n ax : Matplotlib 3d axis, optional (default: None)\n If the axis is None, a new 3d axis will be created\n\n q : array-like, shape (3,), optional (default: [0, 0, 0])\n Vector to a point on the screw axis\n\n s_axis : array-like, shape (3,), optional (default: [1, 0, 0])\n Direction vector of the screw axis\n\n h : float, optional (default: 1)\n Pitch of the screw. The pitch is the ratio of translation and rotation\n of the screw axis. Infinite pitch indicates pure translation.\n\n theta : float, optional (default: 1)\n Rotation angle. h * theta is the translation.\n\n A2B : array-like, shape (4, 4), optional (default: I)\n Origin of the screw\n\n s : float, optional (default: 1)\n Scaling of the axis and angle that will be drawn\n\n ax_s : float, optional (default: 1)\n Scaling of the new matplotlib 3d axis\n\n alpha : float, optional (default: 1)\n Alpha channel of plotted lines\n\n kwargs : dict, optional (default: {})\n Additional arguments for the plotting functions, e.g. color\n\n Returns\n -------\n ax : Matplotlib 3d axis\n New or old axis\n \"\"\"\n from ..plot_utils import make_3d_axis, Arrow3D\n from ..rotations import (\n vector_projection,\n angle_between_vectors,\n perpendicular_to_vectors,\n slerp_weights,\n )\n\n if ax is None:\n ax = make_3d_axis(ax_s)\n\n if A2B is None:\n A2B = np.eye(4)\n\n q, s_axis, h = check_screw_parameters(q, s_axis, h)\n\n origin_projected_on_screw_axis = q + vector_projection(-q, s_axis)\n\n pure_translation = np.isinf(h)\n\n if not pure_translation:\n screw_axis_to_old_frame = -origin_projected_on_screw_axis\n screw_axis_to_rotated_frame = perpendicular_to_vectors(\n s_axis, screw_axis_to_old_frame\n )\n screw_axis_to_translated_frame = h * s_axis\n\n arc = np.empty((100, 3))\n angle = angle_between_vectors(\n screw_axis_to_old_frame, screw_axis_to_rotated_frame\n )\n for i, t in enumerate(\n zip(\n np.linspace(0, 2 * theta / np.pi, len(arc)),\n np.linspace(0.0, 1.0, len(arc)),\n )\n ):\n t1, t2 = t\n w1, w2 = slerp_weights(angle, t1)\n arc[i] = (\n origin_projected_on_screw_axis\n + w1 * screw_axis_to_old_frame\n + w2 * screw_axis_to_rotated_frame\n + screw_axis_to_translated_frame * t2 * theta\n )\n\n q = transform(A2B, vector_to_point(q))[:3]\n s_axis = transform(A2B, vector_to_direction(s_axis))[:3]\n if not pure_translation:\n arc = transform(A2B, vectors_to_points(arc))[:, :3]\n origin_projected_on_screw_axis = transform(\n A2B, vector_to_point(origin_projected_on_screw_axis)\n )[:3]\n\n # Screw axis\n ax.scatter(q[0], q[1], q[2], color=\"r\")\n if pure_translation:\n s_axis *= theta\n ax.scatter(\n q[0] + s_axis[0], q[1] + s_axis[1], q[2] + s_axis[2], color=\"r\"\n )\n ax.plot(\n [q[0] - s * s_axis[0], q[0] + (1 + s) * s_axis[0]],\n [q[1] - s * s_axis[1], q[1] + (1 + s) * s_axis[1]],\n [q[2] - s * s_axis[2], q[2] + (1 + s) * s_axis[2]],\n \"--\",\n c=\"k\",\n alpha=alpha,\n )\n axis_arrow = Arrow3D(\n [q[0], q[0] + s_axis[0]],\n [q[1], q[1] + s_axis[1]],\n [q[2], q[2] + s_axis[2]],\n mutation_scale=20,\n lw=3,\n arrowstyle=\"-|>\",\n color=\"k\",\n alpha=alpha,\n )\n ax.add_artist(axis_arrow)\n\n if not pure_translation:\n # Transformation\n ax.plot(\n arc[:, 0],\n arc[:, 1],\n arc[:, 2],\n color=\"k\",\n lw=3,\n alpha=alpha,\n **kwargs,\n )\n arrow_coords = np.vstack((arc[-1], arc[-1] + (arc[-1] - arc[-2]))).T\n angle_arrow = Arrow3D(\n arrow_coords[0],\n arrow_coords[1],\n arrow_coords[2],\n mutation_scale=20,\n lw=3,\n arrowstyle=\"-|>\",\n color=\"k\",\n alpha=alpha,\n )\n ax.add_artist(angle_arrow)\n\n for i in [0, -1]:\n arc_bound = np.vstack((origin_projected_on_screw_axis, arc[i])).T\n ax.plot(\n arc_bound[0],\n arc_bound[1],\n arc_bound[2],\n \"--\",\n c=\"k\",\n alpha=alpha,\n )\n\n return ax\n" }, { "path": "pytransform3d/transformations/_plot.pyi", "content": "import numpy as np\nimport numpy.typing as npt\nfrom mpl_toolkits.mplot3d import Axes3D\nfrom typing import Union\n\ndef plot_transform(\n ax: Union[None, Axes3D] = ...,\n A2B: Union[None, npt.ArrayLike] = ...,\n s: float = ...,\n ax_s: float = ...,\n name: Union[None, str] = ...,\n strict_check: bool = ...,\n **kwargs,\n) -> Axes3D: ...\ndef plot_screw(\n ax: Union[None, Axes3D] = ...,\n q: npt.ArrayLike = ...,\n s_axis: npt.ArrayLike = ...,\n h: float = ...,\n theta: float = ...,\n A2B: Union[None, npt.ArrayLike] = ...,\n s: float = ...,\n ax_s: float = ...,\n alpha: float = ...,\n **kwargs,\n) -> Axes3D: ...\n" }, { "path": "pytransform3d/transformations/_pq.py", "content": "\"\"\"Position+quaternion operations.\"\"\"\n\nimport numpy as np\n\nfrom ._transform import transform_from\nfrom .. import rotations\n\n\ndef check_pq(pq):\n \"\"\"Input validation for position and orientation quaternion.\n\n Parameters\n ----------\n pq : array-like, shape (7,)\n Position and orientation quaternion: (x, y, z, qw, qx, qy, qz)\n\n Returns\n -------\n pq : array, shape (7,)\n Validated position and orientation quaternion:\n (x, y, z, qw, qx, qy, qz)\n\n Raises\n ------\n ValueError\n If input is invalid\n \"\"\"\n pq = np.asarray(pq, dtype=np.float64)\n if pq.ndim != 1 or pq.shape[0] != 7:\n raise ValueError(\n \"Expected position and orientation quaternion in a \"\n \"1D array, got array-like object with shape %s\" % (pq.shape,)\n )\n return pq\n\n\ndef pq_slerp(start, end, t):\n \"\"\"Spherical linear interpolation of position and quaternion.\n\n We will use spherical linear interpolation (SLERP) for the quaternion and\n linear interpolation for the position.\n\n Parameters\n ----------\n start : array-like, shape (7,)\n Position and orientation quaternion: (x, y, z, qw, qx, qy, qz)\n\n end : array-like, shape (7,)\n Position and orientation quaternion: (x, y, z, qw, qx, qy, qz)\n\n t : float in [0, 1]\n Position between start and end\n\n Returns\n -------\n pq_t : array-like, shape (7,)\n Position and orientation quaternion: (x, y, z, qw, qx, qy, qz)\n\n See Also\n --------\n dual_quaternion_sclerp :\n An alternative approach is screw linear interpolation (ScLERP) with\n dual quaternions.\n\n pytransform3d.rotations.axis_angle_slerp :\n SLERP for axis-angle representation.\n\n pytransform3d.rotations.quaternion_slerp :\n SLERP for quaternions.\n\n pytransform3d.rotations.rotor_slerp :\n SLERP for rotors.\n \"\"\"\n start = check_pq(start)\n end = check_pq(end)\n start_p, start_q = np.array_split(start, (3,))\n end_p, end_q = np.array_split(end, (3,))\n q_t = rotations.quaternion_slerp(start_q, end_q, t, shortest_path=True)\n p_t = start_p + t * (end_p - start_p)\n return np.hstack((p_t, q_t))\n\n\ndef transform_from_pq(pq):\n \"\"\"Compute transformation matrix from position and quaternion.\n\n Parameters\n ----------\n pq : array-like, shape (7,)\n Position and orientation quaternion: (x, y, z, qw, qx, qy, qz)\n\n Returns\n -------\n A2B : array, shape (4, 4)\n Transformation matrix from frame A to frame B\n \"\"\"\n pq = check_pq(pq)\n return transform_from(rotations.matrix_from_quaternion(pq[3:]), pq[:3])\n\n\ndef dual_quaternion_from_pq(pq):\n \"\"\"Compute dual quaternion from position and quaternion.\n\n Parameters\n ----------\n pq : array-like, shape (7,)\n Position and orientation quaternion: (x, y, z, qw, qx, qy, qz)\n\n Returns\n -------\n dq : array, shape (8,)\n Unit dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n \"\"\"\n pq = check_pq(pq)\n real = pq[3:]\n dual = 0.5 * rotations.concatenate_quaternions(np.r_[0, pq[:3]], real)\n return np.hstack((real, dual))\n" }, { "path": "pytransform3d/transformations/_pq.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef check_pq(pq: npt.ArrayLike) -> np.ndarray: ...\ndef pq_slerp(\n start: npt.ArrayLike, end: npt.ArrayLike, t: float\n) -> np.ndarray: ...\ndef transform_from_pq(pq: npt.ArrayLike) -> np.ndarray: ...\ndef dual_quaternion_from_pq(pq: npt.ArrayLike) -> np.ndarray: ...\n" }, { "path": "pytransform3d/transformations/_random.py", "content": "\"\"\"Random transform generation.\"\"\"\n\nimport numpy as np\n\nfrom ._screws import transform_from_exponential_coordinates\nfrom ._transform import check_transform\nfrom ..rotations import norm_vector\n\n\ndef random_transform(\n rng=np.random.default_rng(0), mean=np.eye(4), cov=np.eye(6)\n):\n r\"\"\"Generate random transform.\n\n Generate :math:`\\Delta \\boldsymbol{T}_{B_{i+1}{B_i}}\n \\boldsymbol{T}_{{B_i}A}`, with :math:`\\Delta \\boldsymbol{T}_{B_{i+1}{B_i}}\n = Exp(S \\theta)` and :math:`\\mathcal{S}\\theta \\sim\n \\mathcal{N}(\\boldsymbol{0}_6, \\boldsymbol{\\Sigma}_{6 \\times 6})`.\n The mean :math:`\\boldsymbol{T}_{{B_i}A}` and the covariance\n :math:`\\boldsymbol{\\Sigma}_{6 \\times 6}` are parameters of the function.\n\n Note that uncertainty is defined in the global frame B, not in the\n body frame A.\n\n Parameters\n ----------\n rng : np.random.Generator, optional (default: random seed 0)\n Random number generator\n\n mean : array-like, shape (4, 4), optional (default: I)\n Mean transform as homogeneous transformation matrix.\n\n cov : array-like, shape (6, 6), optional (default: I)\n Covariance of noise in exponential coordinate space.\n\n Returns\n -------\n A2B : array, shape (4, 4)\n Random transform from frame A to frame B\n \"\"\"\n mean = check_transform(mean)\n Stheta = random_exponential_coordinates(rng=rng, cov=cov)\n delta = transform_from_exponential_coordinates(Stheta)\n return np.dot(delta, mean)\n\n\ndef random_screw_axis(rng=np.random.default_rng(0)):\n r\"\"\"Generate random screw axis.\n\n Each component of v will be sampled from a standard normal distribution\n :math:`\\mathcal{N}(\\mu=0, \\sigma=1)`. Components of :math:`\\omega` will\n be sampled from a standard normal distribution and normalized.\n\n Parameters\n ----------\n rng : np.random.Generator, optional (default: random seed 0)\n Random number generator\n\n Returns\n -------\n screw_axis : array, shape (6,)\n Screw axis described by 6 values\n (omega_1, omega_2, omega_3, v_1, v_2, v_3),\n where the first 3 components are related to rotation and the last 3\n components are related to translation.\n \"\"\"\n omega = norm_vector(rng.standard_normal(size=3))\n v = rng.standard_normal(size=3)\n return np.hstack((omega, v))\n\n\ndef random_exponential_coordinates(rng=np.random.default_rng(0), cov=np.eye(6)):\n r\"\"\"Generate random exponential coordinates.\n\n Each component of Stheta will be sampled from a standard normal\n distribution :math:`\\mathcal{N}(\\boldsymbol{0}_6,\n \\boldsymbol{\\Sigma}_{6 \\times 6})`.\n\n Parameters\n ----------\n rng : np.random.Generator, optional (default: random seed 0)\n Random number generator\n\n cov : array-like, shape (6, 6), optional (default: I)\n Covariance of normal distribution.\n\n Returns\n -------\n Stheta : array, shape (6,)\n Exponential coordinates of transformation:\n S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,\n where the first 3 components are related to rotation and the last 3\n components are related to translation. Theta is the rotation angle\n and h * theta the translation. Theta should be >= 0. Negative rotations\n will be represented by a negative screw axis instead. This is relevant\n if you want to recover theta from exponential coordinates.\n \"\"\"\n return rng.multivariate_normal(mean=np.zeros(6), cov=cov)\n" }, { "path": "pytransform3d/transformations/_random.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef random_transform(\n rng: np.random.Generator = ...,\n mean: npt.ArrayLike = ...,\n cov: npt.ArrayLike = ...,\n) -> np.ndarray: ...\ndef random_screw_axis(rng: np.random.Generator = ...) -> np.ndarray: ...\ndef random_exponential_coordinates(\n rng: np.random.Generator = ..., cov: npt.ArrayLike = ...\n) -> np.ndarray: ...\n" }, { "path": "pytransform3d/transformations/_screws.py", "content": "\"\"\"Conversions between transform representations.\"\"\"\n\nimport numpy as np\nfrom numpy.testing import assert_array_almost_equal\n\nfrom ._transform import translate_transform\nfrom ..rotations import (\n eps,\n norm_vector,\n norm_angle,\n cross_product_matrix,\n matrix_from_compact_axis_angle,\n check_skew_symmetric_matrix,\n left_jacobian_SO3,\n)\n\n\ndef check_screw_parameters(q, s_axis, h):\n r\"\"\"Input validation of screw parameters.\n\n The parameters :math:`(\\boldsymbol{q}, \\hat{\\boldsymbol{s}}, h)`\n describe a screw.\n\n Parameters\n ----------\n q : array-like, shape (3,)\n Vector to a point on the screw axis\n\n s_axis : array-like, shape (3,)\n Direction vector of the screw axis\n\n h : float\n Pitch of the screw. The pitch is the ratio of translation and rotation\n of the screw axis. Infinite pitch indicates pure translation.\n\n Returns\n -------\n q : array, shape (3,)\n Vector to a point on the screw axis. Will be set to zero vector when\n pitch is infinite (pure translation).\n\n s_axis : array, shape (3,)\n Unit direction vector of the screw axis\n\n h : float\n Pitch of the screw. The pitch is the ratio of translation and rotation\n of the screw axis. Infinite pitch indicates pure translation.\n\n Raises\n ------\n ValueError\n If input is invalid\n \"\"\"\n s_axis = np.asarray(s_axis, dtype=np.float64)\n if s_axis.ndim != 1 or s_axis.shape[0] != 3:\n raise ValueError(\n \"Expected 3D vector with shape (3,), got array-like \"\n \"object with shape %s\" % (s_axis.shape,)\n )\n if np.linalg.norm(s_axis) == 0.0:\n raise ValueError(\"s_axis must not have norm 0\")\n\n q = np.asarray(q, dtype=np.float64)\n if q.ndim != 1 or q.shape[0] != 3:\n raise ValueError(\n \"Expected 3D vector with shape (3,), got array-like \"\n \"object with shape %s\" % (q.shape,)\n )\n if np.isinf(h): # pure translation\n q = np.zeros(3)\n\n return q, norm_vector(s_axis), h\n\n\ndef check_screw_axis(screw_axis):\n r\"\"\"Input validation of screw axis.\n\n A screw axis\n\n .. math::\n\n \\mathcal{S}\n = \\left[\\begin{array}{c}\\boldsymbol{\\omega}\\\\\n \\boldsymbol{v}\\end{array}\\right] \\in \\mathbb{R}^6\n\n consists of a part that describes rotation and a part that describes\n translation.\n\n Parameters\n ----------\n screw_axis : array-like, shape (6,)\n Screw axis described by 6 values\n (omega_1, omega_2, omega_3, v_1, v_2, v_3),\n where the first 3 components are related to rotation and the last 3\n components are related to translation.\n\n Returns\n -------\n screw_axis : array, shape (6,)\n Screw axis described by 6 values\n (omega_1, omega_2, omega_3, v_1, v_2, v_3),\n where the first 3 components are related to rotation and the last 3\n components are related to translation.\n\n Raises\n ------\n ValueError\n If input is invalid\n \"\"\"\n screw_axis = np.asarray(screw_axis, dtype=np.float64)\n if screw_axis.ndim != 1 or screw_axis.shape[0] != 6:\n raise ValueError(\n \"Expected 3D vector with shape (6,), got array-like \"\n \"object with shape %s\" % (screw_axis.shape,)\n )\n\n omega_norm = np.linalg.norm(screw_axis[:3])\n if (\n abs(omega_norm - 1.0) > 10.0 * np.finfo(float).eps\n and abs(omega_norm) > 10.0 * np.finfo(float).eps\n ):\n raise ValueError(\n \"Norm of rotation axis must either be 0 or 1, but it is %g.\"\n % omega_norm\n )\n if abs(omega_norm) < np.finfo(float).eps:\n v_norm = np.linalg.norm(screw_axis[3:])\n if abs(v_norm - 1.0) > np.finfo(float).eps:\n raise ValueError(\n \"If the norm of the rotation axis is 0, then the direction \"\n \"vector must have norm 1, but it is %g.\" % v_norm\n )\n\n return screw_axis\n\n\ndef check_exponential_coordinates(Stheta):\n \"\"\"Input validation for exponential coordinates of transformation.\n\n Exponential coordinates of a transformation :math:`\\\\mathcal{S}\\\\theta\n \\\\in \\\\mathbb{R}^6` are the product of a screw axis and a scalar\n :math:`\\\\theta`.\n\n Parameters\n ----------\n Stheta : array-like, shape (6,)\n Exponential coordinates of transformation:\n S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,\n where the first 3 components are related to rotation and the last 3\n components are related to translation. Theta is the rotation angle\n and h * theta the translation. Theta should be >= 0. Negative rotations\n will be represented by a negative screw axis instead. This is relevant\n if you want to recover theta from exponential coordinates.\n\n Returns\n -------\n Stheta : array, shape (6,)\n Exponential coordinates of transformation:\n S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,\n where the first 3 components are related to rotation and the last 3\n components are related to translation. Theta is the rotation angle\n and h * theta the translation. Theta should be >= 0. Negative rotations\n will be represented by a negative screw axis instead. This is relevant\n if you want to recover theta from exponential coordinates.\n\n Raises\n ------\n ValueError\n If input is invalid\n \"\"\"\n Stheta = np.asarray(Stheta, dtype=np.float64)\n if Stheta.ndim != 1 or Stheta.shape[0] != 6:\n raise ValueError(\n \"Expected array-like with shape (6,), got array-like \"\n \"object with shape %s\" % (Stheta.shape,)\n )\n return Stheta\n\n\ndef check_screw_matrix(screw_matrix, tolerance=1e-6, strict_check=True):\n \"\"\"Input validation for screw matrix.\n\n A screw matrix consists of the cross-product matrix of a rotation\n axis and a translation.\n\n .. math::\n\n \\\\left[\\\\mathcal S\\\\right]\n =\n \\\\left( \\\\begin{array}{cc}\n \\\\left[\\\\boldsymbol{\\\\omega}\\\\right] & \\\\boldsymbol v\\\\\\\\\n \\\\boldsymbol 0 & 0\\\\\\\\\n \\\\end{array} \\\\right)\n =\n \\\\left(\n \\\\begin{matrix}\n 0 & -\\\\omega_3 & \\\\omega_2 & v_1\\\\\\\\\n \\\\omega_3 & 0 & -\\\\omega_1 & v_2\\\\\\\\\n -\\\\omega_2 & \\\\omega_1 & 0 & v_3\\\\\\\\\n 0 & 0 & 0 & 0\\\\\\\\\n \\\\end{matrix}\n \\\\right)\n \\\\in se(3) \\\\subset \\\\mathbb{R}^{4 \\\\times 4}\n\n Parameters\n ----------\n screw_matrix : array-like, shape (4, 4)\n A screw matrix consists of a cross-product matrix that represents an\n axis of rotation, a translation, and a row of zeros.\n\n tolerance : float, optional (default: 1e-6)\n Tolerance threshold for checks.\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if [omega].T is not numerically close enough to\n -[omega]. Otherwise we print a warning.\n\n Returns\n -------\n screw_matrix : array, shape (4, 4)\n A screw matrix consists of a cross-product matrix that represents an\n axis of rotation, a translation, and a row of zeros.\n\n Raises\n ------\n ValueError\n If input is invalid\n \"\"\"\n screw_matrix = np.asarray(screw_matrix, dtype=np.float64)\n if (\n screw_matrix.ndim != 2\n or screw_matrix.shape[0] != 4\n or screw_matrix.shape[1] != 4\n ):\n raise ValueError(\n \"Expected array-like with shape (4, 4), got array-like \"\n \"object with shape %s\" % (screw_matrix.shape,)\n )\n if any(screw_matrix[3] != 0.0):\n raise ValueError(\"Last row of screw matrix must only contains zeros.\")\n\n check_skew_symmetric_matrix(screw_matrix[:3, :3], tolerance, strict_check)\n\n omega_norm = np.linalg.norm(\n [screw_matrix[2, 1], screw_matrix[0, 2], screw_matrix[1, 0]]\n )\n\n if (\n abs(omega_norm - 1.0) > np.finfo(float).eps\n and abs(omega_norm) > np.finfo(float).eps\n ):\n raise ValueError(\n \"Norm of rotation axis must either be 0 or 1, but it is %g.\"\n % omega_norm\n )\n if abs(omega_norm) < np.finfo(float).eps:\n v_norm = np.linalg.norm(screw_matrix[:3, 3])\n if (\n abs(v_norm - 1.0) > np.finfo(float).eps\n and abs(v_norm) > np.finfo(float).eps\n ):\n raise ValueError(\n \"If the norm of the rotation axis is 0, then the direction \"\n \"vector must have norm 1 or 0, but it is %g.\" % v_norm\n )\n\n return screw_matrix\n\n\ndef check_transform_log(transform_log, tolerance=1e-6, strict_check=True):\n \"\"\"Input validation for logarithm of transformation.\n\n The logarithm of a transformation :math:`\\\\left[\\\\mathcal{S}\\\\right]\\\\theta\n \\\\in se(3) \\\\subset \\\\mathbb{R}^{4 \\\\times 4}` are the product of a screw\n matrix and a scalar :math:`\\\\theta`.\n\n Parameters\n ----------\n transform_log : array-like, shape (4, 4)\n Matrix logarithm of transformation matrix: [S] * theta.\n\n tolerance : float, optional (default: 1e-6)\n Tolerance threshold for checks.\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if [omega].T is not numerically close enough to\n -[omega]. Otherwise we print a warning.\n\n Returns\n -------\n transform_log : array, shape (4, 4)\n Matrix logarithm of transformation matrix: [S] * theta.\n\n Raises\n ------\n ValueError\n If input is invalid\n \"\"\"\n transform_log = np.asarray(transform_log, dtype=np.float64)\n if (\n transform_log.ndim != 2\n or transform_log.shape[0] != 4\n or transform_log.shape[1] != 4\n ):\n raise ValueError(\n \"Expected array-like with shape (4, 4), got array-like \"\n \"object with shape %s\" % (transform_log.shape,)\n )\n if any(transform_log[3] != 0.0):\n raise ValueError(\n \"Last row of logarithm of transformation must only \"\n \"contains zeros.\"\n )\n\n check_skew_symmetric_matrix(transform_log[:3, :3], tolerance, strict_check)\n\n return transform_log\n\n\ndef norm_exponential_coordinates(Stheta):\n \"\"\"Normalize exponential coordinates of transformation.\n\n Parameters\n ----------\n Stheta : array-like, shape (6,)\n Exponential coordinates of transformation:\n S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,\n where the first 3 components are related to rotation and the last 3\n components are related to translation. Theta is the rotation angle\n and h * theta the translation. Theta should be >= 0. Negative rotations\n will be represented by a negative screw axis instead. This is relevant\n if you want to recover theta from exponential coordinates.\n\n Returns\n -------\n Stheta : array, shape (6,)\n Normalized exponential coordinates of transformation with theta in\n [0, pi]. Note that in the case of pure translation no normalization\n is required because the representation is unique. In the case of\n rotation by pi, there is an ambiguity that will be resolved so that\n the screw pitch is positive.\n \"\"\"\n theta = np.linalg.norm(Stheta[:3])\n if theta == 0.0:\n return Stheta\n\n screw_axis = Stheta / theta\n q, s_axis, h = screw_parameters_from_screw_axis(screw_axis)\n if abs(theta - np.pi) < eps and h < 0:\n h *= -1.0\n s_axis *= -1.0\n theta_normed = norm_angle(theta)\n h_normalized = h * theta / theta_normed\n screw_axis = screw_axis_from_screw_parameters(q, s_axis, h_normalized)\n\n return screw_axis * theta_normed\n\n\ndef assert_exponential_coordinates_equal(Stheta1, Stheta2):\n \"\"\"Raise an assertion if exp. coordinates are not approximately equal.\n\n Parameters\n ----------\n Stheta1 : array-like, shape (6,)\n Exponential coordinates of transformation:\n S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,\n where the first 3 components are related to rotation and the last 3\n components are related to translation.\n\n Stheta2 : array-like, shape (6,)\n Exponential coordinates of transformation:\n S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,\n where the first 3 components are related to rotation and the last 3\n components are related to translation.\n\n args : tuple\n Positional arguments that will be passed to\n `assert_array_almost_equal`\n\n kwargs : dict\n Positional arguments that will be passed to\n `assert_array_almost_equal`\n \"\"\"\n Stheta1 = norm_exponential_coordinates(Stheta1)\n Stheta2 = norm_exponential_coordinates(Stheta2)\n assert_array_almost_equal(Stheta1, Stheta2)\n\n\ndef assert_screw_parameters_equal(\n q1, s_axis1, h1, theta1, q2, s_axis2, h2, theta2, *args, **kwargs\n):\n \"\"\"Raise an assertion if two sets of screw parameters are not similar.\n\n Note that the screw axis can be inverted. In this case theta and h have\n to be adapted.\n\n Parameters\n ----------\n q1 : array, shape (3,)\n Vector to a point on the screw axis that is orthogonal to s_axis\n\n s_axis1 : array, shape (3,)\n Unit direction vector of the screw axis\n\n h1 : float\n Pitch of the screw. The pitch is the ratio of translation and rotation\n of the screw axis. Infinite pitch indicates pure translation.\n\n theta1 : float\n Parameter of the transformation: theta is the angle of rotation\n and h * theta the translation.\n\n q2 : array, shape (3,)\n Vector to a point on the screw axis that is orthogonal to s_axis\n\n s_axis2 : array, shape (3,)\n Unit direction vector of the screw axis\n\n h2 : float\n Pitch of the screw. The pitch is the ratio of translation and rotation\n of the screw axis. Infinite pitch indicates pure translation.\n\n theta2 : float\n Parameter of the transformation: theta is the angle of rotation\n and h * theta the translation.\n\n args : tuple\n Positional arguments that will be passed to\n `assert_array_almost_equal`\n\n kwargs : dict\n Positional arguments that will be passed to\n `assert_array_almost_equal`\n \"\"\"\n # normalize thetas\n theta1_new = norm_angle(theta1)\n h1 *= theta1 / theta1_new\n theta1 = theta1_new\n\n theta2_new = norm_angle(theta2)\n h2 *= theta2 / theta2_new\n theta2 = theta2_new\n\n # q1 and q2 can be any points on the screw axis, that is, they must be a\n # linear combination of each other and the screw axis (which one does not\n # matter since they should be identical or mirrored)\n q1_to_q2 = q2 - q1\n factors = q1_to_q2 / s_axis2\n assert abs(factors[0] - factors[1]) < eps\n assert abs(factors[1] - factors[2]) < eps\n try:\n assert_array_almost_equal(s_axis1, s_axis2, *args, **kwargs)\n assert abs(h1 - h2) < eps\n assert abs(theta1 - theta2) < eps\n except AssertionError: # possibly mirrored screw axis\n s_axis1_new = -s_axis1\n # make sure that we keep the direction of rotation\n theta1_new = 2.0 * np.pi - theta1\n # adjust pitch: switch sign and update rotation component\n h1 = -h1 / theta1_new * theta1\n theta1 = theta1_new\n\n # we have to normalize the angle again\n theta1_new = norm_angle(theta1)\n h1 *= theta1 / theta1_new\n theta1 = theta1_new\n\n assert_array_almost_equal(s_axis1_new, s_axis2, *args, **kwargs)\n assert abs(h1 - h2) < eps\n assert abs(theta1 - theta2) < eps\n\n\ndef screw_parameters_from_screw_axis(screw_axis):\n \"\"\"Compute screw parameters from screw axis.\n\n Note that there is not just one solution since q can be any point on the\n screw axis. We select q so that it is orthogonal to s_axis.\n\n Parameters\n ----------\n screw_axis : array-like, shape (6,)\n Screw axis described by 6 values\n (omega_1, omega_2, omega_3, v_1, v_2, v_3),\n where the first 3 components are related to rotation and the last 3\n components are related to translation.\n\n Returns\n -------\n q : array, shape (3,)\n Vector to a point on the screw axis that is orthogonal to s_axis\n\n s_axis : array, shape (3,)\n Unit direction vector of the screw axis\n\n h : float\n Pitch of the screw. The pitch is the ratio of translation and rotation\n of the screw axis. Infinite pitch indicates pure translation.\n \"\"\"\n screw_axis = check_screw_axis(screw_axis)\n\n omega = screw_axis[:3]\n v = screw_axis[3:]\n\n omega_norm = np.linalg.norm(omega)\n if abs(omega_norm) < np.finfo(float).eps: # pure translation\n q = np.zeros(3)\n s_axis = v\n h = np.inf\n else:\n s_axis = omega\n h = omega.dot(v)\n moment = v - h * s_axis\n q = np.cross(s_axis, moment)\n return q, s_axis, h\n\n\ndef screw_axis_from_screw_parameters(q, s_axis, h):\n \"\"\"Compute screw axis representation from screw parameters.\n\n Parameters\n ----------\n q : array-like, shape (3,)\n Vector to a point on the screw axis\n\n s_axis : array-like, shape (3,)\n Direction vector of the screw axis\n\n h : float\n Pitch of the screw. The pitch is the ratio of translation and rotation\n of the screw axis. Infinite pitch indicates pure translation.\n\n Returns\n -------\n screw_axis : array, shape (6,)\n Screw axis described by 6 values\n (omega_1, omega_2, omega_3, v_1, v_2, v_3),\n where the first 3 components are related to rotation and the last 3\n components are related to translation.\n \"\"\"\n q, s_axis, h = check_screw_parameters(q, s_axis, h)\n\n if np.isinf(h): # pure translation\n return np.r_[0.0, 0.0, 0.0, s_axis]\n return np.r_[s_axis, np.cross(q, s_axis) + h * s_axis]\n\n\ndef screw_axis_from_exponential_coordinates(Stheta):\n \"\"\"Compute screw axis and theta from exponential coordinates.\n\n Parameters\n ----------\n Stheta : array-like, shape (6,)\n Exponential coordinates of transformation:\n S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,\n where the first 3 components are related to rotation and the last 3\n components are related to translation. Theta is the rotation angle\n and h * theta the translation. Theta should be >= 0. Negative rotations\n will be represented by a negative screw axis instead. This is relevant\n if you want to recover theta from exponential coordinates.\n\n Returns\n -------\n screw_axis : array, shape (6,)\n Screw axis described by 6 values\n (omega_1, omega_2, omega_3, v_1, v_2, v_3),\n where the first 3 components are related to rotation and the last 3\n components are related to translation.\n\n theta : float\n Parameter of the transformation: theta is the angle of rotation\n and h * theta the translation.\n \"\"\"\n Stheta = check_exponential_coordinates(Stheta)\n\n omega_theta = Stheta[:3]\n v_theta = Stheta[3:]\n theta = np.linalg.norm(omega_theta)\n if theta < np.finfo(float).eps:\n theta = np.linalg.norm(v_theta)\n if theta < np.finfo(float).eps:\n return np.zeros(6), 0.0\n return Stheta / theta, theta\n\n\ndef screw_axis_from_screw_matrix(screw_matrix):\n \"\"\"Compute screw axis from screw matrix.\n\n Parameters\n ----------\n screw_matrix : array-like, shape (4, 4)\n A screw matrix consists of a cross-product matrix that represents an\n axis of rotation, a translation, and a row of zeros.\n\n Returns\n -------\n screw_axis : array, shape (6,)\n Screw axis described by 6 values\n (omega_1, omega_2, omega_3, v_1, v_2, v_3),\n where the first 3 components are related to rotation and the last 3\n components are related to translation.\n \"\"\"\n screw_matrix = check_screw_matrix(screw_matrix)\n\n screw_axis = np.empty(6)\n screw_axis[0] = screw_matrix[2, 1]\n screw_axis[1] = screw_matrix[0, 2]\n screw_axis[2] = screw_matrix[1, 0]\n screw_axis[3:] = screw_matrix[:3, 3]\n return screw_axis\n\n\ndef exponential_coordinates_from_screw_axis(screw_axis, theta):\n \"\"\"Compute exponential coordinates from screw axis and theta.\n\n Parameters\n ----------\n screw_axis : array-like, shape (6,)\n Screw axis described by 6 values\n (omega_1, omega_2, omega_3, v_1, v_2, v_3),\n where the first 3 components are related to rotation and the last 3\n components are related to translation.\n\n theta : float\n Parameter of the transformation: theta is the angle of rotation\n and h * theta the translation.\n\n Returns\n -------\n Stheta : array, shape (6,)\n Exponential coordinates of transformation:\n S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,\n where the first 3 components are related to rotation and the last 3\n components are related to translation. Theta is the rotation angle\n and h * theta the translation. Theta should be >= 0. Negative rotations\n will be represented by a negative screw axis instead. This is relevant\n if you want to recover theta from exponential coordinates.\n \"\"\"\n screw_axis = check_screw_axis(screw_axis)\n return screw_axis * theta\n\n\ndef exponential_coordinates_from_transform_log(transform_log, check=True):\n r\"\"\"Compute exponential coordinates from logarithm of transformation.\n\n Extracts the vector :math:`\\mathcal{S} \\theta =\n (\\hat{\\boldsymbol{\\omega}}, \\boldsymbol{v}) \\theta \\in \\mathbb{R}^6` from\n the matrix\n\n .. math::\n\n \\left(\n \\begin{array}{cccc}\n 0 & -\\omega_3 & \\omega_2 & v_1\\\\\n \\omega_3 & 0 & -\\omega_1 & v_2\\\\\n -\\omega_2 & \\omega_1 & 0 & v_3\\\\\n 0 & 0 & 0 & 0\n \\end{array}\n \\right)\n \\theta = \\left[ \\mathcal{S} \\right] \\theta \\in so(3).\n\n Parameters\n ----------\n transform_log : array-like, shape (4, 4)\n Matrix logarithm of transformation matrix: [S] * theta.\n\n check : bool, optional (default: True)\n Check if logarithm of transformation is valid\n\n Returns\n -------\n Stheta : array, shape (6,)\n Exponential coordinates of transformation:\n S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,\n where S is the screw axis, the first 3 components are related to\n rotation and the last 3 components are related to translation.\n Theta is the rotation angle and h * theta the translation.\n \"\"\"\n if check:\n transform_log = check_transform_log(transform_log)\n\n Stheta = np.empty(6)\n Stheta[0] = transform_log[2, 1]\n Stheta[1] = transform_log[0, 2]\n Stheta[2] = transform_log[1, 0]\n Stheta[3:] = transform_log[:3, 3]\n return Stheta\n\n\ndef screw_matrix_from_screw_axis(screw_axis):\n \"\"\"Compute screw matrix from screw axis.\n\n Parameters\n ----------\n screw_axis : array-like, shape (6,)\n Screw axis described by 6 values\n (omega_1, omega_2, omega_3, v_1, v_2, v_3),\n where the first 3 components are related to rotation and the last 3\n components are related to translation.\n\n Returns\n -------\n screw_matrix : array, shape (4, 4)\n A screw matrix consists of a cross-product matrix that represents an\n axis of rotation, a translation, and a row of zeros.\n \"\"\"\n screw_axis = check_screw_axis(screw_axis)\n\n omega = screw_axis[:3]\n v = screw_axis[3:]\n screw_matrix = np.zeros((4, 4))\n screw_matrix[:3, :3] = cross_product_matrix(omega)\n screw_matrix[:3, 3] = v\n return screw_matrix\n\n\ndef screw_matrix_from_transform_log(transform_log):\n \"\"\"Compute screw matrix from logarithm of transformation.\n\n Parameters\n ----------\n transform_log : array-like, shape (4, 4)\n Matrix logarithm of transformation matrix: [S] * theta.\n\n Returns\n -------\n screw_matrix : array, shape (4, 4)\n A screw matrix consists of a cross-product matrix that represents an\n axis of rotation, a translation, and a row of zeros.\n \"\"\"\n transform_log = check_transform_log(transform_log)\n\n omega = np.array(\n [transform_log[2, 1], transform_log[0, 2], transform_log[1, 0]]\n )\n theta = np.linalg.norm(omega)\n if abs(theta) < np.finfo(float).eps:\n theta = np.linalg.norm(transform_log[:3, 3])\n if abs(theta) < np.finfo(float).eps:\n return np.zeros((4, 4)), 0.0\n return transform_log / theta, theta\n\n\ndef transform_log_from_exponential_coordinates(Stheta):\n r\"\"\"Compute matrix logarithm of transformation from exponential coordinates.\n\n Builds the matrix\n\n .. math::\n\n \\left(\n \\begin{array}{cccc}\n 0 & -\\omega_3 & \\omega_2 & v_1\\\\\n \\omega_3 & 0 & -\\omega_1 & v_2\\\\\n -\\omega_2 & \\omega_1 & 0 & v_3\\\\\n 0 & 0 & 0 & 0\n \\end{array}\n \\right) \\theta\n = \\left[ \\mathcal{S} \\right] \\theta \\in so(3)\n\n from the vector :math:`\\mathcal{S} \\theta = (\\hat{\\boldsymbol{\\omega}},\n \\boldsymbol{v}) \\theta \\in \\mathbb{R}^6`.\n\n Parameters\n ----------\n Stheta : array-like, shape (6,)\n Exponential coordinates of transformation:\n S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,\n where S is the screw axis, the first 3 components are related to\n rotation and the last 3 components are related to translation.\n Theta is the rotation angle and h * theta the translation.\n\n Returns\n -------\n transform_log : array, shape (4, 4)\n Matrix logarithm of transformation matrix: [S] * theta.\n \"\"\"\n check_exponential_coordinates(Stheta)\n\n omega = Stheta[:3]\n v = Stheta[3:]\n transform_log = np.zeros((4, 4))\n transform_log[:3, :3] = cross_product_matrix(omega)\n transform_log[:3, 3] = v\n return transform_log\n\n\ndef transform_log_from_screw_matrix(screw_matrix, theta):\n \"\"\"Compute matrix logarithm of transformation from screw matrix and theta.\n\n Parameters\n ----------\n screw_matrix : array-like, shape (4, 4)\n A screw matrix consists of a cross-product matrix that represents an\n axis of rotation, a translation, and a row of zeros.\n\n theta : float\n Parameter of the transformation: theta is the angle of rotation\n and h * theta the translation.\n\n Returns\n -------\n transform_log : array, shape (4, 4)\n Matrix logarithm of transformation matrix: [S] * theta.\n \"\"\"\n screw_matrix = check_screw_matrix(screw_matrix)\n return screw_matrix * theta\n\n\ndef transform_from_exponential_coordinates(Stheta, check=True):\n r\"\"\"Compute transformation matrix from exponential coordinates.\n\n Exponential map.\n\n .. math::\n\n Exp: \\mathcal{S} \\theta \\in \\mathbb{R}^6\n \\rightarrow \\boldsymbol{T} \\in SE(3)\n\n .. math::\n\n Exp(\\mathcal{S}\\theta) =\n Exp\\left(\\left(\\begin{array}{c}\n \\hat{\\boldsymbol{\\omega}}\\\\\n \\boldsymbol{v}\n \\end{array}\\right)\\theta\\right)\n =\n \\exp(\\left[\\mathcal{S}\\right] \\theta)\n =\n \\left(\\begin{array}{cc}\n Exp(\\hat{\\boldsymbol{\\omega}} \\theta) &\n \\boldsymbol{J}(\\theta)\\boldsymbol{v}\\theta\\\\\n \\boldsymbol{0} & 1\n \\end{array}\\right),\n\n where :math:`\\boldsymbol{J}(\\theta)` is the left Jacobian of :math:`SO(3)`\n (see :func:`~pytransform3d.rotations.left_jacobian_SO3`).\n\n Parameters\n ----------\n Stheta : array-like, shape (6,)\n Exponential coordinates of transformation:\n S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,\n where S is the screw axis, the first 3 components are related to\n rotation and the last 3 components are related to translation.\n Theta is the rotation angle and h * theta the translation.\n\n check : bool, optional (default: True)\n Check if exponential coordinates are valid\n\n Returns\n -------\n A2B : array, shape (4, 4)\n Transformation matrix from frame A to frame B\n \"\"\"\n if check:\n Stheta = check_exponential_coordinates(Stheta)\n\n omega_theta = Stheta[:3]\n theta = np.linalg.norm(omega_theta)\n\n if theta == 0.0: # only translation\n return translate_transform(np.eye(4), Stheta[3:], check=check)\n\n omega_theta = Stheta[:3]\n v_theta = Stheta[3:]\n\n A2B = np.eye(4)\n A2B[:3, :3] = matrix_from_compact_axis_angle(omega_theta)\n J = left_jacobian_SO3(omega_theta)\n A2B[:3, 3] = np.dot(J, v_theta)\n return A2B\n\n\ndef transform_from_transform_log(transform_log):\n r\"\"\"Compute transformation from matrix logarithm of transformation.\n\n Exponential map.\n\n .. math::\n\n \\exp: \\left[ \\mathcal{S} \\right] \\theta \\in se(3)\n \\rightarrow \\boldsymbol{T} \\in SE(3)\n\n .. math::\n\n \\exp([\\mathcal{S}]\\theta) =\n \\exp\\left(\\left(\\begin{array}{cc}\n \\left[\\hat{\\boldsymbol{\\omega}}\\right] & \\boldsymbol{v}\\\\\n \\boldsymbol{0} & 0\n \\end{array}\\right)\\theta\\right) =\n \\left(\\begin{array}{cc}\n Exp(\\hat{\\boldsymbol{\\omega}} \\theta) &\n \\boldsymbol{J}(\\theta)\\boldsymbol{v}\\theta\\\\\n \\boldsymbol{0} & 1\n \\end{array}\\right),\n\n where :math:`\\boldsymbol{J}(\\theta)` is the left Jacobian of :math:`SO(3)`\n (see :func:`~pytransform3d.rotations.left_jacobian_SO3`).\n\n Parameters\n ----------\n transform_log : array-like, shape (4, 4)\n Matrix logarithm of transformation matrix: [S] * theta.\n\n Returns\n -------\n A2B : array, shape (4, 4)\n Transform from frame A to frame B\n \"\"\"\n transform_log = check_transform_log(transform_log)\n\n omega_theta = np.array(\n [transform_log[2, 1], transform_log[0, 2], transform_log[1, 0]]\n )\n v_theta = transform_log[:3, 3]\n theta = np.linalg.norm(omega_theta)\n\n if theta == 0.0: # only translation\n return translate_transform(np.eye(4), v_theta)\n\n A2B = np.eye(4)\n A2B[:3, :3] = matrix_from_compact_axis_angle(omega_theta)\n J = left_jacobian_SO3(omega_theta)\n A2B[:3, 3] = np.dot(J, v_theta)\n return A2B\n\n\ndef dual_quaternion_from_screw_parameters(q, s_axis, h, theta):\n \"\"\"Compute dual quaternion from screw parameters.\n\n Parameters\n ----------\n q : array-like, shape (3,)\n Vector to a point on the screw axis\n\n s_axis : array-like, shape (3,)\n Direction vector of the screw axis\n\n h : float\n Pitch of the screw. The pitch is the ratio of translation and rotation\n of the screw axis. Infinite pitch indicates pure translation.\n\n theta : float\n Parameter of the transformation: theta is the angle of rotation\n and h * theta the translation.\n\n Returns\n -------\n dq : array, shape (8,)\n Unit dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n \"\"\"\n q, s_axis, h = check_screw_parameters(q, s_axis, h)\n\n if np.isinf(h): # pure translation\n d = theta\n theta = 0\n else:\n d = h * theta\n moment = np.cross(q, s_axis)\n\n half_distance = 0.5 * d\n sin_half_angle = np.sin(0.5 * theta)\n cos_half_angle = np.cos(0.5 * theta)\n\n real_w = cos_half_angle\n real_vec = sin_half_angle * s_axis\n dual_w = -half_distance * sin_half_angle\n dual_vec = sin_half_angle * moment + half_distance * cos_half_angle * s_axis\n\n return np.r_[real_w, real_vec, dual_w, dual_vec]\n" }, { "path": "pytransform3d/transformations/_screws.pyi", "content": "import numpy as np\nimport numpy.typing as npt\nfrom typing import Tuple\n\ndef check_screw_parameters(\n q: npt.ArrayLike, s_axis: npt.ArrayLike, h: float\n) -> Tuple[np.ndarray, np.ndarray, float]: ...\ndef check_screw_axis(screw_axis: npt.ArrayLike) -> np.ndarray: ...\ndef check_exponential_coordinates(Stheta: npt.ArrayLike) -> np.ndarray: ...\ndef check_screw_matrix(\n screw_matrix: npt.ArrayLike,\n tolerance: float = ...,\n strict_check: bool = ...,\n) -> np.ndarray: ...\ndef check_transform_log(\n transform_log: npt.ArrayLike,\n tolerance: float = ...,\n strict_check: bool = ...,\n) -> np.ndarray: ...\ndef norm_exponential_coordinates(Stheta: npt.ArrayLike) -> np.ndarray: ...\ndef assert_exponential_coordinates_equal(\n Stheta1: npt.ArrayLike, Stheta2: npt.ArrayLike\n): ...\ndef assert_screw_parameters_equal(\n q1: npt.ArrayLike,\n s_axis1: npt.ArrayLike,\n h1: float,\n theta1: float,\n q2: npt.ArrayLike,\n s_axis2: npt.ArrayLike,\n h2: float,\n theta2: float,\n *args,\n **kwargs,\n): ...\ndef screw_parameters_from_screw_axis(\n screw_axis: npt.ArrayLike,\n) -> Tuple[np.ndarray, np.ndarray, float]: ...\ndef screw_axis_from_screw_parameters(\n q: npt.ArrayLike, s_axis: npt.ArrayLike, h: float\n) -> np.ndarray: ...\ndef screw_axis_from_exponential_coordinates(\n Stheta: npt.ArrayLike,\n) -> Tuple[np.ndarray, float]: ...\ndef screw_axis_from_screw_matrix(screw_matrix: npt.ArrayLike) -> np.ndarray: ...\ndef exponential_coordinates_from_screw_axis(\n screw_axis: npt.ArrayLike, theta: float\n) -> np.ndarray: ...\ndef exponential_coordinates_from_transform_log(\n transform_log: npt.ArrayLike, check: bool = ...\n) -> np.ndarray: ...\ndef screw_matrix_from_screw_axis(screw_axis: npt.ArrayLike) -> np.ndarray: ...\ndef screw_matrix_from_transform_log(\n transform_log: npt.ArrayLike,\n) -> np.ndarray: ...\ndef transform_log_from_exponential_coordinates(\n Stheta: npt.ArrayLike,\n) -> np.ndarray: ...\ndef transform_log_from_screw_matrix(\n screw_matrix: npt.ArrayLike, theta: float\n) -> np.ndarray: ...\ndef transform_from_exponential_coordinates(\n Stheta: npt.ArrayLike, check: bool = ...\n) -> np.ndarray: ...\ndef transform_from_transform_log(\n transform_log: npt.ArrayLike,\n) -> np.ndarray: ...\ndef dual_quaternion_from_screw_parameters(\n q: npt.ArrayLike, s_axis: npt.ArrayLike, h: float, theta: float\n) -> np.ndarray: ...\n" }, { "path": "pytransform3d/transformations/_transform.py", "content": "\"\"\"Transformation matrices.\"\"\"\n\nimport warnings\n\nimport numpy as np\nfrom numpy.testing import assert_array_almost_equal\n\nfrom ..rotations import (\n matrix_requires_renormalization,\n check_matrix,\n assert_rotation_matrix,\n quaternion_from_matrix,\n compact_axis_angle_from_matrix,\n left_jacobian_SO3_inv,\n cross_product_matrix,\n concatenate_quaternions,\n)\n\n\ndef check_transform(A2B, strict_check=True):\n \"\"\"Input validation of transform.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Transform from frame A to frame B\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the transformation matrix is not numerically\n close enough to a real transformation matrix. Otherwise we print a\n warning.\n\n Returns\n -------\n A2B : array, shape (4, 4)\n Validated transform from frame A to frame B\n\n Raises\n ------\n ValueError\n If input is invalid\n \"\"\"\n A2B = np.asarray(A2B, dtype=np.float64)\n if A2B.ndim != 2 or A2B.shape[0] != 4 or A2B.shape[1] != 4:\n raise ValueError(\n \"Expected homogeneous transformation matrix with \"\n \"shape (4, 4), got array-like object with shape %s\" % (A2B.shape,)\n )\n check_matrix(A2B[:3, :3], strict_check=strict_check)\n if not np.allclose(A2B[3], np.array([0.0, 0.0, 0.0, 1.0])):\n error_msg = (\n \"Excpected homogeneous transformation matrix with \"\n \"[0, 0, 0, 1] at the bottom, got %r\" % A2B\n )\n if strict_check:\n raise ValueError(error_msg)\n warnings.warn(error_msg, UserWarning, stacklevel=2)\n return A2B\n\n\ndef transform_requires_renormalization(A2B, tolerance=1e-6):\n r\"\"\"Check if transformation matrix requires renormalization.\n\n This function will check if :math:`R R^T \\approx I`.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Transform from frame A to frame B with a rotation matrix that should\n be orthonormal.\n\n tolerance : float, optional (default: 1e-6)\n Tolerance for check.\n\n Returns\n -------\n required : bool\n Indicates if renormalization is required.\n\n See Also\n --------\n pytransform3d.rotations.matrix_requires_renormalization\n Check if a rotation matrix needs renormalization.\n pytransform3d.rotations.norm_matrix : Orthonormalize rotation matrix.\n \"\"\"\n return matrix_requires_renormalization(np.asarray(A2B[:3, :3]), tolerance)\n\n\ndef assert_transform(A2B, *args, **kwargs):\n \"\"\"Raise an assertion if the transform is not a homogeneous matrix.\n\n See numpy.testing.assert_array_almost_equal for a more detailed\n documentation of the other parameters.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Transform from frame A to frame B\n\n args : tuple\n Positional arguments that will be passed to\n `assert_array_almost_equal`\n\n kwargs : dict\n Positional arguments that will be passed to\n `assert_array_almost_equal`\n \"\"\"\n assert_rotation_matrix(A2B[:3, :3], *args, **kwargs)\n assert_array_almost_equal(\n A2B[3], np.array([0.0, 0.0, 0.0, 1.0]), *args, **kwargs\n )\n\n\ndef transform_from(R, p, strict_check=True):\n r\"\"\"Make transformation from rotation matrix and translation.\n\n .. math::\n\n \\boldsymbol{T}_{BA} = \\left(\n \\begin{array}{cc}\n \\boldsymbol{R} & \\boldsymbol{p}\\\\\n \\boldsymbol{0} & 1\n \\end{array}\n \\right) \\in SE(3)\n\n Parameters\n ----------\n R : array-like, shape (3, 3)\n Rotation matrix\n\n p : array-like, shape (3,)\n Translation\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the transformation matrix is not numerically\n close enough to a real transformation matrix. Otherwise we print a\n warning.\n\n Returns\n -------\n A2B : array, shape (4, 4)\n Transform from frame A to frame B\n \"\"\"\n A2B = rotate_transform(np.eye(4), R, strict_check=strict_check, check=False)\n A2B = translate_transform(A2B, p, strict_check=strict_check, check=False)\n return A2B\n\n\ndef translate_transform(A2B, p, strict_check=True, check=True):\n \"\"\"Sets the translation of a transform.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Transform from frame A to frame B\n\n p : array-like, shape (3,)\n Translation\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the transformation matrix is not numerically\n close enough to a real transformation matrix. Otherwise we print a\n warning.\n\n check : bool, optional (default: True)\n Check if transformation matrix is valid\n\n Returns\n -------\n A2B : array, shape (4, 4)\n Transform from frame A to frame B\n \"\"\"\n if check:\n A2B = check_transform(A2B, strict_check=strict_check)\n out = A2B.copy()\n out[:3, -1] = p\n return out\n\n\ndef rotate_transform(A2B, R, strict_check=True, check=True):\n \"\"\"Sets the rotation of a transform.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Transform from frame A to frame B\n\n R : array-like, shape (3, 3)\n Rotation matrix\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the transformation matrix is not numerically\n close enough to a real transformation matrix. Otherwise we print a\n warning.\n\n check : bool, optional (default: True)\n Check if transformation matrix is valid\n\n Returns\n -------\n A2B : array, shape (4, 4)\n Transform from frame A to frame B\n \"\"\"\n if check:\n A2B = check_transform(A2B, strict_check=strict_check)\n out = A2B.copy()\n out[:3, :3] = R\n return out\n\n\ndef pq_from_transform(A2B, strict_check=True):\n \"\"\"Compute position and quaternion from transformation matrix.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Transformation matrix from frame A to frame B\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the transformation matrix is not numerically\n close enough to a real transformation matrix. Otherwise we print a\n warning.\n\n Returns\n -------\n pq : array, shape (7,)\n Position and orientation quaternion: (x, y, z, qw, qx, qy, qz)\n \"\"\"\n A2B = check_transform(A2B, strict_check=strict_check)\n return np.hstack((A2B[:3, 3], quaternion_from_matrix(A2B[:3, :3])))\n\n\ndef transform_log_from_transform(A2B, strict_check=True):\n r\"\"\"Compute matrix logarithm of transformation from transformation.\n\n Logarithmic map.\n\n .. math::\n\n \\log: \\boldsymbol{T} \\in SE(3)\n \\rightarrow \\left[ \\mathcal{S} \\right] \\theta \\in se(3)\n\n .. math::\n\n \\log(\\boldsymbol{T}) =\n \\log\\left(\n \\begin{array}{cc}\n \\boldsymbol{R} & \\boldsymbol{p}\\\\\n \\boldsymbol{0} & 1\n \\end{array}\n \\right)\n =\n \\left(\n \\begin{array}{cc}\n \\log\\boldsymbol{R} & \\boldsymbol{J}^{-1}(\\theta) \\boldsymbol{p}\\\\\n \\boldsymbol{0} & 0\n \\end{array}\n \\right)\n =\n \\left(\n \\begin{array}{cc}\n \\hat{\\boldsymbol{\\omega}} \\theta\n & \\boldsymbol{v} \\theta\\\\\n \\boldsymbol{0} & 0\n \\end{array}\n \\right)\n =\n \\left[\\mathcal{S}\\right]\\theta,\n\n where :math:`\\boldsymbol{J}^{-1}(\\theta)` is the inverse left Jacobian of\n :math:`SO(3)` (see :func:`~pytransform3d.rotations.left_jacobian_SO3_inv`).\n\n Parameters\n ----------\n A2B : array, shape (4, 4)\n Transform from frame A to frame B\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the transformation matrix is not numerically\n close enough to a real transformation matrix. Otherwise we print a\n warning.\n\n Returns\n -------\n transform_log : array, shape (4, 4)\n Matrix logarithm of transformation matrix: [S] * theta.\n \"\"\"\n A2B = check_transform(A2B, strict_check=strict_check)\n\n R = A2B[:3, :3]\n p = A2B[:3, 3]\n\n transform_log = np.zeros((4, 4))\n\n if np.linalg.norm(np.eye(3) - R) < np.finfo(float).eps:\n transform_log[:3, 3] = p\n return transform_log\n\n omega_theta = compact_axis_angle_from_matrix(R)\n theta = np.linalg.norm(omega_theta)\n\n if theta == 0:\n return transform_log\n\n J_inv = left_jacobian_SO3_inv(omega_theta)\n v_theta = np.dot(J_inv, p)\n\n transform_log[:3, :3] = cross_product_matrix(omega_theta)\n transform_log[:3, 3] = v_theta\n\n return transform_log\n\n\ndef exponential_coordinates_from_transform(A2B, strict_check=True, check=True):\n r\"\"\"Compute exponential coordinates from transformation matrix.\n\n Logarithmic map.\n\n .. math::\n\n Log: \\boldsymbol{T} \\in SE(3)\n \\rightarrow \\mathcal{S} \\theta \\in \\mathbb{R}^6\n\n .. math::\n\n Log(\\boldsymbol{T}) =\n Log\\left(\n \\begin{array}{cc}\n \\boldsymbol{R} & \\boldsymbol{p}\\\\\n \\boldsymbol{0} & 1\n \\end{array}\n \\right)\n =\n \\left(\n \\begin{array}{c}\n Log(\\boldsymbol{R})\\\\\n \\boldsymbol{J}^{-1}(\\theta) \\boldsymbol{p}\n \\end{array}\n \\right)\n =\n \\left(\n \\begin{array}{c}\n \\hat{\\boldsymbol{\\omega}}\\\\\n \\boldsymbol{v}\n \\end{array}\n \\right)\n \\theta\n =\n \\mathcal{S}\\theta,\n\n where :math:`\\boldsymbol{J}^{-1}(\\theta)` is the inverse left Jacobian of\n :math:`SO(3)` (see :func:`~pytransform3d.rotations.left_jacobian_SO3_inv`).\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Transformation matrix from frame A to frame B\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the transformation matrix is not numerically\n close enough to a real transformation matrix. Otherwise we print a\n warning.\n\n check : bool, optional (default: True)\n Check if transformation matrix is valid\n\n Returns\n -------\n Stheta : array, shape (6,)\n Exponential coordinates of transformation:\n S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta,\n where S is the screw axis, the first 3 components are related to\n rotation and the last 3 components are related to translation.\n Theta is the rotation angle and h * theta the translation.\n \"\"\"\n if check:\n A2B = check_transform(A2B, strict_check=strict_check)\n\n R = A2B[:3, :3]\n p = A2B[:3, 3]\n\n if np.linalg.norm(np.eye(3) - R) < np.finfo(float).eps:\n return np.r_[0.0, 0.0, 0.0, p]\n\n omega_theta = compact_axis_angle_from_matrix(R, check=check)\n theta = np.linalg.norm(omega_theta)\n\n if theta == 0:\n return np.r_[0.0, 0.0, 0.0, p]\n\n v_theta = np.dot(left_jacobian_SO3_inv(omega_theta), p)\n\n return np.hstack((omega_theta, v_theta))\n\n\ndef dual_quaternion_from_transform(A2B):\n \"\"\"Compute dual quaternion from transformation matrix.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Transform from frame A to frame B\n\n Returns\n -------\n dq : array, shape (8,)\n Unit dual quaternion to represent transform:\n (pw, px, py, pz, qw, qx, qy, qz)\n \"\"\"\n A2B = check_transform(A2B)\n real = quaternion_from_matrix(A2B[:3, :3])\n dual = 0.5 * concatenate_quaternions(np.r_[0, A2B[:3, 3]], real)\n return np.hstack((real, dual))\n\n\ndef adjoint_from_transform(A2B, strict_check=True, check=True):\n r\"\"\"Compute adjoint representation of a transformation matrix.\n\n The adjoint representation of a transformation\n :math:`\\left[Ad_{\\boldsymbol{T}_{BA}}\\right] \\in \\mathbb{R}^{6 \\times 6}`\n from frame A to frame B translates a twist from frame A to frame B\n through the adjoint map\n\n .. math::\n\n \\mathcal{V}_{B}\n = \\left[Ad_{\\boldsymbol{T}_{BA}}\\right] \\mathcal{V}_A\n\n The corresponding transformation matrix operation is\n\n .. math::\n\n \\left[\\mathcal{V}_{B}\\right]\n = \\boldsymbol{T}_{BA} \\left[\\mathcal{V}_A\\right]\n \\boldsymbol{T}_{BA}^{-1}\n\n We can also use the adjoint representation to transform a wrench from frame\n A to frame B:\n\n .. math::\n\n \\mathcal{F}_B\n = \\left[ Ad_{\\boldsymbol{T}_{AB}} \\right]^T \\mathcal{F}_A\n\n Note that not only the adjoint is transposed but also the transformation is\n inverted.\n\n Adjoint representations have the following properties:\n\n .. math::\n\n \\left[Ad_{\\boldsymbol{T}_1 \\boldsymbol{T}_2}\\right]\n = \\left[Ad_{\\boldsymbol{T}_1}\\right]\n \\left[Ad_{\\boldsymbol{T}_2}\\right]\n\n .. math::\n\n \\left[Ad_{\\boldsymbol{T}}\\right]^{-1} =\n \\left[Ad_{\\boldsymbol{T}^{-1}}\\right]\n\n For a transformation matrix\n\n .. math::\n\n \\boldsymbol T =\n \\left( \\begin{array}{cc}\n \\boldsymbol R & \\boldsymbol t\\\\\n \\boldsymbol 0 & 1\\\\\n \\end{array} \\right)\n\n the adjoint is defined as\n\n .. math::\n\n \\left[Ad_{\\boldsymbol{T}}\\right]\n =\n \\left( \\begin{array}{cc}\n \\boldsymbol R & \\boldsymbol 0\\\\\n \\left[\\boldsymbol{t}\\right]_{\\times}\\boldsymbol R & \\boldsymbol R\\\\\n \\end{array} \\right),\n\n where :math:`\\left[\\boldsymbol{t}\\right]_{\\times}` is the cross-product\n matrix (see :func:`~pytransform3d.rotations.cross_product_matrix`) of the\n translation component.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Transform from frame A to frame B\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the transformation matrix is not numerically\n close enough to a real transformation matrix. Otherwise we print a\n warning.\n\n check : bool, optional (default: True)\n Check if transformation matrix is valid\n\n Returns\n -------\n adj_A2B : array, shape (6, 6)\n Adjoint representation of transformation matrix\n \"\"\"\n if check:\n A2B = check_transform(A2B, strict_check)\n\n R = A2B[:3, :3]\n p = A2B[:3, 3]\n\n adj_A2B = np.zeros((6, 6))\n adj_A2B[:3, :3] = R\n adj_A2B[3:, :3] = np.dot(cross_product_matrix(p), R)\n adj_A2B[3:, 3:] = R\n return adj_A2B\n" }, { "path": "pytransform3d/transformations/_transform.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef check_transform(\n A2B: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef transform_requires_renormalization(\n A2B: npt.ArrayLike, tolerance: float = ...\n) -> bool: ...\ndef assert_transform(A2B: npt.ArrayLike, *args, **kwargs): ...\ndef transform_from(\n R: npt.ArrayLike, p: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef translate_transform(\n A2B: npt.ArrayLike,\n p: npt.ArrayLike,\n strict_check: bool = ...,\n check: bool = ...,\n) -> np.ndarray: ...\ndef rotate_transform(\n A2B: npt.ArrayLike,\n R: npt.ArrayLike,\n strict_check: bool = ...,\n check: bool = ...,\n) -> np.ndarray: ...\ndef pq_from_transform(\n A2B: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef transform_log_from_transform(\n A2B: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef exponential_coordinates_from_transform(\n A2B: npt.ArrayLike, strict_check: bool = ..., check: bool = ...\n) -> np.ndarray: ...\ndef dual_quaternion_from_transform(A2B: npt.ArrayLike) -> np.ndarray: ...\ndef adjoint_from_transform(\n A2B: npt.ArrayLike, strict_check: bool = ..., check: bool = ...\n) -> np.ndarray: ...\n" }, { "path": "pytransform3d/transformations/_transform_operations.py", "content": "\"\"\"Transform operations.\"\"\"\n\nimport numpy as np\n\nfrom ._screws import transform_from_exponential_coordinates\nfrom ._transform import check_transform, exponential_coordinates_from_transform\nfrom ..rotations import (\n axis_angle_from_matrix,\n matrix_from_axis_angle,\n norm_vector,\n)\n\n\ndef invert_transform(A2B, strict_check=True, check=True):\n \"\"\"Invert transform.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Transform from frame A to frame B\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the transformation matrix is not numerically\n close enough to a real transformation matrix. Otherwise we print a\n warning.\n\n check : bool, optional (default: True)\n Check if transformation matrix is valid\n\n Returns\n -------\n B2A : array, shape (4, 4)\n Transform from frame B to frame A\n\n See Also\n --------\n pytransform3d.uncertainty.invert_uncertain_transform :\n Invert transformation with uncertainty.\n\n pytransform3d.trajectories.invert_transforms :\n Invert multiple transformations.\n \"\"\"\n if check:\n A2B = check_transform(A2B, strict_check=strict_check)\n # NOTE there is a faster version, but it is not faster than matrix\n # inversion with numpy:\n # ( R t )^-1 ( R^T -R^T*t )\n # ( 0 1 ) = ( 0 1 )\n return np.linalg.inv(A2B)\n\n\ndef vector_to_point(v):\n \"\"\"Convert 3D vector to position.\n\n A point (x, y, z) given by the components of a vector will be represented\n by [x, y, z, 1] in homogeneous coordinates to which we can apply a\n transformation.\n\n Parameters\n ----------\n v : array-like, shape (3,)\n 3D vector that contains x, y, and z\n\n Returns\n -------\n p : array-like, shape (4,)\n Point vector with 1 as last element\n\n See Also\n --------\n pytransform3d.transformations.vectors_to_points\n Convert 3D vectors to points in homogeneous coordinates.\n\n pytransform3d.transformations.vector_to_direction\n Convert one 3D vector to a direction in homogeneous coordinates.\n\n pytransform3d.transformations.vectors_to_directions\n Convert 3D vectors to directions in homogeneous coordinates.\n \"\"\"\n return np.hstack((v, 1))\n\n\ndef vectors_to_points(V):\n \"\"\"Convert 3D vectors to positions.\n\n A point (x, y, z) given by the components of a vector will be represented\n by [x, y, z, 1] in homogeneous coordinates to which we can apply a\n transformation.\n\n Parameters\n ----------\n V : array-like, shape (n_points, 3)\n Each row is a 3D vector that contains x, y, and z\n\n Returns\n -------\n P : array-like, shape (n_points, 4)\n Each row is a point vector with 1 as last element\n\n See Also\n --------\n pytransform3d.transformations.vector_to_point\n Convert one 3D vector to a point in homogeneous coordinates.\n\n pytransform3d.transformations.vector_to_direction\n Convert one 3D vector to a direction in homogeneous coordinates.\n\n pytransform3d.transformations.vectors_to_directions\n Convert 3D vectors to directions in homogeneous coordinates.\n \"\"\"\n return np.hstack((V, np.ones((len(V), 1))))\n\n\ndef vector_to_direction(v):\n \"\"\"Convert 3D vector to direction.\n\n A direction (x, y, z) given by the components of a vector will be\n represented by [x, y, z, 0] in homogeneous coordinates to which we can\n apply a transformation.\n\n Parameters\n ----------\n v : array-like, shape (3,)\n 3D vector that contains x, y, and z\n\n Returns\n -------\n p : array-like, shape (4,)\n Direction vector with 0 as last element\n\n See Also\n --------\n pytransform3d.transformations.vector_to_point\n Convert one 3D vector to a point in homogeneous coordinates.\n\n pytransform3d.transformations.vectors_to_points\n Convert 3D vectors to points in homogeneous coordinates.\n\n pytransform3d.transformations.vectors_to_directions\n Convert 3D vectors to directions in homogeneous coordinates.\n \"\"\"\n return np.hstack((v, 0))\n\n\ndef vectors_to_directions(V):\n \"\"\"Convert 3D vectors to directions.\n\n A direction (x, y, z) given by the components of a vector will be\n represented by [x, y, z, 0] in homogeneous coordinates to which we can\n apply a transformation.\n\n Parameters\n ----------\n V : array-like, shape (n_directions, 3)\n Each row is a 3D vector that contains x, y, and z\n\n Returns\n -------\n P : array-like, shape (n_directions, 4)\n Each row is a direction vector with 0 as last element\n\n See Also\n --------\n pytransform3d.transformations.vector_to_point\n Convert one 3D vector to a point in homogeneous coordinates.\n\n pytransform3d.transformations.vectors_to_points\n Convert 3D vectors to points in homogeneous coordinates.\n\n pytransform3d.transformations.vector_to_direction\n Convert one 3D vector to a direction in homogeneous coordinates.\n \"\"\"\n return np.hstack((V, np.zeros((len(V), 1))))\n\n\ndef concat(A2B, B2C, strict_check=True, check=True):\n r\"\"\"Concatenate transformations.\n\n We use the extrinsic convention, which means that B2C is left-multiplied\n to A2B. In mathematical notation:\n\n .. math::\n\n \\boldsymbol{T}_{CB} \\boldsymbol{T}_{BA} = \\boldsymbol{T}_{CA}.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Transform from frame A to frame B\n\n B2C : array-like, shape (4, 4)\n Transform from frame B to frame C\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the transformation matrix is not numerically\n close enough to a real transformation matrix. Otherwise we print a\n warning.\n\n check : bool, optional (default: True)\n Check if transformation matrices are valid\n\n Returns\n -------\n A2C : array-like, shape (4, 4)\n Transform from frame A to frame C\n\n See Also\n --------\n pytransform3d.trajectories.concat_one_to_many :\n Concatenate one transformation with multiple transformations.\n\n pytransform3d.trajectories.concat_many_to_one :\n Concatenate multiple transformations with one.\n\n pytransform3d.uncertainty.concat_globally_uncertain_transforms :\n Concatenate two independent globally uncertain transformations.\n\n pytransform3d.uncertainty.concat_locally_uncertain_transforms :\n Concatenate two independent locally uncertain transformations.\n \"\"\"\n if check:\n A2B = check_transform(A2B, strict_check=strict_check)\n B2C = check_transform(B2C, strict_check=strict_check)\n return B2C.dot(A2B)\n\n\ndef transform(A2B, PA, strict_check=True):\n \"\"\"Transform point or list of points or directions.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Transform from frame A to frame B\n\n PA : array-like, shape (4,) or (n_points, 4)\n One of multiple points or directions in frame A that are represented\n by homogeneous coordinates.\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the transformation matrix is not numerically\n close enough to a real transformation matrix. Otherwise we print a\n warning.\n\n Returns\n -------\n PB : array-like, shape (4,) or (n_points, 4)\n One of multiple points or directions in frame B that are represented\n by homogeneous coordinates.\n\n Raises\n ------\n ValueError\n If dimensions are incorrect\n\n See Also\n --------\n pytransform3d.transformations.vector_to_point\n Convert one 3D vector to a point in homogeneous coordinates.\n\n pytransform3d.transformations.vectors_to_points\n Convert 3D vectors to points in homogeneous coordinates.\n\n pytransform3d.transformations.vector_to_direction\n Convert one 3D vector to a direction in homogeneous coordinates.\n\n pytransform3d.transformations.vectors_to_directions\n Convert 3D vectors to directions in homogeneous coordinates.\n \"\"\"\n A2B = check_transform(A2B, strict_check=strict_check)\n PA = np.asarray(PA)\n\n if PA.ndim == 1:\n return np.dot(A2B, PA)\n\n if PA.ndim == 2:\n return np.dot(PA, A2B.T)\n\n raise ValueError(\"Cannot transform array with more than 2 dimensions\")\n\n\ndef scale_transform(\n A2B,\n s_xr=1.0,\n s_yr=1.0,\n s_zr=1.0,\n s_r=1.0,\n s_xt=1.0,\n s_yt=1.0,\n s_zt=1.0,\n s_t=1.0,\n s_d=1.0,\n strict_check=True,\n):\n \"\"\"Scale a transform from A to reference frame B.\n\n See algorithm 10 from \"Analytic Approaches for Design and Operation of\n Haptic Human-Machine Interfaces\" (Bertold Bongardt).\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Transform from frame A to frame B\n\n s_xr : float, optional (default: 1)\n Scaling of x-component of the rotation axis\n\n s_yr : float, optional (default: 1)\n Scaling of y-component of the rotation axis\n\n s_zr : float, optional (default: 1)\n Scaling of z-component of the rotation axis\n\n s_r : float, optional (default: 1)\n Scaling of the rotation\n\n s_xt : float, optional (default: 1)\n Scaling of z-component of the translation\n\n s_yt : float, optional (default: 1)\n Scaling of z-component of the translation\n\n s_zt : float, optional (default: 1)\n Scaling of z-component of the translation\n\n s_t : float, optional (default: 1)\n Scaling of the translation\n\n s_d : float, optional (default: 1)\n Scaling of the whole transform (displacement)\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the transformation matrix is not numerically\n close enough to a real transformation matrix. Otherwise we print a\n warning.\n\n Returns\n -------\n A2B_scaled\n Scaled transform from frame A to frame B (actually this is a transform\n from A to another frame C)\n \"\"\"\n A2B = check_transform(A2B, strict_check=strict_check)\n A2B_scaled = np.eye(4)\n\n R = A2B[:3, :3]\n t = A2B[:3, 3]\n\n S_t = np.array([s_xt, s_yt, s_zt])\n A2B_scaled[:3, 3] = s_d * s_t * S_t * t\n\n a = axis_angle_from_matrix(R)\n a_new = np.empty(4)\n a_new[3] = s_d * s_r * a[3]\n S_r = np.array([s_xr, s_yr, s_zr])\n a_new[:3] = norm_vector(S_r * a[:3])\n A2B_scaled[:3, :3] = matrix_from_axis_angle(a_new)\n\n return A2B_scaled\n\n\ndef transform_sclerp(start, end, t):\n r\"\"\"Screw linear interpolation (ScLERP) for transformation matrices.\n\n We compute the difference between two poses\n :math:`\\boldsymbol{T}_{AB} = \\boldsymbol{T}^{-1}_{WA}\\boldsymbol{T}_{WB}`,\n convert it to exponential coordinates\n :math:`Log(\\boldsymbol{T}_{AB}) = \\mathcal{S}\\theta_{AB}`, compute a\n fraction of it :math:`t\\mathcal{S}\\theta_{AB}` with :math:`t \\in [0, 1]`,\n and use the exponential map to concatenate the fraction of the difference\n :math:`\\boldsymbol{T}_{WA} Exp(t\\mathcal{S}\\theta_{AB})`.\n\n Parameters\n ----------\n start : array-like, shape (4, 4)\n Transformation matrix to represent start pose.\n\n end : array-like, shape (4, 4)\n Transformation matrix to represent end pose.\n\n t : float in [0, 1]\n Position between start and goal.\n\n Returns\n -------\n A2B_t : array, shape (4, 4)\n Interpolated pose.\n\n See Also\n --------\n dual_quaternion_sclerp :\n ScLERP for dual quaternions.\n\n pq_slerp :\n An alternative approach is spherical linear interpolation (SLERP) with\n position and quaternion.\n \"\"\"\n end2start = np.dot(invert_transform(start), end)\n return np.dot(start, transform_power(end2start, t))\n\n\ndef transform_power(A2B, t):\n r\"\"\"Compute power of a transformation matrix with respect to scalar.\n\n .. math::\n\n \\boldsymbol{T}_{BA}^t\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Transform from frame A to frame B.\n\n t : float\n Exponent.\n\n Returns\n -------\n A2B_t : array, shape (4, 4)\n Transformation matrix.\n \"\"\"\n Stheta = exponential_coordinates_from_transform(A2B)\n return transform_from_exponential_coordinates(Stheta * t)\n" }, { "path": "pytransform3d/transformations/_transform_operations.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef invert_transform(\n A2B: npt.ArrayLike, strict_check: bool = ..., check: bool = ...\n) -> np.ndarray: ...\ndef vector_to_point(v: npt.ArrayLike) -> np.ndarray: ...\ndef vectors_to_points(V: npt.ArrayLike) -> np.ndarray: ...\ndef vector_to_direction(v: npt.ArrayLike) -> np.ndarray: ...\ndef vectors_to_directions(V: npt.ArrayLike) -> np.ndarray: ...\ndef concat(\n A2B: npt.ArrayLike,\n B2C: npt.ArrayLike,\n strict_check: bool = ...,\n check: bool = ...,\n) -> np.ndarray: ...\ndef transform(\n A2B: npt.ArrayLike, PA: npt.ArrayLike, strict_check: bool = ...\n) -> np.ndarray: ...\ndef scale_transform(\n A2B: npt.ArrayLike,\n s_xr: float = ...,\n s_yr: float = ...,\n s_zr: float = ...,\n s_r: float = ...,\n s_xt: float = ...,\n s_yt: float = ...,\n s_zt: float = ...,\n s_t: float = ...,\n s_d: float = ...,\n strict_check: bool = ...,\n) -> np.ndarray: ...\ndef transform_sclerp(\n start: npt.ArrayLike, end: npt.ArrayLike, t: float\n) -> np.ndarray: ...\ndef transform_power(A2B: npt.ArrayLike, t: float) -> np.ndarray: ...\n" }, { "path": "pytransform3d/transformations/test/test_dual_quaternion.py", "content": "import numpy as np\nimport pytest\nfrom numpy.testing import assert_array_almost_equal\n\nimport pytransform3d.rotations as pr\nimport pytransform3d.transformations as pt\n\n\ndef test_check_dual_quaternion():\n dq1 = [0, 0]\n with pytest.raises(ValueError, match=\"Expected dual quaternion\"):\n pt.check_dual_quaternion(dq1)\n\n dq2 = [[0, 0]]\n with pytest.raises(ValueError, match=\"Expected dual quaternion\"):\n pt.check_dual_quaternion(dq2)\n\n dq3 = pt.check_dual_quaternion([0] * 8, unit=False)\n assert dq3.shape[0] == 8\n assert pt.dual_quaternion_requires_renormalization(dq3)\n dq4 = pt.check_dual_quaternion(dq3, unit=True)\n assert not pt.dual_quaternion_requires_renormalization(dq4)\n\n dq5 = np.array(\n [\n 0.94508498,\n 0.0617101,\n -0.06483886,\n 0.31432811,\n -0.07743254,\n 0.04985168,\n -0.26119618,\n 0.1691491,\n ]\n )\n dq5_not_orthogonal = np.copy(dq5)\n dq5_not_orthogonal[4:] = np.round(dq5_not_orthogonal[4:], 1)\n assert pt.dual_quaternion_requires_renormalization(dq5_not_orthogonal)\n\n\ndef test_normalize_dual_quaternion():\n dq = [1, 0, 0, 0, 0, 0, 0, 0]\n norm = pt.dual_quaternion_squared_norm(dq)\n assert_array_almost_equal(norm, [1, 0])\n assert not pt.dual_quaternion_requires_renormalization(dq)\n dq_norm = pt.check_dual_quaternion(dq)\n pt.assert_unit_dual_quaternion(dq_norm)\n assert_array_almost_equal(dq, dq_norm)\n assert_array_almost_equal(dq, pt.norm_dual_quaternion(dq))\n\n dq = [0, 0, 0, 0, 0, 0, 0, 0]\n norm = pt.dual_quaternion_squared_norm(dq)\n assert_array_almost_equal(norm, [0, 0])\n assert pt.dual_quaternion_requires_renormalization(dq)\n dq_norm = pt.check_dual_quaternion(dq)\n pt.assert_unit_dual_quaternion(dq_norm)\n assert_array_almost_equal([1, 0, 0, 0, 0, 0, 0, 0], dq_norm)\n assert_array_almost_equal(dq_norm, pt.norm_dual_quaternion(dq))\n\n dq = [0, 0, 0, 0, 0.3, 0.5, 0, 0.2]\n norm = pt.dual_quaternion_squared_norm(dq)\n assert_array_almost_equal(norm, [0, 0])\n assert pt.dual_quaternion_requires_renormalization(dq)\n dq_norm = pt.check_dual_quaternion(dq)\n assert pt.dual_quaternion_requires_renormalization(dq_norm)\n assert_array_almost_equal([1, 0, 0, 0, 0.3, 0.5, 0, 0.2], dq_norm)\n assert not pt.dual_quaternion_requires_renormalization(\n pt.norm_dual_quaternion(dq)\n )\n\n rng = np.random.default_rng(999)\n for _ in range(5): # real norm != 1\n A2B = pt.random_transform(rng)\n dq = rng.standard_normal() * pt.dual_quaternion_from_transform(A2B)\n dq_norm = pt.check_dual_quaternion(dq)\n pt.assert_unit_dual_quaternion(dq_norm)\n assert_array_almost_equal(dq_norm, pt.norm_dual_quaternion(dq))\n\n for _ in range(5): # real and dual quaternion are not orthogonal\n A2B = pt.random_transform(rng)\n dq = pt.dual_quaternion_from_transform(A2B)\n dq_roundoff_error = np.copy(dq)\n dq_roundoff_error[4:] = np.round(dq_roundoff_error[4:], 3)\n assert pt.dual_quaternion_requires_renormalization(dq_roundoff_error)\n dq_norm = pt.norm_dual_quaternion(dq_roundoff_error)\n pt.assert_unit_dual_quaternion(dq_norm)\n assert_array_almost_equal(\n pt.dual_quaternion_squared_norm(dq_norm), [1, 0]\n )\n assert not pt.dual_quaternion_requires_renormalization(dq_norm)\n assert_array_almost_equal(dq, dq_norm, decimal=3)\n\n for _ in range(50):\n dq = rng.standard_normal(size=8)\n dq_norm = pt.norm_dual_quaternion(dq)\n assert_array_almost_equal(\n pt.dual_quaternion_squared_norm(dq_norm), [1, 0]\n )\n assert not pt.dual_quaternion_requires_renormalization(dq_norm)\n pt.assert_unit_dual_quaternion(dq_norm)\n\n\ndef test_dual_quaternion_double():\n rng = np.random.default_rng(4183)\n A2B = pt.random_transform(rng)\n dq = pt.dual_quaternion_from_transform(A2B)\n dq_double = pt.dual_quaternion_double(dq)\n pt.assert_unit_dual_quaternion_equal(dq, dq_double)\n assert_array_almost_equal(A2B, pt.transform_from_dual_quaternion(dq_double))\n\n\ndef test_dual_quaternion_concatenation():\n rng = np.random.default_rng(1000)\n for _ in range(5):\n A2B = pt.random_transform(rng)\n B2C = pt.random_transform(rng)\n A2C = pt.concat(A2B, B2C)\n dq1 = pt.dual_quaternion_from_transform(A2B)\n dq2 = pt.dual_quaternion_from_transform(B2C)\n dq3 = pt.concatenate_dual_quaternions(dq2, dq1)\n A2C2 = pt.transform_from_dual_quaternion(dq3)\n assert_array_almost_equal(A2C, A2C2)\n\n\ndef test_dual_quaternion_applied_to_point():\n rng = np.random.default_rng(1000)\n for _ in range(5):\n p_A = pr.random_vector(rng, 3)\n A2B = pt.random_transform(rng)\n dq = pt.dual_quaternion_from_transform(A2B)\n p_B = pt.dq_prod_vector(dq, p_A)\n assert_array_almost_equal(\n p_B, pt.transform(A2B, pt.vector_to_point(p_A))[:3]\n )\n\n\ndef test_dual_quaternion_sclerp_same_dual_quaternions():\n rng = np.random.default_rng(19)\n pose = pt.random_transform(rng)\n dq = pt.dual_quaternion_from_transform(pose)\n dq2 = pt.dual_quaternion_sclerp(dq, dq, 0.5)\n assert_array_almost_equal(dq, dq2)\n\n\ndef test_dual_quaternion_sclerp():\n rng = np.random.default_rng(22)\n pose1 = pt.random_transform(rng)\n pose2 = pt.random_transform(rng)\n dq1 = pt.dual_quaternion_from_transform(pose1)\n dq2 = pt.dual_quaternion_from_transform(pose2)\n\n n_steps = 100\n\n # Ground truth: screw linear interpolation\n pose12pose2 = pt.concat(pose2, pt.invert_transform(pose1))\n Stheta = pt.exponential_coordinates_from_transform(pose12pose2)\n offsets = np.array(\n [\n pt.transform_from_exponential_coordinates(Stheta * t)\n for t in np.linspace(0, 1, n_steps)\n ]\n )\n interpolated_poses = np.array(\n [pt.concat(offset, pose1) for offset in offsets]\n )\n\n # Dual quaternion ScLERP\n sclerp_interpolated_dqs = np.vstack(\n [\n pt.dual_quaternion_sclerp(dq1, dq2, t)\n for t in np.linspace(0, 1, n_steps)\n ]\n )\n sclerp_interpolated_poses_from_dqs = np.array(\n [\n pt.transform_from_dual_quaternion(dq)\n for dq in sclerp_interpolated_dqs\n ]\n )\n\n # Transformation matrix ScLERP\n sclerp_interpolated_transforms = np.array(\n [\n pt.transform_sclerp(pose1, pose2, t)\n for t in np.linspace(0, 1, n_steps)\n ]\n )\n\n for t in range(n_steps):\n assert_array_almost_equal(\n interpolated_poses[t], sclerp_interpolated_poses_from_dqs[t]\n )\n assert_array_almost_equal(\n interpolated_poses[t], sclerp_interpolated_transforms[t]\n )\n\n\ndef test_dual_quaternion_sclerp_sign_ambiguity():\n n_steps = 10\n rng = np.random.default_rng(2323)\n T1 = pt.random_transform(rng)\n dq1 = pt.dual_quaternion_from_transform(T1)\n dq2 = np.copy(dq1)\n\n if np.sign(dq1[0]) != np.sign(dq2[0]):\n dq2 *= -1.0\n traj_q = [\n pt.dual_quaternion_sclerp(dq1, dq2, t)\n for t in np.linspace(0, 1, n_steps)\n ]\n path_length = np.sum(\n [np.linalg.norm(r - s) for r, s in zip(traj_q[:-1], traj_q[1:])]\n )\n\n dq2 *= -1.0\n traj_q_opposing = [\n pt.dual_quaternion_sclerp(dq1, dq2, t)\n for t in np.linspace(0, 1, n_steps)\n ]\n path_length_opposing = np.sum(\n [\n np.linalg.norm(r - s)\n for r, s in zip(traj_q_opposing[:-1], traj_q_opposing[1:])\n ]\n )\n\n assert path_length_opposing == path_length\n\n\ndef test_compare_dual_quaternion_and_transform_power():\n rng = np.random.default_rng(44329)\n for _ in range(20):\n t = rng.normal()\n A2B = pt.random_transform(rng)\n dq = pt.dual_quaternion_from_transform(A2B)\n assert_array_almost_equal(\n pt.transform_power(A2B, t),\n pt.transform_from_dual_quaternion(pt.dual_quaternion_power(dq, t)),\n )\n\n\ndef test_screw_parameters_from_dual_quaternion():\n dq = np.array([1, 0, 0, 0, 0, 0, 0, 0])\n q, s_axis, h, theta = pt.screw_parameters_from_dual_quaternion(dq)\n assert_array_almost_equal(q, np.zeros(3))\n assert_array_almost_equal(s_axis, np.array([1, 0, 0]))\n assert np.isinf(h)\n assert pytest.approx(theta) == 0\n\n dq = pt.dual_quaternion_from_pq(np.array([1.2, 1.3, 1.4, 1, 0, 0, 0]))\n q, s_axis, h, theta = pt.screw_parameters_from_dual_quaternion(dq)\n assert_array_almost_equal(q, np.zeros(3))\n assert_array_almost_equal(s_axis, pr.norm_vector(np.array([1.2, 1.3, 1.4])))\n assert np.isinf(h)\n assert pytest.approx(theta) == np.linalg.norm(np.array([1.2, 1.3, 1.4]))\n\n rng = np.random.default_rng(1001)\n quat = pr.random_quaternion(rng)\n a = pr.axis_angle_from_quaternion(quat)\n dq = pt.dual_quaternion_from_pq(np.r_[0, 0, 0, quat])\n q, s_axis, h, theta = pt.screw_parameters_from_dual_quaternion(dq)\n assert_array_almost_equal(q, np.zeros(3))\n assert_array_almost_equal(s_axis, a[:3])\n assert_array_almost_equal(h, 0)\n assert_array_almost_equal(theta, a[3])\n\n for _ in range(5):\n A2B = pt.random_transform(rng)\n dq = pt.dual_quaternion_from_transform(A2B)\n Stheta = pt.exponential_coordinates_from_transform(A2B)\n S, theta = pt.screw_axis_from_exponential_coordinates(Stheta)\n q, s_axis, h = pt.screw_parameters_from_screw_axis(S)\n q2, s_axis2, h2, theta2 = pt.screw_parameters_from_dual_quaternion(dq)\n pt.assert_screw_parameters_equal(\n q, s_axis, h, theta, q2, s_axis2, h2, theta2\n )\n" }, { "path": "pytransform3d/transformations/test/test_pq.py", "content": "import numpy as np\nimport pytest\nfrom numpy.testing import assert_array_almost_equal\n\nimport pytransform3d.rotations as pr\nimport pytransform3d.transformations as pt\n\n\ndef test_check_pq():\n \"\"\"Test input validation for position and orientation quaternion.\"\"\"\n q = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0])\n q2 = pt.check_pq(q)\n assert_array_almost_equal(q, q2)\n\n q3 = [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]\n q4 = pt.check_pq(q3)\n assert_array_almost_equal(q3, q4)\n assert len(q3) == q4.shape[0]\n\n A2B = np.eye(4)\n with pytest.raises(ValueError, match=\"position and orientation quaternion\"):\n pt.check_pq(A2B)\n q5 = np.zeros(8)\n with pytest.raises(ValueError, match=\"position and orientation quaternion\"):\n pt.check_pq(q5)\n\n\ndef test_pq_slerp():\n start = np.array([0.2, 0.3, 0.4, 1.0, 0.0, 0.0, 0.0])\n end = np.array([1.0, 0.5, 0.8, 0.0, 1.0, 0.0, 0.0])\n pq_05 = pt.pq_slerp(start, end, 0.5)\n assert_array_almost_equal(pq_05, [0.6, 0.4, 0.6, 0.707107, 0.707107, 0, 0])\n pq_025 = pt.pq_slerp(start, end, 0.25)\n assert_array_almost_equal(pq_025, [0.4, 0.35, 0.5, 0.92388, 0.382683, 0, 0])\n pq_075 = pt.pq_slerp(start, end, 0.75)\n assert_array_almost_equal(pq_075, [0.8, 0.45, 0.7, 0.382683, 0.92388, 0, 0])\n\n\ndef test_transform_from_pq():\n \"\"\"Test conversion from position and quaternion to homogeneous matrix.\"\"\"\n pq = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0])\n A2B = pt.transform_from_pq(pq)\n assert_array_almost_equal(A2B, np.eye(4))\n\n\ndef test_conversions_between_dual_quternion_and_pq():\n rng = np.random.default_rng(1000)\n for _ in range(5):\n pq = pr.random_vector(rng, 7)\n pq[3:] /= np.linalg.norm(pq[3:])\n dq = pt.dual_quaternion_from_pq(pq)\n pq2 = pt.pq_from_dual_quaternion(dq)\n assert_array_almost_equal(pq, pq2)\n dq2 = pt.dual_quaternion_from_pq(pq2)\n pt.assert_unit_dual_quaternion_equal(dq, dq2)\n" }, { "path": "pytransform3d/transformations/test/test_screws.py", "content": "import numpy as np\nimport pytest\nfrom numpy.testing import assert_array_almost_equal\n\nimport pytransform3d.rotations as pr\nimport pytransform3d.transformations as pt\n\n\ndef test_check_screw_parameters():\n q = [100.0, -20.3, 1e-3]\n s_axis = pr.norm_vector(np.array([-1.0, 2.0, 3.0])).tolist()\n h = 0.2\n\n with pytest.raises(ValueError, match=\"Expected 3D vector with shape\"):\n pt.check_screw_parameters([0.0], s_axis, h)\n with pytest.raises(ValueError, match=\"Expected 3D vector with shape\"):\n pt.check_screw_parameters(q, [0.0], h)\n with pytest.raises(ValueError, match=\"s_axis must not have norm 0\"):\n pt.check_screw_parameters(q, np.zeros(3), h)\n\n q2, s_axis2, h2 = pt.check_screw_parameters(q, 2.0 * np.array(s_axis), h)\n assert_array_almost_equal(q, q2)\n assert pytest.approx(h) == h2\n assert pytest.approx(np.linalg.norm(s_axis2)) == 1.0\n\n q2, s_axis2, h2 = pt.check_screw_parameters(q, s_axis, h)\n assert_array_almost_equal(q, q2)\n assert_array_almost_equal(s_axis, s_axis2)\n assert pytest.approx(h) == h2\n\n q2, s_axis2, h2 = pt.check_screw_parameters(q, s_axis, np.inf)\n assert_array_almost_equal(np.zeros(3), q2)\n assert_array_almost_equal(s_axis, s_axis2)\n assert np.isinf(h2)\n\n\ndef test_check_screw_axis():\n rng = np.random.default_rng(73)\n omega = pr.norm_vector(pr.random_vector(rng, 3))\n v = pr.random_vector(rng, 3)\n\n with pytest.raises(ValueError, match=\"Expected 3D vector with shape\"):\n pt.check_screw_axis(np.r_[0, 1, v])\n\n with pytest.raises(\n ValueError, match=\"Norm of rotation axis must either be 0 or 1\"\n ):\n pt.check_screw_axis(np.r_[2 * omega, v])\n\n with pytest.raises(\n ValueError, match=\"If the norm of the rotation axis is 0\"\n ):\n pt.check_screw_axis(np.r_[0, 0, 0, v])\n\n S_pure_translation = np.r_[0, 0, 0, pr.norm_vector(v)]\n S = pt.check_screw_axis(S_pure_translation)\n assert_array_almost_equal(S, S_pure_translation)\n\n S_both = np.r_[omega, v]\n S = pt.check_screw_axis(S_both)\n assert_array_almost_equal(S, S_both)\n\n\ndef test_check_exponential_coordinates():\n with pytest.raises(ValueError, match=\"Expected array-like with shape\"):\n pt.check_exponential_coordinates([0])\n\n Stheta = [0.0, 1.0, 2.0, -5.0, -2, 3]\n Stheta2 = pt.check_exponential_coordinates(Stheta)\n assert_array_almost_equal(Stheta, Stheta2)\n\n\ndef test_check_screw_matrix():\n with pytest.raises(ValueError, match=\"Expected array-like with shape\"):\n pt.check_screw_matrix(np.zeros((1, 4, 4)))\n with pytest.raises(ValueError, match=\"Expected array-like with shape\"):\n pt.check_screw_matrix(np.zeros((3, 4)))\n with pytest.raises(ValueError, match=\"Expected array-like with shape\"):\n pt.check_screw_matrix(np.zeros((4, 3)))\n\n with pytest.raises(\n ValueError, match=\"Last row of screw matrix must only \" \"contains zeros\"\n ):\n pt.check_screw_matrix(np.eye(4))\n\n screw_matrix = (\n pt.screw_matrix_from_screw_axis(\n np.array([1.0, 0.0, 0.0, 1.0, 0.0, 0.0])\n )\n * 1.1\n )\n with pytest.raises(\n ValueError, match=\"Norm of rotation axis must either be 0 or 1\"\n ):\n pt.check_screw_matrix(screw_matrix)\n\n screw_matrix = (\n pt.screw_matrix_from_screw_axis(\n np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0])\n )\n * 1.1\n )\n with pytest.raises(\n ValueError, match=\"If the norm of the rotation axis is 0\"\n ):\n pt.check_screw_matrix(screw_matrix)\n\n screw_matrix = pt.screw_matrix_from_screw_axis(\n np.array([1.0, 0.0, 0.0, 1.0, 0.0, 0.0])\n )\n screw_matrix2 = pt.check_screw_matrix(screw_matrix)\n assert_array_almost_equal(screw_matrix, screw_matrix2)\n\n screw_matrix = pt.screw_matrix_from_screw_axis(\n np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0])\n )\n screw_matrix2 = pt.check_screw_matrix(screw_matrix)\n assert_array_almost_equal(screw_matrix, screw_matrix2)\n\n screw_matrix = pt.screw_matrix_from_screw_axis(\n np.array([1.0, 0.0, 0.0, 1.0, 0.0, 0.0])\n )\n screw_matrix[0, 0] = 0.0001\n with pytest.raises(ValueError, match=\"Expected skew-symmetric matrix\"):\n pt.check_screw_matrix(screw_matrix)\n\n\ndef test_check_transform_log():\n with pytest.raises(ValueError, match=\"Expected array-like with shape\"):\n pt.check_transform_log(np.zeros((1, 4, 4)))\n with pytest.raises(ValueError, match=\"Expected array-like with shape\"):\n pt.check_transform_log(np.zeros((3, 4)))\n with pytest.raises(ValueError, match=\"Expected array-like with shape\"):\n pt.check_transform_log(np.zeros((4, 3)))\n\n with pytest.raises(\n ValueError,\n match=\"Last row of logarithm of transformation must \"\n \"only contains zeros\",\n ):\n pt.check_transform_log(np.eye(4))\n transform_log = (\n pt.screw_matrix_from_screw_axis(\n np.array([1.0, 0.0, 0.0, 1.0, 0.0, 0.0])\n )\n * 1.1\n )\n transform_log2 = pt.check_transform_log(transform_log)\n assert_array_almost_equal(transform_log, transform_log2)\n\n transform_log = (\n pt.screw_matrix_from_screw_axis(\n np.array([1.0, 0.0, 0.0, 1.0, 0.0, 0.0])\n )\n * 1.1\n )\n transform_log[0, 0] = 0.0001\n with pytest.raises(ValueError, match=\"Expected skew-symmetric matrix\"):\n pt.check_transform_log(transform_log)\n\n\ndef test_norm_exponential_coordinates():\n Stheta_only_translation = np.array([0.0, 0.0, 0.0, 100.0, 25.0, -23.0])\n Stheta_only_translation2 = pt.norm_exponential_coordinates(\n Stheta_only_translation\n )\n assert_array_almost_equal(Stheta_only_translation, Stheta_only_translation2)\n pt.assert_exponential_coordinates_equal(\n Stheta_only_translation, Stheta_only_translation2\n )\n\n rng = np.random.default_rng(381)\n\n # 180 degree rotation ambiguity\n for _ in range(10):\n q = rng.standard_normal(size=3)\n s = pr.norm_vector(rng.standard_normal(size=3))\n h = rng.standard_normal()\n Stheta = pt.screw_axis_from_screw_parameters(q, s, h) * np.pi\n Stheta2 = pt.screw_axis_from_screw_parameters(q, -s, -h) * np.pi\n assert_array_almost_equal(\n pt.transform_from_exponential_coordinates(Stheta),\n pt.transform_from_exponential_coordinates(Stheta2),\n )\n assert_array_almost_equal(\n pt.norm_exponential_coordinates(Stheta),\n pt.norm_exponential_coordinates(Stheta2),\n )\n pt.assert_exponential_coordinates_equal(Stheta, Stheta2)\n\n for _ in range(10):\n Stheta = rng.standard_normal(size=6)\n # ensure that theta is not within [-pi, pi]\n i = rng.integers(0, 3)\n Stheta[i] = np.sign(Stheta[i]) * (np.pi + rng.random())\n Stheta_norm = pt.norm_exponential_coordinates(Stheta)\n assert not np.all(Stheta == Stheta_norm)\n pt.assert_exponential_coordinates_equal(Stheta, Stheta_norm)\n\n A2B = pt.transform_from_exponential_coordinates(Stheta)\n Stheta2 = pt.exponential_coordinates_from_transform(A2B)\n assert_array_almost_equal(Stheta2, Stheta_norm)\n\n\ndef test_assert_screw_parameters_equal():\n q = np.array([1, 2, 3])\n s_axis = pr.norm_vector(np.array([-2, 3, 5]))\n h = 2\n theta = 1\n pt.assert_screw_parameters_equal(\n q, s_axis, h, theta, q + 484.3 * s_axis, s_axis, h, theta\n )\n\n with pytest.raises(AssertionError):\n pt.assert_screw_parameters_equal(\n q, s_axis, h, theta, q + 484.3, s_axis, h, theta\n )\n\n s_axis_mirrored = -s_axis\n theta_mirrored = 2.0 * np.pi - theta\n h_mirrored = -h * theta / theta_mirrored\n pt.assert_screw_parameters_equal(\n q, s_axis_mirrored, h_mirrored, theta_mirrored, q, s_axis, h, theta\n )\n\n\ndef test_conversions_between_screw_axis_and_parameters():\n rng = np.random.default_rng(98)\n\n q = pr.random_vector(rng, 3)\n s_axis = pr.norm_vector(pr.random_vector(rng, 3))\n h = np.inf\n screw_axis = pt.screw_axis_from_screw_parameters(q, s_axis, h)\n assert_array_almost_equal(screw_axis, np.r_[0, 0, 0, s_axis])\n q2, s_axis2, h2 = pt.screw_parameters_from_screw_axis(screw_axis)\n\n assert_array_almost_equal(np.zeros(3), q2)\n assert_array_almost_equal(s_axis, s_axis2)\n assert_array_almost_equal(h, h2)\n\n for _ in range(10):\n s_axis = pr.norm_vector(pr.random_vector(rng, 3))\n # q has to be orthogonal to s_axis so that we reconstruct it exactly\n q = pr.perpendicular_to_vector(s_axis)\n h = rng.random() + 0.5\n\n screw_axis = pt.screw_axis_from_screw_parameters(q, s_axis, h)\n q2, s_axis2, h2 = pt.screw_parameters_from_screw_axis(screw_axis)\n\n assert_array_almost_equal(q, q2)\n assert_array_almost_equal(s_axis, s_axis2)\n assert_array_almost_equal(h, h2)\n\n\ndef test_conversions_between_exponential_coordinates_and_transform():\n A2B = np.eye(4)\n Stheta = pt.exponential_coordinates_from_transform(A2B)\n assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0])\n A2B2 = pt.transform_from_exponential_coordinates(Stheta)\n assert_array_almost_equal(A2B, A2B2)\n\n A2B = pt.translate_transform(np.eye(4), [1.0, 5.0, 0.0])\n Stheta = pt.exponential_coordinates_from_transform(A2B)\n assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 1.0, 5.0, 0.0])\n A2B2 = pt.transform_from_exponential_coordinates(Stheta)\n assert_array_almost_equal(A2B, A2B2)\n\n A2B = pt.rotate_transform(\n np.eye(4), pr.active_matrix_from_angle(2, 0.5 * np.pi)\n )\n Stheta = pt.exponential_coordinates_from_transform(A2B)\n assert_array_almost_equal(Stheta, [0.0, 0.0, 0.5 * np.pi, 0.0, 0.0, 0.0])\n A2B2 = pt.transform_from_exponential_coordinates(Stheta)\n assert_array_almost_equal(A2B, A2B2)\n\n rng = np.random.default_rng(52)\n for _ in range(5):\n A2B = pt.random_transform(rng)\n Stheta = pt.exponential_coordinates_from_transform(A2B)\n A2B2 = pt.transform_from_exponential_coordinates(Stheta)\n assert_array_almost_equal(A2B, A2B2)\n\n\ndef test_transform_from_exponential_coordinates_without_check():\n Stheta = np.zeros(7)\n with pytest.raises(ValueError, match=\"could not broadcast input array\"):\n pt.transform_from_exponential_coordinates(Stheta, check=False)\n\n\ndef test_conversions_between_screw_axis_and_exponential_coordinates():\n S = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0])\n theta = 1.0\n Stheta = pt.exponential_coordinates_from_screw_axis(S, theta)\n S2, theta2 = pt.screw_axis_from_exponential_coordinates(Stheta)\n assert_array_almost_equal(S, S2)\n assert pytest.approx(theta) == theta2\n\n S = np.zeros(6)\n theta = 0.0\n S2, theta2 = pt.screw_axis_from_exponential_coordinates(np.zeros(6))\n assert_array_almost_equal(S, S2)\n assert pytest.approx(theta) == theta2\n\n rng = np.random.default_rng(33)\n for _ in range(5):\n S = pt.random_screw_axis(rng)\n theta = rng.random()\n Stheta = pt.exponential_coordinates_from_screw_axis(S, theta)\n S2, theta2 = pt.screw_axis_from_exponential_coordinates(Stheta)\n assert_array_almost_equal(S, S2)\n assert pytest.approx(theta) == theta2\n\n\ndef test_conversions_between_exponential_coordinates_and_transform_log():\n rng = np.random.default_rng(22)\n for _ in range(5):\n Stheta = rng.standard_normal(size=6)\n transform_log = pt.transform_log_from_exponential_coordinates(Stheta)\n Stheta2 = pt.exponential_coordinates_from_transform_log(transform_log)\n assert_array_almost_equal(Stheta, Stheta2)\n\n\ndef test_exponential_coordinates_from_transform_log_without_check():\n transform_log = np.ones((4, 4))\n Stheta = pt.exponential_coordinates_from_transform_log(\n transform_log, check=False\n )\n assert_array_almost_equal(Stheta, np.ones(6))\n\n\ndef test_conversions_between_screw_matrix_and_screw_axis():\n rng = np.random.default_rng(83)\n for _ in range(5):\n S = pt.random_screw_axis(rng)\n S_mat = pt.screw_matrix_from_screw_axis(S)\n S2 = pt.screw_axis_from_screw_matrix(S_mat)\n assert_array_almost_equal(S, S2)\n\n\ndef test_conversions_between_screw_matrix_and_transform_log():\n S_mat = np.array(\n [\n [0.0, 0.0, 0.0, 1.0],\n [0.0, 0.0, 0.0, 0.0],\n [0.0, 0.0, 0.0, 0.0],\n [0.0, 0.0, 0.0, 0.0],\n ]\n )\n theta = 1.0\n transform_log = pt.transform_log_from_screw_matrix(S_mat, theta)\n S_mat2, theta2 = pt.screw_matrix_from_transform_log(transform_log)\n assert_array_almost_equal(S_mat, S_mat2)\n assert pytest.approx(theta) == theta2\n\n S_mat = np.zeros((4, 4))\n theta = 0.0\n transform_log = pt.transform_log_from_screw_matrix(S_mat, theta)\n S_mat2, theta2 = pt.screw_matrix_from_transform_log(transform_log)\n assert_array_almost_equal(S_mat, S_mat2)\n assert pytest.approx(theta) == theta2\n\n rng = np.random.default_rng(65)\n for _ in range(5):\n S = pt.random_screw_axis(rng)\n theta = float(np.random.random())\n S_mat = pt.screw_matrix_from_screw_axis(S)\n transform_log = pt.transform_log_from_screw_matrix(S_mat, theta)\n S_mat2, theta2 = pt.screw_matrix_from_transform_log(transform_log)\n assert_array_almost_equal(S_mat, S_mat2)\n assert pytest.approx(theta) == theta2\n\n\ndef test_conversions_between_transform_and_transform_log():\n A2B = np.eye(4)\n transform_log = pt.transform_log_from_transform(A2B)\n assert_array_almost_equal(transform_log, np.zeros((4, 4)))\n A2B2 = pt.transform_from_transform_log(transform_log)\n assert_array_almost_equal(A2B, A2B2)\n\n rng = np.random.default_rng(84)\n A2B = pt.transform_from(np.eye(3), p=pr.random_vector(rng, 3))\n transform_log = pt.transform_log_from_transform(A2B)\n A2B2 = pt.transform_from_transform_log(transform_log)\n assert_array_almost_equal(A2B, A2B2)\n\n for _ in range(5):\n A2B = pt.random_transform(rng)\n transform_log = pt.transform_log_from_transform(A2B)\n A2B2 = pt.transform_from_transform_log(transform_log)\n assert_array_almost_equal(A2B, A2B2)\n\n\ndef test_dual_quaternion_from_screw_parameters():\n q = np.zeros(3)\n s_axis = np.array([1, 0, 0])\n h = np.inf\n theta = 0.0\n dq = pt.dual_quaternion_from_screw_parameters(q, s_axis, h, theta)\n assert_array_almost_equal(dq, np.array([1, 0, 0, 0, 0, 0, 0, 0]))\n\n q = np.zeros(3)\n s_axis = pr.norm_vector(np.array([2.3, 2.4, 2.5]))\n h = np.inf\n theta = 3.6\n dq = pt.dual_quaternion_from_screw_parameters(q, s_axis, h, theta)\n pq = pt.pq_from_dual_quaternion(dq)\n assert_array_almost_equal(pq, np.r_[s_axis * theta, 1, 0, 0, 0])\n\n q = np.zeros(3)\n s_axis = pr.norm_vector(np.array([2.4, 2.5, 2.6]))\n h = 0.0\n theta = 4.1\n dq = pt.dual_quaternion_from_screw_parameters(q, s_axis, h, theta)\n pq = pt.pq_from_dual_quaternion(dq)\n assert_array_almost_equal(pq[:3], [0, 0, 0])\n assert_array_almost_equal(\n pr.axis_angle_from_quaternion(pq[3:]),\n pr.norm_axis_angle(np.r_[s_axis, theta]),\n )\n\n rng = np.random.default_rng(1001)\n for _ in range(5):\n A2B = pt.random_transform(rng)\n Stheta = pt.exponential_coordinates_from_transform(A2B)\n S, theta = pt.screw_axis_from_exponential_coordinates(Stheta)\n q, s_axis, h = pt.screw_parameters_from_screw_axis(S)\n dq = pt.dual_quaternion_from_screw_parameters(q, s_axis, h, theta)\n pt.assert_unit_dual_quaternion(dq)\n\n dq_expected = pt.dual_quaternion_from_transform(A2B)\n pt.assert_unit_dual_quaternion_equal(dq, dq_expected)\n" }, { "path": "pytransform3d/transformations/test/test_transform.py", "content": "import warnings\n\nimport numpy as np\nimport pytest\nfrom numpy.testing import assert_array_almost_equal\n\nimport pytransform3d.rotations as pr\nimport pytransform3d.transformations as pt\n\n\ndef test_check_transform():\n \"\"\"Test input validation for transformation matrix.\"\"\"\n A2B = np.eye(3)\n with pytest.raises(\n ValueError,\n match=\"Expected homogeneous transformation matrix with shape\",\n ):\n pt.check_transform(A2B)\n\n A2B = np.eye(4, dtype=int)\n A2B = pt.check_transform(A2B)\n assert type(A2B) is np.ndarray\n assert A2B.dtype == np.float64\n\n A2B[:3, :3] = np.array([[1, 1, 1], [0, 0, 0], [2, 2, 2]])\n with pytest.raises(ValueError, match=\"rotation matrix\"):\n pt.check_transform(A2B)\n\n A2B = np.eye(4)\n A2B[3, :] = np.array([0.1, 0.0, 0.0, 1.0])\n with pytest.raises(ValueError, match=\"homogeneous transformation matrix\"):\n pt.check_transform(A2B)\n\n rng = np.random.default_rng(0)\n A2B = pt.random_transform(rng)\n A2B2 = pt.check_transform(A2B)\n assert_array_almost_equal(A2B, A2B2)\n\n\ndef test_deactivate_transform_precision_error():\n A2B = np.eye(4)\n A2B[0, 0] = 2.0\n A2B[3, 0] = 3.0\n with pytest.raises(ValueError, match=\"Expected rotation matrix\"):\n pt.check_transform(A2B)\n\n n_expected_warnings = 2\n try:\n warnings.filterwarnings(\"always\", category=UserWarning)\n with warnings.catch_warnings(record=True) as w:\n pt.check_transform(A2B, strict_check=False)\n assert len(w) == n_expected_warnings\n finally:\n warnings.filterwarnings(\"default\", category=UserWarning)\n\n\ndef test_transform_requires_renormalization():\n assert pt.transform_requires_renormalization(np.eye(4) + 1e-6)\n assert not pt.transform_requires_renormalization(np.eye(4))\n\n\ndef test_translate_transform_with_check():\n A2B_broken = np.zeros((4, 4))\n with pytest.raises(ValueError, match=\"rotation matrix\"):\n pt.translate_transform(A2B_broken, np.zeros(3))\n\n\ndef test_rotate_transform_with_check():\n A2B_broken = np.zeros((4, 4))\n with pytest.raises(ValueError, match=\"rotation matrix\"):\n pt.rotate_transform(A2B_broken, np.eye(3))\n\n\ndef test_pq_from_transform():\n \"\"\"Test conversion from homogeneous matrix to position and quaternion.\"\"\"\n A2B = np.eye(4)\n pq = pt.pq_from_transform(A2B)\n assert_array_almost_equal(pq, np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]))\n\n\ndef test_exponential_coordinates_from_almost_identity_transform():\n A2B = np.array(\n [\n [\n 0.9999999999999999,\n -1.5883146449068575e-16,\n 4.8699079321578667e-17,\n -7.54265065748827e-05,\n ],\n [\n 5.110044286978025e-17,\n 0.9999999999999999,\n 1.1798895336935056e-17,\n 9.340523179823812e-05,\n ],\n [\n 3.0048299647976294e-18,\n 5.4741890703482423e-17,\n 1.0,\n -7.803584869947588e-05,\n ],\n [0, 0, 0, 1],\n ]\n )\n Stheta = pt.exponential_coordinates_from_transform(A2B)\n assert_array_almost_equal(np.zeros(6), Stheta, decimal=4)\n\n\ndef test_exponential_coordinates_from_transform_without_check():\n transform = np.ones((4, 4))\n Stheta = pt.exponential_coordinates_from_transform(transform, check=False)\n assert_array_almost_equal(Stheta, np.array([0, 0, 0, 1, 1, 1]))\n\n\ndef test_transform_log_from_almost_identity_transform():\n A2B = np.array(\n [\n [\n 0.9999999999999999,\n -1.5883146449068575e-16,\n 4.8699079321578667e-17,\n -7.54265065748827e-05,\n ],\n [\n 5.110044286978025e-17,\n 0.9999999999999999,\n 1.1798895336935056e-17,\n 9.340523179823812e-05,\n ],\n [\n 3.0048299647976294e-18,\n 5.4741890703482423e-17,\n 1.0,\n -7.803584869947588e-05,\n ],\n [0, 0, 0, 1],\n ]\n )\n transform_log = pt.transform_log_from_transform(A2B)\n assert_array_almost_equal(np.zeros((4, 4)), transform_log)\n\n\ndef test_conversions_between_dual_quaternion_and_transform():\n rng = np.random.default_rng(1000)\n for _ in range(5):\n A2B = pt.random_transform(rng)\n dq = pt.dual_quaternion_from_transform(A2B)\n A2B2 = pt.transform_from_dual_quaternion(dq)\n assert_array_almost_equal(A2B, A2B2)\n dq2 = pt.dual_quaternion_from_transform(A2B2)\n pt.assert_unit_dual_quaternion_equal(dq, dq2)\n for _ in range(5):\n p = pr.random_vector(rng, 3)\n q = pr.random_quaternion(rng)\n dq = pt.dual_quaternion_from_pq(np.hstack((p, q)))\n A2B = pt.transform_from_dual_quaternion(dq)\n dq2 = pt.dual_quaternion_from_transform(A2B)\n pt.assert_unit_dual_quaternion_equal(dq, dq2)\n A2B2 = pt.transform_from_dual_quaternion(dq2)\n assert_array_almost_equal(A2B, A2B2)\n\n\ndef test_adjoint_of_transformation():\n rng = np.random.default_rng(94)\n for _ in range(5):\n A2B = pt.random_transform(rng)\n theta_dot = 3.0 * float(rng.random())\n S = pt.random_screw_axis(rng)\n\n V_A = S * theta_dot\n\n adj_A2B = pt.adjoint_from_transform(A2B)\n V_B = adj_A2B.dot(V_A)\n\n S_mat = pt.screw_matrix_from_screw_axis(S)\n V_mat_A = S_mat * theta_dot\n V_mat_B = np.dot(np.dot(A2B, V_mat_A), pt.invert_transform(A2B))\n\n S_B, theta_dot2 = pt.screw_axis_from_exponential_coordinates(V_B)\n V_mat_B2 = pt.screw_matrix_from_screw_axis(S_B) * theta_dot2\n assert pytest.approx(theta_dot) == theta_dot2\n assert_array_almost_equal(V_mat_B, V_mat_B2)\n\n\ndef test_adjoint_from_transform_without_check():\n transform = np.ones((4, 4))\n adjoint = pt.adjoint_from_transform(transform, check=False)\n assert_array_almost_equal(adjoint[:3, :3], np.ones((3, 3)))\n assert_array_almost_equal(adjoint[3:, 3:], np.ones((3, 3)))\n assert_array_almost_equal(adjoint[3:, :3], np.zeros((3, 3)))\n assert_array_almost_equal(adjoint[:3, 3:], np.zeros((3, 3)))\n" }, { "path": "pytransform3d/transformations/test/test_transform_operations.py", "content": "import numpy as np\nimport pytest\nfrom numpy.testing import assert_array_almost_equal\n\nimport pytransform3d.rotations as pr\nimport pytransform3d.transformations as pt\n\n\ndef test_invert_transform():\n \"\"\"Test inversion of transformations.\"\"\"\n rng = np.random.default_rng(0)\n for _ in range(5):\n R = pr.matrix_from_axis_angle(pr.random_axis_angle(rng))\n p = pr.random_vector(rng)\n A2B = pt.transform_from(R, p)\n B2A = pt.invert_transform(A2B)\n A2B2 = np.linalg.inv(B2A)\n assert_array_almost_equal(A2B, A2B2)\n\n\ndef test_invert_transform_without_check():\n \"\"\"Test inversion of transformations.\"\"\"\n rng = np.random.default_rng(0)\n for _ in range(5):\n A2B = rng.standard_normal(size=(4, 4))\n A2B = A2B + A2B.T\n B2A = pt.invert_transform(A2B, check=False)\n A2B2 = np.linalg.inv(B2A)\n assert_array_almost_equal(A2B, A2B2)\n\n\ndef test_vector_to_point():\n \"\"\"Test conversion from vector to homogenous coordinates.\"\"\"\n v = np.array([1, 2, 3])\n pA = pt.vector_to_point(v)\n assert_array_almost_equal(pA, [1, 2, 3, 1])\n\n rng = np.random.default_rng(0)\n R = pr.matrix_from_axis_angle(pr.random_axis_angle(rng))\n p = pr.random_vector(rng)\n A2B = pt.transform_from(R, p)\n pt.assert_transform(A2B)\n _ = pt.transform(A2B, pA)\n\n\ndef test_vectors_to_points():\n \"\"\"Test conversion from vectors to homogenous coordinates.\"\"\"\n V = np.array([[1, 2, 3], [2, 3, 4]])\n PA = pt.vectors_to_points(V)\n assert_array_almost_equal(PA, [[1, 2, 3, 1], [2, 3, 4, 1]])\n\n rng = np.random.default_rng(0)\n V = rng.standard_normal(size=(10, 3))\n for i, p in enumerate(pt.vectors_to_points(V)):\n assert_array_almost_equal(p, pt.vector_to_point(V[i]))\n\n\ndef test_vector_to_direction():\n \"\"\"Test conversion from vector to direction in homogenous coordinates.\"\"\"\n v = np.array([1, 2, 3])\n dA = pt.vector_to_direction(v)\n assert_array_almost_equal(dA, [1, 2, 3, 0])\n\n rng = np.random.default_rng(0)\n R = pr.matrix_from_axis_angle(pr.random_axis_angle(rng))\n p = pr.random_vector(rng)\n A2B = pt.transform_from(R, p)\n pt.assert_transform(A2B)\n _ = pt.transform(A2B, dA)\n\n\ndef test_vectors_to_directions():\n \"\"\"Test conversion from vectors to directions in homogenous coordinates.\"\"\"\n V = np.array([[1, 2, 3], [2, 3, 4]])\n DA = pt.vectors_to_directions(V)\n assert_array_almost_equal(DA, [[1, 2, 3, 0], [2, 3, 4, 0]])\n\n rng = np.random.default_rng(0)\n V = rng.standard_normal(size=(10, 3))\n for i, d in enumerate(pt.vectors_to_directions(V)):\n assert_array_almost_equal(d, pt.vector_to_direction(V[i]))\n\n\ndef test_concat():\n \"\"\"Test concatenation of transforms.\"\"\"\n rng = np.random.default_rng(0)\n for _ in range(5):\n A2B = pt.random_transform(rng)\n pt.assert_transform(A2B)\n\n B2C = pt.random_transform(rng)\n pt.assert_transform(B2C)\n\n A2C = pt.concat(A2B, B2C)\n pt.assert_transform(A2C)\n\n p_A = np.array([0.3, -0.2, 0.9, 1.0])\n p_C = pt.transform(A2C, p_A)\n\n C2A = pt.invert_transform(A2C)\n p_A2 = pt.transform(C2A, p_C)\n\n assert_array_almost_equal(p_A, p_A2)\n\n C2A2 = pt.concat(pt.invert_transform(B2C), pt.invert_transform(A2B))\n p_A3 = pt.transform(C2A2, p_C)\n assert_array_almost_equal(p_A, p_A3)\n\n\ndef test_transform():\n \"\"\"Test transformation of points.\"\"\"\n PA = np.array([[1, 2, 3, 1], [2, 3, 4, 1]])\n\n rng = np.random.default_rng(0)\n A2B = pt.random_transform(rng)\n\n PB = pt.transform(A2B, PA)\n p0B = pt.transform(A2B, PA[0])\n p1B = pt.transform(A2B, PA[1])\n assert_array_almost_equal(PB, np.array([p0B, p1B]))\n\n with pytest.raises(\n ValueError, match=\"Cannot transform array with more than 2 dimensions\"\n ):\n pt.transform(A2B, np.zeros((2, 2, 4)))\n\n\ndef test_scale_transform():\n \"\"\"Test scaling of transforms.\"\"\"\n rng = np.random.default_rng(0)\n A2B = pt.random_transform(rng)\n\n A2B_scaled1 = pt.scale_transform(A2B, s_xt=0.5, s_yt=0.5, s_zt=0.5)\n A2B_scaled2 = pt.scale_transform(A2B, s_t=0.5)\n assert_array_almost_equal(A2B_scaled1, A2B_scaled2)\n\n A2B_scaled1 = pt.scale_transform(A2B, s_xt=0.5, s_yt=0.5, s_zt=0.5, s_r=0.5)\n A2B_scaled2 = pt.scale_transform(A2B, s_t=0.5, s_r=0.5)\n A2B_scaled3 = pt.scale_transform(A2B, s_d=0.5)\n assert_array_almost_equal(A2B_scaled1, A2B_scaled2)\n assert_array_almost_equal(A2B_scaled1, A2B_scaled3)\n\n A2B_scaled = pt.scale_transform(A2B, s_xr=0.0)\n a_scaled = pr.axis_angle_from_matrix(A2B_scaled[:3, :3])\n assert_array_almost_equal(a_scaled[0], 0.0)\n A2B_scaled = pt.scale_transform(A2B, s_yr=0.0)\n a_scaled = pr.axis_angle_from_matrix(A2B_scaled[:3, :3])\n assert_array_almost_equal(a_scaled[1], 0.0)\n A2B_scaled = pt.scale_transform(A2B, s_zr=0.0)\n a_scaled = pr.axis_angle_from_matrix(A2B_scaled[:3, :3])\n assert_array_almost_equal(a_scaled[2], 0.0)\n" }, { "path": "pytransform3d/transformations/test/test_transformations_jacobians.py", "content": "import numpy as np\nfrom numpy.testing import assert_array_almost_equal\n\nimport pytransform3d.transformations as pt\n\n\ndef test_jacobian_se3():\n Stheta = np.zeros(6)\n\n J = pt.left_jacobian_SE3(Stheta)\n J_series = pt.left_jacobian_SE3_series(Stheta, 20)\n assert_array_almost_equal(J, J_series)\n\n J_inv = pt.left_jacobian_SE3_inv(Stheta)\n J_inv_serias = pt.left_jacobian_SE3_inv_series(Stheta, 20)\n assert_array_almost_equal(J_inv, J_inv_serias)\n\n J_inv_J = np.dot(J_inv, J)\n assert_array_almost_equal(J_inv_J, np.eye(6))\n\n rng = np.random.default_rng(0)\n for _ in range(5):\n Stheta = pt.random_exponential_coordinates(rng)\n\n J = pt.left_jacobian_SE3(Stheta)\n J_series = pt.left_jacobian_SE3_series(Stheta, 20)\n assert_array_almost_equal(J, J_series)\n\n J_inv = pt.left_jacobian_SE3_inv(Stheta)\n J_inv_serias = pt.left_jacobian_SE3_inv_series(Stheta, 20)\n assert_array_almost_equal(J_inv, J_inv_serias)\n\n J_inv_J = np.dot(J_inv, J)\n assert_array_almost_equal(J_inv_J, np.eye(6))\n" }, { "path": "pytransform3d/transformations/test/test_transformations_random.py", "content": "import numpy as np\n\nimport pytransform3d.transformations as pt\n\n\ndef test_random_screw_axis():\n rng = np.random.default_rng(893)\n for _ in range(5):\n S = pt.random_screw_axis(rng)\n pt.check_screw_axis(S)\n" }, { "path": "pytransform3d/uncertainty/__init__.py", "content": "\"\"\"Operations related to uncertain transformations.\n\nSee :doc:`user_guide/uncertainty` for more information.\n\"\"\"\n\nfrom ._composition import (\n concat_globally_uncertain_transforms,\n concat_locally_uncertain_transforms,\n)\nfrom ._frechet_mean import (\n estimate_gaussian_rotation_matrix_from_samples,\n estimate_gaussian_transform_from_samples,\n frechet_mean,\n)\nfrom ._fusion import pose_fusion\nfrom ._invert import invert_uncertain_transform\nfrom ._plot import (\n to_ellipsoid,\n to_projected_ellipsoid,\n plot_projected_ellipsoid,\n)\n\n__all__ = [\n \"concat_globally_uncertain_transforms\",\n \"concat_locally_uncertain_transforms\",\n \"frechet_mean\",\n \"estimate_gaussian_rotation_matrix_from_samples\",\n \"estimate_gaussian_transform_from_samples\",\n \"pose_fusion\",\n \"invert_uncertain_transform\",\n \"to_ellipsoid\",\n \"to_projected_ellipsoid\",\n \"plot_projected_ellipsoid\",\n]\n" }, { "path": "pytransform3d/uncertainty/_composition.py", "content": "\"\"\"Composition of uncertain transformations.\"\"\"\n\nimport numpy as np\n\nfrom ..transformations import (\n adjoint_from_transform,\n concat,\n invert_transform,\n)\n\n\ndef concat_globally_uncertain_transforms(mean_A2B, cov_A2B, mean_B2C, cov_B2C):\n r\"\"\"Concatenate two independent globally uncertain transformations.\n\n We assume that the two distributions are independent.\n\n Each of the two transformations is globally uncertain (not in the local /\n body frame), that is, samples are generated through\n\n .. math::\n\n \\boldsymbol{T} = Exp(\\boldsymbol{\\xi}) \\overline{\\boldsymbol{T}},\n\n where :math:`\\boldsymbol{T} \\in SE(3)` is a sampled transformation matrix,\n :math:`\\overline{\\boldsymbol{T}} \\in SE(3)` is the mean transformation,\n and :math:`\\boldsymbol{\\xi} \\in \\mathbb{R}^6` are exponential coordinates\n of transformations and are distributed according to a Gaussian\n distribution with zero mean and covariance :math:`\\boldsymbol{\\Sigma} \\in\n \\mathbb{R}^{6 \\times 6}`, that is, :math:`\\boldsymbol{\\xi} \\sim\n \\mathcal{N}(\\boldsymbol{0}, \\boldsymbol{\\Sigma})`.\n\n The concatenation order is the same as in\n :func:`~pytransform3d.transformations.concat`, that is, the transformation\n B2C is left-multiplied to A2B. Note that the order of arguments is\n different from\n :func:`~pytransform3d.uncertainty.concat_locally_uncertain_transforms`.\n\n Hence, the full model is\n\n .. math::\n\n Exp(_C\\boldsymbol{\\xi'}) \\overline{\\boldsymbol{T}}_{CA} =\n Exp(_C\\boldsymbol{\\xi}) \\overline{\\boldsymbol{T}}_{CB}\n Exp(_B\\boldsymbol{\\xi}) \\overline{\\boldsymbol{T}}_{BA},\n\n where :math:`_B\\boldsymbol{\\xi} \\sim \\mathcal{N}(\\boldsymbol{0},\n \\boldsymbol{\\Sigma}_{BA})`, :math:`_C\\boldsymbol{\\xi} \\sim\n \\mathcal{N}(\\boldsymbol{0}, \\boldsymbol{\\Sigma}_{CB})`, and\n :math:`_C\\boldsymbol{\\xi'} \\sim \\mathcal{N}(\\boldsymbol{0},\n \\boldsymbol{\\Sigma}_{CA})`.\n\n This version of Barfoot and Furgale [1]_ approximates the covariance up to\n 4th-order terms. Note that it is still an approximation of the covariance\n after concatenation of the two transforms.\n\n Parameters\n ----------\n mean_A2B : array, shape (4, 4)\n Mean of transform from A to B.\n\n cov_A2B : array, shape (6, 6)\n Covariance of transform from A to B. Models uncertainty in frame B.\n\n mean_B2C : array, shape (4, 4)\n Mean of transform from B to C.\n\n cov_B2C : array, shape (6, 6)\n Covariance of transform from B to C. Models uncertainty in frame C.\n\n Returns\n -------\n mean_A2C : array, shape (4, 4)\n Mean of new pose.\n\n cov_A2C : array, shape (6, 6)\n Covariance of new pose. Models uncertainty in frame C.\n\n See Also\n --------\n concat_locally_uncertain_transforms :\n Concatenate two independent locally uncertain transformations.\n\n pytransform3d.transformations.concat :\n Concatenate two transformations.\n\n References\n ----------\n .. [1] Barfoot, T. D., Furgale, P. T. (2014).\n Associating Uncertainty With Three-Dimensional Poses for Use in\n Estimation Problems. IEEE Transactions on Robotics, 30(3), pp. 679-693,\n doi: 10.1109/TRO.2014.2298059.\n \"\"\"\n mean_A2C = concat(mean_A2B, mean_B2C)\n\n ad_B2C = adjoint_from_transform(mean_B2C)\n cov_A2B_in_C = np.dot(ad_B2C, np.dot(cov_A2B, ad_B2C.T))\n second_order_terms = cov_B2C + cov_A2B_in_C\n\n cov_A2C = second_order_terms + _compound_cov_fourth_order_terms(\n cov_B2C, cov_A2B_in_C\n )\n\n return mean_A2C, cov_A2C\n\n\ndef _compound_cov_fourth_order_terms(cov1, cov2_prime):\n cov1 = _swap_cov(cov1)\n cov2_prime = _swap_cov(cov2_prime)\n\n cov1_11 = cov1[:3, :3]\n cov1_22 = cov1[3:, 3:]\n cov1_12 = cov1[:3, 3:]\n\n cov2_11 = cov2_prime[:3, :3]\n cov2_22 = cov2_prime[3:, 3:]\n cov2_12 = cov2_prime[:3, 3:]\n\n A1 = np.block(\n [\n [_covop1(cov1_22), _covop1(cov1_12 + cov1_12.T)],\n [np.zeros((3, 3)), _covop1(cov1_22)],\n ]\n )\n A2 = np.block(\n [\n [_covop1(cov2_22), _covop1(cov2_12 + cov2_12.T)],\n [np.zeros((3, 3)), _covop1(cov2_22)],\n ]\n )\n B_11 = (\n _covop2(cov1_22, cov2_11)\n + _covop2(cov1_12.T, cov2_12)\n + _covop2(cov1_12, cov2_12.T)\n + _covop2(cov1_11, cov2_22)\n )\n B_12 = _covop2(cov1_22, cov2_12.T) + _covop2(cov1_12.T, cov2_22)\n B_22 = _covop2(cov1_22, cov2_22)\n B = np.block([[B_11, B_12], [B_12.T, B_22]])\n\n return _swap_cov(\n (\n np.dot(A1, cov2_prime)\n + np.dot(cov2_prime, A1.T)\n + np.dot(A2, cov1)\n + np.dot(cov1, A2.T)\n )\n / 12.0\n + B / 4.0\n )\n\n\ndef _swap_cov(cov):\n return np.block([[cov[3:, 3:], cov[3:, :3]], [cov[:3, 3:], cov[:3, :3]]])\n\n\ndef _covop1(A):\n return -np.trace(A) * np.eye(len(A)) + A\n\n\ndef _covop2(A, B):\n return np.dot(_covop1(A), _covop1(B)) + _covop1(np.dot(B, A))\n\n\ndef concat_locally_uncertain_transforms(mean_A2B, mean_B2C, cov_A, cov_B):\n r\"\"\"Concatenate two independent locally uncertain transformations.\n\n We assume that the two distributions are independent.\n\n Each of the two transformations is locally uncertain (not in the global /\n world frame), that is, samples are generated through\n\n .. math::\n\n \\boldsymbol{T} = \\overline{\\boldsymbol{T}} Exp(\\boldsymbol{\\xi}),\n\n where :math:`\\boldsymbol{T} \\in SE(3)` is a sampled transformation matrix,\n :math:`\\overline{\\boldsymbol{T}} \\in SE(3)` is the mean transformation,\n and :math:`\\boldsymbol{\\xi} \\in \\mathbb{R}^6` are exponential coordinates\n of transformations and are distributed according to a Gaussian\n distribution with zero mean and covariance :math:`\\boldsymbol{\\Sigma} \\in\n \\mathbb{R}^{6 \\times 6}`, that is, :math:`\\boldsymbol{\\xi} \\sim\n \\mathcal{N}(\\boldsymbol{0}, \\boldsymbol{\\Sigma})`.\n\n The concatenation order is the same as in\n :func:`~pytransform3d.transformations.concat`, that is, the transformation\n B2C is left-multiplied to A2B. Note that the order of arguments is\n different from\n :func:`~pytransform3d.uncertainty.concat_globally_uncertain_transforms`.\n\n Hence, the full model is\n\n .. math::\n\n \\overline{\\boldsymbol{T}}_{CA} Exp(_A\\boldsymbol{\\xi'}) =\n \\overline{\\boldsymbol{T}}_{CB} Exp(_B\\boldsymbol{\\xi})\n \\overline{\\boldsymbol{T}}_{BA} Exp(_A\\boldsymbol{\\xi}),\n\n where :math:`_B\\boldsymbol{\\xi} \\sim \\mathcal{N}(\\boldsymbol{0},\n \\boldsymbol{\\Sigma}_B)`, :math:`_A\\boldsymbol{\\xi} \\sim\n \\mathcal{N}(\\boldsymbol{0}, \\boldsymbol{\\Sigma}_A)`, and\n :math:`_A\\boldsymbol{\\xi'} \\sim \\mathcal{N}(\\boldsymbol{0},\n \\boldsymbol{\\Sigma}_{A,total})`.\n\n This version of Meyer et al. [1]_ approximates the covariance up to\n 2nd-order terms.\n\n Parameters\n ----------\n mean_A2B : array, shape (4, 4)\n Mean of transform from A to B: :math:`\\overline{\\boldsymbol{T}}_{BA}`.\n\n mean_B2C : array, shape (4, 4)\n Mean of transform from B to C: :math:`\\overline{\\boldsymbol{T}}_{CB}`.\n\n cov_A : array, shape (6, 6)\n Covariance of noise in frame A: :math:`\\boldsymbol{\\Sigma}_A`. Noise\n samples are right-multiplied with the mean transform A2B.\n\n cov_B : array, shape (6, 6)\n Covariance of noise in frame B: :math:`\\boldsymbol{\\Sigma}_B`. Noise\n samples are right-multiplied with the mean transform B2C.\n\n Returns\n -------\n mean_A2C : array, shape (4, 4)\n Mean of new pose.\n\n cov_A_total : array, shape (6, 6)\n Covariance of accumulated noise in frame A.\n\n See Also\n --------\n concat_globally_uncertain_transforms :\n Concatenate two independent globally uncertain transformations.\n\n pytransform3d.transformations.concat :\n Concatenate two transformations.\n\n References\n ----------\n .. [1] Meyer, L., Strobl, K. H., Triebel, R. (2022). The Probabilistic\n Robot Kinematics Model and its Application to Sensor Fusion.\n In IEEE/RSJ International Conference on Intelligent Robots and Systems\n (IROS), Kyoto, Japan (pp. 3263-3270),\n doi: 10.1109/IROS47612.2022.9981399.\n https://elib.dlr.de/191928/1/202212_ELIB_PAPER_VERSION_with_copyright.pdf\n \"\"\"\n mean_A2C = concat(mean_A2B, mean_B2C)\n\n ad_B2A = adjoint_from_transform(invert_transform(mean_A2B))\n cov_B_in_A = np.dot(ad_B2A, np.dot(cov_B, ad_B2A.T))\n cov_A_total = cov_B_in_A + cov_A\n\n return mean_A2C, cov_A_total\n" }, { "path": "pytransform3d/uncertainty/_composition.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef concat_globally_uncertain_transforms(\n mean_A2B: npt.ArrayLike,\n cov_A2B: npt.ArrayLike,\n mean_B2C: npt.ArrayLike,\n cov_B2C: npt.ArrayLike,\n) -> np.ndarray: ...\ndef concat_locally_uncertain_transforms(\n mean_A2B: npt.ArrayLike,\n mean_B2C: npt.ArrayLike,\n cov_A: npt.ArrayLike,\n cov_B: npt.ArrayLike,\n) -> np.ndarray: ...\n" }, { "path": "pytransform3d/uncertainty/_frechet_mean.py", "content": "\"\"\"Frechet mean and related functions.\"\"\"\n\nimport numpy as np\n\nfrom ..batch_rotations import (\n axis_angles_from_matrices,\n matrices_from_compact_axis_angles,\n)\nfrom ..trajectories import (\n concat_many_to_one,\n exponential_coordinates_from_transforms,\n transforms_from_exponential_coordinates,\n)\nfrom ..transformations import (\n concat,\n invert_transform,\n)\n\n\ndef frechet_mean(\n samples,\n mean0,\n exp,\n log,\n inv,\n concat_one_to_one,\n concat_many_to_one,\n n_iter=20,\n):\n r\"\"\"Compute the Fréchet mean of samples on a smooth Riemannian manifold.\n\n The mean is computed with an iterative optimization algorithm [1]_ [2]_:\n\n 1. For a set of samples :math:`\\{x_1, \\ldots, x_n\\}`, we initialize\n the estimated mean :math:`\\bar{x}_0`, e.g., as :math:`x_1`.\n 2. For a fixed number of steps :math:`K`, in each iteration :math:`k` we\n improve the estimation of the mean by\n\n a. Computing the distance of each sample to the current estimate of the\n mean in tangent space with\n :math:`d_{i,k} \\leftarrow \\log (x_i \\cdot \\bar{x}_k^{-1})`.\n b. Updating the estimate of the mean with\n :math:`\\bar{x}_{k+1} \\leftarrow\n \\exp(\\frac{1}{N}\\sum_i d_{i,k}) \\cdot \\bar{x}_k`.\n\n 3. Return :math:`\\bar{x}_K`.\n\n Parameters\n ----------\n samples : array-like, shape (n_samples, ...)\n Samples on a smooth Riemannian manifold.\n\n mean0 : array-like, shape (...)\n Initial guess for the mean on the manifold.\n\n exp : callable\n Exponential map from the tangent space to the manifold.\n\n log : callable\n Logarithmic map from the manifold to the tangent space.\n\n inv : callable\n Computes the inverse of an element on the manifold.\n\n concat_one_to_one : callable\n Concatenates elements on the manifold.\n\n concat_many_to_one : callable\n Concatenates multiple elements on the manifold to one element on the\n manifold.\n\n n_iter : int, optional (default: 20)\n Number of iterations of the optimization algorithm.\n\n Returns\n -------\n mean : array, shape (...)\n Fréchet mean on the manifold.\n\n mean_diffs : array, shape (n_samples, n_tangent_space_components)\n Differences between the mean and the samples in the tangent space.\n These can be used to compute the covariance. They are returned to\n avoid recomputing them.\n\n See Also\n --------\n estimate_gaussian_rotation_matrix_from_samples\n Uses the Frechet mean to compute the mean of a Gaussian distribution\n over rotations.\n\n estimate_gaussian_transform_from_samples\n Uses the Frechet mean to compute the mean of a Gaussian distribution\n over transformations.\n\n References\n ----------\n .. [1] Fréchet, M. (1948). Les éléments aléatoires de nature quelconque\n dans un espace distancié. Annales de l’Institut Henri Poincaré, 10(3),\n 215–310.\n\n .. [2] Pennec, X. (2006). Intrinsic Statistics on Riemannian Manifolds:\n Basic Tools for Geometric Measurements. J Math Imaging Vis 25, 127-154.\n https://doi.org/10.1007/s10851-006-6228-4\n \"\"\"\n assert len(samples) > 0\n samples = np.asarray(samples)\n mean = np.copy(mean0)\n for _ in range(n_iter):\n mean_diffs = log(concat_many_to_one(samples, inv(mean)))\n avg_mean_diff = np.mean(mean_diffs, axis=0)\n mean = concat_one_to_one(mean, exp(avg_mean_diff))\n return mean, mean_diffs\n\n\ndef estimate_gaussian_rotation_matrix_from_samples(samples):\n \"\"\"Estimate Gaussian distribution over rotations from samples.\n\n Computes the Fréchet mean of the samples and the covariance in tangent\n space (exponential coordinates of rotation / rotation vectors) using an\n unbiased estimator as outlines by Eade [1]_.\n\n Parameters\n ----------\n samples : array-like, shape (n_samples, 3, 3)\n Sampled rotations represented by rotation matrices.\n\n Returns\n -------\n mean : array, shape (3, 3)\n Mean of the Gaussian distribution as rotation matrix.\n\n cov : array, shape (3, 3)\n Covariance of the Gaussian distribution in exponential coordinates.\n\n See Also\n --------\n frechet_mean\n Algorithm used to compute the mean of the Gaussian.\n\n References\n ----------\n .. [1] Eade, E. (2017). Lie Groups for 2D and 3D Transformations.\n https://ethaneade.com/lie.pdf\n \"\"\"\n\n def compact_axis_angles_from_matrices(Rs):\n A = axis_angles_from_matrices(Rs)\n return A[:, :3] * A[:, 3, np.newaxis]\n\n mean, mean_diffs = frechet_mean(\n samples=samples,\n mean0=samples[0],\n exp=matrices_from_compact_axis_angles,\n log=compact_axis_angles_from_matrices,\n inv=lambda R: R.T,\n concat_one_to_one=lambda R1, R2: np.dot(R2, R1),\n concat_many_to_one=concat_many_to_one,\n n_iter=20,\n )\n\n cov = np.cov(mean_diffs, rowvar=False, bias=True)\n return mean, cov\n\n\ndef estimate_gaussian_transform_from_samples(samples):\n \"\"\"Estimate Gaussian distribution over transformations from samples.\n\n Computes the Fréchet mean of the samples and the covariance in tangent\n space (exponential coordinates of transformation) using an unbiased\n estimator as outlines by Eade [1]_.\n\n Parameters\n ----------\n samples : array-like, shape (n_samples, 4, 4)\n Sampled transformations represented by homogeneous matrices.\n\n Returns\n -------\n mean : array, shape (4, 4)\n Mean as homogeneous transformation matrix.\n\n cov : array, shape (6, 6)\n Covariance of distribution in exponential coordinate space.\n\n See Also\n --------\n frechet_mean\n Algorithm used to compute the mean of the Gaussian.\n\n References\n ----------\n .. [1] Eade, E. (2017). Lie Groups for 2D and 3D Transformations.\n https://ethaneade.com/lie.pdf\n \"\"\"\n mean, mean_diffs = frechet_mean(\n samples=samples,\n mean0=samples[0],\n exp=transforms_from_exponential_coordinates,\n log=exponential_coordinates_from_transforms,\n inv=invert_transform,\n concat_one_to_one=concat,\n concat_many_to_one=concat_many_to_one,\n n_iter=20,\n )\n\n cov = np.cov(mean_diffs, rowvar=False, bias=True)\n return mean, cov\n" }, { "path": "pytransform3d/uncertainty/_frechet_mean.pyi", "content": "import numpy as np\nimport numpy.typing as npt\nfrom typing import Tuple, Callable\n\ndef frechet_mean(\n samples: npt.ArrayLike,\n mean0: npt.ArrayLike,\n exp: Callable,\n log: Callable,\n inv: Callable,\n concat_one_to_one: Callable,\n concat_many_to_one: Callable,\n n_iter: int = ...,\n) -> Tuple[np.ndarray, np.ndarray]: ...\ndef estimate_gaussian_rotation_matrix_from_samples(\n samples: npt.ArrayLike,\n) -> Tuple[np.ndarray, np.ndarray]: ...\ndef estimate_gaussian_transform_from_samples(\n samples: npt.ArrayLike,\n) -> Tuple[np.ndarray, np.ndarray]: ...\n" }, { "path": "pytransform3d/uncertainty/_fusion.py", "content": "\"\"\"Fusion of poses.\"\"\"\n\nimport numpy as np\n\nfrom ..transformations import (\n exponential_coordinates_from_transform,\n invert_transform,\n left_jacobian_SE3_inv,\n transform_from_exponential_coordinates,\n)\n\n\ndef pose_fusion(means, covs):\n \"\"\"Fuse Gaussian distributions of multiple poses.\n\n Parameters\n ----------\n means : array-like, shape (n_poses, 4, 4)\n Homogeneous transformation matrices.\n\n covs : array-like, shape (n_poses, 6, 6)\n Covariances of pose distributions in exponential coordinate space.\n\n Returns\n -------\n mean : array, shape (4, 4)\n Fused pose mean.\n\n cov : array, shape (6, 6)\n Fused pose covariance.\n\n V : float\n Error of optimization objective.\n\n References\n ----------\n .. [1] Barfoot, T. D., Furgale, P. T. (2014).\n Associating Uncertainty With Three-Dimensional Poses for Use in\n Estimation Problems. IEEE Transactions on Robotics, 30(3), pp. 679-693,\n doi: 10.1109/TRO.2014.2298059.\n \"\"\"\n n_poses = len(means)\n covs_inv = [np.linalg.inv(cov) for cov in covs]\n\n mean = np.eye(4)\n LHS = np.empty((6, 6))\n RHS = np.empty(6)\n for _ in range(20):\n LHS[:, :] = 0.0\n RHS[:] = 0.0\n for k in range(n_poses):\n x_ik = exponential_coordinates_from_transform(\n np.dot(mean, invert_transform(means[k]))\n )\n J_inv = left_jacobian_SE3_inv(x_ik)\n J_invT_S = np.dot(J_inv.T, covs_inv[k])\n LHS += np.dot(J_invT_S, J_inv)\n RHS += np.dot(J_invT_S, x_ik)\n x_i = np.linalg.solve(-LHS, RHS)\n mean = np.dot(transform_from_exponential_coordinates(x_i), mean)\n\n cov = np.linalg.inv(LHS)\n\n V = 0.0\n for k in range(n_poses):\n x_ik = exponential_coordinates_from_transform(\n np.dot(mean, invert_transform(means[k]))\n )\n V += 0.5 * np.dot(x_ik, np.dot(covs_inv[k], x_ik))\n return mean, cov, V\n" }, { "path": "pytransform3d/uncertainty/_fusion.pyi", "content": "import numpy as np\nimport numpy.typing as npt\n\ndef pose_fusion(means: npt.ArrayLike, covs: npt.ArrayLike) -> np.ndarray: ...\n" }, { "path": "pytransform3d/uncertainty/_invert.py", "content": "\"\"\"Inversion of uncertain transformations.\"\"\"\n\nimport numpy as np\n\nfrom ..transformations import (\n adjoint_from_transform,\n invert_transform,\n)\n\n\ndef invert_uncertain_transform(mean, cov):\n r\"\"\"Invert uncertain transform.\n\n For the mean :math:`\\boldsymbol{T}_{BA}`, the inverse is simply\n :math:`\\boldsymbol{T}_{BA}^{-1} = \\boldsymbol{T}_{AB}`.\n\n For the covariance, we need the adjoint of the inverse transformation\n :math:`\\left[Ad_{\\boldsymbol{T}_{BA}^{-1}}\\right]`:\n\n .. math::\n\n \\boldsymbol{\\Sigma}_{\\boldsymbol{T}_{AB}}\n =\n \\left[Ad_{\\boldsymbol{T}_{BA}^{-1}}\\right]\n \\boldsymbol{\\Sigma}_{\\boldsymbol{T}_{BA}}\n \\left[Ad_{\\boldsymbol{T}_{BA}^{-1}}\\right]^T\n\n Parameters\n ----------\n mean : array-like, shape (4, 4)\n Mean of transform from frame A to frame B.\n\n cov : array, shape (6, 6)\n Covariance of transform from frame A to frame B in exponential\n coordinate space.\n\n Returns\n -------\n mean_inv : array, shape (4, 4)\n Mean of transform from frame B to frame A.\n\n cov_inv : array, shape (6, 6)\n Covariance of transform from frame B to frame A in exponential\n coordinate space.\n\n See Also\n --------\n pytransform3d.transformations.invert_transform :\n Invert transformation without uncertainty.\n\n References\n ----------\n .. [1] Mangelson, G., Vasudevan, E. (2019). Characterizing the Uncertainty\n of Jointly Distributed Poses in the Lie Algebra,\n https://arxiv.org/pdf/1906.07795.pdf\n \"\"\"\n mean_inv = invert_transform(mean)\n ad_inv = adjoint_from_transform(mean_inv)\n cov_inv = np.dot(ad_inv, np.dot(cov, ad_inv.T))\n return mean_inv, cov_inv\n" }, { "path": "pytransform3d/uncertainty/_invert.pyi", "content": "from typing import Tuple\n\nimport numpy as np\nimport numpy.typing as npt\n\ndef invert_uncertain_transform(\n mean: npt.ArrayLike, cov: npt.ArrayLike\n) -> Tuple[np.ndarray, np.ndarray]: ...\n" }, { "path": "pytransform3d/uncertainty/_plot.py", "content": "\"\"\"Plotting of uncertain transformations.\"\"\"\n\nimport numpy as np\n\nfrom .._geometry import unit_sphere_surface_grid\nfrom ..trajectories import (\n transforms_from_exponential_coordinates,\n)\nfrom ..transformations import (\n transform_from,\n)\n\n\ndef to_ellipsoid(mean, cov):\n \"\"\"Compute error ellipsoid.\n\n An error ellipsoid indicates the equiprobable surface. The resulting\n ellipsoid includes one standard deviation of the data along each main\n axis, which covers approximately 68.27% of the data. Multiplying the\n radii with factors > 1 will increase the coverage. The usual factors\n for Gaussian distributions apply:\n\n * 1 - 68.27%\n * 1.65 - 90%\n * 1.96 - 95%\n * 2 - 95.45%\n * 2.58 - 99%\n * 3 - 99.73%\n\n Parameters\n ----------\n mean : array-like, shape (3,)\n Mean of distribution.\n\n cov : array-like, shape (3, 3)\n Covariance of distribution.\n\n Returns\n -------\n ellipsoid2origin : array, shape (4, 4)\n Ellipsoid frame in world frame. Note that there are multiple solutions\n possible for the orientation because an ellipsoid is symmetric.\n A body-fixed rotation around a main axis by 180 degree results in the\n same ellipsoid.\n\n radii : array, shape (3,)\n Radii of ellipsoid, coinciding with standard deviations along the\n three axes of the ellipsoid. These are sorted in ascending order.\n \"\"\"\n from scipy import linalg\n\n radii, R = linalg.eigh(cov)\n if np.linalg.det(R) < 0: # undo reflection (exploit symmetry)\n R *= -1\n ellipsoid2origin = transform_from(R=R, p=mean)\n return ellipsoid2origin, np.sqrt(np.abs(radii))\n\n\ndef to_projected_ellipsoid(mean, cov, factor=1.96, n_steps=20):\n \"\"\"Compute projected error ellipsoid.\n\n An error ellipsoid shows equiprobable points. This is a projection of a\n Gaussian distribution in exponential coordinate space to 3D.\n\n Parameters\n ----------\n mean : array-like, shape (4, 4)\n Mean of pose distribution.\n\n cov : array-like, shape (6, 6)\n Covariance of pose distribution in exponential coordinate space.\n\n factor : float, optional (default: 1.96)\n Multiple of the standard deviations that should be plotted.\n\n n_steps : int, optional (default: 20)\n Number of discrete steps plotted in each dimension.\n\n Returns\n -------\n x : array, shape (n_steps, n_steps)\n Coordinates on x-axis of grid on projected ellipsoid.\n\n y : array, shape (n_steps, n_steps)\n Coordinates on y-axis of grid on projected ellipsoid.\n\n z : array, shape (n_steps, n_steps)\n Coordinates on z-axis of grid on projected ellipsoid.\n \"\"\"\n from scipy import linalg\n\n vals, vecs = linalg.eigh(cov)\n order = vals.argsort()[::-1]\n vals, vecs = vals[order], vecs[:, order]\n\n radii = factor * np.sqrt(vals[:3])\n\n # Grid on ellipsoid in exponential coordinate space\n x, y, z = unit_sphere_surface_grid(n_steps)\n x *= radii[0]\n y *= radii[1]\n z *= radii[2]\n P = np.column_stack((x.reshape(-1), y.reshape(-1), z.reshape(-1)))\n P = np.dot(P, vecs[:, :3].T)\n\n # Grid in Cartesian space\n T_diff = transforms_from_exponential_coordinates(P)\n # same as T_diff[m, :3, :3].T.dot(T_diff[m, :3, 3]) for each m\n P = np.einsum(\"ikj,ik->ij\", T_diff[:, :3, :3], T_diff[:, :3, 3])\n P = (np.dot(P, mean[:3, :3].T) + mean[np.newaxis, :3, 3]).T\n\n shape = x.shape\n x = P[0].reshape(*shape)\n y = P[1].reshape(*shape)\n z = P[2].reshape(*shape)\n\n return x, y, z\n\n\ndef plot_projected_ellipsoid(\n ax,\n mean,\n cov,\n factor=1.96,\n wireframe=True,\n n_steps=20,\n color=None,\n alpha=1.0,\n linewidth=1.0,\n): # pragma: no cover\n \"\"\"Plots projected equiprobable ellipsoid in 3D.\n\n An error ellipsoid shows equiprobable points. This is a projection of a\n Gaussian distribution in exponential coordinate space to 3D.\n\n Parameters\n ----------\n ax : axis\n Matplotlib axis.\n\n mean : array-like, shape (4, 4)\n Mean pose.\n\n cov : array-like, shape (6, 6)\n Covariance in exponential coordinate space.\n\n factor : float, optional (default: 1.96)\n Multiple of the standard deviations that should be plotted.\n\n wireframe : bool, optional (default: True)\n Plot wireframe of ellipsoid and surface otherwise.\n\n n_steps : int, optional (default: 20)\n Number of discrete steps plotted in each dimension.\n\n color : str, optional (default: None)\n Color in which the equiprobably lines should be plotted.\n\n alpha : float, optional (default: 1.0)\n Alpha value for lines.\n\n linewidth : float, optional (default: 1.0)\n Line width for wireframe plot.\n\n Returns\n -------\n ax : axis\n Matplotlib axis.\n \"\"\"\n x, y, z = to_projected_ellipsoid(mean, cov, factor, n_steps)\n\n if wireframe:\n ax.plot_wireframe(\n x,\n y,\n z,\n rstride=2,\n cstride=2,\n color=color,\n alpha=alpha,\n linewidth=linewidth,\n )\n else:\n ax.plot_surface(x, y, z, color=color, alpha=alpha, linewidth=0)\n\n return ax\n" }, { "path": "pytransform3d/uncertainty/_plot.pyi", "content": "import numpy as np\nimport numpy.typing as npt\nfrom typing import Tuple, Union\nfrom mpl_toolkits.mplot3d import Axes3D\n\ndef to_ellipsoid(\n mean: npt.ArrayLike, cov: npt.ArrayLike\n) -> Tuple[np.ndarray, np.ndarray]: ...\ndef to_projected_ellipsoid(\n mean: npt.ArrayLike,\n cov: npt.ArrayLike,\n factor: float = ...,\n n_steps: int = ...,\n) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: ...\ndef plot_projected_ellipsoid(\n ax: Union[None, Axes3D],\n mean: npt.ArrayLike,\n cov: npt.ArrayLike,\n factor: float = ...,\n wireframe: bool = ...,\n n_steps: int = ...,\n color: str = ...,\n alpha: float = ...,\n linewidth: float = ...,\n) -> Axes3D: ...\n" }, { "path": "pytransform3d/uncertainty/test/test_uncertainty.py", "content": "import numpy as np\nimport pytest\nfrom numpy.testing import assert_array_almost_equal\n\nimport pytransform3d.rotations as pr\nimport pytransform3d.transformations as pt\nimport pytransform3d.uncertainty as pu\n\n\ndef test_estimate_gaussian_rotation_matrix_from_samples():\n rng = np.random.default_rng(34)\n mean = pr.matrix_from_axis_angle([0.3, 0.2, 0.5, 0.25 * np.pi])\n cov = np.diag([0.1, 0.05, 0.09]) ** 2\n samples = np.array([pr.random_matrix(rng, mean, cov) for _ in range(1000)])\n mean_est, cov_est = pu.estimate_gaussian_rotation_matrix_from_samples(\n samples\n )\n assert_array_almost_equal(mean, mean_est, decimal=2)\n assert_array_almost_equal(cov, cov_est, decimal=2)\n\n\ndef test_same_fuse_poses():\n mean1 = np.array(\n [\n [0.8573, -0.2854, 0.4285, 3.5368],\n [-0.1113, 0.7098, 0.6956, -3.5165],\n [-0.5026, -0.6440, 0.5767, -0.9112],\n [0.0, 0.0, 0.0, 1.0000],\n ]\n )\n mean1[:3, :3] = pr.norm_matrix(mean1[:3, :3])\n mean2 = np.array(\n [\n [0.5441, -0.6105, 0.5755, -1.0935],\n [0.8276, 0.5032, -0.2487, 5.5992],\n [-0.1377, 0.6116, 0.7791, 0.2690],\n [0.0, 0.0, 0.0, 1.0000],\n ]\n )\n mean2[:3, :3] = pr.norm_matrix(mean2[:3, :3])\n mean3 = np.array(\n [\n [-0.0211, -0.7869, 0.6167, -3.0968],\n [-0.2293, 0.6042, 0.7631, 2.0868],\n [-0.9731, -0.1254, -0.1932, 2.0239],\n [0.0, 0.0, 0.0, 1.0000],\n ]\n )\n mean3[:3, :3] = pr.norm_matrix(mean3[:3, :3])\n alpha = 5.0\n cov1 = alpha * np.diag([0.1, 0.2, 0.1, 2.0, 1.0, 1.0])\n cov2 = alpha * np.diag([0.1, 0.1, 0.2, 1.0, 3.0, 1.0])\n cov3 = alpha * np.diag([0.2, 0.1, 0.1, 1.0, 1.0, 5.0])\n means = [mean1, mean2, mean3]\n covs = [cov1, cov2, cov3]\n mean_est, cov_est, V = pu.pose_fusion(means, covs)\n mean_exp = np.array(\n [\n [0.2967, -0.7157, 0.6323, -1.4887],\n [0.5338, 0.6733, 0.5116, 0.9935],\n [-0.7918, 0.1857, 0.5818, -2.7035],\n [0.0, 0.0, 0.0, 1.0000],\n ]\n )\n cov_exp = np.array(\n [\n [\n 0.14907707,\n -0.01935277,\n -0.0107348,\n -0.02442925,\n -0.09843835,\n 0.0054134,\n ],\n [\n -0.01935277,\n 0.14648459,\n 0.02055571,\n 0.11121064,\n 0.06272014,\n -0.08553834,\n ],\n [\n -0.0107348,\n 0.02055571,\n 0.15260209,\n -0.07451066,\n 0.06531188,\n -0.01890897,\n ],\n [\n -0.02442925,\n 0.11121064,\n -0.07451066,\n 2.10256906,\n 0.13695598,\n -0.29705468,\n ],\n [\n -0.09843835,\n 0.06272014,\n 0.06531188,\n 0.13695598,\n 2.29286157,\n -0.58004,\n ],\n [\n 0.0054134,\n -0.08553834,\n -0.01890897,\n -0.29705468,\n -0.58004,\n 2.34528443,\n ],\n ]\n )\n assert_array_almost_equal(mean_exp, mean_est, decimal=4)\n assert_array_almost_equal(cov_exp, cov_est)\n assert pytest.approx(V, abs=1e-4) == 4.6537\n\n\ndef test_invert_pose():\n rng = np.random.default_rng(2)\n\n for _ in range(5):\n T = pt.random_transform(rng)\n cov = np.diag(rng.random((6, 6)))\n\n T_inv, cov_inv = pu.invert_uncertain_transform(T, cov)\n\n T2, cov2 = pu.invert_uncertain_transform(T_inv, cov_inv)\n\n assert_array_almost_equal(T, T2)\n assert_array_almost_equal(cov, cov2)\n\n\ndef test_sample_estimate_gaussian():\n rng = np.random.default_rng(2000)\n mean = pt.transform_from(R=np.eye(3), p=np.array([0.0, 0.0, 0.5]))\n cov = np.diag([0.001, 0.001, 0.5, 0.001, 0.001, 0.001])\n samples = np.array(\n [pt.random_transform(rng, mean, cov) for _ in range(1000)]\n )\n mean_est, cov_est = pu.estimate_gaussian_transform_from_samples(samples)\n assert_array_almost_equal(mean, mean_est, decimal=2)\n assert_array_almost_equal(cov, cov_est, decimal=2)\n\n\ndef test_concat_globally_uncertain_transforms():\n cov_pose_chol = np.diag([0, 0, 0.03, 0, 0, 0])\n cov_pose = np.dot(cov_pose_chol, cov_pose_chol.T)\n velocity_vector = np.array([0, 0, 0, 1.0, 0, 0])\n T_vel = pt.transform_from_exponential_coordinates(velocity_vector)\n n_steps = 100\n\n T_est = np.eye(4)\n cov_est = np.zeros((6, 6))\n for _ in range(n_steps):\n T_est, cov_est = pu.concat_globally_uncertain_transforms(\n T_est, cov_est, T_vel, cov_pose\n )\n assert_array_almost_equal(\n T_est,\n np.array(\n [[1, 0, 0, n_steps], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]\n ),\n )\n # achievable with second-order terms\n assert cov_est[2, 2] > 0\n assert cov_est[4, 4] > 0\n assert cov_est[2, 4] != 0\n assert cov_est[4, 2] != 0\n # achievable only with fourth-order terms\n assert cov_est[3, 3] > 0\n assert cov_est[4, 2] != 0\n assert cov_est[2, 4] != 0\n\n\ndef test_concat_locally_uncertain_transforms():\n mean_A2B = pt.transform_from(\n R=pr.matrix_from_euler([0.5, 0.0, 0.0], 0, 1, 2, True),\n p=np.array([1.0, 0.0, 0.0]),\n )\n mean_B2C = pt.transform_from(\n R=pr.matrix_from_euler([0.0, 0.5, 0.0], 0, 1, 2, True),\n p=np.array([0.0, 1.0, 0.0]),\n )\n cov_A = np.diag([1.0, 0, 0, 0, 0, 0])\n cov_B = np.diag([0, 1.0, 0, 0, 0, 0])\n\n mean_A2C, cov_A_total = pu.concat_locally_uncertain_transforms(\n mean_A2B, mean_B2C, cov_A, np.zeros((6, 6))\n )\n assert_array_almost_equal(mean_A2C, pt.concat(mean_A2B, mean_B2C))\n assert_array_almost_equal(cov_A_total, cov_A)\n\n mean_A2C, cov_A_total = pu.concat_locally_uncertain_transforms(\n np.eye(4), mean_B2C, np.zeros((6, 6)), cov_B\n )\n assert_array_almost_equal(cov_A_total, cov_B)\n\n mean_A2C, cov_A_total = pu.concat_locally_uncertain_transforms(\n mean_A2B, mean_B2C, np.zeros((6, 6)), cov_B\n )\n ad_B2A = pt.adjoint_from_transform(pt.invert_transform(mean_A2B))\n assert_array_almost_equal(cov_A_total, ad_B2A.dot(cov_B).dot(ad_B2A.T))\n\n\ndef test_to_ellipsoid():\n mean = np.array([0.1, 0.2, 0.3])\n cov = np.array([[25.0, 0.0, 0.0], [0.0, 4.0, 0.0], [0.0, 0.0, 9.0]])\n ellipsoid2origin, radii = pu.to_ellipsoid(mean, cov)\n assert_array_almost_equal(ellipsoid2origin[:3, 3], mean)\n assert_array_almost_equal(\n ellipsoid2origin[:3, :3], np.array([[0, 0, 1], [1, 0, 0], [0, 1, 0]])\n )\n assert_array_almost_equal(radii, np.array([2.0, 3.0, 5.0]))\n\n rng = np.random.default_rng(28)\n for _ in range(5):\n R = pr.matrix_from_axis_angle(pr.random_axis_angle(rng))\n std_devs = np.array([1.0, 2.0, 3.0])\n cov = np.dot(R, np.dot(np.diag(std_devs**2), R.T))\n ellipsoid2origin, radii = pu.to_ellipsoid(mean, cov)\n assert_array_almost_equal(ellipsoid2origin[:3, 3], mean)\n # multiple solutions for the rotation matrix are possible because of\n # ellipsoid symmetries, hence, we only check the absolute values\n assert_array_almost_equal(np.abs(ellipsoid2origin[:3, :3]), np.abs(R))\n assert_array_almost_equal(radii, std_devs)\n\n\ndef test_projected_ellipsoid():\n mean = np.eye(4)\n cov = np.eye(6)\n x, y, z = pu.to_projected_ellipsoid(mean, cov, factor=1.0, n_steps=20)\n P = np.column_stack((x.reshape(-1), y.reshape(-1), z.reshape(-1)))\n r = np.linalg.norm(P, axis=1)\n assert_array_almost_equal(r, np.ones_like(3))\n\n pos_mean = np.array([0.5, -5.3, 10.5])\n mean = pt.transform_from(R=np.eye(3), p=pos_mean)\n x, y, z = pu.to_projected_ellipsoid(mean, cov, factor=1.0, n_steps=20)\n P = np.column_stack((x.reshape(-1), y.reshape(-1), z.reshape(-1)))\n r = np.linalg.norm(P - pos_mean[np.newaxis], axis=1)\n assert_array_almost_equal(r, np.ones_like(3))\n\n mean = np.eye(4)\n cov = np.diag([1, 1, 1, 4, 1, 1])\n x, y, z = pu.to_projected_ellipsoid(mean, cov, factor=1.0, n_steps=20)\n P = np.column_stack((x.reshape(-1), y.reshape(-1), z.reshape(-1)))\n r = np.linalg.norm(P, axis=1)\n assert np.all(r >= 1.0)\n assert np.all(r <= 2.0)\n" }, { "path": "pytransform3d/urdf.py", "content": "\"\"\"Load transformations from URDF files.\n\nSee :doc:`user_guide/transform_manager` for more information.\n\"\"\"\n\nimport os\nimport re\nimport warnings\n\nimport numpy as np\nfrom lxml import etree\n\nfrom .rotations import (\n matrix_from_euler,\n matrix_from_axis_angle,\n norm_vector,\n)\nfrom .transform_manager import TransformManager\nfrom .transformations import transform_from, concat\n\n\nclass UrdfTransformManager(TransformManager):\n \"\"\"Transformation manager that can load URDF files.\n\n The Unified Robot Description Format (URDF) [1]_ is the most common format\n to describe kinematic and dynamic properties of robots [2]_. This\n transformation manager allows to set the joint configurations of a robot\n loaded from a URDF file and provides an interface to its geometric and\n visual representation.\n\n .. warning::\n\n Note that this module requires the Python package lxml.\n\n .. note::\n\n Joint angles must be given in radians.\n\n Parameters\n ----------\n strict_check : bool, optional (default: True)\n Raise a ValueError if the transformation matrix is not numerically\n close enough to a real transformation matrix. Otherwise we print a\n warning.\n\n check : bool, optional (default: True)\n Check if transformation matrices are valid and requested nodes exist,\n which might significantly slow down some operations.\n\n References\n ----------\n .. [1] ROS Wiki: urdf, http://wiki.ros.org/urdf\n\n .. [2] Tola, D., Corke, P. (2023). Understanding URDF: A Dataset and\n Analysis. In IEEE Robotics and Automation Letters 9(5), pp. 4479-4486,\n doi: 10.1109/LRA.2024.3381482. https://arxiv.org/abs/2308.00514\n \"\"\"\n\n def __init__(self, strict_check=True, check=True):\n super(UrdfTransformManager, self).__init__(strict_check, check)\n self._joints = {}\n self.collision_objects = []\n self.visuals = []\n\n def add_joint(\n self,\n joint_name,\n from_frame,\n to_frame,\n child2parent,\n axis,\n limits=(float(\"-inf\"), float(\"inf\")),\n joint_type=\"revolute\",\n ):\n \"\"\"Add joint.\n\n Parameters\n ----------\n joint_name : str\n Name of the joint\n\n from_frame : Hashable\n Child link of the joint\n\n to_frame : Hashable\n Parent link of the joint\n\n child2parent : array-like, shape (4, 4)\n Transformation from child to parent\n\n axis : array-like, shape (3,)\n Rotation axis of the joint (defined in the child frame)\n\n limits : pair of float, optional (default: (-inf, inf))\n Lower and upper joint angle limit\n\n joint_type : str, optional (default: 'revolute')\n Joint type: revolute, prismatic, or fixed (continuous is the same\n as revolute)\n \"\"\"\n self.add_transform(from_frame, to_frame, child2parent)\n self._joints[joint_name] = (\n from_frame,\n to_frame,\n child2parent,\n norm_vector(axis),\n limits,\n joint_type,\n )\n\n def set_joint(self, joint_name, value):\n \"\"\"Set joint position.\n\n Note that joint values are clipped to their limits.\n\n Parameters\n ----------\n joint_name : str\n Name of the joint\n\n value : float\n Joint angle in radians in case of revolute joints or position\n in case of prismatic joint.\n\n Raises\n ------\n KeyError\n If joint_name is unknown\n \"\"\"\n if joint_name not in self._joints:\n raise KeyError(\"Joint '%s' is not known\" % joint_name)\n from_frame, to_frame, child2parent, axis, limits, joint_type = (\n self._joints[joint_name]\n )\n # this is way faster than np.clip:\n value = min(max(value, limits[0]), limits[1])\n if joint_type == \"revolute\":\n joint_rotation = matrix_from_axis_angle(np.hstack((axis, (value,))))\n joint2A = transform_from(\n joint_rotation, np.zeros(3), strict_check=self.strict_check\n )\n elif joint_type == \"prismatic\":\n joint_offset = value * axis\n joint2A = transform_from(\n np.eye(3), joint_offset, strict_check=self.strict_check\n )\n else:\n assert joint_type == \"fixed\"\n warnings.warn(\n \"Trying to set a fixed joint\", UserWarning, stacklevel=2\n )\n return\n self.add_transform(\n from_frame,\n to_frame,\n concat(\n joint2A,\n child2parent,\n strict_check=self.strict_check,\n check=self.check,\n ),\n )\n\n def get_joint_limits(self, joint_name):\n \"\"\"Get limits of a joint.\n\n Parameters\n ----------\n joint_name : str\n Name of the joint\n\n Returns\n -------\n limits : pair of float\n Lower and upper joint angle limit\n\n Raises\n ------\n KeyError\n If joint_name is unknown\n \"\"\"\n if joint_name not in self._joints:\n raise KeyError(\"Joint '%s' is not known\" % joint_name)\n return self._joints[joint_name][4]\n\n def load_urdf(self, urdf_xml, mesh_path=None, package_dir=None):\n \"\"\"Load URDF file into transformation manager.\n\n Parameters\n ----------\n urdf_xml : str\n Robot definition in URDF\n\n mesh_path : str, optional (default: None)\n Path in which we search for meshes that are defined in the URDF.\n Meshes will be ignored if it is set to None and no 'package_dir'\n is given.\n\n package_dir : str, optional (default: None)\n Some URDFs start file names with 'package://' to refer to the ROS\n package in which these files (textures, meshes) are located. This\n variable defines to which path this prefix will be resolved.\n \"\"\"\n robot_name, links, joints = parse_urdf(\n urdf_xml, mesh_path, package_dir, self.strict_check\n )\n initialize_urdf_transform_manager(self, robot_name, links, joints)\n\n def plot_visuals(\n self,\n frame,\n ax=None,\n ax_s=1,\n wireframe=False,\n convex_hull_of_mesh=True,\n alpha=0.3,\n ): # pragma: no cover\n \"\"\"Plot all visuals in a given reference frame.\n\n Visuals can be boxes, spheres, cylinders, or meshes. Note that visuals\n that cannot be connected to the reference frame are omitted.\n\n Parameters\n ----------\n frame : Hashable\n Reference frame\n\n ax : Matplotlib 3d axis, optional (default: None)\n If the axis is None, a new 3d axis will be created\n\n ax_s : float, optional (default: 1)\n Scaling of the new matplotlib 3d axis\n\n wireframe : bool, optional (default: False)\n Plot wireframe (surface otherwise)\n\n convex_hull_of_mesh : bool, optional (default: True)\n Displays convex hull of meshes instead of the original mesh. This\n makes plotting a lot faster with complex meshes.\n\n alpha : float, optional (default: 0.3)\n Alpha value of the surface / wireframe that will be plotted\n\n Returns\n -------\n ax : Matplotlib 3d axis\n New or old axis\n \"\"\"\n return self._plot_objects(\n self.visuals, frame, ax, ax_s, wireframe, convex_hull_of_mesh, alpha\n )\n\n def plot_collision_objects(\n self,\n frame,\n ax=None,\n ax_s=1,\n wireframe=True,\n convex_hull_of_mesh=True,\n alpha=1.0,\n ): # pragma: no cover\n \"\"\"Plot all collision objects in a given reference frame.\n\n Collision objects can be boxes, spheres, cylinders, or meshes. Note\n that collision objects that cannot be connected to the reference frame\n are omitted.\n\n Parameters\n ----------\n frame : Hashable\n Reference frame\n\n ax : Matplotlib 3d axis, optional (default: None)\n If the axis is None, a new 3d axis will be created\n\n ax_s : float, optional (default: 1)\n Scaling of the new matplotlib 3d axis\n\n wireframe : bool, optional (default: True)\n Plot wireframe (surface otherwise)\n\n convex_hull_of_mesh : bool, optional (default: True)\n Displays convex hull of meshes instead of the original mesh. This\n makes plotting a lot faster with complex meshes.\n\n alpha : float, optional (default: 1)\n Alpha value of the surface / wireframe that will be plotted\n\n Returns\n -------\n ax : Matplotlib 3d axis\n New or old axis\n \"\"\"\n return self._plot_objects(\n self.collision_objects,\n frame,\n ax,\n ax_s,\n wireframe,\n convex_hull_of_mesh,\n alpha,\n )\n\n def _plot_objects(\n self,\n objects,\n frame,\n ax=None,\n ax_s=1,\n wireframe=True,\n convex_hull_of_mesh=True,\n alpha=1.0,\n ): # pragma: no cover\n \"\"\"Plot all objects in a given reference frame.\n\n Objects can be boxes, spheres, cylinders, or meshes. Note that objects\n that cannot be connected to the reference frame are omitted.\n\n Parameters\n ----------\n objects : list\n Objects that will be plotted\n\n frame : Hashable\n Reference frame\n\n ax : Matplotlib 3d axis, optional (default: None)\n If the axis is None, a new 3d axis will be created\n\n ax_s : float, optional (default: 1)\n Scaling of the new matplotlib 3d axis\n\n wireframe : bool, optional (default: True)\n Plot wireframe (surface otherwise)\n\n convex_hull_of_mesh : bool, optional (default: True)\n Displays convex hull of meshes instead of the original mesh. This\n makes plotting a lot faster with complex meshes.\n\n alpha : float, optional (default: 1)\n Alpha value of the surface / wireframe that will be plotted\n\n Returns\n -------\n ax : Matplotlib 3d axis\n New or old axis\n \"\"\"\n if ax is None:\n from .plot_utils import make_3d_axis\n\n ax = make_3d_axis(ax_s)\n for obj in objects:\n ax = obj.plot(\n self,\n frame,\n ax,\n wireframe=wireframe,\n convex_hull=convex_hull_of_mesh,\n alpha=alpha,\n )\n return ax\n\n\ndef parse_urdf(urdf_xml, mesh_path=None, package_dir=None, strict_check=True):\n \"\"\"Parse information from URDF file.\n\n Parameters\n ----------\n urdf_xml : str\n Robot definition in URDF\n\n mesh_path : str, optional (default: None)\n Path in which we search for meshes that are defined in the URDF.\n Meshes will be ignored if it is set to None and no 'package_dir'\n is given.\n\n package_dir : str, optional (default: None)\n Some URDFs start file names with 'package://' to refer to the ROS\n package in which these files (textures, meshes) are located. This\n variable defines to which path this prefix will be resolved.\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the transformation matrix is not numerically\n close enough to a real transformation matrix. Otherwise we print a\n warning.\n\n Returns\n -------\n robot_name : str\n Name of the robot\n\n links : list of Link\n Links of the robot\n\n joints : list of Joint\n Joints of the robot\n\n Raises\n ------\n UrdfException\n If URDF is not valid\n \"\"\"\n # lxml does not allow whitespaces in the beginning\n urdf_xml = urdf_xml.strip()\n # lxml complains about unicode strings that start with unicode encoding\n # declaration. Hence, we have to convert them to bytes first.\n urdf_xml = bytes(urdf_xml.encode(\"utf-8\"))\n try:\n root = etree.XML(\n urdf_xml, parser=etree.XMLParser(recover=True, remove_comments=True)\n )\n except etree.XMLSyntaxError as e:\n raise UrdfException(\"Invalid XML.\") from e\n\n # URDF XML schema:\n # https://github.com/ros/urdfdom/blob/master/xsd/urdf.xsd\n\n # If there is an XML namespace (xmlns=...) specified, this will break\n # parsing as the root tag, and the tag of all elements underneath root,\n # will have a {namespace} prefix. As such, normalize the namespace to the\n # default (i.e. no namespace).\n\n _normalize_namespace(root)\n\n if root.tag != \"robot\":\n raise UrdfException(\"Robot tag is missing.\")\n\n tree = etree.ElementTree(root)\n\n if \"name\" not in root.attrib:\n raise UrdfException(\"Attribute 'name' is missing in robot tag.\")\n\n robot_name = root.attrib[\"name\"]\n\n materials = dict(\n [_parse_material(material) for material in tree.findall(\"material\")]\n )\n\n links = [\n _parse_link(link, materials, mesh_path, package_dir, strict_check)\n for link in tree.findall(\"link\")\n ]\n\n link_names = [link.name for link in links]\n joints = [\n _parse_joint(joint, link_names, strict_check)\n for joint in tree.findall(\"joint\")\n ]\n\n return robot_name, links, joints\n\n\ndef initialize_urdf_transform_manager(tm, robot_name, links, joints):\n \"\"\"Initializes transform manager from previously parsed URDF data.\n\n Parameters\n ----------\n tm : UrdfTransformManager\n Transform manager\n\n robot_name : str\n Name of the robot\n\n links : list of Link\n Links of the robot\n\n joints : list of Joint\n Joints of the robot\n \"\"\"\n tm.add_transform(links[0].name, robot_name, np.eye(4))\n _add_links(tm, links)\n _add_joints(tm, joints)\n\n\ndef _normalize_namespace(root):\n \"\"\"Normalizes the namespace of the root node and all elements underneath.\"\"\"\n root.tag = re.sub(r\"\\{.*?\\}\", \"\", root.tag)\n for element in root.iter():\n element.tag = re.sub(r\"\\{.*?\\}\", \"\", element.tag)\n\n\ndef _parse_material(material):\n \"\"\"Parse material.\"\"\"\n if \"name\" not in material.attrib:\n raise UrdfException(\"Material name is missing.\")\n colors = material.findall(\"color\")\n if len(colors) > 1:\n raise UrdfException(\"More than one color is not allowed.\")\n color = _parse_color(colors[0]) if len(colors) == 1 else None\n # TODO texture is currently ignored\n return material.attrib[\"name\"], color\n\n\ndef _parse_color(color):\n \"\"\"Parse color.\"\"\"\n if \"rgba\" not in color.attrib:\n raise UrdfException(\"Attribute 'rgba' of color tag is missing.\")\n return np.fromstring(color.attrib[\"rgba\"], sep=\" \")\n\n\ndef _parse_link(link, materials, mesh_path, package_dir, strict_check):\n \"\"\"Create link.\"\"\"\n if \"name\" not in link.attrib:\n raise UrdfException(\"Link name is missing.\")\n\n result = Link()\n result.name = link.attrib[\"name\"]\n\n visuals, visual_transforms = _parse_link_children(\n link, \"visual\", materials, mesh_path, package_dir, strict_check\n )\n result.visuals = visuals\n result.transforms.extend(visual_transforms)\n\n collision_objects, collision_object_transforms = _parse_link_children(\n link, \"collision\", dict(), mesh_path, package_dir, strict_check\n )\n result.collision_objects = collision_objects\n result.transforms.extend(collision_object_transforms)\n\n inertial = link.find(\"inertial\")\n if inertial is not None:\n result.inertial_frame[:, :] = _parse_origin(inertial, strict_check)\n result.mass = _parse_mass(inertial)\n result.inertia[:, :] = _parse_inertia(inertial)\n result.transforms.append(\n (\n \"inertial_frame:%s\" % result.name,\n result.name,\n result.inertial_frame,\n )\n )\n\n return result\n\n\ndef _parse_link_children(\n link, child_type, materials, mesh_path, package_dir, strict_check\n):\n \"\"\"Parse collision objects or visuals.\"\"\"\n children = link.findall(child_type)\n shape_objects = []\n transforms = []\n for i, child in enumerate(children):\n name = \"%s:%s/%s\" % (\n child_type,\n link.attrib[\"name\"],\n child.attrib.get(\"name\", i),\n )\n\n color = None\n if child_type == \"visual\":\n material = child.find(\"material\")\n if material is not None:\n material_name, color = _parse_material(material)\n if color is None and material_name in materials:\n color = materials[material_name]\n\n child2link = _parse_origin(child, strict_check)\n transforms.append((name, link.attrib[\"name\"], child2link))\n\n shape_objects.extend(\n _parse_geometry(child, name, color, mesh_path, package_dir)\n )\n return shape_objects, transforms\n\n\ndef _parse_geometry(child, name, color, mesh_path, package_dir):\n \"\"\"Parse geometric primitives (box, cylinder, sphere) or meshes.\"\"\"\n geometry = child.find(\"geometry\")\n if geometry is None:\n raise UrdfException(\"Missing geometry tag in link '%s'\" % name)\n result = []\n for shape_type in [\"box\", \"cylinder\", \"sphere\", \"mesh\"]:\n shapes = geometry.findall(shape_type)\n Cls = shape_classes[shape_type]\n for shape in shapes:\n shape_object = Cls(\n name, mesh_path=mesh_path, package_dir=package_dir, color=color\n )\n shape_object.parse(shape)\n result.append(shape_object)\n return result\n\n\ndef _parse_origin(entry, strict_check):\n \"\"\"Parse transformation.\"\"\"\n origin = entry.find(\"origin\")\n translation = np.zeros(3)\n rotation = np.eye(3)\n if origin is not None:\n if \"xyz\" in origin.attrib:\n translation = np.fromstring(origin.attrib[\"xyz\"], sep=\" \")\n if \"rpy\" in origin.attrib:\n roll_pitch_yaw = np.fromstring(origin.attrib[\"rpy\"], sep=\" \")\n # URDF and KDL use the active convention for rotation matrices.\n # For more details on how the URDF parser handles the\n # conversion from Euler angles, see this blog post:\n # https://orbitalstation.wordpress.com/tag/quaternion/\n rotation = matrix_from_euler(roll_pitch_yaw, 0, 1, 2, True)\n return transform_from(rotation, translation, strict_check=strict_check)\n\n\ndef _parse_mass(inertial):\n \"\"\"Parse link mass.\"\"\"\n mass = inertial.find(\"mass\")\n if mass is not None and \"value\" in mass.attrib:\n return float(mass.attrib[\"value\"])\n else:\n return 0.0\n\n\ndef _parse_inertia(inertial):\n \"\"\"Parse inertia matrix.\"\"\"\n inertia = inertial.find(\"inertia\")\n if inertia is None:\n return np.zeros((3, 3))\n\n ixx = float(inertia.attrib.get(\"ixx\", 0.0))\n iyy = float(inertia.attrib.get(\"iyy\", 0.0))\n izz = float(inertia.attrib.get(\"izz\", 0.0))\n ixy = float(inertia.attrib.get(\"ixy\", 0.0))\n ixz = float(inertia.attrib.get(\"ixz\", 0.0))\n iyz = float(inertia.attrib.get(\"iyz\", 0.0))\n return np.array([[ixx, ixy, ixz], [ixy, iyy, iyz], [ixz, iyz, izz]])\n\n\ndef _parse_joint(joint, link_names, strict_check):\n \"\"\"Create joint object.\"\"\"\n j = Joint()\n\n if \"name\" not in joint.attrib:\n raise UrdfException(\"Joint name is missing.\")\n j.joint_name = joint.attrib[\"name\"]\n\n if \"type\" not in joint.attrib:\n raise UrdfException(\n \"Joint type is missing in joint '%s'.\" % j.joint_name\n )\n\n parent = joint.find(\"parent\")\n if parent is None:\n raise UrdfException(\"No parent specified in joint '%s'\" % j.joint_name)\n if \"link\" not in parent.attrib:\n raise UrdfException(\n \"No parent link name given in joint '%s'.\" % j.joint_name\n )\n j.parent = parent.attrib[\"link\"]\n if j.parent not in link_names:\n raise UrdfException(\n \"Parent link '%s' of joint '%s' is not \"\n \"defined.\" % (j.parent, j.joint_name)\n )\n\n child = joint.find(\"child\")\n if child is None:\n raise UrdfException(\"No child specified in joint '%s'\" % j.joint_name)\n if \"link\" not in child.attrib:\n raise UrdfException(\n \"No child link name given in joint '%s'.\" % j.joint_name\n )\n j.child = child.attrib[\"link\"]\n if j.child not in link_names:\n raise UrdfException(\n \"Child link '%s' of joint '%s' is not \"\n \"defined.\" % (j.child, j.joint_name)\n )\n\n j.joint_type = joint.attrib[\"type\"]\n\n if j.joint_type in [\"planar\", \"floating\"]:\n raise UrdfException(\"Unsupported joint type '%s'\" % j.joint_type)\n if j.joint_type not in [\"revolute\", \"continuous\", \"prismatic\", \"fixed\"]:\n raise UrdfException(\n \"Joint type '%s' is not allowed in a URDF \"\n \"document.\" % j.joint_type\n )\n\n j.child2parent = _parse_origin(joint, strict_check)\n\n j.joint_axis = np.array([1, 0, 0])\n if j.joint_type in [\"revolute\", \"continuous\", \"prismatic\"]:\n axis = joint.find(\"axis\")\n if axis is not None and \"xyz\" in axis.attrib:\n j.joint_axis = np.fromstring(axis.attrib[\"xyz\"], sep=\" \")\n\n j.limits = _parse_limits(joint)\n return j\n\n\ndef _parse_limits(joint):\n \"\"\"Parse joint limits.\"\"\"\n limit = joint.find(\"limit\")\n lower, upper = float(\"-inf\"), float(\"inf\")\n if limit is not None:\n if \"lower\" in limit.attrib:\n lower = float(limit.attrib[\"lower\"])\n if \"upper\" in limit.attrib:\n upper = float(limit.attrib[\"upper\"])\n return lower, upper\n\n\ndef _add_links(tm, links):\n \"\"\"Add previously parsed links.\n\n Parameters\n ----------\n tm : UrdfTransformManager\n Transform manager\n\n links : list of Link\n Joint information from URDF\n \"\"\"\n for link in links:\n tm.visuals.extend(link.visuals)\n tm.collision_objects.extend(link.collision_objects)\n\n for from_frame, to_frame, transform in link.transforms:\n tm.add_transform(from_frame, to_frame, transform)\n\n\ndef _add_joints(tm, joints):\n \"\"\"Add previously parsed joints.\n\n Parameters\n ----------\n tm : UrdfTransformManager\n Transform manager\n\n joints : list of Joint\n Joint information from URDF\n \"\"\"\n for joint in joints:\n if joint.joint_type in [\"revolute\", \"continuous\"]:\n tm.add_joint(\n joint.joint_name,\n joint.child,\n joint.parent,\n joint.child2parent,\n joint.joint_axis,\n joint.limits,\n \"revolute\",\n )\n elif joint.joint_type == \"prismatic\":\n tm.add_joint(\n joint.joint_name,\n joint.child,\n joint.parent,\n joint.child2parent,\n joint.joint_axis,\n joint.limits,\n \"prismatic\",\n )\n else:\n assert joint.joint_type == \"fixed\"\n tm.add_joint(\n joint.joint_name,\n joint.child,\n joint.parent,\n joint.child2parent,\n joint.joint_axis,\n (0.0, 0.0),\n \"fixed\",\n )\n\n\nclass Link(object):\n \"\"\"Link from URDF file.\n\n This class is only required temporarily while we parse the URDF.\n\n Attributes\n ----------\n name : str\n Link name\n\n visuals : list of Geometry\n Visual geometries\n\n collision_objects : list of Geometry\n Geometries for collision calculation\n\n transforms : list\n Transformations given as tuples: name of frame A, name of frame B,\n transform A2B\n\n inertial_frame : array, shape (4, 4)\n Pose of inertial frame with respect to the link\n\n mass : float\n Mass of the link\n\n inertia : array, shape (3, 3)\n Inertia matrix\n \"\"\"\n\n def __init__(self):\n self.name = None\n self.visuals = []\n self.collision_objects = []\n self.transforms = []\n self.inertial_frame = np.eye(4)\n self.mass = 0.0\n self.inertia = np.zeros((3, 3))\n\n\nclass Joint(object):\n \"\"\"Joint from URDF file.\n\n This class is only required temporarily while we parse the URDF.\n\n Attributes\n ----------\n child : str\n Name of the child\n\n parent : str\n Name of the parent frame\n\n child2parent : array-like, shape (4, 4)\n Transformation from child to parent\n\n joint_name : str\n Name of the joint that defines the transformation\n\n joint_axis : array-like, shape (3,)\n Rotation axis of the joint (defined in the child frame)\n\n joint_type : str\n Either 'fixed' or 'revolute'\n\n limits : pair of float\n Lower and upper joint angle limit\n \"\"\"\n\n def __init__(self):\n self.child = None\n self.parent = None\n self.child2parent = np.eye(4)\n self.joint_name = None\n self.joint_axis = None\n self.joint_type = \"fixed\"\n self.limits = float(\"-inf\"), float(\"inf\")\n\n\nclass Geometry(object):\n \"\"\"Geometrical object.\"\"\"\n\n def __init__(self, frame, mesh_path, package_dir, color):\n self.frame = frame\n self.mesh_path = mesh_path\n self.package_dir = package_dir\n self.color = color\n\n def parse(self, xml):\n \"\"\"Parse parameters of geometry.\"\"\"\n\n def plot(\n self, tm, frame, ax=None, alpha=0.3, wireframe=True, convex_hull=True\n ):\n \"\"\"Plot geometry.\"\"\"\n\n\nclass Box(Geometry):\n \"\"\"Geometrical object: box.\"\"\"\n\n def __init__(self, frame, mesh_path, package_dir, color):\n super(Box, self).__init__(frame, mesh_path, package_dir, color)\n self.size = np.zeros(3)\n\n def parse(self, xml):\n \"\"\"Parse box size.\"\"\"\n if \"size\" in xml.attrib:\n self.size[:] = np.fromstring(xml.attrib[\"size\"], sep=\" \")\n\n def plot(\n self, tm, frame, ax=None, alpha=0.3, wireframe=True, convex_hull=True\n ): # pragma: no cover\n \"\"\"Plot box.\"\"\"\n A2B = tm.get_transform(self.frame, frame)\n color = self.color if self.color is not None else \"k\"\n from .plot_utils import plot_box\n\n return plot_box(\n ax, self.size, A2B, wireframe=wireframe, alpha=alpha, color=color\n )\n\n\nclass Sphere(Geometry):\n \"\"\"Geometrical object: sphere.\"\"\"\n\n def __init__(self, frame, mesh_path, package_dir, color):\n super(Sphere, self).__init__(frame, mesh_path, package_dir, color)\n self.radius = 0.0\n\n def parse(self, xml):\n \"\"\"Parse sphere radius.\"\"\"\n if \"radius\" not in xml.attrib:\n raise UrdfException(\"Sphere has no radius.\")\n self.radius = float(xml.attrib[\"radius\"])\n\n def plot(\n self, tm, frame, ax=None, alpha=0.3, wireframe=True, convex_hull=True\n ): # pragma: no cover\n \"\"\"Plot sphere.\"\"\"\n center = tm.get_transform(self.frame, frame)[:3, 3]\n color = self.color if self.color is not None else \"k\"\n from .plot_utils import plot_sphere\n\n return plot_sphere(\n ax,\n self.radius,\n center,\n wireframe=wireframe,\n alpha=alpha,\n color=color,\n )\n\n\nclass Cylinder(Geometry):\n \"\"\"Geometrical object: cylinder.\"\"\"\n\n def __init__(self, frame, mesh_path, package_dir, color):\n super(Cylinder, self).__init__(frame, mesh_path, package_dir, color)\n self.radius = 0.0\n self.length = 0.0\n\n def parse(self, xml):\n \"\"\"Parse cylinder radius and length.\"\"\"\n if \"radius\" not in xml.attrib:\n raise UrdfException(\"Cylinder has no radius.\")\n self.radius = float(xml.attrib[\"radius\"])\n if \"length\" not in xml.attrib:\n raise UrdfException(\"Cylinder has no length.\")\n self.length = float(xml.attrib[\"length\"])\n\n def plot(\n self, tm, frame, ax=None, alpha=0.3, wireframe=True, convex_hull=True\n ): # pragma: no cover\n \"\"\"Plot cylinder.\"\"\"\n A2B = tm.get_transform(self.frame, frame)\n color = self.color if self.color is not None else \"k\"\n from .plot_utils import plot_cylinder\n\n return plot_cylinder(\n ax,\n self.length,\n self.radius,\n 0.0,\n A2B,\n wireframe=wireframe,\n alpha=alpha,\n color=color,\n )\n\n\nclass Mesh(Geometry):\n \"\"\"Geometrical object: mesh.\"\"\"\n\n def __init__(self, frame, mesh_path, package_dir, color):\n super(Mesh, self).__init__(frame, mesh_path, package_dir, color)\n self.filename = None\n self.scale = np.ones(3)\n\n def parse(self, xml):\n \"\"\"Parse mesh filename and scale.\"\"\"\n if self.mesh_path is None and self.package_dir is None:\n self.filename = None\n else:\n if \"filename\" not in xml.attrib:\n raise UrdfException(\"Mesh has no filename.\")\n if self.mesh_path is not None:\n self.filename = os.path.join(\n self.mesh_path, xml.attrib[\"filename\"]\n )\n else:\n assert self.package_dir is not None\n self.filename = xml.attrib[\"filename\"].replace(\n \"package://\", self.package_dir\n )\n if \"scale\" in xml.attrib:\n self.scale = np.fromstring(xml.attrib[\"scale\"], sep=\" \")\n\n def plot(\n self, tm, frame, ax=None, alpha=0.3, wireframe=True, convex_hull=True\n ): # pragma: no cover\n \"\"\"Plot mesh.\"\"\"\n from .plot_utils import plot_mesh\n\n A2B = tm.get_transform(self.frame, frame)\n color = self.color if self.color is not None else \"k\"\n return plot_mesh(\n ax,\n self.filename,\n A2B,\n self.scale,\n wireframe=wireframe,\n convex_hull=convex_hull,\n alpha=alpha,\n color=color,\n )\n\n\nshape_classes = {\n \"box\": Box,\n \"sphere\": Sphere,\n \"cylinder\": Cylinder,\n \"mesh\": Mesh,\n}\n\n\nclass UrdfException(Exception):\n \"\"\"Exception while parsing URDF files.\"\"\"\n" }, { "path": "pytransform3d/urdf.pyi", "content": "from typing import Dict, Tuple, List, Union, Type, Hashable\n\nimport numpy as np\nimport numpy.typing as npt\nfrom lxml.etree import _Element as Element\nfrom mpl_toolkits.mplot3d import Axes3D\n\nfrom .transform_manager import TransformManager\n\nclass UrdfTransformManager(TransformManager):\n _joints: Dict[\n str,\n Tuple[\n Hashable, Hashable, np.ndarray, np.ndarray, Tuple[float, float], str\n ],\n ]\n collision_objects: List[Geometry]\n visuals: List[Geometry]\n\n def __init__(self, strict_check: bool = ..., check: bool = ...): ...\n def add_joint(\n self,\n joint_name: str,\n from_frame: Hashable,\n to_frame: Hashable,\n child2parent: npt.ArrayLike,\n axis: npt.ArrayLike,\n limits: Tuple[float, float] = ...,\n joint_type: str = ...,\n ): ...\n def set_joint(self, joint_name: str, value: float): ...\n def get_joint_limits(self, joint_name: str) -> Tuple[float, float]: ...\n def load_urdf(\n self,\n urdf_xml: str,\n mesh_path: Union[None, str] = ...,\n package_dir: Union[None, str] = ...,\n ): ...\n def plot_visuals(\n self,\n frame: Hashable,\n ax: Union[None, Axes3D] = ...,\n ax_s: float = ...,\n wireframe: bool = ...,\n convex_hull_of_mesh: bool = ...,\n alpha: float = ...,\n ): ...\n def plot_collision_objects(\n self,\n frame: Hashable,\n ax: Union[None, Axes3D] = ...,\n ax_s: float = ...,\n wireframe: bool = ...,\n convex_hull_of_mesh: bool = ...,\n alpha: float = ...,\n ): ...\n def _plot_objects(\n self,\n objects,\n frame: Hashable,\n ax: Union[None, Axes3D] = ...,\n ax_s: float = ...,\n wireframe: bool = ...,\n convex_hull_of_mesh: bool = ...,\n alpha: float = ...,\n ): ...\n\nclass Link(object):\n name: Union[None, str]\n visuals: List[Geometry]\n collision_objects: List[Geometry]\n transforms: List[Tuple[str, str, np.ndarray]]\n inertial_frame: np.ndarray\n mass: float\n inertia: np.ndarray\n\nclass Joint(object):\n child: Union[None, str]\n parent: Union[None, str]\n child2parent: np.ndarray\n joint_name: Union[None, str]\n joint_axis: Union[None, np.ndarray]\n joint_type: str\n limits: Tuple[float, float]\n\nclass Geometry(object):\n frame: str\n mesh_path: Union[None, str]\n package_dir: Union[None, str]\n color: npt.ArrayLike\n\n def __init__(\n self,\n frame: str,\n mesh_path: Union[None, str],\n package_dir: Union[None, str],\n color: str,\n ): ...\n def parse(self, xml: Element): ...\n def plot(\n self,\n tm: UrdfTransformManager,\n frame: Hashable,\n ax: Union[None, Axes3D] = ...,\n alpha: float = ...,\n wireframe: bool = ...,\n convex_hull: bool = ...,\n ): ...\n\nclass Box(Geometry):\n size: np.ndarray\n\nclass Sphere(Geometry):\n radius: float\n\nclass Cylinder(Geometry):\n radius: float\n length: float\n\nclass Mesh(Geometry):\n filename: Union[None, str]\n scale: np.ndarray\n\nclass UrdfException(Exception): ...\n\ndef parse_urdf(\n urdf_xml: str,\n mesh_path: Union[str, None] = ...,\n package_dir: Union[str, None] = ...,\n strict_check: bool = ...,\n) -> Tuple[str, List[Link], List[Joint]]: ...\ndef initialize_urdf_transform_manager(\n tm: UrdfTransformManager,\n robot_name: str,\n links: List[Link],\n joints: List[Joint],\n): ...\n\nshape_classes: Dict[str, Union[Type[Geometry]]]\n" }, { "path": "pytransform3d/visualizer/__init__.py", "content": "\"\"\"Optional 3D renderer based on Open3D's visualizer.\"\"\"\n\nimport warnings\n\ntry:\n import open3d as o3d # noqa: F401\n from ._artists import (\n Artist,\n Line3D,\n PointCollection3D,\n Vector3D,\n Frame,\n Trajectory,\n Camera,\n Box,\n Sphere,\n Cylinder,\n Mesh,\n Ellipsoid,\n Capsule,\n Cone,\n Plane,\n Graph,\n )\n from ._figure import figure, Figure\n\n __all__ = [\n \"figure\",\n \"Figure\",\n \"Artist\",\n \"Line3D\",\n \"PointCollection3D\",\n \"Vector3D\",\n \"Frame\",\n \"Trajectory\",\n \"Camera\",\n \"Box\",\n \"Sphere\",\n \"Cylinder\",\n \"Mesh\",\n \"Ellipsoid\",\n \"Capsule\",\n \"Cone\",\n \"Plane\",\n \"Graph\",\n ]\nexcept ImportError:\n warnings.warn(\n \"3D visualizer is not available. Install open3d.\",\n ImportWarning,\n stacklevel=2,\n )\n" }, { "path": "pytransform3d/visualizer/_artists.py", "content": "\"\"\"Visualizer artists.\"\"\"\n\nimport warnings\nfrom itertools import chain\n\nimport numpy as np\nimport open3d as o3d\n\nfrom .. import rotations as pr\nfrom .. import transformations as pt\nfrom .. import urdf\nfrom .._mesh_loader import load_mesh\n\n\nclass Artist:\n \"\"\"Abstract base class for objects that can be rendered.\"\"\"\n\n def add_artist(self, figure):\n \"\"\"Add artist to figure.\n\n Parameters\n ----------\n figure : Figure\n Figure to which the artist will be added.\n \"\"\"\n for g in self.geometries:\n figure.add_geometry(g)\n\n @property\n def geometries(self):\n \"\"\"Expose geometries.\n\n Returns\n -------\n geometries : list\n List of geometries that can be added to the visualizer.\n \"\"\"\n return []\n\n\nclass Line3D(Artist):\n \"\"\"A line.\n\n Parameters\n ----------\n P : array-like, shape (n_points, 3)\n Points of which the line consists.\n\n c : array-like, shape (n_points - 1, 3) or (3,), optional (default: black)\n Color can be given as individual colors per line segment or as one\n color for each segment. A color is represented by 3 values between\n 0 and 1 indicate representing red, green, and blue respectively.\n \"\"\"\n\n def __init__(self, P, c=(0, 0, 0)):\n self.line_set = o3d.geometry.LineSet()\n self.set_data(P, c)\n\n def set_data(self, P, c=None):\n \"\"\"Update data.\n\n Parameters\n ----------\n P : array-like, shape (n_points, 3)\n Points of which the line consists.\n\n c : array-like, shape (n_points - 1, 3) or (3,), optional (default: black)\n Color can be given as individual colors per line segment or\n as one color for each segment. A color is represented by 3\n values between 0 and 1 indicate representing red, green, and\n blue respectively.\n \"\"\"\n self.line_set.points = o3d.utility.Vector3dVector(P)\n self.line_set.lines = o3d.utility.Vector2iVector(\n np.hstack(\n (\n np.arange(len(P) - 1)[:, np.newaxis],\n np.arange(1, len(P))[:, np.newaxis],\n )\n )\n )\n\n if c is not None:\n try:\n if len(c[0]) == 3:\n self.line_set.colors = o3d.utility.Vector3dVector(c)\n except TypeError: # one color for all segments\n self.line_set.colors = o3d.utility.Vector3dVector(\n [c for _ in range(len(P) - 1)]\n )\n\n @property\n def geometries(self):\n \"\"\"Expose geometries.\n\n Returns\n -------\n geometries : list\n List of geometries that can be added to the visualizer.\n \"\"\"\n return [self.line_set]\n\n\nclass PointCollection3D(Artist):\n \"\"\"Collection of points.\n\n Parameters\n ----------\n P : array, shape (n_points, 3)\n Points\n\n s : float, optional (default: 0.05)\n Scaling of the spheres that will be drawn.\n\n c : array-like, shape (3,) or (n_points, 3), optional (default: black)\n A color is represented by 3 values between 0 and 1 indicate\n representing red, green, and blue respectively.\n \"\"\"\n\n def __init__(self, P, s=0.05, c=None):\n self._points = []\n self._P = np.zeros_like(P)\n\n if c is not None:\n c = np.asarray(c)\n if c.ndim == 1:\n c = np.tile(c, (len(P), 1))\n\n for i in range(len(P)):\n point = o3d.geometry.TriangleMesh.create_sphere(\n radius=s, resolution=10\n )\n if c is not None:\n n_vertices = len(point.vertices)\n colors = np.zeros((n_vertices, 3))\n colors[:] = c[i]\n point.vertex_colors = o3d.utility.Vector3dVector(colors)\n point.compute_vertex_normals()\n self._points.append(point)\n\n self.set_data(P)\n\n def set_data(self, P):\n \"\"\"Update data.\n\n Parameters\n ----------\n P : array, shape (n_points, 3)\n Points\n \"\"\"\n P = np.copy(P)\n for i, point, p, previous_p in zip(\n range(len(self._P)), self._points, P, self._P\n ):\n if any(np.isnan(p)):\n P[i] = previous_p\n else:\n point.translate(p - previous_p)\n\n self._P = P\n\n @property\n def geometries(self):\n \"\"\"Expose geometries.\n\n Returns\n -------\n geometries : list\n List of geometries that can be added to the visualizer.\n \"\"\"\n return self._points\n\n\nclass Vector3D(Artist):\n \"\"\"A vector.\n\n Parameters\n ----------\n start : array-like, shape (3,), optional (default: [0, 0, 0])\n Start of the vector\n\n direction : array-like, shape (3,), optional (default: [1, 0, 0])\n Direction of the vector\n\n c : array-like, shape (3,), optional (default: black)\n A color is represented by 3 values between 0 and 1 indicate\n representing red, green, and blue respectively.\n \"\"\"\n\n def __init__(\n self, start=np.zeros(3), direction=np.array([1, 0, 0]), c=(0, 0, 0)\n ):\n self.vector = o3d.geometry.TriangleMesh.create_arrow(\n cylinder_radius=0.035,\n cone_radius=0.07,\n cylinder_height=0.8,\n cone_height=0.2,\n )\n self.set_data(start, direction, c)\n\n def set_data(self, start, direction, c=None):\n \"\"\"Update data.\n\n Parameters\n ----------\n start : array-like, shape (3,)\n Start of the vector\n\n direction : array-like, shape (3,)\n Direction of the vector\n\n c : array-like, shape (3,), optional (default: black)\n A color is represented by 3 values between 0 and 1 indicate\n representing red, green, and blue respectively.\n \"\"\"\n length = np.linalg.norm(direction)\n z = direction / length\n x, y = pr.plane_basis_from_normal(z)\n R = np.column_stack((x, y, z))\n A2B = pt.transform_from(R, start)\n\n new_vector = o3d.geometry.TriangleMesh.create_arrow(\n cylinder_radius=0.035 * length,\n cone_radius=0.07 * length,\n cylinder_height=0.8 * length,\n cone_height=0.2 * length,\n )\n self.vector.vertices = new_vector.vertices\n self.vector.triangles = new_vector.triangles\n self.vector.transform(A2B)\n if c is not None:\n self.vector.paint_uniform_color(c)\n self.vector.compute_vertex_normals()\n\n @property\n def geometries(self):\n \"\"\"Expose geometries.\n\n Returns\n -------\n geometries : list\n List of geometries that can be added to the visualizer.\n \"\"\"\n return [self.vector]\n\n\nclass Frame(Artist):\n \"\"\"Coordinate frame.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Transform from frame A to frame B\n\n label : str, optional (default: None)\n Name of the frame\n\n s : float, optional (default: 1)\n Length of basis vectors\n \"\"\"\n\n def __init__(self, A2B, label=None, s=1.0):\n self.A2B = None\n self.label = None\n self.s = s\n\n self.frame = o3d.geometry.TriangleMesh.create_coordinate_frame(\n size=self.s\n )\n\n self.set_data(A2B, label)\n\n def set_data(self, A2B, label=None):\n \"\"\"Update data.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Transform from frame A to frame B\n\n label : str, optional (default: None)\n Name of the frame\n \"\"\"\n previous_A2B = self.A2B\n if previous_A2B is None:\n previous_A2B = np.eye(4)\n self.A2B = A2B\n self.label = label\n if label is not None:\n warnings.warn(\n \"This viewer does not support text. Frame label \"\n \"will be ignored.\",\n UserWarning,\n stacklevel=2,\n )\n\n self.frame.transform(pt.invert_transform(previous_A2B, check=False))\n self.frame.transform(self.A2B)\n\n @property\n def geometries(self):\n \"\"\"Expose geometries.\n\n Returns\n -------\n geometries : list\n List of geometries that can be added to the visualizer.\n \"\"\"\n return [self.frame]\n\n\nclass Trajectory(Artist):\n \"\"\"Trajectory of poses.\n\n Parameters\n ----------\n H : array-like, shape (n_steps, 4, 4)\n Sequence of poses represented by homogeneous matrices\n\n n_frames : int, optional (default: 10)\n Number of frames that should be plotted to indicate the rotation\n\n s : float, optional (default: 1)\n Scaling of the frames that will be drawn\n\n c : array-like, shape (3,), optional (default: black)\n A color is represented by 3 values between 0 and 1 indicate\n representing red, green, and blue respectively.\n \"\"\"\n\n def __init__(self, H, n_frames=10, s=1.0, c=(0, 0, 0)):\n self.H = H\n self.n_frames = n_frames\n self.s = s\n self.c = c\n\n self.key_frames = []\n self.line = Line3D(H[:, :3, 3], c)\n\n self.key_frames_indices = np.linspace(\n 0, len(self.H) - 1, self.n_frames, dtype=np.int64\n )\n for key_frame_idx in self.key_frames_indices:\n self.key_frames.append(Frame(self.H[key_frame_idx], s=self.s))\n\n self.set_data(H)\n\n def set_data(self, H):\n \"\"\"Update data.\n\n Parameters\n ----------\n H : array-like, shape (n_steps, 4, 4)\n Sequence of poses represented by homogeneous matrices\n \"\"\"\n self.line.set_data(H[:, :3, 3])\n for i, key_frame_idx in enumerate(self.key_frames_indices):\n self.key_frames[i].set_data(H[key_frame_idx])\n\n @property\n def geometries(self):\n \"\"\"Expose geometries.\n\n Returns\n -------\n geometries : list\n List of geometries that can be added to the visualizer.\n \"\"\"\n return self.line.geometries + list(\n chain(*[kf.geometries for kf in self.key_frames])\n )\n\n\nclass Sphere(Artist):\n \"\"\"Sphere.\n\n Parameters\n ----------\n radius : float, optional (default: 1)\n Radius of the sphere\n\n A2B : array-like, shape (4, 4)\n Center of the sphere\n\n resolution : int, optional (default: 20)\n The resolution of the sphere. The longitudes will be split into\n resolution segments (i.e. there are resolution + 1 latitude lines\n including the north and south pole). The latitudes will be split\n into 2 * resolution segments (i.e. there are 2 * resolution\n longitude lines.)\n\n c : array-like, shape (3,), optional (default: None)\n Color\n \"\"\"\n\n def __init__(self, radius=1.0, A2B=np.eye(4), resolution=20, c=None):\n self.sphere = o3d.geometry.TriangleMesh.create_sphere(\n radius, resolution\n )\n if c is not None:\n n_vertices = len(self.sphere.vertices)\n colors = np.zeros((n_vertices, 3))\n colors[:] = c\n self.sphere.vertex_colors = o3d.utility.Vector3dVector(colors)\n self.sphere.compute_vertex_normals()\n self.A2B = None\n self.set_data(A2B)\n\n def set_data(self, A2B):\n \"\"\"Update data.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Center of the sphere.\n \"\"\"\n previous_A2B = self.A2B\n if previous_A2B is None:\n previous_A2B = np.eye(4)\n self.A2B = A2B\n\n self.sphere.transform(pt.invert_transform(previous_A2B, check=False))\n self.sphere.transform(self.A2B)\n\n @property\n def geometries(self):\n \"\"\"Expose geometries.\n\n Returns\n -------\n geometries : list\n List of geometries that can be added to the visualizer.\n \"\"\"\n return [self.sphere]\n\n\nclass Box(Artist):\n \"\"\"Box.\n\n Parameters\n ----------\n size : array-like, shape (3,), optional (default: [1, 1, 1])\n Size of the box per dimension\n\n A2B : array-like, shape (4, 4), optional (default: I)\n Center of the box\n\n c : array-like, shape (3,), optional (default: None)\n Color\n \"\"\"\n\n def __init__(self, size=np.ones(3), A2B=np.eye(4), c=None):\n self.half_size = np.asarray(size) / 2.0\n width, height, depth = size\n self.box = o3d.geometry.TriangleMesh.create_box(width, height, depth)\n if c is not None:\n n_vertices = len(self.box.vertices)\n colors = np.zeros((n_vertices, 3))\n colors[:] = c\n self.box.vertex_colors = o3d.utility.Vector3dVector(colors)\n self.box.compute_vertex_normals()\n self.A2B = None\n self.set_data(A2B)\n\n def set_data(self, A2B):\n \"\"\"Update data.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Center of the box.\n \"\"\"\n previous_A2B = self.A2B\n if previous_A2B is None:\n self.box.transform(\n pt.transform_from(R=np.eye(3), p=-self.half_size)\n )\n previous_A2B = np.eye(4)\n self.A2B = A2B\n\n self.box.transform(pt.invert_transform(previous_A2B, check=False))\n self.box.transform(self.A2B)\n\n @property\n def geometries(self):\n \"\"\"Expose geometries.\n\n Returns\n -------\n geometries : list\n List of geometries that can be added to the visualizer.\n \"\"\"\n return [self.box]\n\n\nclass Cylinder(Artist):\n \"\"\"Cylinder.\n\n A cylinder is the volume covered by a disk moving along a line segment.\n\n Parameters\n ----------\n length : float, optional (default: 1)\n Length of the cylinder.\n\n radius : float, optional (default: 1)\n Radius of the cylinder.\n\n A2B : array-like, shape (4, 4)\n Pose of the cylinder. The position corresponds to the center of the\n line segment and the z-axis to the direction of the line segment.\n\n resolution : int, optional (default: 20)\n The circles will be split into resolution segments.\n\n split : int, optional (default: 4)\n The height will be split into split segments\n\n c : array-like, shape (3,), optional (default: None)\n Color\n \"\"\"\n\n def __init__(\n self,\n length=2.0,\n radius=1.0,\n A2B=np.eye(4),\n resolution=20,\n split=4,\n c=None,\n ):\n self.cylinder = o3d.geometry.TriangleMesh.create_cylinder(\n radius=radius, height=length, resolution=resolution, split=split\n )\n if c is not None:\n n_vertices = len(self.cylinder.vertices)\n colors = np.zeros((n_vertices, 3))\n colors[:] = c\n self.cylinder.vertex_colors = o3d.utility.Vector3dVector(colors)\n self.cylinder.compute_vertex_normals()\n self.A2B = None\n self.set_data(A2B)\n\n def set_data(self, A2B):\n \"\"\"Update data.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Center of the cylinder.\n \"\"\"\n previous_A2B = self.A2B\n if previous_A2B is None:\n previous_A2B = np.eye(4)\n self.A2B = A2B\n\n self.cylinder.transform(pt.invert_transform(previous_A2B, check=False))\n self.cylinder.transform(self.A2B)\n\n @property\n def geometries(self):\n \"\"\"Expose geometries.\n\n Returns\n -------\n geometries : list\n List of geometries that can be added to the visualizer.\n \"\"\"\n return [self.cylinder]\n\n\nclass Mesh(Artist):\n \"\"\"Mesh.\n\n Parameters\n ----------\n filename : str\n Path to mesh file\n\n A2B : array-like, shape (4, 4)\n Center of the mesh\n\n s : array-like, shape (3,), optional (default: [1, 1, 1])\n Scaling of the mesh that will be drawn\n\n c : array-like, shape (n_vertices, 3) or (3,), optional (default: None)\n Color(s)\n\n convex_hull : bool, optional (default: False)\n Compute convex hull of mesh.\n \"\"\"\n\n def __init__(\n self, filename, A2B=np.eye(4), s=np.ones(3), c=None, convex_hull=False\n ):\n mesh = load_mesh(filename)\n if convex_hull:\n mesh.convex_hull()\n\n self.mesh = mesh.get_open3d_mesh()\n self.mesh.vertices = o3d.utility.Vector3dVector(\n np.asarray(self.mesh.vertices) * s\n )\n self.mesh.compute_vertex_normals()\n if c is not None:\n n_vertices = len(self.mesh.vertices)\n colors = np.zeros((n_vertices, 3))\n colors[:] = c\n self.mesh.vertex_colors = o3d.utility.Vector3dVector(colors)\n self.A2B = None\n self.set_data(A2B)\n\n def set_data(self, A2B):\n \"\"\"Update data.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Center of the mesh.\n \"\"\"\n previous_A2B = self.A2B\n if previous_A2B is None:\n previous_A2B = np.eye(4)\n self.A2B = A2B\n\n self.mesh.transform(pt.invert_transform(previous_A2B, check=False))\n self.mesh.transform(self.A2B)\n\n @property\n def geometries(self):\n \"\"\"Expose geometries.\n\n Returns\n -------\n geometries : list\n List of geometries that can be added to the visualizer.\n \"\"\"\n return [self.mesh]\n\n\nclass Ellipsoid(Artist):\n \"\"\"Ellipsoid.\n\n Parameters\n ----------\n radii : array-like, shape (3,)\n Radii along the x-axis, y-axis, and z-axis of the ellipsoid.\n\n A2B : array-like, shape (4, 4)\n Pose of the ellipsoid.\n\n resolution : int, optional (default: 20)\n The resolution of the ellipsoid. The longitudes will be split into\n resolution segments (i.e. there are resolution + 1 latitude lines\n including the north and south pole). The latitudes will be split\n into 2 * resolution segments (i.e. there are 2 * resolution\n longitude lines.)\n\n c : array-like, shape (3,), optional (default: None)\n Color\n \"\"\"\n\n def __init__(self, radii, A2B=np.eye(4), resolution=20, c=None):\n self.ellipsoid = o3d.geometry.TriangleMesh.create_sphere(\n 1.0, resolution\n )\n vertices = np.asarray(self.ellipsoid.vertices)\n vertices *= np.asarray(radii)[np.newaxis]\n self.ellipsoid.vertices = o3d.utility.Vector3dVector(vertices)\n self.ellipsoid.compute_vertex_normals()\n if c is not None:\n n_vertices = len(self.ellipsoid.vertices)\n colors = np.zeros((n_vertices, 3))\n colors[:] = c\n self.ellipsoid.vertex_colors = o3d.utility.Vector3dVector(colors)\n self.A2B = None\n self.set_data(A2B)\n\n def set_data(self, A2B):\n \"\"\"Update data.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Center of the ellipsoid.\n \"\"\"\n previous_A2B = self.A2B\n if previous_A2B is None:\n previous_A2B = np.eye(4)\n self.A2B = A2B\n\n self.ellipsoid.transform(pt.invert_transform(previous_A2B, check=False))\n self.ellipsoid.transform(self.A2B)\n\n @property\n def geometries(self):\n \"\"\"Expose geometries.\n\n Returns\n -------\n geometries : list\n List of geometries that can be added to the visualizer.\n \"\"\"\n return [self.ellipsoid]\n\n\nclass Capsule(Artist):\n \"\"\"Capsule.\n\n A capsule is the volume covered by a sphere moving along a line segment.\n\n Parameters\n ----------\n height : float, optional (default: 1)\n Height of the capsule along its z-axis.\n\n radius : float, optional (default: 1)\n Radius of the capsule.\n\n A2B : array-like, shape (4, 4)\n Pose of the capsule. The position corresponds to the center of the line\n segment and the z-axis to the direction of the line segment.\n\n resolution : int, optional (default: 20)\n The resolution of the half spheres. The longitudes will be split into\n resolution segments (i.e. there are resolution + 1 latitude lines\n including the north and south pole). The latitudes will be split\n into 2 * resolution segments (i.e. there are 2 * resolution\n longitude lines.)\n\n c : array-like, shape (3,), optional (default: None)\n Color\n \"\"\"\n\n def __init__(\n self, height=1, radius=1, A2B=np.eye(4), resolution=20, c=None\n ):\n self.capsule = o3d.geometry.TriangleMesh.create_sphere(\n radius, resolution\n )\n vertices = np.asarray(self.capsule.vertices)\n vertices[vertices[:, 2] < 0, 2] -= 0.5 * height\n vertices[vertices[:, 2] > 0, 2] += 0.5 * height\n self.capsule.vertices = o3d.utility.Vector3dVector(vertices)\n self.capsule.compute_vertex_normals()\n if c is not None:\n n_vertices = len(self.capsule.vertices)\n colors = np.zeros((n_vertices, 3))\n colors[:] = c\n self.capsule.vertex_colors = o3d.utility.Vector3dVector(colors)\n self.A2B = None\n self.set_data(A2B)\n\n def set_data(self, A2B):\n \"\"\"Update data.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Start of the capsule's line segment.\n \"\"\"\n previous_A2B = self.A2B\n if previous_A2B is None:\n previous_A2B = np.eye(4)\n self.A2B = A2B\n\n self.capsule.transform(pt.invert_transform(previous_A2B, check=False))\n self.capsule.transform(self.A2B)\n\n @property\n def geometries(self):\n \"\"\"Expose geometries.\n\n Returns\n -------\n geometries : list\n List of geometries that can be added to the visualizer.\n \"\"\"\n return [self.capsule]\n\n\nclass Cone(Artist):\n \"\"\"Cone.\n\n Parameters\n ----------\n height : float, optional (default: 1)\n Height of the cone along its z-axis.\n\n radius : float, optional (default: 1)\n Radius of the cone.\n\n A2B : array-like, shape (4, 4)\n Pose of the cone, which is the center of its circle.\n\n resolution : int, optional (default: 20)\n The circle will be split into resolution segments.\n\n c : array-like, shape (3,), optional (default: None)\n Color\n \"\"\"\n\n def __init__(\n self, height=1, radius=1, A2B=np.eye(4), resolution=20, c=None\n ):\n self.cone = o3d.geometry.TriangleMesh.create_cone(\n radius, height, resolution\n )\n self.cone.compute_vertex_normals()\n if c is not None:\n n_vertices = len(self.cone.vertices)\n colors = np.zeros((n_vertices, 3))\n colors[:] = c\n self.cone.vertex_colors = o3d.utility.Vector3dVector(colors)\n self.A2B = None\n self.set_data(A2B)\n\n def set_data(self, A2B):\n \"\"\"Update data.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Center of the cone's circle.\n \"\"\"\n previous_A2B = self.A2B\n if previous_A2B is None:\n previous_A2B = np.eye(4)\n self.A2B = A2B\n\n self.cone.transform(pt.invert_transform(previous_A2B, check=False))\n self.cone.transform(self.A2B)\n\n @property\n def geometries(self):\n \"\"\"Expose geometries.\n\n Returns\n -------\n geometries : list\n List of geometries that can be added to the visualizer.\n \"\"\"\n return [self.cone]\n\n\nclass Plane(Artist):\n \"\"\"Plane.\n\n The plane will be defined either by a normal and a point in the plane or\n by the Hesse normal form, which only needs a normal and the distance to\n the origin from which we can compute the point in the plane as d * normal.\n\n A plane will be visualized by a square.\n\n Parameters\n ----------\n normal : array-like, shape (3,), optional (default: [0, 0, 1])\n Plane normal.\n\n d : float, optional (default: None)\n Distance to origin in Hesse normal form.\n\n point_in_plane : array-like, shape (3,), optional (default: None)\n Point in plane.\n\n s : float, optional (default: 1)\n Scaling of the plane that will be drawn.\n\n c : array-like, shape (3,), optional (default: None)\n Color.\n \"\"\"\n\n def __init__(\n self,\n normal=np.array([0.0, 0.0, 1.0]),\n d=None,\n point_in_plane=None,\n s=1.0,\n c=None,\n ):\n self.triangles = np.array(\n [\n [0, 1, 2],\n [1, 3, 2],\n [2, 1, 0],\n [2, 3, 1],\n ]\n )\n vertices = np.zeros((4, 3))\n self.plane = o3d.geometry.TriangleMesh(\n o3d.utility.Vector3dVector(vertices),\n o3d.utility.Vector3iVector(self.triangles),\n )\n\n self.set_data(normal, d, point_in_plane, s, c)\n\n def set_data(self, normal, d=None, point_in_plane=None, s=None, c=None):\n \"\"\"Update data.\n\n Parameters\n ----------\n normal : array-like, shape (3,)\n Plane normal.\n\n d : float, optional (default: None)\n Distance to origin in Hesse normal form.\n\n point_in_plane : array-like, shape (3,), optional (default: None)\n Point in plane.\n\n s : float, optional (default: 1)\n Scaling of the plane that will be drawn.\n\n c : array-like, shape (3,), optional (default: None)\n Color.\n\n Raises\n ------\n ValueError\n If neither 'd' nor 'point_in_plane' is defined.\n \"\"\"\n normal = np.asarray(normal)\n if point_in_plane is None:\n if d is None:\n raise ValueError(\n \"Either 'd' or 'point_in_plane' has to be defined!\"\n )\n point_in_plane = d * normal\n else:\n point_in_plane = np.asarray(point_in_plane)\n\n x_axis, y_axis = pr.plane_basis_from_normal(normal)\n vertices = np.array(\n [\n point_in_plane + s * x_axis + s * y_axis,\n point_in_plane - s * x_axis + s * y_axis,\n point_in_plane + s * x_axis - s * y_axis,\n point_in_plane - s * x_axis - s * y_axis,\n ]\n )\n self.plane.vertices = o3d.utility.Vector3dVector(vertices)\n self.plane.compute_vertex_normals()\n if c is not None:\n self.plane.paint_uniform_color(c)\n\n @property\n def geometries(self):\n \"\"\"Expose geometries.\n\n Returns\n -------\n geometries : list\n List of geometries that can be added to the visualizer.\n \"\"\"\n return [self.plane]\n\n\nclass Camera(Artist):\n \"\"\"Camera.\n\n Parameters\n ----------\n M : array-like, shape (3, 3)\n Intrinsic camera matrix that contains the focal lengths on the diagonal\n and the center of the the image in the last column. It does not matter\n whether values are given in meters or pixels as long as the unit is the\n same as for the sensor size.\n\n cam2world : array-like, shape (4, 4), optional (default: I)\n Transformation matrix of camera in world frame. We assume that the\n position is given in meters.\n\n virtual_image_distance : float, optional (default: 1)\n Distance from pinhole to virtual image plane that will be displayed.\n We assume that this distance is given in meters. The unit has to be\n consistent with the unit of the position in cam2world.\n\n sensor_size : array-like, shape (2,), optional (default: [1920, 1080])\n Size of the image sensor: (width, height). It does not matter whether\n values are given in meters or pixels as long as the unit is the same as\n for the sensor size.\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the transformation matrix is not numerically\n close enough to a real transformation matrix. Otherwise we print a\n warning.\n \"\"\"\n\n def __init__(\n self,\n M,\n cam2world=None,\n virtual_image_distance=1,\n sensor_size=(1920, 1080),\n strict_check=True,\n ):\n self.M = None\n self.cam2world = None\n self.virtual_image_distance = None\n self.sensor_size = None\n self.strict_check = strict_check\n\n self.line_set = o3d.geometry.LineSet()\n\n if cam2world is None:\n cam2world = np.eye(4)\n\n self.set_data(M, cam2world, virtual_image_distance, sensor_size)\n\n def set_data(\n self,\n M=None,\n cam2world=None,\n virtual_image_distance=None,\n sensor_size=None,\n ):\n \"\"\"Update camera parameters.\n\n Parameters\n ----------\n M : array-like, shape (3, 3), optional (default: old value)\n Intrinsic camera matrix that contains the focal lengths on the\n diagonal and the center of the the image in the last column. It\n does not matter whether values are given in meters or pixels as\n long as the unit is the same as for the sensor size.\n\n cam2world : array-like, shape (4, 4), optional (default: old value)\n Transformation matrix of camera in world frame. We assume that\n the position is given in meters.\n\n virtual_image_distance : float, optional (default: old value)\n Distance from pinhole to virtual image plane that will be\n displayed. We assume that this distance is given in meters.\n The unit has to be consistent with the unit of the position\n in cam2world.\n\n sensor_size : array-like, shape (2,), optional (default: old value)\n Size of the image sensor: (width, height). It does not matter\n whether values are given in meters or pixels as long as the\n unit is the same as for the sensor size.\n \"\"\"\n if M is not None:\n self.M = M\n if cam2world is not None:\n self.cam2world = pt.check_transform(\n cam2world, strict_check=self.strict_check\n )\n if virtual_image_distance is not None:\n self.virtual_image_distance = virtual_image_distance\n if sensor_size is not None:\n self.sensor_size = sensor_size\n\n camera_center_in_world = cam2world[:3, 3]\n focal_length = np.mean(np.diag(M[:2, :2]))\n sensor_corners_in_cam = np.array(\n [\n [0, 0, focal_length],\n [0, sensor_size[1], focal_length],\n [sensor_size[0], sensor_size[1], focal_length],\n [sensor_size[0], 0, focal_length],\n ]\n )\n sensor_corners_in_cam[:, 0] -= M[0, 2]\n sensor_corners_in_cam[:, 1] -= M[1, 2]\n sensor_corners_in_world = pt.transform(\n cam2world, pt.vectors_to_points(sensor_corners_in_cam)\n )[:, :3]\n virtual_image_corners = (\n virtual_image_distance\n / focal_length\n * (sensor_corners_in_world - camera_center_in_world[np.newaxis])\n + camera_center_in_world[np.newaxis]\n )\n\n up = virtual_image_corners[0] - virtual_image_corners[1]\n camera_line_points = np.vstack(\n (\n camera_center_in_world,\n virtual_image_corners[0],\n virtual_image_corners[1],\n virtual_image_corners[2],\n virtual_image_corners[3],\n virtual_image_corners[0] + 0.1 * up,\n 0.5 * (virtual_image_corners[0] + virtual_image_corners[3])\n + 0.5 * up,\n virtual_image_corners[3] + 0.1 * up,\n )\n )\n\n self.line_set.points = o3d.utility.Vector3dVector(camera_line_points)\n self.line_set.lines = o3d.utility.Vector2iVector(\n np.array(\n [\n [0, 1],\n [0, 2],\n [0, 3],\n [0, 4],\n [1, 2],\n [2, 3],\n [3, 4],\n [4, 1],\n [5, 6],\n [6, 7],\n [7, 5],\n ]\n )\n )\n\n @property\n def geometries(self):\n \"\"\"Expose geometries.\n\n Returns\n -------\n geometries : list\n List of geometries that can be added to the visualizer.\n \"\"\"\n return [self.line_set]\n\n\nclass Graph(Artist):\n \"\"\"Graph of connected frames.\n\n Parameters\n ----------\n tm : TransformManager\n Representation of the graph\n\n frame : str\n Name of the base frame in which the graph will be displayed\n\n show_frames : bool, optional (default: False)\n Show coordinate frames\n\n show_connections : bool, optional (default: False)\n Draw lines between frames of the graph\n\n show_visuals : bool, optional (default: False)\n Show visuals that are stored in the graph\n\n show_collision_objects : bool, optional (default: False)\n Show collision objects that are stored in the graph\n\n show_name : bool, optional (default: False)\n Show names of frames\n\n whitelist : list, optional (default: all)\n List of frames that should be displayed\n\n convex_hull_of_collision_objects : bool, optional (default: False)\n Show convex hull of collision objects.\n\n s : float, optional (default: 1)\n Scaling of the frames that will be drawn\n \"\"\"\n\n def __init__(\n self,\n tm,\n frame,\n show_frames=False,\n show_connections=False,\n show_visuals=False,\n show_collision_objects=False,\n show_name=False,\n whitelist=None,\n convex_hull_of_collision_objects=False,\n s=1.0,\n ):\n self.tm = tm\n self.frame = frame\n self.show_frames = show_frames\n self.show_connections = show_connections\n self.show_visuals = show_visuals\n self.show_collision_objects = show_collision_objects\n self.whitelist = whitelist\n self.convex_hull_of_collision_objects = convex_hull_of_collision_objects\n self.s = s\n\n if self.frame not in self.tm.nodes:\n raise KeyError(\"Unknown frame '%s'\" % self.frame)\n\n self.nodes = list(sorted(self.tm._whitelisted_nodes(whitelist)))\n\n self.frames = {}\n if self.show_frames:\n for node in self.nodes:\n try:\n node2frame = self.tm.get_transform(node, frame)\n name = node if show_name else None\n self.frames[node] = Frame(node2frame, name, self.s)\n except KeyError:\n pass # Frame is not connected to the reference frame\n\n self.connections = {}\n if self.show_connections:\n for frame_names in self.tm.transforms.keys():\n from_frame, to_frame = frame_names\n if from_frame in self.tm.nodes and to_frame in self.tm.nodes:\n try:\n self.tm.get_transform(from_frame, self.frame)\n self.tm.get_transform(to_frame, self.frame)\n self.connections[frame_names] = o3d.geometry.LineSet()\n except KeyError:\n pass # Frame is not connected to reference frame\n\n self.visuals = {}\n if show_visuals and hasattr(self.tm, \"visuals\"):\n self.visuals.update(_objects_to_artists(self.tm.visuals))\n self.collision_objects = {}\n if show_collision_objects and hasattr(self.tm, \"collision_objects\"):\n self.collision_objects.update(\n _objects_to_artists(\n self.tm.collision_objects, convex_hull_of_collision_objects\n )\n )\n\n self.set_data()\n\n def set_data(self):\n \"\"\"Indicate that data has been updated.\"\"\"\n if self.show_frames:\n for node in self.nodes:\n try:\n node2frame = self.tm.get_transform(node, self.frame)\n self.frames[node].set_data(node2frame)\n except KeyError:\n pass # Frame is not connected to the reference frame\n\n if self.show_connections:\n for frame_names in self.connections:\n from_frame, to_frame = frame_names\n try:\n from2ref = self.tm.get_transform(from_frame, self.frame)\n to2ref = self.tm.get_transform(to_frame, self.frame)\n\n points = np.vstack((from2ref[:3, 3], to2ref[:3, 3]))\n self.connections[frame_names].points = (\n o3d.utility.Vector3dVector(points)\n )\n self.connections[frame_names].lines = (\n o3d.utility.Vector2iVector(np.array([[0, 1]]))\n )\n except KeyError:\n pass # Frame is not connected to the reference frame\n\n for frame, obj in self.visuals.items():\n A2B = self.tm.get_transform(frame, self.frame)\n obj.set_data(A2B)\n\n for frame, obj in self.collision_objects.items():\n A2B = self.tm.get_transform(frame, self.frame)\n obj.set_data(A2B)\n\n @property\n def geometries(self):\n \"\"\"Expose geometries.\n\n Returns\n -------\n geometries : list\n List of geometries that can be added to the visualizer.\n \"\"\"\n geometries = []\n if self.show_frames:\n for f in self.frames.values():\n geometries += f.geometries\n if self.show_connections:\n geometries += list(self.connections.values())\n for obj in self.visuals.values():\n geometries += obj.geometries\n for obj in self.collision_objects.values():\n geometries += obj.geometries\n return geometries\n\n\ndef _objects_to_artists(objects, convex_hull=False):\n \"\"\"Convert geometries from URDF to artists.\n\n Parameters\n ----------\n objects : list of Geometry\n Objects parsed from URDF.\n\n convex_hull : bool, optional (default: False)\n Compute convex hull for each object.\n\n Returns\n -------\n artists : dict\n Mapping from frame names to artists.\n \"\"\"\n artists = {}\n for obj in objects:\n if obj.color is None:\n color = None\n else:\n # we loose the alpha channel as it is not supported by Open3D\n color = (obj.color[0], obj.color[1], obj.color[2])\n try:\n if isinstance(obj, urdf.Sphere):\n artist = Sphere(radius=obj.radius, c=color)\n elif isinstance(obj, urdf.Box):\n artist = Box(obj.size, c=color)\n elif isinstance(obj, urdf.Cylinder):\n artist = Cylinder(obj.length, obj.radius, c=color)\n else:\n assert isinstance(obj, urdf.Mesh)\n artist = Mesh(\n obj.filename, s=obj.scale, c=color, convex_hull=convex_hull\n )\n artists[obj.frame] = artist\n except RuntimeError as e:\n warnings.warn(str(e), RuntimeWarning, stacklevel=1)\n return artists\n" }, { "path": "pytransform3d/visualizer/_artists.pyi", "content": "import typing\nfrom typing import List, Union, Any, Dict\n\nimport numpy.typing as npt\nimport open3d as o3d\n\nfrom ..transform_manager import TransformManager\n\nif typing.TYPE_CHECKING:\n from ._figure import Figure\n\nclass Artist:\n def add_artist(self, figure: Figure): ...\n @property\n def geometries(self) -> List[o3d.geometry.Geometry3D]: ...\n\nclass Line3D(Artist):\n def __init__(self, P: npt.ArrayLike, c: npt.ArrayLike = ...): ...\n def set_data(\n self, P: npt.ArrayLike, c: Union[None, npt.ArrayLike] = ...\n ): ...\n @property\n def geometries(self) -> List[o3d.geometry.Geometry3D]: ...\n\nclass PointCollection3D(Artist):\n def __init__(\n self,\n P: npt.ArrayLike,\n s: float = ...,\n c: Union[None, npt.ArrayLike] = ...,\n ): ...\n def set_data(self, P: npt.ArrayLike): ...\n @property\n def geometries(self) -> List[o3d.geometry.Geometry3D]: ...\n\nclass Vector3D(Artist):\n def __init__(\n self,\n start: npt.ArrayLike,\n direction: npt.ArrayLike,\n c: Union[None, npt.ArrayLike] = ...,\n ): ...\n def set_data(\n self,\n start: npt.ArrayLike,\n direction: npt.ArrayLike,\n c: npt.ArrayLike = ...,\n ): ...\n @property\n def geometries(self) -> List[o3d.geometry.Geometry3D]: ...\n\nclass Frame(Artist):\n def __init__(\n self, A2B: npt.ArrayLike, label: Union[None, str] = ..., s: float = ...\n ): ...\n def set_data(self, A2B: npt.ArrayLike, label: Union[None, str] = ...): ...\n @property\n def geometries(self) -> List[o3d.geometry.Geometry3D]: ...\n\nclass Trajectory(Artist):\n def __init__(\n self,\n H: npt.ArrayLike,\n n_frames: int = ...,\n s: float = ...,\n c: npt.ArrayLike = ...,\n ): ...\n def set_data(self, H: npt.ArrayLike): ...\n @property\n def geometries(self) -> List[o3d.geometry.Geometry3D]: ...\n\nclass Sphere(Artist):\n def __init__(\n self,\n radius: float = ...,\n A2B: npt.ArrayLike = ...,\n resolution: int = ...,\n c: Union[None, npt.ArrayLike] = ...,\n ): ...\n def set_data(self, A2B: npt.ArrayLike): ...\n @property\n def geometries(self) -> List[o3d.geometry.Geometry3D]: ...\n\nclass Box(Artist):\n def __init__(\n self,\n size: npt.ArrayLike = ...,\n A2B: npt.ArrayLike = ...,\n c: Union[None, npt.ArrayLike] = ...,\n ): ...\n def set_data(self, A2B: npt.ArrayLike): ...\n @property\n def geometries(self) -> List[o3d.geometry.Geometry3D]: ...\n\nclass Cylinder(Artist):\n def __init__(\n self,\n length: float = ...,\n radius: float = ...,\n A2B: npt.ArrayLike = ...,\n resolution: int = ...,\n split: int = ...,\n c: Union[None, npt.ArrayLike] = ...,\n ): ...\n def set_data(self, A2B: npt.ArrayLike): ...\n @property\n def geometries(self) -> List[o3d.geometry.Geometry3D]: ...\n\nclass Mesh(Artist):\n def __init__(\n self,\n filename: str,\n A2B: npt.ArrayLike = ...,\n s: npt.ArrayLike = ...,\n c: Union[None, npt.ArrayLike] = ...,\n convex_hull: bool = ...,\n ): ...\n def set_data(self, A2B: npt.ArrayLike): ...\n @property\n def geometries(self) -> List[o3d.geometry.Geometry3D]: ...\n\nclass Ellipsoid(Artist):\n def __init__(\n self,\n radii: npt.ArrayLike,\n A2B: npt.ArrayLike = ...,\n resolution: int = ...,\n c: Union[None, npt.ArrayLike] = ...,\n ): ...\n def set_data(self, A2B: npt.ArrayLike): ...\n @property\n def geometries(self) -> List[o3d.geometry.Geometry3D]: ...\n\nclass Capsule(Artist):\n def __init__(\n self,\n height: float = ...,\n radius: float = ...,\n A2B: npt.ArrayLike = ...,\n resolution: int = ...,\n c: Union[None, npt.ArrayLike] = ...,\n ): ...\n def set_data(self, A2B: npt.ArrayLike): ...\n @property\n def geometries(self) -> List[o3d.geometry.Geometry3D]: ...\n\nclass Cone(Artist):\n def __init__(\n self,\n height: float = ...,\n radius: float = ...,\n A2B: npt.ArrayLike = ...,\n resolution: int = ...,\n c: Union[None, npt.ArrayLike] = ...,\n ): ...\n def set_data(self, A2B: npt.ArrayLike): ...\n @property\n def geometries(self) -> List[o3d.geometry.Geometry3D]: ...\n\nclass Plane(Artist):\n def __init__(\n self,\n normal: npt.ArrayLike = ...,\n d: Union[None, float] = ...,\n point_in_plane: Union[None, npt.ArrayLike] = ...,\n s: float = ...,\n c: Union[None, npt.ArrayLike] = ...,\n ): ...\n def set_data(\n self,\n normal: npt.ArrayLike,\n d: Union[None, float] = ...,\n point_in_plane: Union[None, npt.ArrayLike] = ...,\n s: float = ...,\n c: Union[None, npt.ArrayLike] = ...,\n ): ...\n @property\n def geometries(self) -> List[o3d.geometry.Geometry3D]: ...\n\nclass Camera(Artist):\n def __init__(\n self,\n M: npt.ArrayLike,\n cam2world: Union[None, npt.ArrayLike] = ...,\n virtual_image_distance: float = ...,\n sensor_size: npt.ArrayLike = ...,\n strict_check: bool = ...,\n ): ...\n def set_data(\n self,\n M: Union[None, npt.ArrayLike],\n cam2world: Union[None, npt.ArrayLike],\n virtual_image_distance: Union[None, float],\n sensor_size: Union[None, npt.ArrayLike],\n ): ...\n @property\n def geometries(self) -> List[o3d.geometry.Geometry3D]: ...\n\nclass Graph(Artist):\n def __init__(\n self,\n tm: TransformManager,\n frame: str,\n show_frames: bool = ...,\n show_connections: bool = ...,\n show_visuals: bool = ...,\n show_collision_objects: bool = ...,\n show_name: bool = ...,\n whitelist: Union[None, List[str]] = ...,\n convex_hull_of_collision_objects: bool = ...,\n s: float = ...,\n ): ...\n def set_data(self): ...\n @property\n def geometries(self) -> List[o3d.geometry.Geometry3D]: ...\n\ndef _objects_to_artists(objects: List[Any]) -> Dict[str, Artist]: ...\n" }, { "path": "pytransform3d/visualizer/_figure.py", "content": "\"\"\"Figure based on Open3D's visualizer.\"\"\"\n\nimport numpy as np\nimport open3d as o3d\n\nfrom ._artists import (\n Line3D,\n PointCollection3D,\n Vector3D,\n Frame,\n Trajectory,\n Camera,\n Box,\n Sphere,\n Cylinder,\n Mesh,\n Ellipsoid,\n Capsule,\n Cone,\n Plane,\n Graph,\n)\nfrom .. import rotations as pr\nfrom .. import trajectories as ptr\nfrom .. import transformations as pt\n\n\nclass Figure:\n \"\"\"The top level container for all the plot elements.\n\n You can close the visualizer with the keys `escape` or `q`.\n\n Parameters\n ----------\n window_name : str, optional (default: Open3D)\n Window title name.\n\n width : int, optional (default: 1920)\n Width of the window.\n\n height : int, optional (default: 1080)\n Height of the window.\n\n with_key_callbacks : bool, optional (default: False)\n Creates a visualizer that allows to register callbacks\n for keys.\n \"\"\"\n\n def __init__(\n self,\n window_name=\"Open3D\",\n width=1920,\n height=1080,\n with_key_callbacks=False,\n ):\n if with_key_callbacks:\n self.visualizer = o3d.visualization.VisualizerWithKeyCallback()\n else:\n self.visualizer = o3d.visualization.Visualizer()\n self.visualizer.create_window(\n window_name=window_name, width=width, height=height\n )\n\n def add_geometry(self, geometry):\n \"\"\"Add geometry to visualizer.\n\n Parameters\n ----------\n geometry : Geometry\n Open3D geometry.\n \"\"\"\n self.visualizer.add_geometry(geometry)\n\n def _remove_geometry(self, geometry):\n \"\"\"Remove geometry to visualizer.\n\n .. warning::\n\n This function is not public because the interface of the\n underlying visualizer might change in the future causing the\n signature of this function to change as well.\n\n Parameters\n ----------\n geometry : Geometry\n Open3D geometry.\n \"\"\"\n self.visualizer.remove_geometry(geometry)\n\n def update_geometry(self, geometry):\n \"\"\"Indicate that geometry has been updated.\n\n Parameters\n ----------\n geometry : Geometry\n Open3D geometry.\n \"\"\"\n self.visualizer.update_geometry(geometry)\n\n def remove_artist(self, artist):\n \"\"\"Remove artist from visualizer.\n\n Parameters\n ----------\n artist : Artist\n Artist that should be removed from this figure.\n \"\"\"\n for g in artist.geometries:\n self._remove_geometry(g)\n\n def set_line_width(self, line_width):\n \"\"\"Set render option line width.\n\n Note: this feature does not work in Open3D's visualizer at the\n moment.\n\n Parameters\n ----------\n line_width : float\n Line width.\n \"\"\"\n self.visualizer.get_render_option().line_width = line_width\n self.visualizer.update_renderer()\n\n def set_zoom(self, zoom):\n \"\"\"Set zoom.\n\n Parameters\n ----------\n zoom : float\n Zoom of the visualizer.\n \"\"\"\n self.visualizer.get_view_control().set_zoom(zoom)\n\n def animate(self, callback, n_frames, loop=False, fargs=()):\n \"\"\"Make animation with callback.\n\n Parameters\n ----------\n callback : callable\n Callback that will be called in a loop to update geometries.\n The first input of the function will be the current frame\n index from [0, `n_frames`). Further arguments can be given as\n `fargs`. The function should return one artist object or a\n list of artists that have been updated.\n\n n_frames : int\n Total number of frames.\n\n loop : bool, optional (default: False)\n Run callback in an infinite loop.\n\n fargs : list, optional (default: [])\n Arguments that will be passed to the callback.\n\n Raises\n ------\n RuntimeError\n When callback does not return any artists\n \"\"\"\n initialized = False\n window_open = True\n while window_open and (loop or not initialized):\n for i in range(n_frames):\n drawn_artists = callback(i, *fargs)\n\n if drawn_artists is None:\n raise RuntimeError(\n \"The animation function must return a \"\n \"sequence of Artist objects.\"\n )\n try:\n drawn_artists = [a for a in drawn_artists]\n except TypeError:\n drawn_artists = [drawn_artists]\n\n for a in drawn_artists:\n for geometry in a.geometries:\n self.update_geometry(geometry)\n\n window_open = self.visualizer.poll_events()\n if not window_open:\n break\n self.visualizer.update_renderer()\n initialized = True\n\n def view_init(self, azim=-60, elev=30):\n \"\"\"Set the elevation and azimuth of the axes.\n\n Parameters\n ----------\n azim : float, optional (default: -60)\n Azimuth angle in the x,y plane in degrees.\n\n elev : float, optional (default: 30)\n Elevation angle in the z plane.\n \"\"\"\n vc = self.visualizer.get_view_control()\n pcp = vc.convert_to_pinhole_camera_parameters()\n distance = np.linalg.norm(pcp.extrinsic[:3, 3])\n R_azim_elev_0_world2camera = np.array(\n [[0, 1, 0], [0, 0, -1], [-1, 0, 0]]\n )\n R_azim_elev_0_camera2world = R_azim_elev_0_world2camera.T\n # azimuth and elevation are defined in world frame\n R_azim = pr.active_matrix_from_angle(2, np.deg2rad(azim))\n R_elev = pr.active_matrix_from_angle(1, np.deg2rad(-elev))\n R_elev_azim_camera2world = R_azim.dot(R_elev).dot(\n R_azim_elev_0_camera2world\n )\n pcp.extrinsic = pt.transform_from( # world2camera\n R=R_elev_azim_camera2world.T, p=[0, 0, distance]\n )\n try:\n vc.convert_from_pinhole_camera_parameters(pcp, allow_arbitrary=True)\n except TypeError:\n vc.convert_from_pinhole_camera_parameters(pcp)\n\n def plot(self, P, c=(0, 0, 0)):\n \"\"\"Plot line.\n\n Parameters\n ----------\n P : array-like, shape (n_points, 3)\n Points of which the line consists.\n\n c : array-like, shape (n_points - 1, 3) or (3,), optional (default: black)\n Color can be given as individual colors per line segment or\n as one color for each segment. A color is represented by 3\n values between 0 and 1 indicate representing red, green, and\n blue respectively.\n\n Returns\n -------\n line : Line3D\n New line.\n \"\"\"\n line3d = Line3D(P, c)\n line3d.add_artist(self)\n return line3d\n\n def scatter(self, P, s=0.05, c=None):\n \"\"\"Plot collection of points.\n\n Parameters\n ----------\n P : array, shape (n_points, 3)\n Points\n\n s : float, optional (default: 0.05)\n Scaling of the spheres that will be drawn.\n\n c : array-like, shape (3,) or (n_points, 3), optional (default: black)\n A color is represented by 3 values between 0 and 1 indicate\n representing red, green, and blue respectively.\n\n Returns\n -------\n point_collection : PointCollection3D\n New point collection.\n \"\"\"\n point_collection = PointCollection3D(P, s, c)\n point_collection.add_artist(self)\n return point_collection\n\n def plot_vector(\n self, start=np.zeros(3), direction=np.array([1, 0, 0]), c=(0, 0, 0)\n ):\n \"\"\"Plot vector.\n\n Parameters\n ----------\n start : array-like, shape (3,), optional (default: [0, 0, 0])\n Start of the vector\n\n direction : array-like, shape (3,), optional (default: [1, 0, 0])\n Direction of the vector\n\n c : array-like, shape (3,), optional (default: black)\n A color is represented by 3 values between 0 and 1 indicate\n representing red, green, and blue respectively.\n\n Returns\n -------\n vector : Vector3D\n New vector.\n \"\"\"\n vector3d = Vector3D(start, direction, c)\n vector3d.add_artist(self)\n return vector3d\n\n def plot_basis(self, R=None, p=np.zeros(3), s=1.0, strict_check=True):\n \"\"\"Plot basis.\n\n Parameters\n ----------\n R : array-like, shape (3, 3), optional (default: I)\n Rotation matrix, each column contains a basis vector\n\n p : array-like, shape (3,), optional (default: [0, 0, 0])\n Offset from the origin\n\n s : float, optional (default: 1)\n Scaling of the frame that will be drawn\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the rotation matrix is not numerically\n close enough to a real rotation matrix. Otherwise we print a\n warning.\n\n Returns\n -------\n Frame : frame\n New frame.\n \"\"\"\n if R is None:\n R = np.eye(3)\n R = pr.check_matrix(R, strict_check=strict_check)\n\n frame = Frame(pt.transform_from(R=R, p=p), s=s)\n frame.add_artist(self)\n\n return frame\n\n def plot_transform(self, A2B=None, s=1.0, name=None, strict_check=True):\n \"\"\"Plot coordinate frame.\n\n Parameters\n ----------\n A2B : array-like, shape (4, 4)\n Transform from frame A to frame B\n\n s : float, optional (default: 1)\n Length of basis vectors\n\n name : str, optional (default: None)\n Name of the frame\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the transformation matrix is not\n numerically close enough to a real transformation matrix.\n Otherwise we print a warning.\n\n Returns\n -------\n Frame : frame\n New frame.\n \"\"\"\n if A2B is None:\n A2B = np.eye(4)\n A2B = pt.check_transform(A2B, strict_check=strict_check)\n\n frame = Frame(A2B, name, s)\n frame.add_artist(self)\n\n return frame\n\n def plot_trajectory(self, P, n_frames=10, s=1.0, c=(0, 0, 0)):\n \"\"\"Trajectory of poses.\n\n Parameters\n ----------\n P : array-like, shape (n_steps, 7), optional (default: None)\n Sequence of poses represented by positions and quaternions in\n the order (x, y, z, w, vx, vy, vz) for each step\n\n n_frames : int, optional (default: 10)\n Number of frames that should be plotted to indicate the\n rotation\n\n s : float, optional (default: 1)\n Scaling of the frames that will be drawn\n\n c : array-like, shape (3,), optional (default: black)\n A color is represented by 3 values between 0 and 1 indicate\n representing red, green, and blue respectively.\n\n Returns\n -------\n trajectory : Trajectory\n New trajectory.\n \"\"\"\n H = ptr.transforms_from_pqs(P)\n trajectory = Trajectory(H, n_frames, s, c)\n trajectory.add_artist(self)\n return trajectory\n\n def plot_sphere(self, radius=1.0, A2B=np.eye(4), resolution=20, c=None):\n \"\"\"Plot sphere.\n\n Parameters\n ----------\n radius : float, optional (default: 1)\n Radius of the sphere\n\n A2B : array-like, shape (4, 4)\n Transform from frame A to frame B\n\n resolution : int, optional (default: 20)\n The resolution of the sphere. The longitudes will be split into\n resolution segments (i.e. there are resolution + 1 latitude\n lines including the north and south pole). The latitudes will\n be split into 2 * resolution segments (i.e. there are\n 2 * resolution longitude lines.)\n\n c : array-like, shape (3,), optional (default: None)\n Color\n\n Returns\n -------\n sphere : Sphere\n New sphere.\n \"\"\"\n sphere = Sphere(radius, A2B, resolution, c)\n sphere.add_artist(self)\n return sphere\n\n def plot_box(self, size=np.ones(3), A2B=np.eye(4), c=None):\n \"\"\"Plot box.\n\n Parameters\n ----------\n size : array-like, shape (3,), optional (default: [1, 1, 1])\n Size of the box per dimension\n\n A2B : array-like, shape (4, 4), optional (default: I)\n Center of the box\n\n c : array-like, shape (3,), optional (default: None)\n Color\n\n Returns\n -------\n box : Box\n New box.\n \"\"\"\n box = Box(size, A2B, c)\n box.add_artist(self)\n return box\n\n def plot_cylinder(\n self,\n length=2.0,\n radius=1.0,\n A2B=np.eye(4),\n resolution=20,\n split=4,\n c=None,\n ):\n \"\"\"Plot cylinder.\n\n Parameters\n ----------\n length : float, optional (default: 1)\n Length of the cylinder.\n\n radius : float, optional (default: 1)\n Radius of the cylinder.\n\n A2B : array-like, shape (4, 4)\n Pose of the cylinder. The position corresponds to the center of the\n line segment and the z-axis to the direction of the line segment.\n\n resolution : int, optional (default: 20)\n The circle will be split into resolution segments\n\n split : int, optional (default: 4)\n The height will be split into split segments\n\n c : array-like, shape (3,), optional (default: None)\n Color\n\n Returns\n -------\n cylinder : Cylinder\n New cylinder.\n \"\"\"\n cylinder = Cylinder(length, radius, A2B, resolution, split, c)\n cylinder.add_artist(self)\n return cylinder\n\n def plot_mesh(self, filename, A2B=np.eye(4), s=np.ones(3), c=None):\n \"\"\"Plot mesh.\n\n Parameters\n ----------\n filename : str\n Path to mesh file\n\n A2B : array-like, shape (4, 4)\n Center of the mesh\n\n s : array-like, shape (3,), optional (default: [1, 1, 1])\n Scaling of the mesh that will be drawn\n\n c : array-like, shape (n_vertices, 3) or (3,), optional (default: None)\n Color(s)\n\n Returns\n -------\n mesh : Mesh\n New mesh.\n \"\"\"\n mesh = Mesh(filename, A2B, s, c)\n mesh.add_artist(self)\n return mesh\n\n def plot_ellipsoid(\n self, radii=np.ones(3), A2B=np.eye(4), resolution=20, c=None\n ):\n \"\"\"Plot ellipsoid.\n\n Parameters\n ----------\n radii : array-like, shape (3,)\n Radii along the x-axis, y-axis, and z-axis of the ellipsoid.\n\n A2B : array-like, shape (4, 4)\n Transform from frame A to frame B\n\n resolution : int, optional (default: 20)\n The resolution of the ellipsoid. The longitudes will be split into\n resolution segments (i.e. there are resolution + 1 latitude\n lines including the north and south pole). The latitudes will\n be split into 2 * resolution segments (i.e. there are\n 2 * resolution longitude lines.)\n\n c : array-like, shape (3,), optional (default: None)\n Color\n\n Returns\n -------\n ellipsoid : Ellipsoid\n New ellipsoid.\n \"\"\"\n ellipsoid = Ellipsoid(radii, A2B, resolution, c)\n ellipsoid.add_artist(self)\n return ellipsoid\n\n def plot_capsule(\n self, height=1, radius=1, A2B=np.eye(4), resolution=20, c=None\n ):\n \"\"\"Plot capsule.\n\n A capsule is the volume covered by a sphere moving along a line\n segment.\n\n Parameters\n ----------\n height : float, optional (default: 1)\n Height of the capsule along its z-axis.\n\n radius : float, optional (default: 1)\n Radius of the capsule.\n\n A2B : array-like, shape (4, 4)\n Pose of the capsule. The position corresponds to the center of the\n line segment and the z-axis to the direction of the line segment.\n\n resolution : int, optional (default: 20)\n The resolution of the capsule. The longitudes will be split into\n resolution segments (i.e. there are resolution + 1 latitude lines\n including the north and south pole). The latitudes will be split\n into 2 * resolution segments (i.e. there are 2 * resolution\n longitude lines.)\n\n c : array-like, shape (3,), optional (default: None)\n Color\n\n Returns\n -------\n capsule : Capsule\n New capsule.\n \"\"\"\n capsule = Capsule(height, radius, A2B, resolution, c)\n capsule.add_artist(self)\n return capsule\n\n def plot_cone(\n self, height=1, radius=1, A2B=np.eye(4), resolution=20, c=None\n ):\n \"\"\"Plot cone.\n\n Parameters\n ----------\n height : float, optional (default: 1)\n Height of the cone along its z-axis.\n\n radius : float, optional (default: 1)\n Radius of the cone.\n\n A2B : array-like, shape (4, 4)\n Pose of the cone.\n\n resolution : int, optional (default: 20)\n The circle will be split into resolution segments.\n\n c : array-like, shape (3,), optional (default: None)\n Color\n\n Returns\n -------\n cone : Cone\n New cone.\n \"\"\"\n cone = Cone(height, radius, A2B, resolution, c)\n cone.add_artist(self)\n return cone\n\n def plot_plane(\n self,\n normal=np.array([0.0, 0.0, 1.0]),\n d=None,\n point_in_plane=None,\n s=1.0,\n c=None,\n ):\n \"\"\"Plot plane.\n\n Parameters\n ----------\n normal : array-like, shape (3,), optional (default: [0, 0, 1])\n Plane normal.\n\n d : float, optional (default: None)\n Distance to origin in Hesse normal form.\n\n point_in_plane : array-like, shape (3,), optional (default: None)\n Point in plane.\n\n s : float, optional (default: 1)\n Scaling of the plane that will be drawn.\n\n c : array-like, shape (3,), optional (default: None)\n Color.\n\n Returns\n -------\n plane : Plane\n New plane.\n \"\"\"\n plane = Plane(normal, d, point_in_plane, s, c)\n plane.add_artist(self)\n return plane\n\n def plot_graph(\n self,\n tm,\n frame,\n show_frames=False,\n show_connections=False,\n show_visuals=False,\n show_collision_objects=False,\n show_name=False,\n whitelist=None,\n convex_hull_of_collision_objects=False,\n s=1.0,\n ):\n \"\"\"Plot graph of connected frames.\n\n Parameters\n ----------\n tm : TransformManager\n Representation of the graph\n\n frame : str\n Name of the base frame in which the graph will be displayed\n\n show_frames : bool, optional (default: False)\n Show coordinate frames\n\n show_connections : bool, optional (default: False)\n Draw lines between frames of the graph\n\n show_visuals : bool, optional (default: False)\n Show visuals that are stored in the graph\n\n show_collision_objects : bool, optional (default: False)\n Show collision objects that are stored in the graph\n\n show_name : bool, optional (default: False)\n Show names of frames\n\n whitelist : list, optional (default: all)\n List of frames that should be displayed\n\n convex_hull_of_collision_objects : bool, optional (default: False)\n Show convex hull of collision objects.\n\n s : float, optional (default: 1)\n Scaling of the frames that will be drawn\n\n Returns\n -------\n graph : Graph\n New graph.\n \"\"\"\n graph = Graph(\n tm,\n frame,\n show_frames,\n show_connections,\n show_visuals,\n show_collision_objects,\n show_name,\n whitelist,\n convex_hull_of_collision_objects,\n s,\n )\n graph.add_artist(self)\n return graph\n\n def plot_camera(\n self,\n M,\n cam2world=None,\n virtual_image_distance=1,\n sensor_size=(1920, 1080),\n strict_check=True,\n ):\n \"\"\"Plot camera in world coordinates.\n\n This function is inspired by Blender's camera visualization. It will\n show the camera center, a virtual image plane, and the top of the\n virtual image plane.\n\n Parameters\n ----------\n M : array-like, shape (3, 3)\n Intrinsic camera matrix that contains the focal lengths on the\n diagonal and the center of the the image in the last column. It\n does not matter whether values are given in meters or pixels as\n long as the unit is the same as for the sensor size.\n\n cam2world : array-like, shape (4, 4), optional (default: I)\n Transformation matrix of camera in world frame. We assume that the\n position is given in meters.\n\n virtual_image_distance : float, optional (default: 1)\n Distance from pinhole to virtual image plane that will be\n displayed. We assume that this distance is given in meters. The\n unit has to be consistent with the unit of the position in\n cam2world.\n\n sensor_size : array-like, shape (2,), optional (default: [1920, 1080])\n Size of the image sensor: (width, height). It does not matter\n whether values are given in meters or pixels as long as the unit is\n the same as for the sensor size.\n\n strict_check : bool, optional (default: True)\n Raise a ValueError if the transformation matrix is not numerically\n close enough to a real transformation matrix. Otherwise we print a\n warning.\n\n Returns\n -------\n camera : Camera\n New camera.\n \"\"\"\n camera = Camera(\n M, cam2world, virtual_image_distance, sensor_size, strict_check\n )\n camera.add_artist(self)\n return camera\n\n def save_image(self, filename):\n \"\"\"Save rendered image to file.\n\n Parameters\n ----------\n filename : str\n Path to file in which the rendered image should be stored\n \"\"\"\n self.visualizer.capture_screen_image(filename, True)\n\n def show(self):\n \"\"\"Display the figure window.\"\"\"\n self.visualizer.run()\n self.visualizer.destroy_window()\n\n\ndef figure(\n window_name=\"Open3D\", width=1920, height=1080, with_key_callbacks=False\n):\n \"\"\"Create a new figure.\n\n Parameters\n ----------\n window_name : str, optional (default: Open3D)\n Window title name.\n\n width : int, optional (default: 1920)\n Width of the window.\n\n height : int, optional (default: 1080)\n Height of the window.\n\n with_key_callbacks : bool, optional (default: False)\n Creates a visualizer that allows to register callbacks\n for keys.\n\n Returns\n -------\n figure : Figure\n New figure.\n \"\"\"\n return Figure(window_name, width, height, with_key_callbacks)\n" }, { "path": "pytransform3d/visualizer/_figure.pyi", "content": "from typing import Callable, Tuple, Any, Union, List\n\nimport numpy.typing as npt\nimport open3d as o3d\n\nfrom ._artists import (\n Artist,\n Frame,\n Trajectory,\n Sphere,\n Box,\n Cylinder,\n Mesh,\n Ellipsoid,\n Capsule,\n Cone,\n Plane,\n Graph,\n Camera,\n)\nfrom ..transform_manager import TransformManager\n\nclass Figure:\n def __init__(\n self,\n window_name: str = ...,\n width: int = ...,\n height: int = ...,\n with_key_callbacks: bool = ...,\n ): ...\n def add_geometry(self, geometry: o3d.geometry.Geometry3D): ...\n def _remove_geometry(self, geometry: o3d.geometry.Geometry3D): ...\n def update_geometry(self, geometry: o3d.geometry.Geometry3D): ...\n def remove_artist(self, artist: Artist): ...\n def set_line_width(self, line_width: float): ...\n def set_zoom(self, zoom: float): ...\n def animate(\n self,\n callback: Callable,\n n_frames: int,\n loop: bool = ...,\n fargs: Tuple[Any, ...] = ...,\n ): ...\n def view_init(self, azim: float = ..., elev: float = ...): ...\n def plot(self, P: npt.ArrayLike, c: npt.ArrayLike = ...): ...\n def scatter(\n self,\n P: npt.ArrayLike,\n s: float = ...,\n c: Union[None, npt.ArrayLike] = ...,\n ): ...\n def plot_vector(\n self,\n start: npt.ArrayLike,\n direction: npt.ArrayLike,\n c: npt.ArrayLike = ...,\n ): ...\n def plot_basis(\n self,\n R: Union[None, npt.ArrayLike] = ...,\n p: npt.ArrayLike = ...,\n s: float = ...,\n strict_check: bool = ...,\n ) -> Frame: ...\n def plot_transform(\n self,\n A2B: Union[None, npt.ArrayLike] = ...,\n s: float = ...,\n name: Union[str, None] = ...,\n strict_check: bool = ...,\n ) -> Frame: ...\n def plot_trajectory(\n self,\n P: npt.ArrayLike,\n n_frames: int = ...,\n s: float = ...,\n c: npt.ArrayLike = ...,\n ) -> Trajectory: ...\n def plot_sphere(\n self,\n radius: float = ...,\n A2B: npt.ArrayLike = ...,\n resolution: int = ...,\n c: Union[None, npt.ArrayLike] = ...,\n ) -> Sphere: ...\n def plot_box(\n self,\n size: npt.ArrayLike = ...,\n A2B: npt.ArrayLike = ...,\n c: Union[None, npt.ArrayLike] = ...,\n ) -> Box: ...\n def plot_cylinder(\n self,\n length: float = ...,\n radius: float = ...,\n A2B: npt.ArrayLike = ...,\n resolution: int = ...,\n split: int = ...,\n c: Union[None, npt.ArrayLike] = ...,\n ) -> Cylinder: ...\n def plot_mesh(\n self,\n filename: str,\n A2B: npt.ArrayLike = ...,\n s: npt.ArrayLike = ...,\n c: Union[None, npt.ArrayLike] = ...,\n ) -> Mesh: ...\n def plot_ellipsoid(\n self,\n radii: npt.ArrayLike = ...,\n A2B: npt.ArrayLike = ...,\n resolution: int = ...,\n c: Union[None, npt.ArrayLike] = ...,\n ) -> Ellipsoid: ...\n def plot_capsule(\n self,\n height: float = ...,\n radius: float = ...,\n A2B: npt.ArrayLike = ...,\n resolution: int = ...,\n c: Union[None, npt.ArrayLike] = ...,\n ) -> Capsule: ...\n def plot_cone(\n self,\n height: float = ...,\n radius: float = ...,\n A2B: npt.ArrayLike = ...,\n resolution: int = ...,\n c: Union[None, npt.ArrayLike] = ...,\n ) -> Cone: ...\n def plot_plane(\n self,\n normal: npt.ArrayLike = ...,\n d: Union[None, float] = ...,\n point_in_plane: Union[None, npt.ArrayLike] = ...,\n s: float = ...,\n c: Union[None, npt.ArrayLike] = ...,\n ) -> Plane: ...\n def plot_graph(\n self,\n tm: TransformManager,\n frame: str,\n show_frames: bool = ...,\n show_connections: bool = ...,\n show_visuals: bool = ...,\n show_collision_objects: bool = ...,\n show_name: bool = ...,\n whitelist: Union[None, List[str]] = ...,\n convex_hull_of_collision_objects: bool = ...,\n s: float = ...,\n ) -> Graph: ...\n def plot_camera(\n self,\n M: npt.ArrayLike,\n cam2world: Union[None, npt.ArrayLike] = ...,\n virtual_image_distance: float = ...,\n sensor_size: npt.ArrayLike = ...,\n strict_check: bool = ...,\n ) -> Camera: ...\n def save_image(self, filename: str): ...\n def show(self): ...\n\ndef figure(\n window_name: str = ...,\n width: int = ...,\n height: int = ...,\n with_key_callbacks: bool = ...,\n) -> Figure: ...\n" }, { "path": "requirements.txt", "content": "numpy\nscipy\nmatplotlib\nlxml\ntrimesh\npycollada\npydot\nopen3d\n" }, { "path": "setup.cfg", "content": "[nosetests]\ncover-package=pytransform3d\nwith-doctest=true\nwith-coverage=true\ncover-html=true\n[tool:pytest]\naddopts=--cov-config=.coveragerc --cov=pytransform3d --cov-report html -W ignore::DeprecationWarning pytransform3d\n" }, { "path": "setup.py", "content": "#!/usr/bin/env python\nfrom setuptools import setup, find_packages\nimport pytransform3d\n\n\nif __name__ == \"__main__\":\n with open(\"README.md\", \"r\") as f:\n long_description = f.read()\n setup(name=\"pytransform3d\",\n version=pytransform3d.__version__,\n author='Alexander Fabisch',\n author_email='afabisch@googlemail.com',\n url='https://github.com/dfki-ric/pytransform3d',\n description='3D transformations for Python',\n long_description=long_description,\n long_description_content_type=\"text/markdown\",\n classifiers=[\n \"Programming Language :: Python :: 2\",\n \"Programming Language :: Python :: 3\",\n \"License :: OSI Approved :: BSD License\",\n \"Operating System :: OS Independent\",\n \"Topic :: Scientific/Engineering :: Mathematics\",\n \"Topic :: Scientific/Engineering :: Visualization\",\n ],\n license='BSD-3-Clause',\n packages=find_packages(),\n install_requires=[\"numpy\", \"scipy\", \"matplotlib\", \"lxml\"],\n extras_require={\n \"all\": [\"pydot\", \"trimesh\", \"pycollada\", \"open3d\"],\n \"doc\": [\"numpydoc\", \"sphinx\", \"sphinx-gallery\",\n \"pydata-sphinx-theme\", \"sphinxcontrib.video\"],\n \"test\": [\"pytest\", \"pytest-cov\"]\n }\n )\n" }, { "path": "test/test_data/reconstruction_camera_matrix.csv", "content": "1601.9552, 0.0, 945.55145\n0.0, 1601.9552, 718.12787\n0.0, 0.0, 1.0" }, { "path": "test/test_data/reconstruction_odometry.csv", "content": "x, y, z, qx, qy, qz, qw\n0.0, 0.0, 0.0, -0.47900817, 0.6978452, -0.4390308, -0.30135536\n0.0, 0.0, 0.0, -0.47900817, 0.6978452, -0.4390308, -0.30135536\n0.0, 0.0, 0.0, -0.47900817, 0.6978452, -0.4390308, -0.30135536\n0.0, 0.0, 0.0, -0.47900817, 0.6978452, -0.4390308, -0.30135536\n0.0, 0.0, 0.0, -0.48147768, 0.6965384, -0.43960878, -0.29959577\n0.0, 0.0, 0.0, -0.48411176, 0.6957959, -0.4396104, -0.2970631\n0.0, 0.0, 0.0, -0.48698434, 0.69521743, -0.43942723, -0.29397732\n0.0, 0.0, 0.0, -0.4908087, 0.69465774, -0.4391291, -0.2893493\n0.0, 0.0, 0.0, -0.49357116, 0.6944391, -0.43876722, -0.28570113\n0.0, 0.0, 0.0, -0.49614492, 0.69449973, -0.43813822, -0.28203756\n0.0, 0.0, 0.0, -0.49855366, 0.69487345, -0.4371126, -0.27843776\n0.0, 0.0, 0.0, 0.501627, -0.69583046, 0.43502498, 0.27375808\n0.0, 0.0, 0.0, 0.5039393, -0.69676524, 0.43308696, 0.2701833\n0.0, 0.0, 0.0, 0.50638056, -0.6977974, 0.43088353, 0.26645204\n0.0, 0.0, 0.0, 0.50989676, -0.69924563, 0.42748955, 0.2613684\n0.0, 0.0, 0.0, 0.5127074, -0.7002776, 0.4247525, 0.25754157\n0.0, 0.0, 0.0, 0.515675, -0.7011822, 0.42196944, 0.2537019\n0.0, 0.0, 0.0, 0.51994044, -0.7020727, 0.4183925, 0.2484021\n0.0, 0.0, 0.0, 0.5233885, -0.7023825, 0.41603673, 0.24420619\n0.0, 0.0, 0.0, 0.5270112, -0.70244205, 0.4139716, 0.23971182\n0.0, 0.0, 0.0, 0.53202796, -0.7022791, 0.41143218, 0.233396\n0.0, 0.0, 0.0, 0.5358095, -0.7020097, 0.4096565, 0.2286306\n0.0, 0.0, 0.0, 0.5395391, -0.7015894, 0.4081023, 0.22388013\n0.0, 0.0, 0.0, 0.5444844, -0.70083135, 0.4062577, 0.21754695\n0.0, 0.0, 0.0, 0.54816055, -0.7000532, 0.40515965, 0.21281716\n0.0, 0.0, 0.0, 0.55169463, -0.6990378, 0.4045015, 0.2082249\n0.0, 0.0, 0.0, 0.55518293, -0.69781166, 0.4042168, 0.20356664\n0.0, 0.0, 0.0, 0.55995613, -0.6961375, 0.40374106, 0.1970655\n0.0, 0.0, 0.0, 0.56351745, -0.6948757, 0.40329278, 0.19222546\n0.0, 0.0, 0.0, 0.56695974, -0.69358385, 0.40285558, 0.18763117\n0.0, 0.0, 0.0, 0.57124156, -0.6918035, 0.40256435, 0.18174969\n0.0, 0.0, 0.0, 0.57424724, -0.6903653, 0.40269834, 0.1773975\n0.0, 0.0, 0.0, 0.5771148, -0.6888815, 0.40305543, 0.17299388\n0.0, 0.0, 0.0, 0.580495, -0.68701404, 0.40376082, 0.1673748\n0.0, 0.0, 0.0, 0.5824233, -0.6857676, 0.40458274, 0.16376396\n0.0, 0.0, 0.0, 0.5838427, -0.68468356, 0.40552488, 0.16088991\n0.0, 0.0, 0.0, 0.58536637, -0.68356895, 0.40639102, 0.15787958\n0.0, 0.0, 0.0, 0.5865285, -0.68293786, 0.40658978, 0.15577173\n0.0, 0.0, 0.0, 0.58786064, -0.68237334, 0.40645206, 0.15356812\n0.0, 0.0, 0.0, 0.5894297, -0.68177325, 0.40609556, 0.15114339\n0.0, 0.0, 0.0, 0.5918106, -0.68087566, 0.40542492, 0.1476453\n0.0, 0.0, 0.0, 0.59370595, -0.6801143, 0.4048617, 0.14506792\n0.0, 0.0, 0.0, 0.59545404, -0.67924446, 0.40455785, 0.14280835\n0.0, 0.0, 0.0, 0.59723514, -0.6779324, 0.40489537, 0.14063233\n0.0, 0.0, 0.0, 0.59808624, -0.6769252, 0.40557697, 0.13990146\n0.0, 0.0, 0.0, 0.59856373, -0.6760633, 0.40627474, 0.14000225\n0.0, 0.0, 0.0, 0.59901625, -0.67530245, 0.4065998, 0.14079304\n0.0, 0.0, 0.0, 0.599484, -0.6749438, 0.40627968, 0.14144456\n0.0, 0.0, 0.0, 0.5999988, -0.6747316, 0.4055674, 0.14231572\n0.0, 0.0, 0.0, 0.60015184, -0.6751722, 0.40396944, 0.14411432\n0.0, 0.0, 0.0, 0.59965354, -0.67621815, 0.4022964, 0.14595243\n0.0, 0.0, 0.0, 0.5993234, -0.67709816, 0.40056533, 0.14797615\n0.0, 0.0, 0.0, 0.5983897, -0.67878985, 0.3978674, 0.15124667\n0.0, 0.0, 0.0, 0.59736574, -0.6803849, 0.39567387, 0.15385929\n0.0, 0.0, 0.0, 0.5966173, -0.68174165, 0.39358315, 0.15610418\n0.0, 0.0, 0.0, 0.59621245, -0.6828864, 0.3915588, 0.15772969\n0.0, 0.0, 0.0, 0.59626675, -0.6840274, 0.389069, 0.1587382\n0.0, 0.0, 0.0, 0.5968434, -0.6845544, 0.38729215, 0.15864396\n0.0, 0.0, 0.0, 0.5977512, -0.6849857, 0.38543516, 0.15788513\n0.0, 0.0, 0.0, 0.5991555, -0.6854156, 0.38308966, 0.15639825\n0.0, 0.0, 0.0, 0.6002076, -0.6856665, 0.38147166, 0.15521483\n0.0, 0.0, 0.0, 0.60116756, -0.6859394, 0.37990543, 0.1541315\n0.0, 0.0, 0.0, 0.60205036, -0.6862335, 0.37841853, 0.15303047\n0.0, 0.0, 0.0, 0.60327816, -0.68657255, 0.37648502, 0.1514351\n0.0, 0.0, 0.0, 0.60422873, -0.6868257, 0.37498215, 0.15022093\n0.0, 0.0, 0.0, 0.6053425, -0.68722165, 0.37307993, 0.1486546\n0.0, 0.0, 0.0, 0.6061194, -0.687433, 0.37195602, 0.14732233\n0.0, 0.0, 0.0, 0.6069242, -0.687459, 0.37127098, 0.1456056\n0.0, 0.0, 0.0, 0.6078877, -0.68728036, 0.37090755, 0.14333837\n0.0, 0.0, 0.0, 0.6095787, -0.68676287, 0.37054825, 0.13951522\n0.0, 0.0, 0.0, 0.6111343, -0.68620473, 0.3702026, 0.13633713\n0.0, 0.0, 0.0, 0.6127308, -0.6856092, 0.36973673, 0.13340046\n0.0, 0.0, 0.0, 0.6147629, -0.6848444, 0.3689829, 0.13002408\n0.0, 0.0, 0.0, 0.61621845, -0.6843221, 0.3683, 0.12780151\n0.0, 0.0, 0.0, 0.617558, -0.683731, 0.3678676, 0.12572768\n0.0, 0.0, 0.0, 0.6192527, -0.6827331, 0.36782208, 0.12291636\n0.0, 0.0, 0.0, 0.6206096, -0.681946, 0.36774793, 0.12064337\n0.0, 0.0, 0.0, 0.6220439, -0.68118757, 0.3674911, 0.11830101\n0.0, 0.0, 0.0, 0.6240476, -0.6802366, 0.36680213, 0.11532117\n0.0, 0.0, 0.0, 0.62551314, -0.6797153, 0.36584365, 0.11348488\n0.0, 0.0, 0.0, 0.62677425, -0.6794832, 0.3645255, 0.11215108\n7.450581e-09, 7.450581e-09, 3.7252903e-09, 0.62548697, -0.6764093, 0.36948106, 0.12132669\n0.0028046146, 0.001567781, 0.0013065189, 0.62593263, -0.6766347, 0.36829233, 0.12138598\n0.0059238, 0.0028972104, 0.0025103241, 0.6260785, -0.676805, 0.36756715, 0.12188167\n0.009311347, 0.004115081, 0.0037264745, 0.6260929, -0.6769722, 0.3670563, 0.12241699\n0.012883923, 0.0053333254, 0.0050433944, 0.6262336, -0.6773039, 0.3661009, 0.12272303\n0.016638517, 0.0065570846, 0.006512668, 0.6265552, -0.6778797, 0.36452198, 0.12260287\n0.01673872, 0.003168425, 0.005734041, 0.62697494, -0.67819804, 0.36336714, 0.12212307\n0.01973583, 0.0024399785, 0.0065159025, 0.6268914, -0.67909485, 0.36181223, 0.12218529\n0.023840725, 0.0023979843, 0.0078021996, 0.6261733, -0.68007904, 0.36090901, 0.123061314\n0.028298095, 0.0026322603, 0.009385433, 0.6253509, -0.6811341, 0.36010763, 0.12375412\n0.032787725, 0.0031247586, 0.011272538, 0.62494385, -0.6821253, 0.35902768, 0.12348895\n0.037272707, 0.0037860898, 0.013417189, 0.624946, -0.68306285, 0.35762137, 0.12237113\n0.041818894, 0.004460244, 0.015693977, 0.6251455, -0.6839592, 0.35601723, 0.12101524\n0.04585798, 0.0046609715, 0.018724333, 0.6267213, -0.68381137, 0.35386586, 0.120006055\n0.05048956, 0.004867859, 0.021280397, 0.6267297, -0.6846113, 0.35254344, 0.119290486\n0.05537515, 0.0050262944, 0.02366931, 0.6266089, -0.6850836, 0.35199365, 0.11883639\n0.06031783, 0.0052325055, 0.026069801, 0.6266511, -0.68522966, 0.35187474, 0.11812233\n0.0652494, 0.005552575, 0.028587114, 0.6270079, -0.6852048, 0.35169685, 0.116895944\n0.06978834, 0.006512766, 0.03039236, 0.62793386, -0.6847005, 0.3516811, 0.114911124\n0.074708745, 0.0070446357, 0.032713976, 0.62869877, -0.68469274, 0.35067412, 0.11384735\n0.0798357, 0.0073456317, 0.035104085, 0.62923056, -0.6848551, 0.3495688, 0.11333177\n0.08505254, 0.007554746, 0.037483536, 0.6296681, -0.6850911, 0.34839356, 0.113092884\n0.09033543, 0.0077198893, 0.039889667, 0.6299896, -0.6854926, 0.34708747, 0.11288568\n0.09573636, 0.007818667, 0.04231205, 0.63010406, -0.6860649, 0.345773, 0.11280416\n0.100577645, 0.007839231, 0.044237517, 0.63035715, -0.6858744, 0.3460606, 0.11166088\n0.10600169, 0.007729262, 0.046429526, 0.6302122, -0.68645024, 0.34505913, 0.11203948\n0.11161738, 0.0075497837, 0.048668355, 0.6299617, -0.6871256, 0.34402356, 0.11249083\n0.117286116, 0.0072958367, 0.050910342, 0.6295405, -0.68790185, 0.34306264, 0.1130382\n0.12299399, 0.0069648772, 0.053159397, 0.6288906, -0.688804, 0.34222704, 0.11369378\n0.12871508, 0.0066192, 0.055431146, 0.6282218, -0.68972635, 0.34138468, 0.11432977\n0.13369647, 0.0063555166, 0.05803233, 0.62809193, -0.68975115, 0.34206977, 0.11283677\n0.13903534, 0.006149709, 0.06049152, 0.6280258, -0.69051474, 0.34057936, 0.11304221\n0.14443067, 0.0059915395, 0.06292351, 0.62806284, -0.6913358, 0.33883217, 0.11306908\n0.14981225, 0.005847618, 0.06530817, 0.6282028, -0.69216573, 0.33685088, 0.11313472\n0.15518685, 0.0056429254, 0.06756631, 0.62832373, -0.69291025, 0.33497116, 0.11348496\n0.16051853, 0.005364783, 0.06966032, 0.62841165, -0.69349176, 0.33336532, 0.11417258\n0.16529238, 0.0050693676, 0.071020484, 0.6288426, -0.6931029, 0.33373812, 0.11306773\n0.17027688, 0.00479836, 0.07275352, 0.62924236, -0.6932548, 0.33243474, 0.11374914\n0.17533523, 0.004541083, 0.07462713, 0.6297286, -0.6933599, 0.3311469, 0.1141736\n0.18044561, 0.0042380095, 0.07654257, 0.63018674, -0.6934681, 0.32992294, 0.11453163\n0.18559085, 0.0038993163, 0.07846738, 0.63055444, -0.693576, 0.32889318, 0.114815705\n0.19077533, 0.0035539716, 0.08040774, 0.630874, -0.69370246, 0.32799062, 0.11487855\n0.1945079, 0.002066499, 0.0821079, 0.63079554, -0.69388235, 0.3281129, 0.11386904\n0.19932035, 0.0012092069, 0.08390927, 0.63066375, -0.69427663, 0.32755423, 0.11380489\n0.20451114, 0.00049579144, 0.0856902, 0.6301897, -0.69488937, 0.32710832, 0.11397312\n0.2098149, -0.00015059253, 0.08745253, 0.6296865, -0.69560075, 0.32649663, 0.11416806\n0.21507862, -0.00072203577, 0.08924295, 0.6292976, -0.69644505, 0.3254272, 0.11421852\n0.21950117, -0.0018376261, 0.09082343, 0.6287516, -0.6976388, 0.32423043, 0.11334076\n0.22429925, -0.0024457276, 0.092588365, 0.6285937, -0.69855356, 0.32272458, 0.11287928\n0.22914046, -0.0028234795, 0.09443043, 0.62866443, -0.69930595, 0.32125235, 0.112022266\n0.233944, -0.003119871, 0.09626341, 0.628965, -0.699852, 0.3197933, 0.1110958\n0.23872936, -0.003474371, 0.09794414, 0.6292164, -0.700321, 0.3183655, 0.11081793\n0.24349317, -0.0039250776, 0.09940849, 0.6293077, -0.7007481, 0.3170469, 0.111378446\n0.24817099, -0.004405548, 0.1007003, 0.6294485, -0.7010003, 0.31585607, 0.11237536\n0.25254413, -0.005269359, 0.10218075, 0.6296524, -0.7010061, 0.31523708, 0.11293227\n0.2568304, -0.005671568, 0.10354875, 0.6303583, -0.70055926, 0.314776, 0.11305401\n0.2609919, -0.0058382913, 0.10489491, 0.6314586, -0.6997798, 0.31443596, 0.11268622\n0.2650517, -0.0059321523, 0.10614723, 0.63276815, -0.6987465, 0.3142657, 0.11222603\n0.26901427, -0.006017886, 0.107290804, 0.6340909, -0.6974737, 0.3146026, 0.11173282\n0.27286038, -0.0061034723, 0.1083456, 0.63533795, -0.69606304, 0.31536642, 0.11129158\n0.2768396, -0.0073539317, 0.10992965, 0.63616484, -0.69501084, 0.31616813, 0.11086842\n0.2804555, -0.0078346925, 0.11098195, 0.6373871, -0.6935999, 0.31678286, 0.110930115\n0.28381833, -0.008060083, 0.11182417, 0.6386758, -0.6921227, 0.31734562, 0.111135535\n0.28694415, -0.00816966, 0.112562865, 0.6401261, -0.6905848, 0.3177049, 0.11133217\n0.28985423, -0.0082798395, 0.11318925, 0.64155275, -0.6892062, 0.31764746, 0.11182522\n0.29258412, -0.008526444, 0.113639966, 0.64255786, -0.6882925, 0.3171187, 0.113174595\n0.29688686, -0.010466282, 0.11475565, 0.64267075, -0.68848246, 0.31557617, 0.115662456\n0.2997967, -0.011479236, 0.11511473, 0.6427508, -0.6885153, 0.31425688, 0.11857719\n0.3020257, -0.012149649, 0.115225255, 0.64271724, -0.6887398, 0.3127083, 0.12151391\n0.3038012, -0.012656333, 0.11528218, 0.64263844, -0.68913, 0.31096745, 0.12415657\n0.30521166, -0.0130476, 0.11535856, 0.6426068, -0.68958014, 0.30917308, 0.12628432\n0.30632442, -0.01335606, 0.11546281, 0.6426324, -0.6899769, 0.30758873, 0.12784582\n0.30859727, -0.014423506, 0.11657925, 0.64215714, -0.6907656, 0.30596069, 0.12986554\n0.30967715, -0.014989564, 0.11695792, 0.6421029, -0.69099575, 0.30513713, 0.13084362\n0.31017056, -0.01539731, 0.11704308, 0.6420592, -0.6910951, 0.3046144, 0.1317481\n0.31030974, -0.015752295, 0.11699557, 0.64198476, -0.69116104, 0.30429497, 0.13250086\n0.31017387, -0.016080106, 0.11688481, 0.6419571, -0.69112366, 0.30421817, 0.13300528\n0.3097844, -0.016394667, 0.116734475, 0.6420425, -0.69096774, 0.30426967, 0.13328502\n0.31056467, -0.017278852, 0.11710709, 0.64177203, -0.69110346, 0.30417582, 0.13409626\n0.31017652, -0.017791621, 0.11697416, 0.64169323, -0.6910637, 0.3042087, 0.13460237\n0.30927864, -0.018176133, 0.11662394, 0.64149284, -0.6911684, 0.3040837, 0.13529985\n0.30808252, -0.018489167, 0.11617535, 0.64121956, -0.6914302, 0.3037631, 0.13597742\n0.30663297, -0.018736713, 0.11564048, 0.6409492, -0.6917536, 0.30330157, 0.13663493\n0.3049276, -0.018907808, 0.11502178, 0.6406936, -0.6920631, 0.30285934, 0.13724649\n0.30568826, -0.019419618, 0.1154508, 0.6402535, -0.6926034, 0.30218336, 0.1380623\n0.30441567, -0.019511834, 0.11500639, 0.6400283, -0.692861, 0.30183938, 0.13856488\n0.30229837, -0.0193733, 0.11420068, 0.6398923, -0.6928902, 0.30187222, 0.1389749\n0.2997021, -0.019041564, 0.113250725, 0.6399225, -0.6926455, 0.30230546, 0.13911417\n0.2967674, -0.01849556, 0.1122811, 0.64014846, -0.69220996, 0.30298644, 0.13876112\n0.2935429, -0.017746434, 0.11132774, 0.6405156, -0.69163436, 0.30390662, 0.13792118\n0.2908701, -0.017033631, 0.110659376, 0.64106035, -0.69101405, 0.30462644, 0.13690801\n0.2873898, -0.016035253, 0.109841034, 0.64159095, -0.69032407, 0.30572468, 0.13544798\n0.28352395, -0.0148134995, 0.10900636, 0.6422342, -0.6896543, 0.30674055, 0.13350005\n0.27945185, -0.013477187, 0.10819678, 0.64287585, -0.68907094, 0.30767673, 0.13125159\n0.27528, -0.012147563, 0.10734862, 0.64331084, -0.6886523, 0.30860463, 0.12912185\n0.27097842, -0.010826545, 0.10635696, 0.6434971, -0.6882827, 0.30981687, 0.12724717\n0.26636755, -0.009339507, 0.10525896, 0.6437774, -0.687818, 0.31113005, 0.12512055\n0.2609185, -0.008290358, 0.10480189, 0.6446157, -0.68717515, 0.31205586, 0.12199238\n0.25557458, -0.0066062957, 0.10407503, 0.6454637, -0.68670046, 0.3127242, 0.11841682\n0.25015804, -0.0047324016, 0.10319373, 0.64635116, -0.68628883, 0.31309944, 0.11491971\n0.24469116, -0.0029505268, 0.10207434, 0.6470264, -0.68596023, 0.31340408, 0.11221935\n0.23926526, -0.0014132126, 0.100661874, 0.6472197, -0.68586415, 0.31377628, 0.110640705\n0.23308718, -0.0014223754, 0.099208504, 0.6470325, -0.6859807, 0.3143105, 0.109491445\n0.22716767, -0.0005131974, 0.097595885, 0.6468646, -0.6861855, 0.31467348, 0.108148396\n0.22126898, 0.000677763, 0.09592757, 0.64664656, -0.6865516, 0.31482518, 0.10667833\n0.21536565, 0.001867339, 0.094203025, 0.6462302, -0.6872795, 0.3144888, 0.105499715\n0.20937857, 0.0029999688, 0.092359796, 0.64579254, -0.6881719, 0.31370413, 0.10469524\n0.2032609, 0.0041095987, 0.09036487, 0.6454177, -0.6890879, 0.3126567, 0.10411296\n0.19658202, 0.005015418, 0.08902684, 0.64507574, -0.69001377, 0.3114829, 0.10361783\n0.19008586, 0.006039694, 0.087118775, 0.6447722, -0.6908436, 0.31055367, 0.10276365\n0.18360734, 0.0070627034, 0.08491917, 0.6445156, -0.6915888, 0.30965516, 0.10207006\n0.17710939, 0.008012898, 0.08250384, 0.6442544, -0.6922987, 0.3087218, 0.10173306\n0.17059898, 0.008878335, 0.07997221, 0.643893, -0.69302136, 0.3079557, 0.101421736\n0.16405168, 0.009708224, 0.07743412, 0.6434304, -0.6937537, 0.30753702, 0.1006175\n0.15741941, 0.010572955, 0.07494986, 0.64292616, -0.6945728, 0.30723777, 0.09909286\n0.14949654, 0.0112205325, 0.072654136, 0.6424492, -0.69538885, 0.30705014, 0.09702424\n0.14233401, 0.011893205, 0.07011676, 0.64172995, -0.6963952, 0.30677146, 0.09543368\n0.13547695, 0.012405612, 0.06732209, 0.64074236, -0.69743085, 0.30670863, 0.094706446\n0.1287756, 0.012713946, 0.0642494, 0.63956916, -0.69839, 0.30690572, 0.09492913\n0.122103006, 0.012900114, 0.06100126, 0.63827455, -0.6992615, 0.30739975, 0.09562562\n0.11455705, 0.012712656, 0.057795063, 0.6369716, -0.70008564, 0.3080935, 0.09605\n0.10741571, 0.01279374, 0.054575466, 0.6358167, -0.7008645, 0.30866033, 0.09620216\n0.10037307, 0.012949472, 0.05131696, 0.6347103, -0.7016115, 0.3091605, 0.09645642\n0.093366906, 0.013089597, 0.04798829, 0.633675, -0.70232195, 0.30948865, 0.097039275\n0.0863412, 0.013144799, 0.044562805, 0.63269275, -0.70299596, 0.30963725, 0.09808764\n0.079249844, 0.013124332, 0.041051414, 0.631705, -0.70359755, 0.309873, 0.0993883\n0.071293116, 0.012372069, 0.037725728, 0.63078433, -0.7040522, 0.31045085, 0.100209296\n0.06368184, 0.012243986, 0.03433169, 0.6300344, -0.7044017, 0.3110195, 0.10070655\n0.05610752, 0.012374029, 0.031007063, 0.6294761, -0.7046916, 0.3114374, 0.100877486\n0.048476562, 0.012614422, 0.02782264, 0.62914336, -0.7048935, 0.3116956, 0.10074548\n0.04071483, 0.012904838, 0.02468967, 0.62896776, -0.7049512, 0.31196454, 0.10060553\n0.03282357, 0.013198724, 0.021516494, 0.6287517, -0.7049354, 0.31241032, 0.10068341\n0.024074674, 0.013118416, 0.018298272, 0.6285022, -0.70468736, 0.3134626, 0.10070753\n0.015586197, 0.013368696, 0.015050981, 0.6284845, -0.7041639, 0.31466976, 0.10071403\n0.007045835, 0.013712725, 0.011790961, 0.62869185, -0.70352715, 0.31565177, 0.10079672\n-0.0016343668, 0.014012486, 0.008505654, 0.6289551, -0.70284426, 0.31650174, 0.10125161\n-0.010493457, 0.014250606, 0.005192753, 0.62925094, -0.7021428, 0.31723502, 0.10198379\n-0.019553855, 0.014457434, 0.0018966831, 0.62952095, -0.7015379, 0.31778324, 0.10276953\n-0.028582722, 0.014031507, -0.0011266582, 0.62973464, -0.7011603, 0.31795487, 0.10350441\n-0.037934475, 0.013992327, -0.0042618355, 0.6299089, -0.7009474, 0.3177878, 0.10439556\n-0.04750879, 0.014074234, -0.0073920493, 0.6300791, -0.7007776, 0.31756574, 0.10518107\n-0.05732148, 0.014225341, -0.0104961395, 0.63040346, -0.7005654, 0.31718358, 0.10580234\n-0.067347266, 0.014407232, -0.013542477, 0.6308591, -0.70033485, 0.3166788, 0.10612442\n-0.07755612, 0.014564998, -0.016559094, 0.6313229, -0.70009774, 0.31622338, 0.10628888\n-0.087637246, 0.014260732, -0.019376718, 0.6317287, -0.6997971, 0.31599227, 0.10654422\n-0.0981157, 0.014182724, -0.022409577, 0.6320073, -0.6993933, 0.3161789, 0.106988505\n-0.10889543, 0.014210582, -0.025508156, 0.6323197, -0.69890106, 0.3165225, 0.107343\n-0.11995356, 0.014299601, -0.0285486, 0.6327652, -0.698268, 0.3170609, 0.1072491\n-0.13130543, 0.014460216, -0.03148369, 0.63344514, -0.6975003, 0.3176096, 0.10660559\n-0.14289747, 0.014654322, -0.03439179, 0.6343141, -0.69668305, 0.3179646, 0.105720215\n-0.15437411, 0.014670387, -0.037221886, 0.63512397, -0.69586176, 0.318361, 0.10507201\n-0.1661765, 0.014616353, -0.040249713, 0.6356622, -0.69501954, 0.31922212, 0.10477806\n-0.17819872, 0.01448445, -0.04337024, 0.63601625, -0.6941502, 0.3204275, 0.104713686\n-0.19041955, 0.014316733, -0.046513047, 0.6363096, -0.69326514, 0.3217747, 0.104664005\n-0.20282558, 0.014160655, -0.04963863, 0.6366405, -0.69244564, 0.32293114, 0.1045144\n-0.21540567, 0.014026794, -0.052745692, 0.63714755, -0.6917352, 0.32353446, 0.104263365\n-0.22781527, 0.013744405, -0.0557397, 0.63782, -0.6911334, 0.3235785, 0.10400527\n-0.24049923, 0.013489291, -0.058856837, 0.6382977, -0.6906172, 0.32376522, 0.1039227\n-0.25327307, 0.013184264, -0.06204373, 0.6384867, -0.6901914, 0.32425046, 0.10407811\n-0.26606017, 0.012853755, -0.06522711, 0.6384502, -0.68991005, 0.32487786, 0.10421074\n-0.27886114, 0.012544898, -0.068359196, 0.6382523, -0.6897819, 0.32553822, 0.10420976\n-0.2915836, 0.012063876, -0.07109907, 0.638121, -0.6897661, 0.32593134, 0.103889935\n-0.3044477, 0.011917828, -0.07388878, 0.63819015, -0.689853, 0.3258977, 0.10298954\n-0.3173656, 0.0119378045, -0.07661848, 0.63840383, -0.6900382, 0.3255508, 0.1015112\n-0.33023205, 0.011978483, -0.07933018, 0.6384376, -0.6903765, 0.3252939, 0.09980716\n-0.3429991, 0.011977129, -0.082100995, 0.6382335, -0.69078916, 0.32530087, 0.09822264\n-0.35567334, 0.011931159, -0.084938556, 0.63785386, -0.69118357, 0.32562116, 0.09684278\n-0.36824906, 0.011845125, -0.08785358, 0.6373796, -0.6916038, 0.325955, 0.0958369\n-0.38045377, 0.011413664, -0.09094691, 0.63665336, -0.69213563, 0.3262736, 0.09574097\n-0.3926324, 0.010999812, -0.09415298, 0.63563627, -0.6927223, 0.32674795, 0.09663373\n-0.40468585, 0.010594001, -0.097409375, 0.6344991, -0.6933292, 0.32725814, 0.09801816\n-0.41658536, 0.010216971, -0.10065884, 0.6333673, -0.69400424, 0.32755512, 0.09955675\n-0.42826843, 0.009839554, -0.10383883, 0.63225484, -0.6947247, 0.32772133, 0.101045474\n-0.43967825, 0.009390667, -0.10694589, 0.6311582, -0.6954976, 0.32771868, 0.1025814\n-0.45080173, 0.008864149, -0.10997732, 0.6301168, -0.6962091, 0.32774013, 0.10407722\n-0.46183908, 0.0085133985, -0.112899676, 0.62915385, -0.69679457, 0.32796362, 0.10527418\n-0.4725953, 0.008136615, -0.11573054, 0.6284082, -0.6973134, 0.328091, 0.10589397\n-0.48315895, 0.007812707, -0.11850922, 0.6280389, -0.69762033, 0.32810926, 0.1060062\n-0.4934864, 0.007477291, -0.121313915, 0.6278712, -0.69781244, 0.32800487, 0.10605808\n-0.5035238, 0.007075444, -0.12419677, 0.6276119, -0.69796544, 0.32805383, 0.10643382\n-0.5132544, 0.0065618875, -0.12721202, 0.62707645, -0.6981657, 0.3283537, 0.1073476\n-0.522453, 0.00570876, -0.1304742, 0.62632054, -0.6983606, 0.32894313, 0.108680286\n-0.5315839, 0.004968323, -0.13375105, 0.62540793, -0.6985813, 0.32968408, 0.11026079\n-0.54053664, 0.004283987, -0.1370013, 0.62451935, -0.6988486, 0.33031058, 0.111718856\n-0.5492577, 0.0036191046, -0.14023048, 0.6237775, -0.6992332, 0.33044863, 0.113040835\n-0.5577471, 0.0029186667, -0.14349154, 0.6231577, -0.6997195, 0.33014196, 0.11433859\n-0.56598705, 0.0022431388, -0.14657265, 0.6226094, -0.7001919, 0.32974455, 0.11557417\n-0.5741228, 0.0014746711, -0.14989395, 0.621993, -0.7004005, 0.33013013, 0.11652429\n-0.5821998, 0.0007132143, -0.15329957, 0.6215302, -0.700195, 0.3312494, 0.117050976\n-0.59021544, -2.8626531e-05, -0.1567479, 0.62126833, -0.69985116, 0.33236483, 0.11733506\n-0.5981252, -0.0007678047, -0.16023965, 0.6210766, -0.6996021, 0.33321193, 0.117433354\n-0.60590786, -0.0015426725, -0.16382794, 0.6208333, -0.69934565, 0.33416894, 0.11752817\n-0.6135916, -0.002805598, -0.16770774, 0.6206731, -0.6989652, 0.33520445, 0.1176881\n-0.62118626, -0.0038526282, -0.17169558, 0.6204968, -0.69846964, 0.33650383, 0.11785182\n-0.6287083, -0.0048751608, -0.17584862, 0.62032783, -0.6978451, 0.338021, 0.11809879\n-0.6361815, -0.0058840094, -0.18015309, 0.6201439, -0.69717705, 0.3396428, 0.1183563\n-0.6436294, -0.0068553463, -0.1846169, 0.6200031, -0.696522, 0.34112158, 0.11869819\n-0.6510451, -0.007753344, -0.18919687, 0.62000483, -0.6958534, 0.34240246, 0.11892216\n-0.6584404, -0.00937931, -0.19398585, 0.62030077, -0.6951336, 0.34329635, 0.11901216\n-0.6657707, -0.01041922, -0.19883308, 0.6205631, -0.69449586, 0.34409165, 0.11907073\n-0.67305964, -0.011194609, -0.2037261, 0.62089324, -0.6938103, 0.34490603, 0.118990615\n-0.6803121, -0.011813633, -0.20867364, 0.6213621, -0.69321245, 0.34534112, 0.1187649\n-0.6875181, -0.012341484, -0.21374308, 0.621857, -0.6927304, 0.3454347, 0.118715\n-0.6946863, -0.012778476, -0.21893454, 0.6223026, -0.6923683, 0.3453195, 0.11882754\n-0.70184916, -0.013510443, -0.2240998, 0.6229718, -0.6919491, 0.34492108, 0.118919894\n-0.7089824, -0.013926702, -0.22943461, 0.6236076, -0.69181097, 0.34398025, 0.11911613\n-0.7160384, -0.014256746, -0.23487943, 0.62421006, -0.69180584, 0.3427927, 0.11941388\n-0.7230795, -0.014534719, -0.24043202, 0.62478805, -0.691944, 0.341337, 0.119759955\n-0.7301823, -0.014781708, -0.24615857, 0.6253885, -0.6922322, 0.33948943, 0.120210916\n-0.73733747, -0.01507392, -0.2521083, 0.6258859, -0.6925673, 0.3376428, 0.12089109\n-0.7450438, -0.016410388, -0.25827274, 0.6262549, -0.69284976, 0.3360504, 0.12179511\n-0.75232106, -0.01732927, -0.26483014, 0.62608534, -0.6934065, 0.3345255, 0.12368146\n-0.759427, -0.018218767, -0.27175254, 0.62564504, -0.6939791, 0.33322203, 0.12619159\n-0.7664925, -0.019151421, -0.27889982, 0.62500906, -0.69445145, 0.33243135, 0.12880264\n-0.7735602, -0.020124085, -0.2861842, 0.62421197, -0.69481474, 0.33219212, 0.13130234\n-0.7806176, -0.02114969, -0.29358283, 0.6231983, -0.6951763, 0.33233613, 0.13381496\n-0.7883824, -0.02273445, -0.30061704, 0.6220917, -0.6955836, 0.33247373, 0.13647902\n-0.7956933, -0.023971656, -0.3079774, 0.6206381, -0.69642615, 0.33230966, 0.13917343\n-0.80295753, -0.025005318, -0.3154797, 0.61920905, -0.6973901, 0.33187568, 0.1417244\n-0.8103268, -0.025851227, -0.323053, 0.61811954, -0.6982471, 0.33114344, 0.14395545\n-0.81780326, -0.026577743, -0.33067575, 0.6173828, -0.69889724, 0.33024165, 0.14601848\n-0.82532525, -0.02727809, -0.3383269, 0.6167359, -0.6993485, 0.32948035, 0.14829369\n-0.8328186, -0.028019983, -0.34594432, 0.6159667, -0.69980013, 0.32874617, 0.1509659\n-0.84071255, -0.02897683, -0.35339034, 0.6152072, -0.7003749, 0.32763878, 0.15377887\n-0.8484645, -0.029719353, -0.3607152, 0.6144951, -0.7011926, 0.32597107, 0.15642098\n-0.85629594, -0.030327685, -0.36785978, 0.6140171, -0.7020514, 0.3238717, 0.15878862\n-0.8642249, -0.030834764, -0.37476602, 0.61375904, -0.7029772, 0.3213797, 0.16074209\n-0.87225413, -0.031197691, -0.38139364, 0.61381155, -0.7039135, 0.31853682, 0.16209672\n-0.88092154, -0.031633183, -0.38830835, 0.6143101, -0.7047111, 0.31535822, 0.16295594\n-0.88942766, -0.03179157, -0.39487106, 0.61518055, -0.70518893, 0.3122609, 0.16356868\n-0.8979354, -0.031819716, -0.40135434, 0.6163993, -0.70503193, 0.30993354, 0.16408215\n-0.9064254, -0.03180285, -0.40786734, 0.6178049, -0.7041967, 0.3086843, 0.1647364\n-0.9148507, -0.031800624, -0.41444448, 0.6192949, -0.70290995, 0.30811974, 0.16569093\n-0.92318034, -0.031886976, -0.4211412, 0.6206912, -0.70155007, 0.30755135, 0.16727814\n-0.9319428, -0.03306754, -0.42828748, 0.62192076, -0.70015943, 0.30693662, 0.16964996\n-0.9402611, -0.033708706, -0.43530485, 0.622785, -0.6988024, 0.3066429, 0.1725809\n-0.94842464, -0.034090362, -0.4421677, 0.6236743, -0.6973148, 0.30675492, 0.17516792\n-0.9565417, -0.034239583, -0.44879907, 0.62478817, -0.69573385, 0.3071079, 0.17685795\n-0.9646512, -0.03420984, -0.45521268, 0.62617207, -0.69417983, 0.3073566, 0.17763653\n-0.9727179, -0.034102485, -0.46147075, 0.62761295, -0.69286186, 0.3072473, 0.17788601\n-0.98077255, -0.03459251, -0.4674551, 0.62897563, -0.69198126, 0.30660972, 0.17760071\n-0.9886564, -0.034742184, -0.47345984, 0.63000786, -0.69146216, 0.3057263, 0.17748685\n-0.99646294, -0.03484284, -0.47946608, 0.6309257, -0.69108033, 0.30472228, 0.1774402\n-1.0042366, -0.034994137, -0.48548496, 0.63172317, -0.6907036, 0.30388752, 0.1775016\n-1.0119612, -0.03524892, -0.49151236, 0.6323074, -0.6903222, 0.30344504, 0.17766157\n-1.0195899, -0.035611413, -0.49748448, 0.63265294, -0.69000953, 0.30334175, 0.17782205\n-1.0270493, -0.03607074, -0.50330913, 0.6328208, -0.6899859, 0.30305916, 0.17779885\n-1.0340706, -0.037225768, -0.50873935, 0.63291484, -0.6902584, 0.30243918, 0.17746206\n-1.0410439, -0.038078308, -0.5141771, 0.63267, -0.69089836, 0.30178773, 0.17695254\n-1.0479335, -0.03884113, -0.51956534, 0.632183, -0.69181514, 0.30115047, 0.17619546\n-1.0547383, -0.039580144, -0.524878, 0.631522, -0.69300544, 0.30046943, 0.1750473\n-1.0615379, -0.04031921, -0.530217, 0.6309236, -0.69410735, 0.29990068, 0.17380981\n-1.0683305, -0.04107747, -0.53566694, 0.6302736, -0.6950016, 0.29977703, 0.1728053\n-1.0743529, -0.041942507, -0.54046404, 0.62934345, -0.6958168, 0.30038798, 0.1718511\n-1.0807014, -0.042632617, -0.5457263, 0.62847227, -0.6962658, 0.30180123, 0.17074074\n-1.0870657, -0.04322789, -0.5511777, 0.6277722, -0.696378, 0.30361056, 0.16964766\n-1.0932693, -0.043806992, -0.55675113, 0.6270203, -0.6964969, 0.30531102, 0.16888666\n-1.0992188, -0.04441586, -0.562391, 0.6260326, -0.6969088, 0.30661893, 0.16848145\n-1.1051463, -0.045426153, -0.56802905, 0.6248446, -0.69761056, 0.30766645, 0.16807751\n-1.1107363, -0.046214264, -0.5737142, 0.62343174, -0.6982863, 0.3092747, 0.16756542\n-1.1161628, -0.04690022, -0.57944244, 0.62210685, -0.6986407, 0.31164804, 0.16661242\n-1.1215261, -0.04744573, -0.58523744, 0.6210778, -0.69856495, 0.31460354, 0.16520886\n-1.1268275, -0.047821015, -0.59114516, 0.6204067, -0.69816923, 0.3176387, 0.16358769\n-1.132007, -0.048051536, -0.5971948, 0.6200068, -0.69771236, 0.32021877, 0.16201489\n-1.1382043, -0.048318043, -0.6039546, 0.61984575, -0.69735706, 0.3220999, 0.16042487\n-1.1435435, -0.04842837, -0.6104332, 0.6197575, -0.697165, 0.3234351, 0.15890646\n-1.1485488, -0.048512932, -0.61691236, 0.6196826, -0.6970442, 0.3244964, 0.15755984\n-1.1534053, -0.048637386, -0.62348586, 0.61959195, -0.6969492, 0.3254229, 0.1564213\n-1.1581745, -0.048843257, -0.6301835, 0.6195051, -0.69693565, 0.32608983, 0.15543298\n-1.1628883, -0.049133442, -0.6370357, 0.61952555, -0.69703144, 0.3262332, 0.15461878\n-1.168624, -0.049003318, -0.6441874, 0.61953884, -0.69732296, 0.32589355, 0.15396628\n-1.173605, -0.04927024, -0.651315, 0.619715, -0.69767636, 0.3251473, 0.15323262\n-1.1783162, -0.04972869, -0.6584915, 0.6200629, -0.69795287, 0.32420033, 0.15257096\n-1.182919, -0.050357413, -0.6658072, 0.6204367, -0.6981318, 0.32320768, 0.15233822\n-1.1874717, -0.051133975, -0.67320967, 0.62075424, -0.69835657, 0.32205468, 0.15245679\n-1.191978, -0.05202742, -0.6806192, 0.62105525, -0.69862306, 0.32071388, 0.15283613\n-1.1977433, -0.05374855, -0.68855596, 0.6212432, -0.69899905, 0.31923452, 0.15344834\n-1.2025316, -0.055098895, -0.69593453, 0.621355, -0.69936925, 0.31794623, 0.15398283\n-1.2069595, -0.056379616, -0.7029567, 0.62134856, -0.69998586, 0.3165071, 0.15417176\n-1.2112751, -0.057651743, -0.7097144, 0.6212308, -0.7009544, 0.31475383, 0.1538348\n-1.2156396, -0.058891453, -0.7163084, 0.6210326, -0.7021733, 0.31288663, 0.15288265\n-1.2202166, -0.06007523, -0.7228669, 0.62095755, -0.7032519, 0.31125602, 0.15155232\n-1.2263527, -0.061299015, -0.72961473, 0.6209597, -0.704039, 0.31014466, 0.15016122\n-1.2317744, -0.06251502, -0.7363025, 0.6210499, -0.70423776, 0.30994582, 0.149264\n-1.2369866, -0.06377305, -0.7430714, 0.621064, -0.7038149, 0.31087735, 0.14926259\n-1.2421044, -0.06508094, -0.74989647, 0.62091434, -0.7028877, 0.3128849, 0.15005715\n-1.2471812, -0.06635024, -0.756607, 0.620829, -0.70171136, 0.31524742, 0.15096845\n-1.2522709, -0.067537814, -0.7630933, 0.6208458, -0.70063686, 0.31724042, 0.15171316\n-1.2637718, -0.06994397, -0.77693546, 0.6206552, -0.6993111, 0.31917515, 0.15452613\n-1.2692062, -0.071164444, -0.7832574, 0.6205875, -0.6988156, 0.31939146, 0.1565786\n-1.2747775, -0.0722384, -0.7894035, 0.6208899, -0.69803965, 0.31959867, 0.15840764\n-1.2805649, -0.073049955, -0.7953801, 0.62183356, -0.69676995, 0.31991252, 0.15965788\n-1.2865036, -0.073589474, -0.8011981, 0.62327015, -0.6950828, 0.3204368, 0.16035688\n-1.2926227, -0.073533095, -0.80618995, 0.6247738, -0.69325304, 0.32128352, 0.16073205\n-1.2985878, -0.073657215, -0.81147003, 0.6262343, -0.6914606, 0.3221764, 0.16098182\n-1.3044329, -0.07378999, -0.81673056, 0.6275866, -0.68977875, 0.32311, 0.16105966\n-1.3102341, -0.073935196, -0.82185245, 0.62876505, -0.6882695, 0.32416362, 0.16080312\n-1.3160344, -0.074145, -0.8268128, 0.6296543, -0.68703663, 0.32532302, 0.16025329\n-1.3218296, -0.07447151, -0.8316663, 0.6301102, -0.6862007, 0.3265076, 0.15963277\n-1.328208, -0.07539575, -0.8364149, 0.629976, -0.68589246, 0.3276224, 0.15920235\n-1.3342136, -0.07623514, -0.84115964, 0.62931836, -0.68607444, 0.32854617, 0.1591151\n-1.3401319, -0.07713272, -0.84590954, 0.62823117, -0.6865536, 0.32951155, 0.15934846\n-1.346111, -0.07809834, -0.8506179, 0.62688863, -0.68707263, 0.33085388, 0.15961675\n-1.3522385, -0.0790829, -0.85523504, 0.6255286, -0.6874885, 0.33256868, 0.15959847\n-1.3585519, -0.0800366, -0.8597402, 0.62425727, -0.687826, 0.33445773, 0.15917394\n-1.3651316, -0.08077489, -0.8637722, 0.6230718, -0.6882836, 0.336177, 0.15821587\n-1.371887, -0.081481755, -0.86782193, 0.6221639, -0.6888089, 0.3375214, 0.15663193\n-1.3788283, -0.0821165, -0.8718403, 0.621406, -0.689393, 0.33861262, 0.1547037\n-1.3859718, -0.08272067, -0.87587583, 0.6206846, -0.6898959, 0.33981064, 0.15271822\n-1.3932989, -0.083357714, -0.8799704, 0.6198524, -0.6903154, 0.34128246, 0.15090959\n-1.4007558, -0.08401651, -0.88409984, 0.61889243, -0.6907481, 0.3428912, 0.14921427\n-1.4083424, -0.08467485, -0.88824475, 0.6177783, -0.69127357, 0.34453174, 0.14760984\n-1.415599, -0.08552025, -0.89118195, 0.6163985, -0.69196486, 0.34615898, 0.14632607\n-1.423184, -0.08639033, -0.8949492, 0.61459607, -0.69281036, 0.34799883, 0.14554068\n-1.4308351, -0.087372616, -0.89892966, 0.61240166, -0.6936875, 0.35032272, 0.14503743\n-1.4385264, -0.088419504, -0.9028416, 0.6100668, -0.6945157, 0.3530489, 0.1443015\n-1.4462909, -0.089475535, -0.9066464, 0.60774505, -0.6952717, 0.3560126, 0.14317173\n-1.454283, -0.09078163, -0.9101364, 0.60518974, -0.69593495, 0.35952652, 0.14198767\n-1.4621347, -0.092109516, -0.913849, 0.60222894, -0.69629943, 0.36406264, 0.14122912\n-1.4699831, -0.09347765, -0.9176801, 0.59910834, -0.6961993, 0.3694389, 0.14103357\n-1.4778281, -0.09488496, -0.9215373, 0.59578997, -0.6959349, 0.37509814, 0.14145727\n-1.4856293, -0.09629487, -0.9252892, 0.59219974, -0.69587004, 0.3805478, 0.14229469\n-1.4933755, -0.09762798, -0.92886025, 0.58848363, -0.6961362, 0.38541546, 0.1433047\n-1.5007423, -0.098732166, -0.9320188, 0.58479196, -0.69683295, 0.3894759, 0.14405066\n-1.508081, -0.099703, -0.9349578, 0.5811727, -0.69800645, 0.39265946, 0.14437374\n-1.5152857, -0.1005619, -0.9376865, 0.57773054, -0.6994968, 0.39509082, 0.14434278\n-1.5223386, -0.10135399, -0.94029975, 0.57447505, -0.7009573, 0.39733607, 0.14408787\n-1.5292666, -0.10206765, -0.9428522, 0.5714634, -0.702078, 0.39992204, 0.14345178\n-1.5360508, -0.102715604, -0.9453805, 0.5687091, -0.70278925, 0.40297562, 0.14236514\n-1.5429409, -0.10376324, -0.94772965, 0.5662639, -0.7030131, 0.406514, 0.14093985\n-1.5494083, -0.104476415, -0.9502246, 0.5638909, -0.7029855, 0.4104076, 0.13929777\n-1.5555668, -0.105076306, -0.95283544, 0.5616709, -0.7028179, 0.41433492, 0.13747458\n-1.5614637, -0.10564593, -0.9555582, 0.5596067, -0.7025197, 0.41823202, 0.13560334\n-1.5671228, -0.106175646, -0.95837784, 0.5577499, -0.7021118, 0.4219914, 0.1337056\n-1.5725856, -0.106642045, -0.9612789, 0.5562769, -0.70167154, 0.42528746, 0.13169572\n-1.577875, -0.107030556, -0.96425486, 0.55517954, -0.70128506, 0.4280243, 0.12949951\n-1.5841856, -0.10801419, -0.96800435, 0.55444473, -0.7009887, 0.4302932, 0.1267022\n-1.5896425, -0.1085663, -0.9713915, 0.5540839, -0.7005845, 0.43231368, 0.12360114\n-1.594761, -0.10903163, -0.9747801, 0.5540006, -0.70003426, 0.43423742, 0.12030464\n-1.5996634, -0.10957913, -0.97833866, 0.553929, -0.6993308, 0.43632406, 0.117134176\n-1.6043425, -0.110312745, -0.98215705, 0.55358243, -0.698557, 0.4386859, 0.11453846\n-1.6089774, -0.11231079, -0.9860379, 0.5528614, -0.6980011, 0.4410378, 0.11235836\n-1.6132575, -0.113706306, -0.9901184, 0.55187833, -0.69761467, 0.44332066, 0.11059312\n-1.6173711, -0.1148713, -0.9942884, 0.55097306, -0.69726956, 0.44545516, 0.10868973\n-1.6213982, -0.1158927, -0.9984687, 0.5502193, -0.6969731, 0.4473949, 0.106418915\n-1.6253633, -0.11680977, -1.0026414, 0.54960775, -0.6967642, 0.44910303, 0.10371802\n-1.6292471, -0.11773279, -1.0069025, 0.54900146, -0.69665414, 0.45062846, 0.10101625\n-1.6329547, -0.1191699, -1.0110499, 0.5480886, -0.6967013, 0.45217273, 0.09872171\n-1.636476, -0.12056595, -1.0155336, 0.5468259, -0.6969659, 0.4536494, 0.09706736\n-1.6398731, -0.12199487, -1.0201609, 0.5453609, -0.6974763, 0.45495024, 0.095542856\n-1.6432062, -0.123443305, -1.024867, 0.5438344, -0.6982613, 0.45591924, 0.09387673\n-1.6464977, -0.124930255, -1.0296576, 0.5422472, -0.6993216, 0.4565404, 0.09213118\n-1.6497412, -0.12649487, -1.0345829, 0.5405608, -0.7005954, 0.45689327, 0.09060205\n-1.6543982, -0.1291207, -1.0401541, 0.53874874, -0.70206237, 0.45701784, 0.08940269\n-1.6580257, -0.13111842, -1.0454752, 0.53682804, -0.7034923, 0.4572493, 0.08852759\n-1.6613307, -0.13279088, -1.0507287, 0.5350242, -0.7047322, 0.45763326, 0.08759734\n-1.6645529, -0.1342204, -1.0559453, 0.5334042, -0.705692, 0.45830193, 0.08624344\n-1.6677554, -0.13546878, -1.0611557, 0.5318646, -0.7063983, 0.45936224, 0.08430744\n-1.67097, -0.13660432, -1.066471, 0.53031325, -0.70673573, 0.46103227, 0.08210841\n-1.6741958, -0.1376785, -1.0720218, 0.52868944, -0.70660675, 0.4634053, 0.08031075\n-1.6779921, -0.13987297, -1.0780562, 0.52689034, -0.7061905, 0.4662436, 0.07936263\n-1.681256, -0.14131477, -1.0840908, 0.52462876, -0.7059599, 0.46907723, 0.07969798\n-1.6842399, -0.14252388, -1.0901304, 0.52193284, -0.7062045, 0.47144583, 0.08124016\n-1.6870872, -0.1436187, -1.0960497, 0.5188475, -0.7068454, 0.47350323, 0.08343414\n-1.6899544, -0.14450368, -1.1017097, 0.5157024, -0.7077007, 0.47529694, 0.08546063\n-1.6943386, -0.14567283, -1.10838, 0.5129459, -0.70858985, 0.47674346, 0.08661744\n-1.6980275, -0.14599663, -1.1137401, 0.5108298, -0.709522, 0.47762287, 0.086647116\n-1.7017006, -0.1457142, -1.1184907, 0.5096482, -0.7103766, 0.47782952, 0.08545575\n-1.7055993, -0.14491554, -1.1229087, 0.5095601, -0.7108424, 0.47768876, 0.0828556\n-1.7097721, -0.14368048, -1.127119, 0.51051414, -0.7106793, 0.4775931, 0.07883582\n-1.7141438, -0.1421569, -1.1312644, 0.51220405, -0.7099019, 0.47772408, 0.073930144\n-1.7185359, -0.14055838, -1.1355145, 0.51398546, -0.7086389, 0.47840554, 0.0691225\n-1.7231383, -0.13952741, -1.1405973, 0.51512873, -0.70720387, 0.47986132, 0.065099776\n-1.7271414, -0.13842715, -1.1453598, 0.51534396, -0.7056849, 0.48219955, 0.06255392\n-1.7307858, -0.13738234, -1.1499523, 0.5148577, -0.70448774, 0.4846663, 0.060967162\n-1.7342215, -0.13637993, -1.1542974, 0.51386833, -0.70390266, 0.4867214, 0.059687164\n-1.7375808, -0.13541143, -1.1582948, 0.5126495, -0.7041024, 0.48790413, 0.058136154\n-1.7412213, -0.13497315, -1.1624424, 0.51126534, -0.7050948, 0.48815662, 0.056144785\n-1.7447999, -0.13426399, -1.1659855, 0.50989735, -0.706385, 0.4879975, 0.05369695\n-1.748482, -0.13348114, -1.1692278, 0.5084995, -0.7076091, 0.48798704, 0.050855238\n-1.7523359, -0.13269188, -1.1723443, 0.50710356, -0.7084851, 0.48848444, 0.047725048\n-1.7563614, -0.13193446, -1.17543, 0.5055766, -0.7088843, 0.4897981, 0.04442184\n-1.7605911, -0.13118365, -1.178516, 0.50390387, -0.70884025, 0.4918928, 0.04083777\n-1.7650428, -0.13103366, -1.1807948, 0.50229836, -0.708554, 0.49426368, 0.036753718\n-1.7697015, -0.13042821, -1.1835709, 0.5007533, -0.70829755, 0.4964976, 0.032418378\n-1.7744707, -0.12971342, -1.1864994, -0.49906772, 0.7082637, -0.49849743, -0.028180964\n-1.7793067, -0.12902611, -1.189491, -0.49715978, 0.7085367, -0.5002226, -0.024193544\n-1.7842011, -0.1284201, -1.1925126, -0.49497026, 0.709019, -0.5018758, -0.02042306\n-1.7891855, -0.12790799, -1.195496, -0.4924926, 0.70967484, -0.50352156, -0.016692411\n-1.7943017, -0.12741686, -1.1983852, -0.48985887, 0.7105086, -0.5050291, -0.01270802\n-1.7991167, -0.12763362, -1.2000203, -0.48710957, 0.71148133, -0.50641096, -0.008154228\n-1.804388, -0.12736124, -1.2023715, -0.48436558, 0.7121622, -0.50813884, -0.0031442135\n-1.8099365, -0.12688112, -1.2050034, -0.48170635, 0.71236616, -0.5103804, 0.0022730054\n-1.8157166, -0.12623498, -1.2078142, -0.47928587, 0.7119273, -0.5132055, 0.008043294\n-1.8216295, -0.12554069, -1.2108046, -0.47696608, 0.71102786, -0.5164846, 0.013649013\n-1.827509, -0.12493232, -1.2140299, -0.47462898, 0.7100477, -0.51982963, 0.018347815\n-1.83258, -0.12502843, -1.2159954, -0.47192207, 0.7095397, -0.52283347, 0.022091262\n-1.8379282, -0.12514517, -1.2187839, -0.4685852, 0.7096557, -0.52552533, 0.025293177\n-1.8433524, -0.12536837, -1.2217448, -0.4649766, 0.7100638, -0.5279995, 0.028680341\n-1.8488448, -0.1256653, -1.2247496, -0.46131164, 0.71034956, -0.5306025, 0.0324959\n-1.8543549, -0.12609123, -1.2278138, -0.45740366, 0.71043956, -0.53360534, 0.036369286\n-1.8591818, -0.12745407, -1.2301562, -0.4533879, 0.7104203, -0.5367917, 0.03996221\n-1.8644124, -0.12821326, -1.2331847, -0.44963047, 0.7103322, -0.5398113, 0.0431788\n-1.8697935, -0.12864365, -1.2364968, -0.44663346, 0.71019614, -0.5422155, 0.046283618\n-1.875234, -0.12887189, -1.2399429, -0.4444227, 0.71000814, -0.544009, 0.049305864\n-1.8806721, -0.12898397, -1.2434763, -0.4427092, 0.70976186, -0.54544836, 0.05227418\n-1.8860515, -0.12906273, -1.2471111, -0.44113946, 0.7094374, -0.546872, 0.055004355\n-1.8908023, -0.1299609, -1.2499156, -0.43957362, 0.7090866, -0.54832315, 0.057557438\n-1.8957523, -0.13031256, -1.2533672, -0.43804374, 0.70865554, -0.5498783, 0.059656147\n-1.9006593, -0.13051172, -1.2570581, -0.43657026, 0.7081448, -0.5515255, 0.061295606\n-1.9054753, -0.13067447, -1.2608641, -0.4351614, 0.7074991, -0.5533345, 0.06245319\n-1.9102464, -0.13078554, -1.2647085, -0.43398508, 0.70671076, -0.55514544, 0.06348494\n-1.9150444, -0.1307975, -1.2685435, -0.4333715, 0.70576715, -0.5566798, 0.064725764\n-1.9197794, -0.1303999, -1.2719738, -0.43349344, 0.7048387, -0.55758184, 0.06624435\n-1.9244913, -0.13029155, -1.2756386, -0.43396792, 0.70418924, -0.557848, 0.06778627\n-1.9290748, -0.13037619, -1.2793883, -0.43450245, 0.7038338, -0.5577325, 0.06899258\n-1.9335127, -0.13057235, -1.2831795, -0.43493286, 0.7036158, -0.55757344, 0.06978541\n-1.937826, -0.13079047, -1.2869701, -0.4353245, 0.7034442, -0.5574111, 0.0703677\n-1.9420397, -0.13098952, -1.2907337, -0.43582103, 0.7032574, -0.5571868, 0.07093554\n-1.9452947, -0.13156483, -1.2939812, -0.43643102, 0.703027, -0.5569179, 0.071578294\n-1.9490405, -0.13187654, -1.2975358, -0.43720052, 0.7027139, -0.55660856, 0.07235784\n-1.9528774, -0.13210672, -1.3011869, -0.43805167, 0.7024384, -0.5561848, 0.0731396\n-1.9566147, -0.13231719, -1.3048699, -0.43881097, 0.70210683, -0.5559476, 0.0735745\n-1.9601831, -0.13253549, -1.3085517, -0.43936825, 0.7016342, -0.5561087, 0.07353856\n-1.9636297, -0.13271764, -1.312166, -0.43993092, 0.7011061, -0.556345, 0.07342526\n-1.9668174, -0.13376594, -1.3157275, -0.44080004, 0.7007341, -0.55609494, 0.07365778\n-1.9700565, -0.13417684, -1.3190432, -0.4417672, 0.7007871, -0.5551889, 0.07419179\n-1.9732336, -0.13440078, -1.3221904, -0.4427626, 0.701012, -0.5540266, 0.07481949\n-1.9762975, -0.13459739, -1.3252485, -0.44359952, 0.7011049, -0.5531801, 0.0752542\n-1.979225, -0.13481544, -1.3282629, -0.444181, 0.70103574, -0.5527788, 0.07541614\n-1.9820224, -0.13505088, -1.331213, -0.44458258, 0.70090383, -0.5526189, 0.07544731\n-1.98495, -0.1350668, -1.3342552, -0.44478762, 0.70080066, -0.55257565, 0.07551472\n-1.9875932, -0.1352242, -1.3369927, -0.4448719, 0.700789, -0.5524916, 0.07574184\n-1.9901544, -0.13533087, -1.3395182, -0.4451342, 0.70073795, -0.55226034, 0.07635614\n-1.9926981, -0.13534716, -1.3419379, -0.44555828, 0.70052654, -0.5520665, 0.0772201\n-1.9951885, -0.13531736, -1.3442937, -0.4459367, 0.70020527, -0.5520469, 0.07808474\n-1.9975817, -0.13527085, -1.3465723, -0.44612363, 0.6999148, -0.55215967, 0.07881971\n-1.9999384, -0.13594504, -1.3486266, -0.44637135, 0.69968957, -0.55214536, 0.07951419\n-2.002161, -0.13602242, -1.3505982, -0.44666287, 0.6995966, -0.5519284, 0.08019866\n-2.0042944, -0.1358431, -1.3524541, -0.44695497, 0.69952095, -0.5516779, 0.08095113\n-2.0063677, -0.13552058, -1.3542082, -0.44721296, 0.69942236, -0.5514665, 0.081813574\n-2.0084133, -0.13508831, -1.355877, -0.44754592, 0.69930875, -0.5511934, 0.082798734\n-2.0104094, -0.13459559, -1.3574548, -0.4480015, 0.699276, -0.5507104, 0.08381935\n-2.0124297, -0.1341286, -1.3593422, -0.4484969, 0.6993306, -0.55010515, 0.08468406\n-2.014217, -0.13369286, -1.3609251, -0.44886023, 0.69945866, -0.54956776, 0.08518974\n-2.0158749, -0.13328795, -1.3623806, -0.4491219, 0.69960976, -0.5491227, 0.08543941\n-2.017487, -0.13287647, -1.3637655, -0.44939083, 0.699685, -0.5487704, 0.08567201\n-2.0190973, -0.1324155, -1.3650718, -0.44978943, 0.6996419, -0.5484447, 0.08601747\n-2.0207024, -0.13191229, -1.3662778, -0.45031703, 0.6995609, -0.5480394, 0.08649814\n-2.0219994, -0.13155149, -1.3680182, -0.45089242, 0.6995753, -0.54747623, 0.08694948\n-2.0233955, -0.13117096, -1.3693199, -0.4514485, 0.6998398, -0.5466493, 0.08713807\n-2.0247905, -0.13080034, -1.370445, -0.45194685, 0.7002839, -0.5456692, 0.08712913\n-2.0261445, -0.13047081, -1.3715109, -0.45230702, 0.7007897, -0.54475474, 0.08691612\n-2.0274389, -0.13019131, -1.3725766, -0.45245993, 0.70126325, -0.54409057, 0.086459726\n-2.028684, -0.1299299, -1.3736497, -0.4524812, 0.7016561, -0.5436645, 0.08583828\n-2.0299778, -0.12994458, -1.3746587, -0.452515, 0.70204014, -0.5432343, 0.0852424\n-2.0311809, -0.12971367, -1.3756251, -0.45264044, 0.7023575, -0.5428004, 0.084724024\n-2.0323925, -0.12930948, -1.376526, -0.45294374, 0.7025233, -0.5423671, 0.08450413\n-2.0336332, -0.12877773, -1.3773735, -0.45338976, 0.702555, -0.54194987, 0.08452489\n-2.0349002, -0.12814549, -1.378163, -0.4539752, 0.7025131, -0.54147804, 0.08475411\n-2.0361762, -0.12744287, -1.3789237, -0.45472273, 0.70246434, -0.54086703, 0.085051164\n-2.0374374, -0.12670366, -1.3796891, -0.45552087, 0.7024358, -0.5401972, 0.08527388\n-2.0384517, -0.12606773, -1.38058, -0.45632693, 0.70252603, -0.5394077, 0.08521891\n-2.0395267, -0.1253757, -1.381429, -0.45708072, 0.70266813, -0.538607, 0.08507095\n-2.0405746, -0.12466467, -1.3822591, -0.4578045, 0.7028684, -0.53776586, 0.08484642\n-2.0415723, -0.12393267, -1.3830631, -0.45854488, 0.70309776, -0.5368754, 0.08458733\n-2.042534, -0.123168185, -1.3838434, -0.45928872, 0.7033757, -0.5359179, 0.08431134\n-2.0432587, -0.12191581, -1.3848467, -0.46000552, 0.7037097, -0.53489554, 0.084108435\n-2.0441444, -0.12088709, -1.385649, -0.46102512, 0.7040154, -0.53360975, 0.084135786\n-2.0450766, -0.11991449, -1.3863714, -0.46218765, 0.7042609, -0.5322453, 0.084345475\n-2.0460134, -0.11896756, -1.3870822, -0.46338293, 0.70437133, -0.531009, 0.08465558\n-2.0469322, -0.11805712, -1.3878251, -0.4645254, 0.70443, -0.52988356, 0.08495788\n-2.0477877, -0.117203206, -1.3885932, -0.4655373, 0.7045188, -0.5288452, 0.085152075\n-2.0484536, -0.11688403, -1.3893005, -0.46649346, 0.7046401, -0.5278279, 0.08522814\n-2.0491138, -0.1163395, -1.3900042, -0.46727103, 0.704787, -0.5269437, 0.0852253\n-2.0497584, -0.115736105, -1.3906808, -0.46785358, 0.7049107, -0.52626663, 0.08518997\n-2.0504127, -0.11507973, -1.3912883, -0.46836674, 0.7049867, -0.52568364, 0.085340366\n-2.051062, -0.11439026, -1.391828, -0.4688789, 0.705034, -0.52511007, 0.08566753\n-2.0516706, -0.113714874, -1.392359, -0.4693381, 0.7050939, -0.5245784, 0.08591734\n-2.0519805, -0.113010585, -1.3929893, -0.46961716, 0.70518523, -0.5242083, 0.08590192\n-2.0523965, -0.112398386, -1.393525, -0.4697634, 0.7052738, -0.5239725, 0.08581417\n-2.052857, -0.11174493, -1.3939847, -0.46995008, 0.7052675, -0.523807, 0.08585393\n-2.053376, -0.11098738, -1.3943638, -0.47035748, 0.7051586, -0.5235291, 0.08621177\n-2.0539358, -0.11014381, -1.3946838, -0.4709705, 0.70496297, -0.52314293, 0.0868068\n-2.054474, -0.10927819, -1.3949672, -0.47162062, 0.7047264, -0.52277917, 0.08738798\n-2.054632, -0.10901921, -1.3955598, -0.47216853, 0.7045469, -0.52244586, 0.08786714\n-2.0549543, -0.10840038, -1.39589, -0.47265175, 0.7043931, -0.52214086, 0.088314444\n-2.0553067, -0.10769105, -1.3961065, -0.47304022, 0.7043031, -0.52183974, 0.08873201\n-2.0556207, -0.10700963, -1.3962891, -0.4733741, 0.70432746, -0.52146065, 0.088986225\n-2.0558805, -0.10638579, -1.3964697, -0.473674, 0.7044547, -0.521009, 0.08902858\n-2.0561032, -0.10579379, -1.3966473, -0.4739729, 0.7046645, -0.52047247, 0.08891537\n-2.056013, -0.105226934, -1.3967279, -0.47434568, 0.70484644, -0.51993227, 0.08864608\n-2.0560668, -0.104621604, -1.3968021, -0.4746491, 0.7050552, -0.51940733, 0.088438846\n-2.0561614, -0.10396038, -1.3967986, -0.47495505, 0.7051834, -0.5189664, 0.08836222\n-2.0562918, -0.103218675, -1.3966844, -0.47532627, 0.7052663, -0.5184985, 0.088451356\n-2.0564482, -0.10240236, -1.3964583, -0.4757964, 0.7054342, -0.5177963, 0.0886976\n-2.056581, -0.101558246, -1.3961505, -0.47632286, 0.7058034, -0.5167662, 0.08894215\n-2.0562139, -0.1006517, -1.3958586, -0.47685274, 0.7063386, -0.51553243, 0.089014694\n-2.0560427, -0.09977273, -1.3955123, -0.47735518, 0.7069287, -0.51426965, 0.08894111\n-2.0558953, -0.098864146, -1.3951439, -0.4777849, 0.70742303, -0.51321787, 0.088777944\n-2.0557287, -0.09788605, -1.3947362, -0.47819495, 0.70772344, -0.5124372, 0.08868514\n-2.0555449, -0.096816964, -1.3942481, -0.47870898, 0.7078587, -0.5117535, 0.08878125\n-2.0551763, -0.09573003, -1.3937597, -0.47936904, 0.70791906, -0.51099646, 0.08909841\n-2.0549502, -0.09452125, -1.3931365, -0.4801483, 0.70793325, -0.5101447, 0.089669354\n-2.0547552, -0.093238614, -1.3924121, -0.48094824, 0.7079061, -0.50929165, 0.090441555\n-2.0545213, -0.09192032, -1.3915813, -0.48172805, 0.707835, -0.50849044, 0.09135239\n-2.0542107, -0.09059931, -1.39066, -0.4824064, 0.7077909, -0.50773776, 0.09229491\n-2.0538108, -0.08929488, -1.3896991, -0.48295107, 0.707854, -0.5069799, 0.09312584\n-2.0532975, -0.088005535, -1.3887255, -0.48330122, 0.7080297, -0.50628847, 0.0937328\n-2.0524426, -0.086187005, -1.3882532, -0.48349032, 0.7082654, -0.5057184, 0.09405375\n-2.0516279, -0.084643826, -1.3873633, -0.48361966, 0.708472, -0.5052525, 0.09433645\n-2.0508456, -0.08307859, -1.3862388, -0.48388797, 0.7086288, -0.5046922, 0.094781116\n-2.0500872, -0.08141726, -1.3849366, -0.48433498, 0.70883167, -0.50385785, 0.09541737\n-2.0493197, -0.07965262, -1.3834684, -0.48493853, 0.7091096, -0.5027264, 0.09625126\n-2.0484762, -0.077825464, -1.381833, -0.48555234, 0.7095134, -0.5013862, 0.0971664\n-2.0471387, -0.07614048, -1.3803791, -0.48601198, 0.7100629, -0.49998713, 0.09806025\n-2.0459404, -0.074298896, -1.3785332, -0.48641738, 0.71058506, -0.49866936, 0.09897408\n-2.0447254, -0.07239079, -1.3764671, -0.48688707, 0.7110598, -0.49733213, 0.09997768\n-2.043435, -0.07047622, -1.3742898, -0.48743013, 0.71147615, -0.4960143, 0.10091253\n-2.0420494, -0.068602815, -1.3720328, -0.48795366, 0.71181214, -0.4948362, 0.10179265\n-2.040585, -0.06676973, -1.3697053, -0.4885177, 0.7120245, -0.49378186, 0.102717824\n-2.0389209, -0.06511851, -1.3680985, -0.48913622, 0.71203405, -0.49299446, 0.10348773\n-2.0371795, -0.06346777, -1.365942, -0.48956317, 0.7120564, -0.49241713, 0.104062326\n-2.0353284, -0.06186052, -1.3635417, -0.48989254, 0.7119967, -0.49204794, 0.10466574\n-2.0333838, -0.060306605, -1.3609716, -0.49016407, 0.7118253, -0.4918425, 0.10552204\n-2.0313816, -0.058786057, -1.3582588, -0.49041656, 0.71149945, -0.49181688, 0.10665924\n-2.0293245, -0.057280127, -1.3554242, -0.49077144, 0.7111326, -0.49171412, 0.107939705\n-2.027201, -0.055938475, -1.3527508, -0.49132723, 0.71070457, -0.49149895, 0.10920241\n-2.0248954, -0.054586325, -1.3498307, -0.4917767, 0.71044487, -0.49120414, 0.11019142\n-2.022366, -0.053291764, -1.3467886, -0.4919784, 0.71035755, -0.49098888, 0.11081194\n-2.019672, -0.052016433, -1.3436264, -0.49198142, 0.7102131, -0.4910608, 0.111404344\n-2.0168884, -0.050738916, -1.3403288, -0.4919662, 0.7097814, -0.49149022, 0.112325154\n-2.0140376, -0.049504187, -1.336921, -0.4919251, 0.709234, -0.49206838, 0.113426045\n-2.010939, -0.048258215, -1.3339889, -0.49182627, 0.70892775, -0.49242288, 0.11422797\n-2.007853, -0.047098193, -1.3305895, -0.49158686, 0.70910543, -0.49226058, 0.114852965\n-2.0047145, -0.045935813, -1.3269023, -0.4913832, 0.70952576, -0.49170318, 0.115515016\n-2.0015175, -0.04472344, -1.3229648, -0.49128416, 0.70998085, -0.49093708, 0.11639499\n-1.9982456, -0.04343793, -1.3187823, -0.4912275, 0.7103498, -0.4901925, 0.117515594\n-1.9948977, -0.042089004, -1.3143489, -0.49121487, 0.7106367, -0.48945537, 0.11889828\n-1.9910502, -0.040422752, -1.3098408, -0.49108738, 0.71091163, -0.48882478, 0.120366454\n-1.9873631, -0.03894384, -1.3050816, -0.4908873, 0.7111268, -0.48835534, 0.12180873\n-1.9837122, -0.03747309, -1.3000889, -0.4906243, 0.71112615, -0.48818305, 0.12355016\n-1.9800578, -0.035954706, -1.2948945, -0.49046138, 0.710873, -0.48817575, 0.12566438\n-1.9763285, -0.034422815, -1.289545, -0.49036312, 0.7105055, -0.4882162, 0.12794884\n-1.9724368, -0.03294592, -1.2841122, -0.4901437, 0.7100625, -0.48850334, 0.13013378\n-1.9681205, -0.031842157, -1.2785045, -0.48978713, 0.7095644, -0.48912168, 0.13185902\n-1.9635911, -0.030669965, -1.2730104, -0.48920983, 0.7092029, -0.4899219, 0.13297155\n-1.9587939, -0.029528128, -1.2675369, -0.48852655, 0.70903087, -0.49070477, 0.13351327\n-1.953703, -0.028449379, -1.2620238, -0.4877697, 0.7090559, -0.4913985, 0.13359609\n-1.9482965, -0.027461926, -1.2564108, -0.48685136, 0.7092335, -0.4921104, 0.13338245\n-1.9426444, -0.026521422, -1.2506326, -0.48600036, 0.70953107, -0.49261892, 0.13302545\n-1.9363644, -0.025656752, -1.2439054, -0.48553345, 0.7099086, -0.49260816, 0.13275608\n-1.9302965, -0.024679722, -1.2374179, -0.4853928, 0.71033204, -0.4921814, 0.13258843\n-1.9242084, -0.023650065, -1.2308496, -0.48548496, 0.7106934, -0.49153948, 0.13269536\n-1.91797, -0.022642065, -1.224127, -0.48553634, 0.7109756, -0.4910075, 0.13296473\n-1.9115006, -0.021712191, -1.217274, -0.48543885, 0.7113323, -0.49055317, 0.13308999\n-1.9047765, -0.020864557, -1.2103091, -0.48528463, 0.7118736, -0.48999608, 0.13281092\n-1.8977373, -0.01958729, -1.2029964, -0.4850905, 0.7126126, -0.48926073, 0.13226657\n-1.8905764, -0.018661441, -1.1956769, -0.48490906, 0.7134275, -0.48841858, 0.13164984\n-1.8832661, -0.01785519, -1.1883183, -0.48472223, 0.7141138, -0.48773572, 0.13114765\n-1.8758243, -0.017086022, -1.1809347, -0.48460513, 0.7145447, -0.48733523, 0.13072148\n-1.868274, -0.016339354, -1.1735215, -0.48470834, 0.71491593, -0.48685628, 0.13009177\n-1.8606032, -0.015609294, -1.1660527, -0.48498106, 0.7154356, -0.4860882, 0.12908696\n-1.8523535, -0.014869222, -1.158537, -0.4853259, 0.71606386, -0.48514476, 0.12785077\n-1.844392, -0.014061691, -1.1509553, -0.4859018, 0.7165664, -0.484137, 0.1266619\n-1.836619, -0.013137022, -1.1433294, -0.48684242, 0.7166366, -0.48326012, 0.12600026\n-1.8289964, -0.012099253, -1.1356745, -0.48808163, 0.71606433, -0.48283735, 0.12608024\n-1.821402, -0.011041951, -1.1280477, -0.48940992, 0.714989, -0.48297897, 0.12649079\n-1.8137031, -0.010084236, -1.1204995, -0.49075127, 0.71401703, -0.48308676, 0.12637265\n-1.8053385, -0.009293698, -1.1133667, -0.49193743, 0.7134061, -0.4829944, 0.12556118\n-1.7972678, -0.008515075, -1.1061877, -0.49322754, 0.71289307, -0.48279947, 0.1241556\n-1.7893474, -0.007701258, -1.0990891, -0.4947734, 0.7122237, -0.48262757, 0.12250416\n-1.7815593, -0.006809932, -1.092072, -0.49673486, 0.7113089, -0.48234478, 0.12098616\n-1.7738577, -0.0058740913, -1.085106, -0.49908876, 0.7103236, -0.4817411, 0.11948321\n-1.7662262, -0.004923129, -1.0781575, 0.50162035, -0.7093114, 0.48095813, -0.11804028\n-1.7588496, -0.004047096, -1.0715412, 0.50431174, -0.70833397, 0.47992176, -0.11665258\n-1.75162, -0.0029739588, -1.0647622, 0.50745267, -0.7073407, 0.4783029, -0.11570329\n-1.7446325, -0.0017455085, -1.0579575, 0.51099813, -0.7061463, 0.47634006, -0.11549157\n-1.7378311, -0.00045923144, -1.0511926, 0.514802, -0.7048696, 0.4740619, -0.11577145\n-1.7310997, 0.0007838635, -1.0445079, 0.5185562, -0.7037113, 0.4715906, -0.11615702\n-1.7243892, 0.001917921, -1.0380073, 0.5218816, -0.70268565, 0.46940657, -0.11631801\n-1.7174613, 0.0033409027, -1.0315089, 0.5245557, -0.70191616, 0.46766815, -0.11593782\n-1.7106315, 0.0043479875, -1.0255326, 0.5266577, -0.701297, 0.46650523, -0.11483468\n-1.7038302, 0.00514341, -1.0199184, 0.5281562, -0.7008366, 0.46593434, -0.113066785\n-1.6971196, 0.005824669, -1.0145556, 0.52933, -0.7004649, 0.46566764, -0.11096057\n-1.6905271, 0.0063924417, -1.0094084, 0.530431, -0.70033157, 0.465216, -0.10840931\n-1.6840426, 0.006831837, -1.0044638, 0.5314782, -0.70054835, 0.4643842, -0.105404\n-1.6777532, 0.006908618, -0.9995795, 0.53261256, -0.70105726, 0.463025, -0.10222708\n-1.6716486, 0.007145195, -0.9948937, 0.5340264, -0.70169663, 0.46108517, -0.09918722\n-1.6658008, 0.0074763657, -0.99029946, 0.5357322, -0.7021275, 0.4589808, -0.096666954\n-1.6602179, 0.007862767, -0.9857484, 0.53759813, -0.7022119, 0.4570649, -0.09475437\n-1.6548315, 0.008240499, -0.98123276, 0.5393708, -0.70203894, 0.4555473, -0.0932574\n-1.6496112, 0.008608785, -0.97671837, 0.5410583, -0.7016578, 0.4543403, -0.092233315\n-1.644321, 0.009287879, -0.9726731, 0.5428232, -0.70094174, 0.45345426, -0.09166702\n-1.639322, 0.009694219, -0.9683635, 0.544627, -0.70008224, 0.45270246, -0.09125059\n-1.6344886, 0.009976737, -0.96402293, 0.54649144, -0.6991889, 0.4519181, -0.09084038\n-1.629663, 0.0101368725, -0.9597532, 0.5482334, -0.69843984, 0.4510733, -0.09030342\n-1.6246995, 0.010091668, -0.95561546, 0.5494517, -0.6979615, 0.4505121, -0.08939342\n-1.6196009, 0.009833582, -0.9516102, 0.5501177, -0.6976788, 0.45038277, -0.088148385\n-1.614124, 0.008975476, -0.9480728, 0.5504382, -0.69748443, 0.45053393, -0.08690349\n-1.608868, 0.008379756, -0.94430804, 0.55073285, -0.69724894, 0.45070693, -0.08602584\n-1.603693, 0.00784903, -0.9404846, 0.5509767, -0.69702804, 0.4508636, -0.085431024\n-1.5985656, 0.0073081017, -0.93667567, 0.55110717, -0.69680494, 0.45111898, -0.08506097\n-1.593489, 0.006753622, -0.9328586, 0.5511683, -0.69657207, 0.45140883, -0.0850341\n-1.5884517, 0.006207603, -0.92900234, 0.55129504, -0.69621027, 0.45171893, -0.08552752\n-1.5831268, 0.005716047, -0.925071, 0.55146194, -0.6956548, 0.45218027, -0.08652861\n-1.5779926, 0.005117275, -0.92117596, 0.55145127, -0.6950844, 0.45281294, -0.08786147\n-1.5728706, 0.0044335052, -0.91729796, 0.55123585, -0.69444585, 0.45371494, -0.08959222\n-1.5676726, 0.003642328, -0.9134914, 0.55086684, -0.69369745, 0.4549226, -0.09151451\n-1.5623256, 0.0027136938, -0.90977806, 0.55022603, -0.6928918, 0.45653653, -0.093416356\n-1.556789, 0.0016716779, -0.9061284, 0.5491727, -0.6919588, 0.45880255, -0.09540623\n-1.5501716, 0.0007592412, -0.90170264, 0.54774225, -0.69085705, 0.46172294, -0.097502664\n-1.5439717, -0.00037630647, -0.8978885, 0.54608893, -0.689559, 0.46512553, -0.099767335\n-1.5378339, -0.001622194, -0.89427036, 0.5443516, -0.68802744, 0.46886268, -0.102309704\n-1.5316521, -0.002929674, -0.89069843, 0.5426632, -0.6863101, 0.47269323, -0.10514765\n-1.5253379, -0.00427033, -0.88714975, 0.54126483, -0.6845513, 0.4761585, -0.10814374\n-1.5187609, -0.0056594834, -0.88366795, 0.54024345, -0.68292147, 0.4790263, -0.1108565\n-1.5115149, -0.0062301382, -0.88006717, 0.53944266, -0.6816191, 0.48130116, -0.11289825\n-1.5040939, -0.0076298714, -0.8767979, 0.53864485, -0.6809578, 0.48287433, -0.11397643\n-1.4963968, -0.00941588, -0.87367195, 0.5376766, -0.6808805, 0.483981, -0.11431599\n-1.4884377, -0.011432432, -0.87055033, 0.53645766, -0.6813388, 0.48470333, -0.11425056\n-1.4802921, -0.013564266, -0.8673208, 0.53516895, -0.681944, 0.4852464, -0.11437862\n-1.4720715, -0.01569572, -0.8640193, 0.5340027, -0.6824211, 0.48574778, -0.11485438\n-1.4634669, -0.017030507, -0.85949904, 0.5330213, -0.6827607, 0.48612943, -0.11577701\n-1.4550893, -0.018740363, -0.8557241, 0.53205234, -0.68267345, 0.48691997, -0.117414474\n-1.446774, -0.020499095, -0.85216933, 0.5308799, -0.6822703, 0.4883095, -0.1192799\n-1.4384135, -0.022197347, -0.84868234, 0.52928305, -0.68162423, 0.490476, -0.121165104\n-1.4299554, -0.023834914, -0.84529275, 0.52739704, -0.6807489, 0.49327427, -0.1229375\n-1.42139, -0.025437742, -0.8420468, 0.5253675, -0.6797764, 0.4963489, -0.12462179\n-1.4117044, -0.02711455, -0.8382445, 0.52327675, -0.6789157, 0.49933085, -0.1261884\n-1.4024781, -0.028669769, -0.83488613, 0.5212077, -0.67816836, 0.5021003, -0.12777077\n-1.393349, -0.030097246, -0.8316149, 0.5190696, -0.67725396, 0.505052, -0.12967761\n-1.3842622, -0.03135729, -0.8283462, 0.51660925, -0.675895, 0.50877774, -0.13200721\n-1.3751878, -0.032436647, -0.8250466, 0.51373184, -0.6740994, 0.5133852, -0.13455535\n-1.3660663, -0.03339009, -0.8217627, 0.51064736, -0.6722134, 0.51823926, -0.13710055\n-1.3563737, -0.03393101, -0.81803805, 0.50767535, -0.6704098, 0.5228089, -0.13959628" }, { "path": "test/test_data/robot_with_visuals.urdf", "content": "\n\n \n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\n\n \n \n \n \n \n\n\n\n \n \n \n \n \n\n\n\n \n \n \n \n \n\n\n\n \n \n \n \n\n\n\n \n \n \n \n\n\n\n \n \n \n \n\n\n\n \n \n \n\n\n" }, { "path": "test/test_data/scene.mtl", "content": "newmtl Box1\n# ambient color\nKa 1.0 0.5 0.5\n# diffuse color\nKd 1.0 0.5 0.5\n# specular color\nKs 0.0 0.0 0.0\n# transparency, opaque in this case\nd 1.0\n# illumination model: highlight on\nillum 2\n\nnewmtl Box2\nKa 0.5 1.0 0.5\nKd 0.5 1.0 0.5\nKs 0.0 0.0 0.0\nd 1.0\nillum 2\n" }, { "path": "test/test_data/scene.obj", "content": "mtllib scene.mtl\n\ng box1\n\n# vertices\nv 0. 0. 0.\nv 1. 0. 0.\nv 0. 0. 1.\nv 1. 0. 1.\nv 0. 1. 0.\nv 1. 1. 0.\nv 0. 1. 1.\nv 1. 1. 1.\n\nusemtl Box1\n# faces\nf 5 8 6\nf 5 7 8\nf 1 3 5\nf 3 7 5\nf 1 2 3\nf 2 4 3\nf 2 6 8\nf 2 8 4\nf 3 4 8\nf 3 8 7\nf 1 5 2\nf 2 5 6\n\ng box2\n\n# vertices\nv 2. 0. 0.\nv 3. 0. 0.\nv 2. 0. 1.\nv 3. 0. 1.\nv 2. 1. 0.\nv 3. 1. 0.\nv 2. 1. 1.\nv 3. 1. 1.\n\nusemtl Box2\n# faces\nf 13 16 14\nf 13 15 16\nf 9 11 13\nf 11 15 13\nf 9 10 11\nf 10 12 11\nf 10 14 16\nf 10 16 12\nf 11 12 16\nf 11 16 15\nf 9 13 10\nf 10 13 14\n" }, { "path": "test/test_data/simple_mechanism.urdf", "content": "\n\n \n \n \n \n\n \n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n \n\n \n \n \n \n \n \n \n\n \n" } ]