From bfecbbb6949ad4349df75256a383a7200eb6305f Mon Sep 17 00:00:00 2001 From: Mark Yeatman Date: Thu, 30 Jan 2025 13:03:46 -0500 Subject: [PATCH] Format everything and double check black works. Disable ruff. --- .pre-commit-config.yaml | 35 ++++ docs/source/conf.py | 10 +- pyproject.toml | 9 +- spatialmath/DualQuaternion.py | 1 - spatialmath/__init__.py | 4 +- spatialmath/base/animate.py | 19 ++- spatialmath/base/quaternions.py | 75 ++++++--- spatialmath/base/transforms2d.py | 12 +- spatialmath/base/transforms3d.py | 25 +-- spatialmath/baseposelist.py | 5 +- spatialmath/baseposematrix.py | 19 ++- spatialmath/pose3d.py | 33 ++-- spatialmath/quaternion.py | 15 +- spatialmath/spline.py | 22 +-- spatialmath/twist.py | 2 +- tests/base/test_numeric.py | 9 - tests/base/test_symbolic.py | 5 +- tests/base/test_transforms.py | 9 - tests/base/test_transforms2d.py | 19 +-- tests/base/test_transforms3d.py | 6 +- tests/base/test_transformsNd.py | 5 +- tests/base/test_vectors.py | 49 +++--- tests/test_baseposelist.py | 22 +-- tests/test_dualquaternion.py | 59 +++---- tests/test_pose3d.py | 8 +- tests/test_quaternion.py | 12 +- tests/test_spline.py | 45 +++-- tests/test_twist.py | 274 ++++++++++++++++--------------- 28 files changed, 437 insertions(+), 371 deletions(-) create mode 100644 .pre-commit-config.yaml diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 00000000..36085fe2 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,35 @@ +repos: +# - repo: https://github.com/charliermarsh/ruff-pre-commit +# # Ruff version. +# rev: 'v0.1.0' +# hooks: +# - id: ruff +# args: ['--fix', '--config', 'pyproject.toml'] + +- repo: https://github.com/psf/black + rev: 'refs/tags/23.10.0:refs/tags/23.10.0' + hooks: + - id: black + language_version: python3.10 + args: ['--config', 'pyproject.toml'] + verbose: true + +- repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.5.0 + hooks: + - id: end-of-file-fixer + - id: debug-statements # Ensure we don't commit `import pdb; pdb.set_trace()` + exclude: | + (?x)^( + docker/ros/web/static/.*| + )$ + - id: trailing-whitespace + exclude: | + (?x)^( + docker/ros/web/static/.*| + (.*/).*\.patch| + )$ +# - repo: https://github.com/pre-commit/mirrors-mypy +# rev: v1.6.1 +# hooks: +# - id: mypy diff --git a/docs/source/conf.py b/docs/source/conf.py index b039bc85..b5fcf84a 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -11,8 +11,6 @@ # add these directories to sys.path here. If the directory is relative to the # documentation root, use os.path.abspath to make it absolute, like shown here. # -import os -import sys # sys.path.insert(0, os.path.abspath('.')) # sys.path.insert(0, os.path.abspath('..')) @@ -67,9 +65,11 @@ # choose UTF-8 encoding to allow for Unicode characters, eg. ansitable # Python session setup, turn off color printing for SE3, set NumPy precision autorun_languages = {} -autorun_languages['pycon_output_encoding'] = 'UTF-8' -autorun_languages['pycon_input_encoding'] = 'UTF-8' -autorun_languages['pycon_runfirst'] = """ +autorun_languages["pycon_output_encoding"] = "UTF-8" +autorun_languages["pycon_input_encoding"] = "UTF-8" +autorun_languages[ + "pycon_runfirst" +] = """ from spatialmath import SE3 SE3._color = False import numpy as np diff --git a/pyproject.toml b/pyproject.toml index 4295f2f3..452bf290 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -28,7 +28,7 @@ keywords = [ "SO(2)", "SE(2)", "SO(3)", "SE(3)", "twist", "product of exponential", "translation", "orientation", "angle-axis", "Lie group", "skew symmetric matrix", - "pose", "translation", "rotation matrix", + "pose", "translation", "rotation matrix", "rigid body transform", "homogeneous transformation", "Euler angles", "roll-pitch-yaw angles", "quaternion", "unit-quaternion", @@ -63,9 +63,9 @@ dev = [ docs = [ "sphinx", - "sphinx-rtd-theme", - "sphinx-autorun", - "sphinxcontrib-jsmath", + "sphinx-rtd-theme", + "sphinx-autorun", + "sphinxcontrib-jsmath", "sphinx-favicon", "sphinx-autodoc-typehints", ] @@ -88,6 +88,7 @@ packages = [ ] [tool.black] +required-version = "23.10.0" line-length = 88 target-version = ['py38'] exclude = "camera_derivatives.py" diff --git a/spatialmath/DualQuaternion.py b/spatialmath/DualQuaternion.py index 3b945d7c..f8ee0f7d 100644 --- a/spatialmath/DualQuaternion.py +++ b/spatialmath/DualQuaternion.py @@ -357,7 +357,6 @@ def SE3(self) -> SE3: if __name__ == "__main__": # pragma: no cover - from spatialmath import SE3, UnitDualQuaternion print(UnitDualQuaternion(SE3())) diff --git a/spatialmath/__init__.py b/spatialmath/__init__.py index 18cb74b4..551481e1 100644 --- a/spatialmath/__init__.py +++ b/spatialmath/__init__.py @@ -17,8 +17,6 @@ from spatialmath.DualQuaternion import DualQuaternion, UnitDualQuaternion from spatialmath.spline import BSplineSE3, InterpSplineSE3, SplineFit -# from spatialmath.Plucker import * -# from spatialmath import base as smb __all__ = [ # pose @@ -46,7 +44,7 @@ "Ellipse", "BSplineSE3", "InterpSplineSE3", - "SplineFit" + "SplineFit", ] try: diff --git a/spatialmath/base/animate.py b/spatialmath/base/animate.py index a2e31f72..1ca8baec 100755 --- a/spatialmath/base/animate.py +++ b/spatialmath/base/animate.py @@ -109,7 +109,9 @@ def __init__( if len(dim) == 2: dim = dim * 3 elif len(dim) != 6: - raise ValueError(f"dim must have 2 or 6 elements, got {dim}. See docstring for details.") + raise ValueError( + f"dim must have 2 or 6 elements, got {dim}. See docstring for details." + ) ax.set_xlim(dim[0:2]) ax.set_ylim(dim[2:4]) ax.set_zlim(dim[4:]) @@ -223,7 +225,7 @@ def update(frame, animation): else: # [unlikely] other types are converted to np array T = np.array(frame) - + if T.shape == (3, 3): T = smb.r2t(T) @@ -606,14 +608,14 @@ def trplot2( smb.trplot2(self.start, ax=self, block=False, **kwargs) def run( - self, + self, movie: Optional[str] = None, axes: Optional[plt.Axes] = None, repeat: bool = False, interval: int = 50, nframes: int = 100, - wait: bool = False, - **kwargs + wait: bool = False, + **kwargs, ): """ Run the animation @@ -663,7 +665,6 @@ def update(frame, animation): animation._draw(T) self.count += 1 # say we're still running - if movie is not None: repeat = False @@ -698,7 +699,9 @@ def update(frame, animation): print("overwriting movie", movie) else: print("creating movie", movie) - FFwriter = animation.FFMpegWriter(fps=1000 / interval, extra_args=["-vcodec", "libx264"]) + FFwriter = animation.FFMpegWriter( + fps=1000 / interval, extra_args=["-vcodec", "libx264"] + ) _ani.save(movie, writer=FFwriter) if wait: @@ -902,8 +905,6 @@ def set_ylabel(self, *args, **kwargs): # plotvol3(2) # tranimate(attitude()) - from spatialmath import base - # T = smb.rpy2r(0.3, 0.4, 0.5) # # smb.tranimate(T, wait=True) # s = smb.tranimate(T, movie=True) diff --git a/spatialmath/base/quaternions.py b/spatialmath/base/quaternions.py index d5652d4a..364a5ea8 100755 --- a/spatialmath/base/quaternions.py +++ b/spatialmath/base/quaternions.py @@ -851,39 +851,49 @@ def qslerp( def _compute_cdf_sin_squared(theta: float): """ Computes the CDF for the distribution of angular magnitude for uniformly sampled rotations. - + :arg theta: angular magnitude :rtype: float :return: cdf of a given angular magnitude :rtype: float - Helper function for uniform sampling of rotations with constrained angular magnitude. + Helper function for uniform sampling of rotations with constrained angular magnitude. This function returns the integral of the pdf of angular magnitudes (2/pi * sin^2(theta/2)). """ return (theta - np.sin(theta)) / np.pi + @lru_cache(maxsize=1) -def _generate_inv_cdf_sin_squared_interp(num_interpolation_points: int = 256) -> interpolate.interp1d: +def _generate_inv_cdf_sin_squared_interp( + num_interpolation_points: int = 256, +) -> interpolate.interp1d: """ Computes an interpolation function for the inverse CDF of the distribution of angular magnitude. - + :arg num_interpolation_points: number of points to use in the interpolation function :rtype: int :return: interpolation function for the inverse cdf of a given angular magnitude :rtype: interpolate.interp1d - Helper function for uniform sampling of rotations with constrained angular magnitude. - This function returns interpolation function for the inverse of the integral of the + Helper function for uniform sampling of rotations with constrained angular magnitude. + This function returns interpolation function for the inverse of the integral of the pdf of angular magnitudes (2/pi * sin^2(theta/2)), which is not analytically defined. """ cdf_sin_squared_interp_angles = np.linspace(0, np.pi, num_interpolation_points) - cdf_sin_squared_interp_values = _compute_cdf_sin_squared(cdf_sin_squared_interp_angles) - return interpolate.interp1d(cdf_sin_squared_interp_values, cdf_sin_squared_interp_angles) + cdf_sin_squared_interp_values = _compute_cdf_sin_squared( + cdf_sin_squared_interp_angles + ) + return interpolate.interp1d( + cdf_sin_squared_interp_values, cdf_sin_squared_interp_angles + ) + -def _compute_inv_cdf_sin_squared(x: ArrayLike, num_interpolation_points: int = 256) -> ArrayLike: +def _compute_inv_cdf_sin_squared( + x: ArrayLike, num_interpolation_points: int = 256 +) -> ArrayLike: """ Computes the inverse CDF of the distribution of angular magnitude. - + :arg x: value for cdf of angular magnitudes :rtype: ArrayLike :arg num_interpolation_points: number of points to use in the interpolation function @@ -891,17 +901,24 @@ def _compute_inv_cdf_sin_squared(x: ArrayLike, num_interpolation_points: int = 2 :return: angular magnitude associate with cdf value :rtype: ArrayLike - Helper function for uniform sampling of rotations with constrained angular magnitude. - This function returns the angle associated with the cdf value derived form integral of + Helper function for uniform sampling of rotations with constrained angular magnitude. + This function returns the angle associated with the cdf value derived form integral of the pdf of angular magnitudes (2/pi * sin^2(theta/2)), which is not analytically defined. """ - inv_cdf_sin_squared_interp = _generate_inv_cdf_sin_squared_interp(num_interpolation_points) + inv_cdf_sin_squared_interp = _generate_inv_cdf_sin_squared_interp( + num_interpolation_points + ) return inv_cdf_sin_squared_interp(x) -def qrand(theta_range:Optional[ArrayLike2] = None, unit: str = "rad", num_interpolation_points: int = 256) -> UnitQuaternionArray: + +def qrand( + theta_range: Optional[ArrayLike2] = None, + unit: str = "rad", + num_interpolation_points: int = 256, +) -> UnitQuaternionArray: """ Random unit-quaternion - + :arg theta_range: angular magnitude range [min,max], defaults to None. :type xrange: 2-element sequence, optional :arg unit: angular units: 'rad' [default], or 'deg' @@ -913,7 +930,7 @@ def qrand(theta_range:Optional[ArrayLike2] = None, unit: str = "rad", num_interp :return: random unit-quaternion :rtype: ndarray(4) - Computes a uniformly distributed random unit-quaternion, with in a maximum + Computes a uniformly distributed random unit-quaternion, with in a maximum angular magnitude, which can be considered equivalent to a random SO(3) rotation. .. runblock:: pycon @@ -924,24 +941,30 @@ def qrand(theta_range:Optional[ArrayLike2] = None, unit: str = "rad", num_interp if theta_range is not None: theta_range = getunit(theta_range, unit) - if(theta_range[0] < 0 or theta_range[1] > np.pi or theta_range[0] > theta_range[1]): - ValueError('Invalid angular range. Must be within the range[0, pi].' - + f' Recieved {theta_range}.') + if ( + theta_range[0] < 0 + or theta_range[1] > np.pi + or theta_range[0] > theta_range[1] + ): + ValueError( + "Invalid angular range. Must be within the range[0, pi]." + + f" Recieved {theta_range}." + ) + + # Sample axis and angle independently, respecting the CDF of the + # angular magnitude under uniform sampling. - # Sample axis and angle independently, respecting the CDF of the - # angular magnitude under uniform sampling. - - # Sample angle using inverse transform sampling based on CDF + # Sample angle using inverse transform sampling based on CDF # of the angular distribution (2/pi * sin^2(theta/2)) theta = _compute_inv_cdf_sin_squared( np.random.uniform( - low=_compute_cdf_sin_squared(theta_range[0]), + low=_compute_cdf_sin_squared(theta_range[0]), high=_compute_cdf_sin_squared(theta_range[1]), ), num_interpolation_points=num_interpolation_points, ) # Sample axis uniformly using 3D normal distributed - v = np.random.randn(3) + v = np.random.randn(3) v /= np.linalg.norm(v) return np.r_[math.cos(theta / 2), (math.sin(theta / 2) * v)] @@ -953,7 +976,7 @@ def qrand(theta_range:Optional[ArrayLike2] = None, unit: str = "rad", num_interp math.sqrt(u[0]) * math.sin(2 * math.pi * u[2]), math.sqrt(u[0]) * math.cos(2 * math.pi * u[2]), ] - + def qmatrix(q: ArrayLike4) -> R4x4: """ diff --git a/spatialmath/base/transforms2d.py b/spatialmath/base/transforms2d.py index 25265fff..ac0696cd 100644 --- a/spatialmath/base/transforms2d.py +++ b/spatialmath/base/transforms2d.py @@ -746,7 +746,7 @@ def trnorm2(T: SE2Array) -> SE2Array: b = unitvec(b) # fmt: off R = np.array([ - [ b[1], b[0]], + [ b[1], b[0]], [-b[0], b[1]] ]) # fmt: on @@ -810,7 +810,7 @@ def tradjoint2(T): (R, t) = smb.tr2rt(cast(SE3Array, T)) # fmt: off return np.block([ - [R, np.c_[t[1], -t[0]].T], + [R, np.c_[t[1], -t[0]].T], [0, 0, 1] ]) # type: ignore # fmt: on @@ -853,12 +853,16 @@ def tr2jac2(T: SE2Array) -> R3x3: @overload -def trinterp2(start: Optional[SO2Array], end: SO2Array, s: float, shortest: bool = True) -> SO2Array: +def trinterp2( + start: Optional[SO2Array], end: SO2Array, s: float, shortest: bool = True +) -> SO2Array: ... @overload -def trinterp2(start: Optional[SE2Array], end: SE2Array, s: float, shortest: bool = True) -> SE2Array: +def trinterp2( + start: Optional[SE2Array], end: SE2Array, s: float, shortest: bool = True +) -> SE2Array: ... diff --git a/spatialmath/base/transforms3d.py b/spatialmath/base/transforms3d.py index 350e1d14..bc2ceb05 100644 --- a/spatialmath/base/transforms3d.py +++ b/spatialmath/base/transforms3d.py @@ -40,7 +40,6 @@ isskew, isskewa, isR, - iseye, tr2rt, Ab2M, ) @@ -1605,12 +1604,16 @@ def trnorm(T: SE3Array) -> SE3Array: @overload -def trinterp(start: Optional[SO3Array], end: SO3Array, s: float, shortest: bool = True) -> SO3Array: +def trinterp( + start: Optional[SO3Array], end: SO3Array, s: float, shortest: bool = True +) -> SO3Array: ... @overload -def trinterp(start: Optional[SE3Array], end: SE3Array, s: float, shortest: bool = True) -> SE3Array: +def trinterp( + start: Optional[SE3Array], end: SE3Array, s: float, shortest: bool = True +) -> SE3Array: ... @@ -1954,15 +1957,15 @@ def rpy2jac(angles: ArrayLike3, order: str = "zyx") -> R3x3: if order == "xyz": # fmt: off - J = np.array([ - [ sp, 0, 1], + J = np.array([ + [ sp, 0, 1], [-cp * sy, cy, 0], [ cp * cy, sy, 0] ]) # type: ignore # fmt: on elif order == "zyx": # fmt: off - J = np.array([ + J = np.array([ [ cp * cy, -sy, 0], [ cp * sy, cy, 0], [-sp, 0, 1], @@ -1970,7 +1973,7 @@ def rpy2jac(angles: ArrayLike3, order: str = "zyx") -> R3x3: # fmt: on elif order == "yxz": # fmt: off - J = np.array([ + J = np.array([ [ cp * sy, cy, 0], [-sp, 0, 1], [ cp * cy, -sy, 0] @@ -2350,7 +2353,7 @@ def rotvelxform( # analytical rates -> angular velocity # fmt: off A = np.array([ - [ S(beta), 0, 1], + [ S(beta), 0, 1], [-S(gamma)*C(beta), C(gamma), 0], # type: ignore [ C(beta)*C(gamma), S(gamma), 0] # type: ignore ]) @@ -2360,7 +2363,7 @@ def rotvelxform( # fmt: off A = np.array([ [0, -S(gamma)/C(beta), C(gamma)/C(beta)], # type: ignore - [0, C(gamma), S(gamma)], + [0, C(gamma), S(gamma)], [1, S(gamma)*T(beta), -C(gamma)*T(beta)] # type: ignore ]) # fmt: on @@ -2724,7 +2727,7 @@ def tr2adjoint(T): (R, t) = tr2rt(T) # fmt: off return np.block([ - [R, skew(t) @ R], + [R, skew(t) @ R], [Z, R] ]) # fmt: on @@ -3432,8 +3435,6 @@ def tranimate(T: Union[SO3Array, SE3Array], **kwargs) -> str: # print(angvelxform([p, q, r], representation='eul')) - import pathlib - # exec( # open( # pathlib.Path(__file__).parent.parent.parent.absolute() diff --git a/spatialmath/baseposelist.py b/spatialmath/baseposelist.py index d729b902..b102b4bb 100644 --- a/spatialmath/baseposelist.py +++ b/spatialmath/baseposelist.py @@ -667,11 +667,12 @@ def unop( else: return [op(x) for x in self.data] + if __name__ == "__main__": from spatialmath import SO3, SO2 - R = SO3([[1,0,0],[0,1,0],[0,0,1]]) + R = SO3([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) print(R.eulervec()) R = SO2([0.3, 0.4, 0.5]) - pass \ No newline at end of file + pass diff --git a/spatialmath/baseposematrix.py b/spatialmath/baseposematrix.py index 1a850600..87071df3 100644 --- a/spatialmath/baseposematrix.py +++ b/spatialmath/baseposematrix.py @@ -377,7 +377,12 @@ def log(self, twist: Optional[bool] = False) -> Union[NDArray, List[NDArray]]: else: return log - def interp(self, end: Optional[bool] = None, s: Union[int, float] = None, shortest: bool = True) -> Self: + def interp( + self, + end: Optional[bool] = None, + s: Union[int, float] = None, + shortest: bool = True, + ) -> Self: """ Interpolate between poses (superclass method) @@ -434,13 +439,19 @@ def interp(self, end: Optional[bool] = None, s: Union[int, float] = None, shorte if self.N == 2: # SO(2) or SE(2) return self.__class__( - [smb.trinterp2(start=self.A, end=end, s=_s, shortest=shortest) for _s in s] + [ + smb.trinterp2(start=self.A, end=end, s=_s, shortest=shortest) + for _s in s + ] ) elif self.N == 3: # SO(3) or SE(3) return self.__class__( - [smb.trinterp(start=self.A, end=end, s=_s, shortest=shortest) for _s in s] + [ + smb.trinterp(start=self.A, end=end, s=_s, shortest=shortest) + for _s in s + ] ) def interp1(self, s: float = None) -> Self: @@ -1692,7 +1703,7 @@ def _op2(left, right: Self, op: Callable): # pylint: disable=no-self-argument if __name__ == "__main__": - from spatialmath import SE3, SE2, SO2 + from spatialmath import SO2 C = SO2(0.5) A = np.array([[10, 0], [0, 1]]) diff --git a/spatialmath/pose3d.py b/spatialmath/pose3d.py index 3b4821e5..9324eda1 100644 --- a/spatialmath/pose3d.py +++ b/spatialmath/pose3d.py @@ -17,7 +17,7 @@ .. inheritance-diagram:: spatialmath.pose3d :top-classes: collections.UserList :parts: 1 - + .. image:: ../figs/pose-values.png """ from __future__ import annotations @@ -35,6 +35,7 @@ from spatialmath.twist import Twist3 from typing import TYPE_CHECKING, Optional + if TYPE_CHECKING: from spatialmath.quaternion import UnitQuaternion @@ -341,7 +342,7 @@ def eulervec(self) -> R3: """ theta, v = smb.tr2angvec(self.R) return theta * v - + # ------------------------------------------------------------------------ # @staticmethod @@ -455,7 +456,9 @@ def Rz(cls, theta, unit: str = "rad") -> Self: return cls([smb.rotz(x, unit=unit) for x in smb.getvector(theta)], check=False) @classmethod - def Rand(cls, N: int = 1, *, theta_range:Optional[ArrayLike2] = None, unit: str = "rad") -> Self: + def Rand( + cls, N: int = 1, *, theta_range: Optional[ArrayLike2] = None, unit: str = "rad" + ) -> Self: """ Construct a new SO(3) from random rotation @@ -481,7 +484,13 @@ def Rand(cls, N: int = 1, *, theta_range:Optional[ArrayLike2] = None, unit: str :seealso: :func:`spatialmath.quaternion.UnitQuaternion.Rand` """ - return cls([smb.q2r(smb.qrand(theta_range=theta_range, unit=unit)) for _ in range(0, N)], check=False) + return cls( + [ + smb.q2r(smb.qrand(theta_range=theta_range, unit=unit)) + for _ in range(0, N) + ], + check=False, + ) @overload @classmethod @@ -1311,11 +1320,11 @@ def yaw_SE2(self, order: str = "zyx") -> SE2: """ if len(self) == 1: if order == "zyx": - return SE2(self.x, self.y, self.rpy(order = order)[2]) + return SE2(self.x, self.y, self.rpy(order=order)[2]) elif order == "xyz": - return SE2(self.z, self.y, self.rpy(order = order)[2]) + return SE2(self.z, self.y, self.rpy(order=order)[2]) elif order == "yxz": - return SE2(self.z, self.x, self.rpy(order = order)[2]) + return SE2(self.z, self.x, self.rpy(order=order)[2]) else: return SE2([e.yaw_SE2() for e in self]) @@ -1601,7 +1610,7 @@ def Rand( xrange: Optional[ArrayLike2] = (-1, 1), yrange: Optional[ArrayLike2] = (-1, 1), zrange: Optional[ArrayLike2] = (-1, 1), - theta_range:Optional[ArrayLike2] = None, + theta_range: Optional[ArrayLike2] = None, unit: str = "rad", ) -> SE3: # pylint: disable=arguments-differ """ @@ -1825,7 +1834,7 @@ def AngleAxis( a rotation of ``θ`` about the vector ``v``. .. math:: - + \mbox{if}\,\, \theta \left\{ \begin{array}{ll} = 0 & \mbox{return identity matrix}\\ \ne 0 & \mbox{v must have a finite length} @@ -2100,11 +2109,7 @@ def Rt( return cls(smb.rt2tr(R, t, check=check), check=check) @classmethod - def CopyFrom( - cls, - T: SE3Array, - check: bool = True - ) -> SE3: + def CopyFrom(cls, T: SE3Array, check: bool = True) -> SE3: """ Create an SE(3) from a 4x4 numpy array that is passed by value. diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index 79bdaf0c..2994b6e6 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -17,7 +17,7 @@ from __future__ import annotations import math import numpy as np -from typing import Any, Type +from typing import Any import spatialmath.base as smb from spatialmath.pose3d import SO3, SE3 from spatialmath.baseposelist import BasePoseList @@ -1005,10 +1005,10 @@ def __init__( """ super().__init__() - # handle: UnitQuaternion(v)`` constructs a unit quaternion with specified elements + # handle: UnitQuaternion(v)`` constructs a unit quaternion with specified elements # from ``v`` which is a 4-vector given as a list, tuple, or ndarray(4) if s is None and smb.isvector(v, 4): - v,s = (s,v) + v, s = (s, v) if v is None: # single argument @@ -1248,7 +1248,9 @@ def Rz(cls, angles: ArrayLike, unit: Optional[str] = "rad") -> UnitQuaternion: ) @classmethod - def Rand(cls, N: int = 1, *, theta_range:Optional[ArrayLike2] = None, unit: str = "rad") -> UnitQuaternion: + def Rand( + cls, N: int = 1, *, theta_range: Optional[ArrayLike2] = None, unit: str = "rad" + ) -> UnitQuaternion: """ Construct a new random unit quaternion @@ -1275,7 +1277,10 @@ def Rand(cls, N: int = 1, *, theta_range:Optional[ArrayLike2] = None, unit: str :seealso: :meth:`UnitQuaternion.Rand` """ - return cls([smb.qrand(theta_range=theta_range, unit=unit) for i in range(0, N)], check=False) + return cls( + [smb.qrand(theta_range=theta_range, unit=unit) for i in range(0, N)], + check=False, + ) @classmethod def Eul(cls, *angles: List[float], unit: Optional[str] = "rad") -> UnitQuaternion: diff --git a/spatialmath/spline.py b/spatialmath/spline.py index 8d30bd80..7f849442 100644 --- a/spatialmath/spline.py +++ b/spatialmath/spline.py @@ -2,12 +2,11 @@ # MIT Licence, see details in top-level file: LICENCE """ -Classes for parameterizing a trajectory in SE3 with splines. +Classes for parameterizing a trajectory in SE3 with splines. """ from abc import ABC, abstractmethod -from functools import cached_property -from typing import List, Optional, Tuple, Set +from typing import List, Optional, Tuple import matplotlib.pyplot as plt import numpy as np @@ -160,11 +159,11 @@ def stochastic_downsample_interpolation( epsilon_angle: float = 1e-1, normalize_time: bool = True, bc_type: str = "not-a-knot", - check_type: str = "local" + check_type: str = "local", ) -> Tuple[InterpSplineSE3, List[int]]: """ - Uses a random dropout to downsample a trajectory with an interpolated spline. Keeps the start and - end points of the trajectory. Takes a random order of the remaining indices, and then checks the error bound + Uses a random dropout to downsample a trajectory with an interpolated spline. Keeps the start and + end points of the trajectory. Takes a random order of the remaining indices, and then checks the error bound of just that point if check_type=="local", checks the error of the whole trajectory is check_type=="global". Local is **much** faster. @@ -175,7 +174,7 @@ def stochastic_downsample_interpolation( interpolation_indices = list(range(len(self.pose_data))) - # randomly attempt to remove poses from the trajectory + # randomly attempt to remove poses from the trajectory # always keep the start and end removal_choices = interpolation_indices.copy() removal_choices.remove(0) @@ -197,14 +196,17 @@ def stochastic_downsample_interpolation( SO3(self.spline.spline_so3(sample_time).as_matrix()) ) euclidean_error = np.linalg.norm( - self.pose_data[candidate_removal_index].t - self.spline.spline_xyz(sample_time) + self.pose_data[candidate_removal_index].t + - self.spline.spline_xyz(sample_time) ) elif check_type == "global": angular_error = self.max_angular_error() euclidean_error = self.max_euclidean_error() else: - raise ValueError(f"check_type must be 'local' of 'global', is {check_type}.") - + raise ValueError( + f"check_type must be 'local' of 'global', is {check_type}." + ) + if (angular_error > epsilon_angle) or (euclidean_error > epsilon_xyz): interpolation_indices.append(candidate_removal_index) interpolation_indices.sort() diff --git a/spatialmath/twist.py b/spatialmath/twist.py index f84a0f1b..dcefa840 100644 --- a/spatialmath/twist.py +++ b/spatialmath/twist.py @@ -655,7 +655,7 @@ def RPY(cls, *pos, **kwargs): scalars. Foo bar! - + Example: .. runblock:: pycon diff --git a/tests/base/test_numeric.py b/tests/base/test_numeric.py index e6b9de50..256a3cb1 100755 --- a/tests/base/test_numeric.py +++ b/tests/base/test_numeric.py @@ -8,9 +8,7 @@ """ import numpy as np -import numpy.testing as nt import unittest -import math from spatialmath.base.numeric import * @@ -18,11 +16,9 @@ class TestNumeric(unittest.TestCase): def test_numjac(self): - pass def test_array2str(self): - x = [1.2345678] s = array2str(x) @@ -52,7 +48,6 @@ def test_array2str(self): self.assertEqual(s, "[ 1, 2, 3 | 4, 5, 6 ]") def test_bresenham(self): - x, y = bresenham((-10, -10), (20, 10)) self.assertIsInstance(x, np.ndarray) self.assertEqual(x.ndim, 1) @@ -91,7 +86,6 @@ def test_bresenham(self): self.assertTrue(all(d <= np.sqrt(2))) def test_mpq(self): - data = np.array([[-1, 1, 1, -1], [-1, -1, 1, 1]]) self.assertEqual(mpq_point(data, 0, 0), 4) @@ -99,7 +93,6 @@ def test_mpq(self): self.assertEqual(mpq_point(data, 0, 1), 0) def test_gauss1d(self): - x = np.arange(-10, 10, 0.02) y = gauss1d(2, 1, x) @@ -109,7 +102,6 @@ def test_gauss1d(self): self.assertAlmostEqual(x[m], 2) def test_gauss2d(self): - r = np.arange(-10, 10, 0.02) X, Y = np.meshgrid(r, r) Z = gauss2d([2, 3], np.eye(2), X, Y) @@ -121,5 +113,4 @@ def test_gauss2d(self): # ---------------------------------------------------------------------------------------# if __name__ == "__main__": - unittest.main() diff --git a/tests/base/test_symbolic.py b/tests/base/test_symbolic.py index 7a503b5b..cc441cc5 100644 --- a/tests/base/test_symbolic.py +++ b/tests/base/test_symbolic.py @@ -46,7 +46,6 @@ def test_issymbol(self): @unittest.skipUnless(_symbolics, "sympy required") def test_functions(self): - theta = symbol("theta") self.assertTrue(isinstance(sin(theta), sp.Expr)) self.assertTrue(isinstance(sin(1.0), float)) @@ -57,12 +56,11 @@ def test_functions(self): self.assertTrue(isinstance(sqrt(theta), sp.Expr)) self.assertTrue(isinstance(sqrt(1.0), float)) - x = (theta - 1) * (theta + 1) - theta ** 2 + x = (theta - 1) * (theta + 1) - theta**2 self.assertTrue(math.isclose(simplify(x).evalf(), -1)) @unittest.skipUnless(_symbolics, "sympy required") def test_constants(self): - x = zero() self.assertTrue(isinstance(x, sp.Expr)) self.assertTrue(math.isclose(x.evalf(), 0)) @@ -82,5 +80,4 @@ def test_constants(self): # ---------------------------------------------------------------------------------------# if __name__ == "__main__": # pragma: no cover - unittest.main() diff --git a/tests/base/test_transforms.py b/tests/base/test_transforms.py index 71b01bb3..67f3e776 100755 --- a/tests/base/test_transforms.py +++ b/tests/base/test_transforms.py @@ -12,13 +12,9 @@ import numpy.testing as nt import unittest from math import pi -import math from scipy.linalg import logm, expm from spatialmath.base import * -from spatialmath.base import sym - -import matplotlib.pyplot as plt class TestLie(unittest.TestCase): @@ -49,7 +45,6 @@ def test_skew(self): ) # check contents, vex already verified def test_vexa(self): - S = np.array([[0, -3, 1], [3, 0, 2], [0, 0, 0]]) nt.assert_array_almost_equal(vexa(S), np.array([1, 2, 3])) @@ -80,7 +75,6 @@ def test_skewa(self): ) # check contents, vexa already verified def test_trlog(self): - # %%% SO(3) tests # zero rotation case nt.assert_array_almost_equal(trlog(np.eye(3)), skew([0, 0, 0])) @@ -189,7 +183,6 @@ def test_trlog(self): # TODO def test_trexp(self): - # %% SO(3) tests # % so(3) @@ -271,7 +264,6 @@ def test_trexp(self): nt.assert_array_almost_equal(trexp(trlog(T)), T) def test_trexp2(self): - # % so(2) # zero rotation case @@ -323,5 +315,4 @@ def test_trnorm(self): # ---------------------------------------------------------------------------------------# if __name__ == "__main__": - unittest.main() diff --git a/tests/base/test_transforms2d.py b/tests/base/test_transforms2d.py index ff099930..f78e38e3 100755 --- a/tests/base/test_transforms2d.py +++ b/tests/base/test_transforms2d.py @@ -12,7 +12,7 @@ import unittest from math import pi import math -from scipy.linalg import logm, expm +from scipy.linalg import logm import pytest import sys @@ -27,7 +27,6 @@ skewa, homtrans, ) -from spatialmath.base.numeric import numjac import matplotlib.pyplot as plt @@ -125,22 +124,14 @@ def test_pos2tr2(self): nt.assert_array_almost_equal( transl2([1, 2]), np.array([[1, 0, 1], [0, 1, 2], [0, 0, 1]]) ) - nt.assert_array_almost_equal( - tr2pos2(pos2tr2(1, 2)), np.array([1, 2]) - ) + nt.assert_array_almost_equal(tr2pos2(pos2tr2(1, 2)), np.array([1, 2])) def test_tr2jac2(self): T = trot2(0.3, t=[4, 5]) jac2 = tr2jac2(T) - nt.assert_array_almost_equal( - jac2[:2, :2], smb.t2r(T) - ) - nt.assert_array_almost_equal( - jac2[:3, 2], np.array([0, 0, 1]) - ) - nt.assert_array_almost_equal( - jac2[2, :3], np.array([0, 0, 1]) - ) + nt.assert_array_almost_equal(jac2[:2, :2], smb.t2r(T)) + nt.assert_array_almost_equal(jac2[:3, 2], np.array([0, 0, 1])) + nt.assert_array_almost_equal(jac2[2, :3], np.array([0, 0, 1])) def test_xyt2tr(self): T = xyt2tr([1, 2, 0]) diff --git a/tests/base/test_transforms3d.py b/tests/base/test_transforms3d.py index 2f1e6049..8b2fb080 100755 --- a/tests/base/test_transforms3d.py +++ b/tests/base/test_transforms3d.py @@ -12,8 +12,7 @@ import numpy.testing as nt import unittest from math import pi -import math -from scipy.linalg import logm, expm +from scipy.linalg import logm from spatialmath.base.transforms3d import * from spatialmath.base.transformsNd import isR, t2r, r2t, rt2tr, skew @@ -518,7 +517,7 @@ def test_tr2angvec(self): nt.assert_array_almost_equal(v, np.r_[0, 1, 0]) true_ang = 1.51 - true_vec = np.array([0., 1., 0.]) + true_vec = np.array([0.0, 1.0, 0.0]) eps = 1e-08 # show that tr2angvec works on true rotation matrix @@ -806,6 +805,7 @@ def test_x2tr(self): x2tr(x, representation="exp"), transl(t) @ r2t(trexp(gamma)) ) + # ---------------------------------------------------------------------------------------# if __name__ == "__main__": unittest.main() diff --git a/tests/base/test_transformsNd.py b/tests/base/test_transformsNd.py index 92d9e2a3..14c7fd42 100755 --- a/tests/base/test_transformsNd.py +++ b/tests/base/test_transformsNd.py @@ -11,8 +11,6 @@ import numpy.testing as nt import unittest from math import pi -import math -from scipy.linalg import logm, expm from spatialmath.base.transformsNd import * from spatialmath.base.transforms3d import trotx, transl, rotx, isrot, ishom @@ -25,7 +23,6 @@ from spatialmath.base.symbolic import symbol except ImportError: _symbolics = False -import matplotlib.pyplot as plt class TestND(unittest.TestCase): @@ -58,7 +55,7 @@ def test_r2t(self): with self.assertRaises(ValueError): r2t(np.eye(3, 4)) - + _ = r2t(np.ones((3, 3)), check=False) with self.assertRaises(ValueError): r2t(np.ones((3, 3)), check=True) diff --git a/tests/base/test_vectors.py b/tests/base/test_vectors.py index 592c2d16..15c6a451 100755 --- a/tests/base/test_vectors.py +++ b/tests/base/test_vectors.py @@ -12,7 +12,6 @@ import unittest from math import pi import math -from scipy.linalg import logm, expm from spatialmath.base.vectors import * @@ -218,18 +217,10 @@ def test_unittwist_norm(self): self.assertIsNone(a[1]) def test_unittwist2(self): - nt.assert_array_almost_equal( - unittwist2([1, 0, 0]), np.r_[1, 0, 0] - ) - nt.assert_array_almost_equal( - unittwist2([0, 2, 0]), np.r_[0, 1, 0] - ) - nt.assert_array_almost_equal( - unittwist2([0, 0, -3]), np.r_[0, 0, -1] - ) - nt.assert_array_almost_equal( - unittwist2([2, 0, -2]), np.r_[1, 0, -1] - ) + nt.assert_array_almost_equal(unittwist2([1, 0, 0]), np.r_[1, 0, 0]) + nt.assert_array_almost_equal(unittwist2([0, 2, 0]), np.r_[0, 1, 0]) + nt.assert_array_almost_equal(unittwist2([0, 0, -3]), np.r_[0, 0, -1]) + nt.assert_array_almost_equal(unittwist2([2, 0, -2]), np.r_[1, 0, -1]) self.assertIsNone(unittwist2([0, 0, 0])) @@ -329,14 +320,30 @@ def test_wrap(self): theta = angle_factor * pi self.assertAlmostEqual(angle_wrap(theta), wrap_mpi_pi(theta)) self.assertAlmostEqual(angle_wrap(-theta), wrap_mpi_pi(-theta)) - self.assertAlmostEqual(angle_wrap(theta=theta, mode="-pi:pi"), wrap_mpi_pi(theta)) - self.assertAlmostEqual(angle_wrap(theta=-theta, mode="-pi:pi"), wrap_mpi_pi(-theta)) - self.assertAlmostEqual(angle_wrap(theta=theta, mode="0:2pi"), wrap_0_2pi(theta)) - self.assertAlmostEqual(angle_wrap(theta=-theta, mode="0:2pi"), wrap_0_2pi(-theta)) - self.assertAlmostEqual(angle_wrap(theta=theta, mode="0:pi"), wrap_0_pi(theta)) - self.assertAlmostEqual(angle_wrap(theta=-theta, mode="0:pi"), wrap_0_pi(-theta)) - self.assertAlmostEqual(angle_wrap(theta=theta, mode="-pi/2:pi/2"), wrap_mpi2_pi2(theta)) - self.assertAlmostEqual(angle_wrap(theta=-theta, mode="-pi/2:pi/2"), wrap_mpi2_pi2(-theta)) + self.assertAlmostEqual( + angle_wrap(theta=theta, mode="-pi:pi"), wrap_mpi_pi(theta) + ) + self.assertAlmostEqual( + angle_wrap(theta=-theta, mode="-pi:pi"), wrap_mpi_pi(-theta) + ) + self.assertAlmostEqual( + angle_wrap(theta=theta, mode="0:2pi"), wrap_0_2pi(theta) + ) + self.assertAlmostEqual( + angle_wrap(theta=-theta, mode="0:2pi"), wrap_0_2pi(-theta) + ) + self.assertAlmostEqual( + angle_wrap(theta=theta, mode="0:pi"), wrap_0_pi(theta) + ) + self.assertAlmostEqual( + angle_wrap(theta=-theta, mode="0:pi"), wrap_0_pi(-theta) + ) + self.assertAlmostEqual( + angle_wrap(theta=theta, mode="-pi/2:pi/2"), wrap_mpi2_pi2(theta) + ) + self.assertAlmostEqual( + angle_wrap(theta=-theta, mode="-pi/2:pi/2"), wrap_mpi2_pi2(-theta) + ) with self.assertRaises(ValueError): angle_wrap(theta=theta, mode="foo") diff --git a/tests/test_baseposelist.py b/tests/test_baseposelist.py index 30da5681..c3f9b311 100644 --- a/tests/test_baseposelist.py +++ b/tests/test_baseposelist.py @@ -2,28 +2,28 @@ import numpy as np from spatialmath.baseposelist import BasePoseList + # create a subclass to test with, its value is a scalar class X(BasePoseList): def __init__(self, value=0, check=False): super().__init__() self.data = [value] - + @staticmethod def _identity(): return 0 @property def shape(self): - return (1,1) + return (1, 1) @staticmethod def isvalid(x): return True -class TestBasePoseList(unittest.TestCase): +class TestBasePoseList(unittest.TestCase): def test_constructor(self): - x = X() self.assertIsInstance(x, X) self.assertEqual(len(x), 1) @@ -43,13 +43,13 @@ def test_setget(self): for i in range(0, 10): x[i] = X(2 * i) - for i,v in enumerate(x): + for i, v in enumerate(x): self.assertEqual(v.A, 2 * i) def test_append(self): x = X.Empty() for i in range(0, 10): - x.append(X(i+1)) + x.append(X(i + 1)) self.assertEqual(len(x), 10) self.assertEqual([xx.A for xx in x], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]) @@ -63,7 +63,7 @@ def test_extend(self): x.extend(y) self.assertEqual(len(x), 10) self.assertEqual([xx.A for xx in x], [1, 2, 3, 4, 5, 10, 11, 12, 13, 14]) - + def test_insert(self): x = X.Alloc(10) for i in range(0, 10): @@ -134,13 +134,13 @@ def test_unop(self): self.assertEqual(x.unop(f), [2, 4, 6, 8, 10]) y = x.unop(f, matrix=True) - self.assertEqual(y.shape, (5,1)) + self.assertEqual(y.shape, (5, 1)) self.assertTrue(np.all(y - np.c_[2, 4, 6, 8, 10].T == 0)) def test_arghandler(self): pass + # ---------------------------------------------------------------------------------------# -if __name__ == '__main__': - - unittest.main() \ No newline at end of file +if __name__ == "__main__": + unittest.main() diff --git a/tests/test_dualquaternion.py b/tests/test_dualquaternion.py index 39c5fc03..ed785313 100644 --- a/tests/test_dualquaternion.py +++ b/tests/test_dualquaternion.py @@ -1,4 +1,3 @@ -import math from math import pi import numpy as np @@ -6,7 +5,6 @@ import unittest from spatialmath import DualQuaternion, UnitDualQuaternion, Quaternion, SE3 -from spatialmath import base def qcompare(x, y): @@ -20,32 +18,29 @@ def qcompare(x, y): y = y.A nt.assert_array_almost_equal(x, y) -class TestDualQuaternion(unittest.TestCase): +class TestDualQuaternion(unittest.TestCase): def test_init(self): + dq = DualQuaternion(Quaternion([1.0, 2, 3, 4]), Quaternion([5.0, 6, 7, 8])) + nt.assert_array_almost_equal(dq.vec, np.r_[1, 2, 3, 4, 5, 6, 7, 8]) - dq = DualQuaternion(Quaternion([1.,2,3,4]), Quaternion([5.,6,7,8])) - nt.assert_array_almost_equal(dq.vec, np.r_[1,2,3,4,5,6,7,8]) - - dq = DualQuaternion([1.,2,3,4,5,6,7,8]) - nt.assert_array_almost_equal(dq.vec, np.r_[1,2,3,4,5,6,7,8]) - dq = DualQuaternion(np.r_[1,2,3,4,5,6,7,8]) - nt.assert_array_almost_equal(dq.vec, np.r_[1,2,3,4,5,6,7,8]) + dq = DualQuaternion([1.0, 2, 3, 4, 5, 6, 7, 8]) + nt.assert_array_almost_equal(dq.vec, np.r_[1, 2, 3, 4, 5, 6, 7, 8]) + dq = DualQuaternion(np.r_[1, 2, 3, 4, 5, 6, 7, 8]) + nt.assert_array_almost_equal(dq.vec, np.r_[1, 2, 3, 4, 5, 6, 7, 8]) def test_pure(self): - - dq = DualQuaternion.Pure([1.,2,3]) - nt.assert_array_almost_equal(dq.vec, np.r_[1,0,0,0, 0,1,2,3]) + dq = DualQuaternion.Pure([1.0, 2, 3]) + nt.assert_array_almost_equal(dq.vec, np.r_[1, 0, 0, 0, 0, 1, 2, 3]) def test_strings(self): - - dq = DualQuaternion(Quaternion([1.,2,3,4]), Quaternion([5.,6,7,8])) + dq = DualQuaternion(Quaternion([1.0, 2, 3, 4]), Quaternion([5.0, 6, 7, 8])) self.assertIsInstance(str(dq), str) self.assertIsInstance(repr(dq), str) def test_conj(self): - dq = DualQuaternion(Quaternion([1.,2,3,4]), Quaternion([5.,6,7,8])) - nt.assert_array_almost_equal(dq.conj().vec, np.r_[1,-2,-3,-4, 5,-6,-7,-8]) + dq = DualQuaternion(Quaternion([1.0, 2, 3, 4]), Quaternion([5.0, 6, 7, 8])) + nt.assert_array_almost_equal(dq.conj().vec, np.r_[1, -2, -3, -4, 5, -6, -7, -8]) # def test_norm(self): # q1 = Quaternion([1.,2,3,4]) @@ -55,26 +50,25 @@ def test_conj(self): # nt.assert_array_almost_equal(dq.norm(), (q1.norm(), q2.norm())) def test_plus(self): - dq = DualQuaternion(Quaternion([1.,2,3,4]), Quaternion([5.,6,7,8])) + dq = DualQuaternion(Quaternion([1.0, 2, 3, 4]), Quaternion([5.0, 6, 7, 8])) s = dq + dq - nt.assert_array_almost_equal(s.vec, 2*np.r_[1,2,3,4,5,6,7,8]) + nt.assert_array_almost_equal(s.vec, 2 * np.r_[1, 2, 3, 4, 5, 6, 7, 8]) def test_minus(self): - dq = DualQuaternion(Quaternion([1.,2,3,4]), Quaternion([5.,6,7,8])) + dq = DualQuaternion(Quaternion([1.0, 2, 3, 4]), Quaternion([5.0, 6, 7, 8])) s = dq - dq nt.assert_array_almost_equal(s.vec, np.zeros((8,))) def test_matrix(self): - - dq1 = DualQuaternion(Quaternion([1.,2,3,4]), Quaternion([5.,6,7,8])) + dq1 = DualQuaternion(Quaternion([1.0, 2, 3, 4]), Quaternion([5.0, 6, 7, 8])) M = dq1.matrix() self.assertIsInstance(M, np.ndarray) - self.assertEqual(M.shape, (8,8)) + self.assertEqual(M.shape, (8, 8)) def test_multiply(self): - dq1 = DualQuaternion(Quaternion([1.,2,3,4]), Quaternion([5.,6,7,8])) - dq2 = DualQuaternion(Quaternion([4,3,2,1]), Quaternion([5,6,7,8])) + dq1 = DualQuaternion(Quaternion([1.0, 2, 3, 4]), Quaternion([5.0, 6, 7, 8])) + dq2 = DualQuaternion(Quaternion([4, 3, 2, 1]), Quaternion([5, 6, 7, 8])) M = dq1.matrix() v = dq2.vec @@ -85,21 +79,19 @@ def test_unit(self): class TestUnitDualQuaternion(unittest.TestCase): - def test_init(self): - - T = SE3.Rx(pi/4) + T = SE3.Rx(pi / 4) dq = UnitDualQuaternion(T) nt.assert_array_almost_equal(dq.SE3().A, T.A) def test_norm(self): - T = SE3.Rx(pi/4) + T = SE3.Rx(pi / 4) dq = UnitDualQuaternion(T) - nt.assert_array_almost_equal(dq.norm(), (1,0)) + nt.assert_array_almost_equal(dq.norm(), (1, 0)) def test_multiply(self): - T1 = SE3.Rx(pi/4) - T2 = SE3.Rz(-pi/3) + T1 = SE3.Rx(pi / 4) + T2 = SE3.Rz(-pi / 3) T = T1 * T2 @@ -111,6 +103,5 @@ def test_multiply(self): # ---------------------------------------------------------------------------------------# -if __name__ == '__main__': # pragma: no cover - +if __name__ == "__main__": # pragma: no cover unittest.main() diff --git a/tests/test_pose3d.py b/tests/test_pose3d.py index 58396441..70b33ce0 100755 --- a/tests/test_pose3d.py +++ b/tests/test_pose3d.py @@ -242,8 +242,8 @@ def test_constructor_TwoVec(self): nt.assert_almost_equal(R.R[:, 0], v3 / np.linalg.norm(v3), 5) def test_conversion(self): - R = SO3.AngleAxis(0.7, [1,2,3]) - q = UnitQuaternion([11,7,3,-6]) + R = SO3.AngleAxis(0.7, [1, 2, 3]) + q = UnitQuaternion([11, 7, 3, -6]) R_from_q = SO3(q.R) q_from_R = UnitQuaternion(R) @@ -716,7 +716,6 @@ def test_functions_lie(self): nt.assert_equal(R, SO3.EulerVec(R.eulervec())) np.testing.assert_equal((R.inv() * R).eulervec(), np.zeros(3)) - def test_rotatedvector(self): v1 = [1, 2, 3] R = SO3.Eul(0.3, 0.4, 0.5) @@ -759,7 +758,6 @@ def test_mean(self): array_compare(m, SO3.RPY(0.1, 0.2, 0.3)) - # ============================== SE3 =====================================# @@ -872,7 +870,7 @@ def test_constructor(self): nt.assert_equal(len(R), 1) self.assertIsInstance(R, SE3) - # random + # random T = SE3.Rand() R = T.R t = T.t diff --git a/tests/test_quaternion.py b/tests/test_quaternion.py index 403b3c8d..75d31b7c 100644 --- a/tests/test_quaternion.py +++ b/tests/test_quaternion.py @@ -48,20 +48,18 @@ def test_constructor_variants(self): nt.assert_array_almost_equal( UnitQuaternion.Rz(-90, "deg").vec, np.r_[1, 0, 0, -1] / math.sqrt(2) ) - + np.random.seed(73) q = UnitQuaternion.Rand(theta_range=(0.1, 0.7)) self.assertIsInstance(q, UnitQuaternion) self.assertLessEqual(q.angvec()[0], 0.7) self.assertGreaterEqual(q.angvec()[0], 0.1) - q = UnitQuaternion.Rand(theta_range=(0.1, 0.7)) self.assertIsInstance(q, UnitQuaternion) self.assertLessEqual(q.angvec()[0], 0.7) self.assertGreaterEqual(q.angvec()[0], 0.1) - def test_constructor(self): qcompare(UnitQuaternion(), [1, 0, 0, 0]) @@ -83,8 +81,8 @@ def test_constructor(self): qcompare(UnitQuaternion(2, [0, 0, 0]), np.r_[1, 0, 0, 0]) qcompare(UnitQuaternion(-2, [0, 0, 0]), np.r_[1, 0, 0, 0]) - qcompare(UnitQuaternion([1, 2, 3, 4]), UnitQuaternion(v = [1, 2, 3, 4])) - qcompare(UnitQuaternion(s = 1, v = [2, 3, 4]), UnitQuaternion(v = [1, 2, 3, 4])) + qcompare(UnitQuaternion([1, 2, 3, 4]), UnitQuaternion(v=[1, 2, 3, 4])) + qcompare(UnitQuaternion(s=1, v=[2, 3, 4]), UnitQuaternion(v=[1, 2, 3, 4])) # from R @@ -824,8 +822,8 @@ def test_constructor(self): nt.assert_array_almost_equal(Quaternion(2, [0, 0, 0]).vec, [2, 0, 0, 0]) nt.assert_array_almost_equal(Quaternion(-2, [0, 0, 0]).vec, [-2, 0, 0, 0]) - qcompare(Quaternion([1, 2, 3, 4]), Quaternion(v = [1, 2, 3, 4])) - qcompare(Quaternion(s = 1, v = [2, 3, 4]), Quaternion(v = [1, 2, 3, 4])) + qcompare(Quaternion([1, 2, 3, 4]), Quaternion(v=[1, 2, 3, 4])) + qcompare(Quaternion(s=1, v=[2, 3, 4]), Quaternion(v=[1, 2, 3, 4])) # pure v = [5, 6, 7] diff --git a/tests/test_spline.py b/tests/test_spline.py index 361bc28f..9f27c608 100644 --- a/tests/test_spline.py +++ b/tests/test_spline.py @@ -27,7 +27,10 @@ def test_evaluation(self): def test_visualize(self): spline = BSplineSE3(self.control_poses) - spline.visualize(sample_times= np.linspace(0, 1.0, 100), animate=True, repeat=False) + spline.visualize( + sample_times=np.linspace(0, 1.0, 100), animate=True, repeat=False + ) + class TestInterpSplineSE3: waypoints = [ @@ -56,14 +59,20 @@ def test_evaluation(self): for time, pose in zip(norm_time, self.waypoints): nt.assert_almost_equal(spline(time).angdist(pose), 0.0) nt.assert_almost_equal(np.linalg.norm(spline(time).t - pose.t), 0.0) - + def test_small_delta_t(self): - InterpSplineSE3(np.linspace(0, InterpSplineSE3._e, len(self.waypoints)), self.waypoints) + InterpSplineSE3( + np.linspace(0, InterpSplineSE3._e, len(self.waypoints)), self.waypoints + ) def test_visualize(self): spline = InterpSplineSE3(self.times, self.waypoints) - spline.visualize(sample_times= np.linspace(0, self.time_horizon, 100), animate=True, repeat=False) - + spline.visualize( + sample_times=np.linspace(0, self.time_horizon, 100), + animate=True, + repeat=False, + ) + class TestSplineFit: num_data_points = 300 @@ -74,7 +83,11 @@ class TestSplineFit: timestamps = np.linspace(0, 1, num_data_points) trajectory = [ SE3.Rt( - t=[t * 0.4, 0.4 * np.sin(t * 2 * np.pi * 0.5), 0.4 * np.cos(t * 2 * np.pi * 0.5)], + t=[ + t * 0.4, + 0.4 * np.sin(t * 2 * np.pi * 0.5), + 0.4 * np.cos(t * 2 * np.pi * 0.5), + ], R=SO3.Rx(t * 2 * np.pi * 0.5), ) for t in timestamps * time_horizon @@ -85,11 +98,15 @@ def test_spline_fit(self): spline, kept_indices = fit.stochastic_downsample_interpolation() fraction_points_removed = 1.0 - len(kept_indices) / self.num_data_points - - assert(fraction_points_removed > 0.2) - assert(len(spline.control_poses)==len(kept_indices)) - assert(len(spline.timepoints)==len(kept_indices)) - - assert( fit.max_angular_error() < np.deg2rad(5.0) ) - assert( fit.max_angular_error() < 0.1 ) - spline.visualize(sample_times= np.linspace(0, self.time_horizon, 100), animate=True, repeat=False) \ No newline at end of file + + assert fraction_points_removed > 0.2 + assert len(spline.control_poses) == len(kept_indices) + assert len(spline.timepoints) == len(kept_indices) + + assert fit.max_angular_error() < np.deg2rad(5.0) + assert fit.max_angular_error() < 0.1 + spline.visualize( + sample_times=np.linspace(0, self.time_horizon, 100), + animate=True, + repeat=False, + ) diff --git a/tests/test_twist.py b/tests/test_twist.py index 12660c7d..70f237a8 100755 --- a/tests/test_twist.py +++ b/tests/test_twist.py @@ -1,5 +1,4 @@ import numpy.testing as nt -import matplotlib.pyplot as plt import unittest """ @@ -7,12 +6,14 @@ """ from math import pi from spatialmath.twist import * + # from spatialmath import super_pose # as sp from spatialmath.base import * from spatialmath.baseposematrix import BasePoseMatrix from spatialmath import SE2, SE3 from spatialmath.twist import BaseTwist + def array_compare(x, y): if isinstance(x, BasePoseMatrix): x = x.A @@ -26,9 +27,7 @@ def array_compare(x, y): class Twist3dTest(unittest.TestCase): - def test_constructor(self): - s = [1, 2, 3, 4, 5, 6] x = Twist3(s) self.assertIsInstance(x, Twist3) @@ -36,29 +35,28 @@ def test_constructor(self): array_compare(x.v, [1, 2, 3]) array_compare(x.w, [4, 5, 6]) array_compare(x.S, s) - - x = Twist3([1,2,3], [4,5,6]) + + x = Twist3([1, 2, 3], [4, 5, 6]) array_compare(x.v, [1, 2, 3]) array_compare(x.w, [4, 5, 6]) array_compare(x.S, s) y = Twist3(x) array_compare(x, y) - + x = Twist3(SE3()) - array_compare(x, [0,0,0,0,0,0]) - - + array_compare(x, [0, 0, 0, 0, 0, 0]) + def test_list(self): x = Twist3([1, 0, 0, 0, 0, 0]) y = Twist3([1, 0, 0, 0, 0, 0]) - + a = Twist3(x) a.append(y) self.assertEqual(len(a), 2) array_compare(a[0], x) array_compare(a[1], y) - + def test_conversion_SE3(self): T = SE3.Rx(0) tw = Twist3(T) @@ -68,134 +66,145 @@ def test_conversion_SE3(self): T = SE3.Rx(0) * SE3(1, 2, 3) array_compare(Twist3(T).SE3(), T) - + def test_conversion_se3(self): s = [1, 2, 3, 4, 5, 6] x = Twist3(s) - - array_compare(x.skewa(), np.array([[ 0., -6., 5., 1.], - [ 6., 0., -4., 2.], - [-5., 4., 0., 3.], - [ 0., 0., 0., 0.]])) - + + array_compare( + x.skewa(), + np.array( + [ + [0.0, -6.0, 5.0, 1.0], + [6.0, 0.0, -4.0, 2.0], + [-5.0, 4.0, 0.0, 3.0], + [0.0, 0.0, 0.0, 0.0], + ] + ), + ) + def test_conversion_Plucker(self): pass - + def test_list_constuctor(self): x = Twist3([1, 0, 0, 0, 0, 0]) - - a = Twist3([x,x,x,x]) + + a = Twist3([x, x, x, x]) self.assertIsInstance(a, Twist3) self.assertEqual(len(a), 4) - + a = Twist3([x.skewa(), x.skewa(), x.skewa(), x.skewa()]) self.assertIsInstance(a, Twist3) self.assertEqual(len(a), 4) - + a = Twist3([x.S, x.S, x.S, x.S]) self.assertIsInstance(a, Twist3) self.assertEqual(len(a), 4) - + s = np.r_[1, 2, 3, 4, 5, 6] a = Twist3([s, s, s, s]) self.assertIsInstance(a, Twist3) self.assertEqual(len(a), 4) - + def test_predicate(self): x = Twist3.UnitRevolute([1, 2, 3], [0, 0, 0]) self.assertFalse(x.isprismatic) - + # check prismatic twist x = Twist3.UnitPrismatic([1, 2, 3]) self.assertTrue(x.isprismatic) - + self.assertTrue(Twist3.isvalid(x.skewa())) self.assertTrue(Twist3.isvalid(x.S)) - + self.assertFalse(Twist3.isvalid(2)) self.assertFalse(Twist3.isvalid(np.eye(4))) - + def test_str(self): x = Twist3([1, 2, 3, 4, 5, 6]) s = str(x) self.assertIsInstance(s, str) self.assertEqual(len(s), 14) - self.assertEqual(s.count('\n'), 0) - + self.assertEqual(s.count("\n"), 0) + x.append(x) s = str(x) self.assertIsInstance(s, str) self.assertEqual(len(s), 29) - self.assertEqual(s.count('\n'), 1) - + self.assertEqual(s.count("\n"), 1) + def test_variant_constructors(self): - # check rotational twist x = Twist3.UnitRevolute([1, 2, 3], [0, 0, 0]) array_compare(x, np.r_[0, 0, 0, unitvec([1, 2, 3])]) - + # check prismatic twist x = Twist3.UnitPrismatic([1, 2, 3]) - array_compare(x, np.r_[unitvec([1, 2, 3]), 0, 0, 0, ]) - + array_compare( + x, + np.r_[ + unitvec([1, 2, 3]), + 0, + 0, + 0, + ], + ) + def test_SE3_twists(self): - tw = Twist3( SE3.Rx(0) ) - array_compare(tw, np.r_[0, 0, 0, 0, 0, 0]) - - tw = Twist3( SE3.Rx(pi / 2) ) - array_compare(tw, np.r_[0, 0, 0, pi / 2, 0, 0]) - - tw = Twist3( SE3.Ry(pi / 2) ) - array_compare(tw, np.r_[0, 0, 0, 0, pi / 2, 0]) - - tw = Twist3( SE3.Rz(pi / 2) ) - array_compare(tw, np.r_[0, 0, 0, 0, 0, pi / 2]) - - tw = Twist3( SE3([1, 2, 3]) ) - array_compare(tw, [1, 2, 3, 0, 0, 0]) - - tw = Twist3( SE3([1, 2, 3]) * SE3.Ry(pi / 2)) - array_compare(tw, np.r_[-pi / 2, 2, pi, 0, pi / 2, 0]) - + tw = Twist3(SE3.Rx(0)) + array_compare(tw, np.r_[0, 0, 0, 0, 0, 0]) + + tw = Twist3(SE3.Rx(pi / 2)) + array_compare(tw, np.r_[0, 0, 0, pi / 2, 0, 0]) + + tw = Twist3(SE3.Ry(pi / 2)) + array_compare(tw, np.r_[0, 0, 0, 0, pi / 2, 0]) + + tw = Twist3(SE3.Rz(pi / 2)) + array_compare(tw, np.r_[0, 0, 0, 0, 0, pi / 2]) + + tw = Twist3(SE3([1, 2, 3])) + array_compare(tw, [1, 2, 3, 0, 0, 0]) + + tw = Twist3(SE3([1, 2, 3]) * SE3.Ry(pi / 2)) + array_compare(tw, np.r_[-pi / 2, 2, pi, 0, pi / 2, 0]) + def test_exp(self): tw = Twist3.UnitRevolute([1, 0, 0], [0, 0, 0]) - array_compare(tw.exp(pi/2), SE3.Rx(pi/2)) - + array_compare(tw.exp(pi / 2), SE3.Rx(pi / 2)) + tw = Twist3.UnitRevolute([0, 1, 0], [0, 0, 0]) - array_compare(tw.exp(pi/2), SE3.Ry(pi/2)) - + array_compare(tw.exp(pi / 2), SE3.Ry(pi / 2)) + tw = Twist3.UnitRevolute([0, 0, 1], [0, 0, 0]) - array_compare(tw.exp(pi/2), SE3.Rz(pi / 2)) - + array_compare(tw.exp(pi / 2), SE3.Rz(pi / 2)) + def test_arith(self): - # check overloaded * T1 = SE3(1, 2, 3) * SE3.Rx(pi / 2) T2 = SE3(4, 5, -6) * SE3.Ry(-pi / 2) - + x1 = Twist3(T1) x2 = Twist3(T2) - array_compare( (x1 * x2).exp(), T1 * T2) - array_compare( (x2 * x1).exp(), T2 * T1) - + array_compare((x1 * x2).exp(), T1 * T2) + array_compare((x2 * x1).exp(), T2 * T1) + def test_prod(self): # check prod T1 = SE3(1, 2, 3) * SE3.Rx(pi / 2) T2 = SE3(4, 5, -6) * SE3.Ry(-pi / 2) - + x1 = Twist3(T1) x2 = Twist3(T2) - + x = Twist3([x1, x2]) - array_compare( x.prod().SE3(), T1 * T2) - + array_compare(x.prod().SE3(), T1 * T2) + class Twist2dTest(unittest.TestCase): - def test_constructor(self): - s = [1, 2, 3] x = Twist2(s) self.assertIsInstance(x, Twist2) @@ -203,33 +212,32 @@ def test_constructor(self): array_compare(x.v, [1, 2]) array_compare(x.w, [3]) array_compare(x.S, s) - - x = Twist2([1,2], 3) + + x = Twist2([1, 2], 3) array_compare(x.v, [1, 2]) array_compare(x.w, [3]) array_compare(x.S, s) y = Twist2(x) array_compare(x, y) - + # construct from SE2 x = Twist2(SE2()) - array_compare(x, [0,0,0]) - - x = Twist2( SE2(0, 0, pi / 2)) + array_compare(x, [0, 0, 0]) + + x = Twist2(SE2(0, 0, pi / 2)) array_compare(x, np.r_[0, 0, pi / 2]) - - x = Twist2( SE2(1, 2,0 )) + + x = Twist2(SE2(1, 2, 0)) array_compare(x, np.r_[1, 2, 0]) - - x = Twist2( SE2(1, 2, pi / 2)) + + x = Twist2(SE2(1, 2, pi / 2)) array_compare(x, np.r_[3 * pi / 4, pi / 4, pi / 2]) - - + def test_list(self): x = Twist2([1, 0, 0]) y = Twist2([1, 0, 0]) - + a = Twist2(x) a.append(y) self.assertEqual(len(a), 2) @@ -237,132 +245,126 @@ def test_list(self): array_compare(a[1], y) def test_variant_constructors(self): - # check rotational twist x = Twist2.UnitRevolute([1, 2]) array_compare(x, np.r_[2, -1, 1]) - + # check prismatic twist x = Twist2.UnitPrismatic([1, 2]) array_compare(x, np.r_[unitvec([1, 2]), 0]) - + def test_conversion_SE2(self): T = SE2(1, 2, 0.3) tw = Twist2(T) array_compare(tw.SE2(), T) self.assertIsInstance(tw.SE2(), SE2) self.assertEqual(len(tw.SE2()), 1) - + def test_conversion_se2(self): s = [1, 2, 3] x = Twist2(s) - - array_compare(x.skewa(), np.array([[ 0., -3., 1.], - [ 3., 0., 2.], - [ 0., 0., 0.]])) + + array_compare( + x.skewa(), np.array([[0.0, -3.0, 1.0], [3.0, 0.0, 2.0], [0.0, 0.0, 0.0]]) + ) def test_list_constuctor(self): x = Twist2([1, 0, 0]) - - a = Twist2([x,x,x,x]) + + a = Twist2([x, x, x, x]) self.assertIsInstance(a, Twist2) self.assertEqual(len(a), 4) - + a = Twist2([x.skewa(), x.skewa(), x.skewa(), x.skewa()]) self.assertIsInstance(a, Twist2) self.assertEqual(len(a), 4) - + a = Twist2([x.S, x.S, x.S, x.S]) self.assertIsInstance(a, Twist2) self.assertEqual(len(a), 4) - + s = np.r_[1, 2, 3] a = Twist2([s, s, s, s]) self.assertIsInstance(a, Twist2) self.assertEqual(len(a), 4) - + def test_predicate(self): x = Twist2.UnitRevolute([1, 2]) self.assertFalse(x.isprismatic) - + # check prismatic twist x = Twist2.UnitPrismatic([1, 2]) self.assertTrue(x.isprismatic) - + self.assertTrue(Twist2.isvalid(x.skewa())) self.assertTrue(Twist2.isvalid(x.S)) - + self.assertFalse(Twist2.isvalid(2)) self.assertFalse(Twist2.isvalid(np.eye(3))) - + def test_str(self): x = Twist2([1, 2, 3]) s = str(x) self.assertIsInstance(s, str) self.assertEqual(len(s), 8) - self.assertEqual(s.count('\n'), 0) - + self.assertEqual(s.count("\n"), 0) + x.append(x) s = str(x) self.assertIsInstance(s, str) self.assertEqual(len(s), 17) - self.assertEqual(s.count('\n'), 1) - + self.assertEqual(s.count("\n"), 1) def test_SE2_twists(self): - tw = Twist2( SE2() ) + tw = Twist2(SE2()) array_compare(tw, np.r_[0, 0, 0]) - - tw = Twist2( SE2(0, 0, pi / 2) ) + + tw = Twist2(SE2(0, 0, pi / 2)) array_compare(tw, np.r_[0, 0, pi / 2]) - - - tw = Twist2( SE2([1, 2, 0]) ) + + tw = Twist2(SE2([1, 2, 0])) array_compare(tw, [1, 2, 0]) - - tw = Twist2( SE2([1, 2, pi / 2])) - array_compare(tw, np.r_[ 3 * pi / 4, pi / 4, pi / 2]) - + + tw = Twist2(SE2([1, 2, pi / 2])) + array_compare(tw, np.r_[3 * pi / 4, pi / 4, pi / 2]) + def test_exp(self): x = Twist2.UnitRevolute([0, 0]) - array_compare(x.exp(pi/2), SE2(0, 0, pi/2)) - + array_compare(x.exp(pi / 2), SE2(0, 0, pi / 2)) + x = Twist2.UnitRevolute([1, 0]) - array_compare(x.exp(pi/2), SE2(1, -1, pi/2)) - + array_compare(x.exp(pi / 2), SE2(1, -1, pi / 2)) + x = Twist2.UnitRevolute([1, 2]) - array_compare(x.exp(pi/2), SE2(3, 1, pi/2)) + array_compare(x.exp(pi / 2), SE2(3, 1, pi / 2)) - def test_arith(self): - # check overloaded * T1 = SE2(1, 2, pi / 2) T2 = SE2(4, 5, -pi / 4) - + x1 = Twist2(T1) x2 = Twist2(T2) - array_compare( (x1 * x2).exp(), (T1 * T2).A) - array_compare( (x2 * x1).exp(), (T2 * T1).A) + array_compare((x1 * x2).exp(), (T1 * T2).A) + array_compare((x2 * x1).exp(), (T2 * T1).A) + + array_compare((x1 * x2).SE2(), (T1 * T2).A) + array_compare((x2 * x1).SE2(), (T2 * T1)) - array_compare( (x1 * x2).SE2(), (T1 * T2).A) - array_compare( (x2 * x1).SE2(), (T2 * T1)) - def test_prod(self): # check prod T1 = SE2(1, 2, pi / 2) T2 = SE2(4, 5, -pi / 4) - + x1 = Twist2(T1) x2 = Twist2(T2) - - x = Twist2([x1, x2]) - array_compare( x.prod().SE2(), T1 * T2) -# ---------------------------------------------------------------------------------------# -if __name__ == '__main__': + x = Twist2([x1, x2]) + array_compare(x.prod().SE2(), T1 * T2) +# ---------------------------------------------------------------------------------------# +if __name__ == "__main__": unittest.main()