chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add mju_euler2Quat.

Browse Source 
PiperOrigin-RevId: 623469398
Change-Id: Ia86b29a764018cd4c3bc57d23616bed7393081f5
This commit is contained in:
Yuval Tassa
2024-04-10 06:01:40 -07:00
committed by Copybara-Service
parent 2b3f336bd3
commit 5a365603f8
11 changed files with 176 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_util_spatial_test.cc
+66 -5
View File
@@ -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