import numpy as np from numbers import Number def _auxillary_func_one(z: Number) -> Number: return (1 - 0.5 * z * _cot(0.5 * z)) / (z ** 2) def _auxillary_func_two(z: Number, rho: Number) -> Number: return ( 0.25 * rho * ((z * _csc(0.5 * z)) ** 2 + 2 * z * _cot(0.5 * z) - 8) / (z ** 4) ) def _cot(z): return 1 / np.tan(z) def _csc(z): return 1 / np.sin(z) def dexpinv_se3(x: np.ndarray, y: np.ndarray) -> np.ndarray: """Calculate \$\mathop{dexpinv}_{x}^{-1} (y)\$ and return the result. Both x and y must be elements of the Lie algebra se(3) represented as one-dimensional arrays of length 6. If this does not suit your implementation, feel free to modify the source code accordingly. Note that this method does not have any safe-guard against singularities that occur when the norm of the first three components of the x-argument becomes too small. To remedy this, it may be desirable to first check this value and if necessary, use a truncated series instead. Parameters ---------- x : np.ndarray numpy array of length 6, element of se(3). y : np.ndarray numpy array of length 6, element of se(3). Returns ------- np.ndarray The value of dexpinv_x y represented as an array of length 6. Raises ------ TypeError Occurs when either x or y is not a numpy array. ValueError Occurs when either x or y is not of the appropriate length. """ if not isinstance(x, np.ndarray) or not isinstance(y, np.ndarray): raise TypeError("x and y must be numpy arrays") if x.shape != (6,) or y.shape != (6,): raise ValueError("x and y must be one-dimensional arrays of length 6") A, a = np.split(x, 2) B, b = np.split(y, 2) alpha = np.linalg.norm(A) rho = np.inner(A, a) c1 = ( B - 0.5 * np.cross(A, B) + _auxillary_func_one(alpha) * np.cross(A, np.cross(A, B)) ) c2 = ( b - 0.5 * (np.cross(a, B) + np.cross(A, b)) + _auxillary_func_two(alpha, rho) * np.cross(A, np.cross(A, B)) + _auxillary_func_one(alpha) * ( np.cross(a, np.cross(A, B)) + np.cross(A, np.cross(a, B)) + np.cross(A, np.cross(A, b)) ) ) return np.hstack((c1, c2))