diff --git a/docs/source/Support/bskReleaseNotes.rst b/docs/source/Support/bskReleaseNotes.rst index 1545321ebc..1768f9c2d0 100644 --- a/docs/source/Support/bskReleaseNotes.rst +++ b/docs/source/Support/bskReleaseNotes.rst @@ -108,6 +108,10 @@ Version |release| ``pyStatefulSysModel.i`` to use this mixin. - Added support for setting the position and velocity of :ref:`MJBody` with pure translational motion. - Added ``getAxis`` and ``isHinge`` to :ref:`MJJoint`. +- Added :ref:`DragGeometryMsgPayload`, which contains geometry information of a body necessary to + compute the drag acting on such body. +- Added :ref:`AerodynamicDrag`, a model intended to be used with MuJoCo dynamics to model + cannonball aerodynamic drag forces and torques acting on bodies. Version 2.8.0 (August 30, 2025) diff --git a/src/architecture/msgPayloadDefC/DragGeometryMsgPayload.h b/src/architecture/msgPayloadDefC/DragGeometryMsgPayload.h new file mode 100644 index 0000000000..40ac9210a1 --- /dev/null +++ b/src/architecture/msgPayloadDefC/DragGeometryMsgPayload.h @@ -0,0 +1,31 @@ +/* + ISC License + + Copyright (c) 2025, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#ifndef DRAG_GEOMETRY_MSG_PAYLOAD_H +#define DRAG_GEOMETRY_MSG_PAYLOAD_H + + +//! @brief Container for basic drag parameters. +typedef struct { + double projectedArea; //!< [m^2] Area of spacecraft projected in velocity direction + double dragCoeff; //!< [-] Nondimensional drag coefficient + double r_CP_S[3]; //!< [m] Position of center of pressure relative to the center of frame S, given in frame S [m] +}DragGeometryMsgPayload; + +#endif diff --git a/src/simulation/mujocoDynamics/aerodynamicDrag/_UnitTest/test_aerodynamicDrag.py b/src/simulation/mujocoDynamics/aerodynamicDrag/_UnitTest/test_aerodynamicDrag.py new file mode 100644 index 0000000000..1fb75a60dd --- /dev/null +++ b/src/simulation/mujocoDynamics/aerodynamicDrag/_UnitTest/test_aerodynamicDrag.py @@ -0,0 +1,385 @@ +# +# ISC License +# +# Copyright (c) 2025, Autonomous Vehicle Systems Lab, University of Colorado at Boulder +# +# Permission to use, copy, modify, and/or distribute this software for any +# purpose with or without fee is hereby granted, provided that the above +# copyright notice and this permission notice appear in all copies. +# +# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES +# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF +# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR +# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES +# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN +# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF +# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. +from typing import Literal + +import pytest +import numpy as np +import numpy.testing as npt + +from Basilisk.utilities import SimulationBaseClass +from Basilisk.architecture import messaging +from Basilisk.utilities import macros +from Basilisk.utilities import RigidBodyKinematics as rbk +from Basilisk.utilities import simIncludeGravBody +from Basilisk.utilities import orbitalMotion +from Basilisk.simulation import pointMassGravityModel +from Basilisk.simulation import exponentialAtmosphere + +try: + from Basilisk.simulation import mujoco + from Basilisk.simulation import MJAerodynamicDrag + from Basilisk.simulation import NBodyGravity + + couldImportMujoco = True +except: + couldImportMujoco = False + +@pytest.mark.skipif(not couldImportMujoco, reason="Compiled Basilisk without --mujoco") +def test_aerodynamicDrag(): + """ + Unit test for verification of AerodynamicDrag. + """ + dt = 1 # s + + scSim = SimulationBaseClass.SimBaseClass() + dynProcess = scSim.CreateNewProcess("test") + dynProcess.addTask(scSim.CreateNewTask("test", macros.sec2nano(dt))) + + # Create the MJScene from a simple cannonball body + scene = mujoco.MJScene("") # empty scene, no multi-body dynamics + scSim.AddModelToTask("test", scene) + + density = 2 # kg/m^3 + cd = 1.5 + area = 2.75 # m^2 + r_CP_S = [1, 0, 0] # m + + v_SN_N = [0, 0.5, 0] # m/s + sigma_SN = [0.1, 0.2, 0.3] + + atmoMsg = messaging.AtmoPropsMsg() + atmoMsg.write(messaging.AtmoPropsMsgPayload(neutralDensity=density)) + + dragGeometryMsg = messaging.DragGeometryMsg() + dragGeometryMsg.write(messaging.DragGeometryMsgPayload( + projectedArea = area, + dragCoeff = cd, + r_CP_S = r_CP_S + )) + + siteFrameMsg = messaging.SCStatesMsg() + siteFrameMsg.write(messaging.SCStatesMsgPayload( + sigma_BN = sigma_SN, + v_BN_N = v_SN_N + )) + + drag = MJAerodynamicDrag.AerodynamicDrag() + drag.dragGeometryInMsg.subscribeTo(dragGeometryMsg) + drag.atmoDensInMsg.subscribeTo(atmoMsg) + drag.referenceFrameStateInMsg.subscribeTo(siteFrameMsg) + scene.AddModelToDynamicsTask(drag) + + scSim.InitializeSimulation() + scSim.ConfigureStopTime(macros.sec2nano(dt)) + scSim.ExecuteSimulation() + + force_S = drag.forceOutMsg.read().force_S + torque_S = drag.torqueOutMsg.read().torque_S + + # Expected + # Rotate velocity from N to S using MRPs + C_BN = np.array(rbk.MRP2C(sigma_SN)) # maps N components to B=S components + v_S = C_BN.dot(v_SN_N) + v_mag = np.linalg.norm(v_SN_N) # drag magnitude depends on speed, frame invariant + if v_mag <= 1e-12: + F_exp = np.zeros(3) + T_exp = np.zeros(3) + else: + F_mag = 0.5 * density * (v_mag ** 2) * cd * area + v_hat_S = v_S / np.linalg.norm(v_S) + F_exp = -F_mag * v_hat_S + T_exp = np.cross(r_CP_S, F_exp) + + npt.assert_allclose(force_S, F_exp, rtol=0.0, atol=1e-12) + npt.assert_allclose(torque_S, T_exp, rtol=0.0, atol=1e-12) + +@pytest.mark.skipif(not couldImportMujoco, reason="Compiled Basilisk without --mujoco") +@pytest.mark.parametrize("orbitCase", ["LPO", "LTO"]) +@pytest.mark.parametrize("planetCase", ["earth", "mars"]) +def test_orbit(orbitCase: Literal["LPO", "LTO"], planetCase: Literal["earth", "mars"], showPlots = False): + """Test equivalent to `simulation/dynamics/dragEffector/_UnitTest/test_atmoDrag.py`""" + def cannonballDragBFrame(dragCoeff, density, area, velInertial, sigmaBN): + speed = np.linalg.norm(velInertial) + if speed <= 0.0: + return np.zeros(3) + + dragDirInertial = -velInertial / speed + dcmBN = rbk.MRP2C(sigmaBN) + dragDirBody = dcmBN.dot(dragDirInertial) + + return 0.5 * dragCoeff * density * area * speed**2 * dragDirBody + + + scSim = SimulationBaseClass.SimBaseClass() + processName = "dragProcess" + taskName = "dragTask" + dynamicsProcess = scSim.CreateNewProcess(processName) + stepNanos = macros.sec2nano(1.0) + dynamicsProcess.addTask(scSim.CreateNewTask(taskName, stepNanos)) + + bodyName = "satellite" + dragSceneXml = fr""" + + + + + + + + + + + """ + scene = mujoco.MJScene(dragSceneXml) + scene.extraEoMCall = True + scSim.AddModelToTask(taskName, scene) + + mjBody = scene.getBody(bodyName) + + planetData = simIncludeGravBody.BODY_DATA[planetCase] + gravitationalParameter = planetData.mu + planetRadius = planetData.radEquator + + gravityModel = NBodyGravity.NBodyGravity() + gravityModel.ModelTag = "gravity" + scene.AddModelToDynamicsTask(gravityModel) + + pointMassModel = pointMassGravityModel.PointMassGravityModel() + pointMassModel.muBody = gravitationalParameter + gravityModel.addGravitySource(planetCase, pointMassModel, True) + gravityModel.addGravityTarget(bodyName, mjBody) + + if planetCase == "earth": + baseDensity = 1.217 + scaleHeight = 8500.0 + else: + baseDensity = 0.020 + scaleHeight = 11100.0 + + atmoModel = exponentialAtmosphere.ExponentialAtmosphere() + atmoModel.ModelTag = "ExpAtmo" + atmoModel.planetRadius = planetRadius + atmoModel.baseDensity = baseDensity + atmoModel.scaleHeight = scaleHeight + scene.AddModelToDynamicsTask(atmoModel) + + comStateMsg = mjBody.getCenterOfMass().stateOutMsg + atmoModel.addSpacecraftToModel(comStateMsg) + + projectedArea = 10.0 + dragCoeff = 2.2 + + dragGeometryMsg = messaging.DragGeometryMsg() + dragGeometryMsg.write( + messaging.DragGeometryMsgPayload( + projectedArea=projectedArea, + dragCoeff=dragCoeff, + r_CP_S=[1.0, 0.0, 0.0], + ) + ) + + dragModel = MJAerodynamicDrag.AerodynamicDrag() + dragModel.ModelTag = "drag" + scene.AddModelToDynamicsTask(dragModel) + forceTorqueDrag: mujoco.MJForceTorqueActuator = dragModel.applyTo(mjBody) + dragModel.atmoDensInMsg.subscribeTo(atmoModel.envOutMsgs[0]) + dragModel.dragGeometryInMsg.subscribeTo(dragGeometryMsg) + + if orbitCase == "LPO": + orbAltMin = 300e3 # m + orbAltMax = orbAltMin + elif orbitCase == "LTO": + orbAltMin = 300e3 # m + orbAltMax = 800e3 # m + + rMin = planetRadius + orbAltMin + rMax = planetRadius + orbAltMax + + elements = orbitalMotion.ClassicElements() + elements.a = (rMin+rMax)/2.0 + elements.e = 1.0 - rMin/elements.a + elements.i = 0.0 + elements.Omega = 0.0 + elements.omega = 0.0 + elements.f = 0.0 + + meanMotion = np.sqrt(gravitationalParameter / elements.a**3) + orbitPeriod = 2.0 * np.pi / meanMotion + finalTimeNanos = macros.sec2nano(orbitPeriod) + + stateRecorder = comStateMsg.recorder() + atmoRecorder = atmoModel.envOutMsgs[0].recorder() + dragForceRecorder = forceTorqueDrag.forceInMsg.recorder() + + scSim.AddModelToTask(taskName, stateRecorder) + scSim.AddModelToTask(taskName, atmoRecorder) + scSim.AddModelToTask(taskName, dragForceRecorder) + + scSim.InitializeSimulation() + + positionInertial, velocityInertial = orbitalMotion.elem2rv(gravitationalParameter, elements) + mjBody.setPosition(positionInertial) + mjBody.setVelocity(velocityInertial) + + scSim.ConfigureStopTime(finalTimeNanos) + scSim.ExecuteSimulation() + + positionData = stateRecorder.r_BN_N + velocityData = stateRecorder.v_BN_N + attitudeData = stateRecorder.sigma_BN + densityData = atmoRecorder.neutralDensity + dragForceData = dragForceRecorder.force_S + + numSteps = dragForceData.shape[0] + referenceDrag = np.zeros_like(dragForceData) + + for timeIndex in range(numSteps): + referenceDrag[timeIndex, :] = cannonballDragBFrame( + dragCoeff=dragCoeff, + density=densityData[timeIndex], + area=projectedArea, + velInertial=velocityData[timeIndex, :], + sigmaBN=attitudeData[timeIndex, :], + ) + + npt.assert_allclose( + dragForceData, + referenceDrag, + rtol=0, + atol=1e-12, + ) + + if showPlots: + import matplotlib.pyplot as plt + + timesSec = stateRecorder.times() * macros.NANO2SEC + timesOrbit = timesSec / orbitPeriod + + # Inertial position components + plt.figure() + fig = plt.gcf() + ax = fig.gca() + ax.ticklabel_format(useOffset=False, style="plain") + for idx in range(3): + plt.plot( + timesOrbit, + positionData[:, idx] / 1e3, + label=f"$r_{{BN,{idx}}}$", + ) + plt.legend(loc="lower right") + plt.xlabel("Time [orbits]") + plt.ylabel("Inertial position [km]") + + # Orbit in perifocal frame + elementsInit = orbitalMotion.rv2elem(gravitationalParameter, positionInertial, velocityInertial) + bParam = elementsInit.a * np.sqrt(1.0 - elementsInit.e**2) + pParam = elementsInit.a * (1.0 - elementsInit.e**2) + + plt.figure(figsize=tuple(np.array((1.0, bParam / elementsInit.a)) * 4.75), dpi=100) + axisLimits = np.array( + [-elementsInit.rApoap, elementsInit.rPeriap, -bParam, bParam] + ) / 1e3 * 1.25 + plt.axis(axisLimits) + + fig = plt.gcf() + ax = fig.gca() + planetColor = "#008800" + planetRadiusKm = planetRadius / 1e3 + ax.add_artist(plt.Circle((0.0, 0.0), planetRadiusKm, color=planetColor)) + + rDataList = [] + fDataList = [] + for idx in range(positionData.shape[0]): + elementsStep = orbitalMotion.rv2elem( + gravitationalParameter, + positionData[idx, 0:3], + velocityData[idx, 0:3], + ) + rDataList.append(elementsStep.rmag) + fDataList.append(elementsStep.f + elementsStep.omega - elementsInit.omega) + + rDataArray = np.array(rDataList) + fDataArray = np.array(fDataList) + + plt.plot( + rDataArray * np.cos(fDataArray) / 1e3, + rDataArray * np.sin(fDataArray) / 1e3, + color="#aa0000", + linewidth=3.0, + ) + + fGrid = np.linspace(0.0, 2.0 * np.pi, 200) + rGrid = pParam / (1.0 + elementsInit.e * np.cos(fGrid)) + plt.plot( + rGrid * np.cos(fGrid) / 1e3, + rGrid * np.sin(fGrid) / 1e3, + "--", + color="#555555", + ) + plt.xlabel("$i_e$ coord [km]") + plt.ylabel("$i_p$ coord [km]") + plt.grid() + + # Semi major axis vs time + plt.figure() + fig = plt.gcf() + ax = fig.gca() + ax.ticklabel_format(useOffset=False, style="plain") + smaData = [] + for idx in range(positionData.shape[0]): + elementsStep = orbitalMotion.rv2elem( + gravitationalParameter, + positionData[idx, 0:3], + velocityData[idx, 0:3], + ) + smaData.append(elementsStep.a / 1e3) + smaData = np.array(smaData) + plt.plot(timesOrbit, smaData, color="#aa0000") + plt.xlabel("Time [orbits]") + plt.ylabel("SMA [km]") + + # Density vs time + plt.figure() + fig = plt.gcf() + ax = fig.gca() + ax.ticklabel_format(useOffset=False, style="sci") + timesAtmoSec = atmoRecorder.times() * macros.NANO2SEC + plt.plot(timesAtmoSec, densityData) + plt.title("Density vs time") + plt.xlabel("Time [s]") + plt.ylabel("Density [kg/m^3]") + + # Drag vs reference + plt.figure() + fig = plt.gcf() + ax = fig.gca() + ax.ticklabel_format(useOffset=False, style="plain") + for idx in range(3): + plt.plot(timesOrbit, dragForceData[:, idx], label=f"F_mod_{idx}") + plt.plot(timesOrbit, referenceDrag[:, idx], "--", label=f"F_ref_{idx}") + plt.xlabel("Time [orbits]") + plt.ylabel("Drag force [N]") + plt.legend() + plt.title("Drag force vs reference") + + plt.show() + + +if __name__ == "__main__": + assert couldImportMujoco + # test_aerodynamicDrag() + test_orbit("LTO","earth",True) diff --git a/src/simulation/mujocoDynamics/aerodynamicDrag/aerodynamicDrag.cpp b/src/simulation/mujocoDynamics/aerodynamicDrag/aerodynamicDrag.cpp new file mode 100644 index 0000000000..d24aad286c --- /dev/null +++ b/src/simulation/mujocoDynamics/aerodynamicDrag/aerodynamicDrag.cpp @@ -0,0 +1,104 @@ +/* + ISC License + + Copyright (c) 2025, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +#include "aerodynamicDrag.h" + +#include + +void +AerodynamicDrag::UpdateState(uint64_t CurrentSimNanos) +{ + const double density = this->atmoDensInMsg().neutralDensity; + + const auto geomPayload = this->dragGeometryInMsg(); + const double cd = geomPayload.dragCoeff; + const double area = geomPayload.projectedArea; + const Eigen::Map r_CP_s(geomPayload.r_CP_S); + + // 'S' denotes the 'site' reference frame + const auto siteStatePayload = this->referenceFrameStateInMsg(); + Eigen::MRPd sigma_SN; + sigma_SN = Eigen::Map(siteStatePayload.sigma_BN); + const Eigen::Matrix3d dcm_SN = sigma_SN.toRotationMatrix().transpose(); + + // inertial velocity of the center of the S frame, expressed in the S frame + const auto v_S = dcm_SN*Eigen::Map(siteStatePayload.v_BN_N); + const auto v = v_S.norm(); + + if (v < 1e-10) + { + this->forceOutMsg.write(&this->forceOutMsg.zeroMsgPayload, this->moduleID, CurrentSimNanos); + this->torqueOutMsg.write(&this->torqueOutMsg.zeroMsgPayload, this->moduleID, CurrentSimNanos); + return; + } + + const Eigen::Vector3d force_S = -0.5 * cd * area * density * v * v_S; + const Eigen::Vector3d torque_S = r_CP_s.cross(force_S); + + ForceAtSiteMsgPayload forcePayload = this->forceOutMsg.zeroMsgPayload; + TorqueAtSiteMsgPayload torquePayload = this->torqueOutMsg.zeroMsgPayload; + std::copy_n(force_S.data(), 3, forcePayload.force_S); + std::copy_n(torque_S.data(), 3, torquePayload.torque_S); + + this->forceOutMsg.write(&forcePayload, this->moduleID, CurrentSimNanos); + this->torqueOutMsg.write(&torquePayload, this->moduleID, CurrentSimNanos);} + +void AerodynamicDrag::Reset(uint64_t /*CurrentSimNanos*/) +{ + if (!this->atmoDensInMsg.isLinked()) { + bskLogger.bskLog(BSK_ERROR, + "AerodynamicDrag: atmoDensInMsg is not linked. " + "Call atmoDensInMsg.subscribeTo() before simulation."); + } + + if (!this->dragGeometryInMsg.isLinked()) { + bskLogger.bskLog(BSK_ERROR, + "AerodynamicDrag: dragGeometryInMsg is not linked. " + "Call dragGeometryInMsg.subscribeTo() before simulation."); + } + + if (!this->referenceFrameStateInMsg.isLinked()) { + bskLogger.bskLog(BSK_ERROR, + "AerodynamicDrag: referenceFrameStateInMsg is not linked. " + "Call referenceFrameStateInMsg.subscribeTo() before simulation."); + } +} + + +MJForceTorqueActuator& +AerodynamicDrag::applyTo(MJBody& body) +{ + return applyTo(body.getCenterOfMass()); +} + +MJForceTorqueActuator& +AerodynamicDrag::applyTo(MJSite& site) +{ + this->referenceFrameStateInMsg.subscribeTo( &site.stateOutMsg ); + const auto actuatorName = ModelTag + "_aerodynamicDrag_" + site.getName(); + auto& actuator = site.getBody().getSpec().getScene().addForceTorqueActuator(actuatorName, site); + return applyTo(actuator); +} + +MJForceTorqueActuator& +AerodynamicDrag::applyTo(MJForceTorqueActuator& actuator) +{ + actuator.forceInMsg.subscribeTo( &this->forceOutMsg ); + actuator.torqueInMsg.subscribeTo( &this->torqueOutMsg ); + return actuator; +} diff --git a/src/simulation/mujocoDynamics/aerodynamicDrag/aerodynamicDrag.h b/src/simulation/mujocoDynamics/aerodynamicDrag/aerodynamicDrag.h new file mode 100644 index 0000000000..5370485b31 --- /dev/null +++ b/src/simulation/mujocoDynamics/aerodynamicDrag/aerodynamicDrag.h @@ -0,0 +1,118 @@ +/* + ISC License + + Copyright (c) 2025, Autonomous Vehicle Systems Lab, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +#ifndef AERODYNAMIC_DRAG_H +#define AERODYNAMIC_DRAG_H + +#include "architecture/utilities/bskLogging.h" +#include "architecture/messaging/messaging.h" +#include "architecture/_GeneralModuleFiles/sys_model.h" +#include "simulation/mujocoDynamics/_GeneralModuleFiles/MJScene.h" + +#include "architecture/msgPayloadDefC/DragGeometryMsgPayload.h" +#include "architecture/msgPayloadDefC/AtmoPropsMsgPayload.h" +#include "architecture/msgPayloadDefC/ForceAtSiteMsgPayload.h" +#include "architecture/msgPayloadDefC/TorqueAtSiteMsgPayload.h" + +/** + * @brief Aerodynamic drag model that computes force and torque at a MuJoCo site. + * + * Updates a force and a torque to represent quasi-steady aerodynamic drag acting + * at a site fixed on a body. Force direction is opposite the relative flow. + * Magnitude follows \f$ F = \tfrac{1}{2}\rho v^2 C_D A \f$. + * Torque is \f$ \tau = r_{CP/S} \times F \f$ in the site frame. + * + * The force and torque output in `forceOutMsg` and `torqueOutMsg` are given + * in the site reference frame whose state is given in `referenceFrameStateInMsg`. + */ +class AerodynamicDrag : public SysModel +{ +public: + /** + * @brief Bind this model to an MJBody target. + * + * If this method is used, only the `dragGeometryInMsg` and + * `atmoDensInMsg` must be linked. + * + * @param body MuJoCo body that will receive the drag force and torque at it's center of mass. + * @return Reference to the actuator on which the forceand torque is applied. + */ + MJForceTorqueActuator& applyTo(MJBody& body); + + /** + * @brief Bind this model to an MJSite target. + * + * If this method is used, only the `dragGeometryInMsg` and + * `atmoDensInMsg` must be linked. + * + * @param site MuJoCo site where the force application point is defined. + * @return Reference to the actuator on which the forceand torque is applied. + */ + MJForceTorqueActuator& applyTo(MJSite& site); + + /** + * @brief Bind this model to an existing force/torque actuator. + * + * If this method is used, the `dragGeometryInMsg`, + * `atmoDensInMsg`, and `referenceFrameStateInMsg` must be linked. + * + * @param actuator Precreated actuator that will receive the computed wrench. + * @return Reference to the same actuator. + * + * @note You + */ + MJForceTorqueActuator& applyTo(MJForceTorqueActuator& actuator); + + + /** + * @brief Advance the model and publish outputs. + * @param CurrentSimNanos Current simulation time in nanoseconds. + */ + void UpdateState(uint64_t CurrentSimNanos) override; + + /** + * @brief Validate all input messages are linked. + * @param CurrentSimNanos Current simulation time in nanoseconds. + */ + void Reset(uint64_t CurrentSimNanos) override; + + +public: + + /** @brief Geometry input. */ + ReadFunctor dragGeometryInMsg; + + /** @brief Atmospheric properties input. */ + ReadFunctor atmoDensInMsg; + + /** @brief State of the reference frame . */ + ReadFunctor referenceFrameStateInMsg; + + /** @brief Output force at the site in the site reference frame. */ + Message forceOutMsg; + + /** @brief Output torque in the site reference frame.*/ + Message torqueOutMsg; + + + /** @brief Logger */ + BSKLogger bskLogger; +}; + + +#endif diff --git a/src/simulation/mujocoDynamics/aerodynamicDrag/aerodynamicDrag.i b/src/simulation/mujocoDynamics/aerodynamicDrag/aerodynamicDrag.i new file mode 100644 index 0000000000..60677b456c --- /dev/null +++ b/src/simulation/mujocoDynamics/aerodynamicDrag/aerodynamicDrag.i @@ -0,0 +1,52 @@ +/* + ISC License + + Copyright (c) 2025, Autonomous Vehicle Systems Lab, University of Colorado Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + +*/ + +%module MJAerodynamicDrag + +%include "architecture/utilities/bskException.swg" +%default_bsk_exception(); + +%{ + #include "aerodynamicDrag.h" +%} + +%pythoncode %{ + from Basilisk.architecture.swig_common_model import * +%} +%include "std_string.i" +%include "swig_conly_data.i" + +%include "sys_model.i" +%include "aerodynamicDrag.h" + +%include "architecture/msgPayloadDefC/DragGeometryMsgPayload.h" +struct DragGeometryMsg_C; +%include "architecture/msgPayloadDefC/AtmoPropsMsgPayload.h" +struct AtmoPropsMsg_C; +%include "architecture/msgPayloadDefC/SCStatesMsgPayload.h" +struct SCStatesMsg_C; +%include "architecture/msgPayloadDefC/ForceAtSiteMsgPayload.h" +struct ForceAtSiteMsg_C; +%include "architecture/msgPayloadDefC/TorqueAtSiteMsgPayload.h" +struct TorqueAtSiteMsg_C; + +%pythoncode %{ +import sys +protectAllClasses(sys.modules[__name__]) +%}