#
# ISC License
#
# Copyright (c) 2025, Autonomous Vehicle Systems Lab, University of Colorado 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 numpy as np
import tempfile
import pytest
from Basilisk import hasBuildFeature
from Basilisk.utilities import SimulationBaseClass
from Basilisk.utilities import unitTestSupport
from Basilisk.architecture import messaging
from Basilisk.utilities import macros
mujocoEnabled = hasBuildFeature("mujoco")
pytestmark = pytest.mark.skipif(
not mujocoEnabled,
reason="Requires Basilisk built with --mujoco True",
)
if mujocoEnabled:
from Basilisk.simulation import mujoco
from Basilisk.simulation import MJSystemCoM
# Common parameters used in both XML and truth calc
HUB_MASS = 50.0
ARM_MASS = 10.0
SX, SY, SZ = 0.5, 0.3, 0.2 # hub half-sizes (m) → face centers at ±SX
L = 2.0 # arm length (m)
THETA_DEG = 30.0 # arm yaw angle for "angled" case
def _xml_single():
return f"""<mujoco>
<option gravity="0 0 0"/>
<worldbody>
<body name="hub">
<freejoint/>
<inertial pos="0 0 0" quat="1 0 0 0" mass="{HUB_MASS}" diaginertia="1 1 1"/>
<geom type="box" size="{SX} {SY} {SZ}" rgba="0.8 0.2 0.2 0.4"/>
</body>
</worldbody>
</mujoco>
"""
def _xml_straight():
# Arms go straight ±X from the face centers.
# Place each arm body at the face center; set its inertial COM at L/2 along arm direction.
# Welded to hub (no joint).
return f"""<mujoco>
<option gravity="0 0 0"/>
<worldbody>
<body name="hub">
<freejoint/>
<inertial pos="0 0 0" quat="1 0 0 0" mass="{HUB_MASS}" diaginertia="10 10 10"/>
<geom type="box" size="{SX} {SY} {SZ}" rgba="0.8 0.2 0.2 0.4"/>
<!-- +X arm -->
<body name="arm_plus" pos="{SX} 0 0">
<!-- arm COM is L/2 out along +X -->
<inertial pos="{L/2} 0 0" quat="1 0 0 0" mass="{ARM_MASS}" diaginertia="5 5 5"/>
<geom type="capsule" fromto="0 0 0 {L} 0 0" size="0.05" rgba="0.2 0.6 1 1"/>
</body>
<!-- -X arm -->
<body name="arm_minus" pos="-{SX} 0 0">
<!-- arm COM is L/2 out along -X -->
<inertial pos="-{L/2} 0 0" quat="1 0 0 0" mass="{ARM_MASS}" diaginertia="5 5 5"/>
<geom type="capsule" fromto="0 0 0 -{L} 0 0" size="0.05" rgba="0.2 0.6 1 1"/>
</body>
</body>
</worldbody>
</mujoco>
"""
def _xml_angled():
# Arms leave ±X faces at yaw = ±θ outward (both tilt toward +Y).
# Right arm direction u_plus = [cosθ, sinθ, 0].
# Left arm direction u_minus = [-cosθ, sinθ, 0] (still outward from -X face).
c = np.cos(np.deg2rad(THETA_DEG))
s = np.sin(np.deg2rad(THETA_DEG))
return f"""<mujoco>
<option gravity="0 0 0"/>
<worldbody>
<body name="hub">
<freejoint/>
<inertial pos="0 0 0" quat="1 0 0 0" mass="{HUB_MASS}" diaginertia="10 10 10"/>
<geom type="box" size="{SX} {SY} {SZ}" rgba="0.8 0.2 0.2 0.4"/>
<!-- +X angled arm -->
<body name="arm_plus" pos="{SX} 0 0">
<inertial pos="{(L/2)*c} {(L/2)*s} 0" quat="1 0 0 0" mass="{ARM_MASS}" diaginertia="5 5 5"/>
<geom type="capsule" fromto="0 0 0 {L*c} {L*s} 0" size="0.05" rgba="0.2 1 0.4 1"/>
</body>
<!-- -X angled arm -->
<body name="arm_minus" pos="-{SX} 0 0">
<inertial pos="{- (L/2)*c} {(L/2)*s} 0" quat="1 0 0 0" mass="{ARM_MASS}" diaginertia="5 5 5"/>
<geom type="capsule" fromto="0 0 0 {-L*c} {L*s} 0" size="0.05" rgba="1 0.8 0.2 1"/>
</body>
</body>
</worldbody>
</mujoco>
"""
def _write_temp_xml(xml_text: str, basename: str) -> str:
tmpdir = tempfile.mkdtemp(prefix="mjcom_")
path = os.path.join(tmpdir, f"{basename}.xml")
with open(path, "w") as f:
f.write(xml_text)
return path
def _expected_com(model: str, r_root, v_root):
r_root = np.asarray(r_root, dtype=float)
v_root = np.asarray(v_root, dtype=float)
if model == "single":
# Only hub
rC = r_root
vC = v_root
elif model == "straight":
# Symmetric equal arms along ±X → CoM at hub center
rC = r_root
vC = v_root
elif model == "angled":
# Compute mass-weighted average of hub @ 0 and two arms at their COMs.
c = np.cos(np.deg2rad(THETA_DEG))
s = np.sin(np.deg2rad(THETA_DEG))
# COM locations of the welded arm bodies in the hub frame:
# right arm COM = face center + (L/2)[c, s, 0]
r_plus = np.array([ SX, 0.0, 0.0]) + np.array([(L/2)*c, (L/2)*s, 0.0])
# left arm COM = (-SX,0,0) + (L/2)[-c, s, 0]
r_minus= np.array([-SX, 0.0, 0.0]) + np.array([-(L/2)*c, (L/2)*s, 0.0])
M = HUB_MASS + 2*ARM_MASS
r_off = (HUB_MASS*np.zeros(3) + ARM_MASS*r_plus + ARM_MASS*r_minus) / M
rC = r_root + r_off
vC = v_root
else:
raise ValueError("unknown model")
return rC, vC
[docs]
@pytest.mark.parametrize("model", ["single", "straight", "angled"])
@pytest.mark.parametrize("moving", [True, False])
@pytest.mark.parametrize("displaced", [True, False])
def test_MJSystemCoM(show_plots, model, moving, displaced):
"""Module Unit Test"""
[testResults, testMessage] = MJSystemCoMTest(show_plots, model, moving, displaced)
assert testResults < 1, testMessage
[docs]
def MJSystemCoMTest(show_plots, model, moving, displaced):
r"""
**Validation Test Description**
This unit test sets up a spacecraft to extract its center of mass.
**Test Parameters**
The spacecraft geometry, velocity, and base position are varied between tests
Args:
model (str): the model of the spacecraft for this parameterized unit test
moving (bool): whether the spacecraft is moving or not for this parameterized unit test
displaced (bool): whether the spacecraft base is displaced or not for this parameterized unit test
**Description of Variables Being Tested**
In this test we are checking the location and velocity of the spacecraft's center of mass
compared to the expected values
"""
testFailCount = 0
testMessages = []
unitTaskName = "unitTask"
unitProcessName = "TestProcess"
if model == "single":
xml_path = _write_temp_xml(_xml_single(), "single")
root_name = "hub"
elif model == "straight":
xml_path = _write_temp_xml(_xml_straight(), "straight")
root_name = "hub"
elif model == "angled":
xml_path = _write_temp_xml(_xml_angled(), "angled")
root_name = "hub"
# Root initial pose
r0 = np.array([0.0, 0.0, 0.0]) if not displaced else np.array([1234.5, -678.9, 42.0]) # m
v0 = np.array([0.0, 0.0, 0.0]) if not moving else np.array([0.12, -0.07, 0.03]) # m/s
unitTestSim = SimulationBaseClass.SimBaseClass()
testProcessRate = macros.sec2nano(0.1)
testProc = unitTestSim.CreateNewProcess(unitProcessName)
testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate))
# create the Mujoco scene
scene = mujoco.MJScene.fromFile(xml_path)
scene.extraEoMCall = True
unitTestSim.AddModelToTask(unitTaskName, scene)
# setup module to be tested
module = MJSystemCoM.MJSystemCoM()
module.ModelTag = "MJSystemCoMTag"
module.scene = scene
scene.AddModelToDynamicsTask(module)
# setup output message recorder objects
comStatesOutMsgRec = module.comStatesOutMsg.recorder()
comStatesOutMsgCRec = module.comStatesOutMsgC.recorder()
unitTestSim.AddModelToTask(unitTaskName, comStatesOutMsgRec)
unitTestSim.AddModelToTask(unitTaskName, comStatesOutMsgCRec)
unitTestSim.InitializeSimulation()
# set initial conditions
scene.getBody("hub").setPosition(r0)
scene.getBody("hub").setVelocity(v0)
unitTestSim.ConfigureStopTime(macros.sec2nano(0.2))
unitTestSim.ExecuteSimulation()
# pull module data and make sure it is correct
r_CN_N_module = comStatesOutMsgRec.r_CN_N[-1,:]
v_CN_N_module = comStatesOutMsgRec.v_CN_N[-1,:]
r_CN_N_module_c = comStatesOutMsgCRec.r_CN_N[-1,:]
v_CN_N_module_c = comStatesOutMsgCRec.v_CN_N[-1,:]
# compute the truth data
r_CN_N_truth0, v_CN_N_truth0 = _expected_com(model, r0, v0)
r_CN_N_truth = r_CN_N_truth0 + v_CN_N_truth0 * 0.2
v_CN_N_truth = v_CN_N_truth0
# Compare
acc = 1e-12
testFailCount, testMessages = unitTestSupport.compareArrayND(
[r_CN_N_truth], [r_CN_N_module], acc, "CoM_position", 3, testFailCount, testMessages
)
testFailCount, testMessages = unitTestSupport.compareArrayND(
[v_CN_N_truth], [v_CN_N_module], acc, "CoM_velocity", 3, testFailCount, testMessages
)
testFailCount, testMessages = unitTestSupport.compareArrayND(
[r_CN_N_module], [r_CN_N_module_c], acc, "cMsgPosition", 3, testFailCount, testMessages
)
testFailCount, testMessages = unitTestSupport.compareArrayND(
[v_CN_N_module], [v_CN_N_module_c], acc, "cMsgVelocity", 3, testFailCount, testMessages
)
if testFailCount == 0:
print("PASSED: " + module.ModelTag)
else:
print(testMessages)
return [testFailCount, "".join(testMessages)]
if __name__ == "__main__":
test_MJSystemCoM(False, "angled", True, False)