Add mju_euler2Quat.

PiperOrigin-RevId: 623469398
Change-Id: Ia86b29a764018cd4c3bc57d23616bed7393081f5

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

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)
 -------------------------------

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]);

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 ---------------------------------------------------------
 

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',

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

python/mujoco/functions.cc

@@ -995,6 +995,7 @@ PYBIND11_MODULE(_functions, pymodule) {
   Def<traits::mju_derivQuat>(pymodule);
   Def<traits::mju_quatIntegrate>(pymodule);
   Def<traits::mju_quatZ2Vec>(pymodule);
+  Def<traits::mju_euler2Quat>(pymodule);
 
   // Poses
   Def<traits::mju_mulPose>(pymodule);

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);
+}

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 ---------------------------------------------------
 

test/engine/engine_util_spatial_test.cc

@@ -19,8 +19,10 @@
 
 #include <gmock/gmock.h>
 #include <gtest/gtest.h>
+#include <gtest/gtest-spi.h>
 #include <mujoco/mjmodel.h>
 #include <mujoco/mjtnum.h>
+#include <mujoco/mujoco.h>
 #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

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);