From e96dc04a011ffcef40c77aad94a7fc260125ef3f Mon Sep 17 00:00:00 2001 From: Hanspeter Schaub Date: Mon, 20 Jul 2026 13:26:37 -0600 Subject: [PATCH 1/8] [#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. --- .../NBodyGravity/NBodyGravity.cpp | 125 +++++++++++++++--- .../NBodyGravity/NBodyGravity.h | 18 +++ .../NBodyGravity/NBodyGravity.i | 9 ++ src/utilities/simIncludeGravBody.py | 84 +++++++++++- 4 files changed, 207 insertions(+), 29 deletions(-) diff --git a/src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.cpp b/src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.cpp index 6950fbab1cc..cafff3491df 100644 --- a/src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.cpp +++ b/src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.cpp @@ -20,21 +20,57 @@ #include "NBodyGravity.h" #include "architecture/utilities/rigidBodyKinematics.h" +#include "simulation/dynamics/_GeneralModuleFiles/gravityEffector.h" + +namespace { +std::shared_ptr +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, BSKLogger& bskLogger) +{ + if (source.stateInMsg.isLinked()) + { + return source.stateInMsg(); + } + if (source.gravBody && source.gravBody->planetBodyInMsg.isLinked()) + { + return source.gravBody->planetBodyInMsg(); + } + bskLogger.bskError("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()); } } @@ -42,22 +78,47 @@ NBodyGravity::Reset(uint64_t CurrentSimNanos) 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, 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 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, bskLogger); + 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, bskLogger); 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); } diff --git a/src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.h b/src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.h index da7741dc8f3..5e1185eb3dc 100644 --- a/src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.h +++ b/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 model; ///< The gravity model associated with the source. ReadFunctor stateInMsg; ///< Input message providing the state of the source. bool isCentralBody; ///< Flag indicating whether this source is the central body. + std::shared_ptr gravBody; ///< Optional canonical gravity-body descriptor. }; /** @@ -102,6 +106,20 @@ class NBodyGravity : public SysModel */ GravitySource& addGravitySource(std::string name, std::shared_ptr 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 gravBody); + /** * @brief Adds a new gravity target to the simulation. * diff --git a/src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.i b/src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.i index 48568d92d02..671b987d337 100644 --- a/src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.i +++ b/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 +); %include "sys_model.i" %include "NBodyGravity.h" diff --git a/src/utilities/simIncludeGravBody.py b/src/utilities/simIncludeGravBody.py index 0ab866d023a..4f30c81945b 100644 --- a/src/utilities/simIncludeGravBody.py +++ b/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] From fd83626ae2bfc5dcaa5360705ee318b7421226bb Mon Sep 17 00:00:00 2001 From: Hanspeter Schaub Date: Mon, 20 Jul 2026 13:27:22 -0600 Subject: [PATCH 2/8] [#1490] update the grav factory comments to be more descriptive to a novice user --- src/utilities/simIncludeGravBody.py | 298 ++++++++++++++++++++-------- 1 file changed, 211 insertions(+), 87 deletions(-) diff --git a/src/utilities/simIncludeGravBody.py b/src/utilities/simIncludeGravBody.py index 4f30c81945b..4f270eaac0d 100644 --- a/src/utilities/simIncludeGravBody.py +++ b/src/utilities/simIncludeGravBody.py @@ -14,22 +14,47 @@ # ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF # OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. -from pathlib import Path -from typing import Optional -from typing import Dict -from typing import Iterable -from typing import Union -from typing import overload -from typing import Sequence -from typing import List -from typing import Protocol -from typing import runtime_checkable -from typing import Any +"""Create gravity-body descriptors and connect them to Basilisk dynamics. + +The usual workflow is to create the bodies first, mark one body as central when +appropriate, and then attach the factory bodies to a dynamics object: + +.. code-block:: python + + from Basilisk.utilities import simIncludeGravBody + + gravFactory = simIncludeGravBody.gravBodyFactory() + earth = gravFactory.createEarth() + earth.isCentralBody = True + gravFactory.addBodiesTo(spacecraftObject) + +The same ``addBodiesTo()`` call accepts a MuJoCo ``MJScene``. In that case the +factory creates the intermediate ``NBodyGravity`` model and connects every +MuJoCo body as a gravity target. + +Call ``createSpiceInterface()`` after creating the gravity bodies. The factory +then connects each body's ephemeris input to the corresponding SPICE output. +""" + +import os from dataclasses import dataclass +from pathlib import Path +from typing import ( + Any, + Dict, + Iterable, + List, + Optional, + Protocol, + Sequence, + Union, + overload, + runtime_checkable, +) from Basilisk import __path__ -from Basilisk.architecture import messaging from Basilisk.architecture import astroConstants +from Basilisk.architecture import messaging from Basilisk.simulation import gravityEffector from Basilisk.simulation import spiceInterface from Basilisk.simulation.gravityEffector import ( @@ -41,12 +66,9 @@ from Basilisk.utilities import simHelpers from Basilisk.utilities.deprecated import deprecationWarn -from Basilisk.utilities.supportDataTools.dataFetcher import get_path, DataFile, POOCH - -# this statement is needed to enable Windows to print ANSI codes in the Terminal -# see https://stackoverflow.com/questions/287871/how-to-print-colored-text-in-terminal-in-python/3332860#3332860 -import os +from Basilisk.utilities.supportDataTools.dataFetcher import DataFile, POOCH, get_path +# Enable ANSI color codes in the Windows terminal. os.system("") @@ -76,6 +98,8 @@ class BodyData: radiusRatio: float = 1 +# ``astroConstants`` stores gravitational parameters in km^3/s^2 and radii in +# km. The multipliers below convert them to the SI units used by the dynamics. BODY_DATA = { "sun": BodyData( identifier="sun", @@ -181,18 +205,36 @@ class BodyData: @runtime_checkable class WithGravField(Protocol): - gravField: Any # cannot be GravityEffector because SWIG classes dont give type info + # SWIG classes do not expose enough static type information to name the + # concrete gravity-effector type here. Runtime protocol checking lets the + # factory recognize objects such as ``spacecraft.Spacecraft`` by interface. + gravField: Any + + +@runtime_checkable +class WithSetGravBodies(Protocol): + def setGravBodies(self, gravBodies: Any) -> None: + ... def _ensure_trailing_sep(path: str) -> str: + """Return a directory path in the format expected by the SPICE C API.""" path = str(path) return path if path.endswith(os.sep) else path + os.sep class gravBodyFactory: - """Class to create gravitational bodies.""" + """Create and retain the gravity bodies used by a simulation. + + ``gravBodies`` is the factory's canonical dictionary of + ``GravBodyData`` descriptors. A descriptor can be shared by conventional + spacecraft dynamics, MuJoCo dynamics, SPICE, and Vizard without copying its + gravity model or configuration. + """ def __init__(self, bodyNames: Iterable[str] = []): + # Body names and any supplied body-fixed frames are kept in insertion + # order because SPICE output messages are connected in that same order. self.spicePlanetFrames: List[str] = [] self.gravBodies: Dict[str, gravityEffector.GravBodyData] = {} self.spiceObject: Optional[spiceInterface.SpiceInterface] = None @@ -220,25 +262,36 @@ def addBodiesTo( 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. + Raises: + TypeError: If the supplied object is not a supported dynamics object. + ValueError: If gravity was already attached to the ``MJScene``. + + .. note:: + + When factory bodies use SPICE ephemerides, add the factory's + ``spiceObject`` to the ``MJScene`` dynamics task before calling this + method. This keeps planet states current at each MuJoCo integrator + substep. """ + # Standard dynamics objects such as ``Spacecraft`` own a gravity + # effector in ``gravField``. Accepting either the owner or the effector + # keeps the long-standing ``addBodiesTo()`` calling convention. if isinstance(objectToAddTheBodies, WithGravField): objectToAddTheBodies = objectToAddTheBodies.gravField - setGravBodies = getattr(objectToAddTheBodies, "setGravBodies", None) - if callable(setGravBodies): + # GravityEffectors expose ``setGravBodies``. Checking for that protocol + # first preserves the conventional path without importing MuJoCo. + if isinstance(objectToAddTheBodies, WithSetGravBodies): bodies = gravityEffector.GravBodyVector(list(self.gravBodies.values())) - setGravBodies(bodies) + objectToAddTheBodies.setGravBodies(bodies) return None return self._addBodiesToMJScene(objectToAddTheBodies) def _addBodiesToMJScene(self, scene: Any) -> Any: - """Attach the factory's gravity bodies to a MuJoCo scene.""" + """Create and attach an ``NBodyGravity`` model for a MuJoCo scene.""" + # MuJoCo is an optional Basilisk build feature. Import it only after the + # conventional gravity-effector path has been ruled out. try: from Basilisk.simulation import NBodyGravity from Basilisk.simulation import mujoco @@ -254,6 +307,8 @@ def _addBodiesToMJScene(self, scene: Any) -> Any: "gravField attribute, or an MJScene." ) + # Attaching twice would duplicate every gravity force actuator. Keep a + # scene-side marker so that the error is caught before modifying it. cacheAttribute = "_gravBodyFactoryNBodyGravity" if getattr(scene, cacheAttribute, None) is not None: raise ValueError( @@ -265,9 +320,15 @@ def _addBodiesToMJScene(self, scene: Any) -> Any: f"{scene.ModelTag}_gravity" if scene.ModelTag else "mujocoScene_gravity" ) + # A factory body is a gravity *source*. The descriptor overload lets + # NBodyGravity read its model, central-body flag, and SPICE input + # directly from the same GravBodyData used by standard spacecraft. for name, gravBody in self.gravBodies.items(): gravity.addGravitySourceFromBody(name, gravBody) + # Gravity is applied independently at each MuJoCo body's center of + # mass. This preserves gravity-gradient forces across articulated or + # otherwise extended systems. for name in scene.getBodyNames(): gravity.addGravityTarget(name, scene.getBody(name)) @@ -283,8 +344,9 @@ def _addBodiesToMJScene(self, scene: Any) -> Any: return gravity - # Note, in the `create` functions below the `isCentralBody` and `useSphericalHarmParams` are - # all set to False in the `GravBodyData()` constructor. + # ``GravBodyData`` starts with a point-mass gravity model and is not a + # central body. Users should set ``isCentralBody = True`` on the appropriate + # body and may replace the point-mass model before simulation initialization. def createBody( self, bodyData: Union[str, BodyData] @@ -300,15 +362,19 @@ def createBody( gravityEffector.GravBodyData: The body object with corresponding data. """ if not isinstance(bodyData, BodyData): + # Accept both ``"mars barycenter"`` and ``"mars_barycenter"`` to + # match common SPICE-name spelling in user scripts. key = bodyData.lower().replace("_", " ") if key not in BODY_DATA: raise ValueError( - f"Body {bodyData} is unkown. " + f"Body {bodyData} is unknown. " f"Valid options are: {', '.join(BODY_DATA)}" ) bodyData = BODY_DATA[key] + # The descriptor owns the gravity configuration; dynamics objects only + # retain a shared reference to it when ``addBodiesTo()`` is called. body = gravityEffector.GravBodyData() body.planetName = bodyData.planetName body.displayName = bodyData.displayName @@ -316,6 +382,9 @@ def createBody( body.mu = bodyData.mu body.radEquator = bodyData.radEquator body.radiusRatio = bodyData.radiusRatio + + # Dictionary insertion order becomes the default SPICE planet-output + # order. ``spicePlanetFrames`` is populated in the same order. self.gravBodies[bodyData.identifier] = body self.spicePlanetFrames.append(bodyData.spicePlanetFrame) return body @@ -333,6 +402,9 @@ def createBodies( Dict[str, gravityEffector.GravBodyData]: A dictionary of gravity body objects held by the gravity factory. """ + # Support either ``createBodies("earth", "moon")`` or + # ``createBodies(["earth", "moon"])`` without making users reshape + # their existing collection. for nameOrIterableOfNames in bodyNames: if isinstance(nameOrIterableOfNames, str): self.createBody(nameOrIterableOfNames) @@ -341,6 +413,9 @@ def createBodies( self.createBody(name) return self.gravBodies + # Named helpers below are intentionally thin wrappers. They provide + # discoverable IDE completion while keeping all initialization in + # ``createBody()``. def createSun(self): return self.createBody("sun") @@ -387,10 +462,11 @@ def createCustomGravObject( """Create a custom gravity body object. Args: - label (str): Gravity body name - mu (float): Gravity constant in m^3/s^2 - displayName (Optional[str], optional): Vizard celestial body name, if not - provided then planetFrame becomes the Vizard name. Defaults to None. + label (str): Gravity body name. + mu (float): Gravitational parameter in m^3/s^2. + displayName (Optional[str], optional): Vizard celestial-body name. If + omitted, Vizard uses the gravity body's ``planetName``. Defaults + to None. modelDictionaryKey (Optional[str], optional): Vizard model key name. If not set, then either the displayName or planetName is used to set the model. Defaults to None. @@ -398,12 +474,15 @@ def createCustomGravObject( Defaults to None. radiusRatio (Optional[float], optional): Ratio of the polar radius to the equatorial radius. Defaults to None. - planetFrame (Optional[str], optional): Name of the spice planet + planetFrame (Optional[str], optional): Name of the SPICE planet frame. Defaults to None. Returns: gravityEffector.GravBodyData: The body object with the given data. """ + # The GravBodyData constructor supplies the default point-mass gravity + # model. This method fills in the independent parameters used to + # initialize that model when the simulation starts. gravBody = gravityEffector.GravBodyData() gravBody.planetName = label gravBody.mu = mu @@ -417,11 +496,17 @@ def createCustomGravObject( gravBody.modelDictionaryKey = "" else: gravBody.modelDictionaryKey = modelDictionaryKey + + # Retaining the descriptor in ``gravBodies`` makes this custom body + # participate in later ``addBodiesTo()`` and SPICE setup calls. self.gravBodies[label] = gravBody if planetFrame is not None: self.spicePlanetFrames.append(planetFrame) return gravBody + # The two overloads document the supported positional and keyword-only + # calling styles for type checkers. The third definition contains the + # runtime implementation shared by both styles. @overload def createSpiceInterface( self, @@ -437,9 +522,11 @@ def createSpiceInterface( spicePlanetFrames: Optional[Sequence[str]] = None, epochInMsg: bool = False, ) -> spiceInterface.SpiceInterface: - """A convenience function to configure a NAIF Spice module for the simulation. - It connects the ``gravBodyData`` objects to the spice planet state messages. Thus, - it must be run after the ``gravBodyData`` objects are created. + """Configure a NAIF SPICE module for the simulation. + + This method connects the factory's ``GravBodyData`` objects to the + corresponding SPICE planet-state messages. It must therefore be called + after the gravity bodies are created. .. note:: @@ -459,21 +546,25 @@ def createSpiceInterface( Args: path (str): The absolute path to the folder that contains the kernels to be loaded. time (str): The time string in a format that SPICE recognizes. - spiceKernelFileNames (Iterable[str], optional): - A list of spice kernel file names including file extension. - Defaults to `['de430.bsp', 'naif0012.tls', 'de-403-masses.tpc', 'pck00010.tpc']`. + spiceKernelFileNames (Iterable[DataFile.EphemerisData], optional): + Support-data entries for the SPICE kernels to load. Defaults to + ``de430.bsp``, ``naif0012.tls``, ``de-403-masses.tpc``, and + ``pck00010.tpc``. spicePlanetNames (Optional[Sequence[str]], optional): - A list of planet names whose Spice data is loaded. If this is not provided, - Spice data is loaded for the bodies created with this factory object. Defaults to None. + Planet names whose SPICE data is loaded. If omitted, the method + uses the bodies created with this factory, in insertion order. + Defaults to None. spicePlanetFrames (Optional[Sequence[str]], optional): - A list of planet frame names to load in Spice. If this is not provided, - frames are loaded for the bodies created with this factory function. Defaults to None. + Planet frame names to load in SPICE. If omitted, the method uses + the frames recorded when the factory bodies were created. + Defaults to None. epochInMsg (bool, optional): - Flag to set an epoch input message for the spice interface. Defaults to False. + If True, create and connect an epoch input message. Defaults to + False. Returns: - spiceInterface.SpiceInterface: The generated SpiceInterface, which is the - same as the stored `self.spiceObject` + spiceInterface.SpiceInterface: The generated ``SpiceInterface``. + The same object is also available as ``self.spiceObject``. """ @overload @@ -492,9 +583,11 @@ def createSpiceInterface( spicePlanetFrames: Optional[Sequence[str]] = None, epochInMsg: bool = False, ) -> spiceInterface.SpiceInterface: - """A convenience function to configure a NAIF Spice module for the simulation. - It connects the ``gravBodyData`` objects to the spice planet state messages. Thus, - it must be run after the ``gravBodyData`` objects are created. + """Configure a NAIF SPICE module for the simulation. + + This method connects the factory's ``GravBodyData`` objects to the + corresponding SPICE planet-state messages. It must therefore be called + after the gravity bodies are created. Unless the ``path`` input is provided, the kernels are loaded from the folder: `%BSK_PATH%/supportData/EphemerisData/`, where `%BSK_PATH%` is replaced by @@ -518,21 +611,25 @@ def createSpiceInterface( Args: path (str): The absolute path to the folder that contains the kernels to be loaded. time (str): The time string in a format that SPICE recognizes. - spiceKernelFileNames (Iterable[str], optional): - A list of spice kernel file names including file extension. - Defaults to `['de430.bsp', 'naif0012.tls', 'de-403-masses.tpc', 'pck00010.tpc']`. + spiceKernelFileNames (Iterable[DataFile.EphemerisData], optional): + Support-data entries for the SPICE kernels to load. Defaults to + ``de430.bsp``, ``naif0012.tls``, ``de-403-masses.tpc``, and + ``pck00010.tpc``. spicePlanetNames (Optional[Sequence[str]], optional): - A list of planet names whose Spice data is loaded. If this is not provided, - Spice data is loaded for the bodies created with this factory object. Defaults to None. + Planet names whose SPICE data is loaded. If omitted, the method + uses the bodies created with this factory, in insertion order. + Defaults to None. spicePlanetFrames (Optional[Sequence[str]], optional): - A list of planet frame names to load in Spice. If this is not provided, - frames are loaded for the bodies created with this factory function. Defaults to None. + Planet frame names to load in SPICE. If omitted, the method uses + the frames recorded when the factory bodies were created. + Defaults to None. epochInMsg (bool, optional): - Flag to set an epoch input message for the spice interface. Defaults to False. + If True, create and connect an epoch input message. Defaults to + False. Returns: - spiceInterface.SpiceInterface: The generated SpiceInterface, which is the - same as the stored `self.spiceObject` + spiceInterface.SpiceInterface: The generated ``SpiceInterface``. + The same object is also available as ``self.spiceObject``. """ def createSpiceInterface( @@ -550,9 +647,11 @@ def createSpiceInterface( epochInMsg: bool = False, spiceKernalFileNames=None, ) -> spiceInterface.SpiceInterface: - """A convenience function to configure a NAIF Spice module for the simulation. - It connects the ``gravBodyData`` objects to the spice planet state messages. Thus, - it must be run after the ``gravBodyData`` objects are created. + """Configure a NAIF SPICE module for the simulation. + + This method connects the factory's ``GravBodyData`` objects to the + corresponding SPICE planet-state messages. It must therefore be called + after the gravity bodies are created. Unless the ``path`` input is provided, the kernels are loaded from the folder: `%BSK_PATH%/supportData/EphemerisData/`, where `%BSK_PATH%` is replaced by @@ -576,46 +675,63 @@ def createSpiceInterface( Args: path (str): The absolute path to the folder that contains the kernels to be loaded. time (str): The time string in a format that SPICE recognizes. - spiceKernelFileNames (Iterable[str], optional): - A list of spice kernel file names including file extension. - Defaults to `['de430.bsp', 'naif0012.tls', 'de-403-masses.tpc', 'pck00010.tpc']`. + spiceKernelFileNames (Iterable[DataFile.EphemerisData], optional): + Support-data entries for the SPICE kernels to load. Defaults to + ``de430.bsp``, ``naif0012.tls``, ``de-403-masses.tpc``, and + ``pck00010.tpc``. spicePlanetNames (Optional[Sequence[str]], optional): - A list of planet names whose Spice data is loaded. If this is not provided, - Spice data is loaded for the bodies created with this factory object. Defaults to None. + Planet names whose SPICE data is loaded. If omitted, the method + uses the bodies created with this factory, in insertion order. + Defaults to None. spicePlanetFrames (Optional[Sequence[str]], optional): - A list of planet frame names to load in Spice. If this is not provided, - frames are loaded for the bodies created with this factory function. Defaults to None. + Planet frame names to load in SPICE. If omitted, the method uses + the frames recorded when the factory bodies were created. + Defaults to None. epochInMsg (bool, optional): - Flag to set an epoch input message for the spice interface. Defaults to False. + If True, create and connect an epoch input message. Defaults to + False. + spiceKernalFileNames: Deprecated misspelling of + ``spiceKernelFileNames``. Returns: - spiceInterface.SpiceInterface: The generated SpiceInterface, which is the - same as the stored `self.spiceObject` + spiceInterface.SpiceInterface: The generated ``SpiceInterface``. + The same object is also available as ``self.spiceObject``. """ if time == "": raise ValueError( - "'time' argument must be provided and a valid SPICE time string" + "'time' must be provided as a valid SPICE time string" ) + # Keep the historical misspelling working while directing users to the + # correctly spelled keyword. if spiceKernalFileNames is not None: spiceKernelFileNames = spiceKernalFileNames deprecationWarn( - f"{gravBodyFactory.createSpiceInterface.__qualname__}.spiceKernalFileNames" + f"{gravBodyFactory.createSpiceInterface.__qualname__}.spiceKernalFileNames", "2024/11/24", "The argument 'spiceKernalFileNames' is deprecated. Use 'spiceKernelFileNames'", ) + # ``%BSK_PATH%`` is retained for compatibility with older scripts. The + # resolved path is also passed to the optional epoch-message helper. if isinstance(path, str) and "%BSK_PATH%" in path: path = path.replace("%BSK_PATH%", list(__path__)[0]) path = Path(path).expanduser().resolve() + # Keep the interface on the factory so callers can schedule it and later + # unload its kernels through this same factory instance. self.spiceObject = spiceInterface.SpiceInterface() self.spiceObject.ModelTag = "SpiceInterfaceData" self.spiceObject.SPICEDataPath = ( str(Path(POOCH.abspath) / "supportData" / "EphemerisData") + os.sep ) self.spiceKernelFileNames.extend(spiceKernelFileNames) + + # Python dictionaries preserve insertion order. By default, SPICE + # outputs therefore follow the same order as ``gravBodies`` below. + # Callers who provide ``spicePlanetNames`` must keep that ordering + # consistent with the factory bodies they intend to connect. self.spicePlanetNames = list(spicePlanetNames or self.gravBodies) if spicePlanetFrames is not None: self.spicePlanetFrames = list(spicePlanetFrames) @@ -626,19 +742,22 @@ def createSpiceInterface( self.spiceObject.planetFrames = list(self.spicePlanetFrames) self.spiceObject.SPICELoaded = True + # A set avoids loading the same kernel more than once if this factory + # accumulated duplicate entries. for file_enum in set(self.spiceKernelFileNames): - # Resolve the file path using pooch if possible + # Prefer the support-data registry. It selects a local checkout + # when available and otherwise retrieves the cached data file. try: resolved_path = Path(get_path(file_enum)).resolve() except Exception: - # Fallback, use the kernel filename directly inside SPICEDataPath + # User-supplied filenames may not be registry entries. Fall + # back to the interface's SPICE data directory for those. fname = getattr(file_enum, "value", str(file_enum)) resolved_path = (Path(self.spiceObject.SPICEDataPath) / fname).resolve() - # Absolute filename SPICE should load + # ``loadSpiceKernel`` accepts the filename and containing directory + # separately, so split the resolved absolute path at this boundary. full_kernel_path = str(resolved_path) - - # ALWAYS load using the full path. resolved_dir = str(resolved_path.parent) + os.sep resolved_name = resolved_path.name load_failed = self.spiceObject.loadSpiceKernel(resolved_name, resolved_dir) @@ -647,13 +766,17 @@ def createSpiceInterface( self.spiceObject.SPICELoaded = False print(f"\033[91mERROR loading kernel:\033[0m {full_kernel_path}") - # Hook grav bodies to the SPICE messages + # Connect each shared descriptor to the matching SPICE output. Both + # conventional GravityEffector and MuJoCo NBodyGravity later read this + # same ``planetBodyInMsg`` connection. for c, gravBodyDataItem in enumerate(self.gravBodies.values()): gravBodyDataItem.planetBodyInMsg.subscribeTo( self.spiceObject.planetStateOutMsgs[c] ) if epochInMsg: + # Store the message on the factory so its Python owner remains alive + # for as long as the SPICE interface is in use. self.epochMsg = simHelpers.timeStringToGregorianUTCMsg( time, dataPath=str(path) ) @@ -662,11 +785,12 @@ def createSpiceInterface( return self.spiceObject def unloadSpiceKernels(self): - """Method to unload spice kernals at the end of a simulation.""" + """Unload this factory's SPICE kernels at the end of a simulation.""" if self.spiceObject is None: return for file_enum in set(self.spiceKernelFileNames): - # Convert enum → string filename + # Kernel lists normally contain DataFile enum members, but retain + # support for callers that supplied filename strings. fname = getattr(file_enum, "value", str(file_enum)) d = _ensure_trailing_sep(self.spiceObject.SPICEDataPath) @@ -680,7 +804,7 @@ def loadGravFromFile( ): """Load the gravitational body spherical harmonics coefficients from a file. - Note that this function calls the `gravityEffector` function `loadGravFromFile()`. + This wrapper delegates to ``gravityEffector.loadGravFromFile()``. Args: fileName (str): The full path to the specified data file. @@ -698,6 +822,6 @@ def loadPolyFromFile(fileName: str, poly: gravityEffector.PolyhedralGravityModel Args: fileName (str): The full path to the specified data file. poly (gravityEffector.PolyhedralGravityModel): - The polyhedarl gravity model container of the body. + The polyhedral gravity model container of the body. """ loadPolyFromFile_python(fileName, poly) From 08e7ecee784a3e0d3c1f3bced5c522aef713bb05 Mon Sep 17 00:00:00 2001 From: Hanspeter Schaub Date: Mon, 20 Jul 2026 14:40:14 -0600 Subject: [PATCH 3/8] [#1490] Add zeroBase origin support to planetEphemeris Add an optional zeroBase setting to PlanetEphemeris so generated planet states can be expressed relative to another body propagated by the same module instance. Preserve the existing heliocentric behavior when zeroBase is left empty. Resolve the selected body during Reset using case-insensitive name matching and report an initialization error when the requested body is not configured in the module. Compute all heliocentric states before subtracting the selected body's position and velocity, ensuring the translation is independent of body ordering. Leave planet names, timestamps, attitudes, attitude rates, and orientation flags unchanged. Expand the PlanetEphemeris unit tests to verify: - the default empty zeroBase value - position and velocity translation at multiple simulation times - zero position and velocity for the selected base body - preservation of attitude and message metadata - case-insensitive body-name matching - rejection of bodies from outside the module instance Document zeroBase configuration, default behavior, and the requirement that the reference body be propagated by the same PlanetEphemeris instance. Clarify that changing the output origin does not introduce hierarchical or mutually coupled orbital propagation. Update scenarioCustomGravBody to use Itokawa as the output origin, placing the planet and spacecraft states in a common asteroid-centered translational frame. --- examples/scenarioCustomGravBody.py | 12 +- .../_UnitTest/test_planetEphemeris.py | 185 ++++++++++++++++++ .../planetEphemeris/planetEphemeris.cpp | 83 ++++++-- .../planetEphemeris/planetEphemeris.h | 6 +- .../planetEphemeris/planetEphemeris.rst | 65 +++++- 5 files changed, 326 insertions(+), 25 deletions(-) diff --git a/examples/scenarioCustomGravBody.py b/examples/scenarioCustomGravBody.py index 06d21259b4a..654fad45ef2 100755 --- a/examples/scenarioCustomGravBody.py +++ b/examples/scenarioCustomGravBody.py @@ -53,9 +53,12 @@ be customized. The gravity body ephemeris states are connected to the :ref:`planetEphemeris` planet state output messages. -Finally, the recorded states will all be relative to the inertial origin at the sun. :ref:`planetEphemeris` does not -have the ``zeroBase`` capability as :ref:`spiceInterface` has. This script also records the asteroid -states so that the plot is done of the spacecraft motion relative to the asteroid. +The ephemeris outputs remain heliocentric because the Sun gravity body is not +connected to an ephemeris message. Its default zero state therefore represents +the Sun at the heliocentric origin. Setting ``planetEphemeris.zeroBase`` to +Itokawa would incorrectly place both the Sun and Itokawa at the origin. To plot +the spacecraft motion relative to the asteroid, the script instead records the +Itokawa state and subtracts it from the spacecraft state during post-processing. The simulation executes and shows a plot of the spacecraft motion relative to the asteroid. @@ -148,6 +151,9 @@ def run(show_plots): # specify celestial object orbit gravBodyEphem.planetElements = planetEphemeris.classicElementVector([oeAsteroid, oeEarth]) + # Keep these outputs heliocentric. The unlinked Sun gravity body below uses + # its default zero state to represent the Sun at the heliocentric origin. + # The asteroid state is subtracted from the spacecraft state when plotting. # specify celestial object orientation gravBodyEphem.rightAscension = planetEphemeris.DoubleVector([0.0 * macros.D2R, 0.0 * macros.D2R]) gravBodyEphem.declination = planetEphemeris.DoubleVector([0.0 * macros.D2R, 0.0 * macros.D2R]) diff --git a/src/simulation/environment/planetEphemeris/_UnitTest/test_planetEphemeris.py b/src/simulation/environment/planetEphemeris/_UnitTest/test_planetEphemeris.py index b531a133be3..0277f05f691 100755 --- a/src/simulation/environment/planetEphemeris/_UnitTest/test_planetEphemeris.py +++ b/src/simulation/environment/planetEphemeris/_UnitTest/test_planetEphemeris.py @@ -46,6 +46,189 @@ from Basilisk.architecture.bskLogging import BasiliskError +def _make_zero_base_test_elements(): + """Create two heliocentric element sets for the zero-base tests. + + Returns: + planetEphemeris.classicElementVector: Independent Earth and Moon-like + heliocentric element sets. + """ + earthElements = planetEphemeris.ClassicElements() + earthElements.a = orbitalMotion.SMA_EARTH * 1000.0 # [m] + earthElements.e = 0.01 # [-] + earthElements.i = 10.0 * macros.D2R # [deg] -> [rad] + earthElements.Omega = 30.0 * macros.D2R # [deg] -> [rad] + earthElements.omega = 20.0 * macros.D2R # [deg] -> [rad] + earthElements.f = 90.0 * macros.D2R # [deg] -> [rad] + + moonElements = planetEphemeris.ClassicElements() + moonElements.a = 1.01 * orbitalMotion.SMA_EARTH * 1000.0 # [m] + moonElements.e = 0.02 # [-] + moonElements.i = 5.0 * macros.D2R # [deg] -> [rad] + moonElements.Omega = 110.0 * macros.D2R # [deg] -> [rad] + moonElements.omega = 220.0 * macros.D2R # [deg] -> [rad] + moonElements.f = 180.0 * macros.D2R # [deg] -> [rad] + + return planetEphemeris.classicElementVector( + [earthElements, moonElements] + ) + + +def _configure_zero_base_test_module(module): + """Configure a ``PlanetEphemeris`` module with two oriented bodies. + + Args: + module (planetEphemeris.PlanetEphemeris): Module to configure. + """ + module.setPlanetNames( + planetEphemeris.StringVector(["earth", "moon"]) + ) + module.planetElements = _make_zero_base_test_elements() + module.rightAscension = planetEphemeris.DoubleVector( + [0.0, 10.0 * macros.D2R] + ) # [rad] + module.declination = planetEphemeris.DoubleVector( + [90.0 * macros.D2R, 80.0 * macros.D2R] + ) # [rad] + module.lst0 = planetEphemeris.DoubleVector( + [0.0, 30.0 * macros.D2R] + ) # [rad] + module.rotRate = planetEphemeris.DoubleVector( + [1.0e-5, 2.0e-5] + ) # [rad/s] + + +def test_zero_base_translation(): + """Verify that ``zeroBase`` translates positions and velocities only. + + Validation Test Description + --------------------------- + Two identical modules propagate the same oriented bodies. One retains the + default heliocentric origin while the other selects Earth as ``zeroBase``. + The relative outputs are compared with differences of the heliocentric + outputs at three simulation times. + + Test Parameter Discussion + ------------------------- + Mixed-case ``"EaRtH"`` verifies case-insensitive body-name matching. The + bodies use distinct orbits and attitudes so position, velocity, and + orientation behavior are all observable. + + Description of Variables Being Tested + --------------------------------------- + The test checks ``PositionVector``, ``VelocityVector``, ``J20002Pfix``, + ``J20002Pfix_dot``, ``computeOrient``, and ``PlanetName``. + """ + simulation = SimulationBaseClass.SimBaseClass() + process = simulation.CreateNewProcess("zeroBaseProcess") + taskStep = 0.5 # [s] + process.addTask( + simulation.CreateNewTask( + "zeroBaseTask", + macros.sec2nano(taskStep), + ) + ) + + heliocentricModule = planetEphemeris.PlanetEphemeris() + heliocentricModule.ModelTag = "heliocentricPlanetEphemeris" + assert heliocentricModule.zeroBase == "" + _configure_zero_base_test_module(heliocentricModule) + + relativeModule = planetEphemeris.PlanetEphemeris() + relativeModule.ModelTag = "relativePlanetEphemeris" + _configure_zero_base_test_module(relativeModule) + relativeModule.zeroBase = "EaRtH" + + simulation.AddModelToTask("zeroBaseTask", heliocentricModule, 10) + simulation.AddModelToTask("zeroBaseTask", relativeModule, 10) + + heliocentricRecorders = [ + message.recorder() for message in heliocentricModule.planetOutMsgs + ] + relativeRecorders = [ + message.recorder() for message in relativeModule.planetOutMsgs + ] + for recorder in heliocentricRecorders + relativeRecorders: + simulation.AddModelToTask("zeroBaseTask", recorder, 0) + + simulation.InitializeSimulation() + simulationDuration = 1.0 # [s] + simulation.ConfigureStopTime(macros.sec2nano(simulationDuration)) + simulation.ExecuteSimulation() + + basePosition = heliocentricRecorders[0].PositionVector + baseVelocity = heliocentricRecorders[0].VelocityVector + positionTolerance = 1.0e-6 # [m] + velocityTolerance = 1.0e-10 # [m/s] + attitudeTolerance = 1.0e-14 # [-] + for bodyIndex in range(2): + expectedPosition = ( + heliocentricRecorders[bodyIndex].PositionVector - basePosition + ) + expectedVelocity = ( + heliocentricRecorders[bodyIndex].VelocityVector - baseVelocity + ) + np.testing.assert_allclose( + relativeRecorders[bodyIndex].PositionVector, + expectedPosition, + rtol=0.0, + atol=positionTolerance, + ) + np.testing.assert_allclose( + relativeRecorders[bodyIndex].VelocityVector, + expectedVelocity, + rtol=0.0, + atol=velocityTolerance, + ) + np.testing.assert_allclose( + relativeRecorders[bodyIndex].J20002Pfix, + heliocentricRecorders[bodyIndex].J20002Pfix, + rtol=0.0, + atol=attitudeTolerance, + ) + np.testing.assert_allclose( + relativeRecorders[bodyIndex].J20002Pfix_dot, + heliocentricRecorders[bodyIndex].J20002Pfix_dot, + rtol=0.0, + atol=attitudeTolerance, + ) + np.testing.assert_array_equal( + relativeRecorders[bodyIndex].computeOrient, + heliocentricRecorders[bodyIndex].computeOrient, + ) + + assert relativeModule.planetOutMsgs[0].read().PlanetName == "earth" + assert relativeModule.planetOutMsgs[1].read().PlanetName == "moon" + + +def test_zero_base_requires_local_body(): + """Verify that ``zeroBase`` rejects bodies owned by another instance. + + Validation Test Description + --------------------------- + A module configured with Earth and Moon selects Mars as its translational + origin. Initialization must stop with a ``BasiliskError`` because Mars is + not propagated by that module instance. + """ + simulation = SimulationBaseClass.SimBaseClass() + process = simulation.CreateNewProcess("invalidZeroBaseProcess") + taskStep = 0.5 # [s] + process.addTask( + simulation.CreateNewTask( + "invalidZeroBaseTask", + macros.sec2nano(taskStep), + ) + ) + + module = planetEphemeris.PlanetEphemeris() + _configure_zero_base_test_module(module) + module.zeroBase = "mars" + simulation.AddModelToTask("invalidZeroBaseTask", module) + + with pytest.raises(BasiliskError, match="not one of the bodies configured"): + simulation.InitializeSimulation() + + # Uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed. # @pytest.mark.skipif(conditionstring) # Uncomment this line if this test has an expected failure, adjust message as needed. @@ -274,6 +457,8 @@ def planetEphemerisTest(show_plots, setRAN, setDEC, setLST, setRate): # stand-along python script # if __name__ == "__main__": + test_zero_base_translation() + test_zero_base_requires_local_body() test_module( False, # show plots flag True, # setRAN diff --git a/src/simulation/environment/planetEphemeris/planetEphemeris.cpp b/src/simulation/environment/planetEphemeris/planetEphemeris.cpp index 991a434cfe5..9496f39479a 100755 --- a/src/simulation/environment/planetEphemeris/planetEphemeris.cpp +++ b/src/simulation/environment/planetEphemeris/planetEphemeris.cpp @@ -17,6 +17,8 @@ */ #include "simulation/environment/planetEphemeris/planetEphemeris.h" +#include +#include #include #include #include "architecture/utilities/astroConstants.h" @@ -24,6 +26,18 @@ #include "architecture/utilities/macroDefinitions.h" #include "architecture/utilities/rigidBodyKinematics.h" +namespace { +/*! Compare two planet names without regard to letter case. */ +bool planetNamesMatch(const std::string& firstName, const std::string& secondName) +{ + return firstName.size() == secondName.size() + && std::equal(firstName.begin(), firstName.end(), secondName.begin(), + [](unsigned char firstCharacter, unsigned char secondCharacter) { + return std::tolower(firstCharacter) == std::tolower(secondCharacter); + }); +} +} // namespace + /*! This constructor initializes the variables. */ @@ -64,14 +78,30 @@ void PlanetEphemeris::setPlanetNames(std::vector names) -void PlanetEphemeris::Reset(uint64_t CurrenSimNanos) +void PlanetEphemeris::Reset(uint64_t CurrentSimNanos) { - this->epochTime = CurrenSimNanos*NANO2SEC; + this->epochTime = CurrentSimNanos*NANO2SEC; /*! - do sanity checks that the vector arrays for planet names and ephemeris have the same length */ if (this->planetElements.size() != this->planetNames.size()) { - bskLogger.bskError("Only %lu planet element sets provided, but %lu plane names are present.", - this->planetElements.size(), this->planetNames.size()); + bskLogger.bskError("Only %zu planet element sets provided, but %zu planet names are present.", + this->planetElements.size(), this->planetNames.size()); + } + + /*! - Resolve the optional translational origin from the bodies owned by this module. */ + this->zeroBaseIndex = -1; + if (!this->zeroBase.empty()) { + for (size_t c = 0; c < this->planetNames.size(); c++) { + if (planetNamesMatch(this->zeroBase, this->planetNames.at(c))) { + this->zeroBaseIndex = static_cast(c); + break; + } + } + if (this->zeroBaseIndex < 0) { + bskLogger.bskError( + "PlanetEphemeris zeroBase '%s' is not one of the bodies configured in this module instance.", + this->zeroBase.c_str()); + } } /*! - See if planet orientation information is set */ @@ -84,28 +114,32 @@ void PlanetEphemeris::Reset(uint64_t CurrenSimNanos) } if (computeAttitudeFlag) { - /*! - check that the right number of planet local sideral time angles are provided */ + /*! - check that the right number of planet local sidereal time angles are provided */ if (this->lst0.size() != this->planetNames.size()) { - bskLogger.bskError("Only %lu planet initial principal rotation angles provided, but %lu planet names are present.", - this->lst0.size(), this->planetNames.size()); + bskLogger.bskError( + "Only %zu planet initial principal rotation angles provided, but %zu planet names are present.", + this->lst0.size(), this->planetNames.size()); } /*! - check that the right number of planet polar axis right ascension angles are provided */ if (this->rightAscension.size() != this->planetNames.size()) { - bskLogger.bskError("Only %lu planet right ascension angles provided, but %lu planet names are present.", - this->rightAscension.size(), this->planetNames.size()); + bskLogger.bskError( + "Only %zu planet right ascension angles provided, but %zu planet names are present.", + this->rightAscension.size(), this->planetNames.size()); } /*! - check that the right number of planet polar axis declination angles are provided */ if (this->declination.size() != this->planetNames.size()) { - bskLogger.bskError("Only %lu planet declination angles provided, but %lu planet names are present.", - this->declination.size(), this->planetNames.size()); + bskLogger.bskError( + "Only %zu planet declination angles provided, but %zu planet names are present.", + this->declination.size(), this->planetNames.size()); } /*! - check that the right number of planet polar rotation rates are provided */ if (this->rotRate.size() != this->planetNames.size()) { - bskLogger.bskError("Only %lu planet rotation rates provided, but %lu planet names are present.", - this->rotRate.size(), this->planetNames.size()); + bskLogger.bskError( + "Only %zu planet rotation rates provided, but %zu planet names are present.", + this->rotRate.size(), this->planetNames.size()); } } @@ -133,6 +167,7 @@ void PlanetEphemeris::UpdateState(uint64_t CurrentSimNanos) double omega_NP_P[3]; // [r/s] angular velocity of inertial frame relative to planet frame in planet frame components double tilde[3][3]; // [] skew-symmetric matrix double theta_PN[3]; // [rad] 3-2-3 planet orientation coordinates + std::vector planetStates(this->planetNames.size()); @@ -146,7 +181,7 @@ void PlanetEphemeris::UpdateState(uint64_t CurrentSimNanos) for(it = this->planetNames.begin(), c=0; it != this->planetNames.end(); it++, c++) { //! - Create new planet output message copy - SpicePlanetStateMsgPayload newPlanet; + SpicePlanetStateMsgPayload& newPlanet = planetStates.at(c); newPlanet = this->planetOutMsgs.at(c)->zeroMsgPayload; //! - specify planet name in output message std::snprintf(newPlanet.PlanetName, sizeof(newPlanet.PlanetName), "%s", it->c_str()); @@ -183,9 +218,25 @@ void PlanetEphemeris::UpdateState(uint64_t CurrentSimNanos) m33SetIdentity(newPlanet.J20002Pfix); // set default zero orientation } - //! - write output message - this->planetOutMsgs.at(c)->write(&newPlanet, this->moduleID, CurrentSimNanos); + } + + /*! - Translate every position and velocity to the selected zero-base origin. */ + if (this->zeroBaseIndex >= 0) { + double zeroBasePosition_N[3]; // [m] zero-base heliocentric position + double zeroBaseVelocity_N[3]; // [m/s] zero-base heliocentric velocity + v3Copy(planetStates.at(this->zeroBaseIndex).PositionVector, zeroBasePosition_N); + v3Copy(planetStates.at(this->zeroBaseIndex).VelocityVector, zeroBaseVelocity_N); + + for (auto& planetState : planetStates) { + v3Subtract(planetState.PositionVector, zeroBasePosition_N, planetState.PositionVector); + v3Subtract(planetState.VelocityVector, zeroBaseVelocity_N, planetState.VelocityVector); + } + } + /*! - Write the translated planet states after every absolute state is available. */ + for (size_t planetIndex = 0; planetIndex < planetStates.size(); planetIndex++) { + this->planetOutMsgs.at(planetIndex)->write( + &planetStates.at(planetIndex), this->moduleID, CurrentSimNanos); } return; } diff --git a/src/simulation/environment/planetEphemeris/planetEphemeris.h b/src/simulation/environment/planetEphemeris/planetEphemeris.h index 65e2718a6e5..87303415b92 100755 --- a/src/simulation/environment/planetEphemeris/planetEphemeris.h +++ b/src/simulation/environment/planetEphemeris/planetEphemeris.h @@ -20,6 +20,7 @@ #ifndef planetEphemeris_H #define planetEphemeris_H +#include #include #include "architecture/_GeneralModuleFiles/sys_model.h" @@ -52,12 +53,15 @@ class PlanetEphemeris: public SysModel { std::vector rotRate; //!< [r/s] planet rotation rate + std::string zeroBase{}; //!< -- Output origin body; empty preserves heliocentric states + BSKLogger bskLogger; //!< -- BSK Logging private: std::vector planetNames; //!< -- Vector of planet names double epochTime; //!< [s] time of provided planet ephemeris epoch - int computeAttitudeFlag; //!< -- flag indicating if the planet orienation information is provided + int computeAttitudeFlag; //!< -- flag indicating whether planet orientation is provided + int zeroBaseIndex = -1; //!< -- index of the body selected as the translational origin }; diff --git a/src/simulation/environment/planetEphemeris/planetEphemeris.rst b/src/simulation/environment/planetEphemeris/planetEphemeris.rst index f12f1d578f7..24cf584b3cf 100644 --- a/src/simulation/environment/planetEphemeris/planetEphemeris.rst +++ b/src/simulation/environment/planetEphemeris/planetEphemeris.rst @@ -4,15 +4,70 @@ Executive Summary The planetEphemeris module uses classical heliocentric orbit elements to specify a planet's position and velocity vectors. -Optionally, the planet's orientation can also be specified through a right ascension angle, a declination angle, and a location sidereal time at epoch. The planet rotation about its third (polar) axis can be specified as well. If the planet attitude information is not complete or missing then an inertially fixed zero planet orientation is modeled. The module is able to receive a stack or vector of classical orbit elements and orientation information to output a series of planet ephemeris messages. The module +By default, every output position and velocity is heliocentric. The optional +``zeroBase`` setting translates all output states to one of the bodies +propagated by the module. This provides the same translational-origin concept +as ``spiceInterface.zeroBase`` while retaining J2000-aligned coordinate axes. + +Optionally, the planet's orientation can also be specified through a right +ascension angle, a declination angle, and a local sidereal time at epoch. The +planet rotation about its third (polar) axis can be specified as well. If the +planet attitude information is incomplete or missing, then an inertially fixed +zero planet orientation is modeled. The module accepts a vector of classical +orbit elements and orientation information and outputs a corresponding series +of planet ephemeris messages. The module :download:`PDF Description ` -contains further information on this module's function, how to run it, as well as testing. +contains further information on the model and its use. + +Reference Origin Selection +-------------------------- + +Set ``zeroBase`` to a configured planet name to express every output position +and velocity relative to that body:: + + ephemeris = planetEphemeris.PlanetEphemeris() + ephemeris.setPlanetNames( + planetEphemeris.StringVector(["earth", "moon"]) + ) + ephemeris.planetElements = planetEphemeris.classicElementVector( + [earthElements, moonElements] + ) + ephemeris.zeroBase = "earth" + +With this configuration, the Earth output has zero position and velocity, and +the Moon output is the heliocentric Moon state minus the heliocentric Earth +state. Planet names, timestamps, attitudes, and attitude rates are unchanged. +Leaving ``zeroBase`` empty preserves the default heliocentric outputs. + +Module Assumptions and Limitations +---------------------------------- + +.. warning:: + + ``zeroBase`` only works for a body configured in the same + ``PlanetEphemeris`` instance. Separate ``PlanetEphemeris`` instances do not + exchange states. Selecting a name that was not supplied to + ``setPlanetNames()`` stops initialization with an error. + +The module propagates each body's orbit independently as a two-body +heliocentric orbit. The ``zeroBase`` translation changes only the output +coordinate origin; it does not create a hierarchical or mutually coupled +planet system. + +User Guide +---------- + +Configure all bodies that must share an output origin on one module instance, +provide one classical-element set per body, and set ``zeroBase`` before +initializing the simulation. Consumers of the resulting messages, including +gravity models, navigation modules, and Vizard, must use states expressed +relative to that same origin. Message Connection Descriptions ------------------------------- -The following table lists all the module input and output messages. The module msg connection is set by the -user from python. The msg type contains a link to the message structure definition, while the description -provides information on what this message is used for. +The following table lists all module input and output messages. The user sets +module message connections from Python. The message type links to the message +structure definition, while the description explains its use. .. bsk-module-io:: planetEphemeris From 7942a9738ac2bc149ed316c0c8e595dd22a1b49e Mon Sep 17 00:00:00 2001 From: Hanspeter Schaub Date: Mon, 20 Jul 2026 14:41:25 -0600 Subject: [PATCH 4/8] [#1490] add new mujoco example showing how to add gravity bodies Example uses both Spice and planetEphemeris message states. --- examples/_default.rst | 1 + examples/mujoco/scenarioMJEarthMoonGravity.py | 687 ++++++++++++++++++ src/tests/test_scenarioMujoco.py | 5 + 3 files changed, 693 insertions(+) create mode 100644 examples/mujoco/scenarioMJEarthMoonGravity.py diff --git a/examples/_default.rst b/examples/_default.rst index d0bcb1217af..3de728bc163 100644 --- a/examples/_default.rst +++ b/examples/_default.rst @@ -217,6 +217,7 @@ It's recommended to study the first 6 scenarios in order: Chained Hinged Panel Deployment Solar Radiation Pressure Model on Arbitrary Surfaces Landing on Asteroid with Contact Physics + Earth-Moon Gravity with SPICE or PlanetEphemeris Docking between Two CubeSats Branching Panel Deployment with Locking Mechanisms Pointing with Reaction Wheels in MuJoCo diff --git a/examples/mujoco/scenarioMJEarthMoonGravity.py b/examples/mujoco/scenarioMJEarthMoonGravity.py new file mode 100644 index 00000000000..e7e16ec88ff --- /dev/null +++ b/examples/mujoco/scenarioMJEarthMoonGravity.py @@ -0,0 +1,687 @@ +# +# 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""" +Overview +-------- + +This tutorial demonstrates how to add Earth and Moon gravity to a +:ref:`MJScene` with :ref:`simIncludeGravBody`. The same MuJoCo +spacecraft and gravity-factory setup can be driven by either: + +* :ref:`spiceInterface`, which reads high-fidelity ephemerides from NAIF + kernels, or +* :ref:`planetEphemeris`, which propagates user-supplied heliocentric + classical orbit elements with two-body Keplerian motion. + +Select the ephemeris source through the ``useSpice`` argument to ``run()``. +The default is ``True``. For example, the analytic ephemeris case is run with:: + + run(showPlots=True, useSpice=False) + +Gravity-factory setup +--------------------- + +Both cases begin by creating the same shared gravity-body descriptors. Earth +is marked as the central body because the MuJoCo spacecraft state is initialized +relative to Earth:: + + gravFactory = simIncludeGravBody.gravBodyFactory() + earth = gravFactory.createEarth() + earth.isCentralBody = True + moon = gravFactory.createMoon() + +Each descriptor contains the body's gravity model, gravitational parameter, +central-body flag, and a planet-state input message. The only difference +between the two cases is which ephemeris output is connected to those input +messages. + +Using SPICE planet states +------------------------- + +The SPICE helper uses the bodies already present in the factory to configure +and connect its output messages:: + + spiceObject = gravFactory.createSpiceInterface( + time="2025 NOVEMBER 15 12:00:00.000", + epochInMsg=True, + ) + spiceObject.zeroBase = "Earth" + scene.AddModelToDynamicsTask(spiceObject, 75) + +The factory creates the SPICE output messages in gravity-body insertion order, +so the first output is Earth and the second is the Moon in this example. +``createSpiceInterface()`` subscribes each gravity body's +``planetBodyInMsg`` to the corresponding output automatically. + +Setting ``zeroBase`` to Earth makes the planet states and the spacecraft state +Earth-relative. This is convenient but is not required by +:ref:`NBodyGravity`; it only requires all gravity-source states +to use one consistent inertial origin. + +Using analytic ``PlanetEphemeris`` states +----------------------------------------- + +The analytic alternative names its output bodies and supplies one +heliocentric classical-element set for each body:: + + planetObject = planetEphemeris.PlanetEphemeris() + planetObject.setPlanetNames( + planetEphemeris.StringVector(["earth", "moon"]) + ) + planetObject.planetElements = planetEphemeris.classicElementVector( + [earthElements, moonElements] + ) + planetObject.zeroBase = "earth" + earth.planetBodyInMsg.subscribeTo(planetObject.planetOutMsgs[0]) + moon.planetBodyInMsg.subscribeTo(planetObject.planetOutMsgs[1]) + scene.AddModelToDynamicsTask(planetObject, 75) + +Unlike the factory's SPICE helper, a separately created ``PlanetEphemeris`` +module does not know which gravity descriptors it should drive. Its output +messages are therefore connected explicitly. + +``PlanetEphemeris`` propagates every supplied element set independently about +the Sun. It does not implement a hierarchical Earth-Moon model. To provide a +reasonable local approximation, this example constructs an initial +heliocentric Moon state by adding a geocentric lunar state to Earth's +heliocentric state, then converts the result to heliocentric elements. This is +useful for short examples and deterministic tests. SPICE should generally be +preferred when accurate Earth-Moon geometry over long intervals is required. + +Setting ``zeroBase`` to Earth translates both outputs after propagation. The +Earth message is therefore zero and the Moon message is Earth-relative. This +keeps the analytic planet messages in the same frame as the Earth-relative +MuJoCo state, which is required for correct Vizard placement. The selected +zero-base body must be configured on this same ``PlanetEphemeris`` instance. + +Execution ordering and committed-state outputs +---------------------------------------------- + +The ephemeris execution order is critical. Basilisk executes models with +higher numeric priorities first; model insertion order in the Python script +does not determine execution order. This example explicitly enforces the +following sequence:: + + Top-level task: + MJScene (0) -> recorders and Vizard + + MJScene dynamics task: + forward kinematics (10000) -> ephemeris (75) -> NBodyGravity (-1) + +MuJoCo's adaptive integrator evaluates dynamics between top-level task ticks. +The ephemeris must therefore run in the scene dynamics task before +``NBodyGravity`` so that every force evaluation uses planet positions for the +current integrator time:: + + scene.AddModelToDynamicsTask( + ephemeris, EPHEMERIS_PRIORITY + ) + +Integrator stages use provisional states. Without an additional evaluation, +messages written by dynamics-task models can therefore describe the last +integrator stage rather than the state committed at the end of the step. This +example enables:: + + scene.extraEoMCall = True + +After integration, ``MJScene`` then executes its dynamics task once more at the +top-level task time using the committed MuJoCo state. This final evaluation +refreshes the forward-kinematics, ephemeris, and gravity outputs before the +recorders and Vizard execute. It does not advance or otherwise change the +integrated state. Because this final dynamics-task evaluation supplies the +current ephemeris messages, the ephemeris module does not also need to be added +to the top-level task. + +Attaching gravity to MuJoCo +--------------------------- + +After either ephemeris has been configured, one factory call completes the +MuJoCo gravity setup:: + + gravity = gravFactory.addBodiesTo(scene) + +The factory creates one :ref:`NBodyGravity` model, adds Earth and +the Moon as gravity sources, and adds every MuJoCo body as a gravity target. +For the articulated CubeSat used here, gravity is applied at the center of mass +of both the hub and the reaction wheel. + +Illustration of Simulation Results +---------------------------------- + +The scenario test runs both ephemeris cases and saves three figures per case. +The first shows the spacecraft trajectory in its orbital plane, including the +Earth surface and the required :math:`R_E + 400` km minimum-radius boundary. +The initial orbit has a 700 km periapsis altitude, and the scenario checks the +recorded three-dimensional radius to ensure it remains above the 400 km +boundary. The second figure shows the Earth-Moon geometry during the +simulation, and the third shows osculating spacecraft elements and the +magnitude of the Moon's third-body acceleration. + +.. image:: /_images/Scenarios/scenarioMJEarthMoonGravity_spice_orbit.svg + :align: center + +.. image:: /_images/Scenarios/scenarioMJEarthMoonGravity_spice_moonGeometry.svg + :align: center + +.. image:: /_images/Scenarios/scenarioMJEarthMoonGravity_spice_diagnostics.svg + :align: center + +.. image:: /_images/Scenarios/scenarioMJEarthMoonGravity_planetEphemeris_orbit.svg + :align: center + +.. image:: /_images/Scenarios/scenarioMJEarthMoonGravity_planetEphemeris_moonGeometry.svg + :align: center + +.. image:: /_images/Scenarios/scenarioMJEarthMoonGravity_planetEphemeris_diagnostics.svg + :align: center +""" + +import os +from typing import Any, Dict, List, Tuple + +import matplotlib.pyplot as plt +import numpy as np + +from Basilisk.simulation import mujoco +from Basilisk.simulation import planetEphemeris +from Basilisk.simulation import svIntegrators +from Basilisk.utilities import SimulationBaseClass +from Basilisk.utilities import macros +from Basilisk.utilities import orbitalMotion +from Basilisk.utilities import simIncludeGravBody +from Basilisk.utilities import vizSupport + + +CURRENT_FOLDER = os.path.dirname(__file__) +XML_PATH = os.path.join(CURRENT_FOLDER, "sat_w_wheel.xml") +FILE_NAME = os.path.basename(os.path.splitext(__file__)[0]) + +TASK_NAME = "simTask" +PROCESS_NAME = "simProcess" +TASK_STEP_SEC = 30.0 # [s] +SIMULATION_DURATION_SEC = 6.0 * 3600.0 # [s] +MINIMUM_SAFE_ALTITUDE_M = 400_000.0 # [m] +INITIAL_PERIAPSIS_ALTITUDE_M = 700_000.0 # [m] +INITIAL_ECCENTRICITY = 0.001 # [-] +SPICE_EPOCH = "2025 NOVEMBER 15 12:00:00.000" +EPHEMERIS_PRIORITY = 75 +SCENE_PRIORITY = 0 +RECORDER_PRIORITY = -100 + + +def _copy_classic_elements( + source: orbitalMotion.ClassicElements, +) -> planetEphemeris.ClassicElements: + """Copy orbital elements into the SWIG type used by ``PlanetEphemeris``. + + Args: + source (orbitalMotion.ClassicElements): Elements to copy. + + Returns: + planetEphemeris.ClassicElements: Equivalent element set accepted by + :ref:`planetEphemeris`. + """ + destination = planetEphemeris.ClassicElements() + destination.a = source.a + destination.e = source.e + destination.i = source.i + destination.Omega = source.Omega + destination.omega = source.omega + destination.f = source.f + return destination + + +def _create_analytic_ephemeris( + earth: Any, + moon: Any, +) -> Tuple[planetEphemeris.PlanetEphemeris, List[Any]]: + """Create and connect an analytic Earth-Moon ephemeris. + + ``PlanetEphemeris`` expects heliocentric elements for every output. The + Moon's initial heliocentric state is therefore formed by adding a simple + geocentric lunar state to a heliocentric Earth state. + + Args: + earth (Any): Earth's shared ``GravBodyData`` descriptor. + moon (Any): The Moon's shared ``GravBodyData`` descriptor. + + Returns: + Tuple[planetEphemeris.PlanetEphemeris, List[Any]]: Configured module + and its Earth/Moon output messages. + """ + sunMu = orbitalMotion.MU_SUN * 1.0e9 # [m^3/s^2] + + earthElements = planetEphemeris.ClassicElements() + earthElements.a = orbitalMotion.AU * 1000.0 # [m] + earthElements.e = 0.0167086 # [-] + earthElements.i = 0.00005 * macros.D2R # [deg] -> [rad] + earthElements.Omega = -11.26064 * macros.D2R # [deg] -> [rad] + earthElements.omega = 102.94719 * macros.D2R # [deg] -> [rad] + earthElements.f = 55.0 * macros.D2R # [deg] -> [rad] + + moonAboutEarth = orbitalMotion.ClassicElements() + moonAboutEarth.a = 384_400_000.0 # [m] + moonAboutEarth.e = 0.0549 # [-] + moonAboutEarth.i = 5.145 * macros.D2R # [deg] -> [rad] + moonAboutEarth.Omega = 125.08 * macros.D2R # [deg] -> [rad] + moonAboutEarth.omega = 318.15 * macros.D2R # [deg] -> [rad] + moonAboutEarth.f = 35.0 * macros.D2R # [deg] -> [rad] + + earthPosition, earthVelocity = orbitalMotion.elem2rv(sunMu, earthElements) + moonRelativePosition, moonRelativeVelocity = orbitalMotion.elem2rv( + earth.mu, moonAboutEarth + ) + moonHeliocentricElements = orbitalMotion.rv2elem( + sunMu, + earthPosition + moonRelativePosition, + earthVelocity + moonRelativeVelocity, + ) + + ephemeris = planetEphemeris.PlanetEphemeris() + ephemeris.ModelTag = "EarthMoonPlanetEphemeris" + ephemeris.setPlanetNames( + planetEphemeris.StringVector(["Earth", "Moon"]) + ) + ephemeris.planetElements = planetEphemeris.classicElementVector( + [earthElements, _copy_classic_elements(moonHeliocentricElements)] + ) + + # A stand-alone PlanetEphemeris module cannot infer which factory body + # corresponds to each output, so make the two subscriptions explicitly. + earth.planetBodyInMsg.subscribeTo(ephemeris.planetOutMsgs[0]) + moon.planetBodyInMsg.subscribeTo(ephemeris.planetOutMsgs[1]) + return ephemeris, list(ephemeris.planetOutMsgs) + + +def _create_figures( + useSpice: bool, + earth: Any, + moon: Any, + spacecraftRecorder: Any, + earthRecorder: Any, + moonRecorder: Any, +) -> Dict[str, plt.Figure]: + """Create the tutorial's orbit, geometry, and diagnostic figures. + + Args: + useSpice (bool): If ``True``, label results as the SPICE case. + earth (Any): Earth's gravity-body descriptor. + moon (Any): The Moon's gravity-body descriptor. + spacecraftRecorder (Any): MuJoCo hub state recorder. + earthRecorder (Any): Earth ephemeris recorder. + moonRecorder (Any): Moon ephemeris recorder. + + Returns: + Dict[str, plt.Figure]: Figures keyed by documentation image name. + """ + modeKey = "spice" if useSpice else "planetEphemeris" + modeLabel = "SPICE" if useSpice else "PlanetEphemeris" + figurePrefix = f"{FILE_NAME}_{modeKey}" + + timeHours = spacecraftRecorder.times() * macros.NANO2HOUR + spacecraftPosition = np.asarray(spacecraftRecorder.r_BN_N) + spacecraftVelocity = np.asarray(spacecraftRecorder.v_BN_N) + moonRelativePosition = ( + np.asarray(moonRecorder.PositionVector) + - np.asarray(earthRecorder.PositionVector) + ) + + semiMajorAxis = np.empty(len(spacecraftPosition)) + eccentricity = np.empty(len(spacecraftPosition)) + orbitRadius = np.empty(len(spacecraftPosition)) + orbitAngle = np.empty(len(spacecraftPosition)) + for index, (position, velocity) in enumerate( + zip(spacecraftPosition, spacecraftVelocity) + ): + elements = orbitalMotion.rv2elem(earth.mu, position, velocity) + semiMajorAxis[index] = elements.a + eccentricity[index] = elements.e + orbitRadius[index] = elements.rmag + orbitAngle[index] = elements.omega + elements.f + + # This is the Moon's direct acceleration on the spacecraft minus the + # acceleration it gives Earth, expressed in the Earth-centered frame. + moonToSpacecraft = moonRelativePosition - spacecraftPosition + moonDirect = ( + moon.mu + * moonToSpacecraft + / np.linalg.norm(moonToSpacecraft, axis=1)[:, None] ** 3 + ) + moonIndirect = ( + moon.mu + * moonRelativePosition + / np.linalg.norm(moonRelativePosition, axis=1)[:, None] ** 3 + ) + lunarThirdBodyAcceleration = np.linalg.norm( + moonDirect - moonIndirect, axis=1 + ) + + plt.close("all") + figures: Dict[str, plt.Figure] = {} + + earthRadiusKm = earth.radEquator / 1000.0 # [km] + minimumSafeRadiusKm = ( + earth.radEquator + MINIMUM_SAFE_ALTITUDE_M + ) / 1000.0 # [km] + orbitRadiusKm = orbitRadius / 1000.0 # [km] + + orbitFigure, orbitAxis = plt.subplots(figsize=(6.0, 5.0)) + earthDisk = plt.Circle( + (0.0, 0.0), + earthRadiusKm, + color="#2a6fbb", + alpha=0.8, + label="Earth", + ) + orbitAxis.add_patch(earthDisk) + minimumRadiusBoundary = plt.Circle( + (0.0, 0.0), + minimumSafeRadiusKm, + color="#e9c46a", + fill=False, + linestyle="--", + linewidth=2.0, + label=r"Minimum radius ($R_E+400$ km)", + ) + orbitAxis.add_patch(minimumRadiusBoundary) + orbitAxis.plot( + orbitRadiusKm * np.cos(orbitAngle), + orbitRadiusKm * np.sin(orbitAngle), + color="#bb3e03", + linewidth=1.0, + label="MuJoCo hub", + ) + orbitAxis.set_aspect("equal", adjustable="box") + orbitAxis.set_xlabel(r"$r_x$ [km]") + orbitAxis.set_ylabel(r"$r_y$ [km]") + orbitAxis.set_title( + f"MuJoCo trajectory in the orbital plane ({modeLabel})" + ) + orbitAxis.legend(loc="best") + orbitAxis.grid(True, alpha=0.3) + orbitFigure.tight_layout() + figures[f"{figurePrefix}_orbit"] = orbitFigure + + geometryFigure, geometryAxes = plt.subplots( + 1, + 2, + figsize=(10.0, 4.5), + ) + geometryAxes[0].plot( + moonRelativePosition[:, 0] / 1.0e6, + moonRelativePosition[:, 1] / 1.0e6, + color="#6a4c93", + linewidth=2.0, + label="Moon trajectory", + ) + geometryAxes[0].scatter( + moonRelativePosition[0, 0] / 1.0e6, + moonRelativePosition[0, 1] / 1.0e6, + color="#2a9d8f", + marker="o", + label="Start", + zorder=3, + ) + geometryAxes[0].scatter( + moonRelativePosition[-1, 0] / 1.0e6, + moonRelativePosition[-1, 1] / 1.0e6, + color="#e76f51", + marker="x", + label="End", + zorder=3, + ) + geometryAxes[0].scatter( + 0.0, + 0.0, + color="#2a6fbb", + marker="o", + label="Earth", + zorder=3, + ) + geometryAxes[0].set_aspect("equal", adjustable="datalim") + geometryAxes[0].set_xlabel(r"Earth-Moon $x$ [$10^3$ km]") + geometryAxes[0].set_ylabel(r"Earth-Moon $y$ [$10^3$ km]") + geometryAxes[0].set_title("Earth-centered geometry") + geometryAxes[0].legend(loc="best") + geometryAxes[0].grid(True, alpha=0.3) + + moonPositionChange = ( + moonRelativePosition - moonRelativePosition[0] + ) / 1000.0 # [m] -> [km] + geometryAxes[1].plot( + moonPositionChange[:, 0], + moonPositionChange[:, 1], + color="#6a4c93", + linewidth=2.0, + ) + geometryAxes[1].scatter( + moonPositionChange[0, 0], + moonPositionChange[0, 1], + color="#2a9d8f", + marker="o", + zorder=3, + ) + geometryAxes[1].scatter( + moonPositionChange[-1, 0], + moonPositionChange[-1, 1], + color="#e76f51", + marker="x", + zorder=3, + ) + geometryAxes[1].set_aspect("equal", adjustable="datalim") + geometryAxes[1].set_xlabel(r"$\Delta r_x$ [km]") + geometryAxes[1].set_ylabel(r"$\Delta r_y$ [km]") + geometryAxes[1].set_title("Lunar motion from initial position") + geometryAxes[1].grid(True, alpha=0.3) + geometryFigure.suptitle( + f"Earth-Moon geometry over 6 hours ({modeLabel})" + ) + geometryFigure.tight_layout() + figures[f"{figurePrefix}_moonGeometry"] = geometryFigure + + diagnosticsFigure, diagnosticAxes = plt.subplots( + 3, + 1, + figsize=(7.0, 7.0), + sharex=True, + ) + diagnosticAxes[0].plot( + timeHours, + semiMajorAxis - semiMajorAxis[0], + color="#005f73", + ) + diagnosticAxes[0].set_ylabel(r"$a-a_0$ [m]") + diagnosticAxes[1].plot( + timeHours, + eccentricity, + color="#ca6702", + ) + diagnosticAxes[1].set_ylabel("Eccentricity [-]") + diagnosticAxes[2].plot( + timeHours, + lunarThirdBodyAcceleration, + color="#6a4c93", + ) + diagnosticAxes[2].set_ylabel(r"$|\mathbf{a}_{Moon,3B}|$ [m/s$^2$]") + diagnosticAxes[2].set_xlabel("Time [h]") + for axis in diagnosticAxes: + axis.grid(True, alpha=0.3) + diagnosticsFigure.suptitle( + f"Osculating orbit and lunar perturbation ({modeLabel})" + ) + diagnosticsFigure.tight_layout() + figures[f"{figurePrefix}_diagnostics"] = diagnosticsFigure + + return figures + + +def run( + showPlots: bool = False, + useSpice: bool = True, +) -> Dict[str, plt.Figure]: + """Run the Earth-Moon gravity tutorial. + + Args: + showPlots (bool, optional): If ``True``, display the generated figures. + Defaults to ``False``. + useSpice (bool, optional): If ``True``, use SPICE planet states. If + ``False``, use analytic ``PlanetEphemeris`` states. Defaults to + ``True``. + + Returns: + Dict[str, plt.Figure]: Generated figures keyed by documentation image + name. + """ + simulation = SimulationBaseClass.SimBaseClass() + process = simulation.CreateNewProcess(PROCESS_NAME) + process.addTask( + simulation.CreateNewTask( + TASK_NAME, + macros.sec2nano(TASK_STEP_SEC), + ) + ) + + scene = mujoco.MJScene.fromFile(XML_PATH) + scene.ModelTag = "earthMoonSpacecraft" + # Dynamics-task messages are normally left at the final integrator-stage + # evaluation, which can use a provisional rather than the committed state. + # Re-evaluate the dynamics task after integration so the ephemeris, gravity, + # and forward-kinematics messages are current for recorders and Vizard. + scene.extraEoMCall = True + simulation.AddModelToTask( + TASK_NAME, + scene, + SCENE_PRIORITY, + ) + + integrator = svIntegrators.svIntegratorRKF45(scene) + integrationTolerance = 1.0e-8 # [-] + integrator.setRelativeTolerance(integrationTolerance) + integrator.setAbsoluteTolerance(integrationTolerance) + scene.setIntegrator(integrator) + + gravFactory = simIncludeGravBody.gravBodyFactory() + earth = gravFactory.createEarth() + earth.isCentralBody = True + moon = gravFactory.createMoon() + + if useSpice: + ephemeris = gravFactory.createSpiceInterface( + time=SPICE_EPOCH, + epochInMsg=True, + ) + ephemerisMessages = list(ephemeris.planetStateOutMsgs) + else: + ephemeris, ephemerisMessages = _create_analytic_ephemeris( + earth, + moon, + ) + ephemeris.zeroBase = "Earth" + + # The adaptive integrator evaluates dynamics between top-level task ticks. + # Run the ephemeris after forward kinematics (priority 10000) but before the + # factory-created NBodyGravity model (default priority -1) at every such + # evaluation. + scene.AddModelToDynamicsTask(ephemeris, EPHEMERIS_PRIORITY) + gravFactory.addBodiesTo(scene) + + hub = scene.getBody("hub") + hubRecorder = hub.getCenterOfMass().stateOutMsg.recorder() + earthRecorder = ephemerisMessages[0].recorder() + moonRecorder = ephemerisMessages[1].recorder() + # Run recorders after MJScene. The extra equations-of-motion call above + # ensures their samples see messages evaluated at the committed state. + simulation.AddModelToTask( + TASK_NAME, + hubRecorder, + RECORDER_PRIORITY, + ) + simulation.AddModelToTask( + TASK_NAME, + earthRecorder, + RECORDER_PRIORITY, + ) + simulation.AddModelToTask( + TASK_NAME, + moonRecorder, + RECORDER_PRIORITY, + ) + + if vizSupport.vizFound: + vizSupport.enableUnityVisualization( + simulation, + TASK_NAME, + scene, + # saveFile=__file__, + ) + + spacecraftElements = orbitalMotion.ClassicElements() + spacecraftElements.e = INITIAL_ECCENTRICITY + spacecraftElements.a = ( + earth.radEquator + INITIAL_PERIAPSIS_ALTITUDE_M + ) / (1.0 - spacecraftElements.e) # [m] + spacecraftElements.i = 28.5 * macros.D2R # [deg] -> [rad] + spacecraftElements.Omega = 48.2 * macros.D2R # [deg] -> [rad] + spacecraftElements.omega = 347.8 * macros.D2R # [deg] -> [rad] + spacecraftElements.f = 85.3 * macros.D2R # [deg] -> [rad] + initialPosition, initialVelocity = orbitalMotion.elem2rv( + earth.mu, + spacecraftElements, + ) + + simulation.InitializeSimulation() + hub.setPosition(initialPosition) + hub.setVelocity(initialVelocity) + + try: + simulation.ConfigureStopTime( + macros.sec2nano(SIMULATION_DURATION_SEC) + ) + simulation.ExecuteSimulation() + finally: + if useSpice: + gravFactory.unloadSpiceKernels() + + minimumSafeRadius = ( + earth.radEquator + MINIMUM_SAFE_ALTITUDE_M + ) # [m] + minimumRecordedRadius = np.min( + np.linalg.norm(np.asarray(hubRecorder.r_BN_N), axis=1) + ) # [m] + if minimumRecordedRadius <= minimumSafeRadius: + raise RuntimeError( + "The MuJoCo spacecraft crossed the required " + f"{MINIMUM_SAFE_ALTITUDE_M / 1000.0:.0f} km altitude boundary." + ) + + figures = _create_figures( + useSpice, + earth, + moon, + hubRecorder, + earthRecorder, + moonRecorder, + ) + if showPlots: + plt.show() + return figures + + +if __name__ == "__main__": + run(showPlots=True, useSpice=True) diff --git a/src/tests/test_scenarioMujoco.py b/src/tests/test_scenarioMujoco.py index aff7777018c..50706c9a945 100644 --- a/src/tests/test_scenarioMujoco.py +++ b/src/tests/test_scenarioMujoco.py @@ -48,6 +48,11 @@ ] SCENARIO_RUN_KWARGS = { + # Run both planet-state sources used by the Earth-Moon gravity tutorial. + "scenarioMJEarthMoonGravity": [ + {"useSpice": True}, + {"useSpice": False}, + ], "scenarioThrArmControl": {"showPlots": False, "timeStep": 0.08, "runTime": 240.0}, # Run the stochastic-drag scenario in both the default (Ornstein-Uhlenbeck) and the # IGBM density-noise modes, so both sets of documentation figures are generated. From 6b1e6ff024d85c1e02b9dfb3258670150f50ffdc Mon Sep 17 00:00:00 2001 From: Hanspeter Schaub Date: Mon, 20 Jul 2026 14:46:42 -0600 Subject: [PATCH 5/8] [#1490] update existing mujoco examples to use gravity factor class --- .../scenarioAttitudeFeedbackRWMuJoCo.py | 25 ++++------ .../mujoco/scenarioFormationFlyingWithDrag.py | 25 +++------- examples/mujoco/scenarioMJSceneVizard.py | 46 ++++++------------- examples/mujoco/scenarioStochasticDrag.py | 26 ++++------- 4 files changed, 37 insertions(+), 85 deletions(-) diff --git a/examples/mujoco/scenarioAttitudeFeedbackRWMuJoCo.py b/examples/mujoco/scenarioAttitudeFeedbackRWMuJoCo.py index 6a3357f3a0f..5634265e178 100644 --- a/examples/mujoco/scenarioAttitudeFeedbackRWMuJoCo.py +++ b/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` 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 diff --git a/examples/mujoco/scenarioFormationFlyingWithDrag.py b/examples/mujoco/scenarioFormationFlyingWithDrag.py index 6e30526c18b..5f959769df0 100644 --- a/examples/mujoco/scenarioFormationFlyingWithDrag.py +++ b/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, @@ -92,8 +93,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 @@ -161,7 +160,6 @@ class SimulationModels: scene: mujoco.MJScene chiefBody: mujoco.MJBody deputyBody: mujoco.MJBody - gravity: NBodyGravity.NBodyGravity atmo: exponentialAtmosphere.ExponentialAtmosphere # Drag models @@ -321,7 +319,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 @@ -334,16 +331,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() @@ -360,7 +350,6 @@ def createSimulationModels( scene=scene, chiefBody=chiefBody, deputyBody=deputyBody, - gravity=gravity, atmo=atmo, ) diff --git a/examples/mujoco/scenarioMJSceneVizard.py b/examples/mujoco/scenarioMJSceneVizard.py index 1813aa5fd5c..b710cbf636b 100644 --- a/examples/mujoco/scenarioMJSceneVizard.py +++ b/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 diff --git a/examples/mujoco/scenarioStochasticDrag.py b/examples/mujoco/scenarioStochasticDrag.py index 06d6f7f56b1..0b137f67746 100644 --- a/examples/mujoco/scenarioStochasticDrag.py +++ b/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` 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() From 451b61be5e6fe6d479f087a2a5087c51e86c3758 Mon Sep 17 00:00:00 2001 From: Hanspeter Schaub Date: Mon, 20 Jul 2026 14:55:29 -0600 Subject: [PATCH 6/8] [#1490] updated mujoco unit tests to use gravity factor class when appropriate --- .../_UnitTest/test_cannonballDrag.py | 20 ++--- .../NBodyGravity/_UnitTest/test_gravity.py | 79 +++++++++---------- 2 files changed, 42 insertions(+), 57 deletions(-) diff --git a/src/simulation/forceTorque/cannonballDrag/_UnitTest/test_cannonballDrag.py b/src/simulation/forceTorque/cannonballDrag/_UnitTest/test_cannonballDrag.py index ea5aeebd9d5..69967d4ad02 100644 --- a/src/simulation/forceTorque/cannonballDrag/_UnitTest/test_cannonballDrag.py +++ b/src/simulation/forceTorque/cannonballDrag/_UnitTest/test_cannonballDrag.py @@ -27,13 +27,11 @@ 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 from Basilisk.simulation import cannonballDrag try: from Basilisk.simulation import mujoco - from Basilisk.simulation import NBodyGravity couldImportMujoco = True except: @@ -190,18 +188,12 @@ def cannonballDragBFrame(dragCoeff, density, area, velInertial, sigmaBN): mjBody = scene.getBody(bodyName) - planetData = simIncludeGravBody.BODY_DATA[planetCase] - gravitationalParameter = planetData.mu - planetRadius = planetData.radEquator # [m] - - gravityModel = NBodyGravity.NBodyGravity() - gravityModel.ModelTag = "gravity" - scene.AddModelToDynamicsTask(gravityModel) - - pointMassModel = pointMassGravityModel.PointMassGravityModel() - pointMassModel.muBody = gravitationalParameter - gravityModel.addGravitySource(planetCase, pointMassModel, True) - gravityModel.addGravityTarget(bodyName, mjBody) + gravFactory = simIncludeGravBody.gravBodyFactory() + planet = gravFactory.createBody(planetCase) + planet.isCentralBody = True + gravitationalParameter = planet.mu + planetRadius = planet.radEquator # [m] + gravFactory.addBodiesTo(scene) if planetCase == "earth": baseDensity = 1.217 # [kg/m^3] diff --git a/src/simulation/mujocoDynamics/NBodyGravity/_UnitTest/test_gravity.py b/src/simulation/mujocoDynamics/NBodyGravity/_UnitTest/test_gravity.py index 089fb4a9386..b3b9f481f3e 100644 --- a/src/simulation/mujocoDynamics/NBodyGravity/_UnitTest/test_gravity.py +++ b/src/simulation/mujocoDynamics/NBodyGravity/_UnitTest/test_gravity.py @@ -248,17 +248,14 @@ def test_dumbbell(showPlots, initialAngularRate): scene.setIntegrator(integ) scSim.AddModelToTask("test", scene) - # Create gravity model and add it to the scene dynamics task - gravity = NBodyGravity.NBodyGravity() - gravity.ModelTag = "gravity" - scene.AddModelToDynamicsTask(gravity) - - # Set a point mass gravity source - gravityModel = pointMassGravityModel.PointMassGravityModel() - gravityModel.muBody = mu - gravity.addGravitySource("earth", gravityModel, True) + # Use a custom factory body to preserve the gravitational parameter defined + # by this test. The factory applies gravity to every body in the scene. + gravFactory = simIncludeGravBody.gravBodyFactory() + earth = gravFactory.createCustomGravObject("earth", mu) + earth.isCentralBody = True + gravity = gravFactory.addBodiesTo(scene) - # Add a gravity target for each of the bodies of the spacecraft + # Retrieve the gravity target that the factory created for each body. # Note that because 'center' has zero mass, no force should be # produced on this body. # Also, create recorders for the state of each body and for the @@ -268,7 +265,7 @@ def test_dumbbell(showPlots, initialAngularRate): bodyStateRecorders = {} for bodyName in bodies: body: mujoco.MJBody = scene.getBody(bodyName) - target = gravity.addGravityTarget(bodyName, body) + target = gravity.getGravityTarget(bodyName) forceRecorders[bodyName] = target.massFixedForceOutMsg.recorder() scSim.AddModelToTask("test", forceRecorders[bodyName]) @@ -626,17 +623,24 @@ def test_mujocoVsSpacecraft( rN, vN = orbitalMotion.elem2rv(mu, oe) - # Create GravBodies, which will be used for the spacecraft - # and also the mujoco sim + # Configure the body names shared by the two simulation paths. Each path + # receives its own factory so that models and ephemeris modules are not + # shared across sequential simulations. bodies = ["earth"] if useThirdBodies: bodies.extend(["sun", "moon"]) - gravFactory = simIncludeGravBody.gravBodyFactory() - gravBodies = gravFactory.createBodies(bodies) - gravBodies["earth"].isCentralBody = True - if useSphericalHarmonics: - ggm03s_path = get_path(DataFile.LocalGravData.GGM03S) - gravBodies["earth"].useSphericalHarmonicsGravityModel(str(ggm03s_path), 4) + + def createGravityFactory(): + """Create an independent factory with the requested gravity models.""" + factory = simIncludeGravBody.gravBodyFactory() + gravBodies = factory.createBodies(bodies) + gravBodies["earth"].isCentralBody = True + if useSphericalHarmonics: + ggm03s_path = get_path(DataFile.LocalGravData.GGM03S) + gravBodies["earth"].useSphericalHarmonicsGravityModel( + str(ggm03s_path), 4 + ) + return factory # A decorator that times how long the function takes and # prints it to the console @@ -659,6 +663,8 @@ def wrap(): @timed def spacecraftSim(): + gravFactory = createGravityFactory() + # Create sim, process, and task scSim = SimulationBaseClass.SimBaseClass() dynProcess = scSim.CreateNewProcess("test") @@ -677,9 +683,7 @@ def spacecraftSim(): scObject.setIntegrator(integSc) scSim.AddModelToTask("test", scObject, 9) - scObject.gravField.gravBodies = spacecraft.GravBodyVector( - list(gravFactory.gravBodies.values()) - ) + gravFactory.addBodiesTo(scObject) # initialize spacecraft parameters scObject.hub.mHub = 100 # kg @@ -705,20 +709,13 @@ def spacecraftSim(): @timed def mujocoSim(): + gravFactory = createGravityFactory() + # Create sim, process, and task scSim = SimulationBaseClass.SimBaseClass() dynProcess = scSim.CreateNewProcess("test") dynProcess.addTask(scSim.CreateNewTask("test", macros.sec2nano(dt))) - # Create the spice interface module, which reports the state of - # planetary bodies - spice = spiceInterface.SpiceInterface() - spice.ModelTag = "SpiceInterface" - spice.addPlanetNames(["earth", "sun", "moon"]) - spice.UTCCalInit = utcCalInit - # spice.zeroBase = 'Earth' # Not actually needed for NBodyGravity - scSim.AddModelToTask("test", spice) - # Create mujoco scene scene = mujoco.MJScene.fromFile(XML_PATH_BALL) scene.ModelTag = "mujocoScene" @@ -726,21 +723,17 @@ def mujocoSim(): scene.setIntegrator(integScene) scSim.AddModelToTask("test", scene) - # Create N-Body Gravity - gravity = NBodyGravity.NBodyGravity() - scene.AddModelToDynamicsTask(gravity) + # MuJoCo evaluates gravity at integrator substeps, so its SPICE model + # belongs in the scene dynamics task ahead of the factory-created + # NBodyGravity model. + if useSpice: + gravFactory.createSpiceInterface(time=utcCalInit) + gravFactory.spiceObject.zeroBase = "Earth" + scene.AddModelToDynamicsTask(gravFactory.spiceObject, 75) - for (name, gravBody), stateOuMsg in zip( - gravBodies.items(), spice.planetStateOutMsgs - ): - source = gravity.addGravitySource( - name, gravBody.gravityModel, isCentralBody=(name == "earth") - ) - if useSpice: - source.stateInMsg.subscribeTo(stateOuMsg) + gravFactory.addBodiesTo(scene) body: mujoco.MJBody = scene.getBody("ball") - gravity.addGravityTarget("ball", body) bodyStateRecorder = body.getCenterOfMass().stateOutMsg.recorder() scSim.AddModelToTask("test", bodyStateRecorder) From 3047326a5afa656eb57f602a2f01274ce4f9e95c Mon Sep 17 00:00:00 2001 From: Hanspeter Schaub Date: Mon, 20 Jul 2026 14:55:53 -0600 Subject: [PATCH 7/8] [#1490] add release note snippets --- .../bskReleaseNotesSnippets/grav-factory-mujoco-scenes.rst | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 docs/source/Support/bskReleaseNotesSnippets/grav-factory-mujoco-scenes.rst diff --git a/docs/source/Support/bskReleaseNotesSnippets/grav-factory-mujoco-scenes.rst b/docs/source/Support/bskReleaseNotesSnippets/grav-factory-mujoco-scenes.rst new file mode 100644 index 00000000000..366376fb9e3 --- /dev/null +++ b/docs/source/Support/bskReleaseNotesSnippets/grav-factory-mujoco-scenes.rst @@ -0,0 +1,3 @@ +- Added ``gravBodyFactory.addBodiesTo()`` support for configuring gravity sources and targets on MuJoCo scenes. +- Added ``zeroBase`` support to :ref:`planetEphemeris` for expressing all outputs relative to a body propagated by the same module instance. +- Added :ref:`scenarioMJEarthMoonGravity`, a MuJoCo Earth-Moon gravity tutorial with selectable SPICE or :ref:`planetEphemeris` planet states. From c0fd86a11ad297e0c50a8d7d0a210144b2a1e4f0 Mon Sep 17 00:00:00 2001 From: Hanspeter Schaub Date: Mon, 20 Jul 2026 17:56:19 -0600 Subject: [PATCH 8/8] [#1490] handle missing NBodyGravity source attitude Treat an all-zero J20002Pfix matrix from a linked gravity-source ephemeris message as an identity transformation. Position-only ephemeris messages leave J20002Pfix at its default zero value. Applying that matrix previously collapsed transformed target positions to zero, which could produce a singular gravity calculation and NaN forces. The new fallback matches the existing GravityEffector behavior and interprets an unspecified source attitude as inertially aligned. Add a MuJoCo regression test using a factory-created central gravity body connected to a position-only ephemeris message. Verify that the computed force remains finite and agrees with the analytical point-mass gravity result. Verification: - NBodyGravity unit tests: 11 passed - MuJoCo dynamics tests: 68 passed - pre-commit checks passed - git diff --check passed --- .../NBodyGravity/NBodyGravity.cpp | 7 ++ .../NBodyGravity/_UnitTest/test_gravity.py | 83 +++++++++++++++++++ 2 files changed, 90 insertions(+) diff --git a/src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.cpp b/src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.cpp index cafff3491df..fce620b0d30 100644 --- a/src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.cpp +++ b/src/simulation/mujocoDynamics/NBodyGravity/NBodyGravity.cpp @@ -237,6 +237,13 @@ NBodyGravity::computeAccelerationFromSource(GravitySource& source, Eigen::Vector { auto spicePayload = readState(source, bskLogger); dcm_sourceFixedJ2000 = c2DArray2EigenMatrix3d(spicePayload.J20002Pfix); + // Position-only ephemeris messages leave the source DCM at its zero + // default. Match GravityEffector by treating that source as inertially + // aligned instead of collapsing every transformed position to zero. + if (dcm_sourceFixedJ2000.isZero()) + { + dcm_sourceFixedJ2000 = Eigen::Matrix3d::Identity(); + } r_sourceJ2000 = cArray2EigenVector3d(spicePayload.PositionVector); } diff --git a/src/simulation/mujocoDynamics/NBodyGravity/_UnitTest/test_gravity.py b/src/simulation/mujocoDynamics/NBodyGravity/_UnitTest/test_gravity.py index b3b9f481f3e..2af03aa10d5 100644 --- a/src/simulation/mujocoDynamics/NBodyGravity/_UnitTest/test_gravity.py +++ b/src/simulation/mujocoDynamics/NBodyGravity/_UnitTest/test_gravity.py @@ -19,6 +19,8 @@ import pytest import time +from Basilisk.architecture import messaging + try: from Basilisk.simulation import mujoco from Basilisk.simulation import NBodyGravity @@ -163,6 +165,87 @@ def test_pointMass(showPlots): assert getattr(oePost, attr) == pytest.approx(getattr(oe, attr), 1e-4) +@pytest.mark.skipif(not couldImportMujoco, reason="Compiled Basilisk without --mujoco") +def test_factory_source_without_attitude(): + """Verify that a position-only factory ephemeris uses an inertial attitude. + + Validation Test Description + --------------------------- + A factory-created central gravity body receives a linked planet-state + message containing position but leaving ``J20002Pfix`` at its all-zero + default. The test applies this source to a unit-mass MuJoCo body and + compares the resulting force with point-mass gravity. + + Test Parameter Discussion + ------------------------- + The source has a nonzero position in the ephemeris reference frame, while + the MuJoCo body is initialized seven million meters from the central body. + This exercises the central-body translation and the missing-attitude + fallback through the shared ``GravBodyData`` path. + + Description of Variables Being Tested + --------------------------------------- + The test verifies that ``massFixedForceOutMsg.force_S`` is finite and + equals the analytical point-mass force in the inertially aligned body frame. + """ + timeStep = 1.0 # [s] + gravitationalParameter = 0.3986004415e15 # [m^3/s^2] + targetRadius = 7000.0 * 1000.0 # [m] + sourcePosition_N = [1000.0 * 1000.0, 0.0, 0.0] # [m] + + simulation = SimulationBaseClass.SimBaseClass() + process = simulation.CreateNewProcess("factoryGravityProcess") + process.addTask( + simulation.CreateNewTask( + "factoryGravityTask", + macros.sec2nano(timeStep), + ) + ) + + sceneXml = """ + + + + + + + + + """ + scene = mujoco.MJScene(sceneXml) + scene.extraEoMCall = True + simulation.AddModelToTask("factoryGravityTask", scene) + + gravFactory = simIncludeGravBody.gravBodyFactory() + earth = gravFactory.createCustomGravObject( + "earth", + gravitationalParameter, + ) + earth.isCentralBody = True + + planetState = messaging.SpicePlanetStateMsgPayload() + planetState.PositionVector = sourcePosition_N + planetStateMsg = messaging.SpicePlanetStateMsg().write(planetState) + earth.planetBodyInMsg.subscribeTo(planetStateMsg) + + gravity = gravFactory.addBodiesTo(scene) + body = scene.getBody("ball") + forceRecorder = gravity.getGravityTarget("ball").massFixedForceOutMsg.recorder() + simulation.AddModelToTask("factoryGravityTask", forceRecorder) + + simulation.InitializeSimulation() + body.setPosition([targetRadius, 0.0, 0.0]) # [m] + simulation.ConfigureStopTime(0) + simulation.ExecuteSimulation() + + expectedForce_S = np.array( + [-gravitationalParameter / targetRadius**2, 0.0, 0.0] + ) # [N] + actualForce_S = forceRecorder.force_S[0] + assert np.all(np.isfinite(actualForce_S)) + np.testing.assert_allclose(actualForce_S, expectedForce_S, rtol=1.0e-12, atol=0.0) + + @pytest.mark.skipif(not couldImportMujoco, reason="Compiled Basilisk without --mujoco") @pytest.mark.parametrize( "showPlots, initialAngularRate", [(False, False), (False, True)]