diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index b32942a5..3ec74da8 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -490,6 +490,20 @@ PYBIND11_MODULE(_specs, m) { }, py::arg("name"), py::return_value_policy::reference_internal); + mjSpec.def_static( + "resolve_orientation", + [](bool degree, const MjTypeVec& sequence, + const raw::MjsOrientation* orientation) -> std::array { + std::array quat = {0, 0, 0, 0}; + const char* err = mjs_resolveOrientation(quat.data(), degree, + sequence.ptr, orientation); + if (err) { + throw pybind11::value_error(err); + } + return quat; + }, + py::arg("degree"), py::arg("sequence") = py::none(), + py::arg("orientation"), py::return_value_policy::copy); // ============================= MJSBODY ===================================== mjsBody.def( diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index c35c6618..41a2043c 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -16,6 +16,7 @@ import gc import inspect +import math import os import textwrap import typing @@ -129,6 +130,18 @@ class SpecsTest(absltest.TestCase): """) self.assertEqual(spec.to_xml(), xml) + def test_resolve_orientation(self): + spec = mujoco.MjSpec() + body = spec.worldbody.add_body(euler=[0, 0, 90]) + quat = mujoco.MjSpec.resolve_orientation( + degree=spec.compiler.degree, + sequence=spec.compiler.eulerseq, + orientation=body.alt, + ) + np.testing.assert_array_almost_equal( + quat, [math.sqrt(2) / 2, 0, 0, math.sqrt(2) / 2] + ) + def test_kwarg(self): # Create a spec. spec = mujoco.MjSpec()