From 3b901865937c322a742f38eb00068eba3545db75 Mon Sep 17 00:00:00 2001 From: Hauke Strasdat Date: Wed, 12 Jun 2024 02:28:32 -0700 Subject: [PATCH] a number of lint/docs/tool tweaks --- .pre-commit-config.yaml | 19 ++- CMakeLists.txt | 4 +- examples/CMakeLists.txt | 3 +- package.xml | 22 --- setup.py | 26 ++- sophus_pybind-stubs/sophus_pybind.pyi | 109 ++++++------ sympy/sophus/complex.py | 59 +++---- sympy/sophus/dual_quaternion.py | 62 ++++--- sympy/sophus/matrix.py | 8 +- sympy/sophus/quaternion.py | 76 ++++----- sympy/sophus/se2.py | 146 ++++++++-------- sympy/sophus/se3.py | 196 +++++++++++++--------- sympy/sophus/so2.py | 113 ++++++------- sympy/sophus/so3.py | 229 +++++++++++++++----------- test/ceres/tests.hpp | 2 +- 15 files changed, 565 insertions(+), 509 deletions(-) delete mode 100644 package.xml diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index fb2cd2fca..dc1da5f3f 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -4,7 +4,7 @@ repos: hooks: - id: trailing-whitespace - id: end-of-file-fixer - exclude: (thirdparty/.*)|(.txt)$ + exclude: (thirdparty/.*)|(SOPHUS_VERSION)|(.txt)$ - id: check-yaml args: ["--unsafe"] - id: check-json @@ -26,3 +26,20 @@ repos: - id: cmake-format # lint does not pass #- id: cmake-lint + - repo: https://github.com/astral-sh/ruff-pre-commit + # Ruff version. + rev: v0.4.8 + hooks: + # Run the linter. + - id: ruff + args: [ --fix ] + exclude: (sophus_pybind-stubs/.*) + # Run the formatter. + - id: ruff-format + - repo: https://github.com/codespell-project/codespell + rev: v2.1.0 + hooks: + - id: codespell + args: + - --ignore-words-list + - "te,tring,crate" diff --git a/CMakeLists.txt b/CMakeLists.txt index 02b4f381a..c4f318527 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,7 @@ cmake_minimum_required(VERSION 3.24) -project(Sophus VERSION 1.24.6) + +file(READ "SOPHUS_VERSION" SOPHUS_VERSION) +project(Sophus VERSION ${SOPHUS_VERSION}) include(CMakePackageConfigHelpers) include(GNUInstallDirs) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index eff08758a..a48f4f96a 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 3.24) project(SophusExample) -find_package(Sophus 1.24.6 REQUIRED) +file(READ "../SOPHUS_VERSION" SOPHUS_VERSION) +find_package(Sophus ${SOPHUS_VERSION} REQUIRED) set(CMAKE_CXX_STANDARD 17) # Release by default Turn on Debug with "-DCMAKE_BUILD_TYPE=Debug" diff --git a/package.xml b/package.xml deleted file mode 100644 index 903cd4870..000000000 --- a/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - sophus - 1.24.6 - - C++ implementation of Lie Groups using Eigen. - - https://github.com/strasdat/sophus - https://github.com/strasdat/sophus/issues - Daniel Stonier - Hauke Strasdat - MIT - - cmake - - eigen - eigen - - - cmake - - diff --git a/setup.py b/setup.py index 43a57ff1a..943884112 100644 --- a/setup.py +++ b/setup.py @@ -1,6 +1,6 @@ import os import re -import shutil +from pathlib import Path import subprocess import sys @@ -17,6 +17,7 @@ "win-arm64": "ARM64", } + # A CMakeExtension needs a sourcedir instead of a file list. # The name must be the _single_ output extension from the CMake build. # If you need multiple extensions, see scikit-build. @@ -69,7 +70,6 @@ def build_extension(self, ext): pass else: - # Single config generators are handled "normally" single_config = any(x in cmake_generator for x in {"NMake", "Ninja"}) @@ -108,22 +108,32 @@ def build_extension(self, ext): os.makedirs(self.build_temp) if not os.path.exists(self.build_lib): - os.makedirs(self.build_lib) + os.makedirs(self.build_lib) - subprocess.check_call(["cmake", ext.sourcedir] + cmake_args, cwd=self.build_temp) + subprocess.check_call( + ["cmake", ext.sourcedir] + cmake_args, cwd=self.build_temp + ) subprocess.check_call( ["cmake", "--build", "."] + build_args, cwd=self.build_temp ) # copy stubs files from sophus_pybind-stubs to lib folder to be installed subprocess.run( - f"cp sophus_pybind-stubs/*.pyi {self.build_lib}", shell=True, check=True, + f"cp sophus_pybind-stubs/*.pyi {self.build_lib}", + shell=True, + check=True, ) subprocess.run( - f"cp sophus_pybind-stubs/*.typed {self.build_lib}", shell=True, check=True, + f"cp sophus_pybind-stubs/*.typed {self.build_lib}", + shell=True, + check=True, ) # copy .so file to lib - subprocess.run(f"cp {self.build_temp}/*.so {self.build_lib}/", shell=True, check=True,) + subprocess.run( + f"cp {self.build_temp}/*.so {self.build_lib}/", + shell=True, + check=True, + ) def main(): @@ -131,7 +141,7 @@ def main(): # logic and declaration, and simpler if you include description/version in a file. setup( name="sophus_pybind", - version="1.24.6", + version=Path("SOPHUS_VERSION").read_text(), description="Sophus python API", long_description="Python API for sophus library", url="https://github.com/strasdat/sophus", diff --git a/sophus_pybind-stubs/sophus_pybind.pyi b/sophus_pybind-stubs/sophus_pybind.pyi index 9fbe96572..f9606e01a 100644 --- a/sophus_pybind-stubs/sophus_pybind.pyi +++ b/sophus_pybind-stubs/sophus_pybind.pyi @@ -1,73 +1,78 @@ from __future__ import annotations import numpy import typing -__all__ = ['SE3', 'SO3', 'interpolate', 'iterativeMean'] + +__all__ = ["SE3", "SO3", "interpolate", "iterativeMean"] + class SE3: @staticmethod @typing.overload - def exp(arg0: numpy.ndarray[numpy.float64[3, 1]], arg1: numpy.ndarray[numpy.float64[3, 1]]) -> SE3: + def exp( + arg0: numpy.ndarray[numpy.float64[3, 1]], + arg1: numpy.ndarray[numpy.float64[3, 1]], + ) -> SE3: """ Create SE3 from a translational_part (3x1) and a rotation vector (3x1) of magnitude in rad. NOTE: translational_part is not translation vector in SE3 """ @staticmethod @typing.overload - def exp(arg0: numpy.ndarray[numpy.float64[m, 3]], arg1: numpy.ndarray[numpy.float64[m, 3]]) -> SE3: + def exp( + arg0: numpy.ndarray[numpy.float64[m, 3]], + arg1: numpy.ndarray[numpy.float64[m, 3]], + ) -> SE3: """ Create a set of SE3 from translational_parts (Nx3) and rotation vectors (Nx3) of magnitude in rad. NOTE: translational_part is not translation vector in SE3 """ @staticmethod @typing.overload - def from_matrix(arg0: numpy.ndarray[numpy.float64[4, 4]]) -> SE3: - ... + def from_matrix(arg0: numpy.ndarray[numpy.float64[4, 4]]) -> SE3: ... @staticmethod @typing.overload - def from_matrix(arg0: numpy.ndarray[numpy.float64]) -> SE3: - ... + def from_matrix(arg0: numpy.ndarray[numpy.float64]) -> SE3: ... @staticmethod @typing.overload - def from_matrix3x4(arg0: numpy.ndarray[numpy.float64[3, 4]]) -> SE3: - ... + def from_matrix3x4(arg0: numpy.ndarray[numpy.float64[3, 4]]) -> SE3: ... @staticmethod @typing.overload - def from_matrix3x4(arg0: numpy.ndarray[numpy.float64]) -> SE3: - ... - def __copy__(self) -> SE3: - ... - def __getitem__(self, arg0: typing.Any) -> SE3: - ... - def __imatmul__(self, arg0: SE3) -> SE3: - ... + def from_matrix3x4(arg0: numpy.ndarray[numpy.float64]) -> SE3: ... + def __copy__(self) -> SE3: ... + def __getitem__(self, arg0: typing.Any) -> SE3: ... + def __imatmul__(self, arg0: SE3) -> SE3: ... @typing.overload def __init__(self) -> None: """ - Default Constructor initializing a group containing 1 identity element + Default Constructor initializing a group containing 1 identity element """ @typing.overload def __init__(self, arg0: SE3) -> None: """ Copy constructor from single element """ - def __len__(self) -> int: - ... + def __len__(self) -> int: ... @typing.overload - def __matmul__(self, arg0: SE3) -> SE3: - ... + def __matmul__(self, arg0: SE3) -> SE3: ... @typing.overload - def __matmul__(self, arg0: numpy.ndarray[numpy.float64[3, n]]) -> numpy.ndarray[numpy.float64[3, n]]: - ... - def __repr__(self) -> str: - ... - def __setitem__(self, arg0: typing.Any, arg1: SE3) -> None: - ... - def __str__(self) -> str: - ... + def __matmul__( + self, arg0: numpy.ndarray[numpy.float64[3, n]] + ) -> numpy.ndarray[numpy.float64[3, n]]: ... + def __repr__(self) -> str: ... + def __setitem__(self, arg0: typing.Any, arg1: SE3) -> None: ... + def __str__(self) -> str: ... @typing.overload - def from_quat_and_translation(self, arg0: numpy.ndarray[numpy.float64[3, 1]], arg1: numpy.ndarray[numpy.float64[3, 1]]) -> SE3: + def from_quat_and_translation( + self, + arg0: numpy.ndarray[numpy.float64[3, 1]], + arg1: numpy.ndarray[numpy.float64[3, 1]], + ) -> SE3: """ Create SE3 from a quaternion as w, [x, y, z], and translation vector """ @typing.overload - def from_quat_and_translation(self, arg0: numpy.ndarray[numpy.float64[m, 3]], arg1: numpy.ndarray[numpy.float64[m, 3]]) -> SE3: + def from_quat_and_translation( + self, + arg0: numpy.ndarray[numpy.float64[m, 3]], + arg1: numpy.ndarray[numpy.float64[m, 3]], + ) -> SE3: """ Create SE3 from a list of quaternion as w_vec: Nx1, xyz_vec: Nx3, and a list of translation vectors: Nx3 """ @@ -99,6 +104,7 @@ class SE3: """ Get the translation component of the transformation. """ + class SO3: @staticmethod def exp(arg0: numpy.ndarray[numpy.float64[m, 3]]) -> SO3: @@ -107,12 +113,10 @@ class SO3: """ @staticmethod @typing.overload - def from_matrix(arg0: numpy.ndarray[numpy.float64[3, 3]]) -> SO3: - ... + def from_matrix(arg0: numpy.ndarray[numpy.float64[3, 3]]) -> SO3: ... @staticmethod @typing.overload - def from_matrix(arg0: numpy.ndarray[numpy.float64]) -> SO3: - ... + def from_matrix(arg0: numpy.ndarray[numpy.float64]) -> SO3: ... @staticmethod @typing.overload def from_quat(arg0: float, arg1: numpy.ndarray[numpy.float64[3, 1]]) -> SO3: @@ -125,36 +129,29 @@ class SO3: """ Create rotations from a list of quaternions as w_vec: Nx1, xyz_vec: Nx3 """ - def __copy__(self) -> SO3: - ... - def __getitem__(self, arg0: typing.Any) -> SO3: - ... - def __imatmul__(self, arg0: SO3) -> SO3: - ... + def __copy__(self) -> SO3: ... + def __getitem__(self, arg0: typing.Any) -> SO3: ... + def __imatmul__(self, arg0: SO3) -> SO3: ... @typing.overload def __init__(self) -> None: """ - Default Constructor initializing a group containing 1 identity element + Default Constructor initializing a group containing 1 identity element """ @typing.overload def __init__(self, arg0: SO3) -> None: """ Copy constructor from single element """ - def __len__(self) -> int: - ... + def __len__(self) -> int: ... @typing.overload - def __matmul__(self, arg0: SO3) -> SO3: - ... + def __matmul__(self, arg0: SO3) -> SO3: ... @typing.overload - def __matmul__(self, arg0: numpy.ndarray[numpy.float64[3, n]]) -> numpy.ndarray[numpy.float64[3, n]]: - ... - def __repr__(self) -> str: - ... - def __setitem__(self, arg0: typing.Any, arg1: SO3) -> None: - ... - def __str__(self) -> str: - ... + def __matmul__( + self, arg0: numpy.ndarray[numpy.float64[3, n]] + ) -> numpy.ndarray[numpy.float64[3, n]]: ... + def __repr__(self) -> str: ... + def __setitem__(self, arg0: typing.Any, arg1: SO3) -> None: ... + def __str__(self) -> str: ... def inverse(self) -> SO3: """ Compute the inverse of the rotations. @@ -171,10 +168,12 @@ class SO3: """ Return quaternion as Nx4 vectors with the order [w x y z]. """ + def interpolate(arg0: SE3, arg1: SE3, arg2: float) -> SE3: """ Interpolate two SE3s of size 1. """ + def iterativeMean(arg0: SE3) -> SE3: """ Compute the iterative mean of a sequence. diff --git a/sympy/sophus/complex.py b/sympy/sophus/complex.py index 6197a4b32..cf747cc69 100644 --- a/sympy/sophus/complex.py +++ b/sympy/sophus/complex.py @@ -4,16 +4,18 @@ class Complex: - """ Complex class """ + """Complex class""" def __init__(self, real, imag): self.real = real self.imag = imag def __mul__(self, right): - """ complex multiplication """ - return Complex(self.real * right.real - self.imag * right.imag, - self.imag * right.real + self.real * right.imag) + """complex multiplication""" + return Complex( + self.real * right.real - self.imag * right.imag, + self.imag * right.real + self.real * right.imag, + ) def __add__(self, right): return Complex(self.real + right.real, self.imag + right.imag) @@ -22,29 +24,29 @@ def __neg__(self): return Complex(-self.real, -self.image) def __truediv__(self, scalar): - """ scalar division """ + """scalar division""" return Complex(self.real / scalar, self.imag / scalar) def __repr__(self): return "( " + repr(self.real) + " + " + repr(self.imag) + "i )" def __getitem__(self, key): - """ We use the following convention [real, imag] """ + """We use the following convention [real, imag]""" if key == 0: return self.real else: return self.imag def squared_norm(self): - """ squared norm when considering the complex number as tuple """ + """squared norm when considering the complex number as tuple""" return self.real**2 + self.imag**2 def conj(self): - """ complex conjugate """ + """complex conjugate""" return Complex(self.real, -self.imag) def inv(self): - """ complex inverse """ + """complex inverse""" return self.conj() / self.squared_norm() @staticmethod @@ -64,48 +66,39 @@ def subs(self, x, y): return Complex(self.real.subs(x, y), self.imag.subs(x, y)) def simplify(self): - return Complex(sympy.simplify(self.real), - sympy.simplify(self.imag)) + return Complex(sympy.simplify(self.real), sympy.simplify(self.imag)) @staticmethod def Da_a_mul_b(a, b): - """ derivatice of complex muliplication wrt left multiplier a """ - return sympy.Matrix([[b.real, -b.imag], - [b.imag, b.real]]) + """derivatice of complex muliplication wrt left multiplier a""" + return sympy.Matrix([[b.real, -b.imag], [b.imag, b.real]]) @staticmethod def Db_a_mul_b(a, b): - """ derivatice of complex muliplication wrt right multiplicand b """ - return sympy.Matrix([[a.real, -a.imag], - [a.imag, a.real]]) + """derivatice of complex muliplication wrt right multiplicand b""" + return sympy.Matrix([[a.real, -a.imag], [a.imag, a.real]]) class TestComplex(unittest.TestCase): def setUp(self): - x, y = sympy.symbols('x y', real=True) - u, v = sympy.symbols('u v', real=True) + x, y = sympy.symbols("x y", real=True) + u, v = sympy.symbols("u v", real=True) self.a = Complex(x, y) self.b = Complex(u, v) def test_multiplications(self): product = self.a * self.a.inv() - self.assertEqual(product.simplify(), - Complex.identity()) + self.assertEqual(product.simplify(), Complex.identity()) product = self.a.inv() * self.a - self.assertEqual(product.simplify(), - Complex.identity()) + self.assertEqual(product.simplify(), Complex.identity()) def test_derivatives(self): - d = sympy.Matrix(2, 2, lambda r, c: sympy.diff( - (self.a * self.b)[r], self.a[c])) - self.assertEqual(d, - Complex.Da_a_mul_b(self.a, self.b)) - d = sympy.Matrix(2, 2, lambda r, c: sympy.diff( - (self.a * self.b)[r], self.b[c])) - self.assertEqual(d, - Complex.Db_a_mul_b(self.a, self.b)) + d = sympy.Matrix(2, 2, lambda r, c: sympy.diff((self.a * self.b)[r], self.a[c])) + self.assertEqual(d, Complex.Da_a_mul_b(self.a, self.b)) + d = sympy.Matrix(2, 2, lambda r, c: sympy.diff((self.a * self.b)[r], self.b[c])) + self.assertEqual(d, Complex.Db_a_mul_b(self.a, self.b)) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() - print('hello') + print("hello") diff --git a/sympy/sophus/dual_quaternion.py b/sympy/sophus/dual_quaternion.py index 6c3ca1c65..c39b71d3c 100644 --- a/sympy/sophus/dual_quaternion.py +++ b/sympy/sophus/dual_quaternion.py @@ -7,57 +7,55 @@ class DualQuaternion: - """ Dual quaternion class """ + """Dual quaternion class""" def __init__(self, real_q, inf_q): - """ Dual quaternion consists of a real quaternion, and an infinitesimal - quaternion """ + """Dual quaternion consists of a real quaternion, and an infinitesimal + quaternion""" self.real_q = real_q self.inf_q = inf_q def __mul__(self, right): - """ dual quaternion multiplication """ - return DualQuaternion(self.real_q * right.real_q, - self.real_q * right.inf_q + - self.inf_q * right.real_q) + """dual quaternion multiplication""" + return DualQuaternion( + self.real_q * right.real_q, + self.real_q * right.inf_q + self.inf_q * right.real_q, + ) def __truediv__(self, scalar): - """ scalar division """ + """scalar division""" return DualQuaternion(self.real_q / scalar, self.inf_q / scalar) def __repr__(self): - return "( " + repr(self.real_q) + \ - " + " + repr(self.inf_q) + ")" + return "( " + repr(self.real_q) + " + " + repr(self.inf_q) + ")" def __getitem__(self, key): - assert (key >= 0 and key < 8) + assert key >= 0 and key < 8 if key < 4: return self.real_q[key] else: return self.inf_q[key - 4] def squared_norm(self): - """ squared norm when considering the dual quaternion as 8-tuple """ + """squared norm when considering the dual quaternion as 8-tuple""" return self.real_q.squared_norm() + self.inf_q.squared_norm() def conj(self): - """ dual quaternion conjugate """ + """dual quaternion conjugate""" return DualQuaternion(self.real_q.conj(), self.inf_q.conj()) def inv(self): - """ dual quaternion inverse """ - return DualQuaternion(self.real_q.inv(), - -self.real_q.inv() * self.inf_q * - self.real_q.inv()) + """dual quaternion inverse""" + return DualQuaternion( + self.real_q.inv(), -self.real_q.inv() * self.inf_q * self.real_q.inv() + ) def simplify(self): - return DualQuaternion(self.real_q.simplify(), - self.inf_q.simplify()) + return DualQuaternion(self.real_q.simplify(), self.inf_q.simplify()) @staticmethod def identity(): - return DualQuaternion(Quaternion.identity(), - Quaternion.zero()) + return DualQuaternion(Quaternion.identity(), Quaternion.zero()) def __eq__(self, other): if isinstance(self, other.__class__): @@ -67,28 +65,24 @@ def __eq__(self, other): class TestDualQuaternion(unittest.TestCase): def setUp(self): - w, s0, s1, s2 = sympy.symbols('w s0 s1 s2', real=True) - x, t0, t1, t2 = sympy.symbols('x t0 t1 t2', real=True) - y, u0, u1, u2 = sympy.symbols('y u0 u1 u2', real=True) - z, v0, v1, v2 = sympy.symbols('z v0 v1 v2', real=True) + w, s0, s1, s2 = sympy.symbols("w s0 s1 s2", real=True) + x, t0, t1, t2 = sympy.symbols("x t0 t1 t2", real=True) + y, u0, u1, u2 = sympy.symbols("y u0 u1 u2", real=True) + z, v0, v1, v2 = sympy.symbols("z v0 v1 v2", real=True) s = Vector3(s0, s1, s2) t = Vector3(t0, t1, t2) u = Vector3(u0, u1, u2) v = Vector3(v0, v1, v2) - self.a = DualQuaternion(Quaternion(w, s), - Quaternion(x, t)) - self.b = DualQuaternion(Quaternion(y, u), - Quaternion(z, v)) + self.a = DualQuaternion(Quaternion(w, s), Quaternion(x, t)) + self.b = DualQuaternion(Quaternion(y, u), Quaternion(z, v)) def test_multiplications(self): product = self.a * self.a.inv() - self.assertEqual(product.simplify(), - DualQuaternion.identity()) + self.assertEqual(product.simplify(), DualQuaternion.identity()) product = self.a.inv() * self.a - self.assertEqual(product.simplify(), - DualQuaternion.identity()) + self.assertEqual(product.simplify(), DualQuaternion.identity()) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/sympy/sophus/matrix.py b/sympy/sophus/matrix.py index 431fa5b1b..5270f9904 100644 --- a/sympy/sophus/matrix.py +++ b/sympy/sophus/matrix.py @@ -6,8 +6,8 @@ def dot(left, right): - assert(isinstance(left, sympy.Matrix)) - assert(isinstance(right, sympy.Matrix)) + assert isinstance(left, sympy.Matrix) + assert isinstance(right, sympy.Matrix) sum = 0 for c in range(0, left.cols): @@ -17,7 +17,7 @@ def dot(left, right): def squared_norm(m): - assert(isinstance(m, sympy.Matrix)) + assert isinstance(m, sympy.Matrix) return dot(m, m) @@ -66,7 +66,7 @@ def proj(v): assert m > 1 assert n == 1 list = [v[i] / v[m - 1] for i in range(0, m - 1)] - r = sympy.Matrix(m - 1, 1, list) + r = sympy.Matrix(m - 1, 1, list) return r diff --git a/sympy/sophus/quaternion.py b/sympy/sophus/quaternion.py index a896fbda9..5286b4ae1 100644 --- a/sympy/sophus/quaternion.py +++ b/sympy/sophus/quaternion.py @@ -1,4 +1,4 @@ -""" run with: python3 -m sophus.quaternion """ +"""run with: python3 -m sophus.quaternion""" import unittest @@ -9,53 +9,54 @@ class Quaternion: - """ Quaternion class """ + """Quaternion class""" def __init__(self, real, vec): - """ Quaternion consists of a real scalar, and an imaginary 3-vector """ + """Quaternion consists of a real scalar, and an imaginary 3-vector""" assert isinstance(vec, sympy.Matrix) assert vec.shape == (3, 1), vec.shape self.real = real self.vec = vec def __mul__(self, right): - """ quaternion multiplication """ - return Quaternion(self[3] * right[3] - self.vec.dot(right.vec), - self[3] * right.vec + right[3] * self.vec + - self.vec.cross(right.vec)) + """quaternion multiplication""" + return Quaternion( + self[3] * right[3] - self.vec.dot(right.vec), + self[3] * right.vec + right[3] * self.vec + self.vec.cross(right.vec), + ) def __add__(self, right): - """ quaternion multiplication """ + """quaternion multiplication""" return Quaternion(self[3] + right[3], self.vec + right.vec) def __neg__(self): return Quaternion(-self[3], -self.vec) def __truediv__(self, scalar): - """ scalar division """ + """scalar division""" return Quaternion(self.real / scalar, self.vec / scalar) def __repr__(self): return "( " + repr(self[3]) + " + " + repr(self.vec) + "i )" def __getitem__(self, key): - """ We use the following convention [vec0, vec1, vec2, real] """ - assert (key >= 0 and key < 4) + """We use the following convention [vec0, vec1, vec2, real]""" + assert key >= 0 and key < 4 if key == 3: return self.real else: return self.vec[key] def squared_norm(self): - """ squared norm when considering the quaternion as 4-tuple """ + """squared norm when considering the quaternion as 4-tuple""" return squared_norm(self.vec) + self.real**2 def conj(self): - """ quaternion conjugate """ + """quaternion conjugate""" return Quaternion(self.real, -self.vec) def inv(self): - """ quaternion inverse """ + """quaternion inverse""" return self.conj() / self.squared_norm() @staticmethod @@ -71,8 +72,7 @@ def subs(self, x, y): def simplify(self): v = sympy.simplify(self.vec) - return Quaternion(sympy.simplify(self.real), - Vector3(v[0], v[1], v[2])) + return Quaternion(sympy.simplify(self.real), Vector3(v[0], v[1], v[2])) def __eq__(self, other): if isinstance(self, other.__class__): @@ -81,33 +81,31 @@ def __eq__(self, other): @staticmethod def Da_a_mul_b(a, b): - """ derivatice of quaternion muliplication wrt left multiplier a """ + """derivatice of quaternion muliplication wrt left multiplier a""" v0 = b.vec[0] v1 = b.vec[1] v2 = b.vec[2] y = b.real - return sympy.Matrix([[y, v2, -v1, v0], - [-v2, y, v0, v1], - [v1, -v0, y, v2], - [-v0, -v1, -v2, y]]) + return sympy.Matrix( + [[y, v2, -v1, v0], [-v2, y, v0, v1], [v1, -v0, y, v2], [-v0, -v1, -v2, y]] + ) @staticmethod def Db_a_mul_b(a, b): - """ derivatice of quaternion muliplication wrt right multiplicand b """ + """derivatice of quaternion muliplication wrt right multiplicand b""" u0 = a.vec[0] u1 = a.vec[1] u2 = a.vec[2] x = a.real - return sympy.Matrix([[x, -u2, u1, u0], - [u2, x, -u0, u1], - [-u1, u0, x, u2], - [-u0, -u1, -u2, x]]) + return sympy.Matrix( + [[x, -u2, u1, u0], [u2, x, -u0, u1], [-u1, u0, x, u2], [-u0, -u1, -u2, x]] + ) class TestQuaternion(unittest.TestCase): def setUp(self): - x, u0, u1, u2 = sympy.symbols('x u0 u1 u2', real=True) - y, v0, v1, v2 = sympy.symbols('y v0 v1 v2', real=True) + x, u0, u1, u2 = sympy.symbols("x u0 u1 u2", real=True) + y, v0, v1, v2 = sympy.symbols("y v0 v1 v2", real=True) u = Vector3(u0, u1, u2) v = Vector3(v0, v1, v2) self.a = Quaternion(x, u) @@ -115,23 +113,17 @@ def setUp(self): def test_multiplications(self): product = self.a * self.a.inv() - self.assertEqual(product.simplify(), - Quaternion.identity()) + self.assertEqual(product.simplify(), Quaternion.identity()) product = self.a.inv() * self.a - self.assertEqual(product.simplify(), - Quaternion.identity()) + self.assertEqual(product.simplify(), Quaternion.identity()) def test_derivatives(self): - d = sympy.Matrix(4, 4, lambda r, c: sympy.diff( - (self.a * self.b)[r], self.a[c])) - self.assertEqual(d, - Quaternion.Da_a_mul_b(self.a, self.b)) - d = sympy.Matrix(4, 4, lambda r, c: sympy.diff( - (self.a * self.b)[r], self.b[c])) - self.assertEqual(d, - Quaternion.Db_a_mul_b(self.a, self.b)) + d = sympy.Matrix(4, 4, lambda r, c: sympy.diff((self.a * self.b)[r], self.a[c])) + self.assertEqual(d, Quaternion.Da_a_mul_b(self.a, self.b)) + d = sympy.Matrix(4, 4, lambda r, c: sympy.diff((self.a * self.b)[r], self.b[c])) + self.assertEqual(d, Quaternion.Db_a_mul_b(self.a, self.b)) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() - print('hello') + print("hello") diff --git a/sympy/sophus/se2.py b/sympy/sophus/se2.py index 42f8377f6..e2abbd8d8 100644 --- a/sympy/sophus/se2.py +++ b/sympy/sophus/se2.py @@ -15,25 +15,24 @@ class Se2: - """ 2 dimensional group of rigid body transformations """ + """2 dimensional group of rigid body transformations""" def __init__(self, so2, t): - """ internally represented by a unit complex number z and a translation - 2-vector """ + """internally represented by a unit complex number z and a translation + 2-vector""" self.so2 = so2 self.t = t @staticmethod def exp(v): - """ exponential map """ + """exponential map""" theta = v[2] so2 = So2.exp(theta) a = so2.z.imag / theta b = (1 - so2.z.real) / theta - t = Vector2(a * v[0] - b * v[1], - b * v[0] + a * v[1]) + t = Vector2(a * v[0] - b * v[1], b * v[0] + a * v[1]) return Se2(so2, t) def log(self): @@ -41,14 +40,12 @@ def log(self): halftheta = 0.5 * theta a = -(halftheta * self.so2.z.imag) / (self.so2.z.real - 1) - V_inv = sympy.Matrix([[a, halftheta], - [-halftheta, a]]) + V_inv = sympy.Matrix([[a, halftheta], [-halftheta, a]]) upsilon = V_inv * self.t return Vector3(upsilon[0], upsilon[1], theta) def calc_Dx_log_this(self): - return sympy.Matrix(3, 4, lambda r, c: sympy.diff(self.log()[r], - self[c])) + return sympy.Matrix(3, 4, lambda r, c: sympy.diff(self.log()[r], self[c])) def __repr__(self): return "Se2: [" + repr(self.so2) + " " + repr(self.t) @@ -57,29 +54,26 @@ def __repr__(self): def hat(v): upsilon = Vector2(v[0], v[1]) theta = v[2] - return So2.hat(theta).\ - row_join(upsilon).\ - col_join(sympy.Matrix.zeros(1, 3)) + return So2.hat(theta).row_join(upsilon).col_join(sympy.Matrix.zeros(1, 3)) def matrix(self): - """ returns matrix representation """ + """returns matrix representation""" R = self.so2.matrix() return (R.row_join(self.t)).col_join(sympy.Matrix(1, 3, [0, 0, 1])) def __mul__(self, right): - """ left-multiplication - either rotation concatenation or point-transform """ + """left-multiplication + either rotation concatenation or point-transform""" if isinstance(right, sympy.Matrix): assert right.shape == (2, 1), right.shape return self.so2 * right + self.t elif isinstance(right, Se2): - return Se2(self.so2 * right.so2, - self.t + self.so2 * right.t) + return Se2(self.so2 * right.so2, self.t + self.so2 * right.t) assert False, "unsupported type: {0}".format(type(right)) def __getitem__(self, key): - """ We use the following convention [q0, q1, q2, q3, t0, t1, t2] """ - assert (key >= 0 and key < 4) + """We use the following convention [q0, q1, q2, q3, t0, t1, t2]""" + assert key >= 0 and key < 4 if key < 2: return self.so2[key] else: @@ -87,20 +81,19 @@ def __getitem__(self, key): @staticmethod def calc_Dx_exp_x(x): - return sympy.Matrix(4, 3, lambda r, c: - sympy.diff(Se2.exp(x)[r], x[c])) + return sympy.Matrix(4, 3, lambda r, c: sympy.diff(Se2.exp(x)[r], x[c])) @staticmethod def Dx_exp_x_at_0(): - return sympy.Matrix([[0, 0, 0], - [0, 0, 1], - [1, 0, 0], - [0, 1, 0]]) + return sympy.Matrix([[0, 0, 0], [0, 0, 1], [1, 0, 0], [0, 1, 0]]) def calc_Dx_this_mul_exp_x_at_0(self, x): - return sympy.Matrix(4, 3, lambda r, c: - sympy.diff((self * Se2.exp(x))[r], x[c])). \ - subs(x[0], 0).subs(x[1], 0).limit(x[2], 0) + return ( + sympy.Matrix(4, 3, lambda r, c: sympy.diff((self * Se2.exp(x))[r], x[c])) + .subs(x[0], 0) + .subs(x[1], 0) + .limit(x[2], 0) + ) @staticmethod def calc_Dx_exp_x_at_0(x): @@ -109,17 +102,18 @@ def calc_Dx_exp_x_at_0(x): @staticmethod def Dxi_x_matrix(x, i): if i < 2: - return So2.Dxi_x_matrix(x, i).\ - row_join(sympy.Matrix.zeros(2, 1)).\ - col_join(sympy.Matrix.zeros(1, 3)) + return ( + So2.Dxi_x_matrix(x, i) + .row_join(sympy.Matrix.zeros(2, 1)) + .col_join(sympy.Matrix.zeros(1, 3)) + ) M = sympy.Matrix.zeros(3, 3) M[i - 2, 2] = 1 return M @staticmethod def calc_Dxi_x_matrix(x, i): - return sympy.Matrix(3, 3, lambda r, c: - sympy.diff(x.matrix()[r, c], x[i])) + return sympy.Matrix(3, 3, lambda r, c: sympy.diff(x.matrix()[r, c], x[i])) @staticmethod def Dxi_exp_x_matrix(x, i): @@ -130,8 +124,9 @@ def Dxi_exp_x_matrix(x, i): @staticmethod def calc_Dxi_exp_x_matrix(x, i): - return sympy.Matrix(3, 3, lambda r, c: - sympy.diff(Se2.exp(x).matrix()[r, c], x[i])) + return sympy.Matrix( + 3, 3, lambda r, c: sympy.diff(Se2.exp(x).matrix()[r, c], x[i]) + ) @staticmethod def Dxi_exp_x_matrix_at_0(i): @@ -141,29 +136,33 @@ def Dxi_exp_x_matrix_at_0(i): @staticmethod def calc_Dxi_exp_x_matrix_at_0(x, i): - return sympy.Matrix(3, 3, lambda r, c: - sympy.diff(Se2.exp(x).matrix()[r, c], x[i]) - ).subs(x[0], 0).subs(x[1], 0).limit(x[2], 0) + return ( + sympy.Matrix(3, 3, lambda r, c: sympy.diff(Se2.exp(x).matrix()[r, c], x[i])) + .subs(x[0], 0) + .subs(x[1], 0) + .limit(x[2], 0) + ) class TestSe2(unittest.TestCase): def setUp(self): upsilon0, upsilon1, theta = sympy.symbols( - 'upsilon[0], upsilon[1], theta', - real=True) - x, y = sympy.symbols('c[0] c[1]', real=True) - p0, p1 = sympy.symbols('p0 p1', real=True) - t0, t1 = sympy.symbols('t[0] t[1]', real=True) - self.upsilon_theta = Vector3( - upsilon0, upsilon1, theta) + "upsilon[0], upsilon[1], theta", real=True + ) + x, y = sympy.symbols("c[0] c[1]", real=True) + p0, p1 = sympy.symbols("p0 p1", real=True) + t0, t1 = sympy.symbols("t[0] t[1]", real=True) + self.upsilon_theta = Vector3(upsilon0, upsilon1, theta) self.t = Vector2(t0, t1) self.a = Se2(So2(Complex(x, y)), self.t) self.p = Vector2(p0, p1) def test_exp_log(self): - for v in [Vector3(0., 1, 0.5), - Vector3(0.1, 0.1, 0.1), - Vector3(0.01, 0.2, 0.03)]: + for v in [ + Vector3(0.0, 1, 0.5), + Vector3(0.1, 0.1, 0.1), + Vector3(0.01, 0.2, 0.03), + ]: w = Se2.exp(v).log() for i in range(0, 3): self.assertAlmostEqual(v[i], w[i]) @@ -174,27 +173,37 @@ def test_matrix(self): point_bar = self.p p1_foo = T_foo_bar * point_bar p2_foo = proj(Tmat_foo_bar * unproj(point_bar)) - self.assertEqual(sympy.simplify(p1_foo - p2_foo), - ZeroVector2()) + self.assertEqual(sympy.simplify(p1_foo - p2_foo), ZeroVector2()) def test_derivatives(self): - self.assertEqual(sympy.simplify( - Se2.calc_Dx_exp_x_at_0(self.upsilon_theta) - - Se2.Dx_exp_x_at_0()), - sympy.Matrix.zeros(4, 3)) + self.assertEqual( + sympy.simplify( + Se2.calc_Dx_exp_x_at_0(self.upsilon_theta) - Se2.Dx_exp_x_at_0() + ), + sympy.Matrix.zeros(4, 3), + ) for i in range(0, 4): - self.assertEqual(sympy.simplify(Se2.calc_Dxi_x_matrix(self.a, i) - - Se2.Dxi_x_matrix(self.a, i)), - sympy.Matrix.zeros(3, 3)) + self.assertEqual( + sympy.simplify( + Se2.calc_Dxi_x_matrix(self.a, i) - Se2.Dxi_x_matrix(self.a, i) + ), + sympy.Matrix.zeros(3, 3), + ) for i in range(0, 3): - self.assertEqual(sympy.simplify( - Se2.Dxi_exp_x_matrix(self.upsilon_theta, i) - - Se2.calc_Dxi_exp_x_matrix(self.upsilon_theta, i)), - sympy.Matrix.zeros(3, 3)) - self.assertEqual(sympy.simplify( - Se2.Dxi_exp_x_matrix_at_0(i) - - Se2.calc_Dxi_exp_x_matrix_at_0(self.upsilon_theta, i)), - sympy.Matrix.zeros(3, 3)) + self.assertEqual( + sympy.simplify( + Se2.Dxi_exp_x_matrix(self.upsilon_theta, i) + - Se2.calc_Dxi_exp_x_matrix(self.upsilon_theta, i) + ), + sympy.Matrix.zeros(3, 3), + ) + self.assertEqual( + sympy.simplify( + Se2.Dxi_exp_x_matrix_at_0(i) + - Se2.calc_Dxi_exp_x_matrix_at_0(self.upsilon_theta, i) + ), + sympy.Matrix.zeros(3, 3), + ) def test_codegen(self): stream = cse_codegen(Se2.calc_Dx_exp_x(self.upsilon_theta)) @@ -214,8 +223,7 @@ def test_codegen(self): file.close() stream.close - stream = cse_codegen(self.a.calc_Dx_this_mul_exp_x_at_0( - self.upsilon_theta)) + stream = cse_codegen(self.a.calc_Dx_this_mul_exp_x_at_0(self.upsilon_theta)) filename = "cpp_gencode/Se2_Dx_this_mul_exp_x_at_0.cpp" # set to true to generate codegen files if False: @@ -249,5 +257,5 @@ def test_codegen(self): stream.close -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/sympy/sophus/se3.py b/sympy/sophus/se3.py index aba76df21..c5adc2e07 100644 --- a/sympy/sophus/se3.py +++ b/sympy/sophus/se3.py @@ -16,11 +16,11 @@ class Se3: - """ 3 dimensional group of rigid body transformations """ + """3 dimensional group of rigid body transformations""" def __init__(self, so3, t): - """ internally represented by a unit quaternion q and a translation - 3-vector """ + """internally represented by a unit quaternion q and a translation + 3-vector""" assert isinstance(so3, So3) assert isinstance(t, sympy.Matrix) assert t.shape == (3, 1), t.shape @@ -30,35 +30,39 @@ def __init__(self, so3, t): @staticmethod def exp(v): - """ exponential map """ + """exponential map""" upsilon = v[0:3, :] omega = Vector3(v[3], v[4], v[5]) so3 = So3.exp(omega) Omega = So3.hat(omega) Omega_sq = Omega * Omega theta = sympy.sqrt(squared_norm(omega)) - V = (sympy.Matrix.eye(3) + - (1 - sympy.cos(theta)) / (theta**2) * Omega + - (theta - sympy.sin(theta)) / (theta**3) * Omega_sq) + V = ( + sympy.Matrix.eye(3) + + (1 - sympy.cos(theta)) / (theta**2) * Omega + + (theta - sympy.sin(theta)) / (theta**3) * Omega_sq + ) return Se3(so3, V * upsilon) def log(self): - omega = self.so3.log() theta = sympy.sqrt(squared_norm(omega)) Omega = So3.hat(omega) half_theta = 0.5 * theta - V_inv = sympy.Matrix.eye(3) - 0.5 * Omega + (1 - theta * sympy.cos( - half_theta) / (2 * sympy.sin(half_theta))) / (theta * theta) *\ - (Omega * Omega) + V_inv = ( + sympy.Matrix.eye(3) + - 0.5 * Omega + + (1 - theta * sympy.cos(half_theta) / (2 * sympy.sin(half_theta))) + / (theta * theta) + * (Omega * Omega) + ) upsilon = V_inv * self.t return upsilon.col_join(omega) def calc_Dx_log_this(self): - return sympy.Matrix(6, 7, lambda r, c: sympy.diff(self.log()[r], - self[c])) + return sympy.Matrix(6, 7, lambda r, c: sympy.diff(self.log()[r], self[c])) def __repr__(self): return "Se3: [" + repr(self.so3) + " " + repr(self.t) @@ -69,34 +73,31 @@ def inverse(self): @staticmethod def hat(v): - """ R^6 => R^4x4 """ + """R^6 => R^4x4""" """ returns 4x4-matrix representation ``Omega`` """ upsilon = Vector3(v[0], v[1], v[2]) omega = Vector3(v[3], v[4], v[5]) - return So3.hat(omega).\ - row_join(upsilon).\ - col_join(sympy.Matrix.zeros(1, 4)) + return So3.hat(omega).row_join(upsilon).col_join(sympy.Matrix.zeros(1, 4)) @staticmethod def vee(Omega): - """ R^4x4 => R^6 """ + """R^4x4 => R^6""" """ returns 6-vector representation of Lie algebra """ """ This is the inverse of the hat-operator """ head = Vector3(Omega[0, 3], Omega[1, 3], Omega[2, 3]) tail = So3.vee(Omega[0:3, 0:3]) - upsilon_omega = \ - Vector6(head[0], head[1], head[2], tail[0], tail[1], tail[2]) + upsilon_omega = Vector6(head[0], head[1], head[2], tail[0], tail[1], tail[2]) return upsilon_omega def matrix(self): - """ returns matrix representation """ + """returns matrix representation""" R = self.so3.matrix() return (R.row_join(self.t)).col_join(sympy.Matrix(1, 4, [0, 0, 0, 1])) def __mul__(self, right): - """ left-multiplication - either rotation concatenation or point-transform """ + """left-multiplication + either rotation concatenation or point-transform""" if isinstance(right, sympy.Matrix): assert right.shape == (3, 1), right.shape return self.so3 * right + self.t @@ -107,8 +108,8 @@ def __mul__(self, right): assert False, "unsupported type: {0}".format(type(right)) def __getitem__(self, key): - """ We use the following convention [q0, q1, q2, q3, t0, t1, t2] """ - assert (key >= 0 and key < 7) + """We use the following convention [q0, q1, q2, q3, t0, t1, t2]""" + assert key >= 0 and key < 7 if key < 4: return self.so3[key] else: @@ -116,44 +117,60 @@ def __getitem__(self, key): @staticmethod def calc_Dx_exp_x(x): - return sympy.Matrix(7, 6, lambda r, c: - sympy.diff(Se3.exp(x)[r], x[c])) + return sympy.Matrix(7, 6, lambda r, c: sympy.diff(Se3.exp(x)[r], x[c])) @staticmethod def Dx_exp_x_at_0(): - return sympy.Matrix([[0.0, 0.0, 0.0, 0.5, 0.0, 0.0], - [0.0, 0.0, 0.0, 0.0, 0.5, 0.0], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.5], - [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [1.0, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.0, 1.0, 0.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 1.0, 0.0, 0.0, 0.0]]) + return sympy.Matrix( + [ + [0.0, 0.0, 0.0, 0.5, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.5, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.5], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [1.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.0, 0.0, 0.0], + ] + ) def calc_Dx_this_mul_exp_x_at_0(self, x): - return sympy.Matrix(7, 6, lambda r, c: - sympy.diff((self * Se3.exp(x))[r], x[c])). \ - subs(x[0], 0).subs(x[1], 0).subs(x[2], 0).\ - subs(x[3], 0).subs(x[4], 0).limit(x[5], 0) + return ( + sympy.Matrix(7, 6, lambda r, c: sympy.diff((self * Se3.exp(x))[r], x[c])) + .subs(x[0], 0) + .subs(x[1], 0) + .subs(x[2], 0) + .subs(x[3], 0) + .subs(x[4], 0) + .limit(x[5], 0) + ) @staticmethod def calc_Dx_exp_x_at_0(x): - return Se3.calc_Dx_exp_x(x).subs(x[0], 0).subs(x[1], 0).subs(x[2], 0).\ - subs(x[3], 0).subs(x[4], 0).limit(x[5], 0) + return ( + Se3.calc_Dx_exp_x(x) + .subs(x[0], 0) + .subs(x[1], 0) + .subs(x[2], 0) + .subs(x[3], 0) + .subs(x[4], 0) + .limit(x[5], 0) + ) @staticmethod def Dxi_x_matrix(x, i): if i < 4: - return So3.Dxi_x_matrix(x, i).\ - row_join(sympy.Matrix.zeros(3, 1)).\ - col_join(sympy.Matrix.zeros(1, 4)) + return ( + So3.Dxi_x_matrix(x, i) + .row_join(sympy.Matrix.zeros(3, 1)) + .col_join(sympy.Matrix.zeros(1, 4)) + ) M = sympy.Matrix.zeros(4, 4) M[i - 4, 3] = 1 return M @staticmethod def calc_Dxi_x_matrix(x, i): - return sympy.Matrix(4, 4, lambda r, c: - sympy.diff(x.matrix()[r, c], x[i])) + return sympy.Matrix(4, 4, lambda r, c: sympy.diff(x.matrix()[r, c], x[i])) @staticmethod def Dxi_exp_x_matrix(x, i): @@ -164,8 +181,9 @@ def Dxi_exp_x_matrix(x, i): @staticmethod def calc_Dxi_exp_x_matrix(x, i): - return sympy.Matrix(4, 4, lambda r, c: - sympy.diff(Se3.exp(x).matrix()[r, c], x[i])) + return sympy.Matrix( + 4, 4, lambda r, c: sympy.diff(Se3.exp(x).matrix()[r, c], x[i]) + ) @staticmethod def Dxi_exp_x_matrix_at_0(i): @@ -175,31 +193,40 @@ def Dxi_exp_x_matrix_at_0(i): @staticmethod def calc_Dxi_exp_x_matrix_at_0(x, i): - return sympy.Matrix(4, 4, lambda r, c: - sympy.diff(Se3.exp(x).matrix()[r, c], x[i]) - ).subs(x[0], 0).subs(x[1], 0).subs(x[2], 0).\ - subs(x[3], 0).subs(x[4], 0).limit(x[5], 0) + return ( + sympy.Matrix(4, 4, lambda r, c: sympy.diff(Se3.exp(x).matrix()[r, c], x[i])) + .subs(x[0], 0) + .subs(x[1], 0) + .subs(x[2], 0) + .subs(x[3], 0) + .subs(x[4], 0) + .limit(x[5], 0) + ) class TestSe3(unittest.TestCase): def setUp(self): upsilon0, upsilon1, upsilon2, omega0, omega1, omega2 = sympy.symbols( - 'upsilon[0], upsilon[1], upsilon[2], omega[0], omega[1], omega[2]', - real=True) - x, v0, v1, v2 = sympy.symbols('q.w() q.x() q.y() q.z()', real=True) - p0, p1, p2 = sympy.symbols('p0 p1 p2', real=True) - t0, t1, t2 = sympy.symbols('t[0] t[1] t[2]', real=True) + "upsilon[0], upsilon[1], upsilon[2], omega[0], omega[1], omega[2]", + real=True, + ) + x, v0, v1, v2 = sympy.symbols("q.w() q.x() q.y() q.z()", real=True) + p0, p1, p2 = sympy.symbols("p0 p1 p2", real=True) + t0, t1, t2 = sympy.symbols("t[0] t[1] t[2]", real=True) v = Vector3(v0, v1, v2) self.upsilon_omega = Vector6( - upsilon0, upsilon1, upsilon2, omega0, omega1, omega2) + upsilon0, upsilon1, upsilon2, omega0, omega1, omega2 + ) self.t = Vector3(t0, t1, t2) self.a = Se3(So3(Quaternion(x, v)), self.t) self.p = Vector3(p0, p1, p2) def test_exp_log(self): - for v in [Vector6(0., 1, 0.5, 2., 1, 0.5), - Vector6(0.1, 0.1, 0.1, 0., 1, 0.5), - Vector6(0.01, 0.2, 0.03, 0.01, 0.2, 0.03)]: + for v in [ + Vector6(0.0, 1, 0.5, 2.0, 1, 0.5), + Vector6(0.1, 0.1, 0.1, 0.0, 1, 0.5), + Vector6(0.01, 0.2, 0.03, 0.01, 0.2, 0.03), + ]: w = Se3.exp(v).log() for i in range(0, 3): self.assertAlmostEqual(v[i], w[i]) @@ -210,28 +237,38 @@ def test_matrix(self): point_bar = self.p p1_foo = T_foo_bar * point_bar p2_foo = proj(Tmat_foo_bar * unproj(point_bar)) - self.assertEqual(sympy.simplify(p1_foo - p2_foo), - ZeroVector3()) + self.assertEqual(sympy.simplify(p1_foo - p2_foo), ZeroVector3()) def test_derivatives(self): - self.assertEqual(sympy.simplify( - Se3.calc_Dx_exp_x_at_0(self.upsilon_omega) - - Se3.Dx_exp_x_at_0()), - sympy.Matrix.zeros(7, 6)) + self.assertEqual( + sympy.simplify( + Se3.calc_Dx_exp_x_at_0(self.upsilon_omega) - Se3.Dx_exp_x_at_0() + ), + sympy.Matrix.zeros(7, 6), + ) for i in range(0, 7): - self.assertEqual(sympy.simplify(Se3.calc_Dxi_x_matrix(self.a, i) - - Se3.Dxi_x_matrix(self.a, i)), - sympy.Matrix.zeros(4, 4)) + self.assertEqual( + sympy.simplify( + Se3.calc_Dxi_x_matrix(self.a, i) - Se3.Dxi_x_matrix(self.a, i) + ), + sympy.Matrix.zeros(4, 4), + ) for i in range(0, 6): - self.assertEqual(sympy.simplify( - Se3.Dxi_exp_x_matrix(self.upsilon_omega, i) - - Se3.calc_Dxi_exp_x_matrix(self.upsilon_omega, i)), - sympy.Matrix.zeros(4, 4)) - self.assertEqual(sympy.simplify( - Se3.Dxi_exp_x_matrix_at_0(i) - - Se3.calc_Dxi_exp_x_matrix_at_0(self.upsilon_omega, i)), - sympy.Matrix.zeros(4, 4)) + self.assertEqual( + sympy.simplify( + Se3.Dxi_exp_x_matrix(self.upsilon_omega, i) + - Se3.calc_Dxi_exp_x_matrix(self.upsilon_omega, i) + ), + sympy.Matrix.zeros(4, 4), + ) + self.assertEqual( + sympy.simplify( + Se3.Dxi_exp_x_matrix_at_0(i) + - Se3.calc_Dxi_exp_x_matrix_at_0(self.upsilon_omega, i) + ), + sympy.Matrix.zeros(4, 4), + ) def test_codegen(self): stream = cse_codegen(self.a.calc_Dx_exp_x(self.upsilon_omega)) @@ -250,8 +287,7 @@ def test_codegen(self): file.close() stream.close - stream = cse_codegen(self.a.calc_Dx_this_mul_exp_x_at_0( - self.upsilon_omega)) + stream = cse_codegen(self.a.calc_Dx_this_mul_exp_x_at_0(self.upsilon_omega)) filename = "cpp_gencode/Se3_Dx_this_mul_exp_x_at_0.cpp" # set to true to generate codegen files if False: @@ -285,5 +321,5 @@ def test_codegen(self): stream.close -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/sympy/sophus/so2.py b/sympy/sophus/so2.py index 5473879b2..8da3ed434 100644 --- a/sympy/sophus/so2.py +++ b/sympy/sophus/so2.py @@ -10,47 +10,41 @@ class So2: - """ 2 dimensional group of orthogonal matrices with determinant 1 """ + """2 dimensional group of orthogonal matrices with determinant 1""" def __init__(self, z): - """ internally represented by a unit complex number z """ + """internally represented by a unit complex number z""" self.z = z @staticmethod def exp(theta): - """ exponential map """ - return So2( - Complex( - sympy.cos(theta), - sympy.sin(theta))) + """exponential map""" + return So2(Complex(sympy.cos(theta), sympy.sin(theta))) def log(self): - """ logarithmic map""" + """logarithmic map""" return sympy.atan2(self.z.imag, self.z.real) def calc_Dx_log_this(self): return sympy.diff(self.log(), self[0]) def calc_Dx_log_exp_x_times_this_at_0(self, x): - return sympy.diff((So2.exp(x)*self).log(), x).limit(x, 0) + return sympy.diff((So2.exp(x) * self).log(), x).limit(x, 0) def __repr__(self): return "So2:" + repr(self.z) @staticmethod def hat(theta): - return sympy.Matrix([[0, -theta], - [theta, 0]]) + return sympy.Matrix([[0, -theta], [theta, 0]]) def matrix(self): - """ returns matrix representation """ - return sympy.Matrix([ - [self.z.real, -self.z.imag], - [self.z.imag, self.z.real]]) + """returns matrix representation""" + return sympy.Matrix([[self.z.real, -self.z.imag], [self.z.imag, self.z.real]]) def __mul__(self, right): - """ left-multiplication - either rotation concatenation or point-transform """ + """left-multiplication + either rotation concatenation or point-transform""" if isinstance(right, sympy.Matrix): assert right.shape == (2, 1), right.shape return self.matrix() * right @@ -63,8 +57,7 @@ def __getitem__(self, key): @staticmethod def calc_Dx_exp_x(x): - return sympy.Matrix(2, 1, lambda r, c: - sympy.diff(So2.exp(x)[r], x)) + return sympy.Matrix(2, 1, lambda r, c: sympy.diff(So2.exp(x)[r], x)) @staticmethod def Dx_exp_x_at_0(): @@ -75,23 +68,20 @@ def calc_Dx_exp_x_at_0(x): return So2.calc_Dx_exp_x(x).limit(x, 0) def calc_Dx_this_mul_exp_x_at_0(self, x): - return sympy.Matrix(2, 1, lambda r, c: - sympy.diff((self * So2.exp(x))[r], x))\ - .limit(x, 0) + return sympy.Matrix( + 2, 1, lambda r, c: sympy.diff((self * So2.exp(x))[r], x) + ).limit(x, 0) @staticmethod def Dxi_x_matrix(x, i): if i == 0: - return sympy.Matrix([[1, 0], - [0, 1]]) + return sympy.Matrix([[1, 0], [0, 1]]) if i == 1: - return sympy.Matrix([[0, -1], - [1, 0]]) + return sympy.Matrix([[0, -1], [1, 0]]) @staticmethod def calc_Dxi_x_matrix(x, i): - return sympy.Matrix(2, 2, lambda r, c: - sympy.diff(x.matrix()[r, c], x[i])) + return sympy.Matrix(2, 2, lambda r, c: sympy.diff(x.matrix()[r, c], x[i])) @staticmethod def Dx_exp_x_matrix(x): @@ -102,8 +92,7 @@ def Dx_exp_x_matrix(x): @staticmethod def calc_Dx_exp_x_matrix(x): - return sympy.Matrix(2, 2, lambda r, c: - sympy.diff(So2.exp(x).matrix()[r, c], x)) + return sympy.Matrix(2, 2, lambda r, c: sympy.diff(So2.exp(x).matrix()[r, c], x)) @staticmethod def Dx_exp_x_matrix_at_0(): @@ -111,22 +100,21 @@ def Dx_exp_x_matrix_at_0(): @staticmethod def calc_Dx_exp_x_matrix_at_0(x): - return sympy.Matrix(2, 2, lambda r, c: - sympy.diff(So2.exp(x).matrix()[r, c], x) - ).limit(x, 0) + return sympy.Matrix( + 2, 2, lambda r, c: sympy.diff(So2.exp(x).matrix()[r, c], x) + ).limit(x, 0) class TestSo2(unittest.TestCase): def setUp(self): - self.theta = sympy.symbols( - 'theta', real=True) - x, y = sympy.symbols('c[0] c[1]', real=True) - p0, p1 = sympy.symbols('p0 p1', real=True) + self.theta = sympy.symbols("theta", real=True) + x, y = sympy.symbols("c[0] c[1]", real=True) + p0, p1 = sympy.symbols("p0 p1", real=True) self.a = So2(Complex(x, y)) self.p = Vector2(p0, p1) def test_exp_log(self): - for theta in [0., 0.5, 0.1]: + for theta in [0.0, 0.5, 0.1]: w = So2.exp(theta).log() self.assertAlmostEqual(theta, w) @@ -136,26 +124,33 @@ def test_matrix(self): point_bar = self.p p1_foo = R_foo_bar * point_bar p2_foo = Rmat_foo_bar * point_bar - self.assertEqual(sympy.simplify(p1_foo - p2_foo), - ZeroVector2()) + self.assertEqual(sympy.simplify(p1_foo - p2_foo), ZeroVector2()) def test_derivatives(self): - self.assertEqual(sympy.simplify(So2.calc_Dx_exp_x_at_0(self.theta) - - So2.Dx_exp_x_at_0()), - sympy.Matrix.zeros(2, 1)) + self.assertEqual( + sympy.simplify(So2.calc_Dx_exp_x_at_0(self.theta) - So2.Dx_exp_x_at_0()), + sympy.Matrix.zeros(2, 1), + ) for i in [0, 1]: - self.assertEqual(sympy.simplify(So2.calc_Dxi_x_matrix(self.a, i) - - So2.Dxi_x_matrix(self.a, i)), - sympy.Matrix.zeros(2, 2)) - - self.assertEqual(sympy.simplify( - So2.Dx_exp_x_matrix(self.theta) - - So2.calc_Dx_exp_x_matrix(self.theta)), - sympy.Matrix.zeros(2, 2)) - self.assertEqual(sympy.simplify( - So2.Dx_exp_x_matrix_at_0() - - So2.calc_Dx_exp_x_matrix_at_0(self.theta)), - sympy.Matrix.zeros(2, 2)) + self.assertEqual( + sympy.simplify( + So2.calc_Dxi_x_matrix(self.a, i) - So2.Dxi_x_matrix(self.a, i) + ), + sympy.Matrix.zeros(2, 2), + ) + + self.assertEqual( + sympy.simplify( + So2.Dx_exp_x_matrix(self.theta) - So2.calc_Dx_exp_x_matrix(self.theta) + ), + sympy.Matrix.zeros(2, 2), + ) + self.assertEqual( + sympy.simplify( + So2.Dx_exp_x_matrix_at_0() - So2.calc_Dx_exp_x_matrix_at_0(self.theta) + ), + sympy.Matrix.zeros(2, 2), + ) def test_codegen(self): stream = cse_codegen(So2.calc_Dx_exp_x(self.theta)) @@ -174,8 +169,7 @@ def test_codegen(self): file.close() stream.close - stream = cse_codegen( - self.a.calc_Dx_this_mul_exp_x_at_0(self.theta)) + stream = cse_codegen(self.a.calc_Dx_this_mul_exp_x_at_0(self.theta)) filename = "cpp_gencode/So2_Dx_this_mul_exp_x_at_0.cpp" # set to true to generate codegen files if False: @@ -208,8 +202,7 @@ def test_codegen(self): file.close() stream.close - stream = cse_codegen(self.a.calc_Dx_log_exp_x_times_this_at_0( - self.theta)) + stream = cse_codegen(self.a.calc_Dx_log_exp_x_times_this_at_0(self.theta)) filename = "cpp_gencode/So2_Dx_log_exp_x_times_this_at_0.cpp" # set to true to generate codegen files @@ -227,5 +220,5 @@ def test_codegen(self): stream.close -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/sympy/sophus/so3.py b/sympy/sophus/so3.py index 6bc344625..9f7dd7b08 100644 --- a/sympy/sophus/so3.py +++ b/sympy/sophus/so3.py @@ -11,35 +11,38 @@ class So3: - """ 3 dimensional group of orthogonal matrices with determinant 1 """ + """3 dimensional group of orthogonal matrices with determinant 1""" def __init__(self, q): - """ internally represented by a unit quaternion q """ + """internally represented by a unit quaternion q""" self.q = q @staticmethod def exp(v): - """ exponential map """ + """exponential map""" theta_sq = squared_norm(v) theta = sympy.sqrt(theta_sq) return So3( - Quaternion( - sympy.cos(0.5 * theta), - sympy.sin(0.5 * theta) / theta * v)) + Quaternion(sympy.cos(0.5 * theta), sympy.sin(0.5 * theta) / theta * v) + ) def log(self): - """ logarithmic map""" + """logarithmic map""" n = sympy.sqrt(squared_norm(self.q.vec)) return 2 * sympy.atan(n / self.q.real) / n * self.q.vec def calc_Dx_log_this(self): - return sympy.Matrix(3, 3, lambda r, c: sympy.diff(self.log()[r], - self[c])) + return sympy.Matrix(3, 3, lambda r, c: sympy.diff(self.log()[r], self[c])) def calc_Dx_log_exp_x_times_this_at_0(self, x): - return sympy.Matrix(3, 3, lambda r, c: - sympy.diff((So3.exp(x)*self).log()[r], x[c])).subs( - x[0], 0).subs(x[1], 0).limit(x[2], 0) + return ( + sympy.Matrix( + 3, 3, lambda r, c: sympy.diff((So3.exp(x) * self).log()[r], x[c]) + ) + .subs(x[0], 0) + .subs(x[1], 0) + .limit(x[2], 0) + ) def __repr__(self): return "So3:" + repr(self.q) @@ -49,9 +52,7 @@ def inverse(self): @staticmethod def hat(o): - return sympy.Matrix([[0, -o[2], o[1]], - [o[2], 0, -o[0]], - [-o[1], o[0], 0]]) + return sympy.Matrix([[0, -o[2], o[1]], [o[2], 0, -o[0]], [-o[1], o[0], 0]]) """vee-operator @@ -66,38 +67,37 @@ def hat(o): | c 0 -a | | -b a 0 | """ + @staticmethod def vee(Omega): - v = Vector3(Omega.row(2).col(1), - Omega.row(0).col(2), - Omega.row(1).col(0)) + v = Vector3(Omega.row(2).col(1), Omega.row(0).col(2), Omega.row(1).col(0)) return v def matrix(self): - """ returns matrix representation """ - return sympy.Matrix([[ - 1 - 2 * self.q.vec[1]**2 - 2 * self.q.vec[2]**2, - 2 * self.q.vec[0] * self.q.vec[1] - - 2 * self.q.vec[2] * self.q[3], - 2 * self.q.vec[0] * self.q.vec[2] + - 2 * self.q.vec[1] * self.q[3] - ], [ - 2 * self.q.vec[0] * self.q.vec[1] + - 2 * self.q.vec[2] * self.q[3], - 1 - 2 * self.q.vec[0]**2 - 2 * self.q.vec[2]**2, - 2 * self.q.vec[1] * self.q.vec[2] - - 2 * self.q.vec[0] * self.q[3] - ], [ - 2 * self.q.vec[0] * self.q.vec[2] - - 2 * self.q.vec[1] * self.q[3], - 2 * self.q.vec[1] * self.q.vec[2] + - 2 * self.q.vec[0] * self.q[3], - 1 - 2 * self.q.vec[0]**2 - 2 * self.q.vec[1]**2 - ]]) + """returns matrix representation""" + return sympy.Matrix( + [ + [ + 1 - 2 * self.q.vec[1] ** 2 - 2 * self.q.vec[2] ** 2, + 2 * self.q.vec[0] * self.q.vec[1] - 2 * self.q.vec[2] * self.q[3], + 2 * self.q.vec[0] * self.q.vec[2] + 2 * self.q.vec[1] * self.q[3], + ], + [ + 2 * self.q.vec[0] * self.q.vec[1] + 2 * self.q.vec[2] * self.q[3], + 1 - 2 * self.q.vec[0] ** 2 - 2 * self.q.vec[2] ** 2, + 2 * self.q.vec[1] * self.q.vec[2] - 2 * self.q.vec[0] * self.q[3], + ], + [ + 2 * self.q.vec[0] * self.q.vec[2] - 2 * self.q.vec[1] * self.q[3], + 2 * self.q.vec[1] * self.q.vec[2] + 2 * self.q.vec[0] * self.q[3], + 1 - 2 * self.q.vec[0] ** 2 - 2 * self.q.vec[1] ** 2, + ], + ] + ) def __mul__(self, right): - """ left-multiplication - either rotation concatenation or point-transform """ + """left-multiplication + either rotation concatenation or point-transform""" if isinstance(right, sympy.Matrix): assert right.shape == (3, 1), right.shape return (self.q * Quaternion(0, right) * self.q.conj()).vec @@ -110,53 +110,72 @@ def __getitem__(self, key): @staticmethod def calc_Dx_exp_x(x): - return sympy.Matrix(4, 3, lambda r, c: - sympy.diff(So3.exp(x)[r], x[c])) + return sympy.Matrix(4, 3, lambda r, c: sympy.diff(So3.exp(x)[r], x[c])) @staticmethod def Dx_exp_x_at_0(): - return sympy.Matrix([[0.5, 0.0, 0.0], - [0.0, 0.5, 0.0], - [0.0, 0.0, 0.5], - [0.0, 0.0, 0.0]]) + return sympy.Matrix( + [[0.5, 0.0, 0.0], [0.0, 0.5, 0.0], [0.0, 0.0, 0.5], [0.0, 0.0, 0.0]] + ) @staticmethod def calc_Dx_exp_x_at_0(x): return So3.calc_Dx_exp_x(x).subs(x[0], 0).subs(x[1], 0).limit(x[2], 0) def calc_Dx_this_mul_exp_x_at_0(self, x): - return sympy.Matrix(4, 3, lambda r, c: - sympy.diff((self * So3.exp(x))[r], x[c]))\ - .subs(x[0], 0).subs(x[1], 0).limit(x[2], 0) + return ( + sympy.Matrix(4, 3, lambda r, c: sympy.diff((self * So3.exp(x))[r], x[c])) + .subs(x[0], 0) + .subs(x[1], 0) + .limit(x[2], 0) + ) def calc_Dx_exp_x_mul_this_at_0(self, x): - return sympy.Matrix(3, 4, lambda r, c: - sympy.diff((self * So3.exp(x))[c], x[r, 0]))\ - .subs(x[0], 0).subs(x[1], 0).limit(x[2], 0) + return ( + sympy.Matrix(3, 4, lambda r, c: sympy.diff((self * So3.exp(x))[c], x[r, 0])) + .subs(x[0], 0) + .subs(x[1], 0) + .limit(x[2], 0) + ) @staticmethod def Dxi_x_matrix(x, i): if i == 0: - return sympy.Matrix([[0, 2 * x[1], 2 * x[2]], - [2 * x[1], -4 * x[0], -2 * x[3]], - [2 * x[2], 2 * x[3], -4 * x[0]]]) + return sympy.Matrix( + [ + [0, 2 * x[1], 2 * x[2]], + [2 * x[1], -4 * x[0], -2 * x[3]], + [2 * x[2], 2 * x[3], -4 * x[0]], + ] + ) if i == 1: - return sympy.Matrix([[-4 * x[1], 2 * x[0], 2 * x[3]], - [2 * x[0], 0, 2 * x[2]], - [-2 * x[3], 2 * x[2], -4 * x[1]]]) + return sympy.Matrix( + [ + [-4 * x[1], 2 * x[0], 2 * x[3]], + [2 * x[0], 0, 2 * x[2]], + [-2 * x[3], 2 * x[2], -4 * x[1]], + ] + ) if i == 2: - return sympy.Matrix([[-4 * x[2], -2 * x[3], 2 * x[0]], - [2 * x[3], -4 * x[2], 2 * x[1]], - [2 * x[0], 2 * x[1], 0]]) + return sympy.Matrix( + [ + [-4 * x[2], -2 * x[3], 2 * x[0]], + [2 * x[3], -4 * x[2], 2 * x[1]], + [2 * x[0], 2 * x[1], 0], + ] + ) if i == 3: - return sympy.Matrix([[0, -2 * x[2], 2 * x[1]], - [2 * x[2], 0, -2 * x[0]], - [-2 * x[1], 2 * x[0], 0]]) + return sympy.Matrix( + [ + [0, -2 * x[2], 2 * x[1]], + [2 * x[2], 0, -2 * x[0]], + [-2 * x[1], 2 * x[0], 0], + ] + ) @staticmethod def calc_Dxi_x_matrix(x, i): - return sympy.Matrix(3, 3, lambda r, c: - sympy.diff(x.matrix()[r, c], x[i])) + return sympy.Matrix(3, 3, lambda r, c: sympy.diff(x.matrix()[r, c], x[i])) @staticmethod def Dxi_exp_x_matrix(x, i): @@ -167,8 +186,9 @@ def Dxi_exp_x_matrix(x, i): @staticmethod def calc_Dxi_exp_x_matrix(x, i): - return sympy.Matrix(3, 3, lambda r, c: - sympy.diff(So3.exp(x).matrix()[r, c], x[i])) + return sympy.Matrix( + 3, 3, lambda r, c: sympy.diff(So3.exp(x).matrix()[r, c], x[i]) + ) @staticmethod def Dxi_exp_x_matrix_at_0(i): @@ -178,26 +198,32 @@ def Dxi_exp_x_matrix_at_0(i): @staticmethod def calc_Dxi_exp_x_matrix_at_0(x, i): - return sympy.Matrix(3, 3, lambda r, c: - sympy.diff(So3.exp(x).matrix()[r, c], x[i]) - ).subs(x[0], 0).subs(x[1], 0).limit(x[2], 0) + return ( + sympy.Matrix(3, 3, lambda r, c: sympy.diff(So3.exp(x).matrix()[r, c], x[i])) + .subs(x[0], 0) + .subs(x[1], 0) + .limit(x[2], 0) + ) class TestSo3(unittest.TestCase): def setUp(self): omega0, omega1, omega2 = sympy.symbols( - 'omega[0], omega[1], omega[2]', real=True) - x, v0, v1, v2 = sympy.symbols('q.w() q.x() q.y() q.z()', real=True) - p0, p1, p2 = sympy.symbols('p0 p1 p2', real=True) + "omega[0], omega[1], omega[2]", real=True + ) + x, v0, v1, v2 = sympy.symbols("q.w() q.x() q.y() q.z()", real=True) + p0, p1, p2 = sympy.symbols("p0 p1 p2", real=True) v = Vector3(v0, v1, v2) self.omega = Vector3(omega0, omega1, omega2) self.a = So3(Quaternion(x, v)) self.p = Vector3(p0, p1, p2) def test_exp_log(self): - for o in [Vector3(0., 1, 0.5), - Vector3(0.1, 0.1, 0.1), - Vector3(0.01, 0.2, 0.03)]: + for o in [ + Vector3(0.0, 1, 0.5), + Vector3(0.1, 0.1, 0.1), + Vector3(0.01, 0.2, 0.03), + ]: w = So3.exp(o).log() for i in range(0, 3): self.assertAlmostEqual(o[i], w[i]) @@ -208,27 +234,36 @@ def test_matrix(self): point_bar = self.p p1_foo = R_foo_bar * point_bar p2_foo = Rmat_foo_bar * point_bar - self.assertEqual(sympy.simplify(p1_foo - p2_foo), - ZeroVector3()) + self.assertEqual(sympy.simplify(p1_foo - p2_foo), ZeroVector3()) def test_derivatives(self): - self.assertEqual(sympy.simplify(So3.calc_Dx_exp_x_at_0(self.omega) - - So3.Dx_exp_x_at_0()), - sympy.Matrix.zeros(4, 3)) + self.assertEqual( + sympy.simplify(So3.calc_Dx_exp_x_at_0(self.omega) - So3.Dx_exp_x_at_0()), + sympy.Matrix.zeros(4, 3), + ) for i in [0, 1, 2, 3]: - self.assertEqual(sympy.simplify(So3.calc_Dxi_x_matrix(self.a, i) - - So3.Dxi_x_matrix(self.a, i)), - sympy.Matrix.zeros(3, 3)) + self.assertEqual( + sympy.simplify( + So3.calc_Dxi_x_matrix(self.a, i) - So3.Dxi_x_matrix(self.a, i) + ), + sympy.Matrix.zeros(3, 3), + ) for i in [0, 1, 2]: - self.assertEqual(sympy.simplify( - So3.Dxi_exp_x_matrix(self.omega, i) - - So3.calc_Dxi_exp_x_matrix(self.omega, i)), - sympy.Matrix.zeros(3, 3)) - self.assertEqual(sympy.simplify( - So3.Dxi_exp_x_matrix_at_0(i) - - So3.calc_Dxi_exp_x_matrix_at_0(self.omega, i)), - sympy.Matrix.zeros(3, 3)) + self.assertEqual( + sympy.simplify( + So3.Dxi_exp_x_matrix(self.omega, i) + - So3.calc_Dxi_exp_x_matrix(self.omega, i) + ), + sympy.Matrix.zeros(3, 3), + ) + self.assertEqual( + sympy.simplify( + So3.Dxi_exp_x_matrix_at_0(i) + - So3.calc_Dxi_exp_x_matrix_at_0(self.omega, i) + ), + sympy.Matrix.zeros(3, 3), + ) def test_codegen(self): stream = cse_codegen(So3.calc_Dx_exp_x(self.omega)) @@ -247,8 +282,7 @@ def test_codegen(self): file.close() stream.close - stream = cse_codegen( - self.a.calc_Dx_this_mul_exp_x_at_0(self.omega)) + stream = cse_codegen(self.a.calc_Dx_this_mul_exp_x_at_0(self.omega)) filename = "cpp_gencode/So3_Dx_this_mul_exp_x_at_0.cpp" # set to true to generate codegen files if False: @@ -281,8 +315,7 @@ def test_codegen(self): file.close() stream.close - stream = cse_codegen(self.a.calc_Dx_log_exp_x_times_this_at_0( - self.omega)) + stream = cse_codegen(self.a.calc_Dx_log_exp_x_times_this_at_0(self.omega)) filename = "cpp_gencode/So3_Dx_log_exp_x_times_this_at_0.cpp" # set to true to generate codegen files @@ -300,5 +333,5 @@ def test_codegen(self): stream.close -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/test/ceres/tests.hpp b/test/ceres/tests.hpp index aacf5f414..63bd4ab3c 100644 --- a/test/ceres/tests.hpp +++ b/test/ceres/tests.hpp @@ -107,7 +107,7 @@ struct LieGroupCeresTests { using Mapper = Mapper::Tangent>; typename Mapper::Map residuals = Mapper::map(sResiduals); - // We are able to mix Sophus types with doubles and Jet types withou + // We are able to mix Sophus types with doubles and Jet types without // needing to cast to T. residuals = (T_aw * T_wa).log(); // Reverse order of multiplication. This forces the compiler to verify