112 lines
4.5 KiB
Python
112 lines
4.5 KiB
Python
"""Bbox-only visual servoing: turn a person ``Detection`` into a velocity.
|
|
|
|
The Go2 has no metric depth in this stack, so :class:`VisualServo` servos
|
|
purely on the detection bounding box:
|
|
|
|
* **Yaw** is a P(ID) law on the *normalised horizontal offset* of the box
|
|
centroid (centre the target).
|
|
* **Forward** is a proportional law on how much shorter the box is than the
|
|
``stop_height_ratio`` setpoint (a box that fills more of the frame is
|
|
closer), throttled down when the heading error is large so the dog turns to
|
|
face the person before charging.
|
|
|
|
Velocity convention (matches the rest of GoWelcome / Unitree
|
|
``SportClient.Move``): ``vx`` forward+, ``vyaw`` CCW/left+ (rad/s). This module
|
|
only produces ``vx`` and ``vyaw``; lateral ``vy`` stays zero in APPROACH.
|
|
|
|
Pure module: imports only :mod:`math`, :mod:`config`, and
|
|
:mod:`gowelcome.types` -- no cv2, no SDK -- so it imports and unit-tests
|
|
off-robot.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
from typing import Tuple
|
|
|
|
from config import ServoConfig
|
|
from gowelcome.types import Detection
|
|
|
|
from gowelcome.control.pid import PIDController
|
|
|
|
|
|
class VisualServo:
|
|
"""Pixel-offset visual servo producing ``(vx, vyaw, arrived)``.
|
|
|
|
Holds a single yaw :class:`~gowelcome.control.pid.PIDController` built from
|
|
the :class:`~config.ServoConfig`; the forward channel is a stateless
|
|
proportional law. Call :meth:`reset` when re-acquiring a target so stale
|
|
integral/derivative state does not leak across approaches.
|
|
"""
|
|
|
|
def __init__(self, cfg: ServoConfig) -> None:
|
|
"""Build the yaw PID from ``cfg``.
|
|
|
|
Args:
|
|
cfg: Servo tuning block (gains, deadband, sign, limits, stop size).
|
|
"""
|
|
self.cfg = cfg
|
|
# The PID also carries the deadband so that, on the tick a target
|
|
# crosses *into* the band, its derivative term is suppressed (the input
|
|
# is pre-zeroed below, so the PID sees error 0 -> inside its own
|
|
# deadband -> no derivative kick). The pre-zeroing of the input is what
|
|
# additionally suppresses the proportional term inside the band.
|
|
self.yaw_pid = PIDController(
|
|
kp=cfg.kp_yaw,
|
|
ki=cfg.ki_yaw,
|
|
kd=cfg.kd_yaw,
|
|
output_limits=(-cfg.max_yaw_rate, cfg.max_yaw_rate),
|
|
deadband=cfg.yaw_deadband,
|
|
)
|
|
|
|
def reset(self) -> None:
|
|
"""Reset the internal yaw PID state (integral + previous error)."""
|
|
self.yaw_pid.reset()
|
|
|
|
def compute(
|
|
self, target: Detection, frame_w: int, frame_h: int, dt: float
|
|
) -> Tuple[float, float, bool]:
|
|
"""Compute one servoing step toward ``target``.
|
|
|
|
Pixel-offset P(ID) controller, bbox-only (no depth).
|
|
|
|
Args:
|
|
target: The person bounding box to servo toward.
|
|
frame_w: Frame width in pixels (for the horizontal offset).
|
|
frame_h: Frame height in pixels (for the height-ratio distance
|
|
proxy).
|
|
dt: Timestep in seconds (fed to the yaw PID derivative/integral).
|
|
|
|
Returns:
|
|
``(vx, vyaw, arrived)`` where ``vx`` is forward velocity (m/s,
|
|
never negative in APPROACH), ``vyaw`` is yaw rate (rad/s, CCW+),
|
|
and ``arrived`` is ``True`` once the box fills at least
|
|
``cfg.stop_height_ratio`` of the frame height.
|
|
"""
|
|
cfg = self.cfg
|
|
|
|
# --- Yaw: centre the target horizontally. -------------------------
|
|
norm_err = target.horizontal_offset(frame_w) # -1..1, + = right of centre
|
|
norm_err_eff = 0.0 if abs(norm_err) < cfg.yaw_deadband else norm_err
|
|
vyaw = cfg.yaw_sign * self.yaw_pid.update(norm_err_eff, dt)
|
|
|
|
# --- Forward: approach until the box fills the frame vertically. ---
|
|
height_ratio = target.height_ratio(frame_h)
|
|
arrived = height_ratio >= cfg.stop_height_ratio
|
|
|
|
if arrived:
|
|
vx = 0.0
|
|
else:
|
|
# Box smaller than the stop size -> positive forward command.
|
|
forward_raw = cfg.kp_forward * (cfg.stop_height_ratio - height_ratio)
|
|
# Throttle forward when heading error is large (don't charge sideways).
|
|
scaling = math.exp(-cfg.forward_heading_falloff * abs(norm_err)) # 0..1
|
|
vx = forward_raw * scaling
|
|
# Never reverse in APPROACH; clamp to the configured max.
|
|
vx = max(0.0, min(cfg.max_forward, vx))
|
|
# Below min_forward, treat as stopped-forward (turning still allowed).
|
|
if vx < cfg.min_forward:
|
|
vx = 0.0
|
|
|
|
return (vx, vyaw, arrived)
|