#!/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()