"""
AttitudeLaw
===========
Spacecraft/sensor pointing law with full quaternion internal storage.
"""
from __future__ import annotations
import numpy as np
import numpy.typing as npt
from ..orbit.frames import (eci_to_ecef, eci_to_lvlh,
ecef_to_eci, lvlh_to_eci,
sun_vec_eci)
from ..orbit.propagation import propagate_analytical
_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., 1., 0., 0.])
else:
# Half-angle construction: axis = [0,0,1] × v, w = 1 + cos θ
axis = np.cross([0., 0., 1.], 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., 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., 1., 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_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])
# ---------------------------------------------------------------------------
# AttitudeLaw
# ---------------------------------------------------------------------------
[docs]
class AttitudeLaw:
"""Spacecraft/sensor pointing law with full quaternion internal storage.
Prefer the factory classmethods :meth:`fixed`, :meth:`track`, and
:meth:`nadir` for typical usage. The constructor is public and may be
called directly by power users or subclasses.
Parameters
----------
mode : str
Pointing mode: ``'fixed'`` or ``'track'``.
q : ndarray of shape (4,), optional
Unit quaternion ``[w, x, y, z]`` defining the body orientation in the
reference frame. Required for ``mode='fixed'``.
frame : str, optional
Reference frame for the quaternion: ``'lvlh'``, ``'eci'``, or
``'ecef'``. Required for ``mode='fixed'``.
target : Spacecraft, optional
Target spacecraft to track. Required for ``mode='track'``.
roll : float, optional
Roll angle (rad) about the boresight axis. Default 0.
Boresight convention
--------------------
The sensor/spacecraft boresight is always **body-z**. The pointing
methods return this axis expressed in the requested frame.
Modes
-----
``'fixed'``
A constant body orientation in a given reference frame, stored as a
unit quaternion ``[w, x, y, z]``.
``'track'``
Points from the host spacecraft toward a target
:class:`~missiontools.Spacecraft`. The 2-DOF pointing direction is
fully defined; the roll DOF is reserved for a future release.
Examples
--------
Nadir pointing::
from missiontools import AttitudeLaw
law = AttitudeLaw.nadir()
p_eci = law.pointing_eci(r, v, t)
Track a target spacecraft::
from missiontools import Spacecraft, AttitudeLaw
target = Spacecraft(...)
law = AttitudeLaw.track(target)
Fixed attitude in LVLH (e.g., body-z pointing along-track)::
law = AttitudeLaw.fixed([0, 1, 0], 'lvlh')
"""
def __init__(self, mode: str, *,
q: npt.NDArray | None = None,
frame: str | None = None,
target=None,
roll: float = 0.0):
self._mode = mode # 'fixed' | 'track'
self._q = q # (4,) unit ndarray [w,x,y,z], or None
self._frame = frame # 'lvlh' | 'eci' | 'ecef', or None
self._target = target # Spacecraft, or None
self._roll = roll # roll about boresight for 'track' mode
# Pre-compute boresight direction in the reference frame for 'fixed'
self._pointing_in_ref = _q_boresight(q) if q is not None else None
# Yaw steering state
self._solar_config = None # AbstractSolarConfig, or None
self._yaw_opt_dir = None # (3,) body-frame direction to face sun
# ------------------------------------------------------------------
# Factory classmethods
# ------------------------------------------------------------------
[docs]
@classmethod
def fixed(cls, vector, frame: str,
roll: float = 0.0) -> AttitudeLaw:
"""Fixed attitude law: constant body orientation in a given 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.
"""
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}"
)
q = _q_from_vec(vec, roll)
return cls('fixed', q=q, frame=frame)
[docs]
@classmethod
def track(cls, target, roll: float = 0.0) -> AttitudeLaw:
"""Target-tracking attitude law: boresight always points toward target.
Parameters
----------
target : Spacecraft
The spacecraft to track.
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. Note that ``roll=0`` does **not**
correspond to a physically meaningful reference orientation such
as orbit-normal or sun-pointing; it is purely a deterministic
convention that changes as the target moves.
Raises
------
TypeError
If ``target`` is not a :class:`~missiontools.Spacecraft`.
"""
from ..spacecraft import Spacecraft # local import avoids circular dep
if not isinstance(target, Spacecraft):
raise TypeError(
f"target must be a Spacecraft instance, "
f"got {type(target).__name__!r}"
)
return cls('track', target=target, roll=roll)
[docs]
@classmethod
def nadir(cls, roll: float = 0.0) -> AttitudeLaw:
"""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.
"""
if roll == 0.0:
q = _NADIR_Q.copy()
else:
q_roll = np.array([np.cos(roll / 2), 0., 0., np.sin(roll / 2)])
q = _q_compose(_NADIR_Q, q_roll)
return cls('fixed', q=q, frame='lvlh')
# ------------------------------------------------------------------
# Yaw steering
# ------------------------------------------------------------------
[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.
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
------
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
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}"
)
# Pre-compute the body-frame direction that should face the sun.
# optimal_angle returns theta measured from basis u=[0,1,0], v=[-1,0,0]
# (when rotation_axis=[0,0,1]). The sun-facing direction is -d(theta):
# -d = -cos(theta)*u - sin(theta)*v = [sin(theta), -cos(theta), 0]
theta = solar_config.optimal_angle(np.array([0., 0., 1.]))
self._yaw_opt_dir = np.array([
np.sin(theta), -np.cos(theta), 0.0,
])
self._solar_config = solar_config
def _compute_yaw_rolls(self, b_eci, r_2d, v_2d, t_arr):
"""Compute per-timestep roll angles for yaw steering.
Parameters
----------
b_eci : ndarray, shape (N, 3)
Boresight unit vectors in ECI (independent of roll).
r_2d, v_2d : ndarray, shape (N, 3)
Spacecraft ECI position and velocity.
t_arr : ndarray, shape (N,)
Timestamps (datetime64[us]).
Returns
-------
ndarray, shape (N,)
Roll angles (rad) that align the optimal body-frame direction
with the sun.
"""
N = len(t_arr)
s_eci = np.atleast_2d(sun_vec_eci(t_arr)) # (N, 3)
# At roll=0, rotate the optimal body-frame direction to ref frame
if self._mode == 'fixed':
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)
else: # 'track'
# b_eci are per-timestep boresight directions; build roll=0 quats
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
# Project d0_eci and sun into the plane perpendicular to boresight
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)
# Signed angle from d0_perp to s_perp about b_eci
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,)
# ------------------------------------------------------------------
# Pointing methods
# ------------------------------------------------------------------
[docs]
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.
"""
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]'))
N = len(r_2d)
if self._mode == 'fixed':
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)
else: # 'track'
r_tgt, _ = propagate_analytical(
t_arr, **self._target.keplerian_params,
propagator_type=self._target.propagator_type,
)
vecs = r_tgt - r_2d
result = vecs / np.linalg.norm(vecs, axis=1, keepdims=True)
return result[0] if scalar else result
[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
[docs]
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.
Supported for both ``'fixed'`` and ``'track'`` modes. For ``'track'``
mode the :attr:`roll` angle stored on the law (default 0) pins the
remaining degree of freedom using the minimum-rotation convention.
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,)``.
"""
v = np.asarray(v_body, dtype=np.float64)
v = v / np.linalg.norm(v)
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]'))
N = len(r_2d)
if self._mode == 'fixed':
if self._solar_config is not None:
# Yaw steering: per-timestep roll
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)
else: # 'track'
r_tgt, _ = propagate_analytical(
t_arr, **self._target.keplerian_params,
propagator_type=self._target.propagator_type,
)
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 # boresight in ECI = direction to target
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