__all__ = ["MTQ_w_RW_QPW"]
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_QPW(MTQ_w_RW_LP):
r"""
Weighted Quadratic–Programming Torque Allocation for Mixed RW–MTQ ADCS.
This controller implements a weighted bounded least-squares (WLS) torque allocator for a
spacecraft with reaction wheels (RWs) and magnetorquers (MTQs). It is designed to behave
between a strict direction-prioritizing allocator and a pure Euclidean-error allocator by
penalizing torque direction error more strongly than torque magnitude error.
The class inherits from :class:`~ADCS.controller.MTQ_w_RW_LP` and solves a box-constrained
weighted least-squares problem 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}.
Direction-Weighted Error Decomposition
--------------------------------------
Let the unit desired torque direction be
.. math::
\hat{\boldsymbol{\tau}} = \frac{\boldsymbol{\tau}_{\mathrm{des}}}{\|\boldsymbol{\tau}_{\mathrm{des}}\|}.
Define projection operators onto the parallel and perpendicular subspaces:
.. math::
P_{\parallel} = \hat{\boldsymbol{\tau}}\hat{\boldsymbol{\tau}}^{\mathsf{T}},
\qquad
P_{\perp} = I_{3 \times 3} - P_{\parallel}.
For a torque residual
.. math::
\boldsymbol{e}(\boldsymbol{u}) = A_{\mathrm{tot}}\boldsymbol{u} - \boldsymbol{\tau}_{\mathrm{des}},
the parallel and perpendicular components are
.. math::
\boldsymbol{e}_{\parallel} = P_{\parallel}\boldsymbol{e},
\qquad
\boldsymbol{e}_{\perp} = P_{\perp}\boldsymbol{e}.
Weighted Bounded Least Squares Allocation
-----------------------------------------
Choose weights :math:`w_{\parallel} > 0` and :math:`w_{\perp} > 0` with
:math:`w_{\perp} \gg w_{\parallel}` to penalize directional error more strongly. The weighted
objective is
.. math::
\min_{\boldsymbol{u}}
\;w_{\parallel}\|\boldsymbol{e}_{\parallel}(\boldsymbol{u})\|_2^2
+
w_{\perp}\|\boldsymbol{e}_{\perp}(\boldsymbol{u})\|_2^2.
This can be written as a standard weighted least-squares norm by defining the square-root
weighting matrix
.. math::
W = \sqrt{w_{\parallel}}\,P_{\parallel} + \sqrt{w_{\perp}}\,P_{\perp},
which yields the equivalent problem
.. math::
\min_{\boldsymbol{u}}
\;\left\|W\left(A_{\mathrm{tot}}\boldsymbol{u} - \boldsymbol{\tau}_{\mathrm{des}}\right)\right\|_2^2.
The actuator saturation bounds are enforced as
.. math::
-u_{i,\max} \le u_i \le u_{i,\max}, \qquad \forall i.
Implementation Parameters
-------------------------
The default configuration uses :math:`w_{\parallel}=1` and :math:`w_{\perp}=100`. This biases
the solution to align the achieved torque direction with :math:`\boldsymbol{\tau}_{\mathrm{des}}`
even when exact magnitude tracking is infeasible due to saturation or MTQ underactuation.
Effectiveness Metric
--------------------
After solving for :math:`\boldsymbol{u}^*`, the achieved torque is
:math:`\boldsymbol{\tau}_{\mathrm{ach}} = A_{\mathrm{tot}}\boldsymbol{u}^*`. The method reports
an effectiveness scalar computed by projecting :math:`\boldsymbol{\tau}_{\mathrm{ach}}` onto the
desired direction:
.. math::
\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 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, c_gain: float, h_target: np.ndarray | list = np.zeros(3)) -> None:
r"""
Initialize the direction-weighted QP torque allocator.
This initializer delegates base controller initialization to
:class:`~ADCS.controller.MTQ_w_RW_LP`. The direction-weighted allocation behavior is
implemented in :meth:`~ADCS.controller.MTQ_w_RW_QPW.allocate_max_torque_in_direction`.
: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 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
"""
super().__init__(est_sat=est_sat, p_gain=p_gain, d_gain=d_gain, c_gain=c_gain, h_target=h_target)
[docs]
def allocate_max_torque_in_direction(self, tau_des: np.ndarray, b_body: np.ndarray, est_sat: EstimatedSatellite) -> tuple[np.ndarray, np.ndarray, float]:
r"""
Allocate bounded RW and MTQ commands using direction-weighted least squares.
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}
from :class:`~ADCS.satellite_hardware.actuators.RW` and
:class:`~ADCS.satellite_hardware.actuators.MTQ` axes provided by
:class:`~ADCS.satellite_hardware.satellite.estimated_satellite.EstimatedSatellite`.
Let the torque residual be
.. math::
\boldsymbol{e}(\boldsymbol{u}) = A_{\mathrm{tot}}\boldsymbol{u} - \boldsymbol{\tau}_{\mathrm{des}}.
Using the unit desired direction :math:`\hat{\boldsymbol{\tau}}` and projection operators
.. math::
P_{\parallel} = \hat{\boldsymbol{\tau}}\hat{\boldsymbol{\tau}}^{\mathsf{T}},
\qquad
P_{\perp} = I_{3 \times 3} - P_{\parallel},
define the square-root weighting matrix
.. math::
W = \sqrt{w_{\parallel}}\,P_{\parallel} + \sqrt{w_{\perp}}\,P_{\perp}.
The allocation solves
.. math::
\min_{\boldsymbol{u}}
\;\left\|W\left(A_{\mathrm{tot}}\boldsymbol{u} - \boldsymbol{\tau}_{\mathrm{des}}\right)\right\|_2^2
subject to actuator bounds
.. math::
-u_{i,\max} \le u_i \le u_{i,\max}, \qquad \forall i.
The implementation uses :math:`w_{\parallel}=1` and :math:`w_{\perp}=100` and solves the
equivalent bounded least-squares problem
.. math::
\min_{\boldsymbol{u}}
\;\left\|(WA_{\mathrm{tot}})\boldsymbol{u} - W\boldsymbol{\tau}_{\mathrm{des}}\right\|_2^2
via :meth:`scipy.optimize.lsq_linear`.
The achieved torque is :math:`\boldsymbol{\tau}_{\mathrm{ach}} = A_{\mathrm{tot}}\boldsymbol{u}^*`.
An effectiveness scalar is computed as
.. 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`
: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)]
# RW torque map: tau_rw = A_rw * u_rw
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)
# MTQ torque map: tau_mtq = ( -skew(B) * A_axes ) * u_mtq
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)
tau_hat = tau_des / (t_mag + 1e-12)
# Projection operators
P_par = np.outer(tau_hat, tau_hat)
P_perp = np.eye(3) - P_par
# Weights (tune w_perp >> w_par)
w_par = 1.0
w_perp = 100.0
# Weighting matrix (square root, since lsq minimizes ||Ax-b||^2)
W = np.sqrt(w_par) * P_par + np.sqrt(w_perp) * P_perp
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:
# fall back: best effort (still keep shape)
return np.zeros(n_rw), np.zeros(n_mtq), 0.0
u_sol = res.x # length n_act
# 4) Compute alpha as "how much of tau_des magnitude you achieved along its direction"
tau_ach = A_total @ u_sol
tau_hat = tau_des / (t_mag + 1e-12)
T_along = float(np.dot(tau_ach, tau_hat)) # signed magnitude along desired direction
alpha = max(0.0, T_along / (t_mag + 1e-12)) # normalize by requested magnitude, clamp to [0, inf)
# Split commands
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