diff --git a/ADCS/satellite_hardware/disturbances/drag_disturbance.py b/ADCS/satellite_hardware/disturbances/drag_disturbance.py index ed4a3b8e..840be0b3 100644 --- a/ADCS/satellite_hardware/disturbances/drag_disturbance.py +++ b/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""" diff --git a/testing/test_disturbances/_helpers.py b/testing/test_disturbances/_helpers.py index 09140063..d21d3899 100644 --- a/testing/test_disturbances/_helpers.py +++ b/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 diff --git a/testing/test_disturbances/drag/test_drag_force.py b/testing/test_disturbances/drag/test_drag_force.py new file mode 100644 index 00000000..9c528c02 --- /dev/null +++ b/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))