Add rotation matrix helpers - #290
Conversation
camUrban
added
the
maintenance
Codecov Report✅ All modified and coverable lines are covered by tests. Additional details and impacted files@@ Coverage Diff @@
## main #290 +/- ##
=======================================
Coverage 94.39% 94.40%
=======================================
Files 47 47
Lines 9694 9701 +7
=======================================
+ Hits 9151 9158 +7
Misses 543 543 ☔ View full report in Codecov by Harness. 🚀 New features to boost your workflow:
|
camUrban
changed the title
camUrban
left a comment
camUrban
left a comment
There was a problem hiding this comment.
Hi @asTejaswinis, thanks so much for your hard work on this! The results look really good so far. There are a few changes I requested. Once you've addressed them, feel free to request another review and we can get this merged! 😃
Also, as you are a first-time contributor, please also add yourself to the bottom of the list of contributors in the README.md (follow one of the pre-existing entries for formatting).
| R_act_BP1_to_E = np.linalg.inv(R_pas_BP1_to_E) | ||
|
|
||
| R_act_E_to_BP1 = R_act_BP1_to_E.T | ||
| R_act_E_to_BP1 = _transformations.invert_R_act(R_pas_BP1_to_E) |
There was a problem hiding this comment.
This is mathematically correct, but I think we want to break this into two steps. First, converting the passive rotation to an active rotation, and then inverting the active rotation to convert "body to earth axes" to "earth to body axes".
The reason is ease of interpret-ability and consistency with the rest of the codebase, as we already have the functions _transformations.convert_T_pas_to_T_act and _transformations.convert_T_act_to_T_pas. We could complement them with two additional functions: _transformations.convert_R_pas_to_R_act and transformations.convert_R_act_to_R_pas.
There was a problem hiding this comment.
_transformations.convert_R_pas_to_R_act and _transformations.convert_R_pas_to_R_act would also benefit from some new unit test classes (tests/unit/transformations_tests.TestConvertRPasToRAct and tests/unit/transformations_tests.TestConvertRActToPas).
| R = R_act | ||
| if passive: | ||
| R = R.T | ||
| R = invert_R_pas(R) |
There was a problem hiding this comment.
We should switch this to R = convert_R_act_to_R_pas(R), once it exists.
| return alpha, beta | ||
|
|
||
|
|
||
| def invert_R_pas(R_pas: np.ndarray) -> np.ndarray: |
There was a problem hiding this comment.
This function should get some unit tests, perhaps in a new class: tests/unit/transformations_tests.TestInvertRPas. You can get some ideas of the number and shape of tests to include by looking at tests/unit/transformations_tests.TestInvertTPas.
| return R_pas.T | ||
|
|
||
|
|
||
| def invert_R_act(R_act: np.ndarray) -> np.ndarray: |
There was a problem hiding this comment.
This function should get some unit tests, perhaps in a new class: tests/unit/transformations_tests.TestInvertRAct. You can get some ideas of the number and shape of tests to include by looking at tests/unit/transformations_tests.TestInvertTAct.
| return np.linalg.inv(R_act).T | ||
|
|
||
|
|
||
| def apply_R_to_vectors( |
There was a problem hiding this comment.
This function should get some unit tests, perhaps in a new class: tests/unit/transformations_tests.TestApplyRToVectors. You can get some ideas of the number and shape of tests to include by looking at tests/unit/transformations_tests.TestApplyTToVectors.
| return np.asarray(np.einsum("ij,...j->...i", R, vectors_A), dtype=float) | ||
|
|
||
|
|
||
| def extract_R_from_T(T: np.ndarray) -> np.ndarray: |
There was a problem hiding this comment.
This function should get some unit tests, perhaps in a new class: tests/unit/transformations_tests.TestExtractRFromT.
|
Also, I merged in some updates from main, so please remember to |
2 participants
Description
Add 3x3 rotation matrix helpers and adopt them at the MuJoCo boundary
Motivation
_transformations.pyprovides homogeneous 4x4 transform helpers (generate_rot_T,invert_T_pas,invert_T_act, andapply_T_to_vectors) but no equivalents for bare 3x3 rotation matrices, so the MuJoCo boundary code does rotation math inline with raw NumPy. The bare operations that need covering are: inverting a passive rotation (currently a.T; the TODO comment inMuJoCoModel.get_statealready proposes naming thisinvert_R_pas), inverting an active rotation (thenp.linalg.invfollowed by.Tin theMuJoCoModelconstructor), extracting the 3x3 rotation block from a 4x4 transform (currentlyT[:3, :3]), and applying a rotation matrix to vectors (currentlyxmat @ qvelandR @ omega, the 3x3 analog ofapply_T_to_vectors). This is correctness-neutral consistency work: the inline math is right, it is just unnamed, unvalidated, and repeated.Relevant Issues
Closes #249.
Changes
invert_R_pas,invert_R_act,apply_R_to_vectors, andextract_R_from_Tfunctions topterasoftware/_transformations.pyalong with reStructuredText docstrings.pterasoftware/_mujoco_model.pyconstructor andget_statethrough the new helpers.pterasoftware/_mujoco_model.py.docs/MUJOCO_CONVENTIONS.md.tests/unit/test_mujoco_model.py.Dependency Updates
None.
Change Magnitude
Minor: Small change such as a bug fix, small enhancement, or documentation update.
Checklist (check each item when completed or not applicable)
mainand is up to date with the upstreammainbranch.--in-place --black). See the style guide for type hints and docstrings for more details.pterasoftwarepackage use type hints. See the style guide for type hints and docstrings for more details.testspackage.testspackage.ascii-only,pre-commit-hooks, andzizmorGitHub actions.lintjob of theCIGitHub action.testjobs of theCIGitHub action.