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

Replace TriangleMeshDistance with custom BVH-based SDF computation.

Browse Source 
The `mjCOctree::ComputeSdfCoeffs` function has been replaced by `mjCOctree::ComputeSdf`. The new implementation removes the dependency on the `triangle_mesh_distance` library. Instead, it uses a provided `mjCBoundingVolumeHierarchy` to perform closest point queries on the mesh. The signed distance is computed by finding the closest triangle face via the BVH and determining the sign based on the dot product of the vector from the closest point on the triangle to the octree vertex and the triangle's normal.

Optional Laplacian smoothing has been added to the SDF coefficients. This helps smooth out potential discontinuities at octree level boundaries.

PiperOrigin-RevId: 875093374
Change-Id: I6fa53243a2dda107ccf255bc6b876ca5a4554d4f
This commit is contained in:
Alessio Quaglino
2026-02-25 04:46:49 -08:00
committed by Copybara-Service
parent 52ed96bc3a
commit 22e3217fc0
5 changed files with 411 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cmake/MujocoDependencies.cmake
-30
View File
@@ -58,11 +58,6 @@ set(MUJOCO_DEP_VERSION_benchmark
CACHE STRING "Version of `benchmark` to be fetched."
)
set(MUJOCO_DEP_VERSION_TriangleMeshDistance
2cb643de1436e1ba8e2be49b07ec5491ac604457
CACHE STRING "Version of `TriangleMeshDistance` to be fetched."
)
mark_as_advanced(MUJOCO_DEP_VERSION_lodepng)
mark_as_advanced(MUJOCO_DEP_VERSION_MarchingCubeCpp)
mark_as_advanced(MUJOCO_DEP_VERSION_tinyxml2)
@@ -73,7 +68,6 @@ mark_as_advanced(MUJOCO_DEP_VERSION_Eigen3)
mark_as_advanced(MUJOCO_DEP_VERSION_abseil)
mark_as_advanced(MUJOCO_DEP_VERSION_gtest)
mark_as_advanced(MUJOCO_DEP_VERSION_benchmark)
mark_as_advanced(MUJOCO_DEP_VERSION_TriangleMeshDistance)
include(FetchContent)
include(FindOrFetch)
@@ -202,30 +196,6 @@ if(CMAKE_POLICY_VERSION_MINIMUM_LOCALLY_DEFINED)
unset(CMAKE_POLICY_VERSION_MINIMUM_LOCALLY_DEFINED)
endif()
if(NOT TARGET trianglemeshdistance)
FetchContent_Declare(
trianglemeshdistance
GIT_REPOSITORY https://github.com/InteractiveComputerGraphics/TriangleMeshDistance.git
GIT_TAG ${MUJOCO_DEP_VERSION_TriangleMeshDistance}
)
FetchContent_GetProperties(trianglemeshdistance)
if(NOT trianglemeshdistance_POPULATED)
FetchContent_Populate(trianglemeshdistance)
# Patch the source code to silence a warning/error related to a loop variable creating a copy.
# Since this is a header only library this fix is less intrusive than disabling the warning for
# any target including the header.
set(TMD_HEADER ${trianglemeshdistance_SOURCE_DIR}/TriangleMeshDistance/include/tmd/TriangleMeshDistance.h)
file(READ ${TMD_HEADER} TMD_CONTENT)
string(REPLACE
"for (const auto edge_count : edges_count) {"
"for (const auto& edge_count : edges_count) {"
TMD_CONTENT "${TMD_CONTENT}")
file(WRITE ${TMD_HEADER} "${TMD_CONTENT}")
include_directories(${trianglemeshdistance_SOURCE_DIR})
endif()
endif()
set(ENABLE_DOUBLE_PRECISION ON)
set(CCD_HIDE_ALL_SYMBOLS ON)
src/user/user_mesh.cc
+1 -1
View File
@@ -779,7 +779,7 @@ void mjCMesh::TryCompile(const mjVFS* vfs) {
// compute sdf coefficients
if (!plugin.active) {
octree_.ComputeSdfCoeffs(vert_.data(), nvert(), face_.data(), nface());
octree_.ComputeSdfCoeffs(vert_.data(), nvert(), face_.data(), nface(), tree_);
}
}
src/user/user_objects.cc
+253 -10
View File
@@ -25,10 +25,12 @@
#include <deque>
#include <functional>
#include <limits>
#include <map>
#include <memory>
#include <new>
#include <optional>
#include <random>
#include <set>
#include <sstream>
#include <string>
#include <string_view>
@@ -47,7 +49,6 @@
#include "user/user_model.h"
#include "user/user_resource.h"
#include "user/user_util.h"
#include <TriangleMeshDistance/include/tmd/TriangleMeshDistance.h>
namespace {
namespace mju = ::mujoco::util;
@@ -375,6 +376,7 @@ void mjCBoundingVolumeHierarchy::RemoveInactiveVolumes(int nmax) {
bvleaf_.erase(bvleaf_.begin() + nmax, bvleaf_.end());
}
const mjCBoundingVolume*
mjCBoundingVolumeHierarchy::AddBoundingVolume(int id, int contype, int conaffinity,
const double* pos, const double* quat,
@@ -553,6 +555,7 @@ void mjCOctree::CopyLevel(int* level) const {
}
}
void mjCOctree::CopyChild(int* child) const {
for (int i = 0; i < node_.size(); ++i) {
for (int j = 0; j < 8; ++j) {
@@ -561,6 +564,7 @@ void mjCOctree::CopyChild(int* child) const {
}
}
void mjCOctree::CopyAabb(mjtNum* aabb) const {
for (int i = 0; i < node_.size(); ++i) {
aabb[i * 6 + 0] = (node_[i].aamm[0] + node_[i].aamm[3]) / 2;
@@ -572,6 +576,7 @@ void mjCOctree::CopyAabb(mjtNum* aabb) const {
}
}
void mjCOctree::CopyCoeff(mjtNum* coeff) const {
for (int i = 0; i < node_.size(); ++i) {
for (int j = 0; j < 8; ++j) {
@@ -580,6 +585,7 @@ void mjCOctree::CopyCoeff(mjtNum* coeff) const {
}
}
void mjCOctree::SetFace(const std::vector<double>& vert, const std::vector<int>& face) {
for (int i = 0; i < face.size(); i += 3) {
std::array<double, 3> v0 = {vert[3*face[i+0]], vert[3*face[i+0]+1], vert[3*face[i+0]+2]};
@@ -622,14 +628,207 @@ void mjCOctree::CreateOctree(const double aamm[6]) {
}
// compute SDF coefficients at octree vertices using triangle mesh distance
void mjCOctree::ComputeSdfCoeffs(const double* vert, int nvert,
const int* face, int nface) {
tmd::TriangleMeshDistance sdf(vert, static_cast<size_t>(nvert),
face, static_cast<size_t>(nface));
namespace {
std::vector<double> coeffs(NumVerts());
std::vector<bool> processed(NumVerts(), false);
double pointBoxDistSq(const double* p, const mjtNum* aabb) {
double dist_sq = 0;
for (int i = 0; i < 3; ++i) {
double lo = aabb[i] - aabb[i + 3];
double hi = aabb[i] + aabb[i + 3];
if (p[i] < lo) {
dist_sq += (lo - p[i]) * (lo - p[i]);
} else if (p[i] > hi) {
dist_sq += (p[i] - hi) * (p[i] - hi);
}
}
return dist_sq;
}
// compute squared distance between point p and triangle (v0, v1, v2),
// and return barycentric coordinates (u,v) of the closest point
double pointTriDistSqWithUV(const double* p, const double* v0, const double* v1,
const double* v2, double& out_u, double& out_v) {
double ab[3] = {v1[0] - v0[0], v1[1] - v0[1], v1[2] - v0[2]};
double ac[3] = {v2[0] - v0[0], v2[1] - v0[1], v2[2] - v0[2]};
double ap[3] = {p[0] - v0[0], p[1] - v0[1], p[2] - v0[2]};
// the closest point on the triangle is determined by partitioning space into Voronoi regions
double d1 = ab[0]*ap[0] + ab[1]*ap[1] + ab[2]*ap[2];
double d2 = ac[0]*ap[0] + ac[1]*ap[1] + ac[2]*ap[2];
// region A (vertex v0)
if (d1 <= 0 && d2 <= 0) {
out_u = 0; out_v = 0;
return ap[0]*ap[0] + ap[1]*ap[1] + ap[2]*ap[2];
}
double bp[3] = {p[0] - v1[0], p[1] - v1[1], p[2] - v1[2]};
double d3 = ab[0]*bp[0] + ab[1]*bp[1] + ab[2]*bp[2];
double d4 = ac[0]*bp[0] + ac[1]*bp[1] + ac[2]*bp[2];
// region B (vertex v1)
if (d3 >= 0 && d4 <= d3) {
out_u = 1; out_v = 0;
return bp[0]*bp[0] + bp[1]*bp[1] + bp[2]*bp[2];
}
// region AB (edge v0-v1)
double vc = d1*d4 - d3*d2;
if (vc <= 0 && d1 >= 0 && d3 <= 0) {
double u = d1 / (d1 - d3);
out_u = u; out_v = 0;
double closest[3] = {v0[0] + u*ab[0], v0[1] + u*ab[1], v0[2] + u*ab[2]};
return (p[0]-closest[0])*(p[0]-closest[0]) +
(p[1]-closest[1])*(p[1]-closest[1]) +
(p[2]-closest[2])*(p[2]-closest[2]);
}
double cp[3] = {p[0] - v2[0], p[1] - v2[1], p[2] - v2[2]};
double d5 = ab[0]*cp[0] + ab[1]*cp[1] + ab[2]*cp[2];
double d6 = ac[0]*cp[0] + ac[1]*cp[1] + ac[2]*cp[2];
// region C (vertex v2)
if (d6 >= 0 && d5 <= d6) {
out_u = 0; out_v = 1;
return cp[0]*cp[0] + cp[1]*cp[1] + cp[2]*cp[2];
}
// region AC (edge v0-v2)
double vb = d5*d2 - d1*d6;
if (vb <= 0 && d2 >= 0 && d6 <= 0) {
double v = d2 / (d2 - d6);
out_u = 0; out_v = v;
double closest[3] = {v0[0] + v*ac[0], v0[1] + v*ac[1], v0[2] + v*ac[2]};
return (p[0]-closest[0])*(p[0]-closest[0]) +
(p[1]-closest[1])*(p[1]-closest[1]) +
(p[2]-closest[2])*(p[2]-closest[2]);
}
// region BC (edge v1-v2)
double va = d3*d6 - d5*d4;
if (va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0) {
double w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
out_u = 1 - w; out_v = w;
double bc[3] = {v2[0] - v1[0], v2[1] - v1[1], v2[2] - v1[2]};
double closest[3] = {v1[0] + w*bc[0], v1[1] + w*bc[1], v1[2] + w*bc[2]};
return (p[0]-closest[0])*(p[0]-closest[0]) +
(p[1]-closest[1])*(p[1]-closest[1]) +
(p[2]-closest[2])*(p[2]-closest[2]);
}
// region ABC (inside triangle)
double denom = 1.0 / (va + vb + vc);
double u = vb * denom;
double v = vc * denom;
out_u = u; out_v = v;
double closest[3] = {v0[0] + u*ab[0] + v*ac[0],
v0[1] + u*ab[1] + v*ac[1],
v0[2] + u*ab[2] + v*ac[2]};
return (p[0]-closest[0])*(p[0]-closest[0]) +
(p[1]-closest[1])*(p[1]-closest[1]) +
(p[2]-closest[2])*(p[2]-closest[2]);
}
// query BVH for closest face to point p, return distance, face index and barycentric coordinates
void queryClosestBVHWithFace(const mjtNum* bvh, const int* child, const int* nodeid,
const double* vert, const int* face, int node_idx,
const double* p, double& best_dist_sq,
int& best_face, double& best_u, double& best_v) {
const mjtNum* aabb = &bvh[node_idx * 6];
if (pointBoxDistSq(p, aabb) >= best_dist_sq) return;
int left = child[node_idx * 2];
int right = child[node_idx * 2 + 1];
if (left == -1 && right == -1) {
int fi = nodeid[node_idx];
if (fi >= 0) {
const double* v0 = vert + face[fi * 3 + 0] * 3;
const double* v1 = vert + face[fi * 3 + 1] * 3;
const double* v2 = vert + face[fi * 3 + 2] * 3;
double u, v;
double dist_sq = pointTriDistSqWithUV(p, v0, v1, v2, u, v);
if (dist_sq < best_dist_sq) {
best_dist_sq = dist_sq;
best_face = fi;
best_u = u;
best_v = v;
}
}
return;
}
if (left >= 0) {
queryClosestBVHWithFace(bvh, child, nodeid, vert, face, left, p,
best_dist_sq, best_face, best_u, best_v);
}
if (right >= 0) {
queryClosestBVHWithFace(bvh, child, nodeid, vert, face, right, p,
best_dist_sq, best_face, best_u, best_v);
}
}
double querySignedDistance(const mjtNum* bvh, const int* child, const int* nodeid,
int nbvh, const double* point,
const double* vert, const int* face) {
if (nbvh == 0) {
return 0;
}
double best_dist_sq = 1e20;
int best_face = -1;
double best_u = 0, best_v = 0;
queryClosestBVHWithFace(bvh, child, nodeid, vert, face, 0, point,
best_dist_sq, best_face, best_u, best_v);
double dist = std::sqrt(best_dist_sq);
double sign = 1.0;
if (best_face >= 0) {
const double* v0 = vert + face[best_face * 3 + 0] * 3;
const double* v1 = vert + face[best_face * 3 + 1] * 3;
const double* v2 = vert + face[best_face * 3 + 2] * 3;
double e1[3] = {v1[0]-v0[0], v1[1]-v0[1], v1[2]-v0[2]};
double e2[3] = {v2[0]-v0[0], v2[1]-v0[1], v2[2]-v0[2]};
double normal[3] = {
e1[1]*e2[2] - e1[2]*e2[1],
e1[2]*e2[0] - e1[0]*e2[2],
e1[0]*e2[1] - e1[1]*e2[0]
};
double closest[3] = {
v0[0] + best_u*(v1[0]-v0[0]) + best_v*(v2[0]-v0[0]),
v0[1] + best_u*(v1[1]-v0[1]) + best_v*(v2[1]-v0[1]),
v0[2] + best_u*(v1[2]-v0[2]) + best_v*(v2[2]-v0[2])
};
double u[3] = {point[0]-closest[0], point[1]-closest[1], point[2]-closest[2]};
double dot = u[0]*normal[0] + u[1]*normal[1] + u[2]*normal[2];
double normal_len = mjuu_normvec(normal, 3);
double eps = 1e-12 * normal_len * dist;
sign = (dot > eps) ? 1.0 : -1.0;
}
return sign * dist;
}
} // namespace
double mjCBoundingVolumeHierarchy::QuerySignedDistance(
const double* point, const double* vert, const int* face) const {
return querySignedDistance(bvh_.data(), child_.data(), nodeid_.data(),
nbvh_, point, vert, face);
}
void mjCOctree::ComputeSdfCoeffs(const double* vert, int nvert, const int* face, int nface,
const mjCBoundingVolumeHierarchy& tree) {
std::vector<double> coeffs(nvert_, 0.0);
std::vector<bool> processed(nvert_, false);
std::deque<int> queue;
if (NumNodes() > 0) {
@@ -647,8 +846,16 @@ void mjCOctree::ComputeSdfCoeffs(const double* vert, int nvert,
continue;
}
if (Hang(vert_id).empty()) {
coeffs[vert_id] = sdf.signed_distance(Vert(vert_id)).distance;
// transform from octree frame (body inertial) back to mesh frame
double p_mesh[3];
mjuu_rotVecQuat(p_mesh, Vert(vert_id), iquat_);
p_mesh[0] += ipos_[0];
p_mesh[1] += ipos_[1];
p_mesh[2] += ipos_[2];
coeffs[vert_id] = tree.QuerySignedDistance(p_mesh, vert, face);
} else {
// hanging node: interpolate from parents
double sum_coeff = 0;
for (int dep_id : Hang(vert_id)) {
sum_coeff += coeffs[dep_id];
@@ -666,6 +873,42 @@ void mjCOctree::ComputeSdfCoeffs(const double* vert, int nvert,
}
}
// optional Laplacian smoothing (smooths octree level transitions)
if (smoothing_iterations_ > 0) {
// build vertex neighbor graph from octree connectivity
std::vector<std::set<int>> neighbors(nvert_);
for (int i = 0; i < NumNodes(); ++i) {
static const int edges[12][2] = {
{0, 1}, {2, 3}, {4, 5}, {6, 7},
{0, 2}, {1, 3}, {4, 6}, {5, 7},
{0, 4}, {1, 5}, {2, 6}, {3, 7}
};
for (const auto& edge : edges) {
int v0 = VertId(i, edge[0]);
int v1 = VertId(i, edge[1]);
neighbors[v0].insert(v1);
neighbors[v1].insert(v0);
}
}
// apply Laplacian smoothing
const double alpha = 0.2;
std::vector<double> sdf_new(nvert_);
for (int iter = 0; iter < smoothing_iterations_; ++iter) {
for (int i = 0; i < nvert_; ++i) {
if (neighbors[i].empty()) {
sdf_new[i] = coeffs[i];
} else {
double avg = 0;
for (int j : neighbors[i]) avg += coeffs[j];
avg /= neighbors[i].size();
sdf_new[i] = (1 - alpha) * coeffs[i] + alpha * avg;
}
}
std::swap(coeffs, sdf_new);
}
}
// copy coefficients to the octree nodes
for (int i = 0; i < NumNodes(); ++i) {
for (int j = 0; j < 8; j++) {
@@ -680,7 +923,7 @@ static double dot2(const double* a, const double* b) {
}
// From M. Schwarz and H.-P. Seidel, "Fast Parallel Surface and Solid Voxelization on GPUs".
// from M. Schwarz and H.-P. Seidel, "Fast Parallel Surface and Solid Voxelization on GPUs".
static bool boxTriangle(const Triangle& v, const double aamm[6]) {
// bounding box tests
for (int i = 0; i < 3; i++) {
src/user/user_objects.h
+12 -2
View File
@@ -200,6 +200,10 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ {
+ sizeof(int) * nodeid_.size() + sizeof(int) * level_.size();
}
// query signed distance from point to mesh surface
double QuerySignedDistance(const double* point, const double* vert,
const int* face) const;
private:
// internal class used during BVH construction, for partial sorting of bounding volumes
struct BVElement {
@@ -275,6 +279,7 @@ struct mjCOctree_ {
std::vector<std::vector<int>> hang_; // hanging nodes status (nvert x 1)
double ipos_[3] = {0, 0, 0};
double iquat_[4] = {1, 0, 0, 0};
int smoothing_iterations_ = 0; // Laplacian smoothing iterations (0 = disabled)
};
class mjCOctree : public mjCOctree_ {
@@ -303,8 +308,13 @@ class mjCOctree : public mjCOctree_ {
void AddCoeff(int n, int v, double coeff) { node_[n].coeff[v] = coeff; }
double Coeff(int n, int v) const { return node_[n].coeff[v]; }
// compute SDF coefficients at octree vertices using triangle mesh distance
void ComputeSdfCoeffs(const double* vert, int nvert, const int* face, int nface);
// Set number of Laplacian smoothing iterations (0 = disabled, default)
void SetSmoothingIterations(int iterations) { smoothing_iterations_ = iterations; }
int SmoothingIterations() const { return smoothing_iterations_; }
// compute SDF coefficients via BVH queries, optionally with Laplacian smoothing
void ComputeSdfCoeffs(const double* vert, int nvert, const int* face, int nface,
const mjCBoundingVolumeHierarchy& tree);
private:
void Make(std::vector<Triangle>& elements);
test/user/user_objects_test.cc
+145
View File
@@ -2682,5 +2682,150 @@ TEST_F(UserObjectsTest, ZeroMass) {
mj_deleteModel(model);
}
// ------------- test Octree SDF computation -----------------------------------
using OctreeSDFTest = MujocoTest;
TEST_F(OctreeSDFTest, SphereSDF) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="sphere" builtin="supersphere" params="16 1 1"/>
</asset>
<worldbody>
<geom name="sdf_sphere" type="sdf" mesh="sphere"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
mjData* data = mj_makeData(model);
ASSERT_THAT(data, NotNull());
EXPECT_GT(model->nmesh, 0);
EXPECT_EQ(model->geom_type[0], mjGEOM_SDF);
int geom_id = 0;
int mesh_id = model->geom_dataid[geom_id];
mjSDF sdf;
const mjpPlugin* null_plugin = nullptr;
sdf.plugin = &null_plugin;
sdf.id = &mesh_id;
sdf.type = mjSDFTYPE_SINGLE;
sdf.geomtype = (mjtGeom*)(model->geom_type + geom_id);
// Analytic SDF for unit sphere: distance = |p| - 1
auto analyticSdf = [](const mjtNum* p) -> double {
return mju_sqrt(p[0]*p[0] + p[1]*p[1] + p[2]*p[2]) - 1.0;
};
int sign_errors = 0;
int total_points = 0;
double sum_sq_error = 0.0;
// Test grid of points
for (double x = -2.0; x <= 2.0; x += 0.5) {
for (double y = -2.0; y <= 2.0; y += 0.5) {
for (double z = -2.0; z <= 2.0; z += 0.5) {
mjtNum p[3] = {x, y, z};
double sdf_dist = mjc_distance(model, data, &sdf, p);
double gt_dist = analyticSdf(p);
if ((sdf_dist < 0) != (gt_dist < 0)) {
sign_errors++;
}
double error = sdf_dist - gt_dist;
sum_sq_error += error * error;
total_points++;
}
}
}
double rmse = mju_sqrt(sum_sq_error / total_points);
EXPECT_LT(sign_errors, total_points / 200)
<< "No more than 0.5% of points should have sign errors";
EXPECT_LT(rmse, 0.11) << "RMSE should be less than 0.11";
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(OctreeSDFTest, TorusSDF) {
static constexpr char xml[] = R"(
<mujoco>
<asset>
<mesh name="torus" builtin="supertorus" params="16 0.3 1 1"/>
</asset>
<worldbody>
<geom name="sdf_torus" type="sdf" mesh="torus"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
mjData* data = mj_makeData(model);
ASSERT_THAT(data, NotNull());
EXPECT_GT(model->nmesh, 0);
EXPECT_EQ(model->geom_type[0], mjGEOM_SDF);
int geom_id = 0;
int mesh_id = model->geom_dataid[geom_id];
mjSDF sdf;
const mjpPlugin* null_plugin = nullptr;
sdf.plugin = &null_plugin;
sdf.id = &mesh_id;
sdf.type = mjSDFTYPE_SINGLE;
sdf.geomtype = (mjtGeom*)(model->geom_type + geom_id);
// Analytic SDF for torus: distance = |p_proj| - r, where p_proj is
// projection of p onto circle of radius R, and r is minor radius.
// R=1, r=0.3
auto analyticSdf = [](const mjtNum* p) -> double {
double xy = mju_sqrt(p[0]*p[0] + p[1]*p[1]);
double vec[2] = {xy - 1.0, p[2]};
return mju_sqrt(vec[0]*vec[0] + vec[1]*vec[1]) - 0.3;
};
int sign_errors = 0;
int total_points = 0;
double sum_sq_error = 0.0;
// Test grid of points
for (double x = -2.0; x <= 2.0; x += 0.5) {
for (double y = -2.0; y <= 2.0; y += 0.5) {
for (double z = -2.0; z <= 2.0; z += 0.5) {
mjtNum p[3] = {x, y, z};
double sdf_dist = mjc_distance(model, data, &sdf, p);
double gt_dist = analyticSdf(p);
if ((sdf_dist < 0) != (gt_dist < 0)) {
sign_errors++;
}
double error = sdf_dist - gt_dist;
sum_sq_error += error * error;
total_points++;
}
}
}
double rmse = mju_sqrt(sum_sq_error / total_points);
EXPECT_LT(sign_errors, total_points / 20)
<< "No more than 5% of points should have sign errors";
EXPECT_LT(rmse, 0.52) << "RMSE should be close to 0.516";
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco