[#1490] update existing mujoco examples to use gravity factor class

Author: schaubh

examples/mujoco/scenarioAttitudeFeedbackRWMuJoCo.py

@@ -50,6 +50,11 @@
 #. ``saturationSingleActuator`` optionally clamps each ``SingleActuatorMsg`` to
    emulate actuator torque limits (for example, reaction wheel ``uMax``).
 
+Earth gravity is configured with :ref:`simIncludeGravBody`.  Calling
+``gravFactory.addBodiesTo(scene)`` creates the intermediate
+:ref:`NBodyGravity<NBodyGravity>` model and automatically registers the bus and
+all three reaction wheels as gravity targets.
+
 The simulation runs for 10 minutes. The attitude error, rate error, wheel
 motor torques, and wheel speeds are plotted at the end.
 
@@ -72,10 +77,8 @@
 
 from Basilisk.architecture import messaging
 from Basilisk.fswAlgorithms import attTrackingError, inertial3D, mrpFeedback, rwMotorTorque
-from Basilisk.simulation import NBodyGravity
 from Basilisk.simulation import arrayMotorTorqueToSingleActuators
 from Basilisk.simulation import mujoco
-from Basilisk.simulation import pointMassGravityModel
 from Basilisk.simulation import saturationSingleActuator
 from Basilisk.simulation import scalarJointStatesToRWSpeed
 from Basilisk.simulation import simpleNav
@@ -240,26 +243,16 @@ def run(showPlots: bool = False):
     rwActs = [scene.addJointSingleActuator(f"rw{i+1}Act", rwJoints[i]) for i in range(numRw)]
 
     # -------------------------------------------------------------------------
-    # 5) Add gravity to the special MuJoCo dynamics task (evaluated at integrator substeps)
+    # 5) Add Earth gravity to every MuJoCo body through the gravity factory
     # -------------------------------------------------------------------------
-    gravity = NBodyGravity.NBodyGravity()
-    gravity.ModelTag = "gravity"
-    scene.AddModelToDynamicsTask(gravity)
-
     gravFactory = simIncludeGravBody.gravBodyFactory()
     earth = gravFactory.createEarth()
     earth.isCentralBody = True
-    scene._vizGravBodies = gravFactory.gravBodies
-
     muEarth = earth.mu  # [m^3/s^2]
-    earthPm = pointMassGravityModel.PointMassGravityModel()
-    earthPm.muBody = muEarth
-    gravity.addGravitySource("earth", earthPm, isCentralBody=True)
 
-    # Apply gravity to all bodies to allow consistent orbital motion.
-    gravity.addGravityTarget("bus", busBody)
-    for i in range(numRw):
-        gravity.addGravityTarget(f"rw{i+1}", rwBodies[i])
+    # This creates the NBodyGravity model, adds Earth as its central source, and
+    # registers the bus and all three reaction wheels as gravity targets.
+    gravFactory.addBodiesTo(scene)
 
     # -------------------------------------------------------------------------
     # 6) Navigation: read the bus state from MJScene and publish standard nav outputs

examples/mujoco/scenarioFormationFlyingWithDrag.py

@@ -24,8 +24,9 @@
 as a single point-mass body with three translational sliding joints (x, y, z).
 Their inertial properties are assigned directly in the XML.
 
-Gravity is modeled using the :ref:`NBodyGravity` model, with a point-mass
-Earth defined by :ref:`pointMassGravityModel`. The exponential atmospheric model
+Gravity is configured with :ref:`simIncludeGravBody`.  Its gravity factory
+creates a point-mass Earth and attaches it to the MuJoCo scene, automatically
+applying gravity to both spacecraft.  The exponential atmospheric model
 (:ref:`exponentialAtmosphere`) is optionally added so that the chief and deputy
 can experience cannonball drag.
 
@@ -52,7 +53,7 @@
 
 * How to construct a multi-body MuJoCo scene with two independent spacecraft
 * How to configure translation-only scenarios
-* How to model gravity on multiple bodies using the :ref:`NBodyGravity` model
+* How to configure gravity on multiple MuJoCo bodies using the gravity factory
 * How to apply aerodynamic forces
 * How to configure and apply a classical-element feedback controller for formation control
 * How to record and visualize inertial trajectories, orbital-element histories,
@@ -91,8 +92,6 @@
 from Basilisk.architecture import messaging
 from Basilisk.simulation import mujoco
 from Basilisk.simulation import svIntegrators
-from Basilisk.simulation import pointMassGravityModel
-from Basilisk.simulation import NBodyGravity
 from Basilisk.simulation import cannonballDrag
 from Basilisk.simulation import MJLinearTimeInvariantSystem
 from Basilisk.simulation import exponentialAtmosphere
@@ -160,7 +159,6 @@ class SimulationModels:
     scene: mujoco.MJScene
     chiefBody: mujoco.MJBody
     deputyBody: mujoco.MJBody
-    gravity: NBodyGravity.NBodyGravity
     atmo: exponentialAtmosphere.ExponentialAtmosphere
 
     # Drag models
@@ -325,7 +323,6 @@ def createSimulationModels(
     # Create the MuJoCo scene and add it as a dynamic object
     scene = mujoco.MJScene(CHIEF_DEPUTY_SCENE_XML)
     scene.extraEoMCall = True
-    scene._vizGravBodies = gravFactory.gravBodies
     scSim.AddModelToTask("test", scene, 1)
 
     # Select integrator
@@ -338,16 +335,9 @@ def createSimulationModels(
     chiefBody: mujoco.MJBody = scene.getBody("chief")
     deputyBody: mujoco.MJBody = scene.getBody("deputy")
 
-    # Central body gravity
-    gravity = NBodyGravity.NBodyGravity()
-    gravity.ModelTag = "gravity"
-    scene.AddModelToDynamicsTask(gravity)
-
-    gravityModel = pointMassGravityModel.PointMassGravityModel()
-    gravityModel.muBody = planet.mu
-    gravity.addGravitySource("earth", gravityModel, True)
-    gravity.addGravityTarget("chief", chiefBody)
-    gravity.addGravityTarget("deputy", deputyBody)
+    # The factory creates the NBodyGravity model, adds Earth as its central
+    # source, and registers both spacecraft as gravity targets.
+    gravFactory.addBodiesTo(scene)
 
     # Exponential atmosphere model
     atmo = exponentialAtmosphere.ExponentialAtmosphere()
@@ -364,7 +354,6 @@ def createSimulationModels(
         scene=scene,
         chiefBody=chiefBody,
         deputyBody=deputyBody,
-        gravity=gravity,
         atmo=atmo,
     )
 

examples/mujoco/scenarioMJSceneVizard.py

@@ -50,8 +50,6 @@
 from Basilisk.architecture import messaging
 from Basilisk.simulation import coarseSunSensor
 from Basilisk.simulation import mujoco
-from Basilisk.simulation import NBodyGravity
-from Basilisk.simulation import pointMassGravityModel
 from Basilisk.simulation import spacecraft
 from Basilisk.simulation import StatefulSysModel
 from Basilisk.simulation import svIntegrators
@@ -197,29 +195,6 @@ def setStowed(scene, panelIds, bodyPrefix):
         ).setPosition(JOINT_START_END[i - 1][0])
 
 
-def addOrbitalGravity(scene, mu, modelCache):
-    """Apply central-body point-mass gravity to every body in ``scene``.
-
-    Uses :ref:`NBodyGravity` with one gravity target per body (hub plus each
-    deployable panel), so the field is evaluated and applied at each body's true
-    center of mass.  The whole structure is then in physical orbital freefall —
-    the appendages are weightless — while the gravity gradient across it is
-    captured.
-    """
-    gravity = NBodyGravity.NBodyGravity()
-    gravity.ModelTag = f"{scene.ModelTag}_gravity"
-    scene.AddModelToDynamicsTask(gravity)
-
-    gravityModel = pointMassGravityModel.PointMassGravityModel()
-    gravityModel.muBody = mu
-    gravity.addGravitySource("earth", gravityModel, True)
-    for name in scene.getBodyNames():
-        gravity.addGravityTarget(name, scene.getBody(name))
-
-    modelCache.extend([gravity, gravityModel])
-    return gravity
-
-
 # Main scenario
 def run(showPlots: bool = False):
     scSim = SimulationBaseClass.SimBaseClass()
@@ -264,11 +239,16 @@ def run(showPlots: bool = False):
         epochInMsg=True,
     )
     spiceObject.zeroBase = "Earth"
-    scSim.AddModelToTask("simTask", spiceObject)
 
-    # vizSupport dedupes planets by name across scenes.
-    primaryScene._vizGravBodies = gravFactory.gravBodies
-    companionScene._vizGravBodies = gravFactory.gravBodies
+    # MuJoCo gravity is evaluated at every integrator substep. Run SPICE after
+    # the scene's high-priority forward kinematics and before gravity (default
+    # priority -1), so every gravity evaluation reads current ephemerides.
+    spiceDynamicsPriority = 75
+    primaryScene.AddModelToDynamicsTask(spiceObject, spiceDynamicsPriority)
+    companionScene.AddModelToDynamicsTask(spiceObject, spiceDynamicsPriority)
+
+    # The regular spacecraft below also needs SPICE at the top-level task rate.
+    scSim.AddModelToTask("simTask", spiceObject)
 
     # Trailer is a regular Basilisk Spacecraft, behind the primary along-track.
     # Uses ``gravFactory.addBodiesTo`` for the standard gravField wiring.
@@ -301,13 +281,13 @@ def run(showPlots: bool = False):
     addDeployment(companionScene, companionPanels, "companion_panel", models)
 
     # Gravity setup.  Each MJScene gets its own NBodyGravity model that applies
-    # central-body gravity as a force at every body's center of mass (hub plus
-    # each panel), preserving the gravity gradient across the structure.
+    # the factory's Earth, Moon, and Sun gravity sources at every body's center
+    # of mass, preserving the gravity gradient across each structure.
     hubBody = primaryScene.getBody("hub")
     companionBody = companionScene.getBody("companion_hub")
 
-    addOrbitalGravity(primaryScene, mu, models)
-    addOrbitalGravity(companionScene, mu, models)
+    gravFactory.addBodiesTo(primaryScene)
+    gravFactory.addBodiesTo(companionScene)
 
     # CSS sensors mounted on the primary hub. The MJScene body's origin state
     # message subscribes the same way a regular Spacecraft body state message

examples/mujoco/scenarioStochasticDrag.py

@@ -25,9 +25,10 @@
 density model. The script illustrates how one defines and handles dynamic states
 that are driven by stochastic terms.
 
-The spacecraft definition is given in ``CANNONBALL_SCENE_XML``; it contains a single
-body for which we set its mass directly. We use the :ref:`NBodyGravity` model to compute
-the gravity acting on this body due to a point-mass Earth.
+The spacecraft definition is given in ``CANNONBALL_SCENE_XML``; it contains a
+single body for which we set its mass directly.  A
+:ref:`simIncludeGravBody` gravity factory creates the point-mass Earth and
+attaches it to the MuJoCo scene.
 
 The drag force acting on the body is computed in the :ref:`CannonballDrag<CannonballDrag>`
 module. This takes as input the state of the spacecraft (so that
@@ -137,8 +138,6 @@
 from Basilisk.architecture import messaging
 from Basilisk.simulation import mujoco
 from Basilisk.simulation import svIntegrators
-from Basilisk.simulation import pointMassGravityModel
-from Basilisk.simulation import NBodyGravity
 from Basilisk.simulation import exponentialAtmosphere
 from Basilisk.simulation import cannonballDrag
 from Basilisk.simulation import MJStochasticAtmDensity
@@ -211,7 +210,6 @@ def run(showPlots: bool = False, useIgbm: bool = False):
     # Create the Mujoco scene (our MuJoCo DynamicObject)
     # Load the XML file that defines the system from a file
     scene = mujoco.MJScene(CANNONBALL_SCENE_XML)
-    scene._vizGravBodies = gravFactory.gravBodies
     scSim.AddModelToTask("test", scene)
 
     # Set a stochastic integrator on the DynamicObject, necessary since we have
@@ -231,18 +229,10 @@ def run(showPlots: bool = False, useIgbm: bool = False):
     body: mujoco.MJBody = scene.getBody("ball")
     dragPoint: mujoco.MJSite = body.getCenterOfMass()
 
-    ### Create the NBodyGravity model
-    # add model to the dynamics task of the scene
-    gravity = NBodyGravity.NBodyGravity()
-    scene.AddModelToDynamicsTask(gravity)
-
-    # Create a point-mass gravity source
-    gravityModel = pointMassGravityModel.PointMassGravityModel()
-    gravityModel.muBody = planet.mu
-    gravity.addGravitySource("earth", gravityModel, True)
-
-    # Create a gravity target from the mujoco body
-    gravity.addGravityTarget("ball", body)
+    ### Add Earth gravity to the MuJoCo scene
+    # The factory creates the NBodyGravity model and registers the cannonball
+    # body as a gravity target.
+    gravFactory.addBodiesTo(scene)
 
     ### Create the density model
     atmo = exponentialAtmosphere.ExponentialAtmosphere()