From d0dcc4c01cf05498903ade721620292915f01d03 Mon Sep 17 00:00:00 2001 From: William Schwend Date: Mon, 9 Mar 2026 10:54:38 -0600 Subject: [PATCH 1/8] [#1311] add Scipy to requirements --- .github/workflows/requirements.txt | 1 + requirements.txt | 1 + 2 files changed, 2 insertions(+) diff --git a/.github/workflows/requirements.txt b/.github/workflows/requirements.txt index ba615567047..ee69f44e9a7 100644 --- a/.github/workflows/requirements.txt +++ b/.github/workflows/requirements.txt @@ -1,6 +1,7 @@ pandas matplotlib numpy +scipy colorama tqdm pillow diff --git a/requirements.txt b/requirements.txt index d5cacca9db8..dfa4aecad27 100644 --- a/requirements.txt +++ b/requirements.txt @@ -3,6 +3,7 @@ pandas>=2.0.3,<=2.3.3; python_version >= "3.9" matplotlib>=3.7.5,<=3.10.7 numpy>=1.24.4,<2.4.0; python_version < "3.13" numpy>=2.0,<2.4.0; python_version >= "3.13" +scipy==1.14.1 colorama==0.4.6 tqdm==4.67.1 pillow>=10.4.0,<=12.0.0 From 671437ef705a035b3078b8aeba67883910656858 Mon Sep 17 00:00:00 2001 From: William Schwend Date: Thu, 19 Feb 2026 10:24:32 -0700 Subject: [PATCH 2/8] [#1311] add support python modules for new example --- examples/mujoco/sat_w_thruster_arms.xml | 56 +++ .../jointThrAllocation.py | 345 ++++++++++++++++++ .../mujoco/thrArmControlSupport/stateMerge.py | 56 +++ .../thrArmControlSupport/thrFiringRound.py | 117 ++++++ 4 files changed, 574 insertions(+) create mode 100644 examples/mujoco/sat_w_thruster_arms.xml create mode 100644 examples/mujoco/thrArmControlSupport/jointThrAllocation.py create mode 100644 examples/mujoco/thrArmControlSupport/stateMerge.py create mode 100644 examples/mujoco/thrArmControlSupport/thrFiringRound.py diff --git a/examples/mujoco/sat_w_thruster_arms.xml b/examples/mujoco/sat_w_thruster_arms.xml new file mode 100644 index 00000000000..5372d1c6d7f --- /dev/null +++ b/examples/mujoco/sat_w_thruster_arms.xml @@ -0,0 +1,56 @@ + + diff --git a/examples/mujoco/thrArmControlSupport/jointThrAllocation.py b/examples/mujoco/thrArmControlSupport/jointThrAllocation.py new file mode 100644 index 00000000000..de3c2d30d3c --- /dev/null +++ b/examples/mujoco/thrArmControlSupport/jointThrAllocation.py @@ -0,0 +1,345 @@ +# +# 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. + +import numpy as np +from scipy.optimize import minimize + +from Basilisk.architecture import messaging, sysModel +from Basilisk.utilities import RigidBodyKinematics as rbk + +def mapMatrix(rVec_B: np.ndarray, fHatVec_B: np.ndarray, r_ComB_B: np.ndarray) -> np.ndarray: + """Build wrench map U=[F;L]=T@f where f are thruster magnitudes.""" + rVec_B = np.asarray(rVec_B, dtype=float) + fHatVec_B = np.asarray(fHatVec_B, dtype=float) + r_ComB_B = np.asarray(r_ComB_B, dtype=float).reshape(3) + forceBlock = fHatVec_B.T + torqueBlock = np.cross(rVec_B - r_ComB_B.reshape(1, 3), fHatVec_B, axis=1).T + return np.vstack((forceBlock, torqueBlock)) + + +class JointThrAllocation(sysModel.SysModel): + """ + Example support module for thruster-on-arm allocation. + + This implementation is intentionally example-oriented with explicit assumptions: + - Arms are serial chains packed in arm order. + - Arm geometry/config comes from THRArmConfigMsgPayload. + - One spacecraft tree is supported (all arms in same kinematic tree). + - Exactly one thruster per arm (configurable check). + - Thruster parent joint is the last joint in each arm (configurable check). + """ + + def __init__(self): + super().__init__() + + # Input messages + self.armConfigInMsg = messaging.THRArmConfigMsgReader() + self.CoMStatesInMsg = messaging.SCStatesMsgReader() + self.hubStatesInMsg = messaging.SCStatesMsgReader() + self.transForceInMsg = messaging.CmdForceInertialMsgReader() + self.rotTorqueInMsg = messaging.CmdTorqueBodyMsgReader() + + # Output messages + self.thrForceOutMsg = messaging.THRArrayCmdForceMsg() + self.desJointAnglesOutMsg = messaging.JointArrayStateMsg() + self.thrForcePayload = messaging.THRArrayCmdForceMsgPayload() + self.jointAnglePayload = messaging.JointArrayStateMsgPayload() + + # Cost settings + self.Wc = np.eye(6) + self.WfScale = 1e-6 + self.Wf = None + + # Optimization settings + self.maxiter = 3000 + self.ftol = 1e-10 + self.errTol = 1e-4 + self.thrForceMax = 2.5 + + # Runtime data + self.nArms = 0 + self.nThr = 0 + self.nJoint = 0 + self.armJointCount = None + self.armJointStart = None + self.thrArmIdx = None + self.thrArmJointIdx = None + self.r_CP_P = None + self.r_TP_P = None + self.sHat_P = None + self.fHat_P = None + self.dcm_C0P = None + self.x0 = None + + def setWf(self, wfIn): + """ + Set thrust-weight term for the cost function. + + Accepted inputs: + - scalar: applies same weight to all thrusters + - vector length nThr: per-thruster weights + """ + wfArr = np.asarray(wfIn, dtype=float) + if wfArr.ndim == 0: + self.WfScale = float(wfArr) + self.Wf = None + return + if wfArr.ndim != 1: + raise ValueError("setWf expects a scalar or 1D vector.") + self.Wf = wfArr.copy() + + def setWc(self, wcIn): + """ + Set wrench tracking weights for the cost function. + + Accepted inputs: + - scalar: wc * I6 + - length-6 vector: diag(wc) + - 6x6 matrix + """ + wcArr = np.asarray(wcIn, dtype=float) + if wcArr.ndim == 0: + self.Wc = float(wcArr) * np.eye(6) + return + if wcArr.ndim == 1 and wcArr.size == 6: + self.Wc = np.diag(wcArr) + return + if wcArr.shape == (6, 6): + self.Wc = wcArr.copy() + return + raise ValueError("setWc expects scalar, length-6 vector, or 6x6 matrix.") + + def resolveWf(self): + """Resolve Wf to a length-nThr vector after nThr is known.""" + if self.Wf is None: + self.Wf = np.full(self.nThr, self.WfScale, dtype=float) + return + + wfArr = np.asarray(self.Wf, dtype=float) + if wfArr.ndim == 0: + self.Wf = np.full(self.nThr, float(wfArr), dtype=float) + return + if wfArr.ndim != 1: + raise ValueError("Wf must be scalar or 1D vector.") + if wfArr.size == 1: + self.Wf = np.full(self.nThr, float(wfArr[0]), dtype=float) + return + if wfArr.size != self.nThr: + raise ValueError(f"Wf vector length {wfArr.size} does not match nThr {self.nThr}.") + self.Wf = wfArr.copy() + + def setThrForceMax(self, thrForceMaxIn): + """ + Set thrust upper bounds. + + Accepted inputs: + - scalar: same upper bound for all thrusters + - vector length nThr: per-thruster upper bounds + """ + thrForceMaxArr = np.asarray(thrForceMaxIn, dtype=float) + if thrForceMaxArr.ndim == 0: + self.thrForceMax = float(thrForceMaxArr) + return + if thrForceMaxArr.ndim != 1: + raise ValueError("setThrForceMax expects a scalar or 1D vector.") + self.thrForceMax = thrForceMaxArr.copy() + + def resolveThrForceMax(self): + """Resolve thrForceMax to a length-nThr vector after nThr is known.""" + thrForceMaxArr = np.asarray(self.thrForceMax, dtype=float) + if thrForceMaxArr.ndim == 0: + return np.full(self.nThr, float(thrForceMaxArr), dtype=float) + if thrForceMaxArr.ndim != 1: + raise ValueError("thrForceMax must be scalar or 1D vector.") + if thrForceMaxArr.size == 1: + return np.full(self.nThr, float(thrForceMaxArr[0]), dtype=float) + if thrForceMaxArr.size != self.nThr: + raise ValueError(f"thrForceMax vector length {thrForceMaxArr.size} does not match nThr {self.nThr}.") + return thrForceMaxArr.copy() + + def parseArmConfig(self): + cfgMsg = self.armConfigInMsg() + + self.thrArmIdx = np.asarray(cfgMsg.thrArmIdx, dtype=int) + self.thrArmJointIdx = np.asarray(cfgMsg.thrArmJointIdx, dtype=int) + self.armJointCount = np.asarray(cfgMsg.armJointCount, dtype=int) + + self.nArms = int(self.armJointCount.size) + self.nThr = int(self.thrArmIdx.size) + self.nJoint = int(np.sum(self.armJointCount)) + + self.armJointStart = np.zeros(self.nArms, dtype=int) + cumulativeCount = 0 + for armIdx in range(self.nArms): + self.armJointStart[armIdx] = cumulativeCount + cumulativeCount += int(self.armJointCount[armIdx]) + + self.r_CP_P = np.asarray(cfgMsg.r_CP_P, dtype=float).reshape(-1, 3) + self.r_TP_P = np.asarray(cfgMsg.r_TP_P, dtype=float).reshape(-1, 3) + self.sHat_P = np.asarray(cfgMsg.shat_P, dtype=float).reshape(-1, 3) + self.fHat_P = np.asarray(cfgMsg.fhat_P, dtype=float).reshape(-1, 3) + self.dcm_C0P = np.asarray(cfgMsg.dcm_C0P, dtype=float).reshape(-1, 9) + + # Convert each flat 9-entry block from column-major to 3x3 matrix. + self.dcm_C0P = np.array([dcmFlat.reshape(3, 3, order="F") for dcmFlat in self.dcm_C0P], dtype=float) + + def initialGuesses(self): + nDecision = self.nJoint + self.nThr + seedList = [] + + guess = np.zeros(nDecision) + guess[self.nJoint:] = 1.0 + seedList.append(guess) + + for angleSeed in (np.pi / 4.0, -np.pi / 4.0, np.pi / 2.0, -np.pi / 2.0): + guess = np.zeros(nDecision) + guess[: self.nJoint] = angleSeed + guess[self.nJoint:] = 1.0 + seedList.append(guess) + + self.x0 = np.vstack(seedList) + + def bounds(self): + nDecision = self.nJoint + self.nThr + lowerBound = np.zeros(nDecision) + upperBound = np.zeros(nDecision) + lowerBound[: self.nJoint] = -np.pi + upperBound[: self.nJoint] = np.pi + lowerBound[self.nJoint:] = 0.0 + upperBound[self.nJoint:] = self.resolveThrForceMax() + return tuple((float(low), float(high)) for low, high in zip(lowerBound, upperBound)) + + def jointPoseFromTheta(self, theta: np.ndarray): + """ + Return joint frame poses in B: + - dcm_cb[k]: DCM from joint frame C_k to body frame B + - r_cb_b[k]: position of joint frame C_k origin in B + """ + dcm_CB = [np.eye(3) for _ in range(self.nJoint)] + r_CB_B = [np.zeros(3) for _ in range(self.nJoint)] + + for armIdx in range(self.nArms): + armStart = int(self.armJointStart[armIdx]) + armJointNum = int(self.armJointCount[armIdx]) + + for jointLocalIdx in range(armJointNum): + jointFlatIdx = armStart + jointLocalIdx + if jointLocalIdx == 0: + dcm_PB = np.eye(3) + r_PB_B = np.zeros(3) + else: + priorJointIdx = jointFlatIdx - 1 + dcm_PB = dcm_CB[priorJointIdx] + r_PB_B = r_CB_B[priorJointIdx] + + dcm_CC0 = rbk.PRV2C(theta[jointFlatIdx] * self.sHat_P[jointFlatIdx]) + dcm_CP = dcm_CC0 @ self.dcm_C0P[jointFlatIdx] + + r_CB_B[jointFlatIdx] = r_PB_B + dcm_PB.T @ self.r_CP_P[jointFlatIdx] + dcm_CB[jointFlatIdx] = dcm_CP @ dcm_PB + + return dcm_CB, r_CB_B + + def mapping(self, theta: np.ndarray, r_ComB_B: np.ndarray): + dcm_CB, r_CB_B = self.jointPoseFromTheta(theta) + + r_TB_B = np.zeros((self.nThr, 3)) + fHatVec_B = np.zeros((self.nThr, 3)) + + for thrIdx in range(self.nThr): + armIdx = int(self.thrArmIdx[thrIdx]) + jointLocalIdx = int(self.thrArmJointIdx[thrIdx]) + jointFlatIdx = int(self.armJointStart[armIdx] + jointLocalIdx) + + r_TB_B[thrIdx] = r_CB_B[jointFlatIdx] + dcm_CB[jointFlatIdx].T @ self.r_TP_P[thrIdx] + fHatVec_B[thrIdx] = dcm_CB[jointFlatIdx].T @ self.fHat_P[thrIdx] + + return mapMatrix(r_TB_B, fHatVec_B, r_ComB_B) + + def cost(self, decisionVar: np.ndarray, r_ComB_B: np.ndarray, desiredWrench_B: np.ndarray) -> float: + theta = decisionVar[: self.nJoint] + thrForces = decisionVar[self.nJoint:] + wrenchMap = self.mapping(theta, r_ComB_B) + wrenchError = desiredWrench_B - wrenchMap @ thrForces + return float(wrenchError.T @ self.Wc @ wrenchError + self.Wf.T @ thrForces) + + def Reset(self, CurrentSimNanos): + self.parseArmConfig() + self.resolveWf() + self.initialGuesses() + + self.desJointAnglesOutMsg.write(messaging.JointArrayStateMsgPayload()) + self.thrForceOutMsg.write(messaging.THRArrayCmdForceMsgPayload()) + + def UpdateState(self, CurrentSimNanos): + comStates = self.CoMStatesInMsg() + hubStates = self.hubStatesInMsg() + + r_ComB_N = np.array(comStates.r_CN_N).reshape(3) - np.array(hubStates.r_BN_N).reshape(3) + sigmaBN = np.array(hubStates.sigma_BN).reshape(3) + dcm_BN = rbk.MRP2C(sigmaBN) + r_ComB_B = dcm_BN @ r_ComB_N + + forceInertialMsg = self.transForceInMsg() + torqueBodyMsg = self.rotTorqueInMsg() + + desiredForce_N = np.array(forceInertialMsg.forceRequestInertial).reshape(3) + desiredForce_B = dcm_BN @ desiredForce_N + desiredTorque_B = np.array(torqueBodyMsg.torqueRequestBody).reshape(3) + desiredWrench_B = np.hstack((desiredForce_B, desiredTorque_B)) + + optOptions = {"maxiter": self.maxiter, "ftol": self.ftol, "disp": False} + boundTuple = self.bounds() + + bestDecision = None + bestErrInf = np.inf + + for initialDecision in self.x0: + optResult = minimize( + fun=lambda decision: self.cost(decision, r_ComB_B, desiredWrench_B), + x0=initialDecision, + bounds=boundTuple, + method="SLSQP", + options=optOptions, + ) + if not optResult.success: + continue + + decisionOpt = optResult.x + wrenchError = ( + desiredWrench_B + - self.mapping(decisionOpt[: self.nJoint], r_ComB_B) @ decisionOpt[self.nJoint:] + ) + errInf = float(np.linalg.norm(wrenchError, ord=np.inf)) + if errInf < bestErrInf: + bestErrInf = errInf + bestDecision = decisionOpt + if bestErrInf <= self.errTol: + break + + if bestDecision is None: + bestDecision = np.zeros(self.nJoint + self.nThr) + + self.thrForcePayload.thrForce = bestDecision[self.nJoint:].tolist() + self.thrForceOutMsg.write(self.thrForcePayload, CurrentSimNanos, self.moduleID) + + self.jointAnglePayload.states.clear() + self.jointAnglePayload.stateDots.clear() + for angleCmd in bestDecision[: self.nJoint]: + self.jointAnglePayload.states.push_back(float(angleCmd)) + self.jointAnglePayload.stateDots.push_back(0.0) + self.desJointAnglesOutMsg.write(self.jointAnglePayload, CurrentSimNanos, self.moduleID) diff --git a/examples/mujoco/thrArmControlSupport/stateMerge.py b/examples/mujoco/thrArmControlSupport/stateMerge.py new file mode 100644 index 00000000000..a490fcbee43 --- /dev/null +++ b/examples/mujoco/thrArmControlSupport/stateMerge.py @@ -0,0 +1,56 @@ +# +# 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. + +from Basilisk.architecture import messaging, sysModel + + +class StateMerge(sysModel.SysModel): + """ + Merge two SCStates messages into one SCStates output message for simpleNav use. + + Inputs: + - attStateInMsg: attitude-centric state source + - transStateInMsg: translation-centric state source + + Output: + - stateOutMsg: merged state output for simpleNav use + """ + + def __init__(self): + super().__init__() + self.attStateInMsg = messaging.SCStatesMsgReader() + self.transStateInMsg = messaging.SCStatesMsgReader() + self.stateOutMsg = messaging.SCStatesMsg() + self.stateOut = messaging.SCStatesMsgPayload() + + def Reset(self, CurrentSimNanos): + self.stateOutMsg.write(messaging.SCStatesMsgPayload()) + + def UpdateState(self, CurrentSimNanos): + attState = self.attStateInMsg() + transState = self.transStateInMsg() + + # Copy over attitude values from attitude source + self.stateOut.sigma_BN = list(attState.sigma_BN) + self.stateOut.omega_BN_B = list(attState.omega_BN_B) + + # Relocate the position/velocity from the translation source + # this is relocation needed so due to later modules calling r_BN_N for things that relate to CoM + self.stateOut.r_BN_N = list(transState.r_CN_N) + self.stateOut.v_BN_N = list(transState.v_CN_N) + + self.stateOutMsg.write(self.stateOut, CurrentSimNanos, self.moduleID) diff --git a/examples/mujoco/thrArmControlSupport/thrFiringRound.py b/examples/mujoco/thrArmControlSupport/thrFiringRound.py new file mode 100644 index 00000000000..88e37fc8780 --- /dev/null +++ b/examples/mujoco/thrArmControlSupport/thrFiringRound.py @@ -0,0 +1,117 @@ +# +# 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. + +import numpy as np + +from Basilisk.architecture import messaging, sysModel + + +class ThrFiringRound(sysModel.SysModel): + """ + Convert thruster force commands into thruster on-time commands. + + Mapping per thruster i: + onTime_i = controlPeriodSec * force_i / thrForceMax_i + + Then rounded to nearest multiple of onTimeResolutionSec. + """ + + def __init__(self): + super().__init__() + + # Input/output messages + self.thrForceInMsg = messaging.THRArrayCmdForceMsgReader() + self.thrOnTimeOutMsg = messaging.THRArrayOnTimeCmdMsg() + self.thrOnTimePayload = messaging.THRArrayOnTimeCmdMsgPayload() + + # Configuration + self.controlPeriodSec = 1.0 + self.onTimeResolutionSec = 0.01 + self.numThrusters = None + self.thrForceMax = 1.0 + + def setControlPeriodSec(self, controlPeriodSecIn: float): + self.controlPeriodSec = float(controlPeriodSecIn) + + def setOnTimeResolutionSec(self, onTimeResolutionSecIn: float): + self.onTimeResolutionSec = float(onTimeResolutionSecIn) + + def setNumThrusters(self, numThrustersIn: int): + self.numThrusters = int(numThrustersIn) + + def setThrForceMax(self, thrForceMaxIn): + """ + Set thrust normalization for force->on-time conversion. + + Accepted: + - scalar: same max force for all thrusters + - vector length nThr: per-thruster max force + """ + thrForceMaxArr = np.asarray(thrForceMaxIn, dtype=float) + if thrForceMaxArr.ndim == 0: + self.thrForceMax = float(thrForceMaxArr) + return + if thrForceMaxArr.ndim != 1: + raise ValueError("setThrForceMax expects scalar or 1D vector.") + for thr in thrForceMaxArr: + if thr <= 0.0: + raise ValueError("setThrForceMax expects positive values.") + self.thrForceMax = thrForceMaxArr.copy() + + def resolveNumThrusters(self, thrForceCmd: np.ndarray) -> int: + + thrForceMaxArr = np.asarray(self.thrForceMax, dtype=float) + if thrForceMaxArr.ndim == 1: + return int(thrForceMaxArr.size) + + return int(thrForceCmd.size) + + def resolveThrForceMax(self, nThr: int) -> np.ndarray: + thrForceMaxArr = np.asarray(self.thrForceMax, dtype=float) + if thrForceMaxArr.ndim == 0: + return np.full(nThr, float(thrForceMaxArr), dtype=float) + if thrForceMaxArr.ndim != 1: + raise ValueError("thrForceMax must be scalar or 1D vector.") + if thrForceMaxArr.size == 1: + return np.full(nThr, float(thrForceMaxArr[0]), dtype=float) + if thrForceMaxArr.size != nThr: + raise ValueError(f"thrForceMax vector length {thrForceMaxArr.size} does not match nThr {nThr}.") + return thrForceMaxArr.copy() + + def Reset(self, CurrentSimNanos): + self.thrOnTimeOutMsg.write(messaging.THRArrayOnTimeCmdMsgPayload()) + + def UpdateState(self, CurrentSimNanos): + thrForceMsg = self.thrForceInMsg() + thrForceCmd = np.asarray(thrForceMsg.thrForce, dtype=float) + + if self.numThrusters is None: + nThr = self.resolveNumThrusters(thrForceCmd) + else: + nThr = self.numThrusters + thrForceMax = self.resolveThrForceMax(nThr) + + # Force commands are one-sided for on-time logic. + forceCmd = np.maximum(thrForceCmd[:nThr], 0.0) + onTimeRequest = np.zeros(nThr) + onTimeRequest = self.controlPeriodSec * forceCmd / thrForceMax + onTimeRequest = np.clip(onTimeRequest, 0.0, self.controlPeriodSec) + onTimeRequest = np.rint(onTimeRequest / self.onTimeResolutionSec) * self.onTimeResolutionSec + onTimeRequest = np.clip(onTimeRequest, 0.0, self.controlPeriodSec) + + self.thrOnTimePayload.OnTimeRequest = onTimeRequest.tolist() + self.thrOnTimeOutMsg.write(self.thrOnTimePayload, CurrentSimNanos, self.moduleID) From 584b9f131b4d1845ee1bd11f818ced297c770c89 Mon Sep 17 00:00:00 2001 From: William Schwend Date: Fri, 20 Feb 2026 13:41:31 -0700 Subject: [PATCH 3/8] [#1311] add Bsk_MujcocSim files for MuJoCo example scenario --- examples/mujoco/BSK_mujocoMasters.py | 94 +++ .../mujoco/mujocoModels/BSK_MujocoDynamics.py | 87 +++ examples/mujoco/mujocoModels/BSK_MujocoFSW.py | 551 ++++++++++++++++++ 3 files changed, 732 insertions(+) create mode 100644 examples/mujoco/BSK_mujocoMasters.py create mode 100644 examples/mujoco/mujocoModels/BSK_MujocoDynamics.py create mode 100644 examples/mujoco/mujocoModels/BSK_MujocoFSW.py diff --git a/examples/mujoco/BSK_mujocoMasters.py b/examples/mujoco/BSK_mujocoMasters.py new file mode 100644 index 00000000000..ea3d0690360 --- /dev/null +++ b/examples/mujoco/BSK_mujocoMasters.py @@ -0,0 +1,94 @@ +# +# 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. +# + +# Get current file path +import inspect +import os +import sys + +# Import architectural modules +from Basilisk.utilities import SimulationBaseClass + +filename = inspect.getframeinfo(inspect.currentframe()).filename +path = os.path.dirname(os.path.abspath(filename)) + +# Import MuJoCo Dynamics and FSW models +sys.path.append(path + '/mujocoModels') + + +class BSKSim(SimulationBaseClass.SimBaseClass): + """Main bskSim simulation class""" + + def __init__(self, fswRate=0.01, dynRate=0.01): + self.dynRate = dynRate + self.fswRate = fswRate + # Create a sim module as an empty container + SimulationBaseClass.SimBaseClass.__init__(self) + + self.DynModels = [] + self.FSWModels = [] + self.DynamicsProcessName = None + self.FSWProcessName = None + self.dynProc = None + self.fswProc = None + + self.dynamics_added = False + self.fsw_added = False + + def get_DynModel(self): + assert (self.dynamics_added is True), "It is mandatory to use a dynamics model as an argument" + return self.DynModels + + def set_DynModel(self, dynModel): + self.dynamics_added = True + self.DynamicsProcessName = 'DynamicsProcess' # Create simulation process name + self.dynProc = self.CreateNewProcess(self.DynamicsProcessName) # Create process + self.DynModels = dynModel.BSKMujocoDynamicsModels(self, self.dynRate) # Create Dynamics class + + def get_FswModel(self): + assert (self.fsw_added is True), "A flight software model has not been added yet" + return self.FSWModels + + def set_FswModel(self, fswModel): + self.fsw_added = True + self.FSWProcessName = "FSWProcess" # Create simulation process name + self.fswProc = self.CreateNewProcess(self.FSWProcessName) # Create process + self.FSWModels = fswModel.BSKMujocoFSWModels(self, self.fswRate) # Create FSW class + + +class BSKScenario(object): + def __init__(self): + self.name = "scenario" + + def configure_initial_conditions(self): + """ + Developer must override this method in their BSK_Scenario derived subclass. + """ + pass + + def log_outputs(self): + """ + Developer must override this method in their BSK_Scenario derived subclass. + """ + pass + + def pull_outputs(self): + """ + Developer must override this method in their BSK_Scenario derived subclass. + """ + pass diff --git a/examples/mujoco/mujocoModels/BSK_MujocoDynamics.py b/examples/mujoco/mujocoModels/BSK_MujocoDynamics.py new file mode 100644 index 00000000000..989567306a2 --- /dev/null +++ b/examples/mujoco/mujocoModels/BSK_MujocoDynamics.py @@ -0,0 +1,87 @@ +# +# 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. + +import os +from Basilisk.simulation import( + mujoco, + MJSystemCoM, + MJSystemMassMatrix, + MJJointReactionForces, + svIntegrators, +) +from Basilisk.utilities import macros as mc + +XML_PATH = os.path.join(os.path.dirname(__file__), "..", "sat_w_thruster_arms.xml") + + +class BSKMujocoDynamicsModels(): + """ + bskSim simulation class that sets up the MuJoCo spacecraft dynamics models. + """ + def __init__(self, SimBase, dynRate): + # Define process name, task name and task time-step + self.processName = SimBase.DynamicsProcessName + self.taskName = "DynamicsTask" + self.processTasksTimeStep = mc.sec2nano(dynRate) + + # Create task + SimBase.dynProc.addTask(SimBase.CreateNewTask(self.taskName, self.processTasksTimeStep)) + + # Instantiate Dyn modules as objects + self.scene = mujoco.MJScene.fromFile(XML_PATH) + self.systemCoM = MJSystemCoM.MJSystemCoM() + self.systemMassMatrix = MJSystemMassMatrix.MJSystemMassMatrix() + self.jointReactionForces = MJJointReactionForces.MJJointReactionForces() + self.integrator = svIntegrators.svIntegratorRKF45(self.scene) + + # Initialize all modules + self.InitAllDynObjects() + + # Assign all initialized modules to tasks + SimBase.AddModelToTask(self.taskName, self.scene, 200) + SimBase.AddModelToTask(self.taskName, self.systemCoM, 199) + SimBase.AddModelToTask(self.taskName, self.systemMassMatrix, 198) + SimBase.AddModelToTask(self.taskName, self.jointReactionForces, 197) + + + # ------------------------------------------------------------------------------------------- # + # These are module-initialization methods + + def setScene(self): + self.scene.extraEoMCall = True + self.scene.ModelTag = "mujocoScene" + self.scene.setIntegrator(self.integrator) + + def setSystemCoM(self): + self.systemCoM.ModelTag = "systemCoM" + self.systemCoM.scene = self.scene + + def setSystemMassMatrix(self): + self.systemMassMatrix.ModelTag = "systemMassMatrix" + self.systemMassMatrix.scene = self.scene + + def setJointReactionForces(self): + self.jointReactionForces.ModelTag = "jointReactionForces" + self.jointReactionForces.scene = self.scene + + + # Global call to initialize every module + def InitAllDynObjects(self): + self.setScene() + self.setSystemCoM() + self.setSystemMassMatrix() + self.setJointReactionForces() diff --git a/examples/mujoco/mujocoModels/BSK_MujocoFSW.py b/examples/mujoco/mujocoModels/BSK_MujocoFSW.py new file mode 100644 index 00000000000..2aa48ddee4b --- /dev/null +++ b/examples/mujoco/mujocoModels/BSK_MujocoFSW.py @@ -0,0 +1,551 @@ +# +# 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. + +import os +import sys +import numpy as np +supportFilePath = os.path.join(os.path.dirname(__file__), "..", "thrArmControlSupport") +sys.path.append(supportFilePath) +import jointThrAllocation +import stateMerge +import thrFiringRound +from Basilisk.architecture import messaging +from Basilisk.utilities import macros as mc +from Basilisk.fswAlgorithms import ( + hingedJointArrayMotor, + jointMotionCompensator, + thrJointCompensation, + inertial3D, + attTrackingError, + mrpFeedback, + inertialCartFeedback, +) +from Basilisk.simulation import ( + thrOnTimeToForce, + simpleNav, +) + + +class BSKMujocoFSWModels: + """Defines the bskSim FSW class""" + def __init__(self, SimBase, fswRate): + # define empty class variables + self.vcMsg = None + self.thrArmConfigMsg = None + self.transRefMsg = None + self.jointTorqueGateways = [] + self.thrForceGateways = [] + self.bodyTorqueGateway = [] + self.thrFiringTimeGateway = None + + # Define process name and task time-steps + self.processName = SimBase.FSWProcessName + self.innerLoopTimeStep = mc.sec2nano(fswRate) + self.outerLoopTimeStep = mc.sec2nano(10.0) + + # Define default values for the scenario + self.numJoints = 8 + self.numThrusters = 2 + self.thrForces = [2.5]*self.numThrusters # N + self.maxJointTorques = [22.5]*self.numJoints # Nm + self.controlWeight = 1.0 + self.thrustWeight = 1e-6 + + # Define the default control gains + self.Tpos = 27.0 + self.Tatt = 10.0 + self.omegaOuter = 2*np.pi/10 + self.omegaInner = 5*self.omegaOuter + piRot = (2*1/3*330.0*(0.79**2+0.69**2))/self.Tatt + self.pRot = piRot + xiRot = 1.0 + self.kRot = piRot**2/(4*xiRot**2*(1/3*330.0*(0.79**2+0.69**2))) + self.kiRot = -1.0 + piPos = 2.0*350.0/self.Tpos + self.pTrans = (piPos * np.eye(3)).flatten().tolist() + xiPos = 1.0 + self.kTrans = (piPos**2/(4*xiPos**2*350.0) * np.eye(3)).flatten().tolist() + kiMotor = self.omegaInner**2 + self.kTheta = (kiMotor * np.eye(8)).flatten().tolist() + xiMotor = 1.5 + piMotor = 2 * xiMotor * self.omegaInner + self.pTheta = (piMotor * np.eye(8)).flatten().tolist() + + + # Define the XML object names for later use + self.hubName = "hub" + self.jointRefs = [ + ("arm_1_base", "arm_1_joint_1"), + ("arm_1_base", "arm_1_joint_2"), + ("arm_1_tip", "arm_1_joint_3"), + ("arm_1_tip", "arm_1_joint_4"), + ("arm_2_base", "arm_2_joint_1"), + ("arm_2_base", "arm_2_joint_2"), + ("arm_2_tip", "arm_2_joint_3"), + ("arm_2_tip", "arm_2_joint_4"), + ] + self.jointActNames = ["u_arm_1_joint_1","u_arm_1_joint_2","u_arm_1_joint_3","u_arm_1_joint_4", + "u_arm_2_joint_1","u_arm_2_joint_2","u_arm_2_joint_3","u_arm_2_joint_4"] + self.thrActNames = ["F_thruster_1","F_thruster_2"] + self.bodyActNames = ["tau_prescribed_1","tau_prescribed_2","tau_prescribed_3"] + + # Define initial event check values + self.lastDesAngles = [0.0]*self.numJoints + self.lastDesAngleRates = [0.0]*self.numJoints + self.lastDesCmdWriteTime = -1 + self.thrustJustActivated = False + + # Instantiate FSW modules as objects + self.inertial3D = inertial3D.inertial3D() + self.stateMerge = stateMerge.StateMerge() + self.navConverter = simpleNav.SimpleNav() + self.attError = attTrackingError.attTrackingError() + self.mrpFeedback = mrpFeedback.mrpFeedback() + self.inertialCartFeedback = inertialCartFeedback.InertialCartFeedback() + self.jointThrAllocation = jointThrAllocation.JointThrAllocation() + self.thrFiringRound = thrFiringRound.ThrFiringRound() + self.hingedJointArrayMotor = hingedJointArrayMotor.HingedJointArrayMotor() + self.jointMotionCompensator = jointMotionCompensator.JointMotionCompensator() + self.thrOnTimeToForce = thrOnTimeToForce.ThrOnTimeToForce() + self.thrJointCompensation = thrJointCompensation.ThrJointCompensation() + + # Create the FSW module gateway messages + self.setupGatewayMsgs(SimBase) + + # Initialize all modules + self.InitAllFSWObjects(SimBase) + + # Create tasks + self.outerLoopTaskName = "outerLoopTask" + self.jointMotionTaskName = "jointMotionTask" + self.thrFiringTaskName = "thrFiringTask" + SimBase.fswProc.addTask(SimBase.CreateNewTask(self.outerLoopTaskName, self.outerLoopTimeStep), 100) + SimBase.fswProc.addTask(SimBase.CreateNewTask(self.jointMotionTaskName, self.innerLoopTimeStep), 99) + SimBase.fswProc.addTask(SimBase.CreateNewTask(self.thrFiringTaskName, self.innerLoopTimeStep), 98) + + # Assign initialized modules to tasks + SimBase.AddModelToTask(self.outerLoopTaskName, self.inertial3D, 50) + SimBase.AddModelToTask(self.outerLoopTaskName, self.stateMerge, 49) + SimBase.AddModelToTask(self.outerLoopTaskName, self.navConverter, 48) + SimBase.AddModelToTask(self.outerLoopTaskName, self.attError, 47) + SimBase.AddModelToTask(self.outerLoopTaskName, self.mrpFeedback, 46) + SimBase.AddModelToTask(self.outerLoopTaskName, self.inertialCartFeedback, 45) + SimBase.AddModelToTask(self.outerLoopTaskName, self.jointThrAllocation, 44) + SimBase.AddModelToTask(self.outerLoopTaskName, self.thrFiringRound, 43) + + SimBase.AddModelToTask(self.jointMotionTaskName, self.hingedJointArrayMotor, 50) + SimBase.AddModelToTask(self.jointMotionTaskName, self.jointMotionCompensator, 49) + + SimBase.AddModelToTask(self.thrFiringTaskName, self.thrOnTimeToForce, 50) + SimBase.AddModelToTask(self.thrFiringTaskName, self.thrJointCompensation, 49) + + # Create events to to be called for changing active controllers + SimBase.fswProc.disableAllTasks() + self.zeroGatewayMsgs() + SimBase.enableTask(self.outerLoopTaskName) + + def conditionJointMotion(self): + """Event condition to use the joint motion controller after the outer + control loop has been updated""" + desAngles = list(self.FSWModels.desAngleReader().states) + desAngleRates = list(self.FSWModels.desAngleReader().stateDots) + currentWriteTime = self.FSWModels.desAngleReader.timeWritten() + if len(desAngles) != self.FSWModels.numJoints or len(desAngleRates) != self.FSWModels.numJoints: + return False + changed = currentWriteTime != self.FSWModels.lastDesCmdWriteTime + if changed: + self.FSWModels.lastDesAngles = desAngles[:] + self.FSWModels.lastDesAngleRates = desAngleRates[:] + self.FSWModels.lastDesCmdWriteTime = currentWriteTime + return changed + + def actionJointMotion(self): + """Event action to activate the joint motion controller when there + is a change in the desired joint angles or thruster forces""" + self.fswProc.disableAllTasks() + self.FSWModels.zeroGatewayMsgs() + self.enableTask(self.FSWModels.outerLoopTaskName) + self.enableTask(self.FSWModels.jointMotionTaskName) + self.setEventActivity("jointMotion", True) + self.setEventActivity("thrFiring", True) + self.setEventActivity("coast", False) + + SimBase.createNewEvent( + "jointMotion", + self.innerLoopTimeStep, + True, + conditionFunction=conditionJointMotion, + actionFunction=actionJointMotion, + ) + + def conditionThrFiring(self): + """Event condition to fire the thrusters once the joints have + reached the desired angles and stopped""" + desStates = np.asarray(self.FSWModels.lastDesAngles, dtype=float) + desStatesDot = np.asarray(self.FSWModels.lastDesAngleRates, dtype=float) + curStates = np.zeros(self.FSWModels.numJoints) + curStatesDot = np.zeros(self.FSWModels.numJoints) + for i in range(self.FSWModels.numJoints): + curStates[i] = self.FSWModels.anglesReaders[i]().state + curStatesDot[i] = self.FSWModels.angleRatesReaders[i]().state + angError = self.FSWModels.wrapAngle(desStates - curStates) + stateError = np.max(np.abs(angError)) + rateError = np.max(np.abs(desStatesDot - curStatesDot)) + return (stateError < 1e-3) and (rateError < 1e-3) + + def actionThrFiring(self): + """Event action to activate the thruster firing when the joints have + reached the desired angles and stopped""" + self.FSWModels.thrustJustActivated = True + self.fswProc.disableAllTasks() + self.FSWModels.zeroGatewayMsgs() + self.enableTask(self.FSWModels.outerLoopTaskName) + self.enableTask(self.FSWModels.thrFiringTaskName) + CurrentSimNanos = self.TotalSim.CurrentNanos + thrOnTimes = self.FSWModels.thrTimeReader().OnTimeRequest + payload = messaging.THRArrayOnTimeCmdMsgPayload() + payload.OnTimeRequest = list(thrOnTimes) + self.FSWModels.thrFiringTimeGateway.write(payload, CurrentSimNanos) + self.setEventActivity("jointMotion", True) + self.setEventActivity("thrFiring", False) + self.setEventActivity("coast", True) + + SimBase.createNewEvent( + "thrFiring", + self.innerLoopTimeStep, + False, + conditionFunction=conditionThrFiring, + actionFunction=actionThrFiring, + ) + + def conditionCoast(self): + """Event condition to coast (no commands) between the time + when the thrusters are done firing and the next outer loop update""" + thrForces = np.zeros(self.FSWModels.numThrusters) + for i, reader in enumerate(self.FSWModels.thrForceReaders): + thrForces[i] = reader().input + if self.FSWModels.thrustJustActivated: + self.FSWModels.thrustJustActivated = False + return False + return (np.max(np.abs(thrForces)) < 1e-12) + + def actionCoast(self): + """Event action to activate coasting (no commands) between the time + when the thrusters are done firing and the next outer loop update""" + self.fswProc.disableAllTasks() + self.FSWModels.zeroGatewayMsgs() + self.enableTask(self.FSWModels.outerLoopTaskName) + self.setEventActivity("jointMotion", True) + self.setEventActivity("thrFiring", False) + self.setEventActivity("coast", False) + + SimBase.createNewEvent( + "coast", + self.innerLoopTimeStep, + False, + conditionFunction=conditionCoast, + actionFunction=actionCoast, + ) + + # ------------------------------------------------------------------------------------------- # + # These are module-initialization methods + + def SetVehicleConfiguration(self): + """Set the spacecraft configuration information""" + vehicleConfigOut = messaging.VehicleConfigMsgPayload() + vehicleConfigOut.massSC = 350.0 + # use the hub inertia for mrp feedback module since will not have instantaneous inertia otherwise + vehicleConfigOut.ISCPntB_B = [1/3*330.0*(0.69**2+0.52**2), 0.0, 0.0, 0.0, 1/3*330.0*(0.79**2+0.52**2), 0.0, 0.0, 0.0, 1/3*330.0*(0.79**2+0.69**2)] + self.vcMsg = messaging.VehicleConfigMsg().write(vehicleConfigOut) + + def SetThrArmConfig(self): + """Set the thruster arm configuration information""" + thrArmConfigOut = messaging.THRArmConfigMsgPayload() + thrArmIdx = [0, 1] + thrArmJointIdx = [3, 3] + armTreeIdx = [0, 0] + armJointCount = [4, 4] + r_CP_P = [ + 0.79, 0.0, 0.0, + 0.0, 0.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + -0.79, 0.0, 0.0, + 0.0, 0.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + ] + r_TP_P = [ + 0.1, 0.0, 0.0, + 0.1, 0.0, 0.0, + ] + shat_P = [ + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0, + 0.0, 1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0, + 0.0, 1.0, 0.0, + ] + fhat_P = [ + -1.0, 0.0, 0.0, + -1.0, 0.0, 0.0, + ] + identity = [ + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0, + ] + arm2BaseDcm = [ + -1.0, 0.0, 0.0, + 0.0, -1.0, 0.0, + 0.0, 0.0, 1.0, + ] + dcm_C0P = ( + identity + identity + identity + identity + + arm2BaseDcm + identity + identity + identity + ) + + thrArmConfigOut.thrArmIdx.clear() + thrArmConfigOut.thrArmJointIdx.clear() + thrArmConfigOut.armTreeIdx.clear() + thrArmConfigOut.armJointCount.clear() + thrArmConfigOut.r_CP_P.clear() + thrArmConfigOut.r_TP_P.clear() + thrArmConfigOut.shat_P.clear() + thrArmConfigOut.fhat_P.clear() + thrArmConfigOut.dcm_C0P.clear() + for v in thrArmIdx: + thrArmConfigOut.thrArmIdx.push_back(v) + for v in thrArmJointIdx: + thrArmConfigOut.thrArmJointIdx.push_back(v) + for v in armTreeIdx: + thrArmConfigOut.armTreeIdx.push_back(v) + for v in armJointCount: + thrArmConfigOut.armJointCount.push_back(v) + for v in r_CP_P: + thrArmConfigOut.r_CP_P.push_back(v) + for v in r_TP_P: + thrArmConfigOut.r_TP_P.push_back(v) + for v in shat_P: + thrArmConfigOut.shat_P.push_back(v) + for v in fhat_P: + thrArmConfigOut.fhat_P.push_back(v) + for v in dcm_C0P: + thrArmConfigOut.dcm_C0P.push_back(v) + self.thrArmConfigMsg = messaging.THRArmConfigMsg().write(thrArmConfigOut) + + def SetTransRef(self): + """Set the translational reference state information""" + transRefOut = messaging.TransRefMsgPayload() + transRefOut.r_RN_N = [0.0, 0.0, 0.0] + transRefOut.v_RN_N = [0.0, 0.0, 0.0] + transRefOut.a_RN_N = [0.0, 0.0, 0.0] + self.transRefMsg = messaging.TransRefMsg().write(transRefOut) + + def SetInertial3D(self): + """Defined the inertial 3D guidance module""" + self.inertial3D.ModelTag = "inertial3D" + self.inertial3D.sigma_R0N = [0.0,0.0,0.0] + + def SetStateMerge(self, SimBase): + """Define the state merge module""" + self.stateMerge.ModelTag = "stateMerge" + self.stateMerge.attStateInMsg.subscribeTo(SimBase.DynModels.scene.getBody(self.hubName).getOrigin().stateOutMsg) + self.stateMerge.transStateInMsg.subscribeTo(SimBase.DynModels.systemCoM.comStatesOutMsg) + + def SetNavConverter(self): + """Define the nav converter module""" + self.navConverter.ModelTag = "navConverter" + self.navConverter.scStateInMsg.subscribeTo(self.stateMerge.stateOutMsg) + + def SetAttError(self): + """Define the attitude tracking error module""" + self.attError.ModelTag = "attError" + self.attError.attRefInMsg.subscribeTo(self.inertial3D.attRefOutMsg) + self.attError.attNavInMsg.subscribeTo(self.navConverter.attOutMsg) + + def SetMrpFeedback(self): + """Define the MRP feedback module""" + self.mrpFeedback.ModelTag = "mrpFeedback" + self.mrpFeedback.guidInMsg.subscribeTo(self.attError.attGuidOutMsg) + self.mrpFeedback.vehConfigInMsg.subscribeTo(self.vcMsg) + self.mrpFeedback.K = self.kRot + self.mrpFeedback.Ki = self.kiRot + self.mrpFeedback.P = self.pRot + + def SetInertialCartFeedback(self): + """Define the inertial cart feedback module""" + self.inertialCartFeedback.ModelTag = "inertialCartFeedback" + self.inertialCartFeedback.deputyNavInMsg.subscribeTo(self.navConverter.transOutMsg) + self.inertialCartFeedback.deputyRefInMsg.subscribeTo(self.transRefMsg) + self.inertialCartFeedback.deputyVehicleConfigInMsg.subscribeTo(self.vcMsg) + self.inertialCartFeedback.setK(self.kTrans) + self.inertialCartFeedback.setP(self.pTrans) + + def SetJointThrAllocation(self, SimBase): + """Define the joint and thruster allocation module""" + self.jointThrAllocation.ModelTag = "jointThrAllocation" + self.jointThrAllocation.armConfigInMsg.subscribeTo(self.thrArmConfigMsg) + self.jointThrAllocation.CoMStatesInMsg.subscribeTo(SimBase.DynModels.systemCoM.comStatesOutMsg) + self.jointThrAllocation.hubStatesInMsg.subscribeTo(SimBase.DynModels.scene.getBody(self.hubName).getOrigin().stateOutMsg) + self.jointThrAllocation.transForceInMsg.subscribeTo(self.inertialCartFeedback.forceOutMsg) + self.jointThrAllocation.rotTorqueInMsg.subscribeTo(self.mrpFeedback.cmdTorqueOutMsg) + + self.jointThrAllocation.setWc(self.controlWeight) + self.jointThrAllocation.setWf(self.thrustWeight) + self.jointThrAllocation.setThrForceMax(self.thrForces) + + self.desAngleReader = messaging.JointArrayStateMsgReader() + self.desAngleReader.subscribeTo(self.jointThrAllocation.desJointAnglesOutMsg) + + def SetThrFiringRound(self): + """Define the thruster firing logic module""" + self.thrFiringRound.ModelTag = "thrFiringRound" + self.thrFiringRound.thrForceInMsg.subscribeTo(self.jointThrAllocation.thrForceOutMsg) + self.thrFiringRound.setControlPeriodSec(self.outerLoopTimeStep*1e-9) + self.thrFiringRound.setOnTimeResolutionSec(self.innerLoopTimeStep*1e-9) + self.thrFiringRound.setNumThrusters(self.numThrusters) + self.thrFiringRound.setThrForceMax(self.thrForces) + self.thrTimeReader = messaging.THRArrayOnTimeCmdMsgReader() + self.thrTimeReader.subscribeTo(self.thrFiringRound.thrOnTimeOutMsg) + + def SetHingedJointArrayMotor(self, SimBase): + """Define the hinged joint array motor module""" + self.hingedJointArrayMotor.ModelTag = "hingedJointArrayMotor" + self.hingedJointArrayMotor.massMatrixInMsg.subscribeTo(SimBase.DynModels.systemMassMatrix.massMatrixOutMsg) + self.hingedJointArrayMotor.reactionForcesInMsg.subscribeTo(SimBase.DynModels.jointReactionForces.reactionForcesOutMsg) + self.hingedJointArrayMotor.desJointStatesInMsg.subscribeTo(self.jointThrAllocation.desJointAnglesOutMsg) + self.anglesReaders = [] + self.angleRatesReaders = [] + for i, (bodyName, jointName) in enumerate(self.jointRefs): + self.hingedJointArrayMotor.addHingedJoint() + joint = SimBase.DynModels.scene.getBody(bodyName).getScalarJoint(jointName) + self.hingedJointArrayMotor.jointStatesInMsgs[i].subscribeTo(joint.stateOutMsg) + self.hingedJointArrayMotor.jointStateDotsInMsgs[i].subscribeTo(joint.stateDotOutMsg) + # Redirect the module outputs to each joint motor torque gateway message + self.hingedJointArrayMotor.motorTorquesOutMsgs[i] = self.jointTorqueGateways[i] + # Set up readers for the angles for use in the event system + angleReader = messaging.ScalarJointStateMsgReader() + angleRateReader = messaging.ScalarJointStateMsgReader() + angleReader.subscribeTo(joint.stateOutMsg) + angleRateReader.subscribeTo(joint.stateDotOutMsg) + self.anglesReaders.append(angleReader) + self.angleRatesReaders.append(angleRateReader) + self.hingedJointArrayMotor.setKtheta(self.kTheta) + self.hingedJointArrayMotor.setPtheta(self.pTheta) + self.hingedJointArrayMotor.setUMax(self.maxJointTorques) + + def SetJointMotionCompensator(self, SimBase): + """Define the joint motion compensator module""" + self.jointMotionCompensator.ModelTag = "jointMotionCompensator" + self.jointMotionCompensator.massMatrixInMsg.subscribeTo(SimBase.DynModels.systemMassMatrix.massMatrixOutMsg) + self.jointMotionCompensator.reactionForcesInMsg.subscribeTo(SimBase.DynModels.jointReactionForces.reactionForcesOutMsg) + self.jointMotionCompensator.addSpacecraft() + for i in range(self.numJoints): + self.jointMotionCompensator.addHingedJoint() + self.jointMotionCompensator.jointTorqueInMsgs[i].subscribeTo(self.jointTorqueGateways[i]) + for i in range(3): + # Redirect the module outputs to each hub motor torque gateway message + self.jointMotionCompensator.hubTorqueOutMsgs[i] = self.bodyTorqueGateway[i] + + def SetThrOnTimeToForce(self): + """Define the thruster on-time to force conversion module""" + self.thrOnTimeToForce.ModelTag = "thrOnTimeToForce" + self.thrOnTimeToForce.onTimeInMsg.subscribeTo(self.thrFiringTimeGateway) + for i in range(self.numThrusters): + self.thrOnTimeToForce.addThruster() + # Redirect the module outputs to each thruster force gateway message + self.thrOnTimeToForce.thrusterForceOutMsgs[i] = self.thrForceGateways[i] + self.thrOnTimeToForce.setThrMag(self.thrForces) + + def SetThrJointCompensation(self, SimBase): + """Define the thruster-joint compensation module""" + self.thrJointCompensation.ModelTag = "thrJointCompensation" + self.thrJointCompensation.armConfigInMsg.subscribeTo(self.thrArmConfigMsg) + self.thrJointCompensation.massMatrixInMsg.subscribeTo(SimBase.DynModels.systemMassMatrix.massMatrixOutMsg) + self.thrJointCompensation.reactionForcesInMsg.subscribeTo(SimBase.DynModels.jointReactionForces.reactionForcesOutMsg) + for i in range(self.numThrusters): + self.thrJointCompensation.addThruster() + self.thrJointCompensation.thrForcesInMsgs[i].subscribeTo(self.thrForceGateways[i]) + for i, (bodyName, jointName) in enumerate(self.jointRefs): + self.thrJointCompensation.addHingedJoint() + joint = SimBase.DynModels.scene.getBody(bodyName).getScalarJoint(jointName) + self.thrJointCompensation.jointStatesInMsgs[i].subscribeTo(joint.stateOutMsg) + # Redirect the module outputs to each joint motor torque gateway message + self.thrJointCompensation.motorTorquesOutMsgs[i] = self.jointTorqueGateways[i] + self.thrJointCompensation.setUMax(self.maxJointTorques) + + def InitAllFSWObjects(self, SimBase): + """Initialize all FSW objects in the simulation""" + self.SetVehicleConfiguration() + self.SetThrArmConfig() + self.SetTransRef() + self.SetInertial3D() + self.SetStateMerge(SimBase) + self.SetNavConverter() + self.SetAttError() + self.SetMrpFeedback() + self.SetInertialCartFeedback() + self.SetJointThrAllocation(SimBase) + self.SetThrFiringRound() + self.SetHingedJointArrayMotor(SimBase) + self.SetJointMotionCompensator(SimBase) + self.SetThrOnTimeToForce() + self.SetThrJointCompensation(SimBase) + + def setupGatewayMsgs(self, SimBase): + """create stand-alone actuator gateway messages such that different modules can write this message (or it can be left as zero) + and provide a common input message for the mujoco actuators""" + self.jointTorqueGateways = [messaging.SingleActuatorMsg() for _ in range(self.numJoints)] + self.thrForceGateways = [messaging.SingleActuatorMsg() for _ in range(self.numThrusters)] + self.bodyTorqueGateway = [messaging.SingleActuatorMsg() for _ in range(3)] + self.thrFiringTimeGateway = messaging.THRArrayOnTimeCmdMsg() + + self.zeroGatewayMsgs() + + # connect the gateway messages to the appropriate mujoco actuator input messages + for i, name in enumerate(self.jointActNames): + SimBase.DynModels.scene.getSingleActuator(name).actuatorInMsg.subscribeTo(self.jointTorqueGateways[i]) + for i, name in enumerate(self.thrActNames): + SimBase.DynModels.scene.getSingleActuator(name).actuatorInMsg.subscribeTo(self.thrForceGateways[i]) + for i, name in enumerate(self.bodyActNames): + SimBase.DynModels.scene.getSingleActuator(name).actuatorInMsg.subscribeTo(self.bodyTorqueGateway[i]) + + # Setup readers for the thruster force gateway messages + self.thrForceReaders = [] + for i in range(self.numThrusters): + reader = messaging.SingleActuatorMsgReader() + reader.subscribeTo(self.thrForceGateways[i]) + self.thrForceReaders.append(reader) + + def zeroGatewayMsgs(self): + zeroActMsg = messaging.SingleActuatorMsgPayload(input=0.0) + zeroTimeMsg = messaging.THRArrayOnTimeCmdMsgPayload() + for m in self.jointTorqueGateways: + m.write(zeroActMsg) + for m in self.thrForceGateways: + m.write(zeroActMsg) + for m in self.bodyTorqueGateway: + m.write(zeroActMsg) + self.thrFiringTimeGateway.write(zeroTimeMsg) + + def wrapAngle(self, angle): + """Helper function to wrap angles to the range [-pi, pi]""" + return (angle + np.pi) % (2 * np.pi) - np.pi From 817284d3fea3b04e0b5fbdf7d6ac0c75083f7a3d Mon Sep 17 00:00:00 2001 From: William Schwend Date: Fri, 20 Feb 2026 13:42:20 -0700 Subject: [PATCH 4/8] [#1311] add new example scenarioThrArmControl --- examples/mujoco/scenarioThrArmControl.py | 249 +++++++++++++++++++++++ 1 file changed, 249 insertions(+) create mode 100644 examples/mujoco/scenarioThrArmControl.py diff --git a/examples/mujoco/scenarioThrArmControl.py b/examples/mujoco/scenarioThrArmControl.py new file mode 100644 index 00000000000..7695b196085 --- /dev/null +++ b/examples/mujoco/scenarioThrArmControl.py @@ -0,0 +1,249 @@ +# +# 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. + +""" +It's recommended to review the following scenario(s) first (and any +recommended scenario(s) that they may have): + +#. ``examples/mujoco/scenarioReactionWheel.py`` +#. ``examples/BskSim/scenario_BasicOrbit.py`` + +Overview +-------- + +This script demonstrates how to simulate the control of a 6-DOF spacecraft with +two attached 4-DOF thruster arms using the MuJoCo physics engine within the BSK_Sim architecture. + +The multi-body systems is created by importing the MuJoCo XML file ``sat_w_thruster_arms.xml`` +which defines the spacecraft hub and two attached 4-DOF thruster arms. It consists of a central hub +with two arms each consisting of 4 hinged joints, 2 rigid arm segments, and a thruster at the end. +MuJoCo single-input actuators are used for each of the spacecraft's thrusters hinged joints, and to apply +an additional torque capability direct to the spacecraft hub. + +The script is found in the folder ``basilisk/examples/mujoco`` and executed by using:: + + python3 scenarioThrArmControl.py + +Two simulation processes are created: one +which contains dynamics modules, and one that contains the Flight Software (FSW) +modules. The Dynamics process contains a single task with the modules: + +#. ``MJScene``: the MuJoCo dynamics module that simulates the multi-body system defined in the XML file +#. ``MJSystemCoM``: extracts the center of mass position and velocity of the multi-body system from the MuJoCo scene +#. ``MJSystemMassMatrix``: extracts the mass matrix of the multi-body system from the MuJoCo scene +#. ``MJJointReactionForces``: extracts the internal reaction forces and torques of the multi-body system from the MuJoCo scene + +The FSW process contains three tasks: the outer loop control task, the joint motion task, and the thruster firing task. +The modules for the outer loop control task include: + +#. ``inertial3D``: generates and inertial reference attitude +#. ``attError``: computes the attitude error between the current and reference attitude +#. ``mrpFeedback``: computes the attitude control torque +#. ``inertialCartFeedback``: computes the positional control force +#. ``jointThrAllocation``: determines the joint angles and thruster forces needed to achieve the desired control torque and force +#. ``thrFiringRound``: determines thruster firing time based on the thruster forces and a specified minimum impulse bit + +The modules for the joint motion task include: + +#. ``hingedJointArrayMotor``: determines the motor commands for the hinged joints of the thruster arms +#. ``jointMotionCompensator``: determines the compensation torques to apply at the spacecraft hub to counteract the reaction torques from moving the thruster arms + +The modules for the thruster firing task include: + +#. ``thrOnTimeToForce``: converts on time to thruster force +#. ``thrJointCompensation``: determines the motor commands to hold hinged joints static during thruster firing + +The different tasks are enabled and disabled using three events: joint motion, thruster firing, and coasting. The +condition to trigger each event and action it performs are as follows: + +- Joint Motion Event: + + - Condition: New desired joint angles are received from the outer control loop + - Action: enables the joint motion task and move the arms to their desired positions + +- Thruster Firing Event: + + - Condition: New desired thruster forces are received from the outer control loop and + the joints have reached their desired positions + - Action: enables the thruster firing task and fire the thrusters to achieve the desired forces + +- Coasting Event: + + - Condition: The thrusters have finished firing and there is time before the next outer loop update + - Action: set commands for all actuators to zero and allow the spacecraft to coast until the next control update + + +Illustration of Simulation Results +---------------------------------- + +:: + + showPlots = True + +.. image:: /_images/Scenarios/scenarioThrArmControl_position.svg + :align: center + +.. image:: /_images/Scenarios/scenarioThrArmControl_attitude.svg + :align: center + + +""" +# Get current file path +import inspect +import os +import sys +import argparse +import matplotlib.pyplot as plt + +from Basilisk.utilities import macros as mc, unitTestSupport + +filename = inspect.getframeinfo(inspect.currentframe()).filename +path = os.path.dirname(os.path.abspath(filename)) +fileNameForImages = os.path.basename(os.path.splitext(__file__)[0]) + +# Import master classes: simulation base class and scenario base class +sys.path.append(path + '/mujocoModels') +import BSK_MujocoDynamics +import BSK_MujocoFSW +from BSK_mujocoMasters import BSKSim, BSKScenario + +# Create scenario child class +class scenarioThrArmControl(BSKSim, BSKScenario): + def __init__(self, fswRate=0.01, dynRate=0.01): + super(scenarioThrArmControl, self).__init__(fswRate=fswRate, dynRate=dynRate) + self.name = "scenarioThrArmControl" + + # Declare empty class variables + self.posRec = None + self.attRec = None + + self.set_DynModel(BSK_MujocoDynamics) + self.set_FswModel(BSK_MujocoFSW) + self.log_outputs() + + def configure_initial_conditions(self): + # Set initial conditions for the MuJoCo simulation here + DynModel = self.get_DynModel() + FSWModel = self.get_FswModel() + hubName = FSWModel.hubName + jointRefs = FSWModel.jointRefs + DynModel.scene.getBody(hubName).setPosition([1.5,-1.0,0.75]) #[m] Initial position of deputy's hub in inertial frame + DynModel.scene.getBody(hubName).setVelocity([5e-3,-5e-3,5e-3]) #[m/s] Initial velocity of deputy's hub in inertial frame + DynModel.scene.getBody(hubName).setAttitude([0.15,-0.1,0.1]) #[-] Initial attitude between the deputy's body frame and the inertial frame expressed in MRP + DynModel.scene.getBody(hubName).setAttitudeRate([0.01,-0.01,0.005]) #[rad/s] Initial angular velocity of the deputy body frame relative to the inertial frame, expressed in the body frame + for _, (bodyName, jointName) in enumerate(jointRefs): + joint = DynModel.scene.getBody(bodyName).getScalarJoint(jointName) + # Setting the joint position to zero causes the spacecraft hub location to be collocated with the + # system center of mass position. + joint.setPosition(0.0) #[rad] Initial joint angle + # Setting the joint velocity to zero ensures that the spacecraft hub velocity is the same as the + # system center of mass velocity. + joint.setVelocity(0.0) #[rad/s] Initial joint angular velocity + + def log_outputs(self): + DynModel = self.get_DynModel() + self.posRec = DynModel.systemCoM.comStatesOutMsg.recorder() + self.attRec = DynModel.scene.getBody("hub").getOrigin().stateOutMsg.recorder() + self.AddModelToTask(DynModel.taskName, self.posRec) + self.AddModelToTask(DynModel.taskName, self.attRec) + + def pull_outputs(self, showPlots: bool = False): + times = self.posRec.times() * mc.NANO2SEC + r_CN_N = self.posRec.r_CN_N + sigma_BN = self.attRec.sigma_BN + + color_x = unitTestSupport.getLineColor(0, 3) + color_y = unitTestSupport.getLineColor(1, 3) + color_z = unitTestSupport.getLineColor(2, 3) + + figureList = {} + + # Plot the spacecraft position + fig1 = plt.figure() + plt.plot(times/60, r_CN_N[:, 0], color=color_x, label='x') + plt.plot(times/60, r_CN_N[:, 1], color=color_y, label='y') + plt.plot(times/60, r_CN_N[:, 2], color=color_z, label='z') + plt.xlabel('Time [min]') + plt.ylabel('Position Error [m]') + plt.legend() + figureList[fileNameForImages + "_position"] = fig1 + + # Plot the spacecraft attitude + fig2 = plt.figure() + plt.plot(times/60, sigma_BN[:, 0], color=color_x, label=r'$\sigma_1$') + plt.plot(times/60, sigma_BN[:, 1], color=color_y, label=r'$\sigma_2$') + plt.plot(times/60, sigma_BN[:, 2], color=color_z, label=r'$\sigma_3$') + plt.xlabel('Time [min]') + plt.ylabel(r'Attitude Error [$\boldsymbol{\sigma}$]') + plt.legend() + figureList[fileNameForImages + "_attitude"] = fig2 + + return figureList + +def runScenario(scenario, runTime: float = 320.0): + # initialize the scenario + scenario.InitializeSimulation() + + # set the initial conditions for MuJoCo + scenario.configure_initial_conditions() + + # configure the run time and execute the simulation + simulationTime = mc.sec2nano(runTime) + scenario.ConfigureStopTime(simulationTime) + scenario.ExecuteSimulation() + +def run(showPlots: bool = False, timeStep: float = 0.01, runTime: float = 320.0): + """ + The scenarios can be run with the following setups parameters: + + Args: + showPlots (bool): Determines if the script should display plots + + """ + scenario = scenarioThrArmControl(fswRate=timeStep, dynRate=timeStep) + runScenario(scenario, runTime) + figureList = scenario.pull_outputs(showPlots) + + if showPlots: + plt.show() + + return figureList + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Run scenarioThrArmControl") + parser.add_argument( + "--showPlots", + action="store_true", + default=True, + help="Display matplotlib figures." + ) + parser.add_argument( + "--timeStep", + type=float, + default=0.01, + help="FSW and dynamics update period [s]." + ) + parser.add_argument( + "--runTime", + type=float, + default=320.0, + help="Simulation run time [s]." + ) + + args = parser.parse_args() + run(showPlots=args.showPlots, timeStep=args.timeStep, runTime=args.runTime) From 9be8b2e8b636167c90462e126366fcda5d032261 Mon Sep 17 00:00:00 2001 From: William Schwend Date: Wed, 18 Mar 2026 14:41:50 -0600 Subject: [PATCH 5/8] [#1311] update test_scenarioMujoco.py for new test --- src/tests/test_scenarioMujoco.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/tests/test_scenarioMujoco.py b/src/tests/test_scenarioMujoco.py index 59cafe06c66..a678a27d6f3 100644 --- a/src/tests/test_scenarioMujoco.py +++ b/src/tests/test_scenarioMujoco.py @@ -48,6 +48,10 @@ if filename.startswith("scenario") and filename.endswith(".py") ] +SCENARIO_RUN_KWARGS = { + "scenarioThrArmControl": {"showPlots": False, "timeStep": 0.075, "runTime": 270.0}, +} + sys.path.append(SCENARIO_FOLDER) @@ -56,7 +60,8 @@ @pytest.mark.scenarioTest def test_scenarios(scenario: str): module = importlib.import_module(scenario) - figureList = module.run() # Every mujoco scenario should have a run function + kwargs = SCENARIO_RUN_KWARGS.get(scenario, {}) + figureList = module.run(**kwargs) # Every mujoco scenario should have a run function if figureList is None: return From 805d93bc0264d4cc6198f24391797b77a049cd21 Mon Sep 17 00:00:00 2001 From: William Schwend Date: Fri, 6 Mar 2026 11:00:28 -0700 Subject: [PATCH 6/8] [#1311] update mujoco example list --- examples/_default.rst | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/examples/_default.rst b/examples/_default.rst index 5033689bb6c..a2797ee91a1 100644 --- a/examples/_default.rst +++ b/examples/_default.rst @@ -219,6 +219,16 @@ It's recommended to study the first 6 scenarios in order: Pointing with Reaction Wheels in MuJoCo Translation-only formation flying under differential drag Satellite Drag with Stochastic (Random) Density using MuJoCo + Spacecraft Control with Thruster Arms using MuJoCo + +Support Files +^^^^^^^^^^^^^ + +.. toctree:: + :maxdepth: 1 + + Master File + Models Folder Constrained Spacecraft Dynamics Simulations ------------------------------------------- From 20bc0733035062f380d0a2053c9af019d203dbf1 Mon Sep 17 00:00:00 2001 From: William Schwend Date: Fri, 6 Mar 2026 11:03:30 -0700 Subject: [PATCH 7/8] [#1311] added release notes snippet --- .../bskReleaseNotesSnippets/1311-scenarioThrArmControl.rst | 1 + 1 file changed, 1 insertion(+) create mode 100644 docs/source/Support/bskReleaseNotesSnippets/1311-scenarioThrArmControl.rst diff --git a/docs/source/Support/bskReleaseNotesSnippets/1311-scenarioThrArmControl.rst b/docs/source/Support/bskReleaseNotesSnippets/1311-scenarioThrArmControl.rst new file mode 100644 index 00000000000..2169912ecc1 --- /dev/null +++ b/docs/source/Support/bskReleaseNotesSnippets/1311-scenarioThrArmControl.rst @@ -0,0 +1 @@ +- Added :ref:`scenarioThrArmControl`, a MuJoCo example scenario for spacecraft control using thruster arms. From 0ba04ce41bcafd7d15fae70dd75f45e80875bad5 Mon Sep 17 00:00:00 2001 From: William Schwend Date: Mon, 2 Mar 2026 11:31:38 -0700 Subject: [PATCH 8/8] [#1311] remove unused variable from InertialCartFeedback --- .../inertialCartFeedback/inertialCartFeedback.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/fswAlgorithms/formationFlying/inertialCartFeedback/inertialCartFeedback.cpp b/src/fswAlgorithms/formationFlying/inertialCartFeedback/inertialCartFeedback.cpp index 52cbef9f702..b479780174a 100644 --- a/src/fswAlgorithms/formationFlying/inertialCartFeedback/inertialCartFeedback.cpp +++ b/src/fswAlgorithms/formationFlying/inertialCartFeedback/inertialCartFeedback.cpp @@ -64,7 +64,6 @@ void InertialCartFeedback::UpdateState(uint64_t CurrentSimNanos) const Eigen::Vector3d deltaR_N = r_BN_N - r_RN_N; const Eigen::Vector3d deltaV_N = v_BN_N - v_RN_N; const double normRDeputy = r_BN_N.norm(); - const double normRDeputyRef = r_RN_N.norm(); Eigen::Vector3d aGravDeputy_N = Eigen::Vector3d::Zero(); if (this->mu > 0.0) {