2144 lines
74 KiB
Python
2144 lines
74 KiB
Python
#!/usr/bin/env python3
|
|
"""Closed-loop bilateral simulation for the heterogeneous 7-DoF arms.
|
|
|
|
This program is deliberately a *simulation validation*, not a replacement for
|
|
the future prototype experiment. Both arms are integrated with their URDF
|
|
rigid-body dynamics. A computed-torque human proxy drives the master, a
|
|
computed-torque controller drives the slave, and a unilateral spring-damper
|
|
wall acts at a documented simulation-only TCP attached to the terminal wrist.
|
|
|
|
Three otherwise identical cases are run:
|
|
|
|
``proposed_energy``
|
|
``tau_m = A(q_m).T @ tau_s`` plus final applied-port energy supervision.
|
|
``direct_energy``
|
|
The former direct baseline ``tau_m = J_m.T @ F_s`` with the same output
|
|
shaping and energy supervision.
|
|
``proposed_no_energy``
|
|
The proposed differential mapping with the energy projection bypassed.
|
|
|
|
The comparison separates the two implementation questions: virtual-work
|
|
consistency of the retargeting map, and the final-port energy safety layer.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import csv
|
|
import json
|
|
import math
|
|
import time
|
|
from collections import deque
|
|
from dataclasses import asdict, dataclass, fields, replace
|
|
from pathlib import Path
|
|
from typing import Any, Iterable
|
|
|
|
import numpy as np
|
|
import pinocchio as pin
|
|
import yaml
|
|
|
|
from core.haptic_render import HapticRenderer
|
|
from core.interaction_estimater import InteractionEstimator
|
|
from core.feedback_protocol import (
|
|
ForwardPacket,
|
|
MapPolicy,
|
|
MapRegistry,
|
|
MapSnapshot,
|
|
MappingKind,
|
|
PacketState,
|
|
ReturnPacket,
|
|
map_return_feedback,
|
|
)
|
|
from core.network_emulator import (
|
|
DeterministicChannel,
|
|
PacketReceiver,
|
|
generate_network_trace,
|
|
)
|
|
from core.model_contract import (
|
|
MASTER_FRAMES,
|
|
MASTER_JOINT_NAMES,
|
|
MASTER_URDF,
|
|
SLAVE_FRAMES,
|
|
SLAVE_JOINT_NAMES,
|
|
SLAVE_URDF,
|
|
TeleoperationModels,
|
|
clip_configuration,
|
|
load_models,
|
|
require_frame,
|
|
)
|
|
from core.sew_mapper2 import BallJointConfig, SEWMapper
|
|
from core.time_domain_popc import TimeDomainPOPC
|
|
from core.wrench_solver import ScaledDLSSolver
|
|
|
|
|
|
DEFAULT_OUTPUT_DIR = Path(__file__).resolve().parents[1] / "output" / "simulation"
|
|
DEFAULT_CONFIG_PATH = Path(__file__).resolve().parent / "config" / "config.yaml"
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class SimulationConfig:
|
|
"""Numerical and controller parameters for the reproducible comparison."""
|
|
|
|
dt: float = 0.002
|
|
duration: float = 4.0
|
|
mapping_hz: float = 50.0
|
|
seed: int = 7
|
|
slave_contact_frame: str = SLAVE_FRAMES["ee"]
|
|
feedback_delay_s: float = 0.080
|
|
forward_delay_s: float = 0.0
|
|
forward_jitter_s: float = 0.0
|
|
return_jitter_s: float = 0.0
|
|
forward_packet_loss: float = 0.0
|
|
return_packet_loss: float = 0.0
|
|
forward_timeout_s: float = 0.20
|
|
return_timeout_s: float = 0.20
|
|
contact_probe_fraction: float = 0.03
|
|
contact_probe_cycles: float = 3.0
|
|
|
|
wall_fraction: float = 0.55
|
|
wall_stiffness: float = 800.0
|
|
wall_damping: float = 45.0
|
|
wall_force_limit: float = 80.0
|
|
wall_transition_depth: float = 0.0001
|
|
|
|
master_kp: tuple[float, ...] = (
|
|
196.0,
|
|
196.0,
|
|
144.0,
|
|
256.0,
|
|
100.0,
|
|
100.0,
|
|
81.0,
|
|
)
|
|
master_kd: tuple[float, ...] = (
|
|
28.0,
|
|
28.0,
|
|
24.0,
|
|
32.0,
|
|
20.0,
|
|
20.0,
|
|
18.0,
|
|
)
|
|
slave_kp: tuple[float, ...] = (
|
|
900.0,
|
|
900.0,
|
|
676.0,
|
|
1156.0,
|
|
400.0,
|
|
324.0,
|
|
324.0,
|
|
)
|
|
slave_kd: tuple[float, ...] = (
|
|
54.0,
|
|
54.0,
|
|
46.8,
|
|
61.2,
|
|
36.0,
|
|
32.4,
|
|
32.4,
|
|
)
|
|
master_acceleration_limits: tuple[float, ...] = (
|
|
100.0,
|
|
100.0,
|
|
120.0,
|
|
120.0,
|
|
160.0,
|
|
160.0,
|
|
160.0,
|
|
)
|
|
slave_acceleration_limits: tuple[float, ...] = (
|
|
120.0,
|
|
120.0,
|
|
150.0,
|
|
150.0,
|
|
180.0,
|
|
180.0,
|
|
180.0,
|
|
)
|
|
master_tracking_effort_fraction: float = 0.65
|
|
slave_tracking_effort_fraction: float = 0.70
|
|
velocity_limit_fraction: float = 0.80
|
|
soft_limit_buffer: float = 0.12
|
|
|
|
feedback_strength: float = 0.50
|
|
haptic_filter_alpha: float = 0.222
|
|
haptic_torque_limits: tuple[float, ...] = (
|
|
6.0,
|
|
6.0,
|
|
4.0,
|
|
3.0,
|
|
1.0,
|
|
1.0,
|
|
1.0,
|
|
)
|
|
haptic_rate_limits: tuple[float, ...] = (
|
|
150.0,
|
|
150.0,
|
|
100.0,
|
|
80.0,
|
|
30.0,
|
|
30.0,
|
|
30.0,
|
|
)
|
|
|
|
energy_min: float = 0.05
|
|
energy_max: float = 0.055
|
|
energy_initial: float = 0.05
|
|
energy_probe_mode: str = "none"
|
|
energy_probe_torque_Nm: float = 0.0
|
|
energy_probe_start_fraction: float = 0.20
|
|
energy_probe_end_fraction: float = 0.80
|
|
|
|
sensor_noise_std: float = 0.001
|
|
wrench_characteristic_length_m: float = 0.30
|
|
wrench_scaled_damping: float = 1e-3
|
|
sensor_bias: tuple[float, ...] = (
|
|
0.080,
|
|
-0.050,
|
|
0.035,
|
|
-0.025,
|
|
0.015,
|
|
-0.010,
|
|
0.020,
|
|
)
|
|
bias_calibration_samples: int = 200
|
|
|
|
joint_limit_margin: float = 1e-5
|
|
differential_step: float = 1e-4
|
|
|
|
def validate(self) -> None:
|
|
if not np.isfinite(self.dt) or self.dt <= 0.0:
|
|
raise ValueError("dt must be finite and positive")
|
|
if not np.isfinite(self.duration) or self.duration <= 0.0:
|
|
raise ValueError("duration must be finite and positive")
|
|
if not np.isfinite(self.mapping_hz) or self.mapping_hz <= 0.0:
|
|
raise ValueError("mapping_hz must be finite and positive")
|
|
if self.mapping_hz > 1.0 / self.dt:
|
|
raise ValueError("mapping_hz cannot exceed the dynamics rate")
|
|
if not 0.0 < self.wall_fraction < 1.0:
|
|
raise ValueError("wall_fraction must lie strictly inside (0, 1)")
|
|
if (
|
|
self.wall_stiffness <= 0.0
|
|
or self.wall_damping < 0.0
|
|
or self.wall_force_limit <= 0.0
|
|
or self.wall_transition_depth < 0.0
|
|
):
|
|
raise ValueError("wall parameters must be non-negative")
|
|
network_times = (
|
|
self.feedback_delay_s,
|
|
self.forward_delay_s,
|
|
self.forward_jitter_s,
|
|
self.return_jitter_s,
|
|
)
|
|
if any(value < 0.0 or not np.isfinite(value) for value in network_times):
|
|
raise ValueError("network delays and jitter must be finite/non-negative")
|
|
if self.forward_timeout_s <= 0.0 or self.return_timeout_s <= 0.0:
|
|
raise ValueError("network timeouts must be positive")
|
|
if not 0.0 <= self.forward_packet_loss <= 1.0:
|
|
raise ValueError("forward_packet_loss must lie in [0, 1]")
|
|
if not 0.0 <= self.return_packet_loss <= 1.0:
|
|
raise ValueError("return_packet_loss must lie in [0, 1]")
|
|
if self.contact_probe_fraction < 0.0 or self.contact_probe_cycles < 0.0:
|
|
raise ValueError("contact probe parameters cannot be negative")
|
|
if not (
|
|
0.0 <= self.energy_min <= self.energy_initial <= self.energy_max
|
|
):
|
|
raise ValueError(
|
|
"energy values must satisfy 0 <= min <= initial <= max"
|
|
)
|
|
if self.energy_probe_mode not in {
|
|
"none",
|
|
"velocity_aligned_generalized",
|
|
}:
|
|
raise ValueError(
|
|
"energy_probe_mode must be 'none' or "
|
|
"'velocity_aligned_generalized'"
|
|
)
|
|
if (
|
|
not np.isfinite(self.energy_probe_torque_Nm)
|
|
or self.energy_probe_torque_Nm < 0.0
|
|
):
|
|
raise ValueError(
|
|
"energy_probe_torque_Nm must be finite and non-negative"
|
|
)
|
|
if not (
|
|
0.0
|
|
<= self.energy_probe_start_fraction
|
|
<= self.energy_probe_end_fraction
|
|
<= 1.0
|
|
):
|
|
raise ValueError(
|
|
"energy probe fractions must satisfy "
|
|
"0 <= start <= end <= 1"
|
|
)
|
|
if len(self.haptic_torque_limits) != 7:
|
|
raise ValueError("haptic_torque_limits must contain seven entries")
|
|
if len(self.haptic_rate_limits) != 7:
|
|
raise ValueError("haptic_rate_limits must contain seven entries")
|
|
if len(self.sensor_bias) != 7:
|
|
raise ValueError("sensor_bias must contain seven entries")
|
|
if (
|
|
self.wrench_characteristic_length_m <= 0.0
|
|
or self.wrench_scaled_damping <= 0.0
|
|
):
|
|
raise ValueError("scaled wrench solver parameters must be positive")
|
|
vector_parameters = (
|
|
self.master_kp,
|
|
self.master_kd,
|
|
self.slave_kp,
|
|
self.slave_kd,
|
|
self.master_acceleration_limits,
|
|
self.slave_acceleration_limits,
|
|
)
|
|
if any(len(values) != 7 for values in vector_parameters):
|
|
raise ValueError("all gain and acceleration-limit vectors need 7 entries")
|
|
if not 0.0 < self.velocity_limit_fraction <= 1.0:
|
|
raise ValueError("velocity_limit_fraction must lie in (0, 1]")
|
|
if not 0.0 < self.master_tracking_effort_fraction <= 1.0:
|
|
raise ValueError("master_tracking_effort_fraction must lie in (0, 1]")
|
|
if not 0.0 < self.slave_tracking_effort_fraction <= 1.0:
|
|
raise ValueError("slave_tracking_effort_fraction must lie in (0, 1]")
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class Scenario:
|
|
key: str
|
|
mapping: str
|
|
supervise_energy: bool
|
|
description: str
|
|
supervisor: str = "tank"
|
|
map_policy: str = "current"
|
|
|
|
|
|
SCENARIOS = (
|
|
Scenario(
|
|
key="proposed_energy",
|
|
mapping=MappingKind.DIFFERENTIAL_RESIDUAL.value,
|
|
supervise_energy=True,
|
|
description="A(q_m)^T tau_s with final applied-port supervision",
|
|
supervisor="tank",
|
|
map_policy=MapPolicy.CURRENT.value,
|
|
),
|
|
Scenario(
|
|
key="direct_energy",
|
|
mapping=MappingKind.DIRECT_MASTER_JACOBIAN.value,
|
|
supervise_energy=True,
|
|
description="J_m^T F_s matched-wrench baseline with tank supervision",
|
|
supervisor="tank",
|
|
map_policy=MapPolicy.CURRENT.value,
|
|
),
|
|
Scenario(
|
|
key="proposed_no_energy",
|
|
mapping=MappingKind.DIFFERENTIAL_RESIDUAL.value,
|
|
supervise_energy=False,
|
|
description="A(q_m)^T tau_s with final projection bypassed",
|
|
supervisor="bypass",
|
|
map_policy=MapPolicy.CURRENT.value,
|
|
),
|
|
Scenario(
|
|
key="matched_wrench_energy",
|
|
mapping=MappingKind.MATCHED_DIFFERENTIAL_WRENCH.value,
|
|
supervise_energy=True,
|
|
description=(
|
|
"A(source)^T J_s^T F_s matched to direct J_m^T F_s input"
|
|
),
|
|
supervisor="tank",
|
|
map_policy=MapPolicy.SOURCE_STAMPED.value,
|
|
),
|
|
Scenario(
|
|
key="proposed_popc",
|
|
mapping=MappingKind.DIFFERENTIAL_RESIDUAL.value,
|
|
supervise_energy=False,
|
|
description="A(q_m)^T tau_s with time-domain PO/PC",
|
|
supervisor="popc",
|
|
map_policy=MapPolicy.CURRENT.value,
|
|
),
|
|
)
|
|
|
|
|
|
def load_simulation_config(path: Path) -> SimulationConfig:
|
|
"""Load the ``simulation`` block while rejecting silent key drift."""
|
|
with path.open("r", encoding="utf-8") as stream:
|
|
document = yaml.safe_load(stream)
|
|
if not isinstance(document, dict) or not isinstance(
|
|
document.get("simulation"), dict
|
|
):
|
|
raise ValueError(f"{path} has no mapping-valued 'simulation' block")
|
|
|
|
raw = dict(document["simulation"])
|
|
if "slave_tcp_frame" in raw:
|
|
raw["slave_contact_frame"] = raw.pop("slave_tcp_frame")
|
|
# The offset is consumed by model_contract.py; it is retained in YAML as
|
|
# provenance, not duplicated as a simulator constructor parameter.
|
|
raw.pop("slave_tcp_offset", None)
|
|
|
|
field_map = {field.name: field for field in fields(SimulationConfig)}
|
|
unknown = sorted(set(raw) - set(field_map))
|
|
if unknown:
|
|
raise ValueError(f"Unknown simulation config keys: {unknown}")
|
|
tuple_fields = {
|
|
field.name
|
|
for field in fields(SimulationConfig)
|
|
if isinstance(field.default, tuple)
|
|
}
|
|
for name in tuple_fields & raw.keys():
|
|
raw[name] = tuple(raw[name])
|
|
config = SimulationConfig(**raw)
|
|
config.validate()
|
|
return config
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class WallContactResult:
|
|
"""One unilateral wall evaluation before and after force limiting."""
|
|
|
|
wrench_applied: np.ndarray
|
|
penetration: float
|
|
force_raw_N: float
|
|
force_applied_N: float
|
|
saturation_active: bool
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class Wall:
|
|
point: np.ndarray
|
|
normal: np.ndarray
|
|
stiffness: float
|
|
damping: float
|
|
force_limit: float
|
|
transition_depth: float = 0.0
|
|
|
|
def contact(
|
|
self,
|
|
position_world: np.ndarray,
|
|
linear_velocity_world: np.ndarray,
|
|
) -> WallContactResult:
|
|
"""Return raw/applied wall force diagnostics and the applied wrench."""
|
|
penetration = max(
|
|
0.0,
|
|
float(np.dot(self.normal, position_world - self.point)),
|
|
)
|
|
if penetration <= 0.0:
|
|
return WallContactResult(
|
|
wrench_applied=np.zeros(6, dtype=float),
|
|
penetration=0.0,
|
|
force_raw_N=0.0,
|
|
force_applied_N=0.0,
|
|
saturation_active=False,
|
|
)
|
|
|
|
normal_velocity = float(np.dot(self.normal, linear_velocity_world))
|
|
if self.transition_depth > 0.0:
|
|
damping_activation = min(1.0, penetration / self.transition_depth)
|
|
else:
|
|
damping_activation = 1.0
|
|
force_raw_N = max(
|
|
0.0,
|
|
self.stiffness * penetration
|
|
+ damping_activation * self.damping * normal_velocity,
|
|
)
|
|
force_applied_N = min(self.force_limit, force_raw_N)
|
|
force = -force_applied_N * self.normal
|
|
return WallContactResult(
|
|
wrench_applied=np.concatenate(
|
|
(force, np.zeros(3, dtype=float))
|
|
),
|
|
penetration=penetration,
|
|
force_raw_N=force_raw_N,
|
|
force_applied_N=force_applied_N,
|
|
saturation_active=force_raw_N > self.force_limit,
|
|
)
|
|
|
|
def wrench(
|
|
self,
|
|
position_world: np.ndarray,
|
|
linear_velocity_world: np.ndarray,
|
|
) -> tuple[np.ndarray, float]:
|
|
"""Return applied wrench/penetration with the legacy call signature."""
|
|
result = self.contact(position_world, linear_velocity_world)
|
|
return result.wrench_applied, result.penetration
|
|
|
|
|
|
@dataclass
|
|
class ScenarioResult:
|
|
scenario: Scenario
|
|
metrics: dict[str, Any]
|
|
logs: dict[str, np.ndarray]
|
|
|
|
|
|
def build_mapper(models: TeleoperationModels) -> SEWMapper:
|
|
"""Construct the bounded mapper with the axes/signs in the real slave URDF."""
|
|
return SEWMapper(
|
|
master_model=models.master,
|
|
slave_model=models.slave,
|
|
m_shoulder=MASTER_FRAMES["shoulder"],
|
|
m_elbow=MASTER_FRAMES["elbow"],
|
|
m_wrist=MASTER_FRAMES["wrist"],
|
|
m_ee=MASTER_FRAMES["ee"],
|
|
s_shoulder=SLAVE_FRAMES["shoulder"],
|
|
s_elbow=SLAVE_FRAMES["elbow"],
|
|
s_wrist=SLAVE_FRAMES["wrist"],
|
|
# Retarget the physical wrist orientation; the added TCP is only used
|
|
# for simulated contact and wrench estimation.
|
|
s_ee=SLAVE_FRAMES["wrist"],
|
|
master_joint_names=MASTER_JOINT_NAMES,
|
|
slave_joint_names=SLAVE_JOINT_NAMES,
|
|
slave_shoulder_cfg=BallJointConfig(
|
|
axis_order="yxy",
|
|
joint_names=SLAVE_JOINT_NAMES[:3],
|
|
signs=(-1.0, 1.0, -1.0),
|
|
),
|
|
slave_wrist_cfg=BallJointConfig(
|
|
axis_order="yzx",
|
|
joint_names=SLAVE_JOINT_NAMES[4:],
|
|
signs=(-1.0, 1.0, 1.0),
|
|
),
|
|
slave_elbow_axis_local=np.array([1.0, 0.0, 0.0]),
|
|
up_dir=np.array([0.0, 0.0, 1.0]),
|
|
)
|
|
|
|
|
|
def master_endpoint_configurations() -> tuple[np.ndarray, np.ndarray]:
|
|
"""Return an interior, smooth, exactly recoverable 4.6 cm wrist reach."""
|
|
q_start = np.array(
|
|
[0.25, 0.25, -0.20, 1.90, -0.10, 0.10, -0.10],
|
|
dtype=float,
|
|
)
|
|
q_end = q_start.copy()
|
|
q_end[3] = 1.80
|
|
return q_start, q_end
|
|
|
|
|
|
def _smooth_transition(
|
|
t: float,
|
|
t0: float,
|
|
t1: float,
|
|
) -> tuple[float, float, float]:
|
|
if t <= t0:
|
|
return 0.0, 0.0, 0.0
|
|
if t >= t1:
|
|
return 1.0, 0.0, 0.0
|
|
duration = t1 - t0
|
|
u = (t - t0) / duration
|
|
position = 10.0 * u**3 - 15.0 * u**4 + 6.0 * u**5
|
|
velocity = (30.0 * u**2 - 60.0 * u**3 + 30.0 * u**4) / duration
|
|
acceleration = (
|
|
60.0 * u - 180.0 * u**2 + 120.0 * u**3
|
|
) / duration**2
|
|
return position, velocity, acceleration
|
|
|
|
|
|
def master_reference(
|
|
t: float,
|
|
duration: float,
|
|
q_start: np.ndarray,
|
|
q_end: np.ndarray,
|
|
contact_probe_fraction: float = 0.0,
|
|
contact_probe_cycles: float = 0.0,
|
|
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
|
"""Approach, probe the delayed contact channel, return, then settle."""
|
|
approach_start = 0.10 * duration
|
|
approach_end = 0.38 * duration
|
|
return_start = 0.53 * duration
|
|
return_end = 0.81 * duration
|
|
|
|
delta = q_end - q_start
|
|
if t < return_start:
|
|
s, sd, sdd = _smooth_transition(t, approach_start, approach_end)
|
|
if (
|
|
approach_end < t < return_start
|
|
and contact_probe_fraction > 0.0
|
|
and contact_probe_cycles > 0.0
|
|
):
|
|
probe_duration = return_start - approach_end
|
|
u = (t - approach_end) / probe_duration
|
|
angular_frequency_u = 2.0 * np.pi * contact_probe_cycles
|
|
envelope = np.sin(np.pi * u) ** 2
|
|
envelope_du = np.pi * np.sin(2.0 * np.pi * u)
|
|
envelope_du2 = 2.0 * np.pi**2 * np.cos(2.0 * np.pi * u)
|
|
carrier = np.sin(angular_frequency_u * u)
|
|
carrier_du = angular_frequency_u * np.cos(
|
|
angular_frequency_u * u
|
|
)
|
|
carrier_du2 = -(angular_frequency_u**2) * carrier
|
|
probe = contact_probe_fraction * envelope * carrier
|
|
probe_du = contact_probe_fraction * (
|
|
envelope_du * carrier + envelope * carrier_du
|
|
)
|
|
probe_du2 = contact_probe_fraction * (
|
|
envelope_du2 * carrier
|
|
+ 2.0 * envelope_du * carrier_du
|
|
+ envelope * carrier_du2
|
|
)
|
|
s += probe
|
|
sd += probe_du / probe_duration
|
|
sdd += probe_du2 / probe_duration**2
|
|
else:
|
|
sr, srd, srdd = _smooth_transition(t, return_start, return_end)
|
|
s, sd, sdd = 1.0 - sr, -srd, -srdd
|
|
return q_start + s * delta, sd * delta, sdd * delta
|
|
|
|
|
|
def _mass_matrix(model: pin.Model, data: pin.Data, q: np.ndarray) -> np.ndarray:
|
|
upper = np.asarray(pin.crba(model, data, q), dtype=float)
|
|
return np.triu(upper) + np.triu(upper, 1).T
|
|
|
|
|
|
def computed_torque(
|
|
model: pin.Model,
|
|
data: pin.Data,
|
|
q: np.ndarray,
|
|
qd: np.ndarray,
|
|
q_ref: np.ndarray,
|
|
qd_ref: np.ndarray,
|
|
qdd_ref: np.ndarray,
|
|
kp: float | np.ndarray,
|
|
kd: float | np.ndarray,
|
|
soft_limit_acceleration: np.ndarray | None = None,
|
|
) -> np.ndarray:
|
|
"""Model-based tracking command for a fixed-base revolute chain."""
|
|
position_error = pin.difference(model, q, q_ref)
|
|
kp_vector = np.broadcast_to(np.asarray(kp, dtype=float), (model.nv,))
|
|
kd_vector = np.broadcast_to(np.asarray(kd, dtype=float), (model.nv,))
|
|
acceleration_command = (
|
|
qdd_ref
|
|
+ kp_vector * position_error
|
|
+ kd_vector * (qd_ref - qd)
|
|
)
|
|
if soft_limit_acceleration is not None:
|
|
acceleration_command = (
|
|
acceleration_command
|
|
+ np.asarray(soft_limit_acceleration, dtype=float)
|
|
)
|
|
nonlinear = pin.nonLinearEffects(model, data, q, qd)
|
|
return nonlinear + _mass_matrix(model, data, q) @ acceleration_command
|
|
|
|
|
|
def _clip_actuator_torque(
|
|
model: pin.Model,
|
|
torque: np.ndarray,
|
|
effort_fraction: float = 1.0,
|
|
) -> tuple[np.ndarray, bool]:
|
|
limits = effort_fraction * np.asarray(model.effortLimit, dtype=float)
|
|
finite_limits = np.where(np.isfinite(limits), limits, np.inf)
|
|
clipped = np.clip(np.asarray(torque, dtype=float), -finite_limits, finite_limits)
|
|
return clipped, bool(np.any(np.abs(clipped - torque) > 1e-12))
|
|
|
|
|
|
def soft_limit_acceleration(
|
|
model: pin.Model,
|
|
q: np.ndarray,
|
|
qd: np.ndarray,
|
|
buffer: float,
|
|
) -> np.ndarray:
|
|
"""Continuous acceleration-domain guard before the last-resort projection."""
|
|
if buffer <= 0.0:
|
|
return np.zeros(model.nv, dtype=float)
|
|
lower_zone = np.asarray(model.lowerPositionLimit, dtype=float) + buffer
|
|
upper_zone = np.asarray(model.upperPositionLimit, dtype=float) - buffer
|
|
acceleration = np.zeros(model.nv, dtype=float)
|
|
below = q < lower_zone
|
|
above = q > upper_zone
|
|
acceleration[below] += 400.0 * (lower_zone[below] - q[below])
|
|
acceleration[below] += 36.0 * np.maximum(-qd[below], 0.0)
|
|
acceleration[above] -= 400.0 * (q[above] - upper_zone[above])
|
|
acceleration[above] -= 36.0 * np.maximum(qd[above], 0.0)
|
|
return acceleration
|
|
|
|
|
|
def integrate_state(
|
|
model: pin.Model,
|
|
q: np.ndarray,
|
|
qd: np.ndarray,
|
|
qdd: np.ndarray,
|
|
dt: float,
|
|
joint_names: Iterable[str],
|
|
margin: float,
|
|
velocity_limit_fraction: float,
|
|
) -> tuple[np.ndarray, np.ndarray, bool, bool]:
|
|
"""Semi-implicit integration with URDF speed and position guards."""
|
|
velocity_limit = (
|
|
velocity_limit_fraction * np.asarray(model.velocityLimit, dtype=float)
|
|
)
|
|
qd_unlimited = qd + qdd * dt
|
|
qd_next = np.clip(qd_unlimited, -velocity_limit, velocity_limit)
|
|
velocity_limited = bool(
|
|
np.any(np.abs(qd_next - qd_unlimited) > 1e-12)
|
|
)
|
|
q_next = pin.integrate(model, q, qd_next * dt)
|
|
q_next, clipped = clip_configuration(
|
|
model,
|
|
q_next,
|
|
tuple(joint_names),
|
|
margin=margin,
|
|
)
|
|
|
|
if clipped:
|
|
# All models in this experiment have nq == nv == 7 scalar revolute
|
|
# joints. At a clipped boundary, cancel only outward velocity.
|
|
lower = np.asarray(model.lowerPositionLimit, dtype=float) + margin
|
|
upper = np.asarray(model.upperPositionLimit, dtype=float) - margin
|
|
at_lower = q_next <= lower + 1e-12
|
|
at_upper = q_next >= upper - 1e-12
|
|
qd_next = qd_next.copy()
|
|
qd_next[at_lower & (qd_next < 0.0)] = 0.0
|
|
qd_next[at_upper & (qd_next > 0.0)] = 0.0
|
|
return q_next, qd_next, clipped, velocity_limited
|
|
|
|
|
|
def frame_kinematics(
|
|
model: pin.Model,
|
|
data: pin.Data,
|
|
q: np.ndarray,
|
|
qd: np.ndarray,
|
|
frame_id: int,
|
|
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
|
pin.forwardKinematics(model, data, q, qd)
|
|
pin.updateFramePlacements(model, data)
|
|
jacobian = pin.computeFrameJacobian(
|
|
model,
|
|
data,
|
|
q,
|
|
frame_id,
|
|
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED,
|
|
)
|
|
position = np.asarray(data.oMf[frame_id].translation, dtype=float).copy()
|
|
linear_velocity = np.asarray(jacobian[:3] @ qd, dtype=float)
|
|
return position, linear_velocity, np.asarray(jacobian, dtype=float)
|
|
|
|
|
|
def make_wall(
|
|
config: SimulationConfig,
|
|
models: TeleoperationModels,
|
|
mapper: SEWMapper,
|
|
) -> tuple[Wall, dict[str, Any], np.ndarray]:
|
|
q_start, q_end = master_endpoint_configurations()
|
|
q_slave_start, start_debug = mapper.retarget(q_start)
|
|
q_slave_end, end_debug = mapper.retarget(q_end, q_s_init=q_slave_start)
|
|
if not start_debug["success"] or not end_debug["success"]:
|
|
raise RuntimeError(
|
|
"Cannot place the wall because endpoint retargeting failed: "
|
|
f"start={start_debug['events']}, end={end_debug['events']}"
|
|
)
|
|
|
|
frame_id = require_frame(models.slave, config.slave_contact_frame)
|
|
data = models.slave.createData()
|
|
p_start, _, _ = frame_kinematics(
|
|
models.slave,
|
|
data,
|
|
q_slave_start,
|
|
np.zeros(models.slave.nv),
|
|
frame_id,
|
|
)
|
|
p_end, _, _ = frame_kinematics(
|
|
models.slave,
|
|
data,
|
|
q_slave_end,
|
|
np.zeros(models.slave.nv),
|
|
frame_id,
|
|
)
|
|
travel = p_end - p_start
|
|
travel_norm = float(np.linalg.norm(travel))
|
|
if travel_norm < 1e-6:
|
|
raise RuntimeError("Retargeted TCP motion is too small to define a wall")
|
|
normal = travel / travel_norm
|
|
point = p_start + config.wall_fraction * travel
|
|
wall = Wall(
|
|
point=point,
|
|
normal=normal,
|
|
stiffness=config.wall_stiffness,
|
|
damping=config.wall_damping,
|
|
force_limit=config.wall_force_limit,
|
|
transition_depth=config.wall_transition_depth,
|
|
)
|
|
metadata = {
|
|
"start_tcp_world_m": p_start.tolist(),
|
|
"end_tcp_world_m": p_end.tolist(),
|
|
"free_space_travel_m": travel_norm,
|
|
"point_world_m": point.tolist(),
|
|
"normal_world": normal.tolist(),
|
|
"fraction_of_free_space_travel": config.wall_fraction,
|
|
"stiffness_N_per_m": config.wall_stiffness,
|
|
"damping_Ns_per_m": config.wall_damping,
|
|
"force_limit_N": config.wall_force_limit,
|
|
"transition_depth_m": config.wall_transition_depth,
|
|
}
|
|
return wall, metadata, q_slave_start
|
|
|
|
|
|
def make_renderer(
|
|
model: pin.Model,
|
|
config: SimulationConfig,
|
|
) -> HapticRenderer:
|
|
return HapticRenderer(
|
|
model,
|
|
chest_frame_name=MASTER_FRAMES["base"],
|
|
ee_frame_name=MASTER_FRAMES["ee"],
|
|
feedback_strength=config.feedback_strength,
|
|
E_init=config.energy_min,
|
|
E_max=config.energy_max,
|
|
alpha_floor=0.0,
|
|
alpha_ceil=1.0,
|
|
E0=config.energy_initial,
|
|
torque_limit=np.asarray(config.haptic_torque_limits, dtype=float),
|
|
torque_rate_limit=np.asarray(config.haptic_rate_limits, dtype=float),
|
|
tau_filter_alpha=config.haptic_filter_alpha,
|
|
)
|
|
|
|
|
|
def _delay_line(
|
|
size: int,
|
|
sample_shape: tuple[int, ...],
|
|
) -> deque[np.ndarray]:
|
|
return deque(
|
|
[np.zeros(sample_shape, dtype=float) for _ in range(size)],
|
|
maxlen=size,
|
|
)
|
|
|
|
|
|
def _push_delayed(
|
|
queue: deque[np.ndarray],
|
|
sample: np.ndarray,
|
|
) -> np.ndarray:
|
|
if queue.maxlen == 0:
|
|
return np.asarray(sample, dtype=float).copy()
|
|
delayed = queue.popleft()
|
|
queue.append(np.asarray(sample, dtype=float).copy())
|
|
return delayed
|
|
|
|
|
|
def _rms(values: np.ndarray) -> float:
|
|
values = np.asarray(values, dtype=float)
|
|
if values.size == 0:
|
|
return 0.0
|
|
return float(np.sqrt(np.mean(np.square(values))))
|
|
|
|
|
|
def _finite_or_none(value: float) -> float | None:
|
|
return float(value) if np.isfinite(value) else None
|
|
|
|
|
|
def simulate_scenario(
|
|
scenario: Scenario,
|
|
config: SimulationConfig,
|
|
models: TeleoperationModels,
|
|
wall: Wall,
|
|
q_slave_start: np.ndarray,
|
|
) -> ScenarioResult:
|
|
"""Run one closed-loop case with an independent but identically seeded state."""
|
|
config.validate()
|
|
rng = np.random.default_rng(config.seed)
|
|
mapper = build_mapper(models)
|
|
renderer = make_renderer(models.master, config)
|
|
estimator = InteractionEstimator(
|
|
models.slave,
|
|
chest_frame_name=SLAVE_FRAMES["base"],
|
|
ee_frame_name=config.slave_contact_frame,
|
|
lambda_damp=1e-3,
|
|
wrench_solver=ScaledDLSSolver(
|
|
config.wrench_characteristic_length_m,
|
|
config.wrench_scaled_damping,
|
|
),
|
|
)
|
|
|
|
sensor_bias = np.asarray(config.sensor_bias, dtype=float)
|
|
calibration = sensor_bias + rng.normal(
|
|
0.0,
|
|
config.sensor_noise_std,
|
|
size=(config.bias_calibration_samples, models.slave.nv),
|
|
)
|
|
estimator.calibrate_bias(calibration)
|
|
|
|
q_start, q_end = master_endpoint_configurations()
|
|
q_m = q_start.copy()
|
|
qd_m = np.zeros(models.master.nv, dtype=float)
|
|
q_s = q_slave_start.copy()
|
|
qd_s = np.zeros(models.slave.nv, dtype=float)
|
|
|
|
q_s_ref, A, map_debug = mapper.retarget_with_differential(
|
|
q_m,
|
|
q_s_init=q_s,
|
|
fd_step=config.differential_step,
|
|
)
|
|
if not map_debug["success"] or not map_debug["differential_valid"]:
|
|
raise RuntimeError(
|
|
"Initial retargeting differential is invalid: "
|
|
f"pose={map_debug['events']}, "
|
|
f"A={map_debug['differential']['events']}"
|
|
)
|
|
differential_feedback_valid = True
|
|
qd_s_ref_hold = np.zeros(models.slave.nv, dtype=float)
|
|
map_registry = MapRegistry(capacity=2048)
|
|
map_registry.add(
|
|
MapSnapshot(
|
|
map_id=0,
|
|
source_index=0,
|
|
source_time=0.0,
|
|
differential=A,
|
|
valid=True,
|
|
)
|
|
)
|
|
active_forward_map_id = 0
|
|
next_map_id = 1
|
|
forward_seq = 0
|
|
|
|
master_data_control = models.master.createData()
|
|
master_data_dynamics = models.master.createData()
|
|
slave_data_control = models.slave.createData()
|
|
slave_data_dynamics = models.slave.createData()
|
|
slave_data_contact = models.slave.createData()
|
|
slave_tcp_id = require_frame(models.slave, config.slave_contact_frame)
|
|
|
|
step_count = int(round(config.duration / config.dt))
|
|
mapping_stride = max(
|
|
1,
|
|
int(round(1.0 / (config.mapping_hz * config.dt))),
|
|
)
|
|
forward_trace = generate_network_trace(
|
|
step_count // mapping_stride + 2,
|
|
base_delay_s=config.forward_delay_s,
|
|
jitter_s=config.forward_jitter_s,
|
|
loss_probability=config.forward_packet_loss,
|
|
seed=config.seed + 1001,
|
|
)
|
|
return_trace = generate_network_trace(
|
|
step_count,
|
|
base_delay_s=config.feedback_delay_s,
|
|
jitter_s=config.return_jitter_s,
|
|
loss_probability=config.return_packet_loss,
|
|
seed=config.seed + 1002,
|
|
)
|
|
forward_channel: DeterministicChannel[ForwardPacket] = (
|
|
DeterministicChannel(forward_trace)
|
|
)
|
|
return_channel: DeterministicChannel[ReturnPacket] = (
|
|
DeterministicChannel(return_trace)
|
|
)
|
|
forward_receiver: PacketReceiver[ForwardPacket] = PacketReceiver(
|
|
config.forward_timeout_s
|
|
)
|
|
return_receiver: PacketReceiver[ReturnPacket] = PacketReceiver(
|
|
config.return_timeout_s
|
|
)
|
|
forward_channel.send(
|
|
ForwardPacket(
|
|
seq=forward_seq,
|
|
source_index=0,
|
|
source_time=0.0,
|
|
map_id=0,
|
|
q_slave_ref=q_s_ref,
|
|
qd_slave_ctrl=qd_s_ref_hold,
|
|
),
|
|
now=0.0,
|
|
)
|
|
forward_seq += 1
|
|
|
|
scalar_keys = (
|
|
"time",
|
|
"sample_index",
|
|
"dt",
|
|
"missed_deadline",
|
|
"contact_force_norm",
|
|
"wall_force_raw_N",
|
|
"wall_force_applied_N",
|
|
"wall_force_saturation_active",
|
|
"penetration",
|
|
"force_estimation_error_norm",
|
|
"moment_estimation_error_norm",
|
|
"raw_master_power",
|
|
"a_defined_slave_power",
|
|
"a_port_identity_error",
|
|
"source_slave_power",
|
|
"actual_power_mismatch_abs",
|
|
"actual_slave_environment_power",
|
|
"candidate_power",
|
|
"applied_power",
|
|
"rho",
|
|
"energy_before",
|
|
"tank_energy",
|
|
"energy_preclip",
|
|
"shadow_energy",
|
|
"popc_damping_gain",
|
|
"map_id",
|
|
"source_map_id",
|
|
"return_source_index",
|
|
"return_packet_age",
|
|
"forward_packet_state",
|
|
"return_packet_state",
|
|
"return_packet_active",
|
|
"master_tracking_error",
|
|
"slave_tracking_error",
|
|
"feedback_torque_norm",
|
|
"mapped_torque_norm",
|
|
"energy_probe_raw_power_W",
|
|
"energy_probe_raw_work_J",
|
|
"energy_probe_envelope",
|
|
"master_joint_limit_active",
|
|
"slave_joint_limit_active",
|
|
"master_velocity_limit_active",
|
|
"slave_velocity_limit_active",
|
|
"master_acceleration_limit_active",
|
|
"slave_acceleration_limit_active",
|
|
"master_torque_saturation_active",
|
|
"slave_torque_saturation_active",
|
|
"haptic_rate_limit_active",
|
|
"haptic_torque_saturation_active",
|
|
)
|
|
vector_keys = (
|
|
"q_master",
|
|
"qd_master",
|
|
"q_master_ref",
|
|
"q_slave",
|
|
"qd_slave",
|
|
"q_slave_ref",
|
|
"tau_slave_external",
|
|
"tau_slave_estimated",
|
|
"tau_slave_residual_source",
|
|
"tau_slave_matched_wrench",
|
|
"qd_slave_source",
|
|
"tau_master_mapped",
|
|
"tau_master_candidate",
|
|
"tau_master_applied",
|
|
"tau_master_accepted",
|
|
"energy_probe_torque_Nm",
|
|
"wrench_external",
|
|
"wrench_estimated",
|
|
"wrench_feedback_source",
|
|
"map_differential",
|
|
"tcp_position",
|
|
)
|
|
log_lists: dict[str, list[np.ndarray | float]] = {
|
|
key: [] for key in scalar_keys + vector_keys
|
|
}
|
|
|
|
map_update_count = 0
|
|
map_pose_success_count = 0
|
|
differential_valid_count = 0
|
|
differential_fallback_count = 0
|
|
mapping_runtimes_ms: list[float] = []
|
|
master_limit_events = 0
|
|
slave_limit_events = 0
|
|
master_velocity_limit_events = 0
|
|
slave_velocity_limit_events = 0
|
|
master_acceleration_limit_events = 0
|
|
slave_acceleration_limit_events = 0
|
|
master_torque_saturation_events = 0
|
|
slave_torque_saturation_events = 0
|
|
haptic_rate_limit_events = 0
|
|
haptic_torque_saturation_events = 0
|
|
energy_identity_errors: list[float] = []
|
|
shadow_energy = config.energy_initial
|
|
energy_probe_raw_work_J = 0.0
|
|
contact_steps = 0
|
|
forward_packets_accepted = 0
|
|
forward_packets_rejected = 0
|
|
return_packets_accepted = 0
|
|
return_packets_rejected = 0
|
|
forward_timeout_steps = 0
|
|
return_timeout_steps = 0
|
|
popc = TimeDomainPOPC(
|
|
initial_energy=config.energy_initial,
|
|
minimum_energy=config.energy_min,
|
|
maximum_energy=config.energy_max,
|
|
)
|
|
|
|
run_start = time.perf_counter()
|
|
for step in range(step_count):
|
|
t = step * config.dt
|
|
q_m_ref, qd_m_ref, qdd_m_ref = master_reference(
|
|
t,
|
|
config.duration,
|
|
q_start,
|
|
q_end,
|
|
config.contact_probe_fraction,
|
|
config.contact_probe_cycles,
|
|
)
|
|
|
|
if step > 0 and step % mapping_stride == 0:
|
|
tic = time.perf_counter()
|
|
q_s_candidate, A_candidate, update_debug = (
|
|
mapper.retarget_with_differential(
|
|
q_m,
|
|
q_s_init=q_s_ref,
|
|
fd_step=config.differential_step,
|
|
)
|
|
)
|
|
mapping_runtimes_ms.append(
|
|
1e3 * (time.perf_counter() - tic)
|
|
)
|
|
map_update_count += 1
|
|
pose_valid = bool(update_debug["success"])
|
|
differential_valid = bool(update_debug["differential_valid"])
|
|
if pose_valid:
|
|
map_pose_success_count += 1
|
|
if update_debug["differential_valid"]:
|
|
A = A_candidate
|
|
differential_valid_count += 1
|
|
qd_s_candidate = np.clip(
|
|
A @ qd_m,
|
|
-config.velocity_limit_fraction
|
|
* np.asarray(models.slave.velocityLimit, dtype=float),
|
|
config.velocity_limit_fraction
|
|
* np.asarray(models.slave.velocityLimit, dtype=float),
|
|
)
|
|
else:
|
|
differential_fallback_count += 1
|
|
qd_s_candidate = np.zeros(models.slave.nv, dtype=float)
|
|
|
|
snapshot_differential = (
|
|
A_candidate
|
|
if np.all(np.isfinite(A_candidate))
|
|
else np.zeros_like(A)
|
|
)
|
|
map_registry.add(
|
|
MapSnapshot(
|
|
map_id=next_map_id,
|
|
source_index=step,
|
|
source_time=t,
|
|
differential=snapshot_differential,
|
|
valid=differential_valid,
|
|
reason_code=0 if differential_valid else 1,
|
|
)
|
|
)
|
|
forward_channel.send(
|
|
ForwardPacket(
|
|
seq=forward_seq,
|
|
source_index=step,
|
|
source_time=t,
|
|
map_id=next_map_id,
|
|
q_slave_ref=(
|
|
q_s_candidate if pose_valid else q_s_ref
|
|
),
|
|
qd_slave_ctrl=qd_s_candidate,
|
|
valid=pose_valid,
|
|
),
|
|
now=t,
|
|
)
|
|
next_map_id += 1
|
|
forward_seq += 1
|
|
|
|
for delivery in forward_channel.poll(t):
|
|
reception = forward_receiver.accept(delivery)
|
|
if reception.accepted:
|
|
forward_packets_accepted += 1
|
|
else:
|
|
forward_packets_rejected += 1
|
|
held_forward = forward_receiver.sample(t)
|
|
if held_forward.packet is not None:
|
|
q_s_ref = held_forward.packet.q_slave_ref.copy()
|
|
qd_s_ref_hold = held_forward.packet.qd_slave_ctrl.copy()
|
|
active_forward_map_id = held_forward.packet.map_id
|
|
elif held_forward.state is PacketState.TIMED_OUT:
|
|
# The position target is held while commanded velocity goes to zero.
|
|
qd_s_ref_hold.fill(0.0)
|
|
forward_timeout_steps += 1
|
|
qd_s_ref = qd_s_ref_hold
|
|
try:
|
|
differential_feedback_valid = map_registry.get(
|
|
active_forward_map_id
|
|
).valid
|
|
except KeyError:
|
|
differential_feedback_valid = False
|
|
|
|
tcp_position, tcp_linear_velocity, J_slave_world = frame_kinematics(
|
|
models.slave,
|
|
slave_data_contact,
|
|
q_s,
|
|
qd_s,
|
|
slave_tcp_id,
|
|
)
|
|
wall_contact = wall.contact(
|
|
tcp_position,
|
|
tcp_linear_velocity,
|
|
)
|
|
wrench_external = wall_contact.wrench_applied
|
|
penetration = wall_contact.penetration
|
|
if penetration > 0.0:
|
|
contact_steps += 1
|
|
tau_slave_external = J_slave_world.T @ wrench_external
|
|
|
|
tau_slave_control = computed_torque(
|
|
models.slave,
|
|
slave_data_control,
|
|
q_s,
|
|
qd_s,
|
|
q_s_ref,
|
|
qd_s_ref,
|
|
np.zeros(models.slave.nv),
|
|
config.slave_kp,
|
|
config.slave_kd,
|
|
soft_limit_acceleration(
|
|
models.slave,
|
|
q_s,
|
|
qd_s,
|
|
config.soft_limit_buffer,
|
|
),
|
|
)
|
|
tau_slave_control, torque_clipped = _clip_actuator_torque(
|
|
models.slave,
|
|
tau_slave_control,
|
|
config.slave_tracking_effort_fraction,
|
|
)
|
|
slave_torque_saturation_events += int(torque_clipped)
|
|
qdd_s_raw = pin.aba(
|
|
models.slave,
|
|
slave_data_dynamics,
|
|
q_s,
|
|
qd_s,
|
|
tau_slave_control + tau_slave_external,
|
|
)
|
|
slave_acceleration_limits = np.asarray(
|
|
config.slave_acceleration_limits,
|
|
dtype=float,
|
|
)
|
|
qdd_s = np.clip(
|
|
qdd_s_raw,
|
|
-slave_acceleration_limits,
|
|
slave_acceleration_limits,
|
|
)
|
|
slave_acceleration_limited = bool(
|
|
np.any(np.abs(qdd_s_raw) > slave_acceleration_limits)
|
|
)
|
|
slave_acceleration_limit_events += int(slave_acceleration_limited)
|
|
|
|
# This is an explicitly simulated load-side equivalent measurement.
|
|
# It satisfies the estimator's declared residual convention:
|
|
# tau_int = tau_meas - (M qdd + h) - calibrated_bias.
|
|
tau_slave_model = estimator._tau_model(q_s, qd_s, qdd_s)
|
|
measurement_noise = rng.normal(
|
|
0.0,
|
|
config.sensor_noise_std,
|
|
models.slave.nv,
|
|
)
|
|
tau_slave_measured = (
|
|
tau_slave_model
|
|
+ tau_slave_external
|
|
+ sensor_bias
|
|
+ measurement_noise
|
|
)
|
|
tau_slave_estimated, wrench_estimated, J_slave_chest = estimator.estimate(
|
|
q_s,
|
|
qd_s,
|
|
qdd_s,
|
|
tau_slave_measured,
|
|
)
|
|
tau_slave_matched_wrench = J_slave_chest.T @ wrench_estimated
|
|
return_channel.send(
|
|
ReturnPacket(
|
|
seq=step,
|
|
source_index=step,
|
|
source_time=t,
|
|
echoed_map_id=active_forward_map_id,
|
|
residual=tau_slave_estimated,
|
|
wrench=wrench_estimated,
|
|
js_t_wrench=tau_slave_matched_wrench,
|
|
qd_slave_actual=qd_s,
|
|
),
|
|
now=t,
|
|
)
|
|
for delivery in return_channel.poll(t):
|
|
reception = return_receiver.accept(delivery)
|
|
if reception.accepted:
|
|
return_packets_accepted += 1
|
|
else:
|
|
return_packets_rejected += 1
|
|
held_return = return_receiver.sample(t)
|
|
delayed_packet = held_return.packet
|
|
return_packet_active = delayed_packet is not None
|
|
if held_return.state is PacketState.TIMED_OUT:
|
|
return_timeout_steps += 1
|
|
|
|
master_jacobian = renderer.CJ_master.chest_jacobian(q_m, qd_m)
|
|
if delayed_packet is None:
|
|
tau_master_mapped = np.zeros(models.master.nv, dtype=float)
|
|
delayed_tau_slave = np.zeros(models.slave.nv, dtype=float)
|
|
delayed_wrench = np.zeros(6, dtype=float)
|
|
delayed_tau_matched = np.zeros(models.slave.nv, dtype=float)
|
|
tau_slave_source_for_mapping = np.zeros(
|
|
models.slave.nv, dtype=float
|
|
)
|
|
qd_slave_source = np.zeros(models.slave.nv, dtype=float)
|
|
selected_map_id = -1
|
|
source_map_id = -1
|
|
return_source_index = -1
|
|
return_packet_age = math.nan
|
|
else:
|
|
feedback = map_return_feedback(
|
|
kind=MappingKind(scenario.mapping),
|
|
packet=delayed_packet,
|
|
master_jacobian=master_jacobian,
|
|
maps=map_registry,
|
|
map_policy=MapPolicy(scenario.map_policy),
|
|
)
|
|
tau_master_mapped = feedback.tau_master_raw
|
|
delayed_tau_slave = delayed_packet.residual.copy()
|
|
delayed_wrench = delayed_packet.wrench.copy()
|
|
delayed_tau_matched = delayed_packet.js_t_wrench.copy()
|
|
tau_slave_source_for_mapping = feedback.tau_slave_source.copy()
|
|
qd_slave_source = delayed_packet.qd_slave_actual.copy()
|
|
selected_map_id = (
|
|
-1
|
|
if feedback.selected_map_id is None
|
|
else feedback.selected_map_id
|
|
)
|
|
source_map_id = delayed_packet.echoed_map_id
|
|
return_source_index = delayed_packet.source_index
|
|
return_packet_age = max(0.0, t - delayed_packet.source_time)
|
|
if not feedback.valid:
|
|
tau_master_mapped = np.zeros(models.master.nv, dtype=float)
|
|
|
|
energy_probe_torque = np.zeros(models.master.nv, dtype=float)
|
|
energy_probe_envelope = 0.0
|
|
normalized_time = t / config.duration
|
|
energy_probe_window = (
|
|
config.energy_probe_end_fraction
|
|
- config.energy_probe_start_fraction
|
|
)
|
|
if (
|
|
config.energy_probe_mode == "velocity_aligned_generalized"
|
|
and energy_probe_window > 0.0
|
|
and config.energy_probe_start_fraction
|
|
<= normalized_time
|
|
<= config.energy_probe_end_fraction
|
|
):
|
|
probe_phase = (
|
|
normalized_time - config.energy_probe_start_fraction
|
|
) / energy_probe_window
|
|
energy_probe_envelope = float(
|
|
np.sin(np.pi * probe_phase) ** 2
|
|
)
|
|
velocity_norm = float(np.linalg.norm(qd_m))
|
|
if velocity_norm > 1e-12:
|
|
energy_probe_torque = (
|
|
config.energy_probe_torque_Nm
|
|
* energy_probe_envelope
|
|
* qd_m
|
|
/ velocity_norm
|
|
)
|
|
tau_master_mapped = (
|
|
tau_master_mapped + energy_probe_torque
|
|
)
|
|
energy_probe_raw_power_W = float(
|
|
np.dot(energy_probe_torque, qd_m)
|
|
)
|
|
energy_probe_raw_work_J += (
|
|
energy_probe_raw_power_W * config.dt
|
|
)
|
|
|
|
popc_damping_gain = 0.0
|
|
if scenario.supervisor == "tank":
|
|
tau_master_applied, rho = renderer.render_mapped_reaction(
|
|
tau_master_mapped,
|
|
qd_m,
|
|
config.dt,
|
|
supervise_energy=True,
|
|
)
|
|
elif scenario.supervisor == "bypass":
|
|
tau_master_applied, rho = renderer.render_mapped_reaction(
|
|
tau_master_mapped,
|
|
qd_m,
|
|
config.dt,
|
|
supervise_energy=False,
|
|
)
|
|
elif scenario.supervisor == "popc":
|
|
tau_master_candidate, candidate_valid = (
|
|
renderer.shape_mapped_reaction_candidate(
|
|
tau_master_mapped,
|
|
qd_m,
|
|
config.dt,
|
|
)
|
|
)
|
|
if candidate_valid:
|
|
tau_master_applied, popc_diagnostics = popc.apply(
|
|
tau_master_candidate,
|
|
qd_m,
|
|
config.dt,
|
|
)
|
|
else:
|
|
tau_master_applied = np.zeros_like(tau_master_candidate)
|
|
_, popc_diagnostics = popc.apply(
|
|
tau_master_applied,
|
|
np.zeros_like(qd_m),
|
|
config.dt,
|
|
)
|
|
renderer.commit_applied(tau_master_applied)
|
|
rho = math.nan
|
|
popc_damping_gain = popc_diagnostics.damping_gain
|
|
else:
|
|
raise ValueError(f"Unknown supervisor: {scenario.supervisor}")
|
|
haptic_rate_limit_events += int(renderer.last_rate_limit_active)
|
|
haptic_torque_saturation_events += int(
|
|
renderer.last_torque_saturation_active
|
|
)
|
|
if scenario.supervisor == "tank":
|
|
diagnostics = renderer.tank.last_diagnostics
|
|
assert diagnostics is not None
|
|
tau_master_candidate = diagnostics.tau_candidate.copy()
|
|
reconstructed_energy_preclip = float(
|
|
diagnostics.E_before - diagnostics.power * config.dt
|
|
)
|
|
energy_identity_errors.append(
|
|
abs(diagnostics.E_preclip - reconstructed_energy_preclip)
|
|
)
|
|
tank_energy = diagnostics.E_after
|
|
energy_before = diagnostics.E_before
|
|
energy_preclip = diagnostics.E_preclip
|
|
candidate_power = diagnostics.candidate_power
|
|
elif scenario.supervisor == "popc":
|
|
tank_energy = popc_diagnostics.observer_after
|
|
energy_before = popc_diagnostics.observer_before
|
|
energy_preclip = popc_diagnostics.observer_preclip
|
|
candidate_power = popc_diagnostics.candidate_power
|
|
else:
|
|
tau_master_candidate = tau_master_applied.copy()
|
|
tank_energy = math.nan
|
|
energy_before = math.nan
|
|
energy_preclip = math.nan
|
|
candidate_power = float(np.dot(tau_master_candidate, qd_m))
|
|
|
|
applied_power = float(np.dot(tau_master_applied, qd_m))
|
|
tau_master_accepted = tau_master_applied.copy()
|
|
# Counterfactual storage obeys the same upper capacity but deliberately
|
|
# has no lower projection. Falling below E_min demonstrates the exact
|
|
# sample at which unsupervised output violates the configured budget.
|
|
shadow_energy = min(
|
|
config.energy_max,
|
|
shadow_energy - candidate_power * config.dt,
|
|
)
|
|
|
|
tau_human = computed_torque(
|
|
models.master,
|
|
master_data_control,
|
|
q_m,
|
|
qd_m,
|
|
q_m_ref,
|
|
qd_m_ref,
|
|
qdd_m_ref,
|
|
config.master_kp,
|
|
config.master_kd,
|
|
soft_limit_acceleration(
|
|
models.master,
|
|
q_m,
|
|
qd_m,
|
|
config.soft_limit_buffer,
|
|
),
|
|
)
|
|
tau_human, master_torque_clipped = _clip_actuator_torque(
|
|
models.master,
|
|
tau_human,
|
|
config.master_tracking_effort_fraction,
|
|
)
|
|
master_torque_saturation_events += int(master_torque_clipped)
|
|
qdd_m_raw = pin.aba(
|
|
models.master,
|
|
master_data_dynamics,
|
|
q_m,
|
|
qd_m,
|
|
tau_human + tau_master_applied,
|
|
)
|
|
master_acceleration_limits = np.asarray(
|
|
config.master_acceleration_limits,
|
|
dtype=float,
|
|
)
|
|
qdd_m = np.clip(
|
|
qdd_m_raw,
|
|
-master_acceleration_limits,
|
|
master_acceleration_limits,
|
|
)
|
|
master_acceleration_limited = bool(
|
|
np.any(np.abs(qdd_m_raw) > master_acceleration_limits)
|
|
)
|
|
master_acceleration_limit_events += int(master_acceleration_limited)
|
|
|
|
try:
|
|
differential_for_power = (
|
|
map_registry.get(selected_map_id).differential
|
|
if selected_map_id >= 0
|
|
else map_registry.latest.differential
|
|
)
|
|
except KeyError:
|
|
differential_for_power = np.zeros(
|
|
(models.slave.nv, models.master.nv), dtype=float
|
|
)
|
|
reference_slave_velocity = differential_for_power @ qd_m
|
|
raw_master_power = float(np.dot(tau_master_mapped, qd_m))
|
|
a_defined_slave_power = float(
|
|
np.dot(tau_slave_source_for_mapping, reference_slave_velocity)
|
|
)
|
|
a_port_identity_error = abs(
|
|
raw_master_power - a_defined_slave_power
|
|
)
|
|
source_slave_power = float(
|
|
np.dot(tau_slave_source_for_mapping, qd_slave_source)
|
|
)
|
|
actual_power_mismatch_abs = abs(
|
|
raw_master_power - source_slave_power
|
|
)
|
|
|
|
scalar_values = {
|
|
"time": t,
|
|
"sample_index": step,
|
|
"dt": config.dt,
|
|
"missed_deadline": 0,
|
|
"contact_force_norm": float(np.linalg.norm(wrench_external[:3])),
|
|
"wall_force_raw_N": wall_contact.force_raw_N,
|
|
"wall_force_applied_N": wall_contact.force_applied_N,
|
|
"wall_force_saturation_active": int(
|
|
wall_contact.saturation_active
|
|
),
|
|
"penetration": penetration,
|
|
"force_estimation_error_norm": float(
|
|
np.linalg.norm(wrench_estimated[:3] - wrench_external[:3])
|
|
),
|
|
"moment_estimation_error_norm": float(
|
|
np.linalg.norm(wrench_estimated[3:] - wrench_external[3:])
|
|
),
|
|
"raw_master_power": raw_master_power,
|
|
"a_defined_slave_power": a_defined_slave_power,
|
|
"a_port_identity_error": a_port_identity_error,
|
|
"source_slave_power": source_slave_power,
|
|
"actual_power_mismatch_abs": actual_power_mismatch_abs,
|
|
"actual_slave_environment_power": float(
|
|
np.dot(tau_slave_external, qd_s)
|
|
),
|
|
"candidate_power": candidate_power,
|
|
"applied_power": applied_power,
|
|
"rho": rho,
|
|
"energy_before": energy_before,
|
|
"tank_energy": tank_energy,
|
|
"energy_preclip": energy_preclip,
|
|
"shadow_energy": shadow_energy,
|
|
"popc_damping_gain": popc_damping_gain,
|
|
"map_id": selected_map_id,
|
|
"source_map_id": source_map_id,
|
|
"return_source_index": return_source_index,
|
|
"return_packet_age": return_packet_age,
|
|
"forward_packet_state": int(held_forward.state),
|
|
"return_packet_state": int(held_return.state),
|
|
"return_packet_active": int(return_packet_active),
|
|
"master_tracking_error": float(
|
|
np.linalg.norm(pin.difference(models.master, q_m, q_m_ref))
|
|
),
|
|
"slave_tracking_error": float(
|
|
np.linalg.norm(pin.difference(models.slave, q_s, q_s_ref))
|
|
),
|
|
"feedback_torque_norm": float(np.linalg.norm(tau_master_applied)),
|
|
"mapped_torque_norm": float(np.linalg.norm(tau_master_mapped)),
|
|
"energy_probe_raw_power_W": energy_probe_raw_power_W,
|
|
"energy_probe_raw_work_J": energy_probe_raw_work_J,
|
|
"energy_probe_envelope": energy_probe_envelope,
|
|
"master_acceleration_limit_active": int(
|
|
master_acceleration_limited
|
|
),
|
|
"slave_acceleration_limit_active": int(
|
|
slave_acceleration_limited
|
|
),
|
|
"master_torque_saturation_active": int(
|
|
master_torque_clipped
|
|
),
|
|
"slave_torque_saturation_active": int(torque_clipped),
|
|
"haptic_rate_limit_active": int(
|
|
renderer.last_rate_limit_active
|
|
),
|
|
"haptic_torque_saturation_active": int(
|
|
renderer.last_torque_saturation_active
|
|
),
|
|
}
|
|
vector_values = {
|
|
"q_master": q_m.copy(),
|
|
"qd_master": qd_m.copy(),
|
|
"q_master_ref": q_m_ref.copy(),
|
|
"q_slave": q_s.copy(),
|
|
"qd_slave": qd_s.copy(),
|
|
"q_slave_ref": q_s_ref.copy(),
|
|
"tau_slave_external": tau_slave_external.copy(),
|
|
"tau_slave_estimated": tau_slave_estimated.copy(),
|
|
"tau_slave_residual_source": delayed_tau_slave.copy(),
|
|
"tau_slave_matched_wrench": delayed_tau_matched.copy(),
|
|
"qd_slave_source": qd_slave_source.copy(),
|
|
"tau_master_mapped": tau_master_mapped.copy(),
|
|
"tau_master_candidate": tau_master_candidate.copy(),
|
|
"tau_master_applied": tau_master_applied.copy(),
|
|
"tau_master_accepted": tau_master_accepted.copy(),
|
|
"energy_probe_torque_Nm": energy_probe_torque.copy(),
|
|
"wrench_external": wrench_external.copy(),
|
|
"wrench_estimated": wrench_estimated.copy(),
|
|
"wrench_feedback_source": delayed_wrench.copy(),
|
|
"map_differential": differential_for_power.reshape(-1).copy(),
|
|
"tcp_position": tcp_position.copy(),
|
|
}
|
|
for key, value in scalar_values.items():
|
|
log_lists[key].append(float(value))
|
|
for key, value in vector_values.items():
|
|
log_lists[key].append(value)
|
|
|
|
q_m, qd_m, master_clipped, master_velocity_limited = integrate_state(
|
|
models.master,
|
|
q_m,
|
|
qd_m,
|
|
qdd_m,
|
|
config.dt,
|
|
MASTER_JOINT_NAMES,
|
|
config.joint_limit_margin,
|
|
config.velocity_limit_fraction,
|
|
)
|
|
q_s, qd_s, slave_clipped, slave_velocity_limited = integrate_state(
|
|
models.slave,
|
|
q_s,
|
|
qd_s,
|
|
qdd_s,
|
|
config.dt,
|
|
SLAVE_JOINT_NAMES,
|
|
config.joint_limit_margin,
|
|
config.velocity_limit_fraction,
|
|
)
|
|
master_limit_events += int(master_clipped)
|
|
slave_limit_events += int(slave_clipped)
|
|
master_velocity_limit_events += int(master_velocity_limited)
|
|
slave_velocity_limit_events += int(slave_velocity_limited)
|
|
log_lists["master_joint_limit_active"].append(
|
|
int(master_clipped)
|
|
)
|
|
log_lists["slave_joint_limit_active"].append(int(slave_clipped))
|
|
log_lists["master_velocity_limit_active"].append(
|
|
int(master_velocity_limited)
|
|
)
|
|
log_lists["slave_velocity_limit_active"].append(
|
|
int(slave_velocity_limited)
|
|
)
|
|
|
|
if not (
|
|
np.all(np.isfinite(q_m))
|
|
and np.all(np.isfinite(qd_m))
|
|
and np.all(np.isfinite(q_s))
|
|
and np.all(np.isfinite(qd_s))
|
|
):
|
|
raise FloatingPointError(
|
|
f"{scenario.key}: non-finite state at t={t:.6f} s"
|
|
)
|
|
|
|
runtime = time.perf_counter() - run_start
|
|
logs = {
|
|
key: np.asarray(values, dtype=float) for key, values in log_lists.items()
|
|
}
|
|
contact_mask = logs["penetration"] > 0.0
|
|
force_active_mask = logs["contact_force_norm"] > 1e-6
|
|
power_scale = _rms(logs["a_defined_slave_power"][contact_mask])
|
|
projection_mask = (
|
|
logs["rho"] < (1.0 - 1e-12)
|
|
if scenario.supervisor == "tank"
|
|
else logs["popc_damping_gain"] > 0.0
|
|
)
|
|
positive_power = np.maximum(logs["applied_power"], 0.0)
|
|
absorbed_power = np.maximum(-logs["applied_power"], 0.0)
|
|
actual_mismatch_numerator = float(
|
|
np.sum(logs["actual_power_mismatch_abs"]) * config.dt
|
|
)
|
|
actual_mismatch_denominator = float(
|
|
0.5
|
|
* np.sum(
|
|
np.abs(logs["raw_master_power"])
|
|
+ np.abs(logs["source_slave_power"])
|
|
)
|
|
* config.dt
|
|
+ 1e-12
|
|
)
|
|
finite_rho = logs["rho"][np.isfinite(logs["rho"])]
|
|
|
|
metrics = {
|
|
"completed": True,
|
|
"finite_state": True,
|
|
"simulated_duration_s": config.duration,
|
|
"wall_contact_fraction": float(np.mean(contact_mask)),
|
|
"wall_contact_duration_s": float(np.sum(contact_mask) * config.dt),
|
|
"wall_force_active_fraction": float(np.mean(force_active_mask)),
|
|
"peak_contact_force_N": float(np.max(logs["contact_force_norm"])),
|
|
"peak_wall_force_raw_N": float(
|
|
np.max(logs["wall_force_raw_N"])
|
|
),
|
|
"wall_force_limit_hit_fraction": float(
|
|
np.mean(logs["wall_force_saturation_active"] > 0.5)
|
|
),
|
|
"wall_force_headroom_min_N": float(
|
|
config.wall_force_limit
|
|
- np.max(logs["wall_force_applied_N"])
|
|
),
|
|
"max_penetration_mm": float(1e3 * np.max(logs["penetration"])),
|
|
"force_estimation_rmse_N": _rms(
|
|
logs["force_estimation_error_norm"]
|
|
),
|
|
"moment_estimation_rmse_Nm": _rms(
|
|
logs["moment_estimation_error_norm"]
|
|
),
|
|
"master_tracking_rmse_rad": _rms(logs["master_tracking_error"]),
|
|
"slave_tracking_rmse_rad": _rms(logs["slave_tracking_error"]),
|
|
"feedback_torque_rms_Nm": _rms(logs["feedback_torque_norm"]),
|
|
"feedback_torque_peak_Nm": float(
|
|
np.max(logs["feedback_torque_norm"])
|
|
),
|
|
"a_port_identity_error_rms_W": _rms(
|
|
logs["a_port_identity_error"]
|
|
),
|
|
"a_port_identity_error_max_W": float(
|
|
np.max(logs["a_port_identity_error"])
|
|
),
|
|
"a_port_identity_relative_rms": float(
|
|
_rms(logs["a_port_identity_error"][contact_mask])
|
|
/ max(power_scale, 1e-12)
|
|
),
|
|
"actual_power_mismatch_normalized": (
|
|
actual_mismatch_numerator / actual_mismatch_denominator
|
|
),
|
|
"slave_environment_net_work_J": float(
|
|
np.sum(logs["actual_slave_environment_power"]) * config.dt
|
|
),
|
|
"positive_energy_delivered_J": float(
|
|
np.sum(positive_power) * config.dt
|
|
),
|
|
"energy_absorbed_J": float(np.sum(absorbed_power) * config.dt),
|
|
"energy_probe_raw_work_J": float(
|
|
logs["energy_probe_raw_work_J"][-1]
|
|
),
|
|
"supervisor_intervention_fraction": float(np.mean(projection_mask)),
|
|
"energy_projection_fraction": (
|
|
float(np.mean(projection_mask))
|
|
if scenario.supervisor == "tank"
|
|
else None
|
|
),
|
|
"energy_projection_contact_fraction": float(
|
|
np.mean(projection_mask[contact_mask])
|
|
if scenario.supervisor == "tank" and np.any(contact_mask)
|
|
else 0.0
|
|
),
|
|
"rho_min": (
|
|
float(np.min(finite_rho)) if finite_rho.size else None
|
|
),
|
|
"tank_energy_min_J": _finite_or_none(
|
|
float(np.nanmin(logs["tank_energy"]))
|
|
if scenario.supervisor in ("tank", "popc")
|
|
else math.nan
|
|
),
|
|
"tank_energy_final_J": _finite_or_none(
|
|
float(logs["tank_energy"][-1])
|
|
if scenario.supervisor in ("tank", "popc")
|
|
else math.nan
|
|
),
|
|
"shadow_energy_min_J": float(np.min(logs["shadow_energy"])),
|
|
"shadow_energy_floor_violation_J": float(
|
|
max(0.0, config.energy_min - np.min(logs["shadow_energy"]))
|
|
),
|
|
"energy_accounting_max_error_J": (
|
|
float(max(energy_identity_errors, default=0.0))
|
|
if scenario.supervisor == "tank"
|
|
else None
|
|
),
|
|
"mapping_updates": map_update_count,
|
|
"mapping_pose_success_rate": float(
|
|
map_pose_success_count / max(map_update_count, 1)
|
|
),
|
|
"differential_valid_rate": float(
|
|
differential_valid_count / max(map_update_count, 1)
|
|
),
|
|
"differential_fallback_count": differential_fallback_count,
|
|
"mapping_runtime_median_ms": float(
|
|
np.median(mapping_runtimes_ms) if mapping_runtimes_ms else 0.0
|
|
),
|
|
"mapping_runtime_p95_ms": float(
|
|
np.percentile(mapping_runtimes_ms, 95.0)
|
|
if mapping_runtimes_ms
|
|
else 0.0
|
|
),
|
|
"mapping_runtime_max_ms": float(
|
|
max(mapping_runtimes_ms, default=0.0)
|
|
),
|
|
"master_joint_limit_events": master_limit_events,
|
|
"slave_joint_limit_events": slave_limit_events,
|
|
"master_velocity_limit_events": master_velocity_limit_events,
|
|
"slave_velocity_limit_events": slave_velocity_limit_events,
|
|
"master_acceleration_limit_events": master_acceleration_limit_events,
|
|
"slave_acceleration_limit_events": slave_acceleration_limit_events,
|
|
"master_torque_saturation_events": master_torque_saturation_events,
|
|
"slave_torque_saturation_events": slave_torque_saturation_events,
|
|
"haptic_rate_limit_events": haptic_rate_limit_events,
|
|
"haptic_torque_saturation_events": haptic_torque_saturation_events,
|
|
"forward_packets_accepted": forward_packets_accepted,
|
|
"forward_packets_rejected": forward_packets_rejected,
|
|
"return_packets_accepted": return_packets_accepted,
|
|
"return_packets_rejected": return_packets_rejected,
|
|
"forward_timeout_steps": forward_timeout_steps,
|
|
"return_timeout_steps": return_timeout_steps,
|
|
"wall_contact_steps": contact_steps,
|
|
"wall_time_s": runtime,
|
|
}
|
|
return ScenarioResult(scenario=scenario, metrics=metrics, logs=logs)
|
|
|
|
|
|
def _write_npz(path: Path, logs: dict[str, np.ndarray]) -> None:
|
|
np.savez_compressed(path, **logs)
|
|
|
|
|
|
def _write_csv(path: Path, logs: dict[str, np.ndarray]) -> None:
|
|
scalar_keys = [
|
|
key for key, value in logs.items() if value.ndim == 1
|
|
]
|
|
vector_keys = [
|
|
key for key, value in logs.items() if value.ndim == 2
|
|
]
|
|
fieldnames = scalar_keys + [
|
|
f"{key}_{index}"
|
|
for key in vector_keys
|
|
for index in range(logs[key].shape[1])
|
|
]
|
|
with path.open("w", encoding="utf-8", newline="") as stream:
|
|
writer = csv.DictWriter(stream, fieldnames=fieldnames)
|
|
writer.writeheader()
|
|
row_count = logs["time"].shape[0]
|
|
for row_index in range(row_count):
|
|
row: dict[str, float] = {
|
|
key: float(logs[key][row_index]) for key in scalar_keys
|
|
}
|
|
for key in vector_keys:
|
|
for column_index, value in enumerate(logs[key][row_index]):
|
|
row[f"{key}_{column_index}"] = float(value)
|
|
writer.writerow(row)
|
|
|
|
|
|
def make_comparison_plot(
|
|
results: list[ScenarioResult],
|
|
config: SimulationConfig,
|
|
output_path: Path,
|
|
) -> None:
|
|
import matplotlib
|
|
|
|
matplotlib.use("Agg")
|
|
import matplotlib.pyplot as plt
|
|
|
|
colors = {
|
|
"proposed_energy": "#0072B2",
|
|
"direct_energy": "#D55E00",
|
|
"proposed_no_energy": "#009E73",
|
|
"matched_wrench_energy": "#CC79A7",
|
|
"proposed_popc": "#E69F00",
|
|
}
|
|
labels = {
|
|
"proposed_energy": r"$A^\mathsf{T}$ + final energy",
|
|
"direct_energy": r"$J_m^\mathsf{T}F$ + final energy",
|
|
"proposed_no_energy": r"$A^\mathsf{T}$, no energy projection",
|
|
"matched_wrench_energy": (
|
|
r"$A_\mathrm{src}^\mathsf{T}J_s^\mathsf{T}F$ + final energy"
|
|
),
|
|
"proposed_popc": r"$A^\mathsf{T}$ + time-domain PO/PC",
|
|
}
|
|
fig, axes = plt.subplots(3, 2, figsize=(13.0, 10.0), sharex=True)
|
|
|
|
for result in results:
|
|
key = result.scenario.key
|
|
color = colors[key]
|
|
label = labels[key]
|
|
time_axis = result.logs["time"]
|
|
axes[0, 0].plot(
|
|
time_axis,
|
|
result.logs["contact_force_norm"],
|
|
color=color,
|
|
label=label,
|
|
)
|
|
axes[0, 1].semilogy(
|
|
time_axis,
|
|
np.maximum(result.logs["a_port_identity_error"], 1e-14),
|
|
color=color,
|
|
label=label,
|
|
)
|
|
axes[1, 0].plot(
|
|
time_axis,
|
|
result.logs["feedback_torque_norm"],
|
|
color=color,
|
|
label=label,
|
|
)
|
|
supervisor_trace = (
|
|
(result.logs["popc_damping_gain"] > 0.0).astype(float)
|
|
if result.scenario.supervisor == "popc"
|
|
else result.logs["rho"]
|
|
)
|
|
axes[1, 1].plot(
|
|
time_axis,
|
|
supervisor_trace,
|
|
color=color,
|
|
label=label,
|
|
)
|
|
if result.scenario.supervisor in ("tank", "popc"):
|
|
axes[2, 0].plot(
|
|
time_axis,
|
|
result.logs["tank_energy"],
|
|
color=color,
|
|
label=label,
|
|
)
|
|
else:
|
|
axes[2, 0].plot(
|
|
time_axis,
|
|
result.logs["shadow_energy"],
|
|
color=color,
|
|
linestyle="--",
|
|
label="unsupervised counterfactual reserve",
|
|
)
|
|
axes[2, 1].plot(
|
|
time_axis,
|
|
1e3 * result.logs["penetration"],
|
|
color=color,
|
|
label=label,
|
|
)
|
|
|
|
axes[0, 0].set_ylabel("contact force [N]")
|
|
axes[0, 0].set_title("Closed-loop wall interaction")
|
|
axes[0, 1].set_ylabel("A-port identity error [W]")
|
|
axes[0, 1].set_title(
|
|
"Raw A-port power consistency (plot floor $10^{-14}$ W)"
|
|
)
|
|
axes[1, 0].set_ylabel(r"$\|\tau_{m,app}\|$ [N m]")
|
|
axes[1, 0].set_title("Applied master haptic torque")
|
|
axes[1, 1].set_ylabel(r"tank $\rho$ / PO-PC active")
|
|
axes[1, 1].set_ylim(-0.04, 1.04)
|
|
axes[1, 1].set_title("Final-port supervisor activity")
|
|
axes[2, 0].axhline(
|
|
config.energy_min,
|
|
color="black",
|
|
linestyle="--",
|
|
linewidth=1.0,
|
|
label="configured E_min",
|
|
)
|
|
axes[2, 0].set_ylabel("tank energy [J]")
|
|
axes[2, 0].set_title("Accounted / counterfactual energy reserve")
|
|
axes[2, 1].set_ylabel("wall penetration [mm]")
|
|
axes[2, 1].set_title("Slave TCP penetration")
|
|
for axis in axes[-1, :]:
|
|
axis.set_xlabel("time [s]")
|
|
for axis in axes.flat:
|
|
axis.grid(True, alpha=0.3)
|
|
axes[0, 0].legend(loc="best", fontsize=8)
|
|
axes[2, 0].legend(loc="best", fontsize=8)
|
|
fig.suptitle(
|
|
"Simulation only — master_7dof.urdf / real_slave_7dof.urdf",
|
|
fontsize=13,
|
|
)
|
|
fig.tight_layout(rect=(0.0, 0.0, 1.0, 0.97))
|
|
fig.savefig(output_path, dpi=180)
|
|
plt.close(fig)
|
|
|
|
|
|
def _json_ready(value: Any) -> Any:
|
|
if isinstance(value, np.ndarray):
|
|
return value.tolist()
|
|
if isinstance(value, (np.floating, np.integer)):
|
|
return value.item()
|
|
if isinstance(value, dict):
|
|
return {key: _json_ready(item) for key, item in value.items()}
|
|
if isinstance(value, (tuple, list)):
|
|
return [_json_ready(item) for item in value]
|
|
return value
|
|
|
|
|
|
def run_comparison(
|
|
config: SimulationConfig,
|
|
output_dir: Path,
|
|
scenarios: tuple[Scenario, ...] = SCENARIOS,
|
|
) -> dict[str, Any]:
|
|
config.validate()
|
|
output_dir.mkdir(parents=True, exist_ok=True)
|
|
models = load_models(add_simulated_tcp=True)
|
|
wall_mapper = build_mapper(models)
|
|
wall, wall_metadata, q_slave_start = make_wall(
|
|
config,
|
|
models,
|
|
wall_mapper,
|
|
)
|
|
|
|
results: list[ScenarioResult] = []
|
|
for scenario in scenarios:
|
|
print(f"[simulation] running {scenario.key}: {scenario.description}")
|
|
result = simulate_scenario(
|
|
scenario,
|
|
config,
|
|
models,
|
|
wall,
|
|
q_slave_start,
|
|
)
|
|
results.append(result)
|
|
_write_npz(output_dir / f"{scenario.key}.npz", result.logs)
|
|
_write_csv(output_dir / f"{scenario.key}.csv", result.logs)
|
|
|
|
make_comparison_plot(
|
|
results,
|
|
config,
|
|
output_dir / "closed_loop_comparison.png",
|
|
)
|
|
summary = {
|
|
"evidence_scope": (
|
|
"Rigid-body closed-loop simulation only; no prototype or human "
|
|
"subject result is claimed."
|
|
),
|
|
"metric_definitions_and_limits": {
|
|
"a_port_identity": (
|
|
"Compares tau_mapped^T qd_m with delayed_tau_s^T "
|
|
"(A qd_m). It is an algebraic implementation-consistency "
|
|
"metric, not equality of the actual master/slave port powers."
|
|
),
|
|
"contact": (
|
|
"wall_contact_fraction/duration/steps use penetration > 0; "
|
|
"wall_force_active_fraction separately uses force norm > 1e-6."
|
|
),
|
|
"energy_stress_protocol": (
|
|
"The tank starts at E_min with only "
|
|
f"{config.energy_max - config.energy_min:.6g} J capacity above "
|
|
"the floor. This deliberately tight budget exercises the "
|
|
"projection and is not a hardware tuning recommendation."
|
|
),
|
|
"shadow_energy": (
|
|
"For no-energy, this reconstructs that scenario's actual "
|
|
"unprojected candidate sequence. For supervised scenarios, "
|
|
"it is a stepwise witness on the supervised closed-loop "
|
|
"candidate sequence, not a second counterfactual simulation."
|
|
),
|
|
"plotting_floor_W": 1e-14,
|
|
"estimator_scope": (
|
|
"Torque bias/noise and load-side measurements are synthetic, "
|
|
"and plant/estimator share one URDF model; hardware robustness "
|
|
"is therefore not established."
|
|
),
|
|
"claims_not_supported": (
|
|
"No claim of prototype performance, human-in-the-loop "
|
|
"stability, global passivity, delay robustness, transparency, "
|
|
"or workspace-wide/statistical robustness."
|
|
),
|
|
"ablation_scope": (
|
|
"proposed_no_energy bypasses only the final energy projection. "
|
|
"It retains feedback gain/filtering, haptic rate and torque "
|
|
"limits, tracking effort limits, acceleration/velocity/position "
|
|
"guards, soft joint-limit guards, and the wall force cap."
|
|
),
|
|
"feedback_chain_comparison": (
|
|
"A.T receives the estimated joint residual, whereas the direct "
|
|
"baseline receives its DLS wrench projection. This is an "
|
|
"end-to-end feedback-chain ablation, not a same-input pure "
|
|
"matrix comparison."
|
|
),
|
|
"slave_reference_sampling": (
|
|
"q_s_ref and qd_s_ref are both sampled and held at the 50 Hz "
|
|
"mapping update rate; qdd_s_ref is zero."
|
|
),
|
|
},
|
|
"models": {
|
|
"master_urdf": str(MASTER_URDF),
|
|
"slave_urdf": str(SLAVE_URDF),
|
|
"slave_contact_frame": config.slave_contact_frame,
|
|
"contact_frame_status": (
|
|
"R_EE_SIM is a simulation-only fixed frame from the repository "
|
|
"MJCF marker; a measured TCP/FT transform is required for hardware"
|
|
if config.slave_contact_frame == SLAVE_FRAMES["ee"]
|
|
else "terminal frame present in real_slave_7dof.urdf"
|
|
),
|
|
},
|
|
"config": asdict(config),
|
|
"wall": wall_metadata,
|
|
"scenarios": {
|
|
result.scenario.key: {
|
|
"mapping": result.scenario.mapping,
|
|
"energy_supervision": result.scenario.supervise_energy,
|
|
"supervisor": result.scenario.supervisor,
|
|
"map_policy": result.scenario.map_policy,
|
|
"description": result.scenario.description,
|
|
"metrics": result.metrics,
|
|
}
|
|
for result in results
|
|
},
|
|
"artifacts": {
|
|
"plot": "closed_loop_comparison.png",
|
|
"machine_readable_logs": [
|
|
f"{result.scenario.key}.npz" for result in results
|
|
],
|
|
"tabular_logs": [
|
|
f"{result.scenario.key}.csv" for result in results
|
|
],
|
|
},
|
|
}
|
|
with (output_dir / "summary.json").open("w", encoding="utf-8") as stream:
|
|
json.dump(_json_ready(summary), stream, indent=2, ensure_ascii=False)
|
|
|
|
print("\nSimulation-only comparison")
|
|
print(
|
|
"scenario contact[N] A-port err[W] "
|
|
"rho_min E_min[J] map p95[ms]"
|
|
)
|
|
for result in results:
|
|
metrics = result.metrics
|
|
tank_min = metrics["tank_energy_min_J"]
|
|
tank_text = " n/a" if tank_min is None else f"{tank_min:8.4f}"
|
|
rho_min = metrics["rho_min"]
|
|
rho_text = " n/a" if rho_min is None else f"{rho_min:9.3f}"
|
|
print(
|
|
f"{result.scenario.key:24s}"
|
|
f"{metrics['peak_contact_force_N']:10.3f}"
|
|
f"{metrics['a_port_identity_error_rms_W']:15.3e}"
|
|
f"{rho_text}"
|
|
f"{tank_text}"
|
|
f"{metrics['mapping_runtime_p95_ms']:13.3f}"
|
|
)
|
|
print(f"\nArtifacts: {output_dir}")
|
|
return summary
|
|
|
|
|
|
def parse_args() -> argparse.Namespace:
|
|
parser = argparse.ArgumentParser(
|
|
description="Run the closed-loop bilateral simulation comparison."
|
|
)
|
|
parser.add_argument(
|
|
"--config",
|
|
type=Path,
|
|
default=DEFAULT_CONFIG_PATH,
|
|
help=f"YAML configuration (default: {DEFAULT_CONFIG_PATH})",
|
|
)
|
|
parser.add_argument(
|
|
"--output-dir",
|
|
type=Path,
|
|
default=DEFAULT_OUTPUT_DIR,
|
|
help=f"artifact directory (default: {DEFAULT_OUTPUT_DIR})",
|
|
)
|
|
parser.add_argument(
|
|
"--duration",
|
|
type=float,
|
|
default=None,
|
|
help="override simulated duration per scenario in seconds",
|
|
)
|
|
parser.add_argument(
|
|
"--dt",
|
|
type=float,
|
|
default=None,
|
|
help="override dynamics integration period in seconds",
|
|
)
|
|
parser.add_argument(
|
|
"--mapping-hz",
|
|
type=float,
|
|
default=None,
|
|
help="override retargeting/differential update rate",
|
|
)
|
|
return parser.parse_args()
|
|
|
|
|
|
def main() -> None:
|
|
args = parse_args()
|
|
config = load_simulation_config(args.config.resolve())
|
|
overrides = {
|
|
key: value
|
|
for key, value in {
|
|
"dt": args.dt,
|
|
"duration": args.duration,
|
|
"mapping_hz": args.mapping_hz,
|
|
}.items()
|
|
if value is not None
|
|
}
|
|
if overrides:
|
|
config = replace(config, **overrides)
|
|
run_comparison(config, args.output_dir.resolve())
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|