From 5a365603f8b320d4db1c76fc5bc04658d9f2932e Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 10 Apr 2024 06:01:40 -0700 Subject: [PATCH] Add `mju_euler2Quat`. PiperOrigin-RevId: 623469398 Change-Id: Ia86b29a764018cd4c3bc57d23616bed7393081f5 --- doc/APIreference/functions.rst | 10 ++++ doc/changelog.rst | 8 ++- doc/includes/references.h | 1 + include/mujoco/mujoco.h | 4 +- introspect/functions.py | 28 ++++++++++ python/mujoco/bindings_test.py | 17 ++++++ python/mujoco/functions.cc | 1 + src/engine/engine_util_spatial.c | 38 +++++++++++++ src/engine/engine_util_spatial.h | 3 ++ test/engine/engine_util_spatial_test.cc | 71 +++++++++++++++++++++++-- unity/Runtime/Bindings/MjBindings.cs | 3 ++ 11 files changed, 176 insertions(+), 8 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index c21e34f7..182f708e 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -2840,6 +2840,16 @@ mju_quatZ2Vec Construct quaternion performing rotation from z-axis to given vector. +.. _mju_euler2Quat: + +mju_euler2Quat +~~~~~~~~~~~~~~ + +.. mujoco-include:: mju_euler2Quat + +Convert sequence of Euler angles (radians) to quaternion. +seq[0,1,2] must be in 'xyzXYZ', lower/upper-case mean intrinsic/extrinsic rotations. + .. _Poses: Poses diff --git a/doc/changelog.rst b/doc/changelog.rst index 81a20c3f..ab6f970e 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -5,10 +5,14 @@ Changelog Upcoming version (not yet released) ----------------------------------- +General +^^^^^^^ +1. Added :ref:`mju_euler2Quat` for converting an Euler-angle sequence to quaternion. + MJX ^^^ -1. Added support for userdata. -2. Added ellipsoid-ellipsoid and ellipsoid-capsule collisions using signed distance functions (SDFs). +2. Added support for userdata. +3. Added ellipsoid-ellipsoid and ellipsoid-capsule collisions using signed distance functions (SDFs). Version 3.1.4 (April 8th, 2024) ------------------------------- diff --git a/doc/includes/references.h b/doc/includes/references.h index 440b6e7d..613dd037 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -2742,6 +2742,7 @@ void mju_mat2Quat(mjtNum quat[4], const mjtNum mat[9]); void mju_derivQuat(mjtNum res[4], const mjtNum quat[4], const mjtNum vel[3]); void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale); void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]); +void mju_euler2Quat(mjtNum quat[4], const mjtNum euler[3], const char* seq); void mju_mulPose(mjtNum posres[3], mjtNum quatres[4], const mjtNum pos1[3], const mjtNum quat1[4], const mjtNum pos2[3], const mjtNum quat2[4]); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 6f6e57a7..3d7c6429 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1033,7 +1033,6 @@ MJAPI void mju_transformSpatial(mjtNum res[6], const mjtNum vec[6], int flg_forc const mjtNum newpos[3], const mjtNum oldpos[3], const mjtNum rotnew2old[9]); - //---------------------------------- Quaternions --------------------------------------------------- // Rotate vector by quaternion. @@ -1072,6 +1071,9 @@ MJAPI void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale); // Construct quaternion performing rotation from z-axis to given vector. MJAPI void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]); +// Convert sequence of Euler angles (radians) to quaternion. +// seq[0,1,2] must be in 'xyzXYZ', lower/upper-case mean intrinsic/extrinsic rotations. +MJAPI void mju_euler2Quat(mjtNum quat[4], const mjtNum euler[3], const char* seq); //---------------------------------- Poses --------------------------------------------------------- diff --git a/introspect/functions.py b/introspect/functions.py index 5a211ffe..01a3a6c6 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -6982,6 +6982,34 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Construct quaternion performing rotation from z-axis to given vector.', # pylint: disable=line-too-long )), + ('mju_euler2Quat', + FunctionDecl( + name='mju_euler2Quat', + return_type=ValueType(name='void'), + parameters=( + FunctionParameterDecl( + name='quat', + type=ArrayType( + inner_type=ValueType(name='mjtNum'), + extents=(4,), + ), + ), + FunctionParameterDecl( + name='euler', + type=ArrayType( + inner_type=ValueType(name='mjtNum', is_const=True), + extents=(3,), + ), + ), + FunctionParameterDecl( + name='seq', + type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + ), + ), + doc="Convert sequence of Euler angles (radians) to quaternion. seq[0,1,2] must be in 'xyzXYZ', lower/upper-case mean intrinsic/extrinsic rotations.", # pylint: disable=line-too-long + )), ('mju_mulPose', FunctionDecl( name='mju_mulPose', diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index ea2e8f7c..42065d35 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -1303,6 +1303,23 @@ Euler integrator, semi-implicit in velocity. mat = np.array([[1., 2., 3.], [4., 5., 6.], [7., 8., 9.]]) self.assertEqual(mujoco.mju_mulVecMatVec(vec1, mat, vec2), 204.) + def test_mju_euler_to_quat(self): + quat = np.zeros(4) + euler = np.array([0, np.pi/2, 0]) + seq = 'xyz' + mujoco.mju_euler2Quat(quat, euler, seq) + expected_quat = np.array([np.sqrt(0.5), 0, np.sqrt(0.5), 0.]) + np.testing.assert_almost_equal(quat, expected_quat) + + error = 'mju_euler2Quat: seq must contain exactly 3 characters' + with self.assertRaisesWithLiteralMatch(mujoco.FatalError, error): + mujoco.mju_euler2Quat(quat, euler, 'xy') + with self.assertRaisesWithLiteralMatch(mujoco.FatalError, error): + mujoco.mju_euler2Quat(quat, euler, 'xyzy') + error = 'mju_euler2Quat: seq[2] is \'p\', should be one of x, y, z, X, Y, Z' + with self.assertRaisesWithLiteralMatch(mujoco.FatalError, error): + mujoco.mju_euler2Quat(quat, euler, 'xYp') + @parameterized.product(flg_html=(False, True), flg_pad=(False, True)) def test_mj_printSchema(self, flg_html, flg_pad): # pylint: disable=invalid-name # Make sure that mj_printSchema doesn't raise an exception diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index db1a874f..0282f8ca 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -995,6 +995,7 @@ PYBIND11_MODULE(_functions, pymodule) { Def(pymodule); Def(pymodule); Def(pymodule); + Def(pymodule); // Poses Def(pymodule); diff --git a/src/engine/engine_util_spatial.c b/src/engine/engine_util_spatial.c index 023b9459..1058087c 100644 --- a/src/engine/engine_util_spatial.c +++ b/src/engine/engine_util_spatial.c @@ -511,3 +511,41 @@ void mju_makeFrame(mjtNum frame[9]) { // zaxis = cross(xaxis, yaxis) mju_cross(frame+6, frame, frame+3); } + + + +// convert sequence of Euler angles (radians) to quaternion +// seq[0,1,2] must be in 'xyzXYZ', lower/upper-case mean intrinsic/extrinsic rotations +void mju_euler2Quat(mjtNum quat[4], const mjtNum euler[3], const char* seq) { + if (strnlen(seq, 4) != 3) { + mjERROR("seq must contain exactly 3 characters"); + } + + // init + double tmp[4] = {1, 0, 0, 0}; + + // loop over euler angles, accumulate rotations + for (int i=0; i<3; i++) { + // construct quaternion rotation + mjtNum rot[4] = {cos(euler[i]/2), 0, 0, 0}; + mjtNum sa = sin(euler[i]/2); + if (seq[i]=='x' || seq[i]=='X') { + rot[1] = sa; + } else if (seq[i]=='y' || seq[i]=='Y') { + rot[2] = sa; + } else if (seq[i]=='z' || seq[i]=='Z') { + rot[3] = sa; + } else { + mjERROR("seq[%d] is '%c', should be one of x, y, z, X, Y, Z", i, seq[i]); + } + + // accumulate rotation + if (seq[i]=='x' || seq[i]=='y' || seq[i]=='z') { + mju_mulQuat(tmp, tmp, rot); // moving axes: post-multiply + } else { + mju_mulQuat(tmp, rot, tmp); // fixed axes: pre-multiply + } + } + + mju_copy4(quat, tmp); +} diff --git a/src/engine/engine_util_spatial.h b/src/engine/engine_util_spatial.h index f7babb85..6d173c5b 100644 --- a/src/engine/engine_util_spatial.h +++ b/src/engine/engine_util_spatial.h @@ -75,6 +75,9 @@ MJAPI void mju_negPose(mjtNum posres[3], mjtNum quatres[4], MJAPI void mju_trnVecPose(mjtNum res[3], const mjtNum pos[3], const mjtNum quat[4], const mjtNum vec[3]); +// convert sequence of Euler angles (radians) to quaternion +// seq[0,1,2] must be in 'xyzXYZ', lower/upper-case mean intrinsic/extrinsic rotations +MJAPI void mju_euler2Quat(mjtNum quat[4], const mjtNum euler[3], const char* seq); //------------------------------ spatial algebra --------------------------------------------------- diff --git a/test/engine/engine_util_spatial_test.cc b/test/engine/engine_util_spatial_test.cc index 04370298..8d968f39 100644 --- a/test/engine/engine_util_spatial_test.cc +++ b/test/engine/engine_util_spatial_test.cc @@ -19,8 +19,10 @@ #include #include +#include #include #include +#include #include "src/engine/engine_util_blas.h" #include "src/engine/engine_util_spatial.h" #include "test/fixture.h" @@ -28,7 +30,9 @@ namespace mujoco { namespace { +using ::testing::DoubleNear; using ::testing::ElementsAre; +using ::testing::Pointwise; using Quat2MatTest = MujocoTest; @@ -103,9 +107,10 @@ TEST_F(RotVecQuatTest, TinyRotation) { ); } -// Alternative way of rotating a vector by explicitly converting the quaternion to a 3x3 matrix -void RotVecQuatWithMatrix(mjtNum res[3], const mjtNum vec[3], const mjtNum quat[4]) { - if (quat[0]==1 && quat[1]==0 && quat[2]==0 && quat[3]==0) { +// Rotate a vector by explicitly converting the quaternion to a 3x3 matrix +void RotVecQuatWithMatrix(mjtNum res[3], const mjtNum vec[3], + const mjtNum quat[4]) { + if (quat[0] == 1 && quat[1] == 0 && quat[2] == 0 && quat[3] == 0) { mju_copy3(res, vec); } else { mjtNum mat[9]; @@ -122,10 +127,10 @@ TEST_F(RotVecQuatTest, TestEquivalence) { // List of angles to rotate by, in degrees mjtNum angles[6] = {0.0, 1e-8, 31, 47, 181, 271}; static const mjtNum eps = 1e-15; - for (auto vec: vecs) { + for (auto vec : vecs) { // Unit-normalize the vector mju_normalize3(vec); - for (auto angleDegree: angles) { + for (auto angleDegree : angles) { // Convert the axis-angle to a quaternion auto angleRad = angleDegree * mjPI / 180; mju_axisAngle2Quat(quat, vec, angleRad); @@ -140,5 +145,61 @@ TEST_F(RotVecQuatTest, TestEquivalence) { } } +using Euler2QuatTest = MujocoTest; + +TEST_F(Euler2QuatTest, BadSeq) { + EXPECT_FATAL_FAILURE({ + mjtNum quat[4]; + mjtNum euler[3] = {0}; + char seq[] = "xiz"; + mju_euler2Quat(quat, euler, seq); + }, "mju_euler2Quat: seq[1] is 'i', should be one of x, y, z, X, Y, Z"); +} + +TEST_F(Euler2QuatTest, BadSeqLength) { + EXPECT_FATAL_FAILURE({ + mjtNum quat[4]; + mjtNum euler[3] = {0}; + char seq[] = "xyzy"; + mju_euler2Quat(quat, euler, seq); + }, "mju_euler2Quat: seq must contain exactly 3 characters"); +} + +TEST_F(Euler2QuatTest, Euler2Quat) { + double quat[4] = {0}; + double tol = 1e-14; + + char seq[] = "xyz"; + double euler[3] = {mjPI, 0, 0}; + double expected[4] = {0, 1, 0, 0}; + mju_euler2Quat(quat, euler, seq); + EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected)); + + euler[1] = mjPI; + double expected2[4] = {0, 0, 0, 1}; + mju_euler2Quat(quat, euler, seq); + EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected2)); + + char seq2[] = "XYZ"; + double expected3[4] = {0, 0, 0, -1}; + mju_euler2Quat(quat, euler, seq2); + EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected3)); + + double euler2[3] = {2*mjPI, 2*mjPI, 2*mjPI}; + double expected4[4] = {-1, 0, 0, 0}; + mju_euler2Quat(quat, euler2, seq); + EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected4)); + mju_euler2Quat(quat, euler2, seq2); + EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected4)); + + double euler3[3] = {mjPI/2, mjPI/2, mjPI/2}; + double expected5[4] = {0, mju_sqrt(.5), 0, mju_sqrt(.5)}; + mju_euler2Quat(quat, euler3, seq); + EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected5)); + mju_euler2Quat(quat, euler3, seq2); + double expected6[4] = {mju_sqrt(.5), 0, mju_sqrt(.5), 0}; + EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected6)); +} + } // namespace } // namespace mujoco diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 4f7b7698..97706459 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -7129,6 +7129,9 @@ public static unsafe extern void mju_quatIntegrate(double* quat, double* vel, do [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mju_quatZ2Vec(double* quat, double* vec); +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern void mju_euler2Quat(double* quat, double* euler, [MarshalAs(UnmanagedType.LPStr)]string seq); + [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mju_mulPose(double* posres, double* quatres, double* pos1, double* quat1, double* pos2, double* quat2);