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

Fix multiple bugs related to connect and weld constraints with site semantics. Fixes #2179

Browse Source 
The introduction of site specification to connects and welds in 3.2.3 conditionally changed the semantics of `mjData.eq_obj1id` and `mjData.eq_obj2id`. These changes were not properly propagated in several places leading to incorrect computations of constraint inertia, readings of affected force/torque sensors and runtime enabling/disabling of such constraints.

PiperOrigin-RevId: 690670420
Change-Id: I55ee8a013cbee8457f8d6c7f33c2981aedafbab6
This commit is contained in:
Yuval Tassa
2024-10-28 10:43:09 -07:00
committed by Copybara-Service
parent 2c86136dd6
commit 864b805a6e
10 changed files with 234 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_solver_test.cc
+32 -7
View File
@@ -14,7 +14,10 @@
// Tests for engine/engine_solver.c
#include <algorithm>
#include <cstdlib>
#include <string>
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
@@ -27,6 +30,28 @@ namespace {
using ::testing::DoubleNear;
using ::testing::NotNull;
using ::testing::Pointwise;
using ::std::vector;
using ::std::abs;
using ::std::max;
// compare two vectors, relative error (reduces size of large vector elements)
inline void ExpectEqRel(vector<mjtNum> v1, vector<mjtNum> v2, mjtNum rtol) {
ASSERT_TRUE(v1.size() == v2.size());
// make scale vector
int n = v1.size();
vector<mjtNum> scale(n);
for (int i = 0; i < n; i++) {
scale[i] = max(1.0, abs(v1[i]) + abs(v2[i]));
}
// scale and compare
for (int i = 0; i < n; i++) {
v1[i] /= scale[i];
v2[i] /= scale[i];
}
EXPECT_THAT(v1, Pointwise(DoubleNear(rtol), v2));
}
using SolverTest = MujocoTest;
@@ -51,7 +76,7 @@ TEST_F(SolverTest, IslandsEquivalent) {
mjData* data_island = mj_makeData(model);
mjData* data_noisland = mj_makeData(model);
mjtNum tol = 2e-4;
mjtNum rtol = 1e-5;
for (bool warmstart : {true, false}) {
if (warmstart) {
@@ -73,8 +98,8 @@ TEST_F(SolverTest, IslandsEquivalent) {
mj_forward(model, data_island);
model->opt.enableflags &= ~mjENBL_ISLAND; // disable islands
EXPECT_THAT(AsVector(data_noisland->qacc, nv),
Pointwise(DoubleNear(tol), AsVector(data_island->qacc, nv)));
ExpectEqRel(AsVector(data_noisland->qacc, nv),
AsVector(data_island->qacc, nv), rtol);
}
}
@@ -102,7 +127,7 @@ TEST_F(SolverTest, OneBigIsland) {
mjData* data_noisland = mj_makeData(model);
int nv = model->nv;
mjtNum tol = 1e-8;
mjtNum rtol = 1e-7;
// save current (default) iterations
int iterations_default = model->opt.iterations;
@@ -144,9 +169,9 @@ TEST_F(SolverTest, OneBigIsland) {
model->opt.enableflags &= ~mjENBL_ISLAND;
model->opt.iterations = iterations_default;
// compare accelerations
EXPECT_THAT(AsVector(data_noisland->qacc, nv),
Pointwise(DoubleNear(tol), AsVector(data_island->qacc, nv)));
// compare accelerations (relative error)
ExpectEqRel(AsVector(data_noisland->qacc, nv),
AsVector(data_island->qacc, nv), rtol);
}
mj_deleteData(data_noisland);