Aerodynamic drag force on Drag_Disturbance (second of the force stack)

ADCS/satellite_hardware/disturbances/drag_disturbance.py

@@ -35,6 +35,13 @@ def _drag_torque_kernel(V_B, rho, COM, normals, areas, centroids, CDs):
     out[2] = ct * (wx*V_B[1] - wy*V_B[0])
     return out
 
+
+def _drag_force_kernel(V_B, rho, normals, areas, CDs):
+    """Aerodynamic drag force summed over geometry faces, in body coordinates."""
+    cos_alpha = np.maximum(0.0, normals @ V_B)
+    face_factors = CDs * areas * cos_alpha
+    return -0.5 * rho * np.sum(face_factors) * V_B
+
 if TYPE_CHECKING:
     from ADCS.satellite_hardware.satellite.satellite import Satellite
 
@@ -242,6 +249,155 @@ def torque(self, sat: Satellite, x: State, os: Orbital_State | Dict[str, np.ndar
 
         return _drag_torque_kernel(V_B, rho, sat.COM,
                                    self.normals, self.areas, self.centroids, self.CDs)
+
+    def force(self, sat: Satellite, x: State, os: Orbital_State) -> np.ndarray:
+        r"""Compute the aerodynamic drag force in the body frame.
+
+        The orbital-state interface supplies the relative atmospheric velocity
+        and density through ``os.get_state_vector(x=x)``:
+
+        - ``"v"``: body-frame relative velocity in km/s
+        - ``"rho"``: atmospheric mass density in kg/m³
+
+        Velocity is converted to m/s. For each face, the clipped incidence and
+        force contribution are:
+
+        .. math::
+
+            s_i = \max(0, \mathbf{n}_i^\top\mathbf{V}_b),
+            \qquad
+            \mathbf{F}_i = -\tfrac{1}{2}\rho C_{D,i} A_i s_i\mathbf{V}_b.
+
+        The total force is :math:`\sum_i \mathbf{F}_i` and has units N. This
+        returns force rather than acceleration; divide by spacecraft mass when
+        an acceleration is required.
+
+        The incidence clipping is piecewise, with the force contribution from
+        a face set to zero when its normal points away from the incoming flow.
+
+        :param sat: Satellite instance (accepted for consistency with the torque API).
+        :type sat: :class:`~ADCS.satellite_hardware.satellite.satellite.Satellite`
+        :param x: Spacecraft state containing the attitude quaternion used to
+            express the flow in body coordinates.
+        :type x: :class:`~ADCS.state.State`
+        :param os: Orbital state providing relative velocity and atmospheric density.
+        :type os: :class:`~ADCS.orbits.orbital_state.Orbital_State`
+        :return: Total aerodynamic force in body coordinates, shape ``(3,)`` [N].
+        :rtype: :class:`numpy.ndarray`
+        """
+        vecs = os.get_state_vector(x=x)
+        V_B = np.asarray(vecs["v"], dtype=float).reshape(3) * 1000.0
+        rho = float(vecs["rho"])
+        return _drag_force_kernel(V_B, rho, self.normals, self.areas, self.CDs)
+
+    def force_qjac(self, sat: Satellite, x: State, os: Orbital_State) -> np.ndarray:
+        r"""Compute the Jacobian of drag force with respect to attitude quaternion.
+
+        The orbital state supplies body-frame velocity ``"v"`` [km/s], density
+        ``"rho"`` [kg/m³], and its quaternion derivative ``"dv"`` through
+        :meth:`~ADCS.orbits.orbital_state.Orbital_State.get_state_vector`.
+        Velocity and its derivative are converted to m/s before evaluating drag.
+
+        For face incidence
+
+        .. math::
+
+            s_i = \max(0, \mathbf{n}_i^\top\mathbf{V}_b),
+
+        the piecewise derivative is gated by whether the face is exposed to the
+        flow. Defining
+
+        .. math::
+
+            C = \sum_i C_{D,i}A_i s_i,
+            \qquad \mathbf{F}_{drag}=-\tfrac{1}{2}\rho C\mathbf{V}_b,
+
+        the Jacobian applies the product rule to both :math:`C` and
+        :math:`\mathbf{V}_b`. Incidence-boundary distribution terms are omitted.
+
+        The returned tensor uses quaternion component first and force component
+        last, shape ``(4, 3)``. Its units are N per quaternion component.
+
+        :param sat: Satellite instance (accepted for consistency with the torque API).
+        :type sat: :class:`~ADCS.satellite_hardware.satellite.satellite.Satellite`
+        :param x: Spacecraft state containing the attitude quaternion.
+        :type x: :class:`~ADCS.state.State`
+        :param os: Orbital state supplying ``"v"``, ``"rho"``, and ``"dv"``.
+        :type os: :class:`~ADCS.orbits.orbital_state.Orbital_State`
+        :return: Quaternion Jacobian :math:`\partial\mathbf{F}_{drag}/\partial\mathbf{q}`, shape ``(4, 3)``.
+        :rtype: :class:`numpy.ndarray`
+        """
+        vecs = os.get_state_vector(x=x)
+        V_B = np.asarray(vecs["v"], dtype=float).reshape(3) * 1000.0
+        rho = float(vecs["rho"])
+        dV__dq = np.asarray(vecs["dv"], dtype=float).reshape(4, 3) * 1000.0
+
+        incidence = self.normals @ V_B
+        face_lit = incidence > 0.0
+        weights = self.CDs * self.areas
+        face_factors = weights * np.maximum(incidence, 0.0)
+        total_factor = float(np.sum(face_factors))
+
+        # d(sum_i CD_i A_i max(n_i . V, 0))/dq, shape (4,).
+        dtotal_factor__dq = (dV__dq @ self.normals.T) @ (weights * face_lit)
+        return -0.5 * rho * (
+            dtotal_factor__dq[:, np.newaxis] * V_B[np.newaxis, :]
+            + total_factor * dV__dq
+        )
+
+    def force_qqhess(self, sat: Satellite, x: State, os: Orbital_State) -> np.ndarray:
+        r"""Compute the Hessian of drag force with respect to attitude quaternion.
+
+        The orbital state supplies body-frame velocity ``"v"`` [km/s], density
+        ``"rho"`` [kg/m³], and its first and second quaternion derivatives
+        ``"dv"`` and ``"ddv"``. Velocity and both derivatives are converted to
+        m/s, consistent with :meth:`force`.
+
+        Let :math:`C=\sum_i C_{D,i}A_i\max(0,\mathbf{n}_i^\top\mathbf{V}_b)`.
+        The force is :math:`-\tfrac{1}{2}\rho C\mathbf{V}_b`; its Hessian
+        contains the second derivative of :math:`C`, both mixed first-derivative
+        terms, and the second derivative of velocity. Derivatives of the clipped
+        face incidence are evaluated piecewise: exposed faces contribute and
+        wake-side faces contribute zero. Distributional terms at zero incidence
+        are neglected.
+
+        The returned tensor is ordered ``(q_i, q_j, force_component)`` with
+        shape ``(4, 4, 3)`` and units N per pair of quaternion components.
+
+        :param sat: Satellite instance (accepted for consistency with the torque API).
+        :type sat: :class:`~ADCS.satellite_hardware.satellite.satellite.Satellite`
+        :param x: Spacecraft state containing the attitude quaternion.
+        :type x: :class:`~ADCS.state.State`
+        :param os: Orbital state supplying ``"v"``, ``"rho"``, ``"dv"``, and ``"ddv"``.
+        :type os: :class:`~ADCS.orbits.orbital_state.Orbital_State`
+        :return: Quaternion Hessian :math:`\partial^2\mathbf{F}_{drag}/\partial\mathbf{q}^2`, shape ``(4, 4, 3)``.
+        :rtype: :class:`numpy.ndarray`
+        """
+        vecs = os.get_state_vector(x=x)
+        V_B = np.asarray(vecs["v"], dtype=float).reshape(3) * 1000.0
+        rho = float(vecs["rho"])
+        dV__dq = np.asarray(vecs["dv"], dtype=float).reshape(4, 3) * 1000.0
+        ddV__dqdq = np.asarray(vecs["ddv"], dtype=float).reshape(4, 4, 3) * 1000.0
+
+        incidence = self.normals @ V_B
+        face_lit = incidence > 0.0
+        weights = self.CDs * self.areas
+        face_factors = weights * np.maximum(incidence, 0.0)
+        total_factor = float(np.sum(face_factors))
+
+        dincidence__dq = dV__dq @ self.normals.T  # (4, Nfaces)
+        ddincidence__dqdq = np.einsum("ija,na->ijn", ddV__dqdq, self.normals)
+        dtotal_factor__dq = dincidence__dq @ (weights * face_lit)
+        ddtotal_factor__dqdq = np.einsum(
+            "ijn,n->ij", ddincidence__dqdq, weights * face_lit
+        )
+
+        return -0.5 * rho * (
+            ddtotal_factor__dqdq[:, :, np.newaxis] * V_B[np.newaxis, np.newaxis, :]
+            + dtotal_factor__dq[:, np.newaxis, np.newaxis] * dV__dq[np.newaxis, :, :]
+            + dtotal_factor__dq[np.newaxis, :, np.newaxis] * dV__dq[:, np.newaxis, :]
+            + total_factor * ddV__dqdq
+        )
 
     def torque_qjac(self, sat: Satellite, x: State, os: Orbital_State) -> np.ndarray:
         r"""

testing/test_disturbances/_helpers.py

@@ -117,8 +117,12 @@ def fd_quat_jac_raw(fun, x: State, eps: float = 1.0e-7) -> np.ndarray:
     for index in range(4):
         delta = np.zeros(4)
         delta[index] = eps
-        plus = fun(State(w=x.w, q=x.q + delta))
-        minus = fun(State(w=x.w, q=x.q - delta))
+        x_plus = x.copy()
+        x_minus = x.copy()
+        x_plus.q = x.q + delta
+        x_minus.q = x.q - delta
+        plus = fun(x_plus)
+        minus = fun(x_minus)
         jac[index, :] = (plus - minus) / (2.0 * eps)
     return jac
 
@@ -129,7 +133,11 @@ def fd_quat_hess_raw(fun, x: State, eps: float = 1.0e-5) -> np.ndarray:
     for index in range(4):
         delta = np.zeros(4)
         delta[index] = eps
-        plus = fd_quat_jac_raw(fun, State(w=x.w, q=x.q + delta), eps=eps)
-        minus = fd_quat_jac_raw(fun, State(w=x.w, q=x.q - delta), eps=eps)
+        x_plus = x.copy()
+        x_minus = x.copy()
+        x_plus.q = x.q + delta
+        x_minus.q = x.q - delta
+        plus = fd_quat_jac_raw(fun, x_plus, eps=eps)
+        minus = fd_quat_jac_raw(fun, x_minus, eps=eps)
         hess[index, :, :] = (plus - minus) / (2.0 * eps)
     return hess

testing/test_disturbances/drag/test_drag_force.py

@@ -0,0 +1,147 @@
+"""Aerodynamic drag force (the torque's companion).
+
+The force is the per-face flat-plate drag summed without lever arms, so the
+torque must equal the sum of (centroid - COM) x (per-face force).  That
+consistency check ties the new force to the already-tested torque.
+"""
+
+import numpy as np
+
+from ADCS.satellite_hardware.disturbances import Drag_Disturbance, GeometryFace
+from testing.test_disturbances._helpers import (
+    fd_quat_hess_raw,
+    fd_quat_jac_raw,
+    make_geometry_config,
+    make_orbital_state,
+    make_satellite,
+    make_state,
+)
+
+
+def per_face_drag_forces(drag: Drag_Disturbance, x, os) -> np.ndarray:
+    vecs = os.get_state_vector(x=x)
+    velocity = vecs["v"] * 1000.0
+    rho = vecs["rho"]
+    face_factors = np.maximum(0.0, drag.normals @ velocity) * drag.areas * drag.CDs
+    return -0.5 * rho * face_factors[:, None] * velocity[None, :]
+
+
+def test_drag_force_single_face_matches_closed_form():
+    drag = Drag_Disturbance(
+        make_geometry_config(
+            GeometryFace(area=2.5, centroid=np.array([0.1, 0.3, -0.2]), normal=np.array([0.0, 1.0, 0.0]), CD=1.7)
+        )
+    )
+    sat = make_satellite(disturbances=[drag])
+    x = make_state()
+    os = make_orbital_state(V=np.array([0.0, 7.4, 0.0]), rho=4.0e-12)
+
+    force = drag.force(sat=sat, x=x, os=os)
+
+    speed = 7400.0
+    expected = -0.5 * 4.0e-12 * 1.7 * 2.5 * speed**2 * np.array([0.0, 1.0, 0.0])
+    assert np.allclose(force, expected)
+    assert force @ np.array([0.0, 1.0, 0.0]) < 0.0  # opposes the velocity
+
+
+def test_drag_force_zero_density_is_zero():
+    drag = Drag_Disturbance(
+        make_geometry_config(
+            GeometryFace(area=2.0, centroid=np.zeros(3), normal=np.array([0.0, 1.0, 0.0]), CD=1.4)
+        )
+    )
+    sat = make_satellite(disturbances=[drag])
+    os = make_orbital_state(V=np.array([0.2, 7.5, -0.1]), rho=0.0)
+
+    assert np.allclose(drag.force(sat=sat, x=make_state(), os=os), np.zeros(3))
+
+
+def test_drag_force_back_facing_face_contributes_zero():
+    drag = Drag_Disturbance(
+        make_geometry_config(
+            GeometryFace(area=2.0, centroid=np.zeros(3), normal=np.array([0.0, -1.0, 0.0]), CD=1.4)
+        )
+    )
+    sat = make_satellite(disturbances=[drag])
+    os = make_orbital_state(V=np.array([0.1, 7.5, 0.2]), rho=3.0e-12)
+
+    assert np.allclose(drag.force(sat=sat, x=make_state(), os=os), np.zeros(3))
+
+
+def test_drag_force_opposing_faces_only_count_the_ram_face():
+    ram = GeometryFace(area=2.0, centroid=np.array([0.0, 0.2, 0.0]), normal=np.array([0.0, 1.0, 0.0]), CD=2.2)
+    wake = GeometryFace(area=2.0, centroid=np.array([0.0, -0.2, 0.0]), normal=np.array([0.0, -1.0, 0.0]), CD=2.2)
+    both = Drag_Disturbance(make_geometry_config(ram, wake))
+    ram_only = Drag_Disturbance(make_geometry_config(ram))
+    sat = make_satellite(disturbances=[both])
+    x = make_state(q=np.array([0.9, 0.1, -0.2, 0.3]))
+    os = make_orbital_state(V=np.array([0.3, 7.4, -0.5]), rho=4.0e-12)
+
+    assert np.allclose(both.force(sat=sat, x=x, os=os), ram_only.force(sat=sat, x=x, os=os))
+
+
+def test_drag_torque_is_lever_arm_cross_per_face_force():
+    drag = Drag_Disturbance(
+        make_geometry_config(
+            GeometryFace(area=2.5, centroid=np.array([0.1, 0.3, -0.2]), normal=np.array([0.0, 1.0, 0.0]), CD=1.7),
+            GeometryFace(area=1.0, centroid=np.array([-0.3, 0.0, 0.1]), normal=np.array([1.0, 1.0, 0.0]), CD=2.0),
+        )
+    )
+    sat = make_satellite(COM=np.array([0.02, -0.01, 0.04]), disturbances=[drag])
+    x = make_state(q=np.array([0.7, -0.2, 0.3, 0.1]))
+    os = make_orbital_state(V=np.array([0.2, 7.4, -0.1]), rho=4.0e-12)
+
+    per_face = per_face_drag_forces(drag, x, os)
+    expected_torque = np.sum(np.cross(drag.centroids - sat.COM, per_face), axis=0)
+
+    assert np.allclose(drag.force(sat=sat, x=x, os=os), per_face.sum(axis=0))
+    assert np.allclose(drag.torque(sat=sat, x=x, os=os), expected_torque)
+
+
+def test_drag_force_qjac_matches_finite_difference():
+    drag = Drag_Disturbance(
+        make_geometry_config(
+            GeometryFace(area=2.0, centroid=np.array([0.0, 0.3, 0.0]), normal=np.array([0.0, 2.0, 0.0]), CD=1.4),
+            GeometryFace(area=0.5, centroid=np.array([0.2, 0.0, 0.0]), normal=np.array([1.0, 0.0, 0.5]), CD=2.0),
+        )
+    )
+    sat = make_satellite(disturbances=[drag])
+    x = make_state(q=np.array([0.6, 0.2, -0.1, 0.4]))
+    os = make_orbital_state(V=np.array([0.01, 7.5, 0.2]), rho=3.0e-12)
+    expected = fd_quat_jac_raw(lambda xx: drag.force(sat=sat, x=xx, os=os), x)
+
+    jacobian = drag.force_qjac(sat=sat, x=x, os=os)
+
+    assert jacobian.shape == (4, 3)
+    assert np.allclose(jacobian, expected, atol=1.0e-9, rtol=1.0e-6)
+
+
+def test_drag_force_qqhess_matches_finite_difference():
+    drag = Drag_Disturbance(
+        make_geometry_config(
+            GeometryFace(area=2.0, centroid=np.array([0.0, 0.3, 0.0]), normal=np.array([0.0, 2.0, 0.0]), CD=1.4),
+            GeometryFace(area=0.5, centroid=np.array([0.2, 0.0, 0.0]), normal=np.array([1.0, 0.0, 0.5]), CD=2.0),
+        )
+    )
+    sat = make_satellite(disturbances=[drag])
+    x = make_state(q=np.array([0.6, 0.2, -0.1, 0.4]))
+    os = make_orbital_state(V=np.array([0.01, 7.5, 0.2]), rho=3.0e-12)
+    expected = fd_quat_hess_raw(lambda xx: drag.force(sat=sat, x=xx, os=os), x)
+
+    hessian = drag.force_qqhess(sat=sat, x=x, os=os)
+
+    assert hessian.shape == (4, 4, 3)
+    assert np.allclose(hessian, expected, atol=1.0e-7, rtol=1.0e-5)
+
+
+def test_drag_force_reaches_satellite_dist_forces():
+    drag = Drag_Disturbance(
+        make_geometry_config(
+            GeometryFace(area=2.5, centroid=np.array([0.1, 0.3, -0.2]), normal=np.array([0.0, 1.0, 0.0]), CD=1.7)
+        )
+    )
+    sat = make_satellite(disturbances=[drag])
+    x = make_state()
+    os = make_orbital_state(V=np.array([0.2, 7.4, -0.1]), rho=4.0e-12)
+
+    assert np.allclose(sat.dist_forces(x=x, os=os), drag.force(sat=sat, x=x, os=os))