Mujoco System Mass Matrix Module #1108
Conversation
Will-Schwend
added
documentation
Will-Schwend
force-pushed
the
feature/Mujoco-System-Mass-Matrix
branch
from
cbcdddc to
0eea9ec
Compare
Will-Schwend
force-pushed
the
feature/Mujoco-System-Mass-Matrix
branch
from
0eea9ec to
f07d39e
Compare
|
@Will-Schwend , feel free to push this PR if @juan-g-bonilla has no issues with your implementation to interface with his code. |
Will-Schwend
force-pushed
the
feature/Mujoco-System-Mass-Matrix
branch
from
f07d39e to
debf501
Compare
|
|
||
| #include "architecture/utilities/macroDefinitions.h" | ||
|
|
||
| #define MJ_MASSMATRIX_DIM (6 + MAX_EFF_CNT) // row/col size |
There was a problem hiding this comment.
Why are we defining MJ_MASSMATRIX_DIM but then using 6 + MAX_EFF_CNT explicitly for the MassMatrix size?
| typedef struct { | ||
| int nbase; //!< [-] number of base DOFs (6 if free base, else 0) | ||
| int nj; //!< [-] number of joint DOFs populated in mass matrix | ||
| double MassMatrix[6 + MAX_EFF_CNT][6 + MAX_EFF_CNT]; //!< [varies by component] system mass matrix in generalized coordinates |
There was a problem hiding this comment.
Should MassMatrix not be massMatrix following the camelCase convention?
There was a problem hiding this comment.
good catch on not using lower came case!
| BSKLogger bskLogger; //!< BSK Logging | ||
| private: | ||
|
|
||
| int nDOF{0}; // number of total DOF |
There was a problem hiding this comment.
A bit pedantic, but maybe these should be size_t or another unsigned integer.
There was a problem hiding this comment.
please make sure there are no new implicit type conversion warnings.
| MJSystemMassMatrix(); | ||
| ~MJSystemMassMatrix() = default; | ||
|
|
||
| void SelfInit(); | ||
| void Reset(uint64_t CurrentSimNanos); | ||
| void UpdateState(uint64_t CurrentSimNanos); |
There was a problem hiding this comment.
I like including the doxygen method comments in the .h instead of the .cpp file. That way, you have the class, variable, and method descriptions all in the same file, instead of having to go to the .cpp to read the documentation. Just a style choice, not required.
There was a problem hiding this comment.
agree, this is a personal choice of the person writing the module :-)
| bodies: [{'m':float,'r':(3,), 'Icom':(3,3)}, ...] | ||
| Return (M6, rC). | ||
| """ | ||
| m_tot = sum(b['m'] for b in bodies) |
There was a problem hiding this comment.
camelCase mTot please!
| def add_allclose(testFailCount, testMessages, name, A, B, rtol=0.0, atol=1e-10): | ||
| try: | ||
| assert_allclose(A, B, rtol=rtol, atol=atol) | ||
| except AssertionError as e: | ||
| testFailCount += 1 | ||
| testMessages.append(f"{name}: {str(e)}\n") | ||
| return testFailCount, testMessages | ||
|
|
||
| def add_equal(testFailCount, testMessages, name, a, b): | ||
| try: | ||
| assert_equal(a, b) | ||
| except AssertionError as e: | ||
| testFailCount += 1 | ||
| testMessages.append(f"{name}: {str(e)}\n") | ||
| return testFailCount, testMessages | ||
|
|
||
| def add_symmetry(testFailCount, testMessages, name, M, tol=1e-12): | ||
| return add_allclose(testFailCount, testMessages, name, M, M.T, rtol=0.0, atol=tol) | ||
|
|
||
| def add_psd(testFailCount, testMessages, name, M, tol=-1e-12): | ||
| try: | ||
| w = np.linalg.eigvalsh(0.5*(M+M.T)) | ||
| # require every eigenvalue >= tol (allow tiny negative for roundoff) | ||
| assert_array_less(np.full_like(w, tol), w) | ||
| except AssertionError as e: | ||
| testFailCount += 1 | ||
| testMessages.append(f"{name}: not PSD; {str(e)}; eigvals={w}\n") | ||
| return testFailCount, testMessages | ||
|
|
||
| def add_padding_zero(testFailCount, testMessages, name, M, used, atol=1e-14): | ||
| try: | ||
| if used < M.shape[0]: | ||
| assert_allclose(M[used:, :], 0.0, rtol=0.0, atol=atol) | ||
| assert_allclose(M[:, used:], 0.0, rtol=0.0, atol=atol) | ||
| except AssertionError as e: | ||
| testFailCount += 1 | ||
| testMessages.append(f"{name}: {str(e)}\n") | ||
| return testFailCount, testMessages |
There was a problem hiding this comment.
Some time ago, we abandoned the pattern:
try:
assert ...
except AssertionError as e:
testFailCount += 1
testMessages.append(f"{name}: {str(e)}\n")
return testFailCount, testMessages
In fact newer tests don't use testFailCount or testMessages at all. Instead, let the actual assertion error propagate, since pytest will capture it and print nicer errors when the test fails.
| testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) | ||
|
|
||
| # create the Mujoco scene | ||
| scene = mujoco.MJScene.fromFile(xml_path) |
There was a problem hiding this comment.
| scene = mujoco.MJScene.fromFile(xml_path) | |
| scene = mujoco.MJScene(xml_fn()) |
if you already have the XML as a string, you can just pass it to the constructor.
Remember to remove _write_tmp!
| testFailCount = 0 | ||
| testMessages = [] |
| import numpy as np | ||
| from numpy.testing import assert_allclose, assert_equal, assert_array_less | ||
| import pytest | ||
| import tempfile |
There was a problem hiding this comment.
Remember to remove unused import.
Description
This PR adds the ability to extract the system mass matrix and number of degrees of freedom from a Mujoco scene with a single spacecraft. This enables the use of these values throughout other Basilisk modules.
Verification
A new unit test
test_MJSystemMassMatrix.pywas created which verifies the degrees of freedom and mass matrix extracted in the moduleDocumentation
New documentation was made for the
MJSystemMassMatrixmodule. Reslease notes were updated to include the new module.Future work
N/A