Johnny Fernandes
7ab69ab0f3
Naming pass: rename functions whose third+ segment is redundant or implementation-detail, sticking to the codebase's preferred ``noun_verb`` / ``verb_noun`` two-concept idiom. Renames are atomic across definitions, callers, and tests. is_penned_position → is_penned modulate_speed_near_sheep → modulate_speed mecanum_kinematics_step → mecanum_step policy_forward_mean → forward_mean Two-concept patterns like ``velocity_to_wheels`` / ``detections_from_scan`` / ``make_strombom_predictor`` are left alone — they're idiomatic converters / factories that read as a single concept, and the longer form aids grep-ability. Docstring polish: * ``herding/config.py`` header drops the "previously lived as a module-level literal" historical framing — we ship as a single thing, so the refactor anecdote no longer earns its keep. The usage examples now mention both ``HERDING_WEBOTS`` and ``HERDING_MEC_WEBOTS`` presets. 126 pytest cases still pass. Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
43 lines
1.3 KiB
Python
43 lines
1.3 KiB
Python
"""Shared action post-processing.
|
|
|
|
Every dog mode routes its action through ``modulate_speed``
|
|
so the magnitude is reduced near sheep — direction (intent) is
|
|
preserved.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
|
|
|
|
SLOW_NEAR_SHEEP = 2.5 # m — distance below which action norm is scaled down
|
|
MIN_SPEED = 0.30 # action norm at zero distance
|
|
|
|
|
|
def modulate_speed(
|
|
vx: float, vy: float,
|
|
dog_xy: tuple[float, float],
|
|
sheep_positions,
|
|
slow_dist: float = SLOW_NEAR_SHEEP,
|
|
min_scale: float = MIN_SPEED,
|
|
) -> tuple[float, float]:
|
|
"""Linearly ramp action magnitude from ``min_scale`` at distance 0
|
|
to 1.0 at ``slow_dist``. ``sheep_positions`` may be a
|
|
``{name: (x, y)}`` dict or an iterable of ``(x, y)`` tuples.
|
|
"""
|
|
if not sheep_positions:
|
|
return vx, vy
|
|
if hasattr(sheep_positions, "values"):
|
|
positions = sheep_positions.values()
|
|
else:
|
|
positions = sheep_positions
|
|
nearest = float("inf")
|
|
for sx, sy in positions:
|
|
d = math.hypot(sx - dog_xy[0], sy - dog_xy[1])
|
|
if d < nearest:
|
|
nearest = d
|
|
if nearest >= slow_dist or nearest == float("inf"):
|
|
return vx, vy
|
|
scale = min_scale + (1.0 - min_scale) * (nearest / slow_dist)
|
|
return vx * scale, vy * scale
|