133 lines
4.9 KiB
Python
133 lines
4.9 KiB
Python
"""Discrete time-domain passivity observer/controller at the master port."""
|
|
|
|
from __future__ import annotations
|
|
|
|
from dataclasses import dataclass
|
|
|
|
import numpy as np
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class POPCDiagnostics:
|
|
tau_candidate: np.ndarray
|
|
tau_applied: np.ndarray
|
|
observer_before: float
|
|
observer_preclip: float
|
|
observer_after: float
|
|
candidate_power: float
|
|
applied_power: float
|
|
damping_gain: float
|
|
intervention_active: bool
|
|
fail_safe_active: bool
|
|
|
|
|
|
class TimeDomainPOPC:
|
|
"""Causal PO/PC using dissipative velocity feedback.
|
|
|
|
Positive ``tau @ qd`` is energy delivered by the device. When the
|
|
candidate would exhaust the observer balance, the controller injects
|
|
``-beta * qd``. This is intentionally a separate baseline from the radial
|
|
tank projection implemented in :mod:`core.haptic_render`.
|
|
"""
|
|
|
|
def __init__(
|
|
self,
|
|
*,
|
|
initial_energy: float = 0.0,
|
|
minimum_energy: float = 0.0,
|
|
maximum_energy: float = np.inf,
|
|
velocity_epsilon: float = 1e-12,
|
|
):
|
|
if not np.isfinite(initial_energy) or not np.isfinite(minimum_energy):
|
|
raise ValueError("initial and minimum energy must be finite")
|
|
if maximum_energy <= minimum_energy:
|
|
raise ValueError("maximum_energy must exceed minimum_energy")
|
|
if not minimum_energy <= initial_energy <= maximum_energy:
|
|
raise ValueError("initial_energy must lie within observer bounds")
|
|
if velocity_epsilon <= 0.0:
|
|
raise ValueError("velocity_epsilon must be positive")
|
|
self.minimum_energy = float(minimum_energy)
|
|
self.maximum_energy = float(maximum_energy)
|
|
self.velocity_epsilon = float(velocity_epsilon)
|
|
self.energy = float(initial_energy)
|
|
self.last_diagnostics: POPCDiagnostics | None = None
|
|
|
|
def reset(self, energy: float | None = None) -> None:
|
|
target = self.energy if energy is None else float(energy)
|
|
if not self.minimum_energy <= target <= self.maximum_energy:
|
|
raise ValueError("reset energy lies outside observer bounds")
|
|
self.energy = target
|
|
self.last_diagnostics = None
|
|
|
|
def apply(
|
|
self,
|
|
tau_candidate: np.ndarray,
|
|
qd_master: np.ndarray,
|
|
dt: float,
|
|
) -> tuple[np.ndarray, POPCDiagnostics]:
|
|
candidate = np.asarray(tau_candidate, dtype=float).reshape(-1)
|
|
velocity = np.asarray(qd_master, dtype=float).reshape(-1)
|
|
if candidate.shape != velocity.shape or candidate.size == 0:
|
|
raise ValueError("candidate and velocity must have equal non-empty shapes")
|
|
before = self.energy
|
|
if (
|
|
not np.isfinite(dt)
|
|
or dt <= 0.0
|
|
or not np.all(np.isfinite(candidate))
|
|
or not np.all(np.isfinite(velocity))
|
|
):
|
|
applied = np.zeros_like(candidate)
|
|
diagnostics = POPCDiagnostics(
|
|
tau_candidate=candidate.copy(),
|
|
tau_applied=applied,
|
|
observer_before=before,
|
|
observer_preclip=before,
|
|
observer_after=before,
|
|
candidate_power=0.0,
|
|
applied_power=0.0,
|
|
damping_gain=0.0,
|
|
intervention_active=True,
|
|
fail_safe_active=True,
|
|
)
|
|
self.last_diagnostics = diagnostics
|
|
return applied, diagnostics
|
|
|
|
candidate_power = float(candidate @ velocity)
|
|
available = max(0.0, before - self.minimum_energy)
|
|
allowed_power = available / dt
|
|
damping_gain = 0.0
|
|
applied = candidate.copy()
|
|
velocity_norm_sq = float(velocity @ velocity)
|
|
if candidate_power > allowed_power:
|
|
if velocity_norm_sq <= self.velocity_epsilon:
|
|
# With an almost-zero velocity, nonzero power is numerical
|
|
# contamination; zero output is the conservative response.
|
|
applied.fill(0.0)
|
|
else:
|
|
damping_gain = (
|
|
candidate_power - allowed_power
|
|
) / velocity_norm_sq
|
|
applied = candidate - damping_gain * velocity
|
|
|
|
applied_power = float(applied @ velocity)
|
|
preclip = before - applied_power * dt
|
|
self.energy = float(
|
|
np.clip(preclip, self.minimum_energy, self.maximum_energy)
|
|
)
|
|
diagnostics = POPCDiagnostics(
|
|
tau_candidate=candidate.copy(),
|
|
tau_applied=applied.copy(),
|
|
observer_before=before,
|
|
observer_preclip=preclip,
|
|
observer_after=self.energy,
|
|
candidate_power=candidate_power,
|
|
applied_power=applied_power,
|
|
damping_gain=damping_gain,
|
|
intervention_active=bool(
|
|
np.any(np.abs(applied - candidate) > 1e-12)
|
|
),
|
|
fail_safe_active=False,
|
|
)
|
|
self.last_diagnostics = diagnostics
|
|
return applied, diagnostics
|