[#1490] Add gravity factory support for MuJoCo scenes

Extend gravBodyFactory.addBodiesTo() to configure MJScene objects in
addition to conventional GravityEffector owners. For each MuJoCo scene,
create an NBodyGravity model, register every factory body as a gravity
source, and apply gravity independently at every MuJoCo body's center
of mass. Retain the model and Vizard gravity metadata on the scene and
reject duplicate gravity attachment.

Allow NBodyGravity sources to retain a shared GravBodyData descriptor.
Resolve the gravity model, central-body flag, and ephemeris input from
the descriptor so the same configuration can be shared by conventional
spacecraft and MuJoCo dynamics. Initialize descriptor-backed gravity
models during Reset and warn when an orientation-dependent model has no
ephemeris input.

Expose descriptor-backed sources to Python as
addGravitySourceFromBody(). Keep the existing manual
addGravitySource(..., isCentralBody=...) interface unchanged, including
support for its keyword argument.

This enables gravity models and SPICE connections created by
simIncludeGravBody to be reused directly by MuJoCo scenes without
duplicating their configuration.

Author: schaubh

src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.cpp

@@ -20,44 +20,105 @@
 #include "NBodyGravity.h"
 
 #include "architecture/utilities/rigidBodyKinematics.h"
+#include "simulation/dynamics/_GeneralModuleFiles/gravityEffector.h"
+
+namespace {
+std::shared_ptr<GravityModel>
+getGravityModel(const GravitySource& source)
+{
+    return source.gravBody ? source.gravBody->gravityModel : source.model;
+}
+
+bool
+isCentral(const GravitySource& source)
+{
+    return source.gravBody ? source.gravBody->isCentralBody : source.isCentralBody;
+}
+
+bool
+isStateLinked(GravitySource& source)
+{
+    return source.stateInMsg.isLinked()
+           || (source.gravBody && source.gravBody->planetBodyInMsg.isLinked());
+}
+
+SpicePlanetStateMsgPayload
+readState(GravitySource& source)
+{
+    if (source.stateInMsg.isLinked())
+    {
+        return source.stateInMsg();
+    }
+    if (source.gravBody && source.gravBody->planetBodyInMsg.isLinked())
+    {
+        return source.gravBody->planetBodyInMsg();
+    }
+    throw BasiliskError("Cannot read an unlinked NBodyGravity source ephemeris message.");
+}
+} // namespace
 
 void
 NBodyGravity::Reset(uint64_t CurrentSimNanos)
 {
-    std::string errorPreffix = "In NBodyGravity '" + ModelTag + "': ";
+    std::string errorPrefix = "In NBodyGravity '" + ModelTag + "': ";
     for (auto&& [name, target] : targets)
     {
         if (!target.centerOfMassStateInMsg.isLinked())
         {
-            bskLogger.bskError("%sTarget '%s' centerOfMassStateInMsg is not linked!", errorPreffix.c_str(), name.c_str());
+            bskLogger.bskError("%sTarget '%s' centerOfMassStateInMsg is not linked!", errorPrefix.c_str(), name.c_str());
         }
 
         if (!target.massPropertiesInMsg.isLinked())
         {
-            bskLogger.bskError("%sTarget '%s' massPropertiesInMsg is not linked!", errorPreffix.c_str(), name.c_str());
+            bskLogger.bskError("%sTarget '%s' massPropertiesInMsg is not linked!", errorPrefix.c_str(), name.c_str());
         }
     }
 
     bool foundCentralSource = false;
 
     for (auto&& [name, source] : sources)
     {
-        if (sources.size() > 1 && !source.stateInMsg.isLinked())
+        if (sources.size() > 1 && !isStateLinked(source))
+        {
+            bskLogger.bskError(
+                "%sSource '%s' has no ephemeris input but there are multiple gravity sources!",
+                errorPrefix.c_str(),
+                name.c_str());
+        }
+
+        auto gravityModel = getGravityModel(source);
+        if (!gravityModel)
         {
-            bskLogger.bskError("%sSource '%s' stateInMsg is not linked but there are more than one sources!", errorPreffix.c_str(), name.c_str());
+            bskLogger.bskError("%sSource '%s' has no gravity model!", errorPrefix.c_str(), name.c_str());
         }
 
-        auto error = source.model->initializeParameters();
+        auto error = source.gravBody
+            ? gravityModel->initializeParameters(*source.gravBody)
+            : gravityModel->initializeParameters();
         if (error)
         {
-            bskLogger.bskError("%sWhile initializing source '%s' gravity model: %s", errorPreffix.c_str(), name.c_str(), error->c_str());
+            bskLogger.bskError(
+                "%sWhile initializing source '%s' gravity model: %s",
+                errorPrefix.c_str(),
+                name.c_str(),
+                error->c_str());
         }
 
-        if (source.isCentralBody)
+        if (gravityModel->dependsOnOrientation() && !isStateLinked(source))
+        {
+            bskLogger.bskLog(
+                BSK_WARNING,
+                "%sSource '%s' uses an orientation-dependent gravity model, but no "
+                "ephemeris input is connected. The body will be treated as non-rotating.",
+                errorPrefix.c_str(),
+                name.c_str());
+        }
+
+        if (isCentral(source))
         {
             if (foundCentralSource)
             {
-                bskLogger.bskError("%sMore than one central body!", errorPreffix.c_str());
+                bskLogger.bskError("%sMore than one central body!", errorPrefix.c_str());
             }
             foundCentralSource = true;
         }
@@ -91,7 +152,27 @@ NBodyGravity::UpdateState(uint64_t CurrentSimNanos)
 GravitySource&
 NBodyGravity::addGravitySource(std::string name, std::shared_ptr<GravityModel> gravityModel, bool isCentralBody)
 {
-    auto[source, actuallyEmplaced] = this->sources.emplace(name, GravitySource{gravityModel, {}, isCentralBody});
+    auto[source, actuallyEmplaced] =
+        this->sources.emplace(name, GravitySource{gravityModel, {}, isCentralBody, nullptr});
+
+    if (!actuallyEmplaced)
+    {
+        bskLogger.bskError("Cannot use repeated source name: %s", name.c_str());
+    }
+
+    return source->second;
+}
+
+GravitySource&
+NBodyGravity::addGravitySource(std::string name, std::shared_ptr<GravBodyData> gravBody)
+{
+    if (!gravBody)
+    {
+        bskLogger.bskError("Cannot add source '%s' from a null GravBodyData.", name.c_str());
+    }
+
+    auto[source, actuallyEmplaced] =
+        this->sources.emplace(name, GravitySource{nullptr, {}, false, std::move(gravBody)});
 
     if (!actuallyEmplaced)
     {
@@ -149,21 +230,21 @@ NBodyGravity::addGravityTarget(std::string name, MJBody& body)
 Eigen::Vector3d
 NBodyGravity::computeAccelerationFromSource(GravitySource& source, Eigen::Vector3d r_J2000)
 {
-    // Orientation and positon of the gravity source in J2000
+    // Orientation and position of the gravity source in J2000
     Eigen::Matrix3d dcm_sourceFixedJ2000 = Eigen::Matrix3d::Identity();
-    Eigen::Vector3d r_sourceJ200= Eigen::Vector3d::Zero();
-    if (source.stateInMsg.isLinked())
+    Eigen::Vector3d r_sourceJ2000 = Eigen::Vector3d::Zero();
+    if (isStateLinked(source))
     {
-        auto spicePayload = source.stateInMsg();
-        dcm_sourceFixedJ2000 = c2DArray2EigenMatrix3d(spicePayload.J20002Pfix); // .transpose()
-        r_sourceJ200 = cArray2EigenVector3d(spicePayload.PositionVector);
+        auto spicePayload = readState(source);
+        dcm_sourceFixedJ2000 = c2DArray2EigenMatrix3d(spicePayload.J20002Pfix);
+        r_sourceJ2000 = cArray2EigenVector3d(spicePayload.PositionVector);
     }
 
     // Transform position in J2000 reference frame to source-fixed reference frame
-    Eigen::Vector3d r_sourceFix = dcm_sourceFixedJ2000 * (r_J2000 - r_sourceJ200);
+    Eigen::Vector3d r_sourceFix = dcm_sourceFixedJ2000 * (r_J2000 - r_sourceJ2000);
 
     // Compute the gravity acceleration on the source-fixed reference frame
-    Eigen::Vector3d grav_sourceFix = source.model->computeField(r_sourceFix);
+    Eigen::Vector3d grav_sourceFix = getGravityModel(source)->computeField(r_sourceFix);
 
     // Convert gravity to J2000 reference frame
     return dcm_sourceFixedJ2000.transpose() * grav_sourceFix;
@@ -200,13 +281,13 @@ NBodyGravity::computeAccelerationOnTarget(GravityTarget& target)
 
     for (auto&& [_, source] : sources)
     {
-        if (!source.isCentralBody) continue;
+        if (!isCentral(source)) continue;
 
         centralSourceExists = true;
 
-        if (!source.stateInMsg.isLinked()) break;
+        if (!isStateLinked(source)) break;
 
-        auto spicePayload = source.stateInMsg();
+        auto spicePayload = readState(source);
         r_centralSourceSpiceRef_N = cArray2EigenVector3d(spicePayload.PositionVector);
     }
 
@@ -228,7 +309,7 @@ NBodyGravity::computeAccelerationOnTarget(GravityTarget& target)
         // If there is a central body, and 'source' is not this one,
         // then we need to take into account the acceleration of the central
         // body due to the effect of this other source.
-        if (centralSourceExists && !source.isCentralBody)
+        if (centralSourceExists && !isCentral(source))
         {
             grav_N -= computeAccelerationFromSource(source, r_centralSourceSpiceRef_N);
         }

src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.h

@@ -34,16 +34,20 @@
 
 #include "simulation/mujocoDynamics/_GeneralModuleFiles/MJScene.h"
 
+class GravBodyData;
+
 /**
  * @brief Represents a gravity source in an N-body simulation.
  *
  * A gravity source is a celestial body or point mass that generates a gravitational field.
+ * It can be configured manually or from a shared ``GravBodyData`` descriptor.
  */
 struct GravitySource
 {
     std::shared_ptr<GravityModel> model; ///< The gravity model associated with the source.
     ReadFunctor<SpicePlanetStateMsgPayload> stateInMsg; ///< Input message providing the state of the source.
     bool isCentralBody; ///< Flag indicating whether this source is the central body.
+    std::shared_ptr<GravBodyData> gravBody; ///< Optional canonical gravity-body descriptor.
 };
 
 /**
@@ -102,6 +106,20 @@ class NBodyGravity : public SysModel
      */
     GravitySource& addGravitySource(std::string name, std::shared_ptr<GravityModel> gravityModel, bool isCentralBody);
 
+    /**
+     * @brief Adds a gravity source described by a ``GravBodyData`` object.
+     *
+     * The source retains the shared descriptor and reads its gravity model,
+     * central-body flag, and ephemeris input live. During ``Reset()``, the
+     * gravity model is initialized from the descriptor.
+     *
+     * @param name The name of the gravity source.
+     * @param gravBody The canonical gravity-body descriptor.
+     * @return A reference to the newly added ``GravitySource``.
+     * @throw BasiliskError If ``gravBody`` is null or the source name is duplicated.
+     */
+    GravitySource& addGravitySource(std::string name, std::shared_ptr<GravBodyData> gravBody);
+
     /**
      * @brief Adds a new gravity target to the simulation.
      *

src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.i

@@ -21,6 +21,7 @@
 %module NBodyGravity
 %{
    #include "NBodyGravity.h"
+   #include "simulation/dynamics/_GeneralModuleFiles/gravityEffector.h"
    #include "simulation/dynamics/_GeneralModuleFiles/dynamicObject.h"
    #include "simulation/mujocoDynamics/_GeneralModuleFiles/MJInterpolators.h"
 %}
@@ -34,6 +35,14 @@ from Basilisk.architecture.swig_common_model import *
 
 %import "simulation/mujocoDynamics/_GeneralModuleFiles/mujoco.i"
 %import "simulation/dynamics/_GeneralModuleFiles/gravityModel.i"
+%import "simulation/dynamics/gravityEffector/gravityEffector.i"
+
+// Keep the existing manual-source function non-overloaded in Python so SWIG
+// retains support for calls that use the ``isCentralBody`` keyword argument.
+%rename(addGravitySourceFromBody) NBodyGravity::addGravitySource(
+    std::string,
+    std::shared_ptr<GravBodyData>
+);
 
 %include "sys_model.i"
 %include "NBodyGravity.h"

src/utilities/simIncludeGravBody.py

@@ -203,18 +203,88 @@ def __init__(self, bodyNames: Iterable[str] = []):
 
     def addBodiesTo(
         self,
-        objectToAddTheBodies: Union[gravityEffector.GravityEffector, WithGravField],
-    ):
-        """Can be called with a GravityEffector or an object that has a gravField
-        variable to set the gravity bodies used in the object.
+        objectToAddTheBodies: Any,
+    ) -> Optional[Any]:
+        """Attach the factory's gravity bodies to a dynamics object.
+
+        A conventional dynamics object may be a ``GravityEffector`` or expose a
+        ``gravField`` attribute. When an ``MJScene`` is provided, this method
+        creates one ``NBodyGravity`` model, adds every factory body as a gravity
+        source, and adds every MuJoCo body as a gravity target.
+
+        Args:
+            objectToAddTheBodies (Any): Dynamics object that should receive the
+                factory's gravity bodies.
+
+        Returns:
+            Optional[Any]: The created ``NBodyGravity`` model for an ``MJScene``;
+                otherwise, ``None``.
+
+        Note:
+            When factory bodies use SPICE ephemerides, add the factory's
+            ``spiceObject`` to the ``MJScene`` dynamics task before the returned
+            ``NBodyGravity`` model. This keeps planet states current at each
+            MuJoCo integrator substep.
         """
-        bodies = gravityEffector.GravBodyVector(list(self.gravBodies.values()))
         if isinstance(objectToAddTheBodies, WithGravField):
             objectToAddTheBodies = objectToAddTheBodies.gravField
-        objectToAddTheBodies.setGravBodies(bodies)  # type: ignore
+
+        setGravBodies = getattr(objectToAddTheBodies, "setGravBodies", None)
+        if callable(setGravBodies):
+            bodies = gravityEffector.GravBodyVector(list(self.gravBodies.values()))
+            setGravBodies(bodies)
+            return None
+
+        return self._addBodiesToMJScene(objectToAddTheBodies)
+
+    def _addBodiesToMJScene(self, scene: Any) -> Any:
+        """Attach the factory's gravity bodies to a MuJoCo scene."""
+        try:
+            from Basilisk.simulation import NBodyGravity
+            from Basilisk.simulation import mujoco
+        except ImportError as error:
+            raise TypeError(
+                "The supplied object does not support gravity-body attachment, "
+                "and this Basilisk build does not include MuJoCo."
+            ) from error
+
+        if not isinstance(scene, mujoco.MJScene):
+            raise TypeError(
+                "addBodiesTo() requires a GravityEffector, an object with a "
+                "gravField attribute, or an MJScene."
+            )
+
+        cacheAttribute = "_gravBodyFactoryNBodyGravity"
+        if getattr(scene, cacheAttribute, None) is not None:
+            raise ValueError(
+                f"Gravity bodies have already been added to MJScene '{scene.ModelTag}'."
+            )
+
+        gravity = NBodyGravity.NBodyGravity()
+        gravity.ModelTag = (
+            f"{scene.ModelTag}_gravity" if scene.ModelTag else "mujocoScene_gravity"
+        )
+
+        for name, gravBody in self.gravBodies.items():
+            gravity.addGravitySourceFromBody(name, gravBody)
+
+        for name in scene.getBodyNames():
+            gravity.addGravityTarget(name, scene.getBody(name))
+
+        scene.AddModelToDynamicsTask(gravity)
+
+        # MJScene stores raw model pointers in its dynamics task, so retain the
+        # Python model for at least as long as the scene.
+        setattr(scene, cacheAttribute, gravity)
+
+        # Match the standard spacecraft path, where Vizard discovers the
+        # attached gravity bodies through the dynamics object.
+        scene._vizGravBodies = self.gravBodies
+
+        return gravity
 
     # Note, in the `create` functions below the `isCentralBody` and `useSphericalHarmParams` are
-    # all set to False in the `GravGodyData()` constructor.
+    # all set to False in the `GravBodyData()` constructor.
 
     def createBody(
         self, bodyData: Union[str, BodyData]