├── .gitattributes ├── .github ├── CODEOWNERS ├── CONTRIBUTORS.md ├── workflows │ ├── publish.yml │ ├── sphinx.yml │ └── master.yml └── svg │ ├── sm_powered.min.svg │ └── sm_powered.svg ├── docs ├── figs │ ├── fig1.png │ ├── icon.png │ ├── animate.gif │ ├── animate.mp4 │ ├── classes.pdf │ ├── classes.png │ ├── pose-values.png │ ├── broadcasting.png │ ├── transforms2d.png │ ├── transforms3d.png │ ├── colored_output.png │ ├── CartesianSnakes_LogoW.png │ ├── classes_spatialvec.dot │ └── classes.dot ├── source │ ├── favicon.ico │ ├── modules.rst │ ├── 2d_line.rst │ ├── _static │ │ ├── favicon-16x16.png │ │ ├── favicon-32x32.png │ │ ├── apple-touch-icon.png │ │ ├── android-chrome-192x192.png │ │ └── android-chrome-512x512.png │ ├── 2d_linesegment.rst │ ├── 2d_polygon.rst │ ├── 2d_ellipse.rst │ ├── func_vector.rst │ ├── func_graphics.rst │ ├── func_quat.rst │ ├── func_2d.rst │ ├── func_args.rst │ ├── func_nd.rst │ ├── func_animation.rst │ ├── func_symbolic.rst │ ├── func_3d.rst │ ├── 6d_m6.rst │ ├── func_numeric.rst │ ├── 6d_f6.rst │ ├── 3d_plane.rst │ ├── classes-3d.rst │ ├── func_2d_graphics.rst │ ├── func_3d_graphics.rst │ ├── 3d_line.rst │ ├── classes-2d.rst │ ├── 6d_spatial.rst │ ├── 6d_force.rst │ ├── functions.rst │ ├── 2d_orient_SO2.rst │ ├── 2d_pose_SE2.rst │ ├── 6d_momentum.rst │ ├── 3d_dualquaternion.rst │ ├── 3d_quaternion.rst │ ├── 3d_orient_SO3.rst │ ├── 6d_inertia.rst │ ├── 6d_velocity.rst │ ├── 6d_acceleration.rst │ ├── 3d_pose_SE3.rst │ ├── 2d_pose_twist.rst │ ├── 3d_pose_twist.rst │ ├── 3d_pose_dualquaternion.rst │ ├── indices.rst │ ├── 3d_orient_unitquaternion.rst │ ├── support.rst │ ├── index.rst │ ├── spatialmath.rst │ └── conf.py ├── .buildinfo ├── Makefile └── generated │ ├── spatialmath.pose2d.html │ ├── spatialmath.pose3d.html │ ├── spatialmath.quaternion.html │ ├── spatialmath.base.quaternions.html │ ├── spatialmath.base.vectors.html │ ├── spatialmath.base.transforms2d.html │ ├── spatialmath.base.transforms3d.html │ └── spatialmath.base.transformsNd.html ├── spatialmath ├── base │ ├── types.py │ ├── _types_35.py │ ├── _types_311.py │ ├── _types_39.py │ ├── README.md │ ├── __init__.py │ └── symbolic.py ├── __init__.py └── timing.py ├── Makefile ├── .pre-commit-config.yaml ├── .gitignore ├── LICENSE ├── tests ├── base │ ├── test_transforms3d_plot.py │ ├── test_symbolic.py │ ├── test_numeric.py │ ├── test_graphics.py │ ├── test_quaternions.py │ └── test_transforms2d.py ├── test_dualquaternion.py ├── test_spline.py ├── test_baseposelist.py ├── test_spatialvector.py ├── test_geom2d.py └── test_geom3d.py ├── pyproject.toml └── symbolic └── angvelxform_dot.ipynb /.gitattributes: -------------------------------------------------------------------------------- 1 | * linguist-vendored 2 | *.py linguist-vendored=false 3 | -------------------------------------------------------------------------------- /.github/CODEOWNERS: -------------------------------------------------------------------------------- 1 | # Global Owners 2 | * @petercorke @jhavl @myeatman-bdai 3 | -------------------------------------------------------------------------------- /docs/figs/fig1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bdaiinstitute/spatialmath-python/HEAD/docs/figs/fig1.png -------------------------------------------------------------------------------- /docs/figs/icon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bdaiinstitute/spatialmath-python/HEAD/docs/figs/icon.png -------------------------------------------------------------------------------- /docs/figs/animate.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bdaiinstitute/spatialmath-python/HEAD/docs/figs/animate.gif -------------------------------------------------------------------------------- /docs/figs/animate.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bdaiinstitute/spatialmath-python/HEAD/docs/figs/animate.mp4 -------------------------------------------------------------------------------- /docs/figs/classes.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bdaiinstitute/spatialmath-python/HEAD/docs/figs/classes.pdf -------------------------------------------------------------------------------- /docs/figs/classes.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bdaiinstitute/spatialmath-python/HEAD/docs/figs/classes.png -------------------------------------------------------------------------------- /docs/figs/pose-values.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bdaiinstitute/spatialmath-python/HEAD/docs/figs/pose-values.png -------------------------------------------------------------------------------- /docs/source/favicon.ico: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bdaiinstitute/spatialmath-python/HEAD/docs/source/favicon.ico -------------------------------------------------------------------------------- /docs/figs/broadcasting.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bdaiinstitute/spatialmath-python/HEAD/docs/figs/broadcasting.png -------------------------------------------------------------------------------- /docs/figs/transforms2d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bdaiinstitute/spatialmath-python/HEAD/docs/figs/transforms2d.png -------------------------------------------------------------------------------- /docs/figs/transforms3d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bdaiinstitute/spatialmath-python/HEAD/docs/figs/transforms3d.png -------------------------------------------------------------------------------- /docs/figs/colored_output.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bdaiinstitute/spatialmath-python/HEAD/docs/figs/colored_output.png -------------------------------------------------------------------------------- /docs/source/modules.rst: -------------------------------------------------------------------------------- 1 | spatialmath 2 | =========== 3 | 4 | .. toctree:: 5 | :maxdepth: 4 6 | 7 | spatialmath 8 | 9 | -------------------------------------------------------------------------------- /docs/source/2d_line.rst: -------------------------------------------------------------------------------- 1 | 2D line 2 | ^^^^^^^ 3 | 4 | .. autoclass:: spatialmath.geom2d.Line2 5 | :members: 6 | :undoc-members: -------------------------------------------------------------------------------- /docs/figs/CartesianSnakes_LogoW.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bdaiinstitute/spatialmath-python/HEAD/docs/figs/CartesianSnakes_LogoW.png -------------------------------------------------------------------------------- /docs/source/_static/favicon-16x16.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bdaiinstitute/spatialmath-python/HEAD/docs/source/_static/favicon-16x16.png -------------------------------------------------------------------------------- /docs/source/_static/favicon-32x32.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bdaiinstitute/spatialmath-python/HEAD/docs/source/_static/favicon-32x32.png -------------------------------------------------------------------------------- /docs/source/_static/apple-touch-icon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bdaiinstitute/spatialmath-python/HEAD/docs/source/_static/apple-touch-icon.png -------------------------------------------------------------------------------- /docs/source/2d_linesegment.rst: -------------------------------------------------------------------------------- 1 | 2D line segment 2 | ^^^^^^^^^^^^^^^ 3 | 4 | .. autoclass:: spatialmath.geom2d.LineSegment2 5 | :members: 6 | :undoc-members: -------------------------------------------------------------------------------- /docs/source/_static/android-chrome-192x192.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bdaiinstitute/spatialmath-python/HEAD/docs/source/_static/android-chrome-192x192.png -------------------------------------------------------------------------------- /docs/source/_static/android-chrome-512x512.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bdaiinstitute/spatialmath-python/HEAD/docs/source/_static/android-chrome-512x512.png -------------------------------------------------------------------------------- /docs/source/2d_polygon.rst: -------------------------------------------------------------------------------- 1 | 2D polgon 2 | ^^^^^^^^^ 3 | 4 | .. autoclass:: spatialmath.geom2d.Polygon2 5 | :members: 6 | :undoc-members: 7 | :special-members: __init__, __str__, __len__ 8 | -------------------------------------------------------------------------------- /docs/source/2d_ellipse.rst: -------------------------------------------------------------------------------- 1 | 2D ellipse 2 | ^^^^^^^^^^ 3 | 4 | .. autoclass:: spatialmath.geom2d.Ellipse 5 | :members: 6 | :undoc-members: 7 | :special-members: __init__, __str__, __len__ 8 | -------------------------------------------------------------------------------- /docs/source/func_vector.rst: -------------------------------------------------------------------------------- 1 | Vectors 2 | ======= 3 | 4 | .. automodule:: spatialmath.base.vectors 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: -------------------------------------------------------------------------------- /docs/source/func_graphics.rst: -------------------------------------------------------------------------------- 1 | Graphics and animation 2 | ====================== 3 | 4 | 5 | .. toctree:: 6 | :maxdepth: 2 7 | 8 | func_2d_graphics 9 | func_3d_graphics 10 | func_animation 11 | 12 | -------------------------------------------------------------------------------- /docs/source/func_quat.rst: -------------------------------------------------------------------------------- 1 | Quaternions 2 | =========== 3 | 4 | .. automodule:: spatialmath.base.quaternions 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: -------------------------------------------------------------------------------- /docs/source/func_2d.rst: -------------------------------------------------------------------------------- 1 | Transforms in 2D 2 | ================ 3 | 4 | .. automodule:: spatialmath.base.transforms2d 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: -------------------------------------------------------------------------------- /docs/source/func_args.rst: -------------------------------------------------------------------------------- 1 | Argument checking 2 | ================= 3 | 4 | .. automodule:: spatialmath.base.argcheck 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: -------------------------------------------------------------------------------- /docs/source/func_nd.rst: -------------------------------------------------------------------------------- 1 | Transforms in ND 2 | ================ 3 | 4 | .. automodule:: spatialmath.base.transformsNd 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: -------------------------------------------------------------------------------- /docs/source/func_animation.rst: -------------------------------------------------------------------------------- 1 | Animation support 2 | ================= 3 | 4 | .. automodule:: spatialmath.base.animate 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: -------------------------------------------------------------------------------- /docs/source/func_symbolic.rst: -------------------------------------------------------------------------------- 1 | Symbolic computation 2 | ==================== 3 | 4 | .. automodule:: spatialmath.base.symbolic 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: -------------------------------------------------------------------------------- /docs/source/func_3d.rst: -------------------------------------------------------------------------------- 1 | Transforms in 3D 2 | ================ 3 | 4 | 5 | .. automodule:: spatialmath.base.transforms3d 6 | :members: 7 | :undoc-members: 8 | :show-inheritance: 9 | :inherited-members: 10 | :special-members: -------------------------------------------------------------------------------- /docs/source/6d_m6.rst: -------------------------------------------------------------------------------- 1 | Spatial M6 2 | ^^^^^^^^^^ 3 | 4 | .. autoclass:: spatialmath.spatialvector.SpatialM6 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :exclude-members: count, copy, index, sort, remove, binop, unop, arghandler -------------------------------------------------------------------------------- /docs/source/func_numeric.rst: -------------------------------------------------------------------------------- 1 | Numerical utility functions 2 | =========================== 3 | 4 | .. automodule:: spatialmath.base.numeric 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: -------------------------------------------------------------------------------- /docs/.buildinfo: -------------------------------------------------------------------------------- 1 | # Sphinx build info version 1 2 | # This file hashes the configuration used when building these files. When it is not found, a full rebuild will be done. 3 | config: 8aa8f71f5a36f2a6c49e8bbeb8797bef 4 | tags: 645f666f9bcd5a90fca523b33c5a78b7 5 | -------------------------------------------------------------------------------- /docs/source/6d_f6.rst: -------------------------------------------------------------------------------- 1 | Spatial F6 2 | ^^^^^^^^^^ 3 | 4 | .. autoclass:: spatialmath.spatialvector.SpatialF6 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | 9 | :exclude-members: count, copy, index, sort, remove, binop, unop, arghandler -------------------------------------------------------------------------------- /docs/source/3d_plane.rst: -------------------------------------------------------------------------------- 1 | Plane 2 | ^^^^^ 3 | 4 | .. autoclass:: spatialmath.geom3d.Plane3 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: __mul__, __rmul__, __eq__, __ne__, __init__, __or__, __xor__ 10 | -------------------------------------------------------------------------------- /docs/source/classes-3d.rst: -------------------------------------------------------------------------------- 1 | 2 | Geometry 3 | -------- 4 | 5 | .. automodule:: spatialmath.geom3d 6 | :members: 7 | :undoc-members: 8 | :show-inheritance: 9 | :inherited-members: 10 | :special-members: __mul__, __rmul__, __eq__, __ne__, __init__, __or__, __xor__ -------------------------------------------------------------------------------- /docs/source/func_2d_graphics.rst: -------------------------------------------------------------------------------- 1 | 2D graphics 2 | =========== 3 | 4 | 2d graphical primitives which build on Matplotlib. 5 | 6 | .. automodule:: spatialmath.base.graphics 7 | :members: plot_point, plot_text, plot_homline, plot_box, plot_arrow, plot_circle, plot_ellipse, plot_polygon, plotvol2 8 | -------------------------------------------------------------------------------- /.github/CONTRIBUTORS.md: -------------------------------------------------------------------------------- 1 | A number of people have contributed to this, and earlier, versions of this Toolbox 2 | 3 | * Jesse Haviland, 2020 (part of the ropy project) 4 | * Luis Fernando Lara Tobar, 2008 5 | * Josh Carrigg Hodson, Aditya Dua, Chee Ho Chan, 2017 (part of the robopy project) 6 | * Peter Corke 7 | -------------------------------------------------------------------------------- /docs/source/func_3d_graphics.rst: -------------------------------------------------------------------------------- 1 | 3D graphics 2 | =========== 3 | 4 | 3d graphical primitives which build on Matplotlib ``plot_wireframe`` and ``plot_surface``. 5 | 6 | .. automodule:: spatialmath.base.graphics 7 | :members: plot_cuboid, plot_sphere, plot_ellipsoid, plot_cylinder, plot_cone, plotvol3 8 | -------------------------------------------------------------------------------- /spatialmath/base/types.py: -------------------------------------------------------------------------------- 1 | import sys 2 | 3 | _version = sys.version_info.minor 4 | 5 | 6 | if _version >= 11: 7 | from spatialmath.base._types_311 import * 8 | elif _version >= 9: 9 | from spatialmath.base._types_39 import * 10 | else: 11 | from spatialmath.base._types_35 import * 12 | -------------------------------------------------------------------------------- /docs/source/3d_line.rst: -------------------------------------------------------------------------------- 1 | 3D line 2 | ^^^^^^^ 3 | 4 | A 3D line using Plücker coordinates. 5 | 6 | .. autoclass:: spatialmath.geom3d.Line3 7 | :members: 8 | :undoc-members: 9 | :show-inheritance: 10 | :inherited-members: 11 | :special-members: __mul__, __rmul__, __eq__, __ne__, __init__, __or__, __xor__ -------------------------------------------------------------------------------- /docs/source/classes-2d.rst: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Geometry 6 | -------- 7 | 8 | .. automodule:: spatialmath.geom2d 9 | :members: 10 | :undoc-members: 11 | :show-inheritance: 12 | :inherited-members: 13 | :special-members: __mul__, __rmul__, __eq__, __ne__, __init__, __or__, __xor__ 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /docs/source/6d_spatial.rst: -------------------------------------------------------------------------------- 1 | Spatial vector 2 | ^^^^^^^^^^^^^^ 3 | 4 | .. autoclass:: spatialmath.spatialvector.SpatialVector 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :special-members: __add__, __sub__, __neg__, __init__ 9 | :exclude-members: count, copy, index, sort, remove, binop, unop, arghandler -------------------------------------------------------------------------------- /docs/source/6d_force.rst: -------------------------------------------------------------------------------- 1 | Spatial force 2 | ^^^^^^^^^^^^^ 3 | 4 | .. autoclass:: spatialmath.spatialvector.SpatialForce 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: __add__, __sub__, __init__ 10 | :exclude-members: count, copy, index, sort, remove, binop, unop, arghandler -------------------------------------------------------------------------------- /docs/source/functions.rst: -------------------------------------------------------------------------------- 1 | ****************** 2 | Function reference 3 | ****************** 4 | 5 | 6 | .. toctree:: 7 | :maxdepth: 2 8 | 9 | func_2d 10 | func_3d 11 | func_nd 12 | func_quat 13 | func_symbolic 14 | func_vector 15 | func_graphics 16 | func_args 17 | func_numeric 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /docs/source/2d_orient_SO2.rst: -------------------------------------------------------------------------------- 1 | SO(2) matrix 2 | ^^^^^^^^^^^^ 3 | 4 | .. autoclass:: spatialmath.pose2d.SO2 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: __mul__, __truediv__, __add__, __sub__, __eq__, __ne__, __pow__, __init__ 10 | :exclude-members: count, copy, index, sort, remove -------------------------------------------------------------------------------- /docs/source/2d_pose_SE2.rst: -------------------------------------------------------------------------------- 1 | SE(2) matrix 2 | ^^^^^^^^^^^^ 3 | 4 | .. autoclass:: spatialmath.pose2d.SE2 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: __mul__, __truediv__, __add__, __sub__, __eq__, __ne__, __pow__, __init__ 10 | :exclude-members: count, copy, index, sort, remove -------------------------------------------------------------------------------- /docs/source/6d_momentum.rst: -------------------------------------------------------------------------------- 1 | Spatial momentum 2 | ^^^^^^^^^^^^^^^^ 3 | 4 | .. autoclass:: spatialmath.spatialvector.SpatialMomentum 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: __add__, __sub__, __init__ 10 | :exclude-members: count, copy, index, sort, remove, binop, unop, arghandler -------------------------------------------------------------------------------- /docs/source/3d_dualquaternion.rst: -------------------------------------------------------------------------------- 1 | Dual Quaternion 2 | ^^^^^^^^^^^^^^^ 3 | 4 | .. autoclass:: spatialmath.DualQuaternion.DualQuaternion 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: __mul__, __truediv__, __add__, __sub__, __init__ 10 | :exclude-members: count, copy, index, sort, remove -------------------------------------------------------------------------------- /docs/source/3d_quaternion.rst: -------------------------------------------------------------------------------- 1 | Quaternion 2 | ^^^^^^^^^^ 3 | 4 | .. autoclass:: spatialmath.quaternion.Quaternion 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: __mul__, __truediv__, __add__, __sub__, __eq__, __ne__, __pow__, __init__ 10 | :exclude-members: count, copy, index, sort, remove -------------------------------------------------------------------------------- /docs/source/3d_orient_SO3.rst: -------------------------------------------------------------------------------- 1 | 2 | 3 | SO(3) matrix 4 | ^^^^^^^^^^^^ 5 | 6 | .. autoclass:: spatialmath.pose3d.SO3 7 | :members: 8 | :undoc-members: 9 | :show-inheritance: 10 | :inherited-members: 11 | :special-members: __mul__, __truediv__, __add__, __sub__, __eq__, __ne__, __pow__, __init__ 12 | :exclude-members: count, copy, index, sort, remove -------------------------------------------------------------------------------- /docs/source/6d_inertia.rst: -------------------------------------------------------------------------------- 1 | Spatial inertia 2 | ^^^^^^^^^^^^^^^ 3 | 4 | .. autoclass:: spatialmath.spatialvector.SpatialInertia 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: __mul__, __rmul__, __add__, __sub__, __init__ 10 | :exclude-members: count, copy, index, sort, remove, binop, unop, arghandler -------------------------------------------------------------------------------- /docs/source/6d_velocity.rst: -------------------------------------------------------------------------------- 1 | Spatial velocity 2 | ^^^^^^^^^^^^^^^^ 3 | 4 | .. autoclass:: spatialmath.spatialvector.SpatialVelocity 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: _A, __add__, __sub__, __xor__, __init__ 10 | :exclude-members: count, copy, index, sort, remove, binop, unop, arghandler -------------------------------------------------------------------------------- /docs/source/6d_acceleration.rst: -------------------------------------------------------------------------------- 1 | Spatial acceleration 2 | ^^^^^^^^^^^^^^^^^^^^ 3 | 4 | .. autoclass:: spatialmath.spatialvector.SpatialAcceleration 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: __add__, __sub__, __init__ 10 | :exclude-members: count, copy, index, sort, remove, binop, unop, arghandler -------------------------------------------------------------------------------- /docs/source/3d_pose_SE3.rst: -------------------------------------------------------------------------------- 1 | SE(3) matrix 2 | ^^^^^^^^^^^^ 3 | 4 | .. autoclass:: spatialmath.pose3d.SE3 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: __mul__, __truediv__, __add__, __sub__, __eq__, __ne__, __pow__, __init__ 10 | :exclude-members: count, copy, index, sort, remove, arghandler, binop, unop -------------------------------------------------------------------------------- /docs/source/2d_pose_twist.rst: -------------------------------------------------------------------------------- 1 | se(2) twist 2 | ^^^^^^^^^^^ 3 | 4 | .. autoclass:: spatialmath.twist.Twist2 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: __mul__, __truediv__, __add__, __sub__, __eq__, __ne__, __pow__, __init__ 10 | :exclude-members: count, copy, index, sort, remove, arghandler, binop, unop, __add__, -------------------------------------------------------------------------------- /docs/source/3d_pose_twist.rst: -------------------------------------------------------------------------------- 1 | se(3) twist 2 | ^^^^^^^^^^^ 3 | 4 | .. autoclass:: spatialmath.twist.Twist3 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: __mul__, __truediv__, __add__, __sub__, __eq__, __ne__, __pow__, __init__ 10 | :exclude-members: count, copy, index, sort, remove, arghandler, binop, unop, __add__, 11 | -------------------------------------------------------------------------------- /docs/source/3d_pose_dualquaternion.rst: -------------------------------------------------------------------------------- 1 | Unit dual quaternion 2 | ^^^^^^^^^^^^^^^^^^^^^ 3 | 4 | .. autoclass:: spatialmath.DualQuaternion.UnitDualQuaternion 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: __mul__, __truediv__, __add__, __sub__, __init__ 10 | :exclude-members: count, copy, index, sort, remove, arghandler, binop, unop -------------------------------------------------------------------------------- /docs/source/indices.rst: -------------------------------------------------------------------------------- 1 | Indices 2 | ======= 3 | 4 | * :ref:`genindex` 5 | * :ref:`modindex` 6 | * :ref:`search` 7 | 8 | 9 | .. autosummary:: 10 | :toctree: generated 11 | 12 | spatialmath.pose3d 13 | spatialmath.quaternion 14 | spatialmath.base.transforms2d 15 | spatialmath.base.transforms3d 16 | spatialmath.base.transformsNd 17 | spatialmath.base.vectors 18 | spatialmath.base.quaternions -------------------------------------------------------------------------------- /docs/source/3d_orient_unitquaternion.rst: -------------------------------------------------------------------------------- 1 | Unit quaternion 2 | ^^^^^^^^^^^^^^^ 3 | 4 | .. autoclass:: spatialmath.quaternion.UnitQuaternion 5 | :members: 6 | :undoc-members: 7 | :show-inheritance: 8 | :inherited-members: 9 | :special-members: __mul__, __truediv__, __add__, __sub__, __eq__, __ne__, __pow__, __init__ 10 | :exclude-members: count, copy, index, sort, remove, arghandler, binop, unop, 11 | 12 | -------------------------------------------------------------------------------- /docs/figs/classes_spatialvec.dot: -------------------------------------------------------------------------------- 1 | # dot -Tpdf classes_spatialvec.dot > classes_spatialvec.pdf ; open classes_spatialvec.pdf 2 | digraph G { 3 | graph [rankdir=BT]; 4 | BasePoseList -> "collections.UserList" 5 | SpatialVector -> BasePoseList 6 | SpatialM6 -> SpatialVector 7 | SpatialF6 -> SpatialVector 8 | SpatialVelocity -> SpatialM6 9 | SpatialAcceleration -> SpatialM6 10 | SpatialMomentum -> SpatialF6 11 | SpatialForce -> SpatialF6 12 | SpatialInertia -> BasePoseList 13 | } -------------------------------------------------------------------------------- /docs/source/support.rst: -------------------------------------------------------------------------------- 1 | ======= 2 | Support 3 | ======= 4 | 5 | The easiest way to get help with the project is to join the ``#crawler`` 6 | channel on Freenode_. We hang out there and you can get real-time help with 7 | your projects. The other good way is to open an issue on Github_. 8 | 9 | The mailing list at https://groups.google.com/forum/#!forum/crawler is also available for support. 10 | 11 | .. _Freenode: irc://freenode.net 12 | .. _Github: http://github.com/example/crawler/issues -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line, and also 5 | # from the environment for the first two. 6 | SPHINXOPTS ?= 7 | SPHINXBUILD ?= sphinx-build 8 | SOURCEDIR = source 9 | BUILDDIR = build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 21 | 22 | open: 23 | open build/html/index.html 24 | -------------------------------------------------------------------------------- /docs/figs/classes.dot: -------------------------------------------------------------------------------- 1 | # dot -Tpdf classes.dot > classes.pdf ; open classes.pdf 2 | # dot -Tpng -Gdpi=150 -Nfontsize=20 -Nfontname=Roboto classes.dot > classes.png 3 | digraph G { 4 | graph [rankdir=BT]; 5 | BasePoseList -> "collections.UserList" 6 | BasePoseMatrix -> BasePoseList 7 | SO2 -> BasePoseMatrix 8 | SO3 -> BasePoseMatrix 9 | SE2 -> SO2 10 | SE3 -> SO3 11 | BaseTwist -> BasePoseList 12 | Twist2 -> BaseTwist 13 | Twist3 -> BaseTwist 14 | Quaternion -> BasePoseList 15 | UnitQuaternion -> Quaternion 16 | Line3 -> BasePoseList 17 | SpatialVector -> BasePoseList 18 | SpatialM6 -> SpatialVector 19 | SpatialF6 -> SpatialVector 20 | SpatialVelocity -> SpatialM6 21 | SpatialAcceleration -> SpatialM6 22 | SpatialMomentum -> SpatialF6 23 | SpatialForce -> SpatialF6 24 | SpatialInertia -> BasePoseList 25 | } 26 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | .FORCE: 2 | 3 | BLUE=\033[0;34m 4 | BLACK=\033[0;30m 5 | 6 | help: 7 | @echo "$(BLUE) make test - run all unit tests" 8 | @echo " make coverage - run unit tests and coverage report" 9 | @echo " make docs - build Sphinx documentation" 10 | @echo " make dist - build dist files" 11 | @echo " make upload - upload to PyPI" 12 | @echo " make clean - remove dist and docs build files" 13 | @echo " make help - this message$(BLACK)" 14 | 15 | test: 16 | pytest 17 | 18 | coverage: 19 | coverage run --source='spatialmath' -m pytest 20 | coverage report 21 | coverage html 22 | open htmlcov/index.html 23 | 24 | docs: .FORCE 25 | (cd docs; make html) 26 | 27 | view: 28 | open docs/build/html/index.html 29 | 30 | dist: .FORCE 31 | #$(MAKE) test 32 | python -m build 33 | ls -lh dist 34 | 35 | upload: .FORCE 36 | twine upload dist/* 37 | 38 | clean: .FORCE 39 | (cd docs; make clean) 40 | -rm -r *.egg-info 41 | -rm -r dist build 42 | -------------------------------------------------------------------------------- /docs/source/index.rst: -------------------------------------------------------------------------------- 1 | .. Spatial Maths package documentation master file, created by 2 | sphinx-quickstart on Sun Apr 12 15:50:23 2020. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | Spatial Maths for Python 7 | ======================== 8 | 9 | 10 | This package provides Python classes and functions to represent, print, plot, 11 | manipulate and covert between many common representations of position, 12 | orientation and pose of objects in 2D or 3D space. This includes 13 | mathematical objects such as rotation matrices :math:`\mat{R} \in \SO{2}, 14 | \SO{3}`, angle sequences, exponential coordinates, homogeneous transformation matrices :math:`\mat{T} \in \SE{2}, \SE{3}`, 15 | unit quaternions :math:`\q \in \mathrm{S}^3`, and twists :math:`S \in \se{2}, 16 | \se{3}`. 17 | 18 | .. toctree:: 19 | :maxdepth: 2 20 | 21 | intro 22 | spatialmath 23 | functions 24 | indices 25 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | # - repo: https://github.com/charliermarsh/ruff-pre-commit 3 | # # Ruff version. 4 | # rev: 'v0.1.0' 5 | # hooks: 6 | # - id: ruff 7 | # args: ['--fix', '--config', 'pyproject.toml'] 8 | 9 | - repo: https://github.com/psf/black 10 | rev: 'refs/tags/23.10.0:refs/tags/23.10.0' 11 | hooks: 12 | - id: black 13 | language_version: python3.10 14 | args: ['--config', 'pyproject.toml'] 15 | verbose: true 16 | 17 | - repo: https://github.com/pre-commit/pre-commit-hooks 18 | rev: v4.5.0 19 | hooks: 20 | - id: end-of-file-fixer 21 | exclude: | 22 | (?x)( 23 | ^docs/ 24 | ) 25 | - id: debug-statements # Ensure we don't commit `import pdb; pdb.set_trace()` 26 | - id: trailing-whitespace 27 | exclude: | 28 | (?x)( 29 | ^docs/ 30 | ) 31 | # - repo: https://github.com/pre-commit/mirrors-mypy 32 | # rev: v1.6.1 33 | # hooks: 34 | # - id: mypy 35 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | ### Python ### 3 | # Byte-compiled / optimized / DLL files 4 | __pycache__/ 5 | *.py[cod] 6 | *$py.class 7 | doc/build 8 | 9 | # C extensions 10 | *.so 11 | 12 | # Distribution / packaging 13 | build/ 14 | develop-eggs/ 15 | dist/ 16 | downloads/ 17 | eggs/ 18 | .eggs/ 19 | wheels/ 20 | pip-wheel-metadata/ 21 | *.egg-info/ 22 | *.egg 23 | MANIFEST 24 | 25 | # Installer logs 26 | pip-log.txt 27 | pip-delete-this-directory.txt 28 | 29 | # Unit test / coverage reports 30 | htmlcov/ 31 | .tox/ 32 | .nox/ 33 | .coverage 34 | .coverage.* 35 | .cache 36 | nosetests.xml 37 | coverage.xml 38 | *.cover 39 | .hypothesis/ 40 | .pytest_cache/ 41 | 42 | # Sphinx documentation 43 | docs/build/ 44 | docs/source/generated 45 | 46 | # PyBuilder 47 | target/ 48 | 49 | # pyenv 50 | .python-version 51 | 52 | # Pyre type checker 53 | .pyre/ 54 | 55 | ### VisualStudioCode ### 56 | .vscode/ 57 | 58 | ### VisualStudioCode Patch ### 59 | # Ignore all local history of files 60 | .history 61 | 62 | ### PyCharm ### 63 | .idea 64 | 65 | .ipynb_checkpoints/ 66 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Peter Corke 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /.github/workflows/publish.yml: -------------------------------------------------------------------------------- 1 | # This workflows will upload a Python Package using Twine when a release is created 2 | # For more information see: https://help.github.com/en/actions/language-and-framework-guides/using-python-with-github-actions#publishing-to-package-registries 3 | 4 | # name: Upload Python Package 5 | 6 | on: 7 | release: 8 | types: [created] 9 | workflow_dispatch: 10 | 11 | jobs: 12 | deploy: 13 | 14 | runs-on: ${{ matrix.os }} 15 | strategy: 16 | max-parallel: 2 17 | matrix: 18 | os: [ubuntu-22.04] 19 | python-version: [3.8] 20 | 21 | steps: 22 | - uses: actions/checkout@v2 23 | - name: Set up Python 24 | uses: actions/setup-python@v5 25 | with: 26 | python-version: ${{ matrix.python-version }} 27 | - name: Install dependencies 28 | run: | 29 | python -m pip install --upgrade pip 30 | pip install -U setuptools wheel twine build 31 | - name: Build and publish 32 | env: 33 | TWINE_USERNAME: __token__ 34 | TWINE_PASSWORD: ${{ secrets.PYPI_PASSWORD }} 35 | run: | 36 | python -m build 37 | ls ./dist/*.whl 38 | twine upload dist/*.gz 39 | twine upload dist/*.whl 40 | -------------------------------------------------------------------------------- /.github/svg/sm_powered.min.svg: -------------------------------------------------------------------------------- 1 | powered byspatial maths -------------------------------------------------------------------------------- /spatialmath/__init__.py: -------------------------------------------------------------------------------- 1 | # print("in spatialmath/__init__") 2 | 3 | from spatialmath.pose2d import SO2, SE2 4 | from spatialmath.pose3d import SO3, SE3 5 | from spatialmath.baseposematrix import BasePoseMatrix 6 | from spatialmath.geom2d import Line2, LineSegment2, Polygon2, Ellipse 7 | from spatialmath.geom3d import Line3, Plane3 8 | from spatialmath.twist import Twist3, Twist2 9 | from spatialmath.spatialvector import ( 10 | SpatialVelocity, 11 | SpatialAcceleration, 12 | SpatialForce, 13 | SpatialMomentum, 14 | SpatialInertia, 15 | ) 16 | from spatialmath.quaternion import Quaternion, UnitQuaternion 17 | from spatialmath.DualQuaternion import DualQuaternion, UnitDualQuaternion 18 | from spatialmath.spline import BSplineSE3, InterpSplineSE3, SplineFit 19 | 20 | 21 | __all__ = [ 22 | # pose 23 | "SO2", 24 | "SE2", 25 | "SO3", 26 | "SE3", 27 | "BasePoseMatrix", 28 | "Quaternion", 29 | "UnitQuaternion", 30 | "DualQuaternion", 31 | "UnitDualQuaternion", 32 | "Twist3", 33 | "Twist2", 34 | "SpatialVelocity", 35 | "SpatialAcceleration", 36 | "SpatialForce", 37 | "SpatialMomentum", 38 | "SpatialInertia", 39 | "Line3", 40 | "Plane3", 41 | "Line2", 42 | "LineSegment2", 43 | "Polygon2", 44 | "Ellipse", 45 | "BSplineSE3", 46 | "InterpSplineSE3", 47 | "SplineFit", 48 | ] 49 | 50 | try: 51 | import importlib.metadata 52 | 53 | __version__ = importlib.metadata.version("spatialmath-python") 54 | except: 55 | pass 56 | -------------------------------------------------------------------------------- /.github/workflows/sphinx.yml: -------------------------------------------------------------------------------- 1 | name: Sphinx 2 | 3 | on: 4 | workflow_call: 5 | 6 | jobs: 7 | sphinx: 8 | runs-on: ubuntu-22.04 9 | if: ${{ github.event_name != 'pull_request' }} 10 | steps: 11 | - uses: actions/checkout@v2 12 | - name: Set up Python 3.8 13 | uses: actions/setup-python@v5 14 | with: 15 | python-version: 3.8 16 | - name: Install dependencies 17 | run: | 18 | python -m pip install --upgrade pip 19 | pip install .[dev,docs] 20 | pip install git+https://github.com/petercorke/sphinx-autorun.git 21 | pip install sympy 22 | sudo apt-get install graphviz 23 | - name: Build docs 24 | run: | 25 | cd docs 26 | make html 27 | # Tell GitHub not to use jekyll to compile the docs 28 | touch build/html/.nojekyll 29 | cd ../ 30 | - name: Commit documentation changes 31 | run: | 32 | git clone https://github.com/petercorke/spatialmath-python.git --branch gh-pages --single-branch gh-pages 33 | cp -r docs/build/html/* gh-pages/ 34 | cd gh-pages 35 | git config --local user.email "action@github.com" 36 | git config --local user.name "GitHub Action" 37 | git add . 38 | git commit -m "Update documentation" -a || true 39 | # The above command will fail if no changes were present, so we ignore 40 | # that. 41 | - name: Push changes 42 | uses: ad-m/github-push-action@master 43 | with: 44 | branch: gh-pages 45 | directory: gh-pages 46 | github_token: ${{ secrets.GITHUB_TOKEN }} 47 | -------------------------------------------------------------------------------- /docs/source/spatialmath.rst: -------------------------------------------------------------------------------- 1 | *************** 2 | Class reference 3 | *************** 4 | 5 | 6 | .. image:: ../figs/classes.png 7 | :width: 600 8 | 9 | (click to enlarged) 10 | 11 | The Spatial Math Toolbox classes form a hierarchy. 12 | They all inherit from the abstract class 13 | ``SMUserList`` which embues them with list-like functionality. 14 | 15 | 16 | 3D-space 17 | ======== 18 | 19 | Pose in 3D 20 | ---------- 21 | 22 | .. toctree:: 23 | :maxdepth: 2 24 | 25 | 3d_pose_SE3 26 | 3d_pose_twist 27 | 3d_pose_dualquaternion 28 | 29 | 30 | Orientation in 3D 31 | ----------------- 32 | 33 | .. toctree:: 34 | :maxdepth: 2 35 | 36 | 3d_orient_SO3 37 | 3d_orient_unitquaternion 38 | 39 | 40 | 6D spatial vectors 41 | ------------------ 42 | 43 | .. toctree:: 44 | :maxdepth: 2 45 | 46 | 6d_spatial.rst 47 | 6d_m6.rst 48 | 6d_velocity.rst 49 | 6d_acceleration.rst 50 | 6d_f6.rst 51 | 6d_force.rst 52 | 6d_momentum.rst 53 | 6d_inertia.rst 54 | 55 | Geometry in 3D 56 | -------------- 57 | 58 | .. toctree:: 59 | :maxdepth: 2 60 | 61 | 3d_line 62 | 3d_plane 63 | 64 | Supporting 65 | ---------- 66 | 67 | .. toctree:: 68 | :maxdepth: 2 69 | 70 | 3d_quaternion 71 | 3d_dualquaternion 72 | 73 | --------------- 74 | 75 | 2D-space 76 | ======== 77 | 78 | Pose in 2D 79 | ---------- 80 | 81 | .. toctree:: 82 | :maxdepth: 2 83 | 84 | 2d_pose_SE2 85 | 2d_pose_twist 86 | 87 | Orientation in 2D 88 | ----------------- 89 | 90 | .. toctree:: 91 | :maxdepth: 2 92 | 93 | 2d_orient_SO2 94 | 95 | Geometry in 2D 96 | -------------- 97 | 98 | .. toctree:: 99 | :maxdepth: 2 100 | 101 | 2d_line 102 | 2d_linesegment 103 | 2d_polygon 104 | 2d_ellipse -------------------------------------------------------------------------------- /.github/workflows/master.yml: -------------------------------------------------------------------------------- 1 | 2 | # This workflow will install Python dependencies, run tests and lint with a variety of Python versions 3 | # For more information see: https://help.github.com/actions/language-and-framework-guides/using-python-with-github-actions 4 | 5 | name: build 6 | 7 | on: 8 | push: 9 | branches: [ master, future ] 10 | pull_request: 11 | 12 | 13 | jobs: 14 | # Run tests on different versions of python 15 | unittest: 16 | runs-on: ${{ matrix.os }} 17 | strategy: 18 | matrix: 19 | os: [windows-latest, ubuntu-22.04, macos-latest] 20 | python-version: ["3.8", "3.9", "3.10", "3.11", "3.12"] 21 | exclude: 22 | - os: windows-latest 23 | python-version: "3.11" 24 | - os: windows-latest 25 | python-version: "3.12" 26 | 27 | steps: 28 | - uses: actions/checkout@v2 29 | - name: Set up Python ${{ matrix.python-version }} 30 | uses: actions/setup-python@v5 31 | with: 32 | python-version: ${{ matrix.python-version }} 33 | - name: Install dependencies 34 | run: | 35 | python -m pip install --upgrade pip 36 | pip install .[dev] 37 | - name: Test with pytest 38 | env: 39 | MPLBACKEND: TkAgg 40 | run: | 41 | pytest -s --ignore=W605 --timeout=50 --timeout_method=thread 42 | 43 | codecov: 44 | # If all tests pass: 45 | # Run coverage and upload to codecov 46 | needs: unittest 47 | runs-on: ubuntu-22.04 48 | steps: 49 | - uses: actions/checkout@v2 50 | - name: Set up Python 3.8 51 | uses: actions/setup-python@v5 52 | with: 53 | python-version: 3.8 54 | - name: Install dependencies 55 | run: | 56 | python -m pip install --upgrade pip 57 | pip install .[dev] 58 | - name: Run coverage 59 | run: | 60 | coverage run --omit='tests/*.py,tests/base/*.py' -m pytest 61 | coverage report 62 | coverage xml 63 | - name: upload coverage to Codecov 64 | uses: codecov/codecov-action@v3 65 | with: 66 | file: ./coverage.xml 67 | sphinx: 68 | # If the above worked: 69 | # Build docs and upload to GH Pages 70 | needs: unittest 71 | uses: ./.github/workflows/sphinx.yml 72 | -------------------------------------------------------------------------------- /.github/svg/sm_powered.svg: -------------------------------------------------------------------------------- 1 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | powered by 16 | spatial maths 17 | 18 | 19 | 20 | 21 | 23 | 25 | 27 | 29 | 31 | 33 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /tests/base/test_transforms3d_plot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Fri Apr 10 14:19:04 2020 5 | 6 | @author: corkep 7 | 8 | """ 9 | 10 | 11 | import numpy as np 12 | import numpy.testing as nt 13 | import unittest 14 | from math import pi 15 | import math 16 | from scipy.linalg import logm, expm 17 | import pytest 18 | import sys 19 | 20 | from spatialmath.base.transforms3d import * 21 | from spatialmath.base.transformsNd import isR, t2r, r2t, rt2tr 22 | 23 | import matplotlib.pyplot as plt 24 | 25 | 26 | class Test3D(unittest.TestCase): 27 | @pytest.mark.skipif( 28 | sys.platform.startswith("darwin") and sys.version_info < (3, 11), 29 | reason="tkinter bug with mac", 30 | ) 31 | def test_plot(self): 32 | plt.figure() 33 | # test options 34 | trplot( 35 | transl(1, 2, 3), 36 | block=False, 37 | frame="A", 38 | style="line", 39 | width=1, 40 | dims=[0, 10, 0, 10, 0, 10], 41 | ) 42 | trplot( 43 | transl(1, 2, 3), 44 | block=False, 45 | frame="A", 46 | style="arrow", 47 | width=1, 48 | dims=[0, 10, 0, 10, 0, 10], 49 | ) 50 | trplot( 51 | transl(1, 2, 3), 52 | block=False, 53 | frame="A", 54 | style="rgb", 55 | width=1, 56 | dims=[0, 10, 0, 10, 0, 10], 57 | ) 58 | trplot(transl(3, 1, 2), block=False, color="red", width=3, frame="B") 59 | trplot( 60 | transl(4, 3, 1) @ trotx(math.pi / 3), 61 | block=False, 62 | color="green", 63 | frame="c", 64 | dims=[0, 4, 0, 4, 0, 4], 65 | ) 66 | 67 | # test for iterable 68 | plt.clf() 69 | T = [transl(1, 2, 3), transl(2, 3, 4), transl(3, 4, 5)] 70 | trplot(T) 71 | 72 | plt.close("all") 73 | 74 | @pytest.mark.skipif( 75 | sys.platform.startswith("darwin") and sys.version_info < (3, 11), 76 | reason="tkinter bug with mac", 77 | ) 78 | def test_animate(self): 79 | tranimate(transl(1, 2, 3), repeat=False, wait=True) 80 | 81 | tranimate(transl(1, 2, 3), repeat=False, wait=True) 82 | # run again, with axes already created 83 | tranimate(transl(1, 2, 3), repeat=False, wait=True, dims=[0, 10, 0, 10, 0, 10]) 84 | 85 | plt.close("all") 86 | # test animate with line not arrow, text, test with SO(3) 87 | 88 | 89 | # ---------------------------------------------------------------------------------------# 90 | if __name__ == "__main__": 91 | unittest.main() 92 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [project] 2 | name = "spatialmath-python" 3 | version = "1.1.15" 4 | authors = [ 5 | { name="Peter Corke", email="rvc@petercorke.com" }, 6 | ] 7 | description = "Provides spatial maths capability for Python." 8 | readme = "README.md" 9 | requires-python = ">=3.7" 10 | classifiers = [ 11 | "Development Status :: 5 - Production/Stable", 12 | # Indicate who your project is intended for 13 | "Intended Audience :: Developers", 14 | # Specify the Python versions you support here. 15 | "Programming Language :: Python :: 3.7", 16 | "Programming Language :: Python :: 3.8", 17 | "Programming Language :: Python :: 3.9", 18 | "Programming Language :: Python :: 3.10", 19 | "Programming Language :: Python :: 3.11", 20 | "Programming Language :: Python :: 3.12", 21 | 22 | "License :: OSI Approved :: MIT License", 23 | "Operating System :: OS Independent", 24 | ] 25 | keywords = [ 26 | "spatial-math", "spatial math", 27 | "SO2", "SE2", "SO3", "SE3", 28 | "SO(2)", "SE(2)", "SO(3)", "SE(3)", 29 | "twist", "product of exponential", "translation", "orientation", 30 | "angle-axis", "Lie group", "skew symmetric matrix", 31 | "pose", "translation", "rotation matrix", 32 | "rigid body transform", "homogeneous transformation", 33 | "Euler angles", "roll-pitch-yaw angles", 34 | "quaternion", "unit-quaternion", 35 | "robotics", "robot vision", "computer vision", 36 | ] 37 | 38 | dependencies = [ 39 | "numpy>=1.22", 40 | "scipy", 41 | "matplotlib", 42 | "ansitable", 43 | "typing_extensions", 44 | "pre-commit", 45 | ] 46 | 47 | [project.urls] 48 | "Homepage" = "https://github.com/bdaiinstitute/spatialmath-python" 49 | "Bug Tracker" = "https://github.com/bdaiinstitute/spatialmath-python/issues" 50 | "Documentation" = "https://bdaiinstitute.github.io/spatialmath-python/" 51 | "Source" = "https://github.com/bdaiinstitute/spatialmath-python" 52 | 53 | [project.optional-dependencies] 54 | 55 | dev = [ 56 | "sympy", 57 | "pytest", 58 | "pytest-timeout", 59 | "pytest-xvfb", 60 | "coverage", 61 | "flake8" 62 | ] 63 | 64 | docs = [ 65 | "sphinx", 66 | "sphinx-rtd-theme", 67 | "sphinx-autorun", 68 | "sphinxcontrib-jsmath", 69 | "sphinx-favicon", 70 | "sphinx-autodoc-typehints", 71 | ] 72 | 73 | ros-humble = [ 74 | "matplotlib==3.5.1", # Large user-base has apt-installed python3-matplotlib (ROS2) which is pinned to this version. 75 | "numpy<2", # Cannot use 2.0 due to matplotlib version pinning. 76 | ] 77 | 78 | [build-system] 79 | 80 | requires = ["setuptools", "oldest-supported-numpy"] 81 | build-backend = "setuptools.build_meta" 82 | 83 | [tool.setuptools] 84 | 85 | packages = [ 86 | "spatialmath", 87 | "spatialmath.base", 88 | ] 89 | 90 | [tool.black] 91 | required-version = "23.10.0" 92 | line-length = 88 93 | target-version = ['py38'] 94 | exclude = "camera_derivatives.py" 95 | -------------------------------------------------------------------------------- /tests/base/test_symbolic.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | import math 3 | 4 | try: 5 | import sympy as sp 6 | 7 | _symbolics = True 8 | except ImportError: 9 | _symbolics = False 10 | 11 | from spatialmath.base.symbolic import * 12 | 13 | 14 | class Test_symbolic(unittest.TestCase): 15 | @unittest.skipUnless(_symbolics, "sympy required") 16 | def test_symbol(self): 17 | theta = symbol("theta") 18 | self.assertTrue(isinstance(theta, sp.Expr)) 19 | self.assertTrue(theta.is_real) 20 | 21 | theta = symbol("theta", real=False) 22 | self.assertTrue(isinstance(theta, sp.Expr)) 23 | self.assertFalse(theta.is_real) 24 | 25 | theta, psi = symbol("theta, psi") 26 | self.assertTrue(isinstance(theta, sp.Expr)) 27 | self.assertTrue(isinstance(psi, sp.Expr)) 28 | 29 | theta, psi = symbol("theta psi") 30 | self.assertTrue(isinstance(theta, sp.Expr)) 31 | self.assertTrue(isinstance(psi, sp.Expr)) 32 | 33 | q = symbol("q:6") 34 | self.assertEqual(len(q), 6) 35 | for _ in q: 36 | self.assertTrue(isinstance(_, sp.Expr)) 37 | self.assertTrue(_.is_real) 38 | 39 | @unittest.skipUnless(_symbolics, "sympy required") 40 | def test_issymbol(self): 41 | theta = symbol("theta") 42 | self.assertFalse(issymbol(3)) 43 | self.assertFalse(issymbol("not a symbol")) 44 | self.assertFalse(issymbol([1, 2])) 45 | self.assertTrue(issymbol(theta)) 46 | 47 | @unittest.skipUnless(_symbolics, "sympy required") 48 | def test_functions(self): 49 | theta = symbol("theta") 50 | self.assertTrue(isinstance(sin(theta), sp.Expr)) 51 | self.assertTrue(isinstance(sin(1.0), float)) 52 | 53 | self.assertTrue(isinstance(cos(theta), sp.Expr)) 54 | self.assertTrue(isinstance(cos(1.0), float)) 55 | 56 | self.assertTrue(isinstance(sqrt(theta), sp.Expr)) 57 | self.assertTrue(isinstance(sqrt(1.0), float)) 58 | 59 | x = (theta - 1) * (theta + 1) - theta**2 60 | self.assertTrue(math.isclose(simplify(x).evalf(), -1)) 61 | 62 | @unittest.skipUnless(_symbolics, "sympy required") 63 | def test_constants(self): 64 | x = zero() 65 | self.assertTrue(isinstance(x, sp.Expr)) 66 | self.assertTrue(math.isclose(x.evalf(), 0)) 67 | 68 | x = one() 69 | self.assertTrue(isinstance(x, sp.Expr)) 70 | self.assertTrue(math.isclose(x.evalf(), 1)) 71 | 72 | x = negative_one() 73 | self.assertTrue(isinstance(x, sp.Expr)) 74 | self.assertTrue(math.isclose(x.evalf(), -1)) 75 | 76 | x = pi() 77 | self.assertTrue(isinstance(x, sp.Expr)) 78 | self.assertTrue(math.isclose(x.evalf(), math.pi)) 79 | 80 | 81 | # ---------------------------------------------------------------------------------------# 82 | if __name__ == "__main__": # pragma: no cover 83 | unittest.main() 84 | -------------------------------------------------------------------------------- /tests/base/test_numeric.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Fri Apr 10 14:19:04 2020 5 | 6 | @author: corkep 7 | 8 | """ 9 | 10 | import numpy as np 11 | import unittest 12 | 13 | 14 | from spatialmath.base.numeric import * 15 | 16 | 17 | class TestNumeric(unittest.TestCase): 18 | def test_numjac(self): 19 | pass 20 | 21 | def test_array2str(self): 22 | x = [1.2345678] 23 | s = array2str(x) 24 | 25 | self.assertIsInstance(s, str) 26 | self.assertEqual(s, "[ 1.23 ]") 27 | 28 | s = array2str(x, fmt="{:.5f}") 29 | self.assertEqual(s, "[ 1.23457 ]") 30 | 31 | s = array2str([1, 2, 3]) 32 | self.assertEqual(s, "[ 1, 2, 3 ]") 33 | 34 | s = array2str([1, 2, 3], valuesep=":") 35 | self.assertEqual(s, "[ 1:2:3 ]") 36 | 37 | s = array2str([1, 2, 3], brackets=("<< ", " >>")) 38 | self.assertEqual(s, "<< 1, 2, 3 >>") 39 | 40 | s = array2str([1, 2e-8, 3]) 41 | self.assertEqual(s, "[ 1, 2e-08, 3 ]") 42 | 43 | s = array2str([1, -2e-14, 3]) 44 | self.assertEqual(s, "[ 1, 0, 3 ]") 45 | 46 | x = np.array([[1, 2, 3], [4, 5, 6]]) 47 | s = array2str(x) 48 | self.assertEqual(s, "[ 1, 2, 3 | 4, 5, 6 ]") 49 | 50 | def test_bresenham(self): 51 | x, y = bresenham((-10, -10), (20, 10)) 52 | self.assertIsInstance(x, np.ndarray) 53 | self.assertEqual(x.ndim, 1) 54 | self.assertIsInstance(y, np.ndarray) 55 | self.assertEqual(y.ndim, 1) 56 | self.assertEqual(len(x), len(y)) 57 | 58 | # test points are no more than sqrt(2) apart 59 | z = np.array([x, y]) 60 | d = np.diff(z, axis=1) 61 | d = np.linalg.norm(d, axis=0) 62 | self.assertTrue(all(d <= np.sqrt(2))) 63 | 64 | x, y = bresenham((20, 10), (-10, -10)) 65 | 66 | # test points are no more than sqrt(2) apart 67 | z = np.array([x, y]) 68 | d = np.diff(z, axis=1) 69 | d = np.linalg.norm(d, axis=0) 70 | self.assertTrue(all(d <= np.sqrt(2))) 71 | 72 | x, y = bresenham((-10, -10), (10, 20)) 73 | 74 | # test points are no more than sqrt(2) apart 75 | z = np.array([x, y]) 76 | d = np.diff(z, axis=1) 77 | d = np.linalg.norm(d, axis=0) 78 | self.assertTrue(all(d <= np.sqrt(2))) 79 | 80 | x, y = bresenham((10, 20), (-10, -10)) 81 | 82 | # test points are no more than sqrt(2) apart 83 | z = np.array([x, y]) 84 | d = np.diff(z, axis=1) 85 | d = np.linalg.norm(d, axis=0) 86 | self.assertTrue(all(d <= np.sqrt(2))) 87 | 88 | def test_mpq(self): 89 | data = np.array([[-1, 1, 1, -1], [-1, -1, 1, 1]]) 90 | 91 | self.assertEqual(mpq_point(data, 0, 0), 4) 92 | self.assertEqual(mpq_point(data, 1, 0), 0) 93 | self.assertEqual(mpq_point(data, 0, 1), 0) 94 | 95 | def test_gauss1d(self): 96 | x = np.arange(-10, 10, 0.02) 97 | y = gauss1d(2, 1, x) 98 | 99 | self.assertEqual(len(x), len(y)) 100 | 101 | m = np.argmax(y) 102 | self.assertAlmostEqual(x[m], 2) 103 | 104 | def test_gauss2d(self): 105 | r = np.arange(-10, 10, 0.02) 106 | X, Y = np.meshgrid(r, r) 107 | Z = gauss2d([2, 3], np.eye(2), X, Y) 108 | 109 | m = np.unravel_index(np.argmax(Z, axis=None), Z.shape) 110 | self.assertAlmostEqual(r[m[0]], 3) 111 | self.assertAlmostEqual(r[m[1]], 2) 112 | 113 | 114 | # ---------------------------------------------------------------------------------------# 115 | if __name__ == "__main__": 116 | unittest.main() 117 | -------------------------------------------------------------------------------- /tests/test_dualquaternion.py: -------------------------------------------------------------------------------- 1 | from math import pi 2 | import numpy as np 3 | 4 | import numpy.testing as nt 5 | import unittest 6 | 7 | from spatialmath import DualQuaternion, UnitDualQuaternion, Quaternion, SE3 8 | 9 | 10 | def qcompare(x, y): 11 | if isinstance(x, Quaternion): 12 | x = x.vec 13 | elif isinstance(x, SMPose): 14 | x = x.A 15 | if isinstance(y, Quaternion): 16 | y = y.vec 17 | elif isinstance(y, SMPose): 18 | y = y.A 19 | nt.assert_array_almost_equal(x, y) 20 | 21 | 22 | class TestDualQuaternion(unittest.TestCase): 23 | def test_init(self): 24 | dq = DualQuaternion(Quaternion([1.0, 2, 3, 4]), Quaternion([5.0, 6, 7, 8])) 25 | nt.assert_array_almost_equal(dq.vec, np.r_[1, 2, 3, 4, 5, 6, 7, 8]) 26 | 27 | dq = DualQuaternion([1.0, 2, 3, 4, 5, 6, 7, 8]) 28 | nt.assert_array_almost_equal(dq.vec, np.r_[1, 2, 3, 4, 5, 6, 7, 8]) 29 | dq = DualQuaternion(np.r_[1, 2, 3, 4, 5, 6, 7, 8]) 30 | nt.assert_array_almost_equal(dq.vec, np.r_[1, 2, 3, 4, 5, 6, 7, 8]) 31 | 32 | def test_pure(self): 33 | dq = DualQuaternion.Pure([1.0, 2, 3]) 34 | nt.assert_array_almost_equal(dq.vec, np.r_[1, 0, 0, 0, 0, 1, 2, 3]) 35 | 36 | def test_strings(self): 37 | dq = DualQuaternion(Quaternion([1.0, 2, 3, 4]), Quaternion([5.0, 6, 7, 8])) 38 | self.assertIsInstance(str(dq), str) 39 | self.assertIsInstance(repr(dq), str) 40 | 41 | def test_conj(self): 42 | dq = DualQuaternion(Quaternion([1.0, 2, 3, 4]), Quaternion([5.0, 6, 7, 8])) 43 | nt.assert_array_almost_equal(dq.conj().vec, np.r_[1, -2, -3, -4, 5, -6, -7, -8]) 44 | 45 | # def test_norm(self): 46 | # q1 = Quaternion([1.,2,3,4]) 47 | # q2 = Quaternion([5.,6,7,8]) 48 | 49 | # dq = DualQuaternion(q1, q2) 50 | # nt.assert_array_almost_equal(dq.norm(), (q1.norm(), q2.norm())) 51 | 52 | def test_plus(self): 53 | dq = DualQuaternion(Quaternion([1.0, 2, 3, 4]), Quaternion([5.0, 6, 7, 8])) 54 | s = dq + dq 55 | nt.assert_array_almost_equal(s.vec, 2 * np.r_[1, 2, 3, 4, 5, 6, 7, 8]) 56 | 57 | def test_minus(self): 58 | dq = DualQuaternion(Quaternion([1.0, 2, 3, 4]), Quaternion([5.0, 6, 7, 8])) 59 | s = dq - dq 60 | nt.assert_array_almost_equal(s.vec, np.zeros((8,))) 61 | 62 | def test_matrix(self): 63 | dq1 = DualQuaternion(Quaternion([1.0, 2, 3, 4]), Quaternion([5.0, 6, 7, 8])) 64 | 65 | M = dq1.matrix() 66 | self.assertIsInstance(M, np.ndarray) 67 | self.assertEqual(M.shape, (8, 8)) 68 | 69 | def test_multiply(self): 70 | dq1 = DualQuaternion(Quaternion([1.0, 2, 3, 4]), Quaternion([5.0, 6, 7, 8])) 71 | dq2 = DualQuaternion(Quaternion([4, 3, 2, 1]), Quaternion([5, 6, 7, 8])) 72 | 73 | M = dq1.matrix() 74 | v = dq2.vec 75 | nt.assert_array_almost_equal(M @ v, (dq1 * dq2).vec) 76 | 77 | def test_unit(self): 78 | pass 79 | 80 | 81 | class TestUnitDualQuaternion(unittest.TestCase): 82 | def test_init(self): 83 | T = SE3.Rx(pi / 4) 84 | dq = UnitDualQuaternion(T) 85 | nt.assert_array_almost_equal(dq.SE3().A, T.A) 86 | 87 | def test_norm(self): 88 | T = SE3.Rx(pi / 4) 89 | dq = UnitDualQuaternion(T) 90 | nt.assert_array_almost_equal(dq.norm(), (1, 0)) 91 | 92 | def test_multiply(self): 93 | T1 = SE3.Rx(pi / 4) 94 | T2 = SE3.Rz(-pi / 3) 95 | 96 | T = T1 * T2 97 | 98 | d1 = UnitDualQuaternion(T1) 99 | d2 = UnitDualQuaternion(T2) 100 | 101 | d = d1 * d2 102 | nt.assert_array_almost_equal(d.SE3().A, T.A) 103 | 104 | 105 | # ---------------------------------------------------------------------------------------# 106 | if __name__ == "__main__": # pragma: no cover 107 | unittest.main() 108 | -------------------------------------------------------------------------------- /tests/test_spline.py: -------------------------------------------------------------------------------- 1 | import numpy.testing as nt 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | import unittest 5 | 6 | from spatialmath import BSplineSE3, SE3, InterpSplineSE3, SplineFit, SO3 7 | 8 | 9 | class TestBSplineSE3(unittest.TestCase): 10 | control_poses = [ 11 | SE3.Trans([e, 2 * np.cos(e / 2 * np.pi), 2 * np.sin(e / 2 * np.pi)]) 12 | * SE3.Ry(e / 8 * np.pi) 13 | for e in range(0, 8) 14 | ] 15 | 16 | @classmethod 17 | def tearDownClass(cls): 18 | plt.close("all") 19 | 20 | def test_constructor(self): 21 | BSplineSE3(self.control_poses) 22 | 23 | def test_evaluation(self): 24 | spline = BSplineSE3(self.control_poses) 25 | nt.assert_almost_equal(spline(0).A, self.control_poses[0].A) 26 | nt.assert_almost_equal(spline(1).A, self.control_poses[-1].A) 27 | 28 | def test_visualize(self): 29 | spline = BSplineSE3(self.control_poses) 30 | spline.visualize( 31 | sample_times=np.linspace(0, 1.0, 100), animate=True, repeat=False 32 | ) 33 | 34 | 35 | class TestInterpSplineSE3: 36 | waypoints = [ 37 | SE3.Trans([e, 2 * np.cos(e / 2 * np.pi), 2 * np.sin(e / 2 * np.pi)]) 38 | * SE3.Ry(e / 8 * np.pi) 39 | for e in range(0, 8) 40 | ] 41 | time_horizon = 10 42 | times = np.linspace(0, time_horizon, len(waypoints)) 43 | 44 | @classmethod 45 | def tearDownClass(cls): 46 | plt.close("all") 47 | 48 | def test_constructor(self): 49 | InterpSplineSE3(self.times, self.waypoints) 50 | 51 | def test_evaluation(self): 52 | spline = InterpSplineSE3(self.times, self.waypoints) 53 | for time, pose in zip(self.times, self.waypoints): 54 | nt.assert_almost_equal(spline(time).angdist(pose), 0.0) 55 | nt.assert_almost_equal(np.linalg.norm(spline(time).t - pose.t), 0.0) 56 | 57 | spline = InterpSplineSE3(self.times, self.waypoints, normalize_time=True) 58 | norm_time = spline.timepoints 59 | for time, pose in zip(norm_time, self.waypoints): 60 | nt.assert_almost_equal(spline(time).angdist(pose), 0.0) 61 | nt.assert_almost_equal(np.linalg.norm(spline(time).t - pose.t), 0.0) 62 | 63 | def test_small_delta_t(self): 64 | InterpSplineSE3( 65 | np.linspace(0, InterpSplineSE3._e, len(self.waypoints)), self.waypoints 66 | ) 67 | 68 | def test_visualize(self): 69 | spline = InterpSplineSE3(self.times, self.waypoints) 70 | spline.visualize( 71 | sample_times=np.linspace(0, self.time_horizon, 100), 72 | animate=True, 73 | repeat=False, 74 | ) 75 | 76 | 77 | class TestSplineFit: 78 | num_data_points = 300 79 | time_horizon = 5 80 | num_viz_points = 100 81 | 82 | # make a helix 83 | timestamps = np.linspace(0, 1, num_data_points) 84 | trajectory = [ 85 | SE3.Rt( 86 | t=[ 87 | t * 0.4, 88 | 0.4 * np.sin(t * 2 * np.pi * 0.5), 89 | 0.4 * np.cos(t * 2 * np.pi * 0.5), 90 | ], 91 | R=SO3.Rx(t * 2 * np.pi * 0.5), 92 | ) 93 | for t in timestamps * time_horizon 94 | ] 95 | 96 | def test_spline_fit(self): 97 | fit = SplineFit(self.timestamps, self.trajectory) 98 | spline, kept_indices = fit.stochastic_downsample_interpolation() 99 | 100 | fraction_points_removed = 1.0 - len(kept_indices) / self.num_data_points 101 | 102 | assert fraction_points_removed > 0.2 103 | assert len(spline.control_poses) == len(kept_indices) 104 | assert len(spline.timepoints) == len(kept_indices) 105 | 106 | assert fit.max_angular_error() < np.deg2rad(5.0) 107 | assert fit.max_angular_error() < 0.1 108 | spline.visualize( 109 | sample_times=np.linspace(0, self.time_horizon, 100), 110 | animate=True, 111 | repeat=False, 112 | ) 113 | -------------------------------------------------------------------------------- /docs/generated/spatialmath.pose2d.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | spatialmath.pose2d — Spatial Maths package 0.7.0 8 | documentation 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 |
30 |
31 |
32 | 33 | 34 |
35 | 36 |
37 |

spatialmath.pose2d

38 |

Classes

39 |
40 | 41 | 42 |
43 | 44 |
45 |
46 | 105 |
106 |
107 |
108 | ©2020, Peter Corke. 109 | 110 | | 111 | Powered by Sphinx 2.4.4 112 | & Alabaster 0.7.12 113 | 114 | | 115 | Page source 117 |
118 | 119 | 120 | 121 | 122 | 123 | -------------------------------------------------------------------------------- /docs/generated/spatialmath.pose3d.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | spatialmath.pose3d — Spatial Maths package 0.7.0 8 | documentation 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 |
30 |
31 |
32 | 33 | 34 |
35 | 36 |
37 |

spatialmath.pose3d

38 |

Classes

39 |
40 | 41 | 42 |
43 | 44 |
45 |
46 | 105 |
106 |
107 |
108 | ©2020, Peter Corke. 109 | 110 | | 111 | Powered by Sphinx 2.4.4 112 | & Alabaster 0.7.12 113 | 114 | | 115 | Page source 117 |
118 | 119 | 120 | 121 | 122 | 123 | -------------------------------------------------------------------------------- /docs/generated/spatialmath.quaternion.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | spatialmath.quaternion — Spatial Maths package 0.7.0 8 | documentation 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 |
30 |
31 |
32 | 33 | 34 |
35 | 36 |
37 |

spatialmath.quaternion

38 |

Classes

39 |
40 | 41 | 42 |
43 | 44 |
45 |
46 | 105 |
106 |
107 |
108 | ©2020, Peter Corke. 109 | 110 | | 111 | Powered by Sphinx 2.4.4 112 | & Alabaster 0.7.12 113 | 114 | | 115 | Page source 117 |
118 | 119 | 120 | 121 | 122 | 123 | -------------------------------------------------------------------------------- /docs/generated/spatialmath.base.quaternions.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | spatialmath.base.quaternions — Spatial Maths package 0.7.0 8 | documentation 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 |
30 |
31 |
32 | 33 | 34 |
35 | 36 |
37 |

spatialmath.base.quaternions

38 |

Created on Fri Apr 10 14:12:56 2020

39 |

@author: Peter Corke

40 |

Functions

41 |
42 | 43 | 44 |
45 | 46 |
47 |
48 | 107 |
108 |
109 |
110 | ©2020, Peter Corke. 111 | 112 | | 113 | Powered by Sphinx 2.4.4 114 | & Alabaster 0.7.12 115 | 116 | | 117 | Page source 119 |
120 | 121 | 122 | 123 | 124 | 125 | -------------------------------------------------------------------------------- /spatialmath/base/_types_35.py: -------------------------------------------------------------------------------- 1 | # for Python <= 3.8 2 | 3 | from typing import ( 4 | overload, 5 | Union, 6 | List, 7 | Tuple, 8 | Type, 9 | TextIO, 10 | Any, 11 | Callable, 12 | Optional, 13 | Iterator, 14 | ) 15 | from typing_extensions import Literal as L 16 | from typing_extensions import Self 17 | 18 | # array like 19 | 20 | # these are input to many functions in spatialmath.base, and can be a list, tuple or 21 | # ndarray. The elements are generally float, but some functions accept symbolic 22 | # arguments as well, which leads to a NumPy array with dtype=object 23 | # 24 | # The variants like ArrayLike2 indicate that a list, tuple or ndarray of length 2 is 25 | # expected. Static checking of tuple length is possible but not a lists. This might be 26 | # possible in future versions of Python, but for now it is a hint to the coder about 27 | # what is expected 28 | 29 | from numpy.typing import DTypeLike, NDArray # , ArrayLike 30 | 31 | from typing import cast 32 | 33 | # from typing import TypeVar 34 | # NDArray = TypeVar('NDArray') 35 | import numpy as np 36 | 37 | 38 | ArrayLike = Union[float, List[float], Tuple[float, ...], NDArray] 39 | ArrayLikePure = Union[List[float], Tuple[float, ...], NDArray] 40 | ArrayLike2 = Union[List, Tuple[float, float], NDArray] 41 | ArrayLike3 = Union[List, Tuple[float, float, float], NDArray] 42 | ArrayLike4 = Union[List, Tuple[float, float, float, float], NDArray] 43 | ArrayLike6 = Union[List, Tuple[float, float, float, float, float, float], NDArray] 44 | 45 | # real vectors 46 | R1 = NDArray[np.floating] # R^1 47 | R2 = NDArray[np.floating] # R^2 48 | R3 = NDArray[np.floating] # R^3 49 | R4 = NDArray[np.floating] # R^4 50 | R6 = NDArray[np.floating] # R^6 51 | R8 = NDArray[np.floating] # R^8 52 | 53 | # real matrices 54 | R1x1 = NDArray # R^{1x1} matrix 55 | R2x2 = NDArray # R^{3x3} matrix 56 | R3x3 = NDArray # R^{3x3} matrix 57 | R4x4 = NDArray # R^{4x4} matrix 58 | R6x6 = NDArray # R^{6x6} matrix 59 | R8x8 = NDArray # R^{8x8} matrix 60 | 61 | R1x3 = NDArray # R^{1x3} row vector 62 | R3x1 = NDArray # R^{3x1} column vector 63 | R1x2 = NDArray # R^{1x2} row vector 64 | R2x1 = NDArray # R^{2x1} column vector 65 | 66 | Points2 = NDArray # R^{2xN} matrix 67 | Points3 = NDArray # R^{2xN} matrix 68 | 69 | RNx3 = NDArray # R^{Nx3} matrix 70 | 71 | 72 | # Lie group elements 73 | SO2Array = NDArray # SO(2) rotation matrix 74 | SE2Array = NDArray # SE(2) rigid-body transform 75 | SO3Array = NDArray # SO(3) rotation matrix 76 | SE3Array = NDArray # SE(3) rigid-body transform 77 | 78 | # Lie algebra elements 79 | so2Array = NDArray # so(2) Lie algebra of SO(2), skew-symmetrix matrix 80 | se2Array = NDArray # se(2) Lie algebra of SE(2), augmented skew-symmetrix matrix 81 | so3Array = NDArray # so(3) Lie algebra of SO(3), skew-symmetrix matrix 82 | se3Array = NDArray # se(3) Lie algebra of SE(3), augmented skew-symmetrix matrix 83 | 84 | # quaternion arrays 85 | QuaternionArray = NDArray 86 | UnitQuaternionArray = NDArray 87 | 88 | Rn = Union[R2, R3] 89 | 90 | SOnArray = Union[SO2Array, SO3Array] 91 | SEnArray = Union[SE2Array, SE3Array] 92 | 93 | sonArray = Union[so2Array, so3Array] 94 | senArray = Union[se2Array, se3Array] 95 | 96 | # __all__ = [ 97 | # overload, 98 | # Union, 99 | # List, 100 | # Tuple, 101 | # Type, 102 | # TextIO, 103 | # Any, 104 | # Callable, 105 | # Optional, 106 | # Iterator, 107 | # ArrayLike, 108 | # ArrayLike2, 109 | # ArrayLike3, 110 | # ArrayLike4, 111 | # ArrayLike6, 112 | # # real vectors 113 | # R2, 114 | # R3, 115 | # R4, 116 | # R6, 117 | # R8, 118 | # # real matrices 119 | # R2x2, 120 | # R3x3, 121 | # R4x4, 122 | # R6x6, 123 | # R8x8, 124 | # R1x3, 125 | # R3x1, 126 | # R1x2, 127 | # R2x1, 128 | # Points2, 129 | # Points3, 130 | # RNx3, 131 | # # Lie group elements 132 | # SO2Array, 133 | # SE2Array, 134 | # SO3Array, 135 | # SE3Array, 136 | # # Lie algebra elements 137 | # so2Array, 138 | # se2Array, 139 | # so3Array, 140 | # se3Array, 141 | # # quaternion arrays 142 | # QuaternionArray, 143 | # UnitQuaternionArray, 144 | # Rn, 145 | # SOnArray, 146 | # SEnArray, 147 | # sonArray, 148 | # senArray, 149 | # ] 150 | Color = Union[str, ArrayLike3] 151 | -------------------------------------------------------------------------------- /docs/generated/spatialmath.base.vectors.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | spatialmath.base.vectors — Spatial Maths package 0.7.0 8 | documentation 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 |
30 |
31 |
32 | 33 | 34 |
35 | 36 |
37 |

spatialmath.base.vectors

38 |

This modules contains functions to create and transform rotation matrices 39 | and homogeneous tranformation matrices.

40 |

Vector arguments are what numpy refers to as array_like and can be a list, 41 | tuple, numpy array, numpy row vector or numpy column vector.

42 |

Functions

43 |
44 | 45 | 46 |
47 | 48 |
49 |
50 | 109 |
110 |
111 |
112 | ©2020, Peter Corke. 113 | 114 | | 115 | Powered by Sphinx 2.4.4 116 | & Alabaster 0.7.12 117 | 118 | | 119 | Page source 121 |
122 | 123 | 124 | 125 | 126 | 127 | -------------------------------------------------------------------------------- /docs/generated/spatialmath.base.transforms2d.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | spatialmath.base.transforms2d — Spatial Maths package 0.7.0 8 | documentation 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 |
30 |
31 |
32 | 33 | 34 |
35 | 36 |
37 |

spatialmath.base.transforms2d

38 |

This modules contains functions to create and transform rotation matrices 39 | and homogeneous tranformation matrices.

40 |

Vector arguments are what numpy refers to as array_like and can be a list, 41 | tuple, numpy array, numpy row vector or numpy column vector.

42 |

Functions

43 |
44 | 45 | 46 |
47 | 48 |
49 |
50 | 109 |
110 |
111 |
112 | ©2020, Peter Corke. 113 | 114 | | 115 | Powered by Sphinx 2.4.4 116 | & Alabaster 0.7.12 117 | 118 | | 119 | Page source 121 |
122 | 123 | 124 | 125 | 126 | 127 | -------------------------------------------------------------------------------- /tests/test_baseposelist.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | import numpy as np 3 | from spatialmath.baseposelist import BasePoseList 4 | 5 | 6 | # create a subclass to test with, its value is a scalar 7 | class X(BasePoseList): 8 | def __init__(self, value=0, check=False): 9 | super().__init__() 10 | self.data = [value] 11 | 12 | @staticmethod 13 | def _identity(): 14 | return 0 15 | 16 | @property 17 | def shape(self): 18 | return (1, 1) 19 | 20 | @staticmethod 21 | def isvalid(x): 22 | return True 23 | 24 | 25 | class TestBasePoseList(unittest.TestCase): 26 | def test_constructor(self): 27 | x = X() 28 | self.assertIsInstance(x, X) 29 | self.assertEqual(len(x), 1) 30 | 31 | x = X.Empty() 32 | self.assertIsInstance(x, X) 33 | self.assertEqual(len(x), 0) 34 | 35 | x = X.Alloc(10) 36 | self.assertIsInstance(x, X) 37 | self.assertEqual(len(x), 10) 38 | for xx in x: 39 | self.assertEqual(xx.A, 0) 40 | 41 | def test_setget(self): 42 | x = X.Alloc(10) 43 | for i in range(0, 10): 44 | x[i] = X(2 * i) 45 | 46 | for i, v in enumerate(x): 47 | self.assertEqual(v.A, 2 * i) 48 | 49 | def test_append(self): 50 | x = X.Empty() 51 | for i in range(0, 10): 52 | x.append(X(i + 1)) 53 | self.assertEqual(len(x), 10) 54 | self.assertEqual([xx.A for xx in x], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]) 55 | 56 | def test_extend(self): 57 | x = X.Alloc(5) 58 | for i in range(0, 5): 59 | x[i] = X(i + 1) 60 | y = X.Alloc(5) 61 | for i in range(0, 5): 62 | y[i] = X(i + 10) 63 | x.extend(y) 64 | self.assertEqual(len(x), 10) 65 | self.assertEqual([xx.A for xx in x], [1, 2, 3, 4, 5, 10, 11, 12, 13, 14]) 66 | 67 | def test_insert(self): 68 | x = X.Alloc(10) 69 | for i in range(0, 10): 70 | x[i] = X(i + 1) 71 | x.insert(5, X(100)) 72 | self.assertEqual(len(x), 11) 73 | self.assertEqual([xx.A for xx in x], [1, 2, 3, 4, 5, 100, 6, 7, 8, 9, 10]) 74 | 75 | def test_pop(self): 76 | x = X.Alloc(10) 77 | for i in range(0, 10): 78 | x[i] = X(i + 1) 79 | 80 | y = x.pop() 81 | self.assertEqual(len(y), 1) 82 | self.assertEqual(y.A, 10) 83 | self.assertEqual(len(x), 9) 84 | self.assertEqual([xx.A for xx in x], [1, 2, 3, 4, 5, 6, 7, 8, 9]) 85 | 86 | def test_clear(self): 87 | x = X.Alloc(10) 88 | x.clear() 89 | self.assertEqual(len(x), 0) 90 | 91 | def test_reverse(self): 92 | x = X.Alloc(5) 93 | for i in range(0, 5): 94 | x[i] = X(i + 1) 95 | x.reverse() 96 | self.assertEqual(len(x), 5) 97 | self.assertEqual([xx.A for xx in x], [5, 4, 3, 2, 1]) 98 | 99 | def test_binop(self): 100 | x = X(2) 101 | y = X(3) 102 | 103 | # singelton x singleton 104 | self.assertEqual(x.binop(y, lambda x, y: x * y), [6]) 105 | self.assertEqual(x.binop(y, lambda x, y: x * y, list1=False), 6) 106 | 107 | y = X.Alloc(5) 108 | for i in range(0, 5): 109 | y[i] = X(i + 1) 110 | 111 | # singelton x non-singleton 112 | self.assertEqual(x.binop(y, lambda x, y: x * y), [2, 4, 6, 8, 10]) 113 | self.assertEqual(x.binop(y, lambda x, y: x * y, list1=False), [2, 4, 6, 8, 10]) 114 | 115 | # non-singelton x singleton 116 | self.assertEqual(y.binop(x, lambda x, y: x * y), [2, 4, 6, 8, 10]) 117 | self.assertEqual(y.binop(x, lambda x, y: x * y, list1=False), [2, 4, 6, 8, 10]) 118 | 119 | # non-singelton x non-singleton 120 | self.assertEqual(y.binop(y, lambda x, y: x * y), [1, 4, 9, 16, 25]) 121 | self.assertEqual(y.binop(y, lambda x, y: x * y, list1=False), [1, 4, 9, 16, 25]) 122 | 123 | def test_unop(self): 124 | x = X(2) 125 | 126 | f = lambda x: 2 * x 127 | 128 | self.assertEqual(x.unop(f), [4]) 129 | self.assertEqual(x.unop(f, matrix=True), np.r_[4]) 130 | 131 | x = X.Alloc(5) 132 | for i in range(0, 5): 133 | x[i] = X(i + 1) 134 | 135 | self.assertEqual(x.unop(f), [2, 4, 6, 8, 10]) 136 | y = x.unop(f, matrix=True) 137 | self.assertEqual(y.shape, (5, 1)) 138 | self.assertTrue(np.all(y - np.c_[2, 4, 6, 8, 10].T == 0)) 139 | 140 | def test_arghandler(self): 141 | pass 142 | 143 | 144 | # ---------------------------------------------------------------------------------------# 145 | if __name__ == "__main__": 146 | unittest.main() 147 | -------------------------------------------------------------------------------- /docs/generated/spatialmath.base.transforms3d.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | spatialmath.base.transforms3d — Spatial Maths package 0.7.0 8 | documentation 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 |
30 |
31 |
32 | 33 | 34 |
35 | 36 |
37 |

spatialmath.base.transforms3d

38 |

This modules contains functions to create and transform 3D rotation matrices 39 | and homogeneous tranformation matrices.

40 |

Vector arguments are what numpy refers to as array_like and can be a list, 41 | tuple, numpy array, numpy row vector or numpy column vector.

42 |

TODO:

43 |
44 |
    45 |

  • trinterp

    • 46 |

  • trjac, trjac2

    • 47 |

  • tranimate, tranimate2

    • 48 |
49 |
50 |

Functions

51 |
52 | 53 | 54 |
55 | 56 |
57 |
58 | 117 |
118 |
119 |
120 | ©2020, Peter Corke. 121 | 122 | | 123 | Powered by Sphinx 2.4.4 124 | & Alabaster 0.7.12 125 | 126 | | 127 | Page source 129 |
130 | 131 | 132 | 133 | 134 | 135 | -------------------------------------------------------------------------------- /docs/generated/spatialmath.base.transformsNd.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | spatialmath.base.transformsNd — Spatial Maths package 0.7.0 8 | documentation 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 |
30 |
31 |
32 | 33 | 34 |
35 | 36 |
37 |

spatialmath.base.transformsNd

38 |

This modules contains functions to create and transform rotation matrices 39 | and homogeneous tranformation matrices.

40 |

Vector arguments are what numpy refers to as array_like and can be a list, 41 | tuple, numpy array, numpy row vector or numpy column vector.

42 |

Versions:

43 |
44 |
    45 |

  1. Luis Fernando Lara Tobar and Peter Corke, 2008

    2. 46 |

  2. Josh Carrigg Hodson, Aditya Dua, Chee Ho Chan, 2017

    3. 47 |

  3. Peter Corke, 2020

    4. 48 |
49 |
50 |

Functions

51 |
52 | 53 | 54 |
55 | 56 |
57 |
58 | 117 |
118 |
119 |
120 | ©2020, Peter Corke. 121 | 122 | | 123 | Powered by Sphinx 2.4.4 124 | & Alabaster 0.7.12 125 | 126 | | 127 | Page source 129 |
130 | 131 | 132 | 133 | 134 | 135 | -------------------------------------------------------------------------------- /spatialmath/base/_types_311.py: -------------------------------------------------------------------------------- 1 | # for Python >= 3.9 2 | 3 | from typing import ( 4 | overload, 5 | cast, 6 | Union, 7 | List, 8 | Tuple, 9 | Type, 10 | TextIO, 11 | Any, 12 | Callable, 13 | Optional, 14 | Iterator, 15 | Self, 16 | ) 17 | from typing import Literal as L 18 | 19 | from numpy import ndarray, dtype, floating 20 | from numpy.typing import NDArray, DTypeLike 21 | 22 | # array like 23 | 24 | # these are input to many functions in spatialmath.base, and can be a list, tuple or 25 | # ndarray. The elements are generally float, but some functions accept symbolic 26 | # arguments as well, which leads to a NumPy array with dtype=object. For now 27 | # symbolics will throw a lint error. Possibly create variants ArrayLikeSym that 28 | # admits symbols and can be used for those functions that accept symbols. 29 | # 30 | # The variants like ArrayLike2 indicate that a list, tuple or ndarray of 31 | # length 2 is expected. Static checking of tuple length is possible, but not for lists. 32 | # This might be possible in future versions of Python, but for now it is a hint to the 33 | # coder about what is expected 34 | 35 | # cannot be a scalar 36 | ArrayLikePure = Union[List[float], Tuple[float, ...], ndarray[Any, dtype[floating]]] 37 | 38 | ArrayLike = Union[float, List[float], Tuple[float, ...], ndarray[Any, dtype[floating]]] 39 | ArrayLike2 = Union[ 40 | List[float], 41 | Tuple[float, float], 42 | ndarray[ 43 | Tuple[L[2]], 44 | dtype[floating], 45 | ], 46 | ] 47 | ArrayLike3 = Union[ 48 | List[float], 49 | Tuple[float, float, float], 50 | ndarray[ 51 | Tuple[L[3]], 52 | dtype[floating], 53 | ], 54 | ] 55 | ArrayLike4 = Union[ 56 | List[float], 57 | Tuple[float, float, float, float], 58 | ndarray[ 59 | Tuple[L[4]], 60 | dtype[floating], 61 | ], 62 | ] 63 | ArrayLike6 = Union[ 64 | List[float], 65 | Tuple[float, float, float, float, float, float], 66 | ndarray[ 67 | Tuple[L[6]], 68 | dtype[floating], 69 | ], 70 | ] 71 | 72 | # real vectors 73 | R1 = ndarray[ 74 | Tuple[L[1]], 75 | dtype[floating], 76 | ] # R^1 77 | R2 = ndarray[ 78 | Tuple[L[2]], 79 | dtype[floating], 80 | ] # R^2 81 | R3 = ndarray[ 82 | Tuple[L[3]], 83 | dtype[floating], 84 | ] # R^3 85 | R4 = ndarray[ 86 | Tuple[L[4]], 87 | dtype[floating], 88 | ] # R^4 89 | R6 = ndarray[ 90 | Tuple[L[6]], 91 | dtype[floating], 92 | ] # R^6 93 | 94 | R8 = ndarray[ 95 | Tuple[L[8,]], 96 | dtype[floating], 97 | ] # R^8 98 | 99 | # real matrices 100 | R1x1 = ndarray[Tuple[L[1], L[1]], dtype[floating]] # R^{1x1} matrix 101 | R2x2 = ndarray[Tuple[L[2], L[2]], dtype[floating]] # R^{2x2} matrix 102 | R3x3 = ndarray[Tuple[L[3], L[3]], dtype[floating]] # R^{3x3} matrix 103 | R4x4 = ndarray[Tuple[L[4], L[4]], dtype[floating]] # R^{4x4} matrix 104 | R6x6 = ndarray[Tuple[L[6], L[6]], dtype[floating]] # R^{6x6} matrix 105 | R8x8 = ndarray[Tuple[L[8], L[8]], dtype[floating]] # R^{8x8} matrix 106 | R1x3 = ndarray[Tuple[L[1], L[3]], dtype[floating]] # R^{1x3} row vector 107 | R3x1 = ndarray[Tuple[L[3], L[1]], dtype[floating]] # R^{3x1} column vector 108 | R1x2 = ndarray[Tuple[L[1], L[2]], dtype[floating]] # R^{1x2} row vector 109 | R2x1 = ndarray[Tuple[L[2], L[1]], dtype[floating]] # R^{2x1} column vector 110 | 111 | # Points2 = ndarray[Tuple[L[2, Any]], dtype[floating]] # R^{2xN} matrix 112 | # Points3 = ndarray[Tuple[L[3, Any]], dtype[floating]] # R^{2xN} matrix 113 | Points2 = NDArray # R^{2xN} matrix 114 | Points3 = NDArray # R^{2xN} matrix 115 | 116 | # RNx3 = ndarray[(Any, 3), dtype[floating]] # R^{Nx3} matrix 117 | RNx3 = NDArray 118 | 119 | # Lie group elements 120 | SO2Array = ndarray[Tuple[L[2], L[2]], dtype[floating]] # SO(2) rotation matrix 121 | SE2Array = ndarray[Tuple[L[3], L[3]], dtype[floating]] # SE(2) rigid-body transform 122 | SO3Array = ndarray[Tuple[L[3], L[3]], dtype[floating]] # SO(3) rotation matrix 123 | SE3Array = ndarray[Tuple[L[4], L[4]], dtype[floating]] # SE(3) rigid-body transform 124 | 125 | # Lie algebra elements 126 | so2Array = ndarray[ 127 | Tuple[L[2, 2]], dtype[floating] 128 | ] # so(2) Lie algebra of SO(2), skew-symmetrix matrix 129 | se2Array = ndarray[ 130 | Tuple[L[3, 3]], dtype[floating] 131 | ] # se(2) Lie algebra of SE(2), augmented skew-symmetrix matrix 132 | so3Array = ndarray[ 133 | Tuple[L[3, 3]], dtype[floating] 134 | ] # so(3) Lie algebra of SO(3), skew-symmetrix matrix 135 | se3Array = ndarray[ 136 | Tuple[L[4, 4]], dtype[floating] 137 | ] # se(3) Lie algebra of SE(3), augmented skew-symmetrix matrix 138 | 139 | # quaternion arrays 140 | QuaternionArray = ndarray[ 141 | Tuple[L[4,]], 142 | dtype[floating], 143 | ] 144 | UnitQuaternionArray = ndarray[ 145 | Tuple[L[4,]], 146 | dtype[floating], 147 | ] 148 | 149 | Rn = Union[R2, R3] 150 | 151 | SOnArray = Union[SO2Array, SO3Array] 152 | SEnArray = Union[SE2Array, SE3Array] 153 | 154 | sonArray = Union[so2Array, so3Array] 155 | senArray = Union[se2Array, se3Array] 156 | 157 | Color = Union[str, ArrayLike3] 158 | -------------------------------------------------------------------------------- /spatialmath/base/_types_39.py: -------------------------------------------------------------------------------- 1 | # for Python >= 3.9 2 | 3 | from typing import ( 4 | overload, 5 | cast, 6 | Union, 7 | List, 8 | Tuple, 9 | Type, 10 | TextIO, 11 | Any, 12 | Callable, 13 | Optional, 14 | Iterator, 15 | ) 16 | from typing import Literal as L 17 | from typing_extensions import Self 18 | 19 | import numpy as np 20 | from numpy import ndarray, dtype, floating 21 | from numpy.typing import NDArray, DTypeLike 22 | 23 | # array like 24 | 25 | # these are input to many functions in spatialmath.base, and can be a list, tuple or 26 | # ndarray. The elements are generally float, but some functions accept symbolic 27 | # arguments as well, which leads to a NumPy array with dtype=object. For now 28 | # symbolics will throw a lint error. Possibly create variants ArrayLikeSym that 29 | # admits symbols and can be used for those functions that accept symbols. 30 | # 31 | # The variants like ArrayLike2 indicate that a list, tuple or ndarray of 32 | # length 2 is expected. Static checking of tuple length is possible, but not for lists. 33 | # This might be possible in future versions of Python, but for now it is a hint to the 34 | # coder about what is expected 35 | 36 | 37 | ArrayLike = Union[float, List[float], Tuple[float, ...], ndarray[Any, dtype[floating]]] 38 | ArrayLikePure = Union[List[float], Tuple[float, ...], ndarray[Any, dtype[floating]]] 39 | ArrayLike2 = Union[ 40 | List[float], 41 | Tuple[float, float], 42 | ndarray[ 43 | Tuple[L[2,]], 44 | dtype[floating], 45 | ], 46 | ] 47 | ArrayLike3 = Union[ 48 | List[float], 49 | Tuple[float, float, float], 50 | ndarray[ 51 | Tuple[L[3,]], 52 | dtype[floating], 53 | ], 54 | ] 55 | ArrayLike4 = Union[ 56 | List[float], 57 | Tuple[float, float, float, float], 58 | ndarray[ 59 | Tuple[L[4,]], 60 | dtype[floating], 61 | ], 62 | ] 63 | ArrayLike6 = Union[ 64 | List[float], 65 | Tuple[float, float, float, float, float, float], 66 | ndarray[ 67 | Tuple[L[6,]], 68 | dtype[floating], 69 | ], 70 | ] 71 | 72 | # real vectors 73 | R1 = ndarray[ 74 | Tuple[L[1]], 75 | dtype[floating], 76 | ] # R^1 77 | R2 = ndarray[ 78 | Tuple[L[2]], 79 | dtype[floating], 80 | ] # R^2 81 | R3 = ndarray[ 82 | Tuple[L[3]], 83 | dtype[floating], 84 | ] # R^3 85 | R4 = ndarray[ 86 | Tuple[L[4]], 87 | dtype[floating], 88 | ] # R^4 89 | R6 = ndarray[ 90 | Tuple[L[6]], 91 | dtype[floating], 92 | ] # R^6 93 | R8 = ndarray[ 94 | Tuple[L[8]], 95 | dtype[floating], 96 | ] # R^8 97 | 98 | # real matrices 99 | R1x1 = ndarray[Tuple[L[1], L[1]], dtype[floating]] # R^{1x1} matrix 100 | R2x2 = ndarray[Tuple[L[2], L[2]], dtype[floating]] # R^{2x2} matrix 101 | R3x3 = ndarray[Tuple[L[3], L[3]], dtype[floating]] # R^{3x3} matrix 102 | R4x4 = ndarray[Tuple[L[4], L[4]], dtype[floating]] # R^{4x4} matrix 103 | R6x6 = ndarray[Tuple[L[6], L[6]], dtype[floating]] # R^{6x6} matrix 104 | R8x8 = ndarray[Tuple[L[8], L[8]], dtype[floating]] # R^{8x8} matrix 105 | R1x3 = ndarray[Tuple[L[1], L[3]], dtype[floating]] # R^{1x3} row vector 106 | R3x1 = ndarray[Tuple[L[3], L[1]], dtype[floating]] # R^{3x1} column vector 107 | R1x2 = ndarray[Tuple[L[1], L[2]], dtype[floating]] # R^{1x2} row vector 108 | R2x1 = ndarray[Tuple[L[2], L[1]], dtype[floating]] # R^{2x1} column vector 109 | 110 | # Points2 = ndarray[Tuple[L[2, Any]], dtype[floating]] # R^{2xN} matrix 111 | # Points3 = ndarray[Tuple[L[3, Any]], dtype[floating]] # R^{2xN} matrix 112 | Points2 = NDArray # R^{2xN} matrix 113 | Points3 = NDArray # R^{2xN} matrix 114 | 115 | # RNx3 = ndarray[(Any, 3), dtype[floating]] # R^{Nx3} matrix 116 | RNx3 = NDArray 117 | 118 | # Lie group elements 119 | SO2Array = ndarray[Tuple[L[2, 2]], dtype[floating]] # SO(2) rotation matrix 120 | SE2Array = ndarray[Tuple[L[3, 3]], dtype[floating]] # SE(2) rigid-body transform 121 | # SO3Array = ndarray[Tuple[L[3, 3]], dtype[floating]] 122 | SO3Array = np.ndarray[Tuple[L[3], L[3]], dtype[floating]] # SO(3) rotation matrix 123 | SE3Array = ndarray[Tuple[L[4], L[4]], dtype[floating]] # SE(3) rigid-body transform 124 | 125 | 126 | # Lie algebra elements 127 | so2Array = ndarray[ 128 | Tuple[L[2, 2]], dtype[floating] 129 | ] # so(2) Lie algebra of SO(2), skew-symmetrix matrix 130 | se2Array = ndarray[ 131 | Tuple[L[3, 3]], dtype[floating] 132 | ] # se(2) Lie algebra of SE(2), augmented skew-symmetrix matrix 133 | so3Array = ndarray[ 134 | Tuple[L[3, 3]], dtype[floating] 135 | ] # so(3) Lie algebra of SO(3), skew-symmetrix matrix 136 | se3Array = ndarray[ 137 | Tuple[L[4, 4]], dtype[floating] 138 | ] # se(3) Lie algebra of SE(3), augmented skew-symmetrix matrix 139 | 140 | # quaternion arrays 141 | QuaternionArray = ndarray[ 142 | Tuple[L[4,]], 143 | dtype[floating], 144 | ] 145 | UnitQuaternionArray = ndarray[ 146 | Tuple[L[4,]], 147 | dtype[floating], 148 | ] 149 | 150 | Rn = Union[R2, R3] 151 | 152 | SOnArray = Union[SO2Array, SO3Array] 153 | SEnArray = Union[SE2Array, SE3Array] 154 | 155 | sonArray = Union[so2Array, so3Array] 156 | senArray = Union[se2Array, se3Array] 157 | 158 | Color = Union[str, ArrayLike3] 159 | -------------------------------------------------------------------------------- /tests/base/test_graphics.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | import pytest 5 | import sys 6 | from spatialmath.base import * 7 | 8 | # test graphics primitives 9 | # TODO check they actually create artists 10 | 11 | 12 | class TestGraphics(unittest.TestCase): 13 | def teardown_method(self, method): 14 | plt.close("all") 15 | 16 | @pytest.mark.skipif( 17 | sys.platform.startswith("darwin") and sys.version_info < (3, 11), 18 | reason="tkinter bug with mac", 19 | ) 20 | def test_plotvol2(self): 21 | plotvol2(5) 22 | 23 | @pytest.mark.skipif( 24 | sys.platform.startswith("darwin") and sys.version_info < (3, 11), 25 | reason="tkinter bug with mac", 26 | ) 27 | def test_plotvol3(self): 28 | plotvol3(5) 29 | 30 | @pytest.mark.skipif( 31 | sys.platform.startswith("darwin") and sys.version_info < (3, 11), 32 | reason="tkinter bug with mac", 33 | ) 34 | def test_plot_point(self): 35 | plot_point((2, 3)) 36 | plot_point(np.r_[2, 3]) 37 | plot_point((2, 3), "x") 38 | plot_point((2, 3), "x", text="foo") 39 | 40 | @pytest.mark.skipif( 41 | sys.platform.startswith("darwin") and sys.version_info < (3, 11), 42 | reason="tkinter bug with mac", 43 | ) 44 | def test_plot_text(self): 45 | plot_text((2, 3), "foo") 46 | plot_text(np.r_[2, 3], "foo") 47 | 48 | @pytest.mark.skipif( 49 | sys.platform.startswith("darwin") and sys.version_info < (3, 11), 50 | reason="tkinter bug with mac", 51 | ) 52 | def test_plot_box(self): 53 | plot_box("r--", centre=(-2, -3), wh=(1, 1)) 54 | plot_box(lt=(1, 1), rb=(2, 0), filled=True, color="b") 55 | plot_box(lrbt=(1, 2, 0, 1), filled=True, color="b") 56 | plot_box(ltrb=(1, 0, 2, 0), filled=True, color="b") 57 | plot_box(lt=(1, 2), wh=(2, 3)) 58 | plot_box(lbwh=(1, 2, 3, 4)) 59 | plot_box(centre=(1, 2), wh=(2, 3)) 60 | 61 | @pytest.mark.skipif( 62 | sys.platform.startswith("darwin") and sys.version_info < (3, 11), 63 | reason="tkinter bug with mac", 64 | ) 65 | def test_plot_circle(self): 66 | plot_circle(1, (0, 0), "r") # red circle 67 | plot_circle(2, (0, 0), "b--") # blue dashed circle 68 | plot_circle(0.5, (0, 0), filled=True, color="y") # yellow filled circle 69 | 70 | @pytest.mark.skipif( 71 | sys.platform.startswith("darwin") and sys.version_info < (3, 11), 72 | reason="tkinter bug with mac", 73 | ) 74 | def test_ellipse(self): 75 | plot_ellipse(np.diag((1, 2)), (0, 0), "r") # red ellipse 76 | plot_ellipse(np.diag((1, 2)), (0, 0), "b--") # blue dashed ellipse 77 | plot_ellipse( 78 | np.diag((1, 2)), centre=(1, 1), filled=True, color="y" 79 | ) # yellow filled ellipse 80 | 81 | @pytest.mark.skipif( 82 | sys.platform.startswith("darwin") and sys.version_info < (3, 11), 83 | reason="tkinter bug with mac", 84 | ) 85 | def test_plot_homline(self): 86 | plot_homline((1, 2, 3)) 87 | plot_homline((2, 1, 3)) 88 | plot_homline((1, -2, 3), "k--") 89 | 90 | @pytest.mark.skipif( 91 | sys.platform.startswith("darwin") and sys.version_info < (3, 11), 92 | reason="tkinter bug with mac", 93 | ) 94 | def test_cuboid(self): 95 | plot_cuboid((1, 2, 3), color="g") 96 | plot_cuboid((1, 2, 3), centre=(2, 3, 4), color="g") 97 | plot_cuboid((1, 2, 3), filled=True, color="y") 98 | 99 | @pytest.mark.skipif( 100 | sys.platform.startswith("darwin") and sys.version_info < (3, 11), 101 | reason="tkinter bug with mac", 102 | ) 103 | def test_sphere(self): 104 | plot_sphere(0.3, color="r") 105 | plot_sphere(1, centre=(1, 1, 1), filled=True, color="b") 106 | 107 | @pytest.mark.skipif( 108 | sys.platform.startswith("darwin") and sys.version_info < (3, 11), 109 | reason="tkinter bug with mac", 110 | ) 111 | def test_ellipsoid(self): 112 | plot_ellipsoid(np.diag((1, 2, 3)), color="r") # red ellipsoid 113 | plot_ellipsoid( 114 | np.diag((1, 2, 3)), centre=(1, 2, 3), filled=True, color="y" 115 | ) # yellow filled ellipsoid 116 | 117 | @pytest.mark.skipif( 118 | sys.platform.startswith("darwin") and sys.version_info < (3, 11), 119 | reason="tkinter bug with mac", 120 | ) 121 | def test_cylinder(self): 122 | plot_cylinder(radius=0.2, centre=(0.5, 0.5, 0), height=[-0.2, 0.2]) 123 | plot_cylinder( 124 | radius=0.2, 125 | centre=(0.5, 0.5, 0), 126 | height=[-0.2, 0.2], 127 | filled=True, 128 | resolution=5, 129 | color="red", 130 | ) 131 | 132 | @pytest.mark.skipif( 133 | sys.platform.startswith("darwin") and sys.version_info < (3, 11), 134 | reason="tkinter bug with mac", 135 | ) 136 | def test_cone(self): 137 | plot_cone(radius=0.2, centre=(0.5, 0.5, 0), height=0.3) 138 | plot_cone( 139 | radius=0.2, 140 | centre=(0.5, 0.5, 0), 141 | height=0.3, 142 | filled=True, 143 | resolution=5, 144 | color="red", 145 | ) 146 | 147 | 148 | # ---------------------------------------------------------------------------------------# 149 | if __name__ == "__main__": 150 | unittest.main(buffer=True) 151 | -------------------------------------------------------------------------------- /spatialmath/base/README.md: -------------------------------------------------------------------------------- 1 | # Spatial Maths for Python 2 | 3 | Spatial maths capability underpins all of robotics and robotic vision. The aim of the `spatialmath` package is to replicate the functionality of the MATLAB® Spatial Math Toolbox while achieving the conflicting high-level design aims of being: 4 | 5 | * as similar as possible to the MATLAB function names and semantics 6 | * as Pythonic as possible 7 | 8 | More detailed design aims include: 9 | 10 | * Python3 support only 11 | * Use Python keyword arguments to replace the RTB string options supported using `tb_optparse` 12 | * Use `numpy` arrays for all rotation and homogeneous transformation matrices, as well as vectors 13 | * Functions that accept a vector can accept a list, tuple, or `np.ndarray` 14 | * By default all `np.ndarray` vectors have the shape `(N,)` but functions also accept row `(1,N)` and column `(N,1)` vectors. This is a gnarly aspect of numpy. 15 | * Unlike RTB these functions do not support sequences, that functionality is supported by the pose classes `SO2`, `SE2`, `SO3`, `SE3`. 16 | 17 | Quick example: 18 | 19 | ``` 20 | import spatialmath as sm 21 | 22 | R = sm.SO3.Rx(30, 'deg') 23 | print(R) 24 | 1 0 0 25 | 0 0.866025 -0.5 26 | 0 0.5 0.866025 27 | ``` 28 | which constructs a rotation about the x-axis by 30 degrees. 29 | 30 | 31 | ## Low-level spatial math 32 | 33 | First lets import the low-level transform functions 34 | 35 | ``` 36 | >>> from spatialmath.base.transforms import * 37 | ``` 38 | 39 | Let's start with a familiar and tangible example: 40 | 41 | ``` 42 | >>> rotx(0.3) 43 | array([[ 1. , 0. , 0. ], 44 | [ 0. , 0.95533649, -0.29552021], 45 | [ 0. , 0.29552021, 0.95533649]]) 46 | 47 | >>> rotx(30, unit='deg') 48 | Out[438]: 49 | array([[ 1. , 0. , 0. ], 50 | [ 0. , 0.8660254, -0.5 ], 51 | [ 0. , 0.5 , 0.8660254]]) 52 | ``` 53 | Remember that these are `numpy` arrays so to perform matrix multiplication you need to use the `@` operator, for example 54 | 55 | ``` 56 | rotx(0.3) @ roty(0.2) 57 | ``` 58 | 59 | Note that the `*` operator performs element-wise multiplication, equivalent to the MATLAB `.*` operator. 60 | 61 | We also support multiple ways of passing vector information to functions that require it: 62 | 63 | * as separate positional arguments 64 | 65 | ``` 66 | transl2(1, 2) 67 | Out[442]: 68 | array([[1., 0., 1.], 69 | [0., 1., 2.], 70 | [0., 0., 1.]]) 71 | ``` 72 | 73 | * as a list or a tuple 74 | 75 | ``` 76 | transl2( [1,2] ) 77 | Out[443]: 78 | array([[1., 0., 1.], 79 | [0., 1., 2.], 80 | [0., 0., 1.]]) 81 | 82 | transl2( (1,2) ) 83 | Out[444]: 84 | array([[1., 0., 1.], 85 | [0., 1., 2.], 86 | [0., 0., 1.]]) 87 | ``` 88 | 89 | * or as a `numpy` array 90 | 91 | ``` 92 | transl2( np.array([1,2]) ) 93 | Out[445]: 94 | array([[1., 0., 1.], 95 | [0., 1., 2.], 96 | [0., 0., 1.]]) 97 | ``` 98 | 99 | trplot example 100 | packages, animation 101 | 102 | There is a single module that deals with quaternions, unit or not, and the representation is a `numpy` array of four elements. As above, functions can accept the `numpy` array, a list, dict or `numpy` row or column vectors. 103 | 104 | ``` 105 | >>> from spatialmath.base.quaternion import * 106 | >>> q = qqmul([1,2,3,4], [5,6,7,8]) 107 | >>> q 108 | array([-60, 12, 30, 24]) 109 | >>> qprint(q) 110 | -60.000000 < 12.000000, 30.000000, 24.000000 > 111 | >>> qnorm(q) 112 | 72.24956747275377 113 | ``` 114 | 115 | 116 | ## High-level classes 117 | 118 | ``` 119 | >>> from spatialmath import * 120 | >>> SO2(.1) 121 | [[ 0.99500417 -0.09983342] 122 | [ 0.09983342 0.99500417]] 123 | ``` 124 | 125 | These classes abstract the low-level numpy arrays into objects that obey the rules associated with the mathematical groups SO(2), SE(2), SO(3), SE(3) as well as twists and quaternions. pose classes `SO2`, `SE2`, `SO3`, `SE3`. 126 | 127 | Using classes ensures type safety, for example it stops us mixing a 2D homogeneous transformation with a 3D rotation matrix -- both are 3x3 matrices. 128 | 129 | These classes are all derived from two parent classes: 130 | 131 | * `RTBPose` which provides common functionality for all 132 | * `UserList` which provdides the ability to act like a list 133 | 134 | The latter is important because frequnetly in robotics we want a sequence, a trajectory, of rotation matrices or poses. However a list of these items has the type `list` and the elements are not enforced to be homogeneous, ie. a list could contain a mixture of classes. 135 | 136 | Another option would be to create a `numpy` array of these objects, the upside being it could be a multi-dimensional array. The downside is that again the array is not guaranteed to be homogeneous. 137 | 138 | 139 | The approach adopted here is to give these classes list superpowers. Using the example of SE(3) but applicable to all 140 | 141 | ``` 142 | T = transl(1,2,3) # create a 4x4 np.array 143 | 144 | a = SE3(T) 145 | a.append(a) # append a copy 146 | a.append(a) # append a copy 147 | type(a) 148 | len(a) 149 | a[1] # extract one element of the list 150 | for x in a: 151 | # do a thing 152 | ``` 153 | 154 | ## Symbolic support 155 | 156 | Some functions have support for symbolic variables, for example 157 | 158 | ``` 159 | import sympy 160 | 161 | theta = sym.symbols('theta') 162 | print(rotx(theta)) 163 | [[1 0 0] 164 | [0 cos(theta) -sin(theta)] 165 | [0 sin(theta) cos(theta)]] 166 | ``` 167 | 168 | The resulting `numpy` array is an array of symbolic objects not numbers – the constants are also symbolic objects. You can read the elements of the matrix 169 | 170 | ``` 171 | a = T[0,0] 172 | 173 | a 174 | Out[258]: 1 175 | 176 | type(a) 177 | Out[259]: int 178 | 179 | a = T[1,1] 180 | a 181 | Out[256]: 182 | cos(theta) 183 | type(a) 184 | Out[255]: cos 185 | ``` 186 | We see that the symbolic constants are converted back to Python numeric types on read. 187 | 188 | Similarly when we assign an element or slice of the symbolic matrix to a numeric value, they are converted to symbolic constants on the way in. 189 | 190 | 191 | 192 | ``` 193 | T[0,3] = 22 194 | print(T) 195 | [[1 0 0 22] 196 | [0 cos(theta) -sin(theta) 0] 197 | [0 sin(theta) cos(theta) 0] 198 | [0 0 0 1]] 199 | ``` 200 | but you can't write a symbolic value into a floating point matrix 201 | 202 | ``` 203 | T=trotx(0.2) 204 | 205 | T[0,3]=theta 206 | Traceback (most recent call last): 207 | 208 | File "", line 1, in 209 | T[0,3]=th 210 | 211 | File "/opt/anaconda3/lib/python3.7/site-packages/sympy/core/expr.py", line 325, in __float__ 212 | raise TypeError("can't convert expression to float") 213 | 214 | TypeError: can't convert expression to float 215 | ``` 216 | 217 | | Function | Symbolic support | 218 | |----------|------------------| 219 | | rot2 | yes | 220 | | transl2 | yes | 221 | | rotx | yes | 222 | | roty | yes | 223 | | rotz | yes | 224 | | transl | yes | 225 | | r2t | yes | 226 | | t2r | yes | 227 | | rotx | yes | 228 | | rotx | yes | 229 | -------------------------------------------------------------------------------- /spatialmath/base/__init__.py: -------------------------------------------------------------------------------- 1 | # Part of Spatial Math Toolbox for Python 2 | # Copyright (c) 2000 Peter Corke 3 | # MIT Licence, see details in top-level file: LICENCE 4 | 5 | from spatialmath.base.argcheck import * # lgtm [py/polluting-import] 6 | from spatialmath.base.quaternions import * # lgtm [py/polluting-import] 7 | from spatialmath.base.transforms2d import * # lgtm [py/polluting-import] 8 | from spatialmath.base.transforms3d import * # lgtm [py/polluting-import] 9 | from spatialmath.base.transformsNd import * # lgtm [py/polluting-import] 10 | from spatialmath.base.vectors import * # lgtm [py/polluting-import] 11 | from spatialmath.base.symbolic import * # lgtm [py/polluting-import] 12 | from spatialmath.base.animate import * # lgtm [py/polluting-import] 13 | from spatialmath.base.graphics import * # lgtm [py/polluting-import] 14 | from spatialmath.base.numeric import * # lgtm [py/polluting-import] 15 | 16 | from spatialmath.base.argcheck import ( 17 | assertmatrix, 18 | ismatrix, 19 | getvector, 20 | assertvector, 21 | isvector, 22 | isscalar, 23 | getunit, 24 | isnumberlist, 25 | isvectorlist, 26 | ) 27 | 28 | # from spatialmath.base.quaternions import ( 29 | # pure, 30 | # qnorm, 31 | # unit, 32 | # isunit, 33 | # isequal, 34 | # q2v, 35 | # v2q, 36 | # qqmul, 37 | # inner, 38 | # qvmul, 39 | # vvmul, 40 | # qpow, 41 | # conj, 42 | # q2r, 43 | # r2q, 44 | # slerp, 45 | # rand, 46 | # matrix, 47 | # dot, 48 | # dotb, 49 | # angle, 50 | # qprint, 51 | # ) 52 | # from spatialmath.base.transforms2d import ( 53 | # rot2, 54 | # trot2, 55 | # transl2, 56 | # ishom2, 57 | # isrot2, 58 | # trlog2, 59 | # trexp2, 60 | # tr2jac2, 61 | # trinterp2, 62 | # trprint2, 63 | # trplot2, 64 | # tranimate2, 65 | # xyt2tr, 66 | # tr2xyt, 67 | # trinv2, 68 | # ) 69 | # from spatialmath.base.transforms3d import ( 70 | # rotx, 71 | # roty, 72 | # rotz, 73 | # trotx, 74 | # troty, 75 | # trotz, 76 | # transl, 77 | # ishom, 78 | # isrot, 79 | # rpy2r, 80 | # rpy2tr, 81 | # eul2r, 82 | # eul2tr, 83 | # angvec2r, 84 | # angvec2tr, 85 | # exp2r, 86 | # exp2tr, 87 | # oa2r, 88 | # oa2tr, 89 | # tr2angvec, 90 | # tr2eul, 91 | # tr2rpy, 92 | # trlog, 93 | # trexp, 94 | # trnorm, 95 | # trinterp, 96 | # delta2tr, 97 | # trinv, 98 | # tr2delta, 99 | # tr2jac, 100 | # rpy2jac, 101 | # eul2jac, 102 | # exp2jac, 103 | # rot2jac, 104 | # angvelxform, 105 | # trprint, 106 | # trplot, 107 | # tranimate, 108 | # ) 109 | # from spatialmath.base.transformsNd import ( 110 | # t2r, 111 | # r2t, 112 | # tr2rt, 113 | # rt2tr, 114 | # Ab2M, 115 | # isR, 116 | # isskew, 117 | # isskewa, 118 | # iseye, 119 | # skew, 120 | # vex, 121 | # skewa, 122 | # vexa, 123 | # h2e, 124 | # e2h, 125 | # homtrans, 126 | # rodrigues, 127 | # ) 128 | from spatialmath.base.vectors import ( 129 | colvec, 130 | unitvec, 131 | unitvec_norm, 132 | norm, 133 | normsq, 134 | isunitvec, 135 | iszerovec, 136 | isunittwist, 137 | isunittwist2, 138 | unittwist, 139 | unittwist_norm, 140 | unittwist2, 141 | angdiff, 142 | removesmall, 143 | cross, 144 | iszero, 145 | wrap_0_2pi, 146 | wrap_mpi_pi, 147 | ) 148 | 149 | # from spatialmath.base.symbolic import * 150 | # from spatialmath.base.animate import Animate, Animate2 151 | # from spatialmath.base.graphics import ( 152 | # plotvol2, 153 | # plotvol3, 154 | # plot_point, 155 | # plot_text, 156 | # plot_box, 157 | # plot_poly, 158 | # circle, 159 | # ellipse, 160 | # sphere, 161 | # ellipsoid, 162 | # plot_box, 163 | # plot_circle, 164 | # plot_ellipse, 165 | # plot_homline, 166 | # plot_sphere, 167 | # plot_ellipsoid, 168 | # plot_cylinder, 169 | # plot_cone, 170 | # plot_cuboid, 171 | # axes_logic, 172 | # isnotebook, 173 | # ) 174 | # from spatialmath.base.numeric import numjac, array2str, bresenham 175 | 176 | 177 | __all__ = [ 178 | # spatialmath.base.argcheck 179 | "assertmatrix", 180 | "ismatrix", 181 | "getvector", 182 | "assertvector", 183 | "isvector", 184 | "isscalar", 185 | "getunit", 186 | "isnumberlist", 187 | "isvectorlist", 188 | # spatialmath.base.quaternions 189 | "qpure", 190 | "qnorm", 191 | "qunit", 192 | "qisunit", 193 | "qisequal", 194 | "q2v", 195 | "v2q", 196 | "qqmul", 197 | "qinner", 198 | "qvmul", 199 | "vvmul", 200 | "qpow", 201 | "qconj", 202 | "q2r", 203 | "r2q", 204 | "qslerp", 205 | "qrand", 206 | "qmatrix", 207 | "qdot", 208 | "qdotb", 209 | "qangle", 210 | "qprint", 211 | "q2str", 212 | # spatialmath.base.transforms2d 213 | "rot2", 214 | "trot2", 215 | "transl2", 216 | "ishom2", 217 | "isrot2", 218 | "trlog2", 219 | "trexp2", 220 | "trnorm2", 221 | "tr2jac2", 222 | "trinterp2", 223 | "trprint2", 224 | "trplot2", 225 | "tranimate2", 226 | "xyt2tr", 227 | "tr2xyt", 228 | "trinv2", 229 | # spatialmath.base.transforms3d 230 | "rotx", 231 | "roty", 232 | "rotz", 233 | "trotx", 234 | "troty", 235 | "trotz", 236 | "transl", 237 | "ishom", 238 | "isrot", 239 | "rpy2r", 240 | "rpy2tr", 241 | "eul2r", 242 | "eul2tr", 243 | "angvec2r", 244 | "angvec2tr", 245 | "exp2r", 246 | "exp2tr", 247 | "oa2r", 248 | "oa2tr", 249 | "rodrigues", 250 | "tr2angvec", 251 | "tr2eul", 252 | "tr2rpy", 253 | "trlog", 254 | "trexp", 255 | "trnorm", 256 | "trinterp", 257 | "delta2tr", 258 | "trinv", 259 | "tr2delta", 260 | "tr2jac", 261 | "tr2adjoint", 262 | "rpy2jac", 263 | "eul2jac", 264 | "exp2jac", 265 | "rot2jac", 266 | "trprint", 267 | "trplot", 268 | "tranimate", 269 | "tr2x", 270 | "x2tr", 271 | "r2x", 272 | "x2r", 273 | "rotvelxform", 274 | "rotvelxform_inv_dot", 275 | # deprecated 276 | "angvelxform", 277 | "angvelxform_dot", 278 | # spatialmath.base.transformsNd 279 | "t2r", 280 | "r2t", 281 | "tr2rt", 282 | "rt2tr", 283 | "Ab2M", 284 | "isR", 285 | "isskew", 286 | "isskewa", 287 | "iseye", 288 | "skew", 289 | "vex", 290 | "skewa", 291 | "vexa", 292 | "h2e", 293 | "e2h", 294 | "homtrans", 295 | # spatialmath.base.vectors 296 | "colvec", 297 | "unitvec", 298 | "unitvec_norm", 299 | "norm", 300 | "normsq", 301 | "isunitvec", 302 | "iszerovec", 303 | "isunittwist", 304 | "isunittwist2", 305 | "unittwist", 306 | "unittwist_norm", 307 | "unittwist2", 308 | "angdiff", 309 | "removesmall", 310 | "cross", 311 | "iszero", 312 | "wrap_0_2pi", 313 | "wrap_mpi_pi", 314 | "wrap_0_pi", 315 | # spatialmath.base.animate 316 | "Animate", 317 | "Animate2", 318 | # spatial.base.graphics 319 | "plotvol2", 320 | "plotvol3", 321 | "plot_point", 322 | "plot_text", 323 | "plot_box", 324 | "plot_polygon", 325 | "circle", 326 | "ellipse", 327 | "sphere", 328 | "ellipsoid", 329 | "plot_box", 330 | "plot_arrow", 331 | "plot_circle", 332 | "plot_ellipse", 333 | "plot_homline", 334 | "plot_sphere", 335 | "plot_ellipsoid", 336 | "plot_cylinder", 337 | "plot_cone", 338 | "plot_cuboid", 339 | "axes_logic", 340 | "expand_dims", 341 | "isnotebook", 342 | # spatial.base.numeric 343 | "numjac", 344 | "numhess", 345 | "array2str", 346 | "str2array", 347 | "bresenham", 348 | "mpq_point", 349 | "gauss1d", 350 | "gauss2d", 351 | ] 352 | -------------------------------------------------------------------------------- /docs/source/conf.py: -------------------------------------------------------------------------------- 1 | # spatialmath 2 | # Configuration file for the Sphinx documentation builder. 3 | # 4 | # This file only contains a selection of the most common options. For a full 5 | # list see the documentation: 6 | # https://www.sphinx-doc.org/en/master/usage/configuration.html 7 | 8 | # -- Path setup -------------------------------------------------------------- 9 | 10 | # If extensions (or modules to document with autodoc) are in another directory, 11 | # add these directories to sys.path here. If the directory is relative to the 12 | # documentation root, use os.path.abspath to make it absolute, like shown here. 13 | # 14 | 15 | # sys.path.insert(0, os.path.abspath('.')) 16 | # sys.path.insert(0, os.path.abspath('..')) 17 | 18 | 19 | # -- Project information ----------------------------------------------------- 20 | 21 | project = "Spatial Maths package" 22 | copyright = "2020-, Peter Corke." 23 | author = "Peter Corke" 24 | try: 25 | import spatialmath 26 | 27 | version = spatialmath.__version__ 28 | except AttributeError: 29 | import re 30 | 31 | with open("../../pyproject.toml", "r") as f: 32 | m = re.compile(r'version\s*=\s*"([0-9\.]+)"').search(f.read()) 33 | version = m[1] 34 | 35 | # -- General configuration --------------------------------------------------- 36 | 37 | # Add any Sphinx extension module names here, as strings. They can be 38 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 39 | # ones. 40 | extensions = [ 41 | "sphinx.ext.autodoc", 42 | "sphinx.ext.todo", 43 | "sphinx.ext.viewcode", 44 | "sphinx.ext.mathjax", 45 | "sphinx.ext.coverage", 46 | "sphinx.ext.doctest", 47 | "sphinx.ext.inheritance_diagram", 48 | "matplotlib.sphinxext.plot_directive", 49 | "sphinx_autodoc_typehints", 50 | "sphinx_autorun", 51 | "sphinx.ext.intersphinx", 52 | "sphinx_favicon", 53 | ] 54 | #'sphinx.ext.autosummary', 55 | # typehints_use_signature_return = True 56 | 57 | # inheritance_node_attrs = dict(style='rounded,filled', fillcolor='lightblue') 58 | inheritance_node_attrs = dict(style="rounded") 59 | 60 | autosummary_generate = True 61 | autodoc_member_order = "groupwise" 62 | # bysource 63 | 64 | # options for spinx_autorun, used for inline examples 65 | # choose UTF-8 encoding to allow for Unicode characters, eg. ansitable 66 | # Python session setup, turn off color printing for SE3, set NumPy precision 67 | autorun_languages = {} 68 | autorun_languages["pycon_output_encoding"] = "UTF-8" 69 | autorun_languages["pycon_input_encoding"] = "UTF-8" 70 | autorun_languages[ 71 | "pycon_runfirst" 72 | ] = """ 73 | from spatialmath import SE3 74 | SE3._color = False 75 | import numpy as np 76 | np.set_printoptions(precision=4, suppress=True) 77 | from ansitable import ANSITable 78 | ANSITable._color = False 79 | """ 80 | 81 | 82 | # Add any paths that contain templates here, relative to this directory. 83 | templates_path = ["_templates"] 84 | 85 | # List of patterns, relative to source directory, that match files and 86 | # directories to ignore when looking for source files. 87 | # This pattern also affects html_static_path and html_extra_path. 88 | exclude_patterns = ["test_*"] 89 | 90 | add_module_names = False 91 | # -- Options for HTML output ------------------------------------------------- 92 | 93 | # The theme to use for HTML and HTML Help pages. See the documentation for 94 | # a list of builtin themes. 95 | # 96 | html_theme = "sphinx_rtd_theme" 97 | # html_theme = 'alabaster' 98 | # html_theme = 'pyramid' 99 | # html_theme = 'sphinxdoc' 100 | 101 | html_theme_options = { 102 | #'github_user': 'petercorke', 103 | #'github_repo': 'spatialmath-python', 104 | #'logo_name': False, 105 | "logo_only": False, 106 | #'description': 'Spatial maths and geometry for Python', 107 | "display_version": True, 108 | "prev_next_buttons_location": "both", 109 | "analytics_id": "G-11Q6WJM565", 110 | } 111 | html_logo = "../figs/CartesianSnakes_LogoW.png" 112 | 113 | # Add any paths that contain custom static files (such as style sheets) here, 114 | # relative to this directory. They are copied after the builtin static files, 115 | # so a file named "default.css" will overwrite the builtin "default.css". 116 | # html_static_path = ['_static'] 117 | 118 | # autodoc_mock_imports = ["numpy", "scipy"] 119 | html_last_updated_fmt = "%d-%b-%Y" 120 | # extensions = ['rst2pdf.pdfbuilder'] 121 | # pdf_documents = [('index', u'rst2pdf', u'Sample rst2pdf doc', u'Your Name'),] 122 | latex_engine = "xelatex" 123 | # maybe need to set graphics path in here somewhere 124 | # \graphicspath{{figures/}{../figures/}{C:/Users/me/Documents/project/figures/}} 125 | # https://stackoverflow.com/questions/63452024/how-to-include-image-files-in-sphinx-latex-pdf-files 126 | latex_elements = { 127 | # The paper size ('letterpaper' or 'a4paper'). 128 | "papersize": "a4paper", 129 | #'releasename':" ", 130 | # Sonny, Lenny, Glenn, Conny, Rejne, Bjarne and Bjornstrup 131 | # 'fncychap': '\\usepackage[Lenny]{fncychap}', 132 | "fncychap": "\\usepackage{fncychap}", 133 | } 134 | 135 | # -------- RVC maths notation -------------------------------------------------------# 136 | 137 | # see https://stackoverflow.com/questions/9728292/creating-latex-math-macros-within-sphinx 138 | mathjax3_config = { 139 | "tex": { 140 | "macros": { 141 | # RVC Math notation 142 | # - not possible to do the if/then/else approach 143 | # - subset only 144 | "presup": [r"\,{}^{\scriptscriptstyle #1}\!", 1], 145 | # groups 146 | "SE": [r"\mathbf{SE}(#1)", 1], 147 | "SO": [r"\mathbf{SO}(#1)", 1], 148 | "se": [r"\mathbf{se}(#1)", 1], 149 | "so": [r"\mathbf{so}(#1)", 1], 150 | # vectors 151 | "vec": [r"\boldsymbol{#1}", 1], 152 | "dvec": [r"\dot{\boldsymbol{#1}}", 1], 153 | "ddvec": [r"\ddot{\boldsymbol{#1}}", 1], 154 | "fvec": [r"\presup{#1}\boldsymbol{#2}", 2], 155 | "fdvec": [r"\presup{#1}\dot{\boldsymbol{#2}}", 2], 156 | "fddvec": [r"\presup{#1}\ddot{\boldsymbol{#2}}", 2], 157 | "norm": [r"\Vert #1 \Vert", 1], 158 | # matrices 159 | "mat": [r"\mathbf{#1}", 1], 160 | "dmat": [r"\dot{\mathbf{#1}}", 1], 161 | "fmat": [r"\presup{#1}\mathbf{#2}", 2], 162 | # skew matrices 163 | "sk": [r"\left[#1\right]", 1], 164 | "skx": [r"\left[#1\right]_{\times}", 1], 165 | "vex": [r"\vee\left( #1\right)", 1], 166 | "vexx": [r"\vee_{\times}\left( #1\right)", 1], 167 | # quaternions 168 | "q": r"\mathring{q}", 169 | "fq": [r"\presup{#1}\mathring{q}", 1], 170 | } 171 | } 172 | } 173 | 174 | 175 | autorun_languages = {} 176 | autorun_languages["pycon_output_encoding"] = "UTF-8" 177 | autorun_languages["pycon_input_encoding"] = "UTF-8" 178 | autorun_languages[ 179 | "pycon_runfirst" 180 | ] = """ 181 | from spatialmath import SE3 182 | SE3._color = False 183 | import numpy as np 184 | np.set_printoptions(precision=4, suppress=True) 185 | """ 186 | 187 | intersphinx_mapping = { 188 | "numpy": ("http://docs.scipy.org/doc/numpy/", None), 189 | "scipy": ("http://docs.scipy.org/doc/scipy/reference/", None), 190 | "matplotlib": ("https://matplotlib.org/stable/", None), 191 | } 192 | 193 | # -------- Options favicon -------------------------------------------------------# 194 | 195 | html_static_path = ["_static"] 196 | # create favicons online using https://favicon.io/favicon-converter/ 197 | favicons = [ 198 | { 199 | "rel": "icon", 200 | "sizes": "16x16", 201 | "href": "favicon-16x16.png", 202 | "type": "image/png", 203 | }, 204 | { 205 | "rel": "icon", 206 | "sizes": "32x32", 207 | "href": "favicon-32x32.png", 208 | "type": "image/png", 209 | }, 210 | { 211 | "rel": "apple-touch-icon", 212 | "sizes": "180x180", 213 | "href": "apple-touch-icon.png", 214 | "type": "image/png", 215 | }, 216 | { 217 | "rel": "android-chrome", 218 | "sizes": "192x192", 219 | "href": "android-chrome-192x192.png", 220 | "type": "image/png", 221 | }, 222 | { 223 | "rel": "android-chrome", 224 | "sizes": "512x512", 225 | "href": "android-chrome-512x512.png", 226 | "type": "image/png", 227 | }, 228 | ] 229 | 230 | autodoc_type_aliases = {"SO3Array": "SO3Array"} 231 | -------------------------------------------------------------------------------- /spatialmath/base/symbolic.py: -------------------------------------------------------------------------------- 1 | # Part of Spatial Math Toolbox for Python 2 | # Copyright (c) 2000 Peter Corke 3 | # MIT Licence, see details in top-level file: LICENCE 4 | 5 | """ 6 | This package provides a light-weight wrapper to support use of SymPy. It 7 | generalizes some common functions so that they can accept numerical or 8 | Symbolic arguments. 9 | 10 | If SymPy is not installed then only the standard numeric operations are 11 | supported. 12 | """ 13 | 14 | import math 15 | from spatialmath.base.types import * 16 | 17 | try: # pragma: no cover 18 | # print('Using SymPy') 19 | import sympy 20 | 21 | _symbolics = True 22 | symtype = (sympy.Expr,) 23 | from sympy import Symbol 24 | 25 | except ImportError: # pragma: no cover 26 | # SymPy is not installed 27 | _symbolics = False 28 | symtype = () 29 | Symbol = Any 30 | 31 | 32 | # ---------------------------------------------------------------------------------------# 33 | 34 | if _symbolics: 35 | 36 | def symbol( 37 | name: str, real: Optional[bool] = True 38 | ) -> Union[Symbol, Tuple[Symbol, ...]]: 39 | """ 40 | Create symbolic variables 41 | 42 | :param name: symbol names 43 | :type name: str 44 | :param real: assume variable is real, defaults to True 45 | :type real: bool, optional 46 | :return: SymPy symbols 47 | :rtype: sympy 48 | 49 | .. runblock:: pycon 50 | 51 | >>> from spatialmath.base.symbolic import * 52 | >>> theta = symbol('theta') 53 | >>> theta 54 | >>> theta, psi = symbol('theta psi') 55 | >>> theta 56 | >>> psi 57 | >>> q = symbol('q_:6') 58 | >>> q 59 | 60 | .. note:: In Jupyter symbols are pretty printed. 61 | 62 | - symbols named after greek letters will appear as greek letters 63 | - underscore means subscript as it does in LaTex, so the symbols ``q`` 64 | above will be subscripted. 65 | 66 | :seealso: :func:`sympy.symbols` 67 | """ 68 | return sympy.symbols(name, real=real) 69 | 70 | 71 | def issymbol(var: Any) -> bool: 72 | """ 73 | Test if variable is symbolic 74 | 75 | :param var: variable to test 76 | :return: whether variable is symbolic 77 | :rtype: bool 78 | 79 | .. runblock:: pycon 80 | 81 | >>> from spatialmath.base.symbolic import * 82 | >>> theta = symbol('theta') 83 | >>> issymbol(theta) 84 | >>> issymbol(3.4) 85 | 86 | """ 87 | if _symbolics: 88 | if isinstance(var, (list, tuple)): 89 | return any([isinstance(x, symtype) for x in var]) 90 | else: 91 | return isinstance(var, symtype) 92 | else: 93 | return False 94 | 95 | 96 | @overload 97 | def sin(theta: float) -> float: 98 | ... 99 | 100 | 101 | @overload 102 | def sin(theta: Symbol) -> Symbol: 103 | ... 104 | 105 | 106 | def sin(theta): 107 | """ 108 | Generalized sine function 109 | 110 | :param θ: argument 111 | :type θ: float or symbolic 112 | :return: sin(θ) 113 | :rtype: float or symbolic 114 | 115 | .. runblock:: pycon 116 | 117 | >>> from spatialmath.base.symbolic import * 118 | >>> theta = symbol('theta') 119 | >>> sin(theta) 120 | >>> sin(0.5) 121 | 122 | :seealso: :func:`sympy.sin` 123 | """ 124 | if issymbol(theta): 125 | return sympy.sin(theta) 126 | else: 127 | return math.sin(theta) 128 | 129 | 130 | @overload 131 | def cos(theta: float) -> float: 132 | ... 133 | 134 | 135 | @overload 136 | def cos(theta: Symbol) -> Symbol: 137 | ... 138 | 139 | 140 | def cos(theta): 141 | """ 142 | Generalized cosine function 143 | 144 | :param θ: argument 145 | :type θ: float or symbolic 146 | :return: cos(θ) 147 | :rtype: float or symbolic 148 | 149 | .. runblock:: pycon 150 | 151 | >>> from spatialmath.base.symbolic import * 152 | >>> theta = symbol('theta') 153 | >>> cos(theta) 154 | >>> cos(0.5) 155 | 156 | :seealso: :func:`sympy.cos` 157 | """ 158 | if issymbol(theta): 159 | return sympy.cos(theta) 160 | else: 161 | return math.cos(theta) 162 | 163 | 164 | @overload 165 | def tan(theta: float) -> float: 166 | ... 167 | 168 | 169 | @overload 170 | def tan(theta: Symbol) -> Symbol: 171 | ... 172 | 173 | 174 | def tan(theta): 175 | """ 176 | Generalized tangent function 177 | 178 | :param θ: argument 179 | :type θ: float or symbolic 180 | :return: tan(θ) 181 | :rtype: float or symbolic 182 | 183 | .. runblock:: pycon 184 | 185 | >>> from spatialmath.base.symbolic import * 186 | >>> theta = symbol('theta') 187 | >>> tan(theta) 188 | >>> tan(0.5) 189 | 190 | :seealso: :func:`sympy.cos` 191 | """ 192 | if issymbol(theta): 193 | return sympy.tan(theta) 194 | else: 195 | return math.tan(theta) 196 | 197 | 198 | @overload 199 | def sqrt(theta: float) -> float: 200 | ... 201 | 202 | 203 | @overload 204 | def sqrt(theta: Symbol) -> Symbol: 205 | ... 206 | 207 | 208 | def sqrt(v): 209 | """ 210 | Generalized sqrt function 211 | 212 | :param v: argument 213 | :type v: float or symbolic 214 | :return: √ v 215 | :rtype: float or symbolic 216 | 217 | .. runblock:: pycon 218 | 219 | >>> from spatialmath.base.symbolic import * 220 | >>> x = symbol('x') 221 | >>> sqrt(x ** 2) 222 | >>> sqrt(4) 223 | 224 | :seealso: :func:`sympy.sqrt` 225 | """ 226 | if issymbol(v): 227 | return sympy.sqrt(v) 228 | else: 229 | return math.sqrt(v) 230 | 231 | 232 | def zero() -> Symbol: 233 | """ 234 | Symbolic constant: zero 235 | 236 | :return: 0 237 | :rtype: symbolic 238 | 239 | .. runblock:: pycon 240 | 241 | >>> from spatialmath.base.symbolic import * 242 | >>> x = symbol('x') 243 | >>> zero() 244 | >>> x + zero() 245 | 246 | :seealso: :func:`sympy.S.Zero` 247 | """ 248 | return sympy.S.Zero 249 | 250 | 251 | def one() -> Symbol: 252 | """ 253 | Symbolic constant: one 254 | 255 | :return: 1 256 | :rtype: symbolic 257 | 258 | .. runblock:: pycon 259 | 260 | >>> from spatialmath.base.symbolic import * 261 | >>> x = symbol('x') 262 | >>> one() 263 | >>> one() * x 264 | 265 | :seealso: :func:`sympy.S.One` 266 | """ 267 | return sympy.S.One 268 | 269 | 270 | def negative_one() -> Symbol: 271 | """ 272 | Symbolic constant: negative one 273 | 274 | :return: -1 275 | :rtype: symbolic 276 | 277 | .. runblock:: pycon 278 | 279 | >>> from spatialmath.base.symbolic import * 280 | >>> x = symbol('x') 281 | >>> negative_one() 282 | >>> negative_one() * x 283 | 284 | :seealso: :func:`sympy.S.NegativeOne` 285 | """ 286 | return sympy.S.NegativeOne 287 | 288 | 289 | def pi() -> Symbol: 290 | """ 291 | Symbolic constant: pi 292 | 293 | :return: π 294 | :rtype: symbolic 295 | 296 | .. runblock:: pycon 297 | 298 | >>> from spatialmath.base.symbolic import * 299 | >>> import math 300 | >>> sin(pi()) 301 | >>> sin(math.pi) 302 | 303 | :seealso: :func:`sympy.S.Pi` 304 | """ 305 | return sympy.S.Pi 306 | 307 | 308 | def simplify(x: Symbol) -> Symbol: 309 | """ 310 | Symbolic simplification 311 | 312 | :param x: expression to simplify 313 | :type x: symbolic 314 | :return: -1 315 | :rtype: symbolic 316 | 317 | .. runblock:: pycon 318 | 319 | >>> from spatialmath.base.symbolic import * 320 | >>> x = symbol('x') 321 | >>> y = (x - 1) * (x + 1) - x ** 2 322 | >>> y 323 | >>> simplify(y) 324 | 325 | :seealso: :func:`sympy.simplify` 326 | """ 327 | if _symbolics: 328 | return sympy.simplify(x) 329 | else: 330 | return x 331 | 332 | 333 | def det(x): 334 | """ 335 | Symbolic determinant 336 | 337 | :param m: matrix 338 | :type x: ndarray with symbolic elements 339 | :return: determinant 340 | :rtype: ndarray with symbolic elements 341 | 342 | .. runblock:: pycon 343 | 344 | >>> from spatialmath.base.symbolic import * 345 | >>> from spatialmath.base import rot2 346 | >>> theta = symbol('theta') 347 | >>> R = rot2(theta) 348 | >>> print(R) 349 | >>> print(det(R)) 350 | >>> simplify(print(det(R))) 351 | 352 | .. note:: Converts to a SymPy ``Matrix`` and then back again. 353 | """ 354 | 355 | return sympy.Matrix(x).det() 356 | -------------------------------------------------------------------------------- /tests/test_spatialvector.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | import numpy.testing as nt 3 | import numpy as np 4 | 5 | from spatialmath.spatialvector import * 6 | 7 | 8 | class TestSpatialVector(unittest.TestCase): 9 | def test_list_powers(self): 10 | x = SpatialVelocity.Empty() 11 | self.assertEqual(len(x), 0) 12 | x.append(SpatialVelocity([1, 2, 3, 4, 5, 6])) 13 | self.assertEqual(len(x), 1) 14 | 15 | x.append(SpatialVelocity([7, 8, 9, 10, 11, 12])) 16 | self.assertEqual(len(x), 2) 17 | 18 | y = x[0] 19 | self.assertIsInstance(y, SpatialVelocity) 20 | self.assertEqual(len(y), 1) 21 | self.assertTrue(all(y.A == np.r_[1, 2, 3, 4, 5, 6])) 22 | 23 | y = x[1] 24 | self.assertIsInstance(y, SpatialVelocity) 25 | self.assertEqual(len(y), 1) 26 | self.assertTrue(all(y.A == np.r_[7, 8, 9, 10, 11, 12])) 27 | 28 | x.insert(0, SpatialVelocity([20, 21, 22, 23, 24, 25])) 29 | 30 | y = x[0] 31 | self.assertIsInstance(y, SpatialVelocity) 32 | self.assertEqual(len(y), 1) 33 | self.assertTrue(all(y.A == np.r_[20, 21, 22, 23, 24, 25])) 34 | 35 | y = x[1] 36 | self.assertIsInstance(y, SpatialVelocity) 37 | self.assertEqual(len(y), 1) 38 | self.assertTrue(all(y.A == np.r_[1, 2, 3, 4, 5, 6])) 39 | 40 | def test_velocity(self): 41 | a = SpatialVelocity([1, 2, 3, 4, 5, 6]) 42 | self.assertIsInstance(a, SpatialVelocity) 43 | self.assertIsInstance(a, SpatialVector) 44 | self.assertIsInstance(a, SpatialM6) 45 | self.assertEqual(len(a), 1) 46 | self.assertTrue(all(a.A == np.r_[1, 2, 3, 4, 5, 6])) 47 | 48 | a = SpatialVelocity(np.r_[1, 2, 3, 4, 5, 6]) 49 | self.assertIsInstance(a, SpatialVelocity) 50 | self.assertIsInstance(a, SpatialVector) 51 | self.assertIsInstance(a, SpatialM6) 52 | self.assertEqual(len(a), 1) 53 | self.assertTrue(all(a.A == np.r_[1, 2, 3, 4, 5, 6])) 54 | 55 | s = str(a) 56 | self.assertIsInstance(s, str) 57 | self.assertEqual(s.count("\n"), 0) 58 | self.assertTrue(s.startswith("SpatialVelocity")) 59 | 60 | r = np.random.rand(6, 10) 61 | a = SpatialVelocity(r) 62 | self.assertIsInstance(a, SpatialVelocity) 63 | self.assertIsInstance(a, SpatialVector) 64 | self.assertIsInstance(a, SpatialM6) 65 | self.assertEqual(len(a), 10) 66 | 67 | b = a[3] 68 | self.assertIsInstance(b, SpatialVelocity) 69 | self.assertIsInstance(b, SpatialVector) 70 | self.assertIsInstance(b, SpatialM6) 71 | self.assertEqual(len(b), 1) 72 | self.assertTrue(all(b.A == r[:, 3])) 73 | 74 | s = str(a) 75 | self.assertIsInstance(s, str) 76 | self.assertEqual(s.count("\n"), 9) 77 | 78 | def test_acceleration(self): 79 | a = SpatialAcceleration([1, 2, 3, 4, 5, 6]) 80 | self.assertIsInstance(a, SpatialAcceleration) 81 | self.assertIsInstance(a, SpatialVector) 82 | self.assertIsInstance(a, SpatialM6) 83 | self.assertEqual(len(a), 1) 84 | self.assertTrue(all(a.A == np.r_[1, 2, 3, 4, 5, 6])) 85 | 86 | a = SpatialAcceleration(np.r_[1, 2, 3, 4, 5, 6]) 87 | self.assertIsInstance(a, SpatialAcceleration) 88 | self.assertIsInstance(a, SpatialVector) 89 | self.assertIsInstance(a, SpatialM6) 90 | self.assertEqual(len(a), 1) 91 | self.assertTrue(all(a.A == np.r_[1, 2, 3, 4, 5, 6])) 92 | 93 | s = str(a) 94 | self.assertIsInstance(s, str) 95 | self.assertEqual(s.count("\n"), 0) 96 | self.assertTrue(s.startswith("SpatialAcceleration")) 97 | 98 | r = np.random.rand(6, 10) 99 | a = SpatialAcceleration(r) 100 | self.assertIsInstance(a, SpatialAcceleration) 101 | self.assertIsInstance(a, SpatialVector) 102 | self.assertIsInstance(a, SpatialM6) 103 | self.assertEqual(len(a), 10) 104 | 105 | b = a[3] 106 | self.assertIsInstance(b, SpatialAcceleration) 107 | self.assertIsInstance(b, SpatialVector) 108 | self.assertIsInstance(b, SpatialM6) 109 | self.assertEqual(len(b), 1) 110 | self.assertTrue(all(b.A == r[:, 3])) 111 | 112 | s = str(a) 113 | self.assertIsInstance(s, str) 114 | 115 | def test_force(self): 116 | a = SpatialForce([1, 2, 3, 4, 5, 6]) 117 | self.assertIsInstance(a, SpatialForce) 118 | self.assertIsInstance(a, SpatialVector) 119 | self.assertIsInstance(a, SpatialF6) 120 | self.assertEqual(len(a), 1) 121 | self.assertTrue(all(a.A == np.r_[1, 2, 3, 4, 5, 6])) 122 | 123 | a = SpatialForce(np.r_[1, 2, 3, 4, 5, 6]) 124 | self.assertIsInstance(a, SpatialForce) 125 | self.assertIsInstance(a, SpatialVector) 126 | self.assertIsInstance(a, SpatialF6) 127 | self.assertEqual(len(a), 1) 128 | self.assertTrue(all(a.A == np.r_[1, 2, 3, 4, 5, 6])) 129 | 130 | s = str(a) 131 | self.assertIsInstance(s, str) 132 | self.assertEqual(s.count("\n"), 0) 133 | self.assertTrue(s.startswith("SpatialForce")) 134 | 135 | r = np.random.rand(6, 10) 136 | a = SpatialForce(r) 137 | self.assertIsInstance(a, SpatialForce) 138 | self.assertIsInstance(a, SpatialVector) 139 | self.assertIsInstance(a, SpatialF6) 140 | self.assertEqual(len(a), 10) 141 | 142 | b = a[3] 143 | self.assertIsInstance(b, SpatialForce) 144 | self.assertIsInstance(b, SpatialVector) 145 | self.assertIsInstance(b, SpatialF6) 146 | self.assertEqual(len(b), 1) 147 | self.assertTrue(all(b.A == r[:, 3])) 148 | 149 | s = str(a) 150 | self.assertIsInstance(s, str) 151 | 152 | def test_momentum(self): 153 | a = SpatialMomentum([1, 2, 3, 4, 5, 6]) 154 | self.assertIsInstance(a, SpatialMomentum) 155 | self.assertIsInstance(a, SpatialVector) 156 | self.assertIsInstance(a, SpatialF6) 157 | self.assertEqual(len(a), 1) 158 | self.assertTrue(all(a.A == np.r_[1, 2, 3, 4, 5, 6])) 159 | 160 | a = SpatialMomentum(np.r_[1, 2, 3, 4, 5, 6]) 161 | self.assertIsInstance(a, SpatialMomentum) 162 | self.assertIsInstance(a, SpatialVector) 163 | self.assertIsInstance(a, SpatialF6) 164 | self.assertEqual(len(a), 1) 165 | self.assertTrue(all(a.A == np.r_[1, 2, 3, 4, 5, 6])) 166 | 167 | s = str(a) 168 | self.assertIsInstance(s, str) 169 | self.assertEqual(s.count("\n"), 0) 170 | self.assertTrue(s.startswith("SpatialMomentum")) 171 | 172 | r = np.random.rand(6, 10) 173 | a = SpatialMomentum(r) 174 | self.assertIsInstance(a, SpatialMomentum) 175 | self.assertIsInstance(a, SpatialVector) 176 | self.assertIsInstance(a, SpatialF6) 177 | self.assertEqual(len(a), 10) 178 | 179 | b = a[3] 180 | self.assertIsInstance(b, SpatialMomentum) 181 | self.assertIsInstance(b, SpatialVector) 182 | self.assertIsInstance(b, SpatialF6) 183 | self.assertEqual(len(b), 1) 184 | self.assertTrue(all(b.A == r[:, 3])) 185 | 186 | s = str(a) 187 | self.assertIsInstance(s, str) 188 | 189 | def test_arith(self): 190 | # just test SpatialVelocity since all types derive from same superclass 191 | 192 | r1 = np.r_[1, 2, 3, 4, 5, 6] 193 | r2 = np.r_[7, 8, 9, 10, 11, 12] 194 | a1 = SpatialVelocity(r1) 195 | a2 = SpatialVelocity(r2) 196 | 197 | self.assertTrue(all((a1 + a2).A == r1 + r2)) 198 | self.assertTrue(all((a1 - a2).A == r1 - r2)) 199 | self.assertTrue(all((-a1).A == -r1)) 200 | 201 | def test_inertia(self): 202 | # constructor 203 | i0 = SpatialInertia() 204 | nt.assert_equal(i0.A, np.zeros((6, 6))) 205 | 206 | i1 = SpatialInertia(np.eye(6, 6)) 207 | nt.assert_equal(i1.A, np.eye(6, 6)) 208 | 209 | i2 = SpatialInertia(m=1, r=(1, 2, 3)) 210 | nt.assert_almost_equal(i2.A, i2.A.T) 211 | 212 | i3 = SpatialInertia(m=1, r=(1, 2, 3), I=np.ones((3, 3))) 213 | nt.assert_almost_equal(i3.A, i3.A.T) 214 | 215 | # addition 216 | m_a, m_b = 1.1, 2.2 217 | r = (1, 2, 3) 218 | i4a, i4b = SpatialInertia(m=m_a, r=r), SpatialInertia(m=m_b, r=r) 219 | nt.assert_almost_equal((i4a + i4b).A, SpatialInertia(m=m_a + m_b, r=r).A) 220 | 221 | # isvalid - note this method is very barebone, to be improved 222 | self.assertTrue(SpatialInertia().isvalid(np.ones((6, 6)), check=False)) 223 | 224 | def test_products(self): 225 | # v x v = a *, v x F6 = a 226 | # a x I, I x a 227 | # v x I, I x v 228 | # twist x v, twist x a, twist x F 229 | pass 230 | 231 | 232 | # ---------------------------------------------------------------------------------------# 233 | if __name__ == "__main__": 234 | unittest.main() 235 | -------------------------------------------------------------------------------- /symbolic/angvelxform_dot.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "attachments": {}, 5 | "cell_type": "markdown", 6 | "metadata": {}, 7 | "source": [ 8 | "# Determine derivative of Jacobian from angular velocity to exponential rates\n", 9 | "\n", 10 | "Peter Corke 2021, updated 1/23\n", 11 | "\n", 12 | "SymPy code to determine the time derivative of the mapping from angular velocity to exponential coordinate rates." 13 | ] 14 | }, 15 | { 16 | "cell_type": "code", 17 | "execution_count": null, 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "from sympy import *" 22 | ] 23 | }, 24 | { 25 | "attachments": {}, 26 | "cell_type": "markdown", 27 | "metadata": {}, 28 | "source": [ 29 | "A rotation matrix can be expressed in terms of exponential coordinates (also called the Euler vector)\n", 30 | "\n", 31 | "$\n", 32 | "\\mathbf{R} = e^{[\\varphi]_\\times} \n", 33 | "$\n", 34 | "where $\\mathbf{R} \\in SO(3)$ and $\\varphi \\in \\mathbb{R}^3$.\n", 35 | "\n", 36 | "The mapping from angular velocity $\\omega$ to exponential coordinate rates $\\dot{\\varphi}$ is\n", 37 | "\n", 38 | "$\n", 39 | "\\dot{\\varphi} = \\mathbf{A} \\omega\n", 40 | "$\n", 41 | "\n", 42 | "where $\\mathbf{A}$ is given by (2.107) of [Robot Dynamics Lecture Notes, Robotic Systems Lab, ETH Zurich, 2017](https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2017/RD_HS2017script.pdf)\n", 43 | "\n", 44 | "\n", 45 | "$\n", 46 | "\\mathbf{A} = \\mathbf{1}_{3 \\times 3} - \\frac{1}{2} [\\varphi]_\\times + [\\varphi]^2_\\times \\frac{1}{\\theta^2} \\left( 1 - \\frac{\\theta}{2} \\frac{\\sin \\theta}{1 - \\cos \\theta} \\right),\n", 47 | "$\n", 48 | "where $\\theta = \\| \\varphi \\|$\n", 49 | "\n", 50 | "We simplify the equation as\n", 51 | "\n", 52 | "$\n", 53 | "\\mathbf{A} = \\mathbf{1}_{3 \\times 3} - \\frac{1}{2} [\\varphi]_\\times + [\\varphi]^2_\\times \\Theta\n", 54 | "$\n", 55 | "\n", 56 | "where\n", 57 | "$\n", 58 | "\\Theta = \\frac{1}{\\theta^2} \\left( 1 - \\frac{\\theta}{2} \\frac{\\sin \\theta}{1 - \\cos \\theta} \\right)\n", 59 | "$\n", 60 | "\n", 61 | "We can find the derivative using the chain rule\n", 62 | "\n", 63 | "$\n", 64 | "\\dot{\\mathbf{A}} = - \\frac{1}{2} [\\dot{\\varphi}]_\\times + \\left( [\\varphi]_\\times [\\dot{\\varphi}]_\\times + [\\dot{\\varphi}]_\\times[\\varphi]_\\times \\right) \\Theta + [\\varphi]^2_\\times \\dot{\\Theta}\n", 65 | "$\n", 66 | "\n", 67 | "noting that the derivative of a matrix squared is\n", 68 | "\n", 69 | "$\n", 70 | "\\frac{d}{dt} (\\mathbf{M}^2) = (\\frac{d}{dt} \\mathbf{M}) \\mathbf{M} + \\mathbf{M} (\\frac{d}{dt} \\mathbf{M})\n", 71 | "$\n", 72 | "\n", 73 | "We start by defining some symbols" 74 | ] 75 | }, 76 | { 77 | "cell_type": "code", 78 | "execution_count": null, 79 | "metadata": {}, 80 | "outputs": [], 81 | "source": [ 82 | "theta, theta_dot, t = symbols('theta theta_dot t', real=True)" 83 | ] 84 | }, 85 | { 86 | "cell_type": "markdown", 87 | "metadata": {}, 88 | "source": [ 89 | "We start by finding an expression for $\\Theta$ which depends on $\\theta(t)$" 90 | ] 91 | }, 92 | { 93 | "cell_type": "code", 94 | "execution_count": null, 95 | "metadata": {}, 96 | "outputs": [], 97 | "source": [ 98 | "theta_t = Function(theta)(t)\n", 99 | "theta_t" 100 | ] 101 | }, 102 | { 103 | "cell_type": "code", 104 | "execution_count": null, 105 | "metadata": {}, 106 | "outputs": [], 107 | "source": [ 108 | "Theta = 1 / theta_t ** 2 * (1 - theta_t / 2 * sin(theta_t) / (1 - cos(theta_t)))\n", 109 | "Theta" 110 | ] 111 | }, 112 | { 113 | "cell_type": "markdown", 114 | "metadata": {}, 115 | "source": [ 116 | "and now determine the derivative" 117 | ] 118 | }, 119 | { 120 | "cell_type": "code", 121 | "execution_count": null, 122 | "metadata": {}, 123 | "outputs": [], 124 | "source": [ 125 | "T_dot = Theta.diff(t)\n", 126 | "T_dot" 127 | ] 128 | }, 129 | { 130 | "cell_type": "markdown", 131 | "metadata": {}, 132 | "source": [ 133 | "which is a somewhat complex expression that depends on $\\theta(t)$ and $\\dot{\\theta}(t)$.\n", 134 | "\n", 135 | "We will remove the time dependency and generate code" 136 | ] 137 | }, 138 | { 139 | "cell_type": "code", 140 | "execution_count": null, 141 | "metadata": {}, 142 | "outputs": [], 143 | "source": [ 144 | "T_dot = T_dot.subs([(theta_t.diff(t), theta_dot), (theta_t, theta)])\n", 145 | "T_dot" 146 | ] 147 | }, 148 | { 149 | "cell_type": "code", 150 | "execution_count": null, 151 | "metadata": {}, 152 | "outputs": [], 153 | "source": [ 154 | "pycode(T_dot)" 155 | ] 156 | }, 157 | { 158 | "cell_type": "markdown", 159 | "metadata": {}, 160 | "source": [ 161 | "In order to evaluate the line above we need an expression for $\\theta$ and $\\dot{\\theta}$. $\\theta$ is the norm of $\\varphi$ whose elements are functions of time" 162 | ] 163 | }, 164 | { 165 | "cell_type": "code", 166 | "execution_count": null, 167 | "metadata": {}, 168 | "outputs": [], 169 | "source": [ 170 | "phi_names = ('varphi_0', 'varphi_1', 'varphi_2')\n", 171 | "phi = [] # names of angles, eg. theta\n", 172 | "phi_t = [] # angles as function of time, eg. theta(t)\n", 173 | "phi_d = [] # derivative of above, eg. d theta(t) / dt\n", 174 | "phi_n = [] # symbol to represent above, eg. theta_dot\n", 175 | "for i in phi_names:\n", 176 | " phi.append(symbols(i, real=True))\n", 177 | " phi_t.append(Function(phi[-1])(t))\n", 178 | " phi_d.append(phi_t[-1].diff(t))\n", 179 | " phi_n.append(i + '_dot')" 180 | ] 181 | }, 182 | { 183 | "cell_type": "markdown", 184 | "metadata": {}, 185 | "source": [ 186 | "Compute the norm" 187 | ] 188 | }, 189 | { 190 | "cell_type": "code", 191 | "execution_count": null, 192 | "metadata": {}, 193 | "outputs": [], 194 | "source": [ 195 | "theta = Matrix(phi_t).norm()\n", 196 | "theta" 197 | ] 198 | }, 199 | { 200 | "cell_type": "markdown", 201 | "metadata": {}, 202 | "source": [ 203 | "and find its derivative" 204 | ] 205 | }, 206 | { 207 | "cell_type": "code", 208 | "execution_count": null, 209 | "metadata": {}, 210 | "outputs": [], 211 | "source": [ 212 | "theta_dot = theta.diff(t)\n", 213 | "theta_dot" 214 | ] 215 | }, 216 | { 217 | "cell_type": "markdown", 218 | "metadata": {}, 219 | "source": [ 220 | "and now remove the time dependenices" 221 | ] 222 | }, 223 | { 224 | "cell_type": "code", 225 | "execution_count": null, 226 | "metadata": {}, 227 | "outputs": [], 228 | "source": [ 229 | "theta_dot = theta_dot.subs(a for a in zip(phi_d, phi_n))\n", 230 | "theta_dot = theta_dot.subs(a for a in zip(phi_t, phi))\n", 231 | "theta_dot" 232 | ] 233 | }, 234 | { 235 | "cell_type": "markdown", 236 | "metadata": {}, 237 | "source": [ 238 | "which is simply the dot product over the norm." 239 | ] 240 | }, 241 | { 242 | "cell_type": "code", 243 | "execution_count": null, 244 | "metadata": {}, 245 | "outputs": [], 246 | "source": [ 247 | "A, t = symbols('A t', real=True)\n", 248 | "A_t = Function(A)(t)\n", 249 | "d = diff(exp(A_t), t)\n", 250 | "print(d)" 251 | ] 252 | } 253 | ], 254 | "metadata": { 255 | "kernelspec": { 256 | "display_name": "Python 3.8.5 ('dev')", 257 | "language": "python", 258 | "name": "python3" 259 | }, 260 | "language_info": { 261 | "codemirror_mode": { 262 | "name": "ipython", 263 | "version": 3 264 | }, 265 | "file_extension": ".py", 266 | "mimetype": "text/x-python", 267 | "name": "python", 268 | "nbconvert_exporter": "python", 269 | "pygments_lexer": "ipython3", 270 | "version": "3.9.15" 271 | }, 272 | "varInspector": { 273 | "cols": { 274 | "lenName": 16, 275 | "lenType": 16, 276 | "lenVar": 40 277 | }, 278 | "kernels_config": { 279 | "python": { 280 | "delete_cmd_postfix": "", 281 | "delete_cmd_prefix": "del ", 282 | "library": "var_list.py", 283 | "varRefreshCmd": "print(var_dic_list())" 284 | }, 285 | "r": { 286 | "delete_cmd_postfix": ") ", 287 | "delete_cmd_prefix": "rm(", 288 | "library": "var_list.r", 289 | "varRefreshCmd": "cat(var_dic_list()) " 290 | } 291 | }, 292 | "types_to_exclude": [ 293 | "module", 294 | "function", 295 | "builtin_function_or_method", 296 | "instance", 297 | "_Feature" 298 | ], 299 | "window_display": false 300 | }, 301 | "vscode": { 302 | "interpreter": { 303 | "hash": "b7d6b0d76025b9176285a6442c3dd6dd39bcfe7241029b7898b7106bd5e9b472" 304 | } 305 | } 306 | }, 307 | "nbformat": 4, 308 | "nbformat_minor": 4 309 | } 310 | -------------------------------------------------------------------------------- /tests/test_geom2d.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Sun Jul 5 14:37:24 2020 5 | 6 | @author: corkep 7 | """ 8 | 9 | from spatialmath.geom2d import * 10 | from spatialmath.pose2d import SE2 11 | 12 | import unittest 13 | import pytest 14 | import sys 15 | import numpy.testing as nt 16 | import spatialmath.base as smb 17 | 18 | 19 | class Polygon2Test(unittest.TestCase): 20 | # Primitives 21 | def test_constructor1(self): 22 | p = Polygon2([(1, 2), (3, 2), (2, 4)]) 23 | self.assertIsInstance(p, Polygon2) 24 | self.assertEqual(len(p), 3) 25 | self.assertEqual(str(p), "Polygon2 with 4 vertices") 26 | nt.assert_array_equal(p.vertices(), np.array([[1, 3, 2], [2, 2, 4]])) 27 | nt.assert_array_equal( 28 | p.vertices(unique=False), np.array([[1, 3, 2, 1], [2, 2, 4, 2]]) 29 | ) 30 | 31 | def test_methods(self): 32 | p = Polygon2(np.array([[-1, 1, 1, -1], [-1, -1, 1, 1]])) 33 | 34 | self.assertEqual(p.area(), 4) 35 | self.assertEqual(p.moment(0, 0), 4) 36 | self.assertEqual(p.moment(1, 0), 0) 37 | self.assertEqual(p.moment(0, 1), 0) 38 | nt.assert_array_equal(p.centroid(), np.r_[0, 0]) 39 | 40 | self.assertEqual(p.radius(), np.sqrt(2)) 41 | nt.assert_array_equal(p.bbox(), np.r_[-1, -1, 1, 1]) 42 | 43 | def test_contains(self): 44 | p = Polygon2(np.array([[-1, 1, 1, -1], [-1, -1, 1, 1]])) 45 | self.assertTrue(p.contains([0, 0], radius=1e-6)) 46 | self.assertTrue(p.contains([1, 0], radius=1e-6)) 47 | self.assertTrue(p.contains([-1, 0], radius=1e-6)) 48 | self.assertTrue(p.contains([0, 1], radius=1e-6)) 49 | self.assertTrue(p.contains([0, -1], radius=1e-6)) 50 | 51 | self.assertFalse(p.contains([0, 1.1], radius=1e-6)) 52 | self.assertFalse(p.contains([0, -1.1], radius=1e-6)) 53 | self.assertFalse(p.contains([1.1, 0], radius=1e-6)) 54 | self.assertFalse(p.contains([-1.1, 0], radius=1e-6)) 55 | 56 | self.assertTrue(p.contains(np.r_[0, -1], radius=1e-6)) 57 | self.assertFalse(p.contains(np.r_[0, 1.1], radius=1e-6)) 58 | 59 | def test_transform(self): 60 | p = Polygon2(np.array([[-1, 1, 1, -1], [-1, -1, 1, 1]])) 61 | 62 | p = p.transformed(SE2(2, 3)) 63 | 64 | self.assertEqual(p.area(), 4) 65 | self.assertEqual(p.moment(0, 0), 4) 66 | self.assertEqual(p.moment(1, 0), 8) 67 | self.assertEqual(p.moment(0, 1), 12) 68 | nt.assert_array_equal(p.centroid(), np.r_[2, 3]) 69 | 70 | def test_intersect(self): 71 | p1 = Polygon2(np.array([[-1, 1, 1, -1], [-1, -1, 1, 1]])) 72 | 73 | p2 = p1.transformed(SE2(2, 3)) 74 | self.assertFalse(p1.intersects(p2)) 75 | 76 | p2 = p1.transformed(SE2(1, 1)) 77 | self.assertTrue(p1.intersects(p2)) 78 | 79 | self.assertTrue(p1.intersects(p1)) 80 | 81 | def test_intersect_line(self): 82 | p = Polygon2(np.array([[-1, 1, 1, -1], [-1, -1, 1, 1]])) 83 | 84 | l = Line2.Join((-10, 0), (10, 0)) 85 | self.assertTrue(p.intersects(l)) 86 | 87 | l = Line2.Join((-10, 1.1), (10, 1.1)) 88 | self.assertFalse(p.intersects(l)) 89 | 90 | @pytest.mark.skipif( 91 | sys.platform.startswith("darwin") and sys.version_info < (3, 11), 92 | reason="tkinter bug with mac", 93 | ) 94 | def test_plot(self): 95 | p = Polygon2(np.array([[-1, 1, 1, -1], [-1, -1, 1, 1]])) 96 | p.plot() 97 | 98 | p.animate(SE2(1, 2)) 99 | 100 | def test_edges(self): 101 | p = Polygon2([(1, 2), (3, 2), (2, 4)]) 102 | e = p.edges() 103 | 104 | e = list(e) 105 | nt.assert_equal(e[0], ((1, 2), (3, 2))) 106 | nt.assert_equal(e[1], ((3, 2), (2, 4))) 107 | nt.assert_equal(e[2], ((2, 4), (1, 2))) 108 | 109 | # p.move(SE2(0, 0, 0.7)) 110 | 111 | 112 | class Line2Test(unittest.TestCase): 113 | def test_constructor(self): 114 | l = Line2([1, 2, 3]) 115 | self.assertEqual(str(l), "Line2: [1. 2. 3.]") 116 | 117 | l = Line2.Join((0, 0), (1, 2)) 118 | nt.assert_equal(l.line, [-2, 1, 0]) 119 | 120 | l = Line2.General(2, 1) 121 | nt.assert_equal(l.line, [2, -1, 1]) 122 | 123 | def test_contains(self): 124 | l = Line2.Join((0, 0), (1, 2)) 125 | 126 | self.assertTrue(l.contains((0, 0))) 127 | self.assertTrue(l.contains((1, 2))) 128 | self.assertTrue(l.contains((2, 4))) 129 | 130 | def test_intersect(self): 131 | l1 = Line2.Join((0, 0), (2, 0)) # y = 0 132 | l2 = Line2.Join((0, 1), (2, 1)) # y = 1 133 | self.assertFalse(l1.intersect(l2)) 134 | 135 | l2 = Line2.Join((2, 1), (2, -1)) # x = 2 136 | self.assertTrue(l1.intersect(l2)) 137 | 138 | def test_intersect_segment(self): 139 | l1 = Line2.Join((0, 0), (2, 0)) # y = 0 140 | self.assertFalse(l1.intersect_segment((2, 1), (2, 3))) 141 | self.assertTrue(l1.intersect_segment((2, 1), (2, -1))) 142 | 143 | 144 | class EllipseTest(unittest.TestCase): 145 | def test_constructor(self): 146 | E = np.array([[1, 1], [1, 3]]) 147 | e = Ellipse(E=E) 148 | nt.assert_almost_equal(e.E, E) 149 | nt.assert_almost_equal(e.centre, [0, 0]) 150 | self.assertAlmostEqual(e.theta, 1.1780972450961724) 151 | 152 | e = Ellipse(radii=(1, 2), theta=0) 153 | nt.assert_almost_equal(e.E, np.diag([1, 0.25])) 154 | nt.assert_almost_equal(e.centre, [0, 0]) 155 | nt.assert_almost_equal(e.radii, [1, 2]) 156 | self.assertAlmostEqual(e.theta, 0) 157 | 158 | e = Ellipse(radii=(1, 2), theta=np.pi / 2) 159 | nt.assert_almost_equal(e.E, np.diag([0.25, 1])) 160 | nt.assert_almost_equal(e.centre, [0, 0]) 161 | nt.assert_almost_equal(e.radii, [2, 1]) 162 | self.assertAlmostEqual(e.theta, np.pi / 2) 163 | 164 | E = np.array([[1, 1], [1, 3]]) 165 | e = Ellipse(E=E, centre=[3, 4]) 166 | nt.assert_almost_equal(e.E, E) 167 | nt.assert_almost_equal(e.centre, [3, 4]) 168 | self.assertAlmostEqual(e.theta, 1.1780972450961724) 169 | 170 | e = Ellipse(radii=(1, 2), theta=0, centre=[3, 4]) 171 | nt.assert_almost_equal(e.E, np.diag([1, 0.25])) 172 | nt.assert_almost_equal(e.centre, [3, 4]) 173 | nt.assert_almost_equal(e.radii, [1, 2]) 174 | self.assertAlmostEqual(e.theta, 0) 175 | 176 | def test_Polynomial(self): 177 | e = Ellipse.Polynomial([2, 3, 1, 0, 0, -1]) 178 | nt.assert_almost_equal(e.E, np.array([[2, 0.5], [0.5, 3]])) 179 | nt.assert_almost_equal(e.centre, [0, 0]) 180 | 181 | def test_FromPerimeter(self): 182 | eref = Ellipse(radii=(1, 2), theta=0, centre=[0, 0]) 183 | p = eref.points() 184 | 185 | e = Ellipse.FromPerimeter(p) 186 | nt.assert_almost_equal(e.radii, eref.radii) 187 | nt.assert_almost_equal(e.centre, eref.centre) 188 | nt.assert_almost_equal(e.theta, eref.theta) 189 | 190 | ## 191 | eref = Ellipse(radii=(1, 2), theta=0, centre=[3, 4]) 192 | p = eref.points() 193 | 194 | e = Ellipse.FromPerimeter(p) 195 | nt.assert_almost_equal(e.radii, eref.radii) 196 | nt.assert_almost_equal(e.centre, eref.centre) 197 | nt.assert_almost_equal(e.theta, eref.theta) 198 | 199 | ## 200 | eref = Ellipse(radii=(1, 2), theta=np.pi / 4, centre=[3, 4]) 201 | p = eref.points() 202 | 203 | e = Ellipse.FromPerimeter(p) 204 | nt.assert_almost_equal(e.radii, eref.radii) 205 | nt.assert_almost_equal(e.centre, eref.centre) 206 | nt.assert_almost_equal(e.theta, eref.theta) 207 | 208 | def test_FromPoints(self): 209 | eref = Ellipse(radii=(1, 2), theta=np.pi / 2, centre=(3, 4)) 210 | rng = np.random.default_rng(0) 211 | 212 | # create 200 random points inside the ellipse 213 | x = [] 214 | while len(x) < 200: 215 | p = rng.uniform(low=1, high=6, size=(2, 1)) 216 | if eref.contains(p): 217 | x.append(p) 218 | x = np.hstack(x) # create 2 x 50 array 219 | 220 | e = Ellipse.FromPoints(x) 221 | nt.assert_almost_equal(e.radii, eref.radii, decimal=1) 222 | nt.assert_almost_equal(e.centre, eref.centre, decimal=1) 223 | nt.assert_almost_equal(e.theta, eref.theta, decimal=1) 224 | 225 | def test_misc(self): 226 | e = Ellipse(radii=(1, 2), theta=np.pi / 2) 227 | self.assertIsInstance(str(e), str) 228 | 229 | self.assertAlmostEqual(e.area, np.pi * 2) 230 | 231 | e = Ellipse(radii=(1, 2), theta=0) 232 | self.assertTrue(e.contains((0, 0))) 233 | self.assertTrue(e.contains((1, 0))) 234 | self.assertTrue(e.contains((-1, 0))) 235 | self.assertTrue(e.contains((0, 2))) 236 | self.assertTrue(e.contains((0, -2))) 237 | 238 | self.assertFalse(e.contains((1.1, 0))) 239 | self.assertFalse(e.contains((-1.1, 0))) 240 | self.assertFalse(e.contains((0, 2.1))) 241 | self.assertFalse(e.contains((0, -2.1))) 242 | 243 | self.assertEqual(e.contains(np.array([[0, 0], [3, 3]]).T), [True, False]) 244 | 245 | 246 | if __name__ == "__main__": 247 | unittest.main() 248 | -------------------------------------------------------------------------------- /spatialmath/timing.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding", t) 3 | """ 4 | Created on Fri Apr 10 14:22:36 2020 5 | 6 | @author", t) 7 | """ 8 | 9 | 10 | import timeit 11 | from ansitable import ANSITable, Column 12 | 13 | N = 100000 14 | 15 | table = ANSITable( 16 | Column("Operation", headalign="^"), 17 | Column("Time (μs)", headalign="^", fmt="{:.2f}"), 18 | border="thick", 19 | ) 20 | 21 | 22 | def result(op, t): 23 | global table 24 | 25 | table.row(op, t / N * 1e6) 26 | 27 | 28 | # ------------------------------------------------------------------------- # 29 | 30 | # transforms_setup = ''' 31 | # from spatialmath import SE3 32 | # from spatialmath import base 33 | 34 | # import numpy as np 35 | # from collections import namedtuple 36 | # Rt = namedtuple('Rt', 'R t') 37 | # X1 = SE3.Rand() 38 | # X2 = SE3.Rand() 39 | # T1 = X1.A 40 | # T2 = X2.A 41 | # R1 = base.t2r(T1) 42 | # R2 = base.t2r(T2) 43 | # t1 = base.transl(T1) 44 | # t2 = base.transl(T2) 45 | # Rt1 = Rt(R1, t1) 46 | # Rt2 = Rt(R2, t2) 47 | # v = np.r_[1,2,3] 48 | # v2 = np.r_[1,2,3, 1] 49 | # ''' 50 | # t = timeit.timeit(stmt='base.getvector(0.2)', setup=transforms_setup, number=N) 51 | # result("getvector(x)", t) 52 | 53 | # t = timeit.timeit(stmt='base.rotx(0.2, unit="rad")', setup=transforms_setup, number=N) 54 | # result("base.rotx", t) 55 | 56 | # t = timeit.timeit(stmt='base.trotx(0.2, unit="rad")', setup=transforms_setup, number=N) 57 | # result("base.trotx", t) 58 | 59 | # t = timeit.timeit(stmt='base.t2r(T1)', setup=transforms_setup, number=N) 60 | # result("base.t2r", t) 61 | 62 | # t = timeit.timeit(stmt='base.r2t(R1)', setup=transforms_setup, number=N) 63 | # result("base.r2t", t) 64 | 65 | # t = timeit.timeit(stmt='T1 @ T2', setup=transforms_setup, number=N) 66 | # result("4x4 @", t) 67 | 68 | # t = timeit.timeit(stmt='T1[:3,:3] @ T2[:3,:3] + T1[:3,:3] @ T2[:3,3]', setup=transforms_setup, number=N) 69 | # result("R1*R2, R1*t", t) 70 | 71 | # t = timeit.timeit(stmt='(Rt1.R @ Rt2.R, Rt1.R @ Rt2.t)', setup=transforms_setup, number=N) 72 | # result("T1 * T2 (R, t)", t) 73 | 74 | # t = timeit.timeit(stmt='base.trinv(T1)', setup=transforms_setup, number=N) 75 | # result("base.trinv", t) 76 | 77 | # t = timeit.timeit(stmt='(Rt1.R.T, -Rt1.R.T @ Rt1.t)', setup=transforms_setup, number=N) 78 | # result("base.trinv (R,t)", t) 79 | 80 | # t = timeit.timeit(stmt='np.linalg.inv(T1)', setup=transforms_setup, number=N) 81 | # result("np.linalg.inv", t) 82 | 83 | # t = timeit.timeit(stmt='T1 @ v2', setup=transforms_setup, number=N) 84 | # result("(4,4) * (4,)", t) 85 | 86 | # # ------------------------------------------------------------------------- # 87 | # table.rule() 88 | 89 | # t = timeit.timeit(stmt='SE3()', setup=transforms_setup, number=N) 90 | # result("SE3()", t) 91 | 92 | # t = timeit.timeit(stmt='SE3.Rx(0.2)', setup=transforms_setup, number=N) 93 | # result("SE3.Rx()", t) 94 | 95 | # t = timeit.timeit(stmt='T1[:3,:3]', setup=transforms_setup, number=N) 96 | # result("T1[:3,:3]", t) 97 | 98 | # t = timeit.timeit(stmt='X1.A', setup=transforms_setup, number=N) 99 | # result("SE3.A", t) 100 | 101 | # t = timeit.timeit(stmt='SE3(T1)', setup=transforms_setup, number=N) 102 | # result("SE3(T1)", t) 103 | 104 | # t = timeit.timeit(stmt='SE3(T1, check=False)', setup=transforms_setup, number=N) 105 | # result("SE3(T1 check=False)", t) 106 | 107 | # t = timeit.timeit(stmt='SE3([T1], check=False)', setup=transforms_setup, number=N) 108 | # result("SE3([T1])", t) 109 | 110 | # t = timeit.timeit(stmt='X1 * X2', setup=transforms_setup, number=N) 111 | # result("SE3 * SE3", t) 112 | 113 | # t = timeit.timeit(stmt='X1.inv()', setup=transforms_setup, number=N) 114 | # result("SE3.inv", t) 115 | 116 | # t = timeit.timeit(stmt='X1 * v', setup=transforms_setup, number=N) 117 | # result("SE3 * v", t) 118 | 119 | # t = timeit.timeit(stmt='a = X1.log()', setup=transforms_setup, number=N) 120 | # result("SE3.log()", t) 121 | 122 | # # ------------------------------------------------------------------------- # 123 | # quat_setup = ''' 124 | # from spatialmath import base 125 | # from spatialmath import UnitQuaternion 126 | # import numpy as np 127 | # q1 = base.rand() 128 | # q2 = base.rand() 129 | # v = np.r_[1,2,3] 130 | # Q1 = UnitQuaternion.Rx(0.2) 131 | # Q2 = UnitQuaternion.Ry(0.3) 132 | # ''' 133 | # table.rule() 134 | 135 | # t = timeit.timeit(stmt='a = UnitQuaternion()', setup=quat_setup, number=N) 136 | # result("UnitQuaternion() ", t) 137 | 138 | # t = timeit.timeit(stmt='a = UnitQuaternion.Rx(0.2)', setup=quat_setup, number=N) 139 | # result("UnitQuaternion.Rx ", t) 140 | 141 | # t = timeit.timeit(stmt='a = Q1 * Q2', setup=quat_setup, number=N) 142 | # result("UnitQuaternion * UnitQuaternion", t) 143 | 144 | # t = timeit.timeit(stmt='a = Q1 * v', setup=quat_setup, number=N) 145 | # result("UnitQuaternion * v", t) 146 | 147 | # t = timeit.timeit(stmt='a = base.qqmul(q1,q2)', setup=quat_setup, number=N) 148 | # result("base.qqmul", t) 149 | 150 | # t = timeit.timeit(stmt='a = base.qvmul(q1,v)', setup=quat_setup, number=N) 151 | # result("base.qvmul", t) 152 | 153 | 154 | # # ------------------------------------------------------------------------- # 155 | # twist_setup = ''' 156 | # from spatialmath import SE3, Twist3 157 | # from spatialmath import base 158 | # import numpy as np 159 | # from math import cos 160 | # S1 = SE3.Rand().Twist3() 161 | # S2 = SE3.Rand().Twist3() 162 | # X1 = SE3.Rand() 163 | # T1 = X1.A 164 | # A1 = X1.Ad() 165 | # se3 = S1.se3() 166 | # s = np.r_[1,2,3,4,5,6] 167 | # v = np.r_[1,2,3] 168 | # ''' 169 | # table.rule() 170 | # t = timeit.timeit(stmt='a = Twist3()', setup=twist_setup, number=N) 171 | # result("Twist3()", t) 172 | 173 | # t = timeit.timeit(stmt='a = X1.Twist3()', setup=twist_setup, number=N) 174 | # result("SE3.Twist3()", t) 175 | 176 | # t = timeit.timeit(stmt='a = S1 * S2', setup=twist_setup, number=N) 177 | # result("Twist3 * Twist3", t) 178 | 179 | # t = timeit.timeit(stmt='a = S1.inv()', setup=twist_setup, number=N) 180 | # result("Twist3.inv()", t) 181 | 182 | # t = timeit.timeit(stmt='a = S1.Ad()', setup=twist_setup, number=N) 183 | # result("Twist3.Ad()", t) 184 | 185 | # t = timeit.timeit(stmt='a = S1.exp(1)', setup=twist_setup, number=N) 186 | # result("Twist3.Exp()", t) 187 | 188 | # t = timeit.timeit(stmt='a = base.skewa(v)', setup=twist_setup, number=N) 189 | # result("skew", t) 190 | 191 | # t = timeit.timeit(stmt='a = base.skewa(s)', setup=twist_setup, number=N) 192 | # result("skewa", t) 193 | 194 | # t = timeit.timeit(stmt='a = base.vexa(se3)', setup=twist_setup, number=N) 195 | # result("vexa", t) 196 | 197 | # t = timeit.timeit(stmt='a = base.trlog(T1)', setup=twist_setup, number=N) 198 | # result("trlog", t) 199 | 200 | # t = timeit.timeit(stmt='a = base.trlog(T1, twist=True)', setup=twist_setup, number=N) 201 | # result("trlog as twist", t) 202 | 203 | # t = timeit.timeit(stmt='a = base.trexp(se3)', setup=twist_setup, number=N) 204 | # result("trexp", t) 205 | 206 | # t = timeit.timeit(stmt='a = A1 @ s', setup=twist_setup, number=N) 207 | # result("(6,6) * (6,)", t) 208 | 209 | # t = timeit.timeit(stmt='a = base.rodrigues(v)', setup=twist_setup, number=N) 210 | # result("rodrigues", t) 211 | 212 | # t = timeit.timeit(stmt='a = cos(0.3)', setup=twist_setup, number=N) 213 | # result("math.cos", t) 214 | 215 | # t = timeit.timeit(stmt='a = np.cos(0.3)', setup=twist_setup, number=N) 216 | # result("np.cos", t) 217 | 218 | # ------------------------------------------------------------------------- # 219 | misc_setup = """ 220 | from spatialmath import base 221 | import numpy as np 222 | s = np.r_[1.0,2,3,4,5,6] 223 | s3 = np.r_[1.0,2,3] 224 | a = np.r_[1.0, 2.0, 3.0] 225 | b = np.r_[-5.0, 4.0, 3.0] 226 | 227 | A = np.random.randn(6,6) 228 | As = (A + A.T) / 2 229 | bb = np.random.randn(6) 230 | """ 231 | table.rule() 232 | 233 | t = timeit.timeit(stmt="c = np.linalg.inv(As)", setup=misc_setup, number=N) 234 | result("np.inv(As)", t) 235 | 236 | t = timeit.timeit(stmt="c = np.linalg.pinv(As)", setup=misc_setup, number=N) 237 | result("np.pinv(As)", t) 238 | 239 | t = timeit.timeit(stmt="c = np.linalg.solve(As, bb)", setup=misc_setup, number=N) 240 | result("np.solve(As, b)", t) 241 | 242 | t = timeit.timeit(stmt="c = np.cross(a,b)", setup=misc_setup, number=N) 243 | result("np.cross()", t) 244 | 245 | t = timeit.timeit(stmt="c = base.cross(a,b)", setup=misc_setup, number=N) 246 | result("cross()", t) 247 | 248 | t = timeit.timeit(stmt="a = np.inner(s,s).sum()", setup=misc_setup, number=N) 249 | result("inner()", t) 250 | 251 | t = timeit.timeit(stmt="a = np.linalg.norm(s) ** 2", setup=misc_setup, number=N) 252 | result("np.norm**2", t) 253 | 254 | t = timeit.timeit(stmt="a = base.normsq(s)", setup=misc_setup, number=N) 255 | result("base.normsq", t) 256 | 257 | t = timeit.timeit(stmt="a = (s ** 2).sum()", setup=misc_setup, number=N) 258 | result("s**2.sum()", t) 259 | 260 | t = timeit.timeit(stmt="a = np.sum(s ** 2)", setup=misc_setup, number=N) 261 | result("np.sum(s ** 2)", t) 262 | 263 | t = timeit.timeit(stmt="a = np.linalg.norm(s)", setup=misc_setup, number=N) 264 | result("np.norm(R6)", t) 265 | t = timeit.timeit(stmt="a = base.norm(s)", setup=misc_setup, number=N) 266 | result("base.norm(R6)", t) 267 | 268 | t = timeit.timeit(stmt="a = np.linalg.norm(s3)", setup=misc_setup, number=N) 269 | result("np.norm(R3)", t) 270 | t = timeit.timeit(stmt="a = base.norm(s3)", setup=misc_setup, number=N) 271 | result("base.norm(R3)", t) 272 | 273 | 274 | table.print() 275 | -------------------------------------------------------------------------------- /tests/base/test_quaternions.py: -------------------------------------------------------------------------------- 1 | # This file is part of the SpatialMath toolbox for Python 2 | # https://github.com/petercorke/spatialmath-python 3 | # 4 | # MIT License 5 | # 6 | # Copyright (c) 1993-2020 Peter Corke 7 | # 8 | # Permission is hereby granted, free of charge, to any person obtaining a copy 9 | # of this software and associated documentation files (the "Software"), to deal 10 | # in the Software without restriction, including without limitation the rights 11 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 12 | # copies of the Software, and to permit persons to whom the Software is 13 | # furnished to do so, subject to the following conditions: 14 | # 15 | # The above copyright notice and this permission notice shall be included in all 16 | # copies or substantial portions of the Software. 17 | # 18 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 21 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 22 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 23 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 24 | # SOFTWARE. 25 | 26 | # Contributors: 27 | # 28 | # 1. Luis Fernando Lara Tobar and Peter Corke, 2008 29 | # 2. Josh Carrigg Hodson, Aditya Dua, Chee Ho Chan, 2017 (robopy) 30 | # 3. Peter Corke, 2020 31 | 32 | import numpy.testing as nt 33 | import unittest 34 | 35 | from spatialmath.base.vectors import * 36 | import spatialmath.base as tr 37 | from spatialmath.base.quaternions import * 38 | import spatialmath as sm 39 | import io 40 | 41 | 42 | class TestQuaternion(unittest.TestCase): 43 | def test_ops(self): 44 | nt.assert_array_almost_equal(qeye(), np.r_[1, 0, 0, 0]) 45 | 46 | nt.assert_array_almost_equal(qpure(np.r_[1, 2, 3]), np.r_[0, 1, 2, 3]) 47 | nt.assert_array_almost_equal(qpure([1, 2, 3]), np.r_[0, 1, 2, 3]) 48 | nt.assert_array_almost_equal(qpure((1, 2, 3)), np.r_[0, 1, 2, 3]) 49 | 50 | nt.assert_equal(qnorm(np.r_[1, 2, 3, 4]), math.sqrt(30)) 51 | nt.assert_equal(qnorm([1, 2, 3, 4]), math.sqrt(30)) 52 | nt.assert_equal(qnorm((1, 2, 3, 4)), math.sqrt(30)) 53 | 54 | nt.assert_array_almost_equal( 55 | qunit(np.r_[1, 2, 3, 4]), np.r_[1, 2, 3, 4] / math.sqrt(30) 56 | ) 57 | nt.assert_array_almost_equal( 58 | qunit([1, 2, 3, 4]), np.r_[1, 2, 3, 4] / math.sqrt(30) 59 | ) 60 | 61 | nt.assert_array_almost_equal( 62 | qqmul(np.r_[1, 2, 3, 4], np.r_[5, 6, 7, 8]), np.r_[-60, 12, 30, 24] 63 | ) 64 | nt.assert_array_almost_equal( 65 | qqmul([1, 2, 3, 4], [5, 6, 7, 8]), np.r_[-60, 12, 30, 24] 66 | ) 67 | nt.assert_array_almost_equal( 68 | qqmul(np.r_[1, 2, 3, 4], np.r_[1, 2, 3, 4]), np.r_[-28, 4, 6, 8] 69 | ) 70 | 71 | nt.assert_array_almost_equal( 72 | qmatrix(np.r_[1, 2, 3, 4]) @ np.r_[5, 6, 7, 8], np.r_[-60, 12, 30, 24] 73 | ) 74 | nt.assert_array_almost_equal( 75 | qmatrix([1, 2, 3, 4]) @ np.r_[5, 6, 7, 8], np.r_[-60, 12, 30, 24] 76 | ) 77 | nt.assert_array_almost_equal( 78 | qmatrix(np.r_[1, 2, 3, 4]) @ np.r_[1, 2, 3, 4], np.r_[-28, 4, 6, 8] 79 | ) 80 | 81 | nt.assert_array_almost_equal(qpow(np.r_[1, 2, 3, 4], 0), np.r_[1, 0, 0, 0]) 82 | nt.assert_array_almost_equal(qpow(np.r_[1, 2, 3, 4], 1), np.r_[1, 2, 3, 4]) 83 | nt.assert_array_almost_equal(qpow([1, 2, 3, 4], 1), np.r_[1, 2, 3, 4]) 84 | nt.assert_array_almost_equal(qpow(np.r_[1, 2, 3, 4], 2), np.r_[-28, 4, 6, 8]) 85 | nt.assert_array_almost_equal(qpow(np.r_[1, 2, 3, 4], -1), np.r_[1, -2, -3, -4]) 86 | nt.assert_array_almost_equal( 87 | qpow(np.r_[1, 2, 3, 4], -2), np.r_[-28, -4, -6, -8] 88 | ) 89 | 90 | nt.assert_equal(qisequal(np.r_[1, 2, 3, 4], np.r_[1, 2, 3, 4]), True) 91 | nt.assert_equal(qisequal(np.r_[1, 2, 3, 4], np.r_[5, 6, 7, 8]), False) 92 | nt.assert_equal( 93 | qisequal( 94 | np.r_[1, 1, 0, 0] / math.sqrt(2), 95 | np.r_[-1, -1, 0, 0] / math.sqrt(2), 96 | unitq=True, 97 | ), 98 | True, 99 | ) 100 | nt.assert_equal(isunitvec(qrand()), True) 101 | 102 | def test_display(self): 103 | s = q2str(np.r_[1, 2, 3, 4]) 104 | nt.assert_equal(isinstance(s, str), True) 105 | nt.assert_equal(s, " 1.0000 < 2.0000, 3.0000, 4.0000 >") 106 | 107 | s = q2str([1, 2, 3, 4]) 108 | nt.assert_equal(s, " 1.0000 < 2.0000, 3.0000, 4.0000 >") 109 | 110 | s = q2str([1, 2, 3, 4], delim=("<<", ">>")) 111 | nt.assert_equal(s, " 1.0000 << 2.0000, 3.0000, 4.0000 >>") 112 | 113 | s = q2str([1, 2, 3, 4], fmt="{:20.6f}") 114 | nt.assert_equal( 115 | s, 116 | " 1.000000 < 2.000000, 3.000000, 4.000000 >", 117 | ) 118 | 119 | # would be nicer to do this with redirect_stdout() from contextlib but that 120 | # fails because file=sys.stdout is maybe assigned at compile time, so when 121 | # contextlib changes sys.stdout, qprint() doesn't see it 122 | 123 | f = io.StringIO() 124 | qprint(np.r_[1, 2, 3, 4], file=f) 125 | nt.assert_equal(f.getvalue().rstrip(), " 1.0000 < 2.0000, 3.0000, 4.0000 >") 126 | 127 | def test_rotation(self): 128 | # rotation matrix to quaternion 129 | nt.assert_array_almost_equal(r2q(tr.rotx(180, "deg")), np.r_[0, 1, 0, 0]) 130 | nt.assert_array_almost_equal(r2q(tr.roty(180, "deg")), np.r_[0, 0, 1, 0]) 131 | nt.assert_array_almost_equal(r2q(tr.rotz(180, "deg")), np.r_[0, 0, 0, 1]) 132 | 133 | # quaternion to rotation matrix 134 | nt.assert_array_almost_equal(q2r(np.r_[0, 1, 0, 0]), tr.rotx(180, "deg")) 135 | nt.assert_array_almost_equal(q2r(np.r_[0, 0, 1, 0]), tr.roty(180, "deg")) 136 | nt.assert_array_almost_equal(q2r(np.r_[0, 0, 0, 1]), tr.rotz(180, "deg")) 137 | 138 | nt.assert_array_almost_equal(q2r([0, 1, 0, 0]), tr.rotx(180, "deg")) 139 | nt.assert_array_almost_equal(q2r([0, 0, 1, 0]), tr.roty(180, "deg")) 140 | nt.assert_array_almost_equal(q2r([0, 0, 0, 1]), tr.rotz(180, "deg")) 141 | 142 | # quaternion - vector product 143 | nt.assert_array_almost_equal( 144 | qvmul(np.r_[0, 1, 0, 0], np.r_[0, 0, 1]), np.r_[0, 0, -1] 145 | ) 146 | nt.assert_array_almost_equal(qvmul([0, 1, 0, 0], [0, 0, 1]), np.r_[0, 0, -1]) 147 | 148 | large_rotation = math.pi + 0.01 149 | q1 = r2q(tr.rotx(large_rotation), shortest=False) 150 | q2 = r2q(tr.rotx(large_rotation), shortest=True) 151 | self.assertLess(q1[0], 0) 152 | self.assertGreater(q2[0], 0) 153 | self.assertTrue(qisequal(q1=q1, q2=q2, unitq=True)) 154 | 155 | def test_slerp(self): 156 | q1 = np.r_[0, 1, 0, 0] 157 | q2 = np.r_[0, 0, 1, 0] 158 | 159 | nt.assert_array_almost_equal(qslerp(q1, q2, 0), q1) 160 | nt.assert_array_almost_equal(qslerp(q1, q2, 1), q2) 161 | nt.assert_array_almost_equal( 162 | qslerp(q1, q2, 0.5), np.r_[0, 1, 1, 0] / math.sqrt(2) 163 | ) 164 | 165 | q1 = [0, 1, 0, 0] 166 | q2 = [0, 0, 1, 0] 167 | 168 | nt.assert_array_almost_equal(qslerp(q1, q2, 0), q1) 169 | nt.assert_array_almost_equal(qslerp(q1, q2, 1), q2) 170 | nt.assert_array_almost_equal( 171 | qslerp(q1, q2, 0.5), np.r_[0, 1, 1, 0] / math.sqrt(2) 172 | ) 173 | 174 | nt.assert_array_almost_equal( 175 | qslerp(r2q(tr.rotx(-0.3)), r2q(tr.rotx(0.3)), 0.5), np.r_[1, 0, 0, 0] 176 | ) 177 | nt.assert_array_almost_equal( 178 | qslerp(r2q(tr.roty(0.3)), r2q(tr.roty(0.5)), 0.5), r2q(tr.roty(0.4)) 179 | ) 180 | 181 | def test_rotx(self): 182 | pass 183 | 184 | def test_r2q(self): 185 | # null rotation case 186 | R = np.eye(3) 187 | nt.assert_array_almost_equal(r2q(R), [1, 0, 0, 0]) 188 | 189 | R = tr.rotx(np.pi / 2) 190 | nt.assert_array_almost_equal(r2q(R), np.r_[1, 1, 0, 0] / np.sqrt(2)) 191 | 192 | R = tr.rotx(-np.pi / 2) 193 | nt.assert_array_almost_equal(r2q(R), np.r_[1, -1, 0, 0] / np.sqrt(2)) 194 | 195 | R = tr.rotx(np.pi) 196 | nt.assert_array_almost_equal(r2q(R), np.r_[0, 1, 0, 0]) 197 | 198 | R = tr.rotx(-np.pi) 199 | nt.assert_array_almost_equal(r2q(R), np.r_[0, 1, 0, 0]) 200 | 201 | # ry 202 | R = tr.roty(np.pi / 2) 203 | nt.assert_array_almost_equal(r2q(R), np.r_[1, 0, 1, 0] / np.sqrt(2)) 204 | 205 | R = tr.roty(-np.pi / 2) 206 | nt.assert_array_almost_equal(r2q(R), np.r_[1, 0, -1, 0] / np.sqrt(2)) 207 | 208 | R = tr.roty(np.pi) 209 | nt.assert_array_almost_equal(r2q(R), np.r_[0, 0, 1, 0]) 210 | 211 | R = tr.roty(-np.pi) 212 | nt.assert_array_almost_equal(r2q(R), np.r_[0, 0, 1, 0]) 213 | 214 | # rz 215 | R = tr.rotz(np.pi / 2) 216 | nt.assert_array_almost_equal(r2q(R), np.r_[1, 0, 0, 1] / np.sqrt(2)) 217 | 218 | R = tr.rotz(-np.pi / 2) 219 | nt.assert_array_almost_equal(r2q(R), np.r_[1, 0, 0, -1] / np.sqrt(2)) 220 | 221 | R = tr.rotz(np.pi) 222 | nt.assert_array_almost_equal(r2q(R), np.r_[0, 0, 0, 1]) 223 | 224 | R = tr.rotz(-np.pi) 225 | nt.assert_array_almost_equal(r2q(R), np.r_[0, 0, 0, 1]) 226 | 227 | # github issue case 228 | R = np.array([[0, -1, 0], [-1, 0, 0], [0, 0, -1]]) 229 | nt.assert_array_almost_equal(r2q(R), np.r_[0, 1, -1, 0] / np.sqrt(2)) 230 | 231 | r1 = sm.SE3.Rx(0.1) 232 | q1a = np.array([9.987503e-01, 4.997917e-02, 0.000000e00, 2.775558e-17]) 233 | q1b = np.array([4.997917e-02, 0.000000e00, 2.775558e-17, 9.987503e-01]) 234 | 235 | nt.assert_array_almost_equal(q1a, r2q(r1.R)) 236 | nt.assert_array_almost_equal(q1a, r2q(r1.R, order="sxyz")) 237 | nt.assert_array_almost_equal(q1b, r2q(r1.R, order="xyzs")) 238 | 239 | with self.assertRaises(ValueError): 240 | nt.assert_array_almost_equal(q1a, r2q(r1.R, order="aaa")) 241 | 242 | def test_qangle(self): 243 | # Test function that calculates angle between quaternions 244 | q1 = [1.0, 0, 0, 0] 245 | q2 = [1 / np.sqrt(2), 0, 1 / np.sqrt(2), 0] # 90deg rotation about y-axis 246 | nt.assert_almost_equal(qangle(q1, q2), np.pi / 2) 247 | 248 | q1 = [1.0, 0, 0, 0] 249 | q2 = [1 / np.sqrt(2), 1 / np.sqrt(2), 0, 0] # 90deg rotation about x-axis 250 | nt.assert_almost_equal(qangle(q1, q2), np.pi / 2) 251 | 252 | 253 | if __name__ == "__main__": 254 | unittest.main() 255 | -------------------------------------------------------------------------------- /tests/test_geom3d.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Sun Jul 5 14:37:24 2020 5 | 6 | @author: corkep 7 | """ 8 | 9 | from spatialmath.geom3d import * 10 | from spatialmath.pose3d import SE3 11 | 12 | import unittest 13 | import numpy.testing as nt 14 | import spatialmath.base as base 15 | import pytest 16 | import sys 17 | 18 | 19 | class Line3Test(unittest.TestCase): 20 | # Primitives 21 | def test_constructor1(self): 22 | # construct from 6-vector 23 | 24 | with self.assertRaises(ValueError): 25 | L = Line3([1, 2, 3, 4, 5, 6], check=True) 26 | 27 | L = Line3([1, 2, 3, 4, 5, 6], check=False) 28 | self.assertIsInstance(L, Line3) 29 | nt.assert_array_almost_equal(L.v, np.r_[1, 2, 3]) 30 | nt.assert_array_almost_equal(L.w, np.r_[4, 5, 6]) 31 | 32 | # construct from object 33 | L2 = Line3(L, check=False) 34 | self.assertIsInstance(L, Line3) 35 | nt.assert_array_almost_equal(L2.v, np.r_[1, 2, 3]) 36 | nt.assert_array_almost_equal(L2.w, np.r_[4, 5, 6]) 37 | 38 | # construct from point and direction 39 | L = Line3.PointDir([1, 2, 3], [4, 5, 6]) 40 | self.assertTrue(L.contains([1, 2, 3])) 41 | nt.assert_array_almost_equal(L.uw, base.unitvec([4, 5, 6])) 42 | 43 | def test_vec(self): 44 | # verify double 45 | L = Line3([1, 2, 3, 4, 5, 6], check=False) 46 | nt.assert_array_almost_equal(L.vec, np.r_[1, 2, 3, 4, 5, 6]) 47 | 48 | def test_constructor2(self): 49 | # 2, point constructor 50 | P = np.r_[2, 3, 7] 51 | Q = np.r_[2, 1, 0] 52 | L = Line3.Join(P, Q) 53 | nt.assert_array_almost_equal(L.w, P - Q) 54 | nt.assert_array_almost_equal(L.v, np.cross(P - Q, Q)) 55 | 56 | # TODO, all combos of list and ndarray 57 | # test all possible input shapes 58 | # L2, = Line3(P, Q) 59 | # self.assertEqual(double(L2), double(L)) 60 | # L2, = Line3(P, Q') 61 | # self.assertEqual(double(L2), double(L)) 62 | # L2, = Line3(P', Q') 63 | # self.assertEqual(double(L2), double(L)) 64 | # L2, = Line3(P, Q) 65 | # self.assertEqual(double(L2), double(L)) 66 | 67 | # # planes constructor 68 | # P = [10, 11, 12]'; w = [1, 2, 3] 69 | # L = Line3.PointDir(P, w) 70 | # self.assertEqual(double(L), [cross(w,P) w]'); %FAIL 71 | # L2, = Line3.PointDir(P', w) 72 | # self.assertEqual(double(L2), double(L)) 73 | # L2, = Line3.PointDir(P, w') 74 | # self.assertEqual(double(L2), double(L)) 75 | # L2, = Line3.PointDir(P', w') 76 | # self.assertEqual(double(L2), double(L)) 77 | 78 | def test_pp(self): 79 | # validate pp and ppd 80 | L = Line3.Join([-1, 1, 2], [1, 1, 2]) 81 | nt.assert_array_almost_equal(L.pp, np.r_[0, 1, 2]) 82 | self.assertEqual(L.ppd, math.sqrt(5)) 83 | 84 | # validate pp 85 | self.assertTrue(L.contains(L.pp)) 86 | 87 | def test_contains(self): 88 | P = [2, 3, 7] 89 | Q = [2, 1, 0] 90 | L = Line3.Join(P, Q) 91 | 92 | # validate contains 93 | self.assertTrue(L.contains([2, 3, 7])) 94 | self.assertTrue(L.contains([2, 1, 0])) 95 | self.assertFalse(L.contains([2, 1, 4])) 96 | 97 | def test_closest(self): 98 | P = [2, 3, 7] 99 | Q = [2, 1, 0] 100 | L = Line3.Join(P, Q) 101 | 102 | p, d = L.closest_to_point(P) 103 | nt.assert_array_almost_equal(p, P) 104 | self.assertAlmostEqual(d, 0) 105 | 106 | # validate closest with given points and origin 107 | p, d = L.closest_to_point(Q) 108 | nt.assert_array_almost_equal(p, Q) 109 | self.assertAlmostEqual(d, 0) 110 | 111 | L = Line3.Join([-1, 1, 2], [1, 1, 2]) 112 | p, d = L.closest_to_point([0, 1, 2]) 113 | nt.assert_array_almost_equal(p, np.r_[0, 1, 2]) 114 | self.assertAlmostEqual(d, 0) 115 | 116 | p, d = L.closest_to_point([5, 1, 2]) 117 | nt.assert_array_almost_equal(p, np.r_[5, 1, 2]) 118 | self.assertAlmostEqual(d, 0) 119 | 120 | p, d = L.closest_to_point([0, 0, 0]) 121 | nt.assert_array_almost_equal(p, L.pp) 122 | self.assertEqual(d, L.ppd) 123 | 124 | p, d = L.closest_to_point([5, 1, 0]) 125 | nt.assert_array_almost_equal(p, [5, 1, 2]) 126 | self.assertAlmostEqual(d, 2) 127 | 128 | @pytest.mark.skipif( 129 | sys.platform.startswith("darwin") and sys.version_info < (3, 11), 130 | reason="tkinter bug with mac", 131 | ) 132 | def test_plot(self): 133 | P = [2, 3, 7] 134 | Q = [2, 1, 0] 135 | L = Line3.Join(P, Q) 136 | 137 | fig = plt.figure() 138 | ax = fig.add_subplot(111, projection="3d", proj_type="ortho") 139 | ax.set_xlim3d(-10, 10) 140 | ax.set_ylim3d(-10, 10) 141 | ax.set_zlim3d(-10, 10) 142 | 143 | L.plot(color="red", linewidth=2) 144 | 145 | def test_eq(self): 146 | w = np.r_[1, 2, 3] 147 | P = np.r_[-2, 4, 3] 148 | 149 | L1 = Line3.Join(P, P + w) 150 | L2 = Line3.Join(P + 2 * w, P + 5 * w) 151 | L3 = Line3.Join(P + np.r_[1, 0, 0], P + w) 152 | 153 | self.assertTrue(L1 == L2) 154 | self.assertFalse(L1 == L3) 155 | 156 | self.assertFalse(L1 != L2) 157 | self.assertTrue(L1 != L3) 158 | 159 | def test_skew(self): 160 | P = [2, 3, 7] 161 | Q = [2, 1, 0] 162 | L = Line3.Join(P, Q) 163 | 164 | m = L.skew() 165 | 166 | self.assertEqual(m.shape, (4, 4)) 167 | nt.assert_array_almost_equal(m + m.T, np.zeros((4, 4))) 168 | 169 | def test_rmul(self): 170 | P = [1, 2, 0] 171 | Q = [1, 2, 10] # vertical line through (1,2) 172 | L = Line3.Join(P, Q) 173 | 174 | # check transformation by SE3 175 | 176 | L2 = SE3() * L 177 | p = L2.intersect_plane([0, 0, 1, 0])[0] # intersects z=0 178 | nt.assert_array_almost_equal(p, [1, 2, 0]) 179 | 180 | L2 = SE3(2, 0, 0) * L # shift line in the x direction 181 | p = L2.intersect_plane([0, 0, 1, 0])[0] # intersects z=0 182 | nt.assert_array_almost_equal(p, [3, 2, 0]) 183 | 184 | L2 = SE3(0, 2, 0) * L # shift line in the y direction 185 | p = L2.intersect_plane([0, 0, 1, 0])[0] # intersects z=0 186 | nt.assert_array_almost_equal(p, [1, 4, 0]) 187 | 188 | L2 = SE3.Rx(np.pi / 2) * L # rotate line about x-axis, now horizontal 189 | nt.assert_array_almost_equal(L2.uw, [0, -1, 0]) 190 | 191 | def test_parallel(self): 192 | L1 = Line3.PointDir([4, 5, 6], [1, 2, 3]) 193 | L2 = Line3.PointDir([5, 5, 6], [1, 2, 3]) 194 | L3 = Line3.PointDir([4, 5, 6], [3, 2, 1]) 195 | 196 | # L1, || L2, but doesnt intersect 197 | # L1, intersects L3 198 | 199 | self.assertTrue(L1.isparallel(L1)) 200 | self.assertTrue(L1 | L1) 201 | 202 | self.assertTrue(L1.isparallel(L2)) 203 | self.assertTrue(L1 | L2) 204 | self.assertTrue(L2.isparallel(L1)) 205 | self.assertTrue(L2 | L1) 206 | self.assertFalse(L1.isparallel(L3)) 207 | self.assertFalse(L1 | L3) 208 | 209 | def test_intersect(self): 210 | L1 = Line3.PointDir([4, 5, 6], [1, 2, 3]) 211 | L2 = Line3.PointDir([5, 5, 6], [1, 2, 3]) 212 | L3 = Line3.PointDir([4, 5, 6], [0, 0, 1]) 213 | L4 = Line3.PointDir([5, 5, 6], [1, 0, 0]) 214 | 215 | # L1, || L2, but doesnt intersect 216 | # L3, intersects L4 217 | self.assertFalse( 218 | L1 ^ L2, 219 | ) 220 | 221 | self.assertTrue( 222 | L3 ^ L4, 223 | ) 224 | 225 | def test_commonperp(self): 226 | L1 = Line3.PointDir([4, 5, 6], [0, 0, 1]) 227 | L2 = Line3.PointDir([6, 5, 6], [0, 1, 0]) 228 | 229 | self.assertFalse(L1 | L2) 230 | self.assertFalse(L1 ^ L2) 231 | 232 | self.assertEqual(L1.distance(L2), 2) 233 | 234 | L = L1.commonperp(L2) # common perp intersects both lines 235 | 236 | self.assertTrue(L ^ L1) 237 | self.assertTrue(L ^ L2) 238 | 239 | def test_line(self): 240 | # mindist 241 | # intersect 242 | # char 243 | # intersect_volume 244 | # mindist 245 | # mtimes 246 | # or 247 | # side 248 | pass 249 | 250 | def test_contains(self): 251 | P = [2, 3, 7] 252 | Q = [2, 1, 0] 253 | L = Line3.Join(P, Q) 254 | 255 | self.assertTrue(L.contains(L.point(0))) 256 | self.assertTrue(L.contains(L.point(1))) 257 | self.assertTrue(L.contains(L.point(-1))) 258 | 259 | def test_point(self): 260 | P = [2, 3, 7] 261 | Q = [2, 1, 0] 262 | L = Line3.Join(P, Q) 263 | 264 | nt.assert_array_almost_equal(L.point(0).flatten(), L.pp) 265 | 266 | for x in (-2, 0, 3): 267 | nt.assert_array_almost_equal(L.lam(L.point(x)), x) 268 | 269 | def test_char(self): 270 | P = [2, 3, 7] 271 | Q = [2, 1, 0] 272 | L = Line3.Join(P, Q) 273 | 274 | s = str(L) 275 | self.assertIsInstance(s, str) 276 | 277 | def test_plane(self): 278 | xyplane = [0, 0, 1, 0] 279 | xzplane = [0, 1, 0, 0] 280 | L = Line3.TwoPlanes(xyplane, xzplane) # x axis 281 | nt.assert_array_almost_equal(L.vec, np.r_[0, 0, 0, -1, 0, 0]) 282 | 283 | L = Line3.Join([-1, 2, 3], [1, 2, 3]) 284 | # line at y=2,z=3 285 | x6 = [1, 0, 0, -6] # x = 6 286 | 287 | # plane_intersect 288 | p, lam = L.intersect_plane(x6) 289 | nt.assert_array_almost_equal(p, np.r_[6, 2, 3]) 290 | nt.assert_array_almost_equal(L.point(lam).flatten(), np.r_[6, 2, 3]) 291 | 292 | x6s = Plane3.PointNormal(n=[1, 0, 0], p=[6, 0, 0]) 293 | p, lam = L.intersect_plane(x6s) 294 | nt.assert_array_almost_equal(p, np.r_[6, 2, 3]) 295 | 296 | nt.assert_array_almost_equal(L.point(lam).flatten(), np.r_[6, 2, 3]) 297 | 298 | def test_methods(self): 299 | # intersection 300 | px = Line3.Join([0, 0, 0], [1, 0, 0]) 301 | # x-axis 302 | py = Line3.Join([0, 0, 0], [0, 1, 0]) 303 | # y-axis 304 | px1 = Line3.Join([0, 1, 0], [1, 1, 0]) 305 | # offset x-axis 306 | 307 | self.assertEqual(px.ppd, 0) 308 | self.assertEqual(px1.ppd, 1) 309 | nt.assert_array_almost_equal(px1.pp, [0, 1, 0]) 310 | 311 | px.intersects(px) 312 | px.intersects(py) 313 | px.intersects(px1) 314 | 315 | # def test_intersect(self): 316 | # px = Line3([0, 0, 0], [1, 0, 0]); # x-axis 317 | # py = Line3([0, 0, 0], [0, 1, 0]); # y-axis 318 | # 319 | # plane.d = [1, 0, 0]; plane.p = 2; # plane x=2 320 | # 321 | # px.intersect_plane(plane) 322 | # py.intersect_plane(plane) 323 | 324 | 325 | if __name__ == "__main__": 326 | unittest.main() 327 | -------------------------------------------------------------------------------- /tests/base/test_transforms2d.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Fri Apr 10 14:19:04 2020 5 | 6 | @author: corkep 7 | 8 | """ 9 | 10 | import numpy as np 11 | import numpy.testing as nt 12 | import unittest 13 | from math import pi 14 | import math 15 | from scipy.linalg import logm 16 | import pytest 17 | import sys 18 | 19 | from spatialmath.base.transforms2d import * 20 | from spatialmath.base.transformsNd import ( 21 | isR, 22 | t2r, 23 | r2t, 24 | rt2tr, 25 | skew, 26 | vexa, 27 | skewa, 28 | homtrans, 29 | ) 30 | 31 | import matplotlib.pyplot as plt 32 | 33 | 34 | class Test2D(unittest.TestCase): 35 | def test_rot2(self): 36 | R = np.array([[1, 0], [0, 1]]) 37 | nt.assert_array_almost_equal(rot2(0), R) 38 | nt.assert_array_almost_equal(rot2(0, unit="rad"), R) 39 | nt.assert_array_almost_equal(rot2(0, unit="deg"), R) 40 | nt.assert_array_almost_equal(rot2(0, "deg"), R) 41 | nt.assert_almost_equal(np.linalg.det(rot2(0)), 1) 42 | 43 | R = np.array([[0, -1], [1, 0]]) 44 | nt.assert_array_almost_equal(rot2(pi / 2), R) 45 | nt.assert_array_almost_equal(rot2(pi / 2, unit="rad"), R) 46 | nt.assert_array_almost_equal(rot2(90, unit="deg"), R) 47 | nt.assert_array_almost_equal(rot2(90, "deg"), R) 48 | nt.assert_almost_equal(np.linalg.det(rot2(pi / 2)), 1) 49 | 50 | R = np.array([[-1, 0], [0, -1]]) 51 | nt.assert_array_almost_equal(rot2(pi), R) 52 | nt.assert_array_almost_equal(rot2(pi, unit="rad"), R) 53 | nt.assert_array_almost_equal(rot2(180, unit="deg"), R) 54 | nt.assert_array_almost_equal(rot2(180, "deg"), R) 55 | nt.assert_almost_equal(np.linalg.det(rot2(pi)), 1) 56 | 57 | def test_trot2(self): 58 | nt.assert_array_almost_equal( 59 | trot2(pi / 2, t=[3, 4]), np.array([[0, -1, 3], [1, 0, 4], [0, 0, 1]]) 60 | ) 61 | nt.assert_array_almost_equal( 62 | trot2(pi / 2, t=(3, 4)), np.array([[0, -1, 3], [1, 0, 4], [0, 0, 1]]) 63 | ) 64 | nt.assert_array_almost_equal( 65 | trot2(pi / 2, t=np.array([3, 4])), 66 | np.array([[0, -1, 3], [1, 0, 4], [0, 0, 1]]), 67 | ) 68 | 69 | def test_Rt(self): 70 | nt.assert_array_almost_equal(rot2(0.3), t2r(trot2(0.3))) 71 | nt.assert_array_almost_equal(trot2(0.3), r2t(rot2(0.3))) 72 | 73 | R = rot2(0.2) 74 | t = [1, 2] 75 | T = rt2tr(R, t) 76 | nt.assert_array_almost_equal(t2r(T), R) 77 | nt.assert_array_almost_equal(transl2(T), np.array(t)) 78 | # TODO 79 | 80 | def test_trlog2(self): 81 | R = rot2(0.5) 82 | nt.assert_array_almost_equal(trlog2(R), skew(0.5)) 83 | 84 | nt.assert_array_almost_equal(trlog2(R, twist=True), 0.5) 85 | 86 | T = transl2(1, 2) @ trot2(0.5) 87 | nt.assert_array_almost_equal(trlog2(T), logm(T)) 88 | 89 | nt.assert_array_almost_equal(trlog2(T, twist=True), vexa(logm(T))) 90 | 91 | def test_trexp2(self): 92 | R = trexp2(skew(0.5)) 93 | nt.assert_array_almost_equal(R, rot2(0.5)) 94 | 95 | T = transl2(1, 2) @ trot2(0.5) 96 | nt.assert_array_almost_equal(trexp2(logm(T)), T) 97 | 98 | def test_trnorm2(self): 99 | R = rot2(0.4) 100 | R = np.round(R, 3) # approx SO(2) 101 | R = trnorm2(R) 102 | self.assertTrue(isrot2(R, check=True)) 103 | 104 | R = rot2(0.4) 105 | R = np.round(R, 3) # approx SO(2) 106 | T = rt2tr(R, [3, 4]) 107 | 108 | T = trnorm2(T) 109 | self.assertTrue(ishom2(T, check=True)) 110 | nt.assert_almost_equal(T[:2, 2], [3, 4]) 111 | 112 | def test_transl2(self): 113 | nt.assert_array_almost_equal( 114 | transl2(1, 2), np.array([[1, 0, 1], [0, 1, 2], [0, 0, 1]]) 115 | ) 116 | nt.assert_array_almost_equal( 117 | transl2([1, 2]), np.array([[1, 0, 1], [0, 1, 2], [0, 0, 1]]) 118 | ) 119 | 120 | def test_pos2tr2(self): 121 | nt.assert_array_almost_equal( 122 | pos2tr2(1, 2), np.array([[1, 0, 1], [0, 1, 2], [0, 0, 1]]) 123 | ) 124 | nt.assert_array_almost_equal( 125 | transl2([1, 2]), np.array([[1, 0, 1], [0, 1, 2], [0, 0, 1]]) 126 | ) 127 | nt.assert_array_almost_equal(tr2pos2(pos2tr2(1, 2)), np.array([1, 2])) 128 | 129 | def test_tr2jac2(self): 130 | T = trot2(0.3, t=[4, 5]) 131 | jac2 = tr2jac2(T) 132 | nt.assert_array_almost_equal(jac2[:2, :2], smb.t2r(T)) 133 | nt.assert_array_almost_equal(jac2[:3, 2], np.array([0, 0, 1])) 134 | nt.assert_array_almost_equal(jac2[2, :3], np.array([0, 0, 1])) 135 | 136 | def test_xyt2tr(self): 137 | T = xyt2tr([1, 2, 0]) 138 | nt.assert_array_almost_equal(T, transl2(1, 2)) 139 | 140 | T = xyt2tr([1, 2, 0.2]) 141 | nt.assert_array_almost_equal(T, rt2tr(rot2(0.2), [1, 2])) 142 | 143 | def test_trinv2(self): 144 | T = rt2tr(rot2(0.2), [1, 2]) 145 | nt.assert_array_almost_equal(trinv2(T) @ T, np.eye(3)) 146 | 147 | def test_tradjoint2(self): 148 | T = xyt2tr([1, 2, 0.2]) 149 | X = [1, 2, 3] 150 | nt.assert_almost_equal(tradjoint2(T) @ X, vexa(T @ skewa(X) @ trinv2(T))) 151 | 152 | def test_points2tr2(self): 153 | p1 = np.random.uniform(size=(2, 5)) 154 | T = xyt2tr([1, 2, 0.2]) 155 | p2 = homtrans(T, p1) 156 | T2 = points2tr2(p1, p2) 157 | nt.assert_almost_equal(T, T2) 158 | 159 | def test_icp2d(self): 160 | p1 = np.random.uniform(size=(2, 30)) 161 | T = xyt2tr([1, 2, 0.2]) 162 | 163 | p2 = homtrans(T, p1) 164 | k = np.random.permutation(p2.shape[1]) 165 | p2 = p2[:, k] 166 | 167 | T2 = ICP2d(p2, p1, T=xyt2tr([1, 2, 0.2])) 168 | nt.assert_almost_equal(T, T2) 169 | 170 | def test_print2(self): 171 | T = transl2(1, 2) @ trot2(0.3) 172 | 173 | s = trprint2(T, file=None) 174 | self.assertIsInstance(s, str) 175 | self.assertEqual(len(s), 15) 176 | 177 | def test_checks(self): 178 | # 2D case, with rotation matrix 179 | R = np.eye(2) 180 | nt.assert_equal(isR(R), True) 181 | nt.assert_equal(isrot2(R), True) 182 | 183 | nt.assert_equal(ishom2(R), False) 184 | nt.assert_equal(isrot2(R, True), True) 185 | 186 | nt.assert_equal(ishom2(R, True), False) 187 | 188 | # 2D case, invalid rotation matrix 189 | R = np.array([[1, 1], [0, 1]]) 190 | nt.assert_equal(isR(R), False) 191 | nt.assert_equal(isrot2(R), True) 192 | nt.assert_equal(ishom2(R), False) 193 | nt.assert_equal(isrot2(R, True), False) 194 | nt.assert_equal(ishom2(R, True), False) 195 | 196 | # 2D case, with homogeneous transformation matrix 197 | T = np.array([[1, 0, 3], [0, 1, 4], [0, 0, 1]]) 198 | nt.assert_equal(isR(T), False) 199 | nt.assert_equal(isrot2(T), False) 200 | 201 | nt.assert_equal(ishom2(T), True) 202 | nt.assert_equal(isrot2(T, True), False) 203 | 204 | nt.assert_equal(ishom2(T, True), True) 205 | 206 | # 2D case, invalid rotation matrix 207 | T = np.array([[1, 1, 3], [0, 1, 4], [0, 0, 1]]) 208 | nt.assert_equal(isR(T), False) 209 | nt.assert_equal(isrot2(T), False) 210 | 211 | nt.assert_equal(ishom2(T), True) 212 | nt.assert_equal(isrot2(T, True), False) 213 | 214 | nt.assert_equal(ishom2(T, True), False) 215 | 216 | # 2D case, invalid bottom row 217 | T = np.array([[1, 1, 3], [0, 1, 4], [9, 0, 1]]) 218 | nt.assert_equal(isR(T), False) 219 | nt.assert_equal(isrot2(T), False) 220 | 221 | nt.assert_equal(ishom2(T), True) 222 | nt.assert_equal(isrot2(T, True), False) 223 | 224 | nt.assert_equal(ishom2(T, True), False) 225 | 226 | def test_trinterp2(self): 227 | R0 = rot2(-0.3) 228 | R1 = rot2(0.3) 229 | 230 | nt.assert_array_almost_equal(trinterp2(start=None, end=R1, s=0), np.eye(2)) 231 | nt.assert_array_almost_equal(trinterp2(start=None, end=R1, s=1), R1) 232 | nt.assert_array_almost_equal( 233 | trinterp2(start=None, end=R1, s=0.5), rot2(0.3 / 2) 234 | ) 235 | 236 | nt.assert_array_almost_equal(trinterp2(start=None, end=R1, s=0), np.eye(2)) 237 | nt.assert_array_almost_equal(trinterp2(start=None, end=R1, s=1), R1) 238 | nt.assert_array_almost_equal( 239 | trinterp2(start=None, end=R1, s=0.5), rot2(0.3 / 2) 240 | ) 241 | 242 | T0 = trot2(-0.3) 243 | T1 = trot2(0.3) 244 | 245 | nt.assert_array_almost_equal(trinterp2(start=T0, end=T1, s=0), T0) 246 | nt.assert_array_almost_equal(trinterp2(start=T0, end=T1, s=1), T1) 247 | nt.assert_array_almost_equal(trinterp2(start=T0, end=T1, s=0.5), np.eye(3)) 248 | 249 | nt.assert_array_almost_equal(trinterp2(start=None, end=T1, s=0), np.eye(3)) 250 | nt.assert_array_almost_equal(trinterp2(start=None, end=T1, s=1), T1) 251 | nt.assert_array_almost_equal( 252 | trinterp2(start=None, end=T1, s=0.5), trot2(0.3 / 2) 253 | ) 254 | 255 | T0 = transl2(-1, -2) 256 | T1 = transl2(1, 2) 257 | 258 | nt.assert_array_almost_equal(trinterp2(start=T0, end=T1, s=0), T0) 259 | nt.assert_array_almost_equal(trinterp2(start=T0, end=T1, s=1), T1) 260 | nt.assert_array_almost_equal(trinterp2(start=T0, end=T1, s=0.5), np.eye(3)) 261 | 262 | T0 = transl2(-1, -2) @ trot2(-0.3) 263 | T1 = transl2(1, 2) @ trot2(0.3) 264 | 265 | nt.assert_array_almost_equal(trinterp2(start=T0, end=T1, s=0), T0) 266 | nt.assert_array_almost_equal(trinterp2(start=T0, end=T1, s=1), T1) 267 | nt.assert_array_almost_equal(trinterp2(start=T0, end=T1, s=0.5), np.eye(3)) 268 | 269 | nt.assert_array_almost_equal(trinterp2(start=T0, end=T1, s=0), T0) 270 | nt.assert_array_almost_equal(trinterp2(start=T0, end=T1, s=1), T1) 271 | nt.assert_array_almost_equal(trinterp2(start=T0, end=T1, s=0.5), np.eye(3)) 272 | 273 | nt.assert_array_almost_equal(trinterp2(start=None, end=T1, s=0), np.eye(3)) 274 | nt.assert_array_almost_equal(trinterp2(start=None, end=T1, s=1), T1) 275 | nt.assert_array_almost_equal( 276 | trinterp2(start=None, end=T1, s=0.5), xyt2tr([0.5, 1, 0.15]) 277 | ) 278 | 279 | @pytest.mark.skipif( 280 | sys.platform.startswith("darwin") and sys.version_info < (3, 11), 281 | reason="tkinter bug with mac", 282 | ) 283 | def test_plot(self): 284 | plt.figure() 285 | trplot2(transl2(1, 2), block=False, frame="A", rviz=True, width=1) 286 | trplot2(transl2(3, 1), block=False, color="red", arrow=True, width=3, frame="B") 287 | trplot2( 288 | transl2(4, 3) @ trot2(math.pi / 3), block=False, color="green", frame="c" 289 | ) 290 | plt.close("all") 291 | 292 | 293 | # ---------------------------------------------------------------------------------------# 294 | if __name__ == "__main__": 295 | unittest.main() 296 | --------------------------------------------------------------------------------