__all__ = ["MTQ_w_RW_QPG"]
import numpy as np
import matplotlib.pyplot as plt
from scipy.spatial import ConvexHull
from scipy.optimize import lsq_linear
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
import itertools
from ADCS.CONOPS.goals import Goal, No_Goal
from ADCS.controller import Controller, MTQ_w_RW_LP
from ADCS.controller.helpers.quaternion_math import vector_alignment_error
from ADCS.satellite_hardware.satellite.estimated_satellite import EstimatedSatellite
from ADCS.orbits.orbital_state import Orbital_State
from ADCS.satellite_hardware.actuators import MTQ, RW
from ADCS.satellite_hardware.sensors import MTM
from ADCS.helpers.math_helpers import rot_mat, skewsym, limit
[docs]
class MTQ_w_RW_QPG(MTQ_w_RW_LP):
r"""
Gyroscopically–Weighted Quadratic Torque Allocation for Mixed RW–MTQ ADCS.
This controller implements a state-dependent weighted bounded least-squares torque allocator
for a spacecraft with reaction wheels (RWs) and magnetorquers (MTQs). It extends
:class:`~ADCS.controller.MTQ_w_RW_LP` by introducing an anisotropic weighting matrix
:math:`W(\boldsymbol{\omega})` that depends on the instantaneous body angular velocity
:math:`\boldsymbol{\omega}`. The weighting modifies the tracking error metric so that torque
error along the spin axis can be penalized differently from error in the transverse plane.
The allocator computes actuator commands that minimize a weighted torque residual subject to
actuator saturation bounds, using :meth:`scipy.optimize.lsq_linear`.
Combined Actuator Torque Model
-------------------------------
Let the desired body torque be :math:`\boldsymbol{\tau}_{\mathrm{des}} \in \mathbb{R}^3` and
define the stacked actuator command vector
.. math::
\boldsymbol{u}
=
\begin{bmatrix}
\boldsymbol{u}_{\mathrm{rw}} \\
\boldsymbol{u}_{\mathrm{mtq}}
\end{bmatrix}
\in \mathbb{R}^{n_{\mathrm{rw}} + n_{\mathrm{mtq}}}.
Reaction wheel torque is generated along wheel axes stacked in
:math:`A_{\mathrm{rw}} \in \mathbb{R}^{3 \times n_{\mathrm{rw}}}`:
.. math::
\boldsymbol{\tau}_{\mathrm{rw}} = A_{\mathrm{rw}} \boldsymbol{u}_{\mathrm{rw}}.
Magnetorquer torque is generated by commanding a dipole moment in the geomagnetic field
:math:`\boldsymbol{B}`:
.. math::
\boldsymbol{\tau}_{\mathrm{mtq}}
=
\boldsymbol{m} \times \boldsymbol{B}
=
-[\boldsymbol{B}]_{\times}\boldsymbol{m}
=
\left(-[\boldsymbol{B}]_{\times} A_{\mathrm{mtq}}\right)\boldsymbol{u}_{\mathrm{mtq}}.
The combined mapping is
.. math::
A_{\mathrm{tot}}
=
\begin{bmatrix}
A_{\mathrm{rw}} & -[\boldsymbol{B}]_{\times} A_{\mathrm{mtq}}
\end{bmatrix},
\qquad
\boldsymbol{\tau}_{\mathrm{ach}} = A_{\mathrm{tot}} \boldsymbol{u}.
Gyroscopically Weighted Error Metric
-------------------------------------
Define the unit spin axis (when :math:`\|\boldsymbol{\omega}\| > 0`) as
.. math::
\hat{\boldsymbol{\omega}} = \frac{\boldsymbol{\omega}}{\|\boldsymbol{\omega}\|}.
The weighting matrix is a rank-1 update to the identity:
.. math::
W(\boldsymbol{\omega})
=
I_{3 \times 3} + \gamma \left(\hat{\boldsymbol{\omega}} \hat{\boldsymbol{\omega}}^{\mathsf{T}}\right).
For :math:`\gamma > 0`, residual components parallel to :math:`\boldsymbol{\omega}` are
penalized more heavily than transverse components. For :math:`\gamma < 0`, the allocator
relatively de-emphasizes spin-axis tracking in favor of transverse tracking. When
:math:`\gamma = 0` or :math:`\|\boldsymbol{\omega}\| \approx 0`, the weighting reduces to
:math:`W = I`.
Weighted Bounded Least Squares Allocation
------------------------------------------
The allocation solves
.. math::
\min_{\boldsymbol{u}}
\;\left\|W(\boldsymbol{\omega})\left(A_{\mathrm{tot}}\boldsymbol{u}-\boldsymbol{\tau}_{\mathrm{des}}\right)\right\|_2^2
subject to box constraints
.. math::
-u_{i,\max} \le u_i \le u_{i,\max}, \qquad \forall i.
This can be implemented by left-multiplying the mapping and target:
.. math::
A_w = W A_{\mathrm{tot}},
\qquad
\boldsymbol{b}_w = W \boldsymbol{\tau}_{\mathrm{des}},
and then solving
.. math::
\min_{\boldsymbol{u}}
\;\left\|A_w \boldsymbol{u} - \boldsymbol{b}_w\right\|_2^2
with the same bounds.
Effectiveness Metric
---------------------
After solving for :math:`\boldsymbol{u}^*`, the achieved torque is
:math:`\boldsymbol{\tau}_{\mathrm{ach}} = A_{\mathrm{tot}}\boldsymbol{u}^*`. An effectiveness
scalar is computed by projecting :math:`\boldsymbol{\tau}_{\mathrm{ach}}` onto the desired
direction:
.. math::
\hat{\boldsymbol{\tau}}
=
\frac{\boldsymbol{\tau}_{\mathrm{des}}}{\|\boldsymbol{\tau}_{\mathrm{des}}\|},
\qquad
\alpha
=
\max\!\left(0,\frac{\boldsymbol{\tau}_{\mathrm{ach}}\cdot\hat{\boldsymbol{\tau}}}{\|\boldsymbol{\tau}_{\mathrm{des}}\|}\right).
:param est_sat: Estimated satellite model providing actuator instances and configuration.
:type est_sat: :class:`~ADCS.satellite_hardware.satellite.estimated_satellite.EstimatedSatellite`
:param p_gain: Proportional gain used by the parent controller law.
:type p_gain: float
:param d_gain: Derivative gain used by the parent controller law.
:type d_gain: float
:param gamma: Weighting scalar that scales the penalty for torque residual parallel to the spin axis.
:type gamma: float
:param c_gain: Additional control gain used by the parent controller law.
:type c_gain: float
:param h_target: Target wheel momentum vector used by the parent mixed-actuation controller.
:type h_target: numpy.ndarray | list
"""
def __init__(self, est_sat: EstimatedSatellite, p_gain: float, d_gain: float, gamma: float, c_gain: float, h_target: np.ndarray | list = np.zeros(3)) -> None:
r"""
Initialize the gyroscopically weighted QP torque allocator.
This initializer stores the weighting gain ``gamma`` and delegates base controller
initialization to :class:`~ADCS.controller.MTQ_w_RW_LP`. The stored gain is used by
:meth:`~ADCS.controller.MTQ_w_RW_QPG.allocate_max_torque_in_direction` to construct
:math:`W(\boldsymbol{\omega})`.
:param est_sat: Estimated satellite model providing actuators and configuration.
:type est_sat: :class:`~ADCS.satellite_hardware.satellite.estimated_satellite.EstimatedSatellite`
:param p_gain: Proportional gain used by the parent control law.
:type p_gain: float
:param d_gain: Derivative gain used by the parent control law.
:type d_gain: float
:param gamma: Weighting scalar for the spin-axis residual penalty.
:type gamma: float
:param c_gain: Additional gain used by the parent control law.
:type c_gain: float
:param h_target: Target wheel momentum vector used for momentum management.
:type h_target: numpy.ndarray | list
:return: None.
:rtype: NoneType
"""
self.gamma = gamma
super().__init__(est_sat=est_sat, p_gain=p_gain, d_gain=d_gain, c_gain=c_gain, h_target=h_target)
[docs]
def find_u(
self,
x_hat: np.ndarray,
sens: np.ndarray,
est_sat: EstimatedSatellite,
os_hat: Orbital_State,
goal: Goal | None = None,
) -> np.ndarray:
r"""
Compute actuator commands for detumble or goal-tracking operation.
If ``goal`` is :class:`~ADCS.CONOPS.goals.No_Goal`, the method computes a detumble and
momentum-management command based on body rates and wheel momentum estimates:
- The geomagnetic field is estimated in the body frame from sensor measurements using the
MTM readout model in the parent class.
- A rate damping torque is computed in the plane perpendicular to the field direction
because MTQs cannot generate torque parallel to :math:`\boldsymbol{B}`.
- A momentum dumping torque is computed from the error between estimated system wheel
momentum and the target vector ``h_target`` and projected into the MTQ-achievable plane.
- MTQ commands are computed and scaled to satisfy saturation. The scaling factor
:math:`\alpha_{\mathrm{mtq}}` is used to scale RW assistance to avoid net spin-up when
MTQs cannot realize the intended damping and dumping torque.
If ``goal`` is not :class:`~ADCS.CONOPS.goals.No_Goal`, the method computes a PD-style
desired torque using pointing error from
:meth:`~ADCS.CONOPS.goals.Goal.error` and rate error relative to the reference generated by
:meth:`~ADCS.CONOPS.goals.Goal.to_ref`. The quaternion alignment error is computed using
:func:`~ADCS.controller.helpers.quaternion_math.vector_alignment_error`. A gyroscopic term
based on the spacecraft inertia and wheel momentum is added, producing the final desired
torque :math:`\boldsymbol{\tau}_{\mathrm{des}}`. This desired torque is allocated using
:meth:`~ADCS.controller.MTQ_w_RW_QPG.allocate_max_torque_in_direction`, which applies the
gyroscopically weighted least-squares metric.
:param x_hat: Estimated state vector containing angular rate, attitude quaternion, and
optionally wheel momentum states.
:type x_hat: numpy.ndarray
:param sens: Sensor measurement vector used to estimate body magnetic field through the
MTM readout model.
:type sens: numpy.ndarray
:param est_sat: Estimated satellite model providing actuators, inertia, and boresight.
:type est_sat: :class:`~ADCS.satellite_hardware.satellite.estimated_satellite.EstimatedSatellite`
:param os_hat: Estimated orbital state used by goal reference generation and error models.
:type os_hat: :class:`~ADCS.orbits.orbital_state.Orbital_State`
:param goal: Optional goal object providing desired pointing and reference rates.
:type goal: :class:`~ADCS.CONOPS.goals.Goal` | None
:return: Command vector for all actuators in the order used by ``est_sat.actuators``.
:rtype: numpy.ndarray
"""
if goal is None:
goal = No_Goal()
w = x_hat[0:3]
q = x_hat[3:7]
if isinstance(goal, No_Goal):
k_w = self.d_gain
k_h = self.c_gain
# --- 1. Magnetic Field (Body Frame) ---
b_body = np.asarray(self.M_mtm_read @ sens, float).reshape(3,)
b_norm = np.linalg.norm(b_body)
if b_norm < 1e-9:
return np.zeros(self.n_actuators)
b_hat = b_body / b_norm
# --- 2. System Momentum Vector (Generic N-RW) ---
# Calculate total vector momentum stored in all wheels
if self.n_rw > 0 and len(x_hat) >= 7 + self.n_rw:
h_rw_scalars = x_hat[7 : 7 + self.n_rw]
h_sys = self.rw_axes @ h_rw_scalars # Matrix (3, N) @ Vec (N,) -> (3,)
else:
h_sys = np.zeros(3)
# --- 3. Rate Damping (Perpendicular to B) ---
# "Magnetic B-dot" logic: dampen rates only in the plane where MTQs can act
w_perp = w - np.dot(w, b_hat) * b_hat
tau_bdot_perp = -k_w * w_perp
# --- 4. Momentum Dumping Torque (3D Vector) ---
# Desired torque on body to reduce system momentum error
h_err = h_sys - self.h_target # Vector subtraction
tau_dump_des = k_h * h_err # Gain * Vector error
# Saturation: Limit the dumping torque magnitude to avoid transient spikes
mag = np.linalg.norm(tau_dump_des)
limit_val = k_h * 0.001 # Example cap (adjust as needed)
if mag > limit_val:
tau_dump_des *= limit_val / (mag + 1e-12)
# Project dump torque into MTQ-achievable plane (Perp to B)
tau_dump_perp = tau_dump_des - np.dot(tau_dump_des, b_hat) * b_hat
# Gating: Avoid dumping if the required torque is parallel to B (uncancellable)
denom = np.linalg.norm(tau_dump_des) + 1e-12
gamma = np.linalg.norm(tau_dump_perp) / denom
tau_dump_cmd = gamma * tau_dump_des
# --- 5. MTQ Allocation & Saturation Check ---
tau_mtq_des = tau_bdot_perp - tau_dump_perp
alpha_mtq = 0.0 # Default to 0: If no MTQs, we cannot dump.
u_mtq_scaled = np.zeros(self.n_mtq) # Default empty/zero
if self.n_mtq > 0:
# Standard Allocation
B_skew = skewsym(b_body)
M_mag_eff = -B_skew @ self.A_mtq
u_mtq_raw = np.linalg.pinv(M_mag_eff) @ tau_mtq_des
# Check saturation
mtq_cmds = u_mtq_raw[self.mtq_indices]
alpha_mtq = 1.0
if np.any(np.abs(mtq_cmds) > self.mtq_umax):
alpha_mtq = np.min(self.mtq_umax / (np.abs(mtq_cmds) + 1e-12))
u_mtq_scaled = alpha_mtq * u_mtq_raw
# --- 6. RW Command (Scaled) ---
# If alpha_mtq is 0 (due to saturation or 0 MTQs), RW torque is reduced
# to 0 to prevent spinning up the body.
tau_rw_req = alpha_mtq * (tau_dump_cmd + tau_bdot_perp)
u_rw = self.M_rw_act @ tau_rw_req
u_rw = np.clip(u_rw, -self.rw_umax, self.rw_umax)
# --- Final Output ---
u_out = u_mtq_scaled + u_rw
else:
n_rw = len([a for a in est_sat.actuators if isinstance(a, RW)])
if len(x_hat) >= 7 + n_rw:
h_rw_states = x_hat[7 : 7 + n_rw]
else:
h_rw_states = np.array([rw.h for rw in est_sat.actuators if isinstance(rw, RW)])
goal_vec_eci, w_ref_eci = goal.to_ref(os0=os_hat)
R_b2i = rot_mat(q)
w_ref_body = R_b2i.T @ w_ref_eci
boresight = est_sat.get_boresight(goal.boresight_name)
q_err = goal.error(q=q, body_boresight=boresight, os0=os_hat)
w_err = w - w_ref_body
tau_pd = -self.p_gain * q_err - self.d_gain * w_err
h_rw_body = np.zeros(3)
rw_counter = 0
for actuator in est_sat.actuators:
if isinstance(actuator, RW):
h_rw_body += np.asarray(actuator.axis).flatten() * h_rw_states[rw_counter]
rw_counter += 1
J = est_sat.J_0
tau_gyro = np.cross(w, J @ w + h_rw_body)
tau_des = tau_pd + tau_gyro
b_body = np.asarray(self.M_mtm_read @ sens, float).reshape(3,)
u_rw_cmd, u_mtq_cmd, alpha = self.allocate_max_torque_in_direction(
tau_des, b_body, est_sat, w
)
u_out = np.zeros(len(est_sat.actuators))
rw_indices = [i for i, a in enumerate(est_sat.actuators) if isinstance(a, RW)]
u_out[rw_indices] = u_rw_cmd
mtq_indices = [i for i, a in enumerate(est_sat.actuators) if isinstance(a, MTQ)]
u_out[mtq_indices] = u_mtq_cmd
# self.plot_torques(tau_des, b_body, est_sat)
return u_out
[docs]
def allocate_max_torque_in_direction(self, tau_des: np.ndarray, b_body: np.ndarray, est_sat: EstimatedSatellite, omega: np.ndarray) -> tuple[np.ndarray, np.ndarray, float]:
r"""
Allocate bounded RW and MTQ commands using a gyroscopically weighted least-squares metric.
The method builds the combined actuator influence matrix
.. math::
A_{\mathrm{tot}}
=
\begin{bmatrix}
A_{\mathrm{rw}} & -[\boldsymbol{B}]_{\times} A_{\mathrm{mtq}}
\end{bmatrix}
using wheel axes from :class:`~ADCS.satellite_hardware.actuators.RW` and MTQ axes from
:class:`~ADCS.satellite_hardware.actuators.MTQ` provided by
:class:`~ADCS.satellite_hardware.satellite.estimated_satellite.EstimatedSatellite`.
The residual torque is
.. math::
\boldsymbol{r}(\boldsymbol{u})
=
A_{\mathrm{tot}}\boldsymbol{u} - \boldsymbol{\tau}_{\mathrm{des}}.
The weighting matrix is defined (when :math:`\|\boldsymbol{\omega}\| > 0`) as
.. math::
W(\boldsymbol{\omega})
=
I_{3 \times 3}
+
\gamma
\frac{\boldsymbol{\omega}\boldsymbol{\omega}^{\mathsf{T}}}{\boldsymbol{\omega}^{\mathsf{T}}\boldsymbol{\omega}}.
The optimization problem is
.. math::
\min_{\boldsymbol{u}}
\;\left\|W(\boldsymbol{\omega})\boldsymbol{r}(\boldsymbol{u})\right\|_2^2
subject to box constraints
.. math::
-u_{i,\max} \le u_i \le u_{i,\max}, \qquad \forall i.
This is implemented by solving an equivalent bounded least-squares problem with
.. math::
A_w = W(\boldsymbol{\omega})A_{\mathrm{tot}},
\qquad
\boldsymbol{b}_w = W(\boldsymbol{\omega})\boldsymbol{\tau}_{\mathrm{des}}.
The solver is :meth:`scipy.optimize.lsq_linear` with the Trust Region Reflective method.
After obtaining :math:`\boldsymbol{u}^*`, the achieved torque is
:math:`\boldsymbol{\tau}_{\mathrm{ach}} = A_{\mathrm{tot}}\boldsymbol{u}^*`. The method
returns an effectiveness scalar
.. math::
\hat{\boldsymbol{\tau}} = \frac{\boldsymbol{\tau}_{\mathrm{des}}}{\|\boldsymbol{\tau}_{\mathrm{des}}\|},
\qquad
\alpha = \max\!\left(0,\frac{\boldsymbol{\tau}_{\mathrm{ach}}\cdot\hat{\boldsymbol{\tau}}}{\|\boldsymbol{\tau}_{\mathrm{des}}\|}\right).
:param tau_des: Desired body-frame torque vector :math:`\boldsymbol{\tau}_{\mathrm{des}}` in N·m.
:type tau_des: numpy.ndarray
:param b_body: Body-frame geomagnetic field vector :math:`\boldsymbol{B}` in tesla.
:type b_body: numpy.ndarray
:param est_sat: Estimated satellite model providing actuator instances and limits.
:type est_sat: :class:`~ADCS.satellite_hardware.satellite.estimated_satellite.EstimatedSatellite`
:param omega: Body angular velocity vector :math:`\boldsymbol{\omega}` in rad/s used to form the weighting matrix.
:type omega: numpy.ndarray
:return: Tuple of RW commands, MTQ commands, and effectiveness scalar :math:`\alpha`.
:rtype: tuple[numpy.ndarray, numpy.ndarray, float]
"""
tau_des = np.asarray(tau_des, float).reshape(3,)
t_mag = np.linalg.norm(tau_des)
if t_mag < 1e-9:
n_rw = len([a for a in est_sat.actuators if isinstance(a, RW)])
n_mtq = len([a for a in est_sat.actuators if isinstance(a, MTQ)])
return np.zeros(n_rw), np.zeros(n_mtq), 1.0
# 1) Setup matrices exactly like before
rws = [a for a in est_sat.actuators if isinstance(a, RW)]
mtqs = [a for a in est_sat.actuators if isinstance(a, MTQ)]
if rws:
A_rw = np.column_stack([np.asarray(rw.axis, float).reshape(3,) for rw in rws])
u_rw_lims = np.array([rw.u_max for rw in rws], dtype=float)
else:
A_rw = np.zeros((3, 0))
u_rw_lims = np.zeros(0, dtype=float)
if mtqs:
b_skew = -skewsym(b_body)
A_mtq_axes = np.column_stack([np.asarray(m.axis, float).reshape(3,) for m in mtqs])
A_mtq = b_skew @ A_mtq_axes
u_mtq_lims = np.array([m.u_max for m in mtqs], dtype=float)
else:
A_mtq = np.zeros((3, 0))
u_mtq_lims = np.zeros(0, dtype=float)
A_total = np.hstack([A_rw, A_mtq]) # (3, n_act)
n_act = A_total.shape[1]
n_rw = len(rws)
n_mtq = len(mtqs)
if n_act == 0:
return np.zeros(n_rw), np.zeros(n_mtq), 0.0
# 2) Bounds: [-u_max, +u_max]
lb = np.concatenate([-u_rw_lims, -u_mtq_lims]) if n_act else np.zeros(0)
ub = np.concatenate([ u_rw_lims, u_mtq_lims]) if n_act else np.zeros(0)
# -----------------------------
# NEW: Weighting W(omega)
# -----------------------------
omega = np.asarray(omega, float).reshape(3,)
om2 = float(np.dot(omega, omega))
gamma = float(getattr(self, "gamma", 0.0)) # uses self.gamma; defaults to 0 if absent
if om2 < 1e-12 or abs(gamma) < 1e-15:
W = np.eye(3)
else:
W = np.eye(3) + gamma * (np.outer(omega, omega) / om2)
# Weighted LSQ: minimize || W (A_total u - tau_des) ||^2
A_w = W @ A_total
b_w = W @ tau_des
res = lsq_linear(A_w, b_w, bounds=(lb, ub), method="trf")
if not res.success:
return np.zeros(n_rw), np.zeros(n_mtq), 0.0
u_sol = res.x
# 4) Compute alpha as before
tau_ach = A_total @ u_sol
tau_hat = tau_des / (t_mag + 1e-12)
T_along = float(np.dot(tau_ach, tau_hat))
alpha = max(0.0, T_along / (t_mag + 1e-12))
u_rw_cmd = u_sol[:n_rw]
u_mtq_cmd = u_sol[n_rw:n_rw + n_mtq]
return u_rw_cmd, u_mtq_cmd, alpha