nb-touch 0.1.0__py3-none-any.whl

nb_touch/__init__.py

@@ -0,0 +1,5 @@
+"""Hardware-agnostic processing and calibration of taxel array touch sensors."""
+
+from importlib.metadata import version
+
+__version__ = version("nb-touch")

nb_touch/calibration/__init__.py

@@ -0,0 +1,9 @@
+"""Taxel orientation calibration from joint torques."""
+
+from nb_touch.calibration.orientation import (
+    CalibrationResult,
+    CalibrationSettings,
+    calibrate_orientations,
+)
+
+__all__ = ["CalibrationResult", "CalibrationSettings", "calibrate_orientations"]

nb_touch/calibration/orientation.py

@@ -0,0 +1,173 @@
+"""Taxel orientation calibration of Pan et al. 2026, Section 3.3: the taxel orientations
+`R_i` that best explain the recorded joint torques. See
+`docs/theory/taxel-orientation.md`.
+"""
+
+from dataclasses import dataclass, field
+
+import numpy as np
+import torch
+
+from nb_touch.calibration.samples import Jacobian, calibration_samples
+from nb_touch.contact.contacts import ContactSettings
+from nb_touch.geometry.frames import (
+    normals_from_orientations,
+    with_estimated_orientations,
+)
+from nb_touch.geometry.so3 import project_to_so3, project_to_so3_torch
+from nb_touch.io.recordings import Recording
+from nb_touch.io.sensor_geometry import SensorGeometry
+
+LEARNING_RATE = 0.1
+"""Adam learning rate, from the paper (Appendix A)."""
+
+STEPS = 100
+"""Adam steps, from the paper (Appendix A)."""
+
+
+@dataclass(frozen=True)
+class CalibrationSettings:
+    """Settings of the taxel orientation calibration.
+
+    Attributes:
+        contact: Active taxel threshold, fewest active taxels per sample, and CoP
+            model σ and λ.
+        learning_rate: Adam learning rate.
+        steps: Adam steps.
+    """
+
+    contact: ContactSettings = field(default_factory=ContactSettings)
+    learning_rate: float = LEARNING_RATE
+    steps: int = STEPS
+
+
+@dataclass(frozen=True)
+class CalibrationResult:
+    """Outcome of the calibration.
+
+    Attributes:
+        geometry: The sensor geometry with the calibrated orientations.
+        losses: `(steps + 1,)` torque MSE, in (N·m)^2, before each step and at the
+            end.
+        num_samples: Number of samples with enough active taxels, used in the fit.
+    """
+
+    geometry: SensorGeometry
+    losses: np.ndarray
+    num_samples: int
+
+
+def predict_torques(orientations: torch.Tensor, samples: dict) -> torch.Tensor:
+    """Return the predicted joint torques `τ = -J^T f_cop` of every sample, `(S, J)`.
+
+    The paper's static equilibrium, gravity neglected: `f_cop` is the force acting
+    on the sensor, and `τ` the torques the joints apply to hold the finger still.
+
+    Args:
+        orientations: Taxel orientations `(T, 3, 3)`, taxel frame to sensor frame.
+        samples: `CalibrationSamples` fields as tensors, from `samples_to_tensors`.
+    """
+    rotated = torch.einsum(
+        "eij,ej->ei", orientations[samples["taxel_index"]], samples["forces"]
+    )
+    parts = torch.einsum("eij,ej->ei", samples["operators"], rotated)
+    num_samples = len(samples["torques"])
+    cop_forces = parts.new_zeros((num_samples, 3))
+    cop_forces = cop_forces.index_add(0, samples["sample_index"], parts)
+    return -torch.einsum("sij,si->sj", samples["jacobians"], cop_forces)
+
+
+def torque_loss(orientations: torch.Tensor, samples: dict) -> torch.Tensor:
+    """Return the mean squared error between predicted and recorded joint torques."""
+    return torch.mean(
+        (predict_torques(orientations, samples) - samples["torques"]) ** 2
+    )
+
+
+def samples_to_tensors(samples) -> dict:
+    """Return the fields of `CalibrationSamples` as float64 or index tensors."""
+    return {
+        name: (
+            torch.as_tensor(value)
+            if name.endswith("_index")
+            else torch.as_tensor(value, dtype=torch.float64)
+        )
+        for name, value in vars(samples).items()
+    }
+
+
+def fit_orientations(
+    prior, samples: dict, settings: CalibrationSettings
+) -> tuple[np.ndarray, np.ndarray]:
+    """Fit free 3×3 matrices, projected to rotations, with Adam.
+
+    Args:
+        prior: Initial orientations `(T, 3, 3)`.
+        samples: From `samples_to_tensors`.
+        settings: `learning_rate` and `steps` are used.
+
+    Returns:
+        `(orientations, losses)`: orientations `(T, 3, 3)` and the loss before each step
+        and at the end, `(steps + 1,)`.
+    """
+    matrices = torch.tensor(prior, dtype=torch.float64, requires_grad=True)
+    optimiser = torch.optim.Adam([matrices], lr=settings.learning_rate)
+    losses = []
+    for _ in range(settings.steps):
+        optimiser.zero_grad()
+        loss = torque_loss(project_to_so3_torch(matrices), samples)
+        loss.backward()
+        optimiser.step()
+        losses.append(loss.item())
+    orientations = project_to_so3(matrices.detach().numpy())
+    with torch.no_grad():
+        losses.append(torque_loss(torch.as_tensor(orientations), samples).item())
+    return orientations, np.array(losses)
+
+
+def calibrate_orientations(
+    recording: Recording,
+    geometry: SensorGeometry,
+    jacobian: Jacobian,
+    link: str | None = None,
+    settings: CalibrationSettings = CalibrationSettings(),
+) -> CalibrationResult:
+    """Estimate the taxel orientations from a recording of random fingertip contacts.
+
+    The prior orientations (from the geometry, or estimated from the positions if
+    missing) are the starting point and give the normals of the CoP model. Taxels
+    never active keep their prior orientation.
+
+    Args:
+        recording: Taxel forces in their taxel frames, joint angles and torques.
+        geometry: Sensor geometry, with or without orientations.
+        jacobian: `jacobian(q, link, point) -> (3, J)`, see `Jacobian`.
+        link: Link the sensor is mounted on, passed to `jacobian`.
+        settings: Calibration settings.
+
+    Raises:
+        ValueError: If the recording and geometry disagree on the taxel count, or
+            no sample has enough active taxels.
+    """
+    if recording.num_taxels != geometry.num_taxels:
+        raise ValueError(
+            f"recording has {recording.num_taxels} taxels, "
+            f"geometry {geometry.num_taxels}"
+        )
+    prior = with_estimated_orientations(geometry)
+    samples = calibration_samples(
+        recording,
+        prior.positions,
+        normals_from_orientations(prior.orientations),
+        jacobian,
+        link,
+        settings.contact,
+    )
+    orientations, losses = fit_orientations(
+        prior.orientations, samples_to_tensors(samples), settings
+    )
+    return CalibrationResult(
+        geometry=SensorGeometry(geometry.name, geometry.positions, orientations),
+        losses=losses,
+        num_samples=samples.num_samples,
+    )

nb_touch/calibration/samples.py

@@ -0,0 +1,138 @@
+"""Everything of a recording the taxel orientation calibration needs, computed once:
+the parts of the CoP model that do not depend on the taxel orientations.
+
+The CoP position uses the force norms only and the normals come from the prior
+orientations, so the CoP, the Jacobian at it and the ridge solve are fixed: only the
+taxel forces in the sensor frame, `R_i f_i`, change with the orientations. See
+`docs/theory/taxel-orientation.md`.
+"""
+
+from collections.abc import Callable
+from dataclasses import dataclass
+
+import numpy as np
+
+from nb_touch.contact.active import active_taxels
+from nb_touch.contact.contacts import ContactSettings
+from nb_touch.contact.cop_model import (
+    cop_force_operator,
+    cop_normal,
+    cop_position,
+    taxel_force_matrices,
+)
+from nb_touch.io.recordings import Recording
+
+Jacobian = Callable[[np.ndarray, str | None, np.ndarray], np.ndarray]
+"""`jacobian(q, link, point) -> (3, J)`: position Jacobian of `point` (sensor frame,
+in m) on `link` at joint angles `q` `(J,)`, expressed in the sensor frame."""
+
+
+@dataclass(frozen=True)
+class CalibrationSamples:
+    """The samples used by the calibration, E active taxels over S samples.
+
+    Attributes:
+        sample_index: `(E,)` sample of each active taxel, in `0..S-1`.
+        taxel_index: `(E,)` taxel of each active taxel.
+        operators: `(E, 3, 3)` ridge solve `K_i`, so the CoP force of a sample is
+            the sum of `K_i R_i f_i` over its active taxels.
+        forces: `(E, 3)` taxel forces `f_i`, in their taxel frames, in N.
+        jacobians: `(S, 3, J)` Jacobians at the CoPs, in the sensor frame.
+        torques: `(S, J)` recorded joint torques, in N·m.
+    """
+
+    sample_index: np.ndarray
+    taxel_index: np.ndarray
+    operators: np.ndarray
+    forces: np.ndarray
+    jacobians: np.ndarray
+    torques: np.ndarray
+
+    @property
+    def num_samples(self) -> int:
+        """Number of samples S used."""
+        return len(self.torques)
+
+
+def checked_jacobian(jacobian, num_joints: int) -> np.ndarray:
+    """Return `jacobian` as an array, raising a `ValueError` unless it is `(3, J)`."""
+    jacobian = np.asarray(jacobian, dtype=float)
+    if jacobian.shape != (3, num_joints):
+        raise ValueError(f"expected a (3, {num_joints}) Jacobian, got {jacobian.shape}")
+    return jacobian
+
+
+def sample_cop(positions, normals, forces, settings: ContactSettings):
+    """Return the active taxels, CoP and ridge solve of one sample.
+
+    Args:
+        positions: Taxel positions `(T, 3)`, in m.
+        normals: Unit outward prior normals `(T, 3)`.
+        forces: Taxel forces `(T, 3)`, in their taxel frames, in N.
+        settings: `threshold`, `min_taxels`, `sigma` and `regularisation` are used.
+
+    Returns:
+        `(taxels, cop, operators)`, or `None` if fewer than `min_taxels` taxels are
+        active or the normal at the CoP is undefined.
+    """
+    settings = settings.from_geometry(positions)
+    taxels = np.flatnonzero(active_taxels(forces, settings.threshold))
+    if len(taxels) < max(settings.min_taxels, 1):
+        return None
+    points, zone_normals = positions[taxels], normals[taxels]
+    cop = cop_position(points, forces[taxels])
+    normal = cop_normal(points, zone_normals, cop)
+    if np.isnan(normal).any():
+        return None
+    matrices = taxel_force_matrices(points, zone_normals, cop, normal, settings.sigma)
+    return taxels, cop, cop_force_operator(matrices, settings.regularisation)
+
+
+def calibration_samples(
+    recording: Recording,
+    positions,
+    normals,
+    jacobian: Jacobian,
+    link: str | None = None,
+    settings: ContactSettings = ContactSettings(),
+) -> CalibrationSamples:
+    """Prepare the samples of a recording for the calibration.
+
+    Args:
+        recording: Taxel forces, joint angles and joint torques.
+        positions: Taxel positions `(T, 3)`, in m.
+        normals: Unit outward prior normals `(T, 3)`.
+        jacobian: See `Jacobian`.
+        link: Link the sensor is mounted on, passed to `jacobian`.
+        settings: Active taxels and CoP model settings.
+
+    Raises:
+        ValueError: If no sample has enough active taxels.
+    """
+    positions = np.asarray(positions, dtype=float)
+    normals = np.asarray(normals, dtype=float)
+    settings = settings.from_geometry(positions)
+    entries, jacobians, torques = [], [], []
+    for forces, q, tau in zip(
+        recording.taxel_forces_n,
+        recording.joint_pos_rad,
+        recording.joint_torque_nm,
+        strict=True,
+    ):
+        found = sample_cop(positions, normals, forces, settings)
+        if found is None:
+            continue
+        taxels, cop, operators = found
+        entries.append((len(torques), taxels, operators, forces[taxels]))
+        jacobians.append(checked_jacobian(jacobian(q, link, cop), len(q)))
+        torques.append(tau)
+    if not torques:
+        raise ValueError("no sample has enough active taxels")
+    return CalibrationSamples(
+        sample_index=np.concatenate([np.full(len(e[1]), e[0]) for e in entries]),
+        taxel_index=np.concatenate([e[1] for e in entries]),
+        operators=np.concatenate([e[2] for e in entries]),
+        forces=np.concatenate([e[3] for e in entries]),
+        jacobians=np.array(jacobians),
+        torques=np.array(torques),
+    )

nb_touch/calibration/synthetic.py

@@ -0,0 +1,158 @@
+"""Synthetic finger and recordings with known taxel orientations, for tests and demos.
+
+The finger is held still, as in the paper's data collection (a stiff PD controller
+holds the joints while an object is pressed on the fingertip), so its Jacobian does
+not depend on the joint angles.
+"""
+
+import numpy as np
+
+from nb_touch.calibration.samples import sample_cop
+from nb_touch.contact.contacts import ContactSettings
+from nb_touch.contact.cop_model import (
+    cop_normal,
+    cop_to_taxel_forces,
+    taxel_force_matrices,
+)
+from nb_touch.geometry.frames import normals_from_orientations
+from nb_touch.geometry.transforms import rotate_vectors, rotations_from_vectors
+from nb_touch.io.recordings import Recording
+from nb_touch.io.sensor_geometry import SensorGeometry
+
+FINGER_AXES = np.array(
+    [[0.0, 0.0, 1.0], [1.0, 0.0, 0.0], [1.0, 0.0, 0.0], [1.0, 0.0, 0.0]]
+)
+"""Unit joint axes of the synthetic 4-joint finger, in the sensor frame: abduction,
+then three flexion joints, like a human finger."""
+
+FINGER_ORIGINS = np.array(
+    [
+        [0.0, -0.1, -0.005],
+        [0.0, -0.1, -0.005],
+        [0.0, -0.055, -0.005],
+        [0.0, -0.02, -0.005],
+    ]
+)
+"""Points on the joint axes, in the sensor frame, in m: the finger runs along -y
+from the sensor."""
+
+RATE_HZ = 20.0
+"""Sampling rate of the synthetic recordings, as in the paper (Appendix A)."""
+
+
+def revolute_jacobian(axes, origins, point) -> np.ndarray:
+    """Return the position Jacobian `(3, J)` of `point` for revolute joints: column
+    `j` is `a_j x (p - o_j)`.
+
+    Args:
+        axes: Unit joint axes `(J, 3)`.
+        origins: A point on each axis `(J, 3)`, in m.
+        point: The point `(3,)`, in m, in the same frame.
+    """
+    offsets = np.asarray(point, dtype=float) - np.asarray(origins, dtype=float)
+    return np.cross(np.asarray(axes, dtype=float), offsets).T
+
+
+def finger_jacobian(q, link, point) -> np.ndarray:
+    """Jacobian `(3, 4)` of the synthetic finger at `point`, in the sensor frame.
+
+    `q` and `link` are ignored: the finger is held still.
+    """
+    return revolute_jacobian(FINGER_AXES, FINGER_ORIGINS, point)
+
+
+def perturbed_orientations(orientations, max_angle: float, rng) -> np.ndarray:
+    """Return each orientation `(n, 3, 3)` turned about a random axis of its own frame,
+    by a random angle up to `max_angle`, in rad.
+    """
+    count = len(orientations)
+    axes = rng.normal(size=(count, 3))
+    axes /= np.linalg.norm(axes, axis=1, keepdims=True)
+    angles = rng.uniform(0.0, max_angle, size=(count, 1))
+    return np.asarray(orientations) @ rotations_from_vectors(axes * angles)
+
+
+def random_cop_force(normal, normal_range, max_slip: float, rng) -> np.ndarray:
+    """Return a push against the outward `normal` of norm in `normal_range`, in N,
+    plus a shear across it in a random direction, up to `max_slip` times the push."""
+    push = rng.uniform(*normal_range)
+    shear = rng.normal(size=3)
+    shear -= (shear @ normal) * normal
+    shear *= rng.uniform(0.0, max_slip) * push / np.linalg.norm(shear)
+    return -push * normal + shear
+
+
+def model_torques(positions, normals, forces, settings: ContactSettings):
+    """Return the joint torques `-J^T f_cop` `(4,)` of the synthetic finger, with the
+    CoP and CoP force estimated from the taxel forces, as the calibration does.
+
+    Args:
+        positions: Taxel positions `(T, 3)`, in m.
+        normals: Unit outward taxel normals `(T, 3)`.
+        forces: Taxel forces `(T, 3)`, in the sensor frame, in N.
+        settings: Active taxels and CoP model settings.
+
+    Returns:
+        The torques, zero if the sample has too few active taxels.
+    """
+    found = sample_cop(positions, normals, forces, settings)
+    if found is None:
+        return np.zeros(len(FINGER_AXES))
+    taxels, cop, operators = found
+    cop_force = np.einsum("nij,nj->i", operators, forces[taxels])
+    return -finger_jacobian(None, None, cop).T @ cop_force
+
+
+def synthetic_recording(
+    geometry: SensorGeometry,
+    orientations,
+    num_samples: int,
+    rng,
+    normal_range=(0.5, 3.0),
+    max_slip: float = 0.5,
+    force_noise: float = 0.0,
+    torque_noise: float = 0.0,
+    settings: ContactSettings = ContactSettings(),
+) -> Recording:
+    """Simulate random contacts on the fingertip of the synthetic finger.
+
+    Each sample presses at a random taxel: the CoP model turns the CoP force into
+    taxel forces with the geometry's normals, and `orientations` take them to the
+    taxel frames. The joint torques, `-J^T f_cop`, come from the CoP and CoP force
+    estimated back from the taxel forces (`model_torques`), so with the true
+    orientations the calibration's model explains them exactly, without the bias of
+    the estimate (`docs/theory/cop.md`, Known bias).
+
+    Args:
+        geometry: Sensor geometry with orientations, giving the normals.
+        orientations: True taxel orientations `(T, 3, 3)`, taxel frame to sensor frame.
+        num_samples: Number of samples N.
+        rng: A `numpy.random.Generator`.
+        normal_range: Smallest and largest push, in N.
+        max_slip: Largest shear over push.
+        force_noise: Standard deviation of the noise on each force component, N.
+        torque_noise: Standard deviation of the noise on each torque, in N·m.
+        settings: Active taxels and CoP model settings, the same as the
+            calibration's.
+    """
+    positions = geometry.positions
+    normals = normals_from_orientations(geometry.orientations)
+    settings = settings.from_geometry(positions)
+    forces, torques = [], []
+    for taxel in rng.integers(len(positions), size=num_samples):
+        cop = positions[taxel]
+        normal = cop_normal(positions, normals, cop)
+        cop_force = random_cop_force(normal, normal_range, max_slip, rng)
+        matrices = taxel_force_matrices(positions, normals, cop, normal, settings.sigma)
+        sensor_forces = cop_to_taxel_forces(cop_force, matrices)
+        forces.append(
+            rotate_vectors(np.transpose(orientations, (0, 2, 1)), sensor_forces)
+        )
+        torques.append(model_torques(positions, normals, sensor_forces, settings))
+    forces, torques = np.array(forces), np.array(torques)
+    return Recording(
+        timestamps_s=np.arange(num_samples) / RATE_HZ,
+        joint_pos_rad=np.zeros_like(torques),
+        joint_torque_nm=torques + rng.normal(0.0, torque_noise, torques.shape),
+        taxel_forces_n=forces + rng.normal(0.0, force_noise, forces.shape),
+    )

nb_touch/contact/__init__.py

@@ -0,0 +1,27 @@
+"""Taxel forces to contacts, each with its CoP, CoP force and stats."""
+
+from nb_touch.contact.contacts import Contact, ContactSettings, find_contacts
+from nb_touch.contact.cop_model import (
+    cop_normal,
+    cop_position,
+    cop_to_taxel_forces,
+    spacing_sigma,
+    taxel_force_matrices,
+    taxel_to_cop_force,
+)
+from nb_touch.contact.detector import ContactDetector
+from nb_touch.contact.filters import make_filter
+
+__all__ = [
+    "Contact",
+    "ContactDetector",
+    "ContactSettings",
+    "cop_normal",
+    "cop_position",
+    "cop_to_taxel_forces",
+    "find_contacts",
+    "make_filter",
+    "spacing_sigma",
+    "taxel_force_matrices",
+    "taxel_to_cop_force",
+]

nb_touch/contact/active.py

@@ -0,0 +1,25 @@
+"""Which taxels are in contact in one frame.
+
+Taken from `dextHand`, `src/dexthand/touch/contact.py`.
+"""
+
+import numpy as np
+
+THRESHOLD = 0.2
+"""Taxel force norm above which a taxel is in contact, in N. Value used on the ORCA
+hand in `dextHand`."""
+
+
+def taxel_norms(forces) -> np.ndarray:
+    """Return the force norm `(n,)` of every taxel, from forces `(n, 3)`."""
+    return np.linalg.norm(np.asarray(forces, dtype=float).reshape(-1, 3), axis=1)
+
+
+def active_taxels(forces, threshold: float = THRESHOLD) -> np.ndarray:
+    """Return a mask `(n,)` of the taxels whose force norm is above `threshold`.
+
+    Args:
+        forces: Taxel forces `(n, 3)`, in N.
+        threshold: Force norm, in N.
+    """
+    return taxel_norms(forces) > threshold