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

Change LoadModelFromString to return a smart pointer, add MakeData, and update tests to have C++ RAII clean up model and data.

Browse Source 
PiperOrigin-RevId: 935980153
Change-Id: I41d25bfab4935494dc984168820cb7cad123cadf
This commit is contained in:
Kyle Bayes
2026-06-22 04:18:29 -07:00
committed by Copybara-Service
parent 34d142ee50
commit 1490336955
39 changed files with 4616 additions and 5908 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_collision_box_test.cc
+21 -30
View File
@@ -72,8 +72,8 @@ TEST_F(MjCollisionBoxTest, BadContacts) {
g2 = g2new;
// call low-level box-box collider
int num = mjc_BoxBox(model, data, precon.data(), g1, g2,
con->includemargin);
int num =
mjc_BoxBox(model, data, precon.data(), g1, g2, con->includemargin);
// allocate and clear arrays marking already matched contacts
mju_zeroInt(match_raw.data(), num);
@@ -83,8 +83,7 @@ TEST_F(MjCollisionBoxTest, BadContacts) {
int nmatched = 0;
for (int i = 0; i < num; i++) {
for (int j = 0; j < data->ncon; j++) {
if (!match[j] &&
precon[i].pos[0] == data->contact[j].pos[0] &&
if (!match[j] && precon[i].pos[0] == data->contact[j].pos[0] &&
precon[i].pos[1] == data->contact[j].pos[1] &&
precon[i].pos[2] == data->contact[j].pos[2]) {
match_raw[i] = match[j] = 1;
@@ -110,17 +109,17 @@ TEST_F(MjCollisionBoxTest, BadContacts) {
if (!match_raw[i]) {
// === check if outside
mjtNum sz1[3] = {size1[0]+margin, size1[1]+margin, size1[2]+margin};
mjtNum sz2[3] = {size2[0]+margin, size2[1]+margin, size2[2]+margin};
mjtNum sz1[3] = {size1[0] + margin, size1[1] + margin,
size1[2] + margin};
mjtNum sz2[3] = {size2[0] + margin, size2[1] + margin,
size2[2] + margin};
// relative distance (1%) outside of which contacts are removed
static mjtNum kRatio = 1.01;
// is the contact outside: 1, inside: -1, within the removal width: 0
int out1 = mju_outsideBox(precon[i].pos, pos1, mat1, sz1,
kRatio);
int out2 = mju_outsideBox(precon[i].pos, pos2, mat2, sz2,
kRatio);
int out1 = mju_outsideBox(precon[i].pos, pos1, mat1, sz1, kRatio);
int out2 = mju_outsideBox(precon[i].pos, pos2, mat2, sz2, kRatio);
// mark as bad if outside one box and not inside the other box
bool outside = (out1 == 1 && out2 != -1) || (out2 == 1 && out1 != -1);
@@ -151,7 +150,6 @@ TEST_F(MjCollisionBoxTest, DuplicateContacts) {
std::vector<int> match_raw(mjMAXCONPAIR);
std::vector<int> match(data->ncon);
int g1 = -1;
int g2 = -1;
for (int c = 0; c < data->ncon; c++) {
@@ -174,8 +172,8 @@ TEST_F(MjCollisionBoxTest, DuplicateContacts) {
g2 = g2new;
// call low-level box-box collider
int num = mjc_BoxBox(model, data, precon.data(), g1, g2,
con->includemargin);
int num =
mjc_BoxBox(model, data, precon.data(), g1, g2, con->includemargin);
// allocate and clear arrays marking already matched contacts
mju_zeroInt(match_raw.data(), num);
@@ -185,8 +183,7 @@ TEST_F(MjCollisionBoxTest, DuplicateContacts) {
int nmatched = 0;
for (int i = 0; i < num; i++) {
for (int j = 0; j < data->ncon; j++) {
if (!match[j] &&
precon[i].pos[0] == data->contact[j].pos[0] &&
if (!match[j] && precon[i].pos[0] == data->contact[j].pos[0] &&
precon[i].pos[1] == data->contact[j].pos[1] &&
precon[i].pos[2] == data->contact[j].pos[2]) {
match_raw[i] = match[j] = 1;
@@ -255,20 +252,17 @@ TEST_F(MjCollisionBoxTest, BoxSphere) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
for (mjtNum z : {-.015, -.00501, -.005, -.00499, 0.0, 0.004}) {
data->qpos[2] = z;
mj_forward(model, data);
mj_forward(model.get(), data.get());
EXPECT_EQ(data->ncon, 2);
EXPECT_THAT(data->contact[0].dist,
MjNear(data->contact[1].dist, 1e-8, 1e-6));
}
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(MjCollisionBoxTest, BoxBoxContactDistance) {
@@ -281,22 +275,19 @@ TEST_F(MjCollisionBoxTest, BoxBoxContactDistance) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model);
mj_kinematics(model, data);
MjDataPtr data = MakeData(model);
mj_kinematics(model.get(), data.get());
mjPreContact precon[mjMAXCONPAIR];
for (mjfCollision collision : {mjc_BoxBox, mjc_Convex}) {
int n = collision(model, data, precon, 0, 1, 0.0);
int n = collision(model.get(), data.get(), precon, 0, 1, 0.0);
for (int i = 0; i < n; i++) {
EXPECT_NEAR(precon[i].dist, -0.5, MjTol(1e-8, 1e-6));
}
}
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
test/engine/engine_collision_driver_test.cc
+90 -112
View File
@@ -14,6 +14,8 @@
// Tests for engine/engine_collision_driver.c.
#include "src/engine/engine_collision_driver.h"
#include <algorithm>
#include <cmath>
#include <cstddef>
@@ -26,22 +28,20 @@
#include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h>
#include "test/fixture.h"
#include "src/engine/engine_collision_driver.h"
namespace mujoco {
namespace {
using MjCollisionTest = MujocoTest;
using GeomPair = std::pair<std::string, std::string>;
using ::testing::IsEmpty;
using ::testing::ElementsAre;
using ::testing::IsEmpty;
using ::testing::NotNull;
// Returns a sorted list of pairs of colliding geom names, where each pair of
// geom names is sorted.
static std::vector<GeomPair> colliding_pairs(
const mjModel* model, const mjData* data) {
static std::vector<GeomPair> colliding_pairs(const mjModel* model,
const mjData* data) {
std::vector<GeomPair> result;
for (int i = 0; i < data->ncon; i++) {
std::string geom1 = mj_id2name(model, mjOBJ_GEOM, data->contact[i].geom[0]);
@@ -53,18 +53,16 @@ static std::vector<GeomPair> colliding_pairs(
}
TEST_F(MjCollisionTest, AllCollisions) {
static const char* const kModelFilePath =
"engine/testdata/collisions.xml";
static const char* const kModelFilePath = "engine/testdata/collisions.xml";
const std::string xml_path = GetTestDataFilePath(kModelFilePath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
mjData* data = mj_makeData(model);
// mjCOL_ALL is the default
mj_fwdPosition(model, data);
EXPECT_THAT(colliding_pairs(model, data), ElementsAre(
GeomPair("box", "sphere_collides"),
GeomPair("box", "sphere_predefined")
));
EXPECT_THAT(colliding_pairs(model, data),
ElementsAre(GeomPair("box", "sphere_collides"),
GeomPair("box", "sphere_predefined")));
mj_deleteData(data);
mj_deleteModel(model);
@@ -72,20 +70,16 @@ TEST_F(MjCollisionTest, AllCollisions) {
TEST_F(MjCollisionTest, EmptyModel) {
char error[1024];
mjModel* model = LoadModelFromString("<mujoco/>", error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
MjModelPtr model = LoadModelFromString("<mujoco/>", error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mj_fwdPosition(model, data);
EXPECT_THAT(colliding_pairs(model, data), IsEmpty());
mj_deleteData(data);
mj_deleteModel(model);
mj_fwdPosition(model.get(), data.get());
EXPECT_THAT(colliding_pairs(model.get(), data.get()), IsEmpty());
}
TEST_F(MjCollisionTest, ZeroedHessian) {
static const char* const kModelFilePath =
"engine/testdata/collisions.xml";
static const char* const kModelFilePath = "engine/testdata/collisions.xml";
const std::string xml_path = GetTestDataFilePath(kModelFilePath);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
mjData* data = mj_makeData(model);
@@ -123,18 +117,15 @@ TEST_F(MjCollisionTest, ContactCount) {
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull());
mj_forward(m, d);
mj_forward(m.get(), d.get());
// there are 8 spheres, all touching the floor
EXPECT_EQ(d->ncon, 8);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(MjCollisionTest, FilterParent) {
@@ -159,12 +150,12 @@ TEST_F(MjCollisionTest, FilterParent) {
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull());
mj_fwdPosition(m, d);
mj_fwdPosition(m.get(), d.get());
// there should be zero contacts, because colliding1 and colliding2 are in
// bodies that have a parent-child relationship, through welds
@@ -172,13 +163,10 @@ TEST_F(MjCollisionTest, FilterParent) {
// when this filtering is disabled, the geoms should collide
m->opt.disableflags |= mjDSBL_FILTERPARENT;
mj_fwdPosition(m, d);
mj_fwdPosition(m.get(), d.get());
EXPECT_THAT(colliding_pairs(m, d),
EXPECT_THAT(colliding_pairs(m.get(), d.get()),
ElementsAre(GeomPair("colliding1", "colliding2")));
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(MjCollisionTest, FilterParentDoesntAffectWorldBody) {
@@ -194,20 +182,17 @@ TEST_F(MjCollisionTest, FilterParentDoesntAffectWorldBody) {
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull());
mj_fwdPosition(m, d);
mj_fwdPosition(m.get(), d.get());
// even though colliding1 and colliding2 are have a parent-child relationship,
// they collide because colliding1 is in <worldbody>
EXPECT_THAT(colliding_pairs(m, d),
EXPECT_THAT(colliding_pairs(m.get(), d.get()),
ElementsAre(GeomPair("colliding1", "colliding2")));
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(MjCollisionTest, TestOBB) {
@@ -219,14 +204,18 @@ TEST_F(MjCollisionTest, TestOBB) {
mjtNum mat2[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
EXPECT_THAT(
mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, 0, NULL, NULL, 0), true);
mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, 0, NULL, NULL, 0),
true);
// rotate by 45 degrees
mat2[0] = 1./mju_sqrt(2.); mat2[1] = -1./mju_sqrt(2.);
mat2[3] = 1./mju_sqrt(2.); mat2[4] = 1./mju_sqrt(2.);
mat2[0] = 1. / mju_sqrt(2.);
mat2[1] = -1. / mju_sqrt(2.);
mat2[3] = 1. / mju_sqrt(2.);
mat2[4] = 1. / mju_sqrt(2.);
EXPECT_THAT(
mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, 0, NULL, NULL, 0), false);
mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, 0, NULL, NULL, 0),
false);
}
TEST_F(MjCollisionTest, PlaneInBody) {
@@ -244,13 +233,11 @@ TEST_F(MjCollisionTest, PlaneInBody) {
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull());
mj_step(m, d);
mj_deleteData(d);
mj_deleteModel(m);
mj_step(m.get(), d.get());
}
TEST_F(MjCollisionTest, PinchingSucceeds) {
@@ -299,13 +286,13 @@ TEST_F(MjCollisionTest, PinchingSucceeds) {
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull());
int lift_id = mj_name2id(m, mjOBJ_ACTUATOR, "lift");
int grasp_id = mj_name2id(m, mjOBJ_ACTUATOR, "grasp");
int lift_id = mj_name2id(m.get(), mjOBJ_ACTUATOR, "lift");
int grasp_id = mj_name2id(m.get(), mjOBJ_ACTUATOR, "grasp");
// Phase 1: Lower gripper.
// The gripper base starts at z=0.5. The finger has length 0.2 (size 0.1),
@@ -316,21 +303,21 @@ TEST_F(MjCollisionTest, PinchingSucceeds) {
for (int i = 0; i < 500; ++i) {
d->ctrl[lift_id] = -0.35; // Lower
d->ctrl[grasp_id] = 0; // Open
mj_step(m, d);
mj_step(m.get(), d.get());
}
// Phase 2: Pinch
for (int i = 0; i < 100; ++i) {
d->ctrl[lift_id] = -0.35; // Hold height
d->ctrl[grasp_id] = 0.8; // Close (max 1)
mj_step(m, d);
mj_step(m.get(), d.get());
}
// Phase 3: Lift
for (int i = 0; i < 1000; ++i) {
d->ctrl[lift_id] = 0.5; // Lift up
d->ctrl[grasp_id] = 0.8; // Keep closed
mj_step(m, d);
mj_step(m.get(), d.get());
}
// Check if cloth is lifted
@@ -352,9 +339,6 @@ TEST_F(MjCollisionTest, PinchingSucceeds) {
// Specialized primitives (mjraw_BoxTriangle, mjraw_CapsuleTriangle) should
// enable stable pinching, so we expect the cloth to be lifted.
EXPECT_GT(avg_z, 0.2) << "Cloth slipped out of gripper!";
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(MjCollisionTest, MarginSumming) {
@@ -376,18 +360,15 @@ TEST_F(MjCollisionTest, MarginSumming) {
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull());
mj_fwdPosition(m, d);
mj_fwdPosition(m.get(), d.get());
// With margin summing, we expect 1 contact
EXPECT_EQ(d->ncon, 1);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(MjCollisionTest, MaxContact) {
@@ -414,48 +395,45 @@ TEST_F(MjCollisionTest, MaxContact) {
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull());
int mesh = mj_name2id(m, mjOBJ_GEOM, "mesh");
int box = mj_name2id(m, mjOBJ_GEOM, "box");
int plane = mj_name2id(m, mjOBJ_GEOM, "plane");
int sphere = mj_name2id(m, mjOBJ_GEOM, "sphere");
int capsule = mj_name2id(m, mjOBJ_GEOM, "capsule");
int ellipsoid = mj_name2id(m, mjOBJ_GEOM, "ellipsoid");
int cylinder = mj_name2id(m, mjOBJ_GEOM, "cylinder");
int mesh = mj_name2id(m.get(), mjOBJ_GEOM, "mesh");
int box = mj_name2id(m.get(), mjOBJ_GEOM, "box");
int plane = mj_name2id(m.get(), mjOBJ_GEOM, "plane");
int sphere = mj_name2id(m.get(), mjOBJ_GEOM, "sphere");
int capsule = mj_name2id(m.get(), mjOBJ_GEOM, "capsule");
int ellipsoid = mj_name2id(m.get(), mjOBJ_GEOM, "ellipsoid");
int cylinder = mj_name2id(m.get(), mjOBJ_GEOM, "cylinder");
EXPECT_EQ(mj_maxContact(m, mesh, box, -1), 4);
EXPECT_EQ(mj_maxContact(m, mesh, plane, -1), 3);
EXPECT_EQ(mj_maxContact(m, box, plane, -1), 4);
EXPECT_EQ(mj_maxContact(m, mesh, mesh, -1), 4);
EXPECT_EQ(mj_maxContact(m, box, box, -1), 8);
EXPECT_EQ(mj_maxContact(m, capsule, capsule, -1), 2);
EXPECT_EQ(mj_maxContact(m, capsule, box, -1), 4);
EXPECT_EQ(mj_maxContact(m, capsule, plane, -1), 2);
EXPECT_EQ(mj_maxContact(m, cylinder, plane, -1), 4);
EXPECT_EQ(mj_maxContact(m, sphere, sphere, -1), 1);
EXPECT_EQ(mj_maxContact(m, sphere, capsule, -1), 1);
EXPECT_EQ(mj_maxContact(m, sphere, box, -1), 1);
EXPECT_EQ(mj_maxContact(m, sphere, mesh, -1), 1);
EXPECT_EQ(mj_maxContact(m, sphere, plane, -1), 1);
EXPECT_EQ(mj_maxContact(m, sphere, cylinder, -1), 1);
EXPECT_EQ(mj_maxContact(m, ellipsoid, ellipsoid, -1), 1);
EXPECT_EQ(mj_maxContact(m, ellipsoid, box, -1), 1);
EXPECT_EQ(mj_maxContact(m, ellipsoid, mesh, -1), 1);
EXPECT_EQ(mj_maxContact(m, ellipsoid, plane, -1), 1);
EXPECT_EQ(mj_maxContact(m, ellipsoid, cylinder, -1), 1);
EXPECT_EQ(mj_maxContact(m, ellipsoid, capsule, -1), 1);
EXPECT_EQ(mj_maxContact(m, capsule, cylinder, -1), 5);
EXPECT_EQ(mj_maxContact(m, capsule, mesh, -1), 5);
EXPECT_EQ(mj_maxContact(m, cylinder, cylinder, -1), 5);
EXPECT_EQ(mj_maxContact(m, cylinder, box, -1), 5);
EXPECT_EQ(mj_maxContact(m, cylinder, mesh, -1), 5);
mj_deleteData(d);
mj_deleteModel(m);
EXPECT_EQ(mj_maxContact(m.get(), mesh, box, -1), 4);
EXPECT_EQ(mj_maxContact(m.get(), mesh, plane, -1), 3);
EXPECT_EQ(mj_maxContact(m.get(), box, plane, -1), 4);
EXPECT_EQ(mj_maxContact(m.get(), mesh, mesh, -1), 4);
EXPECT_EQ(mj_maxContact(m.get(), box, box, -1), 8);
EXPECT_EQ(mj_maxContact(m.get(), capsule, capsule, -1), 2);
EXPECT_EQ(mj_maxContact(m.get(), capsule, box, -1), 4);
EXPECT_EQ(mj_maxContact(m.get(), capsule, plane, -1), 2);
EXPECT_EQ(mj_maxContact(m.get(), cylinder, plane, -1), 4);
EXPECT_EQ(mj_maxContact(m.get(), sphere, sphere, -1), 1);
EXPECT_EQ(mj_maxContact(m.get(), sphere, capsule, -1), 1);
EXPECT_EQ(mj_maxContact(m.get(), sphere, box, -1), 1);
EXPECT_EQ(mj_maxContact(m.get(), sphere, mesh, -1), 1);
EXPECT_EQ(mj_maxContact(m.get(), sphere, plane, -1), 1);
EXPECT_EQ(mj_maxContact(m.get(), sphere, cylinder, -1), 1);
EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, ellipsoid, -1), 1);
EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, box, -1), 1);
EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, mesh, -1), 1);
EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, plane, -1), 1);
EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, cylinder, -1), 1);
EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, capsule, -1), 1);
EXPECT_EQ(mj_maxContact(m.get(), capsule, cylinder, -1), 5);
EXPECT_EQ(mj_maxContact(m.get(), capsule, mesh, -1), 5);
EXPECT_EQ(mj_maxContact(m.get(), cylinder, cylinder, -1), 5);
EXPECT_EQ(mj_maxContact(m.get(), cylinder, box, -1), 5);
EXPECT_EQ(mj_maxContact(m.get(), cylinder, mesh, -1), 5);
}
} // namespace
test/engine/engine_collision_gjk_test.cc
+97 -111
View File
@@ -42,9 +42,6 @@ using ::testing::ElementsAre;
using ::testing::Pointwise;
using ::testing::DoubleNear;
using TestModel = std::unique_ptr<mjModel, void (*)(mjModel*)>;
using TestData = std::unique_ptr<mjData, void (*)(mjData*)>;
constexpr mjtNum kTolerance = 1e-6;
constexpr int kMaxIterations = 1000;
constexpr char kEllipsoidXml[] = R"(
@@ -66,18 +63,7 @@ constexpr char kEllipsoidXml[] = R"(
</keyframe>
</mujoco>)";
TestModel LoadModel(std::string_view xml) {
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
EXPECT_NE(model, nullptr) << "Failed to load model: " << error;
return TestModel(model, mj_deleteModel);
}
TestData MakeData(const mjModel* model) {
return TestData(mj_makeData(model), mj_deleteData);
}
mjtNum GeomDist(const TestModel& m, const TestData& d, int g1, int g2,
mjtNum GeomDist(const MjModelPtr& m, const MjDataPtr& d, int g1, int g2,
mjtNum x1[3], mjtNum x2[3], mjtNum cutoff = mjMAX_LIMIT) {
mjCCDConfig config;
mjCCDStatus status;
@@ -102,8 +88,8 @@ mjtNum GeomDist(const TestModel& m, const TestData& d, int g1, int g2,
}
int Penetration(mjCCDStatus& status, mjtNum& depth, std::vector<mjtNum>& dir,
std::vector<mjtNum>& pos, const TestModel& model,
const TestData& data, int g1, int g2, mjtNum margin = 0,
std::vector<mjtNum>& pos, const MjModelPtr& model,
const MjDataPtr& data, int g1, int g2, mjtNum margin = 0,
int max_contacts = 1) {
mjCCDObj obj1, obj2;
mjc_initCCDObj(&obj1, model.get(), data.get(), g1, margin);
@@ -180,8 +166,8 @@ TEST_F(MjGjkTest, SphereSphereDist) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -203,8 +189,8 @@ TEST_F(MjGjkTest, SphereSphereDistCutoff) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -223,8 +209,8 @@ TEST_F(MjGjkTest, SphereSphereNoDist) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -247,8 +233,8 @@ TEST_F(MjGjkTest, SphereSphereIntersect) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -284,8 +270,8 @@ TEST_F(MjGjkTest, BoxBoxDepth) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -313,8 +299,8 @@ TEST_F(MjGjkTest, BoxBoxDepth2) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat + 9;
@@ -358,8 +344,8 @@ TEST_F(MjGjkTest, BoxBoxDepth3) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat;
@@ -421,8 +407,8 @@ TEST_F(MjGjkTest, BoxBoxSize05) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat;
@@ -479,8 +465,8 @@ TEST_F(MjGjkTest, BoxBoxSize05b) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat;
@@ -540,8 +526,8 @@ TEST_F(MjGjkTest, BoxBoxSize05c) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat;
@@ -601,8 +587,8 @@ TEST_F(MjGjkTest, BoxBoxTouching) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -627,8 +613,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -661,8 +647,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD2) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -695,8 +681,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD3) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat + 9;
@@ -737,8 +723,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD4) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat;
@@ -800,8 +786,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD5) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat;
@@ -864,8 +850,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD6) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat + 9;
@@ -910,8 +896,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD7) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat;
@@ -969,8 +955,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD8) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat;
@@ -1027,8 +1013,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD9) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat;
@@ -1086,8 +1072,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD10) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
mjtNum* xpos = data->geom_xpos;
@@ -1123,8 +1109,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD11) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
mjtNum* xpos = data->geom_xpos;
@@ -1184,8 +1170,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD12) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
mjtNum* xpos = data->geom_xpos;
@@ -1243,8 +1229,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD13) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
mjtNum* xpos = data->geom_xpos;
@@ -1305,8 +1291,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD14) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
mjtNum* xpos = data->geom_xpos;
@@ -1362,8 +1348,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD15) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat;
@@ -1441,8 +1427,8 @@ TEST_F(MjGjkTest, SmallBoxMesh) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -1485,8 +1471,8 @@ TEST_F(MjGjkTest, BoxMesh) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -1515,8 +1501,8 @@ TEST_F(MjGjkTest, BoxMesh2) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -1545,8 +1531,8 @@ TEST_F(MjGjkTest, BoxMeshPrune) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -1577,8 +1563,8 @@ TEST_F(MjGjkTest, MeshMesh) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -1609,8 +1595,8 @@ TEST_F(MjGjkTest, MeshMeshPrune) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -1633,8 +1619,8 @@ TEST_F(MjGjkTest, BoxEdge) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "box1");
@@ -1657,8 +1643,8 @@ TEST_F(MjGjkTest, BoxEdge2) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat;
@@ -1715,8 +1701,8 @@ TEST_F(MjGjkTest, BoxEdgeEdge) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat;
@@ -1779,8 +1765,8 @@ TEST_F(MjGjkTest, MeshEdge) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "box1");
@@ -1819,8 +1805,8 @@ TEST_F(MjGjkTest, MeshEdge2) {
</mujoco>
)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "floor");
@@ -1835,8 +1821,8 @@ TEST_F(MjGjkTest, MeshEdge2) {
}
TEST_F(MjGjkTest, EllipsoidEllipsoidPenetrating) {
TestModel model = LoadModel(kEllipsoidXml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(kEllipsoidXml);
MjDataPtr data = MakeData(model);
mj_resetDataKeyframe(model.get(), data.get(), 0);
mj_forward(model.get(), data.get());
@@ -1861,8 +1847,8 @@ TEST_F(MjGjkTest, EllipsoidEllipsoid) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -1884,8 +1870,8 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidSlowConvergence) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat;
@@ -1943,8 +1929,8 @@ TEST_F(MjGjkTest, BoxBox) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -1963,8 +1949,8 @@ TEST_F(MjGjkTest, BoxBoxLarge) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -2013,8 +1999,8 @@ static constexpr char xml[] = R"(
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -2046,8 +2032,8 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -2071,8 +2057,8 @@ TEST_F(MjGjkTest, CapsuleCapsule) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -2101,8 +2087,8 @@ TEST_F(MjGjkTest, CylinderBoxMargin) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
// margin=0.1 means forces generated when dist<0.1
@@ -2121,8 +2107,8 @@ TEST_F(MjGjkTest, BoxEdgeFlipped) {
</worldbody>
</mujoco>)";
TestModel model = LoadModel(xml);
TestData data = MakeData(model.get());
MjModelPtr model = LoadModelFromString(xml);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
test/engine/engine_collision_sdf_test.cc
+24 -27
View File
@@ -14,14 +14,16 @@
// Tests for sdf collisions.
#include "src/engine/engine_collision_sdf.h"
#include <cstdio>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtype.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_collision_sdf.h"
#include "test/fixture.h"
namespace mujoco {
@@ -48,22 +50,24 @@ static constexpr char kSdfModel[] = R"(
TEST_F(SdfTest, SdfPrimitive) {
char error[1024];
mjModel* model = LoadModelFromString(kSdfModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
MjModelPtr model = LoadModelFromString(kSdfModel, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull());
ASSERT_THAT(model->ngeom, kgeoms);
mjSDF sdf;
const mjpPlugin* null_plugin = NULL;
mjtNum gradient[3], dist[kgeoms][kpoints] = {
{0, 0, 0, 0, 1, 1}, // plane
{-1, 0, 0, mju_sqrt(2)-1, mju_sqrt(2)-1, mju_sqrt(3)-1}, // sphere
{(mjtNum)-.1, (mjtNum).9, (mjtNum).9,
mju_sqrt(2)-(mjtNum).1, (mjtNum).9, mju_sqrt(2)-(mjtNum).1}, // capsule
{-1, 0, 0, mju_sqrt(2)-1, 0, mju_sqrt(2)-1}, // cylinder
{-mju_sqrt(3), 0, 0, 0, 0, 0}, // box
};
mjtNum gradient[3],
dist[kgeoms][kpoints] = {
{0, 0, 0, 0, 1, 1}, // plane
{-1, 0, 0, mju_sqrt(2) - 1, mju_sqrt(2) - 1,
mju_sqrt(3) - 1}, // sphere
{(mjtNum)-.1, (mjtNum).9, (mjtNum).9, mju_sqrt(2) - (mjtNum).1,
(mjtNum).9, mju_sqrt(2) - (mjtNum).1}, // capsule
{-1, 0, 0, mju_sqrt(2) - 1, 0, mju_sqrt(2) - 1}, // cylinder
{-mju_sqrt(3), 0, 0, 0, 0, 0}, // box
};
mjtNum points[kpoints][3] = {{0, 0, 0}, {1, 0, 0}, {0, 1, 0},
{1, 1, 0}, {0, 1, 1}, {1, 1, 1}};
@@ -71,16 +75,13 @@ TEST_F(SdfTest, SdfPrimitive) {
sdf.plugin = &null_plugin;
sdf.id = &i;
sdf.type = mjSDFTYPE_SINGLE;
sdf.geomtype = (mjtGeom*)(model->geom_type+i);
sdf.geomtype = (mjtGeom*)(model->geom_type + i);
for (int j = 0; j < kpoints; j++) {
EXPECT_NEAR(mjc_distance(model, data, &sdf, points[j]), dist[i][j],
MjTol(1e-9, 5e-7));
mjc_gradient(model, data, &sdf, gradient, points[j]);
EXPECT_NEAR(mjc_distance(model.get(), data.get(), &sdf, points[j]),
dist[i][j], MjTol(1e-9, 5e-7));
mjc_gradient(model.get(), data.get(), &sdf, gradient, points[j]);
}
}
mj_deleteData(data);
mj_deleteModel(model);
}
static constexpr char kFlexSdfModel[] = R"(
@@ -118,9 +119,9 @@ static constexpr char kFlexSdfModel[] = R"(
TEST_F(SdfTest, FlexSdfCollision) {
char error[1024];
mjModel* model = LoadModelFromString(kFlexSdfModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
MjModelPtr model = LoadModelFromString(kFlexSdfModel, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull());
// check there is at least one flex
@@ -128,7 +129,7 @@ TEST_F(SdfTest, FlexSdfCollision) {
// simulate for a few steps to let flex fall onto torus
for (int i = 0; i < 100; i++) {
mj_step(model, data);
mj_step(model.get(), data.get());
}
// should have contacts between flex and SDF
@@ -143,11 +144,7 @@ TEST_F(SdfTest, FlexSdfCollision) {
}
}
EXPECT_TRUE(has_flex_contact) << "Expected at least one flex contact";
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/engine/engine_core_constraint_test.cc
+132 -177
View File
@@ -14,6 +14,8 @@
// Tests for engine/engine_core_constraint.c.
#include "src/engine/engine_core_constraint.h"
#include <array>
#include <cstring>
#include <string>
@@ -23,7 +25,6 @@
#include <gtest/gtest.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_core_constraint.h"
#include "src/engine/engine_core_util.h"
#include "src/engine/engine_support.h"
#include "src/engine/engine_util_misc.h"
@@ -51,31 +52,32 @@ TEST_F(CoreConstraintTest, RestPenetration) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, testing::NotNull()) << error;
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), testing::NotNull()) << error;
mjtNum gravity = -model->opt.gravity[2];
mjtNum damping_ratio = 0.8;
mjData* data = mj_makeData(model);
MjDataPtr data = MakeData(model);
for (const mjtNum reference : {-100.0, -10.0, 0.1, 0.01}) {
for (const mjtNum impedance : {0.3, 0.9, 0.99}) {
// set solimp
for (int i=0; i < model->ngeom; i++) {
model->geom_solimp[i*mjNIMP + 0] = impedance;
model->geom_solimp[i*mjNIMP + 1] = impedance;
for (int i = 0; i < model->ngeom; i++) {
model->geom_solimp[i * mjNIMP + 0] = impedance;
model->geom_solimp[i * mjNIMP + 1] = impedance;
}
// set solref
for (int i=0; i < model->ngeom; i++) {
model->geom_solref[i*mjNREF + 0] = reference;
model->geom_solref[i*mjNREF + 1] = reference < 0 ? -10 : damping_ratio;
for (int i = 0; i < model->ngeom; i++) {
model->geom_solref[i * mjNREF + 0] = reference;
model->geom_solref[i * mjNREF + 1] =
reference < 0 ? -10 : damping_ratio;
}
// simulate for 50 seconds
mj_resetData(model, data);
mj_resetData(model.get(), data.get());
while (data->time < 50) {
mjtNum time = data->time;
mj_step(model, data);
mj_step(model.get(), data.get());
ASSERT_GT(data->time, time) << "Divergence detected";
}
@@ -91,9 +93,6 @@ TEST_F(CoreConstraintTest, RestPenetration) {
EXPECT_THAT(depth, MjNear(expected_depth, 1e-10, 1e-3));
}
}
mj_deleteData(data);
mj_deleteModel(model);
}
static const char* const kDoflessContactPath =
@@ -153,7 +152,7 @@ TEST_F(CoreConstraintTest, EqualityBodySite) {
mj_resetData(model, data);
// turn site-defined equalities off, equivalent body-defined equalities on
for (int e=0; e < 4; e++) data->eq_active[e] = 1 - data->eq_active[e];
for (int e = 0; e < 4; e++) data->eq_active[e] = 1 - data->eq_active[e];
// simulate again, get diag(A)
while (data->time < 0.1) {
@@ -220,12 +219,12 @@ TEST_F(CoreConstraintTest, ConstraintUpdateImpl) {
// iterate over islands, check match
mjtNum cost2 = 0;
for (int island=0; island < nisland; island++) {
for (int island = 0; island < nisland; island++) {
// clear outputs from data2
for (int i=0; i < nefc; i++) d2->efc_state[i] = -1;
for (int i = 0; i < nefc; i++) d2->efc_state[i] = -1;
mju_zero(d2->efc_force, nefc);
mju_zero(d2->qfrc_constraint, nv);
for (int i=0; i < d2->ncon; i++) mju_zero(d2->contact[i].H, 36);
for (int i = 0; i < d2->ncon; i++) mju_zero(d2->contact[i].H, 36);
// sizes and indices, in this island
int efcnum = d2->island_nefc[island];
@@ -237,26 +236,19 @@ TEST_F(CoreConstraintTest, ConstraintUpdateImpl) {
// update constraints for this island
mjtNum cost2i;
int ne = d2->island_ne[island];
int nf = d2->island_nf[island];
int ne = d2->island_ne[island];
int nf = d2->island_nf[island];
int adr = d2->island_iefcadr[island];
int* state = d2->iefc_state + adr;
mjtNum *force = d2->iefc_force + adr;
mj_constraintUpdate_impl(ne, nf, efcnum,
d2->iefc_D + adr,
d2->iefc_R + adr,
d2->iefc_frictionloss + adr,
jari,
d2->iefc_type + adr,
d2->iefc_id + adr,
d2->contact,
state,
force,
&cost2i,
mjtNum* force = d2->iefc_force + adr;
mj_constraintUpdate_impl(ne, nf, efcnum, d2->iefc_D + adr,
d2->iefc_R + adr, d2->iefc_frictionloss + adr,
jari, d2->iefc_type + adr, d2->iefc_id + adr,
d2->contact, state, force, &cost2i,
/*flg_coneHessian=*/1);
// compare nefc vectors
for (int c=0; c < efcnum; c++) {
for (int c = 0; c < efcnum; c++) {
int i = map2efc[c];
EXPECT_EQ(d2->efc_island[i], island);
EXPECT_EQ(state[c], d1->efc_state[i]);
@@ -265,11 +257,11 @@ TEST_F(CoreConstraintTest, ConstraintUpdateImpl) {
// compare cone Hessians
if (cone == mjCONE_ELLIPTIC) {
for (int c=0; c < d2->ncon; c++) {
for (int c = 0; c < d2->ncon; c++) {
int efcadr = d2->contact[c].efc_address;
if (d2->efc_island[efcadr] == island &&
d2->efc_state[efcadr] == mjCNSTRSTATE_CONE) {
for (int j=0; j < 36; j++) {
for (int j = 0; j < 36; j++) {
EXPECT_THAT(d2->contact[c].H[j],
MjNear(d1->contact[c].H[j], 1e-12, 1e-4));
}
@@ -308,9 +300,9 @@ TEST_F(CoreConstraintTest, FlexvertEquality) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, testing::NotNull()) << error;
mjData* data = mj_makeData(model);
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), testing::NotNull()) << error;
MjDataPtr data = MakeData(model);
ASSERT_EQ(model->neq, 1);
ASSERT_EQ(model->eq_type[0], mjEQ_FLEXVERT);
ASSERT_EQ(model->nflex, 1);
@@ -318,8 +310,8 @@ TEST_F(CoreConstraintTest, FlexvertEquality) {
ASSERT_EQ(model->flex_edgenum[0], 16);
// step1 to populate flexvert_length
mj_step1(model, data);
EXPECT_EQ(data->ne, 2*model->flex_vertnum[0]);
mj_step1(model.get(), data.get());
EXPECT_EQ(data->ne, 2 * model->flex_vertnum[0]);
EXPECT_EQ(data->nefc, 18);
for (int i = 0; i < 18; ++i) {
EXPECT_EQ(data->efc_type[i], mjCNSTR_EQUALITY);
@@ -334,9 +326,9 @@ TEST_F(CoreConstraintTest, FlexvertEquality) {
for (int i = 0; i < 3; ++i) {
mju_zero(qvel.data(), model->nv);
for (int j = 0; j < model->flex_vertnum[0]; ++j) {
qvel[3*j+i] = 1.0;
qvel[3 * j + i] = 1.0;
}
mj_mulJacVec(model, data, Jqvel.data(), qvel.data());
mj_mulJacVec(model.get(), data.get(), Jqvel.data(), qvel.data());
for (int j = 0; j < data->nefc; ++j) {
EXPECT_NEAR(Jqvel[j], 0, MjTol(1e-9, 1e-5));
}
@@ -355,14 +347,11 @@ TEST_F(CoreConstraintTest, FlexvertEquality) {
qvel[3 * j + 1] = linvel[1];
qvel[3 * j + 2] = linvel[2];
}
mj_mulJacVec(model, data, Jqvel.data(), qvel.data());
mj_mulJacVec(model.get(), data.get(), Jqvel.data(), qvel.data());
for (int j = 0; j < data->nefc; ++j) {
EXPECT_NEAR(Jqvel[j], 0, MjTol(1e-9, 1e-5));
}
}
mj_deleteData(data);
mj_deleteModel(model);
}
// Test flex strain constraint with pinned nodes attached to freejoint parent
@@ -392,12 +381,12 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) {
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m.get(), NotNull()) << error.data();
MjDataPtr d = MakeData(m);
mj_resetData(m, d);
mj_forward(m, d);
mj_resetData(m.get(), d.get());
mj_forward(m.get(), d.get());
// Check that we have constraints
EXPECT_GT(d->nefc, 0) << "No constraints generated";
@@ -425,7 +414,7 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) {
for (int i = 0; i < d->nefc; i++) {
if (d->efc_type[i] == mjCNSTR_EQUALITY) {
for (int j = 0; j < nv; j++) {
mjtNum val = d->efc_J[i*nv + j];
mjtNum val = d->efc_J[i * nv + j];
if (mju_isBad(val) || mju_abs(val) > 1e10) {
has_bad_jacobian = true;
}
@@ -449,12 +438,12 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) {
mju_copy(d->qpos, qpos0.data(), m->nq);
mjtNum dqpos[100] = {0};
dqpos[j] = eps;
mj_integratePos(m, d->qpos, dqpos, 1);
mj_forward(m, d);
mj_integratePos(m.get(), d->qpos, dqpos, 1);
mj_forward(m.get(), d.get());
for (int i = 0; i < num_constraints_to_check; i++) {
mjtNum fd = (d->efc_pos[i] - efc_pos0[i]) / eps;
mjtNum analytic = d->efc_J[i*nv + j];
mjtNum analytic = d->efc_J[i * nv + j];
// Use relative tolerance with absolute floor to handle near-zero values
mjtNum tol = mju_max(1e-8, 0.1 * (mju_abs(fd) + mju_abs(analytic)));
if (mju_abs(fd - analytic) > tol) {
@@ -467,12 +456,12 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) {
// Test rotation invariance: rotate via freejoint quaternion
mju_copy(d->qpos, qpos0.data(), m->nq);
mjtNum angle = 0.785398; // 45 degrees
d->qpos[3] = mju_cos(angle/2); // w
mjtNum angle = 0.785398; // 45 degrees
d->qpos[3] = mju_cos(angle / 2); // w
d->qpos[4] = 0;
d->qpos[5] = 0;
d->qpos[6] = mju_sin(angle/2); // z
mj_forward(m, d);
d->qpos[6] = mju_sin(angle / 2); // z
mj_forward(m.get(), d.get());
mjtNum max_strain_rotated = 0;
for (int i = 0; i < d->ne; i++) {
@@ -494,11 +483,11 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) {
mju_copy(d->qpos, qpos_rot.data(), m->nq);
mjtNum dqpos[100] = {0};
dqpos[j] = eps;
mj_integratePos(m, d->qpos, dqpos, 1);
mj_forward(m, d);
mj_integratePos(m.get(), d->qpos, dqpos, 1);
mj_forward(m.get(), d.get());
mjtNum fd = (d->efc_pos[0] - efc_pos_rot[0]) / eps;
mjtNum analytic = d->efc_J[0*nv + j];
mjtNum analytic = d->efc_J[0 * nv + j];
mjtNum tol = 0.1 * (mju_abs(fd) + mju_abs(analytic) + 1e-8);
if ((mju_abs(fd) > 1e-8 || mju_abs(analytic) > 1e-8) &&
mju_abs(fd - analytic) > tol) {
@@ -510,12 +499,12 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) {
// Reset for simulation
mju_copy(d->qpos, qpos0.data(), m->nq);
mj_forward(m, d);
mj_forward(m.get(), d.get());
// Run simulation only if checks pass
if (!has_bad_constraint && !has_bad_jacobian) {
for (int i = 0; i < 2000; i++) {
mj_step(m, d);
mj_step(m.get(), d.get());
ASSERT_FALSE(mju_isBad(d->qpos[0]))
<< "Simulation became unstable at step " << i;
@@ -529,9 +518,6 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) {
}
}
}
mj_deleteData(d);
mj_deleteModel(m);
}
// Test flex strain constraint WITHOUT pinned nodes (simpler case)
@@ -554,12 +540,12 @@ TEST_F(CoreConstraintTest, StrainConstraintNoPinning) {
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m.get(), NotNull()) << error.data();
MjDataPtr d = MakeData(m);
mj_resetData(m, d);
mj_forward(m, d);
mj_resetData(m.get(), d.get());
mj_forward(m.get(), d.get());
// Check constraints
EXPECT_GT(d->ne, 0) << "Expected strain constraints";
@@ -581,7 +567,7 @@ TEST_F(CoreConstraintTest, StrainConstraintNoPinning) {
bool has_bad_jacobian = false;
for (int i = 0; i < d->ne; i++) {
for (int j = 0; j < nv; j++) {
if (mju_isBad(d->efc_J[i*nv + j])) {
if (mju_isBad(d->efc_J[i * nv + j])) {
has_bad_jacobian = true;
}
}
@@ -592,12 +578,12 @@ TEST_F(CoreConstraintTest, StrainConstraintNoPinning) {
std::vector<mjtNum> qpos0(m->nq);
mju_copy(qpos0.data(), d->qpos, m->nq);
// Rotate by 45 degrees around Z axis via quaternion
mjtNum angle = 0.785398; // 45 degrees
d->qpos[3] = mju_cos(angle/2); // w
d->qpos[4] = 0; // x
d->qpos[5] = 0; // y
d->qpos[6] = mju_sin(angle/2); // z
mj_forward(m, d);
mjtNum angle = 0.785398; // 45 degrees
d->qpos[3] = mju_cos(angle / 2); // w
d->qpos[4] = 0; // x
d->qpos[5] = 0; // y
d->qpos[6] = mju_sin(angle / 2); // z
mj_forward(m.get(), d.get());
mjtNum max_strain_rotated = 0;
for (int i = 0; i < d->ne; i++) {
@@ -610,15 +596,12 @@ TEST_F(CoreConstraintTest, StrainConstraintNoPinning) {
// Run simulation for a few steps to check stability
mju_copy(d->qpos, qpos0.data(), m->nq);
mj_forward(m, d);
mj_forward(m.get(), d.get());
for (int i = 0; i < 100; i++) {
mj_step(m, d);
mj_step(m.get(), d.get());
ASSERT_FALSE(mju_isBad(d->qpos[0])) << "Simulation unstable at step " << i;
}
mj_deleteData(d);
mj_deleteModel(m);
}
// Test flex strain constraint with quadratic interpolation
@@ -641,12 +624,12 @@ TEST_F(CoreConstraintTest, StrainConstraintQuadratic) {
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m.get(), NotNull()) << error.data();
MjDataPtr d = MakeData(m);
mj_resetData(m, d);
mj_forward(m, d);
mj_resetData(m.get(), d.get());
mj_forward(m.get(), d.get());
// Check constraints generated
EXPECT_GT(d->ne, 0) << "Expected strain constraints";
@@ -665,7 +648,7 @@ TEST_F(CoreConstraintTest, StrainConstraintQuadratic) {
bool has_bad_jacobian = false;
for (int i = 0; i < d->ne; i++) {
for (int j = 0; j < nv; j++) {
if (mju_isBad(d->efc_J[i*nv + j])) {
if (mju_isBad(d->efc_J[i * nv + j])) {
has_bad_jacobian = true;
}
}
@@ -674,13 +657,9 @@ TEST_F(CoreConstraintTest, StrainConstraintQuadratic) {
// Run simulation for a few steps
for (int i = 0; i < 100; i++) {
mj_step(m, d);
ASSERT_FALSE(mju_isBad(d->qpos[0]))
<< "Simulation unstable at step " << i;
mj_step(m.get(), d.get());
ASSERT_FALSE(mju_isBad(d->qpos[0])) << "Simulation unstable at step " << i;
}
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(CoreConstraintTest, ShellModeBendZeroForceAtRest) {
@@ -701,11 +680,11 @@ TEST_F(CoreConstraintTest, ShellModeBendZeroForceAtRest) {
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), testing::NotNull()) << error;
MjDataPtr d = MakeData(m);
mj_forward(m, d);
mj_forward(m.get(), d.get());
// Check number of equalities
EXPECT_EQ(m->neq, 6);
@@ -719,9 +698,6 @@ TEST_F(CoreConstraintTest, ShellModeBendZeroForceAtRest) {
EXPECT_NEAR(d->efc_pos[i], 0, 1e-10)
<< "nonzero constraint residual at " << i;
}
mj_deleteData(d);
mj_deleteModel(m);
}
// Test quadratic passive forces (no constraints) for stability
@@ -741,13 +717,13 @@ TEST_F(CoreConstraintTest, QuadraticPassiveForceStability) {
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m.get(), NotNull()) << error.data();
MjDataPtr d = MakeData(m);
// Run for 500 steps — should stay stable
for (int i = 0; i < 500; i++) {
mj_step(m, d);
mj_step(m.get(), d.get());
ASSERT_FALSE(mju_isBad(d->qpos[0]))
<< "Passive quadratic unstable at step " << i;
for (int j = 0; j < m->nv; j++) {
@@ -755,9 +731,6 @@ TEST_F(CoreConstraintTest, QuadraticPassiveForceStability) {
<< "Velocity exploded at step " << i;
}
}
mj_deleteData(d);
mj_deleteModel(m);
}
// Test quadratic with anisotropic cells (like what mesh bounding box creates)
@@ -782,27 +755,24 @@ TEST_F(CoreConstraintTest, QuadraticAnisotropicStrain) {
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m.get(), NotNull()) << error.data();
MjDataPtr d = MakeData(m);
mj_forward(m, d);
mj_forward(m.get(), d.get());
EXPECT_GT(d->ne, 0) << "Expected strain constraints";
// Run for 200 steps with gravity + contact
for (int i = 0; i < 200; i++) {
mj_step(m, d);
mj_step(m.get(), d.get());
ASSERT_FALSE(mju_isBad(d->qpos[0]))
<< "Anisotropic quadratic unstable at step " << i;
for (int j = 0; j < m->nv; j++) {
ASSERT_LT(mju_abs(d->qvel[j]), 1000.0)
<< "Velocity exploded at step " << i
<< ", qvel[" << j << "]=" << d->qvel[j];
<< "Velocity exploded at step " << i << ", qvel[" << j
<< "]=" << d->qvel[j];
}
}
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(CoreConstraintTest, ContactSharedDofJacobian) {
@@ -826,21 +796,18 @@ TEST_F(CoreConstraintTest, ContactSharedDofJacobian) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
ASSERT_EQ(model->nv, 3);
ASSERT_TRUE(mj_isSparse(model));
mjData* data = mj_makeData(model);
ASSERT_TRUE(mj_isSparse(model.get()));
MjDataPtr data = MakeData(model);
mj_forward(model, data);
mj_forward(model.get(), data.get());
ASSERT_EQ(data->ncon, 1);
ASSERT_GE(data->nefc, 1);
EXPECT_EQ(data->efc_J_rownnz[0], 2);
mj_deleteData(data);
mj_deleteModel(model);
}
static const char* const kJdotvConnect2dPath =
@@ -852,10 +819,8 @@ static const char* const kJdotvWeld3dPath =
// validate mj_Jdotv against finite-differenced constraint Jacobian
TEST_F(CoreConstraintTest, JdotvFiniteDifference) {
for (const char* path : {kJdotvConnect2dPath,
kJdotvConnect3dPath,
kJdotvWeld3dPath}) {
for (const char* path :
{kJdotvConnect2dPath, kJdotvConnect3dPath, kJdotvWeld3dPath}) {
const std::string xml_path = GetTestDataFilePath(path);
char err[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, err, sizeof(err));
@@ -876,8 +841,8 @@ TEST_F(CoreConstraintTest, JdotvFiniteDifference) {
// get dense J_0 (ne x nv)
std::vector<mjtNum> J0(ne * nv);
if (mj_isSparse(m)) {
mju_sparse2dense(J0.data(), d->efc_J, ne, nv,
d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind);
mju_sparse2dense(J0.data(), d->efc_J, ne, nv, d->efc_J_rownnz,
d->efc_J_rowadr, d->efc_J_colind);
} else {
mju_copy(J0.data(), d->efc_J, ne * nv);
}
@@ -900,8 +865,8 @@ TEST_F(CoreConstraintTest, JdotvFiniteDifference) {
ASSERT_EQ(d->ne, ne) << "constraint count changed after integration";
std::vector<mjtNum> Jh(ne * nv);
if (mj_isSparse(m)) {
mju_sparse2dense(Jh.data(), d->efc_J, ne, nv,
d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind);
mju_sparse2dense(Jh.data(), d->efc_J, ne, nv, d->efc_J_rownnz,
d->efc_J_rowadr, d->efc_J_colind);
} else {
mju_copy(Jh.data(), d->efc_J, ne * nv);
}
@@ -911,7 +876,7 @@ TEST_F(CoreConstraintTest, JdotvFiniteDifference) {
std::vector<mjtNum> jdv_fd(ne, 0);
for (int i = 0; i < ne; i++) {
for (int j = 0; j < nv; j++) {
jdv_fd[i] -= (Jh[i*nv+j] - J0[i*nv+j]) / h * qvel0[j];
jdv_fd[i] -= (Jh[i * nv + j] - J0[i * nv + j]) / h * qvel0[j];
}
}
@@ -927,9 +892,8 @@ TEST_F(CoreConstraintTest, JdotvFiniteDifference) {
// Test 2: forward-inverse identity preserved with Jdot*v correction
TEST_F(CoreConstraintTest, JdotvFwdInvIdentity) {
for (const char* path : {kJdotvConnect2dPath,
kJdotvConnect3dPath,
kJdotvWeld3dPath}) {
for (const char* path :
{kJdotvConnect2dPath, kJdotvConnect3dPath, kJdotvWeld3dPath}) {
const std::string xml_path = GetTestDataFilePath(path);
char err[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, err, sizeof(err));
@@ -946,8 +910,7 @@ TEST_F(CoreConstraintTest, JdotvFwdInvIdentity) {
mjtNum epsilon = MjTol(1e-10, 1e-2);
EXPECT_LT(fwdinv, epsilon)
<< "fwdinv broken for " << path
<< " (fwdinv=" << fwdinv << ")";
<< "fwdinv broken for " << path << " (fwdinv=" << fwdinv << ")";
mj_deleteData(d);
mj_deleteModel(m);
@@ -964,10 +927,9 @@ struct StrainConstraintTestCase {
std::string flex_xyaxes;
};
class StrainConstraintRotatedTest : public CoreConstraintTest,
public ::testing::WithParamInterface<
StrainConstraintTestCase> {
};
class StrainConstraintRotatedTest
: public CoreConstraintTest,
public ::testing::WithParamInterface<StrainConstraintTestCase> {};
TEST_P(StrainConstraintRotatedTest, ResidualIsZero) {
auto param = GetParam();
@@ -1006,11 +968,11 @@ TEST_P(StrainConstraintRotatedTest, ResidualIsZero) {
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml.c_str(), error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(xml.c_str(), error.data(), error.size());
ASSERT_THAT(m.get(), NotNull()) << error.data();
MjDataPtr d = MakeData(m);
mj_forward(m, d);
mj_forward(m.get(), d.get());
// Check we have strain constraints
EXPECT_GT(d->ne, 0) << "Expected strain constraints";
@@ -1021,24 +983,19 @@ TEST_P(StrainConstraintRotatedTest, ResidualIsZero) {
for (int i = 0; i < d->ne; i++) {
max_pos = mju_max(max_pos, mju_abs(d->efc_pos[i]));
}
EXPECT_LT(max_pos, 1e-6)
<< "Strain constraint residual should be ~0"
<< " (max_pos=" << max_pos << ")";
EXPECT_LT(max_pos, 1e-6) << "Strain constraint residual should be ~0"
<< " (max_pos=" << max_pos << ")";
// Verify stability
for (int i = 0; i < 200; i++) {
mj_step(m, d);
ASSERT_FALSE(mju_isBad(d->qpos[0]))
<< "Simulation unstable at step " << i;
mj_step(m.get(), d.get());
ASSERT_FALSE(mju_isBad(d->qpos[0])) << "Simulation unstable at step " << i;
for (int j = 0; j < m->nv; j++) {
ASSERT_LT(mju_abs(d->qvel[j]), 1000.0)
<< "Velocity exploded at step " << i
<< ", qvel[" << j << "]=" << d->qvel[j];
<< "Velocity exploded at step " << i << ", qvel[" << j
<< "]=" << d->qvel[j];
}
}
mj_deleteData(d);
mj_deleteModel(m);
}
INSTANTIATE_TEST_SUITE_P(
@@ -1106,11 +1063,11 @@ TEST_F(CoreConstraintTest, ShellModeContactJacobian) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, testing::NotNull()) << error;
mjData* data = mj_makeData(model);
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), testing::NotNull()) << error;
MjDataPtr data = MakeData(model);
mj_forward(model, data);
mj_forward(model.get(), data.get());
// find central vertex index (13 for 3x3x3 grid)
int central_idx = 13;
@@ -1121,7 +1078,8 @@ TEST_F(CoreConstraintTest, ShellModeContactJacobian) {
int rest = central_idx % (nx * ny);
int j = rest / nx;
int i = rest % nx;
ASSERT_TRUE(i > 0 && i < nx-1 && j > 0 && j < ny-1 && k > 0 && k < nz-1);
ASSERT_TRUE(i > 0 && i < nx - 1 && j > 0 && j < ny - 1 && k > 0 &&
k < nz - 1);
// create manual contact with central vertex
mjContact con;
@@ -1135,21 +1093,21 @@ TEST_F(CoreConstraintTest, ShellModeContactJacobian) {
con.flex[1] = 0;
con.vert[1] = central_idx;
con.dim = 1;
mju_copy3(con.pos, data->flexvert_xpos + 3*central_idx);
con.frame[0] = 0; con.frame[1] = 0; con.frame[2] = 1; // normal
mju_copy3(con.pos, data->flexvert_xpos + 3 * central_idx);
con.frame[0] = 0;
con.frame[1] = 0;
con.frame[2] = 1; // normal
// buffer for Jacobian
std::vector<mjtNum> jacdif(3*model->nv, 0.0);
std::vector<mjtNum> jacdif(3 * model->nv, 0.0);
// call mj_contactJacobian
mj_contactJacobian(model, data, &con, 1, nullptr, jacdif.data(),
nullptr, nullptr, nullptr,
nullptr, nullptr, nullptr,
mj_contactJacobian(model.get(), data.get(), &con, 1, nullptr, jacdif.data(),
nullptr, nullptr, nullptr, nullptr, nullptr, nullptr,
nullptr);
// check that boundary nodes have non-zero entries, and central node has zero
bool boundary_has_dof = false;
bool interior_has_dof = false;
@@ -1185,9 +1143,6 @@ TEST_F(CoreConstraintTest, ShellModeContactJacobian) {
<< "Boundary nodes should receive contact force";
EXPECT_FALSE(interior_has_dof)
<< "Interior nodes should not receive contact force";
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
test/engine/engine_core_smooth_test.cc
+165 -197
View File
@@ -15,8 +15,6 @@
// Tests for engine/engine_core_smooth.c.
#include "src/engine/engine_core_smooth.h"
#include "src/engine/engine_util_misc.h"
#include "src/engine/engine_util_sparse.h"
#include <algorithm>
#include <string>
@@ -30,6 +28,8 @@
#include <mujoco/mjspec.h>
#include <mujoco/mjxmacro.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_util_misc.h"
#include "src/engine/engine_util_sparse.h"
#include "test/fixture.h"
namespace mujoco {
@@ -44,7 +44,6 @@ using ::testing::NotNull;
using ::testing::Pointwise;
using CoreSmoothTest = MujocoTest;
constexpr bool EndsWith(std::string_view str, std::string_view suffix) {
return str.size() >= suffix.size() &&
str.substr(str.size() - suffix.size()) == suffix;
@@ -64,31 +63,28 @@ TEST_F(CoreSmoothTest, MjDataWorldBodyValuesAreInitialized) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
mj_resetDataDebug(model, data, 'd');
mj_forward(model, data);
mj_rnePostConstraint(model, data);
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mj_resetDataDebug(model.get(), data.get(), 'd');
mj_forward(model.get(), data.get());
mj_rnePostConstraint(model.get(), data.get());
{
#define X(type, name, d0, d1) \
if constexpr (std::string_view(#d0) == "nbody") { \
absl::Span<type> values(data->name, model->d0 * d1); \
if constexpr (EndsWith(#name, "quat")) { \
EXPECT_THAT(values, ElementsAre(1, 0, 0, 0)) << #name; \
} else if constexpr (EndsWith(#name, "mat")) { \
EXPECT_THAT(values, ElementsAre(1, 0, 0, 0, 1, 0, 0, 0, 1)) << #name; \
} else if constexpr (std::string_view(#type) == "mjtNum") { \
EXPECT_THAT(values, Each(MjNear(0.0, 1e-7, 1e-7))) << #name; \
} \
}
#define X(type, name, d0, d1) \
if constexpr (std::string_view(#d0) == "nbody") { \
absl::Span<type> values(data->name, model->d0 * d1); \
if constexpr (EndsWith(#name, "quat")) { \
EXPECT_THAT(values, ElementsAre(1, 0, 0, 0)) << #name; \
} else if constexpr (EndsWith(#name, "mat")) { \
EXPECT_THAT(values, ElementsAre(1, 0, 0, 0, 1, 0, 0, 0, 1)) << #name; \
} else if constexpr (std::string_view(#type) == "mjtNum") { \
EXPECT_THAT(values, Each(MjNear(0.0, 1e-7, 1e-7))) << #name; \
} \
}
MJDATA_POINTERS
#undef X
}
mj_deleteData(data);
mj_deleteModel(model);
}
// --------------------------- mj_kinematics -----------------------------------
@@ -101,16 +97,13 @@ TEST_F(CoreSmoothTest, MjKinematicsWorldXipos) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mj_resetDataDebug(model, data, 'd');
mj_kinematics(model, data);
mj_resetDataDebug(model.get(), data.get(), 'd');
mj_kinematics(model.get(), data.get());
EXPECT_THAT(AsVector(&data->xipos[0], 3), ElementsAre(0, 0, 0));
mj_deleteData(data);
mj_deleteModel(model);
}
// ----------------------------- mj_tendon -------------------------------------
@@ -145,19 +138,16 @@ TEST_F(CoreSmoothTest, FixedTendonSortedIndices) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
ASSERT_EQ(model->ntendon, 1);
ASSERT_EQ(model->nwrap, 3);
mjData* data = mj_makeData(model);
mj_fwdPosition(model, data);
MjDataPtr data = MakeData(model);
mj_fwdPosition(model.get(), data.get());
mjtNum* J = data->ten_J;
EXPECT_THAT(vector<mjtNum>(J, J + 3), ElementsAre(1, 2, 3));
mj_deleteData(data);
mj_deleteModel(model);
}
static const char* const kTen_J0 = "engine/testdata/core_smooth/ten_J0.xml";
@@ -232,33 +222,33 @@ TEST_F(CoreSmoothTest, TendonArmature) {
mj_forward(m, d);
// get full M, includes both CRB and tendon inertia
vector<mjtNum> M(nv*nv);
vector<mjtNum> M(nv * nv);
mj_fullM(m, d, M.data());
// put only CRB inertia in M2
mj_crb(m, d);
mju_scatter(d->qM, d->M, m->mapM2M, m->nC);
vector<mjtNum> M2(nv*nv);
vector<mjtNum> M2(nv * nv);
mj_fullM(m, d, M2.data());
vector<mjtNum> ten_J(nv); // tendon Jacobian
vector<mjtNum> ten_M(nv*nv); // tendon inertia
vector<mjtNum> ten_J(nv); // tendon Jacobian
vector<mjtNum> ten_M(nv * nv); // tendon inertia
// add tendon inertias to M2 using outer product
for (int j=0; j < m->ntendon; j++) {
for (int j = 0; j < m->ntendon; j++) {
// get tendon Jacobian
int rowadr = m->ten_J_rowadr[j];
int* rownnz = m->ten_J_rownnz + j;
int zero = 0;
mju_sparse2dense(ten_J.data(), d->ten_J + rowadr, 1, nv,
rownnz, &zero, m->ten_J_colind + rowadr);
mju_sparse2dense(ten_J.data(), d->ten_J + rowadr, 1, nv, rownnz, &zero,
m->ten_J_colind + rowadr);
// get tendon inertia only, using outer product
mju_mulMatMat(ten_M.data(), ten_J.data(), ten_J.data(), nv, 1, nv);
mju_scl(ten_M.data(), ten_M.data(), m->tendon_armature[j], nv * nv);
// manually add values, at nonzeros only
for (int i=0; i < nv*nv; i++) {
for (int i = 0; i < nv * nv; i++) {
if (M[i]) M2[i] += ten_M[i];
}
}
@@ -304,7 +294,7 @@ TEST_F(CoreSmoothTest, TendonArmatureConservesEnergy) {
mj_step(m, d);
ASSERT_GT(d->time, time) << "Divergence detected";
mjtNum energy_t = d->energy[0] + d->energy[1];
EXPECT_NEAR(energy_t, energy_0, MjTol(eps, 3*eps));
EXPECT_NEAR(energy_t, energy_0, MjTol(eps, 3 * eps));
}
}
mj_deleteData(d);
@@ -378,13 +368,13 @@ TEST_F(CoreSmoothTest, TendonInertiaEquivalent) {
mjtNum time = d->time;
mj_step(m, d);
ASSERT_GT(d->time, time) << "Divergence detected";
vector<mjtNum> xpos = AsVector(d->geom_xpos + 3*gid, 3);
vector<mjtNum> xpos = AsVector(d->geom_xpos + 3 * gid, 3);
time = d_e->time;
mj_step(m_e, d_e);
ASSERT_GT(d_e->time, time) << "Divergence detected";
vector<mjtNum> xpos_e = AsVector(d_e->geom_xpos + 3*gid_e, 3);
EXPECT_THAT(xpos, Pointwise(MjNear(eps, 10*eps), xpos_e));
vector<mjtNum> xpos_e = AsVector(d_e->geom_xpos + 3 * gid_e, 3);
EXPECT_THAT(xpos, Pointwise(MjNear(eps, 10 * eps), xpos_e));
}
mj_deleteData(d);
mj_deleteModel(m);
@@ -393,20 +383,18 @@ TEST_F(CoreSmoothTest, TendonInertiaEquivalent) {
}
}
// --------------------------- connect constraint ------------------------------
// test that bodies hanging on connects lead to expected force sensor readings
void TestConnect(const char* const filepath) {
const std::string xml_path = GetTestDataFilePath(filepath);
mjModel* model =
mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
mjData* data = mj_makeData(model);
// settle physics:
for (int i=0; i < 1000; i++) {
for (int i = 0; i < 1000; i++) {
mj_step(model, data);
}
for (int i=0; i < 3; i++) {
for (int i = 0; i < 3; i++) {
EXPECT_THAT(data->sensordata[i] - model->sensor_user[i],
MjNear(0, 1e-6, 2e-4));
}
@@ -414,116 +402,100 @@ void TestConnect(const char* const filepath) {
mj_deleteModel(model);
}
TEST_F(CoreSmoothTest, RnePostConnectForceSlide) {
constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/connect/force_slide.xml";
TestConnect(kModelFilePath);
}
TEST_F(CoreSmoothTest, RnePostConnectForceSlideRotated) {
constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/connect/force_slide_rotated.xml";
TestConnect(kModelFilePath);
}
TEST_F(CoreSmoothTest, RnePostConnectForceFree) {
constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/connect/force_free.xml";
TestConnect(kModelFilePath);
}
TEST_F(CoreSmoothTest, RnePostConnectTorque) {
constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/connect/torque_free.xml";
TestConnect(kModelFilePath);
}
TEST_F(CoreSmoothTest, RnePostConnectMultipleConstraints) {
constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/connect/multiple_constraints.xml";
TestConnect(kModelFilePath);
}
// --------------------------- weld constraint ---------------------------------
// test that bodies attached with welds lead to expected force sensor readings
void TestWeld(const char* const filepath) {
const std::string xml_path = GetTestDataFilePath(filepath);
mjModel* model =
mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
mjData* data = mj_makeData(model);
// settle physics:
for (int i=0; i < 1000; i++) {
for (int i = 0; i < 1000; i++) {
mj_step(model, data);
}
for (int sensor_index=0; sensor_index < model->nsensor; sensor_index++) {
for (int i=0; i < 3; i++) {
EXPECT_NEAR(
data->sensordata[model->sensor_adr[sensor_index] + i],
model->sensor_user[model->nuser_sensor*sensor_index + i],
MjTol(1e-6, 5e-5));
for (int sensor_index = 0; sensor_index < model->nsensor; sensor_index++) {
for (int i = 0; i < 3; i++) {
EXPECT_NEAR(data->sensordata[model->sensor_adr[sensor_index] + i],
model->sensor_user[model->nuser_sensor * sensor_index + i],
MjTol(1e-6, 5e-5));
}
}
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(CoreSmoothTest, RnePostWeldForceFree) {
constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/weld/force_free.xml";
TestWeld(kModelFilePath);
}
TEST_F(CoreSmoothTest, RnePostWeldForceFreeRotated) {
constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/weld/force_free_rotated.xml";
TestWeld(kModelFilePath);
}
TEST_F(CoreSmoothTest, RnePostWeldForceTorqueFree) {
constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/weld/force_torque_free.xml";
TestWeld(kModelFilePath);
}
TEST_F(CoreSmoothTest, RnePostWeldForceTorqueFreeRotated) {
constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/weld/force_torque_free_rotated.xml";
TestWeld(kModelFilePath);
}
TEST_F(CoreSmoothTest, WeldRatioForceFree) {
constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/weld/tfratio0_force_free.xml";
TestConnect(kModelFilePath);
}
TEST_F(CoreSmoothTest, WeldRatioForceSlide) {
constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide.xml";
TestConnect(kModelFilePath);
}
TEST_F(CoreSmoothTest, WeldRatioTorqueFree) {
constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/weld/tfratio0_torque_free.xml";
TestConnect(kModelFilePath);
}
TEST_F(CoreSmoothTest, WeldRatioForceSlideRotated) {
constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/weld/"
@@ -555,7 +527,7 @@ TEST_F(CoreSmoothTest, EqualityBodySite) {
mj_resetData(model, data);
// turn site-defined equalities off, equivalent body-defined equalities on
for (int e=0; e < 4; e++) data->eq_active[e] = 1 - data->eq_active[e];
for (int e = 0; e < 4; e++) data->eq_active[e] = 1 - data->eq_active[e];
// simulate again, get sensordata
while (data->time < 0.1) {
@@ -586,7 +558,7 @@ TEST_F(CoreSmoothTest, RefsiteBringsToPose) {
mjtNum targetpos[] = {.01, .02, .03};
mjtNum targetrot[] = {.1, .2, .3};
mju_copy3(data->ctrl, targetpos);
mju_copy3(data->ctrl+3, targetrot);
mju_copy3(data->ctrl + 3, targetrot);
// step for 5 seconds
while (data->time < 10) {
@@ -601,13 +573,14 @@ TEST_F(CoreSmoothTest, RefsiteBringsToPose) {
// check that position matches target to within 1e-3 length units
mjtNum relpos[3];
mju_sub3(relpos, data->site_xpos+3*site_id, data->site_xpos+3*refsite_id);
mju_sub3(relpos, data->site_xpos + 3 * site_id,
data->site_xpos + 3 * refsite_id);
EXPECT_THAT(relpos, Pointwise(MjNear(1e-3, 5e-3), targetpos));
// check that orientation matches target to within 0.06 radians
mjtNum site_xquat[4], refsite_xquat[4], relrot[3];
mju_mat2Quat(refsite_xquat, data->site_xmat+9*refsite_id);
mju_mat2Quat(site_xquat, data->site_xmat+9*site_id);
mju_mat2Quat(refsite_xquat, data->site_xmat + 9 * refsite_id);
mju_mat2Quat(site_xquat, data->site_xmat + 9 * site_id);
mju_subQuat(relrot, site_xquat, refsite_xquat);
EXPECT_THAT(relrot, Pointwise(MjNear(0.06, 0.06), targetrot));
@@ -632,7 +605,7 @@ TEST_F(CoreSmoothTest, RefsiteConservesMomentum) {
mjtNum time = data->time;
mj_step(model, data);
ASSERT_GT(data->time, time) << "Divergence detected";
for (int i=0; i < 6; i++) {
for (int i = 0; i < 6; i++) {
EXPECT_LT(mju_abs(data->sensordata[i]), eps);
}
}
@@ -654,33 +627,33 @@ TEST_F(CoreSmoothTest, FactorI) {
// dense L matrix
int nv = model->nv;
vector<mjtNum> Ldense(nv*nv, 0);
mju_sparse2dense(Ldense.data(), data->qLD, nv, nv,
model->M_rownnz, model->M_rowadr, model->M_colind);
for (int i=0; i < nv; i++) {
vector<mjtNum> Ldense(nv * nv, 0);
mju_sparse2dense(Ldense.data(), data->qLD, nv, nv, model->M_rownnz,
model->M_rowadr, model->M_colind);
for (int i = 0; i < nv; i++) {
// set diagonal to 1
Ldense[i*nv+i] = 1;
Ldense[i * nv + i] = 1;
}
// dense D matrix
vector<mjtNum> Ddense(nv*nv);
mju_sparse2dense(Ddense.data(), data->qLD, nv, nv,
model->M_rownnz, model->M_rowadr, model->M_colind);
for (int i=0; i < nv; i++) {
for (int j=0; j < nv; j++) {
vector<mjtNum> Ddense(nv * nv);
mju_sparse2dense(Ddense.data(), data->qLD, nv, nv, model->M_rownnz,
model->M_rowadr, model->M_colind);
for (int i = 0; i < nv; i++) {
for (int j = 0; j < nv; j++) {
// zero everything except the diagonal
if (i != j) Ddense[i*nv+j] = 0;
if (i != j) Ddense[i * nv + j] = 0;
}
}
// perform multiplication: M = L^T * D * L
vector<mjtNum> tmp(nv*nv);
vector<mjtNum> M(nv*nv);
vector<mjtNum> tmp(nv * nv);
vector<mjtNum> M(nv * nv);
mju_mulMatMat(tmp.data(), Ddense.data(), Ldense.data(), nv, nv, nv);
mju_mulMatTMat(M.data(), Ldense.data(), tmp.data(), nv, nv, nv);
// dense M matrix
vector<mjtNum> Mexpected(nv*nv);
vector<mjtNum> Mexpected(nv * nv);
mj_fullM(model, data, Mexpected.data());
// expect matrices to match to floating point precision
@@ -693,12 +666,12 @@ TEST_F(CoreSmoothTest, FactorI) {
// Convert legacy-format symmetric matrix to dense (local helper for tests).
static void legacyToDense(const mjModel* m, mjtNum* dst, const mjtNum* M) {
int adr = 0, nv = m->nv;
mju_zero(dst, nv*nv);
mju_zero(dst, nv * nv);
for (int i = 0; i < nv; i++) {
int j = i;
while (j >= 0) {
dst[i*nv+j] = M[adr];
dst[j*nv+i] = M[adr];
dst[i * nv + j] = M[adr];
dst[j * nv + i] = M[adr];
j = m->dof_parentid[j];
adr++;
}
@@ -723,31 +696,32 @@ TEST_F(CoreSmoothTest, SolveLDs) {
mju_scatter(LDlegacy.data(), d->qLD, m->mapM2M, nC);
// compare LD and LDs densified matrices
vector<mjtNum> LDdense(nv*nv);
mju_sparse2dense(LDdense.data(), d->qLD, nv, nv,
m->M_rownnz, m->M_rowadr, m->M_colind);
vector<mjtNum> LDdense2(nv*nv);
vector<mjtNum> LDdense(nv * nv);
mju_sparse2dense(LDdense.data(), d->qLD, nv, nv, m->M_rownnz, m->M_rowadr,
m->M_colind);
vector<mjtNum> LDdense2(nv * nv);
legacyToDense(m, LDdense2.data(), LDlegacy.data());
// expect lower triangles to match exactly
for (int i=0; i < nv; i++) {
for (int j=0; j < i; j++) {
EXPECT_NEAR(LDdense[i*nv+j], LDdense2[i*nv+j], MjTol(1e-14, 1e-6));
for (int i = 0; i < nv; i++) {
for (int j = 0; j < i; j++) {
EXPECT_NEAR(LDdense[i * nv + j], LDdense2[i * nv + j],
MjTol(1e-14, 1e-6));
}
}
// compare legacy and CSR LD vector solve
vector<mjtNum> vec(nv);
vector<mjtNum> vec2(nv);
for (int i=0; i < nv; i++) vec[i] = vec2[i] = 20 + 30*i;
for (int i=0; i < nv; i+=2) vec[i] = vec2[i] = 0;
for (int i = 0; i < nv; i++) vec[i] = vec2[i] = 20 + 30 * i;
for (int i = 0; i < nv; i += 2) vec[i] = vec2[i] = 0;
mj_solveLD_legacy(m, vec.data(), 1, LDlegacy.data(), d->qLDiagInv);
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, 1,
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, 1, m->M_rownnz, m->M_rowadr,
m->M_colind, nullptr);
// expect vectors to match up to floating point precision
for (int i=0; i < nv; i++) {
for (int i = 0; i < nv; i++) {
EXPECT_NEAR(vec[i], vec2[i], MjTol(1e-14, 5e-6));
}
@@ -772,17 +746,17 @@ TEST_F(CoreSmoothTest, SolveLDmultipleVectors) {
// compare n LD and LDs vector solve
int n = 3;
vector<mjtNum> vec(nv*n);
vector<mjtNum> vec2(nv*n);
for (int i=0; i < nv*n; i++) vec[i] = vec2[i] = 2 + 3*i;
for (int i=0; i < nv*n; i+=3) vec[i] = vec2[i] = 0;
vector<mjtNum> vec(nv * n);
vector<mjtNum> vec2(nv * n);
for (int i = 0; i < nv * n; i++) vec[i] = vec2[i] = 2 + 3 * i;
for (int i = 0; i < nv * n; i += 3) vec[i] = vec2[i] = 0;
mj_solveLD_legacy(m, vec.data(), n, LDlegacy.data(), d->qLDiagInv);
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n,
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n, m->M_rownnz, m->M_rowadr,
m->M_colind, nullptr);
// expect vectors to match up to floating point precision
for (int i=0; i < nv*n; i++) {
for (int i = 0; i < nv * n; i++) {
EXPECT_NEAR(vec[i], vec2[i], MjTol(1e-14, 5e-6));
}
@@ -802,27 +776,27 @@ TEST_F(CoreSmoothTest, SolveM2) {
// inverse square root of D from inertia LDL decomposition
int nv = m->nv;
vector<mjtNum> sqrtInvD(nv);
for (int i=0; i < nv; i++) {
for (int i = 0; i < nv; i++) {
int diag = m->M_rowadr[i] + m->M_rownnz[i] - 1;
sqrtInvD[i] = 1 / mju_sqrt(d->qLD[diag]);
}
// compare full solve and half solve
int n = 3;
vector<mjtNum> vec(nv*n);
vector<mjtNum> vec2(nv*n);
for (int i=0; i < nv*n; i++) vec[i] = vec2[i] = 2 + 3*i;
for (int i=0; i < nv*n; i+=3) vec[i] = vec2[i] = 0;
vector<mjtNum> res(nv*n);
vector<mjtNum> vec(nv * n);
vector<mjtNum> vec2(nv * n);
for (int i = 0; i < nv * n; i++) vec[i] = vec2[i] = 2 + 3 * i;
for (int i = 0; i < nv * n; i += 3) vec[i] = vec2[i] = 0;
vector<mjtNum> res(nv * n);
mj_solveM2(m, d, res.data(), vec.data(), sqrtInvD.data(), n);
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n,
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n, m->M_rownnz, m->M_rowadr,
m->M_colind, nullptr);
// expect equality of dot(v, M^-1 * v) and dot(M^-1/2 * v, M^-1/2 * v)
for (int i=0; i < n; i++) {
EXPECT_NEAR(mju_dot(vec2.data() + i*nv, vec.data() + i*nv, nv),
mju_dot(res.data() + i*nv, res.data() + i*nv, nv),
for (int i = 0; i < n; i++) {
EXPECT_NEAR(mju_dot(vec2.data() + i * nv, vec.data() + i * nv, nv),
mju_dot(res.data() + i * nv, res.data() + i * nv, nv),
MjTol(1e-10, 1e-2));
}
@@ -856,8 +830,8 @@ TEST_F(CoreSmoothTest, FactorIs) {
vector<mjtNum> qLDiagInvExpected(d->qLDiagInv, d->qLDiagInv + nv);
vector<mjtNum> qLDiagInv(nv, 0);
mj_factorI(qLD.data(), qLDiagInv.data(), nv,
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr);
mj_factorI(qLD.data(), qLDiagInv.data(), nv, m->M_rownnz, m->M_rowadr,
m->M_colind, nullptr);
// expect outputs to match to floating point precision
EXPECT_THAT(qLD, Pointwise(MjNear(1e-12, 1e-4), qLDexpected));
@@ -891,9 +865,9 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
// check that nJfv is correct:
// corner vertices: 2 * (1+3) * 3 + 2 * (1+2) * 3 = 42
@@ -903,33 +877,33 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
EXPECT_EQ(m->nJfv, 123);
// Run kinematics to populate xpos/xmat initially
mj_fwdKinematics(m, d);
mj_fwdKinematics(m.get(), d.get());
// Check invariants for scale=1
// The constraints should be satisfied
int nvert = m->flex_vertnum[0];
ASSERT_EQ(nvert, 9);
for (int i=0; i < nvert; i++) {
EXPECT_NEAR(d->flexvert_length[2*i+0], 0.0, MjTol(1e-5, 5e-5));
EXPECT_NEAR(d->flexvert_length[2*i+1], 0.0, MjTol(1e-5, 5e-5));
for (int i = 0; i < nvert; i++) {
EXPECT_NEAR(d->flexvert_length[2 * i + 0], 0.0, MjTol(1e-5, 5e-5));
EXPECT_NEAR(d->flexvert_length[2 * i + 1], 0.0, MjTol(1e-5, 5e-5));
}
// set qvel to rigid rotation
ASSERT_EQ(m->nv, 3*nvert);
ASSERT_EQ(m->nv, 3 * nvert);
mju_zero(d->qvel, m->nv);
for (int i=0; i < nvert; i++) {
const mjtNum* p = d->xpos + 3*m->flex_vertbodyid[i];
d->qvel[3*i+0] = -p[1];
d->qvel[3*i+1] = p[0];
d->qvel[3*i+2] = 1.0;
for (int i = 0; i < nvert; i++) {
const mjtNum* p = d->xpos + 3 * m->flex_vertbodyid[i];
d->qvel[3 * i + 0] = -p[1];
d->qvel[3 * i + 1] = p[0];
d->qvel[3 * i + 2] = 1.0;
}
// check that Jacobian times velocity is zero for rigid body motion
vector<mjtNum> Jv(2*nvert, 0);
for (int i=0; i < 2*nvert; i++) {
vector<mjtNum> Jv(2 * nvert, 0);
for (int i = 0; i < 2 * nvert; i++) {
int row_start = m->flexvert_J_rowadr[i];
int row_nnz = m->flexvert_J_rownnz[i];
for (int j=0; j < row_nnz; j++) {
for (int j = 0; j < row_nnz; j++) {
Jv[i] += d->flexvert_J[row_start + j] *
d->qvel[m->flexvert_J_colind[row_start + j]];
}
@@ -941,37 +915,37 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
int edges[] = {1, 3, 5, 7};
int center[] = {4};
for (int i : corners) {
EXPECT_EQ(m->flexvert_J_rownnz[2*i+0], (i == 0 || i == 8) ? 12 : 9);
EXPECT_EQ(m->flexvert_J_rownnz[2*i+1], (i == 0 || i == 8) ? 12 : 9);
EXPECT_EQ(m->flexvert_J_rownnz[2 * i + 0], (i == 0 || i == 8) ? 12 : 9);
EXPECT_EQ(m->flexvert_J_rownnz[2 * i + 1], (i == 0 || i == 8) ? 12 : 9);
}
for (int i : edges) {
EXPECT_EQ(m->flexvert_J_rownnz[2*i+0], 15);
EXPECT_EQ(m->flexvert_J_rownnz[2*i+1], 15);
EXPECT_EQ(m->flexvert_J_rownnz[2 * i + 0], 15);
EXPECT_EQ(m->flexvert_J_rownnz[2 * i + 1], 15);
}
for (int i : center) {
EXPECT_EQ(m->flexvert_J_rownnz[2*i+0], 21);
EXPECT_EQ(m->flexvert_J_rownnz[2*i+1], 21);
EXPECT_EQ(m->flexvert_J_rownnz[2 * i + 0], 21);
EXPECT_EQ(m->flexvert_J_rownnz[2 * i + 1], 21);
}
// check rowadr
EXPECT_EQ(m->flexvert_J_rowadr[0], 0);
for (int i=1; i < 2*nvert; i++) {
for (int i = 1; i < 2 * nvert; i++) {
EXPECT_EQ(m->flexvert_J_rowadr[i],
m->flexvert_J_rowadr[i-1] + m->flexvert_J_rownnz[i-1]);
m->flexvert_J_rowadr[i - 1] + m->flexvert_J_rownnz[i - 1]);
}
// check that colind are sorted and unique
int nnzJ = 0;
for (int i = 0; i < 2*nvert; i++) {
for (int i = 0; i < 2 * nvert; i++) {
nnzJ += m->flexvert_J_rownnz[i];
}
EXPECT_EQ(nnzJ, 2*m->nJfv);
for (int i=0; i < 2*nvert; i++) {
EXPECT_EQ(nnzJ, 2 * m->nJfv);
for (int i = 0; i < 2 * nvert; i++) {
int row_start = m->flexvert_J_rowadr[i];
int row_nnz = m->flexvert_J_rownnz[i];
for (int j=0; j < row_nnz-1; j++) {
EXPECT_LE(m->flexvert_J_colind[row_start+j],
m->flexvert_J_colind[row_start+j+1]);
for (int j = 0; j < row_nnz - 1; j++) {
EXPECT_LE(m->flexvert_J_colind[row_start + j],
m->flexvert_J_colind[row_start + j + 1]);
}
}
@@ -979,8 +953,8 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
auto fd_check = [&](double tolerance) {
std::vector<mjtNum> qpos0(m->nq);
mju_copy(qpos0.data(), d->qpos, m->nq);
mj_kinematics(m, d);
mj_flex(m, d);
mj_kinematics(m.get(), d.get());
mj_flex(m.get(), d.get());
mjtNum eps = MjTol(1e-6, 1e-4);
int nflexvert = m->flex_vertnum[0];
@@ -988,15 +962,15 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
std::vector<mjtNum> qpos_backup(m->nq);
mju_copy(qpos_backup.data(), d->qpos, m->nq);
for (int i=0; i < m->nv; ++i) {
for (int i = 0; i < m->nv; ++i) {
std::vector<mjtNum> qvel(m->nv, 0);
qvel[i] = 1.0;
// plus
mju_copy(d->qpos, qpos_backup.data(), m->nq);
mj_integratePos(m, d->qpos, qvel.data(), eps);
mj_kinematics(m, d);
mj_flex(m, d);
mj_integratePos(m.get(), d->qpos, qvel.data(), eps);
mj_kinematics(m.get(), d.get());
mj_flex(m.get(), d.get());
std::vector<mjtNum> L_plus(2 * nflexvert);
for (int e = 0; e < 2 * nflexvert; ++e) {
L_plus[e] = d->flexvert_length[e];
@@ -1004,21 +978,21 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
// minus
mju_copy(d->qpos, qpos_backup.data(), m->nq);
mj_integratePos(m, d->qpos, qvel.data(), -eps);
mj_kinematics(m, d);
mj_flex(m, d);
mj_integratePos(m.get(), d->qpos, qvel.data(), -eps);
mj_kinematics(m.get(), d.get());
mj_flex(m.get(), d.get());
std::vector<mjtNum> L_minus(2 * nflexvert);
for (int e = 0; e < 2 * nflexvert; ++e) {
L_minus[e] = d->flexvert_length[e];
}
for (int e = 0; e < 2 * nflexvert; ++e) {
jac_fd[e*m->nv + i] = (L_plus[e] - L_minus[e]) / (2*eps);
jac_fd[e * m->nv + i] = (L_plus[e] - L_minus[e]) / (2 * eps);
}
}
mju_copy(d->qpos, qpos_backup.data(), m->nq);
mj_kinematics(m, d);
mj_flex(m, d);
mj_kinematics(m.get(), d.get());
mj_flex(m.get(), d.get());
// Compare with analytic
std::vector<mjtNum> jac_analytic(2 * nflexvert * m->nv);
@@ -1027,27 +1001,27 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
int row_start = m->flexvert_J_rowadr[e];
int row_nnz = m->flexvert_J_rownnz[e];
for (int i = 0; i < row_nnz; ++i) {
jac_analytic[e*m->nv + m->flexvert_J_colind[row_start+i]] =
d->flexvert_J[row_start+i];
jac_analytic[e * m->nv + m->flexvert_J_colind[row_start + i]] =
d->flexvert_J[row_start + i];
}
}
EXPECT_THAT(jac_analytic, Not(Each(MjNear(0.0, 1e-7, 1e-4))));
EXPECT_THAT(jac_analytic, Pointwise(MjNear(tolerance, 1e-1), jac_fd));
mju_copy(d->qpos, qpos0.data(), m->nq);
mj_kinematics(m, d);
mj_flex(m, d);
mj_kinematics(m.get(), d.get());
mj_flex(m.get(), d.get());
};
fd_check(MjTol(5e-5, 5e-2));
// Set qpos to put flex in scale=2 configuration.
for (int i=0; i < nvert; i++) {
d->qpos[3*i+0] = d->xpos[3*(i+1)+0];
d->qpos[3*i+1] = d->xpos[3*(i+1)+1];
d->qpos[3*i+2] = d->xpos[3*(i+1)+2];
for (int i = 0; i < nvert; i++) {
d->qpos[3 * i + 0] = d->xpos[3 * (i + 1) + 0];
d->qpos[3 * i + 1] = d->xpos[3 * (i + 1) + 1];
d->qpos[3 * i + 2] = d->xpos[3 * (i + 1) + 2];
}
mj_fwdKinematics(m, d);
mj_fwdKinematics(m.get(), d.get());
// Get mass scaling factor
mjtNum scale = 1.0;
@@ -1062,19 +1036,16 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
// Invariant 0: Trace(E) = 3 + 3 = 6
// Invariant 1: Det(C) - 1 = 4 * 4 - 1 = 15
// Note: constraints are now scaled by sqrt(mass)
for (int i=0; i < nvert; i++) {
for (int i = 0; i < nvert; i++) {
EXPECT_NEAR(d->flexvert_length[2 * i + 0], 6.0 * scale, MjTol(1e-5, 5e-4));
EXPECT_NEAR(d->flexvert_length[2 * i + 1], 15.0 * scale, MjTol(1e-5, 5e-4));
}
// Perturb z-positions so configuration is not flat
for (int i=0; i < nvert; i++) {
d->qpos[3*i+2] += 0.01 * (i%2 ? 1 : -1);
for (int i = 0; i < nvert; i++) {
d->qpos[3 * i + 2] += 0.01 * (i % 2 ? 1 : -1);
}
fd_check(5e-5);
mj_deleteData(d);
mj_deleteModel(m);
}
// Test failure case for flexvert_J sparsity with skipped flexes
@@ -1107,12 +1078,12 @@ TEST_F(CoreSmoothTest, FlexvertJSparsitySkippedFlex) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// Forward dynamics to compute Jacobians
mj_forward(model, data);
mj_forward(model.get(), data.get());
// Check sparsity overlap
// Flex 0 starts at row 0
@@ -1136,9 +1107,6 @@ TEST_F(CoreSmoothTest, FlexvertJSparsitySkippedFlex) {
// Verify that Flex 2 starts AFTER Flex 0 ends
EXPECT_GE(f2_start_adr, f0_end_adr)
<< "Flex 2 Jacobian overwrites Flex 0 Jacobian due to skipped Flex 1";
mj_deleteData(data);
mj_deleteModel(model);
}
// Test stability of flexvert constraint under different integrator/solver
test/engine/engine_core_util_test.cc
+171 -192
View File
@@ -49,10 +49,10 @@ TEST_F(FlexGatherStateTest, mju_flexGatherState_Grid) {
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
mj_forward(model, data);
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
ASSERT_EQ(model->nflex, 1);
int f = 0;
@@ -63,29 +63,26 @@ TEST_F(FlexGatherStateTest, mju_flexGatherState_Grid) {
ASSERT_TRUE(model->flex_centered[f]);
for (int i = 0; i < nodenum; i++) {
int b = model->flex_nodebodyid[nstart + i];
mjtNum x = data->xpos[3*b + 0];
mjtNum y = data->xpos[3*b + 1];
mjtNum z = data->xpos[3*b + 2];
mjtNum x = data->xpos[3 * b + 0];
mjtNum y = data->xpos[3 * b + 1];
mjtNum z = data->xpos[3 * b + 2];
// Rotate 90 degrees around Z: (x, y, z) -> (-y, x, z)
data->xpos[3*b + 0] = -y;
data->xpos[3*b + 1] = x;
data->xpos[3*b + 2] = z;
data->xpos[3 * b + 0] = -y;
data->xpos[3 * b + 1] = x;
data->xpos[3 * b + 2] = z;
}
std::vector<mjtNum> xpos(3 * nodenum);
mju_flexGatherState(model, data, f, xpos.data(), NULL);
mju_flexGatherState(model.get(), data.get(), f, xpos.data(), NULL);
// Verify that gathered xpos matches the rotated data->xpos
for (int i = 0; i < nodenum; i++) {
int b = model->flex_nodebodyid[nstart + i];
EXPECT_NEAR(xpos[3*i + 0], data->xpos[3*b + 0], 1e-5);
EXPECT_NEAR(xpos[3*i + 1], data->xpos[3*b + 1], 1e-5);
EXPECT_NEAR(xpos[3*i + 2], data->xpos[3*b + 2], 1e-5);
EXPECT_NEAR(xpos[3 * i + 0], data->xpos[3 * b + 0], 1e-5);
EXPECT_NEAR(xpos[3 * i + 1], data->xpos[3 * b + 1], 1e-5);
EXPECT_NEAR(xpos[3 * i + 2], data->xpos[3 * b + 2], 1e-5);
}
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(FlexGatherStateTest, mju_flexGatherState_ShellMode) {
@@ -102,15 +99,15 @@ TEST_F(FlexGatherStateTest, mju_flexGatherState_ShellMode) {
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
ASSERT_EQ(model->nflex, 1);
int f = 0;
model->flex_interp[f] = -1;
mjData* data = mj_makeData(model);
mj_forward(model, data);
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
int nodenum = model->flex_nodenum[f];
int nstart = model->flex_nodeadr[f];
@@ -118,26 +115,23 @@ TEST_F(FlexGatherStateTest, mju_flexGatherState_ShellMode) {
// Move boundary nodes, keep interior node stuck (it is pinned)
mjtNum shift[3] = {0.1, 0.2, 0.3};
for (int i = 0; i < nodenum; i++) {
if (i == 13) continue; // Skip center node
if (i == 13) continue; // Skip center node
int b = model->flex_nodebodyid[nstart + i];
data->xpos[3*b + 0] += shift[0];
data->xpos[3*b + 1] += shift[1];
data->xpos[3*b + 2] += shift[2];
data->xpos[3 * b + 0] += shift[0];
data->xpos[3 * b + 1] += shift[1];
data->xpos[3 * b + 2] += shift[2];
}
std::vector<mjtNum> xpos(3 * nodenum);
mju_flexGatherState(model, data, f, xpos.data(), NULL);
mju_flexGatherState(model.get(), data.get(), f, xpos.data(), NULL);
// Verify that gathered xpos for center node (13) is the TFI reconstructed position
EXPECT_NEAR(xpos[3*13 + 0], shift[0], 1e-5);
EXPECT_NEAR(xpos[3*13 + 1], shift[1], 1e-5);
EXPECT_NEAR(xpos[3*13 + 2], shift[2], 1e-5);
mj_deleteData(data);
mj_deleteModel(model);
// Verify that gathered xpos for center node (13) is the TFI reconstructed
// position
EXPECT_NEAR(xpos[3 * 13 + 0], shift[0], 1e-5);
EXPECT_NEAR(xpos[3 * 13 + 1], shift[1], 1e-5);
EXPECT_NEAR(xpos[3 * 13 + 2], shift[2], 1e-5);
}
using AngMomMatTest = MujocoTest;
static constexpr char AngMomTestingModel[] = R"(
@@ -172,26 +166,26 @@ static constexpr char AngMomTestingModel[] = R"(
// compare subtree angular momentum computed in two ways
TEST_F(AngMomMatTest, CompareAngMom) {
char error[1024];
mjModel* model =
MjModelPtr model =
LoadModelFromString(AngMomTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
ASSERT_THAT(model.get(), NotNull()) << error;
int nv = model->nv;
int bodyid = mj_name2id(model, mjOBJ_BODY, "link1");
int bodyid = mj_name2id(model.get(), mjOBJ_BODY, "link1");
mjData* data = mj_makeData(model);
MjDataPtr data = MakeData(model);
// reset to the keyframe with some angular velocities
mj_resetDataKeyframe(model, data, 0);
mj_forward(model, data);
mj_resetDataKeyframe(model.get(), data.get(), 0);
mj_forward(model.get(), data.get());
// get the reference value of angular momentum
mj_subtreeVel(model, data);
mj_subtreeVel(model.get(), data.get());
mjtNum angmom_ref[3];
mju_copy3(angmom_ref, data->subtree_angmom+3*bodyid);
mju_copy3(angmom_ref, data->subtree_angmom + 3 * bodyid);
// compute angular momentum using the angular momentum matrix
mjtNum* angmom_mat = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv);
mj_angmomMat(model, data, angmom_mat, bodyid);
mjtNum* angmom_mat = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv);
mj_angmomMat(model.get(), data.get(), angmom_mat, bodyid);
mjtNum angmom_test[3];
mju_mulMatVec(angmom_test, angmom_mat, data->qvel, 3, nv);
@@ -201,28 +195,26 @@ TEST_F(AngMomMatTest, CompareAngMom) {
}
mju_free(angmom_mat);
mj_deleteData(data);
mj_deleteModel(model);
}
// compare subtree angular momentum matrix: analytical and findiff
TEST_F(AngMomMatTest, CompareAngMomMats) {
char error[1024];
mjModel* model =
MjModelPtr model =
LoadModelFromString(AngMomTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
ASSERT_THAT(model.get(), NotNull()) << error;
int nv = model->nv;
int bodyid = mj_name2id(model, mjOBJ_BODY, "link1");
mjData* data = mj_makeData(model);
mjtNum* angmom_mat = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv);
mjtNum* angmom_mat_fd = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv);
int bodyid = mj_name2id(model.get(), mjOBJ_BODY, "link1");
MjDataPtr data = MakeData(model);
mjtNum* angmom_mat = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv);
mjtNum* angmom_mat_fd = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv);
// reset to the keyframe with some angular velocities
mj_resetDataKeyframe(model, data, 0);
mj_forward(model, data);
mj_resetDataKeyframe(model.get(), data.get(), 0);
mj_forward(model.get(), data.get());
// compute the angular momentum matrix using the analytical method
mj_angmomMat(model, data, angmom_mat, bodyid);
mj_angmomMat(model.get(), data.get(), angmom_mat, bodyid);
// compute the angular momentum matrix using finite differences
static const mjtNum eps = MjTol(1e-6, 1e-3);
@@ -230,34 +222,32 @@ TEST_F(AngMomMatTest, CompareAngMomMats) {
// reset vel, forward nudge i-th dof, get angmom
mju_copy(data->qvel, model->key_qvel, model->nv);
data->qvel[i] += eps;
mj_forward(model, data);
mj_subtreeVel(model, data);
mj_forward(model.get(), data.get());
mj_subtreeVel(model.get(), data.get());
mjtNum agmf[3];
mju_copy3(agmf, data->subtree_angmom+3*bodyid);
mju_copy3(agmf, data->subtree_angmom + 3 * bodyid);
// reset vel, backward nudge i-th dof, get angmom
mju_copy(data->qvel, model->key_qvel, model->nv);
data->qvel[i] -= eps;
mj_forward(model, data);
mj_subtreeVel(model, data);
mj_forward(model.get(), data.get());
mj_subtreeVel(model.get(), data.get());
mjtNum agmb[3];
mju_copy3(agmb, data->subtree_angmom+3*bodyid);
mju_copy3(agmb, data->subtree_angmom + 3 * bodyid);
// finite-difference the angmom matrix
for (int j = 0; j < 3; j++) {
angmom_mat_fd[nv*j+i] = (agmf[j] - agmb[j]) / (2 * eps);
angmom_mat_fd[nv * j + i] = (agmf[j] - agmb[j]) / (2 * eps);
}
}
// compare the two matrices
for (int i = 0; i < 3*nv; i++) {
for (int i = 0; i < 3 * nv; i++) {
EXPECT_THAT(angmom_mat_fd[i], MjNear(angmom_mat[i], 1e-8, 2e-4));
}
mju_free(angmom_mat_fd);
mju_free(angmom_mat);
mj_deleteData(data);
mj_deleteModel(model);
}
using JacobianTest = MujocoTest;
@@ -297,100 +287,97 @@ static constexpr char kJacobianTestingModel[] = R"(
// compare analytic and finite-differenced subtree-com Jacobian
TEST_F(JacobianTest, SubtreeJac) {
char error[1024];
mjModel* model =
MjModelPtr model =
LoadModelFromString(kJacobianTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
ASSERT_THAT(model.get(), NotNull()) << error;
int nv = model->nv;
int bodyid = mj_name2id(model, mjOBJ_BODY, "main");
mjData* data = mj_makeData(model);
mjtNum* jac_subtree = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv);
mjtNum* qpos = (mjtNum*) mju_malloc(sizeof(mjtNum)*model->nq);
mjtNum* nudge = (mjtNum*) mju_malloc(sizeof(mjtNum)*nv);
int bodyid = mj_name2id(model.get(), mjOBJ_BODY, "main");
MjDataPtr data = MakeData(model);
mjtNum* jac_subtree = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv);
mjtNum* qpos = (mjtNum*)mju_malloc(sizeof(mjtNum) * model->nq);
mjtNum* nudge = (mjtNum*)mju_malloc(sizeof(mjtNum) * nv);
// all we need for Jacobians are kinematics and CoM-related quantities
mj_kinematics(model, data);
mj_comPos(model, data);
mj_kinematics(model.get(), data.get());
mj_comPos(model.get(), data.get());
// get subtree CoM Jacobian of free body
mj_jacSubtreeCom(model, data, jac_subtree, bodyid);
mj_jacSubtreeCom(model.get(), data.get(), jac_subtree, bodyid);
// save current subtree-com and qpos, clear nudge
mjtNum subtree_com[3];
mju_copy3(subtree_com, data->subtree_com+3*bodyid);
mju_copy3(subtree_com, data->subtree_com + 3 * bodyid);
mju_copy(qpos, data->qpos, model->nq);
mju_zero(nudge, nv);
// compare analytic Jacobian to finite-difference approximation
static const mjtNum eps = 1e-6;
for (int i=0; i < nv; i++) {
for (int i = 0; i < nv; i++) {
// reset qpos, nudge i-th dof, update data->qpos, reset nudge
mju_copy(data->qpos, qpos, model->nq);
nudge[i] = 1;
mj_integratePos(model, data->qpos, nudge, eps);
mj_integratePos(model.get(), data->qpos, nudge, eps);
nudge[i] = 0;
// kinematics and comPos to get nudged com
mj_kinematics(model, data);
mj_comPos(model, data);
mj_kinematics(model.get(), data.get());
mj_comPos(model.get(), data.get());
// compare finite-differenced and analytic Jacobian
for (int j=0; j < 3; j++) {
mjtNum findiff = (data->subtree_com[3*bodyid+j] - subtree_com[j]) / eps;
EXPECT_THAT(jac_subtree[nv*j+i], MjNear(findiff, eps, 1e-2));
for (int j = 0; j < 3; j++) {
mjtNum findiff =
(data->subtree_com[3 * bodyid + j] - subtree_com[j]) / eps;
EXPECT_THAT(jac_subtree[nv * j + i], MjNear(findiff, eps, 1e-2));
}
}
mju_free(nudge);
mju_free(qpos);
mju_free(jac_subtree);
mj_deleteData(data);
mj_deleteModel(model);
}
// confirm that applying linear forces via the subtree-com Jacobian only creates
// the expected linear accelerations (no accelerations of internal joints)
TEST_F(JacobianTest, SubtreeJacNoInternalAcc) {
char error[1024];
mjModel* model =
MjModelPtr model =
LoadModelFromString(kJacobianTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
ASSERT_THAT(model.get(), NotNull()) << error;
int nv = model->nv;
int bodyid = mj_name2id(model, mjOBJ_BODY, "main");
mjData* data = mj_makeData(model);
mjtNum* jac_subtree = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv);
int bodyid = mj_name2id(model.get(), mjOBJ_BODY, "main");
MjDataPtr data = MakeData(model);
mjtNum* jac_subtree = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv);
// all we need for Jacobians are kinematics and CoM-related quantities
mj_kinematics(model, data);
mj_comPos(model, data);
mj_kinematics(model.get(), data.get());
mj_comPos(model.get(), data.get());
// get subtree CoM Jacobian of free body
mj_jacSubtreeCom(model, data, jac_subtree, bodyid);
mj_jacSubtreeCom(model.get(), data.get(), jac_subtree, bodyid);
// uncomment for debugging
// mju_printMat(jac_subtree, 3, nv);
// call fwdPosition since we'll need the factorised mass matrix in the test
mj_fwdPosition(model, data);
mj_fwdPosition(model.get(), data.get());
// treating the subtree Jacobian as the projection of 3 axis-aligned unit
// forces into joint space, solve for the resulting accelerations in-place
mj_solveM(model, data, jac_subtree, jac_subtree, 3);
mj_solveM(model.get(), data.get(), jac_subtree, jac_subtree, 3);
// expect to find accelerations of magnitude 1/subtreemass in the first 3
// coordinates of the free joint and 0s elsewhere, since applying forces to
// the CoM should accelerate the whole mechanism without any internal motion
int body_dofadr = model->body_dofadr[bodyid];
mjtNum invtreemass = 1.0/model->body_subtreemass[bodyid];
mjtNum invtreemass = 1.0 / model->body_subtreemass[bodyid];
for (int r = 0; r < 3; r++) {
for (int c = 0; c < nv; c++) {
mjtNum expected = c - body_dofadr == r ? invtreemass : 0.0;
EXPECT_THAT(jac_subtree[nv*r+c], MjNear(expected, max_abs_err, 1e-4));
EXPECT_THAT(jac_subtree[nv * r + c], MjNear(expected, max_abs_err, 1e-4));
}
}
mju_free(jac_subtree);
mj_deleteData(data);
mj_deleteModel(model);
}
static constexpr char kQuat[] = R"(
@@ -519,65 +506,65 @@ static constexpr char kHinge[] = R"(
TEST_F(JacobianTest, JacDot) {
for (auto xml : {kHinge, kQuat, kTelescope, kFreeBall, kQuatlessPendulum}) {
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
int nv = model->nv;
mjData* data = mj_makeData(model);
MjDataPtr data = MakeData(model);
// load keyframe if present, step for a bit
if (model->nkey) mj_resetDataKeyframe(model, data, 0);
if (model->nkey) mj_resetDataKeyframe(model.get(), data.get(), 0);
while (data->time < 0.1) {
mj_step(model, data);
mj_step(model.get(), data.get());
}
// minimal call required for mj_jacDot outputs to be valid
mj_kinematics(model, data);
mj_comPos(model, data);
mj_comVel(model, data);
mj_kinematics(model.get(), data.get());
mj_comPos(model.get(), data.get());
mj_comVel(model.get(), data.get());
// get bodyid
int bodyid = mj_name2id(model, mjOBJ_BODY, "query");
int bodyid = mj_name2id(model.get(), mjOBJ_BODY, "query");
EXPECT_GT(bodyid, 0);
// get site position
int siteid = mj_name2id(model, mjOBJ_SITE, "query");
int siteid = mj_name2id(model.get(), mjOBJ_SITE, "query");
EXPECT_GT(siteid, -1);
mjtNum point[3];
mju_copy3(point, data->site_xpos+3*siteid);
mju_copy3(point, data->site_xpos + 3 * siteid);
// jac, jac_dot
std::vector<mjtNum> jacp(3*nv);
std::vector<mjtNum> jacr(3*nv);
mj_jac(model, data, jacp.data(), jacr.data(), point, bodyid);
std::vector<mjtNum> jacp_dot(3*nv);
std::vector<mjtNum> jacr_dot(3*nv);
mj_jacDot(model, data, jacp_dot.data(), jacr_dot.data(), point, bodyid);
std::vector<mjtNum> jacp(3 * nv);
std::vector<mjtNum> jacr(3 * nv);
mj_jac(model.get(), data.get(), jacp.data(), jacr.data(), point, bodyid);
std::vector<mjtNum> jacp_dot(3 * nv);
std::vector<mjtNum> jacr_dot(3 * nv);
mj_jacDot(model.get(), data.get(), jacp_dot.data(), jacr_dot.data(), point,
bodyid);
// jac_h: jacobian after integrating qpos with a timestep of h
const mjtNum h = MjTol(1e-7, 5e-4);
mj_integratePos(model, data->qpos, data->qvel, h);
mj_kinematics(model, data);
mj_comPos(model, data);
std::vector<mjtNum> jacp_h(3*nv);
std::vector<mjtNum> jacr_h(3*nv);
mju_copy3(point, data->site_xpos+3*siteid); // get updated site position
mj_jac(model, data, jacp_h.data(), jacr_h.data(), point, bodyid);
mj_integratePos(model.get(), data->qpos, data->qvel, h);
mj_kinematics(model.get(), data.get());
mj_comPos(model.get(), data.get());
std::vector<mjtNum> jacp_h(3 * nv);
std::vector<mjtNum> jacr_h(3 * nv);
mju_copy3(point,
data->site_xpos + 3 * siteid); // get updated site position
mj_jac(model.get(), data.get(), jacp_h.data(), jacr_h.data(), point,
bodyid);
// jac_dot_h finite-difference approximation
std::vector<mjtNum> jacp_dot_h(3*nv);
mju_sub(jacp_dot_h.data(), jacp_h.data(), jacp.data(), 3*nv);
mju_scl(jacp_dot_h.data(), jacp_dot_h.data(), 1/h, 3*nv);
std::vector<mjtNum> jacr_dot_h(3*nv);
mju_sub(jacr_dot_h.data(), jacr_h.data(), jacr.data(), 3*nv);
mju_scl(jacr_dot_h.data(), jacr_dot_h.data(), 1/h, 3*nv);
std::vector<mjtNum> jacp_dot_h(3 * nv);
mju_sub(jacp_dot_h.data(), jacp_h.data(), jacp.data(), 3 * nv);
mju_scl(jacp_dot_h.data(), jacp_dot_h.data(), 1 / h, 3 * nv);
std::vector<mjtNum> jacr_dot_h(3 * nv);
mju_sub(jacr_dot_h.data(), jacr_h.data(), jacr.data(), 3 * nv);
mju_scl(jacr_dot_h.data(), jacr_dot_h.data(), 1 / h, 3 * nv);
// compare finite-differenced and analytic
mjtNum tol = 1e-5;
EXPECT_THAT(jacp_dot, Pointwise(MjNear(tol, 5e-2), jacp_dot_h));
EXPECT_THAT(jacr_dot, Pointwise(MjNear(tol, 5e-2), jacr_dot_h));
mj_deleteData(data);
mj_deleteModel(model);
}
}
@@ -585,34 +572,35 @@ TEST_F(JacobianTest, JacDot) {
TEST_F(JacobianTest, JacDotSparse) {
for (auto xml : {kHinge, kQuat, kTelescope, kFreeBall, kQuatlessPendulum}) {
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
int nv = model->nv;
mjData* data = mj_makeData(model);
MjDataPtr data = MakeData(model);
// load keyframe if present, step for a bit
if (model->nkey) mj_resetDataKeyframe(model, data, 0);
if (model->nkey) mj_resetDataKeyframe(model.get(), data.get(), 0);
while (data->time < 0.1) {
mj_step(model, data);
mj_step(model.get(), data.get());
}
// minimal call required for mj_jacDot outputs to be valid
mj_kinematics(model, data);
mj_comPos(model, data);
mj_comVel(model, data);
mj_kinematics(model.get(), data.get());
mj_comPos(model.get(), data.get());
mj_comVel(model.get(), data.get());
// get bodyid and site position
int bodyid = mj_name2id(model, mjOBJ_BODY, "query");
int bodyid = mj_name2id(model.get(), mjOBJ_BODY, "query");
EXPECT_GT(bodyid, 0);
int siteid = mj_name2id(model, mjOBJ_SITE, "query");
int siteid = mj_name2id(model.get(), mjOBJ_SITE, "query");
EXPECT_GT(siteid, -1);
mjtNum point[3];
mju_copy3(point, data->site_xpos+3*siteid);
mju_copy3(point, data->site_xpos + 3 * siteid);
// dense jacDot
std::vector<mjtNum> jacp_dense(3*nv);
std::vector<mjtNum> jacr_dense(3*nv);
mj_jacDot(model, data, jacp_dense.data(), jacr_dense.data(), point, bodyid);
std::vector<mjtNum> jacp_dense(3 * nv);
std::vector<mjtNum> jacr_dense(3 * nv);
mj_jacDot(model.get(), data.get(), jacp_dense.data(), jacr_dense.data(),
point, bodyid);
// compute body chain using public mjModel fields
std::vector<int> chain(nv);
@@ -629,32 +617,28 @@ TEST_F(JacobianTest, JacDotSparse) {
EXPECT_GT(NV, 0);
// sparse jacDot
std::vector<mjtNum> jacp_sparse(3*NV);
std::vector<mjtNum> jacr_sparse(3*NV);
mj_jacDotSparse(model, data, jacp_sparse.data(), jacr_sparse.data(),
point, bodyid, NV, chain.data());
std::vector<mjtNum> jacp_sparse(3 * NV);
std::vector<mjtNum> jacr_sparse(3 * NV);
mj_jacDotSparse(model.get(), data.get(), jacp_sparse.data(),
jacr_sparse.data(), point, bodyid, NV, chain.data());
// expand sparse to dense and compare
std::vector<mjtNum> jacp_expanded(3*nv, 0);
std::vector<mjtNum> jacr_expanded(3*nv, 0);
std::vector<mjtNum> jacp_expanded(3 * nv, 0);
std::vector<mjtNum> jacr_expanded(3 * nv, 0);
for (int ci = 0; ci < NV; ci++) {
int di = chain[ci];
for (int r = 0; r < 3; r++) {
jacp_expanded[di+r*nv] = jacp_sparse[ci+r*NV];
jacr_expanded[di+r*nv] = jacr_sparse[ci+r*NV];
jacp_expanded[di + r * nv] = jacp_sparse[ci + r * NV];
jacr_expanded[di + r * nv] = jacr_sparse[ci + r * NV];
}
}
// expect bitwise equality
EXPECT_EQ(jacp_expanded, jacp_dense);
EXPECT_EQ(jacr_expanded, jacr_dense);
mj_deleteData(data);
mj_deleteModel(model);
}
}
// validate rotational Jacobian used in welds
TEST_F(JacobianTest, WeldRotJacobian) {
#ifdef mjUSESINGLE
@@ -678,23 +662,23 @@ TEST_F(JacobianTest, WeldRotJacobian) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, testing::NotNull()) << error;
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), testing::NotNull()) << error;
ASSERT_EQ(model->nq, 7);
ASSERT_EQ(model->nv, 6);
static const int nv = 6; // for increased readability
mjData* data = mj_makeData(model);
MjDataPtr data = MakeData(model);
// arbitrary initial values for the ball and hinge joints
mjtNum qpos0[7] = {.5, .5, .5, .5, .7, .8, .9};
// compute required quantities using mj_step1
mj_step1(model, data);
mj_step1(model.get(), data.get());
// get orientation error
mjtNum res[3];
// compute rotation residual following formula in mj_instantiateEquality
auto RotationResidual = [](const mjModel *model, mjData *data,
auto RotationResidual = [](const mjModel* model, mjData* data,
const mjtNum qpos[7], const mjtNum dqpos[6],
mjtNum res[3]) {
// copy configuration, compute required quantities with mj_step1
@@ -710,80 +694,75 @@ TEST_F(JacobianTest, WeldRotJacobian) {
// compute orientation residual
mjtNum quat1[4], quat2[4], quat3[4];
mju_copy4(quat1, data->xquat+4*1);
mju_negQuat(quat2, data->xquat+4*2);
mju_copy4(quat1, data->xquat + 4 * 1);
mju_negQuat(quat2, data->xquat + 4 * 2);
mju_mulQuat(quat3, quat2, quat1);
mju_copy3(res, quat3+1);
mju_copy3(res, quat3 + 1);
};
RotationResidual(model, data, qpos0, NULL, res);
RotationResidual(model.get(), data.get(), qpos0, NULL, res);
// compute Jacobian with finite-differencing
mjtNum jacFD[3*nv];
mjtNum jacFD[3 * nv];
mjtNum dqpos[nv] = {0};
mjtNum dres[3];
const mjtNum eps = 1e-6;
for (int i=0; i < nv; i++) {
for (int i = 0; i < nv; i++) {
// nudge i-th dof
dqpos[i] = eps;
// get nudged residual
RotationResidual(model, data, qpos0, dqpos, dres);
RotationResidual(model.get(), data.get(), qpos0, dqpos, dres);
// remove nudge
dqpos[i] = 0.0;
// compute Jacobian column
for (int j=0; j < 3; j++) {
jacFD[nv*j + i] = (dres[j] - res[j]) / eps;
for (int j = 0; j < 3; j++) {
jacFD[nv * j + i] = (dres[j] - res[j]) / eps;
}
}
// reset mjData to qpos0
mju_copy(data->qpos, qpos0, 7);
mj_step1(model, data);
mj_step1(model.get(), data.get());
// intermediate quaternions quat1 and quat2
mjtNum quat1[4], negQuat2[4];
mju_copy4(quat1, data->xquat+4*1);
mju_negQuat(negQuat2, data->xquat+4*2);
mju_copy4(quat1, data->xquat + 4 * 1);
mju_negQuat(negQuat2, data->xquat + 4 * 2);
// get analytical Jacobian following formula in mj_instantiateEquality
mjtNum jacdif[3*nv], jac0[3*nv], jac1[3*nv];
mjtNum jacdif[3 * nv], jac0[3 * nv], jac1[3 * nv];
mjtNum point[3] = {0};
// rotational Jacobian difference
mj_jacDifPair(model, data, NULL, 2, 1, point, point,
NULL, NULL, NULL, jac0, jac1, jacdif, mj_isSparse(model),
mj_jacDifPair(model.get(), data.get(), NULL, 2, 1, point, point, NULL, NULL,
NULL, jac0, jac1, jacdif, mj_isSparse(model.get()),
/*flg_skipcommon=*/0);
// formula: 0.5 * neg(quat2) * (jac1-jac2) * quat1
mjtNum axis[3], quat3[4], quat4[4];
for (int j=0; j < nv; j++) {
for (int j = 0; j < nv; j++) {
// axis = [jac1-jac2]_col(j)
axis[0] = jacdif[0*nv+j];
axis[1] = jacdif[1*nv+j];
axis[2] = jacdif[2*nv+j];
axis[0] = jacdif[0 * nv + j];
axis[1] = jacdif[1 * nv + j];
axis[2] = jacdif[2 * nv + j];
// apply formula
mju_mulQuatAxis(quat3, negQuat2, axis);
mju_mulQuat(quat4, quat3, quat1);
// correct Jacobian
jacdif[0*nv+j] = 0.5*quat4[1];
jacdif[1*nv+j] = 0.5*quat4[2];
jacdif[2*nv+j] = 0.5*quat4[3];
jacdif[0 * nv + j] = 0.5 * quat4[1];
jacdif[1 * nv + j] = 0.5 * quat4[2];
jacdif[2 * nv + j] = 0.5 * quat4[3];
}
// test that analytical and finite-differenced Jacobians match
EXPECT_THAT(AsVector(jacFD, 3*nv),
Pointwise(MjNear(eps, 1e-3), AsVector(jacdif, 3*nv)));
mj_deleteData(data);
mj_deleteModel(model);
EXPECT_THAT(AsVector(jacFD, 3 * nv),
Pointwise(MjNear(eps, 1e-3), AsVector(jacdif, 3 * nv)));
}
} // namespace
} // namespace mujoco
test/engine/engine_derivative_test.cc
+257 -311
View File
File diff suppressed because it is too large Load Diff
test/engine/engine_forward_test.cc
+539 -746
View File
File diff suppressed because it is too large Load Diff
test/engine/engine_io_test.cc
+188 -220
View File
@@ -15,7 +15,6 @@
// Tests for engine/{engine_io.c and engine_memory.c}.
#include "src/engine/engine_io.h"
#include "src/engine/engine_memory.h"
#include <array>
#include <cstdint>
@@ -26,11 +25,12 @@
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <gtest/gtest-spi.h> // IWYU pragma: keep
#include <gtest/gtest.h>
#include <absl/strings/str_format.h>
#include <mujoco/mjxmacro.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_memory.h"
#include "src/engine/engine_thread.h"
#include "test/fixture.h"
@@ -57,19 +57,19 @@ TEST_F(EngineIoTest, VerifySizeModel) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
std::filesystem::path temp_file = (
std::filesystem::temp_directory_path() / "model.mjb");
std::filesystem::path temp_file =
(std::filesystem::temp_directory_path() / "model.mjb");
mj_saveModel(model, temp_file.string().c_str(), NULL, 0);
mj_saveModel(model.get(), temp_file.string().c_str(), NULL, 0);
std::uintmax_t file_size = std::filesystem::file_size(temp_file);
int model_size = mj_sizeModel(model);
int model_size = mj_sizeModel(model.get());
std::filesystem::remove(temp_file);
mj_deleteModel(model);
EXPECT_EQ(file_size, model_size);
}
@@ -87,15 +87,13 @@ TEST_F(EngineIoTest, MakeDataLoadsQpos0) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
model->qpos0[0] = 1;
mjData* data = mj_makeData(model);
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull());
EXPECT_EQ(data->qpos[0], 1);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(EngineIoTest, MakeDataLoadsMocapBodies) {
@@ -110,43 +108,40 @@ TEST_F(EngineIoTest, MakeDataLoadsMocapBodies) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull());
EXPECT_EQ(data->mocap_pos[0], 42);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(EngineIoTest, MakeDataReturnsNullOnFailure) {
constexpr char xml[] = "<mujoco/>";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
// fail mj_makeData intentionally with a bad size
model->nbody = -1;
MockWarningHandler warning_handler;
warning_handler.ExpectWarnings();
mjData* data = mj_makeData(model);
MjDataPtr data = MakeData(model);
EXPECT_THAT(data, IsNull());
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(EngineIoTest, ResetVariableSizes) {
constexpr char xml[] = "<mujoco/>";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
ASSERT_THAT(model, NotNull()) << "Failed to create mjData";
MjDataPtr data = MakeData(model);
ASSERT_THAT(model.get(), NotNull()) << "Failed to create mjData";
// don't call mj_forward, vars should be reset
EXPECT_EQ(data->ne, 0);
@@ -155,25 +150,23 @@ TEST_F(EngineIoTest, ResetVariableSizes) {
EXPECT_EQ(data->nJ, 0);
EXPECT_EQ(data->nA, 0);
EXPECT_EQ(data->ncon, 0);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(EngineIoTest, MakeDataResetsAllArenaPointerSizes) {
constexpr char xml[] = "<mujoco />";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull());
// without calling mj_forward, all array sizes should be zero.
EXPECT_EQ(data->parena, 0) << "expecting empty arena";
#define X(type, name, nr, nc) \
EXPECT_EQ(nr*nc, 0) << "expecting (" #nr " x " #nc ") to be zero";
EXPECT_EQ(nr* nc, 0) << "expecting (" #nr " x " #nc ") to be zero";
#undef MJ_D
#undef MJ_M
@@ -185,9 +178,6 @@ TEST_F(EngineIoTest, MakeDataResetsAllArenaPointerSizes) {
#define MJ_D(n) n
#define MJ_M(n) n
#undef X
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(EngineIoTest, MjvCopyModel) {
@@ -202,28 +192,27 @@ TEST_F(EngineIoTest, MjvCopyModel) {
</mujoco>
)";
char error[1024];
mjModel* model1 = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model1, NotNull()) << error;
MjModelPtr model1 = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model1.get(), NotNull()) << error;
mjModel* model2 = mj_copyModel(nullptr, model1);
mjModel* model2 = mj_copyModel(nullptr, model1.get());
ASSERT_THAT(model2, NotNull()) << error;
model1->mesh_vert[0] = 0.1;
model1->geom_rgba[0] = 0.2;
mj_copyModel(model2, model1);
mj_copyModel(model2, model1.get());
EXPECT_FLOAT_EQ(model2->mesh_vert[0], 0.1);
EXPECT_FLOAT_EQ(model2->geom_rgba[0], 0.2);
model1->mesh_vert[0] = 0.3;
model1->geom_rgba[0] = 0.4;
mjv_copyModel(model2, model1);
mjv_copyModel(model2, model1.get());
EXPECT_FLOAT_EQ(model2->mesh_vert[0], 0.1); // unchanged
EXPECT_FLOAT_EQ(model2->geom_rgba[0], 0.4);
mj_deleteModel(model2);
mj_deleteModel(model1);
}
TEST_F(EngineIoTest, MjvCopyData) {
@@ -239,23 +228,21 @@ TEST_F(EngineIoTest, MjvCopyData) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data1 = mj_makeData(model);
mj_forward(model, data1);
MjDataPtr data1 = MakeData(model);
mj_forward(model.get(), data1.get());
EXPECT_THAT(data1->efc_J, NotNull());
mjData* data2 = mj_copyData(nullptr, model, data1);
mjData* data2 = mj_copyData(nullptr, model.get(), data1.get());
EXPECT_THAT(data2->efc_J, NotNull());
mj_deleteData(data2);
data2 = mjv_copyData(nullptr, model, data1);
data2 = mjv_copyData(nullptr, model.get(), data1.get());
EXPECT_THAT(data2->efc_J, IsNull());
mj_deleteData(data2);
mj_deleteData(data1);
mj_deleteModel(model);
}
using ValidateReferencesTest = MujocoTest;
@@ -273,15 +260,14 @@ TEST_F(ValidateReferencesTest, BodyReferences) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
EXPECT_THAT(mj_validateReferences(model), IsNull());
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
model->jnt_bodyid[0] = 2;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("jnt_bodyid"));
mj_deleteModel(model);
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("jnt_bodyid"));
}
TEST_F(ValidateReferencesTest, AddressRange) {
@@ -298,20 +284,19 @@ TEST_F(ValidateReferencesTest, AddressRange) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
EXPECT_THAT(mj_validateReferences(model), IsNull());
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
model->body_jntnum[1] = 3;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("body_jntadr"));
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("body_jntadr"));
model->body_jntnum[1] = 2;
// Could be more strict and test for -1, but at the moment the code is a bit
// lenient.
model->body_jntadr[1] = -2;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("body_jntadr"));
mj_deleteModel(model);
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("body_jntadr"));
}
TEST_F(ValidateReferencesTest, AddressRangeNegativeNum) {
@@ -328,16 +313,16 @@ TEST_F(ValidateReferencesTest, AddressRangeNegativeNum) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
EXPECT_THAT(mj_validateReferences(model), IsNull());
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
// jntadr + jntnum is within safe range, but jntnum is negative.
model->body_jntadr[1] += 5;
model->body_jntnum[1] -= 5;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("body_jntnum"));
mj_deleteModel(model);
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("body_jntnum"));
}
TEST_F(ValidateReferencesTest, GeomCondim) {
@@ -353,16 +338,15 @@ TEST_F(ValidateReferencesTest, GeomCondim) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
EXPECT_THAT(mj_validateReferences(model), IsNull());
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
model->geom_condim[0] = 7;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("geom_condim"));
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("geom_condim"));
model->geom_condim[0] = -1;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("geom_condim"));
mj_deleteModel(model);
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("geom_condim"));
}
TEST_F(ValidateReferencesTest, HField) {
@@ -375,18 +359,17 @@ TEST_F(ValidateReferencesTest, HField) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
EXPECT_THAT(mj_validateReferences(model), IsNull());
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
model->hfield_adr[0] = -2;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("hfield_adr"));
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("hfield_adr"));
model->hfield_adr[0] = 0;
model->hfield_ncol[0] = 4;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("hfield_adr"));
mj_deleteModel(model);
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("hfield_adr"));
}
TEST_F(ValidateReferencesTest, Texture) {
@@ -399,18 +382,17 @@ TEST_F(ValidateReferencesTest, Texture) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
EXPECT_THAT(mj_validateReferences(model), IsNull());
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
model->tex_adr[0] = -2;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("tex_adr"));
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("tex_adr"));
model->tex_adr[0] = 0;
model->tex_height[0] = 4;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("tex_adr"));
mj_deleteModel(model);
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("tex_adr"));
}
TEST_F(ValidateReferencesTest, GeomPairs) {
@@ -433,19 +415,18 @@ TEST_F(ValidateReferencesTest, GeomPairs) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
EXPECT_THAT(mj_validateReferences(model), IsNull());
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
// Invalid geomid=4
model->pair_signature[0] = (1 << 16) | 5;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("pair_body1"));
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("pair_body1"));
model->pair_signature[0] = (5 << 16) | 1;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("pair_body2"));
mj_deleteModel(model);
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("pair_body2"));
}
TEST_F(ValidateReferencesTest, SensorsAddress) {
@@ -482,12 +463,12 @@ TEST_F(ValidateReferencesTest, SensorsAddress) {
for (const std::string& sensor_string : sensor_strings) {
std::string xml = absl::StrFormat(xml_template, sensor_string);
std::array<char, 1024> error;
mjModel* model =
MjModelPtr model =
LoadModelFromString(xml.c_str(), error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
EXPECT_THAT(mj_validateReferences(model), IsNull());
mj_deleteModel(model);
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
}
}
@@ -503,11 +484,11 @@ TEST_F(ValidateReferencesTest, SensorsAddressUser) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
EXPECT_THAT(mj_validateReferences(model), IsNull());
mj_deleteModel(model);
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
}
TEST_F(ValidateReferencesTest, SensorsObj) {
@@ -527,25 +508,24 @@ TEST_F(ValidateReferencesTest, SensorsObj) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
model->sensor_objtype[0] = -1;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("sensor_objtype"));
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("sensor_objtype"));
model->sensor_objtype[0] = mjOBJ_SITE;
model->sensor_objid[0] = model->nsite;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("sensor_objid"));
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("sensor_objid"));
model->sensor_objid[0] = 0;
model->sensor_reftype[0] = -1;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("sensor_reftype"));
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("sensor_reftype"));
model->sensor_reftype[0] = mjOBJ_BODY;
model->sensor_refid[0] = model->nbody;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("sensor_refid"));
mj_deleteModel(model);
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("sensor_refid"));
}
TEST_F(ValidateReferencesTest, MoreBodiesThanGeoms) {
@@ -565,10 +545,10 @@ TEST_F(ValidateReferencesTest, MoreBodiesThanGeoms) {
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
EXPECT_THAT(mj_validateReferences(model), IsNull());
mj_deleteModel(model);
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
}
TEST_F(ValidateReferencesTest, BodyExcludes) {
@@ -585,18 +565,17 @@ TEST_F(ValidateReferencesTest, BodyExcludes) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
EXPECT_THAT(mj_validateReferences(model), IsNull());
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
// Invalid bodyid=3
model->exclude_signature[0] = (1 << 16) | 4;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("exclude_body1"));
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("exclude_body1"));
model->exclude_signature[0] = (4 << 16) | 2;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("exclude_body2"));
mj_deleteModel(model);
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("exclude_body2"));
}
TEST_F(ValidateReferencesTest, EqualityConstraints) {
@@ -634,38 +613,37 @@ TEST_F(ValidateReferencesTest, EqualityConstraints) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
EXPECT_THAT(mj_validateReferences(model), IsNull());
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
// connect constraint
model->eq_obj1id[0] = -1;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj1id"));
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj1id"));
model->eq_obj1id[0] = model->nbody;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj1id"));
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj1id"));
model->eq_obj1id[0] = 1;
model->eq_obj2id[0] = -2;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj2id"));
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj2id"));
model->eq_obj2id[0] = model->nbody;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj2id"));
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj2id"));
model->eq_obj2id[0] = 0;
// weld constraint
model->eq_obj1id[1] = -1;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj1id"));
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj1id"));
model->eq_obj1id[1] = model->nbody;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj1id"));
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj1id"));
model->eq_obj1id[1] = 1;
model->eq_obj2id[1] = -2;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj2id"));
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj2id"));
model->eq_obj2id[1] = model->nbody;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj2id"));
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("eq_obj2id"));
model->eq_obj2id[1] = model->nbody - 1;
mj_deleteModel(model);
}
TEST_F(ValidateReferencesTest, Tuples) {
@@ -692,19 +670,19 @@ TEST_F(ValidateReferencesTest, Tuples) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
EXPECT_THAT(mj_validateReferences(model), IsNull());
EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
model->tuple_objtype[0] = -1;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("tuple_objtype"));
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("tuple_objtype"));
model->tuple_objtype[0] = mjOBJ_BODY;
model->tuple_objid[0] = model->nbody;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("tuple_objid"));
EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("tuple_objid"));
model->tuple_objid[0] = 1;
mj_deleteModel(model);
}
TEST_F(EngineIoTest, CanMarkAndFreeStack) {
@@ -716,20 +694,18 @@ TEST_F(EngineIoTest, CanMarkAndFreeStack) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull());
auto pstack_before = data->pstack;
mj_markStack(data);
mj_markStack(data.get());
EXPECT_GT(data->pstack, pstack_before);
mj_freeStack(data);
mj_freeStack(data.get());
EXPECT_EQ(data->pstack, pstack_before);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(EngineIoTest, LargeMemory) {
@@ -740,27 +716,25 @@ TEST_F(EngineIoTest, LargeMemory) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull());
// allocate 2.3G of mjtNums
mj_markStack(data);
mj_markStack(data.get());
size_t num = 2300000000 / sizeof(mjtNum);
mjtNum* testNum = mj_stackAllocNum(data, num);
testNum[num-1] = 1;
mj_freeStack(data);
mjtNum* testNum = mj_stackAllocNum(data.get(), num);
testNum[num - 1] = 1;
mj_freeStack(data.get());
// allocate 2.3G of bytes
mj_markStack(data);
mj_markStack(data.get());
num = 2300000000;
char* testByte = (char*) mj_stackAllocByte(data, num, alignof(char));
testByte[num-1] = 1;
mj_freeStack(data);
mj_deleteData(data);
mj_deleteModel(model);
char* testByte = (char*)mj_stackAllocByte(data.get(), num, alignof(char));
testByte[num - 1] = 1;
mj_freeStack(data.get());
}
TEST_F(EngineIoTest, VeryLargeMemory) {
@@ -771,31 +745,29 @@ TEST_F(EngineIoTest, VeryLargeMemory) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
if (!model) {
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
if (!model.get()) {
// in some test environments, 8GB is too large
EXPECT_THAT(error.data(), HasSubstr("Could not allocate memory"));
} else {
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull());
// allocate 7G of mjtNums
mj_markStack(data);
mj_markStack(data.get());
size_t num = 7516192768ull / sizeof(mjtNum);
mjtNum* testNum = mj_stackAllocNum(data, num);
testNum[num-1] = 1;
mj_freeStack(data);
mjtNum* testNum = mj_stackAllocNum(data.get(), num);
testNum[num - 1] = 1;
mj_freeStack(data.get());
// allocate 7G of bytes
mj_markStack(data);
mj_markStack(data.get());
num = 7516192768ull;
char* testByte = (char*) mj_stackAllocByte(data, num, alignof(char));
testByte[num-1] = 1;
mj_freeStack(data);
mj_deleteData(data);
mj_deleteModel(model);
char* testByte = (char*)mj_stackAllocByte(data.get(), num, alignof(char));
testByte[num - 1] = 1;
mj_freeStack(data.get());
}
}
@@ -822,23 +794,22 @@ TEST_F(EngineIoTest, TestStackShardingForThreads) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull());
mju_threadpool(data, 10);
mju_threadpool(data.get(), 10);
constexpr int kTasks = 1000;
TestFunctionArgs test_function_args;
mju_dispatch(model, data, TestFunction, &test_function_args, kTasks);
mju_dispatch(model.get(), data.get(), TestFunction, &test_function_args,
kTasks);
for (int i = 0; i < kTasks; ++i) {
EXPECT_EQ(i, test_function_args.stack_output[i]);
}
mj_deleteData(data);
mj_deleteModel(model);
}
#ifdef ADDRESS_SANITIZER
@@ -858,37 +829,36 @@ TEST_F(EngineIoTest, CanDetectStackFrameLeakage) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull());
// MarkFreeStack correctly calls mj_freeStack, should not error.
MarkFreeStack(data, /* free= */ true);
MarkFreeStack(data.get(), /* free= */ true);
// MarkFreeStack calls mj_markStack without mj_freeStack, the next call to
// mj_freeStack should detect the stack frame leakage.
mj_markStack(data);
MarkFreeStack(data, /* free= */ false);
mj_markStack(data.get());
MarkFreeStack(data.get(), /* free= */ false);
EXPECT_THAT(
MjuErrorMessageFrom(mj_freeStack)(data),
MjuErrorMessageFrom(mj_freeStack)(data.get()),
ContainsRegex(
"mj_markStack in MarkFreeStack at .*engine_io_test\\.cc.* has no "
"corresponding mj_freeStack"));
// Dangling stack frames should be detected in mj_deleteData.
mj_resetData(model, data);
mj_markStack(data);
mj_resetData(model.get(), data.get());
mj_markStack(data.get());
EXPECT_THAT(
MjuErrorMessageFrom(mj_deleteData)(data),
MjuErrorMessageFrom(mj_deleteData)(data.get()),
ContainsRegex(
"mj_markStack in .+EngineIoTest_CanDetectStackFrameLeakage.+ has no "
"corresponding mj_freeStack"));
mj_resetData(model, data);
mj_deleteData(data);
mj_deleteModel(model);
mj_resetData(model.get(), data.get());
}
TEST_F(EngineIoTest, RedZoneAlignmentTest) {
@@ -900,19 +870,17 @@ TEST_F(EngineIoTest, RedZoneAlignmentTest) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull());
mj_markStack(data);
mj_stackAllocByte(data, 1, 1);
mj_stackAllocByte(data, 1, 1);
mj_freeStack(data);
mj_deleteData(data);
mj_deleteModel(model);
mj_markStack(data.get());
mj_stackAllocByte(data.get(), 1, 1);
mj_stackAllocByte(data.get(), 1, 1);
mj_freeStack(data.get());
}
#endif
@@ -930,15 +898,15 @@ TEST_F(EngineIoTest, LoadModelBufferRejectsOverflowingSizes) {
constexpr char xml[] = "<mujoco />";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
// save model to a buffer
int bufsize = mj_sizeModel(model);
int bufsize = mj_sizeModel(model.get());
ASSERT_GT(bufsize, 0);
std::vector<char> buffer(bufsize);
mj_saveModel(model, nullptr, buffer.data(), bufsize);
mj_deleteModel(model);
mj_saveModel(model.get(), nullptr, buffer.data(), bufsize);
// locate the size fields in the buffer (after 5-int header)
const int header_bytes = 5 * sizeof(int);
test/engine/engine_island_test.cc
+25 -55
View File
@@ -14,6 +14,8 @@
// Tests for engine/engine_island.c.
#include "src/engine/engine_island.h"
#include <string>
#include <vector>
@@ -21,7 +23,6 @@
#include <gtest/gtest.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_island.h"
#include "src/engine/engine_util_sparse.h"
#include "test/fixture.h"
@@ -34,16 +35,11 @@ using ::testing::NotNull;
using ::testing::Pointwise;
using IslandTest = MujocoTest;
TEST_F(IslandTest, FloodFillSingleton) {
// adjacency matrix for the graph 0 1 2
// U U
// (3 singletons, 0 and 2 have self-edges)
mjtNum mat[9] = {
1, 0, 0,
0, 0, 0,
0, 0, 1
};
mjtNum mat[9] = {1, 0, 0, 0, 0, 0, 0, 0, 1};
constexpr int nr = 3;
constexpr int nnz = 2;
int rownnz[nr];
@@ -54,7 +50,7 @@ TEST_F(IslandTest, FloodFillSingleton) {
// outputs / scratch
int island[nr];
int scratch[2*nr];
int scratch[2 * nr];
// flood fill
int nisland = mj_floodFill(island, nr, rownnz, rowadr, colind, scratch);
@@ -63,14 +59,9 @@ TEST_F(IslandTest, FloodFillSingleton) {
EXPECT_THAT(island, ElementsAre(0, -1, 1));
}
TEST_F(IslandTest, FloodFill1) {
// adjacency matrix for the graph 0 - 1 - 2
mjtNum mat[9] = {
0, 1, 0,
1, 0, 1,
0, 1, 0
};
mjtNum mat[9] = {0, 1, 0, 1, 0, 1, 0, 1, 0};
constexpr int nr = 3;
constexpr int nnz = 4;
int rownnz[nr];
@@ -89,17 +80,11 @@ TEST_F(IslandTest, FloodFill1) {
EXPECT_THAT(island, ElementsAre(0, 0, 0));
}
TEST_F(IslandTest, FloodFill2) {
// adjacency matrix for the graph 6 – 1 – 4 0 – 3 – 5 – 2
mjtNum mat[49] = {
0, 0, 0, 1, 0, 0, 0,
0, 0, 0, 0, 1, 0, 1,
0, 0, 0, 0, 0, 1, 0,
1, 0, 0, 0, 0, 1, 0,
0, 1, 0, 0, 0, 0, 0,
0, 0, 1, 1, 0, 0, 0,
0, 1, 0, 0, 0, 0, 0,
0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0,
0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
};
constexpr int nr = 7;
constexpr int nnz = 10;
@@ -119,15 +104,11 @@ TEST_F(IslandTest, FloodFill2) {
EXPECT_THAT(island, ElementsAre(0, 1, 0, 0, 1, 0, 1));
}
TEST_F(IslandTest, FloodFill3a) {
// adjacency matrix for the graph 0 2 1 – 3
// U
mjtNum mat[16] = {
0, 0, 0, 0,
0, 0, 0, 1,
0, 0, 1, 0,
0, 1, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0,
};
constexpr int nr = 4;
constexpr int nnz = 3;
@@ -147,7 +128,6 @@ TEST_F(IslandTest, FloodFill3a) {
EXPECT_THAT(island, ElementsAre(-1, 0, 1, 0));
}
TEST_F(IslandTest, FloodFill3b) {
/*
adjacency matrix for the graph 1 – 2 3 4 – 5
@@ -155,13 +135,8 @@ TEST_F(IslandTest, FloodFill3b) {
0 – 6
*/
mjtNum mat[49] = {
0, 0, 0, 0, 1, 0, 1,
0, 1, 1, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0,
1, 0, 0, 0, 0, 1, 1,
0, 0, 0, 0, 1, 0, 1,
1, 0, 0, 0, 1, 1, 0,
0, 0, 0, 0, 1, 0, 1, 0, 1, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 1, 0, 1, 1, 0, 0, 0, 1, 1, 0,
};
constexpr int nr = 7;
constexpr int nnz = 13;
@@ -181,8 +156,7 @@ TEST_F(IslandTest, FloodFill3b) {
EXPECT_THAT(island, ElementsAre(0, 1, 1, -1, 0, 0, 0));
}
static const char* const kAbacusPath =
"engine/testdata/island/abacus.xml";
static const char* const kAbacusPath = "engine/testdata/island/abacus.xml";
TEST_F(IslandTest, Abacus) {
const std::string xml_path = GetTestDataFilePath(kAbacusPath);
@@ -207,10 +181,10 @@ TEST_F(IslandTest, Abacus) {
}
// sizes
int nv = model->nv;
int nefc = data->nefc;
int nv = model->nv;
int nefc = data->nefc;
int nisland = data->nisland;
int nidof = data->nidof;
int nidof = data->nidof;
// 4 dofs, 12 constraints, 2 islands
EXPECT_EQ(nv, 4);
@@ -268,9 +242,9 @@ TEST_F(IslandTest, Abacus) {
}
// local variables
nefc = data->nefc;
nisland = data->nisland;
nidof = data->nidof;
nefc = data->nefc;
nisland = data->nisland;
nidof = data->nidof;
EXPECT_EQ(nisland, 3);
EXPECT_EQ(nidof, 4);
@@ -314,8 +288,8 @@ TEST_F(IslandTest, DenseSparse) {
}
// sizes
int nv = model->nv;
int nefc = data1->nefc;
int nv = model->nv;
int nefc = data1->nefc;
int nisland = data1->nisland;
// expect sparse and dense to be identical
@@ -327,8 +301,7 @@ TEST_F(IslandTest, DenseSparse) {
AsVector(data2->island_idofadr, nisland));
EXPECT_EQ(AsVector(data1->island_nv, nisland),
AsVector(data2->island_nv, nisland));
EXPECT_EQ(AsVector(data1->dof_island, nv),
AsVector(data2->dof_island, nv));
EXPECT_EQ(AsVector(data1->dof_island, nv), AsVector(data2->dof_island, nv));
EXPECT_EQ(AsVector(data1->map_idof2dof, nv),
AsVector(data2->map_idof2dof, nv));
EXPECT_EQ(AsVector(data1->map_dof2idof, nv),
@@ -462,12 +435,10 @@ TEST_F(IslandTest, EqualityConstraintOfTendons) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
mj_forward(model, data);
mj_deleteData(data);
mj_deleteModel(model);
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
}
TEST_F(IslandTest, PGSIsland) {
@@ -496,8 +467,7 @@ TEST_F(IslandTest, PGSIsland) {
// solve without islands
m->opt.disableflags |= mjDSBL_ISLAND;
mj_forward(m, d);
std::vector<mjtNum> qfrc_mono(d->qfrc_constraint,
d->qfrc_constraint + m->nv);
std::vector<mjtNum> qfrc_mono(d->qfrc_constraint, d->qfrc_constraint + m->nv);
// expect close match (inexact due to randomized constraint visitation order)
EXPECT_THAT(qfrc_island, Pointwise(MjNear(1e-3, 1e-3), qfrc_mono));
test/engine/engine_passive_test.cc
+156 -220
View File
@@ -49,28 +49,25 @@ TEST_F(PassiveTest, DisableFlags) {
)";
char error[1024];
mjModel* m = LoadModelFromString(flex_xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
MjModelPtr m = LoadModelFromString(flex_xml, error, sizeof(error));
ASSERT_THAT(m.get(), testing::NotNull()) << error;
MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m.get(), d.get(), 0);
mj_forward(m, d);
mj_forward(m.get(), d.get());
EXPECT_MJTNUM_EQ(d->qacc[0], 11);
m->opt.disableflags = mjDSBL_DAMPER;
mj_forward(m, d);
mj_forward(m.get(), d.get());
EXPECT_MJTNUM_EQ(d->qacc[0], 10);
m->opt.disableflags = mjDSBL_SPRING;
mj_forward(m, d);
mj_forward(m.get(), d.get());
EXPECT_MJTNUM_EQ(d->qacc[0], 1);
m->opt.disableflags = mjDSBL_SPRING | mjDSBL_DAMPER;
mj_forward(m, d);
mj_forward(m.get(), d.get());
EXPECT_EQ(d->qacc[0], -10);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(PassiveTest, GravcompNestedBody) {
@@ -89,17 +86,14 @@ TEST_F(PassiveTest, GravcompNestedBody) {
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
mj_forward(m, d);
mj_forward(m.get(), d.get());
EXPECT_GT(d->qacc[2], 0);
EXPECT_NEAR(d->qacc[2], 2.0, 0.1);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(PassiveTest, PolyStiffnessSlide) {
@@ -119,16 +113,13 @@ TEST_F(PassiveTest, PolyStiffnessSlide) {
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m.get(), d.get(), 0);
mj_forward(m, d);
mj_forward(m.get(), d.get());
EXPECT_MJTNUM_EQ(d->qfrc_spring[0], -48);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(PassiveTest, PolyStiffnessAntiSymmetric) {
@@ -148,16 +139,13 @@ TEST_F(PassiveTest, PolyStiffnessAntiSymmetric) {
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m.get(), d.get(), 0);
mj_forward(m, d);
mj_forward(m.get(), d.get());
EXPECT_MJTNUM_EQ(d->qfrc_spring[0], 8);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(PassiveTest, PolyStiffnessTendon) {
@@ -183,21 +171,18 @@ TEST_F(PassiveTest, PolyStiffnessTendon) {
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
m->tendon_stiffness[0] = 10;
m->tendon_stiffnesspoly[0] = 5;
m->tendon_stiffnesspoly[1] = 1;
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m.get(), d.get(), 0);
mj_forward(m, d);
mj_forward(m.get(), d.get());
EXPECT_MJTNUM_EQ(d->qfrc_spring[0], -48);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(PassiveTest, PolyStiffnessEnergy) {
@@ -221,21 +206,18 @@ TEST_F(PassiveTest, PolyStiffnessEnergy) {
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m.get(), d.get(), 0);
mj_forward(m, d);
mj_forward(m.get(), d.get());
mjtNum total_energy = d->energy[0] + d->energy[1];
for (int i = 0; i < 100; i++) {
mj_step(m, d);
mj_step(m.get(), d.get());
EXPECT_NEAR(d->energy[0] + d->energy[1], total_energy, 0.002);
}
mj_deleteData(d);
mj_deleteModel(m);
}
// ------------------------ ellipsoid fluid model ------------------------------
@@ -261,11 +243,11 @@ TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) {
)";
char error[1024];
mjModel* m2 = LoadModelFromString(two_bodies_xml, error, sizeof(error));
ASSERT_THAT(m2, NotNull()) << error;
mjData* d2 = mj_makeData(m2);
MjModelPtr m2 = LoadModelFromString(two_bodies_xml, error, sizeof(error));
ASSERT_THAT(m2.get(), NotNull()) << error;
MjDataPtr d2 = MakeData(m2);
for (int i = 0; i < 6; i++) {
d2->qvel[i] = (mjtNum) i+1;
d2->qvel[i] = (mjtNum)i + 1;
}
d2->qpos[3] = 0.5;
d2->qpos[4] = 0.5;
@@ -285,11 +267,11 @@ TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) {
</mujoco>
)";
mjModel* m1 = LoadModelFromString(one_body_xml, error, sizeof(error));
ASSERT_THAT(m1, NotNull()) << error;
mjData* d1 = mj_makeData(m1);
MjModelPtr m1 = LoadModelFromString(one_body_xml, error, sizeof(error));
ASSERT_THAT(m1.get(), NotNull()) << error;
MjDataPtr d1 = MakeData(m1);
for (int i = 0; i < 6; i++) {
d1->qvel[i] = (mjtNum) i+1;
d1->qvel[i] = (mjtNum)i + 1;
}
d1->qpos[3] = 0.5;
d1->qpos[4] = 0.5;
@@ -301,16 +283,11 @@ TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) {
EXPECT_EQ(m1->nv, m2->nv);
mj_forward(m2, d2);
mj_forward(m1, d1);
mj_forward(m2.get(), d2.get());
mj_forward(m1.get(), d1.get());
for (int i = 0; i < m1->nv; i++) {
EXPECT_NEAR(d2->qfrc_passive[i], d1->qfrc_passive[i], tol);
}
mj_deleteData(d1);
mj_deleteModel(m1);
mj_deleteData(d2);
mj_deleteModel(m2);
}
TEST_F(EllipsoidFluidTest, DefaultsPropagate) {
@@ -334,16 +311,13 @@ TEST_F(EllipsoidFluidTest, DefaultsPropagate) {
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
EXPECT_THAT(AsVector(model->geom_fluid, 6),
ElementsAre(0, 0, 0, 0, 0, 0));
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_THAT(AsVector(model->geom_fluid, 6), ElementsAre(0, 0, 0, 0, 0, 0));
EXPECT_THAT(AsVector(model->geom_fluid + mjNFLUID, 6),
ElementsAre(1, 2, 3, 4, 5, 6));
mj_deleteModel(model);
}
// ------------------------------ tendons --------------------------------------
using TendonTest = MujocoTest;
@@ -359,7 +333,7 @@ TEST_F(TendonTest, SpringrangeDeadband) {
// initial state outside deadband: spring is active
mj_forward(model, data);
mjtNum expected_force = model->tendon_stiffness[0] *
(model->tendon_lengthspring[1] - data->ten_length[0]);
(model->tendon_lengthspring[1] - data->ten_length[0]);
EXPECT_EQ(expected_force, data->qfrc_passive[0]);
// put body inside deadband: spring is inactive
@@ -391,12 +365,10 @@ TEST_F(ElasticityTest, FlexCompatibility) {
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(flex_xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
MjModelPtr m = LoadModelFromString(flex_xml, error, sizeof(error));
ASSERT_THAT(m.get(), testing::NotNull()) << error;
mjData* d = mj_makeData(m);
mj_deleteData(d);
mj_deleteModel(m);
MjDataPtr d = MakeData(m);
}
// -------------------------------- shell -----------------------------------
@@ -413,39 +385,37 @@ TEST_F(ElasticityTest, ElasticEnergyShell) {
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
mjData* d = mj_makeData(m);
mj_kinematics(m, d);
mj_flex(m, d);
MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
ASSERT_THAT(m.get(), testing::NotNull()) << error;
MjDataPtr d = MakeData(m);
mj_kinematics(m.get(), d.get());
mj_flex(m.get(), d.get());
// check that a plane is in the kernel of the energy
for (mjtNum scale = 1; scale < 4; scale++) {
for (int e = 0; e < m->flex_edgenum[0]; e++) {
int* edge = m->flex_edge + 2*(m->flex_edgeadr[0] + e);
int* flap = m->flex_edgeflap + 2*(m->flex_edgeadr[0] + e);
int* edge = m->flex_edge + 2 * (m->flex_edgeadr[0] + e);
int* flap = m->flex_edgeflap + 2 * (m->flex_edgeadr[0] + e);
int v[4] = {edge[0], edge[1], flap[0], flap[1]};
if (v[3]== -1) {
if (v[3] == -1) {
continue;
}
mjtNum energy = 0;
mjtNum volume = 1./2.;
mjtNum volume = 1. / 2.;
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
for (int x = 0; x < 3; x++) {
mjtNum elongation1 = scale * d->flexvert_xpos[3*v[i]+x];
mjtNum elongation2 = scale * d->flexvert_xpos[3*v[j]+x];
energy += m->flex_bending[17*e+4*i+j] * elongation1 * elongation2;
mjtNum elongation1 = scale * d->flexvert_xpos[3 * v[i] + x];
mjtNum elongation2 = scale * d->flexvert_xpos[3 * v[j] + x];
energy +=
m->flex_bending[17 * e + 4 * i + j] * elongation1 * elongation2;
}
}
}
EXPECT_NEAR(
4*energy/volume, 0, std::numeric_limits<float>::epsilon());
EXPECT_NEAR(4 * energy / volume, 0,
std::numeric_limits<float>::epsilon());
}
}
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(ElasticityTest, CurvedShell) {
@@ -487,12 +457,12 @@ TEST_F(ElasticityTest, CurvedShell) {
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
mjData* d = mj_makeData(m);
mj_kinematics(m, d);
mj_flex(m, d);
mj_passive(m, d);
MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
ASSERT_THAT(m.get(), testing::NotNull()) << error;
MjDataPtr d = MakeData(m);
mj_kinematics(m.get(), d.get());
mj_flex(m.get(), d.get());
mj_passive(m.get(), d.get());
// v1 force component is in-plane along v1-v0 edge (y-axis)
EXPECT_NEAR(d->qfrc_spring[3], 0, 1e-6);
@@ -505,9 +475,6 @@ TEST_F(ElasticityTest, CurvedShell) {
// v3 force component is in-plane along v3-v0 edge (z-axis)
EXPECT_NEAR(d->qfrc_spring[9], 0, 1e-6);
EXPECT_NEAR(d->qfrc_spring[10], 0, 1e-6);
mj_deleteData(d);
mj_deleteModel(m);
}
// -------------------------------- membrane -----------------------------------
@@ -525,12 +492,12 @@ TEST_F(ElasticityTest, ElasticEnergyMembrane) {
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
ASSERT_THAT(m.get(), testing::NotNull()) << error;
MjDataPtr d = MakeData(m);
mj_kinematics(m, d);
mj_flex(m, d);
mj_kinematics(m.get(), d.get());
mj_flex(m.get(), d.get());
mjtNum* metric = m->flex_stiffness + 21 * m->flex_elemadr[0];
// check that if the entire geometry is rescaled by a factor "scale", then
@@ -539,26 +506,24 @@ TEST_F(ElasticityTest, ElasticEnergyMembrane) {
for (mjtNum scale = 1; scale < 4; scale++) {
for (int t = 0; t < m->flex_elemnum[0]; t++) {
mjtNum energy = 0;
mjtNum volume = 1./2.;
mjtNum volume = 1. / 2.;
int idx = 0;
for (int e1 = 0; e1 < 3; e1++) {
for (int e2 = e1; e2 < 3; e2++) {
int idx1 = m->flex_elemedge[3*t+e1 + m->flex_elemedgeadr[0]];
int idx2 = m->flex_elemedge[3*t+e2 + m->flex_elemedgeadr[0]];
int idx1 = m->flex_elemedge[3 * t + e1 + m->flex_elemedgeadr[0]];
int idx2 = m->flex_elemedge[3 * t + e2 + m->flex_elemedgeadr[0]];
mjtNum elong1 =
scale * m->flexedge_length0[idx1] * m->flexedge_length0[idx1];
mjtNum elong2 =
scale * m->flexedge_length0[idx2] * m->flexedge_length0[idx2];
energy += metric[21*t+idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.);
energy +=
metric[21 * t + idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.);
}
}
const mjtNum tol = MjTol(std::numeric_limits<float>::epsilon(), 1e-5);
EXPECT_NEAR(4*energy/volume, 2*scale*scale, tol);
EXPECT_NEAR(4 * energy / volume, 2 * scale * scale, tol);
}
}
mj_deleteData(d);
mj_deleteModel(m);
}
// -------------------------------- solid -----------------------------------
@@ -576,12 +541,12 @@ TEST_F(ElasticityTest, ElasticEnergySolid) {
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
ASSERT_THAT(m.get(), testing::NotNull()) << error;
MjDataPtr d = MakeData(m);
mj_kinematics(m, d);
mj_flex(m, d);
mj_kinematics(m.get(), d.get());
mj_flex(m.get(), d.get());
mjtNum* metric = m->flex_stiffness + 21 * m->flex_elemadr[0];
// check that if the entire geometry is rescaled by a factor "scale", then
@@ -590,26 +555,24 @@ TEST_F(ElasticityTest, ElasticEnergySolid) {
for (mjtNum scale = 1; scale < 4; scale++) {
for (int t = 0; t < m->flex_elemnum[0]; t++) {
mjtNum energy = 0;
mjtNum volume = 1./6.;
mjtNum volume = 1. / 6.;
int idx = 0;
for (int e1 = 0; e1 < 6; e1++) {
for (int e2 = e1; e2 < 6; e2++) {
int idx1 = m->flex_elemedge[6*t+e1 + m->flex_elemedgeadr[0]];
int idx2 = m->flex_elemedge[6*t+e2 + m->flex_elemedgeadr[0]];
int idx1 = m->flex_elemedge[6 * t + e1 + m->flex_elemedgeadr[0]];
int idx2 = m->flex_elemedge[6 * t + e2 + m->flex_elemedgeadr[0]];
mjtNum elong1 =
scale * m->flexedge_length0[idx1] * m->flexedge_length0[idx1];
mjtNum elong2 =
scale * m->flexedge_length0[idx2] * m->flexedge_length0[idx2];
energy += metric[21*t+idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.);
energy +=
metric[21 * t + idx++] * elong1 * elong2 * (e1 == e2 ? 1. : 2.);
}
}
const mjtNum tol = MjTol(std::numeric_limits<float>::epsilon(), 1e-4);
EXPECT_NEAR(energy/volume, 3*scale*scale, tol);
EXPECT_NEAR(energy / volume, 3 * scale * scale, tol);
}
}
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(PassiveTest, PolynomialStiffnessJoint) {
@@ -626,19 +589,16 @@ TEST_F(PassiveTest, PolynomialStiffnessJoint) {
</keyframe>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml);
ASSERT_THAT(m, NotNull());
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
MjModelPtr m = LoadModelFromString(xml);
ASSERT_THAT(m.get(), NotNull());
MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m.get(), d.get(), 0);
mj_forward(m.get(), d.get());
mjtNum x = 0.5;
mjtNum a = 2, b = 3, c = 4;
mjtNum expected = -(a + b * x + c * x * x) * x;
EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(PassiveTest, PolynomialStiffnessNegativeDisplacement) {
@@ -655,19 +615,16 @@ TEST_F(PassiveTest, PolynomialStiffnessNegativeDisplacement) {
</keyframe>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml);
ASSERT_THAT(m, NotNull());
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
MjModelPtr m = LoadModelFromString(xml);
ASSERT_THAT(m.get(), NotNull());
MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m.get(), d.get(), 0);
mj_forward(m.get(), d.get());
mjtNum x = -0.5;
mjtNum a = 2, b = 3, c = 4;
mjtNum expected = -(a + b * x + c * x * x) * x;
EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(PassiveTest, PolyStiffnessFixedTendon) {
@@ -693,19 +650,16 @@ TEST_F(PassiveTest, PolyStiffnessFixedTendon) {
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m.get(), d.get(), 0);
mj_forward(m, d);
mj_forward(m.get(), d.get());
mjtNum x = d->ten_length[0] - m->tendon_lengthspring[1];
mjtNum expected = -(10 + 5*x + 1*x*x) * x;
mjtNum expected = -(10 + 5 * x + 1 * x * x) * x;
EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(PassiveTest, PolyStiffnessSpatialTendon) {
@@ -734,23 +688,18 @@ TEST_F(PassiveTest, PolyStiffnessSpatialTendon) {
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m.get(), d.get(), 0);
mj_forward(m, d);
mj_forward(m.get(), d.get());
mjtNum x = d->ten_length[0] - m->tendon_lengthspring[1];
mjtNum expected = -x * (10 + 5*x + 1*x*x);
mjtNum expected = -x * (10 + 5 * x + 1 * x * x);
EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(PassiveTest, PolynomialDampingJoint) {
static constexpr char xml[] = R"(
<mujoco>
@@ -765,19 +714,16 @@ TEST_F(PassiveTest, PolynomialDampingJoint) {
</keyframe>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml);
ASSERT_THAT(m, NotNull());
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
MjModelPtr m = LoadModelFromString(xml);
ASSERT_THAT(m.get(), NotNull());
MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m.get(), d.get(), 0);
mj_forward(m.get(), d.get());
mjtNum v = 0.5;
mjtNum a = 2, b = 3, c = 4;
mjtNum expected = -(a * v + b * v * mju_abs(v) + c * v * v * v);
EXPECT_NEAR(d->qfrc_damper[0], expected, 1e-12);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(PassiveTest, PolynomialDampingNegativeVelocity) {
@@ -794,19 +740,16 @@ TEST_F(PassiveTest, PolynomialDampingNegativeVelocity) {
</keyframe>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml);
ASSERT_THAT(m, NotNull());
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
mj_forward(m, d);
MjModelPtr m = LoadModelFromString(xml);
ASSERT_THAT(m.get(), NotNull());
MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m.get(), d.get(), 0);
mj_forward(m.get(), d.get());
mjtNum v = -0.5;
mjtNum a = 2, b = 3, c = 4;
mjtNum expected = -(a * v + b * v * mju_abs(v) + c * v * v * v);
EXPECT_NEAR(d->qfrc_damper[0], expected, 1e-12);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(PassiveTest, PolynomialDampingTendon) {
@@ -832,19 +775,16 @@ TEST_F(PassiveTest, PolynomialDampingTendon) {
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
mj_resetDataKeyframe(m, d, 0);
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m.get(), d.get(), 0);
mj_forward(m, d);
mj_forward(m.get(), d.get());
mjtNum v = d->ten_velocity[0];
mjtNum expected = -(10*v + 5*v*mju_abs(v) + 1*v*v*v);
mjtNum expected = -(10 * v + 5 * v * mju_abs(v) + 1 * v * v * v);
EXPECT_NEAR(d->qfrc_damper[0], expected, 1e-12);
mj_deleteData(d);
mj_deleteModel(m);
}
// shell-mode (elastic2d=stretch) flexcomp must have zero passive spring forces
@@ -867,20 +807,17 @@ TEST_F(ElasticityTest, ShellModeZeroForceAtRest) {
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), testing::NotNull()) << error;
MjDataPtr d = MakeData(m);
mj_forward(m, d);
mj_forward(m.get(), d.get());
// all spring forces should be zero at rest
for (int i = 0; i < m->nv; i++) {
EXPECT_NEAR(d->qfrc_spring[i], 0, 1e-10)
<< "nonzero spring force at DOF " << i;
}
mj_deleteData(d);
mj_deleteModel(m);
}
// interpolated shell bending must produce zero spring forces at rest
@@ -901,16 +838,16 @@ TEST_F(ElasticityTest, InterpBendingZeroForceAtRest) {
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), testing::NotNull()) << error;
MjDataPtr d = MakeData(m);
// verify bending data was compiled
const mjtNum* bdata = m->flex_bending + m->flex_bendingadr[0];
int nedge = (int)bdata[0];
EXPECT_GT(nedge, 0) << "no bending edges compiled";
mj_forward(m, d);
mj_forward(m.get(), d.get());
// all spring forces should be zero at rest
for (int i = 0; i < m->nv; i++) {
@@ -944,9 +881,6 @@ TEST_F(ElasticityTest, InterpBendingZeroForceAtRest) {
EXPECT_GT(n_flat, 0) << "no intra-surface edges found";
EXPECT_GT(n_corner, 0) << "no corner edges found";
EXPECT_EQ(n_flat + n_corner, nedge);
mj_deleteData(d);
mj_deleteModel(m);
}
// interpolated shell bending must produce zero forces after a rigid rotation
@@ -967,21 +901,23 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) {
)";
char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), testing::NotNull()) << error;
MjDataPtr d = MakeData(m);
// compute geometric center from body positions (skip world body)
mjtNum center[3] = {0, 0, 0};
int nnodes = 0;
for (int b = 1; b < m->nbody; b++) {
center[0] += m->body_pos[3*b + 0];
center[1] += m->body_pos[3*b + 1];
center[2] += m->body_pos[3*b + 2];
center[0] += m->body_pos[3 * b + 0];
center[1] += m->body_pos[3 * b + 1];
center[2] += m->body_pos[3 * b + 2];
nnodes++;
}
ASSERT_GT(nnodes, 0);
center[0] /= nnodes; center[1] /= nnodes; center[2] /= nnodes;
center[0] /= nnodes;
center[1] /= nnodes;
center[2] /= nnodes;
// rotation: 45 degrees about (1,1,1)/sqrt(3)
mjtNum angle = 45 * 3.14159265358979 / 180.0;
@@ -995,9 +931,9 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) {
// new_pos = center + R * (body_pos - center)
// qpos = new_pos - body_pos
for (int b = 1; b < m->nbody; b++) {
mjtNum rel[3] = {m->body_pos[3*b+0] - center[0],
m->body_pos[3*b+1] - center[1],
m->body_pos[3*b+2] - center[2]};
mjtNum rel[3] = {m->body_pos[3 * b + 0] - center[0],
m->body_pos[3 * b + 1] - center[1],
m->body_pos[3 * b + 2] - center[2]};
mjtNum rotated[3];
mju_rotVecQuat(rotated, rel, neg_quat);
@@ -1007,7 +943,10 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) {
int qadr = m->jnt_qposadr[jid];
int axis = -1;
for (int a = 0; a < 3; a++) {
if (m->jnt_axis[3*jid + a] != 0) { axis = a; break; }
if (m->jnt_axis[3 * jid + a] != 0) {
axis = a;
break;
}
}
if (axis >= 0) {
d->qpos[qadr] =
@@ -1016,7 +955,7 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) {
}
}
mj_forward(m, d);
mj_forward(m.get(), d.get());
// spring forces should still be zero after rigid rotation
const mjtNum tol = MjTol(1e-6, 1e-3);
@@ -1024,9 +963,6 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) {
EXPECT_NEAR(d->qfrc_spring[i], 0, tol)
<< "nonzero spring force at DOF " << i << " after rigid rotation";
}
mj_deleteData(d);
mj_deleteModel(m);
}
} // namespace
test/engine/engine_plugin_test.cc
+55 -65
View File
@@ -68,13 +68,9 @@ class BaseTestPlugin {
advance_count = 0;
}
void Compute() {
compute_count += stride;
}
void Compute() { compute_count += stride; }
void Advance() {
advance_count += stride;
}
void Advance() { advance_count += stride; }
protected:
int stride;
@@ -458,11 +454,12 @@ TEST_F(MujocoTest, EmptyPluginDisallowed) {
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, testing::IsNull()) << error.data();
EXPECT_THAT(error.data(), HasSubstr(
"neither 'plugin' nor 'instance' is specified for body 'world'"));
mj_deleteModel(m);
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m.get(), testing::IsNull()) << error.data();
EXPECT_THAT(
error.data(),
HasSubstr(
"neither 'plugin' nor 'instance' is specified for body 'world'"));
}
TEST_F(MujocoTest, FirstPartyPlugins) {
@@ -480,13 +477,13 @@ TEST_F(EnginePluginTest, NoAttributePlugin) {
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, testing::NotNull()) << error.data();
mj_deleteModel(m);
MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m.get(), testing::NotNull()) << error.data();
}
TEST_F(EnginePluginTest, MultiplePluginTableBlocks) {
EXPECT_EQ(mjp_pluginCount(), kNumTruePlugins + kNumFakePlugins + kNumTestPlugins);
EXPECT_EQ(mjp_pluginCount(),
kNumTruePlugins + kNumFakePlugins + kNumTestPlugins);
const mjpPlugin* last_plugin = nullptr;
int table_count = 0;
@@ -494,7 +491,7 @@ TEST_F(EnginePluginTest, MultiplePluginTableBlocks) {
int slot;
std::string name = absl::StrFormat("mujoco.test.fake%u", i);
const mjpPlugin* plugin = mjp_getPlugin(name.c_str(), &slot);
EXPECT_EQ(slot, kNumTruePlugins+i);
EXPECT_EQ(slot, kNumTruePlugins + i);
EXPECT_THAT(plugin, NotNull());
if (plugin - last_plugin != 1) {
++table_count;
@@ -513,16 +510,17 @@ TEST_F(EnginePluginTest, RegisterIdenticalPlugin) {
EXPECT_EQ(RegisterSensorPlugin(), kNumTruePlugins);
EXPECT_EQ(RegisterActuatorPlugin(), kNumTruePlugins + kNumFakePlugins + 1);
EXPECT_EQ(RegisterPassivePlugin(), kNumTruePlugins + kNumFakePlugins + 2);
EXPECT_EQ(mjp_pluginCount(), kNumTruePlugins + kNumFakePlugins + kNumTestPlugins);
EXPECT_EQ(mjp_pluginCount(),
kNumTruePlugins + kNumFakePlugins + kNumTestPlugins);
}
TEST_F(EnginePluginTest, SaveXml) {
char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), testing::NotNull()) << error;
std::string saved_xml = SaveAndReadXml(m);
std::string saved_xml = SaveAndReadXml(m.get());
std::string_view expected_xml(xml);
const std::string extension_open = "<extension>";
@@ -561,12 +559,9 @@ TEST_F(EnginePluginTest, SaveXml) {
EXPECT_THAT(saved_xml, HasSubstr(expected_sensor_section));
EXPECT_THAT(saved_xml, HasSubstr(expected_actuator_section));
mj_deleteModel(m);
// make sure that the saved XML can still be compiled
mjModel* m2 = LoadModelFromString(saved_xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
mj_deleteModel(m2);
MjModelPtr m2 = LoadModelFromString(saved_xml, error, sizeof(error));
ASSERT_THAT(m.get(), testing::NotNull()) << error;
}
TEST_F(EnginePluginTest, SensorPlugin) {
@@ -575,8 +570,8 @@ TEST_F(EnginePluginTest, SensorPlugin) {
EXPECT_EQ(expected_init_count, expected_destroy_count);
char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), testing::NotNull()) << error;
// mj_makeModel calls mj_makeData and mj_deleteData internally
expected_init_count += 3;
@@ -585,10 +580,10 @@ TEST_F(EnginePluginTest, SensorPlugin) {
EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count);
EXPECT_EQ(m->nplugin, 7);
EXPECT_EQ(mj_name2id(m, mjOBJ_PLUGIN, "twosensors"), 0);
EXPECT_EQ(mj_name2id(m, mjOBJ_PLUGIN, "threesensors"), 1);
EXPECT_EQ(mj_name2id(m.get(), mjOBJ_PLUGIN, "twosensors"), 0);
EXPECT_EQ(mj_name2id(m.get(), mjOBJ_PLUGIN, "threesensors"), 1);
mjData* d = mj_makeData(m);
MjDataPtr d = MakeData(m);
expected_init_count += 3;
EXPECT_EQ(TestSensor::InitCount(), expected_init_count);
EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count);
@@ -597,32 +592,30 @@ TEST_F(EnginePluginTest, SensorPlugin) {
for (int j = 0; j < 10; ++j) {
EXPECT_THAT(*reinterpret_cast<mjtNum(*)[3]>(d->plugin_state +
m->plugin_stateadr[0]),
testing::ElementsAreArray<int>({i+1, 2*j, j}));
testing::ElementsAreArray<int>({i + 1, 2 * j, j}));
EXPECT_THAT(*reinterpret_cast<mjtNum(*)[3]>(d->plugin_state +
m->plugin_stateadr[1]),
testing::ElementsAreArray<int>({3*(i+1), 6*j, 3*j}));
testing::ElementsAreArray<int>({3 * (i + 1), 6 * j, 3 * j}));
EXPECT_THAT(*reinterpret_cast<mjtNum(*)[3]>(d->plugin_state +
m->plugin_stateadr[5]),
testing::ElementsAreArray<int>({5*(i+1), 10*j, 5*j}));
testing::ElementsAreArray<int>({5 * (i + 1), 10 * j, 5 * j}));
EXPECT_THAT(*reinterpret_cast<mjtNum(*)[18]>(d->sensordata),
testing::ElementsAreArray<int>({ i+1, 2*j, j,
5*(i+1), 10*j, 5*j,
3*(i+1), 6*j, 3*j,
i+1, 2*j, j,
3*(i+1), 6*j, 3*j,
3*(i+1), 6*j, 3*j}));
mj_step(m, d);
mj_forward(m, d);
testing::ElementsAreArray<int>(
{i + 1, 2 * j, j, 5 * (i + 1), 10 * j, 5 * j, 3 * (i + 1),
6 * j, 3 * j, i + 1, 2 * j, j, 3 * (i + 1), 6 * j, 3 * j,
3 * (i + 1), 6 * j, 3 * j}));
mj_step(m.get(), d.get());
mj_forward(m.get(), d.get());
}
mj_resetData(m, d);
mj_resetData(m.get(), d.get());
}
mj_deleteData(d);
d.reset();
m.reset();
expected_destroy_count += 3;
EXPECT_EQ(TestSensor::InitCount(), expected_init_count);
EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count);
mj_deleteModel(m);
EXPECT_EQ(TestSensor::InitCount(), expected_init_count);
EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count);
}
@@ -633,8 +626,8 @@ TEST_F(EnginePluginTest, ActuatorPlugin) {
EXPECT_EQ(expected_init_count, expected_destroy_count);
char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), testing::NotNull()) << error;
// mj_makeModel calls mj_makeData and mj_deleteData internally
expected_init_count += 2;
@@ -643,9 +636,9 @@ TEST_F(EnginePluginTest, ActuatorPlugin) {
EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count);
EXPECT_EQ(m->nplugin, 7);
EXPECT_EQ(mj_name2id(m, mjOBJ_PLUGIN, "actuator2"), 2);
EXPECT_EQ(mj_name2id(m.get(), mjOBJ_PLUGIN, "actuator2"), 2);
mjData* d = mj_makeData(m);
MjDataPtr d = MakeData(m);
expected_init_count += 2;
EXPECT_EQ(TestActuator::InitCount(), expected_init_count);
EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count);
@@ -654,33 +647,33 @@ TEST_F(EnginePluginTest, ActuatorPlugin) {
for (int j = 0; j < 10; ++j) {
EXPECT_THAT(*reinterpret_cast<mjtNum(*)[3]>(d->plugin_state +
m->plugin_stateadr[2]),
testing::ElementsAreArray<int>({2*(i+1), 4*j, 2*j}));
testing::ElementsAreArray<int>({2 * (i + 1), 4 * j, 2 * j}));
EXPECT_THAT(*reinterpret_cast<mjtNum(*)[3]>(d->plugin_state +
m->plugin_stateadr[3]),
testing::ElementsAreArray<int>({4*(i+1), 8*j, 4*j}));
testing::ElementsAreArray<int>({4 * (i + 1), 8 * j, 4 * j}));
EXPECT_THAT(*reinterpret_cast<mjtNum(*)[2]>(d->actuator_force),
testing::ElementsAreArray<mjtNum>(
{(mjtNum)0.125*j, (mjtNum)0.25*j}));
mj_step(m, d);
mj_forward(m, d);
{(mjtNum)0.125 * j, (mjtNum)0.25 * j}));
mj_step(m.get(), d.get());
mj_forward(m.get(), d.get());
}
mj_resetData(m, d);
mj_resetData(m.get(), d.get());
}
mj_deleteData(d);
d.reset();
m.reset();
expected_destroy_count += 2;
EXPECT_EQ(TestActuator::InitCount(), expected_init_count);
EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count);
mj_deleteModel(m);
EXPECT_EQ(TestActuator::InitCount(), expected_init_count);
EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count);
}
TEST_F(EnginePluginTest, FilteredActuatorPlugin) {
char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error;
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), testing::NotNull()) << error;
// Expecting 7 actuator state variables: 3x2 from actuator2 instances, and 1
// from setting dyntype="filter" on one of the plugin actuators
@@ -692,12 +685,12 @@ TEST_F(EnginePluginTest, FilteredActuatorPlugin) {
EXPECT_EQ(m->actuator_actadr[1], 0);
EXPECT_EQ(m->actuator_actadr[2], 3);
mjData* d = mj_makeData(m);
MjDataPtr d = MakeData(m);
EXPECT_EQ(d->act[0], 0.0);
mju_fill(d->ctrl, 1, m->nu);
// start with nonzero act for the filter
d->act[6] = 0.5;
mj_step(m, d);
mj_step(m.get(), d.get());
for (int i = 0; i < 6; ++i) {
// act_dot should be computed by the plugin
@@ -715,9 +708,6 @@ TEST_F(EnginePluginTest, FilteredActuatorPlugin) {
EXPECT_THAT(d->act_dot[6], MjNear(expected_act_dot, 1e-6, 1e-4));
EXPECT_THAT(d->act[6],
MjNear(0.5 + expected_act_dot * m->opt.timestep, 1e-6, 1e-4));
mj_deleteData(d);
mj_deleteModel(m);
}
} // namespace
test/engine/engine_print_test.cc
+8 -10
View File
@@ -24,7 +24,7 @@
#include "test/fixture.h"
#ifdef MEMORY_SANITIZER
#include <sanitizer/msan_interface.h>
#include <sanitizer/msan_interface.h>
#endif
namespace mujoco {
@@ -38,7 +38,6 @@ using ::testing::NotNull;
using EnginePrintTest = MujocoTest;
constexpr const char* NullFile() {
#ifdef _WIN32
return "NUL";
@@ -60,27 +59,26 @@ TEST_F(EnginePrintTest, PrintDataWorksWithMsan) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
mjData* data = mj_makeData(model);
MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull());
// Mark qacc_smooth[0] as initialized.
data->qacc_smooth[0] = 0;
// This will read "uninitialized" values from mjData, but shouldn't fail.
mj_printData(model, data, NullFile());
mj_printData(model.get(), data.get(), NullFile());
// After mj_printData, poisoned status should be restored correctly, so
// qacc_smooth[0] should be marked initialzed.
EXPECT_EQ(data->qacc_smooth[0], 0);
#ifdef MEMORY_SANITIZER
EXPECT_THAT(__msan_test_shadow(data->buffer, data->nbuffer), Not(Eq(-1))) <<
"Expecting some of data->buffer to be marked uninitialized";
EXPECT_THAT(__msan_test_shadow(data->buffer, data->nbuffer), Not(Eq(-1)))
<< "Expecting some of data->buffer to be marked uninitialized";
#endif
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
test/engine/engine_ray_test.cc
+129 -139
View File
@@ -14,6 +14,8 @@
// Tests for ray casting.
#include "src/engine/engine_ray.h"
#include <cstring>
#include <string>
@@ -23,7 +25,6 @@
#include <mujoco/mjmodel.h>
#include <mujoco/mjtype.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_ray.h"
#include "test/fixture.h"
namespace mujoco {
@@ -85,9 +86,10 @@ using RayTest = MujocoTest;
TEST_F(RayTest, NoExclusions) {
char error[1024];
mjModel* model = LoadModelFromString(kRayCastingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
MjModelPtr model =
LoadModelFromString(kRayCastingModel, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull());
mjtNum pnt[] = {0.0, 0.0, 0.0};
@@ -97,20 +99,19 @@ TEST_F(RayTest, NoExclusions) {
int bodyexclude = -1;
int geomid = -1;
mj_kinematics(model, data);
mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static,
bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "static_group1");
mj_kinematics(model.get(), data.get());
mjtNum distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup,
flg_static, bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "static_group1");
EXPECT_MJTNUM_EQ(distance, 0.9);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(RayTest, Exclusions) {
char error[1024];
mjModel* model = LoadModelFromString(kRayCastingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
MjModelPtr model =
LoadModelFromString(kRayCastingModel, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull());
mjtNum pnt[] = {0.0, 0.0, 0.0};
@@ -120,40 +121,38 @@ TEST_F(RayTest, Exclusions) {
int bodyexclude = -1;
int geomid = -1;
mj_kinematics(model, data);
mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static,
bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "static_group1");
mj_kinematics(model.get(), data.get());
mjtNum distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup,
flg_static, bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "static_group1");
EXPECT_NEAR(distance, 0.9, MjTol(1e-12, 1e-5));
// Exclude nearest geom
geomgroup[1] = 0;
distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude,
&geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group0");
distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup, flg_static,
bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "group0");
EXPECT_NEAR(distance, 2.9, MjTol(1e-12, 1e-5));
geomgroup[0] = 0;
distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude,
&geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group2");
distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup, flg_static,
bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "group2");
EXPECT_NEAR(distance, 4.9, MjTol(1e-12, 1e-5));
geomgroup[2] = 0;
distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude,
&geomid, nullptr);
distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup, flg_static,
bodyexclude, &geomid, nullptr);
EXPECT_EQ(geomid, -1);
EXPECT_NEAR(distance, -1, MjTol(1e-12, 1e-5));
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(RayTest, ExcludeStatic) {
char error[1024];
mjModel* model = LoadModelFromString(kRayCastingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
MjModelPtr model =
LoadModelFromString(kRayCastingModel, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull());
mjtNum pnt[] = {0.0, 0.0, 0.0};
@@ -163,38 +162,36 @@ TEST_F(RayTest, ExcludeStatic) {
int bodyexclude = -1;
int geomid = -1;
mj_kinematics(model, data);
mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static,
bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group0");
mj_kinematics(model.get(), data.get());
mjtNum distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup,
flg_static, bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "group0");
EXPECT_NEAR(distance, 2.9, MjTol(1e-12, 1e-5));
mj_deleteData(data);
mj_deleteModel(model);
}
// ------------------------------- mj_multiRay --------------------------------
TEST_F(RayTest, MultiRayEqualsSingleRay) {
char error[1024];
mjModel* m = LoadModelFromString(kRayCastingModel, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(kRayCastingModel, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull());
mj_forward(m, d);
mj_forward(m.get(), d.get());
// create ray array
constexpr int N = 80;
constexpr int M = 60;
mjtNum vec[3*N*M];
mjtNum vec[3 * N * M];
mjtNum pnt[3] = {-1, 0, 0};
mjtNum cone[4][3] = {{1, .2, -.2}, {1, .2, .2}, {1, -.2, -.2}, {1, -.2, .2}};
memset(vec, 0, 3*N*M*sizeof(mjtNum));
memset(vec, 0, 3 * N * M * sizeof(mjtNum));
for (int i = 0; i < N; ++i) {
for (int j = 0; j < M; ++j) {
for (int k = 0; k < 3; ++k) {
vec[3 * (i * M + j) + k] = i * cone[0][k] / (N - 1) +
j * cone[1][1] / (M - 1) +
vec[3 * (i * M + j) + k] = i * cone[0][k] / (N - 1) +
j * cone[1][1] / (M - 1) +
(N - i - 1) * cone[2][k] / (N - 1) +
(M - j - 1) * cone[3][k] / (M - 1);
}
@@ -202,10 +199,10 @@ TEST_F(RayTest, MultiRayEqualsSingleRay) {
}
// compute intersections with multiray functions
mjtNum dist_multiray[N*M];
int rgeomid_multiray[N*M];
mj_multiRay(m, d, pnt, vec, NULL, 1, -1, rgeomid_multiray, dist_multiray,
nullptr, N * M, mjMAXVAL);
mjtNum dist_multiray[N * M];
int rgeomid_multiray[N * M];
mj_multiRay(m.get(), d.get(), pnt, vec, NULL, 1, -1, rgeomid_multiray,
dist_multiray, nullptr, N * M, mjMAXVAL);
// compare results with single ray function
mjtNum dist;
@@ -214,39 +211,37 @@ TEST_F(RayTest, MultiRayEqualsSingleRay) {
for (int i = 0; i < N; ++i) {
for (int j = 0; j < M; ++j) {
int idx = i * M + j;
dist = mj_ray(m, d, pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid, nullptr);
dist = mj_ray(m.get(), d.get(), pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid,
nullptr);
EXPECT_MJTNUM_EQ(dist, dist_multiray[idx]);
EXPECT_EQ(rgeomid, rgeomid_multiray[idx]);
nhits += dist >= 0;
}
}
EXPECT_GT(nhits, 10);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) {
char error[1024];
mjModel* m = LoadModelFromString(kRayCastingModel, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(kRayCastingModel, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull());
mj_forward(m, d);
mj_forward(m.get(), d.get());
// create ray array
constexpr int N = 80;
constexpr int M = 60;
mjtNum vec[3*N*M];
mjtNum vec[3 * N * M];
mjtNum pnt[3] = {-1, 0, 0};
mjtNum cone[4][3] = {{1, .2, -.2}, {1, .2, .2}, {1, -.2, -.2}, {1, -.2, .2}};
memset(vec, 0, 3*N*M*sizeof(mjtNum));
memset(vec, 0, 3 * N * M * sizeof(mjtNum));
for (int i = 0; i < N; ++i) {
for (int j = 0; j < M; ++j) {
for (int k = 0; k < 3; ++k) {
vec[3 * (i * M + j) + k] = i * cone[0][k] / (N - 1) +
j * cone[1][1] / (M - 1) +
vec[3 * (i * M + j) + k] = i * cone[0][k] / (N - 1) +
j * cone[1][1] / (M - 1) +
(N - i - 1) * cone[2][k] / (N - 1) +
(M - j - 1) * cone[3][k] / (M - 1);
}
@@ -254,10 +249,10 @@ TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) {
}
// compute intersections with multiray normal function
mjtNum dist_multiray[N*M];
int rgeomid_multiray[N*M];
mjtNum normal_multiray[3*N*M];
mj_multiRay(m, d, pnt, vec, NULL, 1, -1, rgeomid_multiray,
mjtNum dist_multiray[N * M];
int rgeomid_multiray[N * M];
mjtNum normal_multiray[3 * N * M];
mj_multiRay(m.get(), d.get(), pnt, vec, NULL, 1, -1, rgeomid_multiray,
dist_multiray, normal_multiray, N * M, mjMAXVAL);
// compare results with single ray normal function
@@ -268,30 +263,27 @@ TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) {
for (int i = 0; i < N; ++i) {
for (int j = 0; j < M; ++j) {
int idx = i * M + j;
dist = mj_ray(m, d, pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid,
dist = mj_ray(m.get(), d.get(), pnt, vec + 3 * idx, NULL, 1, -1, &rgeomid,
normal);
EXPECT_MJTNUM_EQ(dist, dist_multiray[idx]);
EXPECT_EQ(rgeomid, rgeomid_multiray[idx]);
EXPECT_MJTNUM_EQ(normal[0], normal_multiray[3*idx]);
EXPECT_MJTNUM_EQ(normal[1], normal_multiray[3*idx + 1]);
EXPECT_MJTNUM_EQ(normal[2], normal_multiray[3*idx + 2]);
EXPECT_MJTNUM_EQ(normal[0], normal_multiray[3 * idx]);
EXPECT_MJTNUM_EQ(normal[1], normal_multiray[3 * idx + 1]);
EXPECT_MJTNUM_EQ(normal[2], normal_multiray[3 * idx + 2]);
nhits += dist >= 0;
}
}
EXPECT_GT(nhits, 10);
mj_deleteData(d);
mj_deleteModel(m);
}
TEST_F(RayTest, EdgeCases) {
char error[1024];
mjModel* m = LoadModelFromString(kSingleGeomModel, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
MjModelPtr m = LoadModelFromString(kSingleGeomModel, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
ASSERT_THAT(m->nbvh, 1);
mjData* d = mj_makeData(m);
MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull());
mj_forward(m, d);
mj_forward(m.get(), d.get());
// spherical bounding box and result arrays
mjtNum geom_ba[4];
@@ -301,51 +293,57 @@ TEST_F(RayTest, EdgeCases) {
// pnt contained in bounding box
mjtNum pnt1[] = {-1, 0, 0};
mju_multiRayPrepare(m, d, pnt1, NULL, NULL, 1, -1, mjMAXVAL, geom_ba, flags);
mju_multiRayPrepare(m.get(), d.get(), pnt1, NULL, NULL, 1, -1, mjMAXVAL,
geom_ba, flags);
EXPECT_MJTNUM_EQ(geom_ba[0], -mjPI);
EXPECT_MJTNUM_EQ(geom_ba[1], 0);
EXPECT_MJTNUM_EQ(geom_ba[2], mjPI);
EXPECT_MJTNUM_EQ(geom_ba[3], mjPI);
EXPECT_MJTNUM_EQ(geom_ba[1], 0);
EXPECT_MJTNUM_EQ(geom_ba[2], mjPI);
EXPECT_MJTNUM_EQ(geom_ba[3], mjPI);
mjtNum vec1[] = {1, 0, 0};
mj_multiRay(m, d, pnt1, vec1, NULL, 1, -1, &rgeomid, &dist, nullptr, 1,
mjMAXVAL);
mj_multiRay(m.get(), d.get(), pnt1, vec1, NULL, 1, -1, &rgeomid, &dist,
nullptr, 1, mjMAXVAL);
EXPECT_FLOAT_EQ(dist, 0.1);
// pnt at phi = Pi, -Pi
mjtNum pnt2[] = {-.5, 0, 0};
mju_multiRayPrepare(m, d, pnt2, NULL, NULL, 1, -1, mjMAXVAL, geom_ba, flags);
mju_multiRayPrepare(m.get(), d.get(), pnt2, NULL, NULL, 1, -1, mjMAXVAL,
geom_ba, flags);
EXPECT_FLOAT_EQ(geom_ba[0], -mjPI); // atan(y<0, x<0)
EXPECT_FLOAT_EQ(geom_ba[2], mjPI); // atan(y>0, x<0)
EXPECT_FLOAT_EQ(geom_ba[2], mjPI); // atan(y>0, x<0)
mjtNum vec2[] = {-1, 0, 0};
mj_multiRay(m, d, pnt2, vec2, NULL, 1, -1, &rgeomid, &dist, nullptr, 1,
mjMAXVAL);
mj_multiRay(m.get(), d.get(), pnt2, vec2, NULL, 1, -1, &rgeomid, &dist,
nullptr, 1, mjMAXVAL);
EXPECT_FLOAT_EQ(dist, 0.4);
// with cutoff
mjtNum cutoff1 = 0.41, cutoff2 = 0.39;
mju_multiRayPrepare(m, d, pnt2, NULL, NULL, 1, -1, cutoff1, geom_ba, flags);
mju_multiRayPrepare(m.get(), d.get(), pnt2, NULL, NULL, 1, -1, cutoff1,
geom_ba, flags);
EXPECT_EQ(flags[0], 0);
mju_multiRayPrepare(m, d, pnt2, NULL, NULL, 1, -1, cutoff2, geom_ba, flags);
mju_multiRayPrepare(m.get(), d.get(), pnt2, NULL, NULL, 1, -1, cutoff2,
geom_ba, flags);
EXPECT_EQ(flags[0], 1);
mj_multiRay(m, d, pnt2, vec2, NULL, 1, -1, &rgeomid, &dist, nullptr, 1,
cutoff2);
mj_multiRay(m.get(), d.get(), pnt2, vec2, NULL, 1, -1, &rgeomid, &dist,
nullptr, 1, cutoff2);
EXPECT_FLOAT_EQ(dist, -1);
// pnt on the boundary of the box
mjtNum pnt3[] = {.1, .1, .05};
mju_multiRayPrepare(m, d, pnt3, NULL, NULL, 1, -1, mjMAXVAL, geom_ba, flags);
mju_multiRayPrepare(m.get(), d.get(), pnt3, NULL, NULL, 1, -1, mjMAXVAL,
geom_ba, flags);
EXPECT_FLOAT_EQ(geom_ba[1], 0);
EXPECT_FLOAT_EQ(geom_ba[3], mjPI);
mjtNum vec3[] = {1, 1, 0};
mj_multiRay(m, d, pnt3, vec3, NULL, 1, -1, &rgeomid, &dist, nullptr, 1,
mjMAXVAL);
mj_multiRay(m.get(), d.get(), pnt3, vec3, NULL, 1, -1, &rgeomid, &dist,
nullptr, 1, mjMAXVAL);
EXPECT_FLOAT_EQ(dist, -1);
// size 0 geom
mjtNum pnt4[] = {-2, 0, 0};
m->geom_aabb[0] = m->geom_aabb[1] = m->geom_aabb[2] = 0;
m->geom_aabb[3] = m->geom_aabb[4] = m->geom_aabb[5] = 0;
mju_multiRayPrepare(m, d, pnt4, NULL, NULL, 1, -1, mjMAXVAL, geom_ba, flags);
mju_multiRayPrepare(m.get(), d.get(), pnt4, NULL, NULL, 1, -1, mjMAXVAL,
geom_ba, flags);
// margin = atan(max_half / dist) where max_half = max(aabb[3..5])
// For a zero-size AABB: max_half = 0, so margin = 0
mjtNum dist4 = mju_dist3(pnt4, d->geom_xpos);
@@ -353,16 +351,13 @@ TEST_F(RayTest, EdgeCases) {
mju_max(m->geom_aabb[3], mju_max(m->geom_aabb[4], m->geom_aabb[5]));
mjtNum margin4 = mju_atan2(max_half4, dist4);
EXPECT_NEAR(geom_ba[0], 0 - margin4, 1e-6);
EXPECT_NEAR(geom_ba[1], mjPI/2 - margin4, 1e-6);
EXPECT_NEAR(geom_ba[1], mjPI / 2 - margin4, 1e-6);
EXPECT_NEAR(geom_ba[2], 0 + margin4, 1e-6);
EXPECT_NEAR(geom_ba[3], mjPI/2 + margin4, 1e-6);
EXPECT_NEAR(geom_ba[3], mjPI / 2 + margin4, 1e-6);
mjtNum vec4[] = {1, 0, 0};
mj_multiRay(m, d, pnt4, vec4, NULL, 1, -1, &rgeomid, &dist, nullptr, 1,
mjMAXVAL);
mj_multiRay(m.get(), d.get(), pnt4, vec4, NULL, 1, -1, &rgeomid, &dist,
nullptr, 1, mjMAXVAL);
EXPECT_FLOAT_EQ(dist, 0.9);
mj_deleteData(d);
mj_deleteModel(m);
}
// ------------------------------- mj_rayMesh ---------------------------------
@@ -377,10 +372,10 @@ mjtNum _rayMesh(const mjModel* m, const mjData* d, int geomid,
// map to local frame
mjtNum lpnt[3], lvec[3];
const mjtNum* pos = d->geom_xpos+3*geomid;
const mjtNum dif[3] = {pnt[0]-pos[0], pnt[1]-pos[1], pnt[2]-pos[2]};
mju_mulMatTVec3(lpnt, d->geom_xmat+9*geomid, dif);
mju_mulMatTVec3(lvec, d->geom_xmat+9*geomid, vec);
const mjtNum* pos = d->geom_xpos + 3 * geomid;
const mjtNum dif[3] = {pnt[0] - pos[0], pnt[1] - pos[1], pnt[2] - pos[2]};
mju_mulMatTVec3(lpnt, d->geom_xmat + 9 * geomid, dif);
mju_mulMatTVec3(lvec, d->geom_xmat + 9 * geomid, vec);
// construct basis vectors of normal plane
mjtNum b0[3] = {1, 1, 1}, b1[3];
@@ -392,7 +387,7 @@ mjtNum _rayMesh(const mjModel* m, const mjData* d, int geomid,
} else {
b0[2] = 0;
}
mju_addScl3(b1, b0, lvec, -mju_dot3(lvec, b0)/mju_dot3(lvec, lvec));
mju_addScl3(b1, b0, lvec, -mju_dot3(lvec, b0) / mju_dot3(lvec, lvec));
mju_normalize3(b1);
mju_cross(b0, b1, lvec);
mju_normalize3(b0);
@@ -403,18 +398,20 @@ mjtNum _rayMesh(const mjModel* m, const mjData* d, int geomid,
// process all triangles
int face, meshid = m->geom_dataid[geomid];
for (face = m->mesh_faceadr[meshid];
face < m->mesh_faceadr[meshid] + m->mesh_facenum[meshid];
face++) {
face < m->mesh_faceadr[meshid] + m->mesh_facenum[meshid]; face++) {
// get float vertices
float* vf[3];
vf[0] = m->mesh_vert + 3*(m->mesh_face[3*face] + m->mesh_vertadr[meshid]);
vf[1] = m->mesh_vert + 3*(m->mesh_face[3*face+1] + m->mesh_vertadr[meshid]);
vf[2] = m->mesh_vert + 3*(m->mesh_face[3*face+2] + m->mesh_vertadr[meshid]);
vf[0] =
m->mesh_vert + 3 * (m->mesh_face[3 * face] + m->mesh_vertadr[meshid]);
vf[1] = m->mesh_vert +
3 * (m->mesh_face[3 * face + 1] + m->mesh_vertadr[meshid]);
vf[2] = m->mesh_vert +
3 * (m->mesh_face[3 * face + 2] + m->mesh_vertadr[meshid]);
// convert to mjtNum
mjtNum v[3][3];
for (int i=0; i < 3; i++) {
for (int j=0; j < 3; j++) {
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
v[i][j] = (mjtNum)vf[i][j];
}
}
@@ -440,16 +437,16 @@ void _rayMeshTest(const mjModel* m) {
// create ray array
constexpr int N = 80;
constexpr int M = 60;
mjtNum vec[3*N*M];
mjtNum vec[3 * N * M];
mjtNum pnt[3] = {1, .2, 0};
mjtNum cone[4][3] = {{-1, -1, -1}, {-1, -1, 1}, {1, -1, 1}, {1, -1, -1}};
memset(vec, 0, 3*N*M*sizeof(mjtNum));
memset(vec, 0, 3 * N * M * sizeof(mjtNum));
for (int i = 0; i < N; ++i) {
for (int j = 0; j < M; ++j) {
for (int k = 0; k < 3; ++k) {
vec[3 * (i * M + j) + k] = i * cone[0][k] / (N - 1) +
j * cone[1][1] / (M - 1) +
vec[3 * (i * M + j) + k] = i * cone[0][k] / (N - 1) +
j * cone[1][1] / (M - 1) +
(N - i - 1) * cone[2][k] / (N - 1) +
(M - j - 1) * cone[3][k] / (M - 1);
}
@@ -466,7 +463,6 @@ void _rayMeshTest(const mjModel* m) {
EXPECT_FLOAT_EQ(dist_new, dist_old);
}
}
mj_deleteData(d);
}
@@ -478,15 +474,13 @@ TEST_F(RayTest, RayMeshPruning) {
const string xml_path =
GetTestDataFilePath("engine/testdata/ray/stanford_bunny.xml");
mjModel* m = mj_loadXML(xml_path.c_str(), NULL, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
_rayMeshTest(m);
mj_deleteModel(m);
MjModelPtr m(mj_loadXML(xml_path.c_str(), NULL, error, sizeof(error)));
ASSERT_THAT(m.get(), NotNull()) << error;
_rayMeshTest(m.get());
m = LoadModelFromString(kCubeletModel, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
_rayMeshTest(m);
mj_deleteModel(m);
ASSERT_THAT(m.get(), NotNull()) << error;
_rayMeshTest(m.get());
}
TEST_F(RayTest, RayHfield) {
@@ -523,21 +517,17 @@ TEST_F(RayTest, RayHfield) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mj_forward(model, data);
mj_forward(model.get(), data.get());
EXPECT_THAT(data->sensordata[0], MjNear(1, 1e-8, 1e-5));
EXPECT_THAT(data->sensordata[1], MjNear(1, 1e-8, 1e-5));
EXPECT_THAT(data->sensordata[2], MjNear(1, 1e-8, 1e-5));
EXPECT_THAT(data->sensordata[3], MjNear(0.5, 1e-8, 1e-5));
mj_deleteData(data);
mj_deleteModel(model);
}
static const char* const kPlaneModel = "engine/testdata/ray/plane.xml";
@@ -603,8 +593,8 @@ TEST_F(RayTest, RayNormal) {
} else {
r = mj_rayFlex(m, d, /*flex_layer*/ 0, /*flg_vert*/ 1,
/*flg_edge*/ 1, /*flg_face*/ 1,
/*flg_skin*/ 1, /*flex_id*/ 0,
pnt, vec, nullptr, normal);
/*flg_skin*/ 1, /*flex_id*/ 0, pnt, vec, nullptr,
normal);
// no sensor comparison: rangefinders only intersect with geoms
}
@@ -623,8 +613,7 @@ TEST_F(RayTest, RayNormal) {
mjtNum eps = 1e-6;
mjtNum ds[2][3];
for (int i = 0; i < 2; ++i) {
mjtNum nudge[3] = {d->site_xmat[0 + i],
d->site_xmat[3 + i],
mjtNum nudge[3] = {d->site_xmat[0 + i], d->site_xmat[3 + i],
d->site_xmat[6 + i]};
mjtNum dr, dpnt[3];
mju_addScl3(dpnt, pnt, nudge, eps);
@@ -650,8 +639,9 @@ TEST_F(RayTest, RayNormal) {
mjtNum expected_neg[3] = {-expected[0], -expected[1], -expected[2]};
// compare analytic with fin-diff approximation
EXPECT_THAT(normal, AnyOf(Pointwise(MjNear(100*eps, 1e-3), expected),
Pointwise(MjNear(100*eps, 1e-3), expected_neg)))
EXPECT_THAT(normal,
AnyOf(Pointwise(MjNear(100 * eps, 1e-3), expected),
Pointwise(MjNear(100 * eps, 1e-3), expected_neg)))
<< path << ", time " << d->time;
// increment count
test/engine/engine_sensor_test.cc
+221 -314
View File
File diff suppressed because it is too large Load Diff
test/engine/engine_setconst_test.cc
+26 -43
View File
@@ -53,7 +53,7 @@ TEST_F(SetConstTest, AwakeActuatedJoint) {
</mujoco>
)";
char error[1024];
mjModel* m;
MjModelPtr m;
string sleep[] = {"auto", "never", "allowed", "init"};
int tsp0[] = {mjSLEEP_AUTO_NEVER, mjSLEEP_NEVER, mjSLEEP_ALLOWED,
@@ -69,10 +69,9 @@ TEST_F(SetConstTest, AwakeActuatedJoint) {
size_t pos2 = xml_copy.find("POLICY2");
xml_copy.replace(pos2, 7, sleep[j]);
m = LoadModelFromString(xml_copy.c_str(), error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
ASSERT_THAT(m.get(), NotNull()) << error;
EXPECT_EQ(m->tree_sleep_policy[0], tsp0[i]);
EXPECT_EQ(m->tree_sleep_policy[1], tsp1[j]);
mj_deleteModel(m);
}
}
}
@@ -97,13 +96,11 @@ TEST_F(SetConstTest, AwakeActuatedSite) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER);
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED);
mj_deleteModel(model);
}
TEST_F(SetConstTest, AwakeActuatedBody) {
@@ -125,13 +122,11 @@ TEST_F(SetConstTest, AwakeActuatedBody) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER);
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED);
mj_deleteModel(model);
}
TEST_F(SetConstTest, AwakeActuatedTendon) {
@@ -161,13 +156,11 @@ TEST_F(SetConstTest, AwakeActuatedTendon) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER);
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED);
mj_deleteModel(model);
}
TEST_F(SetConstTest, AwakeStiffTendonMultiTree) {
@@ -194,13 +187,11 @@ TEST_F(SetConstTest, AwakeStiffTendonMultiTree) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER);
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_NEVER);
mj_deleteModel(model);
}
TEST_F(SetConstTest, SleepyTendonSingleTree) {
@@ -227,12 +218,10 @@ TEST_F(SetConstTest, SleepyTendonSingleTree) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_ALLOWED);
mj_deleteModel(model);
}
TEST_F(SetConstTest, SleepyTendonZeroStiffness) {
@@ -259,13 +248,11 @@ TEST_F(SetConstTest, SleepyTendonZeroStiffness) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_ALLOWED);
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED);
mj_deleteModel(model);
}
TEST_F(SetConstTest, TendonTreeId) {
@@ -336,16 +323,16 @@ TEST_F(SetConstTest, TendonTreeId) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
int t_static_id = mj_name2id(model, mjOBJ_TENDON, "T_static");
int t_tree1_id = mj_name2id(model, mjOBJ_TENDON, "T_tree1");
int t_intertree12_id = mj_name2id(model, mjOBJ_TENDON, "T_intertree12");
int t_intertree123_id = mj_name2id(model, mjOBJ_TENDON, "T_intertree123");
int t_static_id = mj_name2id(model.get(), mjOBJ_TENDON, "T_static");
int t_tree1_id = mj_name2id(model.get(), mjOBJ_TENDON, "T_tree1");
int t_intertree12_id = mj_name2id(model.get(), mjOBJ_TENDON, "T_intertree12");
int t_intertree123_id = mj_name2id(model.get(), mjOBJ_TENDON, "T_intertree123");
int b1_1_treeid = model->body_treeid[mj_name2id(model, mjOBJ_BODY, "B1_1")];
int b2_1_treeid = model->body_treeid[mj_name2id(model, mjOBJ_BODY, "B2_1")];
int b1_1_treeid = model->body_treeid[mj_name2id(model.get(), mjOBJ_BODY, "B1_1")];
int b2_1_treeid = model->body_treeid[mj_name2id(model.get(), mjOBJ_BODY, "B2_1")];
// Tendon 1: Not associated with any tree
EXPECT_EQ(model->tendon_treenum[t_static_id], 0);
@@ -368,8 +355,6 @@ TEST_F(SetConstTest, TendonTreeId) {
EXPECT_EQ(model->tendon_treeid[2*t_intertree123_id], b1_1_treeid);
EXPECT_EQ(model->tendon_treeid[2*t_intertree123_id+1], b2_1_treeid);
// The third tree ID is not stored in tendon_treeid
mj_deleteModel(model);
}
TEST_F(SetConstTest, SleepingNotAllowed) {
@@ -415,8 +400,8 @@ TEST_F(SetConstTest, SleepingNotAllowed) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
EXPECT_THAT(model, IsNull()) << error;
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
EXPECT_THAT(model.get(), IsNull()) << error;
EXPECT_THAT(string(error), HasSubstr(
"tree 1 connected to tendon 0 which spans more than 2 trees, "
"sleeping not allowed"));
@@ -447,8 +432,8 @@ TEST_F(SetConstTest, DofLength) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
mjtNum tol = 1e-5;
@@ -470,8 +455,6 @@ TEST_F(SetConstTest, DofLength) {
EXPECT_NEAR(model->dof_length[8], 5, tol);
EXPECT_NEAR(model->dof_length[9], 5, tol);
EXPECT_NEAR(model->dof_length[10], 5, tol);
mj_deleteModel(model);
}
} // namespace
test/engine/engine_sleep_test.cc
+72 -107
View File
@@ -14,26 +14,27 @@
// Tests for engine/engine_sleep.c.
#include "src/engine/engine_sleep.h"
#include <string>
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <gtest/gtest-spi.h>
#include <gtest/gtest.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_sleep.h"
#include "test/fixture.h"
namespace mujoco {
namespace {
using ::testing::ElementsAre;
using ::testing::IsNull;
using ::testing::HasSubstr;
using ::testing::NotNull;
using ::std::string;
using ::std::vector;
using ::testing::ElementsAre;
using ::testing::HasSubstr;
using ::testing::IsNull;
using ::testing::NotNull;
using SleepTest = MujocoTest;
@@ -72,13 +73,13 @@ static constexpr char kSimple[] = R"(
</mujoco>
)";
static constexpr int kAwake = -(1+mjMINAWAKE);
static constexpr int kAwake = -(1 + mjMINAWAKE);
TEST_F(SleepTest, MjSleepUpdate) {
char error[1024];
mjModel* m = LoadModelFromString(kSimple, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(kSimple, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
// ntree = 2, nbody = 5, nv = 5, njnt = 3, ngeom = 6
// body 0: world, 1 geom
@@ -93,96 +94,69 @@ TEST_F(SleepTest, MjSleepUpdate) {
EXPECT_EQ(m->njnt, 3);
EXPECT_EQ(m->ngeom, 6);
EXPECT_THAT(AsVector(m->body_treeid, m->nbody),
ElementsAre(-1, 0, -1, 1, 1));
EXPECT_THAT(AsVector(m->dof_bodyid, m->nv),
ElementsAre(1, 1, 1, 3, 4));
EXPECT_THAT(AsVector(m->body_treeid, m->nbody), ElementsAre(-1, 0, -1, 1, 1));
EXPECT_THAT(AsVector(m->dof_bodyid, m->nv), ElementsAre(1, 1, 1, 3, 4));
EXPECT_THAT(AsVector(m->geom_bodyid, m->ngeom),
ElementsAre(0, 1, 2, 3, 3, 4));
EXPECT_THAT(AsVector(m->jnt_bodyid, m->njnt),
ElementsAre(1, 3, 4));
EXPECT_THAT(AsVector(m->jnt_bodyid, m->njnt), ElementsAre(1, 3, 4));
// Test Case 1: Initial state
EXPECT_THAT(AsVector(d->tree_asleep, m->ntree),
ElementsAre(kAwake, kAwake));
EXPECT_THAT(AsVector(d->tree_asleep, m->ntree), ElementsAre(kAwake, kAwake));
EXPECT_EQ(d->ntree_awake, 2);
EXPECT_EQ(d->nv_awake, 5);
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake),
ElementsAre(0, 1, 2, 3, 4));
EXPECT_THAT(AsVector(d->tree_awake, m->ntree),
ElementsAre(1, 1));
EXPECT_THAT(AsVector(d->body_awake, m->nbody),
ElementsAre(mjS_STATIC,
mjS_AWAKE,
mjS_STATIC,
mjS_AWAKE,
mjS_AWAKE));
EXPECT_THAT(AsVector(d->tree_awake, m->ntree), ElementsAre(1, 1));
EXPECT_THAT(
AsVector(d->body_awake, m->nbody),
ElementsAre(mjS_STATIC, mjS_AWAKE, mjS_STATIC, mjS_AWAKE, mjS_AWAKE));
// Test Case 2: Call mj_sleepUpdate, expect no changes
mj_updateSleep(m, d);
mj_updateSleep(m.get(), d.get());
EXPECT_EQ(d->ntree_awake, 2);
EXPECT_EQ(d->nv_awake, 5);
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake),
ElementsAre(0, 1, 2, 3, 4));
EXPECT_THAT(AsVector(d->body_awake, m->nbody),
ElementsAre(mjS_STATIC,
mjS_AWAKE,
mjS_STATIC,
mjS_AWAKE,
mjS_AWAKE));
EXPECT_THAT(AsVector(d->tree_awake, m->ntree),
ElementsAre(1, 1));
EXPECT_THAT(
AsVector(d->body_awake, m->nbody),
ElementsAre(mjS_STATIC, mjS_AWAKE, mjS_STATIC, mjS_AWAKE, mjS_AWAKE));
EXPECT_THAT(AsVector(d->tree_awake, m->ntree), ElementsAre(1, 1));
// Test Case 3: Tree 0 asleep
d->tree_asleep[0] = 0; d->tree_asleep[1] = -1;
mj_updateSleep(m, d);
d->tree_asleep[0] = 0;
d->tree_asleep[1] = -1;
mj_updateSleep(m.get(), d.get());
EXPECT_EQ(d->ntree_awake, 1);
EXPECT_EQ(d->nv_awake, 2);
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake),
ElementsAre(3, 4));
EXPECT_THAT(AsVector(d->tree_awake, m->ntree),
ElementsAre(0, 1));
EXPECT_THAT(AsVector(d->body_awake, m->nbody),
ElementsAre(mjS_STATIC,
mjS_ASLEEP,
mjS_STATIC,
mjS_AWAKE,
mjS_AWAKE));
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), ElementsAre(3, 4));
EXPECT_THAT(AsVector(d->tree_awake, m->ntree), ElementsAre(0, 1));
EXPECT_THAT(
AsVector(d->body_awake, m->nbody),
ElementsAre(mjS_STATIC, mjS_ASLEEP, mjS_STATIC, mjS_AWAKE, mjS_AWAKE));
// Test Case 4: Tree 1 asleep
d->tree_asleep[0] = -1; d->tree_asleep[1] = 1;
mj_updateSleep(m, d);
d->tree_asleep[0] = -1;
d->tree_asleep[1] = 1;
mj_updateSleep(m.get(), d.get());
EXPECT_EQ(d->ntree_awake, 1);
EXPECT_EQ(d->nv_awake, 3);
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake),
ElementsAre(0, 1, 2));
EXPECT_THAT(AsVector(d->tree_awake, m->ntree),
ElementsAre(1, 0));
EXPECT_THAT(AsVector(d->body_awake, m->nbody),
ElementsAre(mjS_STATIC,
mjS_AWAKE,
mjS_STATIC,
mjS_ASLEEP,
mjS_ASLEEP));
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), ElementsAre(0, 1, 2));
EXPECT_THAT(AsVector(d->tree_awake, m->ntree), ElementsAre(1, 0));
EXPECT_THAT(
AsVector(d->body_awake, m->nbody),
ElementsAre(mjS_STATIC, mjS_AWAKE, mjS_STATIC, mjS_ASLEEP, mjS_ASLEEP));
// Test Case 5: All trees asleep
d->tree_asleep[0] = 0; d->tree_asleep[1] = 1;
mj_updateSleep(m, d);
d->tree_asleep[0] = 0;
d->tree_asleep[1] = 1;
mj_updateSleep(m.get(), d.get());
EXPECT_EQ(d->ntree_awake, 0);
EXPECT_EQ(d->nv_awake, 0);
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake),
ElementsAre());
EXPECT_THAT(AsVector(d->tree_awake, m->ntree),
ElementsAre(0, 0));
EXPECT_THAT(AsVector(d->body_awake, m->nbody),
ElementsAre(mjS_STATIC,
mjS_ASLEEP,
mjS_STATIC,
mjS_ASLEEP,
mjS_ASLEEP));
mj_deleteData(d);
mj_deleteModel(m);
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), ElementsAre());
EXPECT_THAT(AsVector(d->tree_awake, m->ntree), ElementsAre(0, 0));
EXPECT_THAT(
AsVector(d->body_awake, m->nbody),
ElementsAre(mjS_STATIC, mjS_ASLEEP, mjS_STATIC, mjS_ASLEEP, mjS_ASLEEP));
}
TEST_F(SleepTest, MjWakeIsland) {
@@ -191,34 +165,29 @@ TEST_F(SleepTest, MjWakeIsland) {
EXPECT_EQ(mj_wakeIsland(asleep, 4, 0, kAwake, nullptr, 0), 0);
EXPECT_THAT(AsVector(asleep, 4), ElementsAre(kAwake, 2, 1, 3));
EXPECT_EQ(mj_wakeIsland(asleep, 4, 1, kAwake, nullptr, 0), 2);
EXPECT_THAT(AsVector(asleep, 4),
ElementsAre(kAwake, kAwake, kAwake, 3));
EXPECT_THAT(AsVector(asleep, 4), ElementsAre(kAwake, kAwake, kAwake, 3));
EXPECT_EQ(mj_wakeIsland(asleep, 4, 3, kAwake, nullptr, 0), 1);
EXPECT_THAT(AsVector(asleep, 4),
ElementsAre(kAwake, kAwake, kAwake, kAwake));
EXPECT_THAT(AsVector(asleep, 4), ElementsAre(kAwake, kAwake, kAwake, kAwake));
}
TEST_F(SleepTest, BadWakeIsland) {
EXPECT_FATAL_FAILURE(
([] {
int asleep_bad1[] = {-1, 0};
mj_wakeIsland(asleep_bad1, 2, 1, kAwake, nullptr, 0);
}()),
"invalid sleep state index -1 when waking tree 1");
EXPECT_FATAL_FAILURE(([] {
int asleep_bad1[] = {-1, 0};
mj_wakeIsland(asleep_bad1, 2, 1, kAwake, nullptr, 0);
}()),
"invalid sleep state index -1 when waking tree 1");
EXPECT_FATAL_FAILURE(
([] {
int asleep_bad2[] = {-1, 2};
mj_wakeIsland(asleep_bad2, 2, 1, kAwake, nullptr, 0);
}()),
"invalid sleep state index 2 when waking tree 1");
EXPECT_FATAL_FAILURE(([] {
int asleep_bad2[] = {-1, 2};
mj_wakeIsland(asleep_bad2, 2, 1, kAwake, nullptr, 0);
}()),
"invalid sleep state index 2 when waking tree 1");
EXPECT_FATAL_FAILURE(
([] {
int asleep_bad3[] = {1, 2, 1};
mj_wakeIsland(asleep_bad3, 3, 0, kAwake, nullptr, 0);
}()),
"tree 0 is not in a cycle");
EXPECT_FATAL_FAILURE(([] {
int asleep_bad3[] = {1, 2, 1};
mj_wakeIsland(asleep_bad3, 3, 0, kAwake, nullptr, 0);
}()),
"tree 0 is not in a cycle");
}
static const char* const kStaticModel = "engine/testdata/sleep/static.xml";
@@ -273,7 +242,7 @@ TEST_F(SleepTest, WakingUnaffectedBySleeping) {
for (mjtJacobian jacobian : {mjJAC_DENSE, mjJAC_SPARSE}) {
m->opt.jacobian = jacobian;
for (mjtIntegrator integrator : // TODO: b/457674312 - Add support for RK4.
{mjINT_EULER, mjINT_IMPLICITFAST, mjINT_IMPLICIT}) {
{mjINT_EULER, mjINT_IMPLICITFAST, mjINT_IMPLICIT}) {
m->opt.integrator = integrator;
// make data with sleeping enabled
@@ -361,7 +330,6 @@ TEST_F(SleepTest, WakingUnaffectedBySleeping) {
mj_deleteModel(m);
}
// Test that waking does not affect sleeping trees for pos/vel-dependent arrays.
// Roll out models where some trees wake and/or sleep. At kCompare intervals,
// copy the state from the mjData with sleeping enabled to another mjData and
@@ -496,8 +464,8 @@ TEST_F(SleepTest, SleepingUnaffectedByWaking) {
for (int i = 0; i < m->nsensor; i++) {
if (m->nuser_sensor == 1 && m->sensor_user[i] == 1) {
EXPECT_TRUE(sensor_mismatch[i])
<< "contact sensor " << i << " comparison was expected to fail";
EXPECT_TRUE(sensor_mismatch[i])
<< "contact sensor " << i << " comparison was expected to fail";
}
}
@@ -555,9 +523,9 @@ TEST_F(SleepTest, MidpointSleepZeroVelocity) {
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
// give initial velocity (both translational and angular)
d->qvel[0] = 0.5;
@@ -569,7 +537,7 @@ TEST_F(SleepTest, MidpointSleepZeroVelocity) {
// step until body goes to sleep
for (int step = 0; step < 1000; step++) {
mj_step(m, d);
mj_step(m.get(), d.get());
if (d->ntree_awake == 0) break;
}
@@ -581,9 +549,6 @@ TEST_F(SleepTest, MidpointSleepZeroVelocity) {
EXPECT_EQ(d->qvel[i], 0.0) << "qvel[" << i << "] not zero after sleep";
EXPECT_EQ(d->qacc[i], 0.0) << "qacc[" << i << "] not zero after sleep";
}
mj_deleteData(d);
mj_deleteModel(m);
}
static const char* const kInitIslandFailModel =
test/engine/engine_support_test.cc
+159 -199
View File
@@ -38,10 +38,6 @@ using ::testing::Ne;
using ::testing::NotNull;
using ::testing::Pointwise;
using Name2idTest = MujocoTest;
static constexpr char name2idTestingModel[] = R"(
@@ -94,79 +90,69 @@ static constexpr char name2idTestingModel[] = R"(
)";
TEST_F(Name2idTest, FindIds) {
char error[1024];
mjModel* model =
LoadModelFromString(name2idTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
char error[1024];
MjModelPtr model =
LoadModelFromString(name2idTestingModel, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "world"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "body1"), 1);
EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "body2"), 2);
EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "body1_geom1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "body1_geom2"), 1);
EXPECT_THAT(mj_name2id(model, mjOBJ_JOINT, "joint2"), 1);
EXPECT_THAT(mj_name2id(model, mjOBJ_MESH, "mesh1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_LIGHT, "light1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_CAMERA, "camera1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_SITE, "site2"), 1);
EXPECT_THAT(mj_name2id(model, mjOBJ_MATERIAL, "material1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_TEXTURE, "texture1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_TENDON, "tendon1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_ACTUATOR, "actuator1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_SENSOR, "sensor1"), 0);
mj_deleteModel(model);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_BODY, "world"), 0);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_BODY, "body1"), 1);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_BODY, "body2"), 2);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_GEOM, "body1_geom1"), 0);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_GEOM, "body1_geom2"), 1);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_JOINT, "joint2"), 1);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_MESH, "mesh1"), 0);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_LIGHT, "light1"), 0);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_CAMERA, "camera1"), 0);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_SITE, "site2"), 1);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_MATERIAL, "material1"), 0);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_TEXTURE, "texture1"), 0);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_TENDON, "tendon1"), 0);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_ACTUATOR, "actuator1"), 0);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_SENSOR, "sensor1"), 0);
}
TEST_F(Name2idTest, MissingIds) {
char error[1024];
mjModel* model =
LoadModelFromString(name2idTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
TEST_F(Name2idTest, MissingIds) {
char error[1024];
MjModelPtr model =
LoadModelFromString(name2idTestingModel, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "abody3"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "abody2_geom2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_JOINT, "joint3"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_MESH, "amesh2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_LIGHT, "alight2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_CAMERA, "acamera2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_SITE, "asite3"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_MATERIAL, "amaterial2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_TEXTURE, "atexture2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_TENDON, "atendon2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_ACTUATOR, "aactuator2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_SENSOR, "asensor2"), -1);
mj_deleteModel(model);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_BODY, "abody3"), -1);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_GEOM, "abody2_geom2"), -1);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_JOINT, "joint3"), -1);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_MESH, "amesh2"), -1);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_LIGHT, "alight2"), -1);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_CAMERA, "acamera2"), -1);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_SITE, "asite3"), -1);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_MATERIAL, "amaterial2"), -1);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_TEXTURE, "atexture2"), -1);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_TENDON, "atendon2"), -1);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_ACTUATOR, "aactuator2"), -1);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_SENSOR, "asensor2"), -1);
}
TEST_F(Name2idTest, EmptyIds) {
char error[1024];
mjModel* model =
LoadModelFromString(name2idTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
char error[1024];
MjModelPtr model =
LoadModelFromString(name2idTestingModel, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, ""), -1);
mj_deleteModel(model);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_BODY, ""), -1);
}
TEST_F(Name2idTest, Namespaces) {
char error[1024];
mjModel* model =
LoadModelFromString(name2idTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
char error[1024];
MjModelPtr model =
LoadModelFromString(name2idTestingModel, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "camera1"), 3);
mj_deleteModel(model);
EXPECT_THAT(mj_name2id(model.get(), mjOBJ_GEOM, "camera1"), 3);
}
using VersionTest = MujocoTest;
TEST_F(VersionTest, MjVersion) {
EXPECT_EQ(mj_version(), mjVERSION_HEADER);
}
TEST_F(VersionTest, MjVersion) { EXPECT_EQ(mj_version(), mjVERSION_HEADER); }
TEST_F(VersionTest, MjVersionString) {
#if GTEST_USES_SIMPLE_RE == 1
@@ -177,7 +163,6 @@ TEST_F(VersionTest, MjVersionString) {
EXPECT_THAT(std::string(mj_versionString()), regex_matcher);
}
using SupportTest = MujocoTest;
// utility: generate two random quaternions with a given angle difference
@@ -188,7 +173,7 @@ void randomQuatPair(mjtNum qa[4], mjtNum qb[4], mjtNum angle, int seed) {
std::normal_distribution<double> dist(0, 1);
// sample qa = qb
for (int i=0; i < 4; i++) {
for (int i = 0; i < 4; i++) {
qa[i] = qb[i] = dist(rng);
}
mju_normalize4(qa);
@@ -196,7 +181,7 @@ void randomQuatPair(mjtNum qa[4], mjtNum qb[4], mjtNum angle, int seed) {
// integrate qb in random direction by angle
mjtNum dir[3];
for (int i=0; i < 3; i++) {
for (int i = 0; i < 3; i++) {
dir[i] = dist(rng);
}
mju_normalize3(dir);
@@ -218,8 +203,8 @@ TEST_F(SupportTest, DifferentiatePosSubQuat) {
const mjtNum eps = 1e-12; // epsilon for float comparison
char error[1024];
mjModel* model = LoadModelFromString(ballJointModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
MjModelPtr model = LoadModelFromString(ballJointModel, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
int seed = 1;
for (mjtNum angle : {0.0, 1e-5, 1e-2}) {
@@ -230,7 +215,7 @@ TEST_F(SupportTest, DifferentiatePosSubQuat) {
// get velocity given timestep
mjtNum qvel[3];
mj_differentiatePos(model, qvel, dt, qpos1, qpos2);
mj_differentiatePos(model.get(), qvel, dt, qpos1, qpos2);
// equivalent computation
mjtNum qneg[4], qdif[4], qvel_expect[3];
@@ -242,8 +227,6 @@ TEST_F(SupportTest, DifferentiatePosSubQuat) {
EXPECT_THAT(AsVector(qvel, 3), Pointwise(MjNear(eps, 1e-3), qvel_expect));
}
}
mj_deleteModel(model);
}
static const char* const kDefaultModel = "testdata/model.xml";
@@ -261,9 +244,9 @@ TEST_F(StateTest, GetSetStateStepEqual) {
std::normal_distribution<double> dist(0, .01);
// set controls and applied joint forces to random values
for (int i=0; i < model->nu; i++) data->ctrl[i] = dist(rng);
for (int i=0; i < model->nv; i++) data->qfrc_applied[i] = dist(rng);
for (int i=0; i < model->neq; i++) data->eq_active[i] = dist(rng) > 0;
for (int i = 0; i < model->nu; i++) data->ctrl[i] = dist(rng);
for (int i = 0; i < model->nv; i++) data->qfrc_applied[i] = dist(rng);
for (int i = 0; i < model->neq; i++) data->eq_active[i] = dist(rng) > 0;
// take one step
mj_step(model, data);
@@ -320,38 +303,35 @@ TEST_F(StateTest, GetSetStateDelay) {
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// verify history buffer exists: nhistory = 2 + 2*5 = 12
EXPECT_EQ(model->nhistory, 12); // [user, cursor, times(5), values(5)]
// state size should include history buffer
int size = mj_stateSize(model, mjSTATE_HISTORY);
int size = mj_stateSize(model.get(), mjSTATE_HISTORY);
EXPECT_EQ(size, model->nhistory);
// step to populate history buffer
data->ctrl[0] = 1.0;
mj_step(model, data);
mj_step(model.get(), data.get());
data->ctrl[0] = 2.0;
mj_step(model, data);
mj_step(model.get(), data.get());
// get history state
vector<mjtNum> history_state(size);
mj_getState(model, data, history_state.data(), mjSTATE_HISTORY);
mj_getState(model.get(), data.get(), history_state.data(), mjSTATE_HISTORY);
// modify the history buffer manually (value at index 7 = 2+5 = after times)
data->history[7] = 99.0; // first value
// set history state back - should restore original
mj_setState(model, data, history_state.data(), mjSTATE_HISTORY);
mj_setState(model.get(), data.get(), history_state.data(), mjSTATE_HISTORY);
// verify restoration
EXPECT_NE(data->history[7], 99.0);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(StateTest, CopyState) {
@@ -367,13 +347,13 @@ TEST_F(StateTest, CopyState) {
// modify d_src
src->time = 1.23;
for (int i=0; i < m->nq; ++i) src->qpos[i] = i*0.1;
for (int i=0; i < m->nv; ++i) src->qvel[i] = i*0.2;
for (int i=0; i < m->na; ++i) src->act[i] = i*0.3;
for (int i=0; i < m->nu; ++i) src->ctrl[i] = i*0.4;
for (int i=0; i < m->nhistory; ++i) src->history[i] = i*0.5;
for (int i = 0; i < m->nq; ++i) src->qpos[i] = i * 0.1;
for (int i = 0; i < m->nv; ++i) src->qvel[i] = i * 0.2;
for (int i = 0; i < m->na; ++i) src->act[i] = i * 0.3;
for (int i = 0; i < m->nu; ++i) src->ctrl[i] = i * 0.4;
for (int i = 0; i < m->nhistory; ++i) src->history[i] = i * 0.5;
for (int i=0; i < m->neq; ++i) src->eq_active[i] = 1 - m->eq_active0[i];
for (int i = 0; i < m->neq; ++i) src->eq_active[i] = 1 - m->eq_active0[i];
// check that states differ
EXPECT_NE(src->time, dst->time);
@@ -413,9 +393,9 @@ TEST_F(StateTest, ExtractState) {
std::normal_distribution<double> dist(0, .01);
// set controls and applied joint forces to random values
for (int i=0; i < model->nu; i++) data->ctrl[i] = dist(rng);
for (int i=0; i < model->nv; i++) data->qfrc_applied[i] = dist(rng);
for (int i=0; i < model->neq; i++) data->eq_active[i] = dist(rng) > 0;
for (int i = 0; i < model->nu; i++) data->ctrl[i] = dist(rng);
for (int i = 0; i < model->nv; i++) data->qfrc_applied[i] = dist(rng);
for (int i = 0; i < model->neq; i++) data->eq_active[i] = dist(rng) > 0;
// take one step
mj_step(model, data);
@@ -518,16 +498,15 @@ TEST_F(InertiaTest, AddMdenseSameAsSparse) {
}
// sparse addM
mj_addM(m, d, dst_sparse.data(), rownnz.data(),
rowadr.data(), colind.data());
mj_addM(m, d, dst_sparse.data(), rownnz.data(), rowadr.data(), colind.data());
// dense addM
mj_addM(m, d, dst_dense.data(), nullptr, nullptr, nullptr);
// dense comparison (lower triangle)
for (int i=0; i < nv; i++) {
for (int j=0; j < nv; j++) {
EXPECT_EQ(dst_dense[i*nv+j], dst_sparse[i*nv+j]);
for (int i = 0; i < nv; i++) {
for (int j = 0; j < nv; j++) {
EXPECT_EQ(dst_dense[i * nv + j], dst_sparse[i * nv + j]);
}
}
@@ -547,13 +526,13 @@ TEST_F(InertiaTest, mulM) {
mj_forward(model, data);
// dense M matrix
vector<mjtNum> Mdense(nv*nv);
mju_sym2dense(Mdense.data(), data->M, nv,
model->M_rownnz, model->M_rowadr, model->M_colind);
vector<mjtNum> Mdense(nv * nv);
mju_sym2dense(Mdense.data(), data->M, nv, model->M_rownnz, model->M_rowadr,
model->M_colind);
// arbitrary RHS vector
vector<mjtNum> vec(nv);
for (int i=0; i < nv; i++) vec[i] = vec[i] = 20 + 30*i;
for (int i = 0; i < nv; i++) vec[i] = vec[i] = 20 + 30 * i;
// multiply directly
vector<mjtNum> res1(nv, 0);
@@ -582,7 +561,7 @@ TEST_F(InertiaTest, mulM2) {
// arbitrary RHS vector
vector<mjtNum> vec(nv);
for (int i=0; i < nv; i++) vec[i] = .2 + .3*i;
for (int i = 0; i < nv; i++) vec[i] = .2 + .3 * i;
// multiply sqrtMvec = M^1/2 * vec
vector<mjtNum> sqrtMvec(nv);
@@ -618,8 +597,8 @@ TEST_F(InertiaTest, FullM) {
// get dense mass matrix from M using mju_sparse2dense
vector<mjtNum> M_CSR(nv * nv);
mju_sparse2dense(M_CSR.data(), d->M, nv, nv,
m->M_rownnz, m->M_rowadr, m->M_colind);
mju_sparse2dense(M_CSR.data(), d->M, nv, nv, m->M_rownnz, m->M_rowadr,
m->M_colind);
// expect lower triangles to match exactly
for (int i = 0; i < nv; ++i) {
@@ -630,8 +609,8 @@ TEST_F(InertiaTest, FullM) {
// get dense LTDL factor (D on the diagonal)
vector<mjtNum> LD(nv * nv);
mju_sparse2dense(LD.data(), d->qLD, nv, nv,
m->M_rownnz, m->M_rowadr, m->M_colind);
mju_sparse2dense(LD.data(), d->qLD, nv, nv, m->M_rownnz, m->M_rowadr,
m->M_colind);
// extract L and D from LD
vector<mjtNum> L = LD;
@@ -686,88 +665,85 @@ static constexpr char GeomDistanceTestingModel2[] = R"(
TEST_F(SupportTest, GeomDistance) {
char error[1024];
mjModel* model =
MjModelPtr model =
LoadModelFromString(GeomDistanceTestingModel1, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
mj_kinematics(model, data);
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mj_kinematics(model.get(), data.get());
// plane-sphere, distmax too small
mjtNum distmax = 0.5;
EXPECT_EQ(mj_geomDistance(model, data, 0, 1, distmax, nullptr), 0.5);
EXPECT_EQ(mj_geomDistance(model.get(), data.get(), 0, 1, distmax, nullptr),
0.5);
mjtNum fromto[6];
EXPECT_EQ(mj_geomDistance(model, data, 0, 1, distmax, fromto), 0.5);
EXPECT_EQ(mj_geomDistance(model.get(), data.get(), 0, 1, distmax, fromto),
0.5);
EXPECT_THAT(fromto, Pointwise(Eq(), vector<mjtNum>{0, 0, 0, 0, 0, 0}));
// plane-sphere
distmax = 1.0;
EXPECT_THAT(mj_geomDistance(model, data, 0, 1, 1.0, fromto),
EXPECT_THAT(mj_geomDistance(model.get(), data.get(), 0, 1, 1.0, fromto),
MjNear(0.8, 1e-12, 1e-5));
mjtNum eps = 1e-12;
EXPECT_THAT(fromto, Pointwise(MjNear(eps, 1e-5),
vector<mjtNum>{0, 0, 0, 0, 0, 0.8}));
EXPECT_THAT(fromto,
Pointwise(MjNear(eps, 1e-5), vector<mjtNum>{0, 0, 0, 0, 0, 0.8}));
// sphere-plane
EXPECT_THAT(mj_geomDistance(model, data, 1, 0, 1.0, fromto),
EXPECT_THAT(mj_geomDistance(model.get(), data.get(), 1, 0, 1.0, fromto),
MjNear(0.8, 1e-12, 1e-5));
EXPECT_THAT(fromto, Pointwise(MjNear(eps, 1e-5),
vector<mjtNum>{0, 0, 0.8, 0, 0, 0}));
EXPECT_THAT(fromto,
Pointwise(MjNear(eps, 1e-5), vector<mjtNum>{0, 0, 0.8, 0, 0, 0}));
// sphere-sphere
EXPECT_THAT(mj_geomDistance(model, data, 1, 2, 1.0, fromto),
EXPECT_THAT(mj_geomDistance(model.get(), data.get(), 1, 2, 1.0, fromto),
MjNear(0.5, 1e-12, 1e-5));
EXPECT_THAT(fromto, Pointwise(MjNear(eps, 1e-5),
vector<mjtNum>{.2, 0, 1, .7, 0, 1}));
EXPECT_THAT(fromto,
Pointwise(MjNear(eps, 1e-5), vector<mjtNum>{.2, 0, 1, .7, 0, 1}));
// sphere-sphere, flipped order
EXPECT_THAT(mj_geomDistance(model, data, 2, 1, 1.0, fromto),
EXPECT_THAT(mj_geomDistance(model.get(), data.get(), 2, 1, 1.0, fromto),
MjNear(0.5, 1e-12, 1e-5));
EXPECT_THAT(fromto, Pointwise(MjNear(eps, 1e-5),
vector<mjtNum>{.7, 0, 1, .2, 0, 1}));
EXPECT_THAT(fromto,
Pointwise(MjNear(eps, 1e-5), vector<mjtNum>{.7, 0, 1, .2, 0, 1}));
// mesh-sphere (close distmax)
distmax = 0.701;
eps = model->opt.ccd_tolerance;
EXPECT_THAT(mj_geomDistance(model, data, 3, 1, distmax, fromto),
MjNear(0.7, eps, eps*100));
EXPECT_THAT(fromto, Pointwise(MjNear(eps, eps*100),
EXPECT_THAT(mj_geomDistance(model.get(), data.get(), 3, 1, distmax, fromto),
MjNear(0.7, eps, eps * 100));
EXPECT_THAT(fromto, Pointwise(MjNear(eps, eps * 100),
vector<mjtNum>{0, 0, .1, 0, 0, .8}));
// mesh-sphere (far distmax)
distmax = 1.0;
EXPECT_THAT(mj_geomDistance(model, data, 3, 1, distmax, fromto),
MjNear(0.7, eps, eps*100));
EXPECT_THAT(fromto, Pointwise(MjNear(eps, eps*100),
EXPECT_THAT(mj_geomDistance(model.get(), data.get(), 3, 1, distmax, fromto),
MjNear(0.7, eps, eps * 100));
EXPECT_THAT(fromto, Pointwise(MjNear(eps, eps * 100),
vector<mjtNum>{0, 0, .1, 0, 0, .8}));
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(SupportTest, GeomDistanceFromToFlipped) {
mjtNum distmax = 10.0;
char error[1024];
mjModel* model =
MjModelPtr model =
LoadModelFromString(GeomDistanceTestingModel2, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
mj_kinematics(model, data);
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mj_kinematics(model.get(), data.get());
mjtNum fromto01[6];
mjtNum fromto10[6];
for (int flag : {0, (int)mjDSBL_NATIVECCD}) {
model->opt.disableflags = flag;
mj_geomDistance(model, data, 0, 1, distmax, fromto01);
mj_geomDistance(model, data, 1, 0, distmax, fromto10);
mj_geomDistance(model.get(), data.get(), 0, 1, distmax, fromto01);
mj_geomDistance(model.get(), data.get(), 1, 0, distmax, fromto10);
mjtNum fromto10flipped[6] = {fromto10[3], fromto10[4], fromto10[5],
fromto10[0], fromto10[1], fromto10[2]};
EXPECT_THAT(AsVector(fromto10flipped, 6),
Pointwise(MjNear(1.0e-12, 1e-5), fromto01));
}
mj_deleteData(data);
mj_deleteModel(model);
}
static constexpr char kSetKeyframeTestingModel[] = R"(
@@ -789,43 +765,38 @@ static constexpr char kSetKeyframeTestingModel[] = R"(
TEST_F(SupportTest, SetKeyframe) {
char error[1024];
mjModel* model =
MjModelPtr model =
LoadModelFromString(kSetKeyframeTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
data->ctrl[0] = 1;
while (data->time < 1) {
mj_step(model, data);
mj_step(model.get(), data.get());
}
mj_setKeyframe(model, data, 1);
mj_setKeyframe(model.get(), data.get(), 1);
EXPECT_EQ(data->time, model->key_time[1]);
EXPECT_EQ(data->ctrl[0], model->key_ctrl[model->nu * 1]);
EXPECT_EQ(data->qpos[0], model->key_qpos[model->nq * 1]);
EXPECT_EQ(data->qvel[0], model->key_qvel[model->nv * 1]);
EXPECT_EQ(data->act[0], model->key_act[model->na * 1]);
mj_step(model, data);
mj_setKeyframe(model, data, 0);
mj_step(model.get(), data.get());
mj_setKeyframe(model.get(), data.get(), 0);
EXPECT_EQ(data->time, model->key_time[0]);
EXPECT_EQ(data->ctrl[0], model->key_ctrl[model->nu * 0]);
EXPECT_EQ(data->qpos[0], model->key_qpos[model->nq * 0]);
EXPECT_EQ(data->qvel[0], model->key_qvel[model->nv * 0]);
EXPECT_EQ(data->act[0], model->key_act[model->na * 0]);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(SupportTest, ContactSensorDim) {
int dataSpec = 1 << mjCONDATA_FOUND |
1 << mjCONDATA_FORCE |
1 << mjCONDATA_DIST |
1 << mjCONDATA_POS |
int dataSpec = 1 << mjCONDATA_FOUND | 1 << mjCONDATA_FORCE |
1 << mjCONDATA_DIST | 1 << mjCONDATA_POS |
1 << mjCONDATA_TANGENT;
EXPECT_EQ(mju_condataSize(dataSpec), 1+3+1+3+3);
EXPECT_EQ(mju_condataSize(dataSpec), 1 + 3 + 1 + 3 + 3);
}
// ------------------------------ ctrl delays --------------------------------
@@ -844,16 +815,14 @@ TEST_F(SupportTest, ReadCtrlNoDelay) {
</actuator>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
ASSERT_THAT(model, NotNull());
mjData* data = mj_makeData(model);
MjModelPtr model = LoadModelFromString(xml);
ASSERT_THAT(model.get(), NotNull());
MjDataPtr data = MakeData(model);
// no delay: should return current ctrl value
data->ctrl[0] = 42.0;
EXPECT_EQ(mj_readCtrl(model, data, 0, data->time, /*order=*/0), 42.0);
mj_deleteData(data);
mj_deleteModel(model);
EXPECT_EQ(mj_readCtrl(model.get(), data.get(), 0, data->time, /*order=*/0),
42.0);
}
TEST_F(SupportTest, ReadCtrlWithDelay) {
@@ -872,9 +841,9 @@ TEST_F(SupportTest, ReadCtrlWithDelay) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// model should have delay configured
// delay = 0.03 seconds, timestep = 0.01, so ndelay = ceil(0.03/0.01) = 3
@@ -884,11 +853,8 @@ TEST_F(SupportTest, ReadCtrlWithDelay) {
// initially, buffer should be filled with constant value (from init)
// reading at current time should return the init value
mjtNum val = mj_readCtrl(model, data, 0, data->time, /*order=*/0);
mjtNum val = mj_readCtrl(model.get(), data.get(), 0, data->time, /*order=*/0);
EXPECT_EQ(val, data->ctrl[0]);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(SupportTest, InitCtrlDelay) {
@@ -907,35 +873,33 @@ TEST_F(SupportTest, InitCtrlDelay) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// verify nhistory
EXPECT_EQ(model->actuator_history[0], 3);
// initialize with custom times and values
// buffer stores: time 0.0 -> value 1.0, time 0.01 -> value 2.0, time 0.02 -> value 3.0
// buffer stores: time 0.0 -> value 1.0, time 0.01 -> value 2.0, time 0.02 ->
// value 3.0
mjtNum times[3] = {0.0, 0.01, 0.02};
mjtNum values[3] = {1.0, 2.0, 3.0};
mj_initCtrlHistory(model, data, 0, times, values);
mj_initCtrlHistory(model.get(), data.get(), 0, times, values);
// mj_readCtrl now auto-subtracts delay: lookup_time = time - delay
// delay = 0.02, so:
// time=0.04 -> lookup at 0.02 -> value 3.0
// time=0.03 -> lookup at 0.01 -> value 2.0
// time=0.02 -> lookup at 0.00 -> value 1.0
mjtNum val = mj_readCtrl(model, data, 0, 0.04, /*order=*/0);
mjtNum val = mj_readCtrl(model.get(), data.get(), 0, 0.04, /*order=*/0);
EXPECT_EQ(val, 3.0);
val = mj_readCtrl(model, data, 0, 0.03, /*order=*/0);
val = mj_readCtrl(model.get(), data.get(), 0, 0.03, /*order=*/0);
EXPECT_EQ(val, 2.0);
val = mj_readCtrl(model, data, 0, 0.02, /*order=*/0);
val = mj_readCtrl(model.get(), data.get(), 0, 0.02, /*order=*/0);
EXPECT_EQ(val, 1.0);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(SupportTest, InitCtrlDelayNullTimes) {
@@ -954,9 +918,9 @@ TEST_F(SupportTest, InitCtrlDelayNullTimes) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// get existing times from buffer
int adr = model->actuator_historyadr[0];
@@ -965,7 +929,7 @@ TEST_F(SupportTest, InitCtrlDelayNullTimes) {
// initialize with NULL times (use existing) and new values
mjtNum values[3] = {10.0, 20.0, 30.0};
mj_initCtrlHistory(model, data, 0, nullptr, values);
mj_initCtrlHistory(model.get(), data.get(), 0, nullptr, values);
// verify times are unchanged
EXPECT_EQ(buf[2], existing_times[0]);
@@ -976,9 +940,6 @@ TEST_F(SupportTest, InitCtrlDelayNullTimes) {
EXPECT_EQ(buf[5], 10.0);
EXPECT_EQ(buf[6], 20.0);
EXPECT_EQ(buf[7], 30.0);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(SupportTest, InitSensorDelay) {
@@ -997,36 +958,35 @@ TEST_F(SupportTest, InitSensorDelay) {
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// verify nsample for sensor
EXPECT_EQ(model->sensor_history[0], 3);
// initialize with custom times and values, phase=0
// buffer stores: time 0.0 -> value 0.5, time 0.01 -> value 0.6, time 0.02 -> value 0.7
// buffer stores: time 0.0 -> value 0.5, time 0.01 -> value 0.6, time 0.02 ->
// value 0.7
mjtNum times[3] = {0.0, 0.01, 0.02};
mjtNum values[3] = {0.5, 0.6, 0.7};
mj_initSensorHistory(model, data, 0, times, values, /*phase=*/0.0);
mj_initSensorHistory(model.get(), data.get(), 0, times, values,
/*phase=*/0.0);
// mj_readSensor now auto-subtracts delay: lookup_time = time - delay
// delay = 0.02, so:
// time=0.04 -> lookup at 0.02 -> value 0.7
// time=0.03 -> lookup at 0.01 -> value 0.6
mjtNum result = 0;
const mjtNum* ptr = mj_readSensor(model, data, 0, 0.04, &result, /*order=*/0);
const mjtNum* ptr =
mj_readSensor(model.get(), data.get(), 0, 0.04, &result, /*order=*/0);
mjtNum val = ptr ? *ptr : result;
EXPECT_NEAR(val, 0.7, 1e-6);
ptr = mj_readSensor(model, data, 0, 0.03, &result, /*order=*/0);
ptr = mj_readSensor(model.get(), data.get(), 0, 0.03, &result, /*order=*/0);
val = ptr ? *ptr : result;
EXPECT_NEAR(val, 0.6, 1e-6);
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/engine/engine_util_container_test.cc
+16 -21
View File
@@ -16,9 +16,9 @@
#include <array>
#include <cstddef>
#include <mujoco/mjdata.h>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mjdata.h>
#include <mujoco/mujoco.h>
#include "test/fixture.h"
@@ -32,10 +32,9 @@ constexpr int GetExpectedStackUsageBytes() {
if constexpr (N <= 0) {
return prev_size;
} else {
constexpr auto RoundUpToAlignment =
[](int x, int alignment) {
return alignment * (x / alignment + ((x % alignment) ? 1 : 0));
};
constexpr auto RoundUpToAlignment = [](int x, int alignment) {
return alignment * (x / alignment + ((x % alignment) ? 1 : 0));
};
constexpr int size_with_arraylist = RoundUpToAlignment(
prev_size + sizeof(mjArrayList), alignof(mjArrayList));
constexpr int size_with_buffer = RoundUpToAlignment(
@@ -49,15 +48,15 @@ class TestMjArrayList : public MujocoTest {};
TEST_F(TestMjArrayList, TestMjArrayListSingleThreaded) {
std::array<char, 1024> error;
mjModel* m = LoadModelFromString("<mujoco/>", error.data(), error.size());
ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error.data();
mjData* d = mj_makeData(m);
mj_markStack(d);
MjModelPtr m = LoadModelFromString("<mujoco/>", error.data(), error.size());
ASSERT_THAT(m.get(), NotNull()) << "Failed to load model: " << error.data();
MjDataPtr d = MakeData(m);
mj_markStack(d.get());
using DataType = int;
constexpr int kInitialCapacity = 10;
mjArrayList* array_list =
mju_arrayListCreate(d, sizeof(DataType), kInitialCapacity);
mju_arrayListCreate(d.get(), sizeof(DataType), kInitialCapacity);
constexpr int kNumElements = 35;
for (int i = 0; i < kNumElements; ++i) {
@@ -78,19 +77,17 @@ TEST_F(TestMjArrayList, TestMjArrayListSingleThreaded) {
EXPECT_EQ(mju_arrayListAt(array_list, kNumElements), nullptr);
EXPECT_EQ(mju_arrayListAt(array_list, 100), nullptr);
mj_freeStack(d);
mj_deleteData(d);
mj_deleteModel(m);
mj_freeStack(d.get());
}
TEST_F(TestMjArrayList, ZeroInitialCapacity) {
char error[1024];
mjModel* m = LoadModelFromString("<mujoco/>", error, sizeof(error));
ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error;
mjData* d = mj_makeData(m);
mj_markStack(d);
MjModelPtr m = LoadModelFromString("<mujoco/>", error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << "Failed to load model: " << error;
MjDataPtr d = MakeData(m);
mj_markStack(d.get());
mjArrayList* array_list =
mju_arrayListCreate(d, sizeof(double), /*initial_capacity=*/0);
mju_arrayListCreate(d.get(), sizeof(double), /*initial_capacity=*/0);
EXPECT_EQ(mju_arrayListSize(array_list), 0);
for (int i = 0; i < 35; ++i) {
@@ -104,9 +101,7 @@ TEST_F(TestMjArrayList, ZeroInitialCapacity) {
}
EXPECT_EQ(mju_arrayListAt(array_list, 35), nullptr);
mj_freeStack(d);
mj_deleteData(d);
mj_deleteModel(m);
mj_freeStack(d.get());
}
} // namespace
test/engine/engine_util_misc_test.cc
+200 -192
View File
@@ -14,19 +14,20 @@
// Tests for engine/engine_util_solve.c.
#include "src/engine/engine_util_misc.h"
#include <array>
#include <vector>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <cstring>
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <gtest/gtest-spi.h>
#include <gtest/gtest.h>
#include <mujoco/mjdata.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_util_misc.h"
#include "test/fixture.h"
namespace mujoco {
@@ -60,11 +61,11 @@ TEST_F(UtilMiscTest, PrintsMemoryWarning) {
TEST_F(UtilMiscTest, Sigmoid) {
// function values
EXPECT_EQ(mju_sigmoid(-1), 0);
EXPECT_EQ(mju_sigmoid(0), 0);
EXPECT_EQ(mju_sigmoid(-1), 0);
EXPECT_EQ(mju_sigmoid(0), 0);
EXPECT_EQ(mju_sigmoid(0.5), 0.5);
EXPECT_EQ(mju_sigmoid(1), 1);
EXPECT_EQ(mju_sigmoid(2), 1);
EXPECT_EQ(mju_sigmoid(1), 1);
EXPECT_EQ(mju_sigmoid(2), 1);
// epsilon for finite-differencing
const mjtNum dx = MjTol(1e-7, 1e-3);
@@ -80,8 +81,8 @@ TEST_F(UtilMiscTest, Sigmoid) {
// derivative at 0.5
const mjtNum x = 0.5;
mjtNum dy_dx_0p5 = (mju_sigmoid(x + dx) - mju_sigmoid(x - dx)) / (2*dx);
mjtNum expected = 30*x*x*x*x - 60*x*x*x + 30*x*x;
mjtNum dy_dx_0p5 = (mju_sigmoid(x + dx) - mju_sigmoid(x - dx)) / (2 * dx);
mjtNum expected = 30 * x * x * x * x - 60 * x * x * x + 30 * x * x;
EXPECT_NEAR(dy_dx_0p5, expected, fd_tol);
}
@@ -125,29 +126,27 @@ TEST_F(UtilMiscTest, SphereWrap) {
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// measure tendon length for keyframe 0
mj_resetDataKeyframe(model, data, 0);
mj_forward(model, data);
mj_resetDataKeyframe(model.get(), data.get(), 0);
mj_forward(model.get(), data.get());
mjtNum ten_length0 = data->sensordata[0];
// measure tendon length for keyframe 1
mj_resetDataKeyframe(model, data, 1);
mj_forward(model, data);
mj_resetDataKeyframe(model.get(), data.get(), 1);
mj_forward(model.get(), data.get());
mjtNum ten_length1 = data->sensordata[0];
// difference should be small
mjtNum diff = ten_length1 - ten_length0;
EXPECT_LT(mju_abs(diff), 1e-3);
mj_deleteData(data);
mj_deleteModel(model);
}
// compute time constant as in Millard et al. (2013) https://doi.org/10.1115/1.4023390
// compute time constant as in Millard et al. (2013)
// https://doi.org/10.1115/1.4023390
mjtNum muscleDynamicsMillard(mjtNum ctrl, mjtNum act, const mjtNum prm[2]) {
// clamp control
mjtNum ctrlclamp = mju_clip(ctrl, 0, 1);
@@ -157,13 +156,13 @@ mjtNum muscleDynamicsMillard(mjtNum ctrl, mjtNum act, const mjtNum prm[2]) {
mjtNum tau;
if (ctrlclamp > act) {
tau = prm[0] * (0.5 + 1.5*actclamp);
tau = prm[0] * (0.5 + 1.5 * actclamp);
} else {
tau = prm[1] / (0.5 + 1.5*actclamp);
tau = prm[1] / (0.5 + 1.5 * actclamp);
}
// filter output
return (ctrlclamp-act) / mjMAX(mjMINVAL, tau);
return (ctrlclamp - act) / mjMAX(mjMINVAL, tau);
}
TEST_F(UtilMiscTest, SmoothMuscleDynamics) {
@@ -188,7 +187,7 @@ TEST_F(UtilMiscTest, SmoothMuscleDynamics) {
EXPECT_EQ(muscleDynamicsMillard(ctrl, act, prm),
mju_muscleDynamics(ctrl, act, prm));
ctrl = 0.6 + eps; // larger than act by just over 0.5*tau_smooth
ctrl = 0.6 + eps; // larger than act by just over 0.5*tau_smooth
EXPECT_EQ(muscleDynamicsMillard(ctrl, act, prm),
mju_muscleDynamics(ctrl, act, prm));
@@ -196,11 +195,11 @@ TEST_F(UtilMiscTest, SmoothMuscleDynamics) {
mjtNum tau_act = 0.2;
mjtNum tau_deact = 0.3;
for (mjtNum dctrl : {0.0, 0.1, 0.2, 1.0, 1.1}) {
mjtNum lower = mju_muscleDynamicsTimescale(-dctrl,
tau_act, tau_deact, tau_smooth);
mjtNum upper = mju_muscleDynamicsTimescale(dctrl,
tau_act, tau_deact, tau_smooth);
EXPECT_EQ(0.5*(upper + lower), 0.5*(tau_act + tau_deact));
mjtNum lower =
mju_muscleDynamicsTimescale(-dctrl, tau_act, tau_deact, tau_smooth);
mjtNum upper =
mju_muscleDynamicsTimescale(dctrl, tau_act, tau_deact, tau_smooth);
EXPECT_EQ(0.5 * (upper + lower), 0.5 * (tau_act + tau_deact));
}
}
@@ -208,13 +207,13 @@ TEST_F(UtilMiscTest, MuscleGainLength) {
mjtNum lmin = 0.5;
mjtNum lmax = 1.5;
EXPECT_EQ(mju_muscleGainLength(0.0, lmin, lmax), 0);
EXPECT_EQ(mju_muscleGainLength(0.5, lmin, lmax), 0);
EXPECT_EQ(mju_muscleGainLength(0.0, lmin, lmax), 0);
EXPECT_EQ(mju_muscleGainLength(0.5, lmin, lmax), 0);
EXPECT_EQ(mju_muscleGainLength(0.75, lmin, lmax), 0.5);
EXPECT_EQ(mju_muscleGainLength(1.0, lmin, lmax), 1);
EXPECT_EQ(mju_muscleGainLength(1.0, lmin, lmax), 1);
EXPECT_EQ(mju_muscleGainLength(1.25, lmin, lmax), 0.5);
EXPECT_EQ(mju_muscleGainLength(1.5, lmin, lmax), 0);
EXPECT_EQ(mju_muscleGainLength(2.0, lmin, lmax), 0);
EXPECT_EQ(mju_muscleGainLength(1.5, lmin, lmax), 0);
EXPECT_EQ(mju_muscleGainLength(2.0, lmin, lmax), 0);
}
TEST_F(UtilMiscTest, MjuSparseMap) {
@@ -250,11 +249,9 @@ TEST_F(UtilMiscTest, MjuSparseMap) {
// Verify the map by checking values
mjtNum mat_res_gathered[nnz_res];
mju_gather(mat_res_gathered, mat_src, map, nnz_res);
EXPECT_THAT(AsVector(mat_res_gathered, nnz_res),
ElementsAre(1, 3, 5, 6));
EXPECT_THAT(AsVector(mat_res_gathered, nnz_res), ElementsAre(1, 3, 5, 6));
}
TEST_F(UtilMiscTest, MjuSparseLower2SymMap) {
// nr = 3
// src = [[1, 0, 0],
@@ -277,8 +274,8 @@ TEST_F(UtilMiscTest, MjuSparseLower2SymMap) {
int map[res_nnz];
int cursor[nr];
mju_lower2SymMap(map, nr, rowadr_res, rownnz_res, colind_res,
rowadr_src, rownnz_src, colind_src, cursor);
mju_lower2SymMap(map, nr, rowadr_res, rownnz_res, colind_res, rowadr_src,
rownnz_src, colind_src, cursor);
// Expected map:
// res(0,0) -> src(0,0) (k=0) => map[0] = 0
@@ -323,8 +320,8 @@ TEST_F(UtilMiscTest, MjuSparseLower2SymMapPartial) {
int map[res_nnz];
int cursor[nr];
mju_lower2SymMap(map, nr, rowadr_res, rownnz_res, colind_res,
rowadr_src, rownnz_src, colind_src, cursor);
mju_lower2SymMap(map, nr, rowadr_res, rownnz_res, colind_res, rowadr_src,
rownnz_src, colind_src, cursor);
// Expected map for the non-zeros in res:
// res(0,0) -> src(0,0) (k=0) => map[0] = 0
@@ -398,23 +395,24 @@ using InterpolationTest = MujocoTest;
TEST_F(InterpolationTest, mju_interpolate3D) {
// quadratic functions should be interpolated exactly if order = 2
auto quadratic_function_1 = [](mjtNum x, mjtNum y, mjtNum z) {
return x*x + y*y + z*z;
return x * x + y * y + z * z;
};
auto quadratic_function_2 = [](mjtNum x, mjtNum y, mjtNum z) {
return x*y*z + y*z*z + x*z*z;
return x * y * z + y * z * z + x * z * z;
};
auto quadratic_function_3 = [](mjtNum x, mjtNum y, mjtNum z) {
return x*y*z + y*z*z + x*z*z + y*y*z + x*x*z + x + y + z;
return x * y * z + y * z * z + x * z * z + y * y * z + x * x * z + x + y +
z;
};
static constexpr int order = 2;
mjtNum coeff[3*(order+1)*(order+1)*(order+1)];
mjtNum coeff[3 * (order + 1) * (order + 1) * (order + 1)];
int index = 0;
for (int i = 0; i <= order; ++i) {
for (int j = 0; j <= order; ++j) {
for (int k = 0; k <= order; ++k) {
coeff[3*index+0] = quadratic_function_1(.5*i, .5*j, .5*k);
coeff[3*index+1] = quadratic_function_2(.5*i, .5*j, .5*k);
coeff[3*index+2] = quadratic_function_3(.5*i, .5*j, .5*k);
coeff[3 * index + 0] = quadratic_function_1(.5 * i, .5 * j, .5 * k);
coeff[3 * index + 1] = quadratic_function_2(.5 * i, .5 * j, .5 * k);
coeff[3 * index + 2] = quadratic_function_3(.5 * i, .5 * j, .5 * k);
index++;
}
}
@@ -460,8 +458,8 @@ TEST_F(InterpolationTest, mju_cellLookup_MultiCell) {
// 2x3x4 grid, trilinear: 3x4x5 = 60 nodes
int cellnum[3] = {2, 3, 4};
int order = 1;
int ny_g = 3*1 + 1; // 4
int nz_g = 4*1 + 1; // 5
int ny_g = 3 * 1 + 1; // 4
int nz_g = 4 * 1 + 1; // 5
// point at (0.75, 0.5, 0.125) -> cell (1, 1, 0)
mjtNum coord[3] = {0.75, 0.5, 0.125};
@@ -485,7 +483,7 @@ TEST_F(InterpolationTest, mju_cellLookup_MultiCell) {
for (int li = 0; li <= 1; li++) {
for (int lj = 0; lj <= 1; lj++) {
for (int lk = 0; lk <= 1; lk++) {
expected[ni++] = (1+li)*ny_g*nz_g + (1+lj)*nz_g + lk;
expected[ni++] = (1 + li) * ny_g * nz_g + (1 + lj) * nz_g + lk;
}
}
}
@@ -519,8 +517,8 @@ TEST_F(InterpolationTest, mju_defGradient) {
mjtNum mat[9];
mjtNum p1[3] = {.5, .5, .5};
mjtNum p2[3] = {.25, .25, .25};
mjtNum dof0[24] = {0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 1, 1,
1, 0, 0, 1, 0, 1, 1, 1, 0, 1, 1, 1};
mjtNum dof0[24] = {0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 1, 1,
1, 0, 0, 1, 0, 1, 1, 1, 0, 1, 1, 1};
// identity
mjtNum dof1[24];
@@ -538,7 +536,7 @@ TEST_F(InterpolationTest, mju_defGradient) {
// constant stretch
mjtNum dof3[24];
for (int i = 0; i < 24; ++i) dof3[i] = 2*dof0[i];
for (int i = 0; i < 24; ++i) dof3[i] = 2 * dof0[i];
mju_defGradient(mat, p1, dof3, order);
EXPECT_THAT(mat, ElementsAreArray({2, 0, 0, 0, 2, 0, 0, 0, 2}));
mju_defGradient(mat, p2, dof3, order);
@@ -546,7 +544,7 @@ TEST_F(InterpolationTest, mju_defGradient) {
// axial stretch
mjtNum dof4[24];
for (int i = 0; i < 24; ++i) dof4[i] = (i%3 == 1 ? 2 : 1)*dof0[i];
for (int i = 0; i < 24; ++i) dof4[i] = (i % 3 == 1 ? 2 : 1) * dof0[i];
mju_defGradient(mat, p1, dof4, order);
EXPECT_THAT(mat, ElementsAreArray({1, 0, 0, 0, 2, 0, 0, 0, 1}));
mju_defGradient(mat, p2, dof4, order);
@@ -557,8 +555,8 @@ TEST_F(InterpolationTest, mju_defGradient) {
for (int i = 0; i < 8; ++i) {
mjtNum quat[4] = {0, 0, 0, 1};
mjtNum axis[3] = {0, 0, 1};
mju_axisAngle2Quat(quat, axis, mjPI/2);
mju_rotVecQuat(dof5 + 3*i, dof0 + 3*i, quat);
mju_axisAngle2Quat(quat, axis, mjPI / 2);
mju_rotVecQuat(dof5 + 3 * i, dof0 + 3 * i, quat);
}
mju_defGradient(mat, p1, dof5, order);
EXPECT_THAT(mat, Pointwise(MjNear(1e-8, 1e-6), {0, -1, 0, 1, 0, 0, 0, 0, 1}));
@@ -571,8 +569,8 @@ TEST_F(InterpolationTest, mju_defGradient) {
for (int i = 0; i < 8; ++i) {
mjtNum quat[4];
mjtNum axis[3] = {0, 0, 1};
mju_axisAngle2Quat(quat, axis, mjPI/6);
mju_rotVecQuat(dof6 + 3*i, dof0 + 3*i, quat);
mju_axisAngle2Quat(quat, axis, mjPI / 6);
mju_rotVecQuat(dof6 + 3 * i, dof0 + 3 * i, quat);
mju_quat2Mat(rot6, quat);
}
mju_defGradient(mat, p1, dof6, order);
@@ -587,9 +585,9 @@ TEST_F(InterpolationTest, mju_defGradient) {
mjtNum quat[4];
mjtNum axis[3] = {0, 0, 1};
mjtNum offset[3] = {-.5, -.5, 0};
mju_axisAngle2Quat(quat, axis, mjPI/6);
mju_add3(dof7 + 3*i, dof0 + 3*i, offset);
mju_rotVecQuat(dof7 + 3*i, dof0 + 3*i, quat);
mju_axisAngle2Quat(quat, axis, mjPI / 6);
mju_add3(dof7 + 3 * i, dof0 + 3 * i, offset);
mju_rotVecQuat(dof7 + 3 * i, dof0 + 3 * i, quat);
mju_quat2Mat(rot7, quat);
}
mju_defGradient(mat, p1, dof7, order);
@@ -618,15 +616,15 @@ TEST_F(InterpolationTest, mju_flexInterpState_MultiCell) {
mjtNum z = (k - 1) * 0.1;
// Apply rotation
xpos[3*idx + 0] = -y;
xpos[3*idx + 1] = x;
xpos[3*idx + 2] = z;
xpos[3 * idx + 0] = -y;
xpos[3 * idx + 1] = x;
xpos[3 * idx + 2] = z;
idx++;
}
}
}
int npc = (order+1)*(order+1)*(order+1);
int npc = (order + 1) * (order + 1) * (order + 1);
std::vector<mjtNum> xpos_c(3 * npc);
mju_flexGatherCellState(order, cy, cz, 0, 0, 0, xpos.data(), NULL, NULL,
@@ -775,7 +773,7 @@ TEST_F(Base64Test, mju_isValidBase64_valid4) {
TEST_F(Base64Test, mju_decodeBase64) {
std::array<std::uint8_t, 5> buffer;
const char *s = "D4a+//A=";
const char* s = "D4a+//A=";
std::size_t n = mju_decodeBase64(buffer.data(), s);
@@ -785,7 +783,7 @@ TEST_F(Base64Test, mju_decodeBase64) {
TEST_F(Base64Test, mju_decodeBase6_align0) {
std::array<std::uint8_t, 3> buffer;
const char *s = "QUJD";
const char* s = "QUJD";
std::size_t n = mju_decodeBase64(buffer.data(), s);
@@ -795,7 +793,7 @@ TEST_F(Base64Test, mju_decodeBase6_align0) {
TEST_F(Base64Test, mju_decodeBase64_align1) {
std::array<std::uint8_t, 2> buffer;
const char *s = "QUI=";
const char* s = "QUI=";
std::size_t n = mju_decodeBase64(buffer.data(), s);
@@ -805,7 +803,7 @@ TEST_F(Base64Test, mju_decodeBase64_align1) {
TEST_F(Base64Test, mju_decodeBase64_align2) {
std::array<std::uint8_t, 1> buffer;
const char *s = "QQ==";
const char* s = "QQ==";
std::size_t n = mju_decodeBase64(buffer.data(), s);
@@ -814,7 +812,7 @@ TEST_F(Base64Test, mju_decodeBase64_align2) {
}
TEST_F(Base64Test, mju_decodeBase64_null) {
const char *s = "";
const char* s = "";
std::size_t n = mju_decodeBase64(NULL, s);
@@ -823,7 +821,7 @@ TEST_F(Base64Test, mju_decodeBase64_null) {
TEST_F(Base64Test, mju_decodeBase64_ones) {
std::array<std::uint8_t, 3> buffer;
const char *s = "////";
const char* s = "////";
std::size_t n = mju_decodeBase64(buffer.data(), s);
@@ -834,7 +832,7 @@ TEST_F(Base64Test, mju_decodeBase64_ones) {
TEST_F(Base64Test, decodeAndEncode) {
std::array<std::uint8_t, 5> buffer1;
std::array<char, 9> buffer2;
const char *s = "D4a+/vA=";
const char* s = "D4a+/vA=";
mju_decodeBase64(buffer1.data(), s);
mju_encodeBase64(buffer2.data(), buffer1.data(), buffer1.size());
@@ -853,15 +851,15 @@ using HistoryTest = MujocoTest;
TEST_F(HistoryTest, Init) {
constexpr int n = 4;
constexpr int dim = 1;
mjtNum buf[2 + n + n*dim];
mjtNum buf[2 + n + n * dim];
std::vector<mjtNum> times = {4, 6, 8, 10};
std::vector<mjtNum> values = {99, 99, 99, 99};
mju_historyInit(buf, n, dim, times.data(), values.data(), 0.0);
// check header
EXPECT_EQ(buf[0], 0.0); // user
EXPECT_EQ(buf[1], static_cast<mjtNum>(n-1)); // cursor = n-1
EXPECT_EQ(buf[0], 0.0); // user
EXPECT_EQ(buf[1], static_cast<mjtNum>(n - 1)); // cursor = n-1
// timestamps: [4, 6, 8, 10] (t=10 is newest)
// values: [99, 99, 99, 99]
@@ -874,13 +872,13 @@ TEST_F(HistoryTest, Init) {
TEST_F(HistoryTest, Init_Vector) {
constexpr int n = 3;
constexpr int dim = 2;
mjtNum buf[2 + n + n*dim];
mjtNum buf[2 + n + n * dim];
std::vector<mjtNum> times = {-2, -1, 0};
std::vector<mjtNum> values = {1.0, 2.0, 1.0, 2.0, 1.0, 2.0};
mju_historyInit(buf, n, dim, times.data(), values.data(), 0.0);
EXPECT_EQ(buf[1], static_cast<mjtNum>(n-1)); // cursor = n-1
EXPECT_EQ(buf[1], static_cast<mjtNum>(n - 1)); // cursor = n-1
// verify via read function
mjtNum res[dim];
@@ -894,7 +892,7 @@ TEST_F(HistoryTest, Append) {
constexpr int n = 4;
constexpr int dim = 1;
// Initialize buffer properly, then insert
mjtNum buf[2 + 2*n];
mjtNum buf[2 + 2 * n];
buf[0] = 0.0;
buf[1] = n - 1;
// timestamps: [4, 6, 8, 10]
@@ -926,7 +924,7 @@ TEST_F(HistoryTest, Append) {
TEST_F(HistoryTest, Append_Multiple) {
constexpr int n = 3;
constexpr int dim = 1;
mjtNum buf[2 + 2*n];
mjtNum buf[2 + 2 * n];
buf[0] = 0.0;
buf[1] = n - 1;
mjtNum times[] = {-2, -1, 0};
@@ -947,20 +945,23 @@ TEST_F(HistoryTest, Append_Multiple) {
TEST_F(HistoryTest, ReadVector_ExactMatch) {
constexpr int n = 3;
constexpr int dim = 2;
mjtNum buf[2 + n + n*dim];
mjtNum buf[2 + n + n * dim];
buf[0] = 0.0;
buf[1] = n - 1;
mjtNum times[] = {0, 1, 2};
mju_copy(buf + 2, times, n);
mju_zero(buf + 2 + n, n*dim);
mju_zero(buf + 2 + n, n * dim);
// set values: t=0->(1,2), t=1->(3,4), t=2->(5,6)
mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0);
slot0[0] = 1.0; slot0[1] = 2.0;
slot0[0] = 1.0;
slot0[1] = 2.0;
mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0);
slot1[0] = 3.0; slot1[1] = 4.0;
slot1[0] = 3.0;
slot1[1] = 4.0;
mjtNum* slot2 = mju_historyInsert(buf, n, dim, 2.0);
slot2[0] = 5.0; slot2[1] = 6.0;
slot2[0] = 5.0;
slot2[1] = 6.0;
mjtNum res[dim];
const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 1.0, 0);
@@ -972,19 +973,22 @@ TEST_F(HistoryTest, ReadVector_ExactMatch) {
TEST_F(HistoryTest, ReadVector_ZOH) {
constexpr int n = 3;
constexpr int dim = 2;
mjtNum buf[2 + n + n*dim];
mjtNum buf[2 + n + n * dim];
buf[0] = 0.0;
buf[1] = n - 1;
mjtNum times[] = {0, 1, 2};
mju_copy(buf + 2, times, n);
mju_zero(buf + 2 + n, n*dim);
mju_zero(buf + 2 + n, n * dim);
mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0);
slot0[0] = 1.0; slot0[1] = 2.0;
slot0[0] = 1.0;
slot0[1] = 2.0;
mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0);
slot1[0] = 3.0; slot1[1] = 4.0;
slot1[0] = 3.0;
slot1[1] = 4.0;
mjtNum* slot2 = mju_historyInsert(buf, n, dim, 2.0);
slot2[0] = 5.0; slot2[1] = 6.0;
slot2[0] = 5.0;
slot2[1] = 6.0;
mjtNum res[dim];
const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 0.5, 0);
@@ -996,19 +1000,22 @@ TEST_F(HistoryTest, ReadVector_ZOH) {
TEST_F(HistoryTest, ReadVector_Linear) {
constexpr int n = 3;
constexpr int dim = 2;
mjtNum buf[2 + n + n*dim];
mjtNum buf[2 + n + n * dim];
buf[0] = 0.0;
buf[1] = n - 1;
mjtNum times[] = {0, 1, 2};
mju_copy(buf + 2, times, n);
mju_zero(buf + 2 + n, n*dim);
mju_zero(buf + 2 + n, n * dim);
mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0);
slot0[0] = 1.0; slot0[1] = 2.0;
slot0[0] = 1.0;
slot0[1] = 2.0;
mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0);
slot1[0] = 3.0; slot1[1] = 4.0;
slot1[0] = 3.0;
slot1[1] = 4.0;
mjtNum* slot2 = mju_historyInsert(buf, n, dim, 2.0);
slot2[0] = 5.0; slot2[1] = 6.0;
slot2[0] = 5.0;
slot2[1] = 6.0;
mjtNum res[dim];
const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 0.5, 1);
@@ -1020,7 +1027,7 @@ TEST_F(HistoryTest, ReadVector_Linear) {
TEST_F(HistoryTest, InsertOutOfOrder) {
constexpr int n = 4;
constexpr int dim = 1;
mjtNum buf[2 + 2*n];
mjtNum buf[2 + 2 * n];
mjtNum res;
auto reset = [&]() {
@@ -1067,7 +1074,7 @@ TEST_F(HistoryTest, InsertOutOfOrder) {
TEST_F(HistoryTest, InsertReplaceOnCollision) {
constexpr int n = 4;
constexpr int dim = 1;
mjtNum buf[2 + 2*n];
mjtNum buf[2 + 2 * n];
mjtNum res;
auto reset = [&]() {
@@ -1129,15 +1136,17 @@ TEST_F(HistoryTest, CubicInterpolation) {
buf[1] = n - 1;
mjtNum times[] = {-1, 0};
mju_copy(buf + 2, times, n);
mju_zero(buf + 2 + n, n*dim);
mju_zero(buf + 2 + n, n * dim);
// Insert (0, 0, 1) and (1, 1, 0).
// Dim 0: 0 -> 1. Spline: p(x) = 3x^2 - 2x^3
// Dim 1: 1 -> 0. Spline: p(x) = 1 - 3x^2 + 2x^3
mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0);
slot0[0] = 0.0; slot0[1] = 1.0;
slot0[0] = 0.0;
slot0[1] = 1.0;
mjtNum* slot1 = mju_historyInsert(buf, n, dim, 1.0);
slot1[0] = 1.0; slot1[1] = 0.0;
slot1[0] = 1.0;
slot1[1] = 0.0;
mjtNum res[2];
@@ -1152,7 +1161,7 @@ TEST_F(HistoryTest, CubicInterpolation) {
// Dim 0: 3*0.25^2 - 2*0.25^3
// Dim 1: 1 - (3*0.25^2 - 2*0.25^3)
mju_historyRead(buf, n, dim, res, 0.25, 2);
mjtNum expected_0_25 = 3*0.25*0.25 - 2*0.25*0.25*0.25;
mjtNum expected_0_25 = 3 * 0.25 * 0.25 - 2 * 0.25 * 0.25 * 0.25;
EXPECT_NEAR(res[0], expected_0_25, MjTol(1e-9, 1e-9));
EXPECT_NEAR(res[1], 1.0 - expected_0_25, MjTol(1e-9, 1e-9));
@@ -1160,7 +1169,7 @@ TEST_F(HistoryTest, CubicInterpolation) {
// Dim 0: 3*0.8^2 - 2*0.8^3
// Dim 1: 1 - (3*0.8^2 - 2*0.8^3)
mju_historyRead(buf, n, dim, res, 0.8, 2);
mjtNum expected_0_8 = 3*0.8*0.8 - 2*0.8*0.8*0.8;
mjtNum expected_0_8 = 3 * 0.8 * 0.8 - 2 * 0.8 * 0.8 * 0.8;
EXPECT_NEAR(res[0], expected_0_8, MjTol(1e-9, 1e-9));
EXPECT_NEAR(res[1], 1.0 - expected_0_8, MjTol(1e-9, 1e-9));
}
@@ -1184,9 +1193,9 @@ TEST_F(FaceStateTest, NodeIndicesSingleCell) {
// create dummy positions for 8 nodes
std::vector<mjtNum> xpos(3 * 8, 0);
for (int i = 0; i < 8; i++) {
xpos[3*i + 0] = (i / 4) * 1.0;
xpos[3*i + 1] = ((i / 2) % 2) * 1.0;
xpos[3*i + 2] = (i % 2) * 1.0;
xpos[3 * i + 0] = (i / 4) * 1.0;
xpos[3 * i + 1] = ((i / 2) % 2) * 1.0;
xpos[3 * i + 2] = (i % 2) * 1.0;
}
// helper: compute expected global node index from (gx, gy, gz)
@@ -1198,8 +1207,8 @@ TEST_F(FaceStateTest, NodeIndicesSingleCell) {
// normal_axis=0, na0=1, na1=2
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL, NULL,
NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 0));
EXPECT_EQ(indices[1], gidx(0, 0, 1));
EXPECT_EQ(indices[2], gidx(0, 1, 0));
@@ -1209,8 +1218,8 @@ TEST_F(FaceStateTest, NodeIndicesSingleCell) {
// face 1: x=max (fixed g[0]=1, varying g[1], g[2])
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 1, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
mju_flexGatherFaceState(order, cx, cy, cz, 1, xpos.data(), NULL, NULL, NULL,
NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(1, 0, 0));
EXPECT_EQ(indices[1], gidx(1, 0, 1));
EXPECT_EQ(indices[2], gidx(1, 1, 0));
@@ -1222,8 +1231,8 @@ TEST_F(FaceStateTest, NodeIndicesSingleCell) {
// loop order: l0→z, l1→x
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 2, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
mju_flexGatherFaceState(order, cx, cy, cz, 2, xpos.data(), NULL, NULL, NULL,
NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 0)); // l0=0(z=0), l1=0(x=0)
EXPECT_EQ(indices[1], gidx(1, 0, 0)); // l0=0(z=0), l1=1(x=1)
EXPECT_EQ(indices[2], gidx(0, 0, 1)); // l0=1(z=1), l1=0(x=0)
@@ -1234,8 +1243,8 @@ TEST_F(FaceStateTest, NodeIndicesSingleCell) {
// normal_axis=1, na0=2(z slow), na1=0(x fast)
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL, NULL,
NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 1, 0)); // l0=0(z=0), l1=0(x=0)
EXPECT_EQ(indices[1], gidx(1, 1, 0)); // l0=0(z=0), l1=1(x=1)
EXPECT_EQ(indices[2], gidx(0, 1, 1)); // l0=1(z=1), l1=0(x=0)
@@ -1246,8 +1255,8 @@ TEST_F(FaceStateTest, NodeIndicesSingleCell) {
// normal_axis=2, na0=0, na1=1
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL, NULL,
NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 0));
EXPECT_EQ(indices[1], gidx(0, 1, 0));
EXPECT_EQ(indices[2], gidx(1, 0, 0));
@@ -1257,8 +1266,8 @@ TEST_F(FaceStateTest, NodeIndicesSingleCell) {
// face 5: z=max (fixed g[2]=1)
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 5, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
mju_flexGatherFaceState(order, cx, cy, cz, 5, xpos.data(), NULL, NULL, NULL,
NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 1));
EXPECT_EQ(indices[1], gidx(0, 1, 1));
EXPECT_EQ(indices[2], gidx(1, 0, 1));
@@ -1285,9 +1294,9 @@ TEST_F(FaceStateTest, NodeIndicesMultiCell) {
int gi = i / 9;
int gj = (i / 3) % 3;
int gk = i % 3;
xpos[3*i + 0] = gi * 0.1;
xpos[3*i + 1] = gj * 0.1;
xpos[3*i + 2] = gk * 0.1;
xpos[3 * i + 0] = gi * 0.1;
xpos[3 * i + 1] = gj * 0.1;
xpos[3 * i + 2] = gk * 0.1;
}
auto gidx = [&](int gx, int gy, int gz) {
@@ -1299,8 +1308,8 @@ TEST_F(FaceStateTest, NodeIndicesMultiCell) {
// na0=1, na1=2: g[0]=0, g[1]=0..1, g[2]=0..1
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL, NULL,
NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 0));
EXPECT_EQ(indices[1], gidx(0, 0, 1));
EXPECT_EQ(indices[2], gidx(0, 1, 0));
@@ -1310,8 +1319,8 @@ TEST_F(FaceStateTest, NodeIndicesMultiCell) {
// face 0 (x=0), quad 3: (q0=1, q1=1) → within_face = 1*2+1 = 3
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL, NULL,
NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 1, 1));
EXPECT_EQ(indices[1], gidx(0, 1, 2));
EXPECT_EQ(indices[2], gidx(0, 2, 1));
@@ -1322,8 +1331,8 @@ TEST_F(FaceStateTest, NodeIndicesMultiCell) {
// g[0] = cx*order = 2
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL, NULL,
NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(2, 0, 0));
EXPECT_EQ(indices[1], gidx(2, 0, 1));
EXPECT_EQ(indices[2], gidx(2, 1, 0));
@@ -1344,9 +1353,9 @@ TEST_F(FaceStateTest, NodeIndicesNonCubicGrid) {
int gi = i / 8;
int gj = (i / 4) % 2;
int gk = i % 4;
xpos[3*i + 0] = gi * 0.1;
xpos[3*i + 1] = gj * 0.1;
xpos[3*i + 2] = gk * 0.1;
xpos[3 * i + 0] = gi * 0.1;
xpos[3 * i + 1] = gj * 0.1;
xpos[3 * i + 2] = gk * 0.1;
}
auto gidx = [&](int gx, int gy, int gz) {
@@ -1366,8 +1375,8 @@ TEST_F(FaceStateTest, NodeIndicesNonCubicGrid) {
// Total flat index = 3 + 3 + 2 = 8
{
int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 8, xpos.data(), NULL, NULL,
NULL, NULL, NULL, indices, NULL);
mju_flexGatherFaceState(order, cx, cy, cz, 8, xpos.data(), NULL, NULL, NULL,
NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 1));
EXPECT_EQ(indices[1], gidx(1, 0, 1));
EXPECT_EQ(indices[2], gidx(0, 0, 2));
@@ -1388,9 +1397,9 @@ TEST_F(FaceStateTest, DataGathering) {
std::vector<mjtNum> xpos0(3 * nnodes);
for (int i = 0; i < nnodes; i++) {
for (int d = 0; d < 3; d++) {
xpos[3*i + d] = 10 * i + d;
vel[3*i + d] = 100 * i + d;
xpos0[3*i + d] = 1000 * i + d;
xpos[3 * i + d] = 10 * i + d;
vel[3 * i + d] = 100 * i + d;
xpos0[3 * i + d] = 1000 * i + d;
}
}
@@ -1408,9 +1417,9 @@ TEST_F(FaceStateTest, DataGathering) {
for (int n = 0; n < npe; n++) {
int gi = indices[n];
for (int d = 0; d < 3; d++) {
EXPECT_EQ(xpos_f[3*n + d], xpos[3*gi + d]);
EXPECT_EQ(vel_f[3*n + d], vel[3*gi + d]);
EXPECT_EQ(xpos0_f[3*n + d], xpos0[3*gi + d]);
EXPECT_EQ(xpos_f[3 * n + d], xpos[3 * gi + d]);
EXPECT_EQ(vel_f[3 * n + d], vel[3 * gi + d]);
EXPECT_EQ(xpos0_f[3 * n + d], xpos0[3 * gi + d]);
}
}
}
@@ -1428,9 +1437,9 @@ TEST_F(FaceStateTest, IdentityRotationAxisAligned) {
for (int i = 0; i <= 1; i++) {
for (int j = 0; j <= 1; j++) {
for (int k = 0; k <= 1; k++) {
xpos[3*idx + 0] = i;
xpos[3*idx + 1] = j;
xpos[3*idx + 2] = k;
xpos[3 * idx + 0] = i;
xpos[3 * idx + 1] = j;
xpos[3 * idx + 2] = k;
idx++;
}
}
@@ -1469,7 +1478,7 @@ TEST_F(FaceStateTest, RotatedCubeRotation) {
mjtNum axis[3] = {0, 0, 1};
mjtNum rot_quat[4];
mju_axisAngle2Quat(rot_quat, axis, mjPI / 2);
mju_rotVecQuat(xpos.data() + 3*idx, orig, rot_quat);
mju_rotVecQuat(xpos.data() + 3 * idx, orig, rot_quat);
idx++;
}
}
@@ -1519,7 +1528,7 @@ TEST_F(FaceStateTest, RotationConsistencyWith3D) {
mjtNum axis[3] = {0, 0, 1};
mjtNum rot_quat[4];
mju_axisAngle2Quat(rot_quat, axis, mjPI / 6);
mju_rotVecQuat(xpos.data() + 3*idx, orig, rot_quat);
mju_rotVecQuat(xpos.data() + 3 * idx, orig, rot_quat);
idx++;
}
}
@@ -1543,8 +1552,8 @@ TEST_F(FaceStateTest, RotationConsistencyWith3D) {
xpos_f.data(), NULL, NULL, NULL, quat_2d);
// quaternions may differ by sign; compare unsigned
mjtNum dot = quat_3d[0]*quat_2d[0] + quat_3d[1]*quat_2d[1] +
quat_3d[2]*quat_2d[2] + quat_3d[3]*quat_2d[3];
mjtNum dot = quat_3d[0] * quat_2d[0] + quat_3d[1] * quat_2d[1] +
quat_3d[2] * quat_2d[2] + quat_3d[3] * quat_2d[3];
EXPECT_NEAR(mju_abs(dot), 1.0, 1e-5)
<< "face " << fe << ": 2D rotation differs from 3D cell rotation";
}
@@ -1559,10 +1568,10 @@ static void MakeRegularGrid(mjtNum* nodexpos, int nx, int ny, int nz) {
for (int i = 0; i < nx; i++) {
for (int j = 0; j < ny; j++) {
for (int k = 0; k < nz; k++) {
int idx = i*ny*nz + j*nz + k;
nodexpos[3*idx+0] = (mjtNum)i;
nodexpos[3*idx+1] = (mjtNum)j;
nodexpos[3*idx+2] = (mjtNum)k;
int idx = i * ny * nz + j * nz + k;
nodexpos[3 * idx + 0] = (mjtNum)i;
nodexpos[3 * idx + 1] = (mjtNum)j;
nodexpos[3 * idx + 2] = (mjtNum)k;
}
}
}
@@ -1571,7 +1580,7 @@ static void MakeRegularGrid(mjtNum* nodexpos, int nx, int ny, int nz) {
TEST_F(ShellTFITest, IdentityGrid) {
// 3x3x3 grid: 1 interior node at (1,1,1)
constexpr int nx = 3, ny = 3, nz = 3;
mjtNum nodexpos[3*nx*ny*nz];
mjtNum nodexpos[3 * nx * ny * nz];
MakeRegularGrid(nodexpos, nx, ny, nz);
// save expected interior position
@@ -1581,44 +1590,44 @@ TEST_F(ShellTFITest, IdentityGrid) {
mju_shellTrackInterior(nodexpos, nx, ny, nz);
// interior node at (1,1,1) should match
int idx = 1*ny*nz + 1*nz + 1;
EXPECT_NEAR(nodexpos[3*idx+0], expected[0], MjTol(1e-12, 1e-5));
EXPECT_NEAR(nodexpos[3*idx+1], expected[1], MjTol(1e-12, 1e-5));
EXPECT_NEAR(nodexpos[3*idx+2], expected[2], MjTol(1e-12, 1e-5));
int idx = 1 * ny * nz + 1 * nz + 1;
EXPECT_NEAR(nodexpos[3 * idx + 0], expected[0], MjTol(1e-12, 1e-5));
EXPECT_NEAR(nodexpos[3 * idx + 1], expected[1], MjTol(1e-12, 1e-5));
EXPECT_NEAR(nodexpos[3 * idx + 2], expected[2], MjTol(1e-12, 1e-5));
}
TEST_F(ShellTFITest, UniformScaling) {
// 3x3x3: scale all boundary nodes by 2x, interior should follow
constexpr int nx = 3, ny = 3, nz = 3;
mjtNum nodexpos[3*nx*ny*nz];
mjtNum nodexpos[3 * nx * ny * nz];
MakeRegularGrid(nodexpos, nx, ny, nz);
// scale all nodes
for (int i = 0; i < 3*nx*ny*nz; i++) {
for (int i = 0; i < 3 * nx * ny * nz; i++) {
nodexpos[i] *= 2.0;
}
// run TFI — interior should be reconstructed to 2*original
mju_shellTrackInterior(nodexpos, nx, ny, nz);
int idx = 1*ny*nz + 1*nz + 1;
EXPECT_NEAR(nodexpos[3*idx+0], 2.0, MjTol(1e-12, 1e-5));
EXPECT_NEAR(nodexpos[3*idx+1], 2.0, MjTol(1e-12, 1e-5));
EXPECT_NEAR(nodexpos[3*idx+2], 2.0, MjTol(1e-12, 1e-5));
int idx = 1 * ny * nz + 1 * nz + 1;
EXPECT_NEAR(nodexpos[3 * idx + 0], 2.0, MjTol(1e-12, 1e-5));
EXPECT_NEAR(nodexpos[3 * idx + 1], 2.0, MjTol(1e-12, 1e-5));
EXPECT_NEAR(nodexpos[3 * idx + 2], 2.0, MjTol(1e-12, 1e-5));
}
TEST_F(ShellTFITest, AffineDeformation) {
// 4x4x4 grid with 8 interior nodes. Apply affine transform to boundary,
// then verify TFI reproduces the same affine transform on interior nodes.
constexpr int nx = 4, ny = 4, nz = 4;
mjtNum nodexpos[3*nx*ny*nz];
mjtNum nodexpos[3 * nx * ny * nz];
MakeRegularGrid(nodexpos, nx, ny, nz);
// affine: F(x,y,z) = A*[x,y,z]^T + b
// A = [[2, 0.5, 0], [0.3, 1.5, 0], [0, 0, 1]], b = [10, 20, 30]
auto affine = [](mjtNum x, mjtNum y, mjtNum z, mjtNum out[3]) {
out[0] = 2.0*x + 0.5*y + 10.0;
out[1] = 0.3*x + 1.5*y + 20.0;
out[0] = 2.0 * x + 0.5 * y + 10.0;
out[1] = 0.3 * x + 1.5 * y + 20.0;
out[2] = z + 30.0;
};
@@ -1626,20 +1635,20 @@ TEST_F(ShellTFITest, AffineDeformation) {
for (int i = 0; i < nx; i++) {
for (int j = 0; j < ny; j++) {
for (int k = 0; k < nz; k++) {
int idx = i*ny*nz + j*nz + k;
affine((mjtNum)i, (mjtNum)j, (mjtNum)k, nodexpos + 3*idx);
int idx = i * ny * nz + j * nz + k;
affine((mjtNum)i, (mjtNum)j, (mjtNum)k, nodexpos + 3 * idx);
}
}
}
// corrupt interior nodes to verify TFI actually reconstructs them
for (int i = 1; i < nx-1; i++) {
for (int j = 1; j < ny-1; j++) {
for (int k = 1; k < nz-1; k++) {
int idx = i*ny*nz + j*nz + k;
nodexpos[3*idx+0] = -999;
nodexpos[3*idx+1] = -999;
nodexpos[3*idx+2] = -999;
for (int i = 1; i < nx - 1; i++) {
for (int j = 1; j < ny - 1; j++) {
for (int k = 1; k < nz - 1; k++) {
int idx = i * ny * nz + j * nz + k;
nodexpos[3 * idx + 0] = -999;
nodexpos[3 * idx + 1] = -999;
nodexpos[3 * idx + 2] = -999;
}
}
}
@@ -1648,17 +1657,17 @@ TEST_F(ShellTFITest, AffineDeformation) {
mju_shellTrackInterior(nodexpos, nx, ny, nz);
// check all interior nodes match affine
for (int i = 1; i < nx-1; i++) {
for (int j = 1; j < ny-1; j++) {
for (int k = 1; k < nz-1; k++) {
int idx = i*ny*nz + j*nz + k;
for (int i = 1; i < nx - 1; i++) {
for (int j = 1; j < ny - 1; j++) {
for (int k = 1; k < nz - 1; k++) {
int idx = i * ny * nz + j * nz + k;
mjtNum expected[3];
affine((mjtNum)i, (mjtNum)j, (mjtNum)k, expected);
EXPECT_NEAR(nodexpos[3*idx+0], expected[0], MjTol(1e-12, 1e-4))
EXPECT_NEAR(nodexpos[3 * idx + 0], expected[0], MjTol(1e-12, 1e-4))
<< "i=" << i << " j=" << j << " k=" << k;
EXPECT_NEAR(nodexpos[3*idx+1], expected[1], MjTol(1e-12, 1e-4))
EXPECT_NEAR(nodexpos[3 * idx + 1], expected[1], MjTol(1e-12, 1e-4))
<< "i=" << i << " j=" << j << " k=" << k;
EXPECT_NEAR(nodexpos[3*idx+2], expected[2], MjTol(1e-12, 1e-4))
EXPECT_NEAR(nodexpos[3 * idx + 2], expected[2], MjTol(1e-12, 1e-4))
<< "i=" << i << " j=" << j << " k=" << k;
}
}
@@ -1668,12 +1677,12 @@ TEST_F(ShellTFITest, AffineDeformation) {
TEST_F(ShellTFITest, BoundaryUnmodified) {
// verify that boundary nodes are not modified by TFI
constexpr int nx = 4, ny = 4, nz = 4;
mjtNum nodexpos[3*nx*ny*nz];
mjtNum nodexpos[3 * nx * ny * nz];
MakeRegularGrid(nodexpos, nx, ny, nz);
// save boundary node values
mjtNum saved[3*nx*ny*nz];
mju_copy(saved, nodexpos, 3*nx*ny*nz);
mjtNum saved[3 * nx * ny * nz];
mju_copy(saved, nodexpos, 3 * nx * ny * nz);
mju_shellTrackInterior(nodexpos, nx, ny, nz);
@@ -1681,14 +1690,13 @@ TEST_F(ShellTFITest, BoundaryUnmodified) {
for (int i = 0; i < nx; i++) {
for (int j = 0; j < ny; j++) {
for (int k = 0; k < nz; k++) {
bool is_boundary = (i == 0 || i == nx-1 ||
j == 0 || j == ny-1 ||
k == 0 || k == nz-1);
bool is_boundary = (i == 0 || i == nx - 1 || j == 0 || j == ny - 1 ||
k == 0 || k == nz - 1);
if (is_boundary) {
int idx = i*ny*nz + j*nz + k;
EXPECT_EQ(nodexpos[3*idx+0], saved[3*idx+0]);
EXPECT_EQ(nodexpos[3*idx+1], saved[3*idx+1]);
EXPECT_EQ(nodexpos[3*idx+2], saved[3*idx+2]);
int idx = i * ny * nz + j * nz + k;
EXPECT_EQ(nodexpos[3 * idx + 0], saved[3 * idx + 0]);
EXPECT_EQ(nodexpos[3 * idx + 1], saved[3 * idx + 1]);
EXPECT_EQ(nodexpos[3 * idx + 2], saved[3 * idx + 2]);
}
}
}
@@ -1698,16 +1706,16 @@ TEST_F(ShellTFITest, BoundaryUnmodified) {
TEST_F(ShellTFITest, NoInteriorSmallGrid) {
// 2x2x2 and 2x3x2: no interior nodes, TFI should be a no-op
constexpr int nx = 2, ny = 3, nz = 2;
mjtNum nodexpos[3*nx*ny*nz];
mjtNum nodexpos[3 * nx * ny * nz];
MakeRegularGrid(nodexpos, nx, ny, nz);
mjtNum saved[3*nx*ny*nz];
mju_copy(saved, nodexpos, 3*nx*ny*nz);
mjtNum saved[3 * nx * ny * nz];
mju_copy(saved, nodexpos, 3 * nx * ny * nz);
mju_shellTrackInterior(nodexpos, nx, ny, nz);
// all nodes unchanged
for (int i = 0; i < 3*nx*ny*nz; i++) {
for (int i = 0; i < 3 * nx * ny * nz; i++) {
EXPECT_EQ(nodexpos[i], saved[i]);
}
}
test/engine/engine_util_solve_test.cc
+137 -159
View File
@@ -35,26 +35,25 @@
namespace mujoco {
namespace {
using ::testing::Pointwise;
using ::testing::ElementsAre;
using ::std::setw;
using ::std::string;
using ::std::vector;
using ::std::setw;
using ::testing::ElementsAre;
using QCQP2Test = MujocoTest;
TEST_F(QCQP2Test, DegenerateAMatrix) {
// A 2x2 matrix with determinant zero.
const mjtNum Ain[9] { 6, -15, 2, -5 };
const mjtNum Ain[9]{6, -15, 2, -5};
// Any values will do for these three inputs.
const mjtNum bin[3] { -12, 49 };
const mjtNum d[3] { 11, 31 };
const mjtNum bin[3]{-12, 49};
const mjtNum d[3]{11, 31};
const mjtNum r = 0.01;
// Make output array explicitly nonzero to simulate uninitialized memory.
mjtNum res[2] { 999, 999 };
mjtNum res[2]{999, 999};
EXPECT_EQ(mju_QCQP2(res, Ain, bin, d, r), 0);
EXPECT_EQ(res[0], 0);
@@ -65,15 +64,15 @@ using QCQP3Test = MujocoTest;
TEST_F(QCQP3Test, DegenerateAMatrix) {
// A 3x3 matrix with determinant zero.
const mjtNum Ain[9] { 1, 4, -2, -3, -7, 5, 2, -9, 0 };
const mjtNum Ain[9]{1, 4, -2, -3, -7, 5, 2, -9, 0};
// Any values will do for these three inputs.
const mjtNum bin[3] { -12, 49, 8 };
const mjtNum d[3] { 11, 31, -23 };
const mjtNum bin[3]{-12, 49, 8};
const mjtNum d[3]{11, 31, -23};
const mjtNum r = 0.1;
// Make output array explicitly nonzero to simulate uninitialized memory.
mjtNum res[3] { 999, 999, 999 };
mjtNum res[3]{999, 999, 999};
EXPECT_EQ(mju_QCQP3(res, Ain, bin, d, r), 0);
EXPECT_EQ(res[0], 0);
@@ -93,10 +92,10 @@ mjtNum objective(const mjtNum* x, const mjtNum* H, const mjtNum* g, int n) {
// utility: test if res is the minimum of a given box-QP problem
bool isQPminimum(const mjtNum* res, const mjtNum* H, const mjtNum* g, int n,
const mjtNum* lower, const mjtNum* upper) {
const mjtNum eps = MjTol(1e-4, 5e-2); // epsilon used for nudging
const mjtNum eps = MjTol(1e-4, 5e-2); // epsilon used for nudging
const mjtNum threshold = MjTol(0, -2e-3); // comparison threshold
bool is_minimum = true;
mjtNum* res_nudge = (mjtNum*) mju_malloc(sizeof(mjtNum)*n);
mjtNum* res_nudge = (mjtNum*)mju_malloc(sizeof(mjtNum) * n);
// get solution value
mjtNum value = objective(res, H, g, n);
@@ -104,7 +103,7 @@ bool isQPminimum(const mjtNum* res, const mjtNum* H, const mjtNum* g, int n,
// compare to nudged solution
mju_copy(res_nudge, res, n);
for (int i=0; i < n; i++) {
for (int i = 0; i < n; i++) {
// nudge down
res_nudge[i] = res[i] - eps;
if (lower) {
@@ -144,12 +143,12 @@ void randomBoxQP(int n, mjtNum* H, mjtNum* g, mjtNum* lower, mjtNum* upper,
std::normal_distribution<double> dist(0, 1);
// square root of H
mjtNum* sqrtH = (mjtNum*) mju_malloc(sizeof(mjtNum)*n*n);
mjtNum* sqrtH = (mjtNum*)mju_malloc(sizeof(mjtNum) * n * n);
for (int i=0; i < n; i++) {
for (int i = 0; i < n; i++) {
g[i] = dist(rng);
lower[i] = 5*dist(rng);
upper[i] = 5*dist(rng);
lower[i] = 5 * dist(rng);
upper[i] = 5 * dist(rng);
// fix invalid bounds
if (lower[i] > upper[i]) {
@@ -159,8 +158,8 @@ void randomBoxQP(int n, mjtNum* H, mjtNum* g, mjtNum* lower, mjtNum* upper,
}
// sample temp
for (int j=0; j < n; j++) {
sqrtH[n*i+j] = dist(rng);
for (int j = 0; j < n; j++) {
sqrtH[n * i + j] = dist(rng);
}
}
@@ -174,21 +173,18 @@ void randomBoxQP(int n, mjtNum* H, mjtNum* g, mjtNum* lower, mjtNum* upper,
TEST_F(BoxQPTest, UnboundedQP) {
// small arrays, allocate on stack
static const int n = 2;
mjtNum H[n*n] = {
2, 0,
0, 2
};
mjtNum H[n * n] = {2, 0, 0, 2};
mjtNum g[n] = {1, 3};
mjtNum res[n] = {0, 0};
mjtNum R[n*(n+7)];
mjtNum R[n * (n + 7)];
int nfree = mju_boxQP(res, R, /*index=*/nullptr, H, g, n,
/*lower=*/nullptr, /*upper=*/nullptr);
// no bounds, expect Newton point
EXPECT_EQ(nfree, 2);
EXPECT_MJTNUM_EQ(res[0], -g[0]/H[0]);
EXPECT_MJTNUM_EQ(res[1], -g[1]/H[3]);
EXPECT_MJTNUM_EQ(res[0], -g[0] / H[0]);
EXPECT_MJTNUM_EQ(res[1], -g[1] / H[3]);
// check that solution is actual minimum
EXPECT_TRUE(isQPminimum(res, H, g, n, /*lower=*/nullptr, /*upper=*/nullptr));
@@ -209,23 +205,20 @@ TEST_F(BoxQPTest, UnboundedQP) {
TEST_F(BoxQPTest, AsymmetricUpperIgnored) {
// small arrays, allocate on stack
static const int n = 2;
mjtNum H[n*n] = {
1, -400,
0, 1
};
mjtNum H[n * n] = {1, -400, 0, 1};
mjtNum g[n] = {1, 3};
mjtNum res[n] = {0, 0};
mjtNum lower[n] = {-2, -2};
mjtNum upper[n] = {0, 0};
int index[n];
mjtNum R[n*(n+7)];
mjtNum R[n * (n + 7)];
// solve box-QP
int nfree = mju_boxQP(res, R, index, H, g, n, lower, upper);
EXPECT_EQ(nfree, 1);
EXPECT_MJTNUM_EQ(res[0], -g[0]/H[0]);
EXPECT_MJTNUM_EQ(res[0], -g[0] / H[0]);
EXPECT_MJTNUM_EQ(res[1], lower[1]);
}
@@ -266,9 +259,9 @@ TEST_F(BoxQPTest, UpperTrianglePoisoned) {
// solve with symmetric H to get the reference result
mju_zero(res, n);
int nfree_ref = mju_boxQPoption(res, R, index, H, g, n, lower, upper,
maxiter, mingrad, backtrack, minstep,
armijo, nullptr, 0);
int nfree_ref =
mju_boxQPoption(res, R, index, H, g, n, lower, upper, maxiter, mingrad,
backtrack, minstep, armijo, nullptr, 0);
ASSERT_GT(nfree_ref, -1);
// save reference
@@ -277,25 +270,25 @@ TEST_F(BoxQPTest, UpperTrianglePoisoned) {
std::vector<mjtNum> R_ref(R, R + nfree_ref * nfree_ref);
// poison the strict upper triangle of H with NaN
for (int i=0; i < n; i++) {
for (int j=i+1; j < n; j++) {
H[n*i+j] = nan;
for (int i = 0; i < n; i++) {
for (int j = i + 1; j < n; j++) {
H[n * i + j] = nan;
}
}
// solve again; result must match because only lower triangle should be read
mju_zero(res, n);
int nfree_poisoned = mju_boxQPoption(res, R, index, H, g, n, lower, upper,
maxiter, mingrad, backtrack, minstep,
armijo, nullptr, 0);
int nfree_poisoned =
mju_boxQPoption(res, R, index, H, g, n, lower, upper, maxiter, mingrad,
backtrack, minstep, armijo, nullptr, 0);
EXPECT_EQ(nfree_poisoned, nfree_ref);
for (int i=0; i < n; i++) {
for (int i = 0; i < n; i++) {
EXPECT_EQ(res[i], res_ref[i]) << "mismatch at index " << i;
}
for (int i=0; i < nfree_ref; i++) {
for (int i = 0; i < nfree_ref; i++) {
EXPECT_EQ(index[i], index_ref[i]) << "index mismatch at " << i;
for (int j=0; j <= i; j++) {
for (int j = 0; j <= i; j++) {
int k = i * nfree_ref + j;
EXPECT_EQ(R[k], R_ref[k]) << "R mismatch at row " << i << ", col " << j;
}
@@ -304,7 +297,7 @@ TEST_F(BoxQPTest, UpperTrianglePoisoned) {
// test mju_boxQP on a single random bounded QP
TEST_F(BoxQPTest, BoundedQP) {
int n = 50; // problem size
int n = 50; // problem size
// allocate on heap
mjtNum *H, *g, *lower, *upper; // inputs
@@ -318,19 +311,19 @@ TEST_F(BoxQPTest, BoundedQP) {
mju_zero(res, n);
// use default options
int maxiter = 100; // maximum number of iterations
mjtNum mingrad = MjTol(1E-16, 1E-5); // minimum squared norm of (unclamped) gradient
mjtNum backtrack = 0.5; // backtrack factor for decreasing stepsize
mjtNum minstep = MjTol(1E-22, 1E-10); // minimum stepsize for linesearch
mjtNum armijo = 0.1; // Armijo parameter
int maxiter = 100; // maximum number of iterations
mjtNum mingrad =
MjTol(1E-16, 1E-5); // minimum squared norm of (unclamped) gradient
mjtNum backtrack = 0.5; // backtrack factor for decreasing stepsize
mjtNum minstep = MjTol(1E-22, 1E-10); // minimum stepsize for linesearch
mjtNum armijo = 0.1; // Armijo parameter
// logging
static const int logsz = 10000;
char log[logsz];
int nfree = mju_boxQPoption(res, R, index, H, g, n, lower, upper,
maxiter, mingrad, backtrack,
minstep, armijo, log, logsz);
int nfree = mju_boxQPoption(res, R, index, H, g, n, lower, upper, maxiter,
mingrad, backtrack, minstep, armijo, log, logsz);
// ADD_FAILURE() << log; // uncomment to print `log` to error log
@@ -340,12 +333,12 @@ TEST_F(BoxQPTest, BoundedQP) {
// verify clamping
int j = nfree > 0 ? 0 : -1;
for (int i=0; i < n; i++) {
for (int i = 0; i < n; i++) {
if (j >= 0 && i == index[j]) { // free dimension
EXPECT_GT(res[i], lower[i]);
EXPECT_LT(res[i], upper[i]);
j++;
} else { // clamped dimension
} else { // clamped dimension
EXPECT_TRUE(res[i] == lower[i] || res[i] == upper[i]);
}
}
@@ -388,7 +381,7 @@ TEST_F(BoxQPTest, BoundedQPvariations) {
// make random box-QP
randomBoxQP(n, H, g, lower, upper, seed++);
mju_scl(H, H, scaleH, n*n);
mju_scl(H, H, scaleH, n * n);
mju_scl(g, g, scaleg, n);
mju_scl(lower, lower, scalebounds, n);
mju_scl(upper, upper, scalebounds, n);
@@ -397,16 +390,16 @@ TEST_F(BoxQPTest, BoundedQPvariations) {
mju_zero(res, n);
// default algorithm options
int maxiter = 100;
mjtNum mingrad = MjTol(1E-16, 1E-5);
mjtNum backtrack = 0.5;
mjtNum minstep = MjTol(1E-22, 1E-10);
mjtNum armijo = 0.1;
int maxiter = 100;
mjtNum mingrad = MjTol(1E-16, 1E-5);
mjtNum backtrack = 0.5;
mjtNum minstep = MjTol(1E-22, 1E-10);
mjtNum armijo = 0.1;
// solve box-QP with logging
int nfree = mju_boxQPoption(res, R, index, H, g, n, lower, upper,
maxiter, mingrad, backtrack,
minstep, armijo, log, logsz);
int nfree =
mju_boxQPoption(res, R, index, H, g, n, lower, upper, maxiter,
mingrad, backtrack, minstep, armijo, log, logsz);
// check solution
EXPECT_GT(nfree, -1) << log;
@@ -435,8 +428,8 @@ TEST_F(BoxQPTest, BoundedQPvariations) {
// average of 4.5 factorizations is expected
EXPECT_LE(meanfactor, 5.0);
std::cerr << "n=" << setw(3) << n
<< ": average of " << meanfactor << " factorizations\n";
std::cerr << "n=" << setw(3) << n << ": average of " << meanfactor
<< " factorizations\n";
}
mju_free(res);
mju_free(R);
@@ -461,13 +454,13 @@ void randomBanded(mjtNum* H, int nTotal, int nBand, int nDense, int seed,
std::normal_distribution<double> dist(0, 1);
// allocate square root
mjtNum* sqrtH = (mjtNum*) mju_malloc(sizeof(mjtNum)*nTotal*nTotal);
mjtNum* sqrtH = (mjtNum*)mju_malloc(sizeof(mjtNum) * nTotal * nTotal);
// sample
for (int i=0; i < nTotal; i++) {
for (int i = 0; i < nTotal; i++) {
if (vec) vec[i] = dist(rng);
for (int j=0; j < nTotal; j++) {
sqrtH[nTotal*i+j] = dist(rng);
for (int j = 0; j < nTotal; j++) {
sqrtH[nTotal * i + j] = dist(rng);
}
}
@@ -475,24 +468,23 @@ void randomBanded(mjtNum* H, int nTotal, int nBand, int nDense, int seed,
mju_mulMatTMat(H, sqrtH, sqrtH, nTotal, nTotal, nTotal);
// set zeros
int nSparse = nTotal-nDense;
for (int i=0; i < nSparse; i++) {
int nSparse = nTotal - nDense;
for (int i = 0; i < nSparse; i++) {
int nzeros = mjMAX(0, i + 1 - nBand);
for (int j=0; j < nzeros; j++) {
H[nTotal*i + j] = 0;
H[nTotal*j + i] = 0;
for (int j = 0; j < nzeros; j++) {
H[nTotal * i + j] = 0;
H[nTotal * j + i] = 0;
}
}
// add regularizer to diagonal
for (int i=0; i < nTotal; i++) {
H[nTotal*i + i] += reg;
for (int i = 0; i < nTotal; i++) {
H[nTotal * i + i] += reg;
}
mju_free(sqrtH);
}
// test banded-vector diagonal values
TEST_F(BandMatrixTest, Diagonal) {
int seed = 1;
@@ -500,10 +492,10 @@ TEST_F(BandMatrixTest, Diagonal) {
for (int nBand : {1, 3}) {
for (int nDense : {0, 2}) {
// allocate
int nB = (nTotal-nDense)*nBand + nDense*nTotal;
mjtNum* B = (mjtNum*) mju_malloc(sizeof(mjtNum)*nB);
int nH = nTotal*nTotal;
mjtNum* H = (mjtNum*) mju_malloc(sizeof(mjtNum)*nH);
int nB = (nTotal - nDense) * nBand + nDense * nTotal;
mjtNum* B = (mjtNum*)mju_malloc(sizeof(mjtNum) * nB);
int nH = nTotal * nTotal;
mjtNum* H = (mjtNum*)mju_malloc(sizeof(mjtNum) * nH);
// make random banded SPD matrix, dense representation
randomBanded(H, nTotal, nBand, nDense, seed++);
@@ -512,8 +504,8 @@ TEST_F(BandMatrixTest, Diagonal) {
mju_dense2Band(B, H, nTotal, nBand, nDense);
// expect diagonals to be equal
for (int i=0; i < nTotal; i++) {
EXPECT_EQ(H[i*nTotal + i], B[mju_bandDiag(i, nTotal, nBand, nDense)]);
for (int i = 0; i < nTotal; i++) {
EXPECT_EQ(H[i * nTotal + i], B[mju_bandDiag(i, nTotal, nBand, nDense)]);
}
mju_free(H);
@@ -529,11 +521,11 @@ TEST_F(BandMatrixTest, Conversion) {
for (int nBand : {0, 1, 3}) {
for (int nDense : {0, 2}) {
// allocate
int nB = (nTotal-nDense)*nBand + nDense*nTotal;
mjtNum* B = (mjtNum*) mju_malloc(sizeof(mjtNum)*nB);
int nH = nTotal*nTotal;
mjtNum* H = (mjtNum*) mju_malloc(sizeof(mjtNum)*nH);
mjtNum* H1 = (mjtNum*) mju_malloc(sizeof(mjtNum)*nH);
int nB = (nTotal - nDense) * nBand + nDense * nTotal;
mjtNum* B = (mjtNum*)mju_malloc(sizeof(mjtNum) * nB);
int nH = nTotal * nTotal;
mjtNum* H = (mjtNum*)mju_malloc(sizeof(mjtNum) * nH);
mjtNum* H1 = (mjtNum*)mju_malloc(sizeof(mjtNum) * nH);
// make random banded SPD matrix, dense
randomBanded(H, nTotal, nBand, nDense, seed++);
@@ -561,13 +553,13 @@ TEST_F(BandMatrixTest, Multiplication) {
for (int nBand : {0, 1, 3}) {
for (int nDense : {0, 2}) {
// allocate
int nB = (nTotal-nDense)*nBand + nDense*nTotal;
mjtNum* B = (mjtNum*) mju_malloc(sizeof(mjtNum)*nB);
int nH = nTotal*nTotal;
mjtNum* H = (mjtNum*) mju_malloc(sizeof(mjtNum)*nH);
mjtNum* vec = (mjtNum*) mju_malloc(sizeof(mjtNum)*nTotal);
mjtNum* res = (mjtNum*) mju_malloc(sizeof(mjtNum)*nTotal);
mjtNum* res1 = (mjtNum*) mju_malloc(sizeof(mjtNum)*nTotal);
int nB = (nTotal - nDense) * nBand + nDense * nTotal;
mjtNum* B = (mjtNum*)mju_malloc(sizeof(mjtNum) * nB);
int nH = nTotal * nTotal;
mjtNum* H = (mjtNum*)mju_malloc(sizeof(mjtNum) * nH);
mjtNum* vec = (mjtNum*)mju_malloc(sizeof(mjtNum) * nTotal);
mjtNum* res = (mjtNum*)mju_malloc(sizeof(mjtNum) * nTotal);
mjtNum* res1 = (mjtNum*)mju_malloc(sizeof(mjtNum) * nTotal);
// make random banded SPD matrix, dense
randomBanded(H, nTotal, nBand, nDense, seed++, vec);
@@ -603,14 +595,14 @@ TEST_F(BandMatrixTest, Factorization) {
for (mjtNum diagadd : {0.0, 1.0}) {
for (int diagmul : {0.0, 1.3}) {
// allocate
int nB = (nTotal-nDense)*nBand + nDense*nTotal;
mjtNum* B = (mjtNum*) mju_malloc(sizeof(mjtNum)*nB);
int nH = nTotal*nTotal;
mjtNum* H = (mjtNum*) mju_malloc(sizeof(mjtNum)*nH);
mjtNum* H1 = (mjtNum*) mju_malloc(sizeof(mjtNum)*nH);
mjtNum* vec = (mjtNum*) mju_malloc(sizeof(mjtNum)*nTotal);
mjtNum* res = (mjtNum*) mju_malloc(sizeof(mjtNum)*nTotal);
mjtNum* res1 = (mjtNum*) mju_malloc(sizeof(mjtNum)*nTotal);
int nB = (nTotal - nDense) * nBand + nDense * nTotal;
mjtNum* B = (mjtNum*)mju_malloc(sizeof(mjtNum) * nB);
int nH = nTotal * nTotal;
mjtNum* H = (mjtNum*)mju_malloc(sizeof(mjtNum) * nH);
mjtNum* H1 = (mjtNum*)mju_malloc(sizeof(mjtNum) * nH);
mjtNum* vec = (mjtNum*)mju_malloc(sizeof(mjtNum) * nTotal);
mjtNum* res = (mjtNum*)mju_malloc(sizeof(mjtNum) * nTotal);
mjtNum* res1 = (mjtNum*)mju_malloc(sizeof(mjtNum) * nTotal);
// make random banded matrix, dense representation
// add regularizer to ensure PD
@@ -620,8 +612,8 @@ TEST_F(BandMatrixTest, Factorization) {
mju_dense2Band(B, H, nTotal, nBand, nDense);
// apply diagadd and diagmul
for (int i=0; i < nTotal; i++) {
H[nTotal*i + i] += diagadd + diagmul*H[nTotal*i + i];
for (int i = 0; i < nTotal; i++) {
H[nTotal * i + i] += diagadd + diagmul * H[nTotal * i + i];
}
// in-place dense factorization
@@ -631,8 +623,8 @@ TEST_F(BandMatrixTest, Factorization) {
EXPECT_EQ(rank, nTotal);
// banded factorization
mjtNum minDiag = mju_cholFactorBand(B, nTotal, nBand, nDense,
diagadd, diagmul);
mjtNum minDiag =
mju_cholFactorBand(B, nTotal, nBand, nDense, diagadd, diagmul);
// expect factorization to have succeeded
EXPECT_GT(minDiag, 0);
@@ -641,8 +633,8 @@ TEST_F(BandMatrixTest, Factorization) {
mju_band2Dense(H1, B, nTotal, nBand, nDense, /*flg_sym=*/0);
// zero upper triangle of H (unused)
for (int i=0; i < nTotal-1; i++) {
mju_zero(H + nTotal*i + i + 1, nTotal - i - 1);
for (int i = 0; i < nTotal - 1; i++) {
mju_zero(H + nTotal * i + i + 1, nTotal - i - 1);
}
// expect numerical equality
@@ -680,14 +672,14 @@ TEST_F(BandMatrixTest, Solve) {
for (int nBand : {1, 3}) {
for (int nDense : {0, 2}) {
// allocate
int nB = (nTotal-nDense)*nBand + nDense*nTotal;
mjtNum* B = (mjtNum*) mju_malloc(sizeof(mjtNum)*nB);
int nH = nTotal*nTotal;
mjtNum* H = (mjtNum*) mju_malloc(sizeof(mjtNum)*nH);
mjtNum* H1 = (mjtNum*) mju_malloc(sizeof(mjtNum)*nH);
mjtNum* vec = (mjtNum*) mju_malloc(sizeof(mjtNum)*nTotal);
mjtNum* res = (mjtNum*) mju_malloc(sizeof(mjtNum)*nTotal);
mjtNum* res1 = (mjtNum*) mju_malloc(sizeof(mjtNum)*nTotal);
int nB = (nTotal - nDense) * nBand + nDense * nTotal;
mjtNum* B = (mjtNum*)mju_malloc(sizeof(mjtNum) * nB);
int nH = nTotal * nTotal;
mjtNum* H = (mjtNum*)mju_malloc(sizeof(mjtNum) * nH);
mjtNum* H1 = (mjtNum*)mju_malloc(sizeof(mjtNum) * nH);
mjtNum* vec = (mjtNum*)mju_malloc(sizeof(mjtNum) * nTotal);
mjtNum* res = (mjtNum*)mju_malloc(sizeof(mjtNum) * nTotal);
mjtNum* res1 = (mjtNum*)mju_malloc(sizeof(mjtNum) * nTotal);
// make random banded matrix, dense representation
// add regularizer to ensure PD
@@ -736,8 +728,8 @@ TEST_F(BandMatrixTest, Solve) {
using EngineUtilSolveTest = MujocoTest;
TEST_F(EngineUtilSolveTest, MjuCholFactorSymbolic) {
mjModel* model = LoadModelFromString("<mujoco/>");
mjData* d = mj_makeData(model);
MjModelPtr model = LoadModelFromString("<mujoco/>");
MjDataPtr d = MakeData(model);
// Test matrix (upper triangular, representing symmetric matrix):
// [10 1 2 3]
@@ -760,7 +752,7 @@ TEST_F(EngineUtilSolveTest, MjuCholFactorSymbolic) {
int LT_rownnz[4], LT_rowadr[4];
int nnz = mju_cholFactorSymbolic(nullptr, L_rownnz, L_rowadr, nullptr,
LT_rownnz, LT_rowadr, nullptr, H_rownnz,
H_rowadr, H_colind, n, d);
H_rowadr, H_colind, n, d.get());
// verify counting phase outputs
EXPECT_EQ(nnz, 8);
@@ -774,7 +766,8 @@ TEST_F(EngineUtilSolveTest, MjuCholFactorSymbolic) {
// phase 2: filling
int L_colind[8], LT_colind[8], LT_pos[8];
mju_cholFactorSymbolic(L_colind, L_rownnz, L_rowadr, LT_colind, LT_rownnz,
LT_rowadr, LT_pos, H_rownnz, H_rowadr, H_colind, n, d);
LT_rowadr, LT_pos, H_rownnz, H_rowadr, H_colind, n,
d.get());
// verify L_colind
EXPECT_THAT(AsVector(L_colind, 8), ElementsAre(0, 0, 1, 0, 2, 0, 2, 3));
@@ -801,9 +794,6 @@ TEST_F(EngineUtilSolveTest, MjuCholFactorSymbolic) {
<< "LT_pos mismatch at column " << c << ", entry " << k;
}
}
mj_deleteData(d);
mj_deleteModel(model);
}
// Test for mju_cholUpdate: rank-one Cholesky update L*L' +/- x*x'
@@ -904,8 +894,8 @@ TEST_F(EngineUtilSolveTest, MjuCholUpdate) {
// Test for mju_cholUpdateSparse: sparse rank-one Cholesky update L'*L +/- x*x'
// Uses sparse reverse Cholesky factorization and sparse rank-one update.
TEST_F(EngineUtilSolveTest, MjuCholUpdateSparse) {
mjModel* model = LoadModelFromString("<mujoco/>");
mjData* d = mj_makeData(model);
MjModelPtr model = LoadModelFromString("<mujoco/>");
MjDataPtr d = MakeData(model);
std::mt19937_64 rng;
rng.seed(123);
@@ -987,12 +977,12 @@ TEST_F(EngineUtilSolveTest, MjuCholUpdateSparse) {
// sparse Cholesky factorization (reverse-order: L'*L)
mju_cholFactorSparse(L_sparse.data(), n, 0, rownnz.data(), rowadr.data(),
colind.data(), d);
colind.data(), d.get());
// apply sparse rank-one update
int rank_sparse = mju_cholUpdateSparse(
L_sparse.data(), x_sparse.data(), n, flg_plus, rownnz.data(),
rowadr.data(), colind.data(), x_nnz, x_ind.data(), d);
rowadr.data(), colind.data(), x_nnz, x_ind.data(), d.get());
EXPECT_EQ(rank_sparse, n)
<< "Sparse update rank loss for n=" << n << ", flg_plus=" << flg_plus;
@@ -1014,16 +1004,13 @@ TEST_F(EngineUtilSolveTest, MjuCholUpdateSparse) {
}
}
}
mj_deleteData(d);
mj_deleteModel(model);
}
// Test that mju_cholFactorSymbolic + mju_cholFactorNumeric produces identical
// results to the reference implementation mju_cholFactorSparse
TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) {
mjModel* model = LoadModelFromString("<mujoco/>");
mjData* d = mj_makeData(model);
MjModelPtr model = LoadModelFromString("<mujoco/>");
MjDataPtr d = MakeData(model);
// test matrix with fill-in: upper triangle structure
int n = 4;
@@ -1044,13 +1031,13 @@ TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) {
int LT_rownnz[4], LT_rowadr[4];
int nnz = mju_cholFactorSymbolic(nullptr, L_rownnz, L_rowadr, nullptr,
LT_rownnz, LT_rowadr, nullptr, HT_rownnz,
HT_rowadr, HT_colind, n, d);
HT_rowadr, HT_colind, n, d.get());
// filling phase: compute L_colind, LT_colind, and LT_pos
int L_colind[16], LT_colind[16], LT_pos[16];
mju_cholFactorSymbolic(L_colind, L_rownnz, L_rowadr, LT_colind, LT_rownnz,
LT_rowadr, LT_pos, HT_rownnz, HT_rowadr, HT_colind, n,
d);
d.get());
// verify LT structure matches what we'd get from a separate transpose
int LT_rownnz_ref[4], LT_rowadr_ref[4], LT_colind_ref[16];
@@ -1080,7 +1067,7 @@ TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) {
mjtNum L_new[16];
int rank_new = mju_cholFactorNumeric(
L_new, n, 1e-10, L_rownnz, L_rowadr, L_colind, LT_rownnz, LT_rowadr,
LT_colind, LT_pos, sparseH, H_rownnz, H_rowadr, H_colind, d);
LT_colind, LT_pos, sparseH, H_rownnz, H_rowadr, H_colind, d.get());
// reference implementation: copy sparse H into L_ref, then factor in-place
mjtNum L_ref[16];
@@ -1099,7 +1086,7 @@ TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) {
L_ref_rownnz[r] = lower_nnz;
}
int rank_ref = mju_cholFactorSparse(L_ref, n, 1e-10, L_ref_rownnz, L_rowadr,
L_ref_colind, d);
L_ref_colind, d.get());
// compare results
EXPECT_EQ(rank_new, rank_ref);
@@ -1110,9 +1097,6 @@ TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) {
for (int i = 0; i < nnz; i++) {
EXPECT_NEAR(L_new[i], L_ref[i], eps) << "mismatch at index " << i;
}
mj_deleteData(d);
mj_deleteModel(model);
}
// ----------------------------- dense LU --------------------------------------
@@ -1122,11 +1106,8 @@ using DenseLUTest = MujocoTest;
// factor identity, solve recovers b exactly
TEST_F(DenseLUTest, Identity) {
constexpr int n = 4;
mjtNum A[n*n] = {
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1,
mjtNum A[n * n] = {
1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1,
};
int pivot[n];
mjtNum b[n] = {1, 2, 3, 4};
@@ -1145,10 +1126,8 @@ TEST_F(DenseLUTest, SmallKnown) {
constexpr int n = 3;
// A = [2 1 1; 4 3 3; 8 7 9], b = [1; 1; 1]
// solution: x = [1; -1; 0] (verified: A*x = [2-1; 4-3; 8-7] = [1;1;1])
mjtNum A[n*n] = {
2, 1, 1,
4, 3, 3,
8, 7, 9,
mjtNum A[n * n] = {
2, 1, 1, 4, 3, 3, 8, 7, 9,
};
int pivot[n];
mjtNum b[n] = {1, 1, 1};
@@ -1158,9 +1137,9 @@ TEST_F(DenseLUTest, SmallKnown) {
mju_solveLU(x, A, b, pivot, n);
mjtNum eps = MjTol(1e-14, 1e-6);
EXPECT_NEAR(x[0], 1, eps);
EXPECT_NEAR(x[0], 1, eps);
EXPECT_NEAR(x[1], -1, eps);
EXPECT_NEAR(x[2], 0, eps);
EXPECT_NEAR(x[2], 0, eps);
}
// random SPD matrices: compare LU solve against Cholesky solve
@@ -1183,7 +1162,7 @@ TEST_F(DenseLUTest, RandomSPD) {
mju_mulMatTMat(A.data(), sqrtH.data(), sqrtH.data(), n, n, n);
// add diagonal regularizer
for (int i = 0; i < n; i++) A[i*n+i] += n;
for (int i = 0; i < n; i++) A[i * n + i] += n;
// generate random rhs
for (int i = 0; i < n; i++) b[i] = dist(rng);
@@ -1202,8 +1181,7 @@ TEST_F(DenseLUTest, RandomSPD) {
// compare
mjtNum eps = MjTol(1e-15, 1e-7);
EXPECT_THAT(AsVector(x_lu.data(), n),
Pointwise(MjNear(eps, eps),
AsVector(x_chol.data(), n)));
Pointwise(MjNear(eps, eps), AsVector(x_chol.data(), n)));
}
}
@@ -1224,9 +1202,9 @@ TEST_F(DenseLUTest, RandomGeneral) {
// random non-symmetric matrix with diagonal dominance
for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++) {
A[i*n+j] = dist(rng);
A[i * n + j] = dist(rng);
}
A[i*n+i] += 2 * n;
A[i * n + i] += 2 * n;
}
mju_copy(A_orig.data(), A.data(), n * n);
@@ -1251,7 +1229,7 @@ TEST_F(DenseLUTest, RandomGeneral) {
TEST_F(DenseLUTest, Singular) {
constexpr int n = 3;
// all zeros: maximally singular
mjtNum A[n*n] = {0};
mjtNum A[n * n] = {0};
int pivot[n];
EXPECT_EQ(mju_factorLU(A, n, pivot), 0);
test/engine/engine_util_sparse_test.cc
+223 -358
View File
File diff suppressed because it is too large Load Diff
test/engine/engine_vis_visualize_test.cc
+7 -13
View File
@@ -44,9 +44,7 @@ class MjvSceneTest : public MujocoTest {
}
}
void FreeSceneObjects() {
mjv_freeScene(&scn_);
}
void FreeSceneObjects() { mjv_freeScene(&scn_); }
mjvScene scn_;
mjvOption opt_;
@@ -80,7 +78,6 @@ TEST_F(MjvSceneTest, UpdateScene) {
mj_deleteData(data_copy);
mj_deleteData(data);
FreeSceneObjects();
mj_deleteModel(model);
}
@@ -101,7 +98,6 @@ TEST_F(MjvSceneTest, UpdateSceneGeomsExhausted) {
mjv_updateScene(model, data, &opt_, &pert_, &cam_, mjCAT_ALL, &scn_);
EXPECT_EQ(scn_.status, 1);
EXPECT_EQ(scn_.ngeom, maxgeoms);
mj_deleteData(data);
FreeSceneObjects();
mj_deleteModel(model);
@@ -118,17 +114,17 @@ TEST_F(MjvSceneTest, PrincipalPointFrustumSign) {
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
ASSERT_THAT(model, NotNull());
mjData* data = mj_makeData(model);
mj_forward(model, data);
MjModelPtr model = LoadModelFromString(xml);
ASSERT_THAT(model.get(), NotNull());
MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get());
InitSceneObjects(model);
InitSceneObjects(model.get());
// point camera at the fixed cam
cam_.type = mjCAMERA_FIXED;
cam_.fixedcamid = 0;
mjv_updateCamera(model, data, &cam_, &scn_);
mjv_updateCamera(model.get(), data.get(), &cam_, &scn_);
float top = scn_.camera[0].frustum_top;
float bottom = scn_.camera[0].frustum_bottom;
@@ -148,9 +144,7 @@ TEST_F(MjvSceneTest, PrincipalPointFrustumSign) {
EXPECT_FLOAT_EQ(top, half - offset);
EXPECT_FLOAT_EQ(bottom, -(half + offset));
mj_deleteData(data);
FreeSceneObjects();
mj_deleteModel(model);
}
} // namespace