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

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
+1 -2
View File
@@ -120,8 +120,7 @@ void BM_CompileManyTextures(benchmark::State& state) {
for (auto s : state) { for (auto s : state) {
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
mj_deleteModel(model);
} }
// Reset cache capacity to default value. // Reset cache capacity to default value.
+21 -30
View File
@@ -72,8 +72,8 @@ TEST_F(MjCollisionBoxTest, BadContacts) {
g2 = g2new; g2 = g2new;
// call low-level box-box collider // call low-level box-box collider
int num = mjc_BoxBox(model, data, precon.data(), g1, g2, int num =
con->includemargin); mjc_BoxBox(model, data, precon.data(), g1, g2, con->includemargin);
// allocate and clear arrays marking already matched contacts // allocate and clear arrays marking already matched contacts
mju_zeroInt(match_raw.data(), num); mju_zeroInt(match_raw.data(), num);
@@ -83,8 +83,7 @@ TEST_F(MjCollisionBoxTest, BadContacts) {
int nmatched = 0; int nmatched = 0;
for (int i = 0; i < num; i++) { for (int i = 0; i < num; i++) {
for (int j = 0; j < data->ncon; j++) { for (int j = 0; j < data->ncon; j++) {
if (!match[j] && if (!match[j] && precon[i].pos[0] == data->contact[j].pos[0] &&
precon[i].pos[0] == data->contact[j].pos[0] &&
precon[i].pos[1] == data->contact[j].pos[1] && precon[i].pos[1] == data->contact[j].pos[1] &&
precon[i].pos[2] == data->contact[j].pos[2]) { precon[i].pos[2] == data->contact[j].pos[2]) {
match_raw[i] = match[j] = 1; match_raw[i] = match[j] = 1;
@@ -110,17 +109,17 @@ TEST_F(MjCollisionBoxTest, BadContacts) {
if (!match_raw[i]) { if (!match_raw[i]) {
// === check if outside // === check if outside
mjtNum sz1[3] = {size1[0]+margin, size1[1]+margin, size1[2]+margin}; mjtNum sz1[3] = {size1[0] + margin, size1[1] + margin,
mjtNum sz2[3] = {size2[0]+margin, size2[1]+margin, size2[2]+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 // relative distance (1%) outside of which contacts are removed
static mjtNum kRatio = 1.01; static mjtNum kRatio = 1.01;
// is the contact outside: 1, inside: -1, within the removal width: 0 // is the contact outside: 1, inside: -1, within the removal width: 0
int out1 = mju_outsideBox(precon[i].pos, pos1, mat1, sz1, int out1 = mju_outsideBox(precon[i].pos, pos1, mat1, sz1, kRatio);
kRatio); int out2 = mju_outsideBox(precon[i].pos, pos2, mat2, sz2, 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 // mark as bad if outside one box and not inside the other box
bool outside = (out1 == 1 && out2 != -1) || (out2 == 1 && out1 != -1); 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_raw(mjMAXCONPAIR);
std::vector<int> match(data->ncon); std::vector<int> match(data->ncon);
int g1 = -1; int g1 = -1;
int g2 = -1; int g2 = -1;
for (int c = 0; c < data->ncon; c++) { for (int c = 0; c < data->ncon; c++) {
@@ -174,8 +172,8 @@ TEST_F(MjCollisionBoxTest, DuplicateContacts) {
g2 = g2new; g2 = g2new;
// call low-level box-box collider // call low-level box-box collider
int num = mjc_BoxBox(model, data, precon.data(), g1, g2, int num =
con->includemargin); mjc_BoxBox(model, data, precon.data(), g1, g2, con->includemargin);
// allocate and clear arrays marking already matched contacts // allocate and clear arrays marking already matched contacts
mju_zeroInt(match_raw.data(), num); mju_zeroInt(match_raw.data(), num);
@@ -185,8 +183,7 @@ TEST_F(MjCollisionBoxTest, DuplicateContacts) {
int nmatched = 0; int nmatched = 0;
for (int i = 0; i < num; i++) { for (int i = 0; i < num; i++) {
for (int j = 0; j < data->ncon; j++) { for (int j = 0; j < data->ncon; j++) {
if (!match[j] && if (!match[j] && precon[i].pos[0] == data->contact[j].pos[0] &&
precon[i].pos[0] == data->contact[j].pos[0] &&
precon[i].pos[1] == data->contact[j].pos[1] && precon[i].pos[1] == data->contact[j].pos[1] &&
precon[i].pos[2] == data->contact[j].pos[2]) { precon[i].pos[2] == data->contact[j].pos[2]) {
match_raw[i] = match[j] = 1; match_raw[i] = match[j] = 1;
@@ -255,20 +252,17 @@ TEST_F(MjCollisionBoxTest, BoxSphere) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
for (mjtNum z : {-.015, -.00501, -.005, -.00499, 0.0, 0.004}) { for (mjtNum z : {-.015, -.00501, -.005, -.00499, 0.0, 0.004}) {
data->qpos[2] = z; data->qpos[2] = z;
mj_forward(model, data); mj_forward(model.get(), data.get());
EXPECT_EQ(data->ncon, 2); EXPECT_EQ(data->ncon, 2);
EXPECT_THAT(data->contact[0].dist, EXPECT_THAT(data->contact[0].dist,
MjNear(data->contact[1].dist, 1e-8, 1e-6)); MjNear(data->contact[1].dist, 1e-8, 1e-6));
} }
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(MjCollisionBoxTest, BoxBoxContactDistance) { TEST_F(MjCollisionBoxTest, BoxBoxContactDistance) {
@@ -281,22 +275,19 @@ TEST_F(MjCollisionBoxTest, BoxBoxContactDistance) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
mj_kinematics(model, data); mj_kinematics(model.get(), data.get());
mjPreContact precon[mjMAXCONPAIR]; mjPreContact precon[mjMAXCONPAIR];
for (mjfCollision collision : {mjc_BoxBox, mjc_Convex}) { 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++) { for (int i = 0; i < n; i++) {
EXPECT_NEAR(precon[i].dist, -0.5, MjTol(1e-8, 1e-6)); EXPECT_NEAR(precon[i].dist, -0.5, MjTol(1e-8, 1e-6));
} }
} }
mj_deleteData(data);
mj_deleteModel(model);
} }
} // namespace } // namespace
+90 -112
View File
@@ -14,6 +14,8 @@
// Tests for engine/engine_collision_driver.c. // Tests for engine/engine_collision_driver.c.
#include "src/engine/engine_collision_driver.h"
#include <algorithm> #include <algorithm>
#include <cmath> #include <cmath>
#include <cstddef> #include <cstddef>
@@ -26,22 +28,20 @@
#include <mujoco/mjmodel.h> #include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h> #include <mujoco/mujoco.h>
#include "test/fixture.h" #include "test/fixture.h"
#include "src/engine/engine_collision_driver.h"
namespace mujoco { namespace mujoco {
namespace { namespace {
using MjCollisionTest = MujocoTest; using MjCollisionTest = MujocoTest;
using GeomPair = std::pair<std::string, std::string>; using GeomPair = std::pair<std::string, std::string>;
using ::testing::IsEmpty;
using ::testing::ElementsAre; using ::testing::ElementsAre;
using ::testing::IsEmpty;
using ::testing::NotNull; using ::testing::NotNull;
// Returns a sorted list of pairs of colliding geom names, where each pair of // Returns a sorted list of pairs of colliding geom names, where each pair of
// geom names is sorted. // geom names is sorted.
static std::vector<GeomPair> colliding_pairs( static std::vector<GeomPair> colliding_pairs(const mjModel* model,
const mjModel* model, const mjData* data) { const mjData* data) {
std::vector<GeomPair> result; std::vector<GeomPair> result;
for (int i = 0; i < data->ncon; i++) { for (int i = 0; i < data->ncon; i++) {
std::string geom1 = mj_id2name(model, mjOBJ_GEOM, data->contact[i].geom[0]); 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) { TEST_F(MjCollisionTest, AllCollisions) {
static const char* const kModelFilePath = static const char* const kModelFilePath = "engine/testdata/collisions.xml";
"engine/testdata/collisions.xml";
const std::string xml_path = GetTestDataFilePath(kModelFilePath); const std::string xml_path = GetTestDataFilePath(kModelFilePath);
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); mjData* data = mj_makeData(model);
// mjCOL_ALL is the default // mjCOL_ALL is the default
mj_fwdPosition(model, data); mj_fwdPosition(model, data);
EXPECT_THAT(colliding_pairs(model, data), ElementsAre( EXPECT_THAT(colliding_pairs(model, data),
GeomPair("box", "sphere_collides"), ElementsAre(GeomPair("box", "sphere_collides"),
GeomPair("box", "sphere_predefined") GeomPair("box", "sphere_predefined")));
));
mj_deleteData(data); mj_deleteData(data);
mj_deleteModel(model); mj_deleteModel(model);
@@ -72,20 +70,16 @@ TEST_F(MjCollisionTest, AllCollisions) {
TEST_F(MjCollisionTest, EmptyModel) { TEST_F(MjCollisionTest, EmptyModel) {
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString("<mujoco/>", error, sizeof(error)); MjModelPtr model = LoadModelFromString("<mujoco/>", error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
mj_fwdPosition(model, data); mj_fwdPosition(model.get(), data.get());
EXPECT_THAT(colliding_pairs(model, data), IsEmpty()); EXPECT_THAT(colliding_pairs(model.get(), data.get()), IsEmpty());
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(MjCollisionTest, ZeroedHessian) { TEST_F(MjCollisionTest, ZeroedHessian) {
static const char* const kModelFilePath = static const char* const kModelFilePath = "engine/testdata/collisions.xml";
"engine/testdata/collisions.xml";
const std::string xml_path = GetTestDataFilePath(kModelFilePath); const std::string xml_path = GetTestDataFilePath(kModelFilePath);
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); mjData* data = mj_makeData(model);
@@ -123,18 +117,15 @@ TEST_F(MjCollisionTest, ContactCount) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull()); ASSERT_THAT(d, NotNull());
mj_forward(m, d); mj_forward(m.get(), d.get());
// there are 8 spheres, all touching the floor // there are 8 spheres, all touching the floor
EXPECT_EQ(d->ncon, 8); EXPECT_EQ(d->ncon, 8);
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(MjCollisionTest, FilterParent) { TEST_F(MjCollisionTest, FilterParent) {
@@ -159,12 +150,12 @@ TEST_F(MjCollisionTest, FilterParent) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull()); 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 // there should be zero contacts, because colliding1 and colliding2 are in
// bodies that have a parent-child relationship, through welds // 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 // when this filtering is disabled, the geoms should collide
m->opt.disableflags |= mjDSBL_FILTERPARENT; 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"))); ElementsAre(GeomPair("colliding1", "colliding2")));
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(MjCollisionTest, FilterParentDoesntAffectWorldBody) { TEST_F(MjCollisionTest, FilterParentDoesntAffectWorldBody) {
@@ -194,20 +182,17 @@ TEST_F(MjCollisionTest, FilterParentDoesntAffectWorldBody) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull()); 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, // even though colliding1 and colliding2 are have a parent-child relationship,
// they collide because colliding1 is in <worldbody> // 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"))); ElementsAre(GeomPair("colliding1", "colliding2")));
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(MjCollisionTest, TestOBB) { TEST_F(MjCollisionTest, TestOBB) {
@@ -219,14 +204,18 @@ TEST_F(MjCollisionTest, TestOBB) {
mjtNum mat2[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; mjtNum mat2[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
EXPECT_THAT( 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 // rotate by 45 degrees
mat2[0] = 1./mju_sqrt(2.); mat2[1] = -1./mju_sqrt(2.); mat2[0] = 1. / mju_sqrt(2.);
mat2[3] = 1./mju_sqrt(2.); mat2[4] = 1./mju_sqrt(2.); mat2[1] = -1. / mju_sqrt(2.);
mat2[3] = 1. / mju_sqrt(2.);
mat2[4] = 1. / mju_sqrt(2.);
EXPECT_THAT( 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) { TEST_F(MjCollisionTest, PlaneInBody) {
@@ -244,13 +233,11 @@ TEST_F(MjCollisionTest, PlaneInBody) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull()); ASSERT_THAT(d, NotNull());
mj_step(m, d); mj_step(m.get(), d.get());
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(MjCollisionTest, PinchingSucceeds) { TEST_F(MjCollisionTest, PinchingSucceeds) {
@@ -299,13 +286,13 @@ TEST_F(MjCollisionTest, PinchingSucceeds) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull()); ASSERT_THAT(d, NotNull());
int lift_id = mj_name2id(m, mjOBJ_ACTUATOR, "lift"); int lift_id = mj_name2id(m.get(), mjOBJ_ACTUATOR, "lift");
int grasp_id = mj_name2id(m, mjOBJ_ACTUATOR, "grasp"); int grasp_id = mj_name2id(m.get(), mjOBJ_ACTUATOR, "grasp");
// Phase 1: Lower gripper. // Phase 1: Lower gripper.
// The gripper base starts at z=0.5. The finger has length 0.2 (size 0.1), // 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) { for (int i = 0; i < 500; ++i) {
d->ctrl[lift_id] = -0.35; // Lower d->ctrl[lift_id] = -0.35; // Lower
d->ctrl[grasp_id] = 0; // Open d->ctrl[grasp_id] = 0; // Open
mj_step(m, d); mj_step(m.get(), d.get());
} }
// Phase 2: Pinch // Phase 2: Pinch
for (int i = 0; i < 100; ++i) { for (int i = 0; i < 100; ++i) {
d->ctrl[lift_id] = -0.35; // Hold height d->ctrl[lift_id] = -0.35; // Hold height
d->ctrl[grasp_id] = 0.8; // Close (max 1) d->ctrl[grasp_id] = 0.8; // Close (max 1)
mj_step(m, d); mj_step(m.get(), d.get());
} }
// Phase 3: Lift // Phase 3: Lift
for (int i = 0; i < 1000; ++i) { for (int i = 0; i < 1000; ++i) {
d->ctrl[lift_id] = 0.5; // Lift up d->ctrl[lift_id] = 0.5; // Lift up
d->ctrl[grasp_id] = 0.8; // Keep closed d->ctrl[grasp_id] = 0.8; // Keep closed
mj_step(m, d); mj_step(m.get(), d.get());
} }
// Check if cloth is lifted // Check if cloth is lifted
@@ -352,9 +339,6 @@ TEST_F(MjCollisionTest, PinchingSucceeds) {
// Specialized primitives (mjraw_BoxTriangle, mjraw_CapsuleTriangle) should // Specialized primitives (mjraw_BoxTriangle, mjraw_CapsuleTriangle) should
// enable stable pinching, so we expect the cloth to be lifted. // enable stable pinching, so we expect the cloth to be lifted.
EXPECT_GT(avg_z, 0.2) << "Cloth slipped out of gripper!"; EXPECT_GT(avg_z, 0.2) << "Cloth slipped out of gripper!";
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(MjCollisionTest, MarginSumming) { TEST_F(MjCollisionTest, MarginSumming) {
@@ -376,18 +360,15 @@ TEST_F(MjCollisionTest, MarginSumming) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull()); ASSERT_THAT(d, NotNull());
mj_fwdPosition(m, d); mj_fwdPosition(m.get(), d.get());
// With margin summing, we expect 1 contact // With margin summing, we expect 1 contact
EXPECT_EQ(d->ncon, 1); EXPECT_EQ(d->ncon, 1);
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(MjCollisionTest, MaxContact) { TEST_F(MjCollisionTest, MaxContact) {
@@ -414,48 +395,45 @@ TEST_F(MjCollisionTest, MaxContact) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull()); ASSERT_THAT(d, NotNull());
int mesh = mj_name2id(m, mjOBJ_GEOM, "mesh"); int mesh = mj_name2id(m.get(), mjOBJ_GEOM, "mesh");
int box = mj_name2id(m, mjOBJ_GEOM, "box"); int box = mj_name2id(m.get(), mjOBJ_GEOM, "box");
int plane = mj_name2id(m, mjOBJ_GEOM, "plane"); int plane = mj_name2id(m.get(), mjOBJ_GEOM, "plane");
int sphere = mj_name2id(m, mjOBJ_GEOM, "sphere"); int sphere = mj_name2id(m.get(), mjOBJ_GEOM, "sphere");
int capsule = mj_name2id(m, mjOBJ_GEOM, "capsule"); int capsule = mj_name2id(m.get(), mjOBJ_GEOM, "capsule");
int ellipsoid = mj_name2id(m, mjOBJ_GEOM, "ellipsoid"); int ellipsoid = mj_name2id(m.get(), mjOBJ_GEOM, "ellipsoid");
int cylinder = mj_name2id(m, mjOBJ_GEOM, "cylinder"); int cylinder = mj_name2id(m.get(), mjOBJ_GEOM, "cylinder");
EXPECT_EQ(mj_maxContact(m, mesh, box, -1), 4); EXPECT_EQ(mj_maxContact(m.get(), mesh, box, -1), 4);
EXPECT_EQ(mj_maxContact(m, mesh, plane, -1), 3); EXPECT_EQ(mj_maxContact(m.get(), mesh, plane, -1), 3);
EXPECT_EQ(mj_maxContact(m, box, plane, -1), 4); EXPECT_EQ(mj_maxContact(m.get(), box, plane, -1), 4);
EXPECT_EQ(mj_maxContact(m, mesh, mesh, -1), 4); EXPECT_EQ(mj_maxContact(m.get(), mesh, mesh, -1), 4);
EXPECT_EQ(mj_maxContact(m, box, box, -1), 8); EXPECT_EQ(mj_maxContact(m.get(), box, box, -1), 8);
EXPECT_EQ(mj_maxContact(m, capsule, capsule, -1), 2); EXPECT_EQ(mj_maxContact(m.get(), capsule, capsule, -1), 2);
EXPECT_EQ(mj_maxContact(m, capsule, box, -1), 4); EXPECT_EQ(mj_maxContact(m.get(), capsule, box, -1), 4);
EXPECT_EQ(mj_maxContact(m, capsule, plane, -1), 2); EXPECT_EQ(mj_maxContact(m.get(), capsule, plane, -1), 2);
EXPECT_EQ(mj_maxContact(m, cylinder, plane, -1), 4); EXPECT_EQ(mj_maxContact(m.get(), cylinder, plane, -1), 4);
EXPECT_EQ(mj_maxContact(m, sphere, sphere, -1), 1); EXPECT_EQ(mj_maxContact(m.get(), sphere, sphere, -1), 1);
EXPECT_EQ(mj_maxContact(m, sphere, capsule, -1), 1); EXPECT_EQ(mj_maxContact(m.get(), sphere, capsule, -1), 1);
EXPECT_EQ(mj_maxContact(m, sphere, box, -1), 1); EXPECT_EQ(mj_maxContact(m.get(), sphere, box, -1), 1);
EXPECT_EQ(mj_maxContact(m, sphere, mesh, -1), 1); EXPECT_EQ(mj_maxContact(m.get(), sphere, mesh, -1), 1);
EXPECT_EQ(mj_maxContact(m, sphere, plane, -1), 1); EXPECT_EQ(mj_maxContact(m.get(), sphere, plane, -1), 1);
EXPECT_EQ(mj_maxContact(m, sphere, cylinder, -1), 1); EXPECT_EQ(mj_maxContact(m.get(), sphere, cylinder, -1), 1);
EXPECT_EQ(mj_maxContact(m, ellipsoid, ellipsoid, -1), 1); EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, ellipsoid, -1), 1);
EXPECT_EQ(mj_maxContact(m, ellipsoid, box, -1), 1); EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, box, -1), 1);
EXPECT_EQ(mj_maxContact(m, ellipsoid, mesh, -1), 1); EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, mesh, -1), 1);
EXPECT_EQ(mj_maxContact(m, ellipsoid, plane, -1), 1); EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, plane, -1), 1);
EXPECT_EQ(mj_maxContact(m, ellipsoid, cylinder, -1), 1); EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, cylinder, -1), 1);
EXPECT_EQ(mj_maxContact(m, ellipsoid, capsule, -1), 1); EXPECT_EQ(mj_maxContact(m.get(), ellipsoid, capsule, -1), 1);
EXPECT_EQ(mj_maxContact(m, capsule, cylinder, -1), 5); EXPECT_EQ(mj_maxContact(m.get(), capsule, cylinder, -1), 5);
EXPECT_EQ(mj_maxContact(m, capsule, mesh, -1), 5); EXPECT_EQ(mj_maxContact(m.get(), capsule, mesh, -1), 5);
EXPECT_EQ(mj_maxContact(m, cylinder, cylinder, -1), 5); EXPECT_EQ(mj_maxContact(m.get(), cylinder, cylinder, -1), 5);
EXPECT_EQ(mj_maxContact(m, cylinder, box, -1), 5); EXPECT_EQ(mj_maxContact(m.get(), cylinder, box, -1), 5);
EXPECT_EQ(mj_maxContact(m, cylinder, mesh, -1), 5); EXPECT_EQ(mj_maxContact(m.get(), cylinder, mesh, -1), 5);
mj_deleteData(d);
mj_deleteModel(m);
} }
} // namespace } // namespace
+97 -111
View File
@@ -42,9 +42,6 @@ using ::testing::ElementsAre;
using ::testing::Pointwise; using ::testing::Pointwise;
using ::testing::DoubleNear; 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 mjtNum kTolerance = 1e-6;
constexpr int kMaxIterations = 1000; constexpr int kMaxIterations = 1000;
constexpr char kEllipsoidXml[] = R"( constexpr char kEllipsoidXml[] = R"(
@@ -66,18 +63,7 @@ constexpr char kEllipsoidXml[] = R"(
</keyframe> </keyframe>
</mujoco>)"; </mujoco>)";
TestModel LoadModel(std::string_view xml) { mjtNum GeomDist(const MjModelPtr& m, const MjDataPtr& d, int g1, int g2,
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 x1[3], mjtNum x2[3], mjtNum cutoff = mjMAX_LIMIT) { mjtNum x1[3], mjtNum x2[3], mjtNum cutoff = mjMAX_LIMIT) {
mjCCDConfig config; mjCCDConfig config;
mjCCDStatus status; 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, int Penetration(mjCCDStatus& status, mjtNum& depth, std::vector<mjtNum>& dir,
std::vector<mjtNum>& pos, const TestModel& model, std::vector<mjtNum>& pos, const MjModelPtr& model,
const TestData& data, int g1, int g2, mjtNum margin = 0, const MjDataPtr& data, int g1, int g2, mjtNum margin = 0,
int max_contacts = 1) { int max_contacts = 1) {
mjCCDObj obj1, obj2; mjCCDObj obj1, obj2;
mjc_initCCDObj(&obj1, model.get(), data.get(), g1, margin); mjc_initCCDObj(&obj1, model.get(), data.get(), g1, margin);
@@ -180,8 +166,8 @@ TEST_F(MjGjkTest, SphereSphereDist) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -203,8 +189,8 @@ TEST_F(MjGjkTest, SphereSphereDistCutoff) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -223,8 +209,8 @@ TEST_F(MjGjkTest, SphereSphereNoDist) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -247,8 +233,8 @@ TEST_F(MjGjkTest, SphereSphereIntersect) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -284,8 +270,8 @@ TEST_F(MjGjkTest, BoxBoxDepth) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -313,8 +299,8 @@ TEST_F(MjGjkTest, BoxBoxDepth2) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat + 9; mjtNum* xmat = data->geom_xmat + 9;
@@ -358,8 +344,8 @@ TEST_F(MjGjkTest, BoxBoxDepth3) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat; mjtNum* xmat = data->geom_xmat;
@@ -421,8 +407,8 @@ TEST_F(MjGjkTest, BoxBoxSize05) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat; mjtNum* xmat = data->geom_xmat;
@@ -479,8 +465,8 @@ TEST_F(MjGjkTest, BoxBoxSize05b) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat; mjtNum* xmat = data->geom_xmat;
@@ -540,8 +526,8 @@ TEST_F(MjGjkTest, BoxBoxSize05c) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat; mjtNum* xmat = data->geom_xmat;
@@ -601,8 +587,8 @@ TEST_F(MjGjkTest, BoxBoxTouching) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -627,8 +613,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -661,8 +647,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD2) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -695,8 +681,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD3) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat + 9; mjtNum* xmat = data->geom_xmat + 9;
@@ -737,8 +723,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD4) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat; mjtNum* xmat = data->geom_xmat;
@@ -800,8 +786,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD5) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat; mjtNum* xmat = data->geom_xmat;
@@ -864,8 +850,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD6) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat + 9; mjtNum* xmat = data->geom_xmat + 9;
@@ -910,8 +896,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD7) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat; mjtNum* xmat = data->geom_xmat;
@@ -969,8 +955,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD8) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat; mjtNum* xmat = data->geom_xmat;
@@ -1027,8 +1013,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD9) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat; mjtNum* xmat = data->geom_xmat;
@@ -1086,8 +1072,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD10) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
mjtNum* xpos = data->geom_xpos; mjtNum* xpos = data->geom_xpos;
@@ -1123,8 +1109,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD11) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
mjtNum* xpos = data->geom_xpos; mjtNum* xpos = data->geom_xpos;
@@ -1184,8 +1170,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD12) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
mjtNum* xpos = data->geom_xpos; mjtNum* xpos = data->geom_xpos;
@@ -1243,8 +1229,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD13) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
mjtNum* xpos = data->geom_xpos; mjtNum* xpos = data->geom_xpos;
@@ -1305,8 +1291,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD14) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
mjtNum* xpos = data->geom_xpos; mjtNum* xpos = data->geom_xpos;
@@ -1362,8 +1348,8 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD15) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat; mjtNum* xmat = data->geom_xmat;
@@ -1441,8 +1427,8 @@ TEST_F(MjGjkTest, SmallBoxMesh) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -1485,8 +1471,8 @@ TEST_F(MjGjkTest, BoxMesh) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -1515,8 +1501,8 @@ TEST_F(MjGjkTest, BoxMesh2) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -1545,8 +1531,8 @@ TEST_F(MjGjkTest, BoxMeshPrune) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -1577,8 +1563,8 @@ TEST_F(MjGjkTest, MeshMesh) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -1609,8 +1595,8 @@ TEST_F(MjGjkTest, MeshMeshPrune) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -1633,8 +1619,8 @@ TEST_F(MjGjkTest, BoxEdge) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "box1"); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "box1");
@@ -1657,8 +1643,8 @@ TEST_F(MjGjkTest, BoxEdge2) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat; mjtNum* xmat = data->geom_xmat;
@@ -1715,8 +1701,8 @@ TEST_F(MjGjkTest, BoxEdgeEdge) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat; mjtNum* xmat = data->geom_xmat;
@@ -1779,8 +1765,8 @@ TEST_F(MjGjkTest, MeshEdge) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "box1"); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "box1");
@@ -1819,8 +1805,8 @@ TEST_F(MjGjkTest, MeshEdge2) {
</mujoco> </mujoco>
)"; )";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "floor"); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "floor");
@@ -1835,8 +1821,8 @@ TEST_F(MjGjkTest, MeshEdge2) {
} }
TEST_F(MjGjkTest, EllipsoidEllipsoidPenetrating) { TEST_F(MjGjkTest, EllipsoidEllipsoidPenetrating) {
TestModel model = LoadModel(kEllipsoidXml); MjModelPtr model = LoadModelFromString(kEllipsoidXml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_resetDataKeyframe(model.get(), data.get(), 0); mj_resetDataKeyframe(model.get(), data.get(), 0);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
@@ -1861,8 +1847,8 @@ TEST_F(MjGjkTest, EllipsoidEllipsoid) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -1884,8 +1870,8 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidSlowConvergence) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
mjtNum* xmat = data->geom_xmat; mjtNum* xmat = data->geom_xmat;
@@ -1943,8 +1929,8 @@ TEST_F(MjGjkTest, BoxBox) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -1963,8 +1949,8 @@ TEST_F(MjGjkTest, BoxBoxLarge) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -2013,8 +1999,8 @@ static constexpr char xml[] = R"(
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -2046,8 +2032,8 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -2071,8 +2057,8 @@ TEST_F(MjGjkTest, CapsuleCapsule) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); int geom1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
@@ -2101,8 +2087,8 @@ TEST_F(MjGjkTest, CylinderBoxMargin) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
// margin=0.1 means forces generated when dist<0.1 // margin=0.1 means forces generated when dist<0.1
@@ -2121,8 +2107,8 @@ TEST_F(MjGjkTest, BoxEdgeFlipped) {
</worldbody> </worldbody>
</mujoco>)"; </mujoco>)";
TestModel model = LoadModel(xml); MjModelPtr model = LoadModelFromString(xml);
TestData data = MakeData(model.get()); MjDataPtr data = MakeData(model);
mj_forward(model.get(), data.get()); mj_forward(model.get(), data.get());
int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
+19 -22
View File
@@ -14,14 +14,16 @@
// Tests for sdf collisions. // Tests for sdf collisions.
#include "src/engine/engine_collision_sdf.h"
#include <cstdio> #include <cstdio>
#include <gmock/gmock.h> #include <gmock/gmock.h>
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <mujoco/mjdata.h> #include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h> #include <mujoco/mjmodel.h>
#include <mujoco/mjtype.h> #include <mujoco/mjtype.h>
#include <mujoco/mujoco.h> #include <mujoco/mujoco.h>
#include "src/engine/engine_collision_sdf.h"
#include "test/fixture.h" #include "test/fixture.h"
namespace mujoco { namespace mujoco {
@@ -48,19 +50,21 @@ static constexpr char kSdfModel[] = R"(
TEST_F(SdfTest, SdfPrimitive) { TEST_F(SdfTest, SdfPrimitive) {
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(kSdfModel, error, sizeof(error)); MjModelPtr model = LoadModelFromString(kSdfModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull()); ASSERT_THAT(data, NotNull());
ASSERT_THAT(model->ngeom, kgeoms); ASSERT_THAT(model->ngeom, kgeoms);
mjSDF sdf; mjSDF sdf;
const mjpPlugin* null_plugin = NULL; const mjpPlugin* null_plugin = NULL;
mjtNum gradient[3], dist[kgeoms][kpoints] = { mjtNum gradient[3],
dist[kgeoms][kpoints] = {
{0, 0, 0, 0, 1, 1}, // plane {0, 0, 0, 0, 1, 1}, // plane
{-1, 0, 0, mju_sqrt(2)-1, mju_sqrt(2)-1, mju_sqrt(3)-1}, // sphere {-1, 0, 0, mju_sqrt(2) - 1, mju_sqrt(2) - 1,
{(mjtNum)-.1, (mjtNum).9, (mjtNum).9, mju_sqrt(3) - 1}, // sphere
mju_sqrt(2)-(mjtNum).1, (mjtNum).9, mju_sqrt(2)-(mjtNum).1}, // capsule {(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 {-1, 0, 0, mju_sqrt(2) - 1, 0, mju_sqrt(2) - 1}, // cylinder
{-mju_sqrt(3), 0, 0, 0, 0, 0}, // box {-mju_sqrt(3), 0, 0, 0, 0, 0}, // box
}; };
@@ -73,14 +77,11 @@ TEST_F(SdfTest, SdfPrimitive) {
sdf.type = mjSDFTYPE_SINGLE; 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++) { for (int j = 0; j < kpoints; j++) {
EXPECT_NEAR(mjc_distance(model, data, &sdf, points[j]), dist[i][j], EXPECT_NEAR(mjc_distance(model.get(), data.get(), &sdf, points[j]),
MjTol(1e-9, 5e-7)); dist[i][j], MjTol(1e-9, 5e-7));
mjc_gradient(model, data, &sdf, gradient, points[j]); mjc_gradient(model.get(), data.get(), &sdf, gradient, points[j]);
} }
} }
mj_deleteData(data);
mj_deleteModel(model);
} }
static constexpr char kFlexSdfModel[] = R"( static constexpr char kFlexSdfModel[] = R"(
@@ -118,9 +119,9 @@ static constexpr char kFlexSdfModel[] = R"(
TEST_F(SdfTest, FlexSdfCollision) { TEST_F(SdfTest, FlexSdfCollision) {
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(kFlexSdfModel, error, sizeof(error)); MjModelPtr model = LoadModelFromString(kFlexSdfModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull()); ASSERT_THAT(data, NotNull());
// check there is at least one flex // 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 // simulate for a few steps to let flex fall onto torus
for (int i = 0; i < 100; i++) { for (int i = 0; i < 100; i++) {
mj_step(model, data); mj_step(model.get(), data.get());
} }
// should have contacts between flex and SDF // 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"; EXPECT_TRUE(has_flex_contact) << "Expected at least one flex contact";
mj_deleteData(data);
mj_deleteModel(model);
} }
} // namespace } // namespace
} // namespace mujoco } // namespace mujoco
+98 -143
View File
@@ -14,6 +14,8 @@
// Tests for engine/engine_core_constraint.c. // Tests for engine/engine_core_constraint.c.
#include "src/engine/engine_core_constraint.h"
#include <array> #include <array>
#include <cstring> #include <cstring>
#include <string> #include <string>
@@ -23,7 +25,6 @@
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <mujoco/mjmodel.h> #include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h> #include <mujoco/mujoco.h>
#include "src/engine/engine_core_constraint.h"
#include "src/engine/engine_core_util.h" #include "src/engine/engine_core_util.h"
#include "src/engine/engine_support.h" #include "src/engine/engine_support.h"
#include "src/engine/engine_util_misc.h" #include "src/engine/engine_util_misc.h"
@@ -51,11 +52,11 @@ TEST_F(CoreConstraintTest, RestPenetration) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, testing::NotNull()) << error; ASSERT_THAT(model.get(), testing::NotNull()) << error;
mjtNum gravity = -model->opt.gravity[2]; mjtNum gravity = -model->opt.gravity[2];
mjtNum damping_ratio = 0.8; 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 reference : {-100.0, -10.0, 0.1, 0.01}) {
for (const mjtNum impedance : {0.3, 0.9, 0.99}) { for (const mjtNum impedance : {0.3, 0.9, 0.99}) {
@@ -68,14 +69,15 @@ TEST_F(CoreConstraintTest, RestPenetration) {
// set solref // set solref
for (int i = 0; i < model->ngeom; i++) { for (int i = 0; i < model->ngeom; i++) {
model->geom_solref[i * mjNREF + 0] = reference; model->geom_solref[i * mjNREF + 0] = reference;
model->geom_solref[i*mjNREF + 1] = reference < 0 ? -10 : damping_ratio; model->geom_solref[i * mjNREF + 1] =
reference < 0 ? -10 : damping_ratio;
} }
// simulate for 50 seconds // simulate for 50 seconds
mj_resetData(model, data); mj_resetData(model.get(), data.get());
while (data->time < 50) { while (data->time < 50) {
mjtNum time = data->time; mjtNum time = data->time;
mj_step(model, data); mj_step(model.get(), data.get());
ASSERT_GT(data->time, time) << "Divergence detected"; 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)); EXPECT_THAT(depth, MjNear(expected_depth, 1e-10, 1e-3));
} }
} }
mj_deleteData(data);
mj_deleteModel(model);
} }
static const char* const kDoflessContactPath = static const char* const kDoflessContactPath =
@@ -242,17 +241,10 @@ TEST_F(CoreConstraintTest, ConstraintUpdateImpl) {
int adr = d2->island_iefcadr[island]; int adr = d2->island_iefcadr[island];
int* state = d2->iefc_state + adr; int* state = d2->iefc_state + adr;
mjtNum* force = d2->iefc_force + adr; mjtNum* force = d2->iefc_force + adr;
mj_constraintUpdate_impl(ne, nf, efcnum, mj_constraintUpdate_impl(ne, nf, efcnum, d2->iefc_D + adr,
d2->iefc_D + adr, d2->iefc_R + adr, d2->iefc_frictionloss + adr,
d2->iefc_R + adr, jari, d2->iefc_type + adr, d2->iefc_id + adr,
d2->iefc_frictionloss + adr, d2->contact, state, force, &cost2i,
jari,
d2->iefc_type + adr,
d2->iefc_id + adr,
d2->contact,
state,
force,
&cost2i,
/*flg_coneHessian=*/1); /*flg_coneHessian=*/1);
// compare nefc vectors // compare nefc vectors
@@ -308,9 +300,9 @@ TEST_F(CoreConstraintTest, FlexvertEquality) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, testing::NotNull()) << error; ASSERT_THAT(model.get(), testing::NotNull()) << error;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
ASSERT_EQ(model->neq, 1); ASSERT_EQ(model->neq, 1);
ASSERT_EQ(model->eq_type[0], mjEQ_FLEXVERT); ASSERT_EQ(model->eq_type[0], mjEQ_FLEXVERT);
ASSERT_EQ(model->nflex, 1); ASSERT_EQ(model->nflex, 1);
@@ -318,7 +310,7 @@ TEST_F(CoreConstraintTest, FlexvertEquality) {
ASSERT_EQ(model->flex_edgenum[0], 16); ASSERT_EQ(model->flex_edgenum[0], 16);
// step1 to populate flexvert_length // step1 to populate flexvert_length
mj_step1(model, data); mj_step1(model.get(), data.get());
EXPECT_EQ(data->ne, 2 * model->flex_vertnum[0]); EXPECT_EQ(data->ne, 2 * model->flex_vertnum[0]);
EXPECT_EQ(data->nefc, 18); EXPECT_EQ(data->nefc, 18);
for (int i = 0; i < 18; ++i) { for (int i = 0; i < 18; ++i) {
@@ -336,7 +328,7 @@ TEST_F(CoreConstraintTest, FlexvertEquality) {
for (int j = 0; j < model->flex_vertnum[0]; ++j) { 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) { for (int j = 0; j < data->nefc; ++j) {
EXPECT_NEAR(Jqvel[j], 0, MjTol(1e-9, 1e-5)); 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 + 1] = linvel[1];
qvel[3 * j + 2] = linvel[2]; 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) { for (int j = 0; j < data->nefc; ++j) {
EXPECT_NEAR(Jqvel[j], 0, MjTol(1e-9, 1e-5)); 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 // Test flex strain constraint with pinned nodes attached to freejoint parent
@@ -392,12 +381,12 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m.get(), NotNull()) << error.data();
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_resetData(m, d); mj_resetData(m.get(), d.get());
mj_forward(m, d); mj_forward(m.get(), d.get());
// Check that we have constraints // Check that we have constraints
EXPECT_GT(d->nefc, 0) << "No constraints generated"; EXPECT_GT(d->nefc, 0) << "No constraints generated";
@@ -449,8 +438,8 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) {
mju_copy(d->qpos, qpos0.data(), m->nq); mju_copy(d->qpos, qpos0.data(), m->nq);
mjtNum dqpos[100] = {0}; mjtNum dqpos[100] = {0};
dqpos[j] = eps; dqpos[j] = eps;
mj_integratePos(m, d->qpos, dqpos, 1); mj_integratePos(m.get(), d->qpos, dqpos, 1);
mj_forward(m, d); mj_forward(m.get(), d.get());
for (int i = 0; i < num_constraints_to_check; i++) { for (int i = 0; i < num_constraints_to_check; i++) {
mjtNum fd = (d->efc_pos[i] - efc_pos0[i]) / eps; mjtNum fd = (d->efc_pos[i] - efc_pos0[i]) / eps;
@@ -472,7 +461,7 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) {
d->qpos[4] = 0; d->qpos[4] = 0;
d->qpos[5] = 0; d->qpos[5] = 0;
d->qpos[6] = mju_sin(angle / 2); // z d->qpos[6] = mju_sin(angle / 2); // z
mj_forward(m, d); mj_forward(m.get(), d.get());
mjtNum max_strain_rotated = 0; mjtNum max_strain_rotated = 0;
for (int i = 0; i < d->ne; i++) { for (int i = 0; i < d->ne; i++) {
@@ -494,8 +483,8 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) {
mju_copy(d->qpos, qpos_rot.data(), m->nq); mju_copy(d->qpos, qpos_rot.data(), m->nq);
mjtNum dqpos[100] = {0}; mjtNum dqpos[100] = {0};
dqpos[j] = eps; dqpos[j] = eps;
mj_integratePos(m, d->qpos, dqpos, 1); mj_integratePos(m.get(), d->qpos, dqpos, 1);
mj_forward(m, d); mj_forward(m.get(), d.get());
mjtNum fd = (d->efc_pos[0] - efc_pos_rot[0]) / eps; 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];
@@ -510,12 +499,12 @@ TEST_F(CoreConstraintTest, BoxShellPinnedParentWithFreejoint) {
// Reset for simulation // Reset for simulation
mju_copy(d->qpos, qpos0.data(), m->nq); mju_copy(d->qpos, qpos0.data(), m->nq);
mj_forward(m, d); mj_forward(m.get(), d.get());
// Run simulation only if checks pass // Run simulation only if checks pass
if (!has_bad_constraint && !has_bad_jacobian) { if (!has_bad_constraint && !has_bad_jacobian) {
for (int i = 0; i < 2000; i++) { for (int i = 0; i < 2000; i++) {
mj_step(m, d); mj_step(m.get(), d.get());
ASSERT_FALSE(mju_isBad(d->qpos[0])) ASSERT_FALSE(mju_isBad(d->qpos[0]))
<< "Simulation became unstable at step " << i; << "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) // Test flex strain constraint WITHOUT pinned nodes (simpler case)
@@ -554,12 +540,12 @@ TEST_F(CoreConstraintTest, StrainConstraintNoPinning) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m.get(), NotNull()) << error.data();
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_resetData(m, d); mj_resetData(m.get(), d.get());
mj_forward(m, d); mj_forward(m.get(), d.get());
// Check constraints // Check constraints
EXPECT_GT(d->ne, 0) << "Expected strain constraints"; EXPECT_GT(d->ne, 0) << "Expected strain constraints";
@@ -597,7 +583,7 @@ TEST_F(CoreConstraintTest, StrainConstraintNoPinning) {
d->qpos[4] = 0; // x d->qpos[4] = 0; // x
d->qpos[5] = 0; // y d->qpos[5] = 0; // y
d->qpos[6] = mju_sin(angle / 2); // z d->qpos[6] = mju_sin(angle / 2); // z
mj_forward(m, d); mj_forward(m.get(), d.get());
mjtNum max_strain_rotated = 0; mjtNum max_strain_rotated = 0;
for (int i = 0; i < d->ne; i++) { 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 // Run simulation for a few steps to check stability
mju_copy(d->qpos, qpos0.data(), m->nq); 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++) { 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; 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 // Test flex strain constraint with quadratic interpolation
@@ -641,12 +624,12 @@ TEST_F(CoreConstraintTest, StrainConstraintQuadratic) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m.get(), NotNull()) << error.data();
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_resetData(m, d); mj_resetData(m.get(), d.get());
mj_forward(m, d); mj_forward(m.get(), d.get());
// Check constraints generated // Check constraints generated
EXPECT_GT(d->ne, 0) << "Expected strain constraints"; EXPECT_GT(d->ne, 0) << "Expected strain constraints";
@@ -674,13 +657,9 @@ TEST_F(CoreConstraintTest, StrainConstraintQuadratic) {
// Run simulation for a few steps // Run simulation for a few steps
for (int i = 0; i < 100; i++) { for (int i = 0; i < 100; i++) {
mj_step(m, d); mj_step(m.get(), d.get());
ASSERT_FALSE(mju_isBad(d->qpos[0])) ASSERT_FALSE(mju_isBad(d->qpos[0])) << "Simulation unstable at step " << i;
<< "Simulation unstable at step " << i;
} }
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(CoreConstraintTest, ShellModeBendZeroForceAtRest) { TEST_F(CoreConstraintTest, ShellModeBendZeroForceAtRest) {
@@ -701,11 +680,11 @@ TEST_F(CoreConstraintTest, ShellModeBendZeroForceAtRest) {
)"; )";
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error; ASSERT_THAT(m.get(), testing::NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_forward(m, d); mj_forward(m.get(), d.get());
// Check number of equalities // Check number of equalities
EXPECT_EQ(m->neq, 6); EXPECT_EQ(m->neq, 6);
@@ -719,9 +698,6 @@ TEST_F(CoreConstraintTest, ShellModeBendZeroForceAtRest) {
EXPECT_NEAR(d->efc_pos[i], 0, 1e-10) EXPECT_NEAR(d->efc_pos[i], 0, 1e-10)
<< "nonzero constraint residual at " << i; << "nonzero constraint residual at " << i;
} }
mj_deleteData(d);
mj_deleteModel(m);
} }
// Test quadratic passive forces (no constraints) for stability // Test quadratic passive forces (no constraints) for stability
@@ -741,13 +717,13 @@ TEST_F(CoreConstraintTest, QuadraticPassiveForceStability) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m.get(), NotNull()) << error.data();
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
// Run for 500 steps — should stay stable // Run for 500 steps — should stay stable
for (int i = 0; i < 500; i++) { for (int i = 0; i < 500; i++) {
mj_step(m, d); mj_step(m.get(), d.get());
ASSERT_FALSE(mju_isBad(d->qpos[0])) ASSERT_FALSE(mju_isBad(d->qpos[0]))
<< "Passive quadratic unstable at step " << i; << "Passive quadratic unstable at step " << i;
for (int j = 0; j < m->nv; j++) { for (int j = 0; j < m->nv; j++) {
@@ -755,9 +731,6 @@ TEST_F(CoreConstraintTest, QuadraticPassiveForceStability) {
<< "Velocity exploded at step " << i; << "Velocity exploded at step " << i;
} }
} }
mj_deleteData(d);
mj_deleteModel(m);
} }
// Test quadratic with anisotropic cells (like what mesh bounding box creates) // Test quadratic with anisotropic cells (like what mesh bounding box creates)
@@ -782,27 +755,24 @@ TEST_F(CoreConstraintTest, QuadraticAnisotropicStrain) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m.get(), NotNull()) << error.data();
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_forward(m, d); mj_forward(m.get(), d.get());
EXPECT_GT(d->ne, 0) << "Expected strain constraints"; EXPECT_GT(d->ne, 0) << "Expected strain constraints";
// Run for 200 steps with gravity + contact // Run for 200 steps with gravity + contact
for (int i = 0; i < 200; i++) { for (int i = 0; i < 200; i++) {
mj_step(m, d); mj_step(m.get(), d.get());
ASSERT_FALSE(mju_isBad(d->qpos[0])) ASSERT_FALSE(mju_isBad(d->qpos[0]))
<< "Anisotropic quadratic unstable at step " << i; << "Anisotropic quadratic unstable at step " << i;
for (int j = 0; j < m->nv; j++) { for (int j = 0; j < m->nv; j++) {
ASSERT_LT(mju_abs(d->qvel[j]), 1000.0) ASSERT_LT(mju_abs(d->qvel[j]), 1000.0)
<< "Velocity exploded at step " << i << "Velocity exploded at step " << i << ", qvel[" << j
<< ", qvel[" << j << "]=" << d->qvel[j]; << "]=" << d->qvel[j];
} }
} }
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(CoreConstraintTest, ContactSharedDofJacobian) { TEST_F(CoreConstraintTest, ContactSharedDofJacobian) {
@@ -826,21 +796,18 @@ TEST_F(CoreConstraintTest, ContactSharedDofJacobian) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
ASSERT_EQ(model->nv, 3); ASSERT_EQ(model->nv, 3);
ASSERT_TRUE(mj_isSparse(model)); ASSERT_TRUE(mj_isSparse(model.get()));
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
mj_forward(model, data); mj_forward(model.get(), data.get());
ASSERT_EQ(data->ncon, 1); ASSERT_EQ(data->ncon, 1);
ASSERT_GE(data->nefc, 1); ASSERT_GE(data->nefc, 1);
EXPECT_EQ(data->efc_J_rownnz[0], 2); EXPECT_EQ(data->efc_J_rownnz[0], 2);
mj_deleteData(data);
mj_deleteModel(model);
} }
static const char* const kJdotvConnect2dPath = static const char* const kJdotvConnect2dPath =
@@ -852,10 +819,8 @@ static const char* const kJdotvWeld3dPath =
// validate mj_Jdotv against finite-differenced constraint Jacobian // validate mj_Jdotv against finite-differenced constraint Jacobian
TEST_F(CoreConstraintTest, JdotvFiniteDifference) { TEST_F(CoreConstraintTest, JdotvFiniteDifference) {
for (const char* path :
for (const char* path : {kJdotvConnect2dPath, {kJdotvConnect2dPath, kJdotvConnect3dPath, kJdotvWeld3dPath}) {
kJdotvConnect3dPath,
kJdotvWeld3dPath}) {
const std::string xml_path = GetTestDataFilePath(path); const std::string xml_path = GetTestDataFilePath(path);
char err[1024]; char err[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, err, sizeof(err)); 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) // get dense J_0 (ne x nv)
std::vector<mjtNum> J0(ne * nv); std::vector<mjtNum> J0(ne * nv);
if (mj_isSparse(m)) { if (mj_isSparse(m)) {
mju_sparse2dense(J0.data(), d->efc_J, ne, nv, mju_sparse2dense(J0.data(), d->efc_J, ne, nv, d->efc_J_rownnz,
d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind); d->efc_J_rowadr, d->efc_J_colind);
} else { } else {
mju_copy(J0.data(), d->efc_J, ne * nv); 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"; ASSERT_EQ(d->ne, ne) << "constraint count changed after integration";
std::vector<mjtNum> Jh(ne * nv); std::vector<mjtNum> Jh(ne * nv);
if (mj_isSparse(m)) { if (mj_isSparse(m)) {
mju_sparse2dense(Jh.data(), d->efc_J, ne, nv, mju_sparse2dense(Jh.data(), d->efc_J, ne, nv, d->efc_J_rownnz,
d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind); d->efc_J_rowadr, d->efc_J_colind);
} else { } else {
mju_copy(Jh.data(), d->efc_J, ne * nv); mju_copy(Jh.data(), d->efc_J, ne * nv);
} }
@@ -927,9 +892,8 @@ TEST_F(CoreConstraintTest, JdotvFiniteDifference) {
// Test 2: forward-inverse identity preserved with Jdot*v correction // Test 2: forward-inverse identity preserved with Jdot*v correction
TEST_F(CoreConstraintTest, JdotvFwdInvIdentity) { TEST_F(CoreConstraintTest, JdotvFwdInvIdentity) {
for (const char* path : {kJdotvConnect2dPath, for (const char* path :
kJdotvConnect3dPath, {kJdotvConnect2dPath, kJdotvConnect3dPath, kJdotvWeld3dPath}) {
kJdotvWeld3dPath}) {
const std::string xml_path = GetTestDataFilePath(path); const std::string xml_path = GetTestDataFilePath(path);
char err[1024]; char err[1024];
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, err, sizeof(err)); 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); mjtNum epsilon = MjTol(1e-10, 1e-2);
EXPECT_LT(fwdinv, epsilon) EXPECT_LT(fwdinv, epsilon)
<< "fwdinv broken for " << path << "fwdinv broken for " << path << " (fwdinv=" << fwdinv << ")";
<< " (fwdinv=" << fwdinv << ")";
mj_deleteData(d); mj_deleteData(d);
mj_deleteModel(m); mj_deleteModel(m);
@@ -964,10 +927,9 @@ struct StrainConstraintTestCase {
std::string flex_xyaxes; std::string flex_xyaxes;
}; };
class StrainConstraintRotatedTest : public CoreConstraintTest, class StrainConstraintRotatedTest
public ::testing::WithParamInterface< : public CoreConstraintTest,
StrainConstraintTestCase> { public ::testing::WithParamInterface<StrainConstraintTestCase> {};
};
TEST_P(StrainConstraintRotatedTest, ResidualIsZero) { TEST_P(StrainConstraintRotatedTest, ResidualIsZero) {
auto param = GetParam(); auto param = GetParam();
@@ -1006,11 +968,11 @@ TEST_P(StrainConstraintRotatedTest, ResidualIsZero) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml.c_str(), error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml.c_str(), error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m.get(), NotNull()) << error.data();
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_forward(m, d); mj_forward(m.get(), d.get());
// Check we have strain constraints // Check we have strain constraints
EXPECT_GT(d->ne, 0) << "Expected 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++) { for (int i = 0; i < d->ne; i++) {
max_pos = mju_max(max_pos, mju_abs(d->efc_pos[i])); max_pos = mju_max(max_pos, mju_abs(d->efc_pos[i]));
} }
EXPECT_LT(max_pos, 1e-6) EXPECT_LT(max_pos, 1e-6) << "Strain constraint residual should be ~0"
<< "Strain constraint residual should be ~0"
<< " (max_pos=" << max_pos << ")"; << " (max_pos=" << max_pos << ")";
// Verify stability // Verify stability
for (int i = 0; i < 200; i++) { for (int i = 0; i < 200; i++) {
mj_step(m, d); mj_step(m.get(), d.get());
ASSERT_FALSE(mju_isBad(d->qpos[0])) ASSERT_FALSE(mju_isBad(d->qpos[0])) << "Simulation unstable at step " << i;
<< "Simulation unstable at step " << i;
for (int j = 0; j < m->nv; j++) { for (int j = 0; j < m->nv; j++) {
ASSERT_LT(mju_abs(d->qvel[j]), 1000.0) ASSERT_LT(mju_abs(d->qvel[j]), 1000.0)
<< "Velocity exploded at step " << i << "Velocity exploded at step " << i << ", qvel[" << j
<< ", qvel[" << j << "]=" << d->qvel[j]; << "]=" << d->qvel[j];
} }
} }
mj_deleteData(d);
mj_deleteModel(m);
} }
INSTANTIATE_TEST_SUITE_P( INSTANTIATE_TEST_SUITE_P(
@@ -1106,11 +1063,11 @@ TEST_F(CoreConstraintTest, ShellModeContactJacobian) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, testing::NotNull()) << error; ASSERT_THAT(model.get(), testing::NotNull()) << error;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
mj_forward(model, data); mj_forward(model.get(), data.get());
// find central vertex index (13 for 3x3x3 grid) // find central vertex index (13 for 3x3x3 grid)
int central_idx = 13; int central_idx = 13;
@@ -1121,7 +1078,8 @@ TEST_F(CoreConstraintTest, ShellModeContactJacobian) {
int rest = central_idx % (nx * ny); int rest = central_idx % (nx * ny);
int j = rest / nx; int j = rest / nx;
int i = 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 // create manual contact with central vertex
mjContact con; mjContact con;
@@ -1136,20 +1094,20 @@ TEST_F(CoreConstraintTest, ShellModeContactJacobian) {
con.vert[1] = central_idx; con.vert[1] = central_idx;
con.dim = 1; con.dim = 1;
mju_copy3(con.pos, data->flexvert_xpos + 3 * central_idx); mju_copy3(con.pos, data->flexvert_xpos + 3 * central_idx);
con.frame[0] = 0; con.frame[1] = 0; con.frame[2] = 1; // normal con.frame[0] = 0;
con.frame[1] = 0;
con.frame[2] = 1; // normal
// buffer for Jacobian // buffer for Jacobian
std::vector<mjtNum> jacdif(3 * model->nv, 0.0); std::vector<mjtNum> jacdif(3 * model->nv, 0.0);
// call mj_contactJacobian // call mj_contactJacobian
mj_contactJacobian(model, data, &con, 1, nullptr, jacdif.data(), mj_contactJacobian(model.get(), data.get(), &con, 1, nullptr, jacdif.data(),
nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr,
nullptr, nullptr, nullptr,
nullptr); nullptr);
// check that boundary nodes have non-zero entries, and central node has zero // check that boundary nodes have non-zero entries, and central node has zero
bool boundary_has_dof = false; bool boundary_has_dof = false;
bool interior_has_dof = false; bool interior_has_dof = false;
@@ -1185,9 +1143,6 @@ TEST_F(CoreConstraintTest, ShellModeContactJacobian) {
<< "Boundary nodes should receive contact force"; << "Boundary nodes should receive contact force";
EXPECT_FALSE(interior_has_dof) EXPECT_FALSE(interior_has_dof)
<< "Interior nodes should not receive contact force"; << "Interior nodes should not receive contact force";
mj_deleteData(data);
mj_deleteModel(model);
} }
} // namespace } // namespace
+61 -93
View File
@@ -15,8 +15,6 @@
// Tests for engine/engine_core_smooth.c. // Tests for engine/engine_core_smooth.c.
#include "src/engine/engine_core_smooth.h" #include "src/engine/engine_core_smooth.h"
#include "src/engine/engine_util_misc.h"
#include "src/engine/engine_util_sparse.h"
#include <algorithm> #include <algorithm>
#include <string> #include <string>
@@ -30,6 +28,8 @@
#include <mujoco/mjspec.h> #include <mujoco/mjspec.h>
#include <mujoco/mjxmacro.h> #include <mujoco/mjxmacro.h>
#include <mujoco/mujoco.h> #include <mujoco/mujoco.h>
#include "src/engine/engine_util_misc.h"
#include "src/engine/engine_util_sparse.h"
#include "test/fixture.h" #include "test/fixture.h"
namespace mujoco { namespace mujoco {
@@ -44,7 +44,6 @@ using ::testing::NotNull;
using ::testing::Pointwise; using ::testing::Pointwise;
using CoreSmoothTest = MujocoTest; using CoreSmoothTest = MujocoTest;
constexpr bool EndsWith(std::string_view str, std::string_view suffix) { constexpr bool EndsWith(std::string_view str, std::string_view suffix) {
return str.size() >= suffix.size() && return str.size() >= suffix.size() &&
str.substr(str.size() - suffix.size()) == suffix; str.substr(str.size() - suffix.size()) == suffix;
@@ -64,12 +63,12 @@ TEST_F(CoreSmoothTest, MjDataWorldBodyValuesAreInitialized) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
mj_resetDataDebug(model, data, 'd'); mj_resetDataDebug(model.get(), data.get(), 'd');
mj_forward(model, data); mj_forward(model.get(), data.get());
mj_rnePostConstraint(model, data); mj_rnePostConstraint(model.get(), data.get());
{ {
#define X(type, name, d0, d1) \ #define X(type, name, d0, d1) \
@@ -86,9 +85,6 @@ TEST_F(CoreSmoothTest, MjDataWorldBodyValuesAreInitialized) {
MJDATA_POINTERS MJDATA_POINTERS
#undef X #undef X
} }
mj_deleteData(data);
mj_deleteModel(model);
} }
// --------------------------- mj_kinematics ----------------------------------- // --------------------------- mj_kinematics -----------------------------------
@@ -101,16 +97,13 @@ TEST_F(CoreSmoothTest, MjKinematicsWorldXipos) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
mj_resetDataDebug(model, data, 'd'); mj_resetDataDebug(model.get(), data.get(), 'd');
mj_kinematics(model, data); mj_kinematics(model.get(), data.get());
EXPECT_THAT(AsVector(&data->xipos[0], 3), ElementsAre(0, 0, 0)); EXPECT_THAT(AsVector(&data->xipos[0], 3), ElementsAre(0, 0, 0));
mj_deleteData(data);
mj_deleteModel(model);
} }
// ----------------------------- mj_tendon ------------------------------------- // ----------------------------- mj_tendon -------------------------------------
@@ -145,19 +138,16 @@ TEST_F(CoreSmoothTest, FixedTendonSortedIndices) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
ASSERT_EQ(model->ntendon, 1); ASSERT_EQ(model->ntendon, 1);
ASSERT_EQ(model->nwrap, 3); ASSERT_EQ(model->nwrap, 3);
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
mj_fwdPosition(model, data); mj_fwdPosition(model.get(), data.get());
mjtNum* J = data->ten_J; mjtNum* J = data->ten_J;
EXPECT_THAT(vector<mjtNum>(J, J + 3), ElementsAre(1, 2, 3)); 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"; static const char* const kTen_J0 = "engine/testdata/core_smooth/ten_J0.xml";
@@ -250,8 +240,8 @@ TEST_F(CoreSmoothTest, TendonArmature) {
int rowadr = m->ten_J_rowadr[j]; int rowadr = m->ten_J_rowadr[j];
int* rownnz = m->ten_J_rownnz + j; int* rownnz = m->ten_J_rownnz + j;
int zero = 0; int zero = 0;
mju_sparse2dense(ten_J.data(), d->ten_J + rowadr, 1, nv, mju_sparse2dense(ten_J.data(), d->ten_J + rowadr, 1, nv, rownnz, &zero,
rownnz, &zero, m->ten_J_colind + rowadr); m->ten_J_colind + rowadr);
// get tendon inertia only, using outer product // get tendon inertia only, using outer product
mju_mulMatMat(ten_M.data(), ten_J.data(), ten_J.data(), nv, 1, nv); mju_mulMatMat(ten_M.data(), ten_J.data(), ten_J.data(), nv, 1, nv);
@@ -393,14 +383,12 @@ TEST_F(CoreSmoothTest, TendonInertiaEquivalent) {
} }
} }
// --------------------------- connect constraint ------------------------------ // --------------------------- connect constraint ------------------------------
// test that bodies hanging on connects lead to expected force sensor readings // test that bodies hanging on connects lead to expected force sensor readings
void TestConnect(const char* const filepath) { void TestConnect(const char* const filepath) {
const std::string xml_path = GetTestDataFilePath(filepath); const std::string xml_path = GetTestDataFilePath(filepath);
mjModel* model = mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
mjData* data = mj_makeData(model); mjData* data = mj_makeData(model);
// settle physics: // settle physics:
for (int i = 0; i < 1000; i++) { for (int i = 0; i < 1000; i++) {
@@ -414,49 +402,42 @@ void TestConnect(const char* const filepath) {
mj_deleteModel(model); mj_deleteModel(model);
} }
TEST_F(CoreSmoothTest, RnePostConnectForceSlide) { TEST_F(CoreSmoothTest, RnePostConnectForceSlide) {
constexpr char kModelFilePath[] = constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/connect/force_slide.xml"; "engine/testdata/core_smooth/rne_post/connect/force_slide.xml";
TestConnect(kModelFilePath); TestConnect(kModelFilePath);
} }
TEST_F(CoreSmoothTest, RnePostConnectForceSlideRotated) { TEST_F(CoreSmoothTest, RnePostConnectForceSlideRotated) {
constexpr char kModelFilePath[] = constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/connect/force_slide_rotated.xml"; "engine/testdata/core_smooth/rne_post/connect/force_slide_rotated.xml";
TestConnect(kModelFilePath); TestConnect(kModelFilePath);
} }
TEST_F(CoreSmoothTest, RnePostConnectForceFree) { TEST_F(CoreSmoothTest, RnePostConnectForceFree) {
constexpr char kModelFilePath[] = constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/connect/force_free.xml"; "engine/testdata/core_smooth/rne_post/connect/force_free.xml";
TestConnect(kModelFilePath); TestConnect(kModelFilePath);
} }
TEST_F(CoreSmoothTest, RnePostConnectTorque) { TEST_F(CoreSmoothTest, RnePostConnectTorque) {
constexpr char kModelFilePath[] = constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/connect/torque_free.xml"; "engine/testdata/core_smooth/rne_post/connect/torque_free.xml";
TestConnect(kModelFilePath); TestConnect(kModelFilePath);
} }
TEST_F(CoreSmoothTest, RnePostConnectMultipleConstraints) { TEST_F(CoreSmoothTest, RnePostConnectMultipleConstraints) {
constexpr char kModelFilePath[] = constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/connect/multiple_constraints.xml"; "engine/testdata/core_smooth/rne_post/connect/multiple_constraints.xml";
TestConnect(kModelFilePath); TestConnect(kModelFilePath);
} }
// --------------------------- weld constraint --------------------------------- // --------------------------- weld constraint ---------------------------------
// test that bodies attached with welds lead to expected force sensor readings // test that bodies attached with welds lead to expected force sensor readings
void TestWeld(const char* const filepath) { void TestWeld(const char* const filepath) {
const std::string xml_path = GetTestDataFilePath(filepath); const std::string xml_path = GetTestDataFilePath(filepath);
mjModel* model = mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
mjData* data = mj_makeData(model); mjData* data = mj_makeData(model);
// settle physics: // settle physics:
for (int i = 0; i < 1000; i++) { for (int i = 0; i < 1000; i++) {
@@ -464,8 +445,7 @@ void TestWeld(const char* const filepath) {
} }
for (int sensor_index = 0; sensor_index < model->nsensor; sensor_index++) { for (int sensor_index = 0; sensor_index < model->nsensor; sensor_index++) {
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
EXPECT_NEAR( EXPECT_NEAR(data->sensordata[model->sensor_adr[sensor_index] + i],
data->sensordata[model->sensor_adr[sensor_index] + i],
model->sensor_user[model->nuser_sensor * sensor_index + i], model->sensor_user[model->nuser_sensor * sensor_index + i],
MjTol(1e-6, 5e-5)); MjTol(1e-6, 5e-5));
} }
@@ -474,56 +454,48 @@ void TestWeld(const char* const filepath) {
mj_deleteModel(model); mj_deleteModel(model);
} }
TEST_F(CoreSmoothTest, RnePostWeldForceFree) { TEST_F(CoreSmoothTest, RnePostWeldForceFree) {
constexpr char kModelFilePath[] = constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/weld/force_free.xml"; "engine/testdata/core_smooth/rne_post/weld/force_free.xml";
TestWeld(kModelFilePath); TestWeld(kModelFilePath);
} }
TEST_F(CoreSmoothTest, RnePostWeldForceFreeRotated) { TEST_F(CoreSmoothTest, RnePostWeldForceFreeRotated) {
constexpr char kModelFilePath[] = constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/weld/force_free_rotated.xml"; "engine/testdata/core_smooth/rne_post/weld/force_free_rotated.xml";
TestWeld(kModelFilePath); TestWeld(kModelFilePath);
} }
TEST_F(CoreSmoothTest, RnePostWeldForceTorqueFree) { TEST_F(CoreSmoothTest, RnePostWeldForceTorqueFree) {
constexpr char kModelFilePath[] = constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/weld/force_torque_free.xml"; "engine/testdata/core_smooth/rne_post/weld/force_torque_free.xml";
TestWeld(kModelFilePath); TestWeld(kModelFilePath);
} }
TEST_F(CoreSmoothTest, RnePostWeldForceTorqueFreeRotated) { TEST_F(CoreSmoothTest, RnePostWeldForceTorqueFreeRotated) {
constexpr char kModelFilePath[] = constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/weld/force_torque_free_rotated.xml"; "engine/testdata/core_smooth/rne_post/weld/force_torque_free_rotated.xml";
TestWeld(kModelFilePath); TestWeld(kModelFilePath);
} }
TEST_F(CoreSmoothTest, WeldRatioForceFree) { TEST_F(CoreSmoothTest, WeldRatioForceFree) {
constexpr char kModelFilePath[] = constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/weld/tfratio0_force_free.xml"; "engine/testdata/core_smooth/rne_post/weld/tfratio0_force_free.xml";
TestConnect(kModelFilePath); TestConnect(kModelFilePath);
} }
TEST_F(CoreSmoothTest, WeldRatioForceSlide) { TEST_F(CoreSmoothTest, WeldRatioForceSlide) {
constexpr char kModelFilePath[] = constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide.xml"; "engine/testdata/core_smooth/rne_post/weld/tfratio0_force_slide.xml";
TestConnect(kModelFilePath); TestConnect(kModelFilePath);
} }
TEST_F(CoreSmoothTest, WeldRatioTorqueFree) { TEST_F(CoreSmoothTest, WeldRatioTorqueFree) {
constexpr char kModelFilePath[] = constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/weld/tfratio0_torque_free.xml"; "engine/testdata/core_smooth/rne_post/weld/tfratio0_torque_free.xml";
TestConnect(kModelFilePath); TestConnect(kModelFilePath);
} }
TEST_F(CoreSmoothTest, WeldRatioForceSlideRotated) { TEST_F(CoreSmoothTest, WeldRatioForceSlideRotated) {
constexpr char kModelFilePath[] = constexpr char kModelFilePath[] =
"engine/testdata/core_smooth/rne_post/weld/" "engine/testdata/core_smooth/rne_post/weld/"
@@ -601,7 +573,8 @@ TEST_F(CoreSmoothTest, RefsiteBringsToPose) {
// check that position matches target to within 1e-3 length units // check that position matches target to within 1e-3 length units
mjtNum relpos[3]; 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)); EXPECT_THAT(relpos, Pointwise(MjNear(1e-3, 5e-3), targetpos));
// check that orientation matches target to within 0.06 radians // check that orientation matches target to within 0.06 radians
@@ -655,8 +628,8 @@ TEST_F(CoreSmoothTest, FactorI) {
// dense L matrix // dense L matrix
int nv = model->nv; int nv = model->nv;
vector<mjtNum> Ldense(nv * nv, 0); vector<mjtNum> Ldense(nv * nv, 0);
mju_sparse2dense(Ldense.data(), data->qLD, nv, nv, mju_sparse2dense(Ldense.data(), data->qLD, nv, nv, model->M_rownnz,
model->M_rownnz, model->M_rowadr, model->M_colind); model->M_rowadr, model->M_colind);
for (int i = 0; i < nv; i++) { for (int i = 0; i < nv; i++) {
// set diagonal to 1 // set diagonal to 1
Ldense[i * nv + i] = 1; Ldense[i * nv + i] = 1;
@@ -664,8 +637,8 @@ TEST_F(CoreSmoothTest, FactorI) {
// dense D matrix // dense D matrix
vector<mjtNum> Ddense(nv * nv); vector<mjtNum> Ddense(nv * nv);
mju_sparse2dense(Ddense.data(), data->qLD, nv, nv, mju_sparse2dense(Ddense.data(), data->qLD, nv, nv, model->M_rownnz,
model->M_rownnz, model->M_rowadr, model->M_colind); model->M_rowadr, model->M_colind);
for (int i = 0; i < nv; i++) { for (int i = 0; i < nv; i++) {
for (int j = 0; j < nv; j++) { for (int j = 0; j < nv; j++) {
// zero everything except the diagonal // zero everything except the diagonal
@@ -724,15 +697,16 @@ TEST_F(CoreSmoothTest, SolveLDs) {
// compare LD and LDs densified matrices // compare LD and LDs densified matrices
vector<mjtNum> LDdense(nv * nv); vector<mjtNum> LDdense(nv * nv);
mju_sparse2dense(LDdense.data(), d->qLD, nv, nv, mju_sparse2dense(LDdense.data(), d->qLD, nv, nv, m->M_rownnz, m->M_rowadr,
m->M_rownnz, m->M_rowadr, m->M_colind); m->M_colind);
vector<mjtNum> LDdense2(nv * nv); vector<mjtNum> LDdense2(nv * nv);
legacyToDense(m, LDdense2.data(), LDlegacy.data()); legacyToDense(m, LDdense2.data(), LDlegacy.data());
// expect lower triangles to match exactly // expect lower triangles to match exactly
for (int i = 0; i < nv; i++) { for (int i = 0; i < nv; i++) {
for (int j = 0; j < i; j++) { for (int j = 0; j < i; j++) {
EXPECT_NEAR(LDdense[i*nv+j], LDdense2[i*nv+j], MjTol(1e-14, 1e-6)); EXPECT_NEAR(LDdense[i * nv + j], LDdense2[i * nv + j],
MjTol(1e-14, 1e-6));
} }
} }
@@ -743,8 +717,8 @@ TEST_F(CoreSmoothTest, SolveLDs) {
for (int i = 0; i < nv; i += 2) vec[i] = vec2[i] = 0; 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_legacy(m, vec.data(), 1, LDlegacy.data(), d->qLDiagInv);
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, 1, mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, 1, m->M_rownnz, m->M_rowadr,
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr); m->M_colind, nullptr);
// expect vectors to match up to floating point precision // expect vectors to match up to floating point precision
for (int i = 0; i < nv; i++) { for (int i = 0; i < nv; i++) {
@@ -778,8 +752,8 @@ TEST_F(CoreSmoothTest, SolveLDmultipleVectors) {
for (int i = 0; i < nv * n; i += 3) vec[i] = vec2[i] = 0; 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_legacy(m, vec.data(), n, LDlegacy.data(), d->qLDiagInv);
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n, mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n, m->M_rownnz, m->M_rowadr,
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr); m->M_colind, nullptr);
// expect vectors to match up to floating point precision // 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++) {
@@ -816,8 +790,8 @@ TEST_F(CoreSmoothTest, SolveM2) {
vector<mjtNum> res(nv * n); vector<mjtNum> res(nv * n);
mj_solveM2(m, d, res.data(), vec.data(), sqrtInvD.data(), n); mj_solveM2(m, d, res.data(), vec.data(), sqrtInvD.data(), n);
mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n, mj_solveLD(vec2.data(), d->qLD, d->qLDiagInv, nv, n, m->M_rownnz, m->M_rowadr,
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr); m->M_colind, nullptr);
// expect equality of dot(v, M^-1 * v) and dot(M^-1/2 * v, M^-1/2 * v) // 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++) { for (int i = 0; i < n; i++) {
@@ -856,8 +830,8 @@ TEST_F(CoreSmoothTest, FactorIs) {
vector<mjtNum> qLDiagInvExpected(d->qLDiagInv, d->qLDiagInv + nv); vector<mjtNum> qLDiagInvExpected(d->qLDiagInv, d->qLDiagInv + nv);
vector<mjtNum> qLDiagInv(nv, 0); vector<mjtNum> qLDiagInv(nv, 0);
mj_factorI(qLD.data(), qLDiagInv.data(), nv, mj_factorI(qLD.data(), qLDiagInv.data(), nv, m->M_rownnz, m->M_rowadr,
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr); m->M_colind, nullptr);
// expect outputs to match to floating point precision // expect outputs to match to floating point precision
EXPECT_THAT(qLD, Pointwise(MjNear(1e-12, 1e-4), qLDexpected)); EXPECT_THAT(qLD, Pointwise(MjNear(1e-12, 1e-4), qLDexpected));
@@ -891,9 +865,9 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
// check that nJfv is correct: // check that nJfv is correct:
// corner vertices: 2 * (1+3) * 3 + 2 * (1+2) * 3 = 42 // corner vertices: 2 * (1+3) * 3 + 2 * (1+2) * 3 = 42
@@ -903,7 +877,7 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
EXPECT_EQ(m->nJfv, 123); EXPECT_EQ(m->nJfv, 123);
// Run kinematics to populate xpos/xmat initially // Run kinematics to populate xpos/xmat initially
mj_fwdKinematics(m, d); mj_fwdKinematics(m.get(), d.get());
// Check invariants for scale=1 // Check invariants for scale=1
// The constraints should be satisfied // The constraints should be satisfied
@@ -979,8 +953,8 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
auto fd_check = [&](double tolerance) { auto fd_check = [&](double tolerance) {
std::vector<mjtNum> qpos0(m->nq); std::vector<mjtNum> qpos0(m->nq);
mju_copy(qpos0.data(), d->qpos, m->nq); mju_copy(qpos0.data(), d->qpos, m->nq);
mj_kinematics(m, d); mj_kinematics(m.get(), d.get());
mj_flex(m, d); mj_flex(m.get(), d.get());
mjtNum eps = MjTol(1e-6, 1e-4); mjtNum eps = MjTol(1e-6, 1e-4);
int nflexvert = m->flex_vertnum[0]; int nflexvert = m->flex_vertnum[0];
@@ -994,9 +968,9 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
// plus // plus
mju_copy(d->qpos, qpos_backup.data(), m->nq); mju_copy(d->qpos, qpos_backup.data(), m->nq);
mj_integratePos(m, d->qpos, qvel.data(), eps); mj_integratePos(m.get(), d->qpos, qvel.data(), eps);
mj_kinematics(m, d); mj_kinematics(m.get(), d.get());
mj_flex(m, d); mj_flex(m.get(), d.get());
std::vector<mjtNum> L_plus(2 * nflexvert); std::vector<mjtNum> L_plus(2 * nflexvert);
for (int e = 0; e < 2 * nflexvert; ++e) { for (int e = 0; e < 2 * nflexvert; ++e) {
L_plus[e] = d->flexvert_length[e]; L_plus[e] = d->flexvert_length[e];
@@ -1004,9 +978,9 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
// minus // minus
mju_copy(d->qpos, qpos_backup.data(), m->nq); mju_copy(d->qpos, qpos_backup.data(), m->nq);
mj_integratePos(m, d->qpos, qvel.data(), -eps); mj_integratePos(m.get(), d->qpos, qvel.data(), -eps);
mj_kinematics(m, d); mj_kinematics(m.get(), d.get());
mj_flex(m, d); mj_flex(m.get(), d.get());
std::vector<mjtNum> L_minus(2 * nflexvert); std::vector<mjtNum> L_minus(2 * nflexvert);
for (int e = 0; e < 2 * nflexvert; ++e) { for (int e = 0; e < 2 * nflexvert; ++e) {
L_minus[e] = d->flexvert_length[e]; L_minus[e] = d->flexvert_length[e];
@@ -1017,8 +991,8 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
} }
} }
mju_copy(d->qpos, qpos_backup.data(), m->nq); mju_copy(d->qpos, qpos_backup.data(), m->nq);
mj_kinematics(m, d); mj_kinematics(m.get(), d.get());
mj_flex(m, d); mj_flex(m.get(), d.get());
// Compare with analytic // Compare with analytic
std::vector<mjtNum> jac_analytic(2 * nflexvert * m->nv); std::vector<mjtNum> jac_analytic(2 * nflexvert * m->nv);
@@ -1035,8 +1009,8 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
EXPECT_THAT(jac_analytic, Pointwise(MjNear(tolerance, 1e-1), jac_fd)); EXPECT_THAT(jac_analytic, Pointwise(MjNear(tolerance, 1e-1), jac_fd));
mju_copy(d->qpos, qpos0.data(), m->nq); mju_copy(d->qpos, qpos0.data(), m->nq);
mj_kinematics(m, d); mj_kinematics(m.get(), d.get());
mj_flex(m, d); mj_flex(m.get(), d.get());
}; };
fd_check(MjTol(5e-5, 5e-2)); fd_check(MjTol(5e-5, 5e-2));
@@ -1047,7 +1021,7 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
d->qpos[3 * i + 1] = d->xpos[3 * (i + 1) + 1]; d->qpos[3 * i + 1] = d->xpos[3 * (i + 1) + 1];
d->qpos[3 * i + 2] = d->xpos[3 * (i + 1) + 2]; 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 // Get mass scaling factor
mjtNum scale = 1.0; mjtNum scale = 1.0;
@@ -1072,9 +1046,6 @@ TEST_F(CoreSmoothTest, FlexVertLengthScaling) {
d->qpos[3 * i + 2] += 0.01 * (i % 2 ? 1 : -1); d->qpos[3 * i + 2] += 0.01 * (i % 2 ? 1 : -1);
} }
fd_check(5e-5); fd_check(5e-5);
mj_deleteData(d);
mj_deleteModel(m);
} }
// Test failure case for flexvert_J sparsity with skipped flexes // Test failure case for flexvert_J sparsity with skipped flexes
@@ -1107,12 +1078,12 @@ TEST_F(CoreSmoothTest, FlexvertJSparsitySkippedFlex) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
// Forward dynamics to compute Jacobians // Forward dynamics to compute Jacobians
mj_forward(model, data); mj_forward(model.get(), data.get());
// Check sparsity overlap // Check sparsity overlap
// Flex 0 starts at row 0 // Flex 0 starts at row 0
@@ -1136,9 +1107,6 @@ TEST_F(CoreSmoothTest, FlexvertJSparsitySkippedFlex) {
// Verify that Flex 2 starts AFTER Flex 0 ends // Verify that Flex 2 starts AFTER Flex 0 ends
EXPECT_GE(f2_start_adr, f0_end_adr) EXPECT_GE(f2_start_adr, f0_end_adr)
<< "Flex 2 Jacobian overwrites Flex 0 Jacobian due to skipped Flex 1"; << "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 stability of flexvert constraint under different integrator/solver
+95 -116
View File
@@ -49,10 +49,10 @@ TEST_F(FlexGatherStateTest, mju_flexGatherState_Grid) {
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
mj_forward(model, data); mj_forward(model.get(), data.get());
ASSERT_EQ(model->nflex, 1); ASSERT_EQ(model->nflex, 1);
int f = 0; int f = 0;
@@ -74,7 +74,7 @@ TEST_F(FlexGatherStateTest, mju_flexGatherState_Grid) {
} }
std::vector<mjtNum> xpos(3 * nodenum); 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 // Verify that gathered xpos matches the rotated data->xpos
for (int i = 0; i < nodenum; i++) { for (int i = 0; i < nodenum; i++) {
@@ -83,9 +83,6 @@ TEST_F(FlexGatherStateTest, mju_flexGatherState_Grid) {
EXPECT_NEAR(xpos[3 * i + 1], data->xpos[3 * b + 1], 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 + 2], data->xpos[3 * b + 2], 1e-5);
} }
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(FlexGatherStateTest, mju_flexGatherState_ShellMode) { TEST_F(FlexGatherStateTest, mju_flexGatherState_ShellMode) {
@@ -102,15 +99,15 @@ TEST_F(FlexGatherStateTest, mju_flexGatherState_ShellMode) {
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
ASSERT_EQ(model->nflex, 1); ASSERT_EQ(model->nflex, 1);
int f = 0; int f = 0;
model->flex_interp[f] = -1; model->flex_interp[f] = -1;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
mj_forward(model, data); mj_forward(model.get(), data.get());
int nodenum = model->flex_nodenum[f]; int nodenum = model->flex_nodenum[f];
int nstart = model->flex_nodeadr[f]; int nstart = model->flex_nodeadr[f];
@@ -126,18 +123,15 @@ TEST_F(FlexGatherStateTest, mju_flexGatherState_ShellMode) {
} }
std::vector<mjtNum> xpos(3 * nodenum); 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 // 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 + 0], shift[0], 1e-5);
EXPECT_NEAR(xpos[3 * 13 + 1], shift[1], 1e-5); EXPECT_NEAR(xpos[3 * 13 + 1], shift[1], 1e-5);
EXPECT_NEAR(xpos[3 * 13 + 2], shift[2], 1e-5); EXPECT_NEAR(xpos[3 * 13 + 2], shift[2], 1e-5);
mj_deleteData(data);
mj_deleteModel(model);
} }
using AngMomMatTest = MujocoTest; using AngMomMatTest = MujocoTest;
static constexpr char AngMomTestingModel[] = R"( static constexpr char AngMomTestingModel[] = R"(
@@ -172,26 +166,26 @@ static constexpr char AngMomTestingModel[] = R"(
// compare subtree angular momentum computed in two ways // compare subtree angular momentum computed in two ways
TEST_F(AngMomMatTest, CompareAngMom) { TEST_F(AngMomMatTest, CompareAngMom) {
char error[1024]; char error[1024];
mjModel* model = MjModelPtr model =
LoadModelFromString(AngMomTestingModel, error, sizeof(error)); LoadModelFromString(AngMomTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
int nv = model->nv; 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 // reset to the keyframe with some angular velocities
mj_resetDataKeyframe(model, data, 0); mj_resetDataKeyframe(model.get(), data.get(), 0);
mj_forward(model, data); mj_forward(model.get(), data.get());
// get the reference value of angular momentum // get the reference value of angular momentum
mj_subtreeVel(model, data); mj_subtreeVel(model.get(), data.get());
mjtNum angmom_ref[3]; 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 // compute angular momentum using the angular momentum matrix
mjtNum* angmom_mat = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv); mjtNum* angmom_mat = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv);
mj_angmomMat(model, data, angmom_mat, bodyid); mj_angmomMat(model.get(), data.get(), angmom_mat, bodyid);
mjtNum angmom_test[3]; mjtNum angmom_test[3];
mju_mulMatVec(angmom_test, angmom_mat, data->qvel, 3, nv); mju_mulMatVec(angmom_test, angmom_mat, data->qvel, 3, nv);
@@ -201,28 +195,26 @@ TEST_F(AngMomMatTest, CompareAngMom) {
} }
mju_free(angmom_mat); mju_free(angmom_mat);
mj_deleteData(data);
mj_deleteModel(model);
} }
// compare subtree angular momentum matrix: analytical and findiff // compare subtree angular momentum matrix: analytical and findiff
TEST_F(AngMomMatTest, CompareAngMomMats) { TEST_F(AngMomMatTest, CompareAngMomMats) {
char error[1024]; char error[1024];
mjModel* model = MjModelPtr model =
LoadModelFromString(AngMomTestingModel, error, sizeof(error)); LoadModelFromString(AngMomTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
int nv = model->nv; 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);
mjtNum* angmom_mat = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv); mjtNum* angmom_mat = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv);
mjtNum* angmom_mat_fd = (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 // reset to the keyframe with some angular velocities
mj_resetDataKeyframe(model, data, 0); mj_resetDataKeyframe(model.get(), data.get(), 0);
mj_forward(model, data); mj_forward(model.get(), data.get());
// compute the angular momentum matrix using the analytical method // 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 // compute the angular momentum matrix using finite differences
static const mjtNum eps = MjTol(1e-6, 1e-3); static const mjtNum eps = MjTol(1e-6, 1e-3);
@@ -230,16 +222,16 @@ TEST_F(AngMomMatTest, CompareAngMomMats) {
// reset vel, forward nudge i-th dof, get angmom // reset vel, forward nudge i-th dof, get angmom
mju_copy(data->qvel, model->key_qvel, model->nv); mju_copy(data->qvel, model->key_qvel, model->nv);
data->qvel[i] += eps; data->qvel[i] += eps;
mj_forward(model, data); mj_forward(model.get(), data.get());
mj_subtreeVel(model, data); mj_subtreeVel(model.get(), data.get());
mjtNum agmf[3]; 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 // reset vel, backward nudge i-th dof, get angmom
mju_copy(data->qvel, model->key_qvel, model->nv); mju_copy(data->qvel, model->key_qvel, model->nv);
data->qvel[i] -= eps; data->qvel[i] -= eps;
mj_forward(model, data); mj_forward(model.get(), data.get());
mj_subtreeVel(model, data); mj_subtreeVel(model.get(), data.get());
mjtNum agmb[3]; mjtNum agmb[3];
mju_copy3(agmb, data->subtree_angmom + 3 * bodyid); mju_copy3(agmb, data->subtree_angmom + 3 * bodyid);
@@ -256,8 +248,6 @@ TEST_F(AngMomMatTest, CompareAngMomMats) {
mju_free(angmom_mat_fd); mju_free(angmom_mat_fd);
mju_free(angmom_mat); mju_free(angmom_mat);
mj_deleteData(data);
mj_deleteModel(model);
} }
using JacobianTest = MujocoTest; using JacobianTest = MujocoTest;
@@ -297,22 +287,22 @@ static constexpr char kJacobianTestingModel[] = R"(
// compare analytic and finite-differenced subtree-com Jacobian // compare analytic and finite-differenced subtree-com Jacobian
TEST_F(JacobianTest, SubtreeJac) { TEST_F(JacobianTest, SubtreeJac) {
char error[1024]; char error[1024];
mjModel* model = MjModelPtr model =
LoadModelFromString(kJacobianTestingModel, error, sizeof(error)); LoadModelFromString(kJacobianTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
int nv = model->nv; int nv = model->nv;
int bodyid = mj_name2id(model, mjOBJ_BODY, "main"); int bodyid = mj_name2id(model.get(), mjOBJ_BODY, "main");
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
mjtNum* jac_subtree = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv); mjtNum* jac_subtree = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv);
mjtNum* qpos = (mjtNum*)mju_malloc(sizeof(mjtNum) * model->nq); mjtNum* qpos = (mjtNum*)mju_malloc(sizeof(mjtNum) * model->nq);
mjtNum* nudge = (mjtNum*)mju_malloc(sizeof(mjtNum) * nv); mjtNum* nudge = (mjtNum*)mju_malloc(sizeof(mjtNum) * nv);
// all we need for Jacobians are kinematics and CoM-related quantities // all we need for Jacobians are kinematics and CoM-related quantities
mj_kinematics(model, data); mj_kinematics(model.get(), data.get());
mj_comPos(model, data); mj_comPos(model.get(), data.get());
// get subtree CoM Jacobian of free body // 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 // save current subtree-com and qpos, clear nudge
mjtNum subtree_com[3]; mjtNum subtree_com[3];
@@ -326,16 +316,17 @@ TEST_F(JacobianTest, SubtreeJac) {
// reset qpos, nudge i-th dof, update data->qpos, reset nudge // reset qpos, nudge i-th dof, update data->qpos, reset nudge
mju_copy(data->qpos, qpos, model->nq); mju_copy(data->qpos, qpos, model->nq);
nudge[i] = 1; nudge[i] = 1;
mj_integratePos(model, data->qpos, nudge, eps); mj_integratePos(model.get(), data->qpos, nudge, eps);
nudge[i] = 0; nudge[i] = 0;
// kinematics and comPos to get nudged com // kinematics and comPos to get nudged com
mj_kinematics(model, data); mj_kinematics(model.get(), data.get());
mj_comPos(model, data); mj_comPos(model.get(), data.get());
// compare finite-differenced and analytic Jacobian // compare finite-differenced and analytic Jacobian
for (int j = 0; j < 3; j++) { for (int j = 0; j < 3; j++) {
mjtNum findiff = (data->subtree_com[3*bodyid+j] - subtree_com[j]) / eps; mjtNum findiff =
(data->subtree_com[3 * bodyid + j] - subtree_com[j]) / eps;
EXPECT_THAT(jac_subtree[nv * j + i], MjNear(findiff, eps, 1e-2)); EXPECT_THAT(jac_subtree[nv * j + i], MjNear(findiff, eps, 1e-2));
} }
} }
@@ -343,38 +334,36 @@ TEST_F(JacobianTest, SubtreeJac) {
mju_free(nudge); mju_free(nudge);
mju_free(qpos); mju_free(qpos);
mju_free(jac_subtree); mju_free(jac_subtree);
mj_deleteData(data);
mj_deleteModel(model);
} }
// confirm that applying linear forces via the subtree-com Jacobian only creates // confirm that applying linear forces via the subtree-com Jacobian only creates
// the expected linear accelerations (no accelerations of internal joints) // the expected linear accelerations (no accelerations of internal joints)
TEST_F(JacobianTest, SubtreeJacNoInternalAcc) { TEST_F(JacobianTest, SubtreeJacNoInternalAcc) {
char error[1024]; char error[1024];
mjModel* model = MjModelPtr model =
LoadModelFromString(kJacobianTestingModel, error, sizeof(error)); LoadModelFromString(kJacobianTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
int nv = model->nv; int nv = model->nv;
int bodyid = mj_name2id(model, mjOBJ_BODY, "main"); int bodyid = mj_name2id(model.get(), mjOBJ_BODY, "main");
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
mjtNum* jac_subtree = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv); mjtNum* jac_subtree = (mjtNum*)mju_malloc(sizeof(mjtNum) * 3 * nv);
// all we need for Jacobians are kinematics and CoM-related quantities // all we need for Jacobians are kinematics and CoM-related quantities
mj_kinematics(model, data); mj_kinematics(model.get(), data.get());
mj_comPos(model, data); mj_comPos(model.get(), data.get());
// get subtree CoM Jacobian of free body // 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 // uncomment for debugging
// mju_printMat(jac_subtree, 3, nv); // mju_printMat(jac_subtree, 3, nv);
// call fwdPosition since we'll need the factorised mass matrix in the test // 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 // treating the subtree Jacobian as the projection of 3 axis-aligned unit
// forces into joint space, solve for the resulting accelerations in-place // 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 // expect to find accelerations of magnitude 1/subtreemass in the first 3
// coordinates of the free joint and 0s elsewhere, since applying forces to // coordinates of the free joint and 0s elsewhere, since applying forces to
@@ -389,8 +378,6 @@ TEST_F(JacobianTest, SubtreeJacNoInternalAcc) {
} }
mju_free(jac_subtree); mju_free(jac_subtree);
mj_deleteData(data);
mj_deleteModel(model);
} }
static constexpr char kQuat[] = R"( static constexpr char kQuat[] = R"(
@@ -519,28 +506,28 @@ static constexpr char kHinge[] = R"(
TEST_F(JacobianTest, JacDot) { TEST_F(JacobianTest, JacDot) {
for (auto xml : {kHinge, kQuat, kTelescope, kFreeBall, kQuatlessPendulum}) { for (auto xml : {kHinge, kQuat, kTelescope, kFreeBall, kQuatlessPendulum}) {
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
int nv = model->nv; int nv = model->nv;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
// load keyframe if present, step for a bit // 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) { 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 // minimal call required for mj_jacDot outputs to be valid
mj_kinematics(model, data); mj_kinematics(model.get(), data.get());
mj_comPos(model, data); mj_comPos(model.get(), data.get());
mj_comVel(model, data); mj_comVel(model.get(), data.get());
// get bodyid // get bodyid
int bodyid = mj_name2id(model, mjOBJ_BODY, "query"); int bodyid = mj_name2id(model.get(), mjOBJ_BODY, "query");
EXPECT_GT(bodyid, 0); EXPECT_GT(bodyid, 0);
// get site position // get site position
int siteid = mj_name2id(model, mjOBJ_SITE, "query"); int siteid = mj_name2id(model.get(), mjOBJ_SITE, "query");
EXPECT_GT(siteid, -1); EXPECT_GT(siteid, -1);
mjtNum point[3]; mjtNum point[3];
mju_copy3(point, data->site_xpos + 3 * siteid); mju_copy3(point, data->site_xpos + 3 * siteid);
@@ -548,20 +535,23 @@ TEST_F(JacobianTest, JacDot) {
// jac, jac_dot // jac, jac_dot
std::vector<mjtNum> jacp(3 * nv); std::vector<mjtNum> jacp(3 * nv);
std::vector<mjtNum> jacr(3 * nv); std::vector<mjtNum> jacr(3 * nv);
mj_jac(model, data, jacp.data(), jacr.data(), point, bodyid); mj_jac(model.get(), data.get(), jacp.data(), jacr.data(), point, bodyid);
std::vector<mjtNum> jacp_dot(3 * nv); std::vector<mjtNum> jacp_dot(3 * nv);
std::vector<mjtNum> jacr_dot(3 * nv); std::vector<mjtNum> jacr_dot(3 * nv);
mj_jacDot(model, data, jacp_dot.data(), jacr_dot.data(), point, bodyid); 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 // jac_h: jacobian after integrating qpos with a timestep of h
const mjtNum h = MjTol(1e-7, 5e-4); const mjtNum h = MjTol(1e-7, 5e-4);
mj_integratePos(model, data->qpos, data->qvel, h); mj_integratePos(model.get(), data->qpos, data->qvel, h);
mj_kinematics(model, data); mj_kinematics(model.get(), data.get());
mj_comPos(model, data); mj_comPos(model.get(), data.get());
std::vector<mjtNum> jacp_h(3 * nv); std::vector<mjtNum> jacp_h(3 * nv);
std::vector<mjtNum> jacr_h(3 * nv); std::vector<mjtNum> jacr_h(3 * nv);
mju_copy3(point, data->site_xpos+3*siteid); // get updated site position mju_copy3(point,
mj_jac(model, data, jacp_h.data(), jacr_h.data(), point, bodyid); 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 // jac_dot_h finite-difference approximation
std::vector<mjtNum> jacp_dot_h(3 * nv); std::vector<mjtNum> jacp_dot_h(3 * nv);
@@ -575,9 +565,6 @@ TEST_F(JacobianTest, JacDot) {
mjtNum tol = 1e-5; mjtNum tol = 1e-5;
EXPECT_THAT(jacp_dot, Pointwise(MjNear(tol, 5e-2), jacp_dot_h)); EXPECT_THAT(jacp_dot, Pointwise(MjNear(tol, 5e-2), jacp_dot_h));
EXPECT_THAT(jacr_dot, Pointwise(MjNear(tol, 5e-2), jacr_dot_h)); EXPECT_THAT(jacr_dot, Pointwise(MjNear(tol, 5e-2), jacr_dot_h));
mj_deleteData(data);
mj_deleteModel(model);
} }
} }
@@ -585,26 +572,26 @@ TEST_F(JacobianTest, JacDot) {
TEST_F(JacobianTest, JacDotSparse) { TEST_F(JacobianTest, JacDotSparse) {
for (auto xml : {kHinge, kQuat, kTelescope, kFreeBall, kQuatlessPendulum}) { for (auto xml : {kHinge, kQuat, kTelescope, kFreeBall, kQuatlessPendulum}) {
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
int nv = model->nv; int nv = model->nv;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
// load keyframe if present, step for a bit // 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) { 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 // minimal call required for mj_jacDot outputs to be valid
mj_kinematics(model, data); mj_kinematics(model.get(), data.get());
mj_comPos(model, data); mj_comPos(model.get(), data.get());
mj_comVel(model, data); mj_comVel(model.get(), data.get());
// get bodyid and site position // 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); 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); EXPECT_GT(siteid, -1);
mjtNum point[3]; mjtNum point[3];
mju_copy3(point, data->site_xpos + 3 * siteid); mju_copy3(point, data->site_xpos + 3 * siteid);
@@ -612,7 +599,8 @@ TEST_F(JacobianTest, JacDotSparse) {
// dense jacDot // dense jacDot
std::vector<mjtNum> jacp_dense(3 * nv); std::vector<mjtNum> jacp_dense(3 * nv);
std::vector<mjtNum> jacr_dense(3 * nv); std::vector<mjtNum> jacr_dense(3 * nv);
mj_jacDot(model, data, jacp_dense.data(), jacr_dense.data(), point, bodyid); mj_jacDot(model.get(), data.get(), jacp_dense.data(), jacr_dense.data(),
point, bodyid);
// compute body chain using public mjModel fields // compute body chain using public mjModel fields
std::vector<int> chain(nv); std::vector<int> chain(nv);
@@ -631,8 +619,8 @@ TEST_F(JacobianTest, JacDotSparse) {
// sparse jacDot // sparse jacDot
std::vector<mjtNum> jacp_sparse(3 * NV); std::vector<mjtNum> jacp_sparse(3 * NV);
std::vector<mjtNum> jacr_sparse(3 * NV); std::vector<mjtNum> jacr_sparse(3 * NV);
mj_jacDotSparse(model, data, jacp_sparse.data(), jacr_sparse.data(), mj_jacDotSparse(model.get(), data.get(), jacp_sparse.data(),
point, bodyid, NV, chain.data()); jacr_sparse.data(), point, bodyid, NV, chain.data());
// expand sparse to dense and compare // expand sparse to dense and compare
std::vector<mjtNum> jacp_expanded(3 * nv, 0); std::vector<mjtNum> jacp_expanded(3 * nv, 0);
@@ -648,13 +636,9 @@ TEST_F(JacobianTest, JacDotSparse) {
// expect bitwise equality // expect bitwise equality
EXPECT_EQ(jacp_expanded, jacp_dense); EXPECT_EQ(jacp_expanded, jacp_dense);
EXPECT_EQ(jacr_expanded, jacr_dense); EXPECT_EQ(jacr_expanded, jacr_dense);
mj_deleteData(data);
mj_deleteModel(model);
} }
} }
// validate rotational Jacobian used in welds // validate rotational Jacobian used in welds
TEST_F(JacobianTest, WeldRotJacobian) { TEST_F(JacobianTest, WeldRotJacobian) {
#ifdef mjUSESINGLE #ifdef mjUSESINGLE
@@ -678,18 +662,18 @@ TEST_F(JacobianTest, WeldRotJacobian) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, testing::NotNull()) << error; ASSERT_THAT(model.get(), testing::NotNull()) << error;
ASSERT_EQ(model->nq, 7); ASSERT_EQ(model->nq, 7);
ASSERT_EQ(model->nv, 6); ASSERT_EQ(model->nv, 6);
static const int nv = 6; // for increased readability 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 // arbitrary initial values for the ball and hinge joints
mjtNum qpos0[7] = {.5, .5, .5, .5, .7, .8, .9}; mjtNum qpos0[7] = {.5, .5, .5, .5, .7, .8, .9};
// compute required quantities using mj_step1 // compute required quantities using mj_step1
mj_step1(model, data); mj_step1(model.get(), data.get());
// get orientation error // get orientation error
mjtNum res[3]; mjtNum res[3];
@@ -716,7 +700,7 @@ TEST_F(JacobianTest, WeldRotJacobian) {
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 // compute Jacobian with finite-differencing
mjtNum jacFD[3 * nv]; mjtNum jacFD[3 * nv];
@@ -728,7 +712,7 @@ TEST_F(JacobianTest, WeldRotJacobian) {
dqpos[i] = eps; dqpos[i] = eps;
// get nudged residual // get nudged residual
RotationResidual(model, data, qpos0, dqpos, dres); RotationResidual(model.get(), data.get(), qpos0, dqpos, dres);
// remove nudge // remove nudge
dqpos[i] = 0.0; dqpos[i] = 0.0;
@@ -741,7 +725,7 @@ TEST_F(JacobianTest, WeldRotJacobian) {
// reset mjData to qpos0 // reset mjData to qpos0
mju_copy(data->qpos, qpos0, 7); mju_copy(data->qpos, qpos0, 7);
mj_step1(model, data); mj_step1(model.get(), data.get());
// intermediate quaternions quat1 and quat2 // intermediate quaternions quat1 and quat2
mjtNum quat1[4], negQuat2[4]; mjtNum quat1[4], negQuat2[4];
@@ -753,8 +737,8 @@ TEST_F(JacobianTest, WeldRotJacobian) {
mjtNum point[3] = {0}; mjtNum point[3] = {0};
// rotational Jacobian difference // rotational Jacobian difference
mj_jacDifPair(model, data, NULL, 2, 1, point, point, mj_jacDifPair(model.get(), data.get(), NULL, 2, 1, point, point, NULL, NULL,
NULL, NULL, NULL, jac0, jac1, jacdif, mj_isSparse(model), NULL, jac0, jac1, jacdif, mj_isSparse(model.get()),
/*flg_skipcommon=*/0); /*flg_skipcommon=*/0);
// formula: 0.5 * neg(quat2) * (jac1-jac2) * quat1 // formula: 0.5 * neg(quat2) * (jac1-jac2) * quat1
@@ -778,12 +762,7 @@ TEST_F(JacobianTest, WeldRotJacobian) {
// test that analytical and finite-differenced Jacobians match // test that analytical and finite-differenced Jacobians match
EXPECT_THAT(AsVector(jacFD, 3 * nv), EXPECT_THAT(AsVector(jacFD, 3 * nv),
Pointwise(MjNear(eps, 1e-3), AsVector(jacdif, 3 * nv))); Pointwise(MjNear(eps, 1e-3), AsVector(jacdif, 3 * nv)));
mj_deleteData(data);
mj_deleteModel(model);
} }
} // namespace } // namespace
} // namespace mujoco } // namespace mujoco
+158 -212
View File
@@ -36,11 +36,11 @@ namespace mujoco {
namespace { namespace {
using ::std::vector; using ::std::vector;
using ::testing::Pointwise;
using ::testing::DoubleNear; using ::testing::DoubleNear;
using ::testing::Eq;
using ::testing::Each; using ::testing::Each;
using ::testing::Eq;
using ::testing::NotNull; using ::testing::NotNull;
using ::testing::Pointwise;
using DerivativeTest = MujocoTest; using DerivativeTest = MujocoTest;
// errors smaller than this are ignored // errors smaller than this are ignored
@@ -59,8 +59,8 @@ static mjtNum RelativeError(mjtNum a, mjtNum b) {
// expect two 2D arrays to have elementwise relative error smaller than eps // expect two 2D arrays to have elementwise relative error smaller than eps
// return maximum absolute error // return maximum absolute error
static mjtNum CompareMatrices(mjtNum* Actual, mjtNum* Expected, static mjtNum CompareMatrices(mjtNum* Actual, mjtNum* Expected, int nrow,
int nrow, int ncol, mjtNum eps) { int ncol, mjtNum eps) {
mjtNum max_error = 0; mjtNum max_error = 0;
for (int i = 0; i < nrow; i++) { for (int i = 0; i < nrow; i++) {
for (int j = 0; j < ncol; j++) { for (int j = 0; j < ncol; j++) {
@@ -68,8 +68,7 @@ static mjtNum CompareMatrices(mjtNum* Actual, mjtNum* Expected,
mjtNum expected = Expected[i * ncol + j]; mjtNum expected = Expected[i * ncol + j];
EXPECT_LT(RelativeError(actual, expected), eps) EXPECT_LT(RelativeError(actual, expected), eps)
<< "error at position (" << i << ", " << j << ")" << "error at position (" << i << ", " << j << ")"
<< "\nexpected = " << expected << "\nexpected = " << expected << "\nactual = " << actual
<< "\nactual = " << actual
<< "\ndiff = " << expected - actual; << "\ndiff = " << expected - actual;
max_error = mjMAX(mju_abs(actual - expected), max_error); max_error = mjMAX(mju_abs(actual - expected), max_error);
} }
@@ -89,8 +88,7 @@ static const char* const kDamperActuatorsPath =
"engine/testdata/actuation/damper.xml"; "engine/testdata/actuation/damper.xml";
static const char* const kDampedPendulumPath = static const char* const kDampedPendulumPath =
"engine/testdata/derivative/damped_pendulum.xml"; "engine/testdata/derivative/damped_pendulum.xml";
static const char* const kLinearPath = static const char* const kLinearPath = "engine/testdata/derivative/linear.xml";
"engine/testdata/derivative/linear.xml";
static const char* const kDCMotorPath = static const char* const kDCMotorPath =
"engine/testdata/derivative/dcmotor.xml"; "engine/testdata/derivative/dcmotor.xml";
static const char* const kModelPath = "testdata/model.xml"; static const char* const kModelPath = "testdata/model.xml";
@@ -98,14 +96,13 @@ static const char* const kModelPath = "testdata/model.xml";
// compare analytic and finite-difference d_smooth/d_qvel // compare analytic and finite-difference d_smooth/d_qvel
TEST_F(DerivativeTest, SmoothDvel) { TEST_F(DerivativeTest, SmoothDvel) {
// run test on all models // run test on all models
for (const char* local_path : {kEnergyConservingPendulumPath, for (const char* local_path :
kTumblingThinObjectPath, {kEnergyConservingPendulumPath, kTumblingThinObjectPath,
kDampedActuatorsPath, kDampedActuatorsPath, kDamperActuatorsPath, kDCMotorPath}) {
kDamperActuatorsPath,
kDCMotorPath}) {
const std::string xml_path = GetTestDataFilePath(local_path); const std::string xml_path = GetTestDataFilePath(local_path);
char error[1024] = ""; char error[1024] = "";
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); mjModel* model =
mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(model, testing::NotNull()) << "Failed to load model: " << error; ASSERT_THAT(model, testing::NotNull()) << "Failed to load model: " << error;
int nD = model->nD; int nD = model->nD;
mjData* data = mj_makeData(model); mjData* data = mj_makeData(model);
@@ -171,13 +168,12 @@ TEST_F(DerivativeTest, DisabledActuators) {
)"; )";
char error[1024]; char error[1024];
mjModel* m1 = LoadModelFromString(xml1, error, sizeof(error)); MjModelPtr m1 = LoadModelFromString(xml1, error, sizeof(error));
ASSERT_THAT(m1, NotNull()) << error; ASSERT_THAT(m1.get(), NotNull()) << error;
mjData* d1 = mj_makeData(m1); MjDataPtr d1 = MakeData(m1);
d1->ctrl[0] = 6; d1->ctrl[0] = 6;
while (d1->time < 1) while (d1->time < 1) mj_step(m1.get(), d1.get());
mj_step(m1, d1);
// model with a position actuator and an intvelocity actuator // model with a position actuator and an intvelocity actuator
static constexpr char xml2[] = R"( static constexpr char xml2[] = R"(
@@ -198,22 +194,16 @@ TEST_F(DerivativeTest, DisabledActuators) {
</mujoco> </mujoco>
)"; )";
mjModel* m2 = LoadModelFromString(xml2); MjModelPtr m2 = LoadModelFromString(xml2);
mjData* d2 = mj_makeData(m2); MjDataPtr d2 = MakeData(m2);
d2->ctrl[0] = 6; d2->ctrl[0] = 6;
d2->ctrl[1] = 6; d2->ctrl[1] = 6;
while (d2->time < 1) while (d2->time < 1) mj_step(m2.get(), d2.get());
mj_step(m2, d2);
// expect same qvel in both models // expect same qvel in both models
EXPECT_EQ(d1->qvel[0], d2->qvel[0]); EXPECT_EQ(d1->qvel[0], d2->qvel[0]);
mj_deleteData(d2);
mj_deleteModel(m2);
mj_deleteData(d1);
mj_deleteModel(m1);
} }
// actuator order has no effect // actuator order has no effect
@@ -242,15 +232,14 @@ TEST_F(DerivativeTest, ActuatorOrder) {
)"; )";
char error[1024]; char error[1024];
mjModel* m1 = LoadModelFromString(xml1, error, sizeof(error)); MjModelPtr m1 = LoadModelFromString(xml1, error, sizeof(error));
ASSERT_THAT(m1, NotNull()) << "Failed to load model: " << error; ASSERT_THAT(m1.get(), NotNull()) << "Failed to load model: " << error;
mjData* d1 = mj_makeData(m1); MjDataPtr d1 = MakeData(m1);
d1->ctrl[0] = 6; d1->ctrl[0] = 6;
d1->ctrl[1] = 6; d1->ctrl[1] = 6;
while (d1->time < 1) while (d1->time < 1) mj_step(m1.get(), d1.get());
mj_step(m1, d1);
// model with stateful actuator second // model with stateful actuator second
static constexpr char xml2[] = R"( static constexpr char xml2[] = R"(
@@ -275,30 +264,24 @@ TEST_F(DerivativeTest, ActuatorOrder) {
</mujoco> </mujoco>
)"; )";
mjModel* m2 = LoadModelFromString(xml2, error, sizeof(error)); MjModelPtr m2 = LoadModelFromString(xml2, error, sizeof(error));
ASSERT_THAT(m2, NotNull()) << "Failed to load model: " << error; ASSERT_THAT(m2.get(), NotNull()) << "Failed to load model: " << error;
mjData* d2 = mj_makeData(m2); MjDataPtr d2 = MakeData(m2);
d2->ctrl[0] = 6; d2->ctrl[0] = 6;
d2->ctrl[1] = 6; d2->ctrl[1] = 6;
while (d2->time < 1) while (d2->time < 1) mj_step(m2.get(), d2.get());
mj_step(m2, d2);
// expect same qvel in both models // expect same qvel in both models
EXPECT_EQ(d1->qvel[0], d2->qvel[0]); EXPECT_EQ(d1->qvel[0], d2->qvel[0]);
EXPECT_EQ(d1->qvel[1], d2->qvel[1]); EXPECT_EQ(d1->qvel[1], d2->qvel[1]);
mj_deleteData(d2);
mj_deleteModel(m2);
mj_deleteData(d1);
mj_deleteModel(m1);
} }
// compare analytic and fin-diff d_qfrc_passive/d_qvel // compare analytic and fin-diff d_qfrc_passive/d_qvel
TEST_F(DerivativeTest, PassiveDvel) { TEST_F(DerivativeTest, PassiveDvel) {
for (const char* local_path : {kTumblingThinObjectPath, for (const char* local_path :
kTumblingThinObjectEllipsoidPath}) { {kTumblingThinObjectPath, kTumblingThinObjectEllipsoidPath}) {
// load model // load model
const std::string xml_path = GetTestDataFilePath(local_path); const std::string xml_path = GetTestDataFilePath(local_path);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
@@ -355,9 +338,8 @@ TEST_F(DerivativeTest, StepSkip) {
// disable warm-starts so we don't need to save qacc_warmstart // disable warm-starts so we don't need to save qacc_warmstart
model->opt.disableflags |= mjDSBL_WARMSTART; model->opt.disableflags |= mjDSBL_WARMSTART;
for (const mjtIntegrator integrator : {mjINT_EULER, for (const mjtIntegrator integrator :
mjINT_IMPLICIT, {mjINT_EULER, mjINT_IMPLICIT, mjINT_IMPLICITFAST}) {
mjINT_IMPLICITFAST}) {
model->opt.integrator = integrator; model->opt.integrator = integrator;
// reset, take 20 steps // reset, take 20 steps
@@ -449,8 +431,8 @@ static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) {
Ac[i * nv + i] = -m->jnt_stiffness[i]; Ac[i * nv + i] = -m->jnt_stiffness[i];
Ac[nv * nv + i * nv + i] = -m->dof_damping[i]; Ac[nv * nv + i * nv + i] = -m->dof_damping[i];
} }
mj_solveLD(Ac, d->qH, d->qHDiagInv, nv, 2*nv, mj_solveLD(Ac, d->qH, d->qHDiagInv, nv, 2 * nv, m->M_rownnz, m->M_rowadr,
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr); m->M_colind, nullptr);
// A = [dt*Ac; Ac] // A = [dt*Ac; Ac]
mju_transpose(A, Ac, 2 * nv, nv); mju_transpose(A, Ac, 2 * nv, nv);
@@ -477,8 +459,8 @@ static void LinearSystem(const mjModel* m, mjData* d, mjtNum* A, mjtNum* B) {
mjtNum* BcT = mj_stackAllocNum(d, nv * nu); mjtNum* BcT = mj_stackAllocNum(d, nv * nu);
mju_sparse2dense(Bc, d->actuator_moment, nu, nv, d->moment_rownnz, mju_sparse2dense(Bc, d->actuator_moment, nu, nv, d->moment_rownnz,
d->moment_rowadr, d->moment_colind); d->moment_rowadr, d->moment_colind);
mj_solveLD(Bc, d->qH, d->qHDiagInv, nv, nu, mj_solveLD(Bc, d->qH, d->qHDiagInv, nv, nu, m->M_rownnz, m->M_rowadr,
m->M_rownnz, m->M_rowadr, m->M_colind, nullptr); m->M_colind, nullptr);
mju_transpose(BcT, Bc, nu, nv); mju_transpose(BcT, Bc, nu, nv);
mju_scl(B, BcT, dt * dt, nu * nv); mju_scl(B, BcT, dt * dt, nu * nv);
mju_scl(B + nu * nv, BcT, dt, nu * nv); mju_scl(B + nu * nv, BcT, dt, nu * nv);
@@ -516,8 +498,8 @@ TEST_F(DerivativeTest, LinearSystem) {
mjtNum* AFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * 2 * nv); mjtNum* AFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * 2 * nv);
mjtNum* BFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu); mjtNum* BFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu);
mjd_transitionFD(model, data, eps, /*centered=*/0, mjd_transitionFD(model, data, eps, /*centered=*/0, AFD, BFD, nullptr,
AFD, BFD, nullptr, nullptr); nullptr);
// uncomment for debugging: // uncomment for debugging:
// PrintMatrix(AFD, 2*nv, 2*nv); // PrintMatrix(AFD, 2*nv, 2*nv);
@@ -530,8 +512,8 @@ TEST_F(DerivativeTest, LinearSystem) {
// central differenced A and B // central differenced A and B
mjtNum* AFDc = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * 2 * nv); mjtNum* AFDc = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * 2 * nv);
mjtNum* BFDc = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu); mjtNum* BFDc = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu);
mjd_transitionFD(model, data, eps, /*centered=*/1, mjd_transitionFD(model, data, eps, /*centered=*/1, AFDc, BFDc, nullptr,
AFDc, BFDc, nullptr, nullptr); nullptr);
// expect central derivatives to be equal to forward differences // expect central derivatives to be equal to forward differences
CompareMatrices(AFD, AFDc, 2 * nv, 2 * nv, eps); CompareMatrices(AFD, AFDc, 2 * nv, 2 * nv, eps);
@@ -573,22 +555,22 @@ TEST_F(DerivativeTest, ClampedCtrlDerivatives) {
// set ctrl to the limits, request forward differences // set ctrl to the limits, request forward differences
data->ctrl[0] = 1; data->ctrl[0] = 1;
data->ctrl[1] = -1; data->ctrl[1] = -1;
mjd_transitionFD(model, data, eps, /*centered=*/0, mjd_transitionFD(model, data, eps, /*centered=*/0, nullptr, BFD, nullptr,
nullptr, BFD, nullptr, nullptr); nullptr);
// expect FD and analytic derivatives to be similar to eps precision // expect FD and analytic derivatives to be similar to eps precision
CompareMatrices(B, BFD, 2 * nv, nu, eps); CompareMatrices(B, BFD, 2 * nv, nu, eps);
// ctrl remains at limits, request central differences // ctrl remains at limits, request central differences
mjd_transitionFD(model, data, eps, /*centered=*/1, mjd_transitionFD(model, data, eps, /*centered=*/1, nullptr, BFD, nullptr,
nullptr, BFD, nullptr, nullptr); nullptr);
// expect FD and analytic derivatives to be similar to eps precision // expect FD and analytic derivatives to be similar to eps precision
CompareMatrices(B, BFD, 2 * nv, nu, eps); CompareMatrices(B, BFD, 2 * nv, nu, eps);
// set ctrl beyond limits, request forward differences // set ctrl beyond limits, request forward differences
data->ctrl[0] = 2; data->ctrl[0] = 2;
data->ctrl[1] = -2; data->ctrl[1] = -2;
mjd_transitionFD(model, data, eps, /*centered=*/0, mjd_transitionFD(model, data, eps, /*centered=*/0, nullptr, BFD, nullptr,
nullptr, BFD, nullptr, nullptr); nullptr);
// expect derivatives to be 0 // expect derivatives to be 0
EXPECT_THAT(AsVector(BFD, 2 * nv * nu), Each(Eq(0.0))); EXPECT_THAT(AsVector(BFD, 2 * nv * nu), Each(Eq(0.0)));
@@ -597,8 +579,8 @@ TEST_F(DerivativeTest, ClampedCtrlDerivatives) {
EXPECT_EQ(data->ctrl[1], -2.0); EXPECT_EQ(data->ctrl[1], -2.0);
// ctrl remains beyond limits, request centered differences // ctrl remains beyond limits, request centered differences
mjd_transitionFD(model, data, eps, /*centered=*/1, mjd_transitionFD(model, data, eps, /*centered=*/1, nullptr, BFD, nullptr,
nullptr, BFD, nullptr, nullptr); nullptr);
// expect derivatives to be 0 // expect derivatives to be 0
EXPECT_THAT(AsVector(BFD, 2 * nv * nu), Each(Eq(0.0))); EXPECT_THAT(AsVector(BFD, 2 * nv * nu), Each(Eq(0.0)));
@@ -631,23 +613,19 @@ TEST_F(DerivativeTest, SensorDerivatives) {
</mujoco> </mujoco>
)"; )";
mjModel* model = LoadModelFromString(xml); MjModelPtr model = LoadModelFromString(xml);
int nv = model->nv, nu = model->nu, ns = model->nsensordata; int nv = model->nv, nu = model->nu, ns = model->nsensordata;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
// finite differenced C and D // finite differenced C and D
mjtNum eps = 1e-6; mjtNum eps = 1e-6;
mjtNum* CFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * ns * 2 * nv); mjtNum* CFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * ns * 2 * nv);
mjtNum* DFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * ns * nu); mjtNum* DFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * ns * nu);
mjd_transitionFD(model, data, eps, /*centered=*/0, mjd_transitionFD(model.get(), data.get(), eps, /*centered=*/0, nullptr,
nullptr, nullptr, CFD, DFD); nullptr, CFD, DFD);
// expected analytic C and D // expected analytic C and D
mjtNum C[6] = { mjtNum C[6] = {1, 0, 0, 1, 0, 0};
1, 0,
0, 1,
0, 0
};
mjtNum D[3] = { mjtNum D[3] = {
0, 0,
@@ -661,8 +639,6 @@ TEST_F(DerivativeTest, SensorDerivatives) {
mju_free(DFD); mju_free(DFD);
mju_free(CFD); mju_free(CFD);
mj_deleteData(data);
mj_deleteModel(model);
} }
// if sensor derivatives aren't requested, don't compute sensors // if sensor derivatives aren't requested, don't compute sensors
@@ -686,9 +662,9 @@ TEST_F(DerivativeTest, SensorSkip) {
</mujoco> </mujoco>
)"; )";
mjModel* model = LoadModelFromString(xml); MjModelPtr model = LoadModelFromString(xml);
int nv = model->nv, nu = model->nu; int nv = model->nv, nu = model->nu;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
// set a sentinel value in the sensor // set a sentinel value in the sensor
data->sensordata[0] = 1337; data->sensordata[0] = 1337;
@@ -696,14 +672,12 @@ TEST_F(DerivativeTest, SensorSkip) {
// finite differenced B // finite differenced B
mjtNum eps = 1e-6; mjtNum eps = 1e-6;
mjtNum* BFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu); mjtNum* BFD = (mjtNum*)mju_malloc(sizeof(mjtNum) * 2 * nv * nu);
mjd_transitionFD(model, data, eps, /*centered=*/0, mjd_transitionFD(model.get(), data.get(), eps, /*centered=*/0, nullptr, BFD,
nullptr, BFD, nullptr, nullptr); nullptr, nullptr);
EXPECT_EQ(data->sensordata[0], 1337) << "sensors should not be recomputed"; EXPECT_EQ(data->sensordata[0], 1337) << "sensors should not be recomputed";
mju_free(BFD); mju_free(BFD);
mj_deleteData(data);
mj_deleteModel(model);
} }
// derivatives don't mutate the state // derivatives don't mutate the state
@@ -733,7 +707,6 @@ TEST_F(DerivativeTest, NoStateMutation) {
data->act[i] = data0->act[i] = (mjtNum)i + 1; data->act[i] = data0->act[i] = (mjtNum)i + 1;
} }
// allocate Jacobians, call derivatives // allocate Jacobians, call derivatives
int ndx = nv + nv + na; int ndx = nv + nv + na;
mjtNum* A = (mjtNum*)mju_malloc(sizeof(mjtNum) * ndx * ndx); mjtNum* A = (mjtNum*)mju_malloc(sizeof(mjtNum) * ndx * ndx);
@@ -754,22 +727,21 @@ TEST_F(DerivativeTest, NoStateMutation) {
mju_free(C); mju_free(C);
mju_free(B); mju_free(B);
mju_free(A); mju_free(A);
mj_deleteData(data);
mj_deleteData(data0); mj_deleteData(data0);
mj_deleteData(data);
mj_deleteModel(model); mj_deleteModel(model);
} }
// compare dense and sparse derivatives of qfrc_bias (RNE) // compare dense and sparse derivatives of qfrc_bias (RNE)
TEST_F(DerivativeTest, DenseSparseRneEquivalent) { TEST_F(DerivativeTest, DenseSparseRneEquivalent) {
// run test on all models // run test on all models
for (const char* local_path : {kEnergyConservingPendulumPath, for (const char* local_path :
kTumblingThinObjectPath, {kEnergyConservingPendulumPath, kTumblingThinObjectPath,
kDampedActuatorsPath, kDampedActuatorsPath, kDamperActuatorsPath, kDCMotorPath}) {
kDamperActuatorsPath,
kDCMotorPath}) {
const std::string xml_path = GetTestDataFilePath(local_path); const std::string xml_path = GetTestDataFilePath(local_path);
char error[1024] = ""; char error[1024] = "";
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); mjModel* model =
mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(model, testing::NotNull()) << "Failed to load model: " << error; ASSERT_THAT(model, testing::NotNull()) << "Failed to load model: " << error;
int nD = model->nD; int nD = model->nD;
mjtNum* qDeriv = (mjtNum*)mju_malloc(sizeof(mjtNum) * nD); mjtNum* qDeriv = (mjtNum*)mju_malloc(sizeof(mjtNum) * nD);
@@ -799,8 +771,8 @@ TEST_F(DerivativeTest, DenseSparseRneEquivalent) {
EXPECT_THAT(AsVector(data->qDeriv, nD), EXPECT_THAT(AsVector(data->qDeriv, nD),
Pointwise(MjNear(1e-12, 5e-5), AsVector(qDeriv, nD))); Pointwise(MjNear(1e-12, 5e-5), AsVector(qDeriv, nD)));
mj_deleteData(data);
mju_free(qDeriv); mju_free(qDeriv);
mj_deleteData(data);
mj_deleteModel(model); mj_deleteModel(model);
} }
} }
@@ -829,21 +801,20 @@ TEST_F(DerivativeTest, LinearSystemInverse) {
// get derivatives // get derivatives
mjtNum eps = 1e-6; mjtNum eps = 1e-6;
mjtByte flg_actuation = 0; mjtByte flg_actuation = 0;
mjd_inverseFD(model, data, eps, flg_actuation, mjd_inverseFD(model, data, eps, flg_actuation, DfDq.data(), DfDv.data(),
DfDq.data(), DfDv.data(), DfDa.data(), DfDa.data(), DsDq.data(), DsDv.data(), DsDa.data(),
DsDq.data(), DsDv.data(), DsDa.data(),
DmDq.data()); DmDq.data());
// expect that position derivatives are the stiffnesses // expect that position derivatives are the stiffnesses
vector<mjtNum> DfDq_expect = {model->jnt_stiffness[0], 0, 0, vector<mjtNum> DfDq_expect = {model->jnt_stiffness[0], 0, 0, 0,
0, model->jnt_stiffness[1], 0, model->jnt_stiffness[1], 0, 0, 0,
0, 0, model->jnt_stiffness[2]}; model->jnt_stiffness[2]};
EXPECT_THAT(DfDq, Pointwise(DoubleNear(eps), DfDq_expect)); EXPECT_THAT(DfDq, Pointwise(DoubleNear(eps), DfDq_expect));
// expect that velocity derivatives are the dampings // expect that velocity derivatives are the dampings
vector<mjtNum> DfDv_expect = {model->dof_damping[0], 0, 0, vector<mjtNum> DfDv_expect = {model->dof_damping[0], 0, 0, 0,
0, model->dof_damping[1], 0, model->dof_damping[1], 0, 0, 0,
0, 0, model->dof_damping[2]}; model->dof_damping[2]};
EXPECT_THAT(DfDv, Pointwise(DoubleNear(eps), DfDv_expect)); EXPECT_THAT(DfDv, Pointwise(DoubleNear(eps), DfDv_expect));
// expect that acceleration derivatives are the mass matrix // expect that acceleration derivatives are the mass matrix
@@ -907,8 +878,8 @@ void randomQuatPair(mjtNum qa[4], mjtNum qb[4], mjtNum angle, int seed) {
} }
// utility: finite-difference Jacobians of mju_subQuat // utility: finite-difference Jacobians of mju_subQuat
static void subQuatFD(mjtNum Da[9], mjtNum Db[9], static void subQuatFD(mjtNum Da[9], mjtNum Db[9], const mjtNum qa[4],
const mjtNum qa[4], const mjtNum qb[4], mjtNum eps) { const mjtNum qb[4], mjtNum eps) {
// subQuat // subQuat
mjtNum y[3]; mjtNum y[3];
mju_subQuat(y, qa, qb); mju_subQuat(y, qa, qb);
@@ -949,7 +920,8 @@ static void subQuatFD(mjtNum Da[9], mjtNum Db[9],
TEST_F(DerivativeTest, SubQuat) { TEST_F(DerivativeTest, SubQuat) {
const int nrepeats = 10; // number of repeats const int nrepeats = 10; // number of repeats
const mjtNum eps = MjTol(1e-7, 1e-3); // epsilon for finite-differencing and comparison const mjtNum eps =
MjTol(1e-7, 1e-3); // epsilon for finite-differencing and comparison
int seed = 1; int seed = 1;
for (int i = 0; i < nrepeats; i++) { for (int i = 0; i < nrepeats; i++) {
@@ -1000,10 +972,9 @@ static void randomQuatVel(mjtNum quat[4], mjtNum vel[3], int seed) {
} }
// utility: finite-difference Jacobians of mju_quatIntegrate // utility: finite-difference Jacobians of mju_quatIntegrate
void mjd_quatIntegrateFD(mjtNum Dquat[9], mjtNum Ds[9], void mjd_quatIntegrateFD(mjtNum Dquat[9], mjtNum Ds[9], mjtNum Dvel[9],
mjtNum Dvel[9], mjtNum Dh[3], mjtNum Dh[3], const mjtNum quat[4],
const mjtNum quat[4], const mjtNum vel[3], const mjtNum vel[3], mjtNum h, mjtNum eps) {
mjtNum h, mjtNum eps) {
// compute y, output of mju_quatIntegrate(quat, vel, h) // compute y, output of mju_quatIntegrate(quat, vel, h)
mjtNum y[4] = {quat[0], quat[1], quat[2], quat[3]}; mjtNum y[4] = {quat[0], quat[1], quat[2], quat[3]};
mju_quatIntegrate(y, vel, h); mju_quatIntegrate(y, vel, h);
@@ -1058,7 +1029,8 @@ void mjd_quatIntegrateFD(mjtNum Dquat[9], mjtNum Ds[9],
TEST_F(DerivativeTest, quatIntegrate) { TEST_F(DerivativeTest, quatIntegrate) {
const int nrepeats = 10; // number of repeats const int nrepeats = 10; // number of repeats
const mjtNum eps = MjTol(1e-7, 1e-3); // epsilon for finite-differencing and comparison const mjtNum eps =
MjTol(1e-7, 1e-3); // epsilon for finite-differencing and comparison
int seed = 1; int seed = 1;
for (int i = 0; i < nrepeats; i++) { for (int i = 0; i < nrepeats; i++) {
@@ -1114,22 +1086,22 @@ TEST_F(DerivativeTest, ForcerangeClampedDerivative) {
)"; )";
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
mjtNum dt_small = 1e-4; mjtNum dt_small = 1e-4;
mjtNum dt_large = 1e-2; mjtNum dt_large = 1e-2;
mjtNum duration = 1.0; mjtNum duration = 1.0;
mjData* d_gt = mj_makeData(m); MjDataPtr d_gt = MakeData(m);
mjData* d_implicit = mj_makeData(m); MjDataPtr d_implicit = MakeData(m);
mjData* d_euler = mj_makeData(m); MjDataPtr d_euler = MakeData(m);
mj_resetData(m, d_gt); mj_resetData(m.get(), d_gt.get());
mj_resetData(m, d_implicit); mj_resetData(m.get(), d_implicit.get());
mj_resetData(m, d_euler); mj_resetData(m.get(), d_euler.get());
d_gt->ctrl[0] = 0.5; d_gt.get()->ctrl[0] = 0.5;
d_implicit->ctrl[0] = 0.5; d_implicit->ctrl[0] = 0.5;
d_euler->ctrl[0] = 0.5; d_euler->ctrl[0] = 0.5;
@@ -1141,8 +1113,8 @@ TEST_F(DerivativeTest, ForcerangeClampedDerivative) {
m->opt.timestep = dt_large; m->opt.timestep = dt_large;
m->opt.integrator = mjINT_IMPLICITFAST; m->opt.integrator = mjINT_IMPLICITFAST;
mj_resetData(m, d_gt); mj_resetData(m.get(), d_gt.get());
d_gt->ctrl[0] = 0.5; d_gt.get()->ctrl[0] = 0.5;
m->opt.timestep = dt_small; m->opt.timestep = dt_small;
m->opt.integrator = mjINT_EULER; m->opt.integrator = mjINT_EULER;
@@ -1151,20 +1123,20 @@ TEST_F(DerivativeTest, ForcerangeClampedDerivative) {
m->opt.integrator = mjINT_EULER; m->opt.integrator = mjINT_EULER;
m->opt.timestep = dt_small; m->opt.timestep = dt_small;
for (int j = 0; j < substeps; j++) { for (int j = 0; j < substeps; j++) {
mj_step(m, d_gt); mj_step(m.get(), d_gt.get());
} }
// euler at large timestep // euler at large timestep
m->opt.timestep = dt_large; m->opt.timestep = dt_large;
mj_step(m, d_euler); mj_step(m.get(), d_euler.get());
// implicitfast at large timestep // implicitfast at large timestep
m->opt.integrator = mjINT_IMPLICITFAST; m->opt.integrator = mjINT_IMPLICITFAST;
mj_step(m, d_implicit); mj_step(m.get(), d_implicit.get());
// accumulate errors // accumulate errors
mjtNum diff_implicit = d_gt->qpos[0] - d_implicit->qpos[0]; mjtNum diff_implicit = d_gt.get()->qpos[0] - d_implicit->qpos[0];
mjtNum diff_euler = d_gt->qpos[0] - d_euler->qpos[0]; mjtNum diff_euler = d_gt.get()->qpos[0] - d_euler->qpos[0];
error_implicit += diff_implicit * diff_implicit; error_implicit += diff_implicit * diff_implicit;
error_euler += diff_euler * diff_euler; error_euler += diff_euler * diff_euler;
} }
@@ -1173,11 +1145,6 @@ TEST_F(DerivativeTest, ForcerangeClampedDerivative) {
EXPECT_LT(error_implicit, error_euler) EXPECT_LT(error_implicit, error_euler)
<< "implicitfast should be more accurate than Euler at large timestep " << "implicitfast should be more accurate than Euler at large timestep "
<< "when forcerange derivatives are correctly handled"; << "when forcerange derivatives are correctly handled";
mj_deleteData(d_euler);
mj_deleteData(d_implicit);
mj_deleteData(d_gt);
mj_deleteModel(m);
} }
TEST_F(DerivativeTest, NonlinearDampingDerivative) { TEST_F(DerivativeTest, NonlinearDampingDerivative) {
@@ -1197,20 +1164,20 @@ TEST_F(DerivativeTest, NonlinearDampingDerivative) {
)"; )";
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
mjtNum dt_small = 1e-4; mjtNum dt_small = 1e-4;
mjtNum dt_large = 1e-2; mjtNum dt_large = 1e-2;
mjtNum duration = 1.0; mjtNum duration = 1.0;
mjData* d_gt = mj_makeData(m); MjDataPtr d_gt = MakeData(m);
mjData* d_enabled = mj_makeData(m); MjDataPtr d_enabled = MakeData(m);
mjData* d_disabled = mj_makeData(m); MjDataPtr d_disabled = MakeData(m);
mj_resetDataKeyframe(m, d_gt, 0); mj_resetDataKeyframe(m.get(), d_gt.get(), 0);
mj_resetDataKeyframe(m, d_enabled, 0); mj_resetDataKeyframe(m.get(), d_enabled.get(), 0);
mj_resetDataKeyframe(m, d_disabled, 0); mj_resetDataKeyframe(m.get(), d_disabled.get(), 0);
m->opt.integrator = mjINT_EULER; m->opt.integrator = mjINT_EULER;
mjtNum error_enabled = 0; mjtNum error_enabled = 0;
@@ -1222,17 +1189,17 @@ TEST_F(DerivativeTest, NonlinearDampingDerivative) {
m->opt.timestep = dt_small; m->opt.timestep = dt_small;
m->opt.disableflags |= mjDSBL_EULERDAMP; // disable implicit damping m->opt.disableflags |= mjDSBL_EULERDAMP; // disable implicit damping
for (int j = 0; j < substeps; j++) { for (int j = 0; j < substeps; j++) {
mj_step(m, d_gt); mj_step(m.get(), d_gt.get());
} }
m->opt.timestep = dt_large; m->opt.timestep = dt_large;
mj_step(m, d_disabled); mj_step(m.get(), d_disabled.get());
m->opt.disableflags &= ~mjDSBL_EULERDAMP; // enable implicit damping m->opt.disableflags &= ~mjDSBL_EULERDAMP; // enable implicit damping
mj_step(m, d_enabled); mj_step(m.get(), d_enabled.get());
mjtNum diff_enabled = d_gt->qvel[0] - d_enabled->qvel[0]; mjtNum diff_enabled = d_gt.get()->qvel[0] - d_enabled.get()->qvel[0];
mjtNum diff_disabled = d_gt->qvel[0] - d_disabled->qvel[0]; mjtNum diff_disabled = d_gt.get()->qvel[0] - d_disabled.get()->qvel[0];
error_enabled += diff_enabled * diff_enabled; error_enabled += diff_enabled * diff_enabled;
error_disabled += diff_disabled * diff_disabled; error_disabled += diff_disabled * diff_disabled;
} }
@@ -1240,11 +1207,6 @@ TEST_F(DerivativeTest, NonlinearDampingDerivative) {
EXPECT_LT(error_enabled, error_disabled) EXPECT_LT(error_enabled, error_disabled)
<< "Euler with implicit damping should be more accurate than without " << "Euler with implicit damping should be more accurate than without "
<< "when nonlinear damping derivatives are correctly handled"; << "when nonlinear damping derivatives are correctly handled";
mj_deleteData(d_disabled);
mj_deleteData(d_enabled);
mj_deleteData(d_gt);
mj_deleteModel(m);
} }
// implicit derivatives should use next activation when actearly is set // implicit derivatives should use next activation when actearly is set
@@ -1274,9 +1236,9 @@ TEST_F(DerivativeTest, ActearlyDerivative) {
)"; )";
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
// set identical ctrl with zero initial activation // set identical ctrl with zero initial activation
d->ctrl[0] = 1.0; d->ctrl[0] = 1.0;
@@ -1285,7 +1247,7 @@ TEST_F(DerivativeTest, ActearlyDerivative) {
d->act[1] = 0.0; d->act[1] = 0.0;
// step computes derivatives during implicit integration // step computes derivatives during implicit integration
mj_step(m, d); mj_step(m.get(), d.get());
// both should have same act_dot // both should have same act_dot
EXPECT_EQ(d->act_dot[0], d->act_dot[1]); EXPECT_EQ(d->act_dot[0], d->act_dot[1]);
@@ -1305,12 +1267,8 @@ TEST_F(DerivativeTest, ActearlyDerivative) {
<< "actearly=true should use next_act=1"; << "actearly=true should use next_act=1";
EXPECT_NEAR(d->qDeriv[diag1], 0.0, 1e-10) EXPECT_NEAR(d->qDeriv[diag1], 0.0, 1e-10)
<< "actearly=false should use current_act=0"; << "actearly=false should use current_act=0";
mj_deleteData(d);
mj_deleteModel(m);
} }
// verify stateful DC motor derivative matches analytical formula // verify stateful DC motor derivative matches analytical formula
TEST_F(DerivativeTest, DCMotorStatefulDerivative) { TEST_F(DerivativeTest, DCMotorStatefulDerivative) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
@@ -1330,19 +1288,19 @@ TEST_F(DerivativeTest, DCMotorStatefulDerivative) {
)"; )";
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
// set nonzero velocity and ctrl // set nonzero velocity and ctrl
d->qvel[0] = 1.0; d->qvel[0] = 1.0;
d->ctrl[0] = 0.5; d->ctrl[0] = 0.5;
// forward to compute act_dot, etc. // forward to compute act_dot, etc.
mj_forward(m, d); mj_forward(m.get(), d.get());
// compute analytical derivatives // compute analytical derivatives
mjd_smooth_vel(m, d, /* flg_bias = */ 1); mjd_smooth_vel(m.get(), d.get(), /* flg_bias = */ 1);
// extract diagonal of qDeriv // extract diagonal of qDeriv
mjtNum qDeriv_diag = d->qDeriv[m->D_rowadr[0] + m->D_rownnz[0] - 1]; mjtNum qDeriv_diag = d->qDeriv[m->D_rowadr[0] + m->D_rownnz[0] - 1];
@@ -1353,12 +1311,8 @@ TEST_F(DerivativeTest, DCMotorStatefulDerivative) {
mjtNum expected = K * (-kd - K) * (1 - mju_exp(-h / te)) / R; mjtNum expected = K * (-kd - K) * (1 - mju_exp(-h / te)) / R;
EXPECT_NEAR(qDeriv_diag, expected, 1e-10) EXPECT_NEAR(qDeriv_diag, expected, 1e-10)
<< "stateful DC motor derivative should match analytical formula"; << "stateful DC motor derivative should match analytical formula";
mj_deleteData(d);
mj_deleteModel(m);
} }
// verify that stateful DC motor derivative converges to stateless as te -> 0 // verify that stateful DC motor derivative converges to stateless as te -> 0
TEST_F(DerivativeTest, DCMotorStatefulConvergesToStateless) { TEST_F(DerivativeTest, DCMotorStatefulConvergesToStateless) {
// stateless DC motor with position controller // stateless DC motor with position controller
@@ -1396,35 +1350,32 @@ TEST_F(DerivativeTest, DCMotorStatefulConvergesToStateless) {
)"; )";
char error[1024]; char error[1024];
mjModel* m_sl = LoadModelFromString(xml_stateless, error, sizeof(error)); MjModelPtr m_sl = LoadModelFromString(xml_stateless, error, sizeof(error));
ASSERT_THAT(m_sl, NotNull()) << error; ASSERT_THAT(m_sl.get(), NotNull()) << error;
mjData* d_sl = mj_makeData(m_sl); MjDataPtr d_sl = MakeData(m_sl);
mjModel* m_sf = LoadModelFromString(xml_stateful, error, sizeof(error)); MjModelPtr m_sf = LoadModelFromString(xml_stateful, error, sizeof(error));
ASSERT_THAT(m_sf, NotNull()) << error; ASSERT_THAT(m_sf.get(), NotNull()) << error;
mjData* d_sf = mj_makeData(m_sf); MjDataPtr d_sf = MakeData(m_sf);
// set identical state // set identical state
d_sl->qvel[0] = d_sf->qvel[0] = 1.0; d_sl.get()->qvel[0] = d_sf.get()->qvel[0] = 1.0;
d_sl->ctrl[0] = d_sf->ctrl[0] = 0.5; d_sl.get()->ctrl[0] = d_sf.get()->ctrl[0] = 0.5;
// forward and compute derivatives // forward and compute derivatives
mj_forward(m_sl, d_sl); mj_forward(m_sl.get(), d_sl.get());
mj_forward(m_sf, d_sf); mj_forward(m_sf.get(), d_sf.get());
mjd_smooth_vel(m_sl, d_sl, 1); mjd_smooth_vel(m_sl.get(), d_sl.get(), 1);
mjd_smooth_vel(m_sf, d_sf, 1); mjd_smooth_vel(m_sf.get(), d_sf.get(), 1);
// extract diagonals // extract diagonals
mjtNum diag_sl = d_sl->qDeriv[m_sl->D_rowadr[0] + m_sl->D_rownnz[0] - 1]; mjtNum diag_sl =
mjtNum diag_sf = d_sf->qDeriv[m_sf->D_rowadr[0] + m_sf->D_rownnz[0] - 1]; d_sl.get()->qDeriv[m_sl->D_rowadr[0] + m_sl->D_rownnz[0] - 1];
mjtNum diag_sf =
d_sf.get()->qDeriv[m_sf->D_rowadr[0] + m_sf->D_rownnz[0] - 1];
EXPECT_NEAR(diag_sf, diag_sl, 1e-6) EXPECT_NEAR(diag_sf, diag_sl, 1e-6)
<< "stateful derivative should converge to stateless as te -> 0"; << "stateful derivative should converge to stateless as te -> 0";
mj_deleteData(d_sf);
mj_deleteModel(m_sf);
mj_deleteData(d_sl);
mj_deleteModel(m_sl);
} }
// Utility: Rotate flex grid // Utility: Rotate flex grid
@@ -1477,8 +1428,8 @@ void RotateFlexGrid(mjModel* model, mjData* data, const char* flex_name,
// Builds K column-by-column using mjd_flexInterp_mul. // Builds K column-by-column using mjd_flexInterp_mul.
// Result is -(h^2 + h*damping) * J'KJ (negative sign matches the old addH // Result is -(h^2 + h*damping) * J'KJ (negative sign matches the old addH
// convention where stiffness is subtracted from the system matrix). // convention where stiffness is subtracted from the system matrix).
static void mulKD_dense(mjModel* m, mjData* d, mjtNum* H_dense, static void mulKD_dense(mjModel* m, mjData* d, mjtNum* H_dense, int nv,
int nv, mjtNum h) { mjtNum h) {
std::vector<mjtNum> e_i(nv, 0); std::vector<mjtNum> e_i(nv, 0);
std::vector<mjtNum> col(nv, 0); std::vector<mjtNum> col(nv, 0);
for (int i = 0; i < nv; i++) { for (int i = 0; i < nv; i++) {
@@ -1510,18 +1461,18 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(kXml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(kXml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
int nD = model->nD; int nD = model->nD;
int nv = model->nv; int nv = model->nv;
ASSERT_EQ(model->nq, 24); // 8 corners * 3 dofs ASSERT_EQ(model->nq, 24); // 8 corners * 3 dofs
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
// iterate over rotations // iterate over rotations
for (mjtNum angle : {0.0, 0.5, 1.0, mjPI / 2, mjPI, 2.0 * mjPI}) { for (mjtNum angle : {0.0, 0.5, 1.0, mjPI / 2, mjPI, 2.0 * mjPI}) {
RotateFlexGrid(model, data, "flex", angle); RotateFlexGrid(model.get(), data.get(), "flex", angle);
mj_forward(model, data); mj_forward(model.get(), data.get());
// part 1: stiffness verification // part 1: stiffness verification
{ {
@@ -1543,7 +1494,7 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
std::vector<mjtNum> H(nv * nv, 0); std::vector<mjtNum> H(nv * nv, 0);
// assemble K into H column-by-column // assemble K into H column-by-column
mulKD_dense(model, data, H.data(), nv, 1.0); mulKD_dense(model.get(), data.get(), H.data(), nv, 1.0);
// restore damping // restore damping
model->flex_damping[0] = save_damping; model->flex_damping[0] = save_damping;
@@ -1553,13 +1504,13 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
// finite difference of mj_passive for stiffness // finite difference of mj_passive for stiffness
mjtNum eps = MjTol(1e-6, 1e-3); mjtNum eps = MjTol(1e-6, 1e-3);
mjData* data_perturbed = mj_copyData(NULL, model, data); mjData* data_perturbed = mj_copyData(NULL, model.get(), data.get());
// apply perturbation // apply perturbation
mju_addToScl(data_perturbed->qpos, vec.data(), eps, nv); mju_addToScl(data_perturbed->qpos, vec.data(), eps, nv);
// recompute geometry/passive // recompute geometry/passive
mj_forward(model, data_perturbed); mj_forward(model.get(), data_perturbed);
// compute FD estimate of K * vec // compute FD estimate of K * vec
// qfrc_passive = -dV/dq => d(qfrc)/dq = -K // qfrc_passive = -dV/dq => d(qfrc)/dq = -K
@@ -1602,7 +1553,8 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
vKv += v[i] * K_full[i * nv + j] * v[j]; vKv += v[i] * K_full[i * nv + j] * v[j];
} }
} }
EXPECT_GE(vKv, MjTol(-1e-8, -1e-5)) << "K matrix is not PSD at angle " << angle; EXPECT_GE(vKv, MjTol(-1e-8, -1e-5))
<< "K matrix is not PSD at angle " << angle;
} }
} }
@@ -1611,12 +1563,12 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
// set velocity non-zero to test damping // set velocity non-zero to test damping
data->qvel[0] = 1.0; data->qvel[0] = 1.0;
mj_forward(model, data); mj_forward(model.get(), data.get());
// get analytic derivatives (without Flex Damping currently) // get analytic derivatives (without Flex Damping currently)
std::vector<mjtNum> qDerivAnalytic(nD); std::vector<mjtNum> qDerivAnalytic(nD);
mju_zero(data->qDeriv, nD); mju_zero(data->qDeriv, nD);
mjd_passive_vel(model, data); mjd_passive_vel(model.get(), data.get());
mju_copy(qDerivAnalytic.data(), data->qDeriv, nD); mju_copy(qDerivAnalytic.data(), data->qDeriv, nD);
// finite-difference derivatives // finite-difference derivatives
@@ -1624,7 +1576,7 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
mju_zero(data->qDeriv, nD); mju_zero(data->qDeriv, nD);
mjtNum eps = MjTol(1e-6, 1e-3); mjtNum eps = MjTol(1e-6, 1e-3);
mjd_passive_velFD(model, data, eps); mjd_passive_velFD(model.get(), data.get(), eps);
mju_copy(qDerivFD.data(), data->qDeriv, nD); mju_copy(qDerivFD.data(), data->qDeriv, nD);
// check that we have non-zero damping (FD should find it) // check that we have non-zero damping (FD should find it)
@@ -1633,10 +1585,10 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
// compute expected flex damping using mulKD_dense // compute expected flex damping using mulKD_dense
// D = 4*H(0.5) - H(1) // D = 4*H(0.5) - H(1)
vector<mjtNum> H1(nv * nv, 0); vector<mjtNum> H1(nv * nv, 0);
mulKD_dense(model, data, H1.data(), nv, 1.0); mulKD_dense(model.get(), data.get(), H1.data(), nv, 1.0);
vector<mjtNum> H2(nv * nv, 0); vector<mjtNum> H2(nv * nv, 0);
mulKD_dense(model, data, H2.data(), nv, 0.5); mulKD_dense(model.get(), data.get(), H2.data(), nv, 0.5);
vector<mjtNum> D(nv * nv); vector<mjtNum> D(nv * nv);
for (int i = 0; i < nv * nv; i++) { for (int i = 0; i < nv * nv; i++) {
@@ -1660,9 +1612,6 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) {
<< "Damping Mismatch at angle: " << angle; << "Damping Mismatch at angle: " << angle;
} }
} }
mj_deleteData(data);
mj_deleteModel(model);
} }
// Test Jacobian under deformation to highlight approximation error // Test Jacobian under deformation to highlight approximation error
@@ -1681,14 +1630,14 @@ TEST_F(DerivativeTest, FlexInterpDerivativesDeformed) {
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(kXml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(kXml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
int nv = model->nv; int nv = model->nv;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
// Apply rotation // Apply rotation
RotateFlexGrid(model, data, "flex", 1.0); // 1 radian rotation RotateFlexGrid(model.get(), data.get(), "flex", 1.0); // 1 radian rotation
// Apply deformation (stretch along X) // Apply deformation (stretch along X)
// qpos is initialized by RotateFlexGrid. // qpos is initialized by RotateFlexGrid.
@@ -1701,14 +1650,14 @@ TEST_F(DerivativeTest, FlexInterpDerivativesDeformed) {
} }
mju_addTo(data->qpos, deformation.data(), nv); mju_addTo(data->qpos, deformation.data(), nv);
mj_forward(model, data); mj_forward(model.get(), data.get());
// 1. Compute Analytic Jacobian (Approximate) // 1. Compute Analytic Jacobian (Approximate)
// We use mulKD_dense to get K_approx // We use mulKD_dense to get K_approx
std::vector<mjtNum> H_approx(nv * nv, 0); std::vector<mjtNum> H_approx(nv * nv, 0);
// h=1, damping=0 => gives K // h=1, damping=0 => gives K
mulKD_dense(model, data, H_approx.data(), nv, 1.0); mulKD_dense(model.get(), data.get(), H_approx.data(), nv, 1.0);
// 2. Compute Finite Difference Jacobian (Ground Truth) // 2. Compute Finite Difference Jacobian (Ground Truth)
// qfrc_passive = -dV/dq // qfrc_passive = -dV/dq
@@ -1717,9 +1666,9 @@ TEST_F(DerivativeTest, FlexInterpDerivativesDeformed) {
mjtNum eps = 1e-6; mjtNum eps = 1e-6;
for (int i = 0; i < nv; i++) { for (int i = 0; i < nv; i++) {
mjData* data_p = mj_copyData(NULL, model, data); mjData* data_p = mj_copyData(NULL, model.get(), data.get());
data_p->qpos[i] += eps; data_p->qpos[i] += eps;
mj_forward(model, data_p); mj_forward(model.get(), data_p);
for (int j = 0; j < nv; j++) { for (int j = 0; j < nv; j++) {
// d(force_j)/d(q_i) // d(force_j)/d(q_i)
@@ -1742,9 +1691,6 @@ TEST_F(DerivativeTest, FlexInterpDerivativesDeformed) {
// exists. // exists.
EXPECT_GT(max_error, 1e-3) EXPECT_GT(max_error, 1e-3)
<< "Jacobian approximation should differ from FD when deformed"; << "Jacobian approximation should differ from FD when deformed";
mj_deleteData(data);
mj_deleteModel(model);
} }
} // namespace } // namespace
File diff suppressed because it is too large Load Diff
+183 -215
View File
@@ -15,7 +15,6 @@
// Tests for engine/{engine_io.c and engine_memory.c}. // Tests for engine/{engine_io.c and engine_memory.c}.
#include "src/engine/engine_io.h" #include "src/engine/engine_io.h"
#include "src/engine/engine_memory.h"
#include <array> #include <array>
#include <cstdint> #include <cstdint>
@@ -26,11 +25,12 @@
#include <vector> #include <vector>
#include <gmock/gmock.h> #include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <gtest/gtest-spi.h> // IWYU pragma: keep #include <gtest/gtest-spi.h> // IWYU pragma: keep
#include <gtest/gtest.h>
#include <absl/strings/str_format.h> #include <absl/strings/str_format.h>
#include <mujoco/mjxmacro.h> #include <mujoco/mjxmacro.h>
#include <mujoco/mujoco.h> #include <mujoco/mujoco.h>
#include "src/engine/engine_memory.h"
#include "src/engine/engine_thread.h" #include "src/engine/engine_thread.h"
#include "test/fixture.h" #include "test/fixture.h"
@@ -57,19 +57,19 @@ TEST_F(EngineIoTest, VerifySizeModel) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, 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();
std::filesystem::path temp_file = ( std::filesystem::path temp_file =
std::filesystem::temp_directory_path() / "model.mjb"); (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); 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); std::filesystem::remove(temp_file);
mj_deleteModel(model);
EXPECT_EQ(file_size, model_size); EXPECT_EQ(file_size, model_size);
} }
@@ -87,15 +87,13 @@ TEST_F(EngineIoTest, MakeDataLoadsQpos0) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, 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();
model->qpos0[0] = 1; model->qpos0[0] = 1;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull()); ASSERT_THAT(data, NotNull());
EXPECT_EQ(data->qpos[0], 1); EXPECT_EQ(data->qpos[0], 1);
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(EngineIoTest, MakeDataLoadsMocapBodies) { TEST_F(EngineIoTest, MakeDataLoadsMocapBodies) {
@@ -110,43 +108,40 @@ TEST_F(EngineIoTest, MakeDataLoadsMocapBodies) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); ASSERT_THAT(model.get(), NotNull())
mjData* data = mj_makeData(model); << "Failed to load model: " << error.data();
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull()); ASSERT_THAT(data, NotNull());
EXPECT_EQ(data->mocap_pos[0], 42); EXPECT_EQ(data->mocap_pos[0], 42);
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(EngineIoTest, MakeDataReturnsNullOnFailure) { TEST_F(EngineIoTest, MakeDataReturnsNullOnFailure) {
constexpr char xml[] = "<mujoco/>"; constexpr char xml[] = "<mujoco/>";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, 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();
// fail mj_makeData intentionally with a bad size // fail mj_makeData intentionally with a bad size
model->nbody = -1; model->nbody = -1;
MockWarningHandler warning_handler; MockWarningHandler warning_handler;
warning_handler.ExpectWarnings(); warning_handler.ExpectWarnings();
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
EXPECT_THAT(data, IsNull()); EXPECT_THAT(data, IsNull());
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(EngineIoTest, ResetVariableSizes) { TEST_F(EngineIoTest, ResetVariableSizes) {
constexpr char xml[] = "<mujoco/>"; constexpr char xml[] = "<mujoco/>";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, 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();
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
ASSERT_THAT(model, NotNull()) << "Failed to create mjData"; ASSERT_THAT(model.get(), NotNull()) << "Failed to create mjData";
// don't call mj_forward, vars should be reset // don't call mj_forward, vars should be reset
EXPECT_EQ(data->ne, 0); EXPECT_EQ(data->ne, 0);
@@ -155,18 +150,16 @@ TEST_F(EngineIoTest, ResetVariableSizes) {
EXPECT_EQ(data->nJ, 0); EXPECT_EQ(data->nJ, 0);
EXPECT_EQ(data->nA, 0); EXPECT_EQ(data->nA, 0);
EXPECT_EQ(data->ncon, 0); EXPECT_EQ(data->ncon, 0);
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(EngineIoTest, MakeDataResetsAllArenaPointerSizes) { TEST_F(EngineIoTest, MakeDataResetsAllArenaPointerSizes) {
constexpr char xml[] = "<mujoco />"; constexpr char xml[] = "<mujoco />";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); ASSERT_THAT(model.get(), NotNull())
mjData* data = mj_makeData(model); << "Failed to load model: " << error.data();
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull()); ASSERT_THAT(data, NotNull());
// without calling mj_forward, all array sizes should be zero. // without calling mj_forward, all array sizes should be zero.
@@ -185,9 +178,6 @@ TEST_F(EngineIoTest, MakeDataResetsAllArenaPointerSizes) {
#define MJ_D(n) n #define MJ_D(n) n
#define MJ_M(n) n #define MJ_M(n) n
#undef X #undef X
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(EngineIoTest, MjvCopyModel) { TEST_F(EngineIoTest, MjvCopyModel) {
@@ -202,28 +192,27 @@ TEST_F(EngineIoTest, MjvCopyModel) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model1 = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model1 = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model1, NotNull()) << 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; ASSERT_THAT(model2, NotNull()) << error;
model1->mesh_vert[0] = 0.1; model1->mesh_vert[0] = 0.1;
model1->geom_rgba[0] = 0.2; 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->mesh_vert[0], 0.1);
EXPECT_FLOAT_EQ(model2->geom_rgba[0], 0.2); EXPECT_FLOAT_EQ(model2->geom_rgba[0], 0.2);
model1->mesh_vert[0] = 0.3; model1->mesh_vert[0] = 0.3;
model1->geom_rgba[0] = 0.4; 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->mesh_vert[0], 0.1); // unchanged
EXPECT_FLOAT_EQ(model2->geom_rgba[0], 0.4); EXPECT_FLOAT_EQ(model2->geom_rgba[0], 0.4);
mj_deleteModel(model2); mj_deleteModel(model2);
mj_deleteModel(model1);
} }
TEST_F(EngineIoTest, MjvCopyData) { TEST_F(EngineIoTest, MjvCopyData) {
@@ -239,23 +228,21 @@ TEST_F(EngineIoTest, MjvCopyData) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data1 = mj_makeData(model); MjDataPtr data1 = MakeData(model);
mj_forward(model, data1); mj_forward(model.get(), data1.get());
EXPECT_THAT(data1->efc_J, NotNull()); 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()); EXPECT_THAT(data2->efc_J, NotNull());
mj_deleteData(data2); mj_deleteData(data2);
data2 = mjv_copyData(nullptr, model, data1); data2 = mjv_copyData(nullptr, model.get(), data1.get());
EXPECT_THAT(data2->efc_J, IsNull()); EXPECT_THAT(data2->efc_J, IsNull());
mj_deleteData(data2); mj_deleteData(data2);
mj_deleteData(data1);
mj_deleteModel(model);
} }
using ValidateReferencesTest = MujocoTest; using ValidateReferencesTest = MujocoTest;
@@ -273,15 +260,14 @@ TEST_F(ValidateReferencesTest, BodyReferences) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, 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()); EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
model->jnt_bodyid[0] = 2; model->jnt_bodyid[0] = 2;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("jnt_bodyid")); EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("jnt_bodyid"));
mj_deleteModel(model);
} }
TEST_F(ValidateReferencesTest, AddressRange) { TEST_F(ValidateReferencesTest, AddressRange) {
@@ -298,20 +284,19 @@ TEST_F(ValidateReferencesTest, AddressRange) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, 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()); EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
model->body_jntnum[1] = 3; 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; model->body_jntnum[1] = 2;
// Could be more strict and test for -1, but at the moment the code is a bit // Could be more strict and test for -1, but at the moment the code is a bit
// lenient. // lenient.
model->body_jntadr[1] = -2; model->body_jntadr[1] = -2;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("body_jntadr")); EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("body_jntadr"));
mj_deleteModel(model);
} }
TEST_F(ValidateReferencesTest, AddressRangeNegativeNum) { TEST_F(ValidateReferencesTest, AddressRangeNegativeNum) {
@@ -328,16 +313,16 @@ TEST_F(ValidateReferencesTest, AddressRangeNegativeNum) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, 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()); EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
// jntadr + jntnum is within safe range, but jntnum is negative. // jntadr + jntnum is within safe range, but jntnum is negative.
model->body_jntadr[1] += 5; model->body_jntadr[1] += 5;
model->body_jntnum[1] -= 5; model->body_jntnum[1] -= 5;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("body_jntnum")); EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("body_jntnum"));
mj_deleteModel(model);
} }
TEST_F(ValidateReferencesTest, GeomCondim) { TEST_F(ValidateReferencesTest, GeomCondim) {
@@ -353,16 +338,15 @@ TEST_F(ValidateReferencesTest, GeomCondim) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, 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()); EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
model->geom_condim[0] = 7; 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; model->geom_condim[0] = -1;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("geom_condim")); EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("geom_condim"));
mj_deleteModel(model);
} }
TEST_F(ValidateReferencesTest, HField) { TEST_F(ValidateReferencesTest, HField) {
@@ -375,18 +359,17 @@ TEST_F(ValidateReferencesTest, HField) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, 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()); EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
model->hfield_adr[0] = -2; 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_adr[0] = 0;
model->hfield_ncol[0] = 4; model->hfield_ncol[0] = 4;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("hfield_adr")); EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("hfield_adr"));
mj_deleteModel(model);
} }
TEST_F(ValidateReferencesTest, Texture) { TEST_F(ValidateReferencesTest, Texture) {
@@ -399,18 +382,17 @@ TEST_F(ValidateReferencesTest, Texture) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, 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()); EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
model->tex_adr[0] = -2; 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_adr[0] = 0;
model->tex_height[0] = 4; model->tex_height[0] = 4;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("tex_adr")); EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("tex_adr"));
mj_deleteModel(model);
} }
TEST_F(ValidateReferencesTest, GeomPairs) { TEST_F(ValidateReferencesTest, GeomPairs) {
@@ -433,19 +415,18 @@ TEST_F(ValidateReferencesTest, GeomPairs) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, 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()); EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
// Invalid geomid=4 // Invalid geomid=4
model->pair_signature[0] = (1 << 16) | 5; 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; model->pair_signature[0] = (5 << 16) | 1;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("pair_body2")); EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("pair_body2"));
mj_deleteModel(model);
} }
TEST_F(ValidateReferencesTest, SensorsAddress) { TEST_F(ValidateReferencesTest, SensorsAddress) {
@@ -482,12 +463,12 @@ TEST_F(ValidateReferencesTest, SensorsAddress) {
for (const std::string& sensor_string : sensor_strings) { for (const std::string& sensor_string : sensor_strings) {
std::string xml = absl::StrFormat(xml_template, sensor_string); std::string xml = absl::StrFormat(xml_template, sensor_string);
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = MjModelPtr model =
LoadModelFromString(xml.c_str(), error.data(), error.size()); 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()); EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
mj_deleteModel(model);
} }
} }
@@ -503,11 +484,11 @@ TEST_F(ValidateReferencesTest, SensorsAddressUser) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, 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()); EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
mj_deleteModel(model);
} }
TEST_F(ValidateReferencesTest, SensorsObj) { TEST_F(ValidateReferencesTest, SensorsObj) {
@@ -527,25 +508,24 @@ TEST_F(ValidateReferencesTest, SensorsObj) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, 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();
model->sensor_objtype[0] = -1; 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_objtype[0] = mjOBJ_SITE;
model->sensor_objid[0] = model->nsite; 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_objid[0] = 0;
model->sensor_reftype[0] = -1; 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_reftype[0] = mjOBJ_BODY;
model->sensor_refid[0] = model->nbody; model->sensor_refid[0] = model->nbody;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("sensor_refid")); EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("sensor_refid"));
mj_deleteModel(model);
} }
TEST_F(ValidateReferencesTest, MoreBodiesThanGeoms) { TEST_F(ValidateReferencesTest, MoreBodiesThanGeoms) {
@@ -565,10 +545,10 @@ TEST_F(ValidateReferencesTest, MoreBodiesThanGeoms) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); ASSERT_THAT(model.get(), NotNull())
EXPECT_THAT(mj_validateReferences(model), IsNull()); << "Failed to load model: " << error.data();
mj_deleteModel(model); EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
} }
TEST_F(ValidateReferencesTest, BodyExcludes) { TEST_F(ValidateReferencesTest, BodyExcludes) {
@@ -585,18 +565,17 @@ TEST_F(ValidateReferencesTest, BodyExcludes) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, 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()); EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
// Invalid bodyid=3 // Invalid bodyid=3
model->exclude_signature[0] = (1 << 16) | 4; 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; model->exclude_signature[0] = (4 << 16) | 2;
EXPECT_THAT(mj_validateReferences(model), HasSubstr("exclude_body2")); EXPECT_THAT(mj_validateReferences(model.get()), HasSubstr("exclude_body2"));
mj_deleteModel(model);
} }
TEST_F(ValidateReferencesTest, EqualityConstraints) { TEST_F(ValidateReferencesTest, EqualityConstraints) {
@@ -634,38 +613,37 @@ TEST_F(ValidateReferencesTest, EqualityConstraints) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, 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()); EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
// connect constraint // connect constraint
model->eq_obj1id[0] = -1; 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; 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_obj1id[0] = 1;
model->eq_obj2id[0] = -2; 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; 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; model->eq_obj2id[0] = 0;
// weld constraint // weld constraint
model->eq_obj1id[1] = -1; 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; 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_obj1id[1] = 1;
model->eq_obj2id[1] = -2; 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; 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; model->eq_obj2id[1] = model->nbody - 1;
mj_deleteModel(model);
} }
TEST_F(ValidateReferencesTest, Tuples) { TEST_F(ValidateReferencesTest, Tuples) {
@@ -692,19 +670,19 @@ TEST_F(ValidateReferencesTest, Tuples) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, 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()); EXPECT_THAT(mj_validateReferences(model.get()), IsNull());
model->tuple_objtype[0] = -1; 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_objtype[0] = mjOBJ_BODY;
model->tuple_objid[0] = model->nbody; 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; model->tuple_objid[0] = 1;
mj_deleteModel(model);
} }
TEST_F(EngineIoTest, CanMarkAndFreeStack) { TEST_F(EngineIoTest, CanMarkAndFreeStack) {
@@ -716,20 +694,18 @@ TEST_F(EngineIoTest, CanMarkAndFreeStack) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, 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();
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull()); ASSERT_THAT(data, NotNull());
auto pstack_before = data->pstack; auto pstack_before = data->pstack;
mj_markStack(data); mj_markStack(data.get());
EXPECT_GT(data->pstack, pstack_before); EXPECT_GT(data->pstack, pstack_before);
mj_freeStack(data); mj_freeStack(data.get());
EXPECT_EQ(data->pstack, pstack_before); EXPECT_EQ(data->pstack, pstack_before);
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(EngineIoTest, LargeMemory) { TEST_F(EngineIoTest, LargeMemory) {
@@ -740,27 +716,25 @@ TEST_F(EngineIoTest, LargeMemory) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); ASSERT_THAT(model.get(), NotNull())
mjData* data = mj_makeData(model); << "Failed to load model: " << error.data();
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull()); ASSERT_THAT(data, NotNull());
// allocate 2.3G of mjtNums // allocate 2.3G of mjtNums
mj_markStack(data); mj_markStack(data.get());
size_t num = 2300000000 / sizeof(mjtNum); size_t num = 2300000000 / sizeof(mjtNum);
mjtNum* testNum = mj_stackAllocNum(data, num); mjtNum* testNum = mj_stackAllocNum(data.get(), num);
testNum[num - 1] = 1; testNum[num - 1] = 1;
mj_freeStack(data); mj_freeStack(data.get());
// allocate 2.3G of bytes // allocate 2.3G of bytes
mj_markStack(data); mj_markStack(data.get());
num = 2300000000; num = 2300000000;
char* testByte = (char*) mj_stackAllocByte(data, num, alignof(char)); char* testByte = (char*)mj_stackAllocByte(data.get(), num, alignof(char));
testByte[num - 1] = 1; testByte[num - 1] = 1;
mj_freeStack(data); mj_freeStack(data.get());
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(EngineIoTest, VeryLargeMemory) { TEST_F(EngineIoTest, VeryLargeMemory) {
@@ -771,31 +745,29 @@ TEST_F(EngineIoTest, VeryLargeMemory) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
if (!model) { if (!model.get()) {
// in some test environments, 8GB is too large // in some test environments, 8GB is too large
EXPECT_THAT(error.data(), HasSubstr("Could not allocate memory")); EXPECT_THAT(error.data(), HasSubstr("Could not allocate memory"));
} else { } else {
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); ASSERT_THAT(model.get(), NotNull())
mjData* data = mj_makeData(model); << "Failed to load model: " << error.data();
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull()); ASSERT_THAT(data, NotNull());
// allocate 7G of mjtNums // allocate 7G of mjtNums
mj_markStack(data); mj_markStack(data.get());
size_t num = 7516192768ull / sizeof(mjtNum); size_t num = 7516192768ull / sizeof(mjtNum);
mjtNum* testNum = mj_stackAllocNum(data, num); mjtNum* testNum = mj_stackAllocNum(data.get(), num);
testNum[num - 1] = 1; testNum[num - 1] = 1;
mj_freeStack(data); mj_freeStack(data.get());
// allocate 7G of bytes // allocate 7G of bytes
mj_markStack(data); mj_markStack(data.get());
num = 7516192768ull; num = 7516192768ull;
char* testByte = (char*) mj_stackAllocByte(data, num, alignof(char)); char* testByte = (char*)mj_stackAllocByte(data.get(), num, alignof(char));
testByte[num - 1] = 1; testByte[num - 1] = 1;
mj_freeStack(data); mj_freeStack(data.get());
mj_deleteData(data);
mj_deleteModel(model);
} }
} }
@@ -822,23 +794,22 @@ TEST_F(EngineIoTest, TestStackShardingForThreads) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, 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();
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull()); ASSERT_THAT(data, NotNull());
mju_threadpool(data, 10); mju_threadpool(data.get(), 10);
constexpr int kTasks = 1000; constexpr int kTasks = 1000;
TestFunctionArgs test_function_args; 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) { for (int i = 0; i < kTasks; ++i) {
EXPECT_EQ(i, test_function_args.stack_output[i]); EXPECT_EQ(i, test_function_args.stack_output[i]);
} }
mj_deleteData(data);
mj_deleteModel(model);
} }
#ifdef ADDRESS_SANITIZER #ifdef ADDRESS_SANITIZER
@@ -858,37 +829,36 @@ TEST_F(EngineIoTest, CanDetectStackFrameLeakage) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, 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();
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull()); ASSERT_THAT(data, NotNull());
// MarkFreeStack correctly calls mj_freeStack, should not error. // 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 // MarkFreeStack calls mj_markStack without mj_freeStack, the next call to
// mj_freeStack should detect the stack frame leakage. // mj_freeStack should detect the stack frame leakage.
mj_markStack(data); mj_markStack(data.get());
MarkFreeStack(data, /* free= */ false); MarkFreeStack(data.get(), /* free= */ false);
EXPECT_THAT( EXPECT_THAT(
MjuErrorMessageFrom(mj_freeStack)(data), MjuErrorMessageFrom(mj_freeStack)(data.get()),
ContainsRegex( ContainsRegex(
"mj_markStack in MarkFreeStack at .*engine_io_test\\.cc.* has no " "mj_markStack in MarkFreeStack at .*engine_io_test\\.cc.* has no "
"corresponding mj_freeStack")); "corresponding mj_freeStack"));
// Dangling stack frames should be detected in mj_deleteData. // Dangling stack frames should be detected in mj_deleteData.
mj_resetData(model, data); mj_resetData(model.get(), data.get());
mj_markStack(data); mj_markStack(data.get());
EXPECT_THAT( EXPECT_THAT(
MjuErrorMessageFrom(mj_deleteData)(data), MjuErrorMessageFrom(mj_deleteData)(data.get()),
ContainsRegex( ContainsRegex(
"mj_markStack in .+EngineIoTest_CanDetectStackFrameLeakage.+ has no " "mj_markStack in .+EngineIoTest_CanDetectStackFrameLeakage.+ has no "
"corresponding mj_freeStack")); "corresponding mj_freeStack"));
mj_resetData(model, data); mj_resetData(model.get(), data.get());
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(EngineIoTest, RedZoneAlignmentTest) { TEST_F(EngineIoTest, RedZoneAlignmentTest) {
@@ -900,19 +870,17 @@ TEST_F(EngineIoTest, RedZoneAlignmentTest) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, 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();
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull()); ASSERT_THAT(data, NotNull());
mj_markStack(data); mj_markStack(data.get());
mj_stackAllocByte(data, 1, 1); mj_stackAllocByte(data.get(), 1, 1);
mj_stackAllocByte(data, 1, 1); mj_stackAllocByte(data.get(), 1, 1);
mj_freeStack(data); mj_freeStack(data.get());
mj_deleteData(data);
mj_deleteModel(model);
} }
#endif #endif
@@ -930,15 +898,15 @@ TEST_F(EngineIoTest, LoadModelBufferRejectsOverflowingSizes) {
constexpr char xml[] = "<mujoco />"; constexpr char xml[] = "<mujoco />";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, 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();
// save model to a buffer // save model to a buffer
int bufsize = mj_sizeModel(model); int bufsize = mj_sizeModel(model.get());
ASSERT_GT(bufsize, 0); ASSERT_GT(bufsize, 0);
std::vector<char> buffer(bufsize); std::vector<char> buffer(bufsize);
mj_saveModel(model, nullptr, buffer.data(), bufsize); mj_saveModel(model.get(), nullptr, buffer.data(), bufsize);
mj_deleteModel(model);
// locate the size fields in the buffer (after 5-int header) // locate the size fields in the buffer (after 5-int header)
const int header_bytes = 5 * sizeof(int); const int header_bytes = 5 * sizeof(int);
+16 -46
View File
@@ -14,6 +14,8 @@
// Tests for engine/engine_island.c. // Tests for engine/engine_island.c.
#include "src/engine/engine_island.h"
#include <string> #include <string>
#include <vector> #include <vector>
@@ -21,7 +23,6 @@
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <mujoco/mjmodel.h> #include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h> #include <mujoco/mujoco.h>
#include "src/engine/engine_island.h"
#include "src/engine/engine_util_sparse.h" #include "src/engine/engine_util_sparse.h"
#include "test/fixture.h" #include "test/fixture.h"
@@ -34,16 +35,11 @@ using ::testing::NotNull;
using ::testing::Pointwise; using ::testing::Pointwise;
using IslandTest = MujocoTest; using IslandTest = MujocoTest;
TEST_F(IslandTest, FloodFillSingleton) { TEST_F(IslandTest, FloodFillSingleton) {
// adjacency matrix for the graph 0 1 2 // adjacency matrix for the graph 0 1 2
// U U // U U
// (3 singletons, 0 and 2 have self-edges) // (3 singletons, 0 and 2 have self-edges)
mjtNum mat[9] = { mjtNum mat[9] = {1, 0, 0, 0, 0, 0, 0, 0, 1};
1, 0, 0,
0, 0, 0,
0, 0, 1
};
constexpr int nr = 3; constexpr int nr = 3;
constexpr int nnz = 2; constexpr int nnz = 2;
int rownnz[nr]; int rownnz[nr];
@@ -63,14 +59,9 @@ TEST_F(IslandTest, FloodFillSingleton) {
EXPECT_THAT(island, ElementsAre(0, -1, 1)); EXPECT_THAT(island, ElementsAre(0, -1, 1));
} }
TEST_F(IslandTest, FloodFill1) { TEST_F(IslandTest, FloodFill1) {
// adjacency matrix for the graph 0 - 1 - 2 // adjacency matrix for the graph 0 - 1 - 2
mjtNum mat[9] = { mjtNum mat[9] = {0, 1, 0, 1, 0, 1, 0, 1, 0};
0, 1, 0,
1, 0, 1,
0, 1, 0
};
constexpr int nr = 3; constexpr int nr = 3;
constexpr int nnz = 4; constexpr int nnz = 4;
int rownnz[nr]; int rownnz[nr];
@@ -89,17 +80,11 @@ TEST_F(IslandTest, FloodFill1) {
EXPECT_THAT(island, ElementsAre(0, 0, 0)); EXPECT_THAT(island, ElementsAre(0, 0, 0));
} }
TEST_F(IslandTest, FloodFill2) { TEST_F(IslandTest, FloodFill2) {
// adjacency matrix for the graph 6 – 1 – 4 0 – 3 – 5 – 2 // adjacency matrix for the graph 6 – 1 – 4 0 – 3 – 5 – 2
mjtNum mat[49] = { mjtNum mat[49] = {
0, 0, 0, 1, 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, 0, 0, 0, 1, 0, 1, 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, 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 nr = 7;
constexpr int nnz = 10; constexpr int nnz = 10;
@@ -119,15 +104,11 @@ TEST_F(IslandTest, FloodFill2) {
EXPECT_THAT(island, ElementsAre(0, 1, 0, 0, 1, 0, 1)); EXPECT_THAT(island, ElementsAre(0, 1, 0, 0, 1, 0, 1));
} }
TEST_F(IslandTest, FloodFill3a) { TEST_F(IslandTest, FloodFill3a) {
// adjacency matrix for the graph 0 2 1 – 3 // adjacency matrix for the graph 0 2 1 – 3
// U // U
mjtNum mat[16] = { mjtNum mat[16] = {
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0,
0, 0, 0, 1,
0, 0, 1, 0,
0, 1, 0, 0,
}; };
constexpr int nr = 4; constexpr int nr = 4;
constexpr int nnz = 3; constexpr int nnz = 3;
@@ -147,7 +128,6 @@ TEST_F(IslandTest, FloodFill3a) {
EXPECT_THAT(island, ElementsAre(-1, 0, 1, 0)); EXPECT_THAT(island, ElementsAre(-1, 0, 1, 0));
} }
TEST_F(IslandTest, FloodFill3b) { TEST_F(IslandTest, FloodFill3b) {
/* /*
adjacency matrix for the graph 1 – 2 3 4 – 5 adjacency matrix for the graph 1 – 2 3 4 – 5
@@ -155,13 +135,8 @@ TEST_F(IslandTest, FloodFill3b) {
0 – 6 0 – 6
*/ */
mjtNum mat[49] = { mjtNum mat[49] = {
0, 0, 0, 0, 1, 0, 1, 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, 1, 1, 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, 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 nr = 7;
constexpr int nnz = 13; constexpr int nnz = 13;
@@ -181,8 +156,7 @@ TEST_F(IslandTest, FloodFill3b) {
EXPECT_THAT(island, ElementsAre(0, 1, 1, -1, 0, 0, 0)); EXPECT_THAT(island, ElementsAre(0, 1, 1, -1, 0, 0, 0));
} }
static const char* const kAbacusPath = static const char* const kAbacusPath = "engine/testdata/island/abacus.xml";
"engine/testdata/island/abacus.xml";
TEST_F(IslandTest, Abacus) { TEST_F(IslandTest, Abacus) {
const std::string xml_path = GetTestDataFilePath(kAbacusPath); const std::string xml_path = GetTestDataFilePath(kAbacusPath);
@@ -327,8 +301,7 @@ TEST_F(IslandTest, DenseSparse) {
AsVector(data2->island_idofadr, nisland)); AsVector(data2->island_idofadr, nisland));
EXPECT_EQ(AsVector(data1->island_nv, nisland), EXPECT_EQ(AsVector(data1->island_nv, nisland),
AsVector(data2->island_nv, nisland)); AsVector(data2->island_nv, nisland));
EXPECT_EQ(AsVector(data1->dof_island, nv), EXPECT_EQ(AsVector(data1->dof_island, nv), AsVector(data2->dof_island, nv));
AsVector(data2->dof_island, nv));
EXPECT_EQ(AsVector(data1->map_idof2dof, nv), EXPECT_EQ(AsVector(data1->map_idof2dof, nv),
AsVector(data2->map_idof2dof, nv)); AsVector(data2->map_idof2dof, nv));
EXPECT_EQ(AsVector(data1->map_dof2idof, nv), EXPECT_EQ(AsVector(data1->map_dof2idof, nv),
@@ -462,12 +435,10 @@ TEST_F(IslandTest, EqualityConstraintOfTendons) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
mj_forward(model, data); mj_forward(model.get(), data.get());
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(IslandTest, PGSIsland) { TEST_F(IslandTest, PGSIsland) {
@@ -496,8 +467,7 @@ TEST_F(IslandTest, PGSIsland) {
// solve without islands // solve without islands
m->opt.disableflags |= mjDSBL_ISLAND; m->opt.disableflags |= mjDSBL_ISLAND;
mj_forward(m, d); mj_forward(m, d);
std::vector<mjtNum> qfrc_mono(d->qfrc_constraint, std::vector<mjtNum> qfrc_mono(d->qfrc_constraint, d->qfrc_constraint + m->nv);
d->qfrc_constraint + m->nv);
// expect close match (inexact due to randomized constraint visitation order) // expect close match (inexact due to randomized constraint visitation order)
EXPECT_THAT(qfrc_island, Pointwise(MjNear(1e-3, 1e-3), qfrc_mono)); EXPECT_THAT(qfrc_island, Pointwise(MjNear(1e-3, 1e-3), qfrc_mono));
+130 -194
View File
@@ -49,28 +49,25 @@ TEST_F(PassiveTest, DisableFlags) {
)"; )";
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(flex_xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(flex_xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error; ASSERT_THAT(m.get(), testing::NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m, d, 0); mj_resetDataKeyframe(m.get(), d.get(), 0);
mj_forward(m, d); mj_forward(m.get(), d.get());
EXPECT_MJTNUM_EQ(d->qacc[0], 11); EXPECT_MJTNUM_EQ(d->qacc[0], 11);
m->opt.disableflags = mjDSBL_DAMPER; m->opt.disableflags = mjDSBL_DAMPER;
mj_forward(m, d); mj_forward(m.get(), d.get());
EXPECT_MJTNUM_EQ(d->qacc[0], 10); EXPECT_MJTNUM_EQ(d->qacc[0], 10);
m->opt.disableflags = mjDSBL_SPRING; m->opt.disableflags = mjDSBL_SPRING;
mj_forward(m, d); mj_forward(m.get(), d.get());
EXPECT_MJTNUM_EQ(d->qacc[0], 1); EXPECT_MJTNUM_EQ(d->qacc[0], 1);
m->opt.disableflags = mjDSBL_SPRING | mjDSBL_DAMPER; m->opt.disableflags = mjDSBL_SPRING | mjDSBL_DAMPER;
mj_forward(m, d); mj_forward(m.get(), d.get());
EXPECT_EQ(d->qacc[0], -10); EXPECT_EQ(d->qacc[0], -10);
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(PassiveTest, GravcompNestedBody) { TEST_F(PassiveTest, GravcompNestedBody) {
@@ -89,17 +86,14 @@ TEST_F(PassiveTest, GravcompNestedBody) {
)"; )";
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_forward(m, d); mj_forward(m.get(), d.get());
EXPECT_GT(d->qacc[2], 0); EXPECT_GT(d->qacc[2], 0);
EXPECT_NEAR(d->qacc[2], 2.0, 0.1); EXPECT_NEAR(d->qacc[2], 2.0, 0.1);
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(PassiveTest, PolyStiffnessSlide) { TEST_F(PassiveTest, PolyStiffnessSlide) {
@@ -119,16 +113,13 @@ TEST_F(PassiveTest, PolyStiffnessSlide) {
)"; )";
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m, d, 0); 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); EXPECT_MJTNUM_EQ(d->qfrc_spring[0], -48);
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(PassiveTest, PolyStiffnessAntiSymmetric) { TEST_F(PassiveTest, PolyStiffnessAntiSymmetric) {
@@ -148,16 +139,13 @@ TEST_F(PassiveTest, PolyStiffnessAntiSymmetric) {
)"; )";
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m, d, 0); 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); EXPECT_MJTNUM_EQ(d->qfrc_spring[0], 8);
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(PassiveTest, PolyStiffnessTendon) { TEST_F(PassiveTest, PolyStiffnessTendon) {
@@ -183,21 +171,18 @@ TEST_F(PassiveTest, PolyStiffnessTendon) {
)"; )";
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
m->tendon_stiffness[0] = 10; m->tendon_stiffness[0] = 10;
m->tendon_stiffnesspoly[0] = 5; m->tendon_stiffnesspoly[0] = 5;
m->tendon_stiffnesspoly[1] = 1; m->tendon_stiffnesspoly[1] = 1;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m, d, 0); 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); EXPECT_MJTNUM_EQ(d->qfrc_spring[0], -48);
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(PassiveTest, PolyStiffnessEnergy) { TEST_F(PassiveTest, PolyStiffnessEnergy) {
@@ -221,21 +206,18 @@ TEST_F(PassiveTest, PolyStiffnessEnergy) {
)"; )";
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m, d, 0); 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]; mjtNum total_energy = d->energy[0] + d->energy[1];
for (int i = 0; i < 100; i++) { 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); EXPECT_NEAR(d->energy[0] + d->energy[1], total_energy, 0.002);
} }
mj_deleteData(d);
mj_deleteModel(m);
} }
// ------------------------ ellipsoid fluid model ------------------------------ // ------------------------ ellipsoid fluid model ------------------------------
@@ -261,9 +243,9 @@ TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) {
)"; )";
char error[1024]; char error[1024];
mjModel* m2 = LoadModelFromString(two_bodies_xml, error, sizeof(error)); MjModelPtr m2 = LoadModelFromString(two_bodies_xml, error, sizeof(error));
ASSERT_THAT(m2, NotNull()) << error; ASSERT_THAT(m2.get(), NotNull()) << error;
mjData* d2 = mj_makeData(m2); MjDataPtr d2 = MakeData(m2);
for (int i = 0; i < 6; i++) { for (int i = 0; i < 6; i++) {
d2->qvel[i] = (mjtNum)i + 1; d2->qvel[i] = (mjtNum)i + 1;
} }
@@ -285,9 +267,9 @@ TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) {
</mujoco> </mujoco>
)"; )";
mjModel* m1 = LoadModelFromString(one_body_xml, error, sizeof(error)); MjModelPtr m1 = LoadModelFromString(one_body_xml, error, sizeof(error));
ASSERT_THAT(m1, NotNull()) << error; ASSERT_THAT(m1.get(), NotNull()) << error;
mjData* d1 = mj_makeData(m1); MjDataPtr d1 = MakeData(m1);
for (int i = 0; i < 6; i++) { for (int i = 0; i < 6; i++) {
d1->qvel[i] = (mjtNum)i + 1; d1->qvel[i] = (mjtNum)i + 1;
} }
@@ -301,16 +283,11 @@ TEST_F(EllipsoidFluidTest, GeomsEquivalentToBodies) {
EXPECT_EQ(m1->nv, m2->nv); EXPECT_EQ(m1->nv, m2->nv);
mj_forward(m2, d2); mj_forward(m2.get(), d2.get());
mj_forward(m1, d1); mj_forward(m1.get(), d1.get());
for (int i = 0; i < m1->nv; i++) { for (int i = 0; i < m1->nv; i++) {
EXPECT_NEAR(d2->qfrc_passive[i], d1->qfrc_passive[i], tol); 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) { TEST_F(EllipsoidFluidTest, DefaultsPropagate) {
@@ -334,16 +311,13 @@ TEST_F(EllipsoidFluidTest, DefaultsPropagate) {
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_THAT(AsVector(model->geom_fluid, 6), EXPECT_THAT(AsVector(model->geom_fluid, 6), ElementsAre(0, 0, 0, 0, 0, 0));
ElementsAre(0, 0, 0, 0, 0, 0));
EXPECT_THAT(AsVector(model->geom_fluid + mjNFLUID, 6), EXPECT_THAT(AsVector(model->geom_fluid + mjNFLUID, 6),
ElementsAre(1, 2, 3, 4, 5, 6)); ElementsAre(1, 2, 3, 4, 5, 6));
mj_deleteModel(model);
} }
// ------------------------------ tendons -------------------------------------- // ------------------------------ tendons --------------------------------------
using TendonTest = MujocoTest; using TendonTest = MujocoTest;
@@ -391,12 +365,10 @@ TEST_F(ElasticityTest, FlexCompatibility) {
)"; )";
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(flex_xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(flex_xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error; ASSERT_THAT(m.get(), testing::NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_deleteData(d);
mj_deleteModel(m);
} }
// -------------------------------- shell ----------------------------------- // -------------------------------- shell -----------------------------------
@@ -413,11 +385,11 @@ TEST_F(ElasticityTest, ElasticEnergyShell) {
)"; )";
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error; ASSERT_THAT(m.get(), testing::NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_kinematics(m, d); mj_kinematics(m.get(), d.get());
mj_flex(m, d); mj_flex(m.get(), d.get());
// check that a plane is in the kernel of the energy // check that a plane is in the kernel of the energy
for (mjtNum scale = 1; scale < 4; scale++) { for (mjtNum scale = 1; scale < 4; scale++) {
@@ -435,17 +407,15 @@ TEST_F(ElasticityTest, ElasticEnergyShell) {
for (int x = 0; x < 3; x++) { for (int x = 0; x < 3; x++) {
mjtNum elongation1 = scale * d->flexvert_xpos[3 * v[i] + x]; mjtNum elongation1 = scale * d->flexvert_xpos[3 * v[i] + x];
mjtNum elongation2 = scale * d->flexvert_xpos[3 * v[j] + x]; mjtNum elongation2 = scale * d->flexvert_xpos[3 * v[j] + x];
energy += m->flex_bending[17*e+4*i+j] * elongation1 * elongation2; energy +=
m->flex_bending[17 * e + 4 * i + j] * elongation1 * elongation2;
} }
} }
} }
EXPECT_NEAR( EXPECT_NEAR(4 * energy / volume, 0,
4*energy/volume, 0, std::numeric_limits<float>::epsilon()); std::numeric_limits<float>::epsilon());
} }
} }
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(ElasticityTest, CurvedShell) { TEST_F(ElasticityTest, CurvedShell) {
@@ -487,12 +457,12 @@ TEST_F(ElasticityTest, CurvedShell) {
)"; )";
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error; ASSERT_THAT(m.get(), testing::NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_kinematics(m, d); mj_kinematics(m.get(), d.get());
mj_flex(m, d); mj_flex(m.get(), d.get());
mj_passive(m, d); mj_passive(m.get(), d.get());
// v1 force component is in-plane along v1-v0 edge (y-axis) // v1 force component is in-plane along v1-v0 edge (y-axis)
EXPECT_NEAR(d->qfrc_spring[3], 0, 1e-6); 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) // 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[9], 0, 1e-6);
EXPECT_NEAR(d->qfrc_spring[10], 0, 1e-6); EXPECT_NEAR(d->qfrc_spring[10], 0, 1e-6);
mj_deleteData(d);
mj_deleteModel(m);
} }
// -------------------------------- membrane ----------------------------------- // -------------------------------- membrane -----------------------------------
@@ -525,12 +492,12 @@ TEST_F(ElasticityTest, ElasticEnergyMembrane) {
)"; )";
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error; ASSERT_THAT(m.get(), testing::NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_kinematics(m, d); mj_kinematics(m.get(), d.get());
mj_flex(m, d); mj_flex(m.get(), d.get());
mjtNum* metric = m->flex_stiffness + 21 * m->flex_elemadr[0]; mjtNum* metric = m->flex_stiffness + 21 * m->flex_elemadr[0];
// check that if the entire geometry is rescaled by a factor "scale", then // check that if the entire geometry is rescaled by a factor "scale", then
@@ -549,16 +516,14 @@ TEST_F(ElasticityTest, ElasticEnergyMembrane) {
scale * m->flexedge_length0[idx1] * m->flexedge_length0[idx1]; scale * m->flexedge_length0[idx1] * m->flexedge_length0[idx1];
mjtNum elong2 = mjtNum elong2 =
scale * m->flexedge_length0[idx2] * m->flexedge_length0[idx2]; 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); 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 ----------------------------------- // -------------------------------- solid -----------------------------------
@@ -576,12 +541,12 @@ TEST_F(ElasticityTest, ElasticEnergySolid) {
)"; )";
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error; ASSERT_THAT(m.get(), testing::NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_kinematics(m, d); mj_kinematics(m.get(), d.get());
mj_flex(m, d); mj_flex(m.get(), d.get());
mjtNum* metric = m->flex_stiffness + 21 * m->flex_elemadr[0]; mjtNum* metric = m->flex_stiffness + 21 * m->flex_elemadr[0];
// check that if the entire geometry is rescaled by a factor "scale", then // check that if the entire geometry is rescaled by a factor "scale", then
@@ -600,16 +565,14 @@ TEST_F(ElasticityTest, ElasticEnergySolid) {
scale * m->flexedge_length0[idx1] * m->flexedge_length0[idx1]; scale * m->flexedge_length0[idx1] * m->flexedge_length0[idx1];
mjtNum elong2 = mjtNum elong2 =
scale * m->flexedge_length0[idx2] * m->flexedge_length0[idx2]; 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); 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) { TEST_F(PassiveTest, PolynomialStiffnessJoint) {
@@ -626,19 +589,16 @@ TEST_F(PassiveTest, PolynomialStiffnessJoint) {
</keyframe> </keyframe>
</mujoco> </mujoco>
)"; )";
mjModel* m = LoadModelFromString(xml); MjModelPtr m = LoadModelFromString(xml);
ASSERT_THAT(m, NotNull()); ASSERT_THAT(m.get(), NotNull());
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m, d, 0); mj_resetDataKeyframe(m.get(), d.get(), 0);
mj_forward(m, d); mj_forward(m.get(), d.get());
mjtNum x = 0.5; mjtNum x = 0.5;
mjtNum a = 2, b = 3, c = 4; mjtNum a = 2, b = 3, c = 4;
mjtNum expected = -(a + b * x + c * x * x) * x; mjtNum expected = -(a + b * x + c * x * x) * x;
EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12); EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12);
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(PassiveTest, PolynomialStiffnessNegativeDisplacement) { TEST_F(PassiveTest, PolynomialStiffnessNegativeDisplacement) {
@@ -655,19 +615,16 @@ TEST_F(PassiveTest, PolynomialStiffnessNegativeDisplacement) {
</keyframe> </keyframe>
</mujoco> </mujoco>
)"; )";
mjModel* m = LoadModelFromString(xml); MjModelPtr m = LoadModelFromString(xml);
ASSERT_THAT(m, NotNull()); ASSERT_THAT(m.get(), NotNull());
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m, d, 0); mj_resetDataKeyframe(m.get(), d.get(), 0);
mj_forward(m, d); mj_forward(m.get(), d.get());
mjtNum x = -0.5; mjtNum x = -0.5;
mjtNum a = 2, b = 3, c = 4; mjtNum a = 2, b = 3, c = 4;
mjtNum expected = -(a + b * x + c * x * x) * x; mjtNum expected = -(a + b * x + c * x * x) * x;
EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12); EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12);
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(PassiveTest, PolyStiffnessFixedTendon) { TEST_F(PassiveTest, PolyStiffnessFixedTendon) {
@@ -693,19 +650,16 @@ TEST_F(PassiveTest, PolyStiffnessFixedTendon) {
)"; )";
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m, d, 0); 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 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); EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12);
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(PassiveTest, PolyStiffnessSpatialTendon) { TEST_F(PassiveTest, PolyStiffnessSpatialTendon) {
@@ -734,23 +688,18 @@ TEST_F(PassiveTest, PolyStiffnessSpatialTendon) {
)"; )";
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m, d, 0); 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 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); EXPECT_NEAR(d->qfrc_spring[0], expected, 1e-12);
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(PassiveTest, PolynomialDampingJoint) { TEST_F(PassiveTest, PolynomialDampingJoint) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
@@ -765,19 +714,16 @@ TEST_F(PassiveTest, PolynomialDampingJoint) {
</keyframe> </keyframe>
</mujoco> </mujoco>
)"; )";
mjModel* m = LoadModelFromString(xml); MjModelPtr m = LoadModelFromString(xml);
ASSERT_THAT(m, NotNull()); ASSERT_THAT(m.get(), NotNull());
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m, d, 0); mj_resetDataKeyframe(m.get(), d.get(), 0);
mj_forward(m, d); mj_forward(m.get(), d.get());
mjtNum v = 0.5; mjtNum v = 0.5;
mjtNum a = 2, b = 3, c = 4; mjtNum a = 2, b = 3, c = 4;
mjtNum expected = -(a * v + b * v * mju_abs(v) + c * v * v * v); mjtNum expected = -(a * v + b * v * mju_abs(v) + c * v * v * v);
EXPECT_NEAR(d->qfrc_damper[0], expected, 1e-12); EXPECT_NEAR(d->qfrc_damper[0], expected, 1e-12);
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(PassiveTest, PolynomialDampingNegativeVelocity) { TEST_F(PassiveTest, PolynomialDampingNegativeVelocity) {
@@ -794,19 +740,16 @@ TEST_F(PassiveTest, PolynomialDampingNegativeVelocity) {
</keyframe> </keyframe>
</mujoco> </mujoco>
)"; )";
mjModel* m = LoadModelFromString(xml); MjModelPtr m = LoadModelFromString(xml);
ASSERT_THAT(m, NotNull()); ASSERT_THAT(m.get(), NotNull());
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m, d, 0); mj_resetDataKeyframe(m.get(), d.get(), 0);
mj_forward(m, d); mj_forward(m.get(), d.get());
mjtNum v = -0.5; mjtNum v = -0.5;
mjtNum a = 2, b = 3, c = 4; mjtNum a = 2, b = 3, c = 4;
mjtNum expected = -(a * v + b * v * mju_abs(v) + c * v * v * v); mjtNum expected = -(a * v + b * v * mju_abs(v) + c * v * v * v);
EXPECT_NEAR(d->qfrc_damper[0], expected, 1e-12); EXPECT_NEAR(d->qfrc_damper[0], expected, 1e-12);
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(PassiveTest, PolynomialDampingTendon) { TEST_F(PassiveTest, PolynomialDampingTendon) {
@@ -832,19 +775,16 @@ TEST_F(PassiveTest, PolynomialDampingTendon) {
)"; )";
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_resetDataKeyframe(m, d, 0); mj_resetDataKeyframe(m.get(), d.get(), 0);
mj_forward(m, d); mj_forward(m.get(), d.get());
mjtNum v = d->ten_velocity[0]; 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); 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 // shell-mode (elastic2d=stretch) flexcomp must have zero passive spring forces
@@ -867,20 +807,17 @@ TEST_F(ElasticityTest, ShellModeZeroForceAtRest) {
)"; )";
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error; ASSERT_THAT(m.get(), testing::NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_forward(m, d); mj_forward(m.get(), d.get());
// all spring forces should be zero at rest // all spring forces should be zero at rest
for (int i = 0; i < m->nv; i++) { for (int i = 0; i < m->nv; i++) {
EXPECT_NEAR(d->qfrc_spring[i], 0, 1e-10) EXPECT_NEAR(d->qfrc_spring[i], 0, 1e-10)
<< "nonzero spring force at DOF " << i; << "nonzero spring force at DOF " << i;
} }
mj_deleteData(d);
mj_deleteModel(m);
} }
// interpolated shell bending must produce zero spring forces at rest // interpolated shell bending must produce zero spring forces at rest
@@ -901,16 +838,16 @@ TEST_F(ElasticityTest, InterpBendingZeroForceAtRest) {
)"; )";
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error; ASSERT_THAT(m.get(), testing::NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
// verify bending data was compiled // verify bending data was compiled
const mjtNum* bdata = m->flex_bending + m->flex_bendingadr[0]; const mjtNum* bdata = m->flex_bending + m->flex_bendingadr[0];
int nedge = (int)bdata[0]; int nedge = (int)bdata[0];
EXPECT_GT(nedge, 0) << "no bending edges compiled"; 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 // all spring forces should be zero at rest
for (int i = 0; i < m->nv; i++) { 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_flat, 0) << "no intra-surface edges found";
EXPECT_GT(n_corner, 0) << "no corner edges found"; EXPECT_GT(n_corner, 0) << "no corner edges found";
EXPECT_EQ(n_flat + n_corner, nedge); EXPECT_EQ(n_flat + n_corner, nedge);
mj_deleteData(d);
mj_deleteModel(m);
} }
// interpolated shell bending must produce zero forces after a rigid rotation // interpolated shell bending must produce zero forces after a rigid rotation
@@ -967,9 +901,9 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) {
)"; )";
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error; ASSERT_THAT(m.get(), testing::NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
// compute geometric center from body positions (skip world body) // compute geometric center from body positions (skip world body)
mjtNum center[3] = {0, 0, 0}; mjtNum center[3] = {0, 0, 0};
@@ -981,7 +915,9 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) {
nnodes++; nnodes++;
} }
ASSERT_GT(nnodes, 0); 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) // rotation: 45 degrees about (1,1,1)/sqrt(3)
mjtNum angle = 45 * 3.14159265358979 / 180.0; mjtNum angle = 45 * 3.14159265358979 / 180.0;
@@ -1007,7 +943,10 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) {
int qadr = m->jnt_qposadr[jid]; int qadr = m->jnt_qposadr[jid];
int axis = -1; int axis = -1;
for (int a = 0; a < 3; a++) { 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) { if (axis >= 0) {
d->qpos[qadr] = 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 // spring forces should still be zero after rigid rotation
const mjtNum tol = MjTol(1e-6, 1e-3); const mjtNum tol = MjTol(1e-6, 1e-3);
@@ -1024,9 +963,6 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) {
EXPECT_NEAR(d->qfrc_spring[i], 0, tol) EXPECT_NEAR(d->qfrc_spring[i], 0, tol)
<< "nonzero spring force at DOF " << i << " after rigid rotation"; << "nonzero spring force at DOF " << i << " after rigid rotation";
} }
mj_deleteData(d);
mj_deleteModel(m);
} }
} // namespace } // namespace
+46 -56
View File
@@ -68,13 +68,9 @@ class BaseTestPlugin {
advance_count = 0; advance_count = 0;
} }
void Compute() { void Compute() { compute_count += stride; }
compute_count += stride;
}
void Advance() { void Advance() { advance_count += stride; }
advance_count += stride;
}
protected: protected:
int stride; int stride;
@@ -458,11 +454,12 @@ TEST_F(MujocoTest, EmptyPluginDisallowed) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, testing::IsNull()) << error.data(); EXPECT_THAT(m.get(), testing::IsNull()) << error.data();
EXPECT_THAT(error.data(), HasSubstr( EXPECT_THAT(
error.data(),
HasSubstr(
"neither 'plugin' nor 'instance' is specified for body 'world'")); "neither 'plugin' nor 'instance' is specified for body 'world'"));
mj_deleteModel(m);
} }
TEST_F(MujocoTest, FirstPartyPlugins) { TEST_F(MujocoTest, FirstPartyPlugins) {
@@ -480,13 +477,13 @@ TEST_F(EnginePluginTest, NoAttributePlugin) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, testing::NotNull()) << error.data(); EXPECT_THAT(m.get(), testing::NotNull()) << error.data();
mj_deleteModel(m);
} }
TEST_F(EnginePluginTest, MultiplePluginTableBlocks) { TEST_F(EnginePluginTest, MultiplePluginTableBlocks) {
EXPECT_EQ(mjp_pluginCount(), kNumTruePlugins + kNumFakePlugins + kNumTestPlugins); EXPECT_EQ(mjp_pluginCount(),
kNumTruePlugins + kNumFakePlugins + kNumTestPlugins);
const mjpPlugin* last_plugin = nullptr; const mjpPlugin* last_plugin = nullptr;
int table_count = 0; int table_count = 0;
@@ -513,16 +510,17 @@ TEST_F(EnginePluginTest, RegisterIdenticalPlugin) {
EXPECT_EQ(RegisterSensorPlugin(), kNumTruePlugins); EXPECT_EQ(RegisterSensorPlugin(), kNumTruePlugins);
EXPECT_EQ(RegisterActuatorPlugin(), kNumTruePlugins + kNumFakePlugins + 1); EXPECT_EQ(RegisterActuatorPlugin(), kNumTruePlugins + kNumFakePlugins + 1);
EXPECT_EQ(RegisterPassivePlugin(), kNumTruePlugins + kNumFakePlugins + 2); EXPECT_EQ(RegisterPassivePlugin(), kNumTruePlugins + kNumFakePlugins + 2);
EXPECT_EQ(mjp_pluginCount(), kNumTruePlugins + kNumFakePlugins + kNumTestPlugins); EXPECT_EQ(mjp_pluginCount(),
kNumTruePlugins + kNumFakePlugins + kNumTestPlugins);
} }
TEST_F(EnginePluginTest, SaveXml) { TEST_F(EnginePluginTest, SaveXml) {
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << 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); std::string_view expected_xml(xml);
const std::string extension_open = "<extension>"; 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_sensor_section));
EXPECT_THAT(saved_xml, HasSubstr(expected_actuator_section)); EXPECT_THAT(saved_xml, HasSubstr(expected_actuator_section));
mj_deleteModel(m);
// make sure that the saved XML can still be compiled // make sure that the saved XML can still be compiled
mjModel* m2 = LoadModelFromString(saved_xml, error, sizeof(error)); MjModelPtr m2 = LoadModelFromString(saved_xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error; ASSERT_THAT(m.get(), testing::NotNull()) << error;
mj_deleteModel(m2);
} }
TEST_F(EnginePluginTest, SensorPlugin) { TEST_F(EnginePluginTest, SensorPlugin) {
@@ -575,8 +570,8 @@ TEST_F(EnginePluginTest, SensorPlugin) {
EXPECT_EQ(expected_init_count, expected_destroy_count); EXPECT_EQ(expected_init_count, expected_destroy_count);
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error; ASSERT_THAT(m.get(), testing::NotNull()) << error;
// mj_makeModel calls mj_makeData and mj_deleteData internally // mj_makeModel calls mj_makeData and mj_deleteData internally
expected_init_count += 3; expected_init_count += 3;
@@ -585,10 +580,10 @@ TEST_F(EnginePluginTest, SensorPlugin) {
EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count); EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count);
EXPECT_EQ(m->nplugin, 7); EXPECT_EQ(m->nplugin, 7);
EXPECT_EQ(mj_name2id(m, mjOBJ_PLUGIN, "twosensors"), 0); EXPECT_EQ(mj_name2id(m.get(), mjOBJ_PLUGIN, "twosensors"), 0);
EXPECT_EQ(mj_name2id(m, mjOBJ_PLUGIN, "threesensors"), 1); EXPECT_EQ(mj_name2id(m.get(), mjOBJ_PLUGIN, "threesensors"), 1);
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
expected_init_count += 3; expected_init_count += 3;
EXPECT_EQ(TestSensor::InitCount(), expected_init_count); EXPECT_EQ(TestSensor::InitCount(), expected_init_count);
EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count); EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count);
@@ -605,24 +600,22 @@ TEST_F(EnginePluginTest, SensorPlugin) {
m->plugin_stateadr[5]), 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), EXPECT_THAT(*reinterpret_cast<mjtNum(*)[18]>(d->sensordata),
testing::ElementsAreArray<int>({ i+1, 2*j, j, testing::ElementsAreArray<int>(
5*(i+1), 10*j, 5*j, {i + 1, 2 * j, j, 5 * (i + 1), 10 * j, 5 * j, 3 * (i + 1),
3*(i+1), 6*j, 3*j, 6 * j, 3 * j, i + 1, 2 * j, 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})); 3 * (i + 1), 6 * j, 3 * j}));
mj_step(m, d); mj_step(m.get(), d.get());
mj_forward(m, d); 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; expected_destroy_count += 3;
EXPECT_EQ(TestSensor::InitCount(), expected_init_count); EXPECT_EQ(TestSensor::InitCount(), expected_init_count);
EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count); EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count);
mj_deleteModel(m);
EXPECT_EQ(TestSensor::InitCount(), expected_init_count); EXPECT_EQ(TestSensor::InitCount(), expected_init_count);
EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count); EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count);
} }
@@ -633,8 +626,8 @@ TEST_F(EnginePluginTest, ActuatorPlugin) {
EXPECT_EQ(expected_init_count, expected_destroy_count); EXPECT_EQ(expected_init_count, expected_destroy_count);
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error; ASSERT_THAT(m.get(), testing::NotNull()) << error;
// mj_makeModel calls mj_makeData and mj_deleteData internally // mj_makeModel calls mj_makeData and mj_deleteData internally
expected_init_count += 2; expected_init_count += 2;
@@ -643,9 +636,9 @@ TEST_F(EnginePluginTest, ActuatorPlugin) {
EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count); EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count);
EXPECT_EQ(m->nplugin, 7); 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; expected_init_count += 2;
EXPECT_EQ(TestActuator::InitCount(), expected_init_count); EXPECT_EQ(TestActuator::InitCount(), expected_init_count);
EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count); EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count);
@@ -661,26 +654,26 @@ TEST_F(EnginePluginTest, ActuatorPlugin) {
EXPECT_THAT(*reinterpret_cast<mjtNum(*)[2]>(d->actuator_force), EXPECT_THAT(*reinterpret_cast<mjtNum(*)[2]>(d->actuator_force),
testing::ElementsAreArray<mjtNum>( testing::ElementsAreArray<mjtNum>(
{(mjtNum)0.125 * j, (mjtNum)0.25 * j})); {(mjtNum)0.125 * j, (mjtNum)0.25 * j}));
mj_step(m, d); mj_step(m.get(), d.get());
mj_forward(m, d); 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; expected_destroy_count += 2;
EXPECT_EQ(TestActuator::InitCount(), expected_init_count); EXPECT_EQ(TestActuator::InitCount(), expected_init_count);
EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count); EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count);
mj_deleteModel(m);
EXPECT_EQ(TestActuator::InitCount(), expected_init_count); EXPECT_EQ(TestActuator::InitCount(), expected_init_count);
EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count); EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count);
} }
TEST_F(EnginePluginTest, FilteredActuatorPlugin) { TEST_F(EnginePluginTest, FilteredActuatorPlugin) {
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error; ASSERT_THAT(m.get(), testing::NotNull()) << error;
// Expecting 7 actuator state variables: 3x2 from actuator2 instances, and 1 // Expecting 7 actuator state variables: 3x2 from actuator2 instances, and 1
// from setting dyntype="filter" on one of the plugin actuators // 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[1], 0);
EXPECT_EQ(m->actuator_actadr[2], 3); EXPECT_EQ(m->actuator_actadr[2], 3);
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
EXPECT_EQ(d->act[0], 0.0); EXPECT_EQ(d->act[0], 0.0);
mju_fill(d->ctrl, 1, m->nu); mju_fill(d->ctrl, 1, m->nu);
// start with nonzero act for the filter // start with nonzero act for the filter
d->act[6] = 0.5; d->act[6] = 0.5;
mj_step(m, d); mj_step(m.get(), d.get());
for (int i = 0; i < 6; ++i) { for (int i = 0; i < 6; ++i) {
// act_dot should be computed by the plugin // 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_dot[6], MjNear(expected_act_dot, 1e-6, 1e-4));
EXPECT_THAT(d->act[6], EXPECT_THAT(d->act[6],
MjNear(0.5 + expected_act_dot * m->opt.timestep, 1e-6, 1e-4)); MjNear(0.5 + expected_act_dot * m->opt.timestep, 1e-6, 1e-4));
mj_deleteData(d);
mj_deleteModel(m);
} }
} // namespace } // namespace
+7 -9
View File
@@ -38,7 +38,6 @@ using ::testing::NotNull;
using EnginePrintTest = MujocoTest; using EnginePrintTest = MujocoTest;
constexpr const char* NullFile() { constexpr const char* NullFile() {
#ifdef _WIN32 #ifdef _WIN32
return "NUL"; return "NUL";
@@ -60,27 +59,26 @@ TEST_F(EnginePrintTest, PrintDataWorksWithMsan) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); ASSERT_THAT(model.get(), NotNull())
mjData* data = mj_makeData(model); << "Failed to load model: " << error.data();
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull()); ASSERT_THAT(data, NotNull());
// Mark qacc_smooth[0] as initialized. // Mark qacc_smooth[0] as initialized.
data->qacc_smooth[0] = 0; data->qacc_smooth[0] = 0;
// This will read "uninitialized" values from mjData, but shouldn't fail. // 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 // After mj_printData, poisoned status should be restored correctly, so
// qacc_smooth[0] should be marked initialzed. // qacc_smooth[0] should be marked initialzed.
EXPECT_EQ(data->qacc_smooth[0], 0); EXPECT_EQ(data->qacc_smooth[0], 0);
#ifdef MEMORY_SANITIZER #ifdef MEMORY_SANITIZER
EXPECT_THAT(__msan_test_shadow(data->buffer, data->nbuffer), Not(Eq(-1))) << EXPECT_THAT(__msan_test_shadow(data->buffer, data->nbuffer), Not(Eq(-1)))
"Expecting some of data->buffer to be marked uninitialized"; << "Expecting some of data->buffer to be marked uninitialized";
#endif #endif
mj_deleteData(data);
mj_deleteModel(model);
} }
} // namespace } // namespace
+95 -105
View File
@@ -14,6 +14,8 @@
// Tests for ray casting. // Tests for ray casting.
#include "src/engine/engine_ray.h"
#include <cstring> #include <cstring>
#include <string> #include <string>
@@ -23,7 +25,6 @@
#include <mujoco/mjmodel.h> #include <mujoco/mjmodel.h>
#include <mujoco/mjtype.h> #include <mujoco/mjtype.h>
#include <mujoco/mujoco.h> #include <mujoco/mujoco.h>
#include "src/engine/engine_ray.h"
#include "test/fixture.h" #include "test/fixture.h"
namespace mujoco { namespace mujoco {
@@ -85,9 +86,10 @@ using RayTest = MujocoTest;
TEST_F(RayTest, NoExclusions) { TEST_F(RayTest, NoExclusions) {
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(kRayCastingModel, error, sizeof(error)); MjModelPtr model =
ASSERT_THAT(model, NotNull()) << error; LoadModelFromString(kRayCastingModel, error, sizeof(error));
mjData* data = mj_makeData(model); ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull()); ASSERT_THAT(data, NotNull());
mjtNum pnt[] = {0.0, 0.0, 0.0}; mjtNum pnt[] = {0.0, 0.0, 0.0};
@@ -97,20 +99,19 @@ TEST_F(RayTest, NoExclusions) {
int bodyexclude = -1; int bodyexclude = -1;
int geomid = -1; int geomid = -1;
mj_kinematics(model, data); mj_kinematics(model.get(), data.get());
mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, mjtNum distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup,
bodyexclude, &geomid, nullptr); flg_static, bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "static_group1"); EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "static_group1");
EXPECT_MJTNUM_EQ(distance, 0.9); EXPECT_MJTNUM_EQ(distance, 0.9);
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(RayTest, Exclusions) { TEST_F(RayTest, Exclusions) {
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(kRayCastingModel, error, sizeof(error)); MjModelPtr model =
ASSERT_THAT(model, NotNull()) << error; LoadModelFromString(kRayCastingModel, error, sizeof(error));
mjData* data = mj_makeData(model); ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull()); ASSERT_THAT(data, NotNull());
mjtNum pnt[] = {0.0, 0.0, 0.0}; mjtNum pnt[] = {0.0, 0.0, 0.0};
@@ -120,40 +121,38 @@ TEST_F(RayTest, Exclusions) {
int bodyexclude = -1; int bodyexclude = -1;
int geomid = -1; int geomid = -1;
mj_kinematics(model, data); mj_kinematics(model.get(), data.get());
mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, mjtNum distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup,
bodyexclude, &geomid, nullptr); flg_static, bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "static_group1"); EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "static_group1");
EXPECT_NEAR(distance, 0.9, MjTol(1e-12, 1e-5)); EXPECT_NEAR(distance, 0.9, MjTol(1e-12, 1e-5));
// Exclude nearest geom // Exclude nearest geom
geomgroup[1] = 0; geomgroup[1] = 0;
distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude, distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup, flg_static,
&geomid, nullptr); bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group0"); EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "group0");
EXPECT_NEAR(distance, 2.9, MjTol(1e-12, 1e-5)); EXPECT_NEAR(distance, 2.9, MjTol(1e-12, 1e-5));
geomgroup[0] = 0; geomgroup[0] = 0;
distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude, distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup, flg_static,
&geomid, nullptr); bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group2"); EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "group2");
EXPECT_NEAR(distance, 4.9, MjTol(1e-12, 1e-5)); EXPECT_NEAR(distance, 4.9, MjTol(1e-12, 1e-5));
geomgroup[2] = 0; geomgroup[2] = 0;
distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, bodyexclude, distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup, flg_static,
&geomid, nullptr); bodyexclude, &geomid, nullptr);
EXPECT_EQ(geomid, -1); EXPECT_EQ(geomid, -1);
EXPECT_NEAR(distance, -1, MjTol(1e-12, 1e-5)); EXPECT_NEAR(distance, -1, MjTol(1e-12, 1e-5));
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(RayTest, ExcludeStatic) { TEST_F(RayTest, ExcludeStatic) {
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(kRayCastingModel, error, sizeof(error)); MjModelPtr model =
ASSERT_THAT(model, NotNull()) << error; LoadModelFromString(kRayCastingModel, error, sizeof(error));
mjData* data = mj_makeData(model); ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
ASSERT_THAT(data, NotNull()); ASSERT_THAT(data, NotNull());
mjtNum pnt[] = {0.0, 0.0, 0.0}; mjtNum pnt[] = {0.0, 0.0, 0.0};
@@ -163,24 +162,22 @@ TEST_F(RayTest, ExcludeStatic) {
int bodyexclude = -1; int bodyexclude = -1;
int geomid = -1; int geomid = -1;
mj_kinematics(model, data); mj_kinematics(model.get(), data.get());
mjtNum distance = mj_ray(model, data, pnt, vec, geomgroup, flg_static, mjtNum distance = mj_ray(model.get(), data.get(), pnt, vec, geomgroup,
bodyexclude, &geomid, nullptr); flg_static, bodyexclude, &geomid, nullptr);
EXPECT_STREQ(mj_id2name(model, mjOBJ_GEOM, geomid), "group0"); EXPECT_STREQ(mj_id2name(model.get(), mjOBJ_GEOM, geomid), "group0");
EXPECT_NEAR(distance, 2.9, MjTol(1e-12, 1e-5)); EXPECT_NEAR(distance, 2.9, MjTol(1e-12, 1e-5));
mj_deleteData(data);
mj_deleteModel(model);
} }
// ------------------------------- mj_multiRay -------------------------------- // ------------------------------- mj_multiRay --------------------------------
TEST_F(RayTest, MultiRayEqualsSingleRay) { TEST_F(RayTest, MultiRayEqualsSingleRay) {
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(kRayCastingModel, error, sizeof(error)); MjModelPtr m = LoadModelFromString(kRayCastingModel, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull()); ASSERT_THAT(d, NotNull());
mj_forward(m, d); mj_forward(m.get(), d.get());
// create ray array // create ray array
constexpr int N = 80; constexpr int N = 80;
@@ -204,8 +201,8 @@ TEST_F(RayTest, MultiRayEqualsSingleRay) {
// compute intersections with multiray functions // compute intersections with multiray functions
mjtNum dist_multiray[N * M]; mjtNum dist_multiray[N * M];
int rgeomid_multiray[N * M]; int rgeomid_multiray[N * M];
mj_multiRay(m, d, pnt, vec, NULL, 1, -1, rgeomid_multiray, dist_multiray, mj_multiRay(m.get(), d.get(), pnt, vec, NULL, 1, -1, rgeomid_multiray,
nullptr, N * M, mjMAXVAL); dist_multiray, nullptr, N * M, mjMAXVAL);
// compare results with single ray function // compare results with single ray function
mjtNum dist; mjtNum dist;
@@ -214,25 +211,23 @@ TEST_F(RayTest, MultiRayEqualsSingleRay) {
for (int i = 0; i < N; ++i) { for (int i = 0; i < N; ++i) {
for (int j = 0; j < M; ++j) { for (int j = 0; j < M; ++j) {
int idx = i * 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_MJTNUM_EQ(dist, dist_multiray[idx]);
EXPECT_EQ(rgeomid, rgeomid_multiray[idx]); EXPECT_EQ(rgeomid, rgeomid_multiray[idx]);
nhits += dist >= 0; nhits += dist >= 0;
} }
} }
EXPECT_GT(nhits, 10); EXPECT_GT(nhits, 10);
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) { TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) {
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(kRayCastingModel, error, sizeof(error)); MjModelPtr m = LoadModelFromString(kRayCastingModel, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull()); ASSERT_THAT(d, NotNull());
mj_forward(m, d); mj_forward(m.get(), d.get());
// create ray array // create ray array
constexpr int N = 80; constexpr int N = 80;
@@ -257,7 +252,7 @@ TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) {
mjtNum dist_multiray[N * M]; mjtNum dist_multiray[N * M];
int rgeomid_multiray[N * M]; int rgeomid_multiray[N * M];
mjtNum normal_multiray[3 * N * M]; mjtNum normal_multiray[3 * N * M];
mj_multiRay(m, d, pnt, vec, NULL, 1, -1, rgeomid_multiray, mj_multiRay(m.get(), d.get(), pnt, vec, NULL, 1, -1, rgeomid_multiray,
dist_multiray, normal_multiray, N * M, mjMAXVAL); dist_multiray, normal_multiray, N * M, mjMAXVAL);
// compare results with single ray normal function // compare results with single ray normal function
@@ -268,7 +263,7 @@ TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) {
for (int i = 0; i < N; ++i) { for (int i = 0; i < N; ++i) {
for (int j = 0; j < M; ++j) { for (int j = 0; j < M; ++j) {
int idx = i * 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); normal);
EXPECT_MJTNUM_EQ(dist, dist_multiray[idx]); EXPECT_MJTNUM_EQ(dist, dist_multiray[idx]);
EXPECT_EQ(rgeomid, rgeomid_multiray[idx]); EXPECT_EQ(rgeomid, rgeomid_multiray[idx]);
@@ -279,19 +274,16 @@ TEST_F(RayTest, MultiRayNormalEqualsSingleRayNormal) {
} }
} }
EXPECT_GT(nhits, 10); EXPECT_GT(nhits, 10);
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(RayTest, EdgeCases) { TEST_F(RayTest, EdgeCases) {
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(kSingleGeomModel, error, sizeof(error)); MjModelPtr m = LoadModelFromString(kSingleGeomModel, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
ASSERT_THAT(m->nbvh, 1); ASSERT_THAT(m->nbvh, 1);
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull()); ASSERT_THAT(d, NotNull());
mj_forward(m, d); mj_forward(m.get(), d.get());
// spherical bounding box and result arrays // spherical bounding box and result arrays
mjtNum geom_ba[4]; mjtNum geom_ba[4];
@@ -301,51 +293,57 @@ TEST_F(RayTest, EdgeCases) {
// pnt contained in bounding box // pnt contained in bounding box
mjtNum pnt1[] = {-1, 0, 0}; 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[0], -mjPI);
EXPECT_MJTNUM_EQ(geom_ba[1], 0); EXPECT_MJTNUM_EQ(geom_ba[1], 0);
EXPECT_MJTNUM_EQ(geom_ba[2], mjPI); EXPECT_MJTNUM_EQ(geom_ba[2], mjPI);
EXPECT_MJTNUM_EQ(geom_ba[3], mjPI); EXPECT_MJTNUM_EQ(geom_ba[3], mjPI);
mjtNum vec1[] = {1, 0, 0}; mjtNum vec1[] = {1, 0, 0};
mj_multiRay(m, d, pnt1, vec1, NULL, 1, -1, &rgeomid, &dist, nullptr, 1, mj_multiRay(m.get(), d.get(), pnt1, vec1, NULL, 1, -1, &rgeomid, &dist,
mjMAXVAL); nullptr, 1, mjMAXVAL);
EXPECT_FLOAT_EQ(dist, 0.1); EXPECT_FLOAT_EQ(dist, 0.1);
// pnt at phi = Pi, -Pi // pnt at phi = Pi, -Pi
mjtNum pnt2[] = {-.5, 0, 0}; 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[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}; mjtNum vec2[] = {-1, 0, 0};
mj_multiRay(m, d, pnt2, vec2, NULL, 1, -1, &rgeomid, &dist, nullptr, 1, mj_multiRay(m.get(), d.get(), pnt2, vec2, NULL, 1, -1, &rgeomid, &dist,
mjMAXVAL); nullptr, 1, mjMAXVAL);
EXPECT_FLOAT_EQ(dist, 0.4); EXPECT_FLOAT_EQ(dist, 0.4);
// with cutoff // with cutoff
mjtNum cutoff1 = 0.41, cutoff2 = 0.39; 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); 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); EXPECT_EQ(flags[0], 1);
mj_multiRay(m, d, pnt2, vec2, NULL, 1, -1, &rgeomid, &dist, nullptr, 1, mj_multiRay(m.get(), d.get(), pnt2, vec2, NULL, 1, -1, &rgeomid, &dist,
cutoff2); nullptr, 1, cutoff2);
EXPECT_FLOAT_EQ(dist, -1); EXPECT_FLOAT_EQ(dist, -1);
// pnt on the boundary of the box // pnt on the boundary of the box
mjtNum pnt3[] = {.1, .1, .05}; 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[1], 0);
EXPECT_FLOAT_EQ(geom_ba[3], mjPI); EXPECT_FLOAT_EQ(geom_ba[3], mjPI);
mjtNum vec3[] = {1, 1, 0}; mjtNum vec3[] = {1, 1, 0};
mj_multiRay(m, d, pnt3, vec3, NULL, 1, -1, &rgeomid, &dist, nullptr, 1, mj_multiRay(m.get(), d.get(), pnt3, vec3, NULL, 1, -1, &rgeomid, &dist,
mjMAXVAL); nullptr, 1, mjMAXVAL);
EXPECT_FLOAT_EQ(dist, -1); EXPECT_FLOAT_EQ(dist, -1);
// size 0 geom // size 0 geom
mjtNum pnt4[] = {-2, 0, 0}; mjtNum pnt4[] = {-2, 0, 0};
m->geom_aabb[0] = m->geom_aabb[1] = m->geom_aabb[2] = 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; 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]) // margin = atan(max_half / dist) where max_half = max(aabb[3..5])
// For a zero-size AABB: max_half = 0, so margin = 0 // For a zero-size AABB: max_half = 0, so margin = 0
mjtNum dist4 = mju_dist3(pnt4, d->geom_xpos); mjtNum dist4 = mju_dist3(pnt4, d->geom_xpos);
@@ -357,12 +355,9 @@ TEST_F(RayTest, EdgeCases) {
EXPECT_NEAR(geom_ba[2], 0 + 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}; mjtNum vec4[] = {1, 0, 0};
mj_multiRay(m, d, pnt4, vec4, NULL, 1, -1, &rgeomid, &dist, nullptr, 1, mj_multiRay(m.get(), d.get(), pnt4, vec4, NULL, 1, -1, &rgeomid, &dist,
mjMAXVAL); nullptr, 1, mjMAXVAL);
EXPECT_FLOAT_EQ(dist, 0.9); EXPECT_FLOAT_EQ(dist, 0.9);
mj_deleteData(d);
mj_deleteModel(m);
} }
// ------------------------------- mj_rayMesh --------------------------------- // ------------------------------- mj_rayMesh ---------------------------------
@@ -403,13 +398,15 @@ mjtNum _rayMesh(const mjModel* m, const mjData* d, int geomid,
// process all triangles // process all triangles
int face, meshid = m->geom_dataid[geomid]; int face, meshid = m->geom_dataid[geomid];
for (face = m->mesh_faceadr[meshid]; for (face = m->mesh_faceadr[meshid];
face < m->mesh_faceadr[meshid] + m->mesh_facenum[meshid]; face < m->mesh_faceadr[meshid] + m->mesh_facenum[meshid]; face++) {
face++) {
// get float vertices // get float vertices
float* vf[3]; float* vf[3];
vf[0] = m->mesh_vert + 3*(m->mesh_face[3*face] + m->mesh_vertadr[meshid]); vf[0] =
vf[1] = m->mesh_vert + 3*(m->mesh_face[3*face+1] + m->mesh_vertadr[meshid]); m->mesh_vert + 3 * (m->mesh_face[3 * face] + m->mesh_vertadr[meshid]);
vf[2] = m->mesh_vert + 3*(m->mesh_face[3*face+2] + 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 // convert to mjtNum
mjtNum v[3][3]; mjtNum v[3][3];
@@ -466,7 +463,6 @@ void _rayMeshTest(const mjModel* m) {
EXPECT_FLOAT_EQ(dist_new, dist_old); EXPECT_FLOAT_EQ(dist_new, dist_old);
} }
} }
mj_deleteData(d); mj_deleteData(d);
} }
@@ -478,15 +474,13 @@ TEST_F(RayTest, RayMeshPruning) {
const string xml_path = const string xml_path =
GetTestDataFilePath("engine/testdata/ray/stanford_bunny.xml"); GetTestDataFilePath("engine/testdata/ray/stanford_bunny.xml");
mjModel* m = mj_loadXML(xml_path.c_str(), NULL, error, sizeof(error)); MjModelPtr m(mj_loadXML(xml_path.c_str(), NULL, error, sizeof(error)));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
_rayMeshTest(m); _rayMeshTest(m.get());
mj_deleteModel(m);
m = LoadModelFromString(kCubeletModel, error, sizeof(error)); m = LoadModelFromString(kCubeletModel, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
_rayMeshTest(m); _rayMeshTest(m.get());
mj_deleteModel(m);
} }
TEST_F(RayTest, RayHfield) { TEST_F(RayTest, RayHfield) {
@@ -523,21 +517,17 @@ TEST_F(RayTest, RayHfield) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model); 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[0], MjNear(1, 1e-8, 1e-5));
EXPECT_THAT(data->sensordata[1], 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[2], MjNear(1, 1e-8, 1e-5));
EXPECT_THAT(data->sensordata[3], MjNear(0.5, 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"; static const char* const kPlaneModel = "engine/testdata/ray/plane.xml";
@@ -603,8 +593,8 @@ TEST_F(RayTest, RayNormal) {
} else { } else {
r = mj_rayFlex(m, d, /*flex_layer*/ 0, /*flg_vert*/ 1, r = mj_rayFlex(m, d, /*flex_layer*/ 0, /*flg_vert*/ 1,
/*flg_edge*/ 1, /*flg_face*/ 1, /*flg_edge*/ 1, /*flg_face*/ 1,
/*flg_skin*/ 1, /*flex_id*/ 0, /*flg_skin*/ 1, /*flex_id*/ 0, pnt, vec, nullptr,
pnt, vec, nullptr, normal); normal);
// no sensor comparison: rangefinders only intersect with geoms // no sensor comparison: rangefinders only intersect with geoms
} }
@@ -623,8 +613,7 @@ TEST_F(RayTest, RayNormal) {
mjtNum eps = 1e-6; mjtNum eps = 1e-6;
mjtNum ds[2][3]; mjtNum ds[2][3];
for (int i = 0; i < 2; ++i) { for (int i = 0; i < 2; ++i) {
mjtNum nudge[3] = {d->site_xmat[0 + i], mjtNum nudge[3] = {d->site_xmat[0 + i], d->site_xmat[3 + i],
d->site_xmat[3 + i],
d->site_xmat[6 + i]}; d->site_xmat[6 + i]};
mjtNum dr, dpnt[3]; mjtNum dr, dpnt[3];
mju_addScl3(dpnt, pnt, nudge, eps); mju_addScl3(dpnt, pnt, nudge, eps);
@@ -650,7 +639,8 @@ TEST_F(RayTest, RayNormal) {
mjtNum expected_neg[3] = {-expected[0], -expected[1], -expected[2]}; mjtNum expected_neg[3] = {-expected[0], -expected[1], -expected[2]};
// compare analytic with fin-diff approximation // compare analytic with fin-diff approximation
EXPECT_THAT(normal, AnyOf(Pointwise(MjNear(100*eps, 1e-3), expected), EXPECT_THAT(normal,
AnyOf(Pointwise(MjNear(100 * eps, 1e-3), expected),
Pointwise(MjNear(100 * eps, 1e-3), expected_neg))) Pointwise(MjNear(100 * eps, 1e-3), expected_neg)))
<< path << ", time " << d->time; << path << ", time " << d->time;
File diff suppressed because it is too large Load Diff
+26 -43
View File
@@ -53,7 +53,7 @@ TEST_F(SetConstTest, AwakeActuatedJoint) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* m; MjModelPtr m;
string sleep[] = {"auto", "never", "allowed", "init"}; string sleep[] = {"auto", "never", "allowed", "init"};
int tsp0[] = {mjSLEEP_AUTO_NEVER, mjSLEEP_NEVER, mjSLEEP_ALLOWED, int tsp0[] = {mjSLEEP_AUTO_NEVER, mjSLEEP_NEVER, mjSLEEP_ALLOWED,
@@ -69,10 +69,9 @@ TEST_F(SetConstTest, AwakeActuatedJoint) {
size_t pos2 = xml_copy.find("POLICY2"); size_t pos2 = xml_copy.find("POLICY2");
xml_copy.replace(pos2, 7, sleep[j]); xml_copy.replace(pos2, 7, sleep[j]);
m = LoadModelFromString(xml_copy.c_str(), error, sizeof(error)); 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[0], tsp0[i]);
EXPECT_EQ(m->tree_sleep_policy[1], tsp1[j]); EXPECT_EQ(m->tree_sleep_policy[1], tsp1[j]);
mj_deleteModel(m);
} }
} }
} }
@@ -97,13 +96,11 @@ TEST_F(SetConstTest, AwakeActuatedSite) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER); EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER);
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED); EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED);
mj_deleteModel(model);
} }
TEST_F(SetConstTest, AwakeActuatedBody) { TEST_F(SetConstTest, AwakeActuatedBody) {
@@ -125,13 +122,11 @@ TEST_F(SetConstTest, AwakeActuatedBody) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER); EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER);
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED); EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED);
mj_deleteModel(model);
} }
TEST_F(SetConstTest, AwakeActuatedTendon) { TEST_F(SetConstTest, AwakeActuatedTendon) {
@@ -161,13 +156,11 @@ TEST_F(SetConstTest, AwakeActuatedTendon) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER); EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER);
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED); EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED);
mj_deleteModel(model);
} }
TEST_F(SetConstTest, AwakeStiffTendonMultiTree) { TEST_F(SetConstTest, AwakeStiffTendonMultiTree) {
@@ -194,13 +187,11 @@ TEST_F(SetConstTest, AwakeStiffTendonMultiTree) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER); EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_NEVER);
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_NEVER); EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_NEVER);
mj_deleteModel(model);
} }
TEST_F(SetConstTest, SleepyTendonSingleTree) { TEST_F(SetConstTest, SleepyTendonSingleTree) {
@@ -227,12 +218,10 @@ TEST_F(SetConstTest, SleepyTendonSingleTree) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_ALLOWED); EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_ALLOWED);
mj_deleteModel(model);
} }
TEST_F(SetConstTest, SleepyTendonZeroStiffness) { TEST_F(SetConstTest, SleepyTendonZeroStiffness) {
@@ -259,13 +248,11 @@ TEST_F(SetConstTest, SleepyTendonZeroStiffness) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_ALLOWED); EXPECT_EQ(model->tree_sleep_policy[0], mjSLEEP_AUTO_ALLOWED);
EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED); EXPECT_EQ(model->tree_sleep_policy[1], mjSLEEP_AUTO_ALLOWED);
mj_deleteModel(model);
} }
TEST_F(SetConstTest, TendonTreeId) { TEST_F(SetConstTest, TendonTreeId) {
@@ -336,16 +323,16 @@ TEST_F(SetConstTest, TendonTreeId) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
int t_static_id = mj_name2id(model, mjOBJ_TENDON, "T_static"); int t_static_id = mj_name2id(model.get(), mjOBJ_TENDON, "T_static");
int t_tree1_id = mj_name2id(model, mjOBJ_TENDON, "T_tree1"); int t_tree1_id = mj_name2id(model.get(), mjOBJ_TENDON, "T_tree1");
int t_intertree12_id = mj_name2id(model, mjOBJ_TENDON, "T_intertree12"); int t_intertree12_id = mj_name2id(model.get(), mjOBJ_TENDON, "T_intertree12");
int t_intertree123_id = mj_name2id(model, mjOBJ_TENDON, "T_intertree123"); 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 b1_1_treeid = model->body_treeid[mj_name2id(model.get(), mjOBJ_BODY, "B1_1")];
int b2_1_treeid = model->body_treeid[mj_name2id(model, mjOBJ_BODY, "B2_1")]; int b2_1_treeid = model->body_treeid[mj_name2id(model.get(), mjOBJ_BODY, "B2_1")];
// Tendon 1: Not associated with any tree // Tendon 1: Not associated with any tree
EXPECT_EQ(model->tendon_treenum[t_static_id], 0); 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], b1_1_treeid);
EXPECT_EQ(model->tendon_treeid[2*t_intertree123_id+1], b2_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 // The third tree ID is not stored in tendon_treeid
mj_deleteModel(model);
} }
TEST_F(SetConstTest, SleepingNotAllowed) { TEST_F(SetConstTest, SleepingNotAllowed) {
@@ -415,8 +400,8 @@ TEST_F(SetConstTest, SleepingNotAllowed) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
EXPECT_THAT(model, IsNull()) << error; EXPECT_THAT(model.get(), IsNull()) << error;
EXPECT_THAT(string(error), HasSubstr( EXPECT_THAT(string(error), HasSubstr(
"tree 1 connected to tendon 0 which spans more than 2 trees, " "tree 1 connected to tendon 0 which spans more than 2 trees, "
"sleeping not allowed")); "sleeping not allowed"));
@@ -447,8 +432,8 @@ TEST_F(SetConstTest, DofLength) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mjtNum tol = 1e-5; 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[8], 5, tol);
EXPECT_NEAR(model->dof_length[9], 5, tol); EXPECT_NEAR(model->dof_length[9], 5, tol);
EXPECT_NEAR(model->dof_length[10], 5, tol); EXPECT_NEAR(model->dof_length[10], 5, tol);
mj_deleteModel(model);
} }
} // namespace } // namespace
+56 -91
View File
@@ -14,26 +14,27 @@
// Tests for engine/engine_sleep.c. // Tests for engine/engine_sleep.c.
#include "src/engine/engine_sleep.h"
#include <string> #include <string>
#include <vector> #include <vector>
#include <gmock/gmock.h> #include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <gtest/gtest-spi.h> #include <gtest/gtest-spi.h>
#include <gtest/gtest.h>
#include <mujoco/mjmodel.h> #include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h> #include <mujoco/mujoco.h>
#include "src/engine/engine_sleep.h"
#include "test/fixture.h" #include "test/fixture.h"
namespace mujoco { namespace mujoco {
namespace { namespace {
using ::testing::ElementsAre;
using ::testing::IsNull;
using ::testing::HasSubstr;
using ::testing::NotNull;
using ::std::string; using ::std::string;
using ::std::vector; using ::std::vector;
using ::testing::ElementsAre;
using ::testing::HasSubstr;
using ::testing::IsNull;
using ::testing::NotNull;
using SleepTest = MujocoTest; using SleepTest = MujocoTest;
@@ -76,9 +77,9 @@ static constexpr int kAwake = -(1+mjMINAWAKE);
TEST_F(SleepTest, MjSleepUpdate) { TEST_F(SleepTest, MjSleepUpdate) {
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(kSimple, error, sizeof(error)); MjModelPtr m = LoadModelFromString(kSimple, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
// ntree = 2, nbody = 5, nv = 5, njnt = 3, ngeom = 6 // ntree = 2, nbody = 5, nv = 5, njnt = 3, ngeom = 6
// body 0: world, 1 geom // body 0: world, 1 geom
@@ -93,96 +94,69 @@ TEST_F(SleepTest, MjSleepUpdate) {
EXPECT_EQ(m->njnt, 3); EXPECT_EQ(m->njnt, 3);
EXPECT_EQ(m->ngeom, 6); EXPECT_EQ(m->ngeom, 6);
EXPECT_THAT(AsVector(m->body_treeid, m->nbody), EXPECT_THAT(AsVector(m->body_treeid, m->nbody), ElementsAre(-1, 0, -1, 1, 1));
ElementsAre(-1, 0, -1, 1, 1)); EXPECT_THAT(AsVector(m->dof_bodyid, m->nv), ElementsAre(1, 1, 1, 3, 4));
EXPECT_THAT(AsVector(m->dof_bodyid, m->nv),
ElementsAre(1, 1, 1, 3, 4));
EXPECT_THAT(AsVector(m->geom_bodyid, m->ngeom), EXPECT_THAT(AsVector(m->geom_bodyid, m->ngeom),
ElementsAre(0, 1, 2, 3, 3, 4)); ElementsAre(0, 1, 2, 3, 3, 4));
EXPECT_THAT(AsVector(m->jnt_bodyid, m->njnt), EXPECT_THAT(AsVector(m->jnt_bodyid, m->njnt), ElementsAre(1, 3, 4));
ElementsAre(1, 3, 4));
// Test Case 1: Initial state // Test Case 1: Initial state
EXPECT_THAT(AsVector(d->tree_asleep, m->ntree), EXPECT_THAT(AsVector(d->tree_asleep, m->ntree), ElementsAre(kAwake, kAwake));
ElementsAre(kAwake, kAwake));
EXPECT_EQ(d->ntree_awake, 2); EXPECT_EQ(d->ntree_awake, 2);
EXPECT_EQ(d->nv_awake, 5); EXPECT_EQ(d->nv_awake, 5);
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake),
ElementsAre(0, 1, 2, 3, 4)); ElementsAre(0, 1, 2, 3, 4));
EXPECT_THAT(AsVector(d->tree_awake, m->ntree), EXPECT_THAT(AsVector(d->tree_awake, m->ntree), ElementsAre(1, 1));
ElementsAre(1, 1)); EXPECT_THAT(
EXPECT_THAT(AsVector(d->body_awake, m->nbody), AsVector(d->body_awake, m->nbody),
ElementsAre(mjS_STATIC, ElementsAre(mjS_STATIC, mjS_AWAKE, mjS_STATIC, mjS_AWAKE, mjS_AWAKE));
mjS_AWAKE,
mjS_STATIC,
mjS_AWAKE,
mjS_AWAKE));
// Test Case 2: Call mj_sleepUpdate, expect no changes // 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->ntree_awake, 2);
EXPECT_EQ(d->nv_awake, 5); EXPECT_EQ(d->nv_awake, 5);
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake),
ElementsAre(0, 1, 2, 3, 4)); ElementsAre(0, 1, 2, 3, 4));
EXPECT_THAT(AsVector(d->body_awake, m->nbody), EXPECT_THAT(
ElementsAre(mjS_STATIC, AsVector(d->body_awake, m->nbody),
mjS_AWAKE, ElementsAre(mjS_STATIC, mjS_AWAKE, mjS_STATIC, mjS_AWAKE, mjS_AWAKE));
mjS_STATIC, EXPECT_THAT(AsVector(d->tree_awake, m->ntree), ElementsAre(1, 1));
mjS_AWAKE,
mjS_AWAKE));
EXPECT_THAT(AsVector(d->tree_awake, m->ntree),
ElementsAre(1, 1));
// Test Case 3: Tree 0 asleep // Test Case 3: Tree 0 asleep
d->tree_asleep[0] = 0; d->tree_asleep[1] = -1; d->tree_asleep[0] = 0;
mj_updateSleep(m, d); d->tree_asleep[1] = -1;
mj_updateSleep(m.get(), d.get());
EXPECT_EQ(d->ntree_awake, 1); EXPECT_EQ(d->ntree_awake, 1);
EXPECT_EQ(d->nv_awake, 2); EXPECT_EQ(d->nv_awake, 2);
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), ElementsAre(3, 4));
ElementsAre(3, 4)); EXPECT_THAT(AsVector(d->tree_awake, m->ntree), ElementsAre(0, 1));
EXPECT_THAT(AsVector(d->tree_awake, m->ntree), EXPECT_THAT(
ElementsAre(0, 1)); AsVector(d->body_awake, m->nbody),
EXPECT_THAT(AsVector(d->body_awake, m->nbody), ElementsAre(mjS_STATIC, mjS_ASLEEP, mjS_STATIC, mjS_AWAKE, mjS_AWAKE));
ElementsAre(mjS_STATIC,
mjS_ASLEEP,
mjS_STATIC,
mjS_AWAKE,
mjS_AWAKE));
// Test Case 4: Tree 1 asleep // Test Case 4: Tree 1 asleep
d->tree_asleep[0] = -1; d->tree_asleep[1] = 1; d->tree_asleep[0] = -1;
mj_updateSleep(m, d); d->tree_asleep[1] = 1;
mj_updateSleep(m.get(), d.get());
EXPECT_EQ(d->ntree_awake, 1); EXPECT_EQ(d->ntree_awake, 1);
EXPECT_EQ(d->nv_awake, 3); EXPECT_EQ(d->nv_awake, 3);
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), ElementsAre(0, 1, 2));
ElementsAre(0, 1, 2)); EXPECT_THAT(AsVector(d->tree_awake, m->ntree), ElementsAre(1, 0));
EXPECT_THAT(AsVector(d->tree_awake, m->ntree), EXPECT_THAT(
ElementsAre(1, 0)); AsVector(d->body_awake, m->nbody),
EXPECT_THAT(AsVector(d->body_awake, m->nbody), ElementsAre(mjS_STATIC, mjS_AWAKE, mjS_STATIC, mjS_ASLEEP, mjS_ASLEEP));
ElementsAre(mjS_STATIC,
mjS_AWAKE,
mjS_STATIC,
mjS_ASLEEP,
mjS_ASLEEP));
// Test Case 5: All trees asleep // Test Case 5: All trees asleep
d->tree_asleep[0] = 0; d->tree_asleep[1] = 1; d->tree_asleep[0] = 0;
mj_updateSleep(m, d); d->tree_asleep[1] = 1;
mj_updateSleep(m.get(), d.get());
EXPECT_EQ(d->ntree_awake, 0); EXPECT_EQ(d->ntree_awake, 0);
EXPECT_EQ(d->nv_awake, 0); EXPECT_EQ(d->nv_awake, 0);
EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), EXPECT_THAT(AsVector(d->dof_awake_ind, d->nv_awake), ElementsAre());
ElementsAre()); EXPECT_THAT(AsVector(d->tree_awake, m->ntree), ElementsAre(0, 0));
EXPECT_THAT(AsVector(d->tree_awake, m->ntree), EXPECT_THAT(
ElementsAre(0, 0)); AsVector(d->body_awake, m->nbody),
EXPECT_THAT(AsVector(d->body_awake, m->nbody), ElementsAre(mjS_STATIC, mjS_ASLEEP, mjS_STATIC, mjS_ASLEEP, mjS_ASLEEP));
ElementsAre(mjS_STATIC,
mjS_ASLEEP,
mjS_STATIC,
mjS_ASLEEP,
mjS_ASLEEP));
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(SleepTest, MjWakeIsland) { TEST_F(SleepTest, MjWakeIsland) {
@@ -191,30 +165,25 @@ TEST_F(SleepTest, MjWakeIsland) {
EXPECT_EQ(mj_wakeIsland(asleep, 4, 0, kAwake, nullptr, 0), 0); EXPECT_EQ(mj_wakeIsland(asleep, 4, 0, kAwake, nullptr, 0), 0);
EXPECT_THAT(AsVector(asleep, 4), ElementsAre(kAwake, 2, 1, 3)); EXPECT_THAT(AsVector(asleep, 4), ElementsAre(kAwake, 2, 1, 3));
EXPECT_EQ(mj_wakeIsland(asleep, 4, 1, kAwake, nullptr, 0), 2); EXPECT_EQ(mj_wakeIsland(asleep, 4, 1, kAwake, nullptr, 0), 2);
EXPECT_THAT(AsVector(asleep, 4), EXPECT_THAT(AsVector(asleep, 4), ElementsAre(kAwake, kAwake, kAwake, 3));
ElementsAre(kAwake, kAwake, kAwake, 3));
EXPECT_EQ(mj_wakeIsland(asleep, 4, 3, kAwake, nullptr, 0), 1); EXPECT_EQ(mj_wakeIsland(asleep, 4, 3, kAwake, nullptr, 0), 1);
EXPECT_THAT(AsVector(asleep, 4), EXPECT_THAT(AsVector(asleep, 4), ElementsAre(kAwake, kAwake, kAwake, kAwake));
ElementsAre(kAwake, kAwake, kAwake, kAwake));
} }
TEST_F(SleepTest, BadWakeIsland) { TEST_F(SleepTest, BadWakeIsland) {
EXPECT_FATAL_FAILURE( EXPECT_FATAL_FAILURE(([] {
([] {
int asleep_bad1[] = {-1, 0}; int asleep_bad1[] = {-1, 0};
mj_wakeIsland(asleep_bad1, 2, 1, kAwake, nullptr, 0); mj_wakeIsland(asleep_bad1, 2, 1, kAwake, nullptr, 0);
}()), }()),
"invalid sleep state index -1 when waking tree 1"); "invalid sleep state index -1 when waking tree 1");
EXPECT_FATAL_FAILURE( EXPECT_FATAL_FAILURE(([] {
([] {
int asleep_bad2[] = {-1, 2}; int asleep_bad2[] = {-1, 2};
mj_wakeIsland(asleep_bad2, 2, 1, kAwake, nullptr, 0); mj_wakeIsland(asleep_bad2, 2, 1, kAwake, nullptr, 0);
}()), }()),
"invalid sleep state index 2 when waking tree 1"); "invalid sleep state index 2 when waking tree 1");
EXPECT_FATAL_FAILURE( EXPECT_FATAL_FAILURE(([] {
([] {
int asleep_bad3[] = {1, 2, 1}; int asleep_bad3[] = {1, 2, 1};
mj_wakeIsland(asleep_bad3, 3, 0, kAwake, nullptr, 0); mj_wakeIsland(asleep_bad3, 3, 0, kAwake, nullptr, 0);
}()), }()),
@@ -361,7 +330,6 @@ TEST_F(SleepTest, WakingUnaffectedBySleeping) {
mj_deleteModel(m); mj_deleteModel(m);
} }
// Test that waking does not affect sleeping trees for pos/vel-dependent arrays. // 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, // 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 // copy the state from the mjData with sleeping enabled to another mjData and
@@ -555,9 +523,9 @@ TEST_F(SleepTest, MidpointSleepZeroVelocity) {
)"; )";
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
// give initial velocity (both translational and angular) // give initial velocity (both translational and angular)
d->qvel[0] = 0.5; d->qvel[0] = 0.5;
@@ -569,7 +537,7 @@ TEST_F(SleepTest, MidpointSleepZeroVelocity) {
// step until body goes to sleep // step until body goes to sleep
for (int step = 0; step < 1000; step++) { for (int step = 0; step < 1000; step++) {
mj_step(m, d); mj_step(m.get(), d.get());
if (d->ntree_awake == 0) break; 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->qvel[i], 0.0) << "qvel[" << i << "] not zero after sleep";
EXPECT_EQ(d->qacc[i], 0.0) << "qacc[" << 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 = static const char* const kInitIslandFailModel =
+125 -165
View File
@@ -38,10 +38,6 @@ using ::testing::Ne;
using ::testing::NotNull; using ::testing::NotNull;
using ::testing::Pointwise; using ::testing::Pointwise;
using Name2idTest = MujocoTest; using Name2idTest = MujocoTest;
static constexpr char name2idTestingModel[] = R"( static constexpr char name2idTestingModel[] = R"(
@@ -95,78 +91,68 @@ static constexpr char name2idTestingModel[] = R"(
TEST_F(Name2idTest, FindIds) { TEST_F(Name2idTest, FindIds) {
char error[1024]; char error[1024];
mjModel* model = MjModelPtr model =
LoadModelFromString(name2idTestingModel, error, sizeof(error)); LoadModelFromString(name2idTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "world"), 0); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_BODY, "world"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "body1"), 1); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_BODY, "body1"), 1);
EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "body2"), 2); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_BODY, "body2"), 2);
EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "body1_geom1"), 0); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_GEOM, "body1_geom1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "body1_geom2"), 1); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_GEOM, "body1_geom2"), 1);
EXPECT_THAT(mj_name2id(model, mjOBJ_JOINT, "joint2"), 1); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_JOINT, "joint2"), 1);
EXPECT_THAT(mj_name2id(model, mjOBJ_MESH, "mesh1"), 0); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_MESH, "mesh1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_LIGHT, "light1"), 0); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_LIGHT, "light1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_CAMERA, "camera1"), 0); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_CAMERA, "camera1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_SITE, "site2"), 1); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_SITE, "site2"), 1);
EXPECT_THAT(mj_name2id(model, mjOBJ_MATERIAL, "material1"), 0); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_MATERIAL, "material1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_TEXTURE, "texture1"), 0); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_TEXTURE, "texture1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_TENDON, "tendon1"), 0); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_TENDON, "tendon1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_ACTUATOR, "actuator1"), 0); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_ACTUATOR, "actuator1"), 0);
EXPECT_THAT(mj_name2id(model, mjOBJ_SENSOR, "sensor1"), 0); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_SENSOR, "sensor1"), 0);
mj_deleteModel(model);
} }
TEST_F(Name2idTest, MissingIds) { TEST_F(Name2idTest, MissingIds) {
char error[1024]; char error[1024];
mjModel* model = MjModelPtr model =
LoadModelFromString(name2idTestingModel, error, sizeof(error)); LoadModelFromString(name2idTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, "abody3"), -1); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_BODY, "abody3"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "abody2_geom2"), -1); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_GEOM, "abody2_geom2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_JOINT, "joint3"), -1); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_JOINT, "joint3"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_MESH, "amesh2"), -1); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_MESH, "amesh2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_LIGHT, "alight2"), -1); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_LIGHT, "alight2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_CAMERA, "acamera2"), -1); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_CAMERA, "acamera2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_SITE, "asite3"), -1); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_SITE, "asite3"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_MATERIAL, "amaterial2"), -1); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_MATERIAL, "amaterial2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_TEXTURE, "atexture2"), -1); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_TEXTURE, "atexture2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_TENDON, "atendon2"), -1); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_TENDON, "atendon2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_ACTUATOR, "aactuator2"), -1); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_ACTUATOR, "aactuator2"), -1);
EXPECT_THAT(mj_name2id(model, mjOBJ_SENSOR, "asensor2"), -1); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_SENSOR, "asensor2"), -1);
mj_deleteModel(model);
} }
TEST_F(Name2idTest, EmptyIds) { TEST_F(Name2idTest, EmptyIds) {
char error[1024]; char error[1024];
mjModel* model = MjModelPtr model =
LoadModelFromString(name2idTestingModel, error, sizeof(error)); LoadModelFromString(name2idTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_THAT(mj_name2id(model, mjOBJ_BODY, ""), -1); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_BODY, ""), -1);
mj_deleteModel(model);
} }
TEST_F(Name2idTest, Namespaces) { TEST_F(Name2idTest, Namespaces) {
char error[1024]; char error[1024];
mjModel* model = MjModelPtr model =
LoadModelFromString(name2idTestingModel, error, sizeof(error)); LoadModelFromString(name2idTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_THAT(mj_name2id(model, mjOBJ_GEOM, "camera1"), 3); EXPECT_THAT(mj_name2id(model.get(), mjOBJ_GEOM, "camera1"), 3);
mj_deleteModel(model);
} }
using VersionTest = MujocoTest; using VersionTest = MujocoTest;
TEST_F(VersionTest, MjVersion) { TEST_F(VersionTest, MjVersion) { EXPECT_EQ(mj_version(), mjVERSION_HEADER); }
EXPECT_EQ(mj_version(), mjVERSION_HEADER);
}
TEST_F(VersionTest, MjVersionString) { TEST_F(VersionTest, MjVersionString) {
#if GTEST_USES_SIMPLE_RE == 1 #if GTEST_USES_SIMPLE_RE == 1
@@ -177,7 +163,6 @@ TEST_F(VersionTest, MjVersionString) {
EXPECT_THAT(std::string(mj_versionString()), regex_matcher); EXPECT_THAT(std::string(mj_versionString()), regex_matcher);
} }
using SupportTest = MujocoTest; using SupportTest = MujocoTest;
// utility: generate two random quaternions with a given angle difference // utility: generate two random quaternions with a given angle difference
@@ -218,8 +203,8 @@ TEST_F(SupportTest, DifferentiatePosSubQuat) {
const mjtNum eps = 1e-12; // epsilon for float comparison const mjtNum eps = 1e-12; // epsilon for float comparison
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(ballJointModel, error, sizeof(error)); MjModelPtr model = LoadModelFromString(ballJointModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
int seed = 1; int seed = 1;
for (mjtNum angle : {0.0, 1e-5, 1e-2}) { for (mjtNum angle : {0.0, 1e-5, 1e-2}) {
@@ -230,7 +215,7 @@ TEST_F(SupportTest, DifferentiatePosSubQuat) {
// get velocity given timestep // get velocity given timestep
mjtNum qvel[3]; mjtNum qvel[3];
mj_differentiatePos(model, qvel, dt, qpos1, qpos2); mj_differentiatePos(model.get(), qvel, dt, qpos1, qpos2);
// equivalent computation // equivalent computation
mjtNum qneg[4], qdif[4], qvel_expect[3]; 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)); EXPECT_THAT(AsVector(qvel, 3), Pointwise(MjNear(eps, 1e-3), qvel_expect));
} }
} }
mj_deleteModel(model);
} }
static const char* const kDefaultModel = "testdata/model.xml"; static const char* const kDefaultModel = "testdata/model.xml";
@@ -320,38 +303,35 @@ TEST_F(StateTest, GetSetStateDelay) {
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
// verify history buffer exists: nhistory = 2 + 2*5 = 12 // verify history buffer exists: nhistory = 2 + 2*5 = 12
EXPECT_EQ(model->nhistory, 12); // [user, cursor, times(5), values(5)] EXPECT_EQ(model->nhistory, 12); // [user, cursor, times(5), values(5)]
// state size should include history buffer // 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); EXPECT_EQ(size, model->nhistory);
// step to populate history buffer // step to populate history buffer
data->ctrl[0] = 1.0; data->ctrl[0] = 1.0;
mj_step(model, data); mj_step(model.get(), data.get());
data->ctrl[0] = 2.0; data->ctrl[0] = 2.0;
mj_step(model, data); mj_step(model.get(), data.get());
// get history state // get history state
vector<mjtNum> history_state(size); 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) // modify the history buffer manually (value at index 7 = 2+5 = after times)
data->history[7] = 99.0; // first value data->history[7] = 99.0; // first value
// set history state back - should restore original // 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 // verify restoration
EXPECT_NE(data->history[7], 99.0); EXPECT_NE(data->history[7], 99.0);
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(StateTest, CopyState) { TEST_F(StateTest, CopyState) {
@@ -518,8 +498,7 @@ TEST_F(InertiaTest, AddMdenseSameAsSparse) {
} }
// sparse addM // sparse addM
mj_addM(m, d, dst_sparse.data(), rownnz.data(), mj_addM(m, d, dst_sparse.data(), rownnz.data(), rowadr.data(), colind.data());
rowadr.data(), colind.data());
// dense addM // dense addM
mj_addM(m, d, dst_dense.data(), nullptr, nullptr, nullptr); mj_addM(m, d, dst_dense.data(), nullptr, nullptr, nullptr);
@@ -548,8 +527,8 @@ TEST_F(InertiaTest, mulM) {
// dense M matrix // dense M matrix
vector<mjtNum> Mdense(nv * nv); vector<mjtNum> Mdense(nv * nv);
mju_sym2dense(Mdense.data(), data->M, nv, mju_sym2dense(Mdense.data(), data->M, nv, model->M_rownnz, model->M_rowadr,
model->M_rownnz, model->M_rowadr, model->M_colind); model->M_colind);
// arbitrary RHS vector // arbitrary RHS vector
vector<mjtNum> vec(nv); vector<mjtNum> vec(nv);
@@ -618,8 +597,8 @@ TEST_F(InertiaTest, FullM) {
// get dense mass matrix from M using mju_sparse2dense // get dense mass matrix from M using mju_sparse2dense
vector<mjtNum> M_CSR(nv * nv); vector<mjtNum> M_CSR(nv * nv);
mju_sparse2dense(M_CSR.data(), d->M, nv, nv, mju_sparse2dense(M_CSR.data(), d->M, nv, nv, m->M_rownnz, m->M_rowadr,
m->M_rownnz, m->M_rowadr, m->M_colind); m->M_colind);
// expect lower triangles to match exactly // expect lower triangles to match exactly
for (int i = 0; i < nv; ++i) { for (int i = 0; i < nv; ++i) {
@@ -630,8 +609,8 @@ TEST_F(InertiaTest, FullM) {
// get dense LTDL factor (D on the diagonal) // get dense LTDL factor (D on the diagonal)
vector<mjtNum> LD(nv * nv); vector<mjtNum> LD(nv * nv);
mju_sparse2dense(LD.data(), d->qLD, nv, nv, mju_sparse2dense(LD.data(), d->qLD, nv, nv, m->M_rownnz, m->M_rowadr,
m->M_rownnz, m->M_rowadr, m->M_colind); m->M_colind);
// extract L and D from LD // extract L and D from LD
vector<mjtNum> L = LD; vector<mjtNum> L = LD;
@@ -686,88 +665,85 @@ static constexpr char GeomDistanceTestingModel2[] = R"(
TEST_F(SupportTest, GeomDistance) { TEST_F(SupportTest, GeomDistance) {
char error[1024]; char error[1024];
mjModel* model = MjModelPtr model =
LoadModelFromString(GeomDistanceTestingModel1, error, sizeof(error)); LoadModelFromString(GeomDistanceTestingModel1, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
mj_kinematics(model, data); mj_kinematics(model.get(), data.get());
// plane-sphere, distmax too small // plane-sphere, distmax too small
mjtNum distmax = 0.5; 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]; 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})); EXPECT_THAT(fromto, Pointwise(Eq(), vector<mjtNum>{0, 0, 0, 0, 0, 0}));
// plane-sphere // plane-sphere
distmax = 1.0; 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)); MjNear(0.8, 1e-12, 1e-5));
mjtNum eps = 1e-12; mjtNum eps = 1e-12;
EXPECT_THAT(fromto, Pointwise(MjNear(eps, 1e-5), EXPECT_THAT(fromto,
vector<mjtNum>{0, 0, 0, 0, 0, 0.8})); Pointwise(MjNear(eps, 1e-5), vector<mjtNum>{0, 0, 0, 0, 0, 0.8}));
// sphere-plane // 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)); MjNear(0.8, 1e-12, 1e-5));
EXPECT_THAT(fromto, Pointwise(MjNear(eps, 1e-5), EXPECT_THAT(fromto,
vector<mjtNum>{0, 0, 0.8, 0, 0, 0})); Pointwise(MjNear(eps, 1e-5), vector<mjtNum>{0, 0, 0.8, 0, 0, 0}));
// sphere-sphere // 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)); MjNear(0.5, 1e-12, 1e-5));
EXPECT_THAT(fromto, Pointwise(MjNear(eps, 1e-5), EXPECT_THAT(fromto,
vector<mjtNum>{.2, 0, 1, .7, 0, 1})); Pointwise(MjNear(eps, 1e-5), vector<mjtNum>{.2, 0, 1, .7, 0, 1}));
// sphere-sphere, flipped order // 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)); MjNear(0.5, 1e-12, 1e-5));
EXPECT_THAT(fromto, Pointwise(MjNear(eps, 1e-5), EXPECT_THAT(fromto,
vector<mjtNum>{.7, 0, 1, .2, 0, 1})); Pointwise(MjNear(eps, 1e-5), vector<mjtNum>{.7, 0, 1, .2, 0, 1}));
// mesh-sphere (close distmax) // mesh-sphere (close distmax)
distmax = 0.701; distmax = 0.701;
eps = model->opt.ccd_tolerance; eps = model->opt.ccd_tolerance;
EXPECT_THAT(mj_geomDistance(model, data, 3, 1, distmax, fromto), EXPECT_THAT(mj_geomDistance(model.get(), data.get(), 3, 1, distmax, fromto),
MjNear(0.7, eps, eps * 100)); MjNear(0.7, eps, eps * 100));
EXPECT_THAT(fromto, Pointwise(MjNear(eps, eps * 100), EXPECT_THAT(fromto, Pointwise(MjNear(eps, eps * 100),
vector<mjtNum>{0, 0, .1, 0, 0, .8})); vector<mjtNum>{0, 0, .1, 0, 0, .8}));
// mesh-sphere (far distmax) // mesh-sphere (far distmax)
distmax = 1.0; distmax = 1.0;
EXPECT_THAT(mj_geomDistance(model, data, 3, 1, distmax, fromto), EXPECT_THAT(mj_geomDistance(model.get(), data.get(), 3, 1, distmax, fromto),
MjNear(0.7, eps, eps * 100)); MjNear(0.7, eps, eps * 100));
EXPECT_THAT(fromto, Pointwise(MjNear(eps, eps * 100), EXPECT_THAT(fromto, Pointwise(MjNear(eps, eps * 100),
vector<mjtNum>{0, 0, .1, 0, 0, .8})); vector<mjtNum>{0, 0, .1, 0, 0, .8}));
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(SupportTest, GeomDistanceFromToFlipped) { TEST_F(SupportTest, GeomDistanceFromToFlipped) {
mjtNum distmax = 10.0; mjtNum distmax = 10.0;
char error[1024]; char error[1024];
mjModel* model = MjModelPtr model =
LoadModelFromString(GeomDistanceTestingModel2, error, sizeof(error)); LoadModelFromString(GeomDistanceTestingModel2, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
mj_kinematics(model, data); mj_kinematics(model.get(), data.get());
mjtNum fromto01[6]; mjtNum fromto01[6];
mjtNum fromto10[6]; mjtNum fromto10[6];
for (int flag : {0, (int)mjDSBL_NATIVECCD}) { for (int flag : {0, (int)mjDSBL_NATIVECCD}) {
model->opt.disableflags = flag; model->opt.disableflags = flag;
mj_geomDistance(model, data, 0, 1, distmax, fromto01); mj_geomDistance(model.get(), data.get(), 0, 1, distmax, fromto01);
mj_geomDistance(model, data, 1, 0, distmax, fromto10); mj_geomDistance(model.get(), data.get(), 1, 0, distmax, fromto10);
mjtNum fromto10flipped[6] = {fromto10[3], fromto10[4], fromto10[5], mjtNum fromto10flipped[6] = {fromto10[3], fromto10[4], fromto10[5],
fromto10[0], fromto10[1], fromto10[2]}; fromto10[0], fromto10[1], fromto10[2]};
EXPECT_THAT(AsVector(fromto10flipped, 6), EXPECT_THAT(AsVector(fromto10flipped, 6),
Pointwise(MjNear(1.0e-12, 1e-5), fromto01)); Pointwise(MjNear(1.0e-12, 1e-5), fromto01));
} }
mj_deleteData(data);
mj_deleteModel(model);
} }
static constexpr char kSetKeyframeTestingModel[] = R"( static constexpr char kSetKeyframeTestingModel[] = R"(
@@ -789,40 +765,35 @@ static constexpr char kSetKeyframeTestingModel[] = R"(
TEST_F(SupportTest, SetKeyframe) { TEST_F(SupportTest, SetKeyframe) {
char error[1024]; char error[1024];
mjModel* model = MjModelPtr model =
LoadModelFromString(kSetKeyframeTestingModel, error, sizeof(error)); LoadModelFromString(kSetKeyframeTestingModel, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
data->ctrl[0] = 1; data->ctrl[0] = 1;
while (data->time < 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->time, model->key_time[1]);
EXPECT_EQ(data->ctrl[0], model->key_ctrl[model->nu * 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->qpos[0], model->key_qpos[model->nq * 1]);
EXPECT_EQ(data->qvel[0], model->key_qvel[model->nv * 1]); EXPECT_EQ(data->qvel[0], model->key_qvel[model->nv * 1]);
EXPECT_EQ(data->act[0], model->key_act[model->na * 1]); EXPECT_EQ(data->act[0], model->key_act[model->na * 1]);
mj_step(model, data); mj_step(model.get(), data.get());
mj_setKeyframe(model, data, 0); mj_setKeyframe(model.get(), data.get(), 0);
EXPECT_EQ(data->time, model->key_time[0]); EXPECT_EQ(data->time, model->key_time[0]);
EXPECT_EQ(data->ctrl[0], model->key_ctrl[model->nu * 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->qpos[0], model->key_qpos[model->nq * 0]);
EXPECT_EQ(data->qvel[0], model->key_qvel[model->nv * 0]); EXPECT_EQ(data->qvel[0], model->key_qvel[model->nv * 0]);
EXPECT_EQ(data->act[0], model->key_act[model->na * 0]); EXPECT_EQ(data->act[0], model->key_act[model->na * 0]);
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(SupportTest, ContactSensorDim) { TEST_F(SupportTest, ContactSensorDim) {
int dataSpec = 1 << mjCONDATA_FOUND | int dataSpec = 1 << mjCONDATA_FOUND | 1 << mjCONDATA_FORCE |
1 << mjCONDATA_FORCE | 1 << mjCONDATA_DIST | 1 << mjCONDATA_POS |
1 << mjCONDATA_DIST |
1 << mjCONDATA_POS |
1 << mjCONDATA_TANGENT; 1 << mjCONDATA_TANGENT;
EXPECT_EQ(mju_condataSize(dataSpec), 1 + 3 + 1 + 3 + 3); EXPECT_EQ(mju_condataSize(dataSpec), 1 + 3 + 1 + 3 + 3);
@@ -844,16 +815,14 @@ TEST_F(SupportTest, ReadCtrlNoDelay) {
</actuator> </actuator>
</mujoco> </mujoco>
)"; )";
mjModel* model = LoadModelFromString(xml); MjModelPtr model = LoadModelFromString(xml);
ASSERT_THAT(model, NotNull()); ASSERT_THAT(model.get(), NotNull());
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
// no delay: should return current ctrl value // no delay: should return current ctrl value
data->ctrl[0] = 42.0; data->ctrl[0] = 42.0;
EXPECT_EQ(mj_readCtrl(model, data, 0, data->time, /*order=*/0), 42.0); EXPECT_EQ(mj_readCtrl(model.get(), data.get(), 0, data->time, /*order=*/0),
42.0);
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(SupportTest, ReadCtrlWithDelay) { TEST_F(SupportTest, ReadCtrlWithDelay) {
@@ -872,9 +841,9 @@ TEST_F(SupportTest, ReadCtrlWithDelay) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
// model should have delay configured // model should have delay configured
// delay = 0.03 seconds, timestep = 0.01, so ndelay = ceil(0.03/0.01) = 3 // 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) // initially, buffer should be filled with constant value (from init)
// reading at current time should return the init value // 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]); EXPECT_EQ(val, data->ctrl[0]);
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(SupportTest, InitCtrlDelay) { TEST_F(SupportTest, InitCtrlDelay) {
@@ -907,35 +873,33 @@ TEST_F(SupportTest, InitCtrlDelay) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
// verify nhistory // verify nhistory
EXPECT_EQ(model->actuator_history[0], 3); EXPECT_EQ(model->actuator_history[0], 3);
// initialize with custom times and values // 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 times[3] = {0.0, 0.01, 0.02};
mjtNum values[3] = {1.0, 2.0, 3.0}; 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 // mj_readCtrl now auto-subtracts delay: lookup_time = time - delay
// delay = 0.02, so: // delay = 0.02, so:
// time=0.04 -> lookup at 0.02 -> value 3.0 // time=0.04 -> lookup at 0.02 -> value 3.0
// time=0.03 -> lookup at 0.01 -> value 2.0 // time=0.03 -> lookup at 0.01 -> value 2.0
// time=0.02 -> lookup at 0.00 -> value 1.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); 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); 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); EXPECT_EQ(val, 1.0);
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(SupportTest, InitCtrlDelayNullTimes) { TEST_F(SupportTest, InitCtrlDelayNullTimes) {
@@ -954,9 +918,9 @@ TEST_F(SupportTest, InitCtrlDelayNullTimes) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
// get existing times from buffer // get existing times from buffer
int adr = model->actuator_historyadr[0]; int adr = model->actuator_historyadr[0];
@@ -965,7 +929,7 @@ TEST_F(SupportTest, InitCtrlDelayNullTimes) {
// initialize with NULL times (use existing) and new values // initialize with NULL times (use existing) and new values
mjtNum values[3] = {10.0, 20.0, 30.0}; 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 // verify times are unchanged
EXPECT_EQ(buf[2], existing_times[0]); EXPECT_EQ(buf[2], existing_times[0]);
@@ -976,9 +940,6 @@ TEST_F(SupportTest, InitCtrlDelayNullTimes) {
EXPECT_EQ(buf[5], 10.0); EXPECT_EQ(buf[5], 10.0);
EXPECT_EQ(buf[6], 20.0); EXPECT_EQ(buf[6], 20.0);
EXPECT_EQ(buf[7], 30.0); EXPECT_EQ(buf[7], 30.0);
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(SupportTest, InitSensorDelay) { TEST_F(SupportTest, InitSensorDelay) {
@@ -997,36 +958,35 @@ TEST_F(SupportTest, InitSensorDelay) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
// verify nsample for sensor // verify nsample for sensor
EXPECT_EQ(model->sensor_history[0], 3); EXPECT_EQ(model->sensor_history[0], 3);
// initialize with custom times and values, phase=0 // 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 times[3] = {0.0, 0.01, 0.02};
mjtNum values[3] = {0.5, 0.6, 0.7}; 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 // mj_readSensor now auto-subtracts delay: lookup_time = time - delay
// delay = 0.02, so: // delay = 0.02, so:
// time=0.04 -> lookup at 0.02 -> value 0.7 // time=0.04 -> lookup at 0.02 -> value 0.7
// time=0.03 -> lookup at 0.01 -> value 0.6 // time=0.03 -> lookup at 0.01 -> value 0.6
mjtNum result = 0; 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; mjtNum val = ptr ? *ptr : result;
EXPECT_NEAR(val, 0.7, 1e-6); 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; val = ptr ? *ptr : result;
EXPECT_NEAR(val, 0.6, 1e-6); EXPECT_NEAR(val, 0.6, 1e-6);
mj_deleteData(data);
mj_deleteModel(model);
} }
} // namespace } // namespace
} // namespace mujoco } // namespace mujoco
+14 -19
View File
@@ -16,9 +16,9 @@
#include <array> #include <array>
#include <cstddef> #include <cstddef>
#include <mujoco/mjdata.h>
#include <gmock/gmock.h> #include <gmock/gmock.h>
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <mujoco/mjdata.h>
#include <mujoco/mujoco.h> #include <mujoco/mujoco.h>
#include "test/fixture.h" #include "test/fixture.h"
@@ -32,8 +32,7 @@ constexpr int GetExpectedStackUsageBytes() {
if constexpr (N <= 0) { if constexpr (N <= 0) {
return prev_size; return prev_size;
} else { } else {
constexpr auto RoundUpToAlignment = constexpr auto RoundUpToAlignment = [](int x, int alignment) {
[](int x, int alignment) {
return alignment * (x / alignment + ((x % alignment) ? 1 : 0)); return alignment * (x / alignment + ((x % alignment) ? 1 : 0));
}; };
constexpr int size_with_arraylist = RoundUpToAlignment( constexpr int size_with_arraylist = RoundUpToAlignment(
@@ -49,15 +48,15 @@ class TestMjArrayList : public MujocoTest {};
TEST_F(TestMjArrayList, TestMjArrayListSingleThreaded) { TEST_F(TestMjArrayList, TestMjArrayListSingleThreaded) {
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString("<mujoco/>", error.data(), error.size()); MjModelPtr m = LoadModelFromString("<mujoco/>", error.data(), error.size());
ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error.data(); ASSERT_THAT(m.get(), NotNull()) << "Failed to load model: " << error.data();
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_markStack(d); mj_markStack(d.get());
using DataType = int; using DataType = int;
constexpr int kInitialCapacity = 10; constexpr int kInitialCapacity = 10;
mjArrayList* array_list = mjArrayList* array_list =
mju_arrayListCreate(d, sizeof(DataType), kInitialCapacity); mju_arrayListCreate(d.get(), sizeof(DataType), kInitialCapacity);
constexpr int kNumElements = 35; constexpr int kNumElements = 35;
for (int i = 0; i < kNumElements; ++i) { 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, kNumElements), nullptr);
EXPECT_EQ(mju_arrayListAt(array_list, 100), nullptr); EXPECT_EQ(mju_arrayListAt(array_list, 100), nullptr);
mj_freeStack(d); mj_freeStack(d.get());
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(TestMjArrayList, ZeroInitialCapacity) { TEST_F(TestMjArrayList, ZeroInitialCapacity) {
char error[1024]; char error[1024];
mjModel* m = LoadModelFromString("<mujoco/>", error, sizeof(error)); MjModelPtr m = LoadModelFromString("<mujoco/>", error, sizeof(error));
ASSERT_THAT(m, NotNull()) << "Failed to load model: " << error; ASSERT_THAT(m.get(), NotNull()) << "Failed to load model: " << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_markStack(d); mj_markStack(d.get());
mjArrayList* array_list = 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); EXPECT_EQ(mju_arrayListSize(array_list), 0);
for (int i = 0; i < 35; ++i) { for (int i = 0; i < 35; ++i) {
@@ -104,9 +101,7 @@ TEST_F(TestMjArrayList, ZeroInitialCapacity) {
} }
EXPECT_EQ(mju_arrayListAt(array_list, 35), nullptr); EXPECT_EQ(mju_arrayListAt(array_list, 35), nullptr);
mj_freeStack(d); mj_freeStack(d.get());
mj_deleteData(d);
mj_deleteModel(m);
} }
} // namespace } // namespace
+67 -59
View File
@@ -14,19 +14,20 @@
// Tests for engine/engine_util_solve.c. // Tests for engine/engine_util_solve.c.
#include "src/engine/engine_util_misc.h"
#include <array> #include <array>
#include <vector>
#include <cmath> #include <cmath>
#include <cstddef> #include <cstddef>
#include <cstdint> #include <cstdint>
#include <cstring> #include <cstring>
#include <vector>
#include <gmock/gmock.h> #include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <gtest/gtest-spi.h> #include <gtest/gtest-spi.h>
#include <gtest/gtest.h>
#include <mujoco/mjdata.h> #include <mujoco/mjdata.h>
#include <mujoco/mujoco.h> #include <mujoco/mujoco.h>
#include "src/engine/engine_util_misc.h"
#include "test/fixture.h" #include "test/fixture.h"
namespace mujoco { namespace mujoco {
@@ -125,29 +126,27 @@ TEST_F(UtilMiscTest, SphereWrap) {
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
// measure tendon length for keyframe 0 // measure tendon length for keyframe 0
mj_resetDataKeyframe(model, data, 0); mj_resetDataKeyframe(model.get(), data.get(), 0);
mj_forward(model, data); mj_forward(model.get(), data.get());
mjtNum ten_length0 = data->sensordata[0]; mjtNum ten_length0 = data->sensordata[0];
// measure tendon length for keyframe 1 // measure tendon length for keyframe 1
mj_resetDataKeyframe(model, data, 1); mj_resetDataKeyframe(model.get(), data.get(), 1);
mj_forward(model, data); mj_forward(model.get(), data.get());
mjtNum ten_length1 = data->sensordata[0]; mjtNum ten_length1 = data->sensordata[0];
// difference should be small // difference should be small
mjtNum diff = ten_length1 - ten_length0; mjtNum diff = ten_length1 - ten_length0;
EXPECT_LT(mju_abs(diff), 1e-3); 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]) { mjtNum muscleDynamicsMillard(mjtNum ctrl, mjtNum act, const mjtNum prm[2]) {
// clamp control // clamp control
mjtNum ctrlclamp = mju_clip(ctrl, 0, 1); mjtNum ctrlclamp = mju_clip(ctrl, 0, 1);
@@ -196,10 +195,10 @@ TEST_F(UtilMiscTest, SmoothMuscleDynamics) {
mjtNum tau_act = 0.2; mjtNum tau_act = 0.2;
mjtNum tau_deact = 0.3; mjtNum tau_deact = 0.3;
for (mjtNum dctrl : {0.0, 0.1, 0.2, 1.0, 1.1}) { for (mjtNum dctrl : {0.0, 0.1, 0.2, 1.0, 1.1}) {
mjtNum lower = mju_muscleDynamicsTimescale(-dctrl, mjtNum lower =
tau_act, tau_deact, tau_smooth); mju_muscleDynamicsTimescale(-dctrl, tau_act, tau_deact, tau_smooth);
mjtNum upper = mju_muscleDynamicsTimescale(dctrl, mjtNum upper =
tau_act, tau_deact, tau_smooth); mju_muscleDynamicsTimescale(dctrl, tau_act, tau_deact, tau_smooth);
EXPECT_EQ(0.5 * (upper + lower), 0.5 * (tau_act + tau_deact)); EXPECT_EQ(0.5 * (upper + lower), 0.5 * (tau_act + tau_deact));
} }
} }
@@ -250,11 +249,9 @@ TEST_F(UtilMiscTest, MjuSparseMap) {
// Verify the map by checking values // Verify the map by checking values
mjtNum mat_res_gathered[nnz_res]; mjtNum mat_res_gathered[nnz_res];
mju_gather(mat_res_gathered, mat_src, map, nnz_res); mju_gather(mat_res_gathered, mat_src, map, nnz_res);
EXPECT_THAT(AsVector(mat_res_gathered, nnz_res), EXPECT_THAT(AsVector(mat_res_gathered, nnz_res), ElementsAre(1, 3, 5, 6));
ElementsAre(1, 3, 5, 6));
} }
TEST_F(UtilMiscTest, MjuSparseLower2SymMap) { TEST_F(UtilMiscTest, MjuSparseLower2SymMap) {
// nr = 3 // nr = 3
// src = [[1, 0, 0], // src = [[1, 0, 0],
@@ -277,8 +274,8 @@ TEST_F(UtilMiscTest, MjuSparseLower2SymMap) {
int map[res_nnz]; int map[res_nnz];
int cursor[nr]; int cursor[nr];
mju_lower2SymMap(map, nr, rowadr_res, rownnz_res, colind_res, mju_lower2SymMap(map, nr, rowadr_res, rownnz_res, colind_res, rowadr_src,
rowadr_src, rownnz_src, colind_src, cursor); rownnz_src, colind_src, cursor);
// Expected map: // Expected map:
// res(0,0) -> src(0,0) (k=0) => map[0] = 0 // res(0,0) -> src(0,0) (k=0) => map[0] = 0
@@ -323,8 +320,8 @@ TEST_F(UtilMiscTest, MjuSparseLower2SymMapPartial) {
int map[res_nnz]; int map[res_nnz];
int cursor[nr]; int cursor[nr];
mju_lower2SymMap(map, nr, rowadr_res, rownnz_res, colind_res, mju_lower2SymMap(map, nr, rowadr_res, rownnz_res, colind_res, rowadr_src,
rowadr_src, rownnz_src, colind_src, cursor); rownnz_src, colind_src, cursor);
// Expected map for the non-zeros in res: // Expected map for the non-zeros in res:
// res(0,0) -> src(0,0) (k=0) => map[0] = 0 // res(0,0) -> src(0,0) (k=0) => map[0] = 0
@@ -404,7 +401,8 @@ TEST_F(InterpolationTest, mju_interpolate3D) {
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) { 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; static constexpr int order = 2;
mjtNum coeff[3 * (order + 1) * (order + 1) * (order + 1)]; mjtNum coeff[3 * (order + 1) * (order + 1) * (order + 1)];
@@ -956,11 +954,14 @@ TEST_F(HistoryTest, ReadVector_ExactMatch) {
// set values: t=0->(1,2), t=1->(3,4), t=2->(5,6) // set values: t=0->(1,2), t=1->(3,4), t=2->(5,6)
mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0); 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); 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); 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]; mjtNum res[dim];
const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 1.0, 0); const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 1.0, 0);
@@ -980,11 +981,14 @@ TEST_F(HistoryTest, ReadVector_ZOH) {
mju_zero(buf + 2 + n, n * dim); mju_zero(buf + 2 + n, n * dim);
mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0); 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); 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); 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]; mjtNum res[dim];
const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 0.5, 0); const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 0.5, 0);
@@ -1004,11 +1008,14 @@ TEST_F(HistoryTest, ReadVector_Linear) {
mju_zero(buf + 2 + n, n * dim); mju_zero(buf + 2 + n, n * dim);
mjtNum* slot0 = mju_historyInsert(buf, n, dim, 0.0); 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); 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); 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]; mjtNum res[dim];
const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 0.5, 1); const mjtNum* ptr = mju_historyRead(buf, n, dim, res, 0.5, 1);
@@ -1135,9 +1142,11 @@ TEST_F(HistoryTest, CubicInterpolation) {
// Dim 0: 0 -> 1. Spline: p(x) = 3x^2 - 2x^3 // Dim 0: 0 -> 1. Spline: p(x) = 3x^2 - 2x^3
// Dim 1: 1 -> 0. Spline: p(x) = 1 - 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); 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); 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]; mjtNum res[2];
@@ -1198,8 +1207,8 @@ TEST_F(FaceStateTest, NodeIndicesSingleCell) {
// normal_axis=0, na0=1, na1=2 // normal_axis=0, na0=1, na1=2
{ {
int indices[4]; int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL, mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL, NULL,
NULL, NULL, NULL, indices, NULL); NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 0)); EXPECT_EQ(indices[0], gidx(0, 0, 0));
EXPECT_EQ(indices[1], gidx(0, 0, 1)); EXPECT_EQ(indices[1], gidx(0, 0, 1));
EXPECT_EQ(indices[2], gidx(0, 1, 0)); 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]) // face 1: x=max (fixed g[0]=1, varying g[1], g[2])
{ {
int indices[4]; int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 1, xpos.data(), NULL, NULL, mju_flexGatherFaceState(order, cx, cy, cz, 1, xpos.data(), NULL, NULL, NULL,
NULL, NULL, NULL, indices, NULL); NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(1, 0, 0)); EXPECT_EQ(indices[0], gidx(1, 0, 0));
EXPECT_EQ(indices[1], gidx(1, 0, 1)); EXPECT_EQ(indices[1], gidx(1, 0, 1));
EXPECT_EQ(indices[2], gidx(1, 1, 0)); EXPECT_EQ(indices[2], gidx(1, 1, 0));
@@ -1222,8 +1231,8 @@ TEST_F(FaceStateTest, NodeIndicesSingleCell) {
// loop order: l0→z, l1→x // loop order: l0→z, l1→x
{ {
int indices[4]; int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 2, xpos.data(), NULL, NULL, mju_flexGatherFaceState(order, cx, cy, cz, 2, xpos.data(), NULL, NULL, NULL,
NULL, NULL, NULL, indices, NULL); NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 0)); // l0=0(z=0), l1=0(x=0) 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[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) 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) // normal_axis=1, na0=2(z slow), na1=0(x fast)
{ {
int indices[4]; int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL, mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL, NULL,
NULL, NULL, NULL, indices, NULL); NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 1, 0)); // l0=0(z=0), l1=0(x=0) 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[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) 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 // normal_axis=2, na0=0, na1=1
{ {
int indices[4]; int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL, mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL, NULL,
NULL, NULL, NULL, indices, NULL); NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 0)); EXPECT_EQ(indices[0], gidx(0, 0, 0));
EXPECT_EQ(indices[1], gidx(0, 1, 0)); EXPECT_EQ(indices[1], gidx(0, 1, 0));
EXPECT_EQ(indices[2], gidx(1, 0, 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) // face 5: z=max (fixed g[2]=1)
{ {
int indices[4]; int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 5, xpos.data(), NULL, NULL, mju_flexGatherFaceState(order, cx, cy, cz, 5, xpos.data(), NULL, NULL, NULL,
NULL, NULL, NULL, indices, NULL); NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 1)); EXPECT_EQ(indices[0], gidx(0, 0, 1));
EXPECT_EQ(indices[1], gidx(0, 1, 1)); EXPECT_EQ(indices[1], gidx(0, 1, 1));
EXPECT_EQ(indices[2], gidx(1, 0, 1)); EXPECT_EQ(indices[2], gidx(1, 0, 1));
@@ -1299,8 +1308,8 @@ TEST_F(FaceStateTest, NodeIndicesMultiCell) {
// na0=1, na1=2: g[0]=0, g[1]=0..1, g[2]=0..1 // na0=1, na1=2: g[0]=0, g[1]=0..1, g[2]=0..1
{ {
int indices[4]; int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL, mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL, NULL,
NULL, NULL, NULL, indices, NULL); NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 0)); EXPECT_EQ(indices[0], gidx(0, 0, 0));
EXPECT_EQ(indices[1], gidx(0, 0, 1)); EXPECT_EQ(indices[1], gidx(0, 0, 1));
EXPECT_EQ(indices[2], gidx(0, 1, 0)); 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 // face 0 (x=0), quad 3: (q0=1, q1=1) → within_face = 1*2+1 = 3
{ {
int indices[4]; int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL, mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL, NULL,
NULL, NULL, NULL, indices, NULL); NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 1, 1)); EXPECT_EQ(indices[0], gidx(0, 1, 1));
EXPECT_EQ(indices[1], gidx(0, 1, 2)); EXPECT_EQ(indices[1], gidx(0, 1, 2));
EXPECT_EQ(indices[2], gidx(0, 2, 1)); EXPECT_EQ(indices[2], gidx(0, 2, 1));
@@ -1322,8 +1331,8 @@ TEST_F(FaceStateTest, NodeIndicesMultiCell) {
// g[0] = cx*order = 2 // g[0] = cx*order = 2
{ {
int indices[4]; int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL, mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL, NULL,
NULL, NULL, NULL, indices, NULL); NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(2, 0, 0)); EXPECT_EQ(indices[0], gidx(2, 0, 0));
EXPECT_EQ(indices[1], gidx(2, 0, 1)); EXPECT_EQ(indices[1], gidx(2, 0, 1));
EXPECT_EQ(indices[2], gidx(2, 1, 0)); EXPECT_EQ(indices[2], gidx(2, 1, 0));
@@ -1366,8 +1375,8 @@ TEST_F(FaceStateTest, NodeIndicesNonCubicGrid) {
// Total flat index = 3 + 3 + 2 = 8 // Total flat index = 3 + 3 + 2 = 8
{ {
int indices[4]; int indices[4];
mju_flexGatherFaceState(order, cx, cy, cz, 8, xpos.data(), NULL, NULL, mju_flexGatherFaceState(order, cx, cy, cz, 8, xpos.data(), NULL, NULL, NULL,
NULL, NULL, NULL, indices, NULL); NULL, NULL, indices, NULL);
EXPECT_EQ(indices[0], gidx(0, 0, 1)); EXPECT_EQ(indices[0], gidx(0, 0, 1));
EXPECT_EQ(indices[1], gidx(1, 0, 1)); EXPECT_EQ(indices[1], gidx(1, 0, 1));
EXPECT_EQ(indices[2], gidx(0, 0, 2)); EXPECT_EQ(indices[2], gidx(0, 0, 2));
@@ -1681,8 +1690,7 @@ TEST_F(ShellTFITest, BoundaryUnmodified) {
for (int i = 0; i < nx; i++) { for (int i = 0; i < nx; i++) {
for (int j = 0; j < ny; j++) { for (int j = 0; j < ny; j++) {
for (int k = 0; k < nz; k++) { for (int k = 0; k < nz; k++) {
bool is_boundary = (i == 0 || i == nx-1 || bool is_boundary = (i == 0 || i == nx - 1 || j == 0 || j == ny - 1 ||
j == 0 || j == ny-1 ||
k == 0 || k == nz - 1); k == 0 || k == nz - 1);
if (is_boundary) { if (is_boundary) {
int idx = i * ny * nz + j * nz + k; int idx = i * ny * nz + j * nz + k;
+39 -61
View File
@@ -35,13 +35,12 @@
namespace mujoco { namespace mujoco {
namespace { namespace {
using ::testing::Pointwise; using ::testing::Pointwise;
using ::testing::ElementsAre; using ::std::setw;
using ::std::string; using ::std::string;
using ::std::vector; using ::std::vector;
using ::std::setw; using ::testing::ElementsAre;
using QCQP2Test = MujocoTest; using QCQP2Test = MujocoTest;
TEST_F(QCQP2Test, DegenerateAMatrix) { TEST_F(QCQP2Test, DegenerateAMatrix) {
@@ -174,10 +173,7 @@ void randomBoxQP(int n, mjtNum* H, mjtNum* g, mjtNum* lower, mjtNum* upper,
TEST_F(BoxQPTest, UnboundedQP) { TEST_F(BoxQPTest, UnboundedQP) {
// small arrays, allocate on stack // small arrays, allocate on stack
static const int n = 2; static const int n = 2;
mjtNum H[n*n] = { mjtNum H[n * n] = {2, 0, 0, 2};
2, 0,
0, 2
};
mjtNum g[n] = {1, 3}; mjtNum g[n] = {1, 3};
mjtNum res[n] = {0, 0}; mjtNum res[n] = {0, 0};
mjtNum R[n * (n + 7)]; mjtNum R[n * (n + 7)];
@@ -209,10 +205,7 @@ TEST_F(BoxQPTest, UnboundedQP) {
TEST_F(BoxQPTest, AsymmetricUpperIgnored) { TEST_F(BoxQPTest, AsymmetricUpperIgnored) {
// small arrays, allocate on stack // small arrays, allocate on stack
static const int n = 2; static const int n = 2;
mjtNum H[n*n] = { mjtNum H[n * n] = {1, -400, 0, 1};
1, -400,
0, 1
};
mjtNum g[n] = {1, 3}; mjtNum g[n] = {1, 3};
mjtNum res[n] = {0, 0}; mjtNum res[n] = {0, 0};
mjtNum lower[n] = {-2, -2}; mjtNum lower[n] = {-2, -2};
@@ -266,9 +259,9 @@ TEST_F(BoxQPTest, UpperTrianglePoisoned) {
// solve with symmetric H to get the reference result // solve with symmetric H to get the reference result
mju_zero(res, n); mju_zero(res, n);
int nfree_ref = mju_boxQPoption(res, R, index, H, g, n, lower, upper, int nfree_ref =
maxiter, mingrad, backtrack, minstep, mju_boxQPoption(res, R, index, H, g, n, lower, upper, maxiter, mingrad,
armijo, nullptr, 0); backtrack, minstep, armijo, nullptr, 0);
ASSERT_GT(nfree_ref, -1); ASSERT_GT(nfree_ref, -1);
// save reference // save reference
@@ -285,9 +278,9 @@ TEST_F(BoxQPTest, UpperTrianglePoisoned) {
// solve again; result must match because only lower triangle should be read // solve again; result must match because only lower triangle should be read
mju_zero(res, n); mju_zero(res, n);
int nfree_poisoned = mju_boxQPoption(res, R, index, H, g, n, lower, upper, int nfree_poisoned =
maxiter, mingrad, backtrack, minstep, mju_boxQPoption(res, R, index, H, g, n, lower, upper, maxiter, mingrad,
armijo, nullptr, 0); backtrack, minstep, armijo, nullptr, 0);
EXPECT_EQ(nfree_poisoned, nfree_ref); EXPECT_EQ(nfree_poisoned, nfree_ref);
for (int i = 0; i < n; i++) { for (int i = 0; i < n; i++) {
@@ -319,7 +312,8 @@ TEST_F(BoxQPTest, BoundedQP) {
// use default options // use default options
int maxiter = 100; // maximum number of iterations int maxiter = 100; // maximum number of iterations
mjtNum mingrad = MjTol(1E-16, 1E-5); // minimum squared norm of (unclamped) gradient mjtNum mingrad =
MjTol(1E-16, 1E-5); // minimum squared norm of (unclamped) gradient
mjtNum backtrack = 0.5; // backtrack factor for decreasing stepsize mjtNum backtrack = 0.5; // backtrack factor for decreasing stepsize
mjtNum minstep = MjTol(1E-22, 1E-10); // minimum stepsize for linesearch mjtNum minstep = MjTol(1E-22, 1E-10); // minimum stepsize for linesearch
mjtNum armijo = 0.1; // Armijo parameter mjtNum armijo = 0.1; // Armijo parameter
@@ -328,9 +322,8 @@ TEST_F(BoxQPTest, BoundedQP) {
static const int logsz = 10000; static const int logsz = 10000;
char log[logsz]; char log[logsz];
int nfree = mju_boxQPoption(res, R, index, H, g, n, lower, upper, int nfree = mju_boxQPoption(res, R, index, H, g, n, lower, upper, maxiter,
maxiter, mingrad, backtrack, mingrad, backtrack, minstep, armijo, log, logsz);
minstep, armijo, log, logsz);
// ADD_FAILURE() << log; // uncomment to print `log` to error log // ADD_FAILURE() << log; // uncomment to print `log` to error log
@@ -404,9 +397,9 @@ TEST_F(BoxQPTest, BoundedQPvariations) {
mjtNum armijo = 0.1; mjtNum armijo = 0.1;
// solve box-QP with logging // solve box-QP with logging
int nfree = mju_boxQPoption(res, R, index, H, g, n, lower, upper, int nfree =
maxiter, mingrad, backtrack, mju_boxQPoption(res, R, index, H, g, n, lower, upper, maxiter,
minstep, armijo, log, logsz); mingrad, backtrack, minstep, armijo, log, logsz);
// check solution // check solution
EXPECT_GT(nfree, -1) << log; EXPECT_GT(nfree, -1) << log;
@@ -435,8 +428,8 @@ TEST_F(BoxQPTest, BoundedQPvariations) {
// average of 4.5 factorizations is expected // average of 4.5 factorizations is expected
EXPECT_LE(meanfactor, 5.0); EXPECT_LE(meanfactor, 5.0);
std::cerr << "n=" << setw(3) << n std::cerr << "n=" << setw(3) << n << ": average of " << meanfactor
<< ": average of " << meanfactor << " factorizations\n"; << " factorizations\n";
} }
mju_free(res); mju_free(res);
mju_free(R); mju_free(R);
@@ -492,7 +485,6 @@ void randomBanded(mjtNum* H, int nTotal, int nBand, int nDense, int seed,
mju_free(sqrtH); mju_free(sqrtH);
} }
// test banded-vector diagonal values // test banded-vector diagonal values
TEST_F(BandMatrixTest, Diagonal) { TEST_F(BandMatrixTest, Diagonal) {
int seed = 1; int seed = 1;
@@ -631,8 +623,8 @@ TEST_F(BandMatrixTest, Factorization) {
EXPECT_EQ(rank, nTotal); EXPECT_EQ(rank, nTotal);
// banded factorization // banded factorization
mjtNum minDiag = mju_cholFactorBand(B, nTotal, nBand, nDense, mjtNum minDiag =
diagadd, diagmul); mju_cholFactorBand(B, nTotal, nBand, nDense, diagadd, diagmul);
// expect factorization to have succeeded // expect factorization to have succeeded
EXPECT_GT(minDiag, 0); EXPECT_GT(minDiag, 0);
@@ -736,8 +728,8 @@ TEST_F(BandMatrixTest, Solve) {
using EngineUtilSolveTest = MujocoTest; using EngineUtilSolveTest = MujocoTest;
TEST_F(EngineUtilSolveTest, MjuCholFactorSymbolic) { TEST_F(EngineUtilSolveTest, MjuCholFactorSymbolic) {
mjModel* model = LoadModelFromString("<mujoco/>"); MjModelPtr model = LoadModelFromString("<mujoco/>");
mjData* d = mj_makeData(model); MjDataPtr d = MakeData(model);
// Test matrix (upper triangular, representing symmetric matrix): // Test matrix (upper triangular, representing symmetric matrix):
// [10 1 2 3] // [10 1 2 3]
@@ -760,7 +752,7 @@ TEST_F(EngineUtilSolveTest, MjuCholFactorSymbolic) {
int LT_rownnz[4], LT_rowadr[4]; int LT_rownnz[4], LT_rowadr[4];
int nnz = mju_cholFactorSymbolic(nullptr, L_rownnz, L_rowadr, nullptr, int nnz = mju_cholFactorSymbolic(nullptr, L_rownnz, L_rowadr, nullptr,
LT_rownnz, LT_rowadr, nullptr, H_rownnz, LT_rownnz, LT_rowadr, nullptr, H_rownnz,
H_rowadr, H_colind, n, d); H_rowadr, H_colind, n, d.get());
// verify counting phase outputs // verify counting phase outputs
EXPECT_EQ(nnz, 8); EXPECT_EQ(nnz, 8);
@@ -774,7 +766,8 @@ TEST_F(EngineUtilSolveTest, MjuCholFactorSymbolic) {
// phase 2: filling // phase 2: filling
int L_colind[8], LT_colind[8], LT_pos[8]; int L_colind[8], LT_colind[8], LT_pos[8];
mju_cholFactorSymbolic(L_colind, L_rownnz, L_rowadr, LT_colind, LT_rownnz, 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 // verify L_colind
EXPECT_THAT(AsVector(L_colind, 8), ElementsAre(0, 0, 1, 0, 2, 0, 2, 3)); 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; << "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' // 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' // Test for mju_cholUpdateSparse: sparse rank-one Cholesky update L'*L +/- x*x'
// Uses sparse reverse Cholesky factorization and sparse rank-one update. // Uses sparse reverse Cholesky factorization and sparse rank-one update.
TEST_F(EngineUtilSolveTest, MjuCholUpdateSparse) { TEST_F(EngineUtilSolveTest, MjuCholUpdateSparse) {
mjModel* model = LoadModelFromString("<mujoco/>"); MjModelPtr model = LoadModelFromString("<mujoco/>");
mjData* d = mj_makeData(model); MjDataPtr d = MakeData(model);
std::mt19937_64 rng; std::mt19937_64 rng;
rng.seed(123); rng.seed(123);
@@ -987,12 +977,12 @@ TEST_F(EngineUtilSolveTest, MjuCholUpdateSparse) {
// sparse Cholesky factorization (reverse-order: L'*L) // sparse Cholesky factorization (reverse-order: L'*L)
mju_cholFactorSparse(L_sparse.data(), n, 0, rownnz.data(), rowadr.data(), mju_cholFactorSparse(L_sparse.data(), n, 0, rownnz.data(), rowadr.data(),
colind.data(), d); colind.data(), d.get());
// apply sparse rank-one update // apply sparse rank-one update
int rank_sparse = mju_cholUpdateSparse( int rank_sparse = mju_cholUpdateSparse(
L_sparse.data(), x_sparse.data(), n, flg_plus, rownnz.data(), 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) EXPECT_EQ(rank_sparse, n)
<< "Sparse update rank loss for n=" << n << ", flg_plus=" << flg_plus; << "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 // Test that mju_cholFactorSymbolic + mju_cholFactorNumeric produces identical
// results to the reference implementation mju_cholFactorSparse // results to the reference implementation mju_cholFactorSparse
TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) { TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) {
mjModel* model = LoadModelFromString("<mujoco/>"); MjModelPtr model = LoadModelFromString("<mujoco/>");
mjData* d = mj_makeData(model); MjDataPtr d = MakeData(model);
// test matrix with fill-in: upper triangle structure // test matrix with fill-in: upper triangle structure
int n = 4; int n = 4;
@@ -1044,13 +1031,13 @@ TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) {
int LT_rownnz[4], LT_rowadr[4]; int LT_rownnz[4], LT_rowadr[4];
int nnz = mju_cholFactorSymbolic(nullptr, L_rownnz, L_rowadr, nullptr, int nnz = mju_cholFactorSymbolic(nullptr, L_rownnz, L_rowadr, nullptr,
LT_rownnz, LT_rowadr, nullptr, HT_rownnz, 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 // filling phase: compute L_colind, LT_colind, and LT_pos
int L_colind[16], LT_colind[16], LT_pos[16]; int L_colind[16], LT_colind[16], LT_pos[16];
mju_cholFactorSymbolic(L_colind, L_rownnz, L_rowadr, LT_colind, LT_rownnz, mju_cholFactorSymbolic(L_colind, L_rownnz, L_rowadr, LT_colind, LT_rownnz,
LT_rowadr, LT_pos, HT_rownnz, HT_rowadr, HT_colind, n, 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 // 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]; 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]; mjtNum L_new[16];
int rank_new = mju_cholFactorNumeric( int rank_new = mju_cholFactorNumeric(
L_new, n, 1e-10, L_rownnz, L_rowadr, L_colind, LT_rownnz, LT_rowadr, 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 // reference implementation: copy sparse H into L_ref, then factor in-place
mjtNum L_ref[16]; mjtNum L_ref[16];
@@ -1099,7 +1086,7 @@ TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) {
L_ref_rownnz[r] = lower_nnz; L_ref_rownnz[r] = lower_nnz;
} }
int rank_ref = mju_cholFactorSparse(L_ref, n, 1e-10, L_ref_rownnz, L_rowadr, 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 // compare results
EXPECT_EQ(rank_new, rank_ref); EXPECT_EQ(rank_new, rank_ref);
@@ -1110,9 +1097,6 @@ TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) {
for (int i = 0; i < nnz; i++) { for (int i = 0; i < nnz; i++) {
EXPECT_NEAR(L_new[i], L_ref[i], eps) << "mismatch at index " << i; EXPECT_NEAR(L_new[i], L_ref[i], eps) << "mismatch at index " << i;
} }
mj_deleteData(d);
mj_deleteModel(model);
} }
// ----------------------------- dense LU -------------------------------------- // ----------------------------- dense LU --------------------------------------
@@ -1123,10 +1107,7 @@ using DenseLUTest = MujocoTest;
TEST_F(DenseLUTest, Identity) { TEST_F(DenseLUTest, Identity) {
constexpr int n = 4; constexpr int n = 4;
mjtNum A[n * n] = { mjtNum A[n * n] = {
1, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1,
}; };
int pivot[n]; int pivot[n];
mjtNum b[n] = {1, 2, 3, 4}; mjtNum b[n] = {1, 2, 3, 4};
@@ -1146,9 +1127,7 @@ TEST_F(DenseLUTest, SmallKnown) {
// A = [2 1 1; 4 3 3; 8 7 9], b = [1; 1; 1] // 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]) // solution: x = [1; -1; 0] (verified: A*x = [2-1; 4-3; 8-7] = [1;1;1])
mjtNum A[n * n] = { mjtNum A[n * n] = {
2, 1, 1, 2, 1, 1, 4, 3, 3, 8, 7, 9,
4, 3, 3,
8, 7, 9,
}; };
int pivot[n]; int pivot[n];
mjtNum b[n] = {1, 1, 1}; mjtNum b[n] = {1, 1, 1};
@@ -1202,8 +1181,7 @@ TEST_F(DenseLUTest, RandomSPD) {
// compare // compare
mjtNum eps = MjTol(1e-15, 1e-7); mjtNum eps = MjTol(1e-15, 1e-7);
EXPECT_THAT(AsVector(x_lu.data(), n), EXPECT_THAT(AsVector(x_lu.data(), n),
Pointwise(MjNear(eps, eps), Pointwise(MjNear(eps, eps), AsVector(x_chol.data(), n)));
AsVector(x_chol.data(), n)));
} }
} }
File diff suppressed because it is too large Load Diff
+7 -13
View File
@@ -44,9 +44,7 @@ class MjvSceneTest : public MujocoTest {
} }
} }
void FreeSceneObjects() { void FreeSceneObjects() { mjv_freeScene(&scn_); }
mjv_freeScene(&scn_);
}
mjvScene scn_; mjvScene scn_;
mjvOption opt_; mjvOption opt_;
@@ -80,7 +78,6 @@ TEST_F(MjvSceneTest, UpdateScene) {
mj_deleteData(data_copy); mj_deleteData(data_copy);
mj_deleteData(data); mj_deleteData(data);
FreeSceneObjects(); FreeSceneObjects();
mj_deleteModel(model); mj_deleteModel(model);
} }
@@ -101,7 +98,6 @@ TEST_F(MjvSceneTest, UpdateSceneGeomsExhausted) {
mjv_updateScene(model, data, &opt_, &pert_, &cam_, mjCAT_ALL, &scn_); mjv_updateScene(model, data, &opt_, &pert_, &cam_, mjCAT_ALL, &scn_);
EXPECT_EQ(scn_.status, 1); EXPECT_EQ(scn_.status, 1);
EXPECT_EQ(scn_.ngeom, maxgeoms); EXPECT_EQ(scn_.ngeom, maxgeoms);
mj_deleteData(data); mj_deleteData(data);
FreeSceneObjects(); FreeSceneObjects();
mj_deleteModel(model); mj_deleteModel(model);
@@ -118,17 +114,17 @@ TEST_F(MjvSceneTest, PrincipalPointFrustumSign) {
</mujoco> </mujoco>
)"; )";
mjModel* model = LoadModelFromString(xml); MjModelPtr model = LoadModelFromString(xml);
ASSERT_THAT(model, NotNull()); ASSERT_THAT(model.get(), NotNull());
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
mj_forward(model, data); mj_forward(model.get(), data.get());
InitSceneObjects(model); InitSceneObjects(model.get());
// point camera at the fixed cam // point camera at the fixed cam
cam_.type = mjCAMERA_FIXED; cam_.type = mjCAMERA_FIXED;
cam_.fixedcamid = 0; 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 top = scn_.camera[0].frustum_top;
float bottom = scn_.camera[0].frustum_bottom; float bottom = scn_.camera[0].frustum_bottom;
@@ -148,9 +144,7 @@ TEST_F(MjvSceneTest, PrincipalPointFrustumSign) {
EXPECT_FLOAT_EQ(top, half - offset); EXPECT_FLOAT_EQ(top, half - offset);
EXPECT_FLOAT_EQ(bottom, -(half + offset)); EXPECT_FLOAT_EQ(bottom, -(half + offset));
mj_deleteData(data);
FreeSceneObjects(); FreeSceneObjects();
mj_deleteModel(model);
} }
} // namespace } // namespace
+6 -2
View File
@@ -157,7 +157,7 @@ const std::string GetModelPath(std::string_view path) { // NOLINT
return absl::StrCat("../model/", path); return absl::StrCat("../model/", path);
} }
mjModel* LoadModelFromString(std::string_view xml, char* error, MjModelPtr LoadModelFromString(std::string_view xml, char* error,
int error_size, mjVFS* vfs) { int error_size, mjVFS* vfs) {
if (error) { if (error) {
error[0] = '\0'; error[0] = '\0';
@@ -197,7 +197,11 @@ mjModel* LoadModelFromString(std::string_view xml, char* error,
} }
SetGlobalXmlSpec(spec); SetGlobalXmlSpec(spec);
return model; return MjModelPtr(model);
}
MjDataPtr MakeData(const MjModelPtr& model) {
return MjDataPtr(mj_makeData(model.get()));
} }
static void AssertModelNotNull(mjModel* model, static void AssertModelNotNull(mjModel* model,
+19 -1
View File
@@ -25,6 +25,7 @@
#include <string> #include <string>
#include <string_view> #include <string_view>
#include <vector> #include <vector>
#include <memory>
#include <gmock/gmock.h> #include <gmock/gmock.h>
#include <gtest/gtest.h> #include <gtest/gtest.h>
@@ -39,6 +40,20 @@ MJAPI mjfLogHandler _mjPRIVATE_setTlsLogHandler(mjfLogHandler handler);
namespace mujoco { namespace mujoco {
struct MjModelDeleter {
void operator()(mjModel* m) const {
mj_deleteModel(m);
}
};
using MjModelPtr = std::unique_ptr<mjModel, MjModelDeleter>;
struct MjDataDeleter {
void operator()(mjData* d) const {
mj_deleteData(d);
}
};
using MjDataPtr = std::unique_ptr<mjData, MjDataDeleter>;
// Runtime scale factor for test tolerances, controlled by MJTOL_SCALE env var. // Runtime scale factor for test tolerances, controlled by MJTOL_SCALE env var.
// Set MJTOL_SCALE=0 to run tests with zero tolerance and see actual residuals. // Set MJTOL_SCALE=0 to run tests with zero tolerance and see actual residuals.
inline mjtNum MjTolScale() { inline mjtNum MjTolScale() {
@@ -189,9 +204,12 @@ const std::string GetModelPath(std::string_view path);
// Returns a newly-allocated mjModel, loaded from the contents of xml. // Returns a newly-allocated mjModel, loaded from the contents of xml.
// On failure returns nullptr and populates the error array if present. // On failure returns nullptr and populates the error array if present.
mjModel* LoadModelFromString(std::string_view xml, char* error = nullptr, MjModelPtr LoadModelFromString(std::string_view xml, char* error = nullptr,
int error_size = 0, mjVFS* vfs = nullptr); int error_size = 0, mjVFS* vfs = nullptr);
// Returns a newly-allocated mjData, initialized using model.
MjDataPtr MakeData(const MjModelPtr& model);
// Returns a newly-allocated mjModel, loaded from the contents in model_path. // Returns a newly-allocated mjModel, loaded from the contents in model_path.
// On failure it asserts that model is null. // On failure it asserts that model is null.
mjModel* LoadModelFromPath(const char* model_path); mjModel* LoadModelFromPath(const char* model_path);
+64 -86
View File
@@ -65,23 +65,21 @@ TEST_F(PidTest, PGain) {
)"; )";
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
absl::Cleanup m_deleter = [m] { mj_deleteModel(m); };
EXPECT_EQ(m->plugin_statenum[0], 0) EXPECT_EQ(m->plugin_statenum[0], 0)
<< "Plugin should have no state variables"; << "Plugin should have no state variables";
EXPECT_EQ(m->actuator_actnum[0], 0) EXPECT_EQ(m->actuator_actnum[0], 0)
<< "Plugin should have no activation variables"; << "Plugin should have no activation variables";
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
absl::Cleanup d_deleter = [d] { mj_deleteData(d); };
// apply the same ctrl to both actuators and see if the same qpos results // apply the same ctrl to both actuators and see if the same qpos results
d->ctrl[0] = 1.0; d->ctrl[0] = 1.0;
d->ctrl[1] = 1.0; d->ctrl[1] = 1.0;
mj_step(m, d); mj_step(m.get(), d.get());
EXPECT_EQ(d->actuator_force[0], d->actuator_force[1]); EXPECT_EQ(d->actuator_force[0], d->actuator_force[1]);
EXPECT_EQ(d->qfrc_actuator[0], d->qfrc_actuator[1]); EXPECT_EQ(d->qfrc_actuator[0], d->qfrc_actuator[1]);
@@ -120,24 +118,22 @@ TEST_F(PidTest, PGainWithFilterExact) {
)"; )";
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
absl::Cleanup m_deleter = [m] { mj_deleteModel(m); };
EXPECT_EQ(m->plugin_statenum[0], 0) EXPECT_EQ(m->plugin_statenum[0], 0)
<< "Plugin should have no state variables"; << "Plugin should have no state variables";
EXPECT_EQ(m->actuator_actnum[0], 1) EXPECT_EQ(m->actuator_actnum[0], 1)
<< "Plugin should have one activation variable"; << "Plugin should have one activation variable";
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
absl::Cleanup d_deleter = [d] { mj_deleteData(d); };
// apply the same ctrl to both actuators and see if the same qpos results // apply the same ctrl to both actuators and see if the same qpos results
d->ctrl[0] = 1.0; d->ctrl[0] = 1.0;
d->ctrl[1] = 1.0; d->ctrl[1] = 1.0;
for (int i = 0; i < 2; i++) { for (int i = 0; i < 2; i++) {
mj_step(m, d); mj_step(m.get(), d.get());
EXPECT_EQ(d->actuator_force[0], d->actuator_force[1]); EXPECT_EQ(d->actuator_force[0], d->actuator_force[1]);
EXPECT_EQ(d->qfrc_actuator[0], d->qfrc_actuator[1]); EXPECT_EQ(d->qfrc_actuator[0], d->qfrc_actuator[1]);
@@ -178,27 +174,25 @@ TEST_F(PidTest, SlewMaxRate) {
)"; )";
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
absl::Cleanup m_deleter = [m] { mj_deleteModel(m); };
// having a slew rate means that there should be one extra state variable // having a slew rate means that there should be one extra state variable
// for the plugin. // for the plugin.
EXPECT_EQ(m->actuator_actnum[0], 1); EXPECT_EQ(m->actuator_actnum[0], 1);
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
absl::Cleanup d_deleter = [d] { mj_deleteData(d); };
// going from ctrl=0.0 to ctrl=1.0 immediately should be equivalent to slowly // going from ctrl=0.0 to ctrl=1.0 immediately should be equivalent to slowly
// incrementing the setpoint // incrementing the setpoint
d->ctrl[0] = d->ctrl[1] = 0.0; d->ctrl[0] = d->ctrl[1] = 0.0;
mj_step(m, d); mj_step(m.get(), d.get());
mjtNum max_slew_rate = 0.75; mjtNum max_slew_rate = 0.75;
for (int i = 0; i < 2; i++) { for (int i = 0; i < 2; i++) {
d->ctrl[0] = 1.0; d->ctrl[0] = 1.0;
d->ctrl[1] = d->time * max_slew_rate; d->ctrl[1] = d->time * max_slew_rate;
mj_step(m, d); mj_step(m.get(), d.get());
EXPECT_EQ(d->actuator_force[0], d->actuator_force[1]) EXPECT_EQ(d->actuator_force[0], d->actuator_force[1])
<< "actuator_force mismatch at step " << i; << "actuator_force mismatch at step " << i;
@@ -246,9 +240,8 @@ TEST_F(PidTest, IntegratedVelocitySlewMaxRate) {
)"; )";
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
absl::Cleanup m_deleter = [m] { mj_deleteModel(m); };
// having a slew rate means that there should be one extra state variable // having a slew rate means that there should be one extra state variable
// for the plugin. // for the plugin.
@@ -256,22 +249,20 @@ TEST_F(PidTest, IntegratedVelocitySlewMaxRate) {
// The integrated-velocity controller should have one activation variable too. // The integrated-velocity controller should have one activation variable too.
ASSERT_EQ(m->actuator_actnum[1], 1); ASSERT_EQ(m->actuator_actnum[1], 1);
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
absl::Cleanup d_deleter = [d] { mj_deleteData(d); };
// going from ctrl=0.0 to ctrl=1.0 immediately should be equivalent to using // going from ctrl=0.0 to ctrl=1.0 immediately should be equivalent to using
// an integrated-velocity controller, with actearly = true // an integrated-velocity controller, with actearly = true
d->ctrl[0] = d->ctrl[1] = 0.0; d->ctrl[0] = d->ctrl[1] = 0.0;
mj_step(m, d); mj_step(m.get(), d.get());
mjtNum max_slew_rate = 0.75; mjtNum max_slew_rate = 0.75;
for (int i = 0; i < 2; i++) { for (int i = 0; i < 2; i++) {
d->ctrl[0] = 1.0; d->ctrl[0] = 1.0;
d->ctrl[1] = max_slew_rate; d->ctrl[1] = max_slew_rate;
mj_step(m, d); mj_step(m.get(), d.get());
EXPECT_EQ(d->act[0], d->act[1]) EXPECT_EQ(d->act[0], d->act[1]) << "act mismatch at step " << i;
<< "act mismatch at step " << i;
EXPECT_EQ(d->actuator_force[0], d->actuator_force[1]) EXPECT_EQ(d->actuator_force[0], d->actuator_force[1])
<< "actuator_force mismatch at step " << i; << "actuator_force mismatch at step " << i;
EXPECT_EQ(d->qfrc_actuator[0], d->qfrc_actuator[1]) EXPECT_EQ(d->qfrc_actuator[0], d->qfrc_actuator[1])
@@ -308,16 +299,14 @@ TEST_F(PidTest, SlewMaxRateUsesFirstCtrl) {
)"; )";
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
absl::Cleanup m_deleter = [m] { mj_deleteModel(m); };
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
absl::Cleanup d_deleter = [d] { mj_deleteData(d); };
// When starting with ctrl = 1.0, there shouldn't be a slew rate restriction // When starting with ctrl = 1.0, there shouldn't be a slew rate restriction
d->ctrl[0] = 1.0; d->ctrl[0] = 1.0;
mj_forward(m, d); mj_forward(m.get(), d.get());
EXPECT_EQ(d->actuator_force[0], 4.0); EXPECT_EQ(d->actuator_force[0], 4.0);
} }
@@ -374,17 +363,15 @@ TEST_F(PidTest, ITerm) {
)"; )";
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
absl::Cleanup m_deleter = [m] { mj_deleteModel(m); };
// only the actuators with an I term should have state. // only the actuators with an I term should have state.
EXPECT_EQ(m->actuator_actnum[0], 0); EXPECT_EQ(m->actuator_actnum[0], 0);
EXPECT_EQ(m->actuator_actnum[1], 1); EXPECT_EQ(m->actuator_actnum[1], 1);
EXPECT_EQ(m->actuator_actnum[2], 1); EXPECT_EQ(m->actuator_actnum[2], 1);
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
absl::Cleanup d_deleter = [d] { mj_deleteData(d); };
// when applying a constant 1.0 control for a while: // when applying a constant 1.0 control for a while:
// - the PD controller should settle to 1.0 - m*g / kp // - the PD controller should settle to 1.0 - m*g / kp
@@ -392,7 +379,7 @@ TEST_F(PidTest, ITerm) {
// - the clamped PID controller should reach 1.0 - (m*g - imax) / kp // - the clamped PID controller should reach 1.0 - (m*g - imax) / kp
d->ctrl[0] = d->ctrl[1] = d->ctrl[2] = 1.0; d->ctrl[0] = d->ctrl[1] = d->ctrl[2] = 1.0;
for (int i = 0; i < 10000; i++) { for (int i = 0; i < 10000; i++) {
mj_step(m, d); mj_step(m.get(), d.get());
} }
EXPECT_THAT(d->qpos[0], MjNear(1.0 - 10 / 40.0, 1e-5, 1e-5)); EXPECT_THAT(d->qpos[0], MjNear(1.0 - 10 / 40.0, 1e-5, 1e-5));
@@ -423,19 +410,17 @@ TEST_F(PidTest, FiniteDifferencing) {
)"; )";
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
absl::Cleanup m_deleter = [m] { mj_deleteModel(m); };
// actuators with an I term and max slew rate should have 2 activation // actuators with an I term and max slew rate should have 2 activation
// variables. // variables.
ASSERT_EQ(m->actuator_actnum[0], 2); ASSERT_EQ(m->actuator_actnum[0], 2);
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
absl::Cleanup d_deleter = [d] { mj_deleteData(d); };
d->ctrl[0] = 1.0; d->ctrl[0] = 1.0;
mj_step(m, d); mj_step(m.get(), d.get());
EXPECT_NE(d->act[0], 0); EXPECT_NE(d->act[0], 0);
mjtNum state_before_finite_differencing = d->act[0]; mjtNum state_before_finite_differencing = d->act[0];
@@ -443,8 +428,8 @@ TEST_F(PidTest, FiniteDifferencing) {
std::vector<mjtNum> B((2 * m->nv + m->na) * m->nu, 0); std::vector<mjtNum> B((2 * m->nv + m->na) * m->nu, 0);
std::vector<mjtNum> C((m->nsensordata) * (2 * m->nv + m->na), 0); std::vector<mjtNum> C((m->nsensordata) * (2 * m->nv + m->na), 0);
std::vector<mjtNum> D((m->nsensordata) * m->nu, 0); std::vector<mjtNum> D((m->nsensordata) * m->nu, 0);
mjd_transitionFD(m, d, /*eps=*/1e-3, /*flg_centered=*/true, A.data(), mjd_transitionFD(m.get(), d.get(), /*eps=*/1e-3, /*flg_centered=*/true,
B.data(), C.data(), D.data()); A.data(), B.data(), C.data(), D.data());
EXPECT_EQ(d->act[0], state_before_finite_differencing) EXPECT_EQ(d->act[0], state_before_finite_differencing)
<< "mjd_transitionFD should not change actuator plugin state."; << "mjd_transitionFD should not change actuator plugin state.";
@@ -481,25 +466,23 @@ TEST_F(PidTest, CtrlClamp) {
)"; )";
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
absl::Cleanup m_deleter = [m] { mj_deleteModel(m); };
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
absl::Cleanup d_deleter = [d] { mj_deleteData(d); };
// when applying a constant 1.0 control for a while, control should be clamped // when applying a constant 1.0 control for a while, control should be clamped
// to 0.75, and the body should reach that point. // to 0.75, and the body should reach that point.
d->ctrl[0] = 1.0; d->ctrl[0] = 1.0;
for (int i = 0; i < 10000; i++) { for (int i = 0; i < 10000; i++) {
mj_step(m, d); mj_step(m.get(), d.get());
} }
EXPECT_THAT(d->qpos[0], MjNear(0.75, 1e-5, 1e-5)); EXPECT_THAT(d->qpos[0], MjNear(0.75, 1e-5, 1e-5));
// when applying 0, it should be clamped to 0.25 // when applying 0, it should be clamped to 0.25
d->ctrl[0] = 0.0; d->ctrl[0] = 0.0;
for (int i = 0; i < 10000; i++) { for (int i = 0; i < 10000; i++) {
mj_step(m, d); mj_step(m.get(), d.get());
} }
EXPECT_THAT(d->qpos[0], MjNear(0.25, 1e-5, 1e-5)); EXPECT_THAT(d->qpos[0], MjNear(0.25, 1e-5, 1e-5));
} }
@@ -544,23 +527,21 @@ TEST_F(PidTest, CopyData) {
)"; )";
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
absl::Cleanup m_deleter = [m] { mj_deleteModel(m); };
mjData* d1 = mj_makeData(m); MjDataPtr d1 = MakeData(m);
absl::Cleanup d1_deleter = [d1] { mj_deleteData(d1); };
d1->ctrl[0] = d1->ctrl[1] = -1.0; d1->ctrl[0] = d1->ctrl[1] = -1.0;
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
mj_step(m, d1); mj_step(m.get(), d1.get());
} }
mjData* d2 = mj_copyData(nullptr, m, d1); mjData* d2 = mj_copyData(nullptr, m.get(), d1.get());
absl::Cleanup d2_deleter = [d2] { mj_deleteData(d2); }; absl::Cleanup d2_deleter = [d2] { mj_deleteData(d2); };
mj_step(m, d1); mj_step(m.get(), d1.get());
mj_step(m, d2); mj_step(m.get(), d2);
EXPECT_EQ(d1->qpos[0], d2->qpos[0]); EXPECT_EQ(d1->qpos[0], d2->qpos[0]);
EXPECT_EQ(d1->qpos[1], d2->qpos[1]); EXPECT_EQ(d1->qpos[1], d2->qpos[1]);
@@ -599,17 +580,15 @@ TEST_F(PidTest, MultipleActuatorsSamePlugin) {
)"; )";
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
absl::Cleanup m_deleter = [m] { mj_deleteModel(m); };
// having a slew rate means that there should be one extra state variable // having a slew rate means that there should be one extra state variable
// for the plugin. // for the plugin.
EXPECT_EQ(m->actuator_actnum[0], 1); EXPECT_EQ(m->actuator_actnum[0], 1);
EXPECT_EQ(m->actuator_actnum[1], 1); EXPECT_EQ(m->actuator_actnum[1], 1);
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
absl::Cleanup d_deleter = [d] { mj_deleteData(d); };
// Set different ctrls for the two actuators, and check that they're // Set different ctrls for the two actuators, and check that they're
// independent. // independent.
@@ -617,7 +596,7 @@ TEST_F(PidTest, MultipleActuatorsSamePlugin) {
d->ctrl[1] = -1.0; d->ctrl[1] = -1.0;
for (int i = 0; i < 2; i++) { for (int i = 0; i < 2; i++) {
mj_step(m, d); mj_step(m.get(), d.get());
EXPECT_EQ(d->actuator_force[0], -d->actuator_force[1]) EXPECT_EQ(d->actuator_force[0], -d->actuator_force[1])
<< "actuator_force mismatch at step " << i; << "actuator_force mismatch at step " << i;
@@ -649,8 +628,8 @@ TEST_F(PidTest, InvalidClamp) {
)"; )";
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
EXPECT_THAT(m, IsNull()); EXPECT_THAT(m.get(), IsNull());
EXPECT_THAT(std::string_view(error), HasSubstr("plugin")); EXPECT_THAT(std::string_view(error), HasSubstr("plugin"));
EXPECT_THAT(std::string_view(error), HasSubstr("imax")); EXPECT_THAT(std::string_view(error), HasSubstr("imax"));
@@ -680,8 +659,8 @@ TEST_F(PidTest, InvalidSlew) {
)"; )";
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
ASSERT_THAT(m, IsNull()); ASSERT_THAT(m.get(), IsNull());
EXPECT_THAT(std::string_view(error), HasSubstr("plugin")); EXPECT_THAT(std::string_view(error), HasSubstr("plugin"));
EXPECT_THAT(std::string_view(error), HasSubstr("slewmax")); EXPECT_THAT(std::string_view(error), HasSubstr("slewmax"));
@@ -717,24 +696,25 @@ TEST_F(PidTest, WrongActdim) {
{ {
std::string no_actdim = std::string no_actdim =
absl::StrReplaceAll(kBaseXml, {{"PLACEHOLDER", ""}}); absl::StrReplaceAll(kBaseXml, {{"PLACEHOLDER", ""}});
mjModel* m = LoadModelFromString(no_actdim, error, sizeof(error)); MjModelPtr m = LoadModelFromString(no_actdim, error, sizeof(error));
EXPECT_THAT(m, IsNull()); EXPECT_THAT(m.get(), IsNull());
EXPECT_THAT(std::string_view(error), HasSubstr("actdim=\"1\"")); EXPECT_THAT(std::string_view(error), HasSubstr("actdim=\"1\""));
} }
{ {
std::string big_actdim = std::string big_actdim =
absl::StrReplaceAll(kBaseXml, {{"PLACEHOLDER", "actdim=\"2\""}}); absl::StrReplaceAll(kBaseXml, {{"PLACEHOLDER", "actdim=\"2\""}});
mjModel* m = LoadModelFromString(big_actdim, error, sizeof(error)); MjModelPtr m = LoadModelFromString(big_actdim, error, sizeof(error));
EXPECT_THAT(m, IsNull()); EXPECT_THAT(m.get(), IsNull());
EXPECT_THAT(std::string_view(error), HasSubstr("actdim=\"1\"")); EXPECT_THAT(std::string_view(error), HasSubstr("actdim=\"1\""));
} }
{ {
std::string dyntype_integrator = absl::StrReplaceAll( std::string dyntype_integrator = absl::StrReplaceAll(
kBaseXml, {{"PLACEHOLDER", "dyntype=\"integrator\" actdim=\"1\""}}); kBaseXml, {{"PLACEHOLDER", "dyntype=\"integrator\" actdim=\"1\""}});
mjModel* m = LoadModelFromString(dyntype_integrator, error, sizeof(error)); MjModelPtr m =
EXPECT_THAT(m, IsNull()); LoadModelFromString(dyntype_integrator, error, sizeof(error));
EXPECT_THAT(m.get(), IsNull());
EXPECT_THAT(std::string_view(error), HasSubstr("actdim=\"2\"")); EXPECT_THAT(std::string_view(error), HasSubstr("actdim=\"2\""));
} }
} }
@@ -769,9 +749,8 @@ TEST_F(PidTest, Keyframe) {
)"; )";
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(kModelXml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(kModelXml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error; ASSERT_THAT(m.get(), NotNull()) << error;
absl::Cleanup m_deleter = [m] { mj_deleteModel(m); };
// having a slew rate means that there should be one extra state variable // having a slew rate means that there should be one extra state variable
// for the plugin. // for the plugin.
@@ -780,10 +759,9 @@ TEST_F(PidTest, Keyframe) {
ASSERT_EQ(m->nkey, 1); ASSERT_EQ(m->nkey, 1);
EXPECT_EQ(m->key_act[0], 1.0); EXPECT_EQ(m->key_act[0], 1.0);
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
absl::Cleanup d_deleter = [d] { mj_deleteData(d); };
mj_resetDataKeyframe(m, d, 0); mj_resetDataKeyframe(m.get(), d.get(), 0);
EXPECT_EQ(d->act[0], 1.0); EXPECT_EQ(d->act[0], 1.0);
} }
} // namespace } // namespace
+13 -16
View File
@@ -28,7 +28,6 @@ namespace {
using ElasticityTest = MujocoTest; using ElasticityTest = MujocoTest;
// -------------------------------- cable ----------------------------------- // -------------------------------- cable -----------------------------------
TEST_F(ElasticityTest, CantileverIntoCircle) { TEST_F(ElasticityTest, CantileverIntoCircle) {
#ifdef mjUSESINGLE #ifdef mjUSESINGLE
@@ -66,25 +65,24 @@ TEST_F(ElasticityTest, CantileverIntoCircle) {
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error; ASSERT_THAT(m.get(), testing::NotNull()) << error;
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
// see Oliver Weeger, Sai-Kit Yeung, Martin L. Dunn, "Isogeometric collocation methods for Cosserat rods and rod // see Oliver Weeger, Sai-Kit Yeung, Martin L. Dunn, "Isogeometric collocation
// structures", section 7.1 (DOI: j.cma.2016.05.009), the torque for achieving an angle phi is phi * E * Iy. // methods for Cosserat rods and rod structures", section 7.1 (DOI:
// j.cma.2016.05.009), the torque for achieving an angle phi is phi * E * Iy.
mjtNum Iy = mjPI * pow(0.005, 4) / 4; mjtNum Iy = mjPI * pow(0.005, 4) / 4;
mjtNum torque = 2 * mjPI * 1e9 * Iy; mjtNum torque = 2 * mjPI * 1e9 * Iy;
for (int i = 0; i < 1300; i++) { for (int i = 0; i < 1300; i++) {
if (i < 300) { if (i < 300) {
d->ctrl[0] += torque / 300; d->ctrl[0] += torque / 300;
} }
mj_step(m, d); mj_step(m.get(), d.get());
} }
EXPECT_NEAR(d->sensordata[0], 0, std::numeric_limits<float>::epsilon()); EXPECT_NEAR(d->sensordata[0], 0, std::numeric_limits<float>::epsilon());
EXPECT_NEAR(d->sensordata[1], 0, std::numeric_limits<float>::epsilon()); EXPECT_NEAR(d->sensordata[1], 0, std::numeric_limits<float>::epsilon());
EXPECT_NEAR(d->sensordata[2], 1, std::numeric_limits<float>::epsilon()); EXPECT_NEAR(d->sensordata[2], 1, std::numeric_limits<float>::epsilon());
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(ElasticityTest, InvalidTxtAttribute) { TEST_F(ElasticityTest, InvalidTxtAttribute) {
@@ -106,8 +104,8 @@ TEST_F(ElasticityTest, InvalidTxtAttribute) {
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
ASSERT_THAT(m, testing::IsNull()); ASSERT_THAT(m.get(), testing::IsNull());
} }
TEST_F(ElasticityTest, InvalidMixedAttribute) { TEST_F(ElasticityTest, InvalidMixedAttribute) {
@@ -129,8 +127,8 @@ TEST_F(ElasticityTest, InvalidMixedAttribute) {
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
ASSERT_THAT(m, testing::IsNull()); ASSERT_THAT(m.get(), testing::IsNull());
} }
TEST_F(ElasticityTest, ValidAttributes) { TEST_F(ElasticityTest, ValidAttributes) {
@@ -152,9 +150,8 @@ TEST_F(ElasticityTest, ValidAttributes) {
char error[1024] = {0}; char error[1024] = {0};
mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error)); MjModelPtr m = LoadModelFromString(cantilever_xml, error, sizeof(error));
ASSERT_THAT(m, testing::NotNull()) << error; ASSERT_THAT(m.get(), testing::NotNull()) << error;
mj_deleteModel(m);
} }
} // namespace } // namespace
+128 -143
View File
@@ -24,12 +24,12 @@
#include <string> #include <string>
#include <vector> #include <vector>
#include <absl/strings/str_format.h>
#include <gmock/gmock.h> #include <gmock/gmock.h>
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <mujoco/mujoco.h> #include <absl/strings/str_format.h>
#include <mujoco/mjspec.h>
#include <mujoco/mjplugin.h> #include <mujoco/mjplugin.h>
#include <mujoco/mjspec.h>
#include <mujoco/mujoco.h>
#include "src/xml/xml_api.h" #include "src/xml/xml_api.h"
#include "test/compare_model.h" #include "test/compare_model.h"
#include "test/fixture.h" #include "test/fixture.h"
@@ -228,7 +228,6 @@ TEST_F(MujocoTest, OriginSpecInvariantToAttachment) {
mjs_findElement(parent_spec, mjOBJ_BODY, "parent_body"); mjs_findElement(parent_spec, mjOBJ_BODY, "parent_body");
EXPECT_EQ(mjs_getOriginSpec(parent_spec_body), parent_spec); EXPECT_EQ(mjs_getOriginSpec(parent_spec_body), parent_spec);
mj_deleteSpec(child_spec); mj_deleteSpec(child_spec);
mj_deleteSpec(parent_spec); mj_deleteSpec(parent_spec);
} }
@@ -404,10 +403,9 @@ TEST_F(MujocoTest, SetToDCMotorNullable) {
double motorconst[2] = {0.05, 0.05}; double motorconst[2] = {0.05, 0.05};
double resistance = 2.0; double resistance = 2.0;
const char* err = mjs_setToDCMotor(actuator, motorconst, resistance, const char* err =
nullptr, nullptr, nullptr, mjs_setToDCMotor(actuator, motorconst, resistance, nullptr, nullptr,
nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, 0);
nullptr, 0);
EXPECT_STREQ(err, ""); EXPECT_STREQ(err, "");
EXPECT_EQ(actuator->gainprm[0], 2.0); EXPECT_EQ(actuator->gainprm[0], 2.0);
EXPECT_EQ(actuator->gainprm[1], 0.05); EXPECT_EQ(actuator->gainprm[1], 0.05);
@@ -427,10 +425,9 @@ TEST_F(MujocoTest, SetToDCMotorDeriveKe) {
double resistance = 2.0; double resistance = 2.0;
double nominal[3] = {12.0, 0, 100.0}; // vn=12, omega0=100 double nominal[3] = {12.0, 0, 100.0}; // vn=12, omega0=100
const char* err = mjs_setToDCMotor(actuator, nullptr, resistance, const char* err =
nominal, nullptr, nullptr, mjs_setToDCMotor(actuator, nullptr, resistance, nominal, nullptr, nullptr,
nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, 0);
nullptr, 0);
EXPECT_STREQ(err, ""); EXPECT_STREQ(err, "");
EXPECT_EQ(actuator->gainprm[0], 2.0); EXPECT_EQ(actuator->gainprm[0], 2.0);
EXPECT_NEAR(actuator->gainprm[1], 0.12, 1e-5); EXPECT_NEAR(actuator->gainprm[1], 0.12, 1e-5);
@@ -447,10 +444,9 @@ TEST_F(MujocoTest, SetToDCMotorFull) {
double saturation[3] = {1.0, 2.0, 3.0}; double saturation[3] = {1.0, 2.0, 3.0};
double controller[6] = {10.0, 20.0, 30.0, 40.0, 50.0, 60.0}; double controller[6] = {10.0, 20.0, 30.0, 40.0, 50.0, 60.0};
const char* err = mjs_setToDCMotor(actuator, motorconst, resistance, const char* err =
nullptr, saturation, nullptr, mjs_setToDCMotor(actuator, motorconst, resistance, nullptr, saturation,
nullptr, controller, nullptr, nullptr, nullptr, controller, nullptr, nullptr, 0);
nullptr, 0);
EXPECT_STREQ(err, ""); EXPECT_STREQ(err, "");
EXPECT_EQ(actuator->gainprm[0], 2.0); // resistance EXPECT_EQ(actuator->gainprm[0], 2.0); // resistance
EXPECT_EQ(actuator->gainprm[1], 0.05); // K EXPECT_EQ(actuator->gainprm[1], 0.05); // K
@@ -473,10 +469,9 @@ TEST_F(MujocoTest, SetToDCMotorLuGre) {
double resistance = 2.0; double resistance = 2.0;
double lugre[5] = {100.0, 1.0, 0.5, 0.7, 10.0}; double lugre[5] = {100.0, 1.0, 0.5, 0.7, 10.0};
const char* err = mjs_setToDCMotor(actuator, motorconst, resistance, const char* err =
nullptr, nullptr, nullptr, mjs_setToDCMotor(actuator, motorconst, resistance, nullptr, nullptr,
nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, lugre, 0);
lugre, 0);
EXPECT_STREQ(err, ""); EXPECT_STREQ(err, "");
EXPECT_EQ(actuator->dynprm[5], 100.0); // stiffness EXPECT_EQ(actuator->dynprm[5], 100.0); // stiffness
EXPECT_EQ(actuator->dynprm[6], 1.0); // damping EXPECT_EQ(actuator->dynprm[6], 1.0); // damping
@@ -764,7 +759,6 @@ TEST_F(MujocoTest, ModifyShellInertiaFails) {
mj_deleteModel(model); mj_deleteModel(model);
} }
TEST_F(MujocoTest, RecompileEdit) { TEST_F(MujocoTest, RecompileEdit) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
@@ -837,43 +831,40 @@ TEST_F(MujocoTest, RecompileCompareObjCache) {
std::array<char, 1024> error; std::array<char, 1024> error;
// load model once // load model once
mjModel* m = LoadModelFromString(xml, error.data(), error.size(), vfs.get()); MjModelPtr m =
LoadModelFromString(xml, error.data(), error.size(), vfs.get());
EXPECT_EQ(m->mesh_vert[0], -0.5); EXPECT_EQ(m->mesh_vert[0], -0.5);
mj_deleteModel(m);
// update cube.obj, load again // update cube.obj, load again
mj_deleteFileVFS(vfs.get(), "cube.obj"); mj_deleteFileVFS(vfs.get(), "cube.obj");
mj_addBufferVFS(vfs.get(), "cube.obj", cube2, sizeof(cube2)); mj_addBufferVFS(vfs.get(), "cube.obj", cube2, sizeof(cube2));
m = LoadModelFromString(xml, error.data(), error.size(), vfs.get()); m = LoadModelFromString(xml, error.data(), error.size(), vfs.get());
EXPECT_EQ(m->mesh_vert[0], -1); EXPECT_EQ(m->mesh_vert[0], -1);
mj_deleteModel(m);
mj_deleteVFS(vfs.get()); mj_deleteVFS(vfs.get());
} }
// tiny RGB 2 x 3 PNG file // tiny RGB 2 x 3 PNG file
static constexpr uint8_t tex1[] = { static constexpr uint8_t tex1[] = {
0x89, 0x50, 0x4e, 0x47, 0x0d, 0x0a, 0x1a, 0x0a, 0x00, 0x00, 0x00, 0x0d, 0x89, 0x50, 0x4e, 0x47, 0x0d, 0x0a, 0x1a, 0x0a, 0x00, 0x00, 0x00,
0x49, 0x48, 0x44, 0x52, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x02, 0x0d, 0x49, 0x48, 0x44, 0x52, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00,
0x08, 0x02, 0x00, 0x00, 0x00, 0x12, 0x16, 0xf1, 0x4d, 0x00, 0x00, 0x00, 0x00, 0x02, 0x08, 0x02, 0x00, 0x00, 0x00, 0x12, 0x16, 0xf1, 0x4d,
0x1c, 0x49, 0x44, 0x41, 0x54, 0x08, 0xd7, 0x63, 0x78, 0xc1, 0xc0, 0xc0, 0x00, 0x00, 0x00, 0x1c, 0x49, 0x44, 0x41, 0x54, 0x08, 0xd7, 0x63,
0xc0, 0xf0, 0xbf, 0xb8, 0xb8, 0x98, 0x81, 0xe1, 0x3f, 0xc3, 0xff, 0xff, 0x78, 0xc1, 0xc0, 0xc0, 0xc0, 0xf0, 0xbf, 0xb8, 0xb8, 0x98, 0x81,
0xff, 0xc5, 0xc4, 0xc4, 0x00, 0x46, 0xd7, 0x07, 0x7f, 0xd2, 0x52, 0xa1, 0xe1, 0x3f, 0xc3, 0xff, 0xff, 0xff, 0xc5, 0xc4, 0xc4, 0x00, 0x46,
0x41, 0x00, 0x00, 0x00, 0x00, 0x49, 0x45, 0x4e, 0x44, 0xae, 0x42, 0x60, 0xd7, 0x07, 0x7f, 0xd2, 0x52, 0xa1, 0x41, 0x00, 0x00, 0x00, 0x00,
0x82 0x49, 0x45, 0x4e, 0x44, 0xae, 0x42, 0x60, 0x82};
};
// previous PNG file, but rotated by 180 degrees // previous PNG file, but rotated by 180 degrees
static constexpr uint8_t tex2[] = { static constexpr uint8_t tex2[] = {
0x89, 0x50, 0x4e, 0x47, 0x0d, 0x0a, 0x1a, 0x0a, 0x00, 0x00, 0x00, 0x0d, 0x89, 0x50, 0x4e, 0x47, 0x0d, 0x0a, 0x1a, 0x0a, 0x00, 0x00, 0x00,
0x49, 0x48, 0x44, 0x52, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x02, 0x0d, 0x49, 0x48, 0x44, 0x52, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00,
0x08, 0x02, 0x00, 0x00, 0x00, 0x12, 0x16, 0xf1, 0x4d, 0x00, 0x00, 0x00, 0x00, 0x02, 0x08, 0x02, 0x00, 0x00, 0x00, 0x12, 0x16, 0xf1, 0x4d,
0x1c, 0x49, 0x44, 0x41, 0x54, 0x08, 0xd7, 0x63, 0x10, 0x13, 0x13, 0xfb, 0x00, 0x00, 0x00, 0x1c, 0x49, 0x44, 0x41, 0x54, 0x08, 0xd7, 0x63,
0xff, 0xff, 0x3f, 0xc3, 0x7f, 0x06, 0x96, 0xd8, 0xd8, 0x58, 0x46, 0x46, 0x10, 0x13, 0x13, 0xfb, 0xff, 0xff, 0x3f, 0xc3, 0x7f, 0x06, 0x96,
0x86, 0x17, 0x0c, 0x0c, 0x00, 0x49, 0x22, 0x06, 0x44, 0xe4, 0x91, 0xb8, 0xd8, 0xd8, 0x58, 0x46, 0x46, 0x86, 0x17, 0x0c, 0x0c, 0x00, 0x49,
0x83, 0x00, 0x00, 0x00, 0x00, 0x49, 0x45, 0x4e, 0x44, 0xae, 0x42, 0x60, 0x22, 0x06, 0x44, 0xe4, 0x91, 0xb8, 0x83, 0x00, 0x00, 0x00, 0x00,
0x82 0x49, 0x45, 0x4e, 0x44, 0xae, 0x42, 0x60, 0x82};
};
TEST_F(MujocoTest, RecompileComparePngCache) { TEST_F(MujocoTest, RecompileComparePngCache) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
@@ -896,10 +887,10 @@ TEST_F(MujocoTest, RecompileComparePngCache) {
std::array<char, 1024> error; std::array<char, 1024> error;
// load model once // load model once
mjModel* m = LoadModelFromString(xml, error.data(), error.size(), vfs.get()); MjModelPtr m =
LoadModelFromString(xml, error.data(), error.size(), vfs.get());
EXPECT_EQ(m->ntexdata, 18); // w x h x rgb = 3 x 2 x 3 EXPECT_EQ(m->ntexdata, 18); // w x h x rgb = 3 x 2 x 3
mjtByte byte = m->tex_data[0]; mjtByte byte = m->tex_data[0];
mj_deleteModel(m);
// update tex.png, load again // update tex.png, load again
mj_deleteFileVFS(vfs.get(), "tex.png"); mj_deleteFileVFS(vfs.get(), "tex.png");
@@ -907,7 +898,6 @@ TEST_F(MujocoTest, RecompileComparePngCache) {
m = LoadModelFromString(xml, error.data(), error.size(), vfs.get()); m = LoadModelFromString(xml, error.data(), error.size(), vfs.get());
EXPECT_NE(m->tex_data[0], byte); EXPECT_NE(m->tex_data[0], byte);
EXPECT_EQ(m->tex_data[15], byte); // first pixel is now last pixel EXPECT_EQ(m->tex_data[15], byte); // first pixel is now last pixel
mj_deleteModel(m);
mj_deleteVFS(vfs.get()); mj_deleteVFS(vfs.get());
} }
@@ -937,14 +927,13 @@ TEST_F(MujocoTest, DisableCache) {
std::array<char, 1024> error; std::array<char, 1024> error;
// load model once // load model once
mjModel* m = LoadModelFromString(xml, error.data(), error.size(), vfs.get()); MjModelPtr m =
LoadModelFromString(xml, error.data(), error.size(), vfs.get());
EXPECT_EQ(mj_getCacheSize(cache), 0); EXPECT_EQ(mj_getCacheSize(cache), 0);
EXPECT_EQ(m->ntexdata, 18); // w x h x rgb = 3 x 2 x 3 EXPECT_EQ(m->ntexdata, 18); // w x h x rgb = 3 x 2 x 3
mj_setCacheCapacity(cache, capacity); mj_setCacheCapacity(cache, capacity);
mj_deleteModel(m);
mj_deleteVFS(vfs.get()); mj_deleteVFS(vfs.get());
} }
@@ -1226,16 +1215,16 @@ TEST_F(MujocoTest, AttachSame) {
EXPECT_THAT(m_attached->body_parentid[4], 0); EXPECT_THAT(m_attached->body_parentid[4], 0);
// compare with expected XML // compare with expected XML
mjModel* m_expected = LoadModelFromString(xml_result, er.data(), er.size()); MjModelPtr m_expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(m_expected, NotNull()) << er.data(); EXPECT_THAT(m_expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(m_attached, m_expected, field), tol) EXPECT_LE(CompareModel(m_attached, m_expected.get(), field), tol)
<< "Expected and attached models are different!\n" << "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';; << "Different field: " << field << '\n';
;
// destroy everything // destroy everything
mj_deleteSpec(parent); mj_deleteSpec(parent);
mj_deleteModel(m_attached); mj_deleteModel(m_attached);
mj_deleteModel(m_expected);
} }
TEST_F(MujocoTest, AttachSpatialTendonWithoutSidesite) { TEST_F(MujocoTest, AttachSpatialTendonWithoutSidesite) {
@@ -1283,13 +1272,15 @@ TEST_F(MujocoTest, AttachSpatialTendonWithoutSidesite) {
mjsSite* attach_site = mjs_addSite(parent_body, 0); mjsSite* attach_site = mjs_addSite(parent_body, 0);
mjs_setName(attach_site->element, "attach_site"); mjs_setName(attach_site->element, "attach_site");
mjs_attach(attach_site->element, mjs_attach(attach_site->element, mjs_findBody(child, "child_body")->element,
mjs_findBody(child, "child_body")->element, "", "_child"); "", "_child");
EXPECT_THAT(mjs_findElement(parent, mjOBJ_TENDON, EXPECT_THAT(
"tendon_with_sidesite_child"), NotNull()); mjs_findElement(parent, mjOBJ_TENDON, "tendon_with_sidesite_child"),
EXPECT_THAT(mjs_findElement(parent, mjOBJ_TENDON, NotNull());
"tendon_without_sidesite_child"), NotNull()); EXPECT_THAT(
mjs_findElement(parent, mjOBJ_TENDON, "tendon_without_sidesite_child"),
NotNull());
mjModel* model = mj_compile(parent, nullptr); mjModel* model = mj_compile(parent, nullptr);
ASSERT_THAT(model, NotNull()) << mjs_getError(parent); ASSERT_THAT(model, NotNull()) << mjs_getError(parent);
@@ -1335,11 +1326,9 @@ TEST_F(MujocoTest, AttachSpatialTendonGitHubIssue3119) {
</mujoco>)"; </mujoco>)";
std::array<char, 1000> er; std::array<char, 1000> er;
mjSpec* parent_spec = mjSpec* parent_spec = mj_parseXMLString(parent_xml, 0, er.data(), er.size());
mj_parseXMLString(parent_xml, 0, er.data(), er.size());
ASSERT_THAT(parent_spec, NotNull()) << er.data(); ASSERT_THAT(parent_spec, NotNull()) << er.data();
mjSpec* child_spec = mjSpec* child_spec = mj_parseXMLString(child_xml, 0, er.data(), er.size());
mj_parseXMLString(child_xml, 0, er.data(), er.size());
ASSERT_THAT(child_spec, NotNull()) << er.data(); ASSERT_THAT(child_spec, NotNull()) << er.data();
mjsBody* parent_body = mjs_findBody(parent_spec, "parent_body"); mjsBody* parent_body = mjs_findBody(parent_spec, "parent_body");
@@ -1348,13 +1337,14 @@ TEST_F(MujocoTest, AttachSpatialTendonGitHubIssue3119) {
mjs_setName(attach_site->element, "attach_site"); mjs_setName(attach_site->element, "attach_site");
mjs_attach(attach_site->element, mjs_attach(attach_site->element,
mjs_findBody(child_spec, "child_body")->element, mjs_findBody(child_spec, "child_body")->element, "", "_child");
"", "_child");
EXPECT_THAT(
mjs_findElement(parent_spec, mjOBJ_TENDON, "tendon_with_sidesite_child"),
NotNull());
EXPECT_THAT(mjs_findElement(parent_spec, mjOBJ_TENDON, EXPECT_THAT(mjs_findElement(parent_spec, mjOBJ_TENDON,
"tendon_with_sidesite_child"), NotNull()); "tendon_without_sidesite_child"),
EXPECT_THAT(mjs_findElement(parent_spec, mjOBJ_TENDON, NotNull());
"tendon_without_sidesite_child"), NotNull());
mjModel* model = mj_compile(parent_spec, nullptr); mjModel* model = mj_compile(parent_spec, nullptr);
ASSERT_THAT(model, NotNull()) << mjs_getError(parent_spec); ASSERT_THAT(model, NotNull()) << mjs_getError(parent_spec);
@@ -1498,17 +1488,17 @@ TEST_F(MujocoTest, AttachDifferent) {
EXPECT_THAT(mjs_findDefault(parent, "attached-cylinder-1"), NotNull()); EXPECT_THAT(mjs_findDefault(parent, "attached-cylinder-1"), NotNull());
// compare with expected XML // compare with expected XML
mjModel* m_expected = LoadModelFromString(xml_result, er.data(), er.size()); MjModelPtr m_expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(m_expected, NotNull()) << er.data(); EXPECT_THAT(m_expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(m_attached, m_expected, field), tol) EXPECT_LE(CompareModel(m_attached, m_expected.get(), field), tol)
<< "Expected and attached models are different!\n" << "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';; << "Different field: " << field << '\n';
;
// destroy everything // destroy everything
mj_deleteSpec(parent); mj_deleteSpec(parent);
mj_deleteSpec(child); mj_deleteSpec(child);
mj_deleteModel(m_attached); mj_deleteModel(m_attached);
mj_deleteModel(m_expected);
} }
TEST_F(MujocoTest, AttachFrame) { TEST_F(MujocoTest, AttachFrame) {
@@ -1633,17 +1623,17 @@ TEST_F(MujocoTest, AttachFrame) {
EXPECT_THAT(m_attached->body_parentid[2], 1); EXPECT_THAT(m_attached->body_parentid[2], 1);
// compare with expected XML // compare with expected XML
mjModel* m_expected = LoadModelFromString(xml_result, er.data(), er.size()); MjModelPtr m_expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(m_expected, NotNull()) << er.data(); EXPECT_THAT(m_expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(m_attached, m_expected, field), tol) EXPECT_LE(CompareModel(m_attached, m_expected.get(), field), tol)
<< "Expected and attached models are different!\n" << "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';; << "Different field: " << field << '\n';
;
// destroy everything // destroy everything
mj_deleteSpec(parent); mj_deleteSpec(parent);
mj_deleteSpec(child); mj_deleteSpec(child);
mj_deleteModel(m_attached); mj_deleteModel(m_attached);
mj_deleteModel(m_expected);
} }
TEST_F(MujocoTest, AttachCompiled) { TEST_F(MujocoTest, AttachCompiled) {
@@ -1691,8 +1681,8 @@ TEST_F(MujocoTest, AttachCompiled) {
// check that attached model can be compiled // check that attached model can be compiled
mjModel* m_attached = mj_compile(parent, 0); mjModel* m_attached = mj_compile(parent, 0);
EXPECT_THAT(m_attached, NotNull()) << EXPECT_THAT(m_attached, NotNull())
"Failed to compile attached model" << mjs_getError(parent); << "Failed to compile attached model" << mjs_getError(parent);
// destroy everything // destroy everything
mj_deleteSpec(parent); mj_deleteSpec(parent);
@@ -1766,16 +1756,15 @@ void TestDetachBody(bool compile) {
EXPECT_THAT(m_detached, NotNull()); EXPECT_THAT(m_detached, NotNull());
// compare with expected XML // compare with expected XML
mjModel* m_expected = LoadModelFromString(xml_result, er.data(), er.size()); MjModelPtr m_expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(m_expected, NotNull()) << er.data(); EXPECT_THAT(m_expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(m_detached, m_expected, field), tol) EXPECT_LE(CompareModel(m_detached, m_expected.get(), field), tol)
<< "Expected and attached models are different!\n" << "Expected and attached models are different!\n"
<< "Different field: " << field << '\n'; << "Different field: " << field << '\n';
// destroy everything // destroy everything
mj_deleteSpec(child); mj_deleteSpec(child);
mj_deleteModel(m_detached); mj_deleteModel(m_detached);
mj_deleteModel(m_expected);
if (m_child) mj_deleteModel(m_child); if (m_child) mj_deleteModel(m_child);
} }
@@ -1836,16 +1825,15 @@ TEST_F(MujocoTest, AttachToSite) {
mjModel* model = mj_compile(parent, 0); mjModel* model = mj_compile(parent, 0);
EXPECT_THAT(model, NotNull()); EXPECT_THAT(model, NotNull());
mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size()); MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(expected, NotNull()) << er.data(); EXPECT_THAT(expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(model, expected, field), tol) EXPECT_LE(CompareModel(model, expected.get(), field), tol)
<< "Expected and attached models are different!\n" << "Expected and attached models are different!\n"
<< "Different field: " << field << '\n'; << "Different field: " << field << '\n';
mj_deleteSpec(parent); mj_deleteSpec(parent);
mj_deleteSpec(child); mj_deleteSpec(child);
mj_deleteModel(model); mj_deleteModel(model);
mj_deleteModel(expected);
} }
TEST_F(MujocoTest, AttachFrameToSite) { TEST_F(MujocoTest, AttachFrameToSite) {
@@ -1902,16 +1890,15 @@ TEST_F(MujocoTest, AttachFrameToSite) {
mjModel* model = mj_compile(parent, 0); mjModel* model = mj_compile(parent, 0);
EXPECT_THAT(model, NotNull()); EXPECT_THAT(model, NotNull());
mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size()); MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(expected, NotNull()) << er.data(); EXPECT_THAT(expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(model, expected, field), tol) EXPECT_LE(CompareModel(model, expected.get(), field), tol)
<< "Expected and attached models are different!\n" << "Expected and attached models are different!\n"
<< "Different field: " << field << '\n'; << "Different field: " << field << '\n';
mj_deleteSpec(parent); mj_deleteSpec(parent);
mj_deleteSpec(child); mj_deleteSpec(child);
mj_deleteModel(model); mj_deleteModel(model);
mj_deleteModel(expected);
} }
TEST_F(MujocoTest, BodyToFrame) { TEST_F(MujocoTest, BodyToFrame) {
@@ -1991,9 +1978,9 @@ TEST_F(MujocoTest, BodyToFrame) {
// compile and compare // compile and compare
mjModel* model2 = mj_compile(parent, 0); mjModel* model2 = mj_compile(parent, 0);
EXPECT_THAT(model2, NotNull()); EXPECT_THAT(model2, NotNull());
mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size()); MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(expected, NotNull()) << er.data(); EXPECT_THAT(expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(model2, expected, field), tol) EXPECT_LE(CompareModel(model2, expected.get(), field), tol)
<< "Expected and attached models are different!\n" << "Expected and attached models are different!\n"
<< "Different field: " << field << '\n'; << "Different field: " << field << '\n';
@@ -2002,7 +1989,6 @@ TEST_F(MujocoTest, BodyToFrame) {
mj_deleteSpec(child2); mj_deleteSpec(child2);
mj_deleteModel(model1); mj_deleteModel(model1);
mj_deleteModel(model2); mj_deleteModel(model2);
mj_deleteModel(expected);
} }
TEST_F(MujocoTest, BodyToFrameWithInertial) { TEST_F(MujocoTest, BodyToFrameWithInertial) {
@@ -2100,9 +2086,9 @@ TEST_F(MujocoTest, AttachSpecToSite) {
// compile and compare // compile and compare
mjModel* model = mj_compile(parent, 0); mjModel* model = mj_compile(parent, 0);
EXPECT_THAT(model, NotNull()); EXPECT_THAT(model, NotNull());
mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size()); MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(expected, NotNull()) << er.data(); EXPECT_THAT(expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(model, expected, field), tol) EXPECT_LE(CompareModel(model, expected.get(), field), tol)
<< "Expected and attached models are different!\n" << "Expected and attached models are different!\n"
<< "Different field: " << field << '\n'; << "Different field: " << field << '\n';
@@ -2113,7 +2099,6 @@ TEST_F(MujocoTest, AttachSpecToSite) {
mj_deleteSpec(parent); mj_deleteSpec(parent);
mj_deleteSpec(child); mj_deleteSpec(child);
mj_deleteModel(model); mj_deleteModel(model);
mj_deleteModel(expected);
} }
TEST_F(MujocoTest, AttachSpecToBody) { TEST_F(MujocoTest, AttachSpecToBody) {
@@ -2181,9 +2166,9 @@ TEST_F(MujocoTest, AttachSpecToBody) {
// compile and compare // compile and compare
mjModel* model = mj_compile(parent, 0); mjModel* model = mj_compile(parent, 0);
EXPECT_THAT(model, NotNull()); EXPECT_THAT(model, NotNull());
mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size()); MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size());
EXPECT_THAT(expected, NotNull()) << er.data(); EXPECT_THAT(expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(model, expected, field), tol) EXPECT_LE(CompareModel(model, expected.get(), field), tol)
<< "Expected and attached models are different!\n" << "Expected and attached models are different!\n"
<< "Different field: " << field << '\n'; << "Different field: " << field << '\n';
@@ -2194,7 +2179,6 @@ TEST_F(MujocoTest, AttachSpecToBody) {
mj_deleteSpec(parent); mj_deleteSpec(parent);
mj_deleteSpec(child); mj_deleteSpec(child);
mj_deleteModel(model); mj_deleteModel(model);
mj_deleteModel(expected);
} }
TEST_F(MujocoTest, PreserveState) { TEST_F(MujocoTest, PreserveState) {
@@ -2246,31 +2230,32 @@ TEST_F(MujocoTest, PreserveState) {
// compile models // compile models
mjModel* model = mj_compile(spec, 0); mjModel* model = mj_compile(spec, 0);
EXPECT_THAT(model, NotNull()); EXPECT_THAT(model, NotNull());
mjModel* m_expected = LoadModelFromString(xml_expected, er.data(), er.size()); MjModelPtr m_expected =
EXPECT_THAT(m_expected, NotNull()); LoadModelFromString(xml_expected, er.data(), er.size());
EXPECT_THAT(m_expected.get(), NotNull());
// create data // create data
mjData* data = mj_makeData(model); mjData* data = mj_makeData(model);
EXPECT_THAT(data, NotNull()); EXPECT_THAT(data, NotNull());
mjData* d_expected = mj_makeData(m_expected); MjDataPtr d_expected = MakeData(m_expected);
EXPECT_THAT(d_expected, NotNull()); EXPECT_THAT(d_expected.get(), NotNull());
// set ctrl // set ctrl
data->ctrl[0] = 1; data->ctrl[0] = 1;
data->ctrl[1] = 2; data->ctrl[1] = 2;
d_expected->ctrl[0] = 2; d_expected.get()->ctrl[0] = 2;
// set mocap // set mocap
data->mocap_pos[3] = 1; data->mocap_pos[3] = 1;
data->mocap_quat[4] = 0; data->mocap_quat[4] = 0;
data->mocap_quat[5] = 1; data->mocap_quat[5] = 1;
d_expected->mocap_pos[0] = 1; d_expected.get()->mocap_pos[0] = 1;
d_expected->mocap_quat[0] = 0; d_expected.get()->mocap_quat[0] = 0;
d_expected->mocap_quat[1] = 1; d_expected.get()->mocap_quat[1] = 1;
// step models // step models
mj_step(model, data); mj_step(model, data);
mj_step(m_expected, d_expected); mj_step(m_expected.get(), d_expected.get());
EXPECT_THAT(data->time, model->opt.timestep); EXPECT_THAT(data->time, model->opt.timestep);
// detach subtree // detach subtree
@@ -2298,7 +2283,7 @@ TEST_F(MujocoTest, PreserveState) {
joint->axis[0] = 0; joint->axis[0] = 0;
joint->axis[1] = 0; joint->axis[1] = 0;
joint->axis[2] = 1; joint->axis[2] = 1;
joint->ref = d_expected->qpos[m_expected->nq-1]; joint->ref = d_expected.get()->qpos[m_expected->nq - 1];
// compile new model // compile new model
mj_recompile(spec, 0, model, data); mj_recompile(spec, 0, model, data);
@@ -2308,13 +2293,13 @@ TEST_F(MujocoTest, PreserveState) {
// compare qpos // compare qpos
EXPECT_EQ(model->nq, m_expected->nq); EXPECT_EQ(model->nq, m_expected->nq);
for (int i = 0; i < model->nq; ++i) { for (int i = 0; i < model->nq; ++i) {
EXPECT_EQ(data->qpos[i], d_expected->qpos[i]) << i; EXPECT_EQ(data->qpos[i], d_expected.get()->qpos[i]) << i;
} }
// compare qvel // compare qvel
EXPECT_EQ(model->nv, m_expected->nv); EXPECT_EQ(model->nv, m_expected->nv);
for (int i = 0; i < model->nv - 1; ++i) { for (int i = 0; i < model->nv - 1; ++i) {
EXPECT_EQ(data->qvel[i], d_expected->qvel[i]) << i; EXPECT_EQ(data->qvel[i], d_expected.get()->qvel[i]) << i;
} }
// second body was added after stepping so qvel should be zero // second body was added after stepping so qvel should be zero
@@ -2323,17 +2308,21 @@ TEST_F(MujocoTest, PreserveState) {
// compare act // compare act
EXPECT_EQ(model->na, m_expected->na); EXPECT_EQ(model->na, m_expected->na);
for (int i = 0; i < model->na; ++i) { for (int i = 0; i < model->na; ++i) {
EXPECT_EQ(data->act[i], d_expected->act[i]) << i; EXPECT_EQ(data->act[i], d_expected.get()->act[i]) << i;
} }
// compare mocap // compare mocap
EXPECT_EQ(model->nmocap, m_expected->nmocap); EXPECT_EQ(model->nmocap, m_expected->nmocap);
for (int i = 0; i < model->nmocap; ++i) { for (int i = 0; i < model->nmocap; ++i) {
for (int j = 0; j < 3; ++j) { for (int j = 0; j < 3; ++j) {
EXPECT_EQ(data->mocap_pos[3*i+j], d_expected->mocap_pos[3*i+j]) << i; EXPECT_EQ(data->mocap_pos[3 * i + j],
d_expected.get()->mocap_pos[3 * i + j])
<< i;
} }
for (int j = 0; j < 4; ++j) { for (int j = 0; j < 4; ++j) {
EXPECT_EQ(data->mocap_quat[4*i+j], d_expected->mocap_quat[4*i+j]) << i; EXPECT_EQ(data->mocap_quat[4 * i + j],
d_expected.get()->mocap_quat[4 * i + j])
<< i;
} }
} }
@@ -2342,10 +2331,8 @@ TEST_F(MujocoTest, PreserveState) {
mj_recompile(spec, 0, model, nullptr); mj_recompile(spec, 0, model, nullptr);
// destroy everything // destroy everything
mj_deleteData(d_expected);
mj_deleteSpec(spec); mj_deleteSpec(spec);
mj_deleteModel(model); mj_deleteModel(model);
mj_deleteModel(m_expected);
} }
TEST_F(MujocoTest, RecompileAttach) { TEST_F(MujocoTest, RecompileAttach) {
@@ -2445,15 +2432,15 @@ TEST_F(MujocoTest, AttachMocap) {
mjModel* model = mj_compile(spec, 0); mjModel* model = mj_compile(spec, 0);
EXPECT_THAT(model, NotNull()); EXPECT_THAT(model, NotNull());
mjModel* m_expected = LoadModelFromString(xml_expected, er.data(), er.size()); MjModelPtr m_expected =
EXPECT_THAT(m_expected, NotNull()) << er.data(); LoadModelFromString(xml_expected, er.data(), er.size());
EXPECT_LE(CompareModel(model, m_expected, field), tol) EXPECT_THAT(m_expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(model, m_expected.get(), field), tol)
<< "Expected and attached models are different!\n" << "Expected and attached models are different!\n"
<< "Different field: " << field << '\n'; << "Different field: " << field << '\n';
mj_deleteSpec(spec); mj_deleteSpec(spec);
mj_deleteModel(model); mj_deleteModel(model);
mj_deleteModel(m_expected);
} }
TEST_F(MujocoTest, ReplicateKeyframe) { TEST_F(MujocoTest, ReplicateKeyframe) {
@@ -2475,8 +2462,8 @@ TEST_F(MujocoTest, ReplicateKeyframe) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, testing::NotNull()) << error.data(); EXPECT_THAT(m.get(), testing::NotNull()) << error.data();
EXPECT_THAT(m->ngeom, 1); EXPECT_THAT(m->ngeom, 1);
EXPECT_THAT(m->nbody, 2); EXPECT_THAT(m->nbody, 2);
@@ -2485,10 +2472,8 @@ TEST_F(MujocoTest, ReplicateKeyframe) {
EXPECT_THAT(m->nq, 1); EXPECT_THAT(m->nq, 1);
EXPECT_THAT(m->key_qpos[0], 1); EXPECT_THAT(m->key_qpos[0], 1);
EXPECT_THAT(m->key_qpos[1], 0); EXPECT_THAT(m->key_qpos[1], 0);
EXPECT_STREQ(mj_id2name(m, mjOBJ_KEY, 0), "keyframe0"); EXPECT_STREQ(mj_id2name(m.get(), mjOBJ_KEY, 0), "keyframe0");
EXPECT_STREQ(mj_id2name(m, mjOBJ_KEY, 1), "keyframe"); EXPECT_STREQ(mj_id2name(m.get(), mjOBJ_KEY, 1), "keyframe");
mj_deleteModel(m);
} }
TEST_F(MujocoTest, AttachUnnamedAssets) { TEST_F(MujocoTest, AttachUnnamedAssets) {
@@ -2690,17 +2675,17 @@ void AttachNestedKeyframe(bool compile) {
std::string field = ""; std::string field = "";
mjModel* m_attached = mj_compile(parent, 0); mjModel* m_attached = mj_compile(parent, 0);
EXPECT_THAT(m_attached, NotNull()); EXPECT_THAT(m_attached, NotNull());
mjModel* m_expected = LoadModelFromString( MjModelPtr m_expected = LoadModelFromString(
compile ? expected_xml : expected_xml_uncompiled, er.data(), er.size()); compile ? expected_xml : expected_xml_uncompiled, er.data(), er.size());
EXPECT_THAT(m_expected, NotNull()) << er.data(); EXPECT_THAT(m_expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(m_attached, m_expected, field), tol) EXPECT_LE(CompareModel(m_attached, m_expected.get(), field), tol)
<< "Expected and attached models are different!\n" << "Expected and attached models are different!\n"
<< "Different field: " << field << '\n';; << "Different field: " << field << '\n';
;
mj_deleteSpec(parent); mj_deleteSpec(parent);
mj_deleteSpec(child); mj_deleteSpec(child);
mj_deleteSpec(gchild); mj_deleteSpec(gchild);
mj_deleteModel(m_expected);
mj_deleteModel(m_attached); mj_deleteModel(m_attached);
mj_deleteModel(m_child); mj_deleteModel(m_child);
} }
@@ -2820,16 +2805,15 @@ TEST_F(MujocoTest, ResizeParentKeyframe) {
mjtNum tol = 0; mjtNum tol = 0;
std::string field = ""; std::string field = "";
mjModel* expected = LoadModelFromString(xml_expected, er.data(), er.size()); MjModelPtr expected = LoadModelFromString(xml_expected, er.data(), er.size());
EXPECT_THAT(expected, NotNull()) << er.data(); EXPECT_THAT(expected.get(), NotNull()) << er.data();
EXPECT_LE(CompareModel(model, expected, field), tol) EXPECT_LE(CompareModel(model, expected.get(), field), tol)
<< "Expected and attached models are different!\n" << "Expected and attached models are different!\n"
<< "Different field: " << field << '\n'; << "Different field: " << field << '\n';
mj_deleteSpec(parent); mj_deleteSpec(parent);
mj_deleteSpec(child); mj_deleteSpec(child);
mj_deleteModel(model); mj_deleteModel(model);
mj_deleteModel(expected);
} }
TEST_F(MujocoTest, KeyframeSizeError) { TEST_F(MujocoTest, KeyframeSizeError) {
@@ -2854,7 +2838,9 @@ TEST_F(MujocoTest, KeyframeSizeError) {
std::array<char, 1000> er; std::array<char, 1000> er;
mjSpec* spec = mj_parseXMLString(xml, 0, er.data(), er.size()); mjSpec* spec = mj_parseXMLString(xml, 0, er.data(), er.size());
EXPECT_THAT(spec, IsNull()); EXPECT_THAT(spec, IsNull());
EXPECT_THAT(er.data(), HasSubstr( EXPECT_THAT(
er.data(),
HasSubstr(
"Keyframe 'invalid_qpos' has invalid qpos size, got 2, should be 1")); "Keyframe 'invalid_qpos' has invalid qpos size, got 2, should be 1"));
} }
@@ -2899,8 +2885,7 @@ TEST_F(MujocoTest, DifferentUnitsAllowed) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjSpec* gchild = mjSpec* gchild = mj_parseXMLString(gchild_xml, 0, error.data(), error.size());
mj_parseXMLString(gchild_xml, 0, error.data(), error.size());
mjSpec* child = mj_parseXMLString(child_xml, 0, error.data(), error.size()); mjSpec* child = mj_parseXMLString(child_xml, 0, error.data(), error.size());
mjSpec* spec = mj_parseXMLString(parent_xml, 0, error.data(), error.size()); mjSpec* spec = mj_parseXMLString(parent_xml, 0, error.data(), error.size());
ASSERT_THAT(spec, NotNull()) << error.data(); ASSERT_THAT(spec, NotNull()) << error.data();
+4 -5
View File
@@ -48,9 +48,8 @@ TEST_F(UserCompositeTest, ShapeCanBeOmitted) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, NotNull()) << error.data(); EXPECT_THAT(m.get(), NotNull()) << error.data();
mj_deleteModel(m);
} }
TEST_F(UserCompositeTest, InvalidShape) { TEST_F(UserCompositeTest, InvalidShape) {
@@ -64,8 +63,8 @@ TEST_F(UserCompositeTest, InvalidShape) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()) << error.data(); EXPECT_THAT(m.get(), IsNull()) << error.data();
EXPECT_THAT(error.data(), EXPECT_THAT(error.data(),
HasSubstr("The curve array contains an invalid shape")); HasSubstr("The curve array contains an invalid shape"));
} }
+152 -198
View File
@@ -24,19 +24,15 @@
#include <mujoco/mujoco.h> #include <mujoco/mujoco.h>
#include "test/fixture.h" #include "test/fixture.h"
namespace mujoco { namespace mujoco {
namespace { namespace {
using ::testing::HasSubstr;
using ::testing::IsNull; using ::testing::IsNull;
using ::testing::NotNull; using ::testing::NotNull;
using ::testing::HasSubstr;
using ::testing::Pointwise; using ::testing::Pointwise;
using UserFlexTest = MujocoTest; using UserFlexTest = MujocoTest;
TEST_F(UserFlexTest, ParentMustHaveName) { TEST_F(UserFlexTest, ParentMustHaveName) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
@@ -46,8 +42,8 @@ TEST_F(UserFlexTest, ParentMustHaveName) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()); EXPECT_THAT(m.get(), IsNull());
EXPECT_THAT(error.data(), HasSubstr("required attribute missing: 'name'")); EXPECT_THAT(error.data(), HasSubstr("required attribute missing: 'name'"));
} }
@@ -60,8 +56,8 @@ TEST_F(UserFlexTest, InvalidDim) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()); EXPECT_THAT(m.get(), IsNull());
EXPECT_THAT(error.data(), HasSubstr("Invalid dim, must be between 1 and 3")); EXPECT_THAT(error.data(), HasSubstr("Invalid dim, must be between 1 and 3"));
} }
@@ -74,8 +70,8 @@ TEST_F(UserFlexTest, CountTooSmall) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()); EXPECT_THAT(m.get(), IsNull());
EXPECT_THAT(error.data(), HasSubstr("Count too small")); EXPECT_THAT(error.data(), HasSubstr("Count too small"));
} }
@@ -95,8 +91,8 @@ TEST_F(UserFlexTest, CellnumZeroInterpolated) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()); EXPECT_THAT(m.get(), IsNull());
EXPECT_THAT(error.data(), HasSubstr("cellcount cannot be 0")); EXPECT_THAT(error.data(), HasSubstr("cellcount cannot be 0"));
} }
@@ -109,8 +105,8 @@ TEST_F(UserFlexTest, SpacingGreaterThanGeometry) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()) << error.data(); EXPECT_THAT(m.get(), IsNull()) << error.data();
EXPECT_THAT(error.data(), EXPECT_THAT(error.data(),
HasSubstr("Spacing must be larger than geometry size")); HasSubstr("Spacing must be larger than geometry size"));
} }
@@ -124,8 +120,8 @@ TEST_F(UserFlexTest, ScaleMinValue) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()) << error.data(); EXPECT_THAT(m.get(), IsNull()) << error.data();
EXPECT_THAT(error.data(), HasSubstr("Scale must be larger than mjMINVAL")); EXPECT_THAT(error.data(), HasSubstr("Scale must be larger than mjMINVAL"));
} }
@@ -138,8 +134,8 @@ TEST_F(UserFlexTest, MassMinValue) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()) << error.data(); EXPECT_THAT(m.get(), IsNull()) << error.data();
EXPECT_THAT(error.data(), EXPECT_THAT(error.data(),
HasSubstr("Mass and inertiabox must be larger than mjMINVAL")); HasSubstr("Mass and inertiabox must be larger than mjMINVAL"));
} }
@@ -153,8 +149,8 @@ TEST_F(UserFlexTest, PointSizeNotMultipleOf3) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()) << error.data(); EXPECT_THAT(m.get(), IsNull()) << error.data();
EXPECT_THAT(error.data(), HasSubstr("Point size must be a multiple of 3")); EXPECT_THAT(error.data(), HasSubstr("Point size must be a multiple of 3"));
} }
@@ -167,8 +163,8 @@ TEST_F(UserFlexTest, ElementSize) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()) << error.data(); EXPECT_THAT(m.get(), IsNull()) << error.data();
EXPECT_THAT(error.data(), EXPECT_THAT(error.data(),
HasSubstr("Element size must be a multiple of dim+1")); HasSubstr("Element size must be a multiple of dim+1"));
} }
@@ -182,8 +178,8 @@ TEST_F(UserFlexTest, PointAndElementNotInDirect) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()) << error.data(); EXPECT_THAT(m.get(), IsNull()) << error.data();
EXPECT_THAT(error.data(), HasSubstr("Point and element required")); EXPECT_THAT(error.data(), HasSubstr("Point and element required"));
} }
@@ -196,8 +192,8 @@ TEST_F(UserFlexTest, UnknownFlexCompType) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()) << error.data(); EXPECT_THAT(m.get(), IsNull()) << error.data();
EXPECT_THAT(error.data(), HasSubstr("invalid keyword: 'unknown'")); EXPECT_THAT(error.data(), HasSubstr("invalid keyword: 'unknown'"));
} }
@@ -212,8 +208,8 @@ TEST_F(UserFlexTest, InvalidPinid) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()) << error.data(); EXPECT_THAT(m.get(), IsNull()) << error.data();
EXPECT_THAT(error.data(), EXPECT_THAT(error.data(),
HasSubstr("element 1 has point id 1, number of points is 1")); HasSubstr("element 1 has point id 1, number of points is 1"));
} }
@@ -227,8 +223,8 @@ TEST_F(UserFlexTest, MeshFileMissing) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()) << error.data(); EXPECT_THAT(m.get(), IsNull()) << error.data();
EXPECT_THAT(error.data(), HasSubstr("File is required")); EXPECT_THAT(error.data(), HasSubstr("File is required"));
} }
@@ -249,8 +245,8 @@ TEST_F(UserFlexTest, CreateBVHSuccess) {
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m, NotNull()) << error.data();
mjData* d = mj_makeData(m); mjData* d = mj_makeData(m);
mj_step(m, d); mj_step(m, d);
mj_deleteModel(m);
mj_deleteData(d); mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(UserFlexTest, RigidFlex) { TEST_F(UserFlexTest, RigidFlex) {
@@ -261,8 +257,8 @@ TEST_F(UserFlexTest, RigidFlex) {
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m, NotNull()) << error.data();
mjData* d = mj_makeData(m); mjData* d = mj_makeData(m);
mj_step(m, d); mj_step(m, d);
mj_deleteModel(m);
mj_deleteData(d); mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(UserFlexTest, FlexNotCollide) { TEST_F(UserFlexTest, FlexNotCollide) {
@@ -277,17 +273,16 @@ TEST_F(UserFlexTest, FlexNotCollide) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m.get(), NotNull()) << error.data();
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_step(m, d); mj_step(m.get(), d.get());
mj_deleteModel(m);
mj_deleteData(d);
} }
TEST_F(UserFlexTest, BoundingBoxCoordinates) { TEST_F(UserFlexTest, BoundingBoxCoordinates) {
#ifdef mjUSESINGLE #ifdef mjUSESINGLE
GTEST_SKIP() << "Float32 rounding in bounding box centering gives ~3e-8 error"; GTEST_SKIP()
<< "Float32 rounding in bounding box centering gives ~3e-8 error";
#endif #endif
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
@@ -298,11 +293,11 @@ TEST_F(UserFlexTest, BoundingBoxCoordinates) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m.get(), NotNull()) << error.data();
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_kinematics(m, d); mj_kinematics(m.get(), d.get());
mj_flex(m, d); mj_flex(m.get(), d.get());
EXPECT_EQ(m->nflexvert, 5 * 5 * 5); EXPECT_EQ(m->nflexvert, 5 * 5 * 5);
EXPECT_EQ(m->nflexelem, 4 * 4 * 4 * 6); EXPECT_EQ(m->nflexelem, 4 * 4 * 4 * 6);
@@ -323,9 +318,6 @@ TEST_F(UserFlexTest, BoundingBoxCoordinates) {
EXPECT_EQ(m->flex_vert0[3 * m->nflexvert - 3], 1); EXPECT_EQ(m->flex_vert0[3 * m->nflexvert - 3], 1);
EXPECT_EQ(m->flex_vert0[3 * m->nflexvert - 2], 1); EXPECT_EQ(m->flex_vert0[3 * m->nflexvert - 2], 1);
EXPECT_EQ(m->flex_vert0[3 * m->nflexvert - 1], 1); EXPECT_EQ(m->flex_vert0[3 * m->nflexvert - 1], 1);
mj_deleteModel(m);
mj_deleteData(d);
} }
TEST_F(UserFlexTest, TrilinearCannotDoSelfCollision) { TEST_F(UserFlexTest, TrilinearCannotDoSelfCollision) {
@@ -339,8 +331,8 @@ TEST_F(UserFlexTest, TrilinearCannotDoSelfCollision) {
</worldbody> </worldbody>
</mujoco> </mujoco>
)"; )";
mjModel* m1 = LoadModelFromString(xml_selfcoll, error.data(), error.size()); MjModelPtr m1 = LoadModelFromString(xml_selfcoll, error.data(), error.size());
EXPECT_THAT(m1, IsNull()) << error.data(); EXPECT_THAT(m1.get(), IsNull()) << error.data();
EXPECT_THAT(error.data(), EXPECT_THAT(error.data(),
HasSubstr("trilinear interpolation cannot do self-collision")); HasSubstr("trilinear interpolation cannot do self-collision"));
static constexpr char xml_internal[] = R"( static constexpr char xml_internal[] = R"(
@@ -352,8 +344,8 @@ TEST_F(UserFlexTest, TrilinearCannotDoSelfCollision) {
</worldbody> </worldbody>
</mujoco> </mujoco>
)"; )";
mjModel* m2 = LoadModelFromString(xml_internal, error.data(), error.size()); MjModelPtr m2 = LoadModelFromString(xml_internal, error.data(), error.size());
EXPECT_THAT(m2, IsNull()) << error.data(); EXPECT_THAT(m2.get(), IsNull()) << error.data();
EXPECT_THAT(error.data(), EXPECT_THAT(error.data(),
HasSubstr("trilinear interpolation cannot do internal")); HasSubstr("trilinear interpolation cannot do internal"));
} }
@@ -370,10 +362,11 @@ TEST_F(UserFlexTest, TrilinearInterpolation) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m1 = LoadModelFromString(xml_trilinear, error.data(), error.size()); MjModelPtr m1 =
ASSERT_THAT(m1, NotNull()) << error.data(); LoadModelFromString(xml_trilinear, error.data(), error.size());
mjData* d1 = mj_makeData(m1); ASSERT_THAT(m1.get(), NotNull()) << error.data();
mj_step(m1, d1); MjDataPtr d1 = MakeData(m1);
mj_step(m1.get(), d1.get());
static constexpr char xml_linear[] = R"( static constexpr char xml_linear[] = R"(
<mujoco> <mujoco>
@@ -385,10 +378,10 @@ TEST_F(UserFlexTest, TrilinearInterpolation) {
</worldbody> </worldbody>
</mujoco> </mujoco>
)"; )";
mjModel* m2 = LoadModelFromString(xml_linear, error.data(), error.size()); MjModelPtr m2 = LoadModelFromString(xml_linear, error.data(), error.size());
ASSERT_THAT(m2, NotNull()) << error.data(); ASSERT_THAT(m2.get(), NotNull()) << error.data();
mjData* d2 = mj_makeData(m2); MjDataPtr d2 = MakeData(m2);
mj_step(m2, d2); mj_step(m2.get(), d2.get());
EXPECT_EQ(m1->nflexvert, m2->nflexvert); EXPECT_EQ(m1->nflexvert, m2->nflexvert);
for (int i = 0; i < 3 * m1->nflexvert; ++i) { for (int i = 0; i < 3 * m1->nflexvert; ++i) {
@@ -443,11 +436,6 @@ TEST_F(UserFlexTest, TrilinearInterpolation) {
EXPECT_EQ(d1->efc_diagA[i], d2->efc_diagA[i]); EXPECT_EQ(d1->efc_diagA[i], d2->efc_diagA[i]);
EXPECT_EQ(d1->efc_D[i], d2->efc_D[i]); EXPECT_EQ(d1->efc_D[i], d2->efc_D[i]);
} }
mj_deleteModel(m1);
mj_deleteModel(m2);
mj_deleteData(d1);
mj_deleteData(d2);
} }
TEST_F(UserFlexTest, StiffnessMatrix) { TEST_F(UserFlexTest, StiffnessMatrix) {
@@ -466,8 +454,8 @@ TEST_F(UserFlexTest, StiffnessMatrix) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m.get(), NotNull()) << error.data();
EXPECT_NE(m->flex_stiffness[m->flex_stiffnessadr[0]], 0); EXPECT_NE(m->flex_stiffness[m->flex_stiffnessadr[0]], 0);
EXPECT_EQ(m->nflexnode, 8); EXPECT_EQ(m->nflexnode, 8);
@@ -480,8 +468,6 @@ TEST_F(UserFlexTest, StiffnessMatrix) {
mju_mulMatVec(res, m->flex_stiffness + m->flex_stiffnessadr[0], ones, mju_mulMatVec(res, m->flex_stiffness + m->flex_stiffnessadr[0], ones,
3 * m->nflexnode, 3 * m->nflexnode); 3 * m->nflexnode, 3 * m->nflexnode);
EXPECT_THAT(res, Pointwise(MjNear(1e-8, 1e-4), zeros)); EXPECT_THAT(res, Pointwise(MjNear(1e-8, 1e-4), zeros));
mj_deleteModel(m);
} }
TEST_F(UserFlexTest, StiffnessCacheDiffersByGeometry) { TEST_F(UserFlexTest, StiffnessCacheDiffersByGeometry) {
@@ -510,19 +496,18 @@ TEST_F(UserFlexTest, StiffnessCacheDiffersByGeometry) {
</mujoco> </mujoco>
)"; )";
mjModel* m_small = LoadModelFromString(xml_small, error.data(), error.size()); MjModelPtr m_small =
ASSERT_THAT(m_small, NotNull()) << error.data(); LoadModelFromString(xml_small, error.data(), error.size());
ASSERT_THAT(m_small.get(), NotNull()) << error.data();
mjModel* m_large = LoadModelFromString(xml_large, error.data(), error.size()); MjModelPtr m_large =
ASSERT_THAT(m_large, NotNull()) << error.data(); LoadModelFromString(xml_large, error.data(), error.size());
ASSERT_THAT(m_large.get(), NotNull()) << error.data();
// Same number of nodes but different stiffness due to different geometry // Same number of nodes but different stiffness due to different geometry
EXPECT_EQ(m_small->nflexnode, m_large->nflexnode); EXPECT_EQ(m_small->nflexnode, m_large->nflexnode);
EXPECT_NE(m_small->flex_stiffness[m_small->flex_stiffnessadr[0]], EXPECT_NE(m_small->flex_stiffness[m_small->flex_stiffnessadr[0]],
m_large->flex_stiffness[m_large->flex_stiffnessadr[0]]); m_large->flex_stiffness[m_large->flex_stiffnessadr[0]]);
mj_deleteModel(m_small);
mj_deleteModel(m_large);
} }
TEST_F(UserFlexTest, LoadTexture) { TEST_F(UserFlexTest, LoadTexture) {
@@ -552,8 +537,8 @@ TEST_F(UserFlexTest, LoadMSHBinary_41_Success) {
EXPECT_EQ(m->nflexelem, 24); EXPECT_EQ(m->nflexelem, 24);
EXPECT_EQ(m->flex_dim[0], 3); EXPECT_EQ(m->flex_dim[0], 3);
mj_step(m, d); mj_step(m, d);
mj_deleteModel(m);
mj_deleteData(d); mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(UserFlexTest, LoadMSHBinary_22_Success) { TEST_F(UserFlexTest, LoadMSHBinary_22_Success) {
@@ -567,8 +552,8 @@ TEST_F(UserFlexTest, LoadMSHBinary_22_Success) {
EXPECT_EQ(m->nflexelem, 24); EXPECT_EQ(m->nflexelem, 24);
EXPECT_EQ(m->flex_dim[0], 3); EXPECT_EQ(m->flex_dim[0], 3);
mj_step(m, d); mj_step(m, d);
mj_deleteModel(m);
mj_deleteData(d); mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(UserFlexTest, LoadMSHSurfaceBinary_41_Success) { TEST_F(UserFlexTest, LoadMSHSurfaceBinary_41_Success) {
@@ -595,8 +580,8 @@ TEST_F(UserFlexTest, LoadMSHSurfaceBinary_41_Success) {
EXPECT_EQ(m->flex_elem[2], 3 - 1); EXPECT_EQ(m->flex_elem[2], 3 - 1);
mj_step(m, d); mj_step(m, d);
mj_deleteModel(m);
mj_deleteData(d); mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(UserFlexTest, LoadMSHSurfaceBinary_22_Success) { TEST_F(UserFlexTest, LoadMSHSurfaceBinary_22_Success) {
@@ -623,8 +608,8 @@ TEST_F(UserFlexTest, LoadMSHSurfaceBinary_22_Success) {
EXPECT_EQ(m->flex_elem[2], 3 - 1); EXPECT_EQ(m->flex_elem[2], 3 - 1);
mj_step(m, d); mj_step(m, d);
mj_deleteModel(m);
mj_deleteData(d); mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(UserFlexTest, LoadMSHBinaryFTETWILD_22_Success) { TEST_F(UserFlexTest, LoadMSHBinaryFTETWILD_22_Success) {
@@ -638,8 +623,8 @@ TEST_F(UserFlexTest, LoadMSHBinaryFTETWILD_22_Success) {
EXPECT_EQ(m->nflexelem, 1073); EXPECT_EQ(m->nflexelem, 1073);
EXPECT_EQ(m->flex_dim[0], 3); EXPECT_EQ(m->flex_dim[0], 3);
mj_step(m, d); mj_step(m, d);
mj_deleteModel(m);
mj_deleteData(d); mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(UserFlexTest, LoadMSHASCII_41_Success) { TEST_F(UserFlexTest, LoadMSHASCII_41_Success) {
@@ -653,8 +638,8 @@ TEST_F(UserFlexTest, LoadMSHASCII_41_Success) {
EXPECT_EQ(m->nflexelem, 24); EXPECT_EQ(m->nflexelem, 24);
EXPECT_EQ(m->flex_dim[0], 3); EXPECT_EQ(m->flex_dim[0], 3);
mj_step(m, d); mj_step(m, d);
mj_deleteModel(m);
mj_deleteData(d); mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(UserFlexTest, LoadMSHASCII_22_Success) { TEST_F(UserFlexTest, LoadMSHASCII_22_Success) {
@@ -668,8 +653,8 @@ TEST_F(UserFlexTest, LoadMSHASCII_22_Success) {
EXPECT_EQ(m->nflexelem, 24); EXPECT_EQ(m->nflexelem, 24);
EXPECT_EQ(m->flex_dim[0], 3); EXPECT_EQ(m->flex_dim[0], 3);
mj_step(m, d); mj_step(m, d);
mj_deleteModel(m);
mj_deleteData(d); mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(UserFlexTest, LoadMSHSurfaceASCII_41_Success) { TEST_F(UserFlexTest, LoadMSHSurfaceASCII_41_Success) {
@@ -696,8 +681,8 @@ TEST_F(UserFlexTest, LoadMSHSurfaceASCII_41_Success) {
EXPECT_EQ(m->flex_elem[2], 3 - 1); EXPECT_EQ(m->flex_elem[2], 3 - 1);
mj_step(m, d); mj_step(m, d);
mj_deleteModel(m);
mj_deleteData(d); mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(UserFlexTest, LoadMSHSurfaceASCII_22_Success) { TEST_F(UserFlexTest, LoadMSHSurfaceASCII_22_Success) {
@@ -724,8 +709,8 @@ TEST_F(UserFlexTest, LoadMSHSurfaceASCII_22_Success) {
EXPECT_EQ(m->flex_elem[2], 3 - 1); EXPECT_EQ(m->flex_elem[2], 3 - 1);
mj_step(m, d); mj_step(m, d);
mj_deleteModel(m);
mj_deleteData(d); mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(UserFlexTest, LoadMSHASCIIFTETWILD_22_Success) { TEST_F(UserFlexTest, LoadMSHASCIIFTETWILD_22_Success) {
@@ -739,69 +724,68 @@ TEST_F(UserFlexTest, LoadMSHASCIIFTETWILD_22_Success) {
EXPECT_EQ(m->nflexelem, 1070); EXPECT_EQ(m->nflexelem, 1070);
EXPECT_EQ(m->flex_dim[0], 3); EXPECT_EQ(m->flex_dim[0], 3);
mj_step(m, d); mj_step(m, d);
mj_deleteModel(m);
mj_deleteData(d); mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(UserFlexTest, LoadMSHASCII_41_MissingNodeHeader_Fail) { TEST_F(UserFlexTest, LoadMSHASCII_41_MissingNodeHeader_Fail) {
const std::string xml_path = const std::string xml_path = GetTestDataFilePath(
GetTestDataFilePath(
"user/testdata/malformed_cube_41_ascii_missing_node_header.xml"); "user/testdata/malformed_cube_41_ascii_missing_node_header.xml");
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
EXPECT_THAT(error.data(), HasSubstr( EXPECT_THAT(error.data(),
"XML Error: Error: All nodes must be in single block")); HasSubstr("XML Error: Error: All nodes must be in single block"));
mj_deleteModel(m); mj_deleteModel(m);
} }
TEST_F(UserFlexTest, LoadMSHASCII_41_MissingNodeIndex_Fail) { TEST_F(UserFlexTest, LoadMSHASCII_41_MissingNodeIndex_Fail) {
const std::string xml_path = const std::string xml_path = GetTestDataFilePath(
GetTestDataFilePath(
"user/testdata/malformed_cube_41_ascii_missing_node_index.xml"); "user/testdata/malformed_cube_41_ascii_missing_node_index.xml");
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
EXPECT_THAT(error.data(), HasSubstr( EXPECT_THAT(error.data(),
"XML Error: Error: Node tags must be sequential")); HasSubstr("XML Error: Error: Node tags must be sequential"));
mj_deleteModel(m); mj_deleteModel(m);
} }
TEST_F(UserFlexTest, LoadMSHASCII_41_MissingElementHeader_Fail) { TEST_F(UserFlexTest, LoadMSHASCII_41_MissingElementHeader_Fail) {
const std::string xml_path = const std::string xml_path = GetTestDataFilePath(
GetTestDataFilePath(
"user/testdata/malformed_cube_41_ascii_missing_element_header.xml"); "user/testdata/malformed_cube_41_ascii_missing_element_header.xml");
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
EXPECT_THAT(error.data(), HasSubstr( EXPECT_THAT(
"XML Error: Error: All elements must be in single block")); error.data(),
HasSubstr("XML Error: Error: All elements must be in single block"));
mj_deleteModel(m); mj_deleteModel(m);
} }
TEST_F(UserFlexTest, LoadMSHASCII_41_MissingElement_Fail) { TEST_F(UserFlexTest, LoadMSHASCII_41_MissingElement_Fail) {
const std::string xml_path = const std::string xml_path = GetTestDataFilePath(
GetTestDataFilePath(
"user/testdata/malformed_cube_41_ascii_missing_element.xml"); "user/testdata/malformed_cube_41_ascii_missing_element.xml");
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
EXPECT_THAT(error.data(), HasSubstr( EXPECT_THAT(error.data(),
"XML Error: Error: Error reading Elements")); HasSubstr("XML Error: Error: Error reading Elements"));
mj_deleteModel(m); mj_deleteModel(m);
} }
TEST_F(UserFlexTest, TEST_F(UserFlexTest,
LoadMSHASCII_41_MismatchBetweenMaxNodesAndNodesInBlock_Fail) { LoadMSHASCII_41_MismatchBetweenMaxNodesAndNodesInBlock_Fail) {
const std::string xml_path = const std::string xml_path = GetTestDataFilePath(
GetTestDataFilePath( "user/testdata/"
"user/testdata/malformed_cube_41_ascii_mismatch_between_max_nodes_and_nodes_in_block.xml"); "malformed_cube_41_ascii_mismatch_between_max_nodes_and_nodes_in_block."
"xml");
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
EXPECT_THAT(error.data(), HasSubstr( EXPECT_THAT(
"XML Error: Error: Maximum number of nodes must be equal to number of nodes in a block\nElement 'flexcomp', line 22\n")); error.data(),
HasSubstr("XML Error: Error: Maximum number of nodes must be equal to "
"number of nodes in a block\nElement 'flexcomp', line 22\n"));
mj_deleteModel(m); mj_deleteModel(m);
} }
TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNumNodes_Fail) { TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNumNodes_Fail) {
const std::string xml_path = const std::string xml_path = GetTestDataFilePath(
GetTestDataFilePath(
"user/testdata/malformed_cube_22_ascii_missing_num_nodes.xml"); "user/testdata/malformed_cube_22_ascii_missing_num_nodes.xml");
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
@@ -812,19 +796,17 @@ TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNumNodes_Fail) {
} }
TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNode_Fail) { TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNode_Fail) {
const std::string xml_path = const std::string xml_path = GetTestDataFilePath(
GetTestDataFilePath(
"user/testdata/malformed_cube_22_ascii_missing_node.xml"); "user/testdata/malformed_cube_22_ascii_missing_node.xml");
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
EXPECT_THAT(error.data(), HasSubstr( EXPECT_THAT(error.data(),
"XML Error: Error: Error reading node tags")); HasSubstr("XML Error: Error: Error reading node tags"));
mj_deleteModel(m); mj_deleteModel(m);
} }
TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNumElements_Fail) { TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNumElements_Fail) {
const std::string xml_path = const std::string xml_path = GetTestDataFilePath(
GetTestDataFilePath(
"user/testdata/malformed_shark_22_ascii_missing_num_elements.xml"); "user/testdata/malformed_shark_22_ascii_missing_num_elements.xml");
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
@@ -835,19 +817,17 @@ TEST_F(UserFlexTest, LoadMSHASCII_22_MissingNumElements_Fail) {
} }
TEST_F(UserFlexTest, LoadMSHASCII_22_MissingElement_Fail) { TEST_F(UserFlexTest, LoadMSHASCII_22_MissingElement_Fail) {
const std::string xml_path = const std::string xml_path = GetTestDataFilePath(
GetTestDataFilePath(
"user/testdata/malformed_cube_22_ascii_missing_element.xml"); "user/testdata/malformed_cube_22_ascii_missing_element.xml");
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
EXPECT_THAT(error.data(), HasSubstr( EXPECT_THAT(error.data(),
"XML Error: Error: Error reading Elements")); HasSubstr("XML Error: Error: Error reading Elements"));
mj_deleteModel(m); mj_deleteModel(m);
} }
TEST_F(UserFlexTest, LoadMSHASCII_dim_missing_in_xml) { TEST_F(UserFlexTest, LoadMSHASCII_dim_missing_in_xml) {
const std::string xml_path = const std::string xml_path = GetTestDataFilePath(
GetTestDataFilePath(
"user/testdata/cube_22_ascii_vol_gmshApp_missing_dim.xml"); "user/testdata/cube_22_ascii_vol_gmshApp_missing_dim.xml");
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
@@ -871,9 +851,8 @@ TEST_F(UserFlexTest, TrilinearUnusedVertices_Crash) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, testing::NotNull()) << error.data(); ASSERT_THAT(m.get(), testing::NotNull()) << error.data();
mj_deleteModel(m);
} }
TEST_F(UserFlexTest, MeshNodePinning) { TEST_F(UserFlexTest, MeshNodePinning) {
@@ -891,8 +870,8 @@ TEST_F(UserFlexTest, MeshNodePinning) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, testing::NotNull()) << error.data(); ASSERT_THAT(m.get(), testing::NotNull()) << error.data();
// Verify that node 0 (corner) is pinned // Verify that node 0 (corner) is pinned
// Trilinear 3D has 8 nodes. // Trilinear 3D has 8 nodes.
@@ -901,14 +880,11 @@ TEST_F(UserFlexTest, MeshNodePinning) {
// m->nbody should reflect this. Flex bodies are added to the model. // m->nbody should reflect this. Flex bodies are added to the model.
// Model has 1 world body + flex bodies. // Model has 1 world body + flex bodies.
EXPECT_EQ(m->nbody, 1 + 7); // 1 world + 7 flex nodes (1 pinned) EXPECT_EQ(m->nbody, 1 + 7); // 1 world + 7 flex nodes (1 pinned)
mj_deleteModel(m);
} }
TEST_F(UserFlexTest, FlexcompMeshLoadsFromVFS) { TEST_F(UserFlexTest, FlexcompMeshLoadsFromVFS) {
// read cube.stl from testdata into a buffer // read cube.stl from testdata into a buffer
const std::string stl_path = const std::string stl_path = GetTestDataFilePath("user/testdata/cube.stl");
GetTestDataFilePath("user/testdata/cube.stl");
FILE* f = fopen(stl_path.c_str(), "rb"); FILE* f = fopen(stl_path.c_str(), "rb");
ASSERT_THAT(f, NotNull()) << "Could not open " << stl_path; ASSERT_THAT(f, NotNull()) << "Could not open " << stl_path;
fseek(f, 0, SEEK_END); fseek(f, 0, SEEK_END);
@@ -938,12 +914,10 @@ TEST_F(UserFlexTest, FlexcompMeshLoadsFromVFS) {
// cleanup // cleanup
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size(), &vfs); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size(), &vfs);
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m.get(), NotNull()) << error.data();
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
mj_step(m, d); mj_step(m.get(), d.get());
mj_deleteData(d);
mj_deleteModel(m);
mj_deleteVFS(&vfs); mj_deleteVFS(&vfs);
} }
@@ -986,22 +960,20 @@ TEST_F(UserFlexTest, FlexAttachConstraintPreserved) {
// First verify the standalone flex model has constraints // First verify the standalone flex model has constraints
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m_standalone = MjModelPtr m_standalone =
LoadModelFromString(flex_xml, error.data(), error.size(), vfs.get()); LoadModelFromString(flex_xml, error.data(), error.size(), vfs.get());
ASSERT_THAT(m_standalone, NotNull()) << error.data(); ASSERT_THAT(m_standalone.get(), NotNull()) << error.data();
mjData* d_standalone = mj_makeData(m_standalone); MjDataPtr d_standalone = MakeData(m_standalone);
mj_forward(m_standalone, d_standalone); mj_forward(m_standalone.get(), d_standalone.get());
int standalone_neq = m_standalone->neq; int standalone_neq = m_standalone->neq;
EXPECT_GT(standalone_neq, 0) << "Standalone flex should have constraints"; EXPECT_GT(standalone_neq, 0) << "Standalone flex should have constraints";
mj_deleteData(d_standalone);
mj_deleteModel(m_standalone);
// Now load the parent model which attaches the flex // Now load the parent model which attaches the flex
mjModel* m_attached = MjModelPtr m_attached =
LoadModelFromString(parent_xml, error.data(), error.size(), vfs.get()); LoadModelFromString(parent_xml, error.data(), error.size(), vfs.get());
ASSERT_THAT(m_attached, NotNull()) << error.data(); ASSERT_THAT(m_attached.get(), NotNull()) << error.data();
mjData* d_attached = mj_makeData(m_attached); MjDataPtr d_attached = MakeData(m_attached);
mj_forward(m_attached, d_attached); mj_forward(m_attached.get(), d_attached.get());
// THE BUG: flex constraints disappear when attached // THE BUG: flex constraints disappear when attached
EXPECT_GT(m_attached->neq, 0) EXPECT_GT(m_attached->neq, 0)
@@ -1009,8 +981,6 @@ TEST_F(UserFlexTest, FlexAttachConstraintPreserved) {
EXPECT_EQ(m_attached->neq, standalone_neq) EXPECT_EQ(m_attached->neq, standalone_neq)
<< "Attached flex should have same number of constraints as standalone"; << "Attached flex should have same number of constraints as standalone";
mj_deleteData(d_attached);
mj_deleteModel(m_attached);
mj_deleteVFS(vfs.get()); mj_deleteVFS(vfs.get());
} }
@@ -1024,11 +994,10 @@ TEST_F(UserFlexTest, FlexNoConstraintsWarning) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m.get(), NotNull()) << error.data();
EXPECT_THAT(error.data(), EXPECT_THAT(error.data(),
HasSubstr("no equality constraints or passive forces")); HasSubstr("no equality constraints or passive forces"));
mj_deleteModel(m);
} }
TEST_F(UserFlexTest, EmptyCellNodePinning) { TEST_F(UserFlexTest, EmptyCellNodePinning) {
@@ -1046,8 +1015,8 @@ TEST_F(UserFlexTest, EmptyCellNodePinning) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m.get(), NotNull()) << error.data();
// A 2x2x2 grid with trilinear order has (2+1)^3 = 27 node positions. // A 2x2x2 grid with trilinear order has (2+1)^3 = 27 node positions.
int nadr = m->flex_nodeadr[0]; int nadr = m->flex_nodeadr[0];
@@ -1065,20 +1034,16 @@ TEST_F(UserFlexTest, EmptyCellNodePinning) {
EXPECT_EQ(pinned, 0); EXPECT_EQ(pinned, 0);
// Verify simulation works // Verify simulation works
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
for (int i = 0; i < 10; i++) { for (int i = 0; i < 10; i++) {
mj_step(m, d); mj_step(m.get(), d.get());
} }
mj_deleteData(d);
mj_deleteModel(m);
} }
TEST_F(UserFlexTest, EmptyCellNodePinningMesh) { TEST_F(UserFlexTest, EmptyCellNodePinningMesh) {
// Load bunny_multicell.xml which has a 3x3x3 grid. // Load bunny_multicell.xml which has a 3x3x3 grid.
// The bunny mesh only occupies some cells, so many nodes should be pinned. // The bunny mesh only occupies some cells, so many nodes should be pinned.
const std::string xml_path = const std::string xml_path = GetModelPath("flex/bunny_multicell.xml");
GetModelPath("flex/bunny_multicell.xml");
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m, NotNull()) << error.data();
@@ -1152,8 +1117,8 @@ TEST_F(UserFlexTest, EmptyCellNodePinningQuadratic) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m.get(), NotNull()) << error.data();
// 3x1x1 quadratic grid: (3*2+1) * (1*2+1) * (1*2+1) = 7*3*3 = 63 nodes // 3x1x1 quadratic grid: (3*2+1) * (1*2+1) * (1*2+1) = 7*3*3 = 63 nodes
int nadr = m->flex_nodeadr[0]; int nadr = m->flex_nodeadr[0];
@@ -1161,7 +1126,7 @@ TEST_F(UserFlexTest, EmptyCellNodePinningQuadratic) {
EXPECT_EQ(nnode, 63); EXPECT_EQ(nnode, 63);
// Count pinned nodes: pinned nodes are assigned to the parent body. // Count pinned nodes: pinned nodes are assigned to the parent body.
int parent_bid = mj_name2id(m, mjOBJ_BODY, "parent"); int parent_bid = mj_name2id(m.get(), mjOBJ_BODY, "parent");
ASSERT_GT(parent_bid, 0); ASSERT_GT(parent_bid, 0);
int pinned = 0; int pinned = 0;
for (int n = nadr; n < nadr + nnode; n++) { for (int n = nadr; n < nadr + nnode; n++) {
@@ -1175,8 +1140,7 @@ TEST_F(UserFlexTest, EmptyCellNodePinningQuadratic) {
// That's 4 * 3 * 3 = 36 nodes. // That's 4 * 3 * 3 = 36 nodes.
EXPECT_EQ(pinned, 36); EXPECT_EQ(pinned, 36);
mj_deleteData(mj_makeData(m)); mj_deleteData(mj_makeData(m.get()));
mj_deleteModel(m);
} }
TEST_F(UserFlexTest, EmptyCellDetectsElements) { TEST_F(UserFlexTest, EmptyCellDetectsElements) {
@@ -1208,8 +1172,8 @@ TEST_F(UserFlexTest, EmptyCellDetectsElements) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m.get(), NotNull()) << error.data();
// 6x6x6 trilinear grid: (6+1)^3 = 343 nodes // 6x6x6 trilinear grid: (6+1)^3 = 343 nodes
int nadr = m->flex_nodeadr[0]; int nadr = m->flex_nodeadr[0];
@@ -1217,7 +1181,7 @@ TEST_F(UserFlexTest, EmptyCellDetectsElements) {
ASSERT_EQ(nnode, 343); ASSERT_EQ(nnode, 343);
// Count pinned nodes: those assigned to the parent body. // Count pinned nodes: those assigned to the parent body.
int parent_bid = mj_name2id(m, mjOBJ_BODY, "parent"); int parent_bid = mj_name2id(m.get(), mjOBJ_BODY, "parent");
ASSERT_GT(parent_bid, 0); ASSERT_GT(parent_bid, 0);
int pinned = 0; int pinned = 0;
for (int n = nadr; n < nadr + nnode; n++) { for (int n = nadr; n < nadr + nnode; n++) {
@@ -1233,8 +1197,7 @@ TEST_F(UserFlexTest, EmptyCellDetectsElements) {
// No cells are empty → 0 nodes pinned. // No cells are empty → 0 nodes pinned.
EXPECT_EQ(pinned, 0); EXPECT_EQ(pinned, 0);
mj_deleteData(mj_makeData(m)); mj_deleteData(mj_makeData(m.get()));
mj_deleteModel(m);
} }
TEST_F(UserFlexTest, TotalMassTrilinear) { TEST_F(UserFlexTest, TotalMassTrilinear) {
@@ -1249,8 +1212,8 @@ TEST_F(UserFlexTest, TotalMassTrilinear) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m.get(), NotNull()) << error.data();
double total_mass = 0; double total_mass = 0;
for (int i = 1; i < m->nbody; ++i) { for (int i = 1; i < m->nbody; ++i) {
@@ -1258,7 +1221,6 @@ TEST_F(UserFlexTest, TotalMassTrilinear) {
} }
EXPECT_NEAR(total_mass, 1.5, 1e-5); EXPECT_NEAR(total_mass, 1.5, 1e-5);
mj_deleteModel(m);
} }
TEST_F(UserFlexTest, TotalMassQuadratic) { TEST_F(UserFlexTest, TotalMassQuadratic) {
@@ -1273,8 +1235,8 @@ TEST_F(UserFlexTest, TotalMassQuadratic) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m.get(), NotNull()) << error.data();
double total_mass = 0; double total_mass = 0;
for (int i = 1; i < m->nbody; ++i) { for (int i = 1; i < m->nbody; ++i) {
@@ -1282,7 +1244,6 @@ TEST_F(UserFlexTest, TotalMassQuadratic) {
} }
EXPECT_NEAR(total_mass, 2.0, 1e-5); EXPECT_NEAR(total_mass, 2.0, 1e-5);
mj_deleteModel(m);
} }
TEST_F(UserFlexTest, Dof2d) { TEST_F(UserFlexTest, Dof2d) {
@@ -1298,7 +1259,8 @@ TEST_F(UserFlexTest, Dof2d) {
</mujoco> </mujoco>
)"; )";
// same model with dof="full" for comparison: 9 vertices, 3 DOFs each -> nv = 27 // same model with dof="full" for comparison: 9 vertices, 3 DOFs each -> nv =
// 27
static constexpr char xml_full[] = R"( static constexpr char xml_full[] = R"(
<mujoco> <mujoco>
<worldbody> <worldbody>
@@ -1313,14 +1275,14 @@ TEST_F(UserFlexTest, Dof2d) {
std::array<char, 1024> error; std::array<char, 1024> error;
// load 2d model // load 2d model
mjModel* m_2d = LoadModelFromString(xml_2d, error.data(), error.size()); MjModelPtr m_2d = LoadModelFromString(xml_2d, error.data(), error.size());
ASSERT_THAT(m_2d, NotNull()) << error.data(); ASSERT_THAT(m_2d.get(), NotNull()) << error.data();
mjData* d_2d = mj_makeData(m_2d); MjDataPtr d_2d = MakeData(m_2d);
// load full model // load full model
mjModel* m_full = LoadModelFromString(xml_full, error.data(), error.size()); MjModelPtr m_full = LoadModelFromString(xml_full, error.data(), error.size());
ASSERT_THAT(m_full, NotNull()) << error.data(); ASSERT_THAT(m_full.get(), NotNull()) << error.data();
mjData* d_full = mj_makeData(m_full); MjDataPtr d_full = MakeData(m_full);
// verify DOF counts // verify DOF counts
EXPECT_EQ(m_2d->nv, 18); // 9 vertices * 2 DOFs EXPECT_EQ(m_2d->nv, 18); // 9 vertices * 2 DOFs
@@ -1340,14 +1302,9 @@ TEST_F(UserFlexTest, Dof2d) {
// simulate a few steps to make sure nothing crashes // simulate a few steps to make sure nothing crashes
for (int i = 0; i < 10; i++) { for (int i = 0; i < 10; i++) {
mj_step(m_2d, d_2d); mj_step(m_2d.get(), d_2d.get());
mj_step(m_full, d_full); mj_step(m_full.get(), d_full.get());
} }
mj_deleteModel(m_2d);
mj_deleteModel(m_full);
mj_deleteData(d_2d);
mj_deleteData(d_full);
} }
TEST_F(UserFlexTest, Vert0RotationInvariant) { TEST_F(UserFlexTest, Vert0RotationInvariant) {
@@ -1380,11 +1337,12 @@ TEST_F(UserFlexTest, Vert0RotationInvariant) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m1 = LoadModelFromString(xml_unrotated, error.data(), error.size()); MjModelPtr m1 =
ASSERT_THAT(m1, NotNull()) << error.data(); LoadModelFromString(xml_unrotated, error.data(), error.size());
ASSERT_THAT(m1.get(), NotNull()) << error.data();
mjModel* m2 = LoadModelFromString(xml_rotated, error.data(), error.size()); MjModelPtr m2 = LoadModelFromString(xml_rotated, error.data(), error.size());
ASSERT_THAT(m2, NotNull()) << error.data(); ASSERT_THAT(m2.get(), NotNull()) << error.data();
// same number of vertices // same number of vertices
ASSERT_EQ(m1->nflexvert, m2->nflexvert); ASSERT_EQ(m1->nflexvert, m2->nflexvert);
@@ -1394,9 +1352,6 @@ TEST_F(UserFlexTest, Vert0RotationInvariant) {
EXPECT_NEAR(m1->flex_vert0[i], m2->flex_vert0[i], 1e-10) EXPECT_NEAR(m1->flex_vert0[i], m2->flex_vert0[i], 1e-10)
<< "vert0 mismatch at index " << i; << "vert0 mismatch at index " << i;
} }
mj_deleteModel(m1);
mj_deleteModel(m2);
} }
TEST_F(UserFlexTest, Load1DFlexFromOBJ) { TEST_F(UserFlexTest, Load1DFlexFromOBJ) {
@@ -1413,4 +1368,3 @@ TEST_F(UserFlexTest, Load1DFlexFromOBJ) {
} // namespace } // namespace
} // namespace mujoco } // namespace mujoco
+155 -204
View File
@@ -37,16 +37,12 @@ namespace {
using MjCMeshTest = MujocoTest; using MjCMeshTest = MujocoTest;
static const char* const kMeshPath = static const char* const kMeshPath = "user/testdata/mesh.xml";
"user/testdata/mesh.xml";
static const char* const kDuplicateVerticesPath = static const char* const kDuplicateVerticesPath =
"user/testdata/duplicate_vertices.xml"; "user/testdata/duplicate_vertices.xml";
static const char* const kCubePath = static const char* const kCubePath = "user/testdata/cube.xml";
"user/testdata/cube.xml"; static const char* const kCubeCompletePath = "user/testdata/cube_complete.obj";
static const char* const kCubeCompletePath = static const char* const kTorusPath = "user/testdata/torus.xml";
"user/testdata/cube_complete.obj";
static const char* const kTorusPath =
"user/testdata/torus.xml";
static const char* const kTorusMaxhullVertPath = static const char* const kTorusMaxhullVertPath =
"user/testdata/torus_maxhullvert.xml"; "user/testdata/torus_maxhullvert.xml";
static const char* const kTorusDefaultMaxhullVertPath = static const char* const kTorusDefaultMaxhullVertPath =
@@ -57,18 +53,14 @@ static const char* const kConvexInertiaPath =
"user/testdata/inertia_convex.xml"; "user/testdata/inertia_convex.xml";
static const char* const kConcaveInertiaPath = static const char* const kConcaveInertiaPath =
"user/testdata/inertia_concave.xml"; "user/testdata/inertia_concave.xml";
static const char* const kShellInertiaPath = static const char* const kShellInertiaPath = "user/testdata/inertia_shell.xml";
"user/testdata/inertia_shell.xml"; static const char* const kTorusQuadsPath = "user/testdata/torus_quads.xml";
static const char* const kTorusQuadsPath =
"user/testdata/torus_quads.xml";
static const char* const kTexturedTorusPath = static const char* const kTexturedTorusPath =
"user/testdata/textured_torus.xml"; "user/testdata/textured_torus.xml";
static const char* const kDuplicateOBJPath = static const char* const kDuplicateOBJPath = "user/testdata/duplicate.xml";
"user/testdata/duplicate.xml";
static const char* const kMalformedFaceOBJPath = static const char* const kMalformedFaceOBJPath =
"user/testdata/malformed_face.xml"; "user/testdata/malformed_face.xml";
static const char* const kCubeSkinPath = static const char* const kCubeSkinPath = "user/testdata/cube_skin.xml";
"user/testdata/cube_skin.xml";
static const char* const kNoDecoderForMeshErrorMsh = static const char* const kNoDecoderForMeshErrorMsh =
"no decoder found for mesh"; "no decoder found for mesh";
@@ -94,13 +86,8 @@ TEST_F(MjCMeshTest, UnknownMeshFormat) {
</mujoco> </mujoco>
)"; )";
std::vector<std::string> invalid_names = { std::vector<std::string> invalid_names = {"noextension", "anobj", "f",
"noextension", "mesh.exe", "file%s"};
"anobj",
"f",
"mesh.exe",
"file%s"
};
mjVFS vfs; mjVFS vfs;
mj_defaultVFS(&vfs); mj_defaultVFS(&vfs);
@@ -108,9 +95,9 @@ TEST_F(MjCMeshTest, UnknownMeshFormat) {
mj_addBufferVFS(&vfs, name.c_str(), nullptr, 0); mj_addBufferVFS(&vfs, name.c_str(), nullptr, 0);
std::string xml = absl::StrFormat(xml_format, name); std::string xml = absl::StrFormat(xml_format, name);
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = MjModelPtr model =
LoadModelFromString(xml.c_str(), error.data(), error.size(), &vfs); LoadModelFromString(xml.c_str(), error.data(), error.size(), &vfs);
ASSERT_THAT(model, testing::IsNull()) ASSERT_THAT(model.get(), testing::IsNull())
<< "Should fail to load a mesh named: " << name; << "Should fail to load a mesh named: " << name;
EXPECT_THAT(error.data(), HasSubstr(kNoDecoderForMeshErrorMsh)); EXPECT_THAT(error.data(), HasSubstr(kNoDecoderForMeshErrorMsh));
EXPECT_THAT(error.data(), HasSubstr(name)); EXPECT_THAT(error.data(), HasSubstr(name));
@@ -141,8 +128,8 @@ TEST_F(MjCMeshTest, LoadMSHWithVFS) {
mj_defaultVFS(vfs.get()); mj_defaultVFS(vfs.get());
// should fallback to OS filesystem // should fallback to OS filesystem
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull()); EXPECT_THAT(model.get(), IsNull());
EXPECT_THAT(error, HasSubstr("Error opening file")); EXPECT_THAT(error, HasSubstr("Error opening file"));
mj_deleteVFS(vfs.get()); mj_deleteVFS(vfs.get());
} }
@@ -167,8 +154,8 @@ TEST_F(MjCMeshTest, LoadOBJWithVFS) {
mj_defaultVFS(vfs.get()); mj_defaultVFS(vfs.get());
// should fallback to OS filesystem // should fallback to OS filesystem
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull()); EXPECT_THAT(model.get(), IsNull());
EXPECT_THAT(error, HasSubstr("Error opening file")); EXPECT_THAT(error, HasSubstr("Error opening file"));
mj_deleteVFS(vfs.get()); mj_deleteVFS(vfs.get());
} }
@@ -193,8 +180,8 @@ TEST_F(MjCMeshTest, LoadSTLWithVFS) {
mj_defaultVFS(vfs.get()); mj_defaultVFS(vfs.get());
// should fallback to OS filesystem // should fallback to OS filesystem
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull()); EXPECT_THAT(model.get(), IsNull());
EXPECT_THAT(error, HasSubstr("Error opening file")); EXPECT_THAT(error, HasSubstr("Error opening file"));
mj_deleteVFS(vfs.get()); mj_deleteVFS(vfs.get());
} }
@@ -221,8 +208,8 @@ TEST_F(MjCMeshTest, LoadMSHWithContentType) {
mj_defaultVFS(vfs.get()); mj_defaultVFS(vfs.get());
// should try opening the file (not found obviously) // should try opening the file (not found obviously)
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull()); EXPECT_THAT(model.get(), IsNull());
EXPECT_THAT(error, HasSubstr("Error opening file")); EXPECT_THAT(error, HasSubstr("Error opening file"));
mj_deleteVFS(vfs.get()); mj_deleteVFS(vfs.get());
} }
@@ -247,8 +234,8 @@ TEST_F(MjCMeshTest, LoadOBJWithContentType) {
mj_defaultVFS(vfs.get()); mj_defaultVFS(vfs.get());
// should try opening the file (not found obviously) // should try opening the file (not found obviously)
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull()); EXPECT_THAT(model.get(), IsNull());
EXPECT_THAT(error, HasSubstr("Error opening file")); EXPECT_THAT(error, HasSubstr("Error opening file"));
mj_deleteVFS(vfs.get()); mj_deleteVFS(vfs.get());
} }
@@ -273,8 +260,8 @@ TEST_F(MjCMeshTest, LoadSTLWithContentType) {
mj_defaultVFS(vfs.get()); mj_defaultVFS(vfs.get());
// should try opening the file (not found obviously) // should try opening the file (not found obviously)
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull()); EXPECT_THAT(model.get(), IsNull());
EXPECT_THAT(error, HasSubstr("Error opening file")); EXPECT_THAT(error, HasSubstr("Error opening file"));
mj_deleteVFS(vfs.get()); mj_deleteVFS(vfs.get());
} }
@@ -300,8 +287,8 @@ TEST_F(MjCMeshTest, LoadMSHWithContentTypeError) {
mj_addBufferVFS(&vfs, "some_file", nullptr, 0); mj_addBufferVFS(&vfs, "some_file", nullptr, 0);
// should error with unknown file type // should error with unknown file type
mjModel* model = LoadModelFromString(xml, error, error_sz, &vfs); MjModelPtr model = LoadModelFromString(xml, error, error_sz, &vfs);
EXPECT_THAT(model, IsNull()); EXPECT_THAT(model.get(), IsNull());
EXPECT_THAT(error, HasSubstr(kNoDecoderForMeshErrorMsh)); EXPECT_THAT(error, HasSubstr(kNoDecoderForMeshErrorMsh));
mj_deleteVFS(&vfs); mj_deleteVFS(&vfs);
} }
@@ -327,8 +314,8 @@ TEST_F(MjCMeshTest, LoadMSHWithInvalidContentType) {
mj_addBufferVFS(&vfs, "some_file", nullptr, 0); mj_addBufferVFS(&vfs, "some_file", nullptr, 0);
// should error with unknown file type // should error with unknown file type
mjModel* model = LoadModelFromString(xml, error, error_sz, &vfs); MjModelPtr model = LoadModelFromString(xml, error, error_sz, &vfs);
EXPECT_THAT(model, IsNull()); EXPECT_THAT(model.get(), IsNull());
EXPECT_THAT(error, HasSubstr(kNoDecoderForMeshErrorMsh)); EXPECT_THAT(error, HasSubstr(kNoDecoderForMeshErrorMsh));
mj_deleteVFS(&vfs); mj_deleteVFS(&vfs);
} }
@@ -353,8 +340,8 @@ TEST_F(MjCMeshTest, LoadMSHWithContentTypeParam) {
mj_defaultVFS(vfs.get()); mj_defaultVFS(vfs.get());
// should try opening the file (not found obviously) // should try opening the file (not found obviously)
mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); MjModelPtr model = LoadModelFromString(xml, error, error_sz, vfs.get());
EXPECT_THAT(model, IsNull()); EXPECT_THAT(model.get(), IsNull());
EXPECT_THAT(error, HasSubstr("Error opening file")); EXPECT_THAT(error, HasSubstr("Error opening file"));
mj_deleteVFS(vfs.get()); mj_deleteVFS(vfs.get());
} }
@@ -438,8 +425,7 @@ TEST_F(MjCMeshTest, SaveMeshOnce) {
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
// Confirm the mesh file is loaded and stored in paths // Confirm the mesh file is loaded and stored in paths
EXPECT_EQ( EXPECT_EQ(std::string(&model->paths[model->mesh_pathadr[0]]), "cube.obj");
std::string(&model->paths[model->mesh_pathadr[0]]), "cube.obj");
std::string saved_xml = SaveAndReadXml(model); std::string saved_xml = SaveAndReadXml(model);
EXPECT_THAT(saved_xml, Not(testing::HasSubstr("vertex"))); EXPECT_THAT(saved_xml, Not(testing::HasSubstr("vertex")));
mj_deleteModel(model); mj_deleteModel(model);
@@ -457,9 +443,8 @@ TEST_F(MjCMeshTest, TinyMeshLoads) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mj_deleteModel(model);
} }
// ------------- test max hull vert ------------------------------------------- // ------------- test max hull vert -------------------------------------------
@@ -498,9 +483,8 @@ TEST_F(MjCMeshTest, FaceNormalAutogenerated) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mj_deleteModel(model);
} }
// ------------- test inertia ------------------------------------------------- // ------------- test inertia -------------------------------------------------
@@ -521,9 +505,8 @@ TEST_F(MjCMeshTest, SmallInertiaLoads) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mj_deleteModel(model);
} }
TEST_F(MjCMeshTest, TinyInertiaFails) { TEST_F(MjCMeshTest, TinyInertiaFails) {
@@ -586,9 +569,11 @@ TEST_F(MjCMeshTest, FlippedFaceFailsExactInertia) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::IsNull()); EXPECT_THAT(model.get(), testing::IsNull());
EXPECT_THAT(error.data(), HasSubstr( EXPECT_THAT(
error.data(),
HasSubstr(
"Error: faces of mesh 'example_mesh' have inconsistent orientation. " "Error: faces of mesh 'example_mesh' have inconsistent orientation. "
"Please check the faces containing the vertices 1 and 2.")); "Please check the faces containing the vertices 1 and 2."));
} }
@@ -622,10 +607,9 @@ TEST_F(MjCMeshTest, FlippedFaceAllowedWorld) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
CheckTetrahedronWasRescaled(model); CheckTetrahedronWasRescaled(model.get());
mj_deleteModel(model);
} }
TEST_F(MjCMeshTest, FlippedFaceAllowedNoMass) { TEST_F(MjCMeshTest, FlippedFaceAllowedNoMass) {
@@ -644,10 +628,9 @@ TEST_F(MjCMeshTest, FlippedFaceAllowedNoMass) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
CheckTetrahedronWasRescaled(model); CheckTetrahedronWasRescaled(model.get());
mj_deleteModel(model);
} }
TEST_F(MjCMeshTest, FlippedFaceAllowedInertial) { TEST_F(MjCMeshTest, FlippedFaceAllowedInertial) {
@@ -667,10 +650,9 @@ TEST_F(MjCMeshTest, FlippedFaceAllowedInertial) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
CheckTetrahedronWasRescaled(model); CheckTetrahedronWasRescaled(model.get());
mj_deleteModel(model);
} }
TEST_F(MjCMeshTest, FlippedFaceAllowedNegligibleArea) { TEST_F(MjCMeshTest, FlippedFaceAllowedNegligibleArea) {
@@ -689,10 +671,9 @@ TEST_F(MjCMeshTest, FlippedFaceAllowedNegligibleArea) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
CheckTetrahedronWasRescaled(model); CheckTetrahedronWasRescaled(model.get());
mj_deleteModel(model);
} }
TEST_F(MjCMeshTest, AreaTooSmall) { TEST_F(MjCMeshTest, AreaTooSmall) {
@@ -711,8 +692,8 @@ TEST_F(MjCMeshTest, AreaTooSmall) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, IsNull()); EXPECT_THAT(model.get(), IsNull());
EXPECT_THAT(error.data(), HasSubstr("mesh surface area is too small")); EXPECT_THAT(error.data(), HasSubstr("mesh surface area is too small"));
} }
@@ -732,10 +713,9 @@ TEST_F(MjCMeshTest, VolumeTooSmall) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::IsNull()); EXPECT_THAT(model.get(), testing::IsNull());
EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small")); EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small"));
mj_deleteModel(model);
} }
TEST_F(MjCMeshTest, VisualVolumeTooSmall) { TEST_F(MjCMeshTest, VisualVolumeTooSmall) {
@@ -759,10 +739,9 @@ TEST_F(MjCMeshTest, VisualVolumeTooSmall) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::IsNull()); EXPECT_THAT(model.get(), testing::IsNull());
EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small")); EXPECT_THAT(error.data(), HasSubstr("mesh volume is too small"));
mj_deleteModel(model);
} }
TEST_F(MjCMeshTest, VisualVolumeSmallAllowedShell) { TEST_F(MjCMeshTest, VisualVolumeSmallAllowedShell) {
@@ -786,12 +765,11 @@ TEST_F(MjCMeshTest, VisualVolumeSmallAllowedShell) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_LE(mju_abs(model->geom_size[0]), 1); EXPECT_LE(mju_abs(model->geom_size[0]), 1);
EXPECT_LE(mju_abs(model->geom_size[1]), 1); EXPECT_LE(mju_abs(model->geom_size[1]), 1);
EXPECT_LE(mju_abs(model->geom_size[2]), 1); EXPECT_LE(mju_abs(model->geom_size[2]), 1);
mj_deleteModel(model);
} }
TEST_F(MjCMeshTest, VolumeSmallAllowedShell) { TEST_F(MjCMeshTest, VolumeSmallAllowedShell) {
@@ -810,12 +788,11 @@ TEST_F(MjCMeshTest, VolumeSmallAllowedShell) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_LE(mju_abs(model->geom_size[0]), 1); EXPECT_LE(mju_abs(model->geom_size[0]), 1);
EXPECT_LE(mju_abs(model->geom_size[1]), 1); EXPECT_LE(mju_abs(model->geom_size[1]), 1);
EXPECT_LE(mju_abs(model->geom_size[2]), 1); EXPECT_LE(mju_abs(model->geom_size[2]), 1);
mj_deleteModel(model);
} }
TEST_F(MjCMeshTest, Flex2DElasticityRequiresPositiveThickness) { TEST_F(MjCMeshTest, Flex2DElasticityRequiresPositiveThickness) {
@@ -829,8 +806,8 @@ TEST_F(MjCMeshTest, Flex2DElasticityRequiresPositiveThickness) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::IsNull()); EXPECT_THAT(model.get(), testing::IsNull());
EXPECT_THAT(error.data(), EXPECT_THAT(error.data(),
HasSubstr("2d elasticity requires positive thickness")); HasSubstr("2d elasticity requires positive thickness"));
} }
@@ -847,9 +824,8 @@ TEST_F(MjCMeshTest, InterpolatedFlexSupportsBendElasticityWithWarning) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::NotNull()) << error.data(); EXPECT_THAT(model.get(), testing::NotNull()) << error.data();
mj_deleteModel(model);
} }
TEST_F(MjCMeshTest, InterpolatedFlexSupportsBothElasticityWithWarning) { TEST_F(MjCMeshTest, InterpolatedFlexSupportsBothElasticityWithWarning) {
@@ -864,9 +840,8 @@ TEST_F(MjCMeshTest, InterpolatedFlexSupportsBothElasticityWithWarning) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::NotNull()) << error.data(); EXPECT_THAT(model.get(), testing::NotNull()) << error.data();
mj_deleteModel(model);
} }
TEST_F(MjCMeshTest, Flex2DElasticityRequires2DFlex) { TEST_F(MjCMeshTest, Flex2DElasticityRequires2DFlex) {
@@ -880,8 +855,8 @@ TEST_F(MjCMeshTest, Flex2DElasticityRequires2DFlex) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::IsNull()); EXPECT_THAT(model.get(), testing::IsNull());
EXPECT_THAT(error.data(), HasSubstr("2d elasticity requires 2d flex")); EXPECT_THAT(error.data(), HasSubstr("2d elasticity requires 2d flex"));
} }
@@ -919,12 +894,13 @@ TEST_F(MjCMeshTest, VolumeNegativeThrowsError) {
std::string xml_str = xml; std::string xml_str = xml;
absl::StrReplaceAll({{"MESH_DEFINITIONS", mesh_definitions}, absl::StrReplaceAll({{"MESH_DEFINITIONS", mesh_definitions},
{"GEOM_DEFINITIONS", geom_definitions}}, &xml_str); {"GEOM_DEFINITIONS", geom_definitions}},
&xml_str);
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml_str.c_str(), MjModelPtr model =
error.data(), error.size()); LoadModelFromString(xml_str.c_str(), error.data(), error.size());
EXPECT_THAT(model, IsNull()); EXPECT_THAT(model.get(), IsNull());
EXPECT_THAT(error.data(), HasSubstr("mesh volume is negative")); EXPECT_THAT(error.data(), HasSubstr("mesh volume is negative"));
} }
} }
@@ -970,8 +946,8 @@ TEST_F(MjCMeshTest, ExactConcaveInertia) {
mjtNum d_cube = .5 - model->body_ipos[5]; mjtNum d_cube = .5 - model->body_ipos[5];
mjtNum d_hole = .55 - model->body_ipos[5]; mjtNum d_hole = .55 - model->body_ipos[5];
mjtNum I1 = I_cube - m_hole * (.8 * .8 + .8 * .8) / 12; mjtNum I1 = I_cube - m_hole * (.8 * .8 + .8 * .8) / 12;
mjtNum I2 = I_cube - m_hole*(.8*.8 + .9*.9)/12 mjtNum I2 = I_cube - m_hole * (.8 * .8 + .9 * .9) / 12 +
+ m_cube*d_cube*d_cube - m_hole*d_hole*d_hole; m_cube * d_cube * d_cube - m_hole * d_hole * d_hole;
EXPECT_NEAR(model->body_mass[1], m_concave_cube, kMaxAbsErr); EXPECT_NEAR(model->body_mass[1], m_concave_cube, kMaxAbsErr);
EXPECT_NEAR(model->body_mass[2], m_concave_cube, kMaxAbsErr); EXPECT_NEAR(model->body_mass[2], m_concave_cube, kMaxAbsErr);
EXPECT_NEAR(model->body_mass[3], m_concave_cube, kMaxAbsErr); EXPECT_NEAR(model->body_mass[3], m_concave_cube, kMaxAbsErr);
@@ -1026,8 +1002,8 @@ TEST_F(MjCMeshTest, MeshPosQuat) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
// loading the mesh results in an offset of the geom's pos and quat due to the // loading the mesh results in an offset of the geom's pos and quat due to the
// fact that the geom's center is not the volumetric center of the mesh. To // fact that the geom's center is not the volumetric center of the mesh. To
// recover the geom's originally specified pose, the offset used is stored in // recover the geom's originally specified pose, the offset used is stored in
@@ -1035,16 +1011,15 @@ TEST_F(MjCMeshTest, MeshPosQuat) {
// and orientation, first invert the specified mesh_pos and mesh_quat // and orientation, first invert the specified mesh_pos and mesh_quat
mjtNum inverse_mesh_pos[3]; mjtNum inverse_mesh_pos[3];
mjtNum inverse_mesh_quat[4]; mjtNum inverse_mesh_quat[4];
mju_negPose(inverse_mesh_pos, inverse_mesh_quat, mju_negPose(inverse_mesh_pos, inverse_mesh_quat, &model->mesh_pos[0],
&model->mesh_pos[0], &model->mesh_quat[0]); &model->mesh_quat[0]);
// apply the inverted mesh_pos and inverted mesh_quat to the geom's pos and // apply the inverted mesh_pos and inverted mesh_quat to the geom's pos and
// quat. It should match the originally specified values // quat. It should match the originally specified values
mjtNum recovered_pos[3]; mjtNum recovered_pos[3];
mjtNum recovered_quat[4]; mjtNum recovered_quat[4];
mju_mulPose(recovered_pos, recovered_quat, mju_mulPose(recovered_pos, recovered_quat, &model->geom_pos[0],
&model->geom_pos[0], &model->geom_quat[0], &model->geom_quat[0], inverse_mesh_pos, inverse_mesh_quat);
inverse_mesh_pos, inverse_mesh_quat);
EXPECT_NEAR(recovered_pos[0], 0, MjTol(1e-12, 1e-6)); EXPECT_NEAR(recovered_pos[0], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[1], 0, MjTol(1e-12, 1e-6)); EXPECT_NEAR(recovered_pos[1], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[2], 0, MjTol(1e-12, 1e-6)); EXPECT_NEAR(recovered_pos[2], 0, MjTol(1e-12, 1e-6));
@@ -1055,11 +1030,10 @@ TEST_F(MjCMeshTest, MeshPosQuat) {
EXPECT_NEAR(recovered_quat[3], 0, MjTol(1e-12, 1e-6)); EXPECT_NEAR(recovered_quat[3], 0, MjTol(1e-12, 1e-6));
// same test on the other geom // same test on the other geom
mju_negPose(inverse_mesh_pos, inverse_mesh_quat, mju_negPose(inverse_mesh_pos, inverse_mesh_quat, &model->mesh_pos[0],
&model->mesh_pos[0], &model->mesh_quat[0]); &model->mesh_quat[0]);
mju_mulPose(recovered_pos, recovered_quat, mju_mulPose(recovered_pos, recovered_quat, &model->geom_pos[3],
&model->geom_pos[3], &model->geom_quat[4], &model->geom_quat[4], inverse_mesh_pos, inverse_mesh_quat);
inverse_mesh_pos, inverse_mesh_quat);
EXPECT_NEAR(recovered_pos[0], 1, MjTol(1e-12, 1e-6)); EXPECT_NEAR(recovered_pos[0], 1, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[1], 2, MjTol(1e-12, 1e-6)); EXPECT_NEAR(recovered_pos[1], 2, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[2], 3, MjTol(1e-12, 1e-6)); EXPECT_NEAR(recovered_pos[2], 3, MjTol(1e-12, 1e-6));
@@ -1068,8 +1042,6 @@ TEST_F(MjCMeshTest, MeshPosQuat) {
EXPECT_NEAR(recovered_quat[1], 0.5, MjTol(1e-12, 1e-6)); EXPECT_NEAR(recovered_quat[1], 0.5, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[2], 0.5, MjTol(1e-12, 1e-6)); EXPECT_NEAR(recovered_quat[2], 0.5, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[3], 0.5, MjTol(1e-12, 1e-6)); EXPECT_NEAR(recovered_quat[3], 0.5, MjTol(1e-12, 1e-6));
mj_deleteModel(model);
} }
TEST_F(MjCMeshTest, MeshPosQuatShellInertia) { TEST_F(MjCMeshTest, MeshPosQuatShellInertia) {
@@ -1085,8 +1057,8 @@ TEST_F(MjCMeshTest, MeshPosQuatShellInertia) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
// loading the mesh results in an offset of the geom's pos and quat due to the // loading the mesh results in an offset of the geom's pos and quat due to the
// fact that the geom's center is not the volumetric center of the mesh. To // fact that the geom's center is not the volumetric center of the mesh. To
// recover the geom's originally specified pose, the offset used is stored in // recover the geom's originally specified pose, the offset used is stored in
@@ -1094,16 +1066,15 @@ TEST_F(MjCMeshTest, MeshPosQuatShellInertia) {
// and orientation, first invert the specified mesh_pos and mesh_quat // and orientation, first invert the specified mesh_pos and mesh_quat
mjtNum inverse_mesh_pos[3]; mjtNum inverse_mesh_pos[3];
mjtNum inverse_mesh_quat[4]; mjtNum inverse_mesh_quat[4];
mju_negPose(inverse_mesh_pos, inverse_mesh_quat, mju_negPose(inverse_mesh_pos, inverse_mesh_quat, &model->mesh_pos[0],
&model->mesh_pos[0], &model->mesh_quat[0]); &model->mesh_quat[0]);
// apply the inverted mesh_pos and inverted mesh_quat to the geom's pos and // apply the inverted mesh_pos and inverted mesh_quat to the geom's pos and
// quat. It should match the originally specified values // quat. It should match the originally specified values
mjtNum recovered_pos[3]; mjtNum recovered_pos[3];
mjtNum recovered_quat[4]; mjtNum recovered_quat[4];
mju_mulPose(recovered_pos, recovered_quat, mju_mulPose(recovered_pos, recovered_quat, &model->geom_pos[0],
&model->geom_pos[0], &model->geom_quat[0], &model->geom_quat[0], inverse_mesh_pos, inverse_mesh_quat);
inverse_mesh_pos, inverse_mesh_quat);
EXPECT_NEAR(recovered_pos[0], 0, MjTol(1e-12, 1e-6)); EXPECT_NEAR(recovered_pos[0], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[1], 0, MjTol(1e-12, 1e-6)); EXPECT_NEAR(recovered_pos[1], 0, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[2], 0, MjTol(1e-12, 1e-6)); EXPECT_NEAR(recovered_pos[2], 0, MjTol(1e-12, 1e-6));
@@ -1114,11 +1085,10 @@ TEST_F(MjCMeshTest, MeshPosQuatShellInertia) {
EXPECT_NEAR(recovered_quat[3], 0, MjTol(1e-12, 1e-6)); EXPECT_NEAR(recovered_quat[3], 0, MjTol(1e-12, 1e-6));
// same test on the other geom // same test on the other geom
mju_negPose(inverse_mesh_pos, inverse_mesh_quat, mju_negPose(inverse_mesh_pos, inverse_mesh_quat, &model->mesh_pos[0],
&model->mesh_pos[0], &model->mesh_quat[0]); &model->mesh_quat[0]);
mju_mulPose(recovered_pos, recovered_quat, mju_mulPose(recovered_pos, recovered_quat, &model->geom_pos[3],
&model->geom_pos[3], &model->geom_quat[4], &model->geom_quat[4], inverse_mesh_pos, inverse_mesh_quat);
inverse_mesh_pos, inverse_mesh_quat);
EXPECT_NEAR(recovered_pos[0], 1, MjTol(1e-12, 1e-6)); EXPECT_NEAR(recovered_pos[0], 1, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[1], 2, MjTol(1e-12, 1e-6)); EXPECT_NEAR(recovered_pos[1], 2, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_pos[2], 3, MjTol(1e-12, 1e-6)); EXPECT_NEAR(recovered_pos[2], 3, MjTol(1e-12, 1e-6));
@@ -1127,7 +1097,6 @@ TEST_F(MjCMeshTest, MeshPosQuatShellInertia) {
EXPECT_NEAR(recovered_quat[1], 0.5, MjTol(1e-12, 1e-6)); EXPECT_NEAR(recovered_quat[1], 0.5, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[2], 0.5, MjTol(1e-12, 1e-6)); EXPECT_NEAR(recovered_quat[2], 0.5, MjTol(1e-12, 1e-6));
EXPECT_NEAR(recovered_quat[3], 0.5, MjTol(1e-12, 1e-6)); EXPECT_NEAR(recovered_quat[3], 0.5, MjTol(1e-12, 1e-6));
mj_deleteModel(model);
} }
TEST_F(MjCMeshTest, MeshScale) { TEST_F(MjCMeshTest, MeshScale) {
@@ -1144,12 +1113,11 @@ TEST_F(MjCMeshTest, MeshScale) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_THAT(AsVector(model->mesh_scale + 0, 3), ElementsAre(1, 1, 1)); EXPECT_THAT(AsVector(model->mesh_scale + 0, 3), ElementsAre(1, 1, 1));
EXPECT_THAT(AsVector(model->mesh_scale + 3, 3), ElementsAre(0.9, 1, -1)); EXPECT_THAT(AsVector(model->mesh_scale + 3, 3), ElementsAre(0.9, 1, -1));
mj_deleteModel(model);
} }
TEST_F(MjCMeshTest, NegativeScaleUserMeshCompiles) { TEST_F(MjCMeshTest, NegativeScaleUserMeshCompiles) {
@@ -1166,9 +1134,8 @@ TEST_F(MjCMeshTest, NegativeScaleUserMeshCompiles) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mj_deleteModel(model);
} }
TEST_F(MjCMeshTest, NegativeScaleUserMeshMatchesPositiveScale) { TEST_F(MjCMeshTest, NegativeScaleUserMeshMatchesPositiveScale) {
@@ -1199,11 +1166,11 @@ TEST_F(MjCMeshTest, NegativeScaleUserMeshMatchesPositiveScale) {
)"; )";
char error[1024]; char error[1024];
mjModel* pos_model = LoadModelFromString(pos_xml, error, sizeof(error)); MjModelPtr pos_model = LoadModelFromString(pos_xml, error, sizeof(error));
ASSERT_THAT(pos_model, NotNull()) << error; ASSERT_THAT(pos_model.get(), NotNull()) << error;
mjModel* neg_model = LoadModelFromString(neg_xml, error, sizeof(error)); MjModelPtr neg_model = LoadModelFromString(neg_xml, error, sizeof(error));
ASSERT_THAT(neg_model, NotNull()) << error; ASSERT_THAT(neg_model.get(), NotNull()) << error;
ASSERT_EQ(pos_model->nmeshface, neg_model->nmeshface); ASSERT_EQ(pos_model->nmeshface, neg_model->nmeshface);
@@ -1212,9 +1179,6 @@ TEST_F(MjCMeshTest, NegativeScaleUserMeshMatchesPositiveScale) {
EXPECT_EQ(pos_model->mesh_face[3 * i + 1], neg_model->mesh_face[3 * i + 2]); EXPECT_EQ(pos_model->mesh_face[3 * i + 1], neg_model->mesh_face[3 * i + 2]);
EXPECT_EQ(pos_model->mesh_face[3 * i + 2], neg_model->mesh_face[3 * i + 1]); EXPECT_EQ(pos_model->mesh_face[3 * i + 2], neg_model->mesh_face[3 * i + 1]);
} }
mj_deleteModel(pos_model);
mj_deleteModel(neg_model);
} }
TEST_F(MjCMeshTest, ShellInertiaTest) { TEST_F(MjCMeshTest, ShellInertiaTest) {
@@ -1231,12 +1195,11 @@ TEST_F(MjCMeshTest, ShellInertiaTest) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
EXPECT_THAT(AsVector(model->mesh_scale + 0, 3), ElementsAre(1, 1, 1)); EXPECT_THAT(AsVector(model->mesh_scale + 0, 3), ElementsAre(1, 1, 1));
EXPECT_THAT(AsVector(model->mesh_scale + 3, 3), ElementsAre(0.9, 1, -1)); EXPECT_THAT(AsVector(model->mesh_scale + 3, 3), ElementsAre(0.9, 1, -1));
mj_deleteModel(model);
} }
// ----------------------------- texcoord ------------------------------------- // ----------------------------- texcoord -------------------------------------
@@ -1251,9 +1214,8 @@ TEST_F(MjCMeshTest, CreateFaceTexCoord) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mj_deleteModel(model);
} }
TEST_F(MjCMeshTest, UseFaceTexCoord) { TEST_F(MjCMeshTest, UseFaceTexCoord) {
@@ -1267,8 +1229,8 @@ TEST_F(MjCMeshTest, UseFaceTexCoord) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, NotNull()) << error.data(); EXPECT_THAT(model.get(), NotNull()) << error.data();
EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[0]], .0); EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[0]], .0);
EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[1]], .2); EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[1]], .2);
EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[2]], .1); EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[2]], .1);
@@ -1281,7 +1243,6 @@ TEST_F(MjCMeshTest, UseFaceTexCoord) {
EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[9]], .1); EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[9]], .1);
EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[10]], .2); EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[10]], .2);
EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[11]], .3); EXPECT_FLOAT_EQ(model->mesh_texcoord[2 * model->mesh_facetexcoord[11]], .3);
mj_deleteModel(model);
} }
TEST_F(MjCMeshTest, MissingTexCoord) { TEST_F(MjCMeshTest, MissingTexCoord) {
@@ -1294,8 +1255,8 @@ TEST_F(MjCMeshTest, MissingTexCoord) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::IsNull()); EXPECT_THAT(model.get(), testing::IsNull());
EXPECT_THAT(error.data(), HasSubstr("texcoord must be 2*nv")); EXPECT_THAT(error.data(), HasSubstr("texcoord must be 2*nv"));
} }
@@ -1312,10 +1273,9 @@ TEST_F(MjCMeshTest, NaNConvexHullDisallowed) {
MockWarningHandler warning_handler; MockWarningHandler warning_handler;
warning_handler.ExpectWarnings(); warning_handler.ExpectWarnings();
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, testing::IsNull()); EXPECT_THAT(model.get(), testing::IsNull());
EXPECT_THAT(error.data(), HasSubstr("vertex coordinate 0 is not finite")); EXPECT_THAT(error.data(), HasSubstr("vertex coordinate 0 is not finite"));
mj_deleteModel(model);
} }
TEST_F(MjCMeshTest, InvalidIndexInFace) { TEST_F(MjCMeshTest, InvalidIndexInFace) {
@@ -1333,10 +1293,9 @@ TEST_F(MjCMeshTest, InvalidIndexInFace) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, IsNull()); ASSERT_THAT(model.get(), IsNull());
EXPECT_THAT(error, HasSubstr("in face 0, vertex index 6 does not exist")); EXPECT_THAT(error, HasSubstr("in face 0, vertex index 6 does not exist"));
mj_deleteModel(model);
} }
TEST_F(MjCMeshTest, QhullCache) { TEST_F(MjCMeshTest, QhullCache) {
@@ -1367,17 +1326,16 @@ TEST_F(MjCMeshTest, QhullCache) {
mj_addFileVFS(&vfs, "", GetTestDataFilePath(kCubeCompletePath).c_str()); mj_addFileVFS(&vfs, "", GetTestDataFilePath(kCubeCompletePath).c_str());
std::array<char, 1000> error; std::array<char, 1000> error;
mjModel* model = LoadModelFromString(xml1, error.data(), error.size(), &vfs); MjModelPtr model =
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); LoadModelFromString(xml1, error.data(), error.size(), &vfs);
ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
EXPECT_THAT(model->mesh_graphadr[0], -1); EXPECT_THAT(model->mesh_graphadr[0], -1);
mj_deleteModel(model);
model = LoadModelFromString(xml2, error.data(), error.size(), &vfs); model = LoadModelFromString(xml2, error.data(), error.size(), &vfs);
ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); ASSERT_THAT(model.get(), NotNull())
<< "Failed to load model: " << error.data();
EXPECT_GT(model->mesh_graphadr[0], -1); EXPECT_GT(model->mesh_graphadr[0], -1);
mj_deleteModel(model);
mj_deleteVFS(&vfs); mj_deleteVFS(&vfs);
} }
@@ -1395,8 +1353,8 @@ TEST_F(MjCMeshTest, ColocatedMeshError) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, IsNull()); EXPECT_THAT(model.get(), IsNull());
EXPECT_THAT(error.data(), HasSubstr("colocated")); EXPECT_THAT(error.data(), HasSubstr("colocated"));
} }
@@ -1414,8 +1372,8 @@ TEST_F(MjCMeshTest, CollinearMeshError) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, IsNull()); EXPECT_THAT(model.get(), IsNull());
EXPECT_THAT(error.data(), HasSubstr("collinear")); EXPECT_THAT(error.data(), HasSubstr("collinear"));
} }
@@ -1433,8 +1391,8 @@ TEST_F(MjCMeshTest, CoplanarMeshError) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, IsNull()); EXPECT_THAT(model.get(), IsNull());
EXPECT_THAT(error.data(), HasSubstr("coplanar")); EXPECT_THAT(error.data(), HasSubstr("coplanar"));
} }
@@ -1478,8 +1436,7 @@ bool AreAabbsAdjacent(const mjtNum* aabb1, const mjtNum* aabb2) {
const mjtNum half_size1 = aabb1[dim + 3]; const mjtNum half_size1 = aabb1[dim + 3];
const mjtNum center2 = aabb2[dim]; const mjtNum center2 = aabb2[dim];
const mjtNum half_size2 = aabb2[dim + 3]; const mjtNum half_size2 = aabb2[dim + 3];
const mjtNum gap = const mjtNum gap = std::abs(center1 - center2) - (half_size1 + half_size2);
std::abs(center1 - center2) - (half_size1 + half_size2);
if (std::abs(gap) < kEps) { if (std::abs(gap) < kEps) {
touching_dims++; touching_dims++;
} else if (gap < -kEps) { } else if (gap < -kEps) {
@@ -1533,15 +1490,15 @@ TEST_F(MjCMeshTest, OctreeIsBalanced) {
const int level2 = model->oct_depth[octree_adr + node2_idx]; const int level2 = model->oct_depth[octree_adr + node2_idx];
if (std::abs(level1 - level2) > 1) { if (std::abs(level1 - level2) > 1) {
if (unbalanced_pairs < 10) { if (unbalanced_pairs < 10) {
ADD_FAILURE() ADD_FAILURE() << "Nodes " << node1_idx << " (level " << level1
<< "Nodes " << node1_idx << " (level " << level1 << ") and " << ") and " << node2_idx << " (level " << level2
<< node2_idx << " (level " << level2 << ") are not balanced." << ") are not balanced."
<< "\nAABB1: center=(" << aabb1[0] << ", " << aabb1[1] << ", " << "\nAABB1: center=(" << aabb1[0] << ", " << aabb1[1]
<< aabb1[2] << "), half_size=(" << aabb1[3] << ", " << ", " << aabb1[2] << "), half_size=(" << aabb1[3]
<< aabb1[4] << ", " << aabb1[5] << ")" << ", " << aabb1[4] << ", " << aabb1[5] << ")"
<< "\nAABB2: center=(" << aabb2[0] << ", " << aabb2[1] << ", " << "\nAABB2: center=(" << aabb2[0] << ", " << aabb2[1]
<< aabb2[2] << "), half_size=(" << aabb2[3] << ", " << ", " << aabb2[2] << "), half_size=(" << aabb2[3]
<< aabb2[4] << ", " << aabb2[5] << ")"; << ", " << aabb2[4] << ", " << aabb2[5] << ")";
} }
unbalanced_pairs++; unbalanced_pairs++;
} }
@@ -1610,8 +1567,7 @@ TEST_F(MjCMeshTest, OctreeHangingNodeInterpolation) {
// decide which node is finer and which is coarser // decide which node is finer and which is coarser
const int finer_node_idx = (level1 > level2) ? node1_idx : node2_idx; const int finer_node_idx = (level1 > level2) ? node1_idx : node2_idx;
const int coarser_node_idx = const int coarser_node_idx = (level1 > level2) ? node2_idx : node1_idx;
(level1 > level2) ? node2_idx : node1_idx;
const mjtNum* coarser_aabb = const mjtNum* coarser_aabb =
&model->oct_aabb[(octree_adr + coarser_node_idx) * 6]; &model->oct_aabb[(octree_adr + coarser_node_idx) * 6];
const mjtNum* finer_aabb = const mjtNum* finer_aabb =
@@ -1729,8 +1685,7 @@ TEST_F(MjCMeshTest, OctreeNotComputedForNonSDF) {
} }
mjtNum CubeSDF(mjtNum p[3], mjtNum b[3]) { mjtNum CubeSDF(mjtNum p[3], mjtNum b[3]) {
mjtNum q[3] = {mju_abs(p[0]) - b[0], mjtNum q[3] = {mju_abs(p[0]) - b[0], mju_abs(p[1]) - b[1],
mju_abs(p[1]) - b[1],
mju_abs(p[2]) - b[2]}; mju_abs(p[2]) - b[2]};
return mju_sqrt(std::pow(std::max(q[0], (mjtNum)0), 2) + return mju_sqrt(std::pow(std::max(q[0], (mjtNum)0), 2) +
std::pow(std::max(q[1], (mjtNum)0), 2) + std::pow(std::max(q[1], (mjtNum)0), 2) +
@@ -1751,12 +1706,12 @@ TEST_F(MjCMeshTest, OctreeCube) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m.get(), NotNull()) << error.data();
EXPECT_EQ(m->noct, 63497); EXPECT_EQ(m->noct, 63497);
mjData* d = mj_makeData(m); MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull()); ASSERT_THAT(d, NotNull());
mj_forward(m, d); mj_forward(m.get(), d.get());
mjSDF sdf; mjSDF sdf;
int instance = 0; int instance = 0;
mjtGeom geomtype = mjGEOM_SDF; mjtGeom geomtype = mjGEOM_SDF;
@@ -1768,11 +1723,10 @@ TEST_F(MjCMeshTest, OctreeCube) {
mjtNum pnt[3][3] = {{1, 1, 1}, {.5, .5, .5}, {.5, 0, 0}}; mjtNum pnt[3][3] = {{1, 1, 1}, {.5, .5, .5}, {.5, 0, 0}};
mjtNum size[3] = {1, 1, 1}; mjtNum size[3] = {1, 1, 1};
for (int i = 0; i < 3; ++i) { for (int i = 0; i < 3; ++i) {
EXPECT_NEAR(mjc_distance(m, d, &sdf, pnt[i]), CubeSDF(pnt[i], size), 1e-1) EXPECT_NEAR(mjc_distance(m.get(), d.get(), &sdf, pnt[i]),
CubeSDF(pnt[i], size), 1e-1)
<< "i = " << i; << "i = " << i;
} }
mj_deleteModel(m);
mj_deleteData(d);
} }
TEST_F(MjCMeshTest, HemisphereSizes) { TEST_F(MjCMeshTest, HemisphereSizes) {
@@ -1786,15 +1740,14 @@ TEST_F(MjCMeshTest, HemisphereSizes) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data(); ASSERT_THAT(model.get(), NotNull()) << error.data();
EXPECT_EQ(model->mesh_vertnum[0], 2 * (0 + 1) * (0 + 2) + 2); EXPECT_EQ(model->mesh_vertnum[0], 2 * (0 + 1) * (0 + 2) + 2);
EXPECT_EQ(model->mesh_vertnum[1], 2 * (1 + 1) * (1 + 2) + 2); EXPECT_EQ(model->mesh_vertnum[1], 2 * (1 + 1) * (1 + 2) + 2);
EXPECT_EQ(model->mesh_vertnum[2], 2 * (2 + 1) * (2 + 2) + 2); EXPECT_EQ(model->mesh_vertnum[2], 2 * (2 + 1) * (2 + 2) + 2);
EXPECT_EQ(model->mesh_facenum[0], 4 * (0 + 1) * (0 + 2)); EXPECT_EQ(model->mesh_facenum[0], 4 * (0 + 1) * (0 + 2));
EXPECT_EQ(model->mesh_facenum[1], 4 * (1 + 1) * (1 + 2)); EXPECT_EQ(model->mesh_facenum[1], 4 * (1 + 1) * (1 + 2));
EXPECT_EQ(model->mesh_facenum[2], 4 * (2 + 1) * (2 + 2)); EXPECT_EQ(model->mesh_facenum[2], 4 * (2 + 1) * (2 + 2));
mj_deleteModel(model);
} }
TEST_F(MjCMeshTest, SphereSizes) { TEST_F(MjCMeshTest, SphereSizes) {
@@ -1808,15 +1761,14 @@ TEST_F(MjCMeshTest, SphereSizes) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data(); ASSERT_THAT(model.get(), NotNull()) << error.data();
EXPECT_EQ(model->mesh_vertnum[0], 2 + 10 * std::pow(4, 0)); EXPECT_EQ(model->mesh_vertnum[0], 2 + 10 * std::pow(4, 0));
EXPECT_EQ(model->mesh_vertnum[1], 2 + 10 * std::pow(4, 1)); EXPECT_EQ(model->mesh_vertnum[1], 2 + 10 * std::pow(4, 1));
EXPECT_EQ(model->mesh_vertnum[2], 2 + 10 * std::pow(4, 2)); EXPECT_EQ(model->mesh_vertnum[2], 2 + 10 * std::pow(4, 2));
EXPECT_EQ(model->mesh_facenum[0], 20 * std::pow(4, 0)); EXPECT_EQ(model->mesh_facenum[0], 20 * std::pow(4, 0));
EXPECT_EQ(model->mesh_facenum[1], 20 * std::pow(4, 1)); EXPECT_EQ(model->mesh_facenum[1], 20 * std::pow(4, 1));
EXPECT_EQ(model->mesh_facenum[2], 20 * std::pow(4, 2)); EXPECT_EQ(model->mesh_facenum[2], 20 * std::pow(4, 2));
mj_deleteModel(model);
} }
TEST_F(MjCMeshTest, MeshMaterial) { TEST_F(MjCMeshTest, MeshMaterial) {
@@ -1835,11 +1787,10 @@ TEST_F(MjCMeshTest, MeshMaterial) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data(); ASSERT_THAT(model.get(), NotNull()) << error.data();
EXPECT_EQ(model->geom_matid[0], 1); EXPECT_EQ(model->geom_matid[0], 1);
EXPECT_EQ(model->geom_matid[1], 0); EXPECT_EQ(model->geom_matid[1], 0);
mj_deleteModel(model);
} }
} // namespace } // namespace
+69 -108
View File
@@ -95,15 +95,13 @@ TEST_F(UserModelTest, WeldRootID) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, NotNull()); EXPECT_THAT(model.get(), NotNull());
EXPECT_THAT(AsVector(model->body_rootid, model->nbody), EXPECT_THAT(AsVector(model->body_rootid, model->nbody),
ElementsAre(0, 1, 1, 3, 3, 5, 5, 7, 7, 9, 9)); ElementsAre(0, 1, 1, 3, 3, 5, 5, 7, 7, 9, 9));
EXPECT_THAT(AsVector(model->body_weldid, model->nbody), EXPECT_THAT(AsVector(model->body_weldid, model->nbody),
ElementsAre(0, 1, 2, 3, 3, 0, 0, 0, 0, 0, 10)); ElementsAre(0, 1, 2, 3, 3, 0, 0, 0, 0, 0, 10));
mj_deleteModel(model);
} }
TEST_F(UserModelTest, RepeatedNames) { TEST_F(UserModelTest, RepeatedNames) {
@@ -119,8 +117,8 @@ TEST_F(UserModelTest, RepeatedNames) {
</mujoco>)"; </mujoco>)";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, IsNull()); EXPECT_THAT(model.get(), IsNull());
EXPECT_THAT(error.data(), HasSubstr("repeated name 'geom1' in geom")); EXPECT_THAT(error.data(), HasSubstr("repeated name 'geom1' in geom"));
} }
@@ -143,8 +141,8 @@ TEST_F(UserModelTest, SameFrame) {
</mujoco>)"; </mujoco>)";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data(); ASSERT_THAT(model.get(), NotNull()) << error.data();
EXPECT_EQ(model->geom_sameframe[0], mjSAMEFRAME_NONE); EXPECT_EQ(model->geom_sameframe[0], mjSAMEFRAME_NONE);
EXPECT_EQ(model->geom_sameframe[1], mjSAMEFRAME_BODY); EXPECT_EQ(model->geom_sameframe[1], mjSAMEFRAME_BODY);
EXPECT_EQ(model->geom_sameframe[2], mjSAMEFRAME_INERTIA); EXPECT_EQ(model->geom_sameframe[2], mjSAMEFRAME_INERTIA);
@@ -152,8 +150,8 @@ TEST_F(UserModelTest, SameFrame) {
EXPECT_EQ(model->geom_sameframe[4], mjSAMEFRAME_INERTIAROT); EXPECT_EQ(model->geom_sameframe[4], mjSAMEFRAME_INERTIAROT);
// make data, get geom_xpos // make data, get geom_xpos
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
mj_kinematics(model, data); mj_kinematics(model.get(), data.get());
auto geom_xpos = AsVector(data->geom_xpos, model->ngeom * 3); auto geom_xpos = AsVector(data->geom_xpos, model->ngeom * 3);
auto geom_xmat = AsVector(data->geom_xmat, model->ngeom * 9); auto geom_xmat = AsVector(data->geom_xmat, model->ngeom * 9);
@@ -161,8 +159,8 @@ TEST_F(UserModelTest, SameFrame) {
for (int i = 0; i < model->ngeom; i++) { for (int i = 0; i < model->ngeom; i++) {
model->geom_sameframe[i] = mjSAMEFRAME_NONE; model->geom_sameframe[i] = mjSAMEFRAME_NONE;
} }
mj_resetData(model, data); mj_resetData(model.get(), data.get());
mj_kinematics(model, data); mj_kinematics(model.get(), data.get());
auto geom_xpos2 = AsVector(data->geom_xpos, model->ngeom * 3); auto geom_xpos2 = AsVector(data->geom_xpos, model->ngeom * 3);
auto geom_xmat2 = AsVector(data->geom_xmat, model->ngeom * 9); auto geom_xmat2 = AsVector(data->geom_xmat, model->ngeom * 9);
@@ -170,9 +168,6 @@ TEST_F(UserModelTest, SameFrame) {
constexpr double eps = 1e-6; constexpr double eps = 1e-6;
EXPECT_THAT(geom_xpos, Pointwise(MjNear(eps, eps), geom_xpos2)); EXPECT_THAT(geom_xpos, Pointwise(MjNear(eps, eps), geom_xpos2));
EXPECT_THAT(geom_xmat, Pointwise(MjNear(eps, eps), geom_xmat2)); EXPECT_THAT(geom_xmat, Pointwise(MjNear(eps, eps), geom_xmat2));
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(UserModelTest, ActuatorSparsity) { TEST_F(UserModelTest, ActuatorSparsity) {
@@ -194,9 +189,8 @@ TEST_F(UserModelTest, ActuatorSparsity) {
</actuator> </actuator>
</mujoco> </mujoco>
)"; )";
mjModel* m = LoadModelFromString(xml); MjModelPtr m = LoadModelFromString(xml);
ASSERT_EQ(m->nJmom, 2); ASSERT_EQ(m->nJmom, 2);
mj_deleteModel(m);
} }
TEST_F(UserModelTest, FixedTendonSparsity) { TEST_F(UserModelTest, FixedTendonSparsity) {
@@ -226,8 +220,8 @@ TEST_F(UserModelTest, FixedTendonSparsity) {
</tendon> </tendon>
</mujoco> </mujoco>
)"; )";
mjModel* m = LoadModelFromString(xml); MjModelPtr m = LoadModelFromString(xml);
ASSERT_THAT(m, NotNull()); ASSERT_THAT(m.get(), NotNull());
EXPECT_EQ(m->nJten, 3); EXPECT_EQ(m->nJten, 3);
EXPECT_EQ(m->ten_J_rownnz[0], 3); EXPECT_EQ(m->ten_J_rownnz[0], 3);
@@ -237,8 +231,6 @@ TEST_F(UserModelTest, FixedTendonSparsity) {
int rowadr = m->ten_J_rowadr[0]; int rowadr = m->ten_J_rowadr[0];
int* colind = m->ten_J_colind + rowadr; int* colind = m->ten_J_colind + rowadr;
EXPECT_THAT(std::vector<int>(colind, colind + 3), ElementsAre(0, 1, 2)); EXPECT_THAT(std::vector<int>(colind, colind + 3), ElementsAre(0, 1, 2));
mj_deleteModel(m);
} }
TEST_F(UserModelTest, NestedZeroMassBodiesOK) { TEST_F(UserModelTest, NestedZeroMassBodiesOK) {
@@ -259,9 +251,8 @@ TEST_F(UserModelTest, NestedZeroMassBodiesOK) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mj_deleteModel(model);
} }
TEST_F(UserModelTest, NestedZeroMassBodiesWithJointOK) { TEST_F(UserModelTest, NestedZeroMassBodiesWithJointOK) {
@@ -286,9 +277,8 @@ TEST_F(UserModelTest, NestedZeroMassBodiesWithJointOK) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error; ASSERT_THAT(model.get(), NotNull()) << error;
mj_deleteModel(model);
} }
TEST_F(UserModelTest, NestedZeroMassBodiesFail) { TEST_F(UserModelTest, NestedZeroMassBodiesFail) {
@@ -309,14 +299,13 @@ TEST_F(UserModelTest, NestedZeroMassBodiesFail) {
</mujoco> </mujoco>
)"; )";
char error[1024]; char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error)); MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, IsNull()); ASSERT_THAT(model.get(), IsNull());
EXPECT_THAT( EXPECT_THAT(
error, error,
HasSubstr( HasSubstr(
"mass and inertia of moving bodies must be larger than mjMINVAL")); "mass and inertia of moving bodies must be larger than mjMINVAL"));
EXPECT_THAT(error, HasSubstr("Element name 'bad'")); EXPECT_THAT(error, HasSubstr("Element name 'bad'"));
mj_deleteModel(model);
} }
TEST_F(UserModelTest, ConvexHullForCollisionMeshes) { TEST_F(UserModelTest, ConvexHullForCollisionMeshes) {
@@ -338,14 +327,14 @@ TEST_F(UserModelTest, ConvexHullForCollisionMeshes) {
</mujoco>)"; </mujoco>)";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data(); ASSERT_THAT(model.get(), NotNull()) << error.data();
int no_hull_id = mj_name2id(model, mjOBJ_MESH, "mesh_no_hull"); int no_hull_id = mj_name2id(model.get(), mjOBJ_MESH, "mesh_no_hull");
int with_hull_contype_id = int with_hull_contype_id =
mj_name2id(model, mjOBJ_MESH, "mesh_with_hull_contype"); mj_name2id(model.get(), mjOBJ_MESH, "mesh_with_hull_contype");
int with_hull_conaffinity_id = int with_hull_conaffinity_id =
mj_name2id(model, mjOBJ_MESH, "mesh_with_hull_conaffinity"); mj_name2id(model.get(), mjOBJ_MESH, "mesh_with_hull_conaffinity");
EXPECT_NE(no_hull_id, -1); EXPECT_NE(no_hull_id, -1);
EXPECT_NE(with_hull_contype_id, -1); EXPECT_NE(with_hull_contype_id, -1);
@@ -359,8 +348,6 @@ TEST_F(UserModelTest, ConvexHullForCollisionMeshes) {
// mesh_with_hull_conaffinity should have a convex hull. // mesh_with_hull_conaffinity should have a convex hull.
EXPECT_NE(model->mesh_graphadr[with_hull_conaffinity_id], -1); EXPECT_NE(model->mesh_graphadr[with_hull_conaffinity_id], -1);
mj_deleteModel(model);
} }
TEST_F(UserModelTest, ConvexHullForPairCollisionMeshes) { TEST_F(UserModelTest, ConvexHullForPairCollisionMeshes) {
@@ -380,17 +367,15 @@ TEST_F(UserModelTest, ConvexHullForPairCollisionMeshes) {
</mujoco>)"; </mujoco>)";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data(); ASSERT_THAT(model.get(), NotNull()) << error.data();
int mesh_in_pair_id = mj_name2id(model, mjOBJ_MESH, "mesh"); int mesh_in_pair_id = mj_name2id(model.get(), mjOBJ_MESH, "mesh");
EXPECT_NE(mesh_in_pair_id, -1); EXPECT_NE(mesh_in_pair_id, -1);
// mesh_in_pair should have a convex hull because it is in a collision pair. // mesh_in_pair should have a convex hull because it is in a collision pair.
EXPECT_NE(model->mesh_graphadr[mesh_in_pair_id], -1); EXPECT_NE(model->mesh_graphadr[mesh_in_pair_id], -1);
mj_deleteModel(model);
} }
// ------------- test automatic inference of nuser_xxx ------------------------- // ------------- test automatic inference of nuser_xxx -------------------------
@@ -406,11 +391,10 @@ TEST_F(UserDataTest, AutoNUserBody) {
</worldbody> </worldbody>
</mujoco> </mujoco>
)"; )";
mjModel* m = LoadModelFromString(xml); MjModelPtr m = LoadModelFromString(xml);
ASSERT_EQ(m->nuser_body, 3); ASSERT_EQ(m->nuser_body, 3);
EXPECT_THAT(GetRow(m->body_user, m->nuser_body, 1), ElementsAre(1, 2, 3)); EXPECT_THAT(GetRow(m->body_user, m->nuser_body, 1), ElementsAre(1, 2, 3));
EXPECT_THAT(GetRow(m->body_user, m->nuser_body, 2), ElementsAre(2, 3, 0)); EXPECT_THAT(GetRow(m->body_user, m->nuser_body, 2), ElementsAre(2, 3, 0));
mj_deleteModel(m);
} }
TEST_F(UserDataTest, AutoNUserJoint) { TEST_F(UserDataTest, AutoNUserJoint) {
@@ -425,11 +409,10 @@ TEST_F(UserDataTest, AutoNUserJoint) {
</worldbody> </worldbody>
</mujoco> </mujoco>
)"; )";
mjModel* m = LoadModelFromString(xml); MjModelPtr m = LoadModelFromString(xml);
ASSERT_EQ(m->nuser_jnt, 3); ASSERT_EQ(m->nuser_jnt, 3);
EXPECT_THAT(GetRow(m->jnt_user, m->nuser_jnt, 0), ElementsAre(1, 2, 3)); EXPECT_THAT(GetRow(m->jnt_user, m->nuser_jnt, 0), ElementsAre(1, 2, 3));
EXPECT_THAT(GetRow(m->jnt_user, m->nuser_jnt, 1), ElementsAre(2, 3, 0)); EXPECT_THAT(GetRow(m->jnt_user, m->nuser_jnt, 1), ElementsAre(2, 3, 0));
mj_deleteModel(m);
} }
TEST_F(UserDataTest, AutoNUserGeom) { TEST_F(UserDataTest, AutoNUserGeom) {
@@ -441,11 +424,10 @@ TEST_F(UserDataTest, AutoNUserGeom) {
</worldbody> </worldbody>
</mujoco> </mujoco>
)"; )";
mjModel* m = LoadModelFromString(xml); MjModelPtr m = LoadModelFromString(xml);
ASSERT_EQ(m->nuser_geom, 3); ASSERT_EQ(m->nuser_geom, 3);
EXPECT_THAT(GetRow(m->geom_user, m->nuser_geom, 0), ElementsAre(1, 2, 3)); EXPECT_THAT(GetRow(m->geom_user, m->nuser_geom, 0), ElementsAre(1, 2, 3));
EXPECT_THAT(GetRow(m->geom_user, m->nuser_geom, 1), ElementsAre(2, 3, 0)); EXPECT_THAT(GetRow(m->geom_user, m->nuser_geom, 1), ElementsAre(2, 3, 0));
mj_deleteModel(m);
} }
TEST_F(UserDataTest, AutoNUserSite) { TEST_F(UserDataTest, AutoNUserSite) {
@@ -457,11 +439,10 @@ TEST_F(UserDataTest, AutoNUserSite) {
</worldbody> </worldbody>
</mujoco> </mujoco>
)"; )";
mjModel* m = LoadModelFromString(xml); MjModelPtr m = LoadModelFromString(xml);
ASSERT_EQ(m->nuser_site, 3); ASSERT_EQ(m->nuser_site, 3);
EXPECT_THAT(GetRow(m->site_user, m->nuser_site, 0), ElementsAre(1, 2, 3)); EXPECT_THAT(GetRow(m->site_user, m->nuser_site, 0), ElementsAre(1, 2, 3));
EXPECT_THAT(GetRow(m->site_user, m->nuser_site, 1), ElementsAre(2, 3, 0)); EXPECT_THAT(GetRow(m->site_user, m->nuser_site, 1), ElementsAre(2, 3, 0));
mj_deleteModel(m);
} }
TEST_F(UserDataTest, AutoNUserCamera) { TEST_F(UserDataTest, AutoNUserCamera) {
@@ -473,11 +454,10 @@ TEST_F(UserDataTest, AutoNUserCamera) {
</worldbody> </worldbody>
</mujoco> </mujoco>
)"; )";
mjModel* m = LoadModelFromString(xml); MjModelPtr m = LoadModelFromString(xml);
ASSERT_EQ(m->nuser_cam, 3); ASSERT_EQ(m->nuser_cam, 3);
EXPECT_THAT(GetRow(m->cam_user, m->nuser_cam, 0), ElementsAre(1, 2, 3)); EXPECT_THAT(GetRow(m->cam_user, m->nuser_cam, 0), ElementsAre(1, 2, 3));
EXPECT_THAT(GetRow(m->cam_user, m->nuser_cam, 1), ElementsAre(2, 3, 0)); EXPECT_THAT(GetRow(m->cam_user, m->nuser_cam, 1), ElementsAre(2, 3, 0));
mj_deleteModel(m);
} }
TEST_F(UserDataTest, AutoNUserTendon) { TEST_F(UserDataTest, AutoNUserTendon) {
@@ -499,11 +479,10 @@ TEST_F(UserDataTest, AutoNUserTendon) {
</tendon> </tendon>
</mujoco> </mujoco>
)"; )";
mjModel* m = LoadModelFromString(xml); MjModelPtr m = LoadModelFromString(xml);
ASSERT_EQ(m->nuser_tendon, 3); ASSERT_EQ(m->nuser_tendon, 3);
EXPECT_THAT(GetRow(m->tendon_user, m->nuser_tendon, 0), ElementsAre(1, 2, 3)); EXPECT_THAT(GetRow(m->tendon_user, m->nuser_tendon, 0), ElementsAre(1, 2, 3));
EXPECT_THAT(GetRow(m->tendon_user, m->nuser_tendon, 1), ElementsAre(2, 3, 0)); EXPECT_THAT(GetRow(m->tendon_user, m->nuser_tendon, 1), ElementsAre(2, 3, 0));
mj_deleteModel(m);
} }
TEST_F(UserDataTest, AutoNUserActuator) { TEST_F(UserDataTest, AutoNUserActuator) {
@@ -521,13 +500,12 @@ TEST_F(UserDataTest, AutoNUserActuator) {
</actuator> </actuator>
</mujoco> </mujoco>
)"; )";
mjModel* m = LoadModelFromString(xml); MjModelPtr m = LoadModelFromString(xml);
ASSERT_EQ(m->nuser_actuator, 3); ASSERT_EQ(m->nuser_actuator, 3);
EXPECT_THAT(GetRow(m->actuator_user, m->nuser_actuator, 0), EXPECT_THAT(GetRow(m->actuator_user, m->nuser_actuator, 0),
ElementsAre(1, 2, 3)); ElementsAre(1, 2, 3));
EXPECT_THAT(GetRow(m->actuator_user, m->nuser_actuator, 1), EXPECT_THAT(GetRow(m->actuator_user, m->nuser_actuator, 1),
ElementsAre(2, 3, 0)); ElementsAre(2, 3, 0));
mj_deleteModel(m);
} }
TEST_F(UserDataTest, AutoNUserSensor) { TEST_F(UserDataTest, AutoNUserSensor) {
@@ -542,11 +520,10 @@ TEST_F(UserDataTest, AutoNUserSensor) {
</sensor> </sensor>
</mujoco> </mujoco>
)"; )";
mjModel* m = LoadModelFromString(xml); MjModelPtr m = LoadModelFromString(xml);
ASSERT_EQ(m->nuser_sensor, 3); ASSERT_EQ(m->nuser_sensor, 3);
EXPECT_THAT(GetRow(m->sensor_user, m->nuser_sensor, 0), ElementsAre(1, 2, 3)); EXPECT_THAT(GetRow(m->sensor_user, m->nuser_sensor, 0), ElementsAre(1, 2, 3));
EXPECT_THAT(GetRow(m->sensor_user, m->nuser_sensor, 1), ElementsAre(2, 3, 0)); EXPECT_THAT(GetRow(m->sensor_user, m->nuser_sensor, 1), ElementsAre(2, 3, 0));
mj_deleteModel(m);
} }
// ------------- test duplicate names ------------------------------------------ // ------------- test duplicate names ------------------------------------------
@@ -584,10 +561,10 @@ TEST_F(FuseStaticTest, FuseStaticEquivalent) {
std::string fuse = absl::StrFormat(xml_template, "true"); std::string fuse = absl::StrFormat(xml_template, "true");
std::string no_fuse = absl::StrFormat(xml_template, "false"); std::string no_fuse = absl::StrFormat(xml_template, "false");
mjModel* m_fuse = LoadModelFromString(fuse.c_str()); MjModelPtr m_fuse = LoadModelFromString(fuse.c_str());
mjModel* m_no_fuse = LoadModelFromString(no_fuse.c_str()); MjModelPtr m_no_fuse = LoadModelFromString(no_fuse.c_str());
ASSERT_THAT(m_fuse, NotNull()); ASSERT_THAT(m_fuse.get(), NotNull());
ASSERT_THAT(m_no_fuse, NotNull()); ASSERT_THAT(m_no_fuse.get(), NotNull());
EXPECT_EQ(m_fuse->nbody, 2) << "Expecting a world body and one other body"; EXPECT_EQ(m_fuse->nbody, 2) << "Expecting a world body and one other body";
EXPECT_EQ(m_no_fuse->nbody, 3) << "Expecting a world body and two others"; EXPECT_EQ(m_no_fuse->nbody, 3) << "Expecting a world body and two others";
@@ -600,20 +577,16 @@ TEST_F(FuseStaticTest, FuseStaticEquivalent) {
EXPECT_EQ(m_no_fuse->body_bvhnum[2], 3); EXPECT_EQ(m_no_fuse->body_bvhnum[2], 3);
EXPECT_EQ(m_fuse->body_bvhnum[1], 3); EXPECT_EQ(m_fuse->body_bvhnum[1], 3);
mjData* d_fuse = mj_makeData(m_fuse); MjDataPtr d_fuse = MakeData(m_fuse);
mjData* d_no_fuse = mj_makeData(m_no_fuse); MjDataPtr d_no_fuse = MakeData(m_no_fuse);
mj_step(m_fuse, d_fuse); mj_step(m_fuse.get(), d_fuse.get());
mj_step(m_no_fuse, d_no_fuse); mj_step(m_no_fuse.get(), d_no_fuse.get());
EXPECT_NEAR(d_fuse->qvel[0], d_no_fuse->qvel[0], MjTol(2e-17, 1e-8)) EXPECT_NEAR(d_fuse.get()->qvel[0], d_no_fuse.get()->qvel[0],
MjTol(2e-17, 1e-8))
<< "Velocity should be the same after 1 step"; << "Velocity should be the same after 1 step";
EXPECT_NE(d_fuse->qvel[0], 0); EXPECT_NE(d_fuse.get()->qvel[0], 0);
mj_deleteData(d_fuse);
mj_deleteData(d_no_fuse);
mj_deleteModel(m_fuse);
mj_deleteModel(m_no_fuse);
} }
TEST_F(FuseStaticTest, FuseStaticActuatorReferencedBody) { TEST_F(FuseStaticTest, FuseStaticActuatorReferencedBody) {
@@ -642,10 +615,9 @@ TEST_F(FuseStaticTest, FuseStaticActuatorReferencedBody) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml_template, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml_template, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m.get(), NotNull()) << error.data();
EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others"; EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others";
mj_deleteModel(m);
} }
TEST_F(FuseStaticTest, FuseStaticLightReferencedBody) { TEST_F(FuseStaticTest, FuseStaticLightReferencedBody) {
@@ -671,10 +643,9 @@ TEST_F(FuseStaticTest, FuseStaticLightReferencedBody) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml_template, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml_template, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m.get(), NotNull()) << error.data();
EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others"; EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others";
mj_deleteModel(m);
} }
TEST_F(FuseStaticTest, FuseStaticForceSensorReferencedBody) { TEST_F(FuseStaticTest, FuseStaticForceSensorReferencedBody) {
@@ -704,10 +675,9 @@ TEST_F(FuseStaticTest, FuseStaticForceSensorReferencedBody) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml_template, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml_template, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m.get(), NotNull()) << error.data();
EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others"; EXPECT_EQ(m->nbody, 3) << "Expecting a world body and two others";
mj_deleteModel(m);
} }
TEST_F(FuseStaticTest, FuseStaticCameraInBody) { TEST_F(FuseStaticTest, FuseStaticCameraInBody) {
@@ -727,11 +697,10 @@ TEST_F(FuseStaticTest, FuseStaticCameraInBody) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m.get(), NotNull()) << error.data();
EXPECT_EQ(m->nbody, 2) << "Static body should be fused"; EXPECT_EQ(m->nbody, 2) << "Static body should be fused";
EXPECT_EQ(m->ncam, 1); EXPECT_EQ(m->ncam, 1);
mj_deleteModel(m);
} }
TEST_F(FuseStaticTest, FuseStaticLightInBody) { TEST_F(FuseStaticTest, FuseStaticLightInBody) {
@@ -750,11 +719,10 @@ TEST_F(FuseStaticTest, FuseStaticLightInBody) {
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data(); ASSERT_THAT(m.get(), NotNull()) << error.data();
EXPECT_EQ(m->nbody, 2) << "Static body should be fused"; EXPECT_EQ(m->nbody, 2) << "Static body should be fused";
EXPECT_EQ(m->nlight, 1); EXPECT_EQ(m->nlight, 1);
mj_deleteModel(m);
} }
// ------------- test discardvisual -------------------------------------------- // ------------- test discardvisual --------------------------------------------
@@ -789,13 +757,12 @@ TEST_F(DiscardVisualTest, DiscardVisualKeepsInertia) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(model, NotNull()) << error.data(); EXPECT_THAT(model.get(), NotNull()) << error.data();
EXPECT_THAT(model->nmesh, 1); EXPECT_THAT(model->nmesh, 1);
EXPECT_THAT(model->body_inertia[3], model->body_inertia[6]); EXPECT_THAT(model->body_inertia[3], model->body_inertia[6]);
EXPECT_THAT(model->body_inertia[4], model->body_inertia[7]); EXPECT_THAT(model->body_inertia[4], model->body_inertia[7]);
EXPECT_THAT(model->body_inertia[5], model->body_inertia[8]); EXPECT_THAT(model->body_inertia[5], model->body_inertia[8]);
mj_deleteModel(model);
} }
TEST_F(DiscardVisualTest, DiscardVisualEquivalent) { TEST_F(DiscardVisualTest, DiscardVisualEquivalent) {
@@ -877,8 +844,7 @@ TEST_F(LengthRangeTest, LengthRangeThreading) {
size_t error_sz = 1024; size_t error_sz = 1024;
std::string field = ""; std::string field = "";
static const char* const kLengthrangePath = static const char* const kLengthrangePath = "user/testdata/lengthrange.xml";
"user/testdata/lengthrange.xml";
const std::string xml_path1 = GetTestDataFilePath(kLengthrangePath); const std::string xml_path1 = GetTestDataFilePath(kLengthrangePath);
mjSpec* spec = mj_parseXML(xml_path1.c_str(), 0, error, error_sz); mjSpec* spec = mj_parseXML(xml_path1.c_str(), 0, error, error_sz);
@@ -1022,20 +988,17 @@ TEST_F(MujocoTest, NestedMeshDir) {
sizeof(cube)); sizeof(cube));
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* child_model = LoadModelFromString(child_xml, error.data(), MjModelPtr child_model =
error.size(), vfs.get()); LoadModelFromString(child_xml, error.data(), error.size(), vfs.get());
EXPECT_THAT(child_model, NotNull()) << error.data(); EXPECT_THAT(child_model.get(), NotNull()) << error.data();
mj_deleteModel(child_model);
mjModel* parent_model = LoadModelFromString(parent_xml, error.data(), MjModelPtr parent_model =
error.size(), vfs.get()); LoadModelFromString(parent_xml, error.data(), error.size(), vfs.get());
EXPECT_THAT(parent_model, NotNull()) << error.data(); EXPECT_THAT(parent_model.get(), NotNull()) << error.data();
mj_deleteModel(parent_model);
mjModel* grandparent_model = LoadModelFromString( MjModelPtr grandparent_model = LoadModelFromString(
grandparent_xml, error.data(), error.size(), vfs.get()); grandparent_xml, error.data(), error.size(), vfs.get());
EXPECT_THAT(grandparent_model, NotNull()) << error.data(); EXPECT_THAT(grandparent_model.get(), NotNull()) << error.data();
mj_deleteModel(grandparent_model);
mj_deleteVFS(vfs.get()); mj_deleteVFS(vfs.get());
} }
@@ -1087,8 +1050,8 @@ TEST_F(DelayBufferTest, ActuatorDelayBufferSizes) {
</actuator> </actuator>
</mujoco> </mujoco>
)"; )";
mjModel* m = LoadModelFromString(xml); MjModelPtr m = LoadModelFromString(xml);
ASSERT_THAT(m, NotNull()); ASSERT_THAT(m.get(), NotNull());
ASSERT_EQ(m->nu, 3); ASSERT_EQ(m->nu, 3);
// nhistory = (2+2*3) + (2+2*10) = 8 + 22 = 30 // nhistory = (2+2*3) + (2+2*10) = 8 + 22 = 30
@@ -1101,8 +1064,6 @@ TEST_F(DelayBufferTest, ActuatorDelayBufferSizes) {
EXPECT_EQ(m->actuator_historyadr[0], -1); EXPECT_EQ(m->actuator_historyadr[0], -1);
EXPECT_EQ(m->actuator_historyadr[1], 0); EXPECT_EQ(m->actuator_historyadr[1], 0);
EXPECT_EQ(m->actuator_historyadr[2], 8); EXPECT_EQ(m->actuator_historyadr[2], 8);
mj_deleteModel(m);
} }
} // namespace } // namespace
File diff suppressed because it is too large Load Diff
+33 -33
View File
@@ -14,6 +14,8 @@
// Tests for xml/xml_api.cc. // Tests for xml/xml_api.cc.
#include "src/xml/xml_api.h"
#include <array> #include <array>
#include <cstddef> #include <cstddef>
#include <cstring> #include <cstring>
@@ -23,18 +25,17 @@
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <mujoco/mjdata.h> #include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h> #include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h>
#include <mujoco/mjspec.h> #include <mujoco/mjspec.h>
#include "src/xml/xml_api.h" #include <mujoco/mujoco.h>
#include "test/fixture.h" #include "test/fixture.h"
namespace mujoco { namespace mujoco {
namespace { namespace {
using ::testing::HasSubstr;
using ::testing::IsNull; using ::testing::IsNull;
using ::testing::NotNull; using ::testing::NotNull;
using ::testing::StartsWith; using ::testing::StartsWith;
using ::testing::HasSubstr;
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
@@ -57,62 +58,56 @@ using LoadXmlTest = MujocoTest;
TEST_F(LoadXmlTest, EmptyModel) { TEST_F(LoadXmlTest, EmptyModel) {
static constexpr char xml[] = "<mujoco/>"; static constexpr char xml[] = "<mujoco/>";
mjModel* model = LoadModelFromString(xml, 0, 0); MjModelPtr model = LoadModelFromString(xml, 0, 0);
ASSERT_THAT(model, NotNull()); ASSERT_THAT(model.get(), NotNull());
EXPECT_EQ(model->nq, 0); EXPECT_EQ(model->nq, 0);
EXPECT_EQ(model->nv, 0); EXPECT_EQ(model->nv, 0);
EXPECT_EQ(model->nu, 0); EXPECT_EQ(model->nu, 0);
EXPECT_EQ(model->na, 0); EXPECT_EQ(model->na, 0);
EXPECT_EQ(model->nbody, 1); // worldbody exists even in empty model EXPECT_EQ(model->nbody, 1); // worldbody exists even in empty model
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
EXPECT_THAT(data, NotNull()); EXPECT_THAT(data, NotNull());
mj_step(model, data); mj_step(model.get(), data.get());
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(LoadXmlTest, InvalidXmlFailsToLoad) { TEST_F(LoadXmlTest, InvalidXmlFailsToLoad) {
static constexpr char invalid_xml[] = "<mujoc"; static constexpr char invalid_xml[] = "<mujoc";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(invalid_xml, error.data(), error.size()); MjModelPtr model =
EXPECT_THAT(model, IsNull()) << "Expected model loading to fail."; LoadModelFromString(invalid_xml, error.data(), error.size());
EXPECT_THAT(model.get(), IsNull()) << "Expected model loading to fail.";
EXPECT_GT(std::strlen(error.data()), 0); EXPECT_GT(std::strlen(error.data()), 0);
if (model) { if (model.get()) {
mj_deleteModel(model);
} }
} }
TEST_F(LoadXmlTest, MultipleBodies) { TEST_F(LoadXmlTest, MultipleBodies) {
std::array<char, 1000> error; std::array<char, 1000> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); MjModelPtr model = LoadModelFromString(xml, 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_EQ(model->nbody, 3); EXPECT_EQ(model->nbody, 3);
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
EXPECT_THAT(data, NotNull()); EXPECT_THAT(data, NotNull());
mj_step(model, data); mj_step(model.get(), data.get());
mj_deleteData(data);
mj_deleteModel(model);
} }
using SaveLastXmlTest = MujocoTest; using SaveLastXmlTest = MujocoTest;
TEST_F(SaveLastXmlTest, EmptyModel) { TEST_F(SaveLastXmlTest, EmptyModel) {
static constexpr char xml[] = "<mujoco/>"; static constexpr char xml[] = "<mujoco/>";
mjModel* model = LoadModelFromString(xml, 0, 0); MjModelPtr model = LoadModelFromString(xml, 0, 0);
mjData* data = mj_makeData(model); MjDataPtr data = MakeData(model);
std::array<char, 1024> error; std::array<char, 1024> error;
error.data()[0] = '\0'; error.data()[0] = '\0';
testing::internal::CaptureStdout(); testing::internal::CaptureStdout();
mj_saveLastXML(nullptr, model, error.data(), error.size()); mj_saveLastXML(nullptr, model.get(), error.data(), error.size());
EXPECT_THAT(testing::internal::GetCapturedStdout(), StartsWith("<mujoco")); EXPECT_THAT(testing::internal::GetCapturedStdout(), StartsWith("<mujoco"));
mj_deleteData(data);
mj_deleteModel(model);
} }
TEST_F(LoadXmlTest, NullFileFails) { TEST_F(LoadXmlTest, NullFileFails) {
@@ -138,8 +133,9 @@ TEST_F(MujocoTest, SaveXmlShortString) {
EXPECT_THAT(model, NotNull()) << "Failed to compile model: " << error.data(); EXPECT_THAT(model, NotNull()) << "Failed to compile model: " << error.data();
std::array<char, 10> out; std::array<char, 10> out;
EXPECT_THAT(mj_saveXMLString(spec, out.data(), out.size(), EXPECT_THAT(mj_saveXMLString(spec, out.data(), out.size(), error.data(),
error.data(), error.size()), 273); error.size()),
273);
EXPECT_STREQ(error.data(), "Output string too short, should be at least 274"); EXPECT_STREQ(error.data(), "Output string too short, should be at least 274");
mj_deleteSpec(spec); mj_deleteSpec(spec);
@@ -156,10 +152,13 @@ TEST_F(MujocoTest, SaveXml) {
std::array<char, 274> out; std::array<char, 274> out;
EXPECT_THAT(mj_saveXMLString(NULL, out.data(), out.size(), error.data(), EXPECT_THAT(mj_saveXMLString(NULL, out.data(), out.size(), error.data(),
error.size()), -1); error.size()),
-1);
EXPECT_STREQ(error.data(), "Cannot write empty model"); EXPECT_STREQ(error.data(), "Cannot write empty model");
EXPECT_THAT(mj_saveXMLString(spec, out.data(), out.size(), error.data(), EXPECT_THAT(mj_saveXMLString(spec, out.data(), out.size(), error.data(),
error.size()), 0) << error.data(); error.size()),
0)
<< error.data();
mjSpec* saved_spec = mj_parseXMLString(xml, 0, error.data(), error.size()); mjSpec* saved_spec = mj_parseXMLString(xml, 0, error.data(), error.size());
EXPECT_THAT(saved_spec, NotNull()) << "Invalid saved spec: " << error.data(); EXPECT_THAT(saved_spec, NotNull()) << "Invalid saved spec: " << error.data();
@@ -197,7 +196,9 @@ TEST_F(MujocoTest, SaveXmlWithDefaultMesh) {
std::array<char, 1024> out; std::array<char, 1024> out;
EXPECT_THAT(mj_saveXMLString(spec, out.data(), out.size(), error.data(), EXPECT_THAT(mj_saveXMLString(spec, out.data(), out.size(), error.data(),
error.size()), 0) << error.data(); error.size()),
0)
<< error.data();
mjSpec* saved_spec = mj_parseXMLString(xml, 0, error.data(), error.size()); mjSpec* saved_spec = mj_parseXMLString(xml, 0, error.data(), error.size());
EXPECT_THAT(saved_spec, NotNull()) << "Invalid saved spec: " << error.data(); EXPECT_THAT(saved_spec, NotNull()) << "Invalid saved spec: " << error.data();
@@ -215,9 +216,8 @@ TEST_F(MujocoTest, SaveXmlWithDefaultMesh) {
TEST_F(MujocoTest, FreeLastXml) { TEST_F(MujocoTest, FreeLastXml) {
static constexpr char xml[] = "<mujoco/>"; static constexpr char xml[] = "<mujoco/>";
mjModel* model = LoadModelFromString(xml, 0, 0); MjModelPtr model = LoadModelFromString(xml, 0, 0);
ASSERT_THAT(model, NotNull()); ASSERT_THAT(model.get(), NotNull());
mj_deleteModel(model);
ASSERT_NE(mj_saveLastXML(nullptr, nullptr, nullptr, 0), 0); ASSERT_NE(mj_saveLastXML(nullptr, nullptr, nullptr, 0), 0);
mj_freeLastXML(); mj_freeLastXML();
ASSERT_EQ(mj_saveLastXML(nullptr, nullptr, nullptr, 0), 0); ASSERT_EQ(mj_saveLastXML(nullptr, nullptr, nullptr, 0), 0);
File diff suppressed because it is too large Load Diff
File diff suppressed because it is too large Load Diff
+15 -24
View File
@@ -45,9 +45,8 @@ TEST_F(MujocoTest, ReadsCapsule) {
</robot> </robot>
)"; )";
std::array<char, 1000> error; std::array<char, 1000> error;
mjModel* model = LoadModelFromString(urdf, error.data(), error.size()); MjModelPtr model = LoadModelFromString(urdf, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data(); ASSERT_THAT(model.get(), NotNull()) << error.data();
mj_deleteModel(model);
} }
TEST_F(MujocoTest, ReadsGeomNames) { TEST_F(MujocoTest, ReadsGeomNames) {
@@ -74,19 +73,17 @@ TEST_F(MujocoTest, ReadsGeomNames) {
</robot> </robot>
)"; )";
std::array<char, 1000> error; std::array<char, 1000> error;
mjModel* model = LoadModelFromString(urdf, error.data(), error.size()); MjModelPtr model = LoadModelFromString(urdf, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data(); ASSERT_THAT(model.get(), NotNull()) << error.data();
// Check the geoms have been loaded with the right names // Check the geoms have been loaded with the right names
int collision_box_id = mj_name2id(model, mjtObj::mjOBJ_GEOM, "collision_box"); int collision_box_id = mj_name2id(model.get(), mjtObj::mjOBJ_GEOM, "collision_box");
ASSERT_GE(collision_box_id, 0); ASSERT_GE(collision_box_id, 0);
EXPECT_EQ(model->geom_type[collision_box_id], mjtGeom::mjGEOM_BOX); EXPECT_EQ(model->geom_type[collision_box_id], mjtGeom::mjGEOM_BOX);
int visual_sphere_id = mj_name2id(model, mjtObj::mjOBJ_GEOM, "visual_sphere"); int visual_sphere_id = mj_name2id(model.get(), mjtObj::mjOBJ_GEOM, "visual_sphere");
ASSERT_GE(visual_sphere_id, 0); ASSERT_GE(visual_sphere_id, 0);
EXPECT_EQ(model->geom_type[visual_sphere_id], mjtGeom::mjGEOM_SPHERE); EXPECT_EQ(model->geom_type[visual_sphere_id], mjtGeom::mjGEOM_SPHERE);
mj_deleteModel(model);
} }
TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesCollisionBeforeVisual) { TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesCollisionBeforeVisual) {
@@ -113,16 +110,14 @@ TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesCollisionBeforeVisual) {
</robot> </robot>
)"; )";
std::array<char, 1000> error; std::array<char, 1000> error;
mjModel* model = LoadModelFromString(urdf, error.data(), error.size()); MjModelPtr model = LoadModelFromString(urdf, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data(); ASSERT_THAT(model.get(), NotNull()) << error.data();
// Check the collision geom gets its name from the URDF. The visual sphere // Check the collision geom gets its name from the URDF. The visual sphere
// should not have a name to avoid duplicates. // should not have a name to avoid duplicates.
int collision_box_id = mj_name2id(model, mjtObj::mjOBJ_GEOM, "shared_name"); int collision_box_id = mj_name2id(model.get(), mjtObj::mjOBJ_GEOM, "shared_name");
ASSERT_GE(collision_box_id, 0); ASSERT_GE(collision_box_id, 0);
EXPECT_EQ(model->geom_type[collision_box_id], mjtGeom::mjGEOM_BOX); EXPECT_EQ(model->geom_type[collision_box_id], mjtGeom::mjGEOM_BOX);
mj_deleteModel(model);
} }
TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesVisualBeforeCollision) { TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesVisualBeforeCollision) {
@@ -149,16 +144,14 @@ TEST_F(MujocoTest, CanLoadUrdfWithNonUniqueNamesVisualBeforeCollision) {
</robot> </robot>
)"; )";
std::array<char, 1000> error; std::array<char, 1000> error;
mjModel* model = LoadModelFromString(urdf, error.data(), error.size()); MjModelPtr model = LoadModelFromString(urdf, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data(); ASSERT_THAT(model.get(), NotNull()) << error.data();
// Check the visual geom gets its name from the URDF. The collision geom // Check the visual geom gets its name from the URDF. The collision geom
// should not have a name to avoid duplicates. // should not have a name to avoid duplicates.
int visual_sphere_id = mj_name2id(model, mjtObj::mjOBJ_GEOM, "shared_name"); int visual_sphere_id = mj_name2id(model.get(), mjtObj::mjOBJ_GEOM, "shared_name");
ASSERT_GE(visual_sphere_id, 0); ASSERT_GE(visual_sphere_id, 0);
EXPECT_EQ(model->geom_type[visual_sphere_id], mjtGeom::mjGEOM_SPHERE); EXPECT_EQ(model->geom_type[visual_sphere_id], mjtGeom::mjGEOM_SPHERE);
mj_deleteModel(model);
} }
TEST_F(MujocoTest, ReadsJointTypes) { TEST_F(MujocoTest, ReadsJointTypes) {
@@ -226,8 +219,8 @@ TEST_F(MujocoTest, ReadsJointTypes) {
</robot> </robot>
)"; )";
std::array<char, 1000> error; std::array<char, 1000> error;
mjModel* model = LoadModelFromString(urdf, error.data(), error.size()); MjModelPtr model = LoadModelFromString(urdf, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data(); ASSERT_THAT(model.get(), NotNull()) << error.data();
constexpr float eps = 1e-6; constexpr float eps = 1e-6;
@@ -241,14 +234,12 @@ TEST_F(MujocoTest, ReadsJointTypes) {
{0.0, 0.0, 1.0} {0.0, 0.0, 1.0}
}; };
for (int i = 0; i < joint_names.size(); ++i) { for (int i = 0; i < joint_names.size(); ++i) {
int id = mj_name2id(model, mjtObj::mjOBJ_JOINT, joint_names[i].c_str()); int id = mj_name2id(model.get(), mjtObj::mjOBJ_JOINT, joint_names[i].c_str());
EXPECT_EQ(model->jnt_type[id], expected_joint_types[i]); EXPECT_EQ(model->jnt_type[id], expected_joint_types[i]);
EXPECT_NEAR(model->jnt_axis[3 * id], expected_axis[i][0], eps); EXPECT_NEAR(model->jnt_axis[3 * id], expected_axis[i][0], eps);
EXPECT_NEAR(model->jnt_axis[3 * id + 1], expected_axis[i][1], eps); EXPECT_NEAR(model->jnt_axis[3 * id + 1], expected_axis[i][1], eps);
EXPECT_NEAR(model->jnt_axis[3 * id + 2], expected_axis[i][2], eps); EXPECT_NEAR(model->jnt_axis[3 * id + 2], expected_axis[i][2], eps);
} }
mj_deleteModel(model);
} }
TEST_F(MujocoTest, RepeatedMeshName) { TEST_F(MujocoTest, RepeatedMeshName) {