Lab039: model two-stage failsafe braking
This commit is contained in:
LittleSam129
229
protocol/two_stage_failsafe.py
Normal file
229
protocol/two_stage_failsafe.py
Normal file
@@ -0,0 +1,229 @@
|
||||
"""Event-driven two-stage longitudinal failsafe model for Lab039.
|
||||
|
||||
The module is intentionally independent from motor hardware. It models an
|
||||
explicit safety state machine and exact one-dimensional constant-acceleration
|
||||
segments; measured rover braking parameters are still required before any
|
||||
real-world use.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from enum import Enum
|
||||
|
||||
|
||||
class SafetyState(str, Enum):
|
||||
NORMAL = "normal"
|
||||
STAGE1_DECELERATION = "stage1_deceleration"
|
||||
STAGE2_BRAKING = "stage2_braking"
|
||||
EMERGENCY_LATCHED = "emergency_latched"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ThresholdPolicy:
|
||||
name: str
|
||||
stage1_seconds: float | None
|
||||
stage2_seconds: float
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
if self.stage2_seconds <= 0.0:
|
||||
raise ValueError("stage2_seconds must be positive")
|
||||
if self.stage1_seconds is not None:
|
||||
if self.stage1_seconds <= 0.0:
|
||||
raise ValueError("stage1_seconds must be positive")
|
||||
if self.stage1_seconds >= self.stage2_seconds:
|
||||
raise ValueError("stage1 threshold must precede stage2")
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DecelerationProfile:
|
||||
name: str
|
||||
stage1_mps2: float
|
||||
stage2_mps2: float
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
if self.stage1_mps2 <= 0.0 or self.stage2_mps2 <= 0.0:
|
||||
raise ValueError("decelerations must be positive")
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class KinematicStep:
|
||||
final_speed_mps: float
|
||||
distance_m: float
|
||||
zero_after_seconds: float | None
|
||||
target_after_seconds: float | None
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ForcedStopResult:
|
||||
speed_at_first_threshold_mps: float
|
||||
speed_at_second_threshold_mps: float
|
||||
distance_to_first_threshold_m: float
|
||||
distance_between_thresholds_m: float
|
||||
braking_distance_m: float
|
||||
total_distance_m: float
|
||||
stop_time_seconds: float
|
||||
|
||||
|
||||
def integrate_kinematics(
|
||||
speed_mps: float,
|
||||
acceleration_mps2: float,
|
||||
duration_seconds: float,
|
||||
*,
|
||||
target_speed_mps: float | None = None,
|
||||
) -> KinematicStep:
|
||||
"""Integrate one constant-acceleration segment exactly.
|
||||
|
||||
Braking is truncated at zero speed. Positive acceleration can optionally
|
||||
be truncated at ``target_speed_mps``. Distance after either bound is
|
||||
reached uses the corresponding constant bounded speed.
|
||||
"""
|
||||
|
||||
if speed_mps < 0.0:
|
||||
raise ValueError("speed must not be negative")
|
||||
if duration_seconds < 0.0:
|
||||
raise ValueError("duration must not be negative")
|
||||
if target_speed_mps is not None and target_speed_mps < 0.0:
|
||||
raise ValueError("target speed must not be negative")
|
||||
if duration_seconds == 0.0:
|
||||
return KinematicStep(speed_mps, 0.0, None, None)
|
||||
if acceleration_mps2 < 0.0:
|
||||
if speed_mps == 0.0:
|
||||
return KinematicStep(0.0, 0.0, 0.0, None)
|
||||
stop_after = speed_mps / -acceleration_mps2
|
||||
moving_time = min(duration_seconds, stop_after)
|
||||
distance = speed_mps * moving_time + 0.5 * acceleration_mps2 * moving_time * moving_time
|
||||
if stop_after <= duration_seconds:
|
||||
return KinematicStep(0.0, max(0.0, distance), stop_after, None)
|
||||
final_speed = speed_mps + acceleration_mps2 * duration_seconds
|
||||
return KinematicStep(max(0.0, final_speed), max(0.0, distance), None, None)
|
||||
if acceleration_mps2 > 0.0 and target_speed_mps is not None:
|
||||
if speed_mps > target_speed_mps + 1e-12:
|
||||
raise ValueError("speed already exceeds target")
|
||||
if speed_mps >= target_speed_mps - 1e-12:
|
||||
return KinematicStep(target_speed_mps, target_speed_mps * duration_seconds, None, 0.0)
|
||||
target_after = (target_speed_mps - speed_mps) / acceleration_mps2
|
||||
accelerating_time = min(duration_seconds, target_after)
|
||||
distance = speed_mps * accelerating_time + 0.5 * acceleration_mps2 * accelerating_time * accelerating_time
|
||||
if target_after <= duration_seconds:
|
||||
distance += target_speed_mps * (duration_seconds - target_after)
|
||||
return KinematicStep(target_speed_mps, distance, None, target_after)
|
||||
return KinematicStep(
|
||||
speed_mps + acceleration_mps2 * duration_seconds,
|
||||
distance,
|
||||
None,
|
||||
None,
|
||||
)
|
||||
final_speed = speed_mps + acceleration_mps2 * duration_seconds
|
||||
if final_speed < -1e-12:
|
||||
raise ValueError("unbounded integration produced negative speed")
|
||||
distance = speed_mps * duration_seconds + 0.5 * acceleration_mps2 * duration_seconds * duration_seconds
|
||||
return KinematicStep(max(0.0, final_speed), max(0.0, distance), None, None)
|
||||
|
||||
|
||||
class TwoStageFailsafe:
|
||||
"""Sequence-aware watchdog FSM with a latched emergency state.
|
||||
|
||||
At an equal timestamp the caller must pass a completely received fresh
|
||||
command to :meth:`receive_command` before calling :meth:`apply_watchdog`.
|
||||
The fresh command then updates the watchdog origin and prevents a stage-1
|
||||
or stage-2 transition based on the previous command. Lab039's event
|
||||
scheduler implements and tests this receive-before-threshold rule.
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
policy: ThresholdPolicy,
|
||||
profile: DecelerationProfile,
|
||||
*,
|
||||
resume_acceleration_mps2: float = 1.0,
|
||||
boot_time_seconds: float = 0.0,
|
||||
) -> None:
|
||||
if resume_acceleration_mps2 <= 0.0:
|
||||
raise ValueError("resume acceleration must be positive")
|
||||
self.policy = policy
|
||||
self.profile = profile
|
||||
self.resume_acceleration_mps2 = resume_acceleration_mps2
|
||||
self.state = SafetyState.NORMAL
|
||||
self.last_sequence = -1
|
||||
self.last_fresh_time_seconds = boot_time_seconds
|
||||
self.emergency_actions = 0
|
||||
self.emergency_duplicates = 0
|
||||
self._emergency_keys: set[tuple[int, int]] = set()
|
||||
|
||||
def receive_command(self, sequence_number: int, receive_time_seconds: float) -> bool:
|
||||
if sequence_number <= self.last_sequence:
|
||||
return False
|
||||
if receive_time_seconds < self.last_fresh_time_seconds:
|
||||
raise ValueError("receive time moved backwards")
|
||||
self.last_sequence = sequence_number
|
||||
self.last_fresh_time_seconds = receive_time_seconds
|
||||
if self.state is not SafetyState.EMERGENCY_LATCHED:
|
||||
self.state = SafetyState.NORMAL
|
||||
return True
|
||||
|
||||
def apply_watchdog(self, now_seconds: float) -> SafetyState:
|
||||
if now_seconds < self.last_fresh_time_seconds:
|
||||
raise ValueError("watchdog time moved backwards")
|
||||
if self.state is SafetyState.EMERGENCY_LATCHED:
|
||||
return self.state
|
||||
age = now_seconds - self.last_fresh_time_seconds
|
||||
if age >= self.policy.stage2_seconds - 1e-12:
|
||||
self.state = SafetyState.STAGE2_BRAKING
|
||||
elif self.policy.stage1_seconds is not None and age >= self.policy.stage1_seconds - 1e-12:
|
||||
self.state = SafetyState.STAGE1_DECELERATION
|
||||
return self.state
|
||||
|
||||
def receive_emergency(self, stream_id: int, sequence_number: int) -> bool:
|
||||
key = stream_id, sequence_number
|
||||
if key in self._emergency_keys:
|
||||
self.emergency_duplicates += 1
|
||||
return False
|
||||
self._emergency_keys.add(key)
|
||||
self.emergency_actions += 1
|
||||
self.state = SafetyState.EMERGENCY_LATCHED
|
||||
return True
|
||||
|
||||
def acceleration_mps2(self, speed_mps: float, target_speed_mps: float) -> float:
|
||||
if speed_mps < 0.0 or target_speed_mps < 0.0:
|
||||
raise ValueError("speeds must not be negative")
|
||||
if self.state is SafetyState.NORMAL:
|
||||
return self.resume_acceleration_mps2 if speed_mps < target_speed_mps - 1e-12 else 0.0
|
||||
if self.state is SafetyState.STAGE1_DECELERATION:
|
||||
return -self.profile.stage1_mps2 if speed_mps > 0.0 else 0.0
|
||||
return -self.profile.stage2_mps2 if speed_mps > 0.0 else 0.0
|
||||
|
||||
|
||||
def forced_stop(
|
||||
initial_speed_mps: float,
|
||||
policy: ThresholdPolicy,
|
||||
profile: DecelerationProfile,
|
||||
) -> ForcedStopResult:
|
||||
"""Exact no-recovery stop after the last fresh command at time zero."""
|
||||
|
||||
if initial_speed_mps < 0.0:
|
||||
raise ValueError("initial speed must not be negative")
|
||||
first_threshold = policy.stage1_seconds if policy.stage1_seconds is not None else policy.stage2_seconds
|
||||
distance_to_first = initial_speed_mps * first_threshold
|
||||
speed_at_first = initial_speed_mps
|
||||
if policy.stage1_seconds is None:
|
||||
speed_at_second = initial_speed_mps
|
||||
distance_between = 0.0
|
||||
else:
|
||||
stage1_duration = policy.stage2_seconds - policy.stage1_seconds
|
||||
stage1 = integrate_kinematics(initial_speed_mps, -profile.stage1_mps2, stage1_duration)
|
||||
speed_at_second = stage1.final_speed_mps
|
||||
distance_between = stage1.distance_m
|
||||
braking = integrate_kinematics(speed_at_second, -profile.stage2_mps2, speed_at_second / profile.stage2_mps2 if speed_at_second else 0.0)
|
||||
stop_time = policy.stage2_seconds + speed_at_second / profile.stage2_mps2
|
||||
if speed_at_second == 0.0 and policy.stage1_seconds is not None:
|
||||
stop_time = policy.stage1_seconds + initial_speed_mps / profile.stage1_mps2
|
||||
return ForcedStopResult(
|
||||
speed_at_first,
|
||||
speed_at_second,
|
||||
distance_to_first,
|
||||
distance_between,
|
||||
braking.distance_m,
|
||||
distance_to_first + distance_between + braking.distance_m,
|
||||
stop_time,
|
||||
)
|
||||
Reference in New Issue
Block a user