"""
Attitude Laws
=============
Spacecraft/sensor pointing laws with full quaternion internal storage.
Hierarchy
---------
:class:`AbstractAttitudeLaw` (ABC)
├── :class:`FixedAttitudeLaw` — constant orientation in a reference frame
├── :class:`TrackAttitudeLaw` — boresight tracks a target spacecraft or ground station
├── :class:`CustomAttitudeLaw` — user-supplied quaternion callback
├── :class:`LimbAttitudeLaw` — body vector grazes an offset ellipsoid
└── :class:`ConditionAttitudeLaw` — routes between attitude laws by condition
"""
from __future__ import annotations
from abc import ABC, abstractmethod
import numpy as np
import numpy.typing as npt
from ..orbit.constants import EARTH_SEMI_MAJOR_AXIS, EARTH_INVERSE_FLATTENING
from ..orbit.frames import (
_lvlh_basis,
eci_ecef_rotation,
eci_to_ecef,
eci_to_lvlh,
ecef_to_eci,
lvlh_to_eci,
sun_vec_eci,
)
from ..orbit.propagation import propagate_analytical
from ..orbit.utils import host_eci_state
_VALID_FRAMES = frozenset({"lvlh", "eci", "ecef"})
# ---------------------------------------------------------------------------
# Private quaternion helpers
# ---------------------------------------------------------------------------
def _q_compose(q1: npt.NDArray, q2: npt.NDArray) -> npt.NDArray:
"""Quaternion product q1 ⊗ q2, both [w, x, y, z]."""
w1, x1, y1, z1 = q1
w2, x2, y2, z2 = q2
return np.array(
[
w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2,
w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2,
w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2,
w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2,
]
)
def _q_from_vec(vec: npt.NDArray, roll: float = 0.0) -> npt.NDArray:
"""Minimum-rotation quaternion aligning body-z to *vec*, then optional roll.
Parameters
----------
vec : npt.NDArray, shape (3,)
Target boresight direction (need not be a unit vector).
roll : float
Roll angle (rad) about the boresight axis. Default 0 gives the
minimum rotation from identity that aligns body-z with *vec*.
Returns
-------
npt.NDArray, shape (4,)
Unit quaternion [w, x, y, z].
"""
v = vec / np.linalg.norm(vec)
dot = v[2] # np.dot([0,0,1], v) = v[2]
if dot < -0.9999:
# 180° edge case: rotate about x-axis
q_min = np.array([0.0, 1.0, 0.0, 0.0])
else:
# Half-angle construction: axis = [0,0,1] × v, w = 1 + cos θ
axis = np.cross([0.0, 0.0, 1.0], v) # = [-v[1], v[0], 0]
half = np.append(1.0 + dot, axis)
q_min = half / np.linalg.norm(half)
if roll == 0.0:
return q_min
# Roll = rotation about body-z (boresight), composed BEFORE q_min
q_roll = np.array([np.cos(roll / 2), 0.0, 0.0, np.sin(roll / 2)])
return _q_compose(q_min, q_roll)
def _q_rotate(q: npt.NDArray, v: npt.NDArray) -> npt.NDArray:
"""Rotate 3D vector v by unit quaternion q (R(q) @ v)."""
w, x, y, z = q
return np.array(
[
(1 - 2 * (y * y + z * z)) * v[0]
+ 2 * (x * y - w * z) * v[1]
+ 2 * (x * z + w * y) * v[2],
2 * (x * y + w * z) * v[0]
+ (1 - 2 * (x * x + z * z)) * v[1]
+ 2 * (y * z - w * x) * v[2],
2 * (x * z - w * y) * v[0]
+ 2 * (y * z + w * x) * v[1]
+ (1 - 2 * (x * x + y * y)) * v[2],
]
)
def _q_from_vec_batch(
vecs: npt.NDArray, roll: float | npt.NDArray = 0.0
) -> npt.NDArray:
"""Vectorized :func:`_q_from_vec` over N directions.
Parameters
----------
vecs : npt.NDArray, shape (N, 3)
Target boresight directions (must be unit vectors).
roll : float or ndarray of shape (N,)
Roll angle(s) (rad) about each boresight axis. Scalar applies
the same roll to every quaternion; an array gives per-element rolls.
Returns
-------
npt.NDArray, shape (N, 4)
Unit quaternions [w, x, y, z].
"""
dot = vecs[:, 2] # (N,) cos θ = v · ẑ
# axis = ẑ × v = [-v[1], v[0], 0]; half = [1 + cos θ, axis]
N = len(vecs)
half = np.empty((N, 4), dtype=np.float64)
half[:, 0] = 1.0 + dot
half[:, 1] = -vecs[:, 1]
half[:, 2] = vecs[:, 0]
half[:, 3] = 0.0
# 180° edge case: dot ≈ −1 → rotate about body-x
mask = dot < -0.9999
half[mask] = [0.0, 1.0, 0.0, 0.0]
q_min = half / np.linalg.norm(half, axis=1, keepdims=True)
roll_arr = np.asarray(roll)
if roll_arr.ndim == 0 and float(roll_arr) == 0.0:
return q_min
# Compose q_min[n] ⊗ q_roll where q_roll = [cr, 0, 0, sr]
# Simplified Hamilton product with x2=y2=0:
cr, sr = np.cos(roll_arr / 2), np.sin(roll_arr / 2)
w, x, y, z = q_min[:, 0], q_min[:, 1], q_min[:, 2], q_min[:, 3]
return np.stack(
[
w * cr - z * sr,
x * cr + y * sr,
y * cr - x * sr,
w * sr + z * cr,
],
axis=1,
)
def _q_rotate_batch(qs: npt.NDArray, v: npt.NDArray) -> npt.NDArray:
"""Rotate a fixed 3-D vector by N quaternions.
Parameters
----------
qs : npt.NDArray, shape (N, 4)
Unit quaternions [w, x, y, z].
v : npt.NDArray, shape (3,)
Vector to rotate.
Returns
-------
npt.NDArray, shape (N, 3)
Rotated vectors (not normalised).
"""
w, x, y, z = qs[:, 0], qs[:, 1], qs[:, 2], qs[:, 3]
vx, vy, vz = v
return np.stack(
[
(1 - 2 * (y * y + z * z)) * vx
+ 2 * (x * y - w * z) * vy
+ 2 * (x * z + w * y) * vz,
2 * (x * y + w * z) * vx
+ (1 - 2 * (x * x + z * z)) * vy
+ 2 * (y * z - w * x) * vz,
2 * (x * z - w * y) * vx
+ 2 * (y * z + w * x) * vy
+ (1 - 2 * (x * x + y * y)) * vz,
],
axis=1,
)
def _q_compose_batch(q1: npt.NDArray, q2: npt.NDArray) -> npt.NDArray:
"""Element-wise Hamilton product of two (N, 4) quaternion arrays."""
w1, x1, y1, z1 = q1[:, 0], q1[:, 1], q1[:, 2], q1[:, 3]
w2, x2, y2, z2 = q2[:, 0], q2[:, 1], q2[:, 2], q2[:, 3]
return np.stack(
[
w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2,
w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2,
w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2,
w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2,
],
axis=1,
)
def _q_align_batch(
v_body: npt.NDArray, d_eci: npt.NDArray, roll: float = 0.0
) -> npt.NDArray:
"""Quaternions that align body-frame unit vector *v_body* with per-sample
ECI unit directions *d_eci*, followed by a right-hand roll of *roll*
radians about *d_eci*.
Parameters
----------
v_body : npt.NDArray, shape (3,)
Body-frame unit vector to be aligned (pre-normalised).
d_eci : npt.NDArray, shape (N, 3)
Per-sample target directions in ECI (pre-normalised).
roll : float
Right-hand rotation about ``d_eci`` applied after alignment.
Returns
-------
npt.NDArray, shape (N, 4)
Unit quaternions [w, x, y, z] such that ``R(q) @ v_body = d_eci``
and the body is rolled by *roll* about ``d_eci``.
"""
N = len(d_eci)
c = d_eci @ v_body # (N,) cos(angle)
axis = np.cross(v_body, d_eci) # (N, 3) |axis| = sin(angle)
half = np.empty((N, 4), dtype=np.float64)
half[:, 0] = 1.0 + c
half[:, 1:] = axis
# 180° edge case: choose any axis perpendicular to v_body
mask = c < -0.9999
if mask.any():
ref = (
np.array([1.0, 0.0, 0.0])
if abs(v_body[0]) < 0.9
else np.array([0.0, 1.0, 0.0])
)
perp = np.cross(v_body, ref)
perp = perp / np.linalg.norm(perp)
half[mask, 0] = 0.0
half[mask, 1:] = perp
q_align = half / np.linalg.norm(half, axis=1, keepdims=True)
if roll == 0.0:
return q_align
# Roll about d_eci in the space frame, applied after alignment:
# q_final = q_roll ⊗ q_align
cr, sr = np.cos(roll / 2), np.sin(roll / 2)
q_roll = np.empty((N, 4), dtype=np.float64)
q_roll[:, 0] = cr
q_roll[:, 1:] = sr * d_eci
return _q_compose_batch(q_roll, q_align)
def _limb_direction(
r_eci: npt.NDArray,
v_eci: npt.NDArray,
t_arr: npt.NDArray,
yaw: float,
A: float,
B: float,
) -> npt.NDArray:
"""Unit vectors pointing at the limb tangent point, per time sample.
Solves the ellipsoid-tangency condition for a ray from the spacecraft
that grazes the offset ellipsoid with equatorial semi-axis ``A`` and
polar semi-axis ``B`` (central-body frame aligned with ECEF axes).
Yaw is measured right-handed about zenith (+R̂) in the LVLH horizontal
plane, with yaw=0 along +Ŝ.
Parameters
----------
r_eci, v_eci : npt.NDArray, shape (N, 3)
Spacecraft ECI position and velocity.
t_arr : npt.NDArray[datetime64[us]], shape (N,)
Sample times.
yaw : float
Yaw angle (rad).
A, B : float
Offset-ellipsoid semi-axes (m).
Returns
-------
npt.NDArray, shape (N, 3)
Unit pointing directions in ECI.
Raises
------
ValueError
If any sample has the spacecraft on or inside the offset ellipsoid,
or if the tangent equation has no real solution.
"""
L = _lvlh_basis(r_eci, v_eci) # (N, 3, 3)
Rz = eci_ecef_rotation(t_arr) # (N, 3, 3)
vecs_eci = np.stack([r_eci, L[:, 0, :], L[:, 1, :], L[:, 2, :]]) # (4, N, 3)
vecs_ecef = np.einsum("nij,knj->kni", Rz, vecs_eci) # (4, N, 3)
r_ecef = vecs_ecef[0]
R_hat_ecef = vecs_ecef[1]
S_hat_ecef = vecs_ecef[2]
W_hat_ecef = vecs_ecef[3]
# Horizontal-yaw direction and nadir direction in ECEF
cy, sy = np.cos(yaw), np.sin(yaw)
p = cy * S_hat_ecef + sy * W_hat_ecef # (N, 3)
q = -R_hat_ecef # (N, 3)
diag_Q = np.array([1.0 / A**2, 1.0 / A**2, 1.0 / B**2])
Qr = r_ecef * diag_Q # (N, 3)
Qp = p * diag_Q
Qq = q * diag_Q
Pr_ = np.einsum("ni,ni->n", p, Qr) # pᵀ Q r
Qr_ = np.einsum("ni,ni->n", q, Qr) # qᵀ Q r
gamma = np.einsum("ni,ni->n", r_ecef, Qr) - 1.0 # rᵀ Q r − 1
Pp = np.einsum("ni,ni->n", p, Qp) # pᵀ Q p
QQ = np.einsum("ni,ni->n", q, Qq) # qᵀ Q q
Pq = np.einsum("ni,ni->n", p, Qq) # pᵀ Q q
if np.any(gamma <= 0):
raise ValueError(
"limb mode: spacecraft is on or inside the offset ellipsoid "
"(altitude_km may be too large or orbit too low)"
)
A_ = Pr_**2 - gamma * Pp
B_ = Pr_ * Qr_ - gamma * Pq
C_ = Qr_**2 - gamma * QQ
disc = B_**2 - A_ * C_
if np.any(disc < 0):
raise ValueError(
"limb mode: tangent equation has no real solution for at "
"least one sample (check yaw_deg and altitude_km)"
)
sqrt_disc = np.sqrt(disc)
tau_a = (-B_ + sqrt_disc) / A_
tau_b = (-B_ - sqrt_disc) / A_
tau = np.where(tau_a > 0, tau_a, tau_b) # pick τ > 0 root
off = np.arctan(tau) # (N,) ∈ (−π/2, π/2)
s, c = np.sin(off), np.cos(off)
d_ecef = s[:, None] * p + c[:, None] * q # (N, 3)
d_ecef = d_ecef / np.linalg.norm(d_ecef, axis=1, keepdims=True)
return np.einsum("nji,nj->ni", Rz, d_ecef)
def _q_boresight(q: npt.NDArray) -> npt.NDArray:
"""Boresight direction (body-z rotated by quaternion q).
Returns the third column of the DCM corresponding to *q*.
Parameters
----------
q : npt.NDArray, shape (4,)
Unit quaternion [w, x, y, z].
Returns
-------
npt.NDArray, shape (3,)
Unit pointing vector in the quaternion's reference frame.
"""
w, x, y, z = q
return np.array(
[
2.0 * (x * z + w * y),
2.0 * (y * z - w * x),
w**2 - x**2 - y**2 + z**2,
]
)
# Nadir quaternion (body-z = −R̂, body-x = Ŝ, body-y = −Ŵ, right-handed)
# DCM (body→LVLH): columns = [[0,1,0], [0,0,-1], [-1,0,0]]
# det = +1; _q_boresight(_NADIR_Q) = [-1, 0, 0] ✓
_NADIR_Q = np.array([-0.5, 0.5, 0.5, -0.5])
# ---------------------------------------------------------------------------
# AbstractAttitudeLaw (ABC)
# ---------------------------------------------------------------------------
[docs]
class AbstractAttitudeLaw(ABC):
"""Base class for spacecraft/sensor pointing laws.
Stores full 3-DOF body orientation as unit quaternions ``[w, x, y, z]``.
The boresight is always **body-z**.
Subclasses must implement :meth:`pointing_eci`, :meth:`rotate_from_body`,
and :meth:`__repr__`. The frame-conversion methods :meth:`pointing_lvlh`
and :meth:`pointing_ecef` are provided by the base class.
Optional yaw steering can be enabled via :meth:`yaw_steering` on
subclasses that support it (:class:`FixedAttitudeLaw` and
:class:`TrackAttitudeLaw`).
"""
def __init__(self) -> None:
self._solar_config = None # AbstractSolarConfig, or None
self._yaw_opt_dir = None # (3,) body-frame direction to face sun
# ------------------------------------------------------------------
# Input coercion (shared by all subclasses)
# ------------------------------------------------------------------
@staticmethod
def _coerce_inputs(r_eci, v_eci, t):
"""Coerce raw inputs to standard internal shapes.
Returns
-------
r_2d : ndarray, shape (N, 3)
v_2d : ndarray, shape (N, 3)
t_arr : ndarray, shape (N,)
scalar : bool
True if the original input was a single sample.
"""
r = np.asarray(r_eci, dtype=np.float64)
scalar = r.ndim == 1
r_2d = np.atleast_2d(r)
v_2d = np.atleast_2d(np.asarray(v_eci, dtype=np.float64))
t_arr = np.atleast_1d(np.asarray(t, dtype="datetime64[us]"))
return r_2d, v_2d, t_arr, scalar
# ------------------------------------------------------------------
# Abstract methods
# ------------------------------------------------------------------
[docs]
@abstractmethod
def pointing_eci(
self,
r_eci: npt.ArrayLike,
v_eci: npt.ArrayLike,
t: npt.ArrayLike,
) -> npt.NDArray[np.floating]:
"""Boresight unit vector(s) in the ECI frame.
Parameters
----------
r_eci : array_like, shape ``(N, 3)`` or ``(3,)``
Host spacecraft ECI position(s) (m).
v_eci : array_like, shape ``(N, 3)`` or ``(3,)``
Host spacecraft ECI velocity(s) (m s⁻¹).
t : array_like of datetime64, shape ``(N,)`` or scalar
Observation epoch(s).
Returns
-------
npt.NDArray[np.floating]
Unit pointing vector(s) in ECI, shape ``(N, 3)`` for array
inputs or ``(3,)`` for scalar inputs.
"""
[docs]
@abstractmethod
def rotate_from_body(
self,
v_body: npt.ArrayLike,
r_eci: npt.ArrayLike,
v_eci: npt.ArrayLike,
t: npt.ArrayLike,
) -> npt.NDArray[np.floating]:
"""Express a spacecraft body-frame vector in the ECI frame.
Parameters
----------
v_body : array_like, shape (3,)
Unit direction in the spacecraft body frame (need not be
pre-normalised).
r_eci : array_like, shape ``(N, 3)`` or ``(3,)``
Host spacecraft ECI position(s) (m).
v_eci : array_like, shape ``(N, 3)`` or ``(3,)``
Host spacecraft ECI velocity(s) (m s⁻¹).
t : array_like of datetime64, shape ``(N,)`` or scalar
Observation epoch(s).
Returns
-------
npt.NDArray[np.floating]
Unit vector(s) in ECI, shape ``(N, 3)`` or ``(3,)``.
"""
@abstractmethod
def __repr__(self) -> str: ...
# ------------------------------------------------------------------
# Concrete pointing methods (frame conversions)
# ------------------------------------------------------------------
[docs]
def pointing_lvlh(
self,
r_eci: npt.ArrayLike,
v_eci: npt.ArrayLike,
t: npt.ArrayLike,
) -> npt.NDArray[np.floating]:
"""Boresight unit vector(s) in the LVLH frame.
Parameters
----------
r_eci : array_like, shape ``(N, 3)`` or ``(3,)``
Host spacecraft ECI position(s) (m).
v_eci : array_like, shape ``(N, 3)`` or ``(3,)``
Host spacecraft ECI velocity(s) (m s⁻¹).
t : array_like of datetime64, shape ``(N,)`` or scalar
Observation epoch(s).
Returns
-------
npt.NDArray[np.floating]
Unit pointing vector(s) in LVLH, shape ``(N, 3)`` for array
inputs or ``(3,)`` for scalar inputs.
"""
r = np.asarray(r_eci, dtype=np.float64)
scalar = r.ndim == 1
r_2d = np.atleast_2d(r)
v_2d = np.atleast_2d(np.asarray(v_eci, dtype=np.float64))
eci = np.atleast_2d(self.pointing_eci(r_eci, v_eci, t)) # (N, 3)
result = eci_to_lvlh(eci, r_2d, v_2d)
return result[0] if scalar else result
[docs]
def pointing_ecef(
self,
r_eci: npt.ArrayLike,
v_eci: npt.ArrayLike,
t: npt.ArrayLike,
) -> npt.NDArray[np.floating]:
"""Boresight unit vector(s) in the ECEF frame.
Parameters
----------
r_eci : array_like, shape ``(N, 3)`` or ``(3,)``
Host spacecraft ECI position(s) (m).
v_eci : array_like, shape ``(N, 3)`` or ``(3,)``
Host spacecraft ECI velocity(s) (m s⁻¹).
t : array_like of datetime64, shape ``(N,)`` or scalar
Observation epoch(s).
Returns
-------
npt.NDArray[np.floating]
Unit pointing vector(s) in ECEF, shape ``(N, 3)`` for array
inputs or ``(3,)`` for scalar inputs.
"""
r = np.asarray(r_eci, dtype=np.float64)
scalar = r.ndim == 1
t_arr = np.atleast_1d(np.asarray(t, dtype="datetime64[us]"))
eci = np.atleast_2d(self.pointing_eci(r_eci, v_eci, t)) # (N, 3)
result = eci_to_ecef(eci, t_arr)
return result[0] if scalar else result
# ------------------------------------------------------------------
# Yaw steering (base: raises NotImplementedError)
# ------------------------------------------------------------------
[docs]
def yaw_steering(self, solar_config) -> None:
"""Enable or disable solar yaw steering.
When active, the roll DOF is controlled dynamically at each timestep
to maximise solar power generation. The boresight direction is
unchanged; only the rotation about the boresight is affected.
Supported by :class:`FixedAttitudeLaw` and :class:`TrackAttitudeLaw`.
Parameters
----------
solar_config : AbstractSolarConfig or None
Solar config whose :meth:`optimal_angle` defines the preferred
sun-facing direction in the body frame. Pass ``None`` to
deactivate yaw steering and revert to the static roll angle.
Raises
------
NotImplementedError
If this attitude law type does not support yaw steering.
TypeError
If ``solar_config`` is not an
:class:`~missiontools.power.AbstractSolarConfig` or ``None``.
"""
if solar_config is None:
self._solar_config = None
self._yaw_opt_dir = None
return
raise NotImplementedError(
f"yaw_steering() is not supported for {type(self).__name__}"
)
def _activate_yaw_steering(self, solar_config) -> None:
"""Shared activation logic for subclasses that support yaw steering."""
from ..power.solar_config import AbstractSolarConfig
if not isinstance(solar_config, AbstractSolarConfig):
raise TypeError(
f"solar_config must be an AbstractSolarConfig instance or "
f"None, got {type(solar_config).__name__!r}"
)
theta = solar_config.optimal_angle(np.array([0.0, 0.0, 1.0]))
self._yaw_opt_dir = np.array(
[
np.sin(theta),
-np.cos(theta),
0.0,
]
)
self._solar_config = solar_config
def _yaw_roll_from_projections(self, b_eci, d0_eci, t_arr):
"""Compute yaw-steering roll angles from boresight and zero-roll direction.
Parameters
----------
b_eci : ndarray, shape (N, 3)
Boresight unit vectors in ECI.
d0_eci : ndarray, shape (N, 3)
Optimal body-frame direction at roll=0, expressed in ECI.
t_arr : ndarray, shape (N,)
Timestamps (datetime64[us]).
Returns
-------
ndarray, shape (N,)
Roll angles (rad).
"""
s_eci = np.atleast_2d(sun_vec_eci(t_arr)) # (N, 3)
d0_dot_b = np.einsum("ij,ij->i", d0_eci, b_eci) # (N,)
d0_perp = d0_eci - d0_dot_b[:, None] * b_eci # (N, 3)
s_dot_b = np.einsum("ij,ij->i", s_eci, b_eci) # (N,)
s_perp = s_eci - s_dot_b[:, None] * b_eci # (N, 3)
cross = np.cross(d0_perp, s_perp) # (N, 3)
sin_a = np.einsum("ij,ij->i", cross, b_eci) # (N,)
cos_a = np.einsum("ij,ij->i", d0_perp, s_perp) # (N,)
return np.arctan2(sin_a, cos_a) # (N,)
# ---------------------------------------------------------------------------
# FixedAttitudeLaw
# ---------------------------------------------------------------------------
[docs]
class FixedAttitudeLaw(AbstractAttitudeLaw):
"""Fixed attitude law: constant body orientation in a reference frame.
Parameters
----------
vector : array_like, shape (3,)
Boresight direction (body-z) expressed in ``frame``. Need not
be a unit vector — it is normalised on input.
frame : {'lvlh', 'eci', 'ecef'}
Reference frame in which ``vector`` is expressed.
roll : float, optional
Roll angle (rad) about the boresight axis. Default 0 gives the
minimum rotation from identity that aligns body-z with ``vector``.
Raises
------
ValueError
If ``frame`` is not recognised or ``vector`` is the zero vector.
Examples
--------
Nadir pointing::
law = FixedAttitudeLaw.nadir()
Body-z along-track in LVLH::
law = FixedAttitudeLaw([0, 1, 0], 'lvlh')
"""
def __init__(self, vector, frame: str, roll: float = 0.0) -> None:
super().__init__()
vec = np.asarray(vector, dtype=np.float64)
if vec.shape != (3,):
raise ValueError(f"vector must have shape (3,), got {vec.shape}")
if np.linalg.norm(vec) == 0.0:
raise ValueError("vector must not be the zero vector")
if frame not in _VALID_FRAMES:
raise ValueError(
f"frame must be one of {sorted(_VALID_FRAMES)}, got {frame!r}"
)
self._q = _q_from_vec(vec, roll)
self._frame = frame
self._pointing_in_ref = _q_boresight(self._q)
[docs]
@classmethod
def nadir(cls, roll: float = 0.0) -> FixedAttitudeLaw:
"""Earth-nadir pointing law (body-z = −R̂ in LVLH).
Full 3-DOF convention at ``roll=0``: body-z = nadir (−R̂),
body-x = along-track (Ŝ), body-y = −orbit-normal (−Ŵ). This is a
right-handed body frame.
Parameters
----------
roll : float, optional
Roll angle (rad) about the boresight (nadir) axis. Default 0
gives body-x = along-track.
"""
obj = cls.__new__(cls)
AbstractAttitudeLaw.__init__(obj)
if roll == 0.0:
q = _NADIR_Q.copy()
else:
q_roll = np.array([np.cos(roll / 2), 0.0, 0.0, np.sin(roll / 2)])
q = _q_compose(_NADIR_Q, q_roll)
obj._q = q
obj._frame = "lvlh"
obj._pointing_in_ref = _q_boresight(q)
return obj
def __repr__(self) -> str:
return (
f"FixedAttitudeLaw(frame={self._frame!r}, "
f"boresight={self._pointing_in_ref})"
)
[docs]
def pointing_eci(self, r_eci, v_eci, t):
r_2d, v_2d, t_arr, scalar = self._coerce_inputs(r_eci, v_eci, t)
N = len(r_2d)
tile = np.tile(self._pointing_in_ref, (N, 1)) # (N, 3)
if self._frame == "eci":
vecs = tile
elif self._frame == "lvlh":
vecs = lvlh_to_eci(tile, r_2d, v_2d)
else:
vecs = ecef_to_eci(tile, t_arr)
result = vecs / np.linalg.norm(vecs, axis=1, keepdims=True)
return result[0] if scalar else result
[docs]
def rotate_from_body(self, v_body, r_eci, v_eci, t):
v = np.asarray(v_body, dtype=np.float64)
v = v / np.linalg.norm(v)
r_2d, v_2d, t_arr, scalar = self._coerce_inputs(r_eci, v_eci, t)
N = len(r_2d)
if self._solar_config is not None:
boresight = self._pointing_in_ref # (3,) in ref frame
b_ref = np.tile(boresight, (N, 1)) # (N, 3) in ref
if self._frame == "eci":
b_eci = b_ref
elif self._frame == "lvlh":
b_eci = lvlh_to_eci(b_ref, r_2d, v_2d)
else:
b_eci = ecef_to_eci(b_ref, t_arr)
b_eci = b_eci / np.linalg.norm(b_eci, axis=1, keepdims=True)
yaw_rolls = self._compute_yaw_rolls(b_eci, r_2d, v_2d, t_arr)
qs = _q_from_vec_batch(
np.tile(boresight, (N, 1)),
roll=yaw_rolls,
) # (N, 4)
vecs_ref = _q_rotate_batch(qs, v) # (N, 3) in ref
if self._frame == "eci":
vecs = vecs_ref
elif self._frame == "lvlh":
vecs = lvlh_to_eci(vecs_ref, r_2d, v_2d)
else:
vecs = ecef_to_eci(vecs_ref, t_arr)
else:
v_ref = _q_rotate(self._q, v) # (3,) in ref frame
tiled = np.tile(v_ref, (N, 1)) # (N, 3)
if self._frame == "eci":
vecs = tiled
elif self._frame == "lvlh":
vecs = lvlh_to_eci(tiled, r_2d, v_2d)
else:
vecs = ecef_to_eci(tiled, t_arr)
result = vecs / np.linalg.norm(vecs, axis=1, keepdims=True)
return result[0] if scalar else result
# -- Yaw steering -------------------------------------------------------
[docs]
def yaw_steering(self, solar_config) -> None:
if solar_config is None:
self._solar_config = None
self._yaw_opt_dir = None
return
self._activate_yaw_steering(solar_config)
def _compute_yaw_rolls(self, b_eci, r_2d, v_2d, t_arr):
N = len(t_arr)
q0 = _q_from_vec(self._pointing_in_ref) # base quat, roll=0
d0_ref = _q_rotate(q0, self._yaw_opt_dir) # (3,) in ref frame
d0_ref_tiled = np.tile(d0_ref, (N, 1)) # (N, 3)
if self._frame == "eci":
d0_eci = d0_ref_tiled
elif self._frame == "lvlh":
d0_eci = lvlh_to_eci(d0_ref_tiled, r_2d, v_2d)
else:
d0_eci = ecef_to_eci(d0_ref_tiled, t_arr)
return self._yaw_roll_from_projections(b_eci, d0_eci, t_arr)
# ---------------------------------------------------------------------------
# TrackAttitudeLaw
# ---------------------------------------------------------------------------
[docs]
class TrackAttitudeLaw(AbstractAttitudeLaw):
"""Target-tracking attitude law: boresight always points toward a target.
Parameters
----------
target : Spacecraft | GroundStation
The object to track. A :class:`~missiontools.Spacecraft` target is
propagated analytically; a :class:`~missiontools.GroundStation`
target is converted from its fixed geodetic position to ECI at each
timestep.
roll : float, optional
Roll angle (rad) about the boresight axis. Default 0 uses the
minimum-rotation convention from :func:`_q_from_vec` to pin the
remaining degree of freedom.
Raises
------
TypeError
If ``target`` is not a :class:`~missiontools.Spacecraft` or
:class:`~missiontools.GroundStation`.
Examples
--------
Track another spacecraft::
target = Spacecraft(...)
law = TrackAttitudeLaw(target)
Track a ground station::
from missiontools import GroundStation
gs = GroundStation(lat=51.5, lon=-0.1)
law = TrackAttitudeLaw(gs)
"""
def __init__(self, target, roll: float = 0.0) -> None:
super().__init__()
from ..spacecraft import Spacecraft
from ..ground_station import GroundStation
if isinstance(target, Spacecraft):
self._is_sc = True
elif isinstance(target, GroundStation):
self._is_sc = False
else:
raise TypeError(
f"target must be a Spacecraft or GroundStation instance, "
f"got {type(target).__name__!r}"
)
self._target = target
self._roll = roll
if not self._is_sc:
from ..orbit.frames import geodetic_to_ecef
self._gs_ecef = geodetic_to_ecef(
np.radians(target.lat),
np.radians(target.lon),
target.alt,
)
def _target_eci(self, t_arr):
r, _ = host_eci_state(self._target, t_arr)
return r
def __repr__(self) -> str:
return f"TrackAttitudeLaw(target={self._target!r})"
[docs]
def pointing_eci(self, r_eci, v_eci, t):
r_2d, v_2d, t_arr, scalar = self._coerce_inputs(r_eci, v_eci, t)
r_tgt = self._target_eci(t_arr)
vecs = r_tgt - r_2d
result = vecs / np.linalg.norm(vecs, axis=1, keepdims=True)
return result[0] if scalar else result
[docs]
def rotate_from_body(self, v_body, r_eci, v_eci, t):
v = np.asarray(v_body, dtype=np.float64)
v = v / np.linalg.norm(v)
r_2d, v_2d, t_arr, scalar = self._coerce_inputs(r_eci, v_eci, t)
r_tgt = self._target_eci(t_arr)
d = r_tgt - r_2d
d = d / np.linalg.norm(d, axis=1, keepdims=True) # (N, 3) unit
if self._solar_config is not None:
b_eci = d
yaw_rolls = self._compute_yaw_rolls(b_eci, r_2d, v_2d, t_arr)
qs = _q_from_vec_batch(d, roll=yaw_rolls)
else:
qs = _q_from_vec_batch(d, roll=self._roll)
vecs = _q_rotate_batch(qs, v) # (N, 3)
result = vecs / np.linalg.norm(vecs, axis=1, keepdims=True)
return result[0] if scalar else result
# -- Yaw steering -------------------------------------------------------
[docs]
def yaw_steering(self, solar_config) -> None:
if solar_config is None:
self._solar_config = None
self._yaw_opt_dir = None
return
self._activate_yaw_steering(solar_config)
def _compute_yaw_rolls(self, b_eci, r_2d, v_2d, t_arr):
qs0 = _q_from_vec_batch(b_eci) # (N, 4) roll=0
d0_eci = _q_rotate_batch(qs0, self._yaw_opt_dir) # (N, 3) in ECI
return self._yaw_roll_from_projections(b_eci, d0_eci, t_arr)
# ---------------------------------------------------------------------------
# CustomAttitudeLaw
# ---------------------------------------------------------------------------
[docs]
class CustomAttitudeLaw(AbstractAttitudeLaw):
"""Custom attitude law defined by a user-supplied callback.
The callback receives the spacecraft state at each timestep and returns
full 3-DOF body attitude quaternions in ECI, giving complete control
over pointing.
Parameters
----------
callback : callable
A function with signature::
callback(t, r_eci, v_eci) -> quaternions
where:
* ``t`` — ``(N,)`` array of ``datetime64[us]`` time instants
* ``r_eci`` — ``(N, 3)`` ECI position array (m)
* ``v_eci`` — ``(N, 3)`` ECI velocity array (m s⁻¹)
* returns — ``(N, 4)`` array of unit quaternions ``[w, x, y, z]``
defining body attitude in ECI
Raises
------
TypeError
If ``callback`` is not callable.
Examples
--------
::
def my_attitude(t, r_eci, v_eci):
N = len(t)
q = np.zeros((N, 4))
q[:, 0] = 1.0 # identity: body frame aligned with ECI
return q
law = CustomAttitudeLaw(my_attitude)
"""
def __init__(self, callback) -> None:
super().__init__()
if not callable(callback):
raise TypeError(
f"callback must be callable, got {type(callback).__name__!r}"
)
self._callback = callback
def __repr__(self) -> str:
cb_name = getattr(self._callback, "__name__", repr(self._callback))
return f"CustomAttitudeLaw(callback={cb_name!r})"
[docs]
def pointing_eci(self, r_eci, v_eci, t):
r_2d, v_2d, t_arr, scalar = self._coerce_inputs(r_eci, v_eci, t)
qs = np.asarray(self._callback(t_arr, r_2d, v_2d), dtype=np.float64)
vecs = _q_rotate_batch(qs, np.array([0.0, 0.0, 1.0]))
result = vecs / np.linalg.norm(vecs, axis=1, keepdims=True)
return result[0] if scalar else result
[docs]
def rotate_from_body(self, v_body, r_eci, v_eci, t):
v = np.asarray(v_body, dtype=np.float64)
v = v / np.linalg.norm(v)
r_2d, v_2d, t_arr, scalar = self._coerce_inputs(r_eci, v_eci, t)
qs = np.asarray(self._callback(t_arr, r_2d, v_2d), dtype=np.float64)
vecs = _q_rotate_batch(qs, v) # (N, 3)
result = vecs / np.linalg.norm(vecs, axis=1, keepdims=True)
return result[0] if scalar else result
# ---------------------------------------------------------------------------
# LimbAttitudeLaw
# ---------------------------------------------------------------------------
[docs]
class LimbAttitudeLaw(AbstractAttitudeLaw):
"""Limb-pointing attitude law: body-frame vector grazes the central body.
The given body-frame vector is continuously aligned with the ray from
the spacecraft that grazes an offset ellipsoid at altitude
``altitude_km`` above the central body.
Parameters
----------
body_vector : array_like, shape (3,)
Body-frame axis to be aligned with the limb direction. Need not
be pre-normalised.
altitude_km : float
Grazing altitude above the central body (km, ≥ 0).
yaw_deg : float, optional
Azimuth of the pointing direction in the LVLH horizontal plane,
measured right-handed about zenith (+R̂) from the +Ŝ
(along-track) axis. Default 0 (forward limb).
roll_deg : float, optional
Right-hand rotation about the outward pointing vector. Default 0.
body_semi_major_axis : float, optional
Equatorial radius of the central body (m). Defaults to Earth
WGS84 (``EARTH_SEMI_MAJOR_AXIS``).
body_flattening : float, optional
Flattening of the central body (dimensionless, in ``[0, 1)``;
``0`` gives a sphere). Defaults to Earth WGS84.
Raises
------
ValueError
If ``body_vector`` is the zero vector or not shape ``(3,)``,
``altitude_km < 0``, ``body_semi_major_axis <= 0``, or
``body_flattening`` is outside ``[0, 1)``.
Examples
--------
Boresight (body-z) at the forward limb 20 km above the surface::
law = LimbAttitudeLaw([0, 0, 1], altitude_km=20)
Cross-track limb view with body-x aligned to the limb::
law = LimbAttitudeLaw([1, 0, 0], altitude_km=50, yaw_deg=90)
"""
def __init__(
self,
body_vector,
altitude_km: float,
*,
yaw_deg: float = 0.0,
roll_deg: float = 0.0,
body_semi_major_axis: float = EARTH_SEMI_MAJOR_AXIS,
body_flattening: float = 1.0 / EARTH_INVERSE_FLATTENING,
) -> None:
super().__init__()
vec = np.asarray(body_vector, dtype=np.float64)
if vec.shape != (3,):
raise ValueError(f"body_vector must have shape (3,), got {vec.shape}")
norm = np.linalg.norm(vec)
if norm == 0.0:
raise ValueError("body_vector must not be the zero vector")
if altitude_km < 0:
raise ValueError(f"altitude_km must be non-negative, got {altitude_km}")
if body_semi_major_axis <= 0:
raise ValueError(
f"body_semi_major_axis must be positive, got {body_semi_major_axis}"
)
if not (0.0 <= body_flattening < 1.0):
raise ValueError(
f"body_flattening must be in [0, 1), got {body_flattening}"
)
altitude_m = altitude_km * 1e3
A = body_semi_major_axis + altitude_m
B = body_semi_major_axis * (1.0 - body_flattening) + altitude_m
self._limb = {
"body_vector": vec / norm,
"yaw": np.deg2rad(yaw_deg),
"roll": np.deg2rad(roll_deg),
"A": A,
"B": B,
}
def __repr__(self) -> str:
return (
f"LimbAttitudeLaw("
f"body_vector={self._limb['body_vector'].tolist()}, "
f"yaw_deg={np.rad2deg(self._limb['yaw']):.3f}, "
f"roll_deg={np.rad2deg(self._limb['roll']):.3f})"
)
[docs]
def pointing_eci(self, r_eci, v_eci, t):
r_2d, v_2d, t_arr, scalar = self._coerce_inputs(r_eci, v_eci, t)
d_eci = _limb_direction(
r_2d,
v_2d,
t_arr,
self._limb["yaw"],
self._limb["A"],
self._limb["B"],
)
qs = _q_align_batch(
self._limb["body_vector"],
d_eci,
self._limb["roll"],
)
vecs = _q_rotate_batch(qs, np.array([0.0, 0.0, 1.0]))
result = vecs / np.linalg.norm(vecs, axis=1, keepdims=True)
return result[0] if scalar else result
[docs]
def rotate_from_body(self, v_body, r_eci, v_eci, t):
v = np.asarray(v_body, dtype=np.float64)
v = v / np.linalg.norm(v)
r_2d, v_2d, t_arr, scalar = self._coerce_inputs(r_eci, v_eci, t)
d_eci = _limb_direction(
r_2d,
v_2d,
t_arr,
self._limb["yaw"],
self._limb["A"],
self._limb["B"],
)
qs = _q_align_batch(
self._limb["body_vector"],
d_eci,
self._limb["roll"],
)
vecs = _q_rotate_batch(qs, v) # (N, 3)
result = vecs / np.linalg.norm(vecs, axis=1, keepdims=True)
return result[0] if scalar else result
# ---------------------------------------------------------------------------
# ConditionAttitudeLaw
# ---------------------------------------------------------------------------
[docs]
class ConditionAttitudeLaw(AbstractAttitudeLaw):
"""Conditional attitude law: route between attitude laws by condition.
Each timestep is dispatched to one of a chain of child attitude laws,
selected by evaluating a list of
:class:`~missiontools.condition.AbstractCondition` predicates in
priority order. When no condition holds, the timestep falls through
to ``default_attitude``.
Parameters
----------
default_attitude : AbstractAttitudeLaw
The fallback law used at any timestep where no condition holds.
condition_attitudes : list of (AbstractCondition, AbstractAttitudeLaw)
Ordered chain of (condition, law) pairs. At each timestep, the
first condition that evaluates to ``True`` selects its paired law;
earlier pairs take precedence over later ones. An empty list is
permitted and produces a degenerate wrapper around
``default_attitude``.
Raises
------
TypeError
If ``default_attitude`` is not an :class:`AbstractAttitudeLaw`, or
any element of ``condition_attitudes`` is not a 2-tuple of
(:class:`~missiontools.condition.AbstractCondition`,
:class:`AbstractAttitudeLaw`).
Notes
-----
Switching between child laws produces an instantaneous pointing
discontinuity at the boundary; no slew dynamics are modelled.
Yaw steering, when enabled via :meth:`yaw_steering`, is propagated
to every child law and to the default; if any child does not
support yaw steering the call raises ``NotImplementedError``.
Examples
--------
Switch from nadir pointing to ground-station tracking when the
spacecraft is in view of a downlink site::
from missiontools import (FixedAttitudeLaw, TrackAttitudeLaw,
GroundStation, Spacecraft)
from missiontools.attitude import ConditionAttitudeLaw
from missiontools.condition import SpaceGroundAccessCondition
sc = Spacecraft(...)
gs = GroundStation(lat=51.5, lon=-0.1)
link_law = TrackAttitudeLaw(...)
nadir = FixedAttitudeLaw.nadir()
cond = SpaceGroundAccessCondition(sc, gs, el_min_deg=5.0)
law = ConditionAttitudeLaw(
default_attitude = nadir,
condition_attitudes = [(cond, link_law)],
)
"""
def __init__(self, default_attitude, condition_attitudes) -> None:
super().__init__()
from ..condition import AbstractCondition
if not isinstance(default_attitude, AbstractAttitudeLaw):
raise TypeError(
f"default_attitude must be an AbstractAttitudeLaw instance, "
f"got {type(default_attitude).__name__!r}"
)
pairs = list(condition_attitudes)
for idx, item in enumerate(pairs):
if not isinstance(item, tuple) or len(item) != 2:
raise TypeError(
f"condition_attitudes[{idx}] must be a 2-tuple "
f"(AbstractCondition, AbstractAttitudeLaw), got {item!r}"
)
cond, law = item
if not isinstance(cond, AbstractCondition):
raise TypeError(
f"condition_attitudes[{idx}][0] must be an "
f"AbstractCondition instance, got "
f"{type(cond).__name__!r}"
)
if not isinstance(law, AbstractAttitudeLaw):
raise TypeError(
f"condition_attitudes[{idx}][1] must be an "
f"AbstractAttitudeLaw instance, got "
f"{type(law).__name__!r}"
)
self._default = default_attitude
self._pairs = pairs
def __repr__(self) -> str:
branches = ", ".join(f"({c!r}, {l!r})" for c, l in self._pairs)
return f"ConditionAttitudeLaw(default={self._default!r}, branches=[{branches}])"
def _resolve_winners(self, t_arr: npt.NDArray) -> npt.NDArray[np.intp]:
"""Per-sample winning child index, or -1 for the default."""
N = len(t_arr)
winners = np.full(N, -1, dtype=np.intp)
if not self._pairs:
return winners
unresolved = np.ones(N, dtype=bool)
for k, (cond, _) in enumerate(self._pairs):
if not unresolved.any():
break
mask = np.asarray(cond.at(t_arr), dtype=bool)
take = unresolved & mask
winners[take] = k
unresolved &= ~mask
return winners
def _dispatch(
self,
method_name: str,
r_2d: npt.NDArray,
v_2d: npt.NDArray,
t_arr: npt.NDArray,
*extra,
) -> npt.NDArray:
"""Scatter-gather: call each child law on its assigned sample subset.
``extra`` is forwarded ahead of (r, v, t) to support
:meth:`rotate_from_body` whose signature is (v_body, r, v, t).
"""
winners = self._resolve_winners(t_arr)
N = len(t_arr)
out = np.empty((N, 3), dtype=np.float64)
unique = np.unique(winners)
for k in unique:
mask = winners == k
law = self._default if k == -1 else self._pairs[k][1]
res = getattr(law, method_name)(*extra, r_2d[mask], v_2d[mask], t_arr[mask])
out[mask] = np.atleast_2d(res)
return out
[docs]
def pointing_eci(self, r_eci, v_eci, t):
r_2d, v_2d, t_arr, scalar = self._coerce_inputs(r_eci, v_eci, t)
vecs = self._dispatch("pointing_eci", r_2d, v_2d, t_arr)
result = vecs / np.linalg.norm(vecs, axis=1, keepdims=True)
return result[0] if scalar else result
[docs]
def rotate_from_body(self, v_body, r_eci, v_eci, t):
v = np.asarray(v_body, dtype=np.float64)
v = v / np.linalg.norm(v)
r_2d, v_2d, t_arr, scalar = self._coerce_inputs(r_eci, v_eci, t)
vecs = self._dispatch("rotate_from_body", r_2d, v_2d, t_arr, v)
result = vecs / np.linalg.norm(vecs, axis=1, keepdims=True)
return result[0] if scalar else result
[docs]
def yaw_steering(self, solar_config) -> None:
self._default.yaw_steering(solar_config)
for _, law in self._pairs:
law.yaw_steering(solar_config)
if solar_config is None:
self._solar_config = None
self._yaw_opt_dir = None
else:
self._solar_config = solar_config