mathrobo 0.0.1__tar.gz

mathrobo-0.0.1/.gitignore

@@ -0,0 +1,4 @@
+__pycache__/
+
+/mrenv
+.python-version

mathrobo-0.0.1/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2024 Taiki Ishigaki
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

mathrobo-0.0.1/PKG-INFO

@@ -0,0 +1,59 @@
+Metadata-Version: 2.4
+Name: mathrobo
+Version: 0.0.1
+Summary: basic mathematical library for robotics reserach
+Project-URL: Homepage, https://github.com/MathRobotics/MathRobo
+Author-email: taiki-ishigaki <taiki000ishigaki@gmail.com>
+License: MIT License
+License-File: LICENSE
+Classifier: License :: OSI Approved :: MIT License
+Classifier: Operating System :: OS Independent
+Classifier: Programming Language :: Python :: 3
+Requires-Python: >=3.9
+Requires-Dist: jax>=0.4.30
+Requires-Dist: numpy>=2.0.2
+Requires-Dist: pytest>=8.4.2
+Requires-Dist: scipy>=1.13.1
+Requires-Dist: sympy>=1.14.0
+Description-Content-Type: text/markdown
+
+# Mathrobo
+
+Mathrobo is a lightweight library designed to support mathematical optimization and computations related to robotics.
+
+## Installation
+
+### Clone the repository
+
+```bash
+git clone https://github.com/MathRobotics/MathRobo.git
+cd MathRobo
+```
+
+### Install dependencies with uv
+
+Sync the environment from `pyproject.toml` and `uv.lock`:
+
+```bash
+uv sync
+```
+
+### Install the package in editable mode
+
+```bash
+uv pip install -e .
+```
+
+## Examples
+Refer to the examples in the `examples` folder, where you can find Jupyter notebooks and scripts demonstrating various use cases of the library.
+
+## Running Tests
+
+Run the test suite with uv:
+
+```bash
+uv run pytest
+```
+
+## Contributing
+Contributions are welcome! Feel free to report issues, suggest features, or submit pull requests.

mathrobo-0.0.1/README.md

@@ -0,0 +1,40 @@
+# Mathrobo
+
+Mathrobo is a lightweight library designed to support mathematical optimization and computations related to robotics.
+
+## Installation
+
+### Clone the repository
+
+```bash
+git clone https://github.com/MathRobotics/MathRobo.git
+cd MathRobo
+```
+
+### Install dependencies with uv
+
+Sync the environment from `pyproject.toml` and `uv.lock`:
+
+```bash
+uv sync
+```
+
+### Install the package in editable mode
+
+```bash
+uv pip install -e .
+```
+
+## Examples
+Refer to the examples in the `examples` folder, where you can find Jupyter notebooks and scripts demonstrating various use cases of the library.
+
+## Running Tests
+
+Run the test suite with uv:
+
+```bash
+uv run pytest
+```
+
+## Contributing
+Contributions are welcome! Feel free to report issues, suggest features, or submit pull requests.

mathrobo-0.0.1/mathrobo/__init__.py

@@ -0,0 +1,8 @@
+#!/usr/bin/env python3.9
+# -*- coding: utf-8 -*-
+# 2024.06.23 Created by T.Ishigaki
+
+from .basic import *
+from .lie import *
+from .calculus import *
+from .transformation import *

mathrobo-0.0.1/mathrobo/basic/__init__.py

@@ -0,0 +1,7 @@
+#!/usr/bin/env python3.9
+# -*- coding: utf-8 -*-
+# 2024.08.17 Created by T.Ishigaki
+
+from .basic import *
+from .factorial_vec import *
+from .cm_vec import *

mathrobo-0.0.1/mathrobo/basic/basic.py

@@ -0,0 +1,133 @@
+#!/usr/bin/env python3.9
+# -*- coding: utf-8 -*-
+# 2024.06.23 Created by T.Ishigaki
+
+import numpy as np
+import sympy as sp
+import jax.numpy as jnp
+
+import math
+
+def lib_error_message():
+    ValueError("Unsupported library. Choose 'numpy', 'sympy' or 'jax'.")
+
+def iszero(x):
+    tolerance = 1e-8  # 許容範囲
+    return math.isclose(x, 0, abs_tol=tolerance)
+
+def sin(theta, LIB = 'numpy'):
+    if LIB == 'numpy':
+        return np.sin(theta)
+    elif LIB == 'sympy':
+        return sp.sin(theta)
+    elif LIB == 'jax':
+        return jnp.sin(theta)
+    else:
+        raise lib_error_message()
+    
+def cos(theta, LIB = 'numpy'):
+    if LIB == 'numpy':
+        return np.cos(theta)
+    elif LIB == 'sympy':
+        return sp.cos(theta)
+    elif LIB == 'jax':
+        return jnp.cos(theta)
+    else:
+        raise lib_error_message()
+
+def zeros(shape, LIB = 'numpy'):
+    if LIB == 'numpy':
+        return np.zeros(shape)
+    elif LIB == 'sympy':
+        if type(shape) == int:
+            return sp.zeros(shape,1)
+        elif type(shape) == tuple and len(shape) == 2:
+            return sp.zeros(shape[0],shape[1])
+    elif LIB == 'jax':
+        return jnp.zeros(shape)
+    else:
+        raise lib_error_message()
+    
+def identity(size, LIB = 'numpy'):
+    if LIB == 'numpy':
+        return np.identity(size)
+    elif LIB == 'sympy':
+        m = sp.zeros(size,size)
+        for i in range(size):
+            m[i,i] = 1
+        return m
+    elif LIB == 'jax':
+        return jnp.identity(size)
+    else:
+        raise lib_error_message()  
+
+def norm(vec, LIB = 'numpy'):
+        if LIB == 'numpy':
+            return np.linalg.norm(vec)
+        elif LIB == 'sympy':
+            return sp.sqrt(vec.dot(vec))
+        elif LIB == 'jax':
+            return jnp.linalg.norm(vec)
+        else:
+            raise lib_error_message()
+        
+def isclose(a, b, LIB = 'numpy'):
+        if LIB == 'numpy':
+                return np.isclose(a, b)
+        elif LIB == 'sympy':
+                return sp.simplify(a - b) == 0
+        elif LIB == 'jax':
+                return jnp.isclose(a, b)
+        else:
+                raise lib_error_message()
+        
+        
+def gq_integrate(func, a, b, digit = 5, LIB = 'numpy'):
+        if LIB == 'numpy':
+            integ = np.zeros((func(0).shape))
+
+            if digit == 3:
+                quad_x = np.array((-0.77459666924, 0, 0.77459666924))
+                quad_weight = np.array((0.55555555555, 0.88888888888, 0.55555555555))
+            elif digit == 4:
+                quad_x = np.array((-0.8611363116, -0.3399810436, 0.3399810436, 0.8611363116))
+                quad_weight = np.array((0.3478548451, 0.6521451549, 0.6521451549, 0.3478548451))
+            elif digit == 5:
+                quad_x = np.array((-0.9061798459, -0.5384693101, 0, 0.5384693101, 0.9061798459))
+                quad_weight = np.array((0.2369268851, 0.4786286705, 0.5688888889, 0.4786286705, 0.2369268851))
+            
+            for i in range(digit):
+                w = quad_weight[i] * (b - a) * 0.5
+                x = quad_x[i] * (b - a) * 0.5 + (a + b) * 0.5
+                integ += w*func(x)
+
+            return integ
+        else:
+            raise ValueError("This method for 'numpy'")
+
+def jac_lie_wrt_scaler(lie, vec, a, dvec, LIB = 'numpy'):
+    m = lie.exp(vec, a, LIB)
+    integ_m = -lie.exp_integ_adj(vec, -a, LIB)
+
+    return m @ lie.hat(integ_m @ dvec, LIB)
+
+def jac_adj_lie_wrt_scaler(lie, vec, a, dvec, LIB = 'numpy'):
+    m = lie.exp_adj(vec, a, LIB)
+    integ_m = -lie.exp_integ_adj(vec, -a, LIB)
+
+    return m @ lie.hat_adj(integ_m @ dvec, LIB)
+
+def jac_lie_v_wrt_vector(lie, vec, a, v, LIB = 'numpy'):
+    m = lie.exp(vec, a, LIB)
+    integ_m = -lie.exp_integ_adj(vec, -a, LIB)
+
+    return m @ lie.hat_commute_adj(v, LIB) @ integ_m
+
+def sympy_to_numpy(sp_mat):
+    return np.array(sp_mat).astype(np.float64)
+
+def sympy_subs_mat(m, vec_str, vec_val):
+    for i in range(len(vec_str)):
+        m = m.subs([(vec_str[i], vec_val[i])])
+
+    return m

mathrobo-0.0.1/mathrobo/basic/cm_vec.py

@@ -0,0 +1,34 @@
+import math
+from typing import Union
+
+import numpy as np
+import jax.numpy as jnp
+
+from .factorial_vec import *
+
+class CMVector:
+    def __init__(self, vecs : Union[np.ndarray, jnp.ndarray]):
+        self._factorial_vector = FactorialVector(vecs)
+        self._n = self._factorial_vector._n
+        self._dim = self._factorial_vector._dim
+        self._len = self._factorial_vector._len
+
+    @staticmethod
+    def set_cmvecs(cm_vecs : Union[np.ndarray, jnp.ndarray]) -> 'CMVector':
+        factorial_vec = FactorialVector.set_ifac_vecs(cm_vecs)
+        return CMVector(factorial_vec.vecs())
+
+    def vecs(self) -> Union[np.ndarray, jnp.ndarray]:
+        return self._factorial_vector.vecs()
+    
+    def cm_vecs(self) -> Union[np.ndarray, jnp.ndarray]:
+        return self._factorial_vector.ifac_vecs()
+    
+    def vec(self) -> Union[np.ndarray, jnp.ndarray]:
+        return self._factorial_vector.vec()
+    
+    def cm_vec(self) -> Union[np.ndarray, jnp.ndarray]:
+        return self._factorial_vector.ifac_vec()    
+    
+    def __repr__(self):
+        return f"CMVector(n={self._n}, dim={self._dim}, len={self._len})\n{self.cm_vecs()}"

mathrobo-0.0.1/mathrobo/basic/factorial_vec.py

@@ -0,0 +1,80 @@
+import math
+from typing import Union
+
+import numpy as np
+import jax.numpy as jnp
+
+class Factorial:
+    @classmethod
+    def mat(cls, n: int, dim: int) -> Union[np.ndarray, jnp.ndarray]:
+        length = n * dim
+        if isinstance(dim, int):
+            mat = np.eye(length)
+        else:
+            mat = jnp.eye(length)
+
+        for i in range(n):
+            mat[i*dim:(i+1)*dim, i*dim:(i+1)*dim] *= math.factorial(i)
+        return mat
+    
+    @classmethod
+    def mat_inv(cls, n: int, dim: int) -> Union[np.ndarray, jnp.ndarray]:
+        length = n * dim
+        if isinstance(dim, int):
+            mat = np.eye(length)
+        else:
+            mat = jnp.eye(length)
+
+        for i in range(n):
+            mat[i*dim:(i+1)*dim, i*dim:(i+1)*dim] /= math.factorial(i)
+        return mat
+class FactorialVector:
+    def __init__(self, vecs : Union[np.ndarray, jnp.ndarray]):
+        self._n = vecs.shape[0]
+        self._dim = vecs.shape[1] if len(vecs.shape) > 1 else 1
+        self._len = vecs.flatten().shape[0]
+        self._vecs = vecs
+        self._factorial_mat = Factorial.mat(self._n, self._dim)
+        self._inverse_factorial_mat = Factorial.mat_inv(self._n, self._dim)
+        self._factorial_vecs = (self._factorial_mat @ vecs.flatten()).reshape(self._n, self._dim)
+        self._inverse_factorial_vecs = (self._inverse_factorial_mat @ vecs.flatten()).reshape(self._n, self._dim)
+
+    @staticmethod
+    def set_fac_vecs(fac_vecs : Union[np.ndarray, jnp.ndarray]) -> 'FactorialVector':
+        n = fac_vecs.shape[0]
+        dim = fac_vecs.shape[1] if len(fac_vecs.shape) > 1 else 1
+        inverse_factorial_mat = Factorial.mat_inv(n, dim)
+        vecs = (inverse_factorial_mat @ fac_vecs.flatten()).reshape(n, dim)
+        return FactorialVector(vecs)
+    
+    @staticmethod
+    def set_ifac_vecs(ifac_vecs : Union[np.ndarray, jnp.ndarray]) -> 'FactorialVector':
+        n = ifac_vecs.shape[0]
+        dim = ifac_vecs.shape[1] if len(ifac_vecs.shape) > 1 else 1
+        factorial_mat = Factorial.mat(n, dim)
+        vecs = (factorial_mat @ ifac_vecs.flatten()).reshape(n, dim)
+        return FactorialVector(vecs)
+
+    def vecs(self) -> Union[np.ndarray, jnp.ndarray]:
+        return self._vecs
+
+    def fac_vecs(self) -> Union[np.ndarray, jnp.ndarray]:
+        return self._factorial_vecs
+    
+    def ifac_vecs(self) -> Union[np.ndarray, jnp.ndarray]:
+        return self._inverse_factorial_vecs
+
+    def vec(self) -> Union[np.ndarray, jnp.ndarray]:
+        return self._vecs.flatten()
+
+    def fac_vec(self) -> Union[np.ndarray, jnp.ndarray]:
+        return self._factorial_vecs.flatten()
+    
+    def ifac_vec(self) -> Union[np.ndarray, jnp.ndarray]:
+        return self._inverse_factorial_vecs.flatten()
+    
+    def fac_mat(self) -> Union[np.ndarray, jnp.ndarray]:
+        return self._factorial_mat
+
+    def ifac_mat(self) -> Union[np.ndarray, jnp.ndarray]:
+        return self._inverse_factorial_mat

mathrobo-0.0.1/mathrobo/calculus/__init__.py

@@ -0,0 +1,7 @@
+#!/usr/bin/env python3.9
+# -*- coding: utf-8 -*-
+# 2025.05.08 Created by T.Ishigaki
+
+from .integral import *
+from .numerical_grad import *
+from .bspline import *

mathrobo-0.0.1/mathrobo/calculus/bspline.py

@@ -0,0 +1,159 @@
+import numpy as np
+from scipy.interpolate import BSpline
+
+# ----------------------------------------------------------------------
+def build_bspline_model(knots, control, degree: int):
+        """
+        Parameters
+        ----------
+        knots   : (m+1,)   non-decreasing knot vector
+        control : (n, D)   control-point matrix
+        degree  : int      spline order k
+
+        Returns
+        -------
+        curve(tq, order)  -> (..., D)
+        jac  (tq, order)  -> (M*D, n*D)
+        """
+        T = np.asarray(knots, dtype=np.float64)
+        P = np.asarray(control, dtype=np.float64)
+        n_ctrl, D = P.shape
+        k = degree
+
+        # --- dimension-wise splines ---------------------------------------
+        spl = [BSpline(T, P[:, d], k) for d in range(D)]
+
+        # cache derivatives up to order k
+        deriv_tbl = [[s.derivative(o) if o else s for o in range(k + 1)] for s in spl]
+
+        # basis splines for Jacobian
+        I = np.eye(n_ctrl)
+        bases = [BSpline(T, I[i], k, extrapolate=False) for i in range(n_ctrl)]
+        bases_der = [[b.derivative(o) if o else b for o in range(k + 1)]
+                                 for b in bases]
+
+        def curve(tq, order: int = 0):
+                if order < 0 or order > k:
+                        raise ValueError(f"order must be 0-{k}")
+                tq = np.asarray(tq, dtype=np.float64)
+                return np.column_stack([tbl[order](tq) for tbl in deriv_tbl])
+
+        def jac(tq, order: int = 0):
+                if order < 0 or order > k:
+                        raise ValueError(f"order must be 0-{k}")
+                tq = np.asarray(tq, dtype=np.float64).ravel()
+                M = tq.size
+
+                # derivative of basis (M, n_ctrl)
+                B = np.column_stack([bases_der[i][order](tq) for i in range(n_ctrl)])
+                return np.kron(B, np.eye(D)).reshape(M * D, n_ctrl * D)
+
+        return curve, jac
+
+def bspline_curve(knots, control, degree, t, order=0):
+        """
+        B-spline curve at evaluation points.
+
+        Parameters
+        ----------
+        knots : array_like, shape (m+1,)
+                Non-decreasing knot vector.
+        control : array_like, shape (n_ctrl, D)
+                Control point matrix.
+        degree : int
+                Spline degree.
+        t : array_like
+                Parameter values at which to evaluate.
+        order : int, optional
+                Derivative order (0 means curve value). Must be 0 <= order <= degree.
+
+        Returns
+        -------
+        curve_vals : ndarray, shape (len(t), D)
+                Evaluated curve or derivative.
+        """
+        T = np.asarray(knots, dtype=np.float64, copy=False)
+        P = np.asarray(control, dtype=np.float64, copy=False)
+        _, D = P.shape
+        k = degree
+
+        if order < 0 or order > k:
+                raise ValueError(f"order must be between 0 and {k}")
+
+        t = np.asarray(t, dtype=np.float64, copy=False)
+        result = []
+        for d in range(D):
+                spline = BSpline(T, P[:, d], k, extrapolate=False)
+                if order > 0:
+                        spline = spline.derivative(order)
+                result.append(spline(t))
+        return np.column_stack(result)
+
+
+def bspline_jacobian(knots, n_ctrl, degree, t, order=0):
+        """
+        Compute the Jacobian of the B-spline curve wrt control points.
+
+        Parameters
+        ----------
+        knots : array_like, shape (m+1,)
+                Non-decreasing knot vector.
+        control : array_like, shape (n_ctrl, D)
+                Control point matrix.
+        degree : int
+                Spline degree.
+        t : array_like
+                Parameter values at which to evaluate.
+        order : int, optional
+                Derivative order. Must be 0 <= order <= degree.
+
+        Returns
+        -------
+        J : ndarray, shape (len(t)*D, n_ctrl*D)
+                Jacobian matrix d(Curve)/d(control).
+        """
+        T = np.asarray(knots, dtype=np.float64, copy=False)
+        k = degree
+
+        if order < 0 or order > k:
+                raise ValueError(f"order must be between 0 and {k}")
+
+        t = np.asarray(t, dtype=np.float64).ravel()
+        I = np.eye(n_ctrl)
+        bases = [BSpline(T, I[i], k, extrapolate=False) for i in range(n_ctrl)]
+        B = np.column_stack([
+                (bases[i].derivative(order) if order > 0 else bases[i])(t)
+                for i in range(n_ctrl)
+        ])  # shape (len(t), n_ctrl)
+
+        J = B
+        return J
+
+
+
+# ----------------------------------------------------------------------
+def jac_numerical(curve, ctrl, knots, k, tq, h=1e-6, order=0):
+        """
+        Finite-difference Jacobian for validation
+        curve(tq, order)  : 0-k 階(k = degree)まで評価
+        ctrl              : (n_ctrl, D)   control-point matrix
+        knots             : (m+1,)        non-decreasing knot vector
+        k                 : int           spline order k
+        tq                : (M,)         evaluation points
+        h                 : float         finite-difference step size
+        order            : int           derivative order (0-k)
+        """
+        n_ctrl, D = ctrl.shape
+        M = len(tq)
+        J_fd = np.empty((M * D, n_ctrl * D))
+
+        base = curve(tq, order).reshape(M * D)
+
+        for i in range(n_ctrl):
+                for d in range(D):
+                        pert = ctrl.copy()
+                        pert[i, d] += h
+                        curve_p, _ = build_bspline_model(knots, pert, k)
+                        col = (curve_p(tq, order).reshape(M * D) - base) / h
+                        J_fd[:, i * D + d] = col
+        return J_fd

mathrobo-0.0.1/mathrobo/calculus/integral.py

@@ -0,0 +1,84 @@
+import math
+import numpy as np
+
+def build_integrator(dof, order, dt, method="euler"):
+        """
+        Construct integration matrices for various numerical integration methods.
+
+        Parameters
+        ----------
+        dof : int
+                Degrees of freedom.
+        order : int
+                Order of the state (e.g., 2 for [q, dq], 3 for [q, dq, ddq]).
+        dt : float
+                Time step.
+        method : str
+                Integration method: "euler", "poly", "rk2", "rk4".
+
+        Returns
+        -------
+        mat : ndarray of shape (dof * order, dof * order)
+                State transition matrix.
+        vec : ndarray of shape (dof * order, dof)
+                Control/integration vector matrix (for highest-order derivative input).
+        """
+        dim = dof * order
+        A = np.eye(dim)
+        B = np.zeros((dim, dof))
+
+        if method == "euler":
+                # 1st-order approximation: x_{k+1} ≈ x_k + dt * dx_k
+                for i in range(order - 1):
+                        A[i * dof:(i + 1) * dof, (i + 1) * dof:(i + 2) * dof] = dt * np.eye(dof)
+                B[(order - 1) * dof:] = dt * np.eye(dof)
+
+        elif method == "poly":
+                # Arbitrary-order Taylor expansion: dt^n / n!
+                for i in range(1, order):
+                        for j in range(i):
+                                power = i - j
+                                coef = (dt ** power) / math.factorial(power)
+                                A[j*dof:(j+1)*dof, i*dof:(i+1)*dof] = coef * np.eye(dof)
+                for i in range(order):
+                        power = order - i
+                        coef = (dt ** power) / math.factorial(power)
+                        B[i*dof:(i+1)*dof] = coef * np.eye(dof)
+
+        elif method == "rk2":
+                # Midpoint method approximation (for 2nd-order accuracy)
+                # x(t+dt) ≈ x(t) + dt * f(x(t) + 0.5*dt*f(x))
+                for i in range(1, order):
+                        for j in range(i):
+                                coef = dt ** (i - j)
+                                if i - j == 1:
+                                        coef += 0.5 * dt ** (i - j)
+                                A[j*dof:(j+1)*dof, i*dof:(i+1)*dof] = coef * np.eye(dof)
+
+                for i in range(order):
+                        coef = dt ** (order - i)
+                        if order - i == 1:
+                                coef += 0.5 * dt ** (order - i)
+                        B[i*dof:(i+1)*dof] = coef * np.eye(dof)
+
+        elif method == "rk4":
+                # RK4 is not straightforwardly expressed in this matrix form,
+                # but we can approximate with higher-weighted Taylor expansion
+                coeffs = [1, 1/2, 1/6]  # leading coefficients for 1st, 2nd, 3rd derivatives
+                for i in range(1, order):
+                        for j in range(i):
+                                power = i - j
+                                if power <= len(coeffs):
+                                        coef = coeffs[power - 1] * dt ** power
+                                        A[j*dof:(j+1)*dof, i*dof:(i+1)*dof] = coef * np.eye(dof)
+
+                for i in range(order):
+                        power = order - i
+                        if power <= len(coeffs):
+                                coef = coeffs[power - 1] * dt ** power
+                                B[i*dof:(i+1)*dof] = coef * np.eye(dof)
+
+        else:
+                raise ValueError(f"Unsupported method: {method}")
+
+        return A, B