Fixed Wrench Projectors, Ad Operators.

Author: luis-marques

.gitignore

@@ -1,6 +1,6 @@
 **/__pycache__
 dist
-
 uv.lock
-
-.vscode/
+.vscode/
+venv/
+.mypy_cache/

README.md

@@ -1,3 +1,8 @@
+# PyMatLie
+
+![workflow](https://github.com/luis-marques/pymatlie/actions/workflows/ci.yml/badge.svg)
+[![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](LICENSE)
+
 ## Installation
 Either install via `pip` through
 ```
@@ -7,7 +12,7 @@ or, for development, clone the repository and install in editable mode using
 ```
 $ python -m venv venv
 $ source venv/bin/activate
-$ pip install -e .
+$ pip install -e ".[dev]"
 ```
 
 ## Usage

examples/.gitkeep

@@ -5,33 +5,31 @@ build-backend = "hatchling.build"
 
 [project]
 name = "pymatlie"
-version = "0.0.1a0"
-authors = [
-  { name="Luis Marques", email="lmarques@umich.edu" },
-]
+version = "0.0.1a1"
+authors = [{ name = "Luis Marques", email = "lmarques@umich.edu" }]
 description = "pymatlie provides an implementation of Matrix Lie Groups (MLG) used in robotics."
 readme = "README.md"
 requires-python = ">=3.9"
 classifiers = [
-    "Programming Language :: Python :: 3",
-    "Operating System :: OS Independent",
+  "Programming Language :: Python :: 3",
+  "Operating System :: OS Independent",
 ]
 license = "MIT"
 license-files = ["LICEN[CS]E*"]
-dependencies = [
-  "torch>=2.1",
-]
+dependencies = ["torch>=2.1"]
 
 
 [project.optional-dependencies]
 dev = [
-    "black",
-    "docformatter",
-    "isort",
-    "mypy",
-    "pylint>=2.14.5",
-    "pytest-pylint>=0.18.0",
-    "pytest>=7.2.2",
+  "black",
+  "docformatter",
+  "isort",
+  "mypy",
+  "pylint>=2.14.5",
+  "pytest-pylint>=0.18.0",
+  "pytest>=7.2.2",
+  "pyyaml",
+  "lark",
 ]
 
 [project.urls]
@@ -57,4 +55,4 @@ exclude = ["venv/*"]
 
 [[tool.mypy.overrides]]
 module = ["matplotlib.*"]
-ignore_missing_imports = true
+ignore_missing_imports = true

pyproject.toml

@@ -2,7 +2,7 @@
 
 from abc import ABC, abstractmethod
 from dataclasses import dataclass, field
-from typing import Optional, Tuple
+from typing import Tuple
 
 import torch
 
@@ -44,7 +44,7 @@ def __post_init__(self):
         assert self.B.shape == (self.g_dim, self.u_dim), f"dynamics_step: B must be ({self.g_dim}, {self.u_dim}), got {self.B.shape}"
         object.__setattr__(self, "B_T", self.B.T)
 
-    def project_to_motion_constraints(self, g: torch.Tensor, xi: torch.Tensor) -> torch.Tensor:
+    def project_to_motion_constraints(self, _g: torch.Tensor, xi: torch.Tensor) -> torch.Tensor:
         """Project the state to the motion constraints."""
         return xi
 
@@ -189,8 +189,9 @@ def right_minus(cls, g_start: torch.Tensor, g_end: torch.Tensor) -> torch.Tensor
     def right_invariant_error(cls, estimated_state: torch.Tensor, true_state: torch.Tensor) -> torch.Tensor:
         """Computes the right invariant error between the estimated state and
         the true state."""
-        assert estimated_state.shape == true_state.shape, f"right_invariant_error: mismatched shapes {estimated_state.shape} vs {true_state.shape}"
-        return estimated_state @ cls.inverse(true_state)
+        # assert estimated_state.shape == true_state.shape, f"right_invariant_error: mismatched shapes {estimated_state.shape} vs {true_state.shape}"
+        # return estimated_state @ cls.inverse(true_state)
+        return true_state @ cls.inverse(estimated_state)
 
     @classmethod
     def left_invariant_error(cls, estimated_state: torch.Tensor, true_state: torch.Tensor) -> torch.Tensor:
@@ -199,7 +200,7 @@ def left_invariant_error(cls, estimated_state: torch.Tensor, true_state: torch.T
         assert estimated_state.shape == true_state.shape, f"right_invariant_error: mismatched shapes {estimated_state.shape} vs {true_state.shape}"
         return cls.inverse(true_state) @ estimated_state
 
-    def f(self, g: torch.Tensor, xi: torch.Tensor, u: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]:
+    def f(self, _g: torch.Tensor, xi: torch.Tensor, u: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]:
         """Update using Euler-Poincare equations."""
         D = self.g_dim
         assert xi.ndim == 2 and xi.shape[1] == D, f"xi must be (N, {D}), got {xi.shape}"
@@ -232,36 +233,44 @@ def update_configuration(self, g: torch.Tensor, xi: torch.Tensor, dt: float) ->
 
     def update_velocity(self, xi: torch.Tensor, dxi: torch.Tensor, dt: float) -> torch.Tensor:
         """Updates the velocity (Lie algebra element xi) using the Lie algebra
-        element dxi.
-        """
+        element dxi."""
         assert xi.ndim == 2 and xi.shape[-1] == self.g_dim, f"update_velocity: xi must be (N, {self.g_dim}), got {xi.shape}"
         assert dxi.ndim == 2 and dxi.shape[-1] == self.g_dim, f"update_velocity: dxi must be (N, {self.g_dim}), got {dxi.shape}"
         return xi + dxi * dt
 
+    @staticmethod
     @abstractmethod
     def map_q_to_configuration(q: torch.Tensor) -> torch.Tensor:
         """Map the configuration vector to the Lie group element."""
         raise NotImplementedError
 
+    @staticmethod
     @abstractmethod
     def map_configuration_to_q(g: torch.Tensor) -> torch.Tensor:
         """Map the Lie Group element to configuration space."""
         raise NotImplementedError
 
+    @staticmethod
     @abstractmethod
     def map_dq_to_velocity(q: torch.Tensor, dq: torch.Tensor) -> torch.Tensor:
-        """ Map the velocity in configuration space to the Lie Algebra velocity."""
+        """Map the velocity in configuration space to the Lie Algebra
+        velocity."""
         raise NotImplementedError
 
+    @staticmethod
     @abstractmethod
     def map_velocity_to_dq(q: torch.Tensor, velocity: torch.Tensor) -> torch.Tensor:
-        """ Map the velocity in Lie Algebra to the configuration space velocity."""
+        """Map the velocity in Lie Algebra to the configuration space
+        velocity."""
         raise NotImplementedError
 
+
+@dataclass(frozen=True)
 class NonholonomicGroup(MatrixLieGroup):
     """Base class for nonholonomic matrix Lie groups."""
 
-    constraint_projection_matrix: Optional[torch.Tensor] = field(init=False, repr=False)  # Enforces A @ xi = 0 constraint
+    constraint_projection_matrix_velocity: torch.Tensor = field(init=False, repr=False)  # Enforces A @ xi = 0 constraint
+    constraint_projection_matrix_wrench: torch.Tensor = field(init=False, repr=False)  # Enforces
 
     def __init__(self, *args, **kwargs):
         super().__init__(*args, **kwargs)
@@ -276,25 +285,29 @@ def __init__(self, *args, **kwargs):
         P = self.inertia_matrix_inv @ A_matrix.T @ lambda_solver @ A_matrix  # inertia_matrix^-1 @ A^T @ (A @ inertia_matrix^-1 @ A^T)^-1 @ A
         I_minus_P = torch.eye(self.g_dim, device=self.inertia_matrix.device) - P
 
-        object.__setattr__(self, "constraint_projection_matrix", I_minus_P.T)
+        PI = (
+            torch.eye(self.g_dim, device=self.inertia_matrix.device)
+            - A_matrix.T @ torch.linalg.inv(A_matrix @ self.inertia_matrix_inv @ A_matrix.T) @ A_matrix @ self.inertia_matrix_inv
+        )
+
+        object.__setattr__(self, "constraint_projection_matrix_velocity", I_minus_P)  # Storing transpose for batch multiplication purposes
+        object.__setattr__(self, "constraint_projection_matrix_wrench", PI)
 
-    def project_to_motion_constraints(self, g: torch.Tensor, xi: torch.Tensor) -> torch.Tensor:
+    def project_to_motion_constraints(self, _g: torch.Tensor, xi: torch.Tensor) -> torch.Tensor:
         """Project the state to the motion constraints."""
-        return xi @ self.constraint_projection_matrix
+        return xi @ self.constraint_projection_matrix_velocity.T
 
     @abstractmethod
-    def get_Pfaffian_A(self, g:torch.Tensor, xi: torch.Tensor) -> torch.Tensor:
+    def get_Pfaffian_A(self, g: torch.Tensor, xi: torch.Tensor) -> torch.Tensor:
         """Computes the Pfaffian A of the nonholonomic group."""
         raise NotImplementedError
-    # TODO: can this be made a property on subclass if they are constant???
 
     def f(self, g: torch.Tensor, xi: torch.Tensor, u: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]:
         """Update using Euler-Poincare equations."""
         # _, xi_dot = super().f(g, xi, u)
         D = self.g_dim
         assert xi.ndim == 2 and xi.shape[1] == D, f"xi must be (N, {D}), got {xi.shape}"
         assert u.shape[1] == self.u_dim and u.ndim == 2, f"u must be (N, {self.u_dim}), got {u.shape}"
-        # TODO: clean this and holonomic to use same interface as state-space dynamics (i.e. get forces, ...)
         Bu = u @ self.B_T  # Bu in batched form
 
         coad = self.coadjoint_operator(xi)  # (N, D, D)
@@ -303,4 +316,4 @@ def f(self, g: torch.Tensor, xi: torch.Tensor, u: torch.Tensor) -> Tuple[torch.T
         xi_dot = (self.inertia_matrix_inv @ rhs.T).T
         xi_dot_proj = self.project_to_motion_constraints(g, xi_dot)
 
-        return xi, xi_dot_proj
+        return xi, xi_dot_proj

src/pymatlie/base_group.py

@@ -4,6 +4,7 @@
 from typing import Tuple
 
 import torch
+
 from pymatlie.base_group import MatrixLieGroup
 from pymatlie.so2 import SO2
 
@@ -53,7 +54,6 @@ def logm(g: torch.Tensor) -> torch.Tensor:
         tau_hat[..., :2, 2] = rho.squeeze(-1)  # Set the translation part
         return tau_hat
 
-    #TODO: change back to this
     # # @torch.compile
     # @staticmethod
     # def exp(tau: torch.Tensor) -> torch.Tensor:
@@ -93,29 +93,28 @@ def left_jacobian(cls, tau: torch.Tensor) -> torch.Tensor:
         jac[..., 1, 2] = (theta * y - x + x * torch.cos(theta) - y * torch.sin(theta)) / (theta**2 + 1e-15)
         return jac
 
-    # TODO: check this
     @classmethod
     def left_jacobian_inverse(cls, tau: torch.Tensor) -> torch.Tensor:
-        """
-        Analytic inverse of the left‐Jacobian J(τ) for SE(2).
-        tau is shape (B,3) = [ρ_x, ρ_y, φ].
-        Returns J(τ)^{-1}, shape (B,3,3).
+        """Analytic inverse of the left‐Jacobian J(τ) for SE(2).
+
+        tau is shape (B,3) = [ρ_x, ρ_y, φ]. Returns J(τ)^{-1}, shape
+        (B,3,3).
         """
         assert tau.ndim == 2 and tau.shape[1] == 3
         B = tau.shape[0]
-        φ    = tau[:, 2]                        # (B,)
+        phi = tau[:, 2]  # (B,)
         # 1) invert the 2×2 SO(2) left‐Jacobian on φ
-        J2_inv = SO2.left_jacobian_inverse(φ.unsqueeze(-1))  # (B,2,2)
+        J2_inv = SO2.left_jacobian_inverse(phi.unsqueeze(-1))  # (B,2,2)
 
         # 2) grab the ɡ terms from your existing left_jacobian
-        full_J = cls.left_jacobian(tau)         # (B,3,3)
-        trans = full_J[:, :2, 2]                # (B,2)
+        full_J = cls.left_jacobian(tau)  # (B,3,3)
+        trans = full_J[:, :2, 2]  # (B,2)
 
         # 3) build the inverse: block‐upper triangular
         inv = torch.zeros((B, 3, 3), device=tau.device, dtype=tau.dtype)
         inv[:, :2, :2] = J2_inv
-        inv[:, :2,  2] = -(J2_inv @ trans.unsqueeze(-1)).squeeze(-1)
-        inv[:,  2,  2] = 1.0
+        inv[:, :2, 2] = -(J2_inv @ trans.unsqueeze(-1)).squeeze(-1)
+        inv[:, 2, 2] = 1.0
         return inv
 
     @staticmethod
@@ -161,18 +160,18 @@ def compute_twist_map_inverse(q: torch.Tensor) -> torch.Tensor:
         """Compute J⁻¹(q)=J(q)^T mapping ξ→q̇ without any pinv."""
         J = SE2.compute_twist_map(q)
         return J.transpose(-2, -1)
-    
+
     @staticmethod
     def map_q_to_configuration(q: torch.Tensor) -> torch.Tensor:
-        assert q.ndim == 2 and q.shape[-1] == SE2.g_dim, f"euclidean_vector_to_g requires shape (N, {SE2.g_dim}), got {q.shape}"
+        assert q.ndim == 2 and q.shape[-1] == SE2.g_dim, f"map_q_to_configuration requires shape (N, {SE2.g_dim}), got {q.shape}"
         g = torch.eye(3, device=q.device).repeat(*q.shape[:-1], 1, 1)
         g[..., 0:2, 2] = q[..., :2]
         g[..., :2, :2] = SO2.exp(q[..., 2].unsqueeze(-1))
         return g
-    
+
     @staticmethod
     def map_configuration_to_q(g: torch.Tensor) -> torch.Tensor:
-        assert g.ndim == 3 and g.shape[-2:] == SE2.matrix_size, f"g_to_euclidean_vector requires shape (N, {SE2.matrix_size}), got {g.shape}"
+        assert g.ndim == 3 and g.shape[-2:] == SE2.matrix_size, f"map_configuration_to_q requires shape (N, {SE2.matrix_size}), got {g.shape}"
         theta = torch.atan2(g[..., 1, 0], g[..., 0, 0])
         x = g[..., 0, 2]  # (N,)
         y = g[..., 1, 2]  # (N,)
@@ -186,12 +185,12 @@ def map_dq_to_velocity(q: torch.Tensor, dq: torch.Tensor) -> torch.Tensor:
         xi = torch.bmm(J_twist, dq.unsqueeze(-1)).squeeze(-1)  # (N, g_dim)
         # assert xi.shape == (dq.shape[0], self.g_dim), f"xi shape mismatch: expected {(dq.shape[0], self.g_dim)}, got {xi.shape}"
         return xi
-    
+
     @staticmethod
-    def map_velocity_to_dq(q: torch.Tensor, vel: torch.Tensor) -> torch.Tensor:
+    def map_velocity_to_dq(q: torch.Tensor, velocity: torch.Tensor) -> torch.Tensor:
         assert q.ndim == 2 and q.shape[1] == SE2.g_dim, f"map_velocity_to_dq requires shape (N, {SE2.g_dim}), got {q.shape}"
-        assert vel.ndim == 2 and vel.shape[1] == SE2.g_dim, f"velocity_to_dq requires shape (N, {SE2.g_dim}), got {vel.shape}"
+        assert velocity.ndim == 2 and velocity.shape[1] == SE2.g_dim, f"velocity_to_dq requires shape (N, {SE2.g_dim}), got {velocity.shape}"
         J_twist_inv = SE2.compute_twist_map_inverse(q)
-        dq = torch.bmm(J_twist_inv, vel.unsqueeze(-1)).squeeze(-1)
+        dq = torch.bmm(J_twist_inv, velocity.unsqueeze(-1)).squeeze(-1)
         # assert dq.shape == (vel.shape[0], self.g_dim), f"dq shape mismatch: expected {(N, self.g_dim)}, got {dq.shape}"
-        return dq
+        return dq

src/pymatlie/se2.py

@@ -1,8 +1,10 @@
 """SO(2) Lie group implementation."""
 
 import torch
+
 from pymatlie.base_group import MatrixLieGroup
-from pymatlie.vecops import sincu, versine_over_x, sinc_taylor, versine_over_x_taylor
+from pymatlie.vecops import sincu, versine_over_x
+
 
 class SO2(MatrixLieGroup):
     """SO(2) Lie group implementation (batch-only)."""
@@ -67,17 +69,16 @@ def left_jacobian(tau: torch.Tensor) -> torch.Tensor:
         tau = tau[..., 0]  # (N,)
         I = torch.eye(2, dtype=tau.dtype, device=tau.device)  # (2, 2)
         skew = SO2.skew_sym().to(tau.device).to(tau.dtype)  # (2, 2)
-        # TODO: compare with torch.sincu and torch.versine_over_x (from vecops...)
-        return sinc_taylor(tau)[..., None, None] * I + versine_over_x_taylor(tau)[..., None, None] * skew
+        return sincu(tau)[..., None, None] * I + versine_over_x(tau)[..., None, None] * skew
 
     @staticmethod
     def left_jacobian_inverse(tau: torch.Tensor) -> torch.Tensor:
         """Inverse of left Jacobian of SO(2), input shape (N, 1), output (N, 2,
         2)."""
         assert tau.ndim == 2 and tau.shape[-1] == SO2.g_dim, "left_jacobian_inverse requires shape (N, 1)"
         tau = tau[..., 0]
-        A = sinc_taylor(tau)  # (N,)
-        B = versine_over_x_taylor(tau)  # (N,)
+        A = sincu(tau)  # (N,)
+        B = versine_over_x(tau)  # (N,)
         denom = A**2 + B**2  # (N,)
 
         row0 = torch.stack([A, B], dim=-1)  # (N, 2)
@@ -91,13 +92,13 @@ def adjoint_matrix(g: torch.Tensor) -> torch.Tensor:
         return torch.eye(2, dtype=g.dtype, device=g.device).repeat(g.shape[0], 1, 1)
 
     @staticmethod
-    def g_to_euclidean_vector(g: torch.Tensor) -> torch.Tensor:
+    def map_configuration_to_q(g: torch.Tensor) -> torch.Tensor:
         """Converts a Lie group element (SO(2) matrix) to a Lie algebra
         element."""
         return SO2.log(g)
 
     @staticmethod
-    def euclidean_vector_to_g(x):
+    def map_q_to_configuration(q: torch.Tensor) -> torch.Tensor:
         """Converts a Lie algebra element (SO(2) vector) to a Lie group
         element."""
-        return SO2.exp(x)
+        return SO2.exp(q)

src/pymatlie/so2.py

@@ -14,23 +14,3 @@ def versine_over_x(x: torch.Tensor) -> torch.Tensor:
     versine_over_x(x) = (1 - cos(x)) / x
     """
     return x.new_tensor(0.5) * x * torch.square(sincu(x / x.new_tensor(2.0)))
-
-
-def sinc_taylor(phi, eps=1e-4):
-    # phi: (...,) tensor
-    small = phi.abs() < eps
-    out   = torch.empty_like(phi)
-    # far from zero: use standard
-    out[~small] = torch.sin(phi[~small]) / phi[~small]
-    # near zero: 1 - φ²/6 + φ⁴/120
-    x = phi[small]
-    out[small] = 1 - x**2/6 + x**4/120
-    return out
-
-def versine_over_x_taylor(phi, eps=1e-4):
-    small = phi.abs() < eps
-    out   = torch.empty_like(phi)
-    out[~small] = (1 - torch.cos(phi[~small])) / phi[~small]
-    x = phi[small]
-    out[small] = x/2 - x**3/24 + x**5/720
-    return out