GNC/guidance/ee_guidance.py — EE camera guidance

EEGuidance turns a scheduled surface aim point $\bm x_\text{surf}(s)$ into the desired EE pose $(\bm p_{ed},\bm z_{ed})$ and twist $\bm\nu_{ed}$ fed to the controller each step.

Standoff construction (§5.4)

Standoff vector from CoM to surface aim point:

$$\bm r = \bm x_\text{surf} - \bm p_c, \quad \rho = \lVert\bm r\rVert, \quad \hat{\bm r} = \bm r/\rho$$

Desired EE position and look axis (eq. 5.7):

$$\bm p_{ed} = \bm p_c + r_\text{cam}\hat{\bm r}, \qquad \bm z_{ed} = \hat{\bm r}$$

Key simplification: the whole desired pose is governed by the single direction $\hat{\bm r}$; the look axis equals the aim direction.

POSE vs ANCHOR

Two OrbitPathMode values route the pose builder.

POSE (adopted default):

$$\bm p_{ed},\bm z_{ed} \leftarrow \text{path}(s_\text{aim}) \quad \text{(eq. 5.7, deterministic)}$$

Scorer never runs. Complete, scheduled coverage guaranteed.

ANCHOR (retained for risk-aware planning):

$$\bm p_{ed},\bm z_{ed} \leftarrow \text{EETargetFinder.choose\_goal}(\bm p_\text{anchor})$$

Vantage $\bm p_\text{anchor}$ is a CoM-orbit point, not $\bm x_\text{surf}$ (placing cameras at the surface would put them inside the target). Scoring: $\Omega = \prod_i \sigma_i^{w_i}$ (eq. 7.2); inert in POSE.

State machine: INITIAL → HOLD

The GuidanceMode IntEnum governs four states; the machine runs once per step in set_ee_target.

INITIAL (startup latch, steps $0\ldots n_\text{su}$):

$$\bm p_e = \bm p_c + \bm R_b \bm p_{ce}^{(0)}, \quad \bm R_e = \bm R_b \bm R_{be}^{(0)}$$

$\bm p_{ce}^{(0)},\bm R_{be}^{(0)}$ latched on the first call; cleared only by reset_runtime_state. If aim_during_initial, steps $1\ldots n_\text{su}$ already run orbit_path_pose under the INITIAL tag.

After initial_phase_complete ($\text{step} \geq n_\text{su}$), POSE mode short-circuits to orbit_path_pose every step; HOLD/TARGETING/FALLBACK are inert in the adopted default.

TARGETING is a one-step reselect event (ANCHOR only): accept_target margin gate → latch new $\bm p_{ce}$ → immediately re-enters HOLD. The EE tracks in HOLD, not TARGETING.

The aim clock

The arclength argument $s_\text{aim}$ passed to orbit_path.eval:

$$s_\text{aim} = \begin{cases} s \cdot \Delta\lambda & \text{progress-synced (closed-loop, default)} \\ k \Delta t v_\text{des} & \text{open-loop (step } k\text{)} \end{cases}$$

$s$ = achieved progress index along the discrete helix $\Lambda$; $\Delta\lambda$ = arc sample spacing; $v_\text{des}$ = constant cruise speed.

Closed-loop: aim driven by where the vehicle actually is, not wall time. Optional knobs (default OFF, bit-identical): floor rate $\dot s_\text{floor}$ breaks the dwell fixed point; lead cap $s_\text{lead}$ bounds open-loop excursion.

Analytic desired twist (§5.5 → §5.6)

Unit-vector rate (tangential projection):

$$\dot{\hat{\bm r}} = \frac{1}{\rho}\bigl(\mathbb{I} - \hat{\bm r}\hat{\bm r}^\top\bigr)\dot{\bm r}, \qquad \dot{\bm r} = \dot{\bm x}_\text{surf} - \bm v_{cd}$$

World-frame desired twist (eq. 5.14):

$$\bm\nu_{ed} = \begin{bmatrix}\bm v_{cd} + r_\text{cam}\dot{\hat{\bm r}} \\ \hat{\bm r}\times\dot{\hat{\bm r}}\end{bmatrix}$$

Angular velocity is roll-free by construction: $\hat{\bm r}\cdot\bm\omega_{ed}=0$.

Converted to circumcentroidal $(\oplus)$ frame (eq. 5.17):

$$\bm\nu_{ed}^\oplus = \bm\nu_{ed}^\text{local} - \bm G_{v_c}\bm v_{cd} + \bm G_{\omega_b}\bm\omega_{bd}$$

Analytic feedforward valid in POSE only; ANCHOR falls back to finite-difference.

Finalization and handoff to controller

Every raw pose passes through consistent_finalize_pose before reaching Desired:

$$\underbrace{\text{raw } \bm p_{ed},\bm z_{ed}}_{\text{path}} \xrightarrow{\text{filter}} \underbrace{(1-\alpha)\bm p_{e,\text{prev}} + \alpha \bm p_{e,\text{raw}}}_{\text{low-pass + rate clamp}} \xrightarrow{\lVert\bm p_{ce}\rVert \leq r_\text{reach}} \text{Desired}.(\bm p_e,\bm R_e,\bm z_e,\bm\nu_e)$$

The controller reads des.nu_e directly. In POSE mode, analytic_desired_nu_e_feedforward writes the closed-form $\bm\nu_{ed}^\text{local}$ (and $\bm\nu_{ed}^\oplus,\dot{\bm\nu}_{ed}^\oplus$ for the acceleration FF into the RHS, eq. 5.18). The EE twist may additionally be scaled by the conditioning ramp $\gamma(\sigma_G)$ via derate_twist_by_gamma.