Repository: dfki-ric/pytransform3d Branch: main Commit: bc7453d6463f Files: 276 Total size: 1.2 MB Directory structure: gitextract_8dwelvlz/ ├── .coveragerc ├── .github/ │ └── workflows/ │ └── python-package.yml ├── .gitignore ├── CITATION.cff ├── CONTRIBUTING.md ├── LICENSE ├── MANIFEST.in ├── README.md ├── doc/ │ ├── Makefile │ ├── make.bat │ └── source/ │ ├── _static/ │ │ ├── custom.css │ │ └── overview.tex │ ├── _templates/ │ │ ├── class.rst │ │ ├── class_without_inherited.rst │ │ └── function.rst │ ├── api.rst │ ├── conf.py │ ├── index.rst │ ├── install.rst │ └── user_guide/ │ ├── camera.rst │ ├── euler_angles.rst │ ├── index.rst │ ├── introduction.rst │ ├── rotations.rst │ ├── transform_manager.rst │ ├── transformation_ambiguities.rst │ ├── transformation_modeling.rst │ ├── transformation_over_time.rst │ ├── transformations.rst │ └── uncertainty.rst ├── examples/ │ ├── README.rst │ ├── animations/ │ │ ├── README.rst │ │ ├── animate_angle_axis_interpolation.py │ │ ├── animate_camera.py │ │ ├── animate_quaternion_integration.py │ │ ├── animate_quaternion_interpolation.py │ │ ├── animate_rotation.py │ │ └── animate_trajectory.py │ ├── apps/ │ │ ├── README.rst │ │ └── app_transformation_editor.py │ ├── plots/ │ │ ├── README.rst │ │ ├── plot_axis_angle.py │ │ ├── plot_axis_angle_from_two_vectors.py │ │ ├── plot_bivector.py │ │ ├── plot_box.py │ │ ├── plot_camera_3d.py │ │ ├── plot_camera_projection.py │ │ ├── plot_camera_trajectory.py │ │ ├── plot_camera_with_image.py │ │ ├── plot_collision_objects.py │ │ ├── plot_compare_rotations.py │ │ ├── plot_concatenate_uncertain_transforms.py │ │ ├── plot_convention_rotation_global_local.py │ │ ├── plot_cylinder.py │ │ ├── plot_dual_quaternion_interpolation.py │ │ ├── plot_euler_angles.py │ │ ├── plot_frames.py │ │ ├── plot_interpolation_for_transform_manager.py │ │ ├── plot_invert_uncertain_transform.py │ │ ├── plot_matrix_from_two_vectors.py │ │ ├── plot_mesh.py │ │ ├── plot_polar_decomposition.py │ │ ├── plot_pose_fusion.py │ │ ├── plot_pose_trajectory.py │ │ ├── plot_quaternion_integrate.py │ │ ├── plot_quaternion_slerp.py │ │ ├── plot_random_geometries.py │ │ ├── plot_random_trajectories.py │ │ ├── plot_robot.py │ │ ├── plot_rotate_cylinder.py │ │ ├── plot_sample_rotations.py │ │ ├── plot_sample_transforms.py │ │ ├── plot_screw.py │ │ ├── plot_sphere.py │ │ ├── plot_spheres.py │ │ ├── plot_spherical_grid.py │ │ ├── plot_straight_line_path.py │ │ ├── plot_transform.py │ │ ├── plot_transform_concatenation.py │ │ ├── plot_transform_manager.py │ │ ├── plot_urdf.py │ │ ├── plot_urdf_with_meshes.py │ │ └── plot_vector.py │ └── visualizations/ │ ├── README.rst │ ├── vis_add_remove_artist.py │ ├── vis_basis.py │ ├── vis_box.py │ ├── vis_camera_3d.py │ ├── vis_capsule.py │ ├── vis_cone.py │ ├── vis_cylinder.py │ ├── vis_ee_wrench.py │ ├── vis_ellipsoid.py │ ├── vis_mesh.py │ ├── vis_moving_basis.py │ ├── vis_moving_cylinder_with_wrench.py │ ├── vis_moving_line.py │ ├── vis_moving_robot.py │ ├── vis_moving_trajectory.py │ ├── vis_moving_urdf_with_meshes.py │ ├── vis_plane.py │ ├── vis_probabilistic_robot_kinematics.py │ ├── vis_scatter.py │ ├── vis_sphere.py │ ├── vis_urdf.py │ ├── vis_urdf_with_meshes.py │ └── vis_vector.py ├── manifest.xml ├── paper/ │ ├── paper.bib │ └── paper.md ├── pyproject.toml ├── pytransform3d/ │ ├── __init__.py │ ├── _geometry.py │ ├── _geometry.pyi │ ├── _mesh_loader.py │ ├── _mesh_loader.pyi │ ├── batch_rotations/ │ │ ├── __init__.py │ │ ├── _angle.py │ │ ├── _angle.pyi │ │ ├── _axis_angle.py │ │ ├── _axis_angle.pyi │ │ ├── _euler.py │ │ ├── _euler.pyi │ │ ├── _matrix.py │ │ ├── _matrix.pyi │ │ ├── _quaternion.py │ │ ├── _quaternion.pyi │ │ ├── _utils.py │ │ ├── _utils.pyi │ │ └── test/ │ │ └── test_batch_rotations.py │ ├── camera.py │ ├── camera.pyi │ ├── coordinates.py │ ├── coordinates.pyi │ ├── editor.py │ ├── plot_utils/ │ │ ├── __init__.py │ │ ├── _artists.py │ │ ├── _artists.pyi │ │ ├── _layout.py │ │ ├── _layout.pyi │ │ ├── _plot_geometries.py │ │ ├── _plot_geometries.pyi │ │ ├── _plot_helpers.py │ │ ├── _plot_helpers.pyi │ │ └── test/ │ │ └── test_plot_utils.py │ ├── py.typed │ ├── rotations/ │ │ ├── __init__.py │ │ ├── _angle.py │ │ ├── _angle.pyi │ │ ├── _axis_angle.py │ │ ├── _axis_angle.pyi │ │ ├── _constants.py │ │ ├── _euler.py │ │ ├── _euler.pyi │ │ ├── _euler_deprecated.py │ │ ├── _euler_deprecated.pyi │ │ ├── _jacobians.py │ │ ├── _jacobians.pyi │ │ ├── _matrix.py │ │ ├── _matrix.pyi │ │ ├── _mrp.py │ │ ├── _mrp.pyi │ │ ├── _plot.py │ │ ├── _plot.pyi │ │ ├── _polar_decomp.py │ │ ├── _polar_decomp.pyi │ │ ├── _quaternion.py │ │ ├── _quaternion.pyi │ │ ├── _random.py │ │ ├── _random.pyi │ │ ├── _rot_log.py │ │ ├── _rot_log.pyi │ │ ├── _rotors.py │ │ ├── _rotors.pyi │ │ ├── _slerp.py │ │ ├── _slerp.pyi │ │ ├── _utils.py │ │ ├── _utils.pyi │ │ └── test/ │ │ ├── test_angle.py │ │ ├── test_axis_angle.py │ │ ├── test_constants.py │ │ ├── test_euler.py │ │ ├── test_euler_deprecated.py │ │ ├── test_matrix.py │ │ ├── test_mrp.py │ │ ├── test_polar_decom.py │ │ ├── test_quaternion.py │ │ ├── test_rot_log.py │ │ ├── test_rotations_jacobians.py │ │ ├── test_rotations_random.py │ │ ├── test_rotor.py │ │ ├── test_slerp.py │ │ └── test_utils.py │ ├── test/ │ │ ├── test_camera.py │ │ ├── test_coordinates.py │ │ ├── test_geometry.py │ │ ├── test_mesh_loader.py │ │ └── test_urdf.py │ ├── trajectories/ │ │ ├── __init__.py │ │ ├── _dual_quaternions.py │ │ ├── _dual_quaternions.pyi │ │ ├── _plot.py │ │ ├── _plot.pyi │ │ ├── _pqs.py │ │ ├── _pqs_.pyi │ │ ├── _random.py │ │ ├── _random.pyi │ │ ├── _screws.py │ │ ├── _screws.pyi │ │ ├── _transforms.py │ │ ├── _transforms.pyi │ │ └── test/ │ │ └── test_trajectories.py │ ├── transform_manager/ │ │ ├── __init__.py │ │ ├── _temporal_transform_manager.py │ │ ├── _temporal_transform_manager.pyi │ │ ├── _transform_graph_base.py │ │ ├── _transform_graph_base.pyi │ │ ├── _transform_manager.py │ │ ├── _transform_manager.pyi │ │ └── test/ │ │ ├── test_temporal_transform_manager.py │ │ └── test_transform_manager.py │ ├── transformations/ │ │ ├── __init__.py │ │ ├── _dual_quaternion.py │ │ ├── _dual_quaternion.pyi │ │ ├── _jacobians.py │ │ ├── _jacobians.pyi │ │ ├── _plot.py │ │ ├── _plot.pyi │ │ ├── _pq.py │ │ ├── _pq.pyi │ │ ├── _random.py │ │ ├── _random.pyi │ │ ├── _screws.py │ │ ├── _screws.pyi │ │ ├── _transform.py │ │ ├── _transform.pyi │ │ ├── _transform_operations.py │ │ ├── _transform_operations.pyi │ │ └── test/ │ │ ├── test_dual_quaternion.py │ │ ├── test_pq.py │ │ ├── test_screws.py │ │ ├── test_transform.py │ │ ├── test_transform_operations.py │ │ ├── test_transformations_jacobians.py │ │ └── test_transformations_random.py │ ├── uncertainty/ │ │ ├── __init__.py │ │ ├── _composition.py │ │ ├── _composition.pyi │ │ ├── _frechet_mean.py │ │ ├── _frechet_mean.pyi │ │ ├── _fusion.py │ │ ├── _fusion.pyi │ │ ├── _invert.py │ │ ├── _invert.pyi │ │ ├── _plot.py │ │ ├── _plot.pyi │ │ └── test/ │ │ └── test_uncertainty.py │ ├── urdf.py │ ├── urdf.pyi │ └── visualizer/ │ ├── __init__.py │ ├── _artists.py │ ├── _artists.pyi │ ├── _figure.py │ └── _figure.pyi ├── requirements.txt ├── setup.cfg ├── setup.py └── test/ └── test_data/ ├── cone.stl ├── frame.ply ├── reconstruction_camera_matrix.csv ├── reconstruction_odometry.csv ├── robot_with_visuals.urdf ├── scan.stl ├── scene.mtl ├── scene.obj └── simple_mechanism.urdf ================================================ FILE CONTENTS ================================================ ================================================ FILE: .coveragerc ================================================ [run] branch = True source = pytransform3d omit = */setup.py pytransform3d/plot_utils.py pytransform3d/editor.py pytransform3d/visualizer.py [report] omit = pytransform3d/plot_utils/*.py pytransform3d/editor.py pytransform3d/visualizer/*.py pytransform3d/*/_plot.py pytransform3d/test/*.py pytransform3d/*/test/*.py exclude_lines = pragma: no cover except ImportError: ================================================ FILE: .github/workflows/python-package.yml ================================================ # This workflow will install Python dependencies, run tests and lint with a variety of Python versions # For more information see: https://help.github.com/actions/language-and-framework-guides/using-python-with-github-actions name: Python package on: push: branches-ignore: - gh-pages jobs: build: runs-on: ubuntu-latest strategy: matrix: python-version: ["3.9", "3.10", "3.11", "3.12", "3.13"] steps: - uses: actions/checkout@v2 - name: Set up Python ${{ matrix.python-version }} uses: actions/setup-python@v2 with: python-version: ${{ matrix.python-version }} - name: Install dependencies run: | sudo apt install graphviz python -m pip install --upgrade pip python -m pip install flake8 python -m pip install -e .[test] pydot trimesh - name: Lint with flake8 run: | # stop the build if there are Python syntax errors or undefined names flake8 . --count --select=E9,F63,F7,F82 --show-source --statistics # exit-zero treats all errors as warnings. The GitHub editor is 127 chars wide flake8 . --count --exit-zero --max-complexity=10 --max-line-length=127 --statistics - name: Test run: | MPLBACKEND=Agg pytest - name: Codecov uses: codecov/codecov-action@v1.3.2 ================================================ FILE: .gitignore ================================================ .idea/ .coverage .venv/ cover/ *~ *.pyc *.vim .ipynb_checkpoints/ venv/ build/ doc/examples doc/source/_apidoc doc/source/_auto_examples .vscode *.egg-info/ ================================================ FILE: CITATION.cff ================================================ cff-version: 1.2.0 message: "If you use this software, please cite it as below." authors: - family-names: Fabisch given-names: Alexander title: "pytransform3d" url: https://github.com/dfki-ric/pytransform3d doi: 10.5281/zenodo.2553450 preferred-citation: type: article authors: - family-names: Fabisch given-names: Alexander orcid: https://orcid.org/0000-0003-2824-7956 title: "pytransform3d: 3D Transformations for Python" doi: 10.21105/joss.01159 journal: Journal of Open Source Software start: 1159 issue: 33 volume: 4 month: 1 year: 2019 ================================================ FILE: CONTRIBUTING.md ================================================ # Contributing Everyone is welcome to contribute. There are several ways to contribute to pytransform3d: you could * send a bug report to the [bug tracker](http://github.com/dfki-ric/pytransform3d/issues) * work on one of the reported issues * write documentation * add a new feature * add tests * add an example ## How to Contribute Code This text is shamelessly copied from [scikit-learn's](https://scikit-learn.org/stable/developers/contributing.html) contribution guidelines. The preferred way to contribute to pytransform3d is to fork the [repository](http://github.com/dfki-ric/pytransform3d/) on GitHub, then submit a "pull request" (PR): 1. [Create an account](https://github.com/signup/free) on GitHub if you do not already have one. 2. Fork the [project repository](http://github.com/dfki-ric/pytransform3d): click on the 'Fork' button near the top of the page. This creates a copy of the code under your account on the GitHub server. 3. Clone this copy to your local disk: $ git clone git@github.com:YourLogin/pytransform3d.git 4. Create a branch to hold your changes: $ git checkout -b my-feature and start making changes. Never work in the `main` branch! 5. Work on this copy, on your computer, using Git to do the version control. When you're done editing, do: $ git add modified_files $ git commit to record your changes in Git, then push them to GitHub with: $ git push -u origin my-feature Finally, go to the web page of the your fork of the pytransform3d repository, and click 'Pull request' to send your changes to the maintainer for review. Make sure that your target branch is 'develop'. In the above setup, your `origin` remote repository points to YourLogin/pytransform3d.git. If you wish to fetch/merge from the main repository instead of your forked one, you will need to add another remote to use instead of `origin`. If we choose the name `upstream` for it, the command will be: $ git remote add upstream https://github.com/dfki-ric/pytransform3d.git (If any of the above seems like magic to you, then look up the [Git documentation](http://git-scm.com/documentation) on the web.) ## Requirements for New Features Adding a new feature to pytransform3d requires a few other changes: * New classes or functions that are part of the public interface must be documented. We use [NumPy's conventions for docstrings](https://github.com/numpy/numpy/blob/master/doc/HOWTO_DOCUMENT.rst.txt). * An entry to the API documentation must be added [here](https://dfki-ric.github.io/pytransform3d/api.html). * Consider writing a simple example script. * Tests: Unit tests for new features are mandatory. They should cover all branches. Exceptions are plotting functions, debug outputs, etc. These are usually hard to test and are not a fundamental part of the library. ## Merge Policy Usually it is not possible to push directly to the develop or main branch for anyone. Only tiny changes, urgent bugfixes, and maintenance commits can be pushed directly to the develop branch by the maintainer without a review. "Tiny" means backwards compatibility is mandatory and all tests must succeed. No new feature must be added. Developers have to submit pull requests. Those will be reviewed and merged by a maintainer. New features must be documented and tested. Breaking changes must be discussed and announced in advance with deprecation warnings. ## Versioning Semantic versioning is used, that is, the major version number will be incremented when the API changes in a backwards incompatible way, the minor version will be incremented when new functionality is added in a backwards compatible manner. ================================================ FILE: LICENSE ================================================ BSD 3-Clause License Copyright 2014-2017 Alexander Fabisch (Robotics Research Group, University of Bremen), 2017-2025 Alexander Fabisch (DFKI GmbH, Robotics Innovation Center), and pytransform3d contributors Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ================================================ FILE: MANIFEST.in ================================================ include pytransform3d/py.typed include pytransform3d/*.pyi include pytransform3d/*/*.pyi ================================================ FILE: README.md ================================================ [![codecov](https://codecov.io/gh/dfki-ric/pytransform3d/branch/main/graph/badge.svg?token=jB10RM3Ujj)](https://codecov.io/gh/dfki-ric/pytransform3d) [![Paper DOI](http://joss.theoj.org/papers/10.21105/joss.01159/status.svg)](https://doi.org/10.21105/joss.01159) [![Release DOI](https://zenodo.org/badge/DOI/10.5281/zenodo.2553450.svg)](https://doi.org/10.5281/zenodo.2553450) # pytransform3d A Python library for transformations in three dimensions. pytransform3d offers... * operations like concatenation and inversion for most common representations of rotation (orientation) and translation (position) * conversions between those representations * clear documentation of transformation conventions * tight coupling with matplotlib to quickly visualize (or animate) transformations * the TransformManager which manages complex chains of transformations (with export to graph visualization as PNG, additionally requires pydot) * the TransformEditor which allows to modify transformations graphically (additionally requires PyQt4/5) * the UrdfTransformManager which is able to load transformations from [URDF](https://wiki.ros.org/urdf) files (additionally requires lxml) * a matplotlib-like interface to Open3D's visualizer to display and animate geometries and transformations (additionally requires Open3D) pytransform3d is used in various domains, for example: * specifying motions of a robot * learning robot movements from human demonstration * sensor fusion for human pose estimation * collision detection for robots The API documentation can be found [here](https://dfki-ric.github.io/pytransform3d/). I gave a talk at EuroSciPy 2023 about pytransform3d. Slides are available [here](https://github.com/AlexanderFabisch/pytransform3d_euroscipy2023/). If you need similar features in JAX (on GPU, vectorized, differentiable), have a look at the experimental library [jaxtransform3d](https://github.com/AlexanderFabisch/jaxtransform3d/). ## Installation Use pip to install the package from PyPI: ```bash pip install 'pytransform3d[all]' ``` or conda: ```bash conda install -c conda-forge pytransform3d ``` Take a look at the [installation instructions](https://dfki-ric.github.io/pytransform3d/install.html) in the documentation for more details. ## Gallery The following plots and visualizations have been generated with pytransform3d. The code for most examples can be found in [the documentation](https://dfki-ric.github.io/pytransform3d/_auto_examples/index.html). Left: [Nao robot](https://www.softbankrobotics.com/emea/en/nao) with URDF from [Bullet3](https://github.com/bulletphysics/bullet3). Right: [Kuka iiwa](https://www.kuka.com/en-de/products/robot-systems/industrial-robots/lbr-iiwa). The animation is based on pytransform3d's visualization interface to [Open3D](http://www.open3d.org/). Visualizations based on [Open3D](http://www.open3d.org/). Various plots based on Matplotlib. Transformation editor based on Qt. ## Example This is just one simple example. You can find more examples in the subfolder `examples/`. ```python import numpy as np import matplotlib.pyplot as plt from pytransform3d import rotations as pr from pytransform3d import transformations as pt from pytransform3d.transform_manager import TransformManager rng = np.random.default_rng(0) ee2robot = pt.transform_from_pq( np.hstack((np.array([0.4, -0.3, 0.5]), pr.random_quaternion(rng)))) cam2robot = pt.transform_from_pq( np.hstack((np.array([0.0, 0.0, 0.8]), pr.q_id))) object2cam = pt.transform_from( pr.active_matrix_from_intrinsic_euler_xyz(np.array([0.0, 0.0, -0.5])), np.array([0.5, 0.1, 0.1])) tm = TransformManager() tm.add_transform("end-effector", "robot", ee2robot) tm.add_transform("camera", "robot", cam2robot) tm.add_transform("object", "camera", object2cam) ee2object = tm.get_transform("end-effector", "object") ax = tm.plot_frames_in("robot", s=0.1) ax.set_xlim((-0.25, 0.75)) ax.set_ylim((-0.5, 0.5)) ax.set_zlim((0.0, 1.0)) plt.show() ``` ![output](https://dfki-ric.github.io/pytransform3d/_images/sphx_glr_plot_transform_manager_001.png) ## Documentation The API documentation can be found [here](https://dfki-ric.github.io/pytransform3d/). The documentation can be found in the directory `doc`. To build the documentation, run e.g. (on linux): ```bash cd doc make html ``` The HTML documentation is now located at `doc/build/html/index.html`. Execute the following command in the main folder of the repository to install the dependencies: ```bash pip install -e '.[doc]' ``` ## Tests You can use pytest to run the tests of this project in the root directory: ```bash pytest ``` A coverage report will be located at `htmlcov/index.html`. Note that you have to install `pytest` to run the tests and `pytest-cov` to obtain the code coverage report. ## Contributing If you wish to report bugs, please use the [issue tracker](https://github.com/dfki-ric/pytransform3d/issues) at Github. If you would like to contribute to pytransform3d, just open an issue or a [pull request](https://github.com/dfki-ric/pytransform3d/pulls). The target branch for pull requests is the develop branch. The development branch will be merged to main for new releases. If you have questions about the software, you should ask them in the [discussion section](https://github.com/dfki-ric/pytransform3d/discussions). The recommended workflow to add a new feature, add documentation, or fix a bug is the following: * Push your changes to a branch (e.g. `feature/x`, `doc/y`, or `fix/z`) of your fork of the pytransform3d repository. * Open a pull request to the latest development branch. There is usually an open merge request from the latest development branch to the main branch. * When the latest development branch is merged to the main branch, a new release will be made. Note that there is a [checklist](https://github.com/dfki-ric/pytransform3d/wiki#checklist-for-new-features) for new features. It is forbidden to directly push to the main branch. Each new version has its own development branch from which a pull request will be opened to the main branch. Only the maintainer of the software is allowed to merge a development branch to the main branch. ## License The library is distributed under the [3-Clause BSD license](https://github.com/dfki-ric/pytransform3d/blob/main/LICENSE). ## Citation If you use pytransform3d for a scientific publication, I would appreciate citation of the following paper: Fabisch, A. (2019). pytransform3d: 3D Transformations for Python. Journal of Open Source Software, 4(33), 1159, [![Paper DOI](http://joss.theoj.org/papers/10.21105/joss.01159/status.svg)](https://doi.org/10.21105/joss.01159) Bibtex entry: ```bibtex @article{Fabisch2019, doi = {10.21105/joss.01159}, url = {https://doi.org/10.21105/joss.01159}, year = {2019}, publisher = {The Open Journal}, volume = {4}, number = {33}, pages = {1159}, author = {Alexander Fabisch}, title = {pytransform3d: 3D Transformations for Python}, journal = {Journal of Open Source Software} } ``` ================================================ FILE: doc/Makefile ================================================ # Makefile for Sphinx documentation # # You can set these variables from the command line. SPHINXOPTS = SPHINXBUILD = sphinx-build PAPER = BUILDDIR = build # User-friendly check for sphinx-build ifeq ($(shell which $(SPHINXBUILD) >/dev/null 2>&1; echo $$?), 1) $(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/) endif # Internal variables. PAPEROPT_a4 = -D latex_paper_size=a4 PAPEROPT_letter = -D latex_paper_size=letter ALLSPHINXOPTS = -d $(BUILDDIR)/doctrees $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) source # the i18n builder cannot share the environment and doctrees with the others I18NSPHINXOPTS = $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) source .PHONY: help clean html dirhtml singlehtml pickle json htmlhelp qthelp devhelp epub latex latexpdf text man changes linkcheck doctest gettext help: @echo "Please use \`make ' where is one of" @echo " html to make standalone HTML files" @echo " dirhtml to make HTML files named index.html in directories" @echo " singlehtml to make a single large HTML file" @echo " pickle to make pickle files" @echo " json to make JSON files" @echo " htmlhelp to make HTML files and a HTML help project" @echo " qthelp to make HTML files and a qthelp project" @echo " devhelp to make HTML files and a Devhelp project" @echo " epub to make an epub" @echo " latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter" @echo " latexpdf to make LaTeX files and run them through pdflatex" @echo " latexpdfja to make LaTeX files and run them through platex/dvipdfmx" @echo " text to make text files" @echo " man to make manual pages" @echo " texinfo to make Texinfo files" @echo " info to make Texinfo files and run them through makeinfo" @echo " gettext to make PO message catalogs" @echo " changes to make an overview of all changed/added/deprecated items" @echo " xml to make Docutils-native XML files" @echo " pseudoxml to make pseudoxml-XML files for display purposes" @echo " linkcheck to check all external links for integrity" @echo " doctest to run all doctests embedded in the documentation (if enabled)" clean: rm -rf $(BUILDDIR)/* rm -rf source/_apidoc rm -rf source/_auto_examples html: $(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html @echo @echo "Build finished. The HTML pages are in $(BUILDDIR)/html." dirhtml: $(SPHINXBUILD) -b dirhtml $(ALLSPHINXOPTS) $(BUILDDIR)/dirhtml @echo @echo "Build finished. The HTML pages are in $(BUILDDIR)/dirhtml." singlehtml: $(SPHINXBUILD) -b singlehtml $(ALLSPHINXOPTS) $(BUILDDIR)/singlehtml @echo @echo "Build finished. The HTML page is in $(BUILDDIR)/singlehtml." pickle: $(SPHINXBUILD) -b pickle $(ALLSPHINXOPTS) $(BUILDDIR)/pickle @echo @echo "Build finished; now you can process the pickle files." json: $(SPHINXBUILD) -b json $(ALLSPHINXOPTS) $(BUILDDIR)/json @echo @echo "Build finished; now you can process the JSON files." htmlhelp: $(SPHINXBUILD) -b htmlhelp $(ALLSPHINXOPTS) $(BUILDDIR)/htmlhelp @echo @echo "Build finished; now you can run HTML Help Workshop with the" \ ".hhp project file in $(BUILDDIR)/htmlhelp." qthelp: $(SPHINXBUILD) -b qthelp $(ALLSPHINXOPTS) $(BUILDDIR)/qthelp @echo @echo "Build finished; now you can run "qcollectiongenerator" with the" \ ".qhcp project file in $(BUILDDIR)/qthelp, like this:" @echo "# qcollectiongenerator $(BUILDDIR)/qthelp/pytransform3d.qhcp" @echo "To view the help file:" @echo "# assistant -collectionFile $(BUILDDIR)/qthelp/pytransform3d.qhc" devhelp: $(SPHINXBUILD) -b devhelp $(ALLSPHINXOPTS) $(BUILDDIR)/devhelp @echo @echo "Build finished." @echo "To view the help file:" @echo "# mkdir -p $$HOME/.local/share/devhelp/pytransform3d" @echo "# ln -s $(BUILDDIR)/devhelp $$HOME/.local/share/devhelp/pytransform3d" @echo "# devhelp" epub: $(SPHINXBUILD) -b epub $(ALLSPHINXOPTS) $(BUILDDIR)/epub @echo @echo "Build finished. The epub file is in $(BUILDDIR)/epub." latex: $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex @echo @echo "Build finished; the LaTeX files are in $(BUILDDIR)/latex." @echo "Run \`make' in that directory to run these through (pdf)latex" \ "(use \`make latexpdf' here to do that automatically)." latexpdf: $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex @echo "Running LaTeX files through pdflatex..." $(MAKE) -C $(BUILDDIR)/latex all-pdf @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." latexpdfja: $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex @echo "Running LaTeX files through platex and dvipdfmx..." $(MAKE) -C $(BUILDDIR)/latex all-pdf-ja @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." text: $(SPHINXBUILD) -b text $(ALLSPHINXOPTS) $(BUILDDIR)/text @echo @echo "Build finished. The text files are in $(BUILDDIR)/text." man: $(SPHINXBUILD) -b man $(ALLSPHINXOPTS) $(BUILDDIR)/man @echo @echo "Build finished. The manual pages are in $(BUILDDIR)/man." texinfo: $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo @echo @echo "Build finished. The Texinfo files are in $(BUILDDIR)/texinfo." @echo "Run \`make' in that directory to run these through makeinfo" \ "(use \`make info' here to do that automatically)." info: $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo @echo "Running Texinfo files through makeinfo..." make -C $(BUILDDIR)/texinfo info @echo "makeinfo finished; the Info files are in $(BUILDDIR)/texinfo." gettext: $(SPHINXBUILD) -b gettext $(I18NSPHINXOPTS) $(BUILDDIR)/locale @echo @echo "Build finished. The message catalogs are in $(BUILDDIR)/locale." changes: $(SPHINXBUILD) -b changes $(ALLSPHINXOPTS) $(BUILDDIR)/changes @echo @echo "The overview file is in $(BUILDDIR)/changes." linkcheck: $(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck @echo @echo "Link check complete; look for any errors in the above output " \ "or in $(BUILDDIR)/linkcheck/output.txt." doctest: $(SPHINXBUILD) -b doctest $(ALLSPHINXOPTS) $(BUILDDIR)/doctest @echo "Testing of doctests in the sources finished, look at the " \ "results in $(BUILDDIR)/doctest/output.txt." xml: $(SPHINXBUILD) -b xml $(ALLSPHINXOPTS) $(BUILDDIR)/xml @echo @echo "Build finished. The XML files are in $(BUILDDIR)/xml." pseudoxml: $(SPHINXBUILD) -b pseudoxml $(ALLSPHINXOPTS) $(BUILDDIR)/pseudoxml @echo @echo "Build finished. The pseudo-XML files are in $(BUILDDIR)/pseudoxml." ================================================ FILE: doc/make.bat ================================================ @ECHO OFF REM Command file for Sphinx documentation if "%SPHINXBUILD%" == "" ( set SPHINXBUILD=sphinx-build ) set BUILDDIR=build set ALLSPHINXOPTS=-d %BUILDDIR%/doctrees %SPHINXOPTS% source set I18NSPHINXOPTS=%SPHINXOPTS% source if NOT "%PAPER%" == "" ( set ALLSPHINXOPTS=-D latex_paper_size=%PAPER% %ALLSPHINXOPTS% set I18NSPHINXOPTS=-D latex_paper_size=%PAPER% %I18NSPHINXOPTS% ) if "%1" == "" goto help if "%1" == "help" ( :help echo.Please use `make ^` where ^ is one of echo. html to make standalone HTML files echo. dirhtml to make HTML files named index.html in directories echo. singlehtml to make a single large HTML file echo. pickle to make pickle files echo. json to make JSON files echo. htmlhelp to make HTML files and a HTML help project echo. qthelp to make HTML files and a qthelp project echo. devhelp to make HTML files and a Devhelp project echo. epub to make an epub echo. latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter echo. text to make text files echo. man to make manual pages echo. texinfo to make Texinfo files echo. gettext to make PO message catalogs echo. changes to make an overview over all changed/added/deprecated items echo. xml to make Docutils-native XML files echo. pseudoxml to make pseudoxml-XML files for display purposes echo. linkcheck to check all external links for integrity echo. doctest to run all doctests embedded in the documentation if enabled goto end ) if "%1" == "clean" ( for /d %%i in (%BUILDDIR%\*) do rmdir /q /s %%i del /q /s %BUILDDIR%\* goto end ) %SPHINXBUILD% 2> nul if errorlevel 9009 ( echo. echo.The 'sphinx-build' command was not found. Make sure you have Sphinx echo.installed, then set the SPHINXBUILD environment variable to point echo.to the full path of the 'sphinx-build' executable. Alternatively you echo.may add the Sphinx directory to PATH. echo. echo.If you don't have Sphinx installed, grab it from echo.http://sphinx-doc.org/ exit /b 1 ) if "%1" == "html" ( %SPHINXBUILD% -b html %ALLSPHINXOPTS% %BUILDDIR%/html if errorlevel 1 exit /b 1 echo. echo.Build finished. The HTML pages are in %BUILDDIR%/html. goto end ) if "%1" == "dirhtml" ( %SPHINXBUILD% -b dirhtml %ALLSPHINXOPTS% %BUILDDIR%/dirhtml if errorlevel 1 exit /b 1 echo. echo.Build finished. The HTML pages are in %BUILDDIR%/dirhtml. goto end ) if "%1" == "singlehtml" ( %SPHINXBUILD% -b singlehtml %ALLSPHINXOPTS% %BUILDDIR%/singlehtml if errorlevel 1 exit /b 1 echo. echo.Build finished. The HTML pages are in %BUILDDIR%/singlehtml. goto end ) if "%1" == "pickle" ( %SPHINXBUILD% -b pickle %ALLSPHINXOPTS% %BUILDDIR%/pickle if errorlevel 1 exit /b 1 echo. echo.Build finished; now you can process the pickle files. goto end ) if "%1" == "json" ( %SPHINXBUILD% -b json %ALLSPHINXOPTS% %BUILDDIR%/json if errorlevel 1 exit /b 1 echo. echo.Build finished; now you can process the JSON files. goto end ) if "%1" == "htmlhelp" ( %SPHINXBUILD% -b htmlhelp %ALLSPHINXOPTS% %BUILDDIR%/htmlhelp if errorlevel 1 exit /b 1 echo. echo.Build finished; now you can run HTML Help Workshop with the ^ .hhp project file in %BUILDDIR%/htmlhelp. goto end ) if "%1" == "qthelp" ( %SPHINXBUILD% -b qthelp %ALLSPHINXOPTS% %BUILDDIR%/qthelp if errorlevel 1 exit /b 1 echo. echo.Build finished; now you can run "qcollectiongenerator" with the ^ .qhcp project file in %BUILDDIR%/qthelp, like this: echo.^> qcollectiongenerator %BUILDDIR%\qthelp\pytransform3d.qhcp echo.To view the help file: echo.^> assistant -collectionFile %BUILDDIR%\qthelp\pytransform3d.ghc goto end ) if "%1" == "devhelp" ( %SPHINXBUILD% -b devhelp %ALLSPHINXOPTS% %BUILDDIR%/devhelp if errorlevel 1 exit /b 1 echo. echo.Build finished. goto end ) if "%1" == "epub" ( %SPHINXBUILD% -b epub %ALLSPHINXOPTS% %BUILDDIR%/epub if errorlevel 1 exit /b 1 echo. echo.Build finished. The epub file is in %BUILDDIR%/epub. goto end ) if "%1" == "latex" ( %SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex if errorlevel 1 exit /b 1 echo. echo.Build finished; the LaTeX files are in %BUILDDIR%/latex. goto end ) if "%1" == "latexpdf" ( %SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex cd %BUILDDIR%/latex make all-pdf cd %BUILDDIR%/.. echo. echo.Build finished; the PDF files are in %BUILDDIR%/latex. goto end ) if "%1" == "latexpdfja" ( %SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex cd %BUILDDIR%/latex make all-pdf-ja cd %BUILDDIR%/.. echo. echo.Build finished; the PDF files are in %BUILDDIR%/latex. goto end ) if "%1" == "text" ( %SPHINXBUILD% -b text %ALLSPHINXOPTS% %BUILDDIR%/text if errorlevel 1 exit /b 1 echo. echo.Build finished. The text files are in %BUILDDIR%/text. goto end ) if "%1" == "man" ( %SPHINXBUILD% -b man %ALLSPHINXOPTS% %BUILDDIR%/man if errorlevel 1 exit /b 1 echo. echo.Build finished. The manual pages are in %BUILDDIR%/man. goto end ) if "%1" == "texinfo" ( %SPHINXBUILD% -b texinfo %ALLSPHINXOPTS% %BUILDDIR%/texinfo if errorlevel 1 exit /b 1 echo. echo.Build finished. The Texinfo files are in %BUILDDIR%/texinfo. goto end ) if "%1" == "gettext" ( %SPHINXBUILD% -b gettext %I18NSPHINXOPTS% %BUILDDIR%/locale if errorlevel 1 exit /b 1 echo. echo.Build finished. The message catalogs are in %BUILDDIR%/locale. goto end ) if "%1" == "changes" ( %SPHINXBUILD% -b changes %ALLSPHINXOPTS% %BUILDDIR%/changes if errorlevel 1 exit /b 1 echo. echo.The overview file is in %BUILDDIR%/changes. goto end ) if "%1" == "linkcheck" ( %SPHINXBUILD% -b linkcheck %ALLSPHINXOPTS% %BUILDDIR%/linkcheck if errorlevel 1 exit /b 1 echo. echo.Link check complete; look for any errors in the above output ^ or in %BUILDDIR%/linkcheck/output.txt. goto end ) if "%1" == "doctest" ( %SPHINXBUILD% -b doctest %ALLSPHINXOPTS% %BUILDDIR%/doctest if errorlevel 1 exit /b 1 echo. echo.Testing of doctests in the sources finished, look at the ^ results in %BUILDDIR%/doctest/output.txt. goto end ) if "%1" == "xml" ( %SPHINXBUILD% -b xml %ALLSPHINXOPTS% %BUILDDIR%/xml if errorlevel 1 exit /b 1 echo. echo.Build finished. The XML files are in %BUILDDIR%/xml. goto end ) if "%1" == "pseudoxml" ( %SPHINXBUILD% -b pseudoxml %ALLSPHINXOPTS% %BUILDDIR%/pseudoxml if errorlevel 1 exit /b 1 echo. echo.Build finished. The pseudo-XML files are in %BUILDDIR%/pseudoxml. goto end ) :end ================================================ FILE: doc/source/_static/custom.css ================================================ :root { --pst-sidebar-width: 100px; --pst-secondary-sidebar-width: 100px; } .bd-main .bd-content .bd-article-container { max-width: 100%; /* Allows content to expand if sidebars are smaller */ } .bd-page-width { max-width: 100%; /* Ensures the overall page uses more width */ } ================================================ FILE: doc/source/_static/overview.tex ================================================ \documentclass{article} \usepackage{amssymb,amsmath} \usepackage[pdftex,active,tightpage]{preview} \setlength\PreviewBorder{2mm} % converted to png via % pdflatex overview.tex % pdftoppm -png overview.pdf > overview.png \usepackage{tikz} \usetikzlibrary{arrows, arrows.meta, positioning} \begin{document} \begin{preview} \newcommand{\matx}{10.1} \newcommand{\maty}{14.2} \begin{tikzpicture} \node[draw,shape=rectangle,rounded corners,fill=cyan!10] (legendconversionrot) at (7.5,18.7) {\large\texttt{pytransform3d.rotations}}; \node[draw,shape=rectangle,rounded corners,fill=blue!10] (legendconversiontr) at (21.5,18.7) {\large\texttt{pytransform3d.transformations}}; \node[draw,shape=rectangle,thick,rounded corners,fill=blue!15] (tmat) at (17,15) {\begin{minipage}{4cm}\textbf{transformation matrix}\\[0.3em] $\boldsymbol{T} \in SE(3)$\\[0.3em] \texttt{transform}\\[0.3em] shape (4, 4) \end{minipage}}; \node[draw,shape=rectangle,thick,rounded corners,fill=blue!15] (dquat) at (21.5,15) {\begin{minipage}{4cm}\textbf{(unit) dual quaternion}\\[0.3em] $\boldsymbol{\sigma}$\\[0.3em] \texttt{dual\_quaternion}\\[0.3em] shape (8,) \end{minipage}}; \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] $(\boldsymbol{p}, \boldsymbol{q}) \in \mathbb{R}^3 \times S^3$\\[0.3em] \texttt{pq}\\[0.3em] shape (7,) \end{minipage}}; \node[draw,shape=rectangle,thick,rounded corners,fill=blue!15] (logtr) at (17,11) {\begin{minipage}{4.9cm}\textbf{logarithm of transformation}\\[0.3em] $\left[\mathcal{S}\right]\theta \in se(3)$\\[0.3em] \texttt{transform\_log}\\[0.3em] shape (4, 4) \end{minipage}}; \node[draw,shape=rectangle,thick,rounded corners] (screwmat) at (22.5,11) {\begin{minipage}{3.8cm}\textbf{screw matrix}\\[0.3em] $\left[\mathcal{S}\right] \in se(3)$\\[0.3em] \texttt{screw\_matrix}\\[0.3em] shape (4, 4) \end{minipage}}; \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] $\mathcal{S}\theta \in \mathbb{R}^6$\\[0.3em] \texttt{exponential\_coordinates}\\[0.3em] shape (6,) \end{minipage}}; \node[draw,shape=rectangle,thick,rounded corners] (spvel) at (24,7) {\begin{minipage}{4cm}\textbf{twist / spatial velocity}\\[0.3em] $\mathcal{S}\dot{\theta} \in \mathbb{R}^6$\\[0.3em] shape (6,) \end{minipage}}; \node[draw,shape=rectangle,thick,rounded corners] (spacc) at (29,7) {\begin{minipage}{3.5cm}\textbf{spatial acceleration}\\[0.3em] $\mathcal{S}\ddot{\theta} \in \mathbb{R}^6$\\[0.3em] shape (6,) \end{minipage}}; \node[draw,shape=rectangle,thick,rounded corners] (scrax) at (17,2) {\begin{minipage}{2.2cm}\textbf{screw axis}\\[0.3em] $\mathcal{S} \in \mathbb{R}^6$\\[0.3em] \texttt{screw\_axis}\\[0.3em] shape (6,) \end{minipage}}; \node[draw,shape=rectangle,thick,rounded corners] (screw) at (22,2) {\begin{minipage}{5.2cm}\textbf{screw parameters}\\[0.3em] $(\boldsymbol{q}, \hat{\boldsymbol{s}}, h) \in \mathbb{R}^3 \times S^2 \times \mathbb{R}$\\[0.3em] \texttt{screw\_parameters}\\[0.3em] tuple[shape (3,), shape (3,), float] \end{minipage}}; \draw[<->] (tmat) -- (logtr) node[midway,left] {exp./log. map}; \draw[->] (expcoord) -- (logtr) node[midway,left] {$\left[\cdot\right]$}; \draw[->] (expcoord) -- (spvel) node[midway,above] {$\frac{\partial \mathcal{S}\theta}{\partial t}$}; \draw[->] (spvel) -- (spacc) node[midway,above] {$\frac{\partial \mathcal{S}\dot{\theta}}{\partial t}$}; \draw[->] (scrax) -- (expcoord) node[midway,left] {$\theta$}; \draw[->] (screwmat) -- (logtr) node[midway,above] {$\theta$}; \draw[<->] (screw) -- (scrax) node[midway,above] {}; \draw [thick,rounded corners,fill=cyan!15] (\matx-0.2,\maty-0.5) rectangle (\matx+3.1,\maty+2.2); \node[anchor=west] (rotmatname) at (\matx,\maty+1.8) {\textbf{rotation matrix}}; \node[anchor=west] (rotmatsymbol) at (\matx,\maty+1.2) {$\boldsymbol{R}$}; \node[anchor=west] (rotmatset) at (\matx+0.4,\maty+1.178) {$\in SO(3)$}; \node[anchor=west] (rotmatpname) at (\matx,\maty+0.6) {\texttt{matrix}}; \node[anchor=west] (rotmatarray) at (\matx,\maty) {shape (3, 3)}; \node (legendname) at (\matx-2,\maty+3) {representation}; \draw[red] (rotmatname) edge (legendname); \node (legendsymbol) at (\matx-3,\maty+1.9) {mathematical symbol}; \draw[red] (rotmatsymbol) edge (legendsymbol); \node (legendset) at (\matx+4,\maty+1.2) {set}; \draw[red] (rotmatset) edge (legendset); \node (legendpname) at (\matx-3.3,\maty+0.7) {\begin{minipage}{3.1cm}\centering name in pytransform3d\end{minipage}}; \draw[red] (rotmatpname) edge (legendpname); \node (legendarray) at (\matx-2,\maty-0.5) {NumPy array}; \draw[red] (rotmatarray) edge (legendarray); \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] $\boldsymbol{q} \in S^3$\\[0.3em] \texttt{quaternion}\\[0.3em] shape (4,) \end{minipage}}; \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] $\left[\boldsymbol{\omega}\right] \in so(3)$\\[0.3em] \texttt{rot\_log}\\[0.3em] shape (3, 3) \end{minipage}}; \node[draw,shape=rectangle,thick,rounded corners,fill=cyan!15] (rotvec) at (11.2,7) {\begin{minipage}{3.3cm}\textbf{rotation vector}\\[0.3em] $\boldsymbol{\omega} = \hat{\boldsymbol{\omega}}\theta \in \mathbb{R}^3$\\[0.3em] \texttt{compact\_axis\_angle}\\[0.3em] shape (3,) \end{minipage}}; \node[draw,shape=rectangle,thick,rounded corners] (angvel) at (6.9,7) {\begin{minipage}{2.8cm}\textbf{angular velocity}\\[0.3em] $\dot{\boldsymbol{\omega}} = \hat{\boldsymbol{\omega}}\dot{\theta} \in \mathbb{R}^3$\\[0.3em] shape (3,) \end{minipage}}; \node[draw,shape=rectangle,thick,rounded corners] (angacc) at (2.5,7) {\begin{minipage}{3.6cm}\textbf{angular acceleration}\\[0.3em] $\hat{\boldsymbol{\omega}}\ddot{\theta} \in \mathbb{R}^3$\\[0.3em] shape (3,) \end{minipage}}; \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] $(\hat{\boldsymbol{\omega}}, \theta) \in S^2 \times \mathbb{R}$\\[0.3em] \texttt{axis\_angle}\\[0.3em] shape (4,) \end{minipage}}; \node[draw,shape=rectangle,thick,rounded corners] (rotax) at (11.2,1.5) {\begin{minipage}{3.8cm}\textbf{rotation axis}\\[0.3em] $\hat{\boldsymbol{\omega}} \in S^2$ \end{minipage}}; \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] $(\alpha, \beta, \gamma) \in \mathbb{R}^3$\\[0.3em] \texttt{euler}\\[0.3em] shape (3,) \end{minipage}}; \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] $\boldsymbol{\psi} = \hat{\boldsymbol{\omega}} \tan{\frac{\theta}{4}} \in \mathbb{R}^3$\\[0.3em] \texttt{mrp}\\[0.3em] shape (3,) \end{minipage}}; \draw[<->] (rotmatarray) -- (logrot) node[midway,left] {exp./log. map}; \draw[->] (rotvec) -- (logrot) node[midway,left] {cross product matrix}; \draw[->] (rotvec) -- (angvel) node[midway,above] {$\frac{\partial \boldsymbol{\omega}}{\partial t}$}; \draw[->] (angvel) -- (angacc) node[midway,above] {$\frac{\partial \dot{\boldsymbol{\omega}}}{\partial t}$}; \draw[->] (axan) -- (rotvec) node[midway,left] {product}; \draw[->] (rotax) -- (axan) node[midway,left] {$\theta$}; \end{tikzpicture} \end{preview} \end{document} ================================================ FILE: doc/source/_templates/class.rst ================================================ {{ fullname | escape | underline}} .. currentmodule:: {{ module }} .. autoclass:: {{ objname }} :members: :show-inheritance: :inherited-members: {% block methods %} .. automethod:: __init__ {% if methods %} .. rubric:: {{ _('Methods') }} .. autosummary:: {% for item in methods %} ~{{ name }}.{{ item }} {%- endfor %} {% endif %} {% endblock %} {% block attributes %} {% if attributes %} .. rubric:: {{ _('Attributes') }} .. autosummary:: {% for item in attributes %} ~{{ name }}.{{ item }} {%- endfor %} {% endif %} {% endblock %} .. minigallery:: {{module}}.{{objname}} :add-heading: ================================================ FILE: doc/source/_templates/class_without_inherited.rst ================================================ {{ fullname | escape | underline}} .. currentmodule:: {{ module }} .. autoclass:: {{ objname }} :members: :show-inheritance: :no-inherited-members: {% block methods %} .. automethod:: __init__ {% if methods %} .. rubric:: {{ _('Methods') }} .. autosummary:: {% for item in methods %} {% if item not in inherited_members %} ~{{ name }}.{{ item }} {% endif %} {%- endfor %} {% endif %} {% endblock %} {% block attributes %} {% if attributes %} .. rubric:: {{ _('Attributes') }} .. autosummary:: {% for item in attributes %} {% if item not in inherited_members %} ~{{ name }}.{{ item }} {% endif %} {%- endfor %} {% endif %} {% endblock %} .. minigallery:: {{module}}.{{objname}} :add-heading: ================================================ FILE: doc/source/_templates/function.rst ================================================ :mod:`{{module}}`.{{objname}} {{ underline }}==================== .. currentmodule:: {{ module }} .. autofunction:: {{ objname }} .. minigallery:: {{module}}.{{objname}} :add-heading: ================================================ FILE: doc/source/api.rst ================================================ .. _api: ================= API Documentation ================= This is the detailed documentation of all public classes and functions. You can also search for specific modules, classes, or functions in the :ref:`genindex`. :mod:`pytransform3d.rotations` ============================== .. automodule:: pytransform3d.rotations :no-members: :no-inherited-members: Rotation Matrix --------------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~check_matrix ~matrix_requires_renormalization ~norm_matrix ~robust_polar_decomposition ~random_matrix ~plot_basis ~assert_rotation_matrix ~matrix_slerp ~matrix_power ~passive_matrix_from_angle ~active_matrix_from_angle ~matrix_from_two_vectors ~matrix_from_euler ~matrix_from_axis_angle ~matrix_from_compact_axis_angle ~matrix_from_quaternion ~matrix_from_rotor Euler Angles ------------ .. autosummary:: :toctree: _apidoc/ :template: function.rst ~euler_near_gimbal_lock ~norm_euler ~assert_euler_equal ~euler_from_quaternion ~euler_from_matrix Axis-Angle ---------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~check_axis_angle ~check_compact_axis_angle ~compact_axis_angle_near_pi ~norm_axis_angle ~norm_compact_axis_angle ~random_axis_angle ~random_compact_axis_angle ~plot_axis_angle ~assert_axis_angle_equal ~assert_compact_axis_angle_equal ~axis_angle_slerp ~axis_angle_from_two_directions ~axis_angle_from_matrix ~axis_angle_from_quaternion ~axis_angle_from_compact_axis_angle ~axis_angle_from_mrp ~compact_axis_angle ~compact_axis_angle_from_matrix ~compact_axis_angle_from_quaternion Logarithm of Rotation --------------------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~check_rot_log ~check_skew_symmetric_matrix ~rot_log_from_compact_axis_angle ~cross_product_matrix Quaternion ---------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~check_quaternion ~check_quaternions ~quaternion_requires_renormalization ~quaternion_double ~pick_closest_quaternion ~random_quaternion ~assert_quaternion_equal ~concatenate_quaternions ~q_prod_vector ~q_conj ~quaternion_slerp ~quaternion_dist ~quaternion_diff ~quaternion_gradient ~quaternion_integrate ~quaternion_from_angle ~quaternion_from_euler ~quaternion_from_matrix ~quaternion_from_axis_angle ~quaternion_from_compact_axis_angle ~quaternion_from_mrp ~quaternion_xyzw_from_wxyz ~quaternion_wxyz_from_xyzw Rotor ----- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~check_rotor ~plot_bivector ~wedge ~plane_normal_from_bivector ~geometric_product ~concatenate_rotors ~rotor_reverse ~rotor_apply ~rotor_slerp ~rotor_from_two_directions ~rotor_from_plane_angle Modified Rodrigues Parameters ----------------------------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~check_mrp ~mrp_near_singularity ~norm_mrp ~assert_mrp_equal ~mrp_double ~concatenate_mrp ~mrp_prod_vector ~mrp_from_axis_angle ~mrp_from_quaternion Jacobians --------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~left_jacobian_SO3 ~left_jacobian_SO3_series ~left_jacobian_SO3_inv ~left_jacobian_SO3_inv_series Utility Functions ----------------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~norm_angle ~norm_vector ~perpendicular_to_vectors ~angle_between_vectors ~vector_projection ~plane_basis_from_normal ~random_vector Deprecated Functions -------------------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~quaternion_from_extrinsic_euler_xyz ~active_matrix_from_intrinsic_euler_xzx ~active_matrix_from_extrinsic_euler_xzx ~active_matrix_from_intrinsic_euler_xyx ~active_matrix_from_extrinsic_euler_xyx ~active_matrix_from_intrinsic_euler_yxy ~active_matrix_from_extrinsic_euler_yxy ~active_matrix_from_intrinsic_euler_yzy ~active_matrix_from_extrinsic_euler_yzy ~active_matrix_from_intrinsic_euler_zyz ~active_matrix_from_extrinsic_euler_zyz ~active_matrix_from_intrinsic_euler_zxz ~active_matrix_from_extrinsic_euler_zxz ~active_matrix_from_intrinsic_euler_xzy ~active_matrix_from_extrinsic_euler_xzy ~active_matrix_from_intrinsic_euler_xyz ~active_matrix_from_extrinsic_euler_xyz ~active_matrix_from_intrinsic_euler_yxz ~active_matrix_from_extrinsic_euler_yxz ~active_matrix_from_intrinsic_euler_yzx ~active_matrix_from_extrinsic_euler_yzx ~active_matrix_from_intrinsic_euler_zyx ~active_matrix_from_extrinsic_euler_zyx ~active_matrix_from_intrinsic_euler_zxy ~active_matrix_from_extrinsic_euler_zxy ~active_matrix_from_extrinsic_roll_pitch_yaw ~intrinsic_euler_xzx_from_active_matrix ~extrinsic_euler_xzx_from_active_matrix ~intrinsic_euler_xyx_from_active_matrix ~extrinsic_euler_xyx_from_active_matrix ~intrinsic_euler_yxy_from_active_matrix ~extrinsic_euler_yxy_from_active_matrix ~intrinsic_euler_yzy_from_active_matrix ~extrinsic_euler_yzy_from_active_matrix ~intrinsic_euler_zyz_from_active_matrix ~extrinsic_euler_zyz_from_active_matrix ~intrinsic_euler_zxz_from_active_matrix ~extrinsic_euler_zxz_from_active_matrix ~intrinsic_euler_xzy_from_active_matrix ~extrinsic_euler_xzy_from_active_matrix ~intrinsic_euler_xyz_from_active_matrix ~extrinsic_euler_xyz_from_active_matrix ~intrinsic_euler_yxz_from_active_matrix ~extrinsic_euler_yxz_from_active_matrix ~intrinsic_euler_yzx_from_active_matrix ~extrinsic_euler_yzx_from_active_matrix ~intrinsic_euler_zyx_from_active_matrix ~extrinsic_euler_zyx_from_active_matrix ~intrinsic_euler_zxy_from_active_matrix ~extrinsic_euler_zxy_from_active_matrix :mod:`pytransform3d.transformations` ==================================== .. automodule:: pytransform3d.transformations :no-members: :no-inherited-members: Transformation Matrix --------------------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~check_transform ~transform_requires_renormalization ~random_transform ~concat ~invert_transform ~transform ~vector_to_point ~vectors_to_points ~vector_to_direction ~vectors_to_directions ~transform_power ~transform_sclerp ~adjoint_from_transform ~plot_transform ~assert_transform ~transform_from ~translate_transform ~rotate_transform ~transform_from_pq ~transform_from_exponential_coordinates ~transform_from_transform_log ~transform_from_dual_quaternion Position and Quaternion ----------------------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~check_pq ~pq_slerp ~pq_from_transform ~pq_from_dual_quaternion Screw Parameters ---------------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~check_screw_parameters ~plot_screw ~assert_screw_parameters_equal ~screw_parameters_from_screw_axis ~screw_parameters_from_dual_quaternion Screw Axis ---------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~check_screw_axis ~random_screw_axis ~screw_axis_from_screw_parameters ~screw_axis_from_exponential_coordinates ~screw_axis_from_screw_matrix Exponential Coordinates ----------------------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~check_exponential_coordinates ~norm_exponential_coordinates ~random_exponential_coordinates ~assert_exponential_coordinates_equal ~exponential_coordinates_from_transform ~exponential_coordinates_from_screw_axis ~exponential_coordinates_from_transform_log Screw Matrix ------------ .. autosummary:: :toctree: _apidoc/ :template: function.rst ~check_screw_matrix ~screw_matrix_from_screw_axis ~screw_matrix_from_transform_log Logarithm of Transformation --------------------------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~check_transform_log ~transform_log_from_exponential_coordinates ~transform_log_from_screw_matrix ~transform_log_from_transform Dual Quaternion --------------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~check_dual_quaternion ~dual_quaternion_requires_renormalization ~norm_dual_quaternion ~dual_quaternion_squared_norm ~assert_unit_dual_quaternion ~assert_unit_dual_quaternion_equal ~dual_quaternion_double ~dq_conj ~dq_q_conj ~concatenate_dual_quaternions ~dq_prod_vector ~dual_quaternion_power ~dual_quaternion_sclerp ~dual_quaternion_from_transform ~dual_quaternion_from_pq ~dual_quaternion_from_screw_parameters Jacobians --------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~left_jacobian_SE3 ~left_jacobian_SE3_series ~left_jacobian_SE3_inv ~left_jacobian_SE3_inv_series Deprecated Functions -------------------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~scale_transform :mod:`pytransform3d.batch_rotations` ==================================== .. automodule:: pytransform3d.batch_rotations :no-members: :no-inherited-members: Rotation Matrices ----------------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~active_matrices_from_angles ~active_matrices_from_intrinsic_euler_angles ~active_matrices_from_extrinsic_euler_angles ~matrices_from_compact_axis_angles ~matrices_from_quaternions Axis-Angle Representation ------------------------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~norm_axis_angles ~axis_angles_from_matrices ~axis_angles_from_quaternions ~cross_product_matrices Quaternions ----------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~batch_concatenate_quaternions ~batch_q_conj ~quaternion_slerp_batch ~smooth_quaternion_trajectory ~quaternions_from_matrices ~batch_quaternion_wxyz_from_xyzw ~batch_quaternion_xyzw_from_wxyz Utility Functions ----------------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~norm_vectors ~angles_between_vectors :mod:`pytransform3d.trajectories` ================================= .. automodule:: pytransform3d.trajectories :no-members: :no-inherited-members: Transformation Matrices ----------------------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~random_trajectories ~invert_transforms ~concat_one_to_many ~concat_many_to_one ~concat_many_to_many ~concat_dynamic ~transforms_from_pqs ~transforms_from_exponential_coordinates ~transforms_from_dual_quaternions Positions and Quaternions ------------------------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~plot_trajectory ~pqs_from_transforms ~pqs_from_dual_quaternions Screw Parameters ---------------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~screw_parameters_from_dual_quaternions Exponential Coordinates ----------------------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~mirror_screw_axis_direction ~exponential_coordinates_from_transforms Dual Quaternions ---------------- .. autosummary:: :toctree: _apidoc/ :template: function.rst ~batch_dq_conj ~batch_dq_q_conj ~batch_concatenate_dual_quaternions ~batch_dq_prod_vector ~dual_quaternions_from_pqs ~dual_quaternions_power ~dual_quaternions_sclerp ~dual_quaternions_from_screw_parameters :mod:`pytransform3d.uncertainty` ================================ .. automodule:: pytransform3d.uncertainty :no-members: :no-inherited-members: .. autosummary:: :toctree: _apidoc/ :template: function.rst ~estimate_gaussian_rotation_matrix_from_samples ~estimate_gaussian_transform_from_samples ~frechet_mean ~invert_uncertain_transform ~concat_globally_uncertain_transforms ~concat_locally_uncertain_transforms ~pose_fusion ~to_ellipsoid ~to_projected_ellipsoid ~plot_projected_ellipsoid :mod:`pytransform3d.coordinates` ================================ .. automodule:: pytransform3d.coordinates :no-members: :no-inherited-members: .. autosummary:: :toctree: _apidoc/ :template: function.rst ~cartesian_from_cylindrical ~cartesian_from_spherical ~cylindrical_from_cartesian ~cylindrical_from_spherical ~spherical_from_cartesian ~spherical_from_cylindrical :mod:`pytransform3d.transform_manager` ====================================== .. automodule:: pytransform3d.transform_manager :no-members: :no-inherited-members: .. autosummary:: :toctree: _apidoc/ :template: class.rst ~TransformGraphBase ~TransformManager ~TemporalTransformManager ~TimeVaryingTransform ~StaticTransform ~NumpyTimeseriesTransform :mod:`pytransform3d.editor` =========================== .. automodule:: pytransform3d.editor :no-members: :no-inherited-members: .. autosummary:: :toctree: _apidoc/ :template: class_without_inherited.rst ~TransformEditor :mod:`pytransform3d.urdf` ========================= .. automodule:: pytransform3d.urdf :no-members: :no-inherited-members: .. autosummary:: :toctree: _apidoc/ :template: class.rst ~UrdfTransformManager ~Link ~Joint ~Geometry ~Box ~Sphere ~Cylinder ~Mesh .. autosummary:: :toctree: _apidoc/ :template: function.rst ~parse_urdf ~initialize_urdf_transform_manager :mod:`pytransform3d.camera` =========================== .. automodule:: pytransform3d.camera :no-members: :no-inherited-members: .. autosummary:: :toctree: _apidoc/ :template: function.rst ~make_world_grid ~make_world_line ~cam2sensor ~sensor2img ~world2image ~plot_camera :mod:`pytransform3d.plot_utils` =============================== .. automodule:: pytransform3d.plot_utils :no-members: :no-inherited-members: .. autosummary:: :toctree: _apidoc/ :template: function.rst ~make_3d_axis ~remove_frame ~plot_vector ~plot_length_variable ~plot_box ~plot_sphere ~plot_spheres ~plot_cylinder ~plot_mesh ~plot_ellipsoid ~plot_capsule ~plot_cone .. autosummary:: :toctree: _apidoc/ :template: class_without_inherited.rst ~Arrow3D ~Frame ~LabeledFrame ~Trajectory ~Camera :mod:`pytransform3d.visualizer` =============================== .. automodule:: pytransform3d.visualizer :no-members: :no-inherited-members: .. autosummary:: :toctree: _apidoc/ :template: function.rst ~figure .. autosummary:: :toctree: _apidoc/ :template: class.rst ~Figure ~Artist ~Line3D ~PointCollection3D ~Vector3D ~Frame ~Trajectory ~Sphere ~Box ~Cylinder ~Mesh ~Ellipsoid ~Capsule ~Cone ~Plane ~Graph ~Camera ================================================ FILE: doc/source/conf.py ================================================ # -*- coding: utf-8 -*- import sys import os import glob import shutil import time from sphinx_gallery.scrapers import figure_rst sys.path.insert(0, os.path.abspath('.')) sys.path.insert(0, os.path.abspath('../..')) extensions = [ "sphinx.ext.autodoc", "sphinx.ext.autosummary", "sphinx.ext.doctest", "sphinx.ext.intersphinx", "sphinx.ext.todo", "sphinx.ext.coverage", "sphinx.ext.mathjax", #"sphinx.ext.imgmath", "sphinx.ext.ifconfig", "sphinx.ext.viewcode", "matplotlib.sphinxext.plot_directive", "numpydoc", "sphinx_gallery.gen_gallery", ] autodoc_default_options = {"member-order": "bysource"} autosummary_generate = True # generate files at doc/source/_apidoc class_members_toctree = False numpydoc_show_class_members = False # Options for the `::plot` directive: # https://matplotlib.org/stable/api/sphinxext_plot_directive_api.html plot_formats = ["png"] plot_include_source = False plot_html_show_formats = False plot_html_show_source_link = False # class template from https://stackoverflow.com/a/62613202/915743 templates_path = ["_templates"] exclude_patterns = [] exclude_trees = ["_templates", "sphinxext"] source_suffix = '.rst' source_encoding = 'utf-8-sig' master_doc = 'index' project = u'pytransform3d' copyright = u"2014-{}, Alexander Fabisch, DFKI GmbH, Robotics Innovation Center".format(time.strftime("%Y")) version = __import__("pytransform3d").__version__ release = __import__("pytransform3d").__version__ language = 'en' today_fmt = '%B %d, %Y' add_function_parentheses = True author = "Alexander Fabisch" show_authors = True pygments_style = 'sphinx' html_logo = "_static/logo.png" html_theme = "pydata_sphinx_theme" html_theme_options = { "logo": { "alt_text": f"pytransform3d {release}", }, "collapse_navigation": True, } html_sidebars = { "install": [], "api": [], } html_static_path = ['_static'] html_css_files = ['custom.css'] html_last_updated_fmt = '%b %d, %Y' html_use_smartypants = True html_show_sourcelink = False html_show_sphinx = False html_show_copyright = True intersphinx_mapping = { "python": (f"https://docs.python.org/{sys.version_info.major}", None), "numpy": ("https://numpy.org/doc/stable", None), "scipy": ("https://docs.scipy.org/doc/scipy/reference", None), "matplotlib": ("https://matplotlib.org/", None) } intersphinx_timeout = 10 class Open3DScraper: def __repr__(self): return f"<{type(self).__name__} object>" def __call__(self, block, block_vars, gallery_conf, **kwargs): """Scrape Open3D images. Parameters ---------- block : tuple A tuple containing the (label, content, line_number) of the block. block_vars : dict Dict of block variables. gallery_conf : dict Contains the configuration of Sphinx-Gallery **kwargs : dict Additional keyword arguments to pass to :meth:`~matplotlib.figure.Figure.savefig`, e.g. ``format='svg'``. The ``format`` kwarg in particular is used to set the file extension of the output file (currently only 'png', 'jpg', and 'svg' are supported). Returns ------- rst : str The ReSTructuredText that will be rendered to HTML containing the images. """ path_current_example = os.path.dirname(block_vars['src_file']) jpgs = sorted(glob.glob(os.path.join( path_current_example, "__open3d_rendered_image.jpg"))) image_names = list() image_path_iterator = block_vars["image_path_iterator"] for jpg in jpgs: this_image_path = image_path_iterator.next() image_names.append(this_image_path) shutil.move(jpg, this_image_path) return figure_rst(image_names, gallery_conf["src_dir"]) def _get_sg_image_scraper(): """Return the callable scraper to be used by Sphinx-Gallery. It allows us to just use strings as they already can be for 'matplotlib' and 'mayavi'. Details on this implementation can be found in `sphinx-gallery/sphinx-gallery/494`_ This is required to make the config pickable. This function must be imported into the top level namespace of pytransform3d. .. _sphinx-gallery/sphinx-gallery/494: https://github.com/sphinx-gallery/sphinx-gallery/pull/494 """ return Open3DScraper() # monkeypatching pytransform3d to make the config pickable __import__("pytransform3d")._get_sg_image_scraper = _get_sg_image_scraper sphinx_gallery_conf = { "examples_dirs": "../../examples", "gallery_dirs": "_auto_examples", "reference_url": {"pytransform3d": None}, "filename_pattern": "/(?:plot|animate|vis)_", "image_scrapers": ("matplotlib", "pytransform3d"), "matplotlib_animations": (True, "mp4"), "backreferences_dir": "_auto_examples/backreferences", "doc_module": "pytransform3d", } ================================================ FILE: doc/source/index.rst ================================================ .. pytransform3d documentation master file, created by sphinx-quickstart on Thu Nov 20 21:01:30 2014. ============= pytransform3d ============= .. toctree:: :hidden: install user_guide/index api _auto_examples/index This documentation explains how you can work with pytransform3d and with 3D transformations in general. ----- Scope ----- pytransform3d 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. The library integrates well with the `scientific Python ecosystem `_ with its core libraries NumPy, SciPy and Matplotlib. We rely on `NumPy `_ for linear algebra and on `Matplotlib `_ to offer plotting functionalities. `SciPy `_ is used if you want to automatically compute transformations from a graph of transformations. pytransform3d offers... * operations for most common representations of rotation / orientation and translation / position * conversions between those representations * clear documentation of conventions * functions to check for common pitfalls (e.g., singularities, ambiguities, discontinuities) * tight coupling with matplotlib to quickly visualize (or animate) transformations * the TransformManager which organizes complex chains of transformations * the TransformEditor which allows to modify transformations graphically * the UrdfTransformManager which is able to load transformations from `URDF `_ files * a matplotlib-like interface to Open3D's visualizer to display geometries and transformations The design philosophy of pytransform3d is to not hide anything; there is no magic: no operator overloading, no classes to store data (rotations or transformations) other than NumPy arrays, and no layers of abstraction. It is as transparent to the user as possible and its interface is mainly functional. -------- Citation -------- If you use pytransform3d for a scientific publication, I would appreciate citation of the following paper: Fabisch, A. (2019). pytransform3d: 3D Transformations for Python. Journal of Open Source Software, 4(33), 1159, https://doi.org/10.21105/joss.01159 ================================================ FILE: doc/source/install.rst ================================================ ============ Installation ============ You can install pytransform3d with pip: .. code-block:: bash pip install pytransform3d or conda (since version 1.8): .. code-block:: bash conda install -c conda-forge pytransform3d (More detailed instructions `here `_.) --------------------- Optional Dependencies --------------------- When using pip, you can install pytransform3d with the options all, doc, and test. * `all` will add support for loading meshes, the 3D visualizer of pytransform3d, and pydot export of `TransformManager` objects. * `doc` will install necessary dependencies to build this documentation. * `test` will install dependencies to run the unit tests. For example, you can call .. code-block:: bash python -m pip install 'pytransform3d[all]' Unfortunately, pip cannot install all dependencies: * If you want to have support for pydot export of `TransformManager` objects, make sure to install graphviz (on Ubuntu: `sudo apt install graphviz`) if you want to use this feature. * If you want to have support for the Qt GUI you have to install PyQt 4 or 5 (on Ubuntu: `sudo apt install python3-pyqt5`; conda: `conda install pyqt`). ------------------------ Installation from Source ------------------------ You can also install from the current git version. pytransform3d is available at `GitHub `_. Clone the repository and go to the main folder. Install dependencies with: .. code-block:: bash pip install -r requirements.txt Install the package with: .. code-block:: bash python setup.py install pip also supports installation from a git repository: .. code-block:: bash pip install git+https://github.com/dfki-ric/pytransform3d.git ================================================ FILE: doc/source/user_guide/camera.rst ================================================ ====== Camera ====== When we know the 3D position of a point in the world we can easily compute where we would see it in a camera image with the pinhole camera model. However, we have to know some parameters of the camera: * camera pose * focal length :math:`f` * sensor width and height * image width and height .. image:: ../_static/camera.png :alt: Camera :align: center :width: 60% | Note that light passes through a pinhole in a real pinhole camera before it will be measured from the sensor so that pixels will be mirrored in the x-y plane. The sensor that we show here actually corresponds to the virtual image plane. The Example :ref:`sphx_glr__auto_examples_plots_plot_camera_with_image.py` shows how a grid is projected on an image with the function :func:`~pytransform3d.camera.world2image`. .. figure:: ../_auto_examples/plots/images/sphx_glr_plot_camera_with_image_001.png :target: ../_auto_examples/plots/plot_camera_with_image.html :align: center Extrinsic and intrinsic camera parameters can be visualized in the following way. The extrinsic camera parameters are fully determined by a transform from world coordinates to camera coordinates or by the pose of the camera in the world. In this illustration, the point indicates the camera center / center of projection, which is the position component of the pose. The orientation determines the direction to and orientation of the virtual image plane. The arrow at the top of the virtual image plane shows the up direction of the image. The field of view is determined from the intrinsic camera parameters. These are given by a matrix .. math:: \left( \begin{array}{ccc} f_x & 0 & c_x\\ 0 & f_y & c_y\\ 0 & 0 & 1 \end{array} \right), where :math:`f_x, f_y` are focal lengths and :math:`c_x, c_y` is the position of the camera center. Together with the image size we can determine the field of view. Values of the intrinsic camera matrix and the image size can be given in pixels or meters to generate the following visualization with :func:`~pytransform3d.camera.plot_camera` (see Example :ref:`sphx_glr__auto_examples_plots_plot_camera_3d.py`). .. figure:: ../_auto_examples/plots/images/sphx_glr_plot_camera_3d_001.png :target: ../_auto_examples/plots/plot_camera_3d.html :align: center You can use this to display a trajectory of camera poses (see Example :ref:`sphx_glr__auto_examples_plots_plot_camera_trajectory.py`). .. figure:: ../_auto_examples/plots/images/sphx_glr_plot_camera_trajectory_001.png :target: ../_auto_examples/plots/plot_camera_trajectory.html :align: center ================================================ FILE: doc/source/user_guide/euler_angles.rst ================================================ .. _euler_angles: ============ Euler Angles ============ Since Euler angles [1]_ are an intuitive way to specify a rotation in 3D, they are often exposed at user interfaces. However, there are 24 different conventions that could be used. Furthermore, you have to find out whether degrees or radians are used to express the angles (we will only use radians in pytransform3d). .. figure:: ../_auto_examples/plots/images/sphx_glr_plot_euler_angles_001.png :target: ../_auto_examples/plots/plot_euler_angles.html :align: center Here we rotate about the extrinsic (fixed) x-axis, y-axis, and z-axis by 90 degrees. -------------- 24 Conventions -------------- Euler angles generally refer to three consecutive rotations about basis vectors. There are proper Euler angles for which we can distinguish 6 conventions: xzx, xyx, yxy, yzy, zyz, and zxz. As you can see, proper Euler angles rotate about the same basis vector during the first and last rotation and they rotate about another basis vector in the second rotation. In addition, there are Cardan (or Tait-Bryan) angles that rotate about three different basis vectors. There are also 6 conventions: xzy, xyz, yxz, yzx, zyx, and zxy. If you read about :doc:`transformation_ambiguities`, you know that the order in which we concatenate rotation matrices matters. We can make extrinsic rotations, in which we rotate about basis vectors of a fixed frame, and we can make intrinsic rotations, in which we rotate about basis vectors of the new, rotated frame. This increases the amount of possible conventions to :math:`2 (6 + 6) = 24` (if we only allow active rotation matrices). --------------- Range of Angles --------------- Euler angles rotate about three basis vectors by the angles :math:`\alpha`, :math:`\beta`, and :math:`\gamma`. If we want to find the Euler angles that correspond to one rotation matrix :math:`\boldsymbol{R}`, there is an infinite number of solutions because we can always add or subtract :math:`2\pi` to one of the angles and get the same result. In addition, for proper Euler angles .. math:: \boldsymbol{R}(\alpha, \beta, \gamma) = \boldsymbol{R}(\alpha + \pi, -\beta, \gamma - \pi). For Cardan angles .. math:: \boldsymbol{R}(\alpha, \beta, \gamma) = \boldsymbol{R}(\alpha + \pi, \pi - \beta, \gamma - \pi). For this reason the proper Euler angles are typically restricted to .. math:: -\pi \leq \alpha < \pi, \qquad 0 \leq \beta \leq \pi, \qquad -\pi \leq \gamma < \pi and Cardan angles are usually restricted to .. math:: -\pi \leq \alpha < \pi, \qquad -\frac{\pi}{2} \leq \beta \leq \frac{\pi}{2}, \qquad -\pi \leq \gamma < \pi to make these representations unique (using :func:`~pytransform3d.rotations.norm_euler`). An alternative convention limits the range of :math:`\alpha` and :math:`\gamma` to :math:`\left[0, 2 \pi\right)`. ----------- Gimbal Lock ----------- The special case of a so-called gimbal lock occurs when the second angle :math:`\beta` is at one of its limits. In this case the axis of rotation for the first and last rotation are either the same or exactly opposite, that is, an infinite number of angles :math:`\alpha` and :math:`\gamma` will represent the same rotation even though we restricted their range to an interval of length :math:`2\pi`: either all pairs of angles that satisfy :math:`\alpha + \gamma = constant` or all pairs of angles that satisfy :math:`\alpha - \gamma = constant`. When we reconstruct Euler angles from a rotation matrix, we set one of these angles to 0 to determine the other. We can check if Euler angles are close to gimbal lock with :func:`~pytransform3d.rotations.euler_near_gimbal_lock`. ----------- Other Names ----------- There are also other names for Euler angles. For example, the extrinsic xyz Cardan angles can also be called roll, pitch, and yaw (or sometimes the intrinsic convention is used here as well). Roll is a rotation about x, pitch is a rotation about y and yaw is a rotation about z. ---------- References ---------- .. [1] Shuster, M. D. (1993). A Survery of Attitude Representations. The Journal of Astronautical Sciences, 41(4), pp. 475-476. http://malcolmdshuster.com/Pub_1993h_J_Repsurv_scan.pdf ================================================ FILE: doc/source/user_guide/index.rst ================================================ ========== User Guide ========== .. toctree:: :numbered: :maxdepth: 2 introduction rotations transformations transformation_ambiguities euler_angles transformation_modeling transform_manager transformation_over_time uncertainty camera ================================================ FILE: doc/source/user_guide/introduction.rst ================================================ ======================================== Introduction to 3D Rigid Transformations ======================================== ------ Basics ------ Following Waldron and Schmiedeler [1]_, we define the basic terms that we use throughout this user guide. .. list-table:: :widths: 15 35 15 35 * - .. image:: ../_static/position.png - **Position** of a rigid body in 3D Euclidean space is expressed as a 3D vector. - .. image:: ../_static/translation.png - **Translation** is a displacement, in which points move along parallel lines by the same distance. * - .. image:: ../_static/frame.png - **Orientation** of a rigid body in 3D Euclidean space is defined by a set of 3 orthogonal basis vectors. - .. image:: ../_static/rotation.png - **Rotation** is a displacement, in which points move about a rotation axis through the origin of the reference frame (fixed point) along a circle by the same angle. * - .. image:: ../_static/position.png :width: 30% :align: center .. image:: ../_static/frame.png :width: 30% :align: center - **Pose** is a combination of position and orientation. - .. image:: ../_static/translation.png :width: 30% :align: center .. image:: ../_static/rotation.png :width: 30% :align: center - A (proper) **rigid transformation** is a combination of translation and rotation. ------ Frames ------ A (coordinate reference) frame in 3D Euclidean space is defined by an origin (position) and 3 orthogonal basis vectors (orientation) and it is attached to a rigid body. The pose (position and orientation) of a rigid body (i.e., of its frame) is always expressed with respect to another frame. .. _Frame Notation: -------------- Frame Notation -------------- Notation is important for describing and understanding of physical quantities in 3D. Furgale [2]_ [3]_ presents one of the most consistent and clear approaches and we use it here. For physical quantities we use the notation :math:`_{A}\boldsymbol{x}_{BC}`, where :math:`\boldsymbol{x}` is a physical quantity of frame C with respect to frame B expressed in frame A. For example, :math:`_{A}\boldsymbol{t}_{BC}` is the translation of C with respect to B measured in A or :math:`_{A}\boldsymbol{\omega}_{BC}` is the orientation vector of C with respect to B measured in A. Since :math:`_A\boldsymbol{t}_{BC}` represents a vector or translation from frame B to frame C expressed in frame A, the position of a point :math:`P` with respect to a frame A in three-dimensional space can be defined by :math:`_A\boldsymbol{p} := _A\boldsymbol{t}_{AP}`. When we define a mapping from some frame A to another frame B that can be expressed as a matrix multiplication, we use the notation :math:`\boldsymbol{M}_{BA}` for the corresponding matrix. We can read this from right to left as a matrix that maps from frame A to frame B through multiplication, for example, when we want to transform a point by .. math:: _B\boldsymbol{p} = \boldsymbol{M}_{BA} {_A\boldsymbol{p}} ------------------------------------ Duality of Transformations and Poses ------------------------------------ We can use a transformation matrix :math:`\boldsymbol{T}_{BA}` that represents a transformation from frame A to frame B to represent the pose (position and orientation) of frame A in frame B (if we use the active transformation convention; see :ref:`transformation_ambiguities` for details). This is just a different interpretation of the same matrix and similar to our interpretation of a vector from A to P :math:`_A\boldsymbol{t}_{AP}` as a point :math:`_A\boldsymbol{p}`. --------------- Representations --------------- At least six numbers are required to express the pose of a rigid body or a transformation between two frames, but there are also redundant representations. We can use many different representations of rotation and / or translation. Here is an overview of the representations that are available in pytransform3d. All representations are stored in NumPy arrays, of which the corresponding shape is shown in this figure. You will find more details on these representations on the following pages. .. figure:: ../_static/overview.png :alt: Overview of representations for rotations and transformations in 3D. :align: center :width: 100% ---------- References ---------- .. [1] Waldron, K., Schmiedeler, J. (2008). Kinematics. In: Siciliano, B., Khatib, O. (eds) Springer Handbook of Robotics. Springer, Berlin, Heidelberg. https://doi.org/10.1007/978-3-540-30301-5_2 .. [2] Furgale, P. (2014). Representing Robot Pose: The good, the bad, and the ugly (slides). In What Sucks in Robotics and How to Fix It: Lessons Learned from Building Complex Systems (ICRA workshop). Note: these slides seem to be lost, but the blog below conveys the same message. http://static.squarespace.com/static/523c5c56e4b0abc2df5e163e/t/53957839e4b05045ad65021d/1402304569659/Workshop+-+Rotations_v102.key.pdf .. [3] Furgale, P. (2014). Representing Robot Pose: The good, the bad, and the ugly (blog). https://web.archive.org/web/20220420162355/http://paulfurgale.info/news/2014/6/9/representing-robot-pose-the-good-the-bad-and-the-ugly ================================================ FILE: doc/source/user_guide/rotations.rst ================================================ =================== SO(3): 3D Rotations =================== The group of all rotations in the 3D Cartesian space is called :math:`SO(3)` (SO: special orthogonal group). It is typically represented by 3D rotation matrices [7]_. The minimum number of components that are required to describe any rotation from :math:`SO(3)` is 3. However, there is no representation that is non-redundant, continuous, and free of singularities. .. figure:: ../_static/rotations.png :alt: Rotations :width: 50% :align: center Overview of the representations and the conversions between them that are available in pytransform3d. Not all representations support all operations directly without conversion to another representation. The following table is an overview. If the operation is not implemented in pytransform3d then it is shown in brackets. +----------------------------------------+---------------+--------------------+---------------+-----------------------+-----------------+ | Representation | Inverse | Rotation of vector | Concatenation | Interpolation | Renormalization | +========================================+===============+====================+===============+=======================+=================+ | Rotation matrix | Transpose | Yes | Yes | SLERP | Required | | :math:`\pmb{R}` | | | | | | +----------------------------------------+---------------+--------------------+---------------+-----------------------+-----------------+ | Axis-angle | Negative axis | No | No | SLERP | Not necessary | | :math:`(\hat{\pmb{\omega}}, \theta)` | | | | | | +----------------------------------------+---------------+--------------------+---------------+-----------------------+-----------------+ | Rotation vector | Negative | No | No | SLERP / `(2)` / `(3)` | Not required | | :math:`\pmb{\omega}` | | | | | | +----------------------------------------+---------------+--------------------+---------------+-----------------------+-----------------+ | Logarithm of rotation | Negative | No | No | SLERP / `(2)` / `(3)` | Not required | | :math:`\left[\pmb{\omega}\right]` | | | | | | +----------------------------------------+---------------+--------------------+---------------+-----------------------+-----------------+ | Quaternion | Conjugate | Yes | Yes | SLERP | Required | | :math:`\pmb{q}` | | | | | | +----------------------------------------+---------------+--------------------+---------------+-----------------------+-----------------+ | Rotor | Reverse | Yes | Yes | SLERP | Required | | :math:`R` | | | | | | +----------------------------------------+---------------+--------------------+---------------+-----------------------+-----------------+ | Euler angles | `(1)` | No | No | No | Not necessary | | :math:`(\alpha, \beta, \gamma)` | | | | | | +----------------------------------------+---------------+--------------------+---------------+-----------------------+-----------------+ | Modified Rodrigues parameters | Negative | No | Yes | No | Not required | | :math:`\pmb{\psi}` | | | | | | +----------------------------------------+---------------+--------------------+---------------+-----------------------+-----------------+ Footnotes: SLERP means Spherical Linear intERPolation. This can either be implemented directly for two instances of the representation or sometimes involves a conversion to a rotation vector that represents the difference of the two orientations. `(1)` Inversion of Euler angles in convention ABC (e.g., xyz) requires using another convention CBA (e.g., zyx), reversing the order of angles, and taking the negative of these. `(2)` Linear interpolation is approximately correct for small differences. `(3)` Fractions of this representation represent partial rotations, but a conversion to another representation is required to interpolate between orientations. --------------- Rotation Matrix --------------- One of the most practical representations of orientation is a rotation matrix .. math:: \boldsymbol R = \left( \begin{array}{ccc} r_{11} & r_{12} & r_{13}\\ r_{21} & r_{22} & r_{23}\\ r_{31} & r_{32} & r_{33}\\ \end{array} \right) \in SO(3). Note that this is a non-minimal representation for orientations because we have 9 values but only 3 degrees of freedom. This is because :math:`\boldsymbol R` is orthonormal, which results in 6 constraints (enforced with :func:`~pytransform3d.rotations.norm_matrix` and checked with :func:`~pytransform3d.rotations.matrix_requires_renormalization` or :func:`~pytransform3d.rotations.check_matrix`): * column vectors must have unit norm (3 constraints) * and must be orthogonal to each other (3 constraints) A more compact representation of these constraints is :math:`\boldsymbol R^T \boldsymbol R = \boldsymbol I \Leftrightarrow \boldsymbol R^T = \boldsymbol R^{-1}`. In addition, :math:`\det(\boldsymbol R) = 1` because we use right-handed coordinate system (:math:`\det(\boldsymbol R) = -1` for left-handed coordinate systems). Hence, the group :math:`SO(3)` is defined as .. math:: SO(3) = \{\boldsymbol{R} \in \mathbb{R}^{3 \times 3} | \boldsymbol{R}\boldsymbol{R}^T = \boldsymbol{I}, \det(\boldsymbol{R}) = 1\}. pytransform3d uses a numpy array of shape (3, 3) to represent rotation matrices and typically we use the variable name R for a rotation matrix. .. warning:: There are two conventions on how to interpret rotations: active or passive rotation. The standard in pytransform3d is an active rotation. We can use a rotation matrix :math:`\boldsymbol R_{BA}` to transform a point :math:`_A\boldsymbol{p}` from frame :math:`A` to frame :math:`B`. .. warning:: There are two different conventions on how to use rotation matrices to apply a rotation to a vector. We can either (pre-)multiply the rotation matrix to a column vector from the left side or we can (post-)multiply it to a row vector from the right side. We will use the **pre-multiplication** convention. This means that we rotate a point :math:`_A\boldsymbol{p}` by .. math:: _B\boldsymbol{p} = \boldsymbol{R}_{BAA} \boldsymbol{p} This is called **linear map**. Note that with our index notation (as explained in :ref:`Frame Notation`) and these conventions, the second index of the rotation matrix and the left index of the point have to be the same (:math:`A` in this example). The rotation is applied incorrectly if this is not the case. *Each column* of a rotation matrix :math:`\boldsymbol{R}_{BA}` is a basis vector of frame :math:`A` with respect to frame :math:`B`. We can plot the basis vectors of an orientation to visualize it. Here, we can see orientation represented by the rotation matrix .. math:: \boldsymbol R = \left( \begin{array}{ccc} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1\\ \end{array} \right). .. plot:: :include-source: from pytransform3d.rotations import plot_basis plot_basis() .. note:: When plotting basis vectors, it is a convention to use red for the x-axis, green for the y-axis and blue for the z-axis (RGB for xyz). We can easily chain multiple rotations: we can apply the rotation defined by :math:`\boldsymbol R_{CB}` after the rotation :math:`\boldsymbol R_{BA}` by applying the rotation .. math:: \boldsymbol R_{CA} = \boldsymbol R_{CB} \boldsymbol R_{BA}. Note that the indices have to align again. Otherwise rotations are not applied in the correct order. .. warning:: There are two different conventions on how to concatenate rotation matrices. Suppose we have a rotation matrix :math:`R_1` and another matrix :math:`R_2` and we want to first rotate by :math:`R_1` and then by :math:`R_2`. If we want to apply both rotations in global coordinates, we have to concatenate them with :math:`R_2 \cdot R_1`. We can also express the second rotation in terms of a local, body-fixed coordinates by :math:`R_1 \cdot R_2`, which means :math:`R_1` defines new coordinates in which :math:`R_2` is applied. Note that this applies to both passive and active rotation matrices. The easiest way to construct rotation matrices is through rotations about the basis vectors with :func:`~pytransform3d.rotations.active_matrix_from_angle`. Multiple rotation matrices that were constructed like this can be concatenated. This will be done, for instance, to obtain rotation matrices from Euler angles (see :doc:`euler_angles`). **Pros** * Supported operations: all except interpolation. * Interpretation: each column is a basis vector. * Singularities: none. * Ambiguities: none that are specific for rotation matrices. **Cons** * Representation: 9 values for 3 degrees of freedom. * Renormalization: expensive in comparison to quaternions. ---------- Axis-Angle ---------- .. figure:: ../_auto_examples/plots/images/sphx_glr_plot_axis_angle_001.png :target: ../_auto_examples/plots/plot_axis_angle.html :width: 50% :align: center Each rotation can be represented by a single rotation about one axis. The axis can be represented as a three-dimensional unit vector and the angle by a scalar: .. math:: \left( \hat{\boldsymbol{\omega}}, \theta \right) = \left( \left( \begin{array}{c}\omega_x\\\omega_y\\\omega_z\end{array} \right), \theta \right) pytransform3d uses a numpy array of shape (4,) for the axis-angle representation of a rotation, where the first 3 entries correspond to the unit axis of rotation and the fourth entry to the rotation angle in radians, and typically we use the variable name a. Note that the axis-angle representation has a singularity at :math:`\theta = 0` as there is an infinite number of rotation axes that represent the identity rotation in this case. However, we can modify the representation to avoid this singularity. --------------- Rotation Vector --------------- Since :math:`||\hat{\boldsymbol{\omega}}|| = 1`, it is possible to write this in a more compact way as a rotation vector [2]_ .. math:: \boldsymbol{\omega} = \hat{\boldsymbol{\omega}} \theta \in \mathbb{R}^3. In code, we call this the compact axis-angle representation. pytransform3d uses a numpy array of shape (3,) for the compact axis-angle representation of a rotation and typically it uses the variable name a. We can also refer to this representation as **exponential coordinates of rotation** [5]_. We can represent angular velocity as :math:`\hat{\boldsymbol{\omega}} \dot{\theta}` and angular acceleration as :math:`\hat{\boldsymbol{\omega}} \ddot{\theta}` so that we can easily do component-wise integration and differentiation with this representation. **Pros** * Representation: minimal. * Supported operations: interpolation; can also represent angular velocity and acceleration. **Cons** * Ambiguities: an angle of 0 and any multiple of :math:`2\pi` represent the same orientation (can be avoided with :func:`~pytransform3d.rotations.norm_compact_axis_angle`, which introduces discontinuities); when :math:`\theta = \pi`, the axes :math:`\hat{\boldsymbol{\omega}}` and :math:`-\hat{\boldsymbol{\omega}}` represent the same rotation. * Supported operations: concatenation and transformation of vectors requires conversion to another representation. --------------------- Logarithm of Rotation --------------------- In addition, we can represent :math:`\hat{\boldsymbol{\omega}} \theta` by the cross-product matrix (:func:`~pytransform3d.rotations.cross_product_matrix`) .. math:: \left[\hat{\boldsymbol{\omega}}\right] \theta = \left( \begin{matrix} 0 & -\omega_3 & \omega_2\\ \omega_3 & 0 & -\omega_1\\ -\omega_2 & \omega_1 & 0\\ \end{matrix} \right) \theta \in so(3) \subset \mathbb{R}^{3 \times 3}, where :math:`\left[\hat{\boldsymbol{\omega}}\right] \theta` is the matrix logarithm of a rotation matrix and :math:`so(3)` is the Lie algebra of the Lie group :math:`SO(3)`. ----------- Quaternions ----------- Quaternions are represented by a scalar / real part :math:`w` and a vector / imaginary part :math:`x \boldsymbol{i} + y \boldsymbol{j} + z \boldsymbol{k}`. .. math:: \boldsymbol{q} = w + x \boldsymbol{i} + y \boldsymbol{j} + z \boldsymbol{k} \in \mathbb{H} .. warning:: There are two different quaternion conventions: Hamilton's convention defines :math:`ijk = -1` and the Shuster or JPL convention (from NASA's Jet Propulsion Laboratory, JPL) defines :math:`ijk = 1` [1]_. These two conventions result in different multiplication operations and conversions to other representations. We use Hamilton's convention. Read `this paper `_ for details about the two conventions and why Hamilton's convention should be used. Section VI A gives further useful hints to identify which convention is used. The unit quaternion space :math:`S^3` can be used to represent orientations with an encoding based on the rotation axis and angle. A rotation quaternion is a four-dimensional unit vector (versor) :math:`\boldsymbol{\hat{q}}`. The following equation describes its relation to axis-angle notation. .. math:: \boldsymbol{\hat{q}} = \left( \begin{array}{c} w\\ x\\ y\\ z\\ \end{array} \right) = \left( \begin{array}{c} \cos \frac{\theta}{2}\\ \omega_x \sin \frac{\theta}{2}\\ \omega_y \sin \frac{\theta}{2}\\ \omega_z \sin \frac{\theta}{2}\\ \end{array} \right) = \left( \begin{array}{c} \cos \frac{\theta}{2}\\ \hat{\boldsymbol{\omega}} \sin \frac{\theta}{2}\\ \end{array} \right) pytransform3d uses a numpy array of shape (4,) for quaternions and typically we use the variable name q. .. warning:: The scalar component :math:`w` of a quaternion is sometimes the first element and sometimes the last element of the versor. We will use the first element to store the scalar component. Since the other convention is also used often, pytransform3d provides the functions :func:`~pytransform3d.rotations.quaternion_wxyz_from_xyzw` and :func:`~pytransform3d.rotations.quaternion_xyzw_from_wxyz` for conversion. .. warning:: The *antipodal* unit quaternions :math:`\boldsymbol{\hat{q}}` and :math:`-\boldsymbol{\hat{q}}` (:func:`~pytransform3d.rotations.quaternion_double`) represent the same rotation (double cover). This must be considered during operations like interpolation, distance calculation, or (approximate) equality checks. The quaternion product (:func:`~pytransform3d.rotations.concatenate_quaternions`) can be used to concatenate rotations like the matrix product can be used to concatenate rotations represented by rotation matrices. The inverse of the rotation represented by the unit quaternion :math:`\boldsymbol{q}` is represented by the conjugate :math:`\boldsymbol{q}^*` (:func:`~pytransform3d.rotations.q_conj`). We can rotate a vector by representing it as a pure quaternion (i.e., with a scalar part of 0) and then left-multiplying the quaternion and right-multiplying its conjugate :math:`\boldsymbol{q}\boldsymbol{v}\boldsymbol{q}^*` with the quaternion product (:func:`~pytransform3d.rotations.q_prod_vector`). **Pros** * Representation: compact. * Renormalization: checked with :func:`~pytransform3d.rotations.quaternion_requires_renormalization`; cheap in comparison to rotation matrices (); less susceptible to round-off errors than matrix representation. * Discontinuities: none. * Computational efficiency: the quaternion product is cheaper than the matrix product. * Singularities: none. **Cons** * Interpretation: not straightforward. * Ambiguities: double cover. ------------ Euler Angles ------------ A complete rotation can be split into three rotations around basis vectors. pytransform3d uses a numpy array of shape (3,) for Euler angles, where each entry corresponds to a rotation angle in radians around one basis vector. The basis vector that will be used and the order of rotation is defined by the convention that we use. See :doc:`euler_angles` for more information. .. warning:: There are 24 different conventions for defining euler angles. There are 12 different valid ways to sequence rotation axes that can be interpreted as extrinsic or intrinsic rotations: XZX, XYX, YXY, YZY, ZYZ, ZXZ, XZY, XYZ, YXZ, YZX, ZYX, and ZXY. **Pros** * Representation: minimal. * Interpretation: easy to interpret for users (when the convention is clearly defined) in comparison to axis-angle or quaternions. **Cons** * Ambiguities: 24 different conventions, infinitely many Euler angles represent the same rotation without proper limits for the angles. * Singularity: gimbal lock. * Supported operations: all operations require conversion to another representation. ------ Rotors ------ .. figure:: ../_auto_examples/plots/images/sphx_glr_plot_bivector_001.png :target: ../_auto_examples/plots/plot_bivector.html :width: 70% :align: center Rotors and quaternions are very similar concepts in 3D. However, rotors are more general as they can be extended to more dimensions [3]_ [4]_. The concept of a quaternion builds on the axis-angle representation, in which we rotate by an angle about a rotation axis (see black arrow in the illustration above). The axis can be computed from the cross product of two vectors (gray arrows). A rotor builds on a plane-angle representation, in which we rotate with a given direction by an angle in a plane (indicated by gray area). The plane can be computed from the wedge product :math:`a \wedge b` (see :func:`~pytransform3d.rotations.wedge`) of two vectors :math:`a` and :math:`b`, which is a so-called bivector. Although both approaches might seem different, in 3D they operate with exactly the same numbers in exactly the same way. .. warning:: The rotors :math:`R` and :math:`-R` represent exactly the same rotation. A rotor is a unit multivector .. math:: R = (a, b_{yz}, b_{zx}, b_{xy}) that consists of a scalar :math:`a` and a bivector :math:`(b_{yz}, b_{zx}, b_{xy})`. The components of a bivector constructed by the wedge product of two vectors can be interpreted as the area of the parallelogram formed by the two vectors projected on the three basis planes yz, zx, and xy (see illustration above). These values also correspond to the x-, y-, and z-components of the cross product of the two vectors, which allows two different interpretations of the same numbers from which we can then derive quaternions on the one hand and rotors on the other hand. .. warning:: In pytransform3d our convention is that we organize the components of a rotor in exactly the same way as we organize the components of the equivalent quaternion. There are other conventions. It is not just possible to change the order of the scalar and the bivector (similar to a quaterion), but also to change the order of bivector components. Pros and cons for rotors are the same as for quaternions as they have the same representation in 3D. ----------------------------- Modified Rodrigues Parameters ----------------------------- Another minimal representation of rotation are modified Rodrigues parameters (MRP) [6]_ [8]_ .. math:: \boldsymbol{\psi} = \tan \left(\frac{\theta}{4}\right) \hat{\boldsymbol{\omega}} This representation is similar to the compact axis-angle representation. However, the angle of rotation is converted to :math:`\tan(\frac{\theta}{4})`. Hence, there is an easy conversion from unit quaternions to MRP (:func:`~pytransform3d.rotations.mrp_from_quaternion`): .. math:: \boldsymbol{\psi} = \frac{ \left( \begin{array}{c} x\\ y\\ z\\ \end{array} \right)}{1 + w} given some quaternion with a scalar :math:`w` and a vector :math:`\left(x, y, z \right)^T`. pytransform3d uses a numpy array of shape (3,) for modified Rodrigues parameters. .. warning:: MRPs have a singuarity at :math:`2 \pi` (see :func:`~pytransform3d.rotations.mrp_near_singularity`) which we can avoid by ensuring the angle of rotation does not exceed :math:`\pi` (with :func:`~pytransform3d.rotations.norm_mrp`). .. warning:: MRPs have two representations for the same rotation: :math:`\boldsymbol{\psi}` and :math:`-\frac{1}{||\boldsymbol{\psi}||^2} \boldsymbol{\psi}` (:func:`~pytransform3d.rotations.mrp_double`) represent the same rotation and correspond to two antipodal unit quaternions [8]_. **Pros** * Representation: minimal. **Cons** * Interpretation: not straightforward. * Singularity: at :math:`\theta = 2 \pi`. * Ambiguity: double cover. * Supported operations: transformation of vectors requires conversion to another representation. ---------- References ---------- .. [1] Sommer, H., Gilitschenski, I., Bloesch, M., Weiss, S., Siegwart, R., Nieto, J. (2018). Why and How to Avoid the Flipped Quaternion Multiplication. Aerospace, 5(3), pp. 2226-4310, doi: 10.3390/aerospace5030072. https://arxiv.org/pdf/1801.07478.pdf .. [2] Gehring, C., Bellicoso, C. D., Bloesch, M., Sommer, H., Fankhauser, P., Hutter, M., Siegwart, R. (2024). Kindr cheat sheet. https://github.com/ANYbotics/kindr/blob/master/doc/cheatsheet/cheatsheet_latest.pdf .. [3] ten Bosch, M. (2020). Let's remove Quaternions from every 3D Engine. https://marctenbosch.com/quaternions/ .. [4] Doran, C. (2015). Applications of Geometric Algebra. http://geometry.mrao.cam.ac.uk/wp-content/uploads/2015/02/01ApplicationsI.pdf .. [5] Dai, J. S. (2015). Euler–Rodrigues formula variations, quaternion conjugation and intrinsic connections, Mechanism and Machine Theory, 92, pp. 144-152, doi: 10.1016/j.mechmachtheory.2015.03.004. https://doi.org/10.1016/j.mechmachtheory.2015.03.004 .. [6] Terzakis, G., Lourakis, M., Ait-Boudaoud, D. (2017). Modified Rodrigues Parameters: An Efficient Representation of Orientation in 3D Vision and Graphics. J Math Imaging Vis, 60, pp. 422-442, doi: 10.1007/s10851-017-0765-x. .. [7] Hauser, K.: Robotic Systems (draft), http://motion.pratt.duke.edu/RoboticSystems/3DRotations.html .. [8] Shuster, M. D. (1993). A Survey of Attitude Representations. Journal of the Astronautical Sciences, 41, 439-517. http://malcolmdshuster.com/Pub_1993h_J_Repsurv_scan.pdf ================================================ FILE: doc/source/user_guide/transform_manager.rst ================================================ ===================================== Organizing Transformations in a Graph ===================================== It is sometimes very difficult to have an overview of all the transformations that are required to calculate another transformation. Suppose you have a robot with a camera that can observe the robot's end-effector and an object that we want to manipulate. We would like to know the position of the end-effector in the object's frame so that we can control it. We can organize transformations in a graph like the one shown in the figure below. Each frame represents a vertex (node) and each transformation an edge (connection). Edges in these graphs are directed, but we can create an edge in the opposite direction by inverting the transformation. If we want to compute a new edge indicated by the dashed red arrow, we have to use known transformations, that is, we compute the shortest path through the undirected graph, concatenate transformations along the path, and invert transformations if required as indicated by the solid red arrow. .. figure:: ../_static/transform_graph.png :width: 35% :align: center Example of a graph representing transformations that connect frames. The :class:`~pytransform3d.transform_manager.TransformManager` does this for you. For the given example, this would be the corresponding code (see also: :ref:`sphx_glr__auto_examples_plots_plot_transform_manager.py`). .. literalinclude:: ../../../examples/plots/plot_transform_manager.py :language: python :lines: 30-35 We can also export the underlying graph structure as a PNG with .. code-block:: python tm.write_png(filename) .. image:: ../_static/graph.png :width: 50% :align: center | A subclass of :class:`~pytransform3d.transform_manager.TransformManager` is :class:`~pytransform3d.urdf.UrdfTransformManager` which can load robot definitions from `URDF `_ files. The same class can be used to display collision objects or visuals from URDF files. The library `trimesh `_ will be used to load meshes. Here is a simple example with one visual that is used for two links: :ref:`sphx_glr__auto_examples_plots_plot_urdf_with_meshes.py`. Note that we can use Matplotlib or Open3D to visualize URDFs or any graph of transformations. In previous versions of this library, an example using pyrender was also included. This has been removed, as there is now another software package that does this: `urdf_viz `_. urdf_viz uses pytransform3d to load URDFs and displays them in pyrender. ================================================ FILE: doc/source/user_guide/transformation_ambiguities.rst ================================================ .. _transformation_ambiguities: ========================================== Transformation Ambiguities and Conventions ========================================== Heterogeneous software systems that consist of proprietary and open source software are often combined when we work with transformations. For example, suppose you want to transfer a trajectory demonstrated by a human to a robot. The human trajectory could be measured from an RGB-D camera, fused with IMU sensors that are attached to the human, and then translated to joint angles by inverse kinematics. That involves at least three different software systems that might all use different conventions for transformations. Sometimes even one software uses more than one convention. The following aspects are of crucial importance to glue and debug transformations in systems with heterogenous and often incompatible software: * Compatibility: Compatibility between heterogenous softwares is a difficult topic. It might involve, for example, communicating between proprietary and open source software or different languages. * Conventions: Lots of different conventions are used for transformations in three dimensions. These have to be determined or specified. * Conversions: We need conversions between these conventions to communicate transformations between different systems. * Visualization: Finally, transformations should be visually verified and that should be as easy as possible. pytransform3d assists in solving these issues. This documentation clearly states all of the used conventions, it aims at making conversions between rotation and transformation conventions as easy as possible, it is tightly coupled with Matplotlib to quickly visualize (or animate) transformations and it is written in Python with few dependencies. Python is a widely adopted language. It is used in many domains and supports a wide spectrum of communication to other software. There are lots of ambiguities in the world of transformations. We try to explain them all here. ---------------------------------------------- Right-handed vs. Left-handed Coordinate System ---------------------------------------------- We typically use a right-handed coordinate system, that is, the x-, y- and z-axis are aligned in a specific way. The name comes from the way how the fingers are attached to the human hand. Try to align your thumb with the imaginary x-axis, your index finger with the y-axis, and your middle finger with the z-axis. It is possible to do this with a right hand in a right-handed system and with the left hand in a left-handed system. .. raw:: html
Right-handed Left-handed
.. plot:: :width: 400px import numpy as np import matplotlib.pyplot as plt from pytransform3d.plot_utils import make_3d_axis plt.figure() ax = make_3d_axis(1) plt.setp(ax, xlim=(-0.05, 1.05), ylim=(-0.05, 1.05), zlim=(-0.05, 1.05), xlabel="X", ylabel="Y", zlabel="Z") basis = np.eye(3) for d, c in enumerate(["r", "g", "b"]): ax.plot([0.0, basis[0, d]], [0.0, basis[1, d]], [0.0, basis[2, d]], color=c, lw=5) plt.show() .. raw:: html .. plot:: :width: 400px import numpy as np import matplotlib.pyplot as plt from pytransform3d.plot_utils import make_3d_axis plt.figure() ax = make_3d_axis(1) plt.setp(ax, xlim=(-0.05, 1.05), ylim=(-0.05, 1.05), zlim=(-1.05, 0.05), xlabel="X", ylabel="Y", zlabel="Z") basis = np.eye(3) basis[:, 2] *= -1.0 for d, c in enumerate(["r", "g", "b"]): ax.plot([0.0, basis[0, d]], [0.0, basis[1, d]], [0.0, basis[2, d]], color=c, lw=5) plt.show() .. raw:: html
.. note:: The default in pytransform3d is a right-handed coordinate system. ------------------------------------------------- Active (Alibi) vs. Passive (Alias) Transformation ------------------------------------------------- There are two different views of transformations: the active and the passive view [1]_ [2]_. .. image:: ../_static/active_passive.png :alt: Passive vs. active transformation :align: center An active transformation * changes the physical position of an object * can be defined in the absence of a coordinate system or does not change the current coordinate system * is the only convention used by mathematicians Another name for active transformation is alibi transformation. A passive transformation * changes the coordinate system in which the object is described * does not change the object * could be used by physicists and engineers (e.g., roboticists) Another name for passive transformation is alias transformation. The following illustration compares the active view with the passive view. The position of the data is interpreted in the frame indicated by solid axes. We use exactly the same transformation matrix in both plots. In the active view, we see that the transformation is applied to the data. The data is physically moved. The dashed basis represents a frame that is moved from the base frame with the same transformation. The data is now interpreted in the old frame. In a passive transformation, we move the frame with the transformation. The data stays at its original position but it is interpreted in the new frame. .. raw:: html
Active Passive
.. plot:: :width: 400px import numpy as np import matplotlib.pyplot as plt from pytransform3d.transformations import transform, plot_transform from pytransform3d.plot_utils import make_3d_axis, Arrow3D plt.figure() ax = make_3d_axis(1) plt.setp(ax, xlim=(-1.05, 1.05), ylim=(-0.55, 1.55), zlim=(-1.05, 1.05), xlabel="X", ylabel="Y", zlabel="Z") ax.view_init(elev=90, azim=-90) ax.set_xticks(()) ax.set_yticks(()) ax.set_zticks(()) rng = np.random.default_rng(42) PA = np.ones((10, 4)) PA[:, :3] = 0.1 * rng.standard_normal(size=(10, 3)) PA[:, 0] += 0.3 PA[:, :3] += 0.3 x_translation = -0.1 y_translation = 0.2 z_rotation = np.pi / 4.0 A2B = np.array([ [np.cos(z_rotation), -np.sin(z_rotation), 0.0, x_translation], [np.sin(z_rotation), np.cos(z_rotation), 0.0, y_translation], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0] ]) PB = transform(A2B, PA) plot_transform(ax=ax, A2B=np.eye(4)) ax.scatter(PA[:, 0], PA[:, 1], PA[:, 2], c="orange") plot_transform(ax=ax, A2B=A2B, ls="--", alpha=0.5) ax.scatter(PB[:, 0], PB[:, 1], PB[:, 2], c="cyan") axis_arrow = Arrow3D( [0.7, 0.3], [0.4, 0.9], [0.2, 0.2], mutation_scale=20, lw=3, arrowstyle="-|>", color="k") ax.add_artist(axis_arrow) plt.tight_layout() plt.show() .. raw:: html .. plot:: :width: 400px import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import proj3d from pytransform3d.transformations import transform, plot_transform from pytransform3d.plot_utils import make_3d_axis, Arrow3D plt.figure() ax = make_3d_axis(1) plt.setp(ax, xlim=(-1.05, 1.05), ylim=(-0.55, 1.55), zlim=(-1.05, 1.05), xlabel="X", ylabel="Y", zlabel="Z") ax.view_init(elev=90, azim=-90) ax.set_xticks(()) ax.set_yticks(()) ax.set_zticks(()) rng = np.random.default_rng(42) PA = np.ones((10, 4)) PA[:, :3] = 0.1 * rng.standard_normal(size=(10, 3)) PA[:, 0] += 0.3 PA[:, :3] += 0.3 x_translation = -0.1 y_translation = 0.2 z_rotation = np.pi / 4.0 A2B = np.array([ [np.cos(z_rotation), -np.sin(z_rotation), 0.0, x_translation], [np.sin(z_rotation), np.cos(z_rotation), 0.0, y_translation], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0] ]) plot_transform(ax=ax, A2B=np.eye(4), ls="--", alpha=0.5) ax.scatter(PA[:, 0], PA[:, 1], PA[:, 2], c="orange") plot_transform(ax=ax, A2B=A2B) axis_arrow = Arrow3D( [0.0, -0.1], [0.0, 0.2], [0.2, 0.2], mutation_scale=20, lw=3, arrowstyle="-|>", color="k") ax.add_artist(axis_arrow) plt.tight_layout() plt.show() .. raw:: html
Using the inverse transformation in the active view gives us exactly the same solution as the original transformation in the passive view and vice versa. It is usually easy to determine whether the active or the passive convention is used by taking a look at the rotation matrix: when we rotate counter-clockwise by an angle :math:`\theta` about the z-axis, the following rotation matrix is usually used in an active transformation: .. math:: \left( \begin{array}{ccc} \cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0 \\ 0 & 0 & 1\\ \end{array} \right) Its transformed version is usually used for a passive transformation: .. math:: \left( \begin{array}{ccc} \cos \theta & \sin \theta & 0 \\ -\sin \theta & \cos \theta & 0 \\ 0 & 0 & 1\\ \end{array} \right) .. warning:: The standard in pytransform3d is an active rotation. ---------------------------------------- Pre-multiply vs. Post-multiply Rotations ---------------------------------------- The same point can be represented by a column vector :math:`\boldsymbol v` or a row vector :math:`\boldsymbol w`. A rotation matrix :math:`\boldsymbol R` can be used to rotate the point by pre-multiplying it to the column vector :math:`\boldsymbol R \boldsymbol v` or by post-multiplying it to the row vector :math:`\boldsymbol w \boldsymbol R`. However, for the same rotation matrix, both approaches are inverse: :math:`\boldsymbol R^T \boldsymbol v = \boldsymbol w \boldsymbol R`. Hence, to achieve the same effect we have to use two different rotation matrices depending on how we multiply them to points. .. note:: The default in pytransform3d are pre-multiplied rotation matrices. --------------------------------------- Intrinsic vs. Extrinsic Transformations --------------------------------------- A similar problem occurs when we want to concatenate rotations or transformations: suppose we have a rotation matrix :math:`R_1` and another matrix :math:`R_2` and we want to first rotate by :math:`R_1` and then by :math:`R_2`. If we want to apply both rotations in global coordinates (global, space-fixed / extrinsic rotation), we have to concatenate them with :math:`R_2 \cdot R_1`. We can also express the second rotation in terms of a local, body-fixed coordinates (local, body-fixed / intrinsic rotation) by :math:`R_1 \cdot R_2`, which means :math:`R_1` defines new coordinates in which :math:`R_2` is applied. Note that this applies to both passive and active rotation matrices. Specifying this convention is particularly relevant when we deal with Euler angles. Here is a comparison between various conventions of concatenation. .. plot:: ../../examples/plots/plot_convention_rotation_global_local.py .. warning:: There are two conventions on how to concatenate rotations and transformations: intrinsic and extrinsic transformation. There is no default in pytransform3d but usually the function name should tell you which convention the function uses. ---------------------------- Naming and Frame Conventions ---------------------------- In addition to transformation and rotation conventions, there are a lot of different naming and frame conventions. Here are some examples. .. figure:: ../_static/conventions_ship.png :alt: Ship conventions :align: center :width: 50% For ships we can use the following convention for the body frame: the x-axis is called surge, the y-axis sway, and the z-axis heave. The origin of the frame is the center of gravity. For the orientation, the names yaw, pitch, and roll are used. Sometimes the body frame is rotated by 180 degrees around the x-axis, so that the y-axis points to the right side and the z-axis down. .. figure:: ../_static/conventions_plane.png :alt: Aircraft conventions :align: center :width: 50% Aircrafts sometimes use the following naming conventions for intrinsic rotations around the z-, y'-, and x''-axis. The rotation about the z-axis is called heading, rotation about the y'-axis is called elevation, and the rotation about the x''-axis is called bank. .. figure:: ../_static/conventions_camera.png :alt: Camera conventions :align: center Cameras or similar sensors are sometimes mounted on pan/tilt units. Typically, first the pan joint rotates the camera about an axis parallel to the y-axis of the camera and the tilt joint rotates the camera about an axis parallel to the x-axis of the camera. When the axes of rotation and of the camera are not perfectly aligned, the camera will also be translated by the rotations. ----------------------------- Conventions of Other Software ----------------------------- The following is an incomplete list of conventions for representations of rotations, orientations, transformations, or poses and coordinate frames other software packages use. It illustrates the diversity that you will find when you combine different software systems. `Blender user interface (computer graphics) `_ * Active rotations * Euler angles (are actually Tait-Bryan angles): external rotations, angles in degree * Quaternion: scalar first `XSens MVNX format (motion capture) `_ * Active rotations * Conventions for coordinate frames * Axis orientation in the world (global): x north, y west, z up (NWU) * Axis orientation on body parts: axes are aligned with world axes when subject stands in T pose * Quaternion and rotation matrix rotate from sensor frame to world frame, that is, they represent the orientation of the sensor with respect to the world * Quaternion: scalar first * Euler angles: extrinsic roll-pitch-yaw (xyz) convention `Bullet (physics engine) `_ * Active rotations * Euler angles: extrinsic roll-pitch-yaw (xyz) convention (getQuaternionFromEuler from pybullet's API) * Quaternion: scalar last and Hamilton multiplication `Eigen (linear algebra library) `_ * Quaternion * Scalar first (constructor) and scalar last (internal) * Hamilton multiplication `Peter Corke's robotics toolbox `_ * Active rotations * Euler angles * Intrinsic zyz convention * Roll-pitch-yaw angles correspond either to intrinsic zyx convention (default) or intrinsic xyz convention, which can be selected by a parameter * Quaternion: scalar first and Hamilton multiplication `Robot Operating System (ROS) `_ `(REP103) `_ * Active transformations * Conventions for coordinate frames * Axis orientation on body: x forward, y left, z up * Axis orientation in the world: x east, y north, z up (ENU) * Axis orientation of optical camera frame (indicated by suffix in topic name): z forward, x right, y down * Euler angles * Active, extrinsic roll-pitch-yaw (xyz) convention (as used, e.g., in origin tag of URDF) can be used * In addition, the yaw-pitch-roll (zyx) convention can be used, but is discouraged * A `PoseStamped `_ is represented with respect to a `frame_id` * When interpreted as active transformation, `TransformStamped `_ represents a transformation from *child frame* to its (parent) *frame* * `Quaternion `_: scalar last `Universal Robot user interface `_ * Conventions for coordinate frames * Default axis orientation of tool center point: z forward (approach direction), x and y axes define the orientation with which we approach the target * Euler angles: extrinsic roll-pitch-yaw (xyz) convention ---------- References ---------- .. [1] Selig, J.M. (2006): Active Versus Passive Transformations in Robotics. IEEE Robotics and Automation Magazine, 13(1), pp. 79-84, doi: 10.1109/MRA.2006.1598057. https://ieeexplore.ieee.org/document/1598057 .. [2] Wikipedia: Active and passive transformation. https://en.wikipedia.org/wiki/Active_and_passive_transformation ================================================ FILE: doc/source/user_guide/transformation_modeling.rst ================================================ ======================== Modeling Transformations ======================== With many transformations it is not easy to keep track of the right sequence. Here are some simple tricks that you can use to keep track of transformations and concatenate them in the correct way. .. image:: ../_static/transformation_modeling.png :alt: Three frames :align: center | When modeling transformations in mathematical equations we often sequence them from right to left (extrinsic convention). Here it makes most sense to give names like :math:`\boldsymbol T_{BA}` for transformations that maps points **from frame** :math:`A` **to frame** :math:`B`, so we can easily recognize that .. math:: \boldsymbol T_{CA} = \boldsymbol T_{CB} \boldsymbol T_{BA} when we read the names of the frames from right to left. Now we can use the transformation matrix :math:`\boldsymbol T_{CA}` to transform a point :math:`_A\boldsymbol{p}` from frame :math:`A` to frame :math:`C` by multiplication :math:`_C\boldsymbol{p} = \boldsymbol{T}_{CA}\,_A\boldsymbol{p}`. This might look differently in code. Here we should prefer the notation `A2B` for a transformation from frame `A` to frame `B`. .. code-block:: from pytransform3d.transformations import concat A2B = ... # transformation from frame A to frame B B2C = ... # transformation from frame B to frame C A2C = concat(A2B, B2C) Now we can verify that the new transformation matrix correctly transforms from frame `A` to frame `C` if we read from left to right. The function `concat` will correctly apply the transformation `B2C` after `A2B`. If we want to transform a point from `C` to `A` we can now use .. code-block:: from pytransform3d.transformations import vector_to_point, transform p_in_A = vector_to_point(...) # point in frame A p_in_C = transform(A2C, p_in_A) This notation is particuarly effective when we use it in combination with longer, more readable frame names, e.g., ``left_hand_palm`` or ``robot_base``. For more complex cases the :class:`~pytransform3d.transform_manager.TransformManager` can help. ================================================ FILE: doc/source/user_guide/transformation_over_time.rst ================================================ .. _transformations_over_time: ======================================== Graphs of Time-Dependent Transformations ======================================== In applications, where the transformations between coordinate frames are dynamic (i.e., changing over time), consider using :class:`~pytransform3d.transform_manager.TemporalTransformManager`. In contrast to the :class:`~pytransform3d.transform_manager.TransformManager`, which deals with static transfomations, it provides an interface for the logic needed to interpolate between transformation samples available over time. We can visualize the lifetime of two dynamic transformations (i.e., 3 coordinate systems) in the figure below. Each circle represents a sample (measurement) holding the transformation from the parent to the child frame. .. figure:: ../_static/tf-trafo-over-time.png :width: 60% :align: center A common use case is to transform points originating from system A to system B at a specific point in time (i.e., :math:`t_q`, where :math:`q` refers to query). Imagine two moving robots A & B reporting their observations between each other. -------------------------------------- Preparing the Transformation Sequences -------------------------------------- First, you need to prepare the transfomation sequences using the :class:`~pytransform3d.transform_manager.NumpyTimeseriesTransform` class: .. literalinclude:: ../../../examples/plots/plot_interpolation_for_transform_manager.py :language: python :lines: 47-60 In this example, the screw linear interpolation (ScLERP) will be used (which operates on dual quaternions, refer to :func:`~pytransform3d.transformations.pq_from_dual_quaternion`). For more control, you may want to add your own implementation of the abstract class :class:`~pytransform3d.transform_manager.TimeVaryingTransform`. Next, you need to pass the transformations to an instance of :class:`~pytransform3d.transform_manager.TemporalTransformManager`: .. literalinclude:: ../../../examples/plots/plot_interpolation_for_transform_manager.py :language: python :lines: 62-65 ------------------------------------ Transform Between Coordinate Systems ------------------------------------ Finally, you can transform between coordinate systems at a particular time :math:`t_q`: .. literalinclude:: ../../../examples/plots/plot_interpolation_for_transform_manager.py :language: python :lines: 67-72 The coordinates of A's origin (blue diamond) transformed to B are visualized in the plot below: .. figure:: ../_auto_examples/plots/images/sphx_glr_plot_interpolation_for_transform_manager_001.png :target: ../_auto_examples/plots/plot_interpolation_for_transform_manager.html :align: center ================================================ FILE: doc/source/user_guide/transformations.rst ================================================ ========================= SE(3): 3D Transformations ========================= The group of all proper rigid transformations (rototranslations) in 3D Cartesian space is :math:`SE(3)` (SE: special Euclidean group). Transformations consist of a rotation and a translation. Those can be represented in different ways just like rotations can be expressed in different ways. The minimum number of components that are required to describe any transformation from :math:`SE(3)` is 6. .. figure:: ../_static/transformations.png :alt: Conversions between representations of transformations :width: 50% :align: center Overview of the representations and the conversions between them that are available in pytransform3d. For most representations of orientations we can find an analogous representation of transformations: * A **transformation matrix** :math:`\boldsymbol T` is similar to a rotation matrix :math:`\boldsymbol R`. * A **screw axis** :math:`\mathcal S` is similar to a rotation axis :math:`\hat{\boldsymbol{\omega}}`. * A **screw matrix** :math:`\left[\mathcal{S}\right]` is similar to a cross-product matrix of a unit rotation axis :math:`\left[\hat{\boldsymbol{\omega}}\right]`. * The **logarithm of a transformation** :math:`\left[\mathcal{S}\right] \theta` is similar to a cross-product matrix of the angle-axis representation :math:`\left[\hat{\boldsymbol{\omega}}\right] \theta`. * The **exponential coordinates** :math:`\mathcal{S} \theta` for rigid body motions are similar to exponential coordinates :math:`\hat{\boldsymbol{\omega}} \theta` for rotations (compact axis-angle representation / rotation vector). * A **twist** :math:`\mathcal V = \mathcal{S} \dot{\theta}` is similar to angular velocity :math:`\hat{\boldsymbol{\omega}} \dot{\theta}`. * A (unit) **dual quaternion** :math:`p_w + p_x i + p_y j + p_z k + \epsilon (q_w + q_x i + q_y j + q_z k)` is similar to a (unit) quaternion :math:`w + x i + y j + z k`. Not all representations support all operations directly without conversion to another representation. The following table is an overview. +----------------------------------------+------------+--------------------------+---------------+------------------+-----------------+ | Representation | Inverse | Transformation of vector | Concatenation | Interpolation | Renormalization | +========================================+============+==========================+===============+==================+=================+ | Transformation matrix | Inverse | Yes | Yes | ScLERP `(1)` | Required | | :math:`\pmb{R}` | | | | | | +----------------------------------------+------------+--------------------------+---------------+------------------+-----------------+ | Exponential coordinates | Negative | No | No | ScLERP `(2)` | Not necessary | | :math:`\mathcal{S}\theta` | | | | | | +----------------------------------------+------------+--------------------------+---------------+------------------+-----------------+ | Logarithm of transformation | Negative | No | No | ScLERP `(2)` | Not necessary | | :math:`\left[\mathcal{S}\theta\right]` | | | | | | +----------------------------------------+------------+--------------------------+---------------+------------------+-----------------+ | Position and quaternion | No | No | No | SLERP `(3)` | Required | | :math:`(\pmb{p}, \pmb{q})` | | | | | | +----------------------------------------+------------+--------------------------+---------------+------------------+-----------------+ | Dual quaternion | Quaternion | Yes | Yes | ScLERP `(1)` | Required | | :math:`\boldsymbol{\sigma}` | Conjugate | | | | | +----------------------------------------+------------+--------------------------+---------------+------------------+-----------------+ `(1)` ScLERP means Screw Linear intERPolation. This usually requires an internal conversion to exponential coordinates. `(2)` Fractions of this representation represent partial transformations, but a conversion to another representation is required to interpolate between poses. `(3)` SLERP means Spherical Linear intERPolation and is applied to the orientation while the translation is linearly interpolated. --------------------- Transformation Matrix --------------------- One of the most convenient ways to represent transformations are transformation matrices. A transformation matrix is a 4x4 matrix of the form .. math:: \boldsymbol T = \left( \begin{array}{cc} \boldsymbol R & \boldsymbol t\\ \boldsymbol 0 & 1\\ \end{array} \right) = \left( \begin{matrix} r_{11} & r_{12} & r_{13} & t_1\\ r_{21} & r_{22} & r_{23} & t_2\\ r_{31} & r_{32} & r_{33} & t_3\\ 0 & 0 & 0 & 1\\ \end{matrix} \right) \in SE(3). It is a partitioned matrix with a 3x3 rotation matrix :math:`\boldsymbol R` and a column vector :math:`\boldsymbol t` that represents the translation. It is also sometimes called the homogeneous representation of a transformation. All transformation matrices of this form generate the special Euclidean group :math:`SE(3)`, that is, .. math:: SE(3) = \{ \boldsymbol{T} = \left( \begin{array}{cc} \boldsymbol{R} & \boldsymbol{t}\\ \boldsymbol{0} & 1 \end{array} \right) \in \mathbb{R}^{4 \times 4} | \boldsymbol{R} \in SO(3), \boldsymbol{t} \in \mathbb{R}^3 \}. pytransform3d uses a numpy array of shape (4, 4) to represent transformation matrices and typically we use the variable name A2B for a transformation matrix, where A corrsponds to the frame from which it transforms and B to the frame to which it transforms. It is possible to transform position vectors or direction vectors with it. Position vectors are represented as a column vector :math:`\left( x,y,z,1 \right)^T`. This will activate the translation part of the transformation in a matrix multiplication (see :func:`~pytransform3d.transformations.vector_to_point`). When we transform a direction vector, we want to deactivate the translation by setting the last component to zero (see :func:`~pytransform3d.transformations.vector_to_direction`): :math:`\left( x,y,z,0 \right)^T`. We can use a transformation matrix :math:`\boldsymbol T_{BA}` to transform a point :math:`{_A}\boldsymbol{p}` from frame :math:`A` to frame :math:`B`: .. math:: \boldsymbol{T}_{BA} {_A}\boldsymbol{p} = \left( \begin{array}{c} \boldsymbol{R}_{BA} {_A}\boldsymbol{p} + {_B}\boldsymbol{t}_{BA}\\ 1\\ \end{array} \right) = {_B}\boldsymbol{p}. You can use :func:`~pytransform3d.transformations.transform` to apply a transformation matrix to a homogeneous vector. **Pros** * Supported operations: all except interpolation. * Interpretation: each column represents either a basis vector or the translation. * Singularities: none. **Cons** * Rrepresentation: 16 values for 6 degrees of freedom. * Renormalization: inherited from rotation matrix. ----------------------- Position and Quaternion ----------------------- An alternative to transformation matrices is the representation in a 7-dimensional vector that consists of the translation and a rotation quaternion: .. math:: \left( x, y, z, q_w, q_x, q_y, q_z \right)^T This representation is more compact than a transformation matrix and is particularly useful if you want to represent a sequence of poses in a 2D array. pytransform3d uses a numpy array of shape (7,) to represent position and quaternion and typically we use the variable name pq. **Pros** * Representation: compact. **Cons** * Supported operation: translation and rotation component are separated and have to be handled individually. ---------------- Screw Parameters ---------------- .. figure:: ../_auto_examples/plots/images/sphx_glr_plot_screw_001.png :target: ../_auto_examples/plots/plot_screw.html :width: 70% :align: center Just like any rotation can be expressed as a rotation by an angle about a 3D unit vector, any transformation (rotation and translation) can be expressed by a motion along a screw axis [2]_ [3]_ [4]_. The **screw parameters** that describe a screw axis include a point vector :math:`\boldsymbol{q}` through which the screw axis passes, a (unit) direction vector :math:`\hat{\boldsymbol{s}}` that indicates the direction of the axis, and the pitch :math:`h`. The pitch represents the ratio of translation and rotation. A screw motion translates along the screw axis and rotates about it. pytransform3d uses two vectors q and `s_axis` of shape (3,) and a scalar h to represent the parameters of a screw. .. image:: ../_static/screw_axis.png :alt: Screw axis :width: 50% :align: center ---------- Screw Axis ---------- A **screw axis** is typically represented by :math:`\mathcal{S} = \left[\begin{array}{c}\boldsymbol{\omega}\\\boldsymbol{v}\end{array}\right] \in \mathbb{R}^6`, where either 1. :math:`||\boldsymbol{\omega}|| = 1` or 2. :math:`||\boldsymbol{\omega}|| = 0` and :math:`||\boldsymbol{v}|| = 1` (only translation). pytransform3d uses a numpy array of shape (6,) to represent a screw axis and typically we use the variable name S or `screw_axis`. In case 1, we can compute the screw axis from screw parameters :math:`(\boldsymbol{q}, \hat{\boldsymbol{s}}, h)` as .. math:: \mathcal{S} = \left[ \begin{array}{c}\hat{\boldsymbol{s}} \\ \boldsymbol{q} \times \hat{\boldsymbol{s}} + h \hat{\boldsymbol{s}}\end{array} \right] In case 2, :math:`h` is infinite and we directly translate along :math:`\hat{\boldsymbol{s}}`. ----------------------- Exponential Coordinates ----------------------- By multiplication with an additional parameter :math:`\theta` we can then define a complete transformation through its exponential coordinates :math:`\mathcal{S} \theta = \left[\begin{array}{c}\boldsymbol{\omega}\theta\\\boldsymbol{v}\theta\end{array}\right] \in \mathbb{R}^6`. This is a minimal representation as it only needs 6 values. pytransform3d uses a numpy array of shape (6,) to represent a exponential coordinates of transformation and typically we use the variable name Stheta. .. warning:: Note that we use the screw theory definition of exponential coordinates and :math:`se(3)` (see next section) used by Lynch and Park (2017) [1]_, and Corke (2017) [2]_. They separate the parameter :math:`\theta` from the screw axis. Additionally, they use the first three components to encode rotation and the last three components to encode translation. There is an alternative definition used by Eade (2017) [3]_ and Sola et al. (2018) [4]_. They use a different order of the 3D vector components and they do not separate :math:`\theta` from the screw axis in their notation. We say exponential coordinates of transformation because there is an exponential map :math:`Exp(\mathcal{S}\theta) = \boldsymbol{T}` (see :func:`~pytransform3d.transformations.transform_from_exponential_coordinates`) that converts them to a transformation matrix. The inverse operation is the logarithmic map :math:`Log(\boldsymbol{T}) = \mathcal{S}\theta` (see :func:`~pytransform3d.transformations.exponential_coordinates_from_transform`). Exponential coordinates can be used to implement screw linear interpolation (ScLERP) because we can easily compute fractions of a transformation in exponential coordinates. We just have to compute the difference between two transformations :math:`\boldsymbol{T}_{AB} = \boldsymbol{T}^{-1}_{WA}\boldsymbol{T}_{WB}`, convert it to exponential coordinates :math:`Log(\boldsymbol{T}_{AB}) = \mathcal{S}\theta_{AB}`, compute a fraction of it :math:`t\mathcal{S}\theta_{AB}` with :math:`t \in [0, 1]`, and use the exponential map to concatenate the fraction of the difference :math:`\boldsymbol{T}_{WA} Exp(t\mathcal{S}\theta_{AB})`. **Pros** * Representation: minimal. * Supported operations: interpolation; can also represent spatial velocity and acceleration. **Cons** * Supported operations: concatenation and transformation of vectors requires conversion to another representation. --------------------------- Logarithm of Transformation --------------------------- Alternatively, we can represent a screw axis :math:`\mathcal S` in a matrix .. math:: \left[\mathcal S\right] = \left( \begin{array}{cc} \left[\boldsymbol{\omega}\right] & \boldsymbol v\\ \boldsymbol 0 & 0\\ \end{array} \right) = \left( \begin{matrix} 0 & -\omega_3 & \omega_2 & v_1\\ \omega_3 & 0 & -\omega_1 & v_2\\ -\omega_2 & \omega_1 & 0 & v_3\\ 0 & 0 & 0 & 0\\ \end{matrix} \right) \in se(3) \subset \mathbb{R}^{4 \times 4} that contains the cross-product matrix of its orientation part and its translation part. This is the **matrix representation of a screw axis** and we will also refer to it as **screw matrix** in the API. pytransform3d uses a numpy array of shape (4, 4) to represent a screw matrix and typically we use the variable name `screw_matrix`. By multiplication with :math:`\theta` we can again generate a full description of a transformation :math:`\left[\mathcal{S}\right] \theta \in se(3)`, which is the **matrix logarithm of a transformation matrix** and :math:`se(3)` is the Lie algebra of Lie group :math:`SE(3)`. pytransform3d uses a numpy array of shape (4, 4) to represent the logarithm of a transformation and typically we use the variable name `transform_log`. We say logarithm of transformation because there is an exponential map :math:`\exp(\left[\mathcal{S}\right]\theta) = \boldsymbol{T}` (see :func:`~pytransform3d.transformations.transform_from_transform_log`) that converts it to a transformation matrix. The inverse operation is the logarithmic map :math:`\log(\boldsymbol{T}) = \left[\mathcal{S}\right]\theta` (see :func:`~pytransform3d.transformations.transform_log_from_transform`). The logarithm of transformation can be used to implement screw linear interpolation (ScLERP) because we can easily compute fractions of a transformation. We just have to compute the difference between two transformations :math:`\boldsymbol{T}_{AB} = \boldsymbol{T}^{-1}_{WA}\boldsymbol{T}_{WB}`, convert it to the logarithm :math:`\log(\boldsymbol{T}_{AB}) = \left[\mathcal{S}\right]\theta_{AB}`, compute a fraction of it :math:`t\left[\mathcal{S}\right]\theta_{AB}` with :math:`t \in [0, 1]`, and use the exponential map to concatenate the fraction of the difference :math:`\boldsymbol{T}_{WA} \exp(t\left[\mathcal{S}\right]\theta_{AB})`. ----- Twist ----- We call spatial velocity (translation and rotation) **twist**. Similarly to the matrix logarithm, a twist :math:`\mathcal{V} = \mathcal{S} \dot{\theta}` is described by a screw axis :math:`\mathcal S` and a scalar :math:`\dot{\theta}` and :math:`\left[\mathcal{V}\right] = \left[\mathcal{S}\right] \dot{\theta} \in se(3)` is the matrix representation of a twist. ---------------- Dual Quaternions ---------------- Similarly to unit quaternions for rotations, unit dual quaternions are an alternative to represent transformations [5]_ [6]_ [7]_. They support similar operations as transformation matrices. A dual quaternion consists of a real quaternion and a dual quaternion: .. math:: \boldsymbol{\sigma} = \boldsymbol{p} + \epsilon \boldsymbol{q} = p_w + p_x i + p_y j + p_z k + \epsilon (q_w + q_x i + q_y j + q_z k), where :math:`\epsilon^2 = 0` and :math:`\epsilon \neq 0`. We use unit dual quaternions to represent transformations. In this case, the real quaternion is a unit quaternion and the dual quaternion is orthogonal to the real quaternion. The real quaternion is used to represent the rotation and the dual quaternion contains information about the rotation and translation. Dual quaternions support similar operations as transformation matrices: inversion through the conjugate of the two individual quaternions :func:`~pytransform3d.transformations.dq_q_conj`, concatenation through :func:`~pytransform3d.transformations.concatenate_dual_quaternions`, and transformation of a point by :func:`~pytransform3d.transformations.dq_prod_vector`. They can be renormalized efficiently (with :func:`~pytransform3d.transformations.check_dual_quaternion`), and interpolation between two dual quaternions is possible (with :func:`~pytransform3d.transformations.dual_quaternion_sclerp`). .. warning:: The unit dual quaternions :math:`\boldsymbol{\sigma} = \boldsymbol{p} + \epsilon \boldsymbol{q}` and :math:`-\boldsymbol{\sigma}` represent exactly the same transformation. The reason for this ambiguity is that the real quaternion :math:`\boldsymbol{p}` represents the orientation component, the dual quaternion encodes the translation component as :math:`\boldsymbol{q} = 0.5 \boldsymbol{t} \boldsymbol{p}`, where :math:`\boldsymbol{t}` is a quaternion with the translation in the vector component and the scalar 0, and rotation quaternions have the same ambiguity. **Pros** * Representation: compact. * Renormalization: cheap in comparison to transformation matrix. * Supported operations: all, including interpolation with ScLERP. * Computational efficiency: the dual quaternion product is slightly cheaper than the matrix product. * Singularities: none. **Cons** * Interpretation: not straightforward. * Ambiguities: double cover. ---------- References ---------- .. [1] Lynch, K. M., Park, F. C. (2017). Modern Robotics. http://hades.mech.northwestern.edu/index.php/Modern_Robotics .. [2] Corke, P. (2017). Robotics, Vision and Control, 2nd Edition, https://link.springer.com/book/10.1007/978-3-319-54413-7 .. [3] Eade, E. (2017). Lie Groups for 2D and 3D Transformations. https://ethaneade.com/lie.pdf .. [4] Sola, J., Deray, J., Atchuthan, D. (2018). A micro Lie theory for state estimation in robotics. Technical Report. http://www.iri.upc.edu/files/scidoc/2089-A-micro-Lie-theory-for-state-estimation-in-robotics.pdf .. [5] Wikipedia: Dual Quaternion. https://en.wikipedia.org/wiki/Dual_quaternion .. [6] Jia, Y.-B.: Dual Quaternions. https://faculty.sites.iastate.edu/jia/files/inline-files/dual-quaternion.pdf .. [7] Kenwright, B. (2012). A Beginners Guide to Dual-Quaternions: What They Are, How They Work, and How to Use Them for 3D Character Hierarchies. In 20th International Conference in Central Europe on Computer Graphics, Visualization and Computer Vision. http://wscg.zcu.cz/WSCG2012/!_WSCG2012-Communications-1.pdf ================================================ FILE: doc/source/user_guide/uncertainty.rst ================================================ ============================== Uncertainty in Transformations ============================== In computer vision and robotics we are never absolutely certain about transformations. It is often a good idea to express the uncertainty explicitly and use it. The module :mod:`pytransform3d.uncertainty` offers tools to handle with uncertain transformations. ---------------------------------------- Gaussian Distribution of Transformations ---------------------------------------- Uncertain transformations are often represented by Gaussian distributions, where the mean of the distribution is represented by a transformation matrix :math:`\boldsymbol{T} \in SE(3)` and the covariance is defined in the tangent space through exponential coordinates :math:`\boldsymbol{\Sigma} \in \mathbb{R}^{6 \times 6}`. .. warning:: It makes a difference whether the uncertainty is defined in the global frame of reference (i.e., left-multiplied) or local frame of reference (i.e., right-multiplied). Unless otherwise stated, we define uncertainty in a global frame, that is, to sample from a Gaussian distribution of transformations :math:`\boldsymbol{T}_{xA} \sim \mathcal{N}(\boldsymbol{T}|\boldsymbol{T}_{BA}, \boldsymbol{\Sigma}_{6 \times 6}))`, we compute :math:`\Delta \boldsymbol{T}_{xB} \boldsymbol{T}_{BA}`, with :math:`\Delta \boldsymbol{T}_{xB} = Exp(\boldsymbol{\xi})` and :math:`\boldsymbol{\xi} \sim \mathcal{N}(\boldsymbol{0}_6, \boldsymbol{\Sigma}_{6 \times 6})`. Hence, the uncertainty is defined in the global frame B, not in the local body frame A. We can use :func:`~pytransform3d.uncertainty.estimate_gaussian_transform_from_samples` to estimate a Gaussian distribution of transformations. We can sample from a known Gaussian distribution of transformations with :func:`~pytransform3d.transformations.random_transform` as illustrated in the Example :ref:`sphx_glr__auto_examples_plots_plot_sample_transforms.py`. .. figure:: ../_auto_examples/plots/images/sphx_glr_plot_sample_transforms_001.png :target: ../_auto_examples/plots/plot_sample_transforms.html :align: center ---------------------------- Visualization of Uncertainty ---------------------------- A typical visual representation of Gaussian distributions in 3D coordinates are equiprobable ellipsoids (obtained with :func:`~pytransform3d.uncertainty.to_ellipsoid`). This is equivalent to showing the :math:`k\sigma, k \in \mathbb{R}` intervals of a 1D Gaussian distribution. However, for transformations there are also interactions between rotation and translation components so that an ellipsoid is not an appropriate representation to visualize the distribution of transformations in 3D. We have to project a 6D hyper-ellipsoid to 3D (for which we can use :func:`~pytransform3d.uncertainty.to_projected_ellipsoid`), which can result in banana-like shapes. ------------------------------------------ Concatenation of Uncertain Transformations ------------------------------------------ There are two different ways of defining uncertainty of transformations when we concatenate multiple transformations. We can define uncertainty in the global frame of reference or in the local frame of reference and it makes a difference. Global frame of reference (:func:`~pytransform3d.uncertainty.concat_globally_uncertain_transforms`): .. math:: Exp(_C\boldsymbol{\xi'}) \overline{\boldsymbol{T}}_{CA} = Exp(_C\boldsymbol{\xi}) \overline{\boldsymbol{T}}_{CB} Exp(_B\boldsymbol{\xi}) \overline{\boldsymbol{T}}_{BA}, where :math:`_B\boldsymbol{\xi} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_{BA})`, :math:`_C\boldsymbol{\xi} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_{CB})`, and :math:`_C\boldsymbol{\xi'} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_{CA})`. This method of concatenating uncertain transformation is used in Example :ref:`sphx_glr__auto_examples_plots_plot_concatenate_uncertain_transforms.py`, which illustrates how the banana distribution is generated. .. figure:: ../_auto_examples/plots/images/sphx_glr_plot_concatenate_uncertain_transforms_001.png :target: ../_auto_examples/plots/plot_concatenate_uncertain_transforms.html :align: center Local frame of reference (:func:`~pytransform3d.uncertainty.concat_locally_uncertain_transforms`): .. math:: \overline{\boldsymbol{T}}_{CA} Exp(_A\boldsymbol{\xi'}) = \overline{\boldsymbol{T}}_{CB} Exp(_B\boldsymbol{\xi}) \overline{\boldsymbol{T}}_{BA} Exp(_A\boldsymbol{\xi}), where :math:`_B\boldsymbol{\xi} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_B)`, :math:`_A\boldsymbol{\xi} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_A)`, and :math:`_A\boldsymbol{\xi'} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_{A,total})`. This method of concatenating uncertain transformations is used in Example :ref:`sphx_glr__auto_examples_visualizations_vis_probabilistic_robot_kinematics.py`, which illustrates probabilistic robot kinematics. .. figure:: ../_auto_examples/visualizations/images/sphx_glr_vis_probabilistic_robot_kinematics_001.png :target: ../_auto_examples/visualizations/vis_probabilistic_robot_kinematics.html :align: center ------------------------- Fusion of Uncertain Poses ------------------------- Fusing of multiple uncertain poses with :func:`~pytransform3d.uncertainty.pose_fusion` is required, for instance, in state estimation and sensor fusion. The Example :ref:`sphx_glr__auto_examples_plots_plot_pose_fusion.py` illustrates this process. .. figure:: ../_auto_examples/plots/images/sphx_glr_plot_pose_fusion_001.png :target: ../_auto_examples/plots/plot_pose_fusion.html :width: 50% :align: center ================================================ FILE: examples/README.rst ================================================ ======== Examples ======== The following examples show how pytransform3d can be used. ================================================ FILE: examples/animations/README.rst ================================================ Matplotlib Animations --------------------- ================================================ FILE: examples/animations/animate_angle_axis_interpolation.py ================================================ """ ============================================== Interpolate Between Axis-Angle Representations ============================================== We can interpolate between two orientations that are represented by an axis and an angle either linearly or with slerp (spherical linear interpolation). Here we compare both methods and measure the angular velocity between two successive steps. We can see that linear interpolation results in a non-constant angular velocity. Usually it is a better idea to interpolate with slerp. """ import matplotlib.animation as animation import matplotlib.pyplot as plt import numpy as np from mpl_toolkits.mplot3d import axes3d # noqa: F401 from pytransform3d import rotations as pr velocity = None last_a = None rotation_axis = None def interpolate_linear(start, end, t): return (1 - t) * start + t * end def update_lines(step, start, end, n_frames, rot, profile): global velocity global last_a if step == 0: velocity = [] last_a = start if step <= n_frames / 2: t = step / float(n_frames / 2 - 1) a = pr.axis_angle_slerp(start, end, t) else: t = (step - n_frames / 2) / float(n_frames / 2 - 1) a = interpolate_linear(end, start, t) R = pr.matrix_from_axis_angle(a) # Draw new frame rot[0].set_data(np.array([0, R[0, 0]]), [0, R[1, 0]]) rot[0].set_3d_properties([0, R[2, 0]]) rot[1].set_data(np.array([0, R[0, 1]]), [0, R[1, 1]]) rot[1].set_3d_properties([0, R[2, 1]]) rot[2].set_data(np.array([0, R[0, 2]]), [0, R[1, 2]]) rot[2].set_3d_properties([0, R[2, 2]]) # Update vector in frame test = R.dot(np.ones(3) / np.sqrt(3.0)) rot[3].set_data( np.array([test[0] / 2.0, test[0]]), [test[1] / 2.0, test[1]] ) rot[3].set_3d_properties([test[2] / 2.0, test[2]]) velocity.append( pr.angle_between_vectors(a[:3], last_a[:3]) + a[3] - last_a[3] ) last_a = a profile.set_data(np.linspace(0, 1, n_frames)[: len(velocity)], velocity) return rot if __name__ == "__main__": # Generate random start and goal np.random.seed(3) start = np.array([0, 0, 0, np.pi]) start[:3] = pr.norm_vector(np.random.randn(3)) end = np.array([0, 0, 0, np.pi]) end[:3] = pr.norm_vector(np.random.randn(3)) n_frames = 100 fig = plt.figure(figsize=(12, 5)) ax = fig.add_subplot(121, projection="3d") ax.set_xlim((-1, 1)) ax.set_ylim((-1, 1)) ax.set_zlim((-1, 1)) ax.set_xlabel("X") ax.set_ylabel("Y") ax.set_zlabel("Z") Rs = pr.matrix_from_axis_angle(start) Re = pr.matrix_from_axis_angle(end) rot = [ ax.plot([0, 1], [0, 0], [0, 0], c="r", lw=3)[0], ax.plot([0, 0], [0, 1], [0, 0], c="g", lw=3)[0], ax.plot([0, 0], [0, 0], [0, 1], c="b", lw=3)[0], ax.plot([0, 1], [0, 1], [0, 1], c="gray", lw=3)[0], ax.plot( [0, Rs[0, 0]], [0, Rs[1, 0]], [0, Rs[2, 0]], c="r", lw=3, alpha=0.5 )[0], ax.plot( [0, Rs[0, 1]], [0, Rs[1, 1]], [0, Rs[2, 1]], c="g", lw=3, alpha=0.5 )[0], ax.plot( [0, Rs[0, 2]], [0, Rs[1, 2]], [0, Rs[2, 2]], c="b", lw=3, alpha=0.5 )[0], ax.plot( [0, Re[0, 0]], [0, Re[1, 0]], [0, Re[2, 0]], c="orange", lw=3, alpha=0.5, )[0], ax.plot( [0, Re[0, 1]], [0, Re[1, 1]], [0, Re[2, 1]], c="turquoise", lw=3, alpha=0.5, )[0], ax.plot( [0, Re[0, 2]], [0, Re[1, 2]], [0, Re[2, 2]], c="violet", lw=3, alpha=0.5, )[0], ] ax = fig.add_subplot(122) ax.set_xlim((0, 1)) ax.set_ylim((0, 1)) profile = ax.plot(0, 0)[0] anim = animation.FuncAnimation( fig, update_lines, n_frames, fargs=(start, end, n_frames, rot, profile), interval=50, blit=False, ) plt.show() ================================================ FILE: examples/animations/animate_camera.py ================================================ """ ============== Animate Camera ============== Animate a camera moving along a circular trajectory while looking at a target. """ import matplotlib.animation as animation import matplotlib.pyplot as plt import numpy as np from pytransform3d.plot_utils import Frame, Camera, make_3d_axis from pytransform3d.rotations import matrix_from_euler from pytransform3d.transformations import transform_from def update_camera(step, n_frames, camera): phi = 2 * np.pi * step / n_frames tf = transform_from( matrix_from_euler([-0.5 * np.pi, phi, 0], 0, 1, 2, False), -10 * np.array([np.sin(phi), np.cos(phi), 0]), ) camera.set_data(tf) return camera if __name__ == "__main__": n_frames = 50 fig = plt.figure(figsize=(5, 5)) ax = make_3d_axis(15) frame = Frame(np.eye(4), label="target", s=3, draw_label_indicator=False) frame.add_frame(ax) fl = 3000 # [pixels] w, h = 1920, 1080 # [pixels] M = np.array(((fl, 0, w // 2), (0, fl, h // 2), (0, 0, 1))) camera = Camera( M, np.eye(4), virtual_image_distance=5, sensor_size=(w, h), c="c", ) camera.add_camera(ax) anim = animation.FuncAnimation( fig, update_camera, n_frames, fargs=(n_frames, camera), interval=50, blit=False, ) plt.show() ================================================ FILE: examples/animations/animate_quaternion_integration.py ================================================ """ ====================== Quaternion Integration ====================== Integrate angular accelerations to a quaternion sequence and animate it. """ import matplotlib.animation as animation import matplotlib.pyplot as plt import numpy as np from mpl_toolkits.mplot3d import axes3d # noqa: F401 from pytransform3d import rotations as pr def update_lines(step, Q, rot): R = pr.matrix_from_quaternion(Q[step]) # Draw new frame rot[0].set_data(np.array([0, R[0, 0]]), [0, R[1, 0]]) rot[0].set_3d_properties([0, R[2, 0]]) rot[1].set_data(np.array([0, R[0, 1]]), [0, R[1, 1]]) rot[1].set_3d_properties([0, R[2, 1]]) rot[2].set_data(np.array([0, R[0, 2]]), [0, R[1, 2]]) rot[2].set_3d_properties([0, R[2, 2]]) return rot if __name__ == "__main__": rng = np.random.default_rng(3) start = pr.random_quaternion(rng) n_frames = 1000 dt = 0.01 angular_accelerations = np.empty((n_frames, 3)) for i in range(n_frames): angular_accelerations[i] = pr.random_compact_axis_angle(rng) # Integrate angular accelerations to velocities angular_velocities = np.vstack( (np.zeros((1, 3)), np.cumsum(angular_accelerations * dt, axis=0)) ) # Integrate angular velocities to quaternions Q = pr.quaternion_integrate(angular_velocities, q0=start, dt=dt) fig = plt.figure(figsize=(4, 3)) ax = fig.add_subplot(111, projection="3d") ax.set_xlim((-1, 1)) ax.set_ylim((-1, 1)) ax.set_zlim((-1, 1)) ax.set_xlabel("X") ax.set_ylabel("Y") ax.set_zlabel("Z") R = pr.matrix_from_quaternion(start) rot = [ ax.plot([0, 1], [0, 0], [0, 0], c="r", lw=3)[0], ax.plot([0, 0], [0, 1], [0, 0], c="g", lw=3)[0], ax.plot([0, 0], [0, 0], [0, 1], c="b", lw=3)[0], ax.plot( [0, R[0, 0]], [0, R[1, 0]], [0, R[2, 0]], c="r", lw=3, alpha=0.3 )[0], ax.plot( [0, R[0, 1]], [0, R[1, 1]], [0, R[2, 1]], c="g", lw=3, alpha=0.3 )[0], ax.plot( [0, R[0, 2]], [0, R[1, 2]], [0, R[2, 2]], c="b", lw=3, alpha=0.3 )[0], ] anim = animation.FuncAnimation( fig, update_lines, n_frames, fargs=(Q, rot), interval=10, blit=False ) plt.show() ================================================ FILE: examples/animations/animate_quaternion_interpolation.py ================================================ """ =========================================== Interpolate Between Quaternion Orientations =========================================== We can interpolate between two orientations that are represented by quaternions either linearly or with slerp (spherical linear interpolation). Here we compare both methods and measure the angular velocity between two successive steps. We can see that linear interpolation results in a non-constant angular velocity. Usually it is a better idea to interpolate with slerp. """ import matplotlib.animation as animation import matplotlib.pyplot as plt import numpy as np from mpl_toolkits.mplot3d import axes3d # noqa: F401 from pytransform3d import rotations as pr velocity = None last_R = None def interpolate_linear(start, end, t): return (1 - t) * start + t * end def update_lines(step, start, end, n_frames, rot, profile): global velocity global last_R if step == 0: velocity = [] last_R = pr.matrix_from_quaternion(start) if step <= n_frames / 2: t = step / float(n_frames / 2 - 1) q = pr.quaternion_slerp(start, end, t) else: t = (step - n_frames / 2) / float(n_frames / 2 - 1) q = interpolate_linear(end, start, t) R = pr.matrix_from_quaternion(q) # Draw new frame rot[0].set_data(np.array([0, R[0, 0]]), [0, R[1, 0]]) rot[0].set_3d_properties([0, R[2, 0]]) rot[1].set_data(np.array([0, R[0, 1]]), [0, R[1, 1]]) rot[1].set_3d_properties([0, R[2, 1]]) rot[2].set_data(np.array([0, R[0, 2]]), [0, R[1, 2]]) rot[2].set_3d_properties([0, R[2, 2]]) # Update vector in frame test = R.dot(np.ones(3) / np.sqrt(3.0)) rot[3].set_data( np.array([test[0] / 2.0, test[0]]), [test[1] / 2.0, test[1]] ) rot[3].set_3d_properties([test[2] / 2.0, test[2]]) velocity.append(np.linalg.norm(R - last_R)) last_R = R profile.set_data(np.linspace(0, 1, n_frames)[: len(velocity)], velocity) return rot if __name__ == "__main__": # Generate random start and goal np.random.seed(3) start = np.array([0, 0, 0, np.pi]) start[:3] = np.random.randn(3) start = pr.quaternion_from_axis_angle(start) end = np.array([0, 0, 0, np.pi]) end[:3] = np.random.randn(3) end = pr.quaternion_from_axis_angle(end) n_frames = 200 fig = plt.figure(figsize=(12, 5)) ax = fig.add_subplot(121, projection="3d") ax.set_xlim((-1, 1)) ax.set_ylim((-1, 1)) ax.set_zlim((-1, 1)) ax.set_xlabel("X") ax.set_ylabel("Y") ax.set_zlabel("Z") Rs = pr.matrix_from_quaternion(start) Re = pr.matrix_from_quaternion(end) rot = [ ax.plot([0, 1], [0, 0], [0, 0], c="r", lw=3)[0], ax.plot([0, 0], [0, 1], [0, 0], c="g", lw=3)[0], ax.plot([0, 0], [0, 0], [0, 1], c="b", lw=3)[0], ax.plot([0, 1], [0, 1], [0, 1], c="gray", lw=3)[0], ax.plot( [0, Rs[0, 0]], [0, Rs[1, 0]], [0, Rs[2, 0]], c="r", lw=3, alpha=0.5 )[0], ax.plot( [0, Rs[0, 1]], [0, Rs[1, 1]], [0, Rs[2, 1]], c="g", lw=3, alpha=0.5 )[0], ax.plot( [0, Rs[0, 2]], [0, Rs[1, 2]], [0, Rs[2, 2]], c="b", lw=3, alpha=0.5 )[0], ax.plot( [0, Re[0, 0]], [0, Re[1, 0]], [0, Re[2, 0]], c="orange", lw=3, alpha=0.5, )[0], ax.plot( [0, Re[0, 1]], [0, Re[1, 1]], [0, Re[2, 1]], c="turquoise", lw=3, alpha=0.5, )[0], ax.plot( [0, Re[0, 2]], [0, Re[1, 2]], [0, Re[2, 2]], c="violet", lw=3, alpha=0.5, )[0], ] ax = fig.add_subplot(122) ax.set_xlim((0, 1)) ax.set_ylim((0, 1)) profile = ax.plot(0, 0)[0] anim = animation.FuncAnimation( fig, update_lines, n_frames, fargs=(start, end, n_frames, rot, profile), interval=50, blit=False, ) plt.show() ================================================ FILE: examples/animations/animate_rotation.py ================================================ """ ================ Animate Rotation ================ Simple animations of frames and trajectories are easy to implement with Matplotlib's FuncAnimation. The following example will generate a frame that will be rotated about the x-axis. """ import matplotlib.animation as animation import matplotlib.pyplot as plt import numpy as np from mpl_toolkits.mplot3d import axes3d # noqa: F401 from pytransform3d import rotations as pr from pytransform3d.plot_utils import Frame def update_frame(step, n_frames, frame): angle = 2.0 * np.pi * (step + 1) / n_frames R = pr.passive_matrix_from_angle(0, angle) A2B = np.eye(4) A2B[:3, :3] = R frame.set_data(A2B) return frame if __name__ == "__main__": n_frames = 50 fig = plt.figure(figsize=(5, 5)) ax = fig.add_subplot(111, projection="3d") ax.set_xlim((-1, 1)) ax.set_ylim((-1, 1)) ax.set_zlim((-1, 1)) ax.set_xlabel("X") ax.set_ylabel("Y") ax.set_zlabel("Z") frame = Frame(np.eye(4), label="rotating frame", s=0.5) frame.add_frame(ax) anim = animation.FuncAnimation( fig, update_frame, n_frames, fargs=(n_frames, frame), interval=50, blit=False, ) plt.show() ================================================ FILE: examples/animations/animate_trajectory.py ================================================ """ ================== Animate Trajectory ================== Animates a trajectory. """ import matplotlib.animation as animation import matplotlib.pyplot as plt import numpy as np from mpl_toolkits.mplot3d import axes3d # noqa: F401 from pytransform3d.plot_utils import Trajectory from pytransform3d.rotations import passive_matrix_from_angle, R_id from pytransform3d.transformations import transform_from, concat def update_trajectory(step, n_frames, trajectory): progress = float(step + 1) / float(n_frames) H = np.zeros((100, 4, 4)) H0 = transform_from(R_id, np.zeros(3)) H_mod = np.eye(4) for i, t in enumerate(np.linspace(0, progress, len(H))): H0[:3, 3] = np.array([t, 0, t]) H_mod[:3, :3] = passive_matrix_from_angle(2, 8 * np.pi * t) H[i] = concat(H0, H_mod) trajectory.set_data(H) return trajectory if __name__ == "__main__": n_frames = 200 fig = plt.figure(figsize=(5, 5)) ax = fig.add_subplot(111, projection="3d") ax.set_xlim((-1, 1)) ax.set_ylim((-1, 1)) ax.set_zlim((-1, 1)) ax.set_xlabel("X") ax.set_ylabel("Y") ax.set_zlabel("Z") H = np.zeros((100, 4, 4)) H[:] = np.eye(4) trajectory = Trajectory(H, show_direction=False, s=0.2, c="k") trajectory.add_trajectory(ax) anim = animation.FuncAnimation( fig, update_trajectory, n_frames, fargs=(n_frames, trajectory), interval=50, blit=False, ) plt.show() ================================================ FILE: examples/apps/README.rst ================================================ GUI Applications ---------------- ================================================ FILE: examples/apps/app_transformation_editor.py ================================================ """ ===================== Transformation Editor ===================== The transformation editor can be used to manipulate transformations. """ from pytransform3d.editor import TransformEditor from pytransform3d.rotations import active_matrix_from_extrinsic_euler_xyx from pytransform3d.transform_manager import TransformManager from pytransform3d.transformations import transform_from tm = TransformManager() tm.add_transform( "tree", "world", transform_from( active_matrix_from_extrinsic_euler_xyx([0, 0.5, 0]), [0, 0, 0.5] ), ) tm.add_transform( "car", "world", transform_from( active_matrix_from_extrinsic_euler_xyx([0.5, 0, 0]), [0.5, 0, 0] ), ) te = TransformEditor(tm, "world", s=0.3) te.show() print("tree to world:") print(te.transform_manager.get_transform("tree", "world")) ================================================ FILE: examples/plots/README.rst ================================================ Matplotlib Figures ------------------ ================================================ FILE: examples/plots/plot_axis_angle.py ================================================ """ ===================================== Axis-Angle Representation of Rotation ===================================== Any rotation can be represented with a single rotation about some axis. Here we see a frame that is rotated in multiple steps around a rotation axis. """ import matplotlib.pyplot as plt import numpy as np from pytransform3d.rotations import ( random_axis_angle, matrix_from_axis_angle, plot_basis, plot_axis_angle, ) original = random_axis_angle(np.random.RandomState(5)) ax = plot_axis_angle(a=original) for fraction in np.linspace(0, 1, 50): a = original.copy() a[-1] = fraction * original[-1] R = matrix_from_axis_angle(a) plot_basis(ax, R, alpha=0.2) plt.subplots_adjust(left=0, right=1, bottom=0, top=1.1) ax.view_init(azim=105, elev=12) plt.show() ================================================ FILE: examples/plots/plot_axis_angle_from_two_vectors.py ================================================ """ ==================================================== Axis-Angle Representation from Two Direction Vectors ==================================================== This example shows how we can compute the axis-angle representation of a rotation that transforms a direction given by a vector 'a' to a direction given by a vector 'b'. We show both vectors, the rotation about the rotation axis and the initial and resulting coordinate frame, where the vector 'b' and its corresponding frame after the rotation are represented by shorter lines. """ import matplotlib.pyplot as plt import numpy as np from pytransform3d.plot_utils import make_3d_axis, plot_vector from pytransform3d.rotations import ( axis_angle_from_two_directions, matrix_from_axis_angle, plot_axis_angle, plot_basis, ) a = np.array([1.0, 0.0, 0.0]) b = np.array([0.76958075, -0.49039301, -0.40897453]) aa = axis_angle_from_two_directions(a, b) ax = make_3d_axis(ax_s=1) plot_vector(ax, start=np.zeros(3), direction=a, s=1.0) plot_vector(ax, start=np.zeros(3), direction=b, s=0.5) plot_axis_angle(ax, aa) plot_basis(ax) plot_basis(ax, R=matrix_from_axis_angle(aa), s=0.5) plt.show() ================================================ FILE: examples/plots/plot_bivector.py ================================================ """ ============= Plot Bivector ============= Visualizes a bivector constructed from the wedge product of two vectors. The two vectors will be displayed in grey together with the parallelogram that they form. Each component of the bivector corresponds to the area of the parallelogram projected on the basis planes. These parallelograms will be shown as well. Furthermore, one black arrow will show the rotation direction of the bivector and one black arrow will represent the normal of the plane that can be extracted by rearranging the elements of the bivector and normalizing the vector. """ import matplotlib.pyplot as plt import numpy as np import pytransform3d.rotations as pr a = np.array([0.6, 0.3, 0.9]) b = np.array([0.4, 0.8, 0.2]) ax = pr.plot_basis() pr.plot_bivector(ax=ax, a=a, b=b) ax.view_init(azim=75, elev=40) ax.set_xlim((-0.5, 1)) ax.set_ylim((-0.5, 1)) ax.set_zlim((-0.5, 1)) plt.tight_layout() plt.show() ================================================ FILE: examples/plots/plot_box.py ================================================ """ ======== Plot Box ======== """ import matplotlib.pyplot as plt import numpy as np from pytransform3d.plot_utils import ( plot_box, plot_length_variable, remove_frame, ) from pytransform3d.transformations import plot_transform plt.figure() ax = plot_box(size=[1, 1, 1], wireframe=False, alpha=0.1, color="k", ax_s=0.6) plot_transform(ax=ax) plot_box(ax=ax, size=[1, 1, 1], wireframe=True, alpha=0.3, linewidth=2) plot_length_variable( ax=ax, start=np.array([-0.5, -0.5, 0.55]), end=np.array([0.5, -0.5, 0.55]), name="a", fontsize=14, fontfamily="serif", ) plot_length_variable( ax=ax, start=np.array([0.55, -0.5, 0.5]), end=np.array([0.55, 0.5, 0.5]), name="b", fontsize=14, fontfamily="serif", ) plot_length_variable( ax=ax, start=np.array([-0.55, -0.5, -0.5]), end=np.array([-0.55, -0.5, 0.5]), name="c", fontsize=14, fontfamily="serif", ) remove_frame(ax) plt.show() ================================================ FILE: examples/plots/plot_camera_3d.py ================================================ """ =========================== Camera Representation in 3D =========================== This visualization is inspired by Blender's camera visualization. It will show the camera center, a virtual image plane at a desired distance to the camera center, and the top direction of the virtual image plane. """ import matplotlib.pyplot as plt import numpy as np import pytransform3d.camera as pc import pytransform3d.transformations as pt cam2world = pt.transform_from_pq([0, 0, 0, np.sqrt(0.5), -np.sqrt(0.5), 0, 0]) # default parameters of a camera in Blender sensor_size = np.array([0.036, 0.024]) intrinsic_matrix = np.array( [ [0.05, 0, sensor_size[0] / 2.0], [0, 0.05, sensor_size[1] / 2.0], [0, 0, 1], ] ) virtual_image_distance = 1 ax = pt.plot_transform(A2B=cam2world, s=0.2) pc.plot_camera( ax, cam2world=cam2world, M=intrinsic_matrix, sensor_size=sensor_size, virtual_image_distance=virtual_image_distance, ) plt.show() ================================================ FILE: examples/plots/plot_camera_projection.py ================================================ """ ================= Camera Projection ================= We can see the camera coordinate frame and a grid of points in the camera coordinate system which will be projected to the sensor. From the coordinates on the sensor we can compute the corresponding pixels. """ import matplotlib.pyplot as plt import numpy as np from pytransform3d.camera import make_world_grid, cam2sensor, sensor2img from pytransform3d.transformations import plot_transform # %% # Camera Details # -------------- # We have to define the focal length and sensor size of the camera in meters as # well as the image size in pixels. The sensor size is extraordinarily large so # that we can see it. focal_length = 0.2 sensor_size = (0.2, 0.15) image_size = (640, 480) # %% # Grid # ---- # We define a grid in 3D world coordinates and compute its projection to the # sensor in 3D. cam_grid = make_world_grid(n_points_per_line=11) - np.array([0, 0, -2, 0]) img_grid_3d = cam_grid * focal_length # %% # Projection # ---------- # First, we project the grid from its original 3D coordinates to its projection # on the sensor, then we convert it to image coordinates. sensor_grid = cam2sensor(cam_grid, focal_length) img_grid = sensor2img(sensor_grid, sensor_size, image_size) # %% # Plotting # -------- # Now we can plot the grid in 3D, projected to the 3D sensor, and projected to # the image. plt.figure(figsize=(12, 5)) ax = plt.subplot(121, projection="3d") ax.set_title("Grid in 3D camera coordinate system") ax.set_xlim((-1, 1)) ax.set_ylim((-1, 1)) ax.set_zlim((0, 2)) ax.set_xlabel("X") ax.set_ylabel("Y") ax.set_zlabel("Z") c = np.arange(len(cam_grid)) ax.scatter(cam_grid[:, 0], cam_grid[:, 1], cam_grid[:, 2], c=c) ax.scatter(img_grid_3d[:, 0], img_grid_3d[:, 1], img_grid_3d[:, 2], c=c) plot_transform(ax) ax = plt.subplot(122, aspect="equal") ax.set_title("Grid in 2D image coordinate system") ax.scatter(img_grid[:, 0], img_grid[:, 1], c=c) ax.set_xlim((0, image_size[0])) ax.set_ylim((0, image_size[1])) plt.show() ================================================ FILE: examples/plots/plot_camera_trajectory.py ================================================ """ ================= Camera Trajectory ================= The following illustration shows a camera's trajectory that has has been estimated from odometry. This specific trajectory has been used to reconstruct a colored mesh from a depth camera and an RGB camera. """ import os import matplotlib.pyplot as plt import numpy as np from cycler import cycle import pytransform3d.camera as pc import pytransform3d.rotations as pr import pytransform3d.trajectories as ptr import pytransform3d.transformations as pt BASE_DIR = "test/test_data/" data_dir = BASE_DIR search_path = "." while ( not os.path.exists(data_dir) and os.path.dirname(search_path) != "pytransform3d" ): search_path = os.path.join(search_path, "..") data_dir = os.path.join(search_path, BASE_DIR) intrinsic_matrix = np.loadtxt( os.path.join(data_dir, "reconstruction_camera_matrix.csv"), delimiter="," ) P = np.loadtxt( os.path.join(data_dir, "reconstruction_odometry.csv"), delimiter=",", skiprows=1, ) for t in range(len(P)): P[t, 3:] = pr.quaternion_wxyz_from_xyzw(P[t, 3:]) cam2world_trajectory = ptr.transforms_from_pqs(P) plt.figure(figsize=(5, 5)) ax = pt.plot_transform(s=0.3) ax = ptr.plot_trajectory(ax, P=P, s=0.1, n_frames=10) image_size = np.array([1920, 1440]) key_frames_indices = np.linspace(0, len(P) - 1, 10, dtype=int) colors = cycle("rgb") for i, c in zip(key_frames_indices, colors): pc.plot_camera( ax, intrinsic_matrix, cam2world_trajectory[i], sensor_size=image_size, virtual_image_distance=0.2, c=c, ) pos_min = np.min(P[:, :3], axis=0) pos_max = np.max(P[:, :3], axis=0) center = (pos_max + pos_min) / 2.0 max_half_extent = max(pos_max - pos_min) / 2.0 ax.set_xlim((center[0] - max_half_extent, center[0] + max_half_extent)) ax.set_ylim((center[1] - max_half_extent, center[1] + max_half_extent)) ax.set_zlim((center[2] - max_half_extent, center[2] + max_half_extent)) ax.view_init(azim=110, elev=40) plt.show() ================================================ FILE: examples/plots/plot_camera_with_image.py ================================================ """ ================ Camera Transform ================ We can see the camera frame and the world frame. There is a grid of points from which we know the world coordinates. If we know the location and orientation of the camera in the world, we can easily compute the location of the points on the image. """ import matplotlib.pyplot as plt import numpy as np from pytransform3d.camera import make_world_grid, world2image, plot_camera from pytransform3d.plot_utils import make_3d_axis from pytransform3d.rotations import active_matrix_from_intrinsic_euler_xyz from pytransform3d.transformations import transform_from, plot_transform cam2world = transform_from( active_matrix_from_intrinsic_euler_xyz([-np.pi + 1, -0.1, 0.3]), [0.2, -1, 0.5], ) focal_length = 0.0036 sensor_size = (0.00367, 0.00274) image_size = (640, 480) intrinsic_camera_matrix = np.array( [ [focal_length, 0, sensor_size[0] / 2], [0, focal_length, sensor_size[1] / 2], [0, 0, 1], ] ) world_grid = make_world_grid(n_points_per_line=101) image_grid = world2image( world_grid, cam2world, sensor_size, image_size, focal_length, kappa=0.4 ) plt.figure(figsize=(12, 5)) ax = make_3d_axis(1, 121, unit="m") ax.view_init(elev=30, azim=-70) plot_transform(ax) plot_transform(ax, A2B=cam2world, s=0.3, name="Camera") plot_camera( ax, intrinsic_camera_matrix, cam2world, sensor_size=sensor_size, virtual_image_distance=0.5, ) ax.set_title("Camera and world frames") ax.scatter(world_grid[:, 0], world_grid[:, 1], world_grid[:, 2], s=1, alpha=0.2) ax.scatter(world_grid[-1, 0], world_grid[-1, 1], world_grid[-1, 2], color="r") ax.view_init(elev=25, azim=-130) ax = plt.subplot(122, aspect="equal") ax.set_title("Camera image") ax.set_xlim(0, image_size[0]) ax.set_ylim(0, image_size[1]) ax.scatter(image_grid[:, 0], -(image_grid[:, 1] - image_size[1])) ax.scatter(image_grid[-1, 0], -(image_grid[-1, 1] - image_size[1]), color="r") plt.show() ================================================ FILE: examples/plots/plot_collision_objects.py ================================================ """ =========================== URDF with Collision Objects =========================== This example shows how to load a URDF description of collision objects and display them. """ import matplotlib.pyplot as plt from pytransform3d.urdf import UrdfTransformManager URDF = """ """ tm = UrdfTransformManager() tm.load_urdf(URDF) ax = tm.plot_frames_in("collision", s=0.1) tm.plot_collision_objects("collision", ax) ax.set_zlim((-0.5, 1.5)) plt.show() ================================================ FILE: examples/plots/plot_compare_rotations.py ================================================ """ ======================================== Compare Various Definitions of Rotations ======================================== """ import matplotlib.pyplot as plt import numpy as np from pytransform3d import rotations as pr ax = pr.plot_basis(R=np.eye(3), ax_s=2, lw=3) axis = 2 angle = np.pi p = np.array([1.0, 1.0, 1.0]) euler = [0, 0, 0] euler[axis] = angle R = pr.active_matrix_from_intrinsic_euler_xyz(euler) pr.plot_basis(ax, R, p) p = np.array([-1.0, 1.0, 1.0]) euler = [0, 0, 0] euler[2 - axis] = angle R = pr.active_matrix_from_intrinsic_euler_zyx(euler) pr.plot_basis(ax, R, p) p = np.array([1.0, 1.0, -1.0]) R = pr.active_matrix_from_angle(axis, angle) pr.plot_basis(ax, R, p) p = np.array([1.0, -1.0, -1.0]) e = [pr.unitx, pr.unity, pr.unitz][axis] a = np.hstack((e, (angle,))) R = pr.matrix_from_axis_angle(a) pr.plot_basis(ax, R, p) pr.plot_axis_angle(ax, a, p) p = np.array([-1.0, -1.0, -1.0]) q = pr.quaternion_from_axis_angle(a) R = pr.matrix_from_quaternion(q) pr.plot_basis(ax, R, p) plt.show() ================================================ FILE: examples/plots/plot_concatenate_uncertain_transforms.py ================================================ """ ================================ Concatenate Uncertain Transforms ================================ In this example, we assume that a robot is moving with constant velocity along the x-axis, however, there is noise in the orientation of the robot that accumulates and leads to different paths when sampling. Uncertainty accumulation leads to the so-called banana distribution, which does not seem Gaussian in Cartesian space, but it is Gaussian in exponential coordinates of SE(3). This example adapted and modified to 3D from Barfoot and Furgale [1]_. The banana distribution was analyzed in detail by Long et al. [2]_. """ import matplotlib.pyplot as plt import numpy as np import pytransform3d.plot_utils as ppu import pytransform3d.trajectories as ptr import pytransform3d.transformations as pt import pytransform3d.uncertainty as pu # %% # We configure the example here. You can change the random seed if you want. # We assume :math:`\Delta t = 1 s` and constant velocity, so that in each step # we concatenate the transformation :math:`\boldsymbol{T}_{vel}` defined # via its exponential coordinates here. The covariance associated with # :math:`\boldsymbol{T}_{vel}` is constructed from its Cholesky decomposition # through :math:`\boldsymbol{LL}^T`. Since it is a diagonal matrix at # the moment, we just define the standard deviation of each velocity component. # In the default configuration we have some uncertainty in the rotational # components of the velocity. rng = np.random.default_rng(0) cov_pose_chol = np.diag([0, 0.02, 0.03, 0, 0, 0]) cov_pose = np.dot(cov_pose_chol, cov_pose_chol.T) velocity_vector = np.array([0, 0, 0, 1.0, 0, 0]) T_vel = pt.transform_from_exponential_coordinates(velocity_vector) n_steps = 100 n_mc_samples = 1000 n_skip_trajectories = 1 # plot every n-th trajectory # %% # We compute each path with respect to the initial pose of the robot, which # we set to :math:`\boldsymbol{I}_{4 \times 4}`. The covariance of the initial # pose is :math:`\boldsymbol{0}_{6 \times 6}`. We concatenate the current # pose with :math:`\boldsymbol{T}_{vel}` in each step and factor in the # uncertainty of the pose and the transformation. Hence, uncertainties will # accumulate in the covariance of the pose. T_est = np.eye(4) path = np.zeros((n_steps + 1, 6)) path[0] = pt.exponential_coordinates_from_transform(T_est) cov_est = np.zeros((6, 6)) for t in range(n_steps): T_est, cov_est = pu.concat_globally_uncertain_transforms( T_est, cov_est, T_vel, cov_pose ) path[t + 1] = pt.exponential_coordinates_from_transform(T_est) # %% # We perform Monte-Carlo sampling of trajectories for comparison. # This implementation is vectorized with NumPy. T = np.eye(4) mc_path = np.zeros((n_steps + 1, n_mc_samples, 4, 4)) mc_path[0, :] = T for t in range(n_steps): noise_samples = ptr.transforms_from_exponential_coordinates( cov_pose_chol.dot(rng.standard_normal(size=(6, n_mc_samples))).T ) step_samples = ptr.concat_many_to_one(noise_samples, T_vel) mc_path[t + 1] = np.einsum("nij,njk->nik", step_samples, mc_path[t]) # Plot the random samples' trajectory lines (in a frame attached to the start) # same as mc_path[t, i, :3, :3].T.dot(mc_path[t, i, :3, 3]), but faster mc_path_vec = np.einsum( "tinm,tin->tim", mc_path[:, :, :3, :3], mc_path[:, :, :3, 3] ) # %% # We plot the MC-sampled trajectories and final poses, mean trajectory, # projected equiprobably hyperellipsoid of the final distribution, and # the equiprobably ellipsoid of the final distribution of the position. ax = ppu.make_3d_axis(100) for i in range(0, mc_path_vec.shape[1], n_skip_trajectories): ax.plot( mc_path_vec[:, i, 0], mc_path_vec[:, i, 1], mc_path_vec[:, i, 2], lw=1, c="b", alpha=0.05, ) ax.scatter( mc_path_vec[-1, :, 0], mc_path_vec[-1, :, 1], mc_path_vec[-1, :, 2], s=3, c="b", ) ptr.plot_trajectory( ax, ptr.pqs_from_transforms(ptr.transforms_from_exponential_coordinates(path)), s=5.0, lw=3, ) pu.plot_projected_ellipsoid( ax, T_est, cov_est, wireframe=False, alpha=0.3, color="y", factor=3.0 ) pu.plot_projected_ellipsoid( ax, T_est, cov_est, wireframe=True, alpha=0.5, color="y", factor=3.0 ) mean_mc = np.mean(mc_path_vec[-1, :], axis=0) cov_mc = np.cov(mc_path_vec[-1, :], rowvar=False) ellipsoid2origin, radii = pu.to_ellipsoid(mean_mc, cov_mc) ppu.plot_ellipsoid( ax, 3.0 * radii, ellipsoid2origin, wireframe=False, alpha=0.1, color="m" ) ppu.plot_ellipsoid( ax, 3.0 * radii, ellipsoid2origin, wireframe=True, alpha=0.3, color="m" ) plt.xlim((-5, 105)) plt.ylim((-50, 50)) plt.xlabel("x") plt.ylabel("y") ax.view_init(elev=70, azim=-90) plt.show() # %% # References # ---------- # .. [1] Barfoot, T. D., Furgale, P. T. (2014). Associating Uncertainty With # Three-Dimensional Poses for Use in Estimation Problems. IEEE Transactions # on Robotics 30(3), pp. 679-693, doi: 10.1109/TRO.2014.2298059. # # .. [2] Long, A. W., Wolfe, K. C., Mashner, M. J., Chirikjian, G. S. (2013). # The Banana Distribution is Gaussian: A Localization Study with Exponential # Coordinates. In Robotics: Science and Systems VIII, pp. 265-272. # http://www.roboticsproceedings.org/rss08/p34.pdf ================================================ FILE: examples/plots/plot_convention_rotation_global_local.py ================================================ """ ================================================================ Convention for Rotation: Passive / Active, Extrinsic / Intrinsic ================================================================ We will compare all possible combinations of passive and active rotations and extrinsic and intrinsic concatenation of rotations. """ import matplotlib.pyplot as plt import numpy as np from mpl_toolkits.mplot3d import proj3d # noqa: F401 from pytransform3d.rotations import ( passive_matrix_from_angle, active_matrix_from_angle, plot_basis, ) # %% # Passive Extrinsic Rotations # --------------------------- plt.figure(figsize=(8, 3)) axes = [plt.subplot(1, 3, 1 + i, projection="3d") for i in range(3)] for ax in axes: plt.setp( ax, xlim=(-1, 1), ylim=(-1, 1), zlim=(-1, 1), xlabel="X", ylabel="Y", zlabel="Z", ) Rx45 = passive_matrix_from_angle(0, np.deg2rad(45)) Rz45 = passive_matrix_from_angle(2, np.deg2rad(45)) axes[0].set_title("Passive Extrinsic Rotations", y=0.95) plot_basis(ax=axes[0], R=np.eye(3)) axes[1].set_title("$R_x(45^{\circ})$", y=0.95) plot_basis(ax=axes[1], R=Rx45) axes[2].set_title("$R_z(45^{\circ}) R_x(45^{\circ})$", y=0.95) plot_basis(ax=axes[2], R=Rz45.dot(Rx45)) plt.tight_layout() # %% # Passive Intrinsic Rotations # --------------------------- plt.figure(figsize=(8, 3)) axes = [plt.subplot(1, 3, 1 + i, projection="3d") for i in range(3)] for ax in axes: plt.setp( ax, xlim=(-1, 1), ylim=(-1, 1), zlim=(-1, 1), xlabel="X", ylabel="Y", zlabel="Z", ) axes[0].set_title("Passive Intrinsic Rotations", y=0.95) plot_basis(ax=axes[0], R=np.eye(3)) axes[1].set_title("$R_x(45^{\circ})$", y=0.95) plot_basis(ax=axes[1], R=Rx45) axes[2].set_title("$R_x(45^{\circ}) R_{z'}(45^{\circ})$", y=0.95) plot_basis(ax=axes[2], R=Rx45.dot(Rz45)) plt.tight_layout() # %% # Active Extrinsic Rotations # -------------------------- plt.figure(figsize=(8, 3)) axes = [plt.subplot(1, 3, 1 + i, projection="3d") for i in range(3)] for ax in axes: plt.setp( ax, xlim=(-1, 1), ylim=(-1, 1), zlim=(-1, 1), xlabel="X", ylabel="Y", zlabel="Z", ) Rx45 = active_matrix_from_angle(0, np.deg2rad(45)) Rz45 = active_matrix_from_angle(2, np.deg2rad(45)) axes[0].set_title("Active Extrinsic Rotations", y=0.95) plot_basis(ax=axes[0], R=np.eye(3)) axes[1].set_title("$R_x(45^{\circ})$", y=0.95) plot_basis(ax=axes[1], R=Rx45) axes[2].set_title("$R_z(45^{\circ}) R_x(45^{\circ})$", y=0.95) plot_basis(ax=axes[2], R=Rz45.dot(Rx45)) plt.tight_layout() # %% # Active Intrinsic Rotations # -------------------------- plt.figure(figsize=(8, 3)) axes = [plt.subplot(1, 3, 1 + i, projection="3d") for i in range(3)] for ax in axes: plt.setp( ax, xlim=(-1, 1), ylim=(-1, 1), zlim=(-1, 1), xlabel="X", ylabel="Y", zlabel="Z", ) axes[0].set_title("Active Intrinsic Rotations", y=0.95) plot_basis(ax=axes[0], R=np.eye(3)) axes[1].set_title("$R_x(45^{\circ})$", y=0.95) plot_basis(ax=axes[1], R=Rx45) axes[2].set_title("$R_x(45^{\circ}) R_{z'}(45^{\circ})$", y=0.95) plot_basis(ax=axes[2], R=Rx45.dot(Rz45)) plt.tight_layout() plt.show() ================================================ FILE: examples/plots/plot_cylinder.py ================================================ """ ========================== Plot Transformed Cylinders ========================== Plots surfaces of transformed cylindrical shells. """ import matplotlib.pyplot as plt import numpy as np from pytransform3d.plot_utils import plot_cylinder, remove_frame from pytransform3d.rotations import random_axis_angle, matrix_from_axis_angle from pytransform3d.transformations import transform_from, plot_transform rng = np.random.default_rng(44) A2B = transform_from( R=matrix_from_axis_angle(random_axis_angle(rng)), p=rng.standard_normal(size=3), ) ax = plot_cylinder( length=1.0, radius=0.3, thickness=0.1, wireframe=False, alpha=0.2 ) plot_transform(ax=ax, A2B=np.eye(4), s=0.3, lw=3) plot_cylinder( ax=ax, length=1.0, radius=0.3, thickness=0.1, A2B=A2B, wireframe=False, alpha=0.2, ) plot_transform(ax=ax, A2B=A2B, s=0.3, lw=3) remove_frame(ax) ax.set_xlim((0, 1.5)) ax.set_ylim((-1.5, 0)) ax.set_zlim((-0.8, 0.7)) plt.show() ================================================ FILE: examples/plots/plot_dual_quaternion_interpolation.py ================================================ """ ============================= Dual Quaternion Interpolation ============================= This example shows interpolated trajectories between two random poses. The red line corresponds to linear interpolation with exponential coordinates, the green line corresponds to linear interpolation with dual quaternions, and the blue line corresponds to screw linear interpolation (ScLERP) with dual quaternions. The true screw motion from pose 1 to pose 2 is shown by a thick, transparent black line in the background of the ScLERP interpolation. """ import matplotlib.pyplot as plt import numpy as np import pytransform3d.plot_utils as ppu import pytransform3d.trajectories as ptr import pytransform3d.transformations as pt # %% # Setup # ----- # We generate two random transformation matrices to represent two poses # between we want to interpolate. These will also be converted to dual # quaternions, exponential coordinates, and position and quaternion. rng = np.random.default_rng(25) pose1 = pt.random_transform(rng) pose2 = pt.random_transform(rng) dq1 = pt.dual_quaternion_from_transform(pose1) dq2 = -pt.dual_quaternion_from_transform(pose2) Stheta1 = pt.exponential_coordinates_from_transform(pose1) Stheta2 = pt.exponential_coordinates_from_transform(pose2) pq1 = pt.pq_from_transform(pose1) pq2 = pt.pq_from_transform(pose2) n_steps = 100 # %% # Ground Truth: Screw Motion # -------------------------- # The ground truth for interpolation of poses is a linear interpolation of # rotation about and translation along the screw axis. We will represent the # difference between the two poses as exponential coordinates, a product of the # screw axis and the magnitude of the transformation. We can use fractions of # the exponential coordinates to smoothly interpolate between the two poses. pose12pose2 = pt.concat(pose2, pt.invert_transform(pose1)) Stheta = pt.exponential_coordinates_from_transform(pose12pose2) offsets = ptr.transforms_from_exponential_coordinates( Stheta[np.newaxis] * np.linspace(0, 1, n_steps)[:, np.newaxis] ) interpolated_poses = ptr.concat_many_to_one(offsets, pose1) # %% # Approximation: Linear Interpolation of Dual Quaternions # ------------------------------------------------------- # An approximately correct solution is linear interpolation and subsequent # normalization of dual quaternions. The problem with dual quaternions is that # they have a double cover and the path of the interpolation might be different # depending on which of the two representation of the pose is selected. In this # case the path does not match the ground truth path, but when we switch from # dq1 to pt.dual_quaternion_double(dq1)---its alternative representation---the # interpolation path is very close to the ground truth. interpolated_dqs = ( np.linspace(1, 0, n_steps)[:, np.newaxis] * dq1 + np.linspace(0, 1, n_steps)[:, np.newaxis] * dq2 ) # renormalization (not required here because it will be done with conversion) interpolated_dqs /= np.linalg.norm(interpolated_dqs[:, :4], axis=1)[ :, np.newaxis ] interpolated_poses_from_dqs = ptr.transforms_from_dual_quaternions( interpolated_dqs ) # %% # Exact Solution: Screw Linear Interpolation (ScLERP) # --------------------------------------------------- # Dual quaternions also support screw linear interpolation (ScLERP) which is # implemented with the dual quaternion power. The dual quaternion power # internally uses the screw parameters of the pose difference to smoothly # interpolate along the screw axis. sclerp_interpolated_dqs = np.vstack( [pt.dual_quaternion_sclerp(dq1, dq2, t) for t in np.linspace(0, 1, n_steps)] ) sclerp_interpolated_poses_from_dqs = ptr.transforms_from_dual_quaternions( sclerp_interpolated_dqs ) # %% # Approximation: Linear Interpolation of Exponential Coordinates # -------------------------------------------------------------- # A more crude approximation is the linear interpolation of exponential # coordinates. interpolated_ecs = ( np.linspace(1, 0, n_steps)[:, np.newaxis] * Stheta1 + np.linspace(0, 1, n_steps)[:, np.newaxis] * Stheta2 ) interpolates_poses_from_ecs = ptr.transforms_from_exponential_coordinates( interpolated_ecs ) # %% # Linear Interpolation of Position + SLERP # ---------------------------------------- # A completly different solution can be obtained by decomposing the poses into # positions and orientations and then using spherical linear interpolation # (SLERP) of the orientation (in this case: quaternions). interpolated_pqs = np.vstack( [pt.pq_slerp(pq1, pq2, t) for t in np.linspace(0, 1, n_steps)] ) interpolated_poses_from_pqs = ptr.transforms_from_pqs(interpolated_pqs) # %% # Plotting # -------- # We show all solutions in one 3D plot. ax = pt.plot_transform(A2B=pose1, s=0.3, ax_s=2) pt.plot_transform(A2B=pose2, s=0.3, ax=ax) traj = ppu.Trajectory( interpolated_poses, s=0.1, c="k", lw=5, alpha=0.5, show_direction=True ) traj.add_trajectory(ax) traj_from_dqs = ppu.Trajectory( interpolated_poses_from_dqs, s=0.1, c="g", show_direction=False ) traj_from_dqs.add_trajectory(ax) traj_from_ecs = ppu.Trajectory( interpolates_poses_from_ecs, s=0.1, c="r", show_direction=False ) traj_from_ecs.add_trajectory(ax) traj_from_dqs_sclerp = ppu.Trajectory( sclerp_interpolated_poses_from_dqs, s=0.1, c="b", show_direction=False ) traj_from_dqs_sclerp.add_trajectory(ax) traj_from_pq_slerp = ppu.Trajectory( interpolated_poses_from_pqs, s=0.1, c="c", show_direction=False ) traj_from_pq_slerp.add_trajectory(ax) plt.legend( [ traj.trajectory, traj_from_dqs.trajectory, traj_from_ecs.trajectory, traj_from_dqs_sclerp.trajectory, traj_from_pq_slerp.trajectory, ], [ "Screw interpolation", "Linear dual quaternion interpolation", "Linear interpolation of exp. coordinates", "Dual quaternion ScLERP", "Linear interpolation of position + SLERP of quaternions", ], loc="best", ) plt.show() ================================================ FILE: examples/plots/plot_euler_angles.py ================================================ """ ============ Euler Angles ============ Any rotation can be represented by three consecutive rotations about three basis vectors. Here we use the extrinsic xyz convention. """ import matplotlib.pyplot as plt import numpy as np from pytransform3d import rotations as pr from pytransform3d.plot_utils import remove_frame alpha, beta, gamma = 0.5 * np.pi, 0.5 * np.pi, 0.5 * np.pi p = np.array([1, 1, 1]) plt.figure(figsize=(5, 5)) ax = pr.plot_basis(R=np.eye(3), p=-1.5 * p, ax_s=2) pr.plot_axis_angle(ax, [1, 0, 0, alpha], -1.5 * p) pr.plot_basis( ax, pr.active_matrix_from_extrinsic_euler_xyz([alpha, 0, 0]), -0.5 * p ) pr.plot_axis_angle(ax, [0, 1, 0, beta], p=-0.5 * p) pr.plot_basis( ax, pr.active_matrix_from_extrinsic_euler_xyz([alpha, beta, 0]), 0.5 * p ) pr.plot_axis_angle(ax, [0, 0, 1, gamma], 0.5 * p) pr.plot_basis( ax, pr.active_matrix_from_extrinsic_euler_xyz([alpha, beta, gamma]), 1.5 * p, lw=5, ) remove_frame(ax) plt.show() ================================================ FILE: examples/plots/plot_frames.py ================================================ """ =============================================== Plot with Respect to Different Reference Frames =============================================== In this example, we will demonstrate how to use the TransformManager. We will add several transforms to the manager and plot all frames in two reference frames ('world' and 'A'). """ import matplotlib.pyplot as plt import numpy as np from pytransform3d.plot_utils import make_3d_axis from pytransform3d.transform_manager import TransformManager from pytransform3d.transformations import random_transform rng = np.random.default_rng(5) A2world = random_transform(rng) B2world = random_transform(rng) A2C = random_transform(rng) D2B = random_transform(rng) tm = TransformManager() tm.add_transform("A", "world", A2world) tm.add_transform("B", "world", B2world) tm.add_transform("A", "C", A2C) tm.add_transform("D", "B", D2B) plt.figure(figsize=(10, 5)) ax = make_3d_axis(2, 121) ax = tm.plot_frames_in("world", ax=ax, alpha=0.6) ax.view_init(30, 20) ax = make_3d_axis(3, 122) ax = tm.plot_frames_in("A", ax=ax, alpha=0.6) ax.view_init(30, 20) plt.show() ================================================ FILE: examples/plots/plot_interpolation_for_transform_manager.py ================================================ """ ================================== Managing Transformations over Time ================================== In this example, given two trajectories of 3D rigid transformations, we will interpolate both and use the transform manager for the target timestep. """ import matplotlib.pyplot as plt import numpy as np from pytransform3d import rotations as pr from pytransform3d import transformations as pt from pytransform3d.transform_manager import ( TemporalTransformManager, NumpyTimeseriesTransform, ) def create_sinusoidal_movement( duration_sec, sample_period, x_velocity, y_start_offset, start_time ): """Create a planar (z=0) sinusoidal movement around x-axis.""" time = np.arange(0, duration_sec, sample_period) + start_time n_steps = len(time) x = np.linspace(0, x_velocity * duration_sec, n_steps) spatial_freq = 1.0 / 5.0 # 1 sinus per 5m omega = 2.0 * np.pi * spatial_freq y = np.sin(omega * x) y += y_start_offset dydx = omega * np.cos(omega * x) yaw = np.arctan2(dydx, np.ones_like(dydx)) pqs = [] for i in range(n_steps): R = pr.active_matrix_from_extrinsic_euler_zyx([yaw[i], 0, 0]) T = pt.transform_from(R, [x[i], y[i], 0]) pq = pt.pq_from_transform(T) pqs.append(pq) return time, np.array(pqs) # create entities A and B together with their transformations from world duration = 10.0 # [s] sample_period = 0.5 # [s] velocity_x = 1 # [m/s] time_A, pqs_A = create_sinusoidal_movement( duration, sample_period, velocity_x, y_start_offset=2.0, start_time=0.1 ) time_B, pqs_B = create_sinusoidal_movement( duration, sample_period, velocity_x, y_start_offset=-2.0, start_time=0.35 ) # package them into an instance of `TimeVaryingTransform` abstract class transform_WA = NumpyTimeseriesTransform(time_A, pqs_A) transform_WB = NumpyTimeseriesTransform(time_B, pqs_B) tm = TemporalTransformManager() tm.add_transform("A", "world", transform_WA) tm.add_transform("B", "world", transform_WB) query_time = 4.9 # [s] or an array of times np.array([4.9, 5.2]) A2B_at_query_time = tm.get_transform_at_time("A", "B", query_time) # transform the origin of A in A (x=0, y=0, z=0) to B origin_of_A_pos = pt.vector_to_point([0, 0, 0]) origin_of_A_in_B_xyz = pt.transform(A2B_at_query_time, origin_of_A_pos)[:-1] # for visualization purposes pq_A = pt.pq_from_transform(transform_WA.as_matrix(query_time)) pq_B = pt.pq_from_transform(transform_WB.as_matrix(query_time)) plt.figure(figsize=(8, 8)) plt.plot(pqs_A[:, 0], pqs_A[:, 1], "bo--", label="trajectory $A(t)$") plt.plot(pqs_B[:, 0], pqs_B[:, 1], "yo--", label="trajectory $B(t)$") plt.scatter(pq_A[0], pq_A[1], color="b", s=120, marker="d", label="$A(t_q)$") plt.scatter(pq_B[0], pq_B[1], color="y", s=120, marker="^", label="$B(t_q)$") plt.text( pq_A[0] + 0.3, pq_A[1] - 0.3, f"origin of A in B:\n(x={origin_of_A_in_B_xyz[0]:.2f}m," + f" y={origin_of_A_in_B_xyz[1]:.2f}m)", ) plt.xlabel("x [m]") plt.ylabel("y [m]") plt.xlim(0, 10) plt.ylim(-5, 5) plt.grid() plt.legend() plt.show() ================================================ FILE: examples/plots/plot_invert_uncertain_transform.py ================================================ """ ========================== Invert Uncertain Transform ========================== We sample from the original transform distribution and from the inverse distribution. Samples are then projected to all 2D planes and plotted. The color indicates the pose distribution. Green is the original distribution and red is the inverse. """ import matplotlib.pyplot as plt import numpy as np import pytransform3d.rotations as pr import pytransform3d.trajectories as ptr import pytransform3d.transformations as pt import pytransform3d.uncertainty as pu n_mc_samples = 1000 rng = np.random.default_rng(1) alpha = 2.0 A2B = pt.transform_from( R=pr.matrix_from_euler([1.5, 0.5, 1.3], 0, 1, 2, True), p=[10.0, -7.0, -5.0] ) variances = alpha * np.array([0.1, 0.2, 0.1, 2.0, 1.0, 1.0]) std_devs = np.sqrt(variances) correlations = np.array( [ [0.5, 0, 0, 0, 0, 0], [0.1, 0.5, 0, 0, 0, 0], [-0.3, 0.1, 0.5, 0, 0, 0], [-0.1, 0.2, 0.3, 0.5, 0, 0], [-0.3, -0.1, -0.2, 0.3, 0.5, 0], [-0.1, 0.1, 0.2, 0.1, 0.3, 0.5], ] ) correlations += correlations.T cov_A2B = correlations * np.outer(std_devs, std_devs) x_A2B = pt.exponential_coordinates_from_transform(A2B) samples_A2B = ptr.exponential_coordinates_from_transforms( [pt.random_transform(rng, A2B, cov_A2B) for _ in range(n_mc_samples)] ) B2A, cov_B2A = pu.invert_uncertain_transform(A2B, cov_A2B) x_B2A = pt.exponential_coordinates_from_transform(B2A) samples_B2A = ptr.exponential_coordinates_from_transforms( [pt.random_transform(rng, B2A, cov_B2A) for _ in range(n_mc_samples)] ) _, axes = plt.subplots(nrows=6, ncols=6, squeeze=True, figsize=(10, 10)) for i in range(6): for j in range(6): if i == j: continue indices = np.array([i, j]) ax = axes[i][j] ax.scatter( samples_A2B[:, i], samples_A2B[:, j], color="g", s=1, alpha=0.3 ) ax.scatter( samples_B2A[:, i], samples_B2A[:, j], color="r", s=1, alpha=0.3 ) ax.scatter(0, 0, color="k") plt.show() ================================================ FILE: examples/plots/plot_matrix_from_two_vectors.py ================================================ """ ========================================== Construct Rotation Matrix from Two Vectors ========================================== We compute rotation matrix from two vectors that form a plane. The x-axis will point in the same direction as the first vector, the y-axis corresponds to the normalized vector rejection of b on a, and the z-axis is the cross product of the other basis vectors. """ import matplotlib.pyplot as plt import numpy as np from pytransform3d.plot_utils import plot_vector from pytransform3d.rotations import ( matrix_from_two_vectors, plot_basis, random_vector, ) rng = np.random.default_rng(1) a = random_vector(rng, 3) * 0.3 b = random_vector(rng, 3) * 0.3 R = matrix_from_two_vectors(a, b) ax = plot_vector(direction=a, color="r") plot_vector(ax=ax, direction=b, color="g") plot_basis(ax=ax, R=R) plt.show() ================================================ FILE: examples/plots/plot_mesh.py ================================================ """ ========= Plot Mesh ========= This example shows how to load an STL mesh. This example must be run from within the main folder because it uses a hard-coded path to the STL file. """ import os import matplotlib.pyplot as plt import numpy as np from pytransform3d.plot_utils import plot_mesh from pytransform3d.transformations import plot_transform BASE_DIR = "test/test_data/" data_dir = BASE_DIR search_path = "." while ( not os.path.exists(data_dir) and os.path.dirname(search_path) != "pytransform3d" ): search_path = os.path.join(search_path, "..") data_dir = os.path.join(search_path, BASE_DIR) ax = plot_mesh( filename=os.path.join(data_dir, "cone.stl"), s=5 * np.ones(3), alpha=0.3 ) plot_transform(ax=ax, A2B=np.eye(4), s=0.3, lw=3) plt.show() ================================================ FILE: examples/plots/plot_polar_decomposition.py ================================================ """ ======================== Plot Polar Decomposition ======================== Robust polar decomposition orthonormalizes basis vectors (i.e., rotation matrices). It is more expensive than standard Gram-Schmidt orthonormalization, but it spreads the error more evenly over all basis vectors. The top row of these plots shows the unnormalized bases that were obtained by randomly rotating one of the columns of the identity matrix. The middle row shows Gram-Schmidt orthonormalization and the bottom row shows orthonormalization through robust polar decomposition. For comparison, we show the unnormalized basis with dashed lines in the last two rows. """ import matplotlib.pyplot as plt import numpy as np from pytransform3d import rotations as pr n_cases = 4 fig, axes = plt.subplots( 3, n_cases, subplot_kw={"projection": "3d"}, figsize=(8, 8) ) ax_s = 1.0 plot_center = np.array([-0.2, -0.2, -0.2]) for ax in axes.flat: ax.set_xticks([]) ax.set_yticks([]) ax.set_zticks([]) ax.set_xlim(-ax_s, ax_s) ax.set_ylim(-ax_s, ax_s) ax.set_zlim(-ax_s, ax_s) titles = ["Unnormalized Bases", "Gram-Schmidt", "Polar Decomposition"] for ax, title in zip(axes[:, 0], titles): ax.set_title(title) rng = np.random.default_rng(46) for i in range(n_cases): random_axis = rng.integers(0, 3) R_unnormalized = np.eye(3) R_unnormalized[:, random_axis] = np.dot( pr.random_matrix(rng, cov=0.1 * np.eye(3)), R_unnormalized[:, random_axis], ) pr.plot_basis(axes[0, i], R_unnormalized, p=plot_center, strict_check=False) R_gs = pr.norm_matrix(R_unnormalized) pr.plot_basis( axes[1, i], R_unnormalized, p=plot_center, strict_check=False, ls="--" ) pr.plot_basis(axes[1, i], R_gs, p=plot_center) R_pd = pr.robust_polar_decomposition(R_unnormalized) pr.plot_basis( axes[2, i], R_unnormalized, p=plot_center, strict_check=False, ls="--" ) pr.plot_basis(axes[2, i], R_pd, p=plot_center) plt.tight_layout() plt.show() ================================================ FILE: examples/plots/plot_pose_fusion.py ================================================ """ ============ Fuse 3 Poses ============ Each of the poses has an associated covariance that is considered during the fusion. Each of the plots shows a projection of the distributions in exponential coordinate space to two dimensions. Red, green, and blue ellipses represent the uncertain poses that will be fused. A black ellipse indicates the fused pose's distribution. This example is from Barfoot, Furgale: Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems, http://ncfrn.mcgill.ca/members/pubs/barfoot_tro14.pdf """ import matplotlib.pyplot as plt import numpy as np import pytransform3d.transformations as pt import pytransform3d.uncertainty as pu # %% # Helper Functions # ---------------- # Here you find some helper functions for plotting. def to_ellipse(cov, factor=1.0): """Compute error ellipse. An error ellipse shows equiprobable points of a 2D Gaussian distribution. Parameters ---------- cov : array-like, shape (2, 2) Covariance of the Gaussian distribution. factor : float, optional (default: 1) One means standard deviation. Returns ------- angle : float Rotation angle of the ellipse. width : float Width of the ellipse (semi axis, not diameter). height : float Height of the ellipse (semi axis, not diameter). """ from scipy import linalg vals, vecs = linalg.eigh(cov) order = vals.argsort()[::-1] vals, vecs = vals[order], vecs[:, order] angle = np.arctan2(*vecs[:, 0][::-1]) width, height = factor * np.sqrt(vals) return angle, width, height def plot_error_ellipse( ax, mean, cov, color=None, alpha=0.25, factors=np.linspace(0.25, 2.0, 8) ): """Plot error ellipse of MVN. Parameters ---------- ax : axis Matplotlib axis. mean : array-like, shape (2,) Mean of the Gaussian distribution. cov : array-like, shape (2, 2) Covariance of the Gaussian distribution. color : str, optional (default: None) Color in which the ellipse should be plotted alpha : float, optional (default: 0.25) Alpha value for ellipse factors : array, optional (default: np.linspace(0.25, 2.0, 8)) Multiples of the standard deviations that should be plotted. """ from matplotlib.patches import Ellipse for factor in factors: angle, width, height = to_ellipse(cov, factor) ell = Ellipse( xy=mean, width=2.0 * width, height=2.0 * height, angle=np.degrees(angle), ) ell.set_alpha(alpha) if color is not None: ell.set_color(color) ax.add_artist(ell) # %% # Example Configuration # --------------------- # A ground truth pose is known in this example. We sample 3 poses with # different covariances around the ground truth pose. x_true = np.array([1.0, 0.0, 0.0, 0.0, 0.0, np.pi / 6.0]) T_true = pt.transform_from_exponential_coordinates(x_true) alpha = 5.0 cov1 = alpha * np.diag([0.1, 0.2, 0.1, 2.0, 1.0, 1.0]) cov2 = alpha * np.diag([0.1, 0.1, 0.2, 1.0, 3.0, 1.0]) cov3 = alpha * np.diag([0.2, 0.1, 0.1, 1.0, 1.0, 5.0]) rng = np.random.default_rng(0) T1 = np.dot( pt.transform_from_exponential_coordinates( pt.random_exponential_coordinates(rng=rng, cov=cov1) ), T_true, ) T2 = np.dot( pt.transform_from_exponential_coordinates( pt.random_exponential_coordinates(rng=rng, cov=cov2) ), T_true, ) T3 = np.dot( pt.transform_from_exponential_coordinates( pt.random_exponential_coordinates(rng=rng, cov=cov3) ), T_true, ) x1 = pt.exponential_coordinates_from_transform(T1) x2 = pt.exponential_coordinates_from_transform(T2) x3 = pt.exponential_coordinates_from_transform(T3) # %% # Fusion # ------ # We can then fuse the 3 sampled poses with their associated covariances. T_est, cov_est, V = pu.pose_fusion([T1, T2, T3], [cov1, cov2, cov3]) # %% # Plotting # -------- # Each subplot shows two dimensions of the exponential coordinate space. # Each sampled pose and its associated covariance matrix is displayed as an # equiprobable ellipse. The fused pose estimate is also shown with its # associated covariance as an equiprobable ellipse. For comparison, the ground # truth pose is shown as a point. x_est = pt.exponential_coordinates_from_transform(T_est) _, axes = plt.subplots( nrows=6, ncols=6, sharex=True, sharey=True, squeeze=True, figsize=(10, 10) ) factors = [1.0] for i in range(6): for j in range(6): if i == j: continue indices = np.array([i, j]) ax = axes[i][j] for x, cov, color in zip([x1, x2, x3], [cov1, cov2, cov3], "rgb"): plot_error_ellipse( ax, x[indices], cov[indices][:, indices], color=color, alpha=0.4, factors=factors, ) plot_error_ellipse( ax, x_est[indices], cov_est[indices][:, indices], color="k", alpha=1, factors=factors, ) ax.scatter(x_true[i], x_true[j]) ax.set_xlim((-10, 10)) ax.set_ylim((-10, 10)) plt.show() ================================================ FILE: examples/plots/plot_pose_trajectory.py ================================================ """ =============== Pose Trajectory =============== Plotting pose trajectories with pytransform3d is easy. """ import matplotlib.pyplot as plt import numpy as np from pytransform3d.batch_rotations import quaternion_slerp_batch from pytransform3d.rotations import q_id from pytransform3d.trajectories import plot_trajectory n_steps = 100000 P = np.empty((n_steps, 7)) P[:, 0] = np.cos(np.linspace(-2 * np.pi, 2 * np.pi, n_steps)) P[:, 1] = np.sin(np.linspace(-2 * np.pi, 2 * np.pi, n_steps)) P[:, 2] = np.linspace(-1, 1, n_steps) q_end = np.array([0.0, 0.0, np.sqrt(0.5), np.sqrt(0.5)]) P[:, 3:] = quaternion_slerp_batch(q_id, q_end, np.linspace(0, 1, n_steps)) ax = plot_trajectory( P=P, s=0.3, n_frames=100, normalize_quaternions=False, lw=2, c="k" ) plt.show() ================================================ FILE: examples/plots/plot_quaternion_integrate.py ================================================ """ ====================== Quaternion Integration ====================== Integrate angular velocities to a sequence of quaternions. """ import matplotlib.pyplot as plt import numpy as np from pytransform3d.rotations import ( quaternion_integrate, matrix_from_quaternion, plot_basis, ) angular_velocities = np.empty((21, 3)) angular_velocities[:, :] = np.array([np.sqrt(0.5), np.sqrt(0.5), 0.0]) angular_velocities *= np.pi Q = quaternion_integrate(angular_velocities, dt=0.1) ax = None for t in range(len(Q)): R = matrix_from_quaternion(Q[t]) p = 2 * (t / (len(Q) - 1) - 0.5) * np.ones(3) ax = plot_basis(ax=ax, s=0.15, R=R, p=p) plt.show() ================================================ FILE: examples/plots/plot_quaternion_slerp.py ================================================ """ ================ Quaternion SLERP ================ For small rotations, linear interpolation of quaternions gives almost the same results as spherical linear interpolation (SLERP). For larger angles there are significant differences as you can see in this example. The outer circle uses linear interpolation and the inner circle uses SLERP. You can play around with the value of 'end_angle' in this example. """ import matplotlib.pyplot as plt import numpy as np from pytransform3d.rotations import ( matrix_from_axis_angle, quaternion_from_matrix, quaternion_slerp, ) from pytransform3d.trajectories import plot_trajectory # %% # We assume the array T represents something like time. # The position follows a sigmoid profile on a circular path over time, hence # velocity is not constant. T = np.linspace(0, 1, 1001) sigmoid = 0.5 * (np.tanh(1.5 * np.pi * (T - 0.5)) + 1.0) radius = 0.5 start_angle = np.deg2rad(0.0) end_angle = np.deg2rad(350.0) R1 = matrix_from_axis_angle([0, 0, 1, 0.5 * np.pi]) R2_start = matrix_from_axis_angle([1, 0, 0, start_angle]) R2_end = matrix_from_axis_angle([1, 0, 0, end_angle]) q_start = quaternion_from_matrix(R1.dot(R2_start)) q_end = quaternion_from_matrix(R1.dot(R2_end)) # %% # The naive linear interpolation method computes a time-weighted average # between the orientation at the start and the orientation at the end. lerp = np.zeros((len(T), 7)) lerp[:, 0] = radius * np.cos(np.deg2rad(90) - end_angle * sigmoid) lerp[:, 2] = radius * np.sin(np.deg2rad(90) - end_angle * sigmoid) if end_angle > np.pi: q_end *= -1.0 lerp[:, 3:] = (1.0 - T)[:, np.newaxis] * q_start + T[:, np.newaxis] * q_end # %% # SLERP is the exact method to interpolate the orientations. slerp = np.zeros((len(T), 7)) slerp[:, 0] = 0.7 * radius * np.cos(np.deg2rad(90) - end_angle * sigmoid) slerp[:, 2] = 0.7 * radius * np.sin(np.deg2rad(90) - end_angle * sigmoid) for i, t in enumerate(T): slerp[i, 3:] = quaternion_slerp(q_start, q_end, t) # %% # The following 3D plot compares the two approaches. ax = plot_trajectory( P=lerp, show_direction=False, n_frames=40, s=0.05, ax_s=0.7 ) ax = plot_trajectory(P=slerp, show_direction=False, n_frames=40, s=0.05, ax=ax) ax.text(0.1, 0, 0, "SLERP") ax.text(0.4, 0, 0.6, "Naive linear interpolation") ax.view_init(elev=10, azim=90) plt.show() ================================================ FILE: examples/plots/plot_random_geometries.py ================================================ """ ====================== Plot Random Geometries ====================== Plotting of several geometric shapes is directly supported by the library. """ import matplotlib.pyplot as plt import numpy as np from pytransform3d.plot_utils import ( make_3d_axis, plot_box, plot_sphere, plot_cylinder, plot_ellipsoid, plot_capsule, plot_cone, ) from pytransform3d.transformations import ( random_transform, plot_transform, translate_transform, ) rng = np.random.default_rng(2832) # %% # A box is defined by its size along the three main axes and its pose. box2origin = random_transform(rng) box_size = rng.random(size=3) * 3 # %% # A sphere is defined by its position and radius. sphere_position = rng.standard_normal(size=3) sphere_radius = float(rng.random()) # %% # A cylinder is defined by its length, radius, and pose. cylinder2origin = random_transform(rng) length = float(rng.random()) * 5 cylinder_radius = float(rng.random()) # %% # An ellipsoid is defined by its 3 radii and pose. ellipsoid2origin = random_transform(rng) radii = rng.random(size=3) * 3 # %% # A capsule is defined by its height, radius, and pose. capsule2origin = random_transform(rng) capsule_height = float(rng.random()) * 2 capsule_radius = float(rng.random()) # %% # A cone is defined by its height, radius, and pose. cone2origin = random_transform(rng) cone_height = float(rng.random()) * 5 cone_radius = float(rng.random()) # %% # The following part shows pytransform3d's 3D plotting functions. ax = make_3d_axis(2) plot_transform(ax=ax, A2B=box2origin, s=0.3) plot_box( ax=ax, A2B=box2origin, size=box_size, color="b", alpha=0.5, wireframe=False ) plot_transform( ax=ax, A2B=translate_transform(np.eye(4), sphere_position), s=0.3 ) plot_sphere( ax=ax, p=sphere_position, radius=sphere_radius, color="y", alpha=0.5, wireframe=False, ) plot_transform(ax=ax, A2B=cylinder2origin, s=0.3) plot_cylinder( ax=ax, A2B=cylinder2origin, length=length, radius=cylinder_radius, color="g", alpha=0.5, wireframe=False, ) plot_transform(ax=ax, A2B=ellipsoid2origin, s=0.3) plot_ellipsoid( ax=ax, A2B=ellipsoid2origin, radii=radii, color="m", alpha=0.5, wireframe=False, ) plot_transform(ax=ax, A2B=capsule2origin, s=0.3) plot_capsule( ax=ax, A2B=capsule2origin, height=capsule_height, radius=capsule_radius, color="r", alpha=0.5, wireframe=False, ) plot_transform(ax=ax, A2B=cone2origin, s=0.3) plot_cone( ax=ax, A2B=cone2origin, height=cone_height, radius=cone_radius, color="c", alpha=0.5, wireframe=False, ) plt.show() ================================================ FILE: examples/plots/plot_random_trajectories.py ================================================ """ =================== Random Trajectories =================== These plots show several randomly generated trajectories. Each row shows a different trajectory. On the left side you can see the position and orientation represented by small coordinate frames. On the right side you can see the positions over time. """ import matplotlib.pyplot as plt import numpy as np import pytransform3d.trajectories as ptr import pytransform3d.transformations as pt # %% # We sample three random trajectories. n_trajectories = 3 trajectories = ptr.random_trajectories( rng=np.random.default_rng(5), n_trajectories=n_trajectories, n_steps=1001, start=np.eye(4), goal=pt.transform_from(R=np.eye(3), p=0.3 * np.ones(3)), scale=[200] * 3 + [50] * 3, ) # %% # We plot the trajectory in 3D on the left and in 2D on the right. plt.figure(figsize=(8, 8)) for i in range(n_trajectories): ax = plt.subplot(n_trajectories, 2, 1 + 2 * i, projection="3d") plt.setp( ax, xlim=(-0.1, 0.5), ylim=(-0.1, 0.5), zlim=(-0.1, 0.5), xlabel="X", ylabel="Y", ) ax.set_xticks(()) ax.set_yticks(()) ax.set_zticks(()) ptr.plot_trajectory( ax=ax, P=ptr.pqs_from_transforms(trajectories[i]), s=0.1 ) ax = plt.subplot(n_trajectories, 2, 2 + 2 * i) for d in range(3): plt.plot(trajectories[i, :, d, 3].T, label="XYZ"[d]) if i != n_trajectories - 1: ax.set_xticks(()) else: ax.set_xlabel("Time step") ax.legend(loc="best") ax.set_ylabel("Position") ax.set_xlim((0, trajectories.shape[1] - 1)) plt.tight_layout() plt.show() ================================================ FILE: examples/plots/plot_robot.py ================================================ """ ===== Robot ===== We see a 6-DOF robot arm with visuals. """ import os import matplotlib.pyplot as plt import numpy as np from pytransform3d.urdf import UrdfTransformManager BASE_DIR = "test/test_data/" data_dir = BASE_DIR search_path = "." while ( not os.path.exists(data_dir) and os.path.dirname(search_path) != "pytransform3d" ): search_path = os.path.join(search_path, "..") data_dir = os.path.join(search_path, BASE_DIR) tm = UrdfTransformManager() filename = os.path.join(data_dir, "robot_with_visuals.urdf") with open(filename, "r") as f: robot_urdf = f.read() tm.load_urdf(robot_urdf, mesh_path=data_dir) tm.set_joint("joint2", 0.2 * np.pi) tm.set_joint("joint3", 0.2 * np.pi) tm.set_joint("joint5", 0.1 * np.pi) tm.set_joint("joint6", 0.5 * np.pi) tm.plot_visuals("robot_arm", ax_s=0.6, alpha=0.7) plt.show() ================================================ FILE: examples/plots/plot_rotate_cylinder.py ================================================ """ =============== Rotate Cylinder =============== In this example, we apply a constant torque (tau) to a cylinder at its center of gravity and plot it at several steps during the acceleration. """ import matplotlib.pyplot as plt import numpy as np from pytransform3d.plot_utils import plot_cylinder from pytransform3d.rotations import matrix_from_compact_axis_angle from pytransform3d.transformations import transform_from, plot_transform def inertia_of_cylinder(mass, length, radius): I_xx = I_yy = 0.25 * mass * radius**2 + 1.0 / 12.0 * mass * length**2 I_zz = 0.5 * mass * radius**2 return np.eye(3) * np.array([I_xx, I_yy, I_zz]) A2B = np.eye(4) length = 1.0 radius = 0.1 mass = 1.0 dt = 0.2 inertia = inertia_of_cylinder(mass, length, radius) tau = np.array([0.05, 0.05, 0.0]) angular_velocity = np.zeros(3) orientation = np.zeros(3) ax = None for p_xy in np.linspace(-2, 2, 21): A2B = transform_from( R=matrix_from_compact_axis_angle(orientation), p=np.array([p_xy, p_xy, 0.0]), ) ax = plot_cylinder( length=length, radius=radius, A2B=A2B, wireframe=False, alpha=0.2, ax_s=2.0, ax=ax, ) plot_transform(ax=ax, A2B=A2B, s=radius, lw=3) angular_acceleration = np.linalg.inv(inertia).dot(tau) angular_velocity += dt * angular_acceleration orientation += dt * angular_velocity ax.view_init(elev=30, azim=70) plt.show() ================================================ FILE: examples/plots/plot_sample_rotations.py ================================================ """ ================================= Compare Rotation Sampling Methods ================================= There are different ways of sampling rotations. We draw random samples of rotations, convert them to rotation matrices, and apply these to the basis vector (1, 0, 0) to obtain points on the unit sphere. """ import matplotlib.pyplot as plt import numpy as np import pytransform3d.batch_rotations as pbr import pytransform3d.plot_utils as ppu import pytransform3d.rotations as pr rng = np.random.default_rng(1223532) n_samples = 2000 v = np.array([1.0, 0.0, 0.0]) plt.figure(figsize=(9, 5)) ax1 = plt.subplot(121, projection="3d") ax1.set_title("Sampled rotation vectors (axis-angle)") ppu.plot_sphere(ax1, radius=1, n_steps=100) rotations = np.vstack( [pr.random_compact_axis_angle(rng) for _ in range(n_samples)] ) R = pbr.matrices_from_compact_axis_angles(rotations) v_R = np.einsum("nij,j->ni", R, v) ax1.scatter(v_R[:, 0], v_R[:, 1], v_R[:, 2]) ax2 = plt.subplot(122, projection="3d") ax2.set_title("Sampled quaternions") ppu.plot_sphere(ax2, radius=1, n_steps=100) rotations = np.vstack([pr.random_quaternion(rng) for _ in range(n_samples)]) R = pbr.matrices_from_quaternions(rotations) v_R = np.einsum("nij,j->ni", R, v) ax2.scatter(v_R[:, 0], v_R[:, 1], v_R[:, 2]) plt.tight_layout() plt.show() ================================================ FILE: examples/plots/plot_sample_transforms.py ================================================ """ ================= Sample Transforms ================= """ import matplotlib.pyplot as plt import numpy as np import pytransform3d.transformations as pt mean = pt.transform_from(R=np.eye(3), p=np.array([0.0, 0.0, 0.5])) cov = np.diag([0.001, 0.001, 0.5, 0.001, 0.001, 0.001]) rng = np.random.default_rng(0) ax = None poses = np.array( [pt.random_transform(rng=rng, mean=mean, cov=cov) for _ in range(1000)] ) for pose in poses: ax = pt.plot_transform(ax=ax, A2B=pose, s=0.3) plt.show() ================================================ FILE: examples/plots/plot_screw.py ================================================ """ ======================================== Plot Transformation through Screw Motion ======================================== A screw axis is represented by the parameters (q, s_axis, h). We can represent any transformation with a screw axis and an additional parameter theta that encodes the rotation angle and through h * theta the translation. Here we visualize a screw axis and the transformation generated from a specific theta. The larger coordinate frame represents the origin of the transformation and the smaller frame represents the transformed frame. The red point indicates the position of q, which is a point on the screw axis. A straight arrow shows the direction of the screw axis. The spiral path represents a displacement of length theta along the screw axis. """ import matplotlib.pyplot as plt import numpy as np from pytransform3d.rotations import active_matrix_from_extrinsic_roll_pitch_yaw from pytransform3d.transformations import ( plot_transform, plot_screw, screw_axis_from_screw_parameters, transform_from_exponential_coordinates, concat, transform_from, ) # Screw parameters q = np.array([-0.2, -0.1, -0.5]) s_axis = np.array([0, 0, 1]) h = 0.05 theta = 5.5 * np.pi Stheta = screw_axis_from_screw_parameters(q, s_axis, h) * theta A2B = transform_from_exponential_coordinates(Stheta) origin = transform_from( active_matrix_from_extrinsic_roll_pitch_yaw([0.5, -0.3, 0.2]), np.array([0.0, 0.1, 0.1]), ) ax = plot_transform(A2B=origin, s=0.4) plot_transform(ax=ax, A2B=concat(A2B, origin), s=0.2) plot_screw( ax=ax, q=q, s_axis=s_axis, h=h, theta=theta, A2B=origin, s=1.5, alpha=0.6 ) ax.view_init(elev=40, azim=170) plt.subplots_adjust(0, 0, 1, 1) plt.show() ================================================ FILE: examples/plots/plot_sphere.py ================================================ """ =========== Plot Sphere =========== """ import matplotlib.pyplot as plt import numpy as np from pytransform3d.plot_utils import plot_sphere, remove_frame from pytransform3d.transformations import plot_transform ax = plot_sphere(radius=0.5, wireframe=False, alpha=0.1, color="k", ax_s=0.5) plot_sphere(ax=ax, radius=0.5, wireframe=True) plot_transform(ax=ax, A2B=np.eye(4), s=0.3, lw=3) remove_frame(ax) plt.show() ================================================ FILE: examples/plots/plot_spheres.py ================================================ """ ===================== Plot Multiple Spheres ===================== Benchmarks plotting of multiple spheres at once and compares it to plotting each sphere individually. """ import time import matplotlib.pyplot as plt import numpy as np from pytransform3d.plot_utils import plot_sphere, plot_spheres n_spheres = 50 random_state = np.random.default_rng(0) P = 2 * random_state.random((n_spheres, 3)) - 1 radii = random_state.random(n_spheres) / 2 colors = random_state.random((n_spheres, 3)) alphas = random_state.random(n_spheres) start = time.time() plot_spheres(p=P, radius=radii, color=colors, alpha=alphas, wireframe=False) end = time.time() time_multi = end - start start = time.time() for p, radius, color, alpha in zip(P, radii, colors, alphas): plot_sphere(p=p, radius=radius, color=color, alpha=alpha, wireframe=False) end = time.time() time_single = end - start speedup = time_single / time_multi print("n_spheres", "single", "\t", "multi", "\t", "speedup", sep="\t") print(n_spheres, "", time_single, time_multi, speedup, sep="\t") plt.show() ================================================ FILE: examples/plots/plot_spherical_grid.py ================================================ """ ============== Spherical Grid ============== Plot a grid in spherical coordinates with rho = 1 as Cartesian points. We can see that the Cartesian distances between points are not regular and there are many points that were converted to the same Cartesian point at the poles of the sphere. """ import matplotlib.pyplot as plt import numpy as np import pytransform3d.coordinates as pc from pytransform3d.plot_utils import make_3d_axis thetas, phis = np.meshgrid( np.linspace(0, np.pi, 11), np.linspace(-np.pi, np.pi, 21) ) rhos = np.ones_like(thetas) spherical_grid = np.column_stack( (rhos.reshape(-1), thetas.reshape(-1), phis.reshape(-1)) ) cartesian_grid = pc.cartesian_from_spherical(spherical_grid) ax = make_3d_axis(ax_s=1, unit="m", n_ticks=6) ax.scatter(cartesian_grid[:, 0], cartesian_grid[:, 1], cartesian_grid[:, 2]) ax.plot(cartesian_grid[:, 0], cartesian_grid[:, 1], cartesian_grid[:, 2]) plt.show() ================================================ FILE: examples/plots/plot_straight_line_path.py ================================================ """ ======================== Plot Straight Line Paths ======================== We will compose a trajectory of multiple straight line paths in exponential coordinates. This is a demonstration of batch conversion from exponential coordinates to transformation matrices. """ import matplotlib.pyplot as plt import numpy as np from pytransform3d.plot_utils import Trajectory, make_3d_axis, remove_frame from pytransform3d.rotations import active_matrix_from_angle from pytransform3d.trajectories import transforms_from_exponential_coordinates from pytransform3d.transformations import ( exponential_coordinates_from_transform, transform_from, ) def time_scaling(t, t_max): """Linear time scaling.""" return np.asarray(t) / t_max def straight_line_path(start, goal, s): """Compute straight line path in exponential coordinates.""" start = np.asarray(start) goal = np.asarray(goal) return ( start[np.newaxis] * (1.0 - s)[:, np.newaxis] + goal[np.newaxis] * s[:, np.newaxis] ) s = time_scaling(np.linspace(0.0, 5.0, 50001), 5.0) start = transform_from( R=active_matrix_from_angle(1, -0.4 * np.pi), p=np.array([-1, -2.5, 0]) ) goal1 = transform_from( R=active_matrix_from_angle(1, -0.1 * np.pi), p=np.array([-1, 1, 0]) ) goal2 = transform_from( R=active_matrix_from_angle(2, -np.pi), p=np.array([-0.65, -0.75, 0]) ) path1 = straight_line_path( exponential_coordinates_from_transform(start), exponential_coordinates_from_transform(goal1), s, ) path2 = straight_line_path( exponential_coordinates_from_transform(goal1), exponential_coordinates_from_transform(goal2), s, ) H = transforms_from_exponential_coordinates(np.vstack((path1, path2))) ax = make_3d_axis(1.0) trajectory = Trajectory(H, n_frames=1000, show_direction=False, s=0.3) trajectory.add_trajectory(ax) ax.view_init(azim=-95, elev=70) ax.set_xlim((-2.2, 1.3)) ax.set_ylim((-2.5, 1)) remove_frame(ax) plt.show() ================================================ FILE: examples/plots/plot_transform.py ================================================ """ =================== Plot Transformation =================== We can display transformations by plotting the basis vectors of the corresponding coordinate frame. """ import matplotlib.pyplot as plt from pytransform3d.plot_utils import make_3d_axis from pytransform3d.transformations import plot_transform ax = make_3d_axis(ax_s=1, unit="m", n_ticks=6) plot_transform(ax=ax) plt.tight_layout() plt.show() ================================================ FILE: examples/plots/plot_transform_concatenation.py ================================================ """ ======================= Transform Concatenation ======================= In this example, we have a point p that is defined in a frame C, we know the transform C2B and B2A. We can construct a transform C2A to extract the position of p in frame A. """ import matplotlib.pyplot as plt import numpy as np import pytransform3d.rotations as pyrot import pytransform3d.transformations as pytr p = np.array([0.0, 0.0, -0.5]) a = np.array([0.0, 0.0, 1.0, np.pi]) B2A = pytr.transform_from(pyrot.matrix_from_axis_angle(a), p) p = np.array([0.3, 0.4, 0.5]) a = np.array([0.0, 0.0, 1.0, -np.pi / 2.0]) C2B = pytr.transform_from(pyrot.matrix_from_axis_angle(a), p) C2A = pytr.concat(C2B, B2A) p = pytr.transform(C2A, np.ones(4)) ax = pytr.plot_transform(A2B=B2A) pytr.plot_transform(ax, A2B=C2A) ax.scatter(p[0], p[1], p[2]) plt.show() ================================================ FILE: examples/plots/plot_transform_manager.py ================================================ """ ====================== Transformation Manager ====================== In this example, we will use the TransformManager to infer a transformation automatically. """ import matplotlib.pyplot as plt import numpy as np from pytransform3d import rotations as pr from pytransform3d import transformations as pt from pytransform3d.transform_manager import TransformManager rng = np.random.default_rng(1) ee2robot = pt.transform_from_pq( np.hstack((np.array([0.4, -0.3, 0.5]), pr.random_quaternion(rng))) ) cam2robot = pt.transform_from_pq( np.hstack((np.array([0.0, 0.0, 0.8]), pr.q_id)) ) object2cam = pt.transform_from( pr.active_matrix_from_intrinsic_euler_xyz(np.array([0.0, 0.0, -0.5])), np.array([0.5, 0.1, 0.1]), ) tm = TransformManager() tm.add_transform("end-effector", "robot", ee2robot) tm.add_transform("camera", "robot", cam2robot) tm.add_transform("object", "camera", object2cam) ee2object = tm.get_transform("end-effector", "object") ax = tm.plot_frames_in("robot", s=0.1) ax.set_xlim((-0.25, 0.75)) ax.set_ylim((-0.5, 0.5)) ax.set_zlim((0.0, 1.0)) plt.show() ================================================ FILE: examples/plots/plot_urdf.py ================================================ """ =========== URDF Joints =========== This example shows how to load a URDF description of a robot, set some joint angles and display relevant frames. """ import matplotlib.pyplot as plt from pytransform3d.urdf import UrdfTransformManager # %% # URDF # ---- # URDFs have to be provided as strings. These can be loaded from files. COMPI_URDF = """ """ tm = UrdfTransformManager() tm.load_urdf(COMPI_URDF) # %% # Set Joints # ---------- # In this example, we define joint angles manually. Joints are identified by # their string names extracted from the URDF. joint_names = ["joint%d" % i for i in range(1, 7)] joint_angles = [0, 0.5, 0.5, 0, 0.5, 0] for name, angle in zip(joint_names, joint_angles): tm.set_joint(name, angle) # %% # Visualization # ------------- # We use matplotlib to visualize the link frames of the URDF and connections # between these. ax = tm.plot_frames_in( "compi", whitelist=["link%d" % d for d in range(1, 7)], s=0.05, show_name=True, ) ax = tm.plot_connections_in("compi", ax=ax) ax.set_xlim((-0.2, 0.8)) ax.set_ylim((-0.5, 0.5)) ax.set_zlim((-0.2, 0.8)) plt.show() ================================================ FILE: examples/plots/plot_urdf_with_meshes.py ================================================ """ ================ URDF with Meshes ================ This example shows how to load a URDF with STL meshes. This example must be run from within the examples folder or the main folder because it uses a hard-coded path to the URDF file and the meshes. """ import os import matplotlib.pyplot as plt from pytransform3d.urdf import UrdfTransformManager BASE_DIR = "test/test_data/" data_dir = BASE_DIR search_path = "." while ( not os.path.exists(data_dir) and os.path.dirname(search_path) != "pytransform3d" ): search_path = os.path.join(search_path, "..") data_dir = os.path.join(search_path, BASE_DIR) tm = UrdfTransformManager() with open(os.path.join(data_dir, "simple_mechanism.urdf"), "r") as f: tm.load_urdf(f.read(), mesh_path=data_dir) tm.set_joint("joint", -1.1) ax = tm.plot_frames_in( "lower_cone", s=0.1, whitelist=["upper_cone", "lower_cone"], show_name=True ) ax = tm.plot_connections_in("lower_cone", ax=ax) tm.plot_visuals("lower_cone", ax=ax) ax.set_xlim((-0.1, 0.15)) ax.set_ylim((-0.1, 0.15)) ax.set_zlim((0.0, 0.25)) plt.show() ================================================ FILE: examples/plots/plot_vector.py ================================================ """ =========== Plot Vector =========== """ import matplotlib.pyplot as plt import numpy as np from pytransform3d.plot_utils import plot_vector plot_vector( # A vector is defined by start, direction, and s (scaling) start=np.array([-0.3, -0.2, -0.3]), direction=np.array([1.0, 1.0, 1.0]), s=0.5, ax_s=0.5, # Scaling of 3D axes lw=0, # Remove line around arrow color="orange", ) plt.show() ================================================ FILE: examples/visualizations/README.rst ================================================ 3D Visualizations ----------------- ================================================ FILE: examples/visualizations/vis_add_remove_artist.py ================================================ """ ===================== Add and Remove Artist ===================== A demonstration of how previously added artists can be removed. """ import os import pytransform3d.visualizer as pv from pytransform3d.urdf import UrdfTransformManager BASE_DIR = "test/test_data/" data_dir = BASE_DIR search_path = "." while ( not os.path.exists(data_dir) and os.path.dirname(search_path) != "pytransform3d" ): search_path = os.path.join(search_path, "..") data_dir = os.path.join(search_path, BASE_DIR) tm = UrdfTransformManager() with open(data_dir + "simple_mechanism.urdf", "r") as f: tm.load_urdf(f.read(), mesh_path=data_dir) tm.set_joint("joint", -1.1) fig = pv.figure() # add graph, box, and sphere graph = fig.plot_graph( tm, "lower_cone", s=0.1, show_frames=True, whitelist=["upper_cone", "lower_cone"], show_connections=True, show_visuals=True, show_name=False, ) box = fig.plot_box([1, 1, 1]) sphere = fig.plot_sphere(2) # remove graph and box fig.remove_artist(graph) fig.remove_artist(box) # add and remove box again box.add_artist(fig) fig.remove_artist(box) fig.view_init() fig.set_zoom(1.2) if "__file__" in globals(): fig.show() else: fig.save_image("__open3d_rendered_image.jpg") ================================================ FILE: examples/visualizations/vis_basis.py ================================================ """ ========================== Visualize Coordinate Frame ========================== """ import numpy as np import pytransform3d.visualizer as pv fig = pv.figure() fig.plot_basis(R=np.eye(3), p=[0.1, 0.2, 0.3]) fig.view_init(azim=15, elev=30) fig.set_zoom(1.0) if "__file__" in globals(): fig.show() else: fig.save_image("__open3d_rendered_image.jpg") ================================================ FILE: examples/visualizations/vis_box.py ================================================ """ ======== Plot Box ======== """ import numpy as np import pytransform3d.visualizer as pv from pytransform3d.rotations import random_axis_angle, matrix_from_axis_angle from pytransform3d.transformations import transform_from rng = np.random.default_rng(42) fig = pv.figure() A2B = transform_from( R=matrix_from_axis_angle(random_axis_angle(rng)), p=np.zeros(3) ) fig.plot_box(size=[0.2, 0.5, 1], A2B=A2B) fig.plot_transform(A2B=A2B) fig.view_init() if "__file__" in globals(): fig.show() else: fig.save_image("__open3d_rendered_image.jpg") ================================================ FILE: examples/visualizations/vis_camera_3d.py ================================================ """ =========================== Camera Representation in 3D =========================== This visualization is inspired by Blender's camera visualization. It will show the camera center, a virtual image plane at a desired distance to the camera center, and the top direction of the virtual image plane. """ import numpy as np import pytransform3d.transformations as pt import pytransform3d.visualizer as pv cam2world = pt.transform_from_pq([0, 0, 0, np.sqrt(0.5), -np.sqrt(0.5), 0, 0]) # default parameters of a camera in Blender sensor_size = np.array([0.036, 0.024]) intrinsic_matrix = np.array( [ [0.05, 0, sensor_size[0] / 2.0], [0, 0.05, sensor_size[1] / 2.0], [0, 0, 1], ] ) virtual_image_distance = 1 fig = pv.Figure() fig.plot_transform(A2B=cam2world, s=0.2) fig.plot_camera( cam2world=cam2world, M=intrinsic_matrix, sensor_size=sensor_size, virtual_image_distance=virtual_image_distance, ) if "__file__" in globals(): fig.show() else: fig.save_image("__open3d_rendered_image.jpg") ================================================ FILE: examples/visualizations/vis_capsule.py ================================================ """ ================= Visualize Capsule ================= """ import numpy as np import pytransform3d.visualizer as pv fig = pv.figure() fig.plot_capsule(height=0.5, radius=0.1, c=(1, 0, 0)) fig.plot_transform(A2B=np.eye(4)) fig.view_init() if "__file__" in globals(): fig.show() else: fig.save_image("__open3d_rendered_image.jpg") ================================================ FILE: examples/visualizations/vis_cone.py ================================================ """ ============== Visualize Cone ============== """ import numpy as np import pytransform3d.visualizer as pv fig = pv.figure() fig.plot_cone(height=1.0, radius=0.3, c=(0, 0, 0.5)) fig.plot_transform(A2B=np.eye(4)) fig.view_init() if "__file__" in globals(): fig.show() else: fig.save_image("__open3d_rendered_image.jpg") ================================================ FILE: examples/visualizations/vis_cylinder.py ================================================ """ =============================== Visualize Transformed Cylinders =============================== Plots transformed cylinders. """ import numpy as np import pytransform3d.visualizer as pv from pytransform3d.rotations import random_axis_angle, matrix_from_axis_angle from pytransform3d.transformations import transform_from fig = pv.figure() rng = np.random.default_rng(42) A2B = transform_from( R=matrix_from_axis_angle(random_axis_angle(rng)), p=rng.standard_normal(size=3), ) fig.plot_cylinder(length=1.0, radius=0.3) fig.plot_transform(A2B=np.eye(4)) fig.plot_cylinder(length=1.0, radius=0.3, A2B=A2B) fig.plot_transform(A2B=A2B) fig.view_init() fig.set_zoom(2) if "__file__" in globals(): fig.show() else: fig.save_image("__open3d_rendered_image.jpg") ================================================ FILE: examples/visualizations/vis_ee_wrench.py ================================================ """ ================ Visualize Wrench ================ We see a 6-DOF robot arm, and we assume that we have a force/torque sensor that measures the force of a spherical mass (gray sphere) at the tool center point (TCP). We can draw the screw representation of the wrench in the TCP frame as a force along a screw axis from the red sphere to the green sphere. Then we use the adjoint representation of the transformation from the base to the TCP to transform the wrench to the robot's base frame. This wrench has a force component and a torque component, which we can also visualize as a screw: the red sphere indicates the point q on the screw axis, the straight black line shows the screw axis, the red line indicates the direction to the initial configuration and the green line indicates the direction to the displaced configuration in which the instantaneous wrench would move the base. """ import os import numpy as np import pytransform3d.transformations as pt import pytransform3d.visualizer as pv from pytransform3d.urdf import UrdfTransformManager def plot_screw( figure, q=np.zeros(3), s_axis=np.array([1.0, 0.0, 0.0]), h=1.0, theta=1.0, A2B=None, s=1.0, ): """Plot transformation about and along screw axis. Parameters ---------- figure : Figure Interface to Open3D's visualizer q : array-like, shape (3,), optional (default: [0, 0, 0]) Vector to a point on the screw axis s_axis : array-like, shape (3,), optional (default: [1, 0, 0]) Direction vector of the screw axis h : float, optional (default: 1) Pitch of the screw. The pitch is the ratio of translation and rotation of the screw axis. Infinite pitch indicates pure translation. theta : float, optional (default: 1) Rotation angle. h * theta is the translation. A2B : array-like, shape (4, 4), optional (default: I) Origin of the screw s : float, optional (default: 1) Scaling of the axis and angle that will be drawn """ from pytransform3d.rotations import ( vector_projection, angle_between_vectors, perpendicular_to_vectors, slerp_weights, ) from pytransform3d.transformations import ( check_screw_parameters, transform, translate_transform, vector_to_point, vector_to_direction, vectors_to_points, ) if A2B is None: A2B = np.eye(4) q, s_axis, h = check_screw_parameters(q, s_axis, h) origin_projected_on_screw_axis = q + vector_projection(-q, s_axis) pure_translation = np.isinf(h) if not pure_translation: screw_axis_to_old_frame = -origin_projected_on_screw_axis screw_axis_to_rotated_frame = perpendicular_to_vectors( s_axis, screw_axis_to_old_frame ) screw_axis_to_translated_frame = h * s_axis arc = np.empty((100, 3)) angle = angle_between_vectors( screw_axis_to_old_frame, screw_axis_to_rotated_frame ) for i, t in enumerate( zip( np.linspace(0, 2 * theta / np.pi, len(arc)), np.linspace(0.0, 1.0, len(arc)), ) ): t1, t2 = t w1, w2 = slerp_weights(angle, t1) arc[i] = ( origin_projected_on_screw_axis + w1 * screw_axis_to_old_frame + w2 * screw_axis_to_rotated_frame + screw_axis_to_translated_frame * t2 * theta ) q = transform(A2B, vector_to_point(q))[:3] s_axis = transform(A2B, vector_to_direction(s_axis))[:3] if not pure_translation: arc = transform(A2B, vectors_to_points(arc))[:, :3] origin_projected_on_screw_axis = transform( A2B, vector_to_point(origin_projected_on_screw_axis) )[:3] # Screw axis Q = translate_transform(np.eye(4), q) fig.plot_sphere(radius=s * 0.02, A2B=Q, c=[1, 0, 0]) if pure_translation: s_axis *= theta Q_plus_S_axis = translate_transform(np.eye(4), q + s_axis) fig.plot_sphere(radius=s * 0.02, A2B=Q_plus_S_axis, c=[0, 1, 0]) P = np.array( [ [q[0] - s * s_axis[0], q[1] - s * s_axis[1], q[2] - s * s_axis[2]], [ q[0] + (1 + s) * s_axis[0], q[1] + (1 + s) * s_axis[1], q[2] + (1 + s) * s_axis[2], ], ] ) figure.plot(P=P, c=[0, 0, 0]) if not pure_translation: # Transformation figure.plot(arc, c=[0, 0, 0]) for i, c in zip([0, -1], [[1, 0, 0], [0, 1, 0]]): arc_bound = np.vstack((origin_projected_on_screw_axis, arc[i])) figure.plot(arc_bound, c=c) BASE_DIR = "test/test_data/" data_dir = BASE_DIR search_path = "." while ( not os.path.exists(data_dir) and os.path.dirname(search_path) != "pytransform3d" ): search_path = os.path.join(search_path, "..") data_dir = os.path.join(search_path, BASE_DIR) tm = UrdfTransformManager() filename = os.path.join(data_dir, "robot_with_visuals.urdf") with open(filename, "r") as f: robot_urdf = f.read() tm.load_urdf(robot_urdf, mesh_path=data_dir) tm.set_joint("joint2", 0.2 * np.pi) tm.set_joint("joint3", 0.2 * np.pi) tm.set_joint("joint5", 0.1 * np.pi) tm.set_joint("joint6", 0.5 * np.pi) ee2base = tm.get_transform("tcp", "robot_arm") base2ee = tm.get_transform("robot_arm", "tcp") mass = 1.0 wrench_in_ee = np.array([0.0, 0.0, 0.0, 0.0, -9.81, 0.0]) * mass wrench_in_base = np.dot(pt.adjoint_from_transform(base2ee).T, wrench_in_ee) fig = pv.figure() fig.plot_graph(tm, "robot_arm", s=0.1, show_visuals=True) fig.plot_transform(s=0.4) fig.plot_transform(A2B=ee2base, s=0.1) mass2base = np.copy(ee2base) mass2base[2, 3] += 0.075 fig.plot_sphere(radius=0.025, A2B=mass2base) S, theta = pt.screw_axis_from_exponential_coordinates(wrench_in_base) q, s, h = pt.screw_parameters_from_screw_axis(S) plot_screw(fig, q, s, h, theta * 0.05) S, theta = pt.screw_axis_from_exponential_coordinates(wrench_in_ee) q, s, h = pt.screw_parameters_from_screw_axis(S) plot_screw(fig, q, s, h, theta * 0.05, A2B=ee2base) fig.view_init() if "__file__" in globals(): fig.show() else: fig.save_image("__open3d_rendered_image.jpg") ================================================ FILE: examples/visualizations/vis_ellipsoid.py ================================================ """ =================== Visualize Ellipsoid =================== """ import numpy as np import pytransform3d.visualizer as pv fig = pv.figure() fig.plot_ellipsoid(radii=[0.2, 1, 0.5], c=(0.5, 0.5, 0)) fig.plot_transform(A2B=np.eye(4)) if "__file__" in globals(): fig.show() else: fig.save_image("__open3d_rendered_image.jpg") ================================================ FILE: examples/visualizations/vis_mesh.py ================================================ """ ============== Visualize Mesh ============== This example shows how to load an STL mesh. This example must be run from within the main folder because it uses a hard-coded path to the STL file. Press 'H' to print the viewer's help message to stdout. """ import os import numpy as np from pytransform3d import visualizer as pv BASE_DIR = "test/test_data/" data_dir = BASE_DIR search_path = "." while ( not os.path.exists(data_dir) and os.path.dirname(search_path) != "pytransform3d" ): search_path = os.path.join(search_path, "..") data_dir = os.path.join(search_path, BASE_DIR) fig = pv.figure() fig.plot_mesh(filename=os.path.join(data_dir, "scan.stl"), s=np.ones(3)) fig.plot_transform(A2B=np.eye(4), s=0.3) if "__file__" in globals(): fig.show() else: fig.save_image("__open3d_rendered_image.jpg") ================================================ FILE: examples/visualizations/vis_moving_basis.py ================================================ """ ================ Animate Rotation ================ Animates a rotation about the x-axis. """ import numpy as np import pytransform3d.visualizer as pv from pytransform3d import rotations as pr def animation_callback(step, n_frames, frame): angle = 2.0 * np.pi * (step + 1) / n_frames R = pr.passive_matrix_from_angle(0, angle) A2B = np.eye(4) A2B[:3, :3] = R frame.set_data(A2B) return frame fig = pv.figure(width=500, height=500) frame = fig.plot_basis(R=np.eye(3), s=0.5) fig.view_init() n_frames = 100 if "__file__" in globals(): fig.animate( animation_callback, n_frames, fargs=(n_frames, frame), loop=True ) fig.show() else: fig.save_image("__open3d_rendered_image.jpg") ================================================ FILE: examples/visualizations/vis_moving_cylinder_with_wrench.py ================================================ """ ============================== Visualize Cylinder with Wrench ============================== We apply a constant body-fixed wrench to a cylinder and integrate acceleration to twist and exponential coordinates of transformation to finally compute the new pose of the cylinder. """ import numpy as np import pytransform3d.visualizer as pv from pytransform3d.transformations import transform_from_exponential_coordinates def spatial_inertia_of_cylinder(mass, length, radius): I_xx = I_yy = 0.25 * mass * radius**2 + 1.0 / 12.0 * mass * length**2 I_zz = 0.5 * mass * radius**2 inertia = np.eye(6) inertia[:3, :3] *= np.array([I_xx, I_yy, I_zz]) inertia[3:, 3:] *= mass return inertia def animation_callback( step, cylinder, cylinder_frame, prev_cylinder2world, Stheta_dot, inertia_inv ): if step == 0: # Reset cylinder state prev_cylinder2world[:, :] = np.eye(4) Stheta_dot[:] = 0.0 # Apply constant wrench wrench_in_cylinder = np.array([0.1, 0.001, 0.001, 0.01, 1.0, 1.0]) dt = 0.0005 Stheta_ddot = np.dot(inertia_inv, wrench_in_cylinder) Stheta_dot += dt * Stheta_ddot cylinder2world = transform_from_exponential_coordinates( dt * Stheta_dot ).dot(prev_cylinder2world) # Update visualization cylinder_frame.set_data(cylinder2world) cylinder.set_data(cylinder2world) prev_cylinder2world[:, :] = cylinder2world return cylinder_frame, cylinder fig = pv.figure() # Definition of cylinder mass = 1.0 length = 0.5 radius = 0.1 inertia_inv = np.linalg.inv( spatial_inertia_of_cylinder(mass=mass, length=length, radius=radius) ) # State of cylinder cylinder2world = np.eye(4) twist = np.zeros(6) cylinder = fig.plot_cylinder(length=length, radius=radius, c=[1, 0.5, 0]) cylinder_frame = fig.plot_transform(A2B=cylinder2world, s=0.5) fig.plot_transform(A2B=np.eye(4), s=0.5) fig.view_init() if "__file__" in globals(): fig.animate( animation_callback, n_frames=10000, fargs=(cylinder, cylinder_frame, cylinder2world, twist, inertia_inv), loop=True, ) fig.show() else: fig.save_image("__open3d_rendered_image.jpg") ================================================ FILE: examples/visualizations/vis_moving_line.py ================================================ """ ============ Animate Line ============ Animates a line. """ import numpy as np import pytransform3d.visualizer as pv def animation_callback(step, n_frames, line): t = step / n_frames P = np.empty((100, 3)) for d in range(P.shape[1]): P[:, d] = np.linspace(0, 1 - t, len(P)) line.set_data(P) return line fig = pv.figure() P = np.zeros((100, 3)) colors = np.empty((99, 3)) for d in range(colors.shape[1]): P[:, d] = np.linspace(0, 1, len(P)) colors[:, d] = np.linspace(0, 1, len(colors)) line = fig.plot(P, colors) frame = fig.plot_basis(R=np.eye(3), s=0.5) fig.view_init() n_frames = 100 if "__file__" in globals(): fig.animate(animation_callback, n_frames, fargs=(n_frames, line), loop=True) fig.show() else: fig.save_image("__open3d_rendered_image.jpg") ================================================ FILE: examples/visualizations/vis_moving_robot.py ================================================ """ ============== Animated Robot ============== In this example we animate a 6-DOF robot arm with cylindrical visuals. """ import os import numpy as np import pytransform3d.visualizer as pv from pytransform3d.urdf import UrdfTransformManager def animation_callback(step, n_frames, tm, graph, joint_names): angle = 0.5 * np.cos(2.0 * np.pi * (step / n_frames)) for joint_name in joint_names: tm.set_joint(joint_name, angle) graph.set_data() return graph BASE_DIR = "test/test_data/" data_dir = BASE_DIR search_path = "." while ( not os.path.exists(data_dir) and os.path.dirname(search_path) != "pytransform3d" ): search_path = os.path.join(search_path, "..") data_dir = os.path.join(search_path, BASE_DIR) tm = UrdfTransformManager() filename = os.path.join(data_dir, "robot_with_visuals.urdf") with open(filename, "r") as f: robot_urdf = f.read() tm.load_urdf(robot_urdf, mesh_path=data_dir) joint_names = ["joint%d" % i for i in range(1, 7)] for joint_name in joint_names: tm.set_joint(joint_name, 0.5) fig = pv.figure() graph = fig.plot_graph( tm, "robot_arm", s=0.1, show_frames=True, show_visuals=True ) fig.view_init() fig.set_zoom(1.5) n_frames = 100 if "__file__" in globals(): fig.animate( animation_callback, n_frames, loop=True, fargs=(n_frames, tm, graph, joint_names), ) fig.show() else: fig.save_image("__open3d_rendered_image.jpg") ================================================ FILE: examples/visualizations/vis_moving_trajectory.py ================================================ """ ================== Animate Trajectory ================== Animates a trajectory. """ import numpy as np import pytransform3d.visualizer as pv from pytransform3d.rotations import passive_matrix_from_angle, R_id from pytransform3d.transformations import transform_from, concat def update_trajectory(step, n_frames, trajectory): progress = 1 - float(step + 1) / float(n_frames) H = np.zeros((100, 4, 4)) H0 = transform_from(R_id, np.zeros(3)) H_mod = np.eye(4) for i, t in enumerate(np.linspace(0, progress, len(H))): H0[:3, 3] = np.array([t, 0, t]) H_mod[:3, :3] = passive_matrix_from_angle(2, 8 * np.pi * t) H[i] = concat(H0, H_mod) trajectory.set_data(H) return trajectory n_frames = 200 fig = pv.figure() H = np.empty((100, 4, 4)) H[:] = np.eye(4) # set initial trajectory to extend view box H[:, 0, 3] = np.linspace(-2, 2, len(H)) H[:, 1, 3] = np.linspace(-2, 2, len(H)) H[:, 2, 3] = np.linspace(0, 4, len(H)) trajectory = pv.Trajectory(H, s=0.2, c=[0, 0, 0]) trajectory.add_artist(fig) fig.view_init() fig.set_zoom(0.5) if "__file__" in globals(): fig.animate( update_trajectory, n_frames, fargs=(n_frames, trajectory), loop=True ) fig.show() else: fig.save_image("__open3d_rendered_image.jpg") ================================================ FILE: examples/visualizations/vis_moving_urdf_with_meshes.py ================================================ """ ========================= Animated URDF with Meshes ========================= This example shows how to load a URDF with STL meshes and animate it. This example must be run from within the examples folder or the main folder because it uses a hard-coded path to the URDF file and the meshes. """ import os import numpy as np import pytransform3d.visualizer as pv from pytransform3d.urdf import UrdfTransformManager def animation_callback(step, n_frames, tm, graph): angle = 2.79253 * np.sin(2.0 * np.pi * (step / n_frames)) tm.set_joint("joint", angle) graph.set_data() return graph BASE_DIR = "test/test_data/" data_dir = BASE_DIR search_path = "." while ( not os.path.exists(data_dir) and os.path.dirname(search_path) != "pytransform3d" ): search_path = os.path.join(search_path, "..") data_dir = os.path.join(search_path, BASE_DIR) tm = UrdfTransformManager() with open(data_dir + "simple_mechanism.urdf", "r") as f: tm.load_urdf(f.read(), mesh_path=data_dir) fig = pv.figure("URDF with meshes") graph = fig.plot_graph( tm, "lower_cone", s=0.1, show_connections=True, show_visuals=True ) fig.view_init() fig.set_zoom(1.2) n_frames = 100 if "__file__" in globals(): fig.animate( animation_callback, n_frames, loop=True, fargs=(n_frames, tm, graph) ) fig.show() else: fig.save_image("__open3d_rendered_image.jpg") ================================================ FILE: examples/visualizations/vis_plane.py ================================================ """ =============== Visualize Plane =============== Visualizes one plane in Hesse normal form and one plane defined by point and normal. """ import numpy as np import pytransform3d.rotations as pr import pytransform3d.visualizer as pv fig = pv.figure() fig.plot_transform(np.eye(4)) rng = np.random.default_rng(8853) fig.plot_plane( pr.norm_vector(rng.standard_normal(3)), rng.standard_normal(), c=(1, 0.5, 0) ) fig.plot_plane( pr.norm_vector(rng.standard_normal(3)), point_in_plane=rng.standard_normal(3), c=(0, 1, 1), ) if "__file__" in globals(): fig.show() else: fig.save_image("__open3d_rendered_image.jpg") ================================================ FILE: examples/visualizations/vis_probabilistic_robot_kinematics.py ================================================ """ ===================================== Probabilistic Product of Exponentials ===================================== We compute the probabilistic forward kinematics of a robot with flexible links or joints and visualize the projected equiprobably ellipsoid of the end-effector's pose distribution. """ import os import numpy as np import open3d as o3d from matplotlib import cbook import pytransform3d.trajectories as ptr import pytransform3d.transformations as pt import pytransform3d.uncertainty as pu import pytransform3d.visualizer as pv from pytransform3d.urdf import UrdfTransformManager # %% # Probabilistic Robot Kinematics # ------------------------------ # # The end-effector's pose distribution is computed based on the Probabilistic # Product of Exponentials PPOE [1]_. # # Our ProbabilisticRobotKinematics class is a subclass of # :class:`~pytransform3d.urdf.UrdfTransformManager`, which loads a description # of a robot from the URDF format. # # The complicated part of this example is the conversion of kinematics # parameters from URDF data to screw axes that are needed for the product # of exponentials formulation of forward kinematics. # # Once we have this information, the implementation of the probabilistic # product of exponentials is straightforward: # # 1. We multiply the screw axis of each joint with the corresponding joint # angle to obtain the exponential coordinates of each relative joint # displacement. # 2. We concatenate the relative joint displacements and the base pose to # obtain the end-effector's pose. This is the original product of # exponentials. # 3. The PPOE modifies the original product of exponentials by transforming # and concatenating the covariances of each transformation. class ProbabilisticRobotKinematics(UrdfTransformManager): """Probabilistic robot kinematics. Parameters ---------- robot_urdf : str URDF description of robot ee_frame : str Name of the end-effector frame base_frame : str Name of the base frame joint_names : list Names of joints in order from base to end effector mesh_path : str, optional (default: None) Path in which we search for meshes that are defined in the URDF. Meshes will be ignored if it is set to None and no 'package_dir' is given. package_dir : str, optional (default: None) Some URDFs start file names with 'package://' to refer to the ROS package in which these files (textures, meshes) are located. This variable defines to which path this prefix will be resolved. """ def __init__( self, robot_urdf, ee_frame, base_frame, joint_names, mesh_path=None, package_dir=None, ): super(ProbabilisticRobotKinematics, self).__init__(check=False) self.load_urdf(robot_urdf, mesh_path=mesh_path, package_dir=package_dir) self.ee2base_home, self.screw_axes_home = self._get_screw_axes( ee_frame, base_frame, joint_names ) self.joint_limits = np.array( [self.get_joint_limits(jn) for jn in joint_names] ) def _get_screw_axes(self, ee_frame, base_frame, joint_names): """Get screw axes of joints in space frame at robot's home position. Parameters ---------- ee_frame : str Name of the end-effector frame base_frame : str Name of the base frame joint_names : list Names of joints in order from base to end effector Returns ------- ee2base_home : array, shape (4, 4) The home configuration (position and orientation) of the end-effector. screw_axes_home : array, shape (n_joints, 6) The joint screw axes in the space frame when the manipulator is at the home position. """ ee2base_home = self.get_transform(ee_frame, base_frame) screw_axes_home = [] for jn in joint_names: ln, _, _, s_axis, limits, joint_type = self._joints[jn] link2base = self.get_transform(ln, base_frame) s_axis = np.dot(link2base[:3, :3], s_axis) q = link2base[:3, 3] if joint_type == "revolute": h = 0.0 elif joint_type == "prismatic": h = np.inf else: raise NotImplementedError( "Joint type %s not supported." % joint_type ) screw_axis = pt.screw_axis_from_screw_parameters(q, s_axis, h) screw_axes_home.append(screw_axis) screw_axes_home = np.row_stack(screw_axes_home) return ee2base_home, screw_axes_home def probabilistic_forward_kinematics(self, thetas, covs): """Compute probabilistic forward kinematics. This is based on the probabilistic product of exponentials. Parameters ---------- thetas : array, shape (n_joints,) A list of joint coordinates. covs : array, shape (n_joints, 6, 6) Covariances of joint transformations. Returns ------- ee2base : array, shape (4, 4) A homogeneous transformation matrix representing the end-effector frame when the joints are at the specified coordinates. cov : array, shape (6, 6) Covariance of the pose in tangent space. """ assert len(thetas) == self.screw_axes_home.shape[0] thetas = np.clip( thetas, self.joint_limits[:, 0], self.joint_limits[:, 1] ) Sthetas = self.screw_axes_home * thetas[:, np.newaxis] joint_displacements = ptr.transforms_from_exponential_coordinates( Sthetas ) T = np.eye(4) cov = np.zeros((6, 6)) for i in range(len(thetas)): T, cov = pu.concat_locally_uncertain_transforms( joint_displacements[i], T, covs[i], cov ) T = T.dot(self.ee2base_home) ad = pt.adjoint_from_transform(self.ee2base_home) cov = ad.dot(cov).dot(ad.T) return T, cov # %% # Mesh Visualization # ------------------ # To visualize the 6D covariance in the tangent space of SE(3), we project its # equiprobable hyper-ellipsoid to 3D and represent it as a mesh. We can then # visualize the mesh with this class. class Surface(pv.Artist): """Surface to be visualized with Open3D. Parameters ---------- x : array, shape (n_steps, n_steps) Coordinates on x-axis of grid on surface. y : array, shape (n_steps, n_steps) Coordinates on y-axis of grid on surface. z : array, shape (n_steps, n_steps) Coordinates on z-axis of grid on surface. c : array-like, shape (3,), optional (default: None) Color """ def __init__(self, x, y, z, c=None): self.c = c self.mesh = o3d.geometry.TriangleMesh() self.set_data(x, y, z) def set_data(self, x, y, z): """Update data. Parameters ---------- x : array, shape (n_steps, n_steps) Coordinates on x-axis of grid on surface. y : array, shape (n_steps, n_steps) Coordinates on y-axis of grid on surface. z : array, shape (n_steps, n_steps) Coordinates on z-axis of grid on surface. """ polys = np.stack( [cbook._array_patch_perimeters(a, 1, 1) for a in (x, y, z)], axis=-1 ) vertices = polys.reshape(-1, 3) triangles = ( [[4 * i + 0, 4 * i + 1, 4 * i + 2] for i in range(len(polys))] + [[4 * i + 2, 4 * i + 3, 4 * i + 0] for i in range(len(polys))] + [[4 * i + 0, 4 * i + 3, 4 * i + 2] for i in range(len(polys))] + [[4 * i + 2, 4 * i + 1, 4 * i + 0] for i in range(len(polys))] ) self.mesh.vertices = o3d.utility.Vector3dVector(vertices) self.mesh.triangles = o3d.utility.Vector3iVector(triangles) if self.c is not None: self.mesh.paint_uniform_color(self.c) self.mesh.compute_vertex_normals() @property def geometries(self): """Expose geometries. Returns ------- geometries : list List of geometries that can be added to the visualizer. """ return [self.mesh] # %% # Then we define a callback to animate the visualization. def animation_callback( step, n_frames, tm, graph, joint_names, thetas, covs, surface ): angle = 0.5 * np.cos(2.0 * np.pi * (0.5 + step / n_frames)) thetas_t = angle * thetas for joint_name, value in zip(joint_names, thetas_t): tm.set_joint(joint_name, value) graph.set_data() T, cov = tm.probabilistic_forward_kinematics(thetas_t, covs) x, y, z = pu.to_projected_ellipsoid(T, cov, factor=1, n_steps=50) surface.set_data(x, y, z) return graph, surface # %% # Setup # ----- # We load the URDF file, BASE_DIR = "test/test_data/" data_dir = BASE_DIR search_path = "." while ( not os.path.exists(data_dir) and os.path.dirname(search_path) != "pytransform3d" ): search_path = os.path.join(search_path, "..") data_dir = os.path.join(search_path, BASE_DIR) filename = os.path.join(data_dir, "robot_with_visuals.urdf") with open(filename, "r") as f: robot_urdf = f.read() # %% # define the kinematic chain that we are interested in, joint_names = ["joint%d" % i for i in range(1, 7)] tm = ProbabilisticRobotKinematics( robot_urdf, "tcp", "linkmount", joint_names, mesh_path=data_dir ) # %% # define the joint angles, thetas = np.array([1, 1, 1, 0, 1, 0]) current_thetas = -0.5 * thetas for joint_name, theta in zip(joint_names, current_thetas): tm.set_joint(joint_name, theta) # %% # and define the covariances of the joints. covs = np.zeros((len(thetas), 6, 6)) covs[0] = np.diag([0, 0, 1, 0, 0, 0]) covs[1] = np.diag([0, 1, 0, 0, 0, 0]) covs[2] = np.diag([0, 1, 0, 0, 0, 0]) covs[4] = np.diag([0, 1, 0, 0, 0, 0]) covs *= 0.05 # %% # PPOE and Visualization # ---------------------- # # Then we can finally use PPOE to compute the end-effector pose and its # covariance. T, cov = tm.probabilistic_forward_kinematics(current_thetas, covs) # %% # We compute the 3D projection of the 6D covariance matrix. x, y, z = pu.to_projected_ellipsoid(T, cov, factor=1, n_steps=50) # %% # The following code visualizes the result. fig = pv.figure() graph = fig.plot_graph(tm, "robot_arm", show_visuals=True) fig.plot_transform(np.eye(4), s=0.3) surface = Surface(x, y, z, c=(0, 0.5, 0.5)) surface.add_artist(fig) fig.view_init(elev=5, azim=50) n_frames = 200 if "__file__" in globals(): fig.animate( animation_callback, n_frames, loop=True, fargs=(n_frames, tm, graph, joint_names, thetas, covs, surface), ) fig.show() else: fig.save_image("__open3d_rendered_image.jpg") # %% # References # ---------- # # .. [1] Meyer, Strobl, Triebel (2022): The Probabilistic Robot Kinematics # Model and its Application to Sensor Fusion. In IEEE/RSJ International # Conference on Intelligent Robots and Systems (IROS), Kyoto, Japan, 2022, # pp. 3263-3270, doi: 10.1109/IROS47612.2022.9981399. # https://elib.dlr.de/191928/1/202212_ELIB_PAPER_VERSION_with_copyright.pdf ================================================ FILE: examples/visualizations/vis_scatter.py ================================================ """ ============ Scatter Plot ============ Visualizes a point collection. """ import numpy as np import pytransform3d.visualizer as pv fig = pv.figure() rng = np.random.default_rng(41) P = rng.standard_normal(size=(100, 3)) colors = np.empty((100, 3)) for d in range(colors.shape[1]): colors[:, d] = np.linspace(0, 1, len(colors)) fig.scatter(P, c=colors) fig.plot_basis(R=np.eye(3), s=0.5) fig.view_init() if "__file__" in globals(): fig.show() else: fig.save_image("__open3d_rendered_image.jpg") ================================================ FILE: examples/visualizations/vis_sphere.py ================================================ """ ================ Visualize Sphere ================ """ import numpy as np import pytransform3d.visualizer as pv fig = pv.figure() fig.plot_sphere(radius=0.5) fig.plot_transform(A2B=np.eye(4)) if "__file__" in globals(): fig.show() else: fig.save_image("__open3d_rendered_image.jpg") ================================================ FILE: examples/visualizations/vis_urdf.py ================================================ """ =========== URDF Joints =========== This example shows how to load a URDF description of a robot, set some joint angles and display relevant frames. """ import pytransform3d.visualizer as pv from pytransform3d.urdf import UrdfTransformManager COMPI_URDF = """ """ tm = UrdfTransformManager() tm.load_urdf(COMPI_URDF) joint_names = ["joint%d" % i for i in range(1, 7)] joint_angles = [0, 0.5, 0.5, 0, 0.5, 0] for name, angle in zip(joint_names, joint_angles): tm.set_joint(name, angle) fig = pv.figure("URDF") fig.plot_graph( tm, "compi", show_frames=True, show_connections=True, whitelist=["link%d" % d for d in range(1, 7)], s=0.05, ) fig.view_init() if "__file__" in globals(): fig.show() else: fig.save_image("__open3d_rendered_image.jpg") ================================================ FILE: examples/visualizations/vis_urdf_with_meshes.py ================================================ """ ================ URDF with Meshes ================ This example shows how to load a URDF with STL meshes. This example must be run from within the examples folder or the main folder because it uses a hard-coded path to the URDF file and the meshes. """ import os import pytransform3d.visualizer as pv from pytransform3d.urdf import UrdfTransformManager BASE_DIR = "test/test_data/" data_dir = BASE_DIR search_path = "." while ( not os.path.exists(data_dir) and os.path.dirname(search_path) != "pytransform3d" ): search_path = os.path.join(search_path, "..") data_dir = os.path.join(search_path, BASE_DIR) tm = UrdfTransformManager() with open(data_dir + "simple_mechanism.urdf", "r") as f: tm.load_urdf(f.read(), mesh_path=data_dir) tm.set_joint("joint", -1.1) fig = pv.figure("URDF with meshes") fig.plot_graph( tm, "lower_cone", s=0.1, show_frames=True, whitelist=["upper_cone", "lower_cone"], show_connections=True, show_visuals=True, show_name=False, ) fig.view_init() fig.set_zoom(1.2) if "__file__" in globals(): fig.show() else: fig.save_image("__open3d_rendered_image.jpg") ================================================ FILE: examples/visualizations/vis_vector.py ================================================ """ ================ Visualize Vector ================ """ import numpy as np import pytransform3d.visualizer as pv fig = pv.figure() fig.plot_vector( start=np.array([0.0, 0.0, 0.0]), direction=np.array([1.0, 1.0, 0.0]), c=(1.0, 0.5, 0.0), ) fig.plot_transform(A2B=np.eye(4)) fig.view_init() if "__file__" in globals(): fig.show() else: fig.save_image("__open3d_rendered_image.jpg") ================================================ FILE: manifest.xml ================================================ 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. Alexander Fabisch/Alexander.Fabisch@dfki.de Alexander Fabisch/Alexander.Fabisch@dfki.de BSD-3-clause https://github.com/dfki-ric/pytransform3d https://raw.githubusercontent.com/dfki-ric/pytransform3d/main/doc/source/_static/logo.png Utilities 2 multi project active python ubuntu windows ================================================ FILE: paper/paper.bib ================================================ @ARTICLE{Gutzeit2018, AUTHOR={Gutzeit, Lisa and Fabisch, Alexander and Otto, Marc and Metzen, Jan Hendrik and Hansen, Jonas and Kirchner, Frank and Kirchner, Elsa Andrea}, TITLE={The BesMan Learning Platform for Automated Robot Skill Learning}, JOURNAL={Frontiers in Robotics and AI}, VOLUME={5}, PAGES={43}, YEAR={2018}, URL={https://www.frontiersin.org/article/10.3389/frobt.2018.00043}, DOI={10.3389/frobt.2018.00043}, ISSN={2296-9144} } @Misc{SciPyLectureNotes, title={Scipy Lecture Notes}, author={Ga{\"e}l Varoquaux and Emmanuelle Gouillart and Olav Vahtras}, url="https://www.scipy-lectures.org/", year=2018, note = {[Online; accessed <3 November 2018>]} } @ARTICLE{Walt2011, author={S. van der Walt and S. C. Colbert and G. Varoquaux}, journal={Computing in Science Engineering}, title={The NumPy Array: A Structure for Efficient Numerical Computation}, year={2011}, volume={13}, number={2}, pages={22-30}, doi={10.1109/MCSE.2011.37}, ISSN={1521-9615}, month={March}, } @Misc{Jones2001, author = {Eric Jones and Travis Oliphant and Pearu Peterson and others}, title = {{SciPy}: Open source scientific tools for {Python}}, year = {2001--}, url = "http://www.scipy.org/", note = {[Online; accessed <3 November 2018>]} } @Article{Hunter2007, Author = {Hunter, J. D.}, Title = {Matplotlib: A 2D graphics environment}, Journal = {Computing In Science \& Engineering}, Volume = {9}, Number = {3}, Pages = {90--95}, publisher = {IEEE COMPUTER SOC}, doi = {10.1109/MCSE.2007.55}, year = 2007 } @misc{Foote2009, author={Tully Foote and Eitan Marder-Eppstein and Wim Meeussen and others}, title={ROS tf2}, year={2009--}, url="http://wiki.ros.org/tf2", note = {[Online; accessed <4 November 2018>]} } @inproceedings{HidalgoCarrio2016, author={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}, title={EnviRe -- Environment Representation for Long-term Autonomy}, booktitle={AI for Long-term Autonomy Workshop on the International Conference on Robotics and Automation (ICRA)}, year=2016 } @misc{Brett2009, author = "Matthew Brett and Christoph Gohlke", title = "transforms3d", year = {2009--}, url = "https://github.com/matthew-brett/transforms3d", note = {[Online; accessed <4 November 2018>]} } @article{Ramasubramani2018, author={Vyas Ramasubramani and Sharon C. Glotzer}, title={rowan: A Python package for working with quaternions}, journal={Journal of Open Source Software}, year=2018, doi={10.21105/joss.00787}, url={https://doi.org/10.21105/joss.00787}, volume=3, issue=27 } ================================================ FILE: paper/paper.md ================================================ --- title: 'pytransform3d: 3D Transformations for Python' tags: - transformations authors: - name: Alexander Fabisch orcid: 0000-0003-2824-7956 affiliation: 1 affiliations: - name: Robotics Innovation Center, DFKI GmbH index: 1 date: 3 November 2018 bibliography: paper.bib --- # Summary pytransform3d is a Python library for transformations in three dimensions. Heterogenous software systems that consist of proprietary and open source software are often combined when we work with transformations. For example, suppose you want to transfer a trajectory demonstrated by a human to a robot. The human trajectory could be measured from an RGB-D camera, fused with inertial measurement units that are attached to the human, and then translated to joint angles by inverse kinematics. That involves at least three different software systems that might all use different conventions for transformations. Sometimes even one software uses more than one convention. The following aspects are of crucial importance to glue and debug transformations in systems with heterogenous and often incompatible software: * Compatibility: Compatibility between heterogenous softwares is a difficult topic. It might involve, for example, communicating between proprietary and open source software or different languages. * Conventions: Lots of different conventions are used for transformations in three dimensions. These have to be determined or specified. * Conversions: We need conversions between these conventions to communicate transformations between different systems. * Visualization: Finally, transformations should be visually verified and that should be as easy as possible. pytransform3d assists in solving these issues. Its documentation clearly states all of the used conventions, it makes conversions between rotation and transformation conventions as easy as possible, it is tightly coupled with Matplotlib to quickly visualize (or animate) transformations and it is written in Python with few dependencies. Python is a widely adopted language. It is used in many domains and supports a wide spectrum of communication to other software. 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. The library integrates well with the scientific Python ecosystem [@SciPyLectureNotes] with its core libraries Numpy, Scipy and Matplotlib. We rely on Numpy [@Walt2011] for linear algebra and on Matplotlib [@Hunter2007] to offer plotting functionalities. Scipy [@Jones2001] is used if you want to automatically compute new transformations from a graph of existing transformations. More advanced features of the library are the TransformManager which manages complex chains of transformations, the TransformEditor which allows to modify transformations graphically (additionally requires PyQt4), and the UrdfTransformManager which is able to load transformations from the Unified Robot Description Format (URDF) (additionally requires beautifulsoup4). ![Transformation Manager: The TransformManager builds a graph of transformations that can be used to automatically infer previously unknown transformations.](plot_transform_manager.png) ![Transformation Editor: The TransformEditor based on PyQt4 can be used to visually modify transformations with a minimal number dependencies.](app_transformation_editor.png) ![A simple URDF file loaded with pytransform3d and displayed in Matplotlib.](plot_urdf.png) One of the strengths of pytransform3d in comparison to most other libraries is its rigorous approach to testing. Unit tests have 100% branch coverage for code that is not related to visualization, there is more test code than library code, there are additional examples, and continuous integration runs with Python 2.7, 3.4, 3.5, and 3.6. The maintainer ensures that this level of quality will not be sacrificed for new features. There are several similar software packages. ROS tf2 [@Foote2009] is probably the most widely used of them. It offers functionality that is similar to the TransformManager of pytransform3d, but also considers temporal aspects of transformations. It also provides conversions between various conventions and visualization can be done with ROS' rviz package. EnviRe [@HidalgoCarrio2016] provides similar functionality. However, both libraries come with a number of dependencies and require complex build tools. Hence, it is not easy to set them up quickly in a new environment with minimum impact to the system, while pytransform3d is a lightweight library that only requires the basic scientific Python software stack that runs on any machine that supports CPython. There are other lightweight Python libraries that offer transformations and conversions between conventions, for example, transforms3d [@Brett2009] and rowan [@Ramasubramani2018], but these do not directly support visualization. ## Research pytransform3d is used in various domains, for example, specifying motions of a robot, learning robot movements from human demonstration, and sensor fusion for human pose estimation. pytransform3d has been used by @Gutzeit2018 in the context of learning robot behaviors from demonstration. # Acknowledgements We would like to thank Manuel Meder and Hendrik Wiese who have given valuable feedback as users to improve pytransform3d. This work was supported through two grants of the German Federal Ministry of Economics and Technology (BMWi, FKZ 50 RA 1216 and FKZ 50 RA 1217) and two grants from the European Union's Horizon 2020 research and innovation program (723853 and 730014). # References ================================================ FILE: pyproject.toml ================================================ [tool.black] line-length = 80 target-version = ["py38", "py39", "py310", "py311", "py312", "py313"] include = ''' /( pytransform3d | examples )\/.*\.pyi?$ ''' exclude = ''' /( .git | __pycache__ | doc | venv | build | dist )/ ''' [tool.ruff] line-length = 80 target-version = "py38" include = [ "pytransform3d/**/*.py", "examples//**/*.py", ] [tool.ruff.lint] select = [ # pycodestyle "E", # Pyflakes "F", # pyupgrade "UP", # flake8-bugbear "B", # flake8-simplify "SIM", # isort "I", ] # all rules can be found here: https://beta.ruff.rs/docs/rules/ ignore=[ "B008", # Do not perform function calls in argument defaults "I001", # isort found an import in the wrong position "SIM108", # Use ternary operator # old Python style, should be deprecated soon "UP004", # Class inherits from `object` "UP008", # Use `super()` instead of `super(__class__, self)` "UP015", # Unnecessary mode argument "UP031", # Use format specifiers instead of percent format "SIM118", # Use `key in dict` instead of `key in dict.keys()` ] [tool.ruff.format] quote-style = "double" ================================================ FILE: pytransform3d/__init__.py ================================================ """3D transformations for Python.""" __version__ = "3.15.0" ================================================ FILE: pytransform3d/_geometry.py ================================================ """Basic functionality for geometrical shapes.""" import numpy as np from .transformations import check_transform def unit_sphere_surface_grid(n_steps): """Create grid on the surface of a unit sphere in 3D. Parameters ---------- n_steps : int Number of discrete steps in each dimension of the surface. Returns ------- x : array, shape (n_steps, n_steps) x-coordinates of grid points. y : array, shape (n_steps, n_steps) y-coordinates of grid points. z : array, shape (n_steps, n_steps) z-coordinates of grid points. """ phi, theta = np.mgrid[ 0.0 : np.pi : n_steps * 1j, 0.0 : 2.0 * np.pi : n_steps * 1j ] sin_phi = np.sin(phi) x = sin_phi * np.cos(theta) y = sin_phi * np.sin(theta) z = np.cos(phi) return x, y, z def transform_surface(surface2origin, x, y, z): """Transform surface grid. Parameters ---------- surface2origin : array-like, shape (4, 4) Pose: transformation that will be applied to the surface grid. x : array, shape (n_steps, n_steps) x-coordinates of grid points. y : array, shape (n_steps, n_steps) y-coordinates of grid points. z : array, shape (n_steps, n_steps) z-coordinates of grid points. Returns ------- x : array, shape (n_steps, n_steps) x-coordinates of transformed grid points. y : array, shape (n_steps, n_steps) y-coordinates of transformed grid points. z : array, shape (n_steps, n_steps) z-coordinates of transformed grid points. """ surface2origin = check_transform(surface2origin) x = np.asarray(x) y = np.asarray(y) z = np.asarray(z) shape = x.shape P = np.column_stack((x.reshape(-1), y.reshape(-1), z.reshape(-1))) P = P.dot(surface2origin[:3, :3].T) + surface2origin[np.newaxis, :3, 3] x = P[:, 0].reshape(*shape) y = P[:, 1].reshape(*shape) z = P[:, 2].reshape(*shape) return x, y, z ================================================ FILE: pytransform3d/_geometry.pyi ================================================ from typing import Tuple import numpy as np import numpy.typing as npt def unit_sphere_surface_grid( n_steps: int, ) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: ... def transform_surface( pose: npt.ArrayLike, x: npt.ArrayLike, y: npt.ArrayLike, z: npt.ArrayLike ) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: ... ================================================ FILE: pytransform3d/_mesh_loader.py ================================================ """Common interface to load meshes.""" import abc import numpy as np def load_mesh(filename): """Load mesh from file. This feature relies on optional dependencies. It can use trimesh or Open3D to load meshes. If both are not available, it will fail. Furthermore, some mesh formats require additional dependencies. For example, loading collada files ('.dae' file ending) requires pycollada and trimesh. Parameters ---------- filename : str File in which the mesh is stored. Returns ------- mesh : MeshBase Mesh instance. """ mesh_loaded = False # since trimesh does not support color for STL files, we try Open3D first if filename.endswith(".stl"): mesh = _Open3DMesh(filename) mesh_loaded = mesh.load() # trimesh is usually better for other formats if not mesh_loaded: # pragma: no cover mesh = _Trimesh(filename) mesh_loaded = mesh.load() if not mesh_loaded: # pragma: no cover mesh = _Open3DMesh(filename) mesh_loaded = mesh.load() if not mesh_loaded: # pragma: no cover raise ImportError( "Could not load mesh from '%s'. Please install one of the " "optional dependencies 'trimesh' or 'open3d'." % filename ) return mesh class MeshBase(abc.ABC): """Abstract base class of meshes. Parameters ---------- filename : str File in which the mesh is stored. """ def __init__(self, filename): self.filename = filename @abc.abstractmethod def load(self): """Load mesh from file. Returns ------- loader_available : bool Is the mesh loader available? """ @abc.abstractmethod def convex_hull(self): """Compute convex hull of mesh.""" @abc.abstractmethod def get_open3d_mesh(self): """Return Open3D mesh. Returns ------- mesh : open3d.geometry.TriangleMesh Open3D mesh. """ @property @abc.abstractmethod def vertices(self): """Vertices.""" @property @abc.abstractmethod def triangles(self): """Triangles.""" class _Trimesh(MeshBase): def __init__(self, filename): super(_Trimesh, self).__init__(filename) self.mesh = None def load(self): try: import trimesh except ImportError as e: # pragma: no cover if e.name == "trimesh": return False else: raise e self.mesh = trimesh.load_mesh(self.filename) if isinstance(self.mesh, trimesh.Scene): open3d_mesh = self._scene_to_open3d_mesh(self.mesh) self.mesh = self._open3d_mesh_to_trimesh(open3d_mesh) return True def _open3d_mesh_to_trimesh(self, open3d_mesh): # pragma: no cover import trimesh if len(open3d_mesh.vertex_colors) == 0: vertex_colors = None else: vertex_colors = open3d_mesh.vertex_colors * 255.0 return trimesh.Trimesh( vertices=np.asarray(open3d_mesh.vertices), faces=np.asarray(open3d_mesh.triangles), vertex_colors=vertex_colors, ) def convex_hull(self): self.mesh = self.mesh.convex_hull def get_open3d_mesh(self): # pragma: no cover return self._trimesh_to_open3d_mesh(self.mesh) def _scene_to_open3d_mesh(self, scene): # pragma: no cover import open3d import trimesh mesh = open3d.geometry.TriangleMesh() for d in scene.dump(): if isinstance(d, trimesh.Trimesh): mesh += self._trimesh_to_open3d_mesh(d) return mesh def _trimesh_to_open3d_mesh(self, tri_mesh): # pragma: no cover import open3d import trimesh mesh = open3d.geometry.TriangleMesh( open3d.utility.Vector3dVector(tri_mesh.vertices), open3d.utility.Vector3iVector(tri_mesh.faces), ) if isinstance(tri_mesh.visual, trimesh.visual.ColorVisuals): mesh.vertex_colors = open3d.utility.Vector3dVector( tri_mesh.visual.vertex_colors[:, :3] / 255.0 ) return mesh @property def vertices(self): return self.mesh.vertices @property def triangles(self): return self.mesh.faces class _Open3DMesh(MeshBase): # pragma: no cover def __init__(self, filename): super(_Open3DMesh, self).__init__(filename) self.mesh = None def load(self): try: import open3d except ImportError as e: if e.name == "open3d": return False else: raise e self.mesh = open3d.io.read_triangle_mesh(self.filename) return True def convex_hull(self): assert self.mesh is not None self.mesh = self.mesh.compute_convex_hull()[0] def get_open3d_mesh(self): return self.mesh @property def vertices(self): return np.asarray(self.mesh.vertices) @property def triangles(self): return np.asarray(self.mesh.triangles) ================================================ FILE: pytransform3d/_mesh_loader.pyi ================================================ import abc from typing import Any import numpy.typing as npt class MeshBase(abc.ABC): filename: str def __init__(self, filename: str): ... @abc.abstractmethod def load(self) -> bool: ... @abc.abstractmethod def convex_hull(self): ... @abc.abstractmethod def get_open3d_mesh(self) -> Any: ... @property @abc.abstractmethod def vertices(self) -> npt.ArrayLike: ... @property @abc.abstractmethod def triangles(self) -> npt.ArrayLike: ... def load_mesh(filename: str) -> MeshBase: ... ================================================ FILE: pytransform3d/batch_rotations/__init__.py ================================================ """Batch operations on rotations in three dimensions - SO(3). Conversions from this module operate on batches of orientations or rotations and can be orders of magnitude faster than a loop of individual conversions. All functions operate on nd arrays, where the last dimension (vectors) or the last two dimensions (matrices) contain individual rotations. """ from ._angle import ( active_matrices_from_angles, ) from ._axis_angle import ( norm_axis_angles, matrices_from_compact_axis_angles, ) from ._euler import ( active_matrices_from_intrinsic_euler_angles, active_matrices_from_extrinsic_euler_angles, ) from ._matrix import ( axis_angles_from_matrices, quaternions_from_matrices, ) from ._quaternion import ( smooth_quaternion_trajectory, batch_concatenate_quaternions, batch_q_conj, quaternion_slerp_batch, axis_angles_from_quaternions, matrices_from_quaternions, batch_quaternion_wxyz_from_xyzw, batch_quaternion_xyzw_from_wxyz, ) from ._utils import ( norm_vectors, angles_between_vectors, cross_product_matrices, ) __all__ = [ "norm_vectors", "norm_axis_angles", "angles_between_vectors", "cross_product_matrices", "active_matrices_from_angles", "active_matrices_from_intrinsic_euler_angles", "active_matrices_from_extrinsic_euler_angles", "matrices_from_compact_axis_angles", "axis_angles_from_matrices", "quaternions_from_matrices", "smooth_quaternion_trajectory", "batch_concatenate_quaternions", "batch_q_conj", "quaternion_slerp_batch", "axis_angles_from_quaternions", "matrices_from_quaternions", "batch_quaternion_xyzw_from_wxyz", "batch_quaternion_wxyz_from_xyzw", ] ================================================ FILE: pytransform3d/batch_rotations/_angle.py ================================================ """Batch operations for angles.""" import numpy as np def active_matrices_from_angles(basis, angles, out=None): """Compute active rotation matrices from rotation about basis vectors. Parameters ---------- basis : int from [0, 1, 2] The rotation axis (0: x, 1: y, 2: z) angles : array-like, shape (...) Rotation angles out : array, shape (..., 3, 3), optional (default: new array) Output array to which we write the result Returns ------- Rs : array, shape (..., 3, 3) Rotation matrices """ angles = np.asarray(angles) c = np.cos(angles) s = np.sin(angles) R_shape = angles.shape + (3, 3) if out is None: out = np.empty(R_shape) out[..., basis, :] = 0.0 out[..., :, basis] = 0.0 out[..., basis, basis] = 1.0 basisp1 = (basis + 1) % 3 basisp2 = (basis + 2) % 3 out[..., basisp1, basisp1] = c out[..., basisp2, basisp2] = c out[..., basisp1, basisp2] = -s out[..., basisp2, basisp1] = s return out ================================================ FILE: pytransform3d/batch_rotations/_angle.pyi ================================================ import numpy as np import numpy.typing as npt from typing import Union def active_matrices_from_angles( basis: int, angles: npt.ArrayLike, out: Union[npt.ArrayLike, None] = ... ) -> np.ndarray: ... ================================================ FILE: pytransform3d/batch_rotations/_axis_angle.py ================================================ """Batch operations for axis-angle representation.""" import numpy as np from ..rotations import norm_angle from ._utils import norm_vectors def norm_axis_angles(a): """Normalize axis-angle representation. Parameters ---------- a : array-like, shape (..., 4) Axis of rotation and rotation angle: (x, y, z, angle) Returns ------- a : array, shape (..., 4) Axis of rotation and rotation angle: (x, y, z, angle). The length of the axis vector is 1 and the angle is in [0, pi). No rotation is represented by [1, 0, 0, 0]. """ a = np.asarray(a) # Handle the case of only one axis-angle instance only_one = a.ndim == 1 a = np.atleast_2d(a) angles = a[..., 3] norm = np.linalg.norm(a[..., :3], axis=-1) no_rot_mask = (angles == 0.0) | (norm == 0.0) rot_mask = ~no_rot_mask res = np.empty_like(a) res[no_rot_mask, :] = np.array([1.0, 0.0, 0.0, 0.0]) res[rot_mask, :3] = a[rot_mask, :3] / norm[rot_mask, np.newaxis] angle_normalized = norm_angle(angles) negative_angle_mask = angle_normalized < 0.0 res[negative_angle_mask, :3] *= -1.0 angle_normalized[negative_angle_mask] *= -1.0 res[rot_mask, 3] = angle_normalized[rot_mask] if only_one: res = res[0] return res def matrices_from_compact_axis_angles(A=None, axes=None, angles=None, out=None): """Compute rotation matrices from compact axis-angle representations. This is called exponential map or Rodrigues' formula. This typically results in an active rotation matrix. Parameters ---------- A : array-like, shape (..., 3) Axes of rotation and rotation angles in compact representation: angle * (x, y, z) axes : array, shape (..., 3) If the unit axes of rotation have been precomputed, you can pass them here. angles : array, shape (...) If the angles have been precomputed, you can pass them here. out : array, shape (..., 3, 3), optional (default: new array) Output array to which we write the result Returns ------- Rs : array, shape (..., 3, 3) Rotation matrices """ if angles is None: thetas = np.linalg.norm(A, axis=-1) else: thetas = np.asarray(angles) if axes is None: omega_unit = norm_vectors(A) else: omega_unit = axes c = np.cos(thetas) s = np.sin(thetas) ci = 1.0 - c ux = omega_unit[..., 0] uy = omega_unit[..., 1] uz = omega_unit[..., 2] uxs = ux * s uys = uy * s uzs = uz * s ciux = ci * ux ciuy = ci * uy ciuxuy = ciux * uy ciuxuz = ciux * uz ciuyuz = ciuy * uz if out is None: out = np.empty(A.shape[:-1] + (3, 3)) out[..., 0, 0] = ciux * ux + c out[..., 0, 1] = ciuxuy - uzs out[..., 0, 2] = ciuxuz + uys out[..., 1, 0] = ciuxuy + uzs out[..., 1, 1] = ciuy * uy + c out[..., 1, 2] = ciuyuz - uxs out[..., 2, 0] = ciuxuz - uys out[..., 2, 1] = ciuyuz + uxs out[..., 2, 2] = ci * uz * uz + c return out ================================================ FILE: pytransform3d/batch_rotations/_axis_angle.pyi ================================================ import numpy as np import numpy.typing as npt from typing import Union def norm_axis_angles(a: npt.ArrayLike) -> np.ndarray: ... def matrices_from_compact_axis_angles( A: Union[npt.ArrayLike, None] = ..., axes: Union[np.ndarray, None] = ..., angles: Union[np.ndarray, None] = ..., out: Union[np.ndarray, None] = ..., ) -> np.ndarray: ... ================================================ FILE: pytransform3d/batch_rotations/_euler.py ================================================ """Euler angles.""" import numpy as np from ._angle import active_matrices_from_angles def active_matrices_from_intrinsic_euler_angles( basis1, basis2, basis3, e, out=None ): """Compute active rotation matrices from intrinsic Euler angles. Parameters ---------- basis1 : int Basis vector of first rotation. 0 corresponds to x axis, 1 to y axis, and 2 to z axis. basis2 : int Basis vector of second rotation. 0 corresponds to x axis, 1 to y axis, and 2 to z axis. basis3 : int Basis vector of third rotation. 0 corresponds to x axis, 1 to y axis, and 2 to z axis. e : array-like, shape (..., 3) Euler angles out : array, shape (..., 3, 3), optional (default: new array) Output array to which we write the result Returns ------- Rs : array, shape (..., 3, 3) Rotation matrices """ e = np.asarray(e) R_shape = e.shape + (3,) R_alpha = active_matrices_from_angles(basis1, e[..., 0].flat) R_beta = active_matrices_from_angles(basis2, e[..., 1].flat) R_gamma = active_matrices_from_angles(basis3, e[..., 2].flat) if out is None: out = np.empty(R_shape) out[:] = np.einsum( "nij,njk->nik", np.einsum("nij,njk->nik", R_alpha, R_beta), R_gamma ).reshape(R_shape) return out def active_matrices_from_extrinsic_euler_angles( basis1, basis2, basis3, e, out=None ): """Compute active rotation matrices from extrinsic Euler angles. Parameters ---------- basis1 : int Basis vector of first rotation. 0 corresponds to x axis, 1 to y axis, and 2 to z axis. basis2 : int Basis vector of second rotation. 0 corresponds to x axis, 1 to y axis, and 2 to z axis. basis3 : int Basis vector of third rotation. 0 corresponds to x axis, 1 to y axis, and 2 to z axis. e : array-like, shape (..., 3) Euler angles out : array, shape (..., 3, 3), optional (default: new array) Output array to which we write the result Returns ------- Rs : array, shape (..., 3, 3) Rotation matrices """ e = np.asarray(e) R_shape = e.shape + (3,) R_alpha = active_matrices_from_angles(basis1, e[..., 0].flat) R_beta = active_matrices_from_angles(basis2, e[..., 1].flat) R_gamma = active_matrices_from_angles(basis3, e[..., 2].flat) if out is None: out = np.empty(R_shape) out[:] = np.einsum( "nij,njk->nik", np.einsum("nij,njk->nik", R_gamma, R_beta), R_alpha ).reshape(R_shape) return out ================================================ FILE: pytransform3d/batch_rotations/_euler.pyi ================================================ import numpy as np import numpy.typing as npt from typing import Union def active_matrices_from_intrinsic_euler_angles( basis1: int, basis2: int, basis3: int, e: npt.ArrayLike, out: Union[npt.ArrayLike, None] = ..., ) -> np.ndarray: ... def active_matrices_from_extrinsic_euler_angles( basis1: int, basis2: int, basis3: int, e: npt.ArrayLike, out: Union[npt.ArrayLike, None] = ..., ) -> np.ndarray: ... ================================================ FILE: pytransform3d/batch_rotations/_matrix.py ================================================ """Batch operations for rotation matrices.""" import numpy as np def axis_angles_from_matrices(Rs, traces=None, out=None): """Compute compact axis-angle representations from rotation matrices. This is called logarithmic map. Parameters ---------- Rs : array-like, shape (..., 3, 3) Rotation matrices traces : array, shape (..., 3) If the traces of rotation matrices been precomputed, you can pass them here. out : array, shape (..., 4), optional (default: new array) Output array to which we write the result Returns ------- A : array, shape (..., 4) Axes of rotation and rotation angles: (x, y, z, angle) """ Rs = np.asarray(Rs) instances_shape = Rs.shape[:-2] if traces is None: traces = np.einsum("nii", Rs.reshape(-1, 3, 3)) if instances_shape: traces = traces.reshape(*instances_shape) else: # this works because indX will be a single boolean and # out[True, n] = value will assign value to out[n], while # out[False, n] = value will not assign value to out[n] traces = traces[0] angles = np.arccos(np.clip((traces - 1.0) / 2.0, -1.0, 1.0)) if out is None: out = np.empty(instances_shape + (4,)) out[..., 0] = Rs[..., 2, 1] - Rs[..., 1, 2] out[..., 1] = Rs[..., 0, 2] - Rs[..., 2, 0] out[..., 2] = Rs[..., 1, 0] - Rs[..., 0, 1] # The threshold is a result from this discussion: # https://github.com/dfki-ric/pytransform3d/issues/43 # The standard formula becomes numerically unstable, however, # Rodrigues' formula reduces to R = I + 2 (ee^T - I), with the # rotation axis e, that is, ee^T = 0.5 * (R + I) and we can find the # squared values of the rotation axis on the diagonal of this matrix. # We can still use the original formula to reconstruct the signs of # the rotation axis correctly. angle_close_to_pi = np.abs(angles - np.pi) < 1e-4 angle_zero = angles == 0.0 angle_not_zero = np.logical_not(angle_zero) Rs_diag = np.einsum("nii->ni", Rs.reshape(-1, 3, 3)) if instances_shape: Rs_diag = Rs_diag.reshape(*(instances_shape + (3,))) else: Rs_diag = Rs_diag[0] out[angle_close_to_pi, :3] = np.sqrt( 0.5 * (Rs_diag[angle_close_to_pi] + 1.0) ) * np.sign(out[angle_close_to_pi, :3]) out[angle_not_zero, :3] /= np.linalg.norm(out[angle_not_zero, :3], axis=-1)[ ..., np.newaxis ] out[angle_zero, 0] = 1.0 out[angle_zero, 1:3] = 0.0 out[..., 3] = angles return out def quaternions_from_matrices(Rs, out=None): """Compute quaternions from rotation matrices. Parameters ---------- Rs : array-like, shape (..., 3, 3) Rotation matrices out : array, shape (..., 4), optional (default: new array) Output array to which we write the result Returns ------- Q : array, shape (..., 4) Unit quaternions to represent rotations: (w, x, y, z) """ Rs = np.asarray(Rs) instances_shape = Rs.shape[:-2] if out is None: out = np.empty(instances_shape + (4,)) traces = np.einsum("nii", Rs.reshape(-1, 3, 3)) if instances_shape: traces = traces.reshape(*instances_shape) else: # this works because indX will be a single boolean and # out[True, n] = value will assign value to out[n], while # out[False, n] = value will not assign value to out[n] traces = traces[0] ind1 = traces > 0.0 s = 2.0 * np.sqrt(1.0 + traces[ind1]) out[ind1, 0] = 0.25 * s out[ind1, 1] = (Rs[ind1, 2, 1] - Rs[ind1, 1, 2]) / s out[ind1, 2] = (Rs[ind1, 0, 2] - Rs[ind1, 2, 0]) / s out[ind1, 3] = (Rs[ind1, 1, 0] - Rs[ind1, 0, 1]) / s ind2 = np.logical_and( np.logical_not(ind1), np.logical_and( Rs[..., 0, 0] > Rs[..., 1, 1], Rs[..., 0, 0] > Rs[..., 2, 2] ), ) s = 2.0 * np.sqrt(1.0 + Rs[ind2, 0, 0] - Rs[ind2, 1, 1] - Rs[ind2, 2, 2]) out[ind2, 0] = (Rs[ind2, 2, 1] - Rs[ind2, 1, 2]) / s out[ind2, 1] = 0.25 * s out[ind2, 2] = (Rs[ind2, 1, 0] + Rs[ind2, 0, 1]) / s out[ind2, 3] = (Rs[ind2, 0, 2] + Rs[ind2, 2, 0]) / s ind3 = np.logical_and(np.logical_not(ind1), Rs[..., 1, 1] > Rs[..., 2, 2]) s = 2.0 * np.sqrt(1.0 + Rs[ind3, 1, 1] - Rs[ind3, 0, 0] - Rs[ind3, 2, 2]) out[ind3, 0] = (Rs[ind3, 0, 2] - Rs[ind3, 2, 0]) / s out[ind3, 1] = (Rs[ind3, 1, 0] + Rs[ind3, 0, 1]) / s out[ind3, 2] = 0.25 * s out[ind3, 3] = (Rs[ind3, 2, 1] + Rs[ind3, 1, 2]) / s ind4 = np.logical_and( np.logical_and(np.logical_not(ind1), np.logical_not(ind2)), np.logical_not(ind3), ) s = 2.0 * np.sqrt(1.0 + Rs[ind4, 2, 2] - Rs[ind4, 0, 0] - Rs[ind4, 1, 1]) out[ind4, 0] = (Rs[ind4, 1, 0] - Rs[ind4, 0, 1]) / s out[ind4, 1] = (Rs[ind4, 0, 2] + Rs[ind4, 2, 0]) / s out[ind4, 2] = (Rs[ind4, 2, 1] + Rs[ind4, 1, 2]) / s out[ind4, 3] = 0.25 * s return out ================================================ FILE: pytransform3d/batch_rotations/_matrix.pyi ================================================ import numpy as np import numpy.typing as npt from typing import Union def axis_angles_from_matrices( Rs: npt.ArrayLike, traces: Union[np.ndarray, None] = ..., out: Union[np.ndarray, None] = ..., ) -> np.ndarray: ... def quaternions_from_matrices( Rs: npt.ArrayLike, out: Union[np.ndarray, None] = ... ) -> np.ndarray: ... ================================================ FILE: pytransform3d/batch_rotations/_quaternion.py ================================================ """Batch operations for quaternions.""" import numpy as np from ._axis_angle import norm_axis_angles from ._utils import norm_vectors from ..rotations import ( angle_between_vectors, slerp_weights, pick_closest_quaternion, ) def smooth_quaternion_trajectory(Q, start_component_positive="x"): """Smooth quaternion trajectory. Quaternion q and -q represent the same rotation but cannot be interpolated well. This function guarantees that two successive quaternions q1 and q2 are closer than q1 and -q2. Parameters ---------- Q : array-like, shape (n_steps, 4) Unit quaternions to represent rotations: (w, x, y, z) start_component_positive : str, optional (default: 'x') Start trajectory with quaternion that has this component positive. Allowed values: 'w' (scalar), 'x', 'y', and 'z'. Returns ------- Q : array, shape (n_steps, 4) Unit quaternions to represent rotations: (w, x, y, z) Raises ------ ValueError If Q has length 0. """ Q = np.copy(Q) if len(Q) == 0: raise ValueError("At least one quaternion is expected.") if Q[0, "wxyz".index(start_component_positive)] < 0.0: Q[0] *= -1.0 q1q2_dists = np.linalg.norm(Q[:-1] - Q[1:], axis=1) q1mq2_dists = np.linalg.norm(Q[:-1] + Q[1:], axis=1) before_jump_indices = np.where(q1q2_dists > q1mq2_dists)[0] # workaround for interpolation artifacts: before_smooth_jump_indices = np.isclose(q1q2_dists, q1mq2_dists) before_smooth_jump_indices = np.where( np.logical_and( before_smooth_jump_indices[:-1], before_smooth_jump_indices[1:] ) )[0] before_jump_indices = np.unique( np.hstack((before_jump_indices, before_smooth_jump_indices)) ).tolist() before_jump_indices.append(len(Q) - 1) slices_to_correct = np.array( list(zip(before_jump_indices[:-1], before_jump_indices[1:])) )[::2] for i, j in slices_to_correct: Q[i + 1 : j + 1] *= -1.0 return Q def batch_concatenate_quaternions(Q1, Q2, out=None): """Concatenate two batches of quaternions. We use Hamilton's quaternion multiplication. Suppose we want to apply two extrinsic rotations given by quaternions q1 and q2 to a vector v. We can either apply q2 to v and then q1 to the result or we can concatenate q1 and q2 and apply the result to v. Parameters ---------- Q1 : array-like, shape (..., 4) First batch of quaternions Q2 : array-like, shape (..., 4) Second batch of quaternions out : array, shape (..., 4), optional (default: new array) Output array to which we write the result Returns ------- Q12 : array, shape (..., 4) Batch of quaternions that represents the concatenated rotations Raises ------ ValueError If the input dimensions are incorrect """ Q1 = np.asarray(Q1) Q2 = np.asarray(Q2) if Q1.ndim != Q2.ndim: raise ValueError( "Number of dimensions must be the same. " "Got %d for Q1 and %d for Q2." % (Q1.ndim, Q2.ndim) ) for d in range(Q1.ndim - 1): if Q1.shape[d] != Q2.shape[d]: raise ValueError( "Size of dimension %d does not match: %d != %d" % (d + 1, Q1.shape[d], Q2.shape[d]) ) if Q1.shape[-1] != 4: raise ValueError( "Last dimension of first argument does not match. A quaternion " "must have 4 entries, got %d" % Q1.shape[-1] ) if Q2.shape[-1] != 4: raise ValueError( "Last dimension of second argument does not match. A quaternion " "must have 4 entries, got %d" % Q2.shape[-1] ) if out is None: out = np.empty_like(Q1) vector_inner_products = np.sum(Q1[..., 1:] * Q2[..., 1:], axis=-1) out[..., 0] = Q1[..., 0] * Q2[..., 0] - vector_inner_products out[..., 1:] = ( Q1[..., 0, np.newaxis] * Q2[..., 1:] + Q2[..., 0, np.newaxis] * Q1[..., 1:] + np.cross(Q1[..., 1:], Q2[..., 1:]) ) return out def batch_q_conj(Q): """Conjugate of quaternions. The conjugate of a unit quaternion inverts the rotation represented by this unit quaternion. The conjugate of a quaternion q is often denoted as q*. Parameters ---------- Q : array-like, shape (..., 4) Unit quaternions to represent rotations: (w, x, y, z) Returns ------- Q_c : array, shape (..., 4,) Conjugates (w, -x, -y, -z) """ Q = np.asarray(Q) out = np.empty_like(Q) out[..., 0] = Q[..., 0] out[..., 1:] = -Q[..., 1:] return out def quaternion_slerp_batch(start, end, t, shortest_path=False): """Spherical linear interpolation for a batch of steps. Parameters ---------- start : array-like, shape (4,) Start unit quaternion to represent rotation: (w, x, y, z) end : array-like, shape (4,) End unit quaternion to represent rotation: (w, x, y, z) t : array-like, shape (n_steps,) Steps between start and goal, must be in interval [0, 1] shortest_path : bool, optional (default: False) Resolve sign ambiguity before interpolation to find the shortest path. The end quaternion will be picked to be close to the start quaternion. Returns ------- Q : array, shape (n_steps, 4) Interpolated unit quaternions """ t = np.asarray(t) if shortest_path: end = pick_closest_quaternion(end, start) angle = angle_between_vectors(start, end) w1, w2 = slerp_weights(angle, t) return ( w1[:, np.newaxis] * start[np.newaxis] + w2[:, np.newaxis] * end[np.newaxis] ) def axis_angles_from_quaternions(qs): """Compute axis-angle from quaternion. This operation is called logarithmic map. We usually assume active rotations. Parameters ---------- qs : array-like, shape (..., 4) Unit quaternion to represent rotation: (w, x, y, z) Returns ------- as : array, shape (..., 4) Axis of rotation and rotation angle: (x, y, z, angle). The angle is constrained to [0, pi) so that the mapping is unique. """ quaternion_vector_part = qs[..., 1:] qvec_norm = np.linalg.norm(quaternion_vector_part, axis=-1) # Vectorized branches bases on norm of the vector part small_p_norm_mask = qvec_norm < np.finfo(float).eps non_zero_mask = ~small_p_norm_mask axes = ( quaternion_vector_part[non_zero_mask] / qvec_norm[non_zero_mask, np.newaxis] ) w_clamped = np.clip(qs[non_zero_mask, 0], -1.0, 1.0) angles = 2.0 * np.arccos(w_clamped) result = np.empty_like(qs) result[non_zero_mask] = norm_axis_angles( np.concatenate((axes, angles[..., np.newaxis]), axis=-1) ) result[small_p_norm_mask] = np.array([1.0, 0.0, 0.0, 0.0]) return result def matrices_from_quaternions(Q, normalize_quaternions=True, out=None): """Compute rotation matrices from quaternions. Parameters ---------- Q : array-like, shape (..., 4) Unit quaternions to represent rotations: (w, x, y, z) normalize_quaternions : bool, optional (default: True) Normalize quaternions before conversion out : array, shape (..., 3, 3), optional (default: new array) Output array to which we write the result Returns ------- Rs : array, shape (..., 3, 3) Rotation matrices """ Q = np.asarray(Q) if normalize_quaternions: Q = norm_vectors(Q) w = Q[..., 0] x = Q[..., 1] y = Q[..., 2] z = Q[..., 3] x2 = 2.0 * x * x y2 = 2.0 * y * y z2 = 2.0 * z * z xy = 2.0 * x * y xz = 2.0 * x * z yz = 2.0 * y * z xw = 2.0 * x * w yw = 2.0 * y * w zw = 2.0 * z * w if out is None: out = np.empty(w.shape + (3, 3)) out[..., 0, 0] = 1.0 - y2 - z2 out[..., 0, 1] = xy - zw out[..., 0, 2] = xz + yw out[..., 1, 0] = xy + zw out[..., 1, 1] = 1.0 - x2 - z2 out[..., 1, 2] = yz - xw out[..., 2, 0] = xz - yw out[..., 2, 1] = yz + xw out[..., 2, 2] = 1.0 - x2 - y2 return out def batch_quaternion_wxyz_from_xyzw(Q_xyzw, out=None): """Converts from x, y, z, w to w, x, y, z convention. Parameters ---------- Q_xyzw : array-like, shape (..., 4) Quaternions with scalar part after vector part out : array, shape (..., 4), optional (default: new array) Output array to which we write the result Returns ------- Q_wxyz : array-like, shape (..., 4) Quaternions with scalar part before vector part """ Q_xyzw = np.asarray(Q_xyzw) if out is None: out = np.empty_like(Q_xyzw) out[..., 0] = Q_xyzw[..., 3] out[..., 1] = Q_xyzw[..., 0] out[..., 2] = Q_xyzw[..., 1] out[..., 3] = Q_xyzw[..., 2] return out def batch_quaternion_xyzw_from_wxyz(Q_wxyz, out=None): """Converts from w, x, y, z to x, y, z, w convention. Parameters ---------- Q_wxyz : array-like, shape (..., 4) Quaternions with scalar part before vector part out : array, shape (..., 4), optional (default: new array) Output array to which we write the result Returns ------- Q_xyzw : array-like, shape (..., 4) Quaternions with scalar part after vector part """ Q_wxyz = np.asarray(Q_wxyz) if out is None: out = np.empty_like(Q_wxyz) out[..., 0] = Q_wxyz[..., 1] out[..., 1] = Q_wxyz[..., 2] out[..., 2] = Q_wxyz[..., 3] out[..., 3] = Q_wxyz[..., 0] return out ================================================ FILE: pytransform3d/batch_rotations/_quaternion.pyi ================================================ import numpy as np import numpy.typing as npt from typing import Union, AnyStr def smooth_quaternion_trajectory( Q: npt.ArrayLike, start_component_positive: AnyStr = ... ) -> np.ndarray: ... def batch_concatenate_quaternions( Q1: npt.ArrayLike, Q2: npt.ArrayLike, out: Union[np.ndarray, None] = ... ) -> np.ndarray: ... def batch_q_conj(Q: npt.ArrayLike) -> np.ndarray: ... def quaternion_slerp_batch( start: npt.ArrayLike, end: npt.ArrayLike, t: npt.ArrayLike, shortest_path: bool = ..., ) -> np.ndarray: ... def axis_angles_from_quaternions(qs: npt.ArrayLike) -> np.ndarray: ... def matrices_from_quaternions( Q: npt.ArrayLike, normalize_quaternions: bool = ..., out: Union[np.ndarray, None] = ..., ) -> np.ndarray: ... def batch_quaternion_wxyz_from_xyzw( Q_xyzw: npt.ArrayLike, out: Union[np.ndarray, None] = ... ) -> np.ndarray: ... def batch_quaternion_xyzw_from_wxyz( Q_wxyz: npt.ArrayLike, out: Union[np.ndarray, None] = ... ) -> np.ndarray: ... ================================================ FILE: pytransform3d/batch_rotations/_utils.py ================================================ """Utility functions.""" import numpy as np def norm_vectors(V, out=None): """Normalize vectors. Parameters ---------- V : array-like, shape (..., n) nd vectors out : array, shape (..., n), optional (default: new array) Output array to which we write the result Returns ------- V_unit : array, shape (..., n) nd unit vectors with norm 1 or zero vectors """ V = np.asarray(V) norms = np.linalg.norm(V, axis=-1) if out is None: out = np.empty_like(V) # Avoid division by zero with np.maximum(..., smallest positive float). # The norm is zero only when the vector is zero so this case does not # require further processing. out[...] = V / np.maximum(norms[..., np.newaxis], np.finfo(float).tiny) return out def angles_between_vectors(A, B): """Compute angle between two vectors. Parameters ---------- A : array-like, shape (..., n) nd vectors B : array-like, shape (..., n) nd vectors Returns ------- angles : array, shape (...) Angles between pairs of vectors from A and B """ A = np.asarray(A) B = np.asarray(B) n_dims = A.shape[-1] A_norms = np.linalg.norm(A, axis=-1) B_norms = np.linalg.norm(B, axis=-1) AdotB = np.einsum( "ni,ni->n", A.reshape(-1, n_dims), B.reshape(-1, n_dims) ).reshape(A.shape[:-1]) return np.arccos(np.clip(AdotB / (A_norms * B_norms), -1.0, 1.0)) def cross_product_matrices(V): """Generate the cross-product matrices of vectors. The cross-product matrix :math:`\\boldsymbol{V}` satisfies the equation .. math:: \\boldsymbol{V} \\boldsymbol{w} = \\boldsymbol{v} \\times \\boldsymbol{w} It is a skew-symmetric (antisymmetric) matrix, i.e. :math:`-\\boldsymbol{V} = \\boldsymbol{V}^T`. Parameters ---------- V : array-like, shape (..., 3) 3d vectors Returns ------- V_cross_product_matrices : array, shape (..., 3, 3) Cross-product matrices of V """ V = np.asarray(V) instances_shape = V.shape[:-1] V_matrices = np.empty(instances_shape + (3, 3)) V_matrices[..., 0, 0] = 0.0 V_matrices[..., 0, 1] = -V[..., 2] V_matrices[..., 0, 2] = V[..., 1] V_matrices[..., 1, 0] = V[..., 2] V_matrices[..., 1, 1] = 0.0 V_matrices[..., 1, 2] = -V[..., 0] V_matrices[..., 2, 0] = -V[..., 1] V_matrices[..., 2, 1] = V[..., 0] V_matrices[..., 2, 2] = 0.0 return V_matrices ================================================ FILE: pytransform3d/batch_rotations/_utils.pyi ================================================ import numpy as np import numpy.typing as npt from typing import Union def norm_vectors( V: npt.ArrayLike, out: Union[npt.ArrayLike, None] = ... ) -> np.ndarray: ... def angles_between_vectors( A: npt.ArrayLike, B: npt.ArrayLike ) -> np.ndarray: ... def cross_product_matrices(V: npt.ArrayLike) -> np.ndarray: ... ================================================ FILE: pytransform3d/batch_rotations/test/test_batch_rotations.py ================================================ import numpy as np import pytest from numpy.testing import assert_array_almost_equal from pytransform3d import batch_rotations as pbr from pytransform3d import rotations as pr def test_norm_vectors_0dims(): rng = np.random.default_rng(8380) V = rng.standard_normal(size=3) V_unit = pbr.norm_vectors(V) assert pytest.approx(np.linalg.norm(V_unit)) == 1.0 def test_norm_vectors_1dim(): rng = np.random.default_rng(8381) V = rng.standard_normal(size=(100, 3)) V_unit = pbr.norm_vectors(V) assert_array_almost_equal(np.linalg.norm(V_unit, axis=1), np.ones(len(V))) def test_norm_vectors_1dim_output_variable(): rng = np.random.default_rng(8381) V = rng.standard_normal(size=(100, 3)) pbr.norm_vectors(V, out=V) assert_array_almost_equal(np.linalg.norm(V, axis=1), np.ones(len(V))) def test_norm_vectors_3dims(): rng = np.random.default_rng(8382) V = rng.standard_normal(size=(8, 2, 8, 3)) V_unit = pbr.norm_vectors(V) assert_array_almost_equal( np.linalg.norm(V_unit, axis=-1), np.ones(V_unit.shape[:-1]) ) def test_norm_vectors_zero(): V = np.zeros((3, 8, 1, 2)) V_unit = pbr.norm_vectors(V) assert_array_almost_equal(V_unit, V) def test_norm_axis_angles(): rng = np.random.default_rng(843) # create a batch of unnormalized axis-angle instances n_rotations = 10 A_unnormalized = np.hstack( ( rng.standard_normal(size=(n_rotations, 3)), np.pi + rng.uniform(size=(n_rotations, 1)), ) ) # cross check scalar version with vectorized version when passing # an 1D array assert_array_almost_equal( pbr.norm_axis_angles(A_unnormalized[0]), pr.norm_axis_angle(A_unnormalized[0]), ) # cross check scalar version with vectorized version when passing # a 3D array assert_array_almost_equal( pbr.norm_axis_angles(np.array([[[0.0, 0.0, 0.0, np.pi]]]))[0, 0], pr.norm_axis_angle(np.array([0.0, 0.0, 0.0, np.pi])), ) # 2D A_norm = pbr.norm_axis_angles(A_unnormalized) for a_unnormalized, a_norm in zip(A_unnormalized, A_norm): # Make sure that the unnormalized version was in fact not normalized. assert pytest.approx(a_unnormalized[3]) != a_norm[3] assert pytest.approx(np.linalg.norm(a_unnormalized[:3])) != 1.0 assert_array_almost_equal(a_norm, pr.norm_axis_angle(a_unnormalized)) # 3D A3D = A_unnormalized.reshape(n_rotations // 2, 2, -1) A3D_norm = pbr.norm_axis_angles(A3D) for a_unnormalized, a_norm in zip( A3D.reshape(n_rotations, -1), A3D_norm.reshape(n_rotations, -1) ): assert_array_almost_equal(a_norm, pr.norm_axis_angle(a_unnormalized)) def test_angles_between_vectors_0dims(): rng = np.random.default_rng(228) A = rng.standard_normal(size=3) B = rng.standard_normal(size=3) angles = pbr.angles_between_vectors(A, B) angles2 = pr.angle_between_vectors(A, B) assert_array_almost_equal(angles, angles2) def test_angles_between_vectors_1dim(): rng = np.random.default_rng(229) A = rng.standard_normal(size=(100, 3)) B = rng.standard_normal(size=(100, 3)) angles = pbr.angles_between_vectors(A, B) angles2 = [pr.angle_between_vectors(a, b) for a, b in zip(A, B)] assert_array_almost_equal(angles, angles2) def test_angles_between_vectors_3dims(): rng = np.random.default_rng(230) A = rng.standard_normal(size=(2, 4, 3, 4)) B = rng.standard_normal(size=(2, 4, 3, 4)) angles = pbr.angles_between_vectors(A, B).ravel() angles2 = [ pr.angle_between_vectors(a, b) for a, b in zip(A.reshape(-1, 4), B.reshape(-1, 4)) ] assert_array_almost_equal(angles, angles2) def test_active_matrices_from_angles_0dims(): R = pbr.active_matrices_from_angles(0, 0.4) assert_array_almost_equal(R, pr.active_matrix_from_angle(0, 0.4)) def test_active_matrices_from_angles_1dim(): Rs = pbr.active_matrices_from_angles(1, [0.4, 0.5, 0.6]) assert_array_almost_equal(Rs[0], pr.active_matrix_from_angle(1, 0.4)) assert_array_almost_equal(Rs[1], pr.active_matrix_from_angle(1, 0.5)) assert_array_almost_equal(Rs[2], pr.active_matrix_from_angle(1, 0.6)) def test_active_matrices_from_angles_3dims(): rng = np.random.default_rng(8383) angles = rng.standard_normal(size=(2, 3, 4)) Rs = pbr.active_matrices_from_angles(2, angles) Rs = Rs.reshape(-1, 3, 3) Rs2 = [ pr.active_matrix_from_angle(2, angle) for angle in angles.reshape(-1) ] assert_array_almost_equal(Rs, Rs2) def test_active_matrices_from_angles_3dims_output_variable(): rng = np.random.default_rng(8384) angles = rng.standard_normal(size=(2, 3, 4)) Rs = np.empty((2, 3, 4, 3, 3)) pbr.active_matrices_from_angles(2, angles, out=Rs) Rs = Rs.reshape(-1, 3, 3) Rs2 = [ pr.active_matrix_from_angle(2, angle) for angle in angles.reshape(-1) ] assert_array_almost_equal(Rs, Rs2) def test_active_matrices_from_intrinsic_euler_angles_0dims(): rng = np.random.default_rng(8383) e = rng.standard_normal(size=3) R = pbr.active_matrices_from_intrinsic_euler_angles(2, 1, 0, e) R2 = pr.active_matrix_from_intrinsic_euler_zyx(e) assert_array_almost_equal(R, R2) def test_active_matrices_from_intrinsic_euler_angles_1dim(): rng = np.random.default_rng(8384) e = rng.standard_normal(size=(10, 3)) Rs = pbr.active_matrices_from_intrinsic_euler_angles(2, 1, 0, e) for i in range(len(e)): Ri = pr.active_matrix_from_intrinsic_euler_zyx(e[i]) assert_array_almost_equal(Rs[i], Ri) def test_active_matrices_from_intrinsic_euler_angles_1dim_output_variables(): rng = np.random.default_rng(8384) e = rng.standard_normal(size=(10, 3)) Rs = np.empty((10, 3, 3)) pbr.active_matrices_from_intrinsic_euler_angles(2, 1, 0, e, out=Rs) for i in range(len(e)): Ri = pr.active_matrix_from_intrinsic_euler_zyx(e[i]) assert_array_almost_equal(Rs[i], Ri) def test_active_matrices_from_intrinsic_euler_angles_3dims(): rng = np.random.default_rng(8385) e = rng.standard_normal(size=(2, 3, 4, 3)) Rs = pbr.active_matrices_from_intrinsic_euler_angles(2, 1, 0, e).reshape( -1, 3, 3 ) e = e.reshape(-1, 3) for i in range(len(e)): Ri = pr.active_matrix_from_intrinsic_euler_zyx(e[i]) assert_array_almost_equal(Rs[i], Ri) def test_active_matrices_from_extrinsic_euler_angles_0dims(): rng = np.random.default_rng(8383) e = rng.standard_normal(size=3) R = pbr.active_matrices_from_extrinsic_euler_angles(2, 1, 0, e) R2 = pr.active_matrix_from_extrinsic_euler_zyx(e) assert_array_almost_equal(R, R2) def test_active_matrices_from_extrinsic_euler_angles_1dim(): rng = np.random.default_rng(8384) e = rng.standard_normal(size=(10, 3)) Rs = pbr.active_matrices_from_extrinsic_euler_angles(2, 1, 0, e) for i in range(len(e)): Ri = pr.active_matrix_from_extrinsic_euler_zyx(e[i]) assert_array_almost_equal(Rs[i], Ri) def test_active_matrices_from_extrinsic_euler_angles_3dim(): rng = np.random.default_rng(8385) e = rng.standard_normal(size=(2, 3, 4, 3)) Rs = pbr.active_matrices_from_extrinsic_euler_angles(2, 1, 0, e).reshape( -1, 3, 3 ) e = e.reshape(-1, 3) for i in range(len(e)): Ri = pr.active_matrix_from_extrinsic_euler_zyx(e[i]) assert_array_almost_equal(Rs[i], Ri) def test_active_matrices_from_extrinsic_euler_angles_1dim_output_variable(): rng = np.random.default_rng(8385) e = rng.standard_normal(size=(10, 3)) Rs = np.empty((10, 3, 3)) pbr.active_matrices_from_extrinsic_euler_angles(2, 1, 0, e, out=Rs) for i in range(len(e)): Ri = pr.active_matrix_from_extrinsic_euler_zyx(e[i]) assert_array_almost_equal(Rs[i], Ri) def test_cross_product_matrix(): rng = np.random.default_rng(3820) v = rng.standard_normal(size=3) assert_array_almost_equal( pbr.cross_product_matrices(v), pr.cross_product_matrix(v) ) def test_cross_product_matrices(): rng = np.random.default_rng(3820) V = rng.standard_normal(size=(2, 2, 3, 3)) V_cpm = pbr.cross_product_matrices(V) V_cpm = V_cpm.reshape(-1, 3, 3) V_cpm2 = [pr.cross_product_matrix(v) for v in V.reshape(-1, 3)] assert_array_almost_equal(V_cpm, V_cpm2) def test_matrices_from_quaternions(): rng = np.random.default_rng(83) for _ in range(5): q = pr.random_quaternion(rng) R = pbr.matrices_from_quaternions( q[np.newaxis], normalize_quaternions=False )[0] q2 = pr.quaternion_from_matrix(R) pr.assert_quaternion_equal(q, q2) for _ in range(5): q = rng.standard_normal(size=4) R = pbr.matrices_from_quaternions( q[np.newaxis], normalize_quaternions=True )[0] q2 = pr.quaternion_from_matrix(R) pr.assert_quaternion_equal(q / np.linalg.norm(q), q2) def test_quaternions_from_matrices(): rng = np.random.default_rng(84) for _ in range(5): q = pr.random_quaternion(rng) R = pr.matrix_from_quaternion(q) q2 = pbr.quaternions_from_matrices(R[np.newaxis])[0] pr.assert_quaternion_equal(q, q2) a = np.array([1.0, 0.0, 0.0, np.pi]) q = pr.quaternion_from_axis_angle(a) R = pr.matrix_from_axis_angle(a) q_from_R = pbr.quaternions_from_matrices(R[np.newaxis])[0] assert_array_almost_equal(q, q_from_R) a = np.array([0.0, 1.0, 0.0, np.pi]) q = pr.quaternion_from_axis_angle(a) R = pr.matrix_from_axis_angle(a) q_from_R = pbr.quaternions_from_matrices(R[np.newaxis])[0] assert_array_almost_equal(q, q_from_R) a = np.array([0.0, 0.0, 1.0, np.pi]) q = pr.quaternion_from_axis_angle(a) R = pr.matrix_from_axis_angle(a) q_from_R = pbr.quaternions_from_matrices(R[np.newaxis])[0] assert_array_almost_equal(q, q_from_R) def test_quaternions_from_matrices_no_batch(): rng = np.random.default_rng(85) for _ in range(5): q = pr.random_quaternion(rng) R = pr.matrix_from_quaternion(q) q2 = pbr.quaternions_from_matrices(R) pr.assert_quaternion_equal(q, q2) a = np.array([1.0, 0.0, 0.0, np.pi]) q = pr.quaternion_from_axis_angle(a) R = pr.matrix_from_axis_angle(a) q_from_R = pbr.quaternions_from_matrices(R) assert_array_almost_equal(q, q_from_R) a = np.array([0.0, 1.0, 0.0, np.pi]) q = pr.quaternion_from_axis_angle(a) R = pr.matrix_from_axis_angle(a) q_from_R = pbr.quaternions_from_matrices(R) assert_array_almost_equal(q, q_from_R) a = np.array([0.0, 0.0, 1.0, np.pi]) q = pr.quaternion_from_axis_angle(a) R = pr.matrix_from_axis_angle(a) q_from_R = pbr.quaternions_from_matrices(R) assert_array_almost_equal(q, q_from_R) def test_quaternions_from_matrices_4d(): rng = np.random.default_rng(84) for _ in range(5): q = pr.random_quaternion(rng) R = pr.matrix_from_quaternion(q) q2 = pbr.quaternions_from_matrices([[R, R], [R, R]]) pr.assert_quaternion_equal(q, q2[0, 0]) pr.assert_quaternion_equal(q, q2[0, 1]) pr.assert_quaternion_equal(q, q2[1, 0]) pr.assert_quaternion_equal(q, q2[1, 1]) def test_axis_angles_from_matrices_0dims(): rng = np.random.default_rng(84) A = rng.standard_normal(size=3) A /= np.linalg.norm(A, axis=-1)[..., np.newaxis] A *= rng.random() * np.pi Rs = pbr.matrices_from_compact_axis_angles(A) A2 = pbr.axis_angles_from_matrices(Rs) A2_compact = A2[:3] * A2[3] assert_array_almost_equal(A, A2_compact) def test_axis_angles_from_matrices(): rng = np.random.default_rng(84) A = rng.standard_normal(size=(2, 3, 3)) A /= np.linalg.norm(A, axis=-1)[..., np.newaxis] A *= rng.random(size=(2, 3, 1)) * np.pi A[0, 0, :] = 0.0 Rs = pbr.matrices_from_compact_axis_angles(A) A2 = pbr.axis_angles_from_matrices(Rs) A2_compact = A2[..., :3] * A2[..., 3, np.newaxis] assert_array_almost_equal(A, A2_compact) def test_axis_angles_from_matrices_output_variable(): rng = np.random.default_rng(84) A = rng.standard_normal(size=(2, 3, 3)) A /= np.linalg.norm(A, axis=-1)[..., np.newaxis] A *= rng.random(size=(2, 3, 1)) * np.pi A[0, 0, :] = 0.0 Rs = np.empty((2, 3, 3, 3)) pbr.matrices_from_compact_axis_angles(A, out=Rs) A2 = np.empty((2, 3, 4)) pbr.axis_angles_from_matrices(Rs, out=A2) A2_compact = A2[..., :3] * A2[..., 3, np.newaxis] assert_array_almost_equal(A, A2_compact) def test_axis_angles_from_matrices_norot(): a = pbr.axis_angles_from_matrices(np.eye(3)) assert_array_almost_equal(a, [1, 0, 0, 0]) A = pbr.axis_angles_from_matrices([np.eye(3), np.eye(3)]) assert_array_almost_equal(A, [[1, 0, 0, 0], [1, 0, 0, 0]]) A = pbr.axis_angles_from_matrices( [[np.eye(3), np.eye(3)], [np.eye(3), np.eye(3)]] ) assert_array_almost_equal( A, [[[1, 0, 0, 0], [1, 0, 0, 0]], [[1, 0, 0, 0], [1, 0, 0, 0]]] ) def test_axis_angles_from_quaternions(): rng = np.random.default_rng(48322) n_rotations = 20 Q = pbr.norm_vectors(rng.standard_normal(size=(n_rotations, 4))) # 1D pr.assert_quaternion_equal( pbr.axis_angles_from_quaternions(Q[0]), pr.axis_angle_from_quaternion(Q[0]), ) # 2D A = pbr.axis_angles_from_quaternions(Q) for a, q in zip(A, Q): pr.assert_quaternion_equal(a, pr.axis_angle_from_quaternion(q)) # 3D Q3D = Q.reshape(n_rotations // 4, 4, 4) A3D = pbr.axis_angles_from_quaternions(Q3D) for a, q in zip(A3D.reshape(n_rotations, 4), Q3D.reshape(n_rotations, 4)): pr.assert_quaternion_equal(a, pr.axis_angle_from_quaternion(q)) def test_quaternion_slerp_batch_zero_angle(): rng = np.random.default_rng(228) q = pr.random_quaternion(rng) Q = pbr.quaternion_slerp_batch(q, q, [0.5]) pr.assert_quaternion_equal(q, Q[0]) def test_quaternion_slerp_batch(): rng = np.random.default_rng(229) q_start = pr.random_quaternion(rng) q_end = pr.random_quaternion(rng) t = np.linspace(0, 1, 101) Q = pbr.quaternion_slerp_batch(q_start, q_end, t) for i in range(len(t)): qi = pr.quaternion_slerp(q_start, q_end, t[i]) pr.assert_quaternion_equal(Q[i], qi) def test_quaternion_slerp_batch_sign_ambiguity(): n_steps = 10 rng = np.random.default_rng(2323) q1 = pr.random_quaternion(rng) a1 = pr.axis_angle_from_quaternion(q1) a2 = np.r_[a1[:3], a1[3] * 1.1] q2 = pr.quaternion_from_axis_angle(a2) if np.sign(q1[0]) != np.sign(q2[0]): q2 *= -1.0 traj_q = pbr.quaternion_slerp_batch( q1, q2, np.linspace(0, 1, n_steps), shortest_path=True ) path_length = np.sum( [pr.quaternion_dist(r, s) for r, s in zip(traj_q[:-1], traj_q[1:])] ) q2 *= -1.0 traj_q_opposing = pbr.quaternion_slerp_batch( q1, q2, np.linspace(0, 1, n_steps), shortest_path=False ) path_length_opposing = np.sum( [ pr.quaternion_dist(r, s) for r, s in zip(traj_q_opposing[:-1], traj_q_opposing[1:]) ] ) assert path_length_opposing > path_length traj_q_opposing_corrected = pbr.quaternion_slerp_batch( q1, q2, np.linspace(0, 1, n_steps), shortest_path=True ) path_length_opposing_corrected = np.sum( [ pr.quaternion_dist(r, s) for r, s in zip( traj_q_opposing_corrected[:-1], traj_q_opposing_corrected[1:] ) ] ) assert pytest.approx(path_length_opposing_corrected) == path_length def test_batch_concatenate_quaternions_mismatch(): Q1 = np.zeros((1, 2, 4)) Q2 = np.zeros((1, 2, 3, 4)) with pytest.raises( ValueError, match="Number of dimensions must be the same." ): pbr.batch_concatenate_quaternions(Q1, Q2) Q1 = np.zeros((1, 2, 4, 4)) Q2 = np.zeros((1, 2, 3, 4)) with pytest.raises(ValueError, match="Size of dimension 3 does not match"): pbr.batch_concatenate_quaternions(Q1, Q2) Q1 = np.zeros((1, 2, 3, 3)) Q2 = np.zeros((1, 2, 3, 4)) with pytest.raises( ValueError, match="Last dimension of first argument does not " "match." ): pbr.batch_concatenate_quaternions(Q1, Q2) Q1 = np.zeros((1, 2, 3, 4)) Q2 = np.zeros((1, 2, 3, 3)) with pytest.raises( ValueError, match="Last dimension of second argument does " "not match." ): pbr.batch_concatenate_quaternions(Q1, Q2) def test_batch_concatenate_quaternions_1d(): rng = np.random.default_rng(230) q1 = pr.random_quaternion(rng) q2 = pr.random_quaternion(rng) q12 = np.empty(4) pbr.batch_concatenate_quaternions(q1, q2, out=q12) assert_array_almost_equal(q12, pr.concatenate_quaternions(q1, q2)) def test_batch_q_conj_1d(): rng = np.random.default_rng(230) q = pr.random_quaternion(rng) assert_array_almost_equal(pr.q_conj(q), pbr.batch_q_conj(q)) def test_batch_concatenate_q_conj(): rng = np.random.default_rng(231) Q = np.array([pr.random_quaternion(rng) for _ in range(10)]) Q = Q.reshape(2, 5, 4) Q_conj = pbr.batch_q_conj(Q) Q_Q_conj = pbr.batch_concatenate_quaternions(Q, Q_conj) assert_array_almost_equal( Q_Q_conj.reshape(-1, 4), np.array([[1, 0, 0, 0]] * 10) ) def test_batch_convert_quaternion_conventions(): q_wxyz = np.array([[1.0, 0.0, 0.0, 0.0], [1.0, 0.0, 0.0, 0.0]]) q_xyzw = pbr.batch_quaternion_xyzw_from_wxyz(q_wxyz) assert_array_almost_equal( q_xyzw, np.array([[0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 1.0]]) ) pbr.batch_quaternion_xyzw_from_wxyz(q_wxyz, out=q_xyzw) assert_array_almost_equal( q_xyzw, np.array([[0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 1.0]]) ) q_wxyz2 = pbr.batch_quaternion_wxyz_from_xyzw(q_xyzw) assert_array_almost_equal(q_wxyz, q_wxyz2) pbr.batch_quaternion_wxyz_from_xyzw(q_xyzw, out=q_wxyz2) assert_array_almost_equal(q_wxyz, q_wxyz2) rng = np.random.default_rng(42) q_wxyz_random = pr.random_quaternion(rng) q_xyzw_random = pbr.batch_quaternion_xyzw_from_wxyz(q_wxyz_random) assert_array_almost_equal(q_xyzw_random[:3], q_wxyz_random[1:]) assert q_xyzw_random[3] == q_wxyz_random[0] q_wxyz_random2 = pbr.batch_quaternion_wxyz_from_xyzw(q_xyzw_random) assert_array_almost_equal(q_wxyz_random, q_wxyz_random2) def test_smooth_quaternion_trajectory(): rng = np.random.default_rng(232) q_start = pr.random_quaternion(rng) if q_start[1] < 0.0: q_start *= -1.0 q_goal = pr.random_quaternion(rng) n_steps = 101 Q = np.empty((n_steps, 4)) for i, t in enumerate(np.linspace(0, 1, n_steps)): Q[i] = pr.quaternion_slerp(q_start, q_goal, t) Q_broken = Q.copy() Q_broken[20:23, :] *= -1.0 Q_broken[80:, :] *= -1.0 Q_smooth = pbr.smooth_quaternion_trajectory(Q_broken) assert_array_almost_equal(Q_smooth, Q) def test_smooth_quaternion_trajectory_start_component_negative(): rng = np.random.default_rng(232) for index in range(4): component = "wxyz"[index] q = pr.random_quaternion(rng) if q[index] > 0.0: q *= -1.0 q_corrected = pbr.smooth_quaternion_trajectory( [q], start_component_positive=component )[0] assert q_corrected[index] > 0.0 def test_smooth_quaternion_trajectory_empty(): with pytest.raises( ValueError, match=r"At least one quaternion is expected" ): pbr.smooth_quaternion_trajectory(np.zeros((0, 4))) ================================================ FILE: pytransform3d/camera.py ================================================ """Transformations related to cameras. See :doc:`user_guide/camera` for more information. """ import numpy as np from .transformations import invert_transform, transform, check_transform def make_world_grid( n_lines=11, n_points_per_line=51, xlim=(-0.5, 0.5), ylim=(-0.5, 0.5) ): """Generate grid in world coordinate frame. The grid will have the form .. code:: +----+----+----+----+----+ | | | | | | +----+----+----+----+----+ | | | | | | +----+----+----+----+----+ | | | | | | +----+----+----+----+----+ | | | | | | +----+----+----+----+----+ | | | | | | +----+----+----+----+----+ on the x-y plane with z=0 for all points. Parameters ---------- n_lines : int, optional (default: 11) Number of lines n_points_per_line : int, optional (default: 51) Number of points per line xlim : tuple, optional (default: (-0.5, 0.5)) Range on x-axis ylim : tuple, optional (default: (-0.5, 0.5)) Range on y-axis Returns ------- world_grid : array-like, shape (2 * n_lines * n_points_per_line, 4) Grid as homogenous coordinate vectors """ world_grid_x = np.vstack( [ make_world_line([xlim[0], y], [xlim[1], y], n_points_per_line) for y in np.linspace(ylim[0], ylim[1], n_lines) ] ) world_grid_y = np.vstack( [ make_world_line([x, ylim[0]], [x, ylim[1]], n_points_per_line) for x in np.linspace(xlim[0], xlim[1], n_lines) ] ) return np.vstack((world_grid_x, world_grid_y)) def make_world_line(p1, p2, n_points): """Generate line in world coordinate frame. Parameters ---------- p1 : array-like, shape (2 or 3,) Start point of the line p2 : array-like, shape (2 or 3,) End point of the line n_points : int Number of points Returns ------- line : array-like, shape (n_points, 4) Samples from line in world frame """ if len(p1) == 2: p1 = [p1[0], p1[1], 0] if len(p2) == 2: p2 = [p2[0], p2[1], 0] return np.array( [ np.linspace(p1[0], p2[0], n_points), np.linspace(p1[1], p2[1], n_points), np.linspace(p1[2], p2[2], n_points), np.ones(n_points), ] ).T def cam2sensor(P_cam, focal_length, kappa=0.0): """Project points from 3D camera coordinate system to sensor plane. Parameters ---------- P_cam : array-like, shape (n_points, 3 or 4) Points in camera coordinates focal_length : float Focal length of the camera kappa : float, optional (default: 0) Camera distortion parameter Returns ------- P_sensor : array-like, shape (n_points, 2) Points on the sensor plane. The result for points that are behind the camera will be a vector of nans. Raises ------ ValueError If input is not valid """ n_points, n_dims = P_cam.shape if n_dims not in (3, 4): raise ValueError( "Expected 3- or 4-dimensional points, got %d " "dimensions" % n_dims ) if focal_length <= 0.0: raise ValueError("Focal length must be greater than 0.") P_sensor = np.empty((n_points, 2)) ahead = P_cam[:, 2] > 0.0 P_sensor[ahead] = P_cam[ahead][:, :2] / P_cam[ahead][:, 2, np.newaxis] behind = np.logical_not(ahead) for n in range(P_sensor.shape[0]): P_sensor[n] *= 1.0 / (1.0 + kappa * np.linalg.norm(P_sensor[n]) ** 2) P_sensor *= focal_length P_sensor[behind] = np.nan return P_sensor def sensor2img(P_sensor, sensor_size, image_size, image_center=None): """Project points from 2D sensor plane to image coordinate system. Parameters ---------- P_sensor : array-like, shape (n_points, 2) Points on camera sensor sensor_size : array-like, shape (2,) Size of the sensor array image_size : array-like, shape (2,) Size of the camera image: (width, height) image_center : array-like, shape (2,), optional (default: image_size / 2) Center of the image Returns ------- P_img : array-like, shape (n_points, 2) Points on image """ P_img = np.asarray(image_size) * P_sensor / np.asarray(sensor_size) if image_center is None: image_center = np.asarray(image_size) / 2 P_img += np.asarray(image_center) return P_img def world2image( P_world, cam2world, sensor_size, image_size, focal_length, image_center=None, kappa=0.0, ): """Project points from 3D world coordinate system to 2D image. Parameters ---------- P_world : array-like, shape (n_points, 4) Points in world coordinates cam2world : array-like, shape (4, 4), optional (default: I) Camera in world frame sensor_size : array-like, shape (2,) Size of the sensor array image_size : array-like, shape (2,) Size of the camera image: (width, height) focal_length : float Focal length of the camera image_center : array-like, shape (2,), optional (default: image_size / 2) Center of the image kappa : float, optional (default: 0) Camera distortion parameter Returns ------- P_img : array-like, shape (n_points, 2) Points on image """ world2cam = invert_transform(cam2world) P_cam = transform(world2cam, P_world) P_sensor = cam2sensor(P_cam, focal_length, kappa) P_img = sensor2img(P_sensor, sensor_size, image_size, image_center) return P_img def plot_camera( ax=None, M=None, cam2world=None, virtual_image_distance=1.0, sensor_size=(1920, 1080), ax_s=1, strict_check=True, **kwargs, ): # pragma: no cover """Plot camera in world coordinates. This function is inspired by Blender's camera visualization. It will show the camera center, a virtual image plane, and the top of the virtual image plane. Parameters ---------- ax : Matplotlib 3d axis, optional (default: None) If the axis is None, a new 3d axis will be created. M : array-like, shape (3, 3) Intrinsic camera matrix that contains the focal lengths on the diagonal and the center of the the image in the last column. It does not matter whether values are given in meters or pixels as long as the unit is the same as for the sensor size. cam2world : array-like, shape (4, 4), optional (default: I) Transformation matrix of camera in world frame. We assume that the position is given in meters. virtual_image_distance : float, optional (default: 1) Distance from pinhole to virtual image plane that will be displayed. We assume that this distance is given in meters. The unit has to be consistent with the unit of the position in cam2world. sensor_size : array-like, shape (2,), optional (default: [1920, 1080]) Size of the image sensor: (width, height). It does not matter whether values are given in meters or pixels as long as the unit is the same as for the sensor size. ax_s : float, optional (default: 1) Scaling of the new matplotlib 3d axis. strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. kwargs : dict, optional (default: {}) Additional arguments for the plotting functions, e.g. alpha. Returns ------- ax : Matplotlib 3d axis New or old axis Raises ------ ValueError If input is not valid """ from .plot_utils import Camera if ax is None: from .plot_utils import make_3d_axis ax = make_3d_axis(ax_s) if M is None: raise ValueError("No intrinsic camera matrix given.") if cam2world is None: cam2world = np.eye(4) cam2world = check_transform(cam2world, strict_check=strict_check) camera = Camera(M, cam2world, virtual_image_distance, sensor_size, **kwargs) camera.add_camera(ax) return ax ================================================ FILE: pytransform3d/camera.pyi ================================================ from typing import Union import numpy as np import numpy.typing as npt from mpl_toolkits.mplot3d import Axes3D def make_world_grid( n_lines: int = ..., n_points_per_line: int = ..., xlim: npt.ArrayLike = ..., ylim: npt.ArrayLike = ..., ) -> np.ndarray: ... def make_world_line( p1: npt.ArrayLike, p2: npt.ArrayLike, n_points: int ) -> np.ndarray: ... def cam2sensor( P_cam: npt.ArrayLike, focal_length: float, kappa: float = ... ) -> np.ndarray: ... def sensor2img( P_sensor: npt.ArrayLike, sensor_size: npt.ArrayLike, image_size: npt.ArrayLike, image_center: Union[npt.ArrayLike, None] = ..., ) -> np.ndarray: ... def world2image( P_world: npt.ArrayLike, cam2world: npt.ArrayLike, sensor_size: npt.ArrayLike, image_size: npt.ArrayLike, focal_length: float, image_center: Union[npt.ArrayLike, None] = ..., kappa: float = ..., ) -> np.ndarray: ... def plot_camera( ax: Union[None, Axes3D] = ..., M: Union[None, npt.ArrayLike] = ..., cam2world: Union[None, npt.ArrayLike] = ..., virtual_image_distance: float = ..., sensor_size: npt.ArrayLike = ..., ax_s: float = ..., strict_check: bool = ..., **kwargs, ) -> Axes3D: ... ================================================ FILE: pytransform3d/coordinates.py ================================================ """Conversions between coordinate systems to represent positions.""" import numpy as np def cartesian_from_cylindrical(p): """Convert cylindrical coordinates to Cartesian coordinates. Parameters ---------- p : array-like, shape (..., 3) Cylindrical coordinates: axial / radial distance (rho), azimuth (phi), and axial coordinate / height (z) Returns ------- q : array, shape (..., 3) Cartesian coordinates (x, y, z) """ p = np.asarray(p, dtype=float) q = np.empty_like(p) q[..., 0] = p[..., 0] * np.cos(p[..., 1]) q[..., 1] = p[..., 0] * np.sin(p[..., 1]) q[..., 2] = p[..., 2] return q def cartesian_from_spherical(p): """Convert spherical coordinates to Cartesian coordinates. Parameters ---------- p : array-like, shape (..., 3) Spherical coordinates: radial distance (rho), inclination / elevation (theta), and azimuth (phi) Returns ------- q : array, shape (..., 3) Cartesian coordinates (x, y, z) """ p = np.asarray(p, dtype=float) q = np.empty_like(p) r_sin_theta = p[..., 0] * np.sin(p[..., 1]) q[..., 0] = np.cos(p[..., 2]) * r_sin_theta q[..., 1] = np.sin(p[..., 2]) * r_sin_theta q[..., 2] = p[..., 0] * np.cos(p[..., 1]) return q def cylindrical_from_cartesian(p): """Convert Cartesian coordinates to cylindrical coordinates. Parameters ---------- p : array-like, shape (..., 3) Cartesian coordinates (x, y, z) Returns ------- q : array, shape (..., 3) Cylindrical coordinates: axial / radial distance (rho >= 0), azimuth (-pi >= phi >= pi), and axial coordinate / height (z) """ p = np.asarray(p, dtype=float) q = np.empty_like(p) q[..., 0] = np.linalg.norm(p[..., :2], axis=-1) q[..., 1] = np.arctan2(p[..., 1], p[..., 0]) q[..., 2] = p[..., 2] return q def cylindrical_from_spherical(p): """Convert spherical coordinates to cylindrical coordinates. Parameters ---------- p : array-like, shape (..., 3) Spherical coordinates: radial distance (rho), inclination / elevation (theta), and azimuth (phi) Returns ------- q : array, shape (..., 3) Cylindrical coordinates: axial / radial distance (rho), azimuth (phi), and axial coordinate / height (z) """ p = np.asarray(p, dtype=float) q = np.empty_like(p) q[..., 0] = p[..., 0] * np.sin(p[..., 1]) q[..., 1] = p[..., 2] q[..., 2] = p[..., 0] * np.cos(p[..., 1]) return q def spherical_from_cartesian(p): """Convert Cartesian coordinates to spherical coordinates. Parameters ---------- p : array-like, shape (..., 3) Cartesian coordinates (x, y, z) Returns ------- q : array, shape (..., 3) Spherical coordinates: radial distance (rho >= 0), inclination / elevation (0 <= theta <= pi), and azimuth (-pi <= phi <= pi) """ p = np.asarray(p, dtype=float) q = np.empty_like(p) q[..., 0] = np.linalg.norm(p, axis=-1) q[..., 1] = np.arctan2(np.linalg.norm(p[..., :2], axis=-1), p[..., 2]) q[..., 2] = np.arctan2(p[..., 1], p[..., 0]) return q def spherical_from_cylindrical(p): """Convert cylindrical coordinates to spherical coordinates. Parameters ---------- p : array-like, shape (..., 3) Cylindrical coordinates: axial / radial distance (rho), azimuth (phi), and axial coordinate / height (z) Returns ------- q : array, shape (..., 3) Spherical coordinates: radial distance (rho), inclination / elevation (theta), and azimuth (phi) """ p = np.asarray(p, dtype=float) q = np.empty_like(p) q[..., 0] = np.linalg.norm(p[..., (0, 2)], axis=-1) q[..., 1] = np.arctan2(p[..., 0], p[..., 2]) q[..., 2] = p[..., 1] return q ================================================ FILE: pytransform3d/coordinates.pyi ================================================ import numpy as np import numpy.typing as npt def cartesian_from_cylindrical(p: npt.ArrayLike) -> np.ndarray: ... def cartesian_from_spherical(p: npt.ArrayLike) -> np.ndarray: ... def cylindrical_from_cartesian(p: npt.ArrayLike) -> np.ndarray: ... def cylindrical_from_spherical(p: npt.ArrayLike) -> np.ndarray: ... def spherical_from_cartesian(p: npt.ArrayLike) -> np.ndarray: ... def spherical_from_cylindrical(p: npt.ArrayLike) -> np.ndarray: ... ================================================ FILE: pytransform3d/editor.py ================================================ """Modify transformations visually.""" qt_available = False qt_version = None try: import PyQt5.QtCore as QtCore from PyQt5.QtWidgets import ( QApplication, QMainWindow, QWidget, QSlider, QDoubleSpinBox, QGridLayout, QLabel, QGroupBox, QHBoxLayout, QComboBox, QVBoxLayout, ) qt_available = True qt_version = 5 except ImportError: try: import PyQt4.QtCore as QtCore from PyQt4.QtGui import ( QApplication, QMainWindow, QWidget, QSlider, QDoubleSpinBox, QGridLayout, QLabel, QGroupBox, QHBoxLayout, QComboBox, QVBoxLayout, ) qt_available = True qt_version = 4 except ImportError: import warnings warnings.warn( "Cannot import PyQt. TransformEditor won't be available.", ImportWarning, stacklevel=2, ) TransformEditor = None if qt_available: import sys from functools import partial from contextlib import contextmanager import numpy as np from .transform_manager import TransformManager from .rotations import ( active_matrix_from_intrinsic_euler_xyz, intrinsic_euler_xyz_from_active_matrix, ) from .transformations import transform_from from mpl_toolkits.mplot3d import Axes3D # noqa: F401 from matplotlib.figure import Figure if qt_version == 5: from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg from matplotlib.backends.backend_qt5agg import NavigationToolbar2QT else: assert qt_version == 4 from matplotlib.backends.backend_qt4agg import FigureCanvasQTAgg from matplotlib.backends.backend_qt4agg import NavigationToolbar2QT @contextmanager def _block_signals(qobject): """Block signals of a QObject in this context.""" signals_blocked = qobject.blockSignals(True) try: yield qobject finally: qobject.blockSignals(signals_blocked) def _internal_repr(A2B): """Compute internal representation of transform.""" p = A2B[:3, 3] R = A2B[:3, :3] e = intrinsic_euler_xyz_from_active_matrix(R) return np.hstack((p, e)) class PositionEulerEditor(QWidget): """Frame editor that represents orientation by Euler angles (XY'Z''). Parameters ---------- base_frame : string Name of the base frame xlim : tuple Lower and upper limit for the x position. Defines the range of the plot and the range of the slider. ylim : tuple Lower and upper limit for the y position. Defines the range of the plot and the range of the slider. zlim : tuple Lower and upper limit for the z position. Defines the range of the plot and the range of the slider. parent : QWidget, optional (default: None) Parent widget. """ frameChanged = QtCore.pyqtSignal() def __init__(self, base_frame, xlim, ylim, zlim, parent=None): super(PositionEulerEditor, self).__init__(parent) self.dim_labels = ["x", "y", "z", "X", "Y'", "Z''"] self.limits = [ xlim, ylim, zlim, (-3.141, 3.141), (-1.570, 1.570), (-3.141, 3.141), ] self.n_slider_steps = [ int(100 * (upper - lower)) + 1 for lower, upper in self.limits ] self.setLayout(self._create(base_frame)) self.A2B = None def _create(self, base_frame): self.sliders = [] self.spinboxes = [] for i in range(len(self.dim_labels)): self.sliders.append(QSlider(QtCore.Qt.Horizontal)) self.sliders[i].setRange(0, self.n_slider_steps[i]) self.sliders[i].valueChanged.connect(partial(self._on_slide, i)) spinbox = QDoubleSpinBox() spinbox.setRange(*self.limits[i]) spinbox.setDecimals(3) spinbox.setSingleStep(0.001) self.spinboxes.append(spinbox) self.spinboxes[i].valueChanged.connect( partial(self._on_pos_edited, i) ) slider_group = QGridLayout() slider_group.addWidget( QLabel("Position"), 0, 0, 1, 3, QtCore.Qt.AlignCenter ) slider_group.addWidget( QLabel("Orientation (Euler angles)"), 0, 3, 1, 3, QtCore.Qt.AlignCenter, ) for i, slider in enumerate(self.sliders): slider_group.addWidget(QLabel(self.dim_labels[i]), 1, i) slider_group.addWidget(slider, 2, i) slider_group.addWidget(self.spinboxes[i], 3, i) slider_groupbox = QGroupBox( "Transformation in frame '%s'" % base_frame ) slider_groupbox.setLayout(slider_group) layout = QHBoxLayout() layout.addWidget(slider_groupbox) layout.addStretch(1) return layout def set_frame(self, A2B): """Set pose of frame. Parameters ---------- A2B : array Transformation matrix """ self.A2B = A2B pose = _internal_repr(self.A2B) for i in range(6): pos = self._pos_to_slider_pos(i, pose[i]) with _block_signals(self.sliders[i]) as slider: slider.setValue(pos) with _block_signals(self.spinboxes[i]) as spinbox: spinbox.setValue(pose[i]) def _on_pos_edited(self, dim, pos): """Slot: value in spinbox changed.""" pose = _internal_repr(self.A2B) pose[dim] = pos self.A2B = transform_from( active_matrix_from_intrinsic_euler_xyz(pose[3:]), pose[:3] ) for i in range(6): pos = self._pos_to_slider_pos(i, pose[i]) with _block_signals(self.sliders[i]) as slider: slider.setValue(pos) self.frameChanged.emit() def _on_slide(self, dim, step): """Slot: slider position changed.""" pose = _internal_repr(self.A2B) v = self._slider_pos_to_pos(dim, step) pose[dim] = v self.A2B = transform_from( active_matrix_from_intrinsic_euler_xyz(pose[3:]), pose[:3] ) self.spinboxes[dim].setValue(v) self.frameChanged.emit() def _pos_to_slider_pos(self, dim, pos): """Compute slider position from value.""" m = self.limits[dim][0] r = self.limits[dim][1] - m slider_pos = int((pos - m) / r * self.n_slider_steps[dim]) slider_pos = np.clip(slider_pos, 0, self.n_slider_steps[dim]) return slider_pos def _slider_pos_to_pos(self, dim, slider_pos): """Create value from slider position.""" m = self.limits[dim][0] r = self.limits[dim][1] - m return m + r * float(slider_pos) / float(self.n_slider_steps[dim]) class TransformEditor(QMainWindow): """GUI to edit transformations. .. warning:: Note that this module requires PyQt4. Parameters ---------- transform_manager : TransformManager All nodes that are reachable from the base frame will be editable frame : string Name of the base frame xlim : tuple, optional (-1, 1) Lower and upper limit for the x position. Defines the range of the plot and the range of the slider. ylim : tuple, optional (-1, 1) Lower and upper limit for the y position. Defines the range of the plot and the range of the slider. zlim : tuple, optional (-1, 1) Lower and upper limit for the z position. Defines the range of the plot and the range of the slider. s : float, optional (default: 1) Scaling of the axis and angle that will be drawn figsize : tuple of integers, optional (default: (10, 10)) Width, height in inches. dpi : integer, optional (default: 100) Resolution of the figure. parent : QWidget, optional (default: None) Parent widget. Attributes ---------- transform_manager : TransformManager Result, all frames are expressed in the base frame """ def __init__( self, transform_manager, base_frame, xlim=(-1.0, 1.0), ylim=(-1.0, 1.0), zlim=(-1.0, 1.0), s=1.0, figsize=(10, 10), dpi=100, window_size=(500, 600), parent=None, ): self.app = QApplication(sys.argv) super(TransformEditor, self).__init__(parent) self.transform_manager = self._init_transform_manager( transform_manager, base_frame ) self.base_frame = base_frame self.xlim = xlim self.ylim = ylim self.zlim = zlim self.s = s self.figsize = figsize self.dpi = dpi self.window_size = window_size self.setWindowTitle("Transformation Editor") self.canvas = None self.fig = None self.axis = None self._create_main_frame() self._on_node_changed(0) def _init_transform_manager(self, transform_manager, frame): """Transform all nodes into the reference frame.""" tm = TransformManager() if frame not in transform_manager.nodes: raise KeyError("Unknown frame '%s'" % frame) for node in transform_manager.nodes: try: node2frame = transform_manager.get_transform(node, frame) tm.add_transform(node, frame, node2frame) except KeyError: pass # Frame is not connected to the reference frame return tm def _create_main_frame(self): """Create main frame and layout.""" self.main_frame = QWidget() self.frame_editor = PositionEulerEditor( self.base_frame, self.xlim, self.ylim, self.zlim ) self.frame_editor.frameChanged.connect(self._on_update) self.frame_selection = self._create_frame_selector() plot = self._create_plot() vbox = QVBoxLayout() vbox.addWidget(self.frame_editor) vbox.addWidget(self.frame_selection) vbox.addWidget(plot) main_layout = QHBoxLayout() main_layout.addLayout(vbox) self.main_frame.setLayout(main_layout) self.setCentralWidget(self.main_frame) self.setGeometry(0, 0, *self.window_size) def _create_frame_selector(self): frame_selection = QComboBox() for node in self.transform_manager.nodes: if node != self.base_frame: frame_selection.addItem(node) frame_selection.activated.connect(self._on_node_changed) return frame_selection def _create_plot(self): plot = QWidget() canvas_group = QGridLayout() self.fig = Figure(self.figsize, dpi=self.dpi) self.fig.subplots_adjust(left=0, right=1, bottom=0, top=1) self.canvas = FigureCanvasQTAgg(self.fig) self.canvas.setParent(self.main_frame) canvas_group.addWidget(self.canvas, 1, 0) mpl_toolbar = NavigationToolbar2QT(self.canvas, self.main_frame) canvas_group.addWidget(mpl_toolbar, 2, 0) plot.setLayout(canvas_group) return plot def _on_node_changed(self, node_idx): """Slot: manipulatable node changed.""" self.node = self.frame_selection.itemText(node_idx) A2B = self.transform_manager.get_transform( self.node, self.base_frame ) self.frame_editor.set_frame(A2B) self._plot() def _on_update(self): """Slot: transformation changed.""" self.transform_manager.add_transform( self.node, self.base_frame, self.frame_editor.A2B ) self._plot() def _plot(self): """Draw plot.""" if self.axis is None: elev, azim = 30, 60 else: elev, azim = self.axis.elev, self.axis.azim self.fig.delaxes(self.axis) self.axis = self.fig.add_subplot(111, projection="3d") self.axis.view_init(elev, azim) self.axis.set_xlim(self.xlim) self.axis.set_ylim(self.ylim) self.axis.set_zlim(self.zlim) p = self.transform_manager.get_transform( self.node, self.base_frame )[:3, 3] self.axis.scatter(p[0], p[1], p[2], s=100) self.transform_manager.plot_frames_in( self.base_frame, ax=self.axis, s=self.s ) self.canvas.draw() def show(self): """Start GUI.""" super(TransformEditor, self).show() self.app.exec_() ================================================ FILE: pytransform3d/plot_utils/__init__.py ================================================ """Utilities for plotting.""" import warnings try: import matplotlib.pyplot as plt # noqa: F401 from ._artists import ( Arrow3D, Frame, LabeledFrame, Trajectory, Camera, ) from ._layout import ( make_3d_axis, remove_frame, ) from ._plot_geometries import ( plot_box, plot_sphere, plot_spheres, plot_cylinder, plot_mesh, plot_ellipsoid, plot_capsule, plot_cone, ) from ._plot_helpers import ( plot_vector, plot_length_variable, ) __all__ = [ "Arrow3D", "Frame", "LabeledFrame", "Trajectory", "Camera", "make_3d_axis", "remove_frame", "plot_box", "plot_sphere", "plot_spheres", "plot_cylinder", "plot_mesh", "plot_ellipsoid", "plot_capsule", "plot_cone", "plot_vector", "plot_length_variable", ] except ImportError as e: if e.name == "matplotlib": warnings.warn( "Matplotlib is not installed, visualization is not " "available", ImportWarning, stacklevel=2, ) else: raise e ================================================ FILE: pytransform3d/plot_utils/_artists.py ================================================ """Matplotlib artists.""" import numpy as np from matplotlib import artist from matplotlib.patches import FancyArrowPatch from mpl_toolkits.mplot3d import proj3d from mpl_toolkits.mplot3d.art3d import Line3D, Text3D class Frame(artist.Artist): """A Matplotlib artist that displays a frame represented by its basis. Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B label : str, optional (default: None) Name of the frame s : float, optional (default: 1) Length of basis vectors draw_label_indicator : bool, optional (default: True) Controls whether the line from the frame origin to frame label is drawn. Other arguments except 'c' and 'color' are passed on to Line3D. """ def __init__(self, A2B, label=None, s=1.0, **kwargs): super(Frame, self).__init__() if "c" in kwargs: kwargs.pop("c") if "color" in kwargs: kwargs.pop("color") self.draw_label_indicator = kwargs.pop("draw_label_indicator", True) self.s = s self.x_axis = Line3D([], [], [], color="r", **kwargs) self.y_axis = Line3D([], [], [], color="g", **kwargs) self.z_axis = Line3D([], [], [], color="b", **kwargs) self.draw_label = label is not None self.label = label if self.draw_label: if self.draw_label_indicator: self.label_indicator = Line3D([], [], [], color="k", **kwargs) self.label_text = Text3D(0, 0, 0, text="", zdir="x") self.set_data(A2B, label) def set_data(self, A2B, label=None): """Set the transformation data. Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B label : str, optional (default: None) Name of the frame """ R = A2B[:3, :3] p = A2B[:3, 3] for d, b in enumerate([self.x_axis, self.y_axis, self.z_axis]): b.set_data( np.array([p[0], p[0] + self.s * R[0, d]]), np.array([p[1], p[1] + self.s * R[1, d]]), ) b.set_3d_properties(np.array([p[2], p[2] + self.s * R[2, d]])) if self.draw_label: if label is None: label = self.label label_pos = p + 0.5 * self.s * (R[:, 0] + R[:, 1] + R[:, 2]) if self.draw_label_indicator: self.label_indicator.set_data( np.array([p[0], label_pos[0]]), np.array([p[1], label_pos[1]]), ) self.label_indicator.set_3d_properties( np.array([p[2], label_pos[2]]) ) self.label_text.set_text(label) self.label_text.set_position([label_pos[0], label_pos[1]]) self.label_text.set_3d_properties(label_pos[2], zdir="x") @artist.allow_rasterization def draw(self, renderer, *args, **kwargs): """Draw the artist.""" for b in [self.x_axis, self.y_axis, self.z_axis]: b.draw(renderer, *args, **kwargs) if self.draw_label: if self.draw_label_indicator: self.label_indicator.draw(renderer, *args, **kwargs) self.label_text.draw(renderer, *args, **kwargs) super(Frame, self).draw(renderer, *args, **kwargs) def add_frame(self, axis): """Add the frame to a 3D axis.""" for b in [self.x_axis, self.y_axis, self.z_axis]: axis.add_line(b) if self.draw_label: if self.draw_label_indicator: axis.add_line(self.label_indicator) axis._add_text(self.label_text) class LabeledFrame(Frame): """Displays a frame represented by its basis with axis labels. Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B label : str, optional (default: None) Name of the frame s : float, optional (default: 1) Length of basis vectors draw_label_indicator : bool, optional (default: True) Controls whether the line from the frame origin to frame label is drawn. Other arguments except 'c' and 'color' are passed on to Line3D. """ def __init__(self, A2B, label=None, s=1.0, **kwargs): self.x_label = Text3D(0, 0, 0, text="", zdir="x") self.y_label = Text3D(0, 0, 0, text="", zdir="x") self.z_label = Text3D(0, 0, 0, text="", zdir="x") super(LabeledFrame, self).__init__(A2B, label=label, s=s, **kwargs) def set_data(self, A2B, label=None): """Set the transformation data. Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B label : str, optional (default: None) Name of the frame """ super(LabeledFrame, self).set_data(A2B, label) R = A2B[:3, :3] p = A2B[:3, 3] x_label_location = p + 1.1 * self.s * R[:, 0] y_label_location = p + 1.1 * self.s * R[:, 1] z_label_location = p + 1.1 * self.s * R[:, 2] self.x_label.set_text("x") self.x_label.set_position(x_label_location[:2]) self.x_label.set_3d_properties(x_label_location[2], zdir="x") self.y_label.set_text("y") self.y_label.set_position(y_label_location[:2]) self.y_label.set_3d_properties(y_label_location[2], zdir="x") self.z_label.set_text("z") self.z_label.set_position(z_label_location[:2]) self.z_label.set_3d_properties(z_label_location[2], zdir="x") @artist.allow_rasterization def draw(self, renderer, *args, **kwargs): """Draw the artist.""" self.x_label.draw(renderer, *args, **kwargs) self.y_label.draw(renderer, *args, **kwargs) self.z_label.draw(renderer, *args, **kwargs) super(LabeledFrame, self).draw(renderer, *args, **kwargs) def add_frame(self, axis): """Add the frame to a 3D axis.""" super(LabeledFrame, self).add_frame(axis) axis._add_text(self.x_label) axis._add_text(self.y_label) axis._add_text(self.z_label) class Trajectory(artist.Artist): """A Matplotlib artist that displays a trajectory. Parameters ---------- H : array-like, shape (n_steps, 4, 4) Sequence of poses represented by homogeneous matrices show_direction : bool, optional (default: True) Plot an arrow to indicate the direction of the trajectory n_frames : int, optional (default: 10) Number of frames that should be plotted to indicate the rotation s : float, optional (default: 1) Scaling of the frames that will be drawn label : str, optional (default: None) Label of the trajectory Other arguments are passed onto Line3D. """ def __init__( self, H, show_direction=True, n_frames=10, s=1.0, label=None, **kwargs ): super(Trajectory, self).__init__() self.show_direction = show_direction self.trajectory = Line3D([], [], [], label=label, **kwargs) self.key_frames = [ Frame(np.eye(4), s=s, **kwargs) for _ in range(n_frames) ] if self.show_direction: self.direction_arrow = Arrow3D( [0, 0], [0, 0], [0, 0], mutation_scale=20, lw=1, arrowstyle="-|>", color="k", ) self.set_data(H) def set_data(self, H): """Set the trajectory data. Parameters ---------- H : array-like, shape (n_steps, 4, 4) Sequence of poses represented by homogeneous matrices """ positions = H[:, :3, 3] self.trajectory.set_data(positions[:, 0], positions[:, 1]) self.trajectory.set_3d_properties(positions[:, 2]) key_frames_indices = np.linspace( 0, len(H) - 1, len(self.key_frames), dtype=np.int64 ) for i, key_frame_idx in enumerate(key_frames_indices): self.key_frames[i].set_data(H[key_frame_idx]) if self.show_direction: start = 0.8 * positions[0] + 0.2 * positions[-1] end = 0.2 * positions[0] + 0.8 * positions[-1] self.direction_arrow.set_data( [start[0], end[0]], [start[1], end[1]], [start[2], end[2]] ) @artist.allow_rasterization def draw(self, renderer, *args, **kwargs): """Draw the artist.""" self.trajectory.draw(renderer, *args, **kwargs) for key_frame in self.key_frames: key_frame.draw(renderer, *args, **kwargs) if self.show_direction: self.direction_arrow.draw(renderer) super(Trajectory, self).draw(renderer, *args, **kwargs) def add_trajectory(self, axis): """Add the trajectory to a 3D axis.""" axis.add_line(self.trajectory) for key_frame in self.key_frames: key_frame.add_frame(axis) if self.show_direction: axis.add_artist(self.direction_arrow) class Arrow3D(FancyArrowPatch): """A Matplotlib patch that represents an arrow in 3D. Source: http://stackoverflow.com/a/11156353/915743 """ def __init__(self, xs, ys, zs, *args, **kwargs): super(Arrow3D, self).__init__((0, 0), (0, 0), *args, **kwargs) self._verts3d = xs, ys, zs def set_data(self, xs, ys, zs): """Set the arrow data. Parameters ---------- xs : iterable List of x positions ys : iterable List of y positions zs : iterable List of z positions """ self._verts3d = xs, ys, zs def draw(self, renderer): """Draw the patch.""" xs3d, ys3d, zs3d = self._verts3d try: M = self.axes.M except AttributeError: # deprecated since matplotlib 3.4, will be removed in 3.6 M = renderer.M xs, ys, zs = proj3d.proj_transform(xs3d, ys3d, zs3d, M) self.set_positions((xs[0], ys[0]), (xs[1], ys[1])) super(Arrow3D, self).draw(renderer) def do_3d_projection(self, renderer=None): # This supports both matplotlib 3.4 and 3.5 return 0 class Camera(artist.Artist): """A Matplotlib artist that displays a camera. This function is inspired by Blender's camera visualization. It will show the camera center, a virtual image plane, and the top of the virtual image plane. Parameters ---------- M : array-like, shape (3, 3) Intrinsic camera matrix that contains the focal lengths on the diagonal and the center of the the image in the last column. It does not matter whether values are given in meters or pixels as long as the unit is the same as for the sensor size. cam2world : array-like, shape (4, 4) Transformation matrix of camera in world frame. We assume that the position is given in meters. virtual_image_distance : float, optional (default: 1) Distance from pinhole to virtual image plane that will be displayed. We assume that this distance is given in meters. sensor_size : array-like, shape (2,), optional (default: [1920, 1080]) Size of the image sensor: (width, height). It does not matter whether values are given in meters or pixels as long as the unit is the same as for the sensor size. kwargs : dict, optional (default: {}) Additional arguments for the plotting functions, e.g. alpha. """ def __init__( self, M, cam2world, virtual_image_distance=1.0, sensor_size=(1920, 1080), **kwargs, ): super(Camera, self).__init__() if "c" in kwargs: color = kwargs.pop("c") elif "color" in kwargs: color = kwargs.pop("color") else: color = "k" self.sensor_corners = _calculate_sensor_corners_in_camera( M, virtual_image_distance, sensor_size ) self.top_corners = _calculate_top_corners_in_camera(self.sensor_corners) self.lines_sensor = [ Line3D([], [], [], color=color, **kwargs) for _ in range(4) ] self.line_top = Line3D([], [], [], color=color, **kwargs) self.set_data(cam2world) def set_data(self, cam2world): """Set the transformation data. Parameters ---------- cam2world : array-like, shape (4, 4) Transform from frame A to frame B """ cam2world = np.asarray(cam2world) sensor_in_world = np.dot( cam2world, np.vstack( (self.sensor_corners.T, np.ones(len(self.sensor_corners))) ), ) for i in range(4): xs, ys, zs = [ [ cam2world[j, 3], sensor_in_world[j, i], sensor_in_world[j, (i + 1) % 4], ] for j in range(3) ] self.lines_sensor[i].set_data_3d(xs, ys, zs) top_in_world = np.dot( cam2world, np.vstack((self.top_corners.T, np.ones(len(self.top_corners)))), ) xs, ys, zs, _ = top_in_world self.line_top.set_data_3d(xs, ys, zs) @artist.allow_rasterization def draw(self, renderer, *args, **kwargs): """Draw the artist.""" for b in self.lines_sensor: b.draw(renderer, *args, **kwargs) self.line_top.draw(renderer, *args, **kwargs) super(Camera, self).draw(renderer, *args, **kwargs) def add_camera(self, axis): """Add the camera to a 3D axis.""" for b in self.lines_sensor: axis.add_line(b) axis.add_line(self.line_top) def _calculate_sensor_corners_in_camera(M, virtual_image_distance, sensor_size): """Calculate the corners of the sensor frame in camera coordinates.""" focal_length = np.mean((M[0, 0], M[1, 1])) sensor_corners = np.array( [ [0, 0, focal_length], [0, sensor_size[1], focal_length], [sensor_size[0], sensor_size[1], focal_length], [sensor_size[0], 0, focal_length], ] ) sensor_corners[:, 0] -= M[0, 2] sensor_corners[:, 1] -= M[1, 2] return virtual_image_distance / focal_length * sensor_corners def _calculate_top_corners_in_camera(sensor_corners): """Calculate the corners of the top triangle in camera coordinates.""" up = sensor_corners[0] - sensor_corners[1] return np.array( [ sensor_corners[0] + 0.1 * up, 0.5 * (sensor_corners[0] + sensor_corners[3]) + 0.5 * up, sensor_corners[3] + 0.1 * up, sensor_corners[0] + 0.1 * up, ] ) ================================================ FILE: pytransform3d/plot_utils/_artists.pyi ================================================ import numpy as np import numpy.typing as npt from mpl_toolkits.mplot3d import Axes3D from mpl_toolkits.mplot3d.art3d import Line3D from matplotlib import artist from matplotlib.patches import FancyArrowPatch from matplotlib.backend_bases import RendererBase from typing import Union, List class Frame(artist.Artist): def __init__( self, A2B: npt.ArrayLike, label: Union[str, None] = ..., s: float = ..., **kwargs, ): ... def set_data(self, A2B: npt.ArrayLike, label: Union[str, None] = ...): ... @artist.allow_rasterization def draw(self, renderer: RendererBase, *args, **kwargs): ... def add_frame(self, axis: Axes3D): ... class LabeledFrame(Frame): def __init__( self, A2B: npt.ArrayLike, label: Union[str, None] = ..., s: float = ..., **kwargs, ): ... def set_data(self, A2B: npt.ArrayLike, label=None): ... @artist.allow_rasterization def draw(self, renderer: RendererBase, *args, **kwargs): ... def add_frame(self, axis: Axes3D): ... class Trajectory(artist.Artist): trajectory: Line3D def __init__( self, H: npt.ArrayLike, show_direction: bool = ..., n_frames: int = ..., s: float = ..., label: Union[str, None] = ..., **kwargs, ): ... def set_data(self, H: npt.ArrayLike): ... @artist.allow_rasterization def draw(self, renderer: RendererBase, *args, **kwargs): ... def add_trajectory(self, axis: Axes3D): ... class Arrow3D(FancyArrowPatch): def __init__( self, xs: npt.ArrayLike, ys: npt.ArrayLike, zs: npt.ArrayLike, *args, **kwargs, ): ... def set_data( self, xs: npt.ArrayLike, ys: npt.ArrayLike, zs: npt.ArrayLike ): ... def draw(self, renderer: RendererBase): ... class Camera(artist.Artist): sensor_corners: np.ndarray top_corners: np.ndarray lines_sensor: List[Line3D] line_top: Line3D def __init__( self, M: npt.ArrayLike, cam2world: npt.ArrayLike, virtual_image_distance: float = ..., sensor_size: npt.ArrayLike = ..., **kwargs, ): ... def set_data(self, cam2world: npt.ArrayLike): ... @artist.allow_rasterization def draw(self, renderer: RendererBase, *args, **kwargs): ... def add_camera(self, axis: Axes3D): ... def _calculate_sensor_corners_in_camera( M: npt.ArrayLike, virtual_image_distance: float, sensor_size: npt.ArrayLike ) -> npt.ArrayLike: ... def _calculate_top_corners_in_camera( sensor_corners: npt.ArrayLike, ) -> npt.ArrayLike: ... ================================================ FILE: pytransform3d/plot_utils/_layout.py ================================================ """Layout utilities for matplotlib.""" import matplotlib.pyplot as plt from matplotlib.ticker import MaxNLocator def make_3d_axis(ax_s, pos=111, unit=None, n_ticks=5): """Generate new 3D axis. Parameters ---------- ax_s : float, optional (default: 1) Scaling of the new matplotlib 3d axis pos : int, optional (default: 111) Position indicator (nrows, ncols, plot_number) unit : str, optional (default: None) Unit of axes. For example, 'm', 'cm', 'km', ... The unit will be shown in the axis label, for example, as 'X [m]'. n_ticks : int, optional (default: 5) Number of ticks on each axis Returns ------- ax : Matplotlib 3d axis New axis """ try: ax = plt.subplot(pos, projection="3d", aspect="equal") except NotImplementedError: # HACK: workaround for bug in new matplotlib versions (ca. 3.02): # "It is not currently possible to manually set the aspect" ax = plt.subplot(pos, projection="3d") if unit is None: xlabel = "X" ylabel = "Y" zlabel = "Z" else: xlabel = "X [%s]" % unit ylabel = "Y [%s]" % unit zlabel = "Z [%s]" % unit plt.setp( ax, xlim=(-ax_s, ax_s), ylim=(-ax_s, ax_s), zlim=(-ax_s, ax_s), xlabel=xlabel, ylabel=ylabel, zlabel=zlabel, ) for axis in [ax.xaxis, ax.yaxis, ax.zaxis]: axis.set_major_locator(MaxNLocator(n_ticks)) try: for axis in [ax.xaxis, ax.yaxis, ax.zaxis]: axis.pane.set_color("white") except AttributeError: # pragma: no cover # fallback for older versions of matplotlib, deprecated since v3.1 for axis in [ax.w_xaxis, ax.w_yaxis, ax.w_zaxis]: axis.pane.set_color("white") return ax def remove_frame(ax, left=0.0, bottom=0.0, right=1.0, top=1.0): """Remove axis and scale bbox. Parameters ---------- ax : Matplotlib 3d axis Axis from which we remove the frame left : float, optional (default: 0) Position of left border (between 0 and 1) bottom : float, optional (default: 0) Position of bottom border (between 0 and 1) right : float, optional (default: 1) Position of right border (between 0 and 1) top : float, optional (default: 1) Position of top border (between 0 and 1) """ ax.axis("off") plt.subplots_adjust(left=left, bottom=bottom, right=right, top=top) ================================================ FILE: pytransform3d/plot_utils/_layout.pyi ================================================ from typing import Union from mpl_toolkits.mplot3d import Axes3D def make_3d_axis( ax_s: float, pos: int = ..., unit: Union[str, None] = ..., n_ticks: int = ..., ) -> Axes3D: ... def remove_frame( ax: Axes3D, left: float = ..., bottom: float = ..., right: float = ..., top: float = ..., ): ... ================================================ FILE: pytransform3d/plot_utils/_plot_geometries.py ================================================ """Plotting functions for geometries.""" import warnings import numpy as np from mpl_toolkits.mplot3d.art3d import Poly3DCollection, Line3DCollection from ._layout import make_3d_axis from .._geometry import unit_sphere_surface_grid, transform_surface from .._mesh_loader import load_mesh from ..transformations import transform def plot_box( ax=None, size=np.ones(3), A2B=np.eye(4), ax_s=1, wireframe=True, color="k", alpha=1.0, linewidth=1.0, ): """Plot box. Parameters ---------- ax : Matplotlib 3d axis, optional (default: None) If the axis is None, a new 3d axis will be created size : array-like, shape (3,), optional (default: [1, 1, 1]) Size of the box per dimension A2B : array-like, shape (4, 4) Center of the box ax_s : float, optional (default: 1) Scaling of the new matplotlib 3d axis wireframe : bool, optional (default: True) Plot wireframe of box and surface otherwise color : str, optional (default: black) Color in which the box should be plotted alpha : float, optional (default: 1) Alpha value of the mesh that will be plotted linewidth : float, optional (default: 1.0) Line width for wireframe plot Returns ------- ax : Matplotlib 3d axis New or old axis """ if ax is None: ax = make_3d_axis(ax_s) vertices = [ [0, 0, 0], [0, 0, 1], [0, 1, 0], [0, 1, 1], [1, 0, 0], [1, 0, 1], [1, 1, 0], [1, 1, 1], ] vertices = (np.array(vertices, dtype=float) - 0.5) * size vertices = np.hstack((vertices, np.ones((len(vertices), 1)))) vertices = transform(A2B, vertices)[:, :3] if wireframe: connections = [ [0, 1], [0, 2], [1, 3], [2, 3], [4, 5], [4, 6], [5, 7], [6, 7], [0, 4], [1, 5], [2, 6], [3, 7], ] surface = Line3DCollection(vertices[connections], linewidth=linewidth) surface.set_color(color) else: faces = [ [0, 1, 2], [1, 2, 3], [4, 5, 6], [5, 6, 7], [0, 1, 4], [1, 4, 5], [2, 6, 7], [2, 3, 7], [0, 4, 6], [0, 2, 6], [1, 5, 7], [1, 3, 7], ] surface = Poly3DCollection(vertices[faces]) surface.set_facecolor(color) surface.set_alpha(alpha) ax.add_collection3d(surface) return ax def plot_sphere( ax=None, radius=1.0, p=np.zeros(3), ax_s=1, wireframe=True, n_steps=20, alpha=1.0, color="k", linewidth=1.0, ): """Plot sphere. Parameters ---------- ax : Matplotlib 3d axis, optional (default: None) If the axis is None, a new 3d axis will be created radius : float, optional (default: 1) Radius of the sphere p : array-like, shape (3,), optional (default: [0, 0, 0]) Center of the sphere ax_s : float, optional (default: 1) Scaling of the new matplotlib 3d axis wireframe : bool, optional (default: True) Plot wireframe of sphere and surface otherwise n_steps : int, optional (default: 20) Number of discrete steps plotted in each dimension alpha : float, optional (default: 1) Alpha value of the sphere that will be plotted color : str, optional (default: black) Color in which the sphere should be plotted linewidth : float, optional (default: 1.0) Line width for wireframe plot Returns ------- ax : Matplotlib 3d axis New or old axis """ if ax is None: ax = make_3d_axis(ax_s) x, y, z = unit_sphere_surface_grid(n_steps) x = p[0] + radius * x y = p[1] + radius * y z = p[2] + radius * z if wireframe: ax.plot_wireframe( x, y, z, rstride=2, cstride=2, color=color, alpha=alpha, linewidth=linewidth, ) else: ax.plot_surface(x, y, z, color=color, alpha=alpha, linewidth=0) return ax def plot_spheres( ax=None, radius=np.ones(1), p=np.zeros((1, 3)), ax_s=1, wireframe=True, n_steps=20, alpha=np.ones(1), color=np.zeros((1, 3)), linewidth=1.0, ): """Plot multiple spheres. Parameters ---------- ax : Matplotlib 3d axis, optional (default: None) If the axis is None, a new 3d axis will be created radius : array-like, shape (n_spheres,), optional (default: 1) Radius of the sphere(s) p : array-like, shape (n_spheres, 3), optional (default: [0, 0, 0]) Center of the sphere(s) ax_s : float, optional (default: 1) Scaling of the new matplotlib 3d axis wireframe : bool, optional (default: True) Plot wireframe of sphere(s) and surface otherwise n_steps : int, optional (default: 20) Number of discrete steps plotted in each dimension alpha : array-like, shape (n_spheres,), optional (default: 1) Alpha value of the sphere(s) that will be plotted color : array-like, shape (n_spheres, 3), optional (default: black) Color in which the sphere(s) should be plotted linewidth : float, optional (default: 1.0) Line width for wireframe plot Returns ------- ax : Matplotlib 3d axis New or old axis """ if ax is None: ax = make_3d_axis(ax_s) radius = np.asarray(radius) p = np.asarray(p) phi, theta = np.mgrid[ 0.0 : np.pi : n_steps * 1j, 0.0 : 2.0 * np.pi : n_steps * 1j ] sin_phi = np.sin(phi) verts = ( radius[..., np.newaxis, np.newaxis, np.newaxis] * np.array( [sin_phi * np.cos(theta), sin_phi * np.sin(theta), np.cos(phi)] )[np.newaxis, ...] + p[..., np.newaxis, np.newaxis] ) colors = np.resize(color, (len(verts), 3)) alphas = np.resize(alpha, len(verts)) for verts_i, color_i, alpha_i in zip(verts, colors, alphas): if wireframe: ax.plot_wireframe( *verts_i, rstride=2, cstride=2, color=color_i, alpha=alpha_i, linewidth=linewidth, ) else: ax.plot_surface(*verts_i, color=color_i, alpha=alpha_i, linewidth=0) return ax def plot_cylinder( ax=None, length=1.0, radius=1.0, thickness=0.0, A2B=np.eye(4), ax_s=1, wireframe=True, n_steps=100, alpha=1.0, color="k", linewidth=1.0, ): """Plot cylinder. A cylinder is the volume covered by a disk moving along a line segment. Parameters ---------- ax : Matplotlib 3d axis, optional (default: None) If the axis is None, a new 3d axis will be created length : float, optional (default: 1) Length of the cylinder radius : float, optional (default: 1) Radius of the cylinder thickness : float, optional (default: 0) Thickness of a cylindrical shell. It will be subtracted from the outer radius to obtain the inner radius. The difference must be greater than 0. A2B : array-like, shape (4, 4) Center of the cylinder ax_s : float, optional (default: 1) Scaling of the new matplotlib 3d axis wireframe : bool, optional (default: True) Plot wireframe of cylinder and surface otherwise n_steps : int, optional (default: 100) Number of discrete steps plotted in each dimension alpha : float, optional (default: 1) Alpha value of the cylinder that will be plotted color : str, optional (default: black) Color in which the cylinder should be plotted linewidth : float, optional (default: 1.0) Line width for wireframe plot Returns ------- ax : Matplotlib 3d axis New or old axis Raises ------ ValueError If thickness is <= 0 """ if ax is None: ax = make_3d_axis(ax_s) inner_radius = radius - thickness if inner_radius <= 0.0: raise ValueError( "Thickness of cylindrical shell results in " "invalid inner radius: %g" % inner_radius ) if wireframe: t = np.linspace(0, length, n_steps) else: t = np.array([0, length]) angles = np.linspace(0, 2 * np.pi, n_steps) t, angles = np.meshgrid(t, angles) A2B = np.asarray(A2B) axis_start = np.dot(A2B, [0, 0, -0.5 * length, 1])[:3] X, Y, Z = _elongated_circular_grid(axis_start, A2B, t, radius, angles) if thickness > 0.0: A2B_left_hand = np.copy(A2B) A2B_left_hand[:3, 2] *= -1.0 axis_end = np.dot(A2B, [0, 0, 0.5 * length, 1])[:3] X_inner, Y_inner, Z_inner = _elongated_circular_grid( axis_end, A2B_left_hand, t, inner_radius, angles ) X = np.hstack((X, X_inner)) Y = np.hstack((Y, Y_inner)) Z = np.hstack((Z, Z_inner)) if wireframe: ax.plot_wireframe( X, Y, Z, rstride=10, cstride=10, alpha=alpha, color=color, linewidth=linewidth, ) else: ax.plot_surface(X, Y, Z, color=color, alpha=alpha, linewidth=0) return ax def plot_mesh( ax=None, filename=None, A2B=np.eye(4), s=np.array([1.0, 1.0, 1.0]), ax_s=1, wireframe=False, convex_hull=False, alpha=1.0, color="k", linewidth=1.0, ): """Plot mesh. Note that this function requires the additional library to load meshes such as trimesh or open3d. Parameters ---------- ax : Matplotlib 3d axis, optional (default: None) If the axis is None, a new 3d axis will be created filename : str, optional (default: None) Path to mesh file. A2B : array-like, shape (4, 4) Pose of the mesh s : array-like, shape (3,), optional (default: [1, 1, 1]) Scaling of the mesh that will be drawn ax_s : float, optional (default: 1) Scaling of the new matplotlib 3d axis wireframe : bool, optional (default: True) Plot wireframe of mesh and surface otherwise convex_hull : bool, optional (default: False) Show convex hull instead of the original mesh. This can be much faster. alpha : float, optional (default: 1) Alpha value of the mesh that will be plotted color : str, optional (default: black) Color in which the mesh should be plotted linewidth : float, optional (default: 1.0) Line width for wireframe plot Returns ------- ax : Matplotlib 3d axis New or old axis """ if ax is None: ax = make_3d_axis(ax_s) if filename is None: warnings.warn( "No filename given for mesh. When you use the " "UrdfTransformManager, make sure to set the mesh path or " "package directory.", UserWarning, stacklevel=2, ) return ax mesh = load_mesh(filename) if convex_hull: mesh.convex_hull() vertices = np.asarray(mesh.vertices) * s vertices = np.hstack((vertices, np.ones((len(vertices), 1)))) vertices = transform(A2B, vertices)[:, :3] faces = vertices[mesh.triangles] if wireframe: surface = Line3DCollection(faces, linewidth=linewidth) surface.set_color(color) else: surface = Poly3DCollection(faces) surface.set_facecolor(color) surface.set_alpha(alpha) ax.add_collection3d(surface) return ax def plot_ellipsoid( ax=None, radii=np.ones(3), A2B=np.eye(4), ax_s=1, wireframe=True, n_steps=20, alpha=1.0, color="k", linewidth=1.0, ): """Plot ellipsoid. Parameters ---------- ax : Matplotlib 3d axis, optional (default: None) If the axis is None, a new 3d axis will be created radii : array-like, shape (3,) Radii along the x-axis, y-axis, and z-axis of the ellipsoid. A2B : array-like, shape (4, 4) Transform from frame A to frame B ax_s : float, optional (default: 1) Scaling of the new matplotlib 3d axis wireframe : bool, optional (default: True) Plot wireframe of ellipsoid and surface otherwise n_steps : int, optional (default: 20) Number of discrete steps plotted in each dimension alpha : float, optional (default: 1) Alpha value of the ellipsoid that will be plotted color : str, optional (default: black) Color in which the ellipsoid should be plotted linewidth : float, optional (default: 1.0) Line width for wireframe plot Returns ------- ax : Matplotlib 3d axis New or old axis """ if ax is None: ax = make_3d_axis(ax_s) radius_x, radius_y, radius_z = radii x, y, z = unit_sphere_surface_grid(n_steps) x *= radius_x y *= radius_y z *= radius_z x, y, z = transform_surface(A2B, x, y, z) if wireframe: ax.plot_wireframe( x, y, z, rstride=2, cstride=2, color=color, alpha=alpha, linewidth=linewidth, ) else: ax.plot_surface(x, y, z, color=color, alpha=alpha, linewidth=0) return ax def plot_capsule( ax=None, A2B=np.eye(4), height=1.0, radius=1.0, ax_s=1, wireframe=True, n_steps=20, alpha=1.0, color="k", linewidth=1.0, ): """Plot capsule. A capsule is the volume covered by a sphere moving along a line segment. Parameters ---------- ax : Matplotlib 3d axis, optional (default: None) If the axis is None, a new 3d axis will be created A2B : array-like, shape (4, 4) Frame of the capsule, located at the center of the line segment. height : float, optional (default: 1) Height of the capsule along its z-axis. radius : float, optional (default: 1) Radius of the capsule. ax_s : float, optional (default: 1) Scaling of the new matplotlib 3d axis wireframe : bool, optional (default: True) Plot wireframe of capsule and surface otherwise n_steps : int, optional (default: 20) Number of discrete steps plotted in each dimension alpha : float, optional (default: 1) Alpha value of the mesh that will be plotted color : str, optional (default: black) Color in which the capsule should be plotted linewidth : float, optional (default: 1.0) Line width for wireframe plot Returns ------- ax : Matplotlib 3d axis New or old axis """ if ax is None: ax = make_3d_axis(ax_s) x, y, z = unit_sphere_surface_grid(n_steps) x *= radius y *= radius z *= radius z[len(z) // 2 :] -= 0.5 * height z[: len(z) // 2] += 0.5 * height x, y, z = transform_surface(A2B, x, y, z) if wireframe: ax.plot_wireframe( x, y, z, rstride=2, cstride=2, color=color, alpha=alpha, linewidth=linewidth, ) else: ax.plot_surface(x, y, z, color=color, alpha=alpha, linewidth=0) return ax def plot_cone( ax=None, height=1.0, radius=1.0, A2B=np.eye(4), ax_s=1, wireframe=True, n_steps=20, alpha=1.0, color="k", linewidth=1.0, ): """Plot cone. Parameters ---------- ax : Matplotlib 3d axis, optional (default: None) If the axis is None, a new 3d axis will be created height : float, optional (default: 1) Height of the cone radius : float, optional (default: 1) Radius of the cone A2B : array-like, shape (4, 4) Center of the cone ax_s : float, optional (default: 1) Scaling of the new matplotlib 3d axis wireframe : bool, optional (default: True) Plot wireframe of cone and surface otherwise n_steps : int, optional (default: 100) Number of discrete steps plotted in each dimension alpha : float, optional (default: 1) Alpha value of the cone that will be plotted color : str, optional (default: black) Color in which the cone should be plotted linewidth : float, optional (default: 1.0) Line width for wireframe plot Returns ------- ax : Matplotlib 3d axis New or old axis """ if ax is None: ax = make_3d_axis(ax_s) if wireframe: t = np.linspace(0, height, n_steps) radii = np.linspace(radius, 0, n_steps) else: t = np.array([0, height]) radii = np.array([radius, 0]) angles = np.linspace(0, 2 * np.pi, n_steps) t, angles = np.meshgrid(t, angles) A2B = np.asarray(A2B) X, Y, Z = _elongated_circular_grid(A2B[:3, 3], A2B, t, radii, angles) if wireframe: ax.plot_wireframe( X, Y, Z, rstride=5, cstride=5, alpha=alpha, color=color, linewidth=linewidth, ) else: ax.plot_surface(X, Y, Z, color=color, alpha=alpha, linewidth=0) return ax def _elongated_circular_grid( bottom_point, A2B, height_fractions, radii, angles ): return [ bottom_point[i] + radii * np.sin(angles) * A2B[i, 0] + radii * np.cos(angles) * A2B[i, 1] + A2B[i, 2] * height_fractions for i in range(3) ] ================================================ FILE: pytransform3d/plot_utils/_plot_geometries.pyi ================================================ import numpy as np import numpy.typing as npt from mpl_toolkits.mplot3d import Axes3D from typing import Union def plot_box( ax: Union[None, Axes3D] = ..., size: npt.ArrayLike = ..., A2B: npt.ArrayLike = ..., ax_s: float = ..., wireframe: bool = ..., color: str = ..., alpha: float = ..., linewidth: float = ..., ) -> Axes3D: ... def plot_sphere( ax: Union[None, Axes3D] = ..., radius: float = ..., p: npt.ArrayLike = ..., ax_s: float = ..., wireframe: bool = ..., n_steps: int = ..., alpha: float = ..., color: str = ..., linewidth: float = ..., ) -> Axes3D: ... def plot_spheres( ax: Union[None, Axes3D] = ..., radius: npt.ArrayLike = ..., p: npt.ArrayLike = ..., ax_s: float = ..., wireframe: bool = ..., n_steps: int = ..., alpha: npt.ArrayLike = ..., color: npt.ArrayLike = ..., linewidth: float = ..., ) -> Axes3D: ... def plot_cylinder( ax: Union[None, Axes3D] = ..., length: float = ..., radius: float = ..., thickness: float = ..., A2B: npt.ArrayLike = ..., ax_s: float = ..., wireframe: bool = ..., n_steps: int = ..., alpha: float = ..., color: str = ..., linewidth: float = ..., ) -> Axes3D: ... def plot_mesh( ax: Union[None, Axes3D] = ..., filename: Union[None, str] = ..., A2B: npt.ArrayLike = ..., s: npt.ArrayLike = ..., ax_s: float = ..., wireframe: bool = ..., convex_hull: bool = ..., alpha: float = ..., color: str = ..., linewidth: float = ..., ) -> Axes3D: ... def plot_ellipsoid( ax: Union[None, Axes3D] = ..., radii: npt.ArrayLike = ..., A2B: npt.ArrayLike = ..., ax_s: float = ..., wireframe: bool = ..., n_steps: int = ..., alpha: float = ..., color: str = ..., linewidth: float = ..., ) -> Axes3D: ... def plot_capsule( ax: Union[None, Axes3D] = ..., A2B: npt.ArrayLike = ..., height: float = ..., radius: float = ..., ax_s: float = ..., wireframe: bool = ..., n_steps: int = ..., alpha: float = ..., color: str = ..., linewidth: float = ..., ) -> Axes3D: ... def plot_cone( ax: Union[None, Axes3D] = ..., height: float = ..., radius: float = ..., A2B: npt.ArrayLike = ..., ax_s: float = ..., wireframe: bool = ..., n_steps: int = ..., alpha: float = ..., color: str = ..., linewidth: float = ..., ) -> Axes3D: ... ================================================ FILE: pytransform3d/plot_utils/_plot_helpers.py ================================================ """Plotting functions.""" import numpy as np from ._artists import Arrow3D from ._layout import make_3d_axis from ..rotations import unitx, unitz, perpendicular_to_vectors, norm_vector def plot_vector( ax=None, start=np.zeros(3), direction=np.array([1, 0, 0]), s=1.0, arrowstyle="simple", ax_s=1, **kwargs, ): """Plot Vector. Draws an arrow from start to start + s * direction. Parameters ---------- ax : Matplotlib 3d axis, optional (default: None) If the axis is None, a new 3d axis will be created start : array-like, shape (3,), optional (default: [0, 0, 0]) Start of the vector direction : array-like, shape (3,), optional (default: [1, 0, 0]) Direction of the vector s : float, optional (default: 1) Scaling of the vector that will be drawn arrowstyle : str, or ArrowStyle, optional (default: 'simple') See matplotlib's documentation of arrowstyle in matplotlib.patches.FancyArrowPatch for more options ax_s : float, optional (default: 1) Scaling of the new matplotlib 3d axis kwargs : dict, optional (default: {}) Additional arguments for the plotting functions, e.g. alpha Returns ------- ax : Matplotlib 3d axis New or old axis """ if ax is None: ax = make_3d_axis(ax_s) axis_arrow = Arrow3D( [start[0], start[0] + s * direction[0]], [start[1], start[1] + s * direction[1]], [start[2], start[2] + s * direction[2]], mutation_scale=20, arrowstyle=arrowstyle, **kwargs, ) ax.add_artist(axis_arrow) return ax def plot_length_variable( ax=None, start=np.zeros(3), end=np.ones(3), name="l", above=False, ax_s=1, color="k", **kwargs, ): """Plot length with text at its center. Parameters ---------- ax : Matplotlib 3d axis, optional (default: None) If the axis is None, a new 3d axis will be created start : array-like, shape (3,), optional (default: [0, 0, 0]) Start point end : array-like, shape (3,), optional (default: [1, 1, 1]) End point name : str, optional (default: 'l') Text in the middle above : bool, optional (default: False) Plot name above line ax_s : float, optional (default: 1) Scaling of the new matplotlib 3d axis color : str, optional (default: black) Color in which the cylinder should be plotted kwargs : dict, optional (default: {}) Additional arguments for the text, e.g. fontsize Returns ------- ax : Matplotlib 3d axis New or old axis """ if ax is None: ax = make_3d_axis(ax_s) direction = end - start length = np.linalg.norm(direction) if above: ax.plot( [start[0], end[0]], [start[1], end[1]], [start[2], end[2]], color=color, ) else: mid1 = start + 0.4 * direction mid2 = start + 0.6 * direction ax.plot( [start[0], mid1[0]], [start[1], mid1[1]], [start[2], mid1[2]], color=color, ) ax.plot( [end[0], mid2[0]], [end[1], mid2[1]], [end[2], mid2[2]], color=color ) if np.linalg.norm(direction / length - unitz) < np.finfo(float).eps: axis = unitx else: axis = unitz mark = ( norm_vector(perpendicular_to_vectors(direction, axis)) * 0.03 * length ) mark_start1 = start + mark mark_start2 = start - mark mark_end1 = end + mark mark_end2 = end - mark ax.plot( [mark_start1[0], mark_start2[0]], [mark_start1[1], mark_start2[1]], [mark_start1[2], mark_start2[2]], color=color, ) ax.plot( [mark_end1[0], mark_end2[0]], [mark_end1[1], mark_end2[1]], [mark_end1[2], mark_end2[2]], color=color, ) text_location = start + 0.45 * direction if above: text_location[2] += 0.3 * length ax.text( text_location[0], text_location[1], text_location[2], name, zdir="x", **kwargs, ) return ax ================================================ FILE: pytransform3d/plot_utils/_plot_helpers.pyi ================================================ import numpy as np import numpy.typing as npt from mpl_toolkits.mplot3d import Axes3D from typing import Union def plot_vector( ax: Union[None, Axes3D] = ..., start: npt.ArrayLike = ..., direction: npt.ArrayLike = ..., s: float = ..., arrowstyle: str = ..., ax_s: float = ..., **kwargs, ) -> Axes3D: ... def plot_length_variable( ax: Union[None, Axes3D] = ..., start: npt.ArrayLike = ..., end: npt.ArrayLike = ..., name: str = ..., above: bool = ..., ax_s: float = ..., color: str = ..., **kwargs, ) -> Axes3D: ... ================================================ FILE: pytransform3d/plot_utils/test/test_plot_utils.py ================================================ import numpy as np import pytest try: import matplotlib # noqa: F401 except ImportError: pytest.skip("matplotlib is required for these tests") from pytransform3d.plot_utils import ( make_3d_axis, remove_frame, Frame, LabeledFrame, Trajectory, Camera, plot_box, plot_sphere, plot_spheres, plot_cylinder, plot_mesh, plot_ellipsoid, plot_capsule, plot_cone, plot_vector, plot_length_variable, ) from numpy.testing import assert_array_almost_equal def test_make_3d_axis(): ax = make_3d_axis(2.0) try: assert ax.name == "3d" assert ax.get_xlim() == (-2, 2) assert ax.get_ylim() == (-2, 2) assert ax.get_zlim() == (-2, 2) finally: ax.remove() def test_make_3d_axis_subplots(): ax1 = make_3d_axis(1.0, 121) ax2 = make_3d_axis(1.0, 122) try: bounds1 = ax1.get_position().bounds bounds2 = ax2.get_position().bounds assert bounds1[0] < bounds2[0] assert bounds1[2] < bounds2[2] finally: ax1.remove() ax2.remove() def test_make_3d_axis_with_units(): ax = make_3d_axis(1.0, unit="m") try: assert ax.get_xlabel() == "X [m]" assert ax.get_ylabel() == "Y [m]" assert ax.get_zlabel() == "Z [m]" finally: ax.remove() def test_frame_removed(): ax = make_3d_axis(1.0) try: remove_frame(ax) bounds = ax.get_position().bounds # regression test assert_array_almost_equal(bounds, (0.125, 0.0, 0.75, 1.0)) finally: ax.remove() def test_frame(): ax = make_3d_axis(1.0) try: frame = Frame(np.eye(4), label="Frame", s=0.1) frame.add_frame(ax) assert len(ax.lines) == 4 # 3 axes and black line to text assert len(ax.texts) == 1 # label finally: ax.remove() def test_frame_no_indicator(): ax = make_3d_axis(1.0) try: frame = Frame( np.eye(4), label="Frame", s=0.1, draw_label_indicator=False ) frame.add_frame(ax) assert len(ax.lines) == 3 # 3 axes and omit black line to text assert len(ax.texts) == 1 # label finally: ax.remove() def test_labeled_frame(): ax = make_3d_axis(1.0) try: frame = LabeledFrame(np.eye(4), label="Frame", s=0.1) frame.add_frame(ax) assert len(ax.lines) == 4 # 3 axes and black line to text assert len(ax.texts) == 4 # label and 3 axis labels finally: ax.remove() def test_trajectory(): ax = make_3d_axis(1.0) try: trajectory = Trajectory( np.array([np.eye(4), np.eye(4)]), s=0.1, n_frames=2 ) trajectory.add_trajectory(ax) assert len(ax.lines) == 7 # 2 * 3 axes + connection line # assert_equal(len(ax.artists), 1) # arrow is not an artist anymore finally: ax.remove() def test_camera(): ax = make_3d_axis(1.0) try: w, h = 1920, 1080 # [pixels] M = np.array(((100, 0, 100), (0, 100, 100), (0, 0, 1))) camera = Camera( M, np.eye(4), virtual_image_distance=5, sensor_size=(w, h) ) camera.add_camera(ax) assert len(ax.lines) == 5 # 4 egdes + 1 triangle finally: ax.remove() def test_plot_box(): ax = make_3d_axis(1.0) try: plot_box(ax, wireframe=False) assert len(ax.collections) == 1 finally: ax.remove() def test_plot_box_wireframe(): ax = make_3d_axis(1.0) try: plot_box(ax, wireframe=True) assert len(ax.collections) == 1 finally: ax.remove() def test_plot_sphere(): ax = make_3d_axis(1.0) try: plot_sphere(ax, wireframe=False) assert len(ax.collections) == 1 finally: ax.remove() def test_plot_sphere_wireframe(): ax = make_3d_axis(1.0) try: plot_sphere(ax, wireframe=True) assert len(ax.collections) == 1 finally: ax.remove() def test_plot_spheres(): ax = make_3d_axis(1.0) try: plot_spheres(ax, wireframe=False) assert len(ax.collections) == 1 finally: ax.remove() def test_plot_spheres_wireframe(): ax = make_3d_axis(1.0) try: plot_spheres(ax, wireframe=True) assert len(ax.collections) == 1 finally: ax.remove() def test_plot_cylinder(): ax = make_3d_axis(1.0) try: plot_cylinder(ax, wireframe=False) assert len(ax.collections) == 1 finally: ax.remove() def test_plot_cylinder_wireframe(): ax = make_3d_axis(1.0) try: plot_cylinder(ax, wireframe=True) assert len(ax.collections) == 1 finally: ax.remove() def test_plot_mesh(): try: import trimesh # noqa: F401 except ImportError: pytest.skip("trimesh is required for this test") ax = make_3d_axis(1.0) try: plot_mesh(ax, filename="test/test_data/cone.stl", wireframe=False) assert len(ax.collections) == 1 finally: ax.remove() def test_plot_mesh_wireframe(): try: import trimesh # noqa: F401 except ImportError: pytest.skip("trimesh is required for this test") ax = make_3d_axis(1.0) try: plot_mesh(ax, filename="test/test_data/cone.stl", wireframe=True) assert len(ax.collections) == 1 finally: ax.remove() def test_plot_ellipsoid(): ax = make_3d_axis(1.0) try: plot_ellipsoid(ax, wireframe=False) assert len(ax.collections) == 1 finally: ax.remove() def test_plot_ellipsoid_wireframe(): ax = make_3d_axis(1.0) try: plot_ellipsoid(ax, wireframe=True) assert len(ax.collections) == 1 finally: ax.remove() def test_plot_capsule(): ax = make_3d_axis(1.0) try: plot_capsule(ax, wireframe=False) assert len(ax.collections) == 1 finally: ax.remove() def test_plot_capsule_wireframe(): ax = make_3d_axis(1.0) try: plot_capsule(ax, wireframe=True) assert len(ax.collections) == 1 finally: ax.remove() def test_plot_cone(): ax = make_3d_axis(1.0) try: plot_cone(ax, wireframe=False) assert len(ax.collections) == 1 finally: ax.remove() def test_plot_cone_wireframe(): ax = make_3d_axis(1.0) try: plot_cone(ax, wireframe=True) assert len(ax.collections) == 1 finally: ax.remove() def test_plot_vector(): ax = make_3d_axis(1.0) try: plot_vector(ax) # assert_equal(len(ax.artists), 1) # arrow is not an artist anymore finally: ax.remove() def test_plot_length_variable(): ax = make_3d_axis(1.0) try: plot_length_variable(ax) assert len(ax.lines) >= 1 assert len(ax.texts) == 1 finally: ax.remove() ================================================ FILE: pytransform3d/py.typed ================================================ ================================================ FILE: pytransform3d/rotations/__init__.py ================================================ """Rotations in three dimensions - SO(3). See :doc:`user_guide/rotations` for more information. """ from ._constants import ( eps, unitx, unity, unitz, q_i, q_j, q_k, q_id, a_id, R_id, p0, ) from ._utils import ( norm_vector, angle_between_vectors, perpendicular_to_vector, vector_projection, perpendicular_to_vectors, plane_basis_from_normal, ) from ._angle import ( norm_angle, active_matrix_from_angle, passive_matrix_from_angle, quaternion_from_angle, ) from ._matrix import ( matrix_requires_renormalization, check_matrix, norm_matrix, assert_rotation_matrix, matrix_from_two_vectors, quaternion_from_matrix, axis_angle_from_matrix, compact_axis_angle_from_matrix, ) from ._euler import ( norm_euler, euler_near_gimbal_lock, assert_euler_equal, matrix_from_euler, euler_from_matrix, euler_from_quaternion, ) from ._axis_angle import ( norm_axis_angle, norm_compact_axis_angle, compact_axis_angle_near_pi, check_axis_angle, check_compact_axis_angle, assert_axis_angle_equal, assert_compact_axis_angle_equal, axis_angle_from_two_directions, matrix_from_axis_angle, matrix_from_compact_axis_angle, axis_angle_from_compact_axis_angle, compact_axis_angle, quaternion_from_axis_angle, quaternion_from_compact_axis_angle, mrp_from_axis_angle, ) from ._rot_log import ( check_skew_symmetric_matrix, check_rot_log, cross_product_matrix, rot_log_from_compact_axis_angle, ) from ._quaternion import ( quaternion_requires_renormalization, check_quaternion, check_quaternions, quaternion_double, pick_closest_quaternion, assert_quaternion_equal, quaternion_integrate, quaternion_gradient, concatenate_quaternions, q_conj, q_prod_vector, quaternion_diff, quaternion_dist, quaternion_from_euler, matrix_from_quaternion, axis_angle_from_quaternion, compact_axis_angle_from_quaternion, mrp_from_quaternion, quaternion_wxyz_from_xyzw, quaternion_xyzw_from_wxyz, ) from ._mrp import ( norm_mrp, check_mrp, mrp_near_singularity, mrp_double, assert_mrp_equal, concatenate_mrp, mrp_prod_vector, quaternion_from_mrp, axis_angle_from_mrp, ) from ._rotors import ( check_rotor, wedge, geometric_product, rotor_apply, rotor_reverse, concatenate_rotors, rotor_from_plane_angle, rotor_from_two_directions, matrix_from_rotor, plane_normal_from_bivector, ) from ._random import ( random_vector, random_axis_angle, random_compact_axis_angle, random_quaternion, random_matrix, ) from ._slerp import ( matrix_slerp, matrix_power, slerp_weights, quaternion_slerp, axis_angle_slerp, rotor_slerp, ) from ._jacobians import ( left_jacobian_SO3, left_jacobian_SO3_series, left_jacobian_SO3_inv, left_jacobian_SO3_inv_series, ) from ._polar_decomp import robust_polar_decomposition from ._plot import ( plot_basis, plot_axis_angle, plot_bivector, ) from ._euler_deprecated import ( active_matrix_from_extrinsic_euler_xyx, active_matrix_from_intrinsic_euler_xyx, active_matrix_from_extrinsic_euler_xyz, active_matrix_from_extrinsic_euler_xzx, active_matrix_from_extrinsic_euler_xzy, active_matrix_from_extrinsic_euler_yxy, active_matrix_from_extrinsic_euler_yxz, active_matrix_from_extrinsic_euler_yzx, active_matrix_from_extrinsic_euler_yzy, active_matrix_from_extrinsic_euler_zxy, active_matrix_from_extrinsic_euler_zxz, active_matrix_from_extrinsic_euler_zyz, active_matrix_from_intrinsic_euler_xyz, active_matrix_from_intrinsic_euler_xzx, active_matrix_from_extrinsic_euler_zyx, active_matrix_from_intrinsic_euler_xzy, active_matrix_from_intrinsic_euler_yxy, active_matrix_from_intrinsic_euler_yxz, active_matrix_from_intrinsic_euler_yzx, active_matrix_from_intrinsic_euler_yzy, active_matrix_from_intrinsic_euler_zxy, active_matrix_from_intrinsic_euler_zxz, active_matrix_from_intrinsic_euler_zyx, active_matrix_from_intrinsic_euler_zyz, active_matrix_from_extrinsic_roll_pitch_yaw, intrinsic_euler_xyz_from_active_matrix, intrinsic_euler_xyx_from_active_matrix, intrinsic_euler_xzx_from_active_matrix, intrinsic_euler_xzy_from_active_matrix, intrinsic_euler_yxy_from_active_matrix, intrinsic_euler_yxz_from_active_matrix, intrinsic_euler_yzx_from_active_matrix, intrinsic_euler_yzy_from_active_matrix, intrinsic_euler_zxy_from_active_matrix, intrinsic_euler_zxz_from_active_matrix, intrinsic_euler_zyx_from_active_matrix, intrinsic_euler_zyz_from_active_matrix, extrinsic_euler_xyx_from_active_matrix, extrinsic_euler_xyz_from_active_matrix, extrinsic_euler_xzx_from_active_matrix, extrinsic_euler_xzy_from_active_matrix, extrinsic_euler_yxy_from_active_matrix, extrinsic_euler_yxz_from_active_matrix, extrinsic_euler_yzx_from_active_matrix, extrinsic_euler_yzy_from_active_matrix, extrinsic_euler_zxy_from_active_matrix, extrinsic_euler_zxz_from_active_matrix, extrinsic_euler_zyx_from_active_matrix, extrinsic_euler_zyz_from_active_matrix, quaternion_from_extrinsic_euler_xyz, ) __all__ = [ "eps", "unitx", "unity", "unitz", "q_i", "q_j", "q_k", "q_id", "a_id", "R_id", "p0", "norm_angle", "norm_vector", "angle_between_vectors", "perpendicular_to_vector", "vector_projection", "perpendicular_to_vectors", "norm_axis_angle", "compact_axis_angle_near_pi", "norm_compact_axis_angle", "matrix_requires_renormalization", "norm_matrix", "quaternion_requires_renormalization", "random_vector", "random_axis_angle", "random_compact_axis_angle", "random_quaternion", "random_matrix", "check_skew_symmetric_matrix", "check_rot_log", "check_matrix", "check_quaternion", "check_quaternions", "check_axis_angle", "check_rotor", "check_compact_axis_angle", "check_mrp", "quaternion_from_axis_angle", "quaternion_from_compact_axis_angle", "quaternion_from_matrix", "quaternion_wxyz_from_xyzw", "quaternion_xyzw_from_wxyz", "quaternion_from_extrinsic_euler_xyz", "axis_angle_from_quaternion", "axis_angle_from_compact_axis_angle", "axis_angle_from_matrix", "axis_angle_from_two_directions", "compact_axis_angle", "compact_axis_angle_from_quaternion", "compact_axis_angle_from_matrix", "matrix_from_quaternion", "matrix_from_compact_axis_angle", "matrix_from_axis_angle", "matrix_from_two_vectors", "active_matrix_from_angle", "norm_euler", "euler_near_gimbal_lock", "matrix_from_euler", "active_matrix_from_extrinsic_euler_xyx", "active_matrix_from_intrinsic_euler_xyx", "active_matrix_from_extrinsic_euler_xyz", "active_matrix_from_extrinsic_euler_xzx", "active_matrix_from_extrinsic_euler_xzy", "active_matrix_from_extrinsic_euler_yxy", "active_matrix_from_extrinsic_euler_yxz", "active_matrix_from_extrinsic_euler_yzx", "active_matrix_from_extrinsic_euler_yzy", "active_matrix_from_extrinsic_euler_zxy", "active_matrix_from_extrinsic_euler_zxz", "active_matrix_from_extrinsic_euler_zyz", "active_matrix_from_intrinsic_euler_xyz", "active_matrix_from_intrinsic_euler_xzx", "active_matrix_from_extrinsic_euler_zyx", "active_matrix_from_intrinsic_euler_xzy", "active_matrix_from_intrinsic_euler_yxy", "active_matrix_from_intrinsic_euler_yxz", "active_matrix_from_intrinsic_euler_yzx", "active_matrix_from_intrinsic_euler_yzy", "active_matrix_from_intrinsic_euler_zxy", "active_matrix_from_intrinsic_euler_zxz", "active_matrix_from_intrinsic_euler_zyx", "active_matrix_from_intrinsic_euler_zyz", "active_matrix_from_extrinsic_roll_pitch_yaw", "passive_matrix_from_angle", "intrinsic_euler_xyz_from_active_matrix", "intrinsic_euler_xyx_from_active_matrix", "intrinsic_euler_xzx_from_active_matrix", "intrinsic_euler_xzy_from_active_matrix", "intrinsic_euler_yxy_from_active_matrix", "intrinsic_euler_yxz_from_active_matrix", "intrinsic_euler_yzx_from_active_matrix", "intrinsic_euler_yzy_from_active_matrix", "intrinsic_euler_zxy_from_active_matrix", "intrinsic_euler_zxz_from_active_matrix", "intrinsic_euler_zyx_from_active_matrix", "intrinsic_euler_zyz_from_active_matrix", "extrinsic_euler_xyx_from_active_matrix", "extrinsic_euler_xyz_from_active_matrix", "extrinsic_euler_xzx_from_active_matrix", "extrinsic_euler_xzy_from_active_matrix", "extrinsic_euler_yxy_from_active_matrix", "extrinsic_euler_yxz_from_active_matrix", "extrinsic_euler_yzx_from_active_matrix", "extrinsic_euler_yzy_from_active_matrix", "extrinsic_euler_zxy_from_active_matrix", "extrinsic_euler_zxz_from_active_matrix", "extrinsic_euler_zyx_from_active_matrix", "extrinsic_euler_zyz_from_active_matrix", "euler_from_matrix", "euler_from_quaternion", "quaternion_from_angle", "quaternion_from_euler", "mrp_near_singularity", "norm_mrp", "mrp_double", "concatenate_mrp", "mrp_prod_vector", "cross_product_matrix", "rot_log_from_compact_axis_angle", "mrp_from_quaternion", "quaternion_from_mrp", "mrp_from_axis_angle", "axis_angle_from_mrp", "quaternion_double", "quaternion_integrate", "quaternion_gradient", "concatenate_quaternions", "q_conj", "q_prod_vector", "quaternion_diff", "quaternion_dist", "matrix_slerp", "matrix_power", "slerp_weights", "pick_closest_quaternion", "quaternion_slerp", "axis_angle_slerp", "assert_euler_equal", "assert_quaternion_equal", "assert_axis_angle_equal", "assert_compact_axis_angle_equal", "assert_rotation_matrix", "assert_mrp_equal", "plot_basis", "plot_axis_angle", "wedge", "geometric_product", "wedge", "geometric_product", "rotor_apply", "rotor_reverse", "concatenate_rotors", "rotor_from_plane_angle", "rotor_from_two_directions", "matrix_from_rotor", "plot_bivector", "rotor_slerp", "plane_normal_from_bivector", "plane_basis_from_normal", "left_jacobian_SO3", "left_jacobian_SO3_series", "left_jacobian_SO3_inv", "left_jacobian_SO3_inv_series", "robust_polar_decomposition", ] ================================================ FILE: pytransform3d/rotations/_angle.py ================================================ """Angle operations.""" import math import numpy as np from ._constants import two_pi def norm_angle(a): """Normalize angle to (-pi, pi]. It is worth noting that using `numpy.ceil` to normalize angles will lose more digits of precision as angles going larger but can keep more digits of precision when angles are around zero. In common use cases, for example, -10.0*pi to 10.0*pi, it performs well. For more discussions on numerical precision: https://github.com/dfki-ric/pytransform3d/pull/263 Parameters ---------- a : float or array-like, shape (n,) Angle(s) in radians Returns ------- a_norm : float or array, shape (n,) Normalized angle(s) in radians """ a = np.asarray(a, dtype=np.float64) return a - (np.ceil((a + np.pi) / two_pi) - 1.0) * two_pi def passive_matrix_from_angle(basis, angle): """Compute passive rotation matrix from rotation about basis vector. Parameters ---------- basis : int from [0, 1, 2] The rotation axis (0: x, 1: y, 2: z) angle : float Rotation angle Returns ------- R : array-like, shape (3, 3) Rotation matrix Raises ------ ValueError If basis is invalid """ c = np.cos(angle) s = np.sin(angle) if basis == 0: R = np.array([[1.0, 0.0, 0.0], [0.0, c, s], [0.0, -s, c]]) elif basis == 1: R = np.array([[c, 0.0, -s], [0.0, 1.0, 0.0], [s, 0.0, c]]) elif basis == 2: R = np.array([[c, s, 0.0], [-s, c, 0.0], [0.0, 0.0, 1.0]]) else: raise ValueError("Basis must be in [0, 1, 2]") return R def active_matrix_from_angle(basis, angle): r"""Compute active rotation matrix from rotation about basis vector. With the angle :math:`\alpha` and :math:`s = \sin{\alpha}, c=\cos{\alpha}`, we construct rotation matrices about the basis vectors as follows: .. math:: \boldsymbol{R}_x(\alpha) = \left( \begin{array}{ccc} 1 & 0 & 0\\ 0 & c & -s\\ 0 & s & c \end{array} \right) .. math:: \boldsymbol{R}_y(\alpha) = \left( \begin{array}{ccc} c & 0 & s\\ 0 & 1 & 0\\ -s & 0 & c \end{array} \right) .. math:: \boldsymbol{R}_z(\alpha) = \left( \begin{array}{ccc} c & -s & 0\\ s & c & 0\\ 0 & 0 & 1 \end{array} \right) Parameters ---------- basis : int from [0, 1, 2] The rotation axis (0: x, 1: y, 2: z) angle : float Rotation angle Returns ------- R : array, shape (3, 3) Rotation matrix Raises ------ ValueError If basis is invalid """ c = np.cos(angle) s = np.sin(angle) if basis == 0: R = np.array([[1.0, 0.0, 0.0], [0.0, c, -s], [0.0, s, c]]) elif basis == 1: R = np.array([[c, 0.0, s], [0.0, 1.0, 0.0], [-s, 0.0, c]]) elif basis == 2: R = np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]]) else: raise ValueError("Basis must be in [0, 1, 2]") return R def quaternion_from_angle(basis, angle): """Compute quaternion from rotation about basis vector. Parameters ---------- basis : int from [0, 1, 2] The rotation axis (0: x, 1: y, 2: z) angle : float Rotation angle Returns ------- q : array, shape (4,) Unit quaternion to represent rotation: (w, x, y, z) Raises ------ ValueError If basis is invalid """ half_angle = 0.5 * angle c = math.cos(half_angle) s = math.sin(half_angle) if basis == 0: q = np.array([c, s, 0.0, 0.0]) elif basis == 1: q = np.array([c, 0.0, s, 0.0]) elif basis == 2: q = np.array([c, 0.0, 0.0, s]) else: raise ValueError("Basis must be in [0, 1, 2]") return q ================================================ FILE: pytransform3d/rotations/_angle.pyi ================================================ import numpy as np import numpy.typing as npt def norm_angle(a: npt.ArrayLike) -> np.ndarray: ... def passive_matrix_from_angle(basis: int, angle: float) -> np.ndarray: ... def active_matrix_from_angle(basis: int, angle: float) -> np.ndarray: ... def quaternion_from_angle(basis: int, angle: float) -> np.ndarray: ... ================================================ FILE: pytransform3d/rotations/_axis_angle.py ================================================ """Axis-angle representation.""" import math import numpy as np from numpy.testing import assert_array_almost_equal from ._angle import norm_angle from ._constants import eps from ._utils import norm_vector, perpendicular_to_vector def check_axis_angle(a): """Input validation of axis-angle representation. Parameters ---------- a : array-like, shape (4,) Axis of rotation and rotation angle: (x, y, z, angle) Returns ------- a : array, shape (4,) Validated axis of rotation and rotation angle: (x, y, z, angle) Raises ------ ValueError If input is invalid """ a = np.asarray(a, dtype=np.float64) if a.ndim != 1 or a.shape[0] != 4: raise ValueError( "Expected axis and angle in array with shape (4,), " "got array-like object with shape %s" % (a.shape,) ) return norm_axis_angle(a) def check_compact_axis_angle(a): """Input validation of compact axis-angle representation. Parameters ---------- a : array-like, shape (3,) Axis of rotation and rotation angle: angle * (x, y, z) Returns ------- a : array, shape (3,) Validated axis of rotation and rotation angle: angle * (x, y, z) Raises ------ ValueError If input is invalid """ a = np.asarray(a, dtype=np.float64) if a.ndim != 1 or a.shape[0] != 3: raise ValueError( "Expected axis and angle in array with shape (3,), " "got array-like object with shape %s" % (a.shape,) ) return norm_compact_axis_angle(a) def norm_axis_angle(a): """Normalize axis-angle representation. Parameters ---------- a : array-like, shape (4,) Axis of rotation and rotation angle: (x, y, z, angle) Returns ------- a : array, shape (4,) Axis of rotation and rotation angle: (x, y, z, angle). The length of the axis vector is 1 and the angle is in [0, pi). No rotation is represented by [1, 0, 0, 0]. """ angle = a[3] norm = np.linalg.norm(a[:3]) if angle == 0.0 or norm == 0.0: return np.array([1.0, 0.0, 0.0, 0.0]) res = np.empty(4) res[:3] = a[:3] / norm angle = norm_angle(angle) if angle < 0.0: angle *= -1.0 res[:3] *= -1.0 res[3] = angle return res def norm_compact_axis_angle(a): """Normalize compact axis-angle representation. Parameters ---------- a : array-like, shape (3,) Axis of rotation and rotation angle: angle * (x, y, z) Returns ------- a : array, shape (3,) Axis of rotation and rotation angle: angle * (x, y, z). The angle is in [0, pi). No rotation is represented by [0, 0, 0]. """ angle = np.linalg.norm(a) if angle == 0.0: return np.zeros(3) axis = a / angle return axis * norm_angle(angle) def compact_axis_angle_near_pi(a, tolerance=1e-6): r"""Check if angle of compact axis-angle representation is near pi. When the angle :math:`\theta = \pi`, both :math:`\hat{\boldsymbol{\omega}}` and :math:`-\hat{\boldsymbol{\omega}}` result in the same rotation. This ambiguity could lead to problems when averaging or interpolating. Parameters ---------- a : array-like, shape (3,) Axis of rotation and rotation angle: angle * (x, y, z). tolerance : float Tolerance of this check. Returns ------- near_pi : bool Angle is near pi. """ theta = np.linalg.norm(a) return abs(theta - np.pi) < tolerance def assert_axis_angle_equal(a1, a2, *args, **kwargs): """Raise an assertion if two axis-angle are not approximately equal. Usually we assume that the rotation axis is normalized to length 1 and the angle is within [0, pi). However, this function ignores these constraints and will normalize the representations before comparison. See numpy.testing.assert_array_almost_equal for a more detailed documentation of the other parameters. Parameters ---------- a1 : array-like, shape (4,) Axis of rotation and rotation angle: (x, y, z, angle) a2 : array-like, shape (4,) Axis of rotation and rotation angle: (x, y, z, angle) args : tuple Positional arguments that will be passed to `assert_array_almost_equal` kwargs : dict Positional arguments that will be passed to `assert_array_almost_equal` """ a1 = norm_axis_angle(a1) a2 = norm_axis_angle(a2) # required despite normalization in case of 180 degree rotation if np.any(np.sign(a1) != np.sign(a2)): a1 = -a1 a1 = norm_axis_angle(a1) assert_array_almost_equal(a1, a2, *args, **kwargs) def assert_compact_axis_angle_equal(a1, a2, *args, **kwargs): """Raise an assertion if two axis-angle are not approximately equal. Usually we assume that the angle is within [0, pi). However, this function ignores this constraint and will normalize the representations before comparison. See numpy.testing.assert_array_almost_equal for a more detailed documentation of the other parameters. Parameters ---------- a1 : array-like, shape (3,) Axis of rotation and rotation angle: angle * (x, y, z) a2 : array-like, shape (3,) Axis of rotation and rotation angle: angle * (x, y, z) args : tuple Positional arguments that will be passed to `assert_array_almost_equal` kwargs : dict Positional arguments that will be passed to `assert_array_almost_equal` """ angle1 = np.linalg.norm(a1) angle2 = np.linalg.norm(a2) # required despite normalization in case of 180 degree rotation if ( abs(angle1) == np.pi and abs(angle2) == np.pi and any(np.sign(a1) != np.sign(a2)) ): a1 = -a1 a1 = norm_compact_axis_angle(a1) a2 = norm_compact_axis_angle(a2) assert_array_almost_equal(a1, a2, *args, **kwargs) def axis_angle_from_two_directions(a, b): """Compute axis-angle representation from two direction vectors. The rotation will transform direction vector a to direction vector b. The direction vectors don't have to be normalized as this will be done internally. Note that there is more than one possible solution. Parameters ---------- a : array-like, shape (3,) First direction vector b : array-like, shape (3,) Second direction vector Returns ------- a : array, shape (4,) Axis of rotation and rotation angle: (x, y, z, angle). The angle is constrained to [0, pi]. """ a = norm_vector(a) b = norm_vector(b) cos_angle = a.dot(b) if abs(-1.0 - cos_angle) < eps: # For 180 degree rotations we have an infinite number of solutions, # but we have to pick one axis. axis = perpendicular_to_vector(a) else: axis = np.cross(a, b) aa = np.empty(4) aa[:3] = norm_vector(axis) aa[3] = np.arccos(max(min(cos_angle, 1.0), -1.0)) return norm_axis_angle(aa) def matrix_from_axis_angle(a): r"""Compute rotation matrix from axis-angle. This is called exponential map or Rodrigues' formula. .. math:: \boldsymbol{R}(\hat{\boldsymbol{\omega}}, \theta) = Exp(\hat{\boldsymbol{\omega}} \theta) = \cos{\theta} \boldsymbol{I} + \sin{\theta} \left[\hat{\boldsymbol{\omega}}\right] + (1 - \cos{\theta}) \hat{\boldsymbol{\omega}}\hat{\boldsymbol{\omega}}^T = \boldsymbol{I} + \sin{\theta} \left[\hat{\boldsymbol{\omega}}\right] + (1 - \cos{\theta}) \left[\hat{\boldsymbol{\omega}}\right]^2 This typically results in an active rotation matrix. Parameters ---------- a : array-like, shape (4,) Axis of rotation and rotation angle: (x, y, z, angle) Returns ------- R : array, shape (3, 3) Rotation matrix """ a = check_axis_angle(a) ux, uy, uz, theta = a c = math.cos(theta) s = math.sin(theta) ci = 1.0 - c R = np.array( [ [ci * ux * ux + c, ci * ux * uy - uz * s, ci * ux * uz + uy * s], [ci * uy * ux + uz * s, ci * uy * uy + c, ci * uy * uz - ux * s], [ci * uz * ux - uy * s, ci * uz * uy + ux * s, ci * uz * uz + c], ] ) # This is equivalent to # R = (np.eye(3) * np.cos(a[3]) + # (1.0 - np.cos(a[3])) * a[:3, np.newaxis].dot(a[np.newaxis, :3]) + # cross_product_matrix(a[:3]) * np.sin(a[3])) # or # w = cross_product_matrix(a[:3]) # R = np.eye(3) + np.sin(a[3]) * w + (1.0 - np.cos(a[3])) * w.dot(w) return R def matrix_from_compact_axis_angle(a): r"""Compute rotation matrix from compact axis-angle. This is called exponential map or Rodrigues' formula. .. math:: Exp(\hat{\boldsymbol{\omega}} \theta) = \cos{\theta} \boldsymbol{I} + \sin{\theta} \left[\hat{\boldsymbol{\omega}}\right] + (1 - \cos{\theta}) \hat{\boldsymbol{\omega}}\hat{\boldsymbol{\omega}}^T = \boldsymbol{I} + \sin{\theta} \left[\hat{\boldsymbol{\omega}}\right] + (1 - \cos{\theta}) \left[\hat{\boldsymbol{\omega}}\right]^2 This typically results in an active rotation matrix. Parameters ---------- a : array-like, shape (3,) Axis of rotation and rotation angle: angle * (x, y, z) Returns ------- R : array-like, shape (3, 3) Rotation matrix """ a = axis_angle_from_compact_axis_angle(a) return matrix_from_axis_angle(a) def axis_angle_from_compact_axis_angle(a): """Compute axis-angle from compact axis-angle representation. We usually assume active rotations. Parameters ---------- a : array-like, shape (3,) Axis of rotation and rotation angle: angle * (x, y, z). Returns ------- a : array, shape (4,) Axis of rotation and rotation angle: (x, y, z, angle). The angle is constrained to [0, pi]. """ a = check_compact_axis_angle(a) angle = np.linalg.norm(a) if angle == 0.0: return np.array([1.0, 0.0, 0.0, 0.0]) axis = a / angle return np.hstack((axis, (angle,))) def compact_axis_angle(a): r"""Compute 3-dimensional axis-angle from a 4-dimensional one. In the 3-dimensional axis-angle representation, the 4th dimension (the rotation) is represented by the norm of the rotation axis vector, which means we map :math:`\left( \hat{\boldsymbol{\omega}}, \theta \right)` to :math:`\boldsymbol{\omega} = \theta \hat{\boldsymbol{\omega}}`. This representation is also called rotation vector or exponential coordinates of rotation. Parameters ---------- a : array-like, shape (4,) Axis of rotation and rotation angle: (x, y, z, angle). Returns ------- a : array, shape (3,) Axis of rotation and rotation angle: angle * (x, y, z) (compact representation). """ a = check_axis_angle(a) return a[:3] * a[3] def quaternion_from_axis_angle(a): """Compute quaternion from axis-angle. This operation is called exponential map. We usually assume active rotations. Parameters ---------- a : array-like, shape (4,) Axis of rotation and rotation angle: (x, y, z, angle) Returns ------- q : array, shape (4,) Unit quaternion to represent rotation: (w, x, y, z) """ a = check_axis_angle(a) half_angle = 0.5 * a[3] q = np.empty(4) q[0] = np.cos(half_angle) q[1:] = np.sin(half_angle) * a[:3] return q def quaternion_from_compact_axis_angle(a): """Compute quaternion from compact axis-angle (exponential map). We usually assume active rotations. Parameters ---------- a : array-like, shape (4,) Axis of rotation and rotation angle: angle * (x, y, z) Returns ------- q : array, shape (4,) Unit quaternion to represent rotation: (w, x, y, z) """ a = axis_angle_from_compact_axis_angle(a) return quaternion_from_axis_angle(a) def mrp_from_axis_angle(a): r"""Compute modified Rodrigues parameters from axis-angle representation. .. math:: \boldsymbol{\psi} = \tan \left(\frac{\theta}{4}\right) \hat{\boldsymbol{\omega}} Parameters ---------- a : array-like, shape (4,) Axis of rotation and rotation angle: (x, y, z, angle) Returns ------- mrp : array, shape (3,) Modified Rodrigues parameters. """ a = check_axis_angle(a) return np.tan(0.25 * a[3]) * a[:3] ================================================ FILE: pytransform3d/rotations/_axis_angle.pyi ================================================ import numpy as np import numpy.typing as npt def norm_axis_angle(a: npt.ArrayLike) -> np.ndarray: ... def norm_compact_axis_angle(a: npt.ArrayLike) -> np.ndarray: ... def compact_axis_angle_near_pi( a: npt.ArrayLike, tolerance: float = ... ) -> bool: ... def check_axis_angle(a: npt.ArrayLike) -> np.ndarray: ... def check_compact_axis_angle(a: npt.ArrayLike) -> np.ndarray: ... def assert_axis_angle_equal( a1: npt.ArrayLike, a2: npt.ArrayLike, *args, **kwargs ): ... def assert_compact_axis_angle_equal( a1: npt.ArrayLike, a2: npt.ArrayLike, *args, **kwargs ): ... def axis_angle_from_two_directions( a: npt.ArrayLike, b: npt.ArrayLike ) -> np.ndarray: ... def matrix_from_axis_angle(a: npt.ArrayLike) -> np.ndarray: ... def matrix_from_compact_axis_angle(a: npt.ArrayLike) -> np.ndarray: ... def axis_angle_from_compact_axis_angle(a: npt.ArrayLike) -> np.ndarray: ... def compact_axis_angle(a: npt.ArrayLike) -> np.ndarray: ... def quaternion_from_axis_angle(a: npt.ArrayLike) -> np.ndarray: ... def quaternion_from_compact_axis_angle(a: npt.ArrayLike) -> np.ndarray: ... def mrp_from_axis_angle(a: npt.ArrayLike) -> np.ndarray: ... ================================================ FILE: pytransform3d/rotations/_constants.py ================================================ """Constants of rotation module.""" import numpy as np unitx = np.array([1.0, 0.0, 0.0]) unity = np.array([0.0, 1.0, 0.0]) unitz = np.array([0.0, 0.0, 1.0]) R_id = np.eye(3) a_id = np.array([1.0, 0.0, 0.0, 0.0]) q_id = np.array([1.0, 0.0, 0.0, 0.0]) q_i = np.array([0.0, 1.0, 0.0, 0.0]) q_j = np.array([0.0, 0.0, 1.0, 0.0]) q_k = np.array([0.0, 0.0, 0.0, 1.0]) p0 = np.array([0.0, 0.0, 0.0]) eps = 1e-7 two_pi = 2.0 * np.pi half_pi = 0.5 * np.pi ================================================ FILE: pytransform3d/rotations/_euler.py ================================================ """Euler angles.""" import math import numpy as np from numpy.testing import assert_array_almost_equal from ._angle import norm_angle, active_matrix_from_angle from ._constants import half_pi, unitx, unity, unitz, eps from ._matrix import check_matrix from ._quaternion import check_quaternion from ._utils import check_axis_index def norm_euler(e, i, j, k): """Normalize Euler angle range. Parameters ---------- e : array-like, shape (3,) Rotation angles in radians about the axes i, j, k in this order. i : int from [0, 1, 2] The first rotation axis (0: x, 1: y, 2: z) j : int from [0, 1, 2] The second rotation axis (0: x, 1: y, 2: z) k : int from [0, 1, 2] The third rotation axis (0: x, 1: y, 2: z) Returns ------- e : array, shape (3,) Extracted rotation angles in radians about the axes i, j, k in this order. The first and last angle are normalized to [-pi, pi]. The middle angle is normalized to either [0, pi] (proper Euler angles) or [-pi/2, pi/2] (Cardan / Tait-Bryan angles). """ check_axis_index("i", i) check_axis_index("j", j) check_axis_index("k", k) alpha, beta, gamma = norm_angle(e) proper_euler = i == k if proper_euler: if beta < 0.0: alpha += np.pi beta *= -1.0 gamma -= np.pi elif abs(beta) > half_pi: alpha += np.pi beta = np.pi - beta gamma -= np.pi return norm_angle([alpha, beta, gamma]) def euler_near_gimbal_lock(e, i, j, k, tolerance=1e-6): """Check if Euler angles are close to gimbal lock. Parameters ---------- e : array-like, shape (3,) Rotation angles in radians about the axes i, j, k in this order. i : int from [0, 1, 2] The first rotation axis (0: x, 1: y, 2: z) j : int from [0, 1, 2] The second rotation axis (0: x, 1: y, 2: z) k : int from [0, 1, 2] The third rotation axis (0: x, 1: y, 2: z) tolerance : float Tolerance for the comparison. Returns ------- near_gimbal_lock : bool Indicates if the Euler angles are near the gimbal lock singularity. """ e = norm_euler(e, i, j, k) beta = e[1] proper_euler = i == k if proper_euler: return abs(beta) < tolerance or abs(beta - np.pi) < tolerance else: return abs(abs(beta) - half_pi) < tolerance def assert_euler_equal(e1, e2, i, j, k, *args, **kwargs): """Raise an assertion if two Euler angles are not approximately equal. Parameters ---------- e1 : array-like, shape (3,) Rotation angles in radians about the axes i, j, k in this order. e2 : array-like, shape (3,) Rotation angles in radians about the axes i, j, k in this order. i : int from [0, 1, 2] The first rotation axis (0: x, 1: y, 2: z) j : int from [0, 1, 2] The second rotation axis (0: x, 1: y, 2: z) k : int from [0, 1, 2] The third rotation axis (0: x, 1: y, 2: z) args : tuple Positional arguments that will be passed to `assert_array_almost_equal` kwargs : dict Positional arguments that will be passed to `assert_array_almost_equal` """ e1 = norm_euler(e1, i, j, k) e2 = norm_euler(e2, i, j, k) assert_array_almost_equal(e1, e2, *args, **kwargs) def matrix_from_euler(e, i, j, k, extrinsic): """General method to compute active rotation matrix from any Euler angles. Parameters ---------- e : array-like, shape (3,) Rotation angles in radians about the axes i, j, k in this order. i : int from [0, 1, 2] The first rotation axis (0: x, 1: y, 2: z) j : int from [0, 1, 2] The second rotation axis (0: x, 1: y, 2: z) k : int from [0, 1, 2] The third rotation axis (0: x, 1: y, 2: z) extrinsic : bool Do we use extrinsic transformations? Intrinsic otherwise. Returns ------- R : array, shape (3, 3) Active rotation matrix Raises ------ ValueError If basis is invalid """ check_axis_index("i", i) check_axis_index("j", j) check_axis_index("k", k) alpha, beta, gamma = e if not extrinsic: i, k = k, i alpha, gamma = gamma, alpha R = ( active_matrix_from_angle(k, gamma) .dot(active_matrix_from_angle(j, beta)) .dot(active_matrix_from_angle(i, alpha)) ) return R def general_intrinsic_euler_from_active_matrix( R, n1, n2, n3, proper_euler, strict_check=True ): """General algorithm to extract intrinsic euler angles from a matrix. The implementation is based on SciPy's implementation: https://github.com/scipy/scipy/blob/743c283bbe79473a03ca2eddaa537661846d8a19/scipy/spatial/transform/_rotation.pyx Parameters ---------- R : array-like, shape (3, 3) Active rotation matrix n1 : array, shape (3,) First rotation axis (basis vector) n2 : array, shape (3,) Second rotation axis (basis vector) n3 : array, shape (3,) Third rotation axis (basis vector) proper_euler : bool Is this an Euler angle convention or a Cardan / Tait-Bryan convention? Proper Euler angles rotate about the same axis twice, for example, z, y', and z''. strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- euler_angles : array, shape (3,) Extracted intrinsic rotation angles in radians about the axes n1, n2, and n3 in this order. The first and last angle are normalized to [-pi, pi]. The middle angle is normalized to either [0, pi] (proper Euler angles) or [-pi/2, pi/2] (Cardan / Tait-Bryan angles). References ---------- .. [1] Shuster, M. D., Markley, F. L. (2006). General Formula for Extracting the Euler Angles. Journal of Guidance, Control, and Dynamics, 29(1), pp 2015-221, doi: 10.2514/1.16622. https://arc.aiaa.org/doi/abs/10.2514/1.16622 """ D = check_matrix(R, strict_check=strict_check) # Differences to the paper: # - we call the angles alpha, beta, and gamma # - we obtain angles from intrinsic rotations, thus some matrices are # transposed like in SciPy's implementation # Step 2 # - Equation 5 n1_cross_n2 = np.cross(n1, n2) lmbda = np.arctan2(np.dot(n1_cross_n2, n3), np.dot(n1, n3)) # - Equation 6 C = np.vstack((n2, n1_cross_n2, n1)) # Step 3 # - Equation 8 CDCT = np.dot(np.dot(C, D), C.T) O = np.dot(CDCT, active_matrix_from_angle(0, lmbda).T) # Step 4 # Fix numerical issue if O_22 is slightly out of range of arccos O_22 = max(min(O[2, 2], 1.0), -1.0) # - Equation 10a beta = lmbda + np.arccos(O_22) safe1 = abs(beta - lmbda) >= np.finfo(float).eps safe2 = abs(beta - lmbda - np.pi) >= np.finfo(float).eps if safe1 and safe2: # Default case, no gimbal lock # Step 5 # - Equation 10b alpha = np.arctan2(O[0, 2], -O[1, 2]) # - Equation 10c gamma = np.arctan2(O[2, 0], O[2, 1]) # Step 7 if proper_euler: valid_beta = 0.0 <= beta <= np.pi else: # Cardan / Tait-Bryan angles valid_beta = -0.5 * np.pi <= beta <= 0.5 * np.pi # - Equation 12 if not valid_beta: alpha += np.pi beta = 2.0 * lmbda - beta gamma -= np.pi else: # Step 6 - Handle gimbal locks # a) gamma = 0.0 if not safe1: # b) alpha = np.arctan2(O[1, 0] - O[0, 1], O[0, 0] + O[1, 1]) else: # c) alpha = np.arctan2(O[1, 0] + O[0, 1], O[0, 0] - O[1, 1]) euler_angles = norm_angle([alpha, beta, gamma]) return euler_angles def euler_from_matrix(R, i, j, k, extrinsic, strict_check=True): """General method to extract any Euler angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Active rotation matrix i : int from [0, 1, 2] The first rotation axis (0: x, 1: y, 2: z) j : int from [0, 1, 2] The second rotation axis (0: x, 1: y, 2: z) k : int from [0, 1, 2] The third rotation axis (0: x, 1: y, 2: z) extrinsic : bool Do we use extrinsic transformations? Intrinsic otherwise. strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- euler_angles : array, shape (3,) Extracted rotation angles in radians about the axes i, j, k in this order. The first and last angle are normalized to [-pi, pi]. The middle angle is normalized to either [0, pi] (proper Euler angles) or [-pi/2, pi/2] (Cardan / Tait-Bryan angles). Raises ------ ValueError If basis is invalid References ---------- .. [1] Shuster, M. D., Markley, F. L. (2006). General Formula for Extracting the Euler Angles. Journal of Guidance, Control, and Dynamics, 29(1), pp 2015-221, doi: 10.2514/1.16622. https://arc.aiaa.org/doi/abs/10.2514/1.16622 """ check_axis_index("i", i) check_axis_index("j", j) check_axis_index("k", k) basis_vectors = [unitx, unity, unitz] proper_euler = i == k if extrinsic: i, k = k, i e = general_intrinsic_euler_from_active_matrix( R, basis_vectors[i], basis_vectors[j], basis_vectors[k], proper_euler, strict_check, ) if extrinsic: e = e[::-1] return e def euler_from_quaternion(q, i, j, k, extrinsic): """General method to extract any Euler angles from quaternions. Parameters ---------- q : array-like, shape (4,) Unit quaternion to represent rotation: (w, x, y, z) i : int from [0, 1, 2] The first rotation axis (0: x, 1: y, 2: z) j : int from [0, 1, 2] The second rotation axis (0: x, 1: y, 2: z) k : int from [0, 1, 2] The third rotation axis (0: x, 1: y, 2: z) extrinsic : bool Do we use extrinsic transformations? Intrinsic otherwise. Returns ------- euler_angles : array, shape (3,) Extracted rotation angles in radians about the axes i, j, k in this order. The first and last angle are normalized to [-pi, pi]. The middle angle is normalized to either [0, pi] (proper Euler angles) or [-pi/2, pi/2] (Cardan / Tait-Bryan angles). Raises ------ ValueError If basis is invalid References ---------- .. [1] Bernardes, E., Viollet, S. (2022). Quaternion to Euler angles conversion: A direct, general and computationally efficient method. PLOS ONE, 17(11), doi: 10.1371/journal.pone.0276302. """ q = check_quaternion(q) check_axis_index("i", i) check_axis_index("j", j) check_axis_index("k", k) i += 1 j += 1 k += 1 # The original algorithm assumes extrinsic convention. Hence, we swap # the order of axes for intrinsic rotation. if not extrinsic: i, k = k, i # Proper Euler angles rotate about the same axis in the first and last # rotation. If this is not the case, they are called Cardan or Tait-Bryan # angles and have to be handled differently. proper_euler = i == k if proper_euler: k = 6 - i - j sign = (i - j) * (j - k) * (k - i) // 2 a = q[0] b = q[i] c = q[j] d = q[k] * sign if not proper_euler: a, b, c, d = a - c, b + d, c + a, d - b # Equation 34 is used instead of Equation 35 as atan2 it is numerically # more accurate than acos. angle_j = 2.0 * math.atan2(math.hypot(c, d), math.hypot(a, b)) # Check for singularities if abs(angle_j) <= eps: singularity = 1 elif abs(angle_j - math.pi) <= eps: singularity = 2 else: singularity = 0 # Equation 25 # (theta_1 + theta_3) / 2 half_sum = math.atan2(b, a) # (theta_1 - theta_3) / 2 half_diff = math.atan2(d, c) if singularity == 0: # no singularity # Equation 32 angle_i = half_sum + half_diff angle_k = half_sum - half_diff elif extrinsic: # singularity angle_k = 0.0 if singularity == 1: angle_i = 2.0 * half_sum else: assert singularity == 2 angle_i = 2.0 * half_diff else: # intrinsic, singularity angle_i = 0.0 if singularity == 1: angle_k = 2.0 * half_sum else: assert singularity == 2 angle_k = -2.0 * half_diff if not proper_euler: # Equation 43 angle_j -= math.pi / 2.0 # Equation 44 angle_i *= sign angle_k = norm_angle(angle_k) angle_i = norm_angle(angle_i) if extrinsic: return np.array([angle_k, angle_j, angle_i]) return np.array([angle_i, angle_j, angle_k]) ================================================ FILE: pytransform3d/rotations/_euler.pyi ================================================ import numpy as np import numpy.typing as npt def norm_euler(e: npt.ArrayLike, i: int, j: int, k: int) -> np.ndarray: ... def euler_near_gimbal_lock( e: npt.ArrayLike, i: int, j: int, k: int, tolerance: float = ... ) -> bool: ... def assert_euler_equal( e1: npt.ArrayLike, e2: npt.ArrayLike, i: int, j: int, k: int, *args, **kwargs, ): ... def matrix_from_euler( e: npt.ArrayLike, i: int, j: int, k: int, extrinsic: bool ) -> np.ndarray: ... def general_intrinsic_euler_from_active_matrix( R: npt.ArrayLike, n1: np.ndarray, n2: np.ndarray, n3: np.ndarray, proper_euler: bool, strict_check: bool = ..., ) -> np.ndarray: ... def euler_from_matrix( R: npt.ArrayLike, i: int, j: int, k: int, extrinsic: bool, strict_check: bool = ..., ) -> np.ndarray: ... def euler_from_quaternion( q: npt.ArrayLike, i: int, j: int, k: int, extrinsic: bool ) -> np.ndarray: ... ================================================ FILE: pytransform3d/rotations/_euler_deprecated.py ================================================ """Deprecated Euler angle functions.""" import warnings from ._angle import active_matrix_from_angle from ._constants import unitx, unity, unitz from ._euler import general_intrinsic_euler_from_active_matrix from ._matrix import quaternion_from_matrix def active_matrix_from_intrinsic_euler_xzx(e): """Compute active rotation matrix from intrinsic xzx Euler angles. Parameters ---------- e : array-like, shape (3,) Angles for rotation around x-, z'-, and x''-axes (intrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(0, alpha) .dot(active_matrix_from_angle(2, beta)) .dot(active_matrix_from_angle(0, gamma)) ) return R def active_matrix_from_extrinsic_euler_xzx(e): """Compute active rotation matrix from extrinsic xzx Euler angles. Parameters ---------- e : array-like, shape (3,) Angles for rotation around x-, z-, and x-axes (extrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(0, gamma) .dot(active_matrix_from_angle(2, beta)) .dot(active_matrix_from_angle(0, alpha)) ) return R def active_matrix_from_intrinsic_euler_xyx(e): """Compute active rotation matrix from intrinsic xyx Euler angles. Parameters ---------- e : array-like, shape (3,) Angles for rotation around x-, y'-, and x''-axes (intrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(0, alpha) .dot(active_matrix_from_angle(1, beta)) .dot(active_matrix_from_angle(0, gamma)) ) return R def active_matrix_from_extrinsic_euler_xyx(e): """Compute active rotation matrix from extrinsic xyx Euler angles. Parameters ---------- e : array-like, shape (3,) Angles for rotation around x-, y-, and x-axes (extrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(0, gamma) .dot(active_matrix_from_angle(1, beta)) .dot(active_matrix_from_angle(0, alpha)) ) return R def active_matrix_from_intrinsic_euler_yxy(e): """Compute active rotation matrix from intrinsic yxy Euler angles. Parameters ---------- e : array-like, shape (3,) Angles for rotation around y-, x'-, and y''-axes (intrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(1, alpha) .dot(active_matrix_from_angle(0, beta)) .dot(active_matrix_from_angle(1, gamma)) ) return R def active_matrix_from_extrinsic_euler_yxy(e): """Compute active rotation matrix from extrinsic yxy Euler angles. Parameters ---------- e : array-like, shape (3,) Angles for rotation around y-, x-, and y-axes (extrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(1, gamma) .dot(active_matrix_from_angle(0, beta)) .dot(active_matrix_from_angle(1, alpha)) ) return R def active_matrix_from_intrinsic_euler_yzy(e): """Compute active rotation matrix from intrinsic yzy Euler angles. Parameters ---------- e : array-like, shape (3,) Angles for rotation around y-, z'-, and y''-axes (intrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(1, alpha) .dot(active_matrix_from_angle(2, beta)) .dot(active_matrix_from_angle(1, gamma)) ) return R def active_matrix_from_extrinsic_euler_yzy(e): """Compute active rotation matrix from extrinsic yzy Euler angles. Parameters ---------- e : array-like, shape (3,) Angles for rotation around y-, z-, and y-axes (extrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(1, gamma) .dot(active_matrix_from_angle(2, beta)) .dot(active_matrix_from_angle(1, alpha)) ) return R def active_matrix_from_intrinsic_euler_zyz(e): """Compute active rotation matrix from intrinsic zyz Euler angles. Parameters ---------- e : array-like, shape (3,) Angles for rotation around z-, y'-, and z''-axes (intrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(2, alpha) .dot(active_matrix_from_angle(1, beta)) .dot(active_matrix_from_angle(2, gamma)) ) return R def active_matrix_from_extrinsic_euler_zyz(e): """Compute active rotation matrix from extrinsic zyz Euler angles. .. warning:: This function was not implemented correctly in versions 1.3 and 1.4 as the order of the angles was reversed, which actually corresponds to intrinsic rotations. This has been fixed in version 1.5. Parameters ---------- e : array-like, shape (3,) Angles for rotation around z-, y-, and z-axes (extrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(2, gamma) .dot(active_matrix_from_angle(1, beta)) .dot(active_matrix_from_angle(2, alpha)) ) return R def active_matrix_from_intrinsic_euler_zxz(e): """Compute active rotation matrix from intrinsic zxz Euler angles. Parameters ---------- e : array-like, shape (3,) Angles for rotation around z-, x'-, and z''-axes (intrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(2, alpha) .dot(active_matrix_from_angle(0, beta)) .dot(active_matrix_from_angle(2, gamma)) ) return R def active_matrix_from_extrinsic_euler_zxz(e): """Compute active rotation matrix from extrinsic zxz Euler angles. .. warning:: This function was not implemented correctly in versions 1.3 and 1.4 as the order of the angles was reversed, which actually corresponds to intrinsic rotations. This has been fixed in version 1.5. Parameters ---------- e : array-like, shape (3,) Angles for rotation around z-, x-, and z-axes (extrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(2, gamma) .dot(active_matrix_from_angle(0, beta)) .dot(active_matrix_from_angle(2, alpha)) ) return R def active_matrix_from_intrinsic_euler_xzy(e): """Compute active rotation matrix from intrinsic xzy Cardan angles. Parameters ---------- e : array-like, shape (3,) Angles for rotation around x-, z'-, and y''-axes (intrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(0, alpha) .dot(active_matrix_from_angle(2, beta)) .dot(active_matrix_from_angle(1, gamma)) ) return R def active_matrix_from_extrinsic_euler_xzy(e): """Compute active rotation matrix from extrinsic xzy Cardan angles. Parameters ---------- e : array-like, shape (3,) Angles for rotation around x-, z-, and y-axes (extrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(1, gamma) .dot(active_matrix_from_angle(2, beta)) .dot(active_matrix_from_angle(0, alpha)) ) return R def active_matrix_from_intrinsic_euler_xyz(e): """Compute active rotation matrix from intrinsic xyz Cardan angles. Parameters ---------- e : array-like, shape (3,) Angles for rotation around x-, y'-, and z''-axes (intrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(0, alpha) .dot(active_matrix_from_angle(1, beta)) .dot(active_matrix_from_angle(2, gamma)) ) return R def active_matrix_from_extrinsic_euler_xyz(e): """Compute active rotation matrix from extrinsic xyz Cardan angles. Parameters ---------- e : array-like, shape (3,) Angles for rotation around x-, y-, and z-axes (extrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(2, gamma) .dot(active_matrix_from_angle(1, beta)) .dot(active_matrix_from_angle(0, alpha)) ) return R def active_matrix_from_intrinsic_euler_yxz(e): """Compute active rotation matrix from intrinsic yxz Cardan angles. Parameters ---------- e : array-like, shape (3,) Angles for rotation around y-, x'-, and z''-axes (intrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(1, alpha) .dot(active_matrix_from_angle(0, beta)) .dot(active_matrix_from_angle(2, gamma)) ) return R def active_matrix_from_extrinsic_euler_yxz(e): """Compute active rotation matrix from extrinsic yxz Cardan angles. Parameters ---------- e : array-like, shape (3,) Angles for rotation around y-, x-, and z-axes (extrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(2, gamma) .dot(active_matrix_from_angle(0, beta)) .dot(active_matrix_from_angle(1, alpha)) ) return R def active_matrix_from_intrinsic_euler_yzx(e): """Compute active rotation matrix from intrinsic yzx Cardan angles. Parameters ---------- e : array-like, shape (3,) Angles for rotation around y-, z'-, and x''-axes (intrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(1, alpha) .dot(active_matrix_from_angle(2, beta)) .dot(active_matrix_from_angle(0, gamma)) ) return R def active_matrix_from_extrinsic_euler_yzx(e): """Compute active rotation matrix from extrinsic yzx Cardan angles. Parameters ---------- e : array-like, shape (3,) Angles for rotation around y-, z-, and x-axes (extrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(0, gamma) .dot(active_matrix_from_angle(2, beta)) .dot(active_matrix_from_angle(1, alpha)) ) return R def active_matrix_from_intrinsic_euler_zyx(e): """Compute active rotation matrix from intrinsic zyx Cardan angles. Parameters ---------- e : array-like, shape (3,) Angles for rotation around z-, y'-, and x''-axes (intrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(2, alpha) .dot(active_matrix_from_angle(1, beta)) .dot(active_matrix_from_angle(0, gamma)) ) return R def active_matrix_from_extrinsic_euler_zyx(e): """Compute active rotation matrix from extrinsic zyx Cardan angles. Parameters ---------- e : array-like, shape (3,) Angles for rotation around z-, y-, and x-axes (extrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(0, gamma) .dot(active_matrix_from_angle(1, beta)) .dot(active_matrix_from_angle(2, alpha)) ) return R def active_matrix_from_intrinsic_euler_zxy(e): """Compute active rotation matrix from intrinsic zxy Cardan angles. Parameters ---------- e : array-like, shape (3,) Angles for rotation around z-, x'-, and y''-axes (intrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(2, alpha) .dot(active_matrix_from_angle(0, beta)) .dot(active_matrix_from_angle(1, gamma)) ) return R def active_matrix_from_extrinsic_euler_zxy(e): """Compute active rotation matrix from extrinsic zxy Cardan angles. Parameters ---------- e : array-like, shape (3,) Angles for rotation around z-, x-, and y-axes (extrinsic rotations) Returns ------- R : array, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) alpha, beta, gamma = e R = ( active_matrix_from_angle(1, gamma) .dot(active_matrix_from_angle(0, beta)) .dot(active_matrix_from_angle(2, alpha)) ) return R def active_matrix_from_extrinsic_roll_pitch_yaw(rpy): """Compute active rotation matrix from extrinsic roll, pitch, and yaw. Parameters ---------- rpy : array-like, shape (3,) Angles for rotation around x- (roll), y- (pitch), and z-axes (yaw), extrinsic rotations Returns ------- R : array-like, shape (3, 3) Rotation matrix """ warnings.warn( "function is deprecated, use matrix_from_euler", DeprecationWarning, stacklevel=2, ) return active_matrix_from_extrinsic_euler_xyz(rpy) def intrinsic_euler_xzx_from_active_matrix(R, strict_check=True): """Compute intrinsic xzx Euler angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around x-, z'-, and x''-axes (intrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unitx, unitz, unitx, True, strict_check ) def extrinsic_euler_xzx_from_active_matrix(R, strict_check=True): """Compute extrinsic xzx Euler angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around x-, z-, and x-axes (extrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unitx, unitz, unitx, True, strict_check )[::-1] def intrinsic_euler_xyx_from_active_matrix(R, strict_check=True): """Compute intrinsic xyx Euler angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around x-, y'-, and x''-axes (intrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unitx, unity, unitx, True, strict_check ) def extrinsic_euler_xyx_from_active_matrix(R, strict_check=True): """Compute extrinsic xyx Euler angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around x-, y-, and x-axes (extrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unitx, unity, unitx, True, strict_check )[::-1] def intrinsic_euler_yxy_from_active_matrix(R, strict_check=True): """Compute intrinsic yxy Euler angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around y-, x'-, and y''-axes (intrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unity, unitx, unity, True, strict_check ) def extrinsic_euler_yxy_from_active_matrix(R, strict_check=True): """Compute extrinsic yxy Euler angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around y-, x-, and y-axes (extrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unity, unitx, unity, True, strict_check )[::-1] def intrinsic_euler_yzy_from_active_matrix(R, strict_check=True): """Compute intrinsic yzy Euler angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around y-, z'-, and y''-axes (intrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unity, unitz, unity, True, strict_check ) def extrinsic_euler_yzy_from_active_matrix(R, strict_check=True): """Compute extrinsic yzy Euler angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around y-, z-, and y-axes (extrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unity, unitz, unity, True, strict_check )[::-1] def intrinsic_euler_zyz_from_active_matrix(R, strict_check=True): """Compute intrinsic zyz Euler angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around z-, y'-, and z''-axes (intrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unitz, unity, unitz, True, strict_check ) def extrinsic_euler_zyz_from_active_matrix(R, strict_check=True): """Compute extrinsic zyz Euler angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around z-, y-, and z-axes (extrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unitz, unity, unitz, True, strict_check )[::-1] def intrinsic_euler_zxz_from_active_matrix(R, strict_check=True): """Compute intrinsic zxz Euler angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around z-, x'-, and z''-axes (intrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unitz, unitx, unitz, True, strict_check ) def extrinsic_euler_zxz_from_active_matrix(R, strict_check=True): """Compute extrinsic zxz Euler angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around z-, x-, and z-axes (extrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unitz, unitx, unitz, True, strict_check )[::-1] def intrinsic_euler_xzy_from_active_matrix(R, strict_check=True): """Compute intrinsic xzy Cardan angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around x-, z'-, and y''-axes (intrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unitx, unitz, unity, False, strict_check ) def extrinsic_euler_xzy_from_active_matrix(R, strict_check=True): """Compute extrinsic xzy Cardan angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around x-, z-, and y-axes (extrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unity, unitz, unitx, False, strict_check )[::-1] def intrinsic_euler_xyz_from_active_matrix(R, strict_check=True): """Compute intrinsic xyz Cardan angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around x-, y'-, and z''-axes (intrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unitx, unity, unitz, False, strict_check ) def extrinsic_euler_xyz_from_active_matrix(R, strict_check=True): """Compute extrinsic xyz Cardan angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around x-, y-, and z-axes (extrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unitz, unity, unitx, False, strict_check )[::-1] def intrinsic_euler_yxz_from_active_matrix(R, strict_check=True): """Compute intrinsic yxz Cardan angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around y-, x'-, and z''-axes (intrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unity, unitx, unitz, False, strict_check ) def extrinsic_euler_yxz_from_active_matrix(R, strict_check=True): """Compute extrinsic yxz Cardan angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around y-, x-, and z-axes (extrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unitz, unitx, unity, False, strict_check )[::-1] def intrinsic_euler_yzx_from_active_matrix(R, strict_check=True): """Compute intrinsic yzx Cardan angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around y-, z'-, and x''-axes (intrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unity, unitz, unitx, False, strict_check ) def extrinsic_euler_yzx_from_active_matrix(R, strict_check=True): """Compute extrinsic yzx Cardan angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around y-, z-, and x-axes (extrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unitx, unitz, unity, False, strict_check )[::-1] def intrinsic_euler_zyx_from_active_matrix(R, strict_check=True): """Compute intrinsic zyx Cardan angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around z-, y'-, and x''-axes (intrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unitz, unity, unitx, False, strict_check ) def extrinsic_euler_zyx_from_active_matrix(R, strict_check=True): """Compute extrinsic zyx Cardan angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around z-, y-, and x-axes (extrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unitx, unity, unitz, False, strict_check )[::-1] def intrinsic_euler_zxy_from_active_matrix(R, strict_check=True): """Compute intrinsic zxy Cardan angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around z-, x'-, and y''-axes (intrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unitz, unitx, unity, False, strict_check ) def extrinsic_euler_zxy_from_active_matrix(R, strict_check=True): """Compute extrinsic zxy Cardan angles from active rotation matrix. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- e : array, shape (3,) Angles for rotation around z-, x-, and y-axes (extrinsic rotations) """ warnings.warn( "function is deprecated, use euler_from_matrix", DeprecationWarning, stacklevel=2, ) return general_intrinsic_euler_from_active_matrix( R, unity, unitx, unitz, False, strict_check )[::-1] def quaternion_from_extrinsic_euler_xyz(e): """Compute quaternion from extrinsic xyz Euler angles. Parameters ---------- e : array-like, shape (3,) Angles for rotation around x-, y-, and z-axes (extrinsic rotations) Returns ------- q : array, shape (4,) Unit quaternion to represent rotation: (w, x, y, z) """ warnings.warn( "quaternion_from_extrinsic_euler_xyz is deprecated, use " "quaternion_from_euler", DeprecationWarning, stacklevel=2, ) R = active_matrix_from_extrinsic_euler_xyz(e) return quaternion_from_matrix(R) ================================================ FILE: pytransform3d/rotations/_euler_deprecated.pyi ================================================ import numpy as np import numpy.typing as npt def active_matrix_from_intrinsic_euler_xzx(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_extrinsic_euler_xzx(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_intrinsic_euler_xyx(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_extrinsic_euler_xyx(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_intrinsic_euler_yxy(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_extrinsic_euler_yxy(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_intrinsic_euler_yzy(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_extrinsic_euler_yzy(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_intrinsic_euler_zyz(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_extrinsic_euler_zyz(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_intrinsic_euler_zxz(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_extrinsic_euler_zxz(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_intrinsic_euler_xzy(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_extrinsic_euler_xzy(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_intrinsic_euler_xyz(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_extrinsic_euler_xyz(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_intrinsic_euler_yxz(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_extrinsic_euler_yxz(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_intrinsic_euler_yzx(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_extrinsic_euler_yzx(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_intrinsic_euler_zyx(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_extrinsic_euler_zyx(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_intrinsic_euler_zxy(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_extrinsic_euler_zxy(e: npt.ArrayLike) -> np.ndarray: ... def active_matrix_from_extrinsic_roll_pitch_yaw( rpy: npt.ArrayLike, ) -> np.ndarray: ... def intrinsic_euler_xzx_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def extrinsic_euler_xzx_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def intrinsic_euler_xyx_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def extrinsic_euler_xyx_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def intrinsic_euler_yxy_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def extrinsic_euler_yxy_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def intrinsic_euler_yzy_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def extrinsic_euler_yzy_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def intrinsic_euler_zyz_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def extrinsic_euler_zyz_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def intrinsic_euler_zxz_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def extrinsic_euler_zxz_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def intrinsic_euler_xzy_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def extrinsic_euler_xzy_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def intrinsic_euler_xyz_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def extrinsic_euler_xyz_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def intrinsic_euler_yxz_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def extrinsic_euler_yxz_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def intrinsic_euler_yzx_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def extrinsic_euler_yzx_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def intrinsic_euler_zyx_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def extrinsic_euler_zyx_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def intrinsic_euler_zxy_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def extrinsic_euler_zxy_from_active_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def quaternion_from_extrinsic_euler_xyz(e: npt.ArrayLike) -> np.ndarray: ... ================================================ FILE: pytransform3d/rotations/_jacobians.py ================================================ """Jacobians of SO(3).""" import math import numpy as np from ._rot_log import cross_product_matrix def left_jacobian_SO3(omega): r"""Left Jacobian of SO(3) at theta (angle of rotation). .. math:: \boldsymbol{J}(\theta) = \frac{\sin{\theta}}{\theta} \boldsymbol{I} + \left(\frac{1 - \cos{\theta}}{\theta}\right) \left[\hat{\boldsymbol{\omega}}\right] + \left(1 - \frac{\sin{\theta}}{\theta} \right) \hat{\boldsymbol{\omega}} \hat{\boldsymbol{\omega}}^T Parameters ---------- omega : array-like, shape (3,) Compact axis-angle representation. Returns ------- J : array, shape (3, 3) Left Jacobian of SO(3). See also -------- left_jacobian_SO3_series : Left Jacobian of SO(3) at theta from Taylor series. left_jacobian_SO3_inv : Inverse left Jacobian of SO(3) at theta (angle of rotation). """ omega = np.asarray(omega) theta = np.linalg.norm(omega) if theta < np.finfo(float).eps: return left_jacobian_SO3_series(omega, 10) omega_unit = omega / theta omega_matrix = cross_product_matrix(omega_unit) return ( np.eye(3) + (1.0 - math.cos(theta)) / theta * omega_matrix + (1.0 - math.sin(theta) / theta) * np.dot(omega_matrix, omega_matrix) ) def left_jacobian_SO3_series(omega, n_terms): """Left Jacobian of SO(3) at theta from Taylor series. Parameters ---------- omega : array-like, shape (3,) Compact axis-angle representation. n_terms : int Number of terms to include in the series. Returns ------- J : array, shape (3, 3) Left Jacobian of SO(3). See Also -------- left_jacobian_SO3 : Left Jacobian of SO(3) at theta (angle of rotation). """ omega = np.asarray(omega) J = np.eye(3) pxn = np.eye(3) px = cross_product_matrix(omega) for n in range(n_terms): pxn = np.dot(pxn, px) / (n + 2) J += pxn return J def left_jacobian_SO3_inv(omega): r"""Inverse left Jacobian of SO(3) at theta (angle of rotation). .. math:: \boldsymbol{J}^{-1}(\theta) = \frac{\theta}{2 \tan{\frac{\theta}{2}}} \boldsymbol{I} - \frac{\theta}{2} \left[\hat{\boldsymbol{\omega}}\right] + \left(1 - \frac{\theta}{2 \tan{\frac{\theta}{2}}}\right) \hat{\boldsymbol{\omega}} \hat{\boldsymbol{\omega}}^T Parameters ---------- omega : array-like, shape (3,) Compact axis-angle representation. Returns ------- J_inv : array, shape (3, 3) Inverse left Jacobian of SO(3). See Also -------- left_jacobian_SO3 : Left Jacobian of SO(3) at theta (angle of rotation). left_jacobian_SO3_inv_series : Inverse left Jacobian of SO(3) at theta from Taylor series. """ omega = np.asarray(omega) theta = np.linalg.norm(omega) if theta < np.finfo(float).eps: return left_jacobian_SO3_inv_series(omega, 10) omega_unit = omega / theta omega_matrix = cross_product_matrix(omega_unit) return ( np.eye(3) - 0.5 * omega_matrix * theta + (1.0 - 0.5 * theta / np.tan(theta / 2.0)) * np.dot(omega_matrix, omega_matrix) ) def left_jacobian_SO3_inv_series(omega, n_terms): """Inverse left Jacobian of SO(3) at theta from Taylor series. Parameters ---------- omega : array-like, shape (3,) Compact axis-angle representation. n_terms : int Number of terms to include in the series. Returns ------- J_inv : array, shape (3, 3) Inverse left Jacobian of SO(3). See Also -------- left_jacobian_SO3_inv : Inverse left Jacobian of SO(3) at theta (angle of rotation). """ from scipy.special import bernoulli omega = np.asarray(omega) J_inv = np.eye(3) pxn = np.eye(3) px = cross_product_matrix(omega) b = bernoulli(n_terms + 1) for n in range(n_terms): pxn = np.dot(pxn, px / (n + 1)) J_inv += b[n + 1] * pxn return J_inv ================================================ FILE: pytransform3d/rotations/_jacobians.pyi ================================================ import numpy as np import numpy.typing as npt def left_jacobian_SO3(omega: npt.ArrayLike) -> np.ndarray: ... def left_jacobian_SO3_series( omega: npt.ArrayLike, n_terms: int ) -> np.ndarray: ... def left_jacobian_SO3_inv(omega: npt.ArrayLike) -> np.ndarray: ... def left_jacobian_SO3_inv_series( omega: npt.ArrayLike, n_terms: int ) -> np.ndarray: ... ================================================ FILE: pytransform3d/rotations/_matrix.py ================================================ """Rotation matrices.""" import warnings import numpy as np from numpy.testing import assert_array_almost_equal from ._axis_angle import compact_axis_angle from ._utils import norm_vector, perpendicular_to_vectors, vector_projection def check_matrix(R, tolerance=1e-6, strict_check=True): r"""Input validation of a rotation matrix. We check whether R multiplied by its inverse is approximately the identity matrix .. math:: \boldsymbol{R}\boldsymbol{R}^T = \boldsymbol{I} and whether the determinant is positive .. math:: det(\boldsymbol{R}) > 0 Parameters ---------- R : array-like, shape (3, 3) Rotation matrix tolerance : float, optional (default: 1e-6) Tolerance threshold for checks. Default tolerance is the same as in assert_rotation_matrix(R). strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- R : array, shape (3, 3) Validated rotation matrix Raises ------ ValueError If input is invalid See Also -------- norm_matrix : Enforces orthonormality of a rotation matrix. robust_polar_decomposition A more expensive orthonormalization method that spreads the error more evenly between the basis vectors. """ R = np.asarray(R, dtype=np.float64) if R.ndim != 2 or R.shape[0] != 3 or R.shape[1] != 3: raise ValueError( "Expected rotation matrix with shape (3, 3), got " "array-like object with shape %s" % (R.shape,) ) RRT = np.dot(R, R.T) if not np.allclose(RRT, np.eye(3), atol=tolerance): error_msg = ( "Expected rotation matrix, but it failed the test " "for inversion by transposition. np.dot(R, R.T) " "gives %r" % RRT ) if strict_check: raise ValueError(error_msg) warnings.warn(error_msg, UserWarning, stacklevel=2) R_det = np.linalg.det(R) if R_det < 0.0: error_msg = ( "Expected rotation matrix, but it failed the test " "for the determinant, which should be 1 but is %g; " "that is, it probably represents a rotoreflection" % R_det ) if strict_check: raise ValueError(error_msg) warnings.warn(error_msg, UserWarning, stacklevel=2) return R def matrix_requires_renormalization(R, tolerance=1e-6): r"""Check if a rotation matrix needs renormalization. This function will check if :math:`R R^T \approx I`. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix that should be orthonormal. tolerance : float, optional (default: 1e-6) Tolerance for check. Returns ------- required : bool Indicates if renormalization is required. See Also -------- norm_matrix : Orthonormalize rotation matrix. robust_polar_decomposition A more expensive orthonormalization method that spreads the error more evenly between the basis vectors. """ R = np.asarray(R, dtype=float) RRT = np.dot(R, R.T) return not np.allclose(RRT, np.eye(3), atol=tolerance) def norm_matrix(R): r"""Orthonormalize rotation matrix. A rotation matrix is defined as .. math:: \boldsymbol R = \left( \begin{array}{ccc} r_{11} & r_{12} & r_{13}\\ r_{21} & r_{22} & r_{23}\\ r_{31} & r_{32} & r_{33}\\ \end{array} \right) \in SO(3) and must be orthonormal, which results in 6 constraints: * column vectors must have unit norm (3 constraints) * and must be orthogonal to each other (3 constraints) A more compact representation of these constraints is :math:`\boldsymbol R^T \boldsymbol R = \boldsymbol I`. Because of numerical problems, a rotation matrix might not satisfy the constraints anymore. This function will enforce them with Gram-Schmidt orthonormalization optimized for 3 dimensions. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix with small numerical errors. Returns ------- R : array, shape (3, 3) Orthonormalized rotation matrix. See Also -------- check_matrix : Checks orthonormality of a rotation matrix. matrix_requires_renormalization Checks if a rotation matrix needs renormalization. robust_polar_decomposition A more expensive orthonormalization method that spreads the error more evenly between the basis vectors. """ R = np.asarray(R) c2 = R[:, 1] c3 = norm_vector(R[:, 2]) c1 = norm_vector(np.cross(c2, c3)) c2 = norm_vector(np.cross(c3, c1)) return np.column_stack((c1, c2, c3)) def assert_rotation_matrix(R, *args, **kwargs): """Raise an assertion if a matrix is not a rotation matrix. The two properties :math:`\\boldsymbol{I} = \\boldsymbol{R R}^T` and :math:`det(R) = 1` will be checked. See numpy.testing.assert_array_almost_equal for a more detailed documentation of the other parameters. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix args : tuple Positional arguments that will be passed to `assert_array_almost_equal` kwargs : dict Positional arguments that will be passed to `assert_array_almost_equal` """ assert_array_almost_equal(np.dot(R, R.T), np.eye(3), *args, **kwargs) assert_array_almost_equal(np.linalg.det(R), 1.0, *args, **kwargs) def matrix_from_two_vectors(a, b): """Compute rotation matrix from two vectors. We assume that the two given vectors form a plane so that we can compute a third, orthogonal vector with the cross product. The x-axis will point in the same direction as a, the y-axis corresponds to the normalized vector rejection of b on a, and the z-axis is the cross product of the other basis vectors. Parameters ---------- a : array-like, shape (3,) First vector, must not be 0 b : array-like, shape (3,) Second vector, must not be 0 or parallel to a Returns ------- R : array, shape (3, 3) Rotation matrix Raises ------ ValueError If vectors are parallel or one of them is the zero vector """ if np.linalg.norm(a) == 0: raise ValueError("a must not be the zero vector.") if np.linalg.norm(b) == 0: raise ValueError("b must not be the zero vector.") c = perpendicular_to_vectors(a, b) if np.linalg.norm(c) == 0: raise ValueError("a and b must not be parallel.") a = norm_vector(a) b_on_a_projection = vector_projection(b, a) b_on_a_rejection = b - b_on_a_projection b = norm_vector(b_on_a_rejection) c = norm_vector(c) return np.column_stack((a, b, c)) def quaternion_from_matrix(R, strict_check=True): """Compute quaternion from rotation matrix. We usually assume active rotations. .. warning:: When computing a quaternion from the rotation matrix there is a sign ambiguity: q and -q represent the same rotation. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- q : array, shape (4,) Unit quaternion to represent rotation: (w, x, y, z) """ R = check_matrix(R, strict_check=strict_check) q = np.empty(4) # Source: # http://www.euclideanspace.com/maths/geometry/rotations/conversions trace = np.trace(R) if trace > 0.0: sqrt_trace = np.sqrt(1.0 + trace) q[0] = 0.5 * sqrt_trace q[1] = 0.5 / sqrt_trace * (R[2, 1] - R[1, 2]) q[2] = 0.5 / sqrt_trace * (R[0, 2] - R[2, 0]) q[3] = 0.5 / sqrt_trace * (R[1, 0] - R[0, 1]) else: if R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]: sqrt_trace = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) q[0] = 0.5 / sqrt_trace * (R[2, 1] - R[1, 2]) q[1] = 0.5 * sqrt_trace q[2] = 0.5 / sqrt_trace * (R[1, 0] + R[0, 1]) q[3] = 0.5 / sqrt_trace * (R[0, 2] + R[2, 0]) elif R[1, 1] > R[2, 2]: sqrt_trace = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) q[0] = 0.5 / sqrt_trace * (R[0, 2] - R[2, 0]) q[1] = 0.5 / sqrt_trace * (R[1, 0] + R[0, 1]) q[2] = 0.5 * sqrt_trace q[3] = 0.5 / sqrt_trace * (R[2, 1] + R[1, 2]) else: sqrt_trace = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) q[0] = 0.5 / sqrt_trace * (R[1, 0] - R[0, 1]) q[1] = 0.5 / sqrt_trace * (R[0, 2] + R[2, 0]) q[2] = 0.5 / sqrt_trace * (R[2, 1] + R[1, 2]) q[3] = 0.5 * sqrt_trace return q def axis_angle_from_matrix(R, strict_check=True, check=True): """Compute axis-angle from rotation matrix. This operation is called logarithmic map. Note that there are two possible solutions for the rotation axis when the angle is 180 degrees (pi). We usually assume active rotations. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. check : bool, optional (default: True) Check if rotation matrix is valid Returns ------- a : array, shape (4,) Axis of rotation and rotation angle: (x, y, z, angle). The angle is constrained to [0, pi]. """ if check: R = check_matrix(R, strict_check=strict_check) cos_angle = (np.trace(R) - 1.0) / 2.0 angle = np.arccos(min(max(-1.0, cos_angle), 1.0)) if angle == 0.0: # R == np.eye(3) return np.array([1.0, 0.0, 0.0, 0.0]) a = np.empty(4) # We can usually determine the rotation axis by inverting Rodrigues' # formula. Subtracting opposing off-diagonal elements gives us # 2 * sin(angle) * e, # where e is the normalized rotation axis. axis_unnormalized = np.array( [R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]] ) if abs(angle - np.pi) < 1e-4: # np.trace(R) close to -1 # The threshold 1e-4 is a result from this discussion: # https://github.com/dfki-ric/pytransform3d/issues/43 # The standard formula becomes numerically unstable, however, # Rodrigues' formula reduces to R = I + 2 (ee^T - I), with the # rotation axis e, that is, ee^T = 0.5 * (R + I) and we can find the # squared values of the rotation axis on the diagonal of this matrix. # We can still use the original formula to reconstruct the signs of # the rotation axis correctly. # In case of floating point inaccuracies: R_diag = np.clip(np.diag(R), -1.0, 1.0) eeT_diag = 0.5 * (R_diag + 1.0) signs = np.sign(axis_unnormalized) signs[signs == 0.0] = 1.0 a[:3] = np.sqrt(eeT_diag) * signs else: a[:3] = axis_unnormalized # The norm of axis_unnormalized is 2.0 * np.sin(angle), that is, we # could normalize with a[:3] = a[:3] / (2.0 * np.sin(angle)), # but the following is much more precise for angles close to 0 or pi: a[:3] /= np.linalg.norm(a[:3]) a[3] = angle return a def compact_axis_angle_from_matrix(R, check=True): """Compute compact axis-angle from rotation matrix. This operation is called logarithmic map. Note that there are two possible solutions for the rotation axis when the angle is 180 degrees (pi). We usually assume active rotations. Parameters ---------- R : array-like, shape (3, 3) Rotation matrix check : bool, optional (default: True) Check if rotation matrix is valid Returns ------- a : array, shape (3,) Axis of rotation and rotation angle: angle * (x, y, z). The angle is constrained to [0, pi]. """ a = axis_angle_from_matrix(R, check=check) return compact_axis_angle(a) ================================================ FILE: pytransform3d/rotations/_matrix.pyi ================================================ import numpy as np import numpy.typing as npt def check_matrix( R: npt.ArrayLike, tolerance: float = ..., strict_check: bool = ... ) -> np.ndarray: ... def matrix_requires_renormalization( R: npt.ArrayLike, tolerance: float = ... ) -> bool: ... def norm_matrix(R: npt.ArrayLike) -> np.ndarray: ... def assert_rotation_matrix(R: npt.ArrayLike, *args, **kwargs): ... def matrix_from_two_vectors( a: npt.ArrayLike, b: npt.ArrayLike ) -> np.ndarray: ... def quaternion_from_matrix( R: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def axis_angle_from_matrix( R: npt.ArrayLike, strict_check: bool = ..., check: bool = ... ) -> np.ndarray: ... def compact_axis_angle_from_matrix( R: npt.ArrayLike, check: bool = ... ) -> np.ndarray: ... ================================================ FILE: pytransform3d/rotations/_mrp.py ================================================ """Modified Rodrigues parameters.""" import numpy as np from numpy.testing import assert_array_almost_equal from ._angle import norm_angle from ._axis_angle import mrp_from_axis_angle from ._constants import two_pi, eps def check_mrp(mrp): """Input validation of modified Rodrigues parameters. Parameters ---------- mrp : array-like, shape (3,) Modified Rodrigues parameters. Returns ------- mrp : array, shape (3,) Validated modified Rodrigues parameters. Raises ------ ValueError If input is invalid """ mrp = np.asarray(mrp) if mrp.ndim != 1 or mrp.shape[0] != 3: raise ValueError( "Expected modified Rodrigues parameters with shape (3,), got " "array-like object with shape %s" % (mrp.shape,) ) return mrp def norm_mrp(mrp): """Normalize angle of modified Rodrigues parameters to range [-pi, pi]. Normalization of modified Rodrigues parameters is required to avoid the singularity at a rotation angle of 2 * pi. Parameters ---------- mrp : array-like, shape (3,) Modified Rodrigues parameters. Returns ------- mrp : array, shape (3,) Modified Rodrigues parameters with angle normalized to [-pi, pi]. """ mrp = check_mrp(mrp) a = axis_angle_from_mrp(mrp) a[3] = norm_angle(a[3]) return mrp_from_axis_angle(a) def mrp_near_singularity(mrp, tolerance=1e-6): """Check if modified Rodrigues parameters are close to singularity. MRPs have a singularity at 2 * pi, i.e., the norm approaches infinity as the angle approaches 2 * pi. Parameters ---------- mrp : array-like, shape (3,) Modified Rodrigues parameters. tolerance : float, optional (default: 1e-6) Tolerance for check. Returns ------- near_singularity : bool MRPs are near singularity. """ check_mrp(mrp) mrp_norm = np.linalg.norm(mrp) angle = np.arctan(mrp_norm) * 4.0 return abs(angle - two_pi) < tolerance def mrp_double(mrp): r"""Other modified Rodrigues parameters representing the same orientation. MRPs have two representations for the same rotation: :math:`\boldsymbol{\psi}` and :math:`-\frac{1}{||\boldsymbol{\psi}||^2} \boldsymbol{\psi}` represent the same rotation and correspond to two antipodal unit quaternions [1]_. No rotation is a special case, in which no second representation exists. Only the zero vector represents no rotation. Parameters ---------- mrp : array-like, shape (3,) Modified Rodrigues parameters. Returns ------- mrp_double : array, shape (3,) Different modified Rodrigues parameters that represent the same orientation. References ---------- .. [1] Shuster, M. D. (1993). A Survey of Attitude Representations. Journal of the Astronautical Sciences, 41, 439-517. http://malcolmdshuster.com/Pub_1993h_J_Repsurv_scan.pdf """ mrp = check_mrp(mrp) norm = np.dot(mrp, mrp) if norm == 0.0: return mrp return mrp / -norm def assert_mrp_equal(mrp1, mrp2, *args, **kwargs): """Raise an assertion if two MRPs are not approximately equal. There are two MRPs that represent the same orientation (double cover). See numpy.testing.assert_array_almost_equal for a more detailed documentation of the other parameters. Parameters ---------- mrp1 : array-like, shape (3,) Modified Rodrigues parameters. mrp1 : array-like, shape (3,) Modified Rodrigues parameters. args : tuple Positional arguments that will be passed to `assert_array_almost_equal` kwargs : dict Positional arguments that will be passed to `assert_array_almost_equal` """ try: assert_array_almost_equal(mrp1, mrp2, *args, **kwargs) except AssertionError: assert_array_almost_equal(mrp1, mrp_double(mrp2), *args, **kwargs) def concatenate_mrp(mrp1, mrp2): r"""Concatenate two rotations defined by modified Rodrigues parameters. Suppose we want to apply two extrinsic rotations given by modified Rodrigues parameters mrp1 and mrp2 to a vector v. We can either apply mrp2 to v and then mrp1 to the result or we can concatenate mrp1 and mrp2 and apply the result to v. The solution for concatenation of two rotations :math:`\boldsymbol{\psi}_1,\boldsymbol{\psi}_2` is given by Shuster [1]_ (Equation 257): .. math:: \boldsymbol{\psi} = \frac{ (1 - ||\boldsymbol{\psi}_1||^2) \boldsymbol{\psi}_2 + (1 - ||\boldsymbol{\psi}_2||^2) \boldsymbol{\psi}_1 - 2 \boldsymbol{\psi}_2 \times \boldsymbol{\psi}_1} {1 + ||\boldsymbol{\psi}_2||^2 ||\boldsymbol{\psi}_1||^2 - 2 \boldsymbol{\psi}_2 \cdot \boldsymbol{\psi}_1}. Parameters ---------- mrp1 : array-like, shape (3,) Modified Rodrigues parameters. mrp2 : array-like, shape (3,) Modified Rodrigues parameters. Returns ------- mrp12 : array, shape (3,) Modified Rodrigues parameters that represent the concatenated rotation of mrp1 and mrp2. References ---------- .. [1] Shuster, M. D. (1993). A Survey of Attitude Representations. Journal of the Astronautical Sciences, 41, 439-517. http://malcolmdshuster.com/Pub_1993h_J_Repsurv_scan.pdf """ mrp1 = check_mrp(mrp1) mrp2 = check_mrp(mrp2) norm1_sq = np.linalg.norm(mrp1) ** 2 norm2_sq = np.linalg.norm(mrp2) ** 2 cross_product = np.cross(mrp2, mrp1) scalar_product = np.dot(mrp2, mrp1) return ( (1.0 - norm1_sq) * mrp2 + (1.0 - norm2_sq) * mrp1 - 2.0 * cross_product ) / (1.0 + norm2_sq * norm1_sq - 2.0 * scalar_product) def mrp_prod_vector(mrp, v): r"""Apply rotation represented by MRPs to a vector. To apply the rotation defined by modified Rodrigues parameters :math:`\boldsymbol{\psi} \in \mathbb{R}^3` to a vector :math:`\boldsymbol{v} \in \mathbb{R}^3`, we left-concatenate the original MRPs and then right-concatenate its inverted (negative) version. Parameters ---------- mrp : array-like, shape (3,) Modified Rodrigues parameters. v : array-like, shape (3,) 3d vector Returns ------- w : array, shape (3,) 3d vector See Also -------- concatenate_mrp : Concatenates MRPs. q_prod_vector : The same operation with a quaternion. """ mrp = check_mrp(mrp) return concatenate_mrp(concatenate_mrp(mrp, v), -mrp) def quaternion_from_mrp(mrp): """Compute quaternion from modified Rodrigues parameters. Parameters ---------- mrp : array-like, shape (3,) Modified Rodrigues parameters. Returns ------- q : array, shape (4,) Unit quaternion to represent rotation: (w, x, y, z) """ mrp = check_mrp(mrp) dot_product_p1 = np.dot(mrp, mrp) + 1.0 q = np.empty(4, dtype=float) q[0] = (2.0 - dot_product_p1) / dot_product_p1 q[1:] = 2.0 * mrp / dot_product_p1 return q def axis_angle_from_mrp(mrp): """Compute axis-angle representation from modified Rodrigues parameters. Parameters ---------- mrp : array-like, shape (3,) Modified Rodrigues parameters. Returns ------- a : array, shape (4,) Axis of rotation and rotation angle: (x, y, z, angle) """ mrp = check_mrp(mrp) mrp_norm = np.linalg.norm(mrp) angle = np.arctan(mrp_norm) * 4.0 if abs(angle) < eps: return np.array([1.0, 0.0, 0.0, 0.0]) axis = mrp / mrp_norm return np.hstack((axis, (angle,))) ================================================ FILE: pytransform3d/rotations/_mrp.pyi ================================================ import numpy as np import numpy.typing as npt def check_mrp(mrp: npt.ArrayLike) -> np.ndarray: ... def norm_mrp(mrp: npt.ArrayLike) -> np.ndarray: ... def mrp_near_singularity( mrp: npt.ArrayLike, tolerance: float = ... ) -> bool: ... def mrp_double(mrp: npt.ArrayLike) -> np.ndarray: ... def assert_mrp_equal( mrp1: npt.ArrayLike, mrp2: npt.ArrayLike, *args, **kwargs ): ... def concatenate_mrp(mrp1: npt.ArrayLike, mrp2: npt.ArrayLike) -> np.ndarray: ... def mrp_prod_vector(mrp: npt.ArrayLike, v: npt.ArrayLike) -> np.ndarray: ... def quaternion_from_mrp(mrp: npt.ArrayLike) -> np.ndarray: ... def axis_angle_from_mrp(mrp: npt.ArrayLike) -> np.ndarray: ... ================================================ FILE: pytransform3d/rotations/_plot.py ================================================ """Plotting functions.""" import numpy as np from ._axis_angle import check_axis_angle from ._constants import a_id, p0, unitx, unity from ._matrix import check_matrix from ._rotors import wedge, plane_normal_from_bivector from ._slerp import slerp_weights from ._utils import perpendicular_to_vectors, angle_between_vectors def plot_basis( ax=None, R=None, p=np.zeros(3), s=1.0, ax_s=1, strict_check=True, **kwargs ): """Plot basis of a rotation matrix. Parameters ---------- ax : Matplotlib 3d axis, optional (default: None) If the axis is None, a new 3d axis will be created R : array-like, shape (3, 3), optional (default: I) Rotation matrix, each column contains a basis vector p : array-like, shape (3,), optional (default: [0, 0, 0]) Offset from the origin s : float, optional (default: 1) Scaling of the frame that will be drawn ax_s : float, optional (default: 1) Scaling of the new matplotlib 3d axis strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. kwargs : dict, optional (default: {}) Additional arguments for the plotting functions, e.g. alpha Returns ------- ax : Matplotlib 3d axis New or old axis """ from ..plot_utils import make_3d_axis, Frame if ax is None: ax = make_3d_axis(ax_s) if R is None: R = np.eye(3) R = check_matrix(R, strict_check=strict_check) A2B = np.eye(4) A2B[:3, :3] = R A2B[:3, 3] = p frame = Frame(A2B, s=s, **kwargs) frame.add_frame(ax) return ax def plot_axis_angle(ax=None, a=a_id, p=p0, s=1.0, ax_s=1, **kwargs): """Plot rotation axis and angle. Parameters ---------- ax : Matplotlib 3d axis, optional (default: None) If the axis is None, a new 3d axis will be created a : array-like, shape (4,), optional (default: [1, 0, 0, 0]) Axis of rotation and rotation angle: (x, y, z, angle) p : array-like, shape (3,), optional (default: [0, 0, 0]) Offset from the origin s : float, optional (default: 1) Scaling of the axis and angle that will be drawn ax_s : float, optional (default: 1) Scaling of the new matplotlib 3d axis kwargs : dict, optional (default: {}) Additional arguments for the plotting functions, e.g. alpha Returns ------- ax : Matplotlib 3d axis New or old axis """ from ..plot_utils import make_3d_axis, Arrow3D a = check_axis_angle(a) if ax is None: ax = make_3d_axis(ax_s) ax.add_artist( Arrow3D( [p[0], p[0] + s * a[0]], [p[1], p[1] + s * a[1]], [p[2], p[2] + s * a[2]], mutation_scale=20, lw=3, arrowstyle="-|>", color="k", ) ) arc = _arc_axis_angle(a, p, s) ax.plot(arc[:-5, 0], arc[:-5, 1], arc[:-5, 2], color="k", lw=3, **kwargs) arrow_coords = np.vstack((arc[-1], arc[-1] + 20 * (arc[-1] - arc[-3]))).T ax.add_artist( Arrow3D( arrow_coords[0], arrow_coords[1], arrow_coords[2], mutation_scale=20, lw=3, arrowstyle="-|>", color="k", ) ) for i in [0, -1]: arc_bound = np.vstack((p + 0.5 * s * a[:3], arc[i])).T ax.plot(arc_bound[0], arc_bound[1], arc_bound[2], "--", c="k") return ax def _arc_axis_angle(a, p, s): # pragma: no cover """Compute arc defined by a around p with scaling s. Parameters ---------- a : array-like, shape (4,) Axis of rotation and rotation angle: (x, y, z, angle) p : array-like, shape (3,), optional (default: [0, 0, 0]) Offset from the origin s : float, optional (default: 1) Scaling of the axis and angle that will be drawn Returns ------- arc : array, shape (100, 3) Coordinates of arc """ p1 = ( unitx if np.abs(a[0]) <= np.finfo(float).eps else perpendicular_to_vectors(unity, a[:3]) ) p2 = perpendicular_to_vectors(a[:3], p1) angle_p1p2 = angle_between_vectors(p1, p2) arc = np.empty((100, 3)) for i, t in enumerate(np.linspace(0, 2 * a[3] / np.pi, len(arc))): w1, w2 = slerp_weights(angle_p1p2, t) arc[i] = p + 0.5 * s * (a[:3] + w1 * p1 + w2 * p2) return arc def plot_bivector(ax=None, a=None, b=None, ax_s=1): """Plot bivector from wedge product of two vectors a and b. The two vectors will be displayed in grey together with the parallelogram that they form. Each component of the bivector corresponds to the area of the parallelogram projected on the basis planes. These parallelograms will be shown as well. Furthermore, one black arrow will show the rotation direction of the bivector and one black arrow will represent the normal of the plane that can be extracted by rearranging the elements of the bivector and normalizing the vector. Parameters ---------- ax : Matplotlib 3d axis, optional (default: None) If the axis is None, a new 3d axis will be created a : array-like, shape (3,), optional (default: [1, 0, 0]) Vector b : array-like, shape (3,), optional (default: [0, 1, 0]) Vector ax_s : float, optional (default: 1) Scaling of the new matplotlib 3d axis Returns ------- ax : Matplotlib 3d axis New or old axis """ from ..plot_utils import make_3d_axis, Arrow3D, plot_vector if ax is None: ax = make_3d_axis(ax_s) if a is None: a = np.array([1, 0, 0]) if b is None: b = np.array([0, 1, 0]) B = wedge(a, b) normal = plane_normal_from_bivector(B) _plot_projected_parallelograms(ax, a, b) arc = _arc_between_vectors(a, b) ax.plot(arc[:-5, 0], arc[:-5, 1], arc[:-5, 2], color="k", lw=2) arrow_coords = np.vstack((arc[-6], arc[-6] + 10 * (arc[-1] - arc[-3]))).T angle_arrow = Arrow3D( arrow_coords[0], arrow_coords[1], arrow_coords[2], mutation_scale=20, lw=2, arrowstyle="-|>", color="k", ) ax.add_artist(angle_arrow) plot_vector(ax=ax, start=np.zeros(3), direction=normal, color="k") return ax def _plot_projected_parallelograms(ax, a, b): """Plot projected parallelograms. We visualize a plane described by two vectors in 3D as a parallelogram. Furthermore, we project the parallelogram to the three basis planes and visualize these projections as parallelograms. Parameters ---------- ax : Matplotlib 3d axis Axis a : array, shape (3,) Vector that defines the first side b : array, shape (3,) Vector that defines the second side """ _plot_parallelogram(ax, a, b, "#aaaaaa", 0.5) a_xy = np.array([a[0], a[1], 0]) b_xy = np.array([b[0], b[1], 0]) _plot_parallelogram(ax, a_xy, b_xy, "#ffff00", 0.5) a_xz = np.array([a[0], 0, a[2]]) b_xz = np.array([b[0], 0, b[2]]) _plot_parallelogram(ax, a_xz, b_xz, "#ff00ff", 0.5) a_yz = np.array([0, a[1], a[2]]) b_yz = np.array([0, b[1], b[2]]) _plot_parallelogram(ax, a_yz, b_yz, "#00ffff", 0.5) def _plot_parallelogram(ax, a, b, color, alpha): """Plot parallelogram. Parameters ---------- ax : Matplotlib 3d axis Axis a : array, shape (3,) Vector that defines the first side b : array, shape (3,) Vector that defines the second side color : str Color alpha : float Alpha of surface """ from ..plot_utils import plot_vector from mpl_toolkits.mplot3d.art3d import Poly3DCollection plot_vector(ax, start=np.zeros(3), direction=a, color=color, alpha=alpha) plot_vector(ax, start=np.zeros(3), direction=b, color=color, alpha=alpha) verts = np.vstack((np.zeros(3), a, b, a + b, b, a)) try: # Matplotlib < 3.5 parallelogram = Poly3DCollection(verts) except ValueError: # Matplotlib >= 3.5 parallelogram = Poly3DCollection(verts.reshape(2, 3, 3)) parallelogram.set_facecolor(color) parallelogram.set_alpha(alpha) ax.add_collection3d(parallelogram) def _arc_between_vectors(a, b): # pragma: no cover """Compute arc defined by a around p with scaling s. Parameters ---------- a : array-like, shape (3,) Vector b : array-like, shape (3,) Vector Returns ------- arc : array, shape (100, 3) Coordinates of arc """ angle = angle_between_vectors(a, b) arc = np.empty((100, 3)) for i, t in enumerate(np.linspace(0, angle, len(arc))): w1, w2 = slerp_weights(angle, t) arc[i] = 0.5 * (w1 * a + w2 * b) return arc ================================================ FILE: pytransform3d/rotations/_plot.pyi ================================================ import numpy as np import numpy.typing as npt from mpl_toolkits.mplot3d import Axes3D from typing import Union def plot_basis( ax: Union[None, Axes3D] = ..., R: Union[None, npt.ArrayLike] = ..., p: npt.ArrayLike = ..., s: float = ..., ax_s: float = ..., strict_check: bool = ..., **kwargs, ) -> Axes3D: ... def plot_axis_angle( ax: Union[None, Axes3D] = ..., a: npt.ArrayLike = ..., p: npt.ArrayLike = ..., s: float = ..., ax_s: float = ..., **kwargs, ) -> Axes3D: ... def plot_bivector( ax: Union[None, Axes3D] = ..., a: Union[None, npt.ArrayLike] = ..., b: Union[None, npt.ArrayLike] = ..., ax_s: float = ..., ): ... ================================================ FILE: pytransform3d/rotations/_polar_decomp.py ================================================ """Polar decomposition.""" import numpy as np from ._axis_angle import matrix_from_compact_axis_angle def robust_polar_decomposition(A, n_iter=20, eps=np.finfo(float).eps): r"""Orthonormalize rotation matrix with robust polar decomposition. Robust polar decomposition [1]_ [2]_ is a computationally more costly method, but it spreads the error more evenly between the basis vectors in comparison to Gram-Schmidt orthonormalization (as in :func:`norm_matrix`). Robust polar decomposition finds an orthonormal matrix that minimizes the Frobenius norm .. math:: ||\boldsymbol{A} - \boldsymbol{R}||^2 between the input :math:`\boldsymbol{A}` that is not orthonormal and the output :math:`\boldsymbol{R}` that is orthonormal. Parameters ---------- A : array-like, shape (3, 3) Matrix that contains a basis vector in each column. The basis does not have to be orthonormal. n_iter : int, optional (default: 20) Maximum number of iterations for which we refine the estimation of the rotation matrix. eps : float, optional (default: np.finfo(float).eps) Precision for termination criterion of iterative refinement. Returns ------- R : array, shape (3, 3) Orthonormalized rotation matrix. See Also -------- norm_matrix The cheaper default orthonormalization method that uses Gram-Schmidt orthonormalization optimized for 3 dimensions. References ---------- .. [1] Selstad, J. (2019). Orthonormalization. https://zalo.github.io/blog/polar-decomposition/ .. [2] Müller, M., Bender, J., Chentanez, N., Macklin, M. (2016). A Robust Method to Extract the Rotational Part of Deformations. In MIG '16: Proceedings of the 9th International Conference on Motion in Games, pp. 55-60, doi: 10.1145/2994258.2994269. """ current_R = np.eye(3) for _ in range(n_iter): column_vector_cross_products = np.cross( current_R, A, axisa=0, axisb=0, axisc=1 ) column_vector_dot_products_sum = np.sum(current_R * A) omega = column_vector_cross_products.sum(axis=0) / ( abs(column_vector_dot_products_sum) + eps ) if np.linalg.norm(omega) < eps: break current_R = np.dot(matrix_from_compact_axis_angle(omega), current_R) return current_R ================================================ FILE: pytransform3d/rotations/_polar_decomp.pyi ================================================ import numpy as np import numpy.typing as npt def robust_polar_decomposition( A: npt.ArrayLike, n_iter: int = ..., eps: float = ... ) -> np.ndarray: ... ================================================ FILE: pytransform3d/rotations/_quaternion.py ================================================ """Quaternion operations.""" import numpy as np from numpy.testing import assert_array_almost_equal from ._angle import quaternion_from_angle from ._axis_angle import ( norm_axis_angle, quaternion_from_compact_axis_angle, compact_axis_angle, ) from ._utils import check_axis_index, norm_vector def check_quaternion(q, unit=True): """Input validation of quaternion representation. Parameters ---------- q : array-like, shape (4,) Quaternion to represent rotation: (w, x, y, z) unit : bool, optional (default: True) Normalize the quaternion so that it is a unit quaternion Returns ------- q : array, shape (4,) Validated quaternion to represent rotation: (w, x, y, z) Raises ------ ValueError If input is invalid """ q = np.asarray(q, dtype=np.float64) if q.ndim != 1 or q.shape[0] != 4: raise ValueError( "Expected quaternion with shape (4,), got " "array-like object with shape %s" % (q.shape,) ) if unit: return norm_vector(q) return q def check_quaternions(Q, unit=True): """Input validation of quaternion representation. Parameters ---------- Q : array-like, shape (n_steps, 4) Quaternions to represent rotations: (w, x, y, z) unit : bool, optional (default: True) Normalize the quaternions so that they are unit quaternions Returns ------- Q : array, shape (n_steps, 4) Validated quaternions to represent rotations: (w, x, y, z) Raises ------ ValueError If input is invalid """ Q_checked = np.asarray(Q, dtype=np.float64) if Q_checked.ndim != 2 or Q_checked.shape[1] != 4: raise ValueError( "Expected quaternion array with shape (n_steps, 4), got " "array-like object with shape %s" % (Q_checked.shape,) ) if unit: for i in range(len(Q)): Q_checked[i] = norm_vector(Q_checked[i]) return Q_checked def quaternion_requires_renormalization(q, tolerance=1e-6): r"""Check if a unit quaternion requires renormalization. Quaternions that represent rotations should have unit norm, so we check :math:`||\boldsymbol{q}|| \approx 1`. Parameters ---------- q : array-like, shape (4,) Quaternion to represent rotation: (w, x, y, z) tolerance : float, optional (default: 1e-6) Tolerance for check. Returns ------- required : bool Renormalization is required. See Also -------- check_quaternion : Normalizes quaternion. """ return abs(np.linalg.norm(q) - 1.0) > tolerance def quaternion_double(q): """Create another quaternion that represents the same orientation. The unit quaternions q and -q represent the same orientation (double cover). Parameters ---------- q : array-like, shape (4,) Unit quaternion. Returns ------- q_double : array, shape (4,) -q See Also -------- pick_closest_quaternion Picks the quaternion that is closest to another one in Euclidean space. """ return -check_quaternion(q, unit=True) def pick_closest_quaternion(quaternion, target_quaternion): """Resolve quaternion ambiguity and pick the closest one to the target. .. warning:: There are always two quaternions that represent the exact same orientation: q and -q. Parameters ---------- quaternion : array-like, shape (4,) Quaternion (w, x, y, z) of which we are unsure whether we want to select quaternion or -quaternion. target_quaternion : array-like, shape (4,) Target quaternion (w, x, y, z) to which we want to be close. Returns ------- closest_quaternion : array, shape (4,) Quaternion that is closest (Euclidean norm) to the target quaternion. """ quaternion = check_quaternion(quaternion) target_quaternion = check_quaternion(target_quaternion) return pick_closest_quaternion_impl(quaternion, target_quaternion) def pick_closest_quaternion_impl(quaternion, target_quaternion): """Resolve quaternion ambiguity and pick the closest one to the target. This is an internal function that does not validate the inputs. Parameters ---------- quaternion : array, shape (4,) Quaternion (w, x, y, z) of which we are unsure whether we want to select quaternion or -quaternion. target_quaternion : array, shape (4,) Target quaternion (w, x, y, z) to which we want to be close. Returns ------- closest_quaternion : array, shape (4,) Quaternion that is closest (Euclidean norm) to the target quaternion. """ if np.linalg.norm(-quaternion - target_quaternion) < np.linalg.norm( quaternion - target_quaternion ): return -quaternion return quaternion def assert_quaternion_equal(q1, q2, *args, **kwargs): """Raise an assertion if two quaternions are not approximately equal. Note that quaternions are equal either if q1 == q2 or if q1 == -q2. See numpy.testing.assert_array_almost_equal for a more detailed documentation of the other parameters. Parameters ---------- q1 : array-like, shape (4,) Unit quaternion to represent rotation: (w, x, y, z) q2 : array-like, shape (4,) Unit quaternion to represent rotation: (w, x, y, z) args : tuple Positional arguments that will be passed to `assert_array_almost_equal` kwargs : dict Positional arguments that will be passed to `assert_array_almost_equal` """ try: assert_array_almost_equal(q1, q2, *args, **kwargs) except AssertionError: assert_array_almost_equal(q1, -q2, *args, **kwargs) def quaternion_integrate(Qd, q0=np.array([1.0, 0.0, 0.0, 0.0]), dt=1.0): """Integrate angular velocities to quaternions. Angular velocities are given in global frame and will be left-multiplied to the initial or previous orientation respectively. Parameters ---------- Qd : array-like, shape (n_steps, 3) Angular velocities in a compact axis-angle representation. Each angular velocity represents the rotational offset after one unit of time. q0 : array-like, shape (4,), optional (default: [1, 0, 0, 0]) Unit quaternion to represent initial orientation: (w, x, y, z) dt : float, optional (default: 1) Time interval between steps. Returns ------- Q : array-like, shape (n_steps, 4) Quaternions to represent rotations: (w, x, y, z) """ Q = np.empty((len(Qd), 4)) Q[0] = q0 for t in range(1, len(Qd)): qd = (Qd[t] + Qd[t - 1]) / 2.0 Q[t] = concatenate_quaternions( quaternion_from_compact_axis_angle(dt * qd), Q[t - 1] ) return Q def quaternion_gradient(Q, dt=1.0): """Time-derivatives of a sequence of quaternions. Note that this function does not provide the exact same functionality for quaternions as `NumPy's gradient function `_ for positions. Gradients are always computed as central differences except the first and last gradient. We additionally accept a parameter dt that defines the time interval between each quaternion. Note that this means that we expect this to be constant for the whole sequence. Parameters ---------- Q : array-like, shape (n_steps, 4) Quaternions to represent rotations: (w, x, y, z) dt : float, optional (default: 1) Time interval between steps. If you have non-constant dt, you can pass 1 and manually divide angular velocities by their corresponding time interval afterwards. Returns ------- A : array, shape (n_steps, 3) Angular velocities in a compact axis-angle representation. Each angular velocity represents the rotational offset after one unit of time. Angular velocities are given in global frame and will be left-multiplied during integration to the initial or previous orientation respectively. """ Q = check_quaternions(Q) Qd = np.empty((len(Q), 3)) Qd[0] = ( compact_axis_angle_from_quaternion( concatenate_quaternions(Q[1], q_conj(Q[0])) ) / dt ) for t in range(1, len(Q) - 1): # divided by two because of central differences Qd[t] = compact_axis_angle_from_quaternion( concatenate_quaternions(Q[t + 1], q_conj(Q[t - 1])) ) / (2.0 * dt) Qd[-1] = ( compact_axis_angle_from_quaternion( concatenate_quaternions(Q[-1], q_conj(Q[-2])) ) / dt ) return Qd def concatenate_quaternions(q1, q2): r"""Concatenate two quaternions. We concatenate two quaternions by quaternion multiplication :math:`\boldsymbol{q}_1\boldsymbol{q}_2`. We use Hamilton's quaternion multiplication. If the two quaternions are divided up into scalar part and vector part each, i.e., :math:`\boldsymbol{q} = (w, \boldsymbol{v}), w \in \mathbb{R}, \boldsymbol{v} \in \mathbb{R}^3`, then the quaternion product is .. math:: \boldsymbol{q}_{12} = (w_1 w_2 - \boldsymbol{v}_1 \cdot \boldsymbol{v}_2, w_1 \boldsymbol{v}_2 + w_2 \boldsymbol{v}_1 + \boldsymbol{v}_1 \times \boldsymbol{v}_2) with the scalar product :math:`\cdot` and the cross product :math:`\times`. Parameters ---------- q1 : array-like, shape (4,) First quaternion q2 : array-like, shape (4,) Second quaternion Returns ------- q12 : array, shape (4,) Quaternion that represents the concatenated rotation q1 * q2 See Also -------- concatenate_rotors : Concatenate rotors, which is the same operation. """ q1 = check_quaternion(q1, unit=False) q2 = check_quaternion(q2, unit=False) q12 = np.empty(4) q12[0] = q1[0] * q2[0] - np.dot(q1[1:], q2[1:]) q12[1:] = q1[0] * q2[1:] + q2[0] * q1[1:] + np.cross(q1[1:], q2[1:]) return q12 def q_prod_vector(q, v): r"""Apply rotation represented by a quaternion to a vector. We use Hamilton's quaternion multiplication. To apply the rotation defined by a unit quaternion :math:`\boldsymbol{q} \in S^3` to a vector :math:`\boldsymbol{v} \in \mathbb{R}^3`, we first represent the vector as a quaternion: we set the scalar part to 0 and the vector part is exactly the original vector :math:`\left(\begin{array}{c}0\\\boldsymbol{v}\end{array}\right) \in \mathbb{R}^4`. Then we left-multiply the quaternion and right-multiply its conjugate .. math:: \left(\begin{array}{c}0\\\boldsymbol{w}\end{array}\right) = \boldsymbol{q} \cdot \left(\begin{array}{c}0\\\boldsymbol{v}\end{array}\right) \cdot \boldsymbol{q}^*. The vector part :math:`\boldsymbol{w}` of the resulting quaternion is the rotated vector. Parameters ---------- q : array-like, shape (4,) Unit quaternion to represent rotation: (w, x, y, z) v : array-like, shape (3,) 3d vector Returns ------- w : array, shape (3,) 3d vector See Also -------- rotor_apply The same operation with a different name. concatenate_quaternions Hamilton's quaternion multiplication. """ q = check_quaternion(q) t = 2 * np.cross(q[1:], v) return v + q[0] * t + np.cross(q[1:], t) def q_conj(q): r"""Conjugate of quaternion. The conjugate of a quaternion :math:`\boldsymbol{q}` is often denoted as :math:`\boldsymbol{q}^*`. For a quaternion :math:`\boldsymbol{q} = w + x \boldsymbol{i} + y \boldsymbol{j} + z \boldsymbol{k}` it is defined as .. math:: \boldsymbol{q}^* = w - x \boldsymbol{i} - y \boldsymbol{j} - z \boldsymbol{k}. The conjugate of a unit quaternion inverts the rotation represented by this unit quaternion. Parameters ---------- q : array-like, shape (4,) Quaternion: (w, x, y, z) Returns ------- q_c : array-like, shape (4,) Conjugate (w, -x, -y, -z) See Also -------- rotor_reverse : Reverse of a rotor, which is the same operation. """ q = check_quaternion(q, unit=False) return np.array([q[0], -q[1], -q[2], -q[3]]) def quaternion_dist(q1, q2): r"""Compute distance between two quaternions. We use the angular metric of :math:`S^3`, which is defined as .. math:: d(q_1, q_2) = \min(|| \log(q_1 * \overline{q_2})||, 2 \pi - || \log(q_1 * \overline{q_2})||) Parameters ---------- q1 : array-like, shape (4,) First quaternion q2 : array-like, shape (4,) Second quaternion Returns ------- dist : float Distance between q1 and q2 """ q1 = check_quaternion(q1) q2 = check_quaternion(q2) q12c = concatenate_quaternions(q1, q_conj(q2)) angle = axis_angle_from_quaternion(q12c)[-1] return min(angle, 2.0 * np.pi - angle) def quaternion_diff(q1, q2): r"""Compute the rotation in angle-axis format that rotates q2 into q1. .. math:: (\hat{\boldsymbol{\omega}}, \theta) = \log (q_1 \overline{q_2}) Parameters ---------- q1 : array-like, shape (4,) First quaternion q2 : array-line, shape (4,) Second quaternion Returns ------- a : array, shape (4,) The rotation in angle-axis format that rotates q2 into q1 """ q1 = check_quaternion(q1) q2 = check_quaternion(q2) q1q2c = concatenate_quaternions(q1, q_conj(q2)) return axis_angle_from_quaternion(q1q2c) def quaternion_from_euler(e, i, j, k, extrinsic): """General conversion to quaternion from any Euler angles. Parameters ---------- e : array-like, shape (3,) Rotation angles in radians about the axes i, j, k in this order. The first and last angle are normalized to [-pi, pi]. The middle angle is normalized to either [0, pi] (proper Euler angles) or [-pi/2, pi/2] (Cardan / Tait-Bryan angles). i : int from [0, 1, 2] The first rotation axis (0: x, 1: y, 2: z) j : int from [0, 1, 2] The second rotation axis (0: x, 1: y, 2: z) k : int from [0, 1, 2] The third rotation axis (0: x, 1: y, 2: z) extrinsic : bool Do we use extrinsic transformations? Intrinsic otherwise. Returns ------- q : array, shape (4,) Unit quaternion to represent rotation: (w, x, y, z) Raises ------ ValueError If basis is invalid """ check_axis_index("i", i) check_axis_index("j", j) check_axis_index("k", k) q0 = quaternion_from_angle(i, e[0]) q1 = quaternion_from_angle(j, e[1]) q2 = quaternion_from_angle(k, e[2]) if not extrinsic: q0, q2 = q2, q0 return concatenate_quaternions(concatenate_quaternions(q2, q1), q0) def matrix_from_quaternion(q): """Compute rotation matrix from quaternion. This typically results in an active rotation matrix. Parameters ---------- q : array-like, shape (4,) Unit quaternion to represent rotation: (w, x, y, z) Returns ------- R : array-like, shape (3, 3) Rotation matrix """ q = check_quaternion(q, unit=True) w, x, y, z = q x2 = 2.0 * x * x y2 = 2.0 * y * y z2 = 2.0 * z * z xy = 2.0 * x * y xz = 2.0 * x * z yz = 2.0 * y * z xw = 2.0 * x * w yw = 2.0 * y * w zw = 2.0 * z * w R = np.array( [ [1.0 - y2 - z2, xy - zw, xz + yw], [xy + zw, 1.0 - x2 - z2, yz - xw], [xz - yw, yz + xw, 1.0 - x2 - y2], ] ) return R def axis_angle_from_quaternion(q): """Compute axis-angle from quaternion. This operation is called logarithmic map. We usually assume active rotations. Parameters ---------- q : array-like, shape (4,) Unit quaternion to represent rotation: (w, x, y, z) Returns ------- a : array, shape (4,) Axis of rotation and rotation angle: (x, y, z, angle). The angle is constrained to [0, pi) so that the mapping is unique. """ q = check_quaternion(q) p = q[1:] p_norm = np.linalg.norm(p) if p_norm < np.finfo(float).eps: return np.array([1.0, 0.0, 0.0, 0.0]) axis = p / p_norm w_clamped = max(min(q[0], 1.0), -1.0) angle = (2.0 * np.arccos(w_clamped),) return norm_axis_angle(np.hstack((axis, angle))) def compact_axis_angle_from_quaternion(q): """Compute compact axis-angle from quaternion (logarithmic map). We usually assume active rotations. Parameters ---------- q : array-like, shape (4,) Unit quaternion to represent rotation: (w, x, y, z) Returns ------- a : array, shape (3,) Axis of rotation and rotation angle: angle * (x, y, z). The angle is constrained to [0, pi]. """ a = axis_angle_from_quaternion(q) return compact_axis_angle(a) def mrp_from_quaternion(q): """Compute modified Rodrigues parameters from quaternion. Parameters ---------- q : array-like, shape (4,) Unit quaternion to represent rotation: (w, x, y, z) Returns ------- mrp : array, shape (3,) Modified Rodrigues parameters. """ q = check_quaternion(q) if q[0] < 0.0: q = -q return q[1:] / (1.0 + q[0]) def quaternion_xyzw_from_wxyz(q_wxyz): """Converts from w, x, y, z to x, y, z, w convention. Parameters ---------- q_wxyz : array-like, shape (4,) Quaternion with scalar part before vector part Returns ------- q_xyzw : array, shape (4,) Quaternion with scalar part after vector part """ q_wxyz = check_quaternion(q_wxyz) return np.array([q_wxyz[1], q_wxyz[2], q_wxyz[3], q_wxyz[0]]) def quaternion_wxyz_from_xyzw(q_xyzw): """Converts from x, y, z, w to w, x, y, z convention. Parameters ---------- q_xyzw : array-like, shape (4,) Quaternion with scalar part after vector part Returns ------- q_wxyz : array, shape (4,) Quaternion with scalar part before vector part """ q_xyzw = check_quaternion(q_xyzw) return np.array([q_xyzw[3], q_xyzw[0], q_xyzw[1], q_xyzw[2]]) ================================================ FILE: pytransform3d/rotations/_quaternion.pyi ================================================ import numpy as np import numpy.typing as npt def check_quaternion(q: npt.ArrayLike, unit: bool = ...) -> np.ndarray: ... def check_quaternions(Q: npt.ArrayLike, unit: bool = ...) -> np.ndarray: ... def quaternion_requires_renormalization( q: npt.ArrayLike, tolerance: float = ... ) -> bool: ... def quaternion_double(q: npt.ArrayLike) -> np.ndarray: ... def pick_closest_quaternion( quaternion: npt.ArrayLike, target_quaternion: npt.ArrayLike ) -> np.ndarray: ... def pick_closest_quaternion_impl( quaternion: np.ndarray, target_quaternion: np.ndarray ) -> np.ndarray: ... def assert_quaternion_equal( q1: npt.ArrayLike, q2: npt.ArrayLike, *args, **kwargs ): ... def quaternion_integrate( Qd: npt.ArrayLike, q0: npt.ArrayLike = ..., dt: float = ... ) -> np.ndarray: ... def quaternion_gradient(Q: npt.ArrayLike, dt: float = ...) -> np.ndarray: ... def concatenate_quaternions( q1: npt.ArrayLike, q2: npt.ArrayLike ) -> np.ndarray: ... def q_prod_vector(q: npt.ArrayLike, v: npt.ArrayLike) -> np.ndarray: ... def q_conj(q: npt.ArrayLike) -> np.ndarray: ... def quaternion_dist(q1: npt.ArrayLike, q2: npt.ArrayLike) -> np.ndarray: ... def quaternion_diff(q1: npt.ArrayLike, q2: npt.ArrayLike) -> np.ndarray: ... def quaternion_from_euler( e: npt.ArrayLike, i: int, j: int, k: int, extrinsic: bool ) -> np.ndarray: ... def matrix_from_quaternion(q: npt.ArrayLike) -> np.ndarray: ... def axis_angle_from_quaternion(q: npt.ArrayLike) -> np.ndarray: ... def compact_axis_angle_from_quaternion(q: npt.ArrayLike) -> np.ndarray: ... def mrp_from_quaternion(q: npt.ArrayLike) -> np.ndarray: ... def quaternion_xyzw_from_wxyz(q_wxyz: npt.ArrayLike) -> np.ndarray: ... def quaternion_wxyz_from_xyzw(q_xyzw: npt.ArrayLike) -> np.ndarray: ... ================================================ FILE: pytransform3d/rotations/_random.py ================================================ """Random rotations.""" import numpy as np from ._axis_angle import matrix_from_compact_axis_angle from ._matrix import check_matrix, norm_matrix from ._utils import norm_vector def random_vector(rng=np.random.default_rng(0), n=3): r"""Generate an nd vector with normally distributed components. Each component will be sampled from :math:`\mathcal{N}(\mu=0, \sigma=1)`. Parameters ---------- rng : np.random.Generator, optional (default: random seed 0) Random number generator n : int, optional (default: 3) Number of vector components Returns ------- v : array, shape (n,) Random vector """ return rng.standard_normal(size=n) def random_axis_angle(rng=np.random.default_rng(0)): r"""Generate random axis-angle. The angle will be sampled uniformly from the interval :math:`[0, \pi)` and each component of the rotation axis will be sampled from :math:`\mathcal{N}(\mu=0, \sigma=1)` and then the axis will be normalized to length 1. Parameters ---------- rng : np.random.Generator, optional (default: random seed 0) Random number generator Returns ------- a : array, shape (4,) Axis of rotation and rotation angle: (x, y, z, angle) """ angle = np.pi * rng.random() a = np.array([0, 0, 0, angle]) a[:3] = norm_vector(rng.standard_normal(size=3)) return a def random_compact_axis_angle(rng=np.random.default_rng(0)): r"""Generate random compact axis-angle. The angle will be sampled uniformly from the interval :math:`[0, \pi)` and each component of the rotation axis will be sampled from :math:`\mathcal{N}(\mu=0, \sigma=1)` and then the axis will be normalized to length 1. Parameters ---------- rng : np.random.Generator, optional (default: random seed 0) Random number generator Returns ------- a : array, shape (3,) Axis of rotation and rotation angle: angle * (x, y, z) """ a = random_axis_angle(rng) return a[:3] * a[3] def random_quaternion(rng=np.random.default_rng(0)): r"""Generate uniform random quaternion. This is similar to scipy's implementation of uniform random sampling of rotations. It is based on Shoemake's algorithm [1]_, but we simplify the implementation by sampling a quaternion from a multivariate standard normal distribution and normalize it. In this way, we obtain a uniform distribution over rotation axes and a distribution proportional to :math:`\sin^2(\theta/2)` over the rotation angles, which together constitute the Haar measure on :math:`SO(3)`. Parameters ---------- rng : np.random.Generator, optional (default: random seed 0) Random number generator Returns ------- q : array, shape (4,) Unit quaternion to represent rotation: (w, x, y, z) References ---------- .. [1] Shoemake, K. (1992). Uniform Random Rotations. Graphics Gems III, pages 124-132. Academic, New York. DOI: 10.1016/B978-0-08-050755-2.50036-1 """ return norm_vector(rng.standard_normal(size=4)) def random_matrix(rng=np.random.default_rng(0), mean=np.eye(3), cov=np.eye(3)): r"""Generate random rotation matrix. Generate :math:`\Delta \boldsymbol{R}_{B_{i+1}{B_i}} \boldsymbol{R}_{{B_i}A}`, with :math:`\Delta \boldsymbol{R}_{B_{i+1}{B_i}} = Exp(\hat{\omega} \theta)` and :math:`\hat{\omega}\theta \sim \mathcal{N}(\boldsymbol{0}_3, \boldsymbol{\Sigma}_{3 \times 3})`. The mean :math:`\boldsymbol{R}_{{B_i}A}` and the covariance :math:`\boldsymbol{\Sigma}_{3 \times 3}` are parameters of the function. Note that uncertainty is defined in the global frame B, not in the body frame A. Parameters ---------- rng : np.random.Generator, optional (default: random seed 0) Random number generator. mean : array-like, shape (3, 3), optional (default: I) Mean rotation matrix. cov : array-like, shape (3, 3), optional (default: I) Covariance of noise in exponential coordinate space. Returns ------- R : array, shape (3, 3) Rotation matrix """ mean = check_matrix(mean) a = rng.multivariate_normal(mean=np.zeros(3), cov=cov) delta = matrix_from_compact_axis_angle(a) return norm_matrix(np.dot(delta, mean)) ================================================ FILE: pytransform3d/rotations/_random.pyi ================================================ import numpy as np import numpy.typing as npt def random_vector( rng: np.random.Generator = ..., n: int = ... ) -> np.ndarray: ... def random_axis_angle(rng: np.random.Generator = ...) -> np.ndarray: ... def random_compact_axis_angle(rng: np.random.Generator = ...) -> np.ndarray: ... def random_quaternion(rng: np.random.Generator = ...) -> np.ndarray: ... def random_matrix( rng: np.random.Generator = ..., mean: npt.ArrayLike = ..., cov: npt.ArrayLike = ..., ) -> np.ndarray: ... ================================================ FILE: pytransform3d/rotations/_rot_log.py ================================================ """Logarithm of rotation.""" import warnings import numpy as np def check_skew_symmetric_matrix(V, tolerance=1e-6, strict_check=True): """Input validation of a skew-symmetric matrix. Check whether the transpose of the matrix is its negative: .. math:: V^T = -V Parameters ---------- V : array-like, shape (3, 3) Cross-product matrix tolerance : float, optional (default: 1e-6) Tolerance threshold for checks. strict_check : bool, optional (default: True) Raise a ValueError if V.T is not numerically close enough to -V. Otherwise we print a warning. Returns ------- V : array, shape (3, 3) Validated cross-product matrix Raises ------ ValueError If input is invalid """ V = np.asarray(V, dtype=np.float64) if V.ndim != 2 or V.shape[0] != 3 or V.shape[1] != 3: raise ValueError( "Expected skew-symmetric matrix with shape (3, 3), " "got array-like object with shape %s" % (V.shape,) ) if not np.allclose(V.T, -V, atol=tolerance): error_msg = ( "Expected skew-symmetric matrix, but it failed the test " "V.T = %r\n-V = %r" % (V.T, -V) ) if strict_check: raise ValueError(error_msg) warnings.warn(error_msg, UserWarning, stacklevel=2) return V check_rot_log = check_skew_symmetric_matrix def cross_product_matrix(v): r"""Generate the cross-product matrix of a vector. The cross-product matrix :math:`\boldsymbol{V}` satisfies the equation .. math:: \boldsymbol{V} \boldsymbol{w} = \boldsymbol{v} \times \boldsymbol{w}. It is a skew-symmetric (antisymmetric) matrix, i.e., :math:`-\boldsymbol{V} = \boldsymbol{V}^T`. Its elements are .. math:: \left[\boldsymbol{v}\right] = \left[\begin{array}{c} v_1\\ v_2\\ v_3 \end{array}\right] = \boldsymbol{V} = \left(\begin{array}{ccc} 0 & -v_3 & v_2\\ v_3 & 0 & -v_1\\ -v_2 & v_1 & 0 \end{array}\right). The function can also be used to compute the logarithm of rotation from a compact axis-angle representation. Parameters ---------- v : array-like, shape (3,) 3d vector Returns ------- V : array, shape (3, 3) Cross-product matrix """ return np.array( [[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]] ) rot_log_from_compact_axis_angle = cross_product_matrix ================================================ FILE: pytransform3d/rotations/_rot_log.pyi ================================================ import numpy as np import numpy.typing as npt def check_skew_symmetric_matrix( V: npt.ArrayLike, tolerance: float = ..., strict_check: bool = ... ) -> np.ndarray: ... def check_rot_log( V: npt.ArrayLike, tolerance: float = ..., strict_check: bool = ... ) -> np.ndarray: ... def cross_product_matrix(v: npt.ArrayLike) -> np.ndarray: ... def rot_log_from_compact_axis_angle(v: npt.ArrayLike) -> np.ndarray: ... ================================================ FILE: pytransform3d/rotations/_rotors.py ================================================ """Rotor operations.""" import numpy as np from ._constants import unitx, unity, unitz, eps from ._quaternion import concatenate_quaternions, q_prod_vector from ._utils import norm_vector, perpendicular_to_vector def check_rotor(rotor): """Input validation of rotor. Parameters ---------- rotor : array-like, shape (4,) Rotor: (a, b_yz, b_zx, b_xy) Returns ------- rotor : array, shape (4,) Validated rotor (with unit norm): (a, b_yz, b_zx, b_xy) Raises ------ ValueError If input is invalid """ rotor = np.asarray(rotor, dtype=np.float64) if rotor.ndim != 1 or rotor.shape[0] != 4: raise ValueError( "Expected rotor with shape (4,), got " "array-like object with shape %s" % (rotor.shape,) ) return norm_vector(rotor) def wedge(a, b): r"""Outer product of two vectors (also exterior or wedge product). .. math:: B = a \wedge b Parameters ---------- a : array-like, shape (3,) Vector: (x, y, z) b : array-like, shape (3,) Vector: (x, y, z) Returns ------- B : array, shape (3,) Bivector that defines the plane that a and b form together: (b_yz, b_zx, b_xy) """ return np.cross(a, b) def plane_normal_from_bivector(B): """Convert bivector to normal vector of a plane. Parameters ---------- B : array-like, shape (3,) Bivector that defines a plane: (b_yz, b_zx, b_xy) Returns ------- n : array, shape (3,) Unit normal of the corresponding plane: (x, y, z) """ return norm_vector(B) def geometric_product(a, b): r"""Geometric product of two vectors. The geometric product consists of the symmetric inner / dot product and the antisymmetric outer product (see :func:`~pytransform3d.rotations.wedge`) of two vectors. .. math:: ab = a \cdot b + a \wedge b The inner product contains the cosine and the outer product contains the sine of the angle of rotation from a to b. Parameters ---------- a : array-like, shape (3,) Vector: (x, y, z) b : array-like, shape (3,) Vector: (x, y, z) Returns ------- ab : array, shape (4,) A multivector (a, b_yz, b_zx, b_xy) composed of scalar and bivector (b_yz, b_zx, b_xy) that form the geometric product of vectors a and b. """ return np.hstack(((np.dot(a, b),), wedge(a, b))) def rotor_reverse(rotor): """Invert rotor. Parameters ---------- rotor : array-like, shape (4,) Rotor: (a, b_yz, b_zx, b_xy) Returns ------- reverse_rotor : array, shape (4,) Reverse of the rotor: (a, b_yz, b_zx, b_xy) See Also -------- q_conj : Quaternion conjugate, which is the same operation. """ rotor = check_rotor(rotor) return np.hstack(((rotor[0],), -rotor[1:])) def concatenate_rotors(rotor1, rotor2): """Concatenate rotors. Suppose we want to apply two extrinsic rotations given by rotors R1 and R2 to a vector v. We can either apply R2 to v and then R1 to the result or we can concatenate R1 and R2 and apply the result to v. Parameters ---------- rotor1 : array-like, shape (4,) Rotor: (a, b_yz, b_zx, b_xy) rotor2 : array-like, shape (4,) Rotor: (a, b_yz, b_zx, b_xy) Returns ------- rotor : array, shape (4,) rotor1 applied to rotor2: (a, b_yz, b_zx, b_xy) See Also -------- concatenate_quaternions : Concatenate quaternions, which is the same operation. """ rotor1 = check_rotor(rotor1) rotor2 = check_rotor(rotor2) return concatenate_quaternions(rotor1, rotor2) def rotor_apply(rotor, v): r"""Apply rotor to vector. .. math:: v' = R v R^* Parameters ---------- rotor : array-like, shape (4,) Rotor: (a, b_yz, b_zx, b_xy) v : array-like, shape (3,) Vector: (x, y, z) Returns ------- v : array, shape (3,) Rotated vector See Also -------- q_prod_vector : The same operation with a different name. """ rotor = check_rotor(rotor) return q_prod_vector(rotor, v) def matrix_from_rotor(rotor): """Compute rotation matrix from rotor. Parameters ---------- rotor : array-like, shape (4,) Rotor: (a, b_yz, b_zx, b_xy) Returns ------- R : array, shape (3, 3) Rotation matrix """ rotor = check_rotor(rotor) return np.column_stack( ( rotor_apply(rotor, unitx), rotor_apply(rotor, unity), rotor_apply(rotor, unitz), ) ) def rotor_from_two_directions(v_from, v_to): """Construct the rotor that rotates one vector to another. Parameters ---------- v_from : array-like, shape (3,) Unit vector (will be normalized internally) v_to : array-like, shape (3,) Unit vector (will be normalized internally) Returns ------- rotor : array, shape (4,) Rotor: (a, b_yz, b_zx, b_xy) """ v_from = norm_vector(v_from) v_to = norm_vector(v_to) cos_angle_p1 = 1.0 + np.dot(v_from, v_to) if cos_angle_p1 < eps: # There is an infinite number of solutions for the plane of rotation. # This solution works with our convention, since the rotation axis is # the same as the plane bivector. plane = perpendicular_to_vector(v_from) else: plane = wedge(v_from, v_to) multivector = np.hstack(((cos_angle_p1,), plane)) return norm_vector(multivector) def rotor_from_plane_angle(B, angle): r"""Compute rotor from plane bivector and angle. Parameters ---------- B : array-like, shape (3,) Unit bivector (b_yz, b_zx, b_xy) that represents the plane of rotation (will be normalized internally) angle : float Rotation angle Returns ------- rotor : array, shape (4,) Rotor: (a, b_yz, b_zx, b_xy) """ a = np.cos(angle / 2.0) sina = np.sin(angle / 2.0) B = norm_vector(B) return np.hstack(((a,), sina * B)) ================================================ FILE: pytransform3d/rotations/_rotors.pyi ================================================ import numpy as np import numpy.typing as npt def check_rotor(a: npt.ArrayLike) -> np.ndarray: ... def wedge(a: npt.ArrayLike, b: npt.ArrayLike) -> np.ndarray: ... def plane_normal_from_bivector(B: npt.ArrayLike) -> np.ndarray: ... def geometric_product(a: npt.ArrayLike, b: npt.ArrayLike) -> np.ndarray: ... def rotor_reverse(rotor: npt.ArrayLike) -> np.ndarray: ... def concatenate_rotors( rotor1: npt.ArrayLike, rotor2: npt.ArrayLike ) -> np.ndarray: ... def rotor_apply(rotor: npt.ArrayLike, v: npt.ArrayLike) -> np.ndarray: ... def matrix_from_rotor(rotor: npt.ArrayLike) -> np.ndarray: ... def rotor_from_two_directions( v_from: npt.ArrayLike, v_to: npt.ArrayLike ) -> np.ndarray: ... def rotor_from_plane_angle(B: npt.ArrayLike, angle: float) -> np.ndarray: ... ================================================ FILE: pytransform3d/rotations/_slerp.py ================================================ """Spherical linear interpolation (SLERP).""" import numpy as np from ._axis_angle import check_axis_angle, matrix_from_compact_axis_angle from ._matrix import compact_axis_angle_from_matrix from ._quaternion import check_quaternion, pick_closest_quaternion_impl from ._rotors import check_rotor from ._utils import angle_between_vectors def matrix_slerp(start, end, t): r"""Spherical linear interpolation (SLERP) for rotation matrices. We compute the difference between two orientations :math:`\boldsymbol{R}_{AB} = \boldsymbol{R}^{-1}_{WA}\boldsymbol{R}_{WB}`, convert it to a rotation vector :math:`Log(\boldsymbol{R}_{AB}) = \boldsymbol{\omega}_{AB}`, compute a fraction of it :math:`t\boldsymbol{\omega}_{AB}` with :math:`t \in [0, 1]`, and use the exponential map to concatenate the fraction of the difference :math:`\boldsymbol{R}_{WA} Exp(t\boldsymbol{\omega}_{AB})`. Parameters ---------- start : array-like, shape (3, 3) Rotation matrix to represent start orientation. end : array-like, shape (3, 3) Rotation matrix to represent end orientation. t : float in [0, 1] Position between start and goal. Returns ------- R_t : array, shape (3, 3) Interpolated orientation. See Also -------- axis_angle_slerp : SLERP axis-angle representation. quaternion_slerp : SLERP for quaternions. rotor_slerp : SLERP for rotors. pytransform3d.transformations.pq_slerp : SLERP for position + quaternion. """ end2start = np.dot(np.transpose(start), end) return np.dot(start, matrix_power(end2start, t)) def matrix_power(R, t): r"""Compute power of a rotation matrix with respect to scalar. .. math:: \boldsymbol{R}^t Parameters ---------- R : array-like, shape (3, 3) Rotation matrix. t : float Exponent. Returns ------- R_t : array, shape (3, 3) Rotation matrix. """ a = compact_axis_angle_from_matrix(R) return matrix_from_compact_axis_angle(a * t) def axis_angle_slerp(start, end, t): """Spherical linear interpolation. Parameters ---------- start : array-like, shape (4,) Start axis of rotation and rotation angle: (x, y, z, angle) end : array-like, shape (4,) Goal axis of rotation and rotation angle: (x, y, z, angle) t : float in [0, 1] Position between start and end Returns ------- a : array, shape (4,) Interpolated axis of rotation and rotation angle: (x, y, z, angle) See Also -------- matrix_slerp : SLERP for rotation matrices. quaternion_slerp : SLERP for quaternions. rotor_slerp : SLERP for rotors. pytransform3d.transformations.pq_slerp : SLERP for position + quaternion. """ start = check_axis_angle(start) end = check_axis_angle(end) angle = angle_between_vectors(start[:3], end[:3]) w1, w2 = slerp_weights(angle, t) w1 = np.array([w1, w1, w1, (1.0 - t)]) w2 = np.array([w2, w2, w2, t]) return w1 * start + w2 * end def quaternion_slerp(start, end, t, shortest_path=False): """Spherical linear interpolation. Parameters ---------- start : array-like, shape (4,) Start unit quaternion to represent rotation: (w, x, y, z) end : array-like, shape (4,) End unit quaternion to represent rotation: (w, x, y, z) t : float in [0, 1] Position between start and end shortest_path : bool, optional (default: False) Resolve sign ambiguity before interpolation to find the shortest path. The end quaternion will be picked to be close to the start quaternion. Returns ------- q : array, shape (4,) Interpolated unit quaternion to represent rotation: (w, x, y, z) See Also -------- matrix_slerp : SLERP for rotation matrices. axis_angle_slerp : SLERP for axis-angle representation. rotor_slerp : SLERP for rotors. pytransform3d.transformations.pq_slerp : SLERP for position + quaternion. """ start = check_quaternion(start) end = check_quaternion(end) if shortest_path: end = pick_closest_quaternion_impl(end, start) angle = angle_between_vectors(start, end) w1, w2 = slerp_weights(angle, t) return w1 * start + w2 * end def rotor_slerp(start, end, t, shortest_path=False): """Spherical linear interpolation. Parameters ---------- start : array-like, shape (4,) Rotor: (a, b_yz, b_zx, b_xy) end : array-like, shape (4,) Rotor: (a, b_yz, b_zx, b_xy) t : float in [0, 1] Position between start and end shortest_path : bool, optional (default: False) Resolve sign ambiguity before interpolation to find the shortest path. The end rotor will be picked to be close to the start rotor. Returns ------- rotor : array, shape (4,) Interpolated rotor: (a, b_yz, b_zx, b_xy) See Also -------- matrix_slerp : SLERP for rotation matrices. axis_angle_slerp : SLERP for axis-angle representation. quaternion_slerp : SLERP for quaternions. pytransform3d.transformations.pq_slerp : SLERP for position + quaternion. """ start = check_rotor(start) end = check_rotor(end) return quaternion_slerp(start, end, t, shortest_path) def slerp_weights(angle, t): """Compute weights of start and end for spherical linear interpolation. Parameters ---------- angle : float Rotation angle. t : float or array, shape (n_steps,) Position between start and end Returns ------- w1 : float or array, shape (n_steps,) Weights for quaternion 1 w2 : float or array, shape (n_steps,) Weights for quaternion 2 """ if angle == 0.0: return np.ones_like(t), np.zeros_like(t) return ( np.sin((1.0 - t) * angle) / np.sin(angle), np.sin(t * angle) / np.sin(angle), ) ================================================ FILE: pytransform3d/rotations/_slerp.pyi ================================================ import numpy as np import numpy.typing as npt from typing import Union def matrix_slerp( start: npt.ArrayLike, end: npt.ArrayLike, t: float ) -> np.ndarray: ... def matrix_power(R: npt.ArrayLike, t: float) -> np.ndarray: ... def axis_angle_slerp( start: npt.ArrayLike, end: npt.ArrayLike, t: float ) -> np.ndarray: ... def quaternion_slerp( start: npt.ArrayLike, end: npt.ArrayLike, t: float, shortest_path: bool = ..., ) -> np.ndarray: ... def rotor_slerp( start: npt.ArrayLike, end: npt.ArrayLike, t: float, shortest_path: bool = ..., ) -> np.ndarray: ... def slerp_weights( angle: float, t: Union[float, npt.ArrayLike] ) -> Union[float, np.ndarray]: ... ================================================ FILE: pytransform3d/rotations/_utils.py ================================================ """Utility functions for rotations.""" import math import numpy as np from ._constants import unitz, eps def check_axis_index(name, i): """Checks axis index. Parameters ---------- name : str Name of the axis. Required for the error message. i : int from [0, 1, 2] Index of the axis (0: x, 1: y, 2: z) Raises ------ ValueError If basis is invalid """ if i not in [0, 1, 2]: raise ValueError("Axis index %s (%d) must be in [0, 1, 2]" % (name, i)) def norm_vector(v): """Normalize vector. Parameters ---------- v : array-like, shape (n,) nd vector Returns ------- u : array, shape (n,) nd unit vector with norm 1 or the zero vector """ norm = np.linalg.norm(v) if norm == 0.0: return v return np.asarray(v) / norm def perpendicular_to_vectors(a, b): """Compute perpendicular vector to two other vectors. Parameters ---------- a : array-like, shape (3,) 3d vector b : array-like, shape (3,) 3d vector Returns ------- c : array, shape (3,) 3d vector that is orthogonal to a and b """ return np.cross(a, b) def perpendicular_to_vector(a): """Compute perpendicular vector to one other vector. There is an infinite number of solutions to this problem. Thus, we restrict the solutions to [1, 0, z] and return [0, 0, 1] if the z component of a is 0. Parameters ---------- a : array-like, shape (3,) 3d vector Returns ------- b : array, shape (3,) A 3d vector that is orthogonal to a. It does not necessarily have unit length. """ if abs(a[2]) < eps: return np.copy(unitz) # Now that we solved the problem for [x, y, 0], we can solve it for all # other vectors by restricting solutions to [1, 0, z] and find z. # The dot product of orthogonal vectors is 0, thus # a[0] * 1 + a[1] * 0 + a[2] * z == 0 or -a[0] / a[2] = z return np.array([1.0, 0.0, -a[0] / a[2]]) def angle_between_vectors(a, b, fast=False): """Compute angle between two vectors. Parameters ---------- a : array-like, shape (n,) nd vector b : array-like, shape (n,) nd vector fast : bool, optional (default: False) Use fast implementation instead of numerically stable solution Returns ------- angle : float Angle between a and b """ if len(a) != 3 or fast: return np.arccos( np.clip( np.dot(a, b) / (np.linalg.norm(a) * np.linalg.norm(b)), -1.0, 1.0, ) ) return np.arctan2(np.linalg.norm(np.cross(a, b)), np.dot(a, b)) def vector_projection(a, b): """Orthogonal projection of vector a on vector b. Parameters ---------- a : array-like, shape (3,) Vector a that will be projected on vector b b : array-like, shape (3,) Vector b on which vector a will be projected Returns ------- a_on_b : array, shape (3,) Vector a """ b_norm_squared = np.dot(b, b) if b_norm_squared == 0.0: return np.zeros(3) return np.dot(a, b) * b / b_norm_squared def plane_basis_from_normal(plane_normal): """Compute two basis vectors of a plane from the plane's normal vector. Note that there are infinitely many solutions because any rotation of the basis vectors about the normal is also a solution. This function deterministically picks one of the solutions. The two basis vectors of the plane together with the normal form an orthonormal basis in 3D space and could be used as columns to form a rotation matrix. Parameters ---------- plane_normal : array-like, shape (3,) Plane normal of unit length. Returns ------- x_axis : array, shape (3,) x-axis of the plane. y_axis : array, shape (3,) y-axis of the plane. """ if abs(plane_normal[0]) >= abs(plane_normal[1]): # x or z is the largest magnitude component, swap them length = math.sqrt( plane_normal[0] * plane_normal[0] + plane_normal[2] * plane_normal[2] ) x_axis = np.array( [-plane_normal[2] / length, 0.0, plane_normal[0] / length] ) y_axis = np.array( [ plane_normal[1] * x_axis[2], plane_normal[2] * x_axis[0] - plane_normal[0] * x_axis[2], -plane_normal[1] * x_axis[0], ] ) else: # y or z is the largest magnitude component, swap them length = math.sqrt( plane_normal[1] * plane_normal[1] + plane_normal[2] * plane_normal[2] ) x_axis = np.array( [0.0, plane_normal[2] / length, -plane_normal[1] / length] ) y_axis = np.array( [ plane_normal[1] * x_axis[2] - plane_normal[2] * x_axis[1], -plane_normal[0] * x_axis[2], plane_normal[0] * x_axis[1], ] ) return x_axis, y_axis ================================================ FILE: pytransform3d/rotations/_utils.pyi ================================================ import numpy as np import numpy.typing as npt from typing import Tuple def check_axis_index(name: str, i: int): ... def norm_vector(v: npt.ArrayLike) -> np.ndarray: ... def perpendicular_to_vectors( a: npt.ArrayLike, b: npt.ArrayLike ) -> np.ndarray: ... def perpendicular_to_vector(a: npt.ArrayLike) -> np.ndarray: ... def angle_between_vectors( a: npt.ArrayLike, b: npt.ArrayLike, fast: bool = ... ) -> float: ... def vector_projection(a: npt.ArrayLike, b: npt.ArrayLike) -> np.ndarray: ... def plane_basis_from_normal( plane_normal: npt.ArrayLike, ) -> Tuple[np.ndarray, np.ndarray]: ... ================================================ FILE: pytransform3d/rotations/test/test_angle.py ================================================ import numpy as np import pytest from numpy.testing import assert_array_almost_equal, assert_array_equal import pytransform3d.rotations as pr def test_norm_angle(): """Test normalization of angle.""" rng = np.random.default_rng(0) a_norm = rng.uniform(-np.pi, np.pi, size=(100,)) for b in np.linspace(-10.0 * np.pi, 10.0 * np.pi, 11): a = a_norm + b assert_array_almost_equal(pr.norm_angle(a), a_norm) assert pytest.approx(pr.norm_angle(-np.pi)) == np.pi assert pytest.approx(pr.norm_angle(np.pi)) == np.pi def test_norm_angle_precision(): # NOTE: it would be better if angles are divided into 1e16 numbers # to test precision of float64 but it is limited by memory a_norm = np.linspace( np.pi, -np.pi, num=1000000, endpoint=False, dtype=np.float64 )[::-1] for b in np.linspace(-10.0 * np.pi, 10.0 * np.pi, 11): a = a_norm + b assert_array_almost_equal(pr.norm_angle(a), a_norm) # eps and epsneg around zero a_eps = np.array([np.finfo(np.float64).eps, -np.finfo(np.float64).eps]) a_epsneg = np.array( [np.finfo(np.float64).epsneg, -np.finfo(np.float64).epsneg] ) assert_array_equal(pr.norm_angle(a_eps), a_eps) assert_array_equal(pr.norm_angle(a_epsneg), a_epsneg) def test_passive_matrix_from_angle(): """Sanity checks for rotation around basis vectors.""" with pytest.raises(ValueError, match="Basis must be in"): pr.passive_matrix_from_angle(-1, 0) with pytest.raises(ValueError, match="Basis must be in"): pr.passive_matrix_from_angle(3, 0) R = pr.passive_matrix_from_angle(0, -0.5 * np.pi) assert_array_almost_equal(R, np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]])) R = pr.passive_matrix_from_angle(0, 0.5 * np.pi) assert_array_almost_equal(R, np.array([[1, 0, 0], [0, 0, 1], [0, -1, 0]])) R = pr.passive_matrix_from_angle(1, -0.5 * np.pi) assert_array_almost_equal(R, np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]])) R = pr.passive_matrix_from_angle(1, 0.5 * np.pi) assert_array_almost_equal(R, np.array([[0, 0, -1], [0, 1, 0], [1, 0, 0]])) R = pr.passive_matrix_from_angle(2, -0.5 * np.pi) assert_array_almost_equal(R, np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]])) R = pr.passive_matrix_from_angle(2, 0.5 * np.pi) assert_array_almost_equal(R, np.array([[0, 1, 0], [-1, 0, 0], [0, 0, 1]])) def test_active_matrix_from_angle(): """Sanity checks for rotation around basis vectors.""" with pytest.raises(ValueError, match="Basis must be in"): pr.active_matrix_from_angle(-1, 0) with pytest.raises(ValueError, match="Basis must be in"): pr.active_matrix_from_angle(3, 0) rng = np.random.default_rng(21) for _ in range(20): basis = rng.integers(0, 3) angle = 2.0 * np.pi * rng.random() - np.pi R_passive = pr.passive_matrix_from_angle(basis, angle) R_active = pr.active_matrix_from_angle(basis, angle) assert_array_almost_equal(R_active, R_passive.T) def test_active_rotation_is_default(): """Test that rotations are active by default.""" Rx = pr.active_matrix_from_angle(0, 0.5 * np.pi) ax = np.array([1, 0, 0, 0.5 * np.pi]) qx = pr.quaternion_from_axis_angle(ax) assert_array_almost_equal(Rx, pr.matrix_from_axis_angle(ax)) assert_array_almost_equal(Rx, pr.matrix_from_quaternion(qx)) Ry = pr.active_matrix_from_angle(1, 0.5 * np.pi) ay = np.array([0, 1, 0, 0.5 * np.pi]) qy = pr.quaternion_from_axis_angle(ay) assert_array_almost_equal(Ry, pr.matrix_from_axis_angle(ay)) assert_array_almost_equal(Ry, pr.matrix_from_quaternion(qy)) Rz = pr.active_matrix_from_angle(2, 0.5 * np.pi) az = np.array([0, 0, 1, 0.5 * np.pi]) qz = pr.quaternion_from_axis_angle(az) assert_array_almost_equal(Rz, pr.matrix_from_axis_angle(az)) assert_array_almost_equal(Rz, pr.matrix_from_quaternion(qz)) def test_quaternion_from_angle(): """Quaternion from rotation around basis vectors.""" with pytest.raises(ValueError, match="Basis must be in"): pr.quaternion_from_angle(-1, 0) with pytest.raises(ValueError, match="Basis must be in"): pr.quaternion_from_angle(3, 0) rng = np.random.default_rng(22) for _ in range(20): basis = rng.integers(0, 3) angle = 2.0 * np.pi * rng.random() - np.pi R = pr.active_matrix_from_angle(basis, angle) q = pr.quaternion_from_angle(basis, angle) Rq = pr.matrix_from_quaternion(q) assert_array_almost_equal(R, Rq) ================================================ FILE: pytransform3d/rotations/test/test_axis_angle.py ================================================ import numpy as np import pytest from numpy.testing import assert_array_almost_equal import pytransform3d.rotations as pr def test_check_axis_angle(): """Test input validation for axis-angle representation.""" a_list = [1, 0, 0, 0] a = pr.check_axis_angle(a_list) assert_array_almost_equal(a_list, a) assert type(a) is np.ndarray assert a.dtype == np.float64 rng = np.random.default_rng(0) a = np.empty(4) a[:3] = pr.random_vector(rng, 3) a[3] = rng.standard_normal() * 4.0 * np.pi a2 = pr.check_axis_angle(a) pr.assert_axis_angle_equal(a, a2) assert pytest.approx(np.linalg.norm(a2[:3])) == 1.0 assert a2[3] > 0 assert np.pi > a2[3] with pytest.raises( ValueError, match="Expected axis and angle in array with shape" ): pr.check_axis_angle(np.zeros(3)) with pytest.raises( ValueError, match="Expected axis and angle in array with shape" ): pr.check_axis_angle(np.zeros((3, 3))) def test_check_compact_axis_angle(): """Test input validation for compact axis-angle representation.""" a_list = [0, 0, 0] a = pr.check_compact_axis_angle(a_list) assert_array_almost_equal(a_list, a) assert type(a) is np.ndarray assert a.dtype == np.float64 rng = np.random.default_rng(0) a = pr.norm_vector(pr.random_vector(rng, 3)) a *= np.pi + rng.standard_normal() * 4.0 * np.pi a2 = pr.check_compact_axis_angle(a) pr.assert_compact_axis_angle_equal(a, a2) assert np.pi > np.linalg.norm(a2) > 0 with pytest.raises( ValueError, match="Expected axis and angle in array with shape" ): pr.check_compact_axis_angle(np.zeros(4)) with pytest.raises( ValueError, match="Expected axis and angle in array with shape" ): pr.check_compact_axis_angle(np.zeros((3, 3))) def test_norm_axis_angle(): """Test normalization of angle-axis representation.""" a = np.array([1.0, 0.0, 0.0, np.pi]) n = pr.norm_axis_angle(a) assert_array_almost_equal(a, n) a = np.array([0.0, 1.0, 0.0, np.pi]) n = pr.norm_axis_angle(a) assert_array_almost_equal(a, n) a = np.array([0.0, 0.0, 1.0, np.pi]) n = pr.norm_axis_angle(a) assert_array_almost_equal(a, n) rng = np.random.default_rng(0) for _ in range(5): a = pr.random_axis_angle(rng) angle = rng.uniform(-20.0, -10.0 + 2.0 * np.pi) a[3] = angle n = pr.norm_axis_angle(a) for angle_offset in np.arange(0.0, 10.1 * np.pi, 2.0 * np.pi): a[3] = angle + angle_offset n2 = pr.norm_axis_angle(a) assert_array_almost_equal(n, n2) def test_compact_axis_angle_near_pi(): assert pr.compact_axis_angle_near_pi( np.pi * pr.norm_vector([0.2, 0.1, -0.3]) ) assert pr.compact_axis_angle_near_pi( (1e-7 + np.pi) * pr.norm_vector([0.2, 0.1, -0.3]) ) assert pr.compact_axis_angle_near_pi( (-1e-7 + np.pi) * pr.norm_vector([0.2, 0.1, -0.3]) ) assert not pr.compact_axis_angle_near_pi( (-1e-5 + np.pi) * pr.norm_vector([0.2, 0.1, -0.3]) ) def test_norm_compact_axis_angle(): """Test normalization of compact angle-axis representation.""" a = np.array([np.pi, 0.0, 0.0]) n = pr.norm_compact_axis_angle(a) assert_array_almost_equal(a, n) a = np.array([0.0, np.pi, 0.0]) n = pr.norm_compact_axis_angle(a) assert_array_almost_equal(a, n) a = np.array([0.0, 0.0, np.pi]) n = pr.norm_compact_axis_angle(a) assert_array_almost_equal(a, n) rng = np.random.default_rng(0) for _ in range(5): a = pr.random_compact_axis_angle(rng) axis = a / np.linalg.norm(a) angle = rng.uniform(-20.0, -10.0 + 2.0 * np.pi) a = axis * angle n = pr.norm_compact_axis_angle(a) for angle_offset in np.arange(0.0, 10.1 * np.pi, 2.0 * np.pi): a = axis * (angle + angle_offset) n2 = pr.norm_compact_axis_angle(a) assert_array_almost_equal(n, n2) def test_axis_angle_from_two_direction_vectors(): """Test calculation of axis-angle from two direction vectors.""" d1 = np.array([1.0, 0.0, 0.0]) a = pr.axis_angle_from_two_directions(d1, d1) pr.assert_axis_angle_equal(a, np.array([1, 0, 0, 0])) a = pr.axis_angle_from_two_directions(d1, np.zeros(3)) pr.assert_axis_angle_equal(a, np.array([1, 0, 0, 0])) a = pr.axis_angle_from_two_directions(np.zeros(3), d1) pr.assert_axis_angle_equal(a, np.array([1, 0, 0, 0])) d2 = np.array([0.0, 1.0, 0.0]) a = pr.axis_angle_from_two_directions(d1, d2) pr.assert_axis_angle_equal(a, np.array([0, 0, 1, 0.5 * np.pi])) d3 = np.array([-1.0, 0.0, 0.0]) a = pr.axis_angle_from_two_directions(d1, d3) pr.assert_axis_angle_equal(a, np.array([0, 0, 1, np.pi])) d4 = np.array([0.0, -1.0, 0.0]) a = pr.axis_angle_from_two_directions(d2, d4) pr.assert_axis_angle_equal(a, np.array([0, 0, 1, np.pi])) a = pr.axis_angle_from_two_directions(d3, d4) pr.assert_axis_angle_equal(a, np.array([0, 0, 1, 0.5 * np.pi])) rng = np.random.default_rng(323) for _ in range(5): R = pr.matrix_from_axis_angle(pr.random_axis_angle(rng)) v1 = pr.random_vector(rng, 3) v2 = R.dot(v1) a = pr.axis_angle_from_two_directions(v1, v2) assert pytest.approx(pr.angle_between_vectors(v1, a[:3])) == 0.5 * np.pi assert pytest.approx(pr.angle_between_vectors(v2, a[:3])) == 0.5 * np.pi assert_array_almost_equal(v2, pr.matrix_from_axis_angle(a).dot(v1)) def test_axis_angle_from_compact_axis_angle(): """Test conversion from compact axis-angle representation.""" ca = [0.0, 0.0, 0.0] a = pr.axis_angle_from_compact_axis_angle(ca) assert_array_almost_equal(a, np.array([1.0, 0.0, 0.0, 0.0])) rng = np.random.default_rng(1) for _ in range(5): ca = pr.random_compact_axis_angle(rng) a = pr.axis_angle_from_compact_axis_angle(ca) assert pytest.approx(np.linalg.norm(ca)) == a[3] assert_array_almost_equal(ca[:3] / np.linalg.norm(ca), a[:3]) def test_compact_axis_angle(): """Test conversion to compact axis-angle representation.""" a = np.array([1.0, 0.0, 0.0, 0.0]) ca = pr.compact_axis_angle(a) assert_array_almost_equal(ca, np.zeros(3)) rng = np.random.default_rng(0) for _ in range(5): a = pr.random_axis_angle(rng) ca = pr.compact_axis_angle(a) assert_array_almost_equal(pr.norm_vector(ca), a[:3]) assert pytest.approx(np.linalg.norm(ca)) == a[3] def test_conversions_axis_angle_quaternion(): """Test conversions between axis-angle and quaternion.""" q = np.array([1, 0, 0, 0]) a = pr.axis_angle_from_quaternion(q) assert_array_almost_equal(a, np.array([1, 0, 0, 0])) q2 = pr.quaternion_from_axis_angle(a) assert_array_almost_equal(q2, q) rng = np.random.default_rng(0) for _ in range(5): a = pr.random_axis_angle(rng) q = pr.quaternion_from_axis_angle(a) a2 = pr.axis_angle_from_quaternion(q) assert_array_almost_equal(a, a2) q2 = pr.quaternion_from_axis_angle(a2) pr.assert_quaternion_equal(q, q2) def test_conversions_compact_axis_angle_quaternion(): """Test conversions between compact axis-angle and quaternion.""" q = np.array([1, 0, 0, 0]) a = pr.compact_axis_angle_from_quaternion(q) assert_array_almost_equal(a, np.array([0, 0, 0])) q2 = pr.quaternion_from_compact_axis_angle(a) assert_array_almost_equal(q2, q) rng = np.random.default_rng(0) for _ in range(5): a = pr.random_compact_axis_angle(rng) q = pr.quaternion_from_compact_axis_angle(a) a2 = pr.compact_axis_angle_from_quaternion(q) assert_array_almost_equal(a, a2) q2 = pr.quaternion_from_compact_axis_angle(a2) pr.assert_quaternion_equal(q, q2) def test_mrp_from_axis_angle(): rng = np.random.default_rng(98343) for _ in range(5): a = pr.random_axis_angle(rng) mrp = pr.mrp_from_axis_angle(a) q = pr.quaternion_from_axis_angle(a) assert_array_almost_equal(mrp, pr.mrp_from_quaternion(q)) assert_array_almost_equal( [0.0, 0.0, 0.0], pr.mrp_from_axis_angle([1.0, 0.0, 0.0, 0.0]) ) assert_array_almost_equal( [0.0, 0.0, 0.0], pr.mrp_from_axis_angle([1.0, 0.0, 0.0, 2.0 * np.pi]) ) assert_array_almost_equal( [1.0, 0.0, 0.0], pr.mrp_from_axis_angle([1.0, 0.0, 0.0, np.pi]) ) ================================================ FILE: pytransform3d/rotations/test/test_constants.py ================================================ from numpy.testing import assert_array_almost_equal import pytransform3d.rotations as pr def test_id_rot(): """Test equivalence of constants that represent no rotation.""" assert_array_almost_equal(pr.R_id, pr.matrix_from_axis_angle(pr.a_id)) assert_array_almost_equal(pr.R_id, pr.matrix_from_quaternion(pr.q_id)) ================================================ FILE: pytransform3d/rotations/test/test_euler.py ================================================ import numpy as np import pytest from numpy.testing import assert_array_almost_equal import pytransform3d.rotations as pr def test_norm_euler(): rng = np.random.default_rng(94322) euler_axes = [ [0, 2, 0], [0, 1, 0], [1, 0, 1], [1, 2, 1], [2, 1, 2], [2, 0, 2], [0, 2, 1], [0, 1, 2], [1, 0, 2], [1, 2, 0], [2, 1, 0], [2, 0, 1], ] for ea in euler_axes: for _ in range(10): e = np.pi + rng.random(3) * np.pi * 2.0 e *= np.sign(rng.standard_normal(3)) e_norm = pr.norm_euler(e, *ea) R1 = pr.matrix_from_euler(e, ea[0], ea[1], ea[2], True) R2 = pr.matrix_from_euler(e_norm, ea[0], ea[1], ea[2], True) assert_array_almost_equal(R1, R2) assert not np.allclose(e, e_norm) assert -np.pi <= e_norm[0] <= np.pi if ea[0] == ea[2]: assert 0.0 <= e_norm[1] <= np.pi else: assert -0.5 * np.pi <= e_norm[1] <= 0.5 * np.pi assert -np.pi <= e_norm[2] <= np.pi pr.assert_euler_equal(e, e_norm, *ea) def test_euler_near_gimbal_lock(): assert pr.euler_near_gimbal_lock([0, 0, 0], 1, 2, 1) assert pr.euler_near_gimbal_lock([0, -1e-7, 0], 1, 2, 1) assert pr.euler_near_gimbal_lock([0, 1e-7, 0], 1, 2, 1) assert pr.euler_near_gimbal_lock([0, np.pi, 0], 1, 2, 1) assert pr.euler_near_gimbal_lock([0, np.pi - 1e-7, 0], 1, 2, 1) assert pr.euler_near_gimbal_lock([0, np.pi + 1e-7, 0], 1, 2, 1) assert not pr.euler_near_gimbal_lock([0, 0.5, 0], 1, 2, 1) assert pr.euler_near_gimbal_lock([0, 0.5 * np.pi, 0], 0, 1, 2) assert pr.euler_near_gimbal_lock([0, 0.5 * np.pi - 1e-7, 0], 0, 1, 2) assert pr.euler_near_gimbal_lock([0, 0.5 * np.pi + 1e-7, 0], 0, 1, 2) assert pr.euler_near_gimbal_lock([0, -0.5 * np.pi, 0], 0, 1, 2) assert pr.euler_near_gimbal_lock([0, -0.5 * np.pi - 1e-7, 0], 0, 1, 2) assert pr.euler_near_gimbal_lock([0, -0.5 * np.pi + 1e-7, 0], 0, 1, 2) assert not pr.euler_near_gimbal_lock([0, 0, 0], 0, 1, 2) def test_assert_euler_almost_equal(): pr.assert_euler_equal( [0.2, 0.3, -0.5], [0.2 + np.pi, -0.3, -0.5 - np.pi], 0, 1, 0 ) pr.assert_euler_equal( [0.2, 0.3, -0.5], [0.2 + np.pi, np.pi - 0.3, -0.5 - np.pi], 0, 1, 2 ) def test_general_matrix_euler_conversions(): """General conversion algorithms between matrix and Euler angles.""" rng = np.random.default_rng(22) euler_axes = [ [0, 2, 0], [0, 1, 0], [1, 0, 1], [1, 2, 1], [2, 1, 2], [2, 0, 2], [0, 2, 1], [0, 1, 2], [1, 0, 2], [1, 2, 0], [2, 1, 0], [2, 0, 1], ] for ea in euler_axes: for extrinsic in [False, True]: for _ in range(5): e = rng.random(3) e[0] = 2.0 * np.pi * e[0] - np.pi e[1] = np.pi * e[1] e[2] = 2.0 * np.pi * e[2] - np.pi proper_euler = ea[0] == ea[2] if proper_euler: e[1] -= np.pi / 2.0 q = pr.quaternion_from_euler(e, ea[0], ea[1], ea[2], extrinsic) R = pr.matrix_from_euler(e, ea[0], ea[1], ea[2], extrinsic) q_R = pr.quaternion_from_matrix(R) pr.assert_quaternion_equal( q, q_R, err_msg=f"axes: {ea}, extrinsic: {extrinsic}" ) e_R = pr.euler_from_matrix(R, ea[0], ea[1], ea[2], extrinsic) e_q = pr.euler_from_quaternion( q, ea[0], ea[1], ea[2], extrinsic ) pr.assert_euler_equal(e_R, e_q, *ea) R_R = pr.matrix_from_euler(e_R, ea[0], ea[1], ea[2], extrinsic) R_q = pr.matrix_from_euler(e_q, ea[0], ea[1], ea[2], extrinsic) assert_array_almost_equal(R_R, R_q) def test_from_quaternion(): """Test conversion from quaternion to Euler angles.""" with pytest.raises( ValueError, match="Axis index i \\(-1\\) must be in \\[0, 1, 2\\]" ): pr.euler_from_quaternion(pr.q_id, -1, 0, 2, True) with pytest.raises( ValueError, match="Axis index i \\(3\\) must be in \\[0, 1, 2\\]" ): pr.euler_from_quaternion(pr.q_id, 3, 0, 2, True) with pytest.raises( ValueError, match="Axis index j \\(-1\\) must be in \\[0, 1, 2\\]" ): pr.euler_from_quaternion(pr.q_id, 2, -1, 2, True) with pytest.raises( ValueError, match="Axis index j \\(3\\) must be in \\[0, 1, 2\\]" ): pr.euler_from_quaternion(pr.q_id, 2, 3, 2, True) with pytest.raises( ValueError, match="Axis index k \\(-1\\) must be in \\[0, 1, 2\\]" ): pr.euler_from_quaternion(pr.q_id, 2, 0, -1, True) with pytest.raises( ValueError, match="Axis index k \\(3\\) must be in \\[0, 1, 2\\]" ): pr.euler_from_quaternion(pr.q_id, 2, 0, 3, True) rng = np.random.default_rng(32) euler_axes = [ [0, 2, 0], [0, 1, 0], [1, 0, 1], [1, 2, 1], [2, 1, 2], [2, 0, 2], [0, 2, 1], [0, 1, 2], [1, 0, 2], [1, 2, 0], [2, 1, 0], [2, 0, 1], ] functions = [ pr.intrinsic_euler_xzx_from_active_matrix, pr.extrinsic_euler_xzx_from_active_matrix, pr.intrinsic_euler_xyx_from_active_matrix, pr.extrinsic_euler_xyx_from_active_matrix, pr.intrinsic_euler_yxy_from_active_matrix, pr.extrinsic_euler_yxy_from_active_matrix, pr.intrinsic_euler_yzy_from_active_matrix, pr.extrinsic_euler_yzy_from_active_matrix, pr.intrinsic_euler_zyz_from_active_matrix, pr.extrinsic_euler_zyz_from_active_matrix, pr.intrinsic_euler_zxz_from_active_matrix, pr.extrinsic_euler_zxz_from_active_matrix, pr.intrinsic_euler_xzy_from_active_matrix, pr.extrinsic_euler_xzy_from_active_matrix, pr.intrinsic_euler_xyz_from_active_matrix, pr.extrinsic_euler_xyz_from_active_matrix, pr.intrinsic_euler_yxz_from_active_matrix, pr.extrinsic_euler_yxz_from_active_matrix, pr.intrinsic_euler_yzx_from_active_matrix, pr.extrinsic_euler_yzx_from_active_matrix, pr.intrinsic_euler_zyx_from_active_matrix, pr.extrinsic_euler_zyx_from_active_matrix, pr.intrinsic_euler_zxy_from_active_matrix, pr.extrinsic_euler_zxy_from_active_matrix, ] inverse_functions = [ pr.active_matrix_from_intrinsic_euler_xzx, pr.active_matrix_from_extrinsic_euler_xzx, pr.active_matrix_from_intrinsic_euler_xyx, pr.active_matrix_from_extrinsic_euler_xyx, pr.active_matrix_from_intrinsic_euler_yxy, pr.active_matrix_from_extrinsic_euler_yxy, pr.active_matrix_from_intrinsic_euler_yzy, pr.active_matrix_from_extrinsic_euler_yzy, pr.active_matrix_from_intrinsic_euler_zyz, pr.active_matrix_from_extrinsic_euler_zyz, pr.active_matrix_from_intrinsic_euler_zxz, pr.active_matrix_from_extrinsic_euler_zxz, pr.active_matrix_from_intrinsic_euler_xzy, pr.active_matrix_from_extrinsic_euler_xzy, pr.active_matrix_from_intrinsic_euler_xyz, pr.active_matrix_from_extrinsic_euler_xyz, pr.active_matrix_from_intrinsic_euler_yxz, pr.active_matrix_from_extrinsic_euler_yxz, pr.active_matrix_from_intrinsic_euler_yzx, pr.active_matrix_from_extrinsic_euler_yzx, pr.active_matrix_from_intrinsic_euler_zyx, pr.active_matrix_from_extrinsic_euler_zyx, pr.active_matrix_from_intrinsic_euler_zxy, pr.active_matrix_from_extrinsic_euler_zxy, ] fi = 0 for ea in euler_axes: for extrinsic in [False, True]: fun = functions[fi] inv_fun = inverse_functions[fi] fi += 1 for _ in range(5): e = rng.random(3) e[0] = 2.0 * np.pi * e[0] - np.pi e[1] = np.pi * e[1] e[2] = 2.0 * np.pi * e[2] - np.pi proper_euler = ea[0] == ea[2] if proper_euler: e[1] -= np.pi / 2.0 # normal case q = pr.quaternion_from_matrix(inv_fun(e)) e1 = pr.euler_from_quaternion(q, ea[0], ea[1], ea[2], extrinsic) e2 = fun(pr.matrix_from_quaternion(q)) assert_array_almost_equal( e1, e2, err_msg=f"axes: {ea}, extrinsic: {extrinsic}" ) # first singularity e[1] = 0.0 q = pr.quaternion_from_matrix(inv_fun(e)) R1 = inv_fun( pr.euler_from_quaternion(q, ea[0], ea[1], ea[2], extrinsic) ) R2 = pr.matrix_from_quaternion(q) assert_array_almost_equal( R1, R2, err_msg=f"axes: {ea}, extrinsic: {extrinsic}" ) # second singularity e[1] = np.pi q = pr.quaternion_from_matrix(inv_fun(e)) R1 = inv_fun( pr.euler_from_quaternion(q, ea[0], ea[1], ea[2], extrinsic) ) R2 = pr.matrix_from_quaternion(q) assert_array_almost_equal( R1, R2, err_msg=f"axes: {ea}, extrinsic: {extrinsic}" ) ================================================ FILE: pytransform3d/rotations/test/test_euler_deprecated.py ================================================ import numpy as np import pytest from numpy.testing import assert_array_almost_equal import pytransform3d.rotations as pr def test_active_matrix_from_intrinsic_euler_zxz(): """Test conversion from intrinsic zxz Euler angles.""" assert_array_almost_equal( pr.active_matrix_from_intrinsic_euler_zxz([0.5 * np.pi, 0, 0]), np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]), ) assert_array_almost_equal( pr.active_matrix_from_intrinsic_euler_zxz( [0.5 * np.pi, 0, 0.5 * np.pi] ), np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]), ) assert_array_almost_equal( pr.active_matrix_from_intrinsic_euler_zxz( [0.5 * np.pi, 0.5 * np.pi, 0] ), np.array([[0, 0, 1], [1, 0, 0], [0, 1, 0]]), ) assert_array_almost_equal( pr.active_matrix_from_intrinsic_euler_zxz( [0.5 * np.pi, 0.5 * np.pi, 0.5 * np.pi] ), np.array([[0, 0, 1], [0, -1, 0], [1, 0, 0]]), ) def test_active_matrix_from_extrinsic_euler_zxz(): """Test conversion from extrinsic zxz Euler angles.""" assert_array_almost_equal( pr.active_matrix_from_extrinsic_euler_zxz([0.5 * np.pi, 0, 0]), np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]), ) assert_array_almost_equal( pr.active_matrix_from_extrinsic_euler_zxz( [0.5 * np.pi, 0, 0.5 * np.pi] ), np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]), ) assert_array_almost_equal( pr.active_matrix_from_extrinsic_euler_zxz( [0.5 * np.pi, 0.5 * np.pi, 0] ), np.array([[0, -1, 0], [0, 0, -1], [1, 0, 0]]), ) assert_array_almost_equal( pr.active_matrix_from_extrinsic_euler_zxz( [0.5 * np.pi, 0.5 * np.pi, 0.5 * np.pi] ), np.array([[0, 0, 1], [0, -1, 0], [1, 0, 0]]), ) def test_active_matrix_from_intrinsic_euler_zyz(): """Test conversion from intrinsic zyz Euler angles.""" assert_array_almost_equal( pr.active_matrix_from_intrinsic_euler_zyz([0.5 * np.pi, 0, 0]), np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]), ) assert_array_almost_equal( pr.active_matrix_from_intrinsic_euler_zyz([0.5 * np.pi, 0, 0]), pr.matrix_from_euler([0.5 * np.pi, 0, 0], 2, 1, 2, False), ) assert_array_almost_equal( pr.active_matrix_from_intrinsic_euler_zyz( [0.5 * np.pi, 0, 0.5 * np.pi] ), np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]), ) assert_array_almost_equal( pr.active_matrix_from_intrinsic_euler_zyz( [0.5 * np.pi, 0.5 * np.pi, 0] ), np.array([[0, -1, 0], [0, 0, 1], [-1, 0, 0]]), ) assert_array_almost_equal( pr.active_matrix_from_intrinsic_euler_zyz( [0.5 * np.pi, 0.5 * np.pi, 0.5 * np.pi] ), np.array([[-1, 0, 0], [0, 0, 1], [0, 1, 0]]), ) def test_active_matrix_from_extrinsic_euler_zyz(): """Test conversion from roll, pitch, yaw.""" assert_array_almost_equal( pr.active_matrix_from_extrinsic_roll_pitch_yaw([0.5 * np.pi, 0, 0]), np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]]), ) assert_array_almost_equal( pr.active_matrix_from_extrinsic_roll_pitch_yaw([0.5 * np.pi, 0, 0]), pr.matrix_from_euler([0.5 * np.pi, 0, 0], 0, 1, 2, True), ) assert_array_almost_equal( pr.active_matrix_from_extrinsic_roll_pitch_yaw( [0.5 * np.pi, 0, 0.5 * np.pi] ), np.array([[0, 0, 1], [1, 0, 0], [0, 1, 0]]), ) assert_array_almost_equal( pr.active_matrix_from_extrinsic_roll_pitch_yaw( [0.5 * np.pi, 0.5 * np.pi, 0] ), np.array([[0, 1, 0], [0, 0, -1], [-1, 0, 0]]), ) assert_array_almost_equal( pr.active_matrix_from_extrinsic_roll_pitch_yaw( [0.5 * np.pi, 0.5 * np.pi, 0.5 * np.pi] ), np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]]), ) def test_active_matrix_from_intrinsic_zyx(): """Test conversion from intrinsic zyx Euler angles.""" rng = np.random.default_rng(844) for _ in range(5): euler_zyx = (rng.random(3) - 0.5) * np.array( [np.pi, 0.5 * np.pi, np.pi] ) s = np.sin(euler_zyx) c = np.cos(euler_zyx) R_from_formula = np.array( [ [ c[0] * c[1], c[0] * s[1] * s[2] - s[0] * c[2], c[0] * s[1] * c[2] + s[0] * s[2], ], [ s[0] * c[1], s[0] * s[1] * s[2] + c[0] * c[2], s[0] * s[1] * c[2] - c[0] * s[2], ], [-s[1], c[1] * s[2], c[1] * c[2]], ] ) # See Lynch, Park: Modern Robotics, page 576 # Normal case, we can reconstruct original angles R = pr.active_matrix_from_intrinsic_euler_zyx(euler_zyx) assert_array_almost_equal(R_from_formula, R) euler_zyx2 = pr.intrinsic_euler_zyx_from_active_matrix(R) assert_array_almost_equal(euler_zyx, euler_zyx2) # Gimbal lock 1, infinite solutions with constraint # alpha - gamma = constant euler_zyx[1] = 0.5 * np.pi R = pr.active_matrix_from_intrinsic_euler_zyx(euler_zyx) euler_zyx2 = pr.intrinsic_euler_zyx_from_active_matrix(R) assert pytest.approx(euler_zyx2[1]) == 0.5 * np.pi assert ( pytest.approx(euler_zyx[0] - euler_zyx[2]) == euler_zyx2[0] - euler_zyx2[2] ) # Gimbal lock 2, infinite solutions with constraint # alpha + gamma = constant euler_zyx[1] = -0.5 * np.pi R = pr.active_matrix_from_intrinsic_euler_zyx(euler_zyx) euler_zyx2 = pr.intrinsic_euler_zyx_from_active_matrix(R) assert pytest.approx(euler_zyx2[1]) == -0.5 * np.pi assert ( pytest.approx(euler_zyx[0] + euler_zyx[2]) == euler_zyx2[0] + euler_zyx2[2] ) def test_active_matrix_from_extrinsic_zyx(): """Test conversion from extrinsic zyx Euler angles.""" rng = np.random.default_rng(844) for _ in range(5): euler_zyx = (rng.random(3) - 0.5) * np.array( [np.pi, 0.5 * np.pi, np.pi] ) # Normal case, we can reconstruct original angles R = pr.active_matrix_from_extrinsic_euler_zyx(euler_zyx) euler_zyx2 = pr.extrinsic_euler_zyx_from_active_matrix(R) assert_array_almost_equal(euler_zyx, euler_zyx2) R2 = pr.active_matrix_from_extrinsic_euler_zyx(euler_zyx2) assert_array_almost_equal(R, R2) # Gimbal lock 1, infinite solutions with constraint # alpha + gamma = constant euler_zyx[1] = 0.5 * np.pi R = pr.active_matrix_from_extrinsic_euler_zyx(euler_zyx) euler_zyx2 = pr.extrinsic_euler_zyx_from_active_matrix(R) assert pytest.approx(euler_zyx2[1]) == 0.5 * np.pi assert ( pytest.approx(euler_zyx[0] + euler_zyx[2]) == euler_zyx2[0] + euler_zyx2[2] ) R2 = pr.active_matrix_from_extrinsic_euler_zyx(euler_zyx2) assert_array_almost_equal(R, R2) # Gimbal lock 2, infinite solutions with constraint # alpha - gamma = constant euler_zyx[1] = -0.5 * np.pi R = pr.active_matrix_from_extrinsic_euler_zyx(euler_zyx) euler_zyx2 = pr.extrinsic_euler_zyx_from_active_matrix(R) assert pytest.approx(euler_zyx2[1]) == -0.5 * np.pi assert ( pytest.approx(euler_zyx[0] - euler_zyx[2]) == euler_zyx2[0] - euler_zyx2[2] ) R2 = pr.active_matrix_from_extrinsic_euler_zyx(euler_zyx2) assert_array_almost_equal(R, R2) def _test_conversion_matrix_euler( matrix_from_euler, euler_from_matrix, proper_euler ): """Test conversions between Euler angles and rotation matrix.""" rng = np.random.default_rng(844) for _ in range(5): euler = (rng.random(3) - 0.5) * np.array([np.pi, 0.5 * np.pi, np.pi]) if proper_euler: euler[1] += 0.5 * np.pi # Normal case, we can reconstruct original angles R = matrix_from_euler(euler) euler2 = euler_from_matrix(R) assert_array_almost_equal(euler, euler2) R2 = matrix_from_euler(euler2) assert_array_almost_equal(R, R2) # Gimbal lock 1 if proper_euler: euler[1] = np.pi else: euler[1] = 0.5 * np.pi R = matrix_from_euler(euler) euler2 = euler_from_matrix(R) assert pytest.approx(euler[1]) == euler2[1] R2 = matrix_from_euler(euler2) assert_array_almost_equal(R, R2) # Gimbal lock 2 if proper_euler: euler[1] = 0.0 else: euler[1] = -0.5 * np.pi R = matrix_from_euler(euler) euler2 = euler_from_matrix(R) assert pytest.approx(euler[1]) == euler2[1] R2 = matrix_from_euler(euler2) assert_array_almost_equal(R, R2) def test_all_euler_matrix_conversions(): """Test all conversion between Euler angles and matrices.""" _test_conversion_matrix_euler( pr.active_matrix_from_intrinsic_euler_xzx, pr.intrinsic_euler_xzx_from_active_matrix, proper_euler=True, ) _test_conversion_matrix_euler( pr.active_matrix_from_extrinsic_euler_xzx, pr.extrinsic_euler_xzx_from_active_matrix, proper_euler=True, ) _test_conversion_matrix_euler( pr.active_matrix_from_intrinsic_euler_xyx, pr.intrinsic_euler_xyx_from_active_matrix, proper_euler=True, ) _test_conversion_matrix_euler( pr.active_matrix_from_extrinsic_euler_xyx, pr.extrinsic_euler_xyx_from_active_matrix, proper_euler=True, ) _test_conversion_matrix_euler( pr.active_matrix_from_intrinsic_euler_yxy, pr.intrinsic_euler_yxy_from_active_matrix, proper_euler=True, ) _test_conversion_matrix_euler( pr.active_matrix_from_extrinsic_euler_yxy, pr.extrinsic_euler_yxy_from_active_matrix, proper_euler=True, ) _test_conversion_matrix_euler( pr.active_matrix_from_intrinsic_euler_yzy, pr.intrinsic_euler_yzy_from_active_matrix, proper_euler=True, ) _test_conversion_matrix_euler( pr.active_matrix_from_extrinsic_euler_yzy, pr.extrinsic_euler_yzy_from_active_matrix, proper_euler=True, ) _test_conversion_matrix_euler( pr.active_matrix_from_intrinsic_euler_zyz, pr.intrinsic_euler_zyz_from_active_matrix, proper_euler=True, ) _test_conversion_matrix_euler( pr.active_matrix_from_extrinsic_euler_zyz, pr.extrinsic_euler_zyz_from_active_matrix, proper_euler=True, ) _test_conversion_matrix_euler( pr.active_matrix_from_intrinsic_euler_zxz, pr.intrinsic_euler_zxz_from_active_matrix, proper_euler=True, ) _test_conversion_matrix_euler( pr.active_matrix_from_extrinsic_euler_zxz, pr.extrinsic_euler_zxz_from_active_matrix, proper_euler=True, ) _test_conversion_matrix_euler( pr.active_matrix_from_intrinsic_euler_xzy, pr.intrinsic_euler_xzy_from_active_matrix, proper_euler=False, ) _test_conversion_matrix_euler( pr.active_matrix_from_extrinsic_euler_xzy, pr.extrinsic_euler_xzy_from_active_matrix, proper_euler=False, ) _test_conversion_matrix_euler( pr.active_matrix_from_intrinsic_euler_xyz, pr.intrinsic_euler_xyz_from_active_matrix, proper_euler=False, ) _test_conversion_matrix_euler( pr.active_matrix_from_extrinsic_euler_xyz, pr.extrinsic_euler_xyz_from_active_matrix, proper_euler=False, ) _test_conversion_matrix_euler( pr.active_matrix_from_intrinsic_euler_yxz, pr.intrinsic_euler_yxz_from_active_matrix, proper_euler=False, ) _test_conversion_matrix_euler( pr.active_matrix_from_extrinsic_euler_yxz, pr.extrinsic_euler_yxz_from_active_matrix, proper_euler=False, ) _test_conversion_matrix_euler( pr.active_matrix_from_intrinsic_euler_yzx, pr.intrinsic_euler_yzx_from_active_matrix, proper_euler=False, ) _test_conversion_matrix_euler( pr.active_matrix_from_extrinsic_euler_yzx, pr.extrinsic_euler_yzx_from_active_matrix, proper_euler=False, ) _test_conversion_matrix_euler( pr.active_matrix_from_intrinsic_euler_zyx, pr.intrinsic_euler_zyx_from_active_matrix, proper_euler=False, ) _test_conversion_matrix_euler( pr.active_matrix_from_extrinsic_euler_zyx, pr.extrinsic_euler_zyx_from_active_matrix, proper_euler=False, ) _test_conversion_matrix_euler( pr.active_matrix_from_intrinsic_euler_zxy, pr.intrinsic_euler_zxy_from_active_matrix, proper_euler=False, ) _test_conversion_matrix_euler( pr.active_matrix_from_extrinsic_euler_zxy, pr.extrinsic_euler_zxy_from_active_matrix, proper_euler=False, ) def test_active_matrix_from_extrinsic_roll_pitch_yaw(): """Test conversion from extrinsic zyz Euler angles.""" assert_array_almost_equal( pr.active_matrix_from_extrinsic_euler_zyz([0.5 * np.pi, 0, 0]), np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]), ) assert_array_almost_equal( pr.active_matrix_from_extrinsic_euler_zyz( [0.5 * np.pi, 0, 0.5 * np.pi] ), np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]), ) assert_array_almost_equal( pr.active_matrix_from_extrinsic_euler_zyz( [0.5 * np.pi, 0.5 * np.pi, 0] ), np.array([[0, 0, 1], [1, 0, 0], [0, 1, 0]]), ) assert_array_almost_equal( pr.active_matrix_from_extrinsic_euler_zyz( [0.5 * np.pi, 0.5 * np.pi, 0.5 * np.pi] ), np.array([[-1, 0, 0], [0, 0, 1], [0, 1, 0]]), ) def test_quaternion_from_extrinsic_euler_xyz(): """Test quaternion_from_extrinsic_euler_xyz.""" rng = np.random.default_rng(0) for _ in range(10): e = rng.uniform(-100, 100, [3]) q = pr.quaternion_from_extrinsic_euler_xyz(e) R_from_q = pr.matrix_from_quaternion(q) R_from_e = pr.active_matrix_from_extrinsic_euler_xyz(e) assert_array_almost_equal(R_from_q, R_from_e) def test_euler_from_quaternion_edge_case(): quaternion = [0.57114154, -0.41689009, -0.57114154, -0.41689009] matrix = pr.matrix_from_quaternion(quaternion) euler_xyz = pr.extrinsic_euler_xyz_from_active_matrix(matrix) assert not np.isnan(euler_xyz).all() ================================================ FILE: pytransform3d/rotations/test/test_matrix.py ================================================ import warnings import numpy as np import pytest from numpy.testing import assert_array_equal, assert_array_almost_equal import pytransform3d.rotations as pr def test_check_matrix(): """Test input validation for rotation matrix.""" R_list = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] R = pr.check_matrix(R_list) assert type(R) is np.ndarray assert R.dtype == np.float64 R_int_array = np.eye(3, dtype=int) R = pr.check_matrix(R_int_array) assert type(R) is np.ndarray assert R.dtype == np.float64 R_array = np.eye(3) R = pr.check_matrix(R_array) assert_array_equal(R_array, R) R = np.eye(4) with pytest.raises(ValueError, match="Expected rotation matrix with shape"): pr.check_matrix(R) R = np.array([[1, 0, 0], [0, 1, 0], [0, 0.1, 1]]) with pytest.raises(ValueError, match="inversion by transposition"): pr.check_matrix(R) with warnings.catch_warnings(record=True) as w: pr.check_matrix(R, strict_check=False) assert len(w) == 1 R = np.array([[1, 0, 1e-16], [0, 1, 0], [0, 0, 1]]) R2 = pr.check_matrix(R) assert_array_equal(R, R2) R = -np.eye(3) with pytest.raises(ValueError, match="determinant"): pr.check_matrix(R) with warnings.catch_warnings(record=True) as w: pr.check_matrix(R, strict_check=False) assert len(w) == 1 def test_check_matrix_threshold(): """Test matrix threshold. See issue #54. """ R = np.array( [ [-9.15361835e-01, 4.01808328e-01, 2.57475872e-02], [5.15480570e-02, 1.80374088e-01, -9.82246499e-01], [-3.99318925e-01, -8.97783496e-01, -1.85819250e-01], ] ) pr.assert_rotation_matrix(R) pr.check_matrix(R) def test_assert_rotation_matrix_behaves_like_check_matrix(): """Test of both checks for rotation matrix validity behave similar.""" rng = np.random.default_rng(2345) for _ in range(5): a = pr.random_axis_angle(rng) R = pr.matrix_from_axis_angle(a) original_value = R[2, 2] for error in [0, 1e-8, 1e-7, 1e-5, 1e-4, 1]: R[2, 2] = original_value + error try: pr.assert_rotation_matrix(R) pr.check_matrix(R) except AssertionError: with pytest.raises( ValueError, match="Expected rotation matrix" ): pr.check_matrix(R) def test_deactivate_rotation_matrix_precision_error(): R = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 2.0]]) with pytest.raises(ValueError, match="Expected rotation matrix"): pr.check_matrix(R) with warnings.catch_warnings(record=True) as w: pr.check_matrix(R, strict_check=False) assert len(w) == 1 def test_matrix_requires_renormalization(): R = np.eye(3) assert not pr.matrix_requires_renormalization(R) R[1, 0] += 1e-3 assert pr.matrix_requires_renormalization(R) rng = np.random.default_rng(39232) R_total = np.eye(3) for _ in range(10): e = pr.random_vector(rng, 3) R = pr.active_matrix_from_extrinsic_roll_pitch_yaw(e) assert not pr.matrix_requires_renormalization(R, tolerance=1e-16) R_total = np.dot(R, R_total) assert pr.matrix_requires_renormalization(R_total, tolerance=1e-16) def test_norm_rotation_matrix(): R = pr.norm_matrix(np.eye(3)) assert_array_equal(R, np.eye(3)) R[1, 0] += np.finfo(float).eps R = pr.norm_matrix(R) assert_array_equal(R, np.eye(3)) assert np.linalg.det(R) == 1.0 R = np.eye(3) R[1, 1] += 0.3 R_norm = pr.norm_matrix(R) assert pytest.approx(np.linalg.det(R_norm)) == 1.0 assert_array_almost_equal(np.eye(3), R_norm) def test_matrix_from_two_vectors(): with pytest.raises(ValueError, match="a must not be the zero vector"): pr.matrix_from_two_vectors(np.zeros(3), np.zeros(3)) with pytest.raises(ValueError, match="b must not be the zero vector"): pr.matrix_from_two_vectors(np.ones(3), np.zeros(3)) with pytest.raises(ValueError, match="a and b must not be parallel"): pr.matrix_from_two_vectors(np.ones(3), np.ones(3)) R = pr.matrix_from_two_vectors(pr.unitx, pr.unity) assert_array_almost_equal(R, np.eye(3)) rng = np.random.default_rng(28) for _ in range(5): a = pr.random_vector(rng, 3) b = pr.random_vector(rng, 3) R = pr.matrix_from_two_vectors(a, b) pr.assert_rotation_matrix(R) assert_array_almost_equal(pr.norm_vector(a), R[:, 0]) assert ( pytest.approx(pr.angle_between_vectors(b, R[:, 2])) == 0.5 * np.pi ) def test_conversions_matrix_axis_angle(): """Test conversions between rotation matrix and axis-angle.""" R = np.eye(3) a = pr.axis_angle_from_matrix(R) pr.assert_axis_angle_equal(a, np.array([1, 0, 0, 0])) R = pr.active_matrix_from_intrinsic_euler_xyz( np.array([-np.pi, -np.pi, 0.0]) ) a = pr.axis_angle_from_matrix(R) pr.assert_axis_angle_equal(a, np.array([0, 0, 1, np.pi])) R = pr.active_matrix_from_intrinsic_euler_xyz( np.array([-np.pi, 0.0, -np.pi]) ) a = pr.axis_angle_from_matrix(R) pr.assert_axis_angle_equal(a, np.array([0, 1, 0, np.pi])) R = pr.active_matrix_from_intrinsic_euler_xyz( np.array([0.0, -np.pi, -np.pi]) ) a = pr.axis_angle_from_matrix(R) pr.assert_axis_angle_equal(a, np.array([1, 0, 0, np.pi])) a = np.array([np.sqrt(0.5), np.sqrt(0.5), 0.0, np.pi]) R = pr.matrix_from_axis_angle(a) a2 = pr.axis_angle_from_matrix(R) pr.assert_axis_angle_equal(a2, a) rng = np.random.default_rng(0) for _ in range(50): a = pr.random_axis_angle(rng) R = pr.matrix_from_axis_angle(a) pr.assert_rotation_matrix(R) a2 = pr.axis_angle_from_matrix(R) pr.assert_axis_angle_equal(a, a2) R2 = pr.matrix_from_axis_angle(a2) assert_array_almost_equal(R, R2) pr.assert_rotation_matrix(R2) def test_compare_axis_angle_from_matrix_to_lynch_park(): """Compare log(R) to the version of Lynch, Park: Modern Robotics.""" R = np.eye(3) a = pr.axis_angle_from_matrix(R) pr.assert_axis_angle_equal(a, [0, 0, 0, 0]) R = pr.passive_matrix_from_angle(2, np.pi) assert pytest.approx(np.trace(R)) == -1 a = pr.axis_angle_from_matrix(R) axis = ( 1.0 / np.sqrt(2.0 * (1 + R[2, 2])) * np.array([R[0, 2], R[1, 2], 1 + R[2, 2]]) ) pr.assert_axis_angle_equal(a, np.hstack((axis, (np.pi,)))) R = pr.passive_matrix_from_angle(1, np.pi) assert pytest.approx(np.trace(R)) == -1 a = pr.axis_angle_from_matrix(R) axis = ( 1.0 / np.sqrt(2.0 * (1 + R[1, 1])) * np.array([R[0, 1], 1 + R[1, 1], R[2, 1]]) ) pr.assert_axis_angle_equal(a, np.hstack((axis, (np.pi,)))) R = pr.passive_matrix_from_angle(0, np.pi) assert pytest.approx(np.trace(R)) == -1 a = pr.axis_angle_from_matrix(R) axis = ( 1.0 / np.sqrt(2.0 * (1 + R[0, 0])) * np.array([1 + R[0, 0], R[1, 0], R[2, 0]]) ) pr.assert_axis_angle_equal(a, np.hstack((axis, (np.pi,)))) # normal case is omitted here def test_conversions_matrix_compact_axis_angle(): """Test conversions between rotation matrix and axis-angle.""" R = np.eye(3) a = pr.compact_axis_angle_from_matrix(R) pr.assert_compact_axis_angle_equal(a, np.zeros(3)) R = pr.active_matrix_from_intrinsic_euler_xyz( np.array([-np.pi, -np.pi, 0.0]) ) a = pr.compact_axis_angle_from_matrix(R) pr.assert_compact_axis_angle_equal(a, np.array([0, 0, np.pi])) R = pr.active_matrix_from_intrinsic_euler_xyz( np.array([-np.pi, 0.0, -np.pi]) ) a = pr.compact_axis_angle_from_matrix(R) pr.assert_compact_axis_angle_equal(a, np.array([0, np.pi, 0])) R = pr.active_matrix_from_intrinsic_euler_xyz( np.array([0.0, -np.pi, -np.pi]) ) a = pr.compact_axis_angle_from_matrix(R) pr.assert_compact_axis_angle_equal(a, np.array([np.pi, 0, 0])) a = np.array([np.sqrt(0.5) * np.pi, np.sqrt(0.5) * np.pi, 0.0]) R = pr.matrix_from_compact_axis_angle(a) a2 = pr.compact_axis_angle_from_matrix(R) pr.assert_compact_axis_angle_equal(a2, a) rng = np.random.default_rng(0) for _ in range(50): a = pr.random_compact_axis_angle(rng) R = pr.matrix_from_compact_axis_angle(a) pr.assert_rotation_matrix(R) a2 = pr.compact_axis_angle_from_matrix(R) pr.assert_compact_axis_angle_equal(a, a2) R2 = pr.matrix_from_compact_axis_angle(a2) assert_array_almost_equal(R, R2) pr.assert_rotation_matrix(R2) def test_issue43(): """Test axis_angle_from_matrix() with angles close to 0 and pi.""" a = np.array([-1.0, 1.0, 1.0, np.pi - 5e-8]) a[:3] = a[:3] / np.linalg.norm(a[:3]) R = pr.matrix_from_axis_angle(a) a2 = pr.axis_angle_from_matrix(R) pr.assert_axis_angle_equal(a, a2) a = np.array([-1.0, 1.0, 1.0, 5e-8]) a[:3] = a[:3] / np.linalg.norm(a[:3]) R = pr.matrix_from_axis_angle(a) a2 = pr.axis_angle_from_matrix(R) pr.assert_axis_angle_equal(a, a2) a = np.array([-1.0, 1.0, 1.0, np.pi + 5e-8]) a[:3] = a[:3] / np.linalg.norm(a[:3]) R = pr.matrix_from_axis_angle(a) a2 = pr.axis_angle_from_matrix(R) pr.assert_axis_angle_equal(a, a2) a = np.array([-1.0, 1.0, 1.0, -5e-8]) a[:3] = a[:3] / np.linalg.norm(a[:3]) R = pr.matrix_from_axis_angle(a) a2 = pr.axis_angle_from_matrix(R) pr.assert_axis_angle_equal(a, a2) def test_issue43_numerical_precision(): """Test numerical precision of angles close to 0 and pi.""" a = np.array([1.0, 1.0, 1.0, np.pi - 1e-7]) a[:3] = a[:3] / np.linalg.norm(a[:3]) R = pr.matrix_from_axis_angle(a) a2 = pr.axis_angle_from_matrix(R) axis_dist = np.linalg.norm(a[:3] - a2[:3]) assert axis_dist < 1e-10 assert abs(a[3] - a2[3]) < 1e-8 a = np.array([1.0, 1.0, 1.0, 1e-7]) a[:3] = a[:3] / np.linalg.norm(a[:3]) R = pr.matrix_from_axis_angle(a) a2 = pr.axis_angle_from_matrix(R) axis_dist = np.linalg.norm(a[:3] - a2[:3]) assert axis_dist < 1e-10 assert abs(a[3] - a2[3]) < 1e-8 def test_conversions_matrix_axis_angle_continuous(): """Test continuous conversions between rotation matrix and axis-angle.""" for angle in np.arange(3.1, 3.2, 0.01): a = np.array([1.0, 0.0, 0.0, angle]) R = pr.matrix_from_axis_angle(a) pr.assert_rotation_matrix(R) a2 = pr.axis_angle_from_matrix(R) pr.assert_axis_angle_equal(a, a2) R2 = pr.matrix_from_axis_angle(a2) assert_array_almost_equal(R, R2) pr.assert_rotation_matrix(R2) def test_matrix_from_quaternion_hamilton(): """Test if the conversion from quaternion to matrix is Hamiltonian.""" q = np.sqrt(0.5) * np.array([1, 0, 0, 1]) R = pr.matrix_from_quaternion(q) assert_array_almost_equal(np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]), R) def test_quaternion_from_matrix_180(): """Test for bug in conversion from 180 degree rotations.""" a = np.array([1.0, 0.0, 0.0, np.pi]) q = pr.quaternion_from_axis_angle(a) R = pr.matrix_from_axis_angle(a) q_from_R = pr.quaternion_from_matrix(R) assert_array_almost_equal(q, q_from_R) a = np.array([0.0, 1.0, 0.0, np.pi]) q = pr.quaternion_from_axis_angle(a) R = pr.matrix_from_axis_angle(a) q_from_R = pr.quaternion_from_matrix(R) assert_array_almost_equal(q, q_from_R) a = np.array([0.0, 0.0, 1.0, np.pi]) q = pr.quaternion_from_axis_angle(a) R = pr.matrix_from_axis_angle(a) q_from_R = pr.quaternion_from_matrix(R) assert_array_almost_equal(q, q_from_R) R = np.array([[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, -1.0]]) with pytest.raises(ValueError, match="Expected rotation matrix"): pr.quaternion_from_matrix(R) R = np.array( [[-1.0, 0.0, 0.0], [0.0, 0.00000001, 1.0], [0.0, 1.0, -0.00000001]] ) q_from_R = pr.quaternion_from_matrix(R) def test_quaternion_from_matrix_180_not_axis_aligned(): """Test for bug in rotation by 180 degrees around arbitrary axes.""" rng = np.random.default_rng(0) for _ in range(10): a = pr.random_axis_angle(rng) a[3] = np.pi q = pr.quaternion_from_axis_angle(a) R = pr.matrix_from_axis_angle(a) q_from_R = pr.quaternion_from_matrix(R) pr.assert_quaternion_equal(q, q_from_R) def test_axis_angle_from_matrix_cos_angle_greater_1(): R = np.array( [ [ 1.0000000000000004, -1.4402617650886727e-08, 2.3816502339526408e-08, ], [ 1.4402617501592725e-08, 1.0000000000000004, 1.2457848566326355e-08, ], [ -2.3816502529500374e-08, -1.2457848247850049e-08, 0.9999999999999999, ], ] ) a = pr.axis_angle_from_matrix(R) assert not any(np.isnan(a)) def test_axis_angle_from_matrix_without_check(): R = -np.eye(3) with warnings.catch_warnings(record=True) as w: a = pr.axis_angle_from_matrix(R, check=False) assert len(w) == 1 assert all(np.isnan(a[:3])) def test_bug_189(): """Test bug #189""" R = np.array( [ [ -1.0000000000000004e00, 2.8285718503485576e-16, 1.0966597378775709e-16, ], [ 1.0966597378775709e-16, -2.2204460492503131e-16, 1.0000000000000002e00, ], [ 2.8285718503485576e-16, 1.0000000000000002e00, -2.2204460492503131e-16, ], ] ) a1 = pr.compact_axis_angle_from_matrix(R) a2 = pr.compact_axis_angle_from_matrix(pr.norm_matrix(R)) assert_array_almost_equal(a1, a2) def test_bug_198(): """Test bug #198""" R = np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]], dtype=float) a = pr.compact_axis_angle_from_matrix(R) R2 = pr.matrix_from_compact_axis_angle(a) assert_array_almost_equal(R, R2) ================================================ FILE: pytransform3d/rotations/test/test_mrp.py ================================================ import numpy as np import pytest from numpy.testing import assert_array_almost_equal import pytransform3d.rotations as pr def test_check_mrp(): with pytest.raises( ValueError, match="Expected modified Rodrigues parameters with shape" ): pr.check_mrp([]) with pytest.raises( ValueError, match="Expected modified Rodrigues parameters with shape" ): pr.check_mrp(np.zeros((3, 4))) def test_norm_mrp(): mrp_norm = pr.norm_mrp(pr.mrp_from_axis_angle([1.0, 0.0, 0.0, 1.5 * np.pi])) assert_array_almost_equal( [-1.0, 0.0, 0.0, 0.5 * np.pi], pr.axis_angle_from_mrp(mrp_norm) ) mrp_norm = pr.norm_mrp( pr.mrp_from_axis_angle([1.0, 0.0, 0.0, -0.5 * np.pi]) ) assert_array_almost_equal( [-1.0, 0.0, 0.0, 0.5 * np.pi], pr.axis_angle_from_mrp(mrp_norm) ) mrp_norm = pr.norm_mrp(pr.mrp_from_axis_angle([1.0, 0.0, 0.0, 2.0 * np.pi])) assert_array_almost_equal( [1.0, 0.0, 0.0, 0.0], pr.axis_angle_from_mrp(mrp_norm) ) mrp_norm = pr.norm_mrp( pr.mrp_from_axis_angle([1.0, 0.0, 0.0, -2.0 * np.pi]) ) assert_array_almost_equal( [1.0, 0.0, 0.0, 0.0], pr.axis_angle_from_mrp(mrp_norm) ) def test_mrp_near_singularity(): axis = np.array([1.0, 0.0, 0.0]) assert pr.mrp_near_singularity(np.tan(2.0 * np.pi / 4.0) * axis) assert pr.mrp_near_singularity(np.tan(2.0 * np.pi / 4.0 - 1e-7) * axis) assert pr.mrp_near_singularity(np.tan(2.0 * np.pi / 4.0 + 1e-7) * axis) assert not pr.mrp_near_singularity(np.tan(np.pi / 4.0) * axis) assert not pr.mrp_near_singularity(np.tan(0.0 / 4.0) * axis) def test_mrp_double(): rng = np.random.default_rng(23238) mrp = pr.random_vector(rng, 3) mrp_double = pr.mrp_double(mrp) q = pr.quaternion_from_mrp(mrp) q_double = pr.quaternion_from_mrp(mrp_double) pr.assert_mrp_equal(mrp, mrp_double) assert not np.allclose(mrp, mrp_double) pr.assert_quaternion_equal(q, q_double) assert not np.allclose(q, q_double) assert_array_almost_equal(np.zeros(3), pr.mrp_double(np.zeros(3))) def test_concatenate_mrp(): rng = np.random.default_rng(283) for _ in range(5): q1 = pr.random_quaternion(rng) q2 = pr.random_quaternion(rng) q12 = pr.concatenate_quaternions(q1, q2) mrp1 = pr.mrp_from_quaternion(q1) mrp2 = pr.mrp_from_quaternion(q2) mrp12 = pr.concatenate_mrp(mrp1, mrp2) pr.assert_quaternion_equal(q12, pr.quaternion_from_mrp(mrp12)) def test_mrp_prod_vector(): rng = np.random.default_rng(2183) v = pr.random_vector(rng, 3) assert_array_almost_equal(v, pr.mrp_prod_vector([0, 0, 0], v)) for _ in range(5): mrp = pr.random_vector(rng, 3) q = pr.quaternion_from_mrp(mrp) v_mrp = pr.mrp_prod_vector(mrp, v) v_q = pr.q_prod_vector(q, v) assert_array_almost_equal(v_mrp, v_q) def test_mrp_quat_conversions(): rng = np.random.default_rng(22) for _ in range(5): q = pr.random_quaternion(rng) mrp = pr.mrp_from_quaternion(q) q2 = pr.quaternion_from_mrp(mrp) pr.assert_quaternion_equal(q, q2) def test_axis_angle_from_mrp(): rng = np.random.default_rng(98343) for _ in range(5): mrp = pr.random_vector(rng, 3) a = pr.axis_angle_from_mrp(mrp) q = pr.quaternion_from_mrp(mrp) pr.assert_axis_angle_equal(a, pr.axis_angle_from_quaternion(q)) pr.assert_axis_angle_equal( pr.axis_angle_from_mrp([np.tan(0.5 * np.pi), 0.0, 0.0]), [1.0, 0.0, 0.0, 0.0], ) pr.assert_axis_angle_equal( pr.axis_angle_from_mrp([0.0, 0.0, 0.0]), [1.0, 0.0, 0.0, 0.0] ) ================================================ FILE: pytransform3d/rotations/test/test_polar_decom.py ================================================ import numpy as np import pytest from numpy.testing import assert_array_almost_equal import pytransform3d.rotations as pr def test_polar_decomposition(): # no iterations required R_norm = pr.robust_polar_decomposition(np.eye(3), n_iter=1, eps=1e-32) assert_array_almost_equal(R_norm, np.eye(3)) rng = np.random.default_rng(33) # scaled valid rotation matrix R = pr.random_matrix(rng) R_norm = pr.robust_polar_decomposition(23.5 * R) assert_array_almost_equal(R_norm, R) # slightly rotated basis vector R = pr.matrix_from_euler(np.deg2rad([-45, 45, 45]), 2, 1, 0, True) R_unnormalized = np.copy(R) R_unnormalized[:, 0] = np.dot( pr.active_matrix_from_angle(2, np.deg2rad(1.0)), R_unnormalized[:, 0] ) # norm_matrix will just fix the orthogonality with the cross product of the # two other basis vectors assert_array_almost_equal(pr.norm_matrix(R_unnormalized), R) # polar decomposition will spread the error more evenly among basis vectors errors = np.linalg.norm((R_unnormalized - R).T, axis=-1) assert pytest.approx(errors.tolist()) == [0.00909314, 0, 0] R_norm = pr.robust_polar_decomposition(R_unnormalized) norm_errors = np.linalg.norm((R_norm - R).T, axis=-1) assert np.all(norm_errors > 0) assert np.all(norm_errors < errors[0]) assert np.std(norm_errors) < np.std(errors) # random rotations of random basis vectors for _ in range(5): R = pr.random_matrix(rng) R_unnormalized = np.copy(R) random_axis = rng.integers(0, 3) R_unnormalized[:, random_axis] = np.dot( pr.active_matrix_from_angle( rng.integers(0, 3), np.deg2rad(rng.uniform(-3, 3)) ), R_unnormalized[:, random_axis], ) R_norm = pr.robust_polar_decomposition(R_unnormalized) errors = np.linalg.norm((R_unnormalized - R).T, axis=-1) norm_errors = np.linalg.norm((R_norm - R).T, axis=-1) assert np.all(norm_errors > 0) assert np.all(norm_errors < errors.sum()) assert np.std(norm_errors) < np.std(errors) ================================================ FILE: pytransform3d/rotations/test/test_quaternion.py ================================================ import numpy as np import pytest from numpy.testing import assert_array_almost_equal, assert_array_equal import pytransform3d.rotations as pr def test_check_quaternion(): """Test input validation for quaternion representation.""" q_list = [1, 0, 0, 0] q = pr.check_quaternion(q_list) assert_array_almost_equal(q_list, q) assert type(q) is np.ndarray assert q.dtype == np.float64 rng = np.random.default_rng(0) q = rng.standard_normal(4) q = pr.check_quaternion(q) assert pytest.approx(np.linalg.norm(q)) == 1.0 with pytest.raises(ValueError, match="Expected quaternion with shape"): pr.check_quaternion(np.zeros(3)) with pytest.raises(ValueError, match="Expected quaternion with shape"): pr.check_quaternion(np.zeros((3, 3))) q = np.array([0.0, 1.2, 0.0, 0.0]) q2 = pr.check_quaternion(q, unit=False) assert_array_almost_equal(q, q2) def test_check_quaternions(): """Test input validation for sequence of quaternions.""" Q_list = [[1, 0, 0, 0]] Q = pr.check_quaternions(Q_list) assert_array_almost_equal(Q_list, Q) assert type(Q) is np.ndarray assert Q.dtype == np.float64 assert Q.ndim == 2 assert_array_equal(Q.shape, (1, 4)) Q = np.array([[2, 0, 0, 0], [3, 0, 0, 0], [4, 0, 0, 0], [5, 0, 0, 0]]) Q = pr.check_quaternions(Q) for i in range(len(Q)): assert pytest.approx(np.linalg.norm(Q[i])) == 1 with pytest.raises( ValueError, match="Expected quaternion array with shape" ): pr.check_quaternions(np.zeros(4)) with pytest.raises( ValueError, match="Expected quaternion array with shape" ): pr.check_quaternions(np.zeros((3, 3))) Q = np.array([[0.0, 1.2, 0.0, 0.0]]) Q2 = pr.check_quaternions(Q, unit=False) assert_array_almost_equal(Q, Q2) def test_quaternion_requires_renormalization(): assert not pr.quaternion_requires_renormalization(pr.q_id) q = pr.q_id + np.array([1e-3, 0.0, 0.0, 0.0]) assert pr.quaternion_requires_renormalization(q) def test_quaternion_double(): rng = np.random.default_rng(2235) for _ in range(5): q1 = pr.random_quaternion(rng) q2 = pr.quaternion_double(q1) pr.assert_quaternion_equal(q1, q2) def test_pick_closest_quaternion(): rng = np.random.default_rng(483) for _ in range(10): q = pr.random_quaternion(rng) assert_array_almost_equal(pr.pick_closest_quaternion(q, q), q) assert_array_almost_equal(pr.pick_closest_quaternion(-q, q), q) def test_quaternion_gradient_integration(): """Test integration of quaternion gradients.""" n_steps = 21 dt = 0.1 rng = np.random.default_rng(3) for _ in range(5): q1 = pr.random_quaternion(rng) q2 = pr.random_quaternion(rng) Q = np.vstack( [pr.quaternion_slerp(q1, q2, t) for t in np.linspace(0, 1, n_steps)] ) angular_velocities = pr.quaternion_gradient(Q, dt) Q2 = pr.quaternion_integrate(angular_velocities, q1, dt) assert_array_almost_equal(Q, Q2) def test_concatenate_quaternions(): """Test concatenation of two quaternions.""" rng = np.random.default_rng(0) for _ in range(5): q1 = pr.quaternion_from_axis_angle(pr.random_axis_angle(rng)) q2 = pr.quaternion_from_axis_angle(pr.random_axis_angle(rng)) R1 = pr.matrix_from_quaternion(q1) R2 = pr.matrix_from_quaternion(q2) q12 = pr.concatenate_quaternions(q1, q2) R12 = np.dot(R1, R2) q12R = pr.quaternion_from_matrix(R12) pr.assert_quaternion_equal(q12, q12R) def test_concatenate_quaternions_list_array(): """Test concatenation of two quaternions given as list and array.""" # Until ea9adc5, this combination of a list and a numpy array raised # a ValueError: q1 = [1, 0, 0, 0] q2 = np.array([0, 0, 0, 1]) q12 = pr.concatenate_quaternions(q1, q2) assert_array_almost_equal(q12, np.array([0, 0, 0, 1])) def test_quaternion_hamilton(): """Test if quaternion multiplication follows Hamilton's convention.""" q_ij = pr.concatenate_quaternions(pr.q_i, pr.q_j) assert_array_equal(pr.q_k, q_ij) q_ijk = pr.concatenate_quaternions(q_ij, pr.q_k) assert_array_equal(-pr.q_id, q_ijk) def test_quaternion_rotation(): """Test quaternion rotation.""" rng = np.random.default_rng(0) for _ in range(5): q = pr.quaternion_from_axis_angle(pr.random_axis_angle(rng)) R = pr.matrix_from_quaternion(q) v = pr.random_vector(rng) vR = np.dot(R, v) vq = pr.q_prod_vector(q, v) assert_array_almost_equal(vR, vq) def test_quaternion_rotation_consistent_with_multiplication(): """Test if quaternion rotation and multiplication are Hamiltonian.""" rng = np.random.default_rng(1) for _ in range(5): v = pr.random_vector(rng) q = pr.random_quaternion(rng) v_im = np.hstack(((0.0,), v)) qv_mult = pr.concatenate_quaternions( q, pr.concatenate_quaternions(v_im, pr.q_conj(q)) )[1:] qv_rot = pr.q_prod_vector(q, v) assert_array_almost_equal(qv_mult, qv_rot) def test_quaternion_conjugate(): """Test quaternion conjugate.""" rng = np.random.default_rng(0) for _ in range(5): q = pr.random_quaternion(rng) v = pr.random_vector(rng) vq = pr.q_prod_vector(q, v) vq2 = pr.concatenate_quaternions( pr.concatenate_quaternions(q, np.hstack(([0], v))), pr.q_conj(q) )[1:] assert_array_almost_equal(vq, vq2) def test_quaternion_invert(): """Test unit quaternion inversion with conjugate.""" q = np.array([0.58183503, -0.75119889, -0.24622332, 0.19116072]) q_inv = pr.q_conj(q) q_q_inv = pr.concatenate_quaternions(q, q_inv) assert_array_almost_equal(pr.q_id, q_q_inv) def test_quaternion_dist(): """Test angular metric of quaternions.""" rng = np.random.default_rng(0) for _ in range(5): q1 = pr.quaternion_from_axis_angle(pr.random_axis_angle(rng)) q2 = pr.quaternion_from_axis_angle(pr.random_axis_angle(rng)) q1_to_q1 = pr.quaternion_dist(q1, q1) assert pytest.approx(q1_to_q1) == 0.0 q2_to_q2 = pr.quaternion_dist(q2, q2) assert pytest.approx(q2_to_q2) == 0.0 q1_to_q2 = pr.quaternion_dist(q1, q2) q2_to_q1 = pr.quaternion_dist(q2, q1) assert pytest.approx(q1_to_q2) == q2_to_q1 assert 2.0 * np.pi > q1_to_q2 def test_quaternion_dist_for_identical_rotations(): """Test angular metric of quaternions q and -q.""" rng = np.random.default_rng(0) for _ in range(5): q = pr.quaternion_from_axis_angle(pr.random_axis_angle(rng)) assert_array_almost_equal( pr.matrix_from_quaternion(q), pr.matrix_from_quaternion(-q) ) assert pr.quaternion_dist(q, -q) == 0.0 def test_quaternion_dist_for_almost_identical_rotations(): """Test angular metric of quaternions q and ca. -q.""" rng = np.random.default_rng(0) for _ in range(5): a = pr.random_axis_angle(rng) q1 = pr.quaternion_from_axis_angle(a) r = 1e-4 * rng.standard_normal(4) q2 = -pr.quaternion_from_axis_angle(a + r) assert pytest.approx(pr.quaternion_dist(q1, q2), abs=1e-3) == 0.0 def test_quaternion_diff(): """Test difference of quaternions.""" rng = np.random.default_rng(0) for _ in range(5): q1 = pr.random_quaternion(rng) q2 = pr.random_quaternion(rng) a_diff = pr.quaternion_diff(q1, q2) # q1 - q2 q_diff = pr.quaternion_from_axis_angle(a_diff) q3 = pr.concatenate_quaternions(q_diff, q2) # q1 - q2 + q2 pr.assert_quaternion_equal(q1, q3) def test_quaternion_from_euler(): """Quaternion from Euler angles.""" with pytest.raises( ValueError, match="Axis index i \\(-1\\) must be in \\[0, 1, 2\\]" ): pr.quaternion_from_euler(np.zeros(3), -1, 0, 2, True) with pytest.raises( ValueError, match="Axis index i \\(3\\) must be in \\[0, 1, 2\\]" ): pr.quaternion_from_euler(np.zeros(3), 3, 0, 2, True) with pytest.raises( ValueError, match="Axis index j \\(-1\\) must be in \\[0, 1, 2\\]" ): pr.quaternion_from_euler(np.zeros(3), 2, -1, 2, True) with pytest.raises( ValueError, match="Axis index j \\(3\\) must be in \\[0, 1, 2\\]" ): pr.quaternion_from_euler(np.zeros(3), 2, 3, 2, True) with pytest.raises( ValueError, match="Axis index k \\(-1\\) must be in \\[0, 1, 2\\]" ): pr.quaternion_from_euler(np.zeros(3), 2, 0, -1, True) with pytest.raises( ValueError, match="Axis index k \\(3\\) must be in \\[0, 1, 2\\]" ): pr.quaternion_from_euler(np.zeros(3), 2, 0, 3, True) euler_axes = [ [0, 2, 0], [0, 1, 0], [1, 0, 1], [1, 2, 1], [2, 1, 2], [2, 0, 2], [0, 2, 1], [0, 1, 2], [1, 0, 2], [1, 2, 0], [2, 1, 0], [2, 0, 1], ] rng = np.random.default_rng(83) for ea in euler_axes: for extrinsic in [False, True]: for _ in range(5): e = rng.random(3) e[0] = 2.0 * np.pi * e[0] - np.pi e[1] = np.pi * e[1] e[2] = 2.0 * np.pi * e[2] - np.pi proper_euler = ea[0] == ea[2] if proper_euler: e[1] -= np.pi / 2.0 q = pr.quaternion_from_euler(e, ea[0], ea[1], ea[2], extrinsic) e2 = pr.euler_from_quaternion(q, ea[0], ea[1], ea[2], extrinsic) q2 = pr.quaternion_from_euler( e2, ea[0], ea[1], ea[2], extrinsic ) pr.assert_quaternion_equal(q, q2) def test_conversions_matrix_quaternion(): """Test conversions between rotation matrix and quaternion.""" R = np.eye(3) a = pr.axis_angle_from_matrix(R) assert_array_almost_equal(a, np.array([1, 0, 0, 0])) rng = np.random.default_rng(0) for _ in range(5): q = pr.random_quaternion(rng) R = pr.matrix_from_quaternion(q) pr.assert_rotation_matrix(R) q2 = pr.quaternion_from_matrix(R) pr.assert_quaternion_equal(q, q2) R2 = pr.matrix_from_quaternion(q2) assert_array_almost_equal(R, R2) pr.assert_rotation_matrix(R2) def test_quaternion_conventions(): """Test conversion of quaternion between wxyz and xyzw.""" q_wxyz = np.array([1.0, 0.0, 0.0, 0.0]) q_xyzw = pr.quaternion_xyzw_from_wxyz(q_wxyz) assert_array_equal(q_xyzw, np.array([0.0, 0.0, 0.0, 1.0])) q_wxyz2 = pr.quaternion_wxyz_from_xyzw(q_xyzw) assert_array_equal(q_wxyz, q_wxyz2) rng = np.random.default_rng(42) q_wxyz_random = pr.random_quaternion(rng) q_xyzw_random = pr.quaternion_xyzw_from_wxyz(q_wxyz_random) assert_array_equal(q_xyzw_random[:3], q_wxyz_random[1:]) assert q_xyzw_random[3] == q_wxyz_random[0] q_wxyz_random2 = pr.quaternion_wxyz_from_xyzw(q_xyzw_random) assert_array_equal(q_wxyz_random, q_wxyz_random2) ================================================ FILE: pytransform3d/rotations/test/test_rot_log.py ================================================ import warnings import numpy as np import pytest from numpy.testing import assert_array_almost_equal import pytransform3d.rotations as pr def test_check_skew_symmetric_matrix(): with pytest.raises( ValueError, match="Expected skew-symmetric matrix with shape" ): pr.check_skew_symmetric_matrix([]) with pytest.raises( ValueError, match="Expected skew-symmetric matrix with shape" ): pr.check_skew_symmetric_matrix(np.zeros((3, 4))) with pytest.raises( ValueError, match="Expected skew-symmetric matrix with shape" ): pr.check_skew_symmetric_matrix(np.zeros((4, 3))) V = np.zeros((3, 3)) V[0, 0] = 0.001 with pytest.raises( ValueError, match="Expected skew-symmetric matrix, but it failed the test", ): pr.check_skew_symmetric_matrix(V) with warnings.catch_warnings(record=True) as w: pr.check_skew_symmetric_matrix(V, strict_check=False) assert len(w) == 1 pr.check_skew_symmetric_matrix(np.zeros((3, 3))) def test_cross_product_matrix(): """Test cross-product matrix.""" rng = np.random.default_rng(0) for _ in range(5): v = pr.random_vector(rng) w = pr.random_vector(rng) V = pr.cross_product_matrix(v) pr.check_skew_symmetric_matrix(V) r1 = np.cross(v, w) r2 = np.dot(V, w) assert_array_almost_equal(r1, r2) ================================================ FILE: pytransform3d/rotations/test/test_rotations_jacobians.py ================================================ import numpy as np from numpy.testing import assert_array_almost_equal import pytransform3d.rotations as pr def test_jacobian_so3(): omega = np.zeros(3) J = pr.left_jacobian_SO3(omega) J_series = pr.left_jacobian_SO3_series(omega, 20) assert_array_almost_equal(J, J_series) J_inv = pr.left_jacobian_SO3_inv(omega) J_inv_series = pr.left_jacobian_SO3_inv_series(omega, 20) assert_array_almost_equal(J_inv, J_inv_series) J_inv_J = np.dot(J_inv, J) assert_array_almost_equal(J_inv_J, np.eye(3)) rng = np.random.default_rng(0) for _ in range(5): omega = pr.random_compact_axis_angle(rng) J = pr.left_jacobian_SO3(omega) J_series = pr.left_jacobian_SO3_series(omega, 20) assert_array_almost_equal(J, J_series) J_inv = pr.left_jacobian_SO3_inv(omega) J_inv_series = pr.left_jacobian_SO3_inv_series(omega, 20) assert_array_almost_equal(J_inv, J_inv_series) J_inv_J = np.dot(J_inv, J) assert_array_almost_equal(J_inv_J, np.eye(3)) ================================================ FILE: pytransform3d/rotations/test/test_rotations_random.py ================================================ import numpy as np from numpy.testing import assert_array_almost_equal import pytransform3d.rotations as pr def test_random_matrices(): rng = np.random.default_rng(33) mean = pr.random_matrix(rng, np.eye(3), np.eye(3)) cov = np.eye(3) * 1e-10 samples = np.stack([pr.random_matrix(rng, mean, cov) for _ in range(10)]) for sample in samples: assert_array_almost_equal(sample, mean, decimal=4) ================================================ FILE: pytransform3d/rotations/test/test_rotor.py ================================================ import numpy as np import pytest from numpy.testing import assert_array_almost_equal import pytransform3d.rotations as pr def test_check_rotor(): r_list = [1, 0, 0, 0] r = pr.check_rotor(r_list) assert isinstance(r, np.ndarray) r2 = np.array([[1, 0, 0, 0]]) with pytest.raises(ValueError, match="Expected rotor with shape"): pr.check_rotor(r2) r3 = np.array([1, 0, 0]) with pytest.raises(ValueError, match="Expected rotor with shape"): pr.check_rotor(r3) r4 = np.array([2, 0, 0, 0]) assert pytest.approx(np.linalg.norm(pr.check_rotor(r4))) == 1.0 def test_outer(): rng = np.random.default_rng(82) for _ in range(5): a = rng.standard_normal(3) Zero = pr.wedge(a, a) assert_array_almost_equal(Zero, np.zeros(3)) b = rng.standard_normal(3) A = pr.wedge(a, b) B = pr.wedge(b, a) assert_array_almost_equal(A, -B) c = rng.standard_normal(3) assert_array_almost_equal(pr.wedge(a, (b + c)), A + pr.wedge(a, c)) def test_plane_normal_from_bivector(): rng = np.random.default_rng(82) for _ in range(5): a = rng.standard_normal(3) b = rng.standard_normal(3) B = pr.wedge(a, b) n = pr.plane_normal_from_bivector(B) assert_array_almost_equal(n, pr.norm_vector(np.cross(a, b))) def test_geometric_product(): rng = np.random.default_rng(83) for _ in range(5): a = rng.standard_normal(3) a2a = pr.geometric_product(a, 2 * a) assert_array_almost_equal(a2a, np.array([np.dot(a, 2 * a), 0, 0, 0])) b = pr.perpendicular_to_vector(a) ab = pr.geometric_product(a, b) assert_array_almost_equal(ab, np.hstack(((0.0,), pr.wedge(a, b)))) def test_geometric_product_creates_rotor_that_rotates_by_double_angle(): rng = np.random.default_rng(83) for _ in range(5): a_unit = pr.norm_vector(rng.standard_normal(3)) b_unit = pr.norm_vector(rng.standard_normal(3)) # a geometric product of two unit vectors is a rotor ab = pr.geometric_product(a_unit, b_unit) assert pytest.approx(np.linalg.norm(ab)) == 1.0 angle = pr.angle_between_vectors(a_unit, b_unit) c = pr.rotor_apply(ab, a_unit) double_angle = pr.angle_between_vectors(a_unit, c) assert pytest.approx(abs(pr.norm_angle(2.0 * angle))) == abs( pr.norm_angle(double_angle) ) def test_rotor_from_two_directions_special_cases(): d1 = np.array([0, 0, 1]) rotor = pr.rotor_from_two_directions(d1, d1) assert_array_almost_equal(rotor, np.array([1, 0, 0, 0])) rotor = pr.rotor_from_two_directions(d1, np.zeros(3)) assert_array_almost_equal(rotor, np.array([1, 0, 0, 0])) d2 = np.array([0, 0, -1]) # 180 degree rotation rotor = pr.rotor_from_two_directions(d1, d2) assert_array_almost_equal(rotor, np.array([0, 1, 0, 0])) assert_array_almost_equal(pr.rotor_apply(rotor, d1), d2) d3 = np.array([1, 0, 0]) d4 = np.array([-1, 0, 0]) rotor = pr.rotor_from_two_directions(d3, d4) assert_array_almost_equal(pr.rotor_apply(rotor, d3), d4) def test_rotor_from_two_directions(): rng = np.random.default_rng(84) for _ in range(5): a = rng.standard_normal(3) b = rng.standard_normal(3) rotor = pr.rotor_from_two_directions(a, b) b2 = pr.rotor_apply(rotor, a) assert_array_almost_equal(pr.norm_vector(b), pr.norm_vector(b2)) assert_array_almost_equal(np.linalg.norm(a), np.linalg.norm(b2)) def test_rotor_concatenation(): rng = np.random.default_rng(85) for _ in range(5): a = rng.standard_normal(3) b = rng.standard_normal(3) c = rng.standard_normal(3) rotor_ab = pr.rotor_from_two_directions(a, b) rotor_bc = pr.rotor_from_two_directions(b, c) rotor_ac = pr.rotor_from_two_directions(a, c) rotor_ac_cat = pr.concatenate_rotors(rotor_bc, rotor_ab) assert_array_almost_equal( pr.rotor_apply(rotor_ac, a), pr.rotor_apply(rotor_ac_cat, a) ) def test_rotor_times_reverse(): rng = np.random.default_rng(85) for _ in range(5): a = rng.standard_normal(3) b = rng.standard_normal(3) rotor = pr.rotor_from_two_directions(a, b) rotor_reverse = pr.rotor_reverse(rotor) result = pr.concatenate_rotors(rotor, rotor_reverse) assert_array_almost_equal(result, [1, 0, 0, 0]) def test_rotor_from_plane_angle(): rng = np.random.default_rng(87) for _ in range(5): a = rng.standard_normal(3) b = rng.standard_normal(3) B = pr.wedge(a, b) angle = 2 * np.pi * rng.random() axis = np.cross(a, b) rotor = pr.rotor_from_plane_angle(B, angle) q = pr.quaternion_from_axis_angle(np.r_[axis, angle]) v = rng.standard_normal(3) assert_array_almost_equal( pr.rotor_apply(rotor, v), pr.q_prod_vector(q, v) ) def test_matrix_from_rotor(): rng = np.random.default_rng(88) for _ in range(5): a = rng.standard_normal(3) b = rng.standard_normal(3) B = pr.wedge(a, b) angle = 2 * np.pi * rng.random() axis = np.cross(a, b) rotor = pr.rotor_from_plane_angle(B, angle) R_rotor = pr.matrix_from_rotor(rotor) q = pr.quaternion_from_axis_angle(np.r_[axis, angle]) R_q = pr.matrix_from_quaternion(q) assert_array_almost_equal(R_rotor, R_q) def test_negative_rotor(): rng = np.random.default_rng(89) for _ in range(5): a = rng.standard_normal(3) b = rng.standard_normal(3) rotor = pr.rotor_from_two_directions(a, b) neg_rotor = pr.norm_vector(-rotor) a2 = pr.rotor_apply(rotor, a) a3 = pr.rotor_apply(neg_rotor, a) assert_array_almost_equal(a2, a3) ================================================ FILE: pytransform3d/rotations/test/test_slerp.py ================================================ import numpy as np import pytest from numpy.testing import assert_array_almost_equal import pytransform3d.rotations as pr def test_q_R_slerps(): """Compare SLERP implementations for quaternions and rotation matrices.""" rng = np.random.default_rng(833234) for _ in range(20): q_start = pr.random_quaternion(rng) q_end = pr.random_quaternion(rng) R_start, R_end = ( pr.matrix_from_quaternion(q_start), pr.matrix_from_quaternion(q_end), ) t = rng.random() q_t = pr.quaternion_slerp(q_start, q_end, t, shortest_path=True) R_t = pr.matrix_slerp(R_start, R_end, t) assert_array_almost_equal(R_t, pr.matrix_from_quaternion(q_t)) def test_rotation_matrix_power(): """Test power of rotation matrices.""" R = pr.random_matrix(rng=np.random.default_rng(2844)) angle = pr.axis_angle_from_matrix(R)[-1] R_m2 = pr.matrix_power(R, -2.0) assert_array_almost_equal(R_m2, R.T.dot(R.T)) R_m1 = pr.matrix_power(R, -1.0) assert_array_almost_equal(R_m1, R.T) R_m05 = pr.matrix_power(R, -0.5) assert pytest.approx(angle) == 2 * pr.axis_angle_from_matrix(R_m05)[-1] R_0 = pr.matrix_power(R, 0.0) assert_array_almost_equal(R_0, np.eye(3)) R_p05 = pr.matrix_power(R, 0.5) assert pytest.approx(angle) == 2 * pr.axis_angle_from_matrix(R_p05)[-1] R_p1 = pr.matrix_power(R, 1.0) assert_array_almost_equal(R_p1, R) R_p2 = pr.matrix_power(R, 2.0) assert_array_almost_equal(R_p2, R.dot(R)) def test_interpolate_axis_angle(): """Test interpolation between two axis-angle rotations with slerp.""" n_steps = 10 rng = np.random.default_rng(1) a1 = pr.random_axis_angle(rng) a2 = pr.random_axis_angle(rng) traj = [pr.axis_angle_slerp(a1, a2, t) for t in np.linspace(0, 1, n_steps)] axis = pr.norm_vector(pr.perpendicular_to_vectors(a1[:3], a2[:3])) angle = pr.angle_between_vectors(a1[:3], a2[:3]) traj2 = [] for t in np.linspace(0, 1, n_steps): inta = np.hstack((axis, (t * angle,))) intaxis = pr.matrix_from_axis_angle(inta).dot(a1[:3]) intangle = (1 - t) * a1[3] + t * a2[3] traj2.append(np.hstack((intaxis, (intangle,)))) assert_array_almost_equal(traj, traj2) def test_interpolate_same_axis_angle(): """Test interpolation between the same axis-angle rotation. See issue #45. """ n_steps = 3 rng = np.random.default_rng(42) a = pr.random_axis_angle(rng) traj = [pr.axis_angle_slerp(a, a, t) for t in np.linspace(0, 1, n_steps)] assert len(traj) == n_steps assert_array_almost_equal(traj[0], a) assert_array_almost_equal(traj[1], a) assert_array_almost_equal(traj[2], a) def test_interpolate_almost_same_axis_angle(): """Test interpolation between almost the same axis-angle rotation.""" n_steps = 3 rng = np.random.default_rng(42) a1 = pr.random_axis_angle(rng) a2 = np.copy(a1) a2[-1] += np.finfo("float").eps traj = [pr.axis_angle_slerp(a1, a2, t) for t in np.linspace(0, 1, n_steps)] assert len(traj) == n_steps assert_array_almost_equal(traj[0], a1) assert_array_almost_equal(traj[1], a1) assert_array_almost_equal(traj[2], a2) def test_interpolate_quaternion(): """Test interpolation between two quaternions with slerp.""" n_steps = 10 rng = np.random.default_rng(0) a1 = pr.random_axis_angle(rng) a2 = pr.random_axis_angle(rng) q1 = pr.quaternion_from_axis_angle(a1) q2 = pr.quaternion_from_axis_angle(a2) traj_q = [ pr.quaternion_slerp(q1, q2, t) for t in np.linspace(0, 1, n_steps) ] traj_R = [pr.matrix_from_quaternion(q) for q in traj_q] R_diff = np.diff(traj_R, axis=0) R_diff_norms = [np.linalg.norm(Rd) for Rd in R_diff] assert_array_almost_equal( R_diff_norms, R_diff_norms[0] * np.ones(n_steps - 1) ) def test_interpolate_quaternion_shortest_path(): """Test SLERP between similar quternions with opposite sign.""" n_steps = 10 rng = np.random.default_rng(2323) q1 = pr.random_quaternion(rng) a1 = pr.axis_angle_from_quaternion(q1) a2 = np.r_[a1[:3], a1[3] * 1.1] q2 = pr.quaternion_from_axis_angle(a2) if np.sign(q1[0]) != np.sign(q2[0]): q2 *= -1.0 traj_q = [ pr.quaternion_slerp(q1, q2, t) for t in np.linspace(0, 1, n_steps) ] path_length = np.sum( [pr.quaternion_dist(r, s) for r, s in zip(traj_q[:-1], traj_q[1:])] ) q2 *= -1.0 traj_q_opposing = [ pr.quaternion_slerp(q1, q2, t) for t in np.linspace(0, 1, n_steps) ] path_length_opposing = np.sum( [ pr.quaternion_dist(r, s) for r, s in zip(traj_q_opposing[:-1], traj_q_opposing[1:]) ] ) assert path_length_opposing > path_length traj_q_opposing_corrected = [ pr.quaternion_slerp(q1, q2, t, shortest_path=True) for t in np.linspace(0, 1, n_steps) ] path_length_opposing_corrected = np.sum( [ pr.quaternion_dist(r, s) for r, s in zip( traj_q_opposing_corrected[:-1], traj_q_opposing_corrected[1:] ) ] ) assert pytest.approx(path_length_opposing_corrected) == path_length def test_interpolate_same_quaternion(): """Test interpolation between the same quaternion rotation. See issue #45. """ n_steps = 3 rng = np.random.default_rng(42) a = pr.random_axis_angle(rng) q = pr.quaternion_from_axis_angle(a) traj = [pr.quaternion_slerp(q, q, t) for t in np.linspace(0, 1, n_steps)] assert len(traj) == n_steps assert_array_almost_equal(traj[0], q) assert_array_almost_equal(traj[1], q) assert_array_almost_equal(traj[2], q) def test_interpolate_shortest_path_same_quaternion(): """Test interpolate along shortest path with same quaternion.""" rng = np.random.default_rng(8353) q = pr.random_quaternion(rng) q_interpolated = pr.quaternion_slerp(q, q, 0.5, shortest_path=True) assert_array_almost_equal(q, q_interpolated) q = np.array([0.0, 1.0, 0.0, 0.0]) q_interpolated = pr.quaternion_slerp(q, -q, 0.5, shortest_path=True) assert_array_almost_equal(q, q_interpolated) q = np.array([0.0, 0.0, 1.0, 0.0]) q_interpolated = pr.quaternion_slerp(q, -q, 0.5, shortest_path=True) assert_array_almost_equal(q, q_interpolated) q = np.array([0.0, 0.0, 0.0, 1.0]) q_interpolated = pr.quaternion_slerp(q, -q, 0.5, shortest_path=True) assert_array_almost_equal(q, q_interpolated) def test_rotor_slerp(): rng = np.random.default_rng(86) for _ in range(5): a_unit = pr.norm_vector(rng.standard_normal(3)) b_unit = pr.norm_vector(rng.standard_normal(3)) rotor1 = pr.rotor_from_two_directions(a_unit, b_unit) axis = pr.norm_vector(np.cross(a_unit, b_unit)) angle = pr.angle_between_vectors(a_unit, b_unit) q1 = pr.quaternion_from_axis_angle(np.r_[axis, angle]) c_unit = pr.norm_vector(rng.standard_normal(3)) d_unit = pr.norm_vector(rng.standard_normal(3)) rotor2 = pr.rotor_from_two_directions(c_unit, d_unit) axis = pr.norm_vector(np.cross(c_unit, d_unit)) angle = pr.angle_between_vectors(c_unit, d_unit) q2 = pr.quaternion_from_axis_angle(np.r_[axis, angle]) rotor_025 = pr.rotor_slerp(rotor1, rotor2, 0.25) q_025 = pr.quaternion_slerp(q1, q2, 0.25) e = rng.standard_normal(3) assert_array_almost_equal( pr.rotor_apply(rotor_025, e), pr.q_prod_vector(q_025, e) ) rotor_075 = pr.rotor_slerp(rotor1, rotor2, 0.25) q_075 = pr.quaternion_slerp(q1, q2, 0.25) e = rng.standard_normal(3) assert_array_almost_equal( pr.rotor_apply(rotor_075, e), pr.q_prod_vector(q_075, e) ) ================================================ FILE: pytransform3d/rotations/test/test_utils.py ================================================ import warnings import numpy as np import pytest from numpy.testing import assert_array_almost_equal import pytransform3d.rotations as pr def test_norm_vector(): """Test normalization of vectors.""" rng = np.random.default_rng(0) for n in range(1, 6): v = pr.random_vector(rng, n) u = pr.norm_vector(v) assert pytest.approx(np.linalg.norm(u)) == 1 def test_norm_zero_vector(): """Test normalization of zero vector.""" normalized = pr.norm_vector(np.zeros(3)) assert np.isfinite(np.linalg.norm(normalized)) def test_perpendicular_to_vectors(): """Test function to compute perpendicular to vectors.""" rng = np.random.default_rng(0) a = pr.norm_vector(pr.random_vector(rng)) a1 = pr.norm_vector(pr.random_vector(rng)) b = pr.norm_vector(pr.perpendicular_to_vectors(a, a1)) c = pr.norm_vector(pr.perpendicular_to_vectors(a, b)) assert pytest.approx(pr.angle_between_vectors(a, b)) == np.pi / 2.0 assert pytest.approx(pr.angle_between_vectors(a, c)) == np.pi / 2.0 assert pytest.approx(pr.angle_between_vectors(b, c)) == np.pi / 2.0 assert_array_almost_equal(pr.perpendicular_to_vectors(b, c), a) assert_array_almost_equal(pr.perpendicular_to_vectors(c, a), b) def test_perpendicular_to_vector(): """Test function to compute perpendicular to vector.""" assert ( pytest.approx( pr.angle_between_vectors( pr.unitx, pr.perpendicular_to_vector(pr.unitx) ) ) == np.pi / 2.0 ) assert ( pytest.approx( pr.angle_between_vectors( pr.unity, pr.perpendicular_to_vector(pr.unity) ) ) == np.pi / 2.0 ) assert ( pytest.approx( pr.angle_between_vectors( pr.unitz, pr.perpendicular_to_vector(pr.unitz) ) ) == np.pi / 2.0 ) rng = np.random.default_rng(0) for _ in range(5): a = pr.norm_vector(pr.random_vector(rng)) assert ( pytest.approx( pr.angle_between_vectors(a, pr.perpendicular_to_vector(a)) ) == np.pi / 2.0 ) b = a - np.array([a[0], 0.0, 0.0]) assert ( pytest.approx( pr.angle_between_vectors(b, pr.perpendicular_to_vector(b)) ) == np.pi / 2.0 ) c = a - np.array([0.0, a[1], 0.0]) assert ( pytest.approx( pr.angle_between_vectors(c, pr.perpendicular_to_vector(c)) ) == np.pi / 2.0 ) d = a - np.array([0.0, 0.0, a[2]]) assert ( pytest.approx( pr.angle_between_vectors(d, pr.perpendicular_to_vector(d)) ) == np.pi / 2.0 ) def test_angle_between_vectors(): """Test function to compute angle between two vectors.""" v = np.array([1, 0, 0]) a = np.array([0, 1, 0, np.pi / 2]) R = pr.matrix_from_axis_angle(a) vR = np.dot(R, v) assert pytest.approx(pr.angle_between_vectors(vR, v)) == a[-1] v = np.array([0, 1, 0]) a = np.array([1, 0, 0, np.pi / 2]) R = pr.matrix_from_axis_angle(a) vR = np.dot(R, v) assert pytest.approx(pr.angle_between_vectors(vR, v)) == a[-1] v = np.array([0, 0, 1]) a = np.array([1, 0, 0, np.pi / 2]) R = pr.matrix_from_axis_angle(a) vR = np.dot(R, v) assert pytest.approx(pr.angle_between_vectors(vR, v)) == a[-1] def test_angle_between_close_vectors(): """Test angle between close vectors. See issue #47. """ a = np.array([0.9689124217106448, 0.24740395925452294, 0.0, 0.0]) b = np.array([0.9689124217106448, 0.247403959254523, 0.0, 0.0]) angle = pr.angle_between_vectors(a, b) assert pytest.approx(angle) == 0.0 def test_angle_to_zero_vector_is_nan(): """Test angle to zero vector.""" a = np.array([1.0, 0.0]) b = np.array([0.0, 0.0]) with warnings.catch_warnings(record=True) as w: angle = pr.angle_between_vectors(a, b) assert len(w) == 1 assert np.isnan(angle) def test_vector_projection_on_zero_vector(): """Test projection on zero vector.""" rng = np.random.default_rng(23) for _ in range(5): a = pr.random_vector(rng, 3) a_on_b = pr.vector_projection(a, np.zeros(3)) assert_array_almost_equal(a_on_b, np.zeros(3)) def test_vector_projection(): """Test orthogonal projection of one vector to another vector.""" a = np.ones(3) a_on_unitx = pr.vector_projection(a, pr.unitx) assert_array_almost_equal(a_on_unitx, pr.unitx) assert pytest.approx(pr.angle_between_vectors(a_on_unitx, pr.unitx)) == 0.0 a2_on_unitx = pr.vector_projection(2 * a, pr.unitx) assert_array_almost_equal(a2_on_unitx, 2 * pr.unitx) assert pytest.approx(pr.angle_between_vectors(a2_on_unitx, pr.unitx)) == 0.0 a_on_unity = pr.vector_projection(a, pr.unity) assert_array_almost_equal(a_on_unity, pr.unity) assert pytest.approx(pr.angle_between_vectors(a_on_unity, pr.unity)) == 0.0 minus_a_on_unity = pr.vector_projection(-a, pr.unity) assert_array_almost_equal(minus_a_on_unity, -pr.unity) assert ( pytest.approx(pr.angle_between_vectors(minus_a_on_unity, pr.unity)) == np.pi ) a_on_unitz = pr.vector_projection(a, pr.unitz) assert_array_almost_equal(a_on_unitz, pr.unitz) assert pytest.approx(pr.angle_between_vectors(a_on_unitz, pr.unitz)) == 0.0 unitz_on_a = pr.vector_projection(pr.unitz, a) assert_array_almost_equal(unitz_on_a, np.ones(3) / 3.0) assert pytest.approx(pr.angle_between_vectors(unitz_on_a, a)) == 0.0 unitx_on_unitx = pr.vector_projection(pr.unitx, pr.unitx) assert_array_almost_equal(unitx_on_unitx, pr.unitx) assert ( pytest.approx(pr.angle_between_vectors(unitx_on_unitx, pr.unitx)) == 0.0 ) def test_plane_basis_from_normal(): x, y = pr.plane_basis_from_normal(pr.unitx) R = np.column_stack((x, y, pr.unitx)) pr.assert_rotation_matrix(R) x, y = pr.plane_basis_from_normal(pr.unity) R = np.column_stack((x, y, pr.unity)) pr.assert_rotation_matrix(R) x, y = pr.plane_basis_from_normal(pr.unitz) R = np.column_stack((x, y, pr.unitz)) pr.assert_rotation_matrix(R) rng = np.random.default_rng(25) for _ in range(5): normal = pr.norm_vector(rng.standard_normal(3)) x, y = pr.plane_basis_from_normal(normal) R = np.column_stack((x, y, normal)) pr.assert_rotation_matrix(R) ================================================ FILE: pytransform3d/test/test_camera.py ================================================ import numpy as np import pytest from numpy.testing import assert_array_almost_equal from pytransform3d.camera import ( make_world_line, make_world_grid, cam2sensor, sensor2img, world2image, ) from pytransform3d.rotations import active_matrix_from_intrinsic_euler_xyz from pytransform3d.transformations import transform_from def test_make_world_line(): line = make_world_line(np.array([0, 0, 0]), np.array([1, 1, 1]), 11) assert len(line) == 11 assert np.array([0, 0, 0, 1]) in line assert np.array([0.5, 0.5, 0.5, 1]) in line assert np.array([1, 1, 1, 1]) in line def test_make_world_grid(): grid = make_world_grid(3, 3, xlim=(-1, 1), ylim=(-1, 1)) assert np.array([-1, -1, 0, 1]) in grid assert np.array([-1, 0, 0, 1]) in grid assert np.array([-1, 1, 0, 1]) in grid assert np.array([0, -1, 0, 1]) in grid assert np.array([0, 0, 0, 1]) in grid assert np.array([0, 1, 0, 1]) in grid assert np.array([1, -1, 0, 1]) in grid assert np.array([1, 0, 0, 1]) in grid assert np.array([1, 1, 0, 1]) in grid def test_cam2sensor_wrong_dimensions(): P_cam = np.ones((1, 2)) with pytest.raises(ValueError, match="3- or 4-dimensional points"): cam2sensor(P_cam, 0.1) P_cam = np.ones((1, 5)) with pytest.raises(ValueError, match="3- or 4-dimensional points"): cam2sensor(P_cam, 0.1) def test_cam2sensor_wrong_focal_length(): P_cam = np.ones((1, 3)) with pytest.raises(ValueError, match="must be greater than 0"): cam2sensor(P_cam, 0.0) def test_cam2sensor_points_behind_camera(): P_cam = np.ones((1, 3)) P_cam[0, 2] = -1.0 P_sensor = cam2sensor(P_cam, 0.1) assert not np.any(np.isfinite(P_sensor)) def test_cam2sensor_projection(): P_cam = np.array([[-0.56776587, 0.03855521, 0.81618344, 1.0]]) P_sensor = cam2sensor(P_cam, 0.0036) assert_array_almost_equal(P_sensor, np.array([[-0.00250429, 0.00017006]])) def test_sensor2img(): P_sensor = np.array( [[-0.00367 / 2, -0.00274 / 2], [0.0, 0.0], [0.00367 / 2, 0.00274 / 2]] ) P_image = sensor2img(P_sensor, (0.00367, 0.00274), (640, 480)) assert_array_almost_equal( P_image, np.array([[0, 0], [320, 240], [640, 480]]) ) P_sensor = np.array([[0.0, 0.0], [0.00367, 0.00274]]) P_image = sensor2img(P_sensor, (0.00367, 0.00274), (640, 480), (0, 0)) assert_array_almost_equal(P_image, np.array([[0, 0], [640, 480]])) def test_world2image(): cam2world = transform_from( active_matrix_from_intrinsic_euler_xyz([np.pi, 0, 0]), [0, 0, 1.5] ) focal_length = 0.0036 sensor_size = (0.00367, 0.00274) image_size = (640, 480) world_grid = make_world_grid() image_grid = world2image( world_grid, cam2world, sensor_size, image_size, focal_length ) expected_grid = make_world_grid( xlim=(110.73569482, 529.26430518), ylim=(450.2189781, 29.7810219) ) assert_array_almost_equal(image_grid, expected_grid[:, :2]) ================================================ FILE: pytransform3d/test/test_coordinates.py ================================================ import numpy as np from numpy.testing import assert_array_almost_equal import pytransform3d.coordinates as pc def test_cylindrical_from_cartesian_edge_cases(): q = pc.cylindrical_from_cartesian(np.zeros(3)) assert_array_almost_equal(q, np.zeros(3)) def test_cartesian_from_cylindrical_edge_cases(): q = pc.cartesian_from_cylindrical(np.zeros(3)) assert_array_almost_equal(q, np.zeros(3)) def test_convert_cartesian_cylindrical(): rng = np.random.default_rng(0) for _ in range(1000): rho = rng.standard_normal() phi = rng.random() * 4 * np.pi - 2 * np.pi z = rng.standard_normal() p = np.array([rho, phi, z]) q = pc.cartesian_from_cylindrical(p) r = pc.cylindrical_from_cartesian(q) assert r[0] >= 0 assert -np.pi <= r[1] <= np.pi s = pc.cartesian_from_cylindrical(r) assert_array_almost_equal(q, s) def test_spherical_from_cartesian_edge_cases(): q = pc.spherical_from_cartesian(np.zeros(3)) assert_array_almost_equal(q, np.zeros(3)) def test_cartesian_from_spherical_edge_cases(): q = pc.cartesian_from_spherical(np.zeros(3)) assert_array_almost_equal(q, np.zeros(3)) def test_convert_cartesian_spherical(): rng = np.random.default_rng(1) for _ in range(1000): rho = rng.standard_normal() theta = rng.random() * 4 * np.pi - 2 * np.pi phi = rng.random() * 4 * np.pi - 2 * np.pi p = np.array([rho, theta, phi]) q = pc.cartesian_from_spherical(p) r = pc.spherical_from_cartesian(q) assert r[0] >= 0 assert 0 <= r[1] <= np.pi assert -np.pi <= r[2] <= np.pi s = pc.cartesian_from_spherical(r) assert_array_almost_equal(q, s) def test_spherical_from_cylindrical_edge_cases(): q = pc.spherical_from_cylindrical(np.zeros(3)) assert_array_almost_equal(q, np.zeros(3)) def test_cylindrical_from_spherical_edge_cases(): q = pc.cylindrical_from_spherical(np.zeros(3)) assert_array_almost_equal(q, np.zeros(3)) def test_convert_cylindrical_spherical(): rng = np.random.default_rng(2) for _ in range(1000): rho = rng.standard_normal() theta = rng.random() * 4 * np.pi - 2 * np.pi phi = rng.random() * 4 * np.pi - 2 * np.pi p = np.array([rho, theta, phi]) q = pc.cylindrical_from_spherical(p) r = pc.spherical_from_cylindrical(q) s = pc.cylindrical_from_spherical(r) assert_array_almost_equal(q, s) def test_integer_inputs(): assert_array_almost_equal( pc.spherical_from_cylindrical([1, 0, 0]), pc.spherical_from_cylindrical([1.0, 0.0, 0.0]), ) assert_array_almost_equal( pc.spherical_from_cartesian([1, 0, 0]), pc.spherical_from_cartesian([1.0, 0.0, 0.0]), ) assert_array_almost_equal( pc.cylindrical_from_cartesian([0, 1, 0]), pc.cylindrical_from_cartesian([0.0, 1.0, 0.0]), ) assert_array_almost_equal( pc.cartesian_from_cylindrical([1, 1, 0]), pc.cartesian_from_cylindrical([1.0, 1.0, 0.0]), ) assert_array_almost_equal( pc.cartesian_from_spherical([1, 1, 0]), pc.cartesian_from_spherical([1.0, 1.0, 0.0]), ) assert_array_almost_equal( pc.cylindrical_from_spherical([1, 1, 0]), pc.cylindrical_from_spherical([1.0, 1.0, 0.0]), ) ================================================ FILE: pytransform3d/test/test_geometry.py ================================================ import numpy as np from numpy.testing import assert_array_equal, assert_array_almost_equal import pytransform3d._geometry as pg import pytransform3d.transformations as pt def test_unit_sphere(): x, y, z = pg.unit_sphere_surface_grid(10) assert_array_equal(x.shape, (10, 10)) assert_array_equal(y.shape, (10, 10)) assert_array_equal(z.shape, (10, 10)) P = np.column_stack((x.reshape(-1), y.reshape(-1), z.reshape(-1))) norms = np.linalg.norm(P, axis=1) assert_array_almost_equal(norms, np.ones_like(norms)) def test_transform_surface(): x, y, z = pg.unit_sphere_surface_grid(10) p = np.array([0.2, -0.5, 0.7]) pose = pt.transform_from(R=np.eye(3), p=p) x, y, z = pg.transform_surface(pose, x, y, z) P = np.column_stack((x.reshape(-1), y.reshape(-1), z.reshape(-1))) norms = np.linalg.norm(P - p[np.newaxis], axis=1) assert_array_almost_equal(norms, np.ones_like(norms)) ================================================ FILE: pytransform3d/test/test_mesh_loader.py ================================================ import numpy as np from pytransform3d import _mesh_loader import pytest def test_trimesh(): mesh = _mesh_loader._Trimesh("test/test_data/cone.stl") loader_available = mesh.load() if not loader_available: pytest.skip("trimesh is required for this test") assert len(mesh.vertices) == 64 assert len(mesh.triangles) == 124 mesh.convex_hull() assert len(mesh.vertices) == 64 def test_trimesh_scene(): mesh = _mesh_loader._Trimesh("test/test_data/scene.obj") try: mesh_loaded = mesh.load() except ImportError as e: if e.name == "open3d": pytest.skip("open3d is required for this test") else: raise e if not mesh_loaded: pytest.skip("trimesh is required for this test") assert mesh_loaded assert len(mesh.vertices) == 16 assert len(mesh.triangles) == 24 mesh.convex_hull() assert len(mesh.vertices) == 8 def test_open3d(): mesh = _mesh_loader._Open3DMesh("test/test_data/cone.stl") loader_available = mesh.load() if not loader_available: pytest.skip("open3d is required for this test") assert len(mesh.vertices) == 295 assert len(mesh.triangles) == 124 o3d_mesh = mesh.get_open3d_mesh() assert len(o3d_mesh.vertices) == 295 mesh.convex_hull() assert len(mesh.vertices) == 64 def test_trimesh_with_open3d(): mesh = _mesh_loader._Trimesh("test/test_data/cone.stl") loader_available = mesh.load() if not loader_available: pytest.skip("trimesh is required for this test") try: o3d_mesh = mesh.get_open3d_mesh() except ImportError: pytest.skip("open3d is required for this test") assert len(o3d_mesh.vertices) == 64 def test_interface(): try: mesh = _mesh_loader.load_mesh("test/test_data/cone.stl") assert len(mesh.triangles) == 124 except ImportError as e: if e.name in ["open3d", "trimesh"]: pytest.skip("trimesh or open3d are required for this test") else: raise e def test_ply_with_color(): try: mesh = _mesh_loader.load_mesh("test/test_data/frame.ply") vertex_colors = np.asarray(mesh.get_open3d_mesh().vertex_colors) except ImportError as e: if e.name in ["open3d", "trimesh"]: pytest.skip("trimesh and open3d are required for this test") else: raise e assert len(vertex_colors) == 1134 def test_interface_with_scene(): try: mesh = _mesh_loader.load_mesh("test/test_data/scene.obj") assert len(mesh.triangles) == 24 except ImportError as e: if e.name in ["open3d", "trimesh"]: pytest.skip("trimesh and open3d are required for this test") else: raise e ================================================ FILE: pytransform3d/test/test_urdf.py ================================================ try: import matplotlib # noqa: F401 matplotlib_available = True except ImportError: matplotlib_available = False import json import os import warnings import numpy as np import pytest from numpy.testing import assert_array_almost_equal from pytransform3d.transformations import transform_from from pytransform3d.urdf import ( UrdfTransformManager, UrdfException, parse_urdf, initialize_urdf_transform_manager, ) COMPI_URDF = """ transmission_interface/SimpleTransmission PositionJointInterface 1 """ def test_empty(): with pytest.raises(UrdfException): UrdfTransformManager().load_urdf("") def test_missing_robot_tag(): with pytest.raises(UrdfException): UrdfTransformManager().load_urdf("") def test_missing_robot_name(): with pytest.raises(UrdfException): UrdfTransformManager().load_urdf("") def test_missing_link_name(): with pytest.raises(UrdfException): UrdfTransformManager().load_urdf( '' ) def test_missing_joint_name(): urdf = """ """ with pytest.raises(UrdfException): UrdfTransformManager().load_urdf(urdf) def test_missing_parent(): urdf = """ """ with pytest.raises(UrdfException): UrdfTransformManager().load_urdf(urdf) def test_missing_child(): urdf = """ """ with pytest.raises(UrdfException): UrdfTransformManager().load_urdf(urdf) def test_missing_parent_link_name(): urdf = """ """ with pytest.raises(UrdfException): UrdfTransformManager().load_urdf(urdf) def test_missing_child_link_name(): urdf = """ """ with pytest.raises(UrdfException): UrdfTransformManager().load_urdf(urdf) def test_xml_namespace(): urdf = """ """ robot_name, links, joints = parse_urdf(urdf) assert robot_name == "robot_name" assert len(links) == 2 assert len(joints) == 1 for link in links: assert link.name in ["link0", "link1"] assert joints[0].joint_name == "joint0" def test_reference_to_unknown_child(): urdf = """ """ with pytest.raises(UrdfException): UrdfTransformManager().load_urdf(urdf) def test_reference_to_unknown_parent(): urdf = """ """ with pytest.raises(UrdfException): UrdfTransformManager().load_urdf(urdf) def test_missing_joint_type(): urdf = """ """ with pytest.raises(UrdfException): UrdfTransformManager().load_urdf(urdf) def test_without_origin(): urdf = """ """ tm = UrdfTransformManager() tm.load_urdf(urdf) link1_to_link0 = tm.get_transform("link1", "link0") assert_array_almost_equal(link1_to_link0, np.eye(4)) def test_with_empty_origin(): urdf = """ """ tm = UrdfTransformManager() tm.load_urdf(urdf) link1_to_link0 = tm.get_transform("link1", "link0") assert_array_almost_equal(link1_to_link0, np.eye(4)) def test_unsupported_joint_type(): urdf = """ """ with pytest.raises(UrdfException): UrdfTransformManager().load_urdf(urdf) def test_unknown_joint_type(): urdf = """ """ with pytest.raises(UrdfException): UrdfTransformManager().load_urdf(urdf) def test_with_empty_axis(): urdf = """ """ tm = UrdfTransformManager() tm.load_urdf(urdf) tm.set_joint("joint0", np.pi) link1_to_link0 = tm.get_transform("link1", "link0") assert_array_almost_equal( link1_to_link0, np.array([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]), ) def test_ee_frame(): tm = UrdfTransformManager() tm.load_urdf(COMPI_URDF) link7_to_linkmount = tm.get_transform("link6", "linkmount") assert_array_almost_equal( link7_to_linkmount, np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1.124], [0, 0, 0, 1]]), ) def test_joint_angles(): tm = UrdfTransformManager() tm.load_urdf(COMPI_URDF) for i in range(1, 7): tm.set_joint("joint%d" % i, 0.1 * i) link7_to_linkmount = tm.get_transform("link6", "linkmount") assert_array_almost_equal( link7_to_linkmount, np.array( [ [0.121698, -0.606672, 0.785582, 0.489351], [0.818364, 0.509198, 0.266455, 0.114021], [-0.561668, 0.610465, 0.558446, 0.924019], [0.0, 0.0, 0.0, 1.0], ] ), ) def test_joint_limits(): tm = UrdfTransformManager() tm.load_urdf(COMPI_URDF) with pytest.raises(KeyError): tm.get_joint_limits("unknown_joint") assert_array_almost_equal(tm.get_joint_limits("joint1"), (-1, 1)) assert_array_almost_equal(tm.get_joint_limits("joint2"), (-1, np.inf)) assert_array_almost_equal(tm.get_joint_limits("joint3"), (-np.inf, 1)) def test_joint_limit_clipping(): tm = UrdfTransformManager() tm.load_urdf(COMPI_URDF) tm.set_joint("joint1", 2.0) link7_to_linkmount = tm.get_transform("link6", "linkmount") assert_array_almost_equal( link7_to_linkmount, np.array( [ [0.5403023, -0.8414710, 0, 0], [0.8414710, 0.5403023, 0, 0], [0, 0, 1, 1.124], [0, 0, 0, 1], ] ), ) tm.set_joint("joint1", -2.0) link7_to_linkmount = tm.get_transform("link6", "linkmount") assert_array_almost_equal( link7_to_linkmount, np.array( [ [0.5403023, 0.8414710, 0, 0], [-0.8414710, 0.5403023, 0, 0], [0, 0, 1, 1.124], [0, 0, 0, 1], ] ), ) def test_fixed_joint(): tm = UrdfTransformManager() tm.load_urdf(COMPI_URDF) tcp_to_link0 = tm.get_transform("tcp", "linkmount") assert_array_almost_equal( tcp_to_link0, np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1.174], [0, 0, 0, 1]]), ) def test_fixed_joint_unchanged_and_warning(): tm = UrdfTransformManager() tm.load_urdf(COMPI_URDF) tcp_to_link0_before = tm.get_transform("tcp", "linkmount") with warnings.catch_warnings(record=True) as w: tm.set_joint("jointtcp", 2.0) assert len(w) == 1 tcp_to_link0_after = tm.get_transform("tcp", "linkmount") assert_array_almost_equal(tcp_to_link0_before, tcp_to_link0_after) def test_unknown_joint(): tm = UrdfTransformManager() tm.load_urdf(COMPI_URDF) with pytest.raises(KeyError): tm.set_joint("unknown_joint", 0) def test_visual_without_geometry(): urdf = """ """ with pytest.raises(UrdfException): UrdfTransformManager().load_urdf(urdf) def test_visual(): urdf = """ """ tm = UrdfTransformManager() tm.load_urdf(urdf) link1_to_link0 = tm.get_transform("link1", "link0") expected_link1_to_link0 = transform_from(np.eye(3), np.array([0, 1, 0])) assert_array_almost_equal(link1_to_link0, expected_link1_to_link0) link1_to_link0 = tm.get_transform("visual:link1/link1_visual", "link0") expected_link1_to_link0 = transform_from(np.eye(3), np.array([0, 1, 1])) assert_array_almost_equal(link1_to_link0, expected_link1_to_link0) def test_collision(): urdf = """ """ tm = UrdfTransformManager() tm.load_urdf(urdf) link1_to_link0 = tm.get_transform("link1", "link0") expected_link1_to_link0 = transform_from(np.eye(3), np.array([0, 0, 1])) assert_array_almost_equal(link1_to_link0, expected_link1_to_link0) link1_to_link0 = tm.get_transform("collision:link1/0", "link0") expected_link1_to_link0 = transform_from(np.eye(3), np.array([0, 0, 2])) assert_array_almost_equal(link1_to_link0, expected_link1_to_link0) def test_unique_frame_names(): urdf = """ """ tm = UrdfTransformManager() tm.load_urdf(urdf) assert "robot_name" in tm.nodes assert "link0" in tm.nodes assert "link1" in tm.nodes assert "visual:link1/link1_geometry" in tm.nodes assert "collision:link1/link1_geometry" in tm.nodes assert len(tm.nodes) == 5 def test_collision_box(): urdf = """ """ tm = UrdfTransformManager() tm.load_urdf(urdf) assert len(tm.collision_objects) == 1 assert_array_almost_equal(tm.collision_objects[0].size, np.array([2, 3, 4])) def test_collision_box_without_size(): urdf = """ """ tm = UrdfTransformManager() tm.load_urdf(urdf) assert len(tm.collision_objects) == 1 assert_array_almost_equal(tm.collision_objects[0].size, np.zeros(3)) def test_collision_sphere(): urdf = """ """ tm = UrdfTransformManager() tm.load_urdf(urdf) assert len(tm.collision_objects) == 1 assert tm.collision_objects[0].radius == 0.123 def test_collision_sphere_without_radius(): urdf = """ """ with pytest.raises(UrdfException): UrdfTransformManager().load_urdf(urdf) def test_collision_cylinder(): urdf = """ """ tm = UrdfTransformManager() tm.load_urdf(urdf) assert len(tm.collision_objects) == 1 assert tm.collision_objects[0].radius == 0.123 assert tm.collision_objects[0].length == 1.234 def test_collision_cylinder_without_radius(): urdf = """ """ with pytest.raises(UrdfException): UrdfTransformManager().load_urdf(urdf) def test_collision_cylinder_without_length(): urdf = """ """ with pytest.raises(UrdfException): UrdfTransformManager().load_urdf(urdf) def test_multiple_collision_objects(): urdf = """ """ tm = UrdfTransformManager() tm.load_urdf(urdf) assert len(tm.collision_objects) == 2 def test_multiple_visuals(): urdf = """ """ tm = UrdfTransformManager() tm.load_urdf(urdf) assert len(tm.visuals) == 2 def test_multiple_parents(): urdf = """ """ tm = UrdfTransformManager() tm.load_urdf(urdf) p0c = tm.get_transform("parent0", "child") p1c = tm.get_transform("parent1", "child") assert p0c[0, 3] == p1c[1, 3] def test_mesh_missing_filename(): urdf = """ """ with pytest.raises(UrdfException): UrdfTransformManager().load_urdf(urdf, mesh_path="") def test_plot_mesh_smoke_without_scale(): if not matplotlib_available: pytest.skip("matplotlib is required for this test") urdf = """ """ BASE_DIR = "test/test_data/" tm = UrdfTransformManager() tm.load_urdf(urdf, mesh_path=BASE_DIR) tm.set_joint("joint", -1.1) ax = tm.plot_frames_in( "lower_cone", s=0.1, whitelist=["upper_cone", "lower_cone"], show_name=True, ) ax = tm.plot_connections_in("lower_cone", ax=ax) tm.plot_visuals("lower_cone", ax=ax) def test_continuous_joint(): urdf = """ """ tm = UrdfTransformManager() tm.load_urdf(urdf) tm.set_joint("joint", 0.5 * np.pi) c2p = tm.get_transform("child", "parent") assert_array_almost_equal( c2p, np.array([[1, 0, 0, 0], [0, 0, -1, 0], [0, 1, 0, 0], [0, 0, 0, 1]]) ) def test_prismatic_joint(): urdf = """ """ tm = UrdfTransformManager() tm.load_urdf(urdf) tm.set_joint("joint", 5.33) c2p = tm.get_transform("child", "parent") assert_array_almost_equal( c2p, np.array([[1, 0, 0, 5.33], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]), ) def test_plot_mesh_smoke_with_scale(): if not matplotlib_available: pytest.skip("matplotlib is required for this test") BASE_DIR = "test/test_data/" tm = UrdfTransformManager() with open(BASE_DIR + "simple_mechanism.urdf", "r") as f: tm.load_urdf(f.read(), mesh_path=BASE_DIR) tm.set_joint("joint", -1.1) ax = tm.plot_frames_in( "lower_cone", s=0.1, whitelist=["upper_cone", "lower_cone"], show_name=True, ) ax = tm.plot_connections_in("lower_cone", ax=ax) tm.plot_visuals("lower_cone", ax=ax) def test_plot_without_mesh(): if not matplotlib_available: pytest.skip("matplotlib is required for this test") BASE_DIR = "test/test_data/" tm = UrdfTransformManager() with open(BASE_DIR + "simple_mechanism.urdf", "r") as f: tm.load_urdf(f.read(), mesh_path=None) tm.set_joint("joint", -1.1) ax = tm.plot_frames_in( "lower_cone", s=0.1, whitelist=["upper_cone", "lower_cone"], show_name=True, ) ax = tm.plot_connections_in("lower_cone", ax=ax) with warnings.catch_warnings(record=True) as w: tm.plot_visuals("lower_cone", ax=ax) assert len(w) == 1 def test_parse_material(): urdf = """ """ tm = UrdfTransformManager() tm.load_urdf(urdf) assert_array_almost_equal(tm.visuals[0].color, np.array([0, 0, 0, 1])) def test_parse_material_without_name(): urdf = """ """ with pytest.raises(UrdfException): UrdfTransformManager().load_urdf(urdf) def test_parse_material_without_color(): urdf = """ """ tm = UrdfTransformManager() tm.load_urdf(urdf) assert tm.visuals[0].color is None def test_parse_material_without_rgba(): urdf = """ """ with pytest.raises(UrdfException): UrdfTransformManager().load_urdf(urdf) def test_parse_material_with_two_colors(): urdf = """ """ with pytest.raises(UrdfException): UrdfTransformManager().load_urdf(urdf) def test_parse_material_local(): urdf = """ """ tm = UrdfTransformManager() tm.load_urdf(urdf) assert_array_almost_equal(tm.visuals[0].color, np.array([1, 0, 0, 1])) def test_plot_mesh_smoke_with_package_dir(): if not matplotlib_available: pytest.skip("matplotlib is required for this test") urdf = """ """ tm = UrdfTransformManager() tm.load_urdf(urdf, package_dir="./") tm.plot_visuals("cone") def test_load_inertial_info(): urdf = """ """ _, links, _ = parse_urdf(urdf) assert len(links) == 1 assert links[0].name == "cone" assert_array_almost_equal(links[0].inertial_frame, np.eye(4)) assert links[0].mass == 0.001 assert_array_almost_equal( links[0].inertia, np.array([[1, 4, 5], [4, 2, 6], [5, 6, 3]]) ) def test_load_inertial_info_sparse_matrix(): urdf = """ """ _, links, _ = parse_urdf(urdf) assert len(links) == 1 assert links[0].name == "cone" assert_array_almost_equal(links[0].inertial_frame, np.eye(4)) assert links[0].mass == 0.001 assert_array_almost_equal(links[0].inertia, np.zeros((3, 3))) def test_load_inertial_info_diagonal_matrix(): urdf = """ """ _, links, _ = parse_urdf(urdf) assert len(links) == 1 assert links[0].name == "cone" assert_array_almost_equal(links[0].inertial_frame, np.eye(4)) assert links[0].mass == 0.001 assert_array_almost_equal(links[0].inertia, np.eye(3)) def test_load_inertial_info_without_matrix(): urdf = """ """ _, links, _ = parse_urdf(urdf) assert len(links) == 1 assert links[0].name == "cone" assert_array_almost_equal(links[0].inertial_frame, np.eye(4)) assert links[0].mass == 0.001 assert_array_almost_equal(links[0].inertia, np.zeros((3, 3))) def test_load_inertial_info_without_mass(): urdf = """ """ _, links, _ = parse_urdf(urdf) assert len(links) == 1 assert links[0].name == "cone" assert_array_almost_equal(links[0].inertial_frame, np.eye(4)) assert links[0].mass == 0.0 assert_array_almost_equal(links[0].inertia, np.zeros((3, 3))) def test_load_urdf_functional(): robot_name, links, joints = parse_urdf(COMPI_URDF) assert robot_name == "compi" assert len(links) == 8 assert len(joints) == 7 tm = UrdfTransformManager() initialize_urdf_transform_manager(tm, robot_name, links, joints) link7_to_linkmount = tm.get_transform("link6", "linkmount") assert_array_almost_equal( link7_to_linkmount, np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1.124], [0, 0, 0, 1]]), ) def test_serialization(): tm = UrdfTransformManager() tm.load_urdf(COMPI_URDF) for i in range(1, 7): tm.set_joint("joint%d" % i, 0.1) filename = "compi.json" try: tm_dict = tm.to_dict() with open(filename, "w") as f: json.dump(tm_dict, f) with open(filename, "r") as f: tm_dict2 = json.load(f) tm2 = UrdfTransformManager.from_dict(tm_dict2) tcp2mount = tm.get_transform("tcp", "linkmount") tcp2mount2 = tm2.get_transform("tcp", "linkmount") assert_array_almost_equal(tcp2mount, tcp2mount2) finally: if os.path.exists(filename): os.remove(filename) def test_rotation_matrix_conversion(): urdf = """ """ tm = UrdfTransformManager() tm.load_urdf(urdf) parent2child = tm.get_transform("parent", "child") assert_array_almost_equal( parent2child, np.array( [ [0.23859, 0.943669, 0.229266, -0.136378], [-0.868696, 0.101862, 0.48476, 0.550047], [0.4341, -0.314821, 0.844065, -1.819024], [0, 0, 0, 1], ] ), ) def test_xml_comment(): urdf = """ transmission_interface/SimpleTransmission PositionJointInterface 1 transmission_interface/SimpleTransmission PositionJointInterface 1 transmission_interface/SimpleTransmission PositionJointInterface 1 transmission_interface/SimpleTransmission PositionJointInterface 1 transmission_interface/SimpleTransmission PositionJointInterface 1 transmission_interface/SimpleTransmission PositionJointInterface 1 """ (robot_name, links, _) = parse_urdf(urdf) assert robot_name == "ur10" assert len(links) == 9 assert links[0].name == "base_link" ================================================ FILE: pytransform3d/trajectories/__init__.py ================================================ """Trajectories in three dimensions - SE(3). Conversions from this module operate on batches of poses or transformations and can be 400 to 1000 times faster than a loop of individual conversions. """ from ._dual_quaternions import ( batch_dq_conj, batch_dq_q_conj, batch_dq_prod_vector, batch_concatenate_dual_quaternions, dual_quaternions_power, dual_quaternions_sclerp, pqs_from_dual_quaternions, screw_parameters_from_dual_quaternions, transforms_from_dual_quaternions, ) from ._plot import ( plot_trajectory, ) from ._pqs import ( transforms_from_pqs, dual_quaternions_from_pqs, ) from ._random import ( random_trajectories, ) from ._screws import ( mirror_screw_axis_direction, transforms_from_exponential_coordinates, dual_quaternions_from_screw_parameters, ) from ._transforms import ( invert_transforms, concat_one_to_many, concat_many_to_one, concat_many_to_many, concat_dynamic, pqs_from_transforms, exponential_coordinates_from_transforms, dual_quaternions_from_transforms, ) __all__ = [ "invert_transforms", "concat_one_to_many", "concat_many_to_one", "concat_many_to_many", "concat_dynamic", "transforms_from_pqs", "pqs_from_transforms", "exponential_coordinates_from_transforms", "transforms_from_exponential_coordinates", "dual_quaternions_from_pqs", "dual_quaternions_from_transforms", "pqs_from_dual_quaternions", "screw_parameters_from_dual_quaternions", "dual_quaternions_from_screw_parameters", "dual_quaternions_power", "dual_quaternions_sclerp", "transforms_from_dual_quaternions", "batch_concatenate_dual_quaternions", "batch_dq_conj", "batch_dq_q_conj", "batch_dq_prod_vector", "mirror_screw_axis_direction", "random_trajectories", "plot_trajectory", ] ================================================ FILE: pytransform3d/trajectories/_dual_quaternions.py ================================================ """Dual quaternion operations.""" import numpy as np from ._screws import ( dual_quaternions_from_screw_parameters, ) from ..batch_rotations import ( batch_concatenate_quaternions, batch_q_conj, axis_angles_from_quaternions, matrices_from_quaternions, ) def batch_dq_conj(dqs): """Conjugate of dual quaternions. There are three different conjugates for dual quaternions. The one that we use here converts (pw, px, py, pz, qw, qx, qy, qz) to (pw, -px, -py, -pz, -qw, qx, qy, qz). It is a combination of the quaternion conjugate and the dual number conjugate. Parameters ---------- dqs : array-like, shape (..., 8) Dual quaternions to represent transforms: (pw, px, py, pz, qw, qx, qy, qz) Returns ------- dq_conjugates : array-like, shape (..., 8) Conjugates of dual quaternions: (pw, -px, -py, -pz, -qw, qx, qy, qz) """ out = np.empty_like(dqs) out[..., 0] = dqs[..., 0] out[..., 1:5] = -dqs[..., 1:5] out[..., 5:] = dqs[..., 5:] return out def batch_dq_q_conj(dqs): """Quaternion conjugate of dual quaternions. For unit dual quaternions that represent transformations, this function is equivalent to the inverse of the corresponding transformation matrix. Parameters ---------- dqs : array-like, shape (..., 8) Unit dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) Returns ------- dq_q_conjugates : array, shape (..., 8) Conjugate of dual quaternion: (pw, -px, -py, -pz, qw, -qx, -qy, -qz) See Also -------- pytransform3d.transformations.dq_q_conj Quaternion conjugate of dual quaternions. """ out = np.empty_like(dqs) out[..., 0] = dqs[..., 0] out[..., 1:4] = -dqs[..., 1:4] out[..., 4] = dqs[..., 4] out[..., 5:8] = -dqs[..., 5:8] return out def batch_concatenate_dual_quaternions(dqs1, dqs2): """Concatenate dual quaternions. Suppose we want to apply two extrinsic transforms given by dual quaternions dq1 and dq2 to a vector v. We can either apply dq2 to v and then dq1 to the result or we can concatenate dq1 and dq2 and apply the result to v. Parameters ---------- dqs1 : array-like, shape (..., 8) Dual quaternions to represent transforms: (pw, px, py, pz, qw, qx, qy, qz) dqs2 : array-like, shape (..., 8) Dual quaternions to represent transforms: (pw, px, py, pz, qw, qx, qy, qz) Returns ------- dqs3 : array, shape (8,) Products of the two batches of dual quaternions: (pw, px, py, pz, qw, qx, qy, qz) """ dqs1 = np.asarray(dqs1) dqs2 = np.asarray(dqs2) out = np.empty_like(dqs1) out[..., :4] = batch_concatenate_quaternions(dqs1[..., :4], dqs2[..., :4]) out[..., 4:] = batch_concatenate_quaternions( dqs1[..., :4], dqs2[..., 4:] ) + batch_concatenate_quaternions(dqs1[..., 4:], dqs2[..., :4]) return out def batch_dq_prod_vector(dqs, V): """Apply transforms represented by a dual quaternions to vectors. Parameters ---------- dqs : array-like, shape (..., 8) Unit dual quaternions V : array-like, shape (..., 3) 3d vectors Returns ------- W : array, shape (3,) 3d vectors """ dqs = np.asarray(dqs) v_dqs = np.empty_like(dqs) v_dqs[..., 0] = 1.0 v_dqs[..., 1:5] = 0.0 v_dqs[..., 5:] = V v_dq_transformed = batch_concatenate_dual_quaternions( batch_concatenate_dual_quaternions(dqs, v_dqs), batch_dq_conj(dqs) ) return v_dq_transformed[..., 5:] def dual_quaternions_power(dqs, ts): """Compute power of unit dual quaternions with respect to scalar. Parameters ---------- dqs : array-like, shape (..., 8) Unit dual quaternions to represent transform: (pw, px, py, pz, qw, qx, qy, qz) t : array-like, shape (...) Exponent Returns ------- dq_ts : array, shape (..., 8) Unit dual quaternions to represent transform: (pw, px, py, pz, qw, qx, qy, qz) ** t See Also -------- pytransform3d.transformations.dual_quaternion_power : Compute power of unit dual quaternion with respect to scalar. """ q, s_axis, h, theta = screw_parameters_from_dual_quaternions(dqs) return dual_quaternions_from_screw_parameters(q, s_axis, h, theta * ts) def dual_quaternions_sclerp(starts, ends, ts): """Screw linear interpolation (ScLERP) for array of dual quaternions. Parameters ---------- starts : array-like, shape (..., 8) Unit dual quaternion to represent start poses: (pw, px, py, pz, qw, qx, qy, qz) end : array-like, shape (..., 8) Unit dual quaternion to represent end poses: (pw, px, py, pz, qw, qx, qy, qz) ts : array-like, shape (...) Positions between starts and goals Returns ------- dq_ts : array, shape (..., 8) Interpolated unit dual quaternion: (pw, px, py, pz, qw, qx, qy, qz) See Also -------- pytransform3d.transformations.dual_quaternion_sclerp : Screw linear interpolation (ScLERP) for dual quaternions. """ starts = np.asarray(starts) ends = np.asarray(ends) ts = np.asarray(ts) if starts.shape != ends.shape: raise ValueError( "The 'starts' and 'ends' arrays must have the same shape." ) if ts.ndim != starts.ndim - 1 or ( ts.ndim > 0 and ts.shape != starts.shape[:-1] ): raise ValueError( "ts array, shape=%s must have the same number of elements as " "starts array, shape=%s" % (ts.shape, starts.shape) ) diffs = batch_concatenate_dual_quaternions(batch_dq_q_conj(starts), ends) powers = dual_quaternions_power(diffs, ts) return batch_concatenate_dual_quaternions(starts, powers) def pqs_from_dual_quaternions(dqs): """Get positions and quaternions from dual quaternions. Parameters ---------- dqs : array-like, shape (..., 8) Dual quaternions to represent transforms: (pw, px, py, pz, qw, qx, qy, qz) Returns ------- pqs : array, shape (..., 7) Poses represented by positions and quaternions in the order (x, y, z, qw, qx, qy, qz) """ dqs = np.asarray(dqs) instances_shape = dqs.shape[:-1] out = np.empty(instances_shape + (7,)) out[..., 3:] = dqs[..., :4] out[..., :3] = ( 2 * batch_concatenate_quaternions( dqs[..., 4:], batch_q_conj(out[..., 3:]) )[..., 1:] ) return out def screw_parameters_from_dual_quaternions(dqs): """Compute screw parameters from dual quaternions. Parameters ---------- dqs : array-like, shape (..., 8) Unit dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) Returns ------- qs : array, shape (..., 3) Vector to a point on the screw axis s_axiss : array, shape (..., 3) Direction vector of the screw axis hs : array, shape (...,) Pitch of the screw. The pitch is the ratio of translation and rotation of the screw axis. Infinite pitch indicates pure translation. thetas : array, shape (...,) Parameter of the transformation: theta is the angle of rotation and h * theta the translation. See Also -------- pytransform3d.transformations.screw_parameters_from_dual_quaternion : Compute screw parameters from dual quaternion. """ reals = dqs[..., :4] duals = dqs[..., 4:] a = axis_angles_from_quaternions(reals) s_axis = np.copy(a[..., :3]) thetas = a[..., 3] translation = ( 2 * batch_concatenate_quaternions(duals, batch_q_conj(reals))[..., 1:] ) # instead of the if/else stamenets in the # screw_parameters_from_dual_quaternion function # we use mask array to enable vectorized operations # the name of the mask represent the according block in # the original function outer_if_mask = np.abs(thetas) < np.finfo(float).eps outer_else_mask = np.logical_not(outer_if_mask) ds = np.linalg.norm(translation, axis=-1) inner_if_mask = ds < np.finfo(float).eps outer_if_inner_if_mask = np.logical_and(outer_if_mask, inner_if_mask) outer_if_inner_else_mask = np.logical_and( outer_if_mask, np.logical_not(inner_if_mask) ) if np.any(outer_if_inner_if_mask): s_axis[outer_if_inner_if_mask] = np.array([1.0, 0.0, 0.0]) if np.any(outer_if_inner_else_mask): s_axis[outer_if_inner_else_mask] = ( translation[outer_if_inner_else_mask] / ds[outer_if_inner_else_mask][..., np.newaxis] ) qs = np.zeros(dqs.shape[:-1] + (3,)) thetas[outer_if_mask] = ds[outer_if_mask] hs = np.full(dqs.shape[:-1], np.inf) if np.any(outer_else_mask): distance = np.einsum( "ij,ij->i", translation[outer_else_mask], s_axis[outer_else_mask] ) moment = 0.5 * ( np.cross(translation[outer_else_mask], s_axis[outer_else_mask]) + ( translation[outer_else_mask] - distance[..., np.newaxis] * s_axis[outer_else_mask] ) / np.tan(0.5 * thetas[outer_else_mask])[..., np.newaxis] ) qs[outer_else_mask] = np.cross(s_axis[outer_else_mask], moment) hs[outer_else_mask] = distance / thetas[outer_else_mask] return qs, s_axis, hs, thetas def transforms_from_dual_quaternions(dqs): """Get transformations from dual quaternions. Parameters ---------- dqs : array-like, shape (..., 8) Dual quaternions to represent transforms: (pw, px, py, pz, qw, qx, qy, qz) Returns ------- A2Bs : array, shape (..., 4, 4) Poses represented by homogeneous matrices """ dqs = np.asarray(dqs) instances_shape = dqs.shape[:-1] out = np.empty(instances_shape + (4, 4)) out[..., :3, :3] = matrices_from_quaternions(dqs[..., :4]) out[..., :3, 3] = ( 2 * batch_concatenate_quaternions( dqs[..., 4:], batch_q_conj(dqs[..., :4]) )[..., 1:] ) out[..., 3, :3] = 0.0 out[..., 3, 3] = 1.0 return out ================================================ FILE: pytransform3d/trajectories/_dual_quaternions.pyi ================================================ import numpy as np import numpy.typing as npt def batch_dq_conj(dqs: npt.ArrayLike) -> np.ndarray: ... def batch_dq_q_conj(dqs: npt.ArrayLike) -> np.ndarray: ... def batch_concatenate_dual_quaternions( dqs1: npt.ArrayLike, dqs2: npt.ArrayLike ) -> np.ndarray: ... def batch_dq_prod_vector( dqs: npt.ArrayLike, V: npt.ArrayLike ) -> np.ndarray: ... def dual_quaternions_power( dqs: npt.ArrayLike, ts: npt.ArrayLike ) -> np.ndarray: ... def dual_quaternions_sclerp( starts: npt.ArrayLike, ends: npt.ArrayLike, ts: npt.ArrayLike ) -> np.ndarray: ... def pqs_from_dual_quaternions(dqs: npt.ArrayLike) -> np.ndarray: ... def screw_parameters_from_dual_quaternions( dqs: npt.ArrayLike, ) -> np.ndarray: ... def transforms_from_dual_quaternions(dqs: npt.ArrayLike) -> np.ndarray: ... ================================================ FILE: pytransform3d/trajectories/_plot.py ================================================ """Plotting utilities.""" from ._pqs import transforms_from_pqs def plot_trajectory( ax=None, P=None, normalize_quaternions=True, show_direction=True, n_frames=10, s=1.0, label=None, ax_s=1, **kwargs, ): # pragma: no cover """Plot pose trajectory. Parameters ---------- ax : Matplotlib 3d axis, optional (default: None) If the axis is None, a new 3d axis will be created P : array-like, shape (n_steps, 7), optional (default: None) Sequence of poses represented by positions and quaternions in the order (x, y, z, w, vx, vy, vz) for each step normalize_quaternions : bool, optional (default: True) Normalize quaternions before plotting show_direction : bool, optional (default: True) Plot an arrow to indicate the direction of the trajectory n_frames : int, optional (default: 10) Number of frames that should be plotted to indicate the rotation s : float, optional (default: 1) Scaling of the frames that will be drawn label : str, optional (default: None) Label of the trajectory ax_s : float, optional (default: 1) Scaling of the new matplotlib 3d axis kwargs : dict, optional (default: {}) Additional arguments for the plotting functions, e.g. alpha Returns ------- ax : Matplotlib 3d axis New or old axis Raises ------ ValueError If trajectory does not contain any elements. """ if P is None or len(P) == 0: raise ValueError("Trajectory does not contain any elements.") if ax is None: from ..plot_utils import make_3d_axis ax = make_3d_axis(ax_s) A2Bs = transforms_from_pqs(P, normalize_quaternions) from ..plot_utils import Trajectory trajectory = Trajectory(A2Bs, show_direction, n_frames, s, label, **kwargs) trajectory.add_trajectory(ax) return ax ================================================ FILE: pytransform3d/trajectories/_plot.pyi ================================================ import numpy as np import numpy.typing as npt from mpl_toolkits.mplot3d import Axes3D from typing import Union def plot_trajectory( ax: Union[None, Axes3D] = ..., P: Union[None, npt.ArrayLike] = ..., normalize_quaternions: bool = ..., show_direction: bool = ..., n_frames: int = ..., s: float = ..., label: Union[str, None] = ..., ax_s: float = ..., **kwargs, ) -> Axes3D: ... ================================================ FILE: pytransform3d/trajectories/_pqs.py ================================================ """Position+quaternion operations.""" import numpy as np from ..batch_rotations import ( matrices_from_quaternions, batch_concatenate_quaternions, ) def transforms_from_pqs(P, normalize_quaternions=True): """Get sequence of homogeneous matrices from positions and quaternions. Parameters ---------- P : array-like, shape (..., 7) Poses represented by positions and quaternions in the order (x, y, z, qw, qx, qy, qz) normalize_quaternions : bool, optional (default: True) Normalize quaternions before conversion Returns ------- A2Bs : array, shape (..., 4, 4) Poses represented by homogeneous matrices """ P = np.asarray(P) instances_shape = P.shape[:-1] A2Bs = np.empty(instances_shape + (4, 4)) A2Bs[..., :3, 3] = P[..., :3] A2Bs[..., 3, :3] = 0.0 A2Bs[..., 3, 3] = 1.0 matrices_from_quaternions( P[..., 3:], normalize_quaternions, out=A2Bs[..., :3, :3] ) return A2Bs def dual_quaternions_from_pqs(pqs): """Get dual quaternions from positions and quaternions. Parameters ---------- pqs : array-like, shape (..., 7) Poses represented by positions and quaternions in the order (x, y, z, qw, qx, qy, qz) Returns ------- dqs : array, shape (..., 8) Dual quaternions to represent transforms: (pw, px, py, pz, qw, qx, qy, qz) """ pqs = np.asarray(pqs) instances_shape = pqs.shape[:-1] out = np.empty(instances_shape + (8,)) # orientation quaternion out[..., :4] = pqs[..., 3:] # use memory temporarily to store position out[..., 4] = 0 out[..., 5:] = pqs[..., :3] out[..., 4:] = 0.5 * batch_concatenate_quaternions( out[..., 4:], out[..., :4] ) return out ================================================ FILE: pytransform3d/trajectories/_pqs_.pyi ================================================ import numpy as np import numpy.typing as npt def transforms_from_pqs( P: npt.ArrayLike, normalize_quaternions: bool = ... ) -> np.ndarray: ... def dual_quaternions_from_pqs(pqs: npt.ArrayLike) -> np.ndarray: ... ================================================ FILE: pytransform3d/trajectories/_random.py ================================================ """Random trajectory generation.""" import numpy as np from ._dual_quaternions import ( transforms_from_dual_quaternions, dual_quaternions_sclerp, ) from ._screws import ( transforms_from_exponential_coordinates, ) from ._transforms import ( concat_many_to_many, dual_quaternions_from_transforms, ) _N_EXP_COORDINATE_DIMS = 6 def random_trajectories( rng=np.random.default_rng(0), n_trajectories=10, n_steps=101, start=np.eye(4), goal=np.eye(4), scale=100.0 * np.ones(6), ): """Generate random trajectories. Create a smooth random trajectory with low accelerations. The generated trajectories consist of a linear movement from start to goal and a superimposed random movement with low accelerations. Hence, the first pose and last pose do not exactly equal the start and goal pose respectively. Parameters ---------- rng : np.random.Generator, optional (default: random seed 0) Random number generator n_trajectories : int, optional (default: 10) Number of trajectories that should be generated. n_steps : int, optional (default: 101) Number of steps in each trajectory. start : array-like, shape (4, 4), optional (default: I) Start pose as transformation matrix. goal : array-like, shape (4, 4), optional (default: I) Goal pose as transformation matrix. scale : array-like, shape (6,), optional (default: [100] * 6) Scaling factor for random deviations from linear movement from start to goal. Returns ------- trajectories : array, shape (n_trajectories, n_steps, 4, 4) Random trajectories between start and goal. """ dt = 1.0 / (n_steps - 1) linear_component = _linear_movement(start, goal, n_steps, dt) L = _acceleration_L(_N_EXP_COORDINATE_DIMS, n_steps, dt) samples = rng.normal( size=(n_trajectories, _N_EXP_COORDINATE_DIMS * n_steps) ) smooth_samples = np.dot(samples, L.T) Sthetas = smooth_samples.reshape( n_trajectories, _N_EXP_COORDINATE_DIMS, n_steps ).transpose([0, 2, 1]) Sthetas *= np.asarray(scale)[np.newaxis, np.newaxis] trajectories = transforms_from_exponential_coordinates(Sthetas) for i in range(n_trajectories): trajectories[i] = concat_many_to_many(trajectories[i], linear_component) return trajectories def _linear_movement(start, goal, n_steps, dt): """Linear movement from start to goal. Parameters ---------- start : array-like, shape (4, 4) Start pose as transformation matrix. goal : array-like, shape (4, 4) Goal pose as transformation matrix. n_steps : int Number of steps. dt : float Time difference between two steps. Returns ------- linear_component : array, shape (n_steps, 4, 4) Linear trajectory from start to goal with equal step sizes. """ time = np.arange(n_steps) * dt start_dq = dual_quaternions_from_transforms(start) goal_dq = dual_quaternions_from_transforms(goal) return transforms_from_dual_quaternions( dual_quaternions_sclerp( np.repeat(start_dq[np.newaxis], n_steps, axis=0), np.repeat(goal_dq[np.newaxis], n_steps, axis=0), time, ) ) def _acceleration_L(n_dims, n_steps, dt): """Cholesky decomposition of a smooth trajectory covariance. Parameters ---------- n_dims : int Number of dimensions. n_steps : int Number of steps in the trajectory. dt : float Time difference between two steps. Returns ------- L : array, shape (n_steps * n_dims, n_steps * n_dims) Cholesky decomposition of covariance created from finite difference matrix. """ A_per_dim = _create_fd_matrix_1d(n_steps, dt) covariance = np.linalg.inv(np.dot(A_per_dim.T, A_per_dim)) L_per_dim = np.linalg.cholesky(covariance) # Copy L for each dimension L = np.zeros((n_dims * n_steps, n_dims * n_steps)) for d in range(n_dims): L[d * n_steps : (d + 1) * n_steps, d * n_steps : (d + 1) * n_steps] = ( L_per_dim ) return L def _create_fd_matrix_1d(n_steps, dt): r"""Create one-dimensional finite difference matrix for second derivative. The finite difference matrix A for the second derivative with respect to time is defined as: .. math:: \ddot(x) = A x Parameters ---------- n_steps : int Number of steps in the trajectory dt : float Time in seconds between successive steps Returns ------- A : array, shape (n_steps + 2, n_steps) Finite difference matrix for second derivative with respect to time """ A = np.zeros((n_steps + 2, n_steps), dtype=float) super_diagonal = (np.arange(n_steps), np.arange(n_steps)) A[super_diagonal] = np.ones(n_steps) sub_diagonal = (np.arange(2, n_steps + 2), np.arange(n_steps)) A[sub_diagonal] = np.ones(n_steps) main_diagonal = (np.arange(1, n_steps + 1), np.arange(n_steps)) A[main_diagonal] = -2 * np.ones(n_steps) return A / (dt**2) ================================================ FILE: pytransform3d/trajectories/_random.pyi ================================================ import numpy as np import numpy.typing as npt def random_trajectories( rng: np.random.Generator = ..., n_trajectories: int = ..., n_steps: int = ..., start: npt.ArrayLike = ..., goal: npt.ArrayLike = ..., scale: npt.ArrayLike = ..., ) -> np.ndarray: ... def _linear_movement( start: npt.ArrayLike, goal: npt.ArrayLike, n_steps: int, dt: float ) -> np.ndarray: ... def _acceleration_L(n_dims: int, n_steps: int, dt: float) -> np.ndarray: ... def _create_fd_matrix_1d(n_steps: int, dt: float) -> np.ndarray: ... ================================================ FILE: pytransform3d/trajectories/_screws.py ================================================ """Representations related to screw theory.""" import numpy as np from ..batch_rotations import ( matrices_from_compact_axis_angles, ) from ..transformations import ( screw_axis_from_exponential_coordinates, screw_parameters_from_screw_axis, screw_axis_from_screw_parameters, ) from ..transformations import ( transform_from_exponential_coordinates, ) def mirror_screw_axis_direction(Sthetas): """Switch to the other representation of the same transformation. We take the negative of the screw axis, invert the rotation angle and adapt the screw pitch accordingly. For this operation we have to convert exponential coordinates to screw parameters first. Parameters ---------- Sthetas : array-like, shape (n_steps, 6) Exponential coordinates of transformation: (omega_x, omega_y, omega_z, v_x, v_y, v_z) Returns ------- Sthetas : array, shape (n_steps, 6) Exponential coordinates of transformation: (omega_x, omega_y, omega_z, v_x, v_y, v_z) """ Sthetas_new = np.empty((len(Sthetas), 6)) for i, Stheta in enumerate(Sthetas): S, theta = screw_axis_from_exponential_coordinates(Stheta) q, s, h = screw_parameters_from_screw_axis(S) s_new = -s theta_new = 2.0 * np.pi - theta h_new = -h * theta / theta_new Stheta_new = ( screw_axis_from_screw_parameters(q, s_new, h_new) * theta_new ) Sthetas_new[i] = Stheta_new return Sthetas_new def transforms_from_exponential_coordinates(Sthetas): """Compute transformations from exponential coordinates. Parameters ---------- Sthetas : array-like, shape (..., 6) Exponential coordinates of transformation: S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta, where S is the screw axis, the first 3 components are related to rotation and the last 3 components are related to translation. Theta is the rotation angle and h * theta the translation. Returns ------- A2Bs : array, shape (..., 4, 4) Poses represented by homogeneous matrices """ Sthetas = np.asarray(Sthetas) if Sthetas.ndim == 1: return transform_from_exponential_coordinates(Sthetas) instances_shape = Sthetas.shape[:-1] t = np.linalg.norm(Sthetas[..., :3], axis=-1) A2Bs = np.empty(instances_shape + (4, 4)) A2Bs[..., 3, :] = (0, 0, 0, 1) ind_only_translation = t == 0.0 if not np.all(ind_only_translation): t[ind_only_translation] = 1.0 screw_axes = Sthetas / t[..., np.newaxis] matrices_from_compact_axis_angles( axes=screw_axes[..., :3], angles=t, out=A2Bs[..., :3, :3] ) # from sympy import * # o0, o1, o2, vx, vy, vz, t = symbols("o0 o1 o2 v_x v_y v_z t") # w = Matrix([[0, -o2, o1], [o2, 0, -o0], [-o1, o0, 0]]) # v = Matrix([[vx], [vy], [vz]]) # p = (eye(3) * t + (1 - cos(t)) * w + (t - sin(t)) * w * w) * v # # Result: # -v_x*(o1**2*(t - sin(t)) + o2**2*(t - sin(t)) - t) # + v_y*(o0*o1*(t - sin(t)) + o2*(cos(t) - 1)) # + v_z*(o0*o2*(t - sin(t)) - o1*(cos(t) - 1)) # v_x*(o0*o1*(t - sin(t)) - o2*(cos(t) - 1)) # - v_y*(o0**2*(t - sin(t)) + o2**2*(t - sin(t)) - t) # + v_z*(o0*(cos(t) - 1) + o1*o2*(t - sin(t))) # v_x*(o0*o2*(t - sin(t)) + o1*(cos(t) - 1)) # - v_y*(o0*(cos(t) - 1) - o1*o2*(t - sin(t))) # - v_z*(o0**2*(t - sin(t)) + o1**2*(t - sin(t)) - t) tms = t - np.sin(t) cm1 = np.cos(t) - 1.0 o0 = screw_axes[..., 0] o1 = screw_axes[..., 1] o2 = screw_axes[..., 2] v0 = screw_axes[..., 3] v1 = screw_axes[..., 4] v2 = screw_axes[..., 5] o01tms = o0 * o1 * tms o12tms = o1 * o2 * tms o02tms = o0 * o2 * tms o0cm1 = o0 * cm1 o1cm1 = o1 * cm1 o2cm1 = o2 * cm1 o00tms = o0 * o0 * tms o11tms = o1 * o1 * tms o22tms = o2 * o2 * tms v0 = v0.reshape(*instances_shape) v1 = v1.reshape(*instances_shape) v2 = v2.reshape(*instances_shape) A2Bs[..., 0, 3] = ( -v0 * (o11tms + o22tms - t) + v1 * (o01tms + o2cm1) + v2 * (o02tms - o1cm1) ) A2Bs[..., 1, 3] = ( v0 * (o01tms - o2cm1) - v1 * (o00tms + o22tms - t) + v2 * (o0cm1 + o12tms) ) A2Bs[..., 2, 3] = ( v0 * (o02tms + o1cm1) - v1 * (o0cm1 - o12tms) - v2 * (o00tms + o11tms - t) ) A2Bs[ind_only_translation, :3, :3] = np.eye(3) A2Bs[ind_only_translation, :3, 3] = Sthetas[ind_only_translation, 3:] return A2Bs def dual_quaternions_from_screw_parameters(qs, s_axis, hs, thetas): """Compute dual quaternions from arrays of screw parameters. Parameters ---------- qs : array-like, shape (..., 3) Vector to a point on the screw axis s_axis : array-like, shape (..., 3) Direction vector of the screw axis hs : array-like, shape (...,) Pitch of the screw. The pitch is the ratio of translation and rotation of the screw axis. Infinite pitch indicates pure translation. thetas : array-like, shape (...,) Parameter of the transformation: theta is the angle of rotation and h * theta the translation. Returns ------- dqs : array, shape (..., 8) Unit dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) See Also -------- pytransform3d.transformations.dual_quaternion_from_screw_parameters : Compute dual quaternion from screw parameters. """ h_is_not_inf_mask = ~np.isinf(hs) mod_thetas = np.where(h_is_not_inf_mask, thetas, 0.0) ds = np.copy(thetas) ds[h_is_not_inf_mask] *= hs[h_is_not_inf_mask] moments = np.cross(qs, s_axis) half_distances = 0.5 * ds half_thetas = 0.5 * mod_thetas sin_half_angles = np.sin(half_thetas) cos_half_angles = np.cos(half_thetas) real_w = cos_half_angles real_vec = sin_half_angles[..., np.newaxis] * s_axis dual_w = -half_distances * sin_half_angles dual_vec = ( sin_half_angles[..., np.newaxis] * moments + half_distances[..., np.newaxis] * cos_half_angles[..., np.newaxis] * s_axis ) result = np.concatenate( [real_w[..., np.newaxis], real_vec, dual_w[..., np.newaxis], dual_vec], axis=-1, ) return result ================================================ FILE: pytransform3d/trajectories/_screws.pyi ================================================ import numpy as np import numpy.typing as npt def mirror_screw_axis_direction(Sthetas: npt.ArrayLike) -> np.ndarray: ... def transforms_from_exponential_coordinates( Sthetas: npt.ArrayLike, ) -> np.ndarray: ... def dual_quaternions_from_screw_parameters( qs: npt.ArrayLike, s_axis: npt.ArrayLike, hs: npt.ArrayLike, thetas: npt.ArrayLike, ) -> np.ndarray: ... ================================================ FILE: pytransform3d/trajectories/_transforms.py ================================================ """Transformation matrices.""" import numpy as np from pytransform3d.batch_rotations import ( quaternions_from_matrices, axis_angles_from_matrices, batch_concatenate_quaternions, ) def invert_transforms(A2Bs): """Invert transforms. Parameters ---------- A2Bs : array-like, shape (..., 4, 4) Transforms from frames A to frames B Returns ------- B2As : array, shape (..., 4, 4) Transforms from frames B to frames A See Also -------- pytransform3d.transformations.invert_transform : Invert one transformation. """ A2Bs = np.asarray(A2Bs) instances_shape = A2Bs.shape[:-2] B2As = np.empty_like(A2Bs) # ( R t )^-1 ( R^T -R^T*t ) # ( 0 1 ) = ( 0 1 ) B2As[..., :3, :3] = A2Bs[..., :3, :3].transpose( tuple(range(A2Bs.ndim - 2)) + (A2Bs.ndim - 1, A2Bs.ndim - 2) ) B2As[..., :3, 3] = np.einsum( "nij,nj->ni", -B2As[..., :3, :3].reshape(-1, 3, 3), A2Bs[..., :3, 3].reshape(-1, 3), ).reshape(*(instances_shape + (3,))) B2As[..., 3, :3] = 0.0 B2As[..., 3, 3] = 1.0 return B2As def concat_one_to_many(A2B, B2Cs): """Concatenate transformation A2B with multiple transformations B2C. We use the extrinsic convention, which means that B2Cs are left-multiplied to A2B. Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B B2Cs : array-like, shape (n_transforms, 4, 4) Transforms from frame B to frame C Returns ------- A2Cs : array, shape (n_transforms, 4, 4) Transforms from frame A to frame C See Also -------- concat_many_to_one : Concatenate multiple transformations with one. pytransform3d.transformations.concat : Concatenate two transformations. """ return np.einsum("nij,jk->nik", B2Cs, A2B) def concat_many_to_one(A2Bs, B2C): """Concatenate multiple transformations A2B with transformation B2C. We use the extrinsic convention, which means that B2C is left-multiplied to A2Bs. Parameters ---------- A2Bs : array-like, shape (n_transforms, 4, 4) Transforms from frame A to frame B B2C : array-like, shape (4, 4) Transform from frame B to frame C Returns ------- A2Cs : array, shape (n_transforms, 4, 4) Transforms from frame A to frame C See Also -------- concat_one_to_many : Concatenate one transformation with multiple transformations. pytransform3d.transformations.concat : Concatenate two transformations. """ return np.einsum("ij,njk->nik", B2C, A2Bs) def concat_many_to_many(A2B, B2C): """Concatenate multiple transformations A2B with transformation B2C. We use the extrinsic convention, which means that B2C is left-multiplied to A2Bs. Parameters ---------- A2B : array-like, shape (n_transforms, 4, 4) Transforms from frame A to frame B B2C : array-like, shape (n_transforms, 4, 4) Transform from frame B to frame C Returns ------- A2Cs : array, shape (n_transforms, 4, 4) Transforms from frame A to frame C See Also -------- concat_many_to_one : Concatenate one transformation with multiple transformations. pytransform3d.transformations.concat : Concatenate two transformations. """ return np.einsum("ijk,ikl->ijl", B2C, A2B) def concat_dynamic(A2B, B2C): """Concatenate multiple transformations A2B,B2C with different shapes. We use the extrinsic convention, which means that B2C is left-multiplied to A2Bs. it can handle different shapes of A2B and B2C dynamically. Parameters ---------- A2B : array-like, shape (n_transforms, 4, 4) or (4, 4) Transforms from frame A to frame B B2C : array-like, shape (n_transforms, 4, 4) or (4, 4) Transform from frame B to frame C Returns ------- A2Cs : array, shape (n_transforms, 4, 4) or (4, 4) Transforms from frame A to frame C See Also -------- concat_many_to_one : Concatenate multiple transformations with one transformation. concat_one_to_many : Concatenate one transformation with multiple transformations. concat_many_to_many : Concatenate multiple transformations with multiple transformations. pytransform3d.transformations.concat : Concatenate two transformations. """ A2B = np.asarray(A2B) B2C = np.asarray(B2C) if B2C.ndim == 2 and A2B.ndim == 2: return B2C.dot(A2B) elif B2C.ndim == 2 and A2B.ndim == 3: return concat_many_to_one(A2B, B2C) elif B2C.ndim == 3 and A2B.ndim == 2: return concat_one_to_many(A2B, B2C) elif B2C.ndim == 3 and A2B.ndim == 3: return concat_many_to_many(A2B, B2C) else: raise ValueError( "Expected ndim 2 or 3; got B2C.ndim=%d, A2B.ndim=%d" % (B2C.ndim, A2B.ndim) ) def pqs_from_transforms(A2Bs): """Get sequence of positions and quaternions from homogeneous matrices. Parameters ---------- A2Bs : array-like, shape (..., 4, 4) Poses represented by homogeneous matrices Returns ------- P : array, shape (n_steps, 7) Poses represented by positions and quaternions in the order (x, y, z, qw, qx, qy, qz) for each step """ A2Bs = np.asarray(A2Bs) instances_shape = A2Bs.shape[:-2] P = np.empty(instances_shape + (7,)) P[..., :3] = A2Bs[..., :3, 3] quaternions_from_matrices(A2Bs[..., :3, :3], out=P[..., 3:]) return P def exponential_coordinates_from_transforms(A2Bs): """Compute exponential coordinates from transformations. Parameters ---------- A2Bs : array-like, shape (..., 4, 4) Poses represented by homogeneous matrices Returns ------- Sthetas : array, shape (..., 6) Exponential coordinates of transformation: S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta, where S is the screw axis, the first 3 components are related to rotation and the last 3 components are related to translation. Theta is the rotation angle and h * theta the translation. """ A2Bs = np.asarray(A2Bs) instances_shape = A2Bs.shape[:-2] Rs = A2Bs[..., :3, :3] ps = A2Bs[..., :3, 3] traces = np.einsum("nii", Rs.reshape(-1, 3, 3)) if instances_shape: # noqa: SIM108 traces = traces.reshape(*instances_shape) else: # this works because indX will be a single boolean and # out[True, n] = value will assign value to out[n], while # out[False, n] = value will not assign value to out[n] traces = traces[0] Sthetas = np.empty(instances_shape + (6,)) omega_thetas = axis_angles_from_matrices(Rs, traces=traces) Sthetas[..., :3] = omega_thetas[..., :3] thetas = omega_thetas[..., 3] # from sympy import * # o0, o1, o2, px, py, pz, t = symbols("o0 o1 o2 p0 p1 p2 theta") # w = Matrix([[0, -o2, o1], [o2, 0, -o0], [-o1, o0, 0]]) # p = Matrix([[px], [py], [pz]]) # v = (eye(3) / t - 0.5 * w + (1 / t - 0.5 / tan(t / 2.0)) * w * w) * p # Result: # p0*(-o1**2*(-0.5/tan(0.5*t) + 1/t) # - o2**2*(-0.5/tan(0.5*t) + 1/t) + 1/t) # + p1*(o0*o1*(-0.5/tan(0.5*t) + 1/t) + 0.5*o2) # + p2*(o0*o2*(-0.5/tan(0.5*t) + 1/t) - 0.5*o1) # p0*(o0*o1*(-0.5/tan(0.5*t) + 1/t) - 0.5*o2) # + p1*(-o0**2*(-0.5/tan(0.5*t) + 1/t) # - o2**2*(-0.5/tan(0.5*t) + 1/t) + 1/t) # + p2*(0.5*o0 + o1*o2*(-0.5/tan(0.5*t) + 1/t)) # p0*(o0*o2*(-0.5/tan(0.5*t) + 1/t) + 0.5*o1) # + p1*(-0.5*o0 + o1*o2*(-0.5/tan(0.5*t) + 1/t)) # + p2*(-o0**2*(-0.5/tan(0.5*t) + 1/t) # - o1**2*(-0.5/tan(0.5*t) + 1/t) + 1/t) thetas = np.maximum(thetas, np.finfo(float).tiny) ti = 1.0 / thetas tan_term = -0.5 / np.tan(thetas / 2.0) + ti o0 = omega_thetas[..., 0] o1 = omega_thetas[..., 1] o2 = omega_thetas[..., 2] p0 = ps[..., 0] p1 = ps[..., 1] p2 = ps[..., 2] o00 = o0 * o0 o01 = o0 * o1 o02 = o0 * o2 o11 = o1 * o1 o12 = o1 * o2 o22 = o2 * o2 Sthetas[..., 3] = ( p0 * ((-o11 - o22) * tan_term + ti) + p1 * (o01 * tan_term + 0.5 * o2) + p2 * (o02 * tan_term - 0.5 * o1) ) Sthetas[..., 4] = ( p0 * (o01 * tan_term - 0.5 * o2) + p1 * ((-o00 - o22) * tan_term + ti) + p2 * (0.5 * o0 + o12 * tan_term) ) Sthetas[..., 5] = ( p0 * (o02 * tan_term + 0.5 * o1) + p1 * (-0.5 * o0 + o12 * tan_term) + p2 * ((-o00 - o11) * tan_term + ti) ) Sthetas *= thetas[..., np.newaxis] ind_only_translation = traces >= 3.0 - np.finfo(float).eps Sthetas[ind_only_translation, :3] = 0.0 Sthetas[ind_only_translation, 3:] = ps[ind_only_translation] return Sthetas def dual_quaternions_from_transforms(A2Bs): """Get dual quaternions from transformations. Parameters ---------- A2Bs : array-like, shape (..., 4, 4) Poses represented by homogeneous matrices Returns ------- dqs : array, shape (..., 8) Dual quaternions to represent transforms: (pw, px, py, pz, qw, qx, qy, qz) """ A2Bs = np.asarray(A2Bs) instances_shape = A2Bs.shape[:-2] out = np.empty(instances_shape + (8,)) # orientation quaternion out[..., :4] = quaternions_from_matrices(A2Bs[..., :3, :3]) # use memory temporarily to store position out[..., 4] = 0 out[..., 5:] = A2Bs[..., :3, 3] out[..., 4:] = 0.5 * batch_concatenate_quaternions( out[..., 4:], out[..., :4] ) return out ================================================ FILE: pytransform3d/trajectories/_transforms.pyi ================================================ import numpy as np import numpy.typing as npt def invert_transforms(A2Bs: npt.ArrayLike) -> np.ndarray: ... def concat_one_to_many( A2B: npt.ArrayLike, B2Cs: npt.ArrayLike ) -> np.ndarray: ... def concat_many_to_one( A2Bs: npt.ArrayLike, B2C: npt.ArrayLike ) -> np.ndarray: ... def concat_many_to_many( A2B: npt.ArrayLike, B2C: npt.ArrayLike ) -> np.ndarray: ... def concat_dynamic(A2B: npt.ArrayLike, B2C: npt.ArrayLike) -> np.ndarray: ... def pqs_from_transforms(A2Bs: npt.ArrayLike) -> np.ndarray: ... def exponential_coordinates_from_transforms( A2Bs: npt.ArrayLike, ) -> np.ndarray: ... def dual_quaternions_from_transforms(A2Bs: npt.ArrayLike) -> np.ndarray: ... ================================================ FILE: pytransform3d/trajectories/test/test_trajectories.py ================================================ import numpy as np import pytest from numpy.testing import assert_array_almost_equal import pytransform3d.batch_rotations as pbr import pytransform3d.rotations as pr import pytransform3d.trajectories as ptr import pytransform3d.transformations as pt def test_invert_transforms_0dims(): rng = np.random.default_rng(0) A2B = pt.random_transform(rng) B2A = pt.invert_transform(A2B) assert_array_almost_equal(B2A, ptr.invert_transforms(A2B)) def test_invert_transforms_1dims(): rng = np.random.default_rng(1) A2Bs = np.empty((3, 4, 4)) B2As = np.empty((3, 4, 4)) for i in range(len(A2Bs)): A2Bs[i] = pt.random_transform(rng) B2As[i] = pt.invert_transform(A2Bs[i]) assert_array_almost_equal(B2As, ptr.invert_transforms(A2Bs)) def test_invert_transforms_2dims(): rng = np.random.default_rng(1) A2Bs = np.empty((9, 4, 4)) B2As = np.empty((9, 4, 4)) for i in range(len(A2Bs)): A2Bs[i] = pt.random_transform(rng) B2As[i] = pt.invert_transform(A2Bs[i]) assert_array_almost_equal( B2As.reshape(3, 3, 4, 4), ptr.invert_transforms(A2Bs.reshape(3, 3, 4, 4)), ) def test_concat_one_to_many(): rng = np.random.default_rng(482) A2B = pt.random_transform(rng) B2C = pt.random_transform(rng) A2C = pt.concat(A2B, B2C) assert_array_almost_equal(A2C, ptr.concat_one_to_many(A2B, [B2C])[0]) B2Cs = [pt.random_transform(rng) for _ in range(5)] A2Cs = [pt.concat(A2B, B2C) for B2C in B2Cs] assert_array_almost_equal(A2Cs, ptr.concat_one_to_many(A2B, B2Cs)) def test_concat_many_to_one(): rng = np.random.default_rng(482) A2B = pt.random_transform(rng) B2C = pt.random_transform(rng) A2C = pt.concat(A2B, B2C) assert_array_almost_equal(A2C, ptr.concat_many_to_one([A2B], B2C)[0]) A2Bs = [pt.random_transform(rng) for _ in range(5)] A2Cs = [pt.concat(A2B, B2C) for A2B in A2Bs] assert_array_almost_equal(A2Cs, ptr.concat_many_to_one(A2Bs, B2C)) def test_concat_dynamic(): rng = np.random.default_rng(84320) n_rotations = 5 A2Bs = np.stack([pt.random_transform(rng) for _ in range(n_rotations)]) B2Cs = np.stack([pt.random_transform(rng) for _ in range(n_rotations)]) A2Cs = ptr.concat_dynamic(A2Bs, B2Cs) for i in range(len(A2Cs)): # check n_rotations - n_rotations case assert_array_almost_equal(A2Cs[i], pt.concat(A2Bs[i], B2Cs[i])) # check 1 - 1 case assert_array_almost_equal(A2Cs[i], ptr.concat_dynamic(A2Bs[i], B2Cs[i])) # check 1 - n_rotations case assert_array_almost_equal( ptr.concat_dynamic(A2Bs[0], B2Cs), ptr.concat_one_to_many(A2Bs[0], B2Cs) ) # check n_rotations - 1 case assert_array_almost_equal( ptr.concat_dynamic(A2Bs, B2Cs[0]), ptr.concat_many_to_one(A2Bs, B2Cs[0]) ) with pytest.raises(ValueError, match="Expected ndim 2 or 3"): ptr.concat_dynamic(A2Bs, B2Cs[np.newaxis]) with pytest.raises(ValueError, match="Expected ndim 2 or 3"): ptr.concat_dynamic(A2Bs[np.newaxis], B2Cs) def test_concat_many_to_many(): rng = np.random.default_rng(84320) n_rotations = 5 A2Bs = np.stack([pt.random_transform(rng) for _ in range(n_rotations)]) B2Cs = np.stack([pt.random_transform(rng) for _ in range(n_rotations)]) A2Cs = ptr.concat_many_to_many(A2Bs, B2Cs) for i in range(len(A2Bs)): assert_array_almost_equal(A2Cs[i], pt.concat(A2Bs[i], B2Cs[i])) with pytest.raises(ValueError): ptr.concat_many_to_many(A2Bs, B2Cs[:-1]) with pytest.raises(ValueError): ptr.concat_many_to_many(A2Bs, B2Cs[0]) def test_transforms_from_pqs_0dims(): rng = np.random.default_rng(0) pq = np.empty(7) pq[:3] = rng.standard_normal(size=3) pq[3:] = pr.random_quaternion(rng) A2B = ptr.transforms_from_pqs(pq, False) assert_array_almost_equal(A2B, pt.transform_from_pq(pq)) def test_transforms_from_pqs_1dim(): P = np.empty((10, 7)) rng = np.random.default_rng(0) P[:, :3] = rng.standard_normal(size=(len(P), 3)) P[:, 3:] = pbr.norm_vectors(rng.standard_normal(size=(len(P), 4))) H = ptr.transforms_from_pqs(P) P2 = ptr.pqs_from_transforms(H) assert_array_almost_equal(P[:, :3], H[:, :3, 3]) assert_array_almost_equal(P[:, :3], P2[:, :3]) for t in range(len(P)): pr.assert_quaternion_equal( P[t, 3:], pr.quaternion_from_matrix(H[t, :3, :3]) ) pr.assert_quaternion_equal(P[t, 3:], P2[t, 3:]) def test_transforms_from_pqs_4dims(): rng = np.random.default_rng(0) P = rng.standard_normal(size=(2, 3, 4, 5, 7)) P[..., 3:] = pbr.norm_vectors(P[..., 3:]) H = ptr.transforms_from_pqs(P) P2 = ptr.pqs_from_transforms(H) assert_array_almost_equal(P[..., :3], H[..., :3, 3]) assert_array_almost_equal(P[..., :3], P2[..., :3]) def test_transforms_from_exponential_coordinates(): A2B = np.eye(4) Stheta = pt.exponential_coordinates_from_transform(A2B) assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) A2B2 = ptr.transforms_from_exponential_coordinates(Stheta[np.newaxis])[0] assert_array_almost_equal(A2B, A2B2) A2B2 = ptr.transforms_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2) A2B2 = ptr.transforms_from_exponential_coordinates([[Stheta], [Stheta]])[ 0, 0 ] assert_array_almost_equal(A2B, A2B2) A2B = pt.translate_transform(np.eye(4), [1.0, 5.0, 0.0]) Stheta = pt.exponential_coordinates_from_transform(A2B) assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 1.0, 5.0, 0.0]) A2B2 = ptr.transforms_from_exponential_coordinates(Stheta[np.newaxis])[0] assert_array_almost_equal(A2B, A2B2) A2B2 = ptr.transforms_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2) A2B2 = ptr.transforms_from_exponential_coordinates([[Stheta], [Stheta]])[ 0, 0 ] assert_array_almost_equal(A2B, A2B2) A2B = pt.rotate_transform( np.eye(4), pr.active_matrix_from_angle(2, 0.5 * np.pi) ) Stheta = pt.exponential_coordinates_from_transform(A2B) assert_array_almost_equal(Stheta, [0.0, 0.0, 0.5 * np.pi, 0.0, 0.0, 0.0]) A2B2 = ptr.transforms_from_exponential_coordinates(Stheta[np.newaxis])[0] assert_array_almost_equal(A2B, A2B2) A2B2 = ptr.transforms_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2) A2B2 = ptr.transforms_from_exponential_coordinates([[Stheta], [Stheta]])[ 0, 0 ] assert_array_almost_equal(A2B, A2B2) rng = np.random.default_rng(53) for _ in range(5): A2B = pt.random_transform(rng) Stheta = pt.exponential_coordinates_from_transform(A2B) A2B2 = ptr.transforms_from_exponential_coordinates(Stheta[np.newaxis])[ 0 ] assert_array_almost_equal(A2B, A2B2) A2B2 = ptr.transforms_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2) A2B2 = ptr.transforms_from_exponential_coordinates( [[Stheta], [Stheta]] )[0, 0] assert_array_almost_equal(A2B, A2B2) def test_exponential_coordinates_from_transforms_0dims(): rng = np.random.default_rng(842) Sthetas = rng.standard_normal(size=6) H = ptr.transforms_from_exponential_coordinates(Sthetas) Sthetas2 = ptr.exponential_coordinates_from_transforms(H) H2 = ptr.transforms_from_exponential_coordinates(Sthetas2) assert_array_almost_equal(H, H2) def test_exponential_coordinates_from_transforms_2dims(): rng = np.random.default_rng(843) Sthetas = rng.standard_normal(size=(4, 4, 6)) H = ptr.transforms_from_exponential_coordinates(Sthetas) Sthetas2 = ptr.exponential_coordinates_from_transforms(H) H2 = ptr.transforms_from_exponential_coordinates(Sthetas2) assert_array_almost_equal(H, H2) def test_dual_quaternions_from_pqs_0dims(): rng = np.random.default_rng(844) pq = rng.standard_normal(size=7) pq[3:] /= np.linalg.norm(pq[3:], axis=-1)[..., np.newaxis] dq = ptr.dual_quaternions_from_pqs(pq) pq2 = ptr.pqs_from_dual_quaternions(dq) assert_array_almost_equal(pq[:3], pq2[:3]) pr.assert_quaternion_equal(pq[3:], pq2[3:]) def test_dual_quaternions_from_pqs_1dim(): rng = np.random.default_rng(845) pqs = rng.standard_normal(size=(20, 7)) pqs[..., 3:] /= np.linalg.norm(pqs[..., 3:], axis=-1)[..., np.newaxis] dqs = ptr.dual_quaternions_from_pqs(pqs) pqs2 = ptr.pqs_from_dual_quaternions(dqs) for pq, pq2 in zip(pqs, pqs2): assert_array_almost_equal(pq[:3], pq2[:3]) pr.assert_quaternion_equal(pq[3:], pq2[3:]) def test_dual_quaternions_from_pqs_2dims(): rng = np.random.default_rng(846) pqs = rng.standard_normal(size=(5, 5, 7)) pqs[..., 3:] /= np.linalg.norm(pqs[..., 3:], axis=-1)[..., np.newaxis] dqs = ptr.dual_quaternions_from_pqs(pqs) pqs2 = ptr.pqs_from_dual_quaternions(dqs) for pq, pq2 in zip(pqs.reshape(-1, 7), pqs2.reshape(-1, 7)): assert_array_almost_equal(pq[:3], pq2[:3]) pr.assert_quaternion_equal(pq[3:], pq2[3:]) def test_batch_concatenate_dual_quaternions_0dims(): rng = np.random.default_rng(847) pqs = rng.standard_normal(size=(2, 7)) pqs[..., 3:] /= np.linalg.norm(pqs[..., 3:], axis=-1)[..., np.newaxis] dqs = ptr.dual_quaternions_from_pqs(pqs) dq1 = dqs[0] dq2 = dqs[1] assert_array_almost_equal( ptr.batch_concatenate_dual_quaternions(dq1, dq2), pt.concatenate_dual_quaternions(dq1, dq2), ) def test_batch_concatenate_dual_quaternions_2dims(): rng = np.random.default_rng(848) pqs = rng.standard_normal(size=(2, 2, 2, 7)) pqs[..., 3:] /= np.linalg.norm(pqs[..., 3:], axis=-1)[..., np.newaxis] dqs = ptr.dual_quaternions_from_pqs(pqs) dqs1 = dqs[0] dqs2 = dqs[1] dqs_result = ptr.batch_concatenate_dual_quaternions(dqs1, dqs2) for dq_result, dq1, dq2 in zip( dqs_result.reshape(-1, 8), dqs1.reshape(-1, 8), dqs2.reshape(-1, 8) ): assert_array_almost_equal( pt.concatenate_dual_quaternions(dq1, dq2), dq_result ) def test_batch_dual_quaternion_vector_product_0dims(): rng = np.random.default_rng(849) pq = rng.standard_normal(size=7) pq[3:] /= np.linalg.norm(pq[3:], axis=-1)[..., np.newaxis] dq = ptr.dual_quaternions_from_pqs(pq) v = rng.standard_normal(size=3) assert_array_almost_equal( ptr.batch_dq_prod_vector(dq, v), pt.dq_prod_vector(dq, v) ) def test_batch_dual_quaternion_vector_product_2dims(): rng = np.random.default_rng(850) pqs = rng.standard_normal(size=(3, 4, 7)) pqs[..., 3:] /= np.linalg.norm(pqs[..., 3:], axis=-1)[..., np.newaxis] dqs = ptr.dual_quaternions_from_pqs(pqs) V = rng.standard_normal(size=(3, 4, 3)) V_transformed = ptr.batch_dq_prod_vector(dqs, V) assert V_transformed.shape == (3, 4, 3) for v_t, dq, v in zip( V_transformed.reshape(-1, 3), dqs.reshape(-1, 8), V.reshape(-1, 3) ): assert_array_almost_equal(v_t, pt.dq_prod_vector(dq, v)) def test_batch_conversions_dual_quaternions_transforms_0dims(): rng = np.random.default_rng(851) pq = rng.standard_normal(size=7) pq[3:] /= np.linalg.norm(pq[3:], axis=-1)[..., np.newaxis] dq = ptr.dual_quaternions_from_pqs(pq) A2B = ptr.transforms_from_dual_quaternions(dq) dq2 = ptr.dual_quaternions_from_transforms(A2B) pt.assert_unit_dual_quaternion_equal(dq, dq2) A2B2 = ptr.transforms_from_dual_quaternions(dq2) assert_array_almost_equal(A2B, A2B2) def test_batch_conversions_dual_quaternions_transforms_3dims(): rng = np.random.default_rng(852) pqs = rng.standard_normal(size=(3, 4, 5, 7)) pqs[..., 3:] /= np.linalg.norm(pqs[..., 3:], axis=-1)[..., np.newaxis] dqs = ptr.dual_quaternions_from_pqs(pqs) A2Bs = ptr.transforms_from_dual_quaternions(dqs) dqs2 = ptr.dual_quaternions_from_transforms(A2Bs) for dq1, dq2 in zip(dqs.reshape(-1, 8), dqs2.reshape(-1, 8)): pt.assert_unit_dual_quaternion_equal(dq1, dq2) A2Bs2 = ptr.transforms_from_dual_quaternions(dqs2) assert_array_almost_equal(A2Bs, A2Bs2) def test_mirror_screw_axis(): pose = np.array( [ [0.10156069, -0.02886784, 0.99441042, 0.6753021], [-0.4892026, -0.87182166, 0.02465395, -0.2085889], [0.86623683, -0.48897203, -0.10266503, 0.30462221], [0.0, 0.0, 0.0, 1.0], ] ) exponential_coordinates = pt.exponential_coordinates_from_transform(pose) mirror_exponential_coordinates = ptr.mirror_screw_axis_direction( exponential_coordinates.reshape(1, 6) )[0] pose2 = pt.transform_from_exponential_coordinates( mirror_exponential_coordinates ) assert_array_almost_equal(pose, pose2) def test_screw_parameters_from_dual_quaternions(): case_idx0 = np.array([1, 0, 0, 0, 0, 0, 0, 0]) case_idx1 = np.array([[1.0, 0.0, 0.0, 0.0, 0.0, 0.6, 0.65, 0.7]]) dqs = np.vstack([case_idx0, case_idx1]) q, s_axis, h, theta = ptr.screw_parameters_from_dual_quaternions(dqs) assert_array_almost_equal(q[0], np.zeros(3)) assert_array_almost_equal(q[0], np.zeros(3)) assert_array_almost_equal(s_axis[0], np.array([1, 0, 0])) assert np.isinf(h[0]) assert pytest.approx(theta[0]) == 0 assert_array_almost_equal(q[1], np.zeros(3)) assert_array_almost_equal( s_axis[1], pbr.norm_vectors(np.array([1.2, 1.3, 1.4])) ) assert np.isinf(h[1]) assert pytest.approx(theta[1]) == np.linalg.norm(np.array([1.2, 1.3, 1.4])) rng = np.random.default_rng(83343) dqs = ptr.dual_quaternions_from_transforms( np.array([pt.random_transform(rng) for _ in range(10)]) ).reshape(5, 2, -1) qs, s_axes, hs, thetas = ptr.screw_parameters_from_dual_quaternions(dqs) for q, s_axis, h, theta, dq in zip( qs.reshape(10, -1), s_axes.reshape(10, -1), hs.reshape(10), thetas.reshape(10), dqs.reshape(10, -1), ): q2, s_axis2, h2, theta2 = pt.screw_parameters_from_dual_quaternion(dq) assert_array_almost_equal(q, q2) assert_array_almost_equal(s_axis, s_axis2) assert pytest.approx(h) == h2 assert pytest.approx(theta) == theta2 q, s_axis, h, theta = ptr.screw_parameters_from_dual_quaternions(case_idx0) q2, s_axis2, h2, theta2 = pt.screw_parameters_from_dual_quaternion( case_idx0 ) assert_array_almost_equal(q, q2) assert_array_almost_equal(s_axis, s_axis2) assert pytest.approx(h) == h2 assert pytest.approx(theta) == theta2 def test_dual_quaternions_from_screw_parameters(): q_0 = np.zeros(3) s_axis_0 = np.array([1, 0, 0]) h_0 = np.inf theta_0 = 0.0 q_1 = np.zeros(3) s_axis_1 = np.array([0.55297409, 0.57701644, 0.6010588]) h_1 = np.inf theta_1 = 3.6 q_2 = np.zeros(3) s_axis_2 = np.array([0.55396089, 0.5770426, 0.6001243]) h_2 = 0.0 theta_2 = 4.1 qs = np.vstack([q_0, q_1, q_2]) s_axis = np.vstack([s_axis_0, s_axis_1, s_axis_2]) hs = np.array([h_0, h_1, h_2]) thetas = np.array([theta_0, theta_1, theta_2]) dqs = ptr.dual_quaternions_from_screw_parameters(qs, s_axis, hs, thetas) pqs = ptr.pqs_from_dual_quaternions(dqs) assert_array_almost_equal(dqs[0], np.array([1, 0, 0, 0, 0, 0, 0, 0])) assert_array_almost_equal(pqs[1], np.r_[s_axis[1] * thetas[1], 1, 0, 0, 0]) assert_array_almost_equal(pqs[2, :3], [0, 0, 0]) rng = np.random.default_rng(83323) dqs = np.array( [ ptr.dual_quaternions_from_transforms(pt.random_transform(rng)) for _ in range(18) ] ) # 1D screw_parameters = ptr.screw_parameters_from_dual_quaternions(dqs[0]) pt.assert_unit_dual_quaternion_equal( dqs[0], ptr.dual_quaternions_from_screw_parameters(*screw_parameters) ) # 2D screw_parameters = ptr.screw_parameters_from_dual_quaternions(dqs) dqs2 = ptr.dual_quaternions_from_screw_parameters(*screw_parameters) for dq1, dq2 in zip(dqs, dqs2): pt.assert_unit_dual_quaternion_equal(dq1, dq2) # 3D dqs = dqs.reshape(6, 3, 8) screw_parameters = ptr.screw_parameters_from_dual_quaternions(dqs) dqs2 = ptr.dual_quaternions_from_screw_parameters(*screw_parameters) for dq1, dq2 in zip(dqs.reshape(-1, 8), dqs2.reshape(-1, 8)): pt.assert_unit_dual_quaternion_equal(dq1, dq2) def test_dual_quaternions_sclerp_same_dual_quaternions(): rng = np.random.default_rng(19) pose0 = pt.random_transform(rng) dq0 = pt.dual_quaternion_from_transform(pose0) pose1 = pt.random_transform(rng) dq1 = pt.dual_quaternion_from_transform(pose1) dqs = np.vstack([dq0, dq1]) ts = np.array([0.5, 0.8]) dqs_res = ptr.dual_quaternions_sclerp(dqs, dqs, ts) assert_array_almost_equal(dqs, dqs_res) with pytest.raises(ValueError, match="must have the same shape"): ptr.dual_quaternions_sclerp(dqs, dqs[:-1], ts) with pytest.raises(ValueError, match="same number of elements"): ptr.dual_quaternions_sclerp(dqs, dqs, ts[:-1]) with pytest.raises(ValueError, match="same number of elements"): ptr.dual_quaternions_sclerp(dqs, dqs, ts[0]) def test_dual_quaternions_sclerp(): rng = np.random.default_rng(4832238) Ts = np.array([pt.random_transform(rng) for _ in range(20)]) # 3D dqs_start = ptr.dual_quaternions_from_transforms(Ts).reshape(5, 4, -1) dqs_end = ptr.dual_quaternions_from_transforms(Ts).reshape(5, 4, -1) ts = np.linspace(0, 1, 20).reshape(5, 4) dqs_int = ptr.dual_quaternions_sclerp(dqs_start, dqs_end, ts) assert dqs_int.shape == (5, 4, 8) for dq_start, dq_end, t, dq_int in zip( dqs_start.reshape(-1, 8), dqs_end.reshape(-1, 8), ts.reshape(-1), dqs_int.reshape(-1, 8), ): pt.assert_unit_dual_quaternion_equal( dq_int, pt.dual_quaternion_sclerp(dq_start, dq_end, t) ) # 1D pt.assert_unit_dual_quaternion_equal( pt.dual_quaternion_sclerp(dqs_start[0, 0], dqs_end[0, 0], ts[0, 0]), ptr.dual_quaternions_sclerp(dqs_start[0, 0], dqs_end[0, 0], ts[0, 0]), ) def test_batch_dq_q_conj(): rng = np.random.default_rng(48338) Ts = np.array([pt.random_transform(rng) for _ in range(20)]) dqs = ptr.dual_quaternions_from_transforms(Ts).reshape(5, 4, -1) # 1D assert_array_almost_equal( pt.transform_from_dual_quaternion( pt.concatenate_dual_quaternions( dqs[0, 0], ptr.batch_dq_q_conj(dqs[0, 0]) ) ), np.eye(4), ) # 3D dqs_inv = ptr.batch_dq_q_conj(dqs) dqs_id = ptr.batch_concatenate_dual_quaternions(dqs, dqs_inv) Is = ptr.transforms_from_dual_quaternions(dqs_id) for I in Is.reshape(-1, 4, 4): assert_array_almost_equal(I, np.eye(4)) def test_random_trajectories(): rng = np.random.default_rng(3427) start = pt.random_transform(rng) goal = pt.random_transform(rng) trajectories = ptr.random_trajectories( rng, n_trajectories=7, n_steps=132, start=start, goal=goal, scale=[100] * 6, ) assert trajectories.shape == (7, 132, 4, 4) assert_array_almost_equal(trajectories[0, 0], start, decimal=2) assert_array_almost_equal(trajectories[0, -1], goal, decimal=2) # check scaling, we assume start and goal are equal so that the linear # movement from start to goal does not interfere with the random motion rng = np.random.default_rng(3429) trajectories1 = ptr.random_trajectories( rng, n_trajectories=3, n_steps=20, scale=[1.0] * 6 ) rng = np.random.default_rng(3429) trajectories2 = ptr.random_trajectories( rng, n_trajectories=3, n_steps=20, scale=[2.0] * 6 ) Sthetas1 = ptr.exponential_coordinates_from_transforms(trajectories1) acc1 = np.abs( np.gradient( np.gradient(Sthetas1, axis=1, edge_order=1), axis=1, edge_order=1 ) ) Sthetas2 = ptr.exponential_coordinates_from_transforms(trajectories2) acc2 = np.abs( np.gradient( np.gradient(Sthetas2, axis=1, edge_order=1), axis=1, edge_order=1 ) ) assert np.all(acc1 <= acc2) ================================================ FILE: pytransform3d/transform_manager/__init__.py ================================================ """Manage complex chains of transformations. See :doc:`user_guide/transform_manager` for more information. """ from ._transform_graph_base import TransformGraphBase from ._transform_manager import TransformManager from ._temporal_transform_manager import ( StaticTransform, TimeVaryingTransform, TemporalTransformManager, NumpyTimeseriesTransform, ) __all__ = [ "TransformGraphBase", "TransformManager", "TemporalTransformManager", "TimeVaryingTransform", "StaticTransform", "NumpyTimeseriesTransform", ] ================================================ FILE: pytransform3d/transform_manager/_temporal_transform_manager.py ================================================ """Temporal transform manager.""" import abc import numpy as np from ._transform_graph_base import TransformGraphBase from ..batch_rotations import norm_vectors from ..trajectories import ( dual_quaternions_from_pqs, pqs_from_dual_quaternions, dual_quaternions_sclerp, transforms_from_pqs, concat_dynamic, invert_transforms, ) from ..transformations import check_transform class TimeVaryingTransform(abc.ABC): """Time-varying rigid transformation. You have to inherit from this abstract base class to use the TemporalTransformManager. Two implementations of the interface that are already available are :class:`StaticTransform` and :class:`NumpyTimeseriesTransform`. """ @abc.abstractmethod def as_matrix(self, query_time): """Get transformation matrix at given time. Parameters ---------- query_time : Union[float, array-like shape (n_queries,)] Query time Returns ------- A2B_t : array, shape (4, 4) or (n_queries, 4, 4) Homogeneous transformation matrix at given time or times """ @abc.abstractmethod def check_transforms(self): """Checks all transformations. Returns ------- self : TimeVaryingTransform Validated transformations. """ class StaticTransform(TimeVaryingTransform): """Transformation, which does not change over time. Parameters ---------- A2B : array-like, shape (4, 4) Homogeneous transformation matrix. """ def __init__(self, A2B): self._A2B = A2B def as_matrix(self, query_time): return self._A2B def check_transforms(self): self._A2B = check_transform(self._A2B) return self class NumpyTimeseriesTransform(TimeVaryingTransform): """Transformation sequence, represented in a numpy array. The interpolation is computed using screw linear interpolation (ScLERP) method. Parameters ---------- time: array, shape (n_steps,) Numeric timesteps corresponding to the transformation samples. You can use, for example, unix timestamps, relative time (starting with 0). pqs : array, shape (n_steps, 7) Time-sequence of transformations, with each row representing a single sample as position-quarternion (PQ) structure. time_clipping : bool, optional (default: False) Clip time to minimum or maximum respectively when query time is out of range of the time series. If this deactivated, we will raise a ValueError when the query time is out of range. """ def __init__(self, time, pqs, time_clipping=False): self.time = np.asarray(time) self._pqs = np.asarray(pqs) self.time_clipping = time_clipping if len(self._pqs.shape) != 2: raise ValueError("Shape of PQ array must be 2-dimensional.") if self.time.size != self._pqs.shape[0]: raise ValueError( "Number of timesteps does not equal to number of PQ samples" ) if self._pqs.shape[1] != 7: raise ValueError("`pqs` matrix shall have 7 columns.") self._min_time = min(self.time) self._max_time = max(self.time) def as_matrix(self, query_time): """Get transformation matrix at given time. Parameters ---------- query_time : Union[float, array-like shape (...)] Query time Returns ------- A2B_t : array, shape (4, 4) or (..., 4, 4) Homogeneous transformation matrix / matrices at given time / times. """ query_time = np.asarray(query_time) pq = self._interpolate_pq_using_sclerp(np.atleast_1d(query_time)) transforms = transforms_from_pqs(pq) output_shape = list(query_time.shape) + [4, 4] return transforms.reshape(*output_shape) def check_transforms(self): self._pqs[:, 3:] = norm_vectors(self._pqs[:, 3:]) return self def _interpolate_pq_using_sclerp(self, query_time_arr): # identify the index of the preceding sample idxs_timestep_earlier_wrt_query_time = ( np.searchsorted(self.time, query_time_arr, side="right") - 1 ) # deal with first and last timestamp before_start = query_time_arr < self._min_time after_end = query_time_arr > self._max_time out_of_range = np.logical_or(before_start, after_end) if self.time_clipping: min_index = 0 max_index = self.time.shape[0] - 2 idxs_timestep_earlier_wrt_query_time = np.clip( idxs_timestep_earlier_wrt_query_time, min_index, max_index ) elif any(out_of_range): indices = np.where(out_of_range)[0] times = query_time_arr[indices] raise ValueError( "Query time at indices %s, time(s): %s is/are out of range of " "time series." % (indices, times) ) idxs_timestep_later_wrt_query_time = ( idxs_timestep_earlier_wrt_query_time + 1 ) if self.time_clipping: before_or_eq_start = query_time_arr <= self._min_time after_or_eq_end = query_time_arr >= self._max_time idxs_timestep_later_wrt_query_time[before_or_eq_start] = ( idxs_timestep_earlier_wrt_query_time[before_or_eq_start] ) idxs_timestep_earlier_wrt_query_time[after_or_eq_end] = ( idxs_timestep_later_wrt_query_time[after_or_eq_end] ) # dual quaternion from preceding sample times_prev = self.time[idxs_timestep_earlier_wrt_query_time] pqs_prev = self._pqs[idxs_timestep_earlier_wrt_query_time] dqs_prev = dual_quaternions_from_pqs(pqs_prev) # dual quaternion from successive sample times_next = self.time[idxs_timestep_later_wrt_query_time] pqs_next = self._pqs[idxs_timestep_later_wrt_query_time] dqs_next = dual_quaternions_from_pqs(pqs_next) # For each query timestamp, determine its normalized time distance # between the preceeding and succeeding sample, since ScLERP works with # relative times t in [0, 1]. If query time equals a time sample # exactly, 0.0 is set. rel_times_between_samples = np.zeros_like(query_time_arr, dtype=float) interpolation_case = times_prev != times_next rel_times_between_samples[interpolation_case] = ( query_time_arr[interpolation_case] - times_prev[interpolation_case] ) / (times_next[interpolation_case] - times_prev[interpolation_case]) dqs_interpolated = dual_quaternions_sclerp( dqs_prev, dqs_next, rel_times_between_samples ) return pqs_from_dual_quaternions(dqs_interpolated) class TemporalTransformManager(TransformGraphBase): """Manage time-varying transformations. See :ref:`transformations_over_time` for more information. Parameters ---------- strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. check : bool, optional (default: True) Check if transformation matrices are valid and requested nodes exist, which might significantly slow down some operations. """ def __init__(self, strict_check=True, check=True): super(TemporalTransformManager, self).__init__(strict_check, check) self._transforms = {} self._current_time = np.array([0.0]) @property def current_time(self): """Current time at which we evaluate transformations.""" return self._current_time @current_time.setter def current_time(self, time): """Set current time at which we evaluate transformations.""" self._current_time = time @property def transforms(self): """Rigid transformations between nodes.""" return { transform_key: transform.as_matrix(self.current_time) for transform_key, transform in self._transforms.items() } def get_transform_at_time(self, from_frame, to_frame, time): """Request a transformation at a given time. Parameters ---------- from_frame : Hashable Name of the frame for which the transformation is requested in the to_frame coordinate system to_frame : Hashable Name of the frame in which the transformation is defined time : Union[float, array-like shape (...)] Time or times at which we request the transformation. If the query time is out of bounds, it will be clipped to either the first or last available time. Returns ------- A2B : array, shape (4, 4) Transformation from 'from_frame' to 'to_frame' Raises ------ KeyError If one of the frames is unknown or there is no connection between them """ previous_time = self.current_time self.current_time = time A2B = self.get_transform(from_frame, to_frame) # revert internal state self.current_time = previous_time return A2B def get_transform(self, from_frame, to_frame): """Request a transformation. The internal current_time will be used for time based transformations. If the query time is out of bounds, it will be clipped to either the first or the last available time. Parameters ---------- from_frame : Hashable Name of the frame for which the transformation is requested in the to_frame coordinate system to_frame : Hashable Name of the frame in which the transformation is defined Returns ------- A2B : Any Transformation from 'from_frame' to 'to_frame' Raises ------ KeyError If one of the frames is unknown or there is no connection between them """ if self.check: if from_frame not in self.nodes: raise KeyError("Unknown frame '%s'" % from_frame) if to_frame not in self.nodes: raise KeyError("Unknown frame '%s'" % to_frame) if self._transform_available((from_frame, to_frame)): return self._get_transform((from_frame, to_frame)) if self._transform_available((to_frame, from_frame)): return invert_transforms( self._get_transform((to_frame, from_frame)) ) i = self.nodes.index(from_frame) j = self.nodes.index(to_frame) if not np.isfinite(self.dist[i, j]): raise KeyError( "Cannot compute path from frame '%s' to frame '%s'." % (from_frame, to_frame) ) path = self._shortest_path(i, j) return self._path_transform(path) def _transform_available(self, key): return key in self._transforms def _set_transform(self, key, A2B): self._transforms[key] = A2B def _get_transform(self, key): return self._transforms[key].as_matrix(self._current_time) def _del_transform(self, key): del self._transforms[key] def _check_transform(self, A2B): return A2B.check_transforms() def _path_transform(self, path): """Convert sequence of node names to rigid transformation.""" A2B = np.eye(4) for from_f, to_f in zip(path[:-1], path[1:]): A2B = concat_dynamic(A2B, self.get_transform(from_f, to_f)) return A2B ================================================ FILE: pytransform3d/transform_manager/_temporal_transform_manager.pyi ================================================ import abc from typing import Dict, Tuple, Hashable, Any, Union import numpy as np import numpy.typing as npt from ._transform_graph_base import TransformGraphBase class TimeVaryingTransform(abc.ABC): @abc.abstractmethod def as_matrix(self, query_time: Union[float, np.ndarray]) -> np.ndarray: ... @abc.abstractmethod def check_transforms(self) -> "TimeVaryingTransform": ... class StaticTransform(TimeVaryingTransform): _A2B: npt.ArrayLike def __init__(self, A2B: npt.ArrayLike): ... def as_matrix(self, query_time: Union[float, np.ndarray]) -> np.ndarray: ... def check_transforms(self) -> "StaticTransform": ... class NumpyTimeseriesTransform(TimeVaryingTransform): time: np.ndarray _pqs: np.ndarray time_clipping: bool _min_time: float _max_time: float def __init__( self, time: npt.ArrayLike, pqs: npt.ArrayLike, time_clipping: bool = ... ): ... def as_matrix(self, query_time: Union[float, np.ndarray]) -> np.ndarray: ... def check_transforms(self) -> "NumpyTimeseriesTransform": ... def _interpolate_pq_using_sclerp( self, query_time: Union[float, np.ndarray] ) -> np.ndarray: ... class TemporalTransformManager(TransformGraphBase): _transforms: Dict[Tuple[Hashable, Hashable], TimeVaryingTransform] _current_time: Union[float, np.ndarray] def __init__(self, strict_check: bool = ..., check: bool = ...): ... @property def transforms(self) -> Dict[Tuple[Hashable, Hashable], np.ndarray]: ... @property def current_time(self) -> Union[float, np.ndarray]: ... @current_time.setter def current_time(self, query_time: Union[float, np.ndarray]): ... def _check_transform( self, A2B: TimeVaryingTransform ) -> TimeVaryingTransform: ... def _transform_available(self, key: Tuple[Hashable, Hashable]) -> bool: ... def _set_transform( self, key: Tuple[Hashable, Hashable], A2B: TimeVaryingTransform ): ... def _get_transform(self, key: Tuple[Hashable, Hashable]) -> Any: ... def _del_transform(self, key: Tuple[Hashable, Hashable]): ... def add_transform( self, from_frame: Hashable, to_frame: Hashable, A2B: TimeVaryingTransform, ) -> "TemporalTransformManager": ... def get_transform( self, from_frame: Hashable, to_frame: Hashable ) -> np.ndarray: ... def get_transform_at_time( self, from_frame: Hashable, to_frame: Hashable, query_time: Union[float, npt.ArrayLike], ) -> np.ndarray: ... ================================================ FILE: pytransform3d/transform_manager/_transform_graph_base.py ================================================ """Common base class of transformation graphs.""" import abc import copy import numpy as np import scipy.sparse as sp from scipy.sparse import csgraph from ..transformations import concat, invert_transform class TransformGraphBase(abc.ABC): """Base class for all graphs of rigid transformations. Parameters ---------- strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. check : bool, optional (default: True) Check if transformation matrices are valid and requested nodes exist, which might significantly slow down some operations. """ def __init__(self, strict_check=True, check=True): self.strict_check = strict_check self.check = check # Names of nodes self.nodes = [] # A pair (self.i[n], self.j[n]) represents indices of connected nodes self.i = [] self.j = [] # We have to store the index n associated to a transformation to be # able to remove the transformation later self.transform_to_ij_index = {} # Connection information as sparse matrix self.connections = sp.csr_matrix((0, 0)) # Result of shortest path algorithm: # distance matrix (distance is the number of transformations) self.dist = np.empty(0) self.predecessors = np.empty(0, dtype=np.int32) self._cached_shortest_paths = {} @property @abc.abstractmethod def transforms(self): """Rigid transformations between nodes.""" @abc.abstractmethod def _check_transform(self, A2B): """Check validity of rigid transformation.""" def _path_transform(self, path): """Convert sequence of node names to rigid transformation.""" A2B = np.eye(4) for from_f, to_f in zip(path[:-1], path[1:]): A2B = concat( A2B, self.get_transform(from_f, to_f), strict_check=self.strict_check, check=self.check, ) return A2B @abc.abstractmethod def _transform_available(self, key): """Check if transformation key is available.""" @abc.abstractmethod def _set_transform(self, key, A2B): """Store transformation under given key.""" @abc.abstractmethod def _get_transform(self, key): """Retrieve stored transformation under given key.""" @abc.abstractmethod def _del_transform(self, key): """Delete transformation stored under given key.""" def has_frame(self, frame): """Check if frame has been registered. Parameters ---------- frame : Hashable Frame name Returns ------- has_frame : bool Frame is registered """ return frame in self.nodes def add_transform(self, from_frame, to_frame, A2B): """Register a transformation. Parameters ---------- from_frame : Hashable Name of the frame for which the transformation is added in the to_frame coordinate system to_frame : Hashable Name of the frame in which the transformation is defined A2B : Any Transformation from 'from_frame' to 'to_frame' Returns ------- self : TransformManager This object for chaining """ if self.check: A2B = self._check_transform(A2B) if from_frame not in self.nodes: self.nodes.append(from_frame) if to_frame not in self.nodes: self.nodes.append(to_frame) transform_key = (from_frame, to_frame) recompute_shortest_path = False if not self._transform_available(transform_key): ij_index = len(self.i) self.i.append(self.nodes.index(from_frame)) self.j.append(self.nodes.index(to_frame)) self.transform_to_ij_index[transform_key] = ij_index recompute_shortest_path = True if recompute_shortest_path: self._recompute_shortest_path() self._set_transform(transform_key, A2B) return self def _recompute_shortest_path(self): n_nodes = len(self.nodes) self.connections = sp.csr_matrix( (np.zeros(len(self.i)), (self.i, self.j)), shape=(n_nodes, n_nodes) ) self.dist, self.predecessors = csgraph.shortest_path( self.connections, unweighted=True, directed=False, method="D", return_predecessors=True, ) self._cached_shortest_paths.clear() def _find_connected_transforms(self, frame): """Find all transformations connected to a frame.""" connected_transforms = [] for from_frame, to_frame in self.transform_to_ij_index.keys(): if from_frame == frame or to_frame == frame: connected_transforms.append((from_frame, to_frame)) return connected_transforms def remove_transform(self, from_frame, to_frame): """Remove a transformation. Nothing happens if there is no such transformation. Parameters ---------- from_frame : Hashable Name of the frame for which the transformation is added in the to_frame coordinate system to_frame : Hashable Name of the frame in which the transformation is defined Returns ------- self : TransformManager This object for chaining """ transform_key = (from_frame, to_frame) if self._transform_available(transform_key): self._del_transform(transform_key) ij_index = self.transform_to_ij_index.pop(transform_key) self.transform_to_ij_index = { k: v if v < ij_index else v - 1 for k, v in self.transform_to_ij_index.items() } del self.i[ij_index], self.j[ij_index] self._recompute_shortest_path() return self def remove_frame(self, frame): """Remove a frame (node) from the graph. Parameters ---------- frame : Hashable The frame to remove. Returns ------- self : TransformManager This object for chaining. """ if frame not in self.nodes: raise KeyError(f"Frame '{frame}' is not in the graph.") # Remove all transformations (edges) associated with the frame for from_frame, to_frame in self._find_connected_transforms(frame): self.remove_transform(from_frame, to_frame) frame_index = self.nodes.index(frame) self.nodes.pop(frame_index) # Adjust the connection indices in self.i and self.j self.i = [ index if index < frame_index else index - 1 for index in self.i ] self.j = [ index if index < frame_index else index - 1 for index in self.j ] # Update the transform_to_ij_index dictionary self.transform_to_ij_index = { nodes: ij_index for nodes, ij_index in self.transform_to_ij_index.items() if frame not in nodes } self._recompute_shortest_path() return self def get_transform(self, from_frame, to_frame): """Request a transformation. Parameters ---------- from_frame : Hashable Name of the frame for which the transformation is requested in the to_frame coordinate system to_frame : Hashable Name of the frame in which the transformation is defined Returns ------- A2B : Any Transformation from 'from_frame' to 'to_frame' Raises ------ KeyError If one of the frames is unknown or there is no connection between them """ if self.check: if from_frame not in self.nodes: raise KeyError("Unknown frame '%s'" % from_frame) if to_frame not in self.nodes: raise KeyError("Unknown frame '%s'" % to_frame) if self._transform_available((from_frame, to_frame)): return self._get_transform((from_frame, to_frame)) if self._transform_available((to_frame, from_frame)): return invert_transform( self._get_transform((to_frame, from_frame)), strict_check=self.strict_check, check=self.check, ) i = self.nodes.index(from_frame) j = self.nodes.index(to_frame) if not np.isfinite(self.dist[i, j]): raise KeyError( "Cannot compute path from frame '%s' to " "frame '%s'." % (from_frame, to_frame) ) path = self._shortest_path(i, j) return self._path_transform(path) def _shortest_path(self, i, j): """Names of nodes along the shortest path between two indices.""" if (i, j) in self._cached_shortest_paths: return self._cached_shortest_paths[(i, j)] path = [] k = i while k != -9999: path.append(self.nodes[k]) k = self.predecessors[j, k] self._cached_shortest_paths[(i, j)] = path return path def connected_components(self): """Get number of connected components. If the number is larger than 1 there will be frames without connections. Returns ------- n_connected_components : int Number of connected components. """ return csgraph.connected_components( self.connections, directed=False, return_labels=False ) def check_consistency(self): """Check consistency of the known transformations. The computational cost of this operation is very high. Returns ------- consistent : bool Is the graph consistent, i.e., if there are two ways of computing A2B, do they give almost identical results? """ for (from_frame, to_frame), A2B in self.transforms.items(): clone = copy.deepcopy(self) clone.remove_transform(from_frame, to_frame) try: A2B_from_path = clone.get_transform(from_frame, to_frame) if not np.allclose(A2B, A2B_from_path): return False except KeyError: # A2B cannot be computed in any other way continue return True ================================================ FILE: pytransform3d/transform_manager/_transform_graph_base.pyi ================================================ import abc from typing import Dict, Tuple, List, Hashable, Any import scipy.sparse as sp import numpy as np import numpy.typing as npt class TransformGraphBase(abc.ABC): strict_check: bool check: bool nodes: List[Hashable] i: List[int] j: List[int] transform_to_ij_index = Dict[Tuple[Hashable, Hashable], int] connections: sp.csr_matrix dist: np.ndarray predecessors: np.ndarray _cached_shortest_paths: Dict[Tuple[int, int], List[Hashable]] def __init__(self, strict_check: bool = ..., check: bool = ...): ... @property @abc.abstractmethod def transforms(self) -> Dict[Tuple[Hashable, Hashable], Any]: ... @abc.abstractmethod def _transform_available(self, key: Tuple[Hashable, Hashable]) -> bool: ... @abc.abstractmethod def _set_transform(self, key: Tuple[Hashable, Hashable], A2B: Any): ... @abc.abstractmethod def _get_transform(self, key: Tuple[Hashable, Hashable]) -> Any: ... @abc.abstractmethod def _del_transform(self, key: Tuple[Hashable, Hashable]): ... @abc.abstractmethod def _check_transform(self, A2B: Any) -> Any: ... def _path_transform(self, path: List[Hashable]) -> np.ndarray: ... def has_frame(self, frame: Hashable) -> bool: ... def add_transform( self, from_frame: Hashable, to_frame: Hashable, A2B: Any ) -> "TransformGraphBase": ... def _recompute_shortest_path(self): ... def _find_connected_transforms(self, frame: Hashable) -> List[Hashable]: ... def remove_transform( self, from_frame: Hashable, to_frame: Hashable ) -> "TransformGraphBase": ... def remove_frame(self, frame: Hashable) -> "TransformGraphBase": ... def get_transform( self, from_frame: Hashable, to_frame: Hashable ) -> np.ndarray: ... def _shortest_path(self, i: int, j: int) -> List[Hashable]: ... def connected_components(self) -> int: ... def check_consistency(self) -> bool: ... ================================================ FILE: pytransform3d/transform_manager/_transform_manager.py ================================================ """Transform manager.""" import numpy as np import scipy.sparse as sp from ._transform_graph_base import TransformGraphBase from ..transformations import check_transform, plot_transform try: # pragma: no cover import pydot PYDOT_AVAILABLE = True except ImportError: PYDOT_AVAILABLE = False class TransformManager(TransformGraphBase): """Manage transformations between frames. This is a simplified version of `ROS tf `_ [1]_ that ignores the temporal aspect. A user can register transformations. The shortest path between all frames will be computed internally which enables us to provide transforms for any connected frames. Suppose we know the transformations A2B, D2C, and B2C. The transform manager can compute any transformation between the frames A, B, C and D. For example, you can request the transformation that represents frame D in frame A. The transformation manager will automatically concatenate the transformations D2C, C2B, and B2A, where C2B and B2A are obtained by inverting B2C and A2B respectively. .. warning:: It is possible to introduce inconsistencies in the transformation manager. Adding A2B and B2A with inconsistent values will result in an invalid state because inconsistencies will not be checked. It seems to be trivial in this simple case but can be computationally complex for large graphs. You can check the consistency explicitly with :func:`TransformManager.check_consistency`. The TransformManager does not directly support serialization because we don't want to decide for a specific format. However, it allows conversion to a dict with only primitive types that is serializable, for instance, as JSON. If a more compact format is required, binary formats like msgpack can be used. Use :func:`TransformManager.to_dict` and :func:`TransformManager.from_dict` for this purpose. Parameters ---------- strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. check : bool, optional (default: True) Check if transformation matrices are valid and requested nodes exist, which might significantly slow down some operations. References ---------- .. [1] Foote, T. (2013). tf: The transform library. In IEEE Conference on Technologies for Practical Robot Applications (TePRA), Woburn, MA, USA, pp. 1-6, doi: 10.1109/TePRA.2013.6556373. """ def __init__(self, strict_check=True, check=True): super(TransformManager, self).__init__(strict_check, check) self._transforms = {} @property def transforms(self): """Rigid transformations between nodes.""" return self._transforms def _check_transform(self, A2B): """Check validity of rigid transformation.""" return check_transform(A2B, strict_check=self.strict_check) def _transform_available(self, key): return key in self._transforms def _set_transform(self, key, A2B): self._transforms[key] = A2B def _get_transform(self, key): return self._transforms[key] def _del_transform(self, key): del self._transforms[key] def plot_frames_in( self, frame, ax=None, s=1.0, ax_s=1, show_name=True, whitelist=None, **kwargs, ): # pragma: no cover """Plot all frames in a given reference frame. Note that frames that cannot be connected to the reference frame are omitted. Parameters ---------- frame : Hashable Reference frame ax : Matplotlib 3d axis, optional (default: None) If the axis is None, a new 3d axis will be created s : float, optional (default: 1) Scaling of the frame that will be drawn ax_s : float, optional (default: 1) Scaling of the new matplotlib 3d axis show_name : bool, optional (default: True) Print node names whitelist : list, optional (default: None) Frames that must be plotted kwargs : dict, optional (default: {}) Additional arguments for the plotting functions, e.g. alpha Returns ------- ax : Matplotlib 3d axis New or old axis Raises ------ KeyError If the frame is unknown """ if frame not in self.nodes: raise KeyError("Unknown frame '%s'" % frame) nodes = self._whitelisted_nodes(whitelist) for node in nodes: try: node2frame = self.get_transform(node, frame) name = node if show_name else None ax = plot_transform( ax, node2frame, s, ax_s, name, strict_check=self.strict_check, **kwargs, ) except KeyError: pass # Frame is not connected to the reference frame return ax def plot_connections_in( self, frame, ax=None, ax_s=1, whitelist=None, **kwargs ): # pragma: no cover """Plot direct frame connections in a given reference frame. A line between each pair of frames for which a direct transformation is known will be plotted. Direct means that either A2B or B2A has been added to the transformation manager. Note that frames that cannot be connected to the reference frame are omitted. Parameters ---------- frame : Hashable Reference frame ax : Matplotlib 3d axis, optional (default: None) If the axis is None, a new 3d axis will be created ax_s : float, optional (default: 1) Scaling of the new matplotlib 3d axis whitelist : list, optional (default: None) Both frames of a connection must be in the whitelist to plot the connection kwargs : dict, optional (default: {}) Additional arguments for the plotting functions, e.g. alpha Returns ------- ax : Matplotlib 3d axis New or old axis Raises ------ KeyError If the frame is unknown """ if frame not in self.nodes: raise KeyError("Unknown frame '%s'" % frame) if ax is None: from ..plot_utils import make_3d_axis ax = make_3d_axis(ax_s) nodes = self._whitelisted_nodes(whitelist) if "c" not in kwargs and "color" not in kwargs: kwargs["color"] = "black" for frame_names in self._transforms: from_frame, to_frame = frame_names if from_frame in nodes and to_frame in nodes: try: from2ref = self.get_transform(from_frame, frame) to2ref = self.get_transform(to_frame, frame) ax.plot( (from2ref[0, 3], to2ref[0, 3]), (from2ref[1, 3], to2ref[1, 3]), (from2ref[2, 3], to2ref[2, 3]), **kwargs, ) except KeyError: pass # Frame is not connected to the reference frame return ax def _whitelisted_nodes(self, whitelist): """Get whitelisted nodes. Parameters ---------- whitelist : list or None Whitelist of frames Returns ------- nodes : set Existing whitelisted nodes Raises ------ KeyError Will be raised if an unknown node is in the whitelist. """ nodes = set(self.nodes) if whitelist is not None: whitelist = set(whitelist) nodes = nodes.intersection(whitelist) nonwhitlisted_nodes = whitelist.difference(nodes) if nonwhitlisted_nodes: raise KeyError( "Whitelist contains unknown nodes: '%s'" % nonwhitlisted_nodes ) return nodes def write_png(self, filename, prog=None): # pragma: no cover """Create PNG from dot graph of the transformations. .. warning:: Note that this method requires the Python package pydot and an existing installation of graphviz on your system. Parameters ---------- filename : str Name of the output file. Should end with '.png'. prog : str, optional (default: dot) Name of GraphViz executable that can be found in the `$PATH` or absolute path to GraphViz executable. Possible options are, for example, 'dot', 'twopi', 'neato', 'circo', 'fdp', 'sfdp'. Raises ------ ImportError If pydot is not available """ if not PYDOT_AVAILABLE: raise ImportError("pydot must be installed to use this feature.") graph = pydot.Dot(graph_type="graph") frame_color = "#dd3322" connection_color = "#d0d0ff" for frame in self.nodes: node = pydot.Node( _dot_display_name(str(frame)), style="filled", fillcolor=frame_color, shape="egg", ) graph.add_node(node) for frames, A2B in self._transforms.items(): frame_a, frame_b = frames connection_name = "%s to %s\n%s" % ( _dot_display_name(str(frame_a)), _dot_display_name(str(frame_b)), str(np.round(A2B, 3)), ) node = pydot.Node( connection_name, style="filled", fillcolor=connection_color, shape="note", ) graph.add_node(node) a_name = _dot_display_name(str(frame_a)) a_edge = pydot.Edge(connection_name, a_name, penwidth=3) graph.add_edge(a_edge) b_name = _dot_display_name(str(frame_b)) b_edge = pydot.Edge(connection_name, b_name, penwidth=3) graph.add_edge(b_edge) graph.write_png(filename, prog=prog) def to_dict(self): """Convert the transform manager to a dict that is serializable. Returns ------- tm_dict : dict Serializable dict. """ return { "class": self.__class__.__name__, "strict_check": self.strict_check, "check": self.check, "transforms": [ (k, v.ravel().tolist()) for k, v in self._transforms.items() ], "nodes": self.nodes, "i": self.i, "j": self.j, "transform_to_ij_index": list(self.transform_to_ij_index.items()), "connections": { "data": self.connections.data.tolist(), "indices": self.connections.indices.tolist(), "indptr": self.connections.indptr.tolist(), }, "dist": self.dist.tolist(), "predecessors": self.predecessors.tolist(), } @staticmethod def from_dict(tm_dict): """Create transform manager from dict. Parameters ---------- tm_dict : dict Serializable dict. Returns ------- tm : TransformManager Deserialized transform manager. """ strict_check = tm_dict.get("strict_check") check = tm_dict.get("check") tm = TransformManager(strict_check=strict_check, check=check) tm.set_transform_manager_state(tm_dict) return tm def set_transform_manager_state(self, tm_dict): """Set state of transform manager from dict. Parameters ---------- tm_dict : dict Serializable dict. """ transforms = tm_dict.get("transforms") self._transforms = { tuple(k): np.array(v).reshape(4, 4) for k, v in transforms } self.nodes = tm_dict.get("nodes") self.i = tm_dict.get("i") self.j = tm_dict.get("j") self.transform_to_ij_index = dict( (tuple(k), v) for k, v in tm_dict.get("transform_to_ij_index") ) connections = tm_dict.get("connections") self.connections = sp.csr_matrix( (connections["data"], connections["indices"], connections["indptr"]) ) n_nodes = len(self.nodes) dist = np.array(tm_dict.get("dist")) self.dist = dist.reshape(n_nodes, n_nodes) predecessors = np.array(tm_dict.get("predecessors"), dtype=np.int32) self.predecessors = predecessors.reshape(n_nodes, n_nodes) def _dot_display_name(name): # pragma: no cover return name.replace("/", "") ================================================ FILE: pytransform3d/transform_manager/_transform_manager.pyi ================================================ from typing import Dict, Tuple, List, Union, Set, Hashable, Any import numpy as np import numpy.typing as npt from mpl_toolkits.mplot3d import Axes3D from ._transform_graph_base import TransformGraphBase PYDOT_AVAILABLE: bool class TransformManager(TransformGraphBase): _transforms: Dict[Tuple[Hashable, Hashable], np.ndarray] def __init__(self, strict_check: bool = ..., check: bool = ...): ... @property def transforms(self) -> Dict[Tuple[Hashable, Hashable], np.ndarray]: ... def _check_transform(self, A2B: npt.ArrayLike) -> np.ndarray: ... def _transform_available(self, key: Tuple[Hashable, Hashable]) -> bool: ... def _set_transform(self, key: Tuple[Hashable, Hashable], A2B: Any): ... def _get_transform(self, key: Tuple[Hashable, Hashable]) -> Any: ... def _del_transform(self, key: Tuple[Hashable, Hashable]): ... def add_transform( self, from_frame: Hashable, to_frame: Hashable, A2B: np.ndarray ) -> "TransformManager": ... def get_transform( self, from_frame: Hashable, to_frame: Hashable ) -> np.ndarray: ... def plot_frames_in( self, frame: Hashable, ax: Union[None, Axes3D] = ..., s: float = ..., ax_s: float = ..., show_name: bool = ..., whitelist: Union[List[str], None] = ..., **kwargs, ) -> Axes3D: ... def plot_connections_in( self, frame: Hashable, ax: Union[None, Axes3D] = ..., ax_s: float = ..., whitelist: Union[List[Hashable], None] = ..., **kwargs, ) -> Axes3D: ... def _whitelisted_nodes( self, whitelist: Union[None, List[Hashable]] ) -> Set[Hashable]: ... def write_png(self, filename: str, prog: Union[str, None] = ...): ... def to_dict(self) -> Dict[str, Any]: ... @staticmethod def from_dict(tm_dict: Dict[str, Any]) -> "TransformManager": ... def set_transform_manager_state(self, tm_dict: Dict[str, Any]): ... ================================================ FILE: pytransform3d/transform_manager/test/test_temporal_transform_manager.py ================================================ import numpy as np import pytest from numpy.testing import assert_array_almost_equal import pytransform3d.rotations as pr import pytransform3d.transformations as pt from pytransform3d.transform_manager import ( TemporalTransformManager, StaticTransform, NumpyTimeseriesTransform, ) def create_sinusoidal_movement( duration_sec, sample_period, x_velocity, y_start_offset, start_time ): """Create a planar (z=0) sinusoidal movement around x-axis.""" time_arr = np.arange(0, duration_sec, sample_period) + start_time N = len(time_arr) x_arr = np.linspace(0, x_velocity * duration_sec, N) spatial_freq = 1 / 5 # 1 sinus per 5m omega = 2 * np.pi * spatial_freq y_arr = np.sin(omega * x_arr) y_arr += y_start_offset dydx_arr = omega * np.cos(omega * x_arr) yaw_arr = np.arctan2(dydx_arr, np.ones_like(dydx_arr)) pq_arr = list() for i in range(N): R = pr.active_matrix_from_extrinsic_euler_zyx([yaw_arr[i], 0, 0]) T = pt.transform_from(R, [x_arr[i], y_arr[i], 0]) pq = pt.pq_from_transform(T) pq_arr.append(pq) return time_arr, np.array(pq_arr) def test_temporal_transform(): rng = np.random.default_rng(0) A2B = pt.random_transform(rng) rng = np.random.default_rng(42) A2C = pt.random_transform(rng) tm = TemporalTransformManager() tm.add_transform("A", "B", StaticTransform(A2B)) tm.add_transform("A", "C", StaticTransform(A2C)) tm.current_time = 1234.0 B2C = tm.get_transform("B", "C") C2B = tm.get_transform("C", "B") B2C_2 = pt.invert_transform(C2B) assert_array_almost_equal(B2C, B2C_2) B2C_3 = tm.get_transform_at_time("B", "C", 1234.0) assert_array_almost_equal(B2C_2, B2C_3) def test_remove_frame_temporal_manager(): """Test removing a frame from the transform manager.""" tm = TemporalTransformManager() rng = np.random.default_rng(0) A2B = pt.random_transform(rng) A2D = pt.random_transform(rng) B2C = pt.random_transform(rng) D2E = pt.random_transform(rng) tm.add_transform("A", "B", StaticTransform(A2B)) tm.add_transform("A", "D", StaticTransform(A2D)) tm.add_transform("B", "C", StaticTransform(B2C)) tm.add_transform("D", "E", StaticTransform(D2E)) assert tm.has_frame("B") A2E = tm.get_transform("A", "E") # Check that connections are correctly represented in self.i and self.j assert tm.i == [ tm.nodes.index("A"), tm.nodes.index("A"), tm.nodes.index("B"), tm.nodes.index("D"), ] assert tm.j == [ tm.nodes.index("B"), tm.nodes.index("D"), tm.nodes.index("C"), tm.nodes.index("E"), ] tm.remove_frame("B") assert not tm.has_frame("B") # Ensure connections involving "B" are removed and the remaining # connections are correctly represented. assert tm.i == [tm.nodes.index("A"), tm.nodes.index("D")] assert tm.j == [tm.nodes.index("D"), tm.nodes.index("E")] with pytest.raises(KeyError, match="Unknown frame"): tm.get_transform("A", "B") with pytest.raises(KeyError, match="Unknown frame"): tm.get_transform("B", "C") assert tm.has_frame("A") assert tm.has_frame("C") assert tm.has_frame("D") assert tm.has_frame("E") # Ensure we cannot retrieve paths involving the removed frame with pytest.raises(KeyError, match="Cannot compute path"): tm.get_transform("A", "C") tm.get_transform("A", "D") tm.get_transform("D", "E") assert_array_almost_equal(A2E, tm.get_transform("A", "E")) def test_internals(): rng = np.random.default_rng(0) A2B = pt.random_transform(rng) rng = np.random.default_rng(42) A2C = pt.random_transform(rng) tm = TemporalTransformManager() tm.add_transform("A", "B", StaticTransform(A2B)) tm.add_transform("A", "C", StaticTransform(A2C)) tm.remove_transform("A", "C") assert ("A", "C") not in tm.transforms def test_numpy_timeseries_transform(): # create entities A and B together with their transformations from world duration = 10.0 # [s] sample_period = 0.5 # [s] velocity_x = 1 # [m/s] time_A, pq_arr_A = create_sinusoidal_movement( duration, sample_period, velocity_x, y_start_offset=0.0, start_time=0.1 ) A2world = NumpyTimeseriesTransform(time_A, pq_arr_A) time_B, pq_arr_B = create_sinusoidal_movement( duration, sample_period, velocity_x, y_start_offset=2.0, start_time=0.35 ) B2world = NumpyTimeseriesTransform(time_B, pq_arr_B) tm = TemporalTransformManager() tm.add_transform("A", "W", A2world) tm.add_transform("B", "W", B2world) query_time = time_A[0] # Start time A2W_at_start = pt.transform_from_pq(pq_arr_A[0, :]) A2W_at_start_2 = tm.get_transform_at_time("A", "W", query_time) assert A2W_at_start_2.ndim == 2 assert_array_almost_equal(A2W_at_start, A2W_at_start_2, decimal=2) query_times = [time_A[0], time_A[0]] # Start times A2Ws_at_start_2 = tm.get_transform_at_time("A", "W", query_times) assert A2Ws_at_start_2.ndim == 3 assert_array_almost_equal(A2W_at_start, A2Ws_at_start_2[0], decimal=2) assert_array_almost_equal(A2W_at_start, A2Ws_at_start_2[1], decimal=2) A2Ws_at_start_2 = tm.get_transform_at_time("A", "W", query_times) assert_array_almost_equal(A2W_at_start, A2Ws_at_start_2[0], decimal=2) assert_array_almost_equal(A2W_at_start, A2Ws_at_start_2[1], decimal=2) assert A2Ws_at_start_2.ndim == 3 query_time = 4.9 # [s] A2B_at_query_time = tm.get_transform_at_time("A", "B", query_time) origin_of_A_pos = pt.vector_to_point([0, 0, 0]) origin_of_A_in_B_pos = pt.transform(A2B_at_query_time, origin_of_A_pos) origin_of_A_in_B_xyz = origin_of_A_in_B_pos[:-1] assert origin_of_A_in_B_xyz[0] == pytest.approx(-1.11, abs=1e-2) assert origin_of_A_in_B_xyz[1] == pytest.approx(-1.28, abs=1e-2) def test_numpy_timeseries_transform_wrong_input_shapes(): n_steps = 10 with pytest.raises(ValueError, match="Number of timesteps"): time = np.arange(n_steps) pqs = np.random.randn(n_steps + 1, 7) NumpyTimeseriesTransform(time, pqs) with pytest.raises(ValueError, match="`pqs` matrix"): time = np.arange(10) pqs = np.random.randn(n_steps, 8) NumpyTimeseriesTransform(time, pqs) with pytest.raises(ValueError, match="Shape of PQ array"): time = np.arange(10) pqs = np.random.randn(n_steps, 8).flatten() NumpyTimeseriesTransform(time, pqs) def test_numpy_timeseries_transform_multiple_query_times(): # create entities A and B together with their transformations from world duration = 10.0 # [s] sample_period = 0.5 # [s] velocity_x = 1 # [m/s] time_A, pq_arr_A = create_sinusoidal_movement( duration, sample_period, velocity_x, y_start_offset=0.0, start_time=0.1 ) A2world = NumpyTimeseriesTransform(time_A, pq_arr_A) time_B, pq_arr_B = create_sinusoidal_movement( duration, sample_period, velocity_x, y_start_offset=2.0, start_time=0.35 ) B2world = NumpyTimeseriesTransform(time_B, pq_arr_B) tm = TemporalTransformManager() tm.add_transform("A", "W", A2world) tm.add_transform("B", "W", B2world) # test if shape is conserved correctly A2B_at_query_time = tm.get_transform_at_time("A", "B", 1.0) assert A2B_at_query_time.shape == (4, 4) A2B_at_query_time = tm.get_transform_at_time("A", "B", np.array([1.0])) assert A2B_at_query_time.shape == (1, 4, 4) query_times = np.array([4.9, 5.2]) # [s] A2B_at_query_time = tm.get_transform_at_time("A", "B", query_times) origin_of_A_pos = pt.vector_to_point([0, 0, 0]) origin_of_A_in_B_pos1 = pt.transform(A2B_at_query_time[0], origin_of_A_pos) origin_of_A_in_B_xyz1 = origin_of_A_in_B_pos1[:-1] origin_of_A_in_B_x1, origin_of_A_in_B_y1 = ( origin_of_A_in_B_xyz1[0], origin_of_A_in_B_xyz1[1], ) assert origin_of_A_in_B_x1 == pytest.approx(-1.11, abs=1e-2) assert origin_of_A_in_B_y1 == pytest.approx(-1.28, abs=1e-2) def test_temporal_transform_manager_incorrect_frame(): duration = 10.0 # [s] sample_period = 0.5 # [s] velocity_x = 1 # [m/s] time_A, pq_arr_A = create_sinusoidal_movement( duration, sample_period, velocity_x, y_start_offset=0.0, start_time=0.1 ) A2world = NumpyTimeseriesTransform(time_A, pq_arr_A) tm = TemporalTransformManager() tm.add_transform("A", "W", A2world) with pytest.raises(KeyError, match="Unknown frame"): tm.get_transform("B", "W") with pytest.raises(KeyError, match="Unknown frame"): tm.get_transform("A", "B") tm = TemporalTransformManager(check=False) tm.add_transform("A", "W", A2world) with pytest.raises(ValueError): tm.get_transform("B", "W") with pytest.raises(ValueError): tm.get_transform("A", "B") def test_temporal_transform_manager_out_of_bounds(): duration = 10.0 # [s] sample_period = 0.5 # [s] velocity_x = 1 # [m/s] time_A, pq_arr_A = create_sinusoidal_movement( duration, sample_period, velocity_x, y_start_offset=0.0, start_time=0.0 ) A2world = NumpyTimeseriesTransform(time_A, pq_arr_A, time_clipping=True) time_B, pq_arr_B = create_sinusoidal_movement( duration, sample_period, velocity_x, y_start_offset=2.0, start_time=0.1 ) B2world = NumpyTimeseriesTransform(time_B, pq_arr_B, time_clipping=True) tm = TemporalTransformManager() tm.add_transform("A", "W", A2world) tm.add_transform("B", "W", B2world) assert min(time_A) == 0.0 assert min(time_B) == 0.1 A2B_at_start_time, A2B_before_start_time = tm.get_transform_at_time( "A", "B", [0.0, -0.1] ) assert_array_almost_equal(A2B_at_start_time, A2B_before_start_time) assert max(time_A) == 9.5 assert max(time_B) == 9.6 A2B_at_end_time, A2B_after_end_time = tm.get_transform_at_time( "A", "B", [9.6, 10.0] ) assert_array_almost_equal(A2B_at_end_time, A2B_after_end_time) A2world.time_clipping = False B2world.time_clipping = False with pytest.raises( ValueError, match="Query time at indices \[0\], time\(s\): \[0.\]" ): tm.get_transform_at_time("A", "B", [0.0, 0.1]) with pytest.raises( ValueError, match=r"Query time at indices \[0 1\], time\(s\): \[ 9.7 10. \]", ): tm.get_transform_at_time("A", "B", [9.7, 10.0]) ================================================ FILE: pytransform3d/transform_manager/test/test_transform_manager.py ================================================ import contextlib import os import pickle import tempfile import warnings import numpy as np import pytest from numpy.testing import assert_array_almost_equal import pytransform3d.rotations as pr import pytransform3d.transformations as pt from pytransform3d import transform_manager from pytransform3d.transform_manager import TransformManager def test_request_added_transform(): """Request an added transform from the transform manager.""" rng = np.random.default_rng(0) A2B = pt.random_transform(rng) tm = TransformManager() assert len(tm.transforms) == 0 tm.add_transform("A", "B", A2B) assert len(tm.transforms) == 1 A2B_2 = tm.get_transform("A", "B") assert_array_almost_equal(A2B, A2B_2) def test_remove_frame(): """Test removing a frame from the transform manager.""" tm = TransformManager() rng = np.random.default_rng(0) A2B = pt.random_transform(rng) A2D = pt.random_transform(rng) B2C = pt.random_transform(rng) D2E = pt.random_transform(rng) tm.add_transform("A", "B", A2B) tm.add_transform("A", "D", A2D) tm.add_transform("B", "C", B2C) tm.add_transform("D", "E", D2E) assert tm.has_frame("B") A2E = tm.get_transform("A", "E") # Check that connections are correctly represented in self.i and self.j assert tm.i == [ tm.nodes.index("A"), tm.nodes.index("A"), tm.nodes.index("B"), tm.nodes.index("D"), ] assert tm.j == [ tm.nodes.index("B"), tm.nodes.index("D"), tm.nodes.index("C"), tm.nodes.index("E"), ] tm.remove_frame("B") assert not tm.has_frame("B") # Ensure connections involving "B" are removed and the remaining # connections are correctly represented. assert tm.i == [tm.nodes.index("A"), tm.nodes.index("D")] assert tm.j == [tm.nodes.index("D"), tm.nodes.index("E")] with pytest.raises(KeyError, match="Unknown frame"): tm.get_transform("A", "B") with pytest.raises(KeyError, match="Unknown frame"): tm.get_transform("B", "C") assert tm.has_frame("A") assert tm.has_frame("C") assert tm.has_frame("D") assert tm.has_frame("E") # Ensure we cannot retrieve paths involving the removed frame with pytest.raises(KeyError, match="Cannot compute path"): tm.get_transform("A", "C") tm.get_transform("A", "D") tm.get_transform("D", "E") assert_array_almost_equal(A2E, tm.get_transform("A", "E")) def test_remove_frame_does_not_exist(): tm = TransformManager() with pytest.raises(KeyError, match="not in the graph"): tm.remove_frame("Any") def test_request_inverse_transform(): """Request an inverse transform from the transform manager.""" rng = np.random.default_rng(0) A2B = pt.random_transform(rng) tm = TransformManager() tm.add_transform("A", "B", A2B) A2B_2 = tm.get_transform("A", "B") assert_array_almost_equal(A2B, A2B_2) B2A = tm.get_transform("B", "A") B2A_2 = pt.invert_transform(A2B) assert_array_almost_equal(B2A, B2A_2) def test_has_frame(): """Check if frames have been registered with transform.""" tm = TransformManager() tm.add_transform("A", "B", np.eye(4)) assert tm.has_frame("A") assert tm.has_frame("B") assert not tm.has_frame("C") def test_transform_not_added(): """Test request for transforms that have not been added.""" rng = np.random.default_rng(0) A2B = pt.random_transform(rng) C2D = pt.random_transform(rng) tm = TransformManager() tm.add_transform("A", "B", A2B) tm.add_transform("C", "D", C2D) with pytest.raises(KeyError, match="Unknown frame"): tm.get_transform("A", "G") with pytest.raises(KeyError, match="Unknown frame"): tm.get_transform("G", "D") with pytest.raises(KeyError, match="Cannot compute path"): tm.get_transform("A", "D") def test_request_concatenated_transform(): """Request a concatenated transform from the transform manager.""" rng = np.random.default_rng(0) A2B = pt.random_transform(rng) B2C = pt.random_transform(rng) F2A = pt.random_transform(rng) tm = TransformManager() tm.add_transform("A", "B", A2B) tm.add_transform("B", "C", B2C) tm.add_transform("D", "E", np.eye(4)) tm.add_transform("F", "A", F2A) A2C = tm.get_transform("A", "C") assert_array_almost_equal(A2C, pt.concat(A2B, B2C)) C2A = tm.get_transform("C", "A") assert_array_almost_equal( C2A, pt.concat(pt.invert_transform(B2C), pt.invert_transform(A2B)) ) F2B = tm.get_transform("F", "B") assert_array_almost_equal(F2B, pt.concat(F2A, A2B)) def test_update_transform(): """Update an existing transform.""" rng = np.random.default_rng(0) A2B1 = pt.random_transform(rng) A2B2 = pt.random_transform(rng) tm = TransformManager() tm.add_transform("A", "B", A2B1) tm.add_transform("A", "B", A2B2) A2B = tm.get_transform("A", "B") # Hack: test depends on internal member assert_array_almost_equal(A2B, A2B2) assert len(tm.i) == 1 assert len(tm.j) == 1 def test_pickle(): """Test if a transform manager can be pickled.""" rng = np.random.default_rng(1) A2B = pt.random_transform(rng) tm = TransformManager() tm.add_transform("A", "B", A2B) _, filename = tempfile.mkstemp(".pickle") try: with open(filename, "wb") as f: pickle.dump(tm, f) with open(filename, "rb") as f: tm2 = pickle.load(f) finally: if os.path.exists(filename): # workaround for permission problem on Windows with contextlib.suppress(OSError): os.remove(filename) A2B2 = tm2.get_transform("A", "B") assert_array_almost_equal(A2B, A2B2) def test_whitelist(): """Test correct handling of whitelists for plotting.""" rng = np.random.default_rng(2) A2B = pt.random_transform(rng) tm = TransformManager() tm.add_transform("A", "B", A2B) nodes = tm._whitelisted_nodes(None) assert {"A", "B"} == nodes nodes = tm._whitelisted_nodes(["A"]) assert {"A"} == nodes with pytest.raises(KeyError, match="unknown nodes"): tm._whitelisted_nodes(["C"]) def test_check_consistency(): """Test correct detection of inconsistent graphs.""" rng = np.random.default_rng(2) tm = TransformManager() A2B = pt.random_transform(rng) tm.add_transform("A", "B", A2B) B2A = pt.random_transform(rng) tm.add_transform("B", "A", B2A) assert not tm.check_consistency() tm = TransformManager() A2B = pt.random_transform(rng) tm.add_transform("A", "B", A2B) assert tm.check_consistency() C2D = pt.random_transform(rng) tm.add_transform("C", "D", C2D) assert tm.check_consistency() B2C = pt.random_transform(rng) tm.add_transform("B", "C", B2C) assert tm.check_consistency() A2D_over_path = tm.get_transform("A", "D") A2D = pt.random_transform(rng) tm.add_transform("A", "D", A2D) assert not tm.check_consistency() tm.add_transform("A", "D", A2D_over_path) assert tm.check_consistency() def test_connected_components(): """Test computation of connected components in the graph.""" tm = TransformManager() tm.add_transform("A", "B", np.eye(4)) assert tm.connected_components() == 1 tm.add_transform("D", "E", np.eye(4)) assert tm.connected_components() == 2 tm.add_transform("B", "C", np.eye(4)) assert tm.connected_components() == 2 tm.add_transform("D", "C", np.eye(4)) assert tm.connected_components() == 1 def test_png_export(): """Test if the graph can be exported to PNG.""" rng = np.random.default_rng(0) ee2robot = pt.transform_from_pq( np.hstack((np.array([0.4, -0.3, 0.5]), pr.random_quaternion(rng))) ) cam2robot = pt.transform_from_pq( np.hstack((np.array([0.0, 0.0, 0.8]), pr.q_id)) ) object2cam = pt.transform_from( pr.active_matrix_from_intrinsic_euler_xyz(np.array([0.0, 0.0, 0.5])), np.array([0.5, 0.1, 0.1]), ) tm = TransformManager() tm.add_transform("end-effector", "robot", ee2robot) tm.add_transform("camera", "robot", cam2robot) tm.add_transform("object", "camera", object2cam) _, filename = tempfile.mkstemp(".png") try: tm.write_png(filename) assert os.path.exists(filename) except ImportError: pytest.skip("pydot is required for this test") finally: if os.path.exists(filename): # workaround for permission problem on Windows with contextlib.suppress(OSError): os.remove(filename) def test_png_export_without_pydot_fails(): """Test graph export without pydot.""" pydot_available = transform_manager._transform_manager.PYDOT_AVAILABLE tm = TransformManager() try: transform_manager._transform_manager.PYDOT_AVAILABLE = False with pytest.raises( ImportError, match="pydot must be installed to use this feature." ): tm.write_png("bla") finally: transform_manager._transform_manager.PYDOT_AVAILABLE = pydot_available def test_deactivate_transform_manager_precision_error(): A2B = np.eye(4) A2B[0, 0] = 2.0 A2B[3, 0] = 3.0 tm = TransformManager() with pytest.raises(ValueError, match="Expected rotation matrix"): tm.add_transform("A", "B", A2B) n_expected_warnings = 6 try: warnings.filterwarnings("always", category=UserWarning) with warnings.catch_warnings(record=True) as w: tm = TransformManager(strict_check=False) tm.add_transform("A", "B", A2B) tm.add_transform("B", "C", np.eye(4)) tm.get_transform("C", "A") assert len(w) == n_expected_warnings finally: warnings.filterwarnings("default", category=UserWarning) def test_deactivate_checks(): tm = TransformManager(check=False) tm.add_transform("A", "B", np.zeros((4, 4))) tm.add_transform("B", "C", np.zeros((4, 4))) A2B = tm.get_transform("A", "C") assert_array_almost_equal(A2B, np.zeros((4, 4))) def test_remove_transform(): tm = TransformManager() tm.add_transform("A", "B", np.eye(4)) tm.add_transform("C", "D", np.eye(4)) with pytest.raises(KeyError, match="Cannot compute path"): tm.get_transform("A", "D") tm.add_transform("B", "C", np.eye(4)) tm.get_transform("A", "C") tm.remove_transform("B", "C") tm.remove_transform("B", "C") # nothing should happen with pytest.raises(KeyError, match="Cannot compute path"): tm.get_transform("A", "D") tm.get_transform("B", "A") tm.get_transform("D", "C") def test_from_to_dict(): rng = np.random.default_rng(2323) tm = TransformManager() A2B = pt.random_transform(rng) tm.add_transform("A", "B", A2B) B2C = pt.random_transform(rng) tm.add_transform("B", "C", B2C) C2D = pt.random_transform(rng) tm.add_transform("C", "D", C2D) tm_dict = tm.to_dict() tm2 = TransformManager.from_dict(tm_dict) tm2_dict = tm2.to_dict() assert_array_almost_equal( tm.get_transform("D", "A"), tm2.get_transform("D", "A") ) assert tm_dict == tm2_dict def test_remove_twice(): tm = TransformManager() tm.add_transform("a", "b", np.eye(4)) tm.add_transform("c", "d", np.eye(4)) tm.remove_transform("a", "b") tm.remove_transform("c", "d") def test_remove_and_add_connection(): rng = np.random.default_rng(5) A2B = pt.random_transform(rng) B2C = pt.random_transform(rng) C2D = pt.random_transform(rng) D2E = pt.random_transform(rng) tm = TransformManager() tm.add_transform("a", "b", A2B) tm.add_transform("b", "c", B2C) tm.add_transform("c", "d", C2D) tm.add_transform("d", "e", D2E) measurement1 = tm.get_transform("a", "e") tm.remove_transform("b", "c") tm.remove_transform("c", "d") tm.add_transform("b", "c", B2C) tm.add_transform("c", "d", C2D) measurement2 = tm.get_transform("a", "e") assert_array_almost_equal(measurement1, measurement2) ================================================ FILE: pytransform3d/transformations/__init__.py ================================================ """Transformations in three dimensions - SE(3). See :doc:`user_guide/transformations` for more information. """ from ._transform import ( transform_requires_renormalization, check_transform, assert_transform, transform_from, rotate_transform, translate_transform, pq_from_transform, transform_log_from_transform, exponential_coordinates_from_transform, dual_quaternion_from_transform, adjoint_from_transform, ) from ._transform_operations import ( invert_transform, scale_transform, concat, vector_to_point, vectors_to_points, vector_to_direction, vectors_to_directions, transform, transform_sclerp, transform_power, ) from ._screws import ( check_screw_parameters, check_screw_axis, check_exponential_coordinates, check_screw_matrix, check_transform_log, norm_exponential_coordinates, assert_exponential_coordinates_equal, assert_screw_parameters_equal, transform_from_transform_log, transform_from_exponential_coordinates, screw_parameters_from_screw_axis, screw_axis_from_screw_parameters, exponential_coordinates_from_screw_axis, screw_axis_from_exponential_coordinates, transform_log_from_exponential_coordinates, exponential_coordinates_from_transform_log, screw_matrix_from_screw_axis, screw_axis_from_screw_matrix, transform_log_from_screw_matrix, screw_matrix_from_transform_log, dual_quaternion_from_screw_parameters, ) from ._pq import check_pq, pq_slerp, transform_from_pq, dual_quaternion_from_pq from ._dual_quaternion import ( dual_quaternion_requires_renormalization, norm_dual_quaternion, dual_quaternion_squared_norm, check_dual_quaternion, assert_unit_dual_quaternion_equal, assert_unit_dual_quaternion, dual_quaternion_double, dq_q_conj, dq_conj, concatenate_dual_quaternions, dual_quaternion_sclerp, dual_quaternion_power, dq_prod_vector, transform_from_dual_quaternion, pq_from_dual_quaternion, screw_parameters_from_dual_quaternion, ) from ._random import ( random_transform, random_screw_axis, random_exponential_coordinates, ) from ._plot import plot_transform, plot_screw from ._jacobians import ( left_jacobian_SE3, left_jacobian_SE3_series, left_jacobian_SE3_inv, left_jacobian_SE3_inv_series, ) __all__ = [ "transform_requires_renormalization", "check_transform", "check_pq", "check_screw_parameters", "check_screw_axis", "check_exponential_coordinates", "check_screw_matrix", "check_transform_log", "dual_quaternion_requires_renormalization", "check_dual_quaternion", "transform_from", "rotate_transform", "translate_transform", "pq_from_transform", "transform_from_pq", "transform_from_transform_log", "transform_log_from_transform", "transform_from_exponential_coordinates", "exponential_coordinates_from_transform", "screw_parameters_from_screw_axis", "screw_axis_from_screw_parameters", "exponential_coordinates_from_screw_axis", "screw_axis_from_exponential_coordinates", "transform_log_from_exponential_coordinates", "exponential_coordinates_from_transform_log", "screw_matrix_from_screw_axis", "screw_axis_from_screw_matrix", "transform_log_from_screw_matrix", "screw_matrix_from_transform_log", "dual_quaternion_from_transform", "transform_from_dual_quaternion", "screw_parameters_from_dual_quaternion", "dual_quaternion_from_screw_parameters", "dual_quaternion_from_pq", "pq_from_dual_quaternion", "adjoint_from_transform", "norm_exponential_coordinates", "invert_transform", "scale_transform", "concat", "vector_to_point", "vectors_to_points", "vector_to_direction", "vectors_to_directions", "transform", "transform_sclerp", "transform_power", "random_transform", "random_screw_axis", "random_exponential_coordinates", "pq_slerp", "norm_dual_quaternion", "dual_quaternion_squared_norm", "dual_quaternion_double", "dq_q_conj", "dq_conj", "concatenate_dual_quaternions", "dual_quaternion_sclerp", "dual_quaternion_power", "dq_prod_vector", "plot_transform", "plot_screw", "assert_transform", "assert_exponential_coordinates_equal", "assert_screw_parameters_equal", "assert_unit_dual_quaternion_equal", "assert_unit_dual_quaternion", "left_jacobian_SE3", "left_jacobian_SE3_series", "left_jacobian_SE3_inv", "left_jacobian_SE3_inv_series", ] ================================================ FILE: pytransform3d/transformations/_dual_quaternion.py ================================================ """Dual quaternion operations.""" import numpy as np from numpy.testing import assert_array_almost_equal from ._screws import dual_quaternion_from_screw_parameters from ._transform import transform_from from ..rotations import ( concatenate_quaternions, q_conj, axis_angle_from_quaternion, matrix_from_quaternion, ) def check_dual_quaternion(dq, unit=True): """Input validation of dual quaternion representation. See http://web.cs.iastate.edu/~cs577/handouts/dual-quaternion.pdf A dual quaternion is defined as .. math:: \\boldsymbol{\\sigma} = \\boldsymbol{p} + \\epsilon \\boldsymbol{q}, where :math:`\\boldsymbol{p}` and :math:`\\boldsymbol{q}` are both quaternions and :math:`\\epsilon` is the dual unit with :math:`\\epsilon^2 = 0`. The first quaternion is also called the real part and the second quaternion is called the dual part. Parameters ---------- dq : array-like, shape (8,) Dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) unit : bool, optional (default: True) Normalize the dual quaternion so that it is a unit dual quaternion. A unit dual quaternion has the properties :math:`p_w^2 + p_x^2 + p_y^2 + p_z^2 = 1` and :math:`p_w q_w + p_x q_x + p_y q_y + p_z q_z = 0`. Returns ------- dq : array, shape (8,) Unit dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) Raises ------ ValueError If input is invalid See Also -------- norm_dual_quaternion Normalization that enforces unit norm and orthogonality of the real and dual quaternion. """ dq = np.asarray(dq, dtype=np.float64) if dq.ndim != 1 or dq.shape[0] != 8: raise ValueError( "Expected dual quaternion with shape (8,), got " "array-like object with shape %s" % (dq.shape,) ) if unit: # Norm of a dual quaternion only depends on the real part because # the dual part vanishes with (1) epsilon ** 2 = 0 and (2) the real # and dual part being orthogonal, i.e., their product is 0. real_norm = np.linalg.norm(dq[:4]) if real_norm == 0.0: return np.r_[1, 0, 0, 0, dq[4:]] return dq / real_norm return dq def dual_quaternion_squared_norm(dq): """Compute squared norm of dual quaternion. Parameters ---------- dq : array-like, shape (8) Dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) Returns ------- squared_norm : array, shape (2,) Squared norm of dual quaternion, which is a dual number with a real and a dual part. """ dq = np.asarray(dq) prod = concatenate_dual_quaternions( dq, dq_q_conj(dq, unit=False), unit=False ) return prod[[0, 4]] def dual_quaternion_requires_renormalization(dq, tolerance=1e-6): r"""Check if dual quaternion requires renormalization. Dual quaternions that represent transformations in 3D should have unit norm (:math:`1 + 0 \epsilon`), that is the real quaternion must have unit norm and the real and the dual quaternion must be orthogonal (their dot product should be 0). This function checks unit norm and orthogonality of the real and dual part. Parameters ---------- dq : array-like, shape (8,) Dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) tolerance : float, optional (default: 1e-6) Tolerance for check. Returns ------- required : bool Indicates if renormalization is required. See Also -------- check_dual_quaternion Input validation of dual quaternion representation. Has an option to normalize the dual quaternion. norm_dual_quaternion Normalization that enforces unit norm and orthogonality of the real and dual quaternion. assert_unit_dual_quaternion Checks unit norm and orthogonality of real and dual quaternion. """ squared_norm = dual_quaternion_squared_norm(dq) return ( abs(squared_norm[0] - 1.0) > tolerance or abs(squared_norm[1]) > tolerance ) def norm_dual_quaternion(dq): """Normalize unit dual quaternion. A unit dual quaternion has a real quaternion with unit norm and an orthogonal real part. Both properties are enforced by multiplying a normalization factor [1]_. This is not always necessary. It is often sufficient to only enforce the unit norm property of the real quaternion. This can also be done with :func:`check_dual_quaternion`. Parameters ---------- dq : array-like, shape (8,) Dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) Returns ------- dq : array, shape (8,) Unit dual quaternion to represent transform with orthogonal real and dual quaternion. See Also -------- check_dual_quaternion Input validation of dual quaternion representation. Has an option to normalize the dual quaternion. dual_quaternion_requires_renormalization Check if normalization is required. References ---------- .. [1] enki (2023). Properly normalizing a dual quaternion. https://stackoverflow.com/a/76313524 """ # 1. ensure unit norm of real quaternion dq = check_dual_quaternion(dq, unit=True) # 2. ensure orthogonality of real and dual quaternion real = dq[:4] dual = dq[4:] dual = dual - np.dot(real, dual) * real return np.hstack((real, dual)) def assert_unit_dual_quaternion(dq, *args, **kwargs): """Raise an assertion if the dual quaternion does not have unit norm. See numpy.testing.assert_array_almost_equal for a more detailed documentation of the other parameters. Parameters ---------- dq : array-like, shape (8,) Unit dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) args : tuple Positional arguments that will be passed to `assert_array_almost_equal` kwargs : dict Positional arguments that will be passed to `assert_array_almost_equal` See Also -------- check_dual_quaternion Input validation of dual quaternion representation. Has an option to normalize the dual quaternion. dual_quaternion_requires_renormalization Check if normalization is required. norm_dual_quaternion Normalization that enforces unit norm and orthogonality of the real and dual quaternion. """ real_sq_norm, dual_sq_norm = dual_quaternion_squared_norm(dq) assert_array_almost_equal(real_sq_norm, 1.0, *args, **kwargs) assert_array_almost_equal(dual_sq_norm, 0.0, *args, **kwargs) # The two previous checks are consequences of the unit norm requirement. # The norm of a dual quaternion is defined as the product of a dual # quaternion and its quaternion conjugate. dq_conj = dq_q_conj(dq) dq_prod_dq_conj = concatenate_dual_quaternions(dq, dq_conj) assert_array_almost_equal( dq_prod_dq_conj, [1, 0, 0, 0, 0, 0, 0, 0], *args, **kwargs ) def assert_unit_dual_quaternion_equal(dq1, dq2, *args, **kwargs): """Raise an assertion if unit dual quaternions are not approximately equal. Note that unit dual quaternions are equal either if dq1 == dq2 or if dq1 == -dq2. See numpy.testing.assert_array_almost_equal for a more detailed documentation of the other parameters. Parameters ---------- dq1 : array-like, shape (8,) Unit dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) dq2 : array-like, shape (8,) Unit dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) args : tuple Positional arguments that will be passed to `assert_array_almost_equal` kwargs : dict Positional arguments that will be passed to `assert_array_almost_equal` """ try: assert_array_almost_equal(dq1, dq2, *args, **kwargs) except AssertionError: assert_array_almost_equal(dq1, -dq2, *args, **kwargs) def dual_quaternion_double(dq): r"""Create another dual quaternion that represents the same transformation. The unit dual quaternions :math:`\boldsymbol{\sigma} = \boldsymbol{p} + \epsilon \boldsymbol{q}` and :math:`-\boldsymbol{\sigma}` represent exactly the same transformation. The reason for this ambiguity is that the real quaternion :math:`\boldsymbol{p}` represents the orientation component, the dual quaternion encodes the translation component as :math:`\boldsymbol{q} = 0.5 \boldsymbol{t} \boldsymbol{p}`, where :math:`\boldsymbol{t}` is a quaternion with the translation in the vector component and the scalar 0, and rotation quaternions have the same ambiguity. Parameters ---------- dq : array-like, shape (8,) Unit dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) Returns ------- dq_double : array, shape (8,) -dq """ return -check_dual_quaternion(dq, unit=True) def dq_conj(dq, unit=True): """Conjugate of dual quaternion. There are three different conjugates for dual quaternions. The one that we use here converts (pw, px, py, pz, qw, qx, qy, qz) to (pw, -px, -py, -pz, -qw, qx, qy, qz). It is a combination of the quaternion conjugate and the dual number conjugate. Parameters ---------- dq : array-like, shape (8,) Unit dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) unit : bool, optional (default: True) Normalize the dual quaternion so that it is a unit dual quaternion. A unit dual quaternion has the properties :math:`p_w^2 + p_x^2 + p_y^2 + p_z^2 = 1` and :math:`p_w q_w + p_x q_x + p_y q_y + p_z q_z = 0`. Returns ------- dq_conjugate : array, shape (8,) Conjugate of dual quaternion: (pw, -px, -py, -pz, -qw, qx, qy, qz) See Also -------- dq_q_conj Quaternion conjugate of dual quaternion. """ dq = check_dual_quaternion(dq, unit=unit) return np.r_[dq[0], -dq[1:5], dq[5:]] def dq_q_conj(dq, unit=True): """Quaternion conjugate of dual quaternion. For unit dual quaternions that represent transformations, this function is equivalent to the inverse of the corresponding transformation matrix. There are three different conjugates for dual quaternions. The one that we use here converts (pw, px, py, pz, qw, qx, qy, qz) to (pw, -px, -py, -pz, qw, -qx, -qy, -qz). It is the quaternion conjugate applied to each of the two quaternions. Parameters ---------- dq : array-like, shape (8,) Unit dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) unit : bool, optional (default: True) Normalize the dual quaternion so that it is a unit dual quaternion. A unit dual quaternion has the properties :math:`p_w^2 + p_x^2 + p_y^2 + p_z^2 = 1` and :math:`p_w q_w + p_x q_x + p_y q_y + p_z q_z = 0`. Returns ------- dq_q_conjugate : array, shape (8,) Conjugate of dual quaternion: (pw, -px, -py, -pz, qw, -qx, -qy, -qz) See Also -------- dq_conj Conjugate of a dual quaternion. """ dq = check_dual_quaternion(dq, unit=unit) return np.r_[dq[0], -dq[1:4], dq[4], -dq[5:]] def concatenate_dual_quaternions(dq1, dq2, unit=True): r"""Concatenate dual quaternions. We concatenate two dual quaternions by dual quaternion multiplication .. math:: (\boldsymbol{p}_1 + \epsilon \boldsymbol{q}_1) (\boldsymbol{p}_2 + \epsilon \boldsymbol{q}_2) = \boldsymbol{p}_1 \boldsymbol{p}_2 + \epsilon ( \boldsymbol{p}_1 \boldsymbol{q}_2 + \boldsymbol{q}_1 \boldsymbol{p}_2) using Hamilton multiplication of quaternions. .. warning:: Note that the order of arguments is different than the order in :func:`concat`. Parameters ---------- dq1 : array-like, shape (8,) Dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) dq2 : array-like, shape (8,) Dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) unit : bool, optional (default: True) Normalize the dual quaternion so that it is a unit dual quaternion. A unit dual quaternion has the properties :math:`p_w^2 + p_x^2 + p_y^2 + p_z^2 = 1` and :math:`p_w q_w + p_x q_x + p_y q_y + p_z q_z = 0`. Returns ------- dq3 : array, shape (8,) Product of the two dual quaternions: (pw, px, py, pz, qw, qx, qy, qz) See Also -------- pytransform3d.rotations.concatenate_quaternions Quaternion multiplication. """ dq1 = check_dual_quaternion(dq1, unit=unit) dq2 = check_dual_quaternion(dq2, unit=unit) real = concatenate_quaternions(dq1[:4], dq2[:4]) dual = concatenate_quaternions(dq1[:4], dq2[4:]) + concatenate_quaternions( dq1[4:], dq2[:4] ) return np.hstack((real, dual)) def dq_prod_vector(dq, v): r"""Apply transform represented by a dual quaternion to a vector. To apply the transformation defined by a unit dual quaternion :math:`\boldsymbol{q}` to a point :math:`\boldsymbol{v} \in \mathbb{R}^3`, we first represent the vector as a dual quaternion: we set the real part to (1, 0, 0, 0) and the dual part is a pure quaternion with the scalar part 0 and the vector as its vector part :math:`\left(\begin{array}{c}0\\\boldsymbol{v}\end{array}\right) \in \mathbb{R}^4`. Then we left-multiply the dual quaternion and right-multiply its dual quaternion conjugate .. math:: \left(\begin{array}{c}1\\0\\0\\0\\0\\\boldsymbol{w}\end{array}\right) = \boldsymbol{q} \cdot \left(\begin{array}{c}1\\0\\0\\0\\0\\\boldsymbol{v}\end{array}\right) \cdot \boldsymbol{q}^*. The vector part of the dual part :math:`\boldsymbol{w}` of the resulting quaternion is the rotated point. Parameters ---------- dq : array-like, shape (8,) Unit dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) v : array-like, shape (3,) 3d vector Returns ------- w : array, shape (3,) 3d vector """ dq = check_dual_quaternion(dq) v_dq = np.r_[1, 0, 0, 0, 0, v] v_dq_transformed = concatenate_dual_quaternions( concatenate_dual_quaternions(dq, v_dq), dq_conj(dq) ) return v_dq_transformed[5:] def dual_quaternion_sclerp(start, end, t): """Screw linear interpolation (ScLERP) for dual quaternions. Although linear interpolation of dual quaternions is possible, this does not result in constant velocities. If you want to generate interpolations with constant velocity, you have to use ScLERP. Parameters ---------- start : array-like, shape (8,) Unit dual quaternion to represent start pose: (pw, px, py, pz, qw, qx, qy, qz) end : array-like, shape (8,) Unit dual quaternion to represent end pose: (pw, px, py, pz, qw, qx, qy, qz) t : float in [0, 1] Position between start and goal Returns ------- dq_t : array, shape (8,) Interpolated unit dual quaternion: (pw, px, py, pz, qw, qx, qy, qz) References ---------- .. [1] Kavan, L., Collins, S., O'Sullivan, C., Zara, J. (2006). Dual Quaternions for Rigid Transformation Blending, Technical report, Trinity College Dublin, https://users.cs.utah.edu/~ladislav/kavan06dual/kavan06dual.pdf See Also -------- transform_sclerp : ScLERP for transformation matrices. pq_slerp : An alternative approach is spherical linear interpolation (SLERP) with position and quaternion. """ start = check_dual_quaternion(start) end = check_dual_quaternion(end) diff = concatenate_dual_quaternions(dq_q_conj(start), end) return concatenate_dual_quaternions(start, dual_quaternion_power(diff, t)) def dual_quaternion_power(dq, t): r"""Compute power of unit dual quaternion with respect to scalar. .. math:: (p + \epsilon q)^t Parameters ---------- dq : array-like, shape (8,) Unit dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) t : float Exponent Returns ------- dq_t : array, shape (8,) Unit dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) ** t """ dq = check_dual_quaternion(dq) q, s_axis, h, theta = screw_parameters_from_dual_quaternion(dq) return dual_quaternion_from_screw_parameters(q, s_axis, h, theta * t) def transform_from_dual_quaternion(dq): """Compute transformation matrix from dual quaternion. Parameters ---------- dq : array-like, shape (8,) Unit dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) Returns ------- A2B : array, shape (4, 4) Transform from frame A to frame B """ dq = check_dual_quaternion(dq) real = dq[:4] dual = dq[4:] R = matrix_from_quaternion(real) p = 2 * concatenate_quaternions(dual, q_conj(real))[1:] return transform_from(R=R, p=p) def pq_from_dual_quaternion(dq): """Compute position and quaternion from dual quaternion. Parameters ---------- dq : array-like, shape (8,) Unit dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) Returns ------- pq : array, shape (7,) Position and orientation quaternion: (x, y, z, qw, qx, qy, qz) """ dq = check_dual_quaternion(dq) real = dq[:4] dual = dq[4:] p = 2 * concatenate_quaternions(dual, q_conj(real))[1:] return np.hstack((p, real)) def screw_parameters_from_dual_quaternion(dq): """Compute screw parameters from dual quaternion. Parameters ---------- dq : array-like, shape (8,) Unit dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) Returns ------- q : array, shape (3,) Vector to a point on the screw axis s_axis : array, shape (3,) Direction vector of the screw axis h : float Pitch of the screw. The pitch is the ratio of translation and rotation of the screw axis. Infinite pitch indicates pure translation. theta : float Parameter of the transformation: theta is the angle of rotation and h * theta the translation. """ dq = check_dual_quaternion(dq, unit=True) real = dq[:4] dual = dq[4:] a = axis_angle_from_quaternion(real) s_axis = a[:3] theta = a[3] translation = 2 * concatenate_quaternions(dual, q_conj(real))[1:] if abs(theta) < np.finfo(float).eps: # pure translation d = np.linalg.norm(translation) if d < np.finfo(float).eps: s_axis = np.array([1, 0, 0]) else: s_axis = translation / d q = np.zeros(3) theta = d h = np.inf return q, s_axis, h, theta distance = np.dot(translation, s_axis) moment = 0.5 * ( np.cross(translation, s_axis) + (translation - distance * s_axis) / np.tan(0.5 * theta) ) dual = np.cross(s_axis, moment) h = distance / theta return dual, s_axis, h, theta ================================================ FILE: pytransform3d/transformations/_dual_quaternion.pyi ================================================ import numpy as np import numpy.typing as npt def check_dual_quaternion( dq: npt.ArrayLike, unit: bool = ... ) -> np.ndarray: ... def dual_quaternion_squared_norm(dq: npt.ArrayLike) -> np.ndarray: ... def dual_quaternion_requires_renormalization( dq: npt.ArrayLike, tolerance: float = ... ) -> bool: ... def norm_dual_quaternion(dq: npt.ArrayLike) -> np.ndarray: ... def assert_unit_dual_quaternion(dq: npt.ArrayLike, *args, **kwargs): ... def assert_unit_dual_quaternion_equal( dq1: npt.ArrayLike, dq2: npt.ArrayLike, *args, **kwargs ): ... def dual_quaternion_double(dq: npt.ArrayLike) -> np.ndarray: ... def dq_conj(dq: npt.ArrayLike, unit: bool = ...) -> np.ndarray: ... def dq_q_conj(dq: npt.ArrayLike, unit: bool = ...) -> np.ndarray: ... def concatenate_dual_quaternions( dq1: npt.ArrayLike, dq2: npt.ArrayLike, unit: bool = ... ) -> np.ndarray: ... def dq_prod_vector(dq: npt.ArrayLike, v: npt.ArrayLike) -> np.ndarray: ... def dual_quaternion_sclerp( start: npt.ArrayLike, end: npt.ArrayLike, t: float ) -> np.ndarray: ... def dual_quaternion_power(dq: npt.ArrayLike, t: float) -> np.ndarray: ... def transform_from_dual_quaternion(dq: npt.ArrayLike) -> np.ndarray: ... def pq_from_dual_quaternion(dq: npt.ArrayLike) -> np.ndarray: ... def screw_parameters_from_dual_quaternion(dq: npt.ArrayLike) -> np.ndarray: ... ================================================ FILE: pytransform3d/transformations/_jacobians.py ================================================ """Jacobians of SE(3).""" import math import numpy as np from ._screws import ( check_exponential_coordinates, screw_axis_from_exponential_coordinates, ) from ..rotations import ( cross_product_matrix, left_jacobian_SO3, left_jacobian_SO3_inv, ) def left_jacobian_SE3(Stheta): r"""Left Jacobian of SE(3). .. math:: \boldsymbol{\mathcal{J}} = \left( \begin{array}{cc} \boldsymbol{J} & \boldsymbol{0}\\ \boldsymbol{Q} & \boldsymbol{J} \end{array} \right), where :math:`\boldsymbol{J}` is the left Jacobian of SO(3) and :math:`\boldsymbol{Q}` is given by Barfoot and Furgale (see reference below). Parameters ---------- Stheta : array-like, shape (6,) Exponential coordinates of transformation: S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta, where S is the screw axis, the first 3 components are related to rotation and the last 3 components are related to translation. Theta is the rotation angle and h * theta the translation. Returns ------- J : array, shape (6, 6) Jacobian of SE(3). See Also -------- left_jacobian_SE3_series : Left Jacobian of SE(3) at theta from Taylor series. left_jacobian_SE3_inv : Left inverse Jacobian of SE(3). References ---------- .. [1] Barfoot, T. D., Furgale, P. T. (2014). Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems. IEEE Transactions on Robotics, 30(3), pp. 679-693, doi: 10.1109/TRO.2014.2298059. """ Stheta = check_exponential_coordinates(Stheta) _, theta = screw_axis_from_exponential_coordinates(Stheta) if theta < np.finfo(float).eps: return left_jacobian_SE3_series(Stheta, 10) phi = Stheta[:3] J = left_jacobian_SO3(phi) return np.block([[J, np.zeros((3, 3))], [_Q(Stheta), J]]) def left_jacobian_SE3_series(Stheta, n_terms): """Left Jacobian of SE(3) at theta from Taylor series. Parameters ---------- Stheta : array-like, shape (6,) Exponential coordinates of transformation: S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta, where S is the screw axis, the first 3 components are related to rotation and the last 3 components are related to translation. Theta is the rotation angle and h * theta the translation. n_terms : int Number of terms to include in the series. Returns ------- J : array, shape (3, 3) Left Jacobian of SE(3). See Also -------- left_jacobian_SE3 : Left Jacobian of SE(3). """ Stheta = check_exponential_coordinates(Stheta) J = np.eye(6) pxn = np.eye(6) px = _curlyhat(Stheta) for n in range(n_terms): pxn = np.dot(pxn, px) / (n + 2) J += pxn return J def left_jacobian_SE3_inv(Stheta): r"""Left inverse Jacobian of SE(3). .. math:: \boldsymbol{\mathcal{J}}^{-1} = \left( \begin{array}{cc} \boldsymbol{J}^{-1} & \boldsymbol{0}\\ -\boldsymbol{J}^{-1}\boldsymbol{Q}\boldsymbol{J}^{-1} & \boldsymbol{J}^{-1} \end{array} \right), where :math:`\boldsymbol{J}` is the left Jacobian of SO(3) and :math:`\boldsymbol{Q}` is given by Barfoot and Furgale (see reference below). Parameters ---------- Stheta : array-like, shape (6,) Exponential coordinates of transformation: S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta, where S is the screw axis, the first 3 components are related to rotation and the last 3 components are related to translation. Theta is the rotation angle and h * theta the translation. Returns ------- J_inv : array, shape (6, 6) Inverse Jacobian of SE(3). See Also -------- left_jacobian_SE3 : Left Jacobian of SE(3). left_jacobian_SE3_inv_series : Left inverse Jacobian of SE(3) at theta from Taylor series. """ Stheta = check_exponential_coordinates(Stheta) _, theta = screw_axis_from_exponential_coordinates(Stheta) if theta < np.finfo(float).eps: return left_jacobian_SE3_inv_series(Stheta, 10) phi = Stheta[:3] J_inv = left_jacobian_SO3_inv(phi) return np.block( [ [J_inv, np.zeros((3, 3))], [-np.dot(J_inv, np.dot(_Q(Stheta), J_inv)), J_inv], ] ) def _Q(Stheta): rho = Stheta[3:] phi = Stheta[:3] ph = np.linalg.norm(phi) px = cross_product_matrix(phi) rx = cross_product_matrix(rho) ph2 = ph * ph ph3 = ph2 * ph ph4 = ph3 * ph ph5 = ph4 * ph cph = math.cos(ph) sph = math.sin(ph) t1 = 0.5 * rx t2 = ( (ph - sph) / ph3 * (np.dot(px, rx) + np.dot(rx, px) + np.dot(px, np.dot(rx, px))) ) m3 = (1.0 - 0.5 * ph * ph - cph) / ph4 t3 = -m3 * ( np.dot(px, np.dot(px, rx)) + np.dot(rx, np.dot(px, px)) - 3 * np.dot(px, np.dot(rx, px)) ) m4 = 0.5 * (m3 - 3.0 * (ph - sph - ph3 / 6.0) / ph5) t4 = -m4 * ( np.dot(px, np.dot(rx, np.dot(px, px))) + np.dot(px, np.dot(px, np.dot(rx, px))) ) Q = t1 + t2 + t3 + t4 return Q def left_jacobian_SE3_inv_series(Stheta, n_terms): """Left inverse Jacobian of SE(3) at theta from Taylor series. Parameters ---------- Stheta : array-like, shape (6,) Exponential coordinates of transformation: S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta, where S is the screw axis, the first 3 components are related to rotation and the last 3 components are related to translation. Theta is the rotation angle and h * theta the translation. n_terms : int Number of terms to include in the series. Returns ------- J_inv : array, shape (3, 3) Inverse left Jacobian of SE(3). See Also -------- left_jacobian_SE3_inv : Left inverse Jacobian of SE(3). """ from scipy.special import bernoulli Stheta = check_exponential_coordinates(Stheta) J_inv = np.eye(6) pxn = np.eye(6) px = _curlyhat(Stheta) b = bernoulli(n_terms + 1) for n in range(n_terms): pxn = np.dot(pxn, px / (n + 1)) J_inv += b[n + 1] * pxn return J_inv def _curlyhat(Stheta): omega_matrix = cross_product_matrix(Stheta[:3]) return np.block( [ [omega_matrix, np.zeros((3, 3))], [cross_product_matrix(Stheta[3:]), omega_matrix], ] ) ================================================ FILE: pytransform3d/transformations/_jacobians.pyi ================================================ import numpy as np import numpy.typing as npt def left_jacobian_SE3(Stheta: npt.ArrayLike) -> np.ndarray: ... def left_jacobian_SE3_series( Stheta: npt.ArrayLike, n_terms: int ) -> np.ndarray: ... def left_jacobian_SE3_inv(Stheta: npt.ArrayLike) -> np.ndarray: ... def left_jacobian_SE3_inv_series( Stheta: npt.ArrayLike, n_terms: int ) -> np.ndarray: ... ================================================ FILE: pytransform3d/transformations/_plot.py ================================================ """Plotting utilities.""" import numpy as np from ._screws import check_screw_parameters from ._transform import check_transform from ._transform_operations import ( transform, vector_to_point, vector_to_direction, vectors_to_points, ) def plot_transform( ax=None, A2B=None, s=1.0, ax_s=1, name=None, strict_check=True, **kwargs ): """Plot transform. Parameters ---------- ax : Matplotlib 3d axis, optional (default: None) If the axis is None, a new 3d axis will be created A2B : array-like, shape (4, 4), optional (default: I) Transform from frame A to frame B s : float, optional (default: 1) Scaling of the axis and angle that will be drawn ax_s : float, optional (default: 1) Scaling of the new matplotlib 3d axis name : string, optional (default: None) Name of the frame, will be used for annotation strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. kwargs : dict, optional (default: {}) Additional arguments for the plotting functions, e.g. alpha Returns ------- ax : Matplotlib 3d axis New or old axis """ from ..plot_utils import make_3d_axis, Frame if ax is None: ax = make_3d_axis(ax_s) if A2B is None: A2B = np.eye(4) A2B = check_transform(A2B, strict_check=strict_check) frame = Frame(A2B, name, s, **kwargs) frame.add_frame(ax) return ax def plot_screw( ax=None, q=np.zeros(3), s_axis=np.array([1.0, 0.0, 0.0]), h=1.0, theta=1.0, A2B=None, s=1.0, ax_s=1, alpha=1.0, **kwargs, ): """Plot transformation about and along screw axis. Parameters ---------- ax : Matplotlib 3d axis, optional (default: None) If the axis is None, a new 3d axis will be created q : array-like, shape (3,), optional (default: [0, 0, 0]) Vector to a point on the screw axis s_axis : array-like, shape (3,), optional (default: [1, 0, 0]) Direction vector of the screw axis h : float, optional (default: 1) Pitch of the screw. The pitch is the ratio of translation and rotation of the screw axis. Infinite pitch indicates pure translation. theta : float, optional (default: 1) Rotation angle. h * theta is the translation. A2B : array-like, shape (4, 4), optional (default: I) Origin of the screw s : float, optional (default: 1) Scaling of the axis and angle that will be drawn ax_s : float, optional (default: 1) Scaling of the new matplotlib 3d axis alpha : float, optional (default: 1) Alpha channel of plotted lines kwargs : dict, optional (default: {}) Additional arguments for the plotting functions, e.g. color Returns ------- ax : Matplotlib 3d axis New or old axis """ from ..plot_utils import make_3d_axis, Arrow3D from ..rotations import ( vector_projection, angle_between_vectors, perpendicular_to_vectors, slerp_weights, ) if ax is None: ax = make_3d_axis(ax_s) if A2B is None: A2B = np.eye(4) q, s_axis, h = check_screw_parameters(q, s_axis, h) origin_projected_on_screw_axis = q + vector_projection(-q, s_axis) pure_translation = np.isinf(h) if not pure_translation: screw_axis_to_old_frame = -origin_projected_on_screw_axis screw_axis_to_rotated_frame = perpendicular_to_vectors( s_axis, screw_axis_to_old_frame ) screw_axis_to_translated_frame = h * s_axis arc = np.empty((100, 3)) angle = angle_between_vectors( screw_axis_to_old_frame, screw_axis_to_rotated_frame ) for i, t in enumerate( zip( np.linspace(0, 2 * theta / np.pi, len(arc)), np.linspace(0.0, 1.0, len(arc)), ) ): t1, t2 = t w1, w2 = slerp_weights(angle, t1) arc[i] = ( origin_projected_on_screw_axis + w1 * screw_axis_to_old_frame + w2 * screw_axis_to_rotated_frame + screw_axis_to_translated_frame * t2 * theta ) q = transform(A2B, vector_to_point(q))[:3] s_axis = transform(A2B, vector_to_direction(s_axis))[:3] if not pure_translation: arc = transform(A2B, vectors_to_points(arc))[:, :3] origin_projected_on_screw_axis = transform( A2B, vector_to_point(origin_projected_on_screw_axis) )[:3] # Screw axis ax.scatter(q[0], q[1], q[2], color="r") if pure_translation: s_axis *= theta ax.scatter( q[0] + s_axis[0], q[1] + s_axis[1], q[2] + s_axis[2], color="r" ) ax.plot( [q[0] - s * s_axis[0], q[0] + (1 + s) * s_axis[0]], [q[1] - s * s_axis[1], q[1] + (1 + s) * s_axis[1]], [q[2] - s * s_axis[2], q[2] + (1 + s) * s_axis[2]], "--", c="k", alpha=alpha, ) axis_arrow = Arrow3D( [q[0], q[0] + s_axis[0]], [q[1], q[1] + s_axis[1]], [q[2], q[2] + s_axis[2]], mutation_scale=20, lw=3, arrowstyle="-|>", color="k", alpha=alpha, ) ax.add_artist(axis_arrow) if not pure_translation: # Transformation ax.plot( arc[:, 0], arc[:, 1], arc[:, 2], color="k", lw=3, alpha=alpha, **kwargs, ) arrow_coords = np.vstack((arc[-1], arc[-1] + (arc[-1] - arc[-2]))).T angle_arrow = Arrow3D( arrow_coords[0], arrow_coords[1], arrow_coords[2], mutation_scale=20, lw=3, arrowstyle="-|>", color="k", alpha=alpha, ) ax.add_artist(angle_arrow) for i in [0, -1]: arc_bound = np.vstack((origin_projected_on_screw_axis, arc[i])).T ax.plot( arc_bound[0], arc_bound[1], arc_bound[2], "--", c="k", alpha=alpha, ) return ax ================================================ FILE: pytransform3d/transformations/_plot.pyi ================================================ import numpy as np import numpy.typing as npt from mpl_toolkits.mplot3d import Axes3D from typing import Union def plot_transform( ax: Union[None, Axes3D] = ..., A2B: Union[None, npt.ArrayLike] = ..., s: float = ..., ax_s: float = ..., name: Union[None, str] = ..., strict_check: bool = ..., **kwargs, ) -> Axes3D: ... def plot_screw( ax: Union[None, Axes3D] = ..., q: npt.ArrayLike = ..., s_axis: npt.ArrayLike = ..., h: float = ..., theta: float = ..., A2B: Union[None, npt.ArrayLike] = ..., s: float = ..., ax_s: float = ..., alpha: float = ..., **kwargs, ) -> Axes3D: ... ================================================ FILE: pytransform3d/transformations/_pq.py ================================================ """Position+quaternion operations.""" import numpy as np from ._transform import transform_from from .. import rotations def check_pq(pq): """Input validation for position and orientation quaternion. Parameters ---------- pq : array-like, shape (7,) Position and orientation quaternion: (x, y, z, qw, qx, qy, qz) Returns ------- pq : array, shape (7,) Validated position and orientation quaternion: (x, y, z, qw, qx, qy, qz) Raises ------ ValueError If input is invalid """ pq = np.asarray(pq, dtype=np.float64) if pq.ndim != 1 or pq.shape[0] != 7: raise ValueError( "Expected position and orientation quaternion in a " "1D array, got array-like object with shape %s" % (pq.shape,) ) return pq def pq_slerp(start, end, t): """Spherical linear interpolation of position and quaternion. We will use spherical linear interpolation (SLERP) for the quaternion and linear interpolation for the position. Parameters ---------- start : array-like, shape (7,) Position and orientation quaternion: (x, y, z, qw, qx, qy, qz) end : array-like, shape (7,) Position and orientation quaternion: (x, y, z, qw, qx, qy, qz) t : float in [0, 1] Position between start and end Returns ------- pq_t : array-like, shape (7,) Position and orientation quaternion: (x, y, z, qw, qx, qy, qz) See Also -------- dual_quaternion_sclerp : An alternative approach is screw linear interpolation (ScLERP) with dual quaternions. pytransform3d.rotations.axis_angle_slerp : SLERP for axis-angle representation. pytransform3d.rotations.quaternion_slerp : SLERP for quaternions. pytransform3d.rotations.rotor_slerp : SLERP for rotors. """ start = check_pq(start) end = check_pq(end) start_p, start_q = np.array_split(start, (3,)) end_p, end_q = np.array_split(end, (3,)) q_t = rotations.quaternion_slerp(start_q, end_q, t, shortest_path=True) p_t = start_p + t * (end_p - start_p) return np.hstack((p_t, q_t)) def transform_from_pq(pq): """Compute transformation matrix from position and quaternion. Parameters ---------- pq : array-like, shape (7,) Position and orientation quaternion: (x, y, z, qw, qx, qy, qz) Returns ------- A2B : array, shape (4, 4) Transformation matrix from frame A to frame B """ pq = check_pq(pq) return transform_from(rotations.matrix_from_quaternion(pq[3:]), pq[:3]) def dual_quaternion_from_pq(pq): """Compute dual quaternion from position and quaternion. Parameters ---------- pq : array-like, shape (7,) Position and orientation quaternion: (x, y, z, qw, qx, qy, qz) Returns ------- dq : array, shape (8,) Unit dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) """ pq = check_pq(pq) real = pq[3:] dual = 0.5 * rotations.concatenate_quaternions(np.r_[0, pq[:3]], real) return np.hstack((real, dual)) ================================================ FILE: pytransform3d/transformations/_pq.pyi ================================================ import numpy as np import numpy.typing as npt def check_pq(pq: npt.ArrayLike) -> np.ndarray: ... def pq_slerp( start: npt.ArrayLike, end: npt.ArrayLike, t: float ) -> np.ndarray: ... def transform_from_pq(pq: npt.ArrayLike) -> np.ndarray: ... def dual_quaternion_from_pq(pq: npt.ArrayLike) -> np.ndarray: ... ================================================ FILE: pytransform3d/transformations/_random.py ================================================ """Random transform generation.""" import numpy as np from ._screws import transform_from_exponential_coordinates from ._transform import check_transform from ..rotations import norm_vector def random_transform( rng=np.random.default_rng(0), mean=np.eye(4), cov=np.eye(6) ): r"""Generate random transform. Generate :math:`\Delta \boldsymbol{T}_{B_{i+1}{B_i}} \boldsymbol{T}_{{B_i}A}`, with :math:`\Delta \boldsymbol{T}_{B_{i+1}{B_i}} = Exp(S \theta)` and :math:`\mathcal{S}\theta \sim \mathcal{N}(\boldsymbol{0}_6, \boldsymbol{\Sigma}_{6 \times 6})`. The mean :math:`\boldsymbol{T}_{{B_i}A}` and the covariance :math:`\boldsymbol{\Sigma}_{6 \times 6}` are parameters of the function. Note that uncertainty is defined in the global frame B, not in the body frame A. Parameters ---------- rng : np.random.Generator, optional (default: random seed 0) Random number generator mean : array-like, shape (4, 4), optional (default: I) Mean transform as homogeneous transformation matrix. cov : array-like, shape (6, 6), optional (default: I) Covariance of noise in exponential coordinate space. Returns ------- A2B : array, shape (4, 4) Random transform from frame A to frame B """ mean = check_transform(mean) Stheta = random_exponential_coordinates(rng=rng, cov=cov) delta = transform_from_exponential_coordinates(Stheta) return np.dot(delta, mean) def random_screw_axis(rng=np.random.default_rng(0)): r"""Generate random screw axis. Each component of v will be sampled from a standard normal distribution :math:`\mathcal{N}(\mu=0, \sigma=1)`. Components of :math:`\omega` will be sampled from a standard normal distribution and normalized. Parameters ---------- rng : np.random.Generator, optional (default: random seed 0) Random number generator Returns ------- screw_axis : array, shape (6,) Screw axis described by 6 values (omega_1, omega_2, omega_3, v_1, v_2, v_3), where the first 3 components are related to rotation and the last 3 components are related to translation. """ omega = norm_vector(rng.standard_normal(size=3)) v = rng.standard_normal(size=3) return np.hstack((omega, v)) def random_exponential_coordinates(rng=np.random.default_rng(0), cov=np.eye(6)): r"""Generate random exponential coordinates. Each component of Stheta will be sampled from a standard normal distribution :math:`\mathcal{N}(\boldsymbol{0}_6, \boldsymbol{\Sigma}_{6 \times 6})`. Parameters ---------- rng : np.random.Generator, optional (default: random seed 0) Random number generator cov : array-like, shape (6, 6), optional (default: I) Covariance of normal distribution. Returns ------- Stheta : array, shape (6,) Exponential coordinates of transformation: S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta, where the first 3 components are related to rotation and the last 3 components are related to translation. Theta is the rotation angle and h * theta the translation. Theta should be >= 0. Negative rotations will be represented by a negative screw axis instead. This is relevant if you want to recover theta from exponential coordinates. """ return rng.multivariate_normal(mean=np.zeros(6), cov=cov) ================================================ FILE: pytransform3d/transformations/_random.pyi ================================================ import numpy as np import numpy.typing as npt def random_transform( rng: np.random.Generator = ..., mean: npt.ArrayLike = ..., cov: npt.ArrayLike = ..., ) -> np.ndarray: ... def random_screw_axis(rng: np.random.Generator = ...) -> np.ndarray: ... def random_exponential_coordinates( rng: np.random.Generator = ..., cov: npt.ArrayLike = ... ) -> np.ndarray: ... ================================================ FILE: pytransform3d/transformations/_screws.py ================================================ """Conversions between transform representations.""" import numpy as np from numpy.testing import assert_array_almost_equal from ._transform import translate_transform from ..rotations import ( eps, norm_vector, norm_angle, cross_product_matrix, matrix_from_compact_axis_angle, check_skew_symmetric_matrix, left_jacobian_SO3, ) def check_screw_parameters(q, s_axis, h): r"""Input validation of screw parameters. The parameters :math:`(\boldsymbol{q}, \hat{\boldsymbol{s}}, h)` describe a screw. Parameters ---------- q : array-like, shape (3,) Vector to a point on the screw axis s_axis : array-like, shape (3,) Direction vector of the screw axis h : float Pitch of the screw. The pitch is the ratio of translation and rotation of the screw axis. Infinite pitch indicates pure translation. Returns ------- q : array, shape (3,) Vector to a point on the screw axis. Will be set to zero vector when pitch is infinite (pure translation). s_axis : array, shape (3,) Unit direction vector of the screw axis h : float Pitch of the screw. The pitch is the ratio of translation and rotation of the screw axis. Infinite pitch indicates pure translation. Raises ------ ValueError If input is invalid """ s_axis = np.asarray(s_axis, dtype=np.float64) if s_axis.ndim != 1 or s_axis.shape[0] != 3: raise ValueError( "Expected 3D vector with shape (3,), got array-like " "object with shape %s" % (s_axis.shape,) ) if np.linalg.norm(s_axis) == 0.0: raise ValueError("s_axis must not have norm 0") q = np.asarray(q, dtype=np.float64) if q.ndim != 1 or q.shape[0] != 3: raise ValueError( "Expected 3D vector with shape (3,), got array-like " "object with shape %s" % (q.shape,) ) if np.isinf(h): # pure translation q = np.zeros(3) return q, norm_vector(s_axis), h def check_screw_axis(screw_axis): r"""Input validation of screw axis. A screw axis .. math:: \mathcal{S} = \left[\begin{array}{c}\boldsymbol{\omega}\\ \boldsymbol{v}\end{array}\right] \in \mathbb{R}^6 consists of a part that describes rotation and a part that describes translation. Parameters ---------- screw_axis : array-like, shape (6,) Screw axis described by 6 values (omega_1, omega_2, omega_3, v_1, v_2, v_3), where the first 3 components are related to rotation and the last 3 components are related to translation. Returns ------- screw_axis : array, shape (6,) Screw axis described by 6 values (omega_1, omega_2, omega_3, v_1, v_2, v_3), where the first 3 components are related to rotation and the last 3 components are related to translation. Raises ------ ValueError If input is invalid """ screw_axis = np.asarray(screw_axis, dtype=np.float64) if screw_axis.ndim != 1 or screw_axis.shape[0] != 6: raise ValueError( "Expected 3D vector with shape (6,), got array-like " "object with shape %s" % (screw_axis.shape,) ) omega_norm = np.linalg.norm(screw_axis[:3]) if ( abs(omega_norm - 1.0) > 10.0 * np.finfo(float).eps and abs(omega_norm) > 10.0 * np.finfo(float).eps ): raise ValueError( "Norm of rotation axis must either be 0 or 1, but it is %g." % omega_norm ) if abs(omega_norm) < np.finfo(float).eps: v_norm = np.linalg.norm(screw_axis[3:]) if abs(v_norm - 1.0) > np.finfo(float).eps: raise ValueError( "If the norm of the rotation axis is 0, then the direction " "vector must have norm 1, but it is %g." % v_norm ) return screw_axis def check_exponential_coordinates(Stheta): """Input validation for exponential coordinates of transformation. Exponential coordinates of a transformation :math:`\\mathcal{S}\\theta \\in \\mathbb{R}^6` are the product of a screw axis and a scalar :math:`\\theta`. Parameters ---------- Stheta : array-like, shape (6,) Exponential coordinates of transformation: S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta, where the first 3 components are related to rotation and the last 3 components are related to translation. Theta is the rotation angle and h * theta the translation. Theta should be >= 0. Negative rotations will be represented by a negative screw axis instead. This is relevant if you want to recover theta from exponential coordinates. Returns ------- Stheta : array, shape (6,) Exponential coordinates of transformation: S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta, where the first 3 components are related to rotation and the last 3 components are related to translation. Theta is the rotation angle and h * theta the translation. Theta should be >= 0. Negative rotations will be represented by a negative screw axis instead. This is relevant if you want to recover theta from exponential coordinates. Raises ------ ValueError If input is invalid """ Stheta = np.asarray(Stheta, dtype=np.float64) if Stheta.ndim != 1 or Stheta.shape[0] != 6: raise ValueError( "Expected array-like with shape (6,), got array-like " "object with shape %s" % (Stheta.shape,) ) return Stheta def check_screw_matrix(screw_matrix, tolerance=1e-6, strict_check=True): """Input validation for screw matrix. A screw matrix consists of the cross-product matrix of a rotation axis and a translation. .. math:: \\left[\\mathcal S\\right] = \\left( \\begin{array}{cc} \\left[\\boldsymbol{\\omega}\\right] & \\boldsymbol v\\\\ \\boldsymbol 0 & 0\\\\ \\end{array} \\right) = \\left( \\begin{matrix} 0 & -\\omega_3 & \\omega_2 & v_1\\\\ \\omega_3 & 0 & -\\omega_1 & v_2\\\\ -\\omega_2 & \\omega_1 & 0 & v_3\\\\ 0 & 0 & 0 & 0\\\\ \\end{matrix} \\right) \\in se(3) \\subset \\mathbb{R}^{4 \\times 4} Parameters ---------- screw_matrix : array-like, shape (4, 4) A screw matrix consists of a cross-product matrix that represents an axis of rotation, a translation, and a row of zeros. tolerance : float, optional (default: 1e-6) Tolerance threshold for checks. strict_check : bool, optional (default: True) Raise a ValueError if [omega].T is not numerically close enough to -[omega]. Otherwise we print a warning. Returns ------- screw_matrix : array, shape (4, 4) A screw matrix consists of a cross-product matrix that represents an axis of rotation, a translation, and a row of zeros. Raises ------ ValueError If input is invalid """ screw_matrix = np.asarray(screw_matrix, dtype=np.float64) if ( screw_matrix.ndim != 2 or screw_matrix.shape[0] != 4 or screw_matrix.shape[1] != 4 ): raise ValueError( "Expected array-like with shape (4, 4), got array-like " "object with shape %s" % (screw_matrix.shape,) ) if any(screw_matrix[3] != 0.0): raise ValueError("Last row of screw matrix must only contains zeros.") check_skew_symmetric_matrix(screw_matrix[:3, :3], tolerance, strict_check) omega_norm = np.linalg.norm( [screw_matrix[2, 1], screw_matrix[0, 2], screw_matrix[1, 0]] ) if ( abs(omega_norm - 1.0) > np.finfo(float).eps and abs(omega_norm) > np.finfo(float).eps ): raise ValueError( "Norm of rotation axis must either be 0 or 1, but it is %g." % omega_norm ) if abs(omega_norm) < np.finfo(float).eps: v_norm = np.linalg.norm(screw_matrix[:3, 3]) if ( abs(v_norm - 1.0) > np.finfo(float).eps and abs(v_norm) > np.finfo(float).eps ): raise ValueError( "If the norm of the rotation axis is 0, then the direction " "vector must have norm 1 or 0, but it is %g." % v_norm ) return screw_matrix def check_transform_log(transform_log, tolerance=1e-6, strict_check=True): """Input validation for logarithm of transformation. The logarithm of a transformation :math:`\\left[\\mathcal{S}\\right]\\theta \\in se(3) \\subset \\mathbb{R}^{4 \\times 4}` are the product of a screw matrix and a scalar :math:`\\theta`. Parameters ---------- transform_log : array-like, shape (4, 4) Matrix logarithm of transformation matrix: [S] * theta. tolerance : float, optional (default: 1e-6) Tolerance threshold for checks. strict_check : bool, optional (default: True) Raise a ValueError if [omega].T is not numerically close enough to -[omega]. Otherwise we print a warning. Returns ------- transform_log : array, shape (4, 4) Matrix logarithm of transformation matrix: [S] * theta. Raises ------ ValueError If input is invalid """ transform_log = np.asarray(transform_log, dtype=np.float64) if ( transform_log.ndim != 2 or transform_log.shape[0] != 4 or transform_log.shape[1] != 4 ): raise ValueError( "Expected array-like with shape (4, 4), got array-like " "object with shape %s" % (transform_log.shape,) ) if any(transform_log[3] != 0.0): raise ValueError( "Last row of logarithm of transformation must only " "contains zeros." ) check_skew_symmetric_matrix(transform_log[:3, :3], tolerance, strict_check) return transform_log def norm_exponential_coordinates(Stheta): """Normalize exponential coordinates of transformation. Parameters ---------- Stheta : array-like, shape (6,) Exponential coordinates of transformation: S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta, where the first 3 components are related to rotation and the last 3 components are related to translation. Theta is the rotation angle and h * theta the translation. Theta should be >= 0. Negative rotations will be represented by a negative screw axis instead. This is relevant if you want to recover theta from exponential coordinates. Returns ------- Stheta : array, shape (6,) Normalized exponential coordinates of transformation with theta in [0, pi]. Note that in the case of pure translation no normalization is required because the representation is unique. In the case of rotation by pi, there is an ambiguity that will be resolved so that the screw pitch is positive. """ theta = np.linalg.norm(Stheta[:3]) if theta == 0.0: return Stheta screw_axis = Stheta / theta q, s_axis, h = screw_parameters_from_screw_axis(screw_axis) if abs(theta - np.pi) < eps and h < 0: h *= -1.0 s_axis *= -1.0 theta_normed = norm_angle(theta) h_normalized = h * theta / theta_normed screw_axis = screw_axis_from_screw_parameters(q, s_axis, h_normalized) return screw_axis * theta_normed def assert_exponential_coordinates_equal(Stheta1, Stheta2): """Raise an assertion if exp. coordinates are not approximately equal. Parameters ---------- Stheta1 : array-like, shape (6,) Exponential coordinates of transformation: S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta, where the first 3 components are related to rotation and the last 3 components are related to translation. Stheta2 : array-like, shape (6,) Exponential coordinates of transformation: S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta, where the first 3 components are related to rotation and the last 3 components are related to translation. args : tuple Positional arguments that will be passed to `assert_array_almost_equal` kwargs : dict Positional arguments that will be passed to `assert_array_almost_equal` """ Stheta1 = norm_exponential_coordinates(Stheta1) Stheta2 = norm_exponential_coordinates(Stheta2) assert_array_almost_equal(Stheta1, Stheta2) def assert_screw_parameters_equal( q1, s_axis1, h1, theta1, q2, s_axis2, h2, theta2, *args, **kwargs ): """Raise an assertion if two sets of screw parameters are not similar. Note that the screw axis can be inverted. In this case theta and h have to be adapted. Parameters ---------- q1 : array, shape (3,) Vector to a point on the screw axis that is orthogonal to s_axis s_axis1 : array, shape (3,) Unit direction vector of the screw axis h1 : float Pitch of the screw. The pitch is the ratio of translation and rotation of the screw axis. Infinite pitch indicates pure translation. theta1 : float Parameter of the transformation: theta is the angle of rotation and h * theta the translation. q2 : array, shape (3,) Vector to a point on the screw axis that is orthogonal to s_axis s_axis2 : array, shape (3,) Unit direction vector of the screw axis h2 : float Pitch of the screw. The pitch is the ratio of translation and rotation of the screw axis. Infinite pitch indicates pure translation. theta2 : float Parameter of the transformation: theta is the angle of rotation and h * theta the translation. args : tuple Positional arguments that will be passed to `assert_array_almost_equal` kwargs : dict Positional arguments that will be passed to `assert_array_almost_equal` """ # normalize thetas theta1_new = norm_angle(theta1) h1 *= theta1 / theta1_new theta1 = theta1_new theta2_new = norm_angle(theta2) h2 *= theta2 / theta2_new theta2 = theta2_new # q1 and q2 can be any points on the screw axis, that is, they must be a # linear combination of each other and the screw axis (which one does not # matter since they should be identical or mirrored) q1_to_q2 = q2 - q1 factors = q1_to_q2 / s_axis2 assert abs(factors[0] - factors[1]) < eps assert abs(factors[1] - factors[2]) < eps try: assert_array_almost_equal(s_axis1, s_axis2, *args, **kwargs) assert abs(h1 - h2) < eps assert abs(theta1 - theta2) < eps except AssertionError: # possibly mirrored screw axis s_axis1_new = -s_axis1 # make sure that we keep the direction of rotation theta1_new = 2.0 * np.pi - theta1 # adjust pitch: switch sign and update rotation component h1 = -h1 / theta1_new * theta1 theta1 = theta1_new # we have to normalize the angle again theta1_new = norm_angle(theta1) h1 *= theta1 / theta1_new theta1 = theta1_new assert_array_almost_equal(s_axis1_new, s_axis2, *args, **kwargs) assert abs(h1 - h2) < eps assert abs(theta1 - theta2) < eps def screw_parameters_from_screw_axis(screw_axis): """Compute screw parameters from screw axis. Note that there is not just one solution since q can be any point on the screw axis. We select q so that it is orthogonal to s_axis. Parameters ---------- screw_axis : array-like, shape (6,) Screw axis described by 6 values (omega_1, omega_2, omega_3, v_1, v_2, v_3), where the first 3 components are related to rotation and the last 3 components are related to translation. Returns ------- q : array, shape (3,) Vector to a point on the screw axis that is orthogonal to s_axis s_axis : array, shape (3,) Unit direction vector of the screw axis h : float Pitch of the screw. The pitch is the ratio of translation and rotation of the screw axis. Infinite pitch indicates pure translation. """ screw_axis = check_screw_axis(screw_axis) omega = screw_axis[:3] v = screw_axis[3:] omega_norm = np.linalg.norm(omega) if abs(omega_norm) < np.finfo(float).eps: # pure translation q = np.zeros(3) s_axis = v h = np.inf else: s_axis = omega h = omega.dot(v) moment = v - h * s_axis q = np.cross(s_axis, moment) return q, s_axis, h def screw_axis_from_screw_parameters(q, s_axis, h): """Compute screw axis representation from screw parameters. Parameters ---------- q : array-like, shape (3,) Vector to a point on the screw axis s_axis : array-like, shape (3,) Direction vector of the screw axis h : float Pitch of the screw. The pitch is the ratio of translation and rotation of the screw axis. Infinite pitch indicates pure translation. Returns ------- screw_axis : array, shape (6,) Screw axis described by 6 values (omega_1, omega_2, omega_3, v_1, v_2, v_3), where the first 3 components are related to rotation and the last 3 components are related to translation. """ q, s_axis, h = check_screw_parameters(q, s_axis, h) if np.isinf(h): # pure translation return np.r_[0.0, 0.0, 0.0, s_axis] return np.r_[s_axis, np.cross(q, s_axis) + h * s_axis] def screw_axis_from_exponential_coordinates(Stheta): """Compute screw axis and theta from exponential coordinates. Parameters ---------- Stheta : array-like, shape (6,) Exponential coordinates of transformation: S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta, where the first 3 components are related to rotation and the last 3 components are related to translation. Theta is the rotation angle and h * theta the translation. Theta should be >= 0. Negative rotations will be represented by a negative screw axis instead. This is relevant if you want to recover theta from exponential coordinates. Returns ------- screw_axis : array, shape (6,) Screw axis described by 6 values (omega_1, omega_2, omega_3, v_1, v_2, v_3), where the first 3 components are related to rotation and the last 3 components are related to translation. theta : float Parameter of the transformation: theta is the angle of rotation and h * theta the translation. """ Stheta = check_exponential_coordinates(Stheta) omega_theta = Stheta[:3] v_theta = Stheta[3:] theta = np.linalg.norm(omega_theta) if theta < np.finfo(float).eps: theta = np.linalg.norm(v_theta) if theta < np.finfo(float).eps: return np.zeros(6), 0.0 return Stheta / theta, theta def screw_axis_from_screw_matrix(screw_matrix): """Compute screw axis from screw matrix. Parameters ---------- screw_matrix : array-like, shape (4, 4) A screw matrix consists of a cross-product matrix that represents an axis of rotation, a translation, and a row of zeros. Returns ------- screw_axis : array, shape (6,) Screw axis described by 6 values (omega_1, omega_2, omega_3, v_1, v_2, v_3), where the first 3 components are related to rotation and the last 3 components are related to translation. """ screw_matrix = check_screw_matrix(screw_matrix) screw_axis = np.empty(6) screw_axis[0] = screw_matrix[2, 1] screw_axis[1] = screw_matrix[0, 2] screw_axis[2] = screw_matrix[1, 0] screw_axis[3:] = screw_matrix[:3, 3] return screw_axis def exponential_coordinates_from_screw_axis(screw_axis, theta): """Compute exponential coordinates from screw axis and theta. Parameters ---------- screw_axis : array-like, shape (6,) Screw axis described by 6 values (omega_1, omega_2, omega_3, v_1, v_2, v_3), where the first 3 components are related to rotation and the last 3 components are related to translation. theta : float Parameter of the transformation: theta is the angle of rotation and h * theta the translation. Returns ------- Stheta : array, shape (6,) Exponential coordinates of transformation: S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta, where the first 3 components are related to rotation and the last 3 components are related to translation. Theta is the rotation angle and h * theta the translation. Theta should be >= 0. Negative rotations will be represented by a negative screw axis instead. This is relevant if you want to recover theta from exponential coordinates. """ screw_axis = check_screw_axis(screw_axis) return screw_axis * theta def exponential_coordinates_from_transform_log(transform_log, check=True): r"""Compute exponential coordinates from logarithm of transformation. Extracts the vector :math:`\mathcal{S} \theta = (\hat{\boldsymbol{\omega}}, \boldsymbol{v}) \theta \in \mathbb{R}^6` from the matrix .. math:: \left( \begin{array}{cccc} 0 & -\omega_3 & \omega_2 & v_1\\ \omega_3 & 0 & -\omega_1 & v_2\\ -\omega_2 & \omega_1 & 0 & v_3\\ 0 & 0 & 0 & 0 \end{array} \right) \theta = \left[ \mathcal{S} \right] \theta \in so(3). Parameters ---------- transform_log : array-like, shape (4, 4) Matrix logarithm of transformation matrix: [S] * theta. check : bool, optional (default: True) Check if logarithm of transformation is valid Returns ------- Stheta : array, shape (6,) Exponential coordinates of transformation: S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta, where S is the screw axis, the first 3 components are related to rotation and the last 3 components are related to translation. Theta is the rotation angle and h * theta the translation. """ if check: transform_log = check_transform_log(transform_log) Stheta = np.empty(6) Stheta[0] = transform_log[2, 1] Stheta[1] = transform_log[0, 2] Stheta[2] = transform_log[1, 0] Stheta[3:] = transform_log[:3, 3] return Stheta def screw_matrix_from_screw_axis(screw_axis): """Compute screw matrix from screw axis. Parameters ---------- screw_axis : array-like, shape (6,) Screw axis described by 6 values (omega_1, omega_2, omega_3, v_1, v_2, v_3), where the first 3 components are related to rotation and the last 3 components are related to translation. Returns ------- screw_matrix : array, shape (4, 4) A screw matrix consists of a cross-product matrix that represents an axis of rotation, a translation, and a row of zeros. """ screw_axis = check_screw_axis(screw_axis) omega = screw_axis[:3] v = screw_axis[3:] screw_matrix = np.zeros((4, 4)) screw_matrix[:3, :3] = cross_product_matrix(omega) screw_matrix[:3, 3] = v return screw_matrix def screw_matrix_from_transform_log(transform_log): """Compute screw matrix from logarithm of transformation. Parameters ---------- transform_log : array-like, shape (4, 4) Matrix logarithm of transformation matrix: [S] * theta. Returns ------- screw_matrix : array, shape (4, 4) A screw matrix consists of a cross-product matrix that represents an axis of rotation, a translation, and a row of zeros. """ transform_log = check_transform_log(transform_log) omega = np.array( [transform_log[2, 1], transform_log[0, 2], transform_log[1, 0]] ) theta = np.linalg.norm(omega) if abs(theta) < np.finfo(float).eps: theta = np.linalg.norm(transform_log[:3, 3]) if abs(theta) < np.finfo(float).eps: return np.zeros((4, 4)), 0.0 return transform_log / theta, theta def transform_log_from_exponential_coordinates(Stheta): r"""Compute matrix logarithm of transformation from exponential coordinates. Builds the matrix .. math:: \left( \begin{array}{cccc} 0 & -\omega_3 & \omega_2 & v_1\\ \omega_3 & 0 & -\omega_1 & v_2\\ -\omega_2 & \omega_1 & 0 & v_3\\ 0 & 0 & 0 & 0 \end{array} \right) \theta = \left[ \mathcal{S} \right] \theta \in so(3) from the vector :math:`\mathcal{S} \theta = (\hat{\boldsymbol{\omega}}, \boldsymbol{v}) \theta \in \mathbb{R}^6`. Parameters ---------- Stheta : array-like, shape (6,) Exponential coordinates of transformation: S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta, where S is the screw axis, the first 3 components are related to rotation and the last 3 components are related to translation. Theta is the rotation angle and h * theta the translation. Returns ------- transform_log : array, shape (4, 4) Matrix logarithm of transformation matrix: [S] * theta. """ check_exponential_coordinates(Stheta) omega = Stheta[:3] v = Stheta[3:] transform_log = np.zeros((4, 4)) transform_log[:3, :3] = cross_product_matrix(omega) transform_log[:3, 3] = v return transform_log def transform_log_from_screw_matrix(screw_matrix, theta): """Compute matrix logarithm of transformation from screw matrix and theta. Parameters ---------- screw_matrix : array-like, shape (4, 4) A screw matrix consists of a cross-product matrix that represents an axis of rotation, a translation, and a row of zeros. theta : float Parameter of the transformation: theta is the angle of rotation and h * theta the translation. Returns ------- transform_log : array, shape (4, 4) Matrix logarithm of transformation matrix: [S] * theta. """ screw_matrix = check_screw_matrix(screw_matrix) return screw_matrix * theta def transform_from_exponential_coordinates(Stheta, check=True): r"""Compute transformation matrix from exponential coordinates. Exponential map. .. math:: Exp: \mathcal{S} \theta \in \mathbb{R}^6 \rightarrow \boldsymbol{T} \in SE(3) .. math:: Exp(\mathcal{S}\theta) = Exp\left(\left(\begin{array}{c} \hat{\boldsymbol{\omega}}\\ \boldsymbol{v} \end{array}\right)\theta\right) = \exp(\left[\mathcal{S}\right] \theta) = \left(\begin{array}{cc} Exp(\hat{\boldsymbol{\omega}} \theta) & \boldsymbol{J}(\theta)\boldsymbol{v}\theta\\ \boldsymbol{0} & 1 \end{array}\right), where :math:`\boldsymbol{J}(\theta)` is the left Jacobian of :math:`SO(3)` (see :func:`~pytransform3d.rotations.left_jacobian_SO3`). Parameters ---------- Stheta : array-like, shape (6,) Exponential coordinates of transformation: S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta, where S is the screw axis, the first 3 components are related to rotation and the last 3 components are related to translation. Theta is the rotation angle and h * theta the translation. check : bool, optional (default: True) Check if exponential coordinates are valid Returns ------- A2B : array, shape (4, 4) Transformation matrix from frame A to frame B """ if check: Stheta = check_exponential_coordinates(Stheta) omega_theta = Stheta[:3] theta = np.linalg.norm(omega_theta) if theta == 0.0: # only translation return translate_transform(np.eye(4), Stheta[3:], check=check) omega_theta = Stheta[:3] v_theta = Stheta[3:] A2B = np.eye(4) A2B[:3, :3] = matrix_from_compact_axis_angle(omega_theta) J = left_jacobian_SO3(omega_theta) A2B[:3, 3] = np.dot(J, v_theta) return A2B def transform_from_transform_log(transform_log): r"""Compute transformation from matrix logarithm of transformation. Exponential map. .. math:: \exp: \left[ \mathcal{S} \right] \theta \in se(3) \rightarrow \boldsymbol{T} \in SE(3) .. math:: \exp([\mathcal{S}]\theta) = \exp\left(\left(\begin{array}{cc} \left[\hat{\boldsymbol{\omega}}\right] & \boldsymbol{v}\\ \boldsymbol{0} & 0 \end{array}\right)\theta\right) = \left(\begin{array}{cc} Exp(\hat{\boldsymbol{\omega}} \theta) & \boldsymbol{J}(\theta)\boldsymbol{v}\theta\\ \boldsymbol{0} & 1 \end{array}\right), where :math:`\boldsymbol{J}(\theta)` is the left Jacobian of :math:`SO(3)` (see :func:`~pytransform3d.rotations.left_jacobian_SO3`). Parameters ---------- transform_log : array-like, shape (4, 4) Matrix logarithm of transformation matrix: [S] * theta. Returns ------- A2B : array, shape (4, 4) Transform from frame A to frame B """ transform_log = check_transform_log(transform_log) omega_theta = np.array( [transform_log[2, 1], transform_log[0, 2], transform_log[1, 0]] ) v_theta = transform_log[:3, 3] theta = np.linalg.norm(omega_theta) if theta == 0.0: # only translation return translate_transform(np.eye(4), v_theta) A2B = np.eye(4) A2B[:3, :3] = matrix_from_compact_axis_angle(omega_theta) J = left_jacobian_SO3(omega_theta) A2B[:3, 3] = np.dot(J, v_theta) return A2B def dual_quaternion_from_screw_parameters(q, s_axis, h, theta): """Compute dual quaternion from screw parameters. Parameters ---------- q : array-like, shape (3,) Vector to a point on the screw axis s_axis : array-like, shape (3,) Direction vector of the screw axis h : float Pitch of the screw. The pitch is the ratio of translation and rotation of the screw axis. Infinite pitch indicates pure translation. theta : float Parameter of the transformation: theta is the angle of rotation and h * theta the translation. Returns ------- dq : array, shape (8,) Unit dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) """ q, s_axis, h = check_screw_parameters(q, s_axis, h) if np.isinf(h): # pure translation d = theta theta = 0 else: d = h * theta moment = np.cross(q, s_axis) half_distance = 0.5 * d sin_half_angle = np.sin(0.5 * theta) cos_half_angle = np.cos(0.5 * theta) real_w = cos_half_angle real_vec = sin_half_angle * s_axis dual_w = -half_distance * sin_half_angle dual_vec = sin_half_angle * moment + half_distance * cos_half_angle * s_axis return np.r_[real_w, real_vec, dual_w, dual_vec] ================================================ FILE: pytransform3d/transformations/_screws.pyi ================================================ import numpy as np import numpy.typing as npt from typing import Tuple def check_screw_parameters( q: npt.ArrayLike, s_axis: npt.ArrayLike, h: float ) -> Tuple[np.ndarray, np.ndarray, float]: ... def check_screw_axis(screw_axis: npt.ArrayLike) -> np.ndarray: ... def check_exponential_coordinates(Stheta: npt.ArrayLike) -> np.ndarray: ... def check_screw_matrix( screw_matrix: npt.ArrayLike, tolerance: float = ..., strict_check: bool = ..., ) -> np.ndarray: ... def check_transform_log( transform_log: npt.ArrayLike, tolerance: float = ..., strict_check: bool = ..., ) -> np.ndarray: ... def norm_exponential_coordinates(Stheta: npt.ArrayLike) -> np.ndarray: ... def assert_exponential_coordinates_equal( Stheta1: npt.ArrayLike, Stheta2: npt.ArrayLike ): ... def assert_screw_parameters_equal( q1: npt.ArrayLike, s_axis1: npt.ArrayLike, h1: float, theta1: float, q2: npt.ArrayLike, s_axis2: npt.ArrayLike, h2: float, theta2: float, *args, **kwargs, ): ... def screw_parameters_from_screw_axis( screw_axis: npt.ArrayLike, ) -> Tuple[np.ndarray, np.ndarray, float]: ... def screw_axis_from_screw_parameters( q: npt.ArrayLike, s_axis: npt.ArrayLike, h: float ) -> np.ndarray: ... def screw_axis_from_exponential_coordinates( Stheta: npt.ArrayLike, ) -> Tuple[np.ndarray, float]: ... def screw_axis_from_screw_matrix(screw_matrix: npt.ArrayLike) -> np.ndarray: ... def exponential_coordinates_from_screw_axis( screw_axis: npt.ArrayLike, theta: float ) -> np.ndarray: ... def exponential_coordinates_from_transform_log( transform_log: npt.ArrayLike, check: bool = ... ) -> np.ndarray: ... def screw_matrix_from_screw_axis(screw_axis: npt.ArrayLike) -> np.ndarray: ... def screw_matrix_from_transform_log( transform_log: npt.ArrayLike, ) -> np.ndarray: ... def transform_log_from_exponential_coordinates( Stheta: npt.ArrayLike, ) -> np.ndarray: ... def transform_log_from_screw_matrix( screw_matrix: npt.ArrayLike, theta: float ) -> np.ndarray: ... def transform_from_exponential_coordinates( Stheta: npt.ArrayLike, check: bool = ... ) -> np.ndarray: ... def transform_from_transform_log( transform_log: npt.ArrayLike, ) -> np.ndarray: ... def dual_quaternion_from_screw_parameters( q: npt.ArrayLike, s_axis: npt.ArrayLike, h: float, theta: float ) -> np.ndarray: ... ================================================ FILE: pytransform3d/transformations/_transform.py ================================================ """Transformation matrices.""" import warnings import numpy as np from numpy.testing import assert_array_almost_equal from ..rotations import ( matrix_requires_renormalization, check_matrix, assert_rotation_matrix, quaternion_from_matrix, compact_axis_angle_from_matrix, left_jacobian_SO3_inv, cross_product_matrix, concatenate_quaternions, ) def check_transform(A2B, strict_check=True): """Input validation of transform. Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. Returns ------- A2B : array, shape (4, 4) Validated transform from frame A to frame B Raises ------ ValueError If input is invalid """ A2B = np.asarray(A2B, dtype=np.float64) if A2B.ndim != 2 or A2B.shape[0] != 4 or A2B.shape[1] != 4: raise ValueError( "Expected homogeneous transformation matrix with " "shape (4, 4), got array-like object with shape %s" % (A2B.shape,) ) check_matrix(A2B[:3, :3], strict_check=strict_check) if not np.allclose(A2B[3], np.array([0.0, 0.0, 0.0, 1.0])): error_msg = ( "Excpected homogeneous transformation matrix with " "[0, 0, 0, 1] at the bottom, got %r" % A2B ) if strict_check: raise ValueError(error_msg) warnings.warn(error_msg, UserWarning, stacklevel=2) return A2B def transform_requires_renormalization(A2B, tolerance=1e-6): r"""Check if transformation matrix requires renormalization. This function will check if :math:`R R^T \approx I`. Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B with a rotation matrix that should be orthonormal. tolerance : float, optional (default: 1e-6) Tolerance for check. Returns ------- required : bool Indicates if renormalization is required. See Also -------- pytransform3d.rotations.matrix_requires_renormalization Check if a rotation matrix needs renormalization. pytransform3d.rotations.norm_matrix : Orthonormalize rotation matrix. """ return matrix_requires_renormalization(np.asarray(A2B[:3, :3]), tolerance) def assert_transform(A2B, *args, **kwargs): """Raise an assertion if the transform is not a homogeneous matrix. See numpy.testing.assert_array_almost_equal for a more detailed documentation of the other parameters. Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B args : tuple Positional arguments that will be passed to `assert_array_almost_equal` kwargs : dict Positional arguments that will be passed to `assert_array_almost_equal` """ assert_rotation_matrix(A2B[:3, :3], *args, **kwargs) assert_array_almost_equal( A2B[3], np.array([0.0, 0.0, 0.0, 1.0]), *args, **kwargs ) def transform_from(R, p, strict_check=True): r"""Make transformation from rotation matrix and translation. .. math:: \boldsymbol{T}_{BA} = \left( \begin{array}{cc} \boldsymbol{R} & \boldsymbol{p}\\ \boldsymbol{0} & 1 \end{array} \right) \in SE(3) Parameters ---------- R : array-like, shape (3, 3) Rotation matrix p : array-like, shape (3,) Translation strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. Returns ------- A2B : array, shape (4, 4) Transform from frame A to frame B """ A2B = rotate_transform(np.eye(4), R, strict_check=strict_check, check=False) A2B = translate_transform(A2B, p, strict_check=strict_check, check=False) return A2B def translate_transform(A2B, p, strict_check=True, check=True): """Sets the translation of a transform. Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B p : array-like, shape (3,) Translation strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. check : bool, optional (default: True) Check if transformation matrix is valid Returns ------- A2B : array, shape (4, 4) Transform from frame A to frame B """ if check: A2B = check_transform(A2B, strict_check=strict_check) out = A2B.copy() out[:3, -1] = p return out def rotate_transform(A2B, R, strict_check=True, check=True): """Sets the rotation of a transform. Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B R : array-like, shape (3, 3) Rotation matrix strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. check : bool, optional (default: True) Check if transformation matrix is valid Returns ------- A2B : array, shape (4, 4) Transform from frame A to frame B """ if check: A2B = check_transform(A2B, strict_check=strict_check) out = A2B.copy() out[:3, :3] = R return out def pq_from_transform(A2B, strict_check=True): """Compute position and quaternion from transformation matrix. Parameters ---------- A2B : array-like, shape (4, 4) Transformation matrix from frame A to frame B strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. Returns ------- pq : array, shape (7,) Position and orientation quaternion: (x, y, z, qw, qx, qy, qz) """ A2B = check_transform(A2B, strict_check=strict_check) return np.hstack((A2B[:3, 3], quaternion_from_matrix(A2B[:3, :3]))) def transform_log_from_transform(A2B, strict_check=True): r"""Compute matrix logarithm of transformation from transformation. Logarithmic map. .. math:: \log: \boldsymbol{T} \in SE(3) \rightarrow \left[ \mathcal{S} \right] \theta \in se(3) .. math:: \log(\boldsymbol{T}) = \log\left( \begin{array}{cc} \boldsymbol{R} & \boldsymbol{p}\\ \boldsymbol{0} & 1 \end{array} \right) = \left( \begin{array}{cc} \log\boldsymbol{R} & \boldsymbol{J}^{-1}(\theta) \boldsymbol{p}\\ \boldsymbol{0} & 0 \end{array} \right) = \left( \begin{array}{cc} \hat{\boldsymbol{\omega}} \theta & \boldsymbol{v} \theta\\ \boldsymbol{0} & 0 \end{array} \right) = \left[\mathcal{S}\right]\theta, where :math:`\boldsymbol{J}^{-1}(\theta)` is the inverse left Jacobian of :math:`SO(3)` (see :func:`~pytransform3d.rotations.left_jacobian_SO3_inv`). Parameters ---------- A2B : array, shape (4, 4) Transform from frame A to frame B strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. Returns ------- transform_log : array, shape (4, 4) Matrix logarithm of transformation matrix: [S] * theta. """ A2B = check_transform(A2B, strict_check=strict_check) R = A2B[:3, :3] p = A2B[:3, 3] transform_log = np.zeros((4, 4)) if np.linalg.norm(np.eye(3) - R) < np.finfo(float).eps: transform_log[:3, 3] = p return transform_log omega_theta = compact_axis_angle_from_matrix(R) theta = np.linalg.norm(omega_theta) if theta == 0: return transform_log J_inv = left_jacobian_SO3_inv(omega_theta) v_theta = np.dot(J_inv, p) transform_log[:3, :3] = cross_product_matrix(omega_theta) transform_log[:3, 3] = v_theta return transform_log def exponential_coordinates_from_transform(A2B, strict_check=True, check=True): r"""Compute exponential coordinates from transformation matrix. Logarithmic map. .. math:: Log: \boldsymbol{T} \in SE(3) \rightarrow \mathcal{S} \theta \in \mathbb{R}^6 .. math:: Log(\boldsymbol{T}) = Log\left( \begin{array}{cc} \boldsymbol{R} & \boldsymbol{p}\\ \boldsymbol{0} & 1 \end{array} \right) = \left( \begin{array}{c} Log(\boldsymbol{R})\\ \boldsymbol{J}^{-1}(\theta) \boldsymbol{p} \end{array} \right) = \left( \begin{array}{c} \hat{\boldsymbol{\omega}}\\ \boldsymbol{v} \end{array} \right) \theta = \mathcal{S}\theta, where :math:`\boldsymbol{J}^{-1}(\theta)` is the inverse left Jacobian of :math:`SO(3)` (see :func:`~pytransform3d.rotations.left_jacobian_SO3_inv`). Parameters ---------- A2B : array-like, shape (4, 4) Transformation matrix from frame A to frame B strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. check : bool, optional (default: True) Check if transformation matrix is valid Returns ------- Stheta : array, shape (6,) Exponential coordinates of transformation: S * theta = (omega_1, omega_2, omega_3, v_1, v_2, v_3) * theta, where S is the screw axis, the first 3 components are related to rotation and the last 3 components are related to translation. Theta is the rotation angle and h * theta the translation. """ if check: A2B = check_transform(A2B, strict_check=strict_check) R = A2B[:3, :3] p = A2B[:3, 3] if np.linalg.norm(np.eye(3) - R) < np.finfo(float).eps: return np.r_[0.0, 0.0, 0.0, p] omega_theta = compact_axis_angle_from_matrix(R, check=check) theta = np.linalg.norm(omega_theta) if theta == 0: return np.r_[0.0, 0.0, 0.0, p] v_theta = np.dot(left_jacobian_SO3_inv(omega_theta), p) return np.hstack((omega_theta, v_theta)) def dual_quaternion_from_transform(A2B): """Compute dual quaternion from transformation matrix. Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B Returns ------- dq : array, shape (8,) Unit dual quaternion to represent transform: (pw, px, py, pz, qw, qx, qy, qz) """ A2B = check_transform(A2B) real = quaternion_from_matrix(A2B[:3, :3]) dual = 0.5 * concatenate_quaternions(np.r_[0, A2B[:3, 3]], real) return np.hstack((real, dual)) def adjoint_from_transform(A2B, strict_check=True, check=True): r"""Compute adjoint representation of a transformation matrix. The adjoint representation of a transformation :math:`\left[Ad_{\boldsymbol{T}_{BA}}\right] \in \mathbb{R}^{6 \times 6}` from frame A to frame B translates a twist from frame A to frame B through the adjoint map .. math:: \mathcal{V}_{B} = \left[Ad_{\boldsymbol{T}_{BA}}\right] \mathcal{V}_A The corresponding transformation matrix operation is .. math:: \left[\mathcal{V}_{B}\right] = \boldsymbol{T}_{BA} \left[\mathcal{V}_A\right] \boldsymbol{T}_{BA}^{-1} We can also use the adjoint representation to transform a wrench from frame A to frame B: .. math:: \mathcal{F}_B = \left[ Ad_{\boldsymbol{T}_{AB}} \right]^T \mathcal{F}_A Note that not only the adjoint is transposed but also the transformation is inverted. Adjoint representations have the following properties: .. math:: \left[Ad_{\boldsymbol{T}_1 \boldsymbol{T}_2}\right] = \left[Ad_{\boldsymbol{T}_1}\right] \left[Ad_{\boldsymbol{T}_2}\right] .. math:: \left[Ad_{\boldsymbol{T}}\right]^{-1} = \left[Ad_{\boldsymbol{T}^{-1}}\right] For a transformation matrix .. math:: \boldsymbol T = \left( \begin{array}{cc} \boldsymbol R & \boldsymbol t\\ \boldsymbol 0 & 1\\ \end{array} \right) the adjoint is defined as .. math:: \left[Ad_{\boldsymbol{T}}\right] = \left( \begin{array}{cc} \boldsymbol R & \boldsymbol 0\\ \left[\boldsymbol{t}\right]_{\times}\boldsymbol R & \boldsymbol R\\ \end{array} \right), where :math:`\left[\boldsymbol{t}\right]_{\times}` is the cross-product matrix (see :func:`~pytransform3d.rotations.cross_product_matrix`) of the translation component. Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. check : bool, optional (default: True) Check if transformation matrix is valid Returns ------- adj_A2B : array, shape (6, 6) Adjoint representation of transformation matrix """ if check: A2B = check_transform(A2B, strict_check) R = A2B[:3, :3] p = A2B[:3, 3] adj_A2B = np.zeros((6, 6)) adj_A2B[:3, :3] = R adj_A2B[3:, :3] = np.dot(cross_product_matrix(p), R) adj_A2B[3:, 3:] = R return adj_A2B ================================================ FILE: pytransform3d/transformations/_transform.pyi ================================================ import numpy as np import numpy.typing as npt def check_transform( A2B: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def transform_requires_renormalization( A2B: npt.ArrayLike, tolerance: float = ... ) -> bool: ... def assert_transform(A2B: npt.ArrayLike, *args, **kwargs): ... def transform_from( R: npt.ArrayLike, p: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def translate_transform( A2B: npt.ArrayLike, p: npt.ArrayLike, strict_check: bool = ..., check: bool = ..., ) -> np.ndarray: ... def rotate_transform( A2B: npt.ArrayLike, R: npt.ArrayLike, strict_check: bool = ..., check: bool = ..., ) -> np.ndarray: ... def pq_from_transform( A2B: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def transform_log_from_transform( A2B: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def exponential_coordinates_from_transform( A2B: npt.ArrayLike, strict_check: bool = ..., check: bool = ... ) -> np.ndarray: ... def dual_quaternion_from_transform(A2B: npt.ArrayLike) -> np.ndarray: ... def adjoint_from_transform( A2B: npt.ArrayLike, strict_check: bool = ..., check: bool = ... ) -> np.ndarray: ... ================================================ FILE: pytransform3d/transformations/_transform_operations.py ================================================ """Transform operations.""" import numpy as np from ._screws import transform_from_exponential_coordinates from ._transform import check_transform, exponential_coordinates_from_transform from ..rotations import ( axis_angle_from_matrix, matrix_from_axis_angle, norm_vector, ) def invert_transform(A2B, strict_check=True, check=True): """Invert transform. Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. check : bool, optional (default: True) Check if transformation matrix is valid Returns ------- B2A : array, shape (4, 4) Transform from frame B to frame A See Also -------- pytransform3d.uncertainty.invert_uncertain_transform : Invert transformation with uncertainty. pytransform3d.trajectories.invert_transforms : Invert multiple transformations. """ if check: A2B = check_transform(A2B, strict_check=strict_check) # NOTE there is a faster version, but it is not faster than matrix # inversion with numpy: # ( R t )^-1 ( R^T -R^T*t ) # ( 0 1 ) = ( 0 1 ) return np.linalg.inv(A2B) def vector_to_point(v): """Convert 3D vector to position. A point (x, y, z) given by the components of a vector will be represented by [x, y, z, 1] in homogeneous coordinates to which we can apply a transformation. Parameters ---------- v : array-like, shape (3,) 3D vector that contains x, y, and z Returns ------- p : array-like, shape (4,) Point vector with 1 as last element See Also -------- pytransform3d.transformations.vectors_to_points Convert 3D vectors to points in homogeneous coordinates. pytransform3d.transformations.vector_to_direction Convert one 3D vector to a direction in homogeneous coordinates. pytransform3d.transformations.vectors_to_directions Convert 3D vectors to directions in homogeneous coordinates. """ return np.hstack((v, 1)) def vectors_to_points(V): """Convert 3D vectors to positions. A point (x, y, z) given by the components of a vector will be represented by [x, y, z, 1] in homogeneous coordinates to which we can apply a transformation. Parameters ---------- V : array-like, shape (n_points, 3) Each row is a 3D vector that contains x, y, and z Returns ------- P : array-like, shape (n_points, 4) Each row is a point vector with 1 as last element See Also -------- pytransform3d.transformations.vector_to_point Convert one 3D vector to a point in homogeneous coordinates. pytransform3d.transformations.vector_to_direction Convert one 3D vector to a direction in homogeneous coordinates. pytransform3d.transformations.vectors_to_directions Convert 3D vectors to directions in homogeneous coordinates. """ return np.hstack((V, np.ones((len(V), 1)))) def vector_to_direction(v): """Convert 3D vector to direction. A direction (x, y, z) given by the components of a vector will be represented by [x, y, z, 0] in homogeneous coordinates to which we can apply a transformation. Parameters ---------- v : array-like, shape (3,) 3D vector that contains x, y, and z Returns ------- p : array-like, shape (4,) Direction vector with 0 as last element See Also -------- pytransform3d.transformations.vector_to_point Convert one 3D vector to a point in homogeneous coordinates. pytransform3d.transformations.vectors_to_points Convert 3D vectors to points in homogeneous coordinates. pytransform3d.transformations.vectors_to_directions Convert 3D vectors to directions in homogeneous coordinates. """ return np.hstack((v, 0)) def vectors_to_directions(V): """Convert 3D vectors to directions. A direction (x, y, z) given by the components of a vector will be represented by [x, y, z, 0] in homogeneous coordinates to which we can apply a transformation. Parameters ---------- V : array-like, shape (n_directions, 3) Each row is a 3D vector that contains x, y, and z Returns ------- P : array-like, shape (n_directions, 4) Each row is a direction vector with 0 as last element See Also -------- pytransform3d.transformations.vector_to_point Convert one 3D vector to a point in homogeneous coordinates. pytransform3d.transformations.vectors_to_points Convert 3D vectors to points in homogeneous coordinates. pytransform3d.transformations.vector_to_direction Convert one 3D vector to a direction in homogeneous coordinates. """ return np.hstack((V, np.zeros((len(V), 1)))) def concat(A2B, B2C, strict_check=True, check=True): r"""Concatenate transformations. We use the extrinsic convention, which means that B2C is left-multiplied to A2B. In mathematical notation: .. math:: \boldsymbol{T}_{CB} \boldsymbol{T}_{BA} = \boldsymbol{T}_{CA}. Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B B2C : array-like, shape (4, 4) Transform from frame B to frame C strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. check : bool, optional (default: True) Check if transformation matrices are valid Returns ------- A2C : array-like, shape (4, 4) Transform from frame A to frame C See Also -------- pytransform3d.trajectories.concat_one_to_many : Concatenate one transformation with multiple transformations. pytransform3d.trajectories.concat_many_to_one : Concatenate multiple transformations with one. pytransform3d.uncertainty.concat_globally_uncertain_transforms : Concatenate two independent globally uncertain transformations. pytransform3d.uncertainty.concat_locally_uncertain_transforms : Concatenate two independent locally uncertain transformations. """ if check: A2B = check_transform(A2B, strict_check=strict_check) B2C = check_transform(B2C, strict_check=strict_check) return B2C.dot(A2B) def transform(A2B, PA, strict_check=True): """Transform point or list of points or directions. Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B PA : array-like, shape (4,) or (n_points, 4) One of multiple points or directions in frame A that are represented by homogeneous coordinates. strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. Returns ------- PB : array-like, shape (4,) or (n_points, 4) One of multiple points or directions in frame B that are represented by homogeneous coordinates. Raises ------ ValueError If dimensions are incorrect See Also -------- pytransform3d.transformations.vector_to_point Convert one 3D vector to a point in homogeneous coordinates. pytransform3d.transformations.vectors_to_points Convert 3D vectors to points in homogeneous coordinates. pytransform3d.transformations.vector_to_direction Convert one 3D vector to a direction in homogeneous coordinates. pytransform3d.transformations.vectors_to_directions Convert 3D vectors to directions in homogeneous coordinates. """ A2B = check_transform(A2B, strict_check=strict_check) PA = np.asarray(PA) if PA.ndim == 1: return np.dot(A2B, PA) if PA.ndim == 2: return np.dot(PA, A2B.T) raise ValueError("Cannot transform array with more than 2 dimensions") def scale_transform( A2B, s_xr=1.0, s_yr=1.0, s_zr=1.0, s_r=1.0, s_xt=1.0, s_yt=1.0, s_zt=1.0, s_t=1.0, s_d=1.0, strict_check=True, ): """Scale a transform from A to reference frame B. See algorithm 10 from "Analytic Approaches for Design and Operation of Haptic Human-Machine Interfaces" (Bertold Bongardt). Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B s_xr : float, optional (default: 1) Scaling of x-component of the rotation axis s_yr : float, optional (default: 1) Scaling of y-component of the rotation axis s_zr : float, optional (default: 1) Scaling of z-component of the rotation axis s_r : float, optional (default: 1) Scaling of the rotation s_xt : float, optional (default: 1) Scaling of z-component of the translation s_yt : float, optional (default: 1) Scaling of z-component of the translation s_zt : float, optional (default: 1) Scaling of z-component of the translation s_t : float, optional (default: 1) Scaling of the translation s_d : float, optional (default: 1) Scaling of the whole transform (displacement) strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. Returns ------- A2B_scaled Scaled transform from frame A to frame B (actually this is a transform from A to another frame C) """ A2B = check_transform(A2B, strict_check=strict_check) A2B_scaled = np.eye(4) R = A2B[:3, :3] t = A2B[:3, 3] S_t = np.array([s_xt, s_yt, s_zt]) A2B_scaled[:3, 3] = s_d * s_t * S_t * t a = axis_angle_from_matrix(R) a_new = np.empty(4) a_new[3] = s_d * s_r * a[3] S_r = np.array([s_xr, s_yr, s_zr]) a_new[:3] = norm_vector(S_r * a[:3]) A2B_scaled[:3, :3] = matrix_from_axis_angle(a_new) return A2B_scaled def transform_sclerp(start, end, t): r"""Screw linear interpolation (ScLERP) for transformation matrices. We compute the difference between two poses :math:`\boldsymbol{T}_{AB} = \boldsymbol{T}^{-1}_{WA}\boldsymbol{T}_{WB}`, convert it to exponential coordinates :math:`Log(\boldsymbol{T}_{AB}) = \mathcal{S}\theta_{AB}`, compute a fraction of it :math:`t\mathcal{S}\theta_{AB}` with :math:`t \in [0, 1]`, and use the exponential map to concatenate the fraction of the difference :math:`\boldsymbol{T}_{WA} Exp(t\mathcal{S}\theta_{AB})`. Parameters ---------- start : array-like, shape (4, 4) Transformation matrix to represent start pose. end : array-like, shape (4, 4) Transformation matrix to represent end pose. t : float in [0, 1] Position between start and goal. Returns ------- A2B_t : array, shape (4, 4) Interpolated pose. See Also -------- dual_quaternion_sclerp : ScLERP for dual quaternions. pq_slerp : An alternative approach is spherical linear interpolation (SLERP) with position and quaternion. """ end2start = np.dot(invert_transform(start), end) return np.dot(start, transform_power(end2start, t)) def transform_power(A2B, t): r"""Compute power of a transformation matrix with respect to scalar. .. math:: \boldsymbol{T}_{BA}^t Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B. t : float Exponent. Returns ------- A2B_t : array, shape (4, 4) Transformation matrix. """ Stheta = exponential_coordinates_from_transform(A2B) return transform_from_exponential_coordinates(Stheta * t) ================================================ FILE: pytransform3d/transformations/_transform_operations.pyi ================================================ import numpy as np import numpy.typing as npt def invert_transform( A2B: npt.ArrayLike, strict_check: bool = ..., check: bool = ... ) -> np.ndarray: ... def vector_to_point(v: npt.ArrayLike) -> np.ndarray: ... def vectors_to_points(V: npt.ArrayLike) -> np.ndarray: ... def vector_to_direction(v: npt.ArrayLike) -> np.ndarray: ... def vectors_to_directions(V: npt.ArrayLike) -> np.ndarray: ... def concat( A2B: npt.ArrayLike, B2C: npt.ArrayLike, strict_check: bool = ..., check: bool = ..., ) -> np.ndarray: ... def transform( A2B: npt.ArrayLike, PA: npt.ArrayLike, strict_check: bool = ... ) -> np.ndarray: ... def scale_transform( A2B: npt.ArrayLike, s_xr: float = ..., s_yr: float = ..., s_zr: float = ..., s_r: float = ..., s_xt: float = ..., s_yt: float = ..., s_zt: float = ..., s_t: float = ..., s_d: float = ..., strict_check: bool = ..., ) -> np.ndarray: ... def transform_sclerp( start: npt.ArrayLike, end: npt.ArrayLike, t: float ) -> np.ndarray: ... def transform_power(A2B: npt.ArrayLike, t: float) -> np.ndarray: ... ================================================ FILE: pytransform3d/transformations/test/test_dual_quaternion.py ================================================ import numpy as np import pytest from numpy.testing import assert_array_almost_equal import pytransform3d.rotations as pr import pytransform3d.transformations as pt def test_check_dual_quaternion(): dq1 = [0, 0] with pytest.raises(ValueError, match="Expected dual quaternion"): pt.check_dual_quaternion(dq1) dq2 = [[0, 0]] with pytest.raises(ValueError, match="Expected dual quaternion"): pt.check_dual_quaternion(dq2) dq3 = pt.check_dual_quaternion([0] * 8, unit=False) assert dq3.shape[0] == 8 assert pt.dual_quaternion_requires_renormalization(dq3) dq4 = pt.check_dual_quaternion(dq3, unit=True) assert not pt.dual_quaternion_requires_renormalization(dq4) dq5 = np.array( [ 0.94508498, 0.0617101, -0.06483886, 0.31432811, -0.07743254, 0.04985168, -0.26119618, 0.1691491, ] ) dq5_not_orthogonal = np.copy(dq5) dq5_not_orthogonal[4:] = np.round(dq5_not_orthogonal[4:], 1) assert pt.dual_quaternion_requires_renormalization(dq5_not_orthogonal) def test_normalize_dual_quaternion(): dq = [1, 0, 0, 0, 0, 0, 0, 0] norm = pt.dual_quaternion_squared_norm(dq) assert_array_almost_equal(norm, [1, 0]) assert not pt.dual_quaternion_requires_renormalization(dq) dq_norm = pt.check_dual_quaternion(dq) pt.assert_unit_dual_quaternion(dq_norm) assert_array_almost_equal(dq, dq_norm) assert_array_almost_equal(dq, pt.norm_dual_quaternion(dq)) dq = [0, 0, 0, 0, 0, 0, 0, 0] norm = pt.dual_quaternion_squared_norm(dq) assert_array_almost_equal(norm, [0, 0]) assert pt.dual_quaternion_requires_renormalization(dq) dq_norm = pt.check_dual_quaternion(dq) pt.assert_unit_dual_quaternion(dq_norm) assert_array_almost_equal([1, 0, 0, 0, 0, 0, 0, 0], dq_norm) assert_array_almost_equal(dq_norm, pt.norm_dual_quaternion(dq)) dq = [0, 0, 0, 0, 0.3, 0.5, 0, 0.2] norm = pt.dual_quaternion_squared_norm(dq) assert_array_almost_equal(norm, [0, 0]) assert pt.dual_quaternion_requires_renormalization(dq) dq_norm = pt.check_dual_quaternion(dq) assert pt.dual_quaternion_requires_renormalization(dq_norm) assert_array_almost_equal([1, 0, 0, 0, 0.3, 0.5, 0, 0.2], dq_norm) assert not pt.dual_quaternion_requires_renormalization( pt.norm_dual_quaternion(dq) ) rng = np.random.default_rng(999) for _ in range(5): # real norm != 1 A2B = pt.random_transform(rng) dq = rng.standard_normal() * pt.dual_quaternion_from_transform(A2B) dq_norm = pt.check_dual_quaternion(dq) pt.assert_unit_dual_quaternion(dq_norm) assert_array_almost_equal(dq_norm, pt.norm_dual_quaternion(dq)) for _ in range(5): # real and dual quaternion are not orthogonal A2B = pt.random_transform(rng) dq = pt.dual_quaternion_from_transform(A2B) dq_roundoff_error = np.copy(dq) dq_roundoff_error[4:] = np.round(dq_roundoff_error[4:], 3) assert pt.dual_quaternion_requires_renormalization(dq_roundoff_error) dq_norm = pt.norm_dual_quaternion(dq_roundoff_error) pt.assert_unit_dual_quaternion(dq_norm) assert_array_almost_equal( pt.dual_quaternion_squared_norm(dq_norm), [1, 0] ) assert not pt.dual_quaternion_requires_renormalization(dq_norm) assert_array_almost_equal(dq, dq_norm, decimal=3) for _ in range(50): dq = rng.standard_normal(size=8) dq_norm = pt.norm_dual_quaternion(dq) assert_array_almost_equal( pt.dual_quaternion_squared_norm(dq_norm), [1, 0] ) assert not pt.dual_quaternion_requires_renormalization(dq_norm) pt.assert_unit_dual_quaternion(dq_norm) def test_dual_quaternion_double(): rng = np.random.default_rng(4183) A2B = pt.random_transform(rng) dq = pt.dual_quaternion_from_transform(A2B) dq_double = pt.dual_quaternion_double(dq) pt.assert_unit_dual_quaternion_equal(dq, dq_double) assert_array_almost_equal(A2B, pt.transform_from_dual_quaternion(dq_double)) def test_dual_quaternion_concatenation(): rng = np.random.default_rng(1000) for _ in range(5): A2B = pt.random_transform(rng) B2C = pt.random_transform(rng) A2C = pt.concat(A2B, B2C) dq1 = pt.dual_quaternion_from_transform(A2B) dq2 = pt.dual_quaternion_from_transform(B2C) dq3 = pt.concatenate_dual_quaternions(dq2, dq1) A2C2 = pt.transform_from_dual_quaternion(dq3) assert_array_almost_equal(A2C, A2C2) def test_dual_quaternion_applied_to_point(): rng = np.random.default_rng(1000) for _ in range(5): p_A = pr.random_vector(rng, 3) A2B = pt.random_transform(rng) dq = pt.dual_quaternion_from_transform(A2B) p_B = pt.dq_prod_vector(dq, p_A) assert_array_almost_equal( p_B, pt.transform(A2B, pt.vector_to_point(p_A))[:3] ) def test_dual_quaternion_sclerp_same_dual_quaternions(): rng = np.random.default_rng(19) pose = pt.random_transform(rng) dq = pt.dual_quaternion_from_transform(pose) dq2 = pt.dual_quaternion_sclerp(dq, dq, 0.5) assert_array_almost_equal(dq, dq2) def test_dual_quaternion_sclerp(): rng = np.random.default_rng(22) pose1 = pt.random_transform(rng) pose2 = pt.random_transform(rng) dq1 = pt.dual_quaternion_from_transform(pose1) dq2 = pt.dual_quaternion_from_transform(pose2) n_steps = 100 # Ground truth: screw linear interpolation pose12pose2 = pt.concat(pose2, pt.invert_transform(pose1)) Stheta = pt.exponential_coordinates_from_transform(pose12pose2) offsets = np.array( [ pt.transform_from_exponential_coordinates(Stheta * t) for t in np.linspace(0, 1, n_steps) ] ) interpolated_poses = np.array( [pt.concat(offset, pose1) for offset in offsets] ) # Dual quaternion ScLERP sclerp_interpolated_dqs = np.vstack( [ pt.dual_quaternion_sclerp(dq1, dq2, t) for t in np.linspace(0, 1, n_steps) ] ) sclerp_interpolated_poses_from_dqs = np.array( [ pt.transform_from_dual_quaternion(dq) for dq in sclerp_interpolated_dqs ] ) # Transformation matrix ScLERP sclerp_interpolated_transforms = np.array( [ pt.transform_sclerp(pose1, pose2, t) for t in np.linspace(0, 1, n_steps) ] ) for t in range(n_steps): assert_array_almost_equal( interpolated_poses[t], sclerp_interpolated_poses_from_dqs[t] ) assert_array_almost_equal( interpolated_poses[t], sclerp_interpolated_transforms[t] ) def test_dual_quaternion_sclerp_sign_ambiguity(): n_steps = 10 rng = np.random.default_rng(2323) T1 = pt.random_transform(rng) dq1 = pt.dual_quaternion_from_transform(T1) dq2 = np.copy(dq1) if np.sign(dq1[0]) != np.sign(dq2[0]): dq2 *= -1.0 traj_q = [ pt.dual_quaternion_sclerp(dq1, dq2, t) for t in np.linspace(0, 1, n_steps) ] path_length = np.sum( [np.linalg.norm(r - s) for r, s in zip(traj_q[:-1], traj_q[1:])] ) dq2 *= -1.0 traj_q_opposing = [ pt.dual_quaternion_sclerp(dq1, dq2, t) for t in np.linspace(0, 1, n_steps) ] path_length_opposing = np.sum( [ np.linalg.norm(r - s) for r, s in zip(traj_q_opposing[:-1], traj_q_opposing[1:]) ] ) assert path_length_opposing == path_length def test_compare_dual_quaternion_and_transform_power(): rng = np.random.default_rng(44329) for _ in range(20): t = rng.normal() A2B = pt.random_transform(rng) dq = pt.dual_quaternion_from_transform(A2B) assert_array_almost_equal( pt.transform_power(A2B, t), pt.transform_from_dual_quaternion(pt.dual_quaternion_power(dq, t)), ) def test_screw_parameters_from_dual_quaternion(): dq = np.array([1, 0, 0, 0, 0, 0, 0, 0]) q, s_axis, h, theta = pt.screw_parameters_from_dual_quaternion(dq) assert_array_almost_equal(q, np.zeros(3)) assert_array_almost_equal(s_axis, np.array([1, 0, 0])) assert np.isinf(h) assert pytest.approx(theta) == 0 dq = pt.dual_quaternion_from_pq(np.array([1.2, 1.3, 1.4, 1, 0, 0, 0])) q, s_axis, h, theta = pt.screw_parameters_from_dual_quaternion(dq) assert_array_almost_equal(q, np.zeros(3)) assert_array_almost_equal(s_axis, pr.norm_vector(np.array([1.2, 1.3, 1.4]))) assert np.isinf(h) assert pytest.approx(theta) == np.linalg.norm(np.array([1.2, 1.3, 1.4])) rng = np.random.default_rng(1001) quat = pr.random_quaternion(rng) a = pr.axis_angle_from_quaternion(quat) dq = pt.dual_quaternion_from_pq(np.r_[0, 0, 0, quat]) q, s_axis, h, theta = pt.screw_parameters_from_dual_quaternion(dq) assert_array_almost_equal(q, np.zeros(3)) assert_array_almost_equal(s_axis, a[:3]) assert_array_almost_equal(h, 0) assert_array_almost_equal(theta, a[3]) for _ in range(5): A2B = pt.random_transform(rng) dq = pt.dual_quaternion_from_transform(A2B) Stheta = pt.exponential_coordinates_from_transform(A2B) S, theta = pt.screw_axis_from_exponential_coordinates(Stheta) q, s_axis, h = pt.screw_parameters_from_screw_axis(S) q2, s_axis2, h2, theta2 = pt.screw_parameters_from_dual_quaternion(dq) pt.assert_screw_parameters_equal( q, s_axis, h, theta, q2, s_axis2, h2, theta2 ) ================================================ FILE: pytransform3d/transformations/test/test_pq.py ================================================ import numpy as np import pytest from numpy.testing import assert_array_almost_equal import pytransform3d.rotations as pr import pytransform3d.transformations as pt def test_check_pq(): """Test input validation for position and orientation quaternion.""" q = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) q2 = pt.check_pq(q) assert_array_almost_equal(q, q2) q3 = [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0] q4 = pt.check_pq(q3) assert_array_almost_equal(q3, q4) assert len(q3) == q4.shape[0] A2B = np.eye(4) with pytest.raises(ValueError, match="position and orientation quaternion"): pt.check_pq(A2B) q5 = np.zeros(8) with pytest.raises(ValueError, match="position and orientation quaternion"): pt.check_pq(q5) def test_pq_slerp(): start = np.array([0.2, 0.3, 0.4, 1.0, 0.0, 0.0, 0.0]) end = np.array([1.0, 0.5, 0.8, 0.0, 1.0, 0.0, 0.0]) pq_05 = pt.pq_slerp(start, end, 0.5) assert_array_almost_equal(pq_05, [0.6, 0.4, 0.6, 0.707107, 0.707107, 0, 0]) pq_025 = pt.pq_slerp(start, end, 0.25) assert_array_almost_equal(pq_025, [0.4, 0.35, 0.5, 0.92388, 0.382683, 0, 0]) pq_075 = pt.pq_slerp(start, end, 0.75) assert_array_almost_equal(pq_075, [0.8, 0.45, 0.7, 0.382683, 0.92388, 0, 0]) def test_transform_from_pq(): """Test conversion from position and quaternion to homogeneous matrix.""" pq = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) A2B = pt.transform_from_pq(pq) assert_array_almost_equal(A2B, np.eye(4)) def test_conversions_between_dual_quternion_and_pq(): rng = np.random.default_rng(1000) for _ in range(5): pq = pr.random_vector(rng, 7) pq[3:] /= np.linalg.norm(pq[3:]) dq = pt.dual_quaternion_from_pq(pq) pq2 = pt.pq_from_dual_quaternion(dq) assert_array_almost_equal(pq, pq2) dq2 = pt.dual_quaternion_from_pq(pq2) pt.assert_unit_dual_quaternion_equal(dq, dq2) ================================================ FILE: pytransform3d/transformations/test/test_screws.py ================================================ import numpy as np import pytest from numpy.testing import assert_array_almost_equal import pytransform3d.rotations as pr import pytransform3d.transformations as pt def test_check_screw_parameters(): q = [100.0, -20.3, 1e-3] s_axis = pr.norm_vector(np.array([-1.0, 2.0, 3.0])).tolist() h = 0.2 with pytest.raises(ValueError, match="Expected 3D vector with shape"): pt.check_screw_parameters([0.0], s_axis, h) with pytest.raises(ValueError, match="Expected 3D vector with shape"): pt.check_screw_parameters(q, [0.0], h) with pytest.raises(ValueError, match="s_axis must not have norm 0"): pt.check_screw_parameters(q, np.zeros(3), h) q2, s_axis2, h2 = pt.check_screw_parameters(q, 2.0 * np.array(s_axis), h) assert_array_almost_equal(q, q2) assert pytest.approx(h) == h2 assert pytest.approx(np.linalg.norm(s_axis2)) == 1.0 q2, s_axis2, h2 = pt.check_screw_parameters(q, s_axis, h) assert_array_almost_equal(q, q2) assert_array_almost_equal(s_axis, s_axis2) assert pytest.approx(h) == h2 q2, s_axis2, h2 = pt.check_screw_parameters(q, s_axis, np.inf) assert_array_almost_equal(np.zeros(3), q2) assert_array_almost_equal(s_axis, s_axis2) assert np.isinf(h2) def test_check_screw_axis(): rng = np.random.default_rng(73) omega = pr.norm_vector(pr.random_vector(rng, 3)) v = pr.random_vector(rng, 3) with pytest.raises(ValueError, match="Expected 3D vector with shape"): pt.check_screw_axis(np.r_[0, 1, v]) with pytest.raises( ValueError, match="Norm of rotation axis must either be 0 or 1" ): pt.check_screw_axis(np.r_[2 * omega, v]) with pytest.raises( ValueError, match="If the norm of the rotation axis is 0" ): pt.check_screw_axis(np.r_[0, 0, 0, v]) S_pure_translation = np.r_[0, 0, 0, pr.norm_vector(v)] S = pt.check_screw_axis(S_pure_translation) assert_array_almost_equal(S, S_pure_translation) S_both = np.r_[omega, v] S = pt.check_screw_axis(S_both) assert_array_almost_equal(S, S_both) def test_check_exponential_coordinates(): with pytest.raises(ValueError, match="Expected array-like with shape"): pt.check_exponential_coordinates([0]) Stheta = [0.0, 1.0, 2.0, -5.0, -2, 3] Stheta2 = pt.check_exponential_coordinates(Stheta) assert_array_almost_equal(Stheta, Stheta2) def test_check_screw_matrix(): with pytest.raises(ValueError, match="Expected array-like with shape"): pt.check_screw_matrix(np.zeros((1, 4, 4))) with pytest.raises(ValueError, match="Expected array-like with shape"): pt.check_screw_matrix(np.zeros((3, 4))) with pytest.raises(ValueError, match="Expected array-like with shape"): pt.check_screw_matrix(np.zeros((4, 3))) with pytest.raises( ValueError, match="Last row of screw matrix must only " "contains zeros" ): pt.check_screw_matrix(np.eye(4)) screw_matrix = ( pt.screw_matrix_from_screw_axis( np.array([1.0, 0.0, 0.0, 1.0, 0.0, 0.0]) ) * 1.1 ) with pytest.raises( ValueError, match="Norm of rotation axis must either be 0 or 1" ): pt.check_screw_matrix(screw_matrix) screw_matrix = ( pt.screw_matrix_from_screw_axis( np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0]) ) * 1.1 ) with pytest.raises( ValueError, match="If the norm of the rotation axis is 0" ): pt.check_screw_matrix(screw_matrix) screw_matrix = pt.screw_matrix_from_screw_axis( np.array([1.0, 0.0, 0.0, 1.0, 0.0, 0.0]) ) screw_matrix2 = pt.check_screw_matrix(screw_matrix) assert_array_almost_equal(screw_matrix, screw_matrix2) screw_matrix = pt.screw_matrix_from_screw_axis( np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0]) ) screw_matrix2 = pt.check_screw_matrix(screw_matrix) assert_array_almost_equal(screw_matrix, screw_matrix2) screw_matrix = pt.screw_matrix_from_screw_axis( np.array([1.0, 0.0, 0.0, 1.0, 0.0, 0.0]) ) screw_matrix[0, 0] = 0.0001 with pytest.raises(ValueError, match="Expected skew-symmetric matrix"): pt.check_screw_matrix(screw_matrix) def test_check_transform_log(): with pytest.raises(ValueError, match="Expected array-like with shape"): pt.check_transform_log(np.zeros((1, 4, 4))) with pytest.raises(ValueError, match="Expected array-like with shape"): pt.check_transform_log(np.zeros((3, 4))) with pytest.raises(ValueError, match="Expected array-like with shape"): pt.check_transform_log(np.zeros((4, 3))) with pytest.raises( ValueError, match="Last row of logarithm of transformation must " "only contains zeros", ): pt.check_transform_log(np.eye(4)) transform_log = ( pt.screw_matrix_from_screw_axis( np.array([1.0, 0.0, 0.0, 1.0, 0.0, 0.0]) ) * 1.1 ) transform_log2 = pt.check_transform_log(transform_log) assert_array_almost_equal(transform_log, transform_log2) transform_log = ( pt.screw_matrix_from_screw_axis( np.array([1.0, 0.0, 0.0, 1.0, 0.0, 0.0]) ) * 1.1 ) transform_log[0, 0] = 0.0001 with pytest.raises(ValueError, match="Expected skew-symmetric matrix"): pt.check_transform_log(transform_log) def test_norm_exponential_coordinates(): Stheta_only_translation = np.array([0.0, 0.0, 0.0, 100.0, 25.0, -23.0]) Stheta_only_translation2 = pt.norm_exponential_coordinates( Stheta_only_translation ) assert_array_almost_equal(Stheta_only_translation, Stheta_only_translation2) pt.assert_exponential_coordinates_equal( Stheta_only_translation, Stheta_only_translation2 ) rng = np.random.default_rng(381) # 180 degree rotation ambiguity for _ in range(10): q = rng.standard_normal(size=3) s = pr.norm_vector(rng.standard_normal(size=3)) h = rng.standard_normal() Stheta = pt.screw_axis_from_screw_parameters(q, s, h) * np.pi Stheta2 = pt.screw_axis_from_screw_parameters(q, -s, -h) * np.pi assert_array_almost_equal( pt.transform_from_exponential_coordinates(Stheta), pt.transform_from_exponential_coordinates(Stheta2), ) assert_array_almost_equal( pt.norm_exponential_coordinates(Stheta), pt.norm_exponential_coordinates(Stheta2), ) pt.assert_exponential_coordinates_equal(Stheta, Stheta2) for _ in range(10): Stheta = rng.standard_normal(size=6) # ensure that theta is not within [-pi, pi] i = rng.integers(0, 3) Stheta[i] = np.sign(Stheta[i]) * (np.pi + rng.random()) Stheta_norm = pt.norm_exponential_coordinates(Stheta) assert not np.all(Stheta == Stheta_norm) pt.assert_exponential_coordinates_equal(Stheta, Stheta_norm) A2B = pt.transform_from_exponential_coordinates(Stheta) Stheta2 = pt.exponential_coordinates_from_transform(A2B) assert_array_almost_equal(Stheta2, Stheta_norm) def test_assert_screw_parameters_equal(): q = np.array([1, 2, 3]) s_axis = pr.norm_vector(np.array([-2, 3, 5])) h = 2 theta = 1 pt.assert_screw_parameters_equal( q, s_axis, h, theta, q + 484.3 * s_axis, s_axis, h, theta ) with pytest.raises(AssertionError): pt.assert_screw_parameters_equal( q, s_axis, h, theta, q + 484.3, s_axis, h, theta ) s_axis_mirrored = -s_axis theta_mirrored = 2.0 * np.pi - theta h_mirrored = -h * theta / theta_mirrored pt.assert_screw_parameters_equal( q, s_axis_mirrored, h_mirrored, theta_mirrored, q, s_axis, h, theta ) def test_conversions_between_screw_axis_and_parameters(): rng = np.random.default_rng(98) q = pr.random_vector(rng, 3) s_axis = pr.norm_vector(pr.random_vector(rng, 3)) h = np.inf screw_axis = pt.screw_axis_from_screw_parameters(q, s_axis, h) assert_array_almost_equal(screw_axis, np.r_[0, 0, 0, s_axis]) q2, s_axis2, h2 = pt.screw_parameters_from_screw_axis(screw_axis) assert_array_almost_equal(np.zeros(3), q2) assert_array_almost_equal(s_axis, s_axis2) assert_array_almost_equal(h, h2) for _ in range(10): s_axis = pr.norm_vector(pr.random_vector(rng, 3)) # q has to be orthogonal to s_axis so that we reconstruct it exactly q = pr.perpendicular_to_vector(s_axis) h = rng.random() + 0.5 screw_axis = pt.screw_axis_from_screw_parameters(q, s_axis, h) q2, s_axis2, h2 = pt.screw_parameters_from_screw_axis(screw_axis) assert_array_almost_equal(q, q2) assert_array_almost_equal(s_axis, s_axis2) assert_array_almost_equal(h, h2) def test_conversions_between_exponential_coordinates_and_transform(): A2B = np.eye(4) Stheta = pt.exponential_coordinates_from_transform(A2B) assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) A2B2 = pt.transform_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2) A2B = pt.translate_transform(np.eye(4), [1.0, 5.0, 0.0]) Stheta = pt.exponential_coordinates_from_transform(A2B) assert_array_almost_equal(Stheta, [0.0, 0.0, 0.0, 1.0, 5.0, 0.0]) A2B2 = pt.transform_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2) A2B = pt.rotate_transform( np.eye(4), pr.active_matrix_from_angle(2, 0.5 * np.pi) ) Stheta = pt.exponential_coordinates_from_transform(A2B) assert_array_almost_equal(Stheta, [0.0, 0.0, 0.5 * np.pi, 0.0, 0.0, 0.0]) A2B2 = pt.transform_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2) rng = np.random.default_rng(52) for _ in range(5): A2B = pt.random_transform(rng) Stheta = pt.exponential_coordinates_from_transform(A2B) A2B2 = pt.transform_from_exponential_coordinates(Stheta) assert_array_almost_equal(A2B, A2B2) def test_transform_from_exponential_coordinates_without_check(): Stheta = np.zeros(7) with pytest.raises(ValueError, match="could not broadcast input array"): pt.transform_from_exponential_coordinates(Stheta, check=False) def test_conversions_between_screw_axis_and_exponential_coordinates(): S = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0]) theta = 1.0 Stheta = pt.exponential_coordinates_from_screw_axis(S, theta) S2, theta2 = pt.screw_axis_from_exponential_coordinates(Stheta) assert_array_almost_equal(S, S2) assert pytest.approx(theta) == theta2 S = np.zeros(6) theta = 0.0 S2, theta2 = pt.screw_axis_from_exponential_coordinates(np.zeros(6)) assert_array_almost_equal(S, S2) assert pytest.approx(theta) == theta2 rng = np.random.default_rng(33) for _ in range(5): S = pt.random_screw_axis(rng) theta = rng.random() Stheta = pt.exponential_coordinates_from_screw_axis(S, theta) S2, theta2 = pt.screw_axis_from_exponential_coordinates(Stheta) assert_array_almost_equal(S, S2) assert pytest.approx(theta) == theta2 def test_conversions_between_exponential_coordinates_and_transform_log(): rng = np.random.default_rng(22) for _ in range(5): Stheta = rng.standard_normal(size=6) transform_log = pt.transform_log_from_exponential_coordinates(Stheta) Stheta2 = pt.exponential_coordinates_from_transform_log(transform_log) assert_array_almost_equal(Stheta, Stheta2) def test_exponential_coordinates_from_transform_log_without_check(): transform_log = np.ones((4, 4)) Stheta = pt.exponential_coordinates_from_transform_log( transform_log, check=False ) assert_array_almost_equal(Stheta, np.ones(6)) def test_conversions_between_screw_matrix_and_screw_axis(): rng = np.random.default_rng(83) for _ in range(5): S = pt.random_screw_axis(rng) S_mat = pt.screw_matrix_from_screw_axis(S) S2 = pt.screw_axis_from_screw_matrix(S_mat) assert_array_almost_equal(S, S2) def test_conversions_between_screw_matrix_and_transform_log(): S_mat = np.array( [ [0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0], ] ) theta = 1.0 transform_log = pt.transform_log_from_screw_matrix(S_mat, theta) S_mat2, theta2 = pt.screw_matrix_from_transform_log(transform_log) assert_array_almost_equal(S_mat, S_mat2) assert pytest.approx(theta) == theta2 S_mat = np.zeros((4, 4)) theta = 0.0 transform_log = pt.transform_log_from_screw_matrix(S_mat, theta) S_mat2, theta2 = pt.screw_matrix_from_transform_log(transform_log) assert_array_almost_equal(S_mat, S_mat2) assert pytest.approx(theta) == theta2 rng = np.random.default_rng(65) for _ in range(5): S = pt.random_screw_axis(rng) theta = float(np.random.random()) S_mat = pt.screw_matrix_from_screw_axis(S) transform_log = pt.transform_log_from_screw_matrix(S_mat, theta) S_mat2, theta2 = pt.screw_matrix_from_transform_log(transform_log) assert_array_almost_equal(S_mat, S_mat2) assert pytest.approx(theta) == theta2 def test_conversions_between_transform_and_transform_log(): A2B = np.eye(4) transform_log = pt.transform_log_from_transform(A2B) assert_array_almost_equal(transform_log, np.zeros((4, 4))) A2B2 = pt.transform_from_transform_log(transform_log) assert_array_almost_equal(A2B, A2B2) rng = np.random.default_rng(84) A2B = pt.transform_from(np.eye(3), p=pr.random_vector(rng, 3)) transform_log = pt.transform_log_from_transform(A2B) A2B2 = pt.transform_from_transform_log(transform_log) assert_array_almost_equal(A2B, A2B2) for _ in range(5): A2B = pt.random_transform(rng) transform_log = pt.transform_log_from_transform(A2B) A2B2 = pt.transform_from_transform_log(transform_log) assert_array_almost_equal(A2B, A2B2) def test_dual_quaternion_from_screw_parameters(): q = np.zeros(3) s_axis = np.array([1, 0, 0]) h = np.inf theta = 0.0 dq = pt.dual_quaternion_from_screw_parameters(q, s_axis, h, theta) assert_array_almost_equal(dq, np.array([1, 0, 0, 0, 0, 0, 0, 0])) q = np.zeros(3) s_axis = pr.norm_vector(np.array([2.3, 2.4, 2.5])) h = np.inf theta = 3.6 dq = pt.dual_quaternion_from_screw_parameters(q, s_axis, h, theta) pq = pt.pq_from_dual_quaternion(dq) assert_array_almost_equal(pq, np.r_[s_axis * theta, 1, 0, 0, 0]) q = np.zeros(3) s_axis = pr.norm_vector(np.array([2.4, 2.5, 2.6])) h = 0.0 theta = 4.1 dq = pt.dual_quaternion_from_screw_parameters(q, s_axis, h, theta) pq = pt.pq_from_dual_quaternion(dq) assert_array_almost_equal(pq[:3], [0, 0, 0]) assert_array_almost_equal( pr.axis_angle_from_quaternion(pq[3:]), pr.norm_axis_angle(np.r_[s_axis, theta]), ) rng = np.random.default_rng(1001) for _ in range(5): A2B = pt.random_transform(rng) Stheta = pt.exponential_coordinates_from_transform(A2B) S, theta = pt.screw_axis_from_exponential_coordinates(Stheta) q, s_axis, h = pt.screw_parameters_from_screw_axis(S) dq = pt.dual_quaternion_from_screw_parameters(q, s_axis, h, theta) pt.assert_unit_dual_quaternion(dq) dq_expected = pt.dual_quaternion_from_transform(A2B) pt.assert_unit_dual_quaternion_equal(dq, dq_expected) ================================================ FILE: pytransform3d/transformations/test/test_transform.py ================================================ import warnings import numpy as np import pytest from numpy.testing import assert_array_almost_equal import pytransform3d.rotations as pr import pytransform3d.transformations as pt def test_check_transform(): """Test input validation for transformation matrix.""" A2B = np.eye(3) with pytest.raises( ValueError, match="Expected homogeneous transformation matrix with shape", ): pt.check_transform(A2B) A2B = np.eye(4, dtype=int) A2B = pt.check_transform(A2B) assert type(A2B) is np.ndarray assert A2B.dtype == np.float64 A2B[:3, :3] = np.array([[1, 1, 1], [0, 0, 0], [2, 2, 2]]) with pytest.raises(ValueError, match="rotation matrix"): pt.check_transform(A2B) A2B = np.eye(4) A2B[3, :] = np.array([0.1, 0.0, 0.0, 1.0]) with pytest.raises(ValueError, match="homogeneous transformation matrix"): pt.check_transform(A2B) rng = np.random.default_rng(0) A2B = pt.random_transform(rng) A2B2 = pt.check_transform(A2B) assert_array_almost_equal(A2B, A2B2) def test_deactivate_transform_precision_error(): A2B = np.eye(4) A2B[0, 0] = 2.0 A2B[3, 0] = 3.0 with pytest.raises(ValueError, match="Expected rotation matrix"): pt.check_transform(A2B) n_expected_warnings = 2 try: warnings.filterwarnings("always", category=UserWarning) with warnings.catch_warnings(record=True) as w: pt.check_transform(A2B, strict_check=False) assert len(w) == n_expected_warnings finally: warnings.filterwarnings("default", category=UserWarning) def test_transform_requires_renormalization(): assert pt.transform_requires_renormalization(np.eye(4) + 1e-6) assert not pt.transform_requires_renormalization(np.eye(4)) def test_translate_transform_with_check(): A2B_broken = np.zeros((4, 4)) with pytest.raises(ValueError, match="rotation matrix"): pt.translate_transform(A2B_broken, np.zeros(3)) def test_rotate_transform_with_check(): A2B_broken = np.zeros((4, 4)) with pytest.raises(ValueError, match="rotation matrix"): pt.rotate_transform(A2B_broken, np.eye(3)) def test_pq_from_transform(): """Test conversion from homogeneous matrix to position and quaternion.""" A2B = np.eye(4) pq = pt.pq_from_transform(A2B) assert_array_almost_equal(pq, np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0])) def test_exponential_coordinates_from_almost_identity_transform(): A2B = np.array( [ [ 0.9999999999999999, -1.5883146449068575e-16, 4.8699079321578667e-17, -7.54265065748827e-05, ], [ 5.110044286978025e-17, 0.9999999999999999, 1.1798895336935056e-17, 9.340523179823812e-05, ], [ 3.0048299647976294e-18, 5.4741890703482423e-17, 1.0, -7.803584869947588e-05, ], [0, 0, 0, 1], ] ) Stheta = pt.exponential_coordinates_from_transform(A2B) assert_array_almost_equal(np.zeros(6), Stheta, decimal=4) def test_exponential_coordinates_from_transform_without_check(): transform = np.ones((4, 4)) Stheta = pt.exponential_coordinates_from_transform(transform, check=False) assert_array_almost_equal(Stheta, np.array([0, 0, 0, 1, 1, 1])) def test_transform_log_from_almost_identity_transform(): A2B = np.array( [ [ 0.9999999999999999, -1.5883146449068575e-16, 4.8699079321578667e-17, -7.54265065748827e-05, ], [ 5.110044286978025e-17, 0.9999999999999999, 1.1798895336935056e-17, 9.340523179823812e-05, ], [ 3.0048299647976294e-18, 5.4741890703482423e-17, 1.0, -7.803584869947588e-05, ], [0, 0, 0, 1], ] ) transform_log = pt.transform_log_from_transform(A2B) assert_array_almost_equal(np.zeros((4, 4)), transform_log) def test_conversions_between_dual_quaternion_and_transform(): rng = np.random.default_rng(1000) for _ in range(5): A2B = pt.random_transform(rng) dq = pt.dual_quaternion_from_transform(A2B) A2B2 = pt.transform_from_dual_quaternion(dq) assert_array_almost_equal(A2B, A2B2) dq2 = pt.dual_quaternion_from_transform(A2B2) pt.assert_unit_dual_quaternion_equal(dq, dq2) for _ in range(5): p = pr.random_vector(rng, 3) q = pr.random_quaternion(rng) dq = pt.dual_quaternion_from_pq(np.hstack((p, q))) A2B = pt.transform_from_dual_quaternion(dq) dq2 = pt.dual_quaternion_from_transform(A2B) pt.assert_unit_dual_quaternion_equal(dq, dq2) A2B2 = pt.transform_from_dual_quaternion(dq2) assert_array_almost_equal(A2B, A2B2) def test_adjoint_of_transformation(): rng = np.random.default_rng(94) for _ in range(5): A2B = pt.random_transform(rng) theta_dot = 3.0 * float(rng.random()) S = pt.random_screw_axis(rng) V_A = S * theta_dot adj_A2B = pt.adjoint_from_transform(A2B) V_B = adj_A2B.dot(V_A) S_mat = pt.screw_matrix_from_screw_axis(S) V_mat_A = S_mat * theta_dot V_mat_B = np.dot(np.dot(A2B, V_mat_A), pt.invert_transform(A2B)) S_B, theta_dot2 = pt.screw_axis_from_exponential_coordinates(V_B) V_mat_B2 = pt.screw_matrix_from_screw_axis(S_B) * theta_dot2 assert pytest.approx(theta_dot) == theta_dot2 assert_array_almost_equal(V_mat_B, V_mat_B2) def test_adjoint_from_transform_without_check(): transform = np.ones((4, 4)) adjoint = pt.adjoint_from_transform(transform, check=False) assert_array_almost_equal(adjoint[:3, :3], np.ones((3, 3))) assert_array_almost_equal(adjoint[3:, 3:], np.ones((3, 3))) assert_array_almost_equal(adjoint[3:, :3], np.zeros((3, 3))) assert_array_almost_equal(adjoint[:3, 3:], np.zeros((3, 3))) ================================================ FILE: pytransform3d/transformations/test/test_transform_operations.py ================================================ import numpy as np import pytest from numpy.testing import assert_array_almost_equal import pytransform3d.rotations as pr import pytransform3d.transformations as pt def test_invert_transform(): """Test inversion of transformations.""" rng = np.random.default_rng(0) for _ in range(5): R = pr.matrix_from_axis_angle(pr.random_axis_angle(rng)) p = pr.random_vector(rng) A2B = pt.transform_from(R, p) B2A = pt.invert_transform(A2B) A2B2 = np.linalg.inv(B2A) assert_array_almost_equal(A2B, A2B2) def test_invert_transform_without_check(): """Test inversion of transformations.""" rng = np.random.default_rng(0) for _ in range(5): A2B = rng.standard_normal(size=(4, 4)) A2B = A2B + A2B.T B2A = pt.invert_transform(A2B, check=False) A2B2 = np.linalg.inv(B2A) assert_array_almost_equal(A2B, A2B2) def test_vector_to_point(): """Test conversion from vector to homogenous coordinates.""" v = np.array([1, 2, 3]) pA = pt.vector_to_point(v) assert_array_almost_equal(pA, [1, 2, 3, 1]) rng = np.random.default_rng(0) R = pr.matrix_from_axis_angle(pr.random_axis_angle(rng)) p = pr.random_vector(rng) A2B = pt.transform_from(R, p) pt.assert_transform(A2B) _ = pt.transform(A2B, pA) def test_vectors_to_points(): """Test conversion from vectors to homogenous coordinates.""" V = np.array([[1, 2, 3], [2, 3, 4]]) PA = pt.vectors_to_points(V) assert_array_almost_equal(PA, [[1, 2, 3, 1], [2, 3, 4, 1]]) rng = np.random.default_rng(0) V = rng.standard_normal(size=(10, 3)) for i, p in enumerate(pt.vectors_to_points(V)): assert_array_almost_equal(p, pt.vector_to_point(V[i])) def test_vector_to_direction(): """Test conversion from vector to direction in homogenous coordinates.""" v = np.array([1, 2, 3]) dA = pt.vector_to_direction(v) assert_array_almost_equal(dA, [1, 2, 3, 0]) rng = np.random.default_rng(0) R = pr.matrix_from_axis_angle(pr.random_axis_angle(rng)) p = pr.random_vector(rng) A2B = pt.transform_from(R, p) pt.assert_transform(A2B) _ = pt.transform(A2B, dA) def test_vectors_to_directions(): """Test conversion from vectors to directions in homogenous coordinates.""" V = np.array([[1, 2, 3], [2, 3, 4]]) DA = pt.vectors_to_directions(V) assert_array_almost_equal(DA, [[1, 2, 3, 0], [2, 3, 4, 0]]) rng = np.random.default_rng(0) V = rng.standard_normal(size=(10, 3)) for i, d in enumerate(pt.vectors_to_directions(V)): assert_array_almost_equal(d, pt.vector_to_direction(V[i])) def test_concat(): """Test concatenation of transforms.""" rng = np.random.default_rng(0) for _ in range(5): A2B = pt.random_transform(rng) pt.assert_transform(A2B) B2C = pt.random_transform(rng) pt.assert_transform(B2C) A2C = pt.concat(A2B, B2C) pt.assert_transform(A2C) p_A = np.array([0.3, -0.2, 0.9, 1.0]) p_C = pt.transform(A2C, p_A) C2A = pt.invert_transform(A2C) p_A2 = pt.transform(C2A, p_C) assert_array_almost_equal(p_A, p_A2) C2A2 = pt.concat(pt.invert_transform(B2C), pt.invert_transform(A2B)) p_A3 = pt.transform(C2A2, p_C) assert_array_almost_equal(p_A, p_A3) def test_transform(): """Test transformation of points.""" PA = np.array([[1, 2, 3, 1], [2, 3, 4, 1]]) rng = np.random.default_rng(0) A2B = pt.random_transform(rng) PB = pt.transform(A2B, PA) p0B = pt.transform(A2B, PA[0]) p1B = pt.transform(A2B, PA[1]) assert_array_almost_equal(PB, np.array([p0B, p1B])) with pytest.raises( ValueError, match="Cannot transform array with more than 2 dimensions" ): pt.transform(A2B, np.zeros((2, 2, 4))) def test_scale_transform(): """Test scaling of transforms.""" rng = np.random.default_rng(0) A2B = pt.random_transform(rng) A2B_scaled1 = pt.scale_transform(A2B, s_xt=0.5, s_yt=0.5, s_zt=0.5) A2B_scaled2 = pt.scale_transform(A2B, s_t=0.5) assert_array_almost_equal(A2B_scaled1, A2B_scaled2) A2B_scaled1 = pt.scale_transform(A2B, s_xt=0.5, s_yt=0.5, s_zt=0.5, s_r=0.5) A2B_scaled2 = pt.scale_transform(A2B, s_t=0.5, s_r=0.5) A2B_scaled3 = pt.scale_transform(A2B, s_d=0.5) assert_array_almost_equal(A2B_scaled1, A2B_scaled2) assert_array_almost_equal(A2B_scaled1, A2B_scaled3) A2B_scaled = pt.scale_transform(A2B, s_xr=0.0) a_scaled = pr.axis_angle_from_matrix(A2B_scaled[:3, :3]) assert_array_almost_equal(a_scaled[0], 0.0) A2B_scaled = pt.scale_transform(A2B, s_yr=0.0) a_scaled = pr.axis_angle_from_matrix(A2B_scaled[:3, :3]) assert_array_almost_equal(a_scaled[1], 0.0) A2B_scaled = pt.scale_transform(A2B, s_zr=0.0) a_scaled = pr.axis_angle_from_matrix(A2B_scaled[:3, :3]) assert_array_almost_equal(a_scaled[2], 0.0) ================================================ FILE: pytransform3d/transformations/test/test_transformations_jacobians.py ================================================ import numpy as np from numpy.testing import assert_array_almost_equal import pytransform3d.transformations as pt def test_jacobian_se3(): Stheta = np.zeros(6) J = pt.left_jacobian_SE3(Stheta) J_series = pt.left_jacobian_SE3_series(Stheta, 20) assert_array_almost_equal(J, J_series) J_inv = pt.left_jacobian_SE3_inv(Stheta) J_inv_serias = pt.left_jacobian_SE3_inv_series(Stheta, 20) assert_array_almost_equal(J_inv, J_inv_serias) J_inv_J = np.dot(J_inv, J) assert_array_almost_equal(J_inv_J, np.eye(6)) rng = np.random.default_rng(0) for _ in range(5): Stheta = pt.random_exponential_coordinates(rng) J = pt.left_jacobian_SE3(Stheta) J_series = pt.left_jacobian_SE3_series(Stheta, 20) assert_array_almost_equal(J, J_series) J_inv = pt.left_jacobian_SE3_inv(Stheta) J_inv_serias = pt.left_jacobian_SE3_inv_series(Stheta, 20) assert_array_almost_equal(J_inv, J_inv_serias) J_inv_J = np.dot(J_inv, J) assert_array_almost_equal(J_inv_J, np.eye(6)) ================================================ FILE: pytransform3d/transformations/test/test_transformations_random.py ================================================ import numpy as np import pytransform3d.transformations as pt def test_random_screw_axis(): rng = np.random.default_rng(893) for _ in range(5): S = pt.random_screw_axis(rng) pt.check_screw_axis(S) ================================================ FILE: pytransform3d/uncertainty/__init__.py ================================================ """Operations related to uncertain transformations. See :doc:`user_guide/uncertainty` for more information. """ from ._composition import ( concat_globally_uncertain_transforms, concat_locally_uncertain_transforms, ) from ._frechet_mean import ( estimate_gaussian_rotation_matrix_from_samples, estimate_gaussian_transform_from_samples, frechet_mean, ) from ._fusion import pose_fusion from ._invert import invert_uncertain_transform from ._plot import ( to_ellipsoid, to_projected_ellipsoid, plot_projected_ellipsoid, ) __all__ = [ "concat_globally_uncertain_transforms", "concat_locally_uncertain_transforms", "frechet_mean", "estimate_gaussian_rotation_matrix_from_samples", "estimate_gaussian_transform_from_samples", "pose_fusion", "invert_uncertain_transform", "to_ellipsoid", "to_projected_ellipsoid", "plot_projected_ellipsoid", ] ================================================ FILE: pytransform3d/uncertainty/_composition.py ================================================ """Composition of uncertain transformations.""" import numpy as np from ..transformations import ( adjoint_from_transform, concat, invert_transform, ) def concat_globally_uncertain_transforms(mean_A2B, cov_A2B, mean_B2C, cov_B2C): r"""Concatenate two independent globally uncertain transformations. We assume that the two distributions are independent. Each of the two transformations is globally uncertain (not in the local / body frame), that is, samples are generated through .. math:: \boldsymbol{T} = Exp(\boldsymbol{\xi}) \overline{\boldsymbol{T}}, where :math:`\boldsymbol{T} \in SE(3)` is a sampled transformation matrix, :math:`\overline{\boldsymbol{T}} \in SE(3)` is the mean transformation, and :math:`\boldsymbol{\xi} \in \mathbb{R}^6` are exponential coordinates of transformations and are distributed according to a Gaussian distribution with zero mean and covariance :math:`\boldsymbol{\Sigma} \in \mathbb{R}^{6 \times 6}`, that is, :math:`\boldsymbol{\xi} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma})`. The concatenation order is the same as in :func:`~pytransform3d.transformations.concat`, that is, the transformation B2C is left-multiplied to A2B. Note that the order of arguments is different from :func:`~pytransform3d.uncertainty.concat_locally_uncertain_transforms`. Hence, the full model is .. math:: Exp(_C\boldsymbol{\xi'}) \overline{\boldsymbol{T}}_{CA} = Exp(_C\boldsymbol{\xi}) \overline{\boldsymbol{T}}_{CB} Exp(_B\boldsymbol{\xi}) \overline{\boldsymbol{T}}_{BA}, where :math:`_B\boldsymbol{\xi} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_{BA})`, :math:`_C\boldsymbol{\xi} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_{CB})`, and :math:`_C\boldsymbol{\xi'} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_{CA})`. This version of Barfoot and Furgale [1]_ approximates the covariance up to 4th-order terms. Note that it is still an approximation of the covariance after concatenation of the two transforms. Parameters ---------- mean_A2B : array, shape (4, 4) Mean of transform from A to B. cov_A2B : array, shape (6, 6) Covariance of transform from A to B. Models uncertainty in frame B. mean_B2C : array, shape (4, 4) Mean of transform from B to C. cov_B2C : array, shape (6, 6) Covariance of transform from B to C. Models uncertainty in frame C. Returns ------- mean_A2C : array, shape (4, 4) Mean of new pose. cov_A2C : array, shape (6, 6) Covariance of new pose. Models uncertainty in frame C. See Also -------- concat_locally_uncertain_transforms : Concatenate two independent locally uncertain transformations. pytransform3d.transformations.concat : Concatenate two transformations. References ---------- .. [1] Barfoot, T. D., Furgale, P. T. (2014). Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems. IEEE Transactions on Robotics, 30(3), pp. 679-693, doi: 10.1109/TRO.2014.2298059. """ mean_A2C = concat(mean_A2B, mean_B2C) ad_B2C = adjoint_from_transform(mean_B2C) cov_A2B_in_C = np.dot(ad_B2C, np.dot(cov_A2B, ad_B2C.T)) second_order_terms = cov_B2C + cov_A2B_in_C cov_A2C = second_order_terms + _compound_cov_fourth_order_terms( cov_B2C, cov_A2B_in_C ) return mean_A2C, cov_A2C def _compound_cov_fourth_order_terms(cov1, cov2_prime): cov1 = _swap_cov(cov1) cov2_prime = _swap_cov(cov2_prime) cov1_11 = cov1[:3, :3] cov1_22 = cov1[3:, 3:] cov1_12 = cov1[:3, 3:] cov2_11 = cov2_prime[:3, :3] cov2_22 = cov2_prime[3:, 3:] cov2_12 = cov2_prime[:3, 3:] A1 = np.block( [ [_covop1(cov1_22), _covop1(cov1_12 + cov1_12.T)], [np.zeros((3, 3)), _covop1(cov1_22)], ] ) A2 = np.block( [ [_covop1(cov2_22), _covop1(cov2_12 + cov2_12.T)], [np.zeros((3, 3)), _covop1(cov2_22)], ] ) B_11 = ( _covop2(cov1_22, cov2_11) + _covop2(cov1_12.T, cov2_12) + _covop2(cov1_12, cov2_12.T) + _covop2(cov1_11, cov2_22) ) B_12 = _covop2(cov1_22, cov2_12.T) + _covop2(cov1_12.T, cov2_22) B_22 = _covop2(cov1_22, cov2_22) B = np.block([[B_11, B_12], [B_12.T, B_22]]) return _swap_cov( ( np.dot(A1, cov2_prime) + np.dot(cov2_prime, A1.T) + np.dot(A2, cov1) + np.dot(cov1, A2.T) ) / 12.0 + B / 4.0 ) def _swap_cov(cov): return np.block([[cov[3:, 3:], cov[3:, :3]], [cov[:3, 3:], cov[:3, :3]]]) def _covop1(A): return -np.trace(A) * np.eye(len(A)) + A def _covop2(A, B): return np.dot(_covop1(A), _covop1(B)) + _covop1(np.dot(B, A)) def concat_locally_uncertain_transforms(mean_A2B, mean_B2C, cov_A, cov_B): r"""Concatenate two independent locally uncertain transformations. We assume that the two distributions are independent. Each of the two transformations is locally uncertain (not in the global / world frame), that is, samples are generated through .. math:: \boldsymbol{T} = \overline{\boldsymbol{T}} Exp(\boldsymbol{\xi}), where :math:`\boldsymbol{T} \in SE(3)` is a sampled transformation matrix, :math:`\overline{\boldsymbol{T}} \in SE(3)` is the mean transformation, and :math:`\boldsymbol{\xi} \in \mathbb{R}^6` are exponential coordinates of transformations and are distributed according to a Gaussian distribution with zero mean and covariance :math:`\boldsymbol{\Sigma} \in \mathbb{R}^{6 \times 6}`, that is, :math:`\boldsymbol{\xi} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma})`. The concatenation order is the same as in :func:`~pytransform3d.transformations.concat`, that is, the transformation B2C is left-multiplied to A2B. Note that the order of arguments is different from :func:`~pytransform3d.uncertainty.concat_globally_uncertain_transforms`. Hence, the full model is .. math:: \overline{\boldsymbol{T}}_{CA} Exp(_A\boldsymbol{\xi'}) = \overline{\boldsymbol{T}}_{CB} Exp(_B\boldsymbol{\xi}) \overline{\boldsymbol{T}}_{BA} Exp(_A\boldsymbol{\xi}), where :math:`_B\boldsymbol{\xi} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_B)`, :math:`_A\boldsymbol{\xi} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_A)`, and :math:`_A\boldsymbol{\xi'} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_{A,total})`. This version of Meyer et al. [1]_ approximates the covariance up to 2nd-order terms. Parameters ---------- mean_A2B : array, shape (4, 4) Mean of transform from A to B: :math:`\overline{\boldsymbol{T}}_{BA}`. mean_B2C : array, shape (4, 4) Mean of transform from B to C: :math:`\overline{\boldsymbol{T}}_{CB}`. cov_A : array, shape (6, 6) Covariance of noise in frame A: :math:`\boldsymbol{\Sigma}_A`. Noise samples are right-multiplied with the mean transform A2B. cov_B : array, shape (6, 6) Covariance of noise in frame B: :math:`\boldsymbol{\Sigma}_B`. Noise samples are right-multiplied with the mean transform B2C. Returns ------- mean_A2C : array, shape (4, 4) Mean of new pose. cov_A_total : array, shape (6, 6) Covariance of accumulated noise in frame A. See Also -------- concat_globally_uncertain_transforms : Concatenate two independent globally uncertain transformations. pytransform3d.transformations.concat : Concatenate two transformations. References ---------- .. [1] Meyer, L., Strobl, K. H., Triebel, R. (2022). The Probabilistic Robot Kinematics Model and its Application to Sensor Fusion. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Kyoto, Japan (pp. 3263-3270), doi: 10.1109/IROS47612.2022.9981399. https://elib.dlr.de/191928/1/202212_ELIB_PAPER_VERSION_with_copyright.pdf """ mean_A2C = concat(mean_A2B, mean_B2C) ad_B2A = adjoint_from_transform(invert_transform(mean_A2B)) cov_B_in_A = np.dot(ad_B2A, np.dot(cov_B, ad_B2A.T)) cov_A_total = cov_B_in_A + cov_A return mean_A2C, cov_A_total ================================================ FILE: pytransform3d/uncertainty/_composition.pyi ================================================ import numpy as np import numpy.typing as npt def concat_globally_uncertain_transforms( mean_A2B: npt.ArrayLike, cov_A2B: npt.ArrayLike, mean_B2C: npt.ArrayLike, cov_B2C: npt.ArrayLike, ) -> np.ndarray: ... def concat_locally_uncertain_transforms( mean_A2B: npt.ArrayLike, mean_B2C: npt.ArrayLike, cov_A: npt.ArrayLike, cov_B: npt.ArrayLike, ) -> np.ndarray: ... ================================================ FILE: pytransform3d/uncertainty/_frechet_mean.py ================================================ """Frechet mean and related functions.""" import numpy as np from ..batch_rotations import ( axis_angles_from_matrices, matrices_from_compact_axis_angles, ) from ..trajectories import ( concat_many_to_one, exponential_coordinates_from_transforms, transforms_from_exponential_coordinates, ) from ..transformations import ( concat, invert_transform, ) def frechet_mean( samples, mean0, exp, log, inv, concat_one_to_one, concat_many_to_one, n_iter=20, ): r"""Compute the Fréchet mean of samples on a smooth Riemannian manifold. The mean is computed with an iterative optimization algorithm [1]_ [2]_: 1. For a set of samples :math:`\{x_1, \ldots, x_n\}`, we initialize the estimated mean :math:`\bar{x}_0`, e.g., as :math:`x_1`. 2. For a fixed number of steps :math:`K`, in each iteration :math:`k` we improve the estimation of the mean by a. Computing the distance of each sample to the current estimate of the mean in tangent space with :math:`d_{i,k} \leftarrow \log (x_i \cdot \bar{x}_k^{-1})`. b. Updating the estimate of the mean with :math:`\bar{x}_{k+1} \leftarrow \exp(\frac{1}{N}\sum_i d_{i,k}) \cdot \bar{x}_k`. 3. Return :math:`\bar{x}_K`. Parameters ---------- samples : array-like, shape (n_samples, ...) Samples on a smooth Riemannian manifold. mean0 : array-like, shape (...) Initial guess for the mean on the manifold. exp : callable Exponential map from the tangent space to the manifold. log : callable Logarithmic map from the manifold to the tangent space. inv : callable Computes the inverse of an element on the manifold. concat_one_to_one : callable Concatenates elements on the manifold. concat_many_to_one : callable Concatenates multiple elements on the manifold to one element on the manifold. n_iter : int, optional (default: 20) Number of iterations of the optimization algorithm. Returns ------- mean : array, shape (...) Fréchet mean on the manifold. mean_diffs : array, shape (n_samples, n_tangent_space_components) Differences between the mean and the samples in the tangent space. These can be used to compute the covariance. They are returned to avoid recomputing them. See Also -------- estimate_gaussian_rotation_matrix_from_samples Uses the Frechet mean to compute the mean of a Gaussian distribution over rotations. estimate_gaussian_transform_from_samples Uses the Frechet mean to compute the mean of a Gaussian distribution over transformations. References ---------- .. [1] Fréchet, M. (1948). Les éléments aléatoires de nature quelconque dans un espace distancié. Annales de l’Institut Henri Poincaré, 10(3), 215–310. .. [2] Pennec, X. (2006). Intrinsic Statistics on Riemannian Manifolds: Basic Tools for Geometric Measurements. J Math Imaging Vis 25, 127-154. https://doi.org/10.1007/s10851-006-6228-4 """ assert len(samples) > 0 samples = np.asarray(samples) mean = np.copy(mean0) for _ in range(n_iter): mean_diffs = log(concat_many_to_one(samples, inv(mean))) avg_mean_diff = np.mean(mean_diffs, axis=0) mean = concat_one_to_one(mean, exp(avg_mean_diff)) return mean, mean_diffs def estimate_gaussian_rotation_matrix_from_samples(samples): """Estimate Gaussian distribution over rotations from samples. Computes the Fréchet mean of the samples and the covariance in tangent space (exponential coordinates of rotation / rotation vectors) using an unbiased estimator as outlines by Eade [1]_. Parameters ---------- samples : array-like, shape (n_samples, 3, 3) Sampled rotations represented by rotation matrices. Returns ------- mean : array, shape (3, 3) Mean of the Gaussian distribution as rotation matrix. cov : array, shape (3, 3) Covariance of the Gaussian distribution in exponential coordinates. See Also -------- frechet_mean Algorithm used to compute the mean of the Gaussian. References ---------- .. [1] Eade, E. (2017). Lie Groups for 2D and 3D Transformations. https://ethaneade.com/lie.pdf """ def compact_axis_angles_from_matrices(Rs): A = axis_angles_from_matrices(Rs) return A[:, :3] * A[:, 3, np.newaxis] mean, mean_diffs = frechet_mean( samples=samples, mean0=samples[0], exp=matrices_from_compact_axis_angles, log=compact_axis_angles_from_matrices, inv=lambda R: R.T, concat_one_to_one=lambda R1, R2: np.dot(R2, R1), concat_many_to_one=concat_many_to_one, n_iter=20, ) cov = np.cov(mean_diffs, rowvar=False, bias=True) return mean, cov def estimate_gaussian_transform_from_samples(samples): """Estimate Gaussian distribution over transformations from samples. Computes the Fréchet mean of the samples and the covariance in tangent space (exponential coordinates of transformation) using an unbiased estimator as outlines by Eade [1]_. Parameters ---------- samples : array-like, shape (n_samples, 4, 4) Sampled transformations represented by homogeneous matrices. Returns ------- mean : array, shape (4, 4) Mean as homogeneous transformation matrix. cov : array, shape (6, 6) Covariance of distribution in exponential coordinate space. See Also -------- frechet_mean Algorithm used to compute the mean of the Gaussian. References ---------- .. [1] Eade, E. (2017). Lie Groups for 2D and 3D Transformations. https://ethaneade.com/lie.pdf """ mean, mean_diffs = frechet_mean( samples=samples, mean0=samples[0], exp=transforms_from_exponential_coordinates, log=exponential_coordinates_from_transforms, inv=invert_transform, concat_one_to_one=concat, concat_many_to_one=concat_many_to_one, n_iter=20, ) cov = np.cov(mean_diffs, rowvar=False, bias=True) return mean, cov ================================================ FILE: pytransform3d/uncertainty/_frechet_mean.pyi ================================================ import numpy as np import numpy.typing as npt from typing import Tuple, Callable def frechet_mean( samples: npt.ArrayLike, mean0: npt.ArrayLike, exp: Callable, log: Callable, inv: Callable, concat_one_to_one: Callable, concat_many_to_one: Callable, n_iter: int = ..., ) -> Tuple[np.ndarray, np.ndarray]: ... def estimate_gaussian_rotation_matrix_from_samples( samples: npt.ArrayLike, ) -> Tuple[np.ndarray, np.ndarray]: ... def estimate_gaussian_transform_from_samples( samples: npt.ArrayLike, ) -> Tuple[np.ndarray, np.ndarray]: ... ================================================ FILE: pytransform3d/uncertainty/_fusion.py ================================================ """Fusion of poses.""" import numpy as np from ..transformations import ( exponential_coordinates_from_transform, invert_transform, left_jacobian_SE3_inv, transform_from_exponential_coordinates, ) def pose_fusion(means, covs): """Fuse Gaussian distributions of multiple poses. Parameters ---------- means : array-like, shape (n_poses, 4, 4) Homogeneous transformation matrices. covs : array-like, shape (n_poses, 6, 6) Covariances of pose distributions in exponential coordinate space. Returns ------- mean : array, shape (4, 4) Fused pose mean. cov : array, shape (6, 6) Fused pose covariance. V : float Error of optimization objective. References ---------- .. [1] Barfoot, T. D., Furgale, P. T. (2014). Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems. IEEE Transactions on Robotics, 30(3), pp. 679-693, doi: 10.1109/TRO.2014.2298059. """ n_poses = len(means) covs_inv = [np.linalg.inv(cov) for cov in covs] mean = np.eye(4) LHS = np.empty((6, 6)) RHS = np.empty(6) for _ in range(20): LHS[:, :] = 0.0 RHS[:] = 0.0 for k in range(n_poses): x_ik = exponential_coordinates_from_transform( np.dot(mean, invert_transform(means[k])) ) J_inv = left_jacobian_SE3_inv(x_ik) J_invT_S = np.dot(J_inv.T, covs_inv[k]) LHS += np.dot(J_invT_S, J_inv) RHS += np.dot(J_invT_S, x_ik) x_i = np.linalg.solve(-LHS, RHS) mean = np.dot(transform_from_exponential_coordinates(x_i), mean) cov = np.linalg.inv(LHS) V = 0.0 for k in range(n_poses): x_ik = exponential_coordinates_from_transform( np.dot(mean, invert_transform(means[k])) ) V += 0.5 * np.dot(x_ik, np.dot(covs_inv[k], x_ik)) return mean, cov, V ================================================ FILE: pytransform3d/uncertainty/_fusion.pyi ================================================ import numpy as np import numpy.typing as npt def pose_fusion(means: npt.ArrayLike, covs: npt.ArrayLike) -> np.ndarray: ... ================================================ FILE: pytransform3d/uncertainty/_invert.py ================================================ """Inversion of uncertain transformations.""" import numpy as np from ..transformations import ( adjoint_from_transform, invert_transform, ) def invert_uncertain_transform(mean, cov): r"""Invert uncertain transform. For the mean :math:`\boldsymbol{T}_{BA}`, the inverse is simply :math:`\boldsymbol{T}_{BA}^{-1} = \boldsymbol{T}_{AB}`. For the covariance, we need the adjoint of the inverse transformation :math:`\left[Ad_{\boldsymbol{T}_{BA}^{-1}}\right]`: .. math:: \boldsymbol{\Sigma}_{\boldsymbol{T}_{AB}} = \left[Ad_{\boldsymbol{T}_{BA}^{-1}}\right] \boldsymbol{\Sigma}_{\boldsymbol{T}_{BA}} \left[Ad_{\boldsymbol{T}_{BA}^{-1}}\right]^T Parameters ---------- mean : array-like, shape (4, 4) Mean of transform from frame A to frame B. cov : array, shape (6, 6) Covariance of transform from frame A to frame B in exponential coordinate space. Returns ------- mean_inv : array, shape (4, 4) Mean of transform from frame B to frame A. cov_inv : array, shape (6, 6) Covariance of transform from frame B to frame A in exponential coordinate space. See Also -------- pytransform3d.transformations.invert_transform : Invert transformation without uncertainty. References ---------- .. [1] Mangelson, G., Vasudevan, E. (2019). Characterizing the Uncertainty of Jointly Distributed Poses in the Lie Algebra, https://arxiv.org/pdf/1906.07795.pdf """ mean_inv = invert_transform(mean) ad_inv = adjoint_from_transform(mean_inv) cov_inv = np.dot(ad_inv, np.dot(cov, ad_inv.T)) return mean_inv, cov_inv ================================================ FILE: pytransform3d/uncertainty/_invert.pyi ================================================ from typing import Tuple import numpy as np import numpy.typing as npt def invert_uncertain_transform( mean: npt.ArrayLike, cov: npt.ArrayLike ) -> Tuple[np.ndarray, np.ndarray]: ... ================================================ FILE: pytransform3d/uncertainty/_plot.py ================================================ """Plotting of uncertain transformations.""" import numpy as np from .._geometry import unit_sphere_surface_grid from ..trajectories import ( transforms_from_exponential_coordinates, ) from ..transformations import ( transform_from, ) def to_ellipsoid(mean, cov): """Compute error ellipsoid. An error ellipsoid indicates the equiprobable surface. The resulting ellipsoid includes one standard deviation of the data along each main axis, which covers approximately 68.27% of the data. Multiplying the radii with factors > 1 will increase the coverage. The usual factors for Gaussian distributions apply: * 1 - 68.27% * 1.65 - 90% * 1.96 - 95% * 2 - 95.45% * 2.58 - 99% * 3 - 99.73% Parameters ---------- mean : array-like, shape (3,) Mean of distribution. cov : array-like, shape (3, 3) Covariance of distribution. Returns ------- ellipsoid2origin : array, shape (4, 4) Ellipsoid frame in world frame. Note that there are multiple solutions possible for the orientation because an ellipsoid is symmetric. A body-fixed rotation around a main axis by 180 degree results in the same ellipsoid. radii : array, shape (3,) Radii of ellipsoid, coinciding with standard deviations along the three axes of the ellipsoid. These are sorted in ascending order. """ from scipy import linalg radii, R = linalg.eigh(cov) if np.linalg.det(R) < 0: # undo reflection (exploit symmetry) R *= -1 ellipsoid2origin = transform_from(R=R, p=mean) return ellipsoid2origin, np.sqrt(np.abs(radii)) def to_projected_ellipsoid(mean, cov, factor=1.96, n_steps=20): """Compute projected error ellipsoid. An error ellipsoid shows equiprobable points. This is a projection of a Gaussian distribution in exponential coordinate space to 3D. Parameters ---------- mean : array-like, shape (4, 4) Mean of pose distribution. cov : array-like, shape (6, 6) Covariance of pose distribution in exponential coordinate space. factor : float, optional (default: 1.96) Multiple of the standard deviations that should be plotted. n_steps : int, optional (default: 20) Number of discrete steps plotted in each dimension. Returns ------- x : array, shape (n_steps, n_steps) Coordinates on x-axis of grid on projected ellipsoid. y : array, shape (n_steps, n_steps) Coordinates on y-axis of grid on projected ellipsoid. z : array, shape (n_steps, n_steps) Coordinates on z-axis of grid on projected ellipsoid. """ from scipy import linalg vals, vecs = linalg.eigh(cov) order = vals.argsort()[::-1] vals, vecs = vals[order], vecs[:, order] radii = factor * np.sqrt(vals[:3]) # Grid on ellipsoid in exponential coordinate space x, y, z = unit_sphere_surface_grid(n_steps) x *= radii[0] y *= radii[1] z *= radii[2] P = np.column_stack((x.reshape(-1), y.reshape(-1), z.reshape(-1))) P = np.dot(P, vecs[:, :3].T) # Grid in Cartesian space T_diff = transforms_from_exponential_coordinates(P) # same as T_diff[m, :3, :3].T.dot(T_diff[m, :3, 3]) for each m P = np.einsum("ikj,ik->ij", T_diff[:, :3, :3], T_diff[:, :3, 3]) P = (np.dot(P, mean[:3, :3].T) + mean[np.newaxis, :3, 3]).T shape = x.shape x = P[0].reshape(*shape) y = P[1].reshape(*shape) z = P[2].reshape(*shape) return x, y, z def plot_projected_ellipsoid( ax, mean, cov, factor=1.96, wireframe=True, n_steps=20, color=None, alpha=1.0, linewidth=1.0, ): # pragma: no cover """Plots projected equiprobable ellipsoid in 3D. An error ellipsoid shows equiprobable points. This is a projection of a Gaussian distribution in exponential coordinate space to 3D. Parameters ---------- ax : axis Matplotlib axis. mean : array-like, shape (4, 4) Mean pose. cov : array-like, shape (6, 6) Covariance in exponential coordinate space. factor : float, optional (default: 1.96) Multiple of the standard deviations that should be plotted. wireframe : bool, optional (default: True) Plot wireframe of ellipsoid and surface otherwise. n_steps : int, optional (default: 20) Number of discrete steps plotted in each dimension. color : str, optional (default: None) Color in which the equiprobably lines should be plotted. alpha : float, optional (default: 1.0) Alpha value for lines. linewidth : float, optional (default: 1.0) Line width for wireframe plot. Returns ------- ax : axis Matplotlib axis. """ x, y, z = to_projected_ellipsoid(mean, cov, factor, n_steps) if wireframe: ax.plot_wireframe( x, y, z, rstride=2, cstride=2, color=color, alpha=alpha, linewidth=linewidth, ) else: ax.plot_surface(x, y, z, color=color, alpha=alpha, linewidth=0) return ax ================================================ FILE: pytransform3d/uncertainty/_plot.pyi ================================================ import numpy as np import numpy.typing as npt from typing import Tuple, Union from mpl_toolkits.mplot3d import Axes3D def to_ellipsoid( mean: npt.ArrayLike, cov: npt.ArrayLike ) -> Tuple[np.ndarray, np.ndarray]: ... def to_projected_ellipsoid( mean: npt.ArrayLike, cov: npt.ArrayLike, factor: float = ..., n_steps: int = ..., ) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: ... def plot_projected_ellipsoid( ax: Union[None, Axes3D], mean: npt.ArrayLike, cov: npt.ArrayLike, factor: float = ..., wireframe: bool = ..., n_steps: int = ..., color: str = ..., alpha: float = ..., linewidth: float = ..., ) -> Axes3D: ... ================================================ FILE: pytransform3d/uncertainty/test/test_uncertainty.py ================================================ import numpy as np import pytest from numpy.testing import assert_array_almost_equal import pytransform3d.rotations as pr import pytransform3d.transformations as pt import pytransform3d.uncertainty as pu def test_estimate_gaussian_rotation_matrix_from_samples(): rng = np.random.default_rng(34) mean = pr.matrix_from_axis_angle([0.3, 0.2, 0.5, 0.25 * np.pi]) cov = np.diag([0.1, 0.05, 0.09]) ** 2 samples = np.array([pr.random_matrix(rng, mean, cov) for _ in range(1000)]) mean_est, cov_est = pu.estimate_gaussian_rotation_matrix_from_samples( samples ) assert_array_almost_equal(mean, mean_est, decimal=2) assert_array_almost_equal(cov, cov_est, decimal=2) def test_same_fuse_poses(): mean1 = np.array( [ [0.8573, -0.2854, 0.4285, 3.5368], [-0.1113, 0.7098, 0.6956, -3.5165], [-0.5026, -0.6440, 0.5767, -0.9112], [0.0, 0.0, 0.0, 1.0000], ] ) mean1[:3, :3] = pr.norm_matrix(mean1[:3, :3]) mean2 = np.array( [ [0.5441, -0.6105, 0.5755, -1.0935], [0.8276, 0.5032, -0.2487, 5.5992], [-0.1377, 0.6116, 0.7791, 0.2690], [0.0, 0.0, 0.0, 1.0000], ] ) mean2[:3, :3] = pr.norm_matrix(mean2[:3, :3]) mean3 = np.array( [ [-0.0211, -0.7869, 0.6167, -3.0968], [-0.2293, 0.6042, 0.7631, 2.0868], [-0.9731, -0.1254, -0.1932, 2.0239], [0.0, 0.0, 0.0, 1.0000], ] ) mean3[:3, :3] = pr.norm_matrix(mean3[:3, :3]) alpha = 5.0 cov1 = alpha * np.diag([0.1, 0.2, 0.1, 2.0, 1.0, 1.0]) cov2 = alpha * np.diag([0.1, 0.1, 0.2, 1.0, 3.0, 1.0]) cov3 = alpha * np.diag([0.2, 0.1, 0.1, 1.0, 1.0, 5.0]) means = [mean1, mean2, mean3] covs = [cov1, cov2, cov3] mean_est, cov_est, V = pu.pose_fusion(means, covs) mean_exp = np.array( [ [0.2967, -0.7157, 0.6323, -1.4887], [0.5338, 0.6733, 0.5116, 0.9935], [-0.7918, 0.1857, 0.5818, -2.7035], [0.0, 0.0, 0.0, 1.0000], ] ) cov_exp = np.array( [ [ 0.14907707, -0.01935277, -0.0107348, -0.02442925, -0.09843835, 0.0054134, ], [ -0.01935277, 0.14648459, 0.02055571, 0.11121064, 0.06272014, -0.08553834, ], [ -0.0107348, 0.02055571, 0.15260209, -0.07451066, 0.06531188, -0.01890897, ], [ -0.02442925, 0.11121064, -0.07451066, 2.10256906, 0.13695598, -0.29705468, ], [ -0.09843835, 0.06272014, 0.06531188, 0.13695598, 2.29286157, -0.58004, ], [ 0.0054134, -0.08553834, -0.01890897, -0.29705468, -0.58004, 2.34528443, ], ] ) assert_array_almost_equal(mean_exp, mean_est, decimal=4) assert_array_almost_equal(cov_exp, cov_est) assert pytest.approx(V, abs=1e-4) == 4.6537 def test_invert_pose(): rng = np.random.default_rng(2) for _ in range(5): T = pt.random_transform(rng) cov = np.diag(rng.random((6, 6))) T_inv, cov_inv = pu.invert_uncertain_transform(T, cov) T2, cov2 = pu.invert_uncertain_transform(T_inv, cov_inv) assert_array_almost_equal(T, T2) assert_array_almost_equal(cov, cov2) def test_sample_estimate_gaussian(): rng = np.random.default_rng(2000) mean = pt.transform_from(R=np.eye(3), p=np.array([0.0, 0.0, 0.5])) cov = np.diag([0.001, 0.001, 0.5, 0.001, 0.001, 0.001]) samples = np.array( [pt.random_transform(rng, mean, cov) for _ in range(1000)] ) mean_est, cov_est = pu.estimate_gaussian_transform_from_samples(samples) assert_array_almost_equal(mean, mean_est, decimal=2) assert_array_almost_equal(cov, cov_est, decimal=2) def test_concat_globally_uncertain_transforms(): cov_pose_chol = np.diag([0, 0, 0.03, 0, 0, 0]) cov_pose = np.dot(cov_pose_chol, cov_pose_chol.T) velocity_vector = np.array([0, 0, 0, 1.0, 0, 0]) T_vel = pt.transform_from_exponential_coordinates(velocity_vector) n_steps = 100 T_est = np.eye(4) cov_est = np.zeros((6, 6)) for _ in range(n_steps): T_est, cov_est = pu.concat_globally_uncertain_transforms( T_est, cov_est, T_vel, cov_pose ) assert_array_almost_equal( T_est, np.array( [[1, 0, 0, n_steps], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]] ), ) # achievable with second-order terms assert cov_est[2, 2] > 0 assert cov_est[4, 4] > 0 assert cov_est[2, 4] != 0 assert cov_est[4, 2] != 0 # achievable only with fourth-order terms assert cov_est[3, 3] > 0 assert cov_est[4, 2] != 0 assert cov_est[2, 4] != 0 def test_concat_locally_uncertain_transforms(): mean_A2B = pt.transform_from( R=pr.matrix_from_euler([0.5, 0.0, 0.0], 0, 1, 2, True), p=np.array([1.0, 0.0, 0.0]), ) mean_B2C = pt.transform_from( R=pr.matrix_from_euler([0.0, 0.5, 0.0], 0, 1, 2, True), p=np.array([0.0, 1.0, 0.0]), ) cov_A = np.diag([1.0, 0, 0, 0, 0, 0]) cov_B = np.diag([0, 1.0, 0, 0, 0, 0]) mean_A2C, cov_A_total = pu.concat_locally_uncertain_transforms( mean_A2B, mean_B2C, cov_A, np.zeros((6, 6)) ) assert_array_almost_equal(mean_A2C, pt.concat(mean_A2B, mean_B2C)) assert_array_almost_equal(cov_A_total, cov_A) mean_A2C, cov_A_total = pu.concat_locally_uncertain_transforms( np.eye(4), mean_B2C, np.zeros((6, 6)), cov_B ) assert_array_almost_equal(cov_A_total, cov_B) mean_A2C, cov_A_total = pu.concat_locally_uncertain_transforms( mean_A2B, mean_B2C, np.zeros((6, 6)), cov_B ) ad_B2A = pt.adjoint_from_transform(pt.invert_transform(mean_A2B)) assert_array_almost_equal(cov_A_total, ad_B2A.dot(cov_B).dot(ad_B2A.T)) def test_to_ellipsoid(): mean = np.array([0.1, 0.2, 0.3]) cov = np.array([[25.0, 0.0, 0.0], [0.0, 4.0, 0.0], [0.0, 0.0, 9.0]]) ellipsoid2origin, radii = pu.to_ellipsoid(mean, cov) assert_array_almost_equal(ellipsoid2origin[:3, 3], mean) assert_array_almost_equal( ellipsoid2origin[:3, :3], np.array([[0, 0, 1], [1, 0, 0], [0, 1, 0]]) ) assert_array_almost_equal(radii, np.array([2.0, 3.0, 5.0])) rng = np.random.default_rng(28) for _ in range(5): R = pr.matrix_from_axis_angle(pr.random_axis_angle(rng)) std_devs = np.array([1.0, 2.0, 3.0]) cov = np.dot(R, np.dot(np.diag(std_devs**2), R.T)) ellipsoid2origin, radii = pu.to_ellipsoid(mean, cov) assert_array_almost_equal(ellipsoid2origin[:3, 3], mean) # multiple solutions for the rotation matrix are possible because of # ellipsoid symmetries, hence, we only check the absolute values assert_array_almost_equal(np.abs(ellipsoid2origin[:3, :3]), np.abs(R)) assert_array_almost_equal(radii, std_devs) def test_projected_ellipsoid(): mean = np.eye(4) cov = np.eye(6) x, y, z = pu.to_projected_ellipsoid(mean, cov, factor=1.0, n_steps=20) P = np.column_stack((x.reshape(-1), y.reshape(-1), z.reshape(-1))) r = np.linalg.norm(P, axis=1) assert_array_almost_equal(r, np.ones_like(3)) pos_mean = np.array([0.5, -5.3, 10.5]) mean = pt.transform_from(R=np.eye(3), p=pos_mean) x, y, z = pu.to_projected_ellipsoid(mean, cov, factor=1.0, n_steps=20) P = np.column_stack((x.reshape(-1), y.reshape(-1), z.reshape(-1))) r = np.linalg.norm(P - pos_mean[np.newaxis], axis=1) assert_array_almost_equal(r, np.ones_like(3)) mean = np.eye(4) cov = np.diag([1, 1, 1, 4, 1, 1]) x, y, z = pu.to_projected_ellipsoid(mean, cov, factor=1.0, n_steps=20) P = np.column_stack((x.reshape(-1), y.reshape(-1), z.reshape(-1))) r = np.linalg.norm(P, axis=1) assert np.all(r >= 1.0) assert np.all(r <= 2.0) ================================================ FILE: pytransform3d/urdf.py ================================================ """Load transformations from URDF files. See :doc:`user_guide/transform_manager` for more information. """ import os import re import warnings import numpy as np from lxml import etree from .rotations import ( matrix_from_euler, matrix_from_axis_angle, norm_vector, ) from .transform_manager import TransformManager from .transformations import transform_from, concat class UrdfTransformManager(TransformManager): """Transformation manager that can load URDF files. The Unified Robot Description Format (URDF) [1]_ is the most common format to describe kinematic and dynamic properties of robots [2]_. This transformation manager allows to set the joint configurations of a robot loaded from a URDF file and provides an interface to its geometric and visual representation. .. warning:: Note that this module requires the Python package lxml. .. note:: Joint angles must be given in radians. Parameters ---------- strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. check : bool, optional (default: True) Check if transformation matrices are valid and requested nodes exist, which might significantly slow down some operations. References ---------- .. [1] ROS Wiki: urdf, http://wiki.ros.org/urdf .. [2] Tola, D., Corke, P. (2023). Understanding URDF: A Dataset and Analysis. In IEEE Robotics and Automation Letters 9(5), pp. 4479-4486, doi: 10.1109/LRA.2024.3381482. https://arxiv.org/abs/2308.00514 """ def __init__(self, strict_check=True, check=True): super(UrdfTransformManager, self).__init__(strict_check, check) self._joints = {} self.collision_objects = [] self.visuals = [] def add_joint( self, joint_name, from_frame, to_frame, child2parent, axis, limits=(float("-inf"), float("inf")), joint_type="revolute", ): """Add joint. Parameters ---------- joint_name : str Name of the joint from_frame : Hashable Child link of the joint to_frame : Hashable Parent link of the joint child2parent : array-like, shape (4, 4) Transformation from child to parent axis : array-like, shape (3,) Rotation axis of the joint (defined in the child frame) limits : pair of float, optional (default: (-inf, inf)) Lower and upper joint angle limit joint_type : str, optional (default: 'revolute') Joint type: revolute, prismatic, or fixed (continuous is the same as revolute) """ self.add_transform(from_frame, to_frame, child2parent) self._joints[joint_name] = ( from_frame, to_frame, child2parent, norm_vector(axis), limits, joint_type, ) def set_joint(self, joint_name, value): """Set joint position. Note that joint values are clipped to their limits. Parameters ---------- joint_name : str Name of the joint value : float Joint angle in radians in case of revolute joints or position in case of prismatic joint. Raises ------ KeyError If joint_name is unknown """ if joint_name not in self._joints: raise KeyError("Joint '%s' is not known" % joint_name) from_frame, to_frame, child2parent, axis, limits, joint_type = ( self._joints[joint_name] ) # this is way faster than np.clip: value = min(max(value, limits[0]), limits[1]) if joint_type == "revolute": joint_rotation = matrix_from_axis_angle(np.hstack((axis, (value,)))) joint2A = transform_from( joint_rotation, np.zeros(3), strict_check=self.strict_check ) elif joint_type == "prismatic": joint_offset = value * axis joint2A = transform_from( np.eye(3), joint_offset, strict_check=self.strict_check ) else: assert joint_type == "fixed" warnings.warn( "Trying to set a fixed joint", UserWarning, stacklevel=2 ) return self.add_transform( from_frame, to_frame, concat( joint2A, child2parent, strict_check=self.strict_check, check=self.check, ), ) def get_joint_limits(self, joint_name): """Get limits of a joint. Parameters ---------- joint_name : str Name of the joint Returns ------- limits : pair of float Lower and upper joint angle limit Raises ------ KeyError If joint_name is unknown """ if joint_name not in self._joints: raise KeyError("Joint '%s' is not known" % joint_name) return self._joints[joint_name][4] def load_urdf(self, urdf_xml, mesh_path=None, package_dir=None): """Load URDF file into transformation manager. Parameters ---------- urdf_xml : str Robot definition in URDF mesh_path : str, optional (default: None) Path in which we search for meshes that are defined in the URDF. Meshes will be ignored if it is set to None and no 'package_dir' is given. package_dir : str, optional (default: None) Some URDFs start file names with 'package://' to refer to the ROS package in which these files (textures, meshes) are located. This variable defines to which path this prefix will be resolved. """ robot_name, links, joints = parse_urdf( urdf_xml, mesh_path, package_dir, self.strict_check ) initialize_urdf_transform_manager(self, robot_name, links, joints) def plot_visuals( self, frame, ax=None, ax_s=1, wireframe=False, convex_hull_of_mesh=True, alpha=0.3, ): # pragma: no cover """Plot all visuals in a given reference frame. Visuals can be boxes, spheres, cylinders, or meshes. Note that visuals that cannot be connected to the reference frame are omitted. Parameters ---------- frame : Hashable Reference frame ax : Matplotlib 3d axis, optional (default: None) If the axis is None, a new 3d axis will be created ax_s : float, optional (default: 1) Scaling of the new matplotlib 3d axis wireframe : bool, optional (default: False) Plot wireframe (surface otherwise) convex_hull_of_mesh : bool, optional (default: True) Displays convex hull of meshes instead of the original mesh. This makes plotting a lot faster with complex meshes. alpha : float, optional (default: 0.3) Alpha value of the surface / wireframe that will be plotted Returns ------- ax : Matplotlib 3d axis New or old axis """ return self._plot_objects( self.visuals, frame, ax, ax_s, wireframe, convex_hull_of_mesh, alpha ) def plot_collision_objects( self, frame, ax=None, ax_s=1, wireframe=True, convex_hull_of_mesh=True, alpha=1.0, ): # pragma: no cover """Plot all collision objects in a given reference frame. Collision objects can be boxes, spheres, cylinders, or meshes. Note that collision objects that cannot be connected to the reference frame are omitted. Parameters ---------- frame : Hashable Reference frame ax : Matplotlib 3d axis, optional (default: None) If the axis is None, a new 3d axis will be created ax_s : float, optional (default: 1) Scaling of the new matplotlib 3d axis wireframe : bool, optional (default: True) Plot wireframe (surface otherwise) convex_hull_of_mesh : bool, optional (default: True) Displays convex hull of meshes instead of the original mesh. This makes plotting a lot faster with complex meshes. alpha : float, optional (default: 1) Alpha value of the surface / wireframe that will be plotted Returns ------- ax : Matplotlib 3d axis New or old axis """ return self._plot_objects( self.collision_objects, frame, ax, ax_s, wireframe, convex_hull_of_mesh, alpha, ) def _plot_objects( self, objects, frame, ax=None, ax_s=1, wireframe=True, convex_hull_of_mesh=True, alpha=1.0, ): # pragma: no cover """Plot all objects in a given reference frame. Objects can be boxes, spheres, cylinders, or meshes. Note that objects that cannot be connected to the reference frame are omitted. Parameters ---------- objects : list Objects that will be plotted frame : Hashable Reference frame ax : Matplotlib 3d axis, optional (default: None) If the axis is None, a new 3d axis will be created ax_s : float, optional (default: 1) Scaling of the new matplotlib 3d axis wireframe : bool, optional (default: True) Plot wireframe (surface otherwise) convex_hull_of_mesh : bool, optional (default: True) Displays convex hull of meshes instead of the original mesh. This makes plotting a lot faster with complex meshes. alpha : float, optional (default: 1) Alpha value of the surface / wireframe that will be plotted Returns ------- ax : Matplotlib 3d axis New or old axis """ if ax is None: from .plot_utils import make_3d_axis ax = make_3d_axis(ax_s) for obj in objects: ax = obj.plot( self, frame, ax, wireframe=wireframe, convex_hull=convex_hull_of_mesh, alpha=alpha, ) return ax def parse_urdf(urdf_xml, mesh_path=None, package_dir=None, strict_check=True): """Parse information from URDF file. Parameters ---------- urdf_xml : str Robot definition in URDF mesh_path : str, optional (default: None) Path in which we search for meshes that are defined in the URDF. Meshes will be ignored if it is set to None and no 'package_dir' is given. package_dir : str, optional (default: None) Some URDFs start file names with 'package://' to refer to the ROS package in which these files (textures, meshes) are located. This variable defines to which path this prefix will be resolved. strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. Returns ------- robot_name : str Name of the robot links : list of Link Links of the robot joints : list of Joint Joints of the robot Raises ------ UrdfException If URDF is not valid """ # lxml does not allow whitespaces in the beginning urdf_xml = urdf_xml.strip() # lxml complains about unicode strings that start with unicode encoding # declaration. Hence, we have to convert them to bytes first. urdf_xml = bytes(urdf_xml.encode("utf-8")) try: root = etree.XML( urdf_xml, parser=etree.XMLParser(recover=True, remove_comments=True) ) except etree.XMLSyntaxError as e: raise UrdfException("Invalid XML.") from e # URDF XML schema: # https://github.com/ros/urdfdom/blob/master/xsd/urdf.xsd # If there is an XML namespace (xmlns=...) specified, this will break # parsing as the root tag, and the tag of all elements underneath root, # will have a {namespace} prefix. As such, normalize the namespace to the # default (i.e. no namespace). _normalize_namespace(root) if root.tag != "robot": raise UrdfException("Robot tag is missing.") tree = etree.ElementTree(root) if "name" not in root.attrib: raise UrdfException("Attribute 'name' is missing in robot tag.") robot_name = root.attrib["name"] materials = dict( [_parse_material(material) for material in tree.findall("material")] ) links = [ _parse_link(link, materials, mesh_path, package_dir, strict_check) for link in tree.findall("link") ] link_names = [link.name for link in links] joints = [ _parse_joint(joint, link_names, strict_check) for joint in tree.findall("joint") ] return robot_name, links, joints def initialize_urdf_transform_manager(tm, robot_name, links, joints): """Initializes transform manager from previously parsed URDF data. Parameters ---------- tm : UrdfTransformManager Transform manager robot_name : str Name of the robot links : list of Link Links of the robot joints : list of Joint Joints of the robot """ tm.add_transform(links[0].name, robot_name, np.eye(4)) _add_links(tm, links) _add_joints(tm, joints) def _normalize_namespace(root): """Normalizes the namespace of the root node and all elements underneath.""" root.tag = re.sub(r"\{.*?\}", "", root.tag) for element in root.iter(): element.tag = re.sub(r"\{.*?\}", "", element.tag) def _parse_material(material): """Parse material.""" if "name" not in material.attrib: raise UrdfException("Material name is missing.") colors = material.findall("color") if len(colors) > 1: raise UrdfException("More than one color is not allowed.") color = _parse_color(colors[0]) if len(colors) == 1 else None # TODO texture is currently ignored return material.attrib["name"], color def _parse_color(color): """Parse color.""" if "rgba" not in color.attrib: raise UrdfException("Attribute 'rgba' of color tag is missing.") return np.fromstring(color.attrib["rgba"], sep=" ") def _parse_link(link, materials, mesh_path, package_dir, strict_check): """Create link.""" if "name" not in link.attrib: raise UrdfException("Link name is missing.") result = Link() result.name = link.attrib["name"] visuals, visual_transforms = _parse_link_children( link, "visual", materials, mesh_path, package_dir, strict_check ) result.visuals = visuals result.transforms.extend(visual_transforms) collision_objects, collision_object_transforms = _parse_link_children( link, "collision", dict(), mesh_path, package_dir, strict_check ) result.collision_objects = collision_objects result.transforms.extend(collision_object_transforms) inertial = link.find("inertial") if inertial is not None: result.inertial_frame[:, :] = _parse_origin(inertial, strict_check) result.mass = _parse_mass(inertial) result.inertia[:, :] = _parse_inertia(inertial) result.transforms.append( ( "inertial_frame:%s" % result.name, result.name, result.inertial_frame, ) ) return result def _parse_link_children( link, child_type, materials, mesh_path, package_dir, strict_check ): """Parse collision objects or visuals.""" children = link.findall(child_type) shape_objects = [] transforms = [] for i, child in enumerate(children): name = "%s:%s/%s" % ( child_type, link.attrib["name"], child.attrib.get("name", i), ) color = None if child_type == "visual": material = child.find("material") if material is not None: material_name, color = _parse_material(material) if color is None and material_name in materials: color = materials[material_name] child2link = _parse_origin(child, strict_check) transforms.append((name, link.attrib["name"], child2link)) shape_objects.extend( _parse_geometry(child, name, color, mesh_path, package_dir) ) return shape_objects, transforms def _parse_geometry(child, name, color, mesh_path, package_dir): """Parse geometric primitives (box, cylinder, sphere) or meshes.""" geometry = child.find("geometry") if geometry is None: raise UrdfException("Missing geometry tag in link '%s'" % name) result = [] for shape_type in ["box", "cylinder", "sphere", "mesh"]: shapes = geometry.findall(shape_type) Cls = shape_classes[shape_type] for shape in shapes: shape_object = Cls( name, mesh_path=mesh_path, package_dir=package_dir, color=color ) shape_object.parse(shape) result.append(shape_object) return result def _parse_origin(entry, strict_check): """Parse transformation.""" origin = entry.find("origin") translation = np.zeros(3) rotation = np.eye(3) if origin is not None: if "xyz" in origin.attrib: translation = np.fromstring(origin.attrib["xyz"], sep=" ") if "rpy" in origin.attrib: roll_pitch_yaw = np.fromstring(origin.attrib["rpy"], sep=" ") # URDF and KDL use the active convention for rotation matrices. # For more details on how the URDF parser handles the # conversion from Euler angles, see this blog post: # https://orbitalstation.wordpress.com/tag/quaternion/ rotation = matrix_from_euler(roll_pitch_yaw, 0, 1, 2, True) return transform_from(rotation, translation, strict_check=strict_check) def _parse_mass(inertial): """Parse link mass.""" mass = inertial.find("mass") if mass is not None and "value" in mass.attrib: return float(mass.attrib["value"]) else: return 0.0 def _parse_inertia(inertial): """Parse inertia matrix.""" inertia = inertial.find("inertia") if inertia is None: return np.zeros((3, 3)) ixx = float(inertia.attrib.get("ixx", 0.0)) iyy = float(inertia.attrib.get("iyy", 0.0)) izz = float(inertia.attrib.get("izz", 0.0)) ixy = float(inertia.attrib.get("ixy", 0.0)) ixz = float(inertia.attrib.get("ixz", 0.0)) iyz = float(inertia.attrib.get("iyz", 0.0)) return np.array([[ixx, ixy, ixz], [ixy, iyy, iyz], [ixz, iyz, izz]]) def _parse_joint(joint, link_names, strict_check): """Create joint object.""" j = Joint() if "name" not in joint.attrib: raise UrdfException("Joint name is missing.") j.joint_name = joint.attrib["name"] if "type" not in joint.attrib: raise UrdfException( "Joint type is missing in joint '%s'." % j.joint_name ) parent = joint.find("parent") if parent is None: raise UrdfException("No parent specified in joint '%s'" % j.joint_name) if "link" not in parent.attrib: raise UrdfException( "No parent link name given in joint '%s'." % j.joint_name ) j.parent = parent.attrib["link"] if j.parent not in link_names: raise UrdfException( "Parent link '%s' of joint '%s' is not " "defined." % (j.parent, j.joint_name) ) child = joint.find("child") if child is None: raise UrdfException("No child specified in joint '%s'" % j.joint_name) if "link" not in child.attrib: raise UrdfException( "No child link name given in joint '%s'." % j.joint_name ) j.child = child.attrib["link"] if j.child not in link_names: raise UrdfException( "Child link '%s' of joint '%s' is not " "defined." % (j.child, j.joint_name) ) j.joint_type = joint.attrib["type"] if j.joint_type in ["planar", "floating"]: raise UrdfException("Unsupported joint type '%s'" % j.joint_type) if j.joint_type not in ["revolute", "continuous", "prismatic", "fixed"]: raise UrdfException( "Joint type '%s' is not allowed in a URDF " "document." % j.joint_type ) j.child2parent = _parse_origin(joint, strict_check) j.joint_axis = np.array([1, 0, 0]) if j.joint_type in ["revolute", "continuous", "prismatic"]: axis = joint.find("axis") if axis is not None and "xyz" in axis.attrib: j.joint_axis = np.fromstring(axis.attrib["xyz"], sep=" ") j.limits = _parse_limits(joint) return j def _parse_limits(joint): """Parse joint limits.""" limit = joint.find("limit") lower, upper = float("-inf"), float("inf") if limit is not None: if "lower" in limit.attrib: lower = float(limit.attrib["lower"]) if "upper" in limit.attrib: upper = float(limit.attrib["upper"]) return lower, upper def _add_links(tm, links): """Add previously parsed links. Parameters ---------- tm : UrdfTransformManager Transform manager links : list of Link Joint information from URDF """ for link in links: tm.visuals.extend(link.visuals) tm.collision_objects.extend(link.collision_objects) for from_frame, to_frame, transform in link.transforms: tm.add_transform(from_frame, to_frame, transform) def _add_joints(tm, joints): """Add previously parsed joints. Parameters ---------- tm : UrdfTransformManager Transform manager joints : list of Joint Joint information from URDF """ for joint in joints: if joint.joint_type in ["revolute", "continuous"]: tm.add_joint( joint.joint_name, joint.child, joint.parent, joint.child2parent, joint.joint_axis, joint.limits, "revolute", ) elif joint.joint_type == "prismatic": tm.add_joint( joint.joint_name, joint.child, joint.parent, joint.child2parent, joint.joint_axis, joint.limits, "prismatic", ) else: assert joint.joint_type == "fixed" tm.add_joint( joint.joint_name, joint.child, joint.parent, joint.child2parent, joint.joint_axis, (0.0, 0.0), "fixed", ) class Link(object): """Link from URDF file. This class is only required temporarily while we parse the URDF. Attributes ---------- name : str Link name visuals : list of Geometry Visual geometries collision_objects : list of Geometry Geometries for collision calculation transforms : list Transformations given as tuples: name of frame A, name of frame B, transform A2B inertial_frame : array, shape (4, 4) Pose of inertial frame with respect to the link mass : float Mass of the link inertia : array, shape (3, 3) Inertia matrix """ def __init__(self): self.name = None self.visuals = [] self.collision_objects = [] self.transforms = [] self.inertial_frame = np.eye(4) self.mass = 0.0 self.inertia = np.zeros((3, 3)) class Joint(object): """Joint from URDF file. This class is only required temporarily while we parse the URDF. Attributes ---------- child : str Name of the child parent : str Name of the parent frame child2parent : array-like, shape (4, 4) Transformation from child to parent joint_name : str Name of the joint that defines the transformation joint_axis : array-like, shape (3,) Rotation axis of the joint (defined in the child frame) joint_type : str Either 'fixed' or 'revolute' limits : pair of float Lower and upper joint angle limit """ def __init__(self): self.child = None self.parent = None self.child2parent = np.eye(4) self.joint_name = None self.joint_axis = None self.joint_type = "fixed" self.limits = float("-inf"), float("inf") class Geometry(object): """Geometrical object.""" def __init__(self, frame, mesh_path, package_dir, color): self.frame = frame self.mesh_path = mesh_path self.package_dir = package_dir self.color = color def parse(self, xml): """Parse parameters of geometry.""" def plot( self, tm, frame, ax=None, alpha=0.3, wireframe=True, convex_hull=True ): """Plot geometry.""" class Box(Geometry): """Geometrical object: box.""" def __init__(self, frame, mesh_path, package_dir, color): super(Box, self).__init__(frame, mesh_path, package_dir, color) self.size = np.zeros(3) def parse(self, xml): """Parse box size.""" if "size" in xml.attrib: self.size[:] = np.fromstring(xml.attrib["size"], sep=" ") def plot( self, tm, frame, ax=None, alpha=0.3, wireframe=True, convex_hull=True ): # pragma: no cover """Plot box.""" A2B = tm.get_transform(self.frame, frame) color = self.color if self.color is not None else "k" from .plot_utils import plot_box return plot_box( ax, self.size, A2B, wireframe=wireframe, alpha=alpha, color=color ) class Sphere(Geometry): """Geometrical object: sphere.""" def __init__(self, frame, mesh_path, package_dir, color): super(Sphere, self).__init__(frame, mesh_path, package_dir, color) self.radius = 0.0 def parse(self, xml): """Parse sphere radius.""" if "radius" not in xml.attrib: raise UrdfException("Sphere has no radius.") self.radius = float(xml.attrib["radius"]) def plot( self, tm, frame, ax=None, alpha=0.3, wireframe=True, convex_hull=True ): # pragma: no cover """Plot sphere.""" center = tm.get_transform(self.frame, frame)[:3, 3] color = self.color if self.color is not None else "k" from .plot_utils import plot_sphere return plot_sphere( ax, self.radius, center, wireframe=wireframe, alpha=alpha, color=color, ) class Cylinder(Geometry): """Geometrical object: cylinder.""" def __init__(self, frame, mesh_path, package_dir, color): super(Cylinder, self).__init__(frame, mesh_path, package_dir, color) self.radius = 0.0 self.length = 0.0 def parse(self, xml): """Parse cylinder radius and length.""" if "radius" not in xml.attrib: raise UrdfException("Cylinder has no radius.") self.radius = float(xml.attrib["radius"]) if "length" not in xml.attrib: raise UrdfException("Cylinder has no length.") self.length = float(xml.attrib["length"]) def plot( self, tm, frame, ax=None, alpha=0.3, wireframe=True, convex_hull=True ): # pragma: no cover """Plot cylinder.""" A2B = tm.get_transform(self.frame, frame) color = self.color if self.color is not None else "k" from .plot_utils import plot_cylinder return plot_cylinder( ax, self.length, self.radius, 0.0, A2B, wireframe=wireframe, alpha=alpha, color=color, ) class Mesh(Geometry): """Geometrical object: mesh.""" def __init__(self, frame, mesh_path, package_dir, color): super(Mesh, self).__init__(frame, mesh_path, package_dir, color) self.filename = None self.scale = np.ones(3) def parse(self, xml): """Parse mesh filename and scale.""" if self.mesh_path is None and self.package_dir is None: self.filename = None else: if "filename" not in xml.attrib: raise UrdfException("Mesh has no filename.") if self.mesh_path is not None: self.filename = os.path.join( self.mesh_path, xml.attrib["filename"] ) else: assert self.package_dir is not None self.filename = xml.attrib["filename"].replace( "package://", self.package_dir ) if "scale" in xml.attrib: self.scale = np.fromstring(xml.attrib["scale"], sep=" ") def plot( self, tm, frame, ax=None, alpha=0.3, wireframe=True, convex_hull=True ): # pragma: no cover """Plot mesh.""" from .plot_utils import plot_mesh A2B = tm.get_transform(self.frame, frame) color = self.color if self.color is not None else "k" return plot_mesh( ax, self.filename, A2B, self.scale, wireframe=wireframe, convex_hull=convex_hull, alpha=alpha, color=color, ) shape_classes = { "box": Box, "sphere": Sphere, "cylinder": Cylinder, "mesh": Mesh, } class UrdfException(Exception): """Exception while parsing URDF files.""" ================================================ FILE: pytransform3d/urdf.pyi ================================================ from typing import Dict, Tuple, List, Union, Type, Hashable import numpy as np import numpy.typing as npt from lxml.etree import _Element as Element from mpl_toolkits.mplot3d import Axes3D from .transform_manager import TransformManager class UrdfTransformManager(TransformManager): _joints: Dict[ str, Tuple[ Hashable, Hashable, np.ndarray, np.ndarray, Tuple[float, float], str ], ] collision_objects: List[Geometry] visuals: List[Geometry] def __init__(self, strict_check: bool = ..., check: bool = ...): ... def add_joint( self, joint_name: str, from_frame: Hashable, to_frame: Hashable, child2parent: npt.ArrayLike, axis: npt.ArrayLike, limits: Tuple[float, float] = ..., joint_type: str = ..., ): ... def set_joint(self, joint_name: str, value: float): ... def get_joint_limits(self, joint_name: str) -> Tuple[float, float]: ... def load_urdf( self, urdf_xml: str, mesh_path: Union[None, str] = ..., package_dir: Union[None, str] = ..., ): ... def plot_visuals( self, frame: Hashable, ax: Union[None, Axes3D] = ..., ax_s: float = ..., wireframe: bool = ..., convex_hull_of_mesh: bool = ..., alpha: float = ..., ): ... def plot_collision_objects( self, frame: Hashable, ax: Union[None, Axes3D] = ..., ax_s: float = ..., wireframe: bool = ..., convex_hull_of_mesh: bool = ..., alpha: float = ..., ): ... def _plot_objects( self, objects, frame: Hashable, ax: Union[None, Axes3D] = ..., ax_s: float = ..., wireframe: bool = ..., convex_hull_of_mesh: bool = ..., alpha: float = ..., ): ... class Link(object): name: Union[None, str] visuals: List[Geometry] collision_objects: List[Geometry] transforms: List[Tuple[str, str, np.ndarray]] inertial_frame: np.ndarray mass: float inertia: np.ndarray class Joint(object): child: Union[None, str] parent: Union[None, str] child2parent: np.ndarray joint_name: Union[None, str] joint_axis: Union[None, np.ndarray] joint_type: str limits: Tuple[float, float] class Geometry(object): frame: str mesh_path: Union[None, str] package_dir: Union[None, str] color: npt.ArrayLike def __init__( self, frame: str, mesh_path: Union[None, str], package_dir: Union[None, str], color: str, ): ... def parse(self, xml: Element): ... def plot( self, tm: UrdfTransformManager, frame: Hashable, ax: Union[None, Axes3D] = ..., alpha: float = ..., wireframe: bool = ..., convex_hull: bool = ..., ): ... class Box(Geometry): size: np.ndarray class Sphere(Geometry): radius: float class Cylinder(Geometry): radius: float length: float class Mesh(Geometry): filename: Union[None, str] scale: np.ndarray class UrdfException(Exception): ... def parse_urdf( urdf_xml: str, mesh_path: Union[str, None] = ..., package_dir: Union[str, None] = ..., strict_check: bool = ..., ) -> Tuple[str, List[Link], List[Joint]]: ... def initialize_urdf_transform_manager( tm: UrdfTransformManager, robot_name: str, links: List[Link], joints: List[Joint], ): ... shape_classes: Dict[str, Union[Type[Geometry]]] ================================================ FILE: pytransform3d/visualizer/__init__.py ================================================ """Optional 3D renderer based on Open3D's visualizer.""" import warnings try: import open3d as o3d # noqa: F401 from ._artists import ( Artist, Line3D, PointCollection3D, Vector3D, Frame, Trajectory, Camera, Box, Sphere, Cylinder, Mesh, Ellipsoid, Capsule, Cone, Plane, Graph, ) from ._figure import figure, Figure __all__ = [ "figure", "Figure", "Artist", "Line3D", "PointCollection3D", "Vector3D", "Frame", "Trajectory", "Camera", "Box", "Sphere", "Cylinder", "Mesh", "Ellipsoid", "Capsule", "Cone", "Plane", "Graph", ] except ImportError: warnings.warn( "3D visualizer is not available. Install open3d.", ImportWarning, stacklevel=2, ) ================================================ FILE: pytransform3d/visualizer/_artists.py ================================================ """Visualizer artists.""" import warnings from itertools import chain import numpy as np import open3d as o3d from .. import rotations as pr from .. import transformations as pt from .. import urdf from .._mesh_loader import load_mesh class Artist: """Abstract base class for objects that can be rendered.""" def add_artist(self, figure): """Add artist to figure. Parameters ---------- figure : Figure Figure to which the artist will be added. """ for g in self.geometries: figure.add_geometry(g) @property def geometries(self): """Expose geometries. Returns ------- geometries : list List of geometries that can be added to the visualizer. """ return [] class Line3D(Artist): """A line. Parameters ---------- P : array-like, shape (n_points, 3) Points of which the line consists. c : array-like, shape (n_points - 1, 3) or (3,), optional (default: black) Color can be given as individual colors per line segment or as one color for each segment. A color is represented by 3 values between 0 and 1 indicate representing red, green, and blue respectively. """ def __init__(self, P, c=(0, 0, 0)): self.line_set = o3d.geometry.LineSet() self.set_data(P, c) def set_data(self, P, c=None): """Update data. Parameters ---------- P : array-like, shape (n_points, 3) Points of which the line consists. c : array-like, shape (n_points - 1, 3) or (3,), optional (default: black) Color can be given as individual colors per line segment or as one color for each segment. A color is represented by 3 values between 0 and 1 indicate representing red, green, and blue respectively. """ self.line_set.points = o3d.utility.Vector3dVector(P) self.line_set.lines = o3d.utility.Vector2iVector( np.hstack( ( np.arange(len(P) - 1)[:, np.newaxis], np.arange(1, len(P))[:, np.newaxis], ) ) ) if c is not None: try: if len(c[0]) == 3: self.line_set.colors = o3d.utility.Vector3dVector(c) except TypeError: # one color for all segments self.line_set.colors = o3d.utility.Vector3dVector( [c for _ in range(len(P) - 1)] ) @property def geometries(self): """Expose geometries. Returns ------- geometries : list List of geometries that can be added to the visualizer. """ return [self.line_set] class PointCollection3D(Artist): """Collection of points. Parameters ---------- P : array, shape (n_points, 3) Points s : float, optional (default: 0.05) Scaling of the spheres that will be drawn. c : array-like, shape (3,) or (n_points, 3), optional (default: black) A color is represented by 3 values between 0 and 1 indicate representing red, green, and blue respectively. """ def __init__(self, P, s=0.05, c=None): self._points = [] self._P = np.zeros_like(P) if c is not None: c = np.asarray(c) if c.ndim == 1: c = np.tile(c, (len(P), 1)) for i in range(len(P)): point = o3d.geometry.TriangleMesh.create_sphere( radius=s, resolution=10 ) if c is not None: n_vertices = len(point.vertices) colors = np.zeros((n_vertices, 3)) colors[:] = c[i] point.vertex_colors = o3d.utility.Vector3dVector(colors) point.compute_vertex_normals() self._points.append(point) self.set_data(P) def set_data(self, P): """Update data. Parameters ---------- P : array, shape (n_points, 3) Points """ P = np.copy(P) for i, point, p, previous_p in zip( range(len(self._P)), self._points, P, self._P ): if any(np.isnan(p)): P[i] = previous_p else: point.translate(p - previous_p) self._P = P @property def geometries(self): """Expose geometries. Returns ------- geometries : list List of geometries that can be added to the visualizer. """ return self._points class Vector3D(Artist): """A vector. Parameters ---------- start : array-like, shape (3,), optional (default: [0, 0, 0]) Start of the vector direction : array-like, shape (3,), optional (default: [1, 0, 0]) Direction of the vector c : array-like, shape (3,), optional (default: black) A color is represented by 3 values between 0 and 1 indicate representing red, green, and blue respectively. """ def __init__( self, start=np.zeros(3), direction=np.array([1, 0, 0]), c=(0, 0, 0) ): self.vector = o3d.geometry.TriangleMesh.create_arrow( cylinder_radius=0.035, cone_radius=0.07, cylinder_height=0.8, cone_height=0.2, ) self.set_data(start, direction, c) def set_data(self, start, direction, c=None): """Update data. Parameters ---------- start : array-like, shape (3,) Start of the vector direction : array-like, shape (3,) Direction of the vector c : array-like, shape (3,), optional (default: black) A color is represented by 3 values between 0 and 1 indicate representing red, green, and blue respectively. """ length = np.linalg.norm(direction) z = direction / length x, y = pr.plane_basis_from_normal(z) R = np.column_stack((x, y, z)) A2B = pt.transform_from(R, start) new_vector = o3d.geometry.TriangleMesh.create_arrow( cylinder_radius=0.035 * length, cone_radius=0.07 * length, cylinder_height=0.8 * length, cone_height=0.2 * length, ) self.vector.vertices = new_vector.vertices self.vector.triangles = new_vector.triangles self.vector.transform(A2B) if c is not None: self.vector.paint_uniform_color(c) self.vector.compute_vertex_normals() @property def geometries(self): """Expose geometries. Returns ------- geometries : list List of geometries that can be added to the visualizer. """ return [self.vector] class Frame(Artist): """Coordinate frame. Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B label : str, optional (default: None) Name of the frame s : float, optional (default: 1) Length of basis vectors """ def __init__(self, A2B, label=None, s=1.0): self.A2B = None self.label = None self.s = s self.frame = o3d.geometry.TriangleMesh.create_coordinate_frame( size=self.s ) self.set_data(A2B, label) def set_data(self, A2B, label=None): """Update data. Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B label : str, optional (default: None) Name of the frame """ previous_A2B = self.A2B if previous_A2B is None: previous_A2B = np.eye(4) self.A2B = A2B self.label = label if label is not None: warnings.warn( "This viewer does not support text. Frame label " "will be ignored.", UserWarning, stacklevel=2, ) self.frame.transform(pt.invert_transform(previous_A2B, check=False)) self.frame.transform(self.A2B) @property def geometries(self): """Expose geometries. Returns ------- geometries : list List of geometries that can be added to the visualizer. """ return [self.frame] class Trajectory(Artist): """Trajectory of poses. Parameters ---------- H : array-like, shape (n_steps, 4, 4) Sequence of poses represented by homogeneous matrices n_frames : int, optional (default: 10) Number of frames that should be plotted to indicate the rotation s : float, optional (default: 1) Scaling of the frames that will be drawn c : array-like, shape (3,), optional (default: black) A color is represented by 3 values between 0 and 1 indicate representing red, green, and blue respectively. """ def __init__(self, H, n_frames=10, s=1.0, c=(0, 0, 0)): self.H = H self.n_frames = n_frames self.s = s self.c = c self.key_frames = [] self.line = Line3D(H[:, :3, 3], c) self.key_frames_indices = np.linspace( 0, len(self.H) - 1, self.n_frames, dtype=np.int64 ) for key_frame_idx in self.key_frames_indices: self.key_frames.append(Frame(self.H[key_frame_idx], s=self.s)) self.set_data(H) def set_data(self, H): """Update data. Parameters ---------- H : array-like, shape (n_steps, 4, 4) Sequence of poses represented by homogeneous matrices """ self.line.set_data(H[:, :3, 3]) for i, key_frame_idx in enumerate(self.key_frames_indices): self.key_frames[i].set_data(H[key_frame_idx]) @property def geometries(self): """Expose geometries. Returns ------- geometries : list List of geometries that can be added to the visualizer. """ return self.line.geometries + list( chain(*[kf.geometries for kf in self.key_frames]) ) class Sphere(Artist): """Sphere. Parameters ---------- radius : float, optional (default: 1) Radius of the sphere A2B : array-like, shape (4, 4) Center of the sphere resolution : int, optional (default: 20) The resolution of the sphere. The longitudes will be split into resolution segments (i.e. there are resolution + 1 latitude lines including the north and south pole). The latitudes will be split into 2 * resolution segments (i.e. there are 2 * resolution longitude lines.) c : array-like, shape (3,), optional (default: None) Color """ def __init__(self, radius=1.0, A2B=np.eye(4), resolution=20, c=None): self.sphere = o3d.geometry.TriangleMesh.create_sphere( radius, resolution ) if c is not None: n_vertices = len(self.sphere.vertices) colors = np.zeros((n_vertices, 3)) colors[:] = c self.sphere.vertex_colors = o3d.utility.Vector3dVector(colors) self.sphere.compute_vertex_normals() self.A2B = None self.set_data(A2B) def set_data(self, A2B): """Update data. Parameters ---------- A2B : array-like, shape (4, 4) Center of the sphere. """ previous_A2B = self.A2B if previous_A2B is None: previous_A2B = np.eye(4) self.A2B = A2B self.sphere.transform(pt.invert_transform(previous_A2B, check=False)) self.sphere.transform(self.A2B) @property def geometries(self): """Expose geometries. Returns ------- geometries : list List of geometries that can be added to the visualizer. """ return [self.sphere] class Box(Artist): """Box. Parameters ---------- size : array-like, shape (3,), optional (default: [1, 1, 1]) Size of the box per dimension A2B : array-like, shape (4, 4), optional (default: I) Center of the box c : array-like, shape (3,), optional (default: None) Color """ def __init__(self, size=np.ones(3), A2B=np.eye(4), c=None): self.half_size = np.asarray(size) / 2.0 width, height, depth = size self.box = o3d.geometry.TriangleMesh.create_box(width, height, depth) if c is not None: n_vertices = len(self.box.vertices) colors = np.zeros((n_vertices, 3)) colors[:] = c self.box.vertex_colors = o3d.utility.Vector3dVector(colors) self.box.compute_vertex_normals() self.A2B = None self.set_data(A2B) def set_data(self, A2B): """Update data. Parameters ---------- A2B : array-like, shape (4, 4) Center of the box. """ previous_A2B = self.A2B if previous_A2B is None: self.box.transform( pt.transform_from(R=np.eye(3), p=-self.half_size) ) previous_A2B = np.eye(4) self.A2B = A2B self.box.transform(pt.invert_transform(previous_A2B, check=False)) self.box.transform(self.A2B) @property def geometries(self): """Expose geometries. Returns ------- geometries : list List of geometries that can be added to the visualizer. """ return [self.box] class Cylinder(Artist): """Cylinder. A cylinder is the volume covered by a disk moving along a line segment. Parameters ---------- length : float, optional (default: 1) Length of the cylinder. radius : float, optional (default: 1) Radius of the cylinder. A2B : array-like, shape (4, 4) Pose of the cylinder. The position corresponds to the center of the line segment and the z-axis to the direction of the line segment. resolution : int, optional (default: 20) The circles will be split into resolution segments. split : int, optional (default: 4) The height will be split into split segments c : array-like, shape (3,), optional (default: None) Color """ def __init__( self, length=2.0, radius=1.0, A2B=np.eye(4), resolution=20, split=4, c=None, ): self.cylinder = o3d.geometry.TriangleMesh.create_cylinder( radius=radius, height=length, resolution=resolution, split=split ) if c is not None: n_vertices = len(self.cylinder.vertices) colors = np.zeros((n_vertices, 3)) colors[:] = c self.cylinder.vertex_colors = o3d.utility.Vector3dVector(colors) self.cylinder.compute_vertex_normals() self.A2B = None self.set_data(A2B) def set_data(self, A2B): """Update data. Parameters ---------- A2B : array-like, shape (4, 4) Center of the cylinder. """ previous_A2B = self.A2B if previous_A2B is None: previous_A2B = np.eye(4) self.A2B = A2B self.cylinder.transform(pt.invert_transform(previous_A2B, check=False)) self.cylinder.transform(self.A2B) @property def geometries(self): """Expose geometries. Returns ------- geometries : list List of geometries that can be added to the visualizer. """ return [self.cylinder] class Mesh(Artist): """Mesh. Parameters ---------- filename : str Path to mesh file A2B : array-like, shape (4, 4) Center of the mesh s : array-like, shape (3,), optional (default: [1, 1, 1]) Scaling of the mesh that will be drawn c : array-like, shape (n_vertices, 3) or (3,), optional (default: None) Color(s) convex_hull : bool, optional (default: False) Compute convex hull of mesh. """ def __init__( self, filename, A2B=np.eye(4), s=np.ones(3), c=None, convex_hull=False ): mesh = load_mesh(filename) if convex_hull: mesh.convex_hull() self.mesh = mesh.get_open3d_mesh() self.mesh.vertices = o3d.utility.Vector3dVector( np.asarray(self.mesh.vertices) * s ) self.mesh.compute_vertex_normals() if c is not None: n_vertices = len(self.mesh.vertices) colors = np.zeros((n_vertices, 3)) colors[:] = c self.mesh.vertex_colors = o3d.utility.Vector3dVector(colors) self.A2B = None self.set_data(A2B) def set_data(self, A2B): """Update data. Parameters ---------- A2B : array-like, shape (4, 4) Center of the mesh. """ previous_A2B = self.A2B if previous_A2B is None: previous_A2B = np.eye(4) self.A2B = A2B self.mesh.transform(pt.invert_transform(previous_A2B, check=False)) self.mesh.transform(self.A2B) @property def geometries(self): """Expose geometries. Returns ------- geometries : list List of geometries that can be added to the visualizer. """ return [self.mesh] class Ellipsoid(Artist): """Ellipsoid. Parameters ---------- radii : array-like, shape (3,) Radii along the x-axis, y-axis, and z-axis of the ellipsoid. A2B : array-like, shape (4, 4) Pose of the ellipsoid. resolution : int, optional (default: 20) The resolution of the ellipsoid. The longitudes will be split into resolution segments (i.e. there are resolution + 1 latitude lines including the north and south pole). The latitudes will be split into 2 * resolution segments (i.e. there are 2 * resolution longitude lines.) c : array-like, shape (3,), optional (default: None) Color """ def __init__(self, radii, A2B=np.eye(4), resolution=20, c=None): self.ellipsoid = o3d.geometry.TriangleMesh.create_sphere( 1.0, resolution ) vertices = np.asarray(self.ellipsoid.vertices) vertices *= np.asarray(radii)[np.newaxis] self.ellipsoid.vertices = o3d.utility.Vector3dVector(vertices) self.ellipsoid.compute_vertex_normals() if c is not None: n_vertices = len(self.ellipsoid.vertices) colors = np.zeros((n_vertices, 3)) colors[:] = c self.ellipsoid.vertex_colors = o3d.utility.Vector3dVector(colors) self.A2B = None self.set_data(A2B) def set_data(self, A2B): """Update data. Parameters ---------- A2B : array-like, shape (4, 4) Center of the ellipsoid. """ previous_A2B = self.A2B if previous_A2B is None: previous_A2B = np.eye(4) self.A2B = A2B self.ellipsoid.transform(pt.invert_transform(previous_A2B, check=False)) self.ellipsoid.transform(self.A2B) @property def geometries(self): """Expose geometries. Returns ------- geometries : list List of geometries that can be added to the visualizer. """ return [self.ellipsoid] class Capsule(Artist): """Capsule. A capsule is the volume covered by a sphere moving along a line segment. Parameters ---------- height : float, optional (default: 1) Height of the capsule along its z-axis. radius : float, optional (default: 1) Radius of the capsule. A2B : array-like, shape (4, 4) Pose of the capsule. The position corresponds to the center of the line segment and the z-axis to the direction of the line segment. resolution : int, optional (default: 20) The resolution of the half spheres. The longitudes will be split into resolution segments (i.e. there are resolution + 1 latitude lines including the north and south pole). The latitudes will be split into 2 * resolution segments (i.e. there are 2 * resolution longitude lines.) c : array-like, shape (3,), optional (default: None) Color """ def __init__( self, height=1, radius=1, A2B=np.eye(4), resolution=20, c=None ): self.capsule = o3d.geometry.TriangleMesh.create_sphere( radius, resolution ) vertices = np.asarray(self.capsule.vertices) vertices[vertices[:, 2] < 0, 2] -= 0.5 * height vertices[vertices[:, 2] > 0, 2] += 0.5 * height self.capsule.vertices = o3d.utility.Vector3dVector(vertices) self.capsule.compute_vertex_normals() if c is not None: n_vertices = len(self.capsule.vertices) colors = np.zeros((n_vertices, 3)) colors[:] = c self.capsule.vertex_colors = o3d.utility.Vector3dVector(colors) self.A2B = None self.set_data(A2B) def set_data(self, A2B): """Update data. Parameters ---------- A2B : array-like, shape (4, 4) Start of the capsule's line segment. """ previous_A2B = self.A2B if previous_A2B is None: previous_A2B = np.eye(4) self.A2B = A2B self.capsule.transform(pt.invert_transform(previous_A2B, check=False)) self.capsule.transform(self.A2B) @property def geometries(self): """Expose geometries. Returns ------- geometries : list List of geometries that can be added to the visualizer. """ return [self.capsule] class Cone(Artist): """Cone. Parameters ---------- height : float, optional (default: 1) Height of the cone along its z-axis. radius : float, optional (default: 1) Radius of the cone. A2B : array-like, shape (4, 4) Pose of the cone, which is the center of its circle. resolution : int, optional (default: 20) The circle will be split into resolution segments. c : array-like, shape (3,), optional (default: None) Color """ def __init__( self, height=1, radius=1, A2B=np.eye(4), resolution=20, c=None ): self.cone = o3d.geometry.TriangleMesh.create_cone( radius, height, resolution ) self.cone.compute_vertex_normals() if c is not None: n_vertices = len(self.cone.vertices) colors = np.zeros((n_vertices, 3)) colors[:] = c self.cone.vertex_colors = o3d.utility.Vector3dVector(colors) self.A2B = None self.set_data(A2B) def set_data(self, A2B): """Update data. Parameters ---------- A2B : array-like, shape (4, 4) Center of the cone's circle. """ previous_A2B = self.A2B if previous_A2B is None: previous_A2B = np.eye(4) self.A2B = A2B self.cone.transform(pt.invert_transform(previous_A2B, check=False)) self.cone.transform(self.A2B) @property def geometries(self): """Expose geometries. Returns ------- geometries : list List of geometries that can be added to the visualizer. """ return [self.cone] class Plane(Artist): """Plane. The plane will be defined either by a normal and a point in the plane or by the Hesse normal form, which only needs a normal and the distance to the origin from which we can compute the point in the plane as d * normal. A plane will be visualized by a square. Parameters ---------- normal : array-like, shape (3,), optional (default: [0, 0, 1]) Plane normal. d : float, optional (default: None) Distance to origin in Hesse normal form. point_in_plane : array-like, shape (3,), optional (default: None) Point in plane. s : float, optional (default: 1) Scaling of the plane that will be drawn. c : array-like, shape (3,), optional (default: None) Color. """ def __init__( self, normal=np.array([0.0, 0.0, 1.0]), d=None, point_in_plane=None, s=1.0, c=None, ): self.triangles = np.array( [ [0, 1, 2], [1, 3, 2], [2, 1, 0], [2, 3, 1], ] ) vertices = np.zeros((4, 3)) self.plane = o3d.geometry.TriangleMesh( o3d.utility.Vector3dVector(vertices), o3d.utility.Vector3iVector(self.triangles), ) self.set_data(normal, d, point_in_plane, s, c) def set_data(self, normal, d=None, point_in_plane=None, s=None, c=None): """Update data. Parameters ---------- normal : array-like, shape (3,) Plane normal. d : float, optional (default: None) Distance to origin in Hesse normal form. point_in_plane : array-like, shape (3,), optional (default: None) Point in plane. s : float, optional (default: 1) Scaling of the plane that will be drawn. c : array-like, shape (3,), optional (default: None) Color. Raises ------ ValueError If neither 'd' nor 'point_in_plane' is defined. """ normal = np.asarray(normal) if point_in_plane is None: if d is None: raise ValueError( "Either 'd' or 'point_in_plane' has to be defined!" ) point_in_plane = d * normal else: point_in_plane = np.asarray(point_in_plane) x_axis, y_axis = pr.plane_basis_from_normal(normal) vertices = np.array( [ point_in_plane + s * x_axis + s * y_axis, point_in_plane - s * x_axis + s * y_axis, point_in_plane + s * x_axis - s * y_axis, point_in_plane - s * x_axis - s * y_axis, ] ) self.plane.vertices = o3d.utility.Vector3dVector(vertices) self.plane.compute_vertex_normals() if c is not None: self.plane.paint_uniform_color(c) @property def geometries(self): """Expose geometries. Returns ------- geometries : list List of geometries that can be added to the visualizer. """ return [self.plane] class Camera(Artist): """Camera. Parameters ---------- M : array-like, shape (3, 3) Intrinsic camera matrix that contains the focal lengths on the diagonal and the center of the the image in the last column. It does not matter whether values are given in meters or pixels as long as the unit is the same as for the sensor size. cam2world : array-like, shape (4, 4), optional (default: I) Transformation matrix of camera in world frame. We assume that the position is given in meters. virtual_image_distance : float, optional (default: 1) Distance from pinhole to virtual image plane that will be displayed. We assume that this distance is given in meters. The unit has to be consistent with the unit of the position in cam2world. sensor_size : array-like, shape (2,), optional (default: [1920, 1080]) Size of the image sensor: (width, height). It does not matter whether values are given in meters or pixels as long as the unit is the same as for the sensor size. strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. """ def __init__( self, M, cam2world=None, virtual_image_distance=1, sensor_size=(1920, 1080), strict_check=True, ): self.M = None self.cam2world = None self.virtual_image_distance = None self.sensor_size = None self.strict_check = strict_check self.line_set = o3d.geometry.LineSet() if cam2world is None: cam2world = np.eye(4) self.set_data(M, cam2world, virtual_image_distance, sensor_size) def set_data( self, M=None, cam2world=None, virtual_image_distance=None, sensor_size=None, ): """Update camera parameters. Parameters ---------- M : array-like, shape (3, 3), optional (default: old value) Intrinsic camera matrix that contains the focal lengths on the diagonal and the center of the the image in the last column. It does not matter whether values are given in meters or pixels as long as the unit is the same as for the sensor size. cam2world : array-like, shape (4, 4), optional (default: old value) Transformation matrix of camera in world frame. We assume that the position is given in meters. virtual_image_distance : float, optional (default: old value) Distance from pinhole to virtual image plane that will be displayed. We assume that this distance is given in meters. The unit has to be consistent with the unit of the position in cam2world. sensor_size : array-like, shape (2,), optional (default: old value) Size of the image sensor: (width, height). It does not matter whether values are given in meters or pixels as long as the unit is the same as for the sensor size. """ if M is not None: self.M = M if cam2world is not None: self.cam2world = pt.check_transform( cam2world, strict_check=self.strict_check ) if virtual_image_distance is not None: self.virtual_image_distance = virtual_image_distance if sensor_size is not None: self.sensor_size = sensor_size camera_center_in_world = cam2world[:3, 3] focal_length = np.mean(np.diag(M[:2, :2])) sensor_corners_in_cam = np.array( [ [0, 0, focal_length], [0, sensor_size[1], focal_length], [sensor_size[0], sensor_size[1], focal_length], [sensor_size[0], 0, focal_length], ] ) sensor_corners_in_cam[:, 0] -= M[0, 2] sensor_corners_in_cam[:, 1] -= M[1, 2] sensor_corners_in_world = pt.transform( cam2world, pt.vectors_to_points(sensor_corners_in_cam) )[:, :3] virtual_image_corners = ( virtual_image_distance / focal_length * (sensor_corners_in_world - camera_center_in_world[np.newaxis]) + camera_center_in_world[np.newaxis] ) up = virtual_image_corners[0] - virtual_image_corners[1] camera_line_points = np.vstack( ( camera_center_in_world, virtual_image_corners[0], virtual_image_corners[1], virtual_image_corners[2], virtual_image_corners[3], virtual_image_corners[0] + 0.1 * up, 0.5 * (virtual_image_corners[0] + virtual_image_corners[3]) + 0.5 * up, virtual_image_corners[3] + 0.1 * up, ) ) self.line_set.points = o3d.utility.Vector3dVector(camera_line_points) self.line_set.lines = o3d.utility.Vector2iVector( np.array( [ [0, 1], [0, 2], [0, 3], [0, 4], [1, 2], [2, 3], [3, 4], [4, 1], [5, 6], [6, 7], [7, 5], ] ) ) @property def geometries(self): """Expose geometries. Returns ------- geometries : list List of geometries that can be added to the visualizer. """ return [self.line_set] class Graph(Artist): """Graph of connected frames. Parameters ---------- tm : TransformManager Representation of the graph frame : str Name of the base frame in which the graph will be displayed show_frames : bool, optional (default: False) Show coordinate frames show_connections : bool, optional (default: False) Draw lines between frames of the graph show_visuals : bool, optional (default: False) Show visuals that are stored in the graph show_collision_objects : bool, optional (default: False) Show collision objects that are stored in the graph show_name : bool, optional (default: False) Show names of frames whitelist : list, optional (default: all) List of frames that should be displayed convex_hull_of_collision_objects : bool, optional (default: False) Show convex hull of collision objects. s : float, optional (default: 1) Scaling of the frames that will be drawn """ def __init__( self, tm, frame, show_frames=False, show_connections=False, show_visuals=False, show_collision_objects=False, show_name=False, whitelist=None, convex_hull_of_collision_objects=False, s=1.0, ): self.tm = tm self.frame = frame self.show_frames = show_frames self.show_connections = show_connections self.show_visuals = show_visuals self.show_collision_objects = show_collision_objects self.whitelist = whitelist self.convex_hull_of_collision_objects = convex_hull_of_collision_objects self.s = s if self.frame not in self.tm.nodes: raise KeyError("Unknown frame '%s'" % self.frame) self.nodes = list(sorted(self.tm._whitelisted_nodes(whitelist))) self.frames = {} if self.show_frames: for node in self.nodes: try: node2frame = self.tm.get_transform(node, frame) name = node if show_name else None self.frames[node] = Frame(node2frame, name, self.s) except KeyError: pass # Frame is not connected to the reference frame self.connections = {} if self.show_connections: for frame_names in self.tm.transforms.keys(): from_frame, to_frame = frame_names if from_frame in self.tm.nodes and to_frame in self.tm.nodes: try: self.tm.get_transform(from_frame, self.frame) self.tm.get_transform(to_frame, self.frame) self.connections[frame_names] = o3d.geometry.LineSet() except KeyError: pass # Frame is not connected to reference frame self.visuals = {} if show_visuals and hasattr(self.tm, "visuals"): self.visuals.update(_objects_to_artists(self.tm.visuals)) self.collision_objects = {} if show_collision_objects and hasattr(self.tm, "collision_objects"): self.collision_objects.update( _objects_to_artists( self.tm.collision_objects, convex_hull_of_collision_objects ) ) self.set_data() def set_data(self): """Indicate that data has been updated.""" if self.show_frames: for node in self.nodes: try: node2frame = self.tm.get_transform(node, self.frame) self.frames[node].set_data(node2frame) except KeyError: pass # Frame is not connected to the reference frame if self.show_connections: for frame_names in self.connections: from_frame, to_frame = frame_names try: from2ref = self.tm.get_transform(from_frame, self.frame) to2ref = self.tm.get_transform(to_frame, self.frame) points = np.vstack((from2ref[:3, 3], to2ref[:3, 3])) self.connections[frame_names].points = ( o3d.utility.Vector3dVector(points) ) self.connections[frame_names].lines = ( o3d.utility.Vector2iVector(np.array([[0, 1]])) ) except KeyError: pass # Frame is not connected to the reference frame for frame, obj in self.visuals.items(): A2B = self.tm.get_transform(frame, self.frame) obj.set_data(A2B) for frame, obj in self.collision_objects.items(): A2B = self.tm.get_transform(frame, self.frame) obj.set_data(A2B) @property def geometries(self): """Expose geometries. Returns ------- geometries : list List of geometries that can be added to the visualizer. """ geometries = [] if self.show_frames: for f in self.frames.values(): geometries += f.geometries if self.show_connections: geometries += list(self.connections.values()) for obj in self.visuals.values(): geometries += obj.geometries for obj in self.collision_objects.values(): geometries += obj.geometries return geometries def _objects_to_artists(objects, convex_hull=False): """Convert geometries from URDF to artists. Parameters ---------- objects : list of Geometry Objects parsed from URDF. convex_hull : bool, optional (default: False) Compute convex hull for each object. Returns ------- artists : dict Mapping from frame names to artists. """ artists = {} for obj in objects: if obj.color is None: color = None else: # we loose the alpha channel as it is not supported by Open3D color = (obj.color[0], obj.color[1], obj.color[2]) try: if isinstance(obj, urdf.Sphere): artist = Sphere(radius=obj.radius, c=color) elif isinstance(obj, urdf.Box): artist = Box(obj.size, c=color) elif isinstance(obj, urdf.Cylinder): artist = Cylinder(obj.length, obj.radius, c=color) else: assert isinstance(obj, urdf.Mesh) artist = Mesh( obj.filename, s=obj.scale, c=color, convex_hull=convex_hull ) artists[obj.frame] = artist except RuntimeError as e: warnings.warn(str(e), RuntimeWarning, stacklevel=1) return artists ================================================ FILE: pytransform3d/visualizer/_artists.pyi ================================================ import typing from typing import List, Union, Any, Dict import numpy.typing as npt import open3d as o3d from ..transform_manager import TransformManager if typing.TYPE_CHECKING: from ._figure import Figure class Artist: def add_artist(self, figure: Figure): ... @property def geometries(self) -> List[o3d.geometry.Geometry3D]: ... class Line3D(Artist): def __init__(self, P: npt.ArrayLike, c: npt.ArrayLike = ...): ... def set_data( self, P: npt.ArrayLike, c: Union[None, npt.ArrayLike] = ... ): ... @property def geometries(self) -> List[o3d.geometry.Geometry3D]: ... class PointCollection3D(Artist): def __init__( self, P: npt.ArrayLike, s: float = ..., c: Union[None, npt.ArrayLike] = ..., ): ... def set_data(self, P: npt.ArrayLike): ... @property def geometries(self) -> List[o3d.geometry.Geometry3D]: ... class Vector3D(Artist): def __init__( self, start: npt.ArrayLike, direction: npt.ArrayLike, c: Union[None, npt.ArrayLike] = ..., ): ... def set_data( self, start: npt.ArrayLike, direction: npt.ArrayLike, c: npt.ArrayLike = ..., ): ... @property def geometries(self) -> List[o3d.geometry.Geometry3D]: ... class Frame(Artist): def __init__( self, A2B: npt.ArrayLike, label: Union[None, str] = ..., s: float = ... ): ... def set_data(self, A2B: npt.ArrayLike, label: Union[None, str] = ...): ... @property def geometries(self) -> List[o3d.geometry.Geometry3D]: ... class Trajectory(Artist): def __init__( self, H: npt.ArrayLike, n_frames: int = ..., s: float = ..., c: npt.ArrayLike = ..., ): ... def set_data(self, H: npt.ArrayLike): ... @property def geometries(self) -> List[o3d.geometry.Geometry3D]: ... class Sphere(Artist): def __init__( self, radius: float = ..., A2B: npt.ArrayLike = ..., resolution: int = ..., c: Union[None, npt.ArrayLike] = ..., ): ... def set_data(self, A2B: npt.ArrayLike): ... @property def geometries(self) -> List[o3d.geometry.Geometry3D]: ... class Box(Artist): def __init__( self, size: npt.ArrayLike = ..., A2B: npt.ArrayLike = ..., c: Union[None, npt.ArrayLike] = ..., ): ... def set_data(self, A2B: npt.ArrayLike): ... @property def geometries(self) -> List[o3d.geometry.Geometry3D]: ... class Cylinder(Artist): def __init__( self, length: float = ..., radius: float = ..., A2B: npt.ArrayLike = ..., resolution: int = ..., split: int = ..., c: Union[None, npt.ArrayLike] = ..., ): ... def set_data(self, A2B: npt.ArrayLike): ... @property def geometries(self) -> List[o3d.geometry.Geometry3D]: ... class Mesh(Artist): def __init__( self, filename: str, A2B: npt.ArrayLike = ..., s: npt.ArrayLike = ..., c: Union[None, npt.ArrayLike] = ..., convex_hull: bool = ..., ): ... def set_data(self, A2B: npt.ArrayLike): ... @property def geometries(self) -> List[o3d.geometry.Geometry3D]: ... class Ellipsoid(Artist): def __init__( self, radii: npt.ArrayLike, A2B: npt.ArrayLike = ..., resolution: int = ..., c: Union[None, npt.ArrayLike] = ..., ): ... def set_data(self, A2B: npt.ArrayLike): ... @property def geometries(self) -> List[o3d.geometry.Geometry3D]: ... class Capsule(Artist): def __init__( self, height: float = ..., radius: float = ..., A2B: npt.ArrayLike = ..., resolution: int = ..., c: Union[None, npt.ArrayLike] = ..., ): ... def set_data(self, A2B: npt.ArrayLike): ... @property def geometries(self) -> List[o3d.geometry.Geometry3D]: ... class Cone(Artist): def __init__( self, height: float = ..., radius: float = ..., A2B: npt.ArrayLike = ..., resolution: int = ..., c: Union[None, npt.ArrayLike] = ..., ): ... def set_data(self, A2B: npt.ArrayLike): ... @property def geometries(self) -> List[o3d.geometry.Geometry3D]: ... class Plane(Artist): def __init__( self, normal: npt.ArrayLike = ..., d: Union[None, float] = ..., point_in_plane: Union[None, npt.ArrayLike] = ..., s: float = ..., c: Union[None, npt.ArrayLike] = ..., ): ... def set_data( self, normal: npt.ArrayLike, d: Union[None, float] = ..., point_in_plane: Union[None, npt.ArrayLike] = ..., s: float = ..., c: Union[None, npt.ArrayLike] = ..., ): ... @property def geometries(self) -> List[o3d.geometry.Geometry3D]: ... class Camera(Artist): def __init__( self, M: npt.ArrayLike, cam2world: Union[None, npt.ArrayLike] = ..., virtual_image_distance: float = ..., sensor_size: npt.ArrayLike = ..., strict_check: bool = ..., ): ... def set_data( self, M: Union[None, npt.ArrayLike], cam2world: Union[None, npt.ArrayLike], virtual_image_distance: Union[None, float], sensor_size: Union[None, npt.ArrayLike], ): ... @property def geometries(self) -> List[o3d.geometry.Geometry3D]: ... class Graph(Artist): def __init__( self, tm: TransformManager, frame: str, show_frames: bool = ..., show_connections: bool = ..., show_visuals: bool = ..., show_collision_objects: bool = ..., show_name: bool = ..., whitelist: Union[None, List[str]] = ..., convex_hull_of_collision_objects: bool = ..., s: float = ..., ): ... def set_data(self): ... @property def geometries(self) -> List[o3d.geometry.Geometry3D]: ... def _objects_to_artists(objects: List[Any]) -> Dict[str, Artist]: ... ================================================ FILE: pytransform3d/visualizer/_figure.py ================================================ """Figure based on Open3D's visualizer.""" import numpy as np import open3d as o3d from ._artists import ( Line3D, PointCollection3D, Vector3D, Frame, Trajectory, Camera, Box, Sphere, Cylinder, Mesh, Ellipsoid, Capsule, Cone, Plane, Graph, ) from .. import rotations as pr from .. import trajectories as ptr from .. import transformations as pt class Figure: """The top level container for all the plot elements. You can close the visualizer with the keys `escape` or `q`. Parameters ---------- window_name : str, optional (default: Open3D) Window title name. width : int, optional (default: 1920) Width of the window. height : int, optional (default: 1080) Height of the window. with_key_callbacks : bool, optional (default: False) Creates a visualizer that allows to register callbacks for keys. """ def __init__( self, window_name="Open3D", width=1920, height=1080, with_key_callbacks=False, ): if with_key_callbacks: self.visualizer = o3d.visualization.VisualizerWithKeyCallback() else: self.visualizer = o3d.visualization.Visualizer() self.visualizer.create_window( window_name=window_name, width=width, height=height ) def add_geometry(self, geometry): """Add geometry to visualizer. Parameters ---------- geometry : Geometry Open3D geometry. """ self.visualizer.add_geometry(geometry) def _remove_geometry(self, geometry): """Remove geometry to visualizer. .. warning:: This function is not public because the interface of the underlying visualizer might change in the future causing the signature of this function to change as well. Parameters ---------- geometry : Geometry Open3D geometry. """ self.visualizer.remove_geometry(geometry) def update_geometry(self, geometry): """Indicate that geometry has been updated. Parameters ---------- geometry : Geometry Open3D geometry. """ self.visualizer.update_geometry(geometry) def remove_artist(self, artist): """Remove artist from visualizer. Parameters ---------- artist : Artist Artist that should be removed from this figure. """ for g in artist.geometries: self._remove_geometry(g) def set_line_width(self, line_width): """Set render option line width. Note: this feature does not work in Open3D's visualizer at the moment. Parameters ---------- line_width : float Line width. """ self.visualizer.get_render_option().line_width = line_width self.visualizer.update_renderer() def set_zoom(self, zoom): """Set zoom. Parameters ---------- zoom : float Zoom of the visualizer. """ self.visualizer.get_view_control().set_zoom(zoom) def animate(self, callback, n_frames, loop=False, fargs=()): """Make animation with callback. Parameters ---------- callback : callable Callback that will be called in a loop to update geometries. The first input of the function will be the current frame index from [0, `n_frames`). Further arguments can be given as `fargs`. The function should return one artist object or a list of artists that have been updated. n_frames : int Total number of frames. loop : bool, optional (default: False) Run callback in an infinite loop. fargs : list, optional (default: []) Arguments that will be passed to the callback. Raises ------ RuntimeError When callback does not return any artists """ initialized = False window_open = True while window_open and (loop or not initialized): for i in range(n_frames): drawn_artists = callback(i, *fargs) if drawn_artists is None: raise RuntimeError( "The animation function must return a " "sequence of Artist objects." ) try: drawn_artists = [a for a in drawn_artists] except TypeError: drawn_artists = [drawn_artists] for a in drawn_artists: for geometry in a.geometries: self.update_geometry(geometry) window_open = self.visualizer.poll_events() if not window_open: break self.visualizer.update_renderer() initialized = True def view_init(self, azim=-60, elev=30): """Set the elevation and azimuth of the axes. Parameters ---------- azim : float, optional (default: -60) Azimuth angle in the x,y plane in degrees. elev : float, optional (default: 30) Elevation angle in the z plane. """ vc = self.visualizer.get_view_control() pcp = vc.convert_to_pinhole_camera_parameters() distance = np.linalg.norm(pcp.extrinsic[:3, 3]) R_azim_elev_0_world2camera = np.array( [[0, 1, 0], [0, 0, -1], [-1, 0, 0]] ) R_azim_elev_0_camera2world = R_azim_elev_0_world2camera.T # azimuth and elevation are defined in world frame R_azim = pr.active_matrix_from_angle(2, np.deg2rad(azim)) R_elev = pr.active_matrix_from_angle(1, np.deg2rad(-elev)) R_elev_azim_camera2world = R_azim.dot(R_elev).dot( R_azim_elev_0_camera2world ) pcp.extrinsic = pt.transform_from( # world2camera R=R_elev_azim_camera2world.T, p=[0, 0, distance] ) try: vc.convert_from_pinhole_camera_parameters(pcp, allow_arbitrary=True) except TypeError: vc.convert_from_pinhole_camera_parameters(pcp) def plot(self, P, c=(0, 0, 0)): """Plot line. Parameters ---------- P : array-like, shape (n_points, 3) Points of which the line consists. c : array-like, shape (n_points - 1, 3) or (3,), optional (default: black) Color can be given as individual colors per line segment or as one color for each segment. A color is represented by 3 values between 0 and 1 indicate representing red, green, and blue respectively. Returns ------- line : Line3D New line. """ line3d = Line3D(P, c) line3d.add_artist(self) return line3d def scatter(self, P, s=0.05, c=None): """Plot collection of points. Parameters ---------- P : array, shape (n_points, 3) Points s : float, optional (default: 0.05) Scaling of the spheres that will be drawn. c : array-like, shape (3,) or (n_points, 3), optional (default: black) A color is represented by 3 values between 0 and 1 indicate representing red, green, and blue respectively. Returns ------- point_collection : PointCollection3D New point collection. """ point_collection = PointCollection3D(P, s, c) point_collection.add_artist(self) return point_collection def plot_vector( self, start=np.zeros(3), direction=np.array([1, 0, 0]), c=(0, 0, 0) ): """Plot vector. Parameters ---------- start : array-like, shape (3,), optional (default: [0, 0, 0]) Start of the vector direction : array-like, shape (3,), optional (default: [1, 0, 0]) Direction of the vector c : array-like, shape (3,), optional (default: black) A color is represented by 3 values between 0 and 1 indicate representing red, green, and blue respectively. Returns ------- vector : Vector3D New vector. """ vector3d = Vector3D(start, direction, c) vector3d.add_artist(self) return vector3d def plot_basis(self, R=None, p=np.zeros(3), s=1.0, strict_check=True): """Plot basis. Parameters ---------- R : array-like, shape (3, 3), optional (default: I) Rotation matrix, each column contains a basis vector p : array-like, shape (3,), optional (default: [0, 0, 0]) Offset from the origin s : float, optional (default: 1) Scaling of the frame that will be drawn strict_check : bool, optional (default: True) Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning. Returns ------- Frame : frame New frame. """ if R is None: R = np.eye(3) R = pr.check_matrix(R, strict_check=strict_check) frame = Frame(pt.transform_from(R=R, p=p), s=s) frame.add_artist(self) return frame def plot_transform(self, A2B=None, s=1.0, name=None, strict_check=True): """Plot coordinate frame. Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B s : float, optional (default: 1) Length of basis vectors name : str, optional (default: None) Name of the frame strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. Returns ------- Frame : frame New frame. """ if A2B is None: A2B = np.eye(4) A2B = pt.check_transform(A2B, strict_check=strict_check) frame = Frame(A2B, name, s) frame.add_artist(self) return frame def plot_trajectory(self, P, n_frames=10, s=1.0, c=(0, 0, 0)): """Trajectory of poses. Parameters ---------- P : array-like, shape (n_steps, 7), optional (default: None) Sequence of poses represented by positions and quaternions in the order (x, y, z, w, vx, vy, vz) for each step n_frames : int, optional (default: 10) Number of frames that should be plotted to indicate the rotation s : float, optional (default: 1) Scaling of the frames that will be drawn c : array-like, shape (3,), optional (default: black) A color is represented by 3 values between 0 and 1 indicate representing red, green, and blue respectively. Returns ------- trajectory : Trajectory New trajectory. """ H = ptr.transforms_from_pqs(P) trajectory = Trajectory(H, n_frames, s, c) trajectory.add_artist(self) return trajectory def plot_sphere(self, radius=1.0, A2B=np.eye(4), resolution=20, c=None): """Plot sphere. Parameters ---------- radius : float, optional (default: 1) Radius of the sphere A2B : array-like, shape (4, 4) Transform from frame A to frame B resolution : int, optional (default: 20) The resolution of the sphere. The longitudes will be split into resolution segments (i.e. there are resolution + 1 latitude lines including the north and south pole). The latitudes will be split into 2 * resolution segments (i.e. there are 2 * resolution longitude lines.) c : array-like, shape (3,), optional (default: None) Color Returns ------- sphere : Sphere New sphere. """ sphere = Sphere(radius, A2B, resolution, c) sphere.add_artist(self) return sphere def plot_box(self, size=np.ones(3), A2B=np.eye(4), c=None): """Plot box. Parameters ---------- size : array-like, shape (3,), optional (default: [1, 1, 1]) Size of the box per dimension A2B : array-like, shape (4, 4), optional (default: I) Center of the box c : array-like, shape (3,), optional (default: None) Color Returns ------- box : Box New box. """ box = Box(size, A2B, c) box.add_artist(self) return box def plot_cylinder( self, length=2.0, radius=1.0, A2B=np.eye(4), resolution=20, split=4, c=None, ): """Plot cylinder. Parameters ---------- length : float, optional (default: 1) Length of the cylinder. radius : float, optional (default: 1) Radius of the cylinder. A2B : array-like, shape (4, 4) Pose of the cylinder. The position corresponds to the center of the line segment and the z-axis to the direction of the line segment. resolution : int, optional (default: 20) The circle will be split into resolution segments split : int, optional (default: 4) The height will be split into split segments c : array-like, shape (3,), optional (default: None) Color Returns ------- cylinder : Cylinder New cylinder. """ cylinder = Cylinder(length, radius, A2B, resolution, split, c) cylinder.add_artist(self) return cylinder def plot_mesh(self, filename, A2B=np.eye(4), s=np.ones(3), c=None): """Plot mesh. Parameters ---------- filename : str Path to mesh file A2B : array-like, shape (4, 4) Center of the mesh s : array-like, shape (3,), optional (default: [1, 1, 1]) Scaling of the mesh that will be drawn c : array-like, shape (n_vertices, 3) or (3,), optional (default: None) Color(s) Returns ------- mesh : Mesh New mesh. """ mesh = Mesh(filename, A2B, s, c) mesh.add_artist(self) return mesh def plot_ellipsoid( self, radii=np.ones(3), A2B=np.eye(4), resolution=20, c=None ): """Plot ellipsoid. Parameters ---------- radii : array-like, shape (3,) Radii along the x-axis, y-axis, and z-axis of the ellipsoid. A2B : array-like, shape (4, 4) Transform from frame A to frame B resolution : int, optional (default: 20) The resolution of the ellipsoid. The longitudes will be split into resolution segments (i.e. there are resolution + 1 latitude lines including the north and south pole). The latitudes will be split into 2 * resolution segments (i.e. there are 2 * resolution longitude lines.) c : array-like, shape (3,), optional (default: None) Color Returns ------- ellipsoid : Ellipsoid New ellipsoid. """ ellipsoid = Ellipsoid(radii, A2B, resolution, c) ellipsoid.add_artist(self) return ellipsoid def plot_capsule( self, height=1, radius=1, A2B=np.eye(4), resolution=20, c=None ): """Plot capsule. A capsule is the volume covered by a sphere moving along a line segment. Parameters ---------- height : float, optional (default: 1) Height of the capsule along its z-axis. radius : float, optional (default: 1) Radius of the capsule. A2B : array-like, shape (4, 4) Pose of the capsule. The position corresponds to the center of the line segment and the z-axis to the direction of the line segment. resolution : int, optional (default: 20) The resolution of the capsule. The longitudes will be split into resolution segments (i.e. there are resolution + 1 latitude lines including the north and south pole). The latitudes will be split into 2 * resolution segments (i.e. there are 2 * resolution longitude lines.) c : array-like, shape (3,), optional (default: None) Color Returns ------- capsule : Capsule New capsule. """ capsule = Capsule(height, radius, A2B, resolution, c) capsule.add_artist(self) return capsule def plot_cone( self, height=1, radius=1, A2B=np.eye(4), resolution=20, c=None ): """Plot cone. Parameters ---------- height : float, optional (default: 1) Height of the cone along its z-axis. radius : float, optional (default: 1) Radius of the cone. A2B : array-like, shape (4, 4) Pose of the cone. resolution : int, optional (default: 20) The circle will be split into resolution segments. c : array-like, shape (3,), optional (default: None) Color Returns ------- cone : Cone New cone. """ cone = Cone(height, radius, A2B, resolution, c) cone.add_artist(self) return cone def plot_plane( self, normal=np.array([0.0, 0.0, 1.0]), d=None, point_in_plane=None, s=1.0, c=None, ): """Plot plane. Parameters ---------- normal : array-like, shape (3,), optional (default: [0, 0, 1]) Plane normal. d : float, optional (default: None) Distance to origin in Hesse normal form. point_in_plane : array-like, shape (3,), optional (default: None) Point in plane. s : float, optional (default: 1) Scaling of the plane that will be drawn. c : array-like, shape (3,), optional (default: None) Color. Returns ------- plane : Plane New plane. """ plane = Plane(normal, d, point_in_plane, s, c) plane.add_artist(self) return plane def plot_graph( self, tm, frame, show_frames=False, show_connections=False, show_visuals=False, show_collision_objects=False, show_name=False, whitelist=None, convex_hull_of_collision_objects=False, s=1.0, ): """Plot graph of connected frames. Parameters ---------- tm : TransformManager Representation of the graph frame : str Name of the base frame in which the graph will be displayed show_frames : bool, optional (default: False) Show coordinate frames show_connections : bool, optional (default: False) Draw lines between frames of the graph show_visuals : bool, optional (default: False) Show visuals that are stored in the graph show_collision_objects : bool, optional (default: False) Show collision objects that are stored in the graph show_name : bool, optional (default: False) Show names of frames whitelist : list, optional (default: all) List of frames that should be displayed convex_hull_of_collision_objects : bool, optional (default: False) Show convex hull of collision objects. s : float, optional (default: 1) Scaling of the frames that will be drawn Returns ------- graph : Graph New graph. """ graph = Graph( tm, frame, show_frames, show_connections, show_visuals, show_collision_objects, show_name, whitelist, convex_hull_of_collision_objects, s, ) graph.add_artist(self) return graph def plot_camera( self, M, cam2world=None, virtual_image_distance=1, sensor_size=(1920, 1080), strict_check=True, ): """Plot camera in world coordinates. This function is inspired by Blender's camera visualization. It will show the camera center, a virtual image plane, and the top of the virtual image plane. Parameters ---------- M : array-like, shape (3, 3) Intrinsic camera matrix that contains the focal lengths on the diagonal and the center of the the image in the last column. It does not matter whether values are given in meters or pixels as long as the unit is the same as for the sensor size. cam2world : array-like, shape (4, 4), optional (default: I) Transformation matrix of camera in world frame. We assume that the position is given in meters. virtual_image_distance : float, optional (default: 1) Distance from pinhole to virtual image plane that will be displayed. We assume that this distance is given in meters. The unit has to be consistent with the unit of the position in cam2world. sensor_size : array-like, shape (2,), optional (default: [1920, 1080]) Size of the image sensor: (width, height). It does not matter whether values are given in meters or pixels as long as the unit is the same as for the sensor size. strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. Returns ------- camera : Camera New camera. """ camera = Camera( M, cam2world, virtual_image_distance, sensor_size, strict_check ) camera.add_artist(self) return camera def save_image(self, filename): """Save rendered image to file. Parameters ---------- filename : str Path to file in which the rendered image should be stored """ self.visualizer.capture_screen_image(filename, True) def show(self): """Display the figure window.""" self.visualizer.run() self.visualizer.destroy_window() def figure( window_name="Open3D", width=1920, height=1080, with_key_callbacks=False ): """Create a new figure. Parameters ---------- window_name : str, optional (default: Open3D) Window title name. width : int, optional (default: 1920) Width of the window. height : int, optional (default: 1080) Height of the window. with_key_callbacks : bool, optional (default: False) Creates a visualizer that allows to register callbacks for keys. Returns ------- figure : Figure New figure. """ return Figure(window_name, width, height, with_key_callbacks) ================================================ FILE: pytransform3d/visualizer/_figure.pyi ================================================ from typing import Callable, Tuple, Any, Union, List import numpy.typing as npt import open3d as o3d from ._artists import ( Artist, Frame, Trajectory, Sphere, Box, Cylinder, Mesh, Ellipsoid, Capsule, Cone, Plane, Graph, Camera, ) from ..transform_manager import TransformManager class Figure: def __init__( self, window_name: str = ..., width: int = ..., height: int = ..., with_key_callbacks: bool = ..., ): ... def add_geometry(self, geometry: o3d.geometry.Geometry3D): ... def _remove_geometry(self, geometry: o3d.geometry.Geometry3D): ... def update_geometry(self, geometry: o3d.geometry.Geometry3D): ... def remove_artist(self, artist: Artist): ... def set_line_width(self, line_width: float): ... def set_zoom(self, zoom: float): ... def animate( self, callback: Callable, n_frames: int, loop: bool = ..., fargs: Tuple[Any, ...] = ..., ): ... def view_init(self, azim: float = ..., elev: float = ...): ... def plot(self, P: npt.ArrayLike, c: npt.ArrayLike = ...): ... def scatter( self, P: npt.ArrayLike, s: float = ..., c: Union[None, npt.ArrayLike] = ..., ): ... def plot_vector( self, start: npt.ArrayLike, direction: npt.ArrayLike, c: npt.ArrayLike = ..., ): ... def plot_basis( self, R: Union[None, npt.ArrayLike] = ..., p: npt.ArrayLike = ..., s: float = ..., strict_check: bool = ..., ) -> Frame: ... def plot_transform( self, A2B: Union[None, npt.ArrayLike] = ..., s: float = ..., name: Union[str, None] = ..., strict_check: bool = ..., ) -> Frame: ... def plot_trajectory( self, P: npt.ArrayLike, n_frames: int = ..., s: float = ..., c: npt.ArrayLike = ..., ) -> Trajectory: ... def plot_sphere( self, radius: float = ..., A2B: npt.ArrayLike = ..., resolution: int = ..., c: Union[None, npt.ArrayLike] = ..., ) -> Sphere: ... def plot_box( self, size: npt.ArrayLike = ..., A2B: npt.ArrayLike = ..., c: Union[None, npt.ArrayLike] = ..., ) -> Box: ... def plot_cylinder( self, length: float = ..., radius: float = ..., A2B: npt.ArrayLike = ..., resolution: int = ..., split: int = ..., c: Union[None, npt.ArrayLike] = ..., ) -> Cylinder: ... def plot_mesh( self, filename: str, A2B: npt.ArrayLike = ..., s: npt.ArrayLike = ..., c: Union[None, npt.ArrayLike] = ..., ) -> Mesh: ... def plot_ellipsoid( self, radii: npt.ArrayLike = ..., A2B: npt.ArrayLike = ..., resolution: int = ..., c: Union[None, npt.ArrayLike] = ..., ) -> Ellipsoid: ... def plot_capsule( self, height: float = ..., radius: float = ..., A2B: npt.ArrayLike = ..., resolution: int = ..., c: Union[None, npt.ArrayLike] = ..., ) -> Capsule: ... def plot_cone( self, height: float = ..., radius: float = ..., A2B: npt.ArrayLike = ..., resolution: int = ..., c: Union[None, npt.ArrayLike] = ..., ) -> Cone: ... def plot_plane( self, normal: npt.ArrayLike = ..., d: Union[None, float] = ..., point_in_plane: Union[None, npt.ArrayLike] = ..., s: float = ..., c: Union[None, npt.ArrayLike] = ..., ) -> Plane: ... def plot_graph( self, tm: TransformManager, frame: str, show_frames: bool = ..., show_connections: bool = ..., show_visuals: bool = ..., show_collision_objects: bool = ..., show_name: bool = ..., whitelist: Union[None, List[str]] = ..., convex_hull_of_collision_objects: bool = ..., s: float = ..., ) -> Graph: ... def plot_camera( self, M: npt.ArrayLike, cam2world: Union[None, npt.ArrayLike] = ..., virtual_image_distance: float = ..., sensor_size: npt.ArrayLike = ..., strict_check: bool = ..., ) -> Camera: ... def save_image(self, filename: str): ... def show(self): ... def figure( window_name: str = ..., width: int = ..., height: int = ..., with_key_callbacks: bool = ..., ) -> Figure: ... ================================================ FILE: requirements.txt ================================================ numpy scipy matplotlib lxml trimesh pycollada pydot open3d ================================================ FILE: setup.cfg ================================================ [nosetests] cover-package=pytransform3d with-doctest=true with-coverage=true cover-html=true [tool:pytest] addopts=--cov-config=.coveragerc --cov=pytransform3d --cov-report html -W ignore::DeprecationWarning pytransform3d ================================================ FILE: setup.py ================================================ #!/usr/bin/env python from setuptools import setup, find_packages import pytransform3d if __name__ == "__main__": with open("README.md", "r") as f: long_description = f.read() setup(name="pytransform3d", version=pytransform3d.__version__, author='Alexander Fabisch', author_email='afabisch@googlemail.com', url='https://github.com/dfki-ric/pytransform3d', description='3D transformations for Python', long_description=long_description, long_description_content_type="text/markdown", classifiers=[ "Programming Language :: Python :: 2", "Programming Language :: Python :: 3", "License :: OSI Approved :: BSD License", "Operating System :: OS Independent", "Topic :: Scientific/Engineering :: Mathematics", "Topic :: Scientific/Engineering :: Visualization", ], license='BSD-3-Clause', packages=find_packages(), install_requires=["numpy", "scipy", "matplotlib", "lxml"], extras_require={ "all": ["pydot", "trimesh", "pycollada", "open3d"], "doc": ["numpydoc", "sphinx", "sphinx-gallery", "pydata-sphinx-theme", "sphinxcontrib.video"], "test": ["pytest", "pytest-cov"] } ) ================================================ FILE: test/test_data/reconstruction_camera_matrix.csv ================================================ 1601.9552, 0.0, 945.55145 0.0, 1601.9552, 718.12787 0.0, 0.0, 1.0 ================================================ FILE: test/test_data/reconstruction_odometry.csv ================================================ x, y, z, qx, qy, qz, qw 0.0, 0.0, 0.0, -0.47900817, 0.6978452, -0.4390308, -0.30135536 0.0, 0.0, 0.0, -0.47900817, 0.6978452, -0.4390308, -0.30135536 0.0, 0.0, 0.0, -0.47900817, 0.6978452, -0.4390308, -0.30135536 0.0, 0.0, 0.0, -0.47900817, 0.6978452, -0.4390308, -0.30135536 0.0, 0.0, 0.0, -0.48147768, 0.6965384, -0.43960878, -0.29959577 0.0, 0.0, 0.0, -0.48411176, 0.6957959, -0.4396104, -0.2970631 0.0, 0.0, 0.0, -0.48698434, 0.69521743, -0.43942723, -0.29397732 0.0, 0.0, 0.0, -0.4908087, 0.69465774, -0.4391291, -0.2893493 0.0, 0.0, 0.0, -0.49357116, 0.6944391, -0.43876722, -0.28570113 0.0, 0.0, 0.0, -0.49614492, 0.69449973, -0.43813822, -0.28203756 0.0, 0.0, 0.0, -0.49855366, 0.69487345, -0.4371126, -0.27843776 0.0, 0.0, 0.0, 0.501627, -0.69583046, 0.43502498, 0.27375808 0.0, 0.0, 0.0, 0.5039393, -0.69676524, 0.43308696, 0.2701833 0.0, 0.0, 0.0, 0.50638056, -0.6977974, 0.43088353, 0.26645204 0.0, 0.0, 0.0, 0.50989676, -0.69924563, 0.42748955, 0.2613684 0.0, 0.0, 0.0, 0.5127074, -0.7002776, 0.4247525, 0.25754157 0.0, 0.0, 0.0, 0.515675, -0.7011822, 0.42196944, 0.2537019 0.0, 0.0, 0.0, 0.51994044, -0.7020727, 0.4183925, 0.2484021 0.0, 0.0, 0.0, 0.5233885, -0.7023825, 0.41603673, 0.24420619 0.0, 0.0, 0.0, 0.5270112, -0.70244205, 0.4139716, 0.23971182 0.0, 0.0, 0.0, 0.53202796, -0.7022791, 0.41143218, 0.233396 0.0, 0.0, 0.0, 0.5358095, -0.7020097, 0.4096565, 0.2286306 0.0, 0.0, 0.0, 0.5395391, -0.7015894, 0.4081023, 0.22388013 0.0, 0.0, 0.0, 0.5444844, -0.70083135, 0.4062577, 0.21754695 0.0, 0.0, 0.0, 0.54816055, -0.7000532, 0.40515965, 0.21281716 0.0, 0.0, 0.0, 0.55169463, -0.6990378, 0.4045015, 0.2082249 0.0, 0.0, 0.0, 0.55518293, -0.69781166, 0.4042168, 0.20356664 0.0, 0.0, 0.0, 0.55995613, -0.6961375, 0.40374106, 0.1970655 0.0, 0.0, 0.0, 0.56351745, -0.6948757, 0.40329278, 0.19222546 0.0, 0.0, 0.0, 0.56695974, -0.69358385, 0.40285558, 0.18763117 0.0, 0.0, 0.0, 0.57124156, -0.6918035, 0.40256435, 0.18174969 0.0, 0.0, 0.0, 0.57424724, -0.6903653, 0.40269834, 0.1773975 0.0, 0.0, 0.0, 0.5771148, -0.6888815, 0.40305543, 0.17299388 0.0, 0.0, 0.0, 0.580495, -0.68701404, 0.40376082, 0.1673748 0.0, 0.0, 0.0, 0.5824233, -0.6857676, 0.40458274, 0.16376396 0.0, 0.0, 0.0, 0.5838427, -0.68468356, 0.40552488, 0.16088991 0.0, 0.0, 0.0, 0.58536637, -0.68356895, 0.40639102, 0.15787958 0.0, 0.0, 0.0, 0.5865285, -0.68293786, 0.40658978, 0.15577173 0.0, 0.0, 0.0, 0.58786064, -0.68237334, 0.40645206, 0.15356812 0.0, 0.0, 0.0, 0.5894297, -0.68177325, 0.40609556, 0.15114339 0.0, 0.0, 0.0, 0.5918106, -0.68087566, 0.40542492, 0.1476453 0.0, 0.0, 0.0, 0.59370595, -0.6801143, 0.4048617, 0.14506792 0.0, 0.0, 0.0, 0.59545404, -0.67924446, 0.40455785, 0.14280835 0.0, 0.0, 0.0, 0.59723514, -0.6779324, 0.40489537, 0.14063233 0.0, 0.0, 0.0, 0.59808624, -0.6769252, 0.40557697, 0.13990146 0.0, 0.0, 0.0, 0.59856373, -0.6760633, 0.40627474, 0.14000225 0.0, 0.0, 0.0, 0.59901625, -0.67530245, 0.4065998, 0.14079304 0.0, 0.0, 0.0, 0.599484, -0.6749438, 0.40627968, 0.14144456 0.0, 0.0, 0.0, 0.5999988, -0.6747316, 0.4055674, 0.14231572 0.0, 0.0, 0.0, 0.60015184, -0.6751722, 0.40396944, 0.14411432 0.0, 0.0, 0.0, 0.59965354, -0.67621815, 0.4022964, 0.14595243 0.0, 0.0, 0.0, 0.5993234, -0.67709816, 0.40056533, 0.14797615 0.0, 0.0, 0.0, 0.5983897, -0.67878985, 0.3978674, 0.15124667 0.0, 0.0, 0.0, 0.59736574, -0.6803849, 0.39567387, 0.15385929 0.0, 0.0, 0.0, 0.5966173, -0.68174165, 0.39358315, 0.15610418 0.0, 0.0, 0.0, 0.59621245, -0.6828864, 0.3915588, 0.15772969 0.0, 0.0, 0.0, 0.59626675, -0.6840274, 0.389069, 0.1587382 0.0, 0.0, 0.0, 0.5968434, -0.6845544, 0.38729215, 0.15864396 0.0, 0.0, 0.0, 0.5977512, -0.6849857, 0.38543516, 0.15788513 0.0, 0.0, 0.0, 0.5991555, -0.6854156, 0.38308966, 0.15639825 0.0, 0.0, 0.0, 0.6002076, -0.6856665, 0.38147166, 0.15521483 0.0, 0.0, 0.0, 0.60116756, -0.6859394, 0.37990543, 0.1541315 0.0, 0.0, 0.0, 0.60205036, -0.6862335, 0.37841853, 0.15303047 0.0, 0.0, 0.0, 0.60327816, -0.68657255, 0.37648502, 0.1514351 0.0, 0.0, 0.0, 0.60422873, -0.6868257, 0.37498215, 0.15022093 0.0, 0.0, 0.0, 0.6053425, -0.68722165, 0.37307993, 0.1486546 0.0, 0.0, 0.0, 0.6061194, -0.687433, 0.37195602, 0.14732233 0.0, 0.0, 0.0, 0.6069242, -0.687459, 0.37127098, 0.1456056 0.0, 0.0, 0.0, 0.6078877, -0.68728036, 0.37090755, 0.14333837 0.0, 0.0, 0.0, 0.6095787, -0.68676287, 0.37054825, 0.13951522 0.0, 0.0, 0.0, 0.6111343, -0.68620473, 0.3702026, 0.13633713 0.0, 0.0, 0.0, 0.6127308, -0.6856092, 0.36973673, 0.13340046 0.0, 0.0, 0.0, 0.6147629, -0.6848444, 0.3689829, 0.13002408 0.0, 0.0, 0.0, 0.61621845, -0.6843221, 0.3683, 0.12780151 0.0, 0.0, 0.0, 0.617558, -0.683731, 0.3678676, 0.12572768 0.0, 0.0, 0.0, 0.6192527, -0.6827331, 0.36782208, 0.12291636 0.0, 0.0, 0.0, 0.6206096, -0.681946, 0.36774793, 0.12064337 0.0, 0.0, 0.0, 0.6220439, -0.68118757, 0.3674911, 0.11830101 0.0, 0.0, 0.0, 0.6240476, -0.6802366, 0.36680213, 0.11532117 0.0, 0.0, 0.0, 0.62551314, -0.6797153, 0.36584365, 0.11348488 0.0, 0.0, 0.0, 0.62677425, -0.6794832, 0.3645255, 0.11215108 7.450581e-09, 7.450581e-09, 3.7252903e-09, 0.62548697, -0.6764093, 0.36948106, 0.12132669 0.0028046146, 0.001567781, 0.0013065189, 0.62593263, -0.6766347, 0.36829233, 0.12138598 0.0059238, 0.0028972104, 0.0025103241, 0.6260785, -0.676805, 0.36756715, 0.12188167 0.009311347, 0.004115081, 0.0037264745, 0.6260929, -0.6769722, 0.3670563, 0.12241699 0.012883923, 0.0053333254, 0.0050433944, 0.6262336, -0.6773039, 0.3661009, 0.12272303 0.016638517, 0.0065570846, 0.006512668, 0.6265552, -0.6778797, 0.36452198, 0.12260287 0.01673872, 0.003168425, 0.005734041, 0.62697494, -0.67819804, 0.36336714, 0.12212307 0.01973583, 0.0024399785, 0.0065159025, 0.6268914, -0.67909485, 0.36181223, 0.12218529 0.023840725, 0.0023979843, 0.0078021996, 0.6261733, -0.68007904, 0.36090901, 0.123061314 0.028298095, 0.0026322603, 0.009385433, 0.6253509, -0.6811341, 0.36010763, 0.12375412 0.032787725, 0.0031247586, 0.011272538, 0.62494385, -0.6821253, 0.35902768, 0.12348895 0.037272707, 0.0037860898, 0.013417189, 0.624946, -0.68306285, 0.35762137, 0.12237113 0.041818894, 0.004460244, 0.015693977, 0.6251455, -0.6839592, 0.35601723, 0.12101524 0.04585798, 0.0046609715, 0.018724333, 0.6267213, -0.68381137, 0.35386586, 0.120006055 0.05048956, 0.004867859, 0.021280397, 0.6267297, -0.6846113, 0.35254344, 0.119290486 0.05537515, 0.0050262944, 0.02366931, 0.6266089, -0.6850836, 0.35199365, 0.11883639 0.06031783, 0.0052325055, 0.026069801, 0.6266511, -0.68522966, 0.35187474, 0.11812233 0.0652494, 0.005552575, 0.028587114, 0.6270079, -0.6852048, 0.35169685, 0.116895944 0.06978834, 0.006512766, 0.03039236, 0.62793386, -0.6847005, 0.3516811, 0.114911124 0.074708745, 0.0070446357, 0.032713976, 0.62869877, -0.68469274, 0.35067412, 0.11384735 0.0798357, 0.0073456317, 0.035104085, 0.62923056, -0.6848551, 0.3495688, 0.11333177 0.08505254, 0.007554746, 0.037483536, 0.6296681, -0.6850911, 0.34839356, 0.113092884 0.09033543, 0.0077198893, 0.039889667, 0.6299896, -0.6854926, 0.34708747, 0.11288568 0.09573636, 0.007818667, 0.04231205, 0.63010406, -0.6860649, 0.345773, 0.11280416 0.100577645, 0.007839231, 0.044237517, 0.63035715, -0.6858744, 0.3460606, 0.11166088 0.10600169, 0.007729262, 0.046429526, 0.6302122, -0.68645024, 0.34505913, 0.11203948 0.11161738, 0.0075497837, 0.048668355, 0.6299617, -0.6871256, 0.34402356, 0.11249083 0.117286116, 0.0072958367, 0.050910342, 0.6295405, -0.68790185, 0.34306264, 0.1130382 0.12299399, 0.0069648772, 0.053159397, 0.6288906, -0.688804, 0.34222704, 0.11369378 0.12871508, 0.0066192, 0.055431146, 0.6282218, -0.68972635, 0.34138468, 0.11432977 0.13369647, 0.0063555166, 0.05803233, 0.62809193, -0.68975115, 0.34206977, 0.11283677 0.13903534, 0.006149709, 0.06049152, 0.6280258, -0.69051474, 0.34057936, 0.11304221 0.14443067, 0.0059915395, 0.06292351, 0.62806284, -0.6913358, 0.33883217, 0.11306908 0.14981225, 0.005847618, 0.06530817, 0.6282028, -0.69216573, 0.33685088, 0.11313472 0.15518685, 0.0056429254, 0.06756631, 0.62832373, -0.69291025, 0.33497116, 0.11348496 0.16051853, 0.005364783, 0.06966032, 0.62841165, -0.69349176, 0.33336532, 0.11417258 0.16529238, 0.0050693676, 0.071020484, 0.6288426, -0.6931029, 0.33373812, 0.11306773 0.17027688, 0.00479836, 0.07275352, 0.62924236, -0.6932548, 0.33243474, 0.11374914 0.17533523, 0.004541083, 0.07462713, 0.6297286, -0.6933599, 0.3311469, 0.1141736 0.18044561, 0.0042380095, 0.07654257, 0.63018674, -0.6934681, 0.32992294, 0.11453163 0.18559085, 0.0038993163, 0.07846738, 0.63055444, -0.693576, 0.32889318, 0.114815705 0.19077533, 0.0035539716, 0.08040774, 0.630874, -0.69370246, 0.32799062, 0.11487855 0.1945079, 0.002066499, 0.0821079, 0.63079554, -0.69388235, 0.3281129, 0.11386904 0.19932035, 0.0012092069, 0.08390927, 0.63066375, -0.69427663, 0.32755423, 0.11380489 0.20451114, 0.00049579144, 0.0856902, 0.6301897, -0.69488937, 0.32710832, 0.11397312 0.2098149, -0.00015059253, 0.08745253, 0.6296865, -0.69560075, 0.32649663, 0.11416806 0.21507862, -0.00072203577, 0.08924295, 0.6292976, -0.69644505, 0.3254272, 0.11421852 0.21950117, -0.0018376261, 0.09082343, 0.6287516, -0.6976388, 0.32423043, 0.11334076 0.22429925, -0.0024457276, 0.092588365, 0.6285937, -0.69855356, 0.32272458, 0.11287928 0.22914046, -0.0028234795, 0.09443043, 0.62866443, -0.69930595, 0.32125235, 0.112022266 0.233944, -0.003119871, 0.09626341, 0.628965, -0.699852, 0.3197933, 0.1110958 0.23872936, -0.003474371, 0.09794414, 0.6292164, -0.700321, 0.3183655, 0.11081793 0.24349317, -0.0039250776, 0.09940849, 0.6293077, -0.7007481, 0.3170469, 0.111378446 0.24817099, -0.004405548, 0.1007003, 0.6294485, -0.7010003, 0.31585607, 0.11237536 0.25254413, -0.005269359, 0.10218075, 0.6296524, -0.7010061, 0.31523708, 0.11293227 0.2568304, -0.005671568, 0.10354875, 0.6303583, -0.70055926, 0.314776, 0.11305401 0.2609919, -0.0058382913, 0.10489491, 0.6314586, -0.6997798, 0.31443596, 0.11268622 0.2650517, -0.0059321523, 0.10614723, 0.63276815, -0.6987465, 0.3142657, 0.11222603 0.26901427, -0.006017886, 0.107290804, 0.6340909, -0.6974737, 0.3146026, 0.11173282 0.27286038, -0.0061034723, 0.1083456, 0.63533795, -0.69606304, 0.31536642, 0.11129158 0.2768396, -0.0073539317, 0.10992965, 0.63616484, -0.69501084, 0.31616813, 0.11086842 0.2804555, -0.0078346925, 0.11098195, 0.6373871, -0.6935999, 0.31678286, 0.110930115 0.28381833, -0.008060083, 0.11182417, 0.6386758, -0.6921227, 0.31734562, 0.111135535 0.28694415, -0.00816966, 0.112562865, 0.6401261, -0.6905848, 0.3177049, 0.11133217 0.28985423, -0.0082798395, 0.11318925, 0.64155275, -0.6892062, 0.31764746, 0.11182522 0.29258412, -0.008526444, 0.113639966, 0.64255786, -0.6882925, 0.3171187, 0.113174595 0.29688686, -0.010466282, 0.11475565, 0.64267075, -0.68848246, 0.31557617, 0.115662456 0.2997967, -0.011479236, 0.11511473, 0.6427508, -0.6885153, 0.31425688, 0.11857719 0.3020257, -0.012149649, 0.115225255, 0.64271724, -0.6887398, 0.3127083, 0.12151391 0.3038012, -0.012656333, 0.11528218, 0.64263844, -0.68913, 0.31096745, 0.12415657 0.30521166, -0.0130476, 0.11535856, 0.6426068, -0.68958014, 0.30917308, 0.12628432 0.30632442, -0.01335606, 0.11546281, 0.6426324, -0.6899769, 0.30758873, 0.12784582 0.30859727, -0.014423506, 0.11657925, 0.64215714, -0.6907656, 0.30596069, 0.12986554 0.30967715, -0.014989564, 0.11695792, 0.6421029, -0.69099575, 0.30513713, 0.13084362 0.31017056, -0.01539731, 0.11704308, 0.6420592, -0.6910951, 0.3046144, 0.1317481 0.31030974, -0.015752295, 0.11699557, 0.64198476, -0.69116104, 0.30429497, 0.13250086 0.31017387, -0.016080106, 0.11688481, 0.6419571, -0.69112366, 0.30421817, 0.13300528 0.3097844, -0.016394667, 0.116734475, 0.6420425, -0.69096774, 0.30426967, 0.13328502 0.31056467, -0.017278852, 0.11710709, 0.64177203, -0.69110346, 0.30417582, 0.13409626 0.31017652, -0.017791621, 0.11697416, 0.64169323, -0.6910637, 0.3042087, 0.13460237 0.30927864, -0.018176133, 0.11662394, 0.64149284, -0.6911684, 0.3040837, 0.13529985 0.30808252, -0.018489167, 0.11617535, 0.64121956, -0.6914302, 0.3037631, 0.13597742 0.30663297, -0.018736713, 0.11564048, 0.6409492, -0.6917536, 0.30330157, 0.13663493 0.3049276, -0.018907808, 0.11502178, 0.6406936, -0.6920631, 0.30285934, 0.13724649 0.30568826, -0.019419618, 0.1154508, 0.6402535, -0.6926034, 0.30218336, 0.1380623 0.30441567, -0.019511834, 0.11500639, 0.6400283, -0.692861, 0.30183938, 0.13856488 0.30229837, -0.0193733, 0.11420068, 0.6398923, -0.6928902, 0.30187222, 0.1389749 0.2997021, -0.019041564, 0.113250725, 0.6399225, -0.6926455, 0.30230546, 0.13911417 0.2967674, -0.01849556, 0.1122811, 0.64014846, -0.69220996, 0.30298644, 0.13876112 0.2935429, -0.017746434, 0.11132774, 0.6405156, -0.69163436, 0.30390662, 0.13792118 0.2908701, -0.017033631, 0.110659376, 0.64106035, -0.69101405, 0.30462644, 0.13690801 0.2873898, -0.016035253, 0.109841034, 0.64159095, -0.69032407, 0.30572468, 0.13544798 0.28352395, -0.0148134995, 0.10900636, 0.6422342, -0.6896543, 0.30674055, 0.13350005 0.27945185, -0.013477187, 0.10819678, 0.64287585, -0.68907094, 0.30767673, 0.13125159 0.27528, -0.012147563, 0.10734862, 0.64331084, -0.6886523, 0.30860463, 0.12912185 0.27097842, -0.010826545, 0.10635696, 0.6434971, -0.6882827, 0.30981687, 0.12724717 0.26636755, -0.009339507, 0.10525896, 0.6437774, -0.687818, 0.31113005, 0.12512055 0.2609185, -0.008290358, 0.10480189, 0.6446157, -0.68717515, 0.31205586, 0.12199238 0.25557458, -0.0066062957, 0.10407503, 0.6454637, -0.68670046, 0.3127242, 0.11841682 0.25015804, -0.0047324016, 0.10319373, 0.64635116, -0.68628883, 0.31309944, 0.11491971 0.24469116, -0.0029505268, 0.10207434, 0.6470264, -0.68596023, 0.31340408, 0.11221935 0.23926526, -0.0014132126, 0.100661874, 0.6472197, -0.68586415, 0.31377628, 0.110640705 0.23308718, -0.0014223754, 0.099208504, 0.6470325, -0.6859807, 0.3143105, 0.109491445 0.22716767, -0.0005131974, 0.097595885, 0.6468646, -0.6861855, 0.31467348, 0.108148396 0.22126898, 0.000677763, 0.09592757, 0.64664656, -0.6865516, 0.31482518, 0.10667833 0.21536565, 0.001867339, 0.094203025, 0.6462302, -0.6872795, 0.3144888, 0.105499715 0.20937857, 0.0029999688, 0.092359796, 0.64579254, -0.6881719, 0.31370413, 0.10469524 0.2032609, 0.0041095987, 0.09036487, 0.6454177, -0.6890879, 0.3126567, 0.10411296 0.19658202, 0.005015418, 0.08902684, 0.64507574, -0.69001377, 0.3114829, 0.10361783 0.19008586, 0.006039694, 0.087118775, 0.6447722, -0.6908436, 0.31055367, 0.10276365 0.18360734, 0.0070627034, 0.08491917, 0.6445156, -0.6915888, 0.30965516, 0.10207006 0.17710939, 0.008012898, 0.08250384, 0.6442544, -0.6922987, 0.3087218, 0.10173306 0.17059898, 0.008878335, 0.07997221, 0.643893, -0.69302136, 0.3079557, 0.101421736 0.16405168, 0.009708224, 0.07743412, 0.6434304, -0.6937537, 0.30753702, 0.1006175 0.15741941, 0.010572955, 0.07494986, 0.64292616, -0.6945728, 0.30723777, 0.09909286 0.14949654, 0.0112205325, 0.072654136, 0.6424492, -0.69538885, 0.30705014, 0.09702424 0.14233401, 0.011893205, 0.07011676, 0.64172995, -0.6963952, 0.30677146, 0.09543368 0.13547695, 0.012405612, 0.06732209, 0.64074236, -0.69743085, 0.30670863, 0.094706446 0.1287756, 0.012713946, 0.0642494, 0.63956916, -0.69839, 0.30690572, 0.09492913 0.122103006, 0.012900114, 0.06100126, 0.63827455, -0.6992615, 0.30739975, 0.09562562 0.11455705, 0.012712656, 0.057795063, 0.6369716, -0.70008564, 0.3080935, 0.09605 0.10741571, 0.01279374, 0.054575466, 0.6358167, -0.7008645, 0.30866033, 0.09620216 0.10037307, 0.012949472, 0.05131696, 0.6347103, -0.7016115, 0.3091605, 0.09645642 0.093366906, 0.013089597, 0.04798829, 0.633675, -0.70232195, 0.30948865, 0.097039275 0.0863412, 0.013144799, 0.044562805, 0.63269275, -0.70299596, 0.30963725, 0.09808764 0.079249844, 0.013124332, 0.041051414, 0.631705, -0.70359755, 0.309873, 0.0993883 0.071293116, 0.012372069, 0.037725728, 0.63078433, -0.7040522, 0.31045085, 0.100209296 0.06368184, 0.012243986, 0.03433169, 0.6300344, -0.7044017, 0.3110195, 0.10070655 0.05610752, 0.012374029, 0.031007063, 0.6294761, -0.7046916, 0.3114374, 0.100877486 0.048476562, 0.012614422, 0.02782264, 0.62914336, -0.7048935, 0.3116956, 0.10074548 0.04071483, 0.012904838, 0.02468967, 0.62896776, -0.7049512, 0.31196454, 0.10060553 0.03282357, 0.013198724, 0.021516494, 0.6287517, -0.7049354, 0.31241032, 0.10068341 0.024074674, 0.013118416, 0.018298272, 0.6285022, -0.70468736, 0.3134626, 0.10070753 0.015586197, 0.013368696, 0.015050981, 0.6284845, -0.7041639, 0.31466976, 0.10071403 0.007045835, 0.013712725, 0.011790961, 0.62869185, -0.70352715, 0.31565177, 0.10079672 -0.0016343668, 0.014012486, 0.008505654, 0.6289551, -0.70284426, 0.31650174, 0.10125161 -0.010493457, 0.014250606, 0.005192753, 0.62925094, -0.7021428, 0.31723502, 0.10198379 -0.019553855, 0.014457434, 0.0018966831, 0.62952095, -0.7015379, 0.31778324, 0.10276953 -0.028582722, 0.014031507, -0.0011266582, 0.62973464, -0.7011603, 0.31795487, 0.10350441 -0.037934475, 0.013992327, -0.0042618355, 0.6299089, -0.7009474, 0.3177878, 0.10439556 -0.04750879, 0.014074234, -0.0073920493, 0.6300791, -0.7007776, 0.31756574, 0.10518107 -0.05732148, 0.014225341, -0.0104961395, 0.63040346, -0.7005654, 0.31718358, 0.10580234 -0.067347266, 0.014407232, -0.013542477, 0.6308591, -0.70033485, 0.3166788, 0.10612442 -0.07755612, 0.014564998, -0.016559094, 0.6313229, -0.70009774, 0.31622338, 0.10628888 -0.087637246, 0.014260732, -0.019376718, 0.6317287, -0.6997971, 0.31599227, 0.10654422 -0.0981157, 0.014182724, -0.022409577, 0.6320073, -0.6993933, 0.3161789, 0.106988505 -0.10889543, 0.014210582, -0.025508156, 0.6323197, -0.69890106, 0.3165225, 0.107343 -0.11995356, 0.014299601, -0.0285486, 0.6327652, -0.698268, 0.3170609, 0.1072491 -0.13130543, 0.014460216, -0.03148369, 0.63344514, -0.6975003, 0.3176096, 0.10660559 -0.14289747, 0.014654322, -0.03439179, 0.6343141, -0.69668305, 0.3179646, 0.105720215 -0.15437411, 0.014670387, -0.037221886, 0.63512397, -0.69586176, 0.318361, 0.10507201 -0.1661765, 0.014616353, -0.040249713, 0.6356622, -0.69501954, 0.31922212, 0.10477806 -0.17819872, 0.01448445, -0.04337024, 0.63601625, -0.6941502, 0.3204275, 0.104713686 -0.19041955, 0.014316733, -0.046513047, 0.6363096, -0.69326514, 0.3217747, 0.104664005 -0.20282558, 0.014160655, -0.04963863, 0.6366405, -0.69244564, 0.32293114, 0.1045144 -0.21540567, 0.014026794, -0.052745692, 0.63714755, -0.6917352, 0.32353446, 0.104263365 -0.22781527, 0.013744405, -0.0557397, 0.63782, -0.6911334, 0.3235785, 0.10400527 -0.24049923, 0.013489291, -0.058856837, 0.6382977, -0.6906172, 0.32376522, 0.1039227 -0.25327307, 0.013184264, -0.06204373, 0.6384867, -0.6901914, 0.32425046, 0.10407811 -0.26606017, 0.012853755, -0.06522711, 0.6384502, -0.68991005, 0.32487786, 0.10421074 -0.27886114, 0.012544898, -0.068359196, 0.6382523, -0.6897819, 0.32553822, 0.10420976 -0.2915836, 0.012063876, -0.07109907, 0.638121, -0.6897661, 0.32593134, 0.103889935 -0.3044477, 0.011917828, -0.07388878, 0.63819015, -0.689853, 0.3258977, 0.10298954 -0.3173656, 0.0119378045, -0.07661848, 0.63840383, -0.6900382, 0.3255508, 0.1015112 -0.33023205, 0.011978483, -0.07933018, 0.6384376, -0.6903765, 0.3252939, 0.09980716 -0.3429991, 0.011977129, -0.082100995, 0.6382335, -0.69078916, 0.32530087, 0.09822264 -0.35567334, 0.011931159, -0.084938556, 0.63785386, -0.69118357, 0.32562116, 0.09684278 -0.36824906, 0.011845125, -0.08785358, 0.6373796, -0.6916038, 0.325955, 0.0958369 -0.38045377, 0.011413664, -0.09094691, 0.63665336, -0.69213563, 0.3262736, 0.09574097 -0.3926324, 0.010999812, -0.09415298, 0.63563627, -0.6927223, 0.32674795, 0.09663373 -0.40468585, 0.010594001, -0.097409375, 0.6344991, -0.6933292, 0.32725814, 0.09801816 -0.41658536, 0.010216971, -0.10065884, 0.6333673, -0.69400424, 0.32755512, 0.09955675 -0.42826843, 0.009839554, -0.10383883, 0.63225484, -0.6947247, 0.32772133, 0.101045474 -0.43967825, 0.009390667, -0.10694589, 0.6311582, -0.6954976, 0.32771868, 0.1025814 -0.45080173, 0.008864149, -0.10997732, 0.6301168, -0.6962091, 0.32774013, 0.10407722 -0.46183908, 0.0085133985, -0.112899676, 0.62915385, -0.69679457, 0.32796362, 0.10527418 -0.4725953, 0.008136615, -0.11573054, 0.6284082, -0.6973134, 0.328091, 0.10589397 -0.48315895, 0.007812707, -0.11850922, 0.6280389, -0.69762033, 0.32810926, 0.1060062 -0.4934864, 0.007477291, -0.121313915, 0.6278712, -0.69781244, 0.32800487, 0.10605808 -0.5035238, 0.007075444, -0.12419677, 0.6276119, -0.69796544, 0.32805383, 0.10643382 -0.5132544, 0.0065618875, -0.12721202, 0.62707645, -0.6981657, 0.3283537, 0.1073476 -0.522453, 0.00570876, -0.1304742, 0.62632054, -0.6983606, 0.32894313, 0.108680286 -0.5315839, 0.004968323, -0.13375105, 0.62540793, -0.6985813, 0.32968408, 0.11026079 -0.54053664, 0.004283987, -0.1370013, 0.62451935, -0.6988486, 0.33031058, 0.111718856 -0.5492577, 0.0036191046, -0.14023048, 0.6237775, -0.6992332, 0.33044863, 0.113040835 -0.5577471, 0.0029186667, -0.14349154, 0.6231577, -0.6997195, 0.33014196, 0.11433859 -0.56598705, 0.0022431388, -0.14657265, 0.6226094, -0.7001919, 0.32974455, 0.11557417 -0.5741228, 0.0014746711, -0.14989395, 0.621993, -0.7004005, 0.33013013, 0.11652429 -0.5821998, 0.0007132143, -0.15329957, 0.6215302, -0.700195, 0.3312494, 0.117050976 -0.59021544, -2.8626531e-05, -0.1567479, 0.62126833, -0.69985116, 0.33236483, 0.11733506 -0.5981252, -0.0007678047, -0.16023965, 0.6210766, -0.6996021, 0.33321193, 0.117433354 -0.60590786, -0.0015426725, -0.16382794, 0.6208333, -0.69934565, 0.33416894, 0.11752817 -0.6135916, -0.002805598, -0.16770774, 0.6206731, -0.6989652, 0.33520445, 0.1176881 -0.62118626, -0.0038526282, -0.17169558, 0.6204968, -0.69846964, 0.33650383, 0.11785182 -0.6287083, -0.0048751608, -0.17584862, 0.62032783, -0.6978451, 0.338021, 0.11809879 -0.6361815, -0.0058840094, -0.18015309, 0.6201439, -0.69717705, 0.3396428, 0.1183563 -0.6436294, -0.0068553463, -0.1846169, 0.6200031, -0.696522, 0.34112158, 0.11869819 -0.6510451, -0.007753344, -0.18919687, 0.62000483, -0.6958534, 0.34240246, 0.11892216 -0.6584404, -0.00937931, -0.19398585, 0.62030077, -0.6951336, 0.34329635, 0.11901216 -0.6657707, -0.01041922, -0.19883308, 0.6205631, -0.69449586, 0.34409165, 0.11907073 -0.67305964, -0.011194609, -0.2037261, 0.62089324, -0.6938103, 0.34490603, 0.118990615 -0.6803121, -0.011813633, -0.20867364, 0.6213621, -0.69321245, 0.34534112, 0.1187649 -0.6875181, -0.012341484, -0.21374308, 0.621857, -0.6927304, 0.3454347, 0.118715 -0.6946863, -0.012778476, -0.21893454, 0.6223026, -0.6923683, 0.3453195, 0.11882754 -0.70184916, -0.013510443, -0.2240998, 0.6229718, -0.6919491, 0.34492108, 0.118919894 -0.7089824, -0.013926702, -0.22943461, 0.6236076, -0.69181097, 0.34398025, 0.11911613 -0.7160384, -0.014256746, -0.23487943, 0.62421006, -0.69180584, 0.3427927, 0.11941388 -0.7230795, -0.014534719, -0.24043202, 0.62478805, -0.691944, 0.341337, 0.119759955 -0.7301823, -0.014781708, -0.24615857, 0.6253885, -0.6922322, 0.33948943, 0.120210916 -0.73733747, -0.01507392, -0.2521083, 0.6258859, -0.6925673, 0.3376428, 0.12089109 -0.7450438, -0.016410388, -0.25827274, 0.6262549, -0.69284976, 0.3360504, 0.12179511 -0.75232106, -0.01732927, -0.26483014, 0.62608534, -0.6934065, 0.3345255, 0.12368146 -0.759427, -0.018218767, -0.27175254, 0.62564504, -0.6939791, 0.33322203, 0.12619159 -0.7664925, -0.019151421, -0.27889982, 0.62500906, -0.69445145, 0.33243135, 0.12880264 -0.7735602, -0.020124085, -0.2861842, 0.62421197, -0.69481474, 0.33219212, 0.13130234 -0.7806176, -0.02114969, -0.29358283, 0.6231983, -0.6951763, 0.33233613, 0.13381496 -0.7883824, -0.02273445, -0.30061704, 0.6220917, -0.6955836, 0.33247373, 0.13647902 -0.7956933, -0.023971656, -0.3079774, 0.6206381, -0.69642615, 0.33230966, 0.13917343 -0.80295753, -0.025005318, -0.3154797, 0.61920905, -0.6973901, 0.33187568, 0.1417244 -0.8103268, -0.025851227, -0.323053, 0.61811954, -0.6982471, 0.33114344, 0.14395545 -0.81780326, -0.026577743, -0.33067575, 0.6173828, -0.69889724, 0.33024165, 0.14601848 -0.82532525, -0.02727809, -0.3383269, 0.6167359, -0.6993485, 0.32948035, 0.14829369 -0.8328186, -0.028019983, -0.34594432, 0.6159667, -0.69980013, 0.32874617, 0.1509659 -0.84071255, -0.02897683, -0.35339034, 0.6152072, -0.7003749, 0.32763878, 0.15377887 -0.8484645, -0.029719353, -0.3607152, 0.6144951, -0.7011926, 0.32597107, 0.15642098 -0.85629594, -0.030327685, -0.36785978, 0.6140171, -0.7020514, 0.3238717, 0.15878862 -0.8642249, -0.030834764, -0.37476602, 0.61375904, -0.7029772, 0.3213797, 0.16074209 -0.87225413, -0.031197691, -0.38139364, 0.61381155, -0.7039135, 0.31853682, 0.16209672 -0.88092154, -0.031633183, -0.38830835, 0.6143101, -0.7047111, 0.31535822, 0.16295594 -0.88942766, -0.03179157, -0.39487106, 0.61518055, -0.70518893, 0.3122609, 0.16356868 -0.8979354, -0.031819716, -0.40135434, 0.6163993, -0.70503193, 0.30993354, 0.16408215 -0.9064254, -0.03180285, -0.40786734, 0.6178049, -0.7041967, 0.3086843, 0.1647364 -0.9148507, -0.031800624, -0.41444448, 0.6192949, -0.70290995, 0.30811974, 0.16569093 -0.92318034, -0.031886976, -0.4211412, 0.6206912, -0.70155007, 0.30755135, 0.16727814 -0.9319428, -0.03306754, -0.42828748, 0.62192076, -0.70015943, 0.30693662, 0.16964996 -0.9402611, -0.033708706, -0.43530485, 0.622785, -0.6988024, 0.3066429, 0.1725809 -0.94842464, -0.034090362, -0.4421677, 0.6236743, -0.6973148, 0.30675492, 0.17516792 -0.9565417, -0.034239583, -0.44879907, 0.62478817, -0.69573385, 0.3071079, 0.17685795 -0.9646512, -0.03420984, -0.45521268, 0.62617207, -0.69417983, 0.3073566, 0.17763653 -0.9727179, -0.034102485, -0.46147075, 0.62761295, -0.69286186, 0.3072473, 0.17788601 -0.98077255, -0.03459251, -0.4674551, 0.62897563, -0.69198126, 0.30660972, 0.17760071 -0.9886564, -0.034742184, -0.47345984, 0.63000786, -0.69146216, 0.3057263, 0.17748685 -0.99646294, -0.03484284, -0.47946608, 0.6309257, -0.69108033, 0.30472228, 0.1774402 -1.0042366, -0.034994137, -0.48548496, 0.63172317, -0.6907036, 0.30388752, 0.1775016 -1.0119612, -0.03524892, -0.49151236, 0.6323074, -0.6903222, 0.30344504, 0.17766157 -1.0195899, -0.035611413, -0.49748448, 0.63265294, -0.69000953, 0.30334175, 0.17782205 -1.0270493, -0.03607074, -0.50330913, 0.6328208, -0.6899859, 0.30305916, 0.17779885 -1.0340706, -0.037225768, -0.50873935, 0.63291484, -0.6902584, 0.30243918, 0.17746206 -1.0410439, -0.038078308, -0.5141771, 0.63267, -0.69089836, 0.30178773, 0.17695254 -1.0479335, -0.03884113, -0.51956534, 0.632183, -0.69181514, 0.30115047, 0.17619546 -1.0547383, -0.039580144, -0.524878, 0.631522, -0.69300544, 0.30046943, 0.1750473 -1.0615379, -0.04031921, -0.530217, 0.6309236, -0.69410735, 0.29990068, 0.17380981 -1.0683305, -0.04107747, -0.53566694, 0.6302736, -0.6950016, 0.29977703, 0.1728053 -1.0743529, -0.041942507, -0.54046404, 0.62934345, -0.6958168, 0.30038798, 0.1718511 -1.0807014, -0.042632617, -0.5457263, 0.62847227, -0.6962658, 0.30180123, 0.17074074 -1.0870657, -0.04322789, -0.5511777, 0.6277722, -0.696378, 0.30361056, 0.16964766 -1.0932693, -0.043806992, -0.55675113, 0.6270203, -0.6964969, 0.30531102, 0.16888666 -1.0992188, -0.04441586, -0.562391, 0.6260326, -0.6969088, 0.30661893, 0.16848145 -1.1051463, -0.045426153, -0.56802905, 0.6248446, -0.69761056, 0.30766645, 0.16807751 -1.1107363, -0.046214264, -0.5737142, 0.62343174, -0.6982863, 0.3092747, 0.16756542 -1.1161628, -0.04690022, -0.57944244, 0.62210685, -0.6986407, 0.31164804, 0.16661242 -1.1215261, -0.04744573, -0.58523744, 0.6210778, -0.69856495, 0.31460354, 0.16520886 -1.1268275, -0.047821015, -0.59114516, 0.6204067, -0.69816923, 0.3176387, 0.16358769 -1.132007, -0.048051536, -0.5971948, 0.6200068, -0.69771236, 0.32021877, 0.16201489 -1.1382043, -0.048318043, -0.6039546, 0.61984575, -0.69735706, 0.3220999, 0.16042487 -1.1435435, -0.04842837, -0.6104332, 0.6197575, -0.697165, 0.3234351, 0.15890646 -1.1485488, -0.048512932, -0.61691236, 0.6196826, -0.6970442, 0.3244964, 0.15755984 -1.1534053, -0.048637386, -0.62348586, 0.61959195, -0.6969492, 0.3254229, 0.1564213 -1.1581745, -0.048843257, -0.6301835, 0.6195051, -0.69693565, 0.32608983, 0.15543298 -1.1628883, -0.049133442, -0.6370357, 0.61952555, -0.69703144, 0.3262332, 0.15461878 -1.168624, -0.049003318, -0.6441874, 0.61953884, -0.69732296, 0.32589355, 0.15396628 -1.173605, -0.04927024, -0.651315, 0.619715, -0.69767636, 0.3251473, 0.15323262 -1.1783162, -0.04972869, -0.6584915, 0.6200629, -0.69795287, 0.32420033, 0.15257096 -1.182919, -0.050357413, -0.6658072, 0.6204367, -0.6981318, 0.32320768, 0.15233822 -1.1874717, -0.051133975, -0.67320967, 0.62075424, -0.69835657, 0.32205468, 0.15245679 -1.191978, -0.05202742, -0.6806192, 0.62105525, -0.69862306, 0.32071388, 0.15283613 -1.1977433, -0.05374855, -0.68855596, 0.6212432, -0.69899905, 0.31923452, 0.15344834 -1.2025316, -0.055098895, -0.69593453, 0.621355, -0.69936925, 0.31794623, 0.15398283 -1.2069595, -0.056379616, -0.7029567, 0.62134856, -0.69998586, 0.3165071, 0.15417176 -1.2112751, -0.057651743, -0.7097144, 0.6212308, -0.7009544, 0.31475383, 0.1538348 -1.2156396, -0.058891453, -0.7163084, 0.6210326, -0.7021733, 0.31288663, 0.15288265 -1.2202166, -0.06007523, -0.7228669, 0.62095755, -0.7032519, 0.31125602, 0.15155232 -1.2263527, -0.061299015, -0.72961473, 0.6209597, -0.704039, 0.31014466, 0.15016122 -1.2317744, -0.06251502, -0.7363025, 0.6210499, -0.70423776, 0.30994582, 0.149264 -1.2369866, -0.06377305, -0.7430714, 0.621064, -0.7038149, 0.31087735, 0.14926259 -1.2421044, -0.06508094, -0.74989647, 0.62091434, -0.7028877, 0.3128849, 0.15005715 -1.2471812, -0.06635024, -0.756607, 0.620829, -0.70171136, 0.31524742, 0.15096845 -1.2522709, -0.067537814, -0.7630933, 0.6208458, -0.70063686, 0.31724042, 0.15171316 -1.2637718, -0.06994397, -0.77693546, 0.6206552, -0.6993111, 0.31917515, 0.15452613 -1.2692062, -0.071164444, -0.7832574, 0.6205875, -0.6988156, 0.31939146, 0.1565786 -1.2747775, -0.0722384, -0.7894035, 0.6208899, -0.69803965, 0.31959867, 0.15840764 -1.2805649, -0.073049955, -0.7953801, 0.62183356, -0.69676995, 0.31991252, 0.15965788 -1.2865036, -0.073589474, -0.8011981, 0.62327015, -0.6950828, 0.3204368, 0.16035688 -1.2926227, -0.073533095, -0.80618995, 0.6247738, -0.69325304, 0.32128352, 0.16073205 -1.2985878, -0.073657215, -0.81147003, 0.6262343, -0.6914606, 0.3221764, 0.16098182 -1.3044329, -0.07378999, -0.81673056, 0.6275866, -0.68977875, 0.32311, 0.16105966 -1.3102341, -0.073935196, -0.82185245, 0.62876505, -0.6882695, 0.32416362, 0.16080312 -1.3160344, -0.074145, -0.8268128, 0.6296543, -0.68703663, 0.32532302, 0.16025329 -1.3218296, -0.07447151, -0.8316663, 0.6301102, -0.6862007, 0.3265076, 0.15963277 -1.328208, -0.07539575, -0.8364149, 0.629976, -0.68589246, 0.3276224, 0.15920235 -1.3342136, -0.07623514, -0.84115964, 0.62931836, -0.68607444, 0.32854617, 0.1591151 -1.3401319, -0.07713272, -0.84590954, 0.62823117, -0.6865536, 0.32951155, 0.15934846 -1.346111, -0.07809834, -0.8506179, 0.62688863, -0.68707263, 0.33085388, 0.15961675 -1.3522385, -0.0790829, -0.85523504, 0.6255286, -0.6874885, 0.33256868, 0.15959847 -1.3585519, -0.0800366, -0.8597402, 0.62425727, -0.687826, 0.33445773, 0.15917394 -1.3651316, -0.08077489, -0.8637722, 0.6230718, -0.6882836, 0.336177, 0.15821587 -1.371887, -0.081481755, -0.86782193, 0.6221639, -0.6888089, 0.3375214, 0.15663193 -1.3788283, -0.0821165, -0.8718403, 0.621406, -0.689393, 0.33861262, 0.1547037 -1.3859718, -0.08272067, -0.87587583, 0.6206846, -0.6898959, 0.33981064, 0.15271822 -1.3932989, -0.083357714, -0.8799704, 0.6198524, -0.6903154, 0.34128246, 0.15090959 -1.4007558, -0.08401651, -0.88409984, 0.61889243, -0.6907481, 0.3428912, 0.14921427 -1.4083424, -0.08467485, -0.88824475, 0.6177783, -0.69127357, 0.34453174, 0.14760984 -1.415599, -0.08552025, -0.89118195, 0.6163985, -0.69196486, 0.34615898, 0.14632607 -1.423184, -0.08639033, -0.8949492, 0.61459607, -0.69281036, 0.34799883, 0.14554068 -1.4308351, -0.087372616, -0.89892966, 0.61240166, -0.6936875, 0.35032272, 0.14503743 -1.4385264, -0.088419504, -0.9028416, 0.6100668, -0.6945157, 0.3530489, 0.1443015 -1.4462909, -0.089475535, -0.9066464, 0.60774505, -0.6952717, 0.3560126, 0.14317173 -1.454283, -0.09078163, -0.9101364, 0.60518974, -0.69593495, 0.35952652, 0.14198767 -1.4621347, -0.092109516, -0.913849, 0.60222894, -0.69629943, 0.36406264, 0.14122912 -1.4699831, -0.09347765, -0.9176801, 0.59910834, -0.6961993, 0.3694389, 0.14103357 -1.4778281, -0.09488496, -0.9215373, 0.59578997, -0.6959349, 0.37509814, 0.14145727 -1.4856293, -0.09629487, -0.9252892, 0.59219974, -0.69587004, 0.3805478, 0.14229469 -1.4933755, -0.09762798, -0.92886025, 0.58848363, -0.6961362, 0.38541546, 0.1433047 -1.5007423, -0.098732166, -0.9320188, 0.58479196, -0.69683295, 0.3894759, 0.14405066 -1.508081, -0.099703, -0.9349578, 0.5811727, -0.69800645, 0.39265946, 0.14437374 -1.5152857, -0.1005619, -0.9376865, 0.57773054, -0.6994968, 0.39509082, 0.14434278 -1.5223386, -0.10135399, -0.94029975, 0.57447505, -0.7009573, 0.39733607, 0.14408787 -1.5292666, -0.10206765, -0.9428522, 0.5714634, -0.702078, 0.39992204, 0.14345178 -1.5360508, -0.102715604, -0.9453805, 0.5687091, -0.70278925, 0.40297562, 0.14236514 -1.5429409, -0.10376324, -0.94772965, 0.5662639, -0.7030131, 0.406514, 0.14093985 -1.5494083, -0.104476415, -0.9502246, 0.5638909, -0.7029855, 0.4104076, 0.13929777 -1.5555668, -0.105076306, -0.95283544, 0.5616709, -0.7028179, 0.41433492, 0.13747458 -1.5614637, -0.10564593, -0.9555582, 0.5596067, -0.7025197, 0.41823202, 0.13560334 -1.5671228, -0.106175646, -0.95837784, 0.5577499, -0.7021118, 0.4219914, 0.1337056 -1.5725856, -0.106642045, -0.9612789, 0.5562769, -0.70167154, 0.42528746, 0.13169572 -1.577875, -0.107030556, -0.96425486, 0.55517954, -0.70128506, 0.4280243, 0.12949951 -1.5841856, -0.10801419, -0.96800435, 0.55444473, -0.7009887, 0.4302932, 0.1267022 -1.5896425, -0.1085663, -0.9713915, 0.5540839, -0.7005845, 0.43231368, 0.12360114 -1.594761, -0.10903163, -0.9747801, 0.5540006, -0.70003426, 0.43423742, 0.12030464 -1.5996634, -0.10957913, -0.97833866, 0.553929, -0.6993308, 0.43632406, 0.117134176 -1.6043425, -0.110312745, -0.98215705, 0.55358243, -0.698557, 0.4386859, 0.11453846 -1.6089774, -0.11231079, -0.9860379, 0.5528614, -0.6980011, 0.4410378, 0.11235836 -1.6132575, -0.113706306, -0.9901184, 0.55187833, -0.69761467, 0.44332066, 0.11059312 -1.6173711, -0.1148713, -0.9942884, 0.55097306, -0.69726956, 0.44545516, 0.10868973 -1.6213982, -0.1158927, -0.9984687, 0.5502193, -0.6969731, 0.4473949, 0.106418915 -1.6253633, -0.11680977, -1.0026414, 0.54960775, -0.6967642, 0.44910303, 0.10371802 -1.6292471, -0.11773279, -1.0069025, 0.54900146, -0.69665414, 0.45062846, 0.10101625 -1.6329547, -0.1191699, -1.0110499, 0.5480886, -0.6967013, 0.45217273, 0.09872171 -1.636476, -0.12056595, -1.0155336, 0.5468259, -0.6969659, 0.4536494, 0.09706736 -1.6398731, -0.12199487, -1.0201609, 0.5453609, -0.6974763, 0.45495024, 0.095542856 -1.6432062, -0.123443305, -1.024867, 0.5438344, -0.6982613, 0.45591924, 0.09387673 -1.6464977, -0.124930255, -1.0296576, 0.5422472, -0.6993216, 0.4565404, 0.09213118 -1.6497412, -0.12649487, -1.0345829, 0.5405608, -0.7005954, 0.45689327, 0.09060205 -1.6543982, -0.1291207, -1.0401541, 0.53874874, -0.70206237, 0.45701784, 0.08940269 -1.6580257, -0.13111842, -1.0454752, 0.53682804, -0.7034923, 0.4572493, 0.08852759 -1.6613307, -0.13279088, -1.0507287, 0.5350242, -0.7047322, 0.45763326, 0.08759734 -1.6645529, -0.1342204, -1.0559453, 0.5334042, -0.705692, 0.45830193, 0.08624344 -1.6677554, -0.13546878, -1.0611557, 0.5318646, -0.7063983, 0.45936224, 0.08430744 -1.67097, -0.13660432, -1.066471, 0.53031325, -0.70673573, 0.46103227, 0.08210841 -1.6741958, -0.1376785, -1.0720218, 0.52868944, -0.70660675, 0.4634053, 0.08031075 -1.6779921, -0.13987297, -1.0780562, 0.52689034, -0.7061905, 0.4662436, 0.07936263 -1.681256, -0.14131477, -1.0840908, 0.52462876, -0.7059599, 0.46907723, 0.07969798 -1.6842399, -0.14252388, -1.0901304, 0.52193284, -0.7062045, 0.47144583, 0.08124016 -1.6870872, -0.1436187, -1.0960497, 0.5188475, -0.7068454, 0.47350323, 0.08343414 -1.6899544, -0.14450368, -1.1017097, 0.5157024, -0.7077007, 0.47529694, 0.08546063 -1.6943386, -0.14567283, -1.10838, 0.5129459, -0.70858985, 0.47674346, 0.08661744 -1.6980275, -0.14599663, -1.1137401, 0.5108298, -0.709522, 0.47762287, 0.086647116 -1.7017006, -0.1457142, -1.1184907, 0.5096482, -0.7103766, 0.47782952, 0.08545575 -1.7055993, -0.14491554, -1.1229087, 0.5095601, -0.7108424, 0.47768876, 0.0828556 -1.7097721, -0.14368048, -1.127119, 0.51051414, -0.7106793, 0.4775931, 0.07883582 -1.7141438, -0.1421569, -1.1312644, 0.51220405, -0.7099019, 0.47772408, 0.073930144 -1.7185359, -0.14055838, -1.1355145, 0.51398546, -0.7086389, 0.47840554, 0.0691225 -1.7231383, -0.13952741, -1.1405973, 0.51512873, -0.70720387, 0.47986132, 0.065099776 -1.7271414, -0.13842715, -1.1453598, 0.51534396, -0.7056849, 0.48219955, 0.06255392 -1.7307858, -0.13738234, -1.1499523, 0.5148577, -0.70448774, 0.4846663, 0.060967162 -1.7342215, -0.13637993, -1.1542974, 0.51386833, -0.70390266, 0.4867214, 0.059687164 -1.7375808, -0.13541143, -1.1582948, 0.5126495, -0.7041024, 0.48790413, 0.058136154 -1.7412213, -0.13497315, -1.1624424, 0.51126534, -0.7050948, 0.48815662, 0.056144785 -1.7447999, -0.13426399, -1.1659855, 0.50989735, -0.706385, 0.4879975, 0.05369695 -1.748482, -0.13348114, -1.1692278, 0.5084995, -0.7076091, 0.48798704, 0.050855238 -1.7523359, -0.13269188, -1.1723443, 0.50710356, -0.7084851, 0.48848444, 0.047725048 -1.7563614, -0.13193446, -1.17543, 0.5055766, -0.7088843, 0.4897981, 0.04442184 -1.7605911, -0.13118365, -1.178516, 0.50390387, -0.70884025, 0.4918928, 0.04083777 -1.7650428, -0.13103366, -1.1807948, 0.50229836, -0.708554, 0.49426368, 0.036753718 -1.7697015, -0.13042821, -1.1835709, 0.5007533, -0.70829755, 0.4964976, 0.032418378 -1.7744707, -0.12971342, -1.1864994, -0.49906772, 0.7082637, -0.49849743, -0.028180964 -1.7793067, -0.12902611, -1.189491, -0.49715978, 0.7085367, -0.5002226, -0.024193544 -1.7842011, -0.1284201, -1.1925126, -0.49497026, 0.709019, -0.5018758, -0.02042306 -1.7891855, -0.12790799, -1.195496, -0.4924926, 0.70967484, -0.50352156, -0.016692411 -1.7943017, -0.12741686, -1.1983852, -0.48985887, 0.7105086, -0.5050291, -0.01270802 -1.7991167, -0.12763362, -1.2000203, -0.48710957, 0.71148133, -0.50641096, -0.008154228 -1.804388, -0.12736124, -1.2023715, -0.48436558, 0.7121622, -0.50813884, -0.0031442135 -1.8099365, -0.12688112, -1.2050034, -0.48170635, 0.71236616, -0.5103804, 0.0022730054 -1.8157166, -0.12623498, -1.2078142, -0.47928587, 0.7119273, -0.5132055, 0.008043294 -1.8216295, -0.12554069, -1.2108046, -0.47696608, 0.71102786, -0.5164846, 0.013649013 -1.827509, -0.12493232, -1.2140299, -0.47462898, 0.7100477, -0.51982963, 0.018347815 -1.83258, -0.12502843, -1.2159954, -0.47192207, 0.7095397, -0.52283347, 0.022091262 -1.8379282, -0.12514517, -1.2187839, -0.4685852, 0.7096557, -0.52552533, 0.025293177 -1.8433524, -0.12536837, -1.2217448, -0.4649766, 0.7100638, -0.5279995, 0.028680341 -1.8488448, -0.1256653, -1.2247496, -0.46131164, 0.71034956, -0.5306025, 0.0324959 -1.8543549, -0.12609123, -1.2278138, -0.45740366, 0.71043956, -0.53360534, 0.036369286 -1.8591818, -0.12745407, -1.2301562, -0.4533879, 0.7104203, -0.5367917, 0.03996221 -1.8644124, -0.12821326, -1.2331847, -0.44963047, 0.7103322, -0.5398113, 0.0431788 -1.8697935, -0.12864365, -1.2364968, -0.44663346, 0.71019614, -0.5422155, 0.046283618 -1.875234, -0.12887189, -1.2399429, -0.4444227, 0.71000814, -0.544009, 0.049305864 -1.8806721, -0.12898397, -1.2434763, -0.4427092, 0.70976186, -0.54544836, 0.05227418 -1.8860515, -0.12906273, -1.2471111, -0.44113946, 0.7094374, -0.546872, 0.055004355 -1.8908023, -0.1299609, -1.2499156, -0.43957362, 0.7090866, -0.54832315, 0.057557438 -1.8957523, -0.13031256, -1.2533672, -0.43804374, 0.70865554, -0.5498783, 0.059656147 -1.9006593, -0.13051172, -1.2570581, -0.43657026, 0.7081448, -0.5515255, 0.061295606 -1.9054753, -0.13067447, -1.2608641, -0.4351614, 0.7074991, -0.5533345, 0.06245319 -1.9102464, -0.13078554, -1.2647085, -0.43398508, 0.70671076, -0.55514544, 0.06348494 -1.9150444, -0.1307975, -1.2685435, -0.4333715, 0.70576715, -0.5566798, 0.064725764 -1.9197794, -0.1303999, -1.2719738, -0.43349344, 0.7048387, -0.55758184, 0.06624435 -1.9244913, -0.13029155, -1.2756386, -0.43396792, 0.70418924, -0.557848, 0.06778627 -1.9290748, -0.13037619, -1.2793883, -0.43450245, 0.7038338, -0.5577325, 0.06899258 -1.9335127, -0.13057235, -1.2831795, -0.43493286, 0.7036158, -0.55757344, 0.06978541 -1.937826, -0.13079047, -1.2869701, -0.4353245, 0.7034442, -0.5574111, 0.0703677 -1.9420397, -0.13098952, -1.2907337, -0.43582103, 0.7032574, -0.5571868, 0.07093554 -1.9452947, -0.13156483, -1.2939812, -0.43643102, 0.703027, -0.5569179, 0.071578294 -1.9490405, -0.13187654, -1.2975358, -0.43720052, 0.7027139, -0.55660856, 0.07235784 -1.9528774, -0.13210672, -1.3011869, -0.43805167, 0.7024384, -0.5561848, 0.0731396 -1.9566147, -0.13231719, -1.3048699, -0.43881097, 0.70210683, -0.5559476, 0.0735745 -1.9601831, -0.13253549, -1.3085517, -0.43936825, 0.7016342, -0.5561087, 0.07353856 -1.9636297, -0.13271764, -1.312166, -0.43993092, 0.7011061, -0.556345, 0.07342526 -1.9668174, -0.13376594, -1.3157275, -0.44080004, 0.7007341, -0.55609494, 0.07365778 -1.9700565, -0.13417684, -1.3190432, -0.4417672, 0.7007871, -0.5551889, 0.07419179 -1.9732336, -0.13440078, -1.3221904, -0.4427626, 0.701012, -0.5540266, 0.07481949 -1.9762975, -0.13459739, -1.3252485, -0.44359952, 0.7011049, -0.5531801, 0.0752542 -1.979225, -0.13481544, -1.3282629, -0.444181, 0.70103574, -0.5527788, 0.07541614 -1.9820224, -0.13505088, -1.331213, -0.44458258, 0.70090383, -0.5526189, 0.07544731 -1.98495, -0.1350668, -1.3342552, -0.44478762, 0.70080066, -0.55257565, 0.07551472 -1.9875932, -0.1352242, -1.3369927, -0.4448719, 0.700789, -0.5524916, 0.07574184 -1.9901544, -0.13533087, -1.3395182, -0.4451342, 0.70073795, -0.55226034, 0.07635614 -1.9926981, -0.13534716, -1.3419379, -0.44555828, 0.70052654, -0.5520665, 0.0772201 -1.9951885, -0.13531736, -1.3442937, -0.4459367, 0.70020527, -0.5520469, 0.07808474 -1.9975817, -0.13527085, -1.3465723, -0.44612363, 0.6999148, -0.55215967, 0.07881971 -1.9999384, -0.13594504, -1.3486266, -0.44637135, 0.69968957, -0.55214536, 0.07951419 -2.002161, -0.13602242, -1.3505982, -0.44666287, 0.6995966, -0.5519284, 0.08019866 -2.0042944, -0.1358431, -1.3524541, -0.44695497, 0.69952095, -0.5516779, 0.08095113 -2.0063677, -0.13552058, -1.3542082, -0.44721296, 0.69942236, -0.5514665, 0.081813574 -2.0084133, -0.13508831, -1.355877, -0.44754592, 0.69930875, -0.5511934, 0.082798734 -2.0104094, -0.13459559, -1.3574548, -0.4480015, 0.699276, -0.5507104, 0.08381935 -2.0124297, -0.1341286, -1.3593422, -0.4484969, 0.6993306, -0.55010515, 0.08468406 -2.014217, -0.13369286, -1.3609251, -0.44886023, 0.69945866, -0.54956776, 0.08518974 -2.0158749, -0.13328795, -1.3623806, -0.4491219, 0.69960976, -0.5491227, 0.08543941 -2.017487, -0.13287647, -1.3637655, -0.44939083, 0.699685, -0.5487704, 0.08567201 -2.0190973, -0.1324155, -1.3650718, -0.44978943, 0.6996419, -0.5484447, 0.08601747 -2.0207024, -0.13191229, -1.3662778, -0.45031703, 0.6995609, -0.5480394, 0.08649814 -2.0219994, -0.13155149, -1.3680182, -0.45089242, 0.6995753, -0.54747623, 0.08694948 -2.0233955, -0.13117096, -1.3693199, -0.4514485, 0.6998398, -0.5466493, 0.08713807 -2.0247905, -0.13080034, -1.370445, -0.45194685, 0.7002839, -0.5456692, 0.08712913 -2.0261445, -0.13047081, -1.3715109, -0.45230702, 0.7007897, -0.54475474, 0.08691612 -2.0274389, -0.13019131, -1.3725766, -0.45245993, 0.70126325, -0.54409057, 0.086459726 -2.028684, -0.1299299, -1.3736497, -0.4524812, 0.7016561, -0.5436645, 0.08583828 -2.0299778, -0.12994458, -1.3746587, -0.452515, 0.70204014, -0.5432343, 0.0852424 -2.0311809, -0.12971367, -1.3756251, -0.45264044, 0.7023575, -0.5428004, 0.084724024 -2.0323925, -0.12930948, -1.376526, -0.45294374, 0.7025233, -0.5423671, 0.08450413 -2.0336332, -0.12877773, -1.3773735, -0.45338976, 0.702555, -0.54194987, 0.08452489 -2.0349002, -0.12814549, -1.378163, -0.4539752, 0.7025131, -0.54147804, 0.08475411 -2.0361762, -0.12744287, -1.3789237, -0.45472273, 0.70246434, -0.54086703, 0.085051164 -2.0374374, -0.12670366, -1.3796891, -0.45552087, 0.7024358, -0.5401972, 0.08527388 -2.0384517, -0.12606773, -1.38058, -0.45632693, 0.70252603, -0.5394077, 0.08521891 -2.0395267, -0.1253757, -1.381429, -0.45708072, 0.70266813, -0.538607, 0.08507095 -2.0405746, -0.12466467, -1.3822591, -0.4578045, 0.7028684, -0.53776586, 0.08484642 -2.0415723, -0.12393267, -1.3830631, -0.45854488, 0.70309776, -0.5368754, 0.08458733 -2.042534, -0.123168185, -1.3838434, -0.45928872, 0.7033757, -0.5359179, 0.08431134 -2.0432587, -0.12191581, -1.3848467, -0.46000552, 0.7037097, -0.53489554, 0.084108435 -2.0441444, -0.12088709, -1.385649, -0.46102512, 0.7040154, -0.53360975, 0.084135786 -2.0450766, -0.11991449, -1.3863714, -0.46218765, 0.7042609, -0.5322453, 0.084345475 -2.0460134, -0.11896756, -1.3870822, -0.46338293, 0.70437133, -0.531009, 0.08465558 -2.0469322, -0.11805712, -1.3878251, -0.4645254, 0.70443, -0.52988356, 0.08495788 -2.0477877, -0.117203206, -1.3885932, -0.4655373, 0.7045188, -0.5288452, 0.085152075 -2.0484536, -0.11688403, -1.3893005, -0.46649346, 0.7046401, -0.5278279, 0.08522814 -2.0491138, -0.1163395, -1.3900042, -0.46727103, 0.704787, -0.5269437, 0.0852253 -2.0497584, -0.115736105, -1.3906808, -0.46785358, 0.7049107, -0.52626663, 0.08518997 -2.0504127, -0.11507973, -1.3912883, -0.46836674, 0.7049867, -0.52568364, 0.085340366 -2.051062, -0.11439026, -1.391828, -0.4688789, 0.705034, -0.52511007, 0.08566753 -2.0516706, -0.113714874, -1.392359, -0.4693381, 0.7050939, -0.5245784, 0.08591734 -2.0519805, -0.113010585, -1.3929893, -0.46961716, 0.70518523, -0.5242083, 0.08590192 -2.0523965, -0.112398386, -1.393525, -0.4697634, 0.7052738, -0.5239725, 0.08581417 -2.052857, -0.11174493, -1.3939847, -0.46995008, 0.7052675, -0.523807, 0.08585393 -2.053376, -0.11098738, -1.3943638, -0.47035748, 0.7051586, -0.5235291, 0.08621177 -2.0539358, -0.11014381, -1.3946838, -0.4709705, 0.70496297, -0.52314293, 0.0868068 -2.054474, -0.10927819, -1.3949672, -0.47162062, 0.7047264, -0.52277917, 0.08738798 -2.054632, -0.10901921, -1.3955598, -0.47216853, 0.7045469, -0.52244586, 0.08786714 -2.0549543, -0.10840038, -1.39589, -0.47265175, 0.7043931, -0.52214086, 0.088314444 -2.0553067, -0.10769105, -1.3961065, -0.47304022, 0.7043031, -0.52183974, 0.08873201 -2.0556207, -0.10700963, -1.3962891, -0.4733741, 0.70432746, -0.52146065, 0.088986225 -2.0558805, -0.10638579, -1.3964697, -0.473674, 0.7044547, -0.521009, 0.08902858 -2.0561032, -0.10579379, -1.3966473, -0.4739729, 0.7046645, -0.52047247, 0.08891537 -2.056013, -0.105226934, -1.3967279, -0.47434568, 0.70484644, -0.51993227, 0.08864608 -2.0560668, -0.104621604, -1.3968021, -0.4746491, 0.7050552, -0.51940733, 0.088438846 -2.0561614, -0.10396038, -1.3967986, -0.47495505, 0.7051834, -0.5189664, 0.08836222 -2.0562918, -0.103218675, -1.3966844, -0.47532627, 0.7052663, -0.5184985, 0.088451356 -2.0564482, -0.10240236, -1.3964583, -0.4757964, 0.7054342, -0.5177963, 0.0886976 -2.056581, -0.101558246, -1.3961505, -0.47632286, 0.7058034, -0.5167662, 0.08894215 -2.0562139, -0.1006517, -1.3958586, -0.47685274, 0.7063386, -0.51553243, 0.089014694 -2.0560427, -0.09977273, -1.3955123, -0.47735518, 0.7069287, -0.51426965, 0.08894111 -2.0558953, -0.098864146, -1.3951439, -0.4777849, 0.70742303, -0.51321787, 0.088777944 -2.0557287, -0.09788605, -1.3947362, -0.47819495, 0.70772344, -0.5124372, 0.08868514 -2.0555449, -0.096816964, -1.3942481, -0.47870898, 0.7078587, -0.5117535, 0.08878125 -2.0551763, -0.09573003, -1.3937597, -0.47936904, 0.70791906, -0.51099646, 0.08909841 -2.0549502, -0.09452125, -1.3931365, -0.4801483, 0.70793325, -0.5101447, 0.089669354 -2.0547552, -0.093238614, -1.3924121, -0.48094824, 0.7079061, -0.50929165, 0.090441555 -2.0545213, -0.09192032, -1.3915813, -0.48172805, 0.707835, -0.50849044, 0.09135239 -2.0542107, -0.09059931, -1.39066, -0.4824064, 0.7077909, -0.50773776, 0.09229491 -2.0538108, -0.08929488, -1.3896991, -0.48295107, 0.707854, -0.5069799, 0.09312584 -2.0532975, -0.088005535, -1.3887255, -0.48330122, 0.7080297, -0.50628847, 0.0937328 -2.0524426, -0.086187005, -1.3882532, -0.48349032, 0.7082654, -0.5057184, 0.09405375 -2.0516279, -0.084643826, -1.3873633, -0.48361966, 0.708472, -0.5052525, 0.09433645 -2.0508456, -0.08307859, -1.3862388, -0.48388797, 0.7086288, -0.5046922, 0.094781116 -2.0500872, -0.08141726, -1.3849366, -0.48433498, 0.70883167, -0.50385785, 0.09541737 -2.0493197, -0.07965262, -1.3834684, -0.48493853, 0.7091096, -0.5027264, 0.09625126 -2.0484762, -0.077825464, -1.381833, -0.48555234, 0.7095134, -0.5013862, 0.0971664 -2.0471387, -0.07614048, -1.3803791, -0.48601198, 0.7100629, -0.49998713, 0.09806025 -2.0459404, -0.074298896, -1.3785332, -0.48641738, 0.71058506, -0.49866936, 0.09897408 -2.0447254, -0.07239079, -1.3764671, -0.48688707, 0.7110598, -0.49733213, 0.09997768 -2.043435, -0.07047622, -1.3742898, -0.48743013, 0.71147615, -0.4960143, 0.10091253 -2.0420494, -0.068602815, -1.3720328, -0.48795366, 0.71181214, -0.4948362, 0.10179265 -2.040585, -0.06676973, -1.3697053, -0.4885177, 0.7120245, -0.49378186, 0.102717824 -2.0389209, -0.06511851, -1.3680985, -0.48913622, 0.71203405, -0.49299446, 0.10348773 -2.0371795, -0.06346777, -1.365942, -0.48956317, 0.7120564, -0.49241713, 0.104062326 -2.0353284, -0.06186052, -1.3635417, -0.48989254, 0.7119967, -0.49204794, 0.10466574 -2.0333838, -0.060306605, -1.3609716, -0.49016407, 0.7118253, -0.4918425, 0.10552204 -2.0313816, -0.058786057, -1.3582588, -0.49041656, 0.71149945, -0.49181688, 0.10665924 -2.0293245, -0.057280127, -1.3554242, -0.49077144, 0.7111326, -0.49171412, 0.107939705 -2.027201, -0.055938475, -1.3527508, -0.49132723, 0.71070457, -0.49149895, 0.10920241 -2.0248954, -0.054586325, -1.3498307, -0.4917767, 0.71044487, -0.49120414, 0.11019142 -2.022366, -0.053291764, -1.3467886, -0.4919784, 0.71035755, -0.49098888, 0.11081194 -2.019672, -0.052016433, -1.3436264, -0.49198142, 0.7102131, -0.4910608, 0.111404344 -2.0168884, -0.050738916, -1.3403288, -0.4919662, 0.7097814, -0.49149022, 0.112325154 -2.0140376, -0.049504187, -1.336921, -0.4919251, 0.709234, -0.49206838, 0.113426045 -2.010939, -0.048258215, -1.3339889, -0.49182627, 0.70892775, -0.49242288, 0.11422797 -2.007853, -0.047098193, -1.3305895, -0.49158686, 0.70910543, -0.49226058, 0.114852965 -2.0047145, -0.045935813, -1.3269023, -0.4913832, 0.70952576, -0.49170318, 0.115515016 -2.0015175, -0.04472344, -1.3229648, -0.49128416, 0.70998085, -0.49093708, 0.11639499 -1.9982456, -0.04343793, -1.3187823, -0.4912275, 0.7103498, -0.4901925, 0.117515594 -1.9948977, -0.042089004, -1.3143489, -0.49121487, 0.7106367, -0.48945537, 0.11889828 -1.9910502, -0.040422752, -1.3098408, -0.49108738, 0.71091163, -0.48882478, 0.120366454 -1.9873631, -0.03894384, -1.3050816, -0.4908873, 0.7111268, -0.48835534, 0.12180873 -1.9837122, -0.03747309, -1.3000889, -0.4906243, 0.71112615, -0.48818305, 0.12355016 -1.9800578, -0.035954706, -1.2948945, -0.49046138, 0.710873, -0.48817575, 0.12566438 -1.9763285, -0.034422815, -1.289545, -0.49036312, 0.7105055, -0.4882162, 0.12794884 -1.9724368, -0.03294592, -1.2841122, -0.4901437, 0.7100625, -0.48850334, 0.13013378 -1.9681205, -0.031842157, -1.2785045, -0.48978713, 0.7095644, -0.48912168, 0.13185902 -1.9635911, -0.030669965, -1.2730104, -0.48920983, 0.7092029, -0.4899219, 0.13297155 -1.9587939, -0.029528128, -1.2675369, -0.48852655, 0.70903087, -0.49070477, 0.13351327 -1.953703, -0.028449379, -1.2620238, -0.4877697, 0.7090559, -0.4913985, 0.13359609 -1.9482965, -0.027461926, -1.2564108, -0.48685136, 0.7092335, -0.4921104, 0.13338245 -1.9426444, -0.026521422, -1.2506326, -0.48600036, 0.70953107, -0.49261892, 0.13302545 -1.9363644, -0.025656752, -1.2439054, -0.48553345, 0.7099086, -0.49260816, 0.13275608 -1.9302965, -0.024679722, -1.2374179, -0.4853928, 0.71033204, -0.4921814, 0.13258843 -1.9242084, -0.023650065, -1.2308496, -0.48548496, 0.7106934, -0.49153948, 0.13269536 -1.91797, -0.022642065, -1.224127, -0.48553634, 0.7109756, -0.4910075, 0.13296473 -1.9115006, -0.021712191, -1.217274, -0.48543885, 0.7113323, -0.49055317, 0.13308999 -1.9047765, -0.020864557, -1.2103091, -0.48528463, 0.7118736, -0.48999608, 0.13281092 -1.8977373, -0.01958729, -1.2029964, -0.4850905, 0.7126126, -0.48926073, 0.13226657 -1.8905764, -0.018661441, -1.1956769, -0.48490906, 0.7134275, -0.48841858, 0.13164984 -1.8832661, -0.01785519, -1.1883183, -0.48472223, 0.7141138, -0.48773572, 0.13114765 -1.8758243, -0.017086022, -1.1809347, -0.48460513, 0.7145447, -0.48733523, 0.13072148 -1.868274, -0.016339354, -1.1735215, -0.48470834, 0.71491593, -0.48685628, 0.13009177 -1.8606032, -0.015609294, -1.1660527, -0.48498106, 0.7154356, -0.4860882, 0.12908696 -1.8523535, -0.014869222, -1.158537, -0.4853259, 0.71606386, -0.48514476, 0.12785077 -1.844392, -0.014061691, -1.1509553, -0.4859018, 0.7165664, -0.484137, 0.1266619 -1.836619, -0.013137022, -1.1433294, -0.48684242, 0.7166366, -0.48326012, 0.12600026 -1.8289964, -0.012099253, -1.1356745, -0.48808163, 0.71606433, -0.48283735, 0.12608024 -1.821402, -0.011041951, -1.1280477, -0.48940992, 0.714989, -0.48297897, 0.12649079 -1.8137031, -0.010084236, -1.1204995, -0.49075127, 0.71401703, -0.48308676, 0.12637265 -1.8053385, -0.009293698, -1.1133667, -0.49193743, 0.7134061, -0.4829944, 0.12556118 -1.7972678, -0.008515075, -1.1061877, -0.49322754, 0.71289307, -0.48279947, 0.1241556 -1.7893474, -0.007701258, -1.0990891, -0.4947734, 0.7122237, -0.48262757, 0.12250416 -1.7815593, -0.006809932, -1.092072, -0.49673486, 0.7113089, -0.48234478, 0.12098616 -1.7738577, -0.0058740913, -1.085106, -0.49908876, 0.7103236, -0.4817411, 0.11948321 -1.7662262, -0.004923129, -1.0781575, 0.50162035, -0.7093114, 0.48095813, -0.11804028 -1.7588496, -0.004047096, -1.0715412, 0.50431174, -0.70833397, 0.47992176, -0.11665258 -1.75162, -0.0029739588, -1.0647622, 0.50745267, -0.7073407, 0.4783029, -0.11570329 -1.7446325, -0.0017455085, -1.0579575, 0.51099813, -0.7061463, 0.47634006, -0.11549157 -1.7378311, -0.00045923144, -1.0511926, 0.514802, -0.7048696, 0.4740619, -0.11577145 -1.7310997, 0.0007838635, -1.0445079, 0.5185562, -0.7037113, 0.4715906, -0.11615702 -1.7243892, 0.001917921, -1.0380073, 0.5218816, -0.70268565, 0.46940657, -0.11631801 -1.7174613, 0.0033409027, -1.0315089, 0.5245557, -0.70191616, 0.46766815, -0.11593782 -1.7106315, 0.0043479875, -1.0255326, 0.5266577, -0.701297, 0.46650523, -0.11483468 -1.7038302, 0.00514341, -1.0199184, 0.5281562, -0.7008366, 0.46593434, -0.113066785 -1.6971196, 0.005824669, -1.0145556, 0.52933, -0.7004649, 0.46566764, -0.11096057 -1.6905271, 0.0063924417, -1.0094084, 0.530431, -0.70033157, 0.465216, -0.10840931 -1.6840426, 0.006831837, -1.0044638, 0.5314782, -0.70054835, 0.4643842, -0.105404 -1.6777532, 0.006908618, -0.9995795, 0.53261256, -0.70105726, 0.463025, -0.10222708 -1.6716486, 0.007145195, -0.9948937, 0.5340264, -0.70169663, 0.46108517, -0.09918722 -1.6658008, 0.0074763657, -0.99029946, 0.5357322, -0.7021275, 0.4589808, -0.096666954 -1.6602179, 0.007862767, -0.9857484, 0.53759813, -0.7022119, 0.4570649, -0.09475437 -1.6548315, 0.008240499, -0.98123276, 0.5393708, -0.70203894, 0.4555473, -0.0932574 -1.6496112, 0.008608785, -0.97671837, 0.5410583, -0.7016578, 0.4543403, -0.092233315 -1.644321, 0.009287879, -0.9726731, 0.5428232, -0.70094174, 0.45345426, -0.09166702 -1.639322, 0.009694219, -0.9683635, 0.544627, -0.70008224, 0.45270246, -0.09125059 -1.6344886, 0.009976737, -0.96402293, 0.54649144, -0.6991889, 0.4519181, -0.09084038 -1.629663, 0.0101368725, -0.9597532, 0.5482334, -0.69843984, 0.4510733, -0.09030342 -1.6246995, 0.010091668, -0.95561546, 0.5494517, -0.6979615, 0.4505121, -0.08939342 -1.6196009, 0.009833582, -0.9516102, 0.5501177, -0.6976788, 0.45038277, -0.088148385 -1.614124, 0.008975476, -0.9480728, 0.5504382, -0.69748443, 0.45053393, -0.08690349 -1.608868, 0.008379756, -0.94430804, 0.55073285, -0.69724894, 0.45070693, -0.08602584 -1.603693, 0.00784903, -0.9404846, 0.5509767, -0.69702804, 0.4508636, -0.085431024 -1.5985656, 0.0073081017, -0.93667567, 0.55110717, -0.69680494, 0.45111898, -0.08506097 -1.593489, 0.006753622, -0.9328586, 0.5511683, -0.69657207, 0.45140883, -0.0850341 -1.5884517, 0.006207603, -0.92900234, 0.55129504, -0.69621027, 0.45171893, -0.08552752 -1.5831268, 0.005716047, -0.925071, 0.55146194, -0.6956548, 0.45218027, -0.08652861 -1.5779926, 0.005117275, -0.92117596, 0.55145127, -0.6950844, 0.45281294, -0.08786147 -1.5728706, 0.0044335052, -0.91729796, 0.55123585, -0.69444585, 0.45371494, -0.08959222 -1.5676726, 0.003642328, -0.9134914, 0.55086684, -0.69369745, 0.4549226, -0.09151451 -1.5623256, 0.0027136938, -0.90977806, 0.55022603, -0.6928918, 0.45653653, -0.093416356 -1.556789, 0.0016716779, -0.9061284, 0.5491727, -0.6919588, 0.45880255, -0.09540623 -1.5501716, 0.0007592412, -0.90170264, 0.54774225, -0.69085705, 0.46172294, -0.097502664 -1.5439717, -0.00037630647, -0.8978885, 0.54608893, -0.689559, 0.46512553, -0.099767335 -1.5378339, -0.001622194, -0.89427036, 0.5443516, -0.68802744, 0.46886268, -0.102309704 -1.5316521, -0.002929674, -0.89069843, 0.5426632, -0.6863101, 0.47269323, -0.10514765 -1.5253379, -0.00427033, -0.88714975, 0.54126483, -0.6845513, 0.4761585, -0.10814374 -1.5187609, -0.0056594834, -0.88366795, 0.54024345, -0.68292147, 0.4790263, -0.1108565 -1.5115149, -0.0062301382, -0.88006717, 0.53944266, -0.6816191, 0.48130116, -0.11289825 -1.5040939, -0.0076298714, -0.8767979, 0.53864485, -0.6809578, 0.48287433, -0.11397643 -1.4963968, -0.00941588, -0.87367195, 0.5376766, -0.6808805, 0.483981, -0.11431599 -1.4884377, -0.011432432, -0.87055033, 0.53645766, -0.6813388, 0.48470333, -0.11425056 -1.4802921, -0.013564266, -0.8673208, 0.53516895, -0.681944, 0.4852464, -0.11437862 -1.4720715, -0.01569572, -0.8640193, 0.5340027, -0.6824211, 0.48574778, -0.11485438 -1.4634669, -0.017030507, -0.85949904, 0.5330213, -0.6827607, 0.48612943, -0.11577701 -1.4550893, -0.018740363, -0.8557241, 0.53205234, -0.68267345, 0.48691997, -0.117414474 -1.446774, -0.020499095, -0.85216933, 0.5308799, -0.6822703, 0.4883095, -0.1192799 -1.4384135, -0.022197347, -0.84868234, 0.52928305, -0.68162423, 0.490476, -0.121165104 -1.4299554, -0.023834914, -0.84529275, 0.52739704, -0.6807489, 0.49327427, -0.1229375 -1.42139, -0.025437742, -0.8420468, 0.5253675, -0.6797764, 0.4963489, -0.12462179 -1.4117044, -0.02711455, -0.8382445, 0.52327675, -0.6789157, 0.49933085, -0.1261884 -1.4024781, -0.028669769, -0.83488613, 0.5212077, -0.67816836, 0.5021003, -0.12777077 -1.393349, -0.030097246, -0.8316149, 0.5190696, -0.67725396, 0.505052, -0.12967761 -1.3842622, -0.03135729, -0.8283462, 0.51660925, -0.675895, 0.50877774, -0.13200721 -1.3751878, -0.032436647, -0.8250466, 0.51373184, -0.6740994, 0.5133852, -0.13455535 -1.3660663, -0.03339009, -0.8217627, 0.51064736, -0.6722134, 0.51823926, -0.13710055 -1.3563737, -0.03393101, -0.81803805, 0.50767535, -0.6704098, 0.5228089, -0.13959628 ================================================ FILE: test/test_data/robot_with_visuals.urdf ================================================ ================================================ FILE: test/test_data/scene.mtl ================================================ newmtl Box1 # ambient color Ka 1.0 0.5 0.5 # diffuse color Kd 1.0 0.5 0.5 # specular color Ks 0.0 0.0 0.0 # transparency, opaque in this case d 1.0 # illumination model: highlight on illum 2 newmtl Box2 Ka 0.5 1.0 0.5 Kd 0.5 1.0 0.5 Ks 0.0 0.0 0.0 d 1.0 illum 2 ================================================ FILE: test/test_data/scene.obj ================================================ mtllib scene.mtl g box1 # vertices v 0. 0. 0. v 1. 0. 0. v 0. 0. 1. v 1. 0. 1. v 0. 1. 0. v 1. 1. 0. v 0. 1. 1. v 1. 1. 1. usemtl Box1 # faces f 5 8 6 f 5 7 8 f 1 3 5 f 3 7 5 f 1 2 3 f 2 4 3 f 2 6 8 f 2 8 4 f 3 4 8 f 3 8 7 f 1 5 2 f 2 5 6 g box2 # vertices v 2. 0. 0. v 3. 0. 0. v 2. 0. 1. v 3. 0. 1. v 2. 1. 0. v 3. 1. 0. v 2. 1. 1. v 3. 1. 1. usemtl Box2 # faces f 13 16 14 f 13 15 16 f 9 11 13 f 11 15 13 f 9 10 11 f 10 12 11 f 10 14 16 f 10 16 12 f 11 12 16 f 11 16 15 f 9 13 10 f 10 13 14 ================================================ FILE: test/test_data/simple_mechanism.urdf ================================================