#
# ISC License
#
# Copyright (c) 2026, 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.
#
r"""
Review these prerequisite examples first:
#. :ref:`scenarioMomentumDumping` for the classic momentum-dumping sequence.
#. :ref:`scenarioAttitudeFeedbackRWMuJoCo` for reaction-wheel control with MuJoCo.
This script demonstrates how to run a reaction wheel momentum dumping
scenario, translated from the classic Basilisk :ref:`scenarioMomentumDumping`
example, using MuJoCo dynamics via :ref:`MJScene<MJScene>` instead of the
traditional hub-centric Basilisk :ref:`spacecraft` dynamics.
Running the Example
-------------------
Use a Basilisk installation with MuJoCo support and the example dependencies
described in :ref:`bskInstall`. When building from source, enable
``--mujoco True``. With that Python environment active, run from the repository
root:
.. code-block:: console
python examples/mujoco/scenarioMomentumDumpingMuJoCo.py
The script displays the result plots. When importing the scenario from
``examples/mujoco``, call ``run(showPlots=False)`` to run without displaying
windows; it still returns a dictionary of Matplotlib figures for inspection
or saving.
Model and Control Setup
-----------------------
The multi-body system is created programmatically as a MuJoCo XML string.
It consists of a free-floating spacecraft bus ("hub") carrying 4 reaction
wheel rigid bodies ("rw1Spin" ... "rw4Spin") mounted at canted spin
axes, and 8 thrusters attached directly to the hub via fixed sites.
Each wheel spin degree of freedom (DOF) is driven by a MuJoCo single-input
torque actuator, and each thruster site has a MuJoCo motor actuator that
applies force along the site's z-axis.
A standard Basilisk flight software (FSW) stack uses the reaction wheels to
point the hub at a fixed inertial attitude. The attitude control system (ACS)
thrusters remove excess wheel momentum in an explicitly requested dumping
sequence:
#. :ref:`inertial3D` generates a fixed inertial attitude reference.
#. :ref:`attTrackingError` computes the attitude and rate tracking errors.
#. :ref:`mrpFeedback` computes the commanded body control torque.
#. :ref:`rwMotorTorque` maps the commanded body torque into individual wheel
motor torque commands.
#. :ref:`thrMomentumManagement` checks the net wheel momentum once on its
first update after each ``Reset()``. If the magnitude exceeds ``hs_min``,
it requests an angular-momentum change, delta-H [N m s], to reduce it
to that level. Otherwise it requests zero change. Later threshold
crossings do not trigger another check unless the module is reset again.
#. :ref:`thrForceMapping` maps the desaturation delta-H request into
individual thruster impulse requests [N s].
#. :ref:`thrMomentumDumping` divides the requested impulses by the configured
maximum thrust [N] to obtain firing durations [s], then distributes them
over discrete on-time pulses.
This chain reuses ``CmdTorqueBodyMsgPayload.torqueRequestBody`` to carry
angular impulse [N m s], and ``THRArrayCmdForceMsgPayload.thrForce`` to carry
linear impulse [N s]. Despite their field names, these values represent
impulses in this application. The subsequent ``thrOnTimeToForce`` adapter
produces the instantaneous force commands [N] sent to the actuators.
See :ref:`scenarioMomentumDumping` for more detail on this control sequence.
Several small adapter modules bridge Basilisk messaging to MuJoCo objects:
#. :ref:`arrayMotorTorqueToSingleActuators` splits the single
``ArrayMotorTorqueMsg`` from ``rwMotorTorque`` into individual
``SingleActuatorMsg`` commands, one per MuJoCo wheel actuator.
#. :ref:`scalarJointStatesToRWSpeed` reads each wheel's spin rate directly
from its MuJoCo joint state and republishes them as a single ``RWSpeedMsg``
for the momentum-management chain.
#. :ref:`thrOnTimeToForce` converts the thruster on-time pulse commands from
``thrMomentumDumping`` into instantaneous force commands for each MuJoCo
thruster actuator.
Earth gravity is configured using :ref:`NBodyGravity<NBodyGravity>` with a
:ref:`pointMassGravityModel<pointMassGravityModel>` as the central body,
applied to the hub only.
The spacecraft is placed on a near-circular low Earth orbit (LEO), and the
wheels are initialized above the 80 N m s momentum threshold. The simulation
runs to 10 seconds, then explicitly resets ``thrMomentumManagement``. Its
next FSW update at 11 seconds publishes a fresh delta-H request. This avoids
the initial request at time zero, whose timestamp is not recognized as a
new request by ``thrMomentumDumping`` after initialization. The simulation
continues to a total elapsed time of 5 minutes while the wheels provide
attitude control and the thrusters execute the requested dumping sequence.
Attitude error, rate tracking error, individual and total wheel momenta,
requested angular-momentum change, wheel speeds, thruster impulse requests,
thruster on-time requests, and delivered thruster forces are plotted at the end.
Illustration of Simulation Results
----------------------------------
.. image:: /_images/Scenarios/scenarioMomentumDumpingMuJoCo_attError.svg
:align: center
.. image:: /_images/Scenarios/scenarioMomentumDumpingMuJoCo_rateError.svg
:align: center
.. image:: /_images/Scenarios/scenarioMomentumDumpingMuJoCo_rwMomenta.svg
:align: center
.. image:: /_images/Scenarios/scenarioMomentumDumpingMuJoCo_DH.svg
:align: center
The delta-H plot shows the command held between momentum-management resets,
not the accumulated momentum removed by the thrusters. Use the wheel-momentum
history above to assess the resulting change in stored wheel momentum.
.. image:: /_images/Scenarios/scenarioMomentumDumpingMuJoCo_rwSpeeds.svg
:align: center
.. image:: /_images/Scenarios/scenarioMomentumDumpingMuJoCo_thrImpulse.svg
:align: center
.. image:: /_images/Scenarios/scenarioMomentumDumpingMuJoCo_OnTimeReq.svg
:align: center
.. image:: /_images/Scenarios/scenarioMomentumDumpingMuJoCo_thrForce.svg
:align: center
"""
import os
import matplotlib.pyplot as plt
import numpy as np
from Basilisk.utilities import SimulationBaseClass, macros, orbitalMotion, simHelpers, simIncludeRW, simIncludeThruster
from Basilisk.simulation import NBodyGravity, mujoco, pointMassGravityModel, simpleNav, thrOnTimeToForce, arrayMotorTorqueToSingleActuators, saturationSingleActuator, scalarJointStatesToRWSpeed
from Basilisk.fswAlgorithms import mrpFeedback, inertial3D, attTrackingError, rwMotorTorque, thrMomentumManagement, thrForceMapping, thrMomentumDumping
from Basilisk.architecture import messaging, sysModel
from Basilisk import __path__
# Used to tag saved figs with name of file
fileName = os.path.basename(os.path.splitext(__file__)[0])
# -------------------------------------------------------------------------
# PLOTTING FUNCTIONS
# -------------------------------------------------------------------------
[docs]
def plot_attitude_error(timeData, dataSigmaBR):
"""Plot the attitude errors."""
fig = plt.figure(num = 1, clear = True)
ax = fig.gca()
ax.ticklabel_format(useOffset = False, style = 'plain')
for idx in range(3):
plt.plot(timeData, dataSigmaBR[:, idx],
color = simHelpers.getLineColor(idx, 3),
label = r'$\sigma_' + str(idx) + r'$')
plt.legend(loc = 'lower right')
plt.xlabel('Time [min]')
plt.ylabel(r'Attitude Error $\sigma_{B/R}$')
return fig
[docs]
def plot_rate_error(timeData, dataOmegaBR):
"""Plot the body angular velocity rate tracking errors."""
fig = plt.figure(num = 2, clear = True)
ax = fig.gca()
ax.ticklabel_format(useOffset = False, style = 'plain')
for idx in range(3):
plt.plot(timeData, dataOmegaBR[:, idx],
color = simHelpers.getLineColor(idx, 3),
label = r'$\omega_{BR,' + str(idx+1) + r'}$')
plt.legend(loc = 'lower right')
plt.xlabel('Time [min]')
plt.ylabel('Rate Tracking Error (rad/s) ')
return fig
[docs]
def plot_rw_momenta(timeData, dataOmegaRw, RW, numRW):
"""Plot the RW momenta."""
# Total momentum norm found by projecting each wheel's momentum onto spin axis and summing
totMomentumNorm = []
for j in range(len(timeData)):
totMomentum = np.zeros(3)
for idx in range(numRW):
for k in range(3):
totMomentum[k] = totMomentum[k] + dataOmegaRw[j, idx] * RW[idx].Js * RW[idx].gsHat_B[k][0]
totMomentumNorm.append(np.linalg.norm(totMomentum))
fig = plt.figure(num = 3, clear = True)
ax = fig.gca()
ax.ticklabel_format(useOffset = False, style = 'plain')
for idx in range(numRW):
plt.plot(timeData, dataOmegaRw[:, idx] * RW[idx].Js,
color = simHelpers.getLineColor(idx, numRW),
label = r'$H_{' + str(idx+1) + r'}$')
plt.plot(timeData, totMomentumNorm, '--',
label = r'$\|H\|$')
plt.legend(loc = 'lower right')
plt.xlabel('Time [min]')
plt.ylabel('RW Momentum (Nms)')
return fig
[docs]
def plot_DH(timeData, dataDH):
"""Plot the requested angular-momentum change in body-frame components.
:param timeData: Sample times [min].
:param dataDH: Requested delta-H vectors [N m s], with one row per sample
and columns for the body x, y, and z components. These commands are
held between momentum-management resets; they are not measurements
of accumulated momentum removal.
:returns: Figure showing the three components of the delta-H request.
:rtype: matplotlib.figure.Figure
"""
fig = plt.figure(num = 4, clear = True)
ax = fig.gca()
ax.ticklabel_format(useOffset = False, style = 'plain')
for idx in range(3):
plt.plot(timeData, dataDH[:, idx],
color = simHelpers.getLineColor(idx, 3),
label = r'$\Delta H_{' + str(idx+1) + r'}$')
plt.legend(loc = 'lower right')
plt.xlabel('Time [min]')
plt.ylabel('Requested momentum change [N m s]')
return fig
[docs]
def plot_rw_speeds(timeData, dataOmegaRW, numRW):
"""Plot the RW spin rates."""
fig = plt.figure(num = 5, clear = True)
ax = fig.gca()
ax.ticklabel_format(useOffset = False, style = 'plain')
plt.figure(5)
for idx in range(numRW):
plt.plot(timeData, dataOmegaRW[:, idx] / macros.RPM,
color = simHelpers.getLineColor(idx, numRW),
label = r'$\Omega_{' + str(idx+1) + r'}$')
plt.legend(loc = 'lower right')
plt.xlabel('Time [min]')
plt.ylabel('RW Speed (RPM) ')
return fig
[docs]
def plot_thrImpulse(timeDataFSW, dataMap, numTh):
"""Plot the Thruster force values."""
fig = plt.figure(num = 6, clear = True)
ax = fig.gca()
ax.ticklabel_format(useOffset = False, style = 'plain')
for idx in range(numTh):
plt.plot(timeDataFSW, dataMap[:, idx],
color = simHelpers.getLineColor(idx, numTh),
label = r'$thrImpulse_{' + str(idx+1) + r'}$')
plt.legend(loc = 'lower right')
plt.xlabel('Time [min]')
plt.ylabel('Impulse requested [Ns]')
return fig
[docs]
def plot_OnTimeRequest(timeData, dataOnTime, numTh):
"""Plot the thruster on time requests."""
fig = plt.figure(num = 7, clear = True)
ax = fig.gca()
ax.ticklabel_format(useOffset = False, style = 'plain')
for idx in range(numTh):
plt.plot(timeData, dataOnTime[:, idx],
color = simHelpers.getLineColor(idx, numTh),
label = r'$OnTimeRequest_{' + str(idx+1) + r'}$')
plt.legend(loc = 'lower right')
plt.xlabel('Time [min]')
plt.ylabel('OnTimeRequest [sec]')
return fig
[docs]
def plot_thrForce(timeDataFSW, dataThr, numTh):
"""Plot the Thruster force values."""
fig = plt.figure(num = 8, clear = True)
ax = fig.gca()
ax.ticklabel_format(useOffset = False, style = 'plain')
for idx in range(numTh):
plt.plot(timeDataFSW, dataThr[idx],
color = simHelpers.getLineColor(idx, numTh),
label = r'$thrForce_{' + str(idx+1) + r'}$')
plt.legend(loc = 'lower right')
plt.xlabel('Time [min]')
plt.ylabel('Thruster force [N]')
return fig
[docs]
def addRWsXML(rwPos: list,
rwAxes: list,
rwFactory: simIncludeRW.rwFactory,
maxMomentum: float = 100.,
baseIndent: int = 3):
"""Build MuJoCo XML for reaction-wheel bodies and motor actuators.
:param rwPos: Wheel mounting positions in the hub frame [m], one
three-component vector per wheel.
:param rwAxes: Unit spin-axis vectors in the hub frame, in the same order
as ``rwPos``.
:param rwFactory: Factory in which the wheel configuration objects are
created and registered.
:param maxMomentum: Maximum momentum specification per wheel [N m s].
Defaults to 100.0 N m s.
:param baseIndent: Number of leading indentation tabs for wheel-body XML.
Defaults to 3.
:returns: A tuple containing the wheel-body XML, motor-actuator XML, and
list of registered wheel configuration objects in mounting order.
:rtype: tuple[str, str, list]
"""
numRW = len(rwPos)
pad = "\t" * baseIndent
rwTags, actTags, RWs = [], [], []
for idx in range(numRW):
n = idx + 1
pos = rwPos[idx]
rwAxis = rwAxes[idx]
# Register wheel with the RW factory to get correct mass/inertia/torque properties
rw = rwFactory.create('Honeywell_HR16', rwAxis, maxMomentum = maxMomentum)
RWs.append(rw)
# XML uses lengths [m], mass [kg], inertia [kg m^2], and motor torque limits [N m].
# XML string defining RW body: single revolute joint about spin axis, inertial props
# taken from the factory-created wheel (Jt, Jt, Js), simple cylinder geom for visuals only
rwTags.append(
f"""{pad}<body name = "rw{n}Spin" pos = "{pos[0]} {pos[1]} {pos[2]}" zaxis = "{rwAxis[0]} {rwAxis[1]} {rwAxis[2]}">
{pad}\t<joint name = "rw{n}Joint" pos = "0 0 0" axis = "0 0 1" ref = "0"/>
{pad}\t<inertial pos = "0 0 0" mass = "{rw.mass}" diaginertia = "{rw.Jt} {rw.Jt} {rw.Js}"/>
{pad}\t<geom name = "rw{n}Geom" type = "cylinder" size = "0.2 0.05" contype = "0" conaffinity = "0"/>
{pad}</body>""")
# Torque-controlled actuator on the wheel joint, limited to the wheel's max motor torque
actTags.append(f'\t\t<motor name = "rw{n}Act" joint = "rw{n}Joint" ctrlrange = "{-rw.u_max} {rw.u_max}" ctrllimited = "true"/>')
return "\n".join(rwTags), "\n".join(actTags), RWs
[docs]
def addThrustersXML(thrustLocs: list,
thrustDirs: list,
thrFactory: simIncludeThruster.thrusterFactory,
maxThrust: float = 5.0,
baseIndent: int = 2):
"""Build MuJoCo XML for thruster sites and force actuators.
:param thrustLocs: Thruster mounting positions in the hub frame [m], one
three-component vector per thruster.
:param thrustDirs: Unit thrust directions in the hub frame, in the same
order as ``thrustLocs``.
:param thrFactory: Factory in which the thruster configuration objects are
created and registered.
:param maxThrust: Maximum thrust per thruster used for the FSW configuration
and MuJoCo actuator control limit [N]. Defaults to 5.0 N.
:param baseIndent: Base number of indentation tabs for the generated XML.
Defaults to 2; site elements receive one additional tab.
:returns: A tuple containing the site XML, force-actuator XML, and list of
registered thruster configuration objects in mounting order.
:rtype: tuple[str, str, list]
"""
numTHRs = len(thrustLocs)
pad = "\t" * baseIndent
thrustTags, actTags, THRs = [], [], []
for idx in range(numTHRs):
n = idx + 1
pos = thrustLocs[idx]
dirVec = thrustDirs[idx]
# Register thruster with thruster factory to get correct FSW config properties
thr = thrFactory.create('MOOG_Monarc_5', pos, dirVec, MaxThrust = maxThrust)
THRs.append(thr)
# Site marks thruster location/orientation on hub, motor applies force along site z-axis
thrustTags.append(f'{pad}\t<site name = "thrusterSite{n}" pos = "{pos[0]} {pos[1]} {pos[2]}" zaxis = "{dirVec[0]} {dirVec[1]} {dirVec[2]}"/>')
actTags.append(f'{pad}<motor name = "thruster{n}" site = "thrusterSite{n}" gear = "0 0 1 0 0 0" ctrlrange = "0 {thr.MaxThrust}"/>')
return "\n".join(thrustTags), "\n".join(actTags), THRs
[docs]
def makeMjXmlString():
"""Build MuJoCo XML for a hub with four wheels and eight ACS thrusters.
The hub, wheel, and thruster properties are specified within this function.
:returns: A tuple containing the complete model XML, wheel configuration
objects, thruster configuration objects, reaction-wheel factory, and
thruster factory, in that order.
:rtype: tuple
"""
# Hub mass properties
hubMass = 2500.0 # kg
hubIxx, hubIyy, hubIzz = 1700.0, 1700.0, 1800.0 # kg*m^2
# All 4 RWs mounted at same offset point, canted along 4 different spin axes
c = 2**-0.5
rwPos = [[0.0, 0.0, 1.28]] * 4 # m
rwAxes = [[c, 0, c], [0, c, c], [-c, 0, c], [0, -c, c]]
# 8 ACS thrusters at hub corners, each aligned with a principal body axis
a, b = 1.0, 1.28 # [m]
thrustLocs = [
[-a, -a, b], [ a, -a, -b], [ a, -a, b], [ a, a, -b],
[ a, a, b], [-a, a, -b], [-a, a, b], [-a, -a, -b],
]
thrustDirs = np.array([
[ 1, 0, 0], [-1, 0, 0], [ 0, 1, 0], [ 0, -1, 0],
[-1, 0, 0], [ 1, 0, 0], [ 0, -1, 0], [ 0, 1, 0],
])
rwFactory = simIncludeRW.rwFactory()
thrFactory = simIncludeThruster.thrusterFactory()
# Generating RW and thruster XML chains
rwBodies, rwActs, RWs = addRWsXML(rwPos, rwAxes, rwFactory)
thrSites, thrActs, THRs = addThrustersXML(thrustLocs, thrustDirs, thrFactory)
# XML lengths are [m], masses [kg], inertia components [kg m^2], and angles [rad].
xml = f"""<mujoco model = "busWithRWsAndThrusters">
<compiler angle = "radian" meshdir = ""/>
<worldbody>
<body name = "hub" pos = "0 0 0">
<freejoint name = "busFree"/>
<inertial pos = "0 0 {b}" mass = "{hubMass}" diaginertia = "{hubIxx} {hubIyy} {hubIzz}"/>
<geom name = "hubVisual" type = "box" size = "1 1 1.28" rgba = "1 1 1 1"/>
{thrSites}
{rwBodies}
</body>
</worldbody>
<actuator>
{rwActs}
{thrActs}
</actuator>
</mujoco>"""
return xml, RWs, THRs, rwFactory, thrFactory
[docs]
def run(showPlots: bool = False):
"""Run five minutes of wheel attitude control and requested desaturation.
:param showPlots: Display the Matplotlib figures and wait for the plot
windows to close when True. Defaults to False; figures are still
generated and returned in either case.
:returns: Dictionary mapping scenario-prefixed names to eight Matplotlib
figures: attitude and rate errors, wheel momenta, requested momentum
change, wheel speeds, thruster impulses, on-times, and forces.
No files are saved by this function.
:rtype: dict
"""
# -------------------------------------------------------------------------
# 1) Simulation configuration and MJScene dynamics model
# -------------------------------------------------------------------------
fswTaskName = "fswTask"
dynTaskName = "dynTask"
recordTaskName = "recordTask"
simProcessName = "simProcess"
# Initializing simulation time/time-steps for dynamics/fsw task
simulationTime = macros.min2nano(5) # [ns]
simulationTimeStepFsw = macros.sec2nano(1) # [ns]
simulationTimeStepDyn = macros.sec2nano(0.1) # [ns]
sim = SimulationBaseClass.SimBaseClass()
dynProcess = sim.CreateNewProcess(simProcessName)
# Run dynamics and navigation, then FSW, then record the current outputs.
dynProcess.addTask(sim.CreateNewTask(dynTaskName, simulationTimeStepDyn), 3)
dynProcess.addTask(sim.CreateNewTask(fswTaskName, simulationTimeStepFsw), 2)
dynProcess.addTask(sim.CreateNewTask(recordTaskName, simulationTimeStepDyn), 1)
# Constructing MJ XML string (hub + 4 RWs + 8 thrusters) and loading into MJScene
xmlString, RWs, THRs, rwFactory, thrFactory = makeMjXmlString()
scene = mujoco.MJScene(xmlString)
scene.ModelTag = "mujocoScene"
scene.extraEoMCall = True # lets custom sys models step alongside MuJoCo's EoM evaluation
sim.AddModelToTask(dynTaskName, scene)
# -------------------------------------------------------------------------
# 2) Retrieve spacecraft components
# -------------------------------------------------------------------------
# Pull handles of hub/RW bodies and actuators from XML
busBody = scene.getBody("hub")
numRWs = len(RWs)
numTHRs = len(THRs)
RWBodies = [scene.getBody(f"rw{i + 1}Spin") for i in range(numRWs)]
RWJoints = [RWBodies[i].getScalarJoint(f"rw{i + 1}Joint") for i in range(numRWs)]
RWActuators = [scene.getSingleActuator(f"rw{i + 1}Act") for i in range(numRWs)]
THRActuators = [scene.getSingleActuator(f"thruster{i + 1}") for i in range(numTHRs)]
# -------------------------------------------------------------------------
# 3) Add gravity
# -------------------------------------------------------------------------
# Adding N-Body gravity model into MJScene
gravity = NBodyGravity.NBodyGravity()
gravity.ModelTag = "gravity"
muEarth = 0.3986004415e15 # [m^3/s^2]
earthPm = pointMassGravityModel.PointMassGravityModel()
earthPm.muBody = muEarth
gravity.addGravitySource("earth", earthPm, isCentralBody = True)
# Gravity effects applied to hub only (RWs treated as rigidly attached, negligible mass)
gravity.addGravityTarget("hub", busBody)
scene.AddModelToDynamicsTask(gravity)
# -------------------------------------------------------------------------
# 4) Initial conditions
# -------------------------------------------------------------------------
oe = orbitalMotion.ClassicElements()
rLEO = 7000. * 1000 # meters
oe.a = rLEO
oe.e = 0.0001 # [-]
oe.i = 33.3 * macros.D2R # [rad]
oe.Omega = 148.2 * macros.D2R # [rad]
oe.omega = 347.8 * macros.D2R # [rad]
oe.f = 335 * macros.D2R # [rad]
rN, vN = orbitalMotion.elem2rv(muEarth, oe)
# -------------------------------------------------------------------------
# 5) Navigation and FSW
# -------------------------------------------------------------------------
# Reading s/c state and publishing standard navigation output
simpleNavObj = simpleNav.SimpleNav()
simpleNavObj.ModelTag = "simpleNav"
simpleNavObj.scStateInMsg.subscribeTo(busBody.getCenterOfMass().stateOutMsg)
sim.AddModelToTask(dynTaskName, simpleNavObj)
# Fixed inertial pointing target
inertial3DObj = inertial3D.inertial3D()
inertial3DObj.ModelTag = "inertial3D"
inertial3DObj.sigma_R0N = [0.0, 0.0, 0.0]
sim.AddModelToTask(fswTaskName, inertial3DObj)
# Tracks attitude error of s/c
attError = attTrackingError.attTrackingError()
attError.ModelTag = "attErrorInertial3D"
attError.attNavInMsg.subscribeTo(simpleNavObj.attOutMsg)
attError.attRefInMsg.subscribeTo(inertial3DObj.attRefOutMsg)
sim.AddModelToTask(fswTaskName, attError)
# MRP feedback control law computing the commanded control torque
mrpControl = mrpFeedback.mrpFeedback()
mrpControl.ModelTag = "mrpFeedback"
sim.AddModelToTask(fswTaskName, mrpControl)
decayTime = 10.0 # s
xi = 1.0 # [-] Damping ratio.
I = np.diag([1700, 1700, 1800]) # [kg m^2]
mrpControl.Ki = -1 # make value negative to turn off integral feedback
mrpControl.P = 3 * np.max(I) / decayTime
mrpControl.K = (mrpControl.P/xi) * (mrpControl.P/xi) / np.max(I)
mrpControl.integralLimit = 2. / mrpControl.Ki * 0.1
controlAxes_B = [1, 0, 0, 0, 1, 0, 0, 0, 1]
# Add module that maps the Lr control torque into the RW motor torques
rwMotorTorqueObj = rwMotorTorque.rwMotorTorque()
rwMotorTorqueObj.ModelTag = "rwMotorTorque"
sim.AddModelToTask(fswTaskName, rwMotorTorqueObj)
# Make the RW control all three body axes
rwMotorTorqueObj.controlAxes_B = controlAxes_B
# Momentum dumping configuration
thrDesatControl = thrMomentumManagement.thrMomentumManagement()
thrDesatControl.ModelTag = "thrMomentumManagement"
sim.AddModelToTask(fswTaskName, thrDesatControl)
thrDesatControl.hs_min = 80 # [N m s] Target net wheel momentum after dumping.
# Map angular impulse [N m s] to individual thruster impulses [N s].
thrForceMappingObj = thrForceMapping.thrForceMapping()
thrForceMappingObj.ModelTag = "thrForceMapping"
sim.AddModelToTask(fswTaskName, thrForceMappingObj)
thrForceMappingObj.controlAxes_B = controlAxes_B
thrForceMappingObj.thrForceSign = 1
thrForceMappingObj.angErrThresh = 3.15 # [rad] Above pi to allow this momentum-dumping mapping.
# Setup the thruster momentum dumping module
thrDump = thrMomentumDumping.thrMomentumDumping()
thrDump.ModelTag = "thrDump"
sim.AddModelToTask(fswTaskName, thrDump)
thrDump.maxCounterValue = 100 # Number of control periods (simulationTimeStepFsw) to wait between two subsequent on-times
thrDump.thrMinFireTime = 0.02 # [s]
# FSW-facing config messages describing all 4 wheels / 8 thrusters
fswRwParamMsg = rwFactory.getConfigMessage()
fswThrParamMsg = thrFactory.getConfigMessage()
# -------------------------------------------------------------------------
# 6) Distribute torques/forces
# -------------------------------------------------------------------------
rwUMax = RWs[0].u_max
arrayToSingles = arrayMotorTorqueToSingleActuators.ArrayMotorTorqueToSingleActuators()
arrayToSingles.ModelTag = "arrayMotorTorqueToSingleActuators"
arrayToSingles.setNumActuators(numRWs)
arrayToSingles.torqueInMsg.subscribeTo(rwMotorTorqueObj.rwMotorTorqueOutMsg)
sim.AddModelToTask(fswTaskName, arrayToSingles)
torqueLimiters = []
for i in range(numRWs):
limiter = saturationSingleActuator.SaturationSingleActuator()
limiter.ModelTag = f"rw{i+1}TorqueLimiter"
limiter.setMinInput(-rwUMax)
limiter.setMaxInput(rwUMax)
limiter.actuatorInMsg.subscribeTo(arrayToSingles.actuatorOutMsgs[i])
sim.AddModelToTask(fswTaskName, limiter)
RWActuators[i].actuatorInMsg.subscribeTo(limiter.actuatorOutMsg)
torqueLimiters.append(limiter)
# thrOnTimeToForce used to convert on-time commands from FSW stack to instant on/off
# thruster actuator messages for MuJoCo thrusters
thrForceConverter = thrOnTimeToForce.ThrOnTimeToForce()
thrForceConverter.ModelTag = "thrOnTimeToForce"
for _ in range(numTHRs):
thrForceConverter.addThruster()
thrForceConverter.setThrMag([thr.MaxThrust for thr in THRs])
scene.AddModelToDynamicsTask(thrForceConverter)
thrForceConverter.onTimeInMsg.subscribeTo(thrDump.thrusterOnTimeOutMsg)
for i in range(numTHRs):
THRActuators[i].actuatorInMsg.subscribeTo(thrForceConverter.thrusterForceOutMsgs[i])
# -------------------------------------------------------------------------
# 6) Message Linking
# -------------------------------------------------------------------------
# Inertia tensor passed into config message, vehicle config created
vehicleConfigOut = messaging.VehicleConfigMsgPayload(ISCPntB_B = [1700,0,0, 0,1700,0, 0,0,1800]) # [kg m^2]
vcMsg = messaging.VehicleConfigMsg().write(vehicleConfigOut)
mrpControl.vehConfigInMsg.subscribeTo(vcMsg)
mrpControl.guidInMsg.subscribeTo(attError.attGuidOutMsg)
mrpControl.rwParamsInMsg.subscribeTo(fswRwParamMsg)
# scalarJointStatesToRWSpeed gathers individual per-joint wheel speeds from MuJoCo,
# republishes them as a single RWSpeedMsg
rwSpeedsAdapter = scalarJointStatesToRWSpeed.ScalarJointStatesToRWSpeed()
rwSpeedsAdapter.ModelTag = "scalarJointStatesToRWSpeed"
rwSpeedsAdapter.setNumJoints(numRWs)
for i in range(numRWs):
rwSpeedsAdapter.jointStateInMsgs[i].subscribeTo(RWJoints[i].stateDotOutMsg)
sim.AddModelToTask(fswTaskName, rwSpeedsAdapter, 100)
mrpControl.rwSpeedsInMsg.subscribeTo(rwSpeedsAdapter.rwSpeedOutMsg)
rwMotorTorqueObj.rwParamsInMsg.subscribeTo(fswRwParamMsg)
thrForceMappingObj.vehConfigInMsg.subscribeTo(vcMsg)
rwMotorTorqueObj.vehControlInMsg.subscribeTo(mrpControl.cmdTorqueOutMsg)
thrDesatControl.rwSpeedsInMsg.subscribeTo(rwSpeedsAdapter.rwSpeedOutMsg)
thrDesatControl.rwConfigDataInMsg.subscribeTo(fswRwParamMsg)
thrForceMappingObj.thrConfigInMsg.subscribeTo(fswThrParamMsg)
thrForceMappingObj.cmdTorqueInMsg.subscribeTo(thrDesatControl.deltaHOutMsg)
thrDump.thrusterConfInMsg.subscribeTo(fswThrParamMsg)
thrDump.deltaHInMsg.subscribeTo(thrDesatControl.deltaHOutMsg)
thrDump.thrusterImpulseInMsg.subscribeTo(thrForceMappingObj.thrForceCmdOutMsg)
# -------------------------------------------------------------------------
# 7) Set up data recording
# -------------------------------------------------------------------------
numDataPoints = 5000
samplingTime = simHelpers.samplingTime(simulationTime, simulationTimeStepDyn, numDataPoints)
sNavRec = simpleNavObj.attOutMsg.recorder(samplingTime)
sim.AddModelToTask(recordTaskName, sNavRec)
dataRec = busBody.getCenterOfMass().stateOutMsg.recorder(samplingTime)
sim.AddModelToTask(recordTaskName, dataRec)
rwMotorLog = rwMotorTorqueObj.rwMotorTorqueOutMsg.recorder(samplingTime)
sim.AddModelToTask(recordTaskName, rwMotorLog)
attErrorLog = attError.attGuidOutMsg.recorder(samplingTime)
sim.AddModelToTask(recordTaskName, attErrorLog)
deltaHLog = thrDesatControl.deltaHOutMsg.recorder(samplingTime)
sim.AddModelToTask(recordTaskName, deltaHLog)
thrMapLog = thrForceMappingObj.thrForceCmdOutMsg.recorder(samplingTime)
sim.AddModelToTask(recordTaskName, thrMapLog)
onTimeLog = thrDump.thrusterOnTimeOutMsg.recorder(samplingTime)
sim.AddModelToTask(recordTaskName, onTimeLog)
# Wheel speeds live as individual MuJoCo joint states, so log each separately and column-stack later for plotting
rwSpeedLogs = []
for i in range(numRWs):
rwSpeedLogs.append(RWJoints[i].stateDotOutMsg.recorder(samplingTime))
sim.AddModelToTask(recordTaskName, rwSpeedLogs[i])
# Delivered force at each thruster actuator, logged individually
thrForceLogs = []
for i in range(numTHRs):
thrForceLogs.append(THRActuators[i].actuatorInMsg.recorder(samplingTime))
sim.AddModelToTask(recordTaskName, thrForceLogs[i])
# -------------------------------------------------------------------------
# 8) Running simulation
# -------------------------------------------------------------------------
sim.InitializeSimulation()
# Setting initial conditions
busFree = busBody.getFreeJoint()
busBody.setPosition(rN)
busFree.setVelocity(vN)
# Initial wheel spin rates, chosen so the wheels start above the 80 Nms desat threshold
initialOmegas = [4000., 2000., 3500., 0.] # [RPM]
for i in range(numRWs):
omega_radps = initialOmegas[i] * macros.RPM
RWJoints[i].setVelocity(omega_radps)
# Coast for 10 s before requesting a new momentum check.
sim.ConfigureStopTime(macros.sec2nano(10.0)) # [ns]
sim.ExecuteSimulation()
# The next FSW update at 11 s writes a fresh delta-H request. A request
# timestamped at zero is not recognized as new by thrMomentumDumping.
thrDesatControl.Reset(macros.sec2nano(10.0)) # [ns]
sim.ConfigureStopTime(simulationTime)
sim.ExecuteSimulation()
# -------------------------------------------------------------------------
# 9) Post processing and plotting
# -------------------------------------------------------------------------
dataSigmaBR = attErrorLog.sigma_BR
dataOmegaBR = attErrorLog.omega_BR_B
# Column-stack per-wheel speed logs into a single (numSamples x numRWs) array
dataOmegaRW = np.column_stack([np.squeeze(rwSpeedLogs[i].state) for i in range(numRWs)])
dataDH = deltaHLog.torqueRequestBody
dataMap = thrMapLog.thrForce
dataOnTime = onTimeLog.OnTimeRequest
# Gather delivered thruster force logs into list of arrays, one per thruster
dataThr = []
for i in range(numTHRs):
dataThr.append(thrForceLogs[i].input)
np.set_printoptions(precision=16)
timeData = rwMotorLog.times() * macros.NANO2MIN
# Generating plots
plt.close("all")
figureList = {}
figureList[fileName + "_attError"] = plot_attitude_error(timeData, dataSigmaBR)
figureList[fileName + "_rateError"] = plot_rate_error(timeData, dataOmegaBR)
figureList[fileName + "_rwMomenta"] = plot_rw_momenta(timeData, dataOmegaRW, RWs, numRWs)
figureList[fileName + "_DH"] = plot_DH(timeData, dataDH)
figureList[fileName + "_rwSpeeds"] = plot_rw_speeds(timeData, dataOmegaRW, numRWs)
figureList[fileName + "_thrImpulse"] = plot_thrImpulse(timeData, dataMap, numTHRs)
figureList[fileName + "_OnTimeReq"] = plot_OnTimeRequest(timeData, dataOnTime, numTHRs)
figureList[fileName + "_thrForce"] = plot_thrForce(timeData, dataThr, numTHRs)
if showPlots:
plt.show()
return figureList
if __name__ == "__main__":
run(showPlots = True)