From aee9e2f91961952b58c50fc2f83a1d740909a22c Mon Sep 17 00:00:00 2001 From: JohnneyLee <110317240+JohnneyLee@users.noreply.github.com> Date: Thu, 25 Jun 2026 10:23:15 +0800 Subject: [PATCH 1/3] Add networked dynamic PETC controller wrapper --- INTEGRATION_MANIFEST.md | 68 +++++++ README_NETWORKED_PETC_PR.md | 57 ++++++ docs/networked_control.md | 83 ++++++++ .../networked_lqr_experiment.py | 83 ++++++++ .../networked_control/run_dynamic_petc.py | 88 +++++++++ .../controllers/networked/__init__.py | 36 ++++ .../controllers/networked/channel.py | 187 ++++++++++++++++++ .../controllers/networked/metrics.py | 48 +++++ .../controllers/networked/scheduler.py | 62 ++++++ .../controllers/networked/trigger.py | 112 +++++++++++ .../controllers/networked/wrapper.py | 167 ++++++++++++++++ .../networked/test_delay_channel.py | 33 ++++ tests/controllers/networked/test_trigger.py | 36 ++++ tests/controllers/networked/test_wrapper.py | 41 ++++ 14 files changed, 1101 insertions(+) create mode 100644 INTEGRATION_MANIFEST.md create mode 100644 README_NETWORKED_PETC_PR.md create mode 100644 docs/networked_control.md create mode 100644 examples/networked_control/networked_lqr_experiment.py create mode 100644 examples/networked_control/run_dynamic_petc.py create mode 100644 safe_control_gym/controllers/networked/__init__.py create mode 100644 safe_control_gym/controllers/networked/channel.py create mode 100644 safe_control_gym/controllers/networked/metrics.py create mode 100644 safe_control_gym/controllers/networked/scheduler.py create mode 100644 safe_control_gym/controllers/networked/trigger.py create mode 100644 safe_control_gym/controllers/networked/wrapper.py create mode 100644 tests/controllers/networked/test_delay_channel.py create mode 100644 tests/controllers/networked/test_trigger.py create mode 100644 tests/controllers/networked/test_wrapper.py diff --git a/INTEGRATION_MANIFEST.md b/INTEGRATION_MANIFEST.md new file mode 100644 index 000000000..b8c3f7110 --- /dev/null +++ b/INTEGRATION_MANIFEST.md @@ -0,0 +1,68 @@ +# Integration Manifest + +## Target Repository + +`learnsyslab/safe-control-gym` + +## Copy These Paths Into a Branch + +```text +safe_control_gym/controllers/networked/ +examples/networked_control/ +tests/controllers/networked/ +docs/networked_control.md +README_NETWORKED_PETC_PR.md +``` + +## Minimal PR Scope + +This contribution adds an optional wrapper. It does not alter existing +controllers, environments, tasks, or experiment code paths unless a user imports +`safe_control_gym.controllers.networked`. + +## Smoke Commands + +```bash +python examples/networked_control/run_dynamic_petc.py --max-delay-steps 3 +python -m pytest tests/controllers/networked -q +``` + +After installing the full safe-control-gym dependencies, run: + +```bash +python examples/networked_control/networked_lqr_experiment.py \ + --algo lqr \ + --task cartpole \ + --overrides ./examples/lqr/config_overrides/cartpole/cartpole_stab.yaml \ + ./examples/lqr/config_overrides/cartpole/lqr_cartpole_stab.yaml +``` + +## Before/After Protocol + +Use the same seed, task, controller, and number of steps. + +```text +Before: base controller with fresh observation every step. +After A: base controller wrapped with fixed periodic delayed transmission. +After B: base controller wrapped with DynamicPETCTrigger. +``` + +Report both task and network metrics: + +```text +task return or RMS tracking error +constraint/safety violation count when available +transmission count +event rate +mean stale steps +max queue depth +mean eta +mean trigger margin +``` + +## Reviewer-Facing Claim + +The module provides a reusable engineering abstraction for networked control +evaluation under sampled, delayed, event-triggered communication. It does not +claim closed-loop ISS for arbitrary safe-control-gym tasks unless the user +supplies compatible task-specific certificate bounds. diff --git a/README_NETWORKED_PETC_PR.md b/README_NETWORKED_PETC_PR.md new file mode 100644 index 000000000..43968a5a9 --- /dev/null +++ b/README_NETWORKED_PETC_PR.md @@ -0,0 +1,57 @@ +# PR: Networked Dynamic PETC Controller Wrapper + +## Purpose + +This overlay adds a practical networked-control wrapper to safe-control-gym. It +lets existing controllers run behind sampled, delayed, event-triggered +communication channels. + +## Files to Add + +```text +safe_control_gym/controllers/networked/ + __init__.py + channel.py + metrics.py + scheduler.py + trigger.py + wrapper.py +examples/networked_control/run_dynamic_petc.py +examples/networked_control/networked_lqr_experiment.py +tests/controllers/networked/ + test_delay_channel.py + test_trigger.py + test_wrapper.py +docs/networked_control.md +``` + +## Public API + +```python +NetworkedControllerWrapper(controller, observation_dim, sample_period_steps, + max_delay_steps, delay_sampler, trigger) +DynamicPETCTrigger(error_weight, signal_weight, eta_decay, eta_growth) +DelayChannel(name, dim, sample_period_steps, max_delay_steps, delay_sampler) +``` + +## Before/After Demonstration + +The minimal demonstration compares a direct proportional controller against the +same controller wrapped by dynamic PETC and bounded random delays: + +```bash +python examples/networked_control/run_dynamic_petc.py --max-delay-steps 3 +``` + +For a full safe-control-gym PR, the same wrapper can be applied to the existing +LQR/MPC examples by replacing `ctrl` with `NetworkedControllerWrapper(ctrl, ...)` +before constructing `BaseExperiment`. + +## Review Checklist + +- The base controller API is unchanged. +- Existing controllers do not import the networked package. +- Delays are measured in sample steps, not seconds. +- The delay channel preserves FIFO delivery order. +- The dynamic trigger is evaluated only at sampling instants. +- The example reports communication metrics separately from task metrics. diff --git a/docs/networked_control.md b/docs/networked_control.md new file mode 100644 index 000000000..7ff77c5da --- /dev/null +++ b/docs/networked_control.md @@ -0,0 +1,83 @@ +# Networked Controller Wrapper + +This module adds a controller wrapper for sampled, delayed, bandwidth-limited +communication. It does not replace LQR, MPC, PID, or learning controllers. It +wraps them and changes which observation reaches the controller at each step. + +## Theory Mapping + +- `max_delay_steps` maps to the maximum allowable delay number in sampling. +- `DelayChannel.queue_depth` maps to the number of transmitted samples waiting + for delivery. +- `sampled_signal` is the source-side sampled signal. +- `held_signal` is the destination-side signal used by the wrapped controller. +- `update_error = held_signal - sampled_signal`. +- `DynamicPETCTrigger.eta` is the dynamic event-trigger memory. + +## Quick Start + +```python +from safe_control_gym.controllers.networked import ( + DynamicPETCTrigger, + NetworkedControllerWrapper, + uniform_delay, +) + +ctrl = make(config.algo, env_func, **config.algo_config) +networked_ctrl = NetworkedControllerWrapper( + ctrl, + observation_dim=env.observation_space.shape[0], + sample_period_steps=1, + max_delay_steps=3, + delay_sampler=uniform_delay(3), + trigger=DynamicPETCTrigger(error_weight=1.0, signal_weight=0.03), +) +``` + +Use `networked_ctrl` anywhere a safe-control-gym controller instance is accepted. + +## Before/After Evaluation + +Run three conditions with the same task, seed, and base controller: + +1. Direct controller with fresh observations. +2. Periodic controller with fixed delayed transmissions. +3. Dynamic PETC wrapper with the same delay sampler. + +Report: + +- task return or tracking error +- transmission count +- event rate +- mean stale steps +- max queue depth +- safety or constraint violations when the task exposes them + +## Example + +```bash +python examples/networked_control/run_dynamic_petc.py --max-delay-steps 3 +``` + +The example uses a small local double-integrator task. It is a smoke test for +the wrapper, not a reproduction of a paper simulation. + +To run a configured safe-control-gym controller behind the wrapper: + +```bash +python examples/networked_control/networked_lqr_experiment.py \ + --algo lqr \ + --task cartpole \ + --overrides ./examples/lqr/config_overrides/cartpole/cartpole_stab.yaml \ + ./examples/lqr/config_overrides/cartpole/lqr_cartpole_stab.yaml +``` + +The integration point is intentionally small: create the normal controller, then +wrap it with `NetworkedControllerWrapper` before constructing `BaseExperiment`. + +## Limits + +The wrapper exposes certificate-like quantities such as update error, queue +depth, and trigger margin. It does not claim plant-level input-to-state +stability unless the user supplies compatible Lyapunov/error weights for the +task. diff --git a/examples/networked_control/networked_lqr_experiment.py b/examples/networked_control/networked_lqr_experiment.py new file mode 100644 index 000000000..9b87d44b7 --- /dev/null +++ b/examples/networked_control/networked_lqr_experiment.py @@ -0,0 +1,83 @@ +'''Run an existing safe-control-gym controller through a networked wrapper.''' + +from functools import partial +import sys +from pathlib import Path + +import numpy as np + +PROJECT_ROOT = Path(__file__).resolve().parents[2] +if str(PROJECT_ROOT) not in sys.path: + sys.path.insert(0, str(PROJECT_ROOT)) + +from safe_control_gym.controllers.networked import DynamicPETCTrigger, NetworkedControllerWrapper, uniform_delay +from safe_control_gym.experiments.base_experiment import BaseExperiment +from safe_control_gym.utils.configuration import ConfigFactory +from safe_control_gym.utils.registration import make + + +def run(gui=False, n_episodes=1, n_steps=None, max_delay_steps=3, sample_period_steps=1, seed=0): + '''Run the configured controller with event-triggered delayed observations. + + The configured controller can be LQR, iLQR, MPC, or another controller with + the standard safe-control-gym select_action(obs, info) method. + ''' + + config = ConfigFactory().merge() + env_func = partial(make, config.task, **config.task_config) + random_env = env_func(gui=False) + + base_ctrl = make(config.algo, env_func, **config.algo_config) + rng = np.random.default_rng(seed) + + all_network_metrics = [] + n_episodes = 1 if n_episodes is None else n_episodes + + for _ in range(n_episodes): + init_obs, _ = random_env.reset() + static_env = env_func(gui=gui, randomized_init=False, init_state=init_obs) + static_train_env = env_func(gui=False, randomized_init=False, init_state=init_obs) + + obs_dim = int(np.asarray(init_obs).reshape(-1).size) + ctrl = NetworkedControllerWrapper( + base_ctrl, + observation_dim=obs_dim, + sample_period_steps=sample_period_steps, + max_delay_steps=max_delay_steps, + delay_sampler=uniform_delay(max_delay_steps, rng=rng), + trigger=DynamicPETCTrigger( + error_weight=1.0, + signal_weight=0.03, + eta_decay=0.02, + eta_growth=0.02, + eta_skip_gain=0.15, + eta_transmit_drop=0.3, + eta_max=0.5, + ), + ) + ctrl.reset_before_run(obs=init_obs, env=static_env) + + experiment = BaseExperiment(env=static_env, ctrl=ctrl, train_env=static_train_env) + experiment.launch_training() + if n_steps is None: + trajs_data, _ = experiment.run_evaluation(training=True, n_episodes=1) + else: + trajs_data, _ = experiment.run_evaluation(training=True, n_steps=n_steps) + + metrics = experiment.compute_metrics(trajs_data) + network_metrics = ctrl.metrics.summary() + all_network_metrics.append(network_metrics) + + print('FINAL METRICS - ' + ', '.join([f'{key}: {value}' for key, value in metrics.items()])) + print('NETWORK METRICS - ' + ', '.join([f'{key}: {value}' for key, value in network_metrics.items()])) + + static_env.close() + static_train_env.close() + + base_ctrl.close() + random_env.close() + return all_network_metrics + + +if __name__ == '__main__': + run() diff --git a/examples/networked_control/run_dynamic_petc.py b/examples/networked_control/run_dynamic_petc.py new file mode 100644 index 000000000..ae0346ade --- /dev/null +++ b/examples/networked_control/run_dynamic_petc.py @@ -0,0 +1,88 @@ +'''Minimal dynamic PETC wrapper demonstration. + +This example intentionally uses a small local plant instead of recreating a +paper example. In safe-control-gym, replace ProportionalController with an LQR, +MPC, PPO, SAC, or PID controller instance and keep the wrapper unchanged. +''' + +import argparse +import sys +from pathlib import Path + +import numpy as np + +PROJECT_ROOT = Path(__file__).resolve().parents[2] +if str(PROJECT_ROOT) not in sys.path: + sys.path.insert(0, str(PROJECT_ROOT)) + +from safe_control_gym.controllers.networked import DynamicPETCTrigger, NetworkedControllerWrapper, uniform_delay + + +class ProportionalController: + '''Small controller with the same select_action shape used by safe-control-gym.''' + + def select_action(self, obs, info=None): + obs = np.asarray(obs, dtype=float) + return np.array([-0.8 * obs[0] - 0.25 * obs[1]]) + + def reset(self): + return None + + def close(self): + return None + + +def simulate(controller, n_steps=200, dt=0.05): + '''Run a double-integrator stabilization task.''' + x = np.array([2.0, 0.0]) + trajectory = [] + for step in range(n_steps): + action = np.asarray(controller.select_action(x, info={'current_step': step}), dtype=float) + u = float(np.clip(action[0], -3.0, 3.0)) + x = np.array([x[0] + dt * x[1], x[1] + dt * u]) + trajectory.append((step, x[0], x[1], u)) + return np.asarray(trajectory) + + +def rms_position(trajectory): + return float(np.sqrt(np.mean(trajectory[:, 1] ** 2))) + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('--steps', type=int, default=200) + parser.add_argument('--max-delay-steps', type=int, default=3) + parser.add_argument('--sample-period-steps', type=int, default=1) + parser.add_argument('--seed', type=int, default=4) + args = parser.parse_args() + + rng = np.random.default_rng(args.seed) + base = ProportionalController() + direct = simulate(base, n_steps=args.steps) + + networked = NetworkedControllerWrapper( + ProportionalController(), + observation_dim=2, + sample_period_steps=args.sample_period_steps, + max_delay_steps=args.max_delay_steps, + delay_sampler=uniform_delay(args.max_delay_steps, rng=rng), + trigger=DynamicPETCTrigger( + error_weight=1.0, + signal_weight=0.03, + eta_decay=0.02, + eta_growth=0.02, + eta_skip_gain=0.15, + eta_transmit_drop=0.3, + eta_max=0.5, + ), + ) + networked.reset_before_run(obs=np.array([2.0, 0.0])) + delayed = simulate(networked, n_steps=args.steps) + + print('direct_rms_position:', f'{rms_position(direct):.4f}') + print('networked_rms_position:', f'{rms_position(delayed):.4f}') + print('networked_metrics:', networked.metrics.summary()) + + +if __name__ == '__main__': + main() diff --git a/safe_control_gym/controllers/networked/__init__.py b/safe_control_gym/controllers/networked/__init__.py new file mode 100644 index 000000000..d8ff78a63 --- /dev/null +++ b/safe_control_gym/controllers/networked/__init__.py @@ -0,0 +1,36 @@ +'''Networked-control wrappers and event-triggered communication models.''' + +from safe_control_gym.controllers.networked.channel import ( + ChannelPacket, + ChannelState, + DelayChannel, + constant_delay, + uniform_delay, +) +from safe_control_gym.controllers.networked.metrics import NetworkedControlMetrics +from safe_control_gym.controllers.networked.scheduler import ( + FullStateScheduler, + RoundRobinScheduler, + ScheduleDecision, + TryOnceDiscardScheduler, +) +from safe_control_gym.controllers.networked.trigger import DynamicPETCTrigger, StaticPETCTrigger, TriggerDecision +from safe_control_gym.controllers.networked.wrapper import NetworkedControllerWrapper, NetworkedStepInfo + +__all__ = [ + 'ChannelPacket', + 'ChannelState', + 'DelayChannel', + 'DynamicPETCTrigger', + 'FullStateScheduler', + 'NetworkedControlMetrics', + 'NetworkedControllerWrapper', + 'NetworkedStepInfo', + 'RoundRobinScheduler', + 'ScheduleDecision', + 'StaticPETCTrigger', + 'TriggerDecision', + 'TryOnceDiscardScheduler', + 'constant_delay', + 'uniform_delay', +] diff --git a/safe_control_gym/controllers/networked/channel.py b/safe_control_gym/controllers/networked/channel.py new file mode 100644 index 000000000..94dfa1ec7 --- /dev/null +++ b/safe_control_gym/controllers/networked/channel.py @@ -0,0 +1,187 @@ +'''Sampled communication channels with bounded delivery delay.''' + +from dataclasses import dataclass, field +from typing import Callable, List, Optional + +import numpy as np + + +ArrayLike = np.ndarray +DelaySampler = Callable[[int, int], int] + + +def constant_delay(delay_steps: int) -> DelaySampler: + '''Return a deterministic delay sampler.''' + delay_steps = int(delay_steps) + + def _sample(_sample_count: int, _transmission_count: int) -> int: + return delay_steps + + return _sample + + +def uniform_delay(max_delay_steps: int, rng: Optional[np.random.Generator] = None) -> DelaySampler: + '''Return an integer delay sampler on [0, max_delay_steps].''' + max_delay_steps = int(max_delay_steps) + rng = np.random.default_rng() if rng is None else rng + + def _sample(_sample_count: int, _transmission_count: int) -> int: + return int(rng.integers(0, max_delay_steps + 1)) + + return _sample + + +@dataclass +class ChannelPacket: + '''A transmitted sample waiting for delivery.''' + + value: ArrayLike + sampled_step: int + transmit_step: int + arrival_step: int + + +@dataclass +class ChannelState: + '''Observable state of one networked channel.''' + + sampled_signal: ArrayLike + held_signal: ArrayLike + update_error: ArrayLike + queue_depth: int + max_delay_steps: int + sample_count: int = 0 + transmission_count: int = 0 + delivery_count: int = 0 + skipped_count: int = 0 + stale_steps: int = 0 + last_sample_step: int = 0 + last_delivery_step: int = 0 + + +@dataclass +class DelayChannel: + '''A FIFO sampled-data channel with a maximum delay measured in samples. + + The class models the paper's practical communication objects: sampled + signal, held destination signal, update error, and the delay backlog l_i. + ''' + + name: str + dim: int + sample_period_steps: int = 1 + max_delay_steps: int = 0 + delay_sampler: Optional[DelaySampler] = None + initial_value: Optional[ArrayLike] = None + _queue: List[ChannelPacket] = field(default_factory=list, init=False) + _sampled_signal: ArrayLike = field(default=None, init=False) + _held_signal: ArrayLike = field(default=None, init=False) + _last_sample_step: int = field(default=0, init=False) + _last_delivery_step: int = field(default=0, init=False) + _last_arrival_step: int = field(default=0, init=False) + _sample_count: int = field(default=0, init=False) + _transmission_count: int = field(default=0, init=False) + _delivery_count: int = field(default=0, init=False) + _skipped_count: int = field(default=0, init=False) + + def __post_init__(self): + if self.sample_period_steps <= 0: + raise ValueError('sample_period_steps must be positive.') + if self.max_delay_steps < 0: + raise ValueError('max_delay_steps must be non-negative.') + if self.delay_sampler is None: + self.delay_sampler = constant_delay(self.max_delay_steps) + value = np.zeros(self.dim, dtype=float) if self.initial_value is None else self._as_vector(self.initial_value) + self.reset(value) + + def reset(self, value: Optional[ArrayLike] = None): + '''Reset channel memory and counters.''' + value = self._held_signal if value is None and self._held_signal is not None else value + value = np.zeros(self.dim, dtype=float) if value is None else self._as_vector(value) + self._queue.clear() + self._sampled_signal = value.copy() + self._held_signal = value.copy() + self._last_sample_step = 0 + self._last_delivery_step = 0 + self._last_arrival_step = 0 + self._sample_count = 0 + self._transmission_count = 0 + self._delivery_count = 0 + self._skipped_count = 0 + + def should_sample(self, step: int) -> bool: + '''Return whether this channel samples at the given control step.''' + return int(step) % self.sample_period_steps == 0 + + def sample(self, value: ArrayLike, step: int) -> ChannelState: + '''Record the latest source-side sample.''' + self._sampled_signal = self._as_vector(value) + self._last_sample_step = int(step) + self._sample_count += 1 + return self.state(step) + + def transmit(self, step: int) -> ChannelPacket: + '''Send the latest sampled value through the delayed channel.''' + step = int(step) + delay_steps = int(self.delay_sampler(self._sample_count, self._transmission_count)) + delay_steps = max(0, min(delay_steps, self.max_delay_steps)) + arrival_step = max(step + delay_steps, self._last_arrival_step) + self._last_arrival_step = arrival_step + packet = ChannelPacket( + value=self._sampled_signal.copy(), + sampled_step=self._last_sample_step, + transmit_step=step, + arrival_step=arrival_step, + ) + self._queue.append(packet) + self._transmission_count += 1 + return packet + + def skip(self): + '''Record that a sampled value was intentionally not transmitted.''' + self._skipped_count += 1 + + def deliver_due(self, step: int) -> int: + '''Deliver all packets whose arrival step has elapsed.''' + step = int(step) + delivered = 0 + pending = [] + for packet in self._queue: + if packet.arrival_step <= step: + self._held_signal = packet.value.copy() + self._last_delivery_step = step + self._delivery_count += 1 + delivered += 1 + else: + pending.append(packet) + self._queue = pending + return delivered + + def state(self, step: int) -> ChannelState: + '''Return the current channel state.''' + step = int(step) + return ChannelState( + sampled_signal=self._sampled_signal.copy(), + held_signal=self._held_signal.copy(), + update_error=self._held_signal - self._sampled_signal, + queue_depth=len(self._queue), + max_delay_steps=self.max_delay_steps, + sample_count=self._sample_count, + transmission_count=self._transmission_count, + delivery_count=self._delivery_count, + skipped_count=self._skipped_count, + stale_steps=max(0, step - self._last_delivery_step), + last_sample_step=self._last_sample_step, + last_delivery_step=self._last_delivery_step, + ) + + @property + def held_signal(self) -> ArrayLike: + '''Latest signal available at the destination side.''' + return self._held_signal.copy() + + def _as_vector(self, value: ArrayLike) -> ArrayLike: + array = np.asarray(value, dtype=float).reshape(-1) + if array.size != self.dim: + raise ValueError(f'Channel {self.name} expected dimension {self.dim}, got {array.size}.') + return array diff --git a/safe_control_gym/controllers/networked/metrics.py b/safe_control_gym/controllers/networked/metrics.py new file mode 100644 index 000000000..0e5ab7dac --- /dev/null +++ b/safe_control_gym/controllers/networked/metrics.py @@ -0,0 +1,48 @@ +'''Metrics for networked-control evaluation.''' + +from dataclasses import dataclass, field +from typing import Dict, List + +import numpy as np + + +@dataclass +class NetworkedControlMetrics: + '''Collect per-step communication metrics.''' + + events: List[Dict[str, float]] = field(default_factory=list) + + def reset(self): + '''Clear all recorded metrics.''' + self.events.clear() + + def record(self, event: Dict[str, float]): + '''Append one per-step metric record.''' + self.events.append(dict(event)) + + def summary(self) -> Dict[str, float]: + '''Return aggregate metrics used for before/after comparisons.''' + if not self.events: + return { + 'samples': 0, + 'transmissions': 0, + 'skips': 0, + 'event_rate': 0.0, + 'mean_stale_steps': 0.0, + 'max_queue_depth': 0.0, + 'mean_trigger_margin': 0.0, + 'mean_eta': 0.0, + } + transmissions = sum(event.get('transmitted', 0.0) for event in self.events) + skips = sum(event.get('skipped', 0.0) for event in self.events) + samples = transmissions + skips + return { + 'samples': int(samples), + 'transmissions': int(transmissions), + 'skips': int(skips), + 'event_rate': float(transmissions / samples) if samples else 0.0, + 'mean_stale_steps': float(np.mean([event.get('stale_steps', 0.0) for event in self.events])), + 'max_queue_depth': float(np.max([event.get('queue_depth', 0.0) for event in self.events])), + 'mean_trigger_margin': float(np.mean([event.get('trigger_margin', 0.0) for event in self.events])), + 'mean_eta': float(np.mean([event.get('eta', 0.0) for event in self.events])), + } diff --git a/safe_control_gym/controllers/networked/scheduler.py b/safe_control_gym/controllers/networked/scheduler.py new file mode 100644 index 000000000..95008b59f --- /dev/null +++ b/safe_control_gym/controllers/networked/scheduler.py @@ -0,0 +1,62 @@ +'''Sensor scheduling helpers for bandwidth-limited channels.''' + +from dataclasses import dataclass +from typing import Iterable, Sequence + +import numpy as np + + +@dataclass +class ScheduleDecision: + '''Scheduled coordinates for one transmission opportunity.''' + + indices: np.ndarray + value: np.ndarray + + +class FullStateScheduler: + '''Transmit every coordinate when the channel has enough capacity.''' + + def select(self, value, step: int, error=None) -> ScheduleDecision: + vector = np.asarray(value, dtype=float).reshape(-1) + return ScheduleDecision(indices=np.arange(vector.size), value=vector.copy()) + + +class RoundRobinScheduler: + '''Transmit fixed-size coordinate blocks in cyclic order.''' + + def __init__(self, block_size: int = 1): + if block_size <= 0: + raise ValueError('block_size must be positive.') + self.block_size = int(block_size) + self._cursor = 0 + + def reset(self): + '''Reset the scheduler cursor.''' + self._cursor = 0 + + def select(self, value, step: int, error=None) -> ScheduleDecision: + vector = np.asarray(value, dtype=float).reshape(-1) + start = self._cursor + indices = [(start + offset) % vector.size for offset in range(self.block_size)] + self._cursor = (self._cursor + self.block_size) % vector.size + indices = np.asarray(indices, dtype=int) + return ScheduleDecision(indices=indices, value=vector[indices].copy()) + + +class TryOnceDiscardScheduler: + '''Transmit coordinates with the largest current update error.''' + + def __init__(self, block_size: int = 1): + if block_size <= 0: + raise ValueError('block_size must be positive.') + self.block_size = int(block_size) + + def select(self, value, step: int, error=None) -> ScheduleDecision: + vector = np.asarray(value, dtype=float).reshape(-1) + if error is None: + scores = np.abs(vector) + else: + scores = np.abs(np.asarray(error, dtype=float).reshape(-1)) + indices = np.argsort(scores)[-self.block_size:][::-1] + return ScheduleDecision(indices=indices, value=vector[indices].copy()) diff --git a/safe_control_gym/controllers/networked/trigger.py b/safe_control_gym/controllers/networked/trigger.py new file mode 100644 index 000000000..1ba7f408c --- /dev/null +++ b/safe_control_gym/controllers/networked/trigger.py @@ -0,0 +1,112 @@ +'''Periodic event-triggered transmission policies.''' + +from dataclasses import dataclass +from typing import Dict + +import numpy as np + +from safe_control_gym.controllers.networked.channel import ChannelState + + +@dataclass +class TriggerDecision: + '''Result of evaluating an event trigger.''' + + transmit: bool + margin: float + eta: float + error_norm: float + signal_norm: float + + +class StaticPETCTrigger: + '''Static periodic event trigger evaluated only at sampling instants.''' + + def __init__(self, error_weight=1.0, signal_weight=0.05, min_error=0.0): + self.error_weight = float(error_weight) + self.signal_weight = float(signal_weight) + self.min_error = float(min_error) + + def reset(self): + '''Reset trigger state.''' + return + + def update_flow(self, channel_state: ChannelState, dt_steps: int = 1): + '''Advance trigger dynamics between samples.''' + return + + def evaluate(self, channel_state: ChannelState) -> TriggerDecision: + '''Return a transmit/skip decision for the current sampled value.''' + error_norm = float(np.linalg.norm(channel_state.update_error)) + signal_norm = float(np.linalg.norm(channel_state.sampled_signal)) + threshold = self.signal_weight * signal_norm + self.min_error + margin = self.error_weight * error_norm - threshold + return TriggerDecision( + transmit=margin >= 0.0, + margin=margin, + eta=0.0, + error_norm=error_norm, + signal_norm=signal_norm, + ) + + +class DynamicPETCTrigger(StaticPETCTrigger): + '''Dynamic PETC trigger with a scalar memory variable eta. + + eta grows when the channel is quiet and decays at sampled decisions. A + larger eta raises the transmission threshold, so historical low-error + behavior allows the wrapper to skip more samples. + ''' + + def __init__( + self, + error_weight=1.0, + signal_weight=0.05, + eta_decay=0.05, + eta_growth=0.01, + eta_transmit_drop=0.5, + eta_skip_gain=0.1, + eta_max=1.0, + min_error=0.0, + ): + super().__init__(error_weight=error_weight, signal_weight=signal_weight, min_error=min_error) + self.eta_decay = float(eta_decay) + self.eta_growth = float(eta_growth) + self.eta_transmit_drop = float(eta_transmit_drop) + self.eta_skip_gain = float(eta_skip_gain) + self.eta_max = float(eta_max) + self.eta = 0.0 + + def reset(self): + '''Reset the trigger memory variable.''' + self.eta = 0.0 + + def update_flow(self, channel_state: ChannelState, dt_steps: int = 1): + '''Advance eta between sampled decisions.''' + error_norm = float(np.linalg.norm(channel_state.update_error)) + signal_norm = float(np.linalg.norm(channel_state.sampled_signal)) + growth = self.eta_growth * (1.0 + signal_norm) / (1.0 + error_norm) + self.eta = max(0.0, min(self.eta_max, self.eta + dt_steps * (growth - self.eta_decay * self.eta))) + + def evaluate(self, channel_state: ChannelState) -> TriggerDecision: + '''Return a transmit/skip decision and update eta by a jump.''' + error_norm = float(np.linalg.norm(channel_state.update_error)) + signal_norm = float(np.linalg.norm(channel_state.sampled_signal)) + threshold = self.signal_weight * signal_norm + self.min_error + self.eta + margin = self.error_weight * error_norm - threshold + transmit = margin >= 0.0 or channel_state.queue_depth >= channel_state.max_delay_steps + 1 + if transmit: + self.eta = max(0.0, self.eta * self.eta_transmit_drop) + else: + self.eta = min(self.eta_max, self.eta + self.eta_skip_gain * max(0.0, -margin)) + return TriggerDecision( + transmit=transmit, + margin=margin, + eta=self.eta, + error_norm=error_norm, + signal_norm=signal_norm, + ) + + def state_dict(self) -> Dict[str, float]: + '''Return serializable trigger state.''' + return {'eta': self.eta} diff --git a/safe_control_gym/controllers/networked/wrapper.py b/safe_control_gym/controllers/networked/wrapper.py new file mode 100644 index 000000000..896869e28 --- /dev/null +++ b/safe_control_gym/controllers/networked/wrapper.py @@ -0,0 +1,167 @@ +'''Controller wrapper for sampled, event-triggered, delayed observations.''' + +from dataclasses import dataclass +from typing import Any, Dict, Optional + +import numpy as np + +from safe_control_gym.controllers.networked.channel import DelayChannel +from safe_control_gym.controllers.networked.metrics import NetworkedControlMetrics +from safe_control_gym.controllers.networked.trigger import DynamicPETCTrigger, TriggerDecision + + +@dataclass +class NetworkedStepInfo: + '''Extra diagnostics returned by the networked wrapper.''' + + observation_used: np.ndarray + transmitted: bool + delivered: int + stale_steps: int + queue_depth: int + trigger_margin: float + eta: float + + +class NetworkedControllerWrapper: + '''Wrap an existing safe-control-gym controller behind a delayed channel. + + The wrapper preserves the base controller API. It sends the held + destination-side observation to the wrapped controller, while the source-side + observation is sampled and transmitted only when the event trigger fires. + ''' + + def __init__( + self, + controller: Any, + observation_dim: int, + sample_period_steps: int = 1, + max_delay_steps: int = 0, + delay_sampler=None, + trigger=None, + name: str = 'observation', + ): + self.controller = controller + self.channel = DelayChannel( + name=name, + dim=observation_dim, + sample_period_steps=sample_period_steps, + max_delay_steps=max_delay_steps, + delay_sampler=delay_sampler, + ) + self.trigger = trigger if trigger is not None else DynamicPETCTrigger() + self.metrics = NetworkedControlMetrics() + self._step = 0 + + def reset(self): + '''Reset wrapper and wrapped controller.''' + self._step = 0 + self.channel.reset() + self.trigger.reset() + self.metrics.reset() + if hasattr(self.controller, 'reset'): + return self.controller.reset() + return None + + def reset_before_run(self, obs=None, info=None, env=None): + '''Reset the channel around a new evaluation run.''' + initial_obs = np.asarray(obs, dtype=float).reshape(-1) if obs is not None else None + self._step = 0 + self.channel.reset(initial_obs) + self.trigger.reset() + self.metrics.reset() + if hasattr(self.controller, 'reset_before_run'): + return self.controller.reset_before_run(obs=obs, info=info, env=env) + return None + + def select_action(self, obs, info: Optional[Dict[str, Any]] = None): + '''Select an action using the delayed held observation.''' + step = self._extract_step(info) + self.channel.deliver_due(step) + channel_state = self.channel.state(step) + self.trigger.update_flow(channel_state) + + transmitted = False + decision = None + if self.channel.should_sample(step): + channel_state = self.channel.sample(obs, step) + decision = self.trigger.evaluate(channel_state) + if decision.transmit: + self.channel.transmit(step) + transmitted = True + else: + self.channel.skip() + + delivered = self.channel.deliver_due(step) + channel_state = self.channel.state(step) + obs_used = channel_state.held_signal + action = self._select_action_from_wrapped(obs_used, info) + + if decision is None: + error_norm = float(np.linalg.norm(channel_state.update_error)) + signal_norm = float(np.linalg.norm(channel_state.sampled_signal)) + eta = float(getattr(self.trigger, 'eta', 0.0)) + margin = float(self.trigger.error_weight * error_norm - self.trigger.signal_weight * signal_norm - eta) + decision = TriggerDecision( + transmit=False, + margin=margin, + eta=eta, + error_norm=error_norm, + signal_norm=signal_norm, + ) + + self.metrics.record({ + 'step': step, + 'transmitted': float(transmitted), + 'skipped': float(not transmitted and self.channel.should_sample(step)), + 'delivered': float(delivered), + 'stale_steps': float(channel_state.stale_steps), + 'queue_depth': float(channel_state.queue_depth), + 'trigger_margin': float(decision.margin), + 'eta': float(decision.eta), + }) + self._step = step + 1 + return action + + def network_info(self) -> NetworkedStepInfo: + '''Return diagnostics for the latest wrapper state.''' + state = self.channel.state(max(0, self._step - 1)) + last = self.metrics.events[-1] if self.metrics.events else {} + return NetworkedStepInfo( + observation_used=state.held_signal, + transmitted=bool(last.get('transmitted', 0.0)), + delivered=int(last.get('delivered', 0.0)), + stale_steps=state.stale_steps, + queue_depth=state.queue_depth, + trigger_margin=float(last.get('trigger_margin', 0.0)), + eta=float(last.get('eta', 0.0)), + ) + + def close(self): + '''Close the wrapped controller if it owns resources.''' + if hasattr(self.controller, 'close'): + return self.controller.close() + return None + + def save(self, path): + '''Delegate checkpointing to the wrapped controller.''' + if hasattr(self.controller, 'save'): + return self.controller.save(path) + return None + + def load(self, path): + '''Delegate loading to the wrapped controller.''' + if hasattr(self.controller, 'load'): + return self.controller.load(path) + return None + + def _extract_step(self, info: Optional[Dict[str, Any]]) -> int: + if info is not None and 'current_step' in info: + return int(info['current_step']) + return self._step + + def _select_action_from_wrapped(self, obs, info): + try: + return self.controller.select_action(obs, info=info) + except TypeError: + return self.controller.select_action(obs) diff --git a/tests/controllers/networked/test_delay_channel.py b/tests/controllers/networked/test_delay_channel.py new file mode 100644 index 000000000..bb2071bc5 --- /dev/null +++ b/tests/controllers/networked/test_delay_channel.py @@ -0,0 +1,33 @@ +import numpy as np + +from safe_control_gym.controllers.networked import DelayChannel + + +def test_delay_channel_delivers_in_transmission_order_when_sampler_reorders(): + delays = iter([2, 0]) + + def sampler(_sample_count, _transmission_count): + return next(delays) + + channel = DelayChannel('obs', dim=1, max_delay_steps=2, delay_sampler=sampler) + channel.sample(np.array([1.0]), step=0) + packet_0 = channel.transmit(step=0) + channel.sample(np.array([2.0]), step=1) + packet_1 = channel.transmit(step=1) + + assert packet_0.arrival_step == 2 + assert packet_1.arrival_step == 2 + assert channel.deliver_due(step=1) == 0 + assert channel.deliver_due(step=2) == 2 + assert np.allclose(channel.held_signal, np.array([2.0])) + + +def test_delay_channel_tracks_update_error_and_staleness(): + channel = DelayChannel('obs', dim=2, max_delay_steps=1) + channel.reset(np.array([1.0, -1.0])) + channel.sample(np.array([2.0, -3.0]), step=4) + state = channel.state(step=4) + + assert np.allclose(state.update_error, np.array([-1.0, 2.0])) + assert state.queue_depth == 0 + assert state.stale_steps == 4 diff --git a/tests/controllers/networked/test_trigger.py b/tests/controllers/networked/test_trigger.py new file mode 100644 index 000000000..6bb572b73 --- /dev/null +++ b/tests/controllers/networked/test_trigger.py @@ -0,0 +1,36 @@ +import numpy as np + +from safe_control_gym.controllers.networked import ChannelState, DynamicPETCTrigger, StaticPETCTrigger + + +def make_state(update_error, sampled_signal=(1.0,), queue_depth=0, max_delay_steps=2): + return ChannelState( + sampled_signal=np.asarray(sampled_signal, dtype=float), + held_signal=np.asarray(sampled_signal, dtype=float) + np.asarray(update_error, dtype=float), + update_error=np.asarray(update_error, dtype=float), + queue_depth=queue_depth, + max_delay_steps=max_delay_steps, + ) + + +def test_static_trigger_transmits_when_error_crosses_threshold(): + trigger = StaticPETCTrigger(error_weight=1.0, signal_weight=0.1) + + assert not trigger.evaluate(make_state([0.01], sampled_signal=[1.0])).transmit + assert trigger.evaluate(make_state([0.5], sampled_signal=[1.0])).transmit + + +def test_dynamic_trigger_eta_grows_after_skips_and_backlog_forces_transmit(): + trigger = DynamicPETCTrigger( + error_weight=1.0, + signal_weight=0.1, + eta_growth=0.0, + eta_skip_gain=0.5, + eta_max=10.0, + ) + skipped = trigger.evaluate(make_state([0.01], sampled_signal=[1.0])) + forced = trigger.evaluate(make_state([0.01], sampled_signal=[1.0], queue_depth=3, max_delay_steps=2)) + + assert not skipped.transmit + assert skipped.eta > 0.0 + assert forced.transmit diff --git a/tests/controllers/networked/test_wrapper.py b/tests/controllers/networked/test_wrapper.py new file mode 100644 index 000000000..158327bba --- /dev/null +++ b/tests/controllers/networked/test_wrapper.py @@ -0,0 +1,41 @@ +import numpy as np + +from safe_control_gym.controllers.networked import ( + DynamicPETCTrigger, + NetworkedControllerWrapper, + constant_delay, +) + + +class ProportionalController: + def __init__(self): + self.observations = [] + + def select_action(self, obs, info=None): + self.observations.append(np.asarray(obs, dtype=float).copy()) + return -np.asarray(obs, dtype=float) + + def reset(self): + self.observations.clear() + + +def test_wrapper_uses_held_observation_until_delivery(): + controller = ProportionalController() + wrapper = NetworkedControllerWrapper( + controller, + observation_dim=1, + sample_period_steps=1, + max_delay_steps=1, + delay_sampler=constant_delay(1), + trigger=DynamicPETCTrigger(error_weight=10.0, signal_weight=0.0), + ) + wrapper.reset_before_run(obs=np.array([0.0])) + + action_0 = wrapper.select_action(np.array([2.0]), info={'current_step': 0}) + action_1 = wrapper.select_action(np.array([3.0]), info={'current_step': 1}) + summary = wrapper.metrics.summary() + + assert np.allclose(action_0, np.array([-0.0])) + assert np.allclose(action_1, np.array([-2.0])) + assert summary['transmissions'] >= 1 + assert summary['max_queue_depth'] >= 0.0 From 806c2006b7cf33999251621eaad32b006d237f62 Mon Sep 17 00:00:00 2001 From: JohnneyLee <110317240+JohnneyLee@users.noreply.github.com> Date: Thu, 25 Jun 2026 10:30:48 +0800 Subject: [PATCH 2/3] Remove PR-only integration notes --- INTEGRATION_MANIFEST.md | 68 ------------------------------------- README_NETWORKED_PETC_PR.md | 57 ------------------------------- 2 files changed, 125 deletions(-) delete mode 100644 INTEGRATION_MANIFEST.md delete mode 100644 README_NETWORKED_PETC_PR.md diff --git a/INTEGRATION_MANIFEST.md b/INTEGRATION_MANIFEST.md deleted file mode 100644 index b8c3f7110..000000000 --- a/INTEGRATION_MANIFEST.md +++ /dev/null @@ -1,68 +0,0 @@ -# Integration Manifest - -## Target Repository - -`learnsyslab/safe-control-gym` - -## Copy These Paths Into a Branch - -```text -safe_control_gym/controllers/networked/ -examples/networked_control/ -tests/controllers/networked/ -docs/networked_control.md -README_NETWORKED_PETC_PR.md -``` - -## Minimal PR Scope - -This contribution adds an optional wrapper. It does not alter existing -controllers, environments, tasks, or experiment code paths unless a user imports -`safe_control_gym.controllers.networked`. - -## Smoke Commands - -```bash -python examples/networked_control/run_dynamic_petc.py --max-delay-steps 3 -python -m pytest tests/controllers/networked -q -``` - -After installing the full safe-control-gym dependencies, run: - -```bash -python examples/networked_control/networked_lqr_experiment.py \ - --algo lqr \ - --task cartpole \ - --overrides ./examples/lqr/config_overrides/cartpole/cartpole_stab.yaml \ - ./examples/lqr/config_overrides/cartpole/lqr_cartpole_stab.yaml -``` - -## Before/After Protocol - -Use the same seed, task, controller, and number of steps. - -```text -Before: base controller with fresh observation every step. -After A: base controller wrapped with fixed periodic delayed transmission. -After B: base controller wrapped with DynamicPETCTrigger. -``` - -Report both task and network metrics: - -```text -task return or RMS tracking error -constraint/safety violation count when available -transmission count -event rate -mean stale steps -max queue depth -mean eta -mean trigger margin -``` - -## Reviewer-Facing Claim - -The module provides a reusable engineering abstraction for networked control -evaluation under sampled, delayed, event-triggered communication. It does not -claim closed-loop ISS for arbitrary safe-control-gym tasks unless the user -supplies compatible task-specific certificate bounds. diff --git a/README_NETWORKED_PETC_PR.md b/README_NETWORKED_PETC_PR.md deleted file mode 100644 index 43968a5a9..000000000 --- a/README_NETWORKED_PETC_PR.md +++ /dev/null @@ -1,57 +0,0 @@ -# PR: Networked Dynamic PETC Controller Wrapper - -## Purpose - -This overlay adds a practical networked-control wrapper to safe-control-gym. It -lets existing controllers run behind sampled, delayed, event-triggered -communication channels. - -## Files to Add - -```text -safe_control_gym/controllers/networked/ - __init__.py - channel.py - metrics.py - scheduler.py - trigger.py - wrapper.py -examples/networked_control/run_dynamic_petc.py -examples/networked_control/networked_lqr_experiment.py -tests/controllers/networked/ - test_delay_channel.py - test_trigger.py - test_wrapper.py -docs/networked_control.md -``` - -## Public API - -```python -NetworkedControllerWrapper(controller, observation_dim, sample_period_steps, - max_delay_steps, delay_sampler, trigger) -DynamicPETCTrigger(error_weight, signal_weight, eta_decay, eta_growth) -DelayChannel(name, dim, sample_period_steps, max_delay_steps, delay_sampler) -``` - -## Before/After Demonstration - -The minimal demonstration compares a direct proportional controller against the -same controller wrapped by dynamic PETC and bounded random delays: - -```bash -python examples/networked_control/run_dynamic_petc.py --max-delay-steps 3 -``` - -For a full safe-control-gym PR, the same wrapper can be applied to the existing -LQR/MPC examples by replacing `ctrl` with `NetworkedControllerWrapper(ctrl, ...)` -before constructing `BaseExperiment`. - -## Review Checklist - -- The base controller API is unchanged. -- Existing controllers do not import the networked package. -- Delays are measured in sample steps, not seconds. -- The delay channel preserves FIFO delivery order. -- The dynamic trigger is evaluated only at sampling instants. -- The example reports communication metrics separately from task metrics. From 12f277b6f2442978ea54c450d17f236d8e1a54f2 Mon Sep 17 00:00:00 2001 From: JohnneyLee <110317240+JohnneyLee@users.noreply.github.com> Date: Thu, 25 Jun 2026 10:55:30 +0800 Subject: [PATCH 3/3] Add mobile robot MPC PETC example --- docs/networked_control.md | 39 +++ .../run_mobile_robot_mpc_petc.py | 268 ++++++++++++++++++ 2 files changed, 307 insertions(+) create mode 100644 examples/networked_control/run_mobile_robot_mpc_petc.py diff --git a/docs/networked_control.md b/docs/networked_control.md index 7ff77c5da..7ca1814f0 100644 --- a/docs/networked_control.md +++ b/docs/networked_control.md @@ -75,6 +75,45 @@ python examples/networked_control/networked_lqr_experiment.py \ The integration point is intentionally small: create the normal controller, then wrap it with `NetworkedControllerWrapper` before constructing `BaseExperiment`. +## Mobile Robot MPC Demo + +The mobile robot MPC example demonstrates a concrete robotics use case: a +unicycle/differential-drive robot tracks a smooth path while localization +observations arrive through the dynamic PETC wrapper. + +```bash +python examples/networked_control/run_mobile_robot_mpc_petc.py \ + --steps 260 \ + --max-delay-steps 4 +``` + +The controller is a dependency-light finite-horizon MPC implemented by sampling +bounded control sequences and rolling out the kinematic model. It uses: + +```text +state: [x, y, theta] +control: [linear velocity, angular velocity] +speed bounds: [0.0, 1.0] m/s +angular velocity bounds: [-2.2, 2.2] rad/s +horizon: 12 steps +candidate sequences: 384 +``` + +One representative run produced: + +```text +Fresh MPC RMS tracking error: 0.0513 m +PETC MPC RMS tracking error: 0.0565 m +Transmissions: 75 / 260 samples +Event rate: 28.85% +Mean stale steps: 4.22 +Max queue depth: 4 +``` + +This illustrates the intended engineering tradeoff: the wrapped MPC used about +29% of the observation transmissions while keeping the RMS path-tracking error +within 5.3 mm of the fresh-observation baseline in this scenario. + ## Limits The wrapper exposes certificate-like quantities such as update error, queue diff --git a/examples/networked_control/run_mobile_robot_mpc_petc.py b/examples/networked_control/run_mobile_robot_mpc_petc.py new file mode 100644 index 000000000..fabb3f731 --- /dev/null +++ b/examples/networked_control/run_mobile_robot_mpc_petc.py @@ -0,0 +1,268 @@ +'''Mobile robot MPC path-tracking demo with a dynamic PETC wrapper.''' + +import argparse +import csv +import sys +from pathlib import Path + +import numpy as np + +PROJECT_ROOT = Path(__file__).resolve().parents[2] +if str(PROJECT_ROOT) not in sys.path: + sys.path.insert(0, str(PROJECT_ROOT)) + +from safe_control_gym.controllers.networked import DynamicPETCTrigger, NetworkedControllerWrapper, uniform_delay + + +class ShootingMPCController: + '''Derivative-free finite-horizon MPC for a unicycle robot. + + The implementation is intentionally dependency-light: it samples bounded + control sequences, rolls out the unicycle dynamics, and applies the first + action from the lowest-cost sequence. + ''' + + def __init__( + self, + path, + dt=0.05, + horizon=12, + candidates=384, + speed_bounds=(0.0, 1.0), + omega_bounds=(-2.2, 2.2), + seed=3, + ): + self.path = np.asarray(path, dtype=float) + self.dt = float(dt) + self.horizon = int(horizon) + self.candidates = int(candidates) + self.speed_bounds = tuple(speed_bounds) + self.omega_bounds = tuple(omega_bounds) + self.rng = np.random.default_rng(seed) + self._warm_start = np.column_stack([ + np.full(self.horizon, 0.65), + np.zeros(self.horizon), + ]) + + def reset(self): + self._warm_start[:, 0] = 0.65 + self._warm_start[:, 1] = 0.0 + + def select_action(self, obs, info=None): + state = np.asarray(obs, dtype=float).reshape(-1) + sequences = self._sample_sequences() + costs = np.array([self._rollout_cost(state, seq) for seq in sequences]) + best = sequences[int(np.argmin(costs))] + self._warm_start[:-1] = best[1:] + self._warm_start[-1] = best[-1] + return best[0].copy() + + def close(self): + return None + + def _sample_sequences(self): + sequences = np.empty((self.candidates, self.horizon, 2), dtype=float) + sequences[0] = self._warm_start + sequences[1] = np.column_stack([np.full(self.horizon, 0.65), np.zeros(self.horizon)]) + + v_min, v_max = self.speed_bounds + w_min, w_max = self.omega_bounds + base_v = self._warm_start[:, 0] + base_w = self._warm_start[:, 1] + for idx in range(2, self.candidates): + noise_scale = 0.16 if idx < self.candidates // 2 else 0.35 + v = np.clip(base_v + self.rng.normal(0.0, noise_scale, self.horizon), v_min, v_max) + w = np.clip(base_w + self.rng.normal(0.0, noise_scale * 3.0, self.horizon), w_min, w_max) + sequences[idx, :, 0] = v + sequences[idx, :, 1] = w + return sequences + + def _rollout_cost(self, state, sequence): + x = state.copy() + total = 0.0 + previous = sequence[0] + for k, action in enumerate(sequence): + x = step_unicycle(x, action, self.dt) + err, heading_err = path_error_and_heading(x, self.path) + v, omega = action + smooth = np.sum((action - previous) ** 2) + total += (1.0 + 0.04 * k) * (10.0 * err ** 2 + 0.55 * heading_err ** 2) + total += 0.02 * omega ** 2 + 0.03 * smooth + total += 0.08 * max(0.0, 0.35 - v) ** 2 + previous = action + goal_distance = np.linalg.norm(x[:2] - self.path[-1]) + total += 0.4 * goal_distance + return float(total) + + +def wrap_angle(angle): + return (angle + np.pi) % (2.0 * np.pi) - np.pi + + +def step_unicycle(state, action, dt): + x, y, theta = state + v, omega = action + return np.array([ + x + dt * v * np.cos(theta), + y + dt * v * np.sin(theta), + wrap_angle(theta + dt * omega), + ]) + + +def make_path(n=500): + xs = np.linspace(0.0, 10.0, n) + ys = 1.1 * np.sin(0.55 * xs) + return np.column_stack([xs, ys]) + + +def path_error_and_heading(state, path): + point = state[:2] + deltas = path - point + distances = np.linalg.norm(deltas, axis=1) + idx = int(np.argmin(distances)) + nearest_error = float(distances[idx]) + next_idx = min(idx + 5, len(path) - 1) + tangent = path[next_idx] - path[max(0, idx - 1)] + path_heading = np.arctan2(tangent[1], tangent[0]) + heading_error = abs(wrap_angle(path_heading - state[2])) + return nearest_error, float(heading_error) + + +def simulate(controller, path, n_steps=260, dt=0.05): + state = np.array([0.0, -0.25, 0.15]) + rows = [] + for step in range(n_steps): + action = np.asarray(controller.select_action(state, info={'current_step': step}), dtype=float) + action = np.array([np.clip(action[0], 0.0, 1.0), np.clip(action[1], -2.2, 2.2)]) + state = step_unicycle(state, action, dt) + err, heading_err = path_error_and_heading(state, path) + rows.append({ + 'step': step, + 'time': step * dt, + 'x': state[0], + 'y': state[1], + 'theta': state[2], + 'v': action[0], + 'omega': action[1], + 'tracking_error': err, + 'heading_error': heading_err, + }) + return rows + + +def summarize(rows): + errors = np.array([row['tracking_error'] for row in rows], dtype=float) + omega = np.array([row['omega'] for row in rows], dtype=float) + return { + 'rms_error_m': float(np.sqrt(np.mean(errors ** 2))), + 'max_error_m': float(np.max(errors)), + 'final_error_m': float(errors[-1]), + 'mean_abs_omega_rad_s': float(np.mean(np.abs(omega))), + } + + +def write_csv(path, rows): + with open(path, 'w', newline='', encoding='utf-8') as handle: + writer = csv.DictWriter(handle, fieldnames=list(rows[0].keys())) + writer.writeheader() + writer.writerows(rows) + + +def plot_result(out_path, path, direct_rows, networked_rows, network_events): + import matplotlib + matplotlib.use('Agg') + import matplotlib.pyplot as plt + + direct_xy = np.array([[row['x'], row['y']] for row in direct_rows]) + network_xy = np.array([[row['x'], row['y']] for row in networked_rows]) + direct_error = np.array([row['tracking_error'] for row in direct_rows]) + network_error = np.array([row['tracking_error'] for row in networked_rows]) + time = np.array([row['time'] for row in direct_rows]) + + fig, axs = plt.subplots(2, 2, figsize=(11, 7)) + axs[0, 0].plot(path[:, 0], path[:, 1], 'k--', label='reference') + axs[0, 0].plot(direct_xy[:, 0], direct_xy[:, 1], label='fresh MPC') + axs[0, 0].plot(network_xy[:, 0], network_xy[:, 1], label='PETC MPC') + axs[0, 0].axis('equal') + axs[0, 0].set_title('Mobile robot MPC path tracking') + axs[0, 0].set_xlabel('x [m]') + axs[0, 0].set_ylabel('y [m]') + axs[0, 0].legend() + + axs[0, 1].plot(time, direct_error, label='fresh MPC') + axs[0, 1].plot(time, network_error, label='PETC MPC') + axs[0, 1].set_title('Tracking error') + axs[0, 1].set_xlabel('time [s]') + axs[0, 1].set_ylabel('nearest-path error [m]') + axs[0, 1].legend() + + event_steps = np.array([event['step'] for event in network_events]) + transmitted = np.array([event['transmitted'] for event in network_events]) + eta = np.array([event['eta'] for event in network_events]) + stale = np.array([event['stale_steps'] for event in network_events]) + axs[1, 0].step(event_steps, transmitted, where='post') + axs[1, 0].set_ylim(-0.1, 1.1) + axs[1, 0].set_title('Transmission decisions') + axs[1, 0].set_xlabel('step') + axs[1, 0].set_ylabel('transmit') + + axs[1, 1].plot(event_steps, eta, label='eta') + axs[1, 1].plot(event_steps, stale, label='stale steps') + axs[1, 1].set_title('Trigger memory and observation age') + axs[1, 1].set_xlabel('step') + axs[1, 1].legend() + + fig.tight_layout() + fig.savefig(out_path, dpi=160) + plt.close(fig) + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('--steps', type=int, default=260) + parser.add_argument('--max-delay-steps', type=int, default=4) + parser.add_argument('--sample-period-steps', type=int, default=1) + parser.add_argument('--seed', type=int, default=11) + parser.add_argument('--output-dir', default=r'C:\My_project_usage\mobile_robot_mpc_petc_output') + args = parser.parse_args() + + output_dir = Path(args.output_dir) + output_dir.mkdir(parents=True, exist_ok=True) + path = make_path() + dt = 0.05 + + direct = ShootingMPCController(path, dt=dt, seed=args.seed) + direct_rows = simulate(direct, path, n_steps=args.steps, dt=dt) + + networked = NetworkedControllerWrapper( + ShootingMPCController(path, dt=dt, seed=args.seed), + observation_dim=3, + sample_period_steps=args.sample_period_steps, + max_delay_steps=args.max_delay_steps, + delay_sampler=uniform_delay(args.max_delay_steps, rng=np.random.default_rng(args.seed + 1)), + trigger=DynamicPETCTrigger( + error_weight=1.0, + signal_weight=0.018, + eta_decay=0.012, + eta_growth=0.023, + eta_skip_gain=0.075, + eta_transmit_drop=0.35, + eta_max=0.32, + ), + ) + networked.reset_before_run(obs=np.array([0.0, -0.25, 0.15])) + networked_rows = simulate(networked, path, n_steps=args.steps, dt=dt) + + write_csv(output_dir / 'fresh_mpc.csv', direct_rows) + write_csv(output_dir / 'petc_mpc.csv', networked_rows) + write_csv(output_dir / 'network_events.csv', networked.metrics.events) + plot_result(output_dir / 'mobile_robot_mpc_petc_result.png', path, direct_rows, networked_rows, networked.metrics.events) + + print('fresh_mpc:', summarize(direct_rows)) + print('petc_mpc:', summarize(networked_rows)) + print('network_metrics:', networked.metrics.summary()) + print('plot:', output_dir / 'mobile_robot_mpc_petc_result.png') + + +if __name__ == '__main__': + main()